B 样条：一步一步看懂控制点的局部作用

一架无人机沿通道绕过障碍。规划器希望把中间一段路线向侧面调整，同时保留进入通道和离开通道时的运动。

B 样条通过相邻曲线段共享控制点，形成一条连续曲线。每个控制点只影响一定范围内的曲线段。

下面用九个控制点生成六段曲线，再移动其中一个点，逐步计算位置怎样变化、各段怎样衔接，以及调整后的运动能否满足约束。

一、九个控制点，怎样组成六段曲线？

1. 确定地图与控制点

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

中间的矩形禁行区域为：

$$5\le x\le7,\qquad 0\le y\le2.$$

它已经计入机体尺寸与预留间距，接触边界也计为碰撞。图中的 $+y$ 表示水平面内的侧向。

给出以下控制点：

控制点	坐标（米）	控制点	坐标（米）	控制点	坐标（米）
$P_0$	$(-2,0)$	$P_3$	$(4,3)$	$P_6$	$(10,0)$
$P_1$	$(0,0)$	$P_4$	$(6,3)$	$P_7$	$(12,0)$
$P_2$	$(2,0)$	$P_5$	$(8,3)$	$P_8$	$(14,0)$

按编号连接这些点，得到控制多边形。中间三个点位于障碍上方，会把曲线向 $+y$ 方向抬起。

本文使用三次均匀 B 样条。三次表示每一段都是最高三次的多项式；本例采用等间隔、没有重复的节点刻度。

2. 每次使用相邻四个点，然后向后移一位

六段曲线分别记为 $C_0$ 到 $C_5$：

曲线段	使用的控制点	整体参数范围
$C_0$	$P_0,P_1,P_2,P_3$	$0\le u\le1$
$C_1$	$P_1,P_2,P_3,P_4$	$1\le u\le2$
$C_2$	$P_2,P_3,P_4,P_5$	$2\le u\le3$
$C_3$	$P_3,P_4,P_5,P_6$	$3\le u\le4$
$C_4$	$P_4,P_5,P_6,P_7$	$4\le u\le5$
$C_5$	$P_5,P_6,P_7,P_8$	$5\le u\le6$

这里用 $u$ 记录整条曲线的进度。整数刻度将参数划成六个区间，每个区间对应一段，这些分界刻度称为节点。

每一段再使用自己的局部参数：

$$s=u-i,\qquad 0\le s\le1.$$

例如，$u=1.5$ 位于 $C_1$，该段已经走到 $s=0.5$；$u=2.5$ 位于 $C_2$，也对应本段的 $s=0.5$。

相邻两段共享三个控制点。 从 $C_1$ 移到 $C_2$ 时，移出 $P_1$、加入 $P_5$，中间的 $P_2,P_3,P_4$ 继续使用。

二、先用熟悉的 Bézier 方法，算出一段曲线

1. 将四个 B 样条控制点组合成四个 Bézier 控制点

对于 $C_i$，使用 $P_i,P_{i+1},P_{i+2},P_{i+3}$，按下面的规则生成一组局部 Bézier 控制点：

$$\begin{aligned} Q_0&=\frac{P_i+4P_{i+1}+P_{i+2}}6,\\ Q_1&=\frac{2P_{i+1}+P_{i+2}}3,\\ Q_2&=\frac{P_{i+1}+2P_{i+2}}3,\\ Q_3&=\frac{P_{i+1}+4P_{i+2}+P_{i+3}}6. \end{aligned}$$

这是一种直接构造本例三次均匀 B 样条的方法：每组选出四个局部 Bézier 控制点，生成一段三次曲线，再向后移一位重复。 后面会逐项核对各段的位置、速度与加速度衔接。

中间两个点有直观的几何含义：$Q_1$ 位于 $P_{i+1}\rightarrow P_{i+2}$ 的三分之一处，$Q_2$ 位于三分之二处。

看第二段 $C_1$，它使用：

$$P_1=(0,0),\quad P_2=(2,0),\quad P_3=(4,3),\quad P_4=(6,3).$$

代入上面的组合规则：

局部 Bézier 控制点	计算	结果
$Q_0$	$[(0,0)+4(2,0)+(4,3)]/6$	$(2,\tfrac12)$
$Q_1$	$[2(2,0)+(4,3)]/3$	$(\tfrac83,1)$
$Q_2$	$[(2,0)+2(4,3)]/3$	$(\tfrac{10}3,2)$
$Q_3$	$[(2,0)+4(4,3)+(6,3)]/6$	$(4,\tfrac52)$

这四个 $Q$ 描述 $C_1$ 的同一条曲线。其中，$Q_0$ 是本段实际起点，$Q_3$ 是本段实际终点。

