CN111364549B

CN111364549B - 一种基于激光雷达的同步建图和自动作业方法及系统

Info

Publication number: CN111364549B
Application number: CN202010126902.5A
Authority: CN
Inventors: 赵斌; 艾云峰; 杨超; 高娇
Original assignee: Jiangsu XCMG Construction Machinery Institute Co Ltd
Current assignee: Jiangsu XCMG Construction Machinery Institute Co Ltd
Priority date: 2020-02-28
Filing date: 2020-02-28
Publication date: 2021-11-09
Anticipated expiration: 2040-02-28
Also published as: CN111364549A

Abstract

本发明公开了一种基于激光雷达的同步建图和自动作业方法及系统，建图和自动作业同时进行，互相依赖，互相促进；自动作业系统控制挖掘机工作装置（回转、动臂、斗杆、铲斗等）运动，安装在挖掘机上的激光雷达、角度传感器等会实时采集，结合特殊数据处理逻辑，获得具有显著优势的环境感知信息，同步建立地图，同步建图是自动作业的前提条件，其优质性对自动作业有根本影响。在生成的环境地图中提取作业面和作业区域物体信息，提供给自动作业系统，辅助其系统做出决策和规划，完成自动作业流程。采用本发明，能够实现作业面检测、作业区域内物体检测，同时根据同步建图的地图信息，也可以实现挖掘机械的自动作业控制。

Description

一种基于激光雷达的同步建图和自动作业方法及系统

技术领域

本发明属于自动作业控制技术领域，涉及一种基于激光雷达的同步建图和自动作业方法及系统。

背景技术

工程机械的智能化甚至无人化是目前发展的趋势，如何能够通过自身传感器感知周围环境，并利用这些信息辅助或者代替人作业，其作业的安全性、生产效率都能够获得很大提升。

中国专利CN104090279A，设计了一种基于激光雷达的挖掘作业面检测装置，通过步进电机带动一线激光雷达在俯仰方向±45°范围内做匀速运动，将一线激光雷达的二维扫描转换为三维扫描，实现一线激光雷达俯仰运动控制、挖掘作业面扫描和三维重构功能。

中国专利CN104476548A提出了一个完整的自动化挖掘作业系统，使用立体激光雷达进行挖掘作业面检测，使用操控盒将工业机器人示教和再现功能与挖掘机器人自主作业控制系统进行结合，下位机采用自调整模糊PID控制技术。

现有技术存在以下缺陷：中国专利CN104090279A和中国专利CN104476548A(为同一个单位申请，是同一套控制系统)的缺点之一在于需要俯仰机构配合单线雷达进行扫描，需要将二维转换为三维，效率有待提高；其缺点之二在于需要等到检测完毕后才能够进行三维重建，并不是同时建图。

发明内容

目的：为了克服现有技术中存在的不足，本发明提供一种基于激光雷达的同步建图和自动作业方法及系统。采用本发明，能够实现作业面检测、作业区域内物体检测，同时根据同步建图的地图信息，也可以实现挖掘机械的自动作业控制，满足恶劣环境及不适宜有人参与情况下挖掘作业需求。

技术方案：为解决上述技术问题，本发明采用的技术方案为：

第一方面，提供一种基于激光雷达的同步建图和自动作业系统，包括工作装置角度传感器、回转角度传感器、多线激光雷达、主控计算装置；

所述工作装置角度传感器，安装在工作装置动臂、斗杆、铲斗上，用于检测挖掘机工作装置动臂、斗杆、铲斗相对于水平面的角度，通过计算得到动臂、斗杆、铲斗位置姿态信息；

所述回转角度传感器，用于检测挖掘机车体在水平面的旋转角度信息；

所述多线激光雷达，用于检测挖掘机周围作业环境信息；

所述主控计算装置，包括：

位姿计算模块，用于获取角度传感器信息，计算激光雷达的位姿和挖掘机工作装置位姿；

环境建图模块，用于接收多线激光雷达数据，经过数据处理建立环境地图，并感知得到作业面以及在作业面内的物体；

自动作业控制模块：根据位置姿态信息、工作装置环境地图和作业面以及在作业面内的物体，做出决策及规划，实现挖掘机自动作业。

在一些实施例中，所述多线激光雷达，其中至少一个激光雷达安装在驾驶室顶部，用于感知车体360度范围内环境信息，扫描面平行于地面，用于对车体周围360度范围内的物体进行感知；至少两个对称安装在挖掘机动臂左右侧面，扫描面均垂直地面，用于对局部作业面环境信息进行感知。

在一些实施例中，所述环境建图模块，用于接收多线激光雷达数据，经过数据处理建立环境地图，并感知得到作业面以及在作业面内的物体；包括：

接收多线激光雷达数据；利用位于驾驶室顶的激光雷达的点云数据，生成车体周围360度范围内物体感知信息；利用安装在挖掘机动臂两侧的激光雷达接收到的点云数据，生成局部作业面感知信息；

在挖掘机运动过程中，将位于挖掘机动臂两侧的激光雷达接收到的点云数据利用后续位姿计算的结果转换到车体坐标系下，并存储；

将转换后的点云和历史存储的点云拼接成一幅“大范围的点云”；

删除陈旧的和冗余的点云信息，维护一个“有效的大范围点云”，即环境地图，用来表征周围的实际环境；

从环境地图中感知作业面以及在作业面内的物体。

第二方面，提供一种基于激光雷达的同步建图和自动作业方法，包括：

获取多线激光雷达数据、工作装置角度传感器和回转角度传感器的信息；

根据工作装置角度传感器和回转角度传感器的信息，结合预先定义的挖掘机工作装置运动学模型，计算多线激光雷达相对于车身的实时位置姿态信息和挖掘机工作装置关节相对于车身的位姿信息；

根据多线激光雷达数据，生成激光点云环境地图，感知得到作业面以及在作业面内的物体；

根据位姿信息、环境地图、作业面以及在作业面内的物体信息，规划作业路线及动作。

在一些实施例中，根据多线激光雷达数据，生成激光点云环境地图，感知得到作业面以及在作业面内的物体；包括：

根据局部作业面感知信息，判断有无影响挖掘机自动作业的障碍物，如果没有，开始自动作业；

删除陈旧的和冗余的点云信息，维护一个“有效的大范围点云”，用来表征周围的实际环境；

根据“有效的大范围点云”，提取得到作业面以及在作业面内的物体。

在一些实施例中，所述的基于激光雷达的同步建图和自动作业方法，运用运动学正解算法计算激光雷达传感器相对于车体坐标系的位置姿态，并基于此将激光雷达点云信息转换到车体坐标系下。

在一些实施例中，在挖掘机运动过程中，随着车体的回转、动臂的升降运动，安装在挖掘机动臂左右两侧的两个激光雷达能够获得更大的感知范围，具体实现为，所有历史点云信息进行拼接，形成“粗”环境点云地图；

去除冗余点和无效点，每一个“方向”上只保留最新的点云信息，从而得到“有效的大范围点云”信息；其中一个“方向”指从车体坐标系引出一条射线的方向。

在一些实施例中，根据“有效的大范围点云”，提取得到作业面以及在作业面内的物体，包括：地面去除、利用聚类算法感知物体、对聚类出的点云提取并识别作业面和作业区域的物体。

在一些实施例中，对聚类出的点云提取并识别作业面和作业区域的物体，包括：

(1)对每一个聚类的点云进行栅格化，每一个栅格保留两个信息：栅格的几何中心坐标，该栅格内点云的最高点的高度；其中，栅格大小为可调参数；

(2)对每一个聚类对应的栅格的边界部分的栅格计算其平均高度和高度的方差，并设置高度阈值和方差阈值，如果聚类对应栅格的平均高度高于高度阈值，并且方差小于方差阈值，则判定为作业区域的物体，否则判定为作业面。

