一种陀螺仪漂移估计方法、装置及定位系统与流程

本发明涉及导航技术领域，尤其涉及一种陀螺仪漂移估计方法、装置及定位系统。

背景技术：

轮式机器人导航的核心包括定位和建图。定位一般采用两级结构：

s1:使用imu(inertialmeasurementunit，惯性测量单元)数据和里程计数据进行航位推算，得到开环积分位置、姿态；

s2:激光雷达基于slam(simultaneouslocalizationandmapping,即时定位与地图构建)算法、使用上一步的结果为初值，进行扫描匹配得出机器人最终的位置、姿态。

其中，航位推算的特点是具有无界累计误差，但是不受环境特征影响，可以给出持续的位姿。激光雷达slam也可以不使用imu和里程计计算的位姿初值，仅靠激光雷达就可以计算出位姿。激光雷达slam算法的位姿经过回环和后端优化之后可以最大程度的消除累计误差。但是激光雷达slam在空旷场景和简单重复场景等无特征环境下会失效。航位推算和纯激光雷达slam算法在累计误差特性、对于环境特征依赖这两方面具有互补特性。

由于imu加里程计定位是开环积分，陀螺仪的微小零偏持续积分就会导致较大的姿态、位置误差，由于没有闭环反馈修正，导致姿态、位置误差是无界的增长。

技术实现要素：

鉴于上述的分析，本发明旨在提供一种陀螺仪漂移估计方法、装置及定位系统，采用机器人的定位误差作为量测量，估计出imu的陀螺仪零偏并反馈，提高航位推算得到的姿态、位置的精度。

本发明的目的主要是通过以下技术方案实现的：

一方面，公开了一种陀螺仪漂移估计方法，包括：

基于imu数据和里程计数据进行航位推算，得到机器人的第一位置

基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；

以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏

进一步地，在所述以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:

构建imu模型和航位推算的系统误差方程；

根据imu模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。

进一步地，所述陀螺仪零偏为一阶马尔科夫过程随机误差。

进一步地，所述卡尔曼滤波的状态空间模型为所述卡尔曼滤波的量测模型为z＝hx+v；

其中，为卡尔曼滤波的状态向量；f为卡尔曼滤波的状态转移矩阵；g为卡尔曼滤波的噪声矩阵；w为卡尔曼滤波的状态噪声向量；z为卡尔曼滤波的量测向量；h为卡尔曼滤波的量测矩阵；v为卡尔曼滤波的量测噪声向量。

进一步地，

所述卡尔曼滤波的状态转移矩阵

所述卡尔曼滤波的噪声矩阵

所述卡尔曼滤波的状态噪声向量

所述卡尔曼滤波的量测矩阵

其中，hf为卡尔曼滤波的比力量测矩阵；hd为卡尔曼滤波的位置量测矩阵。

进一步地，所述采用卡尔曼滤波实时估计陀螺仪零偏包括：

当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏。

进一步地，所述陀螺仪零偏用于修正陀螺仪量测的角速率。

进一步地，还包括陀螺仪状态监测，所述陀螺仪状态监测包括：

获取卡尔曼滤波产生的测量新息；

对所述测量新息进行归一化得到测量新息的幅值；

比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。

另一方面，还公开了一种陀螺仪漂移估计装置，包括，

航位推算模块，基于imu数据和里程计数据进行航位推算，得到机器人的第一位置

定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；

陀螺仪零偏估计模块，以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏

另一方面，还公开了一种定位系统，包括imu单元、里程计、平面激光雷达和如上所述的陀螺仪漂移估计装置；

所述imu单元、里程计分别与所述航位推算模块连接，分别输出imu数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置

所述平面激光雷达，用于采用slam算法计算机器人的第二位置并输出至所述定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpⁿ；

所述陀螺仪零偏估计模块，以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述imu单元，用于修正imu单元中陀螺仪量测的角速率。

本发明方案至少可以实现如下之一有益效果：

以基于imu数据和里程计数据进行航位推算得到机器人位置与基于平面激光雷达计算机器人位置之间的定位误差为量测量，采用卡尔曼滤波实时估计出准确的陀螺仪零偏；并将实时估计出的陀螺仪零偏反馈到imu进行扣除，解决了航位推算过程中由于陀螺仪零偏持续积分引起的姿态、位置误差，提高了航位推算的姿态、位置精度。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例一提供的一种陀螺仪漂移估计方法的流程图；

图2为本发明实施例二提供的一种陀螺仪漂移估计装置的结构示意图；

图3为本发明实施例三提供的一种定位系统的结构示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

实施例一、

本发明实施例公开了一种陀螺仪漂移估计方法，该方法基于平面激光雷达slam定位估计惯性测量单元的陀螺仪漂移，用于陀螺仪的角速率修正。该方法能够应用于轮式机器人，例如轮式工业机器人、轮式家用机器人等，下面的实施例将以家用扫地机器人为例进行说明。

具体方法如图1所示，包括以下步骤：

步骤s1、基于imu(inertialmeasurementunit，惯性测量单元)数据和里程计数据进行航位推算，得到机器人的第一位置

步骤s2、基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；

步骤s3、以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏

imu包含了三个单轴的加速度计和三个单轴的陀螺仪。其中，加速度计，用于检测机器人在载体坐标系的三轴加速度信号；陀螺仪，用于检测机器人在载体坐标系的三轴角速率信号。

通过imu测量机器人在三维空间(载体坐标系)中的角速率和加速度，以此解算得到机器人的姿态矩阵

可选的，采用imu的六轴融合算法，将加速度计检测的三轴加速度信号和陀螺仪检测的三轴角速率信号进行融合得到机器人的姿态矩阵

具体的，本实施例中，根据姿态矩阵和里程计测量的增量数据(δl，δr)，进行航位推算(deadreckoning,dr)得到机器人的第一位置

利用平面激光雷达采用slam(simultaneouslocalizationandmapping,即时定位与地图构建)算法，计算的机器人第二位置

slam算法可以根据平面激光雷达的探测信息，构建机器人周围环境的地图并定位机器人在地图中的位置，该位置即为上述机器人第二位置

slam算法可采用hectorslam、cartographer、gmapping等中的任一算法；

优选的，本实施例采用在长廊及低特征场景中建图效果较好的gmapping算法；gmapping算法可以实时构建室内地图，在构建小场景地图所需的计算量较小且定位精度较高；相比hectorslam算法对激光雷达频率要求低、鲁棒性高；而相比cartographer算法在构建小场景地图时，不需要太多的粒子并且没有回环检测，因此计算量小于

cartographer而精度相当。

根据机器人的第一位置和第二位置计算定位误差δpⁿ，即

基于imu数据和里程计数据进行航位推算，得到机器人的第一位置

通过基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏能够使得基于imu数据和里程计数据进行航位推算得到的位置更为精确。

由于在本实施例的航位推算(dr)中，不考虑里程计的速度误差以及刻度系数误差，航位推算(dr)的姿态、位置误差主要由于陀螺仪的微小零偏持续积分而导致；因此，在对航位推算结果进行姿态、位置误差校正时，可仅考虑因陀螺仪零偏造成的误差。

优选的，在所述以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:

构建imu模型和航位推算的系统误差方程；

根据imu模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。

构建imu模型可以是对imu中的三个单轴的陀螺仪采用白噪声和一阶马尔科夫过程进行建模，得到陀螺仪零偏模型。例如，wε为陀螺仪的角速率白噪声，wrgi是陀螺仪的三个轴的一阶马尔科夫过程激励白噪声，为陀螺仪的一阶马尔科夫过程随机误差。对陀螺仪零偏进行求导，得到微分方程：

该微分过程即为一阶马尔科夫过程。其中，x,y,z为陀螺仪的三个坐标轴，τgi是陀螺仪的三个轴的相关时间常数。

优选的，建立的航位推算系统误差方程，如下：

其中，公式中的为机器人的姿态矩阵，为imu的姿态失准角；为速度误差、δpⁿ为定位误差，为陀螺零偏漂移，wε为陀螺角速率白噪声。

根据imu模型和航位推算系统误差方程构建卡尔曼滤波模型：

其中，为卡尔曼滤波模型的状态空间模型；z＝hx+v为卡尔曼滤波模型的量测模型。

式(3)中，为卡尔曼滤波的向量；f为卡尔曼滤波的状态转移矩阵；g为卡尔曼滤波的噪声矩阵；w为卡尔曼滤波的状态噪声向量；z为卡尔曼滤波的量测向量；h为卡尔曼滤波的量测矩阵；v为卡尔曼滤波的量测噪声向量。

卡尔曼滤波的状态向量中的，姿态失准角是导致速度误差和位置误差的向量，需要在状态向量中表达；定位误差δpⁿ是量测量；速度误差是中间变量，用于在位置误差和陀螺仪零偏之间建立关系；陀螺零偏漂移是状态向量中的待估计量。

通过将低精度的惯性器件的时间相关误差模型为一阶马尔科夫过程模型，能够在长时间组合滤波后避免滤波器过度收敛现象导致的滤波器抗干扰性能变差，且能够降低陀螺仪建模维数和滤波计算量。

且若惯性器件误差中存在较大随机常值成分，可通过滤波器的惯性器件误差反馈校正，消除随机常值误差的影响；

更优选的，卡尔曼滤波状态空间模型中，

卡尔曼滤波的状态转移矩阵

卡尔曼滤波的噪声矩阵

卡尔曼滤波的状态噪声向量

卡尔曼滤波的量测矩阵

其中，hf为卡尔曼滤波的比力量测矩阵；hd为卡尔曼滤波的位置量测矩阵；wε为陀螺角速率白噪声，wrg是一阶马尔科夫过程激励白噪声。

本实施例使用比力误差、位置误差双观测量构成卡尔曼滤波的量测矩阵，其中，增加比力误差观测量可以阻尼水平姿态(俯仰、横滚)漂移，让位置误差与陀螺仪漂移误差关系简化，让位置误差观测可以更精确的估计陀螺仪漂移。

具体的，hf＝[fⁿ×03×303×303×3]，

hd＝[03×303×3i3×303×3]；

并且，卡尔曼滤波的量测量为比力误差δfⁿ＝fⁿ+gⁿ；

其中，fⁿ为比力在导航坐标系下的投影，gⁿ为重力在导航坐标系下的投影。

卡尔曼滤波的状态转移矩阵f中的βg为陀螺仪的误差模型；

具体的，βg＝diag(1/τx1/τy1/τz)；τx、τy、τz分别为陀螺仪在x、y、z轴上的相关时间参数。

根据卡尔曼滤波模型的状态空间模型和量测模型，使用卡尔曼滤波算法估计卡尔曼滤波的状态向量中的陀螺仪零偏待其中的陀螺仪零偏估值稳定后，输出陀螺仪零偏

优选的，当相邻两次的陀螺仪零偏估值的差值小于0.001时，即当时，判定陀螺仪零偏估值稳定，输出陀螺仪零偏否则，不输出。其中，k为卡尔曼滤波的时刻(次数)，k为自然数。这样避免了陀螺仪零偏或者存在估值不准导致的陀螺仪零偏结果不可用的情况。

imu模型中的陀螺仪角速率为根据以下公式进行陀螺仪量测角速率修正；

使用修正后的陀螺仪量测角速率参与后续的六轴融合算法。

通过将输出的上述陀螺仪零偏反馈至imu模型，用于修正陀螺仪量测角速率，以提高陀螺仪检测角速率的准确性。

由于，在扫地机器人运动过程中，当扫地机器人打滑时，航位推算的位置也会不准，以此位置计算定位误差δpⁿ，会使卡尔曼滤波后得到的陀螺仪零偏误差增大，从而造成陀螺仪量测角速率修偏的问题。

需要注意的是，对于激光雷达slam来说，也可能存在因为环境特征缺乏导致的定位不准确，同样，以此位置计算定位误差δpⁿ，也将造成陀螺仪量测角速率修偏的问题。

由于上述两种状况都会导致估计的陀螺仪零偏不可用，因此，需要检测出该两种状况并在该两种状况发生时断开陀螺仪零偏反馈。

优选的，本实施例通过设置状态监测进行陀螺仪状态检测，并在检测到相应状况时，停止陀螺仪零偏反馈；

具体的，陀螺仪状态监测步骤如下：

1)获取卡尔曼滤波产生的测量新息

卡尔曼滤波的测量新息

式中的zk为k时刻的卡尔曼滤波量测值，为k时刻的先验量测值；

2)对所述测量新息进行归一化得到测量新息的幅值

测量新息的幅值利用下式计算得到：

式中，为测量新息的协方差，即为测量噪声协方差rk以及转换到测量空间的状态估计的误差协方差之和，即

下标k为卡尔曼滤波的时刻(次数)。

3)比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。

优选的，可对测量新息的幅值进行滤波，判断新的测量是否和以前的测量一致，以检验陀螺仪零偏估计是否异常。优选地，卡尔曼状态异常阈值可优选为滤波值的99.73％。

在一个理想的卡尔曼(kalman)滤波器中，测量新息的幅值为零均值单位方差的高斯分布，并且一旦估计的状态已经收敛到其对应的真值，这些新息后续的连续输出几乎独立。

考虑到在测量噪声变大或是测量存在时间间断时，状态不确定性变大都会导致测量新息变大，导致测量新息的幅值有波动；同时，考虑到当有测量故障时，比如里程计打滑导致位置误差引入了非算法因素，将会导致测量新息的幅值变化更大。通过对测量新息的幅值滤波可以滤掉短期错误数据，如源于激光slam匹配的误定位、测量毛刺噪声等错误数据，以提高状态监测的准确性。

实施例二、

本实施例公开了一种陀螺仪漂移估计装置，该装置能够应用于轮式机器人，例如轮式工业机器人、轮式家用机器人等，下面的实施例将以家用扫地机器人为例进行说明。

如图2所示，包括：

航位推算模块，基于imu数据和里程计数据进行航位推算，得到机器人的第一位置

定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；

陀螺仪零偏估计模块，以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏

优选的，本实施例中采用与实施例一中相同的方法，构建imu模型和航位推算的系统误差方程；并根据imu模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。

对imu中的三个单轴的陀螺仪采用白噪声和一阶马尔科夫过程进行建模，得到的陀螺仪零偏为一阶马尔科夫过程随机误差。

构建卡尔曼滤波的状态空间模型为量测模型为z＝hx+v。

其中，为卡尔曼滤波的状态空间模型；f为卡尔曼滤波的状态转移矩阵；g为卡尔曼滤波的噪声矩阵；w为卡尔曼滤波的状态噪声向量；z为卡尔曼滤波的量测模型；h为卡尔曼滤波的量测矩阵；v为卡尔曼滤波的量测噪声向量。

具体的模型中的各参数矩阵的取值与实施例一中的相同。

根据建立的卡尔曼滤波的状态空间模型和量测模型，使用卡尔曼滤波算法估计卡尔曼滤波的状态向量中的

当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏

优选的，所述陀螺仪零偏反馈到imu模型，用于修正陀螺仪量测的角速率。

优选的，本实施例的定位装置还包括状态监测模块；用于对扫地机器人运动进行监测，在扫地机器人运动过程中，当扫地机器人打滑时，航位推算算法的位置也会不准，或者对于激光雷达slam，存在因为环境特征缺乏导致的定位不准确时，防止估计位置误差使陀螺仪零偏修偏问题；

状态监测模块与所述陀螺仪零偏估计模块连接，根据获得卡尔曼滤波过程中产生的测量新息，判断卡尔曼滤波得到的陀螺仪零偏是否异常，如果异常，则停止对陀螺仪量测角速率进行修正。

具体的，状态监测模块，获得卡尔曼滤波过程中产生的测量新息并对测量信息进行归一化得到测量新息的幅值将测量新息的幅值与设置的状态异常阈值进行比较；当测量新息的幅值大于陀螺仪状态异常阈值时，判断陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。

实施例三、

本实施例还公开了一种定位系统，如图3所示，包括imu单元、里程计、平面激光雷达和如上所述的陀螺仪漂移估计装置；

所述imu单元、里程计分别与陀螺仪漂移估计装置的航位推算模块连接，分别输出imu数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置

所述平面激光雷达，用于采用slam算法计算机器人的第二位置并输出至所述陀螺仪漂移估计装置的定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpⁿ；

所述陀螺仪零偏估计模块，以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述imu单元，用于修正imu单元中陀螺仪量测的角速率，提高航位推算中得到的姿态、位置的精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。