[发明专利]一种陀螺仪漂移估计方法、装置及定位系统

说明书

本发明涉及一种陀螺仪漂移估计方法、装置及定位系统，属于导航技术领域，解决陀螺仪零偏引起的定位误差；方法包括：基于IMU数据和里程计数据进行航位推算，得到机器人的第一位置基于平面激光雷达计算机器人的第二位置根据机器人的第一位置和第二位置计算定位误差δpⁿ；以定位误差δpⁿ为量测量，采用卡尔曼滤波实时估计陀螺仪零偏本发明通过实时估计陀螺仪的零偏并反馈扣除，提高了航位推算算法得到的姿态、位置的精度。

技术领域

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

背景技术

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

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

其中，为卡尔曼滤波的状态向量；F为卡尔曼滤波的状态转移矩阵；G为卡尔曼滤波的噪声矩阵；W为卡尔曼滤波的状态噪声向量；Z为卡尔曼滤波的量测向量；H为卡尔曼滤波的量测矩阵；V为卡尔曼滤波的量测噪声向量。

进一步地，

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

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

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

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