CN109631876A - 一种基于单相机导航影像的巡视探测器定位方法 - Google Patents

Abstract

本发明提供一种基于单相机导航影像的巡视探测器定位方法，涉及航空航天技术领域。该方法首先定义多个坐标系，然后计算某一导航相机摄影中心在巡视器零位坐标系下的坐标及导航影像中特征点在导航影像中对应的像点的像平面直角坐标，进而得到该点在零位坐标系、本体坐标系和巡视坐标系下的坐标；根据导航影像摄影中心位置，结合零位坐标系原点与摄影中心的偏移量、本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量，得到巡视器本体坐标系原点在巡视坐标系下的坐标，实现最终定位。本发明提供的基于单相机导航影像的巡视探测器定位方法，即使在一台导航相机发生故障时，采用两个控制点即可实现高精度定位，依赖的初始条件更少。

Description

一种基于单相机导航影像的巡视探测器定位方法

技术领域

本发明涉及航空航天技术领域，尤其涉及一种基于单相机导航影像的巡视探测器定位方法。

背景技术

巡视探测器的高精度导航定位是能否开展深空科学考察任务的关键技术之一。高精度的定位可以确保巡视器的安全，高精度的姿态信息可以确保其与地面控制系统保持较好的指向角度。现有巡视探测器高精度定位方法主要包括基于里程计和IMU的定位方法、基于摄影测量的定位方法。基于里程计和IMU的定位方法存在累积误差，因此，必须采用基于摄影测量的方法进行必要的修正。在控制点丰富的区域，采用现有的光束法等摄影测量定位方法可以完成定位，但是其需要依赖里程计和IMU测量的姿态数据作为初值；而纹理匮乏的区域，控制点数量少，可能无法满足巡视探测器定位所需的数量，造成定位失败。

针对巡视探测器的定位方法，国内很多学者展开了相关研究。2006年石德乐等人提到了星地敏感器测量、甚高频多普勒跟踪、惯性导航系统和里程计的组合测量以及视觉里程计技术，以恒星和无线电系统来确定巡视探测器的粗略位置，然后根据前后立体图像的匹配点对采用最大似然法估计巡视探测器的位姿信息。2014年吴伟仁等通过提取匹配特征点，并根据左图像特征点在其右图像中提取同名特征点，然后将这些特征点作为观测点，建立两站点双相机测量定位模型，通过立体空间交会算法解算得到巡视探测器当前站点相对于上一个站点的位姿信息。2014年刘传凯等提出了利用惯导和视觉组合进行月面导航与精确定位的实现方法，通过IMU姿态测量参数，结合Affine-SIFT匹配，进而通过光束法平差多次迭代计算获得精确的定位结果。2014年王保丰等将SIFT匹配、相关系数匹配、最小二乘匹配和光束法平差等多项技术融合，实现了相邻站间月面巡视器的导航定位。2014年马友青等将系数矩阵和像点坐标观测值视为包含随机误差，使加权总体最小二乘平差理论应用于月面巡视探测器立体图像相对定向模型，并将理论和加权总体最小二乘平差理论引入到月面巡视探测器立体图像条带网模型中，实现了探测器的定位。2014年刘绍琴等提出了基于DOM匹配的定位方法，利用导航相机生成的DOM与嫦娥二号生成的DOM进行特征匹配，，根据匹配的结果实现月球车在嫦娥二号卫星影像上的定位。2014年许柏等提出了通过多种坐标系统相互转换方法，实现了巡视器直接定位。2015年刘少创等提出了适用于月面环境无高精度控制点的立体图像条带网定位方法。在精确相机标定参数的基础上计算出立体相机的摄影基线和相对方位元素，然后，通过像点匹配、前方交会完成图像的立体模型构建，根据不同摄站间的连接点序列建立立体图像条带网，最终通过最小二乘平差，直接获得月面巡视探测器的位置与姿态信息。2015年徐辛超等提出了基于影像的单摄站定位方法，验证了空间后方交会迭代法、直接法和线性变换法的精度和稳定性，最终得出空间后方交会迭代法效果最优的结论。2018年邢琰等提出了一种利用巡视器的视觉系统进行导航定位的方法。利用巡视探测器的视觉系统(一般为立体相机)分别对太阳、地球进行观测，获取太阳、球图像，通过像心提取得到日地矢量。利用加速度计测量巡视器的倾斜姿态，并结合星历计算，引入双矢量定姿计算，求解出巡视器的航向及位置信息。2013年Ning等提出了一种基于INS/CNS(天体导航系统)组合的初始位置和姿态确定方法，用于辅助进行后续巡视探测器的定位。2008至2009年Li和Di等采用光束法平差模型实现了火星车的视觉定位解算。该方法的基本原理是将导航相机和全景相机在不同站点拍摄的图像连接起来构成图像网，通过对图像网的摄影测量进行光束法平差，提高图像位置和方位参数以及地面点位置的精度和一致性。

