一种角加速度传感器与陀螺仪组合的角运动测量方法

1.本发明属于自动控制技术领域，具体涉及一种角加速度传感器与陀螺仪组合的角运动测量方法。


背景技术：

2.姿态估计技术是无人车和机器人等载体进行路径规划和运动控制的关键和基础，因此研究者针对如何获取载体精确的姿态信息开展了大量研究。在姿态解算领域，陀螺仪作为最为广泛使用的传感器，可利用其获取的角速度进行积分获取载体运动的姿态角数据。
3.激光和光纤陀螺仪的精度较高，经过积分可以获取精度较高的姿态解算结果，但是此类陀螺仪设备的成本较高，体积较大，不便于大规模的应用。mems陀螺仪的体积小、功耗低、成本低、易于批量生产，在无人驾驶和机器人领域的应用逐渐增多，不过此类陀螺仪的精度有限，长时间运行时误差较大。与此同时，角加速度传感器作为一种惯性器件，可以直接获取载体运动角加速度数据，其工作原理和误差特性与陀螺仪不同，采集的角加速度数据可经过两次积分获取载体运动的姿态信息，因此可以采用角加速度传感器与陀螺仪组合的方案，利用两种传感器特性的互补性，提升角运动测量的精度。


技术实现要素：

4.有鉴于此，本发明的目的是提供一种角加速度传感器与陀螺仪组合的角运动测量方法，可利用两者传感器的信息融合，提升角运动测量的精度。
5.一种角加速度传感器与陀螺仪组合的角运动测量方法，根据需要选择姿态层信息融合方法或数据层信息融合方法进行角运动的测量；
6.其中，所述姿态层信息融合方法包括：
7.步骤11、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度和角加速度数据，通过滤波去除数据中的随机噪声和尖峰噪声；
8.步骤12、将陀螺仪和角加速度传感器的数据对齐；
9.步骤13、对于陀螺仪，采用四元数法对载体姿态角进行描述，然后利用步骤12获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ
gyro
；
10.步骤14、对于角加速度传感器，利用步骤12获取的角加速度数据积分获取对应时刻的角速度数据；
11.步骤15、采用四元数法对载体姿态角进行描述，然后利用步骤14中获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ
aa
；
12.步骤16、步骤13利用陀螺仪获取的姿态角数据ψ
gyro
和步骤15利用角加速度传感器获取的姿态角数据ψ
aa
，构建为扩展卡尔曼滤波的量测方程，公式如下：
13.z
att
＝ψ
gyro-ψ
aa
＝h
att
·
δx
att
+v
att
14.其中，z
att
是卡尔曼滤波方程的观测量，δx
att
是卡尔曼滤波方程的状态量，h
att
是卡
尔曼滤波方程的观测矩阵，v
att
是卡尔曼滤波方程的观测噪声；
15.步骤17、利用步骤16构建的关于姿态角的量测方程，进行卡尔曼滤波，获取姿态角测量的误差估计；利用滤波结果，对基于陀螺仪获取的姿态角数据进行误差反馈校正；重复步骤11到步骤17，获取连续的载体运动的角运动数据；
16.所述数据层信息融合方法包括：
17.步骤21、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度ω
gyro
和角加速度数据，并去除数据中的随机噪声和尖峰噪声；
18.步骤22、将陀螺仪和角加速度传感器的数据对齐；
19.步骤23、对于角加速度传感器，首先利用步骤22获取的角加速度数据积分获取对应时刻的角速度数据ω
aa
；
20.步骤24、基于步骤23中通过角加速度积分获取的角速度数据ω
aa
，以及步骤22中对齐后的角速度数据ω
gyro
，构建为扩展卡尔曼滤波的量测方程，公式如下：
21.z
ang
＝ω
gyro-ω
aa
＝h
ang
·
δx
ang
+v
ang
22.其中，z
ang
是卡尔曼滤波方程的观测量，δx
ang
是卡尔曼滤波方程的状态量，h
ang
是卡尔曼滤波方程的观测矩阵，v
ang
是卡尔曼滤波方程的观测噪声；
23.步骤25、利用步骤24构建的关于角速度的量测方程，进行卡尔曼滤波，获取角速度测量的误差估计；利用滤波结果，对基于陀螺仪获取的角速度数据进行误差反馈校正；
24.步骤26、采用四元数法对载体姿态角进行描述，然后利用步骤25中获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ；重复步骤21到步骤26，获取连续的载体运动的角运动数据。
25.本发明具有如下有益效果：
26.本发明在陀螺仪测量的角速度数据的基础上，引入角加速度传感器直接测量的角加速度数据，设计一种角加速度传感器与陀螺仪的信息融合方法；该方法考虑了角加速度传感器和陀螺仪不同的误差特性，设计了两种不同的传感器数据组合方案，分别在姿态层和数据层对角加速度和角速度进行信息融合，提升角速度测量的精度，进而提高载体姿态解算的精度；相较于利用陀螺仪阵列提升角速度测量精度的方式，本发明的方法安装方式较简单，对环境的适应性更强。同时，引入的角加速度传感器成本较低，便于开展大规模应用。
附图说明
27.图1为本发明角加速度传感器与陀螺仪姿态层组合的流程框图。
28.图2为本发明角加速度传感器与陀螺仪数据层组合的流程框图。
29.图3为本发明方法与纯陀螺仪在三个轴向上的姿态解算结果。
30.图4为本发明方法与纯陀螺仪在三个轴向上的姿态解算误差结果。
具体实施方式
31.下面结合附图并举实施例，对本发明进行详细描述。
32.针对现有低成本陀螺仪长时间运行时误差逐渐发散的问题，利用误差特性不同的角加速度传感器，引入角加速度数据与角速度进行信息融合。本发明提供了一种角加速度
传感器与陀螺仪组合的角运动测量方法，包含在姿态层或数据层两种不同的组合模式。姿态层的组合模式两种传感器独立工作，实现方式简单，不易受传感器故障影响；数据层组合模式从原始数据层面进行组合，消除原始数据误差，精度更高，两种组合模式可根据实际工况进行选取。姿态层和数据层两种组合模式的具体过程分别如图1和图2所示，包括：
33.步骤11、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度和角加速度数据，通过滤波去除传感器原始数据中的随机噪声和尖峰噪声。
34.步骤12、由于两种惯性传感器的采样频率存在差异，通过数据插值的方式，实现步骤11中两种传感器数据的数据对齐。
35.对于图1所示的角加速度传感器与陀螺仪姿态层组合模式，两种传感器分别独立工作解算载体姿态，利用卡尔曼滤波器实现两种传感器解算的载体姿态的组合，步骤如下：
36.步骤13、对于陀螺仪，采用四元数法对载体姿态角进行描述，然后利用步骤12获取的角速度数据，对姿态更新微分方程进行求解，即可获取载体运动的姿态角数据ψ
gyro
。
37.步骤14、对于角加速度传感器，首先利用步骤12获取的角加速度数据积分获取对应时刻的角速度数据，公式如下：
[0038][0039]
其中，δt为角加速度传感器离散采样周期，为三个轴向上k时刻的角加速度数据，为三个轴向上k时刻的角速度数据。
[0040]
步骤15、同样采用四元数法对载体姿态角进行描述，然后利用步骤14中获取的角速度数据，对姿态更新微分方程进行求解，即可获取载体运动的姿态角数据ψ
aa
。
[0041]
步骤16、步骤13利用陀螺仪获取的姿态角数据ψ
gyro
和步骤15利用角加速度传感器获取的姿态角数据ψ
aa
，两者物理意义相同，但是数据源不同，因此将两者相减构建为扩展卡尔曼滤波的量测方程，公式如下：
[0042]zatt
＝ψ
gyro-ψ
aa
＝h
att
·
δx
att
+v
att
[0043]
其中，z
att
是卡尔曼滤波方程的观测量，δx
att
是卡尔曼滤波方程的状态量，h
att
是卡尔曼滤波方程的观测矩阵，v
att
是卡尔曼滤波方程的观测噪声。
[0044]
步骤17、利用步骤16构建的关于姿态角的量测方程，进行卡尔曼滤波，获取姿态角测量的误差估计。利用滤波结果，对基于陀螺仪获取的姿态角数据进行误差反馈校正。重复步骤11到步骤17，即可获取连续的载体运动的角运动数据。
[0045]
对于图2所示的角加速度传感器与陀螺仪数据层组合模式，直接在原始数据层面上，利用卡尔曼滤波器对角加速度传感器和陀螺仪两种传感器的信息进行融合，获取精度更高的角速度数据，进一步解算获取载体姿态，步骤如下：
[0046]
步骤21、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度和角加速度数据，通过滤波去除传感器原始数据中的随机噪声和尖峰噪声。
[0047]
步骤22、由于两种惯性传感器的采样频率存在差异，通过数据插值的方式，实现步骤21中两种传感器数据的数据对齐。
[0048]
步骤23、对于角加速度传感器，首先利用步骤22获取的角加速度数据积分获取对应时刻的角速度数据，公式如下：
[0049]
ω
aa
(k)＝ω
aa
(k-1)+α
aa
(k)δt
[0050]
其中，δt为角加速度传感器离散采样周期，α
aa
(k)为k时刻采集的角加速度数据，ω
aa
(k)为k时刻的角速度数据。
[0051]
步骤24、步骤23中通过角加速度积分获取的角速度数据ω
aa
，以及步骤22中陀螺仪采集获取的角速度数据ω
gyro
，两者物理意义相同，但是数据源不同，因此将两者相减构建为扩展卡尔曼滤波的量测方程，公式如下：
[0052]zang
＝ω
gyro-ω
aa
＝h
ang
·
δx
ang
+v
ang
[0053]
其中，z
ang
是卡尔曼滤波方程的观测量，δx
ang
是卡尔曼滤波方程的状态量，h
ang
是卡尔曼滤波方程的观测矩阵，v
ang
是卡尔曼滤波方程的观测噪声。
[0054]
步骤25、利用步骤24构建的关于角速度的量测方程，进行卡尔曼滤波，获取角速度测量的误差估计。利用滤波结果，对基于陀螺仪获取的角速度数据进行误差反馈校正，提升角速度数据的测量精度。
[0055]
步骤26、采用四元数法对载体姿态角进行描述，然后利用步骤25中获取的角速度数据，对姿态更新微分方程进行求解，即可获取载体运动的姿态角数据ψ。重复步骤21到步骤26，即可获取连续的载体运动的角运动数据。
[0056]
本发明在实验室标准三轴转台上开展了实验测试进行精度验证。实验时，转台上搭载了包含低成本陀螺仪的惯导以及角加速度传感器，通过控制转台在三个轴向上的转动获取角运动数据。实验时，转台上同时搭载了高精度激光惯导，用于提供角运动真值。纯陀螺仪测量法和本发明提出的角加速度传感器与陀螺仪组合的角运动测量方法(数据层)的测量结果如图3和图4所示，具体误差指标如下：
[0057][0058][0059]
通过实验结果可知，相较于纯陀螺仪的姿态解算结果，本发明提出的角加速度传感器与陀螺仪组合的角运动测量方法将姿态角解算结果的最大残差相对减小了35.04％，姿态角解算结果的平均残差相对减小了20.81％，提升角运动测量的精度。
[0060]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。
凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种角加速度传感器与陀螺仪组合的角运动测量方法，其特征在于，根据需要选择姿态层信息融合方法或数据层信息融合方法进行角运动的测量；其中，所述姿态层信息融合方法包括：步骤11、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度和角加速度数据，通过滤波去除数据中的随机噪声和尖峰噪声；步骤12、将陀螺仪和角加速度传感器的数据对齐；步骤13、对于陀螺仪，采用四元数法对载体姿态角进行描述，然后利用步骤12获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ
gyro
；步骤14、对于角加速度传感器，利用步骤12获取的角加速度数据积分获取对应时刻的角速度数据；步骤15、采用四元数法对载体姿态角进行描述，然后利用步骤14中获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ
aa
；步骤16、步骤13利用陀螺仪获取的姿态角数据ψ
gyro
和步骤15利用角加速度传感器获取的姿态角数据ψ
aa
，构建为扩展卡尔曼滤波的量测方程，公式如下：z
att
＝ψ
gyro-ψ
aa
＝h
att
·
δx
att
+v
att
其中，z
att
是卡尔曼滤波方程的观测量，δx
att
是卡尔曼滤波方程的状态量，h
att
是卡尔曼滤波方程的观测矩阵，v
att
是卡尔曼滤波方程的观测噪声；步骤17、利用步骤16构建的关于姿态角的量测方程，进行卡尔曼滤波，获取姿态角测量的误差估计；利用滤波结果，对基于陀螺仪获取的姿态角数据进行误差反馈校正；重复步骤11到步骤17，获取连续的载体运动的角运动数据；所述数据层信息融合方法包括：步骤21、利用陀螺仪和角加速度传感器，分别采集载体运动时的角速度ω
gyro
和角加速度数据，并去除数据中的随机噪声和尖峰噪声；步骤22、将陀螺仪和角加速度传感器的数据对齐；步骤23、对于角加速度传感器，首先利用步骤22获取的角加速度数据积分获取对应时刻的角速度数据ω
aa
；步骤24、基于步骤23中通过角加速度积分获取的角速度数据ω
aa
，以及步骤22中对齐后的角速度数据ω
gyro
，构建为扩展卡尔曼滤波的量测方程，公式如下：z
ang
＝ω
gyro-ω
aa
＝h
ang
·
δx
ang
+v
ang
其中，z
ang
是卡尔曼滤波方程的观测量，δx
ang
是卡尔曼滤波方程的状态量，h
ang
是卡尔曼滤波方程的观测矩阵，v
ang
是卡尔曼滤波方程的观测噪声；步骤25、利用步骤24构建的关于角速度的量测方程，进行卡尔曼滤波，获取角速度测量的误差估计；利用滤波结果，对基于陀螺仪获取的角速度数据进行误差反馈校正；步骤26、采用四元数法对载体姿态角进行描述，然后利用步骤25中获取的角速度数据，对姿态更新微分方程进行求解，获取载体运动的姿态角数据ψ；重复步骤21到步骤26，获取连续的载体运动的角运动数据。

技术总结
本发明公开了一种角加速度传感器与陀螺仪组合的角运动测量方法，该方法考虑了角加速度传感器和陀螺仪不同的误差特性，设计了两种不同的传感器数据组合方案，分别在姿态层和数据层对角加速度和角速度进行信息融合，提升角速度测量的精度，进而提高载体姿态解算的精度；相较于利用陀螺仪阵列提升角速度测量精度的方式，本发明的方法安装方式较简单，对环境的适应性更强。同时，引入的角加速度传感器成本较低，便于开展大规模应用。便于开展大规模应用。便于开展大规模应用。


技术研发人员：王美玲 翟朝阳 刘钰倩 肖志恒
受保护的技术使用者：北京理工大学
技术研发日：2023.07.04
技术公布日：2023/10/8