CN103344245B - 火星进入段imu和甚高频无线电组合导航的ud-skf方法 - Google Patents

火星进入段imu和甚高频无线电组合导航的ud-skf方法 Download PDF

Info

Publication number
CN103344245B
CN103344245B CN201310286466.8A CN201310286466A CN103344245B CN 103344245 B CN103344245 B CN 103344245B CN 201310286466 A CN201310286466 A CN 201310286466A CN 103344245 B CN103344245 B CN 103344245B
Authority
CN
China
Prior art keywords
mars
formula
centerdot
matrix
gamma
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
CN201310286466.8A
Other languages
English (en)
Other versions
CN103344245A (zh
Inventor
傅惠民
娄泰山
王治华
张勇波
吴云章
肖强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN201310286466.8A priority Critical patent/CN103344245B/zh
Publication of CN103344245A publication Critical patent/CN103344245A/zh
Application granted granted Critical
Publication of CN103344245B publication Critical patent/CN103344245B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,步骤如下:一、建立火星进入段的动力学方程;二、建立火星进入段的量测方程;三、对上述动力学方程(5)和量测方程(6)进行离散化;四、采用UD-SKF滤波算法,并输出导航信息。本发明所采用的UD-SKF滤波算法与传统的EKF相比,不仅将量测方程的偏差信息融合到滤波过程中,极大地改善了滤波效果,提高了导航精度,而且利用UD分解,减少了滤波过程中出现较大误差甚至发散的情况,增强滤波过程的稳定性,从而能对探测器进行实时高效地估计导航状态。

Description

火星进入段IMU和甚高频无线电组合导航的UD-SKF方法
技术领域
本发明涉及一种火星进入段组合导航的自主导航方法,具体涉及以一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,属于航天自主导航技术领域.
背景技术
2012年8月,美国的火星探测器“好奇号”成功登陆火星,着陆点距离预定目标点2385米。但是,未来的火星探测导航任务要求探测器要达到定点着陆的要求,也即是着陆精度在1000米之内。在着陆之前,火星探测器会经历火星进入段、下降段和着陆段(简称EDL),其中火星进入段的导航对探测器的最终着陆精度影响最为显著。这主要是由于火星进入段气动加热环境恶劣,且探测器包裹在整流罩内,导致很多导航敏感器无法正常工作,只能主要依靠惯性测量单元(简称IMU)提供导航信息;其次由于火星和地球之间的通信大延迟,无法利用深空网实时提供较为准确的导航信息。因此,如何规划火星进入段的自主导航方案和自主导航滤波算法成为火星探测器能否定点着陆的首要问题。
目前,大量学者提出了比较新颖的火星进入段自主导航方案,较著名的是Levesque等提出的基于星间甚高频无线电测量通信的自主导航方案(参见Levesque,JF.Advancednavigationandguidanceforhigh-precisionplanetarylandingonMars.Ph.D.Thesis,DepartmentofElectricalEngineering,UniversitedeSherbrooke,2006.)。此导航方案解决了导航信息不足导致的不可观性的问题,但是初始误差、大气模型的不确定性、探测器参数和气动特性的不确定性、信标的位置偏差和信标与探测器之间的通信偏差等导致此自主导航方案也存在导航精度不一定满足未来火星探测任务的要求。
实时高效的自主导航滤波算法也是解决这类问题的有效途径。目前火星进入段导航方式采用的是对IMU的输出进行积分,递推得到相应的探测器状态,但是此种方法收惯性单元漂移和偏差的影响较大,随着时间推移导航误差积累较大。由于火星进入段动力学具有很强的非线性,因此对探测器进行实时估计的算法必须是非线性滤波算法。工程上应用最多的是扩展卡尔曼滤波(简称EKF),此方法易于工程实现,但是由于线性化产生的截断误差会导致滤波精度和稳定性降低(参见Lightsey,EG,Mogensen,AE,Burkhart,PD,etal.,Real-timenavigationformarsmissionsusingthemarsnetwork.JournalofSpacecraftandRockets,2008.45(3):519-533.)。有学者提出了无迹卡尔曼滤波(简称UKF),并应用于火星进入段自主导航中,估计精度较EKF高,但是计算量较EKF大(参见Levesque,JF.Advancednavigationandguidanceforhigh-precisionplanetarylandingonMars.Ph.D.Thesis,DepartmentofElectricalEngineering,UniversitedeSherbrooke,2006.)。但是这两类方法无法有效地处理上述问题的不确定性和偏差,即使采用将这些不确定性或偏差进行状态扩维,然后进行估计,也能取得较好地效果。但是这种处理方式无疑增加了计算量和计算复杂度,不利于对探测器的状态进行实时估计。
本发明在提出的火星进入段IMU和甚高频无线电组合导航的UD-SKF滤波将这些偏差的方差加入状态方差矩阵的计算中,但是不估计这些偏差。此自主导航滤波算法成功地考虑了这些偏差的影响,但是又避免估计它们,有效地提高了导航精度,同时也大量的减少了计算量和计算复杂度,有利于火星进入段对探测器的进入状态进行实时高效地估计。
发明内容
本发明的目的是提供一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法。针对现有的非线性自主导航滤波算法无法消除这些偏差带来较大误差,甚至导致发散的现象,或者进行状态扩维后,导致计算量和计算复杂度急剧增加的不利情况,本发明所提出的基于UD-SKF滤波算法成功地考虑了这些偏差的影响,但是又避免估计它们,有效地提高了导航精度,同时也大量的减少了计算量和计算复杂度,有利于火星进入段对探测器的进入状态进行实时高效地估计。
本发明涉及一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,它包括以下四个步骤(具体流程图参见图1):
步骤一、建立火星进入段的动力学方程
在火星进入段,将探测器看作是一个无动力的质点。假设进入段的大气相对火星是静止的,同时忽略火星的自转,建立火星进入段的三自由度动力学方程:
r · = v sin γ
θ · = v cos γ sin ψ r cos λ
λ · = v cos γ cos ψ r
v · = - D - g ( r ) sin γ - - - ( 1 )
γ · = ( v r - g ( r ) v ) cos γ + L v cos φ
ψ · = v r sin ψ cos γ tan λ + L v cos γ sin φ
式中,r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离(高度)、探测器的速度、经度、纬度、飞行路径角和航向角,火星重力加速度g(r)=μ/r2,μ是火星重力加速度常数,D和L分别为阻力加速度和升力加速度:
D = C D S m q ‾ - - - ( 2 )
L = C L S m q ‾ - - - ( 3 )
式中CD和CL分别为阻力系数和升力系数,S为探测器参考面积,m为探测器质量,为动压,ρ是火星大气密度,此处采用指数大气密度模型:
ρ = ρ 0 exp { r 0 - r h s } - - - ( 4 )
式中ρ0是标称密度,r0为火星径向基准位置,hs为大气定标高度。
取状态向量为X=[rθλvγψ]T,考虑到系统噪声向量w(t),则动力学程(1)可简写为
X · ( t ) = f ( X ( t ) , t ) + w ( t ) - - - ( 5 )
式中,f(X(t),t)为系统非线性连续状态转移函数,w(t)为零均值的高斯白噪声。
步骤二、建立火星进入段的量测方程
本发明以基于星间甚高频无线电测量通信的自主导航方案为基础,也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息,建立火星进入段量测方程:
Z=h(X(t),t)+b+v(t)(6)
式中,
h ( X ( t ) , t ) = a v R ~ T
av=[-D-D·L/D·sinφD·L/D·cosφ]T(7)
R ~ = [ R ~ 1 , R ~ 2 , · · · , R ~ m ] T
v(t)=[ηaξR]T
式中,av为速度系下的加速度,表示双向测距信息,维数m由有效的火星在轨信标和火星表面固定信标数目决定,b为常值偏差向量,v(t)为零均值的高斯白噪声,ηa为IMU测量信息的测量噪声向量,ξR为双向测距的测量噪声向量。
其中,IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度,测量模型为
a ~ = a + b a + η a - - - ( 8 )
式中,表示各个轴的加速度测量值,a表示真实的加速度,ba加速度计常值漂移,测量噪声ηa为零均值的高斯白噪声。
其中,双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离,无线电通信双向测距示意图参见图2,可表示为:
R ~ = R + b R + ξ R - - - ( 9 )
R = ( r l - r k ) T ( r l - r k ) - - - ( 10 )
式中,rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标(k=o)或火星表面固定信标(k=b),bR为双向测距的常值偏差,测距噪声ξR为零均值的高斯白噪声。
步骤三、对上述动力学方程(5)和量测方程(6)进行离散化
Xk+1=F(Xk)+wk(11)
式中,k=1,2,3,…,F(Xk)为f(X(t),t)离散后的非线性状态转移函数,为h(X(t),t)离散后的非线性量测函数,wk和vk互不相关,且其方差矩阵分别为Qk和Rk
然后将方程(11)和(12)进行线性化,也即将式(11)中的非线性离散函数F(Xk)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的动力学方程
Xk+1k+1/kXk+Uk+wk(13)
式中,
Φ k + 1 / k = ∂ F ( X k ) ∂ X k | x k = x ^ k - - - ( 14 )
U k = F ( X ^ k ) - ∂ F ( X k ) ∂ X k | x k = x ^ k · X ^ k - - - ( 15 )
然后,再将式(12)中的非线性离散函数围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的增量量测方程
Zk=HkXk+Yk+vk(16)
式中,
通过上述过程,就得到了线性化后的动力学方程和量测方程,式中Uk和Yk为非随机的外作用项。
步骤四、采用UD-SKF滤波算法,并输出导航信息。
考虑式(6)中的常值偏差向量b未能精确建模,故施密特-卡尔曼滤波算法(SchmidtKalmanfilter,简称SKF)考虑将其方差融入到算法中,也即是考虑了偏差与状态的互协方差,但是不估计这些偏差。同时,在计算增益矩阵时采用UD分解以确保滤波的数值稳定性。
其中步骤四所采用的UD-SKF滤波算法实现步骤为:
1.将常值偏差向量b扩维进入状态向量,则动力学方程(13)和量测方程(16)变为
X k + 1 b k + 1 = Φ k + 1 | k 0 0 I X k b k + w k 0 - - - ( 19 )
Z k = [ H k , I ] X k b k + v k - - - ( 20 )
式中,我们考虑的常值偏差向量满足条件:bk=bk-1。其方差矩阵B0
B0=Cov{b0}=Cov{bk}(21)
和偏差与状态的互协方差矩阵Ck
C k = E { X ~ k b T } = E { ( X k - X ^ k ) b T } , - - - ( 22 )
并且初始值为C0=0。式中,为卡尔曼滤波第k步的状态估计。
同时,相应与动力学方程(19)和量测方程(20)第k步的误差方差阵
式中,为Ck转置矩阵,Pk为状态Xk的误差方差阵
P k = E { X ~ k X ~ k T } = E { ( X k - X ^ k ) ( X k - X ^ k ) T } - - - ( 24 )
然后,初始化状态向量X0及误差方差阵P0
2.由第k步的状态估计可得,第k+1步的状态一步预测
X ^ k + 1 / k = Φ k + 1 | k X ^ k - - - ( 25 )
3.第k+1步的一步预测误差方差矩阵
则可得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k
P k + 1 | k = Φ k + 1 | k P k Φ k + 1 | k T + Q k - - - ( 27 )
Ck+1|kk+1|kCk(28)
4.第k+1步的滤波增益矩阵
其中,由于不需要估计偏差,故强制令偏差项的增益矩阵为零。
则状态的增益矩阵Kk+1
K k + 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] Ω k + 1 - 1 - - - ( 30 )
Ω k + 1 = H k + 1 P k + 1 | k H k + 1 T + C k + 1 | k T H k + 1 T + H k + 1 C k + 1 | k + B 0 + R k + 1 - - - ( 31 )
由于式(31)中的Ωk+1在滤波过程中无法保持非负定性,此时式(30)中的增益矩阵Kk+1在求逆运算中会产生较大的误差,甚至会发散。为了保持滤波的数值稳定性,本发明采用UD分解来计算增益矩阵Kk+1,会起到很好的效果。UD分解的详细步骤如下:
1)将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式:
Ωk=UDUT(32)
2)将式(32)代入是式(30)中,得到
K k UDU T = P k + 1 | k H k + 1 T + C k + 1 | k - - - ( 33 )
UDU T K k T = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 34 )
3)令则有
UDU T X = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 35 )
4)可以通过以下的迭代过程得到
UX 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T
DX2=X1(36)
UTX=X2
5)最后令然后即可得到Kk
5.第k+1步的状态估计
X ^ k + 1 = X ^ k + 1 / k + K k + 1 ( Z k + 1 - H k + 1 X ^ k + 1 / k ) - - - ( 37 )
6.第k+1步的估计误差方差矩阵
则状态的估计误差方差矩阵Pk+1
P k + 1 = P k + 1 | k - K k + 1 Ω k + 1 K k + 1 T - - - ( 39 )
和偏差与状态的互协方差矩阵为
Ck+1=Ck+1|k-Kk+1(Hk+1Ck+1|k+B0)(40)
通过以上6步循环进行即可得到探测器的实时状态估计值,包含探测器的高度、速度、经度、纬度、飞行路径角和航向角。本发明的UD‐SKF滤波算法的原理图参见图3。
本发明共由步骤一、步骤二、步骤三和步骤四四个步骤组成,通过建立IMU和信标无线电组合导航的量测方程,然后利用UD‐SKF滤波算法消除测量信息中的误差的影响,并增强滤波的稳定性,达到高效实时估计探测器导航状态的目的。
本发明的原理是:由于火星进入段气动加热环境恶劣,且探测器包裹在整流罩内,导致很多导航敏感器无法正常工作,只能主要依靠惯性测量单元(简称IMU)提供导航信息,而仅依靠IMU提供的导航信息无法满足完探测器任务精确导航的要求。由于在火星进入段甚高频无线电可以进行通信,因此利用探测器与预先投放的火星在轨信标或火星表面固定信标之间的甚高频无线电双向通信测出探测器与信标之间的距离作为新的测量信息加入量测方程。经可观性理论分析和仿真验证表明,与三个信标进行双向通信测距即可满足完全可观的,且状态估计更加精确。但是由于IMU提供的测量信息带有常值偏差和双向测距存在通信偏差,这些偏差将给导航状态估计带来较大的偏差,甚至可能导致发散。因此,采用UD‐SKF滤波算法将这些偏差的方差融合到对状态的估计中而不去估计这些偏差,同时使用UD分解将来避免增益矩阵可能产生的较多误差,增强滤波过程的稳定性,从而能对探测器进行实时高效地估计导航状态。
其中,在步骤一中所述的“建立火星进入段的动力学方程”,是指令
f ( X ( t ) , t ) = v sin γ v cos γ sin ψ r cos λ v cos γ cos ψ r - D - g ( r ) sin γ ( v r - g ( r ) v ) cos γ + L v cos φ v r sin ψ cos γ tan λ + L v cos γ sin φ - - - ( 41 )
得到式(5)。
其中,在步骤二中所述的“建立火星进入段的量测方程”,是指令式(7)中的第一式为
h ( X ( t ) , t ) = [ - D , - D · L / D · sin φ , D · L / D · cos φ , R ~ 1 , R ~ 2 , · · · , R ~ m ] T - - - ( 42 )
得到式(6)。
其中,在步骤三中所述的“对动力学方程(5)和量测方程(6)进行离散化”,所采用的方法为泰勒级数展开法。泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式:
f ( x ) = Σ n = 0 ∞ f ( n ) ( a ) n ! ( x - a ) n - - - ( 43 )
式中,n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数。实际应用中,泰勒级数需要截断,只取有限项,因此会产生相应的截断误差。
本发明的优点是:本发明所采用的UD‐SKF滤波算法与传统的EKF相比,增加了少量的计算量,不仅将量测方程的偏差信息融合到滤波过程中,极大地改善了滤波效果,提高了导航精度,而且利用UD分解,减少了滤波过程中出现较大误差甚至发散的情况,增强滤波过程的稳定性,从而能对探测器进行实时高效地估计导航状态。
附图说明
图1为本发明的流程图。
图2为本发明中的测量信息——无线电通信双向测距示意图。
图3为本发明中的滤波方法——UD‐SKF滤波算法的原理图。
图4为本发明的UD‐SKF滤波算法与EKF导航误差对比图。
图中符号说明如下:
图2中“rorbiter”表示在轨信标与火星中心的距离向量,“rlander”表示探测器与火星中心的距离向量,“rbeacon”表示火星表面固定信标与火星中心的距离向量。
图4中“EKFErrors”表示EKF滤波算法的误差,“UD‐SKFErrors”表本发明的UD‐SKF滤波算法的误差,“taxis”表示时间轴,“AltitudeError”表示探测器的高度误差,“VelocityError”表示探测器的速度误差,“FPAError”表示探测器的飞行路径角误差,“LongitudeError”表示探测器的经度误差,“LatitudeError”表示探测器的纬度误差,“AzimuthError”表示探测器的航向角误差。
具体实施方式
下面结合附图和实施例对本发明作详细说明。
本发明涉及一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,其计算流程图如图1所示和UD-SKF算法方法流程图如图2所示,它包括以下四个步骤:
步骤一、建立火星进入段的动力学方程
在火星进入段,将探测器看作是一个无动力的质点。假设进入段的大气相对火星是静止的,同时忽略火星的自转,建立火星进入段的三自由度动力学方程:
r · = v sin γ
θ · = v cos γ sin ψ r cos λ
λ · = v cos γ cos ψ r
v · = - D - g ( r ) sin γ - - - ( 1 )
γ · = ( v r - g ( r ) v ) cos γ + L v cos φ
ψ · = v r sin ψ cos γ tan λ + L v cos γ sin φ
式中,r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离(高度)、探测器的速度、经度、纬度、飞行路径角和航向角,火星重力加速度g(r)=μ/r2,μ是火星重力加速度常数(μ=42828.29×109m3/s2),D和L分别为阻力加速度和升力加速度:
D = C D S m q ‾ - - - ( 2 )
L = C L S m q ‾ - - - ( 3 )
式中CD和CL分别为阻力系数和升力系数,S为探测器参考面积,m为探测器质量,为动压,ρ是火星大气密度,此处采用指数大气密度模型:
ρ = ρ 0 exp { r 0 - r h s } - - - ( 4 )
式中ρ0是标称密度,r0为火星径向基准位置(r0=7.5km),hs为大气定标高度(hs=7.5km)。
取状态向量为X=[rθλvγψ]T,考虑到系统噪声向量w(t),则动力学方程(1)可简写为
X · ( t ) = f ( X ( t ) , t ) + w ( t ) - - - ( 5 )
式中,f(X(t),t)为系统非线性连续状态转移函数,w(t)为零均值的高斯白噪声。
其中,本发明的仿真验证中的探测器的参考值和估计值如下表所示:
其中,本发明的仿真验证中的弹道系数(B=CDS/m)、升阻比(L/D=CL/CD)和标称大气密度参考值和估计值分别如下表所示:
步骤二、建立火星进入段的量测方程
本发明以基于星间甚高频无线电测量通信的自主导航方案为基础,也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息,建立火星进入段量测方程:
Z=h(X(t),t)+b+v(t)(6)
式中,
h ( X ( t ) , t ) = a v R ~ T
av=[-D-D·L/D·sinφD·L/D·cosφ]T(7)
R ~ = [ R ~ 1 , R ~ 2 , · · · , R ~ m ] T
v(t)=[ηaξR]T
式中,av为速度系下的加速度,表示双向测距信息,维数m由有效的火星在轨信标和火星表面固定信标数目决定,b为常值偏差向量,v(t)为零均值的高斯白噪声,ηa为IMU测量信息的测量噪声向量,ξR为双向测距的测量噪声向量。
其中,IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度,测量模型为
a ~ = a + b a + η a - - - ( 8 )
式中,表示各个轴的加速度测量值,a表示真实的加速度,ba加速度计常值漂移,测量噪声ηa为零均值的高斯白噪声。
其中,双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离,无线电通信双向测距示意图参见图2,可表示为:
R ~ = R + b R + ξ R - - - ( 9 )
R = ( r l - r k ) T ( r l - r k ) - - - ( 10 )
式中,rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标(k=o)或火星表面固定信标(k=b),bR为双向测距的常值偏差,测距噪声ξR为零均值的高斯白噪声。
其中,本发明的仿真验证中选择一个火星在轨信标和两个火星表面固定信标,具体参数参见下表:
其中,本发明的仿真验证中对于测量中IMU的加速度常值偏差取为0.003m/s2和通信偏差针对在轨信标和固定信标分别取为24m和45m。
步骤三、对上述动力学方程(5)和量测方程(6)进行离散化
Xk+1=F(Xk)+wk(11)
式中,k=1,2,3,…,F(Xk)为f(X(t),t)离散后的非线性状态转移函数,为h(X(t),t)离散后的非线性量测函数,wk和vk互不相关,且其方差矩阵分别为Qk和Rk
然后将方程(11)和(12)进行线性化,也即将式(11)中的非线性离散函数F(Xk)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的动力学方程
Xk+1k+1/kXk+Uk+wk(13)
式中,
Φ k + 1 / k = ∂ F ( X k ) ∂ X k | x k = x ^ k - - - ( 14 )
U k = F ( X ^ k ) - ∂ F ( X k ) ∂ X k | x k = x ^ k · X ^ k - - - ( 15 )
然后,再将式(12)中的非线性离散函数围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的增量量测方程
Zk=HkXk+Yk+vk(16)
式中,
通过上述过程,就得到了线性化后的动力学方程和量测方程,式中Uk和Yk为非随机的外作用项。
步骤四、采用UD-SKF滤波算法,并输出导航信息。
考虑式(6)中的常值偏差向量b未能精确建模,故施密特-卡尔曼滤波算法(SchmidtKalmanfilter,简称SKF)考虑将其方差融入到算法中,也即是考虑了偏差与状态的互协方差,但是不估计这些偏差。同时,在计算增益矩阵时采用UD分解以确保滤波的数值稳定性。
其中步骤四所采用的UD-SKF滤波算法实现步骤为:
7.将常值偏差向量b扩维进入状态向量,则动力学方程(13)和量测方程(16)变为
X k + 1 b k + 1 = Φ k + 1 | k 0 0 I = X k b k + w k 0 - - - ( 19 )
Z k = [ H k , I ] X k b k + v k - - - ( 20 )
式中,我们考虑的常值偏差向量满足条件:bk=bk-1。其方差矩阵B0
B0=Cov{b0}=Cov{bk}(21)
和偏差与状态的互协方差矩阵Ck
C k = E { X ~ k b T } = E { ( X k - X ^ k ) b T } , - - - ( 22 )
并且初始值为C0=0。式中,为卡尔曼滤波第k步的状态估计。
同时,相应与动力学方程(19)和量测方程(20)第k步的误差方差阵
式中,为Ck转置矩阵,Pk为状态Xk的误差方差阵
P k = E { X ~ k X ~ k T } = E { ( X k - X ^ k ) ( X k - X ^ k ) T } - - - ( 24 )
然后,初始化状态向量X0及误差方差阵P0
8.由第k步的状态估计可得,第k+1步的状态一步预测
X ^ k + 1 / k = Φ k + 1 | k X ^ k - - - ( 25 )
9.第k+1步的一步预测误差方差矩阵
则可得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k
P k + 1 | k = Φ k + 1 | k P k Φ k + 1 | k T + Q k - - - ( 27 )
Ck+1|kk+1|kCk(28)
10.第k+1步的滤波增益矩阵
其中,由于不需要估计偏差,故强制令偏差项的增益矩阵为零。
则状态的增益矩阵Kk+1
K k + 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] Ω k + 1 - 1 - - - ( 30 )
Ω k + 1 = H k + 1 P k + 1 | k H k + 1 T + C k + 1 | k T H k + 1 T + H k + 1 C k + 1 | k + B 0 + R k + 1 - - - ( 31 )
由于式(31)中的Ωk+1在滤波过程中无法保持非负定性,此时式(30)中的增益矩阵Kk+1在求逆运算中会产生较大的误差,甚至会发散。为了保持滤波的数值稳定性,本发明采用UD分解来计算增益矩阵Kk+1,会起到很好的效果。UD分解的详细步骤如下:
1)将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式:
Ωk=UDUT(32)
2)将式(32)代入是式(30)中,得到
K k UDU T = P k + 1 | k H k + 1 T + C k + 1 | k - - - ( 33 )
UDU T K k T = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 34 )
3)令则有
UDU T X = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 35 )
4)可以通过以下的迭代过程得到
UX 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T
DX2=X1(36)
UTX=X2
5)最后令然后即可得到Kk
11.第k+1步的状态估计
X ^ k + 1 = X ^ k + 1 / k + K k + 1 ( Z k + 1 - H k + 1 X ^ k + 1 / k ) - - - ( 37 )
12.第k+1步的估计误差方差矩阵
则状态的估计误差方差矩阵Pk+1
P k + 1 = P k + 1 | k - K k + 1 Ω k + 1 K k + 1 T - - - ( 39 )
和偏差与状态的互协方差矩阵为
Ck+1=Ck+1|k-Kk+1(Hk+1Ck+1|k+B0)(40)
通过以上6步循环进行即可得到探测器的实时状态估计值,包含探测器的高度、速度、经度、纬度、飞行路径角和航向角。本发明的UD‐SKF滤波算法的原理图参见图3。
其中,在步骤一中所述的“建立火星进入段的动力学方程”,是指令
f ( X ( t ) , t ) = v sin γ v cos γ sin ψ r cos λ v cos γ cos ψ r - D - g ( r ) sin γ ( v r - g ( r ) v ) cos γ + L v cos φ v r sin ψ cos γ tan λ + L v cos γ sin φ - - - ( 41 )
得到式(5)。
其中,在步骤二中所述的“建立火星进入段的量测方程”,是指令式(7)中的第一式为
h ( X ( t ) , t ) = [ - D , - D · L / D · sin φ , D · L / D · cos φ , R ~ 1 , R ~ 2 , R ~ 3 ] T - - - ( 42 )
得到式(6)。由于是具体实施中采用三个信标,故式(42)中只有三个信标的距离信息。
其中,在步骤三中所述的“对动力学方程(5)和量测方程(6)进行离散化”,所采用的方法为泰勒级数展开法。泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式:
f ( x ) = Σ n = 0 ∞ f ( n ) ( a ) n ! ( x - a ) n - - - ( 43 )
式中,n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数。实际应用中,泰勒级数需要截断,只取有限项,因此会产生相应的截断误差。
从图4的UD‐SKF滤波算法与EKF导航误差对比图可以看出,由EKF滤波算法所估计的状态仅有航向角是收敛的,其余的状态都是发散的,而由UD‐SKF滤波算法所估计的状态除了速度的收敛速度较慢之外,其余的状态都很快地收敛到参考值。从二者的对比来看,明显UD‐SKF滤波算法要远远好于EKF滤波算法,且最终的滤波效果也好于EKF滤波算法(见下表)。

