CN104613984B - 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 - Google Patents
临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 Download PDFInfo
- Publication number
- CN104613984B CN104613984B CN201510064836.2A CN201510064836A CN104613984B CN 104613984 B CN104613984 B CN 104613984B CN 201510064836 A CN201510064836 A CN 201510064836A CN 104613984 B CN104613984 B CN 104613984B
- Authority
- CN
- China
- Prior art keywords
- equation
- error
- matrix
- noise
- observation
- 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.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,包括四个步骤:步骤一,根据临近空间飞行器传递对准系统的工作原理和特点,建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程;步骤二,根据系统的误差方程建立模型不确定的状态方程和观测方程;步骤三,给出状态变量初始值(x0)和预测误差方差矩阵初始值(Σ0|0),给出稀疏网格求积分点集(ξj,εj;j=1,2,…Np),给出鲁棒滤波参数γ和ε;步骤四,利用鲁棒滤波对系统状态进行估计并对子惯导系统进行误差修正,完成传递对准过程。本发明适用于临近空间飞行器动态条件下主子惯导系统具有模型不确定性的传递对准。
Description
技术领域
本发明涉及导航系统非线性鲁棒滤波领域,具体涉及一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法。
背景技术
传递对准是解决高超声速飞行器在动基座条件下初始对准问题的主要方法,一般采用非线性卡尔曼滤波器对系统的状态进行估计。基于非线性高斯逼近的滤波器其性能取决于系统模型以及干扰特性假设的精确程度。在实际工作中,高超声速飞行器的外挂武器和传感器吊舱一般悬挂在机翼或机腹下,而飞行器在高速机动飞行情况下,受空气气流、载荷变更、发动机噪声等多种因素的影响,机体会发生时变结构变形,复合材料的更多使用和现代战斗机的高机动特性使机身和机翼的弹性特性增强,产生的弹性变形对传递对准的精度影响更加剧烈。临近空间飞行器传递对准时,存在动态杆臂和挠曲变形难以建模等问题,迫切需要利用模型不确定的鲁棒滤波方法来解决。
目前模型不确定的鲁棒滤波方法主要是鲁棒EKF算法,该算法由于EKF将非线性系统线性化,在强非线性时将会引起较大的估计误差,并且在线性化处理时需要用雅克比(Jacobian)矩阵,其繁琐的计算过程导致该方法实现相对困难;基于稀疏网格理论的积分点配置策略以其积分点数目少、计算精度高的特性,形成了一系列基于矩匹配法、Gauss-Hermite准则以及Kronrod-Patterson准则的稀疏网格求积分非线性高斯滤波算法。基于此,研究一种精度高鲁棒性强的滤波方法,成为了行业发展的方向。
发明内容
发明目的:为了解决临近空间飞行器传递对准中存在动态杆臂和挠曲变形难以建模等而导致传递对准性能下降,利用模型不确定的鲁棒滤波方法,将鲁棒算法与一系列稀疏网格求积分非线性滤波技术结合,提供一种精度高鲁棒性强的滤波方法。
技术方案:一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,该鲁棒滤波方法具体步骤如下:
步骤1)根据临近空间飞行器传递对准系统的工作原理和特点,建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程;
步骤2)根据系统的误差方程建立模型不确定的状态方程和观测方程;
xk=f(xk-1)+Φ(xk-1)ηk+Gk|k-1wk-1 (1)
zk=h(xk)+Ψ(xk)υk+vk (2)
式中:
xk是n维状态向量,zk是m维观测向量,f(·)和h(·)分别对应非线性状态方程和观测方程;Gk|k-1是n×r维系统过程噪声输入矩阵,wk-1是r维系统过程噪声序列,vk是m维系统观测噪声序列;Φ(·)∈Rn×n是系统状态方程中模型不确定性部分的有界输入矩阵,Ψ(·)∈Rm×m是系统观测方程中模型不确定性部分的有界输入矩阵;ηk∈Rn是系统状态方程中模型不确定性未知有界变量,υk∈Rm是系统观测方程中模型不确定性未知有界变量;
步骤3)给出状态变量初始值(x0)和预测误差方差矩阵初始值(Σ0|0),给出Np个稀疏网格求积分点集(ξj,εj),其中j=1,2,…,Np;
状态变量初始值x0=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]T;
预测误差方差矩阵初始值:
系统过程噪声初始值
系统观测噪声初始值
式中:
φx0、φy0和φz0是初始数学平台失准角;
δVx0、δVy0和δVz0是初始速度误差;δSx0、δSy0和δSz0是初始位置误差;
εgx0、εgy0和εgz0是陀螺仪常值漂移初值;▽ax0、▽ay0和▽az0是加速度计常值偏移初值;μx0、μy0和μz0是主子惯导间安装误差初值;
wgx、wgy和wgz是陀螺仪随机噪声;wax、way和waz是加速度计随机噪声;
σax、σay和σaz是姿态观测噪声标准差;σvx、σvy和σvz是速度观测噪声标准差;
σsx、σsy和σsz是位置观测噪声标准差;
根据稀疏网格求积分准则给定一组积分点集(ξj,εj)其中j=1,2,…Np,Np表示积分点集的个数;
其中Np=2n2+2n+1,n为状态变量维数;
给定满足γ>1和ε>0的鲁棒滤波参数;具体为:第一组γ1=500,ε1=0.05:
第二组γ1=100,ε1=0.01:
步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断k+1是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,否则返回初始采样过程进行下一次估计;
2、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程具体为:
1.1)数学平台失准角误差方程
其中:
i是发射点惯性坐标系,此处也是导航坐标系;
是惯导解算的发射点惯性坐标系,即数学平台坐标系;i系依次经过三次变换可得数学平台坐标系系,三次转动角分别为:绕z轴旋转φz、绕y轴旋转φy和绕x轴旋转φx;
发射点惯性坐标系下数学平台失准角φi=[φx φy φz]T;
b是弹体坐标系,即子惯导坐标系,也可以用bs表示;原点Ob为弹体质心,Obxb轴为弹体纵轴对称轴,指向弹体头部;Obyb轴在弹体纵向对称面内,并垂直于纵轴Obzb向上;Obzb按照右手规则确定;
为子惯导解算的姿态矩阵,也可以用表示,表示弹体坐标系b到数学平台坐标系的姿态转换矩阵;
为陀螺测量误差,且 为三只陀螺仪测量误差的随机常值部分,wg=[wgx wgy wgz]T为三只陀螺仪测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
1.2)速度误差方程
惯性坐标系下速度误差微分方程为,
式中:
δVi=[δVx δVy δVz]T;
是子惯导IMU的加速度计测量值;
是子惯导IMU的加速度计测量误差,且 为加速度计测量误差的随机常值部分,wa=[wax way wgz]T为三只加速度计测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
是系至i系的变换阵;
δgi是引力场模型的引力加速度误差;
1.3)位置误差方程
发射点惯性坐标系下位置误差δSi微分方程为,
式中:δSi=[δSx δSy δSz]T;
1.4)姿态匹配观测方程
式中:
bm是载体坐标系即载机坐标系;
bs是子惯导坐标系;
是主惯导的姿态阵,是的转置矩阵;
—主惯导的姿态误差角,可将其视为白噪声;
是bm系到bs系的变换矩阵;
姿态匹配观测方程由观测矩阵Zdcm矩阵求得,具体为:
式中:Zam是姿态匹配的观测量,
1.5)速度匹配观测方程
Zv=Hvxk+vv (14)
式中:
vv是速度匹配等效观测噪声;
1.6)位置匹配观测方程
Zs=Hsxk+vs (15)
式中:
3、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤2)模型不确定的状态方程和观测方程具体为:
2.1)xk是xk=[φx φy φz δVx δVy δVz δSx δSy δSz εgx εgy εgz ▽ax ▽ay ▽azμx μy μz]T
可简写为
包括导航系为发射点惯性坐标系下失准角φi=[φx φy φz]T、速度误差δVi=[δVxδVyδVz]T、位置误差δSi=[δSx δSy δSz]T、陀螺仪随机常值误差εg i=[εgx εgy εgz]T、加速度计随机常值误差安装误差μi=[μx μy μz]T;系统噪声向量为:
2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为
ηk满足如下关系:
式中:ηk=[ηφ 01×12]T,ηφ∈R6,
2.3)模型不确定的状态方程具体为:
2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为:
Ψφ是姿态匹配模型不确定部分,主要由初始安装误差确定,具体为:
2.5)模型不确定的观测方程具体为:
υφ∈R3是与姿态相关的观测模型不确定性未知有界变量,具体为
υk=[υφ 01×6]T,
观测向量为:zk=[Za Zv Zs]T;其中姿态观测量Za=[Zax Zay Zaz]T,速度观测量Zv=[Zvx Zvy Zvz]T,位置观测量Zs=[Zsx Zsy Zsz]T;
4、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断k+1是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,否则返回初始采样过程进行下一次估计,具体为:
4.1)初始采样
4.2)时间更新
χj,k|k-1=f(γj,k-1) (17)
其中
4.3)重采样
4.4)量测更新
4.5)鲁棒更新
式中:
Σk-1|k-1为预测误差方差矩阵;为状态变量;Σk|k-1为一步预测误差方差矩阵;A为Σk-1|k-1或Σk|k-1通过Cholupdate或SVD分解得到的矩阵;ξj为稀疏网格求积分点集采样点;ωj为稀疏网格求积分点集采样点对应的权重;γj,k-1为初始采样点;Np为积分点集个数;χj,k|k-1为时间更新的Sigma点;为时间更新的状态变量一步预测值;Rk为系统观测噪声矩阵;为计算系统观测噪声矩阵;Qk-1为系统过程噪声矩阵;G k|k-1为系统过程噪声输入矩阵;为计算系统过程噪声矩阵;为重采样的Sigma点;为观测一步预测值;Σzz,k|k-1为观测一步预测误差方差矩阵;Σxz,k|k-1为协误差方差矩阵;Kk为滤波增益矩阵;为状态估计值;Σk|k为估计误差方差矩阵。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
本发明的有点在于
(1)本发明采用鲁棒算法与稀疏网格求积分滤波相结合,形成了一套高精度高鲁棒性的滤波方法。
(2)本发明在滤波过程中不需要增加额外的计算量,只需要选择两个实数鲁棒因子,即可保证系统的鲁棒性,所以具有容易实现的优点。
(3)本发明不需要对系统的杆臂挠曲建模,减小系统维数,减小系统计算量。
(4)本发明的鲁棒滤波方法对实际物理系统中由于物理参数的测量误差、运行环境的变化或系统辨识不精确而引起的模型不确定性均具有较好的适应性。
本发明提出的方案通过如下仿真实验加以验证:
(1)传感器数据采样时间为1ms,滤波周期Tf为20ms,仿真时间6分钟;
(2)初始位置为北纬31.98°,东经118.8°,高度50Km,初始速度为3马赫,安装误差角为5′5′5′,陀螺仪常值漂移为0.01°/h,随机漂移为0.001°/h,加速度计常偏为0.1mg,随机为0.05mg;
(3)静态杆臂为0.15m,0.15m,0.30m,未建模动态杆臂为8-12mm,8-14mm,25-30mm;
(4)横滚角从0°到34°做摇翼机动,俯仰角和航向角从0°到10°做匀速变化;仿真过程中注入挠曲变形干扰,采用“姿态+速度+位置”匹配方式进行传递对准,系统的状态方程和观测方程不对主子惯导间的杆臂和挠曲变形进行建模;
(5)子惯导初始失准角分别为40°/30°/20°,滤波器的初始条件设为
x0=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]T;
Σ0|0=diag{(30°)2,(20°)2,(40°)2,(0.5m/s)2,(0.5m/s)2,(0.5m/s)2,(10.0m)2,(10.0m)2,(10.0m)2,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,(0.1mg)2,(0.1mg)2,(0.1mg)2,(5′)2,(5′)2,(5′)2}T;
Q0=diag{(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,(0.05mg)2,(0.05mg)2,(0.05mg)2};
R0=diag{(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,(0.5m/s)2,(0.5m/s)2,(0.5m/s)2,(10.0m)2,(10.0m)2,(10.0m)2};
(6)分别仿真两组不同鲁棒滤波参数γ1=500和ε1=0.05,鲁棒滤波参数γ2=100和ε2=0.01。
附图说明
图1为基于临近空间飞行器传递对准模型不确定性的鲁棒滤波流程框图;
图2为鲁棒滤波参数γ1=500和ε1=0.05仿真传递对准姿态误差曲线图;
图3为鲁棒滤波参数γ2=100和ε2=0.01仿真传递对准姿态误差曲线图。
具体实施方式
下面结合附图对本发明做更进一步的解释。
根据下述实施例,可以更好的理解本发明。如图1所示,本发明的一种基于临近空间飞行器传递对准模型不确定的鲁棒滤波方法,具体步骤如下:
步骤1)根据临近空间飞行器传递对准系统的工作原理和特点,建立系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程;
步骤2)根据系统的误差方程建立模型不确定的状态方程和观测方程;
xk=f(xk-1)+Φ(xk-1)ηk+Gk|k-1wk-1 (1)
zk=h(xk)+Ψ(xk)υk+vk (2)
式中:
xk是n维状态向量,zk是m维观测向量,f(·)和h(·)分别对应非线性状态方程和观测方程;Gk|k-1是n×r维系统过程噪声输入矩阵,wk-1是r维系统过程噪声序列,vk是m维系统观测噪声序列;Φ(·)∈Rn×n是系统状态方程中模型不确定性部分的有界输入矩阵,Ψ(·)∈Rm×m是系统观测方程中模型不确定性部分的有界输入矩阵;ηk∈Rn是系统状态方程中模型不确定性未知有界变量,υk∈Rm是系统观测方程中模型不确定性未知有界变量;
步骤3)给出状态变量初始值(x0)和预测误差方差矩阵初始值(Σ0|0),给出Np个稀疏网格求积分点集(ξj,εj),其中j=1,2,…,Np;
状态变量初始值x0=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]T;
预测误差方差矩阵初始值:
系统过程噪声初始值
系统观测噪声初始值
式中:
φx0、φy0和φz0是初始数学平台失准角;
δVx0、δVy0和δVz0是初始速度误差;δSx0、δSy0和δSz0是初始位置误差;
εgx0、εgy0和εgz0是陀螺仪常值漂移初值;▽ax0、▽ay0和▽az0是加速度计常值偏移初值;μx0、μy0和μz0是主子惯导间安装误差初值;
wgx、wgy和wgz是陀螺仪随机噪声;wax、way和waz是加速度计随机噪声;
σax、σay和σaz是姿态观测噪声标准差;σvx、σvy和σvz是速度观测噪声标准差;
σsx、σsy和σsz是位置观测噪声标准差;
根据稀疏网格求积分准则给定一组积分点集(ξj,εj)其中j=1,2,…Np,Np表示积分点集的个数;
其中Np=2n2+2n+1,n为状态变量维数;
给定满足γ>1和ε>0的鲁棒滤波参数;具体为:第一组γ1=500,ε1=0.05:
第二组γ1=100,ε1=0.01:
步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断k+1是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,否则返回初始采样过程进行下一次估计;
2、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程,具体为:
1.1)数学平台失准角误差方程
其中:
i是发射点惯性坐标系,此处也是导航坐标系;
是惯导解算的发射点惯性坐标系,即数学平台坐标系;i系依次经过三次变换可得数学平台坐标系系,三次转动角分别为:绕z轴旋转φz、绕y轴旋转φy和绕x轴旋转φx;
发射点惯性坐标系下数学平台失准角φi=[φx φy φz]T;
b是弹体坐标系,即子惯导坐标系,也可以用bs表示;原点Ob为弹体质心,Obxb轴为弹体纵轴对称轴,指向弹体头部;Obyb轴在弹体纵向对称面内,并垂直于纵轴Obzb向上;Obzb按照右手规则确定;
为子惯导解算的姿态矩阵,也可以用表示,表示弹体坐标系b到数学平台坐标系的姿态转换矩阵;
为陀螺测量误差,且 为三只陀螺仪测量误差的随机常值部分,wg=[wgx wgy wgz]T为三只陀螺仪测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
1.2)速度误差方程
惯性坐标系下速度误差微分方程为,
式中:
δVi=[δVx δVy δVz]T;
是子惯导IMU的加速度计测量值;
是子惯导IMU的加速度计测量误差,且▽a i=[▽ax ▽ay ▽az]T为加速度计测量误差的随机常值部分,wa=[wax way wgz]T为三只加速度计测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
是系至i系的变换阵;
δgi是引力场模型的引力加速度误差;
1.3)位置误差方程
发射点惯性坐标系下位置误差δSi微分方程为,
式中:δSi=[δSx δSy δSz]T;
1.4)姿态匹配观测方程
式中:
bm是载体坐标系即载机坐标系;
bs是子惯导坐标系;
是主惯导的姿态阵,是的转置矩阵;
—主惯导的姿态误差角,可将其视为白噪声;
是bm系到bs系的变换矩阵;
姿态匹配观测方程由观测矩阵Zdcm矩阵求得,具体为:
式中:Zam是姿态匹配的观测量,
1.5)速度匹配观测方程
Zv=Hvxk+vv (14)
式中:
vv是速度匹配等效观测噪声;
1.6)位置匹配观测方程
Zs=Hsxk+vs (15)
式中:
3、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤2)模型不确定的状态方程和观测方程具体为:
2.1)xk是xk=[φx φy φz δVx δVy δVz δSx δSy δSz εgx εgy εgz ▽ax ▽ay ▽azμx μy μz]T
可简写为
包括导航系为发射点惯性坐标系下失准角φi=[φx φy φz]T、速度误差δVi=[δVx δVy δVz]T、位置误差δSi=[δSx δSy δSz]T、陀螺仪随机常值误差εg i=[εgx εgy εgz]T、加速度计随机常值误差▽a i=[▽ax ▽ay ▽az]T、安装误差μi=[μx μy μz]T;系统噪声向量为:wk=[εgx εgy εgz ▽ax ▽ay ▽az]T;
2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为
ηk满足如下关系:
式中:ηk=[ηφ 01×12]T,ηφ∈R6,
2.3)模型不确定的状态方程具体为:
2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为:
Ψφ是姿态匹配模型不确定部分,主要由初始安装误差确定,具体为:
2.5)模型不确定的观测方程具体为:
υφ∈R3是与姿态相关的观测模型不确定性未知有界变量,具体为
υk=[υφ 01×6]T,
观测向量为:zk=[Za Zv Zs]T;其中姿态观测量Za=[Zax Zay Zaz]T,速度观测量Zv=[Zvx Zvy Zvz]T,位置观测量Zs=[Zsx Zsy Zsz]T;
4、临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,否则返回初始采样过程进行下一次估计,具体为:
4.1)初始采样
4.2)时间更新
χj,k|k-1=f(γj,k-1) (17)
其中
4.3)重采样
4.4)量测更新
4.5)鲁棒更新
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (3)
1.一种临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,该鲁棒滤波方法具体步骤如下:
步骤1)建立临近空间飞行器传递对准系统的数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程;
步骤2)根据数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程建立模型不确定的状态方程和观测方程;
xk=f(xk-1)+Φ(xk-1)ηk+Gk|k-1wk-1 (1)
zk=h(xk)+Ψ(xk)υk+vk (2)
式中:
xk是n维状态向量,zk是m维观测向量,f(·)和h(·)分别对应非线性状态方程和观测方程;Gk|k-1是n×r维系统过程噪声输入矩阵,wk-1是r维系统过程噪声序列,vk是m维系统观测噪声序列;Φ(·)∈Rn×n是系统状态方程中模型不确定性部分的有界输入矩阵,Ψ(·)∈Rm ×m是系统观测方程中模型不确定性部分的有界输入矩阵;ηk∈Rn是系统状态方程中模型不确定性未知有界变量,υk∈Rm是系统观测方程中模型不确定性未知有界变量;具体如下:
2.1)xk是
可简写为
包括导航系为发射点惯性坐标系下失准角φi=[φx φy φz]T、速度误差δVi=[δVx δVy δVz]T、位置误差δSi=[δSx δSy δSz]T、陀螺仪随机常值误差εg i=[εgx εgy εgz]T、加速度计随机常值误差安装误差μi=[μx μy μz]T;系统噪声向量为:
2.2)系统状态方程中模型不确定性部分的有界输入矩阵Φ(·)具体为
ηk满足如下关系:
式中:ηk=[ηφ 01×12]T,ηφ∈R6,ε为鲁棒滤波参数;
2.3)模型不确定的状态方程具体为:
式中,为子惯导解算的姿态矩阵;是子惯导IMU的加速度计测量值;δgi是引力场模型的引力加速度误差;
2.4)系统观测方程中模型不确定性部分的有界输入矩阵Ψ(·)具体为:
Ψφ是姿态匹配模型不确定部分,主要由初始安装误差确定,具体为:
2.5)模型不确定的观测方程具体为:
υφ∈R3是与姿态相关的观测模型不确定性未知有界变量,具体为
观测向量为:zk=[Za Zv Zs]T;其中姿态观测量Za=[Zax Zay Zaz]T,速度观测量Zv=[ZvxZvy Zvz]T,位置观测量Zs=[Zsx Zsy Zsz]T;ε为鲁棒滤波参数;Zam是姿态匹配的观测量;
步骤3)给出状态变量初始值(x0)和预测误差方差矩阵初始值(Σ0|0),给出Np个稀疏网格求积分点集(ξj,εj),其中j=1,2,…,Np;
状态变量初始值x0=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]T;
预测误差方差矩阵初始值:
系统过程噪声初始值
系统观测噪声初始值
式中:
φx0、φy0和φz0是初始数学平台失准角;
δVx0、δVy0和δVz0是初始速度误差;δSx0、δSy0和δSz0是初始位置误差;
εgx0、εgy0和εgz0是陀螺仪常值漂移初值;和是加速度计常值偏移初值;μx0、μy0和μz0是主子惯导间安装误差初值;
wgx、wgy和wgz是陀螺仪随机噪声;wax、way和waz是加速度计随机噪声;
σax、σay和σaz是姿态观测噪声标准差;σvx、σvy和σvz是速度观测噪声标准差;
σsx、σsy和σsz是位置观测噪声标准差;
根据稀疏网格求积分准则给定一组积分点集(ξj,εj)其中j=1,2,…Np,Np表示积分点集的个数;
其中Np=2n2+2n+1,n为状态变量维数;
给定满足γ>1和ε>0的鲁棒滤波参数;具体为:第一组γ1=500,ε1=0.05:
第二组γ1=100,ε1=0.01:
步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断k+1是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,否则返回初始采样过程进行下一次估计。
2.根据权利要求1所述临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤1)系统数学平台失准角误差方程、速度误差方程、位置误差方程和观测方程,具体为:
1.1)数学平台失准角误差方程
其中:
i是发射点惯性坐标系,此处也是导航坐标系;
是惯导解算的发射点惯性坐标系,即数学平台坐标系;i系依次经过三次变换可得数学平台坐标系系,三次转动角分别为:绕z轴旋转φz、绕y轴旋转φy和绕x轴旋转φx;
发射点惯性坐标系下数学平台失准角φi=[φx φy φz]T;
b是弹体坐标系,即子惯导坐标系,也可以用bs表示;原点Ob为弹体质心,Obxb轴为弹体纵轴对称轴,指向弹体头部;Obyb轴在弹体纵向对称面内,并垂直于纵轴Obzb向上;Obzb按照右手规则确定;
为子惯导解算的姿态矩阵,也可以用表示,表示弹体坐标系b到数学平台坐标系的姿态转换矩阵;
为陀螺测量误差,且为三只陀螺仪测量误差的随机常值部分,wg=[wgx wgy wgz]T为三只陀螺仪测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
1.2)速度误差方程
惯性坐标系下速度误差微分方程为,
式中:
δVi=[δVx δVy δVz]T;
是子惯导IMU的加速度计测量值;
是子惯导IMU的加速度计测量误差,且为加速度计测量误差的随机常值部分,wa=[wax way wgz]T为三只加速度计测量误差的随机噪声部分,随机噪声均假设为高斯白噪声;
是系至i系的变换阵;
δgi是引力场模型的引力加速度误差;
1.3)位置误差方程
发射点惯性坐标系下位置误差δSi微分方程为,
式中:δSi=[δSx δSy δSz]T;
1.4)姿态匹配观测方程
式中:
bm是载体坐标系即载机坐标系;
bs是子惯导坐标系;
是主惯导的姿态阵,是的转置矩阵;
—主惯导的姿态误差角,可将其视为白噪声;
是bm系到bs系的变换矩阵;
姿态匹配观测方程由观测矩阵Zdcm矩阵求得,具体为:
式中:Zam是姿态匹配的观测量,
1.5)速度匹配观测方程
Zv=Hvxk+vv (14)
式中:
vv是速度匹配等效观测噪声;
1.6)位置匹配观测方程
Zs=Hsxk+vs (15)
式中:
3.根据权利要求1所述临近空间飞行器传递对准模型不确定性的鲁棒滤波方法,其特征在于,所述步骤4)利用鲁棒稀疏网格求积分滤波经过初始采样、时间更新、重采样、量测更新和鲁棒更新过程对临近空间飞行器传递对准系统状态进行估计,并对子惯导系统进行误差修正,判断k+1是否大于等于步长L,如果是,状态估计结束,完成传递对准过程,具体为:
4.1)初始采样
4.2)时间更新
χj,k|k-1=f(γj,k-1) (17)
其中
4.3)重采样
4.4)量测更新
4.5)鲁棒更新
判断k+1是否大于等于步长L,如果是,状态估计完成,结束计算,否则返回步骤4.1)进行下一次估计;
式中:
Σk-1|k-1为预测误差方差矩阵;为状态变量;Σk|k-1为一步预测误差方差矩阵;A为Σk-1|k-1或Σk|k-1通过Cholupdate或SVD分解得到的矩阵;ξj为稀疏网格求积分点集采样点;ωj为稀疏网格求积分点集采样点对应的权重;γj,k-1为初始采样点;Np为积分点集个数;χj,k|k-1为时间更新的Sigma点;为时间更新的状态变量一步预测值;Rk为系统观测噪声矩阵;为计算系统观测噪声矩阵;Qk-1为系统过程噪声矩阵;Gk|k-1为系统过程噪声输入矩阵;为计算系统过程噪声矩阵;为重采样的Sigma点;为观测一步预测值;Σzz,k|k-1为观测一步预测误差方差矩阵;Σxz,k|k-1为协误差方差矩阵;Kk为滤波增益矩阵;为状态估计值;Σk|k为估计误差方差矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510064836.2A CN104613984B (zh) | 2015-02-06 | 2015-02-06 | 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510064836.2A CN104613984B (zh) | 2015-02-06 | 2015-02-06 | 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104613984A CN104613984A (zh) | 2015-05-13 |
CN104613984B true CN104613984B (zh) | 2018-09-21 |
Family
ID=53148542
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510064836.2A Active CN104613984B (zh) | 2015-02-06 | 2015-02-06 | 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104613984B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106403938B (zh) * | 2016-08-25 | 2019-04-09 | 北京航空航天大学 | 一种针对小型无人机多源复合振动干扰的系统滤波方法 |
CN108241380B (zh) * | 2018-01-24 | 2020-11-03 | 北京航空航天大学 | 高速无人飞行器的控制方法、装置和高速无人飞行器 |
CN111551151B (zh) * | 2020-06-04 | 2021-05-14 | 江苏集萃智能光电系统研究所有限公司 | 基于双目视觉的临近空间飞行器相对位姿测量方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7248206B1 (en) * | 2005-06-10 | 2007-07-24 | Lockheed Martin Corporation | Instantaneous multisensor angular bias autoregistration |
CN102359786A (zh) * | 2011-07-19 | 2012-02-22 | 北京航空航天大学 | 一种基于超球体采样的初始对准方法 |
CN103256942A (zh) * | 2013-04-26 | 2013-08-21 | 哈尔滨工程大学 | 传递对准中考虑杆臂补偿的变形角测量方法 |
CN103776449A (zh) * | 2014-02-26 | 2014-05-07 | 北京空间飞行器总体设计部 | 一种提高鲁棒性的动基座初始对准方法 |
CN104215244A (zh) * | 2014-08-22 | 2014-12-17 | 南京航空航天大学 | 基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法 |
-
2015
- 2015-02-06 CN CN201510064836.2A patent/CN104613984B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7248206B1 (en) * | 2005-06-10 | 2007-07-24 | Lockheed Martin Corporation | Instantaneous multisensor angular bias autoregistration |
CN102359786A (zh) * | 2011-07-19 | 2012-02-22 | 北京航空航天大学 | 一种基于超球体采样的初始对准方法 |
CN103256942A (zh) * | 2013-04-26 | 2013-08-21 | 哈尔滨工程大学 | 传递对准中考虑杆臂补偿的变形角测量方法 |
CN103776449A (zh) * | 2014-02-26 | 2014-05-07 | 北京空间飞行器总体设计部 | 一种提高鲁棒性的动基座初始对准方法 |
CN104215244A (zh) * | 2014-08-22 | 2014-12-17 | 南京航空航天大学 | 基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法 |
Non-Patent Citations (4)
Title |
---|
基于稀疏高斯积分的舰机传递对准滤波方法;梁浩等;《中国惯性技术学报》;20141031;第22卷(第5期);全文 * |
稀疏网格平方根求积分非线性滤波器;伍宗伟等;《电子学报》;20120731;第40卷(第7期);全文 * |
稀疏网格求积分滤波算法在SINS/GPS紧组合导航中的应用;程向红等;《中国惯性技术学报》;20141231;第22卷(第6期);正文第1-3节 * |
稀疏网格高斯滤波器在SINS初始对准中的应用;冉昌艳等;《中国惯性技术学报》;20131031;第21卷(第5期);正文第1-5节 * |
Also Published As
Publication number | Publication date |
---|---|
CN104613984A (zh) | 2015-05-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109596018B (zh) | 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 | |
CN105737823B (zh) | 一种基于五阶ckf的gps/sins/cns组合导航方法 | |
CN107314718A (zh) | 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航系统的轨迹修正方法 | |
CN103900574B (zh) | 一种基于迭代容积卡尔曼滤波姿态估计方法 | |
CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN103575299A (zh) | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 | |
CN102486377A (zh) | 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法 | |
CN105783943A (zh) | 一种基于无迹卡尔曼滤波的极区环境下舰船大方位失准角传递对准方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN106500693A (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
CN103884340B (zh) | 一种深空探测定点软着陆过程的信息融合导航方法 | |
CN104764467A (zh) | 空天飞行器惯性传感器误差在线自适应标定方法 | |
CN103344260A (zh) | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 | |
CN104215244B (zh) | 基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN104613984B (zh) | 临近空间飞行器传递对准模型不确定性的鲁棒滤波方法 | |
CN104359496A (zh) | 基于垂线偏差补偿的高精度姿态修正方法 | |
CN109724626A (zh) | 一种针对极区传递对准动态挠曲杆臂效应的模型补偿方法 | |
CN109059914A (zh) | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 | |
CN109211232B (zh) | 一种基于最小二乘滤波的炮弹姿态估计方法 | |
CN109029499A (zh) | 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法 | |
CN106679612A (zh) | 一种基于惯性测量匹配的非线性挠曲变形估计方法 | |
Yuan et al. | Dynamic initial alignment of the MEMS-based low-cost SINS for AUV based on unscented Kalman filter |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |