CN112338916B - Mechanical arm obstacle avoidance path planning method and system based on rapid 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 rapid expansion random tree. And adding sampling points to two random trees in turn, when a certain random point is added to one random tree, activating the other tree by generating the sampling point next time, adding a new node to the tree when adding the random point to the random tree, randomly generating a random node in a space searched by a path, finding a node closest to the random node on the tree, judging whether a connecting line of the sampling point and the nearest node collides with an obstacle, and adding the sampling point to the tree if the connecting line does not collide. The invention fuses the RRT algorithm for searching the father node with the lowest cost and the bidirectional RRT algorithm, so that the mechanical arm obtains the path with the optimal guaranteed cost and the searching speed when carrying out path planning.

Description

Mechanical arm obstacle avoidance path planning method and system based on rapid 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 rapid expansion random tree.

Background

The flexible production is a production mode which mainly depends on high-flexibility manufacturing equipment to realize multiple varieties and small batches, and a programming method taking tasks as basic units is widely applied gradually to meet the requirement of flexible production. Under the background, the mechanical arm is required to be capable of rapidly and efficiently completing obstacle avoidance path planning, namely, planning a path capable of driving the mechanical arm to move from the initial pose to the target pose without collision under the condition of given 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-algorithm, probability roadmap algorithm, fast-expanding random tree algorithm, etc. The artificial potential field method has the problems of sinking local minima and interval oscillation; the calculation amount of the algorithm A increases exponentially along with the increase of the dimension of the search space, and the algorithm A is not suitable for a high-freedom mechanical arm; the learning phase of the probabilistic roadmap algorithm takes longer time and has poor adaptability to dynamic environments.

The fast expanding random tree algorithm is one random sampling based single query global path planning method, and has the basic idea of randomly generating sampling points in the search space, connecting feasible sampling points in the free space in certain mode with the searching starting point as tree root to form a search tree model, expanding the search tree model continuously through a step method until connecting the target points, and completing obstacle avoidance path searching. The rapid expansion random tree algorithm does not need to accurately model in a search space, has small calculated amount, and is suitable for obstacle avoidance path search in a high-dimensional space. However, due to the random sampling characteristic of the algorithm, the path moving cost obtained by searching is not optimal, and the path planning speed is low.

Disclosure of Invention

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

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

A robot arm obstacle avoidance path planning method based on a rapid expansion random tree comprises the following steps:

Step 1: setting an initial pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the initial 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 rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the minimum distance cost, obtaining an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

If the nodes meet, obtaining paths from the root node of the updated active tree to the root node of the initial dormancy tree through all the nodes as planned paths;

if not, taking the updated activity tree as an initial activity tree, and executing the step 3;

step 3: inserting random nodes into the initial dormancy tree according to a rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormancy tree according to the lowest distance cost, obtaining an updated dormancy tree, and judging whether the initial activity tree meets the updated dormancy tree;

if the nodes meet, obtaining paths passing through all the nodes from the root node of the initial active tree to the root node of the updated dormancy tree as planned paths;

if not, the updated dormancy tree is used as the initial dormancy tree, and the step 2 is executed again to insert new random nodes.

Further, the step 2 of inserting random nodes on the initial active tree T _a according to the fast extended random tree algorithm includes the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new, adding p _new into T _a if E _new meets the feasibility, and executing step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on the initial dormancy tree T _b, and obtaining a node p _nearesto on a T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing the step 3;

Step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new to p _newo to obtain an updated initial activity tree; otherwise, step 3 is executed.

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

Step b1: the Euclidean distance between any node on the T _a and the random node P _rand is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the initial active tree, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Where D (pneari, pnew) represents the Euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the Euclidean distance between current parent nodes p _curpar and p _new, and Cost _curpar represents the Cost of moving from the active tree root node to p _curpar.

Further, selecting a child node of a random node on the initial active tree based on the lowest distance cost comprises the following sub-steps:

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

For the node P _nocoli currently judged in P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the initial active tree, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (pnocoli, pnew) represents the Euclidean distance between p _nocoli and p _new, costnew represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli.

The mechanical arm obstacle avoidance path planning system based on the rapid 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 the initial pose and the target pose of the mechanical arm, and building a joint configuration space according to the initial 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 the root node of the initial activity tree is the starting point of the joint configuration space; inserting random nodes into the initial active tree according to the rapid expansion random tree algorithm, and judging whether the updated active tree meets the initial dormancy tree or not; if the two paths meet, executing a path planning module; if not, taking the updated activity tree as an initial activity tree, and executing a dormancy tree generating module;

The dormancy tree generating module is used for establishing an initial dormancy tree, and a root node of the initial dormancy tree is a target point of the joint configuration space; inserting random nodes into the initial dormancy tree according to a rapid expansion random tree algorithm to obtain an updated dormancy tree, and judging whether the initial activity tree meets the updated dormancy tree or not; if the two paths meet, executing a path planning module; if not, taking the updated dormancy tree as an initial dormancy tree, and executing an activity tree generation module;

The cost optimization module is used for selecting father nodes and child nodes of the random nodes on the initial active tree according to the lowest distance cost and selecting father nodes and child nodes of the random nodes 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 dormancy tree as planned paths according to the active tree generating module; and the path passing through all nodes between the root node of the initial active tree and the root node of the updated dormancy tree is obtained as a planned path according to the dormancy tree generating module.

Further, the activity tree generation module inserts random nodes on the initial activity tree T _a, including the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new, adding p _new into T _a if E _new meets the feasibility, and executing step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on the initial dormancy tree T _b, and obtaining a node p _nearesto on a T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing a dormancy tree generating module;

Step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new to p _newo to obtain an updated initial activity tree; otherwise, executing the dormancy tree generating module.

Further, the selecting the parent node of the random node on the initial activity tree T _a according to the lowest distance cost in the cost optimization module includes the following sub-steps:

Step b1: the Euclidean distance between any node on the T _a and the random node P _rand is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the initial active tree, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Where D (pneari, pnew) represents the Euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the Euclidean distance between current parent nodes p _curpar and p _new, and Cost _curpar represents the Cost of moving from the active tree root node to p _curpar.

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

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

For the node P _nocoli currently judged in P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the initial active tree, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (pnocoli, pnew) represents the Euclidean distance between p _nocoli and p _new, costnew represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli.

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 (3) utilizing a cost optimization iterative process when the feasible sampling points are added into the tree model to enable the obstacle avoidance path obtained by searching to tend to be optimal in cost. The invention combines the RRT algorithm for searching the father node with the lowest cost with the bidirectional RRT algorithm. The new algorithm can ensure the optimal cost and the faster 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, the next sampling point generation activates the other tree B, and the sampling point is attempted to be added to the random tree B, so that the speed of path searching is improved.

3. The method integrates the thought of a greedy algorithm, avoids blindness caused by random sampling to a certain extent, and reduces the time consumption of path searching.

Drawings

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

FIG. 2 is a diagram illustrating a configuration of a collision between a robot arm and an obstacle in an embodiment;

Fig. 3 is a schematic diagram of searching obstacle avoidance paths in a configuration space by using rrt_connect algorithm according to the present invention;

fig. 4 illustrates obstacle avoidance movement of a planar 2R robot in an embodiment.

Detailed Description

The meaning of each technical term appearing in the present invention is explained first as follows:

Configuration of robot): indicating the positions of all points of the robot. The number of the smallest real coordinates of the robot configuration contained in the robot with n degrees of freedom is n.

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

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

Step 1: setting an initial pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the initial 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 rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the minimum distance cost, obtaining an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

If the nodes meet, obtaining paths from the root node of the updated active tree to the root node of the initial dormancy tree through all the nodes as planned paths;

if not, taking the updated activity tree as an initial activity tree, and executing the step 3;

step 3: inserting random nodes into the initial dormancy tree according to a rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormancy tree according to the lowest distance cost, obtaining an updated dormancy tree, and judging whether the initial activity tree meets the updated dormancy tree;

if the nodes meet, obtaining paths passing through all the nodes from the root node of the initial active tree to the root node of the updated dormancy tree as planned paths;

if not, the updated dormancy tree is used as the initial dormancy tree, and the step 2 is executed again to insert new random nodes.

Specifically, the inserting the random node into the T _a according to the fast extended random tree algorithm in the step 2 includes the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new by adopting a collision detection algorithm, adding p _new into an activity tree T _a if E _new meets the feasibility, and executing a step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on T _b, and obtaining a node p _nearesto on T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing the step 3;

Step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new on T _a into p _newo and adding the p _newo into T _a; otherwise, step 3 is executed.

Specifically, the euclidean distance between any node p _ai and the random node p _rand on T _a:

Where p _aij represents the j-th axis coordinate of node p _ai and p _randj represents the j-th axis coordinate of random node p _rand; n represents the dimension of the joint configuration space, j ε n.

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

the feasibility of the random node prand is judged by a collision detection algorithm, if the feasibility of the random node prand is satisfied The feasibility is satisfied wherein OBS represents the space constituted by the obstacle.

The link feasibility algorithm comprises the following steps:

ca1: the connecting line is equally divided into m discrete points by the following,

Wherein p _ij represents the j-th axis coordinate of the i-th discrete point on the line; p _startj represents the j-th axis coordinate of the connection start point p _start, p _endj represents the j-th axis coordinate of the connection end point p _end, and m is a positive integer;

ca2: and judging the feasibility of all m discrete points by using a point feasibility algorithm collision detection algorithm. If any point is not satisfied The connection is deemed to be infeasible.

Specifically, in step a4, if p _newo is p _nearesto, T _a and T _b meet, and the next step is not performed.

Specifically, in step 2, selecting the parent node of the random node on T _a according to the lowest distance cost includes the following sub-steps:

Step b1: the Euclidean distance between any node and a random node P _rand on the activity tree T _a is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in the P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the tree model T _a, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Where D (pneari, pnew) represents the Euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the Euclidean distance between current parent nodes p _curpar and p _new, and Cost _curpar represents the Cost of moving from the active tree root node to p _curpar.

Specifically, selecting the child node of the random node on T _a according to the lowest distance cost includes the following sub-steps:

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

for the node P _nocoli currently judged in the P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the tree model T _a, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (pnocoli, pnew) represents the Euclidean distance between p _nocoli and p _new, costnew represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli.

Specifically, the method of inserting the random node on the T _b according to the fast expansion random tree algorithm and selecting the father node and the son node of the random node on the T _b according to the minimum distance cost are obtained by making the T _a and the T _b replace each other.

The embodiment also discloses a robot arm obstacle avoidance path planning system based on the rapid expansion random tree, which comprises an input module, an active tree T _a generation module, a dormant tree T _b generation module, a cost optimization module and a path planning module;

the input module is used for setting the initial pose and the target pose of the mechanical arm, and building a joint configuration space according to the initial 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 T _a generation module is used for establishing an activity tree T _a by taking a starting point of the joint configuration space as a root node; inserting random nodes on the T _a according to the fast expanding random tree algorithm, judging whether the T _a meets the T _b or not, and if so, completing path planning; if not, executing a dormancy tree T _b generation module;

The dormancy tree T _b generation module is used for establishing a dormancy tree T _b by taking a target point of the joint configuration space as a root node; inserting random nodes on the T _b according to the fast expanding random tree algorithm, judging whether the T _a meets the T _b or not, and if so, completing path planning; if not, returning to the execution activity tree T _a generation module;

The cost optimization module is used for selecting the father node and the child node of the T _a or the upper random node according to the lowest distance cost and selecting the father node and the child node of the upper random node of the T _b according to the lowest distance cost;

The path planning module is used for acquiring the connecting lines from the T _a root node to all the nodes passing through the T _b root node as planned paths.

Specifically, the activity tree T _a generation module inserts a random node on T _a comprising the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new by adopting a collision detection algorithm, adding p _new into an activity tree T _a if E _new meets the feasibility, and executing a step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on T _b, and obtaining a node p _nearesto on T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing a dormancy tree T _b generation module;

Step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new on T _a into p _newo and adding the p _newo into T _a; otherwise, the sleep tree T _b generation module is executed.

Specifically, in the activity tree T _a generation module, if p _newo is p _nearesto, T _a and T _b meet, and the next step is not executed.

Specifically, the selecting the parent node of the random node on the T _a according to the lowest distance cost in the cost optimization module includes the following sub-steps:

Step b1: the Euclidean distance between any node and a random node P _rand on the activity tree T _a is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in the P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the tree model T _a, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Where D (pneari, pnew) represents the Euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the Euclidean distance between current parent nodes p _curpar and p _new, and Cost _curpar represents the Cost of moving from the active tree root node to p _curpar.

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

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

for the node P _nocoli currently judged in the P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the tree model T _a, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (pnocoli, pnew) represents the Euclidean distance between p _nocoli and p _new, costnew represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli.

Example 1

According to the mechanical arm obstacle avoidance path planning method, the mechanical arm with two degrees of freedom is used as a research object to be unfolded and verified. As shown in fig. 1, a plan view of a planar 2R robot arm is drawn by Matlab software. In the figure, the black and gray rectangles are respectively the initial 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 width of each connecting rod of the mechanical arm are 40mm and 12mm respectively. The two rotation joint movement ranges of the plane 2R mechanical arm are as follows: (-180 °,180 °). The discrete and filled square cells in fig. 2 represent obstructions in the workspace.

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

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

