一种基于势能场启发式搜索的自动驾驶换道轨迹规划算法

技术领域

本发明属于自动驾驶轨迹规划算法领域。

背景技术

常用的自动驾驶换道轨迹规划模型采用多项式进行拟合，如五次多项式，计 算出安全、舒适的轨迹供无人驾驶车辆完成预定的行驶任务。另一种方法是基于 深度学习的轨迹规划方法，通过训练人类驾驶轨迹，从而模仿人类的驾驶行为完 成轨迹规划。这些方法一般考虑车辆间隙、车辆速度等因素或者设定一系列的安 全规则防止与动态车辆碰撞。然而缺乏对道路条件的考虑，如弯曲道路线性，匝 道加速车道长度等道路约束；另一方面，交通规则的影响，如路权、车道限速和 交叉口信号等因素也没有考虑在内，因此这些轨迹规划算法缺乏对复杂道路环适 用性。

势能场的换道轨迹规划方法可同时考虑车辆的纵向和横向安全状态，评估了 多种交通因素引起的风险。另一优势方面，各种因素的风险都可以方便叠加，从 而揭示驾驶员-车辆-道路相互作用及其对驾驶安全的影响，并可以预测由于动态 变化而引起的驾驶安全趋势。通过描述道路边界风险、动态车辆风险、静态障碍 物风险和交通规则风险，可基于势能场理论进行一般交叉口区域、直线路段、弯 曲路段、匝道出入口、施工区域等多种场景的轨迹规划，从而构建一个轨迹规划 的统一框架。

现有自动驾驶换道轨迹规划相关研究存在的缺陷如下：(1)仅考虑交互车辆 的风险情况，缺乏对复杂道路环境的综合描述。(2)在势能场的基础上采用梯度 下降方法生成的轨迹可能不满足车辆运动学模型的要求，从而不被执行。

发明内容

本发明的目的是：一种基于势能场理论和A-star算法的自动驾驶换道轨迹规 划算法。本发明涉及一种应用势能场理论进行自动驾驶车辆的换道轨迹规划算法， 应用于复杂道路环境下的自动驾驶换道轨迹规划。通过考虑环境车辆的位置影响， 与车辆运动趋势构建动态车辆势能场模型；对道路环境中车道线和道路边界势能 场分别采用高斯分布和反比函数描述。在势能场的基础上，标定车辆不可接受的 最小风险阈值，划分车辆轨迹不可侵入区域，采用A-star算法搜索风险值最小 的路径。本发明采用自适应模型预测控制的方法求解车辆的实际轨迹，实现对规 划轨迹的跟踪。

本发明采用的技术方案是：

步骤1：依据道路交通环境中各要素的特征，建立动态车辆、车道线和车道 边界的势能函数。道路环境按0.1m×0.1m分辨率计算胡环境栅格风险值，得到 风险势能场。

环境车辆势能函数：

其中q＝(x

车道线势能函数：

其中W

其中W

步骤2：在风险势能场的基础上，采用A-star算法搜索风险值最小的路径， 每t时刻根据当前的风险势能场生成车辆的参考轨迹。A-star算法是一种常用的 路径查找和图形遍历算法，它可以被认为是Dijkstra算法的扩展，由于借助启发 函数的引导，A-star算法通常拥有更好的性能。A-star算法通过以下函数来计算 每个节点的优先级：f(n)＝g(n)+h(n)。

其中：f(n)是节点n的综合优先级。当选择下一个要遍历的节点时，选取综 合优先级最高(值最小)的节点；g(n)是节点n距离起点的代价；h(n)是节点n 距离终点的预计代价，即A-star算法的启发函数，通过调节启发函数可以控制算 法的速度和精确度

采用A-star算法规划路径，首先标定车辆不可接受的最小风险阈值，在这个 的基础上可划分车辆不可侵入区域，在这个范围内不进行轨迹规划；这里采用的 A-star启发函数为节点与目标点的距离的高斯函数，h(n)小于等于节点n到终点 的代价，保证A-star算法一定能够找到最短路径。而节点移动更新时设置移动代 价为t时刻移动节点的风险值，使算法能尽快找到一条风险值较小的最优路径。 利用A-star规划所得到的位置信息蕴含了减小动态车辆风险、车道风险、道路边 界风险等目标。

步骤3：为求解实际轨迹设定约束条件。预测第p步时，车辆位置为

步骤4：设计MPC模型，采用Bicycle model建立自动驾驶车辆运动学模型， 以A-star规划轨迹为参考求解安全、舒适、可执行的自动驾驶换道轨迹。基于 车辆运动学模型和步骤3中安全约束条件设置MPC两个约束条件，舒适性的条件 是通过在MPC中约束加速度的变化率来实现。MPC的优化目标是使得生成轨迹与 参考轨迹的误差最小，同时保证控制量的变化最小。

其中，J为损失函数，N

s.t.

k＝0,…,N

其中，

本发明的优点是：

(1)基于势能场理论能综合描述自动驾驶汽车面对的人-车-路环境风险，并 且不同类型主题的风险值可以叠加计算；

(2)基于A-star算法对最低风险路径进行搜索，提高了规划轨迹的搜索效 率，适应自动驾驶汽车快速决策的要求。

(3)MPC方法通过求解一个带约束的二次规划问题，可以得到安全、舒适 和满足运动学模型要求的自动驾驶换道规划轨迹。

附图说明

图1：换道环境势能场图

图2：MPC安全约束空间

图3：本发明的计算整体步骤图

图4：实施例换道特征

图5：实施例换道轨迹

具体实施方式

下面结合上海自然驾驶的典型换道场景和具体的算法实施步骤介绍实施例。

实施例

本发明基于上海自然驾驶数据设计实施例，根据上海自然驾驶的换道数据的 中位值设计典型的车辆换道场景。在上海自然驾驶换道可接受间隙研究中，快速 路环境(限速60km/h～80km/h)下换道车辆与前车的初始间隙中位值为1.25s， 与后车的初始间隙时距中位值为1.18s。因此，设置前后车速为20m/s，与前车初 始距离为24m，与后车初始距离为22m，本车速度为20m/s的实施场景。

通过考虑环境车辆的位置影响，与车辆运动趋势构建动态车辆势能场模型； 对道路环境中车道线和道路边界势能场分别采用高斯分布和反比函数描述。在势 能场的基础上，标定车辆不可接受的最小风险阈值，划分车辆轨迹不可侵入区域， 采用A-star算法搜索风险值最小的路径。本发明采用自适应模型预测控制的方 法求解车辆的实际轨迹，实现对规划轨迹的跟踪。

具体实施步骤是：

步骤1：依据道路交通环境中各要素的特征，建立动态车辆、车道线和车道 边界的势能函数。道路环境按0.1m×0.1m分辨率计算胡环境栅格风险值，得到 风险势能场。

环境车辆势能函数：

其中q＝(x

车道线势能函数：

其中W

其中W

步骤2：在风险势能场的基础上，采用A-star算法搜索风险值最小的路径， 每t时刻根据当前的风险势能场生成车辆的参考轨迹。A-star算法是一种常用的 路径查找和图形遍历算法，它可以被认为是Dijkstra算法的扩展，由于借助启发 函数的引导，A-star算法通常拥有更好的性能。A-star算法通过以下函数来计算 每个节点的优先级：f(n)＝g(n)+h(n)。

其中：f(n)是节点n的综合优先级。当选择下一个要遍历的节点时，选取综 合优先级最高(值最小)的节点；g(n)是节点n距离起点的代价；h(n)是节点n 距离终点的预计代价，即A-star算法的启发函数，通过调节启发函数可以控制算 法的速度和精确度。

采用A-star算法规划路径，首先标定车辆不可接受的最小风险阈值，在这个 的基础上可划分车辆不可侵入区域，在这个范围内不进行轨迹规划；这里采用的 A-star启发函数为节点与目标点的距离的高斯函数，h(n)小于等于节点n到终点 的代价，保证A-star算法一定能够找到最短路径。而节点移动更新时设置移动代 价为t时刻移动节点的风险值，使算法能尽快找到一条风险值较小的最优路径。 利用A-star规划所得到的位置信息蕴含了减小动态车辆风险、车道风险、道路边 界风险等目标。

步骤3：为求解实际轨迹设定约束条件。预测第p步时，车辆位置为

步骤4：设计MPC模型，采用Bicycle model建立自动驾驶车辆运动学模型， 以A-star规划轨迹为参考求解安全、舒适、可执行的自动驾驶换道轨迹。基于 车辆运动学模型和步骤3中安全约束条件设置MPC两个约束条件，舒适性的条件 是通过在MPC中约束加速度的变化率来实现。MPC的优化目标是使得生成轨迹与 参考轨迹的误差最小，同时保证控制量的变化最小。

其中，J为损失函数，N

s.t.

k＝0,…,N

其中，

实施结果轨迹见图4。