1、第 卷 第 期年 月武 汉 大 学 学 报 信 息 科 学 版 收稿日期 ：项目资助 ：国家自然科学联合基金 （）；郑州轻工业学院博士基金 （）。第一作者 ：丁国强 ，博士 ，副教授 ，主要从事非线性最优滤波理论与算法 、导航自动化技术研究 。：文章编号 ：（）的姿态估计算法丁国强马军霞熊明周卫东 郑州轻工业学院电气信息工程学院，河南 郑州 ， 郑州轻工业学院软件学院，河南 郑州 ， 哈尔滨工程大学自动化学院，黑龙江 哈尔滨 ，摘要 ：基于飞行器载体 姿态计算精度要求 ，提出了一种 （ ）的球面径向容积卡尔曼 （ ，）姿态确定算法 。该算法把笛卡尔坐标系中状态向量变换到球坐标系中 ，通过 求积
2、计算获得 个球面径向容积点及其权值系数来逼近计算系统状态估计及其方差矩阵 ，其计算精度可达到三阶 ；采用四元数姿态建模方法构建新型状态变量与噪声向量相关的姿态方程模型，利用伪观测向量构建观测噪声与四元数相关的观测方程模型 ，设计系统噪声方差分离计算算法进行系统噪声方差计算 ，引入拉格朗日乘子算法计算四元数估计均值 ，最后利用 姿态估计仿真系统开展的 的 姿态模型算法进行仿真验证 。通过与中心差滤波 （）和无迹卡尔曼滤波 （）算法计算结果进行对比 ，可以看出 算法具有计算精度高以及数值计算稳定等特点 。关键词 ：捷联惯性导航 ；姿态确定 ；球面径向容积法 ；非线性卡尔曼算法 ；拉格朗日乘子中图法
3、分类号 ：；文献标志码 ：姿态估计是载体姿态控制的关键问题 ，一般是通过载体捷联惯导系统 （ ，）姿态模型和辅助观测信息利用高精度估计算法获得载体姿态估计结果 ，载体姿态估计精度会直接影响系统姿态 、速度及位置量的计算精度 。目前姿态实时解算方法主要有旋转矢量法 、欧拉法 、四元数法以及修正罗德里格参数法等，其中四元数法应用最为广泛 。但四元数必须进行归一化约束变换 ，该步骤限制了非线性滤波算法的应用 。对此 ，采用乘性扩展卡尔曼 （）算法处理，但是算法高阶截断误差使其无法获得理想计算结果 ；等提出了算法，来避免四元数归一化约束 。近年来 ，载体姿态的四元数卡尔曼滤波 （ ，）算法获得得到广泛
4、应用，算法采用四元数伪观测模型 ，观测噪声与四元数向量是乘性关系 ，递推更新计算中四元数向量被强制归一化获得单位四元数向量 。目前针对非线性系统应用的采样逼近卡尔曼算法获得快速发展 ，主要有无迹卡尔曼滤波 （ ，）算法、中心差分滤波 （ ，）算法以及类似的二阶插值滤波 （）算法和滤波 （）算法等 。算法复杂 ，计算负担重 ；算法在高维场合应用中易于发散 ；和算法数值计算稳定性较差 。设计高精度稳定数值计算的最优滤波算法成为载体姿态估计的关键问题 ，而基于球面径向容积积分逼近卡尔曼滤波算法的计算精度可以达到三阶，以上 ，计算稳定性明显优于与算法 。本文基于姿态估计高精度和数值计算稳定性要求提出了
5、一种四元数姿态估计的球面径向容积卡尔曼 （）模型算法 ，设计了新型四元数观测方程 。姿态估计方程和观测方程具有相同的噪声项表达式 。利用姿态仿真平台开展模型算法验证研究 。仿真结果表明了的对准模型算法的有效性及计算优势 。武 汉 大 学 学 报 信 息 科 学 版 年 月 非线性最优估计算法 球面径向容积逼近算法在高斯域内考虑函数（）数值积分 ：（ ）（ ） （ ） 式中 ，维状态向量，利用极坐标系中的方向向量和球径表示为，其中，且有，），向量定义了单位超球面，则有 ：（ ）（ ） （ ） （ ） （） （）定义球面积分（） （ ） （），重新整理式 （）为 ：（ ）（ ） （） （ ） 按照
6、数值积分 ，取个高斯求积点获得径向积分逼近计算 ：（）（）（）同样 ，取个求积点获得球面积分逼近 ：（ ） （） （ ）从而获得球面径向容积积分逼近计算 ：（） （ ）（）若取加权指数函数（）和概率密度分布加权函数（）有 ：（） ，（） ；，（ ），且槡槡，那么 ，有 ：（）（）槡槡（ ）（）取，可获得三阶球面径向数值逼近计算 ： （ ） 槡（ ） （ ）槡（ ）槡（ ）槡（ ） 槡（ ）槡（ ） 其中 ， 槡（ ），则个求积点为 ：槡，槡，烅烄烆（）式中 ，表示第列单位向量 ，其第个元素为，有容积逼近求积式 ：（） ；，（ ）槡（ ）槡（ ）（）式中 ，为权值系数 ，。 最优估计算法设计考虑
7、非线性系统离散化模型方程 ： ，（ ） ，（ ）（）式中 ，和表示系统状态向量和量测向量 ；和是相互独立的过程噪声和观测噪声 ，具有（，），（，）分布 。若观测向量序列，系统状态向量和观测向量后验密度函数服从高斯分布 。若时刻系统状态向量估计为，估计方差矩阵为，则对其实施乔里斯基分解 ： ， ，利用式 （）的求积点构造状态向量和过程噪声的采样点及其权值为 ：， ， ，烅烄烆其中 ，。那么 “，；“，经由式 （）状态函数映射后获得的采样点更新值为 ：， ，（ ），则系统状态向量的预测均值为 ： ，（ ），系统状态向量预测方差为 ： ，对系统状态向量预测方差和观测噪声方差矩阵同时展开乔里斯基分解
8、，获得乔里斯基因子，则可以构造采样点为 ：， ， ，烅烄烆经由式 （）观测方程映射获得采样点更新值 ：第 卷第 期 丁国强等 ：的 姿态估计算法， ，（ ），那么观测向量预测值为 ： ，（ ） ，其预测方差矩阵为 ：， ，同时状态向量和观测向量交叉协方差为 ：， ，那么 ，由 （，）（，） （ ）， ， ，可以获得系统状态向量时刻的估计值及其方差 。 姿态估计模型算法 姿态方程四元数是表示载体姿态旋转的一类特殊四维矢量 ，定 义 四 元 数 为，其 中，为向量分量 ，为实数分量 ，其运算要求必须满足规范化限制 ，即有 ： 那么由四元数表示的姿态矩阵（）（）（）（）（）式中 ，（） ；（） ；是
9、单位矩阵 ；#为斜对称矩阵。根据四元数微分方程 ，结合式 （）进一步得到系统姿态四元数微分方程 ： （ ）（ ） （） （ ）（） （）式中 ，（） 。系统姿态四元数微分方程中的角速率由载体坐标系中三轴速率陀螺测量得到 ，陀螺测量得到的角速率方程为 ：珚（） （）（）（）（） （）（）式中 ，珚（）为陀螺量测输出角速率向量 ；（）为陀螺随时间的漂移量 ；（）和（）分别为陀螺测量噪声和陀螺漂移的互不相关的高斯随机白噪声 ，满足（）（，）和（）（，），从而姿态四元数微分方程为 ： （ ）（珚） （ ） （）（） 姿态四元数观测方程利用飞行器携带的辅助观测设备如星敏感器等可以获得精确的参考观测向量，
10、若在载体系中得到的观测向量为珚，那么由观测模型，有 ：珚（ ）式中 ，珚是一个具有观测噪声的维观测向量 ；观测噪声是观测传感器的误差向量 ，具有（，）分布高斯白噪声 。根据文献 可以得到的伪观测模型方程 ：（ ） （）（）式中 ，珚（ ）珚（ ）珚（ ） 。则由式 （）展开次观测获得的观测方程 ： ， ，珚（ ）（）式中 ， ，珚（ ） ， ；珚珚，珚，珚 。伪观测方程中的第一项是四元数向量的线性函数 ，其矩阵秩为。伪观测方程中噪声项（）是四元数相关项 ，与四元数方程噪声项相似 ，可以方便开展观测噪声方差计算 。 模型噪声方差分解算法若考虑观测噪声与函数向量相关的观测方程改为 （ ），那么状态
11、向量均值为： （ ） （ ） （ ）（ ）定义外积计算式 ： （ ）对其两边求期望值得到 ： （ ） （ ） （ ） （ ） （）式中 ， ，式 （）可以简化为 ： （ ） 其中 ， （ ）（ ） ，可简化为 ：武 汉 大 学 学 报 信 息 科 学 版 年 月（ ）（ ） （） （ ）（ ）（ ）其中 ，表示乘积 ，且对于，有（ ）成立 ，从而可得到 ： （ ） （ ）（ ）（ ）那么 ，向量的噪声方差为 ： （ ） （ ）（ ）（ ）（ ） （ ） （ ） （ ）（ ）（）若噪声向量 是具有零均值高斯白噪声 ，则式 （）计算可以简化为 ： （ ）（ ） 四元数加权均值算法若四元数由时刻到时
12、刻更新操作产生误差四元数为 ：， ， ，其中 ，是时刻更新计算的四元数 ，为四元数的一步预测值 ，那么，向量部分，可表示为 ：， ， ， 考虑算法中四元数均值计算加权系数 ，可以把四元数加权求均值问题转化为代价函数的极值计算问题 ，考虑四元数代价函数 ，（ ），（ ）其中 ， ， 由于，设拉格朗日算子，利用拉格朗日乘法构造代价函数：珔（ ）（ ）那么 ，可以通过计算珔函数最小值 ，得到 ： 代价函数珔最小矩阵的最小特征值的特征向量就是所求均值四元数 。 系统模型仿真研究本文采用姿态估计平台系统开展系统模型算法研究 ，光纤陀螺仪测量载体旋转角速度 ，利用两个光轴垂直安装的星敏感器获得星光矢量观测
13、量 。仿真初始条件设置为载体初始位置为北纬，东经，初始速度设为，载体初始姿态角，陀螺常值漂移为，随机漂移噪声设为；星敏感器测量噪声为，系统仿真时间。、算法同属于贝叶斯最优滤波算法 ，但由于采用不同的采样策略 ，为 验 证姿态四元数模型算法的有效性及其数值计算比较的优势 ，本文针对系统模型开展种算法对比仿真实验 ，从而获得的图的纵摇姿态失准角估计误差，图的横摇姿态失准角估计误差数据和图的偏航失准角估计误差数据以及表的估计误差数据 。从仿真比较数据可以看出 ，四元数姿态估计模型算法的计算精度明显优于和算法计算精度 ，小角度的纵摇角和横摇角估计得到的图和 图估计误差数据中算法估计精度在之内 ，而和算
14、法分别为和，相比于算法 ，和算法的数值计算稳定性明显降低 ；同样在初始偏航失准角取值为情况下 ，算法 计 算 精 度 优 于 其 余 两 种 算 法 ，且算法数值收敛很快 ，稳定性也优于其余两种算法 ，这说明算法在姿态估计计算方面具有精度优势 ，原因在于算法采用球面径向容积法可以达到三阶计算精度 ，而算法的斯特林插值法和算法的无迹变换法的计算精度仅能达到二阶 。图 纵摇角估计误差曲线 第 卷第 期 丁国强等 ：的 姿态估计算法图 横摇角估计误差数据曲线 图 偏航角估计误差数据曲线 另外 ，从表可以看出 ，算法的迭代时间小 ，算法收敛时间与其相近 ，算法计算周期较长 ，原因在于和算法仅采用个采样
15、点 ，而算法需要个采样点 ，且和算法仅有一个采样点调整参数 ，而算法需要个参数调整采样点权值系数 ；同时系统两种噪声方差采用同样计算式展开计算 ，该措施也会影响到算法计算效率 。表 姿态角估计误差 算法 （） （） （） 表 迭代周期时间数据 估计算法 迭代时间 结语基于姿态估计高精度稳定数值计算要求 ，利用球面径向积分数值逼近法设计计算精度达到三阶的算法 ，实现提高模型算法计算精度目的 ；利用四元数构建一类乘性噪声的姿态四元数误差模型 ，系统方程中噪声与四元数状态向量具有乘性关系 ，利用噪声分离算法可用较小计算负担获得稳定计算性能 。经 由姿态仿真平台验证 ，相比于和算法 ， 姿态模型算法具
16、有计算精度高且计算稳定特点 ，在飞行器载体姿态估计应用中具有适用价值 。参考文献 ， ， ： ，：（张天光 ，王秀萍 ，王丽霞捷联惯性导航技术 （第二版 ）第二版 北京 ：国防工业出版社 ，：） ， ，（）： ， ，（）： ， ，（）： ， ， ，（）： ， ，（）： ， ， （ ），（）：（丁国强 ，周卫东 ，郝燕玲 传递对准误差模型及其 算法 华中科技大学学报 （自然科学版 ），（）：） ， ， ， ，（）：（丁国强 ，周卫东 ，郝燕玲 插值非线性滤波技术在大失准角传递对准中的应用研究 弹箭与制导学报 ，武 汉 大 学 学 报 信 息 科 学 版 年 月，（）：） ， ， ，（）： ，（）： ， ， ，（）： ， ，（）： ， ，（）：（刘志平 ，张书毕方差 协方差分量估计的概括平差因子法 武汉大学学报 信息科学版 ，（）：） ， ， ， ， ， ， ， ， ，： （） ， ， ， ， ， ； ： ； ； ； ； ： ， ， ： ： ，； ，
