WO2023197092A1 - 一种基于改进rrt算法的无人机路径规划方法 - Google Patents

一种基于改进rrt算法的无人机路径规划方法 Download PDF

Info

Publication number
WO2023197092A1
WO2023197092A1 PCT/CN2022/086013 CN2022086013W WO2023197092A1 WO 2023197092 A1 WO2023197092 A1 WO 2023197092A1 CN 2022086013 W CN2022086013 W CN 2022086013W WO 2023197092 A1 WO2023197092 A1 WO 2023197092A1
Authority
WO
WIPO (PCT)
Prior art keywords
node
search
new
rand
index
Prior art date
Application number
PCT/CN2022/086013
Other languages
English (en)
French (fr)
Inventor
崔金钟
叶茂
曹益荣
柳箐汶
Original Assignee
电子科技大学
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 电子科技大学 filed Critical 电子科技大学
Priority to PCT/CN2022/086013 priority Critical patent/WO2023197092A1/zh
Publication of WO2023197092A1 publication Critical patent/WO2023197092A1/zh

Links

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/12—Target-seeking control

Definitions

  • the invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to an unmanned aerial vehicle track planning method that improves the RRT algorithm.
  • Unmanned aerial vehicles have the characteristics of flexible movement and easy operation, and are widely used in military and civilian fields. Unmanned aerial vehicles have shown great advantages in performing missions at low altitudes. In the process of realizing unmanned aerial vehicles to autonomously perform flight tasks, trajectory planning is an extremely important link. The UAV should be able to acquire a feasible path that can avoid obstacles and reach the target point.
  • the trajectory planning problem of unmanned aerial vehicles can be regarded as a further development of robot path planning.
  • many algorithms have been proposed, such as potential field method, grid method, bionic algorithm, A* algorithm, etc.
  • Traditional path planning algorithms such as the artificial potential field method, use a method based on vector synthesis to plan the robot's motion path through the combined force of the repulsive force of obstacles to the car and the attraction of the target point to the car.
  • the obstacle avoidance strategy of the artificial potential field is very effective, but when the combined force of attraction and repulsion is zero, the car falls into a local minimum.
  • the RRT algorithm avoids modeling the space by performing collision detection on random sampling points in the state space, and can effectively solve path planning problems in high-dimensional spaces and complex constraints.
  • the RRT algorithm also has its own shortcomings. Due to its strong sampling randomness, it takes a long time to plan the path, its real-time performance is not high, and it is difficult to plan the optimal path.
  • the present invention proposes an adaptive expansion strategy based on the last expansion collision situation, and improves the RRT algorithm by optimizing the selection of parent nodes and optimizing the path after planning.
  • a RRT-based trajectory planning algorithm that can be used in three-dimensional space, mainly used in UAV scenarios. The specific steps are as follows
  • Step 1 Obtain the parameter information required for track planning, including the environmental range C free of the track planning, and the obstacle information inside C free , the starting point q init of the track planning, and the end point q goal of the track planning.
  • Step 2 with q init as the root node, establish a random search tree T search within the scope of C free .
  • T search has only one node q init .
  • an index based on spatial location is established for the nodes in T search .
  • the data structure of T search is a tree structure, in which each node should contain the following information: the position information of the current node (x, y, z), the distance information d of the current node to q init , and the parent node pointer of the current node.
  • q f the set Q child of all child node pointers of the current node.
  • the nodes in T search are based on spatial location indexes, which are characterized by:
  • the value of the key is a string in the (a, b, c) style, and the value is a subset of the nodes in the search tree.
  • step length It should not be set too small, otherwise a large number of invalid queries will easily occur.
  • Step 3 Generate a sampling point q rand for expansion within the free space C free , where the generation strategy of q rand is adaptively changed based on the last expansion collision situation.
  • n is determined as follows. If the search tree is expanded, using q goal as the q rand of this expansion and the collision detection of this expansion passes (as described in claim 1, step 4), then the value of n is added one. If any collision detection fails, the value of n is set to 0.
  • Step 4 Let the Euclidean distance between a certain node q i and q rand in T search be D(q rand ,q i ), and find a node that satisfies min(D(q rand ,q i )) in T search and set it to q nearest , and then let q nearest grow a certain distance in the direction of q rand to obtain a new node q new , and tentatively determine its parent node as q nearest , and then perform collision detection on the path (q new , q nearest ). If If the test fails, return to step 3. The growth length of q new changes adaptively based on the last expansion collision situation.
  • Step 4.1 assuming that the spatial position of q rand is (x r ,y r ,z r ), there should be an index query range ⁇ , and it should satisfy For the three data of x r , y r , and z r , respectively ⁇ , thus combining 8 sampling points.
  • Step 4.2 According to q rand and the eight sampling points, obtain their corresponding key values in the index, and then take the union K of these key values. Then, all key values in the set K are searched to obtain the corresponding search tree subset, and the node union set Q is obtained.
  • Step 4.3 if Q is not an empty set, obtain the node q nearest that satisfies min(D(q rand ,q i )) from Q. Otherwise, abandon the index and search directly from the search tree.
  • n is determined as follows. If the search tree is expanded, using q goal as the q rand of this expansion and the collision detection of this expansion passes (as described in claim 1, step 4), then the value of n is added one. If any collision detection fails, the value of n is set to 0. And ⁇ (1,+ ⁇ ) is used to control the speed of adaptive change.
  • Step 5 According to the spatial position of q new , obtain the T search node set Q best that is close to q new in the index. In Q best , find the parent node with the smallest cost, that is, when passing through this parent node, the distance from q init to q new is minimized. Add q new to T search .
  • Step 5.1 assuming the spatial position of q new is (x new , y new , z new ), there should be a search range ⁇ , and it should satisfy For the three data of x new , y new and z new , respectively ⁇ , thus combining 8 sampling points.
  • Step 5.2 According to q new and the eight sampling points, obtain their corresponding key values in the index, and then take the union K of these key values. Then, all key values in the set K are searched to obtain the corresponding search tree subset, and the node union Q best is obtained.
  • Step 6 Check whether D(q new ,q goal ) is less than a specific threshold ⁇ . If D(q new ,q goal ) ⁇ , it is determined that the planning is successful. Otherwise, return to step 3 and continue to expand T search .
  • Step 7 Optimize the obtained path (q init ,...,q i ,...,q goal ), and use q init as the starting point to find in order the farthest node q s1 (q init , q s1 ) that can pass the collision detection , and then use q s1 as the starting point to find the farthest node q s2 that (q s1 , q s2 ) can pass collision detection, and so on until it is extended to q goal .
  • the present invention improves the problem of unstable running time of the traditional RRT algorithm and non-optimal path; it also designs a node index of the search tree to speed up the selection of the RRT algorithm.
  • the speed of the nearest node; a better parent node selection method and a final path optimization method are designed to ensure that the obtained path is relatively optimal.
  • Figure 1 is a flow chart of the RRT improvement algorithm in the present invention.
  • Figure 2 is a schematic diagram of the index structure in the present invention.
  • Figure 3 is a flow chart of an expansion in the RRT algorithm of the present invention.
  • Figure 4 is a two-dimensional schematic diagram of parent node optimization in the present invention.
  • Figure 5 is a two-dimensional schematic diagram of the final path optimization of the present invention.
  • FIG. 1 is a flow chart of the improved RRT algorithm in the present invention. It can be seen that the specific steps are as follows:
  • Step 1 perform initialization and parameter setting.
  • the environment C free where the drone flies should be input into the program in the form of a three-dimensional map, and the position coordinate information of the starting point q init and the end point q goal of this pathfinding should be determined.
  • the relevant parameters of this algorithm need to be set according to the characteristics of the C free environment, including the step size of each index coverage in the index.
  • Step 2 Start preparation for route planning.
  • the index is maintained using three-dimensional array pairs, whose keys are strings in the form (a,b,c) and whose values are a subset of the nodes in the search tree.
  • Step 3 Start the expansion. First, obtain the expanded sampling point q rand .
  • the steps are as follows, also shown in Figure 3:
  • Step 3.1 Generate a random number in the range of 0-1, make a judgment based on the pg value calculated after the last expansion, and select the next step based on the result.
  • Step 3.2a if the random number is less than p g , then this expansion uses q goal as q rand .
  • Step 3.2b if the random number is greater than p g , then this expansion is based on the boundaries of C free in three dimensions (x min , x max ), (y min , y max ), (z min , z max ), respectively. Use them as the upper and lower boundaries to generate three random numbers. Assume that the obtained values are (x r , y r , z r ) respectively. This coordinate is the coordinate of q rand .
  • Step 4 Find the nearest node q nearest with distance q rand in T search .
  • the steps are as follows:
  • Step 4.1 first try to perform a quick search through the index. For the three data x r , y r , and z r , ⁇ respectively, thus combining 8 sampling points.
  • Step 4.2 use q rand and the coordinates of the eight sampling points to calculate their corresponding key values in the index, and then take the union K of these key values. Then, all keys in the set K are searched to obtain the corresponding search tree subset, and the node union set Q is obtained. According to the characteristics of Q, choose the next step.
  • Step 4.3a if Q is not an empty set, obtain the node q nearest that satisfies min(D(q rand ,q i )) from Q.
  • Step 4.3b if Q is an empty set, directly traverse the node set of T search , and obtain the node q nearest that satisfies min(D(q rand ,q i )).
  • Step 5 Generate a new node q new .
  • the steps are as follows:
  • Step 5.1 use the coordinates of q rand and q nearest to find the difference, and obtain the vector (x r -x n ,y r -y n ,z r -z n ). After calculation, set it to (x direction ,y direction ,z direction ), this vector represents the direction information of q nearest pointing to q rand . Then choose the next step based on whether random q rand is used in step 3.
  • Step 5.2a if it is random q rand , then the growth length this time is d min , and the specific coordinates should be calculated using the following formula:
  • Step 5.2b if q goal is used as q rand , then the growth length this time is the growth length d calculated after the last expansion.
  • the specific coordinates should be calculated using the following formula:
  • Step 6 Perform collision detection on the path (q new , q nearest ) and update the values of p g and d.
  • the steps are as follows:
  • Step 6.1 perform collision detection, and update the values of p g and d based on the results.
  • the formula for updating p g is as follows:
  • n The value of n is determined as follows. If the q goal is used as the q rand of this expansion when the search tree is expanded and the collision detection of this expansion passes, the value of n is increased by one. If any collision detection fails, the value of n is set to 0. And ⁇ (1,+ ⁇ ) is used to control the speed of adaptive change.
  • Step 6.2a if the collision detection fails, return to step 3.
  • Step 6.2b if the collision detection passes, take q nearest as the parent node, add q new to T search , and update the distance information from q new to q init via q nearest .
  • Step 7 update the parent node of q new and find the parent node with the minimum cost.
  • the steps are as follows:
  • Step 7.1 use the index to find the potential parent node. For the three data x new , y new , z new , ⁇ respectively, thus combining 8 sampling points.
  • Step 7.2 According to q new and the eight sampling points, obtain their corresponding key values in the index, and then take the union K of these key values. Then, all keys in the set K are searched to obtain the corresponding search tree subset, and the node union Q best is obtained.
  • Step 7.3 traverse the nodes in Q best , and for each node, calculate the distance to q init when q new passes through the node, and compare it with the distance when passing through the current parent node (this operation is not expensive, The distance information from the current node to q init is stored in the node). If it is smaller, then collision detection is performed. After passing, the parent node is updated to the current node, and the cost information of q new to the starting point is updated. Until the end of the traversal, its two-dimensional schematic diagram is shown in Figure 4.
  • Step 8 Check whether D(q new , q goal ) is less than a specific threshold ⁇ . If D(q new , q goal ) ⁇ , it is determined that the planning is successful. Otherwise, return to step 3 and continue to expand T search .
  • Step 9 Optimize the obtained path (q init ,...,q i ,...,q goal ), and use q init as the starting point to find in order the farthest node q s1 (q init , q s1 ) that can pass collision detection , and then use q s1 as the starting point to find the farthest node q s2 that (q s1 , q s2 ) can pass collision detection, and so on until it is extended to q goal .

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)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

一种基于RRT算法(快速随机扩展树，Rapid-exploration Random Tree)改进的无人机航迹规划方法，属于无人机技术领域，其包括如下步骤：初始化和参数设定；初始化随机搜索树，随机树在扩展过程中，在没有遇到障碍时，会逐渐增强其向目标点方向快速靠拢的倾向；一旦遇到障碍，将会立即降低这种倾向，最大化生长的随机性，从而使得随机树更高效地避开障碍物。同时，在每次生长时，会选择代价更小的父节点，而不是简单地选择最近节点。改进后的方法提高了规划效率和速度、降低了规划路径的距离。最后还会对规划好路径进行进一步的优化，进一步缩短规划路径长度。

Description

一种基于改进RRT算法的无人机路径规划方法 技术领域
本发明属于无人飞行器技术领域，尤其涉及一种改进RRT算法的无人机航迹规划方法。
背景技术
无人飞行器具有行动灵活、易于操作等特点，被广泛应用于军事领域和民用领域。无人飞行器在低空飞行执行任务过程中展现出了极大的优势，在实现无人飞行器自主执行飞行任务的过程中，航迹规划是极为重要的一个环节。无人飞行器应当能够获取能够规避障碍物并到达目标点的可行路径。
无人飞行器的航迹规划问题，可以看作是机器人路径规划的进一步发展。针对路径规划问题，曾经提出许多算法，如势场法、栅格法、仿生算法、A*算法等。传统的路径规划算法，如人工势场法，它采用的是基于矢量合成的方法，通过障碍物对小车的排斥力与目标点对小车的吸引力的合力作用下规划机器人的运动路径。在已知局部信息的情况下，人工势场的避障策略十分有效，但当吸引力与斥力的合力为零时，小车就陷入了陷入局部极小的情况。
大部分传统算法都需要对环境进行建模和预处理，在向高维空间进行扩展时，会极大地增加算法的复杂性。而RRT算法通过对状态空间中的随机采样点进行碰撞检测，避免了对空间的建模，能够有效的解决高维空间和复杂约束的路径规划问题。但是RRT算法也有自己的缺点，由于其较强的采样随机性，所以它规划路径用时较长，实时性不高且很难规划出最优路径。
发明内容
本发明针对RRT算法存在的问题，提出了基于上次扩展碰撞情况的自适应扩展策略，并通过优化父节点选择并在规划后优化路径的方式来对RRT算法进行改进。实现一个可用于三维空间内的基于RRT的航迹规划算法，主要用在无人机场景下，其具体步骤如下
步骤1，获取航迹规划所需的参数信息，包括航迹规划的环境范围C free，以及C free内部的障碍物信息，航迹规划的出发点q init，航迹规划的终点q goal。
步骤2，以q init为根节点，在C free的范围内，建立随机搜索树T search，初始时T search只有一个节点q init。同时为T search中的节点建立基于空间位置的索引。
其中T search的数据结构为一个树形结构，其中每个节点应当包含以下信息，当前节点的位置信息(x,y,z)，当前节点到达q init的距离信息d，当前节点的父节点指针q f，当前节点的所有子节点指针集合Q child。
而T search中的节点基于空间位置的索引，其特征在于：
使用三维数组进行快速索引，在三维数组组成的索引的键值对中，键的值为(a,b,c)样式的字符串，而值为搜索树中节点的子集。
对于搜索空间C free在三个维度上都有边界(x min,x max)、(y min,y max)、(z min,z max)，对于每一组边界，按需设置一个步长
Figure PCTCN2022086013-appb-000001
每隔
Figure PCTCN2022086013-appb-000002
设置一个索引边界x i、y i、z i。对于T search中的任意节点q，设它的空间位置为(x q,y q,z q)，其中x q∈(x t,x t+1]、y q∈(y t,y t+1]、z q∈(z t,z t+1]。那么q应当处于键为(a q,b q,c q)的节点集合中。
其中步长
Figure PCTCN2022086013-appb-000003
不应设置过小，否则容易出现大量无效查询。
在查找的过程中，需要根据对应点的空间位置，运算得出对应点的值在三维数组中的真实下标，即索引(a,b,c)。对于其计算方法，有如下公式：
Figure PCTCN2022086013-appb-000004
步骤3，在自由空间C free范围内，生成一个用于扩展的采样点q rand，其中q rand的生成策略是基于上次扩展碰撞情况而自适应变化的。
其中扩展采样点q rand生成策略，其特征在于：
需要有一个概率值p g，其表示本次扩展不使用随机方式来选取采样点q rand，而是直接使用q goal作为本次扩展的q rand的概率。在开始算法前，应当确定p g自适应变化的最大值p gmax和最小值p gmin，应满足0<p gmin<p gmax<1。
而每次搜索树扩展时使用的p g具体值由以下公式确定：
Figure PCTCN2022086013-appb-000005
其中n的值确定方式如下，若搜索树在扩展时，使用q goal作为本次扩展的q rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。
步骤4，设T search中某个节点q i与q rand的欧式距离为D(q rand,q i)，在T search中找到一个满足min(D(q rand,q i))的节点设为q nearest，然后让q nearest朝向q rand的方向生长一段距离，得到新节点q new，并暂定它的父节点为q nearest，之后对(q new,q nearest)这段路径进行碰撞检测，若检测不通过，则返回步骤3。其中q new的生长长度是基于上次扩展碰撞情况而自适应变化的。
其中q nearest的选取，需要使用之前提到的三维数组索引来进行加速，其步骤如下：
步骤4.1，设q rand的空间位置为(x r,y r,z r)，应当有一个索引查询范围ω，且应当满足
Figure PCTCN2022086013-appb-000006
对于x r,y r,z r三个数据，分别±ω，从而组合出8个采样点。
步骤4.2，根据q rand以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键值进行查找，得到对应的搜索树子集，并取得节点并集Q。
步骤4.3，若Q不为空集合，则从Q中获取满足min(D(q rand,q i))的节点q nearest。否则放弃索引，直接从搜索树中查找。
而q new的生长长度选取策略，其特征在于：
设置一个最小步长d min，当本次扩展随机方式来选取采样点q rand，那么本次生长长度为d min。
否则本次生长的长度为：
Figure PCTCN2022086013-appb-000007
其中n的值确定方式如下，若搜索树在扩展时，使用q goal作为本次扩展的q rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。而δ∈(1,+∞)，用于控制自适应变化的速度。
步骤5，根据q new的空间位置，在索引中，获取那些距离q new较近的T search节点集合Q best。在Q best中，找到代价最小的父节点，即通过此父节点时，使得q init～q new的距离最小。将q new加入T search。
其中节点集合Q best的选取，也需要通过三维数组索引来加速，其步骤为：
步骤5.1，设q new的空间位置为(x new,y new,z new)，应当有一个搜索范围μ，且应当满足
Figure PCTCN2022086013-appb-000008
对于x new,y new,z new三个数据，分别±μ，从而组合出8个采样点。
步骤5.2，根据q new以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键值进行查找，得到对应的搜索树子集，并取得节点并集Q best。
步骤6，检测D(q new,q goal)是否小于特定阈值τ，若D(q new,q goal)<τ则判定规划成功，否则返回步骤3，继续扩展T search。
步骤7，针对得到的路径(q init,…,q i,…,q goal)，进行优化，以q init为起点按顺序寻找(q init,q s1)可以通过碰撞检测的最远节点q s1，之后又以q s1为起点寻找(q s1,q s2)可以通过碰撞检测的最远节点q s2，以此类推直到扩展到q goal。得到的新路径(q init,…,q si,…,q goal)。
本发明通过设计一个基于上次扩展碰撞情况的自适应扩展策略，从而改进了传统RRT算法运行时长不稳定，路径不是最优化的问题；设计了一种搜索树的节点索引，加速了RRT算法选择最近节点的速度；设计了一个更优父节点的选择方式以及最终路径优化方法，从而保证得到的路径相对来说是最优化的。
附图说明
图1是本发明中RRT改进算法的流程图。
图2是本发明中的索引结构示意图。
图3是本发明中RRT算法中一次扩展的流程图。
图4是本发明中的父节点优化的二维示意图。
图5是本发明最终路径优化的二维示意图。
具体实施方式
下面将结合附图对本发明进行进一步地描述。
图1是本发明中改进的RRT算法流程图，可以看出其具体步骤如下：
步骤1，进行初始化和参数设定。
首先应该将无人机飞行的环境C free以三维地图的形式输入程序，并确定本次寻路的起点q init和终点q goal的位置坐标信息。此外，还需要根据环境C free的特点来设定本次算法的 相关参数，其中包括索引中，每一块索引覆盖范围的步长
Figure PCTCN2022086013-appb-000009
朝向目标扩展概率p g自适应变化的最大值p gmax和最小值p gmin；使用索引查找q nearest的寻找范围ω；每次扩展的最小步长d min；最优父节点寻找范围μ；用于控制自适应生长长度变化速率的参数δ；判定抵达终点的阈值τ。
步骤2，开始进行航迹规划的准备。
初始化随机搜索树T search，其中只有一个节点q init。同时也需要为T search中的节点建立基于空间位置的索引，其结构如图2所示。
该索引使用三维数组对来维护，其键的值为(a,b,c)样式的字符串，而值为搜索树中节点的子集。
将搜索空间C free在三个维度上的边界(x min,x max)、(y min,y max)、(z min,z max)使用之前确定好的索引覆盖范围的步长
Figure PCTCN2022086013-appb-000010
进行划分，将C free划分为若干个空间分区，之后T search生成的新节点，都会根据其坐标位置将其加入到对应空间分区的节点子集中去。
步骤3，开始扩展，首先获取扩展采样点q rand，其步骤如下，也如图3所示：
步骤3.1，进行一次范围0-1的随机数生成，并根据上次扩展结束后计算得到的p g值，来进行一次判断，根据其结果选择下一步。
步骤3.2a，若随机数小于p g，那么本次扩展使用q goal作为q rand。
步骤3.2b，若随机数大于p g，那么本次扩展根据C free在三个维度上的边界(x min,x max)、(y min,y max)、(z min,z max)，分别以它们为上下边界进行三次随机数生成，设得到的值分别为(x r,y r,z r)，此坐标即为q rand的坐标。
步骤4，找到T search中距离q rand最近节点q nearest，其步骤如下：
步骤4.1，首先尝试通过索引来进行快速查找，对于x r,y r,z r三个数据，分别±ω，从而组合出8个采样点。
步骤4.2，使用q rand以及八个采样点的坐标，分别计算出其在索引对应的键值，之后对这些键值取并集K。之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q。根据Q的特点，选择下一步。
步骤4.3a，若Q不为空集合，则从Q中获取满足min(D(q rand,q i))的节点q nearest。
步骤4.3b，若Q为空集合，则直接遍历T search的节点集合，从中获取满足min(D(q rand,q i))的节点q nearest。
步骤5，生成新节点q new，其步骤如下：
步骤5.1，使用q rand和q nearest的坐标求差值，获取到向量(x r-x n,y r-y n,z r-z n)，计算后设为(x direction,y direction,z direction)，该向量表示了q nearest指向q rand的方向信息。之后根据步骤3中是否使用随机q rand，来选择下一步。
步骤5.2a，若是随机q rand，那么本次生长长度为d min，具体到坐标应该用以下公式计算：
Figure PCTCN2022086013-appb-000011
步骤5.2b，若是使用q goal作为q rand，那么本次生长长度为上次扩展结束后计算得到的生长长度d，具体到坐标应该用以下公式计算：
Figure PCTCN2022086013-appb-000012
步骤6，针对(q new,q nearest)这段路径进行碰撞检测，并更新p g和d的值，其步骤如下：
步骤6.1，进行碰撞检测，根据结果来对p g和d的值进行更新，更新p g的公式如下：
Figure PCTCN2022086013-appb-000013
更新d的公式如下：
Figure PCTCN2022086013-appb-000014
其中n的值确定方式如下，若搜索树在扩展时，使用q goal作为本次扩展的q rand且本次扩展的碰撞检测通过，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。而δ∈(1,+∞)，用于控制自适应变化的速度。
之后还需要根据碰撞检测结果来确定下一步：
步骤6.2a，若碰撞检测不通过，则返回步骤3。
步骤6.2b，若碰撞检测通过，则以q nearest为父节点，将q new加入T search，并更新q new经过q nearest到达q init的距离信息。
步骤7，更新q new的父节点，寻找代价最小父节点，其步骤如下：
步骤7.1，使用索引来找到潜在父节点，对于x new,y new,z new三个数据，分别±μ，从而组合出8个采样点。
步骤7.2，根据q new以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q best。
步骤7.3，对Q best中的节点进行遍历，对每一个节点，进行q new经过该节点时，到达q init的距离计算，并与经过当前父节点时的距离进行比较(本运算代价不大，在节点中存有当前节点到达q init的距离信息)，若是更小，那么再进行碰撞检测，通过后，则更新父节点为当前节点，并更新q new到达起点的代价信息。直到遍历结束，其二维示意图如图4所示。
步骤8，检测D(q new,q goal)是否小于特定阈值τ，若D(q new,q goal)<τ则判定规划成功，否则返回步骤3，继续扩展T search。
步骤9，针对得到的路径(q init,…,q i,…,q goal)，进行优化，以q init为起点按顺序寻找(q init,q s1)可以通过碰撞检测的最远节点q s1，之后又以q s1为起点寻找(q s1,q s2)可以通过碰撞检测的最远节点q s2，以此类推直到扩展到q goal。得到的新路径(q init,…,q si,…,q goal)。整个过程如图5所示。