The mechanical arm movement process shown in fig. 4 can be obtained by driving the 2R mechanical arm by using the joint angle sequence. From fig. 4 it can be observed that the planar 2R robot arm moves from the starting pose to the target pose and in the process does not collide with any obstacle provided in fig. 1.

Claims (2)

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

Step 1: setting an initial pose and a target pose of the mechanical arm, and establishing a joint configuration space according to the initial 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 rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial active tree according to the minimum distance cost, obtaining an updated active tree, and judging whether the updated active tree meets the initial dormant tree or not;

If the nodes meet, obtaining paths from the root node of the updated active tree to the root node of the initial dormancy tree through all the nodes as planned paths;

if not, taking the updated activity tree as an initial activity tree, and executing the step 3;

the inserting of random nodes on the initial active tree T _a according to the fast extended random tree algorithm includes the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new, adding p _new into T _a if E _new meets the feasibility, and executing step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on the initial dormancy tree T _b, and obtaining a node p _nearesto on a T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing the step 3;

Step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new to p _newo to obtain an updated initial activity tree; otherwise, executing the step 3;

the method comprises the steps of judging the feasibility of the point and judging the feasibility of the connecting line, wherein the feasibility algorithm of the point comprises the following steps:

the feasibility of the random node prand is judged by a collision detection algorithm, if the feasibility of the random node prand is satisfied The feasibility is satisfied wherein OBS represents a space constituted by an obstacle;

The link feasibility algorithm comprises the following steps:

ca1: the connecting line is equally divided into m discrete points by the following,

Wherein p _ij represents the j-th axis coordinate of the i-th discrete point on the line; p _startj represents the j-th axis coordinate of the connection start point p _start, p _endj represents the j-th axis coordinate of the connection end point p _end, and m is a positive integer;

ca2: judging the feasibility of all m discrete points by using a point feasibility algorithm collision detection algorithm; if any point is not satisfied The connection is deemed not viable;

The parent node for selecting the random node on the initial active tree according to the lowest distance cost comprises the following sub-steps:

Step b1: the Euclidean distance between any node on the T _a and the random node P _rand is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the initial active tree, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Wherein D (p _neari,p_new) represents the euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the euclidean distance between current parent nodes p _curpar and p _new, cost _curpar represents the Cost of moving from the active tree root node to p _curpar;

The sub-node for selecting the random node on the initial active tree according to the lowest distance cost comprises the following sub-steps:

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

For the node P _nocoli currently judged in P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the initial active tree, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (p _nocoli,p_new) represents the Euclidean distance between p _nocoli and p _new, cost _new represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli;

step 3: inserting random nodes into the initial dormancy tree according to a rapid expansion random tree algorithm, selecting father nodes and child nodes of the random nodes on the initial dormancy tree according to the lowest distance cost, obtaining an updated dormancy tree, and judging whether the initial activity tree meets the updated dormancy tree;

if the nodes meet, obtaining paths passing through all the nodes from the root node of the initial active tree to the root node of the updated dormancy tree as planned paths;

if not, the updated dormancy tree is used as the initial dormancy tree, and the step 2 is executed again to insert new random nodes.

2. The mechanical arm obstacle avoidance path planning system based on the rapid expansion random tree is characterized by comprising an input module, an active tree generating module, a dormant tree generating module, a cost optimizing module and a path planning module;

the input module is used for setting the initial pose and the target pose of the mechanical arm, and building a joint configuration space according to the initial 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 the root node of the initial activity tree is the starting point of the joint configuration space; inserting random nodes into the initial active tree according to the rapid expansion random tree algorithm, and judging whether the updated active tree meets the initial dormancy tree or not; if the two paths meet, executing a path planning module; if not, taking the updated activity tree as an initial activity tree, and executing a dormancy tree generating module;

The activity tree generation module inserts random nodes on the initial activity tree T _a comprising the following sub-steps:

Step a1: generating a random node p _rand in the joint configuration space, judging the feasibility of p _rand, and executing the step a2 if p _rand meets the feasibility; otherwise, returning to the execution step a1 to generate a new random node;

Step a2: calculating Euclidean distances between p _rand and all nodes on T _a, and obtaining a node p _nearest on T _a with the minimum Euclidean distance;

Obtaining a connection line between p _nearest and p _rand, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as random nodes p _new, and executing the step a3; otherwise, returning to the execution step a1 to generate a new random node;

Step a3: acquiring a connection line E _new between p _nearest and p _new, judging the feasibility of the connection line E _new, adding p _new into T _a if E _new meets the feasibility, and executing step a4; otherwise, returning to the execution step a1 to generate a new random node;

