一种基于不变特征的锥套位姿解算方法

摘要

本发明公开了一种基于不变特征的锥套位姿解算方法，包括获取可用于不变特征构建三维空间信息的物理量；提取圆特征的图像信息，并根据圆特征解算位姿，得到两组可能的位姿解；提取图像中三线特征的图像信息，计算得分得到有效三线特征；利用有效三线特征，分别求解得到每个三线特征的位置参数；根据模型坐标系与世界坐标系的约束关系，选择正确姿态解；求解锥套姿态初始解；根据模型坐标系与世界坐标系的约束关系，求解锥套位置初始解；优化锥套位姿。本发明克服了自主空中加油近距视觉导航中锥套形变会影响特征三维空间信息的准确性从而导致单目视觉位姿解算精度下降问题，具有更简单的图像信息与空间信息的对应关系，便于特征的配准。

权利要求

1.一种基于不变特征的锥套位姿解算方法，其特征在于，包括如下步骤：步骤1、获取可用于不变特征构建三维空间信息的物理量，所述物理量包括圆特征和三线特征；

步骤2、提取圆特征的图像信息，并根据圆特征解算位姿，得到两组可能的位姿解，包括两个法向姿态 和两个位置 ，其中 ；

步骤3、提取图像中三线特征的图像信息，计算得分，得到有效三线特征；

步骤4、利用有效三线特征，构建计算机视觉中的P3L问题，分别求解得到每个有效三线特征的位置参数 和姿态参数 ，每个有效三线特征可解算到2到4组位姿解，故N个有效三线特征可产生 组到 待定的解；

步骤5、根据模型坐标系与世界坐标系的约束关系，选择步骤3、4中正确姿态解；

步骤6、求解锥套姿态初始解；

步骤7、根据模型坐标系与世界坐标系的约束关系，求解锥套位置初始解；

步骤8、优化锥套位姿。

2.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤1中，所述不变特征包括：（1）锥套加油口的单个圆形特征；

（2）锥套每个伞骨三角块上的三条不同方向的线特征，下称三线特征，记为，其中 为从伞骨内侧提取到的线特征， 为从伞骨外侧提取到的线特征， 为剩余一个方向的线特征， 和 为从三线特征空间物理量拟合出的代表线段的首尾端点。

3.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤1中，所述圆特征为能恢复出圆空间结构的物理量，包括圆的半径、直径或圆上若干点的坐标；所述三线特征为能恢复出三角形空间结构的物理量，包括三线特征所构成的三角形的三边长、两边长及夹角。

4.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤3具体为：提取图像中可观测的三线特征 ，对三线特征按计算得分，并进行降序排列，得到前N个得分最高的三线特征索引向量 和得分向量 ，其中 和 是第i条线拟合得到的首尾端点，下称为有效三线特征， 代表向量取模， 代表向量叉乘。

5.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤5中，所述模型坐标系与世界坐标系的约束关系包括：（1）圆特征的模型坐标系与世界坐标系保持一致；

（2）多个三线特征模型坐标系的Y轴与世界坐标系Z轴在一直保持正交的状态，即世界坐标系Z轴始终垂直于多个三线特征所在平面法向量张成的平面；

（3）所有三线特征模型坐标系的原点位于以圆特征模型坐标系原点为圆心，半径与加油口半径r相同的圆上。

6.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤5具体为：取步骤4中解算得到的三线特征模型坐标系Y轴方向，记为 ；结合圆特征求解得到的两个圆平面法向量 和 ，按 计算误差，从每个三线特征的多组姿态参数中选取出符合实际位姿的圆平面法向量和三线特征模型坐标系Y轴，其中 为有效三线特征的个数， 代表解的数量为2到4个， 代表向量转置， 表示取最小值，选择误差和较小的那组为锥套姿态对应的圆平面法向量和三线特征模型坐标系Y轴向量 ，当 和 相近时，选择误差分布更一致的那组解。

7.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤6具体为：根据模型坐标系与世界坐标系之间的约束关系，构建线性约束方程组，其矩阵表达形式为 利用奇异值分解求解得到锥套世界坐标系的Z轴方向 ，其中为选取的 构建的矩阵， 代表向量转置，

为选取的 构建得到的矩

阵， ， ， ，分别代表 三个维度的值；以步骤3中得分最高的三线特征模型坐标系的原点为世界坐标系X轴正方向，根据正交性得到世界坐标系Y轴，即可得到锥套姿态初始解 。

