一种保证无人车在三维地形上安全行驶的运动规划方法

摘要

本发明涉及一种考虑三维地形的运动规划方法，尤其涉及一种无人车辆三维地形中的运动规划，属于无人车辆运动规划领域。本发明为了保证无人车辆在三维地形面上的行驶安全性，增强在三维地形中运动规划的合理性，提出了一种保证无人车在三维地形上安全行驶的运动规划方法。主要内容分为四部分，地形环境表示与位姿投影方法是后续操作的基础。地形可通行性判别算法考虑到路面粗糙度与地面形状对可通行性的影响，保证车辆静态稳定性。三维地形路径规划包括参考路径生成和可跟踪路径生成两个步骤。速度规划包括初始速度曲线生成和速度曲线平滑两个步骤。

权利要求

1.一种保证无人车在三维地形上安全行驶的运动规划方法，其特征在于：包括如下步骤：

步骤一、得到车辆在地形面上的投影位姿采用三维激光点云表示地形环境，地形面S由地图坐标系M下表示的N个数据点组成，即pi为第i个数据点；为了分析在pi位置处的车辆位姿，利用主元分析法获取该处地形曲面的法向量，并将法向量作为新位姿的z轴向量 以两平面交线方向上的单位向量作为新位姿的x轴向量 所述两平面为以原位姿y轴向量 勾法向的平面，以及以新位姿z轴向量 为法向的平面；最终以三个单位轴向量 构成的旋转矩阵和pi的位置构成了车辆在地形面上的投影位姿；

步骤二、在步骤一得到的点云数据pi基础上，对前方的地面进行分析，从而判断生成的路径是否可用；

通过分析路面粗糙度来判断地形是否有典型障碍物或过大的起伏；所述的路面粗糙度与传统意义上的粗糙度不同，离散地形面S上任意一点pi处的粗糙度的求取方法为：利用车辆感知模块获取以pi为球心rplane为半径的球内的所有数据点，记为Bplane(pi)，并结合Bplane(pi)中的数据点用主元分析法求出pi处的法向量ni；采用半径rplane的球体内的数据点做平面拟合，采用半径rres的球体内的数据点做粗糙度值计算，所述rplane大于rres；

利用车辆感知模块获取以pi为球心rres为半径的球内的所有数据点，记为Bres(pi)；Bres(pi)内每个点到拟合平面的符号投影距离为其中pj为Bres(pi)中的数据点；

pi点处的粗糙度ρi定义为：

根据车辆尺寸定义一个最大路面粗糙度值ρmax，当地形上一点处的粗糙度值ρi＞ρmax时，则认为该处的路面粗糙度值不满足通行条件，规划的路径不可经过此处；

当ρi≤ρmax时，还需分析地面形状来判断pi处地形是否能满足车辆静态稳定性要求；获得pi处车辆投影位姿后，求取相应的车辆欧拉角，为了使车辆能够在该处保持静态稳定性，需要使车辆侧倾角ψ与俯仰角θ在一定阈值范围内，即：|ψ|≤ψmax

θmin≤θ≤θmax

其中，ψmax是最大侧倾稳定角阈值，θmin、θmax分别是最小俯仰角阈值和最大俯仰角阈值；

从而保证车辆不会发生侧向移动、侧向翻转和纵向翻转；

步骤三、路径规划：生成三维参考路径，再根据参考路径生成车辆能够跟踪的最终路径；

1)、生成三维参考路径

通过构建搜索图并使用A*搜索得到连接起始点和目标点的一系列离散节点，该过程整体算法框架与传统A*算法相同，但节点扩展方法与二维平面上不同；

节点扩展方法为：先在当前节点的切平面上生成多个平面子节点，所述平面子节点以Δθ为间隔均匀地分布在父节点nparent四周；每一个平面子节点距离父节点的距离dj由这一方向上的曲面曲率决定，依据地形面曲率决定扩展距离，能够在比较平坦的地方以大步长扩展，而在地面凸起或凹陷的地方谨慎地使用小步长；地形面在任一方向上的曲率采用近似的方式计算，在地形面上求出θj方向上距父节点距离为dmax处位置的法向量，用所述法向量与父节点处的法向量夹角作为θj方向上的曲率；

得到切平面上的平面子节点后，将平面子节点投影到地形面上并利用步骤二的可通行性判断方法进行判断，将满足路面粗糙度通行条件的节点放入子节点集合中；通过欧式距离计算每一个子节点的代价值和启发值；此时生成一系列位于地形面上的、连接起始点和目标点的、位于无碰撞区域的稀疏离散节点(T0，T1，…，Tn‑1)，每一个节点Ti均由一个6自由度位姿表示；

将所述稀疏离散节点构建成光滑连续的曲线；首先对节点进行平滑，对节点ni进行平滑操作的方法为：节点ni‑1、ni、ni+1构成三角形，ni‑1ni+1确定一条直线，节点ni在直线ni‑1ni+1上的投影点为E，由ni向E移动必定会使路径ni‑1 ni ni+1变短；取直线Eni的中点为点F，判断F点投影到地形上位置处是否满足路面粗糙度可通行条件，若满足，则将中间点F作为新的节点ni；若不满足，则取Fni的中点为新的F点，再次进行判断；即经过不断靠近节点ni的多次二分法，找到满足粗糙度的中心点；再对邻近的下一个节点ni+1进行同样的平滑操作；对除了两端节点之外的节点都进行一遍平滑操作，即完成了一个平滑循环周期；此时离散路径{ni}的总长会减少，经过多次平滑循环周期后，直至所有节点都无法再调整或路径总长几乎不变时，整个迭代平滑算法结束，输出平滑后的离散节点；

在离散节点基础上利用插值法生成连续的参考路径曲线；为避免出现龙格现象，采用分段函数来进行插值；为了保证曲线的曲率连续，每一段曲线要求至少二阶可导，因而选用三次参数方程表示，利用高斯消元法求解得到曲率连续的三维参考路径；

2)、根据三维参考路径生成车辆能够跟踪的最终路径为了满足车辆运动学约束，使用基本运动单元作为子节点；车辆本身位于由4个车轮所决定的平面上，因此在局部小范围内可以认为车辆在切平面上运动；以弧长为参数的车辆运动学方程为：

其中(x，y)为切平面上的位置，φ为车辆航向，′表示对弧长的导数，δ为前轮偏角，κ(s)为曲率；以曲率κ(s)作为控制量，为了增大曲线自由度，以五次弧长多项式表示曲率κ(s)，生成运动基元：

2 3 4 5

κ(s)＝κ0+bs+cs+ds+es+fs其中b、c、d、e、f为待求解的未知参数，κ0为初始曲率；

添加路径平滑度目标函数，采用非线性优化的方式求解未知参数；定义路径平滑度目标函数：

其中sf为运动基元长度；最小化该目标函数得到平均曲率较小的曲线；对以弧长为参数的运动学方程积分得到任意末点状态为：其中x0、y0、φ0为车辆的初始状态。结合端点状态约束条件就可构成具有等式约束的非线性优化问题，决策变量为五次多项式系数(b，c，d，e，f)和路径总长sf，对非线性优化问题进行求解得到运动基元；

将搜索图边的代价值Jedge设计为节点距离、地面粗糙度、车辆倾斜程度、转向代价四项指标的加权和；

为了减少节点扩展数量，提高搜索效率，利用参考路径提供的引导信息设计启发值；当路径搜索算法扩展到子节点ni时，先将ni投影到三维参考路径上得到投影点np，子节点ni到目标点的真实距离表示为ni到参考路径的投影距离再加上投影点np到目标点间的参考线长sref，将子节点ni到目标点的真实距离作为启发参考之一；由于参考路径光滑连续，参考路径切向信息也作为启发参考，则启发值定义为：h＝wl·(||ni‑np||2+sref)+wheading·Δθ其中，wl、wheading分别是距离项和航向项的权重，Δθ是当前子节点航向和参考线上投影点处的切向的差值；最终A*算法中的代价值为启发值和真实代价值的加权组合：f＝wg·g+wh·h

其中，wg和wh分别为代价值和启发值的权重，g为到达当前子节点所经过的所有Jedge之和；

基于建立的运动基元和设计的代价值扩展节点；在车辆当前节点的切平面上选取运动基元末端状态作为候选子节点，将切平面上的候选子节点投影到地形面上，对投影处既进行路面粗糙度层面上的可通行性检查，还结合车辆静态稳定性检查地面形状可通行性，当两个通行性都满足时，将候选子节点加入子节点集；当目标点被扩展后，完成路径规划；

步骤四、速度规划：初始速度曲线生成和速度曲线平滑安全约束包括：侧滑约束、地面接触约束和侧翻约束；性能约束包括：驱动力约束、平滑度、时间效率和任务约束；

首先根据安全约束生成一个初始的速度曲线Vf，所述速度曲线能够保证车辆在行驶过程中的安全性；对车辆进行动力学分析后，分别求出地面接触约束、侧滑约束和侧翻约束所决定的最大速度曲线Vtouch、Vslide和Vrollover，并取Vtouch、Vslide和Vrollover和预先设置的任务约束速度曲线Vtask中的较小值作为初始最大速度Vupper；然后利用迭代速度规划算法生成初始速度曲线，开始迭代前使用初始最大速度初始化Vf，并使用预设的初始速度v0和终点速度vf来固定Vf的端点，迭代时不会修改两个端点处的速度；当要修改第i个点处的速度曲线时，首先求得第i‑1个点处的最大加速度amax，然后再根据运动学关系由vi‑1和amax推导得到vi，再计算第i个点处的最小加速度amin，并由vi+1和amin反推vi；当修改后的速度曲线和修改前的速度曲线几乎没有变化时，则迭代算法停止并返回Vf作为初始速度曲线；

对初始速度曲线进行平滑处理，将速度曲线平滑问题建立为最优控制问题进行求解；

为了提高速度曲线的平滑度，在优化条件中限制速度曲线的加速度和加速度变化率；同时加入时间效率优化目标，达到速度曲线平滑与时间消耗之间的平衡；

所以速度曲线平滑优化问题定义为：在满足起点和终点的速度、加速度约束和安全约束的条件下，求解使车辆加速度、加速度变化率与行驶时间消耗三者的加权和最小化的速度曲线；采用直接配置点法将问题离散化，将初始速度曲线作为初值，采用非线性优化求解器IPOPT进行求解；最终得到的结果就是满足车辆行驶稳定性且较快的速度曲线。

2.如权利要求1所述的一种保证无人车在三维地形上安全行驶的运动规划方法，其特征在于：所述三维参考路径生成时，在节点扩展时，根据运用需求，保证在车辆前进方向进行节点扩展。

说明书

一种保证无人车在三维地形上安全行驶的运动规划方法

技术领域

[0001] 本发明涉及一种考虑三维地形的运动规划方法，尤其涉及一种无人车辆三维地形中的运动规划，属于无人车辆运动规划领域。

背景技术

[0002] 无人驾驶技术作为当今车辆的前沿技术受到了各国学者的广泛关注，无人车辆的运动规划技术是无人驾驶车辆的核心技术之一。随着近些年对城市自动驾驶领域投入了大
量研究资金和研究精力，无人车辆的运动规划技术研究也取得了丰硕成果。但目前针对越
野环境、非平坦路面场景尚没有成熟的运动规划解决方案，如在以越野环境为主的军用领
域，无人车辆仍然以遥控或是半自主的形式为主。越野环境相较于城市环境最大的差异在
于地形丰富多变，给运动规划带来的难度也大大增加。

[0003] 现有的无人车辆运动规划算法多是在二维平面上进行规划，忽略了地面形状的影响，没有考虑车辆在非平坦路面上发生俯仰、侧倾等姿态变化时对车辆行驶稳定性的影响。
也有一些运动规划方法和理论考虑了越野环境中的一些地形特性，但这些方法仍然存在考
虑不完备、难以找到解、求解效率低等缺陷。无人车辆在越野环境、非平坦路面场景中的运
动规划技术仍然是当前研究的难点和热点。

发明内容

[0004] 本发明为了保证无人车辆在三维地形面上的行驶安全性，增强在三维地形中运动规划的合理性，提出了一种保证无人车在三维地形上安全行驶的运动规划方法。主要内容
分为四部分，地形环境表示与位姿投影方法是后续操作的基础。地形可通行性判别算法考
虑到路面粗糙度与地面形状对可通行性的影响，保证车辆静态稳定性。三维地形路径规划
包括参考路径生成和可跟踪路径生成两个步骤。速度规划包括初始速度曲线生成和速度曲
线平滑两个步骤。

[0005] 本发明的目的是通过下述技术方案实现的：

[0006] 一种保证无人车在三维地形上安全行驶的运动规划方法，包括如下步骤：

[0007] 步骤一、得到车辆在地形面上的投影位姿

[0008] 采用三维激光点云表示地形环境，地形面S由地图坐标系M下表示的N个数据点组成，即 i＝1,2,…,N；pi为第i个数据点；为了分析在pi位置处的车辆位姿，利用
主元分析法获取该处地形曲面的法向量，并将法向量作为新位姿的z轴向量 以两平面交
线方向上的单位向量作为新位姿的x轴向量 所述两平面为以原位姿y轴向量 为法向的平
面，以及以新位姿z轴向量 为法向的平面；最终以三个单位轴向量 构成的旋转矩
阵和pi的位置构成了车辆在地形面上的投影位姿；

[0009] 步骤二、在步骤一得到的点云数据pi基础上，对前方的地面进行分析，从而判断生成的路径是否可用；

[0010] 通过分析路面粗糙度来判断地形是否有典型障碍物或过大的起伏；所述的路面粗糙度与传统意义上的粗糙度不同，离散地形面S上任意一点pi处的粗糙度的求取方法为：利
用车辆感知模块获取以pi为球心rplane为半径的球内的所有数据点，记为Bplane(pi)，并结合
Bplane(pi)中的数据点用主元分析法求出pi处的法向量ni；采用半径rplane的球体内的数据点
做平面拟合，采用半径rres的球体内的数据点做粗糙度值计算，所述rplane大于rres；

[0011] 利用车辆感知模块获取以pi为球心rres为半径的球内的所有数据点，记为Bres(pi)；Bres(pi)内每个点到拟合平面的符号投影距离为

[0012]

[0013] 其中pj为Bres(pi)中的数据点；

[0014] pi点处的粗糙度ρi定义为：

[0015]

[0016] 根据车辆尺寸定义一个最大路面粗糙度值ρmax，当地形上一点处的粗糙度值ρi>ρmax时，则认为该处的路面粗糙度值不满足通行条件，规划的路径不可经过此处；

[0017] 当ρi≤ρmax时，还需分析地面形状来判断pi处地形是否能满足车辆静态稳定性要求；获得pi处车辆投影位姿后，求取相应的车辆欧拉角，为了使车辆能够在该处保持静态稳
定性，需要使车辆侧倾角ψ与俯仰角θ在一定阈值范围内，即：

[0018] |ψ|≤ψmax

[0019] θmin≤θ≤θmax

[0020] 其中，ψmax是最大侧倾稳定角阈值，θmin、θmax分别是最小俯仰角阈值和最大俯仰角阈值；从而保证车辆不会发生侧向移动、侧向翻转和纵向翻转；

[0021] 步骤三、路径规划：生成三维参考路径，再根据参考路径生成车辆能够跟踪的最终路径；

[0022] 1)、生成三维参考路径

[0023] 通过构建搜索图并使用A*搜索得到连接起始点和目标点的一系列离散节点，该过程整体算法框架与传统A*算法相同，但节点扩展方法与二维平面上不同；

[0024] 节点扩展方法为：先在当前节点的切平面上生成多个平面子节点，所述平面子节点以Δθ为间隔均匀地分布在父节点nparent四周；每一个平面子节点距离父节点的距离dj由
这一方向上的曲面曲率决定，依据地形面曲率决定扩展距离，能够在比较平坦的地方以大
步长扩展，而在地面凸起或凹陷的地方谨慎地使用小步长；地形面在任一方向上的曲率采
用近似的方式计算，在地形面上求出θj方向上距父节点距离为dmax处位置的法向量，用所述
法向量与父节点处的法向量夹角作为θj方向上的曲率；

[0025] 得到切平面上的平面子节点后，将平面子节点投影到地形面上并利用步骤二的可通行性判断方法进行判断，将满足路面粗糙度通行条件的节点放入子节点集合中；通过欧
式距离计算每一个子节点的代价值和启发值；此时生成一系列位于地形面上的、连接起始
点和目标点的、位于无碰撞区域的稀疏离散节点(T0,T1,…,Tn‑1)，每一个节点Ti均由一个6
自由度位姿表示；

[0026] 将所述稀疏离散节点构建成光滑连续的曲线；首先对节点进行平滑，对节点ni进行平滑操作的方法为：节点ni‑1、ni、ni+1构成三角形，ni‑1ni+1确定一条直线，节点ni在直线ni‑
1ni+1上的投影点为E，由ni向E移动必定会使路径ni‑1nini+1变短；取直线Eni的中点为点F，判
断F点投影到地形上位置处是否满足路面粗糙度可通行条件，若满足，则将中间点F作为新
的节点ni；若不满足，则取Fni的中点为新的F点，再次进行判断；即经过不断靠近节点ni的多
次二分法，找到满足粗糙度的中心点；再对邻近的下一个节点ni+1进行同样的平滑操作；对
除了两端节点之外的节点都进行一遍平滑操作，即完成了一个平滑循环周期；此时离散路
径{ni}的总长会减少，经过多次平滑循环周期后，直至所有节点都无法再调整或路径总长
几乎不变时，整个迭代平滑算法结束，输出平滑后的离散节点；

[0027] 在离散节点基础上利用插值法生成连续的参考路径曲线；为避免出现龙格现象，采用分段函数来进行插值；为了保证曲线的曲率连续，每一段曲线要求至少二阶可导，因而
选用三次参数方程表示，利用高斯消元法求解得到曲率连续的三维参考路径；

[0028] 2)、根据三维参考路径生成车辆能够跟踪的最终路径

[0029] 为了满足车辆运动学约束，使用基本运动单元作为子节点；车辆本身位于由4个车轮所决定的平面上，因此在局部小范围内可以认为车辆在切平面上运动；以弧长为参数的
车辆运动学方程为：

[0030]

[0031] 其中(x,y)为切平面上的位置，φ为车辆航向，′表示对弧长的导数，δ为前轮偏角，κ(s)为曲率；以曲率κ(s)作为控制量，为了增大曲线自由度，以五次弧长多项式表示曲率κ
(s)，生成运动基元：

[0032] κ(s)＝κ0+bs+cs2+ds3+es4+fs5

[0033] 其中b、c、d、e、f为待求解的未知参数，κ0为初始曲率；

[0034] 添加路径平滑度目标函数，采用非线性优化的方式求解未知参数；定义路径平滑度目标函数：

[0035]

[0036] 其中sf为运动基元长度；最小化该目标函数得到平均曲率较小的曲线；对以弧长为参数的运动学方程积分得到任意末点状态为：

[0037]

[0038] 其中x0、y0、φ0为车辆的初始状态。结合端点状态约束条件就可构成具有等式约束的非线性优化问题，决策变量为五次多项式系数(b,c,d,e,f)和路径总长sf，对非线性优化
问题进行求解得到运动基元；

[0039] 将搜索图边的代价值Jedge设计为节点距离、地面粗糙度、车辆倾斜程度、转向代价四项指标的加权和；

[0040] 为了减少节点扩展数量，提高搜索效率，利用参考路径提供的引导信息设计启发值；当路径搜索算法扩展到子节点ni时，先将ni投影到三维参考路径上得到投影点np，子节
点ni到目标点的真实距离表示为ni到参考路径的投影距离再加上投影点np到目标点间的参
考线长sref，将子节点ni到目标点的真实距离作为启发参考之一；由于参考路径光滑连续，
参考路径切向信息也作为启发参考，则启发值定义为：

[0041] h＝wl·(‖ni‑np‖2+sref)+wheading·Δθ

[0042] 其中，wl、wheading分别是距离项和航向项的权重，Δθ是当前子节点航向和参考线上投影点处的切向的差值；最终A*算法中的代价值为启发值和真实代价值的加权组合：

[0043] f＝wg·g+wh·h

[0044] 其中，wg和wh分别为代价值和启发值的权重，g为到达当前子节点所经过的所有Jedge之和；

[0045] 基于建立的运动基元和设计的代价值扩展节点；在车辆当前节点的切平面上选取运动基元末端状态作为候选子节点，将切平面上的候选子节点投影到地形面上，对投影处
既进行路面粗糙度层面上的可通行性检查，还结合车辆静态稳定性检查地面形状可通行
性，当两个通行性都满足时，将候选子节点加入子节点集；当目标点被扩展后，完成路径规
划；

[0046] 步骤四、速度规划：初始速度曲线生成和速度曲线平滑

[0047] 安全约束包括：侧滑约束、地面接触约束和侧翻约束；性能约束包括：驱动力约束、平滑度、时间效率和任务约束；

[0048] 首先根据安全约束生成一个初始的速度曲线Vf，所述速度曲线能够保证车辆在行驶过程中的安全性；对车辆进行动力学分析后，分别求出地面接触约束、侧滑约束和侧翻约
束所决定的最大速度曲线Vtouch、Vslide和Vrollover，并取Vtouch、Vslide和Vrollover和预先设置的任
务约束速度曲线Vtask中的较小值作为初始最大速度Vupper；然后利用迭代速度规划算法生成
初始速度曲线，开始迭代前使用初始最大速度初始化Vf，并使用预设的初始速度v0和终点速
度vf来固定Vf的端点，迭代时不会修改两个端点处的速度；当要修改第i个点处的速度曲线
时，首先求得第i‑1个点处的最大加速度amax，然后再根据运动学关系由vi‑1和amax推导得到
vi，再计算第i个点处的最小加速度amin，并由vi+1和amin反推vi；当修改后的速度曲线和修改
前的速度曲线几乎没有变化时，则迭代算法停止并返回Vf作为初始速度曲线；

[0049] 对初始速度曲线进行平滑处理，将速度曲线平滑问题建立为最优控制问题进行求解；为了提高速度曲线的平滑度，在优化条件中限制速度曲线的加速度和加速度变化率；同
时加入时间效率优化目标，达到速度曲线平滑与时间消耗之间的平衡；

[0050] 所以速度曲线平滑优化问题定义为：在满足起点和终点的速度、加速度约束和安全约束的条件下，求解使车辆加速度、加速度变化率与行驶时间消耗三者的加权和最小化
的速度曲线；采用直接配置点法将问题离散化，将初始速度曲线作为初值，采用非线性优化
求解器IPOPT进行求解；最终得到的结果就是满足车辆行驶稳定性且较快的速度曲线。

[0051] 在所述三维参考路径生成时，在节点扩展时，根据运用需求，保证在车辆前进方向进行节点扩展。

[0052] 有益效果：

[0053] 1、本发明公开的一种基于地面性质的可通行性判别方法，以路面的最大“跳变”距离衡量路面粗糙度，对典型障碍物与起伏过大路面进行考虑，且对地面形状进行分析，保证
地形面上车辆的静态稳定性，在路径规划中提高车辆安全性。

[0054] 2、本发明公开的一种基于地面曲率的路径搜索节点扩展方法，能够在比较平坦的地方以大步长扩展，而在地面凸起或凹陷的地方谨慎地使用小步长，比起定步长的方式具
有更高的搜索图构建效率，且更适合三维地形路径规划。

[0055] 3、本发明公开的一种基于多约束的速度规划方法，通过分层递进的策略，考虑了常见的端点约束、任务约束、平滑度指标和时间效率指标，同时还能够满足车辆侧滑约束、
侧翻约束、地面接触约束、驱动力约束等安全相关约束，比起平面路径的速度规划方法更能
保障车辆的行驶安全。

附图说明

[0056] 图1规划场景图；

[0057] 图2运动规划的过程；

[0058] 图3车辆位姿示意图；

[0059] 图4路面粗糙度的求取方法示意图；

[0060] 图5三维参考路径生成中的节点扩展模式；

[0061] 图6节点平滑操作示意图；

[0062] 图7运动基元示例；

[0063] 图8启发值示意图；

[0064] 图9车辆沿规划路径行驶的坐标系；

[0065] 图10侧倾临界受力示意图；

[0066] 图11路径规划结果；

[0067] 图12速度规划结果。

具体实施方式

[0068] 为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

[0069] 实施例1：

[0070] 当一辆无人驾驶车辆行驶在图1所示的野外环境中时，野外环境中地形的高低起伏可能会给车辆行驶带来危险。为了避开不可通行区域，保持车辆的静态稳定性，同时确保
车辆在运动中的行驶稳定性，需要利用本发明的考虑三维地形的运动规划方法，进行合理
的运动规划，运动规划的过程如图2所示。本例中车辆属于阿克曼转向车辆。

[0071] 一种保证无人车在三维地形上安全行驶的运动规划方法，包括如下步骤：

[0072] 1.对地形环境和车辆位姿的描述

[0073] 在运动规划开始前，首先需要对地形环境和车辆位姿进行合理的描述。采用三维激光点云表示地形环境，地形面S由地图坐标系M下表示的N个数据点组成，即 i
＝1,2,…,N。车辆位姿表示方法如图3所示，为了分析在pi位置处的车辆位姿，利用主元分
析法获取该处地形曲面的法向量，并将其作为新位姿的z轴向量 然后以原位姿y轴向量
为法向的平面和以新位姿z轴向量 为法向的平面二者交线方向上的单位向量作为新位姿
的x轴向量 最终以三个单位轴向量 构成的旋转矩阵和pi的位置构成了车辆在地
形面上的投影位姿。

[0074] 有了对环境和车辆位姿的描述方法后，就可以进行考虑三维地形的车辆运动规划，先进行路径规划，再进行速度规划。

[0075] 2.在点云数据基础上，对前方的地面进行分析，从而判断生成的路径是否可用

[0076] 为了在车辆前方三维地形上生成无碰撞且满足车辆静态稳定性的路径，需要在感知模块提供的点云数据基础上，对前方的地面进行分析，从而判断生成的路径是否可用。

