CN103927738A - 一种大间距模式下基于双目视觉图像的行星车定位方法 - Google Patents

Abstract

本发明公开一种大间距模式下基于双目视觉图像的行星车定位方法，包括以下步骤：行星车在大间距的两个位置分别拍摄图像；对两个位置拍摄的左相机图像进行A-SIFT匹配；双目相机左右相机图像的立体匹配；匹配特征点的正确性检查；行星车当前位置与姿态的计算。采用本发明的技术方案，使行星车在未知环境行进时精确计算自身位置和姿态，实现行星车自主定位。

Description

一种大间距模式下基于双目视觉图像的行星车定位方法

技术领域

本发明涉及行星车定位技术领域，尤其涉及一种大间距模式下基于视觉图像的行星车定位方法。

背景技术

计算机视觉技术，早在上个世纪70年代的探月活动中就开始得到应用。美国上世纪90年代开始研制的无人导航车，多采用双目视觉测量的导航技术。2003年JPL研发的勇气号和机遇号，以及2012年研发的好奇号，依靠自身的立体相机，实现了在火星表面的长距离安全行走。我国一些高校和科研院所也开始了行星车的研制。目前利用深空站跟踪数据和同波束干涉测量对行星车进行相对定位的精度约在百米量级，该指标远不能满足行星车在陆面行进的定位精度要求，因此行星车一般采用双目视觉系统来实现行星车的自主定位，通过双目视觉系统拍摄图像之间的匹配与特征点的空间关系解算行星车的位置和姿态。然而在大间距模式下利用双目视觉系统进行定位过程中，很难自动匹配图像特征点，并且构建多站点双相机测量定位模型，无法使行星车在未知环境行进时精确计算自身位置和姿势，实现行星车自主定位。

发明内容

本发明要解决的技术问题是，提供一种大间距模式下基于双目视觉图像的行星车定位方法，实现行星车自主定位。

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

一种大间距模式下基于双目视觉图像的行星车定位方法包括以下步骤：

S1、行星车在大间距的两个位置分别拍摄图像：将行星车分别移动到两个不同的位置，利用行星车的双目相机拍摄图像，两个位置分别称为上一站和当前站；

S2、行星车在不同位置拍摄图像的Affine-SIFT匹配：根据行星车在上一站和当前站拍摄图像时相机方向，分为同方向拍摄图像的Affine-SIFT匹配和对望图像的Affine-SIFT匹配，提取上一站和当前站图像的匹配特征点集；当选用同方向模式时，将上一站的左图像与当前站的左图像直接进行Affine-SIFT匹配；当选用对望模式时，首先将上一站和当前站的左图像生成相应的DOM正射影像图，然后将所述相应的DOM正射影像图进行Affine-SIFT匹配；

S3、双目相机左右相机图像的立体匹配：根据同一站点左右相机图像和上一站与当前站点的匹配特征点集，通过相关系数匹配的方法，确定左右图像素点的匹配关系；再根据同一站点左右相机图像和相关系数匹配得到的匹配点坐标，通过左右图像的最小二乘匹配，将图像匹配精度提高到子像素等级；

S4、匹配特征点的正确性检查：利用前方交会算法对立体匹配的结果进行检查，剔除错误的匹配特征点；

S5、计算行星车当前站点位置与姿态：利用光束平差法以共线方程为依据，根据行星车在上一站点左右相机的位置和姿态以及左右相机图像的匹配特征点集，构建多站点双相机测量定位模型，形成包含相机位置和姿态信息与匹配特征点坐标信息的统一误差方程和法方程，进而求解行星车在当前站点的位姿信息。

本发明利用Affine-SIFT算法对上一站与当前站拍摄的左相机图像进行匹配，然后将匹配特征点作为输入，通过相关系数匹配算法进行上一站左、右图像和当前站左、右图像的特征匹配，利用最小二乘法使得影像匹配精度达到子像素等级；利用前方交会算法对上述的匹配结果进行检查，剔除错误匹配点，确保最终定位结算中匹配点输入的正确性；利用行星车双目摄像头在不同位置对目标区的拍摄关系，构建了多站点双相机测量定位模型，实现了通过光束平差算法对行星车位姿的求解。通过S2、S3、S4、S5四个步骤，能够准确地求解行星车的定位问题，通过S2、S3很好地解决行星车在大间距模式下拍摄图像的自动匹配问题，通过S5的行星车定位模型很好地实现了行星车位姿的解算。采用本发明技术方案，使行星车在未知环境行进时精确计算自身位置和姿态，实现行星车自主定位。

附图说明

图1为大间距模式下基于双目视觉对行星车进行定位的流程图；

图2为立体匹配中左图像目标区和右图像搜索区的关系示意图；

图3为立体匹配中最小二乘图像匹配的迭代过程示意图。

具体实施方式

如图1所示，本实施例提供一种大间距模式下基于双目视觉图像的行星车定位方法包括以下步骤：

S1、行星车在大间距的两个位置分别拍摄图像

将行星车分别移动到两个不同的位置，利用行星车的双目相机拍摄图像，两个位置分别称为上一站和当前站。

S2、行星车在不同位置拍摄的图像进行特征提取与匹配

Affine-SIFT方法是一种具备完全仿射不变特征的特征提取与匹配算法。该算法是在SIFT算法的基础上通过增加仿射变换处理改进而来的，能够处理旋转和相对倾斜较大的图像间的匹配；Affine-SIFT通过模拟相机的光轴旋转对图像进行预处理来模拟图像的倾斜变化，实现完全意义上的仿射，能够提高匹配的成功率与正确性。结合Affine-SIFT匹配算法的特点，将行星车在不同位置拍摄的图像进行特征提取与匹配。

根据行星车在上一站和当前站拍摄图像时相机方向，分为同方向拍摄图像的Affine-SIFT匹配和对望图像的Affine-SIFT匹配，提取上一站拍摄的左图像和当前站拍摄的左图像的匹配特征点集 $\{(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i),(x_{\mathrm{CL}}^i,y_{\mathrm{CL}}^i)|i=\mathrm{0,1},...,N\}.$

当为同方向模式时，将上一站相机拍摄的左图像与当前站相机拍摄的左图像(即L_上、L_当)直接进行Affine-SIFT特征提取与匹配。

当为对望模式时，先将上一站相机和当前站相机拍摄的左图像(即L_上、L_当)生成相应的DOM正射影像图，即L′_上、L′_当，再对所述相应的DOM正射影像图(即L′_上和L′_当)进行Affine-SIFT特征提取与匹配。

其中，将上一站和当前站的左图像(即L_上、L_当)生成相应的DOM正射影像图，即L′_上、L′_当，包括四个步骤：

1)确立像点坐标与地面点坐标的转换关系

地面坐标P(X，Y，Z)、相机的外方位参数原始图像上相应的像点坐标p(x，y)满足相机拍摄过程的共线方程，即满足：

$\begin{array}{c}x=-f\frac{a_1(X-X_s)+b_1(Y-Y_s)+c_1(Z-Z_s)}{a_3(X-X_s)+b_3(Y-Y_s)+c_3(Z-Z_s)},\\ y=-f\frac{a_2(X-X_s)+b_2(Y-Y_s)+c_2(Z-Z_s)}{a_3(X-X_s)+b_3(Y-Y_s)+c_3(Z-Z_s)}.\end{array}---\left(1\right)$

其中，a₁，a₂，a₃，b₁，b₂，b₃，c₁，c₂，c₃是从摄像机坐标到像平面坐标系旋转变换矩阵的元素，X_s，Y_s，Z_s为平移参数，为旋转参数，即满足：

$\left[\begin{array}{c}x\\ y\\ -f\end{array}\right]=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ a_2& b_2& c_2\\ a_3& b_3& c_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right],$

其中，(x，y)是像平面坐标，f是焦距。

2)确立地面点坐标与DOM图像点坐标转换关系

设地面坐标为P(X，Y，Z)，对应的正射影像上一点P的坐标为(X′，Y′)，DOM正射影像在下角图廓点对应的地面坐标为(X₀，Y₀，Z₀)，DOM正射影像比例尺分母为M，则地面坐标P(X，Y，Z)与DOM正射影像坐标(X′，Y′)间的转换关系表示为：

X=X₀+MX′     (2)

Y=Y₀+MY′

其中，Z是P点的高程，令Z=Z₀，将地面三维坐标点按照公式式(2)统一降低到同一平面，然后对平面中的每一个坐标进行灰度赋值即可生成的DOM图。

3)确立像点坐标与DOM图像点坐标的转换关系

联立公式(1)(2)，可以得到原始图像的像点坐标p(x，y)与DOM正射影响坐标(X′，Y′)的转换关系，即：

$\begin{array}{c}x=-f\frac{a_1(X_0+{\mathrm{MX}}^{\′}-X_s)+b_1(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_1{Z}_s}{a_3(X_0+{\mathrm{MX}}^{\′}-X_s)+b_3(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_3{Z}_s},\\ y=-f\frac{a_2(X_0+{\mathrm{MX}}^{\′}-X_s)+b_2(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_2{Z}_s}{a_3(X_0+{\mathrm{MX}}^{\′}-X_s)+b_3(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_3{Z}_s}.\end{array}---\left(3\right)$

3)DOM图的灰度内插及内插点的灰度赋值

根据公式(3)求得DOM正射影像图点的坐标(X′，Y′)与原始图像点坐标(x，y)的转换关系。由于所求得的像点坐标不一定正好落在其扫描采样的点上，为此这个像点的灰度值不能直接读出，必须进行灰度内插。采用双线性内插方法，求得P点的灰度值G(X′，Y′)的公式为：

$\begin{array}{c}G(X^{\′},Y^{\′})=\frac{1}{{\mathrm{\Δ}}^2}[(x_1-X^{\′})(y_1-Y^{\′})g(x_1,y_1)+(x_1-X^{\′})(Y^{\′}-y_0)g(x_1,y_0)+\\ (X^{\′}-x_0)\left(Y^{\′}{-y}_0\right)g(x_0,y_0)+(X^{\′}-x_0)(y_1-Y^{\′})g(x_0,y_1)],\end{array}---\left(4\right)$

其中，g(x_i，y_i)表示原始图像(x_i，y_i)像素点的灰度值，为DOM正射影像的扫描采样间隔。

依次对DOM正射影像的每个像素完成上述运算，即获得DOM正射影像，即为L′_上、L′_当。

S3、双目相机左右图像的立体匹配

步骤一：左右图像的相关系数匹配：以同一站点左右相机图像和上一站与当前站点的匹配特征点集作为输入，通过相关系数匹配的方法，快速确定左右图像素点的匹配关系。主要步骤如下：

1)以上一站或当前站点左图像的某个匹配特征点作为目标点，以此点为中心选取m×n个像素的灰度阵列作为目标区，如图2左侧目标区所示。

2)为了在右图像上搜索左图目标点的匹配点，先估计出该匹配点可能存在的大致范围，确定一个k×l(k>m，l>n)个像素的灰度阵列作为搜索区，如图2右侧搜索区所示。

3)依次在搜索区中取出m×n个像素灰度阵列(搜索窗口通常取m=n)，计算其与目标区的相似性测度 ${\mathrm{\ρ}}_{\mathrm{ij}}(i=i_0-\frac{l}{2}+\frac{n}{2},...,i_0+\frac{l}{2}-\frac{n}{2};j=j_0-\frac{k}{2}+\frac{m}{2},...,j_0+\frac{k}{2}-\frac{m}{2}),$ (i₀，j₀)为搜索区中心像素。当ρ_ij取得最大值时，则认为该搜索窗口的中心像素是目标点的匹配点，记匹配点坐标为即或

${\mathrm{\ρ}}_{x_R,y_R^i}=\max \left\{{\mathrm{\ρ}}_{\mathrm{ij}}|\begin{array}{c}i=i_0-\frac{l}{2}+\frac{n}{2},...,i_0+\frac{l}{2}-\frac{n}{2},\\ j=j_0-\frac{k}{2}+\frac{m}{2},...,j_0+\frac{k}{2}-\frac{m}{2}.\end{array}\right\}---\left(5\right)$

其中，相似性测度ρ_ij通过目标区图像和搜索区以为中心的m×n个像素阵列的相关系数ρ_k计算。

${\mathrm{\ρ}}_k=\frac{{\mathrm{\σ}}_{{\mathrm{gg}}^{\′}}}{\sqrt{{\mathrm{\σ}}_{\mathrm{gg}}{\mathrm{\σ}}_{g^{\′}{g}^{\′}}}}(k=\mathrm{0,1},...,m-n)---\left(6\right)$

设目标点在左图像中的坐标为(x，y)，搜索区内某个点的坐标为(x′，y′)，(x′，y′)相对于(x，y)的偏移表示为(k₁，k₂)，g(x，y)是以(x，y)为中心的目标区图像，g′(x′，y′)是搜索区中以(x′，y′)为中心的搜索区内m×n像素的图像区域，g_ij是为目标区坐标(i，j)的图像灰度，是以(x，y)为中心的目标区影像灰度的平均值，是以(x′，y′)为中心的目标区图像灰度的平均值，σ_gg′是以(x，y)为中心的目标区影像和以(x′，y′)为中心的影像的方差，σ_gg以(x，y)为中心的目标区图像的方差，σ_g′g′是搜索区中以(x′，y′)为中心的图像的方差。它们之间的关系式：

$\stackrel{\‾}{g}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}},$

${\stackrel{\‾}{g}}^{\′}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{i+k_1,j+k_2}^{\′},$

${\mathrm{\σ}}_{\mathrm{gg}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}}^2-g^2,---\left(7\right)$

${\mathrm{\σ}}_{g^{\′}{g}^{\′}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g_{i+k_1,j+k_2}^{\′}}^2-{\stackrel{\‾}{g}}^{\′}{\stackrel{\‾}{g}}^{\′},$

${\mathrm{\σ}}_{{\mathrm{gg}}^{\′}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}}{g}_{i+k_1,j+k_2}-\mathrm{gg}.$

通过左右图像的相关系数匹配，得到上一站与当前站点的匹配特征点集对应的上一站点和当前站点左右图像的匹配点集： $\{(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i,x_{\mathrm{CL}}^i,y_{\mathrm{CL}}^i),(x_{\mathrm{FR}}^i,y_{\mathrm{FR}}^i,x_{\mathrm{CR}}^i,y_{\mathrm{CR}}^i)|i=\mathrm{0,1},...,N\}.,$ 其中，为相关系数匹配的匹配点，为的相关系数匹配的匹配点。

步骤二：左右图像的最小二乘匹配：以同一站点左右相机图像和相关系数匹配得到的匹配点坐标 $\{(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i,x_{\mathrm{CL}}^i),(x_{\mathrm{FR}}^i,y_{\mathrm{FR}}^i,x_{\mathrm{CR}}^i,y_{\mathrm{CR}}^i)|i=\mathrm{0,1},...,N\}.$ 作为输入，通过左右图像的最小二乘匹配，将图像匹配精度提高到子像素等级。如图3所示，主要步骤如下：

1)几何畸变改正：设左图像中特征点坐标为或根据几何变形参数a₀，a₁，a₂，b₀，b₁，b₂将左方图像窗口的像片点变换至右方图像点或因到的变换和到的变换形式完全相同，后面仅对到的变换进行描述。

$\begin{array}{c}{x}_{\mathrm{FR}}^i=a_0+a_1{x}_{\mathrm{FL}}^i+a_2{y}_{\mathrm{FL}}^i,\\ x_{\mathrm{FR}}^i=b_0+b_1{x}_{\mathrm{FL}}^i+b_2{y}_{\mathrm{FL}}^i.\end{array}---\left(8\right)$

考虑右图像相对于左图像的线性灰度畸变可得：

$g_1(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i)+n_1(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i)=h_0+h_1{g}_2(a_0+a_1{x}_{\mathrm{FL}}^i+a_2{y}_{\mathrm{FL}}^i,b_0+b_1{x}_{\mathrm{FL}}^i+b_2{y}_{\mathrm{FL}}^i)+n_2(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i)---\left(9\right)$

其中，g₁和_g2表示左图像和右图像中像素点的灰度值，n₁和n₂表示左图像和右图像中像素点的灰度噪声，h₀、h₁和h₂分别表示左右图像线性灰度变换的辐射畸变参数。

2)重采样：根据公式(9)采用双线性内插进行灰度重采样计算线性化后，可得最小二乘图像匹配的误差方程式：

v=c₁dh₀+c₂dh₁+c₃da₀+c₄da₁+c₃da₂+c₆db₀+c₇db₁+c₈db₂-Δg     (10)其中，dh₀，dh₁，da₀，…db₂是畸变参数的改正值，Δg是相应像素的灰度差，c_i，i=1，2，…，8是畸变参数改正数的系数，v是像点误差。

3)辐射畸变改正：利用由最小二乘图像匹配误差方程求得的辐射畸变参数h₀，h₁，对上述重采样结果作辐射改正，即

4)计算左图像区域与经过几何、辐射改正后的右图像区域的灰度阵列之间的相关系数ρ。若相关系数ρ小于前一次迭代后所求得的相关系数，则计算最佳匹配点，迭代结束；否则进行步骤5)；

5)采用最小二乘图像匹配，解求变形参数的改正值dh₀，dh₁，da₀…；

6)计算变形参数：设是前一次变形参数，是本次迭代所求得的改正值，则几何变形参数通过如下关系改正：

$\⇒\begin{array}{c}\left[\begin{array}{c}1\\ x_{\mathrm{FR}}^i\\ y_{\mathrm{FR}}^i\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ a_0^i& a_1^i& a_2^i\\ b_0^i& b_1^i& b_2^i\end{array}\right]\left[\begin{array}{c}1\\ x_{\mathrm{FL}}^i\\ y_{\mathrm{FL}}^i\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ {\mathrm{da}}_0^i& 1+{\mathrm{da}}_1^i& {\mathrm{da}}_2^i\\ {\mathrm{db}}_0^i& {\mathrm{db}}_1^i& {\mathrm{dn}}_2^i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ a_0^{i-1}& a_1^{i-1}& a_2^{i-1}\\ b_0^{i-1}& b_1^{i-1}& b_2^{i-1}\end{array}\right]\left[\begin{array}{c}1\\ x_{\mathrm{FL}}^i\\ y_{\mathrm{FL}}^i\end{array}\right],\\ \begin{array}{c}{a}_0^i=a_0^{i-1}+{\mathrm{da}}_0^i+a_0^{i-1}{\mathrm{da}}_1^i+b_0^{i-1}{\mathrm{da}}_2^i\\ a_1^i=a_1^{i-1}+a_1^{i-1}{\mathrm{da}}_1^i+b_2^{i-1}{\mathrm{da}}_2^i\\ a_2^i=a_2^{i-1}+a_2^{i-1}{\mathrm{da}}_1^i+b_2^{i-1}{\mathrm{da}}_2^i\\ b_0^i=b_0^{i-1}+{\mathrm{db}}_0^i+a_0^{i-1}{\mathrm{db}}_1^i+b_0^{i-1}{\mathrm{db}}_2^i\\ b_1^i=b_1^{i-1}+a_1^{i-1}{\mathrm{db}}_1^i+b_1^{i-1}{\mathrm{db}}_2^i\\ b_2^i=b_2^{i-1}+a_2^{i-1}{\mathrm{db}}_1^i+b_2^{i-1}{\mathrm{db}}_2^i\end{array}\end{array}$

辐射畸变参数通过如下关系改正：

$\⇒\begin{array}{c}\left[\begin{array}{c}1\\ g_1\end{array}\right]\left[\begin{array}{cc}1& 0\\ {\mathrm{dh}}_0^i& 1+{\mathrm{dh}}_1^i\end{array}\right]\left[\begin{array}{cc}1& 0\\ h_0^{i-1}& h_1^{i-1}\end{array}\right]\left[\begin{array}{c}1\\ g_2\end{array}\right],\\ \begin{array}{c}{h}_0^i=h_0^{i-1}+{\mathrm{dh}}_0^i+h_0^{i-1}{\mathrm{dh}}_1^i,\\ h_1^i=h_1^{i-1}+h_1^{i-1}{\mathrm{dh}}_1^i.\end{array}\end{array}$

7)计算左图目标点的右图匹配点：根据最小二乘匹配的精度理论可知，坐标精度取决于图像灰度的梯度根据梯度的平方为权，在左图像以为中心的区域窗口W_L内对坐标作加权平均：

$x_{\mathrm{FLt}}^i=\underset{x\∈W_L}{\mathrm{\Σ}}x\·{{\stackrel{\·}{g}}_x}^2/\underset{x\∈W_L}{\mathrm{\Σ}}{{\stackrel{\·}{g}}_x}^2,$

$y_{\mathrm{FLt}}^i=\underset{y\∈W_L}{\mathrm{\Σ}}y\·{{\stackrel{\·}{g}}_y}^2/\underset{y\∈W_L}{\mathrm{\Σ}}{{\stackrel{\·}{g}}_y}^2.$

以它作为目标点坐标，根据最小二乘图像匹配所求得的几何变换参数求得上一站右图像中的匹配点坐标：

$x_{\mathrm{FRt}}^i=a_0+a_1{x}_{\mathrm{FLt}}^i+a_2{y}_{\mathrm{FLt}}^i,$

$y_{\mathrm{FRt}}^i=b_0+b_1{x}_{\mathrm{FLt}}^i+b_2{y}_{\mathrm{FLt}}^i.$

同理可得到当前站左右图像中的匹配点坐标和通过左右图像的最小二乘匹配，将相关系数匹配后得到的上一站点和当前站点左右图像的匹配点集 $\{(x_{\mathrm{FL}}^i,y_{\mathrm{FL}}^i,x_{\mathrm{CL}}^i),(x_{\mathrm{FR}}^i,y_{\mathrm{FR}}^i,x_{\mathrm{CR}}^i,y_{\mathrm{CR}}^i)|i=\mathrm{0,1},...,N\}.$ 进行更新，得到新上一站点和当前站点左右图像的匹配点集：

$\{(x_{\mathrm{FLt}}^i,y_{\mathrm{FLt}}^i,x_{\mathrm{CLt}}^i,y_{\mathrm{CLt}}^i),(x_{\mathrm{FRt}}^i,y_{\mathrm{FRt}}^i,x_{\mathrm{CRt}}^i,y_{\mathrm{CRt}}^i)|i=\mathrm{0,1},...,N\}.$

S4、匹配特征点的正确性检查

利用前方交会算法对立体匹配的结果进行检查，剔除错误的匹配点。具体步骤如下：

1)将上一站点和当前站点左右相机图像的匹配特征点集和作为输入，利用前方交会的方法计算获得其对应的地面观测点P_i，i=1，2，…，n的在上一站点和当前站点相机坐标系(左相机坐标系或右相机坐标系)下的三维坐标，设在前一站坐标系下的三维坐标计作P_i(X_i，Y_i，Z_i)，在当前站坐标系下的三维坐标计作P_i′(X_i′，Y_i′，Z_i′)；

2)利用获得的匹配点在前一站和当前站坐标系下的坐标计算它们对应的中心点然后计算每点与P_c的距离D_i=|P_i-P_c|，D_i′=|P_i′-P_c′|；

3)计算ΔD_i=|D_i-D_i′|，剔除满足的P_i点对应的上一站点和当前站点左右图像的匹配点和其中α是权重系数，一般情况下α=3；

4)重复步骤2)和3)，直至所有点对应的ΔD_i满足

剔除错误特征点后，输出上一站点和当前站点左右相机图像的匹配特征点集，其中包含的特征点数量要求大于等于4组。新的上一站点和当前站点左右相机图像的匹配特征点集表示为：

$\{(x_{\mathrm{FLt}}^i,y_{\mathrm{FLt}}^i,x_{\mathrm{CLt}}^i,y_{\mathrm{CLt}}^i),(x_{\mathrm{FRt}}^i,y_{\mathrm{FRt}}^i,x_{\mathrm{CRt}}^i,y_{\mathrm{CRt}}^i)|i=\mathrm{0,1},...,N\}.$

S5、计算行星车当前位置与姿态

利用光束法平差以共线方程为依据，将行星车在上一站点左右相机的位置和姿态以及左右相机图像的匹配特征点 $\{(x_{\mathrm{FLt}}^i,y_{\mathrm{FLt}}^i,x_{\mathrm{CLt}}^i,y_{\mathrm{CLt}}^i),(x_{\mathrm{FRt}}^i,y_{\mathrm{FRt}}^i,x_{\mathrm{CRt}}^i,y_{\mathrm{CRt}}^i)|i=\mathrm{0,1},...,N\}.$ 作为整体输入，构建多站点双相机测量定位模型，形成包含相机位置和姿态信息与匹配特征点坐标的统一误差方程和法方程，求解行星车在当前站点的位姿信息，具体步骤如下：

1)根据行星车在上一站点左右相机位置和姿态和通过前方交会计算图像匹配特征点和对应的观测点的地面坐标初值P_i(X_i，Y_i，Z_i)，然后根据观测点地面坐标初值采用后方交会方法计算当前站相机位置和姿态初值和作为光束法平差迭代计算的初始输入值；

2)将空间交会共线方程(公式(2))线性化，使用精确匹配特征点的像点坐标为观测值建立误差方程并线性化，则有公式(11)：

其中v_x，v_y为像点误差，a₁₁，a₁₂…a₂₆为误差方程系数，分别为当前站点的位姿的改正值，ΔX，ΔY，ΔZ为观测点的位置的改正值，l_x、l_y为相应误差方程式的常数项，有公式(12)：

将公式(12)简化，可得公式(13)，其中i为相机的序号，j为观测点的序号：

A_ijΔ_i+B_ijΔ_j-l_i=v_i     (13)

3)根据误差方程(13)，可得相应的法方程，求解行星车当前站点的位姿的改正数和观测点的位置的改正数ΔX，ΔY，ΔZ。然后利用改正数对行星车当前站点相机位置和姿态初值、观测点地面坐标初值进行改正，反复多次迭代，直至改正数小于规定限差，实现计算收敛。

通过上述求解过程，最终求得当前站左相机的精确位置和姿态即为行星车的位置和姿态。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (3)

1.一种大间距模式下基于双目视觉图像的行星车定位方法，其特征在于，包括以下步骤：

S1、行星车在大间距的两个位置分别拍摄图像：将行星车分别移动到两个不同的位置，利用行星车的双目相机拍摄图像，两个位置分别称为上一站和当前站；

S2、行星车在不同位置拍摄图像的Affine-SIFT匹配：根据行星车在上一站和当前站拍摄图像时相机方向，分为同方向拍摄图像的Affine-SIFT匹配和对望图像的Affine-SIFT匹配，提取上一站拍摄的左图像和当前站拍摄的左图像的匹配特征点集；当选用同方向模式时，将上一站的左图像与当前站的左图像直接进行Affine-SIFT匹配；当选用对望模式时，首先将上一站和当前站的左图像生成相应的DOM正射影像图，再将所述相应的DOM正射影像图进行Affine-SIFT匹配；

S3、双目相机左右相机图像的立体匹配：根据同一站点左右相机图像和上一站与当前站点的匹配特征点集，通过左右图像的相关系数匹配方法，得到上一站与当前站点的匹配特征点集；再通过左右图像的最小二乘匹配方法，将图像匹配精度提高到子像素等级；

S4、匹配特征点的正确性检查：利用前方交会算法对立体匹配的结果进行检查，剔除错误的匹配特征点；

S5、计算行星车当前位置与姿态：利用光束法平差以共线方程为依据，根据行星车在上一站点左右相机的位置和姿态以及左右相机图像的匹配特征点集，构建多站点双相机测量定位模型，形成包含相机位置和姿态信息与匹配特征点坐标信息的统一误差方程和法方程，进而求解行星车在当前站点的位姿信息。

2.如权利要求1所述的一种大间距模式下基于双目视觉图像的行星车定位方法，其特征在于，在S2中，当为对望模式时，将上一站和当前站的左图像生成相应DOM正射影像图，包括四个步骤：

1)确立像点坐标与地面点坐标的转换关系

地面坐标P(X，Y，Z)、相机的外方位参数原始图像上相应的像点坐标p(x，y)满足相机拍摄过程的共线方程，即满足：

$\begin{array}{c}x=-f\frac{a_1(X-X_s)+b_1(Y-Y_s)+c_1(Z-Z_s)}{a_3(X-X_s)+b_3(Y-Y_s)+c_3(Z-Z_s)},\\ y=-f\frac{a_2(X-X_s)+b_2(Y-Y_s)+c_2(Z-Z_s)}{a_3(X-X_s)+b_3(Y-Y_s)+c_3(Z-Z_s)}.\end{array}---\left(1\right)$

其中，a₁，a₂，a₃，b₁，b₂，b₃，c₁，c₂，c₃是从摄像机坐标到像平面坐标系旋转变换矩阵的元素

2)确立地面点坐标与DOM图像点坐标转换关系

设地面坐标为P(X，Y，Z)，对应的正射影像上一点P的坐标为(X′，Y′)，DOM正射影像在下角图廓点对应的地面坐标为(X₀，Y₀，Z₀)，DOM正射影像比例尺分母为M，则地面坐标P(X，Y，Z)与DOM正射影像坐标(X′，Y′)间的转换关系表示为：

X=X₀+MX′    (2)

Y=Y₀+MY′

其中，Z是P点的高程，

3)确立像点坐标与DOM图像点坐标的转换关系

根据公式(1)(2)，得到原始图像的像点坐标p(x，y)与DOM正射影响坐标(X′，Y′)的转换关系，即：

$\begin{array}{c}x=-f\frac{a_1(X_0+{\mathrm{MX}}^{\′}-X_s)+b_1(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_1{Z}_s}{a_3(X_0+{\mathrm{MX}}^{\′}-X_s)+b_3(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_3{Z}_s},\\ y=-f\frac{a_2(X_0+{\mathrm{MX}}^{\′}-X_s)+b_2(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_2{Z}_s}{a_3(X_0+{\mathrm{MX}}^{\′}-X_s)+b_3(Y_0+{\mathrm{MY}}^{\′}-Y_s)-c_3{Z}_s}.\end{array}---\left(3\right)$

3)DOM图的灰度内插及内插点的灰度赋值

根据公式(3)求得DOM正射影像图点的坐标(X′，Y′)与原始图像点坐标(x，y)的转换关系，采用双线性内插方法，求得P点的灰度值G(X′，Y′)的公式为：

$\begin{array}{c}G(X^{\′},Y^{\′})=\frac{1}{{\mathrm{\Δ}}^2}[(x_1-X^{\′})(y_1-Y^{\′})g(x_1,y_1)+(x_1-X^{\′})(Y^{\′}-y_0)g(x_1,y_0)+\\ (X^{\′}-x_0)\left(Y^{\′}{-y}_0\right)g(x_0,y_0)+(X^{\′}-x_0)(y_1-Y^{\′})g(x_0,y_1)],\end{array}$

其中，g(x_i，y_i)表示原始图像(x_i，y_i)像素点的灰度值，为DOM正射影像的扫描采样间隔；

依次对DOM正射影像的每个像素完成上述运算，即获得DOM正射影像。

3.如权利要求1或2所述的一种大间距模式下基于双目视觉图像的行星车定位方法，其特征在于，S5具体包括以下步骤：

1)根据行星车在上一站的位置和姿态和通过前方交会计算图像匹配特征点和对应的观测点的地面坐标初值P_i(X_i，Y_i，Z_i)，然后根据观测点地面坐标初值采用后方交会方法计算当前站相机位置和姿态初值和作为光束法平差迭代计算的初始输入值；

2)将空间交会共线方程线性化，使用精确匹配特征点的像点坐标为观测值建立误差方程并线性化，则有公式(4)：

其中v_x，v_y为像点误差，a₁₁，a₁₂…a₂₆为误差方程系数，分别为外方位角元素和线元素改正值，ΔX，ΔY，ΔZ为特征点对应物方点三维坐标的改正值，l_x、l_y为相应误差方程式的常数项，则：

将公式(5)简化，可得公式(6)，其中，i为相机的序号，j为观测点的序号：

A_ijΔ_i+B_ijΔ_j-l_i=v_i     (6)

3)根据误差方程(6)，可得相应的法方程，求解行星车当前站点的位姿的改正数和观测点的位置的改正数ΔX，ΔY，ΔZ；然后利用改正数对行星车当前站点相机位置和姿态初值、观测点地面坐标初值进行改正，反复多次迭代，直至改正数小于规定限差，实现计算收敛；

通过上述求解过程，最终求得当前站左相机的精确位置和姿态即为行星车的位置和姿态。