B 样条轨迹优化：一步一步计算控制点的移动

一架无人机用 12 秒从障碍物左侧飞到右侧。轨迹由九个 B 样条控制点描述，中间的控制点可以向侧面移动，改变绕行位置和途中运动。

规划器需要比较两方面的需求：让运动变化平缓，以及让轨迹在指定位置离障碍更远。把它们写成代价后，就能计算控制点朝哪个方向移动、移动多少，以及何时停止。

下面保留起终状态和时间安排，只调整一个控制点的侧向坐标，完整算出一次轨迹优化。

一、先确定：这一轮允许调整什么？

1. 地图与初始轨迹

在固定高度的水平面内，用 $(x,y)$ 描述无人机中心，坐标以米计。图中的 $+y$ 是水平面内的侧向。

地图范围为 $-3\le x\le15$、$-1\le y\le6$。矩形禁行区域为：

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

禁行区域已经计入机体尺寸和预留间距，中心点接触边界也计为碰撞。

采用三次均匀 B 样条，控制点为：

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

初始取 $q=3$。每段使用相邻四个控制点，共六段，每段持续 $h=2$ 秒，总时间为 12 秒。

本例沿用等间隔、没有重复的节点设置。完整节点向量为：

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

有效参数区间为 $0\le u\le6$，时间对应 $u=t/2$。

2. 只把 $P_4$ 的侧向坐标作为变量

本次优化固定其余八个控制点、全部横坐标和节点时间，只选择 $P_4=(6,q)$ 中的 $q$。数值计算中的 $q$ 使用以米为单位的坐标数值。

给它规定可选范围：

$$\boxed{3\le q\le5}.$$

这个范围连同固定控制点，定义了本次要比较的轨迹集合。后面将验证，其中每条轨迹都满足本例的几何与运动要求：

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

首尾各三个控制点保持不变，因此所有候选都从 $(0,0)$ 进入、在 $(12,0)$ 离开，起终速度均为 $(1,0)$ 米/秒，加速度均为零。

每选择一个 $q$，就得到一条完整的 12 秒轨迹。 优化的任务是给这些选择评分，再寻找分数较低的那个。

二、控制点怎样影响轨迹与障碍的间距？

1. 先找到一个能够直接计算的位置

第 6 秒对应 $u=3$，正好是中间两段的连接处。根据三次均匀 B 样条的端点权重：

$$p(6)=\frac{P_3+4P_4+P_5}{6}.$$

代入控制点：

$$p(6)=\frac{(4,3)+4(6,q)+(8,3)}6 =\boxed{\left(6,\ 1+\frac23q\right)}.$$

初始 $q=3$ 时，轨迹经过 $(6,3)$。如果 $q$ 增加 0.1，曲线在这个时刻的侧向位置增加：

$$\frac23\times0.1\approx0.06667\text{ 米}.$$

控制点的移动量乘以它在该处的权重，就是曲线点的移动量。 第 6 秒处，$P_4$ 的权重为 $2/3$。

2. 从曲线位置计算障碍距离

第 6 秒的位置始终位于矩形上方，横坐标为 6。最近的障碍边界点是 $(6,2)$，所以额外间距为：

$$\boxed{d(q)=\left(1+\frac23q\right)-2=\frac23q-1}.$$

这里测量的是到已经膨胀的禁行区的距离。机体尺寸已经包含在地图中。

控制点坐标 $q$（米）	第 6 秒的曲线位置	到禁行区的额外间距（米）
3.0	$(6,3)$	1.0
4.5	$(6,4)$	2.0
4.875	$(6,4.25)$	2.25
5.0	$(6,13/3)$	约 2.3333

表中的距离只对应第 6 秒。全程避障将通过各段的凸区域进行验证。

3. 为间距不足设置代价

本例新增一项偏好：希望第 6 秒的额外间距达到 $d_{\mathrm{ref}}=2.25$ 米。将低于这个数的部分平方，得到：

$$\boxed{ J_d(q)=\bigl[\max(0,\ 2.25-d(q))\bigr]^2 }.$$

