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

权利要求书

1.一种自动泊车路径跟踪及车辆控制方法，其特征在于包括如下步骤：

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]时，目标点为P_i，转步骤6)，否则i＝i+1，转步骤5)；

5)比较i与N+1，若i＜N+1，转步骤4)，否则转步骤7)；

6)车辆向目标点P_i逼近，转步骤3)；

7)路径跟踪结束；

步骤6)中，车辆向目标点P_i逼近的过程中，确定速度控制量的方法为：

v_control＝P_real[3]+Δv，|P_real[3]+Δv|≤v_limt

步骤6)中，车辆向目标点P_i逼近的过程中，确定前轮转角控制量的方法为：

如果P_real[2]＞P[i][2]，

如果P_real[2]＜P[i][2]，

u_control＝P_real[4]+Δu，|P_real[4]+Δu|≤u_limt

2.如权利要求1所述的自动泊车路径跟踪及车辆控制方法，其特征在于，所述方法基于以下控制系统实现：包括路径跟踪控制器、惯性导航模块、陀螺仪、传感器单元、电源模块；其中，所述电源模块分别与所述路径跟踪控制器、惯性导航模块、陀螺仪和传感器单元相连，用于给各模块供电；电源模块还包含电路保护单元，可在电源接反的情况下自动断开，可在系统电流过大的情况下自动切断电源；所述传感器单元包含轮速传感器和方向盘转角传感器，轮速传感器用于采集车辆行驶速度，方向盘转角传感器用于采集方向盘转角，传感器单元输出端与路径跟踪控制器相连；所述惯性导航模块用于获得车辆当前的坐标，输出端与路径跟踪控制器相连；所述陀螺仪用于获得车辆当前的车身角，输出端与路径跟踪控制器相连；所述路径跟踪控制器包括数据处理单元和微控制单元MCU，数据处理单元用于实时处理传感器单元传来的信息，并将处理后得到的车辆状态信息发送给MCU，MCU用于接收来自于数据处理单元、惯性导航模块和陀螺仪的信息，将得到的车辆状态信息与参考车辆状态信息相比较，利用跟踪算法算出当前的速度和方向盘转角的最佳增量。