python 三次曲线拟合算法_无人驾驶路径规划技术-三次样条插值(Cubic Spline Interpolation)曲线及Python代码实现...
2020年1月27日
|
by YoungTimes
|
No comments
无人驾驶路径规划技术-三次样条插值(Cubic Spline Interpolation)曲线及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$，则有：
a. $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()
拟合效果如下：
参考链接
完整代码链接：
或者关注微信公众号:【半杯茶的小酒杯】并回复”Cubic-Spline-Python20200127″获取完整代码。
相关文章
除非注明，否则均为[半杯茶的小酒杯]原创文章，转载必须以链接形式标明本文链接