一种基于惯性传感器的动态自适应姿态估计方法

1.本发明属于传感器自适应估计领域，特别是涉及一种基于惯性传感器的动态自适应姿态估计方法。


背景技术：

2.基于惯性传感器的姿态测量系统由于其自主式、低成本、小体积的特点，广泛应用于动作捕捉、虚拟现实、机器人等领域。它通过融合陀螺仪和加速度计的输出来得到载体的水平姿态角。
3.陀螺仪在短时间积分得到的姿态角精度较高，但在长时间的运动情况下，会产生累积误差。当载体处于静止或匀速运动状态时，不考虑加速度计的零偏等误差，加速度计仅敏感重力加速度，可以计算得到准确的水平姿态角,但是在运动状态时，运动加速度的存在，会导致加速度计算得到的水平姿态角误差增大。因此，如何利用陀螺仪和加速度计的信息，提高载体在长时间的运动状态下的姿态估计精度，是姿态测量系统重要的研究内容。


技术实现要素：

4.本发明的目的是提供一种基于惯性传感器的动态自适应姿态估计方法，以解决上述现有技术存在的问题。
5.为实现上述目的，本发明提供了一种基于惯性传感器的动态自适应姿态估计方法，包括：
6.基于惯性传感器和姿态估计结果获取载体机动状态数据；
7.对所述载体机动状态数据获取自适应补偿系数和陀螺仪零偏估计数据；
8.对所述自适应补偿系数和陀螺仪零偏估计数据进行处理，生成互补滤波算法；
9.基于所述互补滤波算法获取自适应姿态估计结果。
10.优选地，所述获取载体机动状态数据的过程包括：
11.获取惯性传感器数据和上一时刻姿态估计结果；
12.基于所述惯性传感器数据和上一时刻姿态估计结果对载体当前机动状态进行评估，获得所述载体机动状态数据。
13.优选地，所述对载体当前机动状态进行评估的过程包括：
14.获取加速度计输出的模值和陀螺仪输出的模值；
15.设置预设阈值，对所述加速度计输出的模值和陀螺仪输出的模值进行计算后与预设阈值进行对比，将所述载体当前机动状态分为非机动状态和机动状态；
16.获取俯仰角误差估计值和横滚角误差估计值，基于俯仰角误差估计值和横滚角误差估计值对所述机动状态进行评估，获得所述载体机动状态数据。
17.优选地，所述获取自适应补偿系数的过程包括：
18.获取理论重力加速度输出和加速度计实际的输出的夹角；
19.选取补偿系数和补偿阈值，基于所述补偿系数和补偿阈值和所述夹角获取所述自
适应补偿系数。
20.优选地，获取陀螺仪零偏估计数据的过程包括：
21.基于预设阈值和所述载体机动状态数据，对加速度计输出的模值和陀螺仪输出的模值进行判断，获得判断结果；
22.基于判断结果，对陀螺仪输出存入滑动窗口中进行集散，获得陀螺仪的零偏；
23.将所述陀螺仪的零偏进行更新处理，获得所述陀螺仪零偏估计数据。
24.优选地，所述生成互补滤波算法的过程包括：
25.选取坐标系，基于所述坐标系获取姿态四元数；
26.基于加速度计测量数据和所述姿态四元数获取三维向量数据；
27.对所述三维向量数据进行计算，获得向量积误差；
28.基于所述自适应补偿系数对所述向量积误差进行调整后，再基于陀螺仪零偏估计数据进行互补滤波，生成所述互补滤波算法。
29.优选地，所述获取自适应补偿系数的过程还包括：
30.判断载体状态，当载体处于非机动状态时，基于非机动状态下加速度计的输出和加速度计实际的输出的夹角调节补偿系数；
31.当载体处于机动状态时，基于俯仰角误差估计值和横滚角误差估计值调节补偿系数。
32.优选地，所述获取自适应姿态估计结果的过程包括：
33.通过非机动状态下加速度计的输出和加速度计实际的输出的夹角、陀螺仪输出的原始角速度、对陀螺仪的输出进行补偿，获得补偿后的角速度；
34.基于补偿后的角速度，对四元数进行更新；
35.基于更新后的四元数结果，获取所述自适应姿态估计结果。
36.本发明的技术效果为：
37.(1)在静态条件下，本发明提出的方法能取得良好的姿态估计效果；
38.(2)在动态条件下，本发明提出的方法精度优于传统的互补滤波；
39.(3)本发明提出的方法受机动影响小于传统的互补滤波。
附图说明
40.构成本技术的一部分的附图用来提供对本技术的进一步理解，本技术的示意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。在附图中：
41.图1为本发明实施例中的算法总体框架示意图；
42.图2为本发明实施例中的非机动状态下k关于||e||2的变化曲线图；
43.图3为本发明实施例中的机动状态下k关于ρ的变化曲线图；
44.图4为本发明实施例中的俯仰角、横滚角的静态测试结果图；
45.图5为本发明实施例中的俯仰角及俯仰角误差测试结果图；
46.图6为本发明实施例中的横滚角及横滚角误差测试结果图。
具体实施方式
47.需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相
互组合。下面将参考附图并结合实施例来详细说明本技术。
48.需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
49.实施例一
50.如图1-2所示，本实施例中提供一种基于惯性传感器的动态自适应姿态估计方法，包括：
51.首先，根据惯性传感器和上一时刻的姿态估计结果，对载体当前的机动状态进行评估。若载体处于机动状态，则计算当前的水平姿态角误差。若判断当前载体处于非机动状态，将陀螺仪数据加入到滑动窗口中，对陀螺仪的零偏进行估计。同时，根据当前载体的机动状态和水平姿态误差角自适应地调试补偿系数的大小。在互补滤波器中，利用加速度信息补偿角速度，并通过角速度积分得到最终的姿态估计结果。
52.进一步优化方案，自适应互补滤波姿态估计算法的原理是利用加速度计测量的重力加速度与当前姿态四元数所组成的三维向量求向量积误差，将该误差经过自适应补偿系数调整后，与陀螺仪输出进行互补滤波，得到当前的姿态四元数。自适应互补滤波姿态估计算法的关键在于补偿系数的自适应调整策略。本实施例提出利用载体自身的运动信息对载体机动状态进行评估，并针对不同的机动状态采取对应的补偿系数的自适应调整策略。同时，提出了基于滑动窗口的陀螺仪零偏估计算法，旨在减小陀螺仪的积分误差。
53.进一步优化方案，不考虑地球自转，当载体处于非机动状态时，加速度计输出的模值为当地的重力加速度，陀螺仪输出为0。令nf为加速度计输出的模值，n
ω
为陀螺仪输出的模值：
[0054][0055][0056]
式中分别代表陀螺仪输出ωb在x、y、z三轴上的分量，在x、y、z三轴上的分量，分别代表加速度计输出fb在x、y、z轴上的分量。
[0057]
考虑到加速度计和陀螺仪的输出存在误差，设置相应的阈值：
[0058][0059]
式中tf和t
ω
代表设置的阈值，g为当地的重力加速度大小。
[0060]
当nf和n
ω
满足公式(3)时，认为载体处于非机动状态，否则认为载体处于机动状态。相较于仅使用n
f n
ω
判断载体的运动状态，本实施例提出同时使用nf和n
ω
来判断载体的运动状态，能够3有效地避免对载体运动状态的误判。
[0061]
当nf和n
ω
不满足公式(3)时，认为载体处于机动状态，此时需要分析非重力加速度引起的姿态误差。
[0062]
令导航坐标系下加速度计的输出为fn，机体坐标系下加速度计的输出为fb，当地的重力加速度为gn，gn＝[0 0
ꢀ‑
g]
t
。
[0063]
当载体处于非机动状态时，fb可以表示为：
[0064][0065]
从n系到b系的旋转矩阵表达式：
[0066][0067]
将公式(5)代入公式(4)中，可以得到：
[0068][0069]
由公式(6)可以求得横滚角γ和俯仰角θ的表达式：
[0070][0071]
令δθ和δγ分别为非重力加速度引起的俯仰角误差和横滚角误差，v
x
、vy、vz分别代表归一化后的重力加速度v在x、y、z轴上的分量。δa
x
、δay、δaz分别代表归一化后的非重力加速度在x、y、z轴上的分量。
[0072]
δθ＝arcsin(vy+δay)-arcsin(vy)
ꢀꢀꢀꢀ
(8)
[0073][0074]
δa
x
、δay、δaz无法直接得到，本实施例通过加速度计输出和当前姿态下的理论重力加速度作差，得到非重力加速度的估计值δa
x
、δay、δaz。
[0075][0076]
公式(8)、(9)可以改写为：
[0077]
δθ＝arcsin(vy+δay)-arcsin(vy)
ꢀꢀꢀꢀ
(11)
[0078][0079]
式中，δθ和δγ分别代表俯仰角误差估计值和横滚角误差估计值。当δθ和δγ越大时，说明载体的机动越大，当δθ和δγ越小时，说明载体的机动越小。
[0080]
进一步优化方案，传统的互补滤波算法中，并没有对陀螺仪的零偏进行估计，增加了陀螺仪积分产生的误差，因此，本实施例提出了一种基于滑动窗口的零偏估计算法。
[0081]
当nf和n
ω
满足公式(3)时，将ωb存入滑动窗口中，令滑动窗口的长度为n，计算陀螺仪的零偏：
[0082][0083]
式中，代表滑动窗口中第i个陀螺仪数据。
[0084]
更新陀螺仪的输出：
[0085][0086]
式中，为补偿后的陀螺仪输出。
[0087]
进一步优化方案，导航坐标系选取东北天(enu)地理坐标系n，机体坐标系选取右前上坐标系b。
[0088]
本实施例采用四元数进行姿态更新。相较于欧拉角法和方向余弦法，四元数法可以全姿态工作，并且计算效率高。
[0089]
四元数q的表达式：
[0090]
q＝q0+q1i+q2j+q3k
ꢀꢀꢀꢀ
(15)
[0091]
将公式(13)对时间t求导，可得：
[0092][0093]
式中，ω
x
、ωy、ωz分别代表角速度在x、y、z轴上的分量。
[0094]
四元数的离散更新公式：
[0095][0096]
式中，δt为采样时间。
[0097]
公式(5)也可以写成四元数的形式[15]：
[0098][0099]
由公式(4)和公式(16)可以得到姿态角的四元数表达式：
[0100][0101]
令v为非机动状态下加速度计的输出。
[0102][0103]
加速度计实际的输出为fb。当载体处于机动状态时，v≠fb，设e为v与fb之间的夹
角，当夹角较小时，可令e＝sine，e可由fb与v叉乘得到：
[0104][0105]
式中，v
x
、vy、vz分别为v在x、y、z轴上的分量。
[0106]
令ω
b0
为陀螺仪输出的原始角速度，ω
b1
为补偿后的角速度，k为补偿系数，可以通过e对陀螺仪的输出进行补偿：
[0107]
ω
b1
＝ω
b0
+k*e
ꢀꢀꢀꢀ
(22)
[0108]
最后利用公式(16)、公式(17)和公式(19)就可以解出当前的姿态角。
[0109]
进一步优化方案，传统的互补滤波中，k是固定值，这会导致非重力加速度过大时，姿态估计的误差增大。因此，本实施例对载体的运动状态进行判断，针对不同的运动状态采取不同的补偿策略。
[0110]
使用载体机动状态评估方法对载体的机动状态进行评估后，根据载体的机动情况来确定补偿系数k的大小。
[0111]
载体处于非机动状态时，为了保证姿态角快速收敛，同时避免加速度计校正时姿态角出现超调，本实施例根据e的大小自适应地调节k：
[0112][0113]
式中，k1和k2代表选取的补偿系数大小，e1和e2代表选取的阈值大小，||e||2代表e的二范数。此时，k关于||e||2的变化曲线如图2所示。
[0114]
载体处于机动状态时，当姿态角误差增大时，应该减小加速度计对陀螺仪的补偿，因此，本实施例提出利用δθ和δγ来自适应地调节k的大小。
[0115][0116][0117]
式中，k
α
代表选取的补偿系数大小，k为衰减因子。
[0118]
此时k关于ρ的变化曲线如图3所示。进一步优化方案，为了验证本实施例提出方法在静态环境下的性能，进行了静态环境下的仿真。
[0119]
仿真参数设置如下：初始俯仰角为20
°
；初始横滚角为30
°
；陀螺零偏为0.1
°
/s；陀螺的系统噪声均方差为0.01
°
/s；加速度计的系统噪声方差为10-3
g；采样频率为200hz，仿真总时长300s。
[0120]
在仿真条件下，用传统互补滤波算法和本实施例提出方法解算出姿态并进行对比，结果如图4所示，仿真条件下的俯仰角误差和横滚角误差如表1所示：
[0121]
表1
[0122][0123]
从图4和表1中可以看出，在静态条件下，互补滤波和本实施例提出的方法都能取得不错的姿态估计效果，本实施例提出的方法由于增加了零偏估计的过程，在零偏估计完成后能减小陀螺仪的积分误差，所以姿态估计效果要优于传统的互补滤波。
[0124]
进一步优化方案，为了验证本实施例提出方法的有效性，本实施例搭建了基于stm32和imu的动态姿态测量系统样机，其中，imu选用scha634芯片，它集成了三轴加速度计，三轴陀螺仪的mems传感器，利用i2c总线将数据传输给主控芯片。系统样机通过串口将传感器原始数据和姿态数据传输给上位机进行显示和记录。shca634各项参数如表2所示。
[0125]
表2
[0126][0127]
参考基准选取动态姿态测量台的角度输出，动态姿态测量台由转位机构，驱动电机，角度编码器等模块组成，通过串口将角度数据传输给上位机进行显示和记录，输出角度误差在0.05
°
以内。
[0128]
为了验证本实施例提出方法在机动条件下的性能，本实施例进行了6组对比实验。前3组实验，将模型样机竖直安装在角度传感器的舵面上，测试俯仰角解算精度；后3组实验，将模型样机水平安装在角度传感器的舵面上，测试横滚角解算精度。启动电机，使角度传感器分别以低速(最大转速为50
°
/s)，中速(最大转速为100
°
/s)，高速(最大转速为200
°
/s)转动，记录角度传感器和模型样机的输出。最后，用传统互补滤波算法和本实施例提出方法解算出姿态并进行对比。
[0129]
俯仰角及俯仰角误差测试结果如图5所示，横滚角及横滚角测试误差如图6所示，动态测试下两种算法的均方根误差和最大误差如表3所示。
[0130]
表3
[0131][0132]
由图5、图6和表3可得：
[0133]
在动态情况下，本实施例提出的方法姿态估计精度优于传统互补滤波算法。在低速、中速、高速运动时，本实施例提出的方法的均方根误差分别为传统互补滤波82％，40％，32％，最大误差为传统互补滤波的31％，44％，39％。
[0134]
本实施例提出的方法姿态估计精度受机动影响小于传统互补滤波算法。模型样机处于高速的运动状态时，相对于低速运动，传统互补滤波算法的俯仰角误差(rms)增加了0.55
°
，横滚角误差(rms)增加了0.81
°
。本实施例提出方法的俯仰角误差(rms)仅增加了0.07
°
，横滚角误差(rms)仅增加了0.19
°
。
[0135]
以上所述，仅为本技术较佳的具体实施方式，但本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本技术的保护范围之内。因此，本技术的保护范围应该以权利要求的保护范围为准。

技术特征：
1.一种基于惯性传感器的动态自适应姿态估计方法，其特征在于，包括以下步骤：基于惯性传感器和姿态估计结果获取载体机动状态数据；对所述载体机动状态数据获取自适应补偿系数和陀螺仪零偏估计数据；对所述自适应补偿系数和陀螺仪零偏估计数据进行处理，生成互补滤波算法；基于所述互补滤波算法获取自适应姿态估计结果。2.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述获取载体机动状态数据的过程包括：获取惯性传感器数据和上一时刻姿态估计结果；基于所述惯性传感器数据和上一时刻姿态估计结果对载体当前机动状态进行评估，获得所述载体机动状态数据。3.根据权利要求2所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述对载体当前机动状态进行评估的过程包括：获取加速度计输出的模值和陀螺仪输出的模值；设置预设阈值，对所述加速度计输出的模值和陀螺仪输出的模值进行计算后与预设阈值进行对比，将所述载体当前机动状态分为非机动状态和机动状态；获取俯仰角误差估计值和横滚角误差估计值，基于俯仰角误差估计值和横滚角误差估计值对所述机动状态进行评估，获得所述载体机动状态数据。4.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述获取自适应补偿系数的过程包括：获取理论重力加速度输出和加速度计实际的输出的夹角；选取补偿系数和补偿阈值，基于所述补偿系数和补偿阈值和所述夹角获取所述自适应补偿系数。5.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，获取陀螺仪零偏估计数据的过程包括：基于预设阈值和所述载体机动状态数据，对加速度计输出的模值和陀螺仪输出的模值进行判断，获得判断结果；基于判断结果，对陀螺仪输出存入滑动窗口中进行集散，获得陀螺仪的零偏；将所述陀螺仪的零偏进行更新处理，获得所述陀螺仪零偏估计数据。6.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述生成互补滤波算法的过程包括：选取坐标系，基于所述坐标系获取姿态四元数；基于加速度计测量数据和所述姿态四元数获取三维向量数据；对所述三维向量数据进行计算，获得向量积误差；基于所述自适应补偿系数对所述向量积误差进行调整后，再基于陀螺仪零偏估计数据进行互补滤波，生成所述互补滤波算法。7.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述获取自适应补偿系数的过程还包括：判断载体状态，当载体处于非机动状态时，基于非机动状态下加速度计的输出和加速度计实际的输出的夹角调节补偿系数；
当载体处于机动状态时，基于俯仰角误差估计值和横滚角误差估计值调节补偿系数。8.根据权利要求1所述的基于惯性传感器的动态自适应姿态估计方法，其特征在于，所述获取自适应姿态估计结果的过程包括：通过非机动状态下加速度计的输出和加速度计实际的输出的夹角、陀螺仪输出的原始角速度、对陀螺仪的输出进行补偿，获得补偿后的角速度；基于补偿后的角速度，对四元数进行更新；基于更新后的四元数结果，获取所述自适应姿态估计结果。

技术总结
本发明公开了一种基于惯性传感器的动态自适应姿态估计方法，包括：基于惯性传感器和姿态估计结果获取载体机动状态数据；对所述载体机动状态数据获取自适应补偿系数和陀螺仪零偏估计数据；对所述自适应补偿系数和陀螺仪零偏估计数据进行处理，生成互补滤波算法；基于所述互补滤波算法获取自适应姿态估计结果。本发明在静态条件下，提出方法能取得良好的姿态估计效果；在动态条件下，本发明提出方法精度优于传统的互补滤波；本发明提出方法受机动影响小于传统的互补滤波。影响小于传统的互补滤波。影响小于传统的互补滤波。


技术研发人员：吕品 余文斌 赖际舟 李志敏 王炳清 孙笑 申玉桂 杜居昌
受保护的技术使用者：南京航空航天大学秦淮创新研究院
技术研发日：2023.03.16
技术公布日：2023/7/20