Bézier 曲线：一步一步用控制点生成和检查轨迹

一架无人机要从障碍物左侧飞到右侧。规划器给出几个控制点，用它们生成一条向侧面绕行的曲线，再安排运动时间，检查速度、加速度和整段避障要求。

Bézier 曲线用空间中的控制点表示多项式。 调整控制点可以改变曲线，控制点之间的几何关系也可以用来表达运动条件与安全区域约束。

下面沿着同一个例子，依次计算曲线位置、控制点移动后的变化、时间轨迹，以及整段曲线的避障范围。

一、先把绕障问题画出来

在固定高度的水平面内，用 $(x,y)$ 表示无人机中心的位置，坐标单位为米。地图范围为 $-1\le x\le9$ 、 $-1\le y\le5$ 。

起点与终点为：

$S=(0,0),\qquad G=(8,0).$

中间的矩形禁行区域为：

$3\le x\le5,\qquad 0\le y\le2.$

这片区域已经计入机体尺寸和预留间距，中心点接触它的边界也计为碰撞。图中的向上表示水平面内的 $+y$ 方向，飞行高度保持不变。

先给出一组候选控制点：

控制点	坐标（米）	在本段曲线中的作用
$P_0$	$(0,0)$	曲线起点
$P_1$	$(2,4)$	调整起点附近的形状与离开方向
$P_2$	$(6,4)$	调整终点附近的形状与进入方向
$P_3$	$(8,0)$	曲线终点

按顺序连接这些点，得到控制多边形。它是描述曲线的辅助折线。

四个控制点、控制多边形、候选曲线和中间的矩形禁行区

接下来计算实线上的点怎样由这四个控制点得到。本文构造的是飞行中的一段转弯运动，曲线两端的速度将在分配时间后计算。

二、连续取点：先算出曲线上的一个位置

1. 从两点之间的插值开始

给定两个点 A、B，用一个参数 $s\in[0,1]$ 表示沿它们的连线前进的比例：

$L(s)=(1-s)A+sB.$

例如，从 $(0,0)$ 到 $(2,4)$ ，取 $s=0.25$ ：

$L(0.25)=0.75(0,0)+0.25(2,4)=(0.5,1).$

这表示横向走了 0.5 米、纵向走了 1 米，正好完成整段位移的四分之一。

2. 在四个控制点之间重复这个操作

先取 $s=0.5$ ，每次插值都变成取中点。

第一层：在相邻控制点之间取中点。

$\begin{aligned} Q_0&=\frac{P_0+P_1}{2}=(1,2),\\ Q_1&=\frac{P_1+P_2}{2}=(4,4),\\ Q_2&=\frac{P_2+P_3}{2}=(7,2). \end{aligned}$

第二层：在刚得到的三个点之间取中点。

$\begin{aligned} H_0&=\frac{Q_0+Q_1}{2}=(2.5,3),\\ H_1&=\frac{Q_1+Q_2}{2}=(5.5,3). \end{aligned}$

第三层：对最后两个点再取一次中点。

$\boxed{B(0.5)=\frac{H_0+H_1}{2}=(4,3)}.$

在 s 等于 0.5 时，四个控制点经过三层插值，得到曲线点 (4,3)

每做一层相邻点插值，点数减少一个：4 个控制点 → 3 个中间点 → 2 个中间点 → 1 个曲线点。这种逐层插值的方法称为 de Casteljau 算法。

3. 换一个参数，再算一次

取 $s=0.25$ ，每一层都使用“前一个点乘 0.75，后一个点乘 0.25”。

层次	本轮得到的点
第一层	$(0.5,1)$ 、 $(3,4)$ 、 $(6.5,3)$
第二层	$(1.125,1.75)$ 、 $(3.875,3.75)$
第三层	$(1.8125,2.25)$

例如，第二层的第一个点为：

$0.75(0.5,1)+0.25(3,4)=(1.125,1.75).$

最后得到：

$\boxed{B(0.25)=(1.8125,2.25)}.$

每次给定一个 $s$ ，重复相同的插值过程，就得到曲线上的一个位置。让 $s$ 从 0 连续变化到 1，这些位置便形成整条曲线。

三、把插值过程整理成一个公式

1. 四个控制点分别获得多大权重？

将三层插值中的表达式依次代入并合并，得到：

$\boxed{ B(s)=(1-s)^3P_0+3(1-s)^2sP_1 +3(1-s)s^2P_2+s^3P_3 }.$

四个权重都是三次多项式，这就是三次 Bézier 曲线。这些权重称为三次 Bernstein 基函数。

例如， $s=0.5$ 时，四个权重依次为：

$\frac18,\quad\frac38,\quad\frac38,\quad\frac18.$

于是：

$B(0.5)=\frac18(0,0)+\frac38(2,4) +\frac38(6,4)+\frac18(8,0)=(4,3).$

结果与逐层取中点一致。

代入两端的参数还可以直接得到：

$B(0)=P_0,\qquad B(1)=P_3.$

曲线经过首尾控制点；内部控制点通过权重影响形状。本例中，内部控制点的 $y$ 坐标为 4，而曲线最高只达到 $y=3$ 。

2. 与普通多项式怎样对应？

把本例的四个坐标代进去，分别整理横纵坐标：

$\begin{aligned} x(s) &=6(1-s)^2s+18(1-s)s^2+8s^3\\ &=6s+6s^2-4s^3,\\[4pt] y(s) &=12(1-s)^2s+12(1-s)s^2\\ &=12s-12s^2. \end{aligned}$

所以，同一条曲线也可以写成：

$\boxed{B(s)=\bigl(6s+6s^2-4s^3,\ 12s-12s^2\bigr)}.$

参数 $s$	曲线位置 $(x,y)$ （米）
0	$(0,0)$
0.25	$(1.8125,2.25)$
0.5	$(4,3)$
0.75	$(6.1875,2.25)$
1	$(8,0)$

控制点表示与幂次系数表示描述的是同一条多项式曲线。前者便于观察几何关系，后者便于直接展开计算。五次、七次多项式也可以采用对应的 Bézier 表示。

四、移动一个控制点，会改变哪些位置？

保持另外三个控制点不动，把 $P_1$ 从 $(2,4)$ 下移到 $(2,1.5)$ ：

$\Delta P_1=(0,-2.5).$

从曲线公式可以直接看出，对应位置的变化为：

$\boxed{\Delta B(s)=3(1-s)^2s\,\Delta P_1}.$

例如，在 $s=0.5$ 时， $P_1$ 的权重为 $3/8$ 。曲线点下降：

$2.5\times\frac38=0.9375\text{ 米}.$

它从原来的 $(4,3)$ 变成：

$B_{\mathrm{new}}(0.5)=(4,2.0625).$

再检查 $s=0.4$ 。此时权重为：

$3\times0.6^2\times0.4=0.432.$

原曲线位置为：

$B(0.4)=(3.104,2.88).$

新位置因此变为：

$\begin{aligned} B_{\mathrm{new}}(0.4) &=(3.104,2.88)+0.432(0,-2.5)\\ &=\boxed{(3.104,1.8)}. \end{aligned}$

这个点位于禁行区 $3\le x\le5$ 、 $0\le y\le2$ 内，说明本次修改产生了碰撞，应当拒绝这条候选曲线。

下移 P1 后，曲线在 s 等于 0.4 时进入禁行区域；其四个控制点仍在禁行区之外

这次计算同时说明两件事：控制点位移会按对应权重传递到曲线；轨迹的碰撞情况需要根据整段曲线判断。本次修改后的四个控制点仍位于禁行区外，曲线中间已经碰到了障碍。

对于这一段三次曲线， $3(1-s)^2s$ 在 $0<s<1$ 内都大于零，因此移动 $P_1$ 会影响整段内部曲线，两端位置保持不变。

五、加入时间，计算速度与加速度

1. 用 8 秒完成原来的曲线

下面继续使用原来的四个控制点。将总时间设为 $T=8$ 秒，采用均匀变化的参数：

$s=\frac tT,\qquad p(t)=B(t/T).$

第 2 秒对应 $s=0.25$ ，第 4 秒对应 $s=0.5$ 。这里均匀增加的是参数，运动速度由曲线导数决定。

对上节的坐标公式求导：

$B'(s)=(6+12s-12s^2,\ 12-24s),$

$B''(s)=(12-24s,\ -24).$

换成实际时间导数：

$\boxed{v(t)=\frac{B'(s)}T,\qquad a(t)=\frac{B''(s)}{T^2}}.$

例如，第 4 秒的 $s=0.5$ ：

$p(4)=(4,3),$

$v(4)=\frac18(9,0)=(1.125,0)\,\mathrm{m/s},$

$a(4)=\frac1{64}(0,-24)=(0,-0.375)\,\mathrm{m/s^2}.$

此时无人机位于绕行曲线最高的 $y$ 坐标处，速度沿 $+x$ 方向，加速度正在使运动方向转向右下方。

2. 端点控制边怎样决定端点速度？

将 Bézier 公式求导，在两端得到：

$v(0)=\frac3T(P_1-P_0),\qquad v(T)=\frac3T(P_3-P_2).$

本例使用 $T=8$ 秒：

$v(0)=\frac38(2,4)=(0.75,1.5)\,\mathrm{m/s},$

$v(8)=\frac38(2,-4)=(0.75,-1.5)\,\mathrm{m/s}.$

它描述一段以非零速度进入、以非零速度离开的转弯运动。与相邻轨迹连接时，需要匹配这些端点状态。

反过来，若任务给定起终速度 $v_0,v_f$ ，就可以用它们确定两个内部控制点：

$\boxed{P_1=P_0+\frac T3v_0,\qquad P_2=P_3-\frac T3v_f}.$

例如，给定 $v_0=(0.75,1.5)$ 、 $T=8$ ，起点控制点便是：

$P_1=(0,0)+\frac83(0.75,1.5)=(2,4).$

边界速度要求可以直接转化为控制点的位置关系。

3. 用有限个向量检查整段运动上限

设本例要求：

$\|v(t)\|\le2\,\mathrm{m/s},\qquad \|a(t)\|\le1\,\mathrm{m/s^2}.$

这里 $\|v\|=\sqrt{v_x^2+v_y^2}$ 表示速度向量的大小。

将速度公式整理成：

$v(s)=(1-s)^2V_0+2s(1-s)V_1+s^2V_2,$

其中三个固定向量为：

$V_0=\frac3T(P_1-P_0),\quad V_1=\frac3T(P_2-P_1),\quad V_2=\frac3T(P_3-P_2).$

它们称为速度曲线的控制向量。取 $T=8$ ：

控制向量	数值（米/秒）	大小（米/秒）
$V_0$	$(0.75,1.5)$	约 1.6771
$V_1$	$(1.5,0)$	1.5000
$V_2$	$(0.75,-1.5)$	约 1.6771

速度是这三个向量的加权平均，权重非负、和为 1。因此：

$\|v(s)\|\le\max(\|V_0\|,\|V_1\|,\|V_2\|)<2.$

具体地说，先对三个向量的大小按权重求和，结果至多等于其中的最大值；加权向量本身的大小又不超过这个和。这个界覆盖了整个参数区间。

加速度采用同样的思路：

$a(s)=(1-s)A_0+sA_1,$

$\begin{aligned} A_0&=\frac6{T^2}(P_2-2P_1+P_0)=(0.1875,-0.375),\\ A_1&=\frac6{T^2}(P_3-2P_2+P_1)=(-0.1875,-0.375). \end{aligned}$

两个向量的大小都约为 $0.4193\,\mathrm{m/s^2}$ ，所以整段加速度大小也不超过这个值。

这给出一组充分条件：导数控制向量都满足大小限制，就能保证对应曲线满足限制。某个控制向量超过上限时，仍需进一步检查实际曲线；控制向量提供的界可能偏保守。

保持控制点不变，对照两种时间安排：

总时间	速度大小上界（米/秒）	加速度大小上界（米/秒²）	检查结果
4 秒	3.3541	1.6771	两项都在端点超过限制
8 秒	1.6771	0.4193	两项全程满足

本例这两个上界恰好都在端点取到。

同一曲线用 4 秒和 8 秒完成时的速度大小；8 秒安排全程低于每秒 2 米

同一曲线用 4 秒和 8 秒完成时的加速度大小；8 秒安排全程低于每二次方秒 1 米

延长时间保留了几何路线，同时降低速度与加速度需求。这里完成的是给定参考曲线的速度、加速度检查，推力与姿态等飞行要求可以继续由运动导数计算。

六、用控制点凸包证明整段绕障

1. 加权平均把曲线限制在哪里？

位置公式中的四个权重都非负，而且：

$(1-s)^3+3(1-s)^2s+3(1-s)s^2+s^3=1.$

因此，曲线上的点是控制点的凸组合。

所有这类组合构成控制点的凸包。在平面中，可以把它想象成橡皮筋围住所有控制点后形成的区域。逐层插值中的每条线段都留在这片区域里，最后得到的曲线点也在其中。

如果一段曲线的所有控制点都位于同一个凸自由区域内，整段曲线就会留在这个区域内。

这里的凸区域，指其中任意两点之间的线段也完全位于该区域中。三角形、矩形，以及若干半平面的交集，都具有这种性质。

2. 把同一条曲线分成两个更小的凸包

本例四个原控制点围成的区域很大，其中包含矩形禁行区。为了进一步判断，可以在 $s=0.5$ 处分割曲线，让前后两半分别对应一组更贴近曲线的控制点。

第二节的插值过程已经提供了这些点：

左半段控制点	坐标	右半段控制点	坐标
$U_0=P_0$	$(0,0)$	$W_0=B(0.5)$	$(4,3)$
$U_1=Q_0$	$(1,2)$	$W_1=H_1$	$(5.5,3)$
$U_2=H_0$	$(2.5,3)$	$W_2=Q_2$	$(7,2)$
$U_3=B(0.5)$	$(4,3)$	$W_3=P_3$	$(8,0)$

用左边四个点生成 $B_L(u)$ ，用右边四个点生成 $B_R(u)$ ，各自的参数 $u$ 都从 0 到 1。把这些坐标代回三次 Bézier 公式，可以核对：

$\boxed{B_L(u)=B(u/2),\qquad B_R(u)=B((1+u)/2)}.$

它们分别覆盖原曲线的前半段与后半段，合起来仍是原来的曲线。分割过程保留了所有曲线位置。

3. 检查左半段对应的安全区域

给左半段选定区域：

$\mathcal F_L=\left\{(x,y)\ \middle|\ 0\le x\le4,\quad \frac34x\le y\le4\right\}.$

它由直线边界围成，是一个凸区域。四个左侧控制点都满足范围要求；再检查下边界：

控制点	$y-\tfrac34x$
$(0,0)$	0
$(1,2)$	1.25
$(2.5,3)$	1.125
$(4,3)$	0

全部大于或等于零，所以左半段控制点都在 $\mathcal F_L$ 内。

这个区域与障碍是否分开，也能直接计算。当它进入障碍的横向范围时， $3\le x\le4$ ，因此区域中的点满足：

$y\ge\frac34x\ge\frac34\times3=2.25.$

障碍最高到 $y=2$ ，左半段的整个凸区域都位于其上方。

4. 对右半段做相同检查

右半段选取：

$\mathcal F_R=\left\{(x,y)\ \middle|\ 4\le x\le8,\quad \frac34(8-x)\le y\le4\right\}.$

四个右侧控制点同样满足这些不等式。进入障碍横向范围时， $4\le x\le5$ ，所以：

$y\ge\frac34(8-x)\ge\frac34\times3=2.25>2.$

右半段的整个区域也与障碍分离。

将同一条曲线在中间分割后，两组控制点分别位于左右凸自由区域内，两个区域均避开矩形障碍

至此，避障结论来自一条完整的包含关系：左半段控制点全部在左侧凸自由区域内，所以左半段曲线也全部在该区域内；右半段同理。两个区域都在地图内并与禁行区分离，因此两段曲线合起来全程无碰撞。这里用有限个控制点的不等式，验证了整个连续区间。

七、分割后，速度与加速度怎样接起来？

原曲线用 8 秒完成。按中点分割后，前后两段各分配 4 秒，局部参数分别为：

$u_L=\frac t4,\qquad u_R=\frac{t-4}{4}.$

这样，第 0～4 秒沿左半段运动，第 4～8 秒沿右半段运动，时间轨迹与分割前保持一致。

首先，两段位置都接到 $(4,3)$ ：

$U_3=W_0=(4,3).$

再检查速度。左半段末端：

$v_L=\frac34(U_3-U_2) =\frac34(1.5,0)=(1.125,0).$

右半段起点：

$v_R=\frac34(W_1-W_0) =\frac34(1.5,0)=(1.125,0).$

加速度也能通过相邻三个控制点计算：

$\begin{aligned} a_L &=\frac6{4^2}(U_3-2U_2+U_1)\\ &=\frac6{16}(0,-1)=(0,-0.375),\\[4pt] a_R &=\frac6{4^2}(W_2-2W_1+W_0)\\ &=\frac6{16}(0,-1)=(0,-0.375). \end{aligned}$

因此，连接处的位置、速度和加速度一致，与第五节直接计算原曲线得到的结果相同。

若两段三次曲线分别持续 $T_L,T_R$ ，速度衔接条件写成：

$\boxed{\frac3{T_L}(U_3-U_2)=\frac3{T_R}(W_1-W_0)}.$

控制边的方向和长度，与对应的分段时间共同决定连接处的实际速度。修改某一段时间后，需要重新核验这些关系。

八、把这个例子整理成轨迹设计过程

本例最终使用原来的四个控制点、8 秒的时间安排，并通过中点分割完成避障验证。

处理环节	本例的具体结果
计算曲线	三层插值或 Bernstein 权重，得到同一条三次曲线
调整形状	下移 $P_1$ 后出现碰撞，该候选被拒绝
安排时间	8 秒方案满足速度和加速度上限
验证避障	两组子段控制点分别位于两个凸自由区域内
检查衔接	第 4 秒两侧位置、速度和加速度相同