[发明专利]一种结合IMU和导航影像的月面巡视器快速定位方法

权利要求书

1.一种结合IMU和导航影像的月面巡视器快速定位方法，其特征在于，包括如下步骤：

步骤1：构建巡视器零位坐标系；

步骤2：提取导航相机焦距f、导航相机与巡视器零位坐标系原点之间的偏移量(B_X，B_Y，B_Z)、巡视器质心与巡视器零位坐标系原点之间的偏移量(ΔX，ΔY，ΔZ)、巡视器俯仰角度、巡视器偏航角度和IMU测得的三轴姿态角；

步骤3：构建像空间辅助坐标系，根据巡视器俯仰角度和巡视器偏航角度计算像空间辅助坐标系相对于巡视器零位坐标系的旋转矩阵R_俯和R_偏；

步骤4：计算着陆器当地铅垂坐标系与巡视器零位坐标系间的旋转矩阵R_IMU；

步骤5：从选定的导航影像中提取具有明显特征的像点a，并根据导航相机标定的畸变参数对像点坐标进行误差纠正，得到像点a坐标(x_a，y_a)；

步骤6：从着陆区数字止射影像DOM及着陆区数字高程模型DEM中提取像点a对应的月面点A在着陆器当地铅垂坐标系下的坐标(X_A，Y_A，Z_A)；

步骤7：计算导航影像摄影中心在当地铅垂坐标系下坐标(X_S，Y_S，Z_S)；

步骡8：根据巡视器质心与巡视器零位坐标系原点之间的偏移量(ΔX，ΔY，ΔZ)、旋转矩阵R_IMU，、导航相机与巡视器零位坐标系原点之间的偏移量(B_X，B_Y，B_Z)及摄影中心(X_S，Y_S，Z_S)，计算巡视器质心坐标(X_C，Y_C，Z_C)，完成巡视器定位。

2.根据权利要求1所述的结合IMU和导航影像的月面巡视器快速定位方法，其特征在于，所述步骤4具体为：

根据IMU的安装参数和地面测试数据计算巡视器零位坐标系与IMU设备在三个坐标轴方向的夹角并根据以及巡视器IMU测得的三轴姿态角得到巡视器零位坐标系相对于着陆器当地铅垂坐标系的三个姿态角为记为进而得到两套坐标系间的旋转矩阵R_IMU。

3.根据权利要求1所述的结合IMU和导航影像的月面巡视器快速定位方法，其特征在于，所述步骤7具体方法为：

按照最小二乘求解公式X＝(A^TA)^-1A^TL，计算导航影像摄影中心当地铅垂坐标系下坐标(X_S，Y_S，Z_S)，其中：

4.根据权利要求1所述的结合IMU和导航影像的月面巡视器快速定位方法，其特征在于，巡视器质心坐标(X_C，Y_C，Z_C)按照如下公式计算：