人体运动捕获系统广泛应用于电影特效制作、人体康复训练等领域，而传统光学运动捕获设备价格昂贵，多受场地和环境因素限制^[1]。随着MEMS （micro-electro-mechanical system）技术的发展，MEMS惯性传感器在可穿戴便携设备中的应用愈发成熟，由加速度计、陀螺仪组成的微惯性测量单元，辅以磁力计构成的人体运动捕获系统成为人体运动跟踪、获取人体运动信息的新途径。陀螺仪随机漂移误差较大，加速度计和磁力计在运动过程中易受高频噪声干扰，影响测量精度^[2]。因此，利用滤波算法对数据进行融合处理是解决该问题的关键。

卡尔曼滤波算法和互补滤波算法是主流的姿态解算滤波算法。卡尔曼滤波器是一种用于时变线性系统的递归滤波器^[3]，扩展卡尔曼滤波器存在线性误差干扰、局部截断误差及滤波发散问题^[4–5]；文献[6]提出了一种线性卡尔曼滤波器，降低了加速度计和磁力计产生的高频干扰；文献[7]提出了一种基于无迹卡尔曼滤波的抗干扰估计算法，减少了白噪声对姿态解算精度的影响。在不同的应用场景，卡尔曼滤波器需要对应建立不同状态方程、过程方程和观测方程，计算量大，运算时间较长^[8–10]。文献[11]提出了一种基于梯度下降的融合算法，可有效抑制随机噪声干扰，但是CPU开销较大。文献[12]设计了一种三输入的互补滤波器，结合卡尔曼滤波算法实现数据的融合。文献[13]提出了一种含双校正因子的互补滤波器，可有效抑制姿态解算的误差，但是收敛速度较慢。文献[14]提出了一种基于互补滤波和共轭梯度下降法的姿态融合算法，提高了算法收敛速率；文献[15]提出了一种梯度下降法结合互补滤波的混合滤波算法（hybrid filter，HF），但是由于互补滤波参数为定量，因此在剧烈运动或外界干扰大的情况下，算法适应性差。

针对以上算法的特点，作者设计了一种基于高斯牛顿算法和互补滤波器的自适应混合滤波融合算法（adaptive hybrid filter，AHF）。将快速高斯牛顿法（fast Gauss Newton algorithm，FGNA）的收敛方向作为观测量对陀螺累积误差进行补偿，引入重力矢量及地磁参考矢量自适应调节互补滤波器参数，提高姿态解算性能。通过静态、动态、磁干扰、线性加速度干扰等状态下实验，对比分析了本文算法与其他主流算法特性及优缺点，实验结果表明：本算法可有效抑制磁干扰和动态情况下的姿态角估计误差，且一次迭代的运算时间相比EKF算法减少约25%，本算法在静态性能、动态性能、抗干扰性和实效性均优于传统滤波算法，能够满足人体运动捕获系统的整体需求。

1 算法框架

本文提出了自适应混合滤波融合算法，整体方案如图1所示。利用优化的快速高斯牛顿算法对加速度计和磁力计输出数据进行姿态信息估计，再通过自适应互补滤波器将上述姿态角与陀螺积分得到的姿态角进行融合。高斯牛顿算法解算的姿态作为观测矢量对陀螺漂移进行补偿，引入重力矢量及地磁参考矢量自适应调节互补滤波器参数，提高系统动态性能。

为便于算法数学模型建立，定义b为载体坐标系，e为地理坐标系，a^b、ω^b、m^b分别为加速度计、陀螺仪、磁力计的输出。 ${\mathit{\boldsymbol{q}}} = \left[ {\begin{array}{*{20}{c}} {{q_1}} & {{q_2}} & {{q_3}} & {{q_4}} \end{array}} \right]$ 为姿态旋转四元数，从载体坐标系到地理坐标系的转换四元数，标准化后将其简化为 ${\hat{q}}$ 。 ${{\hat{q}}_{\omega ,t}}$ 为陀螺仪数据解算的姿态， ${{\hat{q}}_{am,t}}$ 为加速度计、磁力计数据解算的姿态， ${{\hat{q}}_{f,t}}$ 为互补滤波器解算的姿态。

