CN111422741B - 一种桥式起重机运动路径规划方法 - Google Patents

Abstract

本发明涉及一种桥式起重机运动路径规划方法，本发明专利采用一种跳跃搜索节点的方法优化A*算法中拓展节点的选择方法，能有效地提高算法工作的效率。此外，改进型A*算法考虑了起重机的运动学模型，增加了一种转弯代价来减少规划路径中的拐点，缓解起动机运转过程中出现的电机反复启停和吊臂摇动问题。将本方法应用于桥式起重机的路径规划，在规划相同路径任务中，改进的A*算法的能够以更快的速度规划出理想行驶路径。同时，本发明专利提供的方法亦能在获取路径最优的同时兼顾起重机的运动学特性，最大限度地缓解吊臂摆动的问题。该方法应用于桥式起重机路径规划的方法不仅能提高起重机的自动装载效率，也为工业安全生产提供了保障。

Description

一种桥式起重机运动路径规划方法

技术领域

本发明涉及公路工程测量领域，尤其涉及一种桥式起重机运动路径规划方法。

背景技术

随着生产制造过程中机械化和智能化水平的不断提高，机器能够开始自主化地产生决策。其中，路径规划是机器智能化研究中的一个热点内容，并在无人驾驶、智能导航，智慧物流等领域得到广泛应用。路径规划要求机械智能体在对环境感知的前提下，自主决策生成从初始点到达目标点的运动路径，对其规划质量评价往往包括：路径长度、时间消耗和能耗代价等。

在工业生产中，生产背景与机械结构的差异性给路径规划赋予了不同的要求。其中，桥式起重机因其负载能力强，工作可靠等优点得到了广泛的应用。桥式起重机的运动系统主要由小车运行机构、大车运行机构两部分连结构成，小车负责水平运动，而大车负责前后行进。起重机运载任务往往需要路径规划算法的参与，将运动任务拆解成具体的行进路线。高效的路径规划不仅能够帮助任务为缩短完成时间，还能够提高其自动转载效率和安全性。传统A*算法所规划的路径通过仅关注于起重机运行路径长度上的最优，其规划路径常存在大量拐点和斜线。桥式起重机由于运动时受大车、小车速度之间制约关系的影响，拐点和斜线都会使得台车反复启停，致使起动机吊车的防摇工作难度升高，同时这也是现场作业中的一项大的安全隐患。因此，需要设计一种有效路径规划算法来配合起重机的工作，它提高起重机的工作效率，同时也可以缓解起重机的在运转过程中吊臂的桥式起重机运动路径规划方法。

发明内容

本发明要解决的问题在于提供一种可以保证效率获得了提高，所规划的路径更符合起桥式重机的运动学特性的桥式起重机运动路径规划方法。

为了保证在使用过程中，能够保证工作规划路线具有无斜线、少拐点，本发明涉及了一种桥式起重机运动路径规划方法，包括以下步骤：

S1：通过传感器或先验知识对工作环境建模，将搜索区域划分为网格；获取起重机吊臂末端的起始点的坐标位置和目标终点的坐标位置；

S2：判断起始点和终点是否在同一连通区域，若是则返回“寻路结束”，若不是则进入S3；

S3：创建OpenSet和ClosedSet，将起始点作为起始节点加入OpenSet，并将ClosedSet设置为空；建立估价函数F(n)＝G(n)+H(n)+E(n)，用于评估OpenSet和ClosedSet中节点的综合优先级。其中，G(n)用于评估从当前节点n位置到起始节点的距离代价；H(n)用于评估当前节点n到达终点的预计代价；E(n)用于估计起重机运动过程中的运动方向改变而付出的转向代价，用于对拐弯的路径进行适当的惩罚；

S4：当需要进行下一个要遍历的节点时，通过估价函数F(n)选取OpenSet中估价值最小的节点作为当前节点，拥有下一时刻的遍历优先级。

S5：判断当前节点是否为目标节点。若是则返回“寻路结束”；若不是则进入S6；

S6：在ClosedSet中加入当前节点，并在OpenSet中删除该当前节点；

S7：采用一种跳点搜索关键节点的方法，在当前节点附近搜索寻找新的跳跃节点作为可行节点，并判断该可行节点是否在OpenSet中；若在，则将当前节点的距离代价函数值G(n)与父节点判断更新；若不是，则将当前节点加入OpenSet中。

S8：通过估价函数F(n)＝G(n)+H(n)+E(n)计算起始节点s(x_s,y_s)与当前节点n(x_n,y_n)的估价值，并进入S6。

本发明的有益效果是，通过采用一种跳跃搜索拓展节点的方法，能够极大地提高算法在大范围空间的搜索效率。此外，该算法还根据起重机的运行特性添加了一种新的约束函数。改进的A*算法的可根据库区内的危险区域和障碍物帮助桥式起重机快速规划出最优最短的路径。改进的A*算法规划所得路径明显没有斜线，曲折程度较小，同时不影响原有启发函数的功能。相较于原有的A*算法，改进后的路径更符合桥式起重机的运动特点。在相同起点与终点的前提下，改进型A*算法不仅执行效率高，而且在获得距离长度最优解的同时还能够有效地减少大车、小车的电机启动及制动次数，提高了转载效率。改进型A*算法中将路径转曲为直的规划思想也为起重机减摇提供了保障。

进一步的，所述步骤S1中将搜索区域划分为网格，依据起重机工作环境所建立的栅格地图中，每个网格都无额外权重，即为价值相同。

进一步的，所述步骤S3中所述的估价函数，估价函数F(n)的构成：移动代价G(n)，成本估算函数H(n)和转向代价函数E(n)；其中，G(n)是工作空间中起点节点到拓展节点所跨越欧几里得距离；H(n)是一种改进曼哈顿距离函数，即H(n)＝|x_s-x|+|y_s-y|*1.01；E(n)是一种适用于起重机运动模型的转向代价函数。

进一步的，所述步骤S3中的转向代价函数E(n)，它能够用于在路径规划中在不遇到障碍物和边界的情况下，减少拐弯次数。在改变路径方向的节点处，增加该节点的“转向代价值”，令E(n)＝E,E为一个设定常数值，而在不改变路径方向的候选节点处，令其转向代价E(n)＝0，再根据A*算法选择估价函数值最小点作为路径点的原理，来保证路径方向在允许的情况下能够快速寻找到最优路径。

进一步的，所述判断起重机运转判断道路中拐点的方法，采用几何方法中判断三点共线法去判断拓展节点是否为拐点。

进一步的，所述步骤S7中所述搜索可行节点，搜索范围为节点的4连通区域，即在节点的上、下、左、右方向处进行搜索。

进一步的，所述步骤7中所述搜索可行节点，根据父节点的方向作为参考方向沿直线搜索，当找到关键节点时则作为跳跃节点，并利用剪枝剪掉非必须的“中间跳跃节点”。“中间跳跃节点”在节点拓展中只具有简单的承接作用，不具备拓展价值。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一种桥式起重机运动路径规划方法的改进A*算法流程图；

图2是本发明一种桥式起重机运动路径规划方法的改进A*算法节点拓展范围示意图；

图3是本发明一种桥式起重机运动路径规划方法的A*算法应用场景建模图；

图4是本发明一种桥式起重机运动路径规划方法的未加拐点惩罚的改进A*算法规划示意图；

图5是本发明一种桥式起重机运动路径规划方法的加入拐点惩罚的改进A*算法规划示意图。

具体实施方式

下面结合具体实施例，对本发明的内容做进一步的详细说明：

本发明要解决的问题在于提供一种可以保证效率获得了提高，所规划的路径更符合起桥式重机的运动学特性的桥式起重机运动路径规划方法。

如图1、图2、图3所示，为了保证在使用过程中，能够保证工作规划路线具有无斜线、少拐点，本发明涉及了一种桥式起重机运动路径规划方法，包括以下步骤：

S1：通过传感器或先验知识对工作环境建模，将搜索区域划分为网格；获取起重机吊臂末端的起始点的坐标位置和目标终点的坐标位置；

S2：判断起始点和终点是否在同一连通区域，若是则返回“寻路结束”，若不是则进入S3；

