无人机规划算法眼中的世界：构型空间、地图与碰撞检测

一架无人机要穿过仓库中的通道，到达货架另一侧。规划器需要知道通道的位置与宽度、机体占据的空间，以及哪些运动能够避开货架。

这些信息由三部分共同表达：构型空间描述无人机的几何放置，地图记录环境，碰撞检测判断候选位置与运动是否可行。三者一起，把真实场景转化为路径规划算法能够处理的问题。

一、规划问题中的位置、构型与自由空间

1. 用构型描述无人机怎样放置

首先建立统一的坐标系，用位置 $p=(x,y,z)$ 描述无人机参考点的位置。起点、终点和障碍物都在同一个坐标系中表达，距离单位也保持一致。

要确定机体占据哪片空间，还需要结合它的形状与朝向。这种确定机器人几何放置方式的描述，称为构型（configuration）。

例如，将无人机包在一个球内，球的中心位置就能确定它占据的区域。这时可以用位置作为构型：$q=p$。

如果用一个长方体描述机体，同一位置下，朝向改变也会改变占据的区域。在平面模型中，构型可以写成 $q=(x,y,\theta)$，其中 $\theta$ 表示朝向角；在三维模型中，则需要位置与三维姿态共同描述。

所有可能构型组成的集合称为构型空间，记作 $\mathcal C$。构型空间中的一个点，对应机体的一种完整放置方式。

构型关注几何关系。进一步考虑运动时，还会把速度等量加入状态，用于描述后续运动能力。

2. 从构型空间中划出自由空间

将机体放在某个构型下，如果与障碍物接触或相交，就把这个构型归入构型障碍空间 $\mathcal C_{\mathrm{obs}}$。其余构型组成自由空间：

$$\mathcal C_{\mathrm{free}}=\mathcal C\setminus\mathcal C_{\mathrm{obs}}.$$

这里的“$\setminus$”表示从一个集合中去掉另一个集合。本文把与障碍边界接触也计为碰撞。

现实中的货架占据一片物理空间；构型障碍空间记录的是所有会让机体碰到货架的放置方式。它同时取决于货架和机体的几何形状。

几何路径规划的任务，就是在自由空间中，找到一条连接起点构型与终点构型的连续路径。

二、障碍物膨胀：把机体尺寸计入规划

1. 从机体避障转为中心点避障

先看一个水平运动的例子：用圆盘表示机体，用平面上的障碍区域表示这一高度下的碰撞限制。

设圆盘半径为 $r$。中心距离障碍物越近，圆盘越容易与障碍相交。当中心到障碍的距离小于或等于 $r$ 时，就会发生接触或碰撞。

因此，可以把障碍物向周围扩展距离 $r$，得到中心点的禁行区域。规划时，只要让中心点始终处于扩展区域之外，就能满足这个圆盘模型的避障要求。

这个操作称为障碍物膨胀。三维球形包络采用同样的思路，把障碍物向周围扩展一个球半径。

对于圆盘或球形包络，这种膨胀可以用 Minkowski 和描述：把圆盘或球的中心放到障碍区域的各个位置，取它覆盖区域的并集。

2. 一条通道究竟还剩多少可用空间？

假设两个货架之间的通道宽度为 $1.20\,\mathrm m$，无人机圆盘包络半径为 $0.25\,\mathrm m$，并预留 $0.10\,\mathrm m$ 的额外间距。

用于膨胀的有效半径为：

$$\rho=r+\delta=0.25+0.10=0.35\,\mathrm m.$$

其中，$r$ 表示机体包络半径，$\delta$ 表示额外间距。

通道两侧各向内占去 $0.35\,\mathrm m$，留给中心点运动的区域宽度为：

$$1.20-2\times0.35=0.50\,\mathrm m.$$

如果把通道横向坐标设为 $0$ 到 $1.20\,\mathrm m$，中心点应位于 $0.35$ 与 $0.85\,\mathrm m$ 之间，避开膨胀后的边界。

实际通道宽度描述的是环境，中心点可通行宽度描述的是这个机体模型在环境中的活动范围。

包络需要覆盖机体、旋转桨叶等可能接触障碍的部分。球形包络的碰撞形状与姿态无关，便于计算；更贴合机体的几何模型能够利用更多通行空间，同时需要处理姿态变化。

额外间距可以为定位误差、地图误差和轨迹跟踪偏差留出空间，其大小需要结合具体系统确定。规划区域的外边界也需要计入机体尺寸，避免机体越出允许范围。

三、地图怎样记录环境？

1. 从点云到空闲、占据与未知

深度相机或激光雷达可以获得环境表面的测量点。将这些点转换到统一坐标系后，就得到点云。

点云反映被观测到的表面。点之间的空白可能对应空闲空间，也可能来自遮挡、量程限制或测量缺失。建图需要结合传感器位置与测量过程，进一步判断空间的状态。

以一次有效测距为例（上图右侧）：从传感器到命中表面的射线提供沿途空闲的证据，命中位置提供占据的证据，表面后方被遮挡的区域仍然缺少信息。

占据地图据此区分三类情况：

地图状态	含义	对规划的作用
空闲	已有观测支持该区域没有障碍	结合机体尺寸检查是否可通行
占据	已有观测支持该区域存在障碍	构成避障限制
未知	尚未观测，或信息不足	按任务的未知空间策略处理

多次观测可以通过占据概率融合，再按设定规则分类。运动被限制在已知空闲空间时，可以将未知区域和地图外部一并作为禁行区域，再计入机体尺寸，使整个机体包络保持在已知空闲区域内。探索任务则会利用已知空闲与未知区域的交界，选择能够获得新观测的位置。

2. 用栅格和体素组织空间

在二维地图中，可以把空间划分为正方形小格；在三维地图中，对应的小立方格称为体素（voxel）。每个格子或体素都可以存储占据状态、观测信息或距离值。

体素描述的是空间怎样划分，占据地图描述的是格子里存储什么信息。一个体素地图可以保存占据值，也可以保存距离值。

设格子边长为 $h$，地图在 $x$ 方向的起始坐标为 $x_0$，位置 $x$ 对应的格子编号为：

$$i=\left\lfloor\frac{x-x_0}{h}\right\rfloor.$$

符号 $\lfloor\cdot\rfloor$ 表示向下取整。其他坐标方向采用相同计算。

例如，取 $h=0.10\,\mathrm m$、$x_0=0$，位置 $x=1.26\,\mathrm m$ 落在编号为 $12$ 的格子内。该格子的范围是 $[1.20,1.30)\,\mathrm m$，中心为 $1.25\,\mathrm m$。

这个转换把连续空间中的坐标，与计算机中的地图索引联系起来。

3. 分辨率影响几何细节与计算规模

格子边长 $h$ 就是地图的空间分辨率。较小的格子能够表达更细的几何结构，也会增加需要存储和处理的数据。

对于相同范围、全部分配存储的三维体素地图，将格子边长减半，每个方向的格子数量都翻倍，总数量变为原来的 $8$ 倍。

障碍物如何写入格子同样重要。例如，已知一根细柱穿过某个格子时，可以将整个格子标为占据。这种保守表示会扩大障碍在地图中的范围。窄通道经过离散化和机体膨胀后，可能失去可用的连接。

八叉树采用分层划分：将三维区域逐层分成八个子区域，对能够合并的同类区域使用较大的节点表示，在需要细节的位置继续细分。OctoMap 就是利用八叉树组织概率占据地图的一种方法。

地图分辨率、观测质量和障碍写入规则，共同决定规划器实际看到的几何环境。

四、距离场：给空间增加“离障碍多远”的信息

1. ESDF 表达最近距离

占据地图提供障碍的位置与范围。欧氏符号距离场（ESDF）进一步为位置 $p$ 提供到最近障碍表面的距离，记为 $d(p)$。

采用障碍外部为正、内部为负、表面为零的约定，距离值同时表达远近与内外关系。例如，$d(p)=0.60\,\mathrm m$ 表示该位置在障碍外，距最近表面 $0.60\,\mathrm m$。

球形包络占去半径 $r$ 的空间，再预留间距 $\delta$，就得到中心点的通行条件：

$$d(p)>r+\delta=\rho.$$

本例中，阈值为 $0.35\,\mathrm m$。距离为 $0.60\,\mathrm m$ 的位置满足间距要求；距离为 $0.20\,\mathrm m$ 的位置落入中心点的禁行区域。

这里的距离针对原始障碍地图。如果已经按 $\rho$ 膨胀地图，便可以直接检查中心点是否进入膨胀区域。两种方式都可以计入机体尺寸，计算时需要明确所查询地图的含义。

