CN108871349A - 一种深空探测器光学导航位姿加权确定方法 - Google Patents

Abstract

本发明公开的一种深空探测器光学导航位姿加权确定方法，涉及深空探测器位姿确定方法，属于深空探测技术领域。本发明实现方法如下：在导航相机图像中利用图像处理算法检测和提取行星表面陨石坑信息；对检测提取的陨石坑信息进行椭圆拟合与陨石坑中心定位；利用拟合的椭圆方程，根据不确定性传播特性，求取测量误差协方差矩阵来描述陨石坑中心的定位不确定性；利用测量不确定性对陨石坑中心进行加权，使得不同陨石坑中心在位姿估计算法中的权值大小不同，进而提高位姿估计的精确性，即基于测量不确定性加权方法实现深空探测器位姿的确定。本发明能够提高位姿确定的精确性，具有实现方法简单的优点。本发明适用于利用陨石坑作为导航路标进行位姿确定的导航系统。

Description

一种深空探测器光学导航位姿加权确定方法

技术领域

本发明涉及深空探测器位姿确定方法，具体涉及适用于利用陨石坑作为导航路标进行位姿确定的导航系统的深空探测器位姿确定方法，属于深空探测技术领域。

背景技术

深空探测航行距离远、时间长，采用传统的测控方式存在较大的测量延迟，技术操作难度大，深空探测通信成本高。传统的惯性导航虽不需要地面深空测控网的支持，但是星载惯性器件在使用过程中存在不同程度的零漂误差、随机误差以及计算误差积累，无法满足深空探测的精确导航任务要求。随着计算机硬件技术的突破和光学敏感器件的发展，使得基于星载计算机和光学导航相机的自主光学导航方法成为研究热点。其中，行星表面陨石坑作为天然地理地形特征路标，无需探测器额外携带路标载荷，有效减少任务复杂度，应用前景广泛。以陨石坑光学信息为主的位姿估计方法已经成为深空探测自主导航的重要方式，如何利用陨石坑光学信息来提高位姿估计精度是当前各国航天科研人员重点关注的研究方向之一。

在已发展的深空探测器位姿估计方法中，在先技术[1](Dementhon D F,Davis LS.Model-based object pose in 25lines of code[J].International Journal ofComputer Vision,1995,15(1-2):123-141.)，针对单张图像，利用四个或更多非共面特征点，在迭代过程中反复地把透视投影模型近似为2次投影的合成，先通过正交投影把所有的三维物体点平行于光轴投影到一个与光轴垂直的合适的空间平面上，然后将该空间平面上的点再透视投影到像平面上，不断找到近似的姿态，直至收敛到精确的估计值。但是该算法没有考虑到用于估计的物体观测误差对迭代的影响，随着迭代次数的增加会产生误差积累的问题。

在先技术[2](参见W.Shao,et al.,Attitude and position determinationbased on craters for precision landing,Proc IMechE Part G:J AerospaceEngineering.2016,Vol.230(10)1934-1942.)，针对光学自主导航问题，提出了一种实时的利用至少三条陨石坑提取边缘的椭圆曲线的位姿估计方法，利用椭圆曲线和二维图像的几何与代数约束，并利用克罗内克积建立了着陆器位置姿态的线性方程。但该方法并没有考虑陨石坑提取时边缘点误差对位姿估计的影响，因此该方法只适用于边缘提取足够精确且没有误匹配的情况。

在先技术[3](参见周润,张征宇,黄叙辉.相机位姿估计的加权正交迭代算法[J].光学学报,2018,38(05):193-199.)，针对测量误差，提出了一种相机位姿估计的加权正交迭代算法，以加权共线误差为目标函数，根据像面重投影误差自动调节权重系数，优化相机位姿估计结果。但是该方法是以共线误差方程为基础加入的权重，并不适用于深空探测以陨石坑边缘特征点为观测量，最终要得到陨石坑中心不确定性的位姿确定。

利用陨石坑光学信息进行深空探测器位姿确定过程中，陨石坑边缘检测不可避免的存在观测误差，使得陨石坑中心存在定位不确定性，现有的深空探测器位姿确定方法未考虑陨石坑中心定位不确定性的影响，进而导致探测器位姿解算精度低。

发明内容

针对现有的深空探测器位姿确定方法未考虑陨石坑中心定位不确定性的影响，进而导致探测器位姿解算精度低的问题。本发明公开的一种深空探测器光学导航位姿加权确定方法要解决的技术问题是：基于测量不确定性加权方法实现深空探测器位姿的确定，通过考虑陨石坑中心定位的不确定性，将原始的陨石坑中心加入测量不确定性的权重，使得不同陨石坑中心在位姿估计算法中的权值大小不同，进而提高位姿确定的精确性，具有实现方法简单的优点。

本发明是通过下述技术方案实现的。

本发明公开的一种深空探测器光学导航位姿加权确定方法，在导航相机图像中利用图像处理算法检测和提取行星表面陨石坑信息。对检测提取的陨石坑信息进行椭圆拟合与陨石坑中心定位。利用拟合的椭圆方程，根据不确定性传播特性，求取测量误差协方差矩阵来描述陨石坑中心的定位不确定性。利用测量不确定性对陨石坑中心进行加权，使得不同陨石坑中心在位姿估计算法中的权值大小不同，进而提高位姿估计的精确性。即基于测量不确定性加权方法实现深空探测器位姿的确定。

本发明公开的一种深空探测器光学导航位姿加权确定方法，包括如下步骤：

步骤1：在导航相机图像中利用图像处理算法检测和提取行星表面陨石坑信息。

读取光学相机拍摄到的目标天体表面地形图像之后，基于图像分割和形态学处理算法等计算机图形处理技术对图像进行陨石坑边缘的检测和提取。

步骤1行星表面陨石坑信息包括图像处理得到的陨石坑边缘点的像素值。

步骤2：对步骤1中检测提取的陨石坑信息进行椭圆拟合与陨石坑中心定位。

当观测椭圆边界点不少于五个时，通过最小二乘法确定陨石坑拟合椭圆方程的系数B,C,D,E,F，进而确定椭圆定位定向参数，得到陨石坑边缘的拟合椭圆方程为x²+2Bxy+Cy²+2Dx+2Ey+F＝0。拟合椭圆的中心(x₀,y₀)为

其中，B,C,D,E,F为椭圆方程的系数。

步骤3：利用步骤2中拟合的椭圆方程，根据不确定性传播特性，求取测量误差协方差矩阵来描述陨石坑中心的定位不确定性。

对n个边缘点写出n个椭圆方程，并用矩阵表示n个误差方程，则求得各特征点的协方差矩阵P，将各特征点视为具有相同方差σ²来处理。最后，根据协方差传播率，由P产生每个陨石坑中心的误差协方差矩阵来描述陨石坑中心的定位不确定性。

其中，K是由步骤1中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵。

作为优选，步骤3实现方法为：

对n个边缘点写出n个椭圆一般方程，并用矩阵表示误差方程为

令

则误差方程写为V＝AX+B。

各特征点的协方差矩阵P为

其中，为V的测量噪声方差阵，I是n×n的单位矩阵，σ²为n个特征点的方差，将各特征点视为具有相同方差σ²来处理。最后，根据协方差传播率，由P产生各陨石坑中心的误差协方差矩阵来描述陨石坑中心的定位不确定性。

其中，K是由步骤2中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵。

将每个陨石坑中心计算得到的误差椭圆画在拍摄的导航图像上，能够直观地显示出拟合陨石坑中心的方向和大小不确定性。

步骤4：利用测量不确定性对陨石坑中心进行加权并解算位置姿态，即基于测量不确定性加权方法实现深空探测器位姿的确定。

对步骤3中求取的测量误差协方差矩阵进行奇异值分解，

利用协方差矩阵定义仿射变换矩阵F

F＝diag(1/σ₁,1/σ₂,1)U^T

(u′,v′,1)^T＝F(u,v,1)^T

经过仿射变换后，得到加权后的像素坐标点(u′,v′)^T，结合已建立的地形数据库中上述像素点对应的陨石坑三维坐标点，将这些数据输入到改进的比例正交投影迭代变换POSIT算法中，即得到更为精确的位姿估计解，即基于测量不确定性加权方法实现深空探测器位姿的确定。

步骤4所述的输入到改进的比例正交投影迭代变换POSIT算法中即得到更为精确的位姿估计解，具体实现方法如下：

待求的探测器的旋转矩阵R和平移矩阵t分别为

定义检测得到的陨石坑中心在导航相机图像平面像素坐标系下的齐次坐标为P_i＝(u_i,v_i,1)^T，非齐次坐标为像点对应的陨石坑中心在目标天体固连坐标系下的匹配得到的齐次坐标为非齐次坐标为其中，i＝1,2,…,n，n为陨石坑匹配数。图像点P_i和数据库匹配点F_i ^w的透视投影关系为

λ_iP_i＝K[R t]P_i ^w

式中，为摄像机内参矩阵，f为导航相机焦距，λ_i为数据库匹配点P_i ^w的深度值。

定义(x,y)是特征点在图像平面物理坐标系的像点坐标，(x′,y′)是特征点经过仿射变换后在图像平面物理坐标系的像点坐标，(X^C,Y^C,Z^C)是特征点在光学导航相机坐标系的坐标，特征点和像点的关系为

则透视投影关系表示为

上式两边同时除以t_z得到

式中，λ＝Z^C/t_z，s＝f/t_z

加入不确定性权阵，将等式左右两边分别左乘仿射变换矩阵得

展开得

由于在探测器任务期间，行星表面各特征点在导航相机坐标系中的z轴变化范围，所述z轴变化范围远远小于行星表面特征点到相机的平均深度。因此认为λ＝Z^C/t_z≈1。

λ＝Z^C/t_z≈1是比例正交投影迭代变换POSIT算法的迭代初值。在λ＝Z^C/t_z≈1初始状态下，物体的所有点在同一个深度上时，透视变换退化为一个比例正交投影。由于给定λ一个估计值，因此先将其看做已知量，删掉第三行方程中就少了4个未知量，更方便求解，即得到

展开得迭代方程为

求解8个未知量[sR₁₁ sR₁₂ sR₁₃ sR₂₁ sR₂₂ sR₂₃ st_x st_y]^T利用最小二乘法求解如下方程

由于λ被视为已知量，一个特征点及对应图像坐标得到2个独立方程，方程中总共有8个独立变量，拆成3个向量分别为

由此计算得到向量sR₁,sR₂，同时R₁,R₂均为单位向量。由于存在计算误差，向量sR₁,sR₂的模长不一定相等，取因此计算得到

t_z＝f/s

t_x＝(st_x)/s

t_y＝(st_y)/s

R₁＝(sR₁)/s

R₂＝(sR₂)/s

R₃＝R₁×R₂

由于迭代初始时，λ≈1，通过上式得到初始的旋转矩阵和平移矩阵，代入下式

λ＝[R₃T/t_z 1][X^W Y^W Z^W 1]^T

通过迭代更新使每个特征点都有更加精确的迭代系数λ，将更新值代入迭代方程中，循环往复，逐步逼近位姿估计的精确解，直至旋转矩阵R和平移向量t稳定时停止迭代。

还包括步骤5：将步骤4基于测量不确定性加权方法确定的深空探测器位姿，用于目标天体表面存在陨石坑导航路标的深空探测器位姿解算，进而实现用于目标天体表面存在陨石坑导航路标的深空探测器的导航。

有益效果：

1、本发明公开的一种深空探测器光学导航位姿加权确定方法，由于考虑陨石坑中心定位的不确定性，将原始的陨石坑中心加入测量不确定性的权重，不确定性大的权重小，不确定性小的权重大，使得不同陨石坑中心在位姿估计算法中的权值大小不同，进而提高位姿估计的精确性。

2、本发明公开的一种深空探测器光学导航位姿加权确定方法，能自主地确定探测器的位置和姿态，能够用于目标天体表面存在陨石坑导航路标的深空探测器，实现用于目标天体表面存在陨石坑导航路标的深空探测器的导航任务。

附图说明

图1是本发明的一种深空探测器光学导航位姿加权确定方法流程示意图；

图2是本发明位姿确定过程中导航相机与像平面和行星表面关系示意图；

图3是本发明步骤1图像处理效果示意图，其中图3a)是导航相机拍摄的原始图像，图3b)是原始图像经过图像处理之后的效果图；

图4是本发明步骤2椭圆拟合定位效果示意图；

图5是本发明步骤3陨石坑边缘拟合椭圆与陨石坑中心误差椭圆示意图；

图6是本发明实例中步骤3模拟的陨石坑边缘的拟合椭圆与陨石坑中心误差椭圆的像平面图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

为了验证本发明的可行性，利用拍摄到的小行星Eros 433表面陨石坑图像信息，将陨石坑经图像处理之后的拟合椭圆及中心作为数学仿真的真实陨石坑边缘和中心，如图5所示，并定义各陨石坑中心在小天体固连坐标系下的深度，得到像点对应的陨石坑中心在世界坐标系下的三维坐标取噪声值为5来模拟图像处理过程中的误差，并假设世界坐标系原点为[500,-300,2000]^T，探测器在世界坐标系下的初始位置为[0,0,2000]^T，初始姿态为[0°,180°,0°]。视场角大小为30°，导航相机焦距为f＝8mm，进行数学仿真验证。

本实施例公开的一种深空探测器光学导航位姿加权确定方法，包括如下步骤：

步骤1：在导航相机图像中利用图像处理算法检测和提取行星表面陨石坑信息。

读取光学相机拍摄到的Eros 433表面地形图像之后，对图像进行高斯噪声滤除，利用LOG算子自动选择阈值进行边缘检测，之后基于八连通法保留连通区域，剔除非边缘点，将光照方向作为先验信息，最后利用光照方向与梯度方向的内积去除伪边缘，完成对24个陨石坑边缘的检测和提取。

步骤2：对步骤1中检测提取的陨石坑信息进行椭圆拟合与陨石坑中心定位。

通过最小二乘法确定24组陨石坑的拟合椭圆方程的系数B_i,C_i,D_i,E_i,F_i,i＝1,2,3...24，进而确定椭圆定位定向参数，得到陨石坑边缘的拟合椭圆方程为x²+2Bxy+Cy²+2Dx+2Ey+F＝0。拟合椭圆的中心(x₀,y₀)为

步骤3：利用步骤2中拟合的椭圆方程，根据不确定性传播特性，求取测量误差协方差矩阵来描述陨石坑中心的定位不确定性。

对每个陨石坑，n个边缘点写出n个椭圆方程，并用矩阵表示n个误差方程，则求得各特征点的协方差矩阵P，将各特征点视为具有相同方差σ²＝5·I_n×n来处理。最后，根据协方差传播率，由P产生每个陨石坑中心的误差协方差矩阵来描述陨石坑中心的定位不确定性。

其中，K是由步骤1中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵。

步骤3具体实现方法为：

对n个边缘点写出n个椭圆一般方程，并用矩阵表示误差方程为

令

则误差方程写为V＝AX+B。

各特征点的协方差矩阵P为

其中，为V的测量噪声方差阵，I是n×n的单位矩阵，σ²为n个特征点的方差。最后，根据协方差传播率，由P产生各陨石坑中心的误差协方差矩阵

其中，K是由步骤2中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵。

如图6所示，将拟合椭圆和中心误差椭圆画在模拟的真实陨石坑图像上，能够直观地显示出拟合陨石坑中心的方向和大小不确定性。

步骤4：利用测量不确定性对陨石坑中心进行加权并解算位置姿态，即基于测量不确定性加权方法实现深空探测器位姿的确定。

对24个陨石坑，利用步骤3中求取的测量误差协方差矩阵进行奇异值分解，

利用协方差矩阵求仿射变换矩阵F

(u′,v′,1)^T＝F(u,v,1)^T

经过仿射变换后，得到加权后的像素坐标点(u′,v′)^T，结合已定义的像素点对应的陨石坑三维坐标点，将这些数据输入到改进的比例正交投影迭代变换POSIT算法中，即得到更为精确的位姿估计解，即基于测量不确定性加权方法实现深空探测器位姿的确定。

步骤4所述的输入到改进的比例正交投影迭代变换POSIT算法中即得到更为精确的位姿估计解，具体实现方法如下：

设待求的探测器的旋转矩阵R和平移矩阵t分别为

定义检测得到的陨石坑中心在导航相机图像平面像素坐标系下的齐次坐标为P_i＝(u_i,v_i,1)^T，非齐次坐标为像点对应的陨石坑中心在目标天体固连坐标系下的齐次坐标为非齐次坐标为其中，i＝1,2,…,24。图像点P_i和数据库匹配点F_i ^w的透视投影关系为

λ_iP_i＝K[R t]P_i ^w

式中，为摄像机内参矩阵，f为导航相机焦距，λ_i为P_i ^w的深度值。

定义(x,y)是特征点在图像平面物理坐标系的像点坐标，(x′,y′)是特征点经过仿射变换后在图像平面物理坐标系的像点坐标，(XC,YC,ZC)是特征点在光学导航相机坐标系的坐标，特征点和像点的关系为

则透视投影关系表示为

上式两边同时除以t_z得到

式中，λ＝Z^C/t_z，s＝f/t_z

加入不确定性权阵，将等式左右两边分别左乘仿射变换矩阵得

展开得

由于在探测器任务期间，行星表面各特征点在导航相机坐标系中的z轴变化范围，所述z轴变化范围远远小于行星表面特征点到相机的平均深度。因此认为λ＝Z^C/t_z≈1。

λ＝Z^C/t_z≈1是比例正交投影迭代变换POSIT算法的迭代初值。在λ＝Z^C/t_z≈1初始状态下，物体的所有点在同一个深度上时，透视变换退化为一个比例正交投影。由于给定λ一个估计值，因此先将其看做已知量，删掉第三行方程中就少了4个未知量，更方便求解，即得到

展开得迭代方程为

求解8个未知量[sR₁₁ sR₁₂ sR₁₃ sR₂₁ sR₂₂ sR₂₃ st_x st_y]^T，利用最小二乘法求解如下方程

由于λ被视为已知量，一个特征点及对应图像坐标得到2个独立方程，方程中总共有8个独立变量，拆成3个向量分别为

由此计算得到向量sR₁,sR₂，同时R₁,R₂均为单位向量。由于存在计算误差，向量sR₁,sR₂的模长不一定相等，取因此计算得到

t_z＝f/s

t_x＝(st_x)/s

t_y＝(st_y)/s

R₁＝(sR₁)/s

R₂＝(sR₂)/s

R₃＝R₁×R₂

由于迭代初始时，λ≈1，通过上式得到初始的旋转矩阵和平移矩阵，代入下式

通过迭代更新使每个特征点都有更加精确的迭代系数λ，将更新值代入迭代方程中，循环往复，逐步逼近位姿估计的精确解，直至旋转矩阵R和平移向量t稳定时停止迭代。

通过1000次的蒙特卡罗仿真得到两种方法的位姿估计精度如下表所示

表1探测器位姿估计结果

	姿态误差	位置误差
			经典算法	[-1.0519,-0.5774,0.0672]T	[0.8291,-0.7906,2.1456]T
加权算法	[-0.0198,0.6649,0.0525]T	[0.5275,0.1482,1.6599]T

由计算结果，本发明提出的方法在相同条件下比传统方法姿态精度能够提高44.48％，位置精度提高28.13％，仿真结果验证了本发明的有效性。由此可知，不确定性越大，陨石坑中心误差就越大，该点在位姿算法中的权重就越小，其误差对整体估算的影响就越小，深空探测器位姿估计的精度就越高。

包括步骤5：将步骤4基于测量不确定性加权方法确定的深空探测器位姿，用于目标天体表面存在陨石坑导航路标的深空探测器位姿解算，进而实现用于目标天体表面存在陨石坑导航路标的深空探测器的导航。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种深空探测器光学导航位姿加权确定方法，其特征在于：包括如下步骤，

步骤1：在导航相机图像中利用图像处理算法检测和提取行星表面陨石坑信息；

步骤2：对步骤1中检测提取的陨石坑信息进行椭圆拟合与陨石坑中心定位；

步骤3：利用步骤2中拟合的椭圆方程，根据不确定性传播特性，求取测量误差协方差矩阵来描述陨石坑中心的定位不确定性；

步骤4：利用测量不确定性对陨石坑中心进行加权并解算位置姿态，即基于测量不确定性加权方法实现深空探测器位姿的确定。

2.如权利要求1所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：还包括步骤5：将步骤4基于测量不确定性加权方法确定的深空探测器位姿，用于目标天体表面存在陨石坑导航路标的深空探测器位姿解算，进而实现用于目标天体表面存在陨石坑导航路标的深空探测器的导航。

3.如权利要求1或2所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤1具体实现方法为，

读取光学相机拍摄到的目标天体表面地形图像之后，基于图像分割和形态学处理算法对图像进行陨石坑边缘的检测和提取；

步骤1行星表面陨石坑信息包括图像处理得到的陨石坑边缘点的像素值。

4.如权利要求3所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤2具体实现方法为，

当观测椭圆边界点不少于五个时，通过最小二乘法确定陨石坑拟合椭圆方程的系数B,C,D,E,F，进而确定椭圆定位定向参数，得到陨石坑边缘的拟合椭圆方程为x²+2Bxy+Cy²+2Dx+2Ey+F＝0；拟合椭圆的中心(x₀,y₀)为

其中，B,C,D,E,F为椭圆方程的系数。

5.如权利要求4所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤3具体实现方法为，

对n个边缘点写出n个椭圆方程，并用矩阵表示n个误差方程，则求得各特征点的协方差矩阵P，将各特征点视为具有相同方差σ²来处理；最后，根据协方差传播率，由P产生每个陨石坑中心的误差协方差矩阵来描述陨石坑中心的定位不确定性；

其中，K是由步骤1中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵。