[0077] 首先通过分析路面粗糙度来判断某处地形是否有典型障碍物或过大的起伏。通过分析路面粗糙度来判断地形是否有典型障碍物或过大的起伏。此处的路面粗糙度与传统意
义上的粗糙度不同，离散地形面S上任意一点pi处的粗糙度的求取方法如图4所示。利用车
辆感知模块获取以pi为球心rplane为半径的球内的所有数据点，记为Bplane(pi)，并结合Bplane
(pi)中的数据点用主元分析法求出pi处的法向量ni。采用半径rplane的球体内的数据点做平
面拟合，采用半径rres的球体内的数据点做粗糙度值计算，其中rplane大于rres。

[0078] 利用车辆感知模块获取以pi为球心rres为半径的球内的所有数据点，记为Bres(pi)；

[0079] Bres(pi)内每个点到拟合平面的符号投影距离为

[0080]

[0081] 为了避免受大噪声数据点的影响，忽略与拟合平面距离过大的点，pi点处的粗糙度定义为：

[0082]

[0083] 这样定义的粗糙度可以看做局部范围内地面最高点和最低点的距离差值，表征了地面上不连续的“跳变”。当这个距离差值过大时，实际上可以将这片区域视为不可通行的
障碍物，根据车辆尺寸定义一个最大路面粗糙度值ρmax，当地形上一点处的粗糙度值ρi>ρmax
时，则认为该点处的路面粗糙度值不满足通行条件，规划的路径不可经过此处。

[0084] 若路面粗糙度符合要求，还需分析地面形状来判断该处地形是否能满足车辆静态稳定性要求。获得该处车辆投影位姿后，求取相应的车辆欧拉角，为了使车辆能够在该处保
持静态稳定性，需要使车辆侧倾角ψ与俯仰角θ在一定阈值范围内，即：

[0085] |ψ|≤ψmax   (2.3)

[0086] θmin≤θ≤θmax   (2.4)

[0087] 其中ψmax是最大侧倾稳定角阈值，θmin、θmax分别是最小俯仰角阈值和最大俯仰角阈值。从而保证车辆不会发生侧向移动、侧向翻转和纵向翻转。

[0088] 3.路径规划

[0089] 有了上述的地面可通行判断方法，接下来就可以分两步进行路径规划，先生成一条参考路径，再根据参考路径生成一条车辆能够跟踪的最终路径。

[0090] 先通过构建搜索图并使用A*搜索得到连接起始点和目标点的一系列离散节点，该过程整体算法框架与传统A*算法相同，但节点扩展方法与二维平面上不同。节点扩展的模
式如图5所示，先在当前节点的切平面上生成N个平面子节点，这些平面子节点以Δθ为间隔
均匀地分布在父节点nparent四周。每一个平面子节点距离父节点的距离dj由这一方向上的
曲面曲率决定，依据地形面曲率决定扩展距离，能够在比较平坦的地方以大步长扩展，而在
地面凸起或凹陷的地方谨慎地使用小步长，比起定步长的方式具有更高的搜索图构建效
率。地形面在某一方向上的曲率采用近似的方式计算，在地形面上求出θj方向上距父节点
距离为1m处位置的法向量，用其与父节点处的法向量夹角(图5中的Δφj)作为θj方向上的
曲率。得到切平面上的子节点后，将其投影到地形面上并利用可通行性判断方法进行判断，
此处只有满足路面粗糙度通行条件的节点才会放入子节点集合中，每一个子节点的代价值
和启发值均采用欧式距离计算。实际应用时不用在整个圆周上都生成子节点，而是要根据
运用需求，如不允许倒车时，则可以根据当前航向设定一个角度范围，保证只向车前方扩展
节点。此时生成的是一系列位于地形面上的、连接起始点和目标点的、位于无碰撞区域的稀
疏离散节点(T0,T1,…,Tn‑1)，每一个节点Ti均由一个6自由度位姿表示。

[0091] 离散节点生成后，需要将这些节点构建成为光滑连续的曲线。首先对节点进行平滑，其操作类似于将一根松散的绳子两端固定，然后减少绳子长度逐渐使绳子绷紧。节点平
滑操作方式如图6所示。节点ni‑1、ni、ni+1构成三角形，首先找到待调整节点ni在边ni‑1ni+1上
的投影点E，由ni向E移动必定会使路径ni‑1nini+1变短。取直线Eni的中点为点F，判断F点投影
到地形上位置处是否满足路面粗糙度可通行条件，若满足，则将中间点F作为新的节点ni；
若不满足，则取Fni的中点为新的F点，再次进行判断。即经过不断靠近节点ni的多次二分法，
找到满足粗糙度的中心点。之后再对邻近的下一个节点ni+1进行同样的平滑操作，对除了两
端节点之外的节点都进行一遍平滑操作，即完成了一个平滑循环周期。此时离散路径{ni}
的总长会减少，经过多次平滑循环周期后，直至所有节点都无法再调整或路径总长几乎不
变时，整个迭代平滑算法结束，输出平滑后的离散节点。

[0092] 得到平滑的离散节点后，利用插值法生成连续的参考路径曲线。为避免出现龙格现象，采用分段函数来进行插值。为了保证曲线的曲率连续，每一段曲线要求至少二阶可
导，因而选用三次参数方程表示，利用高斯消元法求解得到曲率连续的三维参考路径。

[0093] 参考路径已经提供了地形面上一条无碰撞的通路，但它没有考虑车辆运动学约束和稳定性要求，所以无法直接让车辆跟踪，接下来需要利用参考路径信息，生成最终的实际
路径。

