CN103033189A - 一种深空探测巡视器惯性/视觉组合导航方法 - Google Patents

Abstract

本发明涉及一种深空探测巡视器惯性/视觉组合导航方法，根据行星固连坐标系下捷联惯导的力学编排，建立巡视器惯性/视觉组合导航的状态模型；利用双目视觉相机对巡视器周围环境进行拍摄，获得立体图像序列，通过图像特征提取算法得到特征点的像素坐标，建立以特征点像素坐标为量测向量的量测模型；并使用Unscented卡尔曼滤波估计深空探测巡视器的位置、速度和姿态。本发明能够有效修正惯导的位置误差，提高导航精度，非常适用于深空探测巡视器的自主导航，本发明属于航天导航技术领域，不仅可以为深空探测巡视器提供高精度导航参数，而且可为其自主导航系统设计提供参考。

Description

一种深空探测巡视器惯性/视觉组合导航方法

技术领域

本发明涉及深空探测巡视器在行星表面巡视勘察时，基于图像特征点像素坐标信息的惯性/视觉组合导航方法，是一种非常适用于深空探测巡视器的自主导航方法。

背景技术

深空探测作为人类航天活动的重要领域，对一个国家的科学研究、经济发展和军事应用价值都有一般航天活动无法比拟的特殊作用。深空探测巡视器作为当前行星际探测的主要工具，在行星表面自动巡视勘测、图像获取、采样分析和返回等方面发挥着重要作用，是当前研究的热点之一。

巡视器要在行星表面陌生的非结构化环境中安全行驶和工作，有效的导航是其可以成功完成预定科学探测任务的前提。由于行星空间环境的特殊性，现有的大多数地面机器人导航方法往往不能用于深空探测巡视器，当前深空探测巡视器主要还是借助地面测控站进行遥测遥制，但我国受地理条件的限制，加之地球和各天体的自转和公转运动的影响，可测控时间较短，而且即使在可测控时段，从测量数据下传到控制信息上传之间也存在较长的时间延迟，如NASA火星探测任务中一个导航控制指令的延迟通常为一个火星日，因此为提高巡视器的安全性、可靠性以及工作效率，较先进的技术是机器人利用自己的传感和控制系统实现自主导航。

目前可以用于深空探测巡视器的自主导航方式主要有：惯性导航（航位推算）、天文导航、视觉导航和组合导航等。惯性导航具有自主性强、短时精度高、导航参数完备等优点，但由于惯性器件存在漂移，其导航误差随时间积累。视觉导航系统具有体积小、重量轻、寿命长、可靠性高、图像信息丰富、抗干扰能力强等优点，对于短距离的障碍检测和路径规划非常有效，但数据更新率低。惯性导航和视觉导航有其各自的特点，并具有互补的特性，将两种导航方法相结合，取长补短，利用信息融合技术进行组合导航，可以最大限度地提高导航的精度和可靠性。目前应用于巡视器的惯性/视觉组合导航方法主要是惯性导航（航位推算）和视觉里程计相结合的方法，在可能存在滑移的情况下，使用视觉里程计完全取代IMU和车轮里程计得到的位置信息，而没有使用卡尔曼滤波及相关算法来融合IMU，车轮编码器和视觉里程计的运动信息。该方法没有通过组合导航的方式来提高导航精度，而是分时使用两种导航方法，且由于视觉里程计是通过运动估计获得相邻时刻的相对导航参数，然后在前一时刻导航参数基础上进行叠加来更新导航结果，其本身误差亦会随时间积累，从而导致导航的精度不高。

发明内容

本发明要解决的技术问题是：克服传统的惯性导航和航位推算相结合的位置估计方法在产生滑移情况下的精度低、安全性差等缺点，弥补惯性导航与视觉里程计组合模式中运动估计误差对组合导航精度影响大这一不足，为深空探测巡视器在行星表面漫游提供一种高精度的惯性/视觉组合导航方法。

本发明解决其技术问题所采用的技术方案为：建立恰当的深空探测巡视器状态模型，通过双目视觉相机连续拍摄获得巡视器周围环境信息的立体图像序列，再通过图像特征提取获得图像中的特征点像素坐标，建立以特征点像素坐标为量测量的量测模型，使用Unscented卡尔曼滤波估计深空探测巡视器的导航参数。

具体包括以下步骤：

1.建立深空探测巡视器惯性/视觉组合导航系统状态模型；

选取行星固连坐标系为全局参考坐标系，其定义为：原点与行星质心重合，Z轴指向行星北极，X轴指向行星赤道面与0°子午线的交点，Y轴在赤道平面里与XOZ构成右手坐标系，根据捷联惯导在行星固连坐标系下的导航解算方程，可得组合导航的状态模型：

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_p^b=\frac{1}{2}\mathrm{\Ω}({\mathrm{\ω}}_{\mathrm{ib}}^b-b_g-C_p^b{\mathrm{\ω}}_{\mathrm{ip}}^p)q_p^b+w_g\\ {\stackrel{\·}{p}}_b^p=v_b^p\\ {\stackrel{\·}{v}}_b^p=C_b^p(f_b-b_a)-2{\mathrm{\ω}}_{\mathrm{ip}}^p\×v_b^p+g^p+w_a\\ {\stackrel{\·}{b}}_g=0\\ {\stackrel{\·}{b}}_a=0\end{array}\right.---\left(1\right)$

式中，为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数，为4×1维的向量，

和

分别为巡视器在行星固连坐标系下的位置和速度，均为3×1维的向量，b_g和b_a分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置，均为3×1维的向量。为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示，由陀螺仪直接测得，f_b为比力信息，由加速度计直接测得，

为行星自转角速率在行星固连坐标系下的矢量表示，为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵，g^p为重力矢量在行星固连坐标系下的表示，Ω(·)为四元数矩阵，w_g和w_a分别为陀螺仪和加速度计的随机误差，均为零均值的高斯白噪声；

式(1)中变量都是与t有关的函数，可简写为

$\stackrel{\·}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(2\right)$

状态变量为

f(X(t),t)为系统非线性连续状态转移函数，状态噪声为w(t)＝[w_g,w_a]^T；

2.建立基于特征点像素坐标为量测量的量测模型；

巡视器本体坐标系与IMU坐标系重合，摄像机坐标系和IMU坐标系之间的转换关系已知，t_k时刻和t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量为

其分别为3×3维的矩阵和3×1维的向量，计算公式为：

$R_k^{k+1}=C_b^c{C}_{p,k+1}^b{\left(C_b^c{C}_{p,k}^b\right)}^T$                                              (3)

$T_k^{k+1}=-C_b^c{C}_{p,k+1}^b(p_{b,k+1}^p+{C_{p,k+1}^b}^T{p}_c^b)+C_b^c{C}_{p,k+1}^b(p_{b,k}^p+{C_{p,k}^b}^T{p}_c^b)$

式中，k表示时间序号，k＝1,2,...，

为摄像机相对巡视器本体坐标系的安装矩阵，为摄像机在巡视器本体坐标系中的位置，为3×1维的向量，由标定可知，和

分别为t_k时刻和t_k+1时刻巡视器在行星固连坐标系中的位置，均为3×1维的向量，

和

分别为t_k时刻和t_k+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵，均为3×3维，

和可根据t_k时刻组合导航的滤波结果获得，

和

可由t_k+1时刻惯导的状态更新结果获得；

跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为：

${\hat{p}}_{i,k+1}^{c_l}=R_k^{k+1}{p}_{i,k}^{c_l}+T_k^{k+1}---\left(4\right)$

式中，k＝1,2,…，为第i个特征点在t_k时刻左摄像机坐标系下的三维坐标，根据三角测量原理和双目视觉系统参数可以获得，

为第i个特征点在t_k+1时刻左摄像机坐标系下三维坐标的估计值，

分别为从t_k时刻到t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量，由式(3)获得；

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在左摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^l=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^l\\ {\hat{v}}_{i,k+1}^l\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_l}{\hat{p}}_{i,k+1,x}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\\ \frac{f_{c_l}{\hat{p}}_{i,k+1,y}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\end{array}\right]+\left[\begin{array}{c}{u}_{l0}\\ v_{l0}\end{array}\right]+{\tilde{v}}_{i,k+1}^l,$ i＝1,2,…,M          (5)

式中，为第i个特征点在左摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在左摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽，(u_l0，v_l0)为左摄像机光轴与成像平面交点的像素坐标，

为左摄像机焦距，

为第i个特征点在t_k+1时刻左摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，

为左图像中特征点提取误差，为2×1维列向量；

根据平行双目立体视觉系统的外部参数，可以得到t_k+1时刻第i个特征点在右摄像机坐标系下三维坐标的估计值

表达式为：

${\hat{p}}_{i,k+1}^{c_r}=R({\hat{p}}_{i,k+1}^{c_l}-T)---\left(6\right)$

式中，R，T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量，由标定可知。

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在右摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^r=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^r\\ {\hat{v}}_{i,k+1}^r\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_r}{\hat{p}}_{i,k+1,x}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\\ \frac{f_{c_r}{\hat{p}}_{i,k+1,y}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\end{array}\right]+\left[\begin{array}{c}{u}_{r0}\\ v_{r0}\end{array}\right]+{\tilde{v}}_{i,k+1}^r,$ i＝1,2,…,M              (7)

式中，

为第i个特征点在右摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在右摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽，其值与左摄像机相同，(u_r0，v_r0)为右摄像机光轴与成像平面交点的像素坐标，为右摄像机焦距，为第i个特征点在t_k+1时刻右摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，为右图像中特征点提取误差，为2×1维列向量；

将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 $Z={[{\hat{Z}}_{1,k+1}^l,{\hat{Z}}_{1,k+1}^r,{\hat{Z}}_{2,k+1}^l,{\hat{Z}}_{2,k+1}^r,...,{\hat{Z}}_{M,k+1}^l,{\hat{Z}}_{M,k+1}^r]}^T,$ 即系统量测量，共4M维，设量测噪声 $v={[{\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}]}^T,$ ${\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}$ 分别为 $u_1^l,v_1^l,u_1^r,v_1^r,u_2^l,v_2^l,u_2^r,v_2^r,...,u_M^l,v_M^l,u_M^r{,v}_M^r$ 的观测误差，由于各变量都是与时间t有关的变量，则可建立像素坐标信息量测模型的表达式为：

Z(t)＝h(X(t),t)+v(t)                   (8)

3.对步骤1中式(2)所示的状态模型及步骤2中式(8)所示的量测模型进行离散化：

X(k+1)＝F(X(k),k)+w(k)                    (9)

Z(k)＝H(X(k),k)+v(k)                     (10)

式中，k＝1,2,…，F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数，H(X(k),k)为h(X(t),t)离散后的非线性量测函数，w(k)、v(k)分别为离散后的系统噪声和量测噪声，它们互不相关；

4.Unscented卡尔曼滤波；

利用Unscented卡尔曼滤波算法，结合步骤1和步骤2所述的状态模型和量测模型进行滤波，利用双目视觉相机拍摄立体图像序列，再进行图像特征提取获得特征点的像素坐标信息，通过量测量与量测模型得到的量测预测值相减得到系统量测残差，用系统这一残差校正状态一步预测的误差；由步骤1可知，状态向量的维数n′为16，因此可得2n′+1即33个Unscented采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一时刻滤波得到的迭代状态值之间的协方差阵，以消除状态模型中模型误差的影响，最终估计出行星固连坐标系下的导航信息；

5.将步骤4中获得的状态量的估计值和状态估计方差用于下一时刻的滤波，并输出状态估计值和状态估计方差，同时，将步骤4中获得的行星固连坐标系中的位置、速度和姿态信息通过矩阵变换转换到以东北天坐标系表示的导航坐标系中并输出。

本发明的原理是：首先建立深空探测巡视器惯性/视觉组合导航模型，利用行星固连坐标系下的捷联惯导解算方程建立其状态模型，提高了计算效率，然后通过对双目视觉系统拍摄的立体图像序列进行图像处理获得的特征点像素坐标信息，建立以图像特征点像素坐标为量测量的量测模型，在一定程度上避免了将视觉导航中的累积误差引入到组合导航系统中，从而提高了最终的导航精度，同时也避免了视觉导航中繁杂的非线性位姿方程求解，有利于提高导航系统的实时性和鲁棒性。由于受到惯性测量仪器精度和图像处理算法精度的制约，系统状态模型和量测模型都存在模型误差，且组合导航系统的状态方程和量测方程均为非线性，因此在组合导航滤波中使用Unscented卡尔曼滤波方法，用量测量与量测模型之间的残差校正状态更新的误差，并解决系统的非线性问题，实现对位置、速度、姿态等导航参数的估计。

本发明与现有技术相比的优点在于：(1)将惯导系统和视觉系统构成组合导航系统，并直接利用特征点的像素坐标作为量测量，避免后续的纯视觉导航运动估计算法引入附加的量测信息误差，提高了量测信息的精度，充分融合了惯性信息和视觉信息，从而提高了导航系统的精度；(2)选取行星固连坐标系为全局坐标系，建立状态方程，简化了量测方程的推导，并在此基础上建立了准确的量测方程，提高了计算效率；(3)使用姿态四元数来描述姿态，避免了欧拉角表示的奇点问题，也提高了姿态解算的精度，加快了计算速度。

附图说明

图1为本发明深空探测巡视器惯性/视觉组合导航方法的流程图；

图2为本发明中双目立体视觉系统成像原理图。

具体实施方式

本实施例所述的巡视器惯性/视觉组合导航方法在状态模型和量测模型中均涉及到许多不同的坐标系及相互之间的转换，坐标系定义如下：

①惯性坐标系（o_px_iy_iz_i）：其原点在行星质心，z_i轴垂直于赤道平面指向北极，x_i轴在赤道平面内指向春分点方向，y_i轴与x_i、z_i轴构成右手正交坐标系。

②行星固连坐标系（o_px_py_pz_p）：其原点和z_p轴指向与惯性坐标系相同，x_p轴在赤道平面内指向本初子午线，y_p轴与x_p、z_p轴构成右手正交坐标系。

③行星大地坐标系（L,λ，h）：坐标依次为纬度、经度和高度。纬度L为过巡视器的行星球面法线与赤道面的夹角，经度λ为巡视器所在的子午面与0°子午面之间的夹角，高度h为巡视器沿球面法线到球面的距离。

④导航坐标系（ox_ny_nz_n）：选取东北天坐标系为导航坐标系，即地理坐标系，其原点位于巡视器质心，x_n轴指向东，y_n轴指向北，z_n轴指向天。

⑤巡视器本体坐标系（ox_by_bz_b）：与巡视器刚性固连，其原点位于巡视器质心，x_b轴沿巡视器横轴指右，y_b轴沿巡视器纵轴指前，z_b轴沿巡视器竖轴并与x_b、y_b轴构成右手直角坐标系。

⑥IMU坐标系：与巡视器本体坐标系重合。

