基于双陀螺仪差分算法的零漂消除方法及系统

本发明涉及惯性导航，具体地，涉及基于双陀螺仪差分算法的零漂消除方法及系统。

背景技术：

1、惯性测量单元（inertial measurement unit，imu）是移动机器人和无人系统的核心传感器之一，用于测量载体的角速度和加速度，进而通过积分运算获得姿态角信息。在移动机器人导航与控制中，航向角（yaw角）的精确测量对于路径跟踪、定位建图和自主导航具有关键意义。

2、陀螺仪作为imu的核心组件，通过测量载体绕各轴的角速度，经积分运算可得到旋转角度。然而，mems陀螺仪存在固有的零偏（zero bias）误差，即在静止状态下输出非零角速度值。该零偏会随积分时间累积，导致角度计算产生严重漂移。此外，零偏还具有温度敏感性，即温漂特性，进一步加剧了误差累积问题。

3、目前，针对陀螺仪零漂问题，主要采用以下技术方案：

4、（1）单陀螺仪配合地磁传感器融合方案

5、该方案采用单个陀螺仪与地磁传感器（电子罗盘）组合，通过卡尔曼滤波或互补滤波算法融合两种传感器数据。陀螺仪提供高频动态响应，地磁传感器提供绝对航向参考，用于定期校正陀螺仪积分漂移。

6、（2）温度补偿算法方案

7、该方案通过建立陀螺仪零偏与温度之间的数学模型，在不同温度点对零偏进行标定，建立温度-零偏查找表或拟合曲线。实际应用时根据实时温度测量值对输出进行补偿。

8、（3）零偏在线估计与扣除方案

9、该方案在系统静止状态下，将陀螺仪输出作为零偏估计值并予以扣除。部分方案采用滑动平均或低通滤波提高零偏估计的稳定性。

10、上述现有技术存在以下技术缺陷：

11、（1）地磁传感器易受干扰，适用场景受限

12、在智能小车、机器人等应用场景中，电机、舵机等大功率电气设备工作时会产生强磁场干扰，导致地磁传感器测量失效或严重失真。因此，在电机干扰环境下无法可靠使用地磁传感器进行航向校正，该方案的适用性受到严重限制。

13、（2）温度补偿算法复杂且补偿不彻底

14、温度补偿需要预先进行大量的温度标定实验，建立精确的温度模型，开发工作量大。此外，温度变化存在滞后性，而陀螺仪零偏对温度变化的响应具有实时性，导致温度补偿存在时滞误差。实际应用中，温度补偿只能部分改善零漂问题，无法彻底消除。

15、（3）在线零偏估计要求静止条件，动态场景不适用

16、在线零偏估计方案要求系统处于静止状态才能准确估计零偏，而移动机器人在运动过程中无法区分角速度输出中的真实信号与零偏分量，因此该方案在动态场景下失效。

17、（4）单传感器方案 inherently 存在零漂累积问题

18、无论采用何种软件补偿算法，单陀螺仪方案本质上无法区分真实角速度与零偏误差，零漂累积问题无法从根本上解决。长时间运行后，角度误差持续累积，严重影响导航精度。

19、（5）传统双陀螺仪方案缺乏零漂特性前置观察

20、现有双陀螺仪方案通常假设两个传感器的零漂特性已知或遵循某种固定模式（如对置安装时假设零漂方向相同），未考虑零漂的个体差异性和方向不确定性。实际应用中，同一型号不同个体的陀螺仪零漂方向和幅度可能存在显著差异，甚至相差数倍，导致传统融合算法失效。

21、而申请号为202311753904 .7的中国发明申请公开了《陀螺仪的标定方法、装置及电子设备》，所述方法包括：实时采集陀螺仪数据，并触发陀螺仪的标定模式；在陀螺仪的标定模式中，采集陀螺仪的静态数据和陀螺仪的动态数据；基于陀螺仪的静态数据确定车辆坐标系z轴在陀螺仪坐标系下的第一单位方向向量；基于陀螺仪的动态数据确定车辆坐标系x轴在陀螺仪坐标系下的第二单位方向向量；根据第一单位方向向量和第二单位方向向量确定车辆坐标系y轴在陀螺仪坐标系下的第三单位方向向量；根据第一单位方向向量、第二单位方向向量和第三单位方向向量确定陀螺仪的零飘误差，并完成陀螺仪的标定。

技术实现思路

1、为解决上述背景技术存在的技术问题，本发明提供了基于双陀螺仪差分算法的零漂消除方法及系统，本发明采用的方案是：

2、本发明第一方面提供了基于双陀螺仪差分算法的零漂消除方法，包括以下步骤：

3、零漂特性前置观察步骤：将第一陀螺仪和第二陀螺仪分别安装于同一载体上，并在静止状态下分别采集二者的角速度输出数据，根据采集的角速度输出数据分析并确定第一陀螺仪和第二陀螺仪的零漂方向关系以及零漂幅度比例系数k；

4、自适应安装策略选择步骤：根据所述零漂方向关系，选择第一陀螺仪与第二陀螺仪的相对安装方式；

5、系数补偿与差分融合步骤：基于所述零漂幅度比例系数k，对第二陀螺仪的角速度输出进行幅度补偿，得到补偿后的角速度；根据所选择的安装方式，将第一陀螺仪的角速度与补偿后的角速度进行加法或减法融合运算，得到融合角速度，以消除零漂；

6、角度积分步骤：对所述融合角速度进行时间积分，得到用于导航控制的航向角。

7、作为一种优选方案，将第一陀螺仪和第二陀螺仪分别安装于同一载体上，并在静止状态下分别采集二者的角速度输出数据，根据采集的角速度输出数据分析并确定第一陀螺仪和第二陀螺仪的零漂方向关系以及零漂幅度比例系数k的方法包括：

8、将第一陀螺仪和第二陀螺仪均正放安装于静止测试平台上；

9、连续采集预定时间内的两个陀螺仪的角速度输出数据；

10、分别计算所述第一陀螺仪和所述第二陀螺仪在静止状态下的平均角速度，作为其零偏值，分别得到零偏值bias_a和零偏值bias_b；

11、根据bias_a和bias_b的正负符号确定零漂方向关系：若符号相反则判定为零漂方向相反，若符号相同则判定为零漂方向相同；

12、计算零漂幅度比例系数k = |bias_a| / |bias_b|。

13、作为一种优选方案，根据所述零漂方向关系，选择第一陀螺仪与第二陀螺仪的相对安装方式的方法包括：

14、当判定所述第一陀螺仪与所述第二陀螺仪的零漂方向相反时，选择同置安装方式，使所述第一陀螺仪与所述第二陀螺仪的测量轴方向相同；

15、当判定所述第一陀螺仪与所述第二陀螺仪的零漂方向相同时，选择对置安装方式，使所述第一陀螺仪与所述第二陀螺仪的测量轴方向相反。

16、作为一种优选方案，当零漂方向相反且采用同置安装时，进行加法融合运算，具体计算过程包括：

17、建立测量方程：

18、ω_a = ω_true + bias_a

19、ω_b = ω_true + bias_b

20、其中，ω_true为真实角速度，ω_a为第一陀螺仪的z轴角速度测量值，ω_b为第二陀螺仪的z轴角速度测量值，bias_a 与 bias_b 符号相反；

21、对第二陀螺仪的角速度输出进行幅度补偿，得到补偿后的角速度，具体公式为：

22、ω_b_comp = ω_b × k

23、将ω_b = ω_true + bias_b代入补偿后的角速度的计算公式：

24、ω_b_comp =（ω_true + bias_b）× k

25、加法融合公式为：

26、ω_fused = (ω_a + ω_b_comp) / (1 + k)

27、将ω_a = ω_true + bias_a、ω_b_comp =（ω_true + bias_b）× k代入加法融合公式：

28、ω_fused = [ω_true + bias_a + (ω_true + bias_b) × k] / (1 + k)

29、= ω_true + (bias_a + bias_b × k) / (1 + k)

30、其中ω_fused为融合角速度。

31、作为一种优选方案，当零漂方向相同且采用对置安装时，进行减法融合运算，具体计算过程包括：

32、建立测量方程：

33、ω_a = ω_true + bias_a

34、ω_b =-ω_true + bias_b

35、其中，ω_true为真实角速度，ω_a为第一陀螺仪的z轴角速度测量值，ω_b为第二陀螺仪的z轴角速度测量值，bias_a 与 bias_b 符号相同；

36、对第二陀螺仪的角速度输出进行幅度补偿，得到补偿后的角速度，具体公式为：

37、ω_b_comp = ω_b × k

38、将ω_b = ω_true + bias_b代入补偿后的角速度的计算公式：

39、ω_b_comp =（-ω_true + bias_b）× k

40、加法融合公式为：

41、ω_fused = (ω_a-ω_b_comp) / (1 + k)

42、将ω_a = ω_true + bias_a、ω_b_comp =（ω_true + bias_b）× k代入加法融合公式：

43、ω_fused = [ω_true + bias_a - (-ω_true + bias_b) × k] / (1 + k)

44、= ω_true + (bias_a - bias_b × k) / (1 + k)

