[发明专利]一种基于互补卡尔曼滤波器的臂环姿态角计算方法

权利要求书

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。