CN116578121B - 基于约束采样的扩展随机树的生成方法及轨迹规划方法 - Google Patents
基于约束采样的扩展随机树的生成方法及轨迹规划方法 Download PDFInfo
- Publication number
- CN116578121B CN116578121B CN202310837145.6A CN202310837145A CN116578121B CN 116578121 B CN116578121 B CN 116578121B CN 202310837145 A CN202310837145 A CN 202310837145A CN 116578121 B CN116578121 B CN 116578121B
- Authority
- CN
- China
- Prior art keywords
- target
- random
- point
- sampling
- determining
- 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.)
- Active
Links
- 238000005070 sampling Methods 0.000 title claims abstract description 412
- 238000000034 method Methods 0.000 title claims abstract description 130
- 108010003272 Hyaluronate lyase Proteins 0.000 claims description 28
- 238000011156 evaluation Methods 0.000 claims description 26
- 238000003860 storage Methods 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 14
- 238000001514 detection method Methods 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 abstract description 22
- 230000008569 process Effects 0.000 description 31
- 238000010586 diagram Methods 0.000 description 13
- 238000007689 inspection Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000009466 transformation Effects 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000000354 decomposition reaction Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 230000007480 spreading Effects 0.000 description 2
- 238000003892 spreading Methods 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 230000004931 aggregating effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
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/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
Abstract
本申请提供了一种基于约束采样的扩展随机树的生成方法及轨迹规划方法。该方法包括：基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的；基于目标采样方式，确定第一随机采样点，且基于第一随机采样点和第二扩展因子，确定第二随机采样点，第二扩展因子是至少基于无人机的当前位置与各障碍物之间的最小的欧式距离以及多个障碍物的最大尺寸确定的；将第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树。本申请解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
Description
技术领域
本申请涉及无人机三维避障领域，具体而言，涉及一种基于约束采样的扩展随机树的生成方法及其生成装置、无人机的轨迹规划方法及其轨迹规划装置、计算机可读存储介质与电子装置。
背景技术
巡检无人机在对配电网进行设备环境类人身安全隐患时排查时，由于既定的巡检路线中会出现一些复杂障碍物，例如交叉线路、密集树丛、建筑物等等，这样导致巡检无人机的工作环境较为恶劣。因此，需要巡检无人机能够规划出一条安全且精确的避障路线，从而避免对巡检无人机的自身及配电网设备造成损坏。
RRT（快速扩展随机树，Rapidly Exploring Random Tree，简称RRT）算法是众多避障算法中的一种基于采样的随机性算法。该RRT算法无需对障碍物以及工作环境进行栅格化的结构建模，并且在极端复杂的环境下，可以寻找出一条可行的避障路径。
但是，由于RRT算法采用随机取样的方式来生成扩展随机树，这样导致采样次数较多以及所获的避障路径非最优可达路径的问题。
因此，亟需一种能够减少采样次数且可以快速生成最优的避障路径的方法。
发明内容
本申请的主要目的在于提供一种基于约束采样的扩展随机树的生成方法及其生成装置、无人机的轨迹规划方法及其轨迹规划装置、计算机可读存储介质与电子装置，以至少解决现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
为了实现上述目的，根据本申请的一个方面，提供了一种基于约束采样的扩展随机树的生成方法，包括：基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
可选地，所述生成方法还包括：采用公式，确定所述第一扩展因子，其中，为所述第一扩展因子，为第一工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量；采用公式，确定所述第二扩展因子，其中，为所述第二扩展因子，为第二工作环境评价系数，为所述无人机当前的位置与第i个所述障碍物之间的最小的欧式距离。
可选地，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，包括：在所述随机概率小于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为朝向所述第二目标点所在的方向进行采样的方式；在所述随机概率大于或者等于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为椭圆约束采样方式。
可选地，基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，包括：从当前的所述扩展随机树的多个节点中，确定与所述第一随机采样点最近的近邻点；采用公式，确定目标扩展步长，其中，为所述目标扩展步长，为所述第二扩展因子，为固定扩展步长且大于所述无人机的最小飞行距离，为所述无人机的当前位置与多个所述障碍物之间的最小的欧式距离，为预定阈值；基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点。
可选地，基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点，包括：采用公式，确定所述第二随机采样点，其中，为所述第二随机采样点，为所述近邻点，为所述第一随机采样点。
可选地，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，包括：以所述第二随机采样点为圆心，生成半径为R的目标球体，并确定处于所述目标球体内且为当前的所述扩展随机树中的节点的目标集合，其中，，为系数，为当前的所述扩展随机树中节点的总个数，所述目标集合中包括多个节点且多个所述节点均通过碰撞检测；将所述目标集合中的多个节点分别与所述第二随机采样点连接，从而得到从所述第一目标点至所述第二随机采样点所有的可达路径，以及确定多个所述可达路径的路径距离，得到各所述可达路径对应的路径距离；将最小的路径距离对应的所述可达路径中，与所述第二随机采样点直接连接的目标节点，作为所述第二随机采样点的父节点，从而得到所述目标扩展随机树，所述目标节点为所述目标集合中的一个节点。
根据本申请的另一方面，提供了一种无人机的轨迹规划方法，包括：生成步骤，采用任意一种所述的生成方法，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
可选地，所述轨迹规划方法还包括：第二重复步骤，在根据所述第一扩展随机树和所述第二扩展随机树无法得到一个所述目标可达路径的情况下，重复所述生成步骤至少一次，直到根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径为止。
可选地，所述轨迹规划方法还包括：采用，确定所述第三扩展因子，其中，为所述第三扩展因子，为第三工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量。
可选地，确定根据所述第一扩展随机树和所述第二扩展随机树，是否可得到一个所述目标可达路径的过程包括：分别对所述第一扩展随机树和所述第二扩展随机树进行遍历，得到第一目标节点和第二目标节点，所述第一目标节点为所述第一扩展随机树中的一个节点，所述第二目标节点为所述第二扩展随机树中的一个节点，且所述第一目标节点与所述第二目标节点之间的距离最小；确定所述第一目标节点和所述第二目标节点之间的欧式距离，得到目标欧式距离；在所述目标欧式距离小于预设距离阈值的情况下，确定根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径，且将所述第一目标节点和所述第二目标节点相连，从而得到一个所述目标可达路径。
可选地，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，包括：采用公式，确定所述目标可达路径对应的动态采样值，其中，为所述动态采样值，为所述第三扩展因子，为所述目标可达路径的路径距离，为所述第一目标点与所述第二目标点之间的欧式距离，为固定采样值。
根据本申请的再一方面，提供了一种基于约束采样的扩展随机树的生成装置，包括：第一确定单元，用于基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；第二确定单元，用于基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；连接单元，用于将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
根据本申请的又一方面，提供了一种无人机的轨迹规划装置，包括：生成单元，用于执行生成步骤，采用所述的生成装置，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；第三确定单元，用于执行确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；第一重复单元，用于执行第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
根据本申请的一方面，提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行任意一种所述的基于约束采样的扩展随机树的生成方法，或者任意一种所述的无人机的轨迹规划方法。
根据本申请的又一方面，提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为通过所述计算机程序执行任意一种所述的基于约束采样的扩展随机树的生成方法，或者任意一种所述的无人机的轨迹规划方法。
应用本申请的技术方案，在确定第一随机采样点时，通过将随机生成的随机概率与第一扩展因子进行比较，来确定第一随机采样点的目标采样方式。由于第一扩展因子至少与工作空间中的多个障碍物的最大尺寸相关，这样不仅可以减少采样次数，还可以保证确定的第一随机采样点较为合理。再基于第一随机采样和第二扩展因子，确定第二随机采样点。由于第二扩展因子至少与无人机的当前位置与各障碍物之间的最小的欧式距离以及多个障碍物的最大尺寸相关，这样保证了确定的第二随机采样点较为合理以及可以快速地确定出第二随机采样点。最后，将第二随机采样点与当前的扩展随机树中节点连接，保证了能够基于第二随机采样点与当前的扩展随机树，确定出最优的一条无人机由第一目标点飞行至第二目标点的至少部分避障轨迹。本申请的生成方法实现了在采样次数较少的基础上，可以快速地得到包括最优路径的目标扩展生成树，保证了无人机由第一目标点飞行至第二目标点的至少部分避障轨迹是最优的，从而解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
附图说明
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1示出了根据本申请的实施例提供的一种基于约束采样的扩展随机树的生成方法的流程示意图；
图2示出了根据本申请的实施例提供的一种重写的示意图；
图3示出了根据本申请的实施例提供的一种重连的示意图；
图4示出了根据本申请的实施例提供的一种无人机的轨迹规划方法的流程示意图；
图5示出了根据本申请的实施例提供的一种第一扩展随机树和第二扩展随机树连接的示意图；
图6示出了根据本申请的实施例提供的一种基于约束采样的RRT算法的巡检无人机避障轨迹规划方法的流程示意图；
图7示出了根据本申请的实施例提供的一种基于约束采样的扩展随机树的生成装置的结构示意图；
图8示出了根据本申请的实施例提供的一种无人机的轨迹规划装置的结构示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
正如背景技术中所介绍的，现有技术中的RRT算法的采样次数较多且生成的避障路径非最优。为解决所述的技术问题，本申请的实施例提供了一种基于约束采样的扩展随机树的生成方法及其生成装置、无人机的轨迹规划方法及其轨迹规划装置、计算机可读存储介质与电子装置。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
在实际的应用过程中，将无人机的运动规划工作空间表示为且，为多维实数空间。障碍物所在的空间可以称作为障碍物空间，且表示为且。在整个工作空间内，除了障碍物空间后，其余为自由空间，其可以表示为，。在自由空间中存在第一目标点（起始点）和第二目标点（终点），可以分别定义为以及。本申请所提及的运动规划问题是在中确定一条最优的从到的路径。
另外，在本申请中，无人机还可以包括双目摄像机、超声波传感器以及GPS（全球定位系统，Global Positioning System）设备。通过将双目摄像机及超声波传感器可以获取工作空间中的无人机与障碍物空间中各障碍物之间的距离信息。通过GPS设备可以获取到无人机当前的位置信息。再通过对距离信息以及位置信息进行坐标转换，从而可以得到障碍物空间中的各障碍物的位置信息及各障碍物的轮廓信息。在得到各障碍物的轮廓信息，可以通过确定各障碍物的相距最远的两个点之间的距离，得到对应的障碍物的最大尺寸。或者可以通过确定包络各障碍物的多边形的最大长度，得到对应的障碍物的最大尺寸。
图1是根据本申请实施例的基于约束采样的扩展随机树的生成方法的流程图。如图1所示，该生成方法包括以下步骤：
步骤S101，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
对于RRT算法来说，其通常采用随机取样的方式来生成扩展随机树。也就是说，RRT算法会在自由空间中进行随机的采样。但是，这种盲目的在自由空间中随机采样的方式，会导致生成扩展随机树的工作中，采样次数较多，且最后得到的可达路径也非最优的问题。因此，本申请中，在确定根节点之后或者在当前的扩展随机树的基础上，在生成第一随机采样点时，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，这样可以避免盲目的在自由空间中进行采样，保证了本申请的生成方法的采样次数较少。
步骤S102，基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
步骤S103，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
在本申请中，在生成一个新节点（例如，第一随机采样点，第二随机采样点）后，需要该新节点进行“身份认证”。具体包括：新节点的三维坐标信息；新节点所属的生长树或；新节点的父节点；它的路径距离。其中，新节点与父节点的距离定义为两点间的欧式距离，新节点的定义为从生长树的根节点经过各中间节点到达新节点的距离，=+。
以一个扩展随机树为例，假设此时扩展随机树包括起点共三个节点、以及。为的父节点，为的父节点。定义初始节点的路径距离为零，即，则，。
所述的基于约束采样的扩展随机树的生成方法中，在确定第一随机采样点时，通过将随机生成的随机概率与第一扩展因子进行比较，来确定第一随机采样点的目标采样方式。由于第一扩展因子至少与工作空间中的多个障碍物的最大尺寸相关，这样不仅可以减少采样次数，还可以保证确定的第一随机采样点较为合理。再基于第一随机采样和第二扩展因子，确定第二随机采样点。由于第二扩展因子至少与无人机的当前位置与各障碍物之间的最小的欧式距离以及多个障碍物的最大尺寸相关，这样保证了确定的第二随机采样点较为合理以及可以快速地确定出第二随机采样点。最后，将第二随机采样点与当前的扩展随机树中节点连接，保证了能够基于第二随机采样点与当前的扩展随机树，确定出最优的一条无人机由第一目标点飞行至第二目标点的至少部分避障轨迹。本申请的生成方法实现了在采样次数较少的基础上，可以快速地得到包括最优路径的目标扩展生成树，保证了无人机由第一目标点飞行至第二目标点的至少部分避障轨迹是最优的，从而解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
具体实现过程中，本申请的生成方法还可以步骤S104：采用公式，确定所述第一扩展因子，其中，为所述第一扩展因子，为第一工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量；采用公式，确定所述第二扩展因子，其中，为所述第二扩展因子，为第二工作环境评价系数，为所述无人机当前的位置与第i个所述障碍物之间的最小的欧式距离。在该实施例中，通过考虑障碍物空间中各个障碍物与无人机的最小的欧式距离和/或各障碍物的最大半径（即各障碍物的最大尺寸），来对工作空间中的障碍物进行准确与高效的评估，得到第一扩展因子和第二扩展因子，这样进一步地保证了较为快速地得到目标扩展随机树，以及进一步地保证了得到的目标扩展随机树中包括最优的可达路径。
具体地，第一工作环境评价系数以及第二工作环境评价系数，均为根据不同的障碍物空间的具体情况进行评估得到的系数。在本申请中，并不对第一工作环境评价系数以及第二工作环境评价系数的大小进行限制，均可以根据实际的障碍物空间的具体情形进行灵活的调整。
为了进一步地保证后续可以较为快速地得到的第一随机采样点，以及进一步地保证得到的第一随机采样点较为合理，本申请的所述步骤S101还可以通过步骤S1011以及步骤S1012来实现：
步骤S1011：在所述随机概率小于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为朝向所述第二目标点所在的方向进行采样的方式；
步骤S1012：在所述随机概率大于或者等于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为椭圆约束采样方式。
对于所述的实施例，引入随机概率。通过比较随机概率与第一扩展安全因子的大小关系，来确定第一随机采样点的目标采样方式。在<时，，即朝向第二目标点所在的方向生成第一随机采样点。当≥时，通过椭圆约束采样方式来得到第一随机采样点，即。需要说明的是生成的第一随机采样点为工作空间中具有三维坐标的采样点。且第二目标点为当前的扩展随机树的非根节点所在的点。也就是说，在当前的扩展随机树的根节点为的情况下，第二目标点为。当然，在当前的扩展随机树的根节点为的情况下，第二目标点为。
具体地，当工作区间障碍物的总数量较少时，即第一扩展因子较大，工作空间中的自由空间较大，第一随机采样点受限约束较小，可以将目标扩展随机树的生长方向朝着第二目标点所在的方向进行扩展；当工作区间中的障碍物的总数量时，即第一扩展因子较小，无障碍的空间较小，第一随机采样点受限约束较大。若是这种情况下，继续朝着第二目标点所在的方向进行扩展容易与障碍物发生碰撞，导致扩展失败。若是考虑用椭圆约束采样方式，这样避免随机采样导致的搜索效率减慢的问题。
椭圆约束采样方式具体是将椭球的两个焦点分别设置为无人机避障轨迹的起始点和终点。起始点和终点之间的最短距离为欧式距离=。定义搜索路径为椭球的长轴的最优路径距离为，搜索路径为椭球的短轴的最优路径距离为为。在之后的迭代中，每找到一次更短的路径，就用这条更短路径的长度作为新的，更新采样椭球子集采样区域。
椭圆约束采样方式的具体步骤包括：
步骤1：在单位球体内进行均匀随机采样，生成采样点，。
步骤2：将样本采样集合由单位球子集变换到椭球子集，生成采样点，。其具体变换过程为：，式中，为单位球区域向椭球区域线性变换的变换矩阵且通过将椭球矩阵进行Cholesky分解得到。
步骤3：将第2步得到的椭球体转换到世界坐标系，获得，式中，F为旋转矩阵，计算方式为：，其中，表示奇异值分解，在三维条件下。
在一些实施例上，所述步骤S102具体可以通过步骤S1021、步骤S1022以及步骤S1023来实现：
步骤S1021：从当前的所述扩展随机树的多个节点中，确定与所述第一随机采样点最近的近邻点；也就是说，与第一随机采样点最近的近邻点为当前的扩展随机树中的一个节点。
步骤S1022：采用公式，确定目标扩展步长，其中，为所述目标扩展步长，为所述第二扩展因子，为固定扩展步长且大于所述无人机的最小飞行距离，为所述无人机的当前位置与多个所述障碍物之间的最小的欧式距离，为预定阈值；
步骤S1023：基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点。
在所述的实施例中，基于第二扩展因子，确定目标扩展步长，这样保证确定的目标扩展步长较为合理，进一步地保证后续确定的第二随机采样点较为合理，进一步地避免与障碍物发生碰撞。
具体地，，其中，为无人机的当前位置与第i个障碍物之间的最小的欧式距离，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量。另外，在本申请中，并不对的具体取值进行限制，其可以根据不同的障碍物场景进行灵活的设置。
在所述的实施例中，所述最小飞行距离为无人机需要至少飞行一定的距离才能实现预定的飞行动作。
另外，在本申请中在确定目标扩展步长时，还可以引入最大转向角这一因素，进一步地保证确定的目标扩展步长较为合理。由于无人机无法在飞行时实现的大角度转向，因此规划算法在采样时需考虑到采样路径的最大转向角。在进行碰撞检测中，除需考虑对无人机的路径位于无障碍空间内，同时需满足在进行新节点的扩展时，保证路径的转向角需满足。
为了较为简单地确定出第二随机采样点，所述步骤S1023还可以通过如下步骤来实现：采用公式，确定所述第二随机采样点，其中，为所述第二随机采样点，为所述近邻点，为所述第一随机采样点。
具体地，在生成第二随机采样点之后，定义该第二随机采样点的父节点为。且该第二随机采样点的路径距离为父节点的路径距离与第二随机采样点与父节点之间的欧式距离：。
在实际的过程中，为了进一步不断地实现最优路径的规划，以及保证目标扩展随机树的冗余通路较少以及路径距离较小。所述步骤S103还可以通过步骤S1031、步骤S1032以及步骤S1033来实现：
步骤S1031：以所述第二随机采样点为圆心，生成半径为R的目标球体，并确定处于所述目标球体内且为当前的所述扩展随机树中的节点的目标集合，其中，，为系数，为当前的所述扩展随机树中节点的总个数，所述目标集合中包括多个节点且多个所述节点均通过碰撞检测；
步骤S1032：将所述目标集合中的多个节点分别与所述第二随机采样点连接，从而得到从所述第一目标点至所述第二随机采样点所有的可达路径，以及确定多个所述可达路径的路径距离，得到各所述可达路径对应的路径距离；
步骤S1033：将最小的路径距离对应的所述可达路径中，与所述第二随机采样点直接连接的目标节点，作为所述第二随机采样点的父节点，从而得到所述目标扩展随机树，所述目标节点为所述目标集合中的一个节点。
具体地，以第二随机采样点生成一个以R为半径的目标球体B；然后，搜索当前的扩展随机树中被包含于目标球体B的所有节点，并将通过碰撞检测的多个节点生成目标集合。对第二随机采样点进行重写，即将目标集合中的节点均假设为第二随机采样点的父节点，分别重新计算从到的路径距离，并将新的距离信息存储至列表。找到列表中具有最小值的那个节点，定义为的父节点。
在实际的应用过程中，由于在得到第二随机采样点之后，将第二随机采样点的父节点设置为。故在将第二随机采样点与当前的扩展随机树连接的过程可以包括重写和重连两个过程。
如图2所示，为重写的过程：通过目标球体B筛选出来的目标集合，其中，为当前的扩展随机树中的节点。将目标集合内的每个节点假设为第二随机采样点的父节点。如图2中的与第二随机采样点之间的虚线所示。显然的，将作为第二随机采样点的父节点时，到的路径距离最小。为图2中标识的虚线带箭头线条所示。因此，将替代作为的父节点。
重连是将第二随机采样点假设为目标集合中各节点的父节点，分别重新计算从到目标集合中的各节点的路径距离。当新生成的路径距离小于与原路径的路径距离，则将第二随机采样点作为该节点的父节点。本申请的另一种具体的实施例中，如图3所示，以为例，将第二随机采样点替代，即通过第二随机采样点作为的父节点，重新生成的路径距离如图3中虚线带箭头线条所示，原始的路径距离如实线带箭头线条。显然的，虚线带箭头线条的路径距离小于实线带箭头线条的路径距离，故可以将的父节点定义为，从而得到目标扩展随机树。也就是说，重写与重连两个过程相辅相成，重新选择第二随机采样点的父节点，使目标扩展随机中新生成的节点路径代价尽可能小，重新布线使得生成第二随机采样点后的目标扩展随机冗余通路减少以及路径代价减小。
本申请的一种典型的实施例中，还提供了一种无人机的轨迹规划方法，如图4所示，该方法包括：
步骤S401：生成步骤，采用任意一种所述的生成方法，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；
本申请的一种具体的实施例中，如图5所示，可以在第一目标点（起始点）与第二目标点（终点）同时生成第一扩展随机树和第二扩展随机树。也就是说，将无人机避障轨迹的起始点和终点，分别作为第一扩展随机树和第二扩展随机树的起始点和终点，即是将无人机轨迹的起始点作为起点，无人机轨迹的终点作为终点；将无人机轨迹的终点作为起点，将起始点作为终点。也就是说，同时从无人机轨迹的起始位置和目标位置出发进行扩散，这样可以减少空白区域的无用搜索，大大提高空间内的搜索速率。同时为避免重复，第一扩展随机树和第二扩展随机树交替进行扩展。
步骤S402：确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；
具体地，如图6所示，在第一扩展随机树和第二扩展随机树可得到一个目标可达路径的情况下，至少基于目标可达路径的路径距离和第三扩展因子，确定目标可达路径对应的动态采样值。
步骤S403：第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
所述的无人机的轨迹规划方法，通过所述的基于约束采样的扩展随机树的生成方法，交替生成对应的节点，从而得到第一扩展随机树和第二扩展随机树，这样实现了较为快速地得到第一扩展随机树和第二扩展随机树；在通过第一扩展随机树和第二扩展随机树可得到一个目标可达路径的情况下，至少基于目标可达路径的路径距离和第三扩展因子，确定目标可达路径对应的动态采样值；在本次确定的目标可达路径对应的动态采样值未达到目标动态采样值的情况下，继续重复执行所述过程，直到得到一条最优的无人机由第一目标点飞行至第二目标点的避障轨迹。由于第三扩展因子与障碍物空间中的障碍物相关，这样保证了确定的动态采样值较为合理和准确，保证了可以得到最优的由第一目标点飞行至第二目标点的避障轨迹，从而解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
在一些具体的实时过程中，本申请的轨迹规划方法还包括步骤S404：第二重复步骤，在根据所述第一扩展随机树和所述第二扩展随机树无法得到一个所述目标可达路径的情况下，重复所述生成步骤至少一次，直到根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径为止。
在一些实现过程中，本申请的轨迹规划方法还包括步骤S405：采用，确定所述第三扩展因子，其中，为所述第三扩展因子，为第三工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量，这样进一步地保证了基于第三扩展因子，确定的目标可达路径的动态采样值较为准确，以及进一步地可以较为搜索出一条最优的目标可达路径。
具体地，第三工作环境评价系数，为根据不同的障碍物空间的具体情况进行评估得到的系数。在本申请中，并不对第三工作环境评价系数的大小进行限制，可以根据实际的障碍物空间的具体情形进行灵活的调整。
为了较为简单地确定出通过第一扩展随机树和第二扩展随机树是否可得到一个目标可达路径，本申请的一个实施例中，确定根据所述第一扩展随机树和所述第二扩展随机树，是否可得到一个所述目标可达路径的过程包括：分别对所述第一扩展随机树和所述第二扩展随机树进行遍历，得到第一目标节点和第二目标节点，所述第一目标节点为所述第一扩展随机树中的一个节点，所述第二目标节点为所述第二扩展随机树中的一个节点，且所述第一目标节点与所述第二目标节点之间的距离最小；确定所述第一目标节点和所述第二目标节点之间的欧式距离，得到目标欧式距离；在所述目标欧式距离小于预设距离阈值的情况下，确定根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径，且将所述第一目标节点和所述第二目标节点相连，从而得到一个所述目标可达路径。
本申请的一种具体的实施例中，遍历第一扩展随机树和第二扩展随机树最近的第一目标节点和第二目标节点；计算第一目标节点和第二目标节点之间的欧式距离，得到目标欧式距离。当目标欧式距离小于预设距离阈值时，将第一目标节点和第二目标节点连通，从而形成一条由至的完整的避障轨迹。
在实际的应用过程中，所述步骤S402还可以通过如下步骤来实现：采用公式，确定所述目标可达路径对应的动态采样值，其中，为所述动态采样值，为所述第三扩展因子，为所述目标可达路径的路径距离，为所述第一目标点与所述第二目标点之间的欧式距离，为固定采样值，这样进一步保证了能够较为准确地对目标可达路径进行评估，进一步地保证了可以得到最优的目标可达路径。
为了使得本领域技术人员能够更加清楚地了解本申请的技术方案，以下将结合具体的实施例对本申请的基于约束采样的RRT算法的巡检无人机避障轨迹规划方法的实现过程进行详细说明。
本实施例涉及一种具体的基于约束采样的RRT算法的巡检无人机避障轨迹规划方法，如图6所示，包括如下步骤：
步骤S1：将无人机避障轨迹的起始点和终点，分别作为第一扩展随机树和第二扩展随机树的起始点和终点，即是将无人机轨迹的起始点作为起点，无人机轨迹的终点作为终点；将无人机轨迹的终点作为起点，将起始点作为终点。也就是说，同时从无人机轨迹的起始位置和目标位置出发进行扩散，这样可以减少空白区域的无用搜索，大大提高空间内的搜索速率。同时为避免重复，第一扩展随机树和第二扩展随机树交替进行扩展。下面以第一扩展随机树扩展第二随机采样点进行详述。
步骤S2：为了避免随机树盲目生成采样点的问题，通过比较随机概率与第一扩展安全因子的大小关系，来确定第一随机采样点的目标采样方式。在<时，，即朝向第二目标点所在的方向生成第一随机采样点。当≥时，通过椭圆约束采样方式来得到第一随机采样点，即。
步骤S3：为避免后续生成的第二随机采样点遇到障碍物，导致扩展失败。采用如下公式确定目标扩展步长：。
步骤S4：在当前树进行搜索，确定与第一随机采样点最近的近邻点。再基于，确定第二随机采样点，并将确定为第二随机采样点的父节点。
步骤S5：将第二随机采样点与当前的随机扩展树进行连接。连接的过程包括重写和重连。
重写：以第二随机采样点生成一个以R为半径的目标球体B；然后，搜索当前的扩展随机树中被包含于目标球体B的所有节点，并将通过碰撞检测的多个节点生成目标集合。对第二随机采样点进行重写，即将目标集合中的节点均假设为第二随机采样点的父节点，分别重新计算从到的路径距离，并将新的距离信息存储至列表。找到列表中具有最小值的那个节点，定义为的父节点。
重连：重连是将第二随机采样点假设为目标集合中各节点的父节点，分别重新计算从到目标集合中的各节点的路径距离。当新生成的路径距离小于与原路径的路径距离，则将第二随机采样点作为该节点的父节点。
步骤S6：采用所述步骤S2至步骤S5，生成第一扩展随机树和第二扩展随机树。
步骤S7：遍历第一扩展随机树和第二扩展随机树最近的第一目标节点和第二目标节点；计算第一目标节点和第二目标节点之间的欧式距离，得到目标欧式距离。
当目标欧式距离小于预设距离阈值时，将第一目标节点和第二目标节点连接，从而形成一条由至的完整的避障轨迹。例如，图5所示的 。
当目标欧式距离大于或者等于预设距离阈值时，确定第一扩展随机树和第二扩展随机树无法连接。即根据第一扩展随机树和第二扩展随机树，无法得到一个目标可达路径。
步骤S8：在第一扩展随机树和第二扩展随机树可以连接的情况下，采用公式，确定所述目标可达路径对应的动态采样值。在目标可达路径的动态采样值未达到目标动态采样值的情况下，依次重复所述步骤S2至所述步骤S7，直到目标可达路径的动态采样值未达到目标动态采样值为止，生成最优的避障轨迹。
本申请实施例还提供了一种基于约束采样的扩展随机树的生成装置，需要说明的是，本申请实施例的基于约束采样的扩展随机树的生成装置可以用于执行本申请实施例所提供的用于基于约束采样的扩展随机树的生成方法。该装置用于实现所述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
以下对本申请实施例提供的基于约束采样的扩展随机树的生成装置进行介绍。
图7是根据本申请实施例的基于约束采样的扩展随机树的生成装置的示意图。如图7所示，该生成装置包括：
第一确定单元10，用于基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
对于RRT算法来说，其通常采用随机取样的方式来生成扩展随机树。也就是说，RRT算法会在自由空间中进行随机的采样。但是，这种盲目的在自由空间中随机采样的方式，会导致生成扩展随机树的工作中，采样次数较多，且最后得到的可达路径也非最优的问题。因此，本申请中，在确定根节点之后或者在当前的扩展随机树的基础上，在生成第一随机采样点时，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，这样可以避免盲目的在自由空间中进行采样，保证了本申请的生成方法的采样次数较少。
第二确定单元20，用于基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
连接单元30，用于将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
在本申请中，在生成一个新节点（例如，第一随机采样点，第二随机采样点）后，需要该新节点进行“身份认证”。具体包括：新节点的三维坐标信息；新节点所属的生长树或；新节点的父节点；它的路径距离。其中，新节点与父节点的距离定义为两点间的欧式距离，新节点的定义为从生长树的根节点经过各中间节点到达新节点的距离，=+。
以一个扩展随机树为例，假设此时扩展随机树包括起点共三个节点、以及。为的父节点，为的父节点。定义初始节点的路径距离为零，即，则，。
所述的基于约束采样的扩展随机树的生成装置中，第一确定单元用于在确定第一随机采样点时，通过将随机生成的随机概率与第一扩展因子进行比较，来确定第一随机采样点的目标采样方式。由于第一扩展因子至少与工作空间中的多个障碍物的最大尺寸相关，这样不仅可以减少采样次数，还可以保证确定的第一随机采样点较为合理。第二确定单元用于基于第一随机采样和第二扩展因子，确定第二随机采样点。由于第二扩展因子至少与无人机的当前位置与各障碍物之间的最小的欧式距离以及多个障碍物的最大尺寸相关，这样保证了确定的第二随机采样点较为合理以及可以快速地确定出第二随机采样点。连接单元，用于将第二随机采样点与当前的扩展随机树中节点连接，保证了能够基于第二随机采样点与当前的扩展随机树，确定出最优的一条无人机由第一目标点飞行至第二目标点的至少部分避障轨迹。本申请的生成装置实现了在采样次数较少的基础上，可以快速地得到包括最优路径的目标扩展生成树，保证了无人机由第一目标点飞行至第二目标点的至少部分避障轨迹是最优的，从而解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
具体实现过程中，本申请的生成装置还可以包括第四确定单元和第五确定单元，其中，所述第四确定单元用于采用公式，确定所述第一扩展因子，其中，为所述第一扩展因子，为第一工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量；所述第五确定单元用于采用公式，确定所述第二扩展因子，其中，为所述第二扩展因子，为第二工作环境评价系数，为所述无人机当前的位置与第i个所述障碍物之间的最小的欧式距离。在该实施例中，通过考虑障碍物空间中各个障碍物与无人机的最小的欧式距离和/或各障碍物的最大半径（即各障碍物的最大尺寸），来对工作空间中的障碍物进行准确与高效的评估，得到第一扩展因子和第二扩展因子，这样进一步地保证了较为快速地得到目标扩展随机树，以及进一步地保证了得到的目标扩展随机树中包括最优的可达路径。
具体地，第一工作环境评价系数以及第二工作环境评价系数，均为根据不同的障碍物空间的具体情况进行评估得到的系数。在本申请中，并不对第一工作环境评价系数以及第二工作环境评价系数的大小进行限制，均可以根据实际的障碍物空间的具体情形进行灵活的调整。
为了进一步地保证后续可以较为快速地得到的第一随机采样点，以及进一步地保证得到的第一随机采样点较为合理，本申请的所述第一确定单元包括第一确定模块和第二确定模块，其中，所述第一确定模块用于在所述随机概率小于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为朝向所述第二目标点所在的方向进行采样的方式；所述第二确定模块用于在所述随机概率大于或者等于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为椭圆约束采样方式。
对于所述的实施例，引入随机概率。通过比较随机概率与第一扩展安全因子的大小关系，来确定第一随机采样点的目标采样方式。在<时，，即朝向第二目标点所在的方向生成第一随机采样点。当≥时，通过椭圆约束采样方式来得到第一随机采样点，即。需要说明的是生成的第一随机采样点为工作空间中具有三维坐标的采样点。且第二目标点为当前的扩展随机树的非根节点所在的点。也就是说，在当前的扩展随机树的根节点为的情况下，第二目标点为。当然，在当前的扩展随机树的根节点为的情况下，第二目标点为。
具体地，当工作区间障碍物的总数量较少时，即第一扩展因子较大，工作空间中的自由空间较大，第一随机采样点受限约束较小，可以将目标扩展随机树的生长方向朝着第二目标点所在的方向进行扩展；当工作区间中的障碍物的总数量时，即第一扩展因子较小，无障碍的空间较小，第一随机采样点受限约束较大。若是这种情况下，继续朝着第二目标点所在的方向进行扩展容易与障碍物发生碰撞，导致扩展失败。若是考虑用椭圆约束采样方式，这样避免随机采样导致的搜索效率减慢的问题。
椭圆约束采样方式具体是将椭球的两个焦点分别设置为无人机避障轨迹的起始点和终点。起始点和终点之间的最短距离为欧式距离。定义搜索路径为椭球的长轴的最优路径距离为，搜索路径为椭球的短轴的最优路径距离为为。在之后的迭代中，每找到一次更短的路径，就用这条更短路径的长度作为新的，更新采样椭球子集采样区域。
椭圆约束采样方式的具体步骤包括：
步骤1：在单位球体内进行均匀随机采样，生成采样点，。
步骤2：将样本采样集合由单位球子集变换到椭球子集，生成采样点，。其具体变换过程为：，式中，为单位球区域向椭球区域线性变换的变换矩阵且通过将椭球矩阵进行Cholesky分解得到。
步骤3：将第2步得到的椭球体转换到世界坐标系，获得。式中，F为旋转矩阵，计算方式为：，，，，其中，表示奇异值分解，在三维条件下。
在一些实施例上，所述第二确定单元包括第三确定模块、第四确定模块和第五确定模块，其中，所述第三确定模块用于从当前的所述扩展随机树的多个节点中，确定与所述第一随机采样点最近的近邻点；也就是说，与第一随机采样点最近的近邻点为当前的扩展随机树中的一个节点。所述第四确定模块用于采用公式，确定目标扩展步长，其中，为所述目标扩展步长，为所述第二扩展因子，为固定扩展步长且大于所述无人机的最小飞行距离，为所述无人机的当前位置与多个所述障碍物之间的最小的欧式距离，为预定阈值；所述第五确定模块：基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点。
在所述的实施例中，基于第二扩展因子，确定目标扩展步长，这样保证确定的目标扩展步长较为合理，进一步地保证后续确定的第二随机采样点较为合理，进一步地避免与障碍物发生碰撞。
具体地，，其中，为无人机的当前位置与第i个障碍物之间的最小的欧式距离，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量。另外，在本申请中，并不对的具体取值进行限制，其可以根据不同的障碍物场景进行灵活的设置。
在所述的实施例中，所述最小飞行距离为无人机需要至少飞行一定的距离才能实现预定的飞行动作。
另外，在本申请中在确定目标扩展步长时，还可以引入最大转向角这一因素，进一步地保证确定的目标扩展步长较为合理。由于无人机无法在飞行时实现的大角度转向，因此规划算法在采样时需考虑到采样路径的最大转向角。在进行碰撞检测中，除需考虑对无人机的路径位于无障碍空间内，同时需满足在进行新节点的扩展时，保证路径的转向角需满足。
为了较为简单地确定出第二随机采样点，所述第五确定模块包括第一确定子模块，用于采用公式，确定所述第二随机采样点，其中，为所述第二随机采样点，为所述近邻点，为所述第一随机采样点。
具体地，在生成第二随机采样点之后，定义该第二随机采样点的父节点为。且该第二随机采样点的路径距离为父节点的路径距离与第二随机采样点与父节点之间的欧式距离：。
在实际的过程中，为了进一步不断地实现最优路径的规划，以及保证目标扩展随机树的冗余通路较少以及路径距离较小。所述连接单元包括第一生成模块、第一连接模块和第二连接模块。其中，所述第一生成模块用于以所述第二随机采样点为圆心，生成半径为R的目标球体，并确定处于所述目标球体内且为当前的所述扩展随机树中的节点的目标集合，其中，，为系数，为当前的所述扩展随机树中节点的总个数，所述目标集合中包括多个节点且多个所述节点均通过碰撞检测；所述第一连接模块用于将所述目标集合中的多个节点分别与所述第二随机采样点连接，从而得到从所述第一目标点至所述第二随机采样点所有的可达路径，以及确定多个所述可达路径的路径距离，得到各所述可达路径对应的路径距离；所述第二连接模块用于将最小的路径距离对应的所述可达路径中，与所述第二随机采样点直接连接的目标节点，作为所述第二随机采样点的父节点，从而得到所述目标扩展随机树，所述目标节点为所述目标集合中的一个节点。
具体地，以第二随机采样点生成一个以R为半径的目标球体B；然后，搜索当前的扩展随机树中被包含于目标球体B的所有节点，并将通过碰撞检测的多个节点生成目标集合。对第二随机采样点进行重写，即将目标集合中的节点均假设为第二随机采样点的父节点，分别重新计算从到的路径距离，并将新的距离信息存储至列表。找到列表中具有最小值的那个节点，定义为的父节点。
在实际的应用过程中，由于在得到第二随机采样点之后，将第二随机采样点的父节点设置为。故在将第二随机采样点与当前的扩展随机树连接的过程可以包括重写和重连两个过程。
如图2所示，为重写的过程：通过目标球体B筛选出来的目标集合，其中，为当前的扩展随机树中的节点。将目标集合内的每个节点假设为第二随机采样点的父节点。如图2中的与第二随机采样点之间的虚线所示。显然的，将作为第二随机采样点的父节点时，到的路径距离最小。为图2中标识的虚线带箭头线条所示。因此，将替代作为的父节点。
重连是将第二随机采样点假设为目标集合中各节点的父节点，分别重新计算从到到目标集合中的各节点的路径距离。当新生成的路径距离小于与原路径的路径距离，则将第二随机采样点作为该节点的父节点。本申请的另一种具体的实施例中，如图3所示，以为例，将第二随机采样点替代，即通过第二随机采样点作为的父节点，重新生成的路径距离如图3中虚线带箭头线条所示，原始的路径距离如实线带箭头线条。显然的，虚线带箭头线条的路径距离小于实线带箭头线条的路径距离，故可以将的父节点定义为，从而得到目标扩展随机树。也就是说，重写与重连两个过程相辅相成，重新选择第二随机采样点的父节点，使目标扩展随机中新生成的节点路径代价尽可能小，重新布线使得生成第二随机采样点后的目标扩展随机冗余通路减少以及路径代价减小。
本申请的一种典型的实施例中，如图8所示，还提供了一种无人机的轨迹规划装置，包括：
生成单元40，用于执行生成步骤，采用所述的生成装置，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；
本申请的一种具体的实施例中，如图5所示，可以在第一目标点（起始点）与第二目标点（终点）同时生成第一扩展随机树和第二扩展随机树。也就是说，将无人机避障轨迹的起始点和终点，分别作为第一扩展随机树和第二扩展随机树的起始点和终点，即是将无人机轨迹的起始点作为起点，无人机轨迹的终点作为终点；将无人机轨迹的终点作为起点，将起始点作为终点。也就是说，同时从无人机轨迹的起始位置和目标位置出发进行扩散，这样可以减少空白区域的无用搜索，大大提高空间内的搜索速率。同时为避免重复，第一扩展随机树和第二扩展随机树交替进行扩展。
第三确定单元50，用于执行确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；
具体地，如图6所示，在第一扩展随机树和第二扩展随机树可得到一个目标可达路径 的情况下，至少基于目标可达路径的路径距离和第三扩展因子，确定目标可达路径对应的动态采样值。
第一重复单元60，用于执行第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
所述的无人机的轨迹规划装置，生成单元用于通过所述的基于约束采样的扩展随机树的生成方法，交替生成对应的节点，从而得到第一扩展随机树和第二扩展随机树，这样实现了较为快速地得到第一扩展随机树和第二扩展随机树；第三确定单元，用于在通过第一扩展随机树和第二扩展随机树可得到一个目标可达路径的情况下，至少基于目标可达路径的路径距离和第三扩展因子，确定目标可达路径对应的动态采样值；第一重复单元用于在本次确定的目标可达路径对应的动态采样值未达到目标动态采样值的情况下，继续重复执行所述过程，直到得到一条最优的无人机由第一目标点飞行至第二目标点的避障轨迹。由于第三扩展因子与障碍物空间中的障碍物相关，这样保证了确定的动态采样值较为合理和准确，保证了可以得到最优的由第一目标点飞行至第二目标点的避障轨迹，从而解决了现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
在一些具体的实时过程中，本申请的轨迹规划装置还包括第二重复单元，用于执行第二重复步骤，在根据所述第一扩展随机树和所述第二扩展随机树无法得到一个所述目标可达路径的情况下，重复所述生成步骤至少一次，直到根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径为止。
在一些实现过程中，本申请的轨迹规划装置还包括第六确定单元，用于采用，确定所述第三扩展因子，其中，为所述第三扩展因子，为第三工作环境评价系数，为第i个所述障碍物的所述最大尺寸，i的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量，这样进一步地保证了基于第三扩展因子，确定的目标可达路径的动态采样值较为准确，以及进一步地可以较为搜索出一条最优的目标可达路径。
具体地，第三工作环境评价系数，为根据不同的障碍物空间的具体情况进行评估得到的系数。在本申请中，并不对第三工作环境评价系数的大小进行限制，可以根据实际的障碍物空间的具体情形进行灵活的调整。
为了较为简单地确定出通过第一扩展随机树和第二扩展随机树是否可得到一个目标可达路径，本申请的一个实施例中，所述第三确定单元包括遍历模块、第六确定模块和第七确定模块，其中，所述遍历模块用于分别对所述第一扩展随机树和所述第二扩展随机树进行遍历，得到第一目标节点和第二目标节点，所述第一目标节点为所述第一扩展随机树中的一个节点，所述第二目标节点为所述第二扩展随机树中的一个节点，且所述第一目标节点与所述第二目标节点之间的距离最小；所述第六确定模块用于确定所述第一目标节点和所述第二目标节点之间的欧式距离，得到目标欧式距离；所述第七确定模块用于在所述目标欧式距离小于预设距离阈值的情况下，确定根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径，且将所述第一目标节点和所述第二目标节点相连，从而得到一个所述目标可达路径。
本申请的一种具体的实施例中，遍历第一扩展随机树和第二扩展随机树最近的第一目标节点和第二目标节点；计算第一目标节点和第二目标节点之间的欧式距离，得到目标欧式距离。当目标欧式距离小于预设距离阈值时，将第一目标节点和第二目标节点连通，从而形成一条由至的完整的避障轨迹。
在实际的应用过程中，所述第三确定单元还包括第八确定模块，用于采用公式，确定所述目标可达路径对应的动态采样值，其中，为所述动态采样值，为所述第三扩展因子，为所述目标可达路径的路径距离，为所述第一目标点与所述第二目标点之间的欧式距离，为固定采样值，这样进一步保证了能够较为准确地对目标可达路径进行评估，进一步地保证了可以得到最优的目标可达路径。
所述基于约束采样的扩展随机树的生成装置包括处理器和存储器，所述第一确定单元、第二确定单元和连接单元等均作为程序单元存储在存储器中，由处理器执行存储在存储器中的所述程序单元来实现相应的功能。所述模块均位于同一处理器中；或者，所述各个模块以任意组合的形式分别位于不同的处理器中。
处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来解决现有技术中的RRT算法的采样次数较多且生成的避障路径非最优的问题。
存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)，存储器包括至少一个存储芯片。
本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行所述基于约束采样的扩展随机树的生成方法。
具体地，基于约束采样的扩展随机树的生成方法包括：
步骤S101，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
步骤S102，基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
步骤S103，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
本发明实施例提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为通过所述计算机程序执行所述的基于约束采样的扩展随机树的生成方法。
具体地，基于约束采样的扩展随机树的生成方法包括：
步骤S101，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
步骤S102，基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
步骤S103，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
本发明实施例提供了一种设备，设备包括处理器、存储器及存储在存储器上并可在处理器上运行的程序，处理器执行程序时实现至少以下步骤：
具体地，基于约束采样的扩展随机树的生成方法包括：
步骤S101，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
步骤S102，基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
步骤S103，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
本文中的设备可以是服务器、PC、PAD、手机等。
本申请还提供了一种计算机程序产品，当在数据处理设备上执行时，适于执行初始化有至少如下方法步骤的程序：具体地，基于约束采样的扩展随机树的生成方法包括：
步骤S101，基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
步骤S102，基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
步骤S103，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器 (CPU)、输入/输出接口、网络接口和内存。
存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存 (PRAM)、静态随机存取存储器 (SRAM)、动态随机存取存储器 (DRAM)、其他类型的随机存取存储器 (RAM)、只读存储器 (ROM)、电可擦除可编程只读存储器 (EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘 (DVD) 或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体 (transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
Claims (12)
1.一种基于约束采样的扩展随机树的生成方法，其特征在于，包括：
基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹；
基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，包括：在所述随机概率小于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为朝向所述第二目标点所在的方向进行采样的方式；在所述随机概率大于或者等于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为椭圆约束采样方式；
基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，包括：从当前的所述扩展随机树的多个节点中，确定与所述第一随机采样点最近的近邻点；采用公式，确定目标扩展步长，其中，为所述目标扩展步长，为所述第二扩展因子，为固定扩展步长且大于所述无人机的最小飞行距离，为所述无人机的当前位置与多个所述障碍物之间的最小的欧式距离，为预定阈值；基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点；
基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点，包括：采用公式，确定所述第二随机采样点，其中，为所述第二随机采样点，为所述近邻点，为所述第一随机采样点。
2.根据权利要求1所述的生成方法，其特征在于，所述生成方法还包括：采用公式，确定所述第一扩展因子，其中，为所述第一扩展因子，为第一工作环境评价系数，为第个所述障碍物的所述最大尺寸，的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量；采用公式，确定所述第二扩展因子，其中，为所述第二扩展因子，为第二工作环境评价系数，为所述无人机当前的位置与第个所述障碍物之间的最小的欧式距离。
3.根据权利要求1所述的生成方法，其特征在于，将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，包括：
以所述第二随机采样点为圆心，生成半径为R的目标球体，并确定处于所述目标球体内且为当前的所述扩展随机树中的节点的目标集合，其中，，为系数，为当前的所述扩展随机树中节点的总个数，所述目标集合中包括多个节点且多个所述节点均通过碰撞检测；
将所述目标集合中的多个节点分别与所述第二随机采样点连接，从而得到从所述第一目标点至所述第二随机采样点所有的可达路径，以及确定多个所述可达路径的路径距离，得到各所述可达路径对应的路径距离；
将最小的路径距离对应的所述可达路径中，与所述第二随机采样点直接连接的目标节点，作为所述第二随机采样点的父节点，从而得到所述目标扩展随机树，所述目标节点为所述目标集合中的一个节点。
4.一种无人机的轨迹规划方法，其特征在于，包括：
生成步骤，采用权利要求1至3中任意一项所述的生成方法，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；
确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；
第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
5.根据权利要求4所述的轨迹规划方法，其特征在于，所述轨迹规划方法还包括：
第二重复步骤，在根据所述第一扩展随机树和所述第二扩展随机树无法得到一个所述目标可达路径的情况下，重复所述生成步骤至少一次，直到根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径为止。
6.根据权利要求4所述的轨迹规划方法，其特征在于，所述轨迹规划方法还包括：采用，确定所述第三扩展因子，其中，为所述第三扩展因子，为第三工作环境评价系数，为第个所述障碍物的所述最大尺寸，的取值从1开始，一直到，为所述工作空间中所述障碍物的总数量。
7.根据权利要求4或者5所述的轨迹规划方法，其特征在于，确定根据所述第一扩展随机树和所述第二扩展随机树，是否可得到一个所述目标可达路径的过程包括：
分别对所述第一扩展随机树和所述第二扩展随机树进行遍历，得到第一目标节点和第二目标节点，所述第一目标节点为所述第一扩展随机树中的一个节点，所述第二目标节点为所述第二扩展随机树中的一个节点，且所述第一目标节点与所述第二目标节点之间的距离最小；
确定所述第一目标节点和所述第二目标节点之间的欧式距离，得到目标欧式距离；
在所述目标欧式距离小于预设距离阈值的情况下，确定根据所述第一扩展随机树和所述第二扩展随机树可得到一个所述目标可达路径，且将所述第一目标节点和所述第二目标节点相连，从而得到一个所述目标可达路径。
8.根据权利要求4所述的轨迹规划方法，其特征在于，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，包括：采用公式，确定所述目标可达路径对应的动态采样值，其中，为所述动态采样值，为所述第三扩展因子，为所述目标可达路径的路径距离，为所述第一目标点与所述第二目标点之间的欧式距离，为固定采样值。
9.一种基于约束采样的扩展随机树的生成装置，其特征在于，包括：
第一确定单元，用于基于随机生成的随机概率与第一扩展因子之间的大小关系，确定第一随机采样点的目标采样方式，所述第一扩展因子是至少基于工作空间中的多个障碍物的最大尺寸确定的，所述工作空间为第一目标点和第二目标点之间的三维空间，所述障碍物的最大尺寸为对应的所述障碍物的相距最远的两个点之间的距离；
第二确定单元，用于基于所述目标采样方式，确定所述第一随机采样点，且基于所述第一随机采样点和第二扩展因子，确定第二随机采样点，所述第二扩展因子是至少基于无人机的当前位置与各所述障碍物之间的最小的欧式距离以及多个所述障碍物的最大尺寸确定的；
连接单元，用于将所述第二随机采样点与当前的扩展随机树进行连接，得到目标扩展随机树，所述目标扩展随机树包括所述无人机由所述第一目标点飞行至所述第二目标点的至少部分避障轨迹；
所述第一确定单元包括第一确定模块和第二确定模块，其中，所述第一确定模块用于在所述随机概率小于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为朝向所述第二目标点所在的方向进行采样的方式；所述第二确定模块用于在所述随机概率大于或者等于所述第一扩展因子的情况下，将所述第一随机采样点的所述目标采样方式确定为椭圆约束采样方式；
所述第二确定单元包括第三确定模块、第四确定模块和第五确定模块，其中，所述第三确定模块用于从当前的所述扩展随机树的多个节点中，确定与所述第一随机采样点最近的近邻点；所述第四确定模块用于采用公式，确定目标扩展步长，其中，为所述目标扩展步长，为所述第二扩展因子，为固定扩展步长且大于所述无人机的最小飞行距离，为所述无人机的当前位置与多个所述障碍物之间的最小的欧式距离，为预定阈值；所述第五确定模块用于基于所述目标扩展步长、所述第一随机采样点和所述近邻点，确定所述第二随机采样点；所述第五确定模块包括第一确定子模块，用于采用公式，确定所述第二随机采样点，其中，为所述第二随机采样点，为所述近邻点，为所述第一随机采样点。
10.一种无人机的轨迹规划装置，其特征在于，包括：
生成单元，用于执行生成步骤，采用权利要求9所述的生成装置，分别生成第一扩展随机树和第二扩展随机树，其中，所述第一扩展随机树的根节点为所述第一目标点和所述第二目标点中的一个，所述第二扩展随机树的根节点为所述第一目标点和所述第二目标点中的另一个，且所述第一扩展随机树和所述第二扩展随机树中的每一个节点均交替生成；
第三确定单元，用于执行确定步骤，在根据所述第一扩展随机树和所述第二扩展随机树可得到一个目标可达路径的情况下，至少基于所述目标可达路径的路径距离和第三扩展因子，确定所述目标可达路径对应的动态采样值，所述第三扩展因子至少基于所述工作空间中的所述障碍物的总数量以及所述最大尺寸确定的，所述目标可达路径为所述无人机可由所述第一目标点飞行至所述第二目标点的可达路径；
第一重复单元，用于执行第一重复步骤，在所述目标可达路径的所述动态采样值未达到目标动态采样值的情况下，依次重复所述生成步骤和所述确定步骤至少一次，直到所述目标可达路径的所述动态采样值达到所述目标动态采样值为止，且将最后一次得到的所述目标可达路径确定为所述无人机由所述第一目标点飞行至所述第二目标点的避障轨迹。
11.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行权利要求1至3中任意一项所述的基于约束采样的扩展随机树的生成方法，或者权利要求4至8中任意一项所述的无人机的轨迹规划方法。
12.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为通过所述计算机程序执行权利要求1至3中任意一项所述的基于约束采样的扩展随机树的生成方法，或者权利要求4至8中任意一项所述的无人机的轨迹规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310837145.6A CN116578121B (zh) | 2023-07-10 | 2023-07-10 | 基于约束采样的扩展随机树的生成方法及轨迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310837145.6A CN116578121B (zh) | 2023-07-10 | 2023-07-10 | 基于约束采样的扩展随机树的生成方法及轨迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116578121A CN116578121A (zh) | 2023-08-11 |
| CN116578121B true CN116578121B (zh) | 2023-11-03 |
Family
ID=87536171
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310837145.6A Active CN116578121B (zh) | 2023-07-10 | 2023-07-10 | 基于约束采样的扩展随机树的生成方法及轨迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116578121B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108458717A (zh) * | 2018-05-07 | 2018-08-28 | 西安电子科技大学 | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 |
| CN111610786A (zh) * | 2020-05-28 | 2020-09-01 | 沈阳理工大学 | 基于改进rrt算法的移动机器人路径规划方法 |
| CN111752306A (zh) * | 2020-08-14 | 2020-10-09 | 西北工业大学 | 一种基于快速扩展随机树的无人机航路规划方法 |
| CN113064426A (zh) * | 2021-03-17 | 2021-07-02 | 安徽工程大学 | 一种改进双向快速搜索随机树算法的智能车路径规划方法 |
| CN113341984A (zh) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | 基于改进rrt算法的机器人路径规划方法和装置 |
| CN113885509A (zh) * | 2021-10-21 | 2022-01-04 | 广东电网有限责任公司广州供电局 | 一种应急电源车路径规划方法、装置、设备及存储介质 |
| CN114489052A (zh) * | 2021-12-31 | 2022-05-13 | 杭州电子科技大学 | 一种改进rrt算法重连策略的路径规划方法 |
| WO2023093378A1 (zh) * | 2021-11-29 | 2023-06-01 | 中移(成都)信息通信科技有限公司 | 路径规划方法及装置、设备、存储介质 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8185239B2 (en) * | 2008-11-13 | 2012-05-22 | MSI Computer (Shenzhen) Co, Ltd. | Moving route planning method and navigation method for avoiding dynamic hindrances for mobile robot device |
| KR101667031B1 (ko) * | 2009-11-02 | 2016-10-17 | 삼성전자 주식회사 | 로봇의 경로 계획 장치 및 그 방법 |
| US10809732B2 (en) * | 2018-09-25 | 2020-10-20 | Mitsubishi Electric Research Laboratories, Inc. | Deterministic path planning for controlling vehicle movement |
| KR20220138438A (ko) * | 2021-02-26 | 2022-10-13 | 현대자동차주식회사 | 이동 로봇의 다중 경로 생성 장치 및 방법 |
| CN112884256B (zh) * | 2021-04-28 | 2021-07-27 | 深圳大学 | 路径规划方法、装置、计算机设备和存储介质 |
-
2023
- 2023-07-10 CN CN202310837145.6A patent/CN116578121B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108458717A (zh) * | 2018-05-07 | 2018-08-28 | 西安电子科技大学 | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 |
| CN111610786A (zh) * | 2020-05-28 | 2020-09-01 | 沈阳理工大学 | 基于改进rrt算法的移动机器人路径规划方法 |
| CN111752306A (zh) * | 2020-08-14 | 2020-10-09 | 西北工业大学 | 一种基于快速扩展随机树的无人机航路规划方法 |
| CN113064426A (zh) * | 2021-03-17 | 2021-07-02 | 安徽工程大学 | 一种改进双向快速搜索随机树算法的智能车路径规划方法 |
| CN113341984A (zh) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | 基于改进rrt算法的机器人路径规划方法和装置 |
| CN113885509A (zh) * | 2021-10-21 | 2022-01-04 | 广东电网有限责任公司广州供电局 | 一种应急电源车路径规划方法、装置、设备及存储介质 |
| WO2023093378A1 (zh) * | 2021-11-29 | 2023-06-01 | 中移(成都)信息通信科技有限公司 | 路径规划方法及装置、设备、存储介质 |
| CN114489052A (zh) * | 2021-12-31 | 2022-05-13 | 杭州电子科技大学 | 一种改进rrt算法重连策略的路径规划方法 |
Non-Patent Citations (4)
| Title |
|---|
| Continuous Curvature Path Planning for Semi-Autonomous Vehicle Maneuvers Using RRT*;Xiaodong Lan 等;《IEEE Xplore》;第1-8页 * |
| 基于改进RRT算法的无人驾驶汽车轨迹规划;贺伊琳 等;《西北大学学报(自然科学版)》;第48卷(第05期);第651-658页 * |
| 基于改进RRT算法的无人驾驶汽车轨迹规划;贺伊琳;高奇;赵丹;刘伟;;西北大学学报(自然科学版)(第05期);第651-658页 * |
| 智能步长对RRT算法及其改进算法的影响;黄元 等;《林业机械与木工设备》;第50卷(第12期);第59-63页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116578121A (zh) | 2023-08-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112529254B (zh) | 一种路径规划方法、装置和电子设备 | |
| CN113287121A (zh) | 神经网络的自动生成 | |
| WO2017172778A1 (en) | Collaborative navigation and mapping | |
| CN111121812B (zh) | 一种路径优化方法、电子设备及存储介质 | |
| Shi et al. | Spark PRM: Using RRTs within PRMs to efficiently explore narrow passages | |
| CN112000115B (zh) | 一种自主式水下机器人的路径规划方法及相关设备 | |
| US11534917B2 (en) | Methods, systems, articles of manufacture and apparatus to improve resource utilization for binary tree structures | |
| CN110031007B (zh) | 一种航迹规划方法、装置及计算机可读存储介质 | |
| KR20180092960A (ko) | 고속 탐색 랜덤화 피드백 기반의 모션 계획 | |
| CN110703797A (zh) | 无人机及其飞行轨迹的生成方法、计算机可读存储介质 | |
| KR20200117690A (ko) | 멀티 홉 이웃을 이용한 컨볼루션 학습 기반의 지식 그래프 완성 방법 및 장치 | |
| Yu et al. | An efficient RRT-based motion planning algorithm for autonomous underwater vehicles under cylindrical sampling constraints | |
| CN116578121B (zh) | 基于约束采样的扩展随机树的生成方法及轨迹规划方法 | |
| Almoaili et al. | Path planning algorithm for unmanned ground vehicles (UGVs) in known static environments | |
| US20200167614A1 (en) | Object localization based on spatial relationships | |
| CN115373422A (zh) | 一种无人机编队变换控制方法、装置及设备 | |
| Lin et al. | Multiple autonomous robots coordination and navigation | |
| CN115221923A (zh) | 一种定位辐射源位置的方法及系统 | |
| US10999814B1 (en) | Spatial location prediction method and device by using artificial intelligence | |
| Mejía et al. | Serverless based control and monitoring for search and rescue robots | |
| JP5182762B2 (ja) | ２次元図形マッチング方法 | |
| CN114061603A (zh) | 路径规划方法、装置、电子设备和存储介质 | |
| Liu et al. | A Neural-Symbolic Approach for Object Navigation | |
| CN112393739A (zh) | 大面积环境中的改进型快速搜索随机树路径规划方法 | |
| Matloob et al. | Exploring the Tradeoffs Between Systematic and Random Exploration in Mobile Sensors |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |