[发明专利]基于视觉建图的全向移动机器人导航方法

权利要求书

1.一种基于视觉建图的全向移动机器人导航方法，其特征在于包括以下步骤：

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

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

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

式中，f_x表示深度相机x轴方向上以像素为单位的焦距，f_y表示深度相机y轴方向上以像素为单位的焦距，u₀表示光轴在x轴方向上与图像中心的以像素为单位的差距，v₀表示光轴在y轴方向上与图像中心的以像素为单位的差距；

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

式中，P₀为深度相机坐标系，P_w为世界坐标系，R为外参旋转矩阵，T为外参平移矩阵，(x₀,y₀,z₀)为物体在相机坐标系下的三维坐标，(x_w,y_w,z_w)为物体在世界坐标系下的三维坐标；

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

式中，v_w1,v_w2,v_w3,v_w4为4个麦克纳姆轮的转速，v_tx为x轴运动的速度，v_ty为y轴运动的速度，ω为自转角速度，a为全向移动机器人宽度，b为全向移动机器人长度；

步骤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₁,y₁)为当前节点在实际地图中的坐标，(x₂,y₂)为当前节点的下一节点坐标，K₁为当前节点与下一节点之间的斜率，K₂为当前节点与前一节点之间的斜率，如果斜率之差大于0.45则判定当前位置为转弯处，则以(x₁,y₂)为原点建立坐标系，以原点为圆心，(x₀,y₀)＝(0,|y₁-y₂|)为起点，r＝|y₁-y₂|为半径应用bresenham画圆算法对路径进行圆弧处理，通过式(6)进行描述，

d＝3-2r (6)

式中，d为决策参数初始值，r为圆弧半径，若d＜0则按照式(7)进行处理，否则按照式(8)处理，得到更新后的(x_i,y_j)，若x_i＜y_j，则将(x_i,y_j)加入路径，同时继续该步骤，直至x_i＝y_j节点更新完成1/8圆弧路径，随后根据圆的对称性对称得到另外1/8圆弧路径，从而得到完整的1/4圆弧路径，转弯处路径绘制完成；

式中，(x_i,y_j)为当前节点坐标，(x_i+1,y_j+1)为更新后的节点坐标，i,j为计数标志；

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

|x_n-x_p|+|y_n-y_p|＞0.2 (9)

式中，(x_n,y_n)为当前目标的检测位置坐标，(x_p,y_p)为目标的历史位置坐标，判断条件成立则对全向移动机器人位置进行矫正，通过式(10)进行描述，

式中，(x_t,y_t)为全向移动机器人的当前位置，为避免重复矫正计算，当识别到目标且对全向移动机器人位置矫正完成后，全向移动机器人移动3秒后再开启检测线程进行重新矫正。