CN106979787B - 一种基于立体导航影像的巡视器定位方法 - Google Patents

Abstract

一种基于立体导航影像的巡视器定位方法，属于深空探测技术领域，包括：获取左导航影像和右导航影像；定义坐标系；提取同名像点及对应的月面点坐标；计算右导航影像摄影中心的坐标；提取导航相机焦距f，计算R_俯和R_偏；按照最小二乘求解公式计算左导航影像摄影中心坐标；计算巡视器质心坐标完成巡视器定位；本发明仅有两对匹配特征点即可完成定位，匹配特征点极少的情况下也能完成定位，增加了方法的适用情况；只获取左导航相机的位置即可得到右导航相机的位置以及巡视器位置；起算数据不存在IMU和里程计等传感器的数据，因此不存在累积误差，克服了定位误差累积增大的缺点，增加了定位方法的适用性。

Description

一种基于立体导航影像的巡视器定位方法

技术领域

本发明属于深空探测技术领域，具体涉及一种基于立体导航影像的巡视器定位方法。

背景技术

巡视器的高精度导航定位技术是深空探测领域中的关键技术之一，高精度的定位结果不仅可以确保巡视器的安全，而且可以使巡视器与地面控制系统通信时具有较好的指向角度，进而为实现资源勘查等科学考察任务提供基础。目前基于摄影测量的巡视器定位方法研究现状如下：

石德乐等(2006)提出了以着陆器为基站，采用立体视觉测量和彩色图像分割技术进行跟踪测量的月面巡视探测器定位方法，对惯导系统加里程计的定位方法进行修正，减小月面巡视器的计算工作量，但是由于里程计的误差，导致该方法实际应用受到限制。刘传凯等(2014)提出了一种基于拼接图像的巡视器定位方法，首先将多幅导航影像进行拼接，然后通过特征匹配得到大量的匹配点，最终通过光束法平差完成巡视器定位。马友青等(2014)提出了基于LM法的光束法平差巡视器导航定位，利用LM算法代替牛顿迭代法完成光束法求解过程。王保丰等(2014)提出了一种基于计算机视觉的定位方法，将SIFT匹配、相关系数匹配、最小二乘匹配和光束法平差等多项技术融合，实现了相邻站间月面巡视器的导航定位。马友青等(2014)提出了基于立体图像的月球车导航定位算法，通过匹配得到了连接点，然后通过相对定向和绝对定向过程完成了巡视器定位。刘少创等通过像点匹配、前方交会完成图像的立体模型构建，然后根据不同摄站间的连接点序列建立立体图像条带网，最终通过最小二乘平差，直接获得月面巡视探测器的位置与姿态信息。吴伟仁等采用光束法平差完成了巡视器定位。刘召芹等(2014)通过对导航相机原始影像进行匹配定位实现月球车的连续定位；采用导航相机生成的DOM与降落相机生成的DOM进行SIFT匹配，并辅助人工选点匹配，完成巡视器基于DOM与底图匹配的定位。但是上述光束法定位方法中需要大量的匹配点作为定位起算数据。徐辛超等(2015)提出了基于空间后方交会迭代法的巡视器单摄站定位方法，但需要至少3个以上的控制点才能完成解算。

此外，针对巡视器的定位方法，国内外专家学者还提出了结合里程计和IMU的定位方法、IMU和摄影测量结合的方法等。里程计和IMU的定位方法中里程计会由于巡视器的打滑现象出现较大误差，从而导致定位结果累积误差增大。IMU和摄影测量结合的方法定位结果较好，但是该方法的起算数据不仅依赖导航影像，而且依赖IMU姿态数据，当IMU数据精度会随着时间增加出现一定的累积误差，会导致定位精度下降；甚至当IMU数据缺失的情况下也就无法完成巡视器定位。因此，需要研究一种依赖外部参数较少，且需要的起算匹配点数量也比较少的巡视器定位方法。

发明内容

针对上述现有技术存在的不足，本发明提供一种基于立体导航影像的巡视器定位方法。

本发明的技术方案如下：

一种基于立体导航影像的巡视器定位方法，包括如下步骤：

步骤1：获取左导航相机和右导航相机同时拍摄的一对立体导航影像即左导航影像和右导航影像；

步骤2：分别定义着陆器当地铅垂坐标系、巡视器零位坐标系、左像平面直角坐标系、右像平面直角坐标系和像空间直角坐标系；

步骤3：从左导航影像和右导航影像中提取同名像点的坐标，并根据导航相机标定的畸变参数对像点坐标进行误差纠正，得到左导航影像中同名像点在左像平面直角坐标系下的坐标[(x_l1,y_l1),(x_l2,y_l2),…，(x_li,y_li)，…，(x_ln,y_ln)]和右导航影像中同名像点在右像平面直角坐标系下的坐标[(x_r1,y_r1),(x_r2,y_r2),…,(x_ri,y_ri),…,(x_rn,y_rn)]，其中，i＝1,2,…,n，n为同名像点对数；

步骤4：从着陆区DOM及着陆区数字高程模型DEM中提取同名像点对应的月面点在着陆器当地铅垂坐标系下坐标[(X_A1,Y_A1,Z_A1),(X_A2,Y_A2,Z_A2),…，(X_Ai,Y_Ai,Z_Ai),…，(X_An,Y_An,Z_An)]；

步骤5：根据巡视器初始状态以及左右导航相机安装位置计算在左导航影像的像空间直角坐标系下右导航影像摄影中心的坐标(lx,ly,lz)；

步骤6：提取导航相机焦距f，并根据巡视器俯仰角度和偏航角度分别计算R_俯和R_偏；

步骤7：按照最小二乘求解公式X＝(A^TA)^-1A^TL，计算左导航影像摄影中心在着陆器当地铅垂坐标系下坐标(X_S1,Y_S1,Z_S1)及像空间辅助坐标系到着陆器当地铅垂坐标系三个轴的旋转角(ω，κ)，其中：

nx＝b2·lz-b3·ly，ny＝b3·lx-b1·lz，nz＝b1·ly-b2·lx，

为像空间直角坐标系与着陆器当地铅垂坐标系的旋转矩阵R对应的三角函数，

步骤8：根据巡视器零位时刻相对于巡视器零位坐标系原点在三个坐标轴方向的偏移量(ΔX，ΔY，ΔZ)、旋转矩阵R、R_俯、R_偏、左导航相机与巡视器零位坐标系原点之间的偏移量(O_X，O_Y，O_Z)及左导航影像摄影中心(X_S1，Y_S1，Z_S1)，按照如下公式计算巡视器质心坐标(X_C，Y_C，Z_C)，完成巡视器定位：

有益效果：本发明提出一种基于立体导航影像的巡视器定位方法，与现有技术相比，具有如下优点：

(1)相对于传统摄影测量定位的方法，至少只需要两对匹配特征点即可完成定位，匹配特征点极少的情况下也能完成定位，增加了方法的适用情况；

(2)首次结合左导航相机和右导航相机的安装位置参数，将右导航相机转换为左导航相机的位置的相关量，即只获取左导航相机的位置即可得到右导航相机的位置以及巡视器位置等信息；

(3)克服了巡视器行驶过程中由于打滑现象造成的里程计累积误差，进而导致采用IMU和里程计组合导航定位过程中，定位误差累积增大的缺点；

(4)由于起算数据中不存在IMU和里程计等传感器的数据，因此不存在累积误差，定位精度优于IMU和里程计组合定位方法；

(5)起算数据仅为左右导航相机的导航影像，依赖的传感器参数少，即使IMU、里程计等设备参数丢失也可以完成定位，增加了定位方法的适用性。

附图说明

图1为本发明一种实施方式的基于立体导航影像的巡视器定位方法流程图；

图2为本发明一种实施方式的巡视器零位坐标系示意图。

具体实施方式

下面结合附图对本发明的一种实施方式作详细说明。

如图1所示，本实施方式的基于立体导航影像的巡视器定位方法，包括：

步骤1：获取左导航相机和右导航相机同时拍摄的一对立体导航影像即左导航影像和右导航影像；

步骤2：分别定义着陆器当地铅垂坐标系、巡视器零位坐标系、左像平面直角坐标系、右像平面直角坐标系、像空间直角坐标系和像空间辅助坐标系；

本实施方式中，所述陆器当地铅垂坐标系原点位于着陆器质心在地面的投影点，Z轴为铅垂线向下方向，XY轴位于与铅垂线垂直的平面内，X轴指向北方向，Y轴指向东方向。月面巡视器导航定位是以着陆器当地铅垂坐标系为参考。

如图2所示，所述巡视器零位坐标系原点位于巡视器桅杆偏航旋转轴线与云台俯仰旋转轴线的交点O，X轴与云台俯仰旋转轴线重合，Y轴为桅杆旋转角为零度时刻，在水平面内垂直于云台俯仰旋转轴线向前，Z轴与XY轴构成右手系；巡视器立体视觉系统中的导航相机安装在桅杆上方的云台上，可以在水平方向绕桅杆进行旋转和垂直方向的俯仰。导航影像在拍摄时，为了拍摄周围地形，围绕桅杆进行一定角度的旋转，称为偏航角，记为P；围绕云台旋转轴进行一定角度的旋转，称为俯仰角，记为F。

所述左像平面直角坐标系(左影像平面内)原点在左像像主点，X轴方向为水平方向向右，平行于影像像素排列的行方向，Y轴方向为垂直向上，平行于像素排列的列方向。

所述右像平面直角坐标系(右影像平面内)原点在右像像主点，X轴方向为水平方向向右，平行于影像像素排列的行方向，Y轴方向为垂直向上，平行于像素排列的列方向。

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

所述像空间辅助坐标系原点位于左导航影像摄影中心S处，三轴方向分别平行于着陆器当地铅垂坐标系的三轴方向。

步骤3：从两张导航影像中提取匹配特征点即同名像点的坐标，并根据导航相机标定的畸变参数对像点坐标进行误差纠正，得到左导航影像中像点在左像平面直角坐标系下的坐标[(x_l1,y_l1),(x_l2,y_l2),…，(x_li,y_li)，…，(x_ln,y_ln)]和右导航影像中同名像点在右像平面直角坐标系下的坐标[(x_r1,y_r1),(x_r2,y_r2),…,(x_ri,y_ri),…,(x_rn,y_rn)]，其中，i＝1,2,…,n，n为匹配特征点对数；

步骤4：从着陆区DOM及着陆区数字高程模型DEM中提取匹配特征点对应的各月面点在着陆器当地铅垂坐标系下坐标[(X_A1,Y_A1,Z_A1),(X_A2,Y_A2,Z_A2),…，(X_Ai,Y_Ai,Z_Ai),…，(X_An,Y_An,Z_An)]；

步骤5：根据巡视器初始状态即无俯仰角度和偏航角度时的状态以及左右导航相机安装位置计算在左导航影像的像空间直角坐标系下右导航影像摄影中心的坐标(lx,ly,lz)；

步骤6：提取导航相机焦距f，并根据巡视器俯仰角度和偏航角度分别计算R_俯和R_偏；

步骤7：按照最小二乘求解公式X＝(A^TA)^-1A^TL，计算左导航影像摄影中心在着陆器当地铅垂坐标系下坐标(X_S1,Y_S1,Z_S1)及像空间辅助坐标系到着陆器当地铅垂坐标系三个轴的旋转角(ω，κ)：

其中：

上述最小二乘公式X＝(A^TA)^-1A^TL推导过程如下：

在左导航影像和右导航影像中提取月面点对应的同名像点坐标(x_li,y_li)和(x_ri,y_ri)，同名像点对应的像空间直角坐标系坐标为(x_li,y_li,-f)和(x_ri,y_ri,-f)，像空间辅助坐标系坐标为(X_li,Y_li,Z_li)和(X_ri,Y_ri,Z_ri)，存在如下等式：

其中，R为左导航影像的像空间直角坐标系与像空间辅助坐标系的三个坐标轴之间旋转角(ω，κ)组成的旋转矩阵，同时，由于着陆器当地铅垂坐标系与像空间辅助坐标系三轴平行，因此，(ω，κ)即为像空间直角坐标系与着陆器当地铅垂坐标系的三轴旋转角；

为旋转矩阵R对应的三角函数。

左导航影像摄影中心在着陆器当地铅垂坐标系下坐标为(X_S1,Y_S1,Z_S1)，右导航影像摄影中心着陆器当地铅垂坐标系下坐标为(X_S2,Y_S2,Z_S2)，对左导航影像有：

对右导航影像有：

在左导航影像的像空间直角坐标系下，左导航影像摄影中心坐标为(0,0,0)，根据左右导航相机安装位置可以得到右导航影像摄影中心的坐标(lx,ly,lz)；在左导航影像的像空间辅助坐标系下，左导航影像摄影中心坐标为(0,0,0)，右导航影像摄影中心坐标为(B_X,B_Y,B_Z)，存在如下关系式：

由于像空间辅助坐标系坐标轴指向与着陆器当地铅垂线坐标系指向一致，存在如下关系式：

结合式(1)和式(2)及旋转矩阵R，可以得到：

由于左导航影像和右导航影像姿态角相同，因此旋转矩阵R也相同，结合式(1)、式(3)和式(5)，可以得到如下关系式：

根据式(6)和式(7)可得：

将式(8)化为误差方程，整理得到系数矩阵A、常数项L、未知数X，得到最小二乘公式X＝(A^TA)A^TL。所述未知数X包括(X_S1，Y_S1，Z_S1)和(ω，κ)。

系数矩阵A中，各偏导数的求解过程如下：公式(8)中，式1和式2对于(X_S1，Y_S1，Z_S1)和(ω，κ)6个变量进行线性化，并求解偏导数的方法，可参照摄影测量学领域中空间后方交会原理中对各变量求解偏导数的方法；

针对式3和式4进行线性化并求解偏导数的方法，具体如下：

首先对公式(8)中式3和式4进行化简如下：

对(X_S1,Y_S1,Z_S1)求偏导得：

同理可得：

对求偏导得：

令则有：

对ω求偏导得：

对κ求偏导得：

步骤8：将摄影中心位置(X_S1，Y_S1，Z_S1)代入下式，计算巡视器质心在当地铅垂坐标系下坐标(X_C，Y_C，Z_C)，完成巡视器定位：

上式的推导过程为：巡视器零位坐标系下，根据巡视器零位时刻左导航相机安装位置得到左导航影像与巡视器零位坐标系原点之间的偏移量为(O_X，O_Y，O_Z)，可得巡视器零位坐标系原点在着陆器当地铅垂坐标系下的坐标(X_O，Y_O，Z_O)为：

其中：R_俯、R_偏分别为根据巡视器立体视觉系统俯仰角度和滚动角度计算的旋转矩阵；

由巡视器设计参数得到巡视器质心在着陆器当地铅垂坐标系下坐标为(X_C，Y_C，Z_C)，巡视器初始状态相对于巡视器零位坐标系原点在三个坐标轴方向的偏移量分别为(ΔX，ΔY，ΔZ)，则可以得到以下关系式：

将式(10)代入式(11)可得：

巡视器在前10个测站利用基于立体导航影像的巡视器定位方法采用两点定位的结果和经典摄影测量光束法定位结果以及两者的互差，如表1所示：

表1定位结果及互差表

由表1互差可以得出，基于立体导航影像的巡视器定位方法能够有效的完成月面巡视器定位。

Claims (3)

1.一种基于立体导航影像的巡视器定位方法，其特征在于，包括如下步骤：

步骤1：获取左导航相机和右导航相机同时拍摄的一对立体导航影像即左导航影像和右导航影像；

步骤2：分别定义着陆器当地铅垂坐标系、巡视器零位坐标系、左像平面直角坐标系、右像平面直角坐标系和像空间直角坐标系；

步骤3：从左导航影像和右导航影像中提取同名像点的坐标，并根据导航相机标定的畸变参数对像点坐标进行误差纠正，得到左导航影像中同名像点在左像平面直角坐标系下的坐标[(x_l1,y_l1),(x_l2,y_l2),…，(x_li,y_li)，…，(x_ln,y_ln)]和右导航影像中同名像点在右像平面直角坐标系下的坐标[(x_r1,y_r1),(x_r2,y_r2),…,(x_ri,y_ri),…,(x_rn,y_rn)]，其中，i＝1,2,…,n，n为同名像点对数；

步骤4：从着陆区DOM及着陆区数字高程模型DEM中提取同名像点对应的月面点在着陆器当地铅垂坐标系下坐标[(X_A1,Y_A1,Z_A1),(X_A2,Y_A2,Z_A2),…，(X_Ai,Y_Ai,Z_Ai),…，(X_An,Y_An,Z_An)]；

步骤5：根据巡视器初始状态以及左右导航相机安装位置计算在左导航影像的像空间直角坐标系下右导航影像摄影中心的坐标(lx,ly,lz)；

步骤6：提取导航相机焦距f，并根据巡视器俯仰角度和偏航角度分别计算R_俯和R_偏；

其中，在巡视器零位坐标系中，围绕桅杆偏航旋转轴线即X轴进行一定角度的旋转，称为偏航角，计算偏航角的旋转矩阵计为R_偏；在巡视器零位坐标系中，围绕云台俯仰旋转轴线即Z轴进行一定角度的旋转，称为俯仰角，计算俯仰角的旋转矩阵计为R_俯；

步骤7：计算左导航影像摄影中心在着陆器当地铅垂坐标系下坐标(X_S1，Y_S1，Z_S1)及像空间直角坐标系到着陆器当地铅垂坐标系三个轴的旋转角

步骤8：根据巡视器零位时刻相对于巡视器零位坐标系原点在三个坐标轴方向的偏移量(ΔX，ΔY，ΔZ)、旋转角R_俯、R_偏、左导航相机与巡视器零位坐标系原点之间的偏移量(O_X，O_Y，O_Z)及左导航影像摄影中心在着陆器当地铅垂坐标系下坐标(X_S1，Y_S1，Z_S1)，计算巡视器质心在着陆器当地铅垂坐标系下坐标(X_C，Y_C，Z_C)，完成巡视器定位。

2.根据权利要求1所述的基于立体导航影像的巡视器定位方法，其特征在于，所述步骤7具体方法为：

按照最小二乘求解公式X＝(A^TA)^-1A^TL，计算左导航影像摄影中心在着陆器当地铅垂坐标系下坐标(X_S1,Y_S1,Z_S1)及像空间直角坐标系到着陆器当地铅垂坐标系三个轴的旋转角其中：

nx＝b2·lz-b3·ly，ny＝b3·lx-b1·lz，nz＝b1·ly-b2·lx，

3.根据权利要求1所述的基于立体导航影像的巡视器定位方法，其特征在于，所述步骤8具体为：按照如下公式计算巡视器质心在着陆器当地铅垂坐标系下坐标(X_C，Y_C，Z_C)：

其中，R为根据旋转角计算的像空间直角坐标系与着陆器当地铅垂坐标系的旋转矩阵，