一种位置传感器和姿态传感器的导航系统及其融合方法

摘要

本发明公开了一种位置传感器和姿态传感器的导航系统及其融合方法，将互补滤波与卡尔曼滤波相结合，简称互补卡尔曼滤波，将经过低通滤波后GPS/气压计测量的位置和速度信息，高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，通过卡尔曼滤波自动调节互补滤波的权值，有效的解决手动调节互补滤波的权值的难题，实现将GPS位置信息与姿态传感器的加速度信息融合输出横向位置与速度信息，将气压计高度信息与姿态传感器输出的z轴加速度进行融合并输出z轴位置和速度信息。融合后的输出的位置和速度信息和融合前普通GPS和气压计输出的位置信息相比更加精确，并且能提供较精确的速度信息。

主权项

一种位置传感器和姿态传感器信息融合方法，其特征在于，将互补滤波与卡尔曼滤波相结合，将经过低通滤波后位置传感器测量的位置和速度信息、高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，位置传感器测量的位置信息和姿态传感器测量的加速度信息经过互补滤波与卡尔曼滤波后进行融合，得到融合后的位置信息和速度信息：1)融合后的速度信息通过以下方程计算得到：v(k｜k‑1)＝v(k‑1｜k‑1)+a_g(k)*dtP₁(k｜k‑1)＝P₁(k‑1｜k‑1)+Q$K_1\left(k\right)=\frac{P_1\left(k\right|k-1)}{P_1\left(k\right|k-1)+R_1}$$v\left(k\right|k)=(1-\frac{K_1\left(k\right)}{z+K_1\left(k\right)})*v\left(k\right|k-1)+\frac{K_1\left(k\right)}{z+K_1\left(k\right)}*v_{before}\left(k\right)$式中，v(k｜k‑1)为根据加速度信息计算得到的k时刻速度信息，v(k‑1｜k‑1)为k‑1时刻融合后的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息，dt为k‑1时刻和k时刻之间的时间间隔，P₁(k｜k‑1)为k时刻根据加速度信息计算得到的速度信息的协方差，P₁(k‑1｜k‑1)为k‑1时刻根据加速度信息计算得到的速度信息的协方差，Q为加速度信息的协方差，K₁(k)为第一卡尔曼滤波增益，v_before(k)为k时刻位置传感器测量的速度信息，R₁为v_before(k)的协方差；v(k｜k)为k时刻融合后的速度信息；2)融合后的位置信息通过以下方程计算得到：$L\left(k\right|k-1)=L(k-1|k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g\left(k\right)*dt*dt$P₂(k｜k‑1)＝P₂(k‑1｜k‑1)+Q$K_2\left(k\right)=\frac{P_2\left(k\right|k-1)}{P_2\left(k\right|k-1)+R_2}$$L\left(k\right|k)=(1-\frac{K_2\left(k\right)}{z+K_2\left(k\right)})*L\left(k\right|k-1)+\frac{K_2\left(k\right)}{z+K_2\left(k\right)}*L_{before}\left(k\right)$式中，L(k｜k‑1)为根据加速度信息计算得到的k时刻位置信息，L(k‑1｜k‑1)为k‑1时刻融合后的位置信息，v_after(k‑1)为k‑1时刻融合后的速度信息，P₂(k｜k‑1)为k时刻根据加速度信息计算得到的位置信息的协方差，P₂(k‑1｜k‑1)为k‑1时刻根据加速度信息计算得到的位置信息的协方差，K₂(k)为第二卡尔曼滤波增益，L_before(k)为k时刻位置传感器测量的位置信息，R₂为L_befo_re(k)的协方差；L(k｜k)为k时刻融合后的位置信息。