6.如权利要求5所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤4具体实现方法为，

对步骤3中求取的测量误差协方差矩阵进行奇异值分解，

利用协方差矩阵定义仿射变换矩阵F

F＝diag(1/σ₁,1/σ₂,1)U^T

(u′,v′,1)^T＝F(u,v,1)^T

经过仿射变换后，得到加权后的像素坐标点(u′,v′)^T，结合已建立的地形数据库中上述像素点对应的陨石坑三维坐标点，将这些数据输入到改进的比例正交投影迭代变换POSIT算法中，即得到更为精确的位姿估计解，即基于测量不确定性加权方法实现深空探测器位姿的确定。

7.如权利要求6所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤3优选实现方法为，

对n个边缘点写出n个椭圆一般方程，并用矩阵表示误差方程为

令

则误差方程写为V＝AX+B；

各特征点的协方差矩阵P为

其中，为V的测量噪声方差阵，I是n×n的单位矩阵，σ²为n个特征点的方差，将各特征点视为具有相同方差σ²来处理；最后，根据协方差传播率，由P产生各陨石坑中心的误差协方差矩阵来描述陨石坑中心的定位不确定性；

其中，K是由步骤2中的形心公式求偏导所得的多项式参数的系数B,C,D,E,F构成的矩阵；

将每个陨石坑中心计算得到的误差椭圆画在拍摄的导航图像上，能够直观地显示出拟合陨石坑中心的方向和大小不确定性。

8.如权利要求7所述的一种深空探测器光学导航位姿加权确定方法，其特征在于：步骤4所述的输入到改进的比例正交投影迭代变换POSIT算法中即得到更为精确的位姿估计解，具体实现方法如下，

待求的探测器的旋转矩阵R和平移矩阵t分别为

定义检测得到的陨石坑中心在导航相机图像平面像素坐标系下的齐次坐标为P_i＝(u_i,v_i,1)^T，非齐次坐标为像点对应的陨石坑中心在目标天体固连坐标系下的匹配得到的齐次坐标为非齐次坐标为其中，i＝1,2,…,n，n为陨石坑匹配数；图像点P_i和数据库匹配点F_i ^w的透视投影关系为

λ_iP_i＝K[R t]P_i ^w

式中，为摄像机内参矩阵，f为导航相机焦距，λ_i为数据库匹配点P_i ^w的深度值；

定义(x,y)是特征点在图像平面物理坐标系的像点坐标，(x′,y′)是特征点经过仿射变换后在图像平面物理坐标系的像点坐标，(X^C,Y^C,Z^C)是特征点在光学导航相机坐标系的坐标，特征点和像点的关系为

则透视投影关系表示为

上式两边同时除以t_z得到

式中，λ＝Z^C/t_z，s＝f/t_z

加入不确定性权阵，将等式左右两边分别左乘仿射变换矩阵得

展开得

由于在探测器任务期间，行星表面各特征点在导航相机坐标系中的z轴变化范围，所述z轴变化范围远远小于行星表面特征点到相机的平均深度；因此认为λ＝Z^C/t_z≈1；

λ＝Z^C/t_z≈1是比例正交投影迭代变换POSIT算法的迭代初值；在λ＝Z^C/t_z≈1初始状态下，物体的所有点在同一个深度上时，透视变换退化为一个比例正交投影；由于给定λ一个估计值，因此先将其看做已知量，删掉第三行方程中就少了4个未知量，更方便求解，即得到

展开得迭代方程为

求解8个未知量[sR₁₁ sR₁₂ sR₁₃ sR₂₁ sR₂₂ sR₂₃ st_x st_y]^T利用最小二乘法求解如下方程

由于λ被视为已知量，一个特征点及对应图像坐标得到2个独立方程，方程中总共有8个独立变量，拆成3个向量分别为

由此计算得到向量sR₁,sR₂，同时R₁,R₂均为单位向量；由于存在计算误差，向量sR₁,sR₂的模长不一定相等，取因此计算得到

t_z＝f/s

t_x＝(st_x)/s

t_y＝(st_y)/s

R₁＝(sR₁)/s

R₂＝(sR₂)/s

R₃＝R₁×R₂

由于迭代初始时，λ≈1，通过上式得到初始的旋转矩阵和平移矩阵，代入下式

通过迭代更新使每个特征点都有更加精确的迭代系数λ，将更新值代入迭代方程中，循环往复，逐步逼近位姿估计的精确解，直至旋转矩阵R和平移向量t稳定时停止迭代。