滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...

自动驾驶运动规划(Motion Planning)是无人驾驶汽车的核心模块之一，它的主要任务之一就是如何生成舒适的、碰撞避免的行驶路径和舒适的运动速度。生成行驶路径最经典方法之一就是是Sampling-Based Planner算法；基于采样的规划器可以规划出可行的轨迹，但这种轨迹往往是折线，为了保证车辆行驶过程中给乘客良好舒适的体验，需要对规划的轨迹进行平滑。Cubic Spline就是一种常用的插值平滑算法，通过一系列的控制点得到一条连续平滑的轨迹。

1、Cubic Spline曲线定义

假定有以下n+1个节点：

把n+1个节点划分成n个区间：

$[(x_0, x_1), (x_1, x_2), ..., (x_{n-1}, x_n)]$ ，Cubic Spline是一个满足以下条件的分段定义曲线：

1）在每个分段区间

$[x_i, x_{i+1}]$ 上，

$S_i(x)$ 是一个三次多项式；

2）

$S(x_i) = y_i$ ；

3）S(x)、

$S^{\prime}(x)$ 、

$S^{\prime \prime}(x)$ 在区间[a, b]上连续，即S(x)曲线是光滑的；

每个分段区间的三次多项式构造形式如下：

$S_i(x) = a_i + b_i(x - x_i) + c_i (x - x_i)^2 + d_i(x - x_i)^3$

n个区间共有n个多项式，每个多项式有

$a_i$ ，

$b_i$ ，

$c_i$ ，

$d_i$ 4个未知数，所以整个三次样条曲线(Cubic Spline)共有4n个未知数。要求解4n个未知数，需要构造4n个方程。

2、Cubic Spline曲线求解

已知:

a) n+1个数据点[

$x_i$ ,

$y_i$ ], i = 0, 1, …, n;

b) 每一分段都是三次多项式函数曲线;

c) 节点达到二阶连续;

d) 左右两端点处特性（自然边界，固定边界，非节点边界）

根据已知点求出每段样条曲线方程中的系数，即可得到曲线方程。

曲线求解过程的推导的过程如下：

1）根据插值和连续性的定义：

$S_i(x_i) = y_i$

$S_i(x_{i+1}) = y_{i + 1}$ 其中i = 0, 1, ..., n-1

2）根据微分连续性的定义:

$S_i^{\prime}(x_{i+1}) = S_{i + 1}^{\prime}(x_{i+1})$

$S_i^{\prime \prime}(x_{i+1}) = S_{i + 1}^{\prime \prime}(x_{i+1})$ 其中i = 0, 1, ..., n-2

3）样条曲线的微分式：

$S_i(x) = a_i + b_i(x - x_i) + c_i (x - x_i)^2 + d_i(x - x_i)^3$

$S_i^{\prime}(x) = b_i + 2c_i(x - x_i) + 3d_i(x - x_i)^2$

$S_i^{\prime \prime}(x) = 2c_i + 6d_i(x - x_i)$

a) 由

$S_i(x_i) = y_i$ 可以得到：

$a_i = y_i$

b) 令

$h_i = x_{i + 1} - x_i$ ，由

$S_i(x_{i+1}) = y_{i + 1}$ 得到：

$a_i + h_i b_i + h_i^2 c_i + h_i^3 d_i = y_{i+1}$

c) 由

$S_i^{\prime}(x_{i+1}) = S_{i + 1}^{\prime}(x_{i+1})$ 推出：

$S_i^{\prime}(x_{i+1}) = b_i + 2c_i(x_{i + 1} - x_i) + 3d_i(x_{i + 1} - x_i)^2 = b_i + 2c_i h + 3d_i h^2$

$S_{i + 1}^{\prime}(x_{i+1}) = b_{i + 1} + 2c_i(x_{i + 1} - x_{i + 1}) + 3d_i(x_{i + 1} - x_{i + 1})^2 = b_{i + 1}$

由此可得：

