[发明专利]一种基于不变特征的锥套位姿解算方法

权利要求书

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轴，即可得到锥套姿态初始解。