上述国内外学者的研究结果虽然都取得了很好的效果，但是定位过程需要的控制点数量较多，此外还需要惯性测量单元获取的姿态参数，在纹理条件匮乏，特征点数量较少的情况下可能无法完成定位，当其中的一台导航相机出现故障时则无法完成定位。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于单相机导航影像的巡视探测器定位方法，实现对巡视探测器的定位。

为解决上述技术问题，本发明所采取的技术方案是：一种基于单相机导航影像的巡视探测器定位方法，包括以下步骤：

步骤1、定义巡视坐标系、巡视器本体坐标系、巡视器零位坐标系、像平面直角坐标系、像空间直角坐标系及像空间辅助坐标系；

所述巡视坐标系：将巡视器与着陆器分离后即将开展巡视考察任务的摄站作为基准，即定义第一摄站位置的巡视器本体坐标系为巡视坐标系；

所述巡视器本体坐标系：原点位于巡视器箱体中心，X轴指向巡视器前进方向，Z轴指向月面铅锤方向，向上为正，Y轴按右手法则确定；

所述巡视器零位坐标系：巡视器立体视觉系统中的导航相机安装在桅杆上方的云台上，在水平方向绕桅杆偏航旋转轴进行竖直方向的旋转，称为偏航角，记为P；绕云台俯仰旋转轴进行俯仰，称为俯仰角，记为F；当巡视器桅杆处于初始状态，桅杆俯仰角和偏航角为零时，定义巡视器零位坐标系原点位于偏航旋转轴线和俯仰旋转轴线的交点，X轴与云台俯仰旋转轴线平行，Z轴与云台偏航旋转轴线平行，XYZ三轴构成右手系；

所述像平面直角坐标系：原点位于导航影像中心，像素的行方向为x方向，向右为正，列方向为y方向，向上为正；

所述像空间直角坐标系：原点位于影像摄影中心S处，x轴y轴分别平行于像平面直角坐标系的对应各轴，z轴和x轴y轴构成右手坐标系；

所述像空间辅助坐标系：原点位于影像摄影中心S处，x、y、z三轴方向分别平行于巡视器零位坐标系的三轴方向；

步骤2、巡视探测器与着陆器分离后开始巡视考察任务；分离后的巡视探测器停留并拍照的位置，即当前摄站巡视探测器在巡视坐标系下的位置(X_E，Y_E，Z_E)，第一摄站巡视探测器位置为(0，0，0)；巡视坐标系与巡视探测器本体坐标系三轴方向夹角组成的旋转矩阵为R_E；

步骤3、巡视器零位时刻，桅杆偏航角度为0，俯仰角度为0，由安装参数或地面标定过程获得巡视器零位坐标系与巡视器本体坐标系在三轴方向的偏移量分别为(ΔX₀，ΔY₀，ΔZ₀)；

步骤4、巡视器零位时刻，某一导航相机摄影中心S的在巡视器零位坐标系下的坐标为(ΔX_S，ΔY_S，ΔZ_S)，由巡视器安装时的安装参数或地面标定过程获得；

步骤5、对于俯仰角度F，偏航角度P，某一导航相机摄影中心S在巡视器零位坐标系下的坐标(X_SP，Y_SP，Z_SP)如下公式所示：

其中，R_P、R_F分别为偏航角度P和俯仰角度F组成的旋转矩阵；

步骤6、设定导航相机焦距为f，导航影像中某一特征点在导航影像中对应的像点的像平面直角坐标为(x，y)，其对应的像空间辅助坐标系坐标为(X_T，Y_T，Z_T)，对应地面点在零位坐标系下的坐标为(X₀，Y₀，Z₀)，该导航相机的摄影中心S在零位坐标系下的位置和三轴旋转角为(X_SP，Y_SP0，Z_SP，0，F，P)，则：

其中，λ为比例系数；

令进一步整理，得：

步骤7、当导航影像中某一特征点在多幅导航影像中出现时，将当前导航影像对应的俯仰和偏航角度带入公式(3)，连列求解得到该特征点在零位坐标系下的坐标(X₀，Y₀，Z₀)，具体求解过程如下：

由于公式3为未知数的非线性方程，因此需要对其线性化后进行求解；令将公式(3)改写为误差方程的形式为：

V＝Ax-l (4)

其中，

上述系数矩阵A中各项偏导数求解过程与摄影测量领域中光束法平差中的求解过程类似，采用最小二乘公式x＝(A^TA)^-1(A^Tl)求解得到该特征点在零位坐标系下的坐标；

步骤8、求解特征点在巡视本体坐标系下的坐标(X_B，Y_B，Z_B)，如下公式所示：

步骤9、求解特征点在巡视坐标系下的坐标(X_D，Y_D，Z_D)，如下公式所示：

步骤10、导航相机在顺时针方向绕固定偏航角度拍摄影像序列，令左导航相机在第i次偏航时拍摄的照片为I_i；在导航相机拍摄的序列影像中提取多个特征点，通过步骤5-步骤9求解出各特征点对应的巡视坐标系下的坐标，并将它们作为巡视器下一个摄站的定位控制点；

步骤11、以步骤10中得到的特征点作为控制点，开始巡视器在第二摄站的定位；设定控制点出现在第二摄站的导航影像I_i和I_i+1中，像点坐标分别为(x_i，y_i)和(x_i+1，y_i+1)，其对应的俯仰角同为F，偏航角分别为P_i和P_i+1，影像对应的外方位元素分别为和其中，为导航影像I_i的摄影中心的位置，为导航影像I_i在像空间直角坐标系三轴方向的旋转角，为导航影像I_i+1的摄影中心的位置，为导航影像I_i+1在像空间直角坐标系三轴方向的旋转角，令两张影像的外方位元素中的三轴方向的夹角组成的旋转矩阵分别为R_i和R_i+1，零位坐标系此刻原点位置为(ΔX_S2，ΔY_S2，ΔZ_S2)，则：

将公式7整理得：

结合步骤10中的像点坐标和影像外方位元素，建立共线方程，如下公式所示：

其中，

步骤12、巡视器定位时外方位元素和为未知量，将公式8和公式9连列，并将其转为可按照最小二乘求解的误差方程的形式，如下公式所示：

X＝(B^TB)^-1B^TL (10)

其中：B为公式8和公式9对各项未知数的偏导数组成的系数矩阵，

步骤13、找到步骤11中提取的控制点在前一摄站的导航影像序列中的对应像点，并通过步骤5-步骤9求解其对应的巡视坐标系下的坐标；

步骤14、结合步骤13求解的控制点巡视坐标系下的坐标，在当前摄站导航影像序列中的像点坐标，代入公式10即可求解和

步骤15、根据导航影像摄影中心位置结合零位坐标系原点与摄影中心的偏移量、巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量，得到巡视器本体坐标系原点在巡视坐标系下的坐标，实现最终定位，具体方法为：

首先计算零位坐标系原点与摄影中心的偏移量(ΔX₁，ΔY₁，ΔZ₁)，如下公式所示：

然后计算巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量(ΔX₂，ΔY₂，ΔZ₂)，如下公式所示：

最终得到巡视器本体坐标系原点在巡视坐标系下的坐标(X_E，Y_E，Z_E)，如下公式所示：

步骤16、当巡视器行驶到下一摄站时，根据指令获取的相机俯仰角和偏航角，重复步骤11-步骤15实现定位。

采用上述技术方案所产生的有益效果在于：本发明提供的一种基于单相机导航影像的巡视探测器定位方法，在仅有单个导航相机的条件下也可以完成巡视器的定位；将不同偏航角下的摄影中心间的相互位置关系作为约束条件，有效的减少了定位过程需要的控制点数量，仅仅采用两个控制点即可完成定位，在巡视器周围环境中特征点数量匮乏的情况下，定位成功率更高；相对现有的巡视器定位方法，本发明方法不需要惯性导航姿态设备获取巡视器在巡视坐标系下的三轴方向的旋转角，仅仅采用两个控制点即可实现巡视器的高精度定位，依赖的初始条件更少。

附图说明

图1为本发明实施例提供的一种基于单相机导航影像的巡视探测器定位方法的流程图；

图2为本发明实施例提供的巡视器部分坐标系与旋转轴的示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

一种基于单相机导航影像的巡视探测器定位方法，如图1所示，包括以下步骤：

步骤1、定义巡视坐标系、巡视器本体坐标系、巡视器零位坐标系、像平面直角坐标系、像空间直角坐标系及像空间辅助坐标系；

所述巡视坐标系：将巡视器与着陆器分离后即将开展巡视考察任务的摄站作为基准，即定义第一摄站位置的巡视器本体坐标系为巡视坐标系；

所述巡视器本体坐标系：原点位于巡视器箱体中心，X轴指向巡视器前进方向，Z轴指向月面铅锤方向，向上为正，Y轴按右手法则确定，如图2所示；

所述巡视器零位坐标系：如图2所示，巡视器立体视觉系统中的导航相机安装在桅杆上方的云台上，在水平方向绕桅杆偏航旋转轴进行竖直方向的旋转，称为偏航角，记为P；绕云台俯仰旋转轴进行俯仰，称为俯仰角，记为F；当巡视器桅杆处于初始状态，桅杆俯仰角和偏航角为零时，定义巡视器零位坐标系原点位于偏航旋转轴线和俯仰旋转轴线的交点，X轴与云台俯仰旋转轴线平行，Z轴与云台偏航旋转轴线平行，XYZ三轴构成右手系；

所述像平面直角坐标系：原点位于导航影像中心，像素的行方向为x方向，向右为正，列方向为y方向，向上为正；

所述像空间直角坐标系：原点位于影像摄影中心S处，x轴y轴分别平行于像平面直角坐标系的对应各轴，z轴和x轴y轴构成右手坐标系；

所述像空间辅助坐标系：原点位于影像摄影中心S处，x、y、z三轴方向分别平行于巡视器零位坐标系的三轴方向；

步骤2、巡视器定位即求解巡视探测器本体坐标系原点在巡视坐标系下的坐标；巡视探测器与着陆器分离后开始巡视考察任务；分离后的巡视探测器停留并拍照的位置，即当前摄站巡视探测器在巡视坐标系下的位置(X_E，Y_E，Z_E)，第一摄站巡视探测器位置为(0，0，0)；巡视坐标系与巡视探测器本体坐标系三轴方向夹角组成的旋转矩阵为R_E；

步骤3、巡视器零位时刻，桅杆偏航角度为0，俯仰角度为0，由安装参数或地面标定过程获得巡视器零位坐标系与巡视器本体坐标系在三轴方向的偏移量分别为(ΔX₀，ΔY₀，ΔZ₀)；

步骤4、巡视器零位时刻，某一导航相机摄影中心S的在巡视器零位坐标系下的坐标为(ΔX_S，ΔY_S，ΔZ_S)，由巡视器安装时的安装参数或地面标定过程获得；

步骤5、对于俯仰角度F，偏航角度P，某一导航相机摄影中心S在巡视器零位坐标系下的坐标(X_SP，Y_SP，Z_SP)如下公式所示：

其中，R_P、R_F分别为偏航角度P和俯仰角度F组成的旋转矩阵；

步骤6、设定导航相机焦距为f，导航影像中某一特征点在导航影像中对应的像点的像平面直角坐标为(x，y)，其对应的像空间辅助坐标系坐标为(X_T，Y_T，Z_T)，对应地面点在零位坐标系下的坐标为(X₀，Y₀，Z₀)，该导航相机的摄影中心S在零位坐标系下的位置和三轴旋转角为(X_SP，Y_SP0，Z_SP，0，F，P)，则：

其中，λ为比例系数；

令进一步整理，得：

步骤7、当导航影像中某一特征点在多幅导航影像中出现时，将当前导航影像对应的俯仰和偏航角度带入公式(3)，连列求解得到该特征点在零位坐标系下的坐标(X₀，Y₀，Z₀)，具体求解过程如下：

由于公式3为未知数的非线性方程，因此需要对其线性化后进行求解；令将公式(3)改写为误差方程的形式为：

v＝Ax-l (4)

其中，

上述系数矩阵A中各项偏导数求解过程与摄影测量领域中光束法平差中的求解过程类似，采用最小二乘公式x＝(A^TA)^-1(A^Tl)求解得到该特征点在零位坐标系下的坐标；

步骤8、求解特征点在巡视本体坐标系下的坐标(X_B，Y_B，Z_B)，如下公式所示：

步骤9、求解特征点在巡视坐标系下的坐标(X_D，Y_D，Z_D)，如下公式所示：

步骤10、导航相机在顺时针方向绕固定偏航角度拍摄影像序列，令左导航相机在第i次偏航时拍摄的照片为I_i；在导航相机拍摄的序列影像中提取多个特征点，通过步骤5-步骤9求解出各特征点对应的巡视坐标系下的坐标，并将它们作为巡视器下一个摄站的定位控制点；

步骤11、以步骤10中得到的特征点作为控制点，开始巡视器在第二摄站的定位；设定控制点出现在第二摄站的导航影像I_i和I_i+1中，像点坐标分别为(x_i，y_i)和(x_i+1，y_i+1)，其对应的俯仰角同为F，偏航角分别为P_i和P_i+1，影像对应的外方位元素分别为和其中，为导航影像I_i的摄影中心的位置，为导航影像I_i在像空间直角坐标系三轴方向的旋转角，为导航影像I_i+1的摄影中心的位置，为导航影像I_i+1在像空间直角坐标系三轴方向的旋转角，令两张影像的外方位元素中的三轴方向的夹角组成的旋转矩阵分别为R_i和R_i+1，零位坐标系此刻原点位置为(ΔX_S2，ΔY_S2，ΔZ_S2)，则：

将公式7整理得：

结合步骤10中的像点坐标和影像外方位元素，建立共线方程，如下公式所示：

其中，

步骤12、巡视器定位时外方位元素和为未知量，将公式8和公式9连列，并将其转为可按照最小二乘求解的误差方程的形式，如下公式所示：

X＝(B^TB)^-1B^TL (10)

其中，B为公式8和公式9对各项未知数的偏导数组成的系数矩阵，

矩阵B中外方位元素的偏导数和矩阵L中各项的求解过程与摄影测量学中外方位元素的各项参数求导过程一致；公式8对各项参数求偏导，结果如下：

由：

得：

步骤13、找到步骤11中提取的控制点在前一摄站的导航影像序列中的对应像点，并通过步骤5-步骤9求解其对应的巡视坐标系下的坐标；

步骤14、结合步骤13求解的控制点巡视坐标系下的坐标，在当前摄站导航影像序列中的像点坐标，代入公式10即可求解和由于公式中存在12个未知量，而一个控制点能够建立7个方程，因此，只需要至少2个点即可实现外方位元素的求解；

步骤15、根据导航影像摄影中心位置结合零位坐标系原点与摄影中心的偏移量、巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量，得到巡视器本体坐标系原点在巡视坐标系下的坐标，实现最终定位，具体方法为：

首先计算零位坐标系原点与摄影中心的偏移量(ΔX₁，ΔY₁，ΔZ₁)，如下公式所示：

然后计算巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量(ΔX₂，ΔY₂，ΔZ₂)，如下公式所示：