8.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤7具体为：取步骤4中解算得到的三线特征的位置 ，结合圆特征求解得到的两个圆心位置参数和 ，根据 计算误差，分别从每个三线特征的多组位置参数中选取出符合世界位姿的圆特征位置  和三线特征位置 ，其中为有效三线特征的个数， 代表解的数量为2到4个， 为锥套加油口的半径， 表示向量取模， 表示取绝对值，选择误差和较小的那组为锥套位置对应的和 ，按 求解锥套位置参数初始解，其中 为选取的 拟合的圆心， 是圆特征解算结果的权重。

9.根据权利要求1所述的基于不变特征的锥套位姿解算方法，其特征在于，步骤8具体为：按 ，以旋转不变的重投影误差方程为目标，以图优化的方法求解最优锥套位置 和姿态 。

说明书

一种基于不变特征的锥套位姿解算方法

技术领域

[0001] 本发明涉及计算机视觉领域，尤其涉及一种自主空中加油场景下单目视觉锥套位姿解算方法。

背景技术

[0002] 空中加油技术自上世纪二十年代发明以来，对航空业产生能力巨大的影响。空中加油技术可以大大提到战斗机的作战能力，也可以为国际航班和运输的减少约30‑40%的燃料。随着无人航空器的发展，自主空中加油(Aerial Autonomous Refueling，AAR)也应运而生，顺势而为。AAR可降低有人机空中加油的操作风险，还可实现无人机的自主加油，是现代航空领域的重要研究方向之一。

[0003] 现有的文献报道中空中加油方式可分成软管式和硬管式两种。其中软管式空中加油方式由于经济性和灵活性，且被更多国家掌握，被证实更适合用于航空器的自主空中加油场景。我国主要应用的即为软管式空中加油方式。软管式加油系统由加油泵、调压系统、加油软管及锥套等组成，尾端的锥套装置连接软管，在运动过程中依靠气动力与软管拖拽力将撑开并在一定范围内运动。受油机的探头进入稳定伞内部的加油口并与之啮合并锁定，即可进行进一步的油料传输。软管式空中加油的关键之一是在近距(距离在1m‑20m之间)的情况下，实时获取加油锥套与受油插头之间精确的实时位姿信息，尤其是在两者靠近的对接阶段(1m以内)，相对位置误差需保持在厘米级。INS（Inertial Navigation System，惯性导航系统）和GPS（Global Positioning System，全球定位系统）等传统的机载导航方式的相对定位精度低且存在应用场景的限制，不适合空中加油近距场景尤其是对接阶段。单目视觉具有结构简单、便于标定、维护方便有效视场大等优点，是目前空中加油对接阶段引导受油机探针与锥套的常用导航方法。

[0004] 自美国率先验证了主空中加油项目（AAR）以来，各国借鉴其利用多个LED光标进行位姿解算的VisNav视觉导航系统的经验，开展了大量研究工作。现有的大多数自主空中加油视觉导航方法，常采用安装在锥套伞骨上的光学标记辅助的位姿解算方法，包括LED光标、光敏材料标记等。此外，也有一部分研究利用锥套自身的圆形特征进行位姿解算，但单圆位姿解算存在二义性的问题。

[0005] 然而，由于锥套伞骨之间采用柔性连接的方式，在飞行过程中依靠气流将伞面撑开，锥套上铰链在加油口不同位置伞骨会随着气流变化在不同作用力的情况下和加油口平面法向量形成不同的夹角，使得锥套稳定伞呈现类似伞状的不同程度的开合状态。而单目视觉位姿测量技术中十分需提前获知三维空间信息，且确保二维图像信息与三维空间信息之间的准确对应。柔性的锥套结构使得三维空间信息处于实时变化的状态，不考虑锥套这一柔性结构的形变导致的空间结构变化会直接影响约束方程的准确性，导致了锥套位姿解算精度的降低，甚至解算失效的情况。

发明内容

[0006] 发明目的：本发明为了克服柔性锥套结构形变带来的锥套位姿估计精度低，从而影响空中加油任务成功与否的问题，本发明从锥套自身结构出发，分析柔性锥套在飞行过程中的形变特点，提出了一种基于不变特征的锥套位姿解算方法。

[0007] 技术方案：本发明的基于不变特征的锥套位姿解算方法，具体包括如下步骤：

[0008] 步骤1、获取可用于不变特征构建三维空间信息的物理量，所述物理量包括圆特征和三线特征；

[0009] 步骤2、提取圆特征的图像信息，并根据圆特征解算位姿，得到两组可能的位姿解，包括两个法向姿态  ( 维的单位向量)和两个位置  ( 维的向量)，其中；