⑦摄像机坐标系（o_cx_cy_cz_c）：分为左摄像机坐标系（C_lx_ly_lz_l）和右摄像机坐标系（C_rx_ry_rz_r），其相应于左、右摄像机的定义均为，原点为摄像机光心，z_c轴沿光轴指向景物方向，x_c轴与像素的行平行并指向右，y_c轴与像素的列平行并指向下，构成右手直角坐标系。

⑧图像物理坐标系（o₁xy）：分为左摄像机图像物理坐标系（O_lX_lY_l）和右摄像机图像物理坐标系（O_rX_rY_r），其分别定义在左、右摄像机的成像平面内，原点为相应摄像机光轴与图像平面的交点，x轴和y轴分别与相应摄像机坐标系的x_c轴和y_c轴平行且方向相同，坐标具有长度单位。

⑨图像像素坐标系（o₀uv）：分为左摄像机图像像素坐标系（o_lu_lv_l）和右摄像机图像像素坐标系（o_ru_rv_r），其定义均为，坐标原点位于图像像素阵列的左上角，u轴和v轴分别平行于摄像机坐标系的x_c轴和y_c轴且方向相同，像素坐标是像点在像素阵列中的行数和列数。

如图1所示，本发明的具体实施方法如下：

1.建立深空探测巡视器惯性/视觉组合导航系统状态模型

设状态向量 $X\left(t\right)=[{\left(q_p^b\right)}^T,{\left(p_b^p\right)}^T,{\left(v_b^p\right)}^T,{b_g}^T,{b_a}^T{]}^T,$ 其中，

为巡视器相对行星固连坐标系的姿态四元数，

和

为巡视器在行星固连坐标系中的三轴位置和速度，b_g和b_a为巡视器本体坐标系下的陀螺常值漂移和加速度计常值零偏，首先初始化巡视器的位置、速度和姿态，根据巡视器的运行特点和行驶环境，选取巡视器在导航坐标系下的位置、速度、姿态初值以及陀螺漂移和加计零偏的滤波初值为：

将位置、速度和姿态初值转换到行星固连坐标系中作为滤波初值，之后建立巡视器惯性/视觉组合导航的状态模型：选取行星固连坐标系为全局参考坐标系，根据捷联惯导在行星固连坐标系下的导航解算方程，可得组合导航系统的状态模型为：

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_p^b=\frac{1}{2}\mathrm{\Ω}({\mathrm{\ω}}_{\mathrm{ib}}^b-b_g-C_p^b{\mathrm{\ω}}_{\mathrm{ip}}^p)q_p^b+w_g\\ {\stackrel{\·}{p}}_b^p=v_b^p\\ {\stackrel{\·}{v}}_b^p=C_b^p(f_b-b_a)-2{\mathrm{\ω}}_{\mathrm{ip}}^p\×v_b^p+g^p+w_a\\ {\stackrel{\·}{b}}_g=0\\ {\stackrel{\·}{b}}_a=0\end{array}\right.---\left(11\right)$

式中，

为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数，为4×1维的向量，

和

分别为巡视器在行星固连坐标系下的位置和速度，均为3×1维的向量，b_g和b_a分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置，均为3×1维的向量。

为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示，由陀螺仪直接测得，f_b为比力信息，由加速度计直接测得，

为行星自转角速率在行星固连坐标系下的矢量表示，为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵，g^p为重力矢量在行星固连坐标系下的表示，Ω(·)为四元数矩阵，w_g和w_a分别为陀螺仪和加速度计的随机误差，均为零均值的高斯白噪声；

式(1)中变量都是与t有关的函数，可简写为：

$\stackrel{\·}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(12\right)$

状态变量为

f(X(t),t)为系统非线性连续状态转移函数，状态噪声为w(t)＝[w_g,w_a]^T。

2.建立基于特征点像素坐标为量测量的量测模型

图2描述了平行双目立体视觉系统的三维模型，由于物体在像平面上一般成倒立的像，为了得到正立的像和方便计算，将摄像机的实际成像平面根据直线C_lC_r进行镜像投影为虚拟的成像平面，L和R分别表示左摄像机成像平面和右摄像机成像平面。O_lX_lY_l和O_rX_rY_r分别为左、右摄像机图像物理坐标系，o_lu_lv_l和o_ru_rv_r分别为左、右摄像机图像像素坐标系，C_l和C_r分别为左、右摄像机镜头透镜的光心，C_lx_ly_lz_l和C_rx_ry_rz_r分别为左摄像机坐标系和右摄像机坐标系，B为基线长度。对获取的立体图像进行SIFT特征提取和立体匹配可以获得同一空间点D_i在左、右摄像机成像平面中像点的像素坐标，根据图像像素坐标与物理坐标之间的转换关系，进而得到特征点在左、右摄像机成像平面中的图像物理坐标，分别为P_l和P_r，图2中，(x_l′，y_l′)为P_l的坐标，然后根据图2所示的平行双目立体视觉系统成像的几何关系进行三维重建可得空间点D_i在左摄像机坐标系下的三维坐标

t_k时刻和t_k+1时刻之间摄像机坐标系的旋转矩阵和平移矢量为

其分别为3×3维的矩阵和3×1维的向量，计算公式为：

$R_k^{k+1}=C_b^c{C}_{p,k+1}^b{\left(C_b^c{C}_{p,k}^b\right)}^T$                                               (13)

$T_k^{k+1}=-C_b^c{C}_{p,k+1}^b(p_{b,k+1}^p+{C_{p,k+1}^b}^T{p}_c^b)+C_b^c{C}_{p,k+1}^b(p_{b,k}^p+{C_{p,k}^b}^T{p}_c^b)$

式中，k表示时间序号，k＝1,2,…，

为摄像机相对巡视器本体坐标系的安装矩阵，

为摄像机在巡视器本体坐标系中的位置，为3×1维的向量，由标定可知，

和分别为t_k时刻和t_k+1时刻巡视器在行星固连坐标系中的位置，均为3×1维的向量，

和

分别为t_k时刻和t_k+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵，均为3×3维，

和

可根据t_k时刻组合导航的滤波结果获得，和可由t_k+1时刻惯导的状态更新结果获得；

跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为：

${\hat{p}}_{i,k+1}^{c_l}=R_k^{k+1}{p}_{i,k}^{c_l}+T_k^{k+1}---\left(14\right)$

式中，k＝1,2,…，

为第i个特征点在t_k时刻左摄像机坐标系下的三维坐标，根据三角测量原理和双目视觉系统参数可以获得，为第i个特征点在t_k+1时刻左摄像机坐标系下三维坐标的估计值，

分别为从t_k时刻到t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量，由式(13)获得；

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在左摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^l=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^l\\ {\hat{v}}_{i,k+1}^l\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_l}{\hat{p}}_{i,k+1,x}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\\ \frac{f_{c_l}{\hat{p}}_{i,k+1,y}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\end{array}\right]+\left[\begin{array}{c}{u}_{l0}\\ v_{l0}\end{array}\right]+{\tilde{v}}_{i,k+1}^l,$ i＝1,2,…,M            (15)

式中，

为第i个特征点在左摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在左摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽，(u_l0，v_l0)为左摄像机光轴与成像平面交点的像素坐标，

为左摄像机焦距，

为第i个特征点在t_k+1时刻左摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，

为左图像中特征点提取误差，为2×1维列向量；

根据平行双目立体视觉系统的外部参数，可以得到t_k+1时刻第i个特征点在右摄像机坐标系下三维坐标的估计值

表达式为：

${\hat{p}}_{i,k+1}^{c_r}=R({\hat{p}}_{i,k+1}^{c_l}-T)---\left(16\right)$

式中，R,T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量，由标定可知。

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在右摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^r=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^r\\ {\hat{v}}_{i,k+1}^r\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_r}{\hat{p}}_{i,k+1,x}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\\ \frac{f_{c_r}{\hat{p}}_{i,k+1,y}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\end{array}\right]+\left[\begin{array}{c}{u}_{r0}\\ v_{r0}\end{array}\right]+{\tilde{v}}_{i,k+1}^r,$ i＝1,2,…,M              (17)

式中，

为第i个特征点在右摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在右摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽，其值与左摄像机相同，(u_r0，v_r0)为右摄像机光轴与成像平面交点的像素坐标，

为右摄像机焦距，为第i个特征点在t_k+1时刻右摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，

为右图像中特征点提取误差，为2×1维列向量；

将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 $Z={[{\hat{Z}}_{1,k+1}^l,{\hat{Z}}_{1,k+1}^r,{\hat{Z}}_{2,k+1}^l,{\hat{Z}}_{2,k+1}^r,...,{\hat{Z}}_{M,k+1}^l,{\hat{Z}}_{M,k+1}^r]}^T,$ 即系统量测量，共4M维，设量测噪声 $v={[{\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}]}^T,$ ${\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}$ 分别为 $u_1^l,v_1^l,u_1^r,v_1^r,u_2^l,v_2^l,u_2^r,v_2^r,...,u_M^l,v_M^l,u_M^r{,v}_M^r$ 的观测误差，由于各变量都是与时间t有关的变量，则可建立像素坐标信息量测模型的表达式为：

Z(t)＝h(X(t),t)+v(t)                   (18)

3.对步骤1中式(12)所示的状态模型及步骤2中式(18)所示的量测模型进行离散化：

X(k+1)＝F(X(k),k)+w(k)                  (19)

Z(k)＝H(X(k),k)+v(k)                       (20)

式中，k＝1,2,…，F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数，H(X(k),k)为h(X(t),t)离散后的非线性量测函数，w(k)、v(k)分别为离散后的系统噪声和量测噪声，它们互不相关；

4.Unscented卡尔曼滤波

利用Unscented卡尔曼滤波算法，结合步骤1和步骤2所述的状态模型和量测模型进行滤波，利用双目视觉相机拍摄立体图像序列，再进行图像特征提取获得特征点的像素坐标信息，通过量测量与量测模型得到的量测预测值相减得到系统量测残差，用系统这一残差校正状态一步预测的误差；由步骤1可知，状态向量的维数n′为16，因此可得2n′+1即33个Unscented采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一时刻滤波得到的迭代状态值之间的协方差阵，以消除状态模型中模型误差的影响，最终输出状态量的滤波估计结果，具体步骤为：

