多旋翼无人机

    返回首页    发表留言
本文作者:李德强
          第九节 位置速度估计
 
 

        速度位置估计较姿态稍稍复杂一些,但是其原理仍然是利用卡尔曼滤波进行最优状态估计。我们先来讨论速度估计的具体步骤:

        1)将加速度计所测量到的加速度乘以当前时刻距离上一时刻的时间间隔 ∆t,得到当前机体坐标系下的速度v_b增量:

        2)利用机体坐标系到本地坐标系的变换矩阵R_b^l将其转换为本地坐标系下的速度增量 ∆v_l,并将其与t_0时刻的最优估计v_0融合作为t_1时刻的预测值v_p:

        3)将GPS测量得到的速度v_m作为速度的测量值,由于GPS所测量得到的速度就是在本地坐标系下的速度,因此不需要进行坐标系转换。

        4)对t_1时刻的预测值和测量值进行数据融合,得到t_1时刻的最优估计v_1:

        接下来,我们再讨论位置估计的具体方法:

        1)先通过对加速度计读数乘以时间间隔得到速度增量,再乘以时间间隔得到位置增量 ∆p_b。同样的, ∆p_b也是在机体坐标系下的:

        2)利用机体坐标系到本地坐标系的变换矩阵R_b^l将其转换为本地坐标系下的位置增量 ∆p_l,并将其与t_0时刻的最优估计p_0融合作为t_1时刻的预测值p_p:

        3)将GPS测量得到的位置p_g,由于GPS测量得到的位置信息是在全局坐标系下的经度(longitude)、纬度(latitude)和高度(altitude),因此需要通过全局坐标系到本地坐标的变换矩阵R_g^l进行变换得到的本地坐标系下位置的测量值p_m:

        4)对t_1时刻的预测值和测量值进行数据融合,得到t_1时刻的最优估计p_1:

        需要注意的是,在对位置进行状态估计时,通常只将GPS测量到的精度和纬度进行融合,而高度的测量值来源是是气压计读数。在转换到本地坐标系当中之后,x和y来源于GPS,而z来源于气压计。另外,在对加速度进行积分得到速度时,需要在本地坐标系下移除垂向的重力加速度对速度的影响。也就是说需要移除垂向重力加速度g乘以 ∆t所得到的速度。

 

    返回首页    返回顶部
  看不清?点击刷新

 

  Copyright © 2015-2019 问渠网 辽ICP备15013245号