《工程机械入场路径规划方法和路径规划装置.pdf》由会员分享，可在线阅读，更多相关《工程机械入场路径规划方法和路径规划装置.pdf（17页珍藏版）》请在专利查询网上搜索。
1、(10)申请公布号 CN 103017757 A (43)申请公布日 2013.04.03 CN 103017757 A *CN103017757A* (21)申请号 201210520284.8 (22)申请日 2012.12.06 G01C 21/00(2006.01) (71)申请人 中联重科股份有限公司 地址 410013 湖南省长沙市岳麓区银盆南路 361 号 (72)发明人 曾杨 黄文华 范卿 李义 (74)专利代理机构 北京康信知识产权代理有限 责任公司 11240 代理人 吴贵明 张永明 (54) 发明名称 工程机械入场路径规划方法和路径规划装置 (57) 摘要 本发明提供了一。
2、种工程机械入场路径规划方 法和路径规划装置。该工程机械的入场路径规划 方法包括 ： 将入场区域内的障碍物的三维模型投 影到水平面上， 得到障碍物的二维投影轮廓 ； 利 用二维投影轮廓分别计算每两个相邻障碍物之 间的最短连线， 并标记最短连线的中点 ； 计算工 程机械经由最短连线的中点到达目标位置的各条 路径， 并对所述各条路径进行路径规划计算 ； 分 别计算工程机械按照经过路径计算的各条路径入 场需要行进的距离、 转弯次数、 和工程机械通过障 碍物时与障碍物的最小距离 ； 按照需要行进的距 离、 转弯次数、 和工程机械通过障碍物时与障碍物 的最小距离对经过路径计算的各条路径进行排 序， 并输出。
3、结果。从而最终得出最优路径方案。 (51)Int.Cl. 权利要求书 2 页 说明书 9 页 附图 5 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书 2 页 说明书 9 页 附图 5 页 1/2 页 2 1. 一种工程机械入场路径规划方法， 其特征在于， 包括 ： 将入场区域内的障碍物的三维模型投影到水平面上， 得到所述障碍物的二维投影轮 廓 ； 利用所述二维投影轮廓分别计算每两个相邻所述障碍物之间的最短连线， 并标记所述 最短连线的中点 ； 计算工程机械经由所述最短连线的中点到达目标位置的各条路径， 并对所述各条路径 进行路径规划计算 ； 分别计算工程机械按照经。
4、过路径计算的各条路径入场需要行进的距离、 转弯次数、 和 所述工程机械通过障碍物时与障碍物的最小距离 ； 按照所述需要行进的距离、 转弯次数、 和所述工程机械通过障碍物时与障碍物的最小 距离对经过路径计算的各条路径进行排序， 并根据排序结果输出各条入场路径。 2. 根据权利要求 1 所述的工程机械入场路径规划方法， 其特征在于， 利用所述二维投 影轮廓分别计算每两个相邻所述障碍物之间的最短连线包括 ： 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算， 得到每个障碍物的包 围盒轮廓 ； 分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线， 作为每两个相邻所述 障碍物之间的最短连线。 。
5、3. 根据权利要求 2 所述的工程机械入场路径规划方法， 其特征在于， 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算， 得到每个障碍物的包 围盒轮廓包括 ： 使用方向包围盒对每个障碍物的二维投影轮廓进行计算， 得到所述每个障 碍物的矩形轮廓 ； 分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线包括 ： 分别计算每两个 相邻所述障碍物的矩形轮廓之间的最短连线。 4. 根据权利要求 1 所述的工程机械入场路径规划方法， 其特征在于， 计算工程机械经 由所述最短连线的中点到达目标位置的各条路径包括 ： 沿当前点向目标点的前进方向进行路径探索 ； 当前进方向上遇到障碍物时， 判断遇到的。
6、障碍物的至少一侧是否存在该障碍物到与该 障碍物相邻的障碍物的最短连线 ； 当遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时， 通过所述最短连线的中点向目标点搜索路径， 并记录当前点到所述中点的路径。 5. 根据权利要求 4 所述的工程机械入场路径规划方法， 其特征在于， 判断遇到的障碍 物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线之后还包括 ： 当所述障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线时， 根据所述工程机械绕行障碍物的安全距离查找距离最短的转向点， 通过所述转向点向目标 点搜索路径， 并记录当前点到转向点的路径。 6. 根。
7、据权利要求 1 所述的工程机械入场路径规划方法， 其特征在于， 对所述各条路径 进行路径规划计算之后还包括 ： 分别判断经过路径计算的各条路径中所有最短连线的距离是否都大于所述工程机械 的转弯安全距离 ； 权 利 要 求 书 CN 103017757 A 2 2/2 页 3 删除出现最短连线的距离小于所述工程机械的转弯安全距离的路径。 7. 根据权利要求 6 所述的工程机械入场路径规划方法， 其特征在于， 所述工程机械的 转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓的最小半径之差加上预设裕值。 8. 根据权利要求 1 所述的工程机械入场路径规划方法， 其特征在于， 分别计算工程机 械按照。
8、经过路径计算的各条路径入场需要行进的距离包括 ： 计算经过路径计算的各条路径中工程机械转弯的中心点、 半径、 和转向位置 ； 分别对经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到所述各条 路径入场需要行进的距离。 9.根据权利要求1至8中任一项所述的工程机械入场路径规划方法， 其特征在于， 计算 工程机械经由所述最短连线的中点到达目标位置的各条路径包括 ： 使用二叉树算法遍历计算工程机械经由所述最短连线的中点到达目标位置的各条路 径。 10. 根据权利要求 1 至 8 中任一项所述的工程机械入场路径规划方法， 其特征在于， 对 所述各条路径进行路径规划计算包括 ： 使用迪杰斯特拉算。
9、法对所述各条路径分别进行路径规划计算。 11. 一种工程机械入场路径规划装置， 其特征在于， 包括 ： 投影模块， 用于将入场区域内的障碍物的三维模型投影到水平面上， 得到所述障碍物 的二维投影轮廓 ； 最短距离中点计算模块， 用于利用所述二维投影轮廓分别计算每两个相邻所述障碍物 之间的最短连线， 并标记所述最短连线的中点 ； 路径搜索模块， 用于计算工程机械经由所述最短连线的中点到达目标位置的各条路 径， 并对所述各条路径进行路径规划计算 ； 路径分析模块， 用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的 距离、 转弯次数、 和所述工程机械通过障碍物时与障碍物的最小距离 ； 路。
10、径优化模块， 用于按照所述需要行进的距离、 转弯次数、 和和所述工程机械通过障碍 物时与障碍物的最小距离对经过路径计算的各条路径进行排序， 并根据排序结果输出各条 入场路径。 12. 根据权利要求 11 所述的工程机械入场路径规划装置， 其特征在于， 还包括 ： 路径安全评价模块， 用于分别判断各条路径中所有最短连线的距离是否都大于所述工 程机械的转弯安全距离， 并删除出现最短连线的距离小于所述工程机械的转弯安全距离的 路径。 权 利 要 求 书 CN 103017757 A 3 1/9 页 4 工程机械入场路径规划方法和路径规划装置 技术领域 0001 本发明涉及工程机械领域， 具体而言， 。
11、涉及一种工程机械入场路径规划方法和路 径规划装置。 背景技术 0002 一般工程机械体积庞大， 而在施工环境较为复杂， 存在各种障碍物。 因此将例如起 重机等大型工程机械布置到工位上， 需要对工程机械的入场路径进行规划。 0003 在工程机械入场路径规划中， 必须保证工程机械安全地通过障碍物之间的缝隙， 防止出现工程机械与障碍物发生碰撞， 另外， 由于工程机械的重量和体积都较大， 进行路径 规划还需要对行进的距离和转向次数进行综合考虑。 0004 现有的工程机械入场一般依靠人工勘查， 通过操作人员的个人经验进行操作， 不 能保证是否是最优方案， 另外也无法保证顺利通过所有障碍物， 有时甚至出现。
12、退回重新选 择路径的情况。目前有文献中提出使用遍历的方法进行计算距离， 首先将障碍物转为凸形 物体， 对障碍物间的间距进行遍历距离计算， 这种方法计算量大， 计算耗时长， 而且没有充 分考虑工程机械的转弯情况， 不满足实际工程机械在三维场景中进行规划需求。 0005 以上现有技术工程机械入场路径规划中没有结合工程机械实际转弯特点的问题， 尚未提出有效的解决方案。 发明内容 0006 本发明旨在提供一种工程机械入场路径规划方法和路径规划装置， 以解决现有技 术中入场路径规划中没有结合工程机械实际转弯特点的问题。 0007 为了实现上述目的， 根据本发明的一个方面， 提供了一种工程机械入场路径规划。
13、 方法。本发明提供的工程机械入场路径规划方法包括 ： 将入场区域内的障碍物的三维模型 投影到水平面上， 得到障碍物的二维投影轮廓 ； 利用二维投影轮廓分别计算每两个相邻障 碍物之间的最短连线， 并标记最短连线的中点 ； 计算工程机械经由最短连线的中点到达目 标位置的各条路径， 并对各条路径进行路径规划计算 ； 分别计算工程机械按照经过路径计 算的各条路径入场需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小 距离 ； 按照需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离对经 过路径计算的各条路径进行排序， 并根据排序结果输出各条入场路径。 0008 进一步。
14、地， 利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线包括 ： 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算， 得到每个障碍物的包围盒 轮廓 ； 分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线， 作为每两个相邻障碍物 之间的最短连线。 0009 进一步地， 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计算， 得到 每个障碍物的包围盒轮廓包括 ： 使用方向包围盒对每个障碍物的二维投影轮廓进行计算， 得到每个障碍物的矩形轮廓 ； 分别计算每两个相邻障碍物的包围盒轮廓之间的最短连线包 说 明 书 CN 103017757 A 4 2/9 页 5 括 ： 分别计算每两个相邻障。
15、碍物的矩形轮廓之间的最短连线。 0010 进一步地， 计算工程机械经由最短连线的中点到达目标位置的各条路径包括 ： 沿 当前点向目标点的前进方向进行路径探索 ； 当前进方向上遇到障碍物时， 判断遇到的障碍 物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物的最短连线 ； 当遇到的障碍物 的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时， 通过最短连线的中点 向目标点搜索路径， 并记录当前点到该中点的路径。 0011 进一步地， 判断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的 障碍物的最短连线之后还包括 ： 当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的 障碍物的最。
16、短连线时， 根据工程机械绕行障碍物的安全距离查找距离最短的转向点， 通过 转向点向目标点搜索路径， 并记录当前点到转向点的路径。 0012 进一步地， 对各条路径进行路径规划计算之后还包括 ： 分别判断经过路径计算的 各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离 ； 删除出现最短连线 的距离小于工程机械的转弯安全距离的路径。 0013 进一步地， 工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓 的最小半径之差加上预设裕值。 0014 进一步地， 分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离 包括 ： 计算经过路径计算的各条路径中工程机械转弯的中心点。
17、、 半径、 和转向位置 ； 分别对 经过路径计算的各条路径中工程机械转弯的弧线距离进行累加得到各条路径入场需要行 进的距离。 0015 进一步地， 计算工程机械经由最短连线的中点到达目标位置的各条路径包括 ： 使 用二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径。 0016 进一步地， 对二叉树各条路径进行路径规划计算包括 ： 使用迪杰斯特拉算法对二 叉树各条路径分别进行路径规划计算。 0017 根据本发明的另一个方面， 提供了一种工程机械入场路径规划装置。该工程机械 入场路径规划装置包括 ： 投影模块， 用于将入场区域内的障碍物的三维模型投影到水平面 上， 得到障碍物的二。
18、维投影轮廓 ； 最短距离中点计算模块， 用于利用二维投影轮廓分别计算 每两个相邻障碍物之间的最短连线， 并标记最短连线的中点 ； 路径搜索模块， 用于计算工程 机械经由最短连线的中点到达目标位置的各条路径， 并对各条路径进行路径规划计算 ； 路 径分析模块， 用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离 ； 路径优化模块， 用于按照需要行 进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离对经过路径计算的各 条路径进行排序， 并根据排序结果输出各条入场路径。 0018 进一步地， 本发明提供的工程机械入场。
19、路径规划装置还包括 ： 路径安全评价模块， 用于分别判断各条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离， 并删 除出现最短连线的距离小于工程机械的转弯安全距离的路径 0019 应用本发明的技术方案， 通过计算障碍物二维投影轮廓的最短连线， 找出最短连 线的中点， 在出现障碍物情况下探索从出发点通过上述最短连线终点到达目的施工地点的 路线， 并在进行路线路径计算后， 按照实际工程机械的行进距离计算路径距离， 统计转弯次 数， 安全间距， 从而可以综合各项条件对各条路径进行考察， 最终输出最优路径方案。解决 说 明 书 CN 103017757 A 5 3/9 页 6 了已知施工环境。
20、的三维场景后的工程机械入场路径规划问题， 计算量小， 可以实现实时进 行路径规划的要求， 符合工程机械的自身移动特性。 附图说明 0020 构成本申请的一部分的说明书附图用来提供对本发明的进一步理解， 本发明的示 意性实施例及其说明用于解释本发明， 并不构成对本发明的不当限定。在附图中 ： 0021 图 1 是根据本发明实施例的工程机械入场路径规划装置的示意图 ； 0022 图 2 是根据本发明实施例的工程机械入场路径规划方法的示意图 ； 0023 图 3A 是根据本发明实施例的工程机械入场路径规划方法对障碍物的三维模型投 影到水平面的示意图 ； 0024 图 3B 是根据本发明实施例的工程机。
21、械入场路径规划方法使用方向包围盒对每个 障碍物的二维投影轮廓进行计算的结果示意图 ； 0025 图 4A 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第 一种情况 ； 0026 图 4B 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第 二种情况 ； 0027 图 4C 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第 三种情况 ； 0028 图 5 是根据本发明实施例的工程机械入场路径规划方法的具体场景的示意图 ； 0029 图 6 是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路 径的示意图 ； 0030 图 7 是根据本发。
22、明实施例的工程机械入场路径规划方法的穿越障碍物的具体路 径的流程图 ； 0031 图 8 是根据本发明实施例的工程机械入场路径规划方法得出的最佳路径的示意 图。 具体实施方式 0032 需要说明的是， 在不冲突的情况下， 本申请中的实施例及实施例中的特征可以相 互组合。下面将参考附图并结合实施例来详细说明本发明。 0033 本发明实施例提供了一种工程机械入场路径规划装置， 图 1 是根据本发明实施例 的工程机械入场路径规划装置的示意图， 如图 1 所示， 本发明实施例的工程机械入场路径 规划装置包括 ： 投影模块 11， 用于将入场区域内的障碍物的三维模型投影到水平面上， 得 到障碍物的二维投。
23、影轮廓 ； 最短距离中点计算模块 13， 用于利用二维投影轮廓分别计算每 两个相邻障碍物之间的最短连线， 并标记最短连线的中点 ； 路径搜索模块 15， 用于计算工 程机械经由最短连线的中点到达目标位置的各条路径， 并对各条路径进行路径规划计算 ； 路径分析模块 17， 用于分别计算工程机械按照经过路径计算的各条路径入场需要行进的距 离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离 ； 路径优化模块 19， 用于按照 需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离对各条路径进 行排序， 并根据排序结果输出各条入场路径。 说 明 书 CN 103017757 A。
24、 6 4/9 页 7 0034 使用本发明的工程机械入场路径规划装置， 计算障碍物二维投影轮廓的最短连 线， 找出最短连线的中点， 探索从出发点通过上述最短连线终点到达目的施工地点的路线 并进行路径规划计算， 对经过路径计算的路线按照实际工程机械的行进距离计算路径距 离， 统计转弯次数， 安全间距， 从而可以综合各项条件对各条路径进行考察， 最终输出最优 路径方案。 解决了已知施工环境的三维场景后的工程机械入场路径规划问题， 计算量小， 可 以实现实时进行路径规划的要求， 符合工程机械的自身移动特性。 0035 为了验证工程机械是否安全顺利通过障碍物之间的空间， 本发明可应用实施的工 程机械入。
25、场路径规划装置， 还可以包括 ： 路径安全评价模块 （图中未示出） ， 用于分别判断各 条路径中所有最短连线的距离是否都大于工程机械的转弯安全距离， 并删除出现最短连线 的距离小于工程机械的转弯安全距离的路径。 使保留下来的所有路径都符合工程机械的安 全通过要求。其中， 工程机械的转弯安全距离为工程机械外轮廓的最大转弯半径与内轮廓 的最小半径之差加上预设裕值。 具体地， 在转弯过程中， 障碍物之间最短距离必须大于外轮 廓的最大转弯半径 Rmax 与内轮廓的最小半径 Rmin 之差 （Rmax-Rmin） +X， 其中 X 为可以预 先设置的转弯安全距离裕值。当障碍物物之间的距离小于 （Rmax。
26、-Rmin） +X 时， 则说明工程 机械不能通过这两个障碍物之间的空间， 该条路径需要抛弃。 如果通过比较， 该路径上所有 障碍物之间的距离都大于 （Rmax-Rmin） +X， 则可以认为该路径畅通。 0036 施工场地中的障碍物可能是建筑、 堆放材料、 已经入场的工程机械、 或者自然存在 的物体， 形状是不规则的， 因此直接计算障碍物之间的最短距离计算量大。因此投影模块 11 的具体执行步骤可以是 ： 使用层次包围盒法分别对每个障碍物的二维投影轮廓进行计 算， 得到每个障碍物的包围盒轮廓 ； 分别计算每两个相邻障碍物的包围盒轮廓之间的最短 连线， 作为每两个相邻障碍物之间的最短连线。 0。
27、037 层次包围盒法是用体积略大而几何特性简单的包围盒来近似地描述复杂的对象， 从而只对包围盒重叠的对象进行进一步的相交测试， 常见的包围盒类型有沿坐标轴的包围 盒 AABB、 包围球、 方向包围盒 OBB、 固定方向凸包等。由于方向包围盒 （Oriented bounding box） 算法是利用最接近的矩形覆盖障碍物的投影轮廓， 而矩形之间的最短距离计算相对简 单， 因此可以优选使用方向包围盒对每个障碍物的二维投影轮廓进行计算， 得到每个障碍 物的矩形轮廓 ； 然后分别计算每两个相邻障碍物的矩形轮廓之间的最短连线。 0038 路径搜索模块 15 的具体执行步骤为 ： 沿当前点向目标点的前进。
28、方向进行路径探 索 ； 当前进方向上遇到障碍物时， 判断遇到的障碍物的至少一侧是否存在该障碍物到与该 障碍物相邻的障碍物的最短连线 ； 当遇到的障碍物的至少一侧存在该障碍物到与该障碍物 相邻的障碍物的最短连线时， 通过最短连线的中点向目标点搜索路径， 并记录当前点到中 点的路径。当障碍物的至少一侧不存在该障碍物到与该障碍物相邻的障碍物的最短连线 时， 根据工程机械绕行障碍物的安全距离查找距离最短的转向点， 通过转向点向目标点搜 索路径， 并记录当前点到转向点的路径。 依次各条连接搜索中点或转向点直到目标点， 就可 以得到从当前点到目标点可能经过的所有路径。也就是当前进方向上遇到障碍物时， 就从。
29、 障碍物两侧的中点或者转向点绕行， 从而分出两条可能的路径， 然后从该中点或转向点继 续向前探索。直至到达目标点， 然后整理出所有可能经过的路径供下一步优化。 0039 路径分析模块 17， 计算各条计算得出的路径中工程机械转弯的中心点、 半径、 和转 向位置的步骤可以是分别对各条路径中工程机械转弯的弧线距离进行累加得到各条路径 说 明 书 CN 103017757 A 7 5/9 页 8 入场需要行进的距离。就是计算工程机械行进的弧线距离而不是直线距离， 使各条路径入 场需要行进的距离更加符合工程机械入场的要求。 0040 路径优化模块 19 的排序条件包括 ： 需要行进的距离、 转弯次数、。
30、 和工程机械通过 障碍物时与障碍物的最小距离。 其中， 需要行进的距离反映了工程机械行进的距离长短 ； 转 弯次数反映了工程机械的路线控制难度 ； 工程机械通过障碍物时与障碍物的最小距离反映 了安全程度。 用户可以根据需要选择排序条件， 或者对以上排序设置权重值进行综合计算， 得到最符合用户要求的入场路径。 0041 以上路径搜索模块 15 计算路径以及进行路径规划的具体算法可以为 ： 用于利用 二叉树算法遍历计算工程机械经由最短连线的中点到达目标位置的各条路径， 并使用迪杰 斯特拉算法 （Dijkstra） 对各条路径进行路径规划计算。迪杰斯特拉 （Dijkstra） 算法是现 有典型的单源。
31、最短路径算法， 用于计算一个节点到其他所有节点的最短路径。以起始点为 中心向外层层扩展， 直到扩展到终点为止。 0042 本发明实施例还提供了一种工程机械入场路径规划方法， 该工程机械入场路径规 划方法可以通过本发明上述实施例所提供的任一种工程机械入场路径规划装置来执行。 图 2 是根据本发明实施例的工程机械入场路径规划方法的示意图， 如图所示， 该工程机械入场 路径规划方法包括 ： 0043 步骤 S21， 将入场区域内的障碍物的三维模型投影到水平面上， 得到障碍物的二维 投影轮廓 ； 0044 步骤 S22， 利用二维投影轮廓分别计算每两个相邻障碍物之间的最短连线， 并标记 最短连线的中点。
32、 ； 0045 步骤 S23， 计算工程机械经由最短连线的中点到达目标位置的各条路径， 并对各条 路径进行路径规划计算 ； 0046 步骤 S24， 分别计算工程机械按照经过路径计算的各条路径入场需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的最小距离 ； 0047 步骤 S25， 按照需要行进的距离、 转弯次数、 和工程机械通过障碍物时与障碍物的 最小距离对各条路径进行排序， 并根据排序结果输出各条入场路径。 0048 利用该方法， 计算障碍物二维投影轮廓的最短连线， 找出最短连线的中点， 探索从 出发点通过上述最短连线终点到达目的施工地点的路线， 并按照实际工程机械的行进距离。
33、 计算路径距离， 统计转弯次数， 安全间距， 从而可以综合各项条件对各条路径进行考察， 最 终输出最优路径方案。解决了已知施工环境的三维场景后的工程机械入场路径规划问题， 计算量小， 可以实现实时进行路径规划的要求， 符合工程机械的自身移动特性。 采用投影的 思路将三维物体之间最短距离计算转化二维物体间的距离的求解， 简化求解， 降低计算量 ； 采用包围盒方法将物体进行封装来求解， 将物体进行封装方式求解， 简化求解方法， 同时求 解精度损失较少 ； 采用从障碍物中心点处入手来寻找最安全的入场路径， 符合实际路径规 划需求 ； 提出解决完整的工程机械入场路径实现方法， 并分步骤给出求解实现的方。
34、法。 0049 其中步骤 S21 具体可以包括 ： 使用层次包围盒法分别对每个障碍物的二维投影轮 廓进行计算， 得到每个障碍物的包围盒轮廓 ； 分别计算每两个相邻障碍物的包围盒轮廓之 间的最短连线， 作为每两个相邻障碍物之间的最短连线。 0050 层次包围盒法是用体积略大而几何特性简单的包围盒来近似地描述复杂的对象， 说 明 书 CN 103017757 A 8 6/9 页 9 从而只对包围盒重叠的对象进行进一步的相交测试， 常见的包围盒类型有沿坐标轴的包围 盒 AABB、 包围球、 方向包围盒 OBB、 固定方向凸包等。由于方向包围盒 （Oriented bounding box） 算法是利。
35、用最接近的矩形覆盖障碍物的投影轮廓， 而矩形之间的最短距离计算相对简 单。因此可以优选使用方向包围盒的算法， 这种情况下， 步骤 S21 中使用层次包围盒法分别 对每个障碍物的二维投影轮廓进行计算， 得到每个障碍物的包围盒轮廓可以具体包括 ： 使 用方向包围盒对每个障碍物的二维投影轮廓进行计算， 得到所述每个障碍物的矩形轮廓 ； 分别计算每两个相邻所述障碍物的包围盒轮廓之间的最短连线可以具体包括 ： 分别计算每 两个相邻所述障碍物的矩形轮廓之间的最短连线。 0051 图 3A 是根据本发明实施例的工程机械入场路径规划方法对障碍物的三维模型投 影到水平面的示意图， 图 3B 是根据本发明实施例的。
36、工程机械入场路径规划方法使用方向 包围盒对每个障碍物的二维投影轮廓进行计算的结果示意图， 如图所示， 一般是同场地上 的障碍物位置和障碍物的形状是不规则的， 对其直接进行距离计算， 计算量大而且对求解 难度大， 使用方向包围和计算后， 将不规则的轮廓使用最接近的矩形代替。 0052 图 4A 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的第 一种情况， 图 4B 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离的 第二种情况， 图 4C 是根据本发明实施例的工程机械入场路径规划方法矩形轮廓最短距离 的第三种情况， 在图 4A 中， 两个障碍物的矩形轮廓之间的最短距离。
37、为与第一障碍物的一条 边的垂线到第二障碍物一个顶点的距离 ； 在图 4B 中， 两个障碍物的矩形轮廓之间的最短距 离为第一障碍物的一个顶点到第二障碍物一条边的垂线的长度。在图 4C 中， 两个障碍物的 矩形轮廓之间的最短距离为一个矩形顶点到另一个障碍物矩形顶点。因此， 可以通过比较 矩形轮廓各顶点到矩形各点、 线之间的距离的方式求解两矩形间的最短距离。 0053 步骤 S23 具体可以包括 ： 沿当前点向目标点的前进方向进行路径探索 ； 当前进方 向上遇到障碍物时用二叉树方法来规划路径， 具体为使用二叉树法向障碍物两侧分叉， 判 断遇到的障碍物的至少一侧是否存在该障碍物到与该障碍物相邻的障碍物。
38、的最短连线 ； 当 遇到的障碍物的至少一侧存在该障碍物到与该障碍物相邻的障碍物的最短连线时， 通过最 短连线的中点向目标点搜索路径， 并记录当前点到中点的路径。当障碍物的至少一侧不存 在该障碍物到与该障碍物相邻的障碍物的最短连线时， 根据工程机械绕行障碍物的安全距 离查找距离最短的转向点， 通过转向点向目标点搜索路径， 并记录当前点到转向点的路径。 从而， 步骤 S23 计算工程机械经由最短连线的中点到达目标位置的各条路径的具体算法为 使用二叉树算法进行遍历计算。 0054 步骤 S23 中对各条路径进行路径规划计算的算法可以具体为 ： 使用迪杰斯特拉算 法对所述各条路径分别进行路径规划计算。。
39、 0055 通过步骤 S23 依次各条连接搜索中点或转向点直到目标点， 就可以得到从当前点 到目标点可能经过的所有路径。也就是当前进方向上遇到障碍物时， 就从障碍物两侧的中 点或者转向点绕行， 从而分出两条可能的路径， 然后从该中点或转向点继续向前探索。 直至 到达目标点， 然后整理出所有可能经过的路径供下一步分析。 0056 下面结合附图对一个具体的路径规划方法进行说明 ： 0057 图 5 是根据本发明实施例的工程机械入场路径规划方法的具体场景的示意图， 如 图 5 所示， A 点是工程机械的当前点， B 点是目标点， 在 A 至 B 的路径当中， 存在着多个障碍 说 明 书 CN 103。
40、017757 A 9 7/9 页 10 物， 图中已经按照方向包围盒方法将障碍物的轮廓处理为矩形， C、 D、 E、 F 点为障碍物之间最 短连线的中点。路径搜索的具体步骤为 ： 0058 以 A 点到 B 点为直径的圆形区域划定为入场区域， 得出区域内相邻障碍物之间最 短连线， 并得出最短连线的中点坐标， 将结果进行保存。保存的数据结构可以是 ： 0059 Zi， Zj， Lij， MidX， Y， Lbj， 其中， Zi 为第 i 个障碍物， Zj 为第 j 个障碍物 ； Lij ： 障碍物 Zi 与 Zj 之间的最短距离 ； MidX， Y ： 最短距离线段的中点坐标 ； Lbj ： B。
41、 点到各中心 点之间的距离。 0060 从 A 点向 B 点进行路径搜索， 当 A 到 B 的方向上遇到障碍物 Zx 时， 获取该障碍 物与其他障碍物之间的最短距离中心点 MidX， Y， 并计算到在 AB 连线左边的最短中心点 CX1,Y1， 在 AB 连线右边的最短中心点 DX2,Y2。 0061 使用二叉树法， 分别以左分叉点C为起始点， 以垂直与C所在的最短连线的方向继 续搜索路径 ； 采用同样的方法， 计算两条分支线上 E、 F， 前进方向遇到的障碍物， 并进行标 识， 计算 X3、 Y3， 和 X4、 Y4 的坐标值。 0062 如果前进方向上不存在无障碍物， 则一直行驶到入场区域。
42、的边缘， 如图中的 a， d 两点， 然后在连线该点与 B 点， 依据以上方法进行探索并继续搜索路径。 0063 如果前进方向存在障碍物， 但障碍物只有一侧与其他障碍物有最短距离， 如图中 点 a， 其前进方向只有障碍物只有一侧存在最短距离中点， 此时采用以下三角形计算方法求 解从 a 到障碍物一侧 c 的路径。点 a 到障碍物端点 b 的距离已知， 而 bc 为绕行障碍物时的 安全距离， 由当 ac 与 bc 垂直时， 此时绕行距离最短， 因此 ac 与 ab 的夹角为 acrsin（bc/ ab） ， 由此可以求解获得 c 点的坐标， 获得 a 处绕行障碍物的路径。 0064 当路径到达目。
43、的点时， 认定搜索完毕一条路径， 保存本条路径， 然后回到上一个分 叉节点， 搜索其他该节点右分叉点后的其他路径， 一直搜索全部节点为止。 记录每条路径经 过的中心点坐标， 并保存所有坐标。 0065 在工程机械转弯过程中， 障碍物之间最短距离必须大于外轮廓的最大转弯半径 Rmax 与内轮廓的最小半径 Rmin 之差 （Rmax-Rmin） +X， 其中 X 为可以预先设置的转弯安全距 离裕值。当障碍物物之间的距离小于 （Rmax-Rmin） +X 时， 则说明工程机械不能通过这两个 障碍物之间的空间， 该条路径需要抛弃。 如果通过比较， 该路径上所有障碍物之间的距离都 大于 （Rmax-Rm。
44、in） +X， 则可以认为该路径畅通。并对所有路径的距离值进行排序输出。 0066 步骤 S24 计算工程机械按照各条路径入场需要行进的距离可以具体包括 ： 计算各 条路径中工程机械转弯的中心点、 半径、 和转向位置 ； 分别对各条路径中工程机械转弯的弧 线距离进行累加得到各条路径入场需要行进的距离。 0067 下面结合附图 6 和图 7， 对工程机械穿越障碍物的具体路径及行进距离进行说明， 图 6 是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的示意 图， 图 7 是根据本发明实施例的工程机械入场路径规划方法的穿越障碍物的具体路径的流 程图， 如图所示， 目前路径中， 工。
45、程机械需要从 A 点首先穿过障碍物 Z1 和障碍物 Z2 之间的 间隙， 然后穿过障碍物Z2和障碍物Z3之间的间隙， 障碍物Z1和障碍物Z2之间最短连线BC 线段的中点为 F， 障碍物 Z2 和障碍物 Z3 之间最短连线 DE 线段的中点为 G。 0068 步骤 S71， 求取 ED 线段延长线与 BC 线段延长线的交点 O1, 比较线段 O1G 和线段 O1F 长度， 记录两者较大的数值为 R1， 较小数值记录为 R2 ； 说 明 书 CN 103017757 A 10 8/9 页 11 0069 步骤 S72， 判断 R1 是否大于工程机械的转弯半径， 如果 R 小于工程机械的转弯半 径，。
46、 则可以确定工程机械无法通过这三个障碍之间的缝隙， 该条路径需要放弃 ； 0070 步骤 S73， 如果 R1 大于工程机械的转弯半径， 延长 O1F 至 O2， 使得 O1O2=R1+R2， 分 别以 O1 和 O2 为圆心， 以 R1 为半径做弧， 获取两条弧线的交点 H ； 0071 步骤 S74， 判断角度 GHF 的补角是否大于工程机械的最小转弯角度， 如果 GHF 的补角大于工程机械的最小转弯角度， 则认为从 FHG 之间的弧线为工程机械穿越障碍物的 具体路径 ； 0072 步骤 S75， 如果 GHF 的补角小于工程机械的最小转弯角度， 判断当前的转弯半径 是否大于 5 倍的 R。
47、1， 如果当前的转弯半径大于 5 倍的 R1， 则可以确定工程机械无法通过这 三个障碍之间的缝隙， 该条路径需要放弃 ； 0073 步骤 S76， 如果当前的转弯半径小于 5 倍的 R1， 分别沿 GD 的方向和 FC 的方向， 以 预设的增值 R 寻找新的 O1 点和 O2 点， 使得 GO1=GO1+ R， FO2=GO1， R 为预先设定的 增值 ； 0074 重复步骤 S73， 直至得到弧线路径或者得出工程机械无法通过的结论。 0075 通过以上步骤得到的 FHG 之间的弧线为工程机械穿越障碍物的具体路径， 对以上 圆弧、 圆心、 转向交点、 以及行进距离进行保存， 可以为工程机械入场。
48、时提供参考。 0076 得出所有可能行进的路径后， 统计各条路径的行进距离， 转向次数， 以及工程机械 通过障碍物时与障碍物的最小距离。制作成路径选择表， 表 1 是本根据本发明实施例的工 程机械入场路径规划方法的路径选择表。 0077 表 1 路径选择表 0078 序号路径距离值转弯次数最小距离 11258 213410 31579 0079 通过表 1 可以依据不同的条件对各条路径进行排序， 从距离优先、 转弯次数最少 优先、 最安全优先三个原则中选择优先顺序， 系统依据该顺序进行输出最优方案。优选地， 可以对以上三种条件按照需求设置权重值， 按照加权累加值进行排序， 从而综合各条件输 出。
49、最佳路径。 0080 依次计算出各障碍物间的转弯半径， 并将所有的路径进行保存。可以依据保存的 各节点的坐标、 转弯半径、 转弯中心等值来计算出入场路径， 指导工程机械入场。 0081 图 8 是根据本发明实施例的工程机械入场路径规划方法得出的最佳路径的示意 图， 如图所示， 工程机械经由 D、 E 点向 B 点入场。这种方式的距离最近。从而解决了如图 5 所示的情况的路径规划。 0082 以上工程机械的入场方法， 适用于各种其种起重机、 混凝土搅拌、 泵送机械、 大型 说 明 书 CN 103017757 A 11 9/9 页 12 运输车辆等。 0083 应用本发明的技术方案， 通过计算障碍物二维投影轮廓的最短连线， 找出最短连 线的中点， 在出现障碍物情况下探索从出发点通过上述最短连线终点到达目的施工地点的 路线， 并在进行路线路径计算后， 按照实际工程机械的行进距离计算路径距离， 统计转弯次 数， 。