CN110319840A - 面向异常步态识别的共轭梯度姿态解算方法 - Google Patents

Abstract

本发明公开了一种面向异常步态识别的共轭梯度姿态解算方法，包括以下步骤：采用共轭梯度法对加速度计和磁力计的数据进行处理，获得解算姿态角，再将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角。本发明通过利用陀螺仪和磁力计与加速度计在频域上的互补特性，采用互补滤波器将磁力计和加速度计的解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角，从而可以提高姿态测量的精度，减小了陀螺仪姿态的误差并且提高了加速度计的动态性能。

Description

面向异常步态识别的共轭梯度姿态解算方法

技术领域

本发明涉及一种面向异常步态识别的共轭梯度姿态解算方法，属于姿态解算技术领域。

背景技术

人体运动是一个异常复杂的过程，研究人体运动在很多领域有着非常重要的意义，通过数据分析出人体步态健康状态，在运动系统疾病和神经系统疾病导致的下肢运动障碍的诊断和康复过程评估乃至残障评定中都发挥重要作用。步行运动包含了大量的人体运动学信息，这种看似简单却蕴含复杂运动学、动力学和生物力学的运动过程，要求肌肉系统，骨骼系统及神经系统的高度协调。步态是一个周期性和节律性很强的运动过程，在一个循环周期内很短的时间就可以提供很多信息，通过对步态信息的采集和定量分析就可评估肌肉骨骼乃至神经系统的功能。在监测仪器问世之前，人们大多通过可视化指标来了解正常的步态应该是什么样的形态。但是步行过程包含的丰富信息，即使对于经验丰富的观察者而言，也很难通过直观的方式全部掌握。而基于测量仪器的步态分析系统可以定量测算人体步行过程中下肢的运动学、动力学和生理学参数。因此，将步态的时间参数和空间参数信息定量化、精细化的必要性促进了步态分析系统的不断发展。

步态识别广泛应用于神经性系统疾病、康复训练等领域，标准步态识别实验室主要利用基于多相机动作捕捉和力平台的非可穿戴系统。尽管它们具有高可靠性，但光学设备价格昂贵，受到场地和环境因素限制，导致这些方法不能被广泛用于临床应用或家庭设置。随着MEMS(micro-electro-mechanical system)技术的发展，MEMS惯性传感器在可穿戴便携设备中的应用愈发成熟，由加速度计、陀螺仪组成的微惯性测量单元，辅以磁力计构成的人体步态识别系统体积小，功率低，价格便宜成为获取人体步态信息的重要途径。但是其仍然存在以下问题：

1、由于陀螺受到漂移和低频噪声影响，单纯用陀螺仪解算的精度将随时间降低；磁力计和加速度计受高频噪声和线性加速度干扰后，利用共轭梯度算法解算姿态难以收敛至最优值；

2、由于载体坐标系中加速度计的测量值只能存在重力加速度，但在实际应用时测试人员的运动不是匀速运动，会产生运动加速度，从而会降低姿态解算的精度；

3、共轭梯度算法的步长是固定不变的，固定的步长只能满足一定速率下的算法精度(即固定的步长只适用于一种情况，速率不同，其对应的固定步长也不同)，当人体处于不同的运动状态(如正常走路、跛脚走路、踮脚走路等等)时，共轭梯度算法步长过小，导致步态信息估计结果误差较大。

发明内容

本发明的目的在于，提供一种面向异常步态识别的共轭梯度姿态解算方法，它可以有效解决现有技术中存在的问题，尤其是由于陀螺受到漂移和低频噪声影响，单纯用陀螺仪解算的精度将随时间降低；磁力计和加速度计受高频噪声和线性加速度干扰后，利用共轭梯度算法解算姿态难以收敛至最优值的问题。

为解决上述技术问题，本发明采用如下的技术方案：一种面向异常步态识别的共轭梯度姿态解算方法，其特征在于，包括以下步骤：采用共轭梯度法对加速度计和磁力计的数据进行处理，获得解算姿态角，再将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角。

