[发明专利]一种姿态估计方法

说明书

本发明公开了一种姿态估计方法，能够对陀螺仪、加速度计、磁力计的读数进行融合，实时的估计被测对象的四元数，进而得到其姿态角。本发明主要包括以下步骤：初始化，读取陀螺仪数据并计算四元数的预测估计，读取加速度计数据并计算动态性函数进而求取融合权重，计算加速度计引出的四元数测量值，计算磁力计引出的四元数测量值，计算四元数观测向量，计算四元数的最终估计。本发明具有较小的计算量，并且对被测对象的运动干扰有一定的鲁棒性。

技术领域

本发明涉及测量技术领域，特别是涉及一种姿态估计方法。

背景技术

四旋翼飞行器又称为四轴飞行器，是一种能够垂直升降的飞行器。因为其具有较强的机动性能、体积小、隐蔽性好等优势而被广泛应用在工业和军事上。在最近几年的研究领域中也成为了新的研究热点。四旋翼飞行器的设计主要包括姿态解算和飞行控制两部分，其中，姿态解算是飞行器设计的核心和重点。文献“周克旻,周蓉,滕婧,陈亦奇.多标记室内小型无人机定位与姿态估计方法[J/OL].计算机工程与应用:1-11”针对目前流行的基于虚拟现实(AR)技术的ArUco标记算法和颜色空间域标记算法设计了一种多标记的无人机跟踪系统。这种方法将Kinect深度摄像机固定在实验环境顶部，直接对有ArUco标记的无人机进行定位跟踪与姿态估计。这种方法免除了机载摄像机，减轻无人机重量，同时也简化了跟踪系统。但是这种方法无法在摄像机视野外工作。文献“付雷,章政,余义.基于改进型显性互补滤波的MENS姿态解算[J].自动化与仪表,2018,33(11):7-12”针对单一的传感器无法准确进行姿态估计以及低成本MENS器件易发散的问题，提出了一种改进显性互补滤波算法。该算法利用加速度计校正陀螺仪漂移引起的姿态误差，并在数据融合之前对加速度原始数据进行巴特沃斯低通滤波，同时设置缓存区保存更新后的四元数向量，减小加速度通过低通滤波器之后产生的延时对姿态估计的影响，提高姿态解算的实时性。但是，文中涉及的K_p、K_i参数的设置具有一定的盲目性，缺乏一定的理论依据。如何根据传感器噪声统计特性变化来自动调整滤波增益一直是一个难解问题。为此，文献“王立,章政,孙平.一种自适应互补滤波姿态估计算法[J].控制工程,2015,22(05):881-886”针对微型四旋翼飞行器捷联惯性导航姿态解算中低成本MEMS器件易发散导致姿态漂移问题，基于四元数姿态更新算法，提出了一种自适应互补滤波姿态估计算法。该算法利用MEMS器件中的加速度计补偿陀螺仪偏差引起的姿态漂移误差，并根据陀螺仪频率响应的截止角速度分段调整补偿系数。但是，陀螺仪的误差主要是温漂，可以通过状态扩充法解决，文中没有针对加速度计的运动噪声变化而设计自适应补偿算法。

发明内容

本发明技术克服现有技术的不足，具有较小的计算量，并且对被测对象的运动干扰有一定的鲁棒性。

为解决上述问题，本发明公开了一种姿态估计方法，具体包括以下步骤：

步骤一：令物体处于静止状态，并检测其姿态角，然后根据姿态角换算出其四元数初始化一个长度为L的队列A，并令该队列为空；初始化融合权重φ，令其为0.5；

步骤二：采集陀螺仪数据w＝[w_x,w_y,x_z]^′，计算四元数向量的预测估计如下：

其中，T表示采样时间间隔，I₄表示4×4单位向量，

步骤三：读取加速度计数据a＝[a_x,a_y,a_z]^′，并加入到队列A中，然后计算检测函数f，如下

其中，表示加速度计本体白噪声方差，g为重力加速度，l(·)表示取二范数；计算检测函数的理论方差如下：