CN109459005A - 一种姿态估计方法 - Google Patents

Abstract

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

Description

一种姿态估计方法

技术领域

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

背景技术

四旋翼飞行器又称为四轴飞行器，是一种能够垂直升降的飞行器。因为其具有较强的机动性能、体积小、隐蔽性好等优势而被广泛应用在工业和军事上。在最近几年的研究领域中也成为了新的研究热点。四旋翼飞行器的设计主要包括姿态解算和飞行控制两部分，其中，姿态解算是飞行器设计的核心和重点。文献“周克旻,周蓉,滕婧,陈亦奇.多标记室内小型无人机定位与姿态估计方法[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(·)表示取二范数；计算检测函数的理论方差如下：

接着，判断是否有如果是，则令否则，令

步骤四：利用a求取四元数，得到加速度计引出的四元数测量值q_a，如下：

其中，

步骤五：读取磁力计数据m＝[m_x,m_y,m_z]′，利用m求取四元数，得到磁力计引出的四元数测量值q_m，如下：

其中h可以通过计算出，R(q)为旋转矩阵，具体为：

步骤六：根据q_a和q_m计算四元数观测向量q_o，如下：其中符号定义为：

其中，ρ＝[ρ₀,ρ₁,ρ₂,ρ₃]，为两个四元数；

步骤七：计算四元数的最终估计如下并对进行归一化得到并令

步骤八：重复步骤二至步骤七，即可得到每个时刻的四元数的最终估计，从中可以换算出姿态角。

与已有的技术相比，本发明具有以下优点：本发明具有较小的计算量，并且对被测对象的运动干扰有一定的鲁棒性。

附图说明

图1为本发明流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。

首先对一些符号进行说明。

陀螺仪数据表示为w＝[w_x,w_y,x_z]′，下标x、y、z表示该变量在x、y、z轴上的分量。类似的，我们定义加速度数据a＝[a_x,a_y,a_z]′、磁力计数据m＝[m_x,m_y,m_z]′。我们用右上角一撇(即’)表示该矩阵的转置。I_n表示n×n单位向量。

本发明具体包括以下步骤：

步骤一：令物体处于静止状态，并检测其姿态角，然后根据姿态角换算出其四元数初始化一个长度为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(·)表示取二范数；计算检测函数的理论方差如下：

接着，判断是否有如果是，则令否则，令

步骤四：利用a求取四元数，得到加速度计引出的四元数测量值q_a，如下：

其中，

步骤五：读取磁力计数据m＝[m_x,m_y,m_z]′，利用m求取四元数，得到磁力计引出的四元数测量值q_m，如下：

其中h可以通过计算出，R(q)为旋转矩阵，具体为：

步骤六：根据q_a和q_m计算四元数观测向量q_o，如下：其中符号定义为：

其中，ρ＝[ρ₀,ρ₁,ρ₂,ρ₃]，为两个四元数；

步骤七：计算四元数的最终估计如下并对进行归一化得到并令

步骤八：重复步骤二至步骤七，即可得到每个时刻的四元数的最终估计，从中可以换算出姿态角。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (1)

1.一种姿态估计方法，其特征在于，包括以下步骤：

步骤一：令物体处于静止状态，并检测其姿态角，然后根据姿态角换算出其四元数初始化一个长度为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(·)表示取二范数；计算检测函数的理论方差如下：

接着，判断是否有如果是，则令否则，令

步骤四：利用a求取四元数，得到加速度计引出的四元数测量值q_a，如下：

其中，

步骤五：读取磁力计数据m＝[m_x,m_y,m_z]′，利用m求取四元数，得到磁力计引出的四元数测量值q_m，如下：

其中h＝[h_x,h_y,h_z]′，h可以通过计算出，R(q)为旋转矩阵，具体为：

步骤六：根据q_a和q_m计算四元数观测向量q_o，如下：其中符号定义为：

其中，ρ＝[ρ₀,ρ₁,ρ₂,ρ₃]，为两个四元数；

步骤七：计算四元数的最终估计如下并对进行归一化得到并令

步骤八：重复步骤二至步骤七，即可得到每个时刻的四元数的最终估计，从中可以换算出姿态角。