有益效果：本发明提供的一种基于激光雷达的同步建图和自动作业方法及系统，建图和自动作业同时进行，互相依赖，互相促进。自动作业系统控制挖掘机工作装置(回转、动臂、斗杆、铲斗等)运动，安装在挖掘机上的激光雷达、角度传感器等会实时采集，结合特殊数据处理逻辑，获得具有显著优势的环境感知信息，同步建立地图，同步建图是自动作业的前提条件，其优质性对自动作业有根本影响。另外，本发明在生成的环境地图中提取作业面和作业区域物体信息，提供给自动作业系统，辅助其系统做出决策和规划，完成自动作业流程。具有以下优点：(1)感知范围大，有效减少感知盲区：挖掘机动臂下方是感知盲区，通过在动臂左右两侧安装多线激光雷达，结合作业时挖掘机动臂随车体360度回转以及升降等运动，能够获取360度范围内的多线立体环境信息，精确检测作业面，减少盲区；

(2)同时建图，信息提供更加及时、准确：动臂两侧的两台多线激光雷达提供的数据量更加丰富，据此建立的周围环境信息电子地图更加精准，有助于自动作业系统决策、规划。

附图说明

图1为本发明实施例的系统模块框图和基本原理；

图2为本发明车体坐标系以及传感器安装示意图；

图3为本发明激光雷达监测到点云转换到车体坐标系示意图(以左侧激光雷达为例)；

图4为本发明通过旋转获得大范围点云以及去除无效点云示意图(以左侧激光雷达为例)；

图5为本发明作业面和作业区域的物体识别流程；

图6为本发明工作流程。

具体实施方式

下面结合具体实施例对本发明作更进一步的说明。相关术语定义如下：

地图---施工作业工程机械360度范围内的局部环境信息。

建图---创建上述地图的过程。

点云---或者叫做激光点云，是由激光雷达感知到的数据，可以表征周围环境的物体分布。

扫描面---激光雷达一个光束旋转一周形成的平面。多线激光雷达能获得一个扫描面的集合。

车体坐标系---为了相关度量角度和距离信息，本发明定义的坐标系统。

正解---已知类机械臂机构各个关节的位置，计算其终端位置的过程或者算法。

位姿---位置姿态，是运动学正解的结果，如无特殊说明，本文中位姿一律基于“车体坐标系”。

实施例1

结合图1，一种基于激光雷达的同步建图和自动作业系统，至少包括工作装置角度传感器(动臂、斗杆、铲斗各一个)、回转角度传感器(一个)、多线激光雷达(三个)、主控计算装置。

所述工作装置角度传感器，安装在工作装置上(其中动臂、斗杆、铲斗各一个)，用于检测挖掘机工作装置(动臂、斗杆、铲斗)相对于水平面的角度，通过计算得到其位置姿态信息。

所述回转角度传感器，用于检测挖掘机车体在水平面的旋转角度信息。

所述多线激光雷达，用于检测挖掘机周围作业环境信息。其中一个安装在驾驶室顶部，用于感知车体360度范围内环境信息，扫描面平行于地面，主要作用是对车体周围360度范围内的物体进行感知；另外两个对称安装在挖掘机动臂左右侧面，扫描面均垂直地面，主要作用是对局部作业面环境信息进行感知，尤其是用以检测前方被挖掘机动臂遮挡部分的盲区。通过合理布置三个多线激光雷达的位置，一方面保证能够感知到车体周围360度范围的物体信息，防止在进行自动作业的回转时发生碰撞；另一方面保证精确感知施工作业区域，识别施工作业面以及在作业面内的物体。

安装在驾驶室顶部的激光雷达能够获取车体周围360度范围内的点云信息，可以在自动作业流程开始前判断周围是否有物体影响挖掘机的自动作业，防止在进行自动作业的回转时发生碰撞；对称安装在挖掘机动臂左右侧面的两个激光雷达能够保证精确感知施工作业区域，判断施工作业面以及在作业面内的物体。开始自动作业时，挖掘机首先对前方作业区进行施工作业，启动后借助挖掘机车体本身的回转，安装在动臂两侧的激光雷达可以获得回转后更大范围的环境信息。

