一种陀螺仪漂移估计方法、装置及定位系统制造方法及图纸

本发明专利技术涉及一种陀螺仪漂移估计方法、装置及定位系统，属于导航技术领域，解决陀螺仪零偏引起的定位误差；方法包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置

A Gyro Drift Estimation Method, Device and Positioning System

【技术实现步骤摘要】
一种陀螺仪漂移估计方法、装置及定位系统
本专利技术涉及导航
，尤其涉及一种陀螺仪漂移估计方法、装置及定位系统。
技术介绍
轮式机器人导航的核心包括定位和建图。定位一般采用两级结构：S1:使用IMU(Inertialmeasurementunit，惯性测量单元)数据和里程计数据进行航位推算，得到开环积分位置、姿态；S2:激光雷达基于SLAM(Simultaneouslocalizationandmapping,即时定位与地图构建)算法、使用上一步的结果为初值，进行扫描匹配得出机器人最终的位置、姿态。其中，航位推算的特点是具有无界累计误差，但是不受环境特征影响，可以给出持续的位姿。激光雷达SLAM也可以不使用IMU和里程计计算的位姿初值，仅靠激光雷达就可以计算出位姿。激光雷达SLAM算法的位姿经过回环和后端优化之后可以最大程度的消除累计误差。但是激光雷达SLAM在空旷场景和简单重复场景等无特征环境下会失效。航位推算和纯激光雷达SLAM算法在累计误差特性、对于环境特征依赖这两方面具有互补特性。由于IMU加里程计定位是开环积分，陀螺仪的微小零偏持续积分就会导致较大的姿态、位置误差，由于没有闭环反馈修正，导致姿态、位置误差是无界的增长。
技术实现思路
鉴于上述的分析，本专利技术旨在提供一种陀螺仪漂移估计方法、装置及定位系统，采用机器人的定位误差作为量测量，估计出IMU的陀螺仪零偏并反馈，提高航位推算得到的姿态、位置的精度。本专利技术的目的主要是通过以下技术方案实现的：一方面，公开了一种陀螺仪漂移估计方法，包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏进一步地，在所述以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:构建IMU模型和航位推算的系统误差方程；根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。进一步地，所述陀螺仪零偏为一阶马尔科夫过程随机误差。进一步地，所述卡尔曼滤波的状态空间模型为所述卡尔曼滤波的量测模型为Z＝HX+V；其中，为卡尔曼滤波的状态向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。进一步地，所述卡尔曼滤波的状态转移矩阵所述卡尔曼滤波的噪声矩阵所述卡尔曼滤波的状态噪声向量所述卡尔曼滤波的量测矩阵其中，Hf为卡尔曼滤波的比力量测矩阵；Hd为卡尔曼滤波的位置量测矩阵。进一步地，所述采用卡尔曼滤波实时估计陀螺仪零偏包括：当相邻两次的陀螺仪零偏估值的差值小于0.001时，陀螺仪零偏估值稳定，输出陀螺仪零偏。进一步地，所述陀螺仪零偏用于修正陀螺仪量测的角速率。进一步地，还包括陀螺仪状态监测，所述陀螺仪状态监测包括：获取卡尔曼滤波产生的测量新息；对所述测量新息进行归一化得到测量新息的幅值；比较测量新息的幅值与预定的陀螺仪状态异常阈值，当测量新息的幅值大于陀螺仪状态异常阈值时，陀螺仪零偏估计异常，否则，陀螺仪零偏估计正常。另一方面，还公开了一种陀螺仪漂移估计装置，包括，航位推算模块，基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置定位误差模块，基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏另一方面，还公开了一种定位系统，包括IMU单元、里程计、平面激光雷达和如上所述的陀螺仪漂移估计装置；所述IMU单元、里程计分别与所述航位推算模块连接，分别输出IMU数据和里程计数据到所述航位推算模块，用于计算机器人的第一位置所述平面激光雷达，用于采用SLAM算法计算机器人的第二位置并输出至所述定位误差模块，根据机器人的第一位置和第二位置计算定位误差δpn；所述陀螺仪零偏估计模块，以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏输出到所述IMU单元，用于修正IMU单元中陀螺仪量测的角速率。本专利技术方案至少可以实现如下之一有益效果：以基于IMU数据和里程计数据进行航位推算得到机器人位置与基于平面激光雷达计算机器人位置之间的定位误差为量测量，采用卡尔曼滤波实时估计出准确的陀螺仪零偏；并将实时估计出的陀螺仪零偏反馈到IMU进行扣除，解决了航位推算过程中由于陀螺仪零偏持续积分引起的姿态、位置误差，提高了航位推算的姿态、位置精度。附图说明附图仅用于示出具体实施例的目的，而并不认为是对本专利技术的限制，在整个附图中，相同的参考符号表示相同的部件。图1为本专利技术实施例一提供的一种陀螺仪漂移估计方法的流程图；图2为本专利技术实施例二提供的一种陀螺仪漂移估计装置的结构示意图；图3为本专利技术实施例三提供的一种定位系统的结构示意图。具体实施方式下面结合附图来具体描述本专利技术的优选实施例，其中，附图构成本申请一部分，并与本专利技术的实施例一起用于阐释本专利技术的原理。实施例一、本专利技术实施例公开了一种陀螺仪漂移估计方法，该方法基于平面激光雷达SLAM定位估计惯性测量单元的陀螺仪漂移，用于陀螺仪的角速率修正。该方法能够应用于轮式机器人，例如轮式工业机器人、轮式家用机器人等，下面的实施例将以家用扫地机器人为例进行说明。具体方法如图1所示，包括以下步骤：步骤S1、基于IMU(Inertialmeasurementunit，惯性测量单元)数据和里程计数据进行航位推算，得到机器人的第一位置步骤S2、基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；步骤S3、以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏IMU包含了三个单轴的加速度计和三个单轴的陀螺仪。其中，加速度计，用于检测机器人在载体坐标系的三轴加速度信号；陀螺仪，用于检测机器人在载体坐标系的三轴角速率信号。通过IMU测量机器人在三维空间(载体坐标系)中的角速率和加速度，以此解算得到机器人的姿态矩阵可选的，采用IMU的六轴融合算法，将加速度计检测的三轴加速度信号和陀螺仪检测的三轴角速率信号进行融合得到机器人的姿态矩阵具体的，本实施例中，根据姿态矩阵和里程计测量的增量数据(ΔL，ΔR)，进行航位推算(DeadReckoning,DR)得到机器人的第一位置利用平面激光雷达采用SLAM(Simultaneouslocalizationandmapping,即时定位与地图构建)算法，计算的机器人第二位置SLAM算法可以根据平面激光雷达的探测信息，构建机器人周围环境的地图并定位机器人在地图中的位置，该位置即为上述机器人第二位置SLAM算法可采用HectorSLAM、Cartographer、Gmapping等中的任一算法；优选的，本实施例采用在长廊及低特征场景中建图效果较好的Gmapping算法；Gmapping算法可以实时构建室内地图，在构建小场景地图所需的计算量较小且定位精度较高；相比HectorSLAM算法对激光雷达频率要求低、鲁棒性高；而相比Cartographer算法在构建小场景地图时，不需要太多的粒子并且没本文档来自技高网...