前述的面向异常步态识别的共轭梯度姿态解算方法，具体包括以下步骤：

S1，将加速度计和磁强计所测的数据用姿态四元数表示，并采用共轭梯度法对姿态四元数进行寻优处理，求得一组基于加速度计与磁强计表征出来的姿态四元数，即共轭梯度法解算姿态四元数；

S2，利用解算姿态角对陀螺仪数据积分得到的姿态角进行补偿，获得最终的姿态角。

本发明中地理坐标系和载体坐标系的转换关系采用四元数进行表述，计算量小，算法简单，而且可以避免欧拉角法在90度时出现的奇点问题；而且通过寻优处理，可以减轻误差干扰，找到最优解，从而可以提高姿态解算的精度和效率。

本发明中，所述的通过互补滤波器将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角，即得：

其中λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，表示融合之后的姿态四元数的导数；k和1-k为两种姿态四元数的权重系数；d_k表示搜索方向。

实验获得权重系数，然后通过以上方法，实现了将解算姿态角与陀螺仪数据积分得到的姿态角进行融合。

前述的面向异常步态识别的共轭梯度姿态解算方法，还包括：将加速度计的重力加速度和运动加速度进行分离，然后利用信任因子调整获得最终的姿态角：

其中，δ_k定义为共轭梯度法解算姿态四元数的信任因子：ε为信任因子的可调参数(通过实验获得)，为运动加速度模长，运动加速度向量为：式中，为当前加速度计的测量值，g^b为重力加速度值，为k-1次的姿态旋转矩阵；0≤k≤1，λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，k和1-k为两种姿态四元数的权重系数。

本发明中，为运动加速度模长，信任因子：通过将测得的运动加速度，代入公式可得信任因子，从而确定了加速度计测得的加速度的可信度，相当于重力加速度和运动加速度所占的比例大小，即将加速度计的重力加速度和运动加速度进行了分离；然后通过利用信任因子对加速度计测得的数值进行修正，从而可以提高姿态解算的精度和算法适应性，促使步态识别技术逐步应用于医疗康复系统治疗中。

前述的面向异常步态识别的共轭梯度姿态解算方法中，共轭梯度法中的步长通过陀螺仪的测量值进行动态调整。

本发明通过利用陀螺仪测量值对共轭梯度算法的步长进行实时调整，从而可以满足人体不同速率下步态识别的要求，相比于固定步长更能提高姿态解算的精度。

优选的，陀螺仪的测量值通过以下方法对共轭梯度法中的步长进行动态调整：

式中，λ_k为共轭梯度法中的步长；α为大于零的常系数；Δt为采样时间；表示载体的运动角速度模长，即陀螺仪的测量角速度模长。

与现有技术相比，本发明具有以下优点：

1、通过利用陀螺仪和磁力计与加速度计在频域上的互补特性，采用互补滤波器将磁力计和加速度计的解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角，从而可以提高姿态测量的精度，减小了陀螺仪姿态的误差并且提高了加速度计的动态性能；

2、通过将重力加速度与运动加速度分离开来进行处理，增加了运动加速度求取的准确性并较好地抑制了运动加速度，减弱了运动加速度的影响，从而可以提高姿态解算的精度和算法适应性，促使步态识别技术逐步应用于医疗康复系统治疗中；

3、通过引入陀螺仪测量值对共轭梯度算法的步长进行动态调整，从而可以满足人体不同速率下步态识别的要求，相比于固定步长更能提高姿态解算的精度，误差更小；

4、本发明的方法在动态性能和实效性方面均优于传统滤波算法，能够满足步态分析系统的整体需求。

5、本发明的方法可以很好的跟踪实际姿态的变化，时效更好，具有更快的收敛速度，即本发明可以提高获取的姿态精度和效率。另外，通过对正常行走与模拟跛脚行走行为进行实验验证，验证了本发明算法可以准确的输出异常步态信息，为后续异常步态识别进行数据处理操作。