Step a4: calculating Euclidean distances between p _new and all nodes on the initial dormancy tree T _b, and obtaining a node p _nearesto on a T _b with the minimum Euclidean distance;

Obtaining a connection line between p _nearesto and p _new, judging the feasibility of all points on the connection line, if nodes meeting the feasibility exist, marking the nodes as p _newo, and executing the step a5; otherwise, executing a dormancy tree generating module;

step a5: acquiring a connection line E _newo between p _newo and p _new, performing feasibility judgment on the connection line E _newo, and if E _newo meets the feasibility judgment, updating p _new to p _newo to obtain an updated initial activity tree; otherwise, executing a dormancy tree generating module;

the method comprises the steps of judging the feasibility of the point and judging the feasibility of the connecting line, wherein the feasibility algorithm of the point comprises the following steps:

the feasibility of the random node prand is judged by a collision detection algorithm, if the feasibility of the random node prand is satisfied The feasibility is satisfied wherein OBS represents a space constituted by an obstacle;

The link feasibility algorithm comprises the following steps:

ca1: the connecting line is equally divided into m discrete points by the following,

Wherein p _ij represents the j-th axis coordinate of the i-th discrete point on the line; p _startj represents the j-th axis coordinate of the connection start point p _start, p _endj represents the j-th axis coordinate of the connection end point p _end, and m is a positive integer;

ca2: judging the feasibility of all m discrete points by using a point feasibility algorithm collision detection algorithm; if any point is not satisfied The connection is deemed not viable;

The parent node of the random node on the initial activity tree T _a is selected according to the lowest distance cost in the cost optimization module, which comprises the following sub-steps:

Step b1: the Euclidean distance between any node on the T _a and the random node P _rand is calculated, and the nodes corresponding to the Euclidean distance smaller than the threshold value of the adjacent point distance are combined into an adjacent point set P _near;

Step b2: traversing all nodes in the adjacent point set P _near to judge until the parent node selection of P _new is finished when all nodes are traversed;

For the node P _neari currently judged in P _near, if P _neari meets the formula I, deleting the connection line between P _new and the current parent node, adding the current parent node updated to P _new by P _neari into the initial active tree, and traversing the next node; otherwise, traversing the next node;

D (p _neari,p_new)+Cost_neari＜D(p_curpar,p_new)+Cost_curpar formula I)

Wherein D (p _neari,p_new) represents the euclidean distance between p _neari and p _new, cost _neari represents the Cost of moving from the root node to p _neari, p _curpar represents the current parent node of p _new, D (p _curpar,p_new) represents the euclidean distance between current parent nodes p _curpar and p _new, cost _curpar represents the Cost of moving from the active tree root node to p _curpar;

The cost optimization module selects the child node of the random node on the T _a according to the lowest distance cost, and the child node comprises the following sub-steps:

Step c1: obtaining connection lines between all nodes in the adjacent point set P _near and P _new respectively, judging the feasibility of all the connection lines in sequence, obtaining a collision-free adjacent point set P _nocol, wherein the P _nocol comprises all the nodes in P _near corresponding to the connection lines meeting the feasibility,

Step c2: traversing all nodes in the collision-free adjacent point set P _nocol to judge until all the sub-nodes of P _new are selected to be finished when all the nodes are traversed;

For the node P _nocoli currently judged in P _nocol, if P _nocoli meets the formula II, deleting the connection line between P _nocoli and the current parent node, adding the current parent node updated to P _nocoli by P _new into the initial active tree, and traversing the next node; otherwise, traversing the next node;

d (p _nocoli,p_new)+Cost_new＜Cost_nocoli type II)

Where D (p _nocoli,p_new) represents the Euclidean distance between p _nocoli and p _new, cost _new represents the Cost of moving from the root node to p _new, and Cost _nocoli represents the Cost of moving from the root node to point p _nocoli;

The dormancy tree generating module is used for establishing an initial dormancy tree, and a root node of the initial dormancy tree is a target point of the joint configuration space; inserting random nodes into the initial dormancy tree according to a rapid expansion random tree algorithm to obtain an updated dormancy tree, and judging whether the initial activity tree meets the updated dormancy tree or not; if the two paths meet, executing a path planning module; if not, taking the updated dormancy tree as an initial dormancy tree, and executing an activity tree generation module;

The cost optimization module is used for selecting father nodes and child nodes of the random nodes on the initial active tree according to the lowest distance cost and selecting father nodes and child nodes of the random nodes 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 dormancy tree as planned paths according to the active tree generating module; and the path passing through all nodes between the root node of the initial active tree and the root node of the updated dormancy tree is obtained as a planned path according to the dormancy tree generating module.