CN113610910B - 一种移动机器人避障方法 - Google Patents

Abstract

本发明公开了一种移动机器人避障方法，属于移动机器人技术领域，包括获取相机的数据和激光雷达的数据；根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障。本发明将RGBD相机和激光雷达结合应用在机器人避障方案中，融合RGBD相机数据和激光雷达数据，可以检测低障碍物，同时提高远距离障碍物检测精度。

Description

一种移动机器人避障方法

技术领域

本发明涉及移动机器人技术领域，特别涉及一种移动机器人避障方法。

背景技术

随着"泛在电力物联网"的发展，电力智能化产品越来越受青睐，移动巡检机器人可以很大程度提高工作效率，进行重复性劳动。在巡检的过程中，为了提高工作的灵活性及鲁棒性，需要机器人具有自动避障的功能。若正常巡检中遇到障碍物特，机器人可以安全可靠的躲避障碍物。

实现上述功能，需要机器人可以观察到周围障碍物。目前机器人观察周围环境主要靠激光雷达，激光雷达的优点是精度高，缺点是存在一定盲区。一维的激光雷达只能看到一个平面，在平面上或者平面下的障碍物看不到。三维激光雷达可以看到周围三维的环境，但是视野存在一定盲区，在靠近激光雷达贴近地面的障碍物观察不到。

发明内容

本发明的目的在于克服现有技术存在的缺陷，可以检测到低障碍物及提高远距离障碍物检测精度。

为实现以上目的，本发明采用一种移动机器人避障方法，包括：

获取相机的数据和激光雷达的数据；

根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障。

进一步地，所述根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障，包括：

获取所述激光雷达发出的激光点云数据和所述相机拍摄的深度图数据，并根据相机内参将深度图数据转换为相机点云数据；

根据事先标定的相对位姿，将激光点云数据和相机点云数据分别转换到机器人坐标系下，得到机器人坐标系下的激光点云数据和相机点云数据；

基于机器人坐标系下的激光点云数据和相机点云数据，进行导航避障。

进一步地，所述相对位姿包括所述相机和所述激光雷达的相对位姿及所述相机和机器人的相对位姿，标定过程包括：

以棋盘格作为参考，获取旋转矩阵和平移矩阵，标定所述相机和所述机器人的相对位姿；

获取所述相机和所述激光雷达的偏航角和相对位姿，标定所述相机和所述激光雷达的相对位姿。

进一步地，所述以棋盘格作为参考，获取旋转矩阵和平移矩阵，标定所述相机和所述机器人的相对位姿，包括：

将所述棋盘格放置于所述机器人已知位置，得到所述棋盘格的角点相对于所述机器人的位置；

获取所述棋盘格的角点在所述相机的三维空间坐标系；

通过ICP算法获取所述相机和所述机器人的相对位姿。

进一步地，所述获取所述相机和所述激光雷达的偏航角和相对位姿，标定所述相机和所述激光雷达的相对位姿，包括：

获取所述相机和所述激光雷达的偏航角yaw和相对位姿t(x,y,z)；

调节偏航角yaw和相对位姿t(x,y,z)，对齐所述激光雷达和所述相机的点云使激光雷达和相机观测到盒子的点云重叠在一起，得到所述激光雷达和所述相机的相对位姿。

进一步地，所述根据事先标定的相对位姿，将激光点云数据和相机点云数据分别转换到机器人坐标系下，得到机器人坐标系下的激光点云数据和相机点云数据，包括：

根据所述相机和机器人的位姿，将所述相机点云数据转换到机器人坐标系下，得到机器人坐标系下的相机点云数据；

根据所述相机和机器人的位姿以及所述相机和激光雷达的相对位姿，蒋所述激光点云数据转换到机器人坐标系下，得到机器人坐标系下的激光点云数据，公式表示如下：

其中，T_lc表示相机和激光雷达的相对位姿，T_rc表示相机和机器人的相对位姿，P_l表示激光雷达观测到的点云数据，P_r表示转换到机器人坐标下的点云数据，表示激光坐标系到相机坐标系的转换关系。

进一步地，所述机器人坐标系下的激光点云数据和相机点云数据，进行导航避障，包括：

对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离；

若障碍物到所述机器人的最近距离小于设定的安全距离，则发布周围环境地图、所述机器人实时位置以及给定机器人目标点；

将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图；

采用绕障算法，根据周围环境地图、所述机器人实时位置、给定的目标点以及代价地图，进行绕障直至达到给定的目标点。

进一步地，所述对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离，包括：

对所述机器人坐标系下的激光点云数据和相机点云数据进行预处理操作，得到处理后的点云数据；

对预处理后的点云数据进行聚类处理，并计算聚类处理后的点云的平均距离，得到障碍物到所述机器人的距离；

对障碍物到所述机器人的距离进行比较，得到障碍物到所述机器人的最近距离。

进一步地，所述将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图，包括：

将所述聚类处理后的点云数据中来自所述激光雷达的点云数据和来自所述相机的点云数据分别放入不同层代价地图中；

叠加两层代价地图，得到所述代价地图。

进一步地，在所述机器人无法进行避障时，还包括：

控制所述机器人遍历180度旋转，得到所述机器人会碰到代价地图障碍物的最大旋转角度；

根据最大旋转角度进行旋转。

与现有技术相比，本发明存在以下技术效果：本发明通过将RGBD相机和激光雷达结合应用在机器人避障方案中，RGBD相机在垂直角度视野大，可以检测低障碍物，激光雷达检测障碍物精度高，融合RGBD相机数据和激光雷达数据，可以检测低障碍物，同时提高远距离障碍物检测精度。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1是一种移动机器人避障方法的流程图；

图2是一种移动机器人避障方法的主要流程图；

图3是欧式聚类流程图；

图4是绕障控制框架图；

图5是棋盘格示意图。

具体实施方式

为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。

如图1至图2所示，本实施例公开了一种移动机器人避障方法，包括如下步骤S1至S2：

S1、获取相机的数据和激光雷达的数据；

S2、根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障。

需要说明的是，由于依靠激光雷达检测障碍物，精度高但存在盲区，且检测不到低障碍物，本实施例结合RGBD相机数据和激光雷达数据，进行机器人避障，RGBD相机在垂直角度视野大，可以检测低障碍物，激光雷达检测障碍物精度高，融合RGBD相机数据和激光雷达数据，可以检测低障碍物，同时提高远距离障碍物检测精度。

作为进一步优选的技术方案，RGBD相机放置在机器人前面，RGBD相机和机器人之间放置激光雷达，激光雷达和RGBD相机存在公共的视野范围。

作为进一步优选的技术方案，上述步骤S2：根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障，具体包括如下步骤S21至S23：

S21、获取所述激光雷达发出的激光点云数据和所述相机拍摄的深度图数据，并根据相机内参将深度图数据转换为相机点云数据；

S22、根据事先标定的相对位姿，将激光点云数据和相机点云数据分别转换到机器人坐标系下，得到机器人坐标系下的激光点云数据和相机点云数据；

S23、基于机器人坐标系下的激光点云数据和相机点云数据，进行导航避障。

作为进一步优选的技术方案，上述步骤S21：获取所述激光雷达发出的激光点云数据和所述相机拍摄的深度图数据，并根据相机内参将深度图数据转换为相机点云数据，具体包括步骤S211至S214：

S211、获取激光雷达发出的激光点云数据；

S212、获取RGBD相机拍摄的深度图数据，主要包括直接获取的深度图和跟彩色图像对齐的深度图，使彩色图具备相应深度信息；

S213、对深度图进行中值滤波处理，滤除椒盐噪声，得到滤波处理后的深度图；

S214、根据相机内参将滤波处理后的深度图转换为相机点云数据，相机内参可以通过相机内部参数获取，也可以通过标定相机获取内参；

本实施例中，相机内参f_x,f_y,c_x,c_y从相机内部获取为：

其中，像素点p_x，p_y为彩色图的像素，空间对应点/>x,y,z是三维坐标，存在以下关系：

因此,由彩色像素点P和对应的深度距离值z，即可获取对应的三维空间点P'：

p'＝M^-1*p*z

其中，*表示乘积。

作为进一步优选的技术方案，上述步骤S22中，事先标定的相对位姿包括相机和激光雷达的相对位姿及相机和机器人的相对位姿，其中：

