[发明专利]一种改进型三次样条插值曲线拟合路径点方法

说明书

本发明公开了一种改进型三次样条插值曲线拟合路径点方法，包括：步骤一，通过图搜索算法得到离散路径点组成的路径；步骤二，对相邻两个路径点使用改进型三次样条插值曲线方法进行插值，得到高密度的连续路径点；步骤三，将由各段三次样条插值曲线得到的插值形成的高密度的路径点依次连接，生成路径。本发明的改进型三次样条插值曲线拟合路径点方法，通过步骤一至三的设置，便可有效的利用样条插值的方式实现拟合路径点的效果了。

技术领域

本发明涉及智能汽车路径规划技术领域，更具体的说是涉及一种改进型三次样条插值曲线拟合路径点方法。

背景技术

智能汽车由于其具有提高道路交通的安全性、经济性以及提高出行便利性的潜能，越来越受到人们的关注。智能汽车的路径规划是决策邻域研究的重点，负责为智能汽车规划出一条安全可行的行驶路径。智能汽车在满足非完整约束的条件下，在包含障碍物信息及可行驶空间的构型空间中，通过路径规划算法可以得到一系列离散的路径点，这些路径点满足车辆运动学约束、无碰撞的要求。如何通过路径点拟合方法将各离散路径点通过曲线拟合得到满足车辆非完整约束、不与空间中的障碍物发生碰撞的连续路径是路径规划的关键问题。

目前常用的路径拟合方法中，主要用到三次贝塞尔曲线拟合方法和B样条曲线拟合方法。在专利文件CN 106651987 A，2017.05.10中，使用三次贝塞尔曲线方法在每个路径点前后的同一直线上插入两个路径点为路径曲线提供方向信息，直线的斜率代表该路径点的方向，使得生成的曲线路径在各路径点上满足方向要求。在专利文件CN109176504 A，2019.01.11中，给定一组路径控制点的坐标可拟合得到一条曲线路径，曲线路径的形状由给定的路径点坐标控制。此外也可利用传统三次样条插值曲线拟合方法，利用各连续路径点的坐标信息可求得各相邻路径点之间三次样条插值曲线表达式，再通过对横坐标等距离离散得到插值路径点。

上述路径拟合方法虽然能根据给定路径点拟合出连续的路径曲线，但仍然存在很多局限性。对于B样条曲线拟合方法生成的曲线路径虽然能保证曲线平滑性，但各路径点不包含角度信息，不能保证满足车辆运动学的最大转向角约束；对于三次贝塞尔曲线虽然可通过预先计算出路径点前后插入的路径点的位置以满足路径点角度要求，但对每个路径点前后进行路径点插入计算量大；对于传统三次样条插值曲线方法，当出现两个路径点在横轴或纵轴方向上靠的过近、自变量反方向变化的情况，传统的三次样条曲线拟合出的曲线路径将会出现任意大的震荡畸变(也称为“龙格现象”)、相邻两段曲线在衔接处一阶导数不连续的现象，导致生成的曲线路径不满足车辆的可行驶性。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种改进型三次样条插值曲线路径拟合方法，用于生成满足车辆运动学约束、无震荡畸变、一阶导数连续的曲线路径。

为实现上述目的，本发明提供了如下技术方案：一种改进型三次样条插值曲线拟合路径点方法，包括：

步骤S1，通过图搜索算法得到离散路径点组成的路径；

步骤S2，对相邻两个路径点使用改进型三次样条插值曲线方法进行插值，得到高密度的连续路径点；

步骤S3，将由各段三次样条插值曲线得到的插值形成的高密度的连续路径点依次连接，生成连续路径。

作为本发明的进一步改进，所述步骤S1中各路径点除了包含车辆位于该路径点的坐标信息外还包含位于该点的车辆横摆角信息。

作为本发明的进一步改进，所述步骤S2中相邻两个路径点间的三次样条插值曲线及插值点的计算步骤如下：

步骤S21，从起始路径点开始，分别根据原坐标系下相邻两个路径点的横摆角信息计算出坐标系旋转角度(对于各相邻路径点都需要依次进行坐标轴旋转)；

步骤S22，将原坐标系以步骤S21计算得到的旋转角度进行旋转(逆时针为旋转正方向)，分别计算坐标系旋转后各相邻两路径点新的坐标和横摆角；