一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法

技术领域

本发明涉及姿态解算与深度学习技术领域，具体涉及一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法。

背景技术

姿态估计是许多导航系统的重要任务之一，在卫星控制，无人机控制，工业机械臂，智能机器人，行人导航中具有广泛的应用；静止情况下的解决方法是利用体坐标系中互不平行的量测向量与对应的已知惯性向量(恒星的方位、地球重力、地球磁场等)进行对比从而完成姿态估计，如四元数估计器(Quaternion Estimator，QUEST)利用最小二乘思想进行姿态解算，得到了广泛关注并且衍生出了许多相关算法；

在运动情况下基于惯性测量单元的姿态估计算法中，可以利用陀螺仪的角速度测量值计算姿态变化量，但由于陀螺仪的漂移使得估计误差会逐渐累积，因此需要加速度计和磁力计等多种传感器的数据进行补偿；姿态解算中需要面对非线性状态估计问题，在非线性状态估计领域中，最常用的算法是扩展卡尔曼滤波(Extended Kalman Filter，EKF)；根据噪声特性的不同，分为加性扩展卡尔曼滤波(Additive Extended Kalman Filter,AEKF)和乘性扩展卡尔曼滤波(Multiplicative Extended Kalman Filter,MEKF)，当状态向量为四元数时，AEKF采用加法对四元数进行修正可能使四元数的模值不再为1而失去四元数的规范性，因此需要对估计结果进行归一化，而MEKF采用了四元数乘积进行修正，这两种算法在本质上是等价的；由于EKF的线性化处理过程会降低算法精度和稳定性，为了解决这一问题，Simon J Julier，Jeffrey K Uhlmann等人开发出了无迹卡尔曼滤波(UnscentedKalman Filter,UKF)，Ienkaran Arasaratnam，Simon Haykin等人开发出了容积卡尔曼滤波(Cubature Kalman Filter,CKF)，它们都基于“对分布进行近似要易于对任意非线性函数的近似”的思想，采用Sigma点对高斯分布进行近似，降低了线性化过程中的精度损失，使得UKF和CKF的性能要优于EKF；UKF在进行无迹变换时可能出现中心权值为负的情况，且当状态向量高于三维时，精度会出现下降；虽然都采用了Sigma点对高斯分布进行近似，但CKF的核心是球面-径向容积法则，在精度和收敛性方面都要优于UKF，且CKF采用的Sigma点比UKF少一个，具有相同的权值，避免了出现权值为负的状况，为了提升CKF的性能，国内外学者对CKF算法提出了许多改进算法，如平方根容积卡尔曼滤波、迭代自适应容积卡尔曼滤波、强跟踪容积卡尔曼滤波，但算法的计算量也会大幅提升；CKF算法在航空器和卫星等飞行器的应用较为广泛，但在行人导航姿态解算中的应用还没有见到；

惯性行人导航算法主要包括零速检测、零速修正和导航解算，对零速检测和零速修正算法的研究较多，而在导航解算方面的研究较少；由于在非零速区间，足部的运动加速度较大，多采用纯陀螺仪解算的方式，即仅依靠角速度测量数据解算姿态，没有充分挖掘和利用加速度计和磁力计的测量数据中的姿态信息；Aida Makni等人对运动加速度进行建模，假设运动加速度为一常值与白噪声的叠加，进而设计了四元数描述符滤波器(Quaternion Descriptor Filter,QDF)，可以解决任意方向存在运动加速度的情况，并在行人导航中进行了实验，其解算精度要优于扩展卡尔曼滤波算法，但该算法存在对运动加速度的模型假设过于简单的问题，无法减小运动加速度对姿态解算的影响；

基于此，亟需设计一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，以解决上述现有技术存在的问题。

发明内容

针对上述存在的问题，本发明旨在提供一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，本方法根据加速度计的测量结果判断加速度拟合区间，在拟合区间内使用拟合结果对加速度计量测值进行补偿，补偿后采用CKF算法估计姿态，进而进行速度和位置的解算，具有速度和位置的解算精度高和鲁棒性好的特点。

为了实现上述目的，本发明所采用的技术方案如下：

一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，包括

步骤1.在行人导航系统中，利用传感器获取行人姿态，建立传感器测量模型；

步骤2.在传感器测量模型的基础上，根据加速度计的测量结果判断加速度拟合区间，在拟合区间内使用拟合结果对运动加速度进行预测补偿；

步骤3.设计用于估计行人导航姿态的容积卡尔曼滤波器，对陀螺仪、加速度计和磁力计的数据进行融合，完成姿态估计。

优选的，步骤1所述的传感器测量模型的建立过程包括

步骤1.1.在行人导航系统中利用传感器获取行人姿态；

