问题描述

已知一条n阶贝塞尔曲线

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

图1

拟合曲线生成过程

其中新的贝塞尔曲线的起点和终点是确定的，需要生成所有的控制点，如公式（1）所示。

在公式（1）中，函数f ff表示贝塞尔曲线，阶数为点的个数减1，t tt表示与点P PP最近的贝塞尔曲线点的时间参数。

参考程序

#!/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])

注意事项

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