45、其中ω_fused为融合角速度。

46、作为一种优选方案，对所述融合角速度进行时间积分，得到用于导航控制的航向角的方法包括：

47、对所述融合角速度进行时间积分的公式为：

48、θ_fused(t) =ω_fused(τ) dτ

49、所述时间积分采用离散化方式实现，采样周期为t_s：

50、θ_fused[k] = θ_fused[k-1]+ ω_fused[k] · t_s

51、其中，θ_fused[k]为第k个采样时刻的融合航向角，ω_fused[k]为第k个采样时刻的融合角速度。

52、本发明第二方面提供了基于双陀螺仪差分算法的零漂消除系统，用于实现前述的方法，包括：

53、载体；

54、固定安装于所述载体上的第一陀螺仪和第二陀螺仪，两者的相对安装方式根据零漂方向关系确定为同置安装或对置安装；

55、主控制器，与两个陀螺仪通信连接，用于执行零漂特性前置观察、自适应安装策略选择、系数补偿与差分融合以及角度积分步骤；

56、边缘计算单元，与所述主控制器通信连接，用于接收融合后的航向角并执行导航控制。

57、作为一种优选方案，所述主控制器包括：

58、零漂特性前置观察模块，用于将第一陀螺仪和第二陀螺仪分别安装于同一载体上，并在静止状态下分别采集二者的角速度输出数据，根据采集的角速度输出数据分析并确定第一陀螺仪和第二陀螺仪的零漂方向关系以及零漂幅度比例系数k；

59、自适应安装策略选择模块，用于根据所述零漂方向关系，选择第一陀螺仪与第二陀螺仪的相对安装方式；

60、系数补偿与差分融合模块，用于基于所述零漂幅度比例系数k，对第二陀螺仪的角速度输出进行幅度补偿，得到补偿后的角速度；根据所选择的安装方式，将第一陀螺仪的角速度与补偿后的角速度进行加法或减法融合运算，得到融合角速度，以消除零漂；

61、角度积分模块，用于对所述融合角速度进行时间积分，得到用于导航控制的航向角。

62、作为一种优选方案，所述主控制器包括：

63、零漂特性前置观察模块，用于将第一陀螺仪和第二陀螺仪分别安装于同一载体上，并在静止状态下分别采集二者的角速度输出数据，根据采集的角速度输出数据分析并确定第一陀螺仪和第二陀螺仪的零漂方向关系以及零漂幅度比例系数k；

64、自适应安装策略选择模块，用于根据所述零漂方向关系，选择第一陀螺仪与第二陀螺仪的相对安装方式；

65、系数补偿与差分融合模块，用于基于所述零漂幅度比例系数k，对第二陀螺仪的角速度输出进行幅度补偿，得到补偿后的角速度；根据所选择的安装方式，将第一陀螺仪的角速度与补偿后的角速度进行加法或减法融合运算，得到融合角速度，以消除零漂；

66、角度积分模块，用于对所述融合角速度进行时间积分，得到用于导航控制的航向角。

67、作为一种优选方案，所述系统还包括与所述主控制器连接的显示模块，用于实时显示第一陀螺仪积分角度、第二陀螺仪积分角度以及航向角。

68、作为一种优选方案，当零漂方向相反时，所述第一陀螺仪和第二陀螺仪采用同置安装，两者的测量轴方向相同；当零漂方向相同时，采用对置安装，两者的测量轴方向相反。

69、相较于现有技术，本发明具有的有益效果是：

70、本发明通过前置观察测定两陀螺仪的零漂方向与幅度比例系数，并根据方向关系自适应选择同置或对置安装，同时对第二陀螺仪进行系数补偿，使无论零漂方向相同或相反、幅度差异大小，均能实现零偏的精确抵消，解决了传统方案对零漂特性的盲目假设和幅度不匹配问题。本发明采用同置安装下的加法融合或对置安装下的减法融合运算，使两个陀螺仪的零偏在代数运算中相互消除，融合后角速度等于真实角速度，零漂抑制率达98%以上，长时间积分航向漂移降低约35倍。本发明仅依赖双陀螺仪差分运算，无需地磁传感器，且两传感器共处相同温度环境，从根本上避免了电机磁场干扰和复杂的温度标定补偿，提高了在电动移动平台上的适用性，并降低了系统开发难度与成本。本发明采用的算法仅涉及乘、加、除运算，计算复杂度极低，可在微控制器上实时执行，硬件仅需两个低成本mems陀螺仪，性价比显著优于高精度光纤陀螺仪或战术级imu，适用于需要长时间自主导航的各类移动机器人、无人车等场景。