$\max(0,\cdot)$ 表示取 0 与括号内数值中较大的一个。达到期望间距时，这一项为零；间距较小时，按缺少多少计算代价。

初始 $q=3$，间距为 1 米，还差：

$$2.25-1=1.25\text{ 米}.$$

因此：

$$J_d(3)=1.25^2=1.5625\,\mathrm{m^2}.$$

当 $q<4.875$ 时，这项代价可以写成：

$$\boxed{J_d(q)=\left(\frac{13}{4}-\frac23q\right)^2}.$$

因为 $2.25-(2q/3-1)=3.25-2q/3$。当 $q\ge4.875$ 时，期望间距已经达到，$J_d=0$。

三、再给整段运动的平滑性评分

1. 每一段的 Jerk 怎样算？

三次 B 样条每段的位置是三次多项式，加速度是一次式，Jerk 则在该段内保持常数。

对第 $i$ 段，局部参数为 $s=(t-ih)/h$。加速度可以写成：

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

其中：

$$E_i=\frac{P_i-2P_{i+1}+P_{i+2}}{h^2}.$$

对时间求导时，$ds/dt=1/h$，所以：

$$j_i=\frac{E_{i+1}-E_i}{h}.$$

把两个 $E$ 展开：

$$\begin{aligned} E_{i+1}-E_i &=\frac{P_{i+1}-2P_{i+2}+P_{i+3}}{h^2} -\frac{P_i-2P_{i+1}+P_{i+2}}{h^2}\\ &=\frac{P_{i+3}-3P_{i+2}+3P_{i+1}-P_i}{h^2}. \end{aligned}$$

于是：

$$\boxed{j_i=\frac{P_{i+3}-3P_{i+2}+3P_{i+1}-P_i}{h^3}}.$$

分子是相邻四个控制点的三阶差，记录控制多边形变化的进一步变化。

2. 把六段都列出来

本例各控制点的横坐标等距，横向 Jerk 为零。将侧向坐标代入，并使用 $h^3=8$：

曲线段	使用的控制点	侧向三阶差	侧向 Jerk（米/秒³）
$C_0$	$P_0$ 至 $P_3$	$3$	$3/8$
$C_1$	$P_1$ 至 $P_4$	$q-9$	$(q-9)/8$
$C_2$	$P_2$ 至 $P_5$	$12-3q$	$(12-3q)/8$
$C_3$	$P_3$ 至 $P_6$	$3q-12$	$(3q-12)/8$
$C_4$	$P_4$ 至 $P_7$	$9-q$	$(9-q)/8$
$C_5$	$P_5$ 至 $P_8$	$-3$	$-3/8$

例如，$C_1$ 的侧向三阶差为：

$$q-3\times3+3\times0-0=q-9.$$

$C_2$ 则为：

$$3-3q+3\times3-0=12-3q.$$

只有中间四段含有 $q$，首尾两段的运动始终不变。

3. 平方后，按各段持续时间相加

使用整段 Jerk 平方积分作为平滑代价：

$$J_s=\int_0^{12}\|j(t)\|^2dt.$$

本例位置、速度、加速度连续，各段交界处允许 Jerk 发生有限跳变。每段内的 Jerk 为常数，持续 2 秒，因此积分可以直接写成：

$$J_s(q)=\frac{2}{8^2} \left[3^2+(q-9)^2+(12-3q)^2 +(3q-12)^2+(9-q)^2+(-3)^2\right].$$

前面的 $2$ 来自持续时间，$8^2$ 来自 Jerk 中分母的平方。把相同平方项合并：

$$J_s(q)=\frac1{32}\left[18+2(q-9)^2+2(12-3q)^2\right].$$

展开得到：

$$\boxed{J_s(q)=\frac58q^2-\frac{45}{8}q+\frac{117}{8}}.$$

这里的代价数值按 $\mathrm{m^2/s^5}$ 计。

代入初始 $q=3$，六段 Jerk 分别为：

