基于视觉建图的全向移动机器人导航方法

技术领域

本发明涉及一种基于视觉建图的全向移动机器人导航方法，属于计算机视觉和移动机器人路径规划技术领域。

背景技术

近年来，以计算机视觉、人工智能、SLAM等代表的高新技术迅速崛起，并且随着传感器与处理器等硬件性能的提升，机器人技术得到了快速的发展。目前工业机器人在全球市场中占比达80％，随着需求的变化和技术的发展，未来面向服务的移动机器人的比重将大为提升，如巡检机器人、扫地机器人、安防机器人等是目前比较主流的移动机器人的应用，移动机器人已经走进了我们的日常生活之中。但是随着产业结构的不断升级和人们需求的不断变化，人们对移动机器人的智能化和自主性提出了新的挑战。移动机器人视觉SLAM与导航是机器人控制与计算机视觉的研究方向之一，其中基于视觉建图的全向移动机器人导航系统，研究内容包括地图构建、路径规划、自主定位等。

目前，现有的移动机器人导航系统中多为使用激光雷达传感器来建立地图和导航，激光雷达价格昂贵，并不能广泛推广和使用，基于视觉传感器的建图和导航方法虽然价格低廉，但在精度上比激光传感器大为降低。此外，基于传统磁轨、二维码等有轨导航的移动机器人工作范围小，工作场景固定，浪费空间资源，并且工作场景变化需要重新铺设导轨或二维码，四轮等无轨导航的移动机器人在狭小空间环境中也难以工作。同时，用于移动机器人导航的传统路径规划算法如A*、Dijkstra算法等时间复杂度不够快，基于采样的RRT算法等路径规划并不最优，基于强化学习等算法的路径规划需要提前大量训练，且需要强大的硬件支持。针对以上三个方面的考虑，有必要设计一种新的全向移动机器人建图和导航方法，使移动机器人能够在工作环境中能够以更低廉的成本构造保证精度的地图，设计路径规划方法使得移动机器人在工作环境中更高效、更流畅的完成工作任务。

发明内容

为了克服现有技术中存在的不足，本发明目的是提供一种基于视觉建图的全向移动机器人导航方法。该发明方法采用全向麦克纳姆轮移动和视觉的方法对工作环境进行建图，同时利用深度学习的方法对工作环境进行识别，对全向移动机器人进行辅助定位，并利用所建地图和相应的路径规划算法进行导航，减轻了目前移动机器人导航成本昂贵、工作场景单一的问题，使机器人能够安全平稳的完成工作任务。

为了实现上述发明目的，解决已有技术中所存在的问题，本发明采取的技术方案是：一种基于视觉建图的全向移动机器人导航方法，其特征在于包括以下步骤：

步骤1、搭建全向移动机器人硬件平台，采用全向麦克纳姆轮并搭配电机进行驱动，全向移动机器人顶层前端安装深度相机作为视觉传感器进行图像获取，全向移动机器人底层安装工控机与下位机，同时搭配电源与显示装置；

步骤2、搭建全向移动机器人软件系统，包括以下子步骤：

(a)标定深度相机内参，准备棋盘格标定板，利用图像录制工具录制深度相机数据集并将图像采集频率降至4Hz，利用标定工具对深度相机进行内参标定，内参矩阵K通过式(1)进行描述，

式中，f

(b)以全向移动机器人第二层中心为原点，在全向移动机器人操作系统中建立世界坐标系，启动深度相机并订阅深度相机发布的节点，标定深度相机的外参，确定深度相机坐标系到世界坐标系的外参旋转矩阵R和外参平移矩阵T，通过式(2)进行描述，

式中，P

(c)根据麦克纳姆轮运动学原理，确定电机的控制方向，编写对应的方向与速度控制函数，控制全向移动机器人向前、后、左、右、右前、右后、左前、左后、正反自转十个方向运动，麦克纳姆轮的运动学分解通过式(3)进行描述，

式中，v

步骤3、基于YOLOv5对工作环境进行识别训练，具体包括以下子步骤：

