CN105549597A - 一种基于环境不确定性的无人车动态路径规划方法 - Google Patents

Abstract

本发明涉及一种基于环境不确定性的无人车动态路径规划方法，包括以下步骤：S1：建立车辆运动学模型；S2：建立动态环境模型和重新规划路径的满足条件；S3：获取无人车的车辆运动状态起始值、车辆运动状态初始目标值和车辆运动状态候选目标值；S4：生成候选路径；S5：基于安全性指标和快速性指标选取得到最优路径；S6：当无人车运动环境满足重新规划路径的满足条件时，重新规划无人车的最优路径。与现有技术相比，本发明不仅可以满足车辆行驶的安全性要求，还能够在满足车辆模型约束的情况下保证行驶效率，通过不同的权重分配实现性能指标的协调优化，同时在多个动态障碍物存在条件下实现实时规划，有效提高无人车行驶的安全性。

Description

一种基于环境不确定性的无人车动态路径规划方法

技术领域

本发明涉及无人驾驶汽车路径规划领域，尤其是涉及一种基于环境不确定性的无人车动态路径规划方法。

背景技术

随着计算机技术和人工智能的发展，无人驾驶汽车(以下简称无人车)在军事、交通、工业生产、物流仓储、日常生活等方面展现出巨大的应用前景。在国防军事方面，无人车多用于执行危险场景下的军事任务，如军事救援和物资输送等。在交通安全方面，无人驾驶技术是促进智能交通系统发展的有效手段，基于人工智能的无人驾驶技术可以提高车辆行驶的主动安全性，能够有效减少驾驶员由于误操作导致的交通事故，从而提高交通行驶效率和安全性。在工业生产、物流仓储方面，无人车可以配合自动化生产线实现全自主无人生产，进一步推进工业生产的自动化和智能化，进而提高生产效率。另外，无人车的出现也将极大地方便人们的工作、旅游等日常生活。

无人驾驶技术主要包括环境信息的感知，驾驶行为的智能决策，无碰撞路径的规划，以及车辆的运动控制等四个部分。路径规划是无人驾驶技术中十分关键的组成部分，为环境感知和运动控制起着承上启下的作用。基于感知系统的环境数据，车辆需要在复杂的道路环境中，基于一定的性能指标(安全性最高、车辆可行驶、能量最优等)规划出一条安全可靠的、车辆可行驶的、从起始位置到目标位置的最短无碰撞路径。

移动机器人的路径规划问题由来已久，且很多学者已经提出了许多成熟的方法。但近10年来，随着无人驾驶技术的迅速发展，无人车的路径规划问题再次成为国内外学者最为关注的问题之一。

基于启发式的搜索算法被用于无人车的路径规划。最常见的有A*、D*算法，分别用于解决静态和动态栅格环境下的路径规划问题。通过引入起始点到目标点之间的启发函数，提出了基于栅格化方法的A*算法。A*对起始栅格与目标栅格之间的距离以及栅格的被占用情况进行估价，向邻居节点中估价最小的栅格扩散而最终到达目标栅格。A*算法在全局环境信息已知、障碍物为静态的情况下，能够很好的规划出一条无碰撞的路径。针对复杂多变的动态环境，提出了动态A*算法(即D*算法)，首先进行全局静态规划，在环境没有更新时，不断保持上一次的搜索结果，当有新的障碍物出现时，对搜索结果进行修正，从而实现动态避障。在栅格化环境中，只要最短路径存在，A*和D*就能够求得最短路径，但是在两种算法中只是将机器人看成质点，并没有考虑机器人的结构特性和模型约束，生成的路径不够平滑，可能导致路径不可行，另外随着栅格数目的增多，算法的运算量会急剧增加。

人工势场法等、遗传算法和神经网络等人工智能方法也被应用于无人车的路径规划。人工势场法通过引进势场的概念，分别建立与目标点和障碍物之间的引力场和斥力场，机器人在引力场和斥力场的共同作用下沿着势场减小的方向行驶。但是这种方法的缺陷是存在极小值点，从而导致机器人陷入死区而不能到达目标点。部分学者将路径规划问题转化为多目标优化问题，将路径的快速性、平顺性等作为优化指标，通过遗传迭代求得最优解，实现避障。神经网络方法也应用于无人车的路径规划，外部的输入激活相应的神经元，从而执行相应的动作到达相应的位置，但算法的运算量会随神经网络规模的增大急剧增加，算法的运算效率较低。

无人车虽然属于移动机器人的一种，但是由于其具有特定的结构和运动学特性，在进行无人车的路径规划时，势必要考虑模型的约束，也就是说，不满足模型约束的路径无人车是无法被跟踪的。另一方面，在实际环境中，无人车的行驶环境复杂多变，无人车对于环境信息、特别是动态障碍物的运动很难具有先验的知识，这也就要求无人车在路径规划时要对动态多变的环境进行预测和威胁评估，考虑动态环境的不确定性，不仅要评估已有的威胁还要评估潜在的威胁，基于此进行路径规划，才能保障规划路径的安全性。

中国专利CN104933228A公开了一种基于速度障碍的无人车实时轨迹规划方法，将动态环境中的运动障碍物投影到车辆的速度空间，并将三次多项式函数光滑可控基元中的速度变量设置为最优速度，从而构造搜索三维位形空间的启发式函数，进而在车辆的位形空间及速度空间进行搜索实现轨迹规划。中国专利CN102591332A公开了一种用于无人驾驶汽车局部路径规划的装置及方法，包括传感器装置以及基于人工势场法的局部路径规划方法，通过视觉传感器探测道路边界并计算出道路中心线，通过雷达探测障碍物信息，并设计了当前位置距离车道中心线的引力函数和距离障碍物的斥力函数，进而计算合力的方向，通过高斯组合隶属度函数系数的方法，解决了可能会陷入局部极小和路径震荡的问题。以上两个专利并没有考虑到车辆所具有的结构特性和运动学特性以及对于移动障碍物的处理，并不能反映障碍物的运动特性，且没有考虑移动障碍物的大小和运动的不确定性，也没有对移动障碍物运动轨迹进行预测，难以反映移动障碍物的潜在威胁。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于环境不确定性的无人车动态路径规划方法，不仅可以满足车辆行驶的安全性要求，还能够在满足车辆模型约束的情况下保证行驶效率，通过不同的权重分配实现性能指标的协调优化，同时在多个动态障碍物存在条件下实现实时规划，有效提高无人车行驶的安全性。

本发明的目的可以通过以下技术方案来实现：

一种基于环境不确定性的无人车动态路径规划方法包括以下步骤：

S1：建立车辆运动学模型，无人车和障碍物均满足车辆运动学模型，所述车辆运动学模型满足以下公式：

$x={\[x,y,\mathrm{\θ},\mathrm{\δ},v,\stackrel{\·}{v}\]}^T---\left(1\right)$

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=vsin\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{v\tan \mathrm{\δ}}{l}\end{array}\right.$

其中，x为车辆运动状态，(x,y)为以车辆后轴中心点为原点建立的坐标系下的横向坐标和纵向坐标，θ为车辆相对于坐标系X轴的航向角，v和δ分别为车辆的速度和前轮转角，l为车辆前轴与后轴之间的距离，分别对应为x、y、θ、v的一阶求导；

S2：建立表征无人车运动环境的动态环境模型，并根据动态环境模型建立重新规划路径的满足条件；

S3：获取无人车的车辆运动状态起始值x₀和车辆运动状态初始目标值x_g，并根据x_g获取多个车辆运动状态候选目标值其中， $x_g={\[x_g,y_g,{\mathrm{\θ}}_g,{\mathrm{\δ}}_g,v_g,{\stackrel{\·}{v}}_g\]}^T,x_h^i={\[x_h^i,y_h^i,{\mathrm{\θ}}_h^i,{\mathrm{\δ}}_h^i,v_h^i,{\stackrel{\·}{v}}_h^i\]}^T,y_h^i=y_g,{\mathrm{\θ}}_h^i={\mathrm{\θ}}_g,{\mathrm{\δ}}_h^i={\mathrm{\δ}}_g,$ 下标0表示起始值，下标g表示初始目标值，下标h表示候选目标值，上标i表示第i个候选目标值，d为设定的横向间隔；

S4：基于车辆运动学模型，生成从x₀到的候选路径；

S5：评估候选路径的安全性指标和快速性指标，基于安全性指标和快速性指标从各候选路径中选取得到最优路径；

S6：跟踪无人车以最优路径运动的过程，当无人车运动环境满足重新规划路径的满足条件时，跳转步骤S3，重新规划无人车的最优路径。

所述动态环境模型包括：

1)建立坐标系，用圆心位于(x′,y′)、半径为r′的圆表示无人车，无人车速度v′＝[v′_x,v′_y]^T，其中，v′_x、v′_y分别表示v′在X轴和Y轴上的速度分量；

2)用圆心位于(x_o,q,y_o,q)、半径为r_q的圆表示障碍物，q表示障碍物的编号，障碍物速度v_o,q＝[v_o,q,x,v_o,q,y]^T，其中，v_o,q,x、v_o,q,y分别表示v_o,q在X轴和Y轴上的速度分量；

3)以(x′,y′)为中心、r₁为半径的区域定义为危险区域，r＜r₁＜r_d，r_d为设定值，以(x′,y′)为中心、r₂为半径的区域定义为警示区域，r_d＜r₂＜r_a，r_a为设定值；

4)定义障碍物正在靠近的满足条件为：Δv_y·Δy＜0，Δv_y表示无人车与障碍物的横向相对速度，Δv_y＝(v_o,q,y-v′_y)，Δy表示无人车与障碍物的横向相对距离，Δy＝(y_o,q-y′)。

所述重新规划路径的满足条件为：

1)障碍物进入危险区域；

2)障碍物进入警示区域且障碍物正在靠近；

3)当前最优路径被跟踪完毕；

4)最优路径与障碍物的运动轨迹出现相交；

当无人车运动环境满足重新规划路径的满足条件中的至少一条时，重新规划无人车的最优路径。

所述步骤S3中车辆运动状态初始目标值x_g中的速度目标值v_g与道路的曲率和道路的限速有关。

所述步骤S4具体为：

41：设车辆的轨迹(x_e,y_e)为六阶多项式，满足以下公式：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{a}_k{t}^k\\ y_e=y_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{b}_k{t}^k\end{array}\right.---\left(2\right)$

其中，t为时间，a_k、b_k为待定系数；

42：结合车辆运动学模型和六阶多项式，根据x₀和将车辆的轨迹(x_e,y_e)表示为：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=f\left(t\right)P^{-1}\left(G_1-{\mathrm{na}}_6\right)+a_6{t}^6\\ y_e=y_e\left(t\right)=f\left(t\right)P^{-1}\left(G_2-{\mathrm{nb}}_6\right)+b_6{t}^6\end{array}\right.---\left(3\right)$

f(t)＝[1,t,t²,t³,t⁴,t⁵]

$P=\left[\begin{array}{cccccc}1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\\ 1& t_h^i& {\left(t_h^i\right)}^2& {\left(t_h^i\right)}^3& {\left(t_h^i\right)}^4& {\left(t_h^i\right)}^5\\ 0& 1& 2t_h^i& 3{\left(t_h^i\right)}^2& 4\left(t_h^i\right)& 5{\left(t_h^i\right)}^4\\ 0& 0& 2& 6t_h^i& 12\left(t_h^i\right)& 20{\left(t_h^i\right)}^3\end{array}\right]$

$G_1={\[x_e\left(t_0\right),{\stackrel{\·}{x}}_e\left(t_0\right),{\stackrel{\·\·}{x}}_e\left(t_0\right),x_e\left(t_h^i\right),{\stackrel{\·}{x}}_e\left(t_h^i\right),{\stackrel{\·\·}{x}}_e\left(t_h^i\right)\]}^T$

$G_2={\[y_e\left(t_0\right),{\stackrel{\·}{y}}_e\left(t_0\right),{\stackrel{\·\·}{y}}_e\left(t_0\right),y_e\left(t_h^i\right),{\stackrel{\·}{y}}_e\left(t_h^i\right),{\stackrel{\·\·}{y}}_e\left(t_h^i\right)\]}^T$

$n={\[t_0^6,6t_0^5,30t_0^4,{\left(t_h^i\right)}^6,6{\left(t_h^i\right)}^5,30{\left(t_h^i\right)}^4\]}^T$

其中，表示对x_e(·)的一阶求导，表示对x_e(·)的二阶求导，表示对y_e(·)的一阶求导，表示对y_e(·)的二阶求导；

43：定义性能指标J(x_e,y_e)为偏离连接x₀和x_g的曲线的偏差之和，满足以下公式：

$J(x_e,y_e)={\∫}_{t_0}^{t_g}\[{(x_e\left(t\right)-x_n\left(t\right))}^2+{\left(y_e\left(t\right)-y_n\left(t\right)\right)}^2\]dt---\left(4\right)$

$\begin{array}{c}{y}_n=y_0\left(1-2\frac{x_n-x_g}{x_0-x_g}\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+y_g\left(1-2\frac{x_n-x_g}{x_g-x_0}\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\\ +{\mathrm{tan\θ}}_0\left(x_n-x_0\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+{\mathrm{tan\θ}}_g\left(x_n-x_g\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\end{array}$

其中，x_n＝x_n(t)，y_n＝y_n(t)，(x_n,y_n)为连接x₀和x_g的曲线；

44：根据x₀和联立式(3)和式(4)求解得到待定系数a_k、b_k，待定系数a_k、b_k代入公式(2)得到车辆的轨迹(x_e,y_e)，即从x₀到的候选路径。

所述步骤S5中根据代价函数从各候选路径中选取得到最优路径，所述代价函数满足以下公式：

J_i＝w₁J_o,i,cp+w₂J_o,i,dev(5)

其中，J_i为从x₀到的候选路径的代价，w₁为对应安全性指标的权重系数，w₂为对应快速性指标的权重系数，J_o,i,cp为从x₀到的候选路径的安全性指标，J_o,i,dev为从x₀到的候选路径的快速性指标，最优路径为J_i最小的候选路径。

所述从x₀到的候选路径的安全性指标J_o,i,cp满足以下公式：

$J_{o,i,cp}=\left\{\begin{array}{cc}\mathrm{\∞},& E_{obs}\∩E_{veh}\≠0\\ \underset{j=1}{\overset{j=N}{\Σ}}{P}_{coll}\left(X_i\right(j),Y_i(j\left)\right),& otherwise\end{array}\right.---\left(6\right)$

其中，E_obs为在预测周期内无人车的位置分布，E_veh为在预测周期内障碍物的位置分布，E_obs∩E_veh≠0表示在每个预测时刻障碍物的位置分布与无人车的位置分布有交集，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，N表示候选路径的长度，P_coll(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的碰撞概率。

所述在预测周期内无人车的位置分布E_obs通过障碍物在预测周期内的控制输入和车辆运动学模型计算得到，所述障碍物在预测周期内的控制输入满足以下公式：

${\tilde{u}}_k={\[{\mathrm{\δ}}_k,v_k\]}^T=u_0+{\mathrm{\ϵ}}_k---\left(7\right)$

u₀＝[δ₀,v₀]^T

ε_k～N[0,δ_k]

其中，δ_k为障碍物在预测周期内的前轮转角输入，v_k为障碍物在预测周期内的速度输入，u₀为障碍物初始控制输入，ε_k为在初始控制输入上叠加的高斯控制输入；

所述在预测周期内障碍物的位置分布E_veh根据无人车在预测周期内的轨迹得到，所述无人车在预测周期内的轨迹满足以下公式：

${\tilde{P}}_k=P_k+{\mathrm{\ξ}}_k---\left(8\right)$

ξ_k～N[0,δ_k]

其中，P_k为待跟踪无人车的路径，ξ_k为在待跟踪无人车的路径上叠加的高斯分布。

所述在从x₀到的候选路径上的第j个点的碰撞概率P_coll(X_i(j),Y_i(j))满足以下公式：

P_coll(X_i(j),Y_i(j))＝p_v(X_i(j),Y_i(j))·p_o(X_i(j),Y_i(j))(9)

其中，p_v(X_i(j),Y_i(j))为无人车在(X_i(j),Y_i(j))处的概率，并由E_obs决定，p_o(X_i(j),Y_i(j))为障碍物在(X_i(j),Y_i(j))处的概率，并由E_veh决定。

所述从x₀到的候选路径的快速性指标J_o,i,dev满足以下公式：

$J_{o,i,dev}=\underset{j=1}{\overset{N}{\Σ}}\sqrt{{\left(X_i\left(j\right)-X_c\left(j\right)\right)}^2+{\left(Y_i\left(j\right)-X_c\left(j\right)\right)}^2}---\left(10\right)$

其中，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，(X_c(j),Y_c(j))表示参考轨迹上的第j个点的坐标，N表示候选路径的长度，所述参考轨迹为从x₀到x_c的中心线路径，x_c＝x₀，y_c＝y_g，θ_c＝θ_g，δ_c＝δ_g，v_c＝v_g， ${\stackrel{\·}{v}}_c={\stackrel{\·}{v}}_g.$

与现有技术相比，本发明具有以下优点：

1)本发明基于车辆运动学模型，考虑车辆起始状态和目标状态等约束，在可行驶区域内在线生成多条候选路径，在对候选路径进行评价和选择时，在考虑障碍物运动特性的同时，结合动态环境的特点，对运动障碍物的运动进行不确定性建模，进而对移动障碍物的运动轨迹进行预测，不仅可以确定障碍物现在的威胁，还可以评估障碍物潜在的威胁，在此基础上，建立兼顾车辆行驶安全性、快速性和平顺性等性能指标的代价函数，提出一种多性能指标下的候选轨迹评估方法，通过优化评价函数选择最优路径。该方法不仅可以满足车辆行驶的安全性要求，还能够在满足车辆模型约束的情况下保证行驶效率，同时由于评价函数是基于多性能指标的加权形式，可以通过不同的权重分配实现性能指标的协调优化。

2)本发明考虑了车辆的结构特性，建立了车辆运动学模型，同时能够处理车辆模型的运动学的线性及非线性约束，车辆运动学模型是研究无车人动态路径的基本单元，可以更好地反应无人车与障碍物的运动状态。

3)本发明考虑了车辆的运动学模型约束、非完整约束和车辆的最小转弯半径约束等，在可行驶道路边界和起始状态、目标状态的约束下，在线生成多条候选路径，候选路径提供了无人车更多的、可行驶的路径选择，是获取最优路径的前提。

4)本发明基于动态环境的特性，可以对移动障碍物的运动不确定性进行建模，得到动态环境模型，进而对移动障碍物的运动轨迹进行预测，不仅可以反映障碍物现存的威胁，还可以体现障碍物潜在的威胁，有效地提高动态环境下障碍物威胁估计的准确率。

5)本发明基于多性能指标优化框架，建立了综合考虑路径的安全性、快速性等性能指标的代价函数，在满足约束条件的基础上通过在线评估，得到各性能指标不同权重分配下的最优路径，实现了不同性能指标之间的协调优化。

6)本发明基于在线生成候选路径、在线选取最优路径的方式，在无论在结构化的高速公路还是在非结构化的城市道路，都能有效地进行路径规划，实时性强，应用范围广，并且没有车型限制，实现在没有先验知识的动态环境中进行规划，适用于结构化的高速公路和非结构化的城市街道以及更为复杂的越野环境，是一种实时的局部路径规划解决方案。

7)本发明兼具创新性实用性，可以在多个动态障碍物存在条件下实现实时规划，能够有效地提高无人车行驶的安全性。

附图说明

图1为本发明流程示意图；

图2为车辆运动学模型示意图；

图3为动态环境模型示意图；

图4为目标速度计算方法的示意图；

图5为障碍物和无人车的预测轨迹示意图；

图6为直道和弯道场景的候选轨迹示意图；

图7为直道场景下路径规划开始的示意图；

图8为直道场景下第一段轨迹跟踪完成的示意图；

图9为直道场景下到达终点的示意图；

图10为直道场景下规划路径的信息示意图；

图11为弯道场景下路径规划开始的示意图；

图12为弯道场景下无人车超过一个障碍物的示意图；

图13为弯道场景下到达终点的示意图；

图14为弯道场景下规划路径的信息示意图；

图15为实验环境感知效果的示意图；

图16为实验场地卫星图；

图17为实车实验下直道避障效果的示意图；

图18为实车试验下弯道避障效果的示意图；

图19为实车试验下数据记录示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，一种基于环境不确定性的无人车动态路径规划方法包括以下步骤：

S1：建立车辆运动学模型，该车辆运动学模型用于反应无人车和障碍物的运动状态。

如图2所示，车辆运动学模型满足以下公式：

$x={\[x,y,\mathrm{\θ},\mathrm{\δ},v,\stackrel{\·}{v}\]}^T---\left(1\right)$

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=v\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{vtan\mathrm{\δ}}{l}\end{array}\right.$

其中，x为车辆运动状态，(x,y)为以车辆后轴中心点为原点建立的坐标系下的横向坐标和纵向坐标，θ为车辆相对于坐标系X轴的航向角，v和δ分别为车辆的速度和前轮转角，l为车辆前轴与后轴之间的距离，分别对应为x、y、θ、v的一阶求导。

S2：建立表征无人车运动环境的动态环境模型，并根据动态环境模型建立重新规划路径的满足条件。

如图3所示，动态环境模型包括：

1)建立坐标系，用圆心位于(x′,y′)、半径为r′的圆表示无人车，无人车速度v′＝[v′_x,v′_y]^T，其中，v′_x、v′_y分别表示v′在X轴和Y轴上的速度分量，该无人车对应的车辆前轴与后轴之间的距离为l′，对应的前轮转向率为ω′；

2)相似地，对于环境中的障碍物，用圆心位于(x_o,q,y_o,q)、半径为r_q的圆表示，q表示障碍物的编号，障碍物速度v_o,q＝[v_o,q,x,v_o,q,y]^T，其中，v_o,q,x、v_o,q,y分别表示v_o,q在X轴和Y轴上的速度分量，图3中，在无人车(x′,y′)的周围有三个障碍物，第一个障碍物的圆心为(x_o,1,y_o,1)，对应的速度为v_o,1，对应的车辆前轴与后轴之间的距离、前轮转向率和半径为(l_o,1,w_o,1,r_o,1)，第二个障碍物的圆心为(x_o,2,y_o,2)，对应的速度为v_o,2，对应的车辆前轴与后轴之间的距离、前轮转向率和半径为(l_o,2,w_o,2,r_o,2)，第三个障碍物的圆心为(x_o,3,y_o,3)，对应的速度为v_o,3，对应的车辆前轴与后轴之间的距离、前轮转向率和半径为(l_o,3,w_o,3,r_o,3)；

3)以(x′,y′)为中心、r₁为半径的区域定义为危险区域，r＜r₁＜r_d，r_d为设定值，当障碍物位于危险区域时会非常危险，类似地，以(x′,y′)为中心、r₂为半径的区域定义为警示区域，r_d＜r₂＜r_a，r_a为设定值，当障碍物位于警示区域时，有可能会与无人车发生碰撞，无人车应时刻注意障碍物的运动情况，图3中，第一个障碍物位于危险区域，第二个障碍物位于警示区域，第三个障碍物既不在危险区域，也不在警示区域，暂时不会与无人车碰撞；

4)定义“障碍物正在靠近”的满足条件为：Δv_y·Δy＜0，Δv_y表示无人车与障碍物的横向相对速度，Δv_y＝(v_o,q,y-v′_y)，Δy表示无人车与障碍物的横向相对距离，Δy＝(y_o,q-y′)。

根据如图2所示的动态环境模型，可以得到重新规划路径的满足条件为：

1)障碍物进入危险区域；

2)障碍物进入警示区域且障碍物正在靠近；

3)当前最优路径被跟踪完毕；

4)最优路径与障碍物的运动轨迹出现相交；

当无人车运动环境满足重新规划路径的满足条件中的至少一条时，重新规划无人车的最优路径。

S3：目标状态确定：

无人车的车辆运动状态起始值已知为x₀反应无人车的起始状态，无人车的车辆运动状态初始目标值x_g反应无人车的目标状态，其中，下标0表示起始值，下标g表示初始目标值，(x_g,y_g,θ_g,δ_g)可由道路环境和路网文件等确定，无人车的目标速度v_g与道路的曲率c_g以及道路的最大限速v_max等有关，当道路曲率c_g较大而速度又很大时，很容易发生侧滑甚至侧翻，因此，道路曲率c_g是设计目标速度v_g的关键因素。目标点(x_g,y_g)处车辆速度v_g的速度上限v_gmax满足与c_g的关系曲线如图4所示，当c_g很小时，此时速度上限v_gmax为道路的限速v_max，当c_g逐渐增大时，速度上限v_gmax与道路曲率c_g的关系为

$v_{gmax}=\sqrt{\frac{a_{cmax}}{c_g}}$

其中，a_cmax是目标点(x_g,y_g)处的最大横向加速度，图4中，当道路曲率取c′_g时，对应的速度上限为v′_gmax，则当前目标速度可取得v_cur，v_cur＜v′_gmax，当c_g进一步增大，由于车辆存在最小转弯半径约束，所以当c_g增加到一定程度时，车辆就不可通过了，此时速度上限v_gmax为零，速度v_g为零。

已知x₀和x_g，可以进行仅考虑初始状态和目标状态的路径规划，包括以下步骤：

A假设车辆的轨迹为(x_e,y_e)，根据车辆的起始状态和终端状态约束可以分别得到关于x_e,y_e的六个方程，假设车辆的轨迹为一个六阶多项式：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{a}_k{t}^k\\ y_e=y_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{b}_k{t}^k\end{array}\right.---\left(2\right)$

其中，t表示时间，a_k、b_k为待定系数，k＝1,2,...,6，将式(2)带入到车辆模型的式(1)中，可以得到：

$\left\{\begin{array}{c}{x}_e\left(t_0\right)=a_0+a_1{t}_0+a_2{t}_0^2+a_3{t}_0^3+a_4{t}_0^4+a_5{t}_0^5+a_6{t}_0^6\\ {\stackrel{\·}{x}}_e\left(t_0\right)=a_1+2a_2{t}_0+3a_3{t}_0^2+4a_4{t}_0^3+5a_5{t}_0^4+6a_6{t}_0^5\\ {\stackrel{\·\·}{x}}_e\left(t_0\right)=2a_2+6a_3{t}_0+12a_4{t}_0^2+20a_5{t}_0^3+30a_6{t}_0^4\\ y_e\left(t_0\right)=b_0+b_1{t}_0+b_2{t}_0^2+b_3{t}_0^3+b_4{t}_0^4+b_5{t}_0^5+b_6{t}_0^6\\ {\stackrel{\·}{y}}_e\left(t_0\right)=b_1+2b_2{t}_0+3b_3{t}_0^2+4b_4{t}_0^3+5b_5{t}_0^4+6b_6{t}_0^5\\ {\stackrel{\·\·}{y}}_e\left(t_0\right)=2b_2+6b_3{t}_0+12b_4{t}_0^2+20b_5{t}_0^3+30b_6{t}_0^4\end{array}\right.$

其中，表示对x_e(·)的一阶求导，表示对x_e(·)的二阶求导，表示对y_e(·)的一阶求导，表示对y_e(·)的二阶求导；

整理成矩阵形式如下

$\left\{\begin{array}{c}{G}_1=Pa+{\mathrm{na}}_6\\ G_2=Pb+{\mathrm{nb}}_6\end{array}\right.$

其中，

$\left\{\begin{array}{c}a={\[a_0,a_1,a_2,a_3,a_4,a_5,a_6\]}^T\\ b={\[b_0,b_1,b_2,b_3,b_4,b_5,b_6\]}^T\\ G_1={\[x_e\left(t_0\right),{\stackrel{\·}{x}}_e\left(t_0\right),{\stackrel{\·\·}{x}}_e\left(t_0\right),x_e\left(t_g\right),{\stackrel{\·}{x}}_e\left(t_g\right),{\stackrel{\·\·}{x}}_e\left(t_g\right)\]}^T\\ G_2={\[y_e\left(t_0\right),{\stackrel{\·}{y}}_e\left(t_0\right),{\stackrel{\·\·}{y}}_e\left(t_0\right),y_e\left(t_g\right),{\stackrel{\·}{y}}_e\left(t_g\right),{\stackrel{\·\·}{y}}_e\left(t_g\right)\]}^T\\ n={\[t_0^6,6t_0^5,30t_0^4,t_g^6,6t_g^5,30t_g^4\]}^T\end{array}\right.$

$P=\left[\begin{array}{cccccc}1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\\ 1& t_g& t_g^2& t_g^3& t_g^4& t_g^5\\ 0& 1& 2t_g& 3t_g^2& 4t_g^3& t_g^4\\ 0& 0& 2& 6t_g& 12t_g^2& 20t_g^3\end{array}\right]$

则从初始状态到目标状态的路径可以表示为

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=f\left(t\right)P^{-1}\left(G_1-{\mathrm{na}}_6\right)+a_6{t}^6\\ y_e=y_e\left(t\right)=f\left(t\right)P^{-1}\left(G_2-{\mathrm{nb}}_6\right)+b_6{t}^6\end{array}\right.---\left(3\right)$

其中，f(t)＝[1,t,t²,t³,t⁴,t⁵]，t₀表示x₀初始状态下对应的时间，t_g表示x_g目标状态下对应的时间。

B定义性能指标。由上可知，待定轨迹是由7组位置参数组成的多项式，而已知条件仅有6组方程，多出的一组参数作为路径的新增的一个自由度，旨在满足自定义的性能指标。

定义性能指标J(x_e,y_e)为偏离连接x₀和x_g的曲线的偏差之和，满足以下公式：

$J(x_e,y_e)={\∫}_{t_0}^{t_g}\[{\left(x_e\left(t\right)-x_n\left(t\right)\right)}^2+{\left(y_e\left(t\right)-y_n\left(t\right)\right)}^2\]dt---\left(4\right)$

其中，x_n＝x_n(t)，y_n＝y_n(t)，(x_n,y_n)为连接x₀和x_g的曲线，可用艾尔米特插值得到，对于曲线(x_n,y_n)上的每一点满足下式：

$\begin{array}{c}{y}_n=y_0\left(1-2\frac{x_n-x_g}{x_0-x_g}\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+y_g\left(1-2\frac{x_n-x_g}{x_g-x_0}\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\\ +{\mathrm{tan\θ}}_0\left(x_n-x_0\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+{\mathrm{tan\θ}}_g\left(x_n-x_g\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\end{array}$

这里的曲线(x_n,y_n)仅考虑了位置和航向等约束、初始状态约束(x₀,y₀,θ₀)和目标状态约束(x_g,y_g,θ_g)。

C求解路径的待定系数。上述式(2)的优化指标为线性二次型，再结合上述式(3)的6组方程，可以确定规划路径的的7组系数，从而求得不考虑障碍物约束条件下从初始状态到目标状态的路径(x_e,y_e)。

上述路径生成是在不考虑障碍物的情况下进行的，当考虑障碍物的威胁时，首先在线生成多条候选路径，然后在线选取各项性能指标综合最优的一条作为最终的行驶路径。因此，生成候选路径前，首先生成一系列候选目标点，候选目标点依据道路边界等确定，具体为道路前方相同纵向距离处横向等间隔分布的一系列点，每个目标点的纵向坐标和航向均相同，具体为：根据x_g获取多个车辆运动状态候选目标值 $x_h^i={\[x_h^i,y_h^i,{\mathrm{\θ}}_h^i,{\mathrm{\δ}}_h^i,v_h^i,{\stackrel{\·}{v}}_h^i\]}^T,y_h^i=y_g,{\mathrm{\θ}}_h^i={\mathrm{\θ}}_g,{\mathrm{\δ}}_h^i={\mathrm{\δ}}_g,v_h^i=v_g,{\stackrel{\·}{v}}_h^i={\stackrel{\·}{v}}_g,$ 下标h表示候选目标值，上标i表示第i个候选目标值，d为设定的横向间隔，为后续生成候选路径做准备。

S4：基于车辆运动学模型，对于每个候选目标点依据仅考虑初始状态和目标状态的路径生成方法，生成一条对应的候选路，步骤S4具体为：

41：设车辆的轨迹(x_e,y_e)为六阶多项式，满足式(2)；

42：结合车辆运动学模型和六阶多项式(2)，根据x₀和将车辆的轨迹(x_e,y_e)表示为：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=f\left(t\right)P^{-1}\left(G_1-{\mathrm{na}}_6\right)+a_6{t}^6\\ y_e=y_e\left(t\right)=f\left(t\right)P^{-1}\left(G_2-{\mathrm{nb}}_6\right)+b_6{t}^6\end{array}\right.x---\left(3\right)$

f(t)＝[1,t,t²,t³,t⁴,t⁵]

$P=\left[\begin{array}{cccccc}1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\\ 1& t_h^i& {\left(t_h^i\right)}^2& {\left(t_h^i\right)}^3& {\left(t_h^i\right)}^4& {\left(t_h^i\right)}^5\\ 0& 1& 2t_h^i& 3{\left(t_h^i\right)}^2& 4{\left(t_h^i\right)}^3& 5{\left(t_h^i\right)}^4\\ 0& 0& 2& 6t_h^i& 12{\left(t_h^i\right)}^2& 20{\left(t_h^i\right)}^3\end{array}\right]$

$G_1={\[x_e\left(t_0\right),{\stackrel{\·}{x}}_e\left(t_0\right),{\stackrel{\·\·}{x}}_e\left(t_0\right),x_e\left(t_h^i\right),{\stackrel{\·}{x}}_e\left(t_h^i\right),{\stackrel{\·\·}{x}}_e\left(t_h^i\right)\]}^T$

$G_2={\[y_e\left(t_0\right),{\stackrel{\·}{y}}_e\left(t_0\right),{\stackrel{\·\·}{y}}_e\left(t_0\right),y_e\left(t_h^i\right),{\stackrel{\·}{y}}_e\left(t_h^i\right),{\stackrel{\·\·}{y}}_e\left(t_h^i\right)\]}^T$

$n={\[t_0^6,6t_0^5,30t_0^4,{\left(t_h^i\right)}^6,6{\left(t_h^i\right)}^5,30{\left(t_h^i\right)}^4\]}^T$

43：根据式(3)获取性能指标J(x_e,y_e)；

44：根据x₀和联立式(3)和式(4)求解得到待定系数a_k、b_k，待定系数a_k、b_k代入公式(2)得到车辆的轨迹(x_e,y_e)，即从x₀到的候选路径生成的候选路径如图6所示。

S5：在线路径的选取主要是考虑路径的安全性、快速性等指标，因此，通过定义兼顾安全性和快速性的性能指标，选择性能指标最优的路径作为最终的路径，具体为：

51：评估候选路径的安全性指标：在考虑障碍物运动不确定性以及无人车自身不确定性的情况下提出了基于碰撞概率的障碍物威胁估计模型，将碰撞概率作为衡量安全性高低的指标，碰撞概率越大，安全性越低，反之，安全性越高。具体步骤为：

511：障碍物运动不确定性分析

障碍物运动的不确定性主要来自障碍物输入的不确定性，无人车传感器仅能获得障碍物当前时刻的状态信息，但障碍物未来一段时间内(预测周期)的控制输入却无法预知，障碍物在预测周期内的运动轨迹也就无法预知，但障碍物的控制输入在未来很短的一段时间内不会发生突变，应该在当前控制输入周围变化，可以基于高斯分布对障碍物的控制输入进行建模，因此，障碍物在预测周期内的控制输入满足以下公式：

${\tilde{u}}_k={\[{\mathrm{\δ}}_k,v_k\]}^T=u_0+{\mathrm{\ϵ}}_k---\left(7\right)$

u₀＝[δ₀,v₀]^T

ε_k～N[0,δ_k]

其中，δ_k为障碍物在预测周期内的前轮转角输入，v_k为障碍物在预测周期内的速度输入，u₀为障碍物初始控制输入，ε_k为在初始控制输入上叠加的高斯控制输入，随着预测的不断推进δ_k会不断变大，因为预测的越远，预测的不确定性会变大。

得到障碍物在未来一段时间内的控制输入，则可由车辆运动学模型(考虑到道路上的障碍物多为车辆)得到障碍物在预测周期内的位置分布E_obs。

512：无人车运动不确定性分析

无人车运动的不确定性主要为控制层跟踪轨迹时引起的偏差，这种不确定性同样符合高斯分布的特性，所以无人车的不确定性可以用高斯分布表示，无人车在预测周期内的轨迹满足以下公式

${\tilde{P}}_k=P_k+{\mathrm{\ξ}}_k---\left(8\right)$

ξ_k～N[0,δ_k]

其中，P_k为待跟踪无人车的路径，ξ_k为在待跟踪无人车的路径上叠加的高斯分布。

不同于障碍物的不确定性，在每个预测时刻δ_k均相同，这样在未来一段时间无人车的位置分布也可以得到，则在预测周期内障碍物的位置分布E_veh根据无人车在预测周期内的轨迹得到。

513：碰撞概率的计算

将无人车和障碍物的位置分布统一于无人车自身坐标系下，得到两者的概率分布如图5所示，将障碍物与无人车在同一时刻出现在同一位置的概率(称之为碰撞概率)作为衡量该位置的安全性指标。即

P_coll(pos)＝p_v(pos)·p_o(pos)

其中，p_v(pos),p_o(pos)分别表示无人车和障碍物在位置pos处的概率。

根据上式的定义可得，在从x₀到的候选路径上的第j个点的碰撞概率P_coll(X_i(j),Y_i(j))满足以下公式：

P_coll(X_i(j),Y_i(j))＝p_v(X_i(j),Y_i(j))·p_o(X_i(j),Y_i(j))(9)

其中，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，p_v(X_i(j),Y_i(j))为无人车在(X_i(j),Y_i(j))处的概率，并由E_obs决定，p_o(X_i(j),Y_i(j))为障碍物在(X_i(j),Y_i(j))处的概率，并由E_veh决定。

图5中包括障碍物的位置分布、无人车的位置分布和碰撞概率的分布(这里为了便于观察，将碰撞概率放大50倍)。

514：安全性指标的评估

路径的安全性主要通过碰撞概率进行评估。在每个预测时刻，如果障碍物的位置分布的包络与无人车的位置分布包络有交集，则认为极有可能会发生碰撞，此时的威胁置为∞。否则，则将路径上每一点的威胁值进行加和作为该条候选路径的安全性指数。第i条从x₀到的候选轨迹的安全性指数J_o,i,cp满足以下公式：

$J_{o,i,cp}=\left\{\begin{array}{cc}\mathrm{\∞},& E_{obs}\∩E_{veh}\≠0\\ \underset{j=1}{\overset{j=N}{\Σ}}{P}_{coll}\left(X_i\left(j\right),Y_i\left(j\right)\right),& otherwise\end{array}\right.---\left(6\right)$

其中，E_obs∩E_veh≠0表示在每个预测时刻障碍物的位置分布与无人车的位置分布有交集，N表示候选路径的长度。

52：评估候选路径的快速性指标，

快速性指标定义为候选路径上的每一点与对应时刻参考轨迹上每一点的偏移之和，参考轨迹为从x₀到x_c的中心线路径，该中心线路径的求法同步骤4的候选路径求法，x_c＝x₀，y_c＝y_g，θ_c＝θ_g，δ_c＝δ_g，v_c＝v_g，第i条从x₀到的候选路径的快速性指标J_o,i,dev满足以下公式：

$J_{o,i,dev}=\underset{j=1}{\overset{N}{\Σ}}\sqrt{{\left(X_i\right(j)-X_c(j\left)\right)}^2+{\left(Y_i\right(j)-X_c(j\left)\right)}^2}---\left(10\right)$

其中，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，(X_c(j),Y_c(j))表示参考轨迹上的第j个点的坐标，N表示候选路径的长度。

53：代价函数设计

综合考虑路径的安全性指标和快速性指标，代价函数设计如下：

J_i＝w₁J_o,i,cp+w₂J_o,i,dev(5)

其中，J_i为从x₀到的候选路径的代价，w₁为对应安全性指标的权重系数，w₂为对应快速性指标的权重系数，J_o,i,cp为从x₀到的候选路径的安全性指标，J_o,i,dev为从x₀到的候选路径的快速性指标，可通过调整w₁、w₂调整对应的性能，选择性能最优的路径作为最终的路径，即最优路径为J_i最小的候选路径。

S6：重规划条件：S5步骤中被选择的路径作为最终的路径直接被运动控制层跟踪，但在无人车轨迹跟踪的过程中，由于无人车的传感器探测距离的限制，对于环境中突然出现或者消失的障碍物，无人车需要判断当前路径是否仍然安全可行，是否需要重新规划。当无人车运动环境满足重新规划路径的满足条件时，跳转步骤S3，无人车需要重新规划无人车的最优路径。

在直道和弯道场景下进行本发明方法的路径规划仿真，为了简化规划过程，假设，无人车起始点的横坐标为当前车道横向的中心点，候选目标点集合的确定时，首先，确定根据路沿情况确定前方预瞄距离处的目标点(一般取的是预瞄距离处车道横向的中心点)，即图7中的深色方块所示的“目标点”，该“目标点”直接作为一个候选目标点，然后以此点为基准，以一边车道线和另一边车道线的中心线为边界，分别向左右以一定的横向偏移d去其余一系列候选目标点，这些候选目标点的纵向坐标和航向角均相同，只是相邻两个候选目标点之间的横向坐标有d的偏移。因为中心线路径是无障碍时无人车的最短路径，图6中目标点与中心线路径的终点重合，且中心线路径为候选路径的其中一条，也是筛选候选路径时的参考轨迹。图6为直道和弯道场景下生成的候选路径示意图。

直道场景下的仿真结果如图7至图10所示。无人车在直道场景下从起点到终点之间划分为两段进行规划，图7中无人车在第一段的起点感应到三个障碍物，考虑到此三个障碍物的运动轨迹进行路径规划得到对应的最优路径，因为第一段轨迹跟踪过程中没有出现重新规划的情况，无人车第一段轨迹的跟踪结果如图8所示，接着开始第二段轨迹的规划，图8中无人车在第二段的起点处感应到另外两个障碍物，考虑到此两个障碍物的运动轨迹进行路径规划得到对应的最优路径，因为第二段轨迹跟踪过程中没有出现重新规划的情况，无人车到达终点，如图9所示，无人车在终点处也感应到三个障碍物，图10为直道场景下无人车规划路径的航向、前轮转角、速度和前轮转向率信息。

弯道场景下的仿真结果如图11至图14所示。图11表示弯道场景下路径规划开始，图12表示弯道场景下无人车超过一个障碍物，图13表示弯道场景下无人车到达终点，图14为弯道场景下无人车规划路径的航向、前轮转角、速度和前轮转向率信息。

图15为实车试验时无人车环境感应效果示意图，图16为实车试验的试验场地卫星图，无人车需在图16中黑色闭合道路上行驶，对应的实车试验结果如图17至图19所示，图17为实车实验下直道避障效果，图18为实车试验下弯道避障效果，图19为实车试验下无人车数据记录，可以看出无人车采用本发明方法行驶的路径与图16的道路一致，且实际所花费的时间远远少于计划的时间，实时性好。

综上，本发明针对无人驾驶车辆的结构和运动学特性，提出一种基于车辆运动学的动态路径规划方法，该方法根据车辆的运动学模型、起始点和目标点状态约束等建立起等式约束，将路径假设为多项式曲线，通过等式约束将路径规划问题转化为多项式系数求解问题，进而求得在不考虑障碍物情况下的路径。当在实际环境中，通过道路信息确定多个候选目标点，并利用上述方法生成对应的候选轨迹，通过对障碍物的运动不确定性和无人车的运动不确定性进行分析建模，提出基于碰撞概率的障碍物威胁估计模型，设计兼顾路径的安全性、快速性等性能指标的代价函数，选择出最优的路径，完成动态环境下的路径规划，为无人驾驶车辆的运动控制提供了基础。

Claims (10)

1.一种基于环境不确定性的无人车动态路径规划方法，其特征在于，包括以下步骤：

S1：建立车辆运动学模型，无人车和障碍物均满足车辆运动学模型，所述车辆运动学模型满足以下公式：

$x={\[x,y,\mathrm{\θ},\mathrm{\δ},v,\stackrel{\·}{v}\]}^T---\left(1\right)$

$\left\{\begin{array}{c}\stackrel{\·}{x}=vcos\mathrm{\θ}\\ \stackrel{\·}{y}=vsin\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\frac{v\tan \mathrm{\δ}}{l}\end{array}\right.$

其中，x为车辆运动状态，(x,y)为以车辆后轴中心点为原点建立的坐标系下的横向坐标和纵向坐标，θ为车辆相对于坐标系X轴的航向角，v和δ分别为车辆的速度和前轮转角，l为车辆前轴与后轴之间的距离，分别对应为x、y、θ、v的一阶求导；

S2：建立表征无人车运动环境的动态环境模型，并根据动态环境模型建立重新规划路径的满足条件；

S3：获取无人车的车辆运动状态起始值x₀和车辆运动状态初始目标值x_g，并根据x_g获取多个车辆运动状态候选目标值其中， $x_g={\[x_g,y_g,{\mathrm{\θ}}_g,{\mathrm{\δ}}_g,v_g,{\stackrel{\·}{v}}_g\]}^T,x_h^i={\[x_h^i,y_h^i,{\mathrm{\θ}}_h^i,{\mathrm{\δ}}_h^i,v_h^i,{\stackrel{\·}{v}}_h^i\]}^T,y_h^i=y_g,{\mathrm{\θ}}_h^i={\mathrm{\θ}}_g,{\mathrm{\δ}}_h^i={\mathrm{\δ}}_g,$ 下标0表示起始值，下标g表示初始目标值，下标h表示候选目标值，上标i表示第i个候选目标值，d为设定的横向间隔；

S4：基于车辆运动学模型，生成从x₀到的候选路径；

S5：评估候选路径的安全性指标和快速性指标，基于安全性指标和快速性指标从各候选路径中选取得到最优路径；

S6：跟踪无人车以最优路径运动的过程，当无人车运动环境满足重新规划路径的满足条件时，跳转步骤S3，重新规划无人车的最优路径。

2.根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述动态环境模型包括：

1)建立坐标系，用圆心位于(x′,y′)、半径为r′的圆表示无人车，无人车速度v′＝[v′_x,v′_y]^T，其中，v′_x、v′_y分别表示v′在X轴和Y轴上的速度分量；

2)用圆心位于(x_o,q,y_o,q)、半径为r_q的圆表示障碍物，q表示障碍物的编号，障碍物速度v_o,q＝[v_o,q,x,v_o,q,y]^T，其中，v_o,q,x、v_o,q,y分别表示v_o,q在X轴和Y轴上的速度分量；

3)以(x′,y′)为中心、r₁为半径的区域定义为危险区域，r＜r₁＜r_d，r_d为设定值，以(x′,y′)为中心、r₂为半径的区域定义为警示区域，r_d＜r₂＜r_a，r_a为设定值；

4)定义障碍物正在靠近的满足条件为：Δv_y·Δy＜0，Δv_y表示无人车与障碍物的横向相对速度，Δv_y＝(v_o,q,y-v′_y)，Δy表示无人车与障碍物的横向相对距离，Δy＝(y_o,q-y′)。

3.根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述重新规划路径的满足条件为：

1)障碍物进入危险区域；

2)障碍物进入警示区域且障碍物正在靠近；

3)当前最优路径被跟踪完毕；

4)最优路径与障碍物的运动轨迹出现相交；

当无人车运动环境满足重新规划路径的满足条件中的至少一条时，重新规划无人车的最优路径。

4.根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述步骤S3中车辆运动状态初始目标值x_g中的速度目标值v_g与道路的曲率和道路的限速有关。

5.根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述步骤S4具体为：

41：设车辆的轨迹(x_e,y_e)为六阶多项式，满足以下公式：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{a}_k{t}^k\\ y_e=y_e\left(t\right)=\underset{k=0}{\overset{6}{\Σ}}{b}_k{t}^k\end{array}\right.---\left(2\right)$

其中，t为时间，a_k、b_k为待定系数；

42：结合车辆运动学模型和六阶多项式，根据x₀和将车辆的轨迹(x_e,y_e)表示为：

$\left\{\begin{array}{c}{x}_e=x_e\left(t\right)=f\left(t\right)P^{-1}\left(G_1-{\mathrm{na}}_6\right)+a_6{t}^6\\ y_e=y_e\left(t\right)=f\left(t\right)P^{-1}\left(G_2-{\mathrm{nb}}_6\right)+b_6{t}^6\end{array}\right.---\left(3\right)$

$f\left(t\right)=\[1,t,t^2,t^3,t^4,t^5\]$

$P=\left[\begin{array}{cccccc}1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\\ 1& t_h^i& {\left(t_h^i\right)}^2& {\left(t_h^i\right)}^3& {\left(t_h^i\right)}^4& {\left(t_h^i\right)}^5\\ 0& 1& 2t_h^i& 3{\left(t_h^i\right)}^2& 4{\left(t_h^i\right)}^3& 5{\left(t_h^i\right)}^4\\ 0& 0& 2& 6t_h^i& 12{\left(t_h^i\right)}^2& 20{\left(t_h^i\right)}^3\end{array}\right]$

$G_1={\[x_e\left(t_0\right),{\stackrel{\·}{x}}_e\left(t_0\right),{\stackrel{\·\·}{x}}_e\left(t_0\right),x_e\left(t_h^i\right),{\stackrel{\·}{x}}_e\left(t_h^i\right),{\stackrel{\·\·}{x}}_e\left(t_h^i\right)\]}^T$

$G_2={\[y_e\left(t_0\right),{\stackrel{\·}{y}}_e\left(t_0\right),{\stackrel{\·\·}{y}}_e\left(t_0\right),y_e\left(t_h^i\right),{\stackrel{\·}{y}}_e\left(t_h^i\right),{\stackrel{\·\·}{y}}_e\left(t_h^i\right)\]}^T$

$n={\[t_0^6,6t_0^5,30t_0^4,{\left(t_h^i\right)}^6,6{\left(t_h^i\right)}^5,30{\left(t_h^i\right)}^4\]}^T$

其中，表示对的一阶求导，表示对的二阶求导，表示对y_e(·)的一阶求导，表示对y_e(·)的二阶求导；

43：定义性能指标J(x_e,y_e)为偏离连接x₀和x_g的曲线的偏差之和，满足以下公式：

$J(x_e,y_e)={\∫}_{t_0}^{t_g}\[{\left(x_e\right(t)-x_n(t\left)\right)}^2+{\left(y_e\right(t)-y_n(t\left)\right)}^2\]dt---\left(4\right)$

$\begin{array}{c}{y}_n=y_0\left(1-2\frac{x_n-x_0}{x_0-x_g}\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+y_g\left(1-2\frac{x_n-x_g}{x_g-x_0}\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\\ +{\mathrm{tan\θ}}_0\left(x_n-x_0\right){\left(\frac{x_n-x_g}{x_0-x_g}\right)}^2+{\mathrm{tan\θ}}_g\left(x_n-x_g\right){\left(\frac{x_n-x_0}{x_g-x_0}\right)}^2\end{array}$

其中，x_n＝x_n(t)，y_n＝y_n(t)，(x_n,y_n)为连接x₀和x_g的曲线；

44：根据x₀和联立式(3)和式(4)求解得到待定系数a_k、b_k，待定系数a_k、b_k代入公式(2)得到车辆的轨迹(x_e,y_e)，即从x₀到的候选路径。

6.根据权利要求1所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述步骤S5中根据代价函数从各候选路径中选取得到最优路径，所述代价函数满足以下公式：

J_i＝w₁J_o,i,cp+w₂J_o,i,dev(5)

其中，J_i为从x₀到的候选路径的代价，w₁为对应安全性指标的权重系数，w₂为对应快速性指标的权重系数，J_o,i,cp为从x₀到的候选路径的安全性指标，J_o,i,dev为从x₀到的候选路径的快速性指标，最优路径为J_i最小的候选路径。

7.根据权利要求6所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述从x₀到的候选路径的安全性指标J_o,i,cp满足以下公式：

$J_{o,i,cp}=\left\{\begin{array}{cc}\mathrm{\∞},& E_{obs}\∩E_{veh}\≠0\\ \underset{j=1}{\overset{j=N}{\mathrm{\Σ}}}{P}_{coll}\left(X_i\left(j\right),Y_i\left(j\right)\right),& otherwise\end{array}\right.---\left(6\right)$

其中，E_obs为在预测周期内无人车的位置分布，E_veh为在预测周期内障碍物的位置分布，E_obs∩E_veh≠0表示在每个预测时刻障碍物的位置分布与无人车的位置分布有交集，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，N表示候选路径的长度，P_coll(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的碰撞概率。

8.根据权利要求7所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述在预测周期内无人车的位置分布E_obs通过障碍物在预测周期内的控制输入和车辆运动学模型计算得到，所述障碍物在预测周期内的控制输入满足以下公式：

${\tilde{u}}_k={\[{\mathrm{\δ}}_k,v_k\]}^T=u_0+{\mathrm{\ϵ}}_k---\left(7\right)$

u₀＝[δ₀,v₀]^T

ε_k～N[0,δ_k]

其中，δ_k为障碍物在预测周期内的前轮转角输入，v_k为障碍物在预测周期内的速度输入，u₀为障碍物初始控制输入，ε_k为在初始控制输入上叠加的高斯控制输入；

所述在预测周期内障碍物的位置分布E_veh根据无人车在预测周期内的轨迹得到，所述无人车在预测周期内的轨迹满足以下公式：

${\tilde{P}}_k=P_k+{\mathrm{\ξ}}_k---\left(8\right)$

ξ_k～N[0,δ_k]

其中，P_k为待跟踪无人车的路径，ξ_k为在待跟踪无人车的路径上叠加的高斯分布。

9.根据权利要求7所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述在从x₀到的候选路径上的第j个点的碰撞概率P_coll(X_i(j),Y_i(j))满足以下公式：

P_coll(X_i(j),Y_i(j))＝p_v(X_i(j),Y_i(j))·p_o(X_i(j),Y_i(j))(9)

其中，p_v(X_i(j),Y_i(j))为无人车在(X_i(j),Y_i(j))处的概率，并由E_obs决定，p_o(X_i(j),Y_i(j))为障碍物在(X_i(j),Y_i(j))处的概率，并由E_veh决定。

10.根据权利要求6所述的一种基于环境不确定性的无人车动态路径规划方法，其特征在于，所述从x₀到的候选路径的快速性指标J_o,i,dev满足以下公式：

$J_{o,i,dev}=\underset{j=1}{\overset{N}{\Σ}}\sqrt{{\left(X_i\right(j)-X_c(j\left)\right)}^2+{\left(Y_i\right(j)-X_c(j\left)\right)}^2}---\left(10\right)$

其中，(X_i(j),Y_i(j))表示在从x₀到的候选路径上的第j个点的坐标，(X_c(j),Y_c(j))表示参考轨迹上的第j个点的坐标，N表示候选路径的长度，所述参考轨迹为从x₀到x_c的中心线路径，x_c＝x₀，y_c＝y_g，θ_c＝θ_g，δ_c＝δ_g，v_c＝v_g， ${\stackrel{\·}{v}}_c={\stackrel{\·}{v}}_g.$