三次样条曲线拟合及Matlab/Python实现

对于形如y = a + bx + c * x^2 + d * x^3 的三次spline曲线拟合的数学原理，我就不多说了。

我接了一个图给大家看看：

数值计算的伪代码如下：

书名是：numerical_methods_for_engineers_for_engineers_chapra_canale_6th_edition
spline interpolation 在18.6章，想了解如何做三次曲线拟合的就去这个书里面找一下。

接下来就是Matlab 和 Python的实现：
Python代码来自https://github.com/gameinskysky/PythonRobotics/tree/master/PathPlanning/FrenetOptimalTrajectory
我稍作了修改：

class Spline:u"""Cubic Spline class"""def __init__(self, x, y):self.b, self.c, self.d, self.w = [], [], [], []self.x = xself.y = yself.nx = len(x)  # dimension of xh = np.diff(x)# calc coefficient cself.a = [iy for iy in y]# calc coefficient cA = self.__calc_A(h)B = self.__calc_B(h)self.c = np.linalg.solve(A, B)#  print(self.c1)# calc spline coefficient b and dfor 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.0self.b.append(tb)def calc(self, t):u"""Calc positionif t is outside of the input x, return None"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = 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.0return resultdef calcd(self, t):u"""Calc first derivativeif t is outside of the input x, return None"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = self.__search_index(t)dx = t - self.x[i]result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0return resultdef calcdd(self, t):u"""Calc second derivative"""if t < self.x[0]:return Noneelif t > self.x[-1]:return Nonei = self.__search_index(t)dx = t - self.x[i]result = 2.0 * self.c[i] + 6.0 * self.d[i] * dxreturn resultdef __search_index(self, x):u"""search data segment index"""return bisect.bisect(self.x, x) - 1def __calc_A(self, h):u"""calc matrix A for spline coefficient c"""A = np.zeros((self.nx, self.nx))A[0, 0] = 1.0for i in range(self.nx - 1):if i != (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.0A[self.nx - 1, self.nx - 2] = 0.0A[self.nx - 1, self.nx - 1] = 1.0#  print(A)return Adef __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]#  print(B)return Bclass Spline2D:u"""2D Cubic Spline class"""def __init__(self, x, y):self.s = self.__calc_s(x, y)self.sx = Spline(self.s, x)self.sy = Spline(self.s, y)def __calc_s(self, x, y):dx = np.diff(x)dy = np.diff(y)self.ds = [math.sqrt(idx ** 2 + idy ** 2)for (idx, idy) in zip(dx, dy)]s = [0]s.extend(np.cumsum(self.ds))return sdef calc_position(self, s):u"""calc position"""x = self.sx.calc(s)y = self.sy.calc(s)return x, ydef calc_curvature(self, s):u"""calc curvature"""dx = self.sx.calcd(s)ddx = self.sx.calcdd(s)dy = self.sy.calcd(s)ddy = self.sy.calcdd(s)k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)return kdef calc_yaw(self, s):u"""calc yaw"""dx = self.sx.calcd(s)dy = self.sy.calcd(s)yaw = math.atan2(dy, dx)return yawdef calc_spline_course(x, y, ds=0.1):sp = Spline2D(x, y)s = list(np.arange(0, sp.s[-1], ds))rx, ry, ryaw, rk = [], [], [], []for i_s in s:ix, iy = sp.calc_position(i_s)rx.append(ix)ry.append(iy)ryaw.append(sp.calc_yaw(i_s))rk.append(sp.calc_curvature(i_s))return rx, ry, ryaw, rk, s

然后再写一个test文件：

import sys;
sys.path.append("H:\Project\TrajectoryPlanningModelDesign\Codes\frenet_optimal\frenet_optimal")#这里写你存放上面这个文件的文件夹目录
import cubic_spline
import numpy as np
import matplotlib.pyplot as plt
x = [-4 ,-2, 0, 2, 4, 6, 10];
y = [1.2, 0.6, 0, 1.5, 3.8, 5, 3];
spline = cubic_spline.Spline(x,y)
rx = np.arange(-4,10,0.1)
ry = [spline.calc(i) for i in rx]
plt.plot(x,y,"og")
plt.plot(rx,ry,"-r")
plt.grid(True)
#因为在jupyter 里面写的，最开始调用一下import sys

结果如图：

把python写成Matlab

clc
clear allx = [-4 -2 0 2 4 6 10];y = [1.2 0.6 0 1.5 3.8 5 3];figureplot(x,y,'ro');hold onN = length(x);A = zeros(N,N);B = zeros(N,1);for i = 1:N-1h(i) = x(i+1) - x(i);endA(1,1) = 1; A(N,N) = 1;for i = 2:N-1A(i,i) = 2*(h(i-1) + h(i));endfor i  =2 : N-1A(i, i+1) = h(i);endfor i  = 2: N-1A(i,i-1) = h(i-1);endfor i = 2:N-1B(i) = 6* (y(i+1)-y(i))/h(i) - 6* (y(i)-y(i-1))/h(i-1);endm= A\Bfor i = 1:Na(i) = y(i);endfor i = 1:Nc(i) = m(i)/2;endfor i = 1:N-1d(i) =( c(i+1)-c(i) )/(3*h(i));endfor i = 1:N-1b(i)  = (a(i+1)-a(i))/h(i)- h(i)/3*(c(i+1)+ 2*c(i));endfor  i= 1:N-1X = x(i):0.1:x(i+1);Y = a(i)+ b(i)*(X-x(i)) + c(i) * (X- x(i)).^2 + d(i) * (X - x(i)).^3;plot(X, Y,'.-')   end

结果如图：

三次样条曲线拟合及Matlab/Python实现相关推荐

matlab python比较_MATLAB与Python的比较
知乎视频www.zhihu.com 我正巧两个语言都比较常用(我是从2010年开始使用MATLAB的, 从2013年开始使用Python.),从我的专栏里面就可以看出来:MATLAB Python ...
matlab ,python,c++关于格式化输出数字的表达
我们想要格式化输出1,2,3,...为001,002,003 ... 那么在matlab,python,c++该如何表达呢? matlab: >> filedir=sprintf( ...
win7+vs2015/13+caffe+matlab+python（CPU only）配置
首先声明本教程可以适用于vs2015 和vs2013 .以vs2015为例. 安装必备软件 vs 2015 /vs2013 matlab 2016a(64bit) 推荐使用Anaconda 2.7 或 ...
判断闰年的Matlab/Python函数
目录写在前面什么是闰年判断闰年的Matlab函数判断闰年的Python函数参考写在前面在处理自然科学数据时,经常需要判断一个年份(这里说的年份都是公历)是否为闰年,本文首先简单介绍闰年的 ...
CNN卷积神经网络案例程序源代码合集matlab/Python等
CNN卷积神经网络案例程序源代码合集matlab/Python等 1.深入理解CNN(包括CNN的过程显示和前向后向推倒,以及CNN的应用举例.) 2.kerasttensorflowCNN(CNN_ ...
VS2015+caffe+matlab+python+CPU
实验平台: Win7 64bit, VS 2015(Professional), matlab 2016b(64bit), python2.7.12, Eclipse IDE for Java Dev ...
样条函数 matlab,三次样条函数及MATLAB
三次样条函数及MATLAB 三次样条函数及MATLAB 三次样条函数及MATLAB function [s0,s1,s2,s3]=cubic_spline(x,y) if any(size(x) ~= ...
aitken插值方法的c++代码_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...
自动驾驶运动规划(Motion Planning)是无人驾驶汽车的核心模块之一,它的主要任务之一就是如何生成舒适的.碰撞避免的行驶路径和舒适的运动速度.生成行驶路径最经典方法之一就是是Sampling ...
滑动轨迹曲线 python_无人驾驶路径规划技术-三次样条插值曲线及Python代码实现...
自动驾驶运动规划(Motion Planning)是无人驾驶汽车的核心模块之一,它的主要任务之一就是如何生成舒适的.碰撞避免的行驶路径和舒适的运动速度.生成行驶路径最经典方法之一就是是Sampling ...

三次样条曲线拟合及Matlab/Python实现

三次样条曲线拟合及Matlab/Python实现相关推荐

最新文章

热门文章