[发明专利]一种附有运动加速度补偿的MARG姿态计算方法

权利要求书

1.一种附有运动加速度补偿的MARG姿态计算方法，该方法包括：

步骤1：利用陀螺仪进行一步预测：

建立系统状态模型：

x_k＝Φ_k-1,kx_k-1+w_k (1)

其中，下标k表示t_k时刻，x_k表示t_k时刻的状态，此模型中取姿态四元数q作为状态，即取x＝q；w_k为状态模型噪声；由t_k-1时刻到t_k时刻的状态转移矩阵Φ_k-1,k为：

其中，I₄表示4维单位阵，T_s表示采样间隔，M(ω_k)定义如下：

其中，ω_x，ω_y，ω_z分别表示x，y，z方向上的角速率；ω_k表示3维角速率矢量，即ω_k＝[ω_x ω_y ω_z]^T；上标T表示矩阵转置；

进行一步预测：

其中，上标表示矢量为预测量，表示从t_k-1时刻到t_k时刻的状态一步预测值，P_k|k-1表示一步预测的方差，P_k-1表示t_k-1时刻状态最优估计的方差，Q_k表示状态模型噪声w_k的方差；

步骤2：利用一步预测值，对加速度计与磁力计进行检测，具体如下：

步骤2-1：借助角速率姿态更新方程(4)，得t_k时刻姿态的一步预测值其对应的载体坐标系到导航坐标系的方向余弦矩阵为简记为其中方向余弦矩阵与四元数的关系如下：

其中，q₀，q₁，q₂，q₃分别表示姿态四元数q中的四个分量，即q＝[q₀,q₁,q₂,q₃]^T.

步骤2-2：对磁场干扰进行检测：

磁力计t_k时刻输出记为有磁力计输出模型：

其中，上标b表示载体坐标系；r_mag(k)表示地磁矢量；b_mag(k)表示磁场干扰量；ε_mag(k)为磁力计噪声，其方差记为其中diag表示对角矩阵，分别表示三轴磁场噪声的方差；计算磁场干扰检测量δr_k：

上标n表示导航坐标系，表示导航坐标系下的地磁矢量；根据3σ原则，当δr_k三轴分量满足：

认为无磁场干扰；反之，则磁场干扰存在；

步骤2-3：对运动加速度进行检测，当运动加速度持续时间大于设定阈值时，进行运动加速度补偿：

加速度计输出模型为：

其中，表示加速度计的输出值，g表示重力加速度；a_k表示载体非重力加速度，ε_acc(k)为加速度计噪声；

计算运动加速度检测量δa_k：

表示由(4)算得的姿态一步预测值，表示加速度计的输出；根据3σ原则，当δa_k三轴分量满足：

认为无运动加速度；反之，则运动加速度存在；其中σ_acc(x),σ_acc(y),σ_acc(z)分别表示加速度计三轴噪声标准差；

当运动加速度持续时间大于设定阈值时，通过滑动窗口进行运动加速度补偿：

对窗口长度为n的运动加速度数据{δa_k-n,…,δa_k-1}进行多项式曲线拟合，多项式拟合阶数和窗口长度根据载体实际运动快慢情况及加速度计采样率进行调整；拟合阶数选择0阶、或1阶、或2阶、或高阶中的一种；

借助拟合多项式计算得t_k时刻运动加速度拟合值根据方差传播定理确定运动加速度拟合值的方差将拟合值补偿值补偿后有：

经补偿后，用于滤波姿态解算，方差记为表示加速度计噪声的方差；

步骤3：针对不同的检测结果执行相应的融合算法：

无磁场干扰，无运动加速度或运动加速度已补偿：转至步骤4；

存在磁干扰，无运动加速度或运动加速度已补偿：转步骤5；

无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；

存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据；

步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数姿态估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计；

步骤4-1：建立加速度和磁感应强度的矢量对方程：

若选取n系为东北天坐标系，则有gⁿ＝[0 0 -g]^T，g，mⁿ＝[0 m_N m_U]^T，其中m_N和m_U分别表示北向与天向的地磁强度，其中，q表示姿态四元数矢量，y_acc表示加速度矢量，无运动加速度时取加速度计的输出运动加速度已补偿时取补偿后的加速度y_mag表示磁感应强度，无磁干扰情况下即磁力计的输出ε_acc表示加速度噪声，ε_mag表示磁感应强度噪声；

步骤4-2：通过快速解算获得量测信息从四元数与方向余弦矩阵关系出发，建立四元数特征值与特征向量方程，给出特征值数值计算公式，基于特征值接近1的数值特性，快速选出真实解；随后，进行矩阵初等变换，得到矢量对方程的四元数解；

计算中间变量矩阵

其中，H_x，H_y，H_z，W为计算过程中的中间变量矩阵，I₃表示3维单位阵，H_x1，H_x2，H_x3分别表示3维矩阵H_x的第1、2、3个分量；H_y1，H_y2，H_y3分别表示3维矩阵H_y的第1、2、3个分量，H_z1，H_z2，H_z3分别表示3维矩阵H_z的第1、2、3个分量；

计算中间参数：

τ₂＝8(H_x3H_y2H_z1-H_x2H_y3H_z1-H_x3H_y1H_z2+H_x1H_y3H_z2+H_x2H_y1H_z3-H_x1H_y2H_z3)

τ₃＝det(W)

其中，det表示求矩阵行列式；

计算特征值：

从(17)中选取最接近1的数作为最终的特征值λ，即

λ＝argmin|λ_i-1|,(i＝1,2,3,4) (18)

其中，argmin表示使得后面式子达到最小时自变量的取值；||表示取绝对值；进行初等变换，

其中，χ，ρ，υ，ζ表示完成初等变换后的矩阵最后一列的四个元素；

获得归一化的四元数

步骤4-3：建立滤波量测模型：

其中，z_k表示量测矢量，取量测转移矩阵为H_k＝I₄，v_k为量测噪声；

步骤4-4：基于卡尔曼滤波进行姿态估计：

P_k＝(I₄-K_kH_k)P_k|k-1 (24)

其中，K_k表示滤波增益，R_k表示量测噪声v_k的方差，P_k|k-1和P_k分别表示状态一步预测和状态最优估计的方差；上标-1表示矩阵求逆运算；

步骤5：利用加速度计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计，并对磁干扰进行估计；具体如下：

步骤5-1：利用加速度计数据建立量测模型：

y_acc＝-C^T(q)gⁿ+ε_acc (25)

步骤5-2：针对系统非线性模型，采用非线性滤波进行状态估计；

步骤6：利用磁力计数据建立量测模型，基于无迹卡尔曼滤波实现状态估计；具体如下：

步骤6-1：利用磁力计数据建立量测模型：

y_mag＝C^T(q)mⁿ+ε_mag (26)

步骤6-2：针对系统非线性模型，采用非线性滤波进行状态估计。