【线】轨迹插值

三阶贝塞尔曲线插值

如果我们把三阶贝塞尔曲线的P0和P3视为原始数据,只要找到P1和P2两个点(控制点),就可以根据三阶贝塞尔曲线公式,计算出P0和P3之间平滑曲线上的任意点。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
# coding=utf-8
import numpy as np
import matplotlib.pyplot as plt

def bezier_curve(p0, p1, p2, p3, inserted):
'''
三阶贝塞尔曲线
p0, p1, p2, p3:点坐标
inserted:p0和p3之间插值的数量
'''
assert isinstance(p0, (tuple, list, np.ndarray)), u'点坐标不是期望的元组、列表或numpy数组类型'
assert isinstance(p0, (tuple, list, np.ndarray)), u'点坐标不是期望的元组、列表或numpy数组类型'
assert isinstance(p0, (tuple, list, np.ndarray)), u'点坐标不是期望的元组、列表或numpy数组类型'
assert isinstance(p0, (tuple, list, np.ndarray)), u'点坐标不是期望的元组、列表或numpy数组类型'

if isinstance(p0, (tuple, list)):
p0 = np.array(p0)
if isinstance(p1, (tuple, list)):
p1 = np.array(p1)
if isinstance(p2, (tuple, list)):
p2 = np.array(p2)
if isinstance(p3, (tuple, list)):
p3 = np.array(p3)

points = list()
for t in np.linspace(0, 1, inserted+2):
points.append(p0*np.power((1-t),3) + 3*p1*t*np.power((1-t),2) + 3*p2*(1-t)*np.power(t,2) + p3*np.power(t,3))

return np.vstack(points)


def interpolation_base_bezier(date_x, date_y, k=0.5, inserted=10, closed=False):
'''
基于三阶贝塞尔曲线的数据插值
k:调整平滑曲线形状的因子,取值一般在0.2~0.6之间。默认值为0.5
inserted:两个原始数据点之间插值的数量。默认值为10
closed:曲线是否封闭,如是,则首尾相连。默认曲线不封闭
'''
assert isinstance(date_x, (list, np.ndarray)), u'x数据集不是期望的列表或numpy数组类型'
assert isinstance(date_y, (list, np.ndarray)), u'y数据集不是期望的列表或numpy数组类型'

if isinstance(date_x, list) and isinstance(date_y, list):
assert len(date_x)==len(date_y), u'x数据集和y数据集长度不匹配'
date_x = np.array(date_x)
date_y = np.array(date_y)
elif isinstance(date_x, np.ndarray) and isinstance(date_y, np.ndarray):
assert date_x.shape==date_y.shape, u'x数据集和y数据集长度不匹配'
else:
raise Exception(u'x数据集或y数据集类型错误')

# 第1步:生成原始数据折线中点集
mid_points = list()
for i in range(1, date_x.shape[0]):
mid_points.append({
'start': (date_x[i-1], date_y[i-1]),
'end': (date_x[i], date_y[i]),
'mid': ((date_x[i]+date_x[i-1])/2.0, (date_y[i]+date_y[i-1])/2.0)
})

if closed:
mid_points.append({
'start': (date_x[-1], date_y[-1]),
'end': (date_x[0], date_y[0]),
'mid': ((date_x[0]+date_x[-1])/2.0, (date_y[0]+date_y[-1])/2.0)
})

# 第2步:找出中点连线及其分割点
split_points = list()
for i in range(len(mid_points)):
if i < (len(mid_points)-1):
j = i+1
elif closed:
j = 0
else:
continue

x00, y00 = mid_points[i]['start']
x01, y01 = mid_points[i]['end']
x10, y10 = mid_points[j]['start']
x11, y11 = mid_points[j]['end']
d0 = np.sqrt(np.power((x00-x01), 2) + np.power((y00-y01), 2))
d1 = np.sqrt(np.power((x10-x11), 2) + np.power((y10-y11), 2))
k_split = 1.0*d0/(d0+d1)

mx0, my0 = mid_points[i]['mid']
mx1, my1 = mid_points[j]['mid']

split_points.append({
'start': (mx0, my0),
'end': (mx1, my1),
'split': (mx0+(mx1-mx0)*k_split, my0+(my1-my0)*k_split)
})