陀螺仪三轴数据扩展成四元数向量形式如式（1）所示，姿态旋转四元数的微分方程式如式（2）所示：

${{\mathit{\boldsymbol{\omega }}}^b} = \left[ {\begin{array}{*{20}{c}}0 & {{\omega _x}} & {{\omega _y}} & {{\omega _{\textit z}}}\end{array}} \right]$

(1)

${{\dot{q}}_\omega } = \frac{1}{2}{\hat{q}} \otimes {{\mathit{\boldsymbol{\omega }}}^b}$

(2)

t+∆t时刻的姿态四元数 ${{\mathit{\boldsymbol{q}}}_{\omega ,t + \Delta t}}$ 及其导数 ${{\dot{q}}_{\omega ,t + \Delta t}}$ 可由式（3）和（4）迭代计算：

${{\mathit{\boldsymbol{q}}}_{\omega ,t + \Delta t}} = {{\hat{q}}_{\omega ,t}} + {{\dot{q}}_{\omega ,t + \Delta t}}\Delta t$

(3)

${{\dot{q}}_{\omega ,t + \Delta t}} = \frac{1}{2}{{\hat{q}}_{\omega ,t}} \otimes {{\mathit{\boldsymbol{\omega }}}_{t + \Delta t}}\!\!\!\!\!\!\!\!\!\!\!^b$

(4)

其中，∆t为采样周期。利用陀螺仪三轴角速度及式（3）和（4），求取更新的姿态四元数。

2 快速高斯–牛顿法

选择地理坐标系为参考坐标系，载体运动时，加速度计和磁力计在载体坐标系下的测量值分别为重力加速度分量和磁场强度分量，用四元数将参考坐标系中的加速度和磁场强度转换到载体坐标中，把转换后的值与当前时刻的测量值作差值，采用高斯牛顿迭代算法估计四元数的值。根据非线性最小均方误差的问题构造目标函数：

$f\left( {{{{\hat{q}}}_{am,t}}} \right) = \frac{1}{2}\varepsilon {\left( {{{{\hat{q}}}_{am,t}}} \right)^{\rm T}}\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right)$

(5)

其中， ${{\hat{q}}_{am,t}} = \left[ {\begin{array}{*{20}{c}} {{{\hat q}_0}} & {{{\hat q}_x}} & {{{\hat q}_y}} & {{{\hat q}_{\textit z}}} \end{array}} \right]$ 为高斯牛顿算法解出的旋转四元数， $\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right)$ 表示误差函数。

已知重力矢量和地磁参考矢量分别为 $ {{\hat{z}}^e}_{a,t} =$ $ \left[ {\begin{array}{*{20}{c}} 0 & 0 & 0 & 1 \end{array}} \right]$ 和 ${{\hat{z}}^e}_{m,t} = \left[ {\begin{array}{*{20}{c}} 0 & {{{\hat m}_x}} & {{{\hat m}_y}} & {{{\hat m}_{\textit z}}} \end{array}} \right]$ 。利用四元数表示旋转的性质可得：

${{\hat{Q}}_{a,t}} = {{\hat{q}}_{am,t}} \otimes {{\hat{z}}^e}_{a,t} \otimes {{\hat{q}}^*}_{am,t}$

(6)

${{\hat{Q}}_{m,t}} = {{\hat{q}}_{am,t}} \otimes {{\hat{z}}^e}_{m,t} \otimes {{\hat{q}}^*}_{am,t}$

(7)

记 ${{\hat{a}}^b}_t$ 、 ${{\hat{m}}^b}_t$ 为加速度计和磁力计测量值的归一化向量设置加速度计和磁力计分别占权重为ρ_a和ρ_m，误差函数写为：