最终得到巡视器本体坐标系原点在巡视坐标系下的坐标(X_E，Y_E，Z_E)，如下公式所示：

步骤16、当巡视器行驶到下一摄站时，根据指令获取的相机俯仰角和偏航角，重复步骤11-步骤15实现定位。

本实施例还提供了采用本发明方法及经典摄影测量光束法进行巡视探测器定位的模拟实验，实验结果及两者的互差如表1所示：

表1两种方法的定位结果与互差

由表1中的互差可以得出，两种定位方法在X方向的平均互差为1.5cm，Y方向的平均互差为0.9cm，精度与经典光束法大致相当，证明本发明提出的基于单相机导航影像的巡视探测器定位方法能够有效的完成巡视器定位。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (4)

1.一种基于单相机导航影像的巡视探测器定位方法，其特征在于：包括以下步骤：

步骤1、定义巡视坐标系、巡视器本体坐标系、巡视器零位坐标系、像平面直角坐标系、像空间直角坐标系及像空间辅助坐标系；

步骤2、巡视探测器与着陆器分离后开始巡视考察任务；分离后的巡视探测器停留并拍照的位置，即当前摄站巡视探测器在巡视坐标系下的位置(X_E，Y_E，Z_E)，第一摄站巡视探测器位置为(0，0，0)；巡视坐标系与巡视探测器本体坐标系三轴方向夹角组成的旋转矩阵为R_E；

步骤3、巡视器零位时刻，桅杆偏航角度为0，俯仰角度为0，由安装参数或地面标定过程获得巡视器零位坐标系与巡视器本体坐标系在三轴方向的偏移量分别为(ΔX₀，ΔY₀，ΔZ₀)；

步骤4、巡视器零位时刻，某一导航相机摄影中心S的在巡视器零位坐标系下的坐标为(ΔX_S，ΔY_S，ΔZ_S)，由巡视器安装时的安装参数或地面标定过程获得；

步骤5、对于俯仰角度F，偏航角度P，某一导航相机摄影中心S在巡视器零位坐标系下的坐标(X_SP，Y_SP，Z_SP)如下公式所示：

其中，R_P、R_F分别为偏航角度P和俯仰角度F组成的旋转矩阵；

步骤6、设定导航相机焦距为f，导航影像中某一特征点在导航影像中对应的像点的像平面直角坐标为(x，y)，其对应的像空间辅助坐标系坐标为(X_T，Y_T，Z_T)，对应地面点在零位坐标系下的坐标为(X₀，Y₀，Z₀)，该导航相机的摄影中心S在零位坐标系下的位置和三轴旋转角为(X_SP，Y_SP0，Z_SP，0，F，P)，则：

其中，λ为比例系数；

令进一步整理，得：

步骤7、当导航影像中某一特征点在多幅导航影像中出现时，将当前导航影像对应的俯仰和偏航角度带入公式(3)，连列求解得到该特征点在零位坐标系下的坐标(X₀，Y₀，Z₀)；

步骤8、求解特征点在巡视本体坐标系下的坐标(X_B，Y_B，Z_B)，如下公式所示：

步骤9、求解特征点在巡视坐标系下的坐标(X_D，Y_D，Z_D)，如下公式所示：

步骤10、导航相机在顺时针方向绕固定偏航角度拍摄影像序列，令左导航相机在第i次偏航时拍摄的照片为I_i；在导航相机拍摄的序列影像中提取多个特征点，通过步骤5-步骤9求解出各特征点对应的巡视坐标系下的坐标，并将它们作为巡视器下一个摄站的定位控制点；

步骤11、以步骤10中得到的特征点作为控制点，开始巡视器在第二摄站的定位；设定控制点出现在第二摄站的导航影像I_i和I_i+1中，像点坐标分别为(x_i，y_i)和(x_i+1，y_i+1)，其对应的俯仰角同为F，偏航角分别为P_i和P_i+1，影像对应的外方位元素分别为和其中，为导航影像I_i的摄影中心的位置，为导航影像I_i在像空间直角坐标系三轴方向的旋转角，为导航影像I_i+1的摄影中心的位置，为导航影像I_i+1在像空间直角坐标系三轴方向的旋转角，令两张影像的外方位元素中的三轴方向的夹角组成的旋转矩阵分别为R_i和R_i+1，零位坐标系此刻原点位置为(ΔX_S2，ΔY_S2，ΔZ_S2)，则：

