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

本发明涉及车辆控制技术领域，特别是关于一种车辆倒车路径跟踪控制方法。

背景技术：

无人驾驶车辆路径跟踪控制是指通过无人驾驶车辆的底层控制器，控制车辆转向角度，使车辆始终沿着期望的路径行驶，无人驾驶车辆的路径跟踪能力是无人驾驶车辆安全行驶的重要保证。路径跟踪控制的目标是减小车辆在行驶过程中与参考路径的偏差，同时保证转向平稳。

现在关于无人驾驶车辆路径跟踪控制的研究主要集中在前进控制，对于无人驾驶车辆的倒车控制鲜有专利发表，在现有的几项专利中，主要关注于自动倒车装置以及倒车路径生成。如在公开号为cn107672586a的专利申请中，提出一种能够实现自动转向自动刹车的倒车辅助系统，通过手机或者遥控手柄来控制车辆实现倒车指令。但是该方法需要用户手动操作车辆，操作繁琐、耗时较长，并且在用户缺乏经验的条件下很难保证倒车的安全性。在公开号为cn109657585a的专利申请中，提出了一种图像处理的方法，通过搜索起点和终点的非障碍单元来获得倒车路径。该方法不需要用户手动操作，更加方便快捷，但是并没有指出如何在倒车过程中控制车辆来跟踪期望倒车路径，方法并不完善。在公开号为cn108437981a的专利申请中，提出了一种倒车过程中的路径跟踪方法，利用车辆当前位置与期望位置的偏差，来修正倒车过程中方向盘的转角。但是该方法仅利用当前车辆与路径的偏差作为输入条件，并未充分利用整段路径的位置和方向信息。

因此，现有的各种无人驾驶车辆倒车控制方法中：一些方法需要用户进行手动操作，导致倒车过程繁琐且缺乏安全性；一些方法只关注于倒车路径的生成，并没有解决倒车过程中的控制问题；一些方法考虑了倒车过程中的路径跟踪问题，但是并没有完全利用整段路径的信息，在跟踪精度方面有所欠缺。

技术实现要素：

本发明的目的在于提供一种车辆倒车路径跟踪控制方法来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种车辆倒车路径跟踪控制方法，该方法包括：

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提供的新的车辆状态刷新问题，如此循环直至到达路径终点。

进一步地，s5中的所述目标函数表示为式(5)：

式(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)-xp(k))*sin(θp(k))+(y(k+t|t)-yp(k))(6)

*cos(θp(k))

式(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个预瞄点的航向角。

进一步地，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+t+1|t)为在t时刻预测的k+t+1时刻的前轮转向角；

l为轴距；

k为第k个预测步；

δmax为相邻时刻内车辆的前轮转角的最大变化量；

δmax为车辆前轮最大转角。

进一步地，式(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)：

efa(t)＝-(xr(t)-xp(0))*sin(θp(0))+(yr(t)-yp(0))(14)

*cos(θp(0))

式(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)的迭代初始值。

进一步地，s3中的“最近路径点p0”的方法包括：

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

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

进一步地，s1采用三次样条插值方法，其具体包括如下步骤：

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

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

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

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

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

本发明由于采取以上技术方案，其具有以下优点：1.能够实现车辆的全自动倒车行驶，而不需要用户进行手动操作，更加简单便捷，节省时间。2.同时在选取预瞄点的个数上，相比于传统的基于预瞄的控制方法只能选取一个预瞄点，本方法采用了多点预瞄的方法，多个预瞄点相对于单预瞄点，能够预测更长的步数与距离，能兼顾整条路径的跟踪误差，从而提高了跟踪的精度，同时也保证了在控制过程中的舒适性，不会产生控制量的突变。3.将预瞄控制和模型预测控制相结合，能够顾及由于模型失配、时变、干扰等引起的不确定性，及时进行弥补，始终把最新的优化建立在实际的基础上，保持实际上的最优。

附图说明

图1为本发明实施例提供的车辆倒车路径跟踪控制方法的流程示意图；

图2为图1中的步骤s1提供的三次样条插值法的流程示意图；

图3为本发明实施例提供的路径跟踪示意图；

图4为图1中的步骤s3提供的获取最近路径点的原理示意图；

图5为图1中的步骤s3提供的路径跟踪控制器的结构示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明实施例所提供的车辆倒车路径跟踪控制方法包括以下步骤：

s1，对已有的参考路径点进行插值，获得一条路径点更加密集的新参考路径。其中，“参考路径”比如可以通过用gps系统记录路径的方式获得。但是，获得的该参考路径有可能会是离散的稀疏路径点，为了获得更密的路径点以保证跟踪的精度，本实施例使用三次样条插值方法对参考路径上起始路径点与目标路径点之间的所有路径点进行插值处理，以获得大地坐标系下由起始点至目标点之间的参考路径上各路径点的坐标(x，y)及航向角θp(如图2所示)。三次样条插值方法具体包括如下步骤：

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

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

s13，根据s12计算得到的s11中的两个所述路径点的新坐标和航向角，计算得到该两个路径点之间的拟合三次样条插值曲线表达式f(x)＝a(x-x′i-1)3+b(x-x′i-1)²+c(x-x′i-1)+d，式中，i-1和i分别表示s11中的两个所述路径点的序号，x′i-1表示第i-1个点在旋转后的坐标系下的横坐标，a、b、c、d为旋转坐标系下s11中的两个所述路径点之间的三次样条插值曲线表达式的系数，即为s13待计算的参数。

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

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

