CN-120395883-B - Mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans

Abstract

The application discloses a mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans, which relates to the technical field of mechanical arm path planning, and comprises the steps of combining a wavefront algorithm and a weighted undirected graph to determine a wrist point working space communication path of a mechanical arm; the method comprises the steps of constructing two random tree groups, respectively expanding and extending two random trees in any random tree group based on wrist point working space communication paths and combining a principal component analysis algorithm, carrying out collision and communication tests based on a new extending node and another corresponding random tree to obtain a test result when the new extending node is obtained, marking that the random tree group is expanded when the test result represents collision-free communication, and carrying out node backtracking according to the two random tree groups which are expanded to obtain an obstacle-free path of the mechanical arm. The application can improve the path planning efficiency and adaptability in a narrow environment.

Inventors

• LIU GUANYANG
• HUANG GE

Assignees

• 北京航空航天大学

Dates

Publication Date

20260508

Application Date

20250606

Claims (7)

1. 1. The mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans is characterized by comprising the following steps: acquiring a starting point configuration, an ending point configuration and a zero position configuration of the mechanical arm; combining a wavefront algorithm and a weighted undirected graph, and determining a wrist point working space communication path of the mechanical arm according to the starting point configuration, the ending point configuration and the zero position configuration, wherein the method comprises the following steps: Determining a first path planning problem with the starting point configuration as a starting point and the zero configuration as an ending point; For any path planning problem, converting into sub-problems from WP x to WP y , wherein when WP x is a starting point, WP y is an ending point and is a sub-problem, and when WP x is an ending point, WP y is a starting point and is another sub-problem; Aiming at any sub-problem from WP x to WP y , a double-tree path-finding strategy and a wavefront algorithm are adopted to perform path searching to obtain an initial communication path, wherein the initial communication path comprises a plurality of spheres, the sphere center of each sphere refers to the space position of each path point, and the radius of each sphere is the distance between the sphere center and the nearest barrier; sampling the interior of each sphere of the initial communication path to obtain a plurality of sampling point sets; based on a preset cleaning rule, cleaning the sampling points of all the sampling point sets, and then forming a standby sampling point set with WP x 、WP y ; Generating a weighted undirected graph according to the standby sampling point set; Searching the shortest path connecting WP x and WP y in the weighted undirected graph by using Dijkstra algorithm to obtain a wrist point working space communication path of the mechanical arm; constructing four random trees, wherein the first random tree starts with a starting point configuration and takes a zero position configuration as targets, the second random tree starts with a finishing point configuration and takes the zero position configuration as targets, the third random tree starts with the zero position configuration and takes the starting point configuration as targets, and the fourth random tree starts with the zero position configuration and takes the finishing point configuration as targets; dividing the first random tree and the third random tree into a first random tree group, and dividing the second random tree and the fourth random tree into another random tree group; for two random trees in any random tree group, respectively performing expansion and extension based on the wrist point working space communication path and in combination with a principal component analysis algorithm, and performing collision and communication test based on the new extension node and another corresponding random tree when the new extension node is obtained, so as to obtain a test result; and when the test result represents collision-free communication, marking that the random tree groups are expanded, and then carrying out node backtracking according to the two random tree groups with the expanded random tree groups so as to obtain an obstacle-free path of the mechanical arm.
2. 2. The method for planning the obstacle avoidance path of the mechanical arm based on the double-balanced rapid exploration random vine according to claim 1, wherein aiming at any one of the sub-problems from WP x to WP y , a double-tree path-finding strategy and a wavefront algorithm are adopted to perform path searching so as to obtain an initial communication path, and the method comprises the following steps: using WP x as a sphere center to generate a sphere, and calculating a corresponding fitness value; Adding the sphere and the corresponding fitness value as one fitness parameter into a fitness sequence; establishing a ball tree structure comprising a preset root node, and adding the ball body into the ball tree structure as a child node of the preset root node; deleting a sphere and a corresponding fitness value which exist in the fitness sequence and have a sphere center positioned in any sphere in the spherical tree structure, so as to realize one-time updating of the fitness sequence; Selecting a sphere s i with the largest fitness value from the fitness sequence after primary updating, adding the sphere s i into the ball tree structure, and deleting the sphere s i and the corresponding fitness value from the fitness sequence at the same time to realize secondary updating of the fitness sequence; Sampling the surface of the sphere s i to obtain a surface sampling point set; when any surface sampling point exists in the surface sampling point set and is positioned outside all spheres in the spherical tree structure, generating a new sphere according to the surface sampling point, calculating a corresponding new fitness value, and adding the new sphere and the corresponding new fitness value as a new fitness parameter into a fitness sequence after secondary updating so as to realize tertiary updating of the fitness sequence; When the distance between the new sphere and the new sphere corresponding to another sub-problem does not meet the preset condition, returning to the step of deleting the sphere and the corresponding fitness value, which exist in the fitness sequence and are located in any sphere in the sphere tree structure, so as to continue to extend the sphere tree structure; stopping extending the ball tree structure when the distance between the new ball and the new ball corresponding to another sub-problem meets a preset condition, and marking the sub-problem; determining a half path between a starting point and an ending point through node backtracking according to the ball tree structures corresponding to the two sub-problems after marking; And determining an initial communication path according to the half path between the starting point and the ending point corresponding to the two path planning problems.
3. 3. The method for planning the obstacle avoidance path of the mechanical arm based on the double-balance rapid exploration random vine, which is disclosed in claim 1, is characterized in that the steps of respectively expanding and extending are carried out based on the wrist point working space communication path and combined with a principal component analysis algorithm, and comprise the following steps: taking a first sphere in the wrist point working space communication path as a reference point, and exploring based on the reference point and a preset probability to determine a random sampling point; Determining a wrist point p near based on the random sampling point, and extending the wrist point p near to the direction of the random sampling point to obtain a new wrist point p new , wherein the wrist point p near is the wrist point position of a node closest to the random sampling point on a random tree; Reversely solving the new wrist point p new to obtain the first three joint angles of the mechanical arm; Performing collision test based on the first three joint angles of the mechanical arm to obtain a first collision result; When the first collision result represents collision, based on the random sampling point and the wrist point p near , the first three joint angles of the mechanical arm are adjusted by combining a principal component analysis algorithm so as to separate from the collision, and updated first three joint angles are obtained; When the first collision result indicates that no collision occurs, acquiring the last three joint angles of the mechanical arm corresponding to the start, and determining a new node q new according to the first three joint angles and the last three joint angles; Based on the new node q new , carrying out overall collision detection of the mechanical arm to obtain a third collision result; When the third collision result represents collision, acquiring the spatial position before collision and the spatial position during collision of the end effector of the mechanical arm; Based on the jacobian matrix, determining an optimization node according to the spatial position before collision and the spatial position during collision; Based on the optimized node, carrying out overall collision detection of the mechanical arm to obtain a fourth collision result; When the fourth collision result indicates collision, carrying out first updating on the preset probability, and returning to the step of searching based on the reference point and the preset probability to determine random sampling points; when the fourth collision result indicates that no collision occurs, marking the optimized node as a new extension node, then performing second updating on the preset probability, and returning to the step of searching based on the reference point and the preset probability to determine random sampling points; and when the third collision result represents that no collision occurs, carrying out first updating on the preset probability, and returning to the step of searching based on the reference point and the preset probability to determine random sampling points.
4. 4. The method for planning an obstacle avoidance path of a robotic arm based on double-balanced fast exploration random vine according to claim 3, wherein based on the random sampling points and the wrist points p near , in combination with a principal component analysis algorithm, the first three joint angles of the robotic arm are adjusted to be separated from collision, so as to obtain updated first three joint angles, comprising: calculating the original extending direction of the wrist point according to the random sampling points and the wrist point p near ; Generating a three-dimensional sphere by taking the wrist point p near as a sphere center, and sampling the inside of the three-dimensional sphere to obtain an internal sampling point set; For any internal sampling point in the internal sampling point set, performing collision test of the wrist point P near and the internal sampling point, and dividing the internal sampling point into a set P obs or a set P free according to a collision result, wherein the collision results of the internal sampling points in the set P obs are all collision, and the collision results of the internal sampling points in the set P free are all non-collision; Determining peripheral obstacle characteristics of a wrist point P near based on a preset judging rule according to the set P obs and the set P free , and determining a new extending direction of the wrist point by combining the original extending direction of the wrist point, wherein a principal component analysis algorithm is combined in the preset judging rule; Extending based on the new extending direction of the wrist points to obtain optimized wrist points; Reversely solving the optimized wrist points to obtain the first three optimized joint angles of the mechanical arm; performing a collision test based on the first three optimized joint angles of the mechanical arm to obtain a second collision result; If the second collision result indicates that no collision occurs, marking the first three optimized joint angles as updated first three joint angles; If the second collision result represents collision, the preset probability is updated for the first time, and then the method returns to the step of searching based on the reference point and the preset probability to determine random sampling points.
5. 5. The method for planning an obstacle avoidance path of a robotic arm based on double-balanced rapid exploration random vine of claim 4, wherein the preset judgment rule comprises: When the set P obs is an empty set or the number of elements in the set P obs is less than a preset number value, the surrounding obstacle characteristic of the wrist point P near is sparse, the preset probability is updated for the first time, and then the method returns to the step of searching based on the reference point and the preset probability to determine random sampling points; When the number of elements in the set P free is less than a preset number value, the surrounding obstacle characteristics of the wrist point P near are that the wrist point P near is in a state of being surrounded by the obstacle and being difficult to extend, the preset probability is updated for the first time, and then the method returns to the step of searching based on the reference point and the preset probability to determine random sampling points; When the element number in the set P obs is larger than a preset number value, carrying out principal component analysis on the set P obs to determine a first feature vector and a first feature value, taking the mean value of the set P obs as a sphere center, and constructing a confidence ellipsoid E obs according to the first feature vector and the first feature value; When the element number in the set P free is larger than a preset number value, carrying out principal component analysis on the set P free to determine a second characteristic vector and a second characteristic value, taking the mean value of the set P free as a sphere center, and constructing a confidence ellipsoid E free according to the second characteristic vector and the second characteristic value; when all elements in the set P free are not within the confidence ellipsoid E obs , the wrist point P near peripheral obstacle is characterized by the wrist point P near being in the vicinity of a convex obstacle; When the existence elements in the set P free are in the confidence ellipsoid E obs and the mean value of the set P free and the mean value of the set P obs meet a first condition, the peripheral obstacle of the wrist point P near is characterized by the wrist point P near being near a slit entrance or a concave obstacle, wherein the first condition is that the mean value of the set P free is not in the confidence ellipsoid E obs and the mean value of the set P obs is in the confidence ellipsoid E free ; When the existence elements in the set P free are in the confidence ellipsoid E obs and the average value of the set P free and the average value of the set P obs meet a second condition, the peripheral obstacle of the wrist point P near is characterized in that the wrist point P near is in the slit, and the second condition is that the average value of the set P free is in the confidence ellipsoid E obs and the average value of the set P obs is in the confidence ellipsoid E free ; When the existence of elements in the set P free is within the confidence ellipsoid E obs , and the mean of the set P free and the mean of the set P obs do not satisfy the first condition and the second condition, the wrist point P near peripheral obstacle is characterized by the wrist point P near being in the vicinity of the convex obstacle.
6. 6. The method for planning the obstacle avoidance path of the mechanical arm based on the double-balance rapid exploration random vine, which is disclosed in claim 5, is characterized in that the determination process of the new extending direction of the wrist point comprises the following steps: When the peripheral obstacle characteristic of the wrist point p near is that the wrist point p near is near the convex obstacle, determining a new extending direction of the wrist point according to the original extending direction of the wrist point, the first characteristic vector and the first characteristic value; When the peripheral obstacle of the wrist point P near is characterized in that the wrist point P near is near a slit entrance or a concave obstacle, generating two new extending directions of the wrist point, wherein one new extending direction of the wrist point is determined according to the original extending direction of the wrist point, the first characteristic vector and the first characteristic value, and the other new extending direction of the wrist point is determined according to the average value of the set P obs and the wrist point P near ; When the peripheral obstacle of the wrist point p near is characterized in that the wrist point p near is positioned in the slit, determining a new extending direction of the wrist point according to the second feature vector.
7. 7. The method for planning an obstacle avoidance path of a robotic arm based on double-balanced fast-exploration random vine of claim 1, wherein generating a weighted undirected graph according to the set of standby sampling points comprises: For any standby sampling point p i in the standby sampling point set, after traversing the standby sampling point set, if any standby sampling point p j meets a preset graph rule, connecting the standby sampling point p i and the standby sampling point p j by using a weighted edge e ij to form a weighted undirected graph; Wherein, the The preset map rule comprises: The distance between the standby sampling point p i and the standby sampling point p j satisfies Wherein, beta is expansion coefficient, d max is the preset expectation of the minimum distance of the sampling point in the maximum sphere; Wherein r i 、r j is the distance between the standby sampling point p i , the standby sampling point p j and the nearest obstacle, respectively, Presetting sphere overlap ratio; Wherein r min is a preset minimum obstacle distance.