将公式7整理得：

结合步骤10中的像点坐标和影像外方位元素，建立共线方程，如下公式所示：

其中，

步骤12、巡视器定位时外方位元素和为未知量，将公式8和公式9连列，并将其转为可按照最小二乘求解的误差方程的形式，如下公式所示：

X＝(B^TB)^-1B^TL (10)

其中：B为公式8和9对各项未知数的偏导数组成的系数矩阵，

步骤13、找到步骤11中提取的控制点在前一摄站的导航影像序列中的对应像点，并通过步骤5-步骤9求解其对应的巡视坐标系下的坐标；

步骤14、结合步骤13求解的控制点巡视坐标系下的坐标，在当前摄站导航影像序列中的像点坐标，代入公式10即可求解和

步骤15、根据导航影像摄影中心位置结合零位坐标系原点与摄影中心的偏移量、巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量，得到巡视器本体坐标系原点在巡视坐标系下的坐标，实现最终定位；

步骤16、当巡视器行驶到下一摄站时，根据指令获取的相机俯仰角和偏航角，重复步骤11-步骤15实现定位。

2.根据权利要求1所述的一种基于单相机导航影像的巡视探测器定位方法，其特征在于：

所述巡视坐标系：将巡视器与着陆器分离后即将开展巡视考察任务的摄站作为基准，即定义第一摄站位置的巡视器本体坐标系为巡视坐标系；

所述巡视器本体坐标系：原点位于巡视器箱体中心，X轴指向巡视器前进方向，Z轴指向月面铅锤方向，向上为正，Y轴按右手法则确定；

所述巡视器零位坐标系：巡视器立体视觉系统中的导航相机安装在桅杆上方的云台上，在水平方向绕桅杆偏航旋转轴进行竖直方向的旋转，称为偏航角，记为P；绕云台俯仰旋转轴进行俯仰，称为俯仰角，记为F；当巡视器桅杆处于初始状态，桅杆俯仰角和偏航角为零时，定义巡视器零位坐标系原点位于偏航旋转轴线和俯仰旋转轴线的交点，X轴与云台俯仰旋转轴线平行，Z轴与云台偏航旋转轴线平行，XYZ三轴构成右手系；

所述像平面直角坐标系：原点位于导航影像中心，像素的行方向为x方向，向右为正，列方向为y方向，向上为正；

所述像空间直角坐标系：原点位于影像摄影中心S处，x轴y轴分别平行于像平面直角坐标系的对应各轴，z轴和x轴y轴构成右手坐标系；

所述像空间辅助坐标系：原点位于影像摄影中心S处，x、y、z三轴方向分别平行于巡视器零位坐标系的三轴方向。

3.根据权利要求1所述的一种基于单相机导航影像的巡视探测器定位方法，其特征在于：所述步骤7的具体求解过程如下：

由于公式3为未知数的非线性方程，因此需要对其线性化后进行求解；令将公式(3)改写为误差方程的形式为：

V＝Ax-l (4)

其中，

上述系数矩阵A中各项偏导数求解过程与摄影测量领域中光束法平差中的求解过程类似，采用最小二乘公式x＝(A^TA)^-1(A^Tl)求解得到该特征点在零位坐标系下的坐标。

4.根据权利要求1所述的一种基于单相机导航影像的巡视探测器定位方法，其特征在于：所述步骤15的具体方法为：

首先计算零位坐标系原点与摄影中心的偏移量(ΔX₁，ΔY₁，ΔZ₁)，如下公式所示：

然后计算巡视器本体坐标系原点与零位坐标系原点在巡视坐标系下的偏移量(ΔX₂，ΔY₂，ΔZ₂)，如下公式所示：

最终得到巡视器本体坐标系原点在巡视坐标系下的坐标(X_E，Y_E，Z_E)，如下公式所示：