$\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right) = \left[ {\begin{array}{*{20}{c}}{{\rho _a}{\varepsilon _a}\left( {{{{\hat{q}}}_{am,t}}} \right)}\\{{\rho _m}{\varepsilon _m}\left( {{{{\hat{q}}}_{am,t}}} \right)}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{\rho _a}\left( {{{{\hat{Q}}}_{a,t}} - {{{\hat{a}}}^b}_t} \right)}\\{{\rho _m}\left( {{{{\hat{Q}}}_{m,t}} - {{{\hat{m}}}^b}_t} \right)}\end{array}} \right]$

(8)

由矩阵二范数定义可得，当 $\left\| {\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right)} \right\| = 0$ 时，方程结果即为所求姿态。由于系统误差、计算误差的存在，该方程的实际值趋近于0，用高斯牛顿迭代法解决上述问题，可得到迭代公式：

$\begin{array}{c}{\hat{q}}(k + 1) = {\hat{q}}(k) - {({\mathit{\boldsymbol{J}}}{(k)^{\rm T}}{\mathit{\boldsymbol{J}}}(k))^{ - 1}}{\mathit{\boldsymbol{J}}}{(k)^{\rm T}}\varepsilon \left( {{\hat{q}}(k)} \right),\\k = 0,1,2, \cdots ,n\end{array}$

(9)

其中， ${\hat{q}}(k)$ 为前一时刻的估计值， ${\hat{q}}(k + 1)$ 为当前估计值，J（k）为误差方程在当前时刻的雅克比行列式：

${\mathit{\boldsymbol{J}}}({\hat{q}}(k)) = \frac{{\partial \varepsilon \left( {{\hat{q}}(k)} \right)}}{{\partial {\hat{q}}(k)}} = \left| {\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}{{\rho _a}\displaystyle\frac{{\partial {\varepsilon _1}}}{{\partial {{{\hat{q}}}_0}}}}\\[10pt]\begin{array}{l}{\rho _m}\displaystyle\frac{{\partial {\varepsilon _2}}}{{\partial {{{\hat{q}}}_0}}}\\[10pt]\begin{array}{*{20}{c}}{} & \vdots \end{array}\end{array}\\[10pt]{{\rho _m}\displaystyle\frac{{\partial {\varepsilon _6}}}{{\partial {{{\hat{q}}}_0}}}}\end{array}} & {\begin{array}{*{20}{c}}{{\rho _a}\displaystyle\frac{{\partial {\varepsilon _1}}}{{\partial {{{\hat{q}}}_x}}}}\\[10pt]\begin{array}{l}{\rho _m}\displaystyle\frac{{\partial {\varepsilon _2}}}{{\partial {{{\hat{q}}}_x}}}\\[10pt]\begin{array}{*{20}{c}}{} & \vdots \end{array}\end{array}\\[10pt]{{\rho _m}\displaystyle\frac{{\partial {\varepsilon _6}}}{{\partial {{{\hat{q}}}_x}}}}\end{array}} & {\begin{array}{*{20}{c}}{{\rho _a}\displaystyle\frac{{\partial {\varepsilon _1}}}{{\partial {{{\hat{q}}}_y}}}}\\[10pt]\begin{array}{l}{\rho _m}\displaystyle\frac{{\partial {\varepsilon _2}}}{{\partial {{{\hat{q}}}_y}}}\\[10pt]\begin{array}{*{20}{c}}{} & \vdots \end{array}\end{array}\\[10pt]{{\rho _m}\displaystyle\frac{{\partial {\varepsilon _6}}}{{\partial {{{\hat{q}}}_y}}}}\end{array}} & {\begin{array}{*{20}{c}}{{\rho _a}\displaystyle\frac{{\partial {\varepsilon _1}}}{{\partial {{{\hat{q}}}_{\textit z}}}}}\\[10pt]\begin{array}{l}{\rho _m}\displaystyle\frac{{\partial {\varepsilon _2}}}{{\partial {{{\hat{q}}}_{\textit z}}}}\\[10pt]\begin{array}{*{20}{c}}{} & \vdots \end{array}\end{array}\\[10pt]{{\rho _m}\displaystyle\frac{{\partial {\varepsilon _6}}}{{\partial {{{\hat{q}}}_{\textit z}}}}}\end{array}}\end{array}} \right|$

