CN113776515B - 一种机器人导航方法、装置、计算机设备和存储介质 - Google Patents

一种机器人导航方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN113776515B
CN113776515B CN202111009211.8A CN202111009211A CN113776515B CN 113776515 B CN113776515 B CN 113776515B CN 202111009211 A CN202111009211 A CN 202111009211A CN 113776515 B CN113776515 B CN 113776515B
Authority
CN
China
Prior art keywords
robot
point cloud
map
point
fused
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
Application number
CN202111009211.8A
Other languages
English (en)
Other versions
CN113776515A (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.)
Hainan Hongpu Technology Co.,Ltd.
Original Assignee
Nanchang Institute of Technology
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 Nanchang Institute of Technology filed Critical Nanchang Institute of Technology
Priority to CN202111009211.8A priority Critical patent/CN113776515B/zh
Publication of CN113776515A publication Critical patent/CN113776515A/zh
Application granted granted Critical
Publication of CN113776515B publication Critical patent/CN113776515B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Abstract

本申请公开的一种机器人导航方法、装置、计算机设备和存储介质，该方法包括：获取经由定位设备传输的3D点云数据，定位设备包括激光雷达和双目摄像头；根据激光雷达与双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对3D点云数据进行融合，得到融合点云集合；根据获取到的机器人的RTK定位信息和RTK定位信息，基于所确定的机器人在同步构建地图中的确切位置，对融合点云集合进行更新，得到更新点集合；确定更新点集合与融合点云集合之间的相似程度，在相似程度满足预设判断条件时，将更新点集合添加到同步构建地图中；基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算最优的导航路径，提高定位精度。

Description

