[发明专利]一种车辆倒车路径跟踪控制方法

权利要求书

1.一种车辆倒车路径跟踪控制方法，其特征在于，包括：

S1，对已有的参考路径点进行插值，获得一条路径点更加密集的新参考路径；

S2，获得车辆状态信息，倒车时车辆的运动速度为v(t)，v(t)取负值；

S3，遍历S1获得的新参考路径上的所有路径点p_i(x_i，y_i)，找出最近路径点p₀，x_i为新参考路径上的所有路径点p_i的横坐标，y_i为新参考路径上的所有路径点p_i的纵坐标；

S4，在S1获得的新参考路径上，以S3找到的最近路径点p₀为起点，根据S2获取的车辆状态信息中的车辆运动速度v(t)，向后搜索N个预瞄点；

S4中的“向后搜索N个预瞄点”具体包括如下方法：

以最近路径点p₀为起点，同时，以|v(t)ΔT|为搜索距离，沿新参考路径向后搜索N个路径点作为预瞄点(x_p(k)，y_p(k))，k＝0，2，...，N-1，ΔT为离散的时间步长，当搜索到达新参考路径终点时或当搜索到达设定的最大预测步数N时，停止搜索；

S5，构建由式(2)至式(4)表示的预测模型、下式(5)所示的目标函数以及系统约束，根据S2获取的当前车辆状态信息和预测模型，预测车辆未来动态，在线求解满足所述目标函数和约束条件的优化问题，获取S4搜索得到的N个预瞄点中每一个预瞄点对应的期望前轮转角，N个预瞄点组成的所述期望前轮转角构成式(20)所表示的最优控制序列；

式(5)中：

Q为权重系数；

Δδ(k+t|t)为在t时刻预测的k+1+t时刻的车辆前轮转向角与上一预测时刻k+t时刻的车辆前轮转向角之差；

e(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心与在t时刻搜索的第k个预瞄点切线的距离，其表示为式(6)：

e(k+t|t)＝-(x(k+t|t)-x_p(k))*sin(θ_p(k))+(y(k+t|t)-y_p(k))*cos(θ_p(k))

式(6)中：

x(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的横坐标；

y(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心点的纵坐标；

x_p(k)为S4中搜索得到的第k个预瞄点的横坐标；

y_p(k)为S4中搜索得到的第k个预瞄点的纵坐标；

θ_p(k)为S4中搜索得到的第k个预瞄点的航向角；

式(20)中，表示在t时刻预测的k+t时刻的前轮转角；

在每一个控制周期内求解如下式(13)所示的优化问题：

式(13)中：

minJ(k)是式(5)提供的目标函数的最小值；

x(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的横坐标；

y(k+1+t|t)为在t时刻预测到的k+1+t时刻的车辆后轴中心点的纵坐标；

为在t时刻预测到的k+t时刻的车身航向角；

为在t时刻预测到的k+1+t时刻的车身航向角；

v(t)为S2获得的车辆速度；

Δt为每个预测步长的时间；

δ(k+t|t)为在t时刻预测的k+t时刻的前轮转向角；

δ(k+1+t|t)为在t时刻预测的k+t+1时刻的前轮转向角；

L为车辆的轴距；

k为第k个预测步；

Δ_max为相邻时刻内车辆的前轮转向角的最大变化量；

δ_max为车辆前轮最大转向角；

S6，使用控制车辆直到下一采样时刻到达，下一观测时刻t+1到达时，重复步骤S2至S5，利用S2提供的新的车辆状态刷新问题，如此循环直至到达路径终点；

式(13)所示的优化问题求解之前，预测时域内的前轮转向角的初始值的具体计算方法包括：

S51.利用步骤S2获得的车辆状态信息，取与步骤S4中相同的步长ΔT，以步骤S3和步骤S4中得到的最近路径点p₀以及搜索到的前N-1个预瞄点作为Stanley方法对应时刻的最近路径点(x_p(k)，y_p(k))，k＝1，2，...，N；

S52.计算当前t时刻车辆后轴中心(x_r(t)，y_r(t))与对应的最近路径点p₀(x_p(0)，y_p(0))的横向偏差e_fa(t)：

式(14)中，θ_p(0)为路径点p₀的航向角，x_r(t)为当前t时刻车辆后轴中心的横坐标，y_r(t)为当前t时刻车辆后轴中心的纵坐标；

S53.计算当前t时刻车身航向角与最近路径点航向角的角度误差θ_e(t)：

式(15)中，为车身航向角，θ_p(0)为路径点p₀的航向角；

S54.计算当前t时刻前轮的期望转角δ′(t)：

式(16)中：

K为权重系数；

θ_e(t)为期望转角δ′(t)的角度误差部分；

S55.利用式(16)求得的δ′(t)和已知的车辆后轴中心坐标(x_r(t)，y_r(t))、速度v、车身航向角前轮转向角δ(t)信息以及车辆运动学公式(17)至式(19)，得到下一时刻t+ΔT的车辆状态信息：

S56.以式(17)至式(19)所得到的车辆状态信息更新车辆状态，以t+ΔT时刻作为当前时刻，选取第一个预瞄点(x_p(1)，y_p(1))作为最近路径点，重复步骤S52至S55，递推出之后每一个预瞄点对应的期望转角δ′(t+k*ΔT)，并将其作为公式(13)所示优化问题中δ(k+t|t)的迭代初始值。