一种飞行巡逻机器人路径规划方法及其系统与流程

1.本发明涉及路径规划技术领域，具体涉及一种飞行巡逻机器人路径规划方法及其系统。


背景技术：

2.随着社会经济技术的发展，机器人技术的普及应用为人类生活带来了极大便利，拥有自主能力的移动机器人一方面降低了人类工作的成本，另一方面也为人类提供了更加丰富全面的信息资源。飞行巡逻机器人作为近年来移动机器人领域的研究热点，其自身的自主性能也在不断提高。路径规划作为移动机器人典型的自主能力，一直是机器人领域的研究热点，其中飞行巡逻机器人在复杂环境下的自主路径规划能力是飞行巡逻机器人发展的核心研究内容。目前飞行巡逻机器人路径规划的研究通常集中在室外空旷环境，相对而言室内环境空间更加狭小，边界条件多，避障难度大，因此对飞行巡逻机器人的精准避障及规划的实时性提出了更大的挑战。近年来国内也开始引进或研发室内飞行巡逻机器人飞行实验系统，在此背景下研究室内飞行巡逻机器人的路径规划算法，实现飞行巡逻机器人在室内复杂环境中的实时动态路径决策和引导功能，并利用现有技术平台进行实验验证，将为飞行巡逻机器人室内智能自主导航技术奠定坚实的基础，具有重要的理论研究意义及应用参考价值。
3.其中，路径规划是指飞行巡逻机器人在一定的环境下，给定该机器人的起点和目标点，通过一些算法、控制、优化方法等来寻找一条安全、动态可行、最优的路径，使飞行巡逻机器人从起点飞向目标点。到目前为止，路径规划算法已经得到了深入的研究，并且提出了各种各样不同的路径规划算法，如人工势场法、智能仿生算法、基于网格的搜索算法和基于采样的算法。
4.基于采样的算法不需要显式构建配置空间，例如众所周知的快速探索随机树(rrt)算法，该算法通过迭代添加随机节点来绘制边缘，并使用碰撞检测方法来验证边缘的有效性，此外，它在高维配置空间中具有优越的性能，同时rrt算法提供了概率完整性，它确保当迭代次数趋于无穷大时，找到可行路径的概率接近1。然而，传统的rrt算法有一些局限性，如收敛速度低和路径质量波动，许多研究人员对rrt算法进行了改进。例如，kuffner等人通过在rrt算法中引入贪婪启发式采样，提出了rrt-connect算法，与传统的rrt算法相比，rrt-connect算法增量地构建两个同时起源于起始状态和目标状态的快速探索随机树，为了提高算法的搜索效率，这些树中的每一棵都使用简单的贪婪启发式算法向其对应的树前进。也有学者提出了一种基于三角不等式的rrt连接重新布线方法，该算法缩短了规划时间，同时也通过重新布线来寻求优化，尽管rrt-connect显示出良好的搜索环境的能力，但与rrt算法类似，rrt-connect的路径质量不稳定，无法保证最优。为了解决rrt不考虑可行解成本的问题，karaman等人提出了考虑路径成本的rrt*算法，rrt*算法具有渐近最优性，同时继承了rrt算法的概率完备性，它可以在更短的时间内获得高质量的可行路径，但是，它的搜索效率很低。其中，本技术所述的rrt*算法是基于采样的算法，而rrt*算法自身存在
上述不足之处。为了进一步提高rrt*算法的性能，nasir等人提出了rrt*智能算法，该算法通过引入智能采样和路径优化技术来帮助接近最优或次优解，比rrt*具有更快的收敛速度，此外，当找到更好的路径时，优化路径的顶点将成为有偏采样的源。然而，随着算法的不断迭代，节点数量无限增加，这对计算机内存是一个重大挑战，为了更有效地使用内存，adiyatov等人提出了rrt*-fn算法，该算法限制了树中节点的最大数量，并在节点数量超过预设值时删除了对优化路径没有帮助的无用节点，这减少了计算机的内存使用，并加快了对相邻节点的搜索。受节点限制的启发，gammell等人提出了informed rrt*算法，该算法通过生成超椭球采样子集来限制节点采样的范围，并在超椭球子集中进行直接采样，以提高算法的收敛速度和最终解的质量，然而，这种算法有一些局限性，事实上，在一些特殊的环境中，如狭窄的室内长走廊和迷宫，由于路径成本大，超椭球采样面积可能过大，甚至整个配置空间都被超椭球包含在内，在这种情况下，informed rrt*的性能与rrt*类似。有的研究人员通过改变树的生长策略，提出了基于三角不等式的quick rrt*算法，这通过在选择最优父节点时考虑相邻节点的父节点来提高算法的收敛速度，然而quick rrt*在诸如狭窄环境之类的特殊环境中是无效的，这又需要对算法继续进行改进。因此，针对上述现有技术的不足有必要进行改进。


技术实现要素：

