心线路径的终点重合，且中心线路径为候选路径的其中一条，也是筛选候选路径时 的参考轨迹。图6为直道和弯道场景下生成的候选路径示意图。
[0191] 直道场景下的仿真结果如图7至图10所示。无人车在直道场景下从起点到终点之 间划分为两段进行规划，图7中无人车在第一段的起点感应到三个障碍物，考虑到此三个障 碍物的运动轨迹进行路径规划得到对应的最优路径，因为第一段轨迹跟踪过程中没有出现 重新规划的情况，无人车第一段轨迹的跟踪结果如图8所示，接着开始第二段轨迹的规划， 图8中无人车在第二段的起点处感应到另外两个障碍物，考虑到此两个障碍物的运动轨迹 进行路径规划得到对应的最优路径，因为第二段轨迹跟踪过程中没有出现重新规划的情 况，无人车到达终点，如图9所示，无人车在终点处也感应到三个障碍物，图10为直道场景下 无人车规划路径的航向、前轮转角、速度和前轮转向率信息。
[0192] 弯道场景下的仿真结果如图11至图14所示。图11表示弯道场景下路径规划开始， 图12表示弯道场景下无人车超过一个障碍物，图13表示弯道场景下无人车到达终点，图14 为弯道场景下无人车规划路径的航向、前轮转角、速度和前轮转向率信息。
[0193] 图15为实车试验时无人车环境感应效果示意图，图16为实车试验的试验场地卫星 图，无人车需在图16中黑色闭合道路上行驶，对应的实车试验结果如图17至图19所示，图17 为实车实验下直道避障效果，图18为实车试验下弯道避障效果，图19为实车试验下无人车 数据记录，可以看出无人车采用本发明方法行驶的路径与图16的道路一致，且实际所花费 的时间远远少于计划的时间，实时性好。
[0194] 综上，本发明针对无人驾驶车辆的结构和运动学特性，提出一种基于车辆运动学 的动态路径规划方法，该方法根据车辆的运动学模型、起始点和目标点状态约束等建立起 等式约束，将路径假设为多项式曲线，通过等式约束将路径规划问题转化为多项式系数求 解问题，进而求得在不考虑障碍物情况下的路径。当在实际环境中，通过道路信息确定多个 候选目标点，并利用上述方法生成对应的候选轨迹，通过对障碍物的运动不确定性和无人 车的运动不确定性进行分析建模，提出基于碰撞概率的障碍物威胁估计模型，设计兼顾路 径的安全性、快速性等性能指标的代价函数，选择出最优的路径，完成动态环境下的路径规 划，为无人驾驶车辆的运动控制提供了基础。
【主权项】
1. 一种基于环境不确定性的无人车动态路径规划方法，其特征在于，包括W下步骤： S1:建立车辆运动学模型，无人车和障碍物均满足车辆运动学模型，所述车辆运动学模 型满足W下公式：(1) 其中，X为车辆运动状态，（X，y)为W车辆后轴中屯、点为原点建立的坐标系下的横向坐 标和纵向坐标，Θ为车辆相对于坐标系X轴的航向角，V和δ分别为车辆的速度和前轮转角，1 为车辆前轴与后轴之间的距离，丈、j、分、V分别对应为x、y、e、v的一阶求导； S2:建立表征无人车运动环境的动态环境模型，并根据动态环境模型建立重新规划路 径的满足条件； 没:获取无人车的车辆运动状态起始値骑η车辆运动状态初始目标位g，并概庶燕取多个车辆运 动状态候选目柄值X*，其中，*。=[而知，0。，成'、*.=['、':，.吗.咬'4',告.,吃]' 语=V % = V .皆1 -為=下标〇表示起始值，下标g表示初始目标值，下标11表示候选目标 值，上标i表示第i个候选目标值，d为设定的横向间隔； S4:基于车辆运动学模型，生成从X0到4的候选路径； S5:评估候选路径的安全性指标和快速性指标，基于安全性指标和快速性指标从各候 选路径中选取得到最优路径； S6:跟踪无人车W最优路径运动的过程，当无人车运动环境满足重新规划路径的满足 条件时，跳转步骤S3，重新规划无人车的最优路径。2. 根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述动态环境模型包括： 1) 建立坐标系，用圆屯、位于(χ/，/)、半径为r/的圆表示无人车，无人车速度γ/= [γ/X， Α]τ，其中，γ/X V y分别表示/在X轴和巧由上的速度分量； 2) 用圆屯、位于(XD,q，y。,q)、半径为。的圆表示障碍物，q表示障碍物的编号，障碍物速度 V。,q=[V。,q,χ，V。,q,y]T，其中，V。,q,χ、V。,q,y分别表示V。,q在X轴和巧由上的速度分量； 3) Κ(χ/，/)为中屯、、ri为半径的区域定义为危险区域，r<ri<rd，rd为设定值，， /)为中屯、、η为半径的区域定义为警示区域，rd<r2<ra，ra为设定值； 4) 定义障碍物正在靠近的满足条件为：Δ vy . Ay<〇, Avy表示无人车与障碍物的横向 相对速度，Δ Vy=(V〇,q,y-v/y)，Ay表示无人车与障碍物的横向相对距离，Ay=(yD,q-/ )。3. 根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述重新规划路径的满足条件为： 1) 障碍物进入危险区域； 2) 障碍物进入警示区域且障碍物正在靠近； 3) 当前最优路径被跟踪完毕； 4)最优路径与障碍物的运动轨迹出现相交； 当无人车运动环境满足重新规划路径的满足条件中的至少一条时，重新规划无人车的 最优路径。4. 根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述步骤S3中车辆运动状态初始目标值xg中的速度目标值vg与道路的曲率和道路的 限速有关。5. 根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述步骤S4具体为： 41:设车辆的轨迹(xe，ye)为六阶多项式，满足W下公式：(2> 其中，t为时间，ak、bk为待定系数； 42 :结合车辆运动学模型和六阶多项式，根据X0和4将车辆的轨迹(Xe，ye)表示为：其中，jtO表示对XeO的一阶求导，^0表示对屯0的二阶求导，欠（·）表示对ye (·)的一阶求导，化〇表示对ye( ·)的二阶求导； 43:定义性能指标J(Xe，ye)为偏离连接X日和Xg的曲线的偏差之和，满足W下公式：其中，Xn = Xn(t)，yn = yn(t)，（Xn，yn)为连接 X0和 Xg 的曲线； 44:根据X日和4,联立式（3)和式(4)求解得到待定系数ak、bk，待定系数ak、bk代入公式 (2)得到车辆的轨迹(Xe，ye)，即从X0到4的候选路径。6. 根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述步骤S5中根据代价函数从各候选路径中选取得到最优路径，所述代价函数满足 W下公式： Ji = Wljo, i, CP+W2J0, i, dev (5) 其中，Ji为从XO到X'/,的候选路径的代价，W1为对应安全性指标的权重系数，W2为对应快 速性指标的权重系数，Jd,WP为从X0到 < 的候选路径的安全性指标，jD,i,dev为从X0到4的候 选路径的快速性指标，最优路径为Ji最小的候选路径。7. 根据权利要求6所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述从X0到的候选路径的安全性指标Jd,WP满足W下公式：(6) 其中，Enbs为在预测周期内无人车的位置分布，Eve功在预测周期内障碍物的位置分布， E。bsnEveh辛0表示在每个预测时刻障碍物的位置分布与无人车的位置分布有交集，（Xl(j)， Yl(j))表示在从xo到4的候选路径上的第j个点的坐标，N表示候选路径的长度，P。。ll(Xl (j)，Yi(j))表示在从X0到4的候选路径上的第j个点的碰撞概率。8. 根据权利要求7所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述在预测周期内无人车的位置分布Enbs通过障碍物在预测周期内的控制输入和车 辆运动学模型计算得到，所述障碍物在预测周期内的控制输入满足W下公式：其中，Sk为障碍物在预测周期内的前轮转角输入，Vk为障碍物在预测周期内的速度输 入，UO为障碍物初始控制输入，Ek为在初始控制输入上叠加的高斯控制输入； 所述在预测周期内障碍物的位置分布Eveh根据无人车在预测周期内的轨迹得到，所述 无人车在预测周期内的轨迹:?满足W下公式：(8) Ck ~N[0，Sk] 其中，Pk为待跟踪无人车的路径，Ck为在待跟踪无人车的路径上叠加的高斯分布。9. 根据权利要求7所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述在从X0到4的候选路径上的第j个点的碰撞概率?。。11化（如，￥1〇))满足^下公 式： Pc〇ii(Xi(j),Yi(j))=Pv(Xi(j),Yi(j)) *p〇(Xi(j),Yi(j)) (9) 其中，Pv(Xi(j)，Yi(j))为无人车在(Xi(j)，Yi(j))处的概率，并由E。bs决定，p。(Xi(j)，Yi (j))为障碍物在(Xi(j)，Yi(j))处的概率，并由Eveh决定。10. 根据权利要求6所述的一种基于环境不确定性的无人车动态路径规划方法，其特征 在于，所述从X0到4的候选路径的快速性指标J。, 1, dev满足W下公式：(10) 其中，（Xi (j)，Yi (j))表示在从X0到的候选路径上的第j个点的坐标，（Xc (j)，Yc(j))表 示参考轨迹上的第j个点的坐标，N表示候选路径的长度，所述参考轨迹为从XO到Xc的中屯、线 路径，x(.二[方(.，於'，沒('，，Xc二xo'yc二yg，目C二目g，Sc二Sg，Vc二Vg，^ 二心皆。
【专利摘要】本发明涉及一种基于环境不确定性的无人车动态路径规划方法，包括以下步骤：S1：建立车辆运动学模型；S2：建立动态环境模型和重新规划路径的满足条件；S3：获取无人车的车辆运动状态起始值、车辆运动状态初始目标值和车辆运动状态候选目标值；S4：生成候选路径；S5：基于安全性指标和快速性指标选取得到最优路径；S6：当无人车运动环境满足重新规划路径的满足条件时，重新规划无人车的最优路径。与现有技术相比，本发明不仅可以满足车辆行驶的安全性要求，还能够在满足车辆模型约束的情况下保证行驶效率，通过不同的权重分配实现性能指标的协调优化，同时在多个动态障碍物存在条件下实现实时规划，有效提高无人车行驶的安全性。
【IPC分类】G05D1/02
【公开号】CN105549597
【申请号】CN201610080558
【发明人】王晓年, 李朝成, 王峻
【申请人】同济大学
【公开日】2016年5月4日
【申请日】2016年2月4日