安全飞行走廊：一步一步把避障区域写进轨迹优化

一架无人机需要用 12 秒绕过矩形障碍。规划器先选定左侧、上方和右侧三块允许飞行的区域，再调整轨迹，让各段曲线留在分配给它们的区域中。

上一篇 B 样条轨迹优化：一步一步计算控制点的移动 用距离代价把轨迹推离障碍，最后再检查是否安全。本篇换一种思路：先规定安全区域，再只在区域内挑选轨迹。

安全飞行走廊用一组相互衔接的凸自由区域，表达轨迹可以经过的范围。 区域边界可以转化为控制点的不等式，直接参与轨迹优化。

下面用九个 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,a)$	$P_6$	$(10,0)$
$P_1$	$(0,0)$	$P_4$	$(6,b)$	$P_7$	$(12,0)$
$P_2$	$(2,0)$	$P_5$	$(8,c)$	$P_8$	$(14,0)$

$a,b,c$ 是本次待求的三个纵坐标。初始方案取：

$$a=b=c=3.$$

每段使用相邻四个控制点，共六段，每段持续 $h=2$ 秒。节点向量保持为：

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

整体参数为 $u=t/2$，有效范围 $0\le u\le6$。第 $i$ 段记为 $C_i(s)$，其中：

$$s=\frac{t-2i}{2},\qquad 0\le s\le1.$$

2. 哪些边界状态保持不变？

首尾各三个控制点固定，因此所有候选都满足：

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

位置、速度、加速度分别按米、米/秒、米/秒²计。下文的 $a,b,c$ 表示控制点坐标，$\mathbf a(t)$ 表示运动加速度。

这是一段以非零速度进入、以非零速度离开的绕障运动。调整 $P_3,P_4,P_5$ 会改变六段曲线的部分或全部内部形状，起终状态仍由固定的控制点保留。

本例要求全程满足：

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

本次以整段 Jerk 平方积分为目标，避障要求通过走廊约束表达。 时间、横坐标和走廊在求解期间都保持不变。

二、先把允许经过的区域画出来

1. 三个矩形组成一条走廊

选择以下三块区域：

区域	横向范围	侧向范围	分配的曲线段
左侧 $\mathcal F_L$	$0\le x\le4.5$	$0\le y\le4.5$	$C_0,C_1$
上方 $\mathcal F_M$	$4\le x\le8$	$2.5\le y\le4.5$	$C_2,C_3$
右侧 $\mathcal F_R$	$7.5\le x\le12$	$0\le y\le4.5$	$C_4,C_5$

这些矩形都在地图内，也都与禁行区域分开：左侧区域最多到 $x=4.5<5$，右侧从 $x=7.5>7$ 开始，上方区域满足 $y\ge2.5>2$。

相邻区域还有交叠。例如，左侧与上方的交叠为：

$$4\le x\le4.5,\qquad 2.5\le y\le4.5.$$

曲线可以通过交叠部分，从一个区域接续到下一个区域。

这里每个矩形都是凸集：其中任意两点的连线仍在矩形内。整条走廊由这些局部区域的并集组成，各段分别使用自己的区域。

2. 哪些曲线段会经过障碍附近？

横坐标等距且始终固定，将它们代入 B 样条公式，可得：

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

所以各段的横向范围为：

曲线段	横向范围（米）	曲线段	横向范围（米）
$C_0$	0～2	$C_3$	6～8
$C_1$	2～4	$C_4$	8～10
$C_2$	4～6	$C_5$	10～12

只有 $C_2,C_3$ 经过障碍的横向范围 $5\le x\le7$。把这两段限制在上方走廊，就能让它们从障碍上方通过；其余各段分别使用左右走廊。

这些区域是本例根据矩形地图直接构造的。它们与曲线段的对应关系，是优化问题的已知输入。

三、把“整段留在区域里”写成有限个条件

1. 为每段换一组局部 Bézier 控制点

第 $i$ 段使用 $P_i,P_{i+1},P_{i+2},P_{i+3}$。对应的四个局部 Bézier 控制点为：

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

上标 $(i)$ 表示所属曲线段。转换保留原曲线，只改变这一段的表示方式。

四个局部控制点全部在同一个凸区域内，便能保证整段 Bézier 曲线在该区域内。 原因是曲线点由这四个点非负加权得到，权重之和为 1。

因此，对每段只需给四个 $Q$ 写出区域条件。

2. 看中间两段：纵坐标怎样依赖三个变量？

把控制点坐标代入：

局部点	$C_2$ 中的坐标	$C_3$ 中的坐标
$Q_0$	$(4,\ (4a+b)/6)$	$(6,\ (a+4b+c)/6)$
$Q_1$	$(14/3,\ (2a+b)/3)$	$(20/3,\ (2b+c)/3)$
$Q_2$	$(16/3,\ (a+2b)/3)$	$(22/3,\ (b+2c)/3)$
$Q_3$	$(6,\ (a+4b+c)/6)$	$(8,\ (b+4c)/6)$

例如，$C_2$ 的第一个点为：

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

横坐标已经在 $[4,8]$ 内，纵坐标需要满足：

$$2.5\le\frac{4a+b}{6}\le4.5.$$

乘以 6，得到：

$$\boxed{15\le4a+b\le27}.$$

每一项都有具体含义：$4a+b$ 决定这段曲线进入上方走廊的位置，15 和 27 来自走廊的上下边界。

对表中其余各点，同样要求纵坐标在 $[2.5,4.5]$ 内。例如：

$$\begin{aligned} 7.5&\le2a+b\le13.5,\\ 7.5&\le a+2b\le13.5,\\ 15&\le a+4b+c\le27,\\ 15&\le b+4c\le27. \end{aligned}$$

左右两侧的曲线段则对各自的 $Q$ 使用 $0\le Q_y\le4.5$。所有约束都只是 $a,b,c$ 的加减与常数倍。

3. 区域边界怎样变成轨迹约束？

以条件 $4a+b\ge15$ 为例，从区域到优化变量共四步：

1. 上方走廊要求 $y\ge2.5$；
2. $C_2$ 第一个局部控制点的纵坐标是 $(4a+b)/6$；
3. 于是要求 $(4a+b)/6\ge2.5$；
4. 得到对优化变量的线性限制 $4a+b\ge15$。

这样，优化器每次选择控制点时，都需要满足明确的不等式。各段的凸包包含关系再把这些有限条件传递到整个连续区间。

四、用三个变量给整条运动评分

1. 六段 Jerk 怎样计算？

对三次均匀 B 样条，第 $i$ 段的 Jerk 为：

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

它来自三次多项式对实际时间求三次导数。每段内 Jerk 恒定，本例 $h^3=8$。横坐标等距，横向 Jerk 为零，只需计算侧向部分：

曲线段	侧向三阶差	侧向 Jerk
$C_0$	$a$	$a/8$
$C_1$	$b-3a$	$(b-3a)/8$
$C_2$	$c-3b+3a$	$(c-3b+3a)/8$
$C_3$	$-3c+3b-a$	$(-3c+3b-a)/8$
$C_4$	$3c-b$	$(3c-b)/8$
$C_5$	$-c$	$-c/8$

例如，$C_2$ 使用侧向坐标 $0,a,b,c$，三阶差就是：

$$c-3b+3a-0.$$

2. 对各段平方并累加

选择平滑代价：

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

每段持续 2 秒，因此：

$$\boxed{ \begin{aligned} J_s(a,b,c)=\frac1{32}\bigl[&a^2+(b-3a)^2+(c-3b+3a)^2\\ &+(-3c+3b-a)^2+(3c-b)^2+c^2\bigr]. \end{aligned} }$$

其中 $1/32=2/8^2$：持续时间乘上 Jerk 分母平方的倒数。以下坐标数值按米计，代价按 $\mathrm{m^2/s^5}$ 计。

初始 $a=b=c=3$ 时，六个三阶差依次为：

$$3,\ -6,\ 3,\ -3,\ 6,\ -3.$$

所以：

$$J_s(3,3,3)=\frac{9+36+9+9+36+9}{32} =\boxed{3.375}.$$

将平方展开并合并，还可以写成：

$$\boxed{ J_s=\frac58a^2-\frac{15}{16}ab+\frac38ac +\frac58b^2-\frac{15}{16}bc+\frac58c^2. }$$

一个控制点同时影响前后若干段，代价中便出现了 $ab$、$bc$ 这样的交叉项。三个位置需要一起选择。

五、把速度与加速度上限也写进问题

1. 相邻控制边限制速度

速度可以表示为导数控制向量的凸组合，控制向量为：

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

横坐标之差恒为 2，因此每个 $D_j$ 的横向分量都是 1。若一条控制边的纵向差为 $\delta$：

$$D_j=(1,\delta/2).$$

要求它的大小不超过 2：

$$1+\frac{\delta^2}{4}\le4 \quad\Longrightarrow\quad |\delta|\le2\sqrt3.$$

