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

权利要求书

1.一种改进型三次样条插值曲线拟合路径点方法，其特征在于：包括：

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

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

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

2.根据权利要求1所述的改进型三次样条插值曲线拟合路径点方法，其特征在于：所述步骤S1中各路径点除了包含车辆位于该路径点的坐标信息外还包含位于该点的车辆横摆角信息。

3.根据权利要求1或2所述的改进型三次样条插值曲线拟合路径点方法，其特征在于：所述步骤S2中相邻两个路径点间的三次样条插值曲线及插值点的计算步骤如下：

步骤S21，从起始路径点开始，分别根据原坐标系下相邻两个路径点的横摆角信息计算出坐标系旋转角度；

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

步骤S23，根据各相邻两点的坐标和横摆角信息，可计算得到各相邻两路径点之间拟合的三次样条插值曲线表达式；

步骤S24，通过对相邻两路径点三次样条插值曲线表达式的横坐标等间距离散得到高密度连续插值路径点，插值曲线在各路径点的一阶导数即为车辆在该点的横摆角正切值；

步骤S25，将原相邻两路径点之间的连续插值路径点通过坐标系反向旋转步骤S21计算得到的坐标系旋转角度，得到原坐标系下原相邻路径点及其之间的连续插值路径点的坐标及横摆角信息；

步骤S26，从起始路径点开始依次重复步骤S21至步骤S25，直到目标路径点结束，得到原坐标系下由起始点至目标点的高密度连续路径点的坐标及横摆角信息。

4.根据权利要求3所述的改进型三次样条插值曲线拟合路径点方法，其特征在于：所述步骤S21中的各相邻两路径点P_i-1＝(x_i-1,y_i-1,θ_i-1)、P_i＝(x_i,y_i,θ_i)的坐标系旋转角度由以下公式计算得出：

θ_(rotate)＝(θ_i-1+θ_i)/2；

式中，θ_(rotate)为坐标系旋转角度，θ_i-1、θ_i分别为原坐标系下相邻两路径点P_i-1＝(x_i-1,y_i-1,θ_i-1)、P_i＝(x_i,y_i,θ_i)的方向角角，x_i-1、y_i-1均为相邻路径点在x轴方向上的值，y_i-1、y_i均为相邻路径点在y轴方向上的值。