[发明专利]一种商用车局部路径规划方法及系统

权利要求书

1.一种商用车局部路径规划方法，其特征在于，包括：

获取车辆当前状态数据、车辆位置距离障碍物数据和车辆位置与距离全局路径数据；

根据预设的路径规划算法对所述车辆当前状态数据、车辆位置距离障碍物数据、车辆位置与距离全局路径数据进行计算，得到原始解，继而根据所述原始解扩展凸空间，使用预设的收益函数对所述原始解进行修正得到修正解，再对所述修正解和所述原始解进行调整从而得出最优路径；其中，所述路径规划算法设置有代价函数，所述收益函数设置有最佳油耗策略、行驶安全策略、行驶效率策略。

2.如权利要求1所述的商用车局部路径规划方法，其特征在于，所述根据预设的路径规划算法对所述车辆当前状态数据、车辆位置距离障碍物数据、车辆位置与距离全局路径数据进行计算，得到原始解，具体为：

根据路径点的连续性、路径点与障碍物的距离和路径点与全局路径的距离因素计算出代价函数，具体地，通过以下公式计算：

计算路径点的连续性，公式如下：

u_smooth＝u(s_i)

u(s_i)＝∑f'(s_i)²

其中，u_smooth表示路径点的连续性，s_i表示第i段路径，u(s_i)表示第i段的曲线的导数；

计算路径点与障碍物的距离，公式如下：

u_{barrier_distance}＝h(d)

其中，u_{barrier-distance}表示路径点与障碍物的距离，(s,l)、(s_i,l_i)分别代表障碍物坐标、某一路径点坐标，d表示某一路径点和障碍物之间的距离；

计算路径点与全局路径的距离，公式如下：

u_{global_distance}＝u(l)

其中，u_{global-distance}表示路径点与全局路径的距离，l_i第i个路径点和参考线之间的垂直距离,u(l)代表路径点与全局路径的距离的拟合程度；

计算出代价函数，具体地，通过以下公式计算：

其中，ω₁、ω₂、ω₃分别代表权重取值，取值是一个(0,1]的数，∑f′(s_i)²表示路径点的连续性，表示路径点与障碍物的距离，表示代表路径点与全局路径的距离的拟合程度；

根据所述代价函数得到改进后的路径规划算法,具体地，通过以下公式计算：

其中，(s_i,l_i)为路径中间状态n_i的坐标，(s,l)为障碍物的坐标，s_i表示第i段路径，l_i第i个路径点和参考线之间的垂直距离；

根据所述改进后的路径规划算法对所述车辆当前状态数据、车辆位置距离障碍物数据、车辆位置与距离全局路径数据进行计算，得到原始解。

3.如权利要求1所述的商用车局路径规划方法，其特征在于，所述根据所述原始解扩展凸空间，具体为：

将所述原始解、障碍物信息输入坐标系中；

通过对所述原始解逐一对道路边界、障碍物上投影，得到凸空间的边界；

对全局路径投影得到所述凸空间的边界的坐标，从而得到凸空间内所有点的坐标。