①初始化状态量

和状态误差方差阵P₀

${\hat{x}}_0=E\left[x_0\right],$ $P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]---\left(21\right)$

②计算状态量采样点

在附近选取一系列采样点，这些采样点的均值和协方差分别为

和P_k-1|k-1，这里

且P_k-1|k-1＝P_k-1。状态变量为16×1维，那么33个样本点χ_0，k，χ_1，k，...,χ_32,k及其权重W₀,W₁…,W₃₂分别如下：

${\mathrm{\χ}}_{0,k}={\hat{x}}_k,$ W₀＝-1

${\mathrm{\χ}}_{j,k}={\hat{x}}_k+\sqrt{n^{\′}+\mathrm{\τ}}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ $W_j=\frac{1}{n^{\′}}---\left(22\right)$

${\mathrm{\χ}}_{j+16,k}={\hat{x}}_k-\sqrt{n^{\′}+\mathrm{\τ}}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ $W_{j+16}=\frac{1}{n^{\′}}$

式中,n′为状态向量的维数，

当P(k|k)＝A^TA时，

取A的第j行，当P(k|k)＝AA^T时，

取A的第j列，j＝1,2,…,n′，则所有采样点的表达式为：

${\mathrm{\χ}}_{k-1}=\left[\begin{array}{ccc}{\hat{x}}_{k-1}& {\hat{x}}_{k-1}+\sqrt{n^{\′}+\mathrm{\τ}}{\left(\sqrt{P_{k-1}}\right)}_j& {\hat{x}}_{k-1}-\sqrt{n^{\′}+\mathrm{\τ}}{\left(\sqrt{P_{k-1}}\right)}_j\end{array}\right]---\left(23\right)$

③时间更新

状态量的一步预测χ_k+1|k为：

χ_k|k-1＝f(χ_k-1，k-1)              (24)

所有采样点状态量的一步预测加权后结果

为：

${\hat{x}}_k^{-}=\underset{j=0}{\overset{32}{\mathrm{\Σ}}}{W}_j{\mathrm{\χ}}_{j,k|k-1}---\left(25\right)$

式中，W_j为第j个采样点的权值；

状态量的估计方差一步预测

为：

$P_k^{-}=\underset{j=0}{\overset{32}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]}^T+Q_k---\left(26\right)$

式中，Q_k为系统噪声协方差；

采样点对应的量测估计值Z_k|k＝1

Z_k|k-1＝h(χ_k|k-1,k)                (27)

采样点量测估计加权值

${\hat{z}}_k^{-}=\underset{j=0}{\overset{32}{\mathrm{\Σ}}}{W}_j{Z}_{j,k|k-1}---\left(28\right)$

④量测更新

量测方差阵

为：

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{j=0}{\overset{32}{\mathrm{\Σ}}}{W}_j[Z_{j,k|k-1}-{\hat{z}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T+R_k---\left(29\right)$

式中，R_k为量测噪声协方差；

状态变量量测量方差阵

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{j=0}{\overset{32}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T---\left(30\right)$

滤波增益K_k为：

$K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}---\left(31\right)$

状态量的估计值

和估计方差P_k为：

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-{\hat{z}}_k^{-})---\left(32\right)$

$P_k=P_k^{-}-K_k{P}_{{\hat{Z}}_k{\hat{Z}}_k}{K}_k^T---\left(33\right)$

将获得的状态量的估计值

和估计方差P_k返回滤波器，用于k+1时刻，k＝1,2,...，最终将状态估计值

和状态估计方差P_k输出，其中状态估计值

包括探测器的速度、位置、姿态四元数、陀螺漂移和加速度计零偏信息，输出的状态估计方差P_k表示了滤波估计的性能。

5.求解导航坐标系中的位置、速度和姿态矩阵

选取东北天坐标系为导航坐标系，由获得的状态变量

和

求解巡视器在导航系中的位置、速度和姿态矩阵。

由巡视器在行星固连坐标系中的直角坐标

向大地坐标(λ，L,h)转换的关系式为：

$\left\{\begin{array}{c}\mathrm{\λ}=\arctan \left(\frac{y}{x}\right)\\ L=\arctan \left[\tan B^{\′}(1+\frac{{\mathrm{ae}}^2}{z}\frac{\sin L}{\mathrm{\ω}})\right]\\ h=\frac{R\cos B^{\′}}{\cos L}-n\end{array}\right.---\left(34\right)$

其中涉及的中间变量的表达式如下：

$\left\{\begin{array}{c}e=\frac{\sqrt{a^2-b^2}}{a}\\ n=\frac{a}{\mathrm{\ω}}\\ \mathrm{\ω}={(1-e^2{\sin }^{2}L)}^{\frac{1}{2}}\\ R={(x^2+y^2+z^2)}^{\frac{1}{2}}\\ B^{\′}=\arctan \left(\frac{z}{\sqrt{x^2+y^2}}\right)\end{array}\right.---\left(35\right)$

式中，a,b,e分别为大地坐标系对应椭圆的半长轴、半短轴和第一偏心率；n为椭球的卯酉圈曲率半径，L,λ，h分别为巡视器所在位置的纬度、经度和高度。

根据上述公式进行迭代即可获得巡视器的大地坐标(λ，L,h)。

地理坐标系相对行星固连坐标系的旋转矩阵为：

巡视器在导航坐标系下的速度为：

$v_b^n=R_p^n{v}_b^p---\left(37\right)$

设

利用四元数和方向余弦矩阵之间的关系，有：

$R_p^b=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(38\right)$

则姿态矩阵为：

$R_n^b=R_p^b\·{\left(R_p^n\right)}^{-1}---\left(39\right)$

根据姿态矩阵

即可求出巡视器相对导航坐标系的三个姿态角。

最后，输出巡视器在导航坐标系中的位置、速度和姿态信息。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种深空探测巡视器的惯性/视觉组合导航方法，其特征在于：首先根据行星固连坐标系下的捷联惯导力学编排方程，建立惯性/视觉组合导航的状态模型，再利用双目视觉相机对周围环境进行拍摄，获得立体图像序列，通过图像特征提取算法得到特征点的像素坐标，建立以特征点像素坐标为量测量的量测模型，并使用Unscented卡尔曼滤波估计深空探测巡视器的位置、速度和姿态；具体包括以下步骤：

①建立深空探测巡视器惯性/视觉组合导航系统状态模型；

选取行星固连坐标系为全局参考坐标系，可得组合导航的状态模型：

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_p^b=\frac{1}{2}\mathrm{\Ω}({\mathrm{\ω}}_{\mathrm{ib}}^b-b_g-C_p^b{\mathrm{\ω}}_{\mathrm{ip}}^p)q_p^b+w_g\\ {\stackrel{\·}{p}}_b^p=v_b^p\\ {\stackrel{\·}{v}}_b^p=C_b^p(f_b-b_a)-2{\mathrm{\ω}}_{\mathrm{ip}}^p\×v_b^p+g^p+w_a\\ {\stackrel{\·}{b}}_g=0\\ {\stackrel{\·}{b}}_a=0\end{array}\right.---\left(1\right)$

式中，

为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数，为4×1维的向量，

和

分别为巡视器在行星固连坐标系下的位置和速度，均为3×1维的向量，b_g和b_a分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置，均为3×1维的向量，

为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示，由陀螺仪直接测得，f_b为比力信息，由加速度计直接测得，为行星自转角速率在行星固连坐标系下的矢量表示，

为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵，g^p为重力矢量在行星固连坐标系下的表示，Ω(·)为四元数矩阵，w_g和w_a分别为陀螺仪和加速度计的随机误差，均为零均值的高斯白噪声；

式(1)中变量都是与t有关的函数，可简写为：

$\stackrel{\·}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(2\right)$

状态变量为

f(X(t),t)为系统非线性连续状态转移函数，状态噪声为w(t)＝[w_g,w_a]^T；

②建立基于特征点像素坐标为量测量的量测模型；

巡视器本体坐标系与IMU坐标系重合，摄像机坐标系和IMU坐标系之间的转换关系已知，t_k时刻和t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量为

其中

为3×3维的矩阵，

为3×1维的向量，计算公式为：

$R_k^{k+1}=C_b^c{C}_{p,k+1}^b{\left(C_b^c{C}_{p,k}^b\right)}^T$                                            (3)

$T_k^{k+1}=-C_b^c{C}_{p,k+1}^b(p_{b,k+1}^p+{C_{p,k+1}^b}^T{p}_c^b)+C_b^c{C}_{p,k+1}^b(p_{b,k}^p+{C_{p,k}^b}^T{p}_c^b)$

式中，k表示时间序号，k＝1,2,…，

为摄像机相对巡视器本体坐标系的安装矩阵，

为摄像机在巡视器本体坐标系中的位置，为3×1维的向量，由标定可知，

和

分别为t_k时刻和t_k+1时刻巡视器在行星固连坐标系中的位置，均为3×1维的向量，

和分别为t_k时刻和t_k+1时刻巡视器本体坐标系相对行星固连坐标系的姿态矩阵，均为3×3维，

和

根据t_k时刻组合导航的滤波结果获得，

和

由t_k+1时刻惯导的状态更新结果获得；

跟踪得到的第i个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为：

${\hat{p}}_{i,k+1}^{c_l}=R_k^{k+1}{p}_{i,k}^{c_l}+T_k^{k+1}---\left(4\right)$

式中，k＝1,2,…，

为第i个特征点在t_k时刻左摄像机坐标系下的三维坐标，

为第i个特征点在t_k+1时刻左摄像机坐标系下三维坐标的估计值，

分别为从t_k时刻到t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量，由式(3)获得；

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在左摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^l=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^l\\ {\hat{v}}_{i,k+1}^l\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_l}{\hat{p}}_{i,k+1,x}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\\ \frac{f_{c_l}{\hat{p}}_{i,k+1,y}^{c_l}}{{\hat{p}}_{i,k+1,z}^{c_l}}\end{array}\right]+\left[\begin{array}{c}{u}_{l0}\\ v_{l0}\end{array}\right]+{\tilde{v}}_{i,k+1}^l,i=\mathrm{1,2},...,M---\left(5\right)$

式中，

为第i个特征点在左摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在左摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在左摄像机成像平面横轴和纵轴方向的长和宽，(u_l0,v_l0)为左摄像机光轴与成像平面交点的像素坐标，

为左摄像机焦距，为第i个特征点在t_k+1时刻左摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，

为左图像中特征点提取误差，为2×1维列向量；

根据平行双目立体视觉系统的外部参数，可以得到t_k+1时刻第i个特征点在右摄像机坐标系下三维坐标的估计值

表达式为：

${\hat{p}}_{i,k+1}^{c_r}=R({\hat{p}}_{i,k+1}^{c_l}-T)---\left(6\right)$

式中，R,T分别为右摄像机坐标系相对于左摄像机坐标系的旋转矩阵和平移矢量；

根据上述求出的

和摄像机成像的针孔模型，可以求出t_k+1时刻特征点在右摄像机成像平面内的像素坐标预测值，对所有M个特征点的表达式为：

${\hat{Z}}_{i,k+1}^r=\left[\begin{array}{c}{\hat{u}}_{i,k+1}^r\\ {\hat{v}}_{i,k+1}^r\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{dX}}& 0\\ 0& \frac{1}{\mathrm{dY}}\end{array}\right]\left[\begin{array}{c}\frac{f_{c_r}{\hat{p}}_{i,k+1,x}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\\ \frac{f_{c_r}{\hat{p}}_{i,k+1,y}^{c_r}}{{\hat{p}}_{i,k+1,z}^{c_r}}\end{array}\right]+\left[\begin{array}{c}{u}_{r0}\\ v_{r0}\end{array}\right]+{\tilde{v}}_{i,k+1}^r,$ i＝1,2,…,M             (7)

式中，为第i个特征点在右摄像机中对应的量测信息，

为第i个特征点t_k+1时刻在右摄像机成像平面中的像素坐标预估值，dX,dY分别为每个像素在右摄像机成像平面横轴和纵轴方向的长和宽，其值与左摄像机相同，(u_r0，v_r0)为右摄像机光轴与成像平面交点的像素坐标，

为右摄像机焦距，

为第i个特征点在t_k+1时刻右摄像机坐标系下的三维坐标预估值，M为t_k时刻到t_k+1时刻之间跟踪到的特征点个数，为右图像中特征点提取误差，为2×1维列向量；

将全部特征点在左、右摄像机成像平面中的像素坐标构成一个列向量 $Z={[{\hat{Z}}_{1,k+1}^l,{\hat{Z}}_{1,k+1}^r,{\hat{Z}}_{2,k+1}^l,{\hat{Z}}_{2,k+1}^r,...,{\hat{Z}}_{M,k+1}^l,{\hat{Z}}_{M,k+1}^r]}^T,$ 即系统量测量，共4M维，设量测噪声 $v={[{\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}]}^T,$ ${\tilde{v}}_1,{\tilde{v}}_2,{\tilde{v}}_3,...,{\tilde{v}}_{4M}$ 分别为 $u_1^l,v_1^l,u_1^r,v_1^r,u_2^l,v_2^l,u_2^r,v_2^r,...,u_M^l,v_M^l,u_M^r{,v}_M^r$ 的观测误差，由于各变量都是与时间t有关的变量，则可建立基于像素坐标量测信息的量测模型表达式为：

Z(t)＝h(X(t),t)+v(t)                    (8)

③对步骤①中式(2)所示的状态模型及步骤②中式(8)所示的量测模型进行离散化：

X(k+1)＝F(X(k),k)+w(k)                   (9)

Z(k)＝H(X(k),k)+v(k)                        (10)

式中，k＝1,2,…，F(X(k),k)为f(X(t),t)离散后的非线性状态转移函数，H(X(k),k)为h(X(t),t)离散后的非线性量测函数，w(k)、v(k)分别为离散后的系统噪声和量测噪声，它们互不相关；

④利用Unscented卡尔曼滤波算法，结合步骤①和步骤②所述的状态模型和量测模型进行滤波，利用双目视觉相机拍摄立体图像序列，再进行特征提取获得特征点的像素坐标信息，通过量测量与量测模型得到的量测预测值相减得到系统量测残差，用系统这一残差校正状态一步预测的误差；根据状态向量可得相应的Unscented采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一时刻滤波得到的迭代状态值之间的协方差阵，以消除状态模型中模型误差的影响；

⑤输出导航信息；

将步骤④中获得的状态量的估计值和状态估计方差用于下一时刻的滤波，并输出状态估计值和状态估计方差，同时，将步骤④中获得的行星固连坐标系中的位置、速度和姿态信息通过矩阵变换转换到以东北天坐标系表示的导航坐标系中并输出。

Images