CN110207647B - 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 - Google Patents

Abstract

本发明主要公开了一种基于互补卡尔曼滤波器的臂环姿态角计算方法，其技术方案：包括以下步骤：步骤1：搭建姿态角测量系统，系统上电启动，陀螺仪、加速度计、磁传感器开始读取数据；步骤2：开始时，手臂平放处于静止水平状态，载体系xyz三轴分别指向水平右前上，采集一段时间序列N长度的传感器数据，计算状态X初始值X0；步骤3：系统噪声阵、量测噪声矩阵初始化；步骤4：状态量X的预测更新；步骤5：量测Z更新；步骤6：滤波增益K计算；步骤7：状态X估计；步骤8：运动结束，反之若运动未结束，则循环执行步骤4～步骤7；对加速度计、陀螺仪、磁传感器测量的角度进行融合，并得到一种更加精确的臂环姿态角计算结果。

Description

一种基于互补卡尔曼滤波器的臂环姿态角计算方法

技术领域

本发明涉及臂姿态角测量技术领域，特别是一种基于互补卡尔曼滤波器的臂环姿态角计算方法。

背景技术

在GINS中，姿态是通过环架上的角度传感器敏感的，无需计算，而在SINS中，所有的导航计算(位置、速度、姿态)都要通过计算得出，而且姿态计算(attitude calculation)的频率最高。

通过臂环佩戴于手腕或者小臂处时，现有的技术方案种实现小臂姿态角测量一般有两种方法，一是基于加速度计和磁分别进行姿态角测量，这种方法测量的精度较差，容易受运动情况干扰，二是基于陀螺仪进行测量，这种方法测量的结果容易随时间漂移。其中专利申请号：201610207713.4，公开了一种基于互补卡尔曼滤波算法计算融合姿态，其公开的技术方案是通过螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度，根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度，最后根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。首先其融合方式过程比较粗糙，融合策略不精准，其次其陀螺仪和加速度计只采用六轴融合，不能完全保证3自由度的姿态解算都精确不发散，只能保证2自由度的水平姿态精度。

发明内容

针对现有技术存在的不足，本发明提供一种基于互补卡尔曼滤波器的臂环姿态角计算方法，对加速度计、陀螺仪、磁传感器测量的角度进行融合，并得到一种更加精确的臂环姿态角计算结果，便可实现人体小臂或者手腕的运动估计，继而更进一步实现人体的运动监测、AR/VR用途。

为了达到上述目的，本发明通过以下技术方案来实现：一种基于互补卡尔曼滤波器的臂环姿态角计算方法，包括以下步骤：

步骤1：搭建姿态角测量系统，系统上电启动，陀螺仪、加速度计、磁传感器开始读取数据；

步骤2：开始时，手臂平放处于静止水平状态，载体系xyz三轴分别指向水平右前上，采集一段时间序列N长度的传感器数据，计算状态X初始值X0；

步骤3：系统噪声阵、量测噪声矩阵初始化；

步骤4：状态量X的预测更新；

步骤5：量测Z更新；

步骤6：滤波增益K计算；

步骤7：状态X估计；

步骤8：运动结束，反之若运动未结束，则循环执行步骤4～步骤7。

本发明进一步：步骤1具体包括：

加速度计三轴数据为fb＝[fx_b fy_b fz_b]^T,陀螺仪三轴数据为Gb＝[Gx_b Gy_b Gz_b]^T,磁传感器三轴数据为Mb＝[Mx_b My_b Mz_b]^T；b表示载体坐标系，原点为臂环中心位置，XYZ三个方向分别指向当前传感器位置的右前上方向。

本发明进一步：步骤2具体包括：

此处的N可以根据实际传感器采样频率取下式中的fb_avr＝[fx_b_avr fy_b_avrfz_b_avr]^T、Gb＝[Gx_b_avr Gy_b_avr Gz_b_avr]^T、Mb_avr＝[Mx_b_avr My_b_avr Mz_b_avr]^T均表示基于这段时间序列N的采样均值，状态X取＝[θ γ ψ w_x w_y w_z]^T，分别表示小臂的俯仰角，横滚角，航向角，x轴角速率，y轴角速率，z轴角速率；初始值X0的具体计算公式如下：

其中g表示当地重力加速度，可取值为9.80665m/s2；D表示地球上当地的磁偏角。

本发明进一步：步骤3具体包括：

系统噪声驱动阵Г和系统噪声矩阵Q为：

