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

技术特征：
1.一种基于障碍车估计与预测的交叉路口轨迹规划与控制方法，其特征在于，包括以下步骤：步骤一、建立无人车动力学模型，无人车运动学模型，以及障碍车的运动模型与量测模型：1)采样时间为t的无人车运动学模型如下所示：式(1)中，(x,y)表示无人车在地面惯性坐标系下的位置，v为车辆质心处的速度，其方向与车辆前进方向存在夹角β，称为车辆侧滑角；表示车辆横摆角，δ
f
表示前轮偏角；l
f
和l
r
为车辆前后轴距，l
w
为车身宽度；选择为状态变量，u＝[a δ
f
]
t
为控制变量；将式(1)表示为：2)无人车动力学模型如下所示：式(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
为控制变量；3)障碍车有两种运动状态，分别为：障碍车在直行道上进行的均匀加速运动和在转弯处进行的协同转弯运动；障碍车的运动模型包括匀加速运动模型和协同转弯运动模型；选取状态变量为设采样周期为t，得到离散形式的匀加速运动模型如下所示：
式(3)中，w
ca
(k)是高斯白噪声，其协方差矩阵为设，所述障碍车做转弯角速度恒为ω0的匀速圆周运动，得到离散形式的协同转弯运动模型如下：式(4)中，w
ct
(k)是高斯白噪声，其协方差矩阵为4)为获得障碍车运动状态的观测量，取观测量为z＝[x y v]
t
，得到障碍车的量测模型如下：是(5)中，w
ob
(k)为量测的高斯白噪声，其协方差矩阵为q
ob
(k)；步骤二、障碍车轨迹的初步预测：1)采用容积卡尔曼滤波算法对无人车状态进行初步预测，得到状态估计值及协方差矩阵如下：式(6)中，为k时刻的最优估计值；为第k时刻的一步预测值；w(k)为容积卡尔曼滤波增益；z(k)为量测模型中观测量；为量测值的预测；p(k|k
‑
1)为第k时刻的一步预测值的协方差矩阵；p
zz
(k|k
‑
1)为量测值的误差协方差矩阵；p(k|k)为状态向量的协方差矩阵；2)设：障碍车在直行车道上做匀加速直线运动，在交叉路口处做协同转弯运动，并且转
弯方向已知，通过容积卡尔曼滤波算法得到障碍车状态估计值后，将状态估计值代入相应的式(3)或(4)中，即得到预测时域n
p
内障碍车状态的模型预测值；引入误差修正因子修正上述模型预测值的误差，预测时域n
p
内障碍车状态预测值如下所示：内障碍车状态预测值如下所示：式(7)和式(8)中，和为第k时刻计算得到的第时刻障碍车状态的实际预测值和模型预测值，和为加速度实际预测值和模型预测值，为加速度估计值，为加速度预测误差；为误差系数，用于衡量加速度预测误差的大小，其选择与障碍车加速度有关；数，用于衡量加速度预测误差的大小，其选择与障碍车加速度有关；的值与障碍车的加速度及预测时刻有关，随着时间的延长，k时刻的误差对后续时刻的影响越来越小，因此和的值与成反比，比例系数在区间(0,10)内；经过上述修正，得到障碍车i在预测时域内的状态预测值：步骤三、无人车轨迹规划采用模型预测自适应动态规划(model predictive adaptive dynamic programming control，mpadp)算法，用于求解轨迹规划，内容如下：1)首先，基于动态规划和贝尔曼最优性原理，得到每个时刻的无限时域目标函数；然后，考虑预测时域内所有时刻的目标函数，将预测时域的所有无限时域目标函数进行叠加，得到堆叠目标函数，作为模型预测自适应动态规划控制算法的目标函数，表示如下：式(10)中，v(k i|k)为第k时刻预测得到的第i个目标函数；2)设计参数逼近器，对每个预测时刻的目标函数进行逼近，参数逼近器为：v(k i|k)＝w
t
(k i|k)φ(ξ(k i|k),u(k i|k)),i＝1,...,n
p
(11)式(11)中，w为参数向量，φ表示回归量；ξ(k i|k)和u(k i|k)分别为第k时刻预测得到的第i个状态量和控制量；模型预测自适应动态规划控制算法的评价模块中，针对性能指标的偏差，设计如下优
化问题：式(12)中：求解上述优化问题，得到最优的参数向量w
*
(
·
|k)；将w
*
(
·
|k)代入到式(12)中，即可得到执行模块的优化问题；将模型预测自适应动态规划控制算法与障碍车轨迹预测算法结合，在障碍车行驶状态未知的情况下，当无人车探测到障碍车时，对障碍车在预测时域n
p
内的位置和速度进行在线预测；执行模块对于直行路段的优化问题为：行模块对于直行路段的优化问题为：式(14)中，n
c
为控制时域，w为参数向量，φ表示回归量；ξ(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表示直行状态集合，表示无人车与障碍车的估计位置的相对距离，表示无人车与障碍车的预测位置的相对距离，计算公式如下：对距离，计算公式如下：对于交叉路口路段，优化问题为：对于交叉路口路段，优化问题为：对于交叉路口路段，优化问题为：式(17)中，f
out
和f
in
分别为与道路外、内边缘避碰的非线性函数；b为车道宽度，r为直行道与交叉路口的连接处的四分之一圆弧的一个半径；χ2表示无人车转弯状态集合；步骤四、无人车动力学模型的动态解耦将各输出量之间的动态耦合作用视为扩张状态，采用扩张状态观测器(extended state observer，eso)进行观测，并反馈到控制器加以补偿；针对式(2)所示的无人车动力学模型，选取输出量y
d
＝[x y v]
t
，该动力学模型即为3维输入3维输出的耦合系统；采用小角度假设对该模型进行简化，得到简化模型：
令u1＝f
lf
，u2＝δ
f
，u3＝f
lr
，则式(8)所述的非线性模型表示为：式(19)中，和f3(ξ
d
,u
d
)为非线性函数，表示如下：b
11
，b
22
和b
33
为相应子对象的输入增益：经上述解耦过程，设计如下扩张状态观测器(extended state observer，eso)估计子对象的实际状态和扩张状态：
式(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
)定义为：步骤五、自抗扰轨迹跟踪控制器设计1)设计自抗扰控制器：包括纵向、横向位置和速度的跟踪微分控制器(tracking differentior，td)、扩张状态观测器和非线性误差反馈控制律(nonlinear state error feedback，nlsef)；其中，所述的跟踪微分控制器安排过渡过程以产生光滑的输入信号和输入微分信号；所述的扩张状态观测器通过控制量和输出量估计系统的状态以及内外扰动；所述的非线性误差反馈控制律将所述的跟踪微分控制器的输出与所述的扩张状态观测器的观测值之间的误差通过非线性组合得到控制量；2)以纵向状态量x为例，跟踪微分控制器的数学模型如下所示：式(26)中，r0为快速因子，h为积分步长，x
r
(k)为输入信号即纵向参考值，x1(k)跟踪输入信号，x2(k)为跟踪输入信号的变化率；函数fhan(x1,x2,r0,h)定义为：
非线性误差反馈控制律的数学表达式如下所示：式(28)中，k
x1
和k
x2
为待整定的参数；u
x
(k)为实际控制量，由误差反馈控制量u
0x
(k)及扰动估计值z
x3
的补偿得出；通过不断地测试得到最优参数，即可得到鲁棒性较强的控制效果；同理，得到横向状态量y和速度状态量v的跟踪微分控制器的数学模型和非线性误差反馈控制律的数学表达式。

技术总结
本发明公开了一种基于障碍车估计与预测的交叉路口轨迹规划与控制方法，主要包括：建立无人车动力学模型，无人车运动学模型，以及障碍车的运动模型与量测模型；障碍车轨迹的初步预测：无人车轨迹规划；无人车动力学模型的动态解耦；自抗扰轨迹跟踪控制器设计。本发明针对无人车在含有其它交通参与者的交叉路口中行驶时，在无人车和障碍车运动学模型、交叉路口道路模型已知的基础上，对障碍车的运动轨迹和未来一段时间内的运动状态进行估计与预测，并在此基础上完成轨迹规划，最后使用基于模型动态解耦的自抗扰控制器进行轨迹跟踪，综合考虑轨迹规划的快速性，轨迹跟踪的准确性等性能指标，为无人车通过交叉路口提供高效可靠的行驶方案。的行驶方案。的行驶方案。


技术研发人员：曲歌 胡超芳 赵凌雪 米涵芃
受保护的技术使用者：天津大学
技术研发日：2021.07.15
技术公布日：2021/9/24