(a)启动深度相机节点对工作环境进行图像获取，设置每0.1s提取一帧，并利用图像录制工具制作数据集，然后使用图片标注工具对数据集中的门、安全出口特征进行标注，得到对应的信息文件，包括标定名称、位置信息，同时将数据集分为训练集、测试集和验证集，占比分别为90％、5％、5％；

(b)对YOLOv5算法进行相关配置，修改训练名称为对应的特征标注名称，选取训练网络并将其中的锚点修改为标注的特征数量，对训练网络中的超参数进行修改，其中filter数量通过式(4)进行描述，

filter＝3*(classes+5) (4)

式中，classes为标注数量，filter为超参数中的网格参数；

(c)对YOLOv5进行训练与测试，选取官方的预训练权重，然后进行50000次训练将损失函数降至0.3以下，在验证集中对训练后的权重进行测试，保证各个检测目标的检测成功率在98％以上，否则继续进行权重优化，最后将训练后的权重与检测算法封装为ROS节点为下一步做准备；

步骤4、基于ORB_SLAM3构建视觉导航地图，驱动全向移动机器人在工作环境中进行移动，构造用于视觉导航地图，具体包括以下子步骤：

(a)依次启动全向移动机器人驱动节点、键盘控制节点、深度相机启动节点、ORB_SLAM3稀疏点云地图构建节点，借助键盘在整个工作区域内移动，使深度相机能够扫描到整个工作区域，ORB_SLAM3节点订阅深度相机发布的RGB图与深度图，选取关键帧建立稀疏点云地图并同时记录深度相机位置，将其保存到轨迹图文件中，扩大关键帧中选取特征点的数量，将稀疏点云地图转化为半稠密点云地图，同时利用点云库中的体素滤波和直通滤波对半稠密点云地图进行噪点去除；

(b)启动八叉树服务，创建八叉树对象，将其分辨率设置为0.05，读取ORB_SLAM3中所建轨迹图文件中的信息，包括关键帧的帧编号、位置和姿态四元数，将关键帧中的特征点转化并拼接成八叉树地图，并根据八叉树地图高度的不同加入色彩信息方便观察以及辨别结果是否准确，同时将八叉树地图高度设置为0.1-2.0米，滤除地面和天花板无关信息；

(c)启动YOLOv5的ROS检测节点，检测工作环境中门、安全出口特征，检测到具体特征后，检测算法会以矩形框框选检测到的特征，通过矩形框的四个顶点计算出对应的形心位置，将此时深度相机采集到的RGB图像与深度图对齐，得到深度图中对应的形心位置处的深度值，根据步骤2得到的深度相机内参矩阵K求出图像坐标系下的矩形框形心在深度相机坐标系下的空间位置，根据步骤2深度相机的外参平移矩阵T和外参旋转矩阵R得到矩形框形心在世界坐标系下的位置，同时利用八叉树服务中的点云地图转化功能包将得到的八叉树地图转化为二维栅格地图，将此刻的位置坐标与目标信息映射于二维栅格地图中，并将其在ROS中发布，用于导航与路径规划；

步骤5、在工作区域内进行导航与路径优化，具体包括以下子步骤：

(a)在建好的二维栅格地图中按照有效面积覆盖最大的原则设置多个全向移动机器人导航点，使全向移动机器人能够在工作环境中进行连续的导航，在导航过程中，只有从当前目标点到达下一目标点后，才会发送新的导航任务，否则不会发送新的导航任务，导航速度控制方面使用动态参数反馈法进行控制，在动态参数配置客户端设置相应的PID参数和机器人的导航速度，同时在不同路段设置不同导航速度；

(b)导航点选取完毕后，各导航点之间首先选用RRT算法进行初步路径规划，路径规划完毕后利用bresenham画圆算法在转弯处对路径进行优化，首先判断路径是否位于转弯处，判断条件通过式(5)进行描述，

式中，坐标(x

d＝3-2r (6)

式中，d为决策参数初始值，r为圆弧半径，若d＜0则按照式(7)进行处理，否则按照式(8)处理，得到更新后的(x

式中，(x

(c)路径规划完毕后，将深度相机的深度信息利用图像转换工具包转化为二维数据，给定全向移动机器人的初始位置，通过蒙特卡洛定位法进行定位，在移动过程中同时开启YOLOv5目标检测线程，当检测到特定目标后，计算此时目标在地图中的位置，同时与在步骤3中获得的历史位置进行比较，判断全向移动机器人位置是否需要矫正，判断条件通过式(9)进行描述，

|x

式中，(x

式中，(x

本发明有益效果是：基于视觉建图的全向移动机器人导航方法，包括以下步骤：(1)搭建全向移动机器人硬件平台，(2)搭建全向移动机器人软件系统，(3)基于YOLOv5对工作环境进行识别训练，(4)基于ORB_SLAM3构建视觉导航地图，(5)在工作区域内进行导航与路径优化。与已有技术相比：本发明具有以下几个优点：一是、利用视觉传感器进行视觉地图构建和导航，与激光传感器相比降低了硬件成本，同时采用麦克纳姆轮的全向移动方式，方便全向移动机器人进行导航。二是、采用深度学习的方式对工作环境特定目标进行识别，为全向移动机器人进行辅助定位，使全向移动机器人可以更加稳定的实现导航任务。三是、将基于采样的RRT路径规划算法与bresenham画圆算法进行融合，使全向移动机器人可以更加快速的通过转弯处。

附图说明

图1是本发明方法步骤流程图。

图2是全向移动机器人组装模型图。

图3是全向移动机器人实物图。

图4是坐标系转换示意图。

图5是目标识别效果图。

图中：(a)是门识别效果图，(b)是紧急出口识别效果图。

图6是工作区域稀疏点云地图。

图中：(a)是特征点提取图，(b)是稀疏点云地图。

图7是工作区域半稠密点云地图。

图8是工作区域八叉树地图。

图9是工作区域二维栅格地图。

图10是目标映射示意地图。

图11是路径规划效果图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，基于视觉建图的全向移动机器人导航方法，包括以下步骤：

步骤1、搭建全向移动机器人硬件平台，全向移动机器人组装模型图，如图2所示，采用全向麦克纳姆轮并搭配电机进行驱动，全向移动机器人顶层前端安装深度相机作为视觉传感器进行图像获取，全向移动机器人底层安装工控机与下位机，同时搭配电源与显示装置，最终全向移动机器人实物图，如图3所示。

步骤2、搭建全向移动机器人软件系统，包括以下子步骤：

(a)标定深度相机内参，准备棋盘格标定板，利用图像录制工具录制深度相机数据集并将图像采集频率降至4Hz，利用标定工具对深度相机进行内参标定，内参矩阵K通过式(1)进行描述，

式中，f

(b)以全向移动机器人第二层中心为原点，在全向移动机器人操作系统中建立世界坐标系，启动深度相机并订阅深度相机发布的节点，标定深度相机的外参，确定深度相机坐标系到世界坐标系的外参旋转矩阵R和外参平移矩阵T，通过式(2)进行描述，

式中，P

(c)根据麦克纳姆轮运动学原理，确定电机的控制方向，编写对应的方向与速度控制函数，控制全向移动机器人向前、后、左、右、右前、右后、左前、左后、正反自转十个方向运动，麦克纳姆轮的运动学分解通过式(3)进行描述，

式中，v

步骤3、基于YOLOv5对工作环境进行识别训练，具体包括以下子步骤：

(a)启动深度相机节点对工作环境进行图像获取，设置每0.1s提取一帧，并利用图像录制工具制作数据集，然后使用图片标注工具对数据集中的门、安全出口特征进行标注，得到对应的信息文件，包括标定名称、位置信息，同时将数据集分为训练集、测试集和验证集，占比分别为90％、5％、5％；

(b)对YOLOv5算法进行相关配置，修改训练名称为对应的特征标注名称，选取训练网络并将其中的锚点修改为标注的特征数量，对训练网络中的超参数进行修改，其中filter数量通过式(4)进行描述，

filter＝3*(classes+5) (4)

式中，classes为标注数量，filter为超参数中的网格参数；

(c)对YOLOv5进行训练与测试，选取官方的预训练权重，然后进行50000次训练将损失函数降至0.3以下，在验证集中对训练后的权重进行测试，保证各个检测目标的检测成功率在98％以上，否则继续进行权重优化，最后将训练后的权重与检测算法封装为ROS节点为下一步做准备，下一步做准备工作区域检测目标识别效果图，如图5所示。

步骤4、基于ORB_SLAM3构建视觉导航地图，驱动全向移动机器人在工作环境中进行移动，构造用于视觉导航地图，具体包括以下子步骤：

(a)依次启动全向移动机器人驱动节点、键盘控制节点、深度相机启动节点、ORB_SLAM3稀疏点云地图构建节点，借助键盘在整个工作区域内移动，使深度相机能够扫描到整个工作区域，ORB_SLAM3节点订阅深度相机发布的RGB图与深度图，选取关键帧建立稀疏点云地图并同时记录深度相机位置，将其保存到轨迹图文件中，工作区域稀疏点云地图如图6所示，扩大关键帧中选取特征点的数量，将稀疏点云地图转化为半稠密点云地图，同时利用点云库中的体素滤波和直通滤波对半稠密点云地图进行噪点去除，工作区域半稠密点云地图，如图7所示。

(b)启动八叉树服务，创建八叉树对象，将其分辨率设置为0.05，读取ORB_SLAM3中所建轨迹图文件中的信息，包括关键帧的帧编号、位置和姿态四元数，将关键帧中的特征点转化并拼接成八叉树地图，并根据八叉树地图高度的不同加入色彩信息方便观察以及辨别结果是否准确，同时将八叉树地图高度设置为0.1-2.0米，滤除地面和天花板无关信息，工作区域八叉树地图，如图8所示。

(c)启动YOLOv5的ROS检测节点，检测工作环境中门、安全出口特征，检测到具体特征后，检测算法会以矩形框框选检测到的特征，通过矩形框的四个顶点计算出对应的形心位置，将此时深度相机采集到的RGB图像与深度图对齐，得到深度图中对应的形心位置处的深度值，根据步骤2得到的深度相机内参矩阵K求出图像坐标系下的矩形框形心在深度相机坐标系下的空间位置，根据步骤2深度相机的外参平移矩阵T和外参旋转矩阵R得到矩形框形心在世界坐标系下的位置，同时利用八叉树服务中的点云地图转化功能包将得到的八叉树地图转化为二维栅格地图，将此刻的位置坐标与目标信息映射于二维栅格地图中，并将其在ROS中发布，用于导航与路径规划；工作区域二维栅格地图，如图9所示，目标映射示意图，如图10所示。

步骤5、在工作区域内进行导航与路径优化，具体包括以下子步骤：

(a)在建好的二维栅格地图中按照有效面积覆盖最大的原则设置多个全向移动机器人导航点，使全向移动机器人能够在工作环境中进行连续的导航，在导航过程中，只有从当前目标点到达下一目标点后，才会发送新的导航任务，否则不会发送新的导航任务，导航速度控制方面使用动态参数反馈法进行控制，在动态参数配置客户端设置相应的PID参数和机器人的导航速度，同时在不同路段设置不同导航速度；

(b)导航点选取完毕后，各导航点之间首先选用RRT算法进行初步路径规划，路径规划完毕后利用bresenham画圆算法在转弯处对路径进行优化，首先判断路径是否位于转弯处，判断条件通过式(5)进行描述，

式中，坐标(x

d＝3-2r (6)

式中，d为决策参数初始值，r为圆弧半径，若d＜0则按照式(7)进行处理，否则按照式(8)处理，得到更新后的(x

式中，(x

(c)路径规划完毕后，将深度相机的深度信息利用图像转换工具包转化为二维数据，给定全向移动机器人的初始位置，通过蒙特卡洛定位法进行定位，在移动过程中同时开启YOLOv5目标检测线程，当检测到特定目标后，计算此时目标在地图中的位置，同时与在步骤3中获得的历史位置进行比较，判断全向移动机器人位置是否需要矫正，判断条件通过式(9)进行描述，

|x

式中，(x

式中，(x