基于障碍车估计与预测的交叉路口轨迹规划与控制方法与流程

1.本发明涉及障碍车的状态预测、无人车的轨迹规划与轨迹跟踪控制，以无人车在交叉路口环境下的通行为背景，通过预测障碍车未来一段时间的运动状态，设计了一种基于障碍车实时估计与预测的交叉路口轨迹规划与控制算法，适用于无人车在复杂场景下快速安全通过交叉路口，属于轨迹规划领域；


背景技术：

2.无人驾驶车辆能够高效快速地响应各种复杂的道路情况，在确保安全的基础上，有效避免了诸如酒后驾车、疲劳驾驶等危险驾驶行为，同时还可以有效提高道路利用率；但实际的道路交通系统中存在多种干扰与约束，车辆在这些复杂场景中需要进行精准的判断、决策与控制来保证行车安全和通行效率；特别是在交叉路口中，场景的复杂性尤为凸显；交叉路口汇集了来自各个方向的交通流，是交通拥堵和交通事故的多发地；不同于一般的一维驾驶场景，交叉路口场景中需要兼顾二维的交通流，且其道路几何形状较为复杂，因此对安全性有更高的要求；
3.无人车在复杂场景下的控制问题可以分为两个部分，即轨迹规划和轨迹跟踪控制；针对轨迹规划问题，首先应该需要考虑针对障碍车的避障问题；在车辆的实际行驶过程中，其它交通参与者的实时状态可通过传感器测量获得，但其未来一段时间内的运动状态无法直接得出；因此如何完成对障碍车准确且可靠的轨迹预测，是保证道路交通安全、减轻道路拥堵的关键问题，也是轨迹规划的重难点之一；其次，在轨迹规划的过程中还应当注意避免与交叉路口处的道路发生碰撞，因此如何在轨迹规划中加入道路碰撞约束也是研究重点之一；在基于上述条件得到期望轨迹后，还需要通过对车辆的执行机构发出控制指令，控制车辆的前进方向和速度等，使车辆能够精确、稳定地跟踪期望轨迹行驶；但车辆模型具有强耦合、强非线性的特点，如何处理模型状态之间的耦合，并且设计控制器在复杂的实际驾驶场景中快速、准确、稳定地完成跟踪任务，也是轨迹跟踪控制研究的重点；因此在考虑避障避碰、快速规划、轨迹跟踪精度等多重性能需求的情况下，为实现无人车在交叉路口快速安全行驶，对障碍车状态预测方法以及无人车轨迹规划和轨迹跟踪方法进行研究具有重要的理论意义和现实意义；


技术实现要素：

4.无人车在交叉路口行驶的过程中，主要目标是使无人车躲避其它交通参与者，安全快速地通过交叉路口；为保证无人车行驶安全，需要根据其它交通参与者的运动状态和交叉路口的路况为无人车规划合理的行驶轨迹，既不能与其它交通参与者发生碰撞，也不能与交叉路口的道路边缘发生碰撞；为符合实际情况，研究中假设障碍车的运动状态是未知的，因此需要对障碍车的未来一段时间内的运动状态进行估计，同时要在轨迹规划中加入道路碰撞约束，这无疑增加了轨迹规划的难度；为保证无人车能够快速通过交叉路口避免发生拥堵，需要设计合理的控制方案使得无人车能够准确快速沿规划好的路径行进；
5.针对上述现有技术，本发明的目的是为无人车在含有其它交通参与者的交叉路口中行驶时提供一个安全快速的行驶方案，即一种基于障碍车实时估计与预测的交叉路口轨迹规划与控制算法；在无人车和障碍车运动学模型、交叉路口道路模型已知的基础上，对障碍车的运动轨迹和未来一段时间内的运动状态进行估计与预测，并在此基础上完成轨迹规划，最后使用基于模型动态解耦的自抗扰控制器进行轨迹跟踪，综合考虑轨迹规划的快速性，轨迹跟踪的准确性等性能指标，为无人车通过交叉路口提供高效可靠的行驶方案。
6.为了解决上述技术问题，本发明提出的一种基于障碍车估计与预测的交叉路口轨迹规划与控制方法，主要包括：
7.步骤一、建立无人车动力学模型，无人车运动学模型，以及障碍车的运动模型与量测模型；
8.步骤二、障碍车轨迹的初步预测：
9.步骤三、无人车轨迹规划；
10.步骤四、无人车动力学模型的动态解耦；
11.步骤五、自抗扰轨迹跟踪控制器设计；
12.本发明综合考虑轨迹规划的快速性，轨迹跟踪的准确性等性能指标，从而为无人车在含有其它交通参与者的交叉路口中行驶时提供一个安全快速、高效可靠的行驶方案。
13.步骤一包括的具体内容如下：
14.1)采样时间为t的无人车运动学模型如下所示：
[0015][0016]
式(1)中，(x,y)表示无人车在地面惯性坐标系下的位置，v为车辆质心处的速度，其方向与车辆前进方向存在夹角β，称为车辆侧滑角；表示车辆横摆角，δ
f
表示前轮偏角；l
f
和l
r
为车辆前后轴距，l
w
为车身宽度；选择为状态变量，u＝[a δ
f
]
t
为控制变量；将式(1)表示为：
[0017]
2)无人车动力学模型如下所示：
[0018]
[0019]
式(2)中，v
x
和v
y
分别为车辆纵向速度和横向速度，对应方向的加速度用a
x
和a
y
表示；v为纵向和横向的合速度，α为轮胎侧偏角，f
l
·
,*
和f
c
·
,*
分别表示轮胎纵向力和侧向力，其中，
·
∈{f,r}表示车辆前后轴，*∈{l,r}表示车辆左右轮胎；m为无人车质量，i
z
代表无人车的转动惯量；选择为状态变量，u
d
＝[δ
f f
lf f
lr
]
t
为控制变量；
[0020]
3)障碍车有两种运动状态，分别为：障碍车在直行道上进行的均匀加速运动和在转弯处进行的协同转弯运动；障碍车的运动模型包括匀加速运动模型和协同转弯运动模型；
[0021]
选取状态变量为设采样周期为t，得到离散形式的匀加速运动模型如下所示：
[0022][0023]
式(3)中，w
ca
(k)是高斯白噪声，其协方差矩阵为
[0024]
设，所述障碍车做转弯角速度恒为ω0的匀速圆周运动，得到离散形式的协同转弯运动模型如下：
[0025][0026]
式(4)中，w
ct
(k)是高斯白噪声，其协方差矩阵为
[0027]
4)为获得障碍车运动状态的观测量，取观测量为z＝[x y v]
t
，得到障碍车的量测模型如下：
[0028][0029]
是(5)中，w
ob
(k)为量测的高斯白噪声，其协方差矩阵为
[0030]
步骤二包括的具体内容如下：
[0031]
1)采用容积卡尔曼滤波算法对无人车状态进行初步预测，得到状态估计值及协方差矩阵如下：
[0032][0033]
式(6)中，为k时刻的最优估计值；为第k时刻的一步预测值；w(k)为容积卡尔曼滤波增益；z(k)为量测模型中观测量；为量测值的预测；p(k|k
‑
1)为第k时刻的一步预测值的协方差矩阵；p
zz
(k|k
‑
1)为量测值的误差协方差矩阵；p(k|k)为状态向量的协方差矩阵；
[0034]
2)设：障碍车在直行车道上做匀加速直线运动，在交叉路口处做协同转弯运动，并且转弯方向已知，通过容积卡尔曼滤波算法得到障碍车状态估计值后，将状态估计值代入相应的式(3)或(4)中，即得到预测时域n
p
内障碍车状态的模型预测值；
[0035]
引入误差修正因子修正上述模型预测值的误差，预测时域n
p
内障碍车状态预测值如下所示：
[0036][0037][0038]
式(7)和式(8)中，和为第k时刻计算得到的第时刻障碍车状态的实际预测值和模型预测值，和为加速度实际预测值和模型预测值，为加速度估计值，为加速度预测误差；为误差系数，用于衡量加速度预测误差的大小，其选择与障碍车加速度有关；和的值与障碍车加速度及预测时刻有关，随着时间的延长，k时刻的误差对后续时刻的影响越来越小，因此和的值与成反比，比例系数应根据实际情况选取，一般在区间(0,10)内；
[0039]
经过上述修正，得到障碍车i在预测时域内的状态预测值：
[0040][0041]
步骤三包括的具体内容如下：
[0042]
采用模型预测自适应动态规划(model predictive adaptive dynamic programming control，mpadp)算法，用于求解轨迹规划，内容如下：
[0043]
1)首先，基于动态规划和贝尔曼最优性原理，得到每个时刻的无限时域目标函数；然后，考虑预测时域内所有时刻的目标函数，将预测时域的所有无限时域目标函数进行叠加，得到堆叠目标函数，作为模型预测自适应动态规划控制算法的目标函数，表示如下：
[0044][0045]
式(10)中，v(k i|k)为第k时刻预测得到的第i个目标函数；
[0046]
2)设计参数逼近器，对每个预测时刻的目标函数进行逼近，参数逼近器为：
[0047]
v(k i|k)＝w
t
(k i|k)φ(ξ(k i|k),u(k i|k)),i＝1,...,n
p
ꢀꢀꢀꢀꢀꢀ
(11)
[0048]
式(11)中，w为参数向量，φ表示回归量；ξ(k i|k)和u(k i|k)分别为第k时刻预测得到的第i个状态量和控制量；
[0049]
模型预测自适应动态规划控制算法的评价模块中，针对性能指标的偏差，设计如下优化问题：
[0050][0051]
式(12)中：
[0052][0053]
求解上述优化问题，得到最优的参数向量w
*
(
·
|k)；将w
*
(
·
|k)代入到式(12)中，即可得到执行模块的优化问题；
[0054]
将模型预测自适应动态规划控制算法与障碍车轨迹预测算法结合，在障碍车行驶状态未知的情况下，当无人车探测到障碍车时，对障碍车在预测时域n
p
内的位置和速度进行在线预测；
[0055]
执行模块对于直行路段的优化问题为：
[0056][0057]
式(14)中，n
c
为控制时域；ξ(k i|k)和u(k i|k)分别为第k时刻预测得到的第i个状态量和控制量；a
min
和a
max
分别为加速度的上下界；n
obs
为障碍车数量；当探测范围d
det
内出现了障碍车时，将加速度的上下界放宽至和δ
fmin
和δ
fmax
分别为前轮偏角的上下界；δu
min
和δu
max
分别为控制增量δu的上下界；d
safe
为安全距离；j＝1,3，d
safe
的取值与无人车所处位置有关，χ1,χ3表示直行状态集合，表示无人车与障碍车的估计位置的相对距离，表示无人车与障碍车的预测位置的相对距离，计算公式如下：
[0058][0059][0060]
对于交叉路口路段，优化问题为：
[0061][0062]
式(17)中，f
out
和f
in
分别为与道路外、内边缘避碰的非线性函数；b为车道宽度，r为直行道与交叉路口的连接处的四分之一圆弧的一个半径；χ2表示无人车转弯状态集合；
[0063]
步骤四包括的具体内容如下：
[0064]
将各输出量之间的动态耦合作用视为扩张状态，采用扩张状态观测器(extended state observer，eso)进行观测，并反馈到控制器加以补偿；
[0065]
针对式(2)所示的无人车动力学模型，选取输出量y
d
＝[x y v]
t
，该动力学模型即为3维输入3维输出的耦合系统；采用小角度假设对该模型进行简化，得到简化模型：
[0066][0067]
令u1＝f
lf
，u2＝δ
f
，u3＝f
lr
，则式(8)所述的非线性模型表示为：
[0068][0069]
式(19)中，和f3(ξ
d
,u
d
)为非线性函数，表示如下：
[0070][0071]
b
11
，b
22
和b
33
为相应子对象的输入增益：
[0072][0073]
经上述解耦过程，设计如下扩张状态观测器(extended state observer，eso)估计子对象的实际状态和扩张状态：
[0074][0075][0076][0077]
式(22)至(24)中，h为积分步长，β
x1
，β
x2
，β
x3
，β
y1
，β
y2
，β
y3
，β
v1
和β
v2
为增益系数，δ
x
，δ
y
，δ
v
，α
x1
，α
x2
，α
y1
，α
y2
，α
v1
和α
v2
为可调参数；以状态量x的eso为例，函数fal(e
x
(k),α
x1
,δ
x
)定义为：
[0078][0079]
步骤五包括的具体内容如下：
[0080]
1)设计自抗扰控制器：包括纵向、横向位置和速度的跟踪微分控制器(tracking differentior，td)、扩张状态观测器和非线性误差反馈控制律(nonlinear state error feedback，nlsef)；其中，所述的跟踪微分控制器安排过渡过程以产生光滑的输入信号和输入微分信号；所述的扩张状态观测器通过控制量和输出量估计系统的状态以及内外扰动；所述的非线性误差反馈控制律将所述的跟踪微分控制器的输出与所述的扩张状态观测器的观测值之间的误差通过非线性组合得到控制量；
[0081]
2)以纵向状态量x为例，跟踪微分控制器的数学模型如下所示：
[0082][0083]
式(26)中，r0为快速因子，h为积分步长，x
r
(k)为输入信号即纵向参考值，x1(k)跟踪输入信号，x2(k)为跟踪输入信号的变化率；函数fhan(x1,x2,r0,h)定义为：
[0084][0085]
非线性误差反馈控制律的数学表达式如下所示：
[0086][0087]
式(28)中，k
x1
和k
x2
为待整定的参数；u
x
(k)为实际控制量，由误差反馈控制量u
0x
(k)及扰动估计值z
x3
的补偿得出；通过不断地测试得到最优参数，即可得到鲁棒性较强的控制效果。同理，根据上述方法可以得出横向状态量y和速度状态量v的跟踪微分控制器的数学模型和非线性误差反馈控制律的数学表达式。
[0088]
与现有技术相比，本发明的有益效果是：
[0089]
本发明所提出的预测方法能够准确预测障碍车辆的轨迹和速度，并且能够准确反映障碍车未来的运动趋势，同时轨迹规划算法能够保证无人车与道路边缘和障碍车均无碰撞，说明障碍车状态预测算法能够为无人车提供较为可靠的预测信息，保证无人车在轨迹规划的过程中成功避障，并且所设计的自抗扰控制器控制效果良好，跟踪误差较小。因此，本发明中的基于障碍车实时估计与预测的交叉路口轨迹规划与控制算法具有可行性。
附图说明
[0090]
图1是实现本发明方法的控制系统流程图；
[0091]
图2是本发明方法中自抗扰控制器结构图；
[0092]
图3是本发明中涉及的交叉路口场景示意图；
[0093]
图4是实施例的轨迹规划仿真结果图。
具体实施方式
[0094]
本发明属于轨迹规划领域，其设计思路是：以无人车在交叉路口环境下的通行为背景，通过预测障碍车未来一段时间的运动状态，设计一种基于障碍车实时估计与预测的交叉路口轨迹规划与控制方法，主要包括障碍车的状态预测、无人车的轨迹规划与轨迹跟踪控制，从而实现无人车在复杂场景下快速安全通过交叉路口；
[0095]
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制；
[0096]
无人驾驶车辆能够提高驾驶安全性，减少人类在驾驶方面的劳动。由于车载传感器无法预知障碍车的驾驶意图，也无法获得未来一段时间内障碍车的运动状态，因此本发明采用容积卡尔曼滤波对障碍车的运动状态进行估计，并设计了误差修正策略，与障碍车的运动学模型结合完成对状态的多步预测。然后基于获得的运动轨迹预测结果对无人车进行轨迹规划。由于无人车动力学模型具有强耦合、非线性的特点，本发明采用动态解耦的方法对模型进行解耦，并利用自抗扰算法设计了轨迹跟踪控制器。本发明的控制系统流程图如图1所示，具体过程如下：
[0097]
1、模型建立
[0098]
本发明涉及到的模型有无人车动力学模型，无人车运动学模型，以及障碍车的运动模型与量测模型。首先介绍交叉路口场景。图3为本发明所研究的交叉路口场景示意图，车道宽度为b，各车道依次以进行命名。直行道与交叉路口的连接处是一个半径为r的四分之一圆弧，o(n1,n2)为该圆弧圆心。本发明假设车辆在路口处可以没有任何限制地左转、右转和直行。r1，r2，r3和r4是位于车道中心线上的点，其中r1r2和r3r4表示直行路段，r2r3表示交叉路口路段。
[0099]
本发明采用采样时间为t的无人车运动学模型如下所示：
[0100][0101][0102]
v(k 1)＝v(k) t
·
a(k)
[0103][0104]
上式中(x,y)表示无人车在地面惯性坐标系下的位置，v为车辆质心处的速度，其方向与车辆前进方向存在夹角β，称为车辆侧滑角。表示车辆横摆角，δ
f
表示前轮偏角。l
f
和l
r
为车辆前后轴距，l
w
为车身宽度。选择为状态变量，u＝[a δ
f
]
t
为控制变量。因此，上式可以表示为：
[0105]
无人车的动力学模型描述为：
[0106][0107][0108][0109][0110][0111]
其中，v
x
和v
y
分别为车辆纵向速度和横向速度，对应方向的加速度用a
x
和a
y
表示。v为纵向和横向的合速度，α为轮胎侧偏角，f
l
·
,*
和f
c
·
,*
分别表示轮胎纵向力和侧向力，其中，
·
∈{f,r}表示车辆前后轴，*∈{l,r}表示车辆左右轮胎。m为无人车质量，i
z
代表无人车的转动惯量。选择为状态变量，u
d
＝[δ
f f
lf f
lr
]
t
为控制变量。
[0112]
本发明所设计场景中，障碍车有两种运动状态，分别为在直行道上进行的均匀加速运动，和在转弯处进行的协同转弯运动。选取状态变量为设采样周期为t，可得到离散形式的匀加速运动模型：
[0113][0114]
其中，w
ca
(k)是高斯白噪声，其协方差矩阵为
[0115]
协同转弯模型假设障碍车做转弯角速度恒为ω0的匀速圆周运动，离散形式的协同转弯运动模型如下：
[0116][0117]
其中，w
ct
(k)是高斯白噪声，其协方差矩阵为
[0118]
为获得障碍车运动状态的观测量，还需设计量测模型。取观测量为z＝[x y v]
t
，量测模型可表示为：
[0119][0120]
其中，w
ob
(k)为量测的高斯白噪声，其协方差矩阵为
[0121]
2、障碍车轨迹的多步预测
[0122]
障碍车的运动模型和量测模型中均带有高斯白噪声，为对障碍车运动状态进行最优估计，需要选取合适的滤波算法减少噪声干扰，以获得干扰较弱的信号。容积卡尔曼滤波算法较传统卡尔曼滤波算法能有效处理复杂度较高的非线性系统，较扩展卡尔曼滤波算法能有效提高精度。因此本发明采用容积卡尔曼滤波对无人车状态进行初步预测，得到状态估计值及协方差矩阵如下：
[0123][0124]
p(k|k)＝p(k|k
‑
1)
‑
w(k)p
zz
(k|k
‑
1)w
t
(k)
[0125]
其中，为k时刻的最优估计值。为第k时刻的一步预测值。w(k)为容积卡尔曼滤波增益。z(k)为量测模型中观测量。为量测值的预测值。p(k|k
‑
1)为第k时刻的一步预测值的协方差矩阵。p
zz
(k|k
‑
1)为量测值的误差协方差矩阵。p(k|k)为状态向量的协方差矩阵。
[0126]
假设障碍车在直行车道上做匀加速直线运动，在交叉路口处做协同转弯运动，并且转弯方向已知，障碍车需要根据自身位置选择相应的滤波模型和预测模型，通过容积卡尔曼滤波算法得到障碍车状态估计值后将状态估计值代入到上述障碍车运动模型中，障碍车在直行车道上做匀加速直线运动则带入上述的离散形式的匀加速运动模型，障碍车在交叉路口处做协同转弯运动则带入上述的离散形式的协同转弯运动模型，即可得到预测时域n
p
内障碍车状态的模型预测值。然而，因干扰的存在，这种基于模型的预测方法无法准确反映无人车未来的运动轨迹。另外，由于无人车无法直接观测到障碍车在预测时域内的加速度，因此，对障碍车状态预测的误差主要由加速度误差导致。本发明针对该问题设计了一种误差修正策略，引入误差修正因子修正模型预测值的误差，得到较为可靠的障碍车状态预测值。
[0127][0128][0129]
其中，和为第k时刻计算得到的第时刻障碍车状
态的实际预测值和模型预测值，和为加速度实际预测值和模型预测值，为加速度估计值，为加速度预测误差。为误差系数，用于衡量加速度预测误差的大小。和的值与障碍车的加速度有关，应根据实际情况选取。
[0130]
经过修正，即可得到障碍车i在预测时域内的状态预测值经过修正，即可得到障碍车i在预测时域内的状态预测值
[0131]
3、无人车轨迹规划
[0132]
本发明提出了模型预测自适应动态规划(model predictive adaptive dynamic programming control，mpadp)算法，用于求解轨迹规划问题。首先，基于动态规划和贝尔曼最优性原理，得到每个时刻的无限时域目标函数。然后，考虑预测时域内所有时刻的目标函数，将预测时域的所有无限时域目标函数进行叠加，得到堆叠目标函数，作为mpadp算法的目标函数。表示如下：
[0133][0134]
其中，v(k i|k)为第k时刻预测得到的第i个目标函数。
[0135]
设计参数逼近器对每个预测时刻的目标函数进行逼近，参数逼近器为：
[0136]
v(k i|k)＝w
t
(k i|k)φ(ξ(k i|k),u(k i|k)),i＝1,...,n
p
[0137]
其中，w为参数向量，φ表示回归量。ξ(k i|k)和u(k i|k)分别为第k时刻预测得到的第i个状态量和控制量。
[0138]
mpadp算法的评价模块中，针对性能指标的偏差，设计如下优化问题：
[0139][0140]
s.t.w
t
(k i|k)φ(ξ(k i|k),u(k i|k
‑
1))＞0
[0141][0142]
其中：
[0143][0144]
求解上述优化问题，即可得到最优的参数向量w
*
(
·
|k)。将w
*
(
·
|k)代入到参数逼近器中，即可得到执行模块的优化问题。
[0145]
将mpadp算法与障碍车轨迹预测算法结合。在障碍车行驶状态未知的情况下，当无人车探测到障碍车时，会对障碍车在预测时域n
p
内的位置和速度进行在线预测。
[0146]
执行模块对于直行路段的优化问题为：
[0147][0148][0149][0150][0151][0152][0153][0154]
其中，n
c
为控制时域。ξ(k i|k)和u(k i|k)分别为第k时刻预测得到的第i个状态量和控制量。a
min
和a
max
分别为加速度的上下界。n
obs
为障碍车数量。当探测范围d
det
内出现了障碍车时，将加速度的上下界放宽至和δ
fmin
和δ
fmax
分别为前轮偏角的上下界。δu
min
和δu
max
分别为控制增量δu的上下界；d
safe
为安全距离；j＝1,3，d
safe
的取值与无人车所处位置有关，χ1,χ3表示直行状态集合，表示无人车与障碍车的估计位置的相对距离，表示无人车与障碍车的预测位置的相对距离，二者计算公式如下：
[0155][0156]
[0157]
对于交叉路口路段，优化问题为：
[0158][0159][0160][0161][0162][0163][0164][0165][0166][0167]
其中，f
out
和f
in
分别为与道路外、内边缘避碰的非线性函数。χ2表示无人车转弯状态集合。
[0168]
4、无人车动力学模型的动态解耦
[0169]
由于无人车的动力学模型具有很强的耦合性，因此本发明将各输出量之间的动态耦合作用视为扩张状态，采用扩张状态观测器(extended state observer，eso)进行观测，并反馈到控制器加以补偿。针对无人车动力学模型，选取输出量y
d
＝[x y v]
t
，动力学模型即为3维输入3维输出的耦合系统。然后采用小角度假设对模型进行简化，即可得到简化模型：
[0170][0171][0172][0173]
令u1＝f
lf
，u2＝δ
f
，u3＝f
lr
，则上述非线性模型可表示为：
[0174][0175]
其中，和f3(ξ
d
,u
d
)为非线性函数，表示如下：
[0176][0177][0178][0179]
b
11
，b
22
和b
33
为相应子对象的输入增益：
[0180][0181][0182][0183]
经上述解耦过程，设计如下eso估计子对象的实际状态和扩张状态：
[0184][0185][0186][0187]
其中，h为积分步长，β
x1
，β
x2
，β
x3
，β
y1
，β
y2
，β
y3
，β
v1
和β
v2
为增益系数，δ
x
，δ
y
，δ
v
，α
x1
，α
x2
，α
y1
，α
y2
，α
v1
和α
v2
为可调参数。以状态量x的eso为例，函数fal(e
x
(k),α
x1
,δ
x
)定义为：
[0188][0189]
5、自抗扰轨迹跟踪控制器设计
[0190]
本发明所设计的自抗扰控制器基本结构如图2所示，该控制器包括纵向、横向位置和速度的跟踪微分控制器(tracking differentior，td)、eso和非线性误差反馈控制律(nonlinear state error feedback，nlsef)。其中，td安排过渡过程以产生光滑的输入信号和输入微分信号；eso通过控制量和输出量估计系统的状态以及内外扰动；nlsef将td的输出与eso的观测值之间的误差通过非线性组合得到控制量。其中，eso已经在上文中说明。以纵向状态量x(图2中i＝1时)为例，自抗扰控制器中的td的数学模型如下所示：
[0191][0192]
其中，r0为快速因子，h为积分步长，x
r
(k)为输入信号即纵向参考值，x1(k)跟踪输入信号，x2(k)为跟踪输入信号的变化率。函数fhan(x1,x2,r0,h)定义为：
[0193][0194]
nlsef的数学表达式如下所示：
[0195][0196]
其中，k
x1
和k
x2
为待整定的参数。u
x
(k)为实际控制量，由误差反馈控制量u
0x
(k)及扰动估计值z
x3
的补偿得出。通过调参，即可得到鲁棒性较强的控制效果。业内人员按照上述方法同样可以得出横向状态量y和速度状态量v的跟踪微分控制器的数学模型和非线性误差反馈控制律的数学表达式，在此不再赘述。
[0197]
该实施方式通过程序编写完成模型建立、算法验证等过程，通过大量的数据，以验证所提出障碍车预测算法在交叉路口场景轨迹规划和轨迹跟踪中应用的有效性；本发明中
以无人车在交叉路口行驶并躲避两辆障碍车为背景，开展测试；在完成数据处理后，得到结果如图4所示；图4为轨迹规划结果图，包括，无人车整体轨迹图，交叉路口处的局部放大图，无人车与障碍车之间的距离示意图即子图(a)。
[0198]
仿真结果表明，本发明所提出的预测方法能够准确预测障碍车辆的轨迹和速度，并且能够准确反映障碍车未来的运动趋势，同时轨迹规划算法能够保证无人车与道路边缘和障碍车均无碰撞，说明障碍车状态预测算法能够为无人车提供较为可靠的预测信息，保证无人车在轨迹规划的过程中成功避障，并且所设计的自抗扰控制器控制效果良好，跟踪误差较小。本发明中的基于障碍车实时估计与预测的交叉路口轨迹规划与控制算法具有可行性。
[0199]
尽管上面结合附图对本发明进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨的情况下，还可以做出很多变形，这些均属于本发明的保护之内。