[发明专利]一种自动泊车路径跟踪及车辆控制系统和方法

摘要

本发明提供了一种自动泊车路径跟踪及车辆控制系统和方法，控制系统包括路径跟踪控制器、惯性导航模块、陀螺仪、传感器单元、电源模块。本发明通过对车辆实时状态量与参考点状态量进行比较，确定目标参考点，并通过算法控制车辆依次向目标参考点逼近，直至路径跟踪完成。本发明可根据规划好的路径对车辆进行较为精确的路径跟踪控制，跟踪过程中实时对车辆状态进行矫正，提高了泊车精度，降低了对泊车位要求和泊车失败率。

主权项

一种自动泊车路径跟踪及车辆控制方法，其特征在于包括如下步骤：1)初始化系统变量，包括计数变量i，速度阈值v_limt，速度控制量v_control，单位时间内速度增量△v,单位时间内速度增量阈值△v_max，前轮转角阈值u_limt，前轮转角控制量u_control，单位时间内前轮转角增量△u，单位时间内前轮转角增量阈值△u_max；2)初始化规划的泊车路径状态特征数组P[N+1][5]，初始化方法如下：2.1)在规划的泊车路径上选择参考点，即在规划的泊车路径上每隔Δs距离取一个参考点，并将每一个参考点依次编号为0，1，…，N；2.2)确定坐标原点，建立世界坐标系XOY，即以编号为N的参考点为坐标原点，建立世界坐标系XOY，其中X轴与泊车位前后两车中心线平行，且方向指向前车，Y轴与X轴垂直且符合右手法则；2.3)确定参考点状态特征参数，建立泊车路径状态特征数组P[N+1][5]，其中，P[k][0]表示k点对应的参考点X坐标值，P[k][1]表示k点对应的参考点Y坐标值，P[k][2]表示k点对应的参考点航向角，顺时针方向为负，逆时针方向为正，P[k][3]表示k点对应的参考点参考速度，向前为正，向后为负，P[k][4]表示k点对应的参考点参考前轮转角，顺时针方向为负，逆时针方向为正，k＝0，1，2...N；3)建立车辆当前状态数组P_real[5]，其中，P_real[0]表示车辆当前X坐标值，P_real[1]表示车辆当前Y坐标值，P_real[2]表示车辆当前航向角，顺时针方向为负，逆时针方向为正，P_real[3]表示当前车速，向前为正，向后为负，P_real[4]表示车辆当前前轮转角，顺时针方向为负，逆时针方向为正；4)寻找目标点，如果P_real[0]＞P[i][0]时，目标点为Pi，转步骤6)，否则i＝i+1，转步骤5)；5)比较i与N+1，若i<N+1，转步骤4)，否则转步骤7)；6)车辆向目标点Pi逼近，转步骤3)；7)路径跟踪结束。