5.本发明主要解决的技术问题是提供一种飞行巡逻机器人路径规划方法及其系统，以提高了rrt*算法在飞行巡逻机器人室内特殊环境下的路径搜索效率。
6.根据第一方面，一种实施例中提供一种飞行巡逻机器人路径规划方法。该飞行巡逻机器人路径规划方法包括：
7.生成从起点到目标点的初始可行路径；
8.生成扩展区域的步骤：基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域；其中，所述初始可行路径位于所述扩展区域的内部；
9.生成渐进优化路径的步骤：在所述扩展区域内执行启发式搜索，以生成一条渐进优化路径；
10.迭代的步骤：依次重复地执行将所述渐进优化路径作为所述初始可行路径的步骤、所述生成扩展区域的步骤以及所述生成渐进优化路径的步骤，直至所述渐进优化路径符合最终优化路径判断标准，将所述符合最终优化路径判断标准的渐进优化路径作为从所述起点到目标点的最终优化路径。
11.一实施例中，基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域，包括：
12.获取与所述初始可行路径中的子路径段对应的子扩展区域；
13.基于所述初始可行路径中的子路径段，将与所述初始可行路径中的子路径段对应的子扩展区域进行拼接，以得到所述与所述初始可行路径对应的扩展区域。
14.一实施例中，所述获取与所述初始可行路径中的子路径段对应的子扩展区域，包括：
15.以所述初始可行路径中所述子路径段的邻接点为子路径段顶点，基于与所述子路径段顶点对应的第一子路径段向量和第二子路径段向量确定由所述子路径段顶点指向所
述初始可行路径的一侧的方向向量；其中，所述初始可行路径由若干所述子路径段彼此首尾连接而成；所述第一子路径段向量和第二子路径段向量是以所述子路径段顶点为邻接点的第一子路径段和第二子路径段所分别对应的向量；
16.基于实际膨胀距离、所述子路径段顶点和所述方向向量，确定与所述子路径段顶点对应的第一子扩展区域顶点和第二子扩展区域顶点；其中，每个所述子扩展区域对应两个所述第一子扩展区域顶点和两个所述第二子扩展区域顶点，所述第一子扩展区域顶点和第二子扩展区域顶点均作为所述子扩展区域的顶点；所述实际膨胀距离为由所述子路径段顶点沿所述方向向量延伸的距离；
17.将由所述子扩展区域对应的两个所述第一子扩展区域顶点和两个所述第二子扩展区域顶点所围成的区域作为所述子扩展区域。
18.一实施例中，所述与所述子路径段顶点对应的第一子扩展区域顶点所对应的向量和第二子扩展区域顶点所对应的向量的表达式分别为：
[0019][0020][0021]
其中，所述表示所述子路径段顶点所对应的向量，i表示所述子路径段顶点的编号，所述表示所述由所述子路径段顶点指向所述初始可行路径的一侧的方向向量，所述d表示所述实际膨胀距离，所述θi是所述与所述子路径段顶点对应的第一子路径段向量和第二子路径段向量之间的夹角的一半。
[0022]
一实施例中，所述实际膨胀距离d的表达式为：
[0023]
d＝k*d
base
，
[0024]
其中，所述k为动态扩展系数，所述d
base
为初始扩展距离；所述k的取值范围为0.75至1.25。
[0025]
一实施例中，所述动态扩展系数k的表达式为：
[0026][0027]
其中，所述i表示当前的迭代次数；所述i
init
表示生成所述初始可行路径时所完成的迭代次数；n表示预设的最大迭代次数。
[0028]
一实施例中，所述初始扩展距离d
base
的表达式为：
[0029][0030]
其中，所述ε表示初始扩展系数，所述l和w分别表示与所述初始可行路径对应的环境平面的长度和宽度。
[0031]
根据第二方面，一种实施例中提供一种改进rrt*的飞行巡逻机器人路径规划系统。该飞行巡逻机器人路径规划系统包括：
[0032]
传感器模块，被配置为获取飞行巡逻机器人的环境数据；
[0033]
飞控模块，被配置为接收所述飞行巡逻机器人的环境数据，基于所述飞行巡逻机
器人的环境数据计算得出所述飞行巡逻机器人的位置和姿态，
[0034]
路径规划模块，被配置为所述飞控模块所计算得出所述飞行巡逻机器人的位置和姿态，利用如本文中任一项实施例所述的飞行巡逻机器人路径规划方法生成从所述起点到目标点的最终优化路径；其中，所述飞控模块能够根据所述飞行巡逻机器人的位置和姿态以及所述最终优化路径对所述飞行巡逻机器人进行控制。
[0035]
一实施例中，所述环境数据包括：超声波测距雷达和激光测距雷达所获得飞行巡逻机器人周围的障碍物距离和角度数据、光流传感器所获得飞行巡逻机器人运动的速度数据、三轴陀螺仪所获得飞行巡逻机器人的角速度和角加速度数据、三轴加速度计所获得飞行巡逻机器人的加速度数据、三轴磁力计所获得飞行巡逻机器人的航向数据和气压计所获得飞行巡逻机器人的飞行高度数据中的多个或全部。
[0036]
根据第三方面，一种实施例中提供一种计算机可读存储介质。该介质包括程序，所述程序能够被处理器执行以实现如本文中任一项实施例所述的飞行巡逻机器人路径规划方法。
[0037]
本技术的有益效果是：
[0038]
本飞行巡逻机器人路径规划方法包括：生成从起点到目标点的初始可行路径；生成扩展区域的步骤：基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域；其中，所述初始可行路径位于所述扩展区域的内部；生成渐进优化路径的步骤：在所述扩展区域内执行启发式搜索，以生成一条渐进优化路径；迭代的步骤：依次重复地执行将所述渐进优化路径作为所述初始可行路径的步骤、所述生成扩展区域的步骤以及所述生成渐进优化路径的步骤，直至所述渐进优化路径符合最终优化路径判断标准，将所述符合最终优化路径判断标准的渐进优化路径作为从所述起点到目标点的最终优化路径，进而提高了rrt*算法在飞行巡逻机器人室内特殊环境下的路径搜索效率。
附图说明
[0039]
图1为一种实施例的飞行巡逻机器人路径规划方法的流程示意图；
[0040]
图2为一种实施例的从起点到目标点的初始可行路径的示意图；其中，x
star
表示初始可行路径的起点，x
goal
表示初始可行路径的目标点，obstacle表示从起点x
star
到目标点x
goal
的环境中的障碍物；
[0041]
图3为一种实施例的生成与所述初始可行路径对应的扩展区域的流程示意图；
[0042]
图4为一种实施例的获取与初始可行路径中的子路径段对应的子扩展区域的流程示意图；
[0043]
图5为一种实施例的与初始可行路径对应的扩展区域的示意图；
[0044]
图6为一种实施例的飞行巡逻机器人路径规划系统的模块示意图。
具体实施方式
[0045]
下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本技术能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申
请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本技术的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。
[0046]
另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。
[0047]
本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本技术所说“连接”、“联接”，如无特别说明，均包括直接和间接连接(联接)。
[0048]
本技术的技术构思是：首先，通过rrt-connect算法，快速探索从起点到目标点之间的环境以找到从起点到目标点的初始可行路径，然后将该初始可行路径扩展为扩展区域，并在扩展区域作为启发式采样区域；之后，在启发式采样区域中迭代搜索，该启发式采样区域也随着路径的不断优化而变化，最终获得连接起点和目标点的最优或次优路径。
[0049]
下面将结合实施例对本技术的技术方案进行详细说明。
[0050]
请参考图1，本技术提出的一种飞行巡逻机器人路径规划方法，包括：
[0051]
步骤s100：生成从起点到目标点的初始可行路径；
[0052]
生成扩展区域的步骤s200：基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域；其中，所述初始可行路径位于所述扩展区域的内部；
[0053]
生成渐进优化路径的步骤s300：在所述扩展区域内执行启发式搜索，以生成一条渐进优化路径；
[0054]
迭代的步骤s400：依次重复地执行将所述渐进优化路径作为所述初始可行路径的步骤、所述生成扩展区域的步骤s200以及所述生成渐进优化路径的步骤s300，直至所述渐进优化路径符合最终优化路径判断标准，将所述符合最终优化路径判断标准的渐进优化路径作为从所述起点到目标点的最终优化路径。
[0055]
需要说明的是，本领域技术人员可以根据实际应用场景而自行设定上述起点和目标点。
[0056]
一些实施例中，上述步骤s100可以利用现有的rrt-connect算法进行贪婪启发式抽样而生成从起点到目标点的初始可行路径。例如，请参考图2，第一步，可以将起点x
star
、目标点x
goal
和从起点x
star
到目标点x
goal
的室内的环境中的障碍物obstacle都作为路径规划的根节点插入。具体的，初始化起点x
star
、目标点x
goal
和障碍物obstacle，将上述起点和目标点的坐标信息加入到随机树(如图2中的tree1和tree2)，并在飞行巡逻机器人的路径规划空间中生成一个随机点x
rand
。第二步，在两个方向上交替构造两个快速探索的随机树(如图2中的tree1和tree2)，以探索配置空间，这使得rrt-connect算法更有针对性，进而缩短搜索路径所需的规划时间。具体的，在随机树tree1上找到距随机点x
rand
的欧氏距离最小的点，并记为x
nearest1
，该点的方向指向x
rand
，朝该点的方向生长一定的步长(即图2中的step)得到新的随机点x
new1
。若新的随机点x
new1
不满足路径规划空间中的障碍物约束，则舍弃该点重新生成随机点；若新的随机点x
new1
满足路径规划空间中的障碍物约束，则把rrt-connect算法生长后的树枝和端点注入到树上。第三步，引入贪婪启发式抽样策略。具体的，在扩展
过程中，位于起点x
star
和目标点x
goal
之间的上述两棵随机树(如图2中的tree1和tree2)会相互吸引。之后，计算得到随机树tree2上距随机点x
new1
的欧氏距离最小的点，并记为x
nearest2
，该点的方向指向x
new1
，朝该点的方向生长一定步长得到随机点x
new2
。若通过碰撞检测，则将x
new2
注入随机树tree2中，同时启动贪婪策略子程序，继续往朝向随机点x
new1
的方向生长，直到遇到障碍物或者两个随机树(如图2中的tree1和tree2)之间的距离小于阈值后停止；若遇到障碍物，则返回主程序交换上述两个随机树并重复上述采样过程，若两个随机树(如图2中的tree1和tree2)之间的距离小于连接阈值，则返回连接点信息，路径搜索结束，即完成了从起点到目标点的初始可行路径的生成工作。
[0057]
需要说明的是，一些实施例中，可以基于起点和目标点生成一个矩形，该矩形即为室外或上述室内的环境。由于上述环境的具体设定属于现有技术，故此处不再对此进行赘述。
[0058]
由于现有rrt*算法的一个显著缺点是，需要大量样本才能获得最佳路径；而从计算负载的角度来看，这对计算机的内存是一个极大的挑战，尤其是在高维空间中，需要更多的迭代来探索更大的空间，这大大降低了算法的效率。对rrt*的改进算法可以通过创建超椭球采样区域来提高算法的收敛速度，然而，在室内走廊较长的狭窄环境中，其超椭球面积无法有效减少。为了进一步提高rrt*算法的收敛速度，本技术引入了一种路径表达式采样方法，并提出了一种飞行巡逻机器人路径规划方法，在找到初始可行路径之前，本飞行巡逻机器人路径规划方法的工作方式与rrt-connect算法的工作方式相同，即，采用步骤s100找到初始可行路径；一旦找到了从起点到目标点的初始可行路径，本飞行巡逻机器人路径规划方法就基于实际膨胀距离对该初始可行路径进行扩展，以便获得扩展区域并在扩展区域中进行启发式采样；最后，得到连接起点和目标点的最优或次优路径以作为步骤s300中的最终优化路径。
[0059]
需要说明的是，本飞行巡逻机器人路径规划方法的关键是如何确定扩展区域，即如何基于实际膨胀距离对初始可行路径进行扩展而生成扩展区域。其中，初始可行路径是由一组依次首尾相连的子路径段组成。
[0060]
请参考图3，基于初始可行路径，生成与初始可行路径对应的扩展区域，包括：
[0061]
步骤s210：获取与初始可行路径中的子路径段对应的子扩展区域；
[0062]
步骤s220：基于初始可行路径中的子路径段，将与初始可行路径中的子路径段对应的子扩展区域进行拼接，以得到与初始可行路径对应的扩展区域。
[0063]
一些实施例中，扩展区域可以被视为由一组呈四边形的子扩展区域，基于初始可行路径拼接而成。
[0064]
请参考图4，获取与初始可行路径中的子路径段对应的子扩展区域，包括：
[0065]
步骤s211：以初始可行路径中子路径段的邻接点为子路径段顶点，基于与子路径段顶点对应的第一子路径段向量和第二子路径段向量确定由子路径段顶点指向初始可行路径的一侧的方向向量；其中，初始可行路径由若干子路径段彼此首尾连接而成；第一子路径段向量和第二子路径段向量是以子路径段顶点为邻接点的第一子路径段和第二子路径段所分别对应的向量；
[0066]
步骤s212：基于实际膨胀距离、子路径段顶点和方向向量，确定与子路径段顶点对应的第一子扩展区域顶点和第二子扩展区域顶点；其中，每个子扩展区域对应两个第一子
扩展区域顶点和两个第二子扩展区域顶点，第一子扩展区域顶点和第二子扩展区域顶点均作为子扩展区域的顶点；实际膨胀距离为由子路径段顶点沿方向向量延伸的距离；
[0067]
步骤s213：将由子扩展区域对应的两个第一子扩展区域顶点和两个第二子扩展区域顶点所围成的区域作为子扩展区域。其中，子扩展区域呈四边形。
[0068]
例如，请参考图5，初始可行路径v1v2v3v4可以由依次首尾连接的三条子路径段(即图3中的子路径段v1v2、v2v3和v3v4)组成。而子路径段v1v2、v2v3和v3v4的邻接点分别为v2和v3。子路径段的邻接点为子路径段的两个端点，即，一个子路径段有两个邻接点。与子路径段v1v2对应的是子扩展区域v
′1v
′2v
″2v
″1；与子路径段v2v3对应的是子扩展区域v
′2v
′3v
″3v
″2；与子路径段v3v4对应的是子扩展区域域v
′3v
′4v
″4v
″3。因此，在对初始可行路径进行扩展(如步骤s212)而生成子扩展区域时，需要找到子扩展区域的每个顶点(如步骤s212中的第一子扩展区域顶点和第二子扩展区域顶点)的位置。
[0069]
需要说明的是，步骤s211中，请参考图4，由子路径段顶点指向初始可行路径的一侧的方向向量的方向为第一子路径段向量和第二子路径段向量所构成的夹角的角平分线所指向的方向。
[0070]
与子路径段顶点对应的第一子扩展区域顶点所对应的向量和第二子扩展区域顶点所对应的向量的表达式分别为：
[0071][0072][0073]
其中，表示子路径段顶点所对应的向量，表示由子路径段顶点指向初始可行路径的一侧的方向向量，d表示实际膨胀距离，θi是与子路径段顶点对应的第一子路径段向量和第二子路径段向量之间的夹角的一半(请参考图中的θ2)。
[0074]
一些实施例中，请参考图5，上述步骤s212中子扩展区域(位于该图左下角的第一个子扩展区域)内的与子路径段顶点v1对应的第一子扩展区域顶点v
′1和第二子扩展区域顶点v
″1可以基于上述与子路径段顶点对应的第一子扩展区域顶点v
′i和第二子扩展区域顶点v
″i的表达式进行确定。同理，位于该图左下角的第一个子扩展区域内的与子路径段顶点v2对应的第一子扩展区域顶点v
′2和第二子扩展区域顶点v
″2也以基于上述与子路径段顶点对应的第一子扩展区域顶点v
′i和第二子扩展区域顶点v
″i的表达式进行确定。
[0075]
一些实施例中，请参考图5，上述步骤s213中，可以将第一子扩展区域顶点v
′1、第二子扩展区域顶点v
″1、第一子扩展区域顶点v
′2和第二子扩展区域顶点v
″2连接起来，而由第一子扩展区域顶点v
′1、第二子扩展区域顶点v
″1、第一子扩展区域顶点v
′2和第二子扩展区域顶点v
″2所围成的区域即为该图左下角的第一个子扩展区域。同理，其他子扩展区域也可以基于上述步骤生成而成。之后，基于初始可行路径v1v2v3v4，将与初始可行路径v1v2v3v4中的子路径段(即图3中的子路径段v1v2、v2v3和v3v4)分别对应的子扩展区域依次拼接起来，即为与初始可行路径对应的扩展区域。例如，当初始可行路径v1v2v3v4为子路径段v1v2、子路径段v2v3和子路径段v3v4构成时，与该初始可行路径中的子路径段分别对应的子扩展区域
分别为该图左下角的第一个子扩展区域v
′1v
′2v
″2v
″1、该图中间的第二个子扩展区域v
′2v
′3v
″3v
″2和该图右上角的第三个子扩展区域v
′3v
′4v
″4v
″3构成。
[0076]
需要说明的是，在使用上述步骤s100获得初始可行路径之后，需要基于实际膨胀距离对初始可行路径进行扩展而得到扩展区域。在这个过程中，实际膨胀距离的选择会显著影响本飞行巡逻机器人路径规划方法的性能。为了更好地适应不同的环境(即从起点到目标点之间的空间环境)，本技术提出了一种自适应路径扩展距离选择方法，以获取恰当的实际膨胀距离。
[0077]
实际膨胀距离d的表达式为：
[0078]
d＝k*d
base
，
[0079]
其中，k为动态扩展系数，d
base
为初始扩展距离；k的取值范围为0.75至1.25。
[0080]
动态扩展系数k的表达式为：
[0081][0082]
其中，i表示当前的迭代次数；i
init
表示生成初始可行路径时所完成的迭代次数；n表示预设的最大迭代次数。
[0083]
需要说明的是，本飞行巡逻机器人路径规划方法在未搜索得到最终优化路径之前一直处于循环迭代状态，而当前迭代次数则是从第1次到n次迭代中的当前次迭代所累积的次数。
[0084]
需要说明的是，设置上述动态扩展系数k，是为了进一步地提高本飞行巡逻机器人路径规划方法的收敛速度。也就是说，动态扩展系数的加入使本飞行巡逻机器人路径规划方法的采样面积随着方法的迭代而由大变小，增加了早期迭代的搜索广度，提高了后期迭代的搜索效率。
[0085]
需要说明的是，上述初始扩展距离d
base
首先基于环境(即从起点到目标点之间的空间环境)的大小来确定。
[0086]
其中，初始扩展距离d
base
的表达式为：
[0087][0088]
其中，ε表示初始扩展系数，l和w分别表示与初始可行路径对应的环境平面的长度和宽度。max{l,w}表示l和w中的最大值。
[0089]
需要说明的是，本领域技术人员可以根据环境的大小和复杂性对ε进行更改，以调整本飞行巡逻机器人路径规划方法在搜索中的广度和效率。例如，对于只有一条路径的室内狭窄环境，应该取较大的ε值来获得较小的扩展区域，进而可以增加获得扩展区域内有效采样点的概率和算法的收敛速度。在一些实施例中，ε＝8。
[0090]
一些实施例中，上述步骤s200中，可以一次性生成与初始可行路径对应的所有子扩展区域，而所有的子扩展区域构成扩展区域。
[0091]
一些实施例中，上述步骤s300，可以采用rrt-connect算法以在扩展区域内执行迭代搜索，进而获得最终优化路径。例如，第一步，采用rrt-connect算法在位于更大范围的随机树tree1上查找距新的随机点x
rand
的欧氏距离最小的点，并将该点记为新的x
nearest1
，该点的方向指向新的x
rand
，朝该点的方向生长一定的步长step得到新的随机点x
new1
。其中，上述
第一步中的“更大范围”指的是上述步骤s200所生成的扩展区域。第二步，计算位于更大范围的随机树tree2上距上述新的随机点x
new1
的欧氏距离最小的点，并将该点记为x
nearest2
，该点的方向指向上述新的x
new1
，朝该点的方向生长一定的步长step得到新的随机点x
new2
。同理，上述第二步中的“更大范围”指的也是上述步骤s200所生成的扩展区域。不断地重复上述第一步和第二步。第三步，将不断地重复上述第一步和第二步而得到的路径规划点连接起来，此时就可以得到飞行巡逻机器人从起点到目标点的一条路径规划轨迹。上述路径规划轨迹即为渐进优化路径。其中，可以通过不断地追溯上述若干的随机点的父节点，进而找到从起点到目标点的该条渐进优化路径。最后，输出最优或次优的无碰撞路径作为最终优化路径。
[0092]
需要说明的是，上述步骤s300，也可以采用还可以采用rrt算法、rrtgoalbias算法或rrtgoalzoom算法等在扩展区域内执行迭代搜索，进而获得最终优化路径。一些实施例中，上述步骤s200中，也可以逐个生成与初始可行路径中每个子路径段对应的子扩展区域。之后，执行上述步骤s300。之后，重复执行步骤s100生成初始可行路径中新的一个子路径段，再重复执行步骤s200和步骤s300，如此往复，最终在由多个子扩展区域构成的扩展区域迭代搜索出上述最终优化路径。最后，输出最优或次优的无碰撞路径作为最终优化路径。
[0093]
需要说明的是，上述步骤s300中的随机树tree1和tree2已经位于上述上述步骤s200所生成的扩展区域的内部。
[0094]
需要说明的是，步骤s300中，在扩展区域已经确定的情况下，在扩展区域内执行迭代搜索(例如利用rrt-connect算法)以获得最终优化路径属于本领域的现有技术，故对最终优化路径的具体获取过程不再赘述。
[0095]
上述迭代的步骤s400中，不断重复地执行将所述渐进优化路径作为所述初始可行路径、生成扩展区域的步骤s200以及生成渐进优化路径的步骤s300，直至所述渐进优化路径符合最终优化路径判断标准，将所述符合最终优化路径判断标准的渐进优化路径作为从所述起点到目标点的最终优化路径。
[0096]
需要说明的是，在充满障碍物的地图/环境平面中，肯定是存在一条最优的从起点到目标点的路径，即最终优化路径。不断重复地执行将所述渐进优化路径作为所述初始可行路径、生成扩展区域的步骤s200以及生成渐进优化路径的步骤s300后所得到的结果是一条逐渐接近最终优化路径的渐进优化路径，直到最终一次迭代后(即，当该次迭代的渐进优化路径符合最终优化路径判断标准时)，就可以得到路径搜索的最终结果，该最终结果不一定是理论情况下的最优路径，但是一定是当前算法迭代情况下距离最优路径误差最小的路径。
[0097]
需要说明的是，最终优化路径判断标准用于判断每一次迭代而得到的渐进优化路径是否是最终优化路径。其中，由于现有技术中已经存在相关的最终优化路径判断标准(例如，在栅格地图上，根据栅格的尺寸可以计算上述渐进优化路径的长度，以及与理论情况下的最优路径的误差值)，因此，上述步骤s400可以直接使用现有的最终优化路径判断标准，此处不再对如何具体使用该最终优化路径判断标准进行赘述。
[0098]
需要说明的是，本飞行巡逻机器人路径规划方法都是经过仿真验证的，也就是说，可以根据仿真结果证明每次不断重复步骤s100、s200和s300进行迭代后得到的渐进优化路径是否是最优优化路径或次优优化路径。
[0099]
可以看出，一些实施例中，与现有技术相比，本技术所提出的一种飞行巡逻机器人路径规划方法具有以下技术效果：
[0100]
1)针对现有rrt*及其改进算法的不足，本发明所提出的一种飞行巡逻机器人路径规划方法，首先利用rrt-connect强大的环境搜索能力生成初始可行路径，然后再基于初始可行路径对初始可行路径进行扩展以获得扩展区域，并在扩展区域内执行迭代启发式采样；
[0101]
2)本飞行巡逻机器人路径规划方法与rrt-connect算法相结合，提高了rrt*算法在飞行巡逻机器人室内特殊环境下的路径搜索效率；
[0102]
3)本飞行巡逻机器人路径规划方法在扩展区域内所执行的启发式采样，提高了节点利用率，加快了算法的收敛速度。
[0103]
以上就是飞行巡逻机器人路径规划方法的一些说明。本技术一些实施例中还公开了一种改进rrt*的飞行巡逻机器人路径规划系统。请参考图6，该飞行巡逻机器人路径规划系统包括：
[0104]
传感器模块100，被配置为获取飞行巡逻机器人的环境数据；
[0105]
飞控模块200，被配置为接收飞行巡逻机器人的环境数据，基于飞行巡逻机器人的环境数据计算得出飞行巡逻机器人的位置和姿态，
[0106]
路径规划模块300，被配置为飞控模块200所计算得出飞行巡逻机器人的位置和姿态，利用权利要求1至7中任一项的飞行巡逻机器人路径规划方法生成从起点到目标点的最终优化路径；
[0107]
其中，飞控模块200能够根据飞行巡逻机器人的位置和姿态以及最终优化路径对飞行巡逻机器人进行控制。
[0108]
一些实施例中，传感器模块100主要包括超声波测距雷达、激光测距雷达、光流传感器、三轴陀螺仪、三轴加速度计、三轴磁力计和气压计等。传感器模块100主要用于采集飞行巡逻机器人的环境数据，然后将采集得到的环境数据通过信息融合发送给飞控模块200，从而可以使得飞控模块200根据传感器模块100所获得的环境数据而得到飞行巡逻机器人在室内的具体位置和姿态，为后续在路径规划模块300进行室内飞行巡逻机器人的路径规划算法计算提供先验知识。此处的先验知识指的是飞行巡逻机器人在室内的具体位置和姿态。
[0109]
一些实施例中，飞控模块200接收传感器模块100所采集的环境数据，并通过数据信息融合，解算得到当前飞行巡逻机器人在室内或室外的具体位置和姿态，然后将飞行巡逻机器人的具体位置和姿态发送至路径规划模块300。
[0110]
一些实施例中，路径规划模块300根据飞控模块200所传输的飞行巡逻机器人当前的具体位置和姿态，采用本文中任意实施例的飞行巡逻机器人路径规划方法规划出飞行巡逻机器人的具体路径(即，上述最终优化路径)，并将规划出的上述最终优化路径发送至飞控模块200。之后，飞控模块200可以根据所获得的飞行巡逻机器人的具体位置和姿态，以及路径规划模块300所传输的上述最终优化路径，采用ekf算法计算飞行巡逻机器人的每个旋翼所对需的pwm波形信号，并将每个旋翼所对需的pwm波形传输至电调(即电子调速器)，之后通过电调将飞控模块200输出的pwm波形信号转化为每个旋翼所对应的电机的转速，以对飞行巡逻机器人进行控制。
[0111]
需要说明的是，飞控模块200根据飞行巡逻机器人的位置和姿态以及最终优化路径对飞行巡逻机器人进行控制的具体过程属于现有技术，故此处不再对此进行赘述。
[0112]
一些实施例中，环境数据包括：超声波测距雷达和激光测距雷达所获得飞行巡逻机器人周围的障碍物距离和角度数据、光流传感器所获得飞行巡逻机器人运动的速度数据、三轴陀螺仪所获得飞行巡逻机器人的角速度和角加速度数据、三轴加速度计所获得飞行巡逻机器人的加速度数据、三轴磁力计所获得飞行巡逻机器人的航向数据和气压计所获得飞行巡逻机器人的飞行高度数据中的多个或全部。
[0113]
需要说明的是，本领域技术人员还可以根据实际需求而对环境数据进行编辑，例如，增加新的子环境数据类别等。而环境数据以及环境数据的获得属于本领域的现有技术，故此处不再对环境数据以及环境数据的获得过程进行赘述。
[0114]
需要说明的是，飞行巡逻机器人路径规划系统的具体运行流程以及相应的技术效果可以参考前述飞行巡逻机器人路径规划方法的论述，此处不再重复论述。
[0115]
以上就是飞行巡逻机器人路径规划方法的一些说明。本技术一些实施例中还公开了一种一种计算机可读存储介质，其特征在于，包括程序，程序能够被处理器执行以实现本文中任一项实施例的方法。
[0116]
本文参照了各种示范实施例进行说明。然而，本领域的技术人员将认识到，在不脱离本文范围的情况下，可以对示范性实施例做出改变和修正。例如，各种操作步骤以及用于执行操作步骤的组件，可以根据特定的应用或考虑与系统的操作相关联的任何数量的成本函数以不同的方式实现(例如一个或多个步骤可以被删除、修改或结合到其他步骤中)。
[0117]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。另外，如本领域技术人员所理解的，本文的原理可以反映在计算机可读存储介质上的计算机程序产品中，该可读存储介质预装有计算机可读程序代码。任何有形的、非暂时性的计算机可读存储介质皆可被使用，包括磁存储设备(硬盘、软盘等)、光学存储设备(cd至rom、dvd、blu ray盘等)、闪存和/或诸如此类。这些计算机程序指令可被加载到通用计算机、专用计算机或其他可编程数据处理设备上以形成机器，使得这些在计算机上或其他可编程数据处理装置上执行的指令可以生成实现指定的功能的装置。这些计算机程序指令也可以存储在计算机可读存储器中，该计算机可读存储器可以指示计算机或其他可编程数据处理设备以特定的方式运行，这样存储在计算机可读存储器中的指令就可以形成一件制造品，包括实现指定功能的实现装置。计算机程序指令也可以加载到计算机或其他可编程数据处理设备上，从而在计算机或其他可编程设备上执行一系列操作步骤以产生一个计算机实现的进程，使得在计算机或其他可编程设备上执行的指令可以提供用于实现指定功能的步骤。
[0118]
虽然在各种实施例中已经示出了本文的原理，但是许多特别适用于特定环境和操作要求的结构、布置、比例、元件、材料和部件的修改可以在不脱离本披露的原则和范围内使用。以上修改和其他改变或修正将被包含在本文的范围之内。
[0119]
前述具体说明已参照各种实施例进行了描述。然而，本领域技术人员将认识到，可以在不脱离本披露的范围的情况下进行各种修正和改变。因此，对于本披露的考虑将是说明性的而非限制性的意义上的，并且所有这些修改都将被包含在其范围内。同样，有关于各种实施例的优点、其他优点和问题的解决方案已如上所述。然而，益处、优点、问题的解决方案以及任何能产生这些的要素，或使其变得更明确的解决方案都不应被解释为关键的、必
需的或必要的。本文中所用的术语“包括”和其任何其他变体，皆属于非排他性包含，这样包括要素列表的过程、方法、文章或设备不仅包括这些要素，还包括未明确列出的或不属于该过程、方法、系统、文章或设备的其他要素。此外，本文中所使用的术语“耦合”和其任何其他变体都是指物理连接、电连接、磁连接、光连接、通信连接、功能连接和/或任何其他连接。
[0120]
具有本领域技术的人将认识到，在不脱离本发明的基本原理的情况下，可以对上述实施例的细节进行许多改变。因此，本发明的范围应仅由权利要求确定。

技术特征：
1.一种飞行巡逻机器人路径规划方法，其特征在于，包括：生成从起点到目标点的初始可行路径；生成扩展区域的步骤：基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域；其中，所述初始可行路径位于所述扩展区域的内部；生成渐进优化路径的步骤：在所述扩展区域内执行启发式搜索，以生成一条渐进优化路径；迭代的步骤：依次重复地执行将所述渐进优化路径作为所述初始可行路径的步骤、所述生成扩展区域的步骤以及所述生成渐进优化路径的步骤，直至所述渐进优化路径符合最终优化路径判断标准，将所述符合最终优化路径判断标准的渐进优化路径作为从所述起点到目标点的最终优化路径。2.如权利要求1所述的飞行巡逻机器人路径规划方法，其特征在于，所述基于所述初始可行路径，生成与所述初始可行路径对应的扩展区域，包括：获取与所述初始可行路径中的子路径段对应的子扩展区域；基于所述初始可行路径中的子路径段，将与所述初始可行路径中的子路径段对应的子扩展区域进行拼接，以得到所述与所述初始可行路径对应的扩展区域。3.如权利要求2所述的飞行巡逻机器人路径规划方法，其特征在于，所述获取与所述初始可行路径中的子路径段对应的子扩展区域，包括：以所述初始可行路径中所述子路径段的邻接点为子路径段顶点，基于与所述子路径段顶点对应的第一子路径段向量和第二子路径段向量确定由所述子路径段顶点指向所述初始可行路径的一侧的方向向量；其中，所述初始可行路径由若干所述子路径段彼此首尾连接而成；所述第一子路径段向量和第二子路径段向量是以所述子路径段顶点为邻接点的第一子路径段和第二子路径段所分别对应的向量；基于实际膨胀距离、所述子路径段顶点和所述方向向量，确定与所述子路径段顶点对应的第一子扩展区域顶点和第二子扩展区域顶点；其中，每个所述子扩展区域对应两个所述第一子扩展区域顶点和两个所述第二子扩展区域顶点，所述第一子扩展区域顶点和第二子扩展区域顶点均作为所述子扩展区域的顶点；所述实际膨胀距离为由所述子路径段顶点沿所述方向向量延伸的距离；将由所述子扩展区域对应的两个所述第一子扩展区域顶点和两个所述第二子扩展区域顶点所围成的区域作为所述子扩展区域。4.如权利要求3所述的飞行巡逻机器人路径规划方法，其特征在于，所述与所述子路径段顶点对应的第一子扩展区域顶点所对应的向量和第二子扩展区域顶点所对应的向量的表达式分别为：的表达式分别为：其中，所述表示所述子路径段顶点所对应的向量，i表示所述子路径段顶点的编号，
所述表示所述由所述子路径段顶点指向所述初始可行路径的一侧的方向向量，所述d表示所述实际膨胀距离，所述θ
i
是所述与所述子路径段顶点对应的第一子路径段向量和第二子路径段向量之间的夹角的一半。5.如权利要求4所述的飞行巡逻机器人路径规划方法，其特征在于，所述实际膨胀距离d的表达式为：d＝k*d
base
，其中，所述k为动态扩展系数，所述d
base
为初始扩展距离；所述k的取值范围为0.75至1.25。6.如权利要求5所述的飞行巡逻机器人路径规划方法，其特征在于，所述动态扩展系数k的表达式为：其中，所述i表示当前的迭代次数；所述i
init
表示生成所述初始可行路径时所完成的迭代次数；n表示预设的最大迭代次数。7.如权利要求5所述的飞行巡逻机器人路径规划方法，其特征在于，所述初始扩展距离d
base
的表达式为：其中，所述ε表示初始扩展系数，所述l和w分别表示与所述初始可行路径对应的环境平面的长度和宽度。8.一种飞行巡逻机器人路径规划系统，其特征在于，包括：传感器模块，被配置为获取飞行巡逻机器人的环境数据；飞控模块，被配置为接收所述飞行巡逻机器人的环境数据，基于所述飞行巡逻机器人的环境数据计算得出所述飞行巡逻机器人的位置和姿态，路径规划模块，被配置为所述飞控模块所计算得出所述飞行巡逻机器人的位置和姿态，利用如权利要求1至7中任一项所述的飞行巡逻机器人路径规划方法生成从所述起点到目标点的最终优化路径；其中，所述飞控模块能够根据所述飞行巡逻机器人的位置和姿态以及所述最终优化路径对所述飞行巡逻机器人进行控制。9.如权利要求8所述的飞行巡逻机器人路径规划系统，其特征在于，所述环境数据包括：超声波测距雷达和激光测距雷达所获得飞行巡逻机器人周围的障碍物距离和角度数据、光流传感器所获得飞行巡逻机器人运动的速度数据、三轴陀螺仪所获得飞行巡逻机器人的角速度和角加速度数据、三轴加速度计所获得飞行巡逻机器人的加速度数据、三轴磁力计所获得飞行巡逻机器人的航向数据和气压计所获得飞行巡逻机器人的飞行高度数据中的多个或全部。10.一种计算机可读存储介质，其特征在于，包括程序，所述程序能够被处理器执行以实现如权利要求1至7中任一项所述的方法。

技术总结
本发明提供一种飞行巡逻机器人路径规划方法及其系统。本方法包括：生成从起点到目标点的初始可行路径；生成扩展区域的步骤：基于初始可行路径，生成与初始可行路径对应的扩展区域；生成渐进优化路径的步骤：在扩展区域内执行启发式搜索，以生成一条渐进优化路径；迭代的步骤：依次重复地执行将渐进优化路径作为初始可行路径的步骤、生成扩展区域的步骤以及生成渐进优化路径的步骤，直至渐进优化路径符合最终优化路径判断标准，将符合最终优化路径判断标准的渐进优化路径作为最终优化路径，进而提高了RRT*算法在飞行巡逻机器人室内特殊环境下的路径搜索效率；在扩展区域内所执行的启发式采样，提高了节点利用率，加快了算法的收敛速度。收敛速度。收敛速度。


技术研发人员：林必毅 林昶荣 任晓波 文新隆
受保护的技术使用者：深圳市华赛睿飞智能科技有限公司
技术研发日：2023.08.02
技术公布日：2023/10/11