[发明专利]一种考虑路径曲率的新型几何学路径跟踪算法

权利要求书

1.一种考虑路径曲率的新型几何学路径跟踪算法，其特征在于，包括以下步骤：

S1.利用GPS以固定频率采集路径离散坐标点得到参考路径点集P；

S2.查找离当前汽车位置最近的路径点p_i(p_i∈P)；

S3.利用最小二乘法计算路径点p_i处对应的路径曲率ρ_i；

S4.根据阿克曼转向几何模型计算ρ_i对应的前轮转角θ_ρi以及前轮方向角

S5.计算汽车当前位置与路径点p_i处的航向偏差yawErr、横向偏差latErr；

S6.根据前轮方向角汽车当前航向角yaw_vehicle、航向偏差yawErr和横向偏差latErr设计路径跟踪控制器：其中为前轮期望方向角，k₁为横向偏差校正系数，k₂为航向偏差校正系数，求得前轮期望转角

2.根据权利要求1所述的一种考虑路径曲率的新型几何学路径跟踪算法，其特征在于：所述的步骤S1中，

所述GPS定位精度为厘米级，所述的参考路径点集P由平面直角表示，是通过GPS输出的经纬度进行坐标转换为通用横轴墨卡托坐标所得，目的是便于计算路径曲率以及将位于全局坐标系X_global-Y_global的路径点转换到移动的车体坐标系X_v-X_v。

3.根据权利要求1所述的一种考虑路径曲率的新型几何学路径跟踪算法，其特征在于：所述步骤S3中，

利用最小二乘法计算路径点p_i(p_i∈P)处对应的路径曲率ρ_i，具体过程为：

分别选取点p_i(x_i,y_i)前后各N个路径点，仿真证明N＝3时有较好的拟合结果，下文均取N＝3进行描述，7个(2N+1)路径点分别为p_i(x_i-3,y_i-3)，p_i(x_i-2,y_i-2)，p_i(x_i-1,y_i-1)，p_i(x_i,y_i)，p_i(x_i+1,y_i+1)，p_i(x_i+2,y_i+2)，p_i(x_i+3,y_i+3)，其中i3；

利用最小二乘法进行三次曲线y＝a_i+b_i·x+c_i·x²+d_i·x³拟合，即根据A·x＝b求矩阵b，由于上述超定方程组无解，所以利用最小二乘原理求近似解由得

其中：

得到三次曲线y＝a_i0+b_i0·x+c_i0·x²+d_i0·x³，

再利用曲率公式计算点p_i(x_i,y_i)点的曲率，得ρ_i。