本发明的难度在于，陀螺仪随机漂移误差较大，加速度计和磁力计在运动过程中易受高频噪声干扰，影响测量精度，因此，利用滤波算法对数据进行融合处理是解决该问题的关键。目前主流的姿态解算算法是卡尔曼滤波算法和互补滤波算法。但是其均考虑人体运动恒定匀速情况，并且计算量过大；而本发明则进一步考虑了异常步态识别时的特点--非匀速性，通过引入陀螺仪测量值对共轭梯度算法的步长进行动态调整，从而可以满足人体不同速率下步态识别的要求，相比于固定步长更能提高姿态解算的性能。

附图说明

图1是本发明的一种实施例的方法模型框架；

图2为载体坐标系和导航地理坐标系以及欧拉角示意图；

图3为静态时三种方法进行姿态解算的均方根误差估计示意图；

图4为加速度计三轴数据输出示意图；

图5为水平滑动时俯仰角估计示意图；

图6为有运动加速度时三种方法对俯仰角与翻滚角估计示意图；

图7三种方法的收敛速度估计示意图；

图8为正常行走时步态输出信息示意图；

图9为模拟跛脚行走时步态输出信息示意图。

下面结合附图和具体实施方式对本发明作进一步的说明。

具体实施方式

本发明的实施例：一种面向异常步态识别的共轭梯度姿态解算方法，如图1所示，包括以下步骤：采用共轭梯度法对加速度计和磁力计的数据进行处理，获得解算姿态角，再将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角。

为了提高姿态解算的精度和效率，具体包括以下步骤：

S1，将加速度计和磁强计所测的数据用姿态四元数表示，并采用共轭梯度法对姿态四元数进行寻优处理，求得一组基于加速度计与磁强计表征出来的姿态四元数，即共轭梯度法解算姿态四元数；

S2，利用解算姿态角对陀螺仪数据积分得到的姿态角进行补偿，获得最终的姿态角。

所述的通过互补滤波器将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角，即得：

其中λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，表示融合之后的姿态四元数的导数；k和1-k为两种姿态四元数的权重系数，k在不同实验系统中取值不固定，依据原则是共轭梯度法解算出的姿态四元数与陀螺仪数据解算出的姿态四元数在姿态融合算法中所占权重大小，取值范围0≤k≤1，比如本实施例可取经验值0.02。

为了减弱运动加速度的影响，提高姿态解算精度，还包括：将加速度计的重力加速度和运动加速度进行分离，然后利用信任因子调整获得最终的姿态角：

其中，δ_k定义为共轭梯度法解算姿态四元数的信任因子：ε为信任因子的可调参数(通过实验获得)，为运动加速度模长，运动加速度向量为：式中，为当前加速度计的测量值，g^b为重力加速度值，为k-1次的姿态旋转矩阵；0≤k≤1，λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，k和1-k为两种姿态四元数的权重系数(k可以取0.02)。

为了满足人体不同速率下步态识别的要求，共轭梯度法中的步长通过陀螺仪的测量值进行动态调整。

具体的，陀螺仪的测量值可通过以下方法对共轭梯度法中的步长进行动态调整：

式中，λ_k为共轭梯度法中的步长；α为实验获得的大于零的常系数，取值依据于实验系统，系统不同取值也不同，多次实验，取使实验图像最优最稳定时的α值；Δt为采样时间；表示载体的运动角速度模长，即陀螺仪的测量角速度模长。

本发明的一种实施例的工作原理：

1.坐标描述

脚为惯性测量器件的载体，由于载体的参数如姿态、位置、速度等需要通过特定的坐标系来表示，所以在计算步态的参数时坐标系的选取很重要。在姿态结算中，通常需要将加速度计、陀螺仪和磁强计测量的数据定义到一个坐标系中，称为载体坐标系，与之对应的为地理坐标系，其中地理坐标系采用东北地理坐标系。载体坐标系和地理坐标系之间的夹角就是描述载体航向的姿态角。X_e,Y_e,Z_e为地理坐标系，满足右手定则，X_b,Y_b,Z_b为绕轴旋转的载体坐标系，φ、θ和ψ分别为载体的横滚角、俯仰角和航向角，如图2所示；

2.四元数姿态解算算法描述

描述地理坐标系和载体坐标系的转换关系通常有三种方法：欧拉角法、方向余弦法、四元数法。但是欧拉角法会在90度时出现奇点问题，余弦矩阵的计算量过大，因此选用计算量小，算法简单的四元数法描述地理坐标系和载体坐标系的转换关系。为了便于数学模型的建立，定义b为载体坐标系，e为地理坐标系，其中a^b、ω^b、m^b分别为加速度计、陀螺仪、磁强计输出。q＝[q₁q₂q₃q₄]为姿态旋转四元数，标准化后将其简化为陀螺仪解算出的姿态为加速度计与磁强计数据解算的姿态为互补滤波器解算的姿态为陀螺仪数据扩展成为四元数向量形式如式：

所以姿态旋转四元数的微分方程为：

而在t+Δt时刻的姿态四元数q_w,t+Δt及其导数可由迭代公式计算：

其中，Δt为采样周期。利用陀螺仪角速度数据及公式(3)(4)，进行姿态四元数的更新计算。方向余弦矩阵与四元数的关系可表示为：

但由于四元数实质上是旋转矢量，它的4个变量并不独立，因此必须规范化处理。本发明所用四元数可以均是规范化的模值为1的四元数，即：

而姿态角与四元数之间的关系可表示为：

θ＝-sin^-1(2q₁q₃+2q₀q₂) (8)

陀螺仪在工作时易受到载体震动的干扰而影响测量精度，并在积分过程中产生较大的累积误差。为获取更加准确的姿态，需要用加速度计和磁强计的测量数据对陀螺仪进行补偿。通常用加速度计数据去补偿俯仰角和横滚角，磁强计数据补偿航向角。但加速度计无法区分重力加速度和运动加速度，磁强计容易受到周围磁场的干扰，且这类误差是随机的，无法预先消除。因此本发明首先用加速度计和磁强计所测的数据，采用共轭梯度法对姿态四元数进行寻优处理。

3.共轭梯度法

共轭梯度法的基本思想是把共轭性与最速下降方法相结合，利用已知点处的梯度构造一组共轭方向，并沿这组方向进行搜索，求出目标函数的极小点。该方法的计算过程是，把初始点的负梯度方向作为第一个搜索方向，利用一维最优步长进行搜索，求出下一个点，然后再利用该点的梯度来构造新的共轭方向，以此类推，直到求出极小点为止。根据非线性最小均方误差的问题构造目标函数：

其中为共轭梯度法解算出的旋转四元数，表示误差函数。

将地理坐标系b中重力加速度g通过四元数法旋转到载体坐标系e中的值，然后减去当前加速度计的测量值作差，这就是通过加速度计表征的旋转矩阵的误差函数，磁强计的误差计算与加速度计误差计算类似。

已知重力矢量与地磁参考矢量为g^b＝[0 0 0 1]和可通过载体地理坐标系下四元数转换到载体坐标系下，表示为：

其中为共轭梯度法解算出的旋转四元数，为其共轭四元数。

设与为加速度计与磁强计测量值归一化之后的矢量表示，则误差函数可以表示为：

由矩阵二范数定义可得，当时，即为所求姿态，使用共轭梯度迭代算法对目标函数进行最优值求解，则载体姿态迭代算法为：

Q_am(k+1)＝Q_am(k)+λ_kd_k (14)

其中，λ_k为最优步长，d_k为搜索方向。

根据共轭梯度算法定义梯度为：

式中为雅克比矩阵。

根据共轭梯度算法理论可知搜索方向d_k的计算为：

