本人的自动驾驶小车，需要始终获得较为准确的位置信息。采用了一款国外的室内GPS设备来进行定位，官方给出的设备误差在±2cm ，使用过程中设备误差确实比较小。但是在信号正常的情况下，总是时不时的会出现短暂性的“飞点”，这些“飞点”对我的车影响很大，因为车上没有其他定位传感器，无法进行数据融合，考虑使用简单算法实现数据滤波的功能。
仔细研究了常见的十大滤波算法（滑动均值滤波，限幅滤波之类的），并不能满足我的要求。
根据滑动均值滤波的思路和微分的思想，想到滑动三次样条插值滤波方法。
该方法的基本原理：
- 对位置坐标点数据进行三次样条拟合，截取一段数据（滑动窗）进行拟合，得出拟合方程
- 对最后一个拟合方程进行求导，将滑动窗的最后一个坐标点的x 值代入求导后的方程，得出该点的斜率k
- 根据微分的思想，对下一个点进行预测，根据GPS的更新频率，得出点与点之间的间隔时间Δt (我这里是100Hz,Δt=0.01s），根据正常点数据计算出Δv（我的车Δv大致是 0.37m/s）
- 根据k、Δt、Δv预测出下一个点的位置。计算公式如下
从网站上找到合适的三次样条插值算法，对其进行了修改，来实现我想要的功能。
最终的python代码如下：
Function函数：
import numpy as np
def runge(y):
y = np.float32(y)
return 1 / (1 + y * y)
def spline3_Parameters(x_vec, opt=0):
# 建立三对角矩阵的 4n 个方程的左边部分
# parameter为二维数组，用来存放参数，size_of_Interval为区间的个数
x_new = np.array(x_vec)
parameter = []
size_of_Interval = len(x_new) - 1
i = 1
# 相邻两区间公共节点处函数值相等的方程，共2n-2个
while i < len(x_new) - 1:
data = np.zeros(size_of_Interval * 4)
data[(i - 1) * 4] = x_new[i] * x_new[i] * x_new[i]
data[(i - 1) * 4 + 1] = x_new[i] * x_new[i]
data[(i - 1) * 4 + 2] = x_new[i]
data[(i - 1) * 4 + 3] = 1
parameter.append(data)
data = np.zeros(size_of_Interval * 4)
data[i * 4] = x_new[i] * x_new[i] * x_new[i]
data[i * 4 + 1] = x_new[i] * x_new[i]
data[i * 4 + 2] = x_new[i]
data[i * 4 + 3] = 1
parameter.append(data)
i += 1
# 左右端点处的函数值。为两个方程, 加上前面的2n-2个方程，一共2n个方程
data = np.zeros(size_of_Interval * 4)
data[0] = x_new[0] * x_new[0] * x_new[0]
data[1] = x_new[0] * x_new[0]
data[2] = x_new[0]
data[3] = 1
parameter.append(data)
data = np.zeros(size_of_Interval * 4)
data[(size_of_Interval - 1) * 4] = x_new[-1] * x_new[-1] * x_new[-1]
data[(size_of_Interval - 1) * 4 + 1] = x_new[-1] * x_new[-1]
data[(size_of_Interval - 1) * 4 + 2] = x_new[-1]
data[(size_of_Interval - 1) * 4 + 3] = 1
parameter.append(data)
# 端点函数一阶导数值相等为n-1个方程。加上前面的方程为3n-1个方程。
i = 1
while i < size_of_Interval:
data = np.zeros(size_of_Interval * 4)
data[(i - 1) * 4] = 3 * x_new[i] * x_new[i]
data[(i - 1) * 4 + 1] = 2 * x_new[i]
data[(i - 1) * 4 + 2] = 1
data[i * 4] = -3 * x_new[i] * x_new[i]
data[i * 4 + 1] = -2 * x_new[i]
data[i * 4 + 2] = -1
parameter.append(data)
i += 1
# 端点函数二阶导数值相等为n-1个方程。加上前面的方程为4n-2个方程。
i = 1
while i < len(x_new) - 1:
data = np.zeros(size_of_Interval * 4)
data[(i - 1) * 4] = 6 * x_new[i]
data[(i - 1) * 4 + 1] = 2
data[i * 4] = -6 * x_new[i]
data[i * 4 + 1] = -2
parameter.append(data)
i += 1
# 两个附加条件
# 默认情况：opt = 0，not-a-knot边界条件：左边两端点三阶导相等，右边两端点三阶导也相等
if opt == 0:
data = np.zeros(size_of_Interval * 4)
data[0] = 6
data[4] = -6
parameter.append(data)
data = np.zeros(size_of_Interval * 4)
data[-4] = 6
data[-8] = -6
parameter.append(data)
# opt = 1，给定左右两端点的一阶导值
if opt == 1:
data = np.zeros(size_of_Interval * 4)
data[0] = 3 * x_new[0] * x_new[0]
data[1] = 2 * x_new[0]
data[2] = 1
parameter.append(data)
data = np.zeros(size_of_Interval * 4)
data[-4] = 3 * x_new[-1] * x_new[-1]
data[-3] = 2 * x_new[-1]
data[-2] = 1
parameter.append(data)
# opt = 2，给定左右两端点的二阶导值
if opt == 2:
data = np.zeros(size_of_Interval * 4)
data[0] = 6 * x_new[0]
data[1] = 2
parameter.append(data)
data = np.zeros(size_of_Interval * 4)
data[-4] = 6 * x_new[-1]
data[-3] = 2
parameter.append(data)
return parameter
def solution_of_equation(functype, parametes, x, y=0, func=runge, opt=0, lval=0, rval=0):
# 建立三对角线性方程组并求解，得到各段三次函数的系数并返回
# functype 表示需要拟合的是给定函数 / 给定点集
size_of_Interval = len(x) - 1
result = np.zeros(size_of_Interval * 4)
i = 1
if functype != 'give_nodes':
raise ValueError("functype should be 'give_nodes' ")
if functype == 'give_nodes':
if len(x) != len(y):
raise ValueError("Expect a node set!")
while i < size_of_Interval:
result[(i - 1) * 2] = y[i]
result[(i - 1) * 2 + 1] = y[i]
i += 1
result[(size_of_Interval - 1) * 2] = y[0]
result[(size_of_Interval - 1) * 2 + 1] = y[-1]
# 默认情况：opt = 0，not-a-knot边界条件：左边两端点三阶导相等，右边两端点三阶导也相等
if opt == 0:
result[-2] = result[-1] = 0
# opt = 1 或 opt = 2，给定左右两端点的一阶导值 / 二阶导值
if opt == 1 or opt == 2:
result[-2] = lval
result[-1] = rval
a = np.array(parametes)
b = np.array(result)
return np.linalg.solve(a, b)spline函数：
import Functions as F
def give_nodes(x, y, q,opt=0, lval=0, rval=0):
if opt == 0 and (lval != 0 or rval != 0):
raise ValueError('There should be no parameters of lval and rval by default')
if opt != 0 and opt != 1 and opt != 0 and opt != 2:
raise ValueError('opt should be 0 or 1 or 2!')
if opt == 0:
res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x), x, y)
if opt == 1:
res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x, 1), x, y, opt=1, lval=lval, rval=rval)
if opt == 2:
res = F.solution_of_equation('give_nodes', F.spline3_Parameters(x, 2), x, y, opt=2, lval=lval, rval=rval)
k = []
for i in range(len(x) - 1):
z='{0:.3f}'.format(3*res[4 * i] * (q ** 2) + 2*res[1 + 4 * i] * q + res[2 + 4 * i] )
k.append(z)
return k[-1]ertrapolation函数：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import spline as sp
import numpy as np
import math
def extrapolation(x,y,delta_V=0.37,delta_T=0.01):
'''
输入参数：进行拟合的数组x,y ; 速度，时间 默认ΔV = 0.37 ΔT = 0.01
返回 (x,y)
'''
xn = None
yn = None
try:
#计算拟合曲线在临界点的斜率k
b =x[-1]
k = sp.give_nodes(x, y, b)
k = float(k)
#判断斜率
if k == 0 and x[-1]>x[-2]:
xn = x[-1]+delta_V * delta_T
yn = y[-1]
elif k == 0 and x[-1]<x[-2]:
xn = x[-1] - delta_V * delta_T
yn = y[-1]
elif k >= 100 :
xn = x[-1]
yn = y[-1] + delta_V * delta_T
elif k <= -100:
xn = x[-1]
yn = y[-1] - delta_V * delta_T
else:
A = math.atan(k)
xn = x[-1]+ math.cos(A)*delta_V * delta_T
yn = y[-1] + math.sin(A)*delta_V * delta_T
except np.linalg.LinAlgError:
pass
return [xn,yn]将插值和GPS发送过来的数据做比较,绝对误差大于某个值返回插值数据，小于返回GPS数据。
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
