[发明专利]基于航向预估的无人驾驶车辆路径跟踪控制方法

权利要求书

1.一种基于航向预估的无人驾驶车辆路径跟踪控制方法，包括如下步骤：

(10)车辆当前时刻状态量获取：获取车辆当前时刻的状态量，包括坐标、实际航向、纵向车速、前轮转角及前轮绝对转角；

(20)可行驶道路终点获取：根据车辆当前位置、实际航向，获取车辆后方最近点，根据纵向车速，获取预期理想道路中前方可行驶道路的终点，即预瞄点和预瞄点处对应的期望航向角；

(30)中间量计算：根据车辆当前位置、纵向车速、前轮转角，计算车辆横向位置偏差、附加航向、车辆预瞄点距离和航向预估量；

(40)预瞄点位置判断：通过在预定的理想道路上预先设置对应的直道起始点和弯道起始点，判断所述预瞄点是否处于直线路径，若否，则转至(60)步骤；

(50)直线路径行驶：根据所述附加航向、航向预估量，以及车辆当前时刻实际航向角与预期道路期望航向角的偏差，调整驾驶方向，使无人驾驶车辆朝着期望直线路径行驶；

(60)弯道路径行驶：根据附加航向、前轮绝对转角，以及车辆当前时刻实际航向角与预期道路的期望航向角的偏差，调整驾驶方向，使无人驾驶车辆朝着期望弯道路径行驶；

所述(10)车辆当前时刻状态量获取步骤包括：

(11)当前坐标获取：通过差分GPS系统获取车辆在大地直角坐标系下的当前位置坐标(X₀,Y₀)；

(12)车辆坐标系建立：以车辆当前位置的质心为原点，速度方向为x轴建立车辆坐标系xoy；

(13)理想道路离散点车辆坐标获取：按下式将预期理想道路上各离散点的坐标转换为车辆坐标系下的坐标，

其中，(x_i,y_i)为预期理想道路上某一点在车辆坐标系下的坐标，(X₀,Y₀)为大地坐标系下当前时刻GPS获取的车辆质心位置的坐标，ψ为大地直角坐标系下车辆当前位置的实际航向，通过惯性导航传感器获得；

(14)纵向车速获取：通过轮速传感器获取车辆当前时刻车速

(15)前轮转角及前轮绝对转角获取：通过转角传感器获取车辆当前时刻前轮实际转角及相应的前轮绝对转角；

所述(20)可行驶道路终点获取步骤包括：

(21)车辆后方最近点获取：从描述预期道路的离散数表中，找到车辆当前位置后方且离车辆最近的点r_j，点r_j在车辆坐标系下的横坐标x_r(j)必为负，且车辆当前位置前方且离车辆最近的点r_j+1的横坐标x_r(j+1)必定为正，则满足关系式

x_r(j)·x_r(j+1)≤0x_r(j)＜0；

(22)前方终点获取：将车辆后方最近点r_j作为搜索的起点，获取预期理想道路中前方可行驶道路的终点，即预瞄点r_k+1，车辆在以当前车速v行驶时间T后，点r_k必定为位于车辆后方且距离车辆最近的点，即点r_k在车辆坐标系下的横坐标x_r(j)必为负，预瞄点r_k+1为r_k前方的点，其横坐标x_r(j+1)必定为正，则满足关系式

x_r(k)·x_r(k+1)≤0x_r(k)＜0；

其特征在于，所述(30)中间量计算步骤包括：

(31)横向位置偏差计算：按下式计算车辆当前位置至预期理想道路的横向位置偏差，

其中，y为横向位置偏差，(X₀,Y₀)为车辆当前位置在大地坐标系下的坐标，(X_r(k+1),Y_r(k+1))为预瞄点r_k+1在大地坐标系下的坐标，ψ_d为预瞄点r_k+1在大地坐标系下的期望航向；

(32)车辆预瞄点距离计算：按下式计算车辆当前位置至预瞄点的距离，

其中，l为车辆当前位置至预瞄点距离，(x_r(k+1),y_r(k+1))为预瞄点在车辆坐标系下的坐标；

(33)附加航向计算：按下式计算附加航向，

其中，为附加航向，y为横向位置偏差，l为车辆当前位置至预瞄点距离；

(34)航向预估量计算：按下式计算航向预估量，

Δθ＝uTsinδ/(a+b)，

其中，Δθ为航向预估量，u为纵向车速，δ为前轮转角，T为采样周期，a、b分别为车辆质心至前后轴的距离。