一种多障碍物环境下的目标轨迹规划和跟踪方法

1.本技术涉及智能交通技术领域，尤其涉及一种多障碍物环境下的目标轨迹规划和跟踪方法。


背景技术：

2.智能驾驶汽车是解决现有交通事故、道路拥堵等问题的有效手段，也是汽车产业当前的发展方向。它主要包含环境感知、轨迹规划、跟踪控制等模块，其中轨迹规划是连接环境感知和跟踪控制的桥梁，可以认为是智能驾驶汽车的大脑。轨迹规划模块根据获得的道路信息进行分析判断，并根据出行任务对驾驶行为做出调整，是智能驾驶汽车能够产业化落地的前提之一。目前的轨迹规划算法大多面向的是单障碍物环境，检测到前方障碍物后，通过换到相邻车道来实现避障。但是，这种方法在每个车道都有障碍物的多障碍物环境中并不适用。而自动驾驶汽车在行驶过程中遭遇的场景多种多样，不全是仅包含单个障碍物的简单场景。如何在多障碍物复杂环境中规划出最优轨迹，实现智能驾驶汽车的安全行驶是本技术需要解决的问题。


技术实现要素：

3.本技术提供了一种多障碍物环境下的目标轨迹规划和跟踪方法，其技术目的是解决多障碍物环境下智能驾驶汽车的安全行驶问题。
4.本技术的上述技术目的是通过以下技术方案得以实现的：
5.一种多障碍物环境下的目标轨迹规划和跟踪方法，包括：
6.s1：获取主车状态信息和周围道路环境信息，对当前车道和周围车道是否有障碍物进行判断，若没有障碍物就转至步骤s7继续对初始目标轨迹e
y0
进行跟踪，若有则转至步骤s2；
7.s2：将初始目标轨迹e
y0
进行平移，生成候补轨迹集，通过横向偏移e
n*1
描述候补轨迹集中的每个候补轨迹，则所述候补轨迹集表示为：
8.e
n*1
＝[e1,e2,...en]
t
；(1)
[0009]
其中，e1表示从右车道边界laner到初始目标轨迹e
y0
的距离，en表示从左车道边界lane
l
到初始目标轨迹e
y0
的距离，则e1和en表示为：
[0010][0011]
s3：设计包括所有障碍物和车道标志线的势场，对每一条候补轨迹受到的障碍物和车道标志线的势场进行计算，通过候补轨迹的势场值决策出最优目标轨迹，即选取势场值最小的候补轨迹作为最优目标轨迹；
[0012]
其中，对每一条候补轨迹受到的障碍物和车道标志线的势场进行计算，表示为：
[0013]
[0014]
其中，表示每个障碍物的势场对每条候补轨迹i的影响；表示交通标志线的势场对每条候补轨迹i的影响；
[0015]
s4：通过样条曲线方法对初始目标轨迹e
y0
到最优目标轨迹的换道轨迹进行规划和跟踪，完成从初始目标轨迹e
y0
到终端横向位置ej的平滑切换，j∈[1,n]；
[0016]
s5：判断主车是否已避开障碍物，若是转至步骤s6，否则转至步骤s4；
[0017]
s6：判断主车是否位于车道中心线，若是转至步骤s7，否则转至步骤s2；
[0018]
s7：通过路径跟踪控制器对目标轨迹进行跟踪。
[0019]
本技术的有益效果在于：本技术所述的多障碍物环境下的目标轨迹规划和跟踪方法，将初始目标轨迹进行平移来生成一簇候补轨迹集，根据所有障碍物的势场和交通标志线势场选取出最优目标轨迹，并通过样条曲线方法规划出安全平顺的轨迹，实现初始目标轨迹到决策最优轨迹的平稳切换。采用模型预测控制方法设计路径跟踪控制器，实现对所规划轨迹的精准跟踪。该方法能够实现智能驾驶汽车在多障碍物环境下的安全行驶，具有很强的实用性，以及广阔的商业应用前景。
附图说明
[0020]
图1为本技术所述方法的流程图；
[0021]
图2为本技术设计的障碍物势场示意图；
[0022]
图3为本技术设计的交通标志线势场示意图；
[0023]
图4为本技术得出的多障碍物环境下的主车行驶轨迹示意图；
[0024]
图5为本技术得出的主车前轮转角和横向加速度示意图。
具体实施方式
[0025]
下面将结合附图对本技术技术方案进行详细说明。
[0026]
如图1所示，本技术所述的多障碍物环境下的目标轨迹规划和跟踪方法，包括：
[0027]
s1：获取主车状态信息和周围道路环境信息，对当前车道和周围车道是否有障碍物进行判断，若没有障碍物就转至步骤s7继续对初始目标轨迹e
y0
进行跟踪，若有则转至步骤s2。
[0028]
s2：将初始目标轨迹e
y0
进行平移，生成候补轨迹集，通过横向偏移e
n*1
描述候补轨迹集中的每个候补轨迹，则所述候补轨迹集表示为：
[0029]en*1
＝[e1,e2,...en]
t
；(1)
[0030]
其中，e1表示从右车道边界laner到初始目标轨迹e
y0
的距离，en表示从左车道边界lane
l
到初始目标轨迹e
y0
的距离，则e1和en表示为：
[0031]
e1＝lane
r-ey0；(2)
[0032]en
＝lane
l-e
y0
[0033]
一般，主车的初始目标轨迹为右车道中心线；每两条候补轨迹之间的距离为0.1m，候补轨迹的数量与道路宽度有关。
[0034]
s3：设计包括所有障碍物和车道标志线的势场，对每一条候补轨迹受到的障碍物和车道标志线的势场进行计算，通过候补轨迹的势场值决策出最优目标轨迹，即选取势场值最小的候补轨迹作为最优目标轨迹；
[0035]
其中，对每一条候补轨迹受到的障碍物和车道标志线的势场进行计算，表示为：
[0036][0037]
其中，表示每个障碍物的势场对每条候补轨迹i的影响；表示交通标志线的势场对每条候补轨迹i的影响。障碍物势场建模如图2所示，需要保证两辆车的车身框架不相互碰撞，并且在避碰过程中两车的横向距离不宜过大，以防止本车超出车道边界。本技术采用分段函数来描述第k个障碍物的横向影响，表示为：
[0038][0039]
其中，di表示障碍物与第i条候补轨迹之间的横向距离；w表示主车宽度；wk表示激光雷达检测到的第k个障碍物的宽度；a
obs
表示强度参数，描述了障碍物势场的大小；κ表示障碍物势场函数的阶数，表示障碍物势场变化的速度。在图2中，w wk表示相对于车辆轮廓的不可触碰势场区域，(w wk)/2表示障碍物势场的影响范围，是可触碰的区域。
[0040]
交通标志线分为两种，一种是不能通行的道路边界线，另一种是可以通过的车道中心线。本技术设计了如图3所示的车道边界强势场约束以及车道中心线弱势场约束，表示为：
[0041][0042]
其中，w
l
表示单个车道的宽度；a1表示车道边界的增益；a2表示车道中心线的增益；k
l
表示交通标志线势场函数的阶数。
[0043]
选择受到障碍物和交通标志线势场函数最小候补轨迹ej作为新的目标轨迹，表示为：
[0044][0045]
其中，m表示候补轨迹的数量。沿着新的目标轨迹，主车能够避开所有障碍物，保持在车道边界内，并尽量不越过道路中心线。越过障碍物后，会选择最近的车道中心线作为新的参考路径，符合人类驾驶员的驾驶习惯。
[0046]
s4：通过样条曲线方法对初始目标轨迹e
y0
到最优目标轨迹的换道轨迹进行规划，完成从初始目标轨迹e
y0
到终端横向位置ej的平滑切换，j∈[1,n]。
[0047]
具体地，所述样条曲线方法为五次样条曲线方法，表示为：
[0048][0049][0050]
其中，x
veh
表示行驶长度；e
y,des
为相对于行驶长度x
veh
的横向偏差；ψ
des
为横向偏差的一阶导数，表示候补轨迹的航向角；x
avo
表示主车换道过程中的总行驶距离，x
avo
＝v
x
*t
lc
，v
x
表示车辆纵向速度，t
lc
表示驾驶员期望的换道时间；且x
veh
＝x
host-(x
obs-x
avo
)；x
host
表示主车的纵向位置；x
obs
表示障碍车的纵向位置。所规划的换道轨迹由ej和x
avo
两个参数决定,ej是根据候补轨迹集和环境势场决策出的最优目标轨迹。
[0051]
s5：判断主车是否已避开障碍物，若是转至步骤s6，否则转至步骤s4。
[0052]
s6：判断主车是否位于车道中心线，若是转至步骤s7，否则转至步骤s2。
[0053]
若主车不在车道中心线上，需要对目标轨迹重新规划。在只有车道势场影响而没有障碍物势场的影响下，最近的车道中心线将会成为新的目标轨迹(最优目标轨迹)。
[0054]
s7：通过路径跟踪控制器对目标轨迹进行跟踪。
[0055]
具体地，上述路径跟踪控制器通过车辆-道路模型构建，包括：
[0056]
采用二自由度车辆模型来描述车辆的横向和横摆运动，假设车辆的前轮转向角δf为小角度，纵向车速v
x
保持不变，则车辆模型表示为：
[0057][0058][0059]
其中，m为车辆质量，iz为车辆的转动惯量，lf表示车辆质心到前轴的距离；lr表示车辆质心到后轴的距离；f
yf
表示前轮总的横向力；f
yr
表示后轮总的横向力；vy表示横向速度；表示车辆的横摆角速度。
[0060]
若前轮侧偏角αf和后轮侧偏角αr都为小角度，则：
[0061][0062][0063]
其中，cf表示前轮侧偏刚度；cr表示后轮侧偏刚度；由于横向速度vy远小于纵向速度v
x
，车辆质心侧偏角β可以表示为β＝vy/v
x
。
[0064]
车辆的运动用全局坐标系描述为：
[0065][0066][0067]
其中，x表示车辆在全局坐标系中的纵向位置；y表示车辆在全局坐标系中的横向位置；ψ表示车辆的横摆角。
[0068]
综合式(9)-(14)，则车辆-道路模型描述为：
[0069]
[0070]
其中，表示状态矢量；u＝δf表示控制量，f表示非线性系统函数。
[0071]
车辆在曲线坐标下的运动描述成线性时变状态空间方程的形式，表示为：
[0072][0073]
y(t)＝cx(t)；(17)
[0074]
其中，均表示时变的系数矩阵，y＝[y,ψ]
t
为输出矢量，c为选择矩阵。
[0075]
公式(16)可以离散为：
[0076][0077]
其中，δt表示系统离散时间；δu表示控制量增量；
[0078]
定义预测时域为n
p
、控制时域为nc，在第k时刻，状态序列、输出序列和控制序列分别表示为：
[0079]
x(k 1|k),x(k 2|k),...,x(k n
p
|k)；(19)；
[0080]
y(k 1|k),y(k 2|k),...,y(k n
p
|k)
[0081]
δu(k|k),δu(k 1|k),...,δu(k n
c-1|k)
[0082]
车辆路径跟踪控制器用于实现对目标路径的跟踪，因此，其目标函数为：
[0083][0084]
其中，q表示状态量增量的权重矩阵；r表示控制量增量的权重矩阵。
[0085]
最终，车辆的路径跟踪控制器表示为：
[0086][0087]
s.t.x(k i|k)＝akx(k i-1|k) bku(k i-1|k)
[0088]
y(k i-1|k)＝cx(k i-1|k),(i＝1,2,...n
p
)
[0089]
u(k i|k)＝u(k i-1|k) δu(k i|k)
[0090]umin
≤u(k i|k)≤u
max
[0091]
δu
min
≤δu(k i|k)≤δu
max
[0092]
(i＝0,1,...,n
c-1)
[0093]
x(k|k)＝x(k).
[0094]
式中，u
min
和u
max
分别表示控制量u的最小限制和最大限制；δu
min
和δu
max
分别表示控制量增量δu的最小限制和最大限制。
[0095]
预测时域内的初始状态为上一时刻车辆的状态量。在求解了上述优化问题之后，得到最优的控制增量序列δu
*
，表示为：
[0096]
δu
*
(k)＝[δu
*
(k|k),δu
*
(k 1|k),...,δu
*
(k n
c-1|k)]；(22)；
[0097]
则k时刻的控制输出表示为：
[0098]
u(k|k)＝u(k-1|k-1) δu
*
(k|k)；(23)；
[0099]
新的控制变量u(k|k)输出给车辆实现路径跟踪；在下一个时间步数k 1时，用来自车辆的更新状态在预测范围内解决一个新的优化问题，最终实现对目标路径的跟踪。
[0100]
以下通过matlab/simulink-carsim联合仿真平台构建多障碍物环境对上述实施例的目标轨迹决策、规划和跟踪方法进行验证。主车以60km/h的速度沿右侧车道的中心线行驶，此时，前方出现四个障碍物，主车必须改变车道。障碍物分别位于(100,0),(105,5),(250,4.5),(255,3)。模型预测路径跟踪控制器的权重矩阵设置为q＝diag(3,60)和r＝200。
[0101]
测试结果如图4和图5所示，当主车遭遇前方第一个障碍物和第二个障碍物时，首先根据所提出的候补轨迹集和道路环境2维势场法确定最优目标轨迹。然后，采用五次样条曲线方法生成安全舒适的避障换道轨迹。最后，采用模型预测路径跟踪控制器来控制车辆跟随换道轨迹。由于两个障碍物的横向位置分别为0和5，车道中心线的影响较小，因此确定的最优目标轨迹为初始目标轨迹向左偏移2.5m。主车在不同时刻的轨迹如图4所示，主车避开第一个障碍物和第二个障碍物后，在交通标志线势场的影响下，选择最近的车道中心线作为新的目标轨迹。
[0102]
第一个障碍物和第二个障碍物验证了所提算法在障碍物位于不同侧时的有效性，为了证明算法的通用性，将第三个障碍物和第四个障碍物设置在同一侧。由于两个障碍物的横向位置分别为4.5和3，根据障碍物势场和车道标志线势场，确定的最优目标轨迹为当前目标轨迹向右偏移3.9m。即初始轨迹右侧0.3m。避开障碍物后，主车将在车道标志线势场的影响下进行左变道，返回右车道中心线。如图5所示，在最大横向变道距离为3.9m，车速为60km/h时，主车最大横向加速度不超过0.34g，表明车辆的乘坐舒适性可以得到保证。
[0103]
以上为本技术示范性实施例，本技术的保护范围由权利要求书及其等效物限定。本领域的技术人员应该了解,本技术不受上述实例的限制,上述实例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等同物界定。