[发明专利]一种合作目标远距离高精度六自由度位姿测量方法

说明书

本发明公开了一种合作目标远距离高精度六自由度位姿测量方法，包括如下步骤：(1)获取合作目标初始六自由度位姿和初始深度值；(2)计算激光光斑圆方程；(3)求取初始位置与相机光心两个三维空间点所确定空间直线的方程；(4)求取光斑圆与空间直线的交点，矫正初始位姿；(5)利用非线性优化方法对矫正结果进行进一步优化。该方法借助激光测距仪所提供的准确深度值来矫正单目视觉位姿测量视线方向上的较大误差，同时能够消除由激光测距仪激光光斑弥散而引入的干扰误差。

技术领域

本发明涉及基于多传感器融合的位姿测量的技术领域，具体涉及一种合作目标远距离高精度六自由度位姿测量方法。

背景技术

目标高精度六自由度位姿测量有助于实现精确高效的机器操作，因此在自主机器人、精密加工和航天器对接等领域得到了广泛应用。单目视觉是一种非接触式的六自由度姿态测量方法，具有硬件复杂度低、精度高、测量范围广等优点，是目前最常用的六自由度姿态测量方法之一。然而，由于模型自身的限制，沿相机光轴的测量精度通常要比其他两个方向的测量精度低很多，根据赵连军等人《单目三点位置测量精度分析》(参见《光学精密工程》，2014年22卷(5))，沿相机光轴的测量精度只有其他两个方向的二十分之一。

根据晁志超等人的《单目摄像机-激光测距传感器位姿测量系统》(参见《光学学报》，2011年31卷(3)85-91页)，利用了激光测距仪所提供的深度值直接替换了单目视觉位姿测量结果中视线方向的位移值，没有考虑激光光斑弥散所带来的的误差，因此它的矫正效果有限，只能达到毫米级精度。

发明内容

本发明针对现有单目视觉测量中沿相机光轴方向的测量误差大的问题，提供了一种合作目标远距离高精度六自由度位姿测量方法，其是一种基于多传感器融合的解决方法。该方法可以利用激光测距仪得到的准确深度信息来矫正相机测得的初始位姿，同时能够消除因激光光斑弥散而引入的测量误差。本方法建立了光斑弥散圆，确定了初始测量位置与相机光心点所在直线，之后将两个传感器的融合问题转换成了求取一个空间圆面和一条空间直线的交点的几何问题。求取的交点即为矫正后的测量位姿，校正后，沿视线方向的位移测量精度高于其他两个方向。

本发明采用的技术方案如下：一种合作目标远距离高精度六自由度位姿测量方法，包括如下步骤：

步骤(1)、获取合作目标图像，根据PnP算法解算合作目标初始位姿T_i＝[R_i|t_i]，其中，R表示姿态信息，t表示位移信息，下标i代表这是初始结果；

所述步骤(1)的具体过程为：

步骤(11)、利用相机成像获取合作目标图像，然后基于灰度重心法提取合作目标特征成像中心点亚像素坐标并进行合作目标上的3D合作特征点P_i^T(i＝1,2,3,4)与对应的图片上的2D成像点的匹配；

步骤(12)、基于PnP算法，利用步骤(11)中的4对2D-3D匹配点进行位姿解算，得到合作目标坐标系相对于相机光心坐标系的位姿转换关系T_i＝[R_i|t_i]。

步骤(2)、利用激光测距仪测量准确深度信息D_a。

步骤(3)、利用步骤(2)中的准确深度信息D_a矫正步骤(1)中的初始位姿T_i＝[R_i|t_i]，获取矫正后的位姿T_a＝[R_a|t_a]，其中，下标a代表这是矫正后的结果；

所述步骤(3)的具体过程为：