[0010] 所述圆特征的图像信息指锥套上圆在图像中成像的椭圆参数，包括中心点坐标(u,v)，长轴a，短轴b以及倾角θ，单位分别为像素和度；对于圆特征三维空间信息包括但不限于圆的半径、直径或圆上若干点的坐标等能恢复出圆空间结构的物理量。

[0011] 步骤3、提取图像中三线特征的图像信息，计算得分，得到有效三线特征；

[0012] 步骤4、利用有效三线特征，构建计算机视觉中的P3L问题，分别求解得到每个三线特征的位置参数 和姿态参数 ，每个三线特征可解算到2到4组位姿解，故N个三线特征可产生 组到 待定的解；

[0013] 步骤5、根据模型坐标系与世界坐标系的约束关系，选择步骤3、4中正确姿态解；

[0014] 步骤6、求解锥套姿态初始解；

[0015] 步骤7、根据模型坐标系与世界坐标系的约束关系，求解锥套位置初始解；

[0016] 步骤8、优化锥套位姿。

[0017] 进一步地，步骤1中，所述不变特征包括：

[0018] （1）锥套加油口的单个圆形特征；

[0019] （2）锥套每个伞骨三角块上的三条不同方向的线特征，下称三线特征，记为，其中 为从伞骨内侧提取到的线特征， 为从伞骨外侧提取到的线特征， 为剩余一个方向的线特征， 和 为从三线特征空间物理量拟合出的代表线段的首尾端点。

[0020] 进一步地，步骤1中，所述圆特征为能恢复出圆空间结构的物理量，包括圆的半径、直径或圆上若干点的坐标；所述三线特征为能恢复出三角形空间结构的物理量，包括三线特征所构成的三角形的三边长、两边长及夹角。

[0021] 进 一 步 地 ，步 骤 3 具 体 为 ：提 取 图 像 中 可 观 测 的 三 线 特 征( 维的向量)，对三线特征按

[0022]

[0023] 计算得分，并进行降序排列，得到前N个得分最高的三线特征索引向量和得分向量 ，其中 ( 维的向量)和 ( 维的向量)是第i条线拟合得到的首尾端点，下称为有效三线特征， 代表向量取模， 代表向量叉乘。

[0024] 进一步地，步骤5中，所述模型坐标系与世界坐标系的约束关系包括：

[0025] （1）圆特征的模型坐标系与世界坐标系保持一致；

[0026] （2）多个三线特征模型坐标系的Y轴与世界坐标系Z轴在一直保持正交的状态，即世界坐标系Z轴始终垂直于多个三线特征所在平面法向量张成的平面；

[0027] （3）所有三线特征模型坐标系的原点位于以圆特征模型坐标系原点为圆心，半径与加油口半径r相同的圆上。

[0028] 进一步地，步骤5具体为：取步骤4中解算得到的三线特征模型坐标系Y轴方向，记为 ；结合圆特征求解得到的两个圆平面法向量 ( 维的单位向量)和 ( 维的单位向量)，按 计算误差，从每个三线特征的多组姿
态参数中选取出符合实际位姿的圆平面法向量和三线特征模型坐标系Y轴 ，其中为有效三线特征的个数， 代表解的数量为2到4个， 代表向量转置，
表示取最小值，选择误差和较小的那组为锥套姿态对应的圆平面法向量 和三线特征模型坐标系Y轴向量 ，当 和 相近时，选择误差分布更
一致的那组解。

[0029] 进一步地，步骤6具体为：根据模型坐标系与世界坐标系之间的约束关系，构建线性约束方程组，其矩阵表达形式为 利用奇异值分解求解得到锥套世界坐标系的Z轴方向 ，其中 ( 维的矩阵)为选取的 构建的矩阵，
代表向量转置， 为选取的
 ( 维的单位向量)构建得到的矩阵， ， ， ，分别代
表 三个维度的值；以步骤3中得分最高的三线特征模型坐标系的原点为世界坐标系X轴正方向，根据正交性得到世界坐标系Y轴，即可得到锥套姿态初始解  ( 维的单位正交阵)。

[0030] 进一步地，步骤7具体为：取步骤4中解算得到的三线特征的位置 ，结合圆特征求解得到的两个圆心位置参数 和 ，根据计算误差，分别从每个三线特征的多组位
置参数中选取出符合世界位姿的圆特征位置  和三线特征位置 ，其中
为有效三线特征的个数， 代表解的数量为2到4个， 为锥套加油口的半径， 表示向量取模， 表示取绝对值，选择误差和较小的那组为锥套位置对应的 和 ，按求解锥套位置参数初始解，其中 为选取的 拟
合的圆心， 是圆特征解算结果的权重。

[0031] 进一步地，步骤8具体为：按 ，以旋转不变的重投影误差方程为目标，以图优化的方法求解最优锥套位置 和姿态 。

[0032] 由于锥套的旋转对称结构，所述最优锥套姿态 中只有矩阵第3列有效，即锥套法向姿态。

[0033] 本发明利用锥套自身结构中的不变特征，引入并建立各不变特征的模型坐标系，将锥套位解算转化为不变特征位姿解算和模型坐标系与世界坐标系变换两个准确的约束关系，从而将柔性锥套形变带来的误差从解算结果中解耦出来。进一步根据锥套的旋转对称结构，构建旋转不变的重投影误差，优化锥套的5自由度位姿参数。

[0034] 所述世界坐标系的进一步说明：视觉位姿解算中通常根据目标物体空间结构建立的三维笛卡尔坐标系被叫做世界坐标系，其在相机坐标系中的6自由度位姿信息即代表求解的目标位姿。

[0035] 所述锥套位姿的进一步说明：由于锥套这类目标是共轴旋转体结构，因此其位姿解算的结果为3自由度的位置和2自由度的姿态，即目标旋转轴的指向，等价于圆平面的法向姿态，故后续以法向姿态指代锥套的姿态。

[0036] 作为本发明的进一步优化方法，所述不变特征包括：

[0037] （1）锥套加油口的单个圆形特征；

[0038] （2）锥套每个伞骨三角块上的三条不同方向的线特征，下称三线特征，记为记为，其中 为从伞骨内侧提取到的线特征， 为从伞骨外侧提取到的线特征， 为剩余一个方向的线特征， 和 为从三线特征空间物理量拟合出的代表线段的首尾端点。

[0039] 所述不变特征指锥套加油口的单个圆形特征和伞骨三角块上的三条不同方向的线特征这种方式组合的用于位姿估计的特征，包括但不限于从锥套自身提取的单个圆特征和若干三线特征以及按单个圆特征和若干三线特征方式布置人工标记再进行提取的特征。

[0040] 作为本发明的进一步优化方法，所述模型坐标系的建立如下：

[0041] （1）圆特征的模型坐标系以加油口圆心为原点，垂直圆平面指向锥套口为Z轴正方向，并在圆平面内选择一组正交基作为XY轴。该模型坐标系与世界坐标系一致，不存在转换；

[0042] （2）对于多个伞骨上的三线特征，选取 和 铰链在加油口的一端作为三线特征模型坐标系原点，Y轴垂直于三线特征所在的伞骨平面，Z轴与 同向，在伞骨平面内根据笛卡尔坐标系法则选取X轴。

[0043] 作为本发明的进一步优化方法，所述模型坐标系与世界坐标系之间的约束关系包括：

[0044] （1）圆特征的模型坐标系与世界坐标系保持一致；

[0045] （2）多个三线特征模型坐标系的Y轴与世界坐标系Z轴在一直保持正交的状态，即世界坐标系Z轴始终垂直于多个三线特征所在平面法向量张成的平面；

[0046] （3）所有三线特征模型坐标系的原点位于以圆特征模型坐标系原点为圆心，半径与加油口半径r相同的圆上。

[0047] 作为本发明的进一步优化方法，所述旋转不变重投影误差可为旋转对称结构体的5自由度位姿优化提供优化目标，在本发明中由于锥套的不变特征由线和圆两部分组成，故包括线特征重投影和圆特征重投影两部分：

[0048] （1）线特征重投影误差为 ，其中为世界坐标系下某空间点  ( 维的向量)投影到图像后的齐次坐标，
为点 所在线投影到图像后的直线表达式， 和 为 的第一个和第二个系数，代表向量转置， 表示M个值求和。

[0049] （2）圆特征重投影误差为，其中 同上， 为
点 所在圆的投影椭圆的矩阵表达形式，且 的第三行第三列的数值 ， 代表向量转置， 表示N个值求和。

[0050] （3）旋转不变的重投影误差方程建立如下：

[0051] ，其中 为线特征权重。

[0052] 作为本发明的进一步优化方法，参与锥套位姿优化的特征点包括参与线特征重投影和圆特征重投影两部分：

[0053] （1）对于线特征，选择锥套上所有三线特征的 与圆特征的交点作为重投影点，共计 个。其空间坐标为 ，其中()为参与线特征优化的点的索引，与三线特征索引保持一
致，由步骤3计算得到， 为空间结构中所有均匀分布 的总数量。

[0054] （2）对于圆特征，均匀选择圆上 个点，参与重投影误差解算。其空间坐标为，其中 。

[0055] 有益效果：与现有技术相比，本发明具有如下显著优点：本发明提供了一种基于不变特征的锥套位姿解算方法，该方法利用锥套结构特征进行位姿解算，通过转化柔性锥套位姿解算问题为分解出柔性结构中稳定准确的二维图像信息和三维空间信息对应约束和建立模型坐标系与代表锥套位姿的世界坐标系之间的约束关系的问题，克服了自主空中加油近距视觉导航中锥套形变会影响特征三维空间信息的准确性从而导致单目视觉位姿解算精度下降，甚至失效的问题；此外，也解决了进依赖加油口单个圆形特征解算位姿存在两组位姿解的二义性问题；再者，本发明中利用不变特征中三线特征对应关系进行位姿求解相较于直接利用线特征对应关系进行位姿求解，具有更简单的图像信息与空间信息的对应关系，便于特征的配准；最后，利用旋转不变的重投影误差，可以有效融合所有可观测特征对锥套的5自由度位姿参数做出准确估计。

附图说明

[0056] 图1是发明所涉及部分坐标系示意图；

[0057] 图2是线特征相机投影示意图；

[0058] 图3是圆特征相机投影和二义性示意图；

[0059] 图4是柔性锥套示意图；

[0060] 图5是锥套不变特征示意图；

[0061] 图6是不变特征模型坐标系与世界坐标系示意图；

[0062] 图7是本发明解算流程图；

[0063] 图8是实验标定场景示意图；

[0064] 图9是仿真实验一结构形变实验条件下的角度误差统计曲线；

[0065] 图10是仿真实验一结构形变实验条件下的平移误差统计曲线；

[0066] 图11是仿真实验一图像噪声实验条件下的角度误差统计曲线；

[0067] 图12是仿真实验一图像噪声实验条件下的平移误差统计曲线；

[0068] 图13是仿真实验一工作距离实验条件下的角度误差统计曲线；

[0069] 图14是仿真实验一工作距离实验条件下的平移误差统计曲线；

[0070] 图15是仿真实验一结构形变实验条件下结构形变为4°时解算结果的箱线图；

[0071] 图16是仿真实验一结构形变实验条件下结构形变为16°时解算结果的箱线图；

[0072] 图17是仿真实验一图像噪声实验条件下图像噪声为2像素时解算结果的箱线图；

[0073] 图18是仿真实验一图像噪声实验条件下图像噪声为6像素时解算结果的箱线图；

[0074] 图19是仿真实验一工作距离实验条件下工作距离为1500mm时解算结果的箱线图；

[0075] 图20是仿真实验一工作距离实验条件下工作距离为3000mm时解算结果的箱线图；

[0076] 图21 仿真实验一不同三线特征数量对比实验的解算时间统计结果；

[0077] 图22是仿真实验二结构形变实验条件下的角度误差统计曲线；

[0078] 图23是仿真实验二结构形变实验条件下的平移误差统计曲线；

[0079] 图24是仿真实验二图像噪声实验条件下的角度误差统计曲线；

[0080] 图25是仿真实验二图像噪声实验条件下的平移误差统计曲线；

[0081] 图26是仿真实验二结构形变实验条件下形变为4°时解算结果的箱线图；

[0082] 图27是仿真实验二结构形变实验条件下形变为20°时解算结果的箱线图；

[0083] 图28是仿真实验二图像噪声实验条件下噪声为2像素时解算结果的箱线图；

[0084] 图29是仿真实验二图像噪声实验条件下噪声为10像素时解算结果的箱线图；

[0085] 图30是仿真实验二不同方法对比实验的解算时间统计结果；

[0086] 图31是真实图像静态实验的解算误差曲线；

[0087] 图32是真实图像静态实验的解算统计结果；

[0088] 图33是室内高精度三轴平台；

[0089] 图34是真实图像动态实验距离为3.5‑2米时的位姿误差统计曲线；

[0090] 图35是真实图像动态实验距离为5‑3.5米时的位姿误差统计曲线。

具体实施方式

[0091] 下面结合附图对本发明的技术方案作进一步说明。

[0092] 图1所示为本发明中所涉及到的部分坐标系示意图，也是一般计算机视觉位姿解算场景中通用的坐标系定义方法，包括目标世界坐标系 ，相机坐标系以及图像坐标系 。

[0093] 图2和图3所示为计算机视觉中线、圆特征投影的示意图。尤其是圆特征的投影在绝大多数情况下，一个投影可存在两个可能的位姿与之对应的情况。

[0094] 图4所示为柔性锥套在完全收拢和张开时的状态，图4中(a)为锥套收拢时的示意图， (b)为锥套完全张开时的示意图。飞行过程中锥套上不同位置的伞骨会在不同的气流作用处于不同的开合角度下。

[0095] 图5所示为1:1还原锥套模型上提取到的不变特征示意图，图5中(a)和 (b)两个不同角度拍摄图片的示意图。

[0096] 图6所示为不变特征模型坐标系与世界坐标系的定义，其中圆特征的模型坐标系与世界坐标系保持一致，如图中  ，三线特征及其模型坐标系的定义如图中。

[0097] 如图7所示，本发明的解算流程如下：

[0098] 步骤1、获取可用于不变特征构建三维空间信息的物理量，所述物理量包括圆特征和三线特征；

[0099] 步骤2、提取圆特征的图像信息，并根据圆特征解算位姿，得到两组可能的位姿解，包括两个法向姿态   ( 维的单位向量)和两个位置  ( 维的向量)，其中；

[0100] 所述圆特征的图像信息指锥套上圆在图像中成像的椭圆参数，包括中心点坐标(u,v)，长轴a，短轴b以及倾角θ，单位分别为像素和度；对于圆特征三维空间信息包括但不限于圆的半径、直径或圆上若干点的坐标等能恢复出圆空间结构的物理量。

[0101] 步骤3、提取图像中三线特征的图像信息，计算得分，得到有效三线特征；

[0102] 步骤4、利用有效三线特征，构建计算机视觉中的P3L问题，分别求解得到每个三线特征的位置参数 和姿态参数 ，每个三线特征可解算到2到4组位姿解，故N个三线特征可产生 组到 待定的解；

[0103] 步骤5、根据模型坐标系与世界坐标系的约束关系，选择步骤3、4中正确姿态解；

[0104] 步骤6、求解锥套姿态初始解；

[0105] 步骤7、根据模型坐标系与世界坐标系的约束关系，求解锥套位置初始解；

[0106] 步骤8、优化锥套位姿。

[0107] 进一步地，步骤1中，所述不变特征包括：

[0108] （1）锥套加油口的单个圆形特征；

[0109] （2）锥套每个伞骨三角块上的三条不同方向的线特征，下称三线特征，记为，其中 为从伞骨内侧提取到的线特征， 为从伞骨外侧提取到的线特征， 为剩余一个方向的线特征， 和 为从三线特征空间物理量拟合出的代表线段的首尾端点。

[0110] 进一步地，步骤1中，所述圆特征为能恢复出圆空间结构的物理量，包括圆的半径、直径或圆上若干点的坐标；所述三线特征为能恢复出三角形空间结构的物理量，包括三线特征所构成的三角形的三边长、两边长及夹角。

[0111] 进 一 步 地 ，步 骤 3 具 体 为 ：提 取 图 像 中 可 观 测 的 三 线 特 征( 维的向量)，对三线特征按

[0112]

[0113] 计算得分，并进行降序排列，得到前N个得分最高的三线特征索引向量和得分向量 ，其中 ( 维的向量)和 ( 维的向量)是第i条线拟合得到的首尾端点，下称为有效三线特征，  代表向量取模， 代表向量叉乘。

[0114] 进一步地，步骤5中，所述模型坐标系与世界坐标系的约束关系包括：

[0115] （1）圆特征的模型坐标系与世界坐标系保持一致；

[0116] （2）多个三线特征模型坐标系的Y轴与世界坐标系Z轴在一直保持正交的状态，即世界坐标系Z轴始终垂直于多个三线特征所在平面法向量张成的平面；

[0117] （3）所有三线特征模型坐标系的原点位于以圆特征模型坐标系原点为圆心，半径与加油口半径r相同的圆上。

[0118] 进一步地，步骤5具体为：取步骤4中解算得到的三线特征模型坐标系Y轴方向，记为 ；结合圆特征求解得到的两个圆平面法向量 ( 维的单位向量)和 ( 维的单位向量)，按 计算误差，从每个三线特征的多组姿
态参数中选取出符合实际位姿的圆平面法向量和三线特征模型坐标系Y轴 ，其中为有效三线特征的个数， 代表解的数量为2到4个， 代表向量转置，
表示取最小值，选择误差和较小的那组为锥套姿态对应的圆平面法向量 和三线特征模型坐标系Y轴向量 ，当 和 相近时，选择误差分布更
一致的那组解。

[0119] 进一步地，步骤6具体为：根据模型坐标系与世界坐标系之间的约束关系，构建线性约束方程组，其矩阵表达形式为 利用奇异值分解求解得到锥套世界坐标系的Z轴方向 ，其中 ( 维的矩阵)为选取的 构建的矩阵，
代表向量转置， 为选取的
 ( 维的单位向量)构建得到的矩阵， ， ， ，分别代
表 三个维度的值；以步骤3中得分最高的三线特征模型坐标系的原点为世界坐标系X轴正方向，根据正交性得到世界坐标系Y轴，即可得到锥套姿态初始解  ( 维的单位正交阵)。

[0120] 进一步地，步骤7具体为：取步骤4中解算得到的三线特征的位置 ，结合圆特征求解得到的两个圆心位置参数 和 ，根据计算误差，分别从每个三线特征的多组位
置参数中选取出符合世界位姿的圆特征位置 和三线特征位置 ，其中
为有效三线特征的个数， 代表解的数量为2到4个， 为锥套加油口的半径， 表示向量取模， 表示取绝对值，选择误差和较小的那组为锥套位置对应的 和 ，按求解锥套位置参数初始解，其中 为选取的 拟
合的圆心， 是圆特征解算结果的权重。

[0121] 进一步地，步骤8具体为：按 ，以旋转不变的重投影误差方程为目标，以图优化的方法求解最优锥套位置 和姿态 。

[0122] 为了验证本发明的有效性，下面设计仿真实验和真实图像实验对其进行验证。

[0123] 针对评价的指标的定义如下：

[0124] 若算法解算法向量与圆心坐标为 ，对应仿真参数 ，实验定义目标位姿误差由角度误差(解算法向量与法向量真值夹角)和平移误差组成，定义如下:

[0125]

[0126] 1) 仿真图像实验验证方案

[0127] 实验在配备16GB RAM和Intel(R) Core(TM) i7‑11700F处理器的电脑上运行。在仿真实验中，相机参数设置分辨率为 的，焦距为主点为 。将加油口圆特征的
半径大小设为 ，三线特征的 长度为 ， 的长度为 ， 和
之间的倾角为 。在理想的未发生变形的情况下， 和圆平面法向量 之间的夹角 为，即三维信息的真值为在该角度下计算获取。

[0128] 每组实验模拟锥套运动随机生成2000个位姿参数并将其投影到图像，其中航向角、俯仰角和关滚转角变化范围为 到 ，目标位于相机坐标系XY轴的位置变化范围为 到 ，Z轴(下文称工作距离)据不同组实验条件变化。

[0129] A. 仿真实验一

[0130] 在仿真实验中研究了三线特征数量(3‑10，分别记为IF_3,…IF_10)对解算结果的影响，并以7个三线特征数量为例，比较了用步骤7解算得到的姿态初始值与位置初始值(记为init_7)和用步骤8的得到的优化位姿结果的关系(IF_7)。仿真实验一分别从结构形变，图像提取噪声和工作距离三方面对上述算法进行考察。实验条件如下：

[0131] (a)结构形变实验条件：设置图像提取噪声均值为0像素，标准差为2像素的高斯白噪声，工作距离设置为2000 mm，目标结构变形为0°到20°范围内每间隔4°进行一组实验。

[0132] (b)图像噪声实验条件：固定工作距离为2500 mm，目标结构形变为10°，设置图像提取噪声为均值为0像素，标准差为0到10像素范围内每间隔2像素进行一组实验。

[0133] (c)工作距离实验条件：固定图像噪声为均值为0像素，标准差为2像素的高斯白噪声，目标结构形变为10°，设置工作距离从1000 mm到5000 mm范围内每间隔500 mm进行一组实验。

[0134] B. 仿真实验二

[0135] 仿真实验二中将本发明提出的锥套位姿解算方法(本发明方法使用8个三线特征参与位姿解算，记为OCL)与常见基于点特征的锥套位姿解算方法(记为PB)、基于圆特征的位姿解算方法(记为CB)和一些基于线特征的位姿解算方法(分别记为Ansar, Mirzaei, ASPnL, LBE, LBL,LDT)进行了对比。仿真实验二分别从结构形变和图像提取噪声两方面对上述算法进行考察。实验条件如下：

[0136] (a)结构形变实验条件：设置图像提取噪声均值为0像素，标准差为2像素的高斯白噪声，工作距离设置为3000 mm，目标结构变形为0°到20°范围内每间隔4°进行一组实验。

[0137] (b)图像噪声实验条件：固定工作距离为2500 mm，目标结构形变为10°，设置图像提取噪声为均值为0像素，标准差为0到10像素范围内每间隔2像素进行一组实验。

[0138] 2) 真实图像实验验证方案

[0139] 为了在实际情况下验证本发明的有效性，在实验中的利用PointGrey的FL3‑U3‑20E4M相机搭配FA1201C型号镜头对如图5所示的1:1还原制作的锥套模型拍拍摄静态图片与动态视频进行实验。为了模拟飞机加油过程中锥套的变形，在每张图片拍摄时对锥套模型进行了不同程度的拉伸和压缩。

[0140] 相机的内参经张正友标定法标定，仿真实验中的设定条件即为对应相机内参数值。所有图像特征经OpenCV计算机视觉库和人工手动筛选两极提取得到。三维信息的获取借鉴文献(孙永荣, 黄斌, 杨博文,等. 多视图场景确定下的空间点位置视觉标定方法[J]. 南京航空航天大学学报, 2015, 47(003):343‑347.)中的方法，在锥套模型上粘贴十字标记点如图8，通过多个场景恢复的三维点来拟合对应的线、圆特征三维信息。图8中(c)和 (d)为两个不同角度拍摄的标定图片的示意图。

[0141] A. 静态实验

[0142] 静态实验中，相机在距离锥套2‑4米范围内，拍摄二十余张图像进行静态实验。解算的真值，即圆特征圆心与圆平面法向量，通过与锥套固连的棋盘求解的得到。

[0143] B. 动态实验

[0144] 在动态实验中，摄像机固连在图33所示的高精度三轴平台上按预设的轨迹拍摄视频。三轴平台Z轴的移动速度为0.3米/秒，相机的帧速率为30帧/秒。由于平台z轴的最大移动范围为1500 mm，因此动态实验的视频分5到3.5 m和3.5到2 m两次采集。将所有视频的每个帧与最后一帧之间的三维相对位置按时间戳对齐与三轴平台的移动进行比较。角度误差的评估与静态实验相同，由棋盘获得法向量的真实。其他附加条件与静态实验相同。

[0145] 如图9‑14所示为仿真实验一的统计结果，图15‑20为从仿真实验一中选取的6组实验结果的箱线图，图21为仿真实验一中每种方法解算耗时的统计结果。从实验结果可以看出，本法发明对抗锥套结构形变有非常好的效果，且在图像噪声较高、工作距离较远时也能有较高的精度，且随着三线特征数量的增加算法的性能表现越佳。对比图中init_7与IF_7曲线可以看出，本发明步骤7解算得到的位置初始解与姿态初始解是有效的，且步骤8中引入的优化算法可以更显著地提高解的精度。此外，当三线特征数量在8块以上时，算法可有较好的表现。多个三线特征进行解算时也能保持在毫秒级的解算耗时，具备高频实时解算的能力。

[0146] 如图22‑25所示为本发明方法与常见的锥套位姿估计算法以及不同线特征位姿解算方法在仿真图像条件下的对比实验的统计结果，图26‑29为从仿真实验二中选取的4组实验结果的箱线图，图30为仿真实验二中每种方法解算耗时的统计结果。图31‑32为本发明方法与常见的锥套位姿估计算法以及不同线特征位姿解算方法在真实图像静态实验条件下的对比实验结果，图34‑35为本发明方法与常见的锥套位姿估计算法在真实图像动态实验条件下的对比实验结果。从实验结果可以看出，由于本发明中在图像观测信息和目标位姿之间建立的是一个准确的不变的约束，能够剔除锥套形变带来的影响，故解算结果在定位误差和角度误差上有着明显的优势。由于旋转不变的重投影优化的加入，本发明对抵抗图像提取噪声和工作距离等影响视觉解算精度的影响因素的能力也十分优秀。此外，本发明也有着较高的实时性，解算均值在毫秒级，十分适用于实时性需求加高的工程应用。因此本发明具有：1、优异的抗锥套结构形变的能力；2、卓越的解算效率和实时性；3、对图像噪声具备较高的鲁棒性，且对工作距离不敏感。