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

权利要求书

1.一种合作目标远距离高精度六自由度位姿测量方法，其特征在于，包括如下步骤：

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

步骤(11)、利用相机成像获取合作目标图像，然后基于灰度重心法提取合作目标特征成像中心点亚像素坐标并进行2D-3D点的匹配；

步骤(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)的具体过程为：

步骤(31)、在相机坐标系中，通过相机所测初始位置坐标与相机光心坐标系原点O＝[0 0 0]^T可以确定一条直线L_OT，其方程为其中，上标i代表这是初始结果，{l,m,n}表示直线的参数；

步骤(32)、激光测距仪坐标系与相机光心坐标系的转换关系已事先标定，可以用一个空间三维点和一个空间方向向量表示：

O_LO＝[x_L,y_L,z_L]^T

n_LO＝[n_xL,n_yL,n_zL]^T

其中，x_L、y_L、z_L分别为激光测距仪坐标系原点位置在相机坐标系X，Y，Z方向的坐标值，n_xL、n_yL、n_zL为相机坐标系转到激光测距仪坐标系的欧拉旋转角；

假设激光光斑弥散成了一个半径为r的圆，已知激光光线在相机坐标系下的表示以及激光测距仪的所测的距离值，求出激光光斑面中心点坐标P＝[x_r,y_r,z_r]^T，进而通过以上条件求解出弥散圆的方程：

其中，x，y，z分别表示空间圆上点在相机坐标系X，Y，Z方向的坐标值；

步骤(33)、求步骤(31)中确定的直线L_OT与步骤(32)中所确定的圆面的交点，所得的相交点即为矫正后的合作目标位置用t_a更新T_i＝[R_i|t_i]中的位移分量，得到初步矫正后的位姿T_a＝[R_a|t_a]；

步骤(4)、利用非线性优化方法对通过步骤(3)中得到的矫正后的位姿T_a＝[R_a|t_a]进行进一步优化，具体优化模型为：

其中，为图片上2D成像点，pro(P_i^T)表示空间合作目标上特征点的重投影函数，λ是权重因子，设为5，表示矫正后的Z方向位移值，表示初始的Z方向位移值，最小化上式，将得到一个更加精确的位姿T′_a＝[R′_a|t′_a]。