CN110986999B - 陀螺仪校漂方法、校漂装置、存储介质、电子设备及无人机 - Google Patents

Abstract

本发明公开了一种陀螺仪校漂方法、校漂装置、存储介质、电子设备及无人机，其包括如下步骤：S1、在静态条件下通过数据交换获取陀螺仪角速率数据以及陀螺仪角速率积分；S2、根据公式计算得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；S3、将所述闭环校漂误差反馈Err与预设校漂阈值进行对比；以及S4、根据存储的校漂补偿值Out_校对陀螺仪漂移量进行校正。本发明基于闭环反馈对陀螺仪进行零位校漂，由此避免陀螺校漂过程中噪声尖峰的影响，提高陀螺仪校漂的精度，同时闭环修正过程会自动实时检测校漂结果是否满足阈值指标，无需人工确认。

Description

陀螺仪校漂方法、校漂装置、存储介质、电子设备及无人机

技术领域

本发明涉及无人机技术领域，具体为一种基于闭环反馈的陀螺仪校漂方法、校漂装置、存储介质、电子设备及无人机。

背景技术

陀螺仪是无人机飞行姿态的感知器，其在工作时容易出现零位漂移现象，因此陀螺仪使用前需要进行校漂操作，以保证后续无人机飞控系统所读数据的准确性，使无人机在飞行过程中能够平稳运行。

常用的陀螺仪零位校漂方式是在陀螺仪绝对静态条件下，对一段时间内陀螺仪获取的数据求和取均值，将该均值作为陀螺仪零位漂移量。但陀螺仪工作时不可避免会存在一些噪声尖峰，另外校正过程中陀螺仪易受外界环境因素影响而产生晃动，如果出现晃动陀螺仪数据尖峰跳动会更大，因此，采用上述方法获取的均值会把误差引入，造成陀螺仪零位校漂不准确，同时该种方式属于开环操作，校漂后陀螺仪零漂能否满足要求还需要人工确认,自动化程度不高。

发明内容

针对现有技术的不足，本发明提供了一种基于闭环反馈的陀螺仪校漂方法、校漂装置、存储介质、电子设备及无人机，其基于闭环反馈对陀螺仪进行零位校漂，由此避免陀螺校漂过程中噪声尖峰的影响，提高陀螺仪校漂的精度，同时闭环修正过程会自动实时检测校漂结果是否满足阈值指标，无需人工确认。

为实现上述目的，本发明提供如下技术方案：

一方面，提供了一种陀螺仪校漂方法，其包括如下步骤：

S1、在静态条件下通过数据交换获取陀螺仪角速率数据；

按照下述公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分；

Gyro_Sum＝(Gyro_Spd+Out_初)×Int_Ts×Int_Cnt (1)；

其中，Gyro_Spd为陀螺仪角速率,Out_初为初始校漂补偿值，Int_Ts为陀螺仪角速率积分周期,Int_Cnt为积分周期的累加个数，Gyro_Sum为陀螺仪角速率积分；

S2、当Int_Cnt≥次数阈值Int_Time后，根据下述公式(2)-(6)计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；

Err＝Gyro_Sum (2)

Out_校＝Kp×Err+Ki×Sum_Err (3)

Sum_Err＝Pre_Sum_Err+Err (4)

Pre_Sum_Err＝Sum_Err (5)

Out_初＝Out_校 (6)；

其中，Kp为比例系数，Ki为积分系数，Sum_Err为当前误差积分累加值，Pre_Sum_Err为前一时刻误差积分累加值，Err为闭环校漂误差反馈；

S3、将所述闭环校漂误差反馈Err与预设校漂阈值进行对比；

若所述闭环校漂误差反馈Err连续M次小于预设校漂阈值，则将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次运算，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储；

以及S4、根据存储的校漂补偿值Out_校对陀螺仪漂移量进行校正。

优选的，按照公式(2)-(6)进行若干次运算的过程中，所述运算包括P、PD、PID、非线性控制运算方式中的一种或几种。

另一方面，还提供一种用于实现上述陀螺仪校漂方法的陀螺仪校漂装置，其包括：

陀螺传感器，其用于在静态条件下通过数据交换获取陀螺仪角速率数据；

外设接口，其与所述陀螺传感器电性连接；

校漂控制器，其通过所述外设接口与所述陀螺传感器电性连接，用于根据公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分；以及当根据公式(2)-(6)进行计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；

以及存储器，其通过所述外设接口与所述校漂控制器电性连接；

若所述闭环校漂误差反馈Err连续M次小于预设校漂阈值，则所述校漂控制器将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器进行存储；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次闭环运算，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，所述校漂控制器将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器并进行存储。

优选的，所述陀螺传感器包括MEMS陀螺、光纤陀螺、挠性陀螺中的一种或几种。

优选的，所述校漂控制器包括中央处理器、数字信号处理器、专用集成电路、现场可编程门阵列、单片机中的一种或几种。

一方面，还提供一种存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述陀螺仪校漂方法。

一方面，还提供一种电子设备，其包括存储介质、处理器及存储在该可读存储介质上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述陀螺仪校漂方法。

一方面，还提供一种无人机，其包括上述校漂装置，或，上述存储介质，或，上述电子设备。

与现有技术相比，本发明具备以下有益效果：

本发明基于闭环反馈对陀螺仪进行零位校漂，由此避免陀螺校漂过程中噪声尖峰的影响，提高陀螺仪校漂的精度，同时闭环修正过程会自动实时检测校漂结果是否满足阈值指标，无需人工确认。

附图说明

图1为本发明实施例一中基于闭环反馈的陀螺仪校漂方法的步骤图；

图2为本发明实施例一中基于闭环反馈的陀螺仪校漂方法的流程图；

图3为本发明实施例三中陀螺仪校漂装置的结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一：

如图1-2所示，本发明中基于闭环反馈的陀螺仪校漂方法包括如下步骤：

S1、在静态条件下通过数据交换获取陀螺仪角速率数据；

按照下述公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分，同时，该陀螺仪角速率积分可作为该时间段内的陀螺仪漂移量；

Gyro_Sum＝(Gyro_Spd+Out_初)×Int_Ts×Int_Cnt (1)；

其中，Gyro_Spd为陀螺仪角速率,Out_初为初始校漂补偿值，Int_Ts为陀螺仪角速率积分周期(例如积分周期可为1ms),Int_Cnt为积分周期的累加个数(一个积分周期内累加一次，单位是‘次’)，Gyro_Sum为陀螺仪角速率积分；

且将积分周期的累加个数Int_Cnt与次数阈值Int_Time进行比较，其中次数阈值Int_Time为根据实际设置的积分次数阈值，例如200次；若Int_Cnt≥Int_Time，则执行下一步骤；若Int_Cnt＜Int_Time，则继续进行按照上述公式(1)获取陀螺仪角速率积分，直至Int_Cnt≥Int_Time；

S2、当Int_Cnt≥Int_Time后，例如积分计数满足200次阈值后，根据下述公式(2)-(6)进行依次循环计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；

Err＝Gyro_Sum (2)

Out_校＝Kp×Err+Ki×Sum_Err (3)

Sum_Err＝Pre_Sum_Err+Err (4)

Pre_Sum_Err＝Sum_Err (5)

Out_初＝Out_校 (6)；

其中，Kp为比例系数，Ki为积分系数(Kp、Ki均为常规PI运算控制参数)，Sum_Err为当前误差积分累加值，Pre_Sum_Err为前一时刻误差积分累加值，Err为闭环校漂误差反馈；

S3、将所述闭环校漂误差反馈Err与预设校漂阈值进行对比；

