系为
[0120]其中，a?ax是目标点（xg，yg)处的最大横向加速度，图4中，当道路曲率取y g时，对 应的速度上限为V' gmax，则当前目标速度可取得VcimVciirCv' gmax，当Cg进一步增大，由于车辆 存在最小转弯半径约束，所以当以增加到一定程度时，车辆就不可通过了，此时速度上限 Vgmax为零，速度Vg为零。
[0121 ]已知XdPXg，可以进行仅考虑初始状态和目标状态的路径规划，包括以下步骤：
[0122] A假设车辆的轨迹为(Xe3，ye3)，根据车辆的起始状态和终端状态约束可以分别得到 关于的六个方程，假设车辆的轨迹为一个六阶多项式：
[0124]其中^表示时间^以⑶待定系数上二^^^乂"等式⑵带入到车辆模型的式 (1)中，可以得到：
[0126] 其中，毛（·)表示对xe( ·)的一阶求导，弋(·)表示对xe( ·)的二阶求导，九(·)表示 对ye( ·)的一阶求导，兄.（·)表示对·)的二阶求导；
[0127] 整理成矩阵形式如下
[0132]则从初始状态到目标状态的路径可以表示为
[0134]其中，f (t) = [ 1，t，t2，t3，t4，t5]，to表示XQ初始状态下对应的时间，tg表示Xg目标状 态下对应的时间。
[0135] B定义性能指标。由上可知，待定轨迹是由7组位置参数组成的多项式，而已知条件 仅有6组方程，多出的一组参数作为路径的新增的一个自由度，旨在满足自定义的性能指 标。
[0136] 定义性能指标J(Xe3，ye3)为偏离连接XdPXg的曲线的偏差之和，满足以下公式：
[0138]其中，xn = xn(t)，yn=yn(t)，（xn，y n)为连接XQ和Xg的曲线，可用艾尔米特插值得到， 对于曲线U，yn)上的每一点满足下式：
[0140] 这里的曲线(xn，yn)仅考虑了位置和航向等约束、初始状态约束(χο,γο,θο)和目标 状态约束(Xg，yg，9g)。
[0141] C求解路径的待定系数。上述式(2)的优化指标为线性二次型，再结合上述式(3)的 6组方程，可以确定规划路径的的7组系数，从而求得不考虑障碍物约束条件下从初始状态 到目标状态的路径(x e，ye)。
[0142] 上述路径生成是在不考虑障碍物的情况下进行的，当考虑障碍物的威胁时，首先在线 生成多条候选路径，然后在线选取各项性能指标综合最优的一条作为最终的行驶路径。因此，生 成候选路径前，首先生成一系列候选目标点，候选目标点依据道路边界等确定，具体为道路前方 相同纵向距离处横向等间隔分布的一系列点，每个目标点的纵向坐标和航向均相同，具体为:根 据Xg获取多个车辆运动状态候选目标值_x丨，:4 =卜;= ν K = xf1 = d，下标h表示候选目标值，上标i表示第i个候选目标值，d为设定的横向间隔，为 后续生成候选路径做准备。
[0143] S4:基于车辆运动学模型，对于每个候选目标点< 依据仅考虑初始状态和目标状 态的路径生成方法，生成一条对应的候选路，步骤S4具体为：
[0144] 41:设车辆的轨迹(Xe3，ye)为六阶多项式，满足式(2);
[0145] 42:结合车辆运动学模型和六阶多项式(2)，根据xo和将车辆的轨迹(xe，y e)表示 为：
[0152] 43:根据式(3)获取性能指标J(xe，ye);
[0153] 44:根据X()和_χ;A.,:联立式⑶和式⑷求解得到待定系数ak、bk，待定系数ak、bk代入 公式(2)得到车辆的轨迹(Xe3，ye3)，即从XQ到]4的候选路径(八夂)，生成的候选路径如图6 所示。
[0154] S5:在线路径的选取主要是考虑路径的安全性、快速性等指标，因此，通过定义兼 顾安全性和快速性的性能指标，选择性能指标最优的路径作为最终的路径，具体为：
[0155] 51:评估候选路径的安全性指标:在考虑障碍物运动不确定性以及无人车自身不 确定性的情况下提出了基于碰撞概率的障碍物威胁估计模型，将碰撞概率作为衡量安全性 高低的指标，碰撞概率越大，安全性越低，反之，安全性越高。具体步骤为：
[0156] 511:障碍物运动不确定性分析
[0157] 障碍物运动的不确定性主要来自障碍物输入的不确定性，无人车传感器仅能获得 障碍物当前时刻的状态信息，但障碍物未来一段时间内（预测周期）的控制输入却无法预 知，障碍物在预测周期内的运动轨迹也就无法预知，但障碍物的控制输入在未来很短的一 段时间内不会发生突变，应该在当前控制输入周围变化，可以基于高斯分布对障碍物的控 制输入进行建模，因此，障碍物在预测周期内的控制输入flf满足以下公式：
[0158] =[^,ν,]； =U〇+￡a (7)
[0159] u〇= [δ0, νο]τ
[0160] ek~N[0，Sk]
[0161] 其中，为障碍物在预测周期内的前轮转角输入，^为障碍物在预测周期内的速度 输入，uo为障碍物初始控制输入，ek为在初始控制输入上叠加的高斯控制输入，随着预测的 不断推进^会不断变大，因为预测的越远，预测的不确定性会变大。
[0162] 得到障碍物在未来一段时间内的控制输入，则可由车辆运动学模型（考虑到道路 上的障碍物多为车辆)得到障碍物在预测周期内的位置分布E〇 bs。
[0163] 512:无人车运动不确定性分析
[0164] 无人车运动的不确定性主要为控制层跟踪轨迹时引起的偏差，这种不确定性同样 符合高斯分布的特性，所以无人车的不确定性可以用高斯分布表示，无人车在预测周期内 的轨迹满足以下公式
[0165] fk=Pk.+4k <8)
[0166] |k~N[0^k]
[0167] 其中，Pk为待跟踪无人车的路径，Ik为在待跟踪无人车的路径上叠加的高斯分布。
[0168] 不同于障碍物的不确定性，在每个预测时刻均相同，这样在未来一段时间无人 车的位置分布也可以得到，则在预测周期内障碍物的位置分布Eveh根据无人车在预测周期 内的轨迹得到。
[0169] 513:碰撞概率的计算
[0170]将无人车和障碍物的位置分布统一于无人车自身坐标系下，得到两者的概率分布 如图5所示，将障碍物与无人车在同一时刻出现在同一位置的概率(称之为碰撞概率)作为 衡量该位置的安全性指标。即
[0171] Pc〇n(pos) =pv(pos) · p〇(pos)
[0172] 其中，pv(pos)，p〇(pos)分别表示无人车和障碍物在位置pos处的概率。
[0173] 根据上式的定义可得，在从xo到的候选路径上的第j个点的碰撞概率 (」），1(」））满足以下公式：
[0174] Pc〇n(Xi(j),Yi(j))=pv(Xi(j),Yi(j)) - p〇(Xi(j),Yi(j)) (9)
[0175] 其中，（^("，^彳^表示在从如到^的候选路径上的第彳个点的坐标，？"^^)，^ (j))为无人车在(Xi(j)，Yi(j))处的概率，并由E〇bs决定，Pci(Xi(j)，Yi(j))为障碍物在(Xi (j)，Yi(j))处的概率，并由Eveh决定。
[0176] 图5中包括障碍物的位置分布、无人车的位置分布和碰撞概率的分布(这里为了便 于观察，将碰撞概率放大50倍）。
[0177] 514:安全性指标的评估
[0178]路径的安全性主要通过碰撞概率进行评估。在每个预测时刻，如果障碍物的位置 分布的包络与无人车的位置分布包络有交集，则认为极有可能会发生碰撞，此时的威胁置 为~。否则，则将路径上每一点的威胁值进行加和作为该条候选路径的安全性指数。第i条 从xo到4的候选轨迹的安全性指数满足以下公式：
[0180]其中，EQbs n Eveh矣0表示在每个预测时刻障碍物的位置分布与无人车的位置分布 有交集，N表示候选路径的长度。
[0181 ] 52:评估候选路径的快速性指标，
[0182]快速性指标定义为候选路径上的每一点与对应时刻参考轨迹上每一点的偏移之 和，参考轨迹为从xo到X。的中心线路径，该中心线路径的求法同步骤4的候选路径求法， :xc = [c.s叉，死sxc = xo，yc = yg，Θ。= 0g，δ。= ，Vc = Vg，兔=..。第i条从χ0到 $ 的候选路径的快速性指标Jndev满足以下公式：
[0184] 其中，（乂"^^"彳"表示在从抑到:^的候选路径上的第彳个点的坐标，^。），^ (j))表示参考轨迹上的第j个点的坐标，N表示候选路径的长度。
[0185] 53:代价函数设计
[0186] 综合考虑路径的安全性指标和快速性指标，代价函数设计如下：
[0187] Ji - Wl J"〇, i，cp+W2j"o, i，dev ( 5 )
[0188] 其中，心为从xo到]4的候选路径的代价，W1为对应安全性指标的权重系数， W2为对应快速性指标的权重系数，J。,:^为从XQ到》4的候选路径的安全性指标，Jo,i,dev为从 X0到]4的候选路径的快速性指标，可通过调整奶、《2调整对应的性能，选择性能最优的路径 作为最终的路径，即最优路径为心最小的候选路径。
[0189] S6:重规划条件:S5步骤中被选择的路径作为最终的路径直接被运动控制层跟踪， 但在无人车轨迹跟踪的过程中，由于无人车的传感器探测距离的限制，对于环境中突然出 现或者消失的障碍物，无人车需要判断当前路径是否仍然安全可行，是否需要重新规划。当 无人车运动环境满足重新规划路径的满足条件时，跳转步骤S3,无人车需要重新规划无人 车的最优路径。
[0190] 在直道和弯道场景下进行本发明方法的路径规划仿真，为了简化规划过程，假设， 无人车起始点的横坐标为当前车道横向的中心点，候选目标点集合的确定时，首先，确定根 据路沿情况确定前方预瞄距离处的目标点（一般取的是预瞄距离处车道横向的中心点），即 图7中的深色方块所示的"目标点"，该"目标点"直接作为一个候选目标点，然后以此点为基 准，以一边车道线和另一边车道线的中心线为边界，分别向左右以一定的横向偏移d去其余 一系列候选目标点，这些候选目标点的纵向坐标和航向角均相同，只是相邻两个候选目标 点之间的横向坐标有d的偏移。因为中心线路径是无障碍时无人车的最短路径，图6中目标 点与中