[发明专利]一种深空探测巡视器惯性/视觉组合导航方法

权利要求书

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

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

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

q·pb=12Ω(ωibb-bg-Cpbωipp)qpb+wgp·bp=vbpv·bp=Cbp(fb-ba)-2ωipp×vbp+gp+wab·g=0b·a=0---(1)]]>

式中，为从行星固连坐标系到巡视器本体坐标系旋转的单位四元数，为4×1维的向量，和分别为巡视器在行星固连坐标系下的位置和速度，均为3×1维的向量，b_g和b_a分别为巡视器本体坐标系下陀螺仪和加速度计的常值偏置，均为3×1维的向量，为巡视器本体坐标系相对惯性坐标系的旋转角速率在巡视器本体坐标系中的表示，由陀螺仪直接测得，f_b为比力信息，由加速度计直接测得，为行星自转角速率在行星固连坐标系下的矢量表示，为巡视器本体坐标系相对行星固连坐标系的方向余弦矩阵，g^p为重力矢量在行星固连坐标系下的表示，Ω(·)为四元数矩阵，w_g和w_a分别为陀螺仪和加速度计的随机误差，均为零均值的高斯白噪声；

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

X·(t)=f(X(t),t)+w(t)---(2)]]>

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

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

巡视器本体坐标系与IMU坐标系重合，摄像机坐标系和IMU坐标系之间的转换关系已知，t_k时刻和t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量为其中为3×3维的矩阵，为3×1维的向量，计算公式为：

Rkk+1=CbcCp,k+1b(CbcCp,kb)T]]>                                           (3)

Tkk+1=-CbcCp,k+1b(pb,k+1p+Cp,k+1bTpcb)+CbcCp,k+1b(pb,kp+Cp,kbTpcb)]]>

式中，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个特征点在相邻时刻摄像机坐标系下的三维坐标关系式为：

p^i,k+1cl=Rkk+1pi,kcl+Tkk+1---(4)]]>

式中，k＝1,2,…，为第i个特征点在t_k时刻左摄像机坐标系下的三维坐标，为第i个特征点在t_k+1时刻左摄像机坐标系下三维坐标的估计值，分别为从t_k时刻到t_k+1时刻摄像机坐标系之间的旋转矩阵和平移矢量，由式(3)获得；

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

Z^i,k+1l=u^i,k+1lv^i,k+1l=1dX001dYfclp^i,k+1,xclp^i,k+1,zclfclp^i,k+1,yclp^i,k+1,zcl+ul0vl0+v~i,k+1l,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个特征点在右摄像机坐标系下三维坐标的估计值表达式为：

p^i,k+1cr=R(p^i,k+1cl-T)---(6)]]>

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

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

Z^i,k+1r=u^i,k+1rv^i,k+1r=1dX001dYfcrp^i,k+1,xcrp^i,k+1,zcrfcrp^i,k+1,ycrp^i,k+1,zcr+ur0vr0+v~i,k+1r,]]>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=[Z^1,k+1l,Z^1,k+1r,Z^2,k+1l,Z^2,k+1r,...,Z^M,k+1l,Z^M,k+1r]T,]]>即系统量测量，共4M维，设量测噪声v=[v~1,v~2,v~3,...,v~4M]T,]]>v~1,v~2,v~3,...,v~4M]]>分别为u1l,v1l,u1r,v1r,u2l,v2l,u2r,v2r,...,uMl,vMl,uMr,vMr]]>的观测误差，由于各变量都是与时间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采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一时刻滤波得到的迭代状态值之间的协方差阵，以消除状态模型中模型误差的影响；

⑤输出导航信息；

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