CN103017757A - 工程机械入场路径规划方法和路径规划装置 - Google Patents

Abstract

本发明提供了一种工程机械入场路径规划方法和路径规划装置。该工程机械的入场路径规划方法包括：将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；计算工程机械经由最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算；分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并输出结果。从而最终得出最优路径方案。

Description

工程机械入场路径规划方法和路径规划装置

技术领域

本发明涉及工程机械领域，具体而言，涉及一种工程机械入场路径规划方法和路径规划装置。

背景技术

一般工程机械体积庞大，而在施工环境较为复杂，存在各种障碍物。因此将例如起重机等大型工程机械布置到工位上，需要对工程机械的入场路径进行规划。

在工程机械入场路径规划中，必须保证工程机械安全地通过障碍物之间的缝隙，防止出现工程机械与障碍物发生碰撞，另外，由于工程机械的重量和体积都较大，进行路径规划还需要对行进的距离和转向次数进行综合考虑。

现有的工程机械入场一般依靠人工勘查，通过操作人员的个人经验进行操作，不能保证是否是最优方案，另外也无法保证顺利通过所有障碍物，有时甚至出现退回重新选择路径的情况。目前有文献中提出使用遍历的方法进行计算距离，首先将障碍物转为凸形物体，对障碍物间的间距进行遍历距离计算，这种方法计算量大，计算耗时长，而且没有充分考虑工程机械的转弯情况，不满足实际工程机械在三维场景中进行规划需求。

以上现有技术工程机械入场路径规划中没有结合工程机械实际转弯特点的问题，尚未提出有效的解决方案。

发明内容

本发明旨在提供一种工程机械入场路径规划方法和路径规划装置，以解决现有技术中入场路径规划中没有结合工程机械实际转弯特点的问题。

为了实现上述目的，根据本发明的一个方面，提供了一种工程机械入场路径规划方法。本发明提供的工程机械入场路径规划方法包括：将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。

进一步地，利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线包括：使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线，作为每两个相邻障碍物之间的最短连线。

进一步地，使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓包括：使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的矩形轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线包括：分别计算每两个相邻障碍物的矩形轮廓之间的最短连线。

进一步地，计算工程机械经由最短连线的中点到达目标位置的各条路径包括：沿当前点向目标点的前进方向进行路径探索；当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过最短连线的中点向目标点搜索路径，并记录当前点到该中点的路径。

进一步地，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线之后还包括：当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据工程机械绕行障碍物的安全距离查找距离最短的转向点，通过转向点向目标点搜索路径，并记录当前点到转向点的路径。

进一步地，对各条路径进行路径规划计算之后还包括：分别判断经过路径计算的各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离；删除出现最短连线的距离小于工程机械的转弯安全距离的路径。

进一步地，工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。

进一步地，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离包括：计算经过路径计算的各条路径中工程机械转弯的中心点、半径、和转向位置；分别对经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行进的距离。

进一步地，计算工程机械经由最短连线的中点到达目标位置的各条路径包括：使用二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径。

进一步地，对二叉树各条路径进行路径规划计算包括：使用迪杰斯特拉算法对二叉树各条路径分别进行路径规划计算。

根据本发明的另一个方面，提供了一种工程机械入场路径规划装置。该工程机械入场路径规划装置包括：投影模块，用于将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；最短距离中点计算模块，用于利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；路径搜索模块，用于计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；路径分析模块，用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；路径优化模块，用于按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。

进一步地，本发明提供的工程机械入场路径规划装置还包括：路径安全评价模块，用于分别判断各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离，并删除出现最短连线的距离小于工程机械的转弯安全距离的路径

应用本发明的技术方案，通过计算障碍物二维投影轮廓的最短连线，找出最短连线的中点，在出现障碍物情况下探索从出发点通过上述最短连线终点到达目的施工地点的路线，并在进行路线路径计算后，按照实际工程机械的行进距离计算路径距离，统计转弯次数，安全间距，从而可以综合各项条件对各条路径进行考察，最终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量小，可以实现实时进行路径规划的要求，符合工程机械的自身移动特性。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例的工程机械入场路径规划装置的示意图；

图2是根据本发明实施例的工程机械入场路径规划方法的示意图；

图3A是根据本发明实施例的工程机械入场路径规划方法对障碍物的三维模型投影到水平面的示意图；

图3B是根据本发明实施例的工程机械入场路径规划方法使用方向包围盒对每个障碍物的二维投影轮廓进行计算的结果示意图；

图4A是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第一种情况；

图4B是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第二种情况；

图4C是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第三种情况；

图5是根据本发明实施例的工程机械入场路径规划方法的具体场景的示意图；

图6是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的示意图；

图7是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的流程图；

图8是根据本发明实施例的工程机械入场路径规划方法得出的最佳路径的示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

本发明实施例提供了一种工程机械入场路径规划装置，图1是根据本发明实施例的工程机械入场路径规划装置的示意图，如图1所示，本发明实施例的工程机械入场路径规划装置包括：投影模块11，用于将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；最短距离中点计算模块13，用于利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；路径搜索模块15，用于计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；路径分析模块17，用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；路径优化模块19，用于按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对各条路径进行排序，并根据排序结果输出各条入场路径。

使用本发明的工程机械入场路径规划装置，计算障碍物二维投影轮廓的最短连线，找出最短连线的中点，探索从出发点通过上述最短连线终点到达目的施工地点的路线并进行路径规划计算，对经过路径计算的路线按照实际工程机械的行进距离计算路径距离，统计转弯次数，安全间距，从而可以综合各项条件对各条路径进行考察，最终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量小，可以实现实时进行路径规划的要求，符合工程机械的自身移动特性。

为了验证工程机械是否安全顺利通过障碍物之间的空间，本发明可应用实施的工程机械入场路径规划装置，还可以包括：路径安全评价模块（图中未示出），用于分别判断各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离，并删除出现最短连线的距离小于工程机械的转弯安全距离的路径。使保留下来的所有路径都符合工程机械的安全通过要求。其中，工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。具体地，在转弯过程中，障碍物之间最短距离必须大于外轮廓的最大转弯半径Rmax与内轮廓的最小半径Rmin之差（Rmax-Rmin）+X，其中X为可以预先设置的转弯安全距离裕值。当障碍物物之间的距离小于（Rmax-Rmin）+X时，则说明工程机械不能通过这两个障碍物之间的空间，该条路径需要抛弃。如果通过比较，该路径上所有障碍物之间的距离都大于（Rmax-Rmin）+X，则可以认为该路径畅通。

施工场地中的障碍物可能是建筑、堆放材料、已经入场的工程机械、或者自然存在的物体，形状是不规则的，因此直接计算障碍物之间的最短距离计算量大。因此投影模块11的具体执行步骤可以是：使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线，作为每两个相邻障碍物之间的最短连线。

层次包围盒法是用体积略大而几何特性简单的包围盒来近似地描述复杂的对象，从而只对包围盒重叠的对象进行进一步的相交测试，常见的包围盒类型有沿坐标轴的包围盒AABB、包围球、方向包围盒OBB、固定方向凸包等。由于方向包围盒（Oriented bounding box）算法是利用最接近的矩形覆盖障碍物的投影轮廓，而矩形之间的最短距离计算相对简单，因此可以优选使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的矩形轮廓；然后分别计算每两个相邻障碍物的矩形轮廓之间的最短连线。

路径搜索模块15的具体执行步骤为：沿当前点向目标点的前进方向进行路径探索；当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过最短连线的中点向目标点搜索路径，并记录当前点到中点的路径。当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据工程机械绕行障碍物的安全距离查找距离最短的转向点，通过转向点向目标点搜索路径，并记录当前点到转向点的路径。依次各条连接搜索中点或转向点直到目标点，就可以得到从当前点到目标点可能经过的所有路径。也就是当前进方向上遇到障碍物时，就从障碍物两侧的中点或者转向点绕行，从而分出两条可能的路径，然后从该中点或转向点继续向前探索。直至到达目标点，然后整理出所有可能经过的路径供下一步优化。

路径分析模块17，计算各条计算得出的路径中工程机械转弯的中心点、半径、和转向位置的步骤可以是分别对各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行进的距离。就是计算工程机械行进的弧线距离而不是直线距离，使各条路径入场需要行进的距离更加符合工程机械入场的要求。

路径优化模块19的排序条件包括：需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离。其中，需要行进的距离反映了工程机械行进的距离长短；转弯次数反映了工程机械的路线控制难度；工程机械通过障碍物时与障碍物的最小距离反映了安全程度。用户可以根据需要选择排序条件，或者对以上排序设置权重值进行综合计算，得到最符合用户要求的入场路径。

以上路径搜索模块15计算路径以及进行路径规划的具体算法可以为：用于利用二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径，并使用迪杰斯特拉算法（Dijkstra）对各条路径进行路径规划计算。迪杰斯特拉（Dijkstra）算法是现有典型的单源最短路径算法，用于计算一个节点到其他所有节点的最短路径。以起始点为中心向外层层扩展，直到扩展到终点为止。

本发明实施例还提供了一种工程机械入场路径规划方法，该工程机械入场路径规划方法可以通过本发明上述实施例所提供的任一种工程机械入场路径规划装置来执行。图2是根据本发明实施例的工程机械入场路径规划方法的示意图，如图所示，该工程机械入场路径规划方法包括：

步骤S21，将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；

步骤S22，利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；

步骤S23，计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；

步骤S24，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；

步骤S25，按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对各条路径进行排序，并根据排序结果输出各条入场路径。

利用该方法，计算障碍物二维投影轮廓的最短连线，找出最短连线的中点，探索从出发点通过上述最短连线终点到达目的施工地点的路线，并按照实际工程机械的行进距离计算路径距离，统计转弯次数，安全间距，从而可以综合各项条件对各条路径进行考察，最终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量小，可以实现实时进行路径规划的要求，符合工程机械的自身移动特性。采用投影的思路将三维物体之间最短距离计算转化二维物体间的距离的求解，简化求解，降低计算量；采用包围盒方法将物体进行封装来求解，将物体进行封装方式求解，简化求解方法，同时求解精度损失较少；采用从障碍物中心点处入手来寻找最安全的入场路径，符合实际路径规划需求；提出解决完整的工程机械入场路径实现方法，并分步骤给出求解实现的方法。

其中步骤S21具体可以包括：使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线，作为每两个相邻障碍物之间的最短连线。

层次包围盒法是用体积略大而几何特性简单的包围盒来近似地描述复杂的对象，从而只对包围盒重叠的对象进行进一步的相交测试，常见的包围盒类型有沿坐标轴的包围盒AABB、包围球、方向包围盒OBB、固定方向凸包等。由于方向包围盒（Oriented bounding box）算法是利用最接近的矩形覆盖障碍物的投影轮廓，而矩形之间的最短距离计算相对简单。因此可以优选使用方向包围盒的算法，这种情况下，步骤S21中使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓可以具体包括：使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到所述每个障碍物的矩形轮廓；分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线可以具体包括：分别计算每两个相邻所述障碍物的矩形轮廓之间的最短连线。

图3A是根据本发明实施例的工程机械入场路径规划方法对障碍物的三维模型投影到水平面的示意图，图3B是根据本发明实施例的工程机械入场路径规划方法使用方向包围盒对每个障碍物的二维投影轮廓进行计算的结果示意图，如图所示，一般是同场地上的障碍物位置和障碍物的形状是不规则的，对其直接进行距离计算，计算量大而且对求解难度大，使用方向包围和计算后，将不规则的轮廓使用最接近的矩形代替。

图4A是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第一种情况，图4B是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第二种情况，图4C是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第三种情况，在图4A中，两个障碍物的矩形轮廓之间的最短距离为与第一障碍物的一条边的垂线到第二障碍物一个顶点的距离；在图4B中，两个障碍物的矩形轮廓之间的最短距离为第一障碍物的一个顶点到第二障碍物一条边的垂线的长度。在图4C中，两个障碍物的矩形轮廓之间的最短距离为一个矩形顶点到另一个障碍物矩形顶点。因此，可以通过比较矩形轮廓各顶点到矩形各点、线之间的距离的方式求解两矩形间的最短距离。

步骤S23具体可以包括：沿当前点向目标点的前进方向进行路径探索；当前进方向上遇到障碍物时用二叉树方法来规划路径，具体为使用二叉树法向障碍物两侧分叉，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过最短连线的中点向目标点搜索路径，并记录当前点到中点的路径。当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据工程机械绕行障碍物的安全距离查找距离最短的转向点，通过转向点向目标点搜索路径，并记录当前点到转向点的路径。从而，步骤S23计算工程机械经由最短连线的中点到达目标位置的各条路径的具体算法为使用二叉树算法进行遍历计算。

步骤S23中对各条路径进行路径规划计算的算法可以具体为：使用迪杰斯特拉算法对所述各条路径分别进行路径规划计算。

通过步骤S23依次各条连接搜索中点或转向点直到目标点，就可以得到从当前点到目标点可能经过的所有路径。也就是当前进方向上遇到障碍物时，就从障碍物两侧的中点或者转向点绕行，从而分出两条可能的路径，然后从该中点或转向点继续向前探索。直至到达目标点，然后整理出所有可能经过的路径供下一步分析。

下面结合附图对一个具体的路径规划方法进行说明：

图5是根据本发明实施例的工程机械入场路径规划方法的具体场景的示意图，如图5所示，A点是工程机械的当前点，B点是目标点，在A至B的路径当中，存在着多个障碍物，图中已经按照方向包围盒方法将障碍物的轮廓处理为矩形，C、D、E、F点为障碍物之间最短连线的中点。路径搜索的具体步骤为：

以A点到B点为直径的圆形区域划定为入场区域，得出区域内相邻障碍物之间最短连线，并得出最短连线的中点坐标，将结果进行保存。保存的数据结构可以是：

[Zi，Zj，Lij，Mid[X，Y]，Lbj]，其中，Zi为第i个障碍物，Zj为第j个障碍物；Lij：障碍物Zi与Zj之间的最短距离；Mid[X，Y]：最短距离线段的中点坐标；Lbj：B点到各中心点之间的距离。

从A点向B点进行路径搜索，当A到B的方向上遇到障碍物Zx时，获取该障碍物与其他障碍物之间的最短距离中心点Mid[X，Y]，并计算到在AB连线左边的最短中心点C[X1,Y1]，在AB连线右边的最短中心点D[X2,Y2]。

使用二叉树法，分别以左分叉点C为起始点，以垂直与C所在的最短连线的方向继续搜索路径；采用同样的方法，计算两条分支线上E、F，前进方向遇到的障碍物，并进行标识，计算X3、Y3，和X4、Y4的坐标值。

如果前进方向上不存在无障碍物，则一直行驶到入场区域的边缘，如图中的a，d两点，然后在连线该点与B点，依据以上方法进行探索并继续搜索路径。

如果前进方向存在障碍物，但障碍物只有一侧与其他障碍物有最短距离，如图中点a，其前进方向只有障碍物只有一侧存在最短距离中点，此时采用以下三角形计算方法求解从a到障碍物一侧c的路径。点a到障碍物端点b的距离已知，而bc为绕行障碍物时的安全距离，由当ac与bc垂直时，此时绕行距离最短，因此ac与ab的夹角为acrsin（bc/ab），由此可以求解获得c点的坐标，获得a处绕行障碍物的路径。

当路径到达目的点时，认定搜索完毕一条路径，保存本条路径，然后回到上一个分叉节点，搜索其他该节点右分叉点后的其他路径，一直搜索全部节点为止。记录每条路径经过的中心点坐标，并保存所有坐标。

