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

权利要求书

1.一种无人车路径平滑方法，其特征在于，包括：

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

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

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

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

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

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

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

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

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

所述利用动态规划算法确定每一层代价值最小的采样点，确定凸可行集边界，并根据凸可行集边界确定避障约束，具体以下公式：

；

其中，和均为每一层最小的代价值，为安全性代价，为平滑性代价，为偏差性代价，j为采样点的序号，i为Frenet坐标系下的离散路点的序号。

2.根据权利要求1所述的一种无人车路径平滑方法，其特征在于，所述根据对应位置的离散路点和参考路点构建以代价函数最小为优化目标，以离散路点和参考路点的连续性约束、车辆朝向约束、车辆实时位置偏差约束及终点约束为约束条件的第一目标函数，具体包括：

利用公式确定代价函数；

利用公式确定离散路点和参考路点的连续性约束；

利用公式确定车辆朝向约束、车辆实时位置偏差约束及终点约束；

其中，为代价函数，、以及均为权重，为提取固定步长的离散路点的个数，和为第i个离散路点在笛卡尔坐标系下坐标，和为第i个参考路点在笛卡尔坐标系下坐标，为第i个离散路点的曲率，为第i+1个离散路点的曲率，为第i个参考路点对应的角度，为第i个离散路点对应的角度，为第i个离散路点与第i个参考路点的角度偏差，和为第i+1个离散路点在笛卡尔坐标系下坐标，为第i+1个离散路点与第i+1个参考路点的角度偏差，为当前车辆实时朝向，为车辆此时位置距离，和为起点在笛卡尔坐标系下坐标，和为终点在笛卡尔坐标系下坐标，和为第n个参考路点在笛卡尔坐标系下坐标，为Frenet坐标系下的第i个离散路点与第i+1离散路点之间的距离。

3.根据权利要求2所述的一种无人车路径平滑方法，其特征在于，所述以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定代价函数最小为优化目标，以Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束和避障约束为约束条件，构建第二目标函数，具体包括：

利用公式确定根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数；

利用公式确定Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离的连续性约束；

利用公式确定避障约束；

其中，为根据Frenet坐标系下的离散路点与相应位置对应的采样点之间的距离确定的代价函数，为Frenet坐标系下的第i个离散路点与相应位置对应的采样点之间的距离，为的一阶导数，为的二阶导数，为Frenet坐标系下的第i+1个离散路点与相应位置对应的采样点之间的距离，为的一阶导数，为Frenet坐标系下的第i个离散路点的累加距离，为Frenet坐标系下的第i+1个离散路点的累加距离，为初始累加距离，为车辆此时累加距离，为凸可行集边界的最大值和最小值。