SINS的SRC_KF姿态估计算法_丁国强

《SINS的SRC_KF姿态估计算法_丁国强》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、第４１卷第３期武汉大学学报·信息科学版Ｖｏｌ．４１Ｎｏ．３２０１６年３月ＧｅｏｍａｔｉｃｓａｎｄＩｎｆｏｒｍａｔｉｏｎＳｃｉｅｎｃｅｏｆＷｕｈａｎＵｎｉｖｅｒｓｉｔｙＭａｒｃｈ２０１６ＤＯＩ：１０．１３２０３／ｊ．ｗｈｕｇｉｓ２０１４０１１５文章编号：１６７１－８８６０（２０１６）０３－０３６７－０６ＳＩＮＳ的ＳＲＣ－ＫＦ姿态估计算法丁国强１２１３马军霞熊明周卫东１郑州轻工业学院电气信息工程学院，河南郑州，４５０００２２郑州轻工业学院软件学院，河南郑州，４５０００２３哈尔滨工程大学自动化学院，黑龙江

2、哈尔滨，１５０００１摘要：基于飞行器载体ＳＩＮＳ姿态计算精度要求，提出了一种ＳＩＮＳ（ｓｔｒａｐ－ｄｏｗｎｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ）的球面径向容积卡尔曼（ｓｐｈｅｒｉｃａｌ－ｒａｄｉａｌｃｕｂａｔｕｒｅＫａｌｍａｎｆｉｌｔｅｒｉｎｇ，ＳＲＣ－ＫＦ）姿态确定算法。该算法把笛卡尔坐标系中状态向量变换到球坐标系中，通过Ｇａｕｓｓ－Ｈｅｒｍｉｔｅ求积计算获得２ｎ个球面径向容积点及其权值系数来逼近计算系统状态估计及其方差矩阵，其计算精度可达到三阶；采用四元数姿态建模方法构建新型ＳＩＮＳ

3、状态变量与噪声向量相关的姿态方程模型，利用伪观测向量构建观测噪声与四元数相关的观测方程模型，设计系统噪声方差分离计算算法进行系统噪声方差计算，引入拉格朗日乘子算法计算四元数估计均值，最后利用ＳＩＮＳ／ＣＣＤ姿态估计仿真系统开展的ＳＩＮＳ的ＳＲＣ－ＫＦ姿态模型算法进行仿真验证。通过与中心差滤波（ＣＤＫＦ）和无迹卡尔曼滤波（ＵＫＦ）算法计算结果进行对比，可以看出ＳＲＣ－ＫＦ算法具有计算精度高以及数值计算稳定等特点。关键词：捷联惯性导航；姿态确定；球面径向容积法；非线性卡尔曼算法；拉格朗日乘子中图法分类号

4、：Ｐ２２３；ＴＰ２７３文献标志码：Ａ姿态估计是载体姿态控制的关键问题，一般算法获得快速发展，主要有无迹卡尔曼滤波（ｕｎ－［６］是通过载体捷联惯导系统（ｓｔｒａｐ－ｄｏｗｎｉｎｅｒｔｉａｌｓｃｅｎｔｅｄＫａｌｍａｎｆｉｌｔｅｒ，ＵＫＦ）算法、中心差分ｎａｖｉｇａｔｉｏｎｓｙｓｔｅｍ，ＳＩＮＳ）姿态模型和辅助观测滤波（ｃｅｎｔｒａｌｄｉｖｉｄｅｄＫａｌｍａｎｆｉｌｔｅｒ，ＣＤＫＦ）算［７］［８］信息利用高精度估计算法获得载体姿态估计结法以及类似的二阶插值滤波（ＳＤＤＦ）算法和［９］果，载体姿态估计精度会

5、直接影响系统姿态、速度Ｇａｕｓｓ－Ｈｅｒｍｉｔｅ滤波（ＧＨＫＦ）算法等。ＧＨＫＦ及位置量的计算精度。目前ＳＩＮＳ姿态实时解算算法复杂，计算负担重；ＵＫＦ算法在高维场合应方法主要有旋转矢量法、欧拉法、四元数法以及修用中易于发散；ＣＤＫＦ和ＳＤＤＦ算法数值计算稳［１］正罗德里格参数法等，其中四元数法应用最为定性较差。设计高精度稳定数值计算的最优滤波广泛。但四元数必须进行归一化约束变换，该步算法成为载体姿态估计的关键问题，而基于球面骤限制了非线性滤波算法的应用。对此，Ｍａｒｋ－径向容积积分逼近卡尔曼滤波算

6、法的计算精度可［２］［１０，１１］ｌｅｙ采用乘性扩展卡尔曼（ＭＥＫＦ）算法处理，但以达到三阶以上，计算稳定性明显优于ＵＫＦ是ＭＥＫＦ算法高阶截断误差使其无法获得理想与ＣＤＫＦ算法。［３，４］计算结果；Ｃｒａｓｓｉｄｉｓ等提出了ＵＳＱＵＥ算法本文基于ＳＩＮＳ姿态估计高精度和数值计算来避免四元数归一化约束。近年来，载体姿态的稳定性要求提出了一种ＳＩＮＳ四元数姿态估计的四元数卡尔曼滤波（ｑｕａｔｅｒｎｉｏｎＫａｌｍａｎｆｉｌｔｅｒｉｎｇ，球面径向容积卡尔曼（ＳＲＣ－ＫＦ）模型算法，设计［５］ＱＫＦ）算法获

7、得得到广泛应用，ＱＫＦ算法采用了新型四元数观测方程。姿态估计方程和观测方四元数伪观测模型，观测噪声与四元数向量是乘程具有相同的噪声项表达式。利用ＳＩＮＳ／ＣＣＤ性关系，递推更新计算中四元数向量被强制归一姿态仿真平台开展模型算法验证研究。仿真结果化获得单位四元数向量。表明了ＳＩＮＳ的ＳＲＣ－ＫＦ对准模型算法的有效性目前针对非线性系统应用的采样逼近卡尔曼及计算优势。收稿日期：２０１４－０２－１７项目资助：国家自然科学联合基金（Ｕ１２０４６０３）；郑州轻工业学院博士基金（２０１１ＢＳＪＪ００４８）。第一作

8、者：丁国强，博士，副教授，主要从事非线性最优滤波理论与算法、导航自动化技术研究。ｄｉｎｇｌｙｉｔ＠１６３．ｃｏｍ３６８武汉大学学报·信息科学版２０１６年３月烄槡ｎｅｉ，ｉ＝１，２，…，ｎ（３）１ＳＲＣ－ＫＦ非线性最优估计算法ξｉ＝烅烆－槡ｎｅｉ－ｎ，ｉ＝ｎ＋１，ｎ＋２，…，２ｎ式中，ｅｉ表示第ｉ列单位向量，其第ｉ个元素为１，１．１球面径向容积逼近算法有容积逼近求积式：在高斯域内考虑函数Ｆ（Ｘ）数值积分：２ｎ１ＴＦ（Ｘ）Ｎ（Ｘ；η，Σ）ｄＸ≈Ｆη＋槡Σｉ＝Ｈ（Ｆ（）Ｘ）