[发明专利]一种无人车路径平滑方法及系统

说明书

本发明涉及一种无人车路径平滑方法及系统。该方法包括提取固定步长的离散路点和参考路点；构建第一目标函数；对第一目标函数进行求解，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；构建第二目标函数；对第二目标函数进行求解。本发明能够在全局导航指引线和车辆实时环境信息已知情况下，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线。

技术领域

本发明涉及路径规划领域，特别是涉及一种无人车路径平滑方法及系统。

背景技术

无人车辆从起点运动到目标点，需要全局导航指引线的引导，全局导航指引线生成是指依据某个或某些性能指标，例如工作代价最小、行走路线最短、行走时间最短等，依据地图信息找到一条最优或者接近最优的路径，属于路径规划问题，常用方法包括基于图搜索和基于采样的规划算法，这些算法目前已经被广泛采用，但得到的路径一般是折线路径，曲率不连续，不利于车辆进行执行；无人车路径平滑是指依据平滑性指标，在全局折线路径的基础上规划出一条连接起始点和目标点、避开障碍物的平滑路径，常用方法一般通过不同曲线模型拟合获得平滑路径，包括多项式曲线、贝塞尔曲线、样条曲线等。

然而上述使用曲线模型拟合的路径平滑方式往往并未考虑车辆实时朝向信息、位置与导航指引线之间的偏差距离信息以及障碍物信息，不能提供带有足够信息的路径平滑曲线。

因此，亟需一种考虑车辆实时朝向及位置偏差的避障平滑路径生成方式，为后续车辆最终运动规划轨迹的生成提供更加准确的参考信息。

发明内容

本发明的目的是提供一种无人车路径平滑方法及系统，能够在全局导航指引线和车辆实时环境信息已知情况下，生成一条考虑车辆实时朝向和位置偏差的无碰撞平滑曲线。

为实现上述目的，本发明提供了如下方案：

一种无人车路径平滑方法，包括：

获取车辆全局导航指引线的离散路点，并进行三次B样条曲线拟合，得到第一拟合曲线；

对第一拟合曲线上的离散路点进行三次自然样条差值拟，得到第二拟合曲线；

对第一拟合曲线和第二拟合曲线分别提取固定步长的离散路点，并将所述第二拟合曲线提取的固定步长的离散路点作为参考路点；离散路点均包括：当前点在笛卡尔坐标系下坐标、当前点对应的角度以及曲率；

根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数；

利用二次规划求解器对第一目标函数进行求解，获得优化后的离散路点，并对优化后的离散路点进行三次自然样条差值拟，之后提取固定步长的离散路点，并转到Frenet坐标系下；

将Frenet坐标系下的离散路点以设定步长在Frenet坐标系的l轴上进行上下偏移采样，确定采样点；

利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束；

以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数；

利用二次规划求解器对第二目标函数进行求解，获得二次优化后的离散路点，进而根据二次优化后的离散路点确定路径平滑曲线。