距离信息也能帮助轨迹优化判断怎样移动曲线。在距离函数可微的位置，梯度描述距离增大的局部方向，优化器可以据此调整轨迹与障碍的间距。

2. TSDF 与 ESDF 的用途

截断符号距离场（TSDF）常用于融合深度观测和重建表面。它保留表面附近一定范围内的有符号距离信息，超出范围的距离受到截断。常见深度融合实现采用与观测方向相关的距离估计。

ESDF 关注到最近障碍表面的欧氏距离，更便于规划器查询净空和计算距离相关的代价。两者可以在建图系统中配合使用，例如由 TSDF 构建 ESDF。

实际距离场由离散地图计算或估计得到。使用距离值时，还需要读取观测有效性，并考虑地图分辨率、插值和测量误差。未知区域的处理应与占据地图采用的规划策略一致。

五、碰撞检测：从一个位置到整段运动

1. 检查一个候选位置

给定一个构型，碰撞检测判断该构型下的机体是否与障碍相交。

采用球形包络时，可以查询原始地图中的障碍距离；采用膨胀地图时，可以查询中心点是否位于可用区域。地图范围和观测状态也属于检查内容。

采用长方体等带朝向的模型时，需要把机体放到给定位置和姿态，再检查它与周围障碍的几何关系。

2. 检查两个位置之间的连接

路径通常由多个点及其连接组成。货架两侧的两个点都可能位于空闲区域，而连接它们的直线会穿过货架。线段碰撞检测负责识别这种情况。

从 $p_A$ 到 $p_B$ 的直线连接可以写成：

$$p(s)=(1-s)p_A+sp_B,\qquad s\in[0,1].$$

$s=0$ 对应起点，$s=1$ 对应终点，中间的取值覆盖整条线段。检查对象是这整段连接。

在占据栅格中，一种做法是遍历线段接触的所有格子，检查是否经过占据、未知或越界区域。处理规则取决于地图的可用空间定义，并要计入机体尺寸。

斜对角连接还涉及格子边角。例如，两个斜对角的空闲格之间，连接可能擦过相邻占据格的角点。按接触也计碰撞的规则，这样的连接需要排除。

3. 检查连续轨迹

对于曲线轨迹，需要沿实际曲线检查运动过程。平滑后的曲线可能在拐角处向内收缩，因此生成曲线后要重新验证它与障碍的关系。

一种常见做法是沿路径或时间取样，对采样位置逐个检查。采样间距决定了检查覆盖的细致程度：两个采样点相隔较远时，中间的细小障碍可能被漏过。按固定时间间隔采样时，飞行速度越高，相邻检查位置通常也越远。

更严格的验证可以采用连续几何相交检测、带保守界的曲线细分，或证明整段曲线位于已知自由区域内。

机体沿一段运动覆盖的全部空间，称为扫掠体积。对照障碍物检查这片体积，可以统一理解点、线段与曲线运动中的碰撞问题。

地图分辨率决定环境怎样被表示，碰撞检查分辨率决定一段运动怎样被检查，两者需要配合选择。

六、将这些信息交给路径规划算法

回到仓库中的通道，可以建立这样一个几何规划问题。

先将观测统一到地图坐标系，构建占据地图，并确定未知区域的处理方式。用覆盖机体的几何包络计入尺寸，再根据预留间距得到中心点的可用区域。

起点与终点先接受机体包络和地图状态的检查。随后，在可用区域内选取候选位置。对于栅格搜索，可以选择格子中心作为节点；对相邻节点生成连接，通过碰撞检测保留可行的边。

以路径长度作为目标时，直线连接的代价就是两个节点之间的欧氏距离。这样便得到了一个包含节点、可行连接和边代价的图。

地图格子是否可用，决定候选节点能否保留；连接过程是否可行，决定节点之间能否建立边。Dijkstra 与 A* 将在这个图上寻找连接起终点的低代价路径。

在连续空间中，也可以通过采样产生候选位置，再用同样的几何模型和碰撞检测建立连接。图搜索与采样规划由此共享了一套基础：对空间、机体和可行运动的明确描述。

小结

构型空间把机体的几何放置转化为规划变量，障碍物膨胀把机体尺寸转化为空间限制，占据地图和距离场提供环境查询，碰撞检测验证候选位置与整段运动。

这些部分共同定义了一个几何规划问题。下一篇《从 Dijkstra 到 A*》在这样的图上搜索通向目标的最短路线。