所述主控计算装置，主要有三个作用：

(1)位姿计算：接收角度传感器信息，计算激光雷达的位姿和挖掘机工作装置位姿，用于其他模块内部处理。

(2)环境建图：

a)接收两个多线激光雷达数据；

b)在挖掘机运动过程中，将接收到的点云数据利用后续位姿计算的结果转换到车体坐标系下，并存储；

c)将转换后的点云和历史存储的点云拼接成一幅“大范围的点云”；

d)及时删除陈旧的和冗余的点云信息，维护一个“有效的大范围点云”，用来表征周围的实际环境；

e)从上述获得的点云中感知挖掘区域和卸载区域。

(3)自动作业控制：接收建图和区域检测的结果，做出决策及规划，实现挖掘机自动作业。

需要说明的是，位姿计算，需要首先定义挖掘机运动学模型，包括挖掘机“车体坐标系”、挖机工作装置尺寸信息(动臂，斗杆，铲斗的长度)；其次获取从工作装置角度传感器和回转角度传感器实时输入的挖掘机工作装置和回转的角度信息；基于上述信息，利用运动学正解算法，计算多线激光雷达和挖掘机工作装置各关节和关键点相对于车身的位姿。

关于激光雷达的安装位置，其中一个要保证安装在驾驶室顶部，扫描面与地面平行；另外两个在动臂左右两侧对称安装，扫描面垂直地面，具体位置可根据实际需要和效果调整；

建图中，为了获得周围作业环境的大范围点云信息，需要从激光雷达输入实时激光点云信息，利用上述获取的激光雷达传感器的位姿，将激光点云映射到“车体坐标系”下；然后进行点云拼接：在挖掘机动臂随着车体回转及升降等运动过程中，不断地进行采集和拼接的过程，将这些点云拼接，形成“粗”环境点云地图。

建图中，为了对环境点云地图滤波，需要及时从获得的周围作业环境的大范围点云信息中删除无效点云信息；因为在不断地作业过程中，周围环境变化后，点云信息变化，不允许变换后的点云和变化前的点云同时存在，由此得到的地图为“精”激光点云地图。

为了区分作业面、作业区域内物体等信息，根据同步建图的信息，基于“精”激光点云地图，需要首先进行物体感知，其中涉及地面去除，物体聚类；其次利用聚类范围内的点云判定某个聚类的种类属性(作业面、作业区域内物体等信息)。

结合图2，本发明采用三个激光雷达，其中一个激光雷达安装在驾驶室顶部，用于感知车体360度范围内环境信息，扫描面平行于地面，主要作用是对车体周围360度范围内的物体进行感知；另外两个对称安装在挖掘机动臂左右侧面，扫描面均垂直地面，主要作用是对局部作业面环境信息进行感知，尤其是用以检测前方被挖掘机动臂遮挡部分的盲区，将实时扫描得到的点云数据发送给主控计算装置。另外，只标注了动臂上的角度传感器，本专利所述自自动作业还需要测量斗杆和铲斗的角度传感器，在图中没有标出。如图2，采用对称传感器的目的是保证挖掘机动臂在随车体转动过程中，不论往哪个方向转动，系统不仅能够感知到挖掘机动臂在随车体转动经过的区域，还能够在转动方向上总是先于铲斗位置感知到环境信息。图中同时展现了激光雷达扫面线的俯视图和侧视图。本发明在挖掘机动臂上安装角度传感器，并且在中心回转体上安装有角度传感器，用来测量车体回转角度。本发明采用的“车体坐标系”如图2所示，相关几何参数也在图2中标出。基于该坐标系，利用运动学正解，能够计算激光雷达相对于原点O₀的位置姿态，左侧激光雷达位置为：T_L＝(l₁c₁c₀-l₂s₀,l₁c₁s₀+l₂c₀,l₁s₁)^T，右侧激光雷达位置为：T_R＝(l₁c₁c₀+l₂s₀,l₁c₁s₀-l₂c₀,l₁s₁)^T。角度传感器可以采用惯性测量器件，或者长度传感器等方案，某一点的位置可以通过常规运动学正解方法计算。

