CN116253165A - 悬臂式斗轮堆取料机的路径规划方法及装置 - Google Patents

Abstract

本发明公开了一种悬臂式斗轮堆取料机的路径规划方法及装置，该路径规划方法包括：建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型；基于运动学模型建立离散化的运动参数空间、及建立堆取料机在给定运动参数下的简化点云模型；提供碰撞检测算法，通过对简化点云模型与物料堆的整体点云模型进行几何求交，实现对离散化运动参数空间中的每一点的可达性进行判定，并将判定结果为可达的点集合生成三维可达性网格集；于三维可达性网格集中搜索、生成符合要求的运动路径。本发明适用于堆取料机的智能任务规划过程，既为堆取料作业提供了任意给定起点和终点的可通行参考路径，又为堆取料机无人值守***建设奠定了理论基础。

Description

悬臂式斗轮堆取料机的路径规划方法及装置

技术领域

本发明涉及自动化和人工智能技术领域，尤其涉及一种悬臂式斗轮堆取料机的路径规划方法及装置。

背景技术

悬臂式斗轮堆取料机(简称堆取料机)是在轨道上运行的大型工业设备，支持大车行走、悬臂回转和悬臂俯仰等多种运动功能，实质上是一类特殊的大型工业机器人，主要用在煤炭和矿石等大宗散状物料的连续堆取料作业中，具体的应用环境包括热电厂、钢铁厂和港口等。

近年来，堆取料机已经从手动控制和半自动控制的方式步入全自动化发展阶段，堆取料机无人值守***也因此成为国内外的关注焦点。无人值守***的一个核心要求是堆取料机能够自主安全抵达料堆的目标点进行堆取料作业，这使得路径规划成为无人值守***需要优先解决的问题。而且，自主规划也是无人值守***智能化水平体现最为充分的一环。

目前，国内外学者针对通用机械臂的路径规划提出了很多方法，但针对堆取料机的路径规划仍然鲜见报道。有鉴于此，特提出本发明。

发明内容

为了克服上述缺陷，本发明提供了一种悬臂式斗轮堆取料机的路径规划方法及装置，既为人工堆取料作业提供了任意给定起点和终点的可通行参考路径，又为悬臂式斗轮堆取料机无人值守***和智慧料场建设奠定了理论基础。

本发明为了解决其技术问题所采用的技术方案是：一种悬臂式斗轮堆取料机的路径规划方法，包括：

建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型；

基于该堆取料机的运动学模型建立离散化的运动参数空间、及建立该堆取料机在给定运动参数下的简化点云模型；

提供碰撞检测算法，通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

于所得三维可达性网格集中搜索、生成符合要求的运动路径。

作为本发明的进一步改进，建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型，包括：

建立堆取料机和物料堆所在料场的全局坐标系、及堆取料机上的多个局部坐标系，多个所述局部坐标系分别与堆取料机的多个运动机构一一对应；

获取所述全局坐标系和多个所述局部坐标系中的每任意两个坐标系之间的变换矩阵；

根据上述变换矩阵，得到被配置为末端执行器的一运动机构上的执行点在所述全局坐标系下的坐标；

基于该末端执行器上的执行点移动至物料堆的待处理目标点后，反向求解出该堆取料机的运动学模型。

作为本发明的进一步改进，该堆取料机的多个运动机构包括：斗轮机构，被配置为该堆取料机的末端执行器；所述斗轮机构的斗轮下边缘上的切入点即为所述末端执行器上的执行点；

俯仰机构，被配置为通过悬臂与所述斗轮机构相连接，并同时能够带动所述悬臂及所述斗轮机构一起进行上下运动；

回转机构，被配置为支撑所述俯仰机构，并同时能够带动所述俯仰机构、所述悬臂及所述斗轮机构一起进行左右转动；

行走机构，被配置为支撑所述回转机构，并同时能够支持该堆取料机整机沿水平直线移动；

相应的，多个局部坐标系分别为与所述斗轮机构相对应的斗轮坐标系、与所述俯仰机构相对应的俯仰坐标系、与所述回转机构相对应的回转坐标系、及与所述行走机构相对应的行走坐标系。

作为本发明的进一步改进，所述行走坐标系为所述全局坐标系沿着所述全局坐标系的Y轴方向平移行走距离L后得到，即：所述行走坐标系相对于所述全局坐标系的变换矩阵M₀1为：

所述回转坐标系为所述行走坐标系依次沿着所述行走坐标系的Z轴方向平移距离L_Y及绕着所述行走坐标系的Z轴旋转α角度后得到，即：所述回转坐标系相对于所述行走坐标系的变换矩阵

为：

上式中，s₁和c₁分别为α角度的正弦值和余弦值；

所述俯仰坐标系为所述回转坐标系依次沿着所述回转坐标系的Z轴方向平移距离L_PY及绕着所述回转坐标系的X轴旋转β角度后得到，即：所述俯仰坐标系相对于所述回转坐标系的变换矩阵

为：/>

上式中，s₂和c₂分别为β角度的正弦值和余弦值；

所述斗轮坐标系为所述俯仰坐标系沿着所述俯仰坐标系的Y轴方向平移距离L_A后得到，即：所述斗轮坐标系相对于所述俯仰坐标系的变换矩阵

为:

上式中，L_A也为悬臂长度；

复合上述变换矩阵

变换矩阵/>

和变换矩阵/>

得到所述俯仰坐标系相对于所述全局坐标系的变换矩阵/>

为：

复合上述变换矩阵

和变换矩阵/>

得到所述斗轮坐标系相对于所述全局坐标系的变换矩阵/>

为:

作为本发明的进一步改进，所述斗轮坐标系下，所述斗轮机构上的执行点的坐标为[0,0,-R,1]^T，式中的R为所述斗轮机构上的斗轮半径；

根据所述变换矩阵

所述执行点在所述全局坐标系下的坐标为：

当所述执行点移动至物料堆的待处理目标点后，反向求解，得到该堆取料机的运动学模型为：

/>

L＝P_y0-c₁(c₂L_A+s₂R)；

上式中的(P_x0,P_y0,P_z0)为所述待处理目标点在所述全局坐标系下的坐标，L、α和β分别为该堆取料机的运动学参数。

作为本发明的进一步改进，基于该堆取料机的运动学模型建立离散化的运动参数空间，包括：

给定该堆取料机的运动学模型中的运动学参数的定义域，得到该堆取料机的运动参数空间；

对所得堆取料机的运动参数空间进行采样，生成三维规则网格，即得到离散化的运动参数空间；

基于该堆取料机的运动学模型建立该堆取料机在给定运动参数下的简化点云模型，包括：

在给定运动参数下，根据所述悬臂和所述斗轮机构中的斗轮的轮廓信息，绘制出所述悬臂和所述斗轮的等效图形；

分别对所述悬臂和所述斗轮的等效图形进行离散数据网格化处理，得到所述俯仰坐标系下的悬臂点云模型A、及所述斗轮坐标系下的斗轮点云模型A；

根据变换矩阵，得到所述悬臂点云模型A在所述全局坐标系下的表现形式，并记为悬臂点云模型B；以及得到所述斗轮点云模型A在所述全局坐标系下的表现形式，并记为斗轮点云模型B；

获取所述悬臂点云模型B与所述斗轮点云模型B的并集，即得到该堆取料机的简化点云模型。

作为本发明的进一步改进，所得三维规则网格的表达式为：

Ω＝{(L_i,α_j,β_k)/i∈[1,I],j∈[1,J],k∈[1,K]}，

式中的L、α和β为该堆取料机的运动学参数；I、J和K分别表示各维度的样本数量；

所得并集的表达式为C(L,α,β)。

作为本发明的进一步改进，提供碰撞检测算法，通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；包括：

建立以数字高程模型表示的物料堆的整体点云模型；

遍历所述简化点云模型，得到所述简化点云模型中每一点所对应的高程；

对所述简化点云模型中每一点所对应的高程与其Z向坐标值进行比对，得到每一点的可达性；

将判定结果为可达的点集合生成三维可达性网格集。

作为本发明的进一步改进，对所述简化点云模型中每一点所对应的高程与其Z向坐标值进行比对，得到每一点的可达性，包括：

若某一点所对应的高程小于其Z向坐标值，则判定该点不会发生碰撞、为可达；而若某一点所对应的高程不小于其Z向坐标值，则判定该点会发生碰撞、为不可达；

所得三维可达性网格集的表达式为：

Φ＝{(L_i,α_j,β_k,R_ijk)/R_ijk＝0,1}；

上式中的R_ijk代表该堆取料机的运动学模型(L_i,α_j,β_k)的可达性，若：

R_ijk＝0，表示不可达；

R_ijk＝1，表示可达。

本发明还提供了一种悬臂式斗轮堆取料机的路径规划装置，包括：第一模型建立模块，用于建立堆取料机的运动学坐标系，并通过坐标系变换得到堆取料机的运动学模型；

第二模型建立模块，用于基于该堆取料机的运动学模型建立离散化的运动参数空间、以及建立该堆取料机在给定运动参数下的简化点云模型；

碰撞检测模块，用于通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

运动路径生成模块，用于在所述三维可达性网格集中搜索、生成符合要求的运动路径。

本发明的有益效果是：本发明通过先建立堆取料机的运动学模型；再构造堆取料机在给定运动参数下的简化点云模型，并同时提供可实现快速碰撞检测(即可达性检测)的碰撞检测算法；最后利用搜索算法生成安全可靠的运动路径。本发明适用于悬臂式斗轮堆取料机的智能任务规划过程，既为人工堆取料作业提供了任意给定起点和终点的可通行参考路径，又为悬臂式斗轮堆取料机无人值守***和智慧料场建设奠定了理论基础。

附图说明

图1为本发明所述悬臂式斗轮堆取料机的路径规划方法的流程图；

图2为本发明建立堆取料机的运动学模型的流程图；

图3为本发明所述悬臂式斗轮堆取料机的运动学坐标系示意图；

图4为本发明建立堆取料机的简化点云模型的流程图；

图5为本发明进行碰撞检测及生成三维可达性网格集的流程图；

图6为本发明所述悬臂式斗轮堆取料机的路径规划装置的方框示意图。

结合附图，作以下说明：

10、行走机构；11、回转机构；12、俯仰机构；13、斗轮机构；14、悬臂；15、轨道；20、第一模型建立模块；21、第二模型建立模块；22、碰撞检测模块；23、运动路径生成模块。

具体实施方式

以下结合附图，对本发明的一个较佳实施例作详细说明。

实施例1：

请参阅附图1所示，其为本发明所述悬臂式斗轮堆取料机的路径规划方法的流程图。

所述悬臂式斗轮堆取料机的路径规划方法，主要包括以下工作步骤：

S1：建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型；

S2：基于该堆取料机的运动学模型建立离散化的运动参数空间、及建立该堆取料机在给定运动参数下的简化点云模型；

S3：提供碰撞检测算法，通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

S4：于所得三维可达性网格集中搜索、生成符合要求的运动路径。

本发明通过先建立堆取料机的运动学模型；再构造堆取料机在给定运动参数下的简化点云模型，并同时提供可实现快速碰撞检测(即可达性检测)的碰撞检测算法；最后利用搜索算法生成安全可靠的运动路径。本发明适用于悬臂式斗轮堆取料机的智能任务规划过程，既为人工堆取料作业提供了任意给定起点和终点的可通行参考路径，又为悬臂式斗轮堆取料机无人值守***和智慧料场建设奠定了理论基础。

实施例2：

请参阅附图2所示，其为本发明建立堆取料机的运动学模型的流程图。

所述堆取料机的运动学模型建立，主要包括以下工作步骤：

S11：建立堆取料机和物料堆所在料场的全局坐标系、及堆取料机上的多个局部坐标系，多个所述局部坐标系分别与堆取料机的多个运动机构一一对应。

公知的，悬臂式斗轮堆取料机包括行走机构10、回转机构11、俯仰机构12和斗轮机构13等多个运动机构，其中，所述行走机构10被配置为能够支持该堆取料机整机沿水平直线移动；所述回转机构11被配置为设置于所述行走机构10上，所述俯仰机构12被配置为设置于所述回转机构11上，并能够在所述回转机构11的带动下进行左右转动；所述斗轮机构13被配置为该堆取料机的末端执行器，所述斗轮机构13的斗轮下边缘上的切入点即为所述末端执行器上的执行点；所述斗轮机构13通过悬臂14与所述俯仰机构12相连接，并能够在所述俯仰机构12的带动下进行一定角度内的上下运动。

相应的，多个局部坐标系分别为与所述斗轮机构13相对应的斗轮坐标系、与所述俯仰机构12相对应的俯仰坐标系、与所述回转机构11相对应的回转坐标系、及与所述行走机构10相对应的行走坐标系，具体可参阅附图3所示。

S12：获取所述全局坐标系和多个所述局部坐标系中的每任意两个坐标系之间的变换矩阵。

具体的，见附图3所示，定义料场(为悬臂式斗轮堆取料机的工作场所)的全局坐标系为O₀-X₀Y₀Z₀，原点位于轨道15上，所述轨道15为堆取料机的所述行走机构10行走的轨道。

所述行走坐标系O₁-X₁Y₁Z₁为所述全局坐标系O₀-X₀Y₀Z₀沿着所述全局坐标系的Y₀轴方向平移行走距离L后得到，即：所述行走坐标系相对于所述全局坐标系的变换矩阵

为：

所述回转坐标系O₂-X₂Y₂Z₂为所述行走坐标系O₁-X₁Y₁Z₁依次沿着所述行走坐标系的Z₁轴方向平移距离L_Y、及绕着所述行走坐标系的Z₁轴旋转α角度(也就是在水平面上进行左右旋转α角度)后得到，也就是说，所述行走坐标系O₁-X₁Y₁Z₁通过两次变换得到所述回转坐标系O₂-X₂Y₂Z₂，首先所述行走坐标系O₁-X₁Y₁Z₁沿着其Z₁轴方向平移距离L_Y，得到中间坐标系O'₂-X'₂Y'₂Z'₂，然后再将中间坐标系O'₂-X'₂Y'₂Z'₂绕着其Z'₂轴(平行于所述行走坐标系的Z₁轴)旋转α角度。

即：所述回转坐标系相对于所述行走坐标系的变换矩阵

为：

上式中，s₁和c₁分别为α角度(也称为回转关节角度)的正弦值和余弦值；

所述俯仰坐标系O₃-X₃Y₃Z₃为所述回转坐标系O₂-X₂Y₂Z₂依次沿着所述回转坐标系的Z₂轴方向平移距离L_PY、及绕着所述回转坐标系的X₂轴旋转β角度(也就是在竖直面上进行上下摆动)后得到，也就是说所述回转坐标系O₂-X₂Y₂Z₂也是通过两次变换得到所述俯仰坐标系O₃-X₃Y₃Z₃的，首先所述回转坐标系O₂-X₂Y₂Z₂沿着其Z₂轴方向平移距离L_PY得到中间坐标系O'₃-X'₃Y'₃Z'₃，然后再将中间坐标系O'₃-X'₃Y'₃Z'₃绕其X'₃轴(平行于所述回转坐标系的X₂轴)旋转β角度。

即：所述俯仰坐标系相对于所述回转坐标系的变换矩阵

为：

上式中，s₂和c₂分别为β角度(也称为俯仰关节角度)的正弦值和余弦值。

所述斗轮坐标系O_t-X_tY_tZ_t为所述俯仰坐标系O₃-X₃Y₃Z₃沿着所述俯仰坐标系的Y₃轴方向平移距离L_A后得到，即：所述斗轮坐标系相对于所述俯仰坐标系的变换矩阵

为:

上式中，L_A也为所述悬臂(14)的长度；

复合上述变换矩阵

变换矩阵/>

和变换矩阵/>

得到所述俯仰坐标系相对于所述全局坐标系的变换矩阵/>

为：

复合上述变换矩阵

和变换矩阵/>

得到所述斗轮坐标系相对于所述全局坐标系的变换矩阵/>

为:/>

S13：根据上述变换矩阵，得到被配置为末端执行器的一运动机构上的执行点在所述全局坐标系下的坐标(即为运动学正向模型)。

由上述S11可知，所述斗轮机构13被配置为该堆取料机的末端执行器，所述斗轮机构13中的斗轮下边缘上的切入点Q即为所述执行点。

令所述斗轮的半径为R，那么在所述斗轮坐标系下，所述斗轮机构13上的执行点Q的坐标为[0,0,-R,1]^T。

进一步的，根据所述变换矩阵

所述执行点Q在所述全局坐标系下的坐标为：

上述表达式实际也为运动学正向模型。

S14：基于该末端执行器上的执行点Q移动至物料堆的待处理目标点P后，反向求解出该堆取料机的运动学模型。

具体的，当所述执行点Q移动至物料堆的待处理目标点P后，所述待处理目标点P在所述全局坐标系下的坐标为(P_x0,P_y0,P_z0)，反向求解，得到该堆取料机的运动学模型为：

L＝P_y0-c₁(c₂L_A+s₂R)。

由该堆取料机的运动学模型可以看出，运动参数(L,α,β)至多存在四组解(运动学参数L、α和β分别为行走距离、回转关节角度和俯仰关节角度)。显然，可行解必须保证参数落入定义域内且无碰撞发生。如果存在多组可行解，可以基于最小变化原则筛选出一组最合理的解。如果不存在可行解，则待处理目标位置不可达。

实施例3：

通常情况下，料场中的物料堆是分布在轨道15两侧的，轨道15为堆取料机的所述行走机构10行走的轨道，与物料堆发生碰撞的机构必须具有回转和俯仰自由度。因所述回转机构11和所述俯仰机构12的支撑部分只能在轨道15范围内回转，所以不会发生碰撞，因此只有所述悬臂14、所述斗轮和所述俯仰机构12的配重部分存在碰撞可能性。再结合配重部分的高度、其俯仰角度的变动范围和物料堆的斜坡形状分布，配重部分基本不存在碰撞风险。因而，从碰撞检测的角度，只需描述所述悬臂14和所述斗轮分别与料堆的位置关系即可。

首先，基于该堆取料机的运动学模型建立离散化的运动参数空间，具体包括以下工作步骤：

S211：给定该堆取料机的运动学模型中的运动学参数的定义域，得到该堆取料机的运动参数空间；

S212：对所得堆取料机的运动参数空间进行采样，生成三维规则网格，即得到离散化的运动参数空间。

如以该堆取料机的运动学参数的定义域为：L∈[0m,300m]，α∈[-120°,120°]，β∈[-30°,30°]为例，对参数进行采样，生成三维规则网格Ω＝{(L_i,α_j,β_k)/i∈[1,I],j∈[1,J],k∈[1,K]}，式中的L、α和β为该堆取料机的运动学参数，I、J和K分别表示各维度的样本数量。Ω是离散化的运动参数空间，如果样本数量的取值越大，则采样间距越小，参数采样越稠密。

基于所得到的离散化的运动参数空间，当给定了运动参数空间中的一个采样点后，就是给定了一组运动参数(行走距离L、回转关节角度α和俯仰关节角度β)的取值。对于每一组运动参数，都可以建立该堆取料机在该运动参数下的简化点云模型。

具体的，建立该堆取料机在给定运动参数下的简化点云模型，包括以下工作步骤：请参阅附图4所示，

S221：在给定运动参数下，根据所述悬臂14和所述斗轮机构13中的斗轮的轮廓信息，绘制出所述悬臂14和所述斗轮的等效图形；

为提高碰撞检测的效率，将所述悬臂14和所述斗轮机构13中的斗轮的三维模型进行等效简化。所述悬臂14等效为“长为L_A、宽为W_A、高为H_A”的矩形，长L_A沿着坐标轴Y₃方向，高H_A沿着坐标轴X₃方向，宽W_A沿着坐标轴Z₃方向，俯仰坐标系的原点O₃位于该矩形左侧表面的中心；所述斗轮等效为“半径为R、高为H”的圆柱，高H沿着坐标轴X_t方向，斗轮坐标系的原点O_t位于圆柱的中心。

S222：分别对所述悬臂14和所述斗轮的等效图形进行离散数据网格化处理(比如可采用三角形网格化处理)，得到所述俯仰坐标系下的悬臂点云模型A、及所述斗轮坐标系下的斗轮点云模型A；

S223：根据变换矩阵

得到所述悬臂点云模型A在所述全局坐标系下的表现形式，并记为悬臂点云模型B；以及根据变换矩阵/>

得到所述斗轮点云模型A在所述全局坐标系下的表现形式，并记为斗轮点云模型B；

S224：获取所述悬臂点云模型B与所述斗轮点云模型B的并集，即得到该堆取料机的简化点云模型。

具体的，所得并集的表达式为C(L,α,β)，式中的L、α和β为该堆取料机的运动学参数。简言之，简化点云模型就是所述悬臂和所述斗轮的包围盒，并以点云模型的形式给出。

实施例4：

请参阅附图5所示，其为本发明进行碰撞检测及生成三维可达性网格集的流程图。

所述进行碰撞检测及生成三维可达性网格集，包括以下工作步骤：

S31：建立以数字高程模型DEM表示的物料堆的整体点云模型；

关于物料堆的整体点云数据的获取，可通过多种方式来获得，比如可通过激光雷达扫描采集的方式获得。

S32：遍历所述简化点云模型为C(L,α,β)，根据所述简化点云模型中每一点的坐标插值，计算得到所述简化点云模型中每一点所对应的高程；

S33：对所述简化点云模型中每一点所对应的高程与其Z向坐标值进行比对，得到每一点的可达性；

具体为：若某一点所对应的高程小于其Z向坐标值，则判定该点不会发生碰撞、为可达；而若某一点所对应的高程不小于其Z向坐标值，则判定该点会发生碰撞、为不可达；

S34：将判定结果为可达的点集合生成三维可达性网格集，所得三维可达性网格集的表达式为：Φ＝{(L_i,α_j,β_k,R_ijk)/R_ijk＝0,1}；

上式中的R_ijk代表该堆取料机的运动学模型(L_i,α_j,β_k)的可达性，若：R_ijk＝0，表示不可达；R_ijk＝1，表示可达。

本实施例中，上述碰撞检测的工作过程实质上是独立的，因此可利用GPU的并行能力来快速判定所述简化点云模型中每一个点的可达性，以生成三维可达性网格集Φ。总之，上述碰撞检测的工作效率高，可很好满足实时任务规划需求，为堆取料机无人值守***建设创造了基础条件。

实施例5：

路径规划的本质是生成一条从起点到终点的离散路径，当然路径中的每一个点都必须是可达的。通常情况下，路径的起点的运动参数和位置都是已知的，路径的终点是指定位置。

本实施例中，利用该堆取料机的运动学模型可以为终点找到一组或几组关于运动参数的可行解。若：起点参数为(L_start,α_start,β_start)，终点参数为(L_end,α_end,β_end)，与它们最近的网格元素分别为(L_i0,α_j0,β_k0)和(L_i1,α_j1,β_k1)，显然只需找到(L_i0,α_j0,β_k0)和(L_i1,α_j1,β_k1)之间的一条路径即可。

工作时，基于给定的三维可达性网格集Φ，利用搜索算法进行路径搜索，搜索算法可采用A星算法、RTT路径搜索算法和PRM路径搜索算法中的一种。并且在路径搜索时还要定义网格点间的距离D，本例采用带权重的欧式距离D：

上式中，ω_L,ω_α,ω_β分别为行走距离、回转关节角度和俯仰关节角度的权重。考虑到行走的耗能相对较大，行走距离的权重是其它两个参数的两倍：ω_L＝2，ω_α＝1，ω_β＝1，以保证搜索时回转和俯仰优先。

实施例6：

本实施例提供了一种悬臂式斗轮堆取料机的路径规划装置，见附图6所示，包括：

第一模型建立模块20，用于建立堆取料机的运动学坐标系，并通过坐标系变换得到堆取料机的运动学模型；

第二模型建立模块21，用于基于该堆取料机的运动学模型建立离散化的运动参数空间、以及建立该堆取料机在给定运动参数下的简化点云模型；

碰撞检测模块22，用于通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

运动路径生成模块23，用于在所述三维可达性网格集中搜索、生成符合要求的运动路径。

本实施例所提供的悬臂式斗轮堆取料机的路径规划装置可执行本发明任意实施例所提供的悬臂式斗轮堆取料机的路径规划方法，并具备执行方法相应的有益效果。

综上所述，本发明通过先建立堆取料机的运动学模型；再构造堆取料机在给定运动参数下的简化点云模型，并同时提供可实现快速碰撞检测(即可达性检测)的碰撞检测算法；最后利用搜索算法生成安全可靠的运动路径。本发明适用于悬臂式斗轮堆取料机的智能任务规划过程，既为人工堆取料作业提供了任意给定起点和终点的可通行参考路径，又为悬臂式斗轮堆取料机无人值守***和智慧料场建设奠定了理论基础。

在以上的描述中阐述了很多具体细节以便于充分理解本发明。但是以上描述仅是本发明的较佳实施例而已，本发明能够以很多不同于在此描述的其它方式来实施，因此本发明不受上面公开的具体实施的限制。同时任何熟悉本领域技术人员在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例。凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (10)

1.一种悬臂式斗轮堆取料机的路径规划方法，其特征在于：包括：

建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型；

基于该堆取料机的运动学模型建立离散化的运动参数空间、及建立该堆取料机在给定运动参数下的简化点云模型；

提供碰撞检测算法，通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

于所得三维可达性网格集中搜索、生成符合要求的运动路径。

2.根据权利要求1所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：建立堆取料机的运动学坐标系，并根据坐标系变换得到堆取料机的运动学模型，包括：

建立堆取料机和物料堆所在料场的全局坐标系、及堆取料机上的多个局部坐标系，多个所述局部坐标系分别与堆取料机的多个运动机构一一对应；

获取所述全局坐标系和多个所述局部坐标系中的每任意两个坐标系之间的变换矩阵；

根据上述变换矩阵，得到被配置为末端执行器的一运动机构上的执行点在所述全局坐标系下的坐标；

基于该末端执行器上的执行点移动至物料堆的待处理目标点后，反向求解出该堆取料机的运动学模型。

3.根据权利要求2所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：该堆取料机的多个运动机构包括：

斗轮机构(13)，被配置为该堆取料机的末端执行器；所述斗轮机构(13)的斗轮下边缘上的切入点即为所述末端执行器上的执行点；

俯仰机构(12)，被配置为通过悬臂(14)与所述斗轮机构(13)相连接，并同时能够带动所述悬臂(14)及所述斗轮机构(13)一起进行上下运动；

回转机构(11)，被配置为支撑所述俯仰机构(12)，并同时能够带动所述俯仰机构(12)、所述悬臂(14)及所述斗轮机构(13)一起进行左右转动；

行走机构(10)，被配置为支撑所述回转机构(11)，并同时能够支持该堆取料机整机沿水平直线移动；

相应的，多个局部坐标系分别为与所述斗轮机构(13)相对应的斗轮坐标系、与所述俯仰机构(12)相对应的俯仰坐标系、与所述回转机构(11)相对应的回转坐标系、及与所述行走机构(10)相对应的行走坐标系。

4.根据权利要求3所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：所述行走坐标系为所述全局坐标系沿着所述全局坐标系的Y轴方向平移行走距离L后得到，即：所述行走坐标系相对于所述全局坐标系的变换矩阵

为：

所述回转坐标系为所述行走坐标系依次沿着所述行走坐标系的Z轴方向平移距离L_Y及绕着所述行走坐标系的Z轴旋转α角度后得到，即：所述回转坐标系相对于所述行走坐标系的变换矩阵

为：

上式中，s₁和c₁分别为α角度的正弦值和余弦值；

所述俯仰坐标系为所述回转坐标系依次沿着所述回转坐标系的Z轴方向平移距离L_PY及绕着所述回转坐标系的X轴旋转β角度后得到，即：所述俯仰坐标系相对于所述回转坐标系的变换矩阵

为：

上式中，s₂和c₂分别为β角度的正弦值和余弦值；

所述斗轮坐标系为所述俯仰坐标系沿着所述俯仰坐标系的Y轴方向平移距离L_A后得到，即：所述斗轮坐标系相对于所述俯仰坐标系的变换矩阵

为:

上式中，L_A也为悬臂长度；

复合上述变换矩阵

变换矩阵/>

和变换矩阵/>

得到所述俯仰坐标系相对于所述全局坐标系的变换矩阵/>

为：

复合上述变换矩阵

和变换矩阵/>

得到所述斗轮坐标系相对于所述全局坐标系的变换矩阵/>

为:

/>

5.根据权利要求4所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：所述斗轮坐标系下，所述斗轮机构(13)上的执行点的坐标为[0,0,-R,1]^T，式中的R为所述斗轮机构(13)上的斗轮半径；

根据所述变换矩阵

所述执行点在所述全局坐标系下的坐标为：

当所述执行点移动至物料堆的待处理目标点后，反向求解，得到该堆取料机的运动学模型为：

L＝P_y0-c₁(c₂L_A+s₂R)；

上式中的(P_x0,P_y0,P_z0)为所述待处理目标点在所述全局坐标系下的坐标，L、α和β分别为该堆取料机的运动学参数。

6.根据权利要求3所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：基于该堆取料机的运动学模型建立离散化的运动参数空间，包括：

给定该堆取料机的运动学模型中的运动学参数的定义域，得到该堆取料机的运动参数空间；

对所得堆取料机的运动参数空间进行采样，生成三维规则网格，即得到离散化的运动参数空间；

基于该堆取料机的运动学模型建立该堆取料机在给定运动参数下的简化点云模型，包括：

在给定运动参数下，根据所述悬臂(14)和所述斗轮机构(13)中的斗轮的轮廓信息，绘制出所述悬臂(14)和所述斗轮的等效图形；

分别对所述悬臂(14)和所述斗轮的等效图形进行离散数据网格化处理，得到所述俯仰坐标系下的悬臂点云模型A、及所述斗轮坐标系下的斗轮点云模型A；

根据变换矩阵，得到所述悬臂点云模型A在所述全局坐标系下的表现形式，并记为悬臂点云模型B；以及得到所述斗轮点云模型A在所述全局坐标系下的表现形式，并记为斗轮点云模型B；

获取所述悬臂点云模型B与所述斗轮点云模型B的并集，即得到该堆取料机的简化点云模型。

7.根据权利要求6所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：所得三维规则网格的表达式为：

Ω＝{(L_i,α_j,β_k)/i∈[1,I],j∈[1,J],k∈[1,K]}，

式中的L、α和β为该堆取料机的运动学参数；I、J和K分别表示各维度的样本数量；

所得并集的表达式为C(L,α,β)。

8.根据权利要求6所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：提供碰撞检测算法，通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；包括：

建立以数字高程模型表示的物料堆的整体点云模型；

遍历所述简化点云模型，得到所述简化点云模型中每一点所对应的高程；

对所述简化点云模型中每一点所对应的高程与其Z向坐标值进行比对，得到每一点的可达性；

将判定结果为可达的点集合生成三维可达性网格集。

9.根据权利要求8所述的悬臂式斗轮堆取料机的路径规划方法，其特征在于：对所述简化点云模型中每一点所对应的高程与其Z向坐标值进行比对，得到每一点的可达性，包括：

若某一点所对应的高程小于其Z向坐标值，则判定该点不会发生碰撞、为可达；而若某一点所对应的高程不小于其Z向坐标值，则判定该点会发生碰撞、为不可达；

所得三维可达性网格集的表达式为：

Φ＝{(L_i,α_j,β_k,R_ijk)/R_ijk＝0,1}；

上式中的R_ijk代表该堆取料机的运动学模型(L_i,α_j,β_k)的可达性，若：

R_ijk＝0，表示不可达；

R_ijk＝1，表示可达。

10.一种悬臂式斗轮堆取料机的路径规划装置，其特征在于：包括：

第一模型建立模块(20)，用于建立堆取料机的运动学坐标系，并通过坐标系变换得到堆取料机的运动学模型；

第二模型建立模块(21)，用于基于该堆取料机的运动学模型建立离散化的运动参数空间、以及建立该堆取料机在给定运动参数下的简化点云模型；

碰撞检测模块(22)，用于通过对所得简化点云模型与所述物料堆的整体点云模型进行几何求交，以实现对所述离散化的运动参数空间中的每一点的可达性进行判定，同时将判定结果为可达的点集合生成三维可达性网格集；

运动路径生成模块(23)，用于在所述三维可达性网格集中搜索、生成符合要求的运动路径。

Images