CN104090279A - 一种基于激光雷达的挖掘作业面检测装置 - Google Patents

Abstract

本发明提供了一种基于激光雷达的挖掘作业面检测装置。本装置需要实现一线激光雷达俯仰运动控制、挖掘作业面扫描和三维重构功能，其技术特点是：通过步进电机带动一线激光雷达在俯仰方向±45°范围内做匀速运动，将一线激光雷达的二维扫描转换为三维扫描。定时采集一线激光雷达的扫描数据，结合一线激光雷达的俯仰角度，经计算后得到检测装置坐标系下的挖掘作业面三维信息，然后将其转换到自主作业挖掘机器人坐标系。对挖掘作业面点云数据进行滤波去噪，采用三角剖分法进行三维重构处理，并采用不规则棱台法进行拟合，获取挖掘作业面数字三维信息，检测结果可通过以太网接口传输到挖掘机器人控制器，为挖掘效果评估和挖掘机器人轨迹规划提供依据。

Description

一种基于激光雷达的挖掘作业面检测装置

技术领域

本发明是用于挖掘机器人进行自主作业时对挖掘作业面进行检测的装置，具体地说，通过对挖掘作业面的检测，采集作业面三维点云数据，实现挖掘作业面的三维重构，确定已挖掘的深度、待挖掘面的形状和在挖掘机器人坐标系中的位置，为挖掘作业轨迹规划和挖掘效果评估提供依据。本发明不仅可用于挖掘机器人自主作业时的挖掘作业面检测，也可用于推土机、平地机等工程装备的作业面检测。

背景技术

自主作业挖掘机器人是一种智能化的军用工程机械，是作战行动中工程保障的重点装备，已成为目前军用工程机械领域的研究热点，国内外很多机构都在开展相关的研究，但目前国内的很多研究工作还处于在实验室进行理论研究和仿真实验阶段。要实现野外环境下真正的无人自主挖掘，对挖掘作业面进行实时检测和三维重构，是需要突破的关键技术之一。目前有很多研究机构使用CCD图像进行挖掘作业面的检测，但由于图像信息会受到很多其他因素的干扰，如天气环境、光照、阴影等，这些因素使得对环境的理解变得困难，导致图像处理的计算量变得很大，处理结果达不到预期效果。因此，迫切需要一种对挖掘作业面进行实时检测和三维重构的装置。

发明内容

为了解决挖掘作业面实时检测与三维重构问题，本发明提供一种基于激光雷达的挖掘作业面检测装置。

本发明的作业面检测装置由一线激光雷达、控制器、步进电机及驱动器、减速器、矩形架、接近开关、限位块、外壳等部分组成。本装置需要实现一线激光雷达俯仰运动控制、挖掘作业面扫描和三维重构功能。

本发明的挖掘作业面检测装置，其工作原理是：一线激光雷达安装在减速器负载端，通过步进电机带动一线激光雷达在俯仰方向±45°范围内做匀速运动，将二维扫描变成三维扫描。激光雷达定时对挖掘作业面进行扫描，通过步进电机的运动步数计算所处的俯仰角，扫描数据与俯仰角度实现时空配准，经过计算得到检测装置坐标系下的挖掘作业面三维点云数据。结合挖掘机器人工作装置的位姿信息，经D-H变换得到挖掘机器人坐标系下的挖掘作业面三维信息。采用棱台法对挖掘作业面进行三维重构，计算出已挖掘的深度、待挖掘面各处的三维信息，为自主作业挖掘机器人的实时轨迹规划提供依据，并可输出三维图形进行显示。