Description

Mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans Technical Field The application relates to the technical field of mechanical arm path planning, in particular to a mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans. Background Obstacle avoidance path planning is an important problem faced by robotic systems during autonomous operation. In a conventional industrial mechanical arm operation environment, obstacles are sparse, and a nuclear industrial mechanical arm has special operation requirements. The mechanical arm for retirement of the nuclear facility needs to enter the facility through the hole with limited size reserved on the outer wall on the premise of not damaging the facility, and key components are operated, which means that the obstacle avoidance path planning algorithm needs to calculate the path for the mechanical arm to enter and exit the hole. In early studies in this area, researchers tended to use graph-based methods. Such algorithms are computationally fast and consume little resources, but they require complex mathematical pre-processing of the obstacle environment, such as converting obstacle information in the task space into the configuration space. These pretreatments are often far more complex than the algorithm itself, require a lot of time and are difficult to run automatically. As computer power increases, researchers have turned to various sampling-based algorithms. The sampling-based algorithm generally does not need to carry out complex mathematical processing on task space or obstacles, and can directly carry out path planning after acquiring all or part of obstacle information. RRT (Rapidly Exploring RandomTree, fast extended random tree algorithm) is a classical algorithm based on sampling, with many variants. However, the existing RRT and variant algorithms are inefficient in a narrow environment and have a weak adaptability to the robot arm. Therefore, there is a need for an efficient and adaptive obstacle avoidance path planning method for mechanical arm design. Disclosure of Invention The application aims to provide a mechanical arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans, which can improve path planning efficiency and adaptability in a narrow environment. In order to achieve the above object, the present application provides the following solutions: the application provides a robot arm obstacle avoidance path planning method based on double-balance rapid exploration of random rattans, which comprises the following steps: acquiring a starting point configuration, an ending point configuration and a zero position configuration of the mechanical arm; combining a wavefront algorithm and a weighted undirected graph, and determining a wrist point working space communication path of the mechanical arm according to the starting point configuration, the ending point configuration and the zero position configuration; constructing four random trees, wherein the first random tree starts with a starting point configuration and takes a zero position configuration as targets, the second random tree starts with a finishing point configuration and takes the zero position configuration as targets, the third random tree starts with the zero position configuration and takes the starting point configuration as targets, and the fourth random tree starts with the zero position configuration and takes the finishing point configuration as targets; dividing the first random tree and the third random tree into a first random tree group, and dividing the second random tree and the fourth random tree into another random tree group; for two random trees in any random tree group, respectively performing expansion and extension based on the wrist point working space communication path and in combination with a principal component analysis algorithm, and performing collision and communication test based on the new extension node and another corresponding random tree when the new extension node is obtained, so as to obtain a test result; and when the test result represents collision-free communication, marking that the random tree groups are expanded, and then carrying out node backtracking according to the two random tree groups with the expanded random tree groups so as to obtain an obstacle-free path of the mechanical arm. According to the specific embodiment provided by the application, the method has the following technical effects that the method combines the wavefront algorithm and the weighted undirected graph, and the wrist point working space communication path of the mechanical arm is determined according to the starting point configuration, the end point configuration and the zero position configuration, wherein the wavefront algorithm is combined with the weighted undirected graph, so that more accurate