系统噪声矩阵Q由陀螺仪的三轴输出噪声方差构成，T为卡尔曼的滤波周期；

量测噪声矩阵R为：

量测噪声矩阵R由互补滤波输出的噪声方差构成。

本发明进一步：步骤4具体包括：

状态量X的预测更新为：

状态X的一步预测协方差矩阵P_k+1/k为：

上式中P_k表示协方差估计矩阵，其初始值为状态的初始误差，可以根据器件的测量精度给定。

本发明进一步：步骤5具体包括：

量测信息由互补滤波进行计算，将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到，量测矩阵H为：

上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数，这可以根据实际系统进行调整，φx_k+1、φy_k+1、φz_k+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角：

与步骤2中计算初始值时类似，不同之处在于，上式中fb＝[fx_b fy_b fz_b]^T、Gb＝[Gx_b Gy_b Gz_b]^T、Mb＝[Mx_b My_b Mz_b]^T表示传感器k+1时刻的取值。

本发明进一步：步骤6具体包括：

滤波增益K计算具体见下：

上式中的H_k+1表示量测矩阵，其为：

本发明进一步：步骤7具体包括：

状态X估计，具体见下式：

此时的估计结果：

便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角，同时可以得到；状态X估计的协方差矩阵P_k+1：

P_k+1＝(I-K_k+1H_k+1)P_k+1/k。

本发明具有有益效果为：

1、本方法可基于臂环中的陀螺仪、加速度计、磁传感器进行9轴融合，输出稳定不发散的小臂姿态角信息，保证3自由度的姿态解算都精确。

2、采用本发明步骤2-6中关于互补卡尔曼滤波的状态设定以及初始值计算、系统噪声阵、系统噪声驱动阵、量测噪声阵、状态预测更新过程、量测更新过程、量测矩阵计算的详细过程和方法，输出的结果误差小、更能反应小臂的实际运动状况。

附图说明

图1为本发明的流程图；

图2为本发明中载体系xyz三轴示意图。

具体实施方式

结合附图，对本发明较佳实施例做进一步详细说明。

如图1所示，一种基于互补卡尔曼滤波器的臂环姿态角计算方法，包括以下步骤：

步骤1：搭建姿态角测量系统，系统上电启动，陀螺仪、加速度计、磁传感器开始读取数据；加速度计三轴数据为fb＝[fx_b fy_b fz_b]^T,陀螺仪三轴数据为Gb＝[Gx_b Gy_bGz_b]^T,磁传感器三轴数据为Mb＝[Mx_b My_b Mz_b]^T；b表示载体坐标系，原点为臂环中心位置，XYZ三个方向分别指向当前传感器位置的右前上方向。

步骤2：开始时，手臂平放处于静止水平状态，载体系xyz三轴分别指向水平右前上，采集一段时间序列N长度的传感器数据，此处的N可以根据实际传感器采样频率取下式中的fb_avr＝[fx_b_avr fy_b_avr fz_b_avr]^T、Gb＝[Gx_b_avr Gy_b_avr Gz_b_avr]^T、Mb_avr＝[Mx_b_avr My_b_avr Mz_b_avr]^T均表示基于这段时间序列N的采样均值，状态X取＝[θ γ ψ w_x w_y w_z]^T，分别表示小臂的俯仰角，横滚角，航向角，x轴角速率，y轴角速率，z轴角速率；初始值X0的具体计算公式如下：

其中g表示当地重力加速度，可取值为9.80665m/s2；D表示地球上当地的磁偏角，可根据资料查询。

步骤3：系统噪声驱动阵Γ和系统噪声矩阵Q为：

系统噪声矩阵Q由陀螺仪的三轴输出噪声方差构成，T为卡尔曼的滤波周期；量测噪声矩阵R为：

量测噪声矩阵R由互补滤波输出的噪声方差构成，实际使用时可以实时根据互补滤波(量测信息)输出进行计算。

步骤4：状态量X的预测更新为：

状态X的一步预测协方差矩阵P_k+1/k为：

上式中P_k表示协方差估计矩阵，其初始值为状态的初始误差，可以根据器件的测量精度给定。

步骤5：量测Z更新，量测信息由互补滤波进行计算，将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到，量测矩阵H为：

上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数，这可以根据实际系统进行调整，φx_k+1、φy_k+1、φz_k+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角：

与步骤2中计算初始值时类似，不同之处在于，上式中fb＝[fx_b fy_b fz_b]^T、Gb＝[Gx_b Gy_b Gz_b]^T、Mb＝[Mx_b My_b Mz_b]^T表示传感器k+1时刻的取值。