2. 用它们算出本段中间的位置

三次 Bézier 在 $s=0.5$ 时的权重为 $1/8,3/8,3/8,1/8$，所以：

$$C_1(0.5)=\frac18Q_0+\frac38Q_1+\frac38Q_2+\frac18Q_3.$$

横坐标为：

$$x=\frac18\times2+\frac38\times\frac83 +\frac38\times\frac{10}3+\frac18\times4=3.$$

纵坐标为：

$$y=\frac18\times\frac12+\frac38\times1 +\frac38\times2+\frac18\times\frac52=1.5.$$

因此：

$$\boxed{C_1(0.5)=(3,1.5)}.$$

3. 合并两次加权，得到 B 样条的局部公式

将四个 $Q$ 的表达式代回 Bézier 公式，再按原控制点收集各项，得到：

$$\boxed{ C_i(s)=b_0(s)P_i+b_1(s)P_{i+1} +b_2(s)P_{i+2}+b_3(s)P_{i+3} },$$

其中：

$$\begin{aligned} b_0(s)&=\frac{(1-s)^3}{6},\\ b_1(s)&=\frac{3s^3-6s^2+4}{6},\\ b_2(s)&=\frac{-3s^3+3s^2+3s+1}{6},\\ b_3(s)&=\frac{s^3}{6}. \end{aligned}$$

这些权重是三次均匀 B 样条基函数在本段上的表达式。

例如，$P_i$ 只出现在 $Q_0$ 中，占它的 $1/6$；$Q_0$ 在 Bézier 公式中的权重是 $(1-s)^3$。两者相乘，就得到 $b_0(s)$。

$P_{i+1}$ 同时出现在四个 $Q$ 中，把对应贡献相加：

$$\begin{aligned} b_1(s) &=\frac46(1-s)^3 +2(1-s)^2s+(1-s)s^2+\frac16s^3\\ &=\frac{4-6s^2+3s^3}{6}. \end{aligned}$$

其余两项采用相同的收集方式。

在 $s=0.5$ 时，四个权重变成：

$$\frac1{48},\quad\frac{23}{48},\quad\frac{23}{48},\quad\frac1{48}.$$

直接使用原控制点也能算出：

$$C_1(0.5)=\frac1{48}(0,0)+\frac{23}{48}(2,0) +\frac{23}{48}(4,3)+\frac1{48}(6,3)=(3,1.5).$$

两套计算得到同一个位置：局部 Bézier 控制点与 B 样条控制点，是这段曲线的两种表示。

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

1. 把中间控制点向上移动 1.5 米

将 $P_4$ 从 $(6,3)$ 改成 $(6,4.5)$，其他控制点和节点刻度保持不变：

$$\Delta P_4=(0,1.5).$$

回看第一节的分组表，只有 $C_1,C_2,C_3,C_4$ 包含 $P_4$。

曲线段	$P_4$ 在本段获得的权重	位置变化
$C_0$	0	保持原样
$C_1$	$b_3(s)$	$b_3(s)(0,1.5)$
$C_2$	$b_2(s)$	$b_2(s)(0,1.5)$
$C_3$	$b_1(s)$	$b_1(s)(0,1.5)$
$C_4$	$b_0(s)$	$b_0(s)(0,1.5)$
$C_5$	0	保持原样

这就是本例的局部支撑：$P_4$ 只在整体参数 $1<u<5$ 内产生位置影响，其余区间的权重为零。在 $u=1$ 和 $u=5$，这个权重也正好为零。

2. 同样移动 1.5 米，各处会移动多少？

在第二段 $C_1$ 的中间，$s=0.5$，$P_4$ 获得 $b_3=1/48$：

$$\Delta y=1.5\times\frac1{48}=0.03125\text{ 米}.$$

原位置 $(3,1.5)$ 变成：

$$\boxed{(3,1.53125)}.$$

在第三段 $C_2$ 的中间，$P_4$ 获得 $b_2=23/48$：

$$\Delta y=1.5\times\frac{23}{48}=0.71875\text{ 米}.$$

原位置为 $(5,2.9375)$，调整后变为：

$$\boxed{(5,3.65625)}.$$

再看整条曲线正中间的 $u=3$。这里是 $C_2$ 的末端，也是 $C_3$ 的起点，$P_4$ 在两侧的权重都为 $4/6$：

$$\Delta y=1.5\times\frac46=1\text{ 米}.$$

因此，原来的 $(6,3)$ 变成 $(6,4)$。

控制点上移 1.5 米，各个曲线位置按自己的权重移动。 靠近影响区间两端时变化较小，中间的变化较大。