S3：创建OpenSet和ClosedSet，将起始点作为起始节点加入OpenSet，并将ClosedSet设置为空；建立估价函数F(n)＝G(n)+H(n)+E(n)，用于评估OpenSet和ClosedSet中节点的综合优先级。其中，G(n)用于评估从当前节点n位置到起始节点的距离代价；H(n)用于评估当前节点n到达终点的预计代价；E(n)用于估计起重机运动过程中的运动方向改变而付出的转向代价，用于对拐弯的路径进行适当的惩罚；

S4：当需要进行下一个要遍历的节点时，通过估价函数F(n)选取OpenSet中估价值最小的节点作为当前节点，拥有下一时刻的遍历优先级。

S5：判断当前节点是否为目标节点。若是则返回“寻路结束”；若不是则进入S6；

S6：在ClosedSet中加入当前节点，并在OpenSet中删除该当前节点；

S7：采用一种跳点搜索关键节点的方法，在当前节点附近搜索寻找新的跳跃节点作为可行节点，并判断该可行节点是否在OpenSet中；若在，则将当前节点的距离代价函数值G(n)与父节点判断更新；若不是，则将当前节点加入OpenSet中。

S8：通过估价函数F(n)＝G(n)+H(n)+E(n)计算起始节点s(x_s,y_s)与当前节点n(x_n,y_n)的估价值，并进入S6。

本发明的有益效果是，通过采用一种跳跃搜索拓展节点的方法，能够极大地提高算法在大范围空间的搜索效率。此外，该算法还根据起重机的运行特性添加了一种新的约束函数。改进的A*算法的可根据库区内的危险区域和障碍物帮助桥式起重机快速规划出最优最短的路径。改进的A*算法规划所得路径明显没有斜线，曲折程度较小，同时不影响原有启发函数的功能。相较于原有的A*算法，改进后的路径更符合桥式起重机的运动特点。在相同起点与终点的前提下，改进型A*算法不仅执行效率高，而且在获得距离长度最优解的同时还能够有效地减少大车、小车的电机启动及制动次数，提高了转载效率。改进型A*算法中将路径转曲为直的规划思想也为起重机减摇提供了保障。

进一步的，所述步骤S1中将搜索区域划分为网格，依据起重机工作环境所建立的栅格地图中，每个网格都无额外权重，即为价值相同。

进一步的，所述步骤S3中所述的估价函数，估价函数F(n)的构成：移动代价G(n)，成本估算函数H(n)和转向代价函数E(n)；其中，G(n)是工作空间中起点节点到拓展节点所跨越欧几里得距离；H(n)是一种改进曼哈顿距离函数，即H(n)＝|x_s-x|+|y_s-y|*1.01；E(n)是一种适用于起重机运动模型的转向代价函数。

进一步的，所述步骤S3中的转向代价函数E(n)，它能够用于在路径规划中在不遇到障碍物和边界的情况下，减少拐弯次数。在改变路径方向的节点处，增加该节点的“转向代价值”，令E(n)＝E,E为一个设定常数值，而在不改变路径方向的候选节点处，令其转向代价E(n)＝0，再根据A*算法选择估价函数值最小点作为路径点的原理，来保证路径方向在允许的情况下能够快速寻找到最优路径。

进一步的，所述判断起重机运转判断道路中拐点的方法，采用几何方法中判断三点共线法去判断拓展节点是否为拐点。

进一步的，所述步骤S7中所述搜索可行节点，搜索范围为节点的4连通区域，即在节点的上、下、左、右方向处进行搜索。

进一步的，所述步骤7中所述搜索可行节点，根据父节点的方向作为参考方向沿直线搜索，当找到关键节点时则作为跳跃节点，并利用剪枝剪掉非必须的“中间跳跃节点”。“中间跳跃节点”在节点拓展中只具有简单的承接作用，不具备拓展价值。

在实际操作中，具体方式如下：

一种跳跃节点搜寻的方式：

在节点的搜寻过程中引入了一种跳点的机制，与传统算法对当前节点8连通区域搜索的方法不同，跳点机制会遵循当且节点的所保持的直线方向展开搜寻，并遵循如下原则：

(1)若当前节点此时左后方不可走且左方可走，则沿当前节点的左方寻找不在ClosedSet中的跳点。

(2)若当前节点此时方向可走，则沿当前节点所指方向寻找不在ClosedSet中的跳点。

(3)若当前节点此时右后方不可走且右方可走，则沿当前节点的右方寻找不在ClosedSet中的跳点。

如果从当前节点x到某一备选节点y为直线移动，节点n是节点y的邻居，若有从当前节点x到节点n且不经过节点y的路径，且路径长度小于或等于从当前节点x经过节点y到节点n的路径，则走到节点y后下一个点不会走到节点n。该方法通过选择跳跃节点避免了对节点的多个连通区域的过度搜索。

一种起重机消除拐点的方法：

在同一个路径规划任务重，算法往往能获取在路径长度上表现为最优的路径。但对于起重机而言，在这些路径中选择拐点更少的路线更加符合其运动规律。因此，本发明专利提出了一种消除拐点的方法。该方法首先需要判断邻近节点是否为拐点，此时采用一种三点共线法去判断拐点：设节点B为当前节点，栅格地图中坐标记为(x_b,y_b)，节点A为节点B的父节点，坐标记为(x_a,y_a)，节点C为路径候选节点，坐标记为(x_c,y_c)。AB所在直线方向代表路径当前的走向，BC所在直线方向则代表了路径选择节点C作为下一节点后路径的走向：

由上式所示，K_AB表示线段AB的斜率，K_BC表示线段BC的斜率，若K_AB＝K_BC，则A、B、C三点共线，节点C不会改变路径的方向，反之，节点C改变了路径的方向，则节点B将是路径中的一个拐点。

然而某些拐点是路径中必须存在的，不能完全消除。因此该机制中采用一种拐点惩罚项，在估价函数F(n)中增添了一种适用于起重机运动模型的转向代价函数E(n)，通过它对于拐点的惩罚来影响起重器遭遇拐点时是否应该立即转弯或者延迟转弯。起重机在不遇障碍物或者工作空间的边界时，不会“盲目”拐弯。如果邻近节点为拐点，增加该节点的“转向代价值”，令E(n)＝E，而在不改变路径方向的候选节点处，令其转向代价E(n)＝0，将E(n)融入估价函数F(n)中，使得不改变路径方向节点整体的估价函数值小于改变路径方向节点整体的估价函数值。由此，该通过找出路径中原本可能成为拐点的节点，通过加上该代价转向函数E(n)来修正起重机的运动路线。

实验案例

虽然传统算法能够为起重机提供最短的运行路径，但大量的拐点会似的起重机的大小车在运动过程中反复启停。为提高传统A*算法在大型工作场景中效率与缓解在规划路径中多拐点对起重机运行造成的影响，改进后的A*算法流程中将使用搜索跳跃节点的方式来提高算法效率，并通过计算候选节点的转向代价函数值，来判断适合路径的节点。

首先说明未加拐点惩罚项的情况。将工作环境栅格化，如图4所示。起重机将从S点出发，到达目的地E点，其中黑色的部分表示障碍物。将起点S加入OpenSet中。从OpenSet中取出F值最小的点S，并从OpenSet中删除，加入ClosedSet中。S的当前方向为空，则沿4个方向寻找跳点，在该图中从S出发只有下、右，2个方向可走，但向下搜索到D遇到边界，向右搜索到F遇到阻挡，因此都没有找到跳点。然后沿下方向寻找跳点。由于F经过垂直方向移动(向下移动)可以到达跳点I，因此F为跳点并加入OpenSet中。

从OpenSet中取出代价值最小的点F，并从OpenSet中删除，加入ClosedSet中。F的当前方向为右方向(从S到F的方向)，沿右(当前方向水平分量)、下(当前方向垂直分量)、2个方向寻找跳点。在F点只有向下可走，因此向下寻找跳点，找到跳点I并加入OpenSet中。

