摘要
已有单目视觉引导中平台间位姿高精度测量方法需要准确的目标平台3D模型，无法消除3D模型误差给位姿测量带来的影响。针对此问题，对目标平台3D模型和位姿进行迭代优化，并提出一种新的单目视觉测量方法：采用稀疏3D关键点集合建模目标平台3D模型，利用序列图像中多视图几何约束信息，将目标稀疏3D关键点集合和6D位姿作为待求解参数，以最小化物方残差建立目标函数，通过求解该最优化问题，迭代优化稀疏3D关键点集合及位姿，通过采用滑动窗口结合关键帧筛选策略，实现实时、在线的高精度单目视觉测量。实验结果表明，通过迭代优化稀疏3D关键点集合及位姿，所提方法实现了目标平台3D模型不准确条件下实时、在线的高精度单目位姿测量，同时提升了目标3D模型精度。
Abstract
In monocular vision-guided high-precision inter-platform pose measurement, existing methods require an accurate 3D model of the target platform and are unable to eliminate the impact of 3D model errors on pose measurement. To address this issue, iterative optimization was performed on the 3D model of the target platform and pose, and a new monocular vision measurement method was proposed. Specifically, the target platform′s 3D model was modeled using a set of sparse 3D keypoints. By leveraging multi-view geometric constraint information in sequential images, the sparse 3D keypoint set of the target and 6D pose were treated as parameters to be solved. An objective function was established to minimize object-space residuals, and through solving this optimization problem, iterative optimization of the sparse 3D keypoint set and pose was achieved. Additionally, a sliding window combined with a keyframe selection strategy was adopted to realize real-time and online high-precision monocular vision measurement. Experimental results demonstrate that, through iterative optimization of the sparse 3D keypoint set and pose, the proposed method achieves real-time, online high-precision monocular pose measurement under the condition of an inaccurate 3D model of target platform, while simultaneously improving the accuracy of the target′s 3D model.
单目视觉引导仅利用平台搭载的单个视觉测量相机完成平台间相对6D位姿参数（3D旋转和3D平移）精确测量[1-3]，具有配置简单、成本低、自主性高、抗干扰能力强等优势，在飞行器着舰、空间平台交会对接等双动平台交会场景中具有广阔应用前景，得到了研究人员的广泛关注。
单目位姿测量是单目视觉引导中的关键技术，当前基于单目视觉的位姿测量方法依据是否使用合作标志可主要归结为合作和非合作两类方法：合作方法通过在目标平台上布设合作标志实现平台间位姿测量[4-7]，算法简单、处理效率高，但应用场景较为受限。非合作方法无须布设合作标志，利用目标自身特征与图像信息完成平台间位姿参数解算，硬件配置简单、应用限制少，但可靠特征提取、定位难度大，是当前单目位姿测量领域研究的热点。已有基于非合作模式的位姿视觉测量方法可进一步分为传统方法和深度学习相关方法。传统方法通过检测图像中目标自身点、直线、轮廓等特征，并结合目标3D先验信息，实现位姿求解[8-10]，此类方法难以处理弱纹理目标，且易受复杂环境干扰等影响。得益于深度神经网络强大的特征提取与表征能力，深度学习相关方法已成为当前单目位姿测量的主流方法，并在多个公开测试数据集上取得了优于传统方法的性能。已有基于深度学习的位姿视觉测量方法依据网络输出形式可归结为直接法和间接法[11]。直接法[12-14]获取输入图像并通过回归网络直接得到6D位姿参数，实现了端到端的网络训练与推理，但位姿测量的精度不高[15]。间接法引入目标的中间模型，结合迭代最近点（iterative closest point，ICP）、透视n点（perspective-n-point，PnP）等算法求解位姿参数。常用的中间模型包括稠密模型、归一化坐标和稀疏关键点。基于稠密模型的位姿测量方法匹配图像或场景中的每个像素[16-18]，具有较高的精度和稳定性，但计算效率较低。基于归一化坐标的方法通过归一化对象坐标空间将同类别物体进行归一化，无须获取目标的精确3D模型[19-22]，但是依赖深度图像去恢复物体的真实尺度。稀疏关键点表示方法将目标建模为稀疏关键点集合，此类方法简单高效，适用于实时位姿测量任务[23-28]，当前，基于稀疏关键点集合的方法已被广泛使用。相较于直接法，间接法通过建立稀疏或稠密的2D-3D对应关系，并求解PnP问题，能够获得更加精确的位姿测量。已有单目位姿测量方法均依赖准确的目标3D先验信息，通过建立2D-3D对应或3D-3D对应解算位姿参数，无法实现3D模型不准确情况下的高精度位姿求解。然而，在实际应用中，准确的目标3D模型可能难以获取，如对于飞行器、卫星等敏感目标，此外，部分交会对象由于加工等因素也会出现理论设计3D模型和目标实物不符的情况，难以保证目标3D模型的准确性。针对目标3D模型不准确条件下的高精度位姿测量，当前尚无有效的解决方法。
多视图几何约束在结构重建中有着广泛的应用，其利用图像间的内在关系恢复物体或场景的三维结构，并同时计算相机位姿。例如，同步定位和建图[29-32]（simultaneous localization and mapping，SLAM）、运动恢复结构[33-36]（structure from motion，SFM）、目标定位[37]和飞行器自身高精度定位[38]等。类似地，在单目位姿测量中，由于相对运动的存在，图像序列提供了多视图间的几何约束。因此，本研究尝试使用多视图几何约束来处理缺少精确的目标3D模型的单目位姿测量问题。传统的SLAM和SFM等方法均利用特征点提取和匹配建立帧间匹配，通过优化求解位姿进行定位。然而，目标3D平台无法基于传统特征点建立对应关系，此外，即使成功建立特征点对应关系，也难以将其与具体的3D模型进行关联。因此，复杂的特征点提取和匹配方法被放弃，转而利用现有的基于神经网络的关键点检测算法直接回归与预定义3D关键点对应的2D投影。
综上所述，单目图像序列内部的多视图几何约束被运用来解决上述问题，在此基础上提出了一种新的单目位姿测量方法，对目标的3D模型和位姿进行迭代优化。与人体姿态估计中使用的关键点类似，使用一组稀疏关键点来表示目标的3D模型，并使用训练好的神经网络从输入图像中检测关键点，结合2D-3D对应关系，通过求解PnP问题得到初始位姿估计。将以线性参数方程形式表示的3D关键点和位姿作为待优化参数，最小化3D关键点和观测视线之间的物方残差来构建优化目标函数，通过求解该优化问题，对目标的3D模型和位姿进行迭代优化。所提方法在目标3D模型不准确的情况下，也能得到高精度的位姿测量结果。此外，滑动窗口结合关键帧筛选策略被采用以实现实时、在线的高精度单目位姿测量。
1 方法
为在目标3D模型不准确的情况下实现可靠的单目位姿测量，提出一种利用图像序列内多视图几何约束的迭代优化方法。需要说明的是，所描述的方法以飞行器着舰为应用背景，但同样适用于其他平台交会场景中的视觉引导任务。
1.1 问题公式和符号
表示从物体坐标系到相机坐标系的刚体变换。
(1)
式中，和分别是的旋转和平移分量（为书写方便，后文简写为R，简写为t），SE（3）为特殊欧氏群，SO（3）为特殊正交群，为三维向量空间。相机内参矩阵K被预先标定且为一固定不变值，如式（2）所示。
(2)
目标3D模型的顶点VO在二维图像上的透视投影被建模为：
(3)
式中，为VO的齐次坐标形式，x=[u v]T为二维图像点，并且π（X）=[x/z y/z]T。
传统上，单目位姿测量提取图像中的2D特征，结合目标的精确3D信息建立2D-3D对应关系并进行高精度6D位姿求解。然而，在目标3D模型不准确时，传统方法无法利用准确的2D-3D对应获得高精度位姿。因此，本研究旨在解决3D模型不准确情况下的单目位姿测量问题。
1.2 3D模型与位姿迭代优化的单目位姿测量
提出使用一组稀疏3D关键点来表示目标的3D模型，通过检测到的2D关键点结合求解PnP问题得到初始位姿，并通过光束法平差（bundle adjustment，BA）对位姿和3D模型进行迭代优化。该迭代优化方法的整体架构如图1所示，所提出的单目位姿测量方法主要包括基于稀疏关键点的3D模型表示、初始位姿估计、3D模型与位姿的迭代优化、滑动窗口和关键帧提取。为简洁起见，图1中仅显示部分用红点标注的关键点。
图1本文方法的整体架构
Fig.1Overall architecture of the proposed method
1.2.1 基于稀疏关键点的3D模型表示
以飞行器着舰视觉引导中的位姿测量为例说明所提方法，并以图2所示的舰船3D模型为目标平台。图2为对舰船简化物理模型进行精确三维扫描得到的3D模型。为了使所提出的迭代优化方法高效可靠，提出使用一组如图2中红点所示的稀疏3D关键点={[xOj yOj zOj 1]T}j=1，2，···，N表示目标的3D模型，其中N为13。与传统SLAM、SFM等领域使用的2D特征点不同，采用语义级关键点来表示目标的3D模型。为减少所提方法中要优化的参数数目，采用线性参数方程形式表示3D稀疏关键点集合。在后续描述中，所提到的目标3D模型即指建立的目标3D关键点集合，所提方法不再对两者进行区分。
1.2.2 初始位姿测量
在初始帧中利用一瞥即识目标检测模型的扩展版（you only look once extended， YOLOX）算法[39]检测目标区域，为了提高效率，在获得初始帧的目标区域后，利用模板与逐像素学习器的集合（sum of template and pixel-wise learners， STAPLE）算法[40]从后续的输入图像中跟踪目标。基于1.2.1节提出的稀疏关键点集合的表示形式，借鉴在人体姿态估计任务中表现优异的实时多人姿态估计（real-time multi-person pose estimation， RTMPose）算法[41]，实现图像中目标关键点的高效准确检测。RTMPose算法通过简单的坐标分类输出2D关键点的位置，简洁、高效且易于部署。最后，采用高效透视n点（Efficient Perspective-n-Point，EPnP）+随机抽样一致性（random sample consensus，RANSAC）方法求解初始位姿。
图2目标3D模型和关键点标注
Fig.23D model and keypoint annotation of target
1.2.3 3D模型和位姿的迭代优化
基于上述得到的初始位姿测量结果，建立优化目标函数，通过最小化物方残差，迭代优化目标3D模型和位姿。
所提出的优化方法需要处理大量的数据和计算过程，参数过多可能导致计算时间过长，同时会使解空间过于复杂，使得优化算法难以收敛到全局最优解。因此，如图3所示，使用线性参数方程形式表示目标3D关键点的坐标，以减少优化目标函数中待优化参数的数目。图3中，ek，eM分别为点P′与和之间的空间距离。
图3基于线性参数方程形式的3D关键点表示和空间同名视线交会
Fig.33D keypoint representation based on linear parametric equation and the intersection of corresponding sight line in space
如图3所示，P为一个表示目标3D模型的待优化的初始关键点，和分别为初始帧，第k帧和第M帧中关于点P的观测视线。在BA中，初始帧的位姿通常作为固定参考的真实位姿。因此，在不考虑首帧2D关键点观测误差的情况下，认为关键点P真实位于上。进一步，如图3所示，将关键点P投影到上，记为P′，并以参数方程形式表示P′。
(4)
其中，[l m n]为的方向矢量，[x0 y0 z0]为上任意一点，q为确定关键点P′位置的参数。通过式（4），单个3D关键点的表示由三维参数[x y z]简化为一维参数q，从而减少了需要优化的参数，同时增加了对目标3D关键点优化方向的约束。
在理想情况下，不同视角的同一关键点的观测视线应相交于空间中同一点，该点即为3D关键点的真实位置。因此，如图3所示，将传统位姿与模型优化中基于最小化重投影残差的优化目标修改为基于最小化多视图内空间同名视线交会残差的优化目标。然而，直接计算多个视线的交会残差较为复杂，且后续优化过程中难以确定目标3D关键点的位置。因此，如图3和图4所示，计算P′与剩余同名观测视线间的物方残差，并利用该物方残差间接表示同名视线间的交会残差。
图4空间物方残差
Fig.4Object-space residual
图4中的物方残差e的计算如式（5）所示。
(5)
其中，为2D关键点观测视线与归一化平面的交点。进一步，提出如下优化目标函数：
(6)
其中，Q={qj}j=1，2，···，N为线性参数表示形式的3D关键点集合，r（qj）为基于参数qj求解第j个待优化3D关键点的三维坐标。通过式（4）和式（6），优化目标中待优化参数的个数由传统优化方法中的6（M-1）+3N减少为6（M-1）+N。同时，对于单个3D关键点，仅需要一个观测方程，因此观测方程的个数由MN减少为N。
由于位姿与模型在优化过程中约束不足，且关键点的误差范围通常较小，采用阈值σ1限制3D关键点的优化范围，防止关键点优化增量过大：若初始关键点与优化后的关键点间的欧氏距离大于σ1，则放弃关键点优化结果，如式（7）所示。
(7)
其中，q*prev为上一次优化中关键点的优化结果。在实际应用中，σ1通常根据预估的3D模型误差大小来设置。
位姿与模型参数同步进行优化通常会导致优化参数的约束较少和优化参数数量过多，难以得到最优解。因此，采用位姿与模型的迭代优化，如式（8）所示。
(8)
式（8）中，在每一次优化时，首先固定目标的3D关键点参数Q作为参考，并对6D位姿（R，t）进行优化，随后将被优化的位姿（R*，t*）再次代入优化目标函数中并保持固定不变，对目标3D关键点进行优化得到Q*。
1.2.4 滑动窗口结合关键帧筛选策略
单目视觉引导需要实时、在线和连续的位姿测量。然而，连续图像的数量会随着时间的推移而增加，此外，在建立的优化目标函数中，残差和位姿优化参数也在不断增加。因此，采用滑动窗口策略约束优化规模，使所提方法只在固定长度L的时间窗口内优化图像的位姿。此外，时间窗口外的位姿保持固定不变。
同时，为了更有效利用多视图间几何约束信息，提高处理效率，采用关键帧的处理策略：在序列图像中选取关键帧，利用关键帧序列迭代优化目标平台3D模型和位姿，针对非关键帧，依据优化得到的目标平台3D模型，通过求解PnP问题，实现高精度位姿求解。参考文献[27]中的关键帧选取策略，根据相机转动的角度大小进行关键帧筛选。
通过采用滑动窗口结合关键帧筛选策略，有效降低了优化过程中的计算规模和冗余，将式（8）修改为：
(9)
其中，下标keyi表示第i个关键帧。
为了确保位姿测量的准确性，在迭代优化后，使用被优化的目标3D模型再次求解PnP问题从而计算当前帧位姿。除首个关键帧外，滑动窗口内其余经过优化的关键帧和新加入的关键帧被用于下一次优化。
2 实验结果与分析
为了验证所提出方法的性能，首先对所提出方法的鲁棒性进行测试分析，随后在仿真数据中对所提出方法进行验证，并与当前部分单目位姿估计方法进行对比。
2.1 实验设置
2.1.1 参数设置
在鲁棒性测试及仿真对比实验中，将模型优化更新阈值设置为模型预估误差，滑动窗口长度设置为20帧。本文方法主要采用C++语言编程实现，所有实验的计算机配置为3.2 GHz AMD Core R7-5800H CPU、RTX 3060 GPU、16 GB RAM。
在典型参数设置下，得益于关键帧筛选和滑动窗口处理策略，本文方法在单帧上的平均处理耗时约为32 ms，其中目标跟踪耗时约为4 ms、关键点检测耗时约为23 ms、优化求解耗时约为4.5 ms、PnP问题求解耗时约为0.15 ms，因此满足实时应用需求。特别说明，对于首帧输入图像，采用目标检测算法定位图像中的目标位置，目标检测耗时长于目标跟踪耗时，约为30 ms，首帧图像处理耗时约为53 ms。
2.1.2 评价指标
采用旋转误差ER和归一化平移误差ET来评价6D位姿中旋转量和平移量的测量精度，并采用3D关键点误差均值EL来评价目标3D模型精度，如式（10）所示。
(10)
其中，Rg和tg分别为旋转和平移分量的真值，Rp和tp分别为旋转和平移分量的估计值，Lg和Lp分别为稀疏3D关键点的真值和估计值，L2为单个关键点误差。
2.2 鲁棒性测试
采用基于多视图几何约束的迭代优化方法来处理缺乏精确3D模型的位姿估计问题。迭代优化方法的效果主要取决于2D关键点检测的准确性和目标3D模型的误差。因此，对所提方法在不同的2D关键点噪声和目标3D模型误差下的表现进行测试。
本节模拟飞行器搜索舰船目标、绕飞调整位姿等过程，并同步生成测试数据和对应的位姿真值。为便于控制3D模型误差程度，依据均匀分布U（a，b）为每个3D关键点在空间的三个维度上分别添加a和b之间的误差。为了模拟2D关键点检测误差，向测试数据中的2D关键点加入图像处理领域常用的高斯误差N（0，σ2）。N（0，σ2）表示加入2D关键点上的高斯观测误差的均值为0，标准差为σ2。随后，将不同水平误差的2D关键点和3D关键点样本成对组合，形成不同的仿真测试数据，如表1所示。
表12D关键点和3D关键点添加误差的参数设置
Tab.1Parameter settings for errors added on 2D keypoints and 3D keypoints
依据式（10）中的误差度量，图5分别给出了多组不同水平误差组合下完整图像序列的位姿旋转误差曲线、平移误差曲线以及目标3D模型误差。测试序列长度为200帧。
在图5中，初始帧的位姿被设置为固定的真实参考值，且关键帧的数量达到滑动窗口长度时才输出位姿优化结果。因此，图5（a）和图5（b）中黄色曲线的前半部分与紫色曲线的前半部分重合。由于目标平台的3D模型和2D检测关键点存在误差，传统的基于直接求解PnP问题的方法无法在整个图像序列上得到准确的位姿旋转和平移测量结果。所提方法利用多视图的几何约束信息迭代优化目标3D模型和位姿参数，能够有效优化具有不同2D-3D误差水平的全图像序列位姿，实现目标3D模型不准确情况下高精度的位姿测量。此外，图5（c）表明，本研究不仅在目标3D模型不准确的情况下实现了高精度的单目位姿测量，而且能够有效减小3D模型的误差。
图5测试序列中的位姿及3D模型误差
Fig.5Pose and 3D model errors in test sequences
接下来，表2给出不同误差组合中所有测试数据的实验结果的均值，其中最小误差以粗体突出显示。表2中的EPnP方法表示直接通过2D-3D对应求解PnP问题得到位姿测量结果。
表2实验数据不同误差组合下的相对位姿与3D稀疏关键点误差均值
Tab.2The average error of relative poses and 3D sparse keypoints under different error combinations on experimental data
如表2所示，目标3D模型误差固定时，2D检测关键点误差越小，所提方法的优化效果越显著。这主要是因为本文方法的实质是通过最小化空间同名视线交会残差来对位姿和3D模型进行迭代优化，而2D检测关键点误差会导致观测视线出现偏差，进而影响后续视线交会的计算。因此，本研究需要2D检测关键点误差小于3D关键点重投影误差，否则无法对3D模型进行有效优化并提升位姿测量的精度。
2.3 仿真图像序列中的实验结果与分析
本节选择仿真图像序列进行实验，仿真图像通过目标3D模型与计算机图形引擎结合生成。具体地，采用视景仿真工具BlenderProc[42]加载舰船3D CAD模型。如图6所示，在仿真环境中，通过设定相机轨迹并进一步渲染生成相应的图像数据。
不同视角下的仿真测试样本如图7所示。此外，在本节实验中，通过均匀分布U（2，3）为3D模型添加误差，并随机生成1 000组样本进行测试。
图6仿真相机轨迹可视化
Fig.6Synthetic camera trajectories visualization
图7不同视角下的仿真测试样本
Fig.7Synthetic test samples under different viewing angles
现有方法通常采用稀疏或密集的2D-3D对应关系，并直接求解PnP问题计算位姿。因此，分别采用当前先进的RTMpose和飞桨轻量级姿态估计（paddle paddle tinypose， PP-TinyPose）方法[43]来检测2D关键点。将所提方法与RTMPose-EPnP和PP-TinyPose-EPnP两种方法进行对比。RTMPose-EPnP和PP-TinyPose-EPnP方法均在关键点检测的基础上进一步求解PnP问题以获得位姿估计结果。
从仿真测试数据中选取三组（第10组、第127组、第453组）完整的位姿与3D模型估计结果进行展示，如图8所示，此外，图中仅展示了图像序列中的有效优化部分。
如图8（a）和图8（b）所示，由于PP-TinyPose方法的2D关键点误差较大，PP-TinyPose-EPnP方法求解的位姿误差明显大于RTMPose-EPnP方法和所提方法。RTMPose方法能够对2D关键点进行准确的检测，但由于目标3D模型误差，RTMPose-EPnP方法求解的位姿误差仍然较大。本研究利用多视图几何约束来迭代优化目标的3D模型和位姿。如图8（c）所示，本文方法通过迭代优化有效减小了目标3D模型误差，进而提高位姿测量精度。相较于两个对比方法，本文方法在整个图像序列中具有最高的位姿测量精度。
接下来，表3展示了所有样本的位姿与3D模型误差的均值统计结果，其中最小误差以粗体突出显示。
图8仿真图像序列中的位姿及3D模型误差
Fig.8Pose and 3D model errors in synthetic image sequences
如表3所示，本文方法通过迭代优化有效减小了目标3D模型的误差，相比于两个对比方法，在整个仿真实验数据中取得了最高的位姿测量精度。
表3仿真图像序列位姿与3D模型误差的均值统计结果
Tab.3The average statistic results of pose and 3D model errors for synthetic image sequences
3 结论
本文针对单目视觉引导中目标3D模型不准确条件下的目标平台间位姿测量问题，迭代优化目标平台3D模型及位姿，并提出一种新的单目视觉测量方法：采用稀疏3D关键点集合建模目标平台3D模型，并采用先进的关键点检测方法实现序列图像中关键点的鲁棒、高效检测。进一步，将以线性参数方程形式表达的目标3D关键点参数与位姿参数作为待优化参数，最小化物方残差来建立优化目标函数，通过求解该优化问题迭代优化目标3D模型与位姿，实现目标3D模型不准确条件下位姿的高精度求解。为了满足视觉引导中位姿测量的实时、在线应用需求，采用滑动窗口约束计算规模，并通过选取关键帧减少计算量和信息冗余，实现实时、在线的目标平台3D模型及位姿迭代优化求解。实验结果表明，本研究可以实现目标3D模型不准确条件下实时、在线的高精度位姿测量，并有效优化目标3D模型。
本文方法尽管取得了明显的位姿与模型迭代优化效果，但依然存在不适应较大2D关键点检测误差的问题，下一步工作中，将探索利用序列图像多视图几何约束信息进一步减小2D检测关键点误差的方法。
致谢
国防科技大学空天科学学院应用力学系图像测量实验室博士生王秋富在仿真图像生成方面提供了指导和帮助，谨致谢意！
