CN111158379B - 一种方向盘零偏自学习的无人车轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种方向盘零偏自学习的无人车轨迹跟踪方法，包括道路线型拟合算法、方向盘零偏估计算法和车辆运动学模型控制算法三部分，其中，道路线型拟合算法以车辆前方的一段期望路点序列为输入，拟合出道路线型参数；方向盘零偏估计算法根据当前及历史的道路线型参数估计方向盘零偏；车辆运动学模型控制算法结合道路线型参数、方向盘零偏和车辆运动学模型，计算并输出方向盘转角，用于车辆轨迹跟踪控制。本发明旨在提高无人车轨迹跟踪控制的智能化水平并提高跟踪精度。

Description

一种方向盘零偏自学习的无人车轨迹跟踪方法

技术领域

本发明涉及一种方向盘零偏自学习的无人车轨迹跟踪方法，属于一种无人车运动控制领域。

背景技术

轨迹跟踪方法是无人驾驶系统最基本的组成部分之一，其性能的优劣直接影响车辆的行驶质量。现有的轨迹跟踪方法基本上分为两大类，基于误差的方法和基于模型的方法。其中基于误差的方法直接以跟踪误差和航向误差对方向盘控制量进行校正，不涉及车辆模型，鲁棒性较好，但跟踪精度较低，且对不同的车辆需要整定不同的参数，调试比较复杂。基于模型的方法将车辆的运动学模型融入控制算法的设计中，能够较好的体现车辆的运动特性，跟踪精度较高，但需要事先确定车辆模型参数。其中方向盘零偏是一个比较重要但不容易确定的参数，现有的方法大多是以试验试凑的方式获得该参数，试验时间较长且不容易获得其准确值。

发明内容

本发明为克服现有技术中的不足之处，提供一种方向盘零偏自学习的无人车轨迹跟踪方法，以期在自动驾驶的过程中，能自行学习方向盘零偏参数，以缩短控制参数调试时间，并获得较为准确的方向盘零偏参数，从而提高跟踪精度。

为达到上述发明目的，本发明所采用的技术方案为：

本发明一种方向盘零偏自学习的无人车轨迹跟踪方法的特点是应用于前轮转向的两轴四轮无人驾驶汽车中，并按如下步骤进行：

步骤一、获取车辆前方的一段期望路点序列和车辆当前时刻k的位置、航向、车速；

步骤二、道路线型拟合算法的实现；

步骤2.1、根据车辆当前时刻k的位置和航向，将车辆前方的一段期望路点序列变换到车辆坐标系中，得到车辆坐标系下的期望路点序列的各个坐标；

步骤2.2、在车辆坐标系下，将车辆前方的一段期望路点序列的各个坐标拟合成二次曲线，记为：y＝ax²+bx+c，其中，a、b、c表示道路线型参数，并分别为道路弯曲程度参数、车辆当前时刻k的航向偏离道路切线方向的程度参数、车辆当前时刻k的位置偏离道路的程度参数；

步骤三、方向盘零偏估计算法的实现；

步骤3.1、计算所需记录的帧数n，且n＝s/[v(k)T]后四舍五入取整数得到，其中，s是所记录的历史时间内车辆行驶的距离，v(k)是车辆当前时刻k的速度，T是采样周期；

步骤3.2、记录第k-n+1时刻到当前时刻k的道路线型参数a和c的历史值，记为a(k-n+1)、a(k-n+2)、…a(k)，c(k-n+1)、c(k-n+2)、…c(k)；

步骤3.3、计算所述道路线型参数a的历史值a(k-n+1)、a(k-n+2)、…a(k)中绝对值的最大值a_m；

步骤3.4、计算道路线型参数c的历史值c(k-n+1)、c(k-n+2)、…c(k)的平均值c_m；

步骤3.5、利用式(1)计算车辆当前时刻k的方向盘零偏w₀(k)：

式(1)中，a_t为道路弯曲程度阈值，h为学习速率因子；

步骤四、车辆运动学模型控制算法的实现；

利用式(2)计算车辆当前时刻k的方向盘转角控制量w(k)：

w(k)＝180iarctan[L(2a(k)+2ζωb(k)/v(k)+ω²c(k)/v(k)²)]/π+w₀(k) (2)

式(2)中，a(k)、b(k)、c(k)分别为车辆当前时刻k的道路线型参数；v(k)为车辆当前时刻k的车速，L是两轴四轮无人驾驶汽车的轴距，i是车辆转向系传动比，π是圆周率，ζ和ω是轨迹跟踪闭环系统的阻尼比和自然频率。

相对于现有技术，本发明的有益效果是：

1、本发明所需的输入，包括期望路点序列、车辆当前时刻k的位置、航向、车速，均为常规的无人车轨迹跟踪控制系统输入，与传统方案相比，不会附加任何检测成本。

2、本发明实现了方向盘零偏参数的自动化整定，方向盘零偏参数在自动驾驶过程中由式(1)计算得到，无须事先由人工确定，缩短了无人车控制参数调试的时间。

3、本发明的式(1)采用误差自学习的方法，在整个自动驾驶过程中，不断学习方向盘零偏参数，而人工试验试凑获得方向盘零偏参数只能事先根据多次试验的误差估计一个近似值，无法在自动驾驶的过程中调整，与之相比本发明能够获得更加准确的方向盘零偏参数，从而提高了轨迹跟踪精度。

4、本发明式(1)采用的学习算法，没有迭代计算，与传统迭代优化算法相比，实现简单，运算速度快。

5、由于方向盘零偏对轨迹跟踪最显著的影响体现在直线段，而本发明式(1)按道路弯曲程度分为直线和弯道两种情况，直线段更新方向盘零偏的估计，弯道段不更新，这样避免了弯道误差对估计值的影响，提高了估计的准确性。

6、本发明步骤2.2中得到的a、b、c三个参数，分别蕴含道路在车辆坐标系下的曲率信息，方向信息和位置信息，能够全面的反映道路的线型特征，为方向盘转角控制量的计算提供依据，同时参数c又代表跟踪误差，是估计方向盘零偏的依据，起到一举两得的作用。

7、本发明步骤3.1中，将所需记录的距离s作为可调参数，而不是将所需记录的帧数n作为可调参数，这样避免了低速和高速下记录的距离不一样，使算法适应不同的车速。

8、本发明中用到的可调参数，包括：所记录的历史时间内车辆行驶的距离s，道路弯曲程度阈值a_t，学习速率因子h，轨迹跟踪闭环系统的阻尼比ζ和自然频率ω，物理意义明确，可以为试验调试提供指导。

9、本发明中用到的固定参数，包括：采样周期T，车辆轴距L，车辆转向系传动比i，均是软件或车辆的标称参数，容易获取。

附图说明

图1为本发明方法的流程图；

图2是本发明的方向盘零偏估计仿真效果图。

具体实施方式

下面对本发明的技术方案做进一步的说明。

本实施例中，一种方向盘零偏自学习的无人车轨迹跟踪方法，是应用于前轮转向的两轴四轮无人驾驶汽车中，如图1所示，是按如下步骤进行：

步骤一、获取车辆前方的一段期望路点序列(可以GPS经纬度点列表示或者平面坐标表示，本例采用前者)和车辆当前时刻k的位置、航向、车速；车辆前方的一段期望路点序列可以从事先由人工采集得到的整段轨迹中截取，也可以由感知和规划算法自动生成，本实施例采用第一种方式，从事先由人工采集得到的整段轨迹中截取车辆前方2m～10m的一段路点序列；车辆当前时刻k的位置、航向、车速可以由不同种类的导航定位系统获得，本例由GPS/IMU定位系统得到。

步骤二、道路线型拟合算法的实现；

步骤2.1、根据车辆当前时刻k的位置和航向，将车辆前方的一段期望路点序列变换到车辆坐标系中，得到车辆坐标系下的期望路点序列的各个坐标；车辆坐标系规定为，以车辆后轴中心为原点，x轴垂直于车辆后轴，指向车辆前进方向，y轴指向车辆左侧，计算方法如式(1)：

式(1)中，lat是期望路点的纬度，lng是期望路点的经度，R是地球半径，lat₀是车辆当前时刻的纬度，lng₀是车辆当前时刻的经度，azm₀是车辆当前时刻的航向，X_t、Y_t是中间变量。x、y是期望路点在车辆坐标系下的坐标。

步骤2.2、在车辆坐标系下，将车辆前方的一段期望路点序列的各个坐标拟合成二次曲线，记为：y＝ax²+bx+c，其中，a、b、c表示道路线型参数，并分别为道路弯曲程度参数、车辆当前时刻k的航向偏离道路切线方向的程度参数、车辆当前时刻k的位置偏离道路的程度参数；本实施例采用最小二乘法拟合，设车辆坐标系下车辆前方的一段期望路点序列表示为(x_j,y_j),j＝1,2,…m，构造矩阵：

Y＝[y₁ y₂ … y_m]^T

则有：

[a b c]^T＝(X^TX)^-1(XTY)

步骤三、方向盘零偏估计算法的实现；

步骤3.1、计算所需记录的帧数n，且n＝s/[v(k)T]后四舍五入并取整数得到，其中，s是所记录的历史时间内车辆行驶的距离，v(k)是车辆当前时刻k的速度，T是采样周期；本实施例中，s＝6m,T＝0.1s；

步骤3.2、记录第k-n+1时刻到当前时刻k的道路线型参数a和c的历史值，记为a(k-n+1)、a(k-n+2)、…a(k)，c(k-n+1)、c(k-n+2)、…c(k)；

步骤3.3、计算道路线型参数a的历史值a(k-n+1)、a(k-n+2)、…a(k)中绝对值的最大值a_m；a_m表示所记录的历史时间内车辆行驶过的道路的弯曲程度；

步骤3.4、计算道路线型参数c的历史值c(k-n+1)、c(k-n+2)、…c(k)的平均值c_m；c_m表示车辆对所记录的历史时间内行驶过的期望轨迹的跟踪误差；

步骤3.5、利用式(2)计算车辆当前时刻k的方向盘零偏w₀(k)：

式(2)中，a_t为道路弯曲程度阈值，由于方向盘零偏最显著的影响体现在车辆对直线的跟踪精度上，因此方向盘零偏估计仅在直线轨迹上起作用，式(1)中a_m＞a_t表示期望路点序列弯曲程度较大，不对方向盘零偏进行学习，a_m≤a_t表示期望路点序列弯曲程度较小，需要对方向盘零偏进行学习。本例中a_t＝0.01，h为学习速率因子，h越大，学习速率越快，但容易引起震荡，本例中h＝10；

步骤四、车辆运动学模型控制算法的实现；

利用式(3)计算车辆当前时刻k的方向盘转角控制量w(k)：

w(k)＝180iarctan[L(2a(k)+2ζωb(k)/v(k)+ω²c(k)/v(k)2)]/π+w0(k) (3)

式(3)中，a(k)、b(k)、c(k)分别为车辆当前时刻k的道路线型参数；v(k)为车辆当前时刻k的车速，L是两轴四轮无人驾驶汽车的轴距，i是车辆转向系传动比，π是圆周率，ζ和ω是轨迹跟踪闭环系统的阻尼比和自然频率。可以证明，采用式(3)的控制量后，轨迹跟踪闭环系统的特征方程为：

σ²+2ζωσ+ω²＝0 (4)

式(4)中，σ是拉普拉斯算子，根据自动控制理论，恰当的选取ζ和ω后，轨迹跟踪闭环系统是渐近稳定的，系统稳态误差会趋向于0，本实施例中，ζ＝0.8,ω＝0.6。

仿真结果如图2所示，仿真软件使用simulink，车辆模型为simulink车辆动力学工具箱提供的车辆3自由度动力学模型，模型设定的方向盘零偏为-20°，从图2中可以看到，估计的方向盘零偏在-20°左右浮动，很好的估计了方向盘零偏，表明了算法的有效性。

Claims (1)

1.一种方向盘零偏自学习的无人车轨迹跟踪方法，其特征是应用于前轮转向的两轴四轮无人驾驶汽车中，并按如下步骤进行：

步骤一、获取车辆前方的一段期望路点序列和车辆当前时刻k的位置、航向、车速；

步骤二、道路线型拟合算法的实现；

步骤2.1、根据车辆当前时刻k的位置和航向，将所述车辆前方的一段期望路点序列变换到车辆坐标系中，得到所述车辆坐标系下的期望路点序列的各个坐标；车辆坐标系规定为，以车辆后轴中心为原点，x轴垂直于车辆后轴，指向车辆前进方向，y轴指向车辆左侧，计算方法如式(1)：

式(1)中，lat是期望路点的纬度，lng是期望路点的经度，R是地球半径，lat₀是车辆当前时刻的纬度，lng₀是车辆当前时刻的经度，azm₀是车辆当前时刻的航向，X_t、Y_t是中间变量；x、y是期望路点在车辆坐标系下的坐标；

步骤2.2、在所述车辆坐标系下，将所述车辆前方的一段期望路点序列的各个坐标拟合成二次曲线，记为：y＝ax²+bx+c，其中，a、b、c表示道路线型参数，并分别为道路弯曲程度参数、车辆当前时刻k的航向偏离道路切线方向的程度参数、车辆当前时刻k的位置偏离道路的程度参数；

步骤三、方向盘零偏估计算法的实现；

步骤3.1、计算所需记录的帧数n，且n＝s/[v(k)T]后四舍五入并取整数得到，其中，s是所记录的历史时间内车辆行驶的距离，v(k)是车辆当前时刻k的速度，T是采样周期；

步骤3.2、记录第k-n+1时刻到当前时刻k的道路线型参数a和c的历史值，记为a(k-n+1)、a(k-n+2)、…a(k)，c(k-n+1)、c(k-n+2)、…c(k)；

步骤3.3、计算所述道路线型参数a的历史值a(k-n+1)、a(k-n+2)、…a(k)中绝对值的最大值a_m；

步骤3.4、计算道路线型参数c的历史值c(k-n+1)、c(k-n+2)、…c(k)的平均值c_m；

步骤3.5、利用式(2)计算车辆当前时刻k的方向盘零偏w₀(k)：

式(2)中，a_t为道路弯曲程度阈值，h为学习速率因子；

步骤四、车辆运动学模型控制算法的实现；

利用式(3)计算车辆当前时刻k的方向盘转角控制量w(k)：

w(k)＝180iarctan[L(2a(k)+2ζωb(k)/v(k)+ω²c(k)/v(k)²)]/π+w₀(k) (3)

式(3)中，a(k)、b(k)、c(k)分别为车辆当前时刻k的道路线型参数；v(k)为车辆当前时刻k的车速，L是两轴四轮无人驾驶汽车的轴距，i是车辆转向系传动比，π是圆周率，ζ和ω是轨迹跟踪闭环系统的阻尼比和自然频率。

Images