一种两级分离融合的姿态和航向估计方法技术

本发明专利技术公开了一种两级分离融合的姿态和航向估计方法，包括：建立系统的卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；f为前后时刻状态向量之间的传递函数；解算上述卡尔曼滤波方程得到状态向量根据计算得到俯仰角θ和横滚角φ。本发明专利技术方法将姿态角和航向角的解算进行两级分离计算，构建的线性滤波方程可以避免扩展卡尔曼滤波和无迹卡尔曼滤波等非线性滤波方法的使用，将极大降低系统计算复杂度。同时，第一级卡尔曼滤波负责利用陀螺仪和加速度计进行姿态角的解算，磁传感器不参与第一级滤波，排除了磁传感器所受干扰对于姿态角估计的影响。

【技术实现步骤摘要】

本专利技术属于惯性导航
，更具体地，涉及一种两级分离融合的姿态和航向估计方法。
技术介绍
在航空航天、车辆导航、机器人和平台稳定控制等诸多领域，航向与姿态是实现运动物体控制的关键参数。对于飞机，没有准确的实时航向与姿态信息，飞机就会失去控制，不能平稳而安全地到达目的地；对于卫星，没有准确的实时姿态信息，就不能按照程序打开太阳能电池帆板，严重影响卫星的定位；对于机器人，需要及时获取自身的姿态信息，并反馈给“大脑”(机器人控制程序)，进而按照程序执行任务，例如越过障碍或者做出特定的动作。航向姿态参考系统(Attitude and heading reference system，AHRS)利用惯性传感器，如陀螺仪、加速度计等，测量物体的惯性参数，通过特定的数据融合算法得到运动物体的姿态和航向参数。航姿参考系统本身不对外发送任何光、电、声等信号，也不接收外界信号，属于完全自主工作，隐蔽性好，抗干扰能力强，可全地域、全天时、全天候工作。以往的平台式惯性导航系统需要复杂的物理平台，体积大，重量大，可靠性差且不易于维护。捷联惯性导航系统依靠设定的导航坐标系构建姿态解算算法，省去了复杂的物理结构平台。并且随着微机电系统(Micro-Electro-Mechanical System，MEMS)技术的不断发展，基于MEMS技术的陀螺仪、加速度计等惯性传感器的精度越来越高，体积越来越小而且成本越来越低，使得捷联惯导系统的发展更加迅速，应用范围更加广泛。姿态更新解算是捷联惯导系统的关键算法，传统的姿态更新算法有欧拉角法、方向余弦法和四元数法。其中四元数法由于算法简单、计算量小被广泛应用于工程实际当中。MEMS陀螺仪存在固有零漂，长时间进行积分运算存在累积误差，不便于单独使用。加速度计和地磁传感器组成的电子罗盘可以解算准静态下物体的欧拉姿态角，由于此方法不需要进行积分运算，因此不存在累积误差，但是解算的姿态角动态响应差，无法在动态条件下应用。目前国内外已经开发多种将两类器件相结合的数据融合方法，出现了包括扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、模糊算法、互补算法在内的多种数据融合方法，使得可以通过MEMS陀螺仪、加速度计和地磁传感器测量数据融合得到精确的被测物体姿态和航向。如附图1所示，传统的航姿参考系统(AHRS)通常采用非线性的扩展卡尔曼滤波(EKF)算法对陀螺仪、加速度计和磁传感器的输出进行数据融合，为控制和导航系统提供运动物体相对于惯性坐标系的姿态和航向信息。相比于线性卡尔曼滤波算法，EKF对非线性函数的Taylor展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性应用于非线性系统估计中。然而该方法也带来了两个缺点：首先，当状态方程和测量方程属于强非线性时EKF违背局部线性假设，Taylor展开式中被忽略的高阶项将带来大的误差，此时EKF算法可能会使滤波发散；另外，由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵，并且涉及高阶矩阵求逆，其繁琐的计算过程导致算法实现相对困难。EKF算法中的这些复杂运算增加了系统航姿信息的解算时间，降低了系统航姿信息的更新计算频率，对系统硬件中信号处理单元的处理能力有较高的要求。此外，现有技术在进行航向和姿态估计时，将陀螺仪、速度计及磁传感器同时用于航向和姿态解算，而姿态角(俯仰角和横滚角)的解算并不需要磁传感器，仅解算航向角的时候需要用到磁传感器。这无疑增加了系统的数据处理负荷。而且，姿态系统中集成的磁传感器很容易受到周围附加磁场的干扰，将加速度计和磁传感器的输出值同时带入滤波器融合，会对姿态角的解算产生不利影响，降低估计精度。
技术实现思路
针对现有技术的以上缺陷或改进需求，本专利技术提供了一种两级分离融合的姿态和航向估计方法。其目的在于通过构建线性的卡尔曼滤波方程，降低系统计算复杂度、减少时间消耗，由此解决现有估计方法中计算复杂度较高、计算时间消耗较多的的技术问题。为实现上述目的，本专利技术提供了一种两级分离融合的姿态和航向估计方法，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，包括：建立航姿参考系统的线性卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；f为前后时刻状态向量之间的传递函数；解算上述卡尔曼滤波方程得到状态向量根据上述状态向量计算得到俯仰角θ和横滚角φ，其中： θ = - arctan ( g x b ( g y b ) 2 + ( g z b ) 2 ) , φ = arctan ( g y b g z b ) . ]]>特别注意的是，本专利技术的一个实施例中，所述卡尔曼滤波状态方程和观测方程均为线性方程。本专利技术的一个实施例中，所述方法还包括：结合磁传感器测量值得到航向角ψ，其中 ψ = arctan ( - m y c o s φ + m z s i n φ m 本文档来自技高网...

【技术保护点】
一种两级分离融合的姿态和航向估计方法，其特征在于，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，包括：建立航向和姿态参考系统的卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；A表示状态向量之间的传递矩阵；解算上述卡尔曼滤波方程得到状态向量根据上述状态向量计算得到俯仰角θ和横滚角φ，其中：θ=-arctan(gxb(gyb)2+(gzb)2),φ=arctan(gybgzb).]]>

【技术特征摘要】
1.一种两级分离融合的姿态和航向估计方法，其特征在于，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，包括：建立航向和姿态参考系统的卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；A表示状态向量之间的传递矩阵；解算上述卡尔曼滤波方程得到状态向量根据上述状态向量计算得到俯仰角θ和横滚角φ，其中： θ = - a r c t a n ( g x b ( g y b ) 2 + ( g z b ) 2 ) , φ = a r c t a n ( g y b g z b ...

【专利技术属性】
技术研发人员：张生志，刘超军，罗璋，余帅，
申请(专利权)人：武汉元生创新科技有限公司，
类型：发明
国别省市：湖北;42