一种基于四维实数矩阵的三维图像匹配导航方法

摘要

本发明提供一种基于四维实数矩阵的三维图像匹配导航方法，该方法通过飞行器实时采集三维实测图，并与预先存储的三维参考图基于四维实数矩阵进行图像匹配；根据图像匹配结果计算飞行器的空间螺旋位移四维矩阵的元素值，并利用飞行器的空间螺旋位移四维矩阵与飞行器的连续空间螺旋位移四维实数变换矩阵的元素一一对应关系，计算飞行器的位置与姿态变化，确定导航参数。运用该方法导航，其计算的精度高，能够达到亚像素级精度，并且算法简单，实时性好。

说明书

技术领域

本发明涉及一种精确图像匹配导航技术，尤其是一种基于四维实数矩阵的 精确三维图像匹配导航方法。

背景技术

图像匹配最早是美国在上世纪70年代从事飞行器辅助导航系统、武器投射 系统的末制导等应用研究中提出的。经过几十年的发展，图像匹配技术已经成为 现代信息处理领域中一项极为重要的技术，在自动导航、计算机视觉、图像三维 重构、遥感图像处理等领域有着广泛而实际的应用。

传统的基于图像匹配的辅助导航方法是通过传感器实时获取平面二维图像 与飞行器存储的图像数据库进行匹配，在图像匹配成功基础上，得出飞行器的位 置信息，推算出飞行器相对航向角、高度变化等信息。

由于用于图像匹配的实测图和数字地图直接有着不同程度的旋转和尺度变 化，而且这两种图像之间的匹配为非相似性匹配，因此匹配算法的精确性成为提 高导航效能的关键；同时，飞行器是一种刚体，在空间具有6个自由度，分别用 三个自由度表示平移和旋转，当飞行器同时存在6个自由度的变化时，一般的图 像匹配方法是针对二维图像的，难以同时克服这些自由度造成的影响。而且大多 数图像匹配算法得出的精度是象素级的，这对于粗匹配来说是可以的，但对于要 求日益增高的精确导航系统来说，图像匹配的精度太低，对惯导的辅助作用也很 小。另一方面，也难以仅仅通过二维图像来精确确定图像之间的平移、旋转等参 数变化。因此，传统的基于图像匹配辅助导航方法将无法精确确定飞行器的位置 与姿态变化。

发明内容

本发明所要解决的技术问题是：提供一种基于四维实数矩阵的图像匹配导航 方法，采用四维实数矩阵方法建立立体三维图像之间的变换关系，对实测三维图 像和参考三维图像之间的一一对应点对利用最小二乘算法计算得到飞行器的三 个姿态角和平移量，从而精确确定飞行器的位置与姿态变化。

本发明为解决上述技术问题，采用如下技术方案：

一种基于四维实数矩阵的三维图像匹配导航方法，该方法首先通过飞行器实 时采集三维实测图，并与预先存储的三维参考图进行图像匹配，获取实测图与参 考图之间的所有一一对应点对；在每次采集图像时，将飞行器的理想位置定义为 初始位置，将飞行器的当前位置定义为目标位置；还包括如下过程：

飞行器的初始位置用r₁表示，目标位置用r₁′表示，飞行器由初始位置到目标 位置的向量方程表示为：

r₁′=Rr₁+t  (1)

其中R表示正交实数旋转矩阵，t表示初始位置到目标位置的位移， r₁=(x,y,z)^T，r₁′=(x′,y′,z′)^T，令$R=\left(\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right),$t=(t₁,t₂,t₃)^T

飞行器的初始位置r₁在四维空间内表示为r₂，目标位置r₁′在四维空间内表示 为r₂′，则飞行器由初始位置到目标位置的向量方程在四维空间内表示为：

r₂′=Pr₂  （2）

其中P表示空间螺旋位移矩阵，r₂=(x,y,z,w)^T，r₂′=(x′,y′,z′,w′)^T，令

$P=\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\\ a_{21}& a_{22}& a_{23}& t_2\\ a_{31}& a_{32}& a_{33}& t_3\\ 0& 0& 0& 1\end{array}\right);$

飞行器从初始位置到目标位置的连续空间螺旋位移四维实数变换矩阵表示 为：

$P(\psi ,S_{\psi },\theta ,S_{\theta },\gamma ,S_{\gamma })=\left(\begin{array}{cccc}\cos \gamma & 0& -\sin \gamma & 0\\ 0& 1& 0& S_{\gamma }\\ \sin \gamma & 0& \cos \gamma & 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}1& 0& 0& S_{\theta }\\ 0& \cos \theta & \sin \theta & 0\\ 0& -\sin \theta & \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}\cos \psi & -\sin \psi & 0& 0\\ \sin \psi & \cos \psi & 0& 0\\ 0& 0& 1& S_{\psi }\\ 0& 0& 0& 1\end{array}\right)=$

其中ψ为航向角、S_ψ为沿Z_b轴的位移、θ为俯仰角、S_θ为沿X_b轴的位移、 γ为横滚角、S_γ为沿Y_b轴的位移，飞行器的坐标系为OX_bY_bZ_b，原点O与飞行 器质心重合，X_b轴沿飞行器横轴向右，Y_b轴沿飞行器纵轴向前，Z_b轴沿飞行器 竖轴向上；

令P中元素与P(ψ,S_ψ,θ,S_θ,γ,S_γ)中元素对应相等构建方程组，获取飞行器从 初始位置到目标位置的姿态角和位移量的计算公式：

根据图像匹配获取的所有一一对应点对，利用最小二乘法计算P中元素的 值，将求得的P中元素的值带入公式（3），得到ψ_主,S_ψ,θ_主,S_θ,γ_主,S_γ的值，其中， ψ_主、θ_主、γ_主分别表示航向角ψ、俯仰角θ、横滚角γ的反三角函数的主值；根 据航向角ψ、俯仰角θ、横滚角γ的定义域和a₁₃,a₂₁,a₂₂,a₂₃,a₃₃的符号计算航向角 ψ，俯仰角θ，横滚角γ的精确值，将ψ,S_ψ,θ,S_θ,γ,S_γ作为导航数据。

所述图像匹配的过程如下：

将参考图与实测图的像素点集，分别定义为A={a₁,a₂,…,a_m}和 B={b₁,b₂,…,b_n}，任意选取两组点对和其中a_i,a_j与b_p,b_q分别 为参考图像素点集A与实测图像素点集B中的点，并且a_i≠a_j,b_p≠b_q，i∈[1， m]、j∈[1，m]、p∈[1，n]、q∈[1，n]，其中m、n为自然数；矢量过 坐标原点，并构成平面，则矢量到矢量的角度为：

$\alpha ={\cos }^{-1}\frac{\overrightarrow{a_i{a}_j}·\overrightarrow{b_p{b}_q}}{\left|\right|\overrightarrow{a_i{a}_j\left|\right|}·\left|\right|\overrightarrow{b_p{b}_q\left|\right|}};$

矢量所构成的平面过坐标原点的法线的法向量为：

$\overrightarrow{n}=\overrightarrow{a_i}{a}_j\times \overrightarrow{b_p{b}_q},$方向向量为：$\overrightarrow{\mathrm{ne}}=\frac{\overrightarrow{n}}{\overrightarrow{\left|\right|n\left|\right|}}=(l,m,n,0)$

其中l,m,n分别为与X,Y,Z轴的方向余弦，那么矢量以方向向量为 过坐标原点的法线为旋转轴，绕旋转轴旋转α角度后，与矢量方向一 致，则该旋转变换用四维矩阵表示为：

$P(\alpha ,0)=$

$\left(\begin{array}{cccc}{l}^2(1-\cos \alpha )+\cos \alpha & \mathrm{ml}(1-\cos \alpha )+n\sin \alpha & \mathrm{nl}(1-\cos \alpha )-m\sin \alpha & 0\\ \mathrm{ml}(1-\cos \alpha )-n\sin \alpha & m^2(1-\cos \alpha )+\cos \alpha & \mathrm{nm}(1-\cos \alpha )+l\sin \alpha & 0\\ \mathrm{nl}(1-\cos \alpha )+m\sin \alpha & \mathrm{nm}(1-\cos \alpha )-l\sin \alpha & n^2(1-\cos \alpha )+\cos \alpha & 0\\ 0& 0& 0& 1\end{array}\right)$

判断该旋转变换四维矩阵P(α,0)是否为最优相似变换矩阵，及其中一组对应 点对是否为一一对应点对；如果P(α,0)是最优相似矩阵，则定义该最优相似矩 阵为P_op(α,0)，同时该一组对应点对为一一对应点对，记录该一组一一对应点对； 否则继续判断；

根据在上述过程中得到的一组一一对应点对和P_op(α,0)计算参考图与实测图 之间的所有一一对应点对。

与现有技术相比，本发明具有如下有益效果：

1.本发明采用四维实数矩阵方法描述了飞行器空间位置与姿态的变换关 系，通过三维实测图像和三维参考图像的精确匹配算法，计算得到飞行器的位置 和姿态变化，因此，本发明构建的数学模型简单。

2.本发明采用最小二乘算法进行三维参考图像与三维实测图像的精确匹配 计算，计算的精度能够达到亚像素级，因此本发明采用的方法定位精度高。

3.本发明采用的算法简单，算法的实时性好。

附图说明

图1是飞行器的空间位置和姿态示意图。

图2是飞行器的空间螺旋位移示意图。

图3是飞行器的坐标系定义示意图。

图4是飞行器连续三次空间螺旋位移图。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

如图1所示，飞行器作为刚体，其在三维空间中具有6个自由度，在图1中 用(x₀,y₀,z₀,β,γ,δ)来表示刚体的位置和姿态。

如图2所示，飞行器在三维空间的运动可以表示为包括绕着某一螺旋轴的 旋转运动（旋转角度为θ）和沿着螺旋轴方向上的平移运动（位移量为S）。由 于空间直线具有4个自由度，即3个自由度描述方向和1个自由度描述直线位置， 需要4个参数来描述一条空间直线，再加上螺旋位移运动的旋转角度θ和位移量 S，共6个参数来描述飞行器的空间运动是合理的。

如图4所示，由于刚体的空间位移可以表示为旋转和平移的组合，那么假设 刚体的初始位置r₁=(x,y.z)^T和目标位置r₁′=(x′,y′,z′)^T，刚体由初始位置运动到 目标位置的向量方程表示为：

r₁′=Rr₁+t  (1)

其中R表示旋转的实数正交3×3矩阵，$R=\left(\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right),$t=(t₁,t₂,t₃)^T是 表示初始位置r₁到目标位置r₁′的位移。用坐标系的方法，可以将方程(1)写作：

$\left(\begin{array}{c}{x}^{\prime }=a_{11}x+a_{12}y+a_{13}z+t_1\\ y^{\prime }=a_{21}x+a_{22}y+a_{23}z+t_2\\ z^{\prime }=a_{31}x+a_{32}y+a_{33}z+t_3\end{array}\right)---\left(2\right)$

针对方程(2)，本发明考虑将其扩展成4×4矩阵。在四维空间中采用四维齐次 坐标，并且采用4个坐标值之间的3个独立的比值。也就是说，用四维齐次坐标 点(X,Y,Z,W)表示三维空间的点(x,y,z)，并且有

$x=\frac{X}{W},$$y=\frac{Y}{W},$$z=\frac{Z}{W}---\left(3\right)$

式(3)意味着：

X=λx,Y=λy,Z=λz,W=λ,(λ≠0)  (4)

在实际应用中，通常选择λ=1，用四维空间内线上的点(x,y,z,1)来表示三维 空间的点(x,y,z)。在不改变原点的情况下，刚体在三维空间中的螺旋位移表达 式(1)可以用四维空间表示为：

r₂′=Pr₂  (5)

其中r₂=(x,y,z,w)^T和r₂′=(x′,y′,z′,w′)^T分别表示四维空间的初始位置和目标 位置，P表示从初始位置r₂到目标位置r₂′的螺旋位移变换矩阵，那么可以将方程 (5)用四维空间的点(x,y,z,w)改写为：

$\left(\begin{array}{c}{x}^{\prime }=a_{11}x+a_{12}y+a_{13}z+a_{14}w\\ y^{\prime }=a_{21}x+a_{22}y+a_{23}z+a_{24}w\\ z^{\prime }=a_{31}x+a_{32}y+a_{33}z+a_{34}w\\ w^{\prime }=a_{41}x+a_{42}y+a_{43}z+a_{44}w\end{array}\right)---\left(6\right)$

应用式(3)的关系，将线变换式(6)线性变换为三维空间点(x,y,z)，形式如下：

$\left(\begin{array}{c}{x}^{\prime }=\frac{a_{11}x+a_{12}y+a_{13}z+a_{14}w}{a_{41}x+a_{42}y+a_{43}z+a_{44}w}\\ y^{\prime }=\frac{a_{21}x+a_{22}y+a_{23}z+a_{24}w}{a_{41}x+a_{42}y+a_{43}z+a_{44}w}\\ z^{\prime }=\frac{a_{31}x+a_{32}y+a_{33}z+a_{34}w}{a_{41}x+a_{42}y+a_{43}z+a_{44}w}\end{array}\right)---\left(7\right)$

由于w=λ,λ≠0，并且选取λ=1，那么式(7)可以化简为：

$\left(\begin{array}{c}{x}^{\prime }=a_{11}x+a_{12}y+a_{13}z+a_{14}\\ y^{\prime }=a_{21}x+a_{22}y+a_{23}z+a_{24}\\ z^{\prime }=a_{31}x+a_{32}y+a_{33}z+a_{34}\end{array}\right)---\left(8\right)$

并且得到a₄₁=a₄₂=a₄₃=0,a₄₄=1，因此方程(8)用四维点(x,y,z,1)表示为：

$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right)=\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& a_{14}\\ a_{21}& a_{22}& a_{23}& a_{24}\\ a_{31}& a_{32}& a_{33}& a_{34}\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)---\left(9\right)$

通过比较式(2)与式(8)，线变换方程与三维空间螺旋位移方程在形式上具有相 似性。当式(8)中的系数3×3子矩阵是单位正交矩阵时，该系数3×3子矩阵表示 刚体的旋转矩阵，那么，式(9)就表示刚体空间螺旋位移，并且此时有：

a₁₄=t₁,a₂₄=t₂,a₃₄=t₃  (10)

因此，飞行器的空间螺旋位移可以用4×4实数矩阵形式完整表示为：

$P=\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\\ a_{21}& a_{22}& a_{23}& t_2\\ a_{31}& a_{32}& a_{33}& t_3\\ 0& 0& 0& 1\end{array}\right)---\left(11\right)$

如图3所示，定义飞行器坐标系OX_bY_bZ_b，其原点与飞行器质心重合，X_b沿飞行器横轴向右，Y_b沿飞行器纵轴向前，Z_b沿飞行器竖轴向上。

如图4所示，定义飞行器的姿态角，即航向角、俯仰角和横滚角。

1.定义飞行器绕竖轴Z方向的转动，飞行器的纵轴Y₁在水平面上的投影与Y 轴正向之间的夹角ψ为航向角，数值以Y轴正向为起点，顺时针方向为正，定义 域为

2.定义飞行器绕横轴X₁转动产生的纵轴Y₂与纵轴Y₁之间的夹角θ为俯仰 角，俯仰角以Y₁为起点，向上为正，向下为负，定义域为[-90°,90°]。

3.定义载体绕纵轴Y₂转动Z_b相对于Z₂的转角γ为横滚角，从竖轴Z₂算起， 顺时针为正，逆时针为负，定义域为

根据飞行器的姿态角定义以及实数4×4矩阵的原理，载体绕竖轴Z_b的航向角 ψ和沿Z_b的位移S_ψ的空间螺旋位移矩阵可以表示为P_Z(ψ,S_ψ)：

$P_Z(\psi ,S_{\psi })=\left(\begin{array}{cccc}\cos \psi & -\sin \psi & 0& 0\\ \sin \psi & \cos \psi & 0& 0\\ 0& 0& 1& S_{\psi }\\ 0& 0& 0& 1\end{array}\right)---\left(12\right)$

载体绕纵轴Y_b的横滚角γ和沿Y_b的位移S_γ的空间螺旋位移矩阵为P_Y(γ,S_γ)：

$P_Y(\gamma ,S_{\gamma })=\left(\begin{array}{cccc}\cos \gamma & 0& -\sin \gamma & 0\\ 0& 1& 0& S_{\gamma }\\ \sin \gamma & 0& \cos \gamma & 0\\ 0& 0& 0& 1\end{array}\right)---\left(13\right)$

载体绕横轴X_b的俯仰角θ和沿X_b的位移S_θ的空间螺旋位移矩阵为 P_X(θ,S_θ)：

$P_X(\theta ,S_{\theta })=\left(\begin{array}{cccc}1& 0& 0& S_{\theta }\\ 0& \cos \theta & \sin \theta & 0\\ 0& -\sin \theta & \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right)---\left(14\right)$

再次参阅图4所示，飞行器从初始位置到目标位置的连续空间螺旋位移可以 表示为：

$P(\psi ,S_{\psi },\theta ,S_{\theta },\gamma ,S_{\gamma })=\left(\begin{array}{cccc}\cos \gamma & 0& -\sin \gamma & 0\\ 0& 1& 0& S_{\gamma }\\ \sin \gamma & 0& \cos \gamma & 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}1& 0& 0& S_{\theta }\\ 0& \cos \theta & \sin \theta & 0\\ 0& -\sin \theta & \cos \theta & 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}\cos \psi & -\sin \psi & 0& 0\\ \sin \psi & \cos \psi & 0& 0\\ 0& 0& 1& S_{\psi }\\ 0& 0& 0& 1\end{array}\right)=---\left(15\right)$

公式(15)与公式(11)形式相同，因此，只要计算得到公式(11)的实数矩阵，就 可以通过矩阵元素之间的对应关系反解出飞行器的姿态角及位移量。

为了得到公式(11)的四维实数矩阵，本发明采用由飞行器的成像传感器实测 立体图像与机载计算机存储的标准参考立体图像进行精确图像匹配，将实测图像 与参考图像之间的变换关系转换为飞行器实际飞行状态与预定飞行状态的变换 关系。

所述的精确图像匹配算法主要包括如下步骤：

(a)确定实测图与参考图之间的待匹配点集；

(b)确定具有最多匹配点对数目的四维实数矩阵和实测图像与参考图像之间 的一组一一对应点对；

(c)在获得了实测图像与参考图像之间的一组一一对应点对和具有最多匹配 点对数目的四维实数矩阵基础之上，确定实测图与参考图中所有的一一对应点 对；

(d)在获得了所有的一一对应点对基础上，采用最小二乘算法计算最优四维 实数变换矩阵。

为了方便描述精确图像匹配算法，首先定义最优四维实数变换矩阵：将立体 图像的像素扩充成四维的点（为三维空间上对应X轴、 Y轴和Z轴的坐标，本发明约定，若文中没有特别说明，像素均为四维点），如 果两幅图像特征点集合之间的相似变换关系为组合旋转变换R和平移变换t，那 么定义两幅立体图像之间的相似变换矩阵为P(R,t)，其中$P(R,t)=\left(\begin{array}{cc}{R}_{3\times 3}& t_{3\times 1}\\ 0_{1\times 3}& 1_{1\times 1}\end{array}\right),$假设参考图和实测图的点集分别为A={a₁,a₂,…,a_m}和B={b₁,b₂,…,b_n}，那么通 过相似变换，对于特征点集合中的一组对应点则有如下关系：

b_p=P(R,t)a_i  (16)

表示成矩阵形式为：

$\left(\begin{array}{c}{x}_{b_p}\\ y_{b_p}\\ z_{b_p}\\ 1\end{array}\right)=\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\\ a_{21}& a_{22}& a_{23}& t_2\\ a_{31}& a_{32}& a_{33}& t_3\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_{a_i}\\ y_{a_i}\\ z_{a_i}\\ 1\end{array}\right)---\left(17\right)$

由于获取的实测图像存在噪声，并与参考图像之间像素存在偏差，因此满足 上述方程的精确变换矩阵并不存在，如果在某个给定相似变换矩阵P和允许的位 置偏差条件||Pa-b||<d下，其中d为条件阈值，点集经过该相似变换矩阵变换 后，若点集中的点满足允许的位置偏差条件，并取到两个点集中一一对应点对数 目最大值时，此时得到的两幅图像之间的最优相似变换矩阵，该最优相似变换矩 阵能最准确反映两幅图像之间的变换关系。

其次定义矢量其中a_i,a_j与b_p,b_q分别为参考图点集和实测图点 集中的点，并且a_i≠a_j,b_p≠b_q，并且和为两组一一对应点对。那 么分别使用变换公式(17)并相减，得到：

$\left(\begin{array}{c}{x}_{b_p}-x_{b_q}\\ y_{b_p}-y_{b_q}\\ z_{b_p}-z_{b_q}\\ 0\end{array}\right)=\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& 0\\ a_{21}& a_{22}& a_{23}& 0\\ a_{31}& a_{32}& a_{33}& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_{a_i}-x_{a_j}\\ y_{a_i}-y_{a_j}\\ z_{a_i}-z_{a_j}\\ 0\end{array}\right)---\left(18\right)$

取$\overrightarrow{a_i{a}_j}=\left(\begin{array}{c}{x}_{a_i}-x_{a_j}\\ y_{a_i}-y_{a_j}\\ z_{a_i}-z_{a_j}\\ 0\end{array}\right),$$\overrightarrow{b_p{b}_q}=\left(\begin{array}{c}{x}_{b_p}-x_{b_q}\\ y_{b_p}-y_{b_q}\\ z_{b_p}-z_{b_q}\\ 0\end{array}\right),$则矢量到矢量的角度为：

$\alpha ={\cos }^{-1}\frac{\overrightarrow{a_i{a}_j}·\overrightarrow{b_p{b}_q}}{\left|\right|\overrightarrow{a_i{a}_j\left|\right|}·\left|\right|\overrightarrow{b_p{b}_q\left|\right|}}---\left(19\right)$

同时有关系式：

$\left(\begin{array}{c}{t}_1=x_{b_p}-a_{11}{x}_{a_i}-a_{12}{y}_{a_i}-a_{13}{z}_{a_i}\\ t_2=y_{b_p}-a_{21}{x}_{a_i}-a_{22}{y}_{a_i}-a_{23}{z}_{a_i}\\ t_3=z_{b_p}-a_{31}{x}_{a_i}-a_{32}{y}_{a_i}-a_{33}{z}_{a_i}\end{array}\right)---\left(20\right)$

矢量所构成的平面过坐标原点的法线的法向量为：

$\overrightarrow{n}=\overrightarrow{a_i}{a}_j\times \overrightarrow{b_p{b}_q}---\left(21\right)$

将法向量单位化后的方向向量：

$\overrightarrow{\mathrm{ne}}=\frac{\overrightarrow{n}}{\overrightarrow{\left|\right|n\left|\right|}}=(l,m,n,0)---\left(22\right)$

其中l,m,n分别为与X,Y,Z轴的方向余弦，那么矢量以方向向量为 过坐标原点的直线为旋转轴，绕旋转轴旋转α角度后，与矢量方向一 致，则该旋转变换可以表示为：

$P(\alpha ,0)=$

$\left(\begin{array}{cccc}{l}^2(1-\cos \alpha )+\cos \alpha & \mathrm{ml}(1-\cos \alpha )+n\sin \alpha & \mathrm{nl}(1-\cos \alpha )-m\sin \alpha & 0\\ \mathrm{ml}(1-\cos \alpha )-n\sin \alpha & m^2(1-\cos \alpha )+\cos \alpha & \mathrm{nm}(1-\cos \alpha )+l\sin \alpha & 0\\ \mathrm{nl}(1-\cos \alpha )+m\sin \alpha & \mathrm{nm}(1-\cos \alpha )-l\sin \alpha & n^2(1-\cos \alpha )+\cos \alpha & 0\\ 0& 0& 0& 1\end{array}\right)---\left(23\right)$

将公式(18)与公式(23)相比较，可以得到如下关系式：

$\left(\begin{array}{c}{a}_{11}^{\prime }=l^2(1-\cos \alpha )+\cos \alpha \\ a_{12}^{\prime }=\mathrm{ml}(1-\cos \alpha )+n\sin \alpha \\ a_{13}^{\prime }=\mathrm{nl}(1-\cos \alpha )-m\sin \alpha \\ a_{21}^{\prime }=\mathrm{ml}(1-\cos \alpha )-n\sin \alpha \\ a_{22}^{\prime }=m^2(1-\cos \alpha )+\cos \alpha \\ a_{23}^{\prime }=\mathrm{nm}(1-\cos \alpha )+l\sin \alpha \\ a_{31}^{\prime }=\mathrm{nl}(1-\cos \alpha )+m\sin \alpha \\ a_{32}^{\prime }=\mathrm{nm}(1-\cos \alpha )-l\sin \alpha \\ a_{33}^{\prime }=n^2(1-\cos \alpha )+\cos \alpha \end{array}\right)---\left(24\right)$

由公式(24)可知，系数a_uv′(u,v=1,2,3)均为分别关于矢量到矢量的角 度α的确定的函数关系式，可以使用系数a_uv′(u,v=1,2,3)来间接表示公式(20)平 移量(t₁,t₂,t₃)与矢量到矢量的角度α之间的关系，因此，两组对应点对 之间的相似变换关系可以利用矢量之间的相似变换关系加以确定。

假设两个点集A={a₁,a₂,…,a_m}和B={b₁,b₂,…,b_n}之间存在k个一一对应 点，即点集之间的相似变换矩阵为P(R,t)，由于相似变换 并不改变点集之间的几何对应关系，则对于任意一组对应点对 $a_i\leftrightarrow b_i,i=\mathrm{1,2},···,k,$存在矢量集合$A_i=\{\overrightarrow{a_i{a}_1},\overrightarrow{a_i{a}_2},···,0,···,\overrightarrow{a_i{a}_k}\}$和 对应矢量之间的相似变换为P(α,0)，由公式(18)、 公式(23)可知，R与α之间存在公式(24)的关系。因此，若为一组对应点 对时，必可以在点集A和B中确定k-1个其他点，用以构成相对应的矢量对，并 且由任意一对矢量所确定的相似变换均为P(α,0)。反之，若确定的点数小于k， 则该为不相互对应的点对，而且可以知道实际的一一对应点对数应小于 或等于k-1，以此类推，可以找到点集最大一一对应点对的个数。定义此时的实 数4×4相似变换矩阵为P_op(α,0)，利用该相似变换矩阵可以判定给定的一组对应 点对是否为点集中的一组一一对应点对。

根据得到的一组一一对应点对和P_op(α,0)计算两幅图像之间所有一一对应点 对，采用最小二乘算法计算最优四维实数变换矩阵。

所述的最小二乘算法原理为：假设给定四维空间的两个一一对应点集 A={a₁,a₂,…,a_n}和B={b₁,b₂,…,b_n}，需要找到一个四维实数变换矩阵P(R,t)， 使得以误差的平方和作为目标函数取得最小值。当目标函数取到最小值时，可以 认为点集A和B在该四维实数变换矩阵下达到最大程度的相似。定义目标函数 为：

$e^2(R,t)=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{\left|\right|b_i-P(R,t)a_i\left|\right|}^2---\left(25\right)$

对于两个一一对应点集中的一组对应点根据相似变换关系可得：

b_i=P(R,t)a_i  (26)

将公式(26)展开成公式(17)的矩阵表示，定义误差向量为：

$\left(\begin{array}{c}{e}_i=b_i-P(R,t)a_i=\left(\begin{array}{c}{e}_{\mathrm{xi}}\\ e_{\mathrm{yi}}\\ e_{\mathrm{zi}}\\ e_{\mathrm{wi}}\end{array}\right)=\left(\begin{array}{c}{x}_{b_i}\\ y_{b_i}\\ z_{b_i}\\ 1\end{array}\right)-\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\\ a_{21}& a_{22}& a_{32}& t_2\\ a_{31}& a_{32}& a_{33}& t_3\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_{a_i}\\ y_{a_i}\\ z_{a_i}\\ 1\end{array}\right)\\ =\left(\begin{array}{c}{x}_{b_i}-(a_{11}{x}_{a_i}+a_{12}{y}_{a_i}+a_{13}{z}_{a_i}+t_1)\\ y_{b_i}-(a_{21}{x}_{a_i}+a_{22}{y}_{a_i}+a_{23}{z}_{a_i}+t_2)\\ z_{b_i}-(a_{31}{x}_{a_i}+a_{32}{y}_{a_i}+a_{33}{z}_{a_i}+t_3)\\ 0\end{array}\right)\end{array}\right)---\left(27\right)$

其中$e_{\mathrm{xi}}=x_{b_i}-(a_{11}{x}_{a_i}+a_{12}{y}_{a_i}+a_{13}{z}_{a_i}+t_1),$$e_{\mathrm{yi}}=y_{b_i}-(a_{21}{x}_{a_i}+a_{22}{y}_{a_i}+a_{23}{z}_{a_i}+t_2),$$e_{\mathrm{zi}}=z_{b_i}-(a_{31}{x}_{a_i}+a_{32}{y}_{a_i}+a_{33}{z}_{a_i}+t_3),$e_wi=0。

依次计算两个一一对应点集的每组对应点对的误差向量e₁,e₂,…,e_n，根据公 式(25)可得：

$\left(\begin{array}{c}{e}^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{\left|\right|e_i\left|\right|}^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{e}_i^T{e}_i=\frac{1}{n}(e_1^T{e}_1+e_2^T{e}_2+···+e_n^T{e}_n)\\ =\frac{1}{n}(\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{xi}}^2+\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{yi}}^2+\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{zi}}^2+\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{wi}}^2)\end{array}\right)---\left(28\right)$

令：

$\left(\begin{array}{c}{e}_x^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{xi}}^2\\ e_y^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{yi}}^2\\ e_z^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{zi}}^2\\ e_w^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{e}_{\mathrm{wi}}^2=0\end{array}\right)---\left(29\right)$

为了使得方程(28)的目标函数e²取得最小值，那么，只要对方程(28)的每一 项都取得最小值，以为例，将其表示为：

$e_x^2=e_x^T{e}_x=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{(\left(\begin{array}{}\end{array}\begin{array}{}\end{array}\begin{array}{cccc}{x}_{a_i}& y_{a_i}& z_{a_i}& 1\end{array}\right){\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\end{array}\right)}^T-x_{b_i})}^2---\left(30\right)$

取$C_{b_i}=\left(\begin{array}{cccc}{x}_{a_i}& y_{a_i}& z_{a_i}& 1\end{array}\right),$$r_x={\left(\begin{array}{cccc}{a}_{11}& a_{12}& a_{13}& t_1\end{array}\right)}^T,$则公式(30)表示为：

$e_x^2=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{(C_{a_i}{r}_x-x_{b_i})}^2---\left(31\right)$

令：

$E_x=\left(\begin{array}{c}{e}_{x1}\\ e_{x2}\\ ·\\ ·\\ ·\\ e_{\mathrm{xn}}\end{array}\right)=\left(\begin{array}{c}{C}_{a_1}\\ C_{a_2}\\ ·\\ ·\\ ·\\ C_{a_n}\end{array}\right)r_x-\left(\begin{array}{c}{x}_{b_1}\\ x_{b_2}\\ ·\\ ·\\ ·\\ x_{b_n}\end{array}\right)={\mathrm{Cr}}_x-a_x---\left(32\right)$

其中$C={\left(\begin{array}{cccc}{C}_{a_1}& C_{a_2}& ···& C_{a_n}\end{array}\right)}^T,$$a_x={\left(\begin{array}{cccc}{x}_{b_1}& x_{b_2}& ···& x_{b_n}\end{array}\right)}^T,$从而有

$e_x^2=\frac{1}{n}\underset{i=0}{\overset{n}{\Sigma }}{e}_{\mathrm{xi}}^2=\frac{1}{n}{E}_x^T{E}_x=\frac{1}{n}{({\mathrm{Cr}}_x-a_x)}^T({\mathrm{Cr}}_x-a_x)$

$=\frac{1}{n}(r_x^T{C}^T{\mathrm{Cr}}_x-a_x^T{\mathrm{Cr}}_x-r_x^T{C}^T{a}_x+a_x^T{a}_x)---\left(33\right)$

$=\frac{1}{n}(r_x^T{C}^T{\mathrm{Cr}}_x-2a_x^T{\mathrm{Cr}}_x+a_x^T{a}_x)$

对公式(33)求关于r_x的导数，令导数为零，公式表示如下：

$\frac{d}{{\mathrm{dr}}_x}\left(e_x^2\right)=\frac{d}{{\mathrm{dr}}_x}(r_x^T{C}^T{\mathrm{Cr}}_x-2a_x^T{\mathrm{Cr}}_x+a_x^T{a}_x)=2C^T{\mathrm{Cr}}_x-2C^T{a}_x=0---\left(34\right)$

解得$\widehat{r_x}={\left[C^{T}C\right]}^{-1}{C}^T{a}_x---\left(35\right)$即为使得最小的解。

同理对公式(29)的其余两个函数式和求解得：

$\left(\begin{array}{c}\widehat{r_y}={\left[C^{T}C\right]}^{-1}{C}^T{a}_y\\ \widehat{r_z}={\left[C^{T}C\right]}^{-1}{C}^T{a}_z\end{array}\right)---\left(36\right)$

$\left(\begin{array}{c}{r}_y={\left(\begin{array}{cccc}{a}_{21}& a_{22}& a_{23}& t_2\end{array}\right)}^T\\ a_y={\left(\begin{array}{cccc}{y}_{b_1}& y_{b_2}& ···& y_{b_n}\end{array}\right)}^T\\ r_z={\left(\begin{array}{cccc}{a}_{31}& a_{32}& a_{33}& t_3\end{array}\right)}^T\\ a_z={\left(\begin{array}{cccc}{z}_{b_1}& z_{b_2}& ···& z_{b_n}\end{array}\right)}^T\end{array}\right)---\left(37\right)$

最终通过公式(35)和公式(37)可以确定最优的四维实数矩阵。根据公式 (15)以及对航向角、俯仰角和横滚角的定义，可以由精确图像匹配算法获得 的四维实数矩阵计算出飞行器相对于参考系的姿态角和位移量：

其中公式(38)中的下标“主”表示反三角函数的主值，根据图像匹配获 取的所有一一对应点对，利用最小二乘法计算P中元素的值，将求得的P中 元素的值带入公式（38），得到ψ_主,S_ψ,θ_主,S_θ,γ_主,S_γ的值，其中，ψ_主、θ_主、γ_主分别表示航向角ψ、俯仰角θ、横滚角γ的反三角函数的主值；根据航向角ψ、 俯仰角θ、横滚角γ的定义域和a₁₃,a₂₁,a₂₂,a₂₃,a₃₃的符号计算航向角ψ，俯仰 角θ，横滚角γ的精确值，将ψ,S_ψ,θ,S_θ,γ,S_γ作为导航数据。根据姿态角的 定义域可知，除俯仰角θ外，航向角ψ和横滚角γ均需进行象限判断，判断 方式如表1和表2所示：

表1

a₂₂的符号 a₂₁的符号 ψ角真值 象限 + + ψ 0°～90° - + ψ+180° 90°～180° - - ψ+180° 180°～270° + - ψ+360° 270°～360°

表2

a₁₃的符号 a₃₃的符号 γ角真值 象限 + + γ 0°～90° + - γ+180° 90°～180° - + γ -90°～0° - - γ-180° -180°～-90°