若所述闭环校漂误差反馈Err连续M次(M为正整数，例如10)小于预设校漂阈值，则将与上述连续M次判断中的第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储，且积分周期个数Int_Cnt与陀螺仪角速率积分Gyro_Sum均清零；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将上述M次判断中、与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次闭环运算(所述闭环运算包括P、PD、PID、非线性控制等运算方式中的一种或几种)，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校，且每次闭环运算完毕后将积分周期个数Int_Cnt与陀螺仪角速率积分Gyro_Sum均清零；如此进行迭代运算，直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储，且积分周期个数Int_Cnt与陀螺仪角速率积分Gyro_Sum均清零；

其中，所述校漂阈值根据实际系统校漂精度需求进行设置，闭环校漂误差反馈Err可以取绝对值与校漂阈值进行判断，例如|Err|＜校漂阈值，或者闭环校漂误差反馈Err可以取正负数与校漂阈值进行大小判断，例如校漂阈值为正数，且当Err＜0时，判断条件为Err＞-校漂阈值，校漂阈值为正数，且当Err≥0时，判断条件为Err＜校漂阈值；

以及S4、根据存储的校漂补偿值Out_校对陀螺仪漂移量进行自动校正。

实施例二：

本实施例提供了一种用于实现实施例一所述陀螺仪校漂方法的陀螺仪校漂装置，如图3所示，其包括：

陀螺传感器1，其用于在静态条件下通过数据交换获取陀螺仪角速率数据；优选的，所述陀螺传感器1包括MEMS陀螺、光纤陀螺、挠性陀螺等惯性角速率测量传感器中的一种或几种；

外设接口2，其与所述陀螺传感器1电性连接；

校漂控制器3，其通过所述外设接口2与所述陀螺传感器1电性连接，用于根据公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分；以及当Int_Cnt≥Int_Time后，根据公式(2)-(6)进行依次循环计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；所述校漂控制器3包括中央处理器(Central Processing Uint,简称CPU)、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、单片机中的一种或几种；

存储器4，其通过所述外设接口2与所述校漂控制器3电性连接；具体的，所述存储器4包括随机存取存储器(Random Acess Memory,RAM)、只读存储器(Read Only Memory,ROM),可编程只读存储器(Programmable Read-Only Memory,PROM)、可擦除只读存储器(Erasable Programmable Read-Only Memory,EPROM)、电可擦除只读存储器(ElectricErasable Programmable Read-Only Memory,EEPROM)中的一种或几种；

若所述闭环校漂误差反馈Err连续M次(M为正整数，例如10)小于预设校漂阈值，则所述校漂控制器3将上述连续M次判断中的与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器4进行存储；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将上述连续M次判断中、与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次闭环运算，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；如此进行迭代运算，直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，所述校漂控制器3将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器4并进行存储。

实施例三：

本实施例提供了一种可读存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述基于闭环反馈的陀螺仪校漂方法。

实施例四：

本实施例提供了一种电子设备，其包括实施例四所述的可读存储介质、处理器及存储在该可读存储介质上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述基于闭环反馈的陀螺仪校漂方法。

实施例五：

本实施例提供了一种无人机，其包括实施例二中的校漂装置，或，实施例三中的可读存储介质，或实施例四所述的电子设备。

综上所述，本发明可通过陀螺仪角速率积分直观反映出静态条件下规定时间段内的陀螺漂移量；同时，利用闭环控制原理，将闭环反馈校正的方法用于陀螺仪校漂操作中，以实现对陀螺仪漂移量的实时校正补偿，由此可以避免陀螺仪校漂过程中噪声尖峰的影响，提高陀螺仪校漂的精度和鲁棒性；且闭环修正过程会自动实时检测校漂结果是否满足阈值指标，并在判断满足校漂阈值指标后将校漂补偿值自动存入存储器，无需人工干预和确认，由此提高自动化程度。

需要说明的是，上述实施例一至五中的技术特征可进行任意组合，且组合而成的技术方案均属于本发明的保护范围。在本文中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (8)

1.一 种陀螺仪校漂方法，其特征在于，包括如下步骤：

S1、在静态条件下通过数据交换获取陀螺仪角速率数据；

按照下述公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分；

Gyro_Sum＝(Gyro_Spd+Out_初)×Int_Ts×Int_Cnt(1)；

其中，Gyro_Spd为陀螺仪角速率,Out_初为初始校漂补偿值，Int_Ts为陀螺仪角速率积分周期,Int_Cnt为积分周期的累加个数，Gyro_Sum为陀螺仪角速率积分；

S2、当Int_Cnt≥次数阈值Int_Time后，根据下述公式(2)-(6)计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；

Err＝Gyro_Sum (2)

Out_校＝Kp×Err+Ki×Sum_Err (3)

Sum_Err＝Pre_Sum_Err+Err (4)

Pre_Sum_Err＝Sum_Err (5)

Out_初＝Out_校 (6)；

其中，Kp为比例系数，Ki为积分系数，Sum_Err为当前误差积分累加值，Pre_Sum_Err为前一时刻误差积分累加值，Err为闭环校漂误差反馈；

S3、将所述闭环校漂误差反馈Err与预设校漂阈值进行对比；

若所述闭环校漂误差反馈Err连续M次小于预设校漂阈值，则将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次运算，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出并进行存储；

以及S4、根据存储的校漂补偿值Out_校对陀螺仪漂移量进行校正。

2.如权利要求1所述的陀螺仪校漂方法，其特征在于，按照公式(2)-(6)进行若干次运算的过程中，所述运算包括P、PD、PID中的一种或几种。

3.一种用于实现权利要求1-2任一项所述陀螺仪校漂方法的陀螺仪校漂装置，其特征在于，包括：

陀螺传感器，其用于在静态条件下通过数据交换获取陀螺仪角速率数据；

外设接口，其与所述陀螺传感器电性连接；

校漂控制器，其通过所述外设接口与所述陀螺传感器电性连接，用于根据公式(1)对陀螺仪角速率进行积分，以获取时间长度为Int_Ts×Int_Cnt的陀螺仪角速率积分；以及当根据公式(2)-(6)进行计算，以得出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；

以及存储器，其通过所述外设接口与所述校漂控制器电性连接；

若所述闭环校漂误差反馈Err连续M次小于预设校漂阈值，则所述校漂控制器将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器进行存储；

若所述闭环校漂误差反馈Err在连续M次判断中至少有一个大于或等于预设校漂阈值，则将与该大于或等于预设校漂阈值的闭环校漂误差反馈Err对应的校漂补偿值Out_校代入公式(1)，以替代Out_初计算出新的闭环校漂误差反馈Err，再按照公式(2)-(6)进行若干次闭环运算，再次计算出若干个闭环校漂误差反馈Err以及与每一闭环校漂误差反馈Err对应的校漂补偿值Out_校；直至闭环校漂误差反馈Err连续M次均小于所述预设校漂阈值时，所述校漂控制器将与第M个闭环校漂误差反馈Err对应的校漂补偿值Out_校自动输出至所述存储器并进行存储。

4.如权利要求3所述的校漂装置，其特征在于，所述陀螺传感器包括MEMS陀螺、光纤陀螺、挠性陀螺中的一种或几种。

5.如权利要求3所述的校漂装置，其特征在于，所述校漂控制器包括中央处理器、数字信号处理器、专用集成电路、现场可编程门阵列、单片机中的一种或几种。

6.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被执行时实现上述权利要求1-2任一项所述的陀螺仪校漂方法。

7.一种电子设备，其包括存储介质、处理器及存储在该存储介质上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述权利要求1-3任一项所述的陀螺仪校漂方法。

8.一种无人机，其特征在于，包括权利要求3-5任一项所述的校漂装置，或，权利要求6所述的存储介质，或，权利要求7所述的电子设备。

Images