在工程机械转弯过程中，障碍物之间最短距离必须大于外轮廓的最大转弯半径Rmax与内轮廓的最小半径Rmin之差（Rmax-Rmin）+X，其中X为可以预先设置的转弯安全距离裕值。当障碍物物之间的距离小于（Rmax-Rmin）+X时，则说明工程机械不能通过这两个障碍物之间的空间，该条路径需要抛弃。如果通过比较，该路径上所有障碍物之间的距离都大于（Rmax-Rmin）+X，则可以认为该路径畅通。并对所有路径的距离值进行排序输出。

步骤S24计算工程机械按照各条路径入场需要行进的距离可以具体包括：计算各条路径中工程机械转弯的中心点、半径、和转向位置；分别对各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行进的距离。

下面结合附图6和图7，对工程机械穿越障碍物的具体路径及行进距离进行说明，图6是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的示意图，图7是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的流程图，如图所示，目前路径中，工程机械需要从A点首先穿过障碍物Z1和障碍物Z2之间的间隙，然后穿过障碍物Z2和障碍物Z3之间的间隙，障碍物Z1和障碍物Z2之间最短连线BC线段的中点为F，障碍物Z2和障碍物Z3之间最短连线DE线段的中点为G。

步骤S71，求取ED线段延长线与BC线段延长线的交点O1,比较线段O1G和线段O1F长度，记录两者较大的数值为R1，较小数值记录为R2；

步骤S72，判断R1是否大于工程机械的转弯半径，如果R小于工程机械的转弯半径，则可以确定工程机械无法通过这三个障碍之间的缝隙，该条路径需要放弃；

步骤S73，如果R1大于工程机械的转弯半径，延长O1F至O2，使得O1O2=R1+R2，分别以O1和O2为圆心，以R1为半径做弧，获取两条弧线的交点H；

步骤S74，判断角度∠GHF的补角是否大于工程机械的最小转弯角度，如果∠GHF的补角大于工程机械的最小转弯角度，则认为从FHG之间的弧线为工程机械穿越障碍物的具体路径；

步骤S75，如果∠GHF的补角小于工程机械的最小转弯角度，判断当前的转弯半径是否大于5倍的R1，如果当前的转弯半径大于5倍的R1，则可以确定工程机械无法通过这三个障碍之间的缝隙，该条路径需要放弃；

步骤S76，如果当前的转弯半径小于5倍的R1，分别沿GD的方向和FC的方向，以预设的增值△R寻找新的O1点和O2点，使得GO1=GO1+△R，FO2=GO1，△R为预先设定的增值；

重复步骤S73，直至得到弧线路径或者得出工程机械无法通过的结论。

通过以上步骤得到的FHG之间的弧线为工程机械穿越障碍物的具体路径，对以上圆弧、圆心、转向交点、以及行进距离进行保存，可以为工程机械入场时提供参考。

得出所有可能行进的路径后，统计各条路径的行进距离，转向次数，以及工程机械通过障碍物时与障碍物的最小距离。制作成路径选择表，表1是本根据本发明实施例的工程机械入场路径规划方法的路径选择表。

表1路径选择表

序号	路径距离值	转弯次数	最小距离
				1	12	5	8
2	13	4	10
				3	15	7	9
…	…	…	…

通过表1可以依据不同的条件对各条路径进行排序，从距离优先、转弯次数最少优先、最安全优先三个原则中选择优先顺序，系统依据该顺序进行输出最优方案。优选地，可以对以上三种条件按照需求设置权重值，按照加权累加值进行排序，从而综合各条件输出最佳路径。

依次计算出各障碍物间的转弯半径，并将所有的路径进行保存。可以依据保存的各节点的坐标、转弯半径、转弯中心等值来计算出入场路径，指导工程机械入场。

图8是根据本发明实施例的工程机械入场路径规划方法得出的最佳路径的示意图，如图所示，工程机械经由D、E点向B点入场。这种方式的距离最近。从而解决了如图5所示的情况的路径规划。

以上工程机械的入场方法，适用于各种其种起重机、混凝土搅拌、泵送机械、大型运输车辆等。

应用本发明的技术方案，通过计算障碍物二维投影轮廓的最短连线，找出最短连线的中点，在出现障碍物情况下探索从出发点通过上述最短连线终点到达目的施工地点的路线，并在进行路线路径计算后，按照实际工程机械的行进距离计算路径距离，统计转弯次数，安全间距，从而可以综合各项条件对各条路径进行考察，最终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量小，可以实现实时进行路径规划的要求，符合工程机械的自身移动特性。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的控制装置来实现，它们可以集中在单个的控制装置上，或者分布在多个控制装置所组成的网络上，可选地，它们可以用控制装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (12)

1.一种工程机械入场路径规划方法，其特征在于，包括：

将入场区域内的障碍物的三维模型投影到水平面上，得到所述障碍物的二维投影轮廓；

利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线，并标记所述最短连线的中点；

计算工程机械经由所述最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算；

分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和所述工程机械通过障碍物时与障碍物的最小距离；

按照所述需要行进的距离、转弯次数、和所述工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。

2.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线包括：

使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；

分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线，作为每两个相邻所述障碍物之间的最短连线。

3.根据权利要求2所述的工程机械入场路径规划方法，其特征在于，

使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓包括：使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到所述每个障碍物的矩形轮廓；

分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线包括：分别计算每两个相邻所述障碍物的矩形轮廓之间的最短连线。

4.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，计算工程机械经由所述最短连线的中点到达目标位置的各条路径包括：

沿当前点向目标点的前进方向进行路径探索；

当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；

当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过所述最短连线的中点向目标点搜索路径，并记录当前点到所述中点的路径。

5.根据权利要求4所述的工程机械入场路径规划方法，其特征在于，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线之后还包括：

当所述障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据所述工程机械绕行障碍物的安全距离查找距离最短的转向点，通过所述转向点向目标点搜索路径，并记录当前点到转向点的路径。

6.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，对所述各条路径进行路径规划计算之后还包括：

分别判断经过路径计算的各条路径中所有最短连线的距离是否都大于所述工程机械的转弯安全距离；

删除出现最短连线的距离小于所述工程机械的转弯安全距离的路径。

7.根据权利要求6所述的工程机械入场路径规划方法，其特征在于，所述工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。

8.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离包括：

计算经过路径计算的各条路径中工程机械转弯的中心点、半径、和转向位置；

分别对经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到所述各条路径入场需要行进的距离。

9.根据权利要求1至8中任一项所述的工程机械入场路径规划方法，其特征在于，计算工程机械经由所述最短连线的中点到达目标位置的各条路径包括：

使用二叉树算法遍历计算工程机械经由所述最短连线的中点到达目标位置的各条路径。

10.根据权利要求1至8中任一项所述的工程机械入场路径规划方法，其特征在于，对所述各条路径进行路径规划计算包括：

使用迪杰斯特拉算法对所述各条路径分别进行路径规划计算。

11.一种工程机械入场路径规划装置，其特征在于，包括：

投影模块，用于将入场区域内的障碍物的三维模型投影到水平面上，得到所述障碍物的二维投影轮廓；

最短距离中点计算模块，用于利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线，并标记所述最短连线的中点；

路径搜索模块，用于计算工程机械经由所述最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算；

路径分析模块，用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和所述工程机械通过障碍物时与障碍物的最小距离；

路径优化模块，用于按照所述需要行进的距离、转弯次数、和和所述工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。

12.根据权利要求11所述的工程机械入场路径规划装置，其特征在于，还包括：

路径安全评价模块，用于分别判断各条路径中所有最短连线的距离是否都大于所述工程机械的转弯安全距离，并删除出现最短连线的距离小于所述工程机械的转弯安全距离的路径。

Images