基于逆投影的多特征融合视觉定位方法

技术领域

本发明涉及视觉测量、视觉定位技术领域，具体涉及一种基于逆投影的多特征融合视觉定位方法。

背景技术

随着科学技术的发展，机械臂不但在地面的应用越来越广泛，在地外天体表面着陆探测中也发挥越来越重要的作用，尤其对于地外天体表面的采样操作具有无可替代性。

地外天体表面着陆采样与返回是世界航天大国探索深空和拓展人类认知外星球和太阳系的重要手段。机械臂是地外天体表面着陆采样任务中必不可少的关键设备，能够按照预定的程序自主进行采样任务或者在地面遥操作中心的控制下进行采样作业，可以代替航天员完成各种采样和样品转移、装罐等操作，实现地外天体表面的无人化采样。无论是执行自主进行采样任务或在地面遥操作中心的控制下进行采样作业，对机械臂进行精确的引导控制是实现采样作业的关键技术之一。然而现有采样任务中机械臂执行采样、放样、抓罐和放罐等操作中的视觉引导操作问题未建立统一约束求解模型进而给出稳定精度的求解方法。

中国发明专利申请(申请号：201611069552.3)公开了一种“视觉定位方法和视觉定位装置”，该视觉定位方法包括：在识别对象上设置多个标识点：根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；根据标识点的三维坐标，建立标识点坐标系；和求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。通过根据本发明的视觉定位方法和视觉定位装置，可以实现不具有显著的自然特征的识别对象的姿态的实时和高精度定位。

该专利的执行需前提是在识别位姿的对象上设置多个标识点，根据该多个标识点之间的位置关系进一步计算得到该识别对象的姿态信息，而我们提出的方法综合利用了人工设置的标志特征和识别对象本身具有的显著自然特征的特点，可以极大地提升视觉定位的精度以及鲁棒性，适用范围更广泛。

发明内容

针对地外天体采样任务中机械臂执行采样、放样、抓罐和放罐等操作中的视觉引导操作存在的精度不高以及未建立统一优化模型求解的问题，本发明提出了基于逆投影的多特征融合视觉定位方法，通过对静态相机测量动态目标和动态相机测量静态目标的过程进行抽象建模，建立了双目、单目相机对自然特征(如圆形目标)和人工设置特征(如靶标特征)的组合定位优化模型，实现了不同操作应用场景下适应不同相机数量的多类特征组合定位，有效提高地外天体采样遥操作中利用机械臂采集样品、放置样品、抓取密封罐和放置密封罐等关键环节的操作精度和自动化程度。

为实现上述目的，本发明采用以下技术方案：基于逆投影的多特征融合视觉定位方法包括：

步骤一：在相机图像中提取目标特征的像素坐标，针对自然特征，如圆形特征，检测其轮廓像素坐标，针对人工设置的靶标特征如棋盘格特征检测其标志点像素坐标。

步骤二：确定空间中存在N个相机，其中N1个为静态相机；N2个为动态相机，当属于相机位姿固定，需要测量特征在空间中的位姿情况下，设置合适的目标位姿X

步骤三：根据前述步骤一中的像素坐标，已知的目标位姿信息或已知的相机位姿信息和相机位姿初始迭代值或目标位姿初始迭代值在空间中重建特征的三维坐标。具体为：

设第i个相机坐标系在世界坐标系的位姿为

其中，

如附图4所示，根据空间中任意特征点的逆投影描述方式，任意目标特征点

其中,

其中

s

将相机视场内的特征按照一定准则分为K组，其中人工设置特征和自然特征分别分在K

n

＝[sinφ

其中n

由此空间点世界坐标

步骤四：建立相机对自然特征(如圆形特征)和人工设置靶标特征(如棋盘格)的相对测量的统一的测量优化模型。根据已知人工设置特征的相对位置和自然特征的几何形状特性构建误差方程；假设仅有靶标和圆形目标两类特征，则逆投影误差函数写作：

其中

在实际应用中可根据测量需求对变量Ω

步骤五：针对公式(7)提出的最小化逆投影误差

那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息

表I 使用L-M法计算空间目标或相机的精确位姿

得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。

附图说明

图1.本发明定位方法框图；

图2.是着陆器、上升器和机械臂上的各类相机；

图3.多特征融合视觉测量模型示意图；

图4.基于逆投影的目标特征点几何表征示意图。

其中：上升器1；着陆器2；四自由度机械臂3；采样器相机甲4；采样器相机乙5；臂载相机6；采样区观测双目相机A/B 7；监视相机C 8；监视相机D 9；机械臂基座10；机械臂末端圆盘11；密封罐12；棋盘状靶标13；机械臂第1关节3.1；机械臂第2关节3.2；机械臂第3关节3.3；机械臂第4关节3.4。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

我国嫦娥五号探测器包括着陆器携带细长结构的四自由度机械臂3，在地面遥操作中心的控制下，利用该机械臂完成表取采样、样品放入密封罐12、样品密封罐12抓取和样品密封罐12放置到上升器1上等操作任务。由于细长机械臂在月面微重力作用下的柔性形变不可预知，因此利用视觉引导的方式对其完成精确操作控制。在嫦娥五号着陆器2、上升器1上带有多个相机，如图2所示，主要包括采样区观测双目相机7、采样器相机甲4、采样器相机乙5、臂载相机6，利用这些相机测量引导完成对采样、放样、抓罐和放罐等操作任务。其中采样区双目观测相机A/B 11固定在着陆器2上，用来在采样过程中对机械臂末端自然特征目标进行视觉定位，监视相机C 8固定在着陆器2上，监视相机D 9固定在上升器1上；臂载相机6固定在四自由度机械臂3上，采样器甲4、采样器乙5相机固定在机械臂末端，臂载相机6和采样器相机用来完成在采样、放样、抓罐和放罐等操作过程中对相机的自身位姿进行视觉定位。如图3所示，机械臂末端圆盘11作为空间圆形目标，在相机中成像为圆形特征，静态相机固定在着陆器2基座上。在放样密封罐12附近装有人工设置的棋盘状靶标13，动态相机即臂载相机和放样器相机安装在机械臂末端。四自由度机械臂包括第一关节3.1；机械臂第2关节3.2；机械臂第3关节3.3；机械臂第4关节3.4，机械臂整体固定在机械臂基座10上。

本发明针对地外天体表取采样过程中的采样、放样、抓罐和放罐等操作中的视觉引导问题，提出了基于逆投影的多特征融合视觉定位算法框架，通过对静态相机测量动态目标和动态相机测量静态目标的过程进行抽象建模，建立了双目、单目相机对自然特征如圆形目标和人工设置特征如靶标特征的组合定位优化模型，最后利用非线性优化算法解决此优化问题，从而获取各类特征在空间中的精确位姿或反解动态相机在空间中的精确位姿达到对各类操作精确视觉引导的效果。

如图1所示，本发明基于逆投影的多特征融合视觉定位方法包括以下步骤：

步骤一：在相机图像中提取空间目标正向投影形成的特征像素坐标；针对自然特征检测其轮廓像素坐标，针对人工设置的特征如靶标特征检测其标志点像素坐标；

步骤二：确定空间中有N个相机，其中N1个为静态相机；N2个为动态相机。当属于相机位姿固定，需要测量空间目标位姿的情况下，设置合适的目标位姿X

步骤三：根据已知的相机位姿信息或目标位姿信息和目标特征的像素坐标反向利用透视投影模型在空间中重建特征的三维坐标；具体步骤为：

假设第i个相机在世界坐标系的位姿为

其中，

根据空间中的任意目标特征点的逆投影描述方式，空间中的任意目标特征点

其中,

其中

v

s

由公式(2)可知，多类特征融合的关键是对各类特征的尺度因子s

其中n

由此

步骤四：根据重建出的空间坐标利用已知的特征之间的固定约束建立相机对自然特征和人工特征的相对测量的统一的测量优化模型。具体步骤为：

根据已知人工设置的特征之间的相对位置和自然特征的几何形状特性构建误差方程。假设仅有靶标和圆形目标两类特征，则逆投影误差可以写作：

其中

在实际应用中可根据测量需求对变量Ω

步骤五：利用非线性优化方法迭代求解位姿测量优化模型，获取空间目标或机械臂臂载相机自身的精确位姿信息，从而实现对机械臂逐步引导控制，执行预定的工作程序，具体为：

针对上述公式(8)提出的最小化逆投影误差

那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息

表I 使用L-M法计算空间目标或相机的精确位姿

得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。