[发明专利]一种基于多传感器的组合导航方法及系统

权利要求书

1.一种基于多传感器的组合导航方法，其特征在于，包括如下具体步骤：

步骤1：初始化IMU与轮式编码器数据，根据里程计的内参获得轮式编码器的线速度和角速度并积分得到位置和角度信息；其中，使用IMU计算的角度信息替代由轮式编码器得到的角度信息；

步骤2：通过视觉SLAM算法获取图像帧提取ORB描述子，将整合后得到的IMU数据与图像的位姿信息进行初始化，并与图像帧进行时间同步，接着用卡尔曼滤波对数据进行优化处理；其中，卡尔曼滤波的预测过程为；

再由观测矩阵和观测噪声方差矩阵得到卡尔曼滤波更新过程为：

步骤3：采集激光雷达数据，将原始数据格式转化为相应直角坐标系的数据，进一步使用扩展卡尔曼滤波将优化后的位姿与转换后的激光雷达数据进行最后的滤波处理；具体步骤为；

首先，由先验概率密度函数

目标状态预测均值为：

目标状态预测方差为：

雅可比矩阵为：

接下来，由系数归一化 得到：

目标状态观测均值：

目标状态观测方差：

卡尔曼增益：

雅可比矩阵为：

最后，由后验概率密度得到：

目标状态后验均值：

目标状态后验方差： 。

2.根据权利要求1所述的基于多传感器的组合导航方法，其特征在于，根据轮式编码器的内参得到线速度和角速度：

。

3.根据权利要求1所述的基于多传感器的组合导航方法，其特征在于，由里程计的线速度和角速度分别进行积分得到角度和位置信息：

。

4.根据权利要求1所述的基于多传感器的组合导航方法，其特征在于，将IMU得到的角度数据替换掉轮式编码器的角度信息。

5.基于多传感器的组合导航系统，用于执行权利要求1任一项所述的基于多传感器的组合导航方法，其特征在于，

包括激光雷达（1）、深度相机（2）、IMU（3）、轮式编码器（4），所述激光雷达（1）、深度相机（2）、IMU（3）、轮式编码器（4）的数据输出端与数据处理模块相连，数据转换与融合模块获取多源传感器数据滤波处理后将融合位姿发送到SLAM建图模块，进一步，SLAM模块将创建的栅格地图上传到Rviz交互界面并下发给定位与导航模块，更进一步，将接受到的导航信息由电机控制模块控制机器人底盘移动。

6.根据权利要求5所述的基于多传感器的组合导航系统，其特征在于，所述IMU(3)为六轴IMU，包括陀螺仪和加速度计，所述加速度计和陀螺仪的输出端均与数据处理模块输入端相连。

7.根据权利要求5所述的基于多传感器的组合导航系统，其特征在于，所述数据转换与融合模块包括卡尔曼滤波器和扩展卡尔曼滤波器。

8.根据权利要求5所述的基于多传感器的组合导航系统，其特征在于，所述输出最后的定位导航信息包括机器人位姿信息和速度信息。