步骤1.2.根据步骤1.1获取的行人姿态，建立传感器测量模型为：

y

y

y

式中，C(q)为地理坐标系N系到载体坐标系B系的旋转矩阵，y

步骤1.3.设加速度计和磁力计的常值零偏已经得到补偿，利用静止状态下陀螺仪的量测均值对漂移估计，并对陀螺仪的测量值进行补偿，得到陀螺仪的量测结果为：

y

优选的，步骤2所述的根据加速度计的测量结果判断加速度拟合区间，在拟合区间内使用拟合结果对运动加速度进行预测补偿的过程包括

步骤2.1.根据传感器测量模型建立运动加速度拟合模型及运动加速度拟合误差模型；

步骤2.2.对拟合区间进行判断。

优选的，步骤2.1所述的建立运动加速度拟合模型及运动加速度拟合误差模型的过程包括

步骤2.1.1.采用k时刻和k-1时刻的运动加速度估计值

式中，k+1时刻的运动加速度a

其中，加速度的预测误差

步骤2.1.2.分析预测误差

步骤2.1.3.对每一个步态周期中拟合区间的拟合误差进行分析，并将式代入式得：

式中，

其中，

得到加速度的量测协方差阵为：

式中，E为数学期望，T表示向量或矩阵的转置。

优选的，步骤2.2所述的对拟合区间进行判断的过程包括

在零速检测的基础上，由于拟合区间位于加速度模值的两个峰值之间，找出步态周期内的两个峰值可以完成对拟合区间的判断。

优选的，步骤3所述的容积卡尔曼滤波器的设计过程包括

