CN107631728A

CN107631728A - 一种星载图像辅助导航方法

Info

Publication number: CN107631728A
Application number: CN201710823612.4A
Authority: CN
Inventors: 徐国栋; 张兆祥; 刘明; 邢雷; 王梓霖; 张光宇; 朱晏辰
Original assignee: Harbin Institute of Technology
Current assignee: Harbin Institute of Technology
Priority date: 2017-09-13
Filing date: 2017-09-13
Publication date: 2018-01-26
Anticipated expiration: 2037-09-13
Also published as: CN107631728B

Abstract

一种星载图像辅助导航方法，属于航天器姿态确定技术领域。本发明是为了解决现有的图像处理辅助姿态确定方法中，由于数据量较大造成处理速度慢，硬件资源消耗较大等问题。本发明所述的方法，首先，对在轨图像进行压缩并计算图像梯度值；加载模板图像，根据压缩过程中获取的黑塞矩阵，进行多尺度的黑塞矩阵特征值求取以及多尺度金字塔运算，并根据最大值准则得到特征点；然后，根据在轨图像的特征点和模板图像的特征点，采用随机抽样一致性算法进行特征点匹配，从而得到两个图像之间的仿射变换矩阵；最后建立相机运动模型，将图像偏移量转换为姿态角变化量。本发明可应用于微小卫星对地观测系统。

Description

一种星载图像辅助导航方法

技术领域

本发明涉及航天器姿态确定技术领域，具体涉及星载图像辅助导航方法。

背景技术

随着遥感卫星分辨率越来越高，高分辨率光学载荷搭载在微小卫星上能有效降低卫星发射成本，并进一步实现卫星组网、组件更换，实现商业化运作等。微小遥感卫星相较于现有的传统卫星同时存在一些新的问题，比如卫星在轨对地指向精度较低，姿态稳定性较差等。这些问题会降低遥感图像的分辨率，甚至影响卫星的正常工作。针对这些问题目前的解决方案是：

1)利用高精度的姿态传感器，比如基于磁流体效应的角速度传感器以及基于流体旋转差动效应的角位移传感器。但是此类传感器成本较高且技术难度较大。

2)利用低成本的惯性传感器和星敏感器进行姿态融合和姿态确定。但是现有传感器短时精度无法达到要求，无法修正卫星拍摄过程中因为卫星颤振引起的图像抖动和模糊等问题。

3)利用高精度遥感图像包含丰富的地理信息，通过在轨图像处理算法可实现低成本高效率的姿态辅助确定。然而由于遥感图像数据量较大，星上计算能力有限，造成处理速度慢，硬件资源消耗较大，因此需要一种改进的匹配策略实现在轨姿态确定。

发明内容

本发明是为了解决现有的图像处理辅助姿态确定方法中，由于数据量较大造成处理速度慢，硬件资源消耗较大等问题，现提供一种星载图像辅助导航方法。

本发明所述一种星载图像辅助导航方法，通过以下技术方案实现：

步骤一、对在轨图像进行图像压缩，在压缩过程中计算图像梯度值；

步骤二、加载模板图像，根据压缩过程中获取的不同尺度下的图像梯度值的黑塞矩阵，进行多尺度的黑塞矩阵特征值求取以及多尺度金字塔运算，并通过最大值准则得到特征点；

步骤三、根据在轨图像的特征点和模板图像的特征点，采用随机抽样一致性算法进行特征点匹配，得到两个图像之间的仿射变换矩阵；然后建立相机运动模型，将图像偏移量转换为姿态角变化量。

本发明与现有技术相比较，最为突出的特点和显著的有益效果是：本发明所述的星载图像辅助导航方法，是一种通过获取多尺度下图像特征点进行图像配准获得姿态偏移信息的方法，充分利用了压缩图像提供的图像信息，相比于现有的SURF算法，该方法无需进行多尺度缩放操作，无需单独求解梯度矩阵，因此，本发明的方法简单，效率高，硬件资源消耗较低，节省计算资源，且易于实现，经过计算，在轨道变化可以忽略的情况下像素偏移0.3个单位的像素会导致姿态角偏移量为1.125μrad。

附图说明

图1为改进的JPEG2000压缩示意图；

图2为多尺度图像特征点获取示意图；

图3为本发明所述的利用在轨图像辅助导航方法的流程图。

具体实施方式

具体实施方式一：参照图3具体说明本实施方式，本实施方式给出的一种星载图像辅助导航方法，具体是按照以下步骤制备的：

步骤一、首先，对在轨图像进行图像压缩，在压缩过程中计算图像梯度值；

步骤二、加载事先保存好的模板图像，根据压缩过程中获取的不同尺度下的图像梯度值的黑塞矩阵，进行多尺度的黑塞矩阵特征值求取以及多尺度金字塔运算，并通过最大值准则(即特征点的获取是根据不同层之间一些点的最大值得到的)得到特征点；

步骤三、根据在轨图像的特征点和模板图像的特征点，采用随机抽样一致性算法进行特征点匹配，从而得到两个图像之间的仿射变换矩阵(一种二维坐标到二维坐标之间的线性变换)；然后建立相机运动模型，将图像偏移量转换为姿态角变化量，同时考虑轨道变化对随机抽样一致性算法的影响。

具体实施方式二：参照图1说明本实施方式，本实施方式与具体实施方式一不同的是：步骤一的具体过程如下：

在FPGA在轨图像处理平台中，在图像经过电子学处理送入平台进行压缩时，进行5/3提升式小波变换，同时在小波变换时实施图像梯度值的求取；为进一步降低图像噪声对梯度值的影响，提高梯度值的稳定性，利用中心点以及与其上下左右邻域的5个值进行图像梯度值的求取，一维变换公式如下：

其中y(n)代表压缩后的下一级图像像素值，x(n)指当前级图像像素值，D_x代表图像横向的梯度值，D_y的表达式与D_x相同，只是在计算方向上不同，D_y代表图像纵向的梯度值；D_xy代表了图像在45度方向上的梯度值，y₁是中间变量，n指图像像素的索引值；而公式(1)与公式(3)因为具有相似的结构，因此公式(3)在FPGA平台的计算中可借用公式(1)的计算资源；从而有效提高了算法的运行效率；因梯度图像同原图像一致，因此在第一级图像压缩之后，获得的梯度值与图像高频分量一同输入到公共存储区中进行保存。为提高后续特征点匹配的效率，梯度信息将不参与图像压缩的量化以及编码运算。

具体实施方式三：参照图2说明本实施方式，本实施方式与具体实施方式二不同的是：

步骤二中对压缩图像进行多尺度特征点提取的具体步骤为：

针对获取的图像梯度值，根据特征值原理，通过对梯度特征值的求取进行特征点判决；计算在轨图像上的像素点I在压缩图像第m层的黑塞矩阵为：

其中，D_x(I,m)是在轨图像上的像素点I在压缩图像第m层横向的梯度值，D_y(I,m)是在轨图像上的像素点I在压缩图像第m层纵向的梯度值D_xy(I,m)是在轨图像上的像素点I在压缩图像第m层在45度方向上的梯度值；

利用黑塞矩阵H的行列式进行特征值的判别：

det(H)＝D_xD_y-wD_xy ² (7)

其中w为近似系数，用于补偿判决式误差；det是一个计算机函数，该函数用于求矩阵的行列式；根据行列式性质，公式(7)取最大值时可以认为对应的像素点是像素点阵附近的特征点，为进一步提高特征点获取稳定性，采用如下判决式进行特征点筛选：

S＝max(det(H,m),Threshold)+max(det(H,m-1),det(H,m),det(H,m+1)) (8)

即，首先利用预设极值进行筛选，小于预设极值的点都被抛弃，之后再利用周围的8个点以及相邻上下两层的周围9个点进行判决，满足最大值的点才会被认为强特征点；Threshold指的是预设极值，S指的是阈值；图2填充区域表现了筛选过程，其中深色填充像素代表目标像素，而浅色填充像素代表周围要比较的像素点。

另外，由于在轨遥感图像会存在进光量不足，导致特征点较小的问题，因此这里的阈值应为自适应阈值，而将特征点数目作为调整目标值，当提取的特征点少于预设的特征点数目S*时，通过调整阈值以增加特征点，直到总的特征点数目符合要求；公式表述如下：

Threshold＝argmin(S>＝S^*,Threshold) (9)。

其它步骤及参数与具体实施方式二相同。

具体实施方式四：本实施方式与具体实施方式三不同的是：步骤三中采用随机抽样一致性算法进行特征点匹配的具体步骤为：

根据在轨图像和模板图像之间存在的仿射变换，即针对模板图像中的任一像素点I′＝(a′,b′)和对于在轨图像中的像素点I＝(a,b)，存在如下对应关系：

其中(a′,b′)为像素点I′的平面坐标，(a,b)为像素点I的平面坐标，A代表旋转矩阵，B代表平移矩阵，可用如下参数表示：

式中θ表示旋转角度，b₁、b₂表示平移的像素大小；在(10)式中，最少需要三组(a,b)像素点才能解出仿射变换矩阵；

然后，利用随机抽样一致性算法解出仿射矩阵。

具体实施方式五：本实施方式与具体实施方式四不同的是：步骤三中随机抽样一致性算法解出仿射变换矩阵的具体步骤为：

1)计算特征向量描述符的欧式距离，之后利用最小值比次小值原理进行特征点匹配；

2)在获得的在轨图像特征点集中随机选取3个点，同时在模板图像特征点集中找到对应的匹配特征点，利用这三组点得到一个仿射变换矩阵；

3)将除上述随机选取3个点之外的点依次代入步骤2)求得的仿射变换矩阵并计算仿射误差，仿射误差采用以下公式计算：

E＝I-[A B]I′ (12)

对得到的误差进行判断，若误差小于特定阈值，则可认为匹配点是内部点，否则认为是外部点，并计算全部的内部点数目，若内部点总数小于预设内部点数值则重复步骤2)～步骤3)，当内部点总数达到预设内部点数值，或者循环次数达到预设循环次数时，退出循环并选取内部点数目最大的点对应的仿射矩阵作为结果。

具体实施方式六：本实施方式与具体实施方式五不同的是：步骤三中利用相机运动模型将图像偏移量转换为姿态角变化量的具体步骤为：

考虑到不同分辨率下，不同型号的相机在拍摄过程中会出现姿态和轨道信息的变化，比如推扫式相机在推扫过程中轨道信息有微小变化；相机成像面上的像素点P_i,j在t_k时刻扫描到了地面目标S_i,j,k的姿态变化，轨道变化与像素偏移量之间的关系推导如下：

设U是像素坐标系相对于体坐标系的基向量，u_i,j表示P_i,j相对于体坐标系的视线方向；u_i,j能够表示为

式中，C_cG,k表示t_k时刻相机坐标系到地球惯性坐标系的转换矩阵，r_i,j,k表示地面点在t_k时刻相对于地球惯性系的坐标向量，r_s,k表示在地球固联坐标系下的卫星位置向量，能够得到下式：

ρ_i,j,k＝||r_i,j,k-r_s,k|| (14)

参数ρ_i,j,k是地面目标点到卫星之间的距离大小；

在t_k+1时刻，相机坐标系到地球惯性坐标系的转换矩阵和地球固联系下的卫星位置向量将会变为：

C_cG,k+1＝exp(-(δθ_k)_×)C_cG,k≈C_cG,k-(δθ_k)_×C_cG,k (15)

r_s,k+1＝r_s,k+δr_k (16)

δr_k是从t_k到tk₊1时刻卫星的轨道位置变化(平移向量)；δθ_k是从t_k到t_k+1时刻卫星的姿态角变化量(旋转向量)；根据公式(13)，P_i,j相对于体坐标系的视线方向从t_k到t_k+1的变化量可以写成：

将公式(15)代入到公式(17)，可以得到：

θ_k是星本体坐标系下卫星的姿态角向量，×表示叉乘；

考虑到地面目标点到卫星之间距离的瞬时变化量δρ_i,j,k可以看作是δr_k在ρ_i,j,k上的投影，因此可以推导出下式：

u是目标点在相机坐标系下的位置矢量，u^T是u的转置，I₃是三阶单位矩阵，ρ_i,j,k+1是t_k+1时刻地面目标点到卫星之间的距离大小；同时，t_k时刻P_i,j相对于体坐标系的视线方向u_i,j,k和图像坐标系中目标点的位置向量p_img可表示如下：

f是对地相机的焦距；

根据几何原理，在体坐标系下的像素位置向量p_body,k可以写成：

p_unit是图像坐标系下目标点位置向量的方向矢量，e是体坐标系下焦距的方向矢量，是e与u_i,j之间的夹角；

因此，通过差分可以得到体坐标系下像素位置矢量的变化量为：

将公式(19)代入到公式(22)，可以得到：

Fδθ_k＝δp_body,k+Dδr_k (23)

其中，D和F为中间变量，具体为：

因为公式(23)中的矩阵D和矩阵F的秩都小于3_，需要使用最小二乘法求解δθ_k，并且最少需要3组像素特征点向量作为输入才能得到解析解；考虑到旋转矩阵A在实际测试中小于平移矩阵B(小于一个数量级)，同时考虑到相机坐标系和图像坐标系重合，因此有：

从而我们可以根据像素位置的变化得到姿态角变化量δθ_k。

其它步骤及参数与具体实施方式五相同。

实施例

采用以下实施例验证本发明的有益效果：

本实施例中，细致考虑了姿态和轨道变化对图像像素偏移的影响。最终结果在实际应用中会根据具体的成像方式进行简化。

在面阵传感器成像中，以及小画幅的推扫式成像中，航天器轨道运动可以视为不变，此时公式中卫星位置向量的变化量δr_k可以看做为0，公式可以进行简化。如果推扫式相机扫描时间较长，或者本身模板图像由于分辨率较低导致图像实际覆盖地区较快，卫星移动无法时间忽略，此时δr_k可通过GPS等星上敏感器实际解算出来。代入公式从而可求出姿态角和像素偏移量之间的关系。

经过计算，在轨道变化可以忽略的情况下像素偏移0.3个单位的像素会导致姿态角偏移量为1.125μrad。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims

1.一种星载图像辅助导航方法，其特征在于：所述星载图像辅助导航方法包括以下步骤：

2.根据权利要求1所述的一种星载图像辅助导航方法，其特征在于：步骤一的具体过程如下：

在FPGA在轨图像处理平台中，在图像经过电子学处理送入平台进行压缩时，进行5/3提升式小波变换，同时在小波变换时实施图像梯度值的求取；利用中心点邻域的值进行图像梯度值的求取：

其中y(n)为压缩后的下一级图像像素值，x(n)指当前级图像像素值，D_x是图像横向的梯度值，D_y是图像纵向的梯度值；D_xy是图像在45度方向上的梯度值，y₁是中间变量，n指图像像素的索引值；在第一级图像压缩之后，获得的梯度值与图像高频分量一同输入到公共存储区中进行保存。

3.根据权利要求2所述的一种星载图像辅助导航方法，其特征在于：步骤二中对压缩图像进行多尺度特征点提取的具体步骤为：

利用黑塞矩阵H的行列式进行特征值的判别：

det(H)＝D_xD_y-wD_xy ² (7)

其中w为近似系数，公式(7)取最大值时对应的像素点是像素点阵附近的特征点，采用如下判决式进行特征点筛选：

S＝max(det(H,m),Threshold)+max(det(H,m-1),det(H,m),det(H,m+1)) (8)

Threshold指的是预设极值，S指的是阈值；阈值为自适应的，将特征点数目作为调整目标值，当提取的特征点少于预设的特征点数目S^*时，通过调整阈值S以增加特征点，直到总的特征点数目符合要求；公式表述如下：

Threshold＝arg min(S>＝S^*,Threshold) (9)。

4.根据权利要求3所述的一种星载图像辅助导航方法，其特征在于：步骤三中采用随机抽样一致性算法进行特征点匹配的具体步骤为：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>a</mi> </mtd> </mtr> <mtr> <mtd> <mi>b</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>A</mi> </mtd> <mtd> <mi>B</mi> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msup> <mi>a</mi> <mo>&prime;</mo> </msup> </mtd> </mtr> <mtr> <mtd> <msup> <mi>b</mi> <mo>&prime;</mo> </msup> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

其中(a′,b′)为I′的平面坐标，(a,b)为I的平面坐标，A代表旋转矩阵，B代表平移矩阵，用如下参数表示：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>A</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&theta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>B</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>b</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>b</mi> <mn>2</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

式中θ表示旋转角度，b₁、b₂表示平移的像素大小；

然后用随机抽样一致性算法解出仿射变换矩阵。

5.根据权利要求4所述的一种星载图像辅助导航方法，其特征在于：步骤三中用随机抽样一致性算法解出仿射变换矩阵的具体步骤为：

E＝I-[A B]I′ (12)

对得到的误差进行判断，若误差小于特定阈值，则匹配点是内部点，否则是外部点，并计算全部的内部点数目，若内部点总数小于预设内部点数值则重复步骤2)～步骤3)，当内部点总数达到预设内部点数值，或者循环次数达到预设循环次数时，退出循环并选取内部点数目最大的点对应的仿射矩阵作为结果。

6.根据权利要求5所述的一种星载图像辅助导航方法，其特征在于：步骤三中利用相机运动模型将图像偏移量转换为姿态角变化量的具体步骤为：

相机成像面上的像素点P_i,j在t_k时刻扫描到了地面目标S_i,j,k的姿态变化；

设U是像素坐标系相对于体坐标系的基向量，u_i,j表示P_i,j相对于体坐标系的视线方向；u_i,_j能够表示为

ρ_i,j,k＝||r_i,j,k-r_s,k|| (14)

参数ρ_i,j,k是地面目标点到卫星之间的距离大小；

在t_k+1时刻，相机坐标系到地球惯性坐标系的转换矩阵和地球固联系下的卫星位置向量变为：

C_cG,k+1＝exp(-(δθ_k)_×)C_cG,k≈C_cG,k-(δθ_k)_×C_cG,k (15)

r_s,k+1＝r_s,k+δr_k (16)

δr_k是从t_k到t_k+1时刻卫星的轨道位置变化；δθ_k是从t_k到t_k+1时刻卫星的姿态角变化量；根据公式(13)，P_i,j相对于体坐标系的视线方向从t_k到t_k+1的变化量为：

<mrow> <msub> <mi>&delta;u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>&times;</mo> </mrow> </msub> <msub> <mi>&theta;</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>c</mi> <mo>,</mo> <mi>G</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mfrac> <mrow> <msub> <mi>&delta;r</mi> <mi>k</mi> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>r</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <msub> <mi>&delta;&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> <mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&delta;&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> <mo>)</mo> </mrow> <msub> <mi>&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>

θ_k是星本体坐标系下卫星的姿态角向量，×表示叉乘；

将地面目标点到卫星之间距离的瞬时变化量δρ_i,j,k看作是δr_k在ρ_i,j,k上的投影，能够得到：

<mrow> <msub> <mi>&delta;u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>&times;</mo> </mrow> </msub> <msub> <mi>&theta;</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>C</mi> <mrow> <mi>c</mi> <mo>,</mo> <mi>G</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mfrac> <mrow> <mo>(</mo> <msub> <mi>I</mi> <mn>3</mn> </msub> <mo>-</mo> <msup> <mi>uu</mi> <mi>T</mi> </msup> <mo>)</mo> <msub> <mi>&delta;r</mi> <mi>k</mi> </msub> </mrow> <msub> <mi>&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>

u是目标点在相机坐标系下的位置矢量，u^T是u的转置，I₃是三阶单位矩阵，ρ_i,j,k+1是t_k+1时刻地面目标点到卫星之间的距离大小；同时，t_k时刻P_i,j相对于体坐标系的视线方向u_i,j,k和图像坐标系中目标点的位置向量p_img能够表示如下：

f是对地相机的焦距；

根据几何原理，在体坐标系下的像素位置向量p_body,k能够表示为：

e是体坐标系下焦距的方向矢量；

通过差分可以得到体坐标系下像素位置矢量的变化量为：

<mrow> <msub> <mi>&delta;p</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>p</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>p</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mfrac> <mrow> <mi>f</mi> <mi>U</mi> <mo>-</mo> <msubsup> <mi>ep</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>T</mi> </msubsup> </mrow> <mrow> <msup> <mi>e</mi> <mi>T</mi> </msup> <msub> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msub> <mi>&delta;u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

将公式(19)代入到公式(22)，得到：

Fδθ_k＝δp_body,k+Dδr_k (23)

其中，

<mrow> <mi>D</mi> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mfrac> <mrow> <mi>f</mi> <mi>U</mi> <mo>-</mo> <msubsup> <mi>ep</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> </mrow> <mi>T</mi> </msubsup> </mrow> <mrow> <msup> <mi>e</mi> <mi>T</mi> </msup> <msub> <mi>u</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msub> <mi>C</mi> <mrow> <mi>c</mi> <mo>,</mo> <mi>G</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mfrac> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msup> <mi>uu</mi> <mi>T</mi> </msup> <mo>)</mo> <msub> <mi>&delta;r</mi> <mi>k</mi> </msub> </mrow> <msub> <mi>&rho;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>

使用最小二乘法求解δθ_k；因旋转矩阵A小于平移矩阵B一个数量级，且相机坐标系和图像坐标系重合，则有：

<mrow> <msub> <mi>&delta;p</mi> <mrow> <mi>b</mi> <mi>o</mi> <mi>d</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>b</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>b</mi> <mn>2</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>

最终得到姿态角变化量δθ_k。