Implicit Swept Volume SDF 附录翻译

A 优化实现细节

对于轨迹生成和优化，我们选择了一个五次多项式。

平滑性项：我们通过最小化轨迹的三阶导数的积分来确保轨迹的平滑性。

$$\begin{array}{rl}{J}_m& ={\int }_0^{T_i}{\Vert p_i^{(3)}(t)\Vert }^{2}dt,\\ \frac{\partial J_m}{\partial c_i}& =2\left({\int }_0^{T_i}{\beta }^{(3)}(t){\beta }^{(3)}(t{)}^{\mathrm{T}}dt\right)c_i,\\ \frac{\partial J_m}{\partial T_i}& =c_i^{\mathrm{T}}{\beta }^{(3)}(T_i){\beta }^{(3)}(T_i{)}^{\mathrm{T}}{c}_i.\end{array}$$

时间惩罚项：我们通过最小化总时间来提高轨迹的激进性。

$$J_t=\sum _{i=1}^N{T}_i,\frac{\partial J_t}{\partial c_i}=0,\frac{\partial J_t}{\partial T_i}=1.$$

动态可行性项：我们以多旋翼无人机的动力学为例，说明轨迹生成方法的细节。为了确保动态可行性，我们对轨迹的导数进行约束。这包括在整个轨迹上限制速度、加速度和急动度的大小，这一原则也适用于其他类型机器人的动力学。

$$\begin{array}{rl}{\mathcal{G}}_d(t)& ={\lambda }_v{\mathcal{G}}_v+{\lambda }_a{\mathcal{G}}_a+{\lambda }_j{\mathcal{G}}_j,\\ {\mathcal{G}}_v& =\dot{p}(t{)}^2-v_m^2,\\ {\mathcal{G}}_a& =\ddot{p}(t{)}^2-a_m^2,\\ {\mathcal{G}}_j& =\ddot{v}(t{)}^2-j_m^2.\end{array}$$

其中$v_m$、$a_m$和$j_m$分别是允许的最大速度、加速度和急动度，${\lambda }_v$、${\lambda }_a$和${\lambda }_j$是相应的权重。对应的梯度为：

$$\frac{\partial Q_x}{\partial {\mathbf{c}}_i}=2{\beta }^{(n)}(t)p^{(n)}(t{)}^{\mathrm{T}},\frac{\partial Q_x}{\partial T_i}=2{\beta }^{(n+1)}(t{)}^{\mathrm{T}}{\mathbf{c}}_i{p}^{(n)}(t),$$

其中x={v,a,j}x = \{v, a, j\}x={v,a,j}，n={1,2,3}n = \{1, 2, 3\}n={1,2,3}。

位置残差项：

$${\mathcal{G}}_p(t)={\mathcal{L}}_{\mu }\left[\Vert p(t)-p^{i(t)}{\Vert }^2\right],$$

$$\frac{\partial {\mathcal{G}}_p}{\partial {\mathbf{c}}_i}=2{\dot{\mathcal{L}}}_{\mu }\beta (t)(p(t)-p^{i(t)}{)}^{\mathrm{T}},\frac{\partial {\mathcal{G}}_p}{\partial T_i}=2{\dot{\mathcal{L}}}_{\mu }{\beta }^{(1)}(t{)}^{\mathrm{T}}{\mathbf{c}}_{i}p(t),$$

其中${\dot{\mathcal{L}}}_{\mu }$可通过对第4.1节中公式(23)求导得到。

姿态残差项：

$${\mathcal{G}}_R(t)={\mathcal{L}}_{\mu }\left[{\Vert R(t{)}^{-1}{R}^{i(t)}-I\Vert }_F^2\right]$$

其中${\Vert R(t{)}^{-1}{R}^{i(t)}-I\Vert }_F^2$可简化为$\mathrm{tr}(6-2R(t{)}^{-1}{R}^{i(t)})$。

为了优化目的，我们使用归一化的四元数$\mathbf{q}=[w,x,y,z{]}^{\mathrm{T}}$来表示旋转。对应的旋转矩阵$R$定义为：

$$R=\left[\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right].$$

特别地，$R^{-1}(t)=R(t{)}^{\mathrm{T}}$。利用这一性质，我们可以轻松计算旋转矩阵$R(t{)}^{-1}$关于四元数元素$w,x,y,z$的偏导数。最终的梯度$\frac{\partial {\mathcal{G}}_R}{\partial {\mathbf{c}}_i}$和$\frac{\partial {\mathcal{G}}_R}{\partial T_i}$可以通过结合 [Faessler et al. 2017] 中的微分平坦性属性以及链式法则得到。

避障项：

$$J_o(t)={\mathcal{L}}_{\mu }\left[s_{\mathrm{thr}}-\mathrm{SVSDF}(p)\right],$$

其中$s_{\mathrm{thr}}$是安全阈值。对应的梯度与 [Zhang et al. 2023] 中研究人员使用的技术一致。

为了得到一个较为准确的近似值，可以方便地计算时间积分惩罚$J^{\star }$及其梯度：

$$\begin{array}{rl}{\mathbf{I}}_i^{\star }& =\frac{T_i}{\kappa }\sum _{j=0}^{k_i}{\bar{\omega }}_j{\mathcal{G}}_{\star }\left(\frac{j}{\kappa }{T}_i\right),{\mathcal{J}}^{\star }=\sum _{i=1}^N{T}_i^{\star },\\ \frac{\partial {\mathcal{J}}^{\star }}{\partial {\mathbf{c}}_i}& =\frac{\partial {\bar{\mathbf{I}}}_i^{\star }}{\partial {\mathcal{G}}_{\star }}\frac{\partial {\mathcal{G}}_{\star }}{\partial {\mathbf{c}}_i},\\ \frac{\partial {\mathcal{J}}^{\star }}{\partial T_i}& =\frac{\partial {\mathcal{I}}_i^{\star }}{\partial T_i}+\frac{\partial {\mathcal{I}}_i^{\star }}{\partial {\mathcal{G}}_{\star }}\frac{\partial {\mathcal{G}}_{\star }}{\partial T_i}=\frac{{\bar{\mathbf{I}}}_i^{\star }}{T_i}+\frac{j}{\kappa }\frac{\partial {\mathcal{I}}_i^{\star }}{\partial {\mathcal{G}}_{\star }}\frac{\partial {\mathcal{G}}_{\star }}{\partial t_{\star }}.\end{array}$$

其中$\left({\bar{\omega }}_0,{\bar{\omega }}_1,\dots ,{\bar{\omega }}_{{\kappa }_i-1},{\bar{\omega }}_{{\kappa }_i}\right)=\left(\frac{1}{2},1,\cdots ,1,\frac{1}{2}\right)$是梯形法则的系数 [Press 2007]。$i=\left(1,2,\cdots ,N\right)$表示第$i$段，$j=\left(1,2,\cdots ,\kappa \right)$。我们将原始问题重新表述为一个无约束的非线性优化问题，并在获得上述优化变量的必要梯度后，使用 L-BFGS 算法 [Liu and Nocedal 1989] 高效求解。

B 消融研究

为了验证我们分层规划器各个组成部分的有效贡献，我们在三维场景中进行了消融实验。实验在两种不同环境中进行：一种是随机障碍物环境（Random Obstacles），另一种是墙壁上有缝隙的环境（Wall with gaps）。对于每种场景，我们比较了分层规划器的不同组成部分被替换时的规划结果。总共进行了四种类型的实验：1）使用标准A算法（在${\mathbb{R}}^3$空间中）代替非对称A作为前端；2）没有中间端；3）在后端使用离散采样评估方法（如 [Geng et al. 2023; Wang et al. 2022] 中的方法）代替我们的连续SVSDF；4）使用包含所有组成部分的完整分层规划器。消融实验的结果如图2所示。非对称A*算法在导航复杂环境时表现出色，尤其是在存在类似墙壁的结构时。当机器人需要在由三面墙壁包围的狭窄缝隙中连续机动时（Wall with gaps），这一点尤为明显。由于CCA（连续碰撞避免）问题的高度非凸性，优化器在没有良好初始路径的情况下，通常难以找到无碰撞的轨迹。当不同形状的机器人在越来越复杂的环境中运行时，这种复杂性被进一步放大，这也凸显了前端的重要性。中间端主要帮助减轻后端优化的负担，尽管它对轨迹质量有一定影响，但不如前端显著。当障碍物位于初始轨迹形成的SV（扫掠体积）内部时，基于SVSDF的后端是不可或缺的。其对CCA的有效梯度贡献帮助SV高效地避开障碍物。从前端到中间端，再到后端，每个部分在任意形状机器人的连续无碰撞规划中都发挥着不可或缺的作用，每个组成部分对整个框架的有效性都至关重要。