从OpenSet中取出代价值最小的点I，并从OpenSet中删除，加入ClosedSet中。I的当前方向为直线方向(从F到I的方向)，在I点时I的左后方不可走且左方、前方可走，因此沿左方、前方寻找跳点，但在前方会遇到边界，只有向左方寻找到跳点Q并加入OpenSet中。

从OpenSet中取出代价值最小的点Q，并从OpenSet中删除，加入ClosedSet中，Q的当前方向为直线方向，Q的左后方不可走且左方、前方可走，因此沿左方、前方寻找跳点，但在前方会遇到边界，只有向左方寻找到跳点E并加入OpenSet中。

从OpenSet中取出代价值最小的点E,E是目标节点，寻路结束，如图4所示，路径是S,F,I,Q,E。上述引入跳点机制的A*算法的寻路效率是明显快于传统的A*算法。

虽然图4中所显示的路径已经能在行驶路径的距离长度上获得最优解，但对于起动机的工作而言，所规划的路径仍然有改进的空间。当引入惩罚机制后，算法在执行过程中遭遇拐点时估价函数F(n)会增加一个惩罚值E(n)＝E来降低拐点节点的价值，此时节点F的价值小于节点A，则算法会朝着下方行进。与上面的行进方式同理，A、C、Q、F会逐步加入到OpenSet中，所行进路线如图5所示。与未加入拐点惩罚项的A*算法相比，在均获得最优路径的同时，引入拐点惩罚项能够额外消除不必要的拐点，这对起重机执行任务更加有利。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (1)

1.一种桥式起重机运动路径规划方法，其特征在于，包括以下步骤：

S1：通过传感器或先验知识对工作环境建模，将搜索区域划分为网格；获取起重机吊臂末端的起始点的坐标位置和目标终点的坐标位置，依据起重机工作环境所建立的栅格地图中，每个网格都无额外权重，即为价值相同；

S2：判断起始点和终点是否在同一连通区域，若是则返回“寻路结束”，若不是则进入S3；

S3：创建OpenSet和ClosedSet，将起始点作为起始节点加入OpenSet，并将ClosedSet设置为空；建立估价函数

，用于评估OpenSet和ClosedSet中节点的综合优先级；其中，

用于评估从当前节点n位置到起始节点的距离代价；

用于评估当前节点n到达终点的预计代价；

用于估计起重机运动过程中的运动方向改变而付出的转向代价，用于对拐弯的路径进行适当的惩罚, 估价函数F(n)的构成：移动代价

，成本估算函数

和转向代价函数

；其中，

是工作空间中起点节点到拓展节点所跨越欧几里得距离；

是一种改进曼哈顿距离函数，即

；

是一种适用于起重机运动模型的转向代价函数, 转向代价函数

，它能够用于在路径规划中在不遇到障碍物和边界的情况下，减少拐弯次数；在改变路径方向的节点处，增加该节点的“转向代价值”，令

,E为一个设定常数值，而在不改变路径方向的候选节点处，令其转向代价

，再根据A*算法选择估价函数值最小点作为路径点的原理，来保证路径方向在允许的情况下能够快速寻找到最优路径，判断起重机运转判断道路中拐点的方法，采用几何方法中判断三点共线法去判断拓展节点是否为拐点；

S4：当需要进行下一个要遍历的节点时，通过估价函数

选取OpenSet中估价值最小的节点作为当前节点，拥有下一时刻的遍历优先级；

S5：判断当前节点是否为目标节点；若是则返回“寻路结束”； 若不是则进入S6；

S6：在ClosedSet中加入当前节点，并在OpenSet中删除该当前节点；

S7：采用一种跳点搜索关键节点的方法，在当前节点附近搜索寻找新的跳跃节点作为可行节点，并判断该可行节点是否在OpenSet中；若在，则将当前节点的距离代价函数值

与父节点判断更新；若不是，则将当前节点加入OpenSet中，搜索范围为节点的4连通区域，即在节点的上、下、左、右方向处进行搜索，根据父节点的方向作为参考方向沿直线搜索，当找到关键节点时则作为跳跃节点，并利用剪枝剪掉非必须的“中间跳跃节点”；

S8：通过估价函数

计算起始节点s

与当前节点n

的估价值，并进入S6。

Images