d_k＝-g_k+β_kd_k-1 (17)

共轭梯度算法中步长计算为：

4.共轭梯度法中的动态步长设计

共轭梯度算法的步长直接决定算法的性能：当且仅当算法的收敛速度比人体实际运行速度快时，解算结果才准确。每次更新一个新的姿态后都需要迭代数次以得到最优解，为了与互补滤波相结合，本发明实际应用共轭梯度算法时每次的釆样数据只进行一次迭代计算，因此共轭梯度算法的步长是固定不变的，固定的步长只能满足一定速率下的算法精度，当人体处于不同的运动状态时，共轭梯度算法步长过小，导致步态信息估计结果误差较大。因此提出了基于动态步长的共轭梯度算法，该步长的大小正比于实际物理的人体运动角速度模长：当人体步行处于慢速运动时，梯度步长较小，满足算法静态精度；而当人体快速运动时，步长大小随之增加。

在共轭梯度法中，步长λ_k与载体运动的角速度、系统的采样时间正相关，即：

式中为标准化的角速度值，Δt为采样时间，α>0，式(21)也表明载体的运动角速度越快，采样时间越长，那么步长也就越长。

载体的运动角速度即为陀螺仪的测量角速度。假设陀螺仪的测量角速度为：

则载体运动角速度模长为

因此设计共轭梯度法中的动态步长为：

5.基于运动加速度抑制的姿态融合算法设计

共轭梯度法可以求得一组基于加速度计与磁强计表征出来的姿态四元数。由于陀螺受到漂移和低频噪声影响，单纯用陀螺仪解算的精度将随时间降低；磁力计和加速度计受高频噪声和线性加速度干扰后，利用共轭梯度算法解算姿态难以收敛至最优值。利用两者在频域上的互补特性，采用互补滤波器对二者进行数据融合可提高姿态测量的精度，减小了陀螺仪姿态的误差并且提高了加速度计的动态性能。基于四元数微分方程和共轭梯度算法的姿态融合算法为：

其中，为共轭梯度法解算出的四元数，为陀螺仪数据解算出的姿态四元数，k和1-k为两种姿态四元数的权重系数，本文取经验值k＝0.02。由公式(3)、(14)、(24)、

(25)可得

其中

5.1运动加速度抑制

在姿态解算算法中，通过加速度计表征的旋转矩阵误差为地理坐标系e中的重力加速度g^e旋转到载体坐标系b中的向量g^b与加速度计测量值的差。因此载体坐标系中加速度计的测量值只能存在重力加速度，但在实验中测试人员的运动不是匀速运动，会产生运动加速度，这样会降低姿态解算的精度。所以要把传感器的重力加速度和运动加速度分离开来处理，如果运动加速度的值过大，式(14)中本次迭代出的四元数会减小。所以运动加速度越大，算法对共轭梯度法解算出的姿态角的不信任程度越深。

定义δ为共轭梯度算法解算姿态四元数的信任因子：

其中，δ为信任因子的可调参数，为运动加速度模长，公式(28)表明运动加速度的值越大，信任因子越低。运动加速度向量为：

式中为当前加速度计的测量值，g^b为重力加速度值，为了消除加速度计测量值对姿态解算的影响为k-1次的姿态旋转矩阵。

加速度计的测量值的信任程度和运动加速度的大小成反比，因此公式(27)可以写成：

因此面向异常步态识别的共轭梯度姿态结算算法解算公式为：

为了验证本发明的效果，发明人还进行了以下实验研究：

1.实验平台

本次实验姿态测量传感器采用BWT901CL传感器作为姿态测量单元，对陀螺仪、加速度计、磁力计进行数据采集，通过蓝牙与上位机进行通信，上位机可显示各传感器的测量数据和姿态欧拉角数据和波形，并将数据导入到文本文件中，进行进一步分析。本实验所用的电脑环境如表1所示。在实验中，系统以200Hz采样频率对传感器进行采样。

表1电脑环境参数

为验证本发明方法进行姿态解算的准确性和实效性，分别从静态，水平滑动，绕轴转动等方面进行验证分析，并与扩展卡尔曼滤波算法(EKF)、梯度下降算法(GDA)进行了对比分析。

2静态测试

分别利用EKF算法、GDA算法和本发明设计的混合滤波算法(简称DSCG，下同)对传感器数据进行处理，解算结果如图3中0～1200样本点区间所示。静止状态下，俯仰角和横滚角的理想值为0°，将所有采样点的解算值与单独使用加速度计解算的角度求解均方根误差。如表2所示，静止状态下，本发明方法的均方根误差控制在0.162°内，均方根误差与其它两种方法差距很小，能够有效避免陀螺漂移问题。

其中，为解算值，θ_i为真实值

表2均方根误差

算法	均方根误差
		GDA	0.158°
EKF	0.150°
		DSCG	0.162°

3水平滑动测试

加速计最大的缺点是无法区分运动加速度和重力加速度，难以单独使用。在运动中产生的线性加速度将对姿态解算精度产生影响，因此采用水平滑动实验验证本发明算法对线性加速度的滤除效果。将姿态测量单元置于水平台面，作水平滑动，图4表示了侧滑运动中三轴加速度计的测量值，其中，X，Y，Z分别为加速度计三轴输出。

理想状态下，水平滑动运动中俯仰角和横滚角为0°，即为参考值。但直接由加速度计、磁力计测量值经过解算，如图5所示，得到的俯仰角误差达到了39.06°，且运动加速度越大，该误差越大。

采用本发明的方法对比梯度下降算法和扩展卡尔曼滤波算法在水平滑动运动状态下的俯仰角与横滚角输出，仿真结果如图6所示，图6显示了有运动加速度时俯仰角与翻滚角估计，表3显示了各算法绝对值的最大误差值与最小误差值，由表3可知，本发明的滤波算法，俯仰角和横滚角的最大误差分别达到0.16°与0.49°，DSCG的俯仰角和横滚角的最大误差均小于GDA和EKF，说明本发明所提出的滤波方法对运动加速度有较好的滤除作用，能够更有效滤除线性加速度的影响。

表3三种算法的误差估计

4绕轴旋转测试

发明人模拟了载体姿态绕轴旋转时的情况，将分别采用扩展卡尔曼滤波、梯度下降法和本发明设计混合滤波算法得到俯仰角估计值和用加速度计得到实际测量值进行比较。如图7所示，姿态变化剧烈时，梯度下降算法有明显的滞后，1.65秒左右才会跟踪上，且有稳态误差。扩展卡尔曼滤波算法轻度滞后，1.5秒左右跟踪上。本发明设计的混合滤波算法很好的跟踪了实际姿态的变化，0.8秒左右即可跟踪上，时效更好，说明了本发明的混合滤波算法比梯度下降法具有更快的收敛速度，即本发明可以提高获取的姿态精度和效率。

5迭代时间

表4给出了本发明算法与其他两种典型方法一次迭代的平均时长的对比。由表4可以看出，EKF一次迭代耗时较长，GDA和本发明算法运算时间相对较短。本发明算法一次迭代时长为0.08ms，相比EKF减少约27％，相比GDA高出约24％，本算法迭代时间较短，计算量小，能够满足步态分析系统的实时性要求。

表4一次迭代时间

6异常步态识别

将传感器(市售产品，上面有XYZ坐标轴)安装固定在脚背上，因为有磁场作用，Y轴朝脚尖，，进行异常步态识别验证。此次实验将采用佩戴者模拟跛脚行为进行验证，佩戴者将在10m测试场地上进行正常行走，模拟跛脚行为行走等动作进行数据采集(即非匀速非等步长行走)，对本发明算法进行验证。使用梯度下降算法与本发明算法进行对比验证，图8、图9分别表示正常行走时步态信息输出和模拟跛脚行走时输出步态信息。与梯度下降算法对比，本发明所提出的算法可以准确地捕获步态信息。