$$\frac38,\quad-\frac68,\quad\frac38,\quad-\frac38,\quad\frac68,\quad-\frac38.$$

平方、乘以各段的 2 秒、再求和：

$$J_s(3)=2\times\frac{9+36+9+9+36+9}{64} =\boxed{3.375\,\mathrm{m^2/s^5}}.$$

四、把两个要求组成同一个目标

1. 统一代价尺度

两项代价的物理单位不同。本例分别用固定参考量归一化：

$$\bar J_s=\frac{J_s}{1\,\mathrm{m^2/s^5}},\qquad \bar J_d=\frac{J_d}{1\,\mathrm{m^2}}.$$

再取相同权重，定义无量纲总分：

$$\boxed{F(q)=\bar J_s(q)+\bar J_d(q)}.$$

参考量、权重和期望间距都是这个算例明确选定的设计参数。

初始轨迹的分数为：

$$\boxed{F(3)=3.375+1.5625=4.9375}.$$

2. 两个目标各自希望控制点去哪里？

平滑代价可以整理成：

$$\bar J_s(q)=\frac58(q-4.5)^2+\frac{63}{32}.$$

因此，只比较平滑项时，最低点在 $q=4.5$。间距项则会一直鼓励增加 $q$，直到 $q=4.875$ 达到期望间距。

选择	归一化平滑项	归一化间距项	总分
$q=3$：初始方案	3.3750	1.5625	4.9375
$q=4.5$：平滑项最低	1.9688	0.0625	2.0313
$q=4.875$：达到期望间距	2.0566	0	2.0566

从 4.5 继续增加 $q$ 时，平滑项上升、间距项下降。总分取决于两者一起变化的结果。

在间距项仍起作用的 $3\le q<4.875$ 内，将两式相加：

$$\boxed{F(q)=\frac{77}{72}q^2-\frac{239}{24}q+\frac{403}{16}}.$$

下面先利用它的变化率，逐轮寻找更好的位置。

五、算出梯度，完成第一次控制点更新

1. 平滑项怎样随 $q$ 变化？

对 $\bar J_s$ 求导：

$$\frac{d\bar J_s}{dq}=\frac54q-\frac{45}{8}.$$

在 $q=3$ 处：

$$\frac{d\bar J_s}{dq}=\frac54\times3-\frac{45}{8} =-\frac{15}{8}=-1.875.$$

这个负值表示：在当前附近增加 $q$，平滑代价会下降。

2. 间距项的导数分三步传回来

在间距不足时，令 $r=2.25-d$，有 $\bar J_d=r^2$。按本例的单位归一化，数值计算分成三步：

$$\frac{d(r^2)}{dr}=2r,\qquad \frac{dr}{dd}=-1,\qquad \frac{dd}{dq}=\frac23.$$

把三个变化率相乘：

$$\boxed{\frac{d\bar J_d}{dq} =-2\bigl(2.25-d(q)\bigr)\times\frac23}.$$

这里的 $2/3$，正是第二节算出的控制点权重。

在 $q=3$ 处，距离为 1，缺少 1.25，所以：

$$\frac{d\bar J_d}{dq} =-2\times1.25\times\frac23 =-\frac53\approx-1.6667.$$

这条计算链可以这样读：

3. 相加，确定移动方向

总分的变化率等于两项之和：

$$F'(3)=-1.875-1.6667\approx-3.5417.$$

准确值为 $-85/24$。本例只有一个变量，这个导数就是梯度。

在当前点附近，它表示：$q$ 每增加一小段，总代价按照负的变化率下降。例如，小幅增加 0.01，总代价的变化近似为：

$$\Delta F\approx F'(3)\times0.01\approx-0.03542.$$

因此这一轮选择向 $+y$ 方向移动控制点。

4. 移动多少？

采用梯度下降更新：

$$\boxed{q_{\mathrm{new}}=q-\eta F'(q)}.$$

$\eta>0$ 控制把当前导数换成多大的调整量。以下按坐标米数和归一化代价进行数值计算，取 $\eta=0.2$。

第一轮：

$$\begin{aligned} q_1 &=3-0.2\left(-\frac{85}{24}\right)\\ &=3+\frac{17}{24}\\ &=\boxed{3.708333\ldots}. \end{aligned}$$

控制点由 $(6,3)$ 移到 $(6,3.708333\ldots)$，向上移动约 0.7083 米。

第 6 秒的曲线点随之上移：

$$\frac23\times\frac{17}{24}=\frac{17}{36}\approx0.47222\text{ 米}.$$

此刻的位置和间距变为：

$$p(6)\approx(6,3.47222),\qquad d\approx1.47222\text{ 米}.$$

重新计算两项代价：

$$\bar J_s(q_1)\approx2.36046,\qquad \bar J_d(q_1)\approx0.60494,$$

$$\boxed{F(q_1)\approx2.96540<4.9375}.$$

候选仍在 $[3,5]$ 中，并通过后面的全区间约束检查，因此接受这次更新。

梯度提供当前的下降方向，更新后的真实代价与约束检查决定是否接受候选。

六、继续更新，直到两个方向的作用平衡

1. 每一轮都重新计算导数

第一轮之后，控制点已经移动，梯度也要在新位置重新计算。

在间距项激活的区间内：

$$\boxed{F'(q)=\frac{77}{36}q-\frac{239}{24}}.$$

代入 $q_1\approx3.708333$：

$$F'(q_1)\approx-2.026620.$$

第二次更新得到：

$$q_2=3.708333-0.2(-2.026620) \approx\boxed{4.113657}.$$

同样继续计算：

已完成更新次数	控制点坐标 $q$（米）	平滑项	间距项	总分	当前梯度
0	3.00000	3.37500	1.56250	4.93750	-3.54167
1	3.70833	2.36046	0.60494	2.96540	-2.02662
2	4.11366	2.06204	0.25762	2.31966	-1.15968
3	4.34559	1.98365	0.12457	2.10822	-0.66359
4	4.47831	1.96904	0.06994	2.03898	-0.37972
5	4.55426	1.97059	0.04572	2.01631	-0.21729
6	4.59771	1.97472	0.03417	2.00889	-0.12434
12	4.65380	1.98353	0.02175	2.00528	-0.00437

表中的代价都已归一化。计算使用完整精度，表格只展示舍入后的数值。

随着 $q$ 接近最低点，梯度的绝对值减小，每一轮的移动量也逐渐缩小。

2. 用方程核对最终位置

这个算例只有一个变量，最低点还可以直接求出。令导数为零：

$$\frac{77}{36}q-\frac{239}{24}=0.$$

两边乘以 72：

$$154q-717=0.$$

所以：

$$\boxed{q^*=\frac{717}{154}\approx4.655844\text{ 米}}.$$

它位于间距项激活的区间中。该区间内总分是开口向上的二次函数；在 $q\ge4.875$ 的剩余区间，间距项为零，平滑项继续上升。因此，它是本例 $3\le q\le5$ 范围内的唯一最低点。

代回得到：

$$\boxed{F(q^*)=\frac{4941}{2464}\approx2.005276}.$$

第 6 秒的曲线位置与间距为：

$$\boxed{p(6)\approx(6,4.103896)},\qquad \boxed{d\approx2.103896\text{ 米}}.$$

此时平滑项的导数约为 $+0.194805$，间距项的导数约为 $-0.194805$，相加正好为零。

两项偏好在这里取得平衡。期望间距与实际间距还差约 0.1461 米，这部分作为软惩罚保留在总分中。需要把 2.25 米设为必须达到的要求时，应明确增加 $d(q)\ge2.25$，在本例中就是 $q\ge4.875$，并重新求解。

3. 更新系数过大时，怎样处理？

回到初始 $q=3$，如果取 $\eta=1$，候选变为：

$$q_{\mathrm{trial}}=3-1\left(-\frac{85}{24}\right) =\frac{157}{24}\approx6.5417.$$

它已经超出允许区间。进一步计算第 6 秒的侧向加速度：

$$a_y(6)=\frac{3-2q+3}{4}=\frac{6-2q}{4} \approx-1.7708\,\mathrm{m/s^2},$$

也超过了大小上限 1。

因此，这个候选被拒绝。可以把 $\eta$ 减半，重新生成候选，再检查范围、运动要求和总代价。这种沿当前方向尝试并缩短更新量的过程，称为回溯线搜索。

正文的 $\eta=0.2$ 在本算例中逐轮通过检查。其他初值、权重或地图下，更新量应由相应的检查结果决定。

七、用整条曲线核验最终结果

1. 首尾状态与段间连续性

只有 $P_4$ 发生变化，首尾三个控制点保持原样。因此：

$$\begin{aligned} p(0)&=(0,0),&p(12)&=(12,0),\\ v(0)&=(1,0),&v(12)&=(1,0),\\ a(0)&=(0,0),&a(12)&=(0,0). \end{aligned}$$

修改影响 $C_1,C_2,C_3,C_4$，首尾两段完全保留。节点设置和每段时间都保持不变，相邻段仍共享相同的控制点组合，位置、速度和加速度继续连续。

2. 速度与加速度上界

速度控制向量为：

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

在 $3\le q\le5$ 内，最大的相邻侧向差仍为 3 米，横向差为 2 米，所以：

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

加速度控制向量为：

$$E_j=\frac{P_j-2P_{j+1}+P_{j+2}}4.$$

七个侧向二阶差依次为：

$$0,\quad3,\quad q-6,\quad6-2q,\quad q-6,\quad3,\quad0.$$

在整个可选区间中，绝对值都不超过 4，因此加速度大小不超过 1。代入最终 $q^*$，最大的绝对值来自中间一项：

$$\boxed{ \|a(t)\|\le\frac{2q^*-6}{4} \approx0.827922<1 }.$$

这是覆盖全时间区间的导数凸组合上界。

检查项目	初始方案	优化后
速度大小的全区间上界（米/秒）	1.8028	1.8028
加速度大小的全区间上界（米/秒²）	0.7500	0.8279
第 6 秒额外间距（米）	1.0000	2.1039
归一化 Jerk 平滑项	3.3750	1.9839
总分	4.9375	2.0053

本次优化减少了累计 Jerk 代价，增加了中点间距；加速度峰值有所增加，仍满足既定上限。各项数值分别描述运动的不同性质。

3. 用凸区域检查连续避障

全部横坐标固定，每段始终满足：

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

因此，只有 $C_2$ 和 $C_3$ 经过障碍的横向范围 $5\le x\le7$。其余四段位于障碍左侧或右侧。

把中间两段转换成局部 Bézier 控制点：

点	$C_2$ 的局部控制点	$C_3$ 的局部控制点
$Q_0$	$(4,\ 2+q/6)$	$(6,\ 1+2q/3)$
$Q_1$	$(14/3,\ 2+q/3)$	$(20/3,\ 1+2q/3)$
$Q_2$	$(16/3,\ 1+2q/3)$	$(22/3,\ 2+q/3)$
$Q_3$	$(6,\ 1+2q/3)$	$(8,\ 2+q/6)$

例如，第一行来自：

$$Q_0^{(2)}=\frac{P_2+4P_3+P_4}{6} =\left(4,\frac{12+q}{6}\right).$$

当 $3\le q\le5$ 时，这八个点都位于凸矩形：

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

具体地，它们的最小侧向坐标至少为 $2+3/6=2.5$，最大侧向坐标至多为 $1+2\times5/3=13/3<4.5$。

代入最终 $q^*$，三种侧向坐标分别为：

$$2+\frac{q^*}{6}\approx2.7760,\qquad 2+\frac{q^*}{3}\approx3.5519,\qquad 1+\frac{2q^*}{3}\approx4.1039.$$

两段曲线分别位于各自控制点的凸包中，因此都留在 $\mathcal F$ 内。整个区域满足 $y\ge2.5>2$，与禁行区域分离。其余各段位于障碍左右两侧，所有段的控制点凸包也都在地图范围内。

这次全程避障结论来自凸包包含关系；第 6 秒的距离代价用于选择偏好的绕行形状。 两者在本例中分工明确。

八、把一个变量的计算扩展到多个控制点

1. 障碍方向怎样传递给控制点？

在本例中，最近边界是矩形顶边。曲线点上移会增加距离，因而距离的空间梯度为：

$$\nabla_p d=(0,1).$$

对于间距不足的曲线点，平方罚项对该点位置的梯度为：

$$\nabla_p\bar J_d=-2(d_{\mathrm{ref}}-d)\,\nabla_p d,$$

这里沿用前面的单位归一化。在初始中点，代入差距 1.25：

$$\nabla_p\bar J_d=(0,-2.5).$$

第 6 秒的曲线点由控制点加权得到，$P_4$ 的权重为 $2/3$。继续沿链式法则传递：

$$\boxed{\nabla_{P_4}\bar J_d =\frac23(0,-2.5)=\left(0,-\frac53\right)}.$$

它的侧向分量正是第五节得到的 $-1.6667$。

一般地，固定时间与节点，在某个评价时刻，若控制点 $P_k$ 的基函数权重为 $\beta_k$，就有：

$$\boxed{ \nabla_{P_k}\bar J_d =-2\beta_k(d_{\mathrm{ref}}-d)\,\nabla_p d },\qquad d<d_{\mathrm{ref}}.$$

这个表达式在距离可微的位置使用。达到期望间距时，平方短缺罚项的梯度为零。

本例的距离与方向由矩形几何直接算出。在更一般的地图上，可以查询距离场中的距离和梯度，再通过同样的基函数权重，把影响传回控制点。

2. 多个评价位置、多项代价怎样共同作用？

如果在多个时刻评价间距，每个曲线点都会对相关控制点产生一份梯度。将这些贡献与平滑项等其他代价的梯度相加，就得到各个控制点的总变化率。

B 样条的局部支撑使一个曲线位置只涉及附近的控制点。固定时间后，导数代价与边界关系也由这些控制点的组合计算。

多点距离代价用于评价选定位置；整条运动的可行性仍由相应的连续验证或保守约束检查。复杂障碍布局还会形成不同绕行方案，搜索结果可以为局部优化提供一条合适的初始路线。

九、把本例整理成可执行流程

下面的流程始终使用完整的平方短缺罚项，超过期望间距后自动令间距代价为零。

固定地图、八个控制点、节点与每段时间
设 q = 3

重复：
    计算当前平滑代价和中点间距代价
    计算它们对 q 的导数，再相加

    如果梯度的绝对值足够小：
        结束迭代

    先取更新系数 η = 0.2

    尝试：
        q_trial = q - η × 当前梯度
        检查 q_trial 是否在 [3,5] 中
        检查整段运动上限、边界状态与避障条件
        计算候选总代价

        如果候选通过检查，而且代价下降：
            接受 q = q_trial
            进入下一轮
        否则：
            将 η 减半，再尝试

用最终 q 生成整条轨迹，保存验证结果

设置迭代次数与最小更新量，可以限制计算预算。配套文件 bspline_optimization_example.py 使用 Python 标准库复算正文，并独立核对多项式导数、有限差分梯度、各段衔接及凸包约束。

运行：

python bspline_optimization_example.py

程序会列出各轮 $q$、两项代价、总分和梯度，再将迭代结果与解析最低点比较。

小结

本例只调整 $P_4$ 的侧向坐标，用 Jerk 平方积分评价整段变化，用一个中点距离项表达额外间距偏好。梯度将这两个目标转化为具体移动，从 $q=3$ 逐步得到 $q\approx4.6558$。

每次移动都会通过 B 样条权重影响附近曲线。总代价检查确认改善，导数上界检查运动需求，凸区域检查整个绕障过程。

控制点是待求量，代价表达取舍，梯度计算移动方向，约束定义允许接受的运动。 下一步可以把多个控制点一起纳入求解，并将安全飞行走廊写成可直接使用的轨迹约束。