步骤3.1.在载体坐标系B(X

步骤3.2.选取欧拉角Φ和陀螺仪漂移b作为状态量，建立状态方程为；

x

其中：

/>

w

式中，x

步骤3.3选取加速度计和磁力计的量测构成量测向量

v

步骤3.4.设计容积卡尔曼滤波更新算法。

优选的，步骤3.1所述的姿态参数的选择过程包括

步骤3.1.1.在载体坐标系B(X

式(11)中，q

四元数需满足归一性：

||q||

步骤3.1.2.用参数冗余小且无归一化要求的欧拉角作为姿态参数：

Φ＝[φ θ ψ]

其中，φ为俯仰角，θ为滚转角，ψ为航向角；

步骤3.1.3.将sinx简记为sx，cosx简记为cx，由欧拉角构成的旋转矩阵为：

欧拉角的微分方程为：

式中：

ω＝[ω

ω为刚体在B系中三轴旋转角速度；采用一阶毕卡算法进行近似计算：

其中，θ≠90°。

优选的，步骤3.4所述的容积卡尔曼滤波更新算法的设计过程包括

步骤3.4.1.设计容积卡尔曼滤波初始化方程

利用加速度计和磁力计解算的姿态角平均值

步骤3.4.2.设计容积卡尔曼滤波时间更新方程；

步骤3.4.3.设计容积卡尔曼滤波量测更新方程。

优选的，步骤3.4.2所述的容积卡尔曼滤波时间更新方程的设计过程包括

(1)生成容积点

式中，

(2)容积点更新

(3)一步状态预测和协方差预测

优选的，步骤3.4.3所述的容积卡尔曼滤波量测更新方程的设计过程包括

(1)容积点和协方差矩阵的计算

a.计算容积点

b.计算量测容积点

c.计算预测量测值，量测协方差矩阵和交叉协方差矩阵为：

(2)增益矩阵、状态量和误差协方差矩阵的更新

计算卡尔曼增益K

本发明的有益效果是：本发明公开了一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，与现有技术相比，本发明的改进之处在于：

在基于足绑式惯性测量单元的行人导航系统中，由于足部运动加速度变化频繁且剧烈，使得常见的姿态融合算法精度下降，为了减小运动加速度对姿态解算的影响，本发明提出了一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，本方法通过对加速度计的输出进行一阶拟合补偿，同时设计了用于估计行人导航姿态的容积卡尔曼滤波器及其容积卡尔曼滤波算法，在非拟合区间则采用三子样旋转矢量法进行姿态更新；通过理论分析和试验验证表明，经过加速度补偿的容积卡尔曼滤波算法应用于行人导航的姿态估计时，相比于纯三子样旋转矢量法具有较高的解算精度，具有速度和位置的解算精度高和鲁棒性好的优点。

附图说明

图1为本发明基于运动加速度补偿的容积卡尔曼滤波姿态解算方法的算法流程图。

图2为传统行人导航算法结构图。

图3为本发明改进的行人导航算法结构图。

图4为本发明MTi-G-710传感器的结构示意图。

图5为本发明三轴运动加速度拟合误差和加速度计测量模值图。

图6为本发明运动加速度拟合误差图。

图7为本发明步态周期长度分布直方图与加速度模值分布图。

图8为本发明拟合区间图曲线。

图9为本发明基于运动加速度补偿和容积卡尔曼滤波的行人导航算法结构图。

图10为本发明容积卡尔曼滤波原理图。

图11为本发明实施例2两种算法的欧拉角估计误差图。

图12为本发明实施例2水平位置和高度解算结果图。

其中：在图6中，图(a)为X轴运动加速度拟合误差图，图(b)为Y轴运动加速度拟合误差图，图(c)为Z轴运动加速度拟合误差图；

在图7中，图(a)为周期长度分布图，图(b)为加速度模值图；

在图11中，图(a)为俯仰角误差曲线图，图(b)为滚转角误差曲线图，图(c)为航向角误差曲线图；

在图12中，图(a)为水平位置解算结果图，图(b)为高度位置解算结果图。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

实施例1：参照附图1-12所示的一种基于运动加速度补偿的容积卡尔曼滤波姿态解算方法，包括

步骤1.在基于足绑式惯性测量单元的行人导航系统中，利用传感器获取行人姿态，建立传感器测量模型，进行速度和位置的解算，其算法结构如图3所示

步骤1.1.首先在基于足绑式惯性测量单元的行人导航系统中利用传感器获取行人姿态，其中所述传感器固定于人体足部，包括陀螺仪，三轴加速度计和三轴磁传感器；

步骤1.2.根据步骤1.1获取的行人姿态，建立传感器测量模型为：

y

y

y

式中C(q)为地理坐标系N系到载体坐标系B系的旋转矩阵，q为四元数，y

步骤1.3.由于陀螺仪的漂移变化比较缓慢，可以采用静止状态下陀螺仪的量测均值对漂移进行估计，而后对陀螺仪的测量值进行补偿，此时陀螺仪的量测结果可以近似表达为(4)式：

y

在(2)～(4)式中，各传感器的噪声项δ

步骤2.在传感器测量模型的基础上，根据加速度计的测量结果判断加速度拟合区间，在拟合区间内使用拟合结果对加速度输出进行补偿，对运动加速度进行预测补偿，预测下一时刻的运动加速度并补偿进加速度计的测量结果，得到更准确的重力加速度测量值，既可以提高行人导航解算精度，也能避免模型过于简单带来的误差

步骤2.1.建立运动加速度拟合模型及运动加速度拟合误差模型

步骤2.1.1.由于在步行过程中会出现幅度较大、持续时间较长的运动加速度

k+1时刻的运动加速度a

其中，加速度的预测误差

步骤2.1.2.根据采集的加速度数据难以获得三种误差独立的统计特征，在下面的数据分析中，将直接分析研究预测误差

从图5可以发现，在非零速区间有一段加速度的拟合误差与零速区间相近，在这段运动过程中可以利用拟合结果对加速度计进行补偿，将这一区间称为拟合区间；

步骤2.1.3.对每一个步态周期中拟合区间的拟合误差进行分析，其误差分布图如图6所示；从6图可见，误差分布近似为正态分布；误差均值和方差如表1所示：

表1：运动加速度拟合误差均值(m/s

这样就近似得到了δ

将式代入式可得：

式中，

其中，

/>

式中，E为数学期望，T表示向量或矩阵的转置。

步骤2.2.对拟合区间进行判断

在行人导航算法中要应用该算法还需要准确判断出拟合区间，由图5可见，该区间位于加速度模值的两个峰值之间，因此只需找出步态周期内的两个峰值即可，对步态持续时间进行统计，图7展示了步态周期长度和一个步态周期之内加速度模值的分布；

其中，图6(a)表明步态周期长度分布在1.3秒至1.6秒之间，图6(b)表明可以进行拟合的区间位于步态周期的0.85秒即第170个采样点附近，在检测到走完一步后，检测前170个采样点和170个采样点以后的峰值，为消除峰值附近的剧烈变化带来的误差，取出区间中间3/4的部分，如图8所示；

由于在拟合区间对加速度补偿后可以近似看作零速区间，因此可以采用容积卡尔曼滤波对陀螺仪、加速度计和磁力计的数据进行融合，对姿态进行估计；在前期研究中，非零速区间的姿态解算算法为三子样旋转矢量法，因此在非拟合区间，仍采用三子样旋转矢量法，仅使用角速度信息进行姿态更新，此时，含加速度补偿的容积卡尔曼滤波的导航解算过程如图9所示；

步骤3.设计用于估计行人导航姿态的容积卡尔曼滤波器，对陀螺仪、加速度计和磁力计的数据进行融合，完成姿态估计

步骤3.1.参数选择

步骤3.1.1.在三维空间中，刚体的姿态可以用载体坐标系B(X

式(11)中q

而用于描述姿态的四元数需要满足归一性：

||q||

步骤3.1.2.考虑到构造容积点时需采用加法，这使得四元数不能满足式，归一化后不能再保证容积点的对称性，影响了后续计算，因此本实施例采用参数冗余小且无归一化要求的欧拉角作为姿态参数；采用ZYX旋转的欧拉角时，其定义如下：

Φ＝[φ θ ψ]

其中，φ为俯仰角，θ为滚转角，ψ为航向角；

步骤3.1.3.为简化方程的表述，将sinx简记为sx，cosx简记为cx，由欧拉角构成的旋转矩阵为：

/>

欧拉角的微分方程为：

式中：

ω＝[ω

ω为刚体在B系中的三轴旋转角速度；在实际应用中，可以采用一阶毕卡算法进行近似计算：

由式可知在解算过程中应避免θ接近90°，在行人导航中，一般不可能出现θ接近90°的情况；

步骤3.2.建立状态方程

选取欧拉角Φ和陀螺仪漂移b作为状态量，即x＝[Φ

x

其中：

w

(20)和(22)式中，x

步骤3.3.建立量测方程

选取加速度计和磁力计的量测构成量测向量

v

式中，C(Φ

步骤3.4.设计容积卡尔曼滤波更新算法

步骤3.4.1.设计容积卡尔曼滤波初始化方程

为加快算法收敛，需要较为准确的初始值，在行走前保持静止，利用加速度计和磁力计解算的姿态角平均值

步骤3.4.2.设计容积卡尔曼滤波时间更新方程

(1)生成容积点

式中，

(2)容积点更新

式中，f函数见(21)式

(3)一步状态预测和协方差预测

步骤3.4.3.设计容积卡尔曼滤波量测更新方程

(1)容积点和协方差矩阵的计算

a.计算容积点

b.计算量测容积点

式中，h函数见(24)式

c.计算预测量测值，量测协方差矩阵和交叉协方差矩阵为：

/>

(2)增益矩阵、状态量和误差协方差矩阵的更新

计算卡尔曼增益K

得到容积卡尔曼滤波算法结构图如图10所示。

实施例2：为验证本发明实施例1所述基于运动加速度补偿的容积卡尔曼滤波姿态解算方法的有效性和优越性，设计本实施例对上述方法进行验证

步骤4.实验验证

步骤4.1.仿真验证

在本步骤中，本实施例采用数值模拟的方法对提出的算法进行评价；传感器的数据采样周期设定为0.01秒，由于容积卡尔曼滤波参与解算的时间周期在0.5秒左右，因此仅作2秒钟的模拟，刚体的三维运动角速度如表2所示；

表2：三维运动角速度

利用欧拉角差分方程(19)可以得到这2秒内的欧拉角；依据式(2)和式(3)可以模拟加速度计和磁力计的测量结果，而后在传感器数据中加入高斯白噪声，加速度计和磁力计噪声的标准差分别为σ

表3：不同时间区间内加入的运动加速度

为了对比算法的估计精度和计算复杂度，在仿真中对比了未加入容积卡尔曼滤波和加入容积卡尔曼滤波后的姿态解算误差，如图11所示；

从图11中可以发现，在加速度拟合区间，加速度补偿的容积卡尔曼滤波算法的精度较高，误差有明显减小；为了更精确地比较算法的精度，采用均方根误差(RMSE)对算法的性能进行比较，均方根误差的解算方式如下：

/>

式中，T表示解算总时间长度，δx

表4：姿态解算的均方根误差

由表4的数据可知，加入CKF的解算后俯仰角误差降低了38.8％，滚转角误差降低了39.8％，航向角误差降低了29.0％，平均精度提高了35.3％；

步骤4.2.试验验证

沿着10m×18m的矩形路线行走，导航解算中使用了课题组前期提出的基于伪标准差和N-P准则的零速检测算法和卡尔曼滤波零速修正算法，最终位置解算结果如图12所示；

路径的起终点水平方向和高度方向误差如表5所示；

表5：解算路径的起终点水平与高度误差

表5表明，加入容积卡尔曼滤波后起终点水平误差减小了56.3％，起终点高度误差减小了20.3％。说明加入了加速度补偿后的CKF算法能够明显提高行人导航的精度。

通过上述实验结果可以看出，本发明实施例1设计的基于运动加速度补偿的容积卡尔曼滤波算法，经过加速度补偿的容积卡尔曼滤波算法应用于行人导航的姿态估计时，相比纯三子样旋转矢量法具有高的解算精度。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。