[发明专利]基于TDS与图像测量的行星动力下降段导航方法

权利要求书

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

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

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

r·=v]]>

v·=Tbl(am-ba-ξm)+g]]>

q·=12Ωq---(1)]]>

b·a=0]]>

b·ω=0]]>

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

Tbl=q02+q12-q22-q322(q0q3+q1q2)2(q1q3-q0q2)2(-q0q3+q1q2)q02-q12+q22-q322(q2q3+q0q1)2(q0q2+q1q3)2(q2q3-q0q1)q02-q12-q22+q32---(2)]]>

Ω为角速度的函数矩阵：

Ω=Ω(ω)=Ω(ωm-bω-ξω)=0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ω1ωzωy-ωx0---(3)]]>

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

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

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

[d1d2···dn]3×n=Tbl·S3×n---(4)]]>

其中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，则有：

v1v2···vn=Sn·3·TlbvxvyvzL---(6)]]>

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

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

Aij=(r→-ρ→i)(r→-ρ→j)|r→-ρ→i||r→-ρ→j|---(7)]]>

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

y=h(x)+υ=RVA+RυVυAυ---(8)]]>

式中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。