CN112338916A - Mechanical arm obstacle avoidance path planning method and system based on fast expansion random tree - Google Patents

Abstract

The invention belongs to the technical field of mechanical arm obstacle avoidance path planning, and discloses a mechanical arm obstacle avoidance path planning method and system based on a fast expansion random tree. Adding sampling points into two random trees in turn, activating the other tree when a random point is added into one of the random trees and adding a new node on the tree when the random point is added into the random tree, wherein a random node needs to be randomly generated in a path searching space firstly, then a node closest to the random node on the tree is found, then whether the connection line of the sampling point and the closest node collides with an obstacle or not is judged, and the sampling point can be added onto the tree if the collision does not occur. The method integrates the RRT algorithm for searching the father node with the lowest cost and the bidirectional RRT algorithm, so that the mechanical arm can obtain a path which ensures the optimal cost and the searching speed when planning the path.

Description

Mechanical arm obstacle avoidance path planning method and system based on fast expansion random tree

Technical Field

The invention belongs to the technical field of mechanical arm obstacle avoidance path planning, and relates to a mechanical arm obstacle avoidance path planning method and system based on a fast expansion random tree.

Background

The flexible production means that a multi-variety and small-batch production mode is realized mainly by means of highly flexible manufacturing equipment, and a programming method taking a task as a basic unit is gradually and widely applied to meet the requirement of flexible production. Under the background, the mechanical arm is required to complete obstacle avoidance path planning quickly and efficiently, namely a path capable of driving the mechanical arm to move from an initial pose to a target pose without collision is planned under the condition of giving environment and obstacle information, the initial pose and the target pose.

The existing mechanical arm obstacle avoidance path planning method comprises the following steps: artificial potential field method, A-star algorithm, probability road map algorithm, fast expansion random tree algorithm and the like. The artificial potential field method has the problems of local minimum values and interval oscillation; the A-algorithm increases exponentially with the increase of the dimension of the search space, and is not suitable for a high-degree-of-freedom mechanical arm; the learning phase of the probabilistic roadmap algorithm consumes long time and has poor adaptability to dynamic environment.

The fast expanding random tree algorithm is a single query global path planning method based on random sampling, and has the basic idea that sampling points are randomly generated in a search space, feasible sampling points in a free space are connected in a certain mode by taking a search starting point as a tree root to form a search tree model, and the search tree model is continuously expanded by a stepping method until a target point is connected to complete obstacle avoidance path search. The fast expansion random tree algorithm does not need accurate modeling in a search space, has small calculated amount and is suitable for searching obstacle avoidance paths in a high-dimensional space. However, due to the random sampling characteristic of the algorithm, the searched path moving cost is not optimal, and the path planning speed is slow.

Disclosure of Invention

The invention aims to provide a mechanical arm obstacle avoidance path planning method based on an improved fast-expanding random tree, which is used for solving the problems of low path planning efficiency and insufficient accuracy when a mechanical arm in the prior art carries out obstacle avoidance.

In order to realize the task, the invention adopts the following technical scheme:

the mechanical arm obstacle avoidance path planning method based on the fast expansion random tree comprises the following steps:

step 1: setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

establishing an initial activity tree, wherein a root node of the initial activity tree is a starting point of a joint configuration space;

establishing an initial dormancy tree, wherein a root node of the initial dormancy tree is a target point of a joint configuration space;

step 2: inserting random nodes into the initial active tree according to a fast expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the lowest distance cost to obtain an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

if the two nodes meet each other, acquiring a path passing through all the nodes between the root node of the updated active tree and the root node of the initial dormant tree as a planned path;

if not, the updated activity tree is used as an initial activity tree, and the step 3 is executed;

and step 3: inserting random nodes into the initial dormant tree according to a fast-expanding random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormant tree according to the lowest distance cost to obtain an updated dormant tree, and judging whether the initial active tree meets the updated dormant tree or not;

if the nodes meet, acquiring a path passing through all the nodes between the root node of the initial activity tree and the root node of the updated dormancy tree as a planned path;

and if the nodes do not meet the preset rule, taking the updated sleep tree as an initial sleep tree, and returning to execute the step 2 to insert new random nodes.

Further, in step 2, the initial active tree T is processed according to the fast-expanding random tree algorithm_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step (ii) ofa 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newConnecting line E between_newTo the connecting line E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newAddition of T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd an initial sleep tree T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe feasibility of all the points on the connection is judged, and if nodes meeting the feasibility exist, the nodes are marked as p_newoAnd performing step a 5; otherwise, executing step 3;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, p is added_newIs updated to p_newoObtaining an updated initial activity tree; otherwise, step 3 is executed.

Further, the step 2 of selecting the parent node of the random node on the initial active tree according to the lowest distance cost comprises the following sub-steps:

step b 1: calculating T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes inJudging until all nodes complete traversal, p_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newEuropean distance between them, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

Further, the step of selecting the child nodes of the random node on the initial active tree according to the lowest distance cost comprises the following substeps:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

The mechanical arm obstacle avoidance path planning system based on the fast expansion random tree comprises an input module, an active tree generation module, a dormant tree generation module, a cost optimization module and a path planning module;

the input module is used for setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

the activity tree generation module is used for establishing an initial activity tree, and a root node of the initial activity tree is a starting point of a joint configuration space; inserting a random node into the initial active tree according to a fast expansion random tree algorithm, and judging whether the updated active tree meets the initial dormant tree or not; if the two paths meet, executing a path planning module; if the two paths do not meet, the updated activity tree is used as an initial activity tree, and a dormant tree generating module is executed;

the sleep tree generation module is used for establishing an initial sleep tree, and a root node of the initial sleep tree is a target point of a joint configuration space; inserting random nodes into the initial dormant tree according to a fast-expanding random tree algorithm to obtain an updated dormant tree, and judging whether the initial active tree meets the updated dormant tree or not; if the two paths meet, executing a path planning module; if the two paths do not meet, the updated sleep tree is used as an initial sleep tree, and an activity tree generation module is executed;

the cost optimization module is used for selecting a father node and a child node of a random node on the initial active tree according to the lowest distance cost and selecting the father node and the child node of the random node on the initial dormant tree according to the lowest distance cost;

the path planning module is used for obtaining paths passing through all nodes between the root node of the updated active tree and the root node of the initial dormant tree according to the active tree generating module and taking the paths as planned paths; and the method is also used for obtaining a path between the root node of the initial active tree and the root node of the updated dormant tree through all the nodes according to the dormant tree generating module as a planned path.

Further, the activity tree generation module generates an initial activity tree T_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step a 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newConnecting line E between_newTo the connecting line E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newAddition of T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd an initial sleep tree T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe feasibility of all the points on the connection is judged, and if nodes meeting the feasibility exist, the nodes are marked as p_newoAnd performing step a 5; otherwise, executing the sleep tree generation module;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, p is added_newIs updated to p_newoObtaining an updated initial activity tree; otherwise, executing the sleep tree generation module.

Further, the initial activity tree T is selected according to the lowest distance cost in the cost optimization module_aThe parent node of the upper random node comprises the following sub-steps:

step b 1: calculating T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes in the node are judged until all nodes are traversed, and p is_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newAm of am betweenDistance of formula, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

Further, the cost optimization module selects T according to the lowest distance cost_aThe child node of the upper random node comprises the following sub-steps:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

Compared with the prior art, the invention has the following technical characteristics:

1. the invention improves on the basis of the traditional fast expanding random tree algorithm. And (4) utilizing a cost optimization iteration process when feasible sampling points are added into the tree model to enable the searched obstacle avoidance path to tend to have the optimal cost. The invention integrates the RRT algorithm for searching the parent node with the lowest cost with the bidirectional RRT algorithm. The new algorithm can ensure the optimal cost and the high searching speed.

2. According to the invention, by constructing the active tree and the dormant tree, after a certain sampling point is added to the random tree A, another tree B is activated by generating the sampling point next time, and the sampling point is tried to be added to the random tree B, so that the speed of path search is improved.

3. The invention integrates the idea of greedy algorithm, thereby avoiding blindness brought by random sampling to a certain extent and reducing time consumption of path search.

Drawings

Fig. 1 is a plan view of a plan scene of a planar 2DOF mechanical arm obstacle avoidance path in an embodiment;

FIG. 2 is a configuration diagram of the robot arm colliding with an obstacle in the embodiment;

fig. 3 is a schematic diagram illustrating an embodiment of searching for an obstacle avoidance path in a configuration space by using the RRT × Connect algorithm of the present invention;

fig. 4 illustrates the obstacle avoidance movement of the plane 2R robot arm in the embodiment.

Detailed Description

The meanings of the technical terms appearing in the present invention are explained first below:

configuration of robot (configuration): indicating the position of all points of the robot. The number of the minimum real coordinates of the robot with n degrees of freedom containing the robot configuration is n.

Configuration space (C-space) of the robot: the representation n-dimensional space includes all possible configurations of the robot, which can be described by a point in its configuration space.

The embodiment discloses a mechanical arm obstacle avoidance path planning method based on a fast expansion random tree, which comprises the following steps:

step 1: setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

establishing an initial activity tree, wherein a root node of the initial activity tree is a starting point of a joint configuration space;

establishing an initial dormancy tree, wherein a root node of the initial dormancy tree is a target point of a joint configuration space;

step 2: inserting random nodes into the initial active tree according to a fast expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the lowest distance cost to obtain an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

if the two nodes meet each other, acquiring a path passing through all the nodes between the root node of the updated active tree and the root node of the initial dormant tree as a planned path;

if not, the updated activity tree is used as an initial activity tree, and the step 3 is executed;

and step 3: inserting random nodes into the initial dormant tree according to a fast-expanding random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormant tree according to the lowest distance cost to obtain an updated dormant tree, and judging whether the initial active tree meets the updated dormant tree or not;

if the nodes meet, acquiring a path passing through all the nodes between the root node of the initial activity tree and the root node of the updated dormancy tree as a planned path;

and if the nodes do not meet the preset rule, taking the updated sleep tree as an initial sleep tree, and returning to execute the step 2 to insert new random nodes.

Specifically, step 2 is performed according to a fast-expanding random tree algorithm at T_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step a 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newConnecting line E between_newUsing collision detection algorithm to pair the connecting lines E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newJoining an active tree T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe feasibility of all the points on the connection is judged, and if nodes meeting the feasibility exist, the nodes are marked as p_newoAnd performing step a 5; otherwise, executing step 3;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, T is determined_aP of (A) to_newIs updated to p_newoAddition of T_aPerforming the following steps; otherwise, step 3 is executed.

Specifically, the T is_aUpper arbitrary node p_aiAnd a random node p_randEuclidean distance between:

wherein p is_aijRepresenting a node p_aiJ-th axis coordinate of (1), p_randjRepresenting a random node p_randThe j-th axis coordinate of (1); n represents the dimension of the joint configuration space, j ∈ n.

Specifically, the collision detection algorithm includes a feasibility judgment for a point and a feasibility judgment for a connection line, where the feasibility algorithm for a point includes:

and judging the feasibility of the random node prand through a collision detection algorithm, and if the point meets the prand Cobs, meeting the feasibility, wherein the OBS represents a space formed by obstacles.

The connection feasibility algorithm comprises the following steps:

ca 1: the connecting line is equally divided into m discrete points by the following formula,

wherein p is_ijA j-th axis coordinate representing an i-th discrete point on the connecting line; p is a radical of_startjRepresents the starting point p of the connecting line_startJ-th axis coordinate of (1), p_endjIndicates the connection termination point p_endM is a positive integer;

ca 2: and judging the feasibility of all m discrete points by using a point feasibility algorithm collision detection algorithm. If any point does not satisfy the prand Cobs, the connection is considered to be infeasible.

Specifically, in step a4, if p_newoIs p_nearestoWhen, T_aAnd T_bMeet and do not execute the next step.

Specifically, T is selected according to the lowest distance cost in step 2_aThe parent node of the upper random node comprises the following sub-steps:

step b 1: computing an activity tree T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes in the node are judged until all nodes are traversed, and p is_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newCurrent parent node of (2) join tree model T_aAnd traversing the next node; otherwise, traversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newEuropean distance between them, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

In particular, T is selected according to the distance cost minimum_aThe child node of the upper random node comprises the following sub-steps:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliCurrent parent node of (2) join tree model T_aAnd traversing the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

Specifically, let T_aAnd T_bIf the two are mutually replaced, the algorithm T is obtained according to the fast expansion random tree algorithm_bMethod for inserting random node and selecting T according to distance cost minimum_bParent and child nodes of the upper random node.

The embodiment also discloses a mechanical arm obstacle avoidance path planning system based on the rapid expansion random tree, which comprises an input module and a movable tree T_aGenerating module and sleep tree T_bThe system comprises a generation module, a cost optimization module and a path planning module;

the input module is used for setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

the movable tree T_aThe generation module is used for establishing the activity tree T by taking the starting point of the joint configuration space as a root node_a(ii) a According to the fast expanding random tree algorithm at T_aInserting random node, judging T_aAnd T_bIf yes, path planning is finished; if not, executing the sleep tree T_bA generation module;

the sleep tree T_bThe generation module is used for establishing the dormancy tree T by taking the target point of the joint configuration space as a root node_b(ii) a According to the fast expanding random tree algorithm at T_bInserting random node, judging T_aAnd T_bIf yes, path planning is finished; if not, returning to execute the activity tree T_aA generation module;

the cost optimization module is used for selecting T according to the lowest distance cost_aOr parent and child nodes of the random node, and is further used for selecting T according to the lowest distance cost_bA parent node and a child node of the upper random node;

the path planning module is used for acquiring T_aRoot node to T_bAnd connecting lines of all nodes passing through the root node are used as a planned path.

In particular, the active tree T_aGenerating module at T_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step a 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newConnecting line E between_newUsing collision detection algorithm to pair the connecting lines E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newJoining an active tree T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe feasibility of all the points on the connection is judged, and if nodes meeting the feasibility exist, the nodes are marked as p_newoAnd performing step a 5; otherwise, executing the sleep tree T_bA generation module;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, T is determined_aP of (A) to_newIs updated to p_newoAddition of T_aPerforming the following steps; otherwise, executing the sleep tree T_bAnd generating a module.

In particular, the active tree T_aIn the generation module, if p_newoIs p_nearestoWhen, T_aAnd T_bMeet and do not execute the next step.

Specifically, T is selected according to the lowest distance cost in the cost optimization module_aThe parent node of the upper random node comprises the following sub-steps:

step b 1: computing an activity tree T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes in the node are judged until all nodes are traversed, and p is_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newCurrent parent node of (2) join tree model T_aAnd traversing the next node; otherwise, traversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newIn betweenEuclidean distance, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

Specifically, the cost optimization module selects T according to the lowest distance cost_aThe child node of the upper random node comprises the following sub-steps:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliCurrent parent node of (2) join tree model T_aAnd traversing the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

Example 1

According to the mechanical arm obstacle avoidance path planning method, the two-degree-of-freedom mechanical arm is used as a research object for verification. As shown in fig. 1, is a top view of a planar 2R robot arm drawn by Matlab software. The black and gray rectangles in the figure are respectively the starting pose (-12.5 degrees, -52.5 degrees) and the target pose (-107.5 degrees, 27.5 degrees) of the mechanical arm, and the length and the width of each connecting rod of the mechanical arm are both 40mm and 12 mm. The two rotational joint motion ranges of the plane 2R mechanical arm are as follows: (-180 °,180 °). The discrete and filled square cells in fig. 2 represent obstacles in the workspace.

As shown in fig. 2, is a model of the workspace obstacle model mapping in joint configuration space. As can be seen from fig. 2, the problem of planning the obstacle avoidance path of the planar 2R robot arm in the working space is converted into a problem of finding a path connecting the start point and the target point in the free area of the joint configuration space.

As shown in fig. 3, the RRT × Connect algorithm is used to construct a random tree in the joint configuration space and find a collision-free path, and the coordinates of each point in the path may form a set of joint angle sequences.

The 2R mechanical arm is driven by the group of joint angle sequences, and the movement process of the mechanical arm shown in figure 4 can be obtained. It can be observed from fig. 4 that the planar 2R robot arm has moved from the start attitude to the target attitude without colliding with any obstacle disposed in fig. 1 in the process.

Claims (8)

1. The mechanical arm obstacle avoidance path planning method based on the fast expansion random tree is characterized by comprising the following steps:

step 1: setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

establishing an initial activity tree, wherein a root node of the initial activity tree is a starting point of a joint configuration space;

establishing an initial dormancy tree, wherein a root node of the initial dormancy tree is a target point of a joint configuration space;

step 2: inserting random nodes into the initial active tree according to a fast expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the lowest distance cost to obtain an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

if the two nodes meet each other, acquiring a path passing through all the nodes between the root node of the updated active tree and the root node of the initial dormant tree as a planned path;

if not, the updated activity tree is used as an initial activity tree, and the step 3 is executed;

and step 3: inserting random nodes into the initial dormant tree according to a fast-expanding random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormant tree according to the lowest distance cost to obtain an updated dormant tree, and judging whether the initial active tree meets the updated dormant tree or not;

if the nodes meet, acquiring a path passing through all the nodes between the root node of the initial activity tree and the root node of the updated dormancy tree as a planned path;

and if the nodes do not meet the preset rule, taking the updated sleep tree as an initial sleep tree, and returning to execute the step 2 to insert new random nodes.

2. The mechanical arm obstacle avoidance path planning method based on the fast expansion stochastic tree as claimed in claim 1, wherein in step 2, the initial active tree T is processed according to the fast expansion stochastic tree algorithm_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step a 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newIs connected withLine E_newTo the connecting line E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newAddition of T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd an initial sleep tree T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe feasibility of all the points on the connection is judged, and if nodes meeting the feasibility exist, the nodes are marked as p_newoAnd performing step a 5; otherwise, executing step 3;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, p is added_newIs updated to p_newoObtaining an updated initial activity tree; otherwise, step 3 is executed.

3. The mechanical arm obstacle avoidance path planning method based on the fast expansion random tree as claimed in claim 2, wherein the step 2 of selecting the parent node of the random node on the initial active tree according to the lowest distance cost comprises the following substeps:

step b 1: calculating T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes in the node are judged until all nodes are traversed, and p is_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwiseTraversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newEuropean distance between them, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

4. The mechanical arm obstacle avoidance path planning method based on the fast expansion random tree as claimed in claim 3, wherein the step of selecting the child nodes of the random nodes on the initial active tree according to the lowest distance cost comprises the substeps of:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

5. The mechanical arm obstacle avoidance path planning system based on the fast expansion random tree is characterized by comprising an input module, an active tree generation module, a dormant tree generation module, a cost optimization module and a path planning module;

the input module is used for setting a starting pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the starting pose and the target pose of the mechanical arm, wherein the joint configuration space comprises a starting point and a target point;

the activity tree generation module is used for establishing an initial activity tree, and a root node of the initial activity tree is a starting point of a joint configuration space; inserting a random node into the initial active tree according to a fast expansion random tree algorithm, and judging whether the updated active tree meets the initial dormant tree or not; if the two paths meet, executing a path planning module; if the two paths do not meet, the updated activity tree is used as an initial activity tree, and a dormant tree generating module is executed;

the sleep tree generation module is used for establishing an initial sleep tree, and a root node of the initial sleep tree is a target point of a joint configuration space; inserting random nodes into the initial dormant tree according to a fast-expanding random tree algorithm to obtain an updated dormant tree, and judging whether the initial active tree meets the updated dormant tree or not; if the two paths meet, executing a path planning module; if the two paths do not meet, the updated sleep tree is used as an initial sleep tree, and an activity tree generation module is executed;

the cost optimization module is used for selecting a father node and a child node of a random node on the initial active tree according to the lowest distance cost and selecting the father node and the child node of the random node on the initial dormant tree according to the lowest distance cost;

the path planning module is used for obtaining paths passing through all nodes between the root node of the updated active tree and the root node of the initial dormant tree according to the active tree generating module and taking the paths as planned paths; and the method is also used for obtaining a path between the root node of the initial active tree and the root node of the updated dormant tree through all the nodes according to the dormant tree generating module as a planned path.

6. The mechanical arm obstacle avoidance path planning system based on the fast expansion stochastic tree as claimed in claim 5, wherein the active tree generation module generates the active tree T at an initial active tree T_aThe upper insertion random node comprises the following substeps:

step a 1: generation of random nodes p in joint configuration space_randTo p for_randMaking a feasibility judgment if p_randIf yes, go to step a 2; otherwise, returning to execute the step a1 to generate a new random node;

step a 2: calculating p_randAnd T_aObtaining the minimum Euclidean distance T of all the nodes_aNode p of_nearest；

Obtaining p_nearestAnd p_randThe feasibility of all the points on the connection is judged, if the nodes meeting the feasibility exist, the nodes are marked as random nodes p_newAnd performing step a 3; otherwise, returning to execute the step a1 to generate a new random node;

step a 3: obtaining p_nearestAnd p_newConnecting line E between_newTo the connecting line E_newMaking a feasibility judgment if E_newIf feasibility is satisfied, p is_newAddition of T_aAnd performing step a 4; otherwise, returning to execute the step a1 to generate a new random node;

step a 4: calculating p_newAnd an initial sleep tree T_bObtaining the minimum Euclidean distance T of all the nodes_bNode p of_nearesto；

Obtaining p_nearestoAnd p_newThe connection between the two points, and the feasibility judgment is carried out on all the points on the connectionIf the node meeting the feasibility exists, the node is marked as p_newoAnd performing step a 5; otherwise, executing the sleep tree generation module;

step a 5: obtaining p_newoAnd p_newConnecting line E between_newoTo the connecting line E_newoMaking a feasibility judgment if E_newoIf the feasibility judgment is satisfied, p is added_newIs updated to p_newoObtaining an updated initial activity tree; otherwise, executing the sleep tree generation module.

7. The mechanical arm obstacle avoidance path planning method based on the improved fast expansion stochastic tree as claimed in claim 5, wherein the initial active tree T is selected according to the lowest distance cost in the cost optimization module_aThe parent node of the upper random node comprises the following sub-steps:

step b 1: calculating T_aUpper arbitrary node and random node p_randThe Euclidean distance between the nodes is smaller than the nodes corresponding to the distance threshold value of the adjacent point, and the nodes are combined into a set P of the adjacent points_near；

Step b 2: traversing a set of proximate points P_nearAll nodes in the node are judged until all nodes are traversed, and p is_newThe parent node selection is finished;

for P_nearNode p currently making a judgment_neariIf p is_neariIf formula I is satisfied, p is deleted_newConnection to its current parent node, will p_neariIs updated to p_newThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curparformula I

Wherein D (p)_neari,p_new) Represents p_neariAnd p_newBetween the Euclidean distance, Cost_neariRepresenting p from the root node_neariMoving cost of p_curparRepresents p_newCurrent parent node of, D (p)_curpar,p_new) Representing the current parent node p_curparAnd p_newEuropean distance between them, Cost_neariRepresenting from active tree root node to p_curparThe movement cost of (2).

8. The mechanical arm obstacle avoidance path planning method based on the improved fast expansion stochastic tree as claimed in claim 7, wherein the cost optimization module selects T according to the lowest distance cost_aThe child node of the upper random node comprises the following sub-steps:

step c 1: obtaining a set of proximity points P_nearAll nodes in the node are respectively connected with p_newConnecting lines between the two, and sequentially judging the feasibility of all the connecting lines to obtain a collision-free near point set P_nocolSaid P is_nocolIncluding P corresponding to all the connection lines satisfying the feasibility_nearThe node(s) in (1) is (are),

step c 2: traversing collision-free set of proximity points P_nocolAll nodes in the node are judged until all nodes are traversed, and p is_newAll child node selection is finished;

for P_nocolNode p currently making a judgment_nocoliIf p is_nocoliIf formula II is satisfied, p is deleted_nocoliConnection to its current parent node, will p_newIs updated to p_nocoliThe current parent node of the tree joins the initial activity tree and traverses the next node; otherwise, traversing the next node;

D(p_nocoli,p_new)+Cost_new＜Cost_nocoliformula II

Wherein D (p)_nocoli,p_new) Represents p_nocoliAnd p_newBetween the Euclidean distance, Cost_newRepresenting p from the root node_newMoving Cost of (1), Cost_nocoliRepresenting from root node to point p_nocoliThe movement cost of (2).

Images