一种车辆倒车路径跟踪控制方法与流程

技术特征：

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

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

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

s3，遍历s1获得的新参考路径上的所有路径点pi(xi，yi)，找出最近路径点p0；

s4，在s1获得的新参考路径上，以s3找到的最近路径点p0为起点，根据s2获取的车辆运动速度v(t)，向后搜索n个预瞄点；

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

以最近路径点p0为起点，同时，以|v(t)δt|为搜索距离，沿新参考路径向后搜索n个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，...，n，δt为离散的时间步长，当搜索到达新参考路径终点时或当搜索到达设定的最大预测步数n时，停止搜索；

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

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

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

2.如权利要求1所述的车辆倒车路径跟踪控制方法，其特征在于，s5中的所述目标函数表示为式(5)：

式(5)中：

q为权重系数；

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

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

式(6)中：

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

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

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

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

θp(k)为s4中搜索得到的第k个预瞄点的航向角。

3.如权利要求1或2所述的车辆倒车路径跟踪控制方法，其特征在于，s5在每一个控制周期内求解如下式(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为车辆前轮最大转角。

4.如权利要求3所述的车辆倒车路径跟踪控制方法，其特征在于，式(13)所示的优化问题求解之前，预测时域内的前轮转向角的初始值的具体计算方法包括：

s51.利用步骤s2获得的车辆状态信息，取与步骤s4中相同的步长δt，以步骤s3和步骤s4中得到的最近路径点p0以及搜索到的前n-1个预瞄点作为stanley方法对应时刻的最近路径点(xp(k)，yp(k))，k＝1，2，...，n；

s52.计算当前t时刻车辆后轴中心(xr(t)，yr(t))与对应的最近路径点p0(xp(0)，yp(0))的横向偏差efa(t)：

式(14)中，θp(0)为路径点p0的航向角；

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

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

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

式(16)中：

k为权重系数；

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

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

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

5.如权利要求1或2所述的车辆倒车路径跟踪控制方法，其特征在于，s3中的“最近路径点p0”的方法包括：

利用式(1)计算路径点pi(xi，yi)与当前时刻t下的车辆后轴中心(xr(t)，yr(t))的距离平方di，并找出距离平方di最小的路径点，该路径点记为最近路径点p0：

di＝(xr(t)-xi)²+(yr(t)-yi)²(1)。

6.如权利要求1或2所述的车辆倒车路径跟踪控制方法，其特征在于，s1采用三次样条插值方法，其具体包括如下步骤：

s11，在参考路径上选定一个路径点，根据大地坐标系下与该选定的路径点相邻的路径点的横摆角信息，计算出坐标系旋转角度；

s12，对大地坐标系进行旋转，旋转角度为步骤s11计算得到的坐标系旋转角度，并计算旋转后的s11中的两个所述路径点的新坐标和航向角；

s13，根据s12计算得到的s11中的两个所述路径点的新坐标和航向角，计算得到该两个路径点之间的拟合三次样条插值曲线表达式；

s14，对s13得到的三次样条插值曲线表达式的横坐标进行等间距离散，获得高密度的插值路径点，进而获得新参考路径。其中，各插值路径点的一阶导数即为该点航向角信息；

s15，对s14的插值路径点进行坐标系旋转，以还原至大地坐标系中，得到大地坐标系下原相邻路径点及其间插值路径点的坐标及航向角信息，其中：旋转方向与s12的旋转方向相反，旋转的角度大小为s11计算得到的坐标系旋转角度。