问题描述

已知一条n阶贝塞尔曲线$L(P0,P1,P2,P3,...,Pn)$（$P0$为起点，$P1$为第一个控制点，$P2$为第二个控制点，$P3$为第三个控制点，$Pn$为终点）和一个点P，拟合一条连接新的n阶贝塞尔曲线$L_1（P{0}_1,P{1}_1,P{2}_1,P{3}_1,...,P{n}_1,）$，$P{0}_1$和点$P$相重合，$P{n}_1$和$Pn$相重合，曲线$L$尽可能和曲线$L_1$相重合，如图1所示。

图1

拟合曲线生成过程

其中新的贝塞尔曲线的起点和终点是确定的，需要生成所有的控制点，如公式（1）所示。
$$\begin{array}{l}P{0}_1=P\\ P{1}_1=f(P1,P2,P3,...,Pn,t)\\ P{2}_1=f(P2,P3,...,Pn,t)\\ P{3}_1=f(P3,P4,...,Pn,t)\\ ...\\ P{n}_1=Pn\end{array}\text{\hspace{1em}(1)}$$
在公式（1）中，函数$f$表示贝塞尔曲线，阶数为点的个数减1，$t$表示与点$P$最近的贝塞尔曲线点的时间参数。

参考程序

#!/usr/bin/env python3
#-*- coding:utf-8 -*-import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import mathclass Point:x = 0.0    #单位my = 0.0    #单位myaw = 0.0    #单位raddef __init__(self, x=0.0, y=0.0, yaw=0.0):self.x = xself.y = yself.yaw = yawdef __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.0dy = 0.0l_t = 1.0 - tdx = 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, dydef calcu1BezierCurvePoint(p0, p1, t):l_t = 1.0 - tx = l_t * p0.x + t * p1.xy = l_t * p0.y + t * p1.yreturn Point(x, y, 0)def calcu2BezierCurvePoint(p0, p1, p2, t):l_t = 1.0 - tx = l_t * l_t * p0.x + 2 * l_t * t * p1.x + t * t * p2.xy = l_t * l_t * p0.y + 2 * l_t * t * p1.y + t * t * p2.yreturn Point(x, y, 0)def calcu3BezierCurvePoint(p0, p1, p2, p3, t):l_t = 1.0 - tx = 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.xy = 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.yreturn Point(x, y, 0)def calcu4BezierCurvePoint(p0, p1, p2, p3, p4, t):l_t = 1.0 - tx = 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.xy = 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.yreturn Point(x, y, 0)def calcu5BezierCurvePoint(p0, p1, p2, p3, p4, p5, t):l_t = 1.0 - tx = 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.xy = 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.ydx, 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.0min_t = 0.0for 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 = dmin_t = treturn min_tdef 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_pointp01 = 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 = p5return p00, p01, p02, p03, p04, p05def 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 = p0p01 = 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_pointreturn p00, p01, p02, p03, p04, p05if __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])

注意事项

1. 点$P$在贝塞尔曲线附近能保证较好的效果；
2. 本文只介绍正向的拟合过程，逆向的拟合过程调换以下顺序即可；