CN113269837B - 一种适用于复杂三维环境的定位导航方法 - Google Patents

一种适用于复杂三维环境的定位导航方法 Download PDF

Info

Publication number
CN113269837B
CN113269837B CN202110457458.XA CN202110457458A CN113269837B CN 113269837 B CN113269837 B CN 113269837B CN 202110457458 A CN202110457458 A CN 202110457458A CN 113269837 B CN113269837 B CN 113269837B
Authority
CN
China
Prior art keywords
point cloud
dimensional
laser radar
global
points
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
CN202110457458.XA
Other languages
English (en)
Other versions
CN113269837A (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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN202110457458.XA priority Critical patent/CN113269837B/zh
Publication of CN113269837A publication Critical patent/CN113269837A/zh
Application granted granted Critical
Publication of CN113269837B publication Critical patent/CN113269837B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/253—Fusion techniques of extracted features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/40—Scaling of whole images or parts thereof, e.g. expanding or contracting
    • G06T3/4007—Scaling of whole images or parts thereof, e.g. expanding or contracting based on interpolation, e.g. bilinear interpolation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/90—Determination of colour characteristics
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Multimedia (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种适用于复杂三维环境的定位导航方法，属于机器人导航技术领域。包括：基于激光雷达和相机，构建移动机器人平台；对激光雷达输出的原始点云进行预处理获得经过滤波和消除运动畸变的点云；对相机进行标定根据标定结果对相机输出的图像进行校正获得消除畸变的图像；对激光雷达和相机进行联合标定，获得激光雷达和相机的外参矩阵；以此获得带有颜色信息的点云；根据消除畸变的图像和带有颜色信息的点云，进行激光雷达和相机组合的同步定位与建图，获得优化的高低频组合里程计；构建包含三维信息和膨胀层的全局二维栅格地图，并进行移动机器人的全局路径规划和局部路径规划，解决了无有效确识别复杂三维障碍物的问题。

Description

一种适用于复杂三维环境的定位导航方法
技术领域
本发明属于机器人导航技术领域，涉及一种适用于复杂三维环境的定位导航方法。
背景技术
近年来，自主移动机器人在社会生产和生活的众多领域得到了广泛的应用，移动机器人的定位、建图与导航是其完成应用任务的基础。其中，定位是指确定移动机器人在局部环境中的实时位姿；建图是指移动机器人对周围环境进行建模；导航是指移动机器人根据已有信息完成点到点的路线规划和运动控制。随着自主移动机器人的应用领域不断扩大，机器人在实际应用中面临着愈发复杂的三维环境，己经不能很好地通过单一传感器来实现精准定位和导航。基于激光雷达或者视觉的定位导航方案都存在着一定的问题。视觉方案难以克服光照变化带来的影响，且构建的稀疏特征点地图不能用于移动机器人的路径规划。而激光雷达方案由于缺乏丰富的环境纹理信息，点云配准精度和效率都会受到影响。因此，使用单一传感器的定位导航***有其局限性。
发明内容
为了克服上述基于单一传感器的移动机器人定位导航***不能适很好地应于越来越复杂的应用环境的技术缺陷，提供一种适用于复杂三维环境的定位导航方法，解决了在复杂三维环境中不能有效定位和避障的问题。
为了达到上述目的，本发明采用以下技术方案予以实现：
本发明公开了一种适用于复杂三维环境的定位导航方法，包括如下步骤：
步骤1、基于激光雷达和相机，构建移动机器人平台；
步骤2、在步骤1构建的移动机器人平台上，对激光雷达输出的原始点云进行预处理，获得经过滤波和消除运动畸变的点云；对相机进行标定，然后根据标定结果对相机输出的图像进行校正，获得消除畸变的图像；对激光雷达和相机进行联合标定，获得激光雷达和相机的外参矩阵；根据所得经过滤波和消除运动畸变的点云、所得消除畸变的图像以及所得激光雷达和相机的外参矩阵，进行点云和图像的数据融合，获得带有颜色信息的点云；
步骤3、根据步骤2获得的消除畸变的图像和带有颜色信息的点云，进行激光雷达和相机组合的同步定位与建图，获得高低频组合里程计；
步骤4、根据步骤3获得的高低频组合里程计和步骤2获得的经过滤波和消除运动畸变的点云，构建包含三维信息和膨胀层的全局二维栅格地图，并基于所构建的包含三维信息和膨胀层的全局二维栅格地图进行移动机器人的全局路径规划和局部路径规划；实现适用于复杂三维环境的定位导航方法。
优选地，步骤2中，对激光雷达输出的原始点云进行预处理，获得经过滤波和消除运动畸变的点云，其具体步骤如下：
对激光雷达输出的原始点云进行统计滤波、体素网格滤波和点云反射强度滤波，获得经过滤波的点云；基于线性插值方法，对所得经过滤波的点云中的每个点进行空间位置修正，获得经过滤波和消除运动畸变的点云。
进一步优选地，基于线性插值方法，对获得的经过滤波的点云中的每个点进行空间位置修正，获得消除运动畸变的点云，包括如下操作：
对经过滤波的点云Pk，在激光雷达实际测量过程中的开始时间tk-1和结束时间tk组成的单位时间间隔[tk-1，tk]中，根据在上一时刻tk-1时激光雷达的位姿Tk-1和tk-2时刻激光雷达的位姿Tk-2线性地估算[tk-1，tk]时间内时刻激光雷达的位姿/>根据/>和/>之间的位姿变换将每个/>时刻的点/>全部转换到tk时刻的参考坐标系，得到初步消除运动畸变的点云/>消除因为运动带来的观测结果漂移，使得每个点/>转换到tk时刻的参考坐标系，等通过点云匹配得到准确的位姿 Tk后，在进行一次位姿线性插值获得更加准确的点云Pk，获得消除运动畸变的点云。
其中，进一步优选地，的计算公式如下：
式中，符号Θ表示特殊欧式群三维变换的逆运算。
优选地，步骤3中，进行激光雷达和相机组合的同步定位与建图的操作，具体包括：
步骤3.1中，使用视觉里程计，对步骤2获得的消除畸变的图像进行帧与帧之间地匹配计算，增程式地获到高频视觉里程计；
步骤3.2中，根据带有颜色信息的点云中每个点临近区域曲率的大小，将带有颜色信息的点云中的曲率最大的若干个点提取出来构建为边缘特征点云，曲率最小的若干个点提取出来构建为平面特征点云；
步骤3.3中，利用颜色信息约束的ICP点云匹配算法，将步骤3.2中提取的边缘特征点云和边缘特征点云地图进行匹配，平面特征点分别和平面特征点地图进行匹配，计算出带有颜色信息的点云相对世界坐标系的位姿变换，从而累积地获得低频激光雷达里程计；
步骤3.4中，对步骤3.1获得的高频视觉里程计和步骤3.3获得的低频激光雷达里程计，进行高低频插值组合，获得高低频组合里程计。
进一步优选地，步骤3.2中，构建边缘特征点云和平面特征点云的具体步骤如下：
根据激光扫描的先后顺序，对于带有颜色信息的点云的点i，选取其前后十个点，点i不包括带有颜色信息的点云中前面五个和后面五个点，计算点i的曲率ci的值；
将带有颜色信息的点云的点根据曲率ci大小进行排序，曲率最大的20点提取为边缘特征点，并将这20个边缘特征点构建为边缘特征点云，曲率最小的20点提取为平面特征点，并将这20个平面特征点构建为平面特征点云。
其中，进一步优选地，曲率ci定义为：
式中，Xi表示点i的坐标，Xj表示点i前后十个点的坐标。
进一步优选地，步骤3.3中，ICP点云匹配算法的步骤如下：
提高颜色信息相同的边缘特征点和平面特征点的权重，降低颜色信息不同的边缘特征点和平面特征点的权重，使得颜色信息融入到点云自身几何信息，构建颜色信息约束的ICP点云匹配算法；在进行边缘特征点云和边缘特征点云地图的匹配时，将边缘特征点云地图转换为KD-tree的形式保存在计算机内存中，利用KD-tree查找边缘特征点云中的点在边缘特征点云地图中的对应点；在进行平面特征点和平面特征点地图的匹配时，将平面特征点地图转换为KD-tree的形式保存在计算机内存中，利用KD-tree查找平面特征点中的点在平面特征点地图中的对应点。
进一步优选地，步骤3.4中，获得高低频组合里程计，包括如下操作：
当接收到低频激光雷达里程计后，将低频激光雷达里程计的位姿信息按照接收到时间戳***到高频视觉里程计对应到的时间戳位置，更新高低频组合里程计的位姿信息，得到高低频组合里程计；
优选地，步骤4具体包括如下步骤：
将经过滤波和消除运动畸变的点云，转换为临时八叉树地图，消除临时八叉树地图中的地面区域，获得局部八叉树地图；
根据步骤3所得高低频组合里程计实时地将所得局部八叉树地图接到世界坐标系，获得到全局八叉树地图；
根据移动机器人相对地面的高度，对所得全局八叉树地图进行二维投影，获得包含三维信息的全局二维栅格地图，得到的包含三维信息的全局二维栅格地图将高于地面且低于移动机器人的障碍物对应的栅格位置为占据状态；
根据移动机器人的水平方向最大横截面的外接圆半径r，对包含三维信息的全局二维栅格地图，为了避免移动机器人和障碍物相撞，设置大小为2r的膨胀系数，然后将全局二维栅格地图中占据状态栅格向外扩张个栅格，其中a为每个栅格的边长，符号/>表示向上取整，获得包含三维信息和膨胀层的全局二维栅格地图。
进一步优选地，基于获得的包含三维信息和膨胀层的全局二维栅格地图，执行导航流程，具体步骤如下：
a、输入包含三维信息和膨胀层的全局二维栅格地图，并进行数据初始化；
b、使用A*算法对移动机器人在包含三维信息和膨胀层的全局二维栅格地图进行全局路径规划；
c、根据经过滤波和消除运动畸变的点云，使用DWS算法进行局部路径规划，使移动机器人可以动态地避开障碍物。
优选地，当全局路径规划失败或者局部路径规划超时，移动机器人就会制动停止运动并进入恢复行为模式，然后重新进行全局路径规划；
当局部路径规划失败但是未超时，移动机器人将会制动停止运动，然后重新进行全局路径规划。
与现有技术相比，本发明具有以下有益效果：
本发明公开了一种适用于复杂三维环境的定位导航方法，通过构建基于激光雷达和相机组合的移动机器人平台，能够解决使用单一传感器的移动机器人平台在复杂环境中的局限性；本发明所述数据预处理和融合方法，消除了点云的噪音，消除了点云的运动畸变和相机的径向畸变，解决了直接利用原始数据进行定位造成的计算效率和精度低下的问题；本实验所述的激光雷达和相机高低频组合的里程计，可以获得高频且稳定的定位信息，解决了使用单一传感器在光照变化或者空间结构单一环境下可能失效的问题。因此，本发明所述适用于复杂三维环境的定位导航方法，在室内外结构化不明显或者光照变化剧烈或者悬空低矮障碍物较多等环境中能够适用，解决了在复杂三维环境中不能有效定位和避障的技术问题。
进一步地，颜色信息约束的ICP点云匹配算法，考虑到了环境的颜色信息，提高了低频激光里程计的精度。
进一步地，构建的包含三维信息和膨胀层的全局二维栅格地图，考虑到了机器人的尺寸和环境中的三维障碍物，使得移动机器人可以有效避开低矮和悬空的三维障碍物。
综上，本发明充分利用激光雷达和相机获得的观测信息，设计了基于激光雷达和相机组合的定位导航方案。这种方案与基于单一传感器的定位导航方案相比，可以使移动机器人在复杂三维环境中实现高频且稳定的定位和导航，避免环境光线变化和动态障碍物等因素的影响。同时，本发明的含有环境三维信息的导航地图可以使移动机器人有效避开低矮和悬空的障碍物，顺利完成导航任务。
附图说明
图1为本发明的整体流程图；
图2为本发明中全局路径规划和局部路径规划的导航规划流程图；
图3为本发明的实施例框架图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
下面结合附图对本发明做进一步详细描述：
1、构建移动机器人平台，包括硬件平台和软件平台
本发明的移动机器人平台主要由移动机器人、激光雷达、相机和中控计算机组成。移动机器人底盘、激光雷达、相机和中控计算机之间进行硬件的固连，并通过数据线进行数据通信。其中，移动机器人底盘负责移动机器人平台的运动，激光雷达和相机负责观测环境，中控计算机负责算法实现。
2、对激光雷达和相机的数据进行预处理和融合
首先，对激光雷达输出的原始点云数据进行预处理。由于激光雷达输出的点云中含有噪音，会影响定位导航的精度，因此要对原始点云中的噪音进行滤除：使用统计滤波器滤除空间中密度小于密度阈值的点，快速剔除那些明显的噪音点；使用体素网格滤波器滤波比较稠密的点云，进行降采样减少点云数据的大小，使得点云的分布更加均匀并可提高后续的计算效率；使用反射强度滤波器滤除反射强度过大和过小的点。这样就获得经过滤波的点云。
其次，基于线性插值方法，所点云中的每个点进行空间位置修正，获得消除运动畸变的点云，具体操作如下：对经过滤波的点云Pk，在激光雷达实际测量过程中的开始时间tk-1和结束时间tk组成的单位时间间隔[tk-1，tk]中，根据在上一时刻tk-1时激光雷达的位姿Tk-1和tk-2时刻激光雷达的位姿Tk-2估算[tk-1，tk]时间内时刻激光雷达的位姿/>根据/>和/>之间的位姿变换将每个/>时刻的点/>全部转换到tk时刻的参考坐标系，得到初步消除运动畸变的点云/>消除因为运动带来的观测结果漂移，使得每个点/>转换到tk时刻的参考坐标系，等通过点云匹配得到准确的位姿Tk后，在进行一次位姿线性插值获得更加准确的点云Pk，获得经过滤波和消除运动畸变的点云。其中，/>的计算公式如下：
式中，符号Θ表示特殊欧式群三维变换的逆运算。
然后，对相机进行标定，并对激光雷达和相机进行联合标定。本发明采用 opencv库中的张正友相机标定方法，使用一个棋格盘标定板对相机进行标定，获得相机的内参矩阵和径向畸变参数，然后根据标定结果对图像进行校正，消除广角镜头相机带来的畸变，获得消除畸变的图像。本发明使用平板标定板进行激光雷达和相机的联合标定：将平板标定板放置在空旷的环境中，在激光雷达和相机的视角范围内对平板标定板进行不同角度和距离的观测，利用平板标定板区域的点云拟合出的平面方程和相机的内参矩阵计算出激光雷达和相机间的外参矩阵即两者之间坐标系的转换关系。再根据相机的内参矩阵、激光雷达和相机的外参矩阵以及点云的空间坐标，计算出点云中的点对应的图像像素点，并将这个像素点的RGB信息传递到对应的点云点中，获得带有颜色信息的点云。
3、构建高低频融合的里程计
首先，在视觉里程计中，利用相机输出的消除畸变的图像进行帧与帧之间的运动状态估计，帧与帧之间图像匹配算法采用PNP算法，可以增程式地获到高频视觉里程计；
然后，提取带有颜色信息的点云的特征点云。根据激光扫描的先后顺序，对于带有颜色信息的点云的点i，选取其前后十个点，点i不包括带有颜色信息的点云中前面五个和后面五个点；计算点i的曲率ci的值，曲率ci定义为：
式中，Xi表示点i的坐标，Xj表示点i前后十个点的坐标；将带有颜色信息的点云的点根据曲率ci大小进行排序，曲率最大的20点提取为边缘特征点，并将这20 个边缘特征点构建为边缘特征点云，曲率最小的20点提取为平面特征点，并将这20个平面特征点构建为平面特征点云。在选取特征点的时候，如果这个点周围五个点中已存在选取好的特征点则跳过这个点，这样可以让特征点分布得更加均匀。
其中，曲率最大的点提取个数和曲率最小的点提取个数均为N，N的大小根据实际环境进行设置；在本发明的具体实施方式中，10≤N≤30。具体地，在选取特征点的时候，如果这个点周围五个点中已存在选取好的特征点则跳过这个点，这样可以让特征点分布得更加均匀。
在激光雷达里程计中，利用颜色信息约束的ICP点云匹配算法，将提取的边缘特征点云和边缘特征点云地图进行匹配，平面特征点分别和平面特征点地图进行匹配，计算出带有颜色信息的点云相对世界坐标系的位姿变换，从而累积地获得低频激光雷达里程计。进行ICP点云匹配算法时，提高颜色信息相同的边缘特征点和平面特征点的权重，降低颜色信息不同的边缘特征点和平面特征点的权重，使得颜色信息融入到点云本身几何信息，构建颜色信息约束的ICP点云匹配算法。在进行边缘特征点云和边缘特征点云地图的匹配时，将边缘特征点云地图转换为 KD-tree的形式保存在计算机内存中，利用KD-tree查找边缘特征点云中的点在边缘特征点云地图中的对应点。同理，在进行平面特征点分别和平面特征点地图进行时也使用相同的查找对应点的方法。
移动机器人的定位信息由视觉里程计和激光雷达里程计进行高低频融合后获得，具体方法操作是:高低频组合里程计的ROS节点分别接受高频视觉里程计和低频激光雷达里程计；当接收到低频激光雷达里程计后，将低频激光雷达里程计的位姿信息按照接收到时间戳***到高频视觉里程计对应到的时间戳位置，更新高低频组合里程计的位姿信息，得到优化的高低频组合里程计；根据优化的高低频组合里程计，将经过滤波和消除运动畸变的点云变换到世界坐标系下，增程式地构建稠密点云地图。在一些无结构或者光照变换剧烈的环境中，单一传感器的定位导航方案不能正常工作。而本发明的高低频组合里程计，当高频视觉里程计或低频激光雷达里程计，由于传感器退化而失效时，剩余的工作模块将自动调整以承担主要的运动估计任务，当传感器退化问题解决后，所有模块恢复正常运行，可以提高***的稳定性。
4、在复杂三维环境中进行定位和导航
为了可以进行移动机器人的路径规划，需要先要构建导航地图。具体操作是：将经过滤波和消除运动畸变的点云，转换为临时八叉树地图，消除临时八叉树地图中的地面区域，获得局部八叉树地图；高低频组合里程计实时地将所得局部八叉树地图接到世界坐标系，获得到全局八叉树地图；根据移动机器人相对地面的高度，对所得全局八叉树地图进行二维投影，获得包含三维信息的全局二维栅格地图，得到的包含三维信息的全局二维栅格地图将高于地面且低于移动机器人的障碍物对应的栅格位置为占据状态；根据移动机器人的水平方向最大横截面的外接圆半径r，对包含三维信息的全局二维栅格地图，为了避免移动机器人和障碍物相撞，设置一个大小为2r的膨胀系数，然后将全局二维栅格地图中占据状态栅格向外扩张其中a为每个栅格的边长，符号/>表示向上取整，获得包含三维信息和膨胀层的全局二维栅格地图。
对于移动机器人的路径规划，分为全局路径规划和局部路径规划。本发明使用A*算法对移动机器人在全局二维栅格地图上进行全局路径规划，获得移动机器人从起始点到目标点需要遵循的全局行进线路。再将激光雷达实时输出的包含障碍物信息的点云转换为二维栅格地图，然后使用DWS算法对移动机器人进行局部路径规划，使其可以动态地避开障碍物。参见图2所示的导航规划流程图，在执行导航任务时，首先会进行全局路径规划，然后进行局部路径规划。如果全局路径规划失败或者局部路径规划超时，移动机器人就会制动停止运动并进入恢复行为模式，然后重新进行规划。如果全局路径规划未完成，或者局部路径规划失败但是未超时，移动机器人将会制动停止运动，然后重新进行全局路径规划。
下面结合具体实施例对本发明做进一步详细描述：
根据上述的具体实施方法，实现了一种具体实施例，参见图3所示的实施例框架图，将本发明所述适用于复杂三维环境的定位导航方法用于此实施例的***中，整个***可以分为四个模块：数据预处理模块、定位建图模块、导航规划模块和底盘控制模块。
在实施例中，整个移动机器人硬件平台硬件之间的数据连接情况如下：激光雷达和笔记本电脑连接，将激光雷达点云传输到笔记本电脑中进行计算处理；相机通过数据线和笔记本电脑相连，将图像数据流传输到笔记本电脑；移动机器人底盘通过数据线和笔记本电脑相连，通过串口协议传输笔记本发出的机器人底盘运动控制指令。并通过ROS进行移动机器人各个模块的编程实现，构建了一种移动机器人的软件平台。使用ROS的master节点管理其他的功能节点的注册和启停，使用一个或多个节点对移动机器人***的数据预处理模块、定位建图模块、导航规划模块和底盘运动控制模块进行算法功能的编程实现，使用ROS的rviz 插件可视化移动机器人高低频组合里程计的轨迹、稠密点云地图、八叉树地图、二维栅格地图和机器人硬件模型等信息。
参见图3所示的实施例框架图，导航规划模块通过接收定位建图模块的定位信息、导航目标点的坐标、机器人***坐标系系变换信息、激光雷达观测到的障碍物信息和二维栅格化导航地图等数据，对移动机器人进行静态全局路径规划和动态局部路径规划来获得移动机器人的运动路线，然后将包含机器人线速度和角速度信息的控制指令发送给移动机器人底盘。底盘控制模块将导航规划模块输出的控制指令进行数据包装，然后通过串口协议发送到移动机器人底盘的主控机上，机器人底盘的主控机将控制指令解析为底盘运动的线速度和角速度。根据两轮差速式移动机器人的运动模型，可以将底盘运动的线速度和角速度指令转换为移动机器人底盘两个轮子相应的转速，然后控制两个轮子电机的转速就可以完成控制指令。这样就能在复杂三维环境中顺利完成整个导航任务。
综上所述，本发明公开了一种适用于复杂三维环境的定位导航方法，主要解决了单一传感器方案不能应对复杂三维环境的问题。包括数据预处理模块、定位建图模块、导航规划模块和底盘运动控制模块。其实现方案为：1.构建移动机器人的硬件和软件平台；2.对数据进行预处理；3.进行机器人定位和导航地图构建； 4.根据定位信息、实时观测信息和导航地图进行路径规划，5.移动机器人执行控制指令完成导航任务。本发明融合了激光雷达和相机的优点，能够在复杂三维环境中运行，不局限于室内、外结构化不明显或者光照变换剧烈或者悬空低矮障碍较多等环境中，具有稳定性强、实时性好、适用范围广等优点。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (8)

1.一种适用于复杂三维环境的定位导航方法，其特征在于，包括如下步骤：
步骤1、基于激光雷达和相机，构建移动机器人平台；
步骤2、在步骤1构建的移动机器人平台上，对激光雷达输出的原始点云进行预处理，获得经过滤波和消除运动畸变的点云；对相机进行标定，然后根据标定结果对相机输出的图像进行校正，获得消除畸变的图像；对激光雷达和相机进行联合标定，获得激光雷达和相机的外参矩阵；根据所得经过滤波和消除运动畸变的点云、所得消除畸变的图像以及所得激光雷达和相机的外参矩阵，进行点云和图像的数据融合，获得带有颜色信息的点云；
步骤3、根据步骤2获得的消除畸变的图像和带有颜色信息的点云，进行激光雷达和相机组合的同步定位与建图，获得高低频组合里程计；
其中，进行激光雷达和相机组合的同步定位与建图的操作，具体包括：
步骤3.1中，使用视觉里程计，对步骤2获得的消除畸变的图像进行帧与帧之间地匹配计算，增程式地获到高频视觉里程计；
步骤3.2中，根据带有颜色信息的点云中每个点临近区域曲率的大小，将带有颜色信息的点云中的曲率最大的若干个点提取出来构建为边缘特征点云，曲率最小的若干个点提取出来构建为平面特征点云；
步骤3.3中，利用颜色信息约束的ICP点云匹配算法，将步骤3.2中提取的边缘特征点云和边缘特征点云地图进行匹配，平面特征点分别和平面特征点地图进行匹配，计算出带有颜色信息的点云相对世界坐标系的位姿变换，从而累积地获得低频激光雷达里程计；
步骤3.4中，对步骤3.1获得的高频视觉里程计和步骤3.3获得的低频激光雷达里程计，进行高低频插值组合，获得高低频组合里程计；
步骤4、根据步骤3获得的高低频组合里程计和步骤2获得的经过滤波和消除运动畸变的点云，构建包含三维信息和膨胀层的全局二维栅格地图，并基于所构建的包含三维信息和膨胀层的全局二维栅格地图进行移动机器人的全局路径规划和局部路径规划；实现适用于复杂三维环境的定位导航方法；
具体包括如下步骤：
将经过滤波和消除运动畸变的点云，转换为临时八叉树地图，消除临时八叉树地图中的地面区域，获得局部八叉树地图；
根据步骤3所得高低频组合里程计实时地将所得局部八叉树地图接到世界坐标系，获得到全局八叉树地图；
根据移动机器人相对地面的高度，对所得全局八叉树地图进行二维投影，获得包含三维信息的全局二维栅格地图，得到的包含三维信息的全局二维栅格地图将高于地面且低于移动机器人的障碍物对应的栅格位置为占据状态；
根据移动机器人的水平方向最大横截面的外接圆半径r，对包含三维信息的全局二维栅格地图，设置大小为2r的膨胀系数，然后将全局二维栅格地图中占据状态栅格向外扩张个栅格，其中a为每个栅格的边长，符号/>表示向上取整，获得包含三维信息和膨胀层的全局二维栅格地图。
2.根据权利要求1所述的一种适用于复杂三维环境的定位导航方法，其特征在于，步骤2中，对激光雷达输出的原始点云进行预处理，获得经过滤波和消除运动畸变的点云，其具体步骤如下：
对激光雷达输出的原始点云进行统计滤波、体素网格滤波和点云反射强度滤波，获得经过滤波的点云；基于线性插值方法，对所得经过滤波的点云中的每个点进行空间位置修正，获得经过滤波和消除运动畸变的点云。
3.根据权利要求2所述的一种适用于复杂三维环境的定位导航方法，其特征在于，基于线性插值方法，对获得的经过滤波的点云中的每个点进行空间位置修正，获得消除运动畸变的点云，包括如下操作：
对经过滤波的点云Pk，在激光雷达实际测量过程中的开始时间tk-1和结束时间tk组成的单位时间间隔[tk-1，tk]中，根据在上一时刻tk-1时激光雷达的位姿Tk-1和tk-2时刻激光雷达的位姿Tk-2线性地估算[tk-1，tk]时间内时刻激光雷达的位姿/>根据/>和/>之间的位姿变换将每个/>时刻的点/>全部转换到tk时刻的参考坐标系，得到初步消除运动畸变的点云消除因为运动带来的观测结果漂移，使得每个点/>转换到tk时刻的参考坐标系，等通过点云匹配得到准确的位姿Tk后，在进行一次位姿线性插值获得更加准确的点云Pk，获得消除运动畸变的点云。
4.根据权利要求1所述的一种适用于复杂三维环境的定位导航方法，其特征在于，步骤3.2中，构建边缘特征点云和平面特征点云的具体步骤如下：
根据激光扫描的先后顺序，对于带有颜色信息的点云的点i，选取其前后十个点，点i不包括带有颜色信息的点云中前面五个和后面五个点，计算点i的曲率ci的值；
将带有颜色信息的点云的点根据曲率ci大小进行排序，曲率最大的20点提取为边缘特征点，并将这20个边缘特征点构建为边缘特征点云，曲率最小的20点提取为平面特征点，并将这20个平面特征点构建为平面特征点云。
5.根据权利要求1所述的一种适用于复杂三维环境的定位导航方法，其特征在于，步骤3.3中，ICP点云匹配算法的步骤如下：
提高颜色信息相同的边缘特征点和平面特征点的权重，降低颜色信息不同的边缘特征点和平面特征点的权重，使得颜色信息融入到点云自身几何信息，构建颜色信息约束的ICP点云匹配算法；
在进行边缘特征点云和边缘特征点云地图的匹配时，将边缘特征点云地图转换为KD-tree的形式保存在计算机内存中，利用KD-tree查找边缘特征点云中的点在边缘特征点云地图中的对应点；
在进行平面特征点和平面特征点地图的匹配时，将平面特征点地图转换为KD-tree的形式保存在计算机内存中，利用KD-tree查找平面特征点中的点在平面特征点地图中的对应点。
6.根据权利要求1所述的一种适用于复杂三维环境的定位导航方法，其特征在于，步骤3.4中，获得高低频组合里程计，包括如下操作：
当接收到低频激光雷达里程计后，将低频激光雷达里程计的位姿信息按照接收到时间戳***到高频视觉里程计对应到的时间戳位置，更新高低频组合里程计的位姿信息，得到高低频组合里程计。
7.根据权利要求1所述的一种适用于复杂三维环境的定位导航方法，其特征在于，基于获得的包含三维信息和膨胀层的全局二维栅格地图，执行导航流程，具体步骤如下：
a、输入包含三维信息和膨胀层的全局二维栅格地图，并进行数据初始化；
b、使用A*算法对移动机器人在包含三维信息和膨胀层的全局二维栅格地图进行全局路径规划；
c、根据经过滤波和消除运动畸变的点云，使用DWS算法进行局部路径规划，使移动机器人可以动态地避开障碍物。
8.根据权利要求7所述的一种适用于复杂三维环境的定位导航方法，其特征在于，当全局路径规划失败或者局部路径规划超时，移动机器人就会制动停止运动并进入恢复行为模式，然后重新进行全局路径规划；
当局部路径规划失败但是未超时，移动机器人将会制动停止运动，然后重新进行全局路径规划。
CN202110457458.XA 2021-04-27 2021-04-27 一种适用于复杂三维环境的定位导航方法 Active CN113269837B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110457458.XA CN113269837B (zh) 2021-04-27 2021-04-27 一种适用于复杂三维环境的定位导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110457458.XA CN113269837B (zh) 2021-04-27 2021-04-27 一种适用于复杂三维环境的定位导航方法

Publications (2)

Publication Number Publication Date
CN113269837A CN113269837A (zh) 2021-08-17
CN113269837B true CN113269837B (zh) 2023-08-18

Family

ID=77229467

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110457458.XA Active CN113269837B (zh) 2021-04-27 2021-04-27 一种适用于复杂三维环境的定位导航方法

Country Status (1)

Country Link
CN (1) CN113269837B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114399587B (zh) * 2021-12-20 2022-11-11 禾多科技（北京）有限公司 三维车道线生成方法、装置、电子设备和计算机可读介质
CN114353799B (zh) * 2021-12-30 2023-09-05 武汉大学 搭载多线激光雷达的无人驾驶平台室内快速全局定位方法
CN114594761B (zh) * 2022-01-05 2023-03-24 美的集团(上海)有限公司 机器人的路径规划方法、电子设备及计算机可读存储介质
CN114372914B (zh) * 2022-01-12 2022-09-13 吉林大学 应用于矿用电铲上的机械式激光雷达点云预处理方法
CN114474061B (zh) * 2022-02-17 2023-08-04 新疆大学 基于云服务的机器人多传感器融合定位导航***及方法
CN114413896A (zh) * 2022-02-25 2022-04-29 陕西弘毅正清智能科技有限公司 一种移动机器人的复合导航方法、装置、设备及存储介质
CN114577214B (zh) * 2022-03-02 2022-09-20 哈尔滨工业大学 一种应用于跨异构多层空间的轮式机器人路径规划方法
CN115019167B (zh) * 2022-05-26 2023-11-07 中国电信股份有限公司 基于移动终端的融合定位方法、***、设备及存储介质
CN116448118B (zh) * 2023-04-17 2023-10-31 深圳市华辰信科电子有限公司 一种扫地机器人的工作路径优化方法和装置
CN116597074A (zh) * 2023-04-18 2023-08-15 五八智能科技(杭州)有限公司 一种多传感器信息融合的方法、***、装置和介质
CN117369480B (zh) * 2023-12-05 2024-02-23 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与***

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015024407A1 (zh) * 2013-08-19 2015-02-26 国家电网公司 基于电力机器人的双目视觉导航***及方法
WO2019127347A1 (zh) * 2017-12-29 2019-07-04 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN112014857A (zh) * 2020-08-31 2020-12-01 上海宇航***工程研究所 用于智能巡检的三维激光雷达定位导航方法及巡检机器人

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015024407A1 (zh) * 2013-08-19 2015-02-26 国家电网公司 基于电力机器人的双目视觉导航***及方法
WO2019127347A1 (zh) * 2017-12-29 2019-07-04 深圳前海达闼云端智能科技有限公司 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN112014857A (zh) * 2020-08-31 2020-12-01 上海宇航***工程研究所 用于智能巡检的三维激光雷达定位导航方法及巡检机器人

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于数据融合的SLAM***研究与路径规划实现;赖秋玲等;《电脑知识与技术》;20171125(第33期);全文 *

Also Published As

Publication number Publication date
CN113269837A (zh) 2021-08-17

Similar Documents

Publication Publication Date Title
CN113269837B (zh) 一种适用于复杂三维环境的定位导航方法
CN110596683B (zh) 一种多组激光雷达外参标定***及其方法
Heng et al. Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system
AU2022201558A1 (en) Pose estimation method and device, related equipment and storage medium
CN111060924B (zh) 一种slam与目标跟踪方法
CN103413352A (zh) 基于rgbd多传感器融合的场景三维重建方法
CN112461210B (zh) 一种空地协同建筑测绘机器人***及其测绘方法
CN113903011B (zh) 一种适用于室内停车场的语义地图构建及定位方法
JP7440005B2 (ja) 高精細地図の作成方法、装置、デバイス及びコンピュータプログラム
CN111521195B (zh) 一种智能机器人
CN112799096B (zh) 基于低成本车载二维激光雷达的地图构建方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN112859110B (zh) 一种基于三维激光雷达的定位导航方法
CN111207753A (zh) 一种多玻璃隔断环境下的同时定位与建图的方法
Agrawal et al. PCE-SLAM: A real-time simultaneous localization and mapping using LiDAR data
CN113724387A (zh) 一种激光与相机融合的地图构建方法
CN116608873A (zh) 一种自动驾驶车辆的多传感器融合定位建图方法
CN109636897B (zh) 一种基于改进RGB-D SLAM的Octomap优化方法
CN115046543A (zh) 一种基于多传感器的组合导航方法及***
CN116698014A (zh) 一种基于多机器人激光slam和视觉slam地图融合与拼接方法
CN116380039A (zh) 一种基于固态激光雷达和点云地图的移动机器人导航***
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
Velat et al. Vision based vehicle localization for autonomous navigation
CN112505723A (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