CN116225034A - 基于Dijkstra算法的全覆盖路径规划方法及系统 - Google Patents
基于Dijkstra算法的全覆盖路径规划方法及系统 Download PDFInfo
- Publication number
- CN116225034A CN116225034A CN202211536497.XA CN202211536497A CN116225034A CN 116225034 A CN116225034 A CN 116225034A CN 202211536497 A CN202211536497 A CN 202211536497A CN 116225034 A CN116225034 A CN 116225034A
- Authority
- CN
- China
- Prior art keywords
- grid
- backtracking
- point
- path
- module
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 73
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000001514 detection method Methods 0.000 claims abstract description 19
- 238000009499 grossing Methods 0.000 claims abstract description 9
- 230000007613 environmental effect Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 8
- 238000004088 simulation Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 8
- 238000012876 topography Methods 0.000 description 8
- 230000004888 barrier function Effects 0.000 description 6
- 239000010432 diamond Substances 0.000 description 5
- 238000011161 development Methods 0.000 description 3
- 238000000354 decomposition reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 229910003460 diamond Inorganic materials 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明提供了一种基于Dijkstra算法的全覆盖路径规划方法及系统，包括：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；Boustrophedon运动访问环境栅格地图，并标记访问信息；Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；根据平滑回溯路径，控制水下机器人运动到回溯点后，转入第二步骤。本发明提出引入Dijkstra算法搜索具有位置不确定性的回溯点，算法找到的第一个回溯点即为最佳回溯点；本发明无需找到所有回溯点以确定最佳回溯点，优化了最佳回溯点的寻找方法。
Description
技术领域
本发明涉及路径规划的技术领域，具体地，涉及基于Dijkstra算法的全覆盖路径规划方法及系统。
背景技术
随着国家基础建设和水上战略步伐的加快，对湖泊的科学认识和高效开发利用有助于国家维护水域权益和实现可持续发展，这需要详细和精确的湖泊地形地貌信息作为保障。此外，湖泊安全、水库监测及水下考古等也需要湖泊地形作为参考依据。水下地形测量的难度较大、危险性较高，因此可使用水下机器人代替人工进行测量。对湖泊地形进行探测时，需要遍历整个湖泊进探测，同时需要避开暗礁等障碍物。同时，特大型湖泊中存在许多岛屿，会阻碍水下机器人的行进，导致湖泊地形测量任务的完成质量下降。因此需要规划有效的全覆盖路径使水下机器人能遍历整个湖泊完成地形探测。
全覆盖路径规划以覆盖率为指标，规划出在工作区域中的无碰撞遍历路径，实现对工作区域中非障碍物区域的遍历访问。全覆盖路径规划算法主要有：随机覆盖法、模板模型法、区域分解法、生物激励法。区域分解法由于其方法简单，易于实现等优点被广泛应用。其原理是将整个工作区域分解为若干个无障碍物的子区域，在子区域内做往复运动或者选运动遍历覆盖子区域，然后将连接各个子区域实现全覆盖路径规划。
然而现有在环境占据信息未知区域中的全覆盖规划算法较少，且现有算法会寻找所有的回溯点，并以欧式距离或者曼哈顿距离最短作为最佳回溯点的判断指标，以该指标判断最佳回溯点时无法避免跨越障碍物区域和未访问区域，从而导致其回溯路径非最短，移动成本增加。因此解决现有回溯点寻找方法的不足对促进水下勘探的发展十分重要。
因此，需要提出一种新的技术方案以改善上述技术问题。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于Dijkstra算法的全覆盖路径规划方法及系统。
根据本发明提供的一种基于Dijkstra算法的全覆盖路径规划方法，所述方法包括如下步骤：
步骤S1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
步骤S2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
步骤S3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
步骤S4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入步骤S2。
优选地，所述步骤S1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
优选地，所述步骤S2的具体实现方式为：设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
优选地，所述步骤S3包括如下步骤：
步骤S3.1：定义回溯点的条件；
步骤S3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
步骤S3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
步骤S3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
步骤S3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复步骤S3.3-步骤S3.5，直到搜索到回溯点，转入步骤S3.6，或者open列表为空，结束规划；
步骤S3.6：去除所得回溯路径中的共线点；
步骤S3.7：去除步骤S3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
优选地，所述步骤S4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入步骤S2开始新的Boustrophedon运动。
本发明还提供一种基于Dijkstra算法的全覆盖路径规划系统，所述系统包括如下模块：
模块M1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
模块M2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
模块M3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
模块M4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入模块M2。
优选地，所述模块M1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
优选地，所述模块M2的具体实现方式为：设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
优选地，所述模块M3包括如下模块：
模块M3.1：定义回溯点的条件；
模块M3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
模块M3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
模块M3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
模块M3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复模块M3.3-模块M3.5，直到搜索到回溯点，转入模块M3.6，或者open列表为空，结束规划；
模块M3.6：去除所得回溯路径中的共线点；
模块M3.7：去除模块M3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
优选地，所述模块M4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入模块M2开始新的Boustrophedon运动。
与现有技术相比，本发明具有如下的有益效果：
1、本发明提出引入Dijkstra算法搜索具有位置不确定性的回溯点，算法找到的第一个回溯点即为最佳回溯点；本发明无需找到所有回溯点以确定最佳回溯点，优化了最佳回溯点的寻找方法；
2、本发明提出以回溯路径最短作为最佳回溯点的判断指标，并基于Dijkstra算法寻找回溯点，可使得到的回溯路径最短；
3、本发明引入Dijkstra算法搜索回溯点，在算法搜索到回溯点的同时返回最佳回溯点和回溯路径，优化了死区脱困方法，降低了算法复杂性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1是本发明提供的一种基于Dijkstra算法的全覆盖路径规划方法的流程图；
图2为本发明Dijkstra算法路径规划的仿真效果图；
图3为本发明Dijkstra算法所得路径去除共线点后的仿真效果图；
图4为本发明Dijkstra算法所得路径去除共线点和多余拐点后的仿真效果图；
图5为验证本发明算法所使用的栅格地图；
图6为其他寻找最佳回溯点算法所得到的回溯点结果；
图7为以回溯路径最短作为最佳回溯点判断指标时得到的回溯点结果；
图8为未以回溯路径最短作为最佳回溯点判断指标的全覆盖路径规划仿真效果图；
图9为本发明区域全覆盖路径规划的仿真效果图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
根据本发明提供的一种基于Dijkstra算法的全覆盖路径规划方法，所述方法包括如下步骤：
步骤S1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
步骤S2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
步骤S3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
步骤S3.1：定义回溯点的条件；
步骤S3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
步骤S3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
步骤S3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
步骤S3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复步骤S3.3-步骤S3.5，直到搜索到回溯点，转入步骤S3.6，或者open列表为空，结束规划；
步骤S3.6：去除所得回溯路径中的共线点；
步骤S3.7：去除步骤S3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
步骤S4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入步骤S2；根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入步骤S2开始新的Boustrophedon运动。
本发明还提供一种基于Dijkstra算法的全覆盖路径规划系统，所述基于Dijkstra算法的全覆盖路径规划系统可以通过执行所述基于Dijkstra算法的全覆盖路径规划方法的流程步骤予以实现，即本领域技术人员可以将所述基于Dijkstra算法的全覆盖路径规划方法理解为所述基于Dijkstra算法的全覆盖路径规划系统的优选实施方式。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
本发明还提供一种基于Dijkstra算法的全覆盖路径规划系统，所述系统包括如下模块：
模块M1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
模块M2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
模块M3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
模块M3.1：定义回溯点的条件；
模块M3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
模块M3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
模块M3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
模块M3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复模块M3.3-模块M3.5，直到搜索到回溯点，转入模块M3.6，或者open列表为空，结束规划；
模块M3.6：去除所得回溯路径中的共线点；
模块M3.7：去除模块M3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
模块M4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入模块M2；根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入模块M2开始新的Boustrophedon运动。
实施例3：
实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
由于人力和信息资源的限制，对湖泊地形进行探测时缺少湖泊中障碍物的占据信息，针对该情况下的区域全覆盖问题，本发明提供了一种基于Dijkstra算法的全覆盖路径规划方法，通过Boustrophedon运动以增量的方式构建和分解环境模型，引入Dijkstra算法，以回溯路径最短为指标寻找最佳回溯点，解决回溯路径非最短问题。
本发明提供了一种基于Dijkstra算法的全覆盖路径规划方法，通过装载声纳和GPS的水下机器人实现上述目的，具体包括下列步骤：
S1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
S2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
S3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
S4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入步骤S2。
步骤S1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息。初始时刻，对水下机器人所处位置周围环境的占据信息是未知的，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。声纳探测距离应不小于两倍水下机器人长度，栅格地图的栅格大小设置应不小于水下机器人长度，且不大于声纳探测距离的1/2。
步骤S2的具体实现方式为：设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问。同时根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
步骤S3包括：
S31：定义回溯点的条件；
S32：设置两个表：open和close，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
S33：寻找open列表中代价函数最小的节点，并设为当前节点；
S34：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
S35：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复步骤S33-S35，直到搜索到回溯点，转入步骤S36，或者open列表为空，结束规划；
S36：去除所得回溯路径中的共线点；
S37：去除步骤S36所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
步骤S4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入步骤S2开始新的Boustrophedon运动。
由于人力和信息资源的限制，对湖泊地形进行探测时缺少湖泊中障碍物的占据信息，针对该情况下的区域全覆盖问题，因此本发明提供了一种基于Dijkstra算法的全覆盖路径规划方法，基于Boustrophedon运动和Dijkstra算法规划水下机器人在农田中的全覆盖作业路径。
如图1所示，本发明基于装载声纳和GPS的水下机器人，提供了一种基于Dijkstra算法的全覆盖路径规划方法，包括下列步骤：
S1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
S2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
S3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
S4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入步骤S2；
步骤S1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息。初始时刻，对水下机器人所处位置周围环境的占据信息是未知的，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图中对应栅格的占据状态与访问栅格地图中对应栅格的访问状态。声纳探测距离应不小于两倍水下机器人长度，栅格地图的栅格大小设置应不小于水下机器人长度，且不大于声纳探测距离的1/2。本发明以水下机器人的长度作为栅格的长度，取声纳的探测距离为两倍的水下机器人长度。
环境栅格地图中的每个栅格有三种状态，分别为：占据、空闲和未知，占据状态表示地图该点存在障碍物，水下机器人不可到达该点；空闲状态表示地图该点不存在障碍物，水下机器人可到达该点；未知状态表示地图没有该点的环境信息，有待进一步探查以确定该点的状态。访问栅格地图中的每个栅格有两种状态，分别为：已访问和未访问，已访问状态表示水下机器人到达过该点；未访问状态表示水下机器人未到达过该点。初始状态，环境栅格地图中每个栅格都为未知状态，访问栅格地图中每个栅格都为未访问状态。环境栅格地图与访问栅格地图的每个栅格相对应。
步骤S2的具体实施方式为：Boustrophedon运动的原理是机器人在每个时间步从当前位置移动到当前位置的四个相邻位置之一中，即移动到当前位置所在栅格的上、下、左、右四个相邻栅格之一中，且该栅格不能为障碍物栅格和已访问栅格。
设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问。同时根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
Boustrophedon运动的移动优先级设置为：右→左→上→下，当在这四个移动方向上都没有未访问过的可行栅格时，Boustrophedon运动到达死区。初始时刻，水下机器人上搭载的声纳在初始位置探测周围的环境占据信息并更新环境栅格地图的状态，同时在访问栅格地图中将初始位置所对应的栅格标记为已访问。移动过程中，每访问一个栅格，就在访问地图中将此栅格标记为已访问，同时根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态。
步骤S3包括：
S31：定义回溯点的条件；
S32：设置两个表：open和close，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
S33：寻找open列表中代价函数最小的节点，并设为当前节点；
S34：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
S35：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复步骤S33-S35，直到搜索到回溯点，转入步骤S36，或者open列表为空，结束规划；
S36：去除所得回溯路径中的共线点；
S37：去除步骤S36所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
进一步的，步骤S31的具体实现方式为：结合环境栅格地图和访问栅格地图，将距离当前栅格最近的未访问自由栅格定义为回溯点。基于Dijkstra算法、环境栅格地图和访问栅格地图寻找回溯点，得到回溯路径。
Dijkstra算法是求解从一个顶点到其余各顶点的最短路径算法，用于解决权值为正的有权图中的最短路径问题。Dijkstra算法的基本原理是从起点开始，以起点为中心逐步向外扩展节点，直到扩展到终点为止。Dijkstra算法具体步骤如步骤S32～S35所示。
步骤S32的具体实现方式为：设置两个表：open和close，算法初始时刻，将水下机器人当前所在位置加入open列表，close列表为空。
步骤S33的具体实现方式为：Dijkstra算法搜索具有不确定性的目标点时，以起点到当前节点n之间的相邻节点间的欧式距离之和表示代价函数f(n)，寻找最佳回溯点。
(x0,y0)表示起始点的栅格索引值，(xi,yi)表示第i个节点的栅格索引值。
步骤S34的具体实现方式为：将当前节点添加到close列表中，然后在open列表中删除该节点。判断该节点是否为已被建图且未被访问过的自由栅格，若是则找到回溯点，否则继续搜寻。
步骤S35的具体实现方式为：若当前节点不是回溯点，则按左→上→右→下的顺序判断该节点的四邻域栅格是否为可行栅格，并将可行栅格加入open列表，重复步骤S33-S35寻找回溯点。若寻找到回溯点，则转入步骤S36对回溯路径进行平滑处理，若找不到回溯点，则结束规划。
步骤S36的具体实现方式为：取步骤S35所得的回溯路径上相邻的三个点，设当前点的位置为(xi，yi)，其余两点位置为(xi-1，yi-1)，(xi-2，yi-2)。共线条件为：
xi-xi-1＝xi-1-xi-2 (2)
yi-yi-1＝yi-1-yi-2 (3)
当相邻三点满足共线条件时去除中间的路径点(xi-1，yi-1)。重复计算直到路径中每三个相邻的路径点都不共线。
步骤S37的具体实现方式为：去除步骤S36所得的去除共线点的路径中的多余拐点。从步骤S36所得路径的第三个路径点开始依次判断当前路径点与前面的路径点的连线是否穿过障碍物。假设步骤S36所得的路径P为P＝[p1,p2,p3,…,pn]，n为步骤S36所得路径的路径点数，将平滑后的路径点存储于SP中，初始时平滑路径SP＝[sp1,sp2]，sp1＝p1，sp2＝p2，从p3开始依次判断P中的路径点pi与SP中的路径点spj之间的连线是否穿过障碍物，i＝1,2,…,n,j＝1,2,…,length(SP)。若P中的路径点pi与SP中的路径点spj的连线不穿过障碍物，则将路径点pi作为spj+1添加到spj后，并清除SP中spj+1后的路径点，结束pi与SP中各点的连线判断，执行pi+1与SP中各点的连线判断；若P中的路径点pi与SP中的路径点spj的连线穿过障碍物且j＝SP的长度-1时，将pi作为spj+2添加到SP中，并结束pi与SP中各点的连线判断，执行pi+1与SP中各点的连线判断；若P中的路径点pi与SP中的路径点spj的连线穿过障碍物且j<SP的长度-1时，继续判断pi与spj+1的连线是否穿过障碍物。
判断两点连线是否穿过障碍物的具体实现方式为：计算两点连线的斜率k，并求出直线方程。当斜率k的绝对值|k|>1时，纵向遍历两点连线所穿过的地图栅格，判断每个栅格点是否为障碍物栅格；当斜率k的绝对值|k|<1时，横向遍历两点连线所穿过的地图栅格，判断每个栅格点是否为障碍物栅格；当斜率k的绝对值|k|＝1时，判断连线与栅格边界交点周围四个栅格是否为障碍物栅格。
纵向遍历的具体实现方式为：从连线起点所在栅格开始，逐个判断与连线和横向栅格边界的交点相邻的栅格是否为障碍物栅格，直到遍历到连线终点所在栅格为止。
横向遍历的具体实现方式为：从连线起点所在栅格开始，逐个判断与连线和纵向栅格边界的交点相邻的栅格是否为障碍物栅格，直到遍历到连线终点所在栅格为止。
步骤S4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后开始新的Boustrophedon运动。
本发明通过Boustrophedon运动构建环境地图，将搜索区域分解为不含独立障碍物的子区域，然后通过Dijkstra算法寻找回溯点和规划回溯路径以连接各个子区域，实现对整个工作区域内非障碍物区域的遍历全覆盖。本发明通过引入Dijkstra算法搜索具有位置不确定性的回溯点，算法找到的第一个回溯点即为最佳回溯点，无需找到所有回溯点以确定最佳回溯点，优化了最佳回溯点的寻找方法；提出以回溯路径最短作为最佳回溯点的判断指标，并基于Dijkstra算法寻找回溯点，可使得到的回溯路径最短；引入Dijkstra算法搜索回溯点，在算法搜索到回溯点的同时返回最佳回溯点和回溯路径，优化了死区脱困方法，降低了算法复杂性。
如图1为本发明实现的流程图。
如图2为本发明Dijkstra算法路径规划的仿真效果图，图中黑色栅格为占据栅格，表示障碍物，白色栅格表示自由栅格，表示无障碍物，起始点栅格和目标点栅格以五角星表示，灰色方块所在栅格表示Dijkstra算法规划的路径点所在栅格，蓝色线表示相邻路径点间的连线。
如图3为本发明Dijkstra算法所得路径去除共线点后的仿真效果图。
如图4为本发明Dijkstra算法所得路径去除共线点和多余拐点后的仿真效果图。
如图5为验证本发明算法所使用的栅格地图，可自由设置栅格地图中障碍物的位置与大小，图中黑色栅格为障碍物占据栅格，白色栅格为自由栅格。
如图6为其他寻找最佳回溯点算法所得到的回溯点结果，图中灰色栅格表示未被探测过的栅格，即占据状态为未知的栅格，黑色栅格表示已被探测过且状态为占据的栅格，白色栅格表示已被探测过且状态为自由的栅格，灰色虚线表示路径，灰色五角星表示起始点栅格，灰色圆点所在栅格为死区点，图中灰色菱形和黑色菱形所在栅格为回溯点所在栅格，且黑色菱形所在栅格为最佳回溯点所在栅格。
如图7为以回溯路径最短作为最佳回溯点判断指标时得到的回溯点结果，该方法不会找到所有回溯点，只找到回溯路径最短的回溯点，图中黑色菱形所在栅格为最佳回溯点所在栅格。
如图8为未以回溯路径最短作为最佳回溯点判断指标的全覆盖路径规划仿真效果图，图中黑色栅格表示占据栅格，白色栅格表示自由栅格，灰色五角星所在栅格分别表示起始点栅格和终点栅格，灰色圆点所在栅格表示死区点所在栅格，即回溯路径起点所在栅格，黑色菱形所在栅格表示回溯路径终点所在栅格，灰色方块所在栅格表示回溯路径上除起点和终点外的路径点所在栅格，灰色虚线表示全覆盖路径，灰色实线表示回溯路径。
如图9为本发明区域全覆盖路径规划的仿真效果图，与图五仿真效果相比，本发明以回溯路径最短作为最佳回溯点的判断指标，可使每次回溯的路径最短。
本领域技术人员可以将本实施例理解为实施例1、实施例2的更为具体的说明。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
Claims (10)
1.一种基于Dijkstra算法的全覆盖路径规划方法，其特征在于，所述方法包括如下步骤：
步骤S1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
步骤S2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
步骤S3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
步骤S4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入步骤S2。
2.根据权利要求1所述的基于Dijkstra算法的全覆盖路径规划方法，其特征在于，所述步骤S1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
3.根据权利要求1所述的基于Dijkstra算法的全覆盖路径规划方法，其特征在于，所述步骤S2的具体实现方式为：设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
4.根据权利要求1所述的基于Dijkstra算法的全覆盖路径规划方法，其特征在于，所述步骤S3包括如下步骤：
步骤S3.1：定义回溯点的条件；
步骤S3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
步骤S3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
步骤S3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
步骤S3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复步骤S3.3-步骤S3.5，直到搜索到回溯点，转入步骤S3.6，或者open列表为空，结束规划；
步骤S3.6：去除所得回溯路径中的共线点；
步骤S3.7：去除步骤S3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
5.根据权利要求1所述的基于Dijkstra算法的全覆盖路径规划方法，其特征在于，所述步骤S4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入步骤S2开始新的Boustrophedon运动。
6.一种基于Dijkstra算法的全覆盖路径规划系统，其特征在于，所述系统包括如下模块：
模块M1：建立两个栅格地图，一个存储声纳探测所得的环境占据信息，另一个标记是否被访问过；
模块M2：Boustrophedon运动访问环境栅格地图，并在访问栅格地图中标记访问信息；
模块M3：Boustrophedon运动到达死区后，利用Dijkstra算法寻找最近的回溯点，若找到回溯点，则平滑回溯路径；若没有找到回溯点，则结束规划；
模块M4：根据平滑回溯路径，控制水下机器人运动到回溯点后，转入模块M2。
7.根据权利要求6所述的基于Dijkstra算法的全覆盖路径规划系统，其特征在于，所述模块M1的具体实现方式为：根据需要进行搜索的区域的大小建立两个栅格地图，一个为环境栅格地图，存储声纳探测所得的环境占据信息；一个为访问栅格地图，存储水下机器人对环境的访问信息；初始时刻，通过GPS获取水下机器人的位置信息，通过装载在水下机器人上的声纳探测周围环境的占据信息，并更新环境栅格地图与访问栅格地图。
8.根据权利要求6所述的基于Dijkstra算法的全覆盖路径规划系统，其特征在于，所述模块M2的具体实现方式为：设置Boustrophedon运动的移动优先级，移动过程中，每访问环境栅格地图中的一个栅格，就在访问地图中将此栅格设置为已访问；根据移动一个栅格后声纳新测得的信息更新环境栅格地图中对应栅格的状态，并对障碍物进行膨胀处理，将声纳测得为占据状态的栅格周围一圈的自由栅格设置为占据状态。
9.根据权利要求6所述的基于Dijkstra算法的全覆盖路径规划系统，其特征在于，所述模块M3包括如下模块：
模块M3.1：定义回溯点的条件；
模块M3.2：设置open列表和close列表，在寻找回溯点的初始时刻，将水下机器人当前所在位置加入open列表，close列表为空；
模块M3.3：寻找open列表中代价函数最小的节点，并设为当前节点；
模块M3.4：将当前节点移出open列表，并加入close列表中，判断当前节点是否为回溯点，若是回溯点，则搜索结束；
模块M3.5：若当前节点不是回溯点，寻找当前节点四邻域内的可行栅格，将其加入open列表，重复模块M3.3-模块M3.5，直到搜索到回溯点，转入模块M3.6，或者open列表为空，结束规划；
模块M3.6：去除所得回溯路径中的共线点；
模块M3.7：去除模块M3.6所得路径中的多余拐点，得到平滑后的Dijkstra回溯路径。
10.根据权利要求6所述的基于Dijkstra算法的全覆盖路径规划系统，其特征在于，所述模块M4的具体实现方式为：根据平滑回溯路径，控制水下机器人从起始路径点按直线运动到下一个路径点，直到到达回溯点，然后转入模块M2开始新的Boustrophedon运动。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211536497.XA CN116225034A (zh) | 2022-12-02 | 2022-12-02 | 基于Dijkstra算法的全覆盖路径规划方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211536497.XA CN116225034A (zh) | 2022-12-02 | 2022-12-02 | 基于Dijkstra算法的全覆盖路径规划方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116225034A true CN116225034A (zh) | 2023-06-06 |
Family
ID=86575683
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211536497.XA Pending CN116225034A (zh) | 2022-12-02 | 2022-12-02 | 基于Dijkstra算法的全覆盖路径规划方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116225034A (zh) |
-
2022
- 2022-12-02 CN CN202211536497.XA patent/CN116225034A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102693581B1 (ko) | 시각적 로봇 기반의 과거 지도 이용 방법 | |
| CN114510056B (zh) | 室内移动机器人的平稳移动全局路径规划方法 | |
| CN104462190B (zh) | 一种基于海量空间轨迹挖掘的在线的位置预测方法 | |
| CN107389079B (zh) | 高精度路径规划方法和系统 | |
| CN105955262A (zh) | 一种基于栅格地图的移动机器人实时分层路径规划方法 | |
| CN109163722B (zh) | 一种仿人机器人路径规划方法及装置 | |
| CN110319845A (zh) | 用于确定两点之间的可达路径的方法、装置和系统 | |
| CN115164907B (zh) | 基于动态权重的a*算法的森林作业机器人路径规划方法 | |
| CN114440916B (zh) | 一种导航方法、装置、设备及存储介质 | |
| CN110220528A (zh) | 一种基于a星算法的自动驾驶无人车双向动态路径规划方法 | |
| CN106204719B (zh) | 基于二维邻域检索的三维场景中海量模型实时调度方法 | |
| CN112114584A (zh) | 一种球形两栖机器人的全局路径规划方法 | |
| CN108917769A (zh) | 一种基于九叉树的机器人自适应栅格地图创建方法 | |
| CN109556609B (zh) | 一种基于人工智能的避碰方法及装置 | |
| CN112417070B (zh) | 路网拓扑构建方法、装置、电子设备及存储介质 | |
| CN109460564B (zh) | 一种电网潮流图布线方法 | |
| Wang et al. | Research on path planning of mobile robot based on improved jump point search algorithm | |
| CN110726416A (zh) | 一种基于障碍区域扩张策略的强化学习路径规划方法 | |
| CN114692357A (zh) | 基于改进元胞自动机算法的三维航路网络规划系统及方法 | |
| CN116225034A (zh) | 基于Dijkstra算法的全覆盖路径规划方法及系统 | |
| CN114815801A (zh) | 一种基于策略-价值网络及mcts的自适应环境路径规划方法 | |
| Wang et al. | Application of A* algorithm in intelligent vehicle path planning | |
| CN115617034B (zh) | 多智能体的环境探索方法、装置、电子设备及存储介质 | |
| CN116839609A (zh) | 全覆盖路径规划方法、装置及计算机可读存储介质 | |
| CN113985892A (zh) | 一种基于改进a＊算法的智能船舶路径规划方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |