轨迹规划之 贝塞尔曲线

前言

本篇开启轨迹规划内容。由寻路算法获得路点后，还要根据机器人的运动学、动力学约束优化生成机器人期望的运动轨迹。

本篇首先从贝塞尔曲线开始

贝塞尔曲线

贝塞尔曲线是常用的图形学设计、轨迹规划等方法。贝塞尔曲线是参数化曲线，n次贝塞尔曲线由n+1个控制点决定。

低次贝塞尔曲线的表达式

二次贝塞尔曲线由$p_0,p_1,p_2$决定：
$$B(t)=(1-t{)}^2{p}_0+2t(1-t)p_1+t^2{p}_2,0\le t\le 1$$

三次贝塞尔曲线由$p_0,p_1,p_2,p_3$决定：
$$B(t)=(1-t{)}^3{p}_0+3t(1-t{)}^2{p}_1+3t^2(1-t)p_2+t^3{p}_3,0\le t\le 1$$

由$B(t)$表达式可知，贝塞尔曲线上每个点实际上是控制点的凸组合，因此，贝塞尔曲线位于控制点形成的凸包内，并且仅通过起始和终止控制点。

贝塞尔曲线有良好的线性性质，对于线性变换$A$和平移$t$：
$$\begin{gathered} AB(t)=(1-t{)}^{3}A{p}_0+3t(1-t{)}^{2}A{p}_1+3t^2(1-t)A{p}_2+t^{3}A{p}_3,0\le t \\ B(t)+t=(1-t{)}^3(p_0+t)+3t(1-t{)}^2(p_1+t)+3t^2(1-t)(p_2+t)+t^3(p_3+t),0\le t \end{gathered}$$
也就是说，对贝塞尔曲线作线性变换和平移，等同于对控制点做线性变换和平移。

贝塞尔曲线的切线

对二次贝塞尔曲线求导：
$$\begin{gathered} B^{\prime }(t)=(-2+2t)p_0+(2-4t)p_1+2t{p}_2 \\ {B}^{\prime }(0)=2p_1-2p_0 \\ {B}^{\prime }(1)=2p_2-2p_1 \end{gathered}$$
可知在起始点和终止点处，贝塞尔曲线的切线与控制线是相切的。

高次贝塞尔曲线

高次贝塞尔曲线表达式

当控制点数量较多时，贝塞尔曲线的次数就越高。此时，贝塞尔曲线的表达式相对复杂，n次贝塞尔曲线：
$$\begin{gathered} B(t)=\sum _{i=0}^n{B}_{n,i}(t)p_i \\ {B}_{n,i}(t)=C_n^i(1-t{)}^{n-i}{t}^i \end{gathered}$$
用矩阵乘法形式简化表示：
$$B(t)=\left[\begin{array}{cccc}{p}_0& p_1& \dots & p_n\end{array}\right]\left[\begin{array}{c}{B}_{n,0}(t)\\ B_{n,1}(t)\\ \dots \\ B_{n,n}(t)\end{array}\right]$$

特别地，三次贝塞尔曲线可以写成如下的矩阵表达式：
$$\begin{gathered} B(t)=\left[\begin{array}{cccc}{p}_0& p_1& p_2& p_3\end{array}\right]\left[\begin{array}{cccc}1& -3& 3& -1\\ 0& 3& -6& 3\\ 0& 0& 3& -3\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}1\\ t\\ t^2\\ t^3\end{array}\right] \\ =G{M}_{B}u(t) \end{gathered}$$
其中，由控制点形成的矩阵$G$称为几何矩阵，多项式系数矩阵$M_B$称为贝塞尔基矩阵。其它的三次曲线，如三次B样条曲线、三次埃尔米特曲线，都可以通过该矩阵形式表示，只要把基矩阵替换为对应的基即可。

贝塞尔曲线的递归性

一次贝塞尔曲线，可以表示为两个“零次贝塞尔曲线”（其实就是两个点）的凸组合：
$$F_0(t)=t{p}_0+(1-t)p_1$$
二次贝塞尔曲线，可以表示为两个一次贝塞尔曲线的凸组合：
$$\begin{gathered} S_0(t)=t(t{p}_0+(1-t)p_1)+(1-t)(t{p}_1+(1-t)p_2) \\ =t^2{p}_0+2t(1-t)p_1+(1-t{)}^2{p}_2 \\ =(1-t{)}^2{p}_0+2t(1-t)p_1+t^2{p}_2 \\ =(1-t)F_0(t)+t{F}_1(t) \end{gathered}$$

三次贝塞尔曲线，可以表示为两个二次贝塞尔曲线的凸组合：
$$T_0(t)=(1-t)S_0(t)+t{S}_1(t)$$

因此，任何高次贝塞尔曲线，最终都可以从一次贝塞尔曲线递归获得。这也是各大博客里下面这张图背后的数学原理。

贝塞尔曲线的连接

当贝塞尔曲线次数增大时，控制点对曲线局部位置形状的影响就变的很小（每次递归时，控制点前乘的系数都小于1）。为了更好的控制曲线形状，同时降低计算量，通常使用分段三次贝塞尔曲线连接来替代高次贝塞尔曲线。

已经贝塞尔曲线必然通过起始和终止控制点，如果直接将两个三次贝塞尔曲线的起点与终点拼接，得到的曲线会出现尖点，如下右图所示。

为了保证运动的平滑性，连接贝塞尔曲线时，需要保证第一条曲线在连接点处的切线与第二条曲线的切线方向相同：
$$\begin{gathered} B_{first}(1)=B_{second}(0) \\ {B}_{first}^{\prime }(1)\Vert B_{second}^{\prime }(0) \\ or{B}_{first}^{\prime }(1)=B_{second}^{\prime }(0) \end{gathered}$$

贝塞尔曲线的速度

贝塞尔曲线$B(t)$并不是匀速运动的，其速度和加速度可由一阶导和二阶导获得。

代码示例1：普通贝塞尔

我写了一个基本表达式的高次贝塞尔曲线python代码示例，带有一个控制点和曲线的matplotlib可视化：

import numpy as np
import matplotlib.pyplot as plt
import math
import time


def bezier_base(n, i, t, pt):
    coeff = math.factorial(n) / (math.factorial(i)*math.factorial(n-i)) * np.power(1-t, n-i) * np.power(t, i)
    base = coeff * pt

    return base


def bezier_point(t, control_points):
    n = len(control_points) - 1
    bezier_pt = np.array([0, 0], dtype=np.float64)
    for i in range(n + 1):
        bezier_pt += bezier_base(n, i, t, control_points[i])

    return bezier_pt


def visualize(bezier_line, bezier_control_points):
    plt.figure()
    plt.plot(bezier_line[:, 0], bezier_line[:, 1], color='red')
    plt.scatter(bezier_control_points[:, 0], bezier_control_points[:, 1])
    plt.show()


if __name__ == '__main__':
    control_points = np.array([(1, 2), (4, 3), (6, 1)])
    bezier_line = []
    start = time.time()
    for t in np.arange(0, 1.01, 0.01):
        bezier_line.append(bezier_point(t, control_points))
    end = time.time()
    print('time cost: {}'.format(end - start))
    bezier_lines = np.stack(bezier_line, axis=0)
    visualize(bezier_lines, control_points)

代码示例2：递归贝塞尔

import numpy as np
import matplotlib.pyplot as plt
import math
import time


def visualize(bezier_line, bezier_control_points):
    plt.figure()
    plt.plot(bezier_line[:, 0], bezier_line[:, 1], color='red')
    plt.scatter(bezier_control_points[:, 0], bezier_control_points[:, 1])
    plt.show()


def recursive_bezier(pts, t):
    while True:
        recursive_pts = np.empty(shape=(0, 2))
        for i in np.arange(0, pts.shape[0] - 1):
            pt = t * pts[i] + (1 - t) * pts[i+1]
            recursive_pts = np.append(recursive_pts, np.expand_dims(pt, axis=0), axis=0)

        pts = recursive_pts
        if len(recursive_pts) == 1:
            print(recursive_pts)
            break

    return recursive_pts[0]


if __name__ == '__main__':
    control_points = np.array([(1, 2), (4, 4), (6, 3), (7, 2)])
    recursive_bezier_line = []

    for t in np.arange(0, 1.01, 0.01):
        recursive_bezier_line.append(recursive_bezier(control_points, t))
    bezier_lines = np.stack(recursive_bezier_line, axis=0)
    visualize(bezier_lines, control_points)

后记

下篇学习记录B样条曲线