7结论

本发明针对步态分析系统存在陀螺仪发散快和累计误差等问题，提出了可变步长的共轭梯度法和互补滤波相结合的混合滤波算法。算法利用共轭梯度算法对加速度计和磁力计数据进行处理，再通过互补滤波器将其与陀螺积分得到的姿态角进行融合，实时调整共轭梯度算的步长，减弱运动加速度的影响，提高姿态解算精度和算法适应性。通过实验验证：在动态环境下采用本发明算法进行处理，俯仰角和横滚角的最大误差分别达到0.16°与0.49°，俯仰角和横滚角的最大误差均小于GDA和EKF，说明本发明所提出的滤波方法对运动加速度有较好的滤除作用，能够更有效滤除线性加速度的影响；另外，本发明设计的混合滤波算法很好的跟踪了实际姿态的变化，0.8秒左右即可跟踪上，时效更好，说明了本发明的混合滤波算法比梯度下降法具有更快的收敛速度，即本发明可以提高获取的姿态精度和效率。即：本发明有效降低了陀螺仪累积误差、运动加速度等对解算精度的干扰，使步态分析系统动态性能有所提高，有效提升了异常步态识别系统的性能。另外，通过对正常行走与模拟跛脚行走行为进行实验验证，验证了本发明算法可以准确的输出异常步态信息，为后续异常步态识别进行数据处理操作。

Claims (6)

1.一种面向异常步态识别的共轭梯度姿态解算方法，其特征在于，包括以下步骤：采用共轭梯度法对加速度计和磁力计的数据进行处理，获得解算姿态角，再将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角。

2.根据权利要求1所述的面向异常步态识别的共轭梯度姿态解算方法，其特征在于，具体包括以下步骤：

S1，将加速度计和磁强计所测的数据用姿态四元数表示，并采用共轭梯度法对姿态四元数进行寻优处理，求得一组基于加速度计与磁强计表征出来的姿态四元数，即共轭梯度法解算姿态四元数；

S2，利用解算姿态角对陀螺仪数据积分得到的姿态角进行补偿，获得最终的姿态角。

3.根据权利要求2所述的面向异常步态识别的共轭梯度姿态解算方法，其特征在于，所述的通过互补滤波器将解算姿态角与陀螺仪数据积分得到的姿态角进行融合，获得最终的姿态角，即得：

其中λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，表示融合之后的姿态四元数的导数；k和1-k为两种姿态四元数的权重系数；d_k表示搜索方向。

4.根据权利要求2所述的面向异常步态识别的共轭梯度姿态解算方法，其特征在于，还包括：将加速度计的重力加速度和运动加速度进行分离，然后利用信任因子调整获得最终的姿态角：

其中，δ_k定义为共轭梯度法解算姿态四元数的信任因子：ε为信任因子的可调参数，为运动加速度模长，运动加速度向量为：式中，为当前加速度计的测量值，g^b为重力加速度值，为k-1次的姿态旋转矩阵；0≤k≤1，λ_k表示共轭梯度法中的步长，表示融合之后的姿态四元数，为共轭梯度法解算出的姿态四元数，为陀螺仪数据解算出的姿态四元数，表示陀螺仪数据解算出的姿态四元数的导数，k和1-k为两种姿态四元数的权重系数。

5.根据权利要求1-4任一所述的面向异常步态识别的共轭梯度姿态解算方法，其特征在于，共轭梯度法中的步长通过陀螺仪的测量值进行动态调整。

6.根据权利要求5所述的面向异常步态识别的共轭梯度姿态解算方法，其特征在于，陀螺仪的测量值通过以下方法对共轭梯度法中的步长进行动态调整：

式中，λ_k为共轭梯度法中的步长；α为大于零的常系数；Δt为采样时间；表示载体的运动角速度模长，即陀螺仪的测量角速度模长。