一种基于环境不确定性的无人车动态路径规划方法
【技术领域】
[0001]本发明涉及无人驾驶汽车路径规划领域，尤其是涉及一种基于环境不确定性的无 人车动态路径规划方法。
【背景技术】
[0002] 随着计算机技术和人工智能的发展，无人驾驶汽车（以下简称无人车)在军事、交 通、工业生产、物流仓储、日常生活等方面展现出巨大的应用前景。在国防军事方面，无人车 多用于执行危险场景下的军事任务，如军事救援和物资输送等。在交通安全方面，无人驾驶 技术是促进智能交通系统发展的有效手段，基于人工智能的无人驾驶技术可以提高车辆行 驶的主动安全性，能够有效减少驾驶员由于误操作导致的交通事故，从而提高交通行驶效 率和安全性。在工业生产、物流仓储方面，无人车可以配合自动化生产线实现全自主无人生 产，进一步推进工业生产的自动化和智能化，进而提高生产效率。另外，无人车的出现也将 极大地方便人们的工作、旅游等日常生活。
[0003] 无人驾驶技术主要包括环境信息的感知，驾驶行为的智能决策，无碰撞路径的规 划，以及车辆的运动控制等四个部分。路径规划是无人驾驶技术中十分关键的组成部分，为 环境感知和运动控制起着承上启下的作用。基于感知系统的环境数据，车辆需要在复杂的 道路环境中，基于一定的性能指标(安全性最高、车辆可行驶、能量最优等)规划出一条安全 可靠的、车辆可行驶的、从起始位置到目标位置的最短无碰撞路径。
[0004] 移动机器人的路径规划问题由来已久，且很多学者已经提出了许多成熟的方法。 但近10年来，随着无人驾驶技术的迅速发展，无人车的路径规划问题再次成为国内外学者 最为关注的问题之一。
[0005] 基于启发式的搜索算法被用于无人车的路径规划。最常见的有A*、D*算法，分别用 于解决静态和动态栅格环境下的路径规划问题。通过引入起始点到目标点之间的启发函 数，提出了基于栅格化方法的A*算法。A*对起始栅格与目标栅格之间的距离以及栅格的被 占用情况进行估价，向邻居节点中估价最小的栅格扩散而最终到达目标栅格。A*算法在全 局环境信息已知、障碍物为静态的情况下，能够很好的规划出一条无碰撞的路径。针对复杂 多变的动态环境，提出了动态A*算法（即D*算法），首先进行全局静态规划，在环境没有更新 时，不断保持上一次的搜索结果，当有新的障碍物出现时，对搜索结果进行修正，从而实现 动态避障。在栅格化环境中，只要最短路径存在，A*和D*就能够求得最短路径，但是在两种 算法中只是将机器人看成质点，并没有考虑机器人的结构特性和模型约束，生成的路径不 够平滑，可能导致路径不可行，另外随着栅格数目的增多，算法的运算量会急剧增加。
[0006] 人工势场法等、遗传算法和神经网络等人工智能方法也被应用于无人车的路径规 划。人工势场法通过引进势场的概念，分别建立与目标点和障碍物之间的引力场和斥力场， 机器人在引力场和斥力场的共同作用下沿着势场减小的方向行驶。但是这种方法的缺陷是 存在极小值点，从而导致机器人陷入死区而不能到达目标点。部分学者将路径规划问题转 化为多目标优化问题，将路径的快速性、平顺性等作为优化指标，通过遗传迭代求得最优 解，实现避障。神经网络方法也应用于无人车的路径规划，外部的输入激活相应的神经元， 从而执行相应的动作到达相应的位置，但算法的运算量会随神经网络规模的增大急剧增 加，算法的运算效率较低。
[0007] 无人车虽然属于移动机器人的一种，但是由于其具有特定的结构和运动学特性， 在进行无人车的路径规划时，势必要考虑模型的约束，也就是说，不满足模型约束的路径无 人车是无法被跟踪的。另一方面，在实际环境中，无人车的行驶环境复杂多变，无人车对于 环境信息、特别是动态障碍物的运动很难具有先验的知识，这也就要求无人车在路径规划 时要对动态多变的环境进行预测和威胁评估，考虑动态环境的不确定性，不仅要评估已有 的威胁还要评估潜在的威胁，基于此进行路径规划，才能保障规划路径的安全性。
[0008] 中国专利CN104933228A公开了一种基于速度障碍的无人车实时轨迹规划方法，将 动态环境中的运动障碍物投影到车辆的速度空间，并将三次多项式函数光滑可控基元中的 速度变量设置为最优速度，从而构造搜索三维位形空间的启发式函数，进而在车辆的位形 空间及速度空间进行搜索实现轨迹规划。中国专利CN102591332A公开了一种用于无人驾驶 汽车局部路径规划的装置及方法，包括传感器装置以及基于人工势场法的局部路径规划方 法，通过视觉传感器探测道路边界并计算出道路中心线，通过雷达探测障碍物信息，并设计 了当前位置距离车道中心线的引力函数和距离障碍物的斥力函数，进而计算合力的方向， 通过高斯组合隶属度函数系数的方法，解决了可能会陷入局部极小和路径震荡的问题。以 上两个专利并没有考虑到车辆所具有的结构特性和运动学特性以及对于移动障碍物的处 理，并不能反映障碍物的运动特性，且没有考虑移动障碍物的大小和运动的不确定性，也没 有对移动障碍物运动轨迹进行预测，难以反映移动障碍物的潜在威胁。
【发明内容】
[0009] 本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于环境不确 定性的无人车动态路径规划方法，不仅可以满足车辆行驶的安全性要求，还能够在满足车 辆模型约束的情况下保证行驶效率，通过不同的权重分配实现性能指标的协调优化，同时 在多个动态障碍物存在条件下实现实时规划，有效提高无人车行驶的安全性。
[0010] 本发明的目的可以通过以下技术方案来实现：
[0011] -种基于环境不确定性的无人车动态路径规划方法包括以下步骤：
[0012] S1:建立车辆运动学模型，无人车和障碍物均满足车辆运动学模型，所述车辆运动 学模型满足以下公式：
[0015]其中，X为车辆运动状态，（x，y)为以车辆后轴中心点为原点建立的坐标系下的横 向坐标和纵向坐标，Θ为车辆相对于坐标系X轴的航向角，v和δ分别为车辆的速度和前轮转 角，1为车辆前轴与后轴之间的距离，i、v沒、分别对应为x、y、9、v的一阶求导；
[0016] S2:建立表征无人车运动环境的动态环境模型，并根据动态环境模型建立重新规 划路径的满足条件；
[0017] S:获取无人车的车辆iiz办It态起始值^哞辆态初始目标值兔，并根据X获取多个车辆 @ > ·?? =K.>^o*^'??^%Λ]，***-[* ? 0 ο,-ν^,Γ? 4=[4..v*,??.4'?.?]^> 4 = \.，《=丨，.-4 下标〇表示起始值，下标g表示初始目标值，下标h表示候选目标 值，上标i表示第i个候选目标值，d为设定的横向间隔；
[0018] S4:基于车辆运动学模型，生成从X0到4的候选路径；
[0019] S5:评估候选路径的安全性指标和快速性指标，基于安全性指标和快速性指标从 各候选路径中选取得到最优路径；
[0020] S6:跟踪无人车以最优路径运动的过程，当无人车运动环境满足重新规划路径的 满足条件时，跳转步骤S3，重新规划无人车的最优路径。
[0021 ]所述动态环境模型包括：
[0022] 1)建立坐标系，用圆心位于(Y，y')、半径为V的圆表示无人车，无人车速度/ = [ν' x，/ y ]τ，其中，/ x、/ j别表示/在X轴和Y轴上的速度分量；
[0023] 2)用圆心位于(^^^^八半径为^的圆表示障碍物^表示障碍物的编号^章碍物 速度￥。,。=[￥。,。,^。4,\] 1'，其中，￥。^￥。4,\分别表示￥。,。在乂轴和7轴上的速度分量；
[0024] 3)以（X7 )为中心、ri为半径的区域定义为危险区域，r<ri<rd，rd为设定值，以 以Y )为中心、?为半径的区域定义为警示区域，rd<r2<ra，ra为设定值；
[0025] 4)定义障碍物正在靠近的满足条件为：Δ Vy · Δ y<〇, Δ Vy表示无人车与障碍物的 横向相对速度，△ Ay表示无人车与障碍物的横向相对距离， y7 )〇
[0026] 所述重新规划路径的满足条件为：
[0027] 1)障碍物进入危险区域；
[0028] 2)障碍物进入警示区域且障碍物正在靠近；
[0029] 3)当肖U最优路径被跟踪完毕；
[0030] 4)最优路径与障碍物的运动轨迹出现相交；
[0031] 当无人车运动环境满足重新规划路径的满足条件中的至少一条时，重新规划无人 车的最优路径。
[0032] 所述步骤S3中车辆运动状态初始目标值xg中的速度目标值^与道路的曲率和道路 的限速有关。
[0033] 所述步骤S4具体为：
[0034] 41:设车辆的轨迹(Χ(3，^)为六阶多项式，满足以下公式：
[0036]其中，t为时间，ak、bk为待定系数；
[0037] 42:结合车辆运动学模型和六阶多项式，根据x〇和4将车辆的轨迹(xe，y e)表示为：
[0044]其中，ie(·)表示对Xe( ·)的一阶求导，毛(·)表示对Xe( ·)的二阶求导，九(·)表示 对ye( ·)的一阶求导，九0表示对ye( ·)的二阶求导；
[0045] 43:定义性能指标J(xe，ye)为偏离连接幼和知的曲线的偏差之和，满足以下公式：
[0048] 其中，xn=xn(t)，yn = yn(t)，（xn，yn)为连接X。和xg的曲线；
[0049] 44:根据xq和& ,联立式⑶和式⑷求解得到待定系数ak、bk，待定系数ak、b k代入 公式(2)得到车辆的轨迹(Χ(3，％)，即从xo到&的候选路径。
[0050] 所述步骤S5中根据代价函数从各候选路径中选取得到最优路径，所述代价函数满 足以下公式：
[0051 ] Ji - W1 J〇, i，cp+W2Jo, i，dev ( 5 )
[0052] 其中，心为从xo到的候选路径的代价，W1为对应安全性指标的权重系数，w2为对 应快速性指标的权重系数，Jm#为从X0到4的候选路径的安全性指标，Jndev为从X0到xi 的候选路径的快速性指标，最优路径为最小的候选路径。
[0053] 所述从xo到xl的候选路径的安全性指标Ju#满足以下公式：
[0055]其中，E-为在预测周期内无人车的位置分布，EV(3h为在预测周期内障碍物的位置 分布，EobsflEveh矣0表示在每个预测时刻障碍物的位