我们运行了蒙特卡洛实验，计算机器人在其最终轨迹上沿最近障碍物的平均最小有符号距离。这一指标用于评估避障的有效性，具体是SV与障碍物之间的最小距离，正值表示有间隙，负值表示碰撞。我们还评估了CCA成功率，定义为SVSDF返回正值的试验次数与总试验次数的比率。

消融研究的统计结果如图2所示，清晰地表明，使用标准A算法作为前端无法在复杂场景中实现无碰撞规划。这并不令人意外，因为标准A算法没有考虑机器人的形状和姿态信息。因此，它生成的前端轨迹质量较低，使得优化器难以在这一高度非凸问题中找到满足约束的轨迹。移除中间端对轨迹质量有一定影响，但由于我们仍然有基于SVSDF的后端和考虑任意形状机器人的前端，轨迹的避障性能优于使用标准A*替换前端的情况。此时，我们观察到后端优化的迭代次数增加，轨迹质量略有下降。因此，我们可以得出结论，中间端的主要作用是减轻后端的优化负担。当使用离散后端时，在复杂场景中实现CCA也具有挑战性。离散后端评估方法可能导致优化器忽略可能导致碰撞的障碍物，从而生成穿过障碍物的轨迹。此外，由于缺乏连续碰撞评估，机器人的复杂形状可能导致避障梯度出现振荡，从而导致CCA的成功率相对较低。

C GSIP收敛性证明

在研究广义半无限规划（GSIP）时，离散化方法的收敛性起着关键作用。我们的证明基于以下原理 [López and Still 2007]：

引理：在连续函数和$Y(x)$的紧致性假设下，如果$Y_k$和$Y$之间的Hausdorff距离随着$k\to \infty$趋于零，则近似解序列将收敛到GSIP的解。

证明：在二维情况下，$Y$是一个圆，而在三维情况下，$Y$是一个球体。离散集合$Y_k$由在给定分辨率${ϵ}_{\text{res}}$下均匀分布在圆（二维）或球体（三维）内的离散点组成。$Y_k$和$Y$之间的Hausdorff距离小于半径$r_k$和$r_{\text{SVSDF}}$之间的差值。我们的算法可以实现任意精度要求$ϵ$，即$r_{\text{SVSDF}}-r^{\ast }\le ϵ$，其中$r^{\ast }$是最终迭代输出。只要在最终迭代中，圆周/球面上任意两个均匀分布的采样点之间的距离小于$ϵ/2$即可。鉴于评估函数$g$的空间连续性，从离散点采样评估的$g^{\ast }$与实际非离散LP($\cdot$)解之间的差异在$ϵ$以内。因此，从离散方法得到的解$r_k$与真实值$r_{\text{SVSDF}}$之间的差异也在$ϵ$以内。以二维为例进行说明，我们只需设计一个均匀分布点的采样策略，具体是一个均匀分布分辨率${ϵ}_{\text{res}}$的更新策略。简单地应用${ϵ}_{\text{res}}^k=\max ({ϵ}_{\text{res}}^{k-1}/2,ϵ/2)$即可确保采样密度增加并满足收敛时的精度要求。在三维场景中，我们可以设计类似的采样策略。

因此，我们基于离散化方法的算法（第3.2节）具有理论上的收敛保证，该方法受到 [Blankenship and Falk 1976; Remez 1962] 的启发。