s2，获得车辆状态信息。图3示意了车辆的位置与参考路径，如图3所示，通过车辆上安装的gps传感器、惯性测量单元(imu)以及安装的其它传感器，获得当前观测时刻t下的车辆后轴中心的坐标(xr(t)，yr(t))和速度v(t)、车身航向角和前轮转向角δ(t)。

s3，计算s1获得的新参考路径上与车辆后轴中心的最近路径点。

如图4所示，遍历新参考路径上的所有路径点pi(xi，yi)，利用式(1)计算路径点pi(xi，yi)与当前时刻t下的车辆后轴中心(xr(t)，yr(t)的距离平方di，并找出距离平方di最小的路径点，该路径点记为最近路径点p0：

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

s4，在s1获得的新参考路径上，以s3找到的最近路径点p0为起点，根据s2获取的车辆运动速度v(t)，向后搜索n个预瞄点。“向前”理解为朝车头的前方的方向，“向后”则相反，为倒车的方向，即朝车尾的方向。

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

以最近路径点p0为起点，同时，以v(t)δt为搜索距离，向后搜索n个路径点作为预瞄点(xp(k)，yp(k))，k＝1，2，...，n。也就是说，第一个预瞄点(xp(1)，yp(1))与最近路径点p0沿新参考路径的距离为|v(t)δt|，第二个预瞄点(xp(2)，yp(2))与第一个预瞄点(xp(1)，yp(1))沿新参考路径的距离为|v(t)δt|，以此类推，第i个预瞄点(xp(j)，yp(j))与第i-1个预瞄点(xp(j-1)，yp(j-1))沿新参考路径的距离为v(t)δt，第n个预瞄点(xp(n)，yp(n))与第n-1个预瞄点(xp(n-1)，yp(n-1))沿新参考路径的距离为|v(t)δt|，δt为离散的时间步长。

上述实施例中，当搜索到达新参考路径终点时，停止搜索。当然，也可以根据设定的预测步数进行控制搜索操作，比如：当搜索到达设定的最大预测步数n时，停止搜索。

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

根据车辆运动学公式，所述预测模型设置为式(2)至式(4)：

式(2)至式(4)中：

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

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

y(k+t|t)为在t时刻预测到的k+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时刻的前轮转向角；

l为车辆的轴距；

k为第k个预测步。

根据车辆跟踪精度和稳定的需求，目标函数设置为式(5)：

式(5)中：

q为权重系数，其需要根据实际试验以及仿真得到，本实施例中取q＝150。

e(k+t|t)为在t时刻预测到的k+t时刻的车辆后轴中心与在t时刻搜索的第k个预瞄点切线的距离，即图3中所示距离efa，该距离表示车辆与参考路径的横向偏差，优化过程中应尽可能地使其e(k+t|t)取最小值，来保证车辆跟踪的精度。e(k+t|t)计算公式为式(6)：

e(k+t|t)＝-(x(k+t|t)-xp(k))*sin(θp(k))+(y(k+t|t)-yp(k))(6)

*cos(θp(k))

式(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个预瞄点的航向角。

δδ(k+t|t)为在t时刻预测的k+1+t时刻的车辆前轮转角与上一预测时刻k+t时刻的车辆前轮转角之差，引入该目标，是为了在保证跟踪精度的情况下限制前轮转角的变化幅度，从而防止方向盘产生较大抖动，保证跟踪的稳定性。

δδ(k+t|t)计算公式为式(7)：

δδ(k+t|t)＝δ(k+1+t|t)-δ(k+t|t)(7)

式(7)中：

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

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

由于车辆作为一个机械系统，存在一些结构上的约束，方向盘转角不能超过允许的最大转角，同时车辆作为运载工具，需要保证乘客的乘坐舒适性，方向盘转角变化率不宜过大，鉴于此，式(6)的可行状态与控制变量空间约束设置为式(8)至式(12)：

-δmax≤δ(k+1+t|t)-δ(k+t|t)≤δmax(11)

-δmax≤δ(k+t|t)≤δmax(12)

式(8)至式(12)中，δmax为相邻时刻内前轮转角的最大变化量，δmax为车辆前轮最大转角，机械结构的限制。

在每一个控制周期内求解优化问题：

根据前面步骤建立的目标函数和约束条件，模型预测控制器需要在每一个周期内求解的带约束优化问题如下所示：

式(13)中，minj(k)是式(5)提供的目标函数的最小值。

在求解优化问题之前，先使用stanley方法通过车辆运动学模型仿真计算出预测时域内的前轮转向角的初始值，具体计算方法如下所示：

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)：

efa(t)＝-(xr(t)-xp(0))*sin(θp(0))+(yr(t)-yp(0))(14)

*cos(θp(0))

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

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

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

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

式(16)中：

k为权重系数，权重系数需要根据不同的情况来调整，具体给出的方法是靠测试和仿真得到一个较优的权重系数，本实施例中取k＝1；

θe(t)为期望转角δ′(t)的角度误差部分，计算如公式(15)所示。

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

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

初始值的引用，能提高求解过程中的收敛速度以及获得最优解的成功率，并且避免问题陷入局部最优。

根据步骤s51至s56求解得到的预测时域内的前轮转向角的初始值，对式(13)提供的优化问题进行求解，并将解作为本实施例所需要的前轮转角的控制量，即n个预瞄点中每一个预瞄点(xp(k)，yp(k))对应的期望前轮转角，n个预瞄点组成的所述期望前轮转角构成式(20)所表示的最优控制序列：

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

上述最优控制序列中，将其中的第一个元素用作n步中该步的控制量。

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

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。