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

说明书

本发明公开了一种基于多传感器的组合导航方法及系统，该方法包括：采集IMU以及轮式编码器的信息进行初始化，根据里程计的内参，得到轮式编码器的线速度和角速度并进行与IMU数据的整合；接着通过深度相机，利用视觉SLAM算法，即得到机器人在任意位置的位姿，共六个自由度的信息，获取图像帧并提取ORB描述子，将整合后的得到的IMU数据与图像的位姿信息进行初始化，采用整合后的IMU的位姿信息作为先验值，并采用视觉SLAM得到的姿态信息作为观测值并与图像帧进行时间同步；接着，使用卡尔曼滤波器进行优化处理；将采集的激光雷达数据进行数据转换并与初步滤波后得到的位姿数据利用扩展卡尔曼滤波器进行滤波优化，并输出最终的最优导航信息。最后根据组合导航方法提供了一种用于移动机器人的导航系统。本发明提供的基于多传感器的组合导航方法及系统，将各传感器进行优势互补，符合智能机器人的导航要求，可保持稳定可靠的导航，提高了导航精度和抗干扰能力以及导航系统的应用范围。

技术领域

本发明涉及多传感器组合导航技术领域，尤其涉及一种基于多传感器的组合导航方法及系统。

背景技术

近些年随着经济的快速发展，互联网、人工智能以及云计算等技术也在蓬勃发展，机器人行业的发展要求越来越高，而且机器人的发展需要依靠高精度的导航定位，对于单一传感器已经不能满足高精度导航的需要，多传感器组合导航技术研究就成了现阶段的一个研究热点。

对于单线激光雷达，由于激光点不够密集，构建的地图常常会出现偏移，由此造成定位不准确而出现偏差的情况，这是由累积误差所导致的；同时在激光SLAM中，闭环检测一直是一大难点：由于获取的激光数据是二维点云数据，无明显特征且相互之间十分相似，所以基于激光数据的闭环检测往往效果不好。

虽然视觉传感器在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作，此外，视觉相机的工作频率具有一定的限制，在高速行驶过程中，视觉更新存在更新延迟现象。

因此，如何将多传感器进行组合从而实现精度更高、鲁棒性更好的定位是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种基于多传感器的组合导航方法及系统，针对室内环境下机器人导航定位，主要基于机器人导航现状，综合考虑IMU、编码器、深度相机、激光雷达的优势，进行优势互补，实现在室内环境下高精度的导航定位。

为了实现上述目的，本发明采用如下技术方案。

一种基于多传感器的组合导航方法，包括如下具体步骤。

采集IMU以及轮式编码器的数据进行初始化，根据里程计的内参，得到轮式编码器的线速度和角速度。

。

机器人在平面上运动时，两个连续的里程计数据之间，在角度上只有绕z轴方向有变化（偏航角），其他角度变化为0，在里程计坐标系下和之间，旋转的变化为： ，由里程计的线速度和角速度进行积分分别得到角度和位置：

。

IMU得到的数据的角度数据为相对数据，使用该角度替代掉轮式编码器计算得到的角度数据。

通过深度相机，利用视觉SLAM算法，即得到机器人在任意位置的位姿，共六个自由度的信息，获取图像帧并提取ORB描述子，将整合后得到的IMU数据与图像的位姿信息进行处理，并与图像帧进行时间同步；接着，使用卡尔曼滤波器进行优化处理。

求解状态转移矩阵系统噪声协方差矩阵，得到卡尔曼滤波的预测过程：

。

用视觉姿态估计值表示相机坐标系到世界坐标系的坐标转换，同时表示滤波器的观测值z,并结合状态方程中的观测矩阵得到观测方程H的表达式；

。