具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种面向异构车辆的统一运动规划方法和系统，对转向机构不同所导致的运动特性差异进行建模，并将其融入到运动规划系统中，以能够提高异构车辆运动控制的精确性。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1和图2所示，本发明提供的一种面向异构车辆的统一运动规划方法，包括：

步骤100：将扩展基元和备选基元进行拼接，得到当前时刻通行区域内的扩展基元簇。扩展基元为异构车辆上一时刻获取的行为基元。备选基元为异构车辆运动基元库中选择的行为基元。当处于初始位置时，直接放置初始速度为0的基元簇，不需要完成与初始位置的拼接。基元的扩展如图4所示。

采用三次Hermite插值完成过渡段中速度的生成，以变速直线运动构建基元之间的过渡。具体形式如下所示：

式中：l ₀=(t-t ₁)/(t ₀-t ₁)，l ₁=(t-t ₀)/(t ₁-t ₀) 由于过渡基元所要关联的起末点加速度都为零，即v _n =0。因此通过对v(t)求导，能够得到过渡基元加速度的函数a(t)，该函数在t _a=(t ₁-t ₀)/2处取得最大值，在最大值处的取值如下式所示：

式中：Δv=v ₁ -v ₀是基元要过渡的速度偏差，求解得到t₁即可生成整个过渡段各点的速度值，构成变速直线基元过渡段。

步骤101：在在线状态下，以扩展基元簇为输入，基于异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价选择出最优的行为基元。该步骤的实施过程可以为：

基于异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价确定扩展基元簇中每一行为基元的代价值。

选择代价值最低的行为基元作为最优的行为基元。

步骤102：基于最优的行为基元得到异构车辆的运动规划结果。该步骤的实施过程可以为：

将最优的行为基元作为新的扩展基元，并返回步骤“将扩展基元和备选基元进行拼接，得到扩展基元簇”，直至生成从起始点到终止点的基元序列。从起始点到终止点的基元序列由不同时刻的最优的行为基元构成。

基于从起始点到终止点的基元序列生成异构车辆运动的期望轨迹时序点，得到异构车辆的运动规划结果。

上述步骤101和步骤102的具体实施过程为：

1）、与参考轨迹和期望速度的偏差代价：

式中：N _p 是生成轨迹上等间隔采样点的总数目，p是高斯函数，j _n 是生成轨迹采样点与参考线匹配点之间的偏差代价。

式中：高斯函数p为标准正态分布，ω_d与ω_h分别是距离偏差与航向偏差的权重系数，d _n 是与参考线的距离偏差，Δθ _n 是与参考线的航向偏差。

式中：v _r 是参考线匹配点上的期望速度，v _end 为基元终点处的速度。

2）、轨迹的平滑度代价：

轨迹的平滑度直接反映了基元上各点曲率的平均变化情况，该数值越小曲率越平滑，轨迹平滑度的定义如下式所示：

（23）

式中：是基元采样点上的曲率值，Δs _n 是相邻采样点之间的间距。是依据基元类型与车辆平台类型设定的权重系数。对于无级转向车辆（包括阿克曼转向轮式车辆与速差转向履带车辆）权重系数的定义如式（24）所示，有级转向履带车辆权重系数的定义如式（25）所示。

（24）

（25）

式中：的下标M表示方向维持，R表示方向反向，上标s表示规定转向半径，g表示非规定转向半径。

S33、碰撞风险代价：

选用六覆盖圆近似的表示车身，通过计算障碍物到圆心的距离作为碰撞风险代价的指标，具体的计算公式如下所示：

（26）

式中：是圆心与障碍物的最近距离，r _l 是车体覆盖圆的半径。

S34、基元的优化选择：

基元的在线选择需要对定义的选择代价进行综合评估，基元选择的总代价如下式所示：

式中：ω _p，ω _v ，ω _s，ω _c是相应代价指标的权重系数。在线选择模块基于计算得到的代价值从基元簇中选择出代价最低的基元，逐步扩展生成从起始点到终止点的基元序列，构成最终的期望轨迹时序点。

其中，为了进一步提高运动规划的精确性，本发明上述采用的异构车辆运动基元库的构建过程为：

在离线状态下，基于异构车辆的差异化行为约束、车辆平台的运动微分约束、车辆平台特性的不等式约束、基元的平滑过渡约束，以轨迹的平滑度为优化目标，得到三种类型车辆平台的异构运动基元库，为后续运动基元的扩展与选择提供备选集合。

在运动基元的优化生成中，车辆平台坐标系的定义及参量定义如下：

轮式阿克曼转向车辆与履带式速差转向车辆的状态参量分别为：

其中，阿克曼转向车辆平台与速差转向车辆平台的示意图如图3所示。式中，x,y是全局坐标系下的位置坐标，θ是航向角，和分别是沿车体坐标系x轴的速度和加速度。δ和ω _δ 是轮式阿克曼转向车辆的前轮偏角和角速率。Δv和ω _Δv 是两侧履带的速差及相应的变化率。

和分别是轮式车辆平台与履带式车辆平台的控制量，其中，是沿车体x坐标轴的加加速度，α _δ 是ω _δ 的变化速率，α _Δv 是ω _Δv 的变化速率，t₁和t_g是基元的起始和终止时间。

1）、以三种异构车辆各自的转向机构结构特点为依据，设置异构车辆的差异化行为约束。

本发明涉及的异构车辆主要有三类，分别是阿克曼转向轮式车辆、无级速差转向履带车辆和有级速差转向履带车辆。典型的驾驶行为包括：直驶、调头、U型弯、直角弯、换道等。每一个行为类别都对应于一组设定的起末点位姿。

以调头行为为例，三种不同类型车辆平台的差异化行为约束为：

阿克曼转向履带车辆的行为约束为，无级速差转向履带车辆的行为约束为，有级转向履带车辆的行为约束为。

有级转向履带车辆的行为约束为，首先设定起始位姿与终止位姿的起末点约束。其次，将作为Dubins曲线的起末状态约束，Dubins曲线通过固定半径圆弧与直线组合的形式完成生成，其中固定半径圆弧的数值设定与有级转向履带车辆的规定转向半径相同。最后，在生成的Dubins曲线上选取N _s 个采样点构成有级转向履带车辆的行为约束，其中，除去起始点b ₁与终止点需要对位置和航向进行约束外，其余各采样点b _n 仅对位置进行约束。如图4所示。

2）、分别对异构车辆平台的运动微分约束进行设置，其中有级/无级速差转向履带车辆的约束设置一致都为，阿克曼转向轮式车辆的约束为。

其中L是轮式车前后轴之间的距离，B是两侧履带之间的间距。，其中和是左右两侧履带的速度。

3）、分别对异构车辆平台特性的不等式约束进行设置，其中有级/无级速差转向履带车辆的约束设置一致都为U _t ，阿克曼转向轮式车辆的约束为Uw。

4）、分别对异构车辆平台特性的平滑过渡约束进行设置，其中有级/无级速差转向履带车辆的约束设置一致都为T _t ，阿克曼转向轮式车辆的约束为T _w。

5）、设置基元优化生成的目标函数，并结合上述约束条件，对优化问题进行求解，得到突显异构车辆独特运动特性的运动基元库。其中有级/无级速差转向履带车辆的优化目标设置一致都为g _t，阿克曼转向轮式车辆的约束为g _w。

式中：c ₁,c ₂,c ₃,c ₄,c ₅是相应的权重系数。该目标函数的值越小，说明生成的轨迹越平滑。

因此优化问题的求解，就是求解g _t/w取值最小时的车辆控制量u，并且在求解过程中需要满足约束条件f、B、U、T。

6）、通过1）-5）的步骤，能够得到一个特定行为下的行为基元，每一个行为基元都包含以等时间隔0.1s分布的轨迹点。在遍历不同的行为约束B后，便生成了包含多种行为模式的异构车辆运动基元库。

对应于上述提供的面向异构车辆的统一运动规划方法，本发明还提供了一种面向异构车辆的统一运动规划系统，如图5所示，该系统包括：基元扩展模块1、基元选择模块2和运动规划模块3。

基元扩展模块1用于将扩展基元和备选基元进行拼接，得到当前时刻通行区域内的扩展基元簇。扩展基元为异构车辆上一时刻获取的行为基元。备选基元为异构车辆运动基元库中选择的行为基元。

基元选择模块2用于在在线状态下，以扩展基元簇为输入，基于异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价选择出最优的行为基元。

运动规划模块3用于基于最优的行为基元得到异构车辆的运动规划结果。

其中，基元选择模块2包括：代价值确定单元和基元选择单元。

代价值确定单元用于基于异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价确定扩展基元簇中每一行为基元的代价值。

基元选择单元用于选择代价值最低的行为基元作为最优的行为基元。

运动规划模块3包括：基元序列生成单元和运动规划单元。

基元序列生成单元用于将最优的行为基元作为新的扩展基元，并返回步骤“将扩展基元和备选基元进行拼接，得到扩展基元簇”，直至生成从起始点到终止点的基元序列。从起始点到终止点的基元序列由不同时刻的最优的行为基元构成。

运动规划单元用于基于从起始点到终止点的基元序列生成异构车辆运动的期望轨迹时序点，得到异构车辆的运动规划结果。

本发明上述提供的面向异构车辆的统一运动规划系统还包括：运动基元库构建模块。

运动基元库构建模块用于构建异构车辆运动基元库。异构车辆运动基元库中的行为基元包括以等时间间隔分布的轨迹点。

上述运动基元库构建模块可以包括：设置单元、行为基元确定单元和运动基元库生成单元。

设置单元用于设置异构车辆的差异化行为约束、异构车辆平台的运动微分约束、异构车辆平台特性的不等式约束、异构车辆平台特性的平滑过渡约束和基元优化生成的目标函数。异构车辆的差异化行为约束包括：阿克曼转向履带车辆行为约束、无极速差转向履带车辆行为约束和有级速差转向履带车辆行为约束。

行为基元确定单元用于基于异构车辆的差异化行为约束、异构车辆平台的运动微分约束、异构车辆平台特性的不等式约束和异构车辆平台特性的平滑过渡约束对基元优化生成的目标函数进行求解，得到特定行为下的行为基元。

运动基元库生成单元用于遍历不同的异构车辆的差异化行为约束下的行为基元生成异构车辆运动基元库。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。