# 第3步:平移中点连线,调整端点,生成控制点
crt_points = list()
for i in range(len(split_points)):
vx, vy = mid_points[i]['end'] # 当前顶点的坐标
dx = vx - split_points[i]['split'][0] # 平移线段x偏移量
dy = vy - split_points[i]['split'][1] # 平移线段y偏移量

sx, sy = split_points[i]['start'][0]+dx, split_points[i]['start'][1]+dy # 平移后线段起点坐标
ex, ey = split_points[i]['end'][0]+dx, split_points[i]['end'][1]+dy # 平移后线段终点坐标

cp0 = sx+(vx-sx)*k, sy+(vy-sy)*k # 控制点坐标
cp1 = ex+(vx-ex)*k, ey+(vy-ey)*k # 控制点坐标

if crt_points:
crt_points[-1].insert(2, cp0)
else:
crt_points.append([mid_points[0]['start'], cp0, mid_points[0]['end']])

if closed:
if i < (len(mid_points)-1):
crt_points.append([mid_points[i+1]['start'], cp1, mid_points[i+1]['end']])
else:
crt_points[0].insert(1, cp1)
else:
if i < (len(mid_points)-2):
crt_points.append([mid_points[i+1]['start'], cp1, mid_points[i+1]['end']])
else:
crt_points.append([mid_points[i+1]['start'], cp1, mid_points[i+1]['end'], mid_points[i+1]['end']])
crt_points[0].insert(1, mid_points[0]['start'])

# 第4步:应用贝塞尔曲线方程插值
out = list()
for item in crt_points:
group = bezier_curve(item[0], item[1], item[2], item[3], inserted)
out.append(group[:-1])

out.append(group[-1:])
out = np.vstack(out)

return out.T[0], out.T[1]


if __name__ == '__main__':

x = np.array([2,4,4,3,2])
y = np.array([2,2,4,3,4])

plt.plot(x, y, 'ro')
x_curve, y_curve = interpolation_base_bezier(x, y, k=0.3, closed=True)
plt.plot(x_curve, y_curve, label='$k=0.3$')
x_curve, y_curve = interpolation_base_bezier(x, y, k=0.4, closed=True)
plt.plot(x_curve, y_curve, label='$k=0.4$')
x_curve, y_curve = interpolation_base_bezier(x, y, k=0.5, closed=True)
plt.plot(x_curve, y_curve, label='$k=0.5$')
x_curve, y_curve = interpolation_base_bezier(x, y, k=0.6, closed=True)
plt.plot(x_curve, y_curve, label='$k=0.6$')
plt.legend(loc='best')

plt.show()

三阶B样条插值

B样条克服了贝塞尔曲线的一些缺点,贝塞尔曲线的每个控制点对整条曲线都有影响,也就是说改变一个控制点的位置整条曲线的形状都会发生变化,而B样条中的每个控制点只会影响曲线的一段参数范围,从而实现了局部修改。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
# coding=utf-8
import numpy as np
import bisect
import matplotlib.pyplot as plt

class interpolation_base_spline:
'''
三次样条类插值
'''
def __init__(self, x, y):
self.a, self.b, self.c, self.d = [], [], [], []

self.x = x
self.y = y

self.nx = len(x)
h = np.diff(x)

self.a = [iy for iy in y]

A = self.__calc_A(h)
B = self.__calc_B(h)
self.m = np.linalg.solve(A, B)
self.c = self.m / 2.0

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):
'''
计算位置
当t超过边界,返回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):
return bisect.bisect(self.x, x) - 1

def __calc_A(self, h):
'''
计算算法第二步中的等号左侧的矩阵表达式A
'''
A = np.zeros((self.nx, self.nx))
A[0, 0] = 1.0
for 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.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):
'''
计算算法第二步中的等号右侧的矩阵表达式B
'''
B = np.zeros(self.nx)
for i in range(self.nx - 2):
B[i + 1] = 6.0 * (self.a[i + 2] - self.a[i + 1]) / h[i + 1] - 6.0 * (self.a[i + 1] - self.a[i]) / h[i]
return B

if __name__ == '__main__':
x0 = [-4., -2, 0.0, 2, 4, 6, 10]
y0 = [1.2, 0.6, 0.0, 1.5, 3.8, 5.0, 3.0]

spline = interpolation_base_spline(x0, y0)
x1 = np.arange(-4.0, 10, 0.01)
y1 = [spline.calc(i) for i in x1]

plt.plot(x0, y0, "og")
plt.plot(x1, y1, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
0%