问题描述
已知一条n阶贝塞尔曲线 L ( P 0 , P 1 , P 2 , P 3 , . . . , P n ) L(P0, P1, P2, P3, ..., Pn) L(P0,P1,P2,P3,...,Pn)( P 0 P0 P0为起点, P 1 P1 P1为第一个控制点, P 2 P2 P2为第二个控制点, P 3 P3 P3为第三个控制点, P n Pn Pn为终点)和一个点P,拟合一条连接新的n阶贝塞尔曲线 L 1 ( P 0 1 , P 1 1 , P 2 1 , P 3 1 , . . . , P n 1 , ) L_{1}(P0_{1}, P1_{1}, P2_{1}, P3_{1},..., Pn_{1}, ) L1(P01,P11,P21,P31,...,Pn1,), P 0 1 P0_{1} P01和点 P P P相重合, P n 1 Pn_{1} Pn1和 P n Pn Pn相重合,曲线 L L L尽可能和曲线 L 1 L_{1} L1相重合,如图1所示。
拟合曲线生成过程
其中新的贝塞尔曲线的起点和终点是确定的,需要生成所有的控制点,如公式(1)所示。
P
0
1
=
P
P
1
1
=
f
(
P
1
,
P
2
,
P
3
,
.
.
.
,
P
n
,
t
)
P
2
1
=
f
(
P
2
,
P
3
,
.
.
.
,
P
n
,
t
)
P
3
1
=
f
(
P
3
,
P
4
,
.
.
.
,
P
n
,
t
)
.
.
.
P
n
1
=
P
n
(1)
\begin{array}{l} P0_{1} = P \\ P1_{1} = f(P1,P2,P3,...,Pn,t) \\ P2_{1} = f(P2, P3, ..., Pn,t) \\ P3_{1} = f(P3, P4, ..., Pn,t) \\ ... \\ Pn_{1} = Pn \end{array} \tag1
P01=PP11=f(P1,P2,P3,...,Pn,t)P21=f(P2,P3,...,Pn,t)P31=f(P3,P4,...,Pn,t)...Pn1=Pn(1)
在公式(1)中,函数
f
f
f表示贝塞尔曲线,阶数为点的个数减1,
t
t
t表示与点
P
P
P最近的贝塞尔曲线点的时间参数。
参考程序
#!/usr/bin/env python3
#-*- coding:utf-8 -*-
import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import math
class Point:
x = 0.0 #单位m
y = 0.0 #单位m
yaw = 0.0 #单位rad
def __init__(self, x=0.0, y=0.0, yaw=0.0):
self.x = x
self.y = y
self.yaw = yaw
def __str__(self):
return ("(x: {}; y: {}; yaw: {})".format(self.x, self.y, self.yaw))
def display(l1, l2):
print(l1[0], l1[1], l1[2], l1[3], l1[4], l1[5])
print(l2[0], l2[1], l2[2], l2[3], l2[4], l2[5])
fig = plt.figure()
# # 连续性的画图
# ax = fig.add_subplot(1, 1, 1)
# # 设置图像显示的时候XY轴比例
# ax.axis("equal")
# # 开启一个画图的窗口
# plt.ion()
# plt.xlim(-10, 10)
# plt.ylim(-10, 10)
x1_list = []
y1_list = []
for t in np.arange(0.0, 1.0, 0.01):
p = calcu5BezierCurvePoint(l1[0], l1[1], l1[2], l1[3], l1[4], l1[5], t)
x1_list.append(p.x)
y1_list.append(p.y)
plt.plot(x1_list, y1_list, '-')
x2_list = []
y2_list = []
for t in np.arange(0.0, 1.0, 0.01):
p = calcu5BezierCurvePoint(l2[0], l2[1], l2[2], l2[3], l2[4], l2[5], t)
x2_list.append(p.x)
y2_list.append(p.y)
plt.plot(x2_list, y2_list, '-')
plt.show()
def calcuSlopeOfBezierCurve(p0, p1, p2, p3, p4, p5, t):
dx = 0.0
dy = 0.0
l_t = 1.0 - t
dx = 5.0 * (l_t * l_t * l_t * l_t * (p1.x - p0.x) + 4 * l_t * l_t * l_t * t * (p2.x - p1.x) +
6 * l_t * l_t * t * t * (p3.x - p2.x) + 4 * l_t * t * t * t * (p4.x - p3.x) + t * t * t * t * (p5.x - p4.x))
dy = 5.0 * (l_t * l_t * l_t * l_t * (p1.y - p0.y) + 4 * l_t * l_t * l_t * t * (p2.y - p1.y) +
6 * l_t * l_t * t * t * (p3.y - p2.y) + 4 * l_t * t * t * t * (p4.y - p3.y) + t * t * t * t * (p5.y - p4.y))
return dx, dy
def calcu1BezierCurvePoint(p0, p1, t):
l_t = 1.0 - t
x = l_t * p0.x + t * p1.x
y = l_t * p0.y + t * p1.y
return Point(x, y, 0)
def calcu2BezierCurvePoint(p0, p1, p2, t):
l_t = 1.0 - t
x = l_t * l_t * p0.x + 2 * l_t * t * p1.x + t * t * p2.x
y = l_t * l_t * p0.y + 2 * l_t * t * p1.y + t * t * p2.y
return Point(x, y, 0)
def calcu3BezierCurvePoint(p0, p1, p2, p3, t):
l_t = 1.0 - t
x = l_t * l_t * l_t * p0.x + 3 * l_t * l_t * t * p1.x + 3 * l_t * t * t * p2.x + t * t * t * p3.x
y = l_t * l_t * l_t * p0.y + 3 * l_t * l_t * t * p1.y + 3 * l_t * t * t * p2.y + t * t * t * p3.y
return Point(x, y, 0)
def calcu4BezierCurvePoint(p0, p1, p2, p3, p4, t):
l_t = 1.0 - t
x = l_t * l_t * l_t * l_t * p0.x + 4 * l_t * l_t * l_t * t * p1.x + 6 * l_t * l_t * t * t * p2.x + \
4 * l_t * t * t * t * p3.x + t * t * t * t * p4.x
y = l_t * l_t * l_t * l_t * p0.y + 4 * l_t * l_t * l_t * t * p1.y + 6 * l_t * l_t * t * t * p2.y + \
4 * l_t * t * t * t * p3.y + t * t * t * t * p4.y
return Point(x, y, 0)
def calcu5BezierCurvePoint(p0, p1, p2, p3, p4, p5, t):
l_t = 1.0 - t
x = l_t * l_t * l_t * l_t * l_t * p0.x + 5 * l_t * l_t * l_t * l_t * t * p1.x + 10 * l_t * l_t * l_t * t * t * p2.x + \
10 * l_t * l_t * t * t * t * p3.x + 5 * l_t * t * t * t * t * p4.x + t * t * t * t * t * p5.x
y = l_t * l_t * l_t * l_t * l_t * p0.y + 5 * l_t * l_t * l_t * l_t * t * p1.y + 10 * l_t * l_t * l_t * t * t * p2.y + \
10 * l_t * l_t * t * t * t * p3.y + 5 * l_t * t * t * t * t * p4.y + t * t * t * t * t * p5.y
dx, dy = calcuSlopeOfBezierCurve(p0, p1, p2, p3, p4, p5, t)
return Point(x, y, math.atan2(dy, dx))
def calcuClosetPointIn5Bezier(p0, p1, p2, p3, p4, p5, start_point):
min_d = 99999999.0
min_t = 0.0
for t in np.arange(0.0, 1.001, 0.01):
p = calcu5BezierCurvePoint(p0, p1, p2, p3, p4, p5, t)
d = math.hypot(p.y - start_point.y, p.x - start_point.x)
if min_d > d:
min_d = d
min_t = t
return min_t
def bezierFit(p0, p1, p2, p3, p4, p5, start_point):
p00 = Point()
p01 = Point()
p02 = Point()
p03 = Point()
p04 = Point()
p05 = Point()
t = calcuClosetPointIn5Bezier(p0, p1, p2, p3, p4, p5, start_point)
p00 = start_point
p01 = calcu4BezierCurvePoint(p1, p2, p3, p4, p5, t)
p02 = calcu3BezierCurvePoint(p2, p3, p4, p5, t)
p03 = calcu2BezierCurvePoint(p3, p4, p5, t)
p04 = calcu1BezierCurvePoint(p4, p5, t)
p05 = p5
return p00, p01, p02, p03, p04, p05
def bezierFit2(p0, p1, p2, p3, p4, p5, end_point):
p00 = Point()
p01 = Point()
p02 = Point()
p03 = Point()
p04 = Point()
p05 = Point()
t = calcuClosetPointIn5Bezier(p0, p1, p2, p3, p4, p5, end_point)
p00 = p0
p01 = calcu1BezierCurvePoint(p0, p1, t)
p02 = calcu2BezierCurvePoint(p0, p1, p2, t)
p03 = calcu3BezierCurvePoint(p0, p1, p2, p3, t)
p04 = calcu4BezierCurvePoint(p0, p1, p2, p3, p4, t)
p05 = end_point
return p00, p01, p02, p03, p04, p05
if __name__ == "__main__":
p0 = Point(0.0, 0.0)
p1 = Point(1.0, 1.0)
p2 = Point(2.0, -1.0)
p3 = Point(3.0, 1.0)
p4 = Point(4.0, -1.0)
p5 = Point(5.0, 0.0)
start_pose = Point(2.5, 0.0)
p00, p01, p02, p03, p04, p05 = bezierFit2(p0, p1, p2, p3, p4, p5, start_pose)
display([p0, p1, p2, p3, p4, p5], [p00, p01, p02, p03, p04, p05])
注意事项
- 点 P P P在贝塞尔曲线附近能保证较好的效果;
- 本文只介绍正向的拟合过程,逆向的拟合过程调换以下顺序即可;