$b_i + 2c_i h + 3d_i h^2 - b_{i+1} = 0$

d) 由

$S_i^{\prime \prime}(x_{i+1}) = S_{i + 1}^{\prime \prime}(x_{i+1})$ 推出：

$2c_i + 6h_i d_i - 2c_{i+1} = 0$

设

$m_i = S_i^{\prime \prime}(x_i) = 2c_i$ ，则有：

$2c_i + 6h_i d_i - 2c_{i+1} = 0$ 可写为：

$m_i + 6h_i d_i - m_{i + 1} = 0$

推出：

$d_i = \frac{m_{i+1} - m_i}{6 h_i}$

b. 将

$c_i$ ，

$d_i$ 代入

$y_i + h_i b_i + h_i^2 c_i + h_i^3d_i = y_{i+1}$ 可得：

$b_i = \frac{y_{i+1} - y_i}{h_i} - \frac{h_i}{2} m_i - \frac{h_i}{6} (m_{i+1} - m_i)$

c. 将

$b_i$ ，

$c_i$ ，

$d_i$ 代入

$b_i + 2h_i c_i + 3h_i^2d_i = b_{i+1}$ 可得：

$h_i m_i + 2(h_i + h_{i+1})m_{i+1} + h_{i+1}m_{i+2} = 6 \bigg[ \frac{y_{i+2} - y_{i+1}}{h_{i+1}} - \frac{y_{i+1} - y_i}{h_i} \bigg]$

根据上述的公式可以得到4n-2个方程，然而有4n个未知数，所以还需要对边界做些约束，所以需要对两端点

$x_0$ 和

$x_n$ 的微分加些限制。选择不是唯一的，3种比较常用的限制如下。

a. 自由边界(Natural)

首尾两端没有受到任何让它们弯曲的力，即

$S^{\prime\prime}=0$ ，具体表示为

$m_0=0$ 和

$m_n=0$ ，则要求解的方程组可写为：

$\begin{aligned} &\left[ \begin{matrix} {1} & {0} & {0} & {0} & {\cdots} & {0} \\ {h_{0}} & {2\left(h_{0}+h_{1}\right)} & {h_{1}} & {0} & {0} & {0} \\ {0} & {h_{1}} & {2\left(h_{1}+h_{2}\right)} & {h_{2}} & {0} & {0} \\ {0} & {0} & {h_{2}} & {2\left(h_{2}+h_{3}\right)} & {h_{3}} & {\vdots} \\ {\vdots} & {\vdots} & {\vdots} & {\vdots} & {\ddots} & {\vdots} \\ {0} & {0} & {0} & {h_{n-2}} & {2\left(h_{n-2}+h_{n-1}\right)} & {h_{n-1}} \\ {0} & {0} & {0} & {0} & {\cdots} & {1} \end{matrix} \right] \left[ \begin{matrix} {m_{0}} \\ {m_{1}} \\ {m_{2}} \\ {m_{3}} \\ {\vdots} \\ {m_{n}} \end{matrix} \right] \\ &=6\left[ \begin{matrix} {0} \\ {\frac{y_{2}-y_{1}}{h_{1}}-\frac{y_{1}-y_{0}}{h_{0}}} \\ {\frac{y_{4}-y_{3}}{h_{3}}-\frac{y_{2}-y_{1}}{h_{1}}} \\ {\frac{y_{4}-y_{3}}{h_{3}}-\frac{y_{3}-y_{2}}{h_{2}}} \\ {\vdots} \\ {\frac{y_{n}-y_{n-1}}{h_{n-1}}-\frac{y_{n-1}-y_{n-2}}{h_{n-2}}} \\ {0} \end{matrix} \right] \end{aligned}$

b. 固定边界(Clamped)