[0094] 为了满足车辆运动学约束，使用基本运动单元作为子节点。车辆本身位于由4个车轮所决定的平面上，因此在局部小范围内可以认为车辆在切平面上运动。以弧长为参数的
车辆运动学方程为：

[0095]

[0096] 其中(x,y)为切平面上的位置，φ为车辆航向，′表示对弧长的导数，δ为前轮偏角，κ(s)为曲率。以曲率κ(s)作为控制量，为了增大曲线自由度，以五次弧长多项式表示曲率κ
(s)，生成运动基元：

[0097] κ(s)＝κ0+bs+cs2+ds3+es4+fs5   (3.2)

[0098] 其中b、c、d、e、f为待求解的未知参数，κ0为初始曲率。

[0099] 添加一个路径平滑度目标函数，采用非线性优化的方式求解未知参数。定义曲线平滑度目标函数：

[0100]

[0101] 其中sf为运动基元长度。最小化该目标函数得到平均曲率较小的曲线。对以弧长为参数的运动学方程积分得到任意末点状态为：

[0102]

[0103] 其中x0、y0、φ0为车辆的初始状态。结合端点状态约束条件就可构成具有等式约束的非线性优化问题，决策变量为五次多项式系数(b,c,d,e,f)和路径总长sf，对非线性优化
问题进行求解得到运动基元。图7为求解出的运动基元示例，包括了前进和后退。其中步长
‑1
为1.5m、最大曲率为0.2m 。

[0104] 有了运动基元，还需要对规划的代价值进行设计。将搜索图边的代价值Jedge设计为节点距离、地面粗糙度、车辆倾斜程度、转向代价四项指标的加权和：

[0105] Jedge＝w1·Jlength+w2·Jroughness+w3·Jslope+w4·Jturning   (3.5)

[0106] 其中节点距离代价指标Jlength以减少最终路径长度为主，可以使用基元长度近似。地面粗糙度代价Jroughness对地形上非平坦区域做出惩罚，需要利用求地形面上粗糙度的方
法得到。车辆倾斜代价Jslope则惩罚那些让车辆有倾斜现象的路径，定义为：

[0107]

[0108] 其中wroll和wpitch分别为侧倾项和俯仰项的权重，车辆转向代价Jturning是为了让车辆尽量保持原来方向行驶，对转向行为做出惩罚，可以直接使用基元路径上的最大曲率作
为代价值。w1、w2、w3、w4分别为相应指标的权重。

[0109] 为了减少节点扩展数量，提高搜索效率，利用参考路径提供的引导信息设计启发值。图8给出了求启发值的示意图。当算法扩展到子节点ni时，先将其投影到三维参考线上
得到投影点np，子节点ni到目标状态的真实距离可以表示为ni到参考线的投影距离再加上
投影点np到目标点间的参考线长sref，。由于参考路径是由分段三次参数曲线表示的，找投
影点时需要先确定将会投影到哪一段曲线上Ci(t)上，然后利用

[0110]

[0111] 可以得到投影点np的位置。上式是一个关于参数t的一元五次方程，可用牛顿法迭代找到根。将子节点ni到目标状态的真实距离作为启发参考之一，同时由于参考线光滑连
续，其切向信息也可以作为启发参考，于是启发值定义为：

[0112] h＝wl·(‖ni‑np‖2+sref)+wheading·Δθ   (3.8)

[0113] 其中wl、wheading分别是距离项和航向项的权重，Δθ是当前子节点航向和参考线上投影点处的切向的差值。最终A*算法中的代价值定义为启发值和真实代价值的加权组合：

[0114] f＝wg·g+wh·h   (3.9)

[0115] 其中wg和wh分别为代价值和启发值的权重，g为到达当前子节点所经过的所有Jedge之和。

[0116] 有了运动基元和代价值之后，就可以进行节点扩展了。在车辆当前节点的切平面上选取图7中曲线末端状态作为候选子节点。将候选子节点投影到地形面上得到真实地形
位姿后，对投影处既进行路面粗糙度层面上的可通行性检查，还结合车辆静态稳定性检查
地面形状可通行性，当两个通行性都满足时，将候选子节点加入子节点集。当目标点被扩展
后，完成路径规划。

[0117] 4.速度规划

[0118] 路径规划结果考虑了车辆的无碰撞和静态稳定性要求，但为了使车辆在起伏路面上行驶时保持稳定性，还需要在多种约束下进行速度规划来保证车辆的行驶稳定性。多种
约束包括安全约束和性能约束两种。安全约束包括：侧滑约束、地面接触约束和侧翻约束。
性能约束包括：驱动力约束、平滑度、时间效率和任务约束。

[0119] 当车辆位于曲面上沿给定曲线行驶时，车体坐标系并不完全与曲线的三个基本向量方向一致，如图9所示，车体坐标系的x轴与曲线切向α一致，z轴与地面法向n一致，y轴方
向由右手系定义得到 区别于曲线的副法向量γ。车辆沿固定曲线行驶时，其速
度方向与路径切向方向α一致，向心加速度方向沿曲线主法向方向β，重力加速度方向为图9
中的z方向。车辆力平衡方程为：

[0120]

[0121] 其中ft代表车辆纵向上的摩擦力，fq代表车辆横向上的摩擦力，FN为地面提供的支持力，弧长对时间的一阶导 和二阶导数 分别表示车辆的纵向速度和纵向加速度，
表示无风时的空气阻力，Cw为与空气阻力系数、空气密度、迎风面积、车辆质量有关的常数。
等式右边的两项分别为线性加速度力和向心加速度力，κ为路径曲率。车辆坐标系下各个轴
上的力表示为

[0122]

[0123]

[0124]

[0125] 基于上述动力学分析，求出满足驱动力约束、侧滑约束、地面接触约束和侧翻约束的速度与加速度值：

[0126] 1)驱动力约束

[0127] 车辆受引擎驱动时，车辆所受纵向摩擦力应该在最大驱动力Fengine_max和最大制动力Fbrake_max之间：

[0128] Fbrake_max≤ft≤Fengine_max   (4.5)

[0129] 将式4.2代入式4.5，可以得到由驱动力和制动力约束引起的加速度限制：

[0130]

[0131] 2)侧滑约束

[0132] 纵向摩擦力与横向摩擦力的合力位于摩擦圆内即可保证车辆不会发生侧滑，即：

[0133]

[0134] 将式4.2和式4.3代入式4.7，得到一个二次不等式方程：

[0135]

[0136] 其中

[0137]

[0138]

[0139]

[0140] 解式4.8，可以得到为了不发生侧滑而应有的加速度约束：

[0141]

[0142] 三个参数c1、c2、c3仅与地形因素有关，在给定地形条件下，需要保证判别式：

[0143]

[0144] 否则意味着式4.7的摩擦圆约束不可能被满足，车辆必定发生侧滑。

[0145] 3)地面接触约束

[0146] 车辆在起伏路面行驶时一般需要保证车辆能够一直与地面接触，此时地面能够一直为车辆提供支持力。将式4.4代入FN≥0可以得到：

[0147]

[0148] 当车辆在地球表面行驶时，地面法向与重力方向的夹角一定为钝角，即gz·n<0，当二次项系数κβ·n≥0时，无论以什么速度行驶均能保证车辆始终与地面接触。当二次项
系数κβ·n<0时，最大速度限制为：

[0149]

[0150] 4)侧翻约束

[0151] 侧翻约束指车辆位于侧坡时防止车辆发生侧翻。如图10中所示，记扭矩顺时针方向为正，则临界情形下车辆不侧翻的条件为：

[0152]

[0153] 其中h为车辆质心到地面的高度，b为车辆质心到车轮的横向距离。可以看到无论顺时针侧翻还是逆时针侧翻都有相同的临界条件，将式4.3和式4.4代入式4.18可得到关于
速度的临界侧翻条件：

[0154]

[0155] 其中

[0156]

[0157]

[0158] 首先根据安全约束生成一个初始的速度曲线Vf，Vf能够保证车辆在行驶过程中的安全性。分别求出侧滑约束(式4.13)地面接触约束(式4.17)、和侧翻约束(式4.19)所决定
的最大速度曲线Vtouch、Vslide和Vrollover，并取Vtouch、Vslide和Vrollover和预先设置的任务约束速
度曲线Vtask中的较小值作为初始最大速度Vupper。然后利用迭代速度规划算法生成初始速度
曲线，开始迭代前使用Vupper初始化Vf，并使用预设的初始速度v0和终点速度vf来固定Vf的端
点，迭代时不会修改两个端点处的速度。当要修改第i个点处的速度曲线时，首先求得第i‑1
个点处的最大加速度amax(由驱动力约束式4.6和侧滑约束式4.12计算得到)，然后再根据运
动学关系由vi‑1和amax推导得到vi，再计算第i个点处的最小加速度amin，并由vi+1和amin反推
vi。当修改后的速度曲线和修改前的速度曲线几乎没有变化时，则迭代算法停止并返回Vf作
为初始速度曲线。

[0159] 初始速度曲线处在各个约束的边界上，是一条比较极端的速度曲线，还需要进行平滑。为了平滑速度曲线，将速度曲线平滑问题建立为最优控制问题进行求解。为了提高速
度曲线的平滑度，在优化条件中限制速度曲线的加速度和加速度变化率，强调曲线整体平
滑度会降低速度曲线的时间效率，车辆会行驶的十分缓慢，为了平衡这个问题，还得加入时
间效率优化目标。所以速度曲线平滑优化问题可以定义为：在满足起点和终点的速度、加速
度约束和安全约束的条件下，求解使车辆加速度、加速度变化率与行驶时间消耗三者的加
权和最小化的速度曲线。采用直接配置点法将问题离散化，将初始速度曲线作为初值，采用
通用的非线性优化求解器IPOPT进行求解。最终得到的结果就是满足车辆行驶稳定性且较
快的速度曲线。

[0160] 利用路径规划方法，能够如图11所示，在三维地形中找到一条无碰撞、满足车辆静态稳定性和运动学约束的路径，图中显示出了场景中不符合路面粗糙度可通行条件的位置
和最终规划出的路线，虽然车辆起始位置正前方存在一条更短的通路，但由于该通路上存
在不符合路面粗糙度可通行条件的点，所以判断无法通过该通路，于是规划算法寻找了另
一条较长的路径，同时在路径上还避开了不满足车辆静态稳定性的地形面，保证了车辆的
安全性和稳定性。速度规划情况如图12所示，在多重约束与曲线平滑方法的作用下，得到最
下方较为平滑的优化结果，该优化结果在保证车辆安全的条件下，能够使车辆行驶更加平
稳且速度较快。可以看出，利用本发明提出的运动规划方法就能够保证在三维地形中使车
辆安全且平稳快速地到达目的位置。

[0161] 以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范
围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明
的保护范围之内。