给每段安排 2 秒，六段共 12 秒。下面按实际时间列出结果：

总时间（秒）	所在位置的 $x$（米）	原 $y$（米）	新 $y$（米）	上移距离（米）
1	1	0.06250	0.06250	0
3	3	1.50000	1.53125	0.03125
5	5	2.93750	3.65625	0.71875
6	6	3.00000	4.00000	1.00000
7	7	2.93750	3.65625	0.71875
9	9	1.50000	1.53125	0.03125
11	11	0.06250	0.06250	0

这里保持了其他所有控制点与节点不变。优化时若同时移动多个控制点，受影响的范围就是它们各自影响区间的并集。

四、相邻两段的位置、速度和加速度怎样接上？

1. 先把两段写成普通多项式

继续使用原控制点。将坐标代入局部 B 样条公式并展开，第二段为：

$$\boxed{ C_1(s)=\left(2+2s,\ \frac12+\frac32s+\frac32s^2-s^3\right) }.$$

下一段为：

$$\boxed{ C_2(s)=\left(4+2s,\ \frac52+\frac32s-\frac32s^2+\frac12s^3\right) }.$$

两式中的 $s$ 各自从 0 到 1。$C_1$ 结束时取 $s=1$，$C_2$ 开始时取 $s=0$。

2. 从参数导数换成时间导数

每段时间记为 $h$，本例 $h=2$ 秒。$C_i$ 从总时间 $ih$ 开始，因此：

$$s=\frac{t-ih}{h},\qquad \frac{ds}{dt}=\frac1h.$$

实际位置为 $p(t)=C_i(s)$。用链式法则：

$$v(t)=\frac{dp}{dt} =\frac{dC_i}{ds}\frac{ds}{dt} =\frac{C_i'(s)}h.$$

再求一次时间导数，$1/h$ 是常数，$C_i'$ 求导时又产生一个 $1/h$：

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

本例每次求时间导数，分别引入 $1/2$，于是速度除以 2，加速度除以 4。

例如，第 3 秒位于 $C_1$，本段从第 2 秒开始：

$$s=\frac{3-2}{2}=0.5.$$

对 $C_1$ 的横纵坐标逐项求导：

$$C_1'(s)=\left(2,\ \frac32+3s-3s^2\right),$$

$$C_1''(s)=(0,\ 3-6s).$$

代入 $s=0.5$，得到这个时刻的完整运动：

$$p(3)=(3,1.5),$$

$$v(3)=\frac12(2,2.25)=(1,1.125)\,\mathrm{m/s},$$

$$a(3)=\frac14(0,0)=(0,0)\,\mathrm{m/s^2}.$$

3. 在第 4 秒，把左侧与右侧放在一起比较

对 $C_2$ 同样求导：

$$C_2'(s)=\left(2,\ \frac32-3s+\frac32s^2\right),$$

$$C_2''(s)=(0,\ -3+3s).$$

第 4 秒恰好是两段的连接处：

运动量	左侧：$C_1$ 取 $s=1$	右侧：$C_2$ 取 $s=0$
位置	$(4,2.5)$	$(4,2.5)$
速度	$(2,1.5)/2=(1,0.75)$	$(2,1.5)/2=(1,0.75)$
加速度	$(0,-3)/4=(0,-0.75)$	$(0,-3)/4=(0,-0.75)$

位置和前两阶时间导数在连接处一致，这就是 $C^2$ 连续。

这套衔接关系也可以对任意相邻段直接核对：

$$\begin{aligned} C_i(1)=C_{i+1}(0) &=\frac{P_{i+1}+4P_{i+2}+P_{i+3}}6,\\ C_i'(1)=C_{i+1}'(0) &=\frac{P_{i+3}-P_{i+1}}2,\\ C_i''(1)=C_{i+1}''(0) &=P_{i+1}-2P_{i+2}+P_{i+3}. \end{aligned}$$

两边使用相同的控制点组合，所以任意修改控制点后，这些等式仍成立。所有段持续时间相同，对时间的导数也相同。

本例在连接处保证到加速度连续。继续求导，第 4 秒左侧 Jerk 为 $(0,-6)/8=(0,-0.75)$，右侧为 $(0,3)/8=(0,0.375)$，允许发生跳变。需要更高阶连续性时，可以选择更高次数并设置对应的节点与边界条件。

五、调整后，整段运动是否仍满足上限？

1. 用相邻控制点的差表示速度

设运动要求为：

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

先对四个局部权重求导：

$$\begin{aligned} b_0'&=-\tfrac12(1-s)^2,& b_1'&=\tfrac32s^2-2s,\\ b_2'&=-\tfrac32s^2+s+\tfrac12,& b_3'&=\tfrac12s^2. \end{aligned}$$

将这些权重乘回控制点，再按相邻点之差分组，得到：

$$\begin{aligned} C_i'(s) &=\frac{(1-s)^2}{2}(P_{i+1}-P_i)\\ &\quad+\left(\frac12+s-s^2\right)(P_{i+2}-P_{i+1})\\ &\quad+\frac{s^2}{2}(P_{i+3}-P_{i+2}). \end{aligned}$$

记：

$$D_j=\frac{P_{j+1}-P_j}{h}.$$

每个 $D_j$ 就是一条相邻控制边除以本段时间。速度写成：

$$v(t)=\frac{(1-s)^2}{2}D_i +\left(\frac12+s-s^2\right)D_{i+1} +\frac{s^2}{2}D_{i+2}.$$

三个权重都非负，相加为 1，所以速度是三个固定向量的凸组合。

例如，原控制点 $P_2=(2,0)$、$P_3=(4,3)$ 产生：

$$D_2=\frac{(4,3)-(2,0)}2=(1,1.5),$$

大小为：

$$\|D_2\|=\sqrt{1^2+1.5^2}=\frac{\sqrt{13}}2\approx1.8028.$$

对全部相邻控制边计算，最大大小就是这个值。$P_4$ 上移后，两条相关控制边的纵向差变成 $+1.5$ 与 $-1.5$ 米，其余控制边保持不变，最大值仍为 $\sqrt{13}/2$。

向量加权后的大小不超过各向量大小的加权和，因此两个方案都满足：

$$\boxed{\|v(t)\|\le\max_j\|D_j\|=\frac{\sqrt{13}}2<2}.$$

2. 再用二阶差表示加速度

对上一节的速度表达式再求导，并换算时间，得到：

$$a(t)=(1-s)E_i+sE_{i+1},$$

其中：

$$\boxed{E_j=\frac{P_j-2P_{j+1}+P_{j+2}}{h^2}}.$$

二阶差可以拆开理解：

$$P_j-2P_{j+1}+P_{j+2} =(P_{j+2}-P_{j+1})-(P_{j+1}-P_j).$$

它记录相邻两条控制边的变化。加速度是相邻两个 $E$ 的线性插值。

例如，使用 $P_1,P_2,P_3$：

$$E_1=\frac{(0,0)-2(2,0)+(4,3)}4=(0,0.75).$$

本例所有控制点的横坐标等距，因此每个 $E_j$ 的横向分量都为 0。分别检查原方案与修改后方案，二阶差的最大纵向绝对值都为 3，除以 $h^2=4$，得到：

$$\boxed{\|a(t)\|\le\max_j\|E_j\|=0.75<1}.$$

检查项目	原方案的全区间上界	修改后的全区间上界	允许值
速度大小（米/秒）	约 1.8028	约 1.8028	2
加速度大小（米/秒²）	0.75	0.75	1

这些是由导数控制向量给出的充分上界，覆盖整个连续区间。速度曲线的实际峰值可以比上界更小。

图中展示了实际变化，凸组合上界提供了整段约束的验证。本次修改改变了部分速度与加速度，但仍满足规定限制。

六、用局部凸包检查整段绕障

1. 每段曲线都有一个局部几何范围

每段的四个权重非负、和为 1，因此：

$$C_i(s)\in\operatorname{conv}(P_i,P_{i+1},P_{i+2},P_{i+3}).$$

也就是说，每一段都位于它所使用的四个控制点的凸包内。 将这四个点限制在同一个凸自由区域中，就能保证该段位于区域内。

还可以使用第二节的转换，得到同一段的四个 Bézier 控制点。这些点本身就是原控制点的凸组合，其凸包通常更加贴近该段曲线。

2. 找出经过障碍横向范围的曲线段

本例横坐标展开后都满足：

$$x_i(s)=2i+2s.$$

因此六段分别覆盖：

曲线段	$C_0$	$C_1$	$C_2$	$C_3$	$C_4$	$C_5$
$x$ 范围	0–2	2–4	4–6	6–8	8–10	10–12

障碍位于 $5\le x\le7$，需要重点检查 $C_2$ 与 $C_3$。前两段完全在障碍左侧，后两段完全在右侧。

原方案的这两段，对应的 Bézier 控制点为：

控制点	$C_2$ 对应的坐标	$C_3$ 对应的坐标
$Q_0$	$(4,2.5)$	$(6,3)$
$Q_1$	$(14/3,3)$	$(20/3,3)$
$Q_2$	$(16/3,3)$	$(22/3,3)$
$Q_3$	$(6,3)$	$(8,2.5)$

这八个点全部位于矩形：

$$\mathcal F=\{(x,y)\mid4\le x\le8,\ 2.5\le y\le4.5\}.$$

$\mathcal F$ 是凸区域，整块区域都在障碍上方：

$$y\ge2.5>2.$$

因此，两段曲线也都留在 $\mathcal F$ 内。

3. 对修改后的控制点再核验一次

上移 $P_4$ 后重新计算这两段的 Bézier 控制点，得到：

控制点	新 $C_2$ 对应的坐标	新 $C_3$ 对应的坐标
$Q_0$	$(4,2.75)$	$(6,4)$
$Q_1$	$(14/3,3.5)$	$(20/3,4)$
$Q_2$	$(16/3,4)$	$(22/3,3.5)$
$Q_3$	$(6,4)$	$(8,2.75)$

它们仍全部位于 $\mathcal F$ 中。其余曲线段的横坐标范围保持不变，仍从障碍左右两侧通过。

所有段的局部 Bézier 控制点也都在地图范围内。因此，本次修改后的完整轨迹满足地图边界与绕障要求。

本次避障验证使用了覆盖整段曲线的凸包范围，约束检查使用了覆盖整段运动的导数上界。

七、把曲线整理成可计算的轨迹

1. 查询任意时刻

本例每段持续 2 秒，整体参数为：

$$u=\frac t2,\qquad 0\le t\le12.$$

先判断 $u$ 落在哪一段，再用 $s=u-i$ 计算：

$$p(t)=C_i(s).$$

例如，第 7 秒：

$$u=3.5,\qquad i=3,\qquad s=0.5.$$

因此查 $C_3(0.5)$。原方案位置为 $(7,2.9375)$，修改后为 $(7,3.65625)$。

在最终时刻 $t=12$，取末段 $C_5$ 的 $s=1$。

若使用通用 B 样条程序，本例的完整节点向量为：

$$U=(-3,-2,-1,0,1,2,3,4,5,6,7,8,9).$$

这 13 个等间隔刻度与 9 个控制点、三次基函数配合使用，有效曲线区间为 $0\le u\le6$。两端各有三个额外刻度，用于定义边界附近的基函数。本文展示的是这种未夹持均匀节点设置下的六段曲线。

另一种常见设置会把首末参数节点各重复四次，使三次 B 样条经过首末控制点，称为端点夹持。它的端部权重随节点设置改变，需要按对应基函数计算。

2. 曲线的真实端点与边界状态

使用本例的权重，轨迹起点为：

$$p(0)=\frac{P_0+4P_1+P_2}{6}=(0,0),$$

终点为：

$$p(12)=\frac{P_6+4P_7+P_8}{6}=(12,0).$$

起点附近的三个控制点还决定起始速度与加速度：

$$v(0)=\frac{P_2-P_0}{2h}=(1,0)\,\mathrm{m/s},$$

$$a(0)=\frac{P_0-2P_1+P_2}{h^2}=(0,0).$$

终点同理，速度为 $(1,0)$ 米/秒，加速度为零。本例描述的是一段以非零速度进入、以非零速度离开的绕障运动。

起终状态由首尾各三个控制点确定。保持这六个点不变，调整中间的 $P_3,P_4,P_5$，可以在保留这些边界状态的同时改变中部形状。

3. 控制点怎样进入轨迹优化？

一次控制点修改可以对应到下面的过程：

这为优化提供了一组有明确几何意义的变量：控制点坐标。局部支撑说明每个变量具体影响哪些段；导数表达式连接运动需求；凸包性质连接安全区域约束。

本例的局部作用建立在固定节点与固定时间安排上。改变节点或各段时间时，需要重新计算对应基函数、时间导数与衔接关系。

配套文件 bspline_example.py 使用 Python 标准库，复算位置、局部 Bézier 转换、控制点修改、衔接关系和全区间约束，并用完整节点向量独立核对基函数。运行：

python bspline_example.py

小结

九个控制点通过相邻四点分组，生成六段三次均匀 B 样条。把一个控制点上移 1.5 米，只改变中间四段；在本例曲线正中间，位置上移 1 米，首尾状态保持不变。

各段共享控制点，并使用统一的组合与时间刻度，使位置、速度和加速度连续。相邻控制点的一阶差与二阶差提供运动上界，局部凸包提供整段曲线的几何范围。

控制点的局部作用、段间连续性和可计算的约束，把曲线形状调整联系成了一个完整的轨迹设计问题。 接下来可以为障碍间距和平滑性写出具体代价，计算控制点应该向哪个方向移动。