一种球形两栖机器人的全局路径规划方法与流程
(一)技术领域：
本发明属于球形两栖机器人领域，具体涉及一种球形两栖机器人的全局路径规划方法。
(二)
背景技术：
：
目前，两栖机器人因其具备环境适应性强、运动高度灵活等优势而被广泛应用，包括多目标点巡航、搜寻救援、勘测探索、管道检查、资源开发、渔业捕捞等方面。机器人的路径规划研究已经成为当前机器人技术中较为重要的研究课题之一。而路径规划的发展也为机器人走向实用、更安全更可靠地执行任务提供了安全保障。目前大多数的机器人规划出的路径由于转折点较多，使得路径不平滑，从而导致效率低下且耗能大，并且只能对单一的目标点进行路径规划，已无法满足现在社会的需求，如果能找到一条转折点较少，路径较平滑，且能使机器人完成多个目标点的路径显得尤为重要，本方法针对以上问题，首先融合了floyd算法平滑路径，其次融合蚁群算法使得机器人完成多个目标点的任务。
技术实现要素：
：
本发明的目的在于提供一种球形两栖机器人的全局路径规划方法，既解决了现有技术中路径由于转折点较多，导致路径不平滑的问题，又解决了只能对单一目标点路径规划的问题，是一种操作简单容易实现的路径规划方法。
本发明的技术方案：一种球形两栖机器人的全局路径规划方法，其特征在于它具体包括如下步骤：
(1)通过两栖机器人所配置的激光雷达全局环境信息进行采集，得到能够描述作业环境信息的环境图像信号；
(2)根据步骤(1)采集到的环境图像信号，基于栅格法，创建两栖机器人的作业环境模型；
(3)基于步骤(1)创建的两栖机器人的作业环境模型，利用a-star算法规划出每两个目标点的路径；
所述步骤(3)中采用a-star算法规划每两个目标点间的规划路径，由以下步骤构成：
(3-1)建立存放待考察节点的open列表和存放从open列表中选出的即考察到的，且评价函数最低的节点的closed列表，closed列表中所有的节点将组成最终的路径；所述待考察节点由a-star算法原理直接给出，从起点开始，以起点为中心，周围与之相邻的节点，即为待考察节点；此外，从待考察节点中选出评价函数最低的节点，再以评价函数最低的节点为中心，周围与之相邻的节点，也是待考察节点；直到找到目标点为止，待考察节点包括已经考察过的节点，或者含有障碍物的节点；
(3-2)确定起始栅格节点s，把起始栅格节点s归入open列表中；
(3-3)在open列表中进行选择，根据式(1)所示的评价函数，计算open列表中的每个待考察节点的评价函数，并从中选择评价函数最低的节点放入到closed列表中；
f(i)＝g(i)+h(i)(1)
其中，i表示第i个节点，g(i)表示从起始栅格节点s到第i个节点的付出代价值，h(i)表示从第i个节点到目标点t的预测代价值。
(3-4)若此时open列表为空，也就是指以当前节点为中心，open列表中的待考查的节点分为两大类，一类是障碍物栅格不能通过，另一类则是已经放入到了closed列表中，导致待考察的节点都不能选择，此时，两种情况都会导致表现为open列表为空，意味着路径规划失败；若此时open列表为非空，则查找open列表中评价函数f(i)最小的节点，将其作为当前节点a，并将该节点a放入到closed列表中；
(3-5)判断步骤(3-4)中选取的节点a是否为目标点，如果是，则流程结束；反之，则以该节点a为父节点在其周围进行子节点扩展，并进行扩展后处理；
所述步骤(3-5)中对子节点进行扩展后处理具体是指：
(3-5-1)若待扩展的子节点即不在closed列表中，也不在open列表中，同时也不在障碍栅格中，则把该节点加入到open表中；
(3-5-2)若待扩展的子节点在closed列表中或者是障碍栅格中，则跳过该子节点；
(3-5-3)若在open列表中已经有该子节点，则判断该子节点的评价函数f(i)值是否小于open列表中的评价函数f(i)值，如果是，则更新open列表中的评价函数f(i)值，依此将待扩展的子节点遍历，并找到评价函数f(i)值最小的子节点。
(3-6)根据(3-5)步骤继续进行子节点扩展，直到当目标节点被放入到closed列表时，则停止搜索，表明找到从起点到目标点通行的路径；当open列表为空且目标节点未被找到时，也要停止搜索，标明此时没有能通行的路径。
(4)利用融合floyd算法对步骤(3)规划出的目标点之间的路径进行平滑度优化，进而得到优化后的路径，并生成距离矩阵e；
所述步骤(4)中利用融合floyd算法对步骤(3)规划出的目标点之间的路径进行平滑度优化，进而得到优化后的路径，并生成距离矩阵e，步骤如下：
(4-1)删除路径节点在同一线上的中间节点，只保留起点、拐点和终点，从而减少计算量；
(4-2)从起始栅格节点s开始，在保留的路径节点之间每隔k步长取点，每取一个点，判断该节点与前一端路径节点之间是否有障碍物，并由公式计算路径旁边障碍物到路径的距离d，如式(2)：
式中，xe、ye为障碍点的坐标，xa,ya是取步长k之前的节点，xb,yb为取步长k之后的节点；
判断该距离d与安全距离d的大小，若在安全距离之外，则该节点满足路径要求，选择当前节点为路径节点，否则不选择；因此，减少了路径的转折点，使得路径变得平滑，且距离也会缩短；
所述步骤(4-2)中的安全距离d是球形两栖机器人的直径。
(4-3)对规划后的路径进行双向优化，即从目标点反向取点，并按照步骤(4-2)进行距离判断，保存路径，并生成距离矩阵e，该路径则为平滑度优化后的路径；
所述步骤(4-3)中的距离矩阵e保存了每两个目标点间的距离，是二维矩阵，其横纵坐标是相同的，均用1,2,3…表示，分别表示坐标点的位置；
(5)结合步骤(4)优化后的路径，融合蚁群算法确定所有目标点的遍历顺序，并找到一条既能遍历所有目标点又能最终返回起点的最短距离，作为该两栖机器人的移动路径；
所述步骤(5)中融合蚁群算法确定所有目标点的遍历顺序，并找到一条既能遍历所有目标点又能最终返回起点的最短距离，具体包括以下内容：
由于蚁群算法可描述为已知所有目标点的距离，因此，可以在每个目标点只经过一次的前提下，求解一条最短的遍历n个目标点的路径，即：
p＝(c1,c2,c3,...cn)(3)
式中，p为目标点的遍历顺序表示，ci为目标点编号，i＝1,2,……,n，d(ci,ci+1)表示相邻编号目标点之间的距离，f(p)遍历n个目标点的路径。
所述目标点的遍历顺序的具体确定方法为：
(5-1)初始化蚁群参数各路径的信息素，蚂蚁数量为m，n为目标点的数量，每一只蚂蚁随机选择n个目标点的某一个目标点为出发点；
(5-2)每只蚂蚁按照如式(5)所示的状态转移概率，计算下一个待访问的目标点，即从第i个目标点移到下一个顶点j，直到所有蚂蚁完成一次周游，即所有目标点被所有蚂蚁遍历；
式中：allowedk为蚂蚁下一步可走的目标点集合；
(5-3)当所有蚂蚁执行完一次循环以后，记录每只蚂蚁经过的路径总长度l以及当前迭代的最短路径，并按公式(6)-(8)的信息素更新公式，更新目标点之间的信息素浓度：
其中，ρ0为信息素挥发系数；nc为当前蚂蚁迭代次数；nmax为蚂蚁最大的迭代次数，δτi，j为目标点i，j之间来自所有蚂蚁的信息素浓度值和，l为第k只蚂蚁在当前搜索中行走距离之和，q表示信息素的强度；
(5-4)若当迭代次数达到最大，则输出最优遍历顺序，将其作为球形两栖机器人在路径规划过程中所遍历目标点的顺序，从而完成机器人的多目标点路径规划；否则，迭代次数加1，并清空蚂蚁路径记录表，返回步骤(5-2)继续执行。
本发明的工作原理：蚁群算法是一种模拟蚂蚁利用信息素确定觅食路径的方法；在觅食过程中优先选取信息素浓度较高的路径，同时也释放定量的信息素，随着信息素的浓度越来越高，形成正反馈，通过遍历所有路径，选择信息素浓度最高的路径作为最优路径。
本发明的优越性：通过改变传统a-star算法搜索策略，融合了floyd算法，对路径进行了平滑度优化，缩短了路径的长度；其次融合蚁群算法解决了多个目标点的遍历顺序问题。
附图说明：
图1为本发明所涉一种球形两栖机器人的全局路径规划方法的融合floyd算法与蚁群算法的总体原理流程图。
图2为本发明所涉一种球形两栖机器人的全局路径规划方法的平滑度优化后与平滑度优化前的对比图。(其中，图2-a为改进前，图2-b是改进后)。
图3为本发明所涉一种球形两栖机器人的全局路径规划方法中融合floyd算法对路径进行平滑度优化的原理流程示意图。
图4为本发明所涉一种球形两栖机器人的全局路径规划方法中融合floyd算法和蚁群算法解决多目标点间路径规划对比图(其中，图4-a为已经融合了floyd算法，没有融合蚁群算法，图4-b是既融合了floyd算法，也融合了蚁群算法)。
具体实施方式：
实施例：一种球形两栖机器人的全局路径规划方法，如图1所示，其特征在于它具体包括如下步骤：
(1)通过两栖机器人所配置的激光雷达全局环境信息进行采集，得到能够描述作业环境信息的环境图像信号；
(2)根据步骤(1)采集到的环境图像信号，基于栅格法，创建两栖机器人的作业环境模型；
(3)基于步骤(1)创建的两栖机器人的作业环境模型，利用a-star算法规划出每两个目标点的路径；
(3-1)建立存放待考察节点的open列表和存放从open列表中选出的即考察到的，且评价函数最低的节点的closed列表，closed列表中所有的节点将组成最终的路径；所述待考察节点由a-star算法原理直接给出，从起点开始，以起点为中心，周围与之相邻的节点，即为待考察节点；此外，从待考察节点中选出评价函数最低的节点，再以评价函数最低的节点为中心，周围与之相邻的节点，也是待考察节点；直到找到目标点为止，待考察节点包括已经考察过的节点，或者含有障碍物的节点；
(3-2)确定起始栅格节点s，把起始栅格节点s归入open列表中；
(3-3)在open列表中进行选择，根据式(1)所示的评价函数，计算open列表中的每个待考察节点的评价函数，并从中选择评价函数最低的节点放入到closed列表中；
f(i)＝g(i)+h(i)(1)
其中，i表示第i个节点，g(i)表示从起始栅格节点s到第i个节点的付出代价值，h(i)表示从第i个节点到目标点t的预测代价值；
(3-4)若此时open列表为空，也就是指以当前节点为中心，open列表中的待考查的节点分为两大类，一类是障碍物栅格不能通过，另一类则是已经放入到了closed列表中，导致待考察的节点都不能选择，此时，两种情况都会导致表现为open列表为空，意味着路径规划失败；若此时open列表为非空，则查找open列表中评价函数f(i)最小的节点，将其作为当前节点a，并将该节点a放入到closed列表中；
(3-5)判断步骤(3-4)中选取的节点a是否为目标点，如果是，则流程结束；反之，则以该节点a为父节点在其周围进行子节点扩展，并进行扩展后处理；
(3-5-1)若待扩展的子节点即不在closed列表中，也不在open列表中，同时也不在障碍栅格中，则把该节点加入到open表中；
(3-5-2)若待扩展的子节点在closed列表中或者是障碍栅格中，则跳过该子节点；
(3-5-3)若在open列表中已经有该子节点，则判断该子节点的评价函数f(i)值是否小于open列表中的评价函数f(i)值，如果是，则更新open列表中的评价函数f(i)值，依此将待扩展的子节点遍历，并找到评价函数f(i)值最小的子节点。
(3-6)根据(3-5)步骤继续进行子节点扩展，直到当目标节点被放入到closed列表时，则停止搜索，表明找到从起点到目标点通行的路径；当open列表为空且目标节点未被找到时，也要停止搜索，标明此时没有能通行的路径。
(4)利用融合floyd算法对步骤(3)规划出的目标点之间的路径进行平滑度优化，如图2所示，进而得到优化后的路径，并生成距离矩阵e，步骤如下，如图2-b所示：
(4-1)删除路径节点在同一线上的中间节点，只保留起点、拐点和终点，从而减少计算量；
(4-2)从起始栅格节点s开始，在保留的路径节点之间每隔k步长取点，每取一个点，判断该节点与前一端路径节点之间是否有障碍物，并由公式计算路径旁边障碍物到路径的距离d，如式(2)：
式中，xe、ye为障碍点的坐标，xa,ya是取步长k之前的节点，xb,yb为取步长k之后的节点；
判断该距离d与安全距离d的大小，安全距离d是球形两栖机器人的直径，若d在安全距离之外，则该节点满足路径要求，选择当前节点为路径节点，否则不选择；因此，减少了路径的转折点，使得路径变得平滑，且距离也会缩短；
(4-3)对规划后的路径进行双向优化，即从目标点反向取点，并按照步骤(4-2)进行距离判断，保存路径，并生成距离矩阵e，该路径则为平滑度优化后的路径；
距离矩阵e保存了每两个目标点间的距离，是二维矩阵，其横纵坐标是相同的，均用1,2,3…表示，分别表示坐标点的位置；比如横坐标是3，代表第三个坐标点的位置，纵坐标是4，代表第四个坐标点的位置，那么它们的交叉处则代表第三个目标点和第四个目标点之间的距离，这个只是一个矩阵用来保存规划好的路径距离，后续融合蚁群算法，直接调用矩阵里的距离就可以。
(5)结合步骤(4)优化后的路径，融合蚁群算法确定所有目标点的遍历顺序，并找到一条既能遍历所有目标点又能最终返回起点的最短距离，作为该两栖机器人的移动路径，如图4所示：
融合蚁群算法确定所有目标点的遍历顺序，并找到一条既能遍历所有目标点又能最终返回起点的最短距离，具体包括以下内容，如图1所示：
由于蚁群算法可描述为已知所有目标点的距离，因此，可以在每个目标点只经过一次的前提下，求解一条最短的遍历n个目标点的路径，即：
p＝(c1,c2,c3,...cn)(3)
式中，p为目标点的遍历顺序表示，ci为目标点编号，i＝1,2,……,n，d(ci,ci+1)表示相邻编号目标点之间的距离，f(p)遍历n个目标点的路径。
目标点的遍历顺序的具体确定方法为，如图1所示：
(5-1)初始化蚁群参数各路径的信息素，蚂蚁数量为m，n为目标点的数量，每一只蚂蚁随机选择n个目标点的某一个目标点为出发点；
(5-2)每只蚂蚁按照如式(5)所示的状态转移概率，计算下一个待访问的目标点，即从第i个目标点移到下一个顶点j，直到所有蚂蚁完成一次周游，即所有目标点被所有蚂蚁遍历；
式中：allowedk为蚂蚁下一步可走的目标点集合；
(5-3)当所有蚂蚁执行完一次循环以后，记录每只蚂蚁经过的路径总长度l以及当前迭代的最短路径，并按公式(6)-(8)的信息素更新公式，更新目标点之间的信息素浓度：
其中，ρ0为信息素挥发系数；nc为当前蚂蚁迭代次数；nmax为蚂蚁最大的迭代次数，δτi，j为目标点i，j之间来自所有蚂蚁的信息素浓度值和，l为第k只蚂蚁在当前搜索中行走距离之和，q表示信息素的强度；
(5-4)若当迭代次数达到最大，则输出最优遍历顺序，将其作为球形两栖机器人在路径规划过程中所遍历目标点的顺序，从而完成机器人的多目标点路径规划；否则，迭代次数加1，并清空蚂蚁路径记录表，返回步骤(5-2)继续执行。
针对本发明的技术改进作如下说明：
(一)a-star算法是基于栅格地图的一种静态路网中求解最短路径的启发式搜索算法，适用于两栖机器人单目标路径规划，其评价函数如下：
f(i)＝g(i)+h(i)，式中：
i表示第i个节点；
g(i)表示从起始栅格节点s到该i节点的付出代价值；
h(i)表示从该i节点到目标点t的预测代价值；
a-star算法中维护两个列表：open列表和closed列表；没有考察到的节点放入到open列表中，考查到的放入closed列表中，步骤为：
(1)生成open列表和closed列表，把起始栅格节点s放入open列表；
(2)确认open列表为非空，查找删除open列表中f(i)值最小的节点，放入到closed列表中；
(3)当前节点若为目标节点，添加到closed列表中；
(4)处理当前节点的相邻子节点，被检测的子节点有三种可能的情况：
(4-1)若待扩展的子节点不在closed列表或open列表中，同时也不在障碍栅格中，则把该节点加入到open表中；
(4-2)若待扩展的子节点在closed列表中或者是障碍栅格中，则跳过该子节点；
(4-3)若在open列表中已经有该子节点，则判断该子节点的f(i)值是否小于open列表中的f(i)值，如果是则更新open列表中的f(i)值，依此将待扩展的子节点遍历，并找到f(i)值最小的子节点。
(5)处理完相邻的子节点，返回(4)循环；
(二)第一处改进：融合floyd算法对路径进行平滑度的优化，得到优化后的路径，生成距离矩阵e，包括以下步骤：
(1)删除路径节点同一线上的中间节点，只保留起点、拐点和终点。
(2)从起始栅格节点s开始，在保留的路径节点之间每隔k步长取点，每取一个节点与前一端路径节点，判断两点之间是否有障碍物，并由公式计算路径旁边障碍物到路径的距离d，判断与安全距离d的大小(d为机器人的直径)，若在安全距离之外，满足要求选择当前节点为路径节点，否则不选择。
其中x0，y0为障碍点的坐标。
(3)对规划后的路径进行双向优化，即从目标点反向取点判断(重复步骤(2)的判断方式)保存路径，生成距离矩阵e。
(三)第二处改进：融合蚁群算法确定所有目标点的遍历顺序，找到一条遍历所有目标点，最终能够返回起点的最短距离。
蚁群算法是一种模拟蚂蚁利用信息素确定觅食路径的方法；在觅食过程中优先选取信息素浓度较高的路径，同时也释放定量的信息素，随着信息素的浓度越来越高，形成正反馈，通过遍历所有路径，选择信息素浓度最高的路径作为最优路径。
蚁群算法可描述为已知所有目标点的距离，在每个目标点只经过一次的前提下，求解一条最短的遍历n个目标点的路径。
p＝(c1,c2,c3,...cn)
ci为目标点编号，式中d(ci,ci+1)为相邻编号目标点之间的距离，p为目标点的遍历顺序表示，f(p)遍历n个目标点的路径。
步骤如下：
(1)初始化；
(2)构建解空间；
每只蚂蚁随机放在不同目标点，并根据概率选择公式计算下一个待访问的目标点，直到所有目标点被所有蚂蚁遍历；
(3)更新信息素：记录每只蚂蚁经过的路径总长度l以及当前迭代的最短路径，并由信息素更新公式，更新目标点之间的信息素浓度。
(4)判断是否终止，若达到最大的迭代次数，输出最优遍历顺序，否则，迭代次数加1，并清空蚂蚁路径记录表，返回步骤2继续执行。
为了验证融合floyd算法的可行性，在matlab2017a软件中进行了实验仿真，结果如图2所示，在仿真地图中，圆圈为起始点，三角形为目标点，起始点和目标点之间的实线代表机器人的路径，对比前为没有经过路径优化的路径，路径长度为20.4853，转折点个数为7个，然而优化后的路径长度为19.2606，转折点个数为3个。由图可知，融合floyd算法在路径长度和路径拐点数都优于传统的a-star算法，拐点数的减少和路径的缩短，使得机器人的转向次数较少，路径缩短，间接地减少了机器人完成任务的时间，同时减少了机器人的耗能，所以拐点数和路径长度是评价路径优劣的一个重要指标。
为了验证融合蚁群算法的可行性，在matlab2017a软件中进行了实验仿真，结果如图4所示，在仿真地图中，圆圈为起始点，三角形为目标点，起始点和目标点之间的实线代表机器人的路径，对比前为单个目标点的路径规划，对比后为融合了蚁群算法的多目标点的路径规划图，在仿真地图中我们选择了10个点作为机器人需要遍历的目标点，从图中可以看出机器人能够找到一条遍历所有目标点，并且躲避障碍物，最终安全的返回原点，使得机器人能够执行连续目标点的多个任务。
综上，改进的a-star算法较之于传统a-star算法有一定的优势。
- 还没有人留言评论。精彩留言会获得点赞！