(1)相机和机器人的相对位姿的标定过程为：以棋盘格作为参考，获取旋转矩阵和平移矩阵，标定所述相机和所述机器人的相对位姿，棋盘格如图5所示，用A4纸正常尺寸打印，正常打印后每个正方形边长5cm，共有9个角点，将打印好的A4纸放置在机器人前方，该角点在机器人坐标系下位置就知道了。接着打开realsenseD435彩色图信息，将深度图映射到彩色图坐标下，通过OpenCV角点检测首先获取9个角点的像素坐标，在获取相应的深度值，最后获取该角点的空间坐标，接着通过ICP求得相对位姿，具体为：

打印一张A4纸棋盘格，具有9个角点，每个棋盘格的边长是0.05cm；

将棋盘格放置机器人已知位置，得到角点相对机器人的位置X＝{x1,x2,…,x9}；

由深度图对齐彩色图，使每个彩色图像素有了对应的深度值，然后通过OpenCV角点检测算法，获取每个角点的彩色图像，由对应的深度图，可以获取角点的深度值，P＝{p1,p2,…,p9}；

已知两个点云集：X＝{x1,x2,…,x9},P＝{p1,p2,…,p9}，求解旋转矩阵R和平移矩阵t，使得下式最小：

其中，N_p是对应的点对总数，x_i,p_i是两个点集合，求解方法：

对W进行SVD分解，σ₁、σ₂、σ₃分别为W矩阵对应的奇异值，U表示V表示对角矩阵。

可以得到解：

R＝UV^T，t＝u_x-Ru_p

根据旋转矩阵R和平移矩阵t可得到相机和机器人相对位姿。

需要说明的是，相机和机器人的位置标定不限于采用棋盘格，本领域技术人员可根据实际情况采用AprilTag等方法进行标定，只要可以获取标定距离相机和机器人的位置即可。

(2)相机和激光雷达的相对位姿的标定过程为：获取所述相机和所述激光雷达的偏航角和相对位姿，标定所述相机和所述激光雷达的相对位姿，具体为：

相机和激光雷达存在公共视野范围，放置一个长高是0.4m，或者尽量大点的的盒子在公共视野范围，激光观测到盒子点云P1，RGBD观测到盒子点云P2，所以两处点云存在以下关系：

P₁＝T_lc*P₂+t；

调节yaw和t对齐激光雷达和RGBD相机的点云使激光雷达和RGBD观测到盒子的点云重叠在一起，即可得到激光雷达和RGBD相机的相对位姿。

由于相机是超前水平放置的，所以相机的Z轴坐标系和机器人X轴坐标系同一方向，考虑到安装误差，存在一个偏航误差yaw,所以：

t＝[x y z]。

作为进一步优选的技术方案，上述步骤S22：根据事先标定的相对位姿，将激光点云数据和相机点云数据分别转换到机器人坐标系下，得到机器人坐标系下的激光点云数据和相机点云数据，包括：

根据所述相机和机器人的位姿，将所述相机点云数据转换到机器人坐标系下，得到机器人坐标系下的相机点云数据；

根据所述相机和机器人的位姿以及所述相机和激光雷达的相对位姿，蒋所述激光点云数据转换到机器人坐标系下，得到机器人坐标系下的激光点云数据，公式表示如下：

其中，T_lc表示相机和激光雷达的相对位姿，T_rc表示相机和机器人的相对位姿，P_l表示激光雷达观测到的点云数据，P_r表示转换到机器人坐标下的点云数据，表示激光坐标系到相机坐标系的转换关系。

作为进一步优选的技术方案，上述步骤S23：基于机器人坐标系下的激光点云数据和相机点云数据，进行导航避障，包括如下步骤S231至S234：

S231、对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离；

S232、若障碍物到所述机器人的最近距离小于设定的安全距离，则发布周围环境地图、所述机器人实时位置以及给定机器人目标点；

S233、将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图；

S234、采用绕障算法，根据周围环境地图、所述机器人实时位置、给定的目标点以及代价地图，进行绕障直至达到给定的目标点。

作为进一步优选的技术方案，如图3所示，上述步骤S231：对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离，包括如下步骤：

(1)对所述机器人坐标系下的激光点云数据和相机点云数据进行预处理操作，得到处理后的点云数据，包括：