(10)

式（9）也可写成式（11），式（11）描述的是传统的n次迭代的高斯牛顿算法，利用初始值 ${\hat{q}}(0)$ 估算姿态 ${\hat{q}}(n + 1)$ 。

$\begin{aligned}[b]{\hat{q}}(n + 1) = & {\hat{q}}(0) - \sum\limits_{k = 0}^n {{{({\mathit{\boldsymbol{J}}}{{(k)}^{\rm T}}{\mathit{\boldsymbol{J}}}(k))}^{ - 1}}{\mathit{\boldsymbol{J}}}{{(k)}^{\rm T}}\varepsilon \left( {{\hat{q}}(k)} \right)}= \\& {\hat{q}}(0) - {\mu _k}{({\mathit{\boldsymbol{J}}}{(0)^{\rm T}}{\mathit{\boldsymbol{J}}}(0))^{ - 1}}{\mathit{\boldsymbol{J}}}{(0)^{\rm T}}\varepsilon \left( {{\hat{q}}(0)} \right)\end{aligned}\!\!\!\!\!\!\!\!\!\!$

(11)

其中： ${\hat{q}}(n + 1)$ 为t+∆t时刻高斯牛顿算法计算的旋转四元数； ${\hat{q}}(0)$ 为t时刻互补滤波算法解算的初始姿态；μ_t为搜索步长，用目标函数二次模型Hessen矩阵可求得。高斯牛顿法忽略了Hessen矩阵中的2阶信息项，只需计算近似项 $H \approx 2{{\mathit{\boldsymbol{J}}}^{\rm T}}{\mathit{\boldsymbol{J}}}$ ，结合梯度 ${\mathit{\boldsymbol{G}}} \approx 2{{\mathit{\boldsymbol{J}}}^{\rm T}}\varepsilon $ 获得较快的收敛速度，记为 $\nabla = - {\left( {{{\mathit{\boldsymbol{J}}}^{\rm T}}{\mathit{\boldsymbol{J}}}} \right)^{ - 1}}{{\mathit{\boldsymbol{J}}}^{\rm T}}\varepsilon $ 。

高斯牛顿法迭代次数受收敛速度约束，收敛速度不小于载体运动速度，收敛速度同时受搜索步长μ_t制约，只要给定搜索步长μ_t一个适当的值，即可缩短迭代时间。因此，式（11）可简化为只迭代一次：

$\begin{aligned}[b]\!\!\!\! {{{\hat{q}}}_{am,t + \Delta t}} = & {{{\hat{q}}}_{f,t}} - {\mu _{t + \Delta t}}{({\mathit{\boldsymbol{J}}}{({{{\hat{q}}}_{f,t}})^{\rm T}}J({{{\hat{q}}}_{f,t}}))^{ - 1}}J{({{{\hat{q}}}_{f,t}})^{\rm T}}\varepsilon \left( {{{{\hat{q}}}_{f,t}}} \right){\rm{ = }}\\&{{{\hat{q}}}_{f,t}} - {\mu _{t + \Delta t}}\frac{{\nabla {{{\hat{q}}}_{f,t}}}}{{\left\| {\nabla {{{\hat{q}}}_{f,t}}} \right\|}}\end{aligned}\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!$

(12)

其中，搜索步长 ${\mu _{t + \Delta t}}$ 的最优值可由式（13）确定， $ \alpha $ 与加速度计和磁力计的噪声有关，一般取 $ \alpha $ >1。

${\mu _{t + \Delta t}} = \alpha \left\| {{{{\dot{q}}}_{\omega ,t + \Delta t}}} \right\|\Delta t$

(13)

姿态四元数 ${{\hat{q}}_{am,t + \Delta t}}$ 可利用式（12）进行迭代求解。通过本文的改进，每次解算只需进行一次迭代，很大程度降低了运算复杂度。

