CN113110451A - 一种深度相机与单线激光雷达融合的移动机器人避障方法 - Google Patents

一种深度相机与单线激光雷达融合的移动机器人避障方法 Download PDF

Info

Publication number
CN113110451A
CN113110451A CN202110401044.5A CN202110401044A CN113110451A CN 113110451 A CN113110451 A CN 113110451A CN 202110401044 A CN202110401044 A CN 202110401044A CN 113110451 A CN113110451 A CN 113110451A
Authority
CN
China
Prior art keywords
obstacle
mobile robot
laser radar
depth
depth camera
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110401044.5A
Other languages
English (en)
Other versions
CN113110451B (zh
Inventor
朱威
巫浩奇
洪力栋
韩慧
陈伟锋
何德峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN202110401044.5A priority Critical patent/CN113110451B/zh
Publication of CN113110451A publication Critical patent/CN113110451A/zh
Application granted granted Critical
Publication of CN113110451B publication Critical patent/CN113110451B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Abstract

本发明涉及一种深度相机与单线激光雷达融合的移动机器人避障方法，包括以下步骤：（1）对深度相机与单线激光雷达进行联合标定；（2）深度图像预处理及裁剪；（3）判断预处理后的深度图像是否需要与激光雷达进行数据融合；（4）使用贝叶斯估计法进行栅格地图融合；（5）使用局部避障算法进行避障。本发明弥补了使用单个深度相机只能获得前方视野范围内的区域不能获得后面和左右两侧环境数据的缺陷，并通过对深度图像进行预处理及裁剪大幅减少了需要计算的深度图像数据，有效加快了避障的数据处理速度。

Description

一种深度相机与单线激光雷达融合的移动机器人避障方法
技术领域
本发明属于机器人领域，具体涉及一种深度相机与单线激光雷达融合的移动机器人避障方法。
背景技术
移动机器人能够代替人类从事一些服务性和带有危险性的工作，已被应用于娱乐、医疗、救援等多个领域。移动机器人时常工作在未知的动态环境下，在移动过程中会遇到各种各样的障碍物；面对复杂的工作环境，有效的避障技术是移动机器人导航的基础，能够有效识别并快速躲避障碍物是完成目标任务的关键。
在实际环境中，使用单一的传感器检测障碍物存在局限性，传感器的安装位置、安装角度等因素都会对避障能力产生影响，对异形障碍物，如中空的桌子、桌边角、伸出的杆子、下部中空的门等等检测能力有限，无法判断出障碍物的具体位置。常用的单线激光雷达只能检测出与其安装位置位于同一水平面的障碍物，而不能检测到架空的障碍物。三维激光雷达可以检测出立体空间的障碍物，但其价格昂贵且处理三维激光雷达获得的三维点云数据会耗费大量的计算资源。深度相机也可以检测出立体空间的障碍物且价格低廉，但使用单个深度相机只能检测出相机前方一小片区域，不能实时检测左右两侧及后面的区域，存在检测角度范围小的缺陷。
申请号为CN201910454798.X的专利公开了一种激光和视觉融合的避障方法，主要采用YOLO目标检测网络对有限类别的特定障碍物进行检测，但该方法在检测障碍物的时候，只能框选到数据集中已有的障碍物，无法实现在复杂场景下数据集中未知障碍物的检测。
申请号为202010829113.8的专利公开了一种使用基于强化学习的3D避障导航系统及方法，主要使用RGB-D相机作为感知模块，在动态环境下使用基于深度强化学习的局部避障。由于RGB-D相机只能识别出前方区域的障碍物信息，所以该方法不能使机器人对左右两侧突然出现的行人进行避让。
发明内容
为了解决移动机器人难以有效避开周围行人及架空障碍物的问题，本发明提出了一种深度相机与单线激光雷达融合的机器人避障方法，该方法使用的传感器比三维激光雷达成本更低，并且弥补了使用单个深度相机只获得前方视野范围内的区域而不能获得后面和左右两侧环境数据的缺陷；将深度相机获取的深度图像经过裁剪和预处理后，通过判断裁剪出的区域范围内是否存在深度相机获取的数据信息所生成的点云数据，若存在则将深度相机获取的数据与激光雷达数据相融合，若不存在则不进行融合，从而大幅减少了需要处理的深度图像数据，提高了避障的计算速度。所述方法包括以下步骤：
(1)对深度相机与单线激光雷达进行联合标定
(1-1)单线激光雷达与深度相机所使用的坐标系不相同，因此需要对它们进行联合标定。本发明先使用张正友提出的棋盘格标定法对深度相机进行标定得到深度相机的内参矩阵和外参矩阵，然后按式(1)和(2)将深度相机坐标系转换到移动机器人本体坐标系：
Figure BDA0003020301300000031
Figure BDA0003020301300000032
其中，u、v分别为深度图像中各个像素点的行列号，cx、cy分别为深度图像中心坐标，d表示深度图像中各个像素点的深度距离，fx、fy分别为深度相机水平、垂直方向的焦距，R为旋转矩阵，T为转换向量；xc、yc和zc分别为深度相机的三轴坐标；本发明中机器人本体坐标系与单线激光雷达坐标系为同一坐标系。
(2)获取一帧深度图像和一帧单线激光雷达数据，所获得的深度图像和单线激光雷达数据为同一帧。
(3)深度图像预处理及裁剪
移动机器人的工作场景具有高度不确定性，相机采集到的深度图像中存在一些多余的背景特征，若将深度图直接与激光数据进行融合就会影响到移动机器人避障。由于不同深度相机的深度图像尺寸不同，为了减少深度图像的计算处理时间，降低处理深度图像的数据量，提高检测精度和融合效率，本发明对采集到的深度图像首先使用双边滤波算法去除噪声，再将深度图像的大小和深度值进行裁剪，最后将裁剪后的深度图像转成3D点云数据。具体方法如下：
(3-1)将步骤(2)获取到的一帧深度图像使用双边滤波算法进行处理，达到保持边缘、降噪平滑的效果。
(3-2)先设定经过步骤(3-1)滤波处理后的深度图像的有效像素区域为水平像素坐标在[cx-width,cx+width]范围内，竖直像素坐标在[cy-height,cy+height]的范围内的矩形区域，其中cx、cy分别为深度图像中的水平像素中点坐标和竖直像素的中点坐标，width为有效像素区域到中心坐标的最大水平像素距离，height为有效像素区域到中心坐标的最大竖直像素距离；再对深度图像进行裁剪，只保留在深度图像有效像素点区域内以及深度值大于等于depthmin且小于等于depthmax的像素点深度数据，其它像素点都设为无效深度值，得到裁剪后的深度图像，预设的像素深度值范围为[10,120]。
(4)判断预处理后的深度图像是否需要与激光雷达数据融合
(4-1)先将裁剪后的深度图像通过深度相机内参矩阵和外参矩阵转换成点云数据，再将点云数据通过深度相机到移动机器人本体的坐标系转换式(式(1)和(2))转换至移动机器人本体坐标系中，最终得到坐标系转换后的点云数据P。
(4-2)在三维空间下，对点云数据P使用半径滤波器进行处理，设置点云邻域半径r，对于P中的每个点云点，统计以其为中心的半径为r的球体内的空间相邻点云点的数量Num1，若Num1小于等于邻近点云点数量阈值THnum1，则将当前点云点消除，否则保留该点云点，最终得到滤除异常空间噪声点云点的点云数据M；r的取值范围为[0.5，5]，单位厘米，THnum1的取值范围为[2,6]。
(4-3)判断裁剪后的深度图像是否需要与激光雷达进行融合建立栅格地图；
在判断是否进行对裁剪后的深度图像与激光雷达进行融合建立栅格地图的决策中，先统计步骤(4-2)得到的点云数据M中的点云点个数Num2，若Num2大于等于总点云数阈值THnum2，说明深度相机检测到障碍物，则跳到下一步，否则不进行融合建立栅格地图，直接使用单线激光雷达数据生成栅格地图，并跳到步骤(6)；THnum2的取值范围为[2,6]。
(5)使用贝叶斯估计法进行栅格地图融合
(5-1)将步骤(4-3)生成的点云数据M中每个点云点都投影到移动机器人坐标系平面上形成二维的伪激光点数据，然后再将伪激光点数据生成的栅格地图与单线激光雷达获得的激光数据所生成的栅格地图，使用经典的贝叶斯递推公式来进行融合；
(5-2)设置占据概率阈值，用于确定栅格地图是否被占据；
(5-3)以深度相机的光心为起点，扫描所有角度，以光心和扫描到的障碍物间的栅格为
Figure BDA0003020301300000051
表示观测到栅格未被占据，此区域可行，以O表示观测到栅格被占据；
(5-4)以E表示障碍物真实存在，
Figure BDA0003020301300000052
表示障碍物不存在，基于式(3)和(4)，得到后验概率：
Figure BDA0003020301300000053
Figure BDA0003020301300000054
其中，p(E)表示先验概率，p（O|E）与
Figure BDA0003020301300000055
表示传感器的测量模型，且
Figure BDA0003020301300000056
(5-5)当递推公式被转换为占据栅格结构图时，如式(5)为改进的融合公式：
Figure BDA0003020301300000061
其中，
Figure BDA0003020301300000062
和
Figure BDA0003020301300000063
分别代表先验地图的栅格占据和未被占据单元的先验概率，po表示栅格单元条件概率，
Figure BDA0003020301300000064
代表传感器通过观测确认栅格是否被占据的条件概率；
(5-6)通过栅格被占据的概率大小来确定栅格地图的生成，被占据的概率越大生成栅格地图的可能性就越大，设置概率阈值后，若计算出的概率大于设定的阈值则可生成栅格地图，小于设定的概率阈值则不生成，相机生成的栅格地图与雷达生成的栅格地图相融合后，新生成的栅格概率补充占据原有的栅格单元的概率。
(6)使用局部避障算法进行避障
利用已生成的栅格地图，采用经典的动态避障算法进行机器人避障。
本发明中，使用单线激光雷达与深度相机相融合的多传感器方案可以准确获取环境中的障碍物信息，有效弥补因单个传感器存在检测角度范围和检测精度的缺陷，提高移动机器人在移动过程中检测障碍物的鲁棒性。
附图说明
图1为本发明流程框图。
图2为深度图像预处理流程框图。
图3为深度相机与单线激光雷达融合流程框图。
图4为深度相机与单线激光光雷达安装相对位置示意图。
具体实施方式
下面结合实施例和附图来详细描述本发明，但本发明并不仅限于此。
本发明深度相机与单线激光雷达相融合的移动机器人避障方法的实现是基于机器人操作系统ROS平台。机器人的转向结构是双轮差速转向，深度相机采用英特尔RealSense D435i深度相机，单线激光雷达使用思岚Rplidar A1。机器人上使用英伟达公司的AI边缘计算平台Jetson Xavier NX作为主控制器，操作系统为Ubuntu18.04+ROSMelodic。坐标系x、y与z分别对应机器人相机的左右、上下及前后方向。其中，坐标系z描述了机器人相对障碍物的位置关系，坐标系y所描述的高度信息可用于深度图裁剪中的无效点。相机光轴与z轴对应，以确定相机相对其他坐标系的转换关系式。此外，本实施例中相机的水平分辨率为1280、竖直分辨率为720。
室内机器人在向前移动过程中主要躲避前方存在的障碍物以及左右两侧突然出现的行人，使用深度相机与单线激光雷达组合获取周围环境数据的方案。激光雷达负责扫描所安装高度平面内障碍物，深度相机负责扫描机器人向前运动的前方立体空间内的障碍物。此方案使用的传感器比三维激光雷达更为廉价，弥补了使用单个深度相机只获得前方视野范围内的区域不能获得后面和左右两侧环境数据的缺陷。将深度相机获取的深度图像经过预处理和先裁剪后再和激光雷达数据相融合，使需要计算的深度图像数据大大减少，提高CPU计算速度、减少了处理数据的时间。
如图1所示，一种深度相机与单线激光雷达相融合的移动机器人避障方法，包括以下步骤：
(1)对移动机器人上的深度相机与单线激光雷达进行联合标定；
(2)获取一帧深度图像和一帧单线激光雷达数据，所获得的深度图像和单线激光雷达数据为同一帧；
(3)深度图像预处理及裁剪；
(4)判断裁剪后的深度图像是否要与激光雷达进行融合建立栅格地图；
(5)使用贝叶斯估计法进行栅格地图融合；
(6)使用局部避障算法进行避障。
步骤(1)具体包括：
单线激光雷达安装于移动机器人下部，深度相机安装于移动机器人上部镜头朝向行进的方向。激光传感器与深度相机所使用的坐标系不相同，所以需要对所使用的单线激光雷达和深度相机进行联合标定。本发明中激光雷达坐标系与移动机器人本体坐标系为同一坐标系，通过标定获得深度相机坐标系相对于移动机器人本体的关系间接获得深度相机坐标系相对于激光雷达坐标系之间的关系。具体步骤如下：
(1-1)本发明使用张正友提出的棋盘格标定法对深度相机进行内参标定，使用9×12大小的棋盘格，格子尺寸为6.5cm×6.5cm。在标定过程中将棋盘格处于深度相机的是视角范围内，通过平放、左偏、右偏、前倾、后倾等使图片中标定板出现在图像的各个位置上，并尽量覆盖实际工作距离，得出深度相机内参矩阵和外参矩阵。
(1-2)深度相机到移动机器人本体的坐标系转换关系式如下：
Figure BDA0003020301300000091
Figure BDA0003020301300000092
其中，式(1)将2D图像坐标系转换至3D相机坐标系，u、v表示图像各个像素点的行列号；cx、cy表示深度图像中心坐标；d表示深度图像中各个像素点的深度距离；fx、fy表示相机水平、垂直方向的焦距；xc、yc和zc分别为深度相机的三轴坐标；式(2)通过旋转矩阵R以及转换向量将相机坐标系下的坐标转换至移动机器人坐标系中。当相机固定在移动机器人距离地面高度为1.5米的位置时，旋转矩阵及转换向量取值如下：
Figure BDA0003020301300000093
Figure BDA0003020301300000094
步骤(2)和(3)具体包括：
本发明中使用的是英特尔RealSense D435i深度相机，D435i深度相机获取的深度图像大小以及所能获取到的深度图的最大或是最小距离是由该深度相机官方提供的SDK包决定的。实际使用中若直接使用官方提供的SDK包获取深度图像数据，所得到得深度距离、深度图像高度、深度图像宽度是比较大的。如果不对原始的深度图进行预处理及裁剪直接与单线激光雷达数据进行融合会耗费大量的计算性能与处理时间。为了减少深度图像的计算处理时间，降低处理深度图像的数据量，提高检测精度和融合效率，本发明对采集到的深度图像首先使用双边滤波算法去除噪声，再将深度图像的大小和深度值进行裁剪，最后将裁剪后的深度图像转成点云数据。具体步骤如下：
(2-1)获取一帧深度图像和一帧单线激光雷达数据，所获得的深度图像和单线激光雷达数据为同一帧。
(3-1)将步骤(2)获取到的一帧深度图像使用OpenCV库中的bilateralFilter函数(双边滤波算法)进行处理，bilateralFilter函数中使用的参数值是bilateralFilter(image,out,25,25*2,25/2)，达到对深度图像保持边缘、降噪平滑的效果。
(3-2)先设定经过步骤(3-1)滤波处理后的深度图像的有效像素区域为水平像素坐标在[cx-width,cx+width]范围内，竖直像素坐标在[cy-height,cy+height]的范围内的矩形区域，其中cx、cy分别为深度图像中的水平像素中点坐标和竖直像素的中点坐标，cx、cy分别取值为640、360，width为有效像素区域到中心坐标的最大水平像素距离取值为150，height为有效像素区域到中心坐标的最大竖直像素距离取值为100，深度值最小阈值depthmin和深度值最大阈值depthmax分别取值为10、120；裁剪过程中只保留水平像素坐标在[490,790]范围内，竖直像素坐标在[260,460]的范围内的矩形区域以及像素深度值在范围[10,120]内的深度数据，其它像素点都设为无效深度值。将裁剪后的深度图像输出。
步骤(4)具体包括：
(4)判断预处理后的深度图像是否需要与激光雷达进行融合建立栅格地图
(4-1)先将裁剪后的深度图像通过深度相机内参矩阵和外参矩阵转换成点云数据，再将点云数据通过深度相机到移动机器人本体的坐标系转换式(式(1)和(2))转换至移动机器人本体坐标系中，最终得到坐标系转换后的点云数据P。
(4-2)对点云数据P使用三维半径滤波器进行处理，设置点云邻域半径r，对于P中的每个点云点，统计以其为中心的半径为r的球体内的空间相邻点云点的数量Num1，若Num1小于等于邻近点云点数量阈值THnum1，则将当前点云点消除，否则保留该点云点，最终得到滤除异常空间噪声点云点的点云数据M；r的取值范围为[0.5，5]，单位厘米，此处取值5，THnum1的取值范围为[2,6]，此处取值4。
(4-3)在判断是否进行对深度相机将点云数据投影到二维平面生成的栅格地图与单线激光雷达生成的栅格地图融合的决策中，先统计步骤(2-4)得到的点云数据M中的点云点个数Num2，若Num2大于等于总点云数阈值THnum2，说明深度相机检测到障碍物，则跳到下一步，否则不进行栅格地图融合，直接使用单线激光雷达数据生成栅格地图，并跳到步骤(5)；THnum2的取值范围为[2，6]，此处取值4。
步骤(5)具体包括：
(5-1)先将步骤(4-2)生成的点云数据M中每个点云点都投影到移动机器人坐标系平面上形成二维的伪激光点数据，然后再将伪激光点数据生成的栅格地图与单线激光雷达获得的激光数据所生成的栅格地图，使用经典的贝叶斯递推公式来进行融合生成新的栅格。
(5-2)设置占据概率阈值，用于确定栅格地图是否被占据。
(5-3)以深度相机的光心为起点，扫描所有角度，以光心和扫描到的障碍物间的栅格为
Figure BDA0003020301300000121
表示观测到栅格未被占据，此区域可行，以O表示观测到栅格被占据。
(5-4)以E表示障碍物真实存在，
Figure BDA0003020301300000122
表示障碍物不存在，基于式(3)和(4)，得到后验概率：
Figure BDA0003020301300000123
Figure BDA0003020301300000124
其中，p(E)表示先验概率，p(O|E)与
Figure BDA0003020301300000125
表示传感器的测量模型，且
Figure BDA0003020301300000126
(5-5)当递推公式被转换为占据栅格结构图时，如式(5)为改进的融合公式：
Figure BDA0003020301300000127
其中，
Figure BDA0003020301300000128
和
Figure BDA0003020301300000129
分别代表先验地图的栅格占据和未被占据单元的先验概率，po表示栅格单元条件概率，
Figure BDA00030203013000001210
代表传感器通过观测确认栅格是否被占据的条件概率。
(5-6)通过栅格被占据的概率大小来确定栅格地图的生成，被占据的概率越大生成栅格地图的可能性就越大，设置概率阈值后，若计算出的概率大于设定的阈值则可生成栅格地图，小于设定的概率阈值则不生成，相机生成的栅格地图与雷达生成的栅格地图相融合后，新生成的栅格概率补充占据原有的栅格单元的概率。
在激光雷达与深度相机实现二维地图融合的过程中采用表1中的融合规则。
表1.单线激光雷达与深度相机融合规则
Figure BDA0003020301300000131
步骤(6)具体包括：
基于步骤(5)得到的栅格，定义单线激光雷达距离障碍物之间的距离为Distance，单位为米，通过单线激光雷达遇到障碍物返回的激光数据计算得到，本发明中Distance值最大为8m。定义机器人与深度相机镜头朝向所在的局部立体空间内障碍物之间的距离为3D_Distance，单位为米，通过深度相机遇到障碍物返回的3D点云数据通过坐标系转换到机器人坐标系计算得到，3D_Distance值最大为经过步骤(2)裁剪得到的深度相机最大深度距离，在本发明中3D_Distance最大为2m。定义移动机器人能够安全移动的情况下周围障碍物距离机器人本体最小距离为Min_Distance，本发明中Min_Distance为0.5m。本发明根据障碍物与移动机器人之间的平面距离Distance和局部立体空间内障碍物之间的距离3D_Distance划分为三类区域：
①安全区域：障碍物距离移动机器人距离范围为[2，8]，单位为米。此时机器人前方无障碍或是障碍物不影响移动机器人能够正常行走。
②障碍区域：障碍物距离移动机器人距离范围为[0.5，2)，单位为米，此时机器人前方有障碍或障碍物影响移动机器人正常行走。
③碰撞区域：障碍物距离移动机器人距离范围为[0，0.5)，单位为米。此时机器人前方有障碍且障碍物影响移动机器人正常行走，机器人立即停止运动。
若检测到障碍物与移动机器人的距离在安全区域范围内，则移动机器人按照全局路径规划的路线移动，深度相机数据不与单线激光雷达数据进行融合。若检测到障碍物与移动机器人的距离在障碍区域范围内，则将深度相机数据与单线激光雷达数据进行融合传递到局部代价地图，通过局部避障的动态窗口算法(DWA)输出控制移动机器人左右主动轮的线速度、角速度以及局部避障路径，从而躲避障碍物。

Claims (9)

1.一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述方法包括以下步骤：
步骤1：对移动机器人上的深度相机与单线激光雷达进行联合标定；
步骤2：获取一帧深度图像和一帧单线激光雷达数据，所获得的深度图像和单线激光雷达数据为同一帧；
步骤3：对深度图像进行预处理；
步骤4：判断预处理后的深度图像是否需要与激光雷达数据融合；
步骤5：使用贝叶斯估计法进行数据融合；
步骤6：使用局部避障算法进行避障。
2.根据权利要求1所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤1中，以棋盘格标定法对深度相机进行标定，得到深度相机的内参矩阵和外参矩阵，按式(1)和(2)将深度相机坐标系转换到移动机器人本体坐标系：
Figure FDA0003020301290000011
Figure FDA0003020301290000012
其中，u、v分别为深度图像中各个像素点的行列号，cx、cy分别为深度图像中心坐标，d表示深度图像中各个像素点的深度距离，fx、fy分别为深度相机水平、垂直方向的焦距，R为旋转矩阵，T为转换向量；xc、yc和zc分别为深度相机的三轴坐标；
移动机器人本体坐标系与单线激光雷达的坐标系一致。
3.根据权利要求1所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤3中，对深度图像进行预处理包括：
步骤3.1：将步骤2获得的深度图像使用双边滤波算法进行处理；
步骤3.2：设定经过步骤3.1滤波处理后的深度图像的有效像素区域为水平像素坐标在[cx-width,cx+width]、竖直像素坐标在[cy-height,cy+height]的矩形区域，其中cx、cy分别为深度图像中心坐标，width为有效像素区域到中心坐标的最大水平像素距离，height为有效像素区域到中心坐标的最大竖直像素距离；
步骤3.3：基于有效像素区域和预设的像素深度值范围，对经过步骤3.1滤波处理后的深度图像进行裁切，输出裁剪后的深度图像。
4.根据权利要求3所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤3.3中，预设的像素深度值范围为[10,120]。
5.根据权利要求1所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤4包括以下步骤：
步骤4.1：将预处理后的深度图像通过深度相机内参矩阵和外参矩阵转换成点云数据，将点云数据通过步骤1的联合标定的方式转换至移动机器人本体坐标系中，得到坐标系转换后的点云数据P；
步骤4.2：在三维空间下，对点云数据P使用半径滤波器进行处理，设置点云邻域半径r，对于P中的每个点云点，统计以其为中心、半径为r的球体内的空间相邻点云点的数量Num1；若Num1小于等于邻近点云点数量阈值THnum1，则将当前点云点消除，否则保留该点云点，得到滤除异常空间噪声点云点的点云数据M；
步骤4.3：统计点云数据M中的点云点个数Num2，若Num2大于等于总点云数阈值THnum2，说明深度相机检测到障碍物，则进行步骤5，否则不进行数据融合，直接使用单线激光雷达数据生成栅格地图并进行步骤6。
6.根据权利要求5所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：r∈[0.5,5]，单位厘米；THnum1∈[2,6]；THnum2∈[2,6]。
7.根据权利要求5所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤5包括以下步骤：
步骤5.1：将步骤4.3生成的点云数据M中每个点云点都投影到移动机器人坐标系平面上形成二维的伪激光点数据，然后再将伪激光点数据生成的栅格地图与单线激光雷达获得的激光数据所生成的栅格地图，使用经典的贝叶斯递推公式来进行融合；
步骤5.2：设置占据概率阈值，用于确定栅格地图是否被占据；
步骤5.3：以深度相机的光心为起点，扫描所有角度，以光心和扫描到的障碍物间的栅格为
Figure FDA0003020301290000031
表示观测到栅格未被占据，此区域可行，以O表示观测到栅格被占据；
步骤5.4：以E表示障碍物真实存在，
Figure FDA0003020301290000032
表示障碍物不存在，基于式(3)和(4)，得到后验概率：
Figure FDA0003020301290000041
Figure FDA0003020301290000042
其中，p(E)表示先验概率，pjO|E)与
Figure FDA0003020301290000043
表示传感器的测量模型，且
Figure FDA0003020301290000044
步骤5.5：当递推公式被转换为占据栅格结构图时，如式(5)为改进的融合公式：
Figure FDA0003020301290000045
其中，
Figure FDA0003020301290000046
和
Figure FDA0003020301290000047
分别代表先验地图的栅格占据和未被占据单元的先验概率，po表示栅格单元条件概率，
Figure FDA0003020301290000048
代表传感器通过观测确认栅格是否被占据的条件概率；
步骤5.6：通过栅格被占据的概率大小来确定栅格地图的生成，被占据的概率越大生成栅格地图的可能性就越大，设置概率阈值后，若计算出的概率大于设定的阈值则可生成栅格地图，小于设定的概率阈值则不生成，相机生成的栅格地图与雷达生成的栅格地图相融合后，新生成的栅格概率补充占据原有的栅格单元的概率。
8.根据权利要求1或7所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤6包括以下步骤：
步骤6.1：基于步骤5得到的栅格，定义单线激光雷达距离障碍物之间的距离为Distance，单位为米；定义移动机器人与深度相机镜头朝向所在的局部立体空间内障碍物之间的距离为3D_Distance，单位为米；定义移动机器人能够安全移动的情况下周围障碍物距离机器人本体最小距离为Min_Distance；
步骤6.2：根据障碍物与移动机器人之间的平面距离Distance和局部立体空间内障碍物之间的距离3D_Distance采用经典的动态避障算法进行机器人避障。
9.根据权利要求8所述的一种深度相机与单线激光雷达融合的移动机器人避障方法，其特征在于：所述步骤6.2中，根据障碍物与移动机器人之间的平面距离Distance和局部立体空间内障碍物之间的距离3D_Distance将融合后的区域分为：
安全区域：机器人前方无障碍或是障碍物不影响移动机器人正常行走；
障碍区域：机器人前方有障碍或障碍物影响移动机器人正常行走；
碰撞区域：机器人前方有障碍且障碍物影响移动机器人正常行走。
CN202110401044.5A 2021-04-14 2021-04-14 一种深度相机与单线激光雷达融合的移动机器人避障方法 Active CN113110451B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110401044.5A CN113110451B (zh) 2021-04-14 2021-04-14 一种深度相机与单线激光雷达融合的移动机器人避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110401044.5A CN113110451B (zh) 2021-04-14 2021-04-14 一种深度相机与单线激光雷达融合的移动机器人避障方法

Publications (2)

Publication Number Publication Date
CN113110451A true CN113110451A (zh) 2021-07-13
CN113110451B CN113110451B (zh) 2023-03-14

Family

ID=76716922

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110401044.5A Active CN113110451B (zh) 2021-04-14 2021-04-14 一种深度相机与单线激光雷达融合的移动机器人避障方法

Country Status (1)

Country Link
CN (1) CN113110451B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113610910A (zh) * 2021-07-30 2021-11-05 合肥科大智能机器人技术有限公司 一种移动机器人避障方法
CN113689502A (zh) * 2021-09-01 2021-11-23 南京信息工程大学 一种多信息融合的障碍物测量方法
CN115376109A (zh) * 2022-10-25 2022-11-22 杭州华橙软件技术有限公司 障碍物检测方法、障碍物检测装置以及存储介质
CN115436964A (zh) * 2022-11-09 2022-12-06 深圳市壹站智汇科技有限公司 物体识别和路径规划方法、搬运车、电子设备及存储介质
CN116385489A (zh) * 2023-04-04 2023-07-04 森丽康科技(北京)有限公司 基于rgbd相机的目标跟随方法及系统
CN116681746A (zh) * 2022-12-29 2023-09-01 广东美的白色家电技术创新中心有限公司 深度图像的确定方法和装置

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106919908A (zh) * 2017-02-10 2017-07-04 百度在线网络技术（北京）有限公司 障碍物识别方法及装置、计算机设备及可读介质
CN109947097A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于视觉和激光融合的机器人定位方法及导航应用
CN110794406A (zh) * 2019-11-12 2020-02-14 北京经纬恒润科技有限公司 多源传感器数据融合系统和方法
CN110929692A (zh) * 2019-12-11 2020-03-27 中国科学院长春光学精密机械与物理研究所 一种基于多传感器信息融合的三维目标检测方法及装置
CN111028285A (zh) * 2019-12-03 2020-04-17 浙江大学 基于双目视觉和激光雷达融合的深度估计方法
CN111291708A (zh) * 2020-02-25 2020-06-16 华南理工大学 融合深度相机的变电站巡检机器人障碍物检测识别方法
CN111580130A (zh) * 2020-05-25 2020-08-25 中国计量大学 一种基于多传感器融合的建图方法
CN112233148A (zh) * 2020-09-14 2021-01-15 浙江大华技术股份有限公司 目标运动的估计方法、设备及计算机存储介质
CN212683967U (zh) * 2020-05-22 2021-03-12 山东省科学院自动化研究所 一种自主移动机器人控制系统及机器人
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106919908A (zh) * 2017-02-10 2017-07-04 百度在线网络技术（北京）有限公司 障碍物识别方法及装置、计算机设备及可读介质
CN109947097A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于视觉和激光融合的机器人定位方法及导航应用
CN110794406A (zh) * 2019-11-12 2020-02-14 北京经纬恒润科技有限公司 多源传感器数据融合系统和方法
CN111028285A (zh) * 2019-12-03 2020-04-17 浙江大学 基于双目视觉和激光雷达融合的深度估计方法
CN110929692A (zh) * 2019-12-11 2020-03-27 中国科学院长春光学精密机械与物理研究所 一种基于多传感器信息融合的三维目标检测方法及装置
CN111291708A (zh) * 2020-02-25 2020-06-16 华南理工大学 融合深度相机的变电站巡检机器人障碍物检测识别方法
CN212683967U (zh) * 2020-05-22 2021-03-12 山东省科学院自动化研究所 一种自主移动机器人控制系统及机器人
CN111580130A (zh) * 2020-05-25 2020-08-25 中国计量大学 一种基于多传感器融合的建图方法
CN112233148A (zh) * 2020-09-14 2021-01-15 浙江大华技术股份有限公司 目标运动的估计方法、设备及计算机存储介质
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
HARYONG SONG;WONSUB CHOI;HAEDONG KIM: "Robust Vision-Based Relative-Localization Approach Using an RGB-Depth Camera and LiDAR Sensor Fusion", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》 *
JING LI；XIN ZHANG；JIEHAO LI；YANYU LIU；JUNZHENG WANG: "Building and optimization of 3D semantic map based on Lidar and camera fusion", 《NEUROCOMPUTING》 *
NAGHAM SHALAL；TOBIAS LOW: "Orchard mapping and mobile robot localisation using on-board camera and laser scanner data fusion – Part A: Tree detection", 《COMPUTERS AND ELECTRONICS IN AGRICULTURE》 *
张毅，杜凡宇，罗元，熊艳: "一种融合激光和深度视觉传感器的SLAM 地图创建方法", 《计算机应用研究》 *
徐颖蕾: "基于多传感器融合的服务机器人室内导航系统研究", 《中国优秀硕士学位论文全文数据库•信息科技辑》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113610910A (zh) * 2021-07-30 2021-11-05 合肥科大智能机器人技术有限公司 一种移动机器人避障方法
CN113610910B (zh) * 2021-07-30 2024-04-09 合肥科大智能机器人技术有限公司 一种移动机器人避障方法
CN113689502A (zh) * 2021-09-01 2021-11-23 南京信息工程大学 一种多信息融合的障碍物测量方法
CN113689502B (zh) * 2021-09-01 2023-06-30 南京信息工程大学 一种多信息融合的障碍物测量方法
CN115376109A (zh) * 2022-10-25 2022-11-22 杭州华橙软件技术有限公司 障碍物检测方法、障碍物检测装置以及存储介质
CN115376109B (zh) * 2022-10-25 2023-03-24 杭州华橙软件技术有限公司 障碍物检测方法、障碍物检测装置以及存储介质
CN115436964A (zh) * 2022-11-09 2022-12-06 深圳市壹站智汇科技有限公司 物体识别和路径规划方法、搬运车、电子设备及存储介质
CN116681746A (zh) * 2022-12-29 2023-09-01 广东美的白色家电技术创新中心有限公司 深度图像的确定方法和装置
CN116681746B (zh) * 2022-12-29 2024-02-09 广东美的白色家电技术创新中心有限公司 深度图像的确定方法和装置
CN116385489A (zh) * 2023-04-04 2023-07-04 森丽康科技(北京)有限公司 基于rgbd相机的目标跟随方法及系统
CN116385489B (zh) * 2023-04-04 2023-10-03 森丽康科技(北京)有限公司 基于rgbd相机的目标跟随方法及系统

Also Published As

Publication number Publication date
CN113110451B (zh) 2023-03-14

Similar Documents

Publication Publication Date Title
CN113110451B (zh) 一种深度相机与单线激光雷达融合的移动机器人避障方法
US10354151B2 (en) Method of detecting obstacle around vehicle
CN111126269B (zh) 三维目标检测方法、装置以及存储介质
AU2015234395B2 (en) Real-time range map generation
JP5157067B2 (ja) 自動走行用マップ作成装置、及び自動走行装置。
EP2209091B1 (en) System and method for object motion detection based on multiple 3D warping and vehicle equipped with such system
CN100494900C (zh) 适用于自主移动车辆的环境感知的单目视觉导航方法
JP4676373B2 (ja) 周辺認識装置、周辺認識方法、プログラム
WO2018020954A1 (ja) 機械学習用データベース作成システム
EP2256690B1 (en) Object motion detection system based on combining 3D warping techniques and a proper object motion detection
KR20180055292A (ko) 다중 라이다 좌표계 통합 방법
WO2019202628A1 (ja) 路面検出装置、路面検出装置を利用した画像表示装置、路面検出装置を利用した障害物検知装置、路面検出方法、路面検出方法を利用した画像表示方法、および路面検出方法を利用した障害物検知方法
Pantilie et al. Real-time obstacle detection using dense stereo vision and dense optical flow
CN106569225A (zh) 一种基于测距传感器的无人车实时避障方法
CN114724110A (zh) 目标检测方法及设备
Kumar et al. Sensor fusion of laser and stereo vision camera for depth estimation and obstacle avoidance
WO2023070113A1 (en) Validating an sfm map using lidar point clouds
CN112068152A (zh) 使用3d扫描仪同时进行2d定位和2d地图创建的方法和系统
CN109895697B (zh) 一种行车辅助提示系统及方法
Badino et al. Stereo-based free space computation in complex traffic scenarios
CN111273316B (zh) 一种基于剖面展开融合的多激光雷达多视点对象检测方法
Pfeiffer et al. Ground truth evaluation of the Stixel representation using laser scanners
CN113610910B (zh) 一种移动机器人避障方法
CN114661051A (zh) 一种基于rgb-d的前置避障系统
CN117331093B (zh) 一种基于铲斗位置剔除的无人驾驶装载机障碍物感知方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant