[发明专利]一种多传感器融合位姿估计方法

说明书

本发明提供了一种多传感器融合位姿估计方法，本发明通过基于动态贝叶斯网络和断裂力学，本发明建立载体坐标系与导航坐标系，对IMU设备输出建模，确定IMU运动学模型；输入激光雷达与IMU各自的自由度信息，基于扩展卡尔曼滤波进行融合姿态估计；不断迭代更新实现滤波。本发明利用激光雷达获得的姿态信息作为观测值校正了IMU输出的值使其与真实值更为接近，在误差统计上相比于其他方法精度上有了一定的提高，做到了修正IMU本身存在的累计误差同时解决旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升。

技术领域

本发明涉及机器人姿态估计技术领域，特别是涉及一种多传感器融合位姿估计方法。

背景技术

现有的姿态估计方法多是通过单独使用激光雷达或惯性传感器为主要手段对机器人位姿进行估计。

在机器人姿态估计方法中，单纯基于激光雷达或imu的方法在室内环境下进行位姿估计的实时性较差，输出频率低由于自身硬件设备的限制，易受到光照的影响，且在机器人在高速旋转等情况下容易出现跟踪失败的情况。而作为惯性传感器，具有姿态估计的输出频率高，高速旋转情况下输出精度高的优点，但由于其位姿信息的输出在长时间工作的情况下会导致误差积累过大。

针对上述问题，拟设计一种融合方式的姿态估计方法，以解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题。

发明内容

为了解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题，本发明提供一种多传感器融合位姿估计方法，具体方案如下：

一种多传感器融合位姿估计方法，包括以下步骤：

步骤1：建立载体坐标系与导航坐标系，对IMU设备输出建模，确定IMU运动学模型；

步骤2：输入激光雷达与IMU各自的自由度信息，基于扩展卡尔曼滤波进行融合姿态估计；

步骤3：不断迭代更新步骤2实现滤波。

进一步地，所述步骤1中建立坐标系具体为：

确定坐标系，常用坐标系分为：载体坐标系与导航坐标系，使用载体的质心表示载体坐标系的原点，坐标轴指向载体的正前方，正右方和正下方；导航坐标系指向东北天方向；载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转/最后绕偏航角转/实现到载体坐标系的变换，余弦矩阵通过下式表示：

进一步地，所述步骤1中对IMU设备输出建模具体为：

对IMU设备输出建模，IMU输出包括加速度计的线加速度以及陀螺仪的角速度，加速度计的实际输出值a_m通过下式表示：

a_m＝a-g+n_a+b_a

其中，a表示加速计在全局结构的准确值，g表示重力加速度；

陀螺仪输出的旋转速度ω_m为：

ω_m＝ω+n_g+b_g

n_a～N(0,N_a)

n_g～N(0,N_g)

其中，ω表示陀螺仪即IMU的旋转速度的理想输出；n_a、n_g分别为加速度计好陀螺仪服从正态分布且均值为0的高斯白噪声，