CN103438890B - 基于tds与图像测量的行星动力下降段导航方法 - Google Patents

Abstract

本发明涉及一种基于TDS与图像测量的行星动力下降段导航方法，属于深空探测技术领域。在动力下降段，着陆器导航系统的惯性单元用于航位递推，是基础敏感器；利用TDS上的多普勒雷达，直接获得着陆器距目标行星表面三个波束方向的距离和相对速度，然后解算出着陆器的高度以及在着陆点固连坐标系三轴方向上的速度；导航相机作为视觉辅助导航敏感器，精确测量着陆区域不同特征点视线方向间的夹角，特征点位置相对着陆点已知，视线夹角包含着陆器相对着陆点的水平位置信息，大大提高了着陆器的水平位置精度。<pb pnum="1" />

Description

基于TDS与图像测量的行星动力下降段导航方法

技术领域

本发明涉及一种基于TDS与图像测量的行星动力下降段导航方法，属于深空探测技术领域。

背景技术

未来火星探测需要着陆器实现定点软着陆。动力下降段是着陆器的主要受控阶段，通过一定的制导算法可以控制着陆器飞往预定目标点，实现精确点着陆，而这必须以导航系统提供的精确位置和速度信息为前提。因此，构建可以精确确定着陆器位置和速度的导航方案是定点着陆任务成功的基础。

以往的火星着陆任务在动力下降段均采用雷达高度计测量着陆器的高度信息，对速度的信息的测量则不尽相同。火星探测漫游者(MER)利用下降图像估计系统(DIMES)对水平速度进行估计，“凤凰号”任务、火星探路者任务以及更早的“海盗号”任务均采用了多普勒雷达对水平速度进行估计。火星科学实验室(MSL)搭载了下降敏感器(TDS)，由不同波束方向的多普勒雷达集合而成，可以确定出着陆器的高度和速度。然而，目前的导航敏感器均无法测得着陆器相对目标点的水平位置，以致于着陆器在动力下降段有较大的水平位置误差，制导系统也就无法控制着陆器实现定点着陆。

导航相机是目前自主导航系统中应用较广的敏感器，可以获得着陆器相对目标点的位置和速度信息。但是图像识别与处理过程繁琐，耗时多，对星载计算机要求高，因此难以单独使用。

发明内容

本发明的目的是为获得动力下降段着陆器准确的位置和速度信息，结合行星动力下降段的导航问题，结合下降敏感器、导航相机、惯性单元，提出一种基于TDS和图像测量的导航方案，用以确定着陆器的高度、速度及相对目标点的位置信息，为未来火星精确软着陆  任务导航方案设计提供技术支持和参考。

在动力下降段，着陆器导航系统的惯性单元用于航位递推，是基础敏感器；所述TDS上有三波束多普勒雷达，直接获得着陆器距目标行星表面三个波束方向的距离和相对速度，然后解算出着陆器的高度以及在着陆点固连坐标系三轴方向上的速度；所述导航相机安装在着陆器上，作为视觉辅助导航敏感器，精确测量着陆区域不同特征点视线方向间的夹角，特征点位置相对着陆点已知，视线夹角包含着陆器相对着陆点的水平位置信息，大大提高了着陆器的水平位置精度。

本发明的技术方案具体包括如下步骤：

步骤1：建立行星动力下降段的着陆器状态模型。

在着陆点固连坐标系下，着陆器状态x包括位置矢量r＝[x,y,z]^T、速度矢量v＝[v_x,v_y,v_z]^T、姿态四元数q＝[q₁,q₂,q₃,q₄]、加速度计漂移陀螺常值漂移 动力下降段的着陆器状态模型为：

$\stackrel{\&CenterDot;}{r}=v$

$\stackrel{\&CenterDot;}{v}=T_b^l(a_m-b_a-{\mathrm{\&xi;}}_m)+g$

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\mathrm{\&Omega;q}---\left(1\right)$

${\stackrel{\&CenterDot;}{b}}_a=0$

${\stackrel{\&CenterDot;}{b}}_{\mathrm{\&omega;}}=0$

其中为着陆器本体系到着陆点固连坐标系的坐标转换矩阵，用四元数表示如下：

$T_b^l=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_0{q}_3+q_1{q}_2\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(-q_0{q}_3+q_1{q}_2\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3+q_0{q}_1\right)\\ 2\left(q_0{q}_2+q_1{q}_3\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

Ω为角速度的函数矩阵： 

$\mathrm{\&Omega;}=\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\mathrm{\&Omega;}({\mathrm{\&omega;}}_m-b_{\mathrm{\&omega;}}-{\mathrm{\&xi;}}_{\mathrm{\&omega;}})=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_1\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]---\left(3\right)$

a_m和ω_m分别为加速度计和陀螺的输出值，ξ_m和ξ_ω分别是加速度计和陀螺的测量误差，g为重力加速度。ω_x、ω_y、ω_z为着陆器的旋转角速度沿本体系三个轴的分量。

步骤2：建立行星动力下降段的着陆器自主导航测量模型。

设不同波束方向在着陆点固连坐标系下的单位矢量表示为 n为波束数量(为了提高导航精度，n≥3)，有：

${\left[\begin{array}{cccc}{d}_1& d_2& ...& d_n\end{array}\right]}_{3\&times;n}=T_b^l\&CenterDot;S_{3\&times;n}---\left(4\right)$

其中S_3×n为n个波束方向在本体系下的单位矢量矩阵。设着陆器沿TDS的不同波束方向距行星表面的距离为r_j(j＝1,2,…n)，则有：

r_j＝z/(d_j·[0 0 1]^T)  (5)

设着陆器沿不同波束方向的速度为v_j(j＝1,2,…n)，则有：

$\left[\begin{array}{c}{v}_1\\ v_2\\ .\\ .\\ .\\ v_n\end{array}\right]=S_{n\&times;3}\&CenterDot;T_l^b{\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]}^L---\left(6\right)$

S_n×3和分别为S_3×n和的转置。

导航相机对着陆区域拍照，提取特征点，得到特征点的像素坐标。若直接以像素坐标作为观测量，则位置信息和姿态信息严重耦合，对状态进行估计时会导致算法复杂繁琐。因此，本发明将导航相机对特征点的观测视线之间的夹角作为观测量。在相机视野范围内找到m个特征点，在着陆点固连系下的位置矢量为ρ_i(i＝1,2,…,m)。n个波束形成个观测角。第i个特征点观测视线和第j个特征点观测视  线之间的夹角表示为：

$A_{ij}=\frac{\left(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i\right)\left(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j\right)}{\left|\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i\right|\left|\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j\right|}---\left(7\right)$

基于TDS和图像测量的导航测量模型为：

$y=h\left(x\right)+\mathrm{\&upsi;}=\left[\begin{array}{c}R\\ V\\ A\end{array}\right]+\left[\begin{array}{c}{R}_{\mathrm{\&upsi;}}\\ V_{\mathrm{\&upsi;}}\\ A_{\mathrm{\&upsi;}}\end{array}\right]---\left(8\right)$

式中R＝[r₁,…,r_n]，V＝[v₁,…,v_n]，A＝[A₁₂,…,A_1m,A₂₃,…,A_2m,…,A_(m-1)m]，R_υ、V_υ、A_υ分别为测量误差。

步骤3：自主导航滤波解算。

根据步骤1得到的行星动力下降段状态方程及步骤2得到的测量模型y＝h(x)+υ，通过导航滤波算法估计着陆器的状态。由于状态方程和测量方程均为非线性，因而宜采用非线性滤波算法，最终输出着陆器的状态信息。

本发明采用扩展卡尔曼滤波(Extend Kalman Filter,EKF)，无迹卡尔曼滤波(Unscented Kalman Filter,UKF)算法以提高导航滤波精度及收敛速度。

有益效果

(1)融合下降敏感器和导航相机的测量信息，对着陆器完整的位置和速度信息进行估计。

(2)TDS测量数据解算时间短，满足了自主导航实时性的需求，弥补了图像处理慢的缺点。相机导航作为辅助导航，又可以准确估计出着陆器的水平位置信息，两者互补。

附图说明

图1为一种基于TDS与图像测量的行星动力下降段导航方法流程图；

图2为具体实施方式中着陆点固连系下的真实轨迹与滤波轨迹  对比，其中(a)为着陆器距目标点x方向的估计距离和真实距离的对比，(b)为着陆器距目标点y方向的估计距离和真实距离的对比，(c)为着陆器估计高度和真实高度的对比，(d)为着陆器x方向的估计速度和真实速度的对比，(e)为着陆器y方向的估计速度和真实速度的对比，(f)为着陆器z方向的估计速度和真实速度的对比。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实施例对发明内容做进一步说明。

本实例针对火星动力下降段基于TDS与图像测量的导航方案，结合三波束TDS的测距测速信息，三个特征点视线之间的夹角信息，采用扩展卡尔曼滤波器进行滤波解算，实现动力下降段的高精度导航。本实例的具体实施方法如下：

步骤1：火星动力下降段状态模型建立

在着陆点固连坐标系下建立着陆器状态模型。着陆器状态x包括位置矢量r＝[x,y,z]^T、速度矢量v＝[v_x,v_y,v_z]^T、姿态四元数q＝[q₁,q₂,q₃,q₄]、加速度计及陀螺常值漂移 动力下降段的状态方程写成(1)式的形式。

$\stackrel{\&CenterDot;}{r}=v$

$\stackrel{\&CenterDot;}{v}=T_b^l(a_m-b_a-{\mathrm{\&xi;}}_m)+g$

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\mathrm{\&Omega;q}---\left(1\right)$

${\stackrel{\&CenterDot;}{b}}_a=0$

${\stackrel{\&CenterDot;}{b}}_{\mathrm{\&omega;}}=0$

其中为本体系到着陆点固连系的坐标转化矩阵：

$T_b^l=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_0{q}_3+q_1{q}_2\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(-q_0{q}_3+q_1{q}_2\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3+q_0{q}_1\right)\\ 2\left(q_0{q}_2+q_1{q}_3\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(2\right)$

Ω为角速度的函数矩阵，表达式如(3)式所示。

$\mathrm{\&Omega;}=\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\mathrm{\&Omega;}({\mathrm{\&omega;}}_m-b_{\mathrm{\&omega;}}-{\mathrm{\&xi;}}_{\mathrm{\&omega;}})=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_1\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right]---\left(3\right)$

a_m和ω_m分别为加速度计和陀螺的输出值，ξ_m和ξ_ω分别是加速度计和陀螺的测量误差，g为重力加速度。

步骤2：行星动力下降段自主导航测量模型建立

下降敏感器TDS可以测得着陆器沿三个波束方向距火星表面的距离及速度，设三个波束方向在着陆点固连坐标系下的单位矢量表示为d_j(j＝1,2,3)，则有：

${\left[\begin{array}{ccc}{d}_1& d_2& d_3\end{array}\right]}_{3\&times;3}=T_B^L\&CenterDot;S_{3\&times;3}---\left(4\right)$

其中S为三个波束方向在本体系下的单位矢量矩阵。设着陆器沿不同波束方向距火星表面的距离为r_j(j＝1,2,3)，则有：

r_j＝z/(d_j·[0 0 1]^T)  (5)

设着陆器沿三个波束方向的速度为v_j(j＝1,2,3)，则有：

$\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\end{array}\right]=S_{3\&times;3}\&CenterDot;T_L^B{\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]}^L---\left(6\right)$

导航相机对着陆区域拍照，提取特征点，可得到特征点的像素坐标。若直接以像素坐标作为观测量，则位置信息和姿态信息严重耦合，对状态进行估计时会导致算法复杂繁琐。因此，此处将特征点观测视线之间的夹角作为观测量。假定在相机视野范围内可以找到三个特征点，在着陆点固连系下的位置矢量为ρ_i(i＝1,2,3)。如此，三条视线可以形成三个观测角。第i个特征点视线和第j个特征点视线之间的夹角表示为：

$A_{ij}=\frac{\left(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i\right)\left(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j\right)}{\left|\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i\right|\left|\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j\right|}---\left(7\right)$

基于TDS和图像测量的导航方案测量模型为：

$y=h\left(x\right)+\mathrm{\&upsi;}=\left[\begin{array}{c}R\\ V\\ A\end{array}\right]+\left[\begin{array}{c}{R}_{\mathrm{\&upsi;}}\\ V_{\mathrm{\&upsi;}}\\ A_{\mathrm{\&upsi;}}\end{array}\right]---\left(8\right)$

式中R＝[r₁,r₂,r₃]，V＝[v₁,v₂,v₃]，A＝[A₁₂,A₁₃,A₂₃]，R_υ、V_υ、A_υ分别为测量误差。

步骤3：自主导航滤波解算

根据行星动力下降段状态方程及测量模型y＝h(x)+υ，通过导航滤波算法可以估计着陆器的状态。由于状态方程和测量方程均为非线性，因而采用扩展卡尔曼滤波(Extend Kalman Filter,EKF)提高导航滤波精度及收敛速度，最终输出着陆器的状态信息。仿真参数设置如表1所示。

表1仿真参数设置

导航精度如表2所示

表2基于TDS与图像测量的导航精度

从图2和表1可以看出，采用基于TDS与图像测量的视觉辅助导航，着陆器在三个方向上的位置和速度的估计值均能快速地趋近真实值，说明该导航方案可以准确地估计出着陆器完整的位置和速度信息。 

Claims (2)

1.基于TDS与图像测量的行星动力下降段导航方法，其特征在于：具体包括如下步骤：

步骤1：建立行星动力下降段的着陆器状态模型；

在着陆点固连坐标系下，着陆器状态x包括位置矢量r＝[x,y,z]^T、速度矢量v＝[v_x,v_y,v_z]^T、姿态四元数q＝[q₁,q₂,q₃,q₄]、加速度计漂移 陀螺常值漂移动力下降段的着陆器状态模型为：

其中为着陆器本体系到着陆点固连坐标系的坐标转换矩阵，用四元数表示如下：

Ω为角速度的函数矩阵：

a_m和ω_m分别为加速度计和陀螺的输出值，ξ_m和ξ_ω分别是加速度计和陀螺的测量误差，g为重力加速度；ω_x、ω_y、ω_z为着陆器的旋转角速度沿本体系三个轴的分量；

步骤2：建立行星动力下降段的着陆器自主导航测量模型；

设不同波束方向在着陆点固连坐标系下的单位矢量表示为d_j,j＝1,2,…,n，n为波束数量，有：

其中S_3×n为n个波束方向在本体系下的单位矢量矩阵；设着陆器沿TDS的不同波束方向距行星表面的距离为r_j,j＝1,2,…n，则有：

r_j＝z/(d_j·[001]^T)   (5) 

设着陆器沿不同波束方向的速度为v_j,j＝1,2,…n，则有：

S_n×3和分别为S_3×n和的转置；

将导航相机对特征点的观测视线之间的夹角作为观测量；在相机视野范围内找到m个特征点，在着陆点固连系下的位置矢量为ρ_i,i＝1,2,…,m；n个波束形成 个观测角；第i个特征点观测视线和第j个特征点观测视线之间的夹角表示为：

基于TDS和图像测量的导航测量模型为：

式中R＝[r₁,…,r_n]，V＝[v₁,…,v_n]，A＝[A₁₂,…,A_1m,A₂₃,…,A_2m,…,A_(m-1)m]，R_υ、V_υ、A_υ分别为测量误差；

步骤3：自主导航滤波解算；

根据步骤1得到的行星动力下降段状态方程及步骤2得到的测量模型y＝h(x)+υ，通过非线性导航滤波算法估计着陆器的状态；最终输出着陆器的状态信息。

2.根据权利要求1所述的基于TDS与图像测量的行星动力下降段导航方法，其特征在于：n≥3。