基于组网导航系统的运动体姿态估计算法

摘要

提出了基于组网导航系统运动体姿态解算的估计算法,在运动体上固定多个导航节点构建基线矢量,利用测姿主节点与其协作邻节点间共享的位置信息和节点间的测距信息,采用空间几何知识和加权最小二乘算法解算基线矢量在地理坐标系下的坐标,进而结合姿态变换矩阵进行姿态角的求解。另外,考虑运动体的动态特性,建立了关于姿态角的运动方程和量测方程,进而采用EKF算法进行姿态估计。仿真结果表明:采用EKF算法能够得到高于加权最小二乘法的估计精度,在影响姿态估计精度的多个因素中,基线长度、主节点定位误差对精度影响较大,而邻节点定位误差对精度几乎无影响。