CN112859859B - 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 - Google Patents
一种基于三维障碍物体素对象映射的动态栅格地图更新方法 Download PDFInfo
- Publication number
- CN112859859B CN112859859B CN202110042272.8A CN202110042272A CN112859859B CN 112859859 B CN112859859 B CN 112859859B CN 202110042272 A CN202110042272 A CN 202110042272A CN 112859859 B CN112859859 B CN 112859859B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- point cloud
- dynamic
- dimensional
- robot
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000013507 mapping Methods 0.000 title claims abstract description 30
- 230000003068 static effect Effects 0.000 claims abstract description 29
- 230000004888 barrier function Effects 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 7
- 230000004927 fusion Effects 0.000 claims description 6
- 230000033001 locomotion Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 2
- 238000000926 separation method Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Optics & Photonics (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明提供了一种基于三维障碍物体素对象映射的动态栅格地图更新方法，该方法通过加载已有的初始静态场景的三维体素地图；机器人本地端上传实时激光数据及位姿信息，服务器节点建立三维场景下不同障碍物对象的体素地图；服务器节点根据不同机器人的形状大小等避障规则发布包含速度、坐标信息的不同的动态障碍物对象的栅格地图；同时服务器节点对障碍物激光点云数据进行补全，并对预测点云的可靠性进行评价和迭代更新，完成基于障碍物对象的动态概率栅格地图映射；机器人本地端订阅自己的障碍物地图并完成实施的路径规划和避障。该方法可以使机器人更充分的利用三维场景下的点云信息，并实现不同机器人的障碍物信息共享。
Description
技术领域
本发明属于机器人地图构建领域，涉及一种基于多线激光雷达及动态二维栅格地图的导航与避障技术，特别提及了一种基于三维障碍物体素对象映射的动态栅格地图更新方法。
背景技术
随着工业自动化以及机器学习技术的不断发展，移动机器人技术在工业、安防、服务业等领域的应用也越来越广泛。不同类型的移动机器人可以在工作空间内协同工作，代替人完成一些复杂，枯燥，重复，危险的任务，这对于将人类从复杂繁重的劳动中解放出来，提高工作和学习效率具有重要意义。室内移动机器人避障是移动机器人领域中研究和应用最为广泛的领域之一。面对动态变化的环境，移动机器人需要解决的核心问题是能够准确获取到环境中的障碍物信息，并且在导航过程中对于动态变化的障碍物做出实时的响应。
对于移动机器人来说，其最基本也最重要的功能就是路径规划和避障，即找到从起点到终点具有避障能力的最优或次优路径。关于路径规划和动态避障的工作，一直以来都是机器人学界关注的对象。很多针对室内环境下机器人的导航和避障工作，都是直接在二维地图下做的。在全局的静态二维地图下完成全局路径规划，然后通过激光雷达数据，图像信息，以及一些辅助信息如距离传感器和碰撞传感器数据，将传感器检测到的动态障碍物信息动态添加到二维地图中，进行局部路径规划绕过障碍物。由于传感器本身的特点，机器人只能获得障碍物在某一个视角上的数据，无法获得障碍物的完整轮廓信息，也无法像人一样凭借已观察到的数据和自己的经验对未观察到的部分进行预测。机器人需要移动一定的距离才能感知到障碍物更多的局部信息，这对于移动机器人的实时避障是不利的。因此，也有一些基于场景理解和障碍物语义信息的避障方案被提出来，取得了相对较好的效果，但对于室内移动机器人场景，也无法获得更加精确的障碍物局部信息来预测辅助避障。
现实世界是三维的，三维环境地图是描述真实三维世界最直接的方法，对于移动机器人的自主导航和避障是非常有效的。而使用三维点云地图进行三维场景下的路径规划和避障具有很高的时间复杂度和空间复杂度，不利于实现实时的避障。在室内环境下通常也无法获取到有效的GPS信号，这对于三维场景下机器人的定位也是一个很大的挑战。为了更好的利用三维场景下的信息，基于以上背景，较为理想的做法是将机器人传感器获取到的实时数据在三维场景下进行融合，同时根据不同机器人的具体参数将动态障碍物信息实时映射到一个动态的二维栅格地图上，扬长避短，由具体的机器人完成二维栅格地图下的导航和避障。
因此，亟需设计一种基于三维障碍物体素对象映射的动态栅格地图更新方法，以提高栅格地图中障碍物信息的完整性，并提高机器人在应用具体的路径规划算法完成动态避障的稳定性。
发明内容
(一)要解决的技术问题
本发明的主要目的在于，提出一种基于三维障碍物体素对象映射的动态栅格地图更新方法，其属于新的二维障碍物栅格地图的动态更新方法，该方法能够基于训练好的点云特征补全网络PF-Net和已观测到的障碍物激光点云数据进行障碍物的特征补全，并对预测点云的可靠性进行评价和迭代更新，完成基于障碍物对象的动态概率栅格地图映射，能够大大提高栅格地图中障碍物信息的完整性，使机器人在应用具体的路径规划算法完成动态避障的稳定性可靠性大大提高。
(二)技术方案
本发明公开了一种基于三维障碍物体素对象映射的动态栅格地图更新方法，该方法包括以下步骤：
步骤一：定义世界坐标系、机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系，定义机器人障碍物地图的更新系统框架；
步骤二：初始化与实际场景对应的全局静态三维体素地图，并生成针对不同机器人的二维静态栅格地图，下发到具体的机器人中；
所述全局静态三维体素地图为固定场景下不包含动态障碍物的初始三维体素地图，不包含动态障碍物；同时所有的机器人本地端都有对应全局静态三维体素地图的二维静态栅格地图，所述二维静态栅格地图描述如下：假设本地机器人的二维地图是由M行N列的栅格组成，则本地栅格地图可以用一个二维矩阵Map(M,N)表示：
其中更新的动态障碍物地图表示为map(m,n)，其中m＜M,n＜N，ρ为栅格map(m,n)处存在障碍物的概率，在动态避障时服务器计算节点会根据三维场景下的障碍物数据计算出对应动态障碍物的二维栅格地图。
步骤三：提取激光雷达数据中的前景信息，针对其中动态障碍物点云数据，基于FastSLAM方法与上一帧点云数据融合，得到更加完整的障碍物点云对象；
步骤四：由步骤三得到的动态障碍物点云对象映射到动态障碍物栅格地图，并下发到具体机器人中，同时将点云输入到点云修补网络进行点云补全；
由动态障碍物点云对象映射到动态障碍物栅格地图的描述如下：由于基于激光数据直接观测并计算得到的障碍物对象具有较高的可信度，映射得到的二维栅格地图中对应栅格的障碍物概率ρ为1，对于预测得到的障碍物局部数据以及其它机器人检测到的动态障碍物，其对应映射得到的二维栅格地图中对应栅格的障碍物概率ρ为满足0<ρ<1的具体概率值，对于得到的具体动态障碍物二维栅格地图，由服务器计算节点实时推送到对应的机器人，同时将所述步骤三中的动态障碍物点云对象输入到点云补全网络中进行具体障碍物对象的点云数据增强；
步骤五：如果有最新的点云修补结果，则计算本次点云补全的可信度，并得到对应的点云置信度系数，然后计算预测点云集对应在二维栅格地图上的障碍物概率，与步骤三的障碍物栅格地图一同下发到具体机器人中；
步骤六：由具体的机器人基于叠加后的二维栅格地图，使用A*、RRT、人工势场的二维导航和避障方式进行动态二维栅格地图下的导航与避障；
步骤七：重复执行步骤三到步骤六，直至到达目标点，完成动态的导航和避障。
进一步的，在步骤一中，世界坐标系为固定坐标系，机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系等其余坐标系下的数据可以通过已知的坐标变换矩阵变换为世界坐标系下的绝对坐标；所述机器人障碍物地图更新系统具体描述为：
a.服务器计算节点：
服务器计算节点负责实时接收每个机器人上传的激光雷达数据，处理之后将障碍物点云转换到全局静态三维体素地图的世界坐标系下，服务器计算节点会根据不同机器人的物理尺寸、运动特性等参数生成对应不同障碍物的动态栅格地图，服务器节点作为机器人系统的ROS master节点，发布针对不同机器人的动态地图话题；
b.机器人本地端：
机器人本地端通过订阅服务器节点发布的动态栅格地图话题获取具体障碍物的动态二维栅格地图，同时机器人本地端会发布实时的激光传感器数据，服务器计算节点可以订阅到不同机器人的传感器数据，然后机器人本地端根据本地全局静态栅格地图与服务器计算节点的动态障碍物栅格地图进行实时的导航和避障。
进一步的，步骤三中，由服务器计算节点提取激光雷达数据中具体障碍物的前景点云数据，然后根据具体障碍物的点云数据，基于FastSLAM方法完成障碍物对象点云构建和融合，通过融合上一帧障碍物的激光点云数据，得到具体障碍物对象更加完整的点云数据。
进一步的，步骤五中，计算预测点云对象的可信度以及对应在二维栅格地图上的障碍物概率，具体步骤描述如下：
a.根据步骤三中得到动态障碍物点云，将其表示为一个独立坐标系下的点云对象R，选取第一次融合时R的中心点作为障碍物坐标系的原点，建立障碍物坐标系Of到大地坐标系Od间的坐标转换矩阵Rf，在Of坐标系下障碍物点云R表示为：
其中，xi、yi、zi为障碍物坐标系下点云对象的XYZ三维空间坐标；
b.将点云R映射到二维障碍物栅格地图上，栅格中的障碍物概率ρ表示为：
在动态障碍物检测的过程中，栅格地图中某个栅格存在障碍物的可信度υ按以下公式进行迭代；
其中υ值越大，表示则该处存在障碍物的概率ρ就接近1，e表示某次观测或预测到的点云中映射到的栅格是否有障碍物，如果有则为1，否则为0；
c.由于激光雷达观测到的障碍物点云集R具有较高的可信度，其相应的障碍物的可信度υ的具有相对较大的初始值；为了更好的描述点云修补算法得到的预测点云对于机器人避障的可信度，引入了一个置信度系数ζ，假设第k-1时刻基于观测到的障碍物点集R得到了修补后的障碍物点云Rk，使用k时刻障碍物点云集R中新增加的点云Fk与预测点云Rk的契合程度来评价点云补全的可信度：
其中Γ表示点云集Fk中的点px到预测点云集中离px最近的点的距离的标准差，由于标准差Γ的值域为[0,+∞)，其值越大预测点云的可信度越差；为了更好的描述标准差Γ对点云置信系数的影响，置信系数ζ表示为：
其中λ为置信度权值，为一个常数；随着机器人的运动，观测到的障碍物点云集R会越来越完整，点云补全的可靠度ζ也会越来越高；
d.将预测点云集对应的二维栅格地图的障碍物概率转化为值域为[0,1]的概率值，表示为：
进一步的，步骤七中，所述服务器计算节点中不同的动态障碍物对象是彼此互相独立的，机器人移动过程中，动态障碍物超出机器人所能检测到的最远距离之后，会无法被检测到，此时将对应的动态障碍物对象进行屏蔽或直接进行删除。
在另外一方面，本发明公开了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如上述任一项所述的基于三维障碍物体素对象映射的动态栅格地图更新方法。
(三)有益效果
本发明的有益效果在于，与现有技术相比，针对室内场景下的移动机器人避障，本发明采用基于实时动态三维体素地图的二维栅格地图动态更新方法，由服务器节点根据每个机器人激光传感器数据进行对象级的障碍物点云构建，并可以根据不同机器人的形状、大小、运行规则等参数生成具体的二维动态障碍物栅格地图，实时更新到本地机器人中；同时服务器节点还可以根据已有的障碍物点云数据进行障碍物点云的补全，使机器人可以根据有限的激光点云信息对障碍物整体的形状大小进行预测，获得更好的避障响应效果。另外，针对多机器人的场景，基于本发明的障碍物地图更新方法能够实现不同机器人的障碍物信息共享，能够显著提升机器人路径规划和避障的智能化水平。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例所需要使用的附图进行简单地介绍：
图1为本发明的机器人障碍物地图更新流程示意图；
图2为本发明基于三维障碍物体素对象映射的动态栅格地图更新方法的流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，在一个具体实施方式中，提出了一种室内场景下动态障碍物栅格地图更新方案，包括服务器计算节点，自由移动的机器人，无线组网基站等。其中服务器计算节点负责收集每个机器人的运行姿态及传感器数据，进行前景障碍物分离，障碍物对象点云构建，点云特征补全，障碍物栅格地图的实时更新。移动机器人主要装配有多线激光雷达，深度视觉相机，编码里程计，陀螺仪，测距传感器，碰撞传感器等基本传感器设备，能够实现基于粒子滤波的定位和基于航迹推算导航和基于栅格地图的导航与避障。
所述实施方式在细节上不局限与文中所提的具体方法，如在室外开放场景中，所述可以获得其它如GPS等更富的传感器数据，本发明所述地图更新方法同样可以适用。在多机器人场景中，为了避免大量传感器数据造成网络拥塞，使地图更新的实时性下降，激光数据的前景数据分离可以由本地机器人完成，只上传障碍物的特征激光数据点。
所述具体实施方式中，机器人本地端和服务器计算节点是基于ROS机器人操作系统实现的，传感器信息，坐标变换以及动态地图的构建、更新与传输都是基于ROS的话题机制实现，但本发明所述地图更新方法不局限于基于ROS框架的实现。
在图1的室内场景下动态障碍物栅格地图更新方案的基础上，如图2所示，本发明提出了一种基于三维障碍物体素对象映射的动态栅格地图更新方法，该方法由原始多线激光雷达数据得到三维障碍物体素对象并进行点云信息增强，然后生成具体的二维障碍物栅格地图并实时下发到具体的机器人；所述基于三维障碍物体素对象映射的动态栅格地图更新方法具体包含如下步骤一～步骤七：
步骤一：定义世界坐标系、机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系，定义机器人障碍物地图的更新系统框架；
进一步的，在步骤一中，世界坐标系为固定坐标系，机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系等其余坐标系下的数据可以通过已知的坐标变换矩阵变换为世界坐标系下的绝对坐标；所述机器人障碍物地图更新系统具体描述为：
a.服务器计算节点：
服务器计算节点负责实时接收每个机器人上传的激光雷达数据，处理之后将障碍物点云转换到全局静态三维体素地图的世界坐标系下，服务器计算节点会根据不同机器人的物理尺寸、运动特性等参数生成对应不同障碍物的动态栅格地图，服务器节点作为机器人系统的ROS master节点，发布针对不同机器人的动态地图话题；
b.机器人本地端：
机器人本地端通过订阅服务器节点发布的动态栅格地图话题获取具体障碍物的动态二维栅格地图，同时机器人本地端会发布实时的激光传感器数据，服务器计算节点可以订阅到不同机器人的传感器数据，然后机器人本地端根据本地全局静态栅格地图与服务器计算节点的动态障碍物栅格地图进行实时的导航和避障。
正如上文所述，该地图更新方法不局限于上述框架的实现，其也可以应用到非ROS的框架中。
步骤二：初始化与实际场景对应的全局静态三维体素地图，并生成针对不同机器人的二维静态栅格地图，下发到具体的机器人中；
进一步的，步骤二中，服务器计算节点保存有实际场景对应的全局静态三维体素地图，全局静态三维体素地图为固定场景下不包含动态障碍物的初始三维体素地图，不包含动态障碍物；同时所有的机器人本地端都有对应全局静态三维体素地图的二维静态栅格地图，所述二维静态栅格地图描述如下：
假设本地机器人的二维地图是由M行N列的栅格组成，则本地栅格地图可以用一个二维矩阵Map(M,N)表示：
其中由服务端更新的动态障碍物地图表示为map(m,n)，其中m＜M,n＜N，ρ为栅格map(m,n)处存在障碍物的概率，在动态避障时服务器计算节点会根据三维场景下的障碍物数据计算出对应动态障碍物的二维栅格地图。
步骤三：提取激光雷达数据中的前景信息，针对其中动态障碍物点云数据，基于FastSLAM方法与上一帧点云数据融合，得到更加完整的障碍物点云对象。
进一步的，步骤三中，由服务器计算节点提取激光雷达数据中具体障碍物的前景点云数据，然后根据具体障碍物的点云数据，基于FastSLAM方法完成障碍物对象点云构建和融合，通过融合上一帧障碍物的激光点云数据，得到具体障碍物对象更加完整的点云数据。
步骤四：由步骤三得到的动态障碍物点云对象映射到动态障碍物栅格地图，并下发到具体机器人中，同时将点云输入到点云修补网络进行点云补全。
进一步的，步骤四中，由动态障碍物点云对象映射到动态障碍物栅格地图的描述如下：由于基于激光数据直接观测并计算得到的障碍物对象具有较高的可信度，映射得到的二维栅格地图中对应栅格的障碍物概率ρ为1，对于预测得到的障碍物局部数据以及其它机器人检测到的动态障碍物，其对应映射得到的二维栅格地图中对应栅格的障碍物概率ρ为满足0<ρ<1的具体概率值，对于得到的具体动态障碍物二维栅格地图，由服务器计算节点实时推送到对应的机器人，同时将所述步骤三中的动态障碍物点云对象输入到点云补全网络中进行具体障碍物对象的点云数据增强。
步骤五：如果有最新的点云修补结果，则计算本次点云补全的可信度，并得到对应的点云置信度系数，然后计算预测点云集对应在二维栅格地图上的障碍物概率，与步骤三的障碍物栅格地图一同下发到具体机器人中。
进一步的，步骤五中，计算预测点云对象的可信度以及对应在二维栅格地图上的障碍物概率，具体步骤描述如下：
a.根据步骤三中得到动态障碍物点云，将其表示为一个独立坐标系下的点云对象R，选取第一次融合时R的中心点作为障碍物坐标系的原点，建立障碍物坐标系Of到大地坐标系Od间的坐标转换矩阵Rf，在Of坐标系下障碍物点云R表示为：
其中，xi、yi、zi为障碍物坐标系下点云对象的三维空间坐标；
b.将点云R映射到二维障碍物栅格地图上，栅格中的障碍物概率ρ表示为：
在动态障碍物检测的过程中，栅格地图中某个栅格存在障碍物的可信度υ按以下公式进行迭代；
其中υ(υ≥0)值越大，表示则该处存在障碍物的概率ρ就接近1，e表示某次观测或预测到的点云中映射到的栅格是否有障碍物，如果有则为1，否则为0；
c.由于激光雷达观测到的障碍物点云集R具有较高的可信度，其相应的障碍物的可信度υ的具有相对较大的初始值；为了更好的描述点云修补算法得到的预测点云对于机器人避障的可信度，引入了一个置信度系数ζ，假设第k-1时刻基于观测到的障碍物点集R得到了修补后的障碍物点云Rk，使用k时刻障碍物点云集R中新增加的点云Fk与预测点云Rk的契合程度来评价点云补全的可信度：
其中Γ表示点云集Fk中的点px到预测点云集中离px最近的点的距离的标准差，由于标准差Γ的值域为[0,+∞)，其值越大预测点云的可信度越差；为了更好的描述标准差Γ对点云置信系数的影响，置信系数ζ表示为：
其中λ为置信度权值，为一个常数；随着机器人的运动，观测到的障碍物点云集R会越来越完整，点云补全的可靠度ζ也会越来越高；
d.将预测点云集对应的二维栅格地图的障碍物概率转化为值域为[0,1]的概率值，表示为：
步骤六：由具体的机器人基于叠加后的二维栅格地图，使用A*、RRT、人工势场等的常规二维导航和避障方案进行动态二维栅格地图下的导航与避障；
步骤七：重复执行步骤三到步骤六，直至到达目标点，完成动态的导航和避障。
进一步的，步骤七中，所述服务器计算节点中不同的动态障碍物对象是彼此互相独立的，机器人移动过程中，动态障碍物超出机器人所能检测到的最远距离之后，会无法被检测到，此时将对应的动态障碍物对象进行屏蔽或直接进行删除。
需要指出的是，在本发明各个实施例中的地图更新方法中各步骤对应的软件功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
最后说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (5)
1.一种基于三维障碍物体素对象映射的动态栅格地图更新方法，其特征在于，该方法包括以下步骤：
步骤一：定义世界坐标系、机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系，定义机器人障碍物地图的更新系统框架；
步骤二：初始化与实际场景对应的全局静态三维体素地图，并生成针对不同机器人的二维静态栅格地图，下发到具体的机器人中；
所述全局静态三维体素地图为固定场景下不包含动态障碍物的初始三维体素地图；同时所有的机器人本地端都有对应全局静态三维体素地图的二维静态栅格地图，所述二维静态栅格地图描述如下：假设本地机器人的二维地图是由M行N列的栅格组成，则本地栅格地图可以用一个二维矩阵Map(p,q)表示：
其中更新的动态障碍物地图表示为Map(p,q)，其中p＜M,q＜N，ρ为栅格Map(p,q)处存在障碍物的概率，在动态避障时服务器计算节点会根据三维场景下的障碍物数据计算出对应动态障碍物的二维栅格地图；
步骤三：提取激光雷达数据中的前景信息，针对其中动态障碍物点云数据，基于FastSLAM方法与上一帧点云数据融合，得到更加完整的障碍物点云对象；
步骤四：由步骤三得到的动态障碍物点云对象映射到动态障碍物栅格地图，并下发到具体机器人中，同时将点云输入到点云修补网络进行点云补全；
由动态障碍物点云对象映射到动态障碍物栅格地图的描述如下：由于基于激光数据直接观测并计算得到的障碍物对象具有较高的可信度，映射得到的二维栅格地图中对应栅格的障碍物概率ρ为1，对于预测得到的障碍物局部数据以及其它机器人检测到的动态障碍物，其对应映射得到的二维栅格地图中对应栅格的障碍物概率ρ为满足0<ρ<1的具体概率值，对于得到的具体动态障碍物二维栅格地图，由服务器计算节点实时推送到对应的机器人，同时将所述步骤三中的动态障碍物点云对象输入到点云补全网络中进行具体障碍物对象的点云数据增强；
步骤五：如果有最新的点云修补结果，则计算本次点云补全的可信度，并得到对应的点云置信度系数，然后计算预测点云集对应在二维栅格地图上的障碍物概率，与步骤三的障碍物栅格地图一同下发到具体机器人中；其中，步骤五中，计算预测点云对象的可信度以及对应在二维栅格地图上的障碍物概率，具体步骤描述如下：
a.根据步骤三中得到动态障碍物点云，将其表示为一个独立坐标系下的点云对象R，选取第一次融合时R的中心点作为障碍物坐标系的原点，建立障碍物坐标系Of到大地坐标系Od间的坐标转换矩阵Rf，在Of坐标系下障碍物点云R表示为：
其中，xi、yi、zi为障碍物坐标系下点云对象的XYZ三维空间坐标；
b.将点云R映射到二维障碍物栅格地图上，栅格中的障碍物概率ρ表示为：
在动态障碍物检测的过程中，栅格地图中某个栅格存在障碍物的可信度υ按以下公式进行迭代；
其中υ值越大，表示则该处存在障碍物的概率ρ就接近1，e表示某次观测或预测到的点云中映射到的栅格是否有障碍物，如果有则为1，否则为0；
c.由于激光雷达观测到的障碍物点云集R具有较高的可信度，其相应的障碍物的可信度υ的具有相对较大的初始值；为了更好的描述点云修补算法得到的预测点云对于机器人避障的可信度，引入了一个置信度系数ζ，假设第k-1时刻基于观测到的障碍物点集R得到了修补后的障碍物点云Rk，使用k时刻障碍物点云集R中新增加的点云Fk与预测点云Rk的契合程度来评价点云补全的可信度：
其中Γ表示点云集Fk中的点px到预测点云集中离px最近的点的距离的标准差，由于标准差Γ的值域为[0,+∞)，其值越大预测点云的可信度越差；为了更好的描述标准差Γ对点云置信系数的影响，置信度系数ζ表示为：
其中λ为置信度权值，为一个常数；随着机器人的运动，观测到的障碍物点云集R会越来越完整，点云补全的置信度系数ζ也会越来越高；
d.将预测点云集对应的二维栅格地图的障碍物概率转化为值域为[0,1]的概率值，表示为：
步骤六：由具体的机器人基于叠加后的二维栅格地图，使用A*、RRT或人工势场的二维导航和避障方式进行动态二维栅格地图下的导航与避障；
步骤七：重复执行步骤三到步骤六，直至到达目标点，完成动态的导航和避障。
2.根据权利要求1所述的动态栅格地图更新方法，其特征在于，在步骤一中，世界坐标系为固定坐标系，机器人本体坐标系、动态障碍物坐标系以及各传感器坐标系等其余坐标系下的数据可以通过已知的坐标变换矩阵变换为世界坐标系下的绝对坐标；所述机器人障碍物地图更新系统具体描述为：
a.服务器计算节点：
服务器计算节点负责实时接收每个机器人上传的激光雷达数据，处理之后将障碍物点云转换到全局静态三维体素地图的世界坐标系下，服务器计算节点会根据不同机器人的物理尺寸、运动特性等参数生成对应不同障碍物的动态栅格地图，服务器节点作为机器人系统的ROS master节点，发布针对不同机器人的动态地图话题；
b.机器人本地端：
机器人本地端通过订阅服务器节点发布的动态栅格地图话题获取具体障碍物的动态二维栅格地图，同时机器人本地端会发布实时的激光传感器数据，服务器计算节点可以订阅到不同机器人的传感器数据，然后机器人本地端根据本地全局静态栅格地图与服务器计算节点的动态障碍物栅格地图进行实时的导航和避障。
3.根据权利要求1所述的动态栅格地图更新方法，其特征在于，步骤三中，由服务器计算节点提取激光雷达数据中具体障碍物的前景点云数据，然后根据具体障碍物的点云数据，基于FastSLAM方法完成障碍物对象点云构建和融合，通过融合上一帧障碍物的激光点云数据，得到具体障碍物对象更加完整的点云数据。
4.根据权利要求1所述的动态栅格地图更新方法，其特征在于，步骤七中，所述服务器计算节点中不同的动态障碍物对象是彼此互相独立的，机器人移动过程中，动态障碍物超出机器人所能检测到的最远距离之后，会无法被检测到，此时将对应的动态障碍物对象进行屏蔽或直接进行删除。
5.一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至4任一项所述的动态栅格地图更新方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110042272.8A CN112859859B (zh) | 2021-01-13 | 2021-01-13 | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110042272.8A CN112859859B (zh) | 2021-01-13 | 2021-01-13 | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112859859A CN112859859A (zh) | 2021-05-28 |
| CN112859859B true CN112859859B (zh) | 2022-04-22 |
Family
ID=76003344
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110042272.8A Active CN112859859B (zh) | 2021-01-13 | 2021-01-13 | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112859859B (zh) |
Families Citing this family (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP7455704B2 (ja) | 2020-08-28 | 2024-03-26 | 株式会社日立製作所 | 移動体制御システムおよび移動体制御方法 |
| CN113640822B (zh) * | 2021-07-07 | 2023-08-18 | 华南理工大学 | 一种基于非地图元素过滤的高精度地图构建方法 |
| CN113520246B (zh) * | 2021-07-30 | 2023-04-04 | 珠海一微半导体股份有限公司 | 移动机器人补偿清洁方法及系统 |
| CN113670292B (zh) * | 2021-08-10 | 2023-10-20 | 追觅创新科技(苏州)有限公司 | 地图的绘制方法和装置、扫地机、存储介质、电子装置 |
| CN113741438B (zh) * | 2021-08-20 | 2024-03-26 | 上海高仙自动化科技发展有限公司 | 路径规划方法、装置、存储介质、芯片及机器人 |
| CN113467483B (zh) * | 2021-08-23 | 2022-07-26 | 中国人民解放军国防科技大学 | 动态环境中基于时空栅格地图的局部路径规划方法及装置 |
| CN113703001A (zh) * | 2021-08-30 | 2021-11-26 | 上海景吾智能科技有限公司 | 一种在机器人已有地图上生成障碍物的方法、系统及介质 |
| CN114353779B (zh) * | 2021-09-30 | 2024-05-10 | 南京晨光集团有限责任公司 | 一种采用点云投影快速更新机器人局部代价地图的方法 |
| CN116027341B (zh) * | 2021-10-25 | 2024-05-03 | 珠海一微半导体股份有限公司 | 基于激光观测方向的栅格和体素定位方法、机器人及芯片 |
| CN114115231B (zh) * | 2021-10-25 | 2023-07-25 | 南京工业大学 | 移动机器人空间位姿点云校正方法及系统 |
| CN114044369B (zh) * | 2021-11-05 | 2023-04-11 | 江苏昱博自动化设备有限公司 | 一种基于自适应巡航技术的码垛机械手的控制方法 |
| CN114415661B (zh) * | 2021-12-15 | 2023-09-22 | 中国农业大学 | 基于压缩三维空间点云的平面激光slam与导航方法 |
| CN114384920B (zh) | 2022-03-23 | 2022-06-10 | 安徽大学 | 一种基于局部栅格地图实时构建的动态避障方法 |
| CN115143951A (zh) * | 2022-05-31 | 2022-10-04 | 深圳市普渡科技有限公司 | 栅格地图更新系统、方法、计算机设备及存储介质 |
| CN114821543B (zh) * | 2022-06-29 | 2022-10-18 | 小米汽车科技有限公司 | 障碍物检测方法、装置、车辆和存储介质 |
| CN115376109B (zh) * | 2022-10-25 | 2023-03-24 | 杭州华橙软件技术有限公司 | 障碍物检测方法、障碍物检测装置以及存储介质 |
| CN115527034B (zh) * | 2022-10-26 | 2023-08-01 | 北京亮道智能汽车技术有限公司 | 一种车端点云动静分割方法、装置及介质 |
| CN117078869A (zh) * | 2022-10-27 | 2023-11-17 | 北京石头创新科技有限公司 | 彩色三维地图显示方法及装置 |
| CN116088503B (zh) * | 2022-12-16 | 2024-06-25 | 深圳市普渡科技有限公司 | 动态障碍物检测方法和机器人 |
| CN116448118B (zh) * | 2023-04-17 | 2023-10-31 | 深圳市华辰信科电子有限公司 | 一种扫地机器人的工作路径优化方法和装置 |
| CN116661468B (zh) * | 2023-08-01 | 2024-04-12 | 深圳市普渡科技有限公司 | 障碍物检测方法、机器人及计算机可读存储介质 |
| CN117146829A (zh) * | 2023-10-30 | 2023-12-01 | 江苏云幕智造科技有限公司 | 基于双目与三维点云的多姿态人形机器人环境导航方法 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108896050B (zh) * | 2018-06-26 | 2021-09-07 | 上海交通大学 | 一种基于激光传感器的移动机器人长期定位系统及方法 |
| CN111381594A (zh) * | 2020-03-09 | 2020-07-07 | 兰剑智能科技股份有限公司 | 基于3d视觉的agv空间避障方法及系统 |
| CN112102151B (zh) * | 2020-07-27 | 2024-05-14 | 广州视源电子科技股份有限公司 | 栅格地图的生成方法、装置、移动智慧设备和存储介质 |
| CN112327851B (zh) * | 2020-11-09 | 2023-08-22 | 达闼机器人股份有限公司 | 基于点云的地图校准方法、系统、机器人及云端平台 |
| CN112526993B (zh) * | 2020-11-30 | 2023-08-08 | 广州视源电子科技股份有限公司 | 栅格地图更新方法、装置、机器人及存储介质 |
| CN112525202A (zh) * | 2020-12-21 | 2021-03-19 | 北京工商大学 | 一种基于多传感器融合的slam定位导航方法及系统 |
| CN112904842B (zh) * | 2021-01-13 | 2022-07-15 | 中南大学 | 一种基于代价势场的移动机器人路径规划与优化方法 |
| CN113093729A (zh) * | 2021-03-10 | 2021-07-09 | 上海工程技术大学 | 一种基于视觉与激光雷达的智能购物小车及控制方法 |
-
2021
- 2021-01-13 CN CN202110042272.8A patent/CN112859859B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112859859A (zh) | 2021-05-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112859859B (zh) | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 | |
| CN112132893B (zh) | 一种适用于室内动态环境的视觉slam方法 | |
| Tang et al. | Geometric A-star algorithm: An improved A-star algorithm for AGV path planning in a port environment | |
| CN108898676B (zh) | 一种虚实物体之间碰撞及遮挡检测方法及系统 | |
| CN109186606B (zh) | 一种基于slam和图像信息的机器人构图及导航方法 | |
| CN109325979B (zh) | 基于深度学习的机器人回环检测方法 | |
| WO2023273169A1 (zh) | 一种融合视觉与激光的2.5d地图构建方法 | |
| Souza et al. | Occupancy-elevation grid: an alternative approach for robotic mapping and navigation | |
| Garrote et al. | 3D point cloud downsampling for 2D indoor scene modelling in mobile robotics | |
| Herath et al. | Fusion-dhl: Wifi, imu, and floorplan fusion for dense history of locations in indoor environments | |
| Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
| CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
| CN113110455A (zh) | 一种未知初始状态的多机器人协同探索方法、装置及系统 | |
| CN106931978B (zh) | 自动构建路网的室内地图生成的方法 | |
| Chen et al. | Multilayer mapping kit for autonomous UAV navigation | |
| Niu et al. | Unmanned aerial vehicle (UAV)-assisted path planning for unmanned ground vehicles (UGVs) via disciplined convex-concave programming | |
| Wu et al. | A non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning | |
| CN112132951B (zh) | 一种基于视觉的网格语义地图的构建方法 | |
| CN112907625A (zh) | 应用于四足仿生机器人的目标跟随方法及系统 | |
| CN116698043A (zh) | 一种室内移动机器人视觉导航方法 | |
| CN111815684A (zh) | 一种基于统一残差模型的空间多元特征配准优化方法及装置 | |
| CN114397894B (zh) | 一种模仿人类记忆的移动机器人目标搜索方法 | |
| CN112419461A (zh) | 一种协同无人系统联合语义建图方法 | |
| Zhang et al. | An improved bicubic interpolation SLAM algorithm based on multi-sensor fusion method for rescue robot | |
| Graichen et al. | Occupancy Grid Map Generation from OSM Indoor Data for Indoor Positioning Applications. |
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 |