专利名称：工程机械入场路径规划方法和路径规划装置的制作方法
技术领域：
本发明涉及工程机械领域，具体而言，涉及一种工程机械入场路径规划方法和路径规划装置。
背景技术：
一般工程机械体积庞大，而在施工环境较为复杂，存在各种障碍物。因此将例如起重机等大型工程机械布置到工位上，需要对工程机械的入场路径进行规划。在工程机械入场路径规划中，必须保证工程机械安全地通过障碍物之间的缝隙，防止出现工程机械与障碍物发生碰撞，另外，由于工程机械的重量和体积都较大，进行路径规划还需要对行进的距离和转向次数进行综合考虑。现有的工程机械入场一般依靠人工勘查，通过操作人员的个人经验进行操作，不能保证是否是最优方案，另外也无法保证顺利通过所有障碍物，有时甚至出现退回重新选择路径的情况。目前有文献中提出使用遍历的方法进行计算距离，首先将障碍物转为凸形物体，对障碍物间的间距进行遍历距离计算，这种方法计算量大，计算耗时长，而且没有充分考虑工程机械的转弯情况，不满足实际工程机械在三维场景中进行规划需求。以上现有技术工程机械入场路径规划中没有结合工程机械实际转弯特点的问题，尚未提出有效的解决方案。
发明内容
本发明旨在提供一种工程机械入场路径规划方法和路径规划装置，以解决现有技术中入场路径规划中没有结合工程机械实际转弯特点的问题。为了实现上述目的，根据本发明的一个方面，提供了一种工程机械入场路径规划方法。本发明提供的工程机械入场路径规划方法包括将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。进一步地，利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线包括使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线，作为每两个相邻障碍物之间的最短连线。进一步地，使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓包括使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的矩形轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线包括分别计算每两个相邻障碍物的矩形轮廓之间的最短连线。进一步地，计算工程机械经由最短连线的中点到达目标位置的各条路径包括沿当前点向目标点的前进方向进行路径探索；当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过最短连线的中点向目标点搜索路径，并记录当前点到该中点的路径。进一步地，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线之后还包括当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据工程机械绕行障碍物的安全距离查找距离最短的转向点，通过转向点向目标点搜索路径，并记录当前点到转向点的路径。进一步地，对各条路径进行路径规划计算之后还包括分别判断经过路径计算的各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离；删除出现最短连线的距离小于工程机械的转弯安全距离的路径。进一步地，工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。进一步地，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离包括计算经过路径计算的各条路径中工程机械转弯的中心点、半径、和转向位置；分别对经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行进的距离。进一步地，计算工程机械经由最短连线的中点到达目标位置的各条路径包括使用二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径。进一步地，对二叉树各条路径进行路径规划计算包括使用迪杰斯特拉算法对二叉树各条路径分别进行路径规划计算。根据本发明的另一个方面，提供了一种工程机械入场路径规划装置。该工程机械入场路径规划装置包括投影�？椋糜诮氤∏蚰诘恼习锏娜Ｐ屯队暗剿矫嫔希玫秸习锏亩队奥掷蛔疃叹嗬胫械慵扑隳？椋糜诶枚队奥掷直鸺扑忝苛礁鱿嗔谡习镏涞淖疃塘撸⒈昙亲疃塘叩闹械悖宦肪端阉髂？椋糜诩扑愎こ袒稻勺疃塘叩闹械愕酱锬勘晡恢玫母魈趼肪叮⒍愿魈趼肪督新肪豆婊扑悖宦肪斗治瞿？椋糜诜直鸺扑愎こ袒蛋凑站肪都扑愕母魈趼肪度氤⌒枰薪木嗬�、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；路径优化�？椋糜诎凑招枰薪木嗬�、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。进一步地，本发明提供的工程机械入场路径规划装置还包括路径安全评价�？椋糜诜直鹋卸细魈趼肪吨兴凶疃塘叩木嗬胧欠穸即笥诠こ袒档淖浒踩嗬耄⑸境鱿肿疃塘叩木嗬胄∮诠こ袒档淖浒踩嗬氲穆肪队τ帽痉⒚鞯募际醴桨福ü扑阏习锒队奥掷淖疃塘撸页鲎疃塘叩闹械悖诔鱿终习锴榭鱿绿剿鞔映龇⒌阃ü鲜鲎疃塘咧盏愕酱锬康氖┕さ氐愕穆废撸⒃诮新废呗肪都扑愫螅凑帐导使こ袒档男薪嗬爰扑懵肪毒嗬耄臣谱浯问踩渚啵佣梢宰酆细飨钐跫愿魈趼肪督锌疾欤钪帐涑鲎钣怕肪斗桨�。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量�。梢允迪质凳苯新肪豆婊囊螅瞎こ袒档淖陨硪贫匦�。
构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中图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、包围球、方向包围盒0BB、固定方向凸包等。由于方向包围盒(Oriented boundingbox)算法是利用最接近的矩形覆盖障碍物的投影轮廓，而矩形之间的最短距离计算相对简单，因此可以优选使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的矩形轮廓；然后分别计算每两个相邻障碍物的矩形轮廓之间的最短连线。路径搜索�？�15的具体执行步骤为沿当前点向目标点的前进方向进行路径探索；当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线；当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过最短连线的中点向目标点搜索路径，并记录当前点到中点的路径。当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据工程机械绕行障碍物的安全距离查找距离最短的转向点，通过转向点向目标点搜索路径,并记录当前点到转向点的路径。依次各条连接搜索中点或转向点直到目标点，就可以得到从当前点到目标点可能经过的所有路径。也就是当前进方向上遇到障碍物时，就从障碍物两侧的中点或者转向点绕行，从而分出两条可能的路径，然后从该中点或转向点继续向前探索。直至到达目标点，然后整理出所有可能经过的路径供下一步优化。路径分析�？�17，计算各条计算得出的路径中工程机械转弯的中心点、半径、和转向位置的步骤可以是分别对各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行进的距离。就是计算工程机械行进的弧线距离而不是直线距离，使各条路径入场需要行进的距离更加符合工程机械入场的要求。路径优化�？�19的排序条件包括需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离。其中，需要行进的距离反映了工程机械行进的距离长短-M弯次数反映了工程机械的路线控制难度；工程机械通过障碍物时与障碍物的最小距离反映了安全程度。用户可以根据需要选择排序条件，或者对以上排序设置权重值进行综合计算，得到最符合用户要求的入场路径。以上路径搜索�？�15计算路径以及进行路径规划的具体算法可以为用于利用二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径，并使用迪杰斯特拉算法(Di jkstra)对各条路径进行路径规划计算。迪杰斯特拉(Di jkstra)算法是现有典型的单源最短路径算法，用于计算一个节点到其他所有节点的最短路径。以起始点为中心向外层层扩展，直到扩展到终点为止。本发明实施例还提供了一种工程机械入场路径规划方法，该工程机械入场路径规划方法可以通过本发明上述实施例所提供的任一种工程机械入场路径规划装置来执行。图2是根据本发明实施例的工程机械入场路径规划方法的示意图，如图所示，该工程机械入场路径规划方法包括步骤S21，将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；步骤S22，利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；步骤S23，计算工程机械经由最短连线的中点到达目标位置的各条路径，并对各条路径进行路径规划计算；步骤S24，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；步骤S25，按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对各条路径进行排序，并根据排序结果输出各条入场路径。利用该方法，计算障碍物二维投影轮廓的最短连线，找出最短连线的中点，探索从出发点通过上述最短连线终点到达目的施工地点的路线，并按照实际工程机械的行进距离计算路径距离，统计转弯次数，安全间距，从而可以综合各项条件对各条路径进行考察，最终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题，计算量�。梢允迪质凳苯新肪豆婊囊螅瞎こ袒档淖陨硪贫匦�。采用投影的思路将三维物体之间最短距离计算转化二维物体间的距离的求解，简化求解，降低计算量；采用包围盒方法将物体进行封装来求解，将物体进行封装方式求解，简化求解方法，同时求解精度损失较少；采用从障碍物中心点处入手来寻找最安全的入场路径，符合实际路径规划需求；提出解决完整的工程机械入场路径实现方法，并分步骤给出求解实现的方法。其中步骤S21具体可以包括使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓；分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线，作为每两个相邻障碍物之间的最短连线。层次包围盒法是用体积略大而几何特性简单的包围盒来近似地描述复杂的对象，从而只对包围盒重叠的对象进行进一步的相交测试，常见的包围盒类型有沿坐标轴的包围盒AABB、包围球、方向包围盒OBB、固定方向凸包等。由于方向包围盒(Oriented boundingbox)算法是利用最接近的矩形覆盖障碍物的投影轮廓，而矩形之间的最短距离计算相对简单。因此可以优选使用方向包围盒的算法，这种情况下，步骤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[XI，Yl]，在AB连线右边的最短中心点D[X2, Y2]。使用二叉树法，分别以左分叉点C为起始点，以垂直与C所在的最短连线的方向继续搜索路径；采用同样的方法，计算两条分支线上E、F，前进方向遇到的障碍物，并进行标识，计算X3、Y3，和X4、Y4的坐标值。如果前进方向上不存在无障碍物，则一直行驶到入场区域的边缘，如图中的a，d两点，然后在连线该点与B点，依据以上方法进行探索并继续搜索路径。如果前进方向存在障碍物，但障碍物只有一侧与其他障碍物有最短距离，如图中点a，其前进方向只有障碍物只有一侧存在最短距离中点，此时采用以下三角形计算方法求解从a到障碍物一侧c的路径。点a到障碍物端点b的距离已知，而be为绕行障碍物时的安全距离，由当ac与be垂直时,此时绕行距离最短，因此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点首先穿过障碍物Zl和障碍物Z2之间的间隙，然后穿过障碍物Z2和障碍物Z3之间的间隙，障碍物Zl和障碍物Z2之间最短连线BC线段的中点为F，障碍物Z2和障碍物Z3之间最短连线DE线段的中点为G。步骤S71，求取ED线段延长线与BC线段延长线的交点01，比较线段OlG和线段OlF长度，记录两者较大的数值为Rl，较小数值记录为R2 ；
步骤S72，判断Rl是否大于工程机械的转弯半径，如果R小于工程机械的转弯半径，则可以确定工程机械无法通过这三个障碍之间的缝隙，该条路径需要放弃；步骤S73，如果Rl大于工程机械的转弯半径，延长OlF至02，使得0102=R1+R2，分别以01和02为圆心，以Rl为半径做�。袢×教趸∠叩慕坏鉎 ；步骤S74，判断角度Z GHF的补角是否大于工程机械的最小转弯角度，如果Z GHF的补角大于工程机械的最小转弯角度，则认为从FHG之间的弧线为工程机械穿越障碍物的具体路径；步骤S75，如果Z GHF的补角小于工程机械的最小转弯角度，判断当前的转弯半径是否大于5倍的R1，如果当前的转弯半径大于5倍的R1，则可以确定工程机械无法通过这三个障碍之间的缝隙，该条路径需要放弃；步骤S76，如果当前的转弯半径小于5倍的Rl，分别沿⑶的方向和FC的方向，以预设的增值A R寻找新的01点和02点，使得GOl=GOl+ A R，F02=G01, A R为预先设定的
增值；重复步骤S73，直至得到弧线路径或者得出工程机械无法通过的结论。通过以上步骤得到的FHG之间的弧线为工程机械穿越障碍物的具体路径，对以上圆弧、圆心、转向交点、以及行进距离进行保存，可以为工程机械入场时提供参考。得出所有可能行进的路径后，统计各条路径的行进距离，转向次数，以及工程机械通过障碍物时与障碍物的最小距离。制作成路径选择表，表I是本根据本发明实施例的工程机械入场路径规划方法的路径选择表。表I路径选择表
权利要求
1.一种工程机械入场路径规划方法，其特征在于，包括 将入场区域内的障碍物的三维模型投影到水平面上，得到所述障碍物的二维投影轮廓； 利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线，并标记所述最短连线的中点； 计算工程机械经由所述最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算； 分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和所述工程机械通过障碍物时与障碍物的最小距离； 按照所述需要行进的距离、转弯次数、和所述工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。
2.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线包括 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓； 分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线，作为每两个相邻所述障碍物之间的最短连线。
3.根据权利要求2所述的工程机械入场路径规划方法，其特征在于， 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算，得到每个障碍物的包围盒轮廓包括使用方向包围盒对每个障碍物的二维投影轮廓进行计算，得到所述每个障碍物的矩形轮廓； 分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线包括分别计算每两个相邻所述障碍物的矩形轮廓之间的最短连线。
4.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，计算工程机械经由所述最短连线的中点到达目标位置的各条路径包括 沿当前点向目标点的前进方向进行路径探索； 当前进方向上遇到障碍物时，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线； 当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时，通过所述最短连线的中点向目标点搜索路径，并记录当前点到所述中点的路径。
5.根据权利要求4所述的工程机械入场路径规划方法，其特征在于，判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线之后还包括 当所述障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时，根据所述工程机械绕行障碍物的安全距离查找距离最短的转向点，通过所述转向点向目标点搜索路径，并记录当前点到转向点的路径。
6.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，对所述各条路径进行路径规划计算之后还包括 分别判断经过路径计算的各条路径中所有最短连线的距离是否都大于所述工程机械的转弯安全距离；删除出现最短连线的距离小于所述工程机械的转弯安全距离的路径。
7.根据权利要求6所述的工程机械入场路径规划方法，其特征在于，所述工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。
8.根据权利要求1所述的工程机械入场路径规划方法，其特征在于，分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离包括 计算经过路径计算的各条路径中工程机械转弯的中心点、半径、和转向位置； 分别对经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到所述各条路径入场需要行进的距离。
9.根据权利要求1至8中任一项所述的工程机械入场路径规划方法，其特征在于，计算工程机械经由所述最短连线的中点到达目标位置的各条路径包括 使用二叉树算法遍历计算工程机械经由所述最短连线的中点到达目标位置的各条路径。
10.根据权利要求1至8中任一项所述的工程机械入场路径规划方法，其特征在于，对所述各条路径进行路径规划计算包括 使用迪杰斯特拉算法对所述各条路径分别进行路径规划计算。
11.一种工程机械入场路径规划装置，其特征在于，包括 投影�？椋糜诮氤∏蚰诘恼习锏娜Ｐ屯队暗剿矫嫔希玫剿稣习锏亩队奥掷� 最短距离中点计算�？椋糜诶盟龆队奥掷直鸺扑忝苛礁鱿嗔谒稣习镏涞淖疃塘撸⒈昙撬鲎疃塘叩闹械悖� 路径搜索模块，用于计算工程机械经由所述最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算； 路径分析�？椋糜诜直鸺扑愎こ袒蛋凑站肪都扑愕母魈趼肪度氤⌒枰薪木嗬搿⒆浯问�、和所述工程机械通过障碍物时与障碍物的最小距离； 路径优化模块，用于按照所述需要行进的距离、转弯次数、和和所述工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并根据排序结果输出各条入场路径。
12.根据权利要求11所述的工程机械入场路径规划装置，其特征在于，还包括 路径安全评价�？椋糜诜直鹋卸细魈趼肪吨兴凶疃塘叩木嗬胧欠穸即笥谒龉こ袒档淖浒踩嗬耄⑸境鱿肿疃塘叩木嗬胄∮谒龉こ袒档淖浒踩嗬氲穆肪�。
全文摘要
本发明提供了一种工程机械入场路径规划方法和路径规划装置。该工程机械的入场路径规划方法包括将入场区域内的障碍物的三维模型投影到水平面上，得到障碍物的二维投影轮廓；利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线，并标记最短连线的中点；计算工程机械经由最短连线的中点到达目标位置的各条路径，并对所述各条路径进行路径规划计算；分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离；按照需要行进的距离、转弯次数、和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各条路径进行排序，并输出结果。从而最终得出最优路径方案。
文档编号G01C21/00GK103017757SQ20121052028
公开日2013年4月3日 申请日期2012年12月6日 优先权日2012年12月6日
发明者曾杨, 黄文华, 范卿, 李义 申请人:中联重科股份有限公司
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