Claims (4)

1.一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,其特征在于:它包括以下四个步骤:
步骤一、建立火星进入段的动力学方程
在火星进入段,将探测器看作是一个无动力的质点,假设进入段的大气相对火星是静止的,同时忽略火星的自转,建立火星进入段的三自由度动力学方程:
r · = v s i n γ
θ · = v c o s γ s i n ψ r c o s λ
λ · = v c o s γ c o s ψ r v · = - D - g ( r ) sin γ - - - ( 1 )
γ · = ( v r - g ( r ) v ) c o s γ + L v c o s φ
ψ · = v r s i n ψ c o s γ t a n λ + L v cos γ s i n φ
式中,r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离即高度、探测器的速度、经度、纬度、飞行路径角和航向角,火星重力加速度g(r)=μ/r2,μ是火星重力加速度常数,D和L分别为阻力加速度和升力加速度:
D = C D S m q ‾ - - - ( 2 )
L = C L S m q ‾ - - - ( 3 )
式中CD和CL分别为阻力系数和升力系数,S为探测器参考面积,m为探测器质量,为动压,ρ是火星大气密度,此处采用指数大气密度模型:
ρ = ρ 0 exp { r 0 - r h s } - - - ( 4 )
式中ρ0是标称密度,r0为火星径向基准位置,hs为大气定标高度;
取状态向量为X=[rθλvγψ]T,考虑到系统噪声向量w(t),则动力学程(1)简写为
X · ( t ) = f ( X ( t ) , t ) + w ( t ) - - - ( 5 )
式中,f(X(t),t)为系统非线性连续状态转移函数,w(t)为零均值的高斯白噪声;
步骤二、建立火星进入段的量测方程
以基于星间甚高频无线电测量通信的自主导航方案为基础,也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息,建立火星进入段量测方程:
Z=h(X(t),t)+b+v(t)(6)
式中,
h ( X ( t ) , t ) = a v R ~ T
a v = - D - D · L / D · sin φ D · L / D · cos φ T - - - ( 7 )
R ~ = R ~ 1 , R ~ 2 , ... , R ~ m T
v ( t ) = η a ξ R T
式中,av为速度系下的加速度,表示双向测距信息,维数m由有效的火星在轨信标和火星表面固定信标数目决定,b为常值偏差向量,v(t)为零均值的高斯白噪声,ηa为IMU测量信息的测量噪声向量,ξR为双向测距的测量噪声向量;
其中,IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度,测量模型为
a ~ = a + b a + η a - - - ( 8 )
式中,表示各个轴的加速度测量值,a表示真实的加速度,ba加速度计常值漂移,测量噪声ηa为零均值的高斯白噪声;
其中,双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离,表示为:
R ~ = R + b R + ξ R - - - ( 9 )
R = ( r l - r k ) T ( r l - r k ) - - - ( 10 )
式中,rl和rk分别表示火星质心惯性系下火星探测器和火星在轨信标或火星表面固定信标,bR为双向测距的常值偏差,测距噪声ξR为零均值的高斯白噪声;
步骤三、对上述动力学方程(5)和量测方程(6)进行离散化
Xk+1=F(Xk)+wk(11)
式中,k=1,2,3,…,F(Xk)为f(X(t),t)离散后的非线性状态转移函数,为h(X(t),t)离散后的非线性量测函数,wk和vk互不相关,且其方差矩阵分别为Qk和Rk
然后将方程(11)和(12)进行线性化,也即将式(11)中的非线性离散函数F(Xk)围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的动力学方程
Xk+1=Φk+1/kXk+Uk+wk(13)
式中,
Φ k + 1 / k = ∂ F ( X k ) ∂ X k | X k = X ^ k - - - ( 14 )
U k = F ( X ^ k ) - ∂ F ( X k ) ∂ X k | X k = X ^ k · X ^ k - - - ( 15 )
然后,再将式(12)中的非线性离散函数围绕估计值展开成泰勒级数,并略去二阶以上项,得线性化之后的增量量测方程:
Zk=HkXk+Yk+vk(16)
式中,
通过上述过程,就得到了线性化后的动力学方程和量测方程,式中Uk和Yk为非随机的外作用项;
步骤四、采用UD-SKF滤波算法,并输出导航信息
考虑式(6)中的常值偏差向量b未能精确建模,故施密特-卡尔曼滤波算法即SKF考虑将其方差融入到算法中,也即是考虑了偏差与状态的互协方差,但是不估计这些偏差,同时,在计算增益矩阵时采用UD分解以确保滤波的数值稳定性;其中步骤四所采用的UD-SKF滤波算法实现步骤为:
步骤4.1.将常值偏差向量b扩维进入状态向量,则动力学方程(13)和量测方程(16)变为
X k + 1 b k + 1 = Φ k + 1 | k 0 0 I X k b k + w k 0 - - - ( 19 )
Z k = [ H k , I ] X k b k + v k - - - ( 20 )
式中,我们考虑的常值偏差向量满足条件:bk=bk-1。其方差矩阵B0
B0=Cov{b0}=Cov{bk}(21)
和偏差与状态的互协方差矩阵Ck
C k = E { X ~ k b T } = E { ( X k - X ^ k ) b T } , - - - ( 22 )
并且初始值为C0=0,式中,为卡尔曼滤波第k步的状态估计;
同时,相应与动力学方程(19)和量测方程(20)第k步的误差方差阵
式中,为Ck转置矩阵,Pk为状态Xk的误差方差阵
P k = E { X ~ k X ~ k T } = E { ( X k - X ^ k ) ( X k - X ^ k ) T } - - - ( 24 )
然后,初始化状态向量X0及误差方差阵P0
步骤4.2.由第k步的状态估计可得,第k+1步的状态一步预测
X ^ k + 1 / k = Φ k + 1 | k X ^ k - - - ( 25 )
步骤4.3.第k+1步的一步预测误差方差矩阵
则得到状态和偏差的一步预测误差方差矩阵Pk+1|k和Ck+1|k
P k + 1 | k = Φ k + 1 | k P k Φ k + 1 | k T + Q k - - - ( 27 )
Ck+1|k=Φk+1|kCk(28)
步骤4.4.第k+1步的滤波增益矩阵
其中,由于不需要估计偏差,故强制令偏差项的增益矩阵为零;
则状态的增益矩阵Kk+1
K k + 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] Ω k + 1 - 1 - - - ( 30 )
Ω k + 1 = H k + 1 P k + 1 | k H k + 1 T + C k + 1 | k T H k + 1 T + H k + 1 C k + 1 | k + B 0 + R k + 1 - - - ( 31 )
由于式(31)中的Ωk+1在滤波过程中无法保持非负定性,此时式(30)中的增益矩阵Kk+1在求逆运算中会产生较大的误差,甚至会发散;为了保持滤波的数值稳定性,采用UD分解来计算增益矩阵Kk+1,会起到很好的效果;UD分解的详细步骤如下:
1)将对称正定矩阵Ωk分解成一个对角阵D和一个单位上三角阵U的形式:
Ωk=UDUT(32)
2)将式(32)代入是式(30)中,得到
K k UDU T = P k + 1 | k H k + 1 T + C k + 1 | k - - - ( 33 )
UDU T K k T = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 34 )
3)令则有
UDU T X = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T - - - ( 35 )
4)通过以下的迭代过程得到
UX 1 = [ P k + 1 | k H k + 1 T + C k + 1 | k ] T
DX2=X1(36)
UTX=X2
5)最后令然后即得到Kk
步骤4.5.第k+1步的状态估计
X ^ k + 1 = X ^ k + 1 / k + K k + 1 ( Z k + 1 - H k + 1 X ^ k + 1 / k ) - - - ( 37 )
步骤4.6.第k+1步的估计误差方差矩阵
则状态的估计误差方差矩阵Pk+1
P k + 1 = P k + 1 | k - K k + 1 Ω k + 1 K k + 1 T - - - ( 39 )
和偏差与状态的互协方差矩阵为
Ck+1=Ck+|k-Kk+1(Hk+1Ck+1|k+B0)(40)
通过步骤4.1至步骤4.6循环进行即得到探测器的实时状态估计值,包含探测器的高度、速度、经度、纬度、飞行路径角和航向角;
通过上述四个步骤建立IMU和信标无线电组合导航的量测方程,然后利用UD-SKF滤波算法消除测量信息中的误差的影响,并增强滤波的稳定性,达到高效实时估计探测器导航状态的目的。
2.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,其特征在于:在步骤一中所述的“建立火星进入段的动力学方程”,是指令
f ( X ( t ) , t ) = v s i n γ v c o s γ sin ψ r c o s λ v c o s γ cos ψ r - D - g ( r ) sin γ ( v r - g ( r ) v ) cos γ + L v c o s φ v r s i n ψ cos γ tan λ + L v cos γ s i n φ - - - ( 41 )
得到式(5)。
3.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,其特征在于:在步骤二中所述的“建立火星进入段的量测方程”,是指令式(7)中的第一式为
h ( X ( t ) , t ) = - D , - D · L / D · sin φ , D · L / D · cos φ , R ~ 1 , R ~ 2 , ... , R ~ m T - - - ( 42 )
得到式(6)。
4.根据权利要求1所述的一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法,其特征在于:在步骤三中所述的“对动力学方程(5)和量测方程(6)进行离散化”,所采用的方法为泰勒级数展开法;泰勒级数在数学上是一个无穷可微的函数f(x)的幂级数展开式:
f ( x ) = Σ n = 0 ∞ f ( n ) ( a ) n ! ( x - a ) n - - - ( 43 )
式中,n!表示n的阶乘,而f(n)(a)表示函数f(x)在点x=a处的n阶导数,实际应用中,泰勒级数需要截断,只取有限项,因此会产生相应的截断误差。
CN201310286466.8A 2013-07-09 2013-07-09 火星进入段imu和甚高频无线电组合导航的ud-skf方法 Expired - Fee Related CN103344245B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310286466.8A CN103344245B (zh) 2013-07-09 2013-07-09 火星进入段imu和甚高频无线电组合导航的ud-skf方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310286466.8A CN103344245B (zh) 2013-07-09 2013-07-09 火星进入段imu和甚高频无线电组合导航的ud-skf方法

Publications (2)

Publication Number Publication Date
CN103344245A CN103344245A (zh) 2013-10-09
CN103344245B true CN103344245B (zh) 2015-11-18

Family

ID=49279061

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310286466.8A Expired - Fee Related CN103344245B (zh) 2013-07-09 2013-07-09 火星进入段imu和甚高频无线电组合导航的ud-skf方法

Country Status (1)

Country Link
CN (1) CN103344245B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616024B (zh) * 2013-11-27 2016-05-04 北京理工大学 一种行星探测进入段自主导航系统可观测度确定方法
CN104794323B (zh) * 2014-12-23 2018-04-03 北京理工大学 一种基于多模型的火星大气进入容中断估计方法
CN106441309B (zh) * 2016-11-14 2019-03-12 郑州轻工业学院 基于协方差交叉融合的火星进入段分布式自主导航方法
CN113844682B (zh) * 2021-09-13 2023-06-16 北京控制工程研究所 一种火星edl过程大动态导航试验验证系统及方法
CN114019792B (zh) * 2021-10-08 2023-08-01 北京控制工程研究所 一种火星大气进入过程升力制导误差分析方法和系统
CN116753961B (zh) * 2023-08-16 2023-10-31 中国船舶集团有限公司第七〇七研究所 基于状态观测的动力定位船舶高速循迹导航方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101865693A (zh) * 2010-06-03 2010-10-20 天津职业技术师范大学 航空用多传感器组合导航系统
CN103063217A (zh) * 2013-01-08 2013-04-24 北京航空航天大学 一种基于星历修正的深空探测器天文/无线电组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8825399B2 (en) * 2008-07-24 2014-09-02 Raytheon Company System and method of passive and autonomous navigation of space vehicles using an extended Kalman filter

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101865693A (zh) * 2010-06-03 2010-10-20 天津职业技术师范大学 航空用多传感器组合导航系统
CN103063217A (zh) * 2013-01-08 2013-04-24 北京航空航天大学 一种基于星历修正的深空探测器天文/无线电组合导航方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Toward Improved Landing Precision on Mars;Aron A.Wolf等;《IEEE Conference Publications》;20111231;第1-8页 *
新型火星EDL导航、制导与控制技术研究;彭玉明;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20111115;全文 *
火星进入段自主导航技术研究现状与展望;崔平远等;《宇航学报》;20130430;第34卷(第4期);第447-456页 *
王轶博.着陆器在火星大气进入段的自主导航方法研究.《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》.2012, *
自适应无迹增量滤波方法;傅惠民等;《航天动力学报》;20130228;第28卷(第2期);第259-263页 *

Also Published As

Publication number Publication date
CN103344245A (zh) 2013-10-09

Similar Documents

Publication Publication Date Title
CN103344245B (zh) 火星进入段imu和甚高频无线电组合导航的ud-skf方法
CN104215259B (zh) 一种基于地磁模量梯度和粒子滤波的惯导误差校正方法
CN103513294B (zh) 一种低低星星跟踪卫星重力场测量性能解析计算方法
CN103389088B (zh) 一种四冗余rfins最优配置方案的确定方法
CN103076017B (zh) 基于可观测度分析的火星进入段自主导航方案设计方法
CN102809376B (zh) 一种基于等值线的辅助导航定位方法
CN110146909A (zh) 一种定位数据处理方法
CN103453907B (zh) 基于分层大气模型的行星进入段导航滤波方法
CN103884340B (zh) 一种深空探测定点软着陆过程的信息融合导航方法
CN105300387B (zh) 一种火星大气进入段非线性非高斯秩滤波方法
Fan et al. An advanced cooperative positioning algorithm based on improved factor graph and sum-product theory for multiple AUVs
US20180017392A1 (en) Apparatus, system, and method for traffic
CN106441372A (zh) 一种基于偏振与重力信息的静基座粗对准方法
CN109084760A (zh) 一种楼宇间导航系统
CN103398725A (zh) 一种基于星敏感器的捷联惯导系统初始对准的方法
CN103017772A (zh) 一种基于可观性分析的光学和脉冲星融合自主导航方法
CN102168979B (zh) 一种基于三角形约束模型的无源导航的等值线匹配方法
Wang et al. Underwater localization and environment mapping using wireless robots
CN103218482A (zh) 一种动力学系统中不确定参数的估计方法
CN106705967A (zh) 一种基于行人航位推算的精度改善的室内定位和方法
CN106441309B (zh) 基于协方差交叉融合的火星进入段分布式自主导航方法
CN104949687A (zh) 一种长时间导航系统全参数精度评估方法
Sabatini et al. Low-cost navigation and guidance systems for Unmanned Aerial Vehicles. Part 2: Attitude determination and control
CN109100750A (zh) 一种基于自适应权值估计的星座导航敏感器调度方法
CN112325878A (zh) 基于ukf与空中无人机节点辅助的地面载体组合导航方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20151118

Termination date: 20170709

CF01 Termination of patent right due to non-payment of annual fee