基于三维点云地图和二维栅格地图的导航系统及应用方法

摘要

本发明公开了一种基于三维点云地图和二维栅格地图的导航系统及应用方法，包括：用于机器人作业任务发布和运行管理的上层应用平台单元；用于加载三维点云地图数据，映射二维导航地图的地图加载与映射单元；用于构建三维点云模型数据，配准定位机器人位置的定位单元；导航控制单元：用于将获取到的机器人当前定位位姿、导航执行速度与转角、任务执行状态反馈给上层应用平台单元的状态与信息反馈单元。本发明提供一种基于三维点云地图和二维栅格地图的导航系统及应用方法，将三维定位与二维导航的组合，提升导航的稳定性，在不过高提升机器人的处理单元性能条件下，机器人可实现更好更稳定的定位与导航，促进机器人的发展以及推广应用。

说明书

技术领域

本发明涉及人工智能的导航定位领域。更具体地说，本发明涉及一种用在机器人导航情况下的基于三维点云地图和二维栅格地图的导航系统及应用方法。

背景技术

随人工智能技术的发展，机器人逐步走进日常生活和工业生产中，无论是代替人作业，还是加入工业生产中提高生产线的智能化，发挥着重要的作用，成为很多行业急需的产品。目前机器人的定位导航系统多建立在二维空间模型下，主要使用单线激光雷达感知周围环境，利用其输出的点云数据进行配准定位，实现机器人定位导航。

现有的技术中，定位导航的二维地图利用单线激光雷达的点云数据进行构建，在遇到地势不平整，环境中存在动态移动或变化物体时，激光雷达感知输出的点云数据中环境特征变化大，地图在构建时，物体轮廓线变化，无法在地图中保留变化的信息，地图中只能保留某一个角度和方向下的环境特征信息，无法同三维的点云地图一样保留环境更多、更全的特征信息。机器人在自主运动时，不可能完全按照建图时的路线运动，激光雷达感知角度和方向一旦发生变化，此时点云数据中的环境特征可能与建图时点云数据中的环境特征不匹配，使得定位导航系统配准定位时误定位，导航出错。

现在机器人完全在三维模型下的定位和导航技术不成熟，并且定位和导航都建立在三维的模型下，对机器人处理单元的性能要求极高，一般性能的工控机性能远不能满足，使用高配置工控机或其他嵌入式板非低功耗处理器，性能的提升会伴随电量消耗高、散热增大等一系列问题，故应用范围受限。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现本发明的这些目的和其它优点，提供了一种基于三维点云地图和二维栅格地图的导航系统，包括：

用于机器人作业任务发布和运行管理的上层应用平台单元；

用于加载三维点云地图数据，映射二维导航地图的地图加载与映射单元；

用于构建三维点云模型数据，配准定位机器人位置的定位单元；

用于规划机器人运动路径，控制机器人移动的导航控制单元；

用于将获取到的机器人当前定位位姿、导航执行速度与转角、任务执行状态反馈给上层应用平台单元的状态与信息反馈单元。

一种导航系统的方法，基于三维点云的匹配技术，根据上层应用平台的任务活动区域，通过地图加载与映射单元将任务活动区域对应的三维点云地图信息映射到二维坐标系下的栅格导航地图中，作为机器人在非封闭式复杂环境下的导航信息。

优选的是，包括：

步骤一，通过多线激光雷达的三维点云数据构建对应的点云地图，基于点云地图数据标注生成矢量地图，构建对应的二维展示图；

步骤二，上层应用平台单元根据当前机器人所需要执行的作业任务，规划出机器人当前作业任务的活动范围区域并发送给地图加载与映射单元，同时将对应的目标任务点发送给导航控制单元；

步骤三，地图加载与映射单元加载点云地图和矢量地图，并基于上层应用平台单元给出的活动范围区域，通过坐标变换将活动范围区域的每个坐标点转换到矢量地图的坐标系下，以在矢量地图中直接找出对应的区域范围，结合点云地图的点云数据，投影映射出导航规划的二维栅格地图；

步骤四，地图加载与映射单元获取机器人的当前位姿，依照设定的定位匹配范围参数，提取当前位姿下所设定范围内的点云数据，为定位单元提供点云配准的模板数据；

步骤五，定位单元基于从多线激光雷达获取的实时点云数据，构建局部三维点云地图作为定位时的配准数据，在定位时将配准数据与模板数据进行配准，以得到机器人在点云地图中位置信息，实现机器人的定位；

步骤六，定位单元将机器人在点云地图中位置信息，通过坐标转换到矢量地图所在的坐标系下，再通过位置点变换投影到二维栅格地图中，完成地图映射；

步骤七，导航控制单元接收上层应用平台单元给出的目标任务点，在二维栅格导航地图中利用A星路径规划算法规划路径，利用状态反馈单元中里程计反馈的信息规划机器人运动；

步骤八，状态与信息反馈单元获取机器人当前的位置，导航执行的速度与转角，以及任务执行的状态给上层应用平台单元，上层应用平台根据反馈的信息判断机器人执行和作业的状态，管理作业任务的发布。

优选的是，在步骤一中，还包括对所述点云地图中的点云数据进行离线处理以去除当中的杂点，同时基于点云地图数据标注生成对应的矢量地图，构建上层应用平台单元所需要的二维展示图。

优选的是，在步骤二中，所述活动范围区域的规划方法被配置为包括：

步骤S20，提取所有需执行作业任务在二维展示图中所对应的坐标点，形成一个平面点的集N1；

步骤S21，对点集N1，使用Graham Scan凸包算法找到点集最外围的所有点，并将最外围的边界点按顺时针或逆时针排列，构建一个包裹所有点的多边形区域；

步骤S22：将所述多边形区域的边界点都替换成它在点云地图中标出的对应点，到得机器人活动范围区域。

优选的是，在步骤二中，上层应用平台单元活动范围区域设置机器人地图加载与映射单元中映射二维导航地图所需的参数；

其中，所述参数为围成多变形区域的边界点P1(x,y,z)到Pn(x,y,z)。

优选的是，在步骤三中，矢量地图与二维栅格地图的映射是基于矢量地图中语义信息，且映射规则包括：

将机器人行走区域的地面在栅格地图中映射为可通行区域；

将道路外或道路内超过机器人越障能力的物体映射成致命障碍物；

将其他道路外无点云信息区域映射成未知区域。

优选的是，在步骤四中，机器人初始时的位姿无法从定位单元获取，通过人工确定大致的位置和方向，通过上层应用平台设定给出，且位姿用三维坐标点(x,y,z)加四元数表示，设定的定位匹配范围参数为圆的半径R。

优选的是，在步骤五中，所述定位单元基于里程计的位移变化量信息，构建预定时间内的局部三维点云地图，且所述局部三维点云地图中的数据在随时间推移进行变化时，总是保留最近一段时间内的点云数据作为定位时的配准数据。

优选的是，在步骤五的定位中，所述定位单元根据状态与信息反馈单元里程计反馈的里程计信息，计算上一次定位时到当前时刻的里程变化范围、机器人航向角的变化范围，限制点云配准时空间方位位置及方向角度的范围不超出里程和航向变化范围，利用正态分布变换NDT算法将配准数据与模板数据进行配准，得到两者参考系坐标系之间的转换关系，以通过坐标变换得到机器人在点云地图中的位置关系，进而得出机器人在全局参考坐标系下的位姿关系，实现机器人的定位。

本发明至少包括以下有益效果：本发明主要解决机器人基于二维栅格地图定位导航的误定位几率大的问题，提高定位导航的稳定性；具体来说，本发明的导航定位系统因采用三维点云地图，可记录更多更复杂的环境信息，克服二维栅格定位地图记录信息的单一，使机器人在更多的环境下的可应用，提高机器人的环境适应能力，进一步将三维定位与二维导航的组合，在不过高提升机器人的处理单元性能条件下，机器人可实现更好更稳定的定位与导航，促进机器人的发展以及推广应用。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明的一个实施例中导航系统的框架结构示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明利用基于三维点云的匹配技术，在机器人定位导航系统中集成支持三维空间定位的模块，完成该模块完成机器人在复杂环境中的定位，同时根据上层应用平台的任务活动区域，基于三维点云地图和标注的矢量地图信息映射二维坐标系下的栅格导航地图，将三维定位与二维导航技术组合，从而实现机器人在非封闭式复杂环境下的稳定导航，解决普通定位方式的定位不稳定、降低应用场景的局限性。

在具体实施前，利用开源的LIO-SAM开源软件包，通过多线激光雷达的三维点云数据进行构建点云地图，构建完成后进行离线处理点云数据，去除当中的杂点，同时利用点云地图数据标注生成高精度的矢量地图，构建上层应用平台单元所用的展示图。

如图1所示，本发明提供一种三维定位与二维导航组合的机器人定位导航系统，包括：上层应用平台单元；用于机器人作业任务发布和运行管理；地图加载与映射单元；用于加载三维点云地图数据，提供定位配准的模板数据，映射二维导航地图；定位单元；用于构建短时间内局部建图的三维点云模型数据，配准定位机器人位置；导航控制单元；用于规划机器人运动路径，控制机器人移动；状态与信息反馈单元；反馈机器人当前的定位位姿，导航执行的速度与转角，任务执行的状态给上层应用平台单元。

上层应用平台单元根据当前机器人所需要执行的作业任务，规划出机器人的活动范围区域，平台通过规划的区域设置机器人地图加载与映射单元中映射二维导航地图所需的参数，参数为围成区域多变形的边界点P1(x,y,z)到Pn(x,y,z)，这些点都是由人工在三维点云地图中根据作业任务位置标注得到，再录入到平台系统使用。规划机器人活动范围区域方法为：

步骤S1：提取所有需执行作业任务在上层平台二维展示图中所对应的坐标点，形成一个平面点的集N1；

步骤S2：，对点集N1，使用Graham Scan凸包算法找到点集最外围的所有点，这些最外围的边界点按顺时针或逆时针排列，可构成一个包裹所有点多边形区域。

步骤S3：将S2步骤中得到的多边形区域的边界点都替换成它在点云地图中标出的对应点，此时所有的点构成的区域即为机器人活动范围区域。

地图加载与映射单元加载点云地图和矢量地图，由于高精度矢量地图是通过点云地图标注而来，他们之间存在坐标位置的直接转换关系，根据上层应用平台给出的机器人活动范围区域，通过两个坐标系下空间点的坐标变换，将活动范围区域的每个坐标点转换到矢量地图的坐标系下，在高精度矢量地图中直接找出对应的区域范围，再根据高精度矢量地图中语义信息，结合点云地图的点云数据，投影映射出导航规划的二维栅格地图，栅格地图为一个二维平面的地图。映射规则是：机器人行走区域的地面在栅格地图中映射为可通行区域，道路外或道路内超过机器人越障能力的物体映射成致命障碍物，其他道路外无点云信息区域映射成未知区域。

地图加载与映射单元获取机器人的当前位姿，依照设定的定位匹配范围参数，提取当前位姿下所设定范围内的点云数据，为定位单元提供点云配准的模板数据。初始时的位姿无法从定位单元获取，必须由人工确定大致的位置和方向，通过上层应用平台设定给出。位姿用三维坐标点(x,y,z)加四元数表示。设定的定位匹配范围参数为圆的半径R。

定位单元通过获取多线激光雷达实时的点云数据，利用里程计的位移变化量信息，构建一段时间内的局部三维点云地图，随时间推移，局部三维点云地图中的数据随之变化，总是保留最近一段时间内的点云数据，构建的局部三维点云地图作为定位时的配准数据。定位时，根据状态与信息反馈单元里程计反馈的里程计信息，计算上一次定位时到当前时刻的里程变化范围、机器人航向角的变化范围，限制点云配准时空间方位位置及方向角度的范围不超出里程和航向变化范围，利用开源PCL库中的NDT算法将配准数据与地图加载与映射单元给出的点云配准模板数据进行配准，得到两者参考系坐标系之间的转换关系，再通过坐标变换进而得出机器人在点云地图中位置关系，得出机器人在全局参考坐标系下的位姿关系，实现机器人的定位。

根据定位单元得到的机器人定位信息，即机器人在点云地图坐标系下的坐标点，利用在点云地图标注高精度矢量地图时这两者基于的坐标系之间所确定的坐标系关系，将机器人位置点变换到高精度矢量地图所在的坐标系下，再根据高精度矢量地图映射栅格地图时确定的这两者所在坐标系之间的坐标系关系，将机器人在高精度地图中的位置点变换投影到栅格地图中，这一过程实现定位单位的机器人实时定位位置映射到导航的栅格地图中，方便导航时在栅格地图中定位到机器人在地图中的位置。

导航控制单元接收上层应用平台单元给出的目标任务点，根据当前的定位位置，在二维栅格导航地图中利用A星路径规划算法规划路径，同时利用状态反馈单元中里程计反馈的信息，使用开源ROS系统中DWA局部规划算法规划机器人运动，这里的目标任务点是指根据每个作业任务人为设置机器人达到的目标地点，人为在二维展示地图中和点云地图中标注得到，点的数据格式为三维空间点坐标。

状态与信息反馈单元中的里程计通过接收导航控制单元给出的机器人运动控制量，结合机器人自身所带的轮速计、IMU传感器信息，估测实际的里程、运动执行速度与角速度、姿态，反馈给导航控制单元，同时也反馈给定位单元。

状态与信息反馈单元获取机器人当前的位置，导航执行的速度与转角，以及任务执行的状态给上层应用平台单元，上层应用平台根据反馈的信息判断机器人执行和作业的状态，管理作业任务的发布。

综上，本发明基于三维点云地图的定位，有效提高机器人的稳定性，可应对上下坡、不平整等地势，相比基于二维栅格地图定位受环境因素影响减小，适应环境的能力增强，应用的面增广，三维定位与二维导航的组合，在现有的一般机器人上能够快速的改造进行实现，提高机器人在市面上的应用，推动工业生产的智能化建设，创造增多的经济价值。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。