【技术保护点】
1.一种陀螺仪漂移估计方法，其特征在于，包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置

【技术特征摘要】
1.一种陀螺仪漂移估计方法，其特征在于，包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpn；以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏2.根据权利要求1所述的陀螺仪漂移估计方法，其特征在于，在所述以定位误差δpn为量测量，采用卡尔曼滤波实时估计陀螺仪零偏之前，还包括:构建IMU模型和航位推算的系统误差方程；根据IMU模型和航位推算系统误差方程构建卡尔曼滤波的状态空间模型和量测模型。3.根据权利要求1或2所述的陀螺仪漂移估计方法，其特征在于，所述陀螺仪零偏为一阶马尔科夫过程随机误差。4.根据权利要求2所述的陀螺仪漂移估计方法，其特征在于，所述卡尔曼滤波的状态空间模型为所述卡尔曼滤波的量测模型为Z＝HX+V；其中，为卡尔曼滤波的状态向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。5.根据权利要求4所述的陀螺仪漂移估计方法，其特征在于，所述卡尔曼滤波的状态转移矩阵所述卡尔曼滤波的噪声矩阵所述卡尔曼滤波的状态噪声向量所述卡尔曼滤波的量测矩阵其中，Hf为卡尔曼滤波的比力量测矩阵；Hd为卡尔曼滤波的位置量测矩阵；wε为陀螺角速率白噪声，wrG是一阶马尔科夫过程激励白噪声。6.根据权利要求1-5任一项所述的陀螺仪漂移估计方法，其特征在于，所述采用卡尔曼滤波实时估计陀螺仪零...

【专利技术属性】
技术研发人员：檀冲，刘兴华，
申请(专利权)人：小狗电器互联网科技北京股份有限公司，
类型：发明
国别省市：北京,11