基于多传感器移动机器人的建图方法与流程

本发明涉及建图领域，具体涉及一种基于多传感器移动机器人的建图方法。

背景技术：

现阶段机器人的移动大多依靠人工路径规划，机器人的自主导航能力依赖于即时定位与建图技术，其核心任务是当机器人进入未知工作环境时，利用传感器信息，对周围环境进行高效率且准确地构建，同时得到设备在空间中的位置与姿态，除了可以应用在机器人领域外，为了实现智能移动设备的自主导航及定位，需要实现对于智能移动设备所处环境的环境建图。

大多数现有产品通常通过两种方式实现环境建图，第一种为直接利用二维测距传感器进行环境建图，但是这无法得到环境的三维信息；第二种为用三维激光雷达或者图像的方法进行环境建图，但是这种方法一般都是先提取三维(或者图像)特征，进行特征匹配，再估算变换矩阵，运算量巨大，无法保证建图的实时性。综上所述，现有技术中提供的环境三维建图的技术方案存在运算量大，无法保证建图实时性的问题。随着计算机技术和人工智能的发展，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。

因此，有必要设计一种基于多传感器移动机器人的建图方法，以解决上述问题。

技术实现要素：

本发明的目的在于提供一种基于多传感器移动机器人的建图方法，利用多传感器优势，并且将创建地图和优化地图分开进行，降低了建图的运算量，同时提高了建图的实时性，并且多传感器进一步提高了建图的准确性，提高了建图质量。

为实现上述目的，本发明提供了一种基于多传感器移动机器人的建图方法，其包括以下步骤：

步骤s1：为机器人安装多种不同的传感器，所述传感器包括惯性测量单元、编码器、双目相机、激光雷达及超声波，每个传感器均与机器人的处理芯片连接；

步骤s2：获取双目相机、惯性测量单元、激光雷达、超声波及编码器的测量数据；

步骤s3：将同一时刻的惯性测量单元数据和编码器数据进行扩展卡尔曼滤波处理，通过运算获得机器人姿态信息和行走路线；

步骤s4：操作机器人在未知环境下移动，将双目相机数据、激光雷达数据、超声波数据转换成由角度和距离构成的数值对格式，并分别构建环境的二维点云图，以得到三张点云图；

步骤s5：将三张点云图融合成一张点云图，并对不同传感器获取的点云信息进行标注；

步骤s6:将同一位置的三种不同点云信息的差值与预设阈值进行对比，去除失真的点云信息和冗余的点云信息，以完成点云图的优化；

步骤s7：将步骤s6中优化后的点云图转化为栅格地图，完成地图的创建。

作为本发明进一步改进的技术方案，所述双目相机、激光雷达及超声波的扫描频率一致。

作为本发明进一步改进的技术方案，步骤s3具体包括以下步骤：

步骤s31：获取同一时刻下的惯性测量单元数据和编码器数据；

步骤s32：以惯性测量单元数据格式为标准，通过运动学解耦和航向推算将编码器数据格式转化为惯性测量单元数据格式；

步骤s33：构建扩展卡尔曼滤波器，获取编码器数据作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；获取惯性测量单元数据作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；发布更新后的状态量及协方差矩阵作为融合后的数据，获取机器人的姿态信息和行走路线。

作为本发明进一步改进的技术方案，步骤s4中对双目相机数据进行格式转化的具体步骤包括：

步骤s41：根据三角形相似原理，计算出障碍物和机器人的距离信息；

步骤s42：利用步骤s3中获取的机器人姿态信息，提取出偏航角；

步骤s43：将同一时刻的距离信息和偏航角组成数值对，完成对双目相机数据格式的转化。

作为本发明进一步改进的技术方案，步骤s5中通过不同颜色对不同传感器获取的点云信息进行标注。

作为本发明进一步改进的技术方案，步骤s6具体为:在融合后的点云图上，将同一位置的三种不同点云信息差值与预设阈值进行对比，若大于预设阈值，则挑选出最大优先级类型的点云信息作为最终的点云信息；若等于预设阈值，则挑选出任意传感器生成的点云信息作为最终的点云信息；若小于预设阈值，则采用卡尔曼滤波融合三种不同点云信息。

作为本发明进一步改进的技术方案，所述激光雷达为2d激光雷达。

作为本发明进一步改进的技术方案，所述机器人的底盘为差分轮结构，并包括底盘及设于底盘上的左轮和右轮。