3 自适应混合滤波融合算法 3.1 基于自适应混合滤波算法的姿态更新

第2节分别阐述了两种解算姿态角的方案。但是，由于陀螺受到漂移和低频噪声影响，单纯用陀螺仪解算的精度将随时间降低；磁力计和加速度计受高频噪声和线性加速度干扰后，利用高斯牛顿算法解算姿态难以收敛至最优值。互补滤波器可用于处理波谱互补的两种噪声源，因此可以用一个高通滤波器处理陀螺仪数据，以及一个低通滤波器处理高斯牛顿算法优化后的加速度计、磁力计数据，将陀螺仪解算的 ${{\hat{q}}_{\omega ,t}}$ 和加速度计、磁力计解算的 ${{\hat{q}}_{am,t}}$ 融合后输出最终的姿态信息，实现最优估计。传统互补滤波器的增益权重是一个定值，本文提出的自适应互补滤波器结合了互补滤波器和卡尔曼滤波器的优点，通过实时调整不同算法的权重大小优化姿态解算结果。

基本互补滤波算法的融合公式为：

${{\hat{q}}_{f,t}} = {k_t}{{\hat{q}}_{am,t}} + (1 - {k_t}){{\hat{q}}_{\omega ,t}},{\rm{ }}0 \le {k_t} \le 1$

(14)

其中， ${{\hat{q}}_{\omega ,t}}$ 为陀螺仪解算的姿态角， ${{\hat{q}}_{am,t}}$ 为加速度计、磁力计经过高斯牛顿算法解算的姿态角，k_t和1–k_t为两种姿态解算方案的权值。k_t的取值要保证 ${{\hat{q}}_{\omega ,t}}$ 的发散加权率和 ${{\hat{q}}_{am,t}}$ 的收敛加权率相等：

$\begin{aligned}[b]&{k_t}\frac{{{\mu _t}}}{{\Delta t}} = \lambda \left( {1 - {k_t}} \right){\text{。}}\\\!\!{\text{即}} \quad\quad\quad\quad\quad\quad\ &{k_t} = \frac{\lambda }{{\displaystyle\frac{{{\mu _t}}}{{\Delta t}} + \lambda }}\end{aligned}\quad\quad\quad\quad\ \ $

(15)

其中， $ \lambda $ 为可变参数。将式（13）、（15）代入式（14）得：

${{\hat{q}}_{f,t}} = \frac{\lambda }{{\alpha \left\| {{{{\dot{q}}}_{\omega ,t + \Delta t}}} \right\| + \lambda }}{{\hat{q}}_{am,t}} + (\frac{{\alpha \left\| {{{{\dot{q}}}_{\omega ,t + \Delta t}}} \right\|}}{{\alpha \left\| {{{{\dot{q}}}_{\omega ,t + \Delta t}}} \right\| + \lambda }}){{\hat{q}}_{\omega ,t}}$

(16)

式（16）中含有两个参数 $ \alpha $ 和 $ \lambda $ ，应用时不易调整。考虑到计算过程中 ${{\hat{q}}_{am,t}}$ 的收敛速率应不小于载体运动的实际速率，式（12）和（15）可写为：

${{\hat{q}}_{am,t + \Delta t}}{\rm{ = }} - {\mu _{t + \Delta t}}\frac{{\nabla {{{\hat{q}}}_{f,t}}}}{{\left\| {\nabla {{{\hat{q}}}_{f,t}}} \right\|}}$

(17)

${k_t} = \frac{\lambda }{{{\mu _t}/\Delta t}} = \frac{{\lambda \Delta t}}{{{\mu _t}}}$

(18)

由于μ_t很大，故k_t≈0，将式（3）、（17）、（18）代入式（14）可得：

${{\hat{q}}_{f,t + \Delta t}} = {{\hat{q}}_{f,t}} + {{\dot{q}}_{f,t + \Delta t}}\Delta t$

(19)

其中，

${{\dot{q}}_{f,t + \Delta t}}{\rm{ = }}{{\dot{q}}_{\omega ,t + \Delta t}} - \lambda {{\dot{q}}_{\nabla ,t + \Delta t}} = {{\dot{q}}_{\omega ,t + \Delta t}} - \lambda \frac{{\nabla {{{\hat{q}}}_{f,t}}}}{{\left\| {\nabla {{{\hat{q}}}_{f,t}}} \right\|}}$

(20)

经过简化后的互补滤波融合算法只有一个参数 $ \lambda$ 。传统互补滤波器的参数 $\lambda $ 是定值，但是在人体运动过程中，可能出现姿态的快速变化。本文对 $\lambda $ 进行动态调节，增强滤波器的自适应调节性能。当运动产生的线性加速度被引入到加速度计的测量值中，导致加速度计的测量结果偏差较大；反之，若运动速率较大，则可能超出陀螺仪量程，造成陀螺误差偏大。因此对 $ \lambda$ 作以下优化：

$\begin{aligned}[b]\!\! \lambda = & 2 - \left( {{e_{a,t}} + {e_{m,t}}} \right) \times 0.9 - \\& \left( {\left| {{e_{ax}}} \right| + \left| {{e_{ay}}} \right| + \left| {{e_{a{\textit z}}}} \right| + \left| {{e_{mx}}} \right| + \left| {{e_{my}}} \right| + \left| {{e_{m{\textit z}}}} \right|} \right) \times 10 \times 0.1\end{aligned}\!\!\!\!\!\!\!\!\!\!\!$

(21)

其中， ${e_{a,t}}$ 、 ${e_{m,t}}$ 为前一周期加速度计、磁力计误差值。取误差：

${{\mathit{\boldsymbol{e}}}_a} = {\left[ {\begin{array}{*{20}{c}}{{e_{a0}}} & {{e_{ax}}} & {{e_{ay}}} & {{e_{a{\textit z}}}}\end{array}} \right]^{\rm{T}}} = {{\hat{z}}^e}_{a,t} \otimes {{\hat{a}}^b}$

(22)

${{\mathit{\boldsymbol{e}}}_m} = {\left[ {\begin{array}{*{20}{c}}{{e_{m0}}} & {{e_{mx}}} & {{e_{my}}} & {{e_{m{\textit z}}}}\end{array}} \right]^{\rm{T}}} = {{\hat{m}}^b} \otimes {{\hat{z}}^e}_{m,t}$

(23)

优化的计算式融入了低通滤波，可降低自适应互补滤波参数的变化率，提升系统连续性和稳定性。

为降低陀螺漂移对测量精度的影响，本文将FGNA算法的收敛方向 ${{\dot{q}}_{\nabla ,t + \Delta t}}$ 作为观测矢量对陀螺漂移误差进行补偿，补偿后的角速率为：

${{\mathit{\boldsymbol{\omega }}}_{c,t + \Delta t}}\!\!\!\!\!\!\!\!\!\!\!\!\!\!^b \ \ \ = {{\mathit{\boldsymbol{\omega }}}_{t + \Delta t}}\!\!\!\!\!\!\!\!\!\!^b \ \ - {{\mathit{\boldsymbol{\omega }}}_{{\rm error},t + \Delta t}}\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!^b$

(24)

其中， ${{\mathit{\boldsymbol{\omega }}}_{{\rm error},t + \Delta t}}\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!^b \quad\quad = 2{\hat{q}}_{f,t + \Delta t}^ * \otimes {{\dot{q}}_{\nabla ,t + \Delta t}}$ 。

改进后的自适应互补滤波算法总框图如图2所示。

3.2 自适应混合滤波算法复杂度分析

复杂度主要用于评判算法的易实现程度。分析算法复杂度最为有效的方法是对其浮点操作数（flops）准确统计。定义一次flops为两个浮点数进行一次加、减、乘、除运算。

本文给出滤波中基本代数运算的flops次数：1）矩阵加减法：A∈R_n×m，B∈R_n×m，计算A±B需nm次flops。2）矩阵相乘：A∈R_n×m，B∈R_m×l，计算AB的flops为2mnl–nl次。3）矩阵求逆：A∈R_n×n，A^–1的flops数为n³。4）矩阵转置：A∈R_n×l，A^T的flops数为nl。用上述诸元素即可对自适应混合滤波算法进行复杂度分析。

构建目标函数如下：

$f\left( {{{{\hat{q}}}_{am,t}}} \right) = \displaystyle\frac{1}{2}\varepsilon {\left( {{{{\hat{q}}}_{am,t}}} \right)^{\rm T}}\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right)$ 。需要n²l+nl次flops。其中，误差函数 $\varepsilon \left( {{{{\hat{q}}}_{am,t}}} \right)$ 需要的n次flops。

计算雅克比行列式需要2nl次flops。

迭代公式如下：

$\begin{aligned}{{{\hat{q}}}_{am,t + \Delta t}} =& {{{\hat{q}}}_{f,t}} - {\mu _{t + \Delta t}}{({\mathit{\boldsymbol{J}}}{({{{\hat{q}}}_{f,t}})^{\rm T}}J({{{\hat{q}}}_{f,t}}))^{ - 1}}J{({{{\hat{q}}}_{f,t}})^{\rm T}}\varepsilon \left( {{{{\hat{q}}}_{f,t}}} \right){\rm{ = }}\\& {{{\hat{q}}}_{f,t}} - {\mu _{t + \Delta t}}\frac{{\nabla {{{\hat{q}}}_{f,t}}}}{{\left\| {\nabla {{{\hat{q}}}_{f,t}}} \right\|}}{\text{。}}\end{aligned}$

需要4n³l+2n²l+3l³+2l次flops。

互补滤波融合公式如下：

${{\hat{q}}_{f,t + \Delta t}} = {{\hat{q}}_{f,t}} + {{\dot{q}}_{f,t + \Delta t}}\Delta t{\text{。}}$

仅需要n²+n次flops。其中，可变参数 $ \lambda $ 需要n次flops。

综上所述，算法总体复杂度为4n³l+n²l+5nl+n²+3n+3l³+2l，可见复杂度为O（n³）和O（l³）量级，即与状态维数和量测维数成3次方关系。而整个算法中，每次解算需要迭代的次数仅为1次。

4 实验验证及结果分析

姿态测量单元采用STM32F103作为MPU，传感器采用MPU6050（集成陀螺仪和加速度计）和HMC5883（磁力计），通过蓝牙进行上位机通信，系统采样频率为100 Hz。系统硬件如图3所示。

为验证本文算法的可靠性和实效性，分别对磁干扰状态和线性加速度干扰状态进行验证分析，并与混合滤波算法（HF）、扩展卡尔曼滤波算法（EKF）、梯度下降算法（GDA）对比分析。

4.1 抗磁干扰实验

静置姿态测量单元于水平台面，将永磁体作为磁干扰源引入实验环境采集数据，磁干扰源如图4所示。

实验环境磁场强度变化如图5所示，红色标记区域为磁干扰状态。

分别利用HF算法、EKF算法、GDA算法和本文设计的AHF融合算法对传感器数据进行处理，解算结果如图6中0～15 000样本点区间所示。静止状态下，俯仰角和横滚角的参考值为0°，航向角的参考值为磁强计解算的磁航向角度，将所有采样点的解算值与参考值比较得出均方根误差。静止无磁干扰状态下，GDA收敛速度慢，漂移快；经测试静止放置3 min，HF、EKF和本文算法静态均方根误差小于0.5°，能够有效避免陀螺漂移问题。静止无磁干扰状态下，HF、EKF和本文算法解算精度优于GDA。

磁力计数据主要参与航向角的解算，因此研究磁干扰对姿态解算的影响，只考虑航向角解算结果。各算法航向角解算误差如图7所示。静止磁干扰状态下，俯仰角和横滚角的参考值为0°，航向角的参考值即为无磁干扰状态下磁强计解算的磁航向角度，将所有采样点的解算值与参考值比较得出均方根误差。HF和GDA受磁干扰影响大，航向角解算均方根误差分别为3.51°和2.95°，EKF和AHF解算航向角均方根误差为0.07°和0.08°，EKF和本文算法有效降低磁干扰对航向的解算影响，能够满足存在磁干扰环境下的姿态角估计精度。

4.2 抗线性加速度干扰实验

自由运动中产生的线性加速度将对姿态解算精度产生影响，因此本文采用水平滑动实验验证算法对线性加速度的滤除效果。将姿态测量单元置于水平台面，作水平滑动，分别用HF和AHF对数据进行融合。图8表示了侧滑运动中三轴加速度计的测量值，其中，a_x、a_y、a_z分别为加速度计三轴输出。

图9为4种算法在水平滑动运动状态下的姿态角估计仿真图。理想状态下，水平滑动运动中俯仰角和横滚角为0°，即为参考值。如图9所示：HF算法对线性加速度的滤除效果最差，俯仰角和横滚角的最大误差分别达到28.9°和43.8°；EKF算法俯仰角和横滚角估计最大误差为12.7°和7.8°；GDA算法分别为1.9°和1.2°，本文算法分别为0.7°和0.6°。因此GDA和本文算法能够有效滤除线性加速度的影响。

分别进行100次静态、动态测试，分别求出每一组实验的均方根误差，再求出100组实验均方根误差的平均值。表1给出了多组实验下不同算法的误差统计。由图5～9和表1可知：在静止无磁干扰状态下，本文算法可有效抑制姿态测量单元三轴陀螺的漂移，并保持稳定，姿态角估计误差小于0.5°；在有磁干扰的环境下，AHF算法对航向角起到较好的补偿作用，避免磁干扰对航向产生的误差，误差均值为0.94°；在动态环境下，AHF能够准确输出3个姿态角，误差均值小于1°，保证了后期实现人体运动捕获的准确性。

表2给出了本文算法与其他3种典型方法一次迭代的平均时长的对比。由表2可以看出，EKF和GDA一次迭代耗时较长，HF和本算法运算时间相对较短。本算法一次迭代时长为4.7 ms，相比EKF和GDA分别减少约25%和55%，相比HF高出约46%，能够满足人体运动捕获系统的实时性要求。

表3给出了本文算法与其他3种算法各方面的性能对比。综合考虑陀螺漂移、磁场干扰、线性加速度干扰及实时性等因素，本文算法的整体性能具有显著的优势。

4.3 人体上臂运动捕获功能实现

运动捕获系统利用佩戴于人体不同位置的姿态测量单元采集人体运动数据，通过无线方式传输至上位机，实时驱动上位机3D人物完成与佩戴运动捕获装置用户相同的动作。为验证本文算法在实际操作中的可行性，用VS2010开发工具和OpenGL图形库，开发了人体运动捕获上位机显示界面，实现了人体上臂运动的捕获功能。

如图10所示，将2个姿态测量单元分别佩戴于用户的小臂及大臂，做前后摆臂运动。实验结果表明，本文算法能够准确、实时地测量人体肢体的位置信息，实现人体运动捕获功能。

5 结　论

本文针对人体运动捕获系统存在陀螺发散快和噪声干扰等问题，提出一种基于高斯牛顿算法和互补滤波器的自适应混合滤波算法。算法利用快速高斯牛顿算法对加速度计和磁力计数据进行处理，再通过互补滤波器将其与陀螺积分得到的姿态角进行融合，实时调整互补滤波器参数，提高姿态解算精度和算法适应性。通过实验验证，静止状态下本文算法姿态角估计误差小于0.5°，磁干扰状态下的误差均值为0.94°；在动态环境下误差均值小于1°，一次迭代的运算时间相比EKF算法减少约25%，该算法静态性能、动态性能、抗干扰性和实效性均优于传统滤波算法，能够满足人体运动捕获系统的整体需求。下一步，需要继续研究高斯牛顿算法的优化问题，考虑更多干扰因素，确定最优步长解，进一步缩减迭代算法的运算时间，降低算法复杂度。