结合图3，基于激光雷达相对于车辆坐标系的位置，本发明将激光雷达采集到的点云信息统一到“车体坐标系”下，从而得到统一到车体坐标系的点云：P_LR＝T_LP_L+T_RP_R，其中，P_L和P_R分别为左右激光雷达监测到的点云。图中，以左侧激光雷达监测到位于弧AB上的点云，这些点云等效于被从车体坐标系原点O₀发出的激光扫描线监测到。

结合图4，挖掘机动臂随着车体的转动，扩大激光雷达的感知范围。将统一到车体坐标系的实时点云和所有历史点云拼接，得到“粗”环境点云地图：P_big＝Σ_tP_LR(t)，其中P_LR(t)为t时刻监测到的点云信息。图4中，弧AB上的点云和弧CA上的点云分别为车体不同旋转角度监测到的点云，这两幅点云拼接后得到更大监测视角的点云信息。然而，机械地不断进行点云拼接，会带来两个问题，一个是激光雷达重复感知的区域如果发生变化，真实的点云信息随之变化，而这个地方将会有两个点云集合对应，因此，应该只保留变化后的点云信息；另外一个问题是，上下帧点云信息会有重复覆盖区域，考虑到系统效率，只应该保留一份点云信息。在机械地拼接后，每一个“方向”会有多个点云信息，本系统比较这些点云的感知时间，只保留最新的点云信息。一个“方向”指从车体坐标系引出一条射线的方向。比如，图4中，射线O₀Q方向上存在三个点云点Q₁，Q₂，Q₃，应该只保留时间最新的点云点。经过这个处理，得到“精”环境点云地图。

结合图5，本发明对“精”环境点云地图进行物体感知，感知结果是属于不同物体的子点云，然后判断每一个聚类是作业面还是作业区域内的物体。以下为步骤：

(1)地面去除，将小于“高度阈值”的点云去除，得到“非地面点”，高度阈值为可调参数，比如，设为0.1m；

(2)对这些点进行聚类，比如用欧式聚类算法，得到属于不同物体的子点云；

(3)对聚类后的每一个点云，判断其为作业面(例如土堆)还是作业区域内的物体(例如矿卡)。判断方法为：首先，对聚类点云栅格化，即，用一幅网格覆盖整个点云，每一个格子大小是可以调整参数，比如，10cm*10cm，取这个小范围内圈住点云的最高点高度作为该栅格的高度值；那么，栅格化的结果为，栅格中心点的坐标以及一个高度值；然后，计算每一个聚类对应栅格的边界栅格的高度值的方差和平均值；最后，判断如果平均高度值高于“高度阈值”，并且，方差小于“方差阈值”，那么判定为物体，否则为作业面。“高度阈值”设置为物体高度的80％左右，方差为0.3m。这个特征能够显著地将物体和作业面分出来。图5给出了这个流程的大致示意图，环境中存在两个物体，一个为作业面(例如土堆)，一个为物体(例如矿卡)，经过上述流程，感知到作业面还是作业区域内的物体。

实施例2

结合图6，一种基于激光雷达的同步建图和自动作业方法，包括：

(1)挖掘机上电启动，开机后进行系统初始化和自诊断；

(2)主控计算装置实时获取工作装置角度传感器和回转角度传感器的信息；

(3)基于上述角度信息和预先定义的挖掘机工作装置运动学模型(包括动臂，斗杆，铲斗的几何结构参数和“车体坐标系”)，计算多线激光雷达相对于车身的实时位置姿态信息和挖掘机工作装置关节相对于车身的位姿信息；

(4)主控计算装置实时获取多线激光雷达采集的点云信息；利用位于驾驶室顶的激光雷达的点云数据，生成车体周围360度范围内物体感知信息；利用安装在挖掘机动臂两侧的激光雷达接收到的点云数据，生成局部作业面感知信息；

(5)主控计算装置将实时点云统一到“车体坐标系”并存储；

(6)根据当前检测到的激光点云地图，区分作业面、作业区域内物体等信息和挖掘机位姿，判断有无影响挖掘机自动作业的障碍物，做出决策、规划，开始进行自动作业。

(7)自动作业过程中，挖掘机产生回转运动，主控计算装置将挖掘机动臂随着车体回转及升降等运动过程中获得的统一到“车体坐标系”的点云和历史存储点云拼接，获得“粗”激光点云地图；

(8)主控计算装置及时去除大范围点云中无效的点云，获得“精”激光点云地图；

(9)主控计算装置在“精”激光点云地图中感知物体，区分作业面、作业区域内物体；其中，依次涉及地面去除，聚类，栅格化等处理。

(10)利用得到的“精”激光点云地图、作业面、作业区域内物体等信息和挖掘机位姿，继续计算，做出决策、规划，实现同步建图与自动作业。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种基于激光雷达的同步建图和自动作业系统，其特征在于，包括工作装置角度传感器、回转角度传感器、多线激光雷达、主控计算装置；

所述多线激光雷达，用于检测挖掘机周围作业环境信息；

所述主控计算装置，包括：

环境建图模块，用于接收多线激光雷达数据，经过数据处理建立环境地图，并感知得到作业面以及在作业面内的物体；包括：

从环境地图中感知作业面以及在作业面内的物体；

2.根据权利要求1所述的基于激光雷达的同步建图和自动作业系统，其特征在于，所述多线激光雷达，其中至少一个激光雷达安装在驾驶室顶部，用于感知车体360度范围内环境信息，扫描面平行于地面，用于对车体周围360度范围内的物体进行感知；至少两个对称安装在挖掘机动臂左右侧面，扫描面均垂直地面，用于对局部作业面环境信息进行感知。

3.一种基于激光雷达的同步建图和自动作业方法，其特征在于，包括：

根据多线激光雷达数据，生成激光点云环境地图，感知得到作业面以及在作业面内的物体；包括：

根据“有效的大范围点云”，提取得到作业面以及在作业面内的物体；

4.根据权利要求3所述的基于激光雷达的同步建图和自动作业方法，其特征在于，运用运动学正解算法计算激光雷达传感器相对于车体坐标系的位置姿态，并基于此将激光雷达点云信息转换到车体坐标系下。

5.根据权利要求3所述的基于激光雷达的同步建图和自动作业方法，其特征在于，在挖掘机运动过程中，随着车体的回转、动臂的升降运动，安装在挖掘机动臂左右两侧的两个激光雷达能够获得更大的感知范围，具体实现为，所有历史点云信息进行拼接，形成“粗”环境点云地图；

6.根据权利要求3所述的基于激光雷达的同步建图和自动作业方法，其特征在于，根据“有效的大范围点云”，提取得到作业面以及在作业面内的物体，包括：地面去除、利用聚类算法感知物体、对聚类出的点云提取并识别作业面和作业区域的物体。

7.根据权利要求6所述的基于激光雷达的同步建图和自动作业方法，其特征在于，对聚类出的点云提取并识别作业面和作业区域的物体，包括：

（1）对每一个聚类的点云进行栅格化，每一个栅格保留两个信息：栅格的几何中心坐标，该栅格内点云的最高点的高度；其中，栅格大小为可调参数；

（2）对每一个聚类对应的栅格的边界部分的栅格计算其平均高度和高度的方差，并设置高度阈值和方差阈值，如果聚类对应栅格的平均高度高于高度阈值，并且方差小于方差阈值，则判定为作业区域的物体，否则判定为作业面。