本例需要检查的非固定纵向差为：

$$a,\qquad b-a,\qquad c-b,\qquad -c.$$

因此加入：

$$\boxed{|a|,\ |b-a|,\ |c-b|,\ |c|\le2\sqrt3}.$$

这里的写法表示四个绝对值分别满足上限。每个条件都能写成线性上下界，例如：

$$-2\sqrt3\le b-a\le2\sqrt3.$$

2. 二阶控制点差限制加速度

加速度是相邻两个向量 $E_j$ 的线性插值：

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

它们的横向分量都是零，侧向分子依次为：

$$0,\quad a,\quad b-2a,\quad a-2b+c,\quad b-2c,\quad c,\quad0.$$

为了保证加速度大小不超过 1，加入：

$$\boxed{|a|,\ |b-2a|,\ |a-2b+c|,\ |b-2c|,\ |c|\le4}.$$

这些导数控制向量条件提供全时间区间的运动上界。 约束住所有参与加权的向量，就约束住了整段速度与加速度。

至此，完整问题已经确定：以 $a,b,c$ 为变量，最小化 $J_s$，同时满足全部局部控制点的走廊条件，以及上面两组运动条件。

六、一步一步求出三个控制点

这个例子可以用手算得到解。先从全部走廊条件中保留两条必要条件：

$$\boxed{4a+b\ge15,\qquad b+4c\ge15}.$$

第一条来自 $C_2$ 的入口，第二条来自 $C_3$ 的出口。

暂时只用这两条约束，会得到一个更大的候选集合。先求这个集合中的最低代价，再将结果代回全部条件：如果它全部通过，就同时是完整问题的最优解。 因为完整问题的候选都包含在这个更大集合中。

1. 先把左右两个变量合到一起

地图、固定控制点、时间和代价都左右对称。令两侧纵坐标的平均值为：

$$z=\frac{a+c}{2}.$$

将代价按平均值整理，可以得到：

$$\boxed{J_s(a,b,c)=J_s(z,b,z)+\frac7{32}(a-c)^2}.$$

后面的平方项大于或等于零。把两侧都改成平均值 $z$，能够保留或降低代价。

两条必要约束取平均后，也得到：

$$4z+b\ge15.$$

因此，这个简化问题至少有一个最优解满足：

$$a=c=z.$$

代价随之变成：

$$\boxed{J_s(z,b,z)=\frac{13z^2-15zb+5b^2}{8}}.$$

2. 为什么最低点会落在 $4z+b=15$ 上？

在只保留这条要求的候选集合中，假设 $4z+b>15$。将 $z,b$ 同时乘以：

$$\lambda=\frac{15}{4z+b},\qquad 0<\lambda<1.$$

新坐标满足：

$$4(\lambda z)+\lambda b=15.$$

代价中的每一项都是坐标的二次项，因此新的代价为：

$$J_{s,\mathrm{new}}=\lambda^2J_s<J_s.$$

例如，若 $4z+b=16$，将两个坐标同时乘 $15/16$，仍满足这条要求，代价变成原来的 $(15/16)^2$。

所以，这个简化问题的最低点满足：

$$4z+b=15,\qquad\boxed{b=15-4z}.$$

三个待求坐标现在都可以用 $z$ 表示：

$$a=z,\qquad b=15-4z,\qquad c=z.$$

3. 代入后，得到一个二次函数

将 $b=15-4z$ 代入代价：

$$J_s(z)=\frac{13z^2-15z(15-4z)+5(15-4z)^2}{8}.$$

分别展开：

$$-15z(15-4z)=-225z+60z^2,$$

$$5(15-4z)^2=1125-600z+80z^2.$$

合并相同次幂：

$$\boxed{J_s(z)=\frac{153z^2-825z+1125}{8}}.$$

在最低点，导数为零：

$$\frac{306z-825}{8}=0.$$

于是：

$$\boxed{z=\frac{825}{306}=\frac{275}{102}\approx2.696078}.$$

代回另外两个坐标：

$$\boxed{ \begin{aligned} a&=c=\frac{275}{102}\approx2.696078,\\ b&=15-4\times\frac{275}{102} =\frac{215}{51}\approx4.215686. \end{aligned} }$$

代价还可以整理为：

$$J_s(z)=\frac{153}{8}\left(z-\frac{275}{102}\right)^2+\frac{875}{544}.$$

因此最低值为：

$$\boxed{J_s=\frac{875}{544}\approx1.608456}.$$

这个结果已经同时给出三个控制点：

控制点	初始位置	优化后位置
$P_3$	$(4,3)$	$(4,2.6961)$
$P_4$	$(6,3)$	$(6,4.2157)$
$P_5$	$(8,3)$	$(8,2.6961)$

两侧稍微降低，中间抬高，整段加速度变化随之重新分配。接下来核验这个候选是否满足完整要求。

七、把候选代回，逐段验证

1. 所有局部控制点是否在对应走廊内？

使用求出的 $a,b,c$，每段四个局部 Bézier 控制点的纵坐标为：

曲线段	$Q_{0,y}$	$Q_{1,y}$	$Q_{2,y}$	$Q_{3,y}$	纵向范围
$C_0$	0	0	0	0.4493	$[0,4.5]$
$C_1$	0.4493	0.8987	1.7974	2.5	$[0,4.5]$
$C_2$	2.5	3.2026	3.7092	3.7092	$[2.5,4.5]$
$C_3$	3.7092	3.7092	3.2026	2.5	$[2.5,4.5]$
$C_4$	2.5	1.7974	0.8987	0.4493	$[0,4.5]$
$C_5$	0.4493	0	0	0	$[0,4.5]$

全部满足各自的上下界。表中采用小数显示，验证使用精确分数。

横坐标方面，每段四个局部控制点依次为：

$$2i,\quad2i+\frac23,\quad2i+\frac43,\quad2i+2.$$

它们也全部位于分配的走廊横向范围内。

例如，中间两段的入口与出口恰好满足：

$$\frac{4a+b}{6}=\frac{b+4c}{6}=2.5.$$

这两项约束在结果中取等号，称为有效约束。这里接触的是允许区域的边界；这条边界位于禁行区上方，仍留有间隔。

由此得到完整的包含关系：

1. 每段的四个局部 Bézier 控制点都在指定凸区域内；
2. 由凸包性质，每段曲线都在指定凸区域内；
3. 各个区域都在地图内，且与禁行区分离；
4. 因此整条连续轨迹满足本例的几何避障要求。

2. 速度上限怎样核验？

将最优坐标代入相邻控制点之差。最大的侧向差是：

$$\max\{|a|,|b-a|,|c-b|,|c|\} =\frac{275}{102}\approx2.696078.$$

于是所有速度控制向量的大小都不超过：

$$\boxed{ \max_j\|D_j\| =\sqrt{1+\left(\frac{275}{204}\right)^2} \approx1.678455<2. }$$

速度是这些向量的凸组合，因此整条轨迹都满足速度限制。

3. 加速度上限怎样核验？

七个侧向二阶差除以 4，得到加速度控制向量的侧向分量：

$$0,\quad0.674020,\quad-0.294118,\quad-0.759804, \quad-0.294118,\quad0.674020,\quad0.$$

最大绝对值为：

$$\boxed{\max_j\|E_j\|=\frac{155}{204}\approx0.759804<1}.$$

每段加速度是相邻两个 $E$ 的插值，所以整个区间都满足限制。

4. 起终状态和段间衔接怎样核验？

首尾各三个控制点保持不变，起终位置、速度、加速度满足第一节的设定。节点与分段时间保持不变，相邻段继续共享同一组控制点关系，位置、速度和加速度连续。

例如，第 4 秒处，$C_1$ 结束，$C_2$ 开始。两侧都给出：

$$\mathbf p(4)=(4,2.5),$$

$$\mathbf v(4)=\frac{P_4-P_2}{2h} =\left(1,\frac{215}{204}\right) \approx(1,1.053922),$$

$$\mathbf a(4)=\frac{P_2-2P_3+P_4}{h^2} =\left(0,-\frac5{17}\right) \approx(0,-0.294118).$$

这里的同一状态既是前一段的末端，也是后一段的起点。

候选已经通过全部走廊、边界与运动条件。 它在只保留两条必要约束的更大集合中取得最低代价，因此也在本例完整约束集合中取得最低代价。

八、从运动曲线看优化改变了什么

计算几个时刻，得到：

时间（秒）	位置（米）	速度（米/秒）	加速度（米/秒²）
0	$(0,0)$	$(1,0)$	$(0,0)$
4	$(4,2.5000)$	$(1,1.0539)$	$(0,-0.2941)$
5	$(5,3.3681)$	$(1,0.6434)$	$(0,-0.5270)$
6	$(6,3.7092)$	$(1,0)$	$(0,-0.7598)$
8	$(8,2.5000)$	$(1,-1.0539)$	$(0,-0.2941)$
12	$(12,0)$	$(1,0)$	$(0,0)$

第 6 秒位于绕行曲线的最高位置。侧向速度为零，侧向加速度为负，运动方向逐渐转向右下方。

比较两种方案：

项目	初始 $a=b=c=3$	优化后
Jerk 平方积分	3.3750	1.6085
第 6 秒侧向位置（米）	3.0000	3.7092
速度控制向量上界（米/秒）	1.8028	1.6785
实际速度峰值（米/秒）	1.5052	1.5189
加速度大小峰值（米/秒²）	0.7500	0.7598
走廊与起终状态	全部满足	全部满足

速度上界与实际峰值分别计算：上界来自导数控制向量，实际峰值来自每段速度函数的极值。优化降低了 Jerk 的累计代价，本例实际速度与加速度峰值略有增加，并保持在限制内。

九、把算例整理成一个约束优化问题

1. 决策变量、目标和约束

本例可以写为：

$$\begin{aligned} \min_{a,b,c}\quad &J_s(a,b,c)\\ \text{满足}\quad &Q_k^{(i)}(a,b,c)\in\mathcal F_i,\quad i=0,\ldots,5,\ k=0,\ldots,3,\\ &|a|,|b-a|,|c-b|,|c|\le2\sqrt3,\\ &|a|,|b-2a|,|a-2b+c|,|b-2c|,|c|\le4. \end{aligned}$$

$\mathcal F_i$ 指第 $i$ 段被分配的走廊。起终要求已经通过固定首尾控制点写入参数化。

目标是线性表达式的平方和，约束可以全部写成线性不等式。因此，这个固定时间、固定横坐标、固定走廊的问题是一个凸二次规划（QP）。

上面的手算利用对称性与两条有效约束求解；更多控制点时，可以直接把这些变量和约束交给求解器。

2. 换成一般的凸多面体，约束仍是线性的

本例的走廊是坐标轴对齐的矩形，每条边界只涉及一个坐标。三维环境中的走廊通常是一般的凸多面体，可以写成若干个半空间的交集：

$$\mathcal F_i=\{\mathbf x\mid A_i\mathbf x\le\mathbf b_i\}.$$

局部控制点 $Q_k^{(i)}$ 是 B 样条控制点的固定线性组合，所以包含条件 $A_iQ_k^{(i)}\le\mathbf b_i$ 仍是控制点的线性不等式，问题依旧是 QP。

实际规划器一般不手工画走廊，而是先用图搜索或采样得到一条无碰撞路径，再沿路径在点云或栅格地图中逐段膨胀出凸区域，让相邻区域彼此交叠。走廊的大小决定可选轨迹的范围：区域越贴合自由空间，可选方案越多；区域过小或相邻区域交叠不足，约束就可能无解。

3. 最优结论对应哪个范围？

本例比较的是三次均匀 B 样条，在 12 秒的固定时间安排下，采用上述走廊分配，并满足控制点与导数充分条件的轨迹。

固定走廊已经选定了“从障碍上方通过”这一绕行方案。其他绕行方向、分段时间或控制点数量，对应另外的候选集合，需要相应地重新建模。

控制点包含条件提供整段安全的充分保证，也会限制曲线的可选范围。需要扩大可选范围时，可以细分曲线段、调整走廊，或采用更精细的连续几何验证。

4. 复算这个例子

配套文件 safe_corridor_example.py 只使用 Python 标准库。运行：

python safe_corridor_example.py

程序从三个独立变量出发，用精确分数求解两条必要约束组成的问题，再核验全部局部 Bézier 控制点、速度与加速度上界、起终状态和段间衔接。

它还会用另一套曲线表示核对位置，用多项式导数重新累计 Jerk 代价，并输出正文中的数值。程序中的区间保证来自凸包与导数界，取样计算用于交叉检查。加上 --json verified.json 参数，可以把计算结果保存为文件。

小结

安全飞行走廊先给每段轨迹规定一个凸自由区域，再把区域边界变成控制点的线性条件。曲线的凸包性质将有限个控制点约束传递到整个连续区间。

在这个九控制点算例中，同时调整三个中间纵坐标，得到约 2.6961、4.2157、2.6961 米。Jerk 平方积分从 3.3750 降到 1.6085，整条曲线通过走廊、速度、加速度及边界状态检查。

走廊规定允许的空间，参数化连接控制点与整段曲线，目标函数决定可行方案之间的取舍。 到这里，轨迹已经给出每个时刻的位置、速度和加速度；下一步把这些加速度换算成多旋翼所需的推力与姿态，检查机体能否真正飞出这条轨迹。