体素滤波，降低分辨率：对输入的点云数据创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合)，然后在每个体素(即，三维立方体)内，用体素中所有点的重心来近似显示体素中其他点，这样该体素就内所有点就用一个重心点最终表示，对于所有体素处理后得到过滤后的点云；

过滤地面，消除地面点云的影响：由于机器人在地面行走时，地面可能不是平整的，所以引入了IMU惯导测量单元传感器，具有三轴加速度计，和三轴陀螺仪，可以结算机器人的姿态。这样可以消除地面不平整造成的影响。IMU可以输出机器人的位姿变化T_Δ。观测到的地面点云在Z轴上的投影是有一定阈值的，设置高度为h，低于这个高度的都是地面，高于这个阈值的属于障碍物。因此将得到的点云相对于机器人坐标系P_r进行如下处理：

P_r'＝T_Δ*P_r

将得到的点云，进行遍历，如果高度大于阈值h，定义为障碍物。

噪声消除，进行外点剔除：除点云数据中的离群点(噪点)，使用的方法为：对于每个点的，计算它到它所有临近点的平均距离，假设得到的是一个高斯分布，那么根据均值与标准差，平均距离在标准范围外的点，就是离群点。

(2)对预处理后的点云数据进行聚类处理，并计算聚类处理后的点云的平均距离，得到障碍物到所述机器人的距离；

需要说明的是，本实施例中点云聚类原理如图3所示，随着不断迭代，最终Q中集合数量及分布会逐渐收敛，这个时候判断Q中数量是否满足要求，如果不满足不予处理。

(3)对障碍物到所述机器人的距离进行比较，得到障碍物到所述机器人的最近距离，具体为：计算聚类后的点云的平均距离，对最终的不同点云聚类求取距离机器人的平局距离，即为障碍物到机器人的距离。然后对这些距离进行比较，找到距离机器人最近的点云，并将此距离做为机器人是否进行避障的判断依据，即上述步骤S232中将障碍物到机器人的最近距离与设定的安全距离进行比较作为是否进行避障的判断依据，具体为：

设置安全距离为1.2m，一旦发现障碍物距离机器人最近距离在1.2m时，即可进行绕障。

作为进一步优选的技术方案，需要说明的是，由于激光雷达的数据属于中远距离，相机的数据是近距离，所以需要放进不同的代价地图层，用于计算不同路径的代价，上述步骤S233：将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图，包括：

将所述聚类处理后的点云数据中来自所述激光雷达的点云数据和来自所述相机的点云数据分别放入不同层代价地图中；

叠加两层代价地图，得到所述代价地图。

需要说明的是，代价地图是一种栅格地图，可以实时更新点云的信息，每个栅格值范围在0～255。如果是0说明，此处没有障碍物。如果有障碍物，相应的栅格被赋值为255。由于机器人具有一定体积，为了将机器人视为一个点，等效于将栅格中的障碍物进行膨胀，所以栅格中的数值在0～255范围，为了描述机器人距离障碍物的距离程度。目前点云来自激光的和RGBD相机的，来自RGBD的点云距离短，来自激光的点云距离远。如果放入同一层代价地图中，激光的点云会影响RGBD相机的数据，代价地图可以分为很多层，所以本实施例提出了将RGBD相机获取的点云单独作为一层，然后叠加两层代价地图。

作为进一步优选的技术方案，上述步骤S234：采用绕障算法，根据周围环境地图、所述机器人实时位置、给定的目标点以及代价地图，进行绕障直至达到给定的目标点，具体为：

本实施例采用的绕障算法是move_base的软件包，如图4所示，绕障框架，算法的输入是需要移动的目标点、周围环境地图、机器人的实时定位，以及处理过后传感器点云，输出控制指令至机器人，机器人根据控制指令绕开障碍物。采用全局规划器使用A*算法，基于代价地图，搜索当前点到目标点的最短路径。局部规划器根据传感器点云，实时更新代价地图栅格分布，由机器人实时定位，以及全局规划器提供的全局路径，实时发布控制机器人避障指令。

作为进一步优选的技术方案，恢复机制作用是当机器人卡在一个位置，或者规定时间内总是在一定区域运动，无法有效的进行避障时，机器人会采取一些恢复机制，例如旋转180度，清除代价地图等操作。在旋转180度恢复机制，本实施例做了改进，在旋转时，如果周围障碍物太近，导致机器人经常会无法旋转180度。本实施例通过遍历180度旋转，可以得到机器人会碰到代价地图障碍物的最大旋转角度，然后根据这个角度进行旋转，会尽可能更新周围的环境，减少卡住的几率。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种移动机器人避障方法，其特征在于，包括：

获取相机的数据和激光雷达的数据；

根据相机的数据和激光雷达的数据进行数据融合处理，进行导航避障，其具体步骤包括：

获取所述激光雷达发出的激光点云数据和所述相机拍摄的深度图数据，并根据相机内参将深度图数据转换为相机点云数据；

根据事先标定的相对位姿，将激光点云数据和相机点云数据分别转换到机器人坐标系下，其中相机为RGBD相机，且RGBD相机放置在机器人前面，RGBD相机和机器人之间放置激光雷达，激光雷达和RGBD相机存在公共的视野范围，得到机器人坐标系下的激光点云数据和相机点云数据，其具体步骤包括：

根据所述相机和机器人的位姿，将所述相机点云数据转换到机器人坐标系下，得到机器人坐标系下的相机点云数据；

根据所述相机和机器人的位姿以及所述相机和激光雷达的相对位姿，蒋所述激光点云数据转换到机器人坐标系下，得到机器人坐标系下的激光点云数据，公式表示如下：

其中，表示相机和激光雷达的相对位姿，/>表示相机和机器人的相对位姿，/>表示激光雷达观测到的点云数据，/>表示转换到机器人坐标下的点云数据，/>表示激光坐标系到相机坐标系的转换关系；

基于机器人坐标系下的激光点云数据和相机点云数据，进行导航避障，其具体步骤包括：

对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离；

若障碍物到所述机器人的最近距离小于设定的安全距离，则发布周围环境地图、所述机器人实时位置以及给定机器人目标点；

将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图；

采用绕障算法，根据周围环境地图、所述机器人实时位置、给定的目标点以及代价地图，进行绕障直至达到给定的目标点；

所述相对位姿包括所述相机和所述激光雷达的相对位姿及所述相机和机器人的相对位姿，标定过程包括：

以棋盘格作为参考，获取旋转矩阵和平移矩阵，标定所述相机和所述机器人的相对位姿，其具体步骤包括：

将所述棋盘格放置于所述机器人已知位置，得到所述棋盘格的角点相对于所述机器人的位置；

获取所述棋盘格的角点在所述相机的三维空间坐标系；

通过ICP算法获取所述相机和所述机器人的相对位姿；

获取所述相机和所述激光雷达的偏航角和相对位姿，标定所述相机和所述激光雷达的相对位姿，其具体步骤包括：

获取所述相机和所述激光雷达的偏航角yaw和相对位姿t(x, y, z) ；

调节偏航角yaw和相对位姿t(x, y, z)，对齐所述激光雷达和所述相机的点云使激光雷达和相机观测到盒子的点云重叠在一起，得到所述激光雷达和所述相机的相对位姿。

2.如权利要求1所述的移动机器人避障方法，其特征在于，所述对所述机器人坐标系下的激光点云数据和相机点云数据进行聚类处理，得到障碍物到所述机器人的最近距离，包括：

对所述机器人坐标系下的激光点云数据和相机点云数据进行预处理操作，得到处理后的点云数据；

对预处理后的点云数据进行聚类处理，并计算聚类处理后的点云的平均距离，得到障碍物到所述机器人的距离；

对障碍物到所述机器人的距离进行比较，得到障碍物到所述机器人的最近距离。

3.如权利要求1所述的移动机器人避障方法，其特征在于，所述将聚类处理后的点云数据投影到二维平面，进行代价地图融合，得到代价地图，包括：

将所述聚类处理后的点云数据中来自所述激光雷达的点云数据和来自所述相机的点云数据分别放入不同层代价地图中；

叠加两层代价地图，得到所述代价地图。

4.如权利要求1所述的移动机器人避障方法，其特征在于，在所述机器人无法进行避障时，还包括：

控制所述机器人遍历180度旋转，得到所述机器人会碰到代价地图障碍物的最大旋转角度；

根据最大旋转角度进行旋转。