本发明的挖掘作业面检测装置，一线激光雷达运动控制的特征是：使用接近开关反馈信息确定一线激光雷达在俯仰方向运动的起始位置，每次上电时，由控制器控制步进电机寻找原点，此后的运动，都以原点为基准进行运动。通过对步进电机的运动步数进行计数来计算激光雷达所处的俯仰角，角度参数用于与激光雷达扫描数据实现时空对准。一线激光雷达俯仰方向±45°范围内为工作区，在工作区两侧各设置一个缓冲区，用于启动时的加速和结束运动时的减速，在工作区内为匀速运动。俯仰方向上运动范围的大小，还可以通过调整接近开关的位置进行调节。

本发明的挖掘作业面检测装置，其特征在于：使用的一线激光雷达扫描周期是25ms，检测距离0.1~30m，视角范围是270°。

本发明的挖掘作业面检测装置，进行挖掘作业面检测与三维重构的步骤如下：

（1）使用接近开关反馈信息确定一线激光雷达在俯仰方向运动的起始位置，控制器控制步进电机寻找原点，使一线激光雷达回到原点位置。

（2）接收挖掘机器人主控计算机输出信息，确定挖掘作业面检测装置在挖掘机坐标系下的位置。

（3）通过控制器控制一线激光雷达的俯仰运动，进行挖掘面扫描，采集扫描数据，并记录扫描位置。

（4）对扫描数据进行滤波去噪。

（5）利用一线激光雷达扫描数据结合检测装置在俯仰方向上的转角，进行采样数据坐标变换，先将其转换为挖掘面检测装置坐标系下的坐标值，再将其转换为挖掘机器人坐标系下的坐标值。

（6）判断检测过程是否完成，如果完成，则进行三维重构，否则继续进行扫描。

（7）利用挖掘作业面检测得到的挖掘作业面三维点云数据进行三维重构，采用三角剖分法构建挖掘作业面三维物理模型。

（8）对挖掘作业面三维物理模型进行六面分割。

（9）对分割出的挖掘作业面物理模型进行六面体拟合。

（10）计算拟合六面体的参数，求取挖掘面的相应尺寸。

（11）进行数据输出。

附图说明

图1是挖掘作业面检测装置组成框图；

图2是挖掘作业面检测装置侧视图；

图3是挖掘作业面检测装置外观示意图；

图4是挖掘作业面检测装置控制原理图；

图5是挖掘作业面检测与三维重构处理流程；

图中：1.限位杆    2.限位转接法兰     3.深沟球轴承     4.俯仰轴     5.一线激光雷达     6.矩形架     7.电机固定法兰     8.减速器     9.壳体     10.联轴器     11.电机固定法兰     12.步进电机     13.电机驱动器     14.橡胶条     15.侧盖     16.密封垫     17.底盖     18.电源转换板     19.计算机扩展底板     20.计算机主板     21.导热垫     22.橡胶垫     23.限位块     24.遮光板     25.接近开关     26.接近开关固定架     27.压线卡     28.对外接口     29.调试接口     30.安装螺孔。

具体实施方式

下面结合附图和实施例对本发明做进一步详述。

在图1中，一线激光雷达5安装在矩形架6内，矩形架6、减速器8、联轴器10、步进电机12、电机驱动器13依次连接，并通过电机固定法兰7和电机固定法兰11固定在壳体9内。电源转换板18通过安装螺柱固定在壳体9内部，与壳体9之间衬有导热垫21，利用壳体9表面进行散热。计算机扩展底板19通过安装螺柱固定在电源转换板18下方，计算机主板20安装在扩展底板上，用于一线激光雷达5的运动控制，数据的采集与处理。计算机发送控制信号到电机驱动器13，驱动步进电机12进行运动，带动安装在矩形架6内的一线激光雷达5进行俯仰运动。一线激光雷达5每隔25ms对其前方30m，270°范围进行一次扫描，与俯仰运动相结合，二维扫描变成三维扫描，实现对挖掘作业面的检测。用于一线激光雷达5俯仰运动控制范围限制的限位杆1安装在一线激光雷达5左侧，通过限位转接法兰2进行连接。为加强壳体的防水功能，在侧盖15四周安装有橡胶条14，底盖17内衬有密封垫16。

在图2中，限位块23嵌入到橡胶垫22中，通过螺钉固定到壳体内侧壁。接近开关25安装在接近开关固定架26上，通过螺钉固定到壳体内侧壁。遮光板24一端在两个接近开关之间，矩形架俯仰运动时带动遮光板24旋转，通过接近开关25确定矩形架运动的起止范围。系统上电时，控制步进电机寻找原点，此后都以原点为基准进行运动控制。在接近起点和终点的两侧各设置一个缓冲区，用于运动开始时的加速和结束时的减速，在中间的工作区范围内进行匀速运动。系统通过对步进电机的运动步数计数来计算一线激光雷达的俯仰角，并与扫描数据进行时空对准，确定空间位置。遮光板24另一端在两个限位块之间，通过限位块对其运动范围进行硬件保护。从电源转换板18引出的各设备供电线缆，从计算机主板20引出的控制线和数据线，使用压线卡27进行固定。对外接口28包含供电接口和以太网接口，系统使用时通过该进口对系统供电，并向外输出数据。调试接口29包含VGA、USB、RS232接口，在系统调试时通过它连接调试计算机、鼠标、键盘、显示器等设备。

在图3中，一线激光雷达5通过四个安装螺孔30固定在矩形架6内，矩形架6的运动范围与一线激光雷达5的运动范围完全一致。

在图4中，控制计算机连接到一线激光雷达5、电机驱动器13、接近开关25，进行控制信号发送和数据采集。对外接口28包含电源接口和以太网接口，通过电源接口为电源转换板18供电，经电源转换后为步进电机12、电机驱动器13、一线激光雷达5、接近开关25、计算机扩展底板19供电。通过以太网接口向外输出数据。调试接口29包含VGA、USB、RS232接口，在系统调试时通过它连接调试计算机、鼠标、键盘、显示器等设备。

图5所示为挖掘作业面检测与三维重构处理流程。利用本装置进行挖掘作业面扫描时，将本装置安装到挖掘机的斗杆中部下方。进行挖掘作业面扫描前，工作装置运动到被检测的挖掘作业面上方，控制斗杆处于合适的位置，使本装置扫描范围能够覆盖被检测作业面范围。进行挖掘作业面检测与三维重构的处理步骤如下：

1）如步骤32所示，使用接近开关反馈信息确定一线激光雷达在俯仰方向运动的起始位置，控制器控制步进电机寻找原点，使一线激光雷达回到原点位置。

2）如步骤33所示，接收挖掘机器人主控计算机输出信息，确定挖掘作业面检测装置在挖掘机坐标系下的位置。

3）如步骤34所示，通过控制器控制一线激光雷达的俯仰运动，进行挖掘面扫描，采集扫描数据，并记录扫描位置。根据扫描范围和扫描时间要求，激光雷达在俯仰方向每隔一定角度扫描一次，采集扫描数据。按照一线激光雷达定义的数据协议对接收到的数据进行解析，得到扫描点在一线激光雷达坐标系中的坐标                                                ，其中为扫描点距离一线激光雷达的距离，为扫描点与一线激光雷达坐标系原点连线与一线激光雷达坐标系轴的夹角。

4）如步骤35所示，对采样的扫描数据进行滤波去噪。

因内部电路噪声和外部环境的影响，激光雷达扫描数据会出现一定的噪声点，对后期数据处理产生较大的影响，需要进行噪声过滤处理。结合扫描数据的特点，选择采取最小近临算法对数据进行聚类分析。具体思路是：对处理数据进行遍历，计算出相邻数据间的距离，设定阈值，若两点间距离小于此阈值则将扫描点归为一类。聚类完成后，如果某一类中扫描点数目小于设定的点数门限值，则视其为噪声点，对其进行剔除。两点间距离的计算公式如下：

 　　　　　　　　　（1）

5）如步骤36所示，利用一线激光雷达扫描数据结合检测装置在俯仰方向上的转角，进行采样数据坐标变换，先将其转换为挖掘面检测装置坐标系下的坐标值，再将其转换为挖掘机器人坐标系下的坐标值。

转换过程基于三角形原理，计算公式如式（2）所示。

                                               （2）

为扫描点到一线激光雷达的距离，为俯仰角（步进电机当前转动角度），为扫描点与挖掘面检测装置坐标系X轴的夹角。

6）如步骤37所示，判断检测过程是否完成，如果没有完成，则返回到步骤34，继续进行挖掘面检测；如果检测完成，则进入到步骤38。

7）如步骤38所示，利用挖掘作业面检测得到的挖掘作业面三维点云数据进行三维重构，采用三角剖分法构建挖掘作业面三维物理模型。

由于扫描过程中逐层扫描的特点，得到的数据为规则数据，在三角剖分时充分考虑相邻扫描线间的内在位置关系，可大大提高三角剖分的速度与效率。本装置采用三角网生成法来完成对激光雷达点云数据的三角剖分，其基本思路是：先找出点集相距最短的两点连接成一条Delaunay边，然后按三角网的判别找出包含此边的Delaunay三角形的另一个端点，依次处理所有新生成的边，直至最终完成。具体步骤是：（a）以任一点为起始点；（b）找出与起始点最近的数据点相互连接形成Delaunay三角网的一条边作为基线，按三角网的判别法则，找出与基线构成Delaunay三角网的第三点；（c）基线的两个端点与第三个点，成为新的基线；（d）迭代以上两步直至所有基线都被处理。

8）如步骤39所示，对步骤38所构建的挖掘作业面三维物理模型进行六面分割。

对于挖掘作业典型工况平底坑，其内腔的六面所对应的法矢量是各向相异的，利用这一特征对6个面进行分割。由三角剖分的各个三角形顶点，，可以确定一个平面，平面方程的一般形式为式（3）所示，则平面的法矢量可以表示为。

                 （3）

由于参数不是相对独立的，大小不是绝对的，所以在求解平面法矢量时必须对矢量进行归一化处理，如式（4）所示。

　　　　（4）

由于平面由N个三角形组成，其法矢不可能协调一致，需要对其进行调整。在法矢调整时，任选一个三角形和其邻近三角形，设两个三角形的法矢分别为，若满足式（5），就判定这两个三角形平行，三角形和三角形的法矢方向变化连续；否则以三角形的法矢为基准，调整三角形的法矢方向，将取反。遍历所有三角形的法矢，使所有三角形的法矢均指向平底坑三维模型内腔曲面的外侧。

　　　　　　（5）

可设定平底坑三维模型内腔六个面的六组法矢量为：(0,0,1)、(0,0,-1)、(0,1,0)、(0,-1,0)、（1,0,0）、（-1,0,0），这六组法矢表示了平底坑三维物理模型六面的法矢量。在实际计算时，需遍历由三角剖分形成的三角形法矢，按照最近邻原则将各三角形归为这六类之一，实现平底坑三维物理模型的内腔六面分割。

9）如步骤40所示，对分割出的挖掘作业面物理模型进行六面体拟合。

因已分割的平底坑三维模型内腔的六个面不可能是规则平面，因此需要对其进行平面拟合，然后计算六个平面的交线，去除交线外的多余部分，形成平底坑三维模型内腔的不规则棱台模型。采用线性最小二乘法求解平面参数，将不规则平面拟合成平面，具体方法是：

设待拟合的平面方程为式（6）所示，为待拟合平面的单位法向量的三个分量，求取（）就能实现曲面拟合成平面。

　　　　（6）

设过拟合平面的点为、待拟合平面的法矢为和待拟合的数据点为，则拟合平面到数据点集的距离平方和最小，即满足式（7），

　　　　　　　　　（7）

其中过平面的点可以通过式（8）获取，即

      （8）

平面法矢可通过求取矩阵最小特征值对应的单位特征向量获得，其中矩阵为式（9）所示，

　　　　　　　　　　　　　　　　　　　（9）

其中，

10）如步骤41所示，计算拟合六面体的参数，求取挖掘面的相应尺寸。

经内腔六面拟合，生成不规则的六面体（棱台），顶面和底面不一定平行，无法直接计算两平面间的距离，可以采用间接的方法计算两平面间的距离。设F1为棱台的上表面，F2为棱台的下表面，h1～h4分别为上表面四个顶点到棱台下表面的距离，顶点到地面的距离由式（10）求得。

设空间中一点，则该点到空间一平面的距离为

   　　　　　　　　　　　　　　　　　　　　　（10）

可以求得不规则倒置棱台上表面的四个顶点到棱台下表面的四个距离h1～h4，按式（11）对其求均值得到该棱台的高度，即已挖掘的深度。

               　　　　　　　　                              （11）

同样，可以求得棱台左右两面和前后两面之间的距离，得到已挖掘平底坑的长度和宽度。

11）如步骤42所示，进行数据输出，将处理后的信息传送至主控计算机，为挖掘效果评估和后续的铲斗挖掘轨迹规划提供依据。

Claims (2)

1.一种基于激光雷达的挖掘作业面检测装置，其特征在于：所述的检测装置含有一线激光雷达（5）、控制器、步进电机（12）、电机驱动器（13）、减速器（8）、矩形架（6）、接近开关（25）、限位块（23）、壳体（9）、联轴器（10）、电源转换板（18）、计算机主板 （20）及控制程序；一线激光雷达（5）安装在矩形架（6）内，矩形架（6）、减速器（8）、联轴器（10）、步进电机（12）、电机驱动器（13）依次连接，并通过电机固定法兰（7）和电机固定法兰（11）固定在壳体（9）内；电源转换板（18）通过安装螺柱固定在壳体（9）内部，与壳体（9）之间衬有导热垫（21），利用壳体（9）表面进行散热；计算机扩展底板（19）通过安装螺柱固定在电源转换板（18）下方，计算机主板（20）安装在扩展底板上，用于一线激光雷达（5）的运动控制，数据的采集与处理；限位块（23）嵌入到橡胶垫（22）中，通过螺钉固定到壳体内侧壁；接近开关（25）安装在接近开关固定架（26）上，通过螺钉固定到壳体内侧壁；遮光板（24）一端在两个接近开关之间，遮光板（24）另一端在两个限位块之间。

2.根据权利要求书1所述的挖掘作业面检测装置，其特征在于：所述的控制程序包括如下步骤：

（1）使用接近开关反馈信息确定一线激光雷达在俯仰方向运动的起始位置，控制器控制步进电机寻找原点，使一线激光雷达回到原点位置；

（2）接收挖掘机器人主控计算机输出信息，确定挖掘作业面检测装置在挖掘机坐标系下的位置；

（3）通过控制器控制一线激光雷达的俯仰运动，进行挖掘面扫描，采集扫描数据，并记录扫描位置；

（4）对扫描数据进行滤波去噪；

（5）利用一线激光雷达扫描数据结合检测装置在俯仰方向上的转角，进行采样数据坐标变换，先将其转换为挖掘面检测装置坐标系下的坐标值，再将其转换为挖掘机器人坐标系下的坐标值；

（6）判断检测过程是否完成，如果完成，则进行三维重构，否则继续进行扫描；

（7）利用挖掘作业面检测得到的挖掘作业面三维点云数据进行三维重构，采用三角剖分法构建挖掘作业面三维物理模型；

（8）对挖掘作业面三维物理模型进行六面分割；

（9）对分割出的挖掘作业面物理模型进行六面体拟合；

（10）计算拟合六面体的参数，求取挖掘面的相应尺寸；

（11）进行数据输出。