$\begin{aligned} S_{0}^{\prime}\left(x_{0}\right)=A & \Longrightarrow b_{0}=A \\ & \Longrightarrow A=\frac{y_{1}-y_{0}}{h_{0}}-\frac{h_{0}}{2} m_{0}-\frac{h_{0}}{6}\left(m_{1}-m_{0}\right) \\ & \Longrightarrow 2 h_{0} m_{0}+h_{0} m_{1}=6\left[\frac{y_{1}-y_{0}}{h_{0}}-A\right] \end{aligned}$

$\begin{aligned} S_{n-1}^{\prime}\left(x_{n}\right)=B & \Longrightarrow b_{n-1}=B \\ \Longrightarrow & h_{n-1} m_{n-1}+2 h_{n-1} m_{n}=6\left[B-\frac{y_{n}-y_{n-1}}{h_{n-1}}\right] \end{aligned}$

将上述两个公式代入方程组，新的方程组左侧为：

$\left[ \begin{matrix} {2 h_{0}} & {h_{0}} & {0} & {\cdots} & {\cdots} & {0} \\ {h_{0}} & {2\left(h_{0}+h_{1}\right)} & {h_{1}} & {0} & {} & {\vdots} \\ {0} & {h_{1}} & {2\left(h_{1}+h_{2}\right)} & {h_{2}} & {0} & {\vdots} \\ {\vdots} & {0} & {\ddots} & {\ddots} & {\ddots} & {0} \\ {0} & {\cdots} & {0} & {h_{n-2}} & {2\left(h_{n-2}+h_{n-1}\right)} & {h_{n-1}} \\ {0} & {\cdots} & {\cdots} & {0} & {h_{n-1}} & {2 h_{n-1}} \end{matrix} \right]$

c.非节点边界(Not-A-Knot)

指定样条曲线的三次微分相等，即：

$S_0^{\prime \prime \prime}(x_1) = S_1^{\prime \prime \prime}(x_1)$

$S_{n-2}^{\prime \prime \prime}(x_{n-1}) = S_{n-1}^{\prime \prime \prime}(x_{n-1})$

根据

$S_i^{\prime \prime \prime}(x) = 6d_i$ 和

$d_i = \frac{m_{i+1} - m_i}{6h_i}$ ，则上述条件变为：

$h_1{m_1 - m_0} = h_0(m_2 - m_1)$

$h_{n_1}(m_{n-1}-m_{n-2}) = h_{n-2}(m_n - m_{n-1})$

新的方程组系数矩阵可写为：

$\left[ \begin{matrix} {-h_{1}} & {h_{0}+h_{1}} & {-h_{0}} & {\cdots} & {\cdots} & {0} \\ {h_{0}} & {2\left(h_{0}+h_{1}\right)} & {h_{1}} & {0} & {} & {\vdots} \\ {0} & {h_{1}} & {2\left(h_{1}+h_{2}\right)} & {h_{2}} & {0} & {\vdots} \\ {\vdots} & {0} & {\ddots} & {\ddots} & {\ddots} & {0} \\ {0} & {\cdots} & {0} & {h_{n-2}} & {2\left(h_{n-2}+h_{n-1}\right)} & {h_{n-1}} \\ {0} & {\cdots} & {\cdots} & {-h_{n-1}} & {2h_{n-2}+h_{n-1}} & {-h_{n-2}} \end{matrix} \right]$

下图可以看出不同的端点边界对样条曲线的影响：

3、算法总结

假设有n+1个数据节点：

$(x_0, y_0), (x_1, y_1), ..., (x_n, y_n)$ ，曲线插值的步骤如下：

a) 计算步长：

$h_i = x_{i+1} - x_i$ ，其中i = 0, 1, ..., n-1;

b) 将数据节点和指定的首尾断点条件代入矩阵方程；

c) 解矩阵方程，求得二次微分方程

$m_i$ ，该矩阵为三对角矩阵；常见解法为高斯消元法，可以对系数矩阵进行LU分解，分解为单位下三角矩阵和上三角矩阵。即：

$B=A x=(L U) x=L(U x)=L y$

d) 计算样条曲线的系数：

$\begin{aligned} &a_{i}=y_{i}\\ &b_{i}=\frac{y_{i+1}-y_{i}}{h_{i}}-\frac{h_{i}}{2} m_{i}-\frac{h_{i}}{6}\left(m_{i+1}-m_{i}\right)\\ &c_{i}=\frac{m_{i}}{2}\\ &d_{i}=\frac{m_{i+1}-m_{i}}{6 h_{i}} \end{aligned}$

其中i=0，1，...，n-1

e. 在每个子区间

$x_{i} \leq x \leq x_{i+1}$ 中，创建方程：

$g_{i}(x)=a_{i}+b_{i}\left(x-x_{i}\right)+c_{i}\left(x-x_{i}\right)^{2}+d_{i}\left(x-x_{i}\right)^{3}$

4、举例

以y=sin(x)为例, x步长为1，x取值范围是[0,10]。对它使用三次样条插值，插值前后对比如下：

5、Python代码实现

三阶样条曲线拟合代码如下：

#! /usr/bin/python

# -*- coding: utf-8 -*-

u"""

Cubic Spline library

author Atsushi Sakai

license: MIT

"""

import math

import numpy as np

class Spline:

u"""

Cubic Spline class

usage:

spline=Spline(x,y)

rx=np.arange(0,4,0.1)

ry=[spline.calc(i) for i in rx]

"""

def __init__(self, x, y):

self.b, self.c, self.d, self.w = [], [], [], []

self.x = x

self.y = y

self.nx = len(x) # dimension of x

h = np.diff(x)

# calc coefficient c

self.a = [iy for iy in y]

# calc coefficient c

A = self.__calc__A(h)

B = self.__calc__B(h)

self.c = np.linalg.solve(A, B)

# print(self.c1)

# calc spline coefficient b and d

for i in range(self.nx - 1):

self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))

tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * (self.c[i + 1] + 2.0 * self.c[i]) / 3.0

self.b.append(tb)

def calc(self, t):

u"""

Calc position

if t is outside of the input x, return None

"""

if t < self.x[0]:

return None

elif t > self.x[-1]:

return None

i = self.__search_index(t)

dx = t - self.x[i]

result = self.a[i] + self.b[i] * dx + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

return result

def __search_index(self, x):

u"""

search data segment index

"""

for i in range(self.nx):

if self.x[i] - x > 0:

return i - 1

def __calc__A(self, h):

u"""

calc matrix A for spline coefficient c

"""

A = np.zeros((self.nx, self.nx))

A[0, 0] = 1.0

for i in range(self.nx - 1):

if i is not self.nx - 2:

A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])

A[i + 1, i] = h[i]

A[i, i + 1] = h[i]

A[0, 1] = 0.0

A[self.nx - 1, self.nx - 2] = 0.0

A[self.nx - 1, self.nx - 1] = 1.0

return A

def __calc__B(self, h):

u"""

calc matrix B for spline coefficient c

"""

B = np.zeros(self.nx)

for i in range(self.nx - 2):

B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]

return B

使用上述代码将点集拟合成曲线：

def test1():

import matplotlib.pyplot as plt

# input

x = [-2.5, 0.0, 2.5, 5.0, 7.5]

y = [0.7, -6, 5, 6.5, 0.0]

# 3d spline interporation

spline = Spline(x, y)

rx = np.arange(-2.5, 7.5, 0.01)

ry = [spline.calc(i) for i in rx]

plt.plot(x, y, "xb")

plt.plot(rx, ry, "-r")

plt.grid(True)

plt.axis("equal")

plt.show()

if __name__ == '__main__':

test1()

拟合效果如下：

注：本文首发于微信公众号，转载请注明出处，谢谢！公众号:半杯茶的小酒杯

个人博客地址:http://www.banbeichadexiaojiubei.comwww.banbeichadexiaojiubei.com

参考链接

如需代码请到公众号:【半杯茶的小酒杯】回复"Cubic-Spline-Python20200127"获取。

相关文章半杯茶的小酒杯：未知环境下的Lidar概率占位栅格图(Occupancy Grid Map) Python代码实现zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶Mapping-占位栅格图(Occupancy Grid Map)zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶中的车辆运动学模型zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶定位算法(十五)基于多传感器融合的状态估计(muti-Sensors Fusion)zhuanlan.zhihu.com自动驾驶路径规划器-Lattice Planner详解www.banbeichadexiaojiubei.com半杯茶的小酒杯：自动驾驶定位算法(十四)-递归贝叶斯滤波(Bayes Filter)zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶定位算法(十三)-粒子滤波(Particle Filter)zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶硬件系统(十二)-激光雷达(Lidar)测量模型zhuanlan.zhihu.com半杯茶的小酒杯：自动驾驶硬件(十一)-Global Navigation Satellite System-GNSSzhuanlan.zhihu.com自动驾驶定位算法(九)-直方图滤波(Histogram Filter)定位www.banbeichadexiaojiubei.com

滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...相关推荐

aitken插值方法的c++代码_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...
自动驾驶运动规划(Motion Planning)是无人驾驶汽车的核心模块之一,它的主要任务之一就是如何生成舒适的.碰撞避免的行驶路径和舒适的运动速度.生成行驶路径最经典方法之一就是是Sampling ...
Dijkstra 路径规划算法原理详解及 Python 代码实现
荷兰数学家 E.W.Dijkstra 于 1959 年提出了 Dijkstra 算法,它是一种适用于非负权值网络的单源最短路径算法,同时也是目前求解最短路径问题的理论上最完备.应用最广的经典算法 ...
无人驾驶路径规划（三）局部路径规划-Frenet坐标系下的动态轨迹规划
前言:对于无人驾驶路径规划系列的第二篇RRT算法的改进部分,由于有些内容属于个人想到的创新点,有想法投一篇小论文所以暂时没有公开,等后续完成后我会再公开介绍.今天第三篇内容开启一个新的算法介绍:Fre ...
自动驾驶路径规划技术-高速公路路径规划
Path Planning - Highway Driving project Github: https://github.com/williamhyin/CarND-Path-Planning E ...
路径规划算法(0) -路径规划与 Dubins, Reeds-Shepp曲线的python实现
参考资料: http://planning.cs.uiuc.edu/node821.html 一般常见的路径规划算法, 如 Dijkstra, A*, BFS算法, 重点关注可达性, 搜索速度和效率上 ...
夜光：多 AGV 小车的路径规划技术的研究（三）
夜光序言: 我们在寂寞中靠近拥抱中痊愈却不敢轻易说爱情有些人爱着爱着就变了而誓言爱着爱着会忘记正文:作为一名科研人员,要沉着冷静~ 基于聚类分区的 AGV 栅格地图建模方法 ~~ 环境建模是 ...
夜光：多 AGV 小车的路径规划技术的研究（二）
夜光序言: 我爱的是你爱你的是我你是我的爱是的我爱你正文:沉下心来,厚积薄发~ 创新点可总结如下: (1) 提出采用基于 Canopy 的 K-means 聚类算法进行地图分区,首先采用 C ...
夜光：多 AGV 小车的路径规划技术的研究（八）
夜光序言: 毅力是什么? 毅力就是坚持,有毅力就代表着努力. 没有毅力,一个人将一事无成, 再宏伟的梦想也只能是一个梦而已, 再宏大的理想也只能是想一想罢了. 正文: 基于 A 星算法和蚁群算法的融合 ...
路径规划之基于插值的规划算法
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人曲线插值拟合算法研究现状简述(包含原文PDF百度云下载链接) 第一类是采用多项式曲线来描述机器人运动轨迹,这是因为多项式 ...

滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...

滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...相关推荐

最新文章

热门文章

滑动轨迹 曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...

滑动轨迹 曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...相关推荐

最新文章

热门文章

滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...

滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...相关推荐