步骤6：滤波增益K计算；滤波增益K计算具体见下：

上式中的H_k+1表示量测矩阵，其为：

步骤7：状态X估计，具体见下式：

此时的估计结果：

便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角，同时可以得到；状态X估计的协方差矩阵P_k+1：

P_k+1＝(I-K_k+1H_k+1)P_k+1/k。

步骤8运动结束，反之若运动未结束，则循环执行步骤4～步骤7

步骤1-7及图1中的基于互补卡尔曼滤波器的数据融合方案，基于臂环中的陀螺仪、加速度计、磁传感器进行9轴融合，输出稳定不发散的小臂姿态角信息，保证3自由度的姿态解算都精确。而采用本发明步骤2-6中关于互补卡尔曼滤波的状态设定以及初始值计算、系统噪声阵、系统噪声驱动阵、量测噪声阵、状态预测更新过程、量测更新过程、量测矩阵计算的详细过程和方法，输出的结果误差小、更能反应小臂的实际运动状况。

上述实施例仅用于解释说明本发明的发明构思，而非对本发明权利保护的限定，凡利用此构思对本发明进行非实质性的改动，均应落入本发明的保护范围。

Claims (4)

1.一种基于互补卡尔曼滤波器的臂环姿态角计算方法，其特征在于，包括以下步骤：

步骤1：搭建姿态角测量系统，系统上电启动，陀螺仪、加速度计、磁传感器开始读取数据；加速度计三轴数据为fb＝[fx_b fy_b fz_b]^T，陀螺仪三轴数据为Gb＝[Gx_b Gy_b Gz_b]^T，磁传感器三轴数据为Mb＝[Mx_b My_b Mz_b]^T；b表示载体坐标系，原点为臂环中心位置，XYZ三个方向分别指向当前传感器位置的右前上方向；

步骤2：开始时，手臂平放处于静止水平状态，载体系xyz三轴分别指向水平右前上，采集一段时间序列N长度的传感器数据，计算状态X初始值X0；

此处的N可以根据实际传感器采样频率取下式中的fb_avr＝[fx_b_avr fy_b_avr fz_b_avr]^T、Gb＝[Gx_b_avr Gy_b_avr Gz_b_avr]^T、Mb_avr＝[Mx_b_avr My_b_avr Mz_b_avr]^T均表示基于这段时间序列N的采样均值，状态X取＝[θ γ ψ w_x w_y w_z]^T，分别表示小臂的俯仰角，横滚角，航向角，x轴角速率，y轴角速率，z轴角速率；初始值X0的具体计算公式如下：

其中g表示当地重力加速度，可取值为9.80665m/s²；D表示地球上当地的磁偏角；

步骤3：系统噪声阵、量测噪声矩阵初始化；

步骤4：状态量X的预测更新；

状态X的一步预测协方差矩阵P_k+1/k为：

上式中P_k表示协方差估计矩阵，其初始值为状态的初始误差，可以根据器件的测量精度给定；

步骤5：量测Z更新；

量测信息由互补滤波进行计算，将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到，量测矩阵H为：

上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数，这可以根据实际系统进行调整，φx_k+1、φy_k+1、φz_k+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角：

上式中fb＝[fx_b fy_b fz_b]^T、Gb＝[Gx_b Gy_b Gz_b]^T、Mb＝[Mx-b My_b Mz_b]^T表示传感器k+1时刻的取值；

步骤6：滤波增益K计算；

步骤7：状态X估计；

步骤8：运动结束，反之若运动未结束，则循环执行步骤4～步骤7。

2.根据权利要求1所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法，其特征在于，步骤3具体包括：

系统噪声驱动阵Γ和系统噪声矩阵Q为：

系统噪声矩阵Q由陀螺仪的三轴输出噪声方差构成，T为卡尔曼的滤波周期；量测噪声矩阵R为：

量测噪声矩阵R由互补滤波输出的噪声方差构成。

3.根据权利要求2所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法，其特征在于，步骤6具体包括：

滤波增益K计算具体见下：

上式中的H_k+1表示量测矩阵，其为：

4.根据权利要求2-3任意一项所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法，其特征在于，步骤7具体包括：

状态X估计，具体见下式：

此时的估计结果：

便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角，同时可以得到；状态X估计的协方差矩阵P_k+1：

P_k+1＝(I-K_k+1H_k+1)P_k+1/k。

Images