一种无人自主小车的轨迹跟踪控制方法与流程

1.本发明属于无人驾驶汽车轨迹跟踪控制技术领域，具体涉及一种无人自主小车的轨迹跟踪控制方法。


背景技术：

2.近几年来，随着无人驾驶技术的快速发展和广泛应用，有关无人驾驶的关键技术吸引了大量国内外学者的青睐和研究。一方面，对无人驾驶技术的研究为现代智能汽车的开发提供了理论基础，另一方面，日益成熟的无人驾驶技术能大大提升车辆乘坐舒适性和汽车行驶安全性，同时为智能交通系统的发展和应用提供技术支撑。
3.pid控制算法就在汽车工业界广泛使用，但是由于无人驾驶汽车行驶过程的内部参数(速度，横摆角，转向角)和外部参数(道路曲率，风阻系数)一直在变化，因此调节参数的过程就会非常复杂，适应性就会很差，很难在实际的无人驾驶车辆中广泛的使用。在比如说一些新兴的算法，诸如强化学习算法，自适应神经网络算法，大多数都是通过预设好道路的条件或者提前获得训练数据集来进行无人驾驶车辆的轨迹研究，根本就无法满足无人驾驶车辆的实施控制需求。


技术实现要素：

4.本发明的目的是提供一种无人自主小车的轨迹跟踪控制方法，能有效提升无人驾驶车辆的轨迹跟踪性能。
5.本发明所采用的技术方案是，一种无人自主小车的轨迹跟踪控制方法，具体按照以下步骤实施：
6.步骤1、建立无人驾驶汽车的二自由度运动学模型；
7.步骤2、构建基于非线性模型预测控制的轨迹跟踪控制器，在控制器的预测模型中加入单步欧拉法；
8.步骤3、设计非线性模型预测控制器的约束方程，构件基于非线性模型预测控制器的控制轨迹输出和参考轨迹差值最小为目标建立目标函数；
9.步骤4、根据目标函数求解下一个时刻控制系统的输入。
10.本发明的特点还在于：
11.步骤1中无人驾驶汽车的二自由度运动学模型即为无人驾驶车辆的运动学状态空间方程。
12.步骤1具体过程为：
13.根据无人驾驶车辆的运动学关系：
[0014][0015]
该无人驾驶车辆的前后运动学约束分别为：
[0016]
[0017][0018]
式中，a和b分别为车辆的前轮轴心和后轮轴心；v
f
和v
r
分别表示前后轮的速度；l为车辆的轴距；δ
f
为前轮偏角；为车辆横摆角；ω为车身横摆率；
[0019]
通过公式(1)和(2)，得到：
[0020][0021]
前轮轴心和后轮轴心的运动学关系满足：
[0022][0023]
将公式(4)代入(5)的导数中：
[0024][0025]
将公式(6)代入公式(3)：
[0026][0027]
进而可得：
[0028][0029]
通过联合公式(4)和公式(7)：
[0030][0031]
该无人驾驶车辆的运动学状态空间方程为：
[0032][0033]
公式(10)被改写为：
[0034][0035]
步骤2具体过程为：
[0036]
将式(11)重构为非线性函数表达式为：
[0037]
[0038]
定义状态向量,即ξ
kout
；u(t)作为控制输入向量,即u
kin
,从而可得：
[0039][0040][0041]
将方程(12)的离散化得到：
[0042][0043]
引入采样时间t，采用一步欧拉法将式(15)的预测模型描述为：
[0044][0045]
其中k表示某一时刻。
[0046]
步骤3设计非线性模型预测控制器的约束方程具体过程为：
[0047]
u
min
(k)≤u(k)≤u
max
(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0048]
δy
min
≤y
‑
y
ref
≤δy
max
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0049]
y
ref
参考道路轨迹，δy
min
和δy
max
之间偏差的最小和最大错误y(k)和y
ref
。
[0050]
步骤3建立目标函数具体过程为：
[0051]
n
p
表示控制输出预测域，n
c
表示控制输出控制域，根据方程(16)的控制时域n
c
表示如下，n
p
＞n
c
，
[0052][0053]
该控制系统输出y(k)和控制输入u(k)如下：
[0054]
y(k 1)＝[y(k 1)，y(k 2)]，
…
，y(k np)]
ꢀꢀꢀꢀꢀꢀ
(20)
[0055]
u(k i)＝[u(k 1)，u(k 2)]，
…
，u(k nc)]
ꢀꢀꢀ
(21)
[0056]
参考轨迹应离散如下：
[0057]
y
ref
(k 1)＝[y
ref
(k 1)，y
ref
(k 2)，
…
，y
ref
(k np)]
t
ꢀꢀ
(22)
[0058]
则目标函数表示为：
[0059]
min||y(k 1)
‑
y
ref
(k 1)||
ꢀꢀꢀꢀꢀꢀ
(23)。
[0060]
步骤4具体过程为：
[0061]
取非线性模型预测控制器的性能函数为j
cost
(k)，权重因此q和r，其中q是一个(nc*np)*(nc*np)的单位矩阵，r是一个(nc*np)的单位矩阵*5得到的矩阵，将式(23)转化为线性二次调节器控制，具体过程为：
[0062][0063]
期望非线性模型预测控制器的优化公式表示为：
[0064]
minj
cost
(k)
ꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0065]
结合非线性模型预测控制器的约束方程，非线性模型预测控制器设计的非线性优化表述为：
[0066][0067]
y(k i
‑
1)＝g[x(k i
‑
1)，u(k i
‑
1)]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)
[0068]
u
min
(k)≤u(k i)≤u
max
(k)
ꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0069]
δy
min
≤y(k i)
‑
y
ref
(k i)≤δy
max
ꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0070]
进而得到无人自主小车的控制输入序列表示为：
[0071]
u(k i)＝[u(k 1)，u(k 2)，
…
u(k n
c
)]
ꢀꢀꢀꢀ
(30)。
[0072]
本发明的有益效果是：
[0073]
本发明一种无人自主小车的轨迹跟踪控制方法，提出的非线性模型预测控制器能够更加快速的跟踪上参考轨迹，并在跟踪的过程中有更小的波动误差和更加平缓的趋势；本发明的控制方法能够有效提高车辆的行驶稳定性和安全性，对应用于无人驾驶车辆的轨迹跟踪控制具有重大意义。
附图说明
[0074]
图1为本发明一种无人自主小车的轨迹跟踪控制方法工作流程图；
[0075]
图2为本发明中的二自由度运动学模型；
[0076]
图3为本发明非线性模型预测控制器的系统原理图；
[0077]
图4为使用本发明方法单圆轨迹跟踪结果图；
[0078]
图5为使用本发明方法轨迹跟踪误差图；
[0079]
图6为使用本发明方法双圆轨迹跟踪结果图；
[0080]
图7为使用本发明方法轨迹跟踪误差图；
[0081]
图8为使用本发明方法实验系统图；
[0082]
图9为使用本发明方法户外实地测试图；
[0083]
图10为使用本发明方法不规则道路跟踪结果图；
[0084]
图11为使用本发明方法轨迹跟踪误差图；
[0085]
图12为使用本发明方法双圆道路跟踪结果图；
[0086]
图13为使用本发明方法轨迹跟踪误差图。
具体实施方式
[0087]
下面结合附图及具体实施方式对本发明进行详细说明。
[0088]
轨迹跟踪问题是无人驾驶车辆开发中的关键技术难题之一，精确的轨迹跟踪已经成为汽车工业领域和学术领域公认的实现自主车辆运动控制的核心技术之一。相较于当下其他控制算法，模型预测算法具有模型预测、滚动优化和在线校正的鲜明特色。为了缓解当前遇到的复杂道路交通问题，发展无人驾驶汽车和智能交通系统成为汽车行业的主流业务。无人驾驶汽车是一项非常综合的研究领域，它包含了车辆、机械、计算机、材料等多个领域和多个学科的综合交叉，而无人驾驶领域内也主要包含了环境感知、路径规划和跟踪控制等三个研究方向。
[0089]
(1)环境感知模块：环境感知就是无人驾驶汽车的“眼睛”，它通过各类传感器来获取周围环境(路面、静态物体、动态物体)的信息，并将各类信息进行融合处理传递给处理器。一般使用的传感器有激光雷达传感器、毫米波雷达传感器、红外传感器和视觉传感器等，对汽车环境精准的识别是自动驾驶汽车的第一步，也是无人驾驶技术的关键一步。
[0090]
(2)路径规划模块：路径规划是在已知汽车的起点和终点的情况下在地图上优化出一条最优路径出来。全局路径规划首先需要构造汽车周围的环境地图，然后在地图上进行合理的路径规划，因为地图是先前构造的，就需要环境感知模块对周围的环境进行实时感知，路径规划的算法有迪杰斯特拉算法，a*算法等。
[0091]
局部路径规划是在汽车周围的局部环境中进行的，通常是根据当前的车速、周围的障碍物等周边环境以及全局路径生成一条平顺、可行的运动轨迹。
[0092]
(3)跟踪控制模块：跟踪控制是无人驾驶汽车的输出端，汽车通过已知的路径计算出当前状态需要的方向盘转角和速度，使得汽车能够按照预设的路径进行安全平稳的行驶。常见的方法有比例积分微分控制算法、线性二次调节算法和模型预测控制算法等。
[0093]
模型预测控制是一种可以依靠数学模型进行迭代优化计算的复杂控制器，它是在有效的时间内利用优化算法从而对输入模型的控制序列进行优化计算，从而产生一组新的控制控制序列，但模型预测控制器一次只对控制序列的第一组控制输入进行优化，然后再重复该步骤，循环迭代计算出新的被控序列。模型预测控制主要有三个关键环节：模型预测、滚动优化、反馈校正。它的优势在于具有控制效果好、鲁棒性强等优点，研究对象可以为线性系统或非线性系统，并能方便的处理被控变量和操纵变量中的各种约束。
[0094]
本发明一种无人自主小车的轨迹跟踪控制方法，如图1所示，具体按照以下步骤实施：
[0095]
步骤1、建立无人驾驶汽车的二自由度运动学模型；无人驾驶汽车的二自由度运动学模型即为无人驾驶车辆的运动学状态空间方程。具体过程为：
[0096]
如图2所示，根据无人驾驶车辆的运动学关系：
[0097][0098]
该无人驾驶车辆的前后运动学约束分别为：
[0099][0100][0101]
式中，a和b分别为车辆的前轮轴心和后轮轴心；v
f
和v
r
分别表示前后轮的速度；l为车辆的轴距；δ
f
为前轮偏角；为车辆横摆角；ω为车身横摆率；
[0102]
通过公式(1)和(2)，得到：
[0103][0104]
前轮轴心和后轮轴心的运动学关系满足：
[0105][0106]
将公式(4)代入(5)的导数中：
[0107][0108]
将公式(6)代入公式(3)：
[0109][0110]
进而可得：
[0111][0112]
通过联合公式(4)和公式(7)：
[0113][0114]
该无人驾驶车辆的运动学状态空间方程为：
[0115][0116]
公式(10)被改写为：
[0117][0118]
步骤2、构建基于非线性模型预测控制的轨迹跟踪控制器，在控制器的预测模型中加入单步欧拉法如图3所示；具体过程为：
[0119]
为便于该无人驾驶汽车的预期非线性预测模型的建立，可将式(11)重构为非线性函数表达式为：
[0120][0121]
定义状态向量,即ξ
kout
；u(t)作为控制输入向量,即u
kin
,从而可得：
[0122][0123][0124]
为了将nmpc控制器设计表示为一个非线性优化问题，将方程(12)的离散化得到：
[0125][0126]
通过引入采样时间t，采用一步欧拉法将式(15)的预测模型描述为：
[0127][0128]
其中k表示某一时刻。
[0129]
步骤3、设计非线性模型预测控制器的约束方程，构件基于非线性模型预测控制器的控制轨迹输出和参考轨迹差值最小为目标建立目标函数；
[0130]
设计非线性模型预测控制器的约束方程具体过程为：
[0131]
u
min
(k)≤u(k)≤u
max
(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0132]
δy
min
≤y
‑
y
ref
≤δy
max
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0133]
y
ref
参考道路轨迹，δy
min
和δy
max
之间偏差的最小和最大错误y(k)和y
ref
，可以调整轧制过程中优化。
[0134]
建立目标函数具体过程为：
[0135]
n
p
表示控制输出预测域，n
c
表示控制输出控制域，根据方程(16)的控制时域n
c
表示如下，n
p
＞n
c
，
[0136][0137]
该控制系统输出y(k)和控制输入u(k)如下：
[0138]
y(k 1)＝[y(k 1)，y(k 2)]，
…
，y(k np)]
ꢀꢀꢀꢀꢀꢀ
(20)
[0139]
u(k i)＝[u(k 1)，u(k 2)]，
…
，u(k nc)]
ꢀꢀꢀꢀ
(21)
[0140]
预期nmpc设计的主要目标n
p
是减少跟踪误差控制输出序列和参考轨迹之间的序列。为此，参考轨迹应离散如下：
[0141]
y
ref
(k 1)＝[y
ref
(k 1)，y
ref
(k 2)，
…
，y
ref
(k np)]
t
ꢀꢀꢀ
(22)
[0142]
则目标函数表示为：
[0143]
min||y(k 1)
‑
y
ref
(k 1)||
ꢀꢀꢀꢀꢀ
(23)。
[0144]
步骤4、根据目标函数求解下一个时刻控制系统的输入；具体过程为：
[0145]
为了求出式(23)所述跟踪误差系统的最小解，需要将式(23)进一步转化为线性二
次调节器(lqr)控制问题。
[0146]
为此，取非线性模型预测控制器的性能函数为j
cost
(k)，权重因此q和r，其中q是一个(nc*np)*(nc*np)的单位矩阵，r是一个(nc*np)的单位矩阵*5得到的矩阵，将式(23)转化为线性二次调节器控制，具体过程为：
[0147][0148]
期望非线性模型预测控制器的优化公式表示为：
[0149]
minj
cost
(k)
ꢀꢀꢀꢀꢀꢀꢀ
(25)
[0150]
求解优化问题时，结合非线性模型预测控制器的约束方程，非线性模型预测控制器设计的非线性优化表述为：
[0151][0152]
y(k i
‑
1)＝g[x(k i
‑
1)，u(k i
‑
1)]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)
[0153]
u
min
(k)≤u(k i)≤u
max
(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0154]
δy
min
≤y(k i)
‑
y
ref
(k i)≤δy
max
ꢀꢀꢀꢀꢀꢀꢀꢀ
(29)
[0155]
进而得到无人自主小车的控制输入序列表示为：
[0156]
u(k i)＝[u(k 1)，u(k 2)，
…
u(k n
c
)]
ꢀꢀꢀꢀꢀ
(30)。
[0157]
为了验证本发明构建的算法的有效性，在matlab中编写的code中，加入所设计的不同道路轨迹的函数，进而结合相应道路环境下的仿真验证，然后搭建自主的实验平台，在实际中测试所涉及控制器的有效性。
[0158]
根据无人驾驶车辆实验需求搭建了户外场地实验平台，该平台由一辆成例缩小的真实赛车、传感器、单片机、控制器以及上位机组成。基于arduino开发板和matlab编程实现所设计控制器的开发。
[0159]
本发明为了验证所构建的控制器的有效性，分别在matlab和实际的实验平台中对所设计的控制器进行了验证，结果表明该策略能够很好的处理非线性的道路轨迹，提高了跟踪精度，改善了行驶稳定性。图4仿真下单圆轨迹的跟踪响应曲线；从图5可以看出该无人驾驶车辆的三种状态对于两种mpc的算法总体上都有趋近于零的趋势，然而所提出的控制器(nmpc)的跟踪误差总体波动更小，达到稳定时候的时间更短。从图6、图7可以看出，本发明提出的控制器在双圆环仿真下可以得到同样的结果，总体上来讲通过仿真结果表明，在三种不同工况仿真条件下，所提出的nmpc均比tmpc有更短的跟踪时间和更小的波动误差，说明了本文所提出的控制算法的适应性和稳定性优于常规方法。
[0160]
图8、图9是所搭建的实验系统示意图，预设的参考轨迹包括一个双圆环及不规则道路轨迹等，在本章节实验环境条件下，该无人小车前轮转向时不考虑任何的偏转角度，车身速度为恒定值。而且在实际的测试过程中取得了令人满意的结果。图10、图11是在不规则道路下的测试，可以得到所提出的控制器在跟踪中比传统的控制器更加快速的跟踪上所设计的参考轨迹，波动误差明显好于传统控制器。图12、图13中，我们可以得到相同的结论。
[0161]
通过实地测试表明，本发明中提出的非线性模型预测控制器能够更加快速的跟踪上参考轨迹，并在跟踪的过程中有更小的波动误差和更加平缓的趋势，从而验证了本发明的控制器的有效性和精确性，提高了车辆的行驶稳定性和安全性，对应用于无人驾驶车辆的轨迹跟踪控制具有重大意义。