本发明的基于多传感器移动机器人的建图方法利用惯性测量单元和编码器数据融合，有效避免了机器人建图过程中由于打滑、震动带来的编码器累计误差，进一步提高了建图的准确性；利用激光雷达、双目相机、超声波分别创建点云图，丰富了传感器的环境感知信息。另外，本发明将创建地图和优化地图分开进行，降低了建图的运算量，提高了建图的实时性；同时利用多传感器进一步提高了建图的准确性和建图质量。

附图说明

图1是本发明基于多传感器移动机器人的建图方法的流程图。

图2是本发明中机器人的底盘的移动示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图和具体实施例对本发明进行详细描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

请参图1所示的基于多传感器移动机器人的建图方法，其包括以下步骤：

步骤s1：为机器人安装多种不同的传感器，传感器包括惯性测量单元(imu)、编码器、双目相机、激光雷达及超声波，每个传感器均与机器人的处理芯片连接；

步骤s2：同时获取双目相机、惯性测量单元、激光雷达、超声波、编码器的测量数据；

步骤s3：将同一时刻的惯性测量单元数据和编码器数据进行扩展卡尔曼滤波处理，通过运算获得机器人姿态信息和行走路线；步骤s3具体包括以下步骤：

步骤s31：获取同一时刻下的惯性测量单元数据和编码器数据，惯性测量单元数据格式为[pitchrollyaw]，编码器数据格式为[vl，vr]；

步骤s32：以惯性测量单元数据格式为标准，通过运动学解耦和航向推算将编码器数据格式转化为惯性测量单元数据格式；

具体的，如图2所示，机器人的底盘为差分轮结构，并包括底盘及设于底盘上的左轮和右轮。由左轮角速度wl和右轮角速度wr相等可得：

其中v,w为底盘中心的线速度和角速度，vl，vr为左右两轮的速度，d为轮子距底盘中心的距离，r为底盘的中心圆弧运动的半径。

将(2)代入角速度公式计算出底盘的角速度：

将(3)代入线速度公式计算出底盘的线速度：

将(3)，(4)代入航迹推算公式(5)，将编码器数据格式[vl，vr]转化为[x,y,θ]：

步骤33：构建扩展卡尔曼滤波器，获取编码器数据作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；获取惯性测量单元数据作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；发布更新后的状态量及协方差矩阵作为融合后的数据，获取机器人的姿态信息和行走路线；

步骤s4：操作机器人在未知环境下移动，将双目相机数据、激光雷达数据及超声波数据转换成角度和距离构成的数值对格式，分别构建环境的二维点云图，得到三张点云图；

其中，双目相机数据格式转化的具体步骤为：

步骤s41：根据三角形相似原理，计算出障碍物和机器人的距离信息；

步骤s42：利用步骤s3中获取的机器人姿态信息，提取出偏航角；

步骤s43：将同一时刻的距离信息和偏航角组成数值对，完成对双目相机数据格式的转化；

步骤s5：将不同传感器构建的点云图融合成一张点云图，将不同的传感器获取的点云信息进行标注；优选的，通过不同颜色对不同传感器获取的点云信息进行标注，当然，也可以以其它的方式进行标识，只要能够对三种不同的点云信息进行区分即可，在此不作限制；

步骤s6：同一位置的三种不同点云信息差值与预设阈值进行对比，若大于阈值，则挑选出最大优先级类型的点云信息作为最终的点云信息；若等于阈值，则挑选出任意传感器生成的点云信息作为最终的点云信息；若小于阈值，则采用卡尔曼滤波融合三种不同点云信息；

步骤s7：将步骤s6中优化后的点云图转化为栅格地图，完成地图创建并保存地图。

本发明中激光雷达、双目相机及超声波的扫描频率一致。优选的，双目相机为kinect双目相机，imu为mpu6050，激光雷达为2d激光雷达，优选为rplidar。

综上所述，本发明的基于多传感器移动机器人的建图方法利用惯性测量单元和编码器数据融合，有效避免了机器人建图过程中由于打滑、震动带来的编码器累计误差，进一步提高了建图的准确性；利用激光雷达、双目相机、超声波分别创建点云图，丰富了传感器的环境感知信息。另外，本发明将创建地图和优化地图分开进行，降低了建图的运算量，提高了建图的实时性；同时利用多传感器进一步提高了建图的准确性和建图质量。

在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。