基于istssrckf的惯性导航初始对准方法
【技术领域】
[0001] 本发明主要涉及导航技术领域，尤其涉及一种基于ISTSSRCKF的惯性导航初始对 准方法。
【背景技术】
[0002] 惯性导航系统进入工作状态之前先要进行初始对准，捷联惯导系统将惯性传感 器一一陀螺仪和加速度计直接与载体固联，通过运算得到计算平台来代替物理平台，因此 捷联惯导系统的初始对准问题就是求解初始时刻的姿态矩阵。SINS初始对准直接影响捷联 系统的导航性能，即导航精度和反应时间。水下自主航行器（AUV)是一种综合了人工智能 和其它先进计算技术的任务控制器，要使水下航行器完成预定的使命，离不开水下导航技 术，它是决定水下航行器技术发展与应用的瓶颈问题。在实际应用环境下，特别是在大失准 角和剧烈晃动条件下，无法使用以线性小失准角为对准模型的经典卡尔曼滤波方法进行滤 波，只能建立非线性对准模型，而传统的EKF、UKF等非线性滤波算法存在高维状态下对准 精度较低和数值不稳定问题，而且应对不确定因素能力差的缺点，因此发明有效应对复杂 环境，具有一定强跟踪能力的SINS非线性晃动基座下的对准方法具有重要的意义。
【发明内容】
[0003] 发明目的：为了克服现有非线性对准技术中存在的不足，本发明提供了一种提高 SINS对准精度的基于ISTSSRCKF的惯性导航初始对准方法。
[0004] 技术方案：为解决上述技术问题，本发明提供了一种基于ISTSSRCKF的惯性导航 初始对准方法，其步骤包括如下：
[0005] 步骤1 :建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS 的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型：
[0006] 其中，所述建立SINS非线性误差模型过程如下：
[0007] 步骤I. 1 :以东北天地理坐标系作为导航坐标系n，以载体右前上方向矢 量右手定则构成坐标系作为载体坐标系b，真实姿态角为=p，y/f，真实速 度为<v/f，真实地理坐标为p=[L人H]T;SINS实际解算出的姿态为 參=Pf 速度为可地理坐标为戶=[Z尤及]f，记由SINS解算 的地理位置建立的坐标系为计算导航坐标系n'，定义SINS姿态角和速度误差分别为? = =C-<，则（J)、SVn的微分方程如下：
【主权项】
1. 一种基于ISTSSRCKF的惯性导航初始对准方法，其特征在于，包括以下步骤： 步骤1 :建立大失准角状态下的SINS晃动基座对准模型，所述对准模型包括SINS的非 线性误差模型、非线性滤波状态模型和非线性滤波量测模型： 其中，所述建立SINS非线性误差模型过程如下： 步骤I. 1 :以东北天地理坐标系作为导航坐标系n，以载体右前上方向矢量右手定则构 成坐标系作为载体坐标系b，真实姿态角为P = R，真实速度为< =R < 真实地理坐标为P=[L λ H]T;SINS实际解算出的姿态为多f 速度为 ^ 地理坐标为戶=P $及T，记由SINS解算的地理位置建立的坐标系为 计算导航坐标系V，定义SINS姿态角和速度误差分别为> = 则φ、 δ vn的微分方程如下： φ = ^0'[(/-?：)?： + €：；δω：η -C：{sb + ω?)] Sf =[Ι -(C:'f]C:'fb +(C:'fC；；'(Vb+ω:)-(2δω;: +δω:Χ -(2?ζ +?:")χδν" 其中，Φ = [Φε Φη 纵摇角、横摇角和航向角误差，δνη= [δνε δνη 3VJT 为东向、北向和天向速度误差，d =[< 4 4]'|为b系下三轴陀螺的常值误差，为b系 下三轴陀螺的随机误差，V6=[▽丨▽丨为b系下三轴加速度计的常值误差，<为b系下 三轴加速度计的随机误差，.严为加速度计的实际输出，f为SINS解算的速度，（?为计算 的导航坐标系下的旋转角速度，?：：为计算的地球旋转角速度，屯""为计算的导航坐标系相 对地球的旋转角速度，却为对应<、< 的计算误差，< 为η系依次旋 转角度Φρ Φη、得到n'系所形成的方向余弦矩阵，Cf为b系到n'系状态转移矩阵， 即计算的姿态矩阵，Cw为欧拉角微分方程的系数矩阵，上标T表示转置，矩阵C,f和Cw具体 为：
C,, 所述非线性滤波状态模型建立过程如下： 步骤1. 2 :取SINS的欧拉平台误差角Φε、Φη、<K，速度误差δ Ve、δ Vn，b系下 的陀螺常值误差<、4、<，b系下的加速度计常值误差< 构成状态向量 x = 式爽~5v" 4 4 4 Vt 在晃动基座下5<和近似为零， 则非线性滤波的状态方程可简化为：
其中，加"、??只取前两维状态，ω(〇 = [〇g ω3 〇1Χ3 〇1Χ2]τ*零均值高斯白噪声过 程，将该非线性滤波状态方程简记为： x{t) = f{x,t) + (〇{t) 所述非线性量测方程的建立如下： 步骤1. 3 :记在b系下的真实速度为vb，在b系下的实际速度为P，利用SINS解算的姿 态矩阵把P转换成F，以T1和K中的东向和北向速度分量作为匹配信息源，则非线性滤波 的量测方程为：
其中，取z的前两维为观测值，V为零均值高斯白噪声过程，并将该连续非线性滤波量 测方程简记为： z (t) = h (X，t) +V ⑴ 步骤2 :以采样周期Ts作为滤波周期，并以T s为步长进行离散化，将 小）=/(.以)+离散化为 Xk= Xh+[f(XH，V1) +ω (V1)] Ts，可得状态方程： Xk= f (Xk-i) + wk-i 将 z (t) = h (X，t) +V (t)离散化为 Zk= h (X k, tk) +V (tk)，可得量测方程： Zk= h (x k) +vk 由状态方程式和量测方程式组成如下离散非线性系统：
式中，Xk为k时刻系统的状态向量；Zk为k时刻系统状态的量测值；随机系统噪声ω k~ N(0, Qk);随机观测噪声vk~N(0, R k); 步骤3 :利用当前量测值zk减去相同时刻的量测预测值毛/w得到当前时刻的残差ε k， 依据强跟踪原理计算次优渐消因子λ,，然后利用λ,修正滤波时间更新过程； 步骤4 :以时间更新的预测估计值4η和预测方差Pk/k_$初始值进行迭代过程，进行 最大迭代次数Nmax次迭代，完成量测更新； 步骤5 :利用滤波器滤波获得的欧拉平台角估计值么和速度估计值<5%修正SINS解算 的姿态矩阵Cf和速度?ξ'，将修正之后的值作为下一次捷联解算的初始值，利用当前获得的 陀螺常值误差估计值.苟和加速度计的常值误差估计值修正下一时刻的陀螺输出武和 加速度计的输出/6，具体修正公式按下式进行计算： C：=C：,C^, ν：^ν：-δν'1，4^?Ι-ε\, fb=fb-vbk 若精度达到预设的初始对准要求则结束，否则继续循环执行步骤2~5,直至初始对准 结束。
2.根据权利要求1所述的基于ISTSSRCKF的惯性导航初始对准方法，其特征在于：所 述2)中，所述依据得到的离散化模型在容积卡尔曼滤波的框架下进行时间更新步骤具体 为： 步骤2. 2 :设置系统状态初始值元=〇,初始误差协方差阵： P0= diag{(l° )20- )2(10° )2(0· 1)2(0.1)2 (0.0Γ Λ)2(0·0Γ Λ)2(0·0Γ /h)2(100yg)2(100yg)2}*10 并对Ptl进行Cholesky分解，得到初始误差协方差阵的特征平方根S μ 步骤2. 3 :采用SSR规则选取采样点，取如下一组向量： a』= [a j;1 aj>2…aj，n]T, j = 1，2, · · ·，η+1，其中η为状态量的个数，贝丨J
选用ξ i表示第i个容积点，m = 2(η+1)个容积点为：
式中η为状态量的个数，本文中η = 10 ;简记非线性多维积分
步骤2. 4 :计算状态一步预测值和一步预测误差协方差阵Pk/k_1: 通过Cholesky分解误差协方差阵Pm1得到S k-1/k-1: Pk-l/k-1 - S k-l/k-lS k-l/k-1 计算 Cubature 点（i = 1，2, ···，n+1 ;m = 2 (n+1))： ~ ^k-l/k-?ζ? Xk-l/k-l 通过状态方程传播Cubature点： X i，k/k-1= f (X i，k-l/k-I) 估计k时刻的状态预测值： 估计k时刻的状态误差协方差：
3. 根据权利要求1所述的基于ISTSSRCKF的惯性导航初始对准方法，其特征在于：所 述步骤3中： 所述依据强跟踪原理计算次优渐消因子λ k运算过程如下： 利用当前量测值Zk减去相同时刻的量测预测值4^得到当前时刻的残差为： ￡k =Zk ~ 取实际输出序列的协方差1·
式中P为遗忘因子，取值范围一般为0.95S P <0.98; 计算矩阵：
式中Pzz，k/k-i为输出预测协方差阵，Pxz，k/k-i为互协方差阵。 计算次优渐消因子Ak:
式中tr[口]表示矩阵的迹。
4. 根据权利要求1所述的基于ISTSSRCKF的惯性导航初始对准方法，其特征在于：所 述步骤4中的迭代CKF算法： 步骤4. I. 1 :利用步骤2. 4中各式，计算状态一步预测值毛^和一步预测误差协方差阵 Pk/k-l; 步骤4. 1. 2 :计算渐消因子λ k，通过Cholesky分解Pk/k_1: Pk/k-l - S k/k-lS k/k-1 计算 Cubature 点（i = 1，2,…，n+1 ;m = 2 (n+1))： XiJdk-1 = Snit + xVk-I 通过观测方程传播Cubature点： Zi，k/k-l - h(X Lk/H) 估计k时刻的观测预测值： 估计自相关协方差阵：
估计互相关协方差阵：
根据步骤3各式计算渐消因子Ak; 步骤4. 1. 3 :利用求出的渐消因子λ k，对状态预测协方差阵P1^1进行如下变换：
步骤4. 2. 1 :量测更新是以为初始值的迭代过程。记第i次迭代的估计值 和方差为却}和^^ = 〇,U,…，况·.): 步骤4. 2. 2 :产生新的因式分解和容积点，chol (□)表示矩阵的Cholesky分解：
其中％表示第j个容积点； 步骤4. 2. 3 :用步骤4. 1. 3中的Pw1重新计算步骤4. 1. 2各式，可依次得到P Akm Ρχζ，k/k-1 ^ 计算第i次迭代的状态和方差估计：
步骤4. 2. 4 :若迭代终止时迭代次数为N，则k时刻的状态估计和协方差为： 义-々.iN) p' - p、N) xk ~ H ，rk - rk 〇
【专利摘要】本发明公开了一种基于迭代强跟踪球面最简相径容积卡尔曼滤波(Iterated Strong Tracking Spherical Simplex-Radial Cubature Kalman Filter,ISTSSRCKF)的初始对准方法，采用SSR规则选取CKF的容积点；将强跟踪滤波中的渐消因子引入到CKF的时间和量测更新方程之中；再将Gauss-Newton迭代算法引入并改进迭代过程中相应的新息方差和协方差。对于在大失准角和晃动基座条件下的初始对准问题，本发明采用SSR规则采样减少了高阶CKF中过多的采样点，引入强跟踪算法克服了模型不准确时CKF性能下降的问题，使用迭代过程充分利用最新量测信息，从而有效地降低状态估计误差，获得优于标准CKF的初始对准精度。
【IPC分类】G01C21-16
【公开号】CN104655131
【申请号】CN201510063634
【发明人】徐晓苏, 田泽鑫, 张涛, 刘义亭, 孙进, 姚逸卿
【申请人】东南大学
【公开日】2015年5月27日
【申请日】2015年2月6日