一种机器人导航方法、装置、计算机设备和存储介质
技术领域
本发明涉及导航技术领域，更具体地说，涉及一种机器人导航方法、装置、计算机设备和存储介质。
背景技术
机器人是一种帮助人们从有限、重复、高危、高负荷的人力工作中解脱出来的机器，其主要目的是提高人类的工作效率和保障人类的安全性，随着相关技术不断完善，当今机器人早已融入我们的生活的方方面面。然而，对于任何移动设备而言，在其环境中导航的能力都很重要，因此，在实际使用的时候，为了避免诸如碰撞和不安全条件(温度、辐射、暴露于天气等)的危险情况，需要对机器人在环境中的所处的实时位置及时进行导航和定位，以使得机器人能够确定自己在参考系中的位置，然后对通往某个目标位置的路径进行规划。
现有的机器人导航方法包括有激光雷达、视觉导航、2DSLAM和3DSLAM(simultaneous localization and mapping，机器人对应构建的周围环境的2D地图和3D地图的同时，定位它在相应地图中的位置)等，虽然上述方法能够解决机器人的定位和导航问题。但是，由于室外环境的复杂性和易变性，例如，在不同的天气下，会导致光照、可见度以及透光性的变化，在雨天会导致场景出现新的障碍物，比如泥潭。这种情况下，激光雷达的传感信息将极大的减少，视觉导航由于雨淋导致信息不够准确而暂时失效，2DSLAM与3DSLAM由于对泥潭不具备感知性从而容易导致机器人陷入泥潭。这也导致了现有的机器人导航方法无法满足不同天气、不同地形下的导航需求，存在定位精度下降乃至无法导航的问题。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足，针对存在定位精度下降乃至无法导航的技术问题，提供一种机器人导航方法、装置、计算机设备和存储介质。
本发明解决上述技术问题的技术方案如下：一种机器人导航方法，所述方法包括：
S1、获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
S2、根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
S3、获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
S4、根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
S5、确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
S6、基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
一种机器人导航装置，所述装置包括获取模块、融合模块、位置确定模块、集合更新模块、地图更新模块和导航模块，其中：
所述获取模块，用于获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
所述融合模块，用于根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
所述位置确定模块，用于获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
所述集合更新模块，用于根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
所述地图更新模块，用于确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
所述导航模块，用于基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
S1、获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
S2、根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
S3、获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
S4、根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
S5、确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
S6、基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
S1、获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
S2、根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
S3、获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
S4、根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
S5、确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
S6、基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
实施本发明的一种机器人导航方法、装置、计算机设备和存储介质，通过双目摄像头和激光雷达来获得3D点云数据，并自动修正点云的正确性，从而实现复杂环境下为机器人导航的目的，确保机器人导航的准确性稳定性和高鲁棒性。另外，在确保机器人稳定运行的同时，又能对环境进行自我学习与适配，机器人很容易便可以集成复杂的导航系统，只需要机器人有前后左右的运动控制量即可使用本技术方案。最后，本技术方案对机器人的形态、性和功率均无具体要求、适配性高、精度好、且对环境的适应能力好，能够满足不同天气、不同地形下的导航需求，提高了定位精度下降，避免无法导航的情况发生。
附图说明
图1为一个实施例中机器人导航方法的流程示意图；
图2为一个实施例中机器人导航装置的结构示意图。
具体实施方式
为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。
在一个实施例中，如图1所示，提供了的一种基于3D点云和深度学习的导航方法，以该方法应用于机器人控制器(该机器人控制器位于机器人上，包括算法固件和提供算法运行的硬件)为例进行说明，包括以下步骤：
S1、获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头。
具体的，机器人控制器从激光雷达和双目摄像头中，获取3D点云数据。需要说明的是，3D点云数据具体指一个三维坐标系统中的一组向量的集合，其中，点云数据除了具有几何位置以外，有的还具有颜色信息。颜色信息通常是通过相机获取彩色影像，然后将对应位置的像素的颜色信息赋予点云中对应的点。
S2、根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的对距离，对所述3D点云数据进行融合，得到相应的融合点云集合。
具体的，机器人控制器通过对获取到的3D点云数据进行融合，以提高点云信息的可信度。其中，融合的阶段包括粗融合阶段和细融合阶段，且在粗融合阶段之后，点云集的点将都处于临近的空间位置，接下来的细融合阶段则进行点云数据集的点集融合。
S3、获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置。
其中，RTK(Real Time Kinematic，即载波相位差分技术)定位信息指的是通过RTK技术实时测得的三维定位结果。在RTK作业模式下，基站采集微信数据，并通过数据链将其观测值和站点坐标信息一起传送给移动站，而移动站通过对所采集到的卫星数据和接收到的数据链进行实时载波相位差分处理，得出厘米级的定位结果。
具体的，机器人控制器在得到RTK定位信息之后，会先将RTK定位信息与自身的IMU姿态数据进行融合，得到具备经纬度、以及方位角的6维姿态信息。接着，再通过得到的6维姿态信息，对融合点云集合进行重新构建，得到重构点集合。接着，会再基于构建得到的重构点集合，进一步确定机器人在同步构建地图中的确切位置。
S4、根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合。
S5、确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
具体的，机器人控制器得到机器人在同步构建地图中的确切位置之后，将再次基于上一步骤的集合重新构建的计算原理，进行点云的精确构建，并将上一步骤添加的点云清除，计算更新点集合与融合点云集合之间的相似程度。并在所得的相似程度满足预设判断条件时，将更新点集合添加到同步构建地图中，以实现对同步构建地图的更新。
在其中一个实施例中，若所得的相似程度小于90％且大于60％，则将最新的点云集添加到同步构建地图(即现有地图)中，以实现对现有地图的更新。当然上述的判断范围，在不同的应用场景中也会有所不同，本申请实施例对其取值并不做限定。
S6、基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
具体的，机器人控制器在进行最优路径规划的时候，会自动地将现有地图上不利于机器人行走的区域划定为障碍物，从而实现在路径规划阶段就避开危险区域。此外，在机器人的可见光图像和3D点云图像回来后，会再次判断是否存在新的危险区域，从而控制机器人避开这些新的危险区域。
在其中一个实施例中，机器人控制器也会自动识别最新点云数据与可见光图像中是否存在危险区域，例如泥坑、土堆等，并将识别到的危险区域自动更新到地图中，以使得能够避开这些识别到的危险区域，进行最优路径的规划，提高路径规划效率。
上述的机器人导航方法，通过双目摄像头和激光雷达来获得3D点云数据，并自动修正点云的正确性，从而实现复杂环境下为机器人导航的目的，确保机器人导航的准确性稳定性和高鲁棒性。另外，在确保机器人稳定运行的同时，又能对环境进行自我学习与适配，机器人很容易便可以集成复杂的导航系统，只需要机器人有前后左右的运动控制量即可使用本技术方案。最后，本技术方案对机器人的形态、性和功率均无具体要求、适配性高、精度好、且对环境的适应能力好，能够满足不同天气、不同地形下的导航需求，提高了定位精度下降，避免无法导航的情况发生。
在一个实施例中，步骤S2中，根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合，包括以下子步骤：
S21、在粗融合阶段，通过下述公式计算粗对齐后的点云数据集：
Figure GDA0003593993350000081
其中，M为根据激光雷达与双目摄像头之间的相对位置，所确定三维变换矩阵；Ox1,Oy1,Oz1为所述粗对齐后的点云数据集；Ix1,Iy1,Iz1为3D点云数据。
具体的，在粗融合阶段，由于激光雷达与所述双目摄像头的安装位置相对固定，因此，在此阶段可以使用公式(1)计算粗对齐后的点云数据。
S22、在细融合阶段，对所述粗对齐后的点云数据集中包括的点集进行融合，并设激光雷达点云图为P，双目点云为Q，点云到小车之间的相对距离为L，以P为索引集，以双目点云Q为中心点，以所述相对距离L为半径。
具体的，当进行完粗融合之后，此时，粗对齐后的点云数据集中包括的各个点集均将处于临近的空间位置；此时，再对粗对齐后的点云数据集中包括的点集进行细融合。当前阶段中，将设激光雷达点云图为P，双目点云为Q，点云到小车之间的距离为L。
S23、在半径L范围内，针对所述索引集P中的各个索引点，分别进行匹配点的搜索，并基于相应索引点与对应搜索到的匹配点之间在距离上的平均值，进行融合点云集合的构建。
具体的，在细融合阶段，机器人控制器将以P为索引集，并寻找半径球范围内，是否存在匹配点，若不存在匹配点，则将匹配失败的索引点定义为New点；若半径球范围内，存在相应的匹配点，则计算距离最近点进行距离上的平均计算，得到新的点云集合O，即得到的融合点云集合。
上述实施例中，通过激光雷达和双目摄像头来获得3D点云数据，运用深度学习自动修正点云的正确性，确保机器人导航的准确性稳定性。
在一个实施例中，步骤S3中，所述获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置，包括：
S31、将所述RTK定位信息与机器人的IMU姿态信息进行融合，得到融合后的姿态信息。
具体的，机器人控制器将在得到RTK定位信息后，将所述RTK定位信息与机器人的IMU姿态信息进行融合，得到具备经纬度、和方位角的6维姿态信息。
S32、基于所述融合后的姿态信息，并根据下述公式，对所述融合点云集合进行重建，得到相应的重构点集合：
Figure GDA0003593993350000091
其中，Ox2,Oy2,Oz2为重构点集合，Ix2,Iy2,Iz2为融合点云集合，lx2,ly2,lz2为机器人当前的位置；θx,θy,θz为融合后的姿态信息。
S33、基于所述重构点集，计算所述重构点集的空间范围A，并基于所述空间范围A，在同步构建地图上选取相同位置、且为空间范围A的两倍大小的区域B。
具体的，机器人控制器将在重构点集构建完成的情况下，计算点集的空间范围A，并在原本地图里选取两倍A的区域B。
S34、将空间范围A和区域B沿着预设的目标坐标轴进行逆向，并往与所述目标坐标轴相适应的目标平面进行投影，得到相应的二位投影Ta,Tb。
S35、基于所述二位投影Ta,Tb，使用蒙特卡罗定位方式确定机器人在同步构建地图中的确切位置。
具体的，基于步骤S34和步骤S35，机器人控制器在确定得到空间范围A和区域B之后，会进行如下的变化：
(1)先分别将A与B沿着Z轴的逆向，往X轴、Y轴的0平面做投影得到相应的二位投影Ta,Tb；接着，再使用蒙特卡罗定位法，找出机器人xy方向的精准位置。
(2)再分别将空间范围A和区域B沿着Y轴逆向，并往X轴和Z轴的0平面做投影，得到相应的二位投影Ta,Tb；接着，再使用蒙特卡罗定位法找出机器人在xz方向上的精准位置。
(3)再分别将A与B沿着X轴逆向，并往Y轴和Z轴的0平面做投影，得到相应的二位投影Ta,Tb；接着，再使用蒙特卡罗定位法找出机器人在yz方向上的精准位置。
(4)之后，再分别求三个投影面xy、xz、yz中x、y、z的分量，该x、y、z的分量即为机器人当前的精准位置。值得注意的是，由于IMU(Inertial measurement unit，惯性测量单元)信号相对稳定，则当RTK由于遮挡而丢失的时候，根据地图本有的信息也能提供高精度的定位，只不过运算的时间会有所增加。
在其中一个实施例中，步骤S5中，所述确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新，包括：
S51、将所述更新点集合带入重构点集合的计算公式中，以对所述融合点云集合进行精确更新。
具体的，机器人控制器将更新点集合带入到步骤S32中示意的计算公式中，进行融合点云集合的精确更新。在一个实施例中，机器人控制器可以再次执行公式(1)将点云精确地构建在OpenCV(基于BSD许可发行的跨平台计算机视觉和机器学习软件库)中，并将上一步骤添加的点云清楚。
S52、基于更新后的更新点集合与所述融合点云集合之间的相似程度，在确定所述相似程度满足预设判断条件时，将更新后的更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
具体的，机器人控制器基于更新后的更新点集合与所述融合点云集合之间的相似程度。并在相似程度满足预设判断条件时，将最新的点云集添加到现有地图中。
在其中一个实施例中，若确定上述的相似程度小于90％且大于60％，则将最新的点云集添加到基于OpenGL(Open Graphics Library，开放图形库)构建的现有地图中。在一个实施例中，机器人控制器可以基于下述计算公式(2)，计算更新后的更新点集合与所述融合点云集合之间的相似程度：
Figure GDA0003593993350000111
在其中一个实施例中，所述基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，包括：通过下述公式计算最优路径f(i)：
Figure GDA0003593993350000112
其中，xi,yi,zi为机器人的当前位置，xt,yt,zt为所需达到的目标位置；h(i)为机器人从当前位置移动到目标位置的估值；g(i)为是当前迭代中，机器人从当前位置移动到目标位置之间的已知最短距离；g(i-1)为上一次迭代中，机器人从所述当前位置移动到所述目标位置之间的已知最短距离；l为机器人的当前位置到目标位置之间的直线距离。
具体的，机器人控制器在进行最优路径计算的时候，首先会定义f(i)＝g(i)+h(i)，其中，g(i)称为代价函数，h(i)称为启发函数。需要说明的是，如果函数h(i)总是小于或等于从i到目标的实际花费，那么机器人控制器就能确保找到最短路径，而函数h(i)的值越小，则机器人控制器要扩展的节点就越多，算法就执行的越慢。因此，在一个实施例中，将函数h(i)取欧几里德距离作为启发值，由于这个启发值通常小于栅格i和终点栅格e之间的实际距离，因此就能保证搜索到的路径距离最短。
在其中一个实施例中，该方法还包括：在基于所述最优路径控制机器人移动时，通过激光雷达与双目融合后的点云和同步构建地图，控制机器人避开设有障碍物的危险区域；当识别到新的危险区域时，通过下述计算公式，判断是否更新所述同步构建地图：
Figure GDA0003593993350000121
其中，k为决定因子；robot[i]为获取到新的危险区域的机器人，对该区域是否为危险区域的判断；Map(x,y,z)为所述同步构建地图中对应点是否为危险区域的初判断；在确定所述决定因子k满足预设条件时，在所述同步构建地图中将新的危险区域所对应的点，标记为危险点。
具体的，机器人控制器将自动的将地图上不利于机器人行走的区域计算为障碍物，从而实现在路径规划阶段就避开危险区域。此外，机器人的可见光图像和3D点云图像回来后，将经过深度学习网络，判断是否存在新的危险区域，从而控制机器人避开这些新的危险区域。
在其中一个实施例中，机器人控制器将通过深度学习，自动识别最新点云数据与可见光图像中是否存在危险区域(泥坑、土堆等)自动更新到地图中；在基于上述步骤识别到新的危险区域后，将该区域的信息自动上传到预设的服务器平台。其中，上述服务器平台上存着当前环境的最新3D点云地图，在服务器平台在接收到障碍物信息后，将通过比对相关机器人的上传数据，来决定是否更新已存储的最新3d点云地图中。在一个实施例中，当机器人控制器确定输出因子k>0.7时，将进行地图更新。
在一个实施例中，当由于模型缺陷未能识别出障碍物，从而导致机器人陷入泥潭，或与障碍物发送轻微碰撞时候，或者本应该是正常区域绕道被人工修正的时候，机器人控制器将自动把近期的样品数据定义为负样本，进行自我学习，同时通知可视范围内的机器人对该区域进行学习，第一个将本区域正确识别率大于80％的数据模型上传到服务器平台。服务器平台在收到模型数据后，将模型在线更新到各个机器人上，实现能力的共享。
在一个实施例中，请参考图2，本申请公开了一种机器人导航装置200，该装置200包括获取模块201、融合模块202、位置确定模块203、集合更新模块204、地图更新模块205和导航模块206，其中：
所述获取模块201，用于获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头。
所述融合模块202，用于根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合。
所述位置确定模块203，用于获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置。
所述集合更新模块204，用于根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合。
所述地图更新模块205，用于确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
所述导航模块206，用于基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航。
在其中一个实施例中，融合模块202还用于在粗融合阶段，通过下述公式计算粗对齐后的点云数据集：
Figure GDA0003593993350000131
其中，M为根据所述激光雷达与所述双目摄像头之间的相对位置，所确定的三维变换矩阵；Ox1,Oy1,Oz1为所述粗对齐后的点云数据集；Ix1,Iy1,Iz1为3D点云数据；在细融合阶段，对所述粗对齐后的点云数据集中包括的点集进行融合，并设激光雷达点云图为P，双目点云为Q，点云到小车之间的相对距离为L，以P为索引集，以双目点云Q为中心点，以所述相对距离L为半径；在半径L范围内，针对所述索引集P中的各个索引点，分别进行匹配点的搜索，并基于相应索引点与对应搜索到的匹配点之间在距离上的平均值，进行融合点云集合的构建。
在其中一个实施例中，位置确定模块203还用于将所述RTK定位信息与机器人的IMU姿态信息进行融合，得到融合后的姿态信息；基于所述融合后的姿态信息，并根据下述公式，对所述融合点云集合进行重建，得到相应的重构点集合：
Figure GDA0003593993350000141
其中，Ox2,Oy2,Oz2为重构点集合，Ox2,Oy2,Oz2为融合点云集合，lx2,ly2,lz2为机器人当前的位置；θx,θy,θz为融合后的姿态信息；基于所述重构点集，计算所述重构点集的空间范围A，并基于所述空间范围A，在同步构建地图上选取相同位置、且为空间范围A的两倍大小的区域B；将空间范围A和区域B沿着预设的目标坐标轴进行逆向，并往与所述目标坐标轴相适应的目标平面进行投影，得到相应的二位投影Ta,Tb；基于所述二位投影Ta,Tb，使用蒙特卡罗定位方式确定机器人在同步构建地图中的确切位置。
在其中一个实施例中，地图更新模块205还用于将所述更新点集合带入重构点集合的计算公式中，以对所述融合点云集合进行精确更新；基于更新后的更新点集合与所述融合点云集合之间的相似程度，在确定所述相似程度满足预设判断条件时，将更新后的更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
在其中一个实施例中，导航模块206还用于通过下述公式计算最优路径f(i)：
Figure GDA0003593993350000142
其中，xi,yi,zi为机器人的当前位置，xt,yt,zt为所需达到的目标位置；h(i)为机器人从当前位置移动到目标位置的估值；g(i)为是当前迭代中，机器人从当前位置移动到目标位置之间的已知最短距离；g(i-1)为上一次迭代中，机器人从所述当前位置移动到所述目标位置之间的已知最短距离；l为机器人的当前位置到目标位置之间的直线距离。
在其中一个实施例中，该装置200还包括避障模块，其中：避障模块，用于在基于所述最优路径控制机器人移动时，通过激光雷达与双目融合后的点云和同步构建地图，控制机器人避开设有障碍物的危险区域；当识别到新的危险区域时，通过下述计算公式，判断是否更新所述同步构建地图：
Figure GDA0003593993350000151
其中，k为决定因子；robot[i]为获取到新的危险区域的机器人，对该区域是否为危险区域的判断；Map(x,y,z)为所述同步构建地图中对应点是否为危险区域的初判断；在确定所述决定因子k满足预设条件时，在所述同步构建地图中将新的危险区域所对应的点，标记为危险点。
上述机器人导航装置，通过双目摄像头和激光雷达来获得3D点云数据，并自动修正点云的正确性，从而实现复杂环境下为机器人导航的目的，确保机器人导航的准确性稳定性和高鲁棒性。另外，在确保机器人稳定运行的同时，又能对环境进行自我学习与适配，机器人很容易便可以集成复杂的导航系统，只需要机器人有前后左右的运动控制量即可使用本技术方案。最后，本技术方案对机器人的形态、性和功率均无具体要求、适配性高、精度好、且对环境的适应能力好，能够满足不同天气、不同地形下的导航需求，提高了定位精度下降，避免无法导航的情况发生。
在一个实施例中，还提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。
上述计算机设备，通过双目摄像头和激光雷达来获得3D点云数据，并自动修正点云的正确性，从而实现复杂环境下为机器人导航的目的，确保机器人导航的准确性稳定性和高鲁棒性。另外，在确保机器人稳定运行的同时，又能对环境进行自我学习与适配，机器人很容易便可以集成复杂的导航系统，只需要机器人有前后左右的运动控制量即可使用本技术方案。最后，本技术方案对机器人的形态、性和功率均无具体要求、适配性高、精度好、且对环境的适应能力好，能够满足不同天气、不同地形下的导航需求，提高了定位精度下降，避免无法导航的情况发生。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
上述存储介质，通过双目摄像头和激光雷达来获得3D点云数据，并自动修正点云的正确性，从而实现复杂环境下为机器人导航的目的，确保机器人导航的准确性稳定性和高鲁棒性。另外，在确保机器人稳定运行的同时，又能对环境进行自我学习与适配，机器人很容易便可以集成复杂的导航系统，只需要机器人有前后左右的运动控制量即可使用本技术方案。最后，本技术方案对机器人的形态、性和功率均无具体要求、适配性高、精度好、且对环境的适应能力好，能够满足不同天气、不同地形下的导航需求，提高了定位精度下降，避免无法导航的情况发生。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种机器人导航方法，其特征在于，所述方法包括：
S1、获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
S2、根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
S3、获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
S4、根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
S5、确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
S6、基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航；
S7：在基于所述最优路径控制机器人移动时，通过激光雷达与双目融合后的点云和同步构建地图，控制机器人避开设有障碍物的危险区域；
当识别到新的危险区域时，通过下述计算公式，判断是否更新所述同步构建地图：
Figure FDA0003593993340000011
其中，k为决定因子；robot[i]为获取到新的危险区域的机器人，对该区域是否为危险区域的判断；Map(x,y,z)为所述同步构建地图中对应点是否为危险区域的初判断；
在确定所述决定因子k满足预设条件时，在所述同步构建地图中将新的危险区域所对应的点，标记为危险点。
2.根据权利要求1所述的方法，其特征在于，步骤S2中，所述根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合，包括：
S21、在粗融合阶段，通过下述公式计算粗对齐后的点云数据集：
Figure FDA0003593993340000021
其中，M为根据所述激光雷达与所述双目摄像头之间的相对位置，所确定的三维变换矩阵；Ox1,Oy1,Oz1为所述粗对齐后的点云数据集；Ix1,Iy1,Iz1为3D点云数据；
S22、在细融合阶段，对所述粗对齐后的点云数据集中包括的点集进行融合，并设激光雷达点云图为P，双目点云为Q，点云到小车之间的相对距离为L，以P为索引集，以双目点云Q为中心点，以所述相对距离L为半径；
S23、在半径L范围内，针对所述索引集P中的各个索引点，分别进行匹配点的搜索，并基于相应索引点与对应搜索到的匹配点之间在距离上的平均值，进行融合点云集合的构建。
3.根据权利要求1所述的方法，其特征在于，步骤S3中，所述获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置，包括：
S31、将所述RTK定位信息与机器人的IMU姿态信息进行融合，得到融合后的姿态信息；
S32、基于所述融合后的姿态信息，并根据下述公式，对所述融合点云集合进行重建，得到相应的重构点集合：
Figure FDA0003593993340000031
其中，Ox2,Oy2,Oz2为重构点集合，Ix2,Iy2,Iz2为融合点云集合，lx2,ly2,lz2为机器人当前的位置；θx,θy,θz为融合后的姿态信息；
S33、基于所述重构点集，计算所述重构点集的空间范围A，并基于所述空间范围A，在同步构建地图上选取相同位置、且为空间范围A的两倍大小的区域B；
S34、将空间范围A和区域B沿着预设的目标坐标轴进行逆向，并往与所述目标坐标轴相适应的目标平面进行投影，得到相应的二位投影Ta,Tb；
S35、基于所述二位投影Ta,Tb，使用蒙特卡罗定位方式确定机器人在同步构建地图中的确切位置。
4.根据权利要求3所述的方法，其特征在于，步骤S5中，所述确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新，包括：
S51、将所述更新点集合带入重构点集合的计算公式中，以对所述融合点云集合进行精确更新；
S52、基于更新后的更新点集合与所述融合点云集合之间的相似程度，在确定所述相似程度满足预设判断条件时，将更新后的更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
5.根据权利要求1所述的方法，其特征在于，所述基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，包括：
通过下述公式计算最优路径f(i)：
f(i)＝g(i)+h(i)
Figure FDA0003593993340000032
g(i)＝g(i-1)+l
其中，xi,yi,zi为机器人的当前位置，xt,yt,zt为所需达到的目标位置；h(i)为机器人从当前位置移动到目标位置的估值；g(i)为是当前迭代中，机器人从当前位置移动到目标位置之间的已知最短距离；g(i-1)为上一次迭代中，机器人从所述当前位置移动到所述目标位置之间的已知最短距离；l为机器人的当前位置到目标位置之间的直线距离。
6.一种机器人导航装置，其特征在于，所述装置包括获取模块、融合模块、位置确定模块、集合更新模块、地图更新模块和导航模块，其中：
所述获取模块，用于获取经由定位设备传输的3D点云数据，所述定位设备包括激光雷达和双目摄像头；
所述融合模块，用于根据所述激光雷达与所述双目摄像头之间的相对位置、以及点云到目标小车之间的相对距离，对所述3D点云数据进行融合，得到相应的融合点云集合；
所述位置确定模块，用于获取机器人的RTK定位信息，结合所述融合点云集合和所述RTK定位信息，确定机器人在同步构建地图中的确切位置；
所述集合更新模块，用于根据机器人在同步构建地图中的确切位置，对所述融合点云集合进行更新，得到更新点集合；
所述地图更新模块，用于确定所述更新点集合与所述融合点云集合之间的相似程度，在所述相似程度满足预设判断条件时，将所述更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新；
所述导航模块，用于基于更新后的地图、机器人的当前位置以及所需达到的目标位置，计算机器人从所述当前位置移动到所述目标位置的最优路径，并基于所述最优路径进行同步导航；
所述导航模块，还用于在基于所述最优路径控制机器人移动时，通过激光雷达与双目融合后的点云和同步构建地图，控制机器人避开设有障碍物的危险区域；
当识别到新的危险区域时，所述地图更新模块通过下述计算公式，判断是否更新所述同步构建地图：
Figure FDA0003593993340000051
其中，k为决定因子；robot[i]为获取到新的危险区域的机器人，对该区域是否为危险区域的判断；Map(x,y,z)为所述同步构建地图中对应点是否为危险区域的初判断；
在确定所述决定因子k满足预设条件时，所述地图更新模块在所述同步构建地图中将新的危险区域所对应的点，标记为危险点。
7.根据权利要求6所述的装置，其特征在于，所述地图更新模块还用于将所述更新点集合带入重构点集合的计算公式中，以对所述融合点云集合进行精确更新；基于更新后的更新点集合与所述融合点云集合之间的相似程度，在确定所述相似程度满足预设判断条件时，将更新后的更新点集合添加到所述同步构建地图中，以实现对所述同步构建地图的更新。
8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至5中任一项所述的方法的步骤。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述的方法的步骤。
CN202111009211.8A 2021-08-31 2021-08-31 一种机器人导航方法、装置、计算机设备和存储介质 Active CN113776515B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111009211.8A CN113776515B (zh) 2021-08-31 2021-08-31 一种机器人导航方法、装置、计算机设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111009211.8A CN113776515B (zh) 2021-08-31 2021-08-31 一种机器人导航方法、装置、计算机设备和存储介质

Publications (2)

Publication Number Publication Date
CN113776515A CN113776515A (zh) 2021-12-10
CN113776515B true CN113776515B (zh) 2022-06-10

Family

ID=78840064

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111009211.8A Active CN113776515B (zh) 2021-08-31 2021-08-31 一种机器人导航方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN113776515B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114078151B (zh) * 2022-01-19 2022-04-22 季华实验室 一种点云融合方法、装置、电子设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108828606A (zh) * 2018-03-22 2018-11-16 中国科学院西安光学精密机械研究所 一种基于激光雷达和双目可见光相机联合测量方法
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
WO2021128297A1 (zh) * 2019-12-27 2021-07-01 深圳市大疆创新科技有限公司 三维点云地图构建方法、系统和设备

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110307838B (zh) * 2019-08-26 2019-12-10 深圳市优必选科技股份有限公司 机器人重定位方法、装置、计算机可读存储介质及机器人
CN113175925B (zh) * 2021-04-14 2023-03-14 武汉理工大学 定位与导航系统以及方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108828606A (zh) * 2018-03-22 2018-11-16 中国科学院西安光学精密机械研究所 一种基于激光雷达和双目可见光相机联合测量方法
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置
WO2021128297A1 (zh) * 2019-12-27 2021-07-01 深圳市大疆创新科技有限公司 三维点云地图构建方法、系统和设备
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Integrating V-SLAM and LiDAR-based SLAM for Map Updating;Yu-Cheng Chang，et al.;《4th IEEE International Conference on Knowledge Innovation and Invention 2021》;20210725;134-137 *
邓世燕等.基于多传感器融合的即时定位与地图构建方法研究.《 第十一届中国卫星导航年会》.2020, *

Also Published As

Publication number Publication date
CN113776515A (zh) 2021-12-10

Similar Documents

Publication Publication Date Title
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN107144285B (zh) 位姿信息确定方法、装置和可移动设备
JP5927735B2 (ja) 地図データ作成装置、自律移動システムおよび自律移動制御装置
Kim et al. Stereo camera localization in 3d lidar maps
KR20180079428A (ko) 자동 로컬리제이션을 위한 장치 및 방법
CN109682373A (zh) 一种无人平台的感知系统
CN111982114B (zh) 一种采用imu数据融合估计三维位姿的救援机器人
Senlet et al. Satellite image based precise robot localization on sidewalks
CN112518739A (zh) 履带式底盘机器人侦察智能化自主导航方法
JP2018081008A (ja) 基準映像地図を用いた自己位置姿勢標定装置
CN110764110B (zh) 路径导航方法、装置及计算机可读存储介质
CN113189613B (zh) 一种基于粒子滤波的机器人定位方法
CN113776515B (zh) 一种机器人导航方法、装置、计算机设备和存储介质
CN112068152A (zh) 使用3d扫描仪同时进行2d定位和2d地图创建的方法和系统
KR102176729B1 (ko) 이동수단의 위치인식방법 및 위치인식시스템
CN114413909A (zh) 一种室内移动机器人定位方法及系统
Tao et al. Automated processing of mobile mapping image sequences
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
CN116007623A (zh) 机器人导航方法、装置和计算机可读存储介质
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
Shao et al. Slam for indoor parking: A comprehensive benchmark dataset and a tightly coupled semantic framework
Peng et al. A novel geo-localisation method using GPS, 3D-GIS and laser scanner for intelligent vehicle navigation in urban areas
Wildermuth et al. Maintaining a common co-ordinate system for a group of robots based on vision
CN113390422B (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
TR01 Transfer of patent right

Effective date of registration: 20230117

Address after: 230000 B-1015, wo Yuan Garden, 81 Ganquan Road, Shushan District, Hefei, Anhui.

Patentee after: HEFEI MINGLONG ELECTRONIC TECHNOLOGY Co.,Ltd.

Address before: 330108 No. 998 Gezaoshan Avenue, Honggutan New District, Nanchang City, Jiangxi Province

Patentee before: NANCHANG INSTITUTE OF SCIENCE & TECHNOLOGY

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230915

Address after: Room 403-A35, Building E, Fuxing City, No. 32 Binhai Avenue, Longhua District, Haikou City, Hainan Province, 570100

Patentee after: Hainan Hongpu Technology Co.,Ltd.

Address before: 230000 B-1015, wo Yuan Garden, 81 Ganquan Road, Shushan District, Hefei, Anhui.

Patentee before: HEFEI MINGLONG ELECTRONIC TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right