面向协同无人机的基于观测预定义轨迹的相对位姿估计优化方法

《Drones》：An Optimization-Based Method for Relative Pose Estimation for Collaborating UAVs Using Observed Predefined Trajectories Guven Cetinkaya and Yakup Genc

　　

在全球导航卫星系统（GNSS）拒止环境中，无人机（UAV）之间的精确相对位姿估计是协同导航、编队控制与集群运行的关键前提。在多无人机系统中，单目视觉由于重量轻、功耗低而具有吸引力；然而，仅依赖方位信息（bearing-only measurements）的观测方式容易引发角度歧义，尤其是在目标运动呈对称或共面（planar）特征时。本文提出了一种用于单目相对位姿估计的几何学框架，该框架利用已知运动模式的观测信息，而非依赖复杂的分布式系统架构。该方法通过将目标无人机在图像平面（image-plane）中的观测轨迹反投影（back-projection）到三维空间，并从相机中心沿几何参数化参考轨迹执行射线投射（ray-casting），从而利用轨迹诱导的几何约束。随后，采用Levenberg–Marquardt（LM，列文伯格–马夸尔特）非线性优化对相对位姿参数进行精化，使其在噪声条件下仍可实现高精度估计。除估计框架本身外，研究人员还通过仿真实验考察了协同轨迹几何形态对角度可观测性（angular observability）的影响。结果表明，尽管数值上能够收敛，共面协同运动仍可能诱发角度歧义；而通过三维轨迹引入适度的离平面激励（out-of-plane excitation）则可显著提升可观测性。除基于仿真的评估外，研究人员还开展了有限的真实飞行实验，以在实际传感条件下对观测到的歧义模式进行定性验证。尤其值得注意的是，三维八字形轨迹在不增加计算复杂度的前提下，能够显著抑制大角度离群误差并提升估计鲁棒性，从而为视觉基础航空场景中通过主动轨迹设计保证可观测性提供了经验证的指导。

该文发表于《Drones》，围绕GNSS拒止环境下协同无人机（UAV）之间的单目相对位姿估计问题，提出了一种以轨迹几何为核心的解析—优化融合方法，并系统讨论了目标运动轨迹对角度可观测性的决定性作用。研究背景在于，多无人机协同执行编队飞行、对接、重构与集群任务时，必须建立稳定可靠的相对参考系。然而，在室内、遮挡或复杂环境中，GNSS性能受限，传统运动捕捉系统又依赖昂贵基础设施，难以满足机动性与部署灵活性的要求。虽然单目视觉具备轻量化、低功耗优势，但只能提供方位观测，无法天然恢复深度与姿态，特别是在目标执行对称或平面轨迹时，容易出现多个不同姿态对应相似图像投影的几何歧义，从而导致数值优化虽收敛却得到错误解。基于此，研究人员开展了以“已知预定义轨迹”为先验的相对位姿估计研究，目标是在不依赖点特征对应、额外传感器或复杂分布式建图架构的条件下，仅利用图像中的目标轨迹恢复两机相对旋转与平移，并进一步明确何种协同轨迹设计能够改善单目可观测性。

本文提出的核心思想，是将目标无人机在观察者相机中的二维轨迹视为时序几何约束，将其反投影为从相机中心出发的三维射线，再与目标预先约定的参数化参考轨迹进行对齐，通过最小化射线与轨迹之间的几何误差求解初始相对位姿^Θ₀。研究人员区分了轨迹执行的运动项与静态未知坐标变换，其中R(t)与T(t)描述目标沿轨迹的时变运动，R₀与T₀则表示观察者坐标系与轨迹坐标系之间待估计的固定相对位姿。由于真实飞行中目标不能完美跟踪理想轨迹，研究进一步引入噪声项ε(t)表征导航误差与漂移。最终，研究人员采用Levenberg–Marquardt（LM，非线性最小二乘优化算法）在李群SE(3)上进行迭代优化，并结合多次随机初始化降低局部极小值风险。研究结论表明：纯平面八字轨迹会显著诱发角度歧义并产生重尾误差分布；拼接式非共面轨迹虽能部分缓解歧义，但仍存在残余尾部风险；持续三维激励的3D八字轨迹能够有效抑制大角度离群误差，在不增加计算复杂度的前提下显著提升相对位姿估计鲁棒性。这一结果的重要意义在于，它将轨迹设计由“被动输入”提升为“主动感知”组成部分，为多无人机在无基础设施、弱感知条件下实现高可靠视觉相对定位提供了轻量化思路。

从方法上看，研究人员主要采用了四类关键技术。其一，建立了基于针孔相机模型（pinhole camera model）的图像轨迹反投影模型，将二维观测点p(t)借助相机内参矩阵K变换为归一化视线方向；其二，构建了射线—轨迹几何误差模型，在轨迹坐标系内通过射线投射（ray-casting）与参考参数轨迹的最近关系定义残差；其三，使用Levenberg–Marquardt（LM）算法结合指数映射在SE(3)上进行非线性优化，以估计相对旋转和平移；其四，通过仿真与有限真实飞行实验开展验证。仿真部分采用合成数据环境、固定单目相机、随机相对位姿采样和高斯随机游走（Gaussian Random Walk）轨迹噪声模型；真实实验部分则使用地面固定相机向上观测四旋翼执行八字轨迹，图像轨迹通过人工初始化与前景分割获得，属于定性验证而非严格定量基准。

在研究结果部分，论文首先在“5.1. Synthetic Data Evaluation”中报告了基于合成数据的系统评估。研究人员在具有精确真值的仿真环境中重复进行了300次实验，在统一噪声水平、优化参数和终止条件下比较不同轨迹构型。结果显示，优化过程在扰动初值下总体能够稳定收敛，但不同轨迹的误差分布存在显著差异。平面八字轨迹（B8）表现出明显的角度歧义，尤其在深度相关方向上容易出现较大旋转误差；拼接八字轨迹（AB8）与三维八字轨迹（3D8）则提供了更好的可观测性与更稳定的收敛行为。这说明单目相对位姿估计的性能并不只受算法本身影响，更直接受目标运动几何激励程度支配。

在“Impact of target UAV trajectory geometry”部分，论文进一步聚焦轨迹几何对估计性能的影响。针对B8、AB8与3D8三种轨迹，研究人员通过相同实验条件进行隔离分析。结果总结指出，B8的旋转误差呈现显著重尾特征，说明即使优化数值收敛，共面激励仍不足以消除角度歧义。AB8通过在第二段轨迹中施加绕y轴旋转，为系统引入局部离平面激励，从而显著改善中位数性能并降低部分极端误差，但由于这种三维激励在时间和空间上并不持续，因此仍存在一定尾部误差。相比之下，3D8在整个运动过程中持续提供非平面激励，其旋转误差分布更紧凑，上分位数误差显著降低，中位数与P95之间差距缩小，表明歧义驱动的极端失败被有效抑制。平移误差也呈现类似趋势，只是程度较弱：平面运动的离散性最大，而引入离平面激励后，平移估计更稳定；AB8在平移中位数和最坏情况方面表现最低，3D8则相较B8仍有明显改善。研究人员据此强调，仅依靠中位误差不足以评价此类问题，必须结合重尾统计特征分析歧义风险。

在执行效率方面，论文指出不同轨迹在样本数和算法参数保持一致时，运行时间差异主要来自优化收敛行为而非输入规模变化。尽管AB8和3D8略微增加了迭代次数，但整体运行时间仍保持在实时可接受范围内，说明通过三维轨迹提升鲁棒性并不会引入额外计算负担。这一点支撑了论文“通过主动轨迹设计提升估计质量而非通过更复杂算法堆叠”的核心主张。

在“5.2. Qualitative Evaluation with Real-World Flights”部分，研究人员进行了有限真实飞行实验，以验证仿真中观察到的歧义模式与收敛特性是否在实际传感条件下依然存在。实验中，四旋翼无人机分别在15 m、20 m和25 m指令高度执行平面八字轨迹，地面单目相机固定朝上拍摄。由于真实相机到无人机的精确垂直距离、安装偏移和传感器同步误差无法完全获知，实验不具备严格真值，因此主要用于定性验证。结果显示，估计得到的垂向平移T?_z与指令飞行高度具有较强相关性，13.74 m、19.54 m和25.45 m的估计深度与15 m、20 m和25 m的指令高度变化趋势一致，偏差主要归因于无人机内部高度估计漂移。此外，优化还稳定恢复出非零横向平移分量T?_x与T?_y，且不同飞行架次之间存在差异，这与图像平面中轨迹质心位置变化一致，说明恢复出的横向位移反映了真实相对运动，而非优化伪影。代表性实验还表明，从故意设置的不准确初值出发，优化后图像重投影误差降低了一个数量级以上，投影轨迹与观测点显著贴合，验证了方法在实际场景中的收敛有效性。

讨论部分系统总结了轨迹几何、角度可观测性与估计鲁棒性之间的关系。研究人员指出，共面或近共面运动会使多个相对朝向产生几乎相同的图像投影，从而在旋转自由度间产生耦合与退化，这种问题在视觉相对位姿估计中具有经典性。平面轨迹导致的并非简单随机噪声，而是由几何不可辨识性引发的偶发但严重失效，因此会在误差统计中表现为明显重尾。三维激励通过让目标穿越更多深度层与视角配置，增加了独立几何约束，改善了问题的数值条件。AB8虽能部分约束旋转，但其激励有限，因此残余歧义未被彻底消除；3D8则因在全程维持三维激励，而显著压缩误差分布并抑制离群值。

在“6.1. Rotation–Translation Coupling”部分，论文进一步讨论了旋转—平移耦合机制。研究人员指出，旋转误差会直接改变反投影射线方向，从而放大深度与尺度不确定性；因此，改善旋转可观测性会间接促进平移估计。与此同时，一旦主要旋转歧义得到解除，平移精度对额外三维激励的敏感度便下降，这解释了为何AB8与3D8在平移中位误差上相近，而3D8在旋转鲁棒性上更具优势。

在“6.2. Implications for Active and Collaborative Motion Design”部分，研究人员将结论提升至主动感知（active perception）与协同运动设计层面。论文认为，轨迹不应被视作单纯输入，而应被纳入相对位姿估计系统设计本身。通过有意识地设计目标运动，使其产生更丰富的离平面激励，可以在不增加传感器、不增加基础设施和不过度提高算法复杂度的情况下，提高单目相对定位的可靠性。这一观点对于多无人机协同感知具有直接方法论意义，说明运动设计本身能够成为感知可观测性增强手段。

对于局限性，论文明确指出该框架建立在协同场景假设之上，即目标轨迹需预先定义并与观察者共享，且实验中默认观测同步、噪声模型受控。仿真虽采用高斯随机游走刻画视觉—惯性里程计（VIO）式漂移，但尚未显式涵盖风扰、下洗等复杂动力学因素；若观察者自身运动，也会引入额外估计噪声。作者同时说明，当前给出的轨迹设计未被证明为全局最优。未来工作方向包括：利用深度学习为优化过程提供更稳健的粗初始化；对系统Jacobian（雅可比矩阵）进行严格秩分析，以形式化描述3D轨迹对可观测性的改善；研究异步感知、局部遮挡、动态环境与移动观察者条件下的鲁棒性；采用强化学习探索更优轨迹形状；将框架扩展至更大规模多智能体系统与自适应轨迹规划；并使用高精度RTK-GNSS开展更大规模户外定量验证。

论文结论部分可译为：本文提出了一种基于轨迹反投影与射线投射（ray-casting）的无人机单目相对位姿估计几何—优化框架。该方法通过利用协同运动目标无人机的图像平面轨迹，并将其与已知参数化运动模型对齐，在不依赖逐点特征对应或额外传感模态的条件下，实现了相对旋转与平移的准确估计。除估计框架本身外，本文的核心贡献在于对轨迹诱导角度可观测性的系统分析。大规模仿真实验表明，纯平面目标运动会导致严重的角度歧义，即使数值收敛，旋转误差分布仍呈重尾；通过拼接式运动引入有限离平面激励可部分缓解该问题，而通过三维八字轨迹实现的持续三维激励则能够有效抑制由歧义驱动的尾部误差。三维运动条件下中位数与高分位旋转误差的接近，凸显了轨迹几何在保障单目相对位姿估计可靠性中的关键作用。结果还表明，旋转可观测性的提升会间接改善平移估计，但在主要旋转歧义解除后，平移精度的增益呈现递减趋势。这些发现说明，仅使用中位误差不足以评价估计可靠性，必须同时考虑误差分布特征。重要的是，所提出的三维轨迹设计在不增加计算成本的情况下提升了估计鲁棒性并抑制了歧义离群值。从更广泛的角度看，本研究支持多无人机系统中的主动协同感知范式，即不将运动视为被动输入，而是通过刻意的轨迹设计来提升可观测性与鲁棒性，为协同航空场景提供了一种轻量且可扩展的替代定位方案。