Claims (5)

  1. 一种基于RRT算法改进的无人机航迹规划方法，其特征在于，在RRT算法中引入了基于上次扩展碰撞情况的自适应扩展策略，并通过优化父节点选择并在规划后优化路径的方式来提高航迹规划的效率并降低路径的距离，具体步骤如下：
    步骤1，获取航迹规划所需的参数信息，包括航迹规划的环境范围C free，以及C free内部的障碍物信息，航迹规划的出发点q init，航迹规划的终点q goal；
    步骤2，以q init为根节点，在C free的范围内，建立随机搜索树T search，初始时T search只有一个节点q init，同时为T search中的节点建立基于空间位置的索引；
    步骤3，在自由空间C free范围内，生成一个用于扩展的采样点q rand，其中q rand的生成策略如下：
    需要有一个概率值p g，其表示本次扩展不使用随机方式来选取采样点q rand，而是直接使用q goal作为本次扩展的q rand的概率，在开始算法前，应当确定p g自适应变化的最大值p gmax和最小值p gmin，应满足0<p gmin<p gmax<1；
    而每次搜索树扩展时使用的p g具体值由以下公式确定：
    Figure PCTCN2022086013-appb-100001
    其中n的值确定方式如下，若搜索树在扩展时，使用q goal作为本次扩展的q rand且本次扩展的碰撞检测通过，则n的值加一，若是任意一次碰撞检测不通过，则n的值置0；
    步骤4，设T search中某个节点q i与q rand的欧式距离为D(q rand,q i)，在T search中找到一个满足min(D(q rand,q i))的节点设为q nearest，然后让q nearest朝向q rand的方向生长一段距离，得到新节点q new，并暂定它的父节点为q nearest，之后对(q new,q nearest)这段路径进行碰撞检测，若检测不通过，则返回步骤3，其中q new的生长长度策略如下：
    设置一个最小步长d min，当本次扩展随机方式来选取采样点q rand，那么本次生长长度为d min。
    否则本次生长的长度为：
    Figure PCTCN2022086013-appb-100002
    其中n的值确定方式如下，若搜索树在扩展时，使用q goal作为本次扩展的q rand且本次扩展的碰撞检测通过，则n的值加一，若是任意一次碰撞检测不通过，则n的值置0，而δ∈(1,+∞)，用于控制自适应变化的速度；
    步骤5，根据q new的空间位置，在索引中，获取那些距离q new较近的T search节点集合Q best，在Q best中，找到代价最小的父节点，即通过此父节点时，使得q init～q new的距离最小，将q new加入T search；
    步骤6，检测D(q new,q goal)是否小于特定阈值τ，若D(q new,q goal)<τ则判定规划成功，否则返回步骤3，继续扩展T search；
    步骤7，针对得到的路径(q init,…,q i,…,q goal)，进行优化，以q init为起点按顺序寻找(q init,q s1)可以通过碰撞检测的最远节点q s1，之后又以q s1为起点寻找(q s1,q s2)可以通过碰撞 检测的最远节点q s2，以此类推直到扩展到q goal，得到的新路径(q init,…,q si,…,q goal)。
  2. 如权利要求1所述的基于RRT算法改进的无人机航迹规划方法，其特征在于，步骤2中所述的T search的数据结构为：
    其为一个树形结构，其中每个节点应当包含以下信息，当前节点的位置信息(x,y,z)，当前节点到达q init的距离信息d，当前节点的父节点指针q f，当前节点的所有子节点指针集合Q child。
  3. 如权利要求1所述的基于RRT算法改进的无人机航迹规划方法，其特征在于，步骤2中所述的T search中的节点基于空间位置的索引为：
    使用三维数组进行快速索引，使用的索引键的值为(a,b,c)样式的字符串，而索引中的值为搜索树中节点的子集；
    对于搜索空间C free在三个维度上都有边界(x min,x max)、(y min,y max)、(z min,z max)，对于每一组边界，按需设置一个步长
    Figure PCTCN2022086013-appb-100003
    每隔
    Figure PCTCN2022086013-appb-100004
    设置一个索引边界x i、y i、z i。对于T search中的任意节点q，设它的空间位置为(x q,y q,z q)，其中x q∈(x t,x t+1]、y q∈(y t,y t+1]、z q∈(z t,z t+1]，那么q应当处于键为(a q,b q,c q)的节点集合中；
    其中步长
    Figure PCTCN2022086013-appb-100005
    不应设置过小，否则容易出现大量无效查询；
    在查找的过程中，需要根据对应点的空间位置，运算得出对应点的值在三维数组中的真实下标，即索引(a,b,c)，对于其计算方法，有如下公式：
    Figure PCTCN2022086013-appb-100006
    对应节点所在的节点子集就存放在三维数组中下标为(a,b,c)的位置上。
  4. 如权利要求3所述的基于RRT算法改进的无人机航迹规划方法，其特征在于，权利要求1步骤4中所述的q nearest的选取方式为：使用上述三维数组索引来进行快速查找，具体步骤如下：
    步骤1，设q rand的空间位置为(x r,y r,z r)，应当有一个索引查询范围ω，且应当满足
    Figure PCTCN2022086013-appb-100007
    对于x r,y r,z r三个数据，分别±ω，从而组合出8个采样点；
    步骤2，根据q rand以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K，之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q；
    步骤3，若Q不为空集合，则从Q中获取满足min(D(q rand,q i))的节点q nearest，否则放弃索引，直接从搜索树中查找。
  5. 如权利要求3所述的基于RRT算法改进的无人机航迹规划方法，其特征在于，权利要求1步骤5中所述的节点集合Q best的获取方式为：使用上述三维数组索引来进行快速查找，具体步骤如下：
    步骤1，设q new的空间位置为(x new,y new,z new)，应当有一个搜索范围μ，且应当满足
    Figure PCTCN2022086013-appb-100008
    对于x new,y new,z new三个数据，分别±μ，从而组合出8个采样点；
    步骤2，根据q new以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些 键值取并集K，之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点集合Q best。
PCT/CN2022/086013 2022-04-11 2022-04-11 一种基于改进rrt算法的无人机路径规划方法 WO2023197092A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/CN2022/086013 WO2023197092A1 (zh) 2022-04-11 2022-04-11 一种基于改进rrt算法的无人机路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2022/086013 WO2023197092A1 (zh) 2022-04-11 2022-04-11 一种基于改进rrt算法的无人机路径规划方法

Publications (1)

Publication Number Publication Date
WO2023197092A1 true WO2023197092A1 (zh) 2023-10-19

Family

ID=88328511

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/086013 WO2023197092A1 (zh) 2022-04-11 2022-04-11 一种基于改进rrt算法的无人机路径规划方法

Country Status (1)

Country Link
WO (1) WO2023197092A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115016475A (zh) * 2022-06-07 2022-09-06 中国人民解放军国防科技大学 一种无人设备的规划方法
CN117124335A (zh) * 2023-10-25 2023-11-28 山东工商学院 一种基于路径标记回溯策略的改进式rrt路径规划方法
CN117470252A (zh) * 2023-12-28 2024-01-30 中闽(福清)风电有限公司 基于rrt*算法的风电场升压站机器人巡检局部路径规划方法
CN118153664A (zh) * 2024-05-10 2024-06-07 杭州彩连科技有限公司 一种启发式强化学习搜索树排版方法
CN118578403A (zh) * 2024-08-05 2024-09-03 杭州芯控智能科技有限公司 一种机械臂避障路径规划方法、系统及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108983780A (zh) * 2018-07-24 2018-12-11 武汉理工大学 一种基于改进rrt*算法的移动机器人路径规划方法
CN112987799A (zh) * 2021-04-16 2021-06-18 电子科技大学 一种基于改进rrt算法的无人机路径规划方法
US20210318697A1 (en) * 2020-04-08 2021-10-14 Lockheed Martin Corporation Autonomous aircraft local planning to avoid obstructions
CN114115362A (zh) * 2021-11-30 2022-03-01 沈阳航空航天大学 一种基于双向apf-rrt*算法的无人机航迹规划方法
CN114237302A (zh) * 2021-11-12 2022-03-25 北京机电工程研究所 一种基于滚动时域的三维实时rrt*航路规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108983780A (zh) * 2018-07-24 2018-12-11 武汉理工大学 一种基于改进rrt*算法的移动机器人路径规划方法
US20210318697A1 (en) * 2020-04-08 2021-10-14 Lockheed Martin Corporation Autonomous aircraft local planning to avoid obstructions
CN112987799A (zh) * 2021-04-16 2021-06-18 电子科技大学 一种基于改进rrt算法的无人机路径规划方法
CN114237302A (zh) * 2021-11-12 2022-03-25 北京机电工程研究所 一种基于滚动时域的三维实时rrt*航路规划方法
CN114115362A (zh) * 2021-11-30 2022-03-01 沈阳航空航天大学 一种基于双向apf-rrt*算法的无人机航迹规划方法

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115016475A (zh) * 2022-06-07 2022-09-06 中国人民解放军国防科技大学 一种无人设备的规划方法
CN115016475B (zh) * 2022-06-07 2024-08-02 中国人民解放军国防科技大学 一种无人设备的规划方法
CN117124335A (zh) * 2023-10-25 2023-11-28 山东工商学院 一种基于路径标记回溯策略的改进式rrt路径规划方法
CN117124335B (zh) * 2023-10-25 2024-01-05 山东工商学院 一种基于路径标记回溯策略的改进式rrt路径规划方法
CN117470252A (zh) * 2023-12-28 2024-01-30 中闽(福清)风电有限公司 基于rrt*算法的风电场升压站机器人巡检局部路径规划方法
CN118153664A (zh) * 2024-05-10 2024-06-07 杭州彩连科技有限公司 一种启发式强化学习搜索树排版方法
CN118578403A (zh) * 2024-08-05 2024-09-03 杭州芯控智能科技有限公司 一种机械臂避障路径规划方法、系统及介质

Similar Documents

Publication Publication Date Title
WO2023197092A1 (zh) 一种基于改进rrt算法的无人机路径规划方法
CN112987799B (zh) 一种基于改进rrt算法的无人机路径规划方法
CN108681787B (zh) 基于改进双向快速扩展随机树算法的无人机路径优化方法
CN110488859A (zh) 一种基于改进Q-learning算法的无人机航路规划方法
CN111930121B (zh) 一种室内移动机器人的混合路径规划方法
CN108919818B (zh) 基于混沌种群变异pio的航天器姿态轨道协同规划方法
Ruan et al. Multi-UAV obstacle avoidance control via multi-objective social learning pigeon-inspired optimization
CN112666981B (zh) 基于原鸽群动态群组学习的无人机集群动态航路规划方法
CN113848919A (zh) 一种基于蚁群算法的室内agv路径规划方法
CN112733251B (zh) 一种多无人飞行器协同航迹规划方法
CN110836670B (zh) 一种求解无人机约束航路规划的混合烟花粒子群协同方法
CN112947594B (zh) 一种面向无人机的航迹规划方法
CN113805609A (zh) 一种混沌迷失鸽群优化机制的无人机群目标搜索方法
CN114489052B (zh) 一种改进rrt算法重连策略的路径规划方法
CN114115362A (zh) 一种基于双向apf-rrt*算法的无人机航迹规划方法
CN112824998A (zh) 马尔可夫决策过程的多无人机协同航路规划方法和装置
CN113341998A (zh) 一种改进蚁群算法的三维水下欠驱动auv路径规划方法
Yang et al. Three-dimensional UAV cooperative path planning based on the MP-CGWO algorithm
CN112363532A (zh) 一种基于quatre算法的多无人机同时起飞集结方法
Wu et al. An improved fast convergent artificial bee colony algorithm for unmanned aerial vehicle path planning in battlefield environment
CN113741186A (zh) 一种基于近端策略优化的双机空战决策方法
CN115357051B (zh) 变形与机动一体化的规避与突防方法
CN114815875B (zh) 一种基于集合满射鸽群智能优化的无人机集群编队控制器调参方法
CN114637331A (zh) 一种基于蚁群算法的无人机多任务路径规划方法及系统
CN111896001A (zh) 一种三维蚁群航迹优化方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22936752

Country of ref document: EP

Kind code of ref document: A1