Mechanical arm obstacle avoidance path planning method based on improved RRT* algorithm

Abstract

A mechanical arm obstacle avoidance path planning method based on an improved RRT * algorithm comprises the steps that firstly, sampling points are controlled and generated through a dynamic ellipsoid sampling strategy, and then a dynamic target direct connection strategy, a random sampling expansion strategy and an improved artificial potential field strategy are tried to be carried out in sequence; when the requirements of a vector threshold value and collision detection are not met after node expansion is carried out on the front-order expansion strategy, the rear-order expansion strategy is carried out, the sequence of the three strategies belongs to a logic chain of efficiency priority (direct connection troubleshooting)-feasibility guarantee (sampling exploration)-optimality optimization (artificial potential field smoothing), the front-order strategy provides necessary input for the rear-order strategy, and the rear-order strategy provides necessary input for the rear-order strategy. The problem that the preorder strategy is not covered is solved through the postorder strategy, the optimal selection of function complementation and risk avoidance in dynamic environment path planning is achieved, the path planning efficiency can be improved, the search time is remarkably shortened, the search difficulty is reduced, the situation of local optimal traps or path failure is avoided, and efficient and safe path planning is achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm obstacle avoidance, and particularly relates to a mechanical arm obstacle avoidance path planning method based on an improved RRT* algorithm. BACKGROUND

[0002] With the continuous innovation and breakthrough of computer technology and intelligent technology, the application field of robots presents an intelligent and diversified development trend, and higher requirements are put forward for the comprehensive performance standards of robots. Robot motion planning problem is concerned due to its application in important fields such as rescue action, assembly plan and biological engineering. In the motion planning scene of mobile robots, Dijkstra algorithm and A* algorithm based on graph search are common methods. Although A* algorithm and the like can find the optimal path, in high-dimensional space, the complexity of such algorithms will be significantly improved, accompanied by a sharp increase in computational amount, which makes them not suitable for robot motion planning. In contrast, the motion planning method based on random sampling can effectively solve these problems.

[0003] The rapid exploring random tree algorithm (RRT) based on random sampling is a classical robot path planning algorithm, which is widely used in the field of mechanical arm obstacle avoidance path planning due to its efficient exploration ability in high-dimensional space and strong exploration characteristics in the global range. The RRT* algorithm, as an improved and optimized version of the RRT algorithm, has the asymptotic optimal characteristic, that is, as the number of sampling points increases, the path searched by the algorithm will gradually approach the optimal path. However, since the sampling points are generated by a random uniform sampling algorithm, there is a lack of target-oriented mechanism, and most of the sampling points may deviate from the target area, thereby causing slow convergence speed of the algorithm; the algorithm uses a fixed step length for searching, and in an open large-scale space, the fixed step length will lead to a decrease in expansion efficiency, while in a narrow channel environment, the step length setting is easy to make the algorithm skip the feasible path and appear the "lock road" phenomenon; the initial path generated by the RRT* algorithm usually has the problems of node redundancy and many bends, which need to be optimized to reduce redundant nodes and smooth the path bends, so that it is more consistent with the path requirements in actual execution. SUMMARY

[0004] The purpose of the present application is to provide a mechanical arm obstacle avoidance path planning method based on an improved RRT* algorithm, which can significantly shorten the search time and reduce the search difficulty, and achieve path smoothing while simplifying the path.

[0005] The technical scheme adopted by the present application to solve the above technical problems is: a mechanical arm obstacle avoidance path planning method based on an improved RRT* algorithm, comprising the following steps: S1, establish a mechanical arm collision motion model and initialize parameters, obtain the mechanical arm workspace obstacle distribution and establish a three-dimensional space map model, and obtain the starting point init and the ending point goal of the mechanical arm motion path; S2, a dynamic ellipsoid sampling strategy is used to control the generation of sampling points S201, the semi-major axis and semi-minor axis of the ellipsoid are initialized, and the formula is: ; In the above formula, a0 is the initial value of the semi-major axis, and the value of a0 is not less than the distance between the starting point qinit and the target point q goal ; b0 is the initial value of the semi-minor axis; is the total number of obstacles in the three-dimensional space map model; is the number of obstacles passed by the line path connecting the starting point q init and the target point q goal ; is the proportion of the total volume of the obstacle in the total volume of the space; S202, the semi-major axis and semi-minor axis of the ellipsoid are iterated multiple times, and the formula is: ; ; In the above formula, is the environmental complexity factor selected according to the obstacle; Substitute a0 and b0 into the iteration formula, until the iteration reaches the threshold value, and the semi-major axis and semi-minor axis obtained by the last iteration are used to establish the ellipsoid space; S203, a spherical coordinate system based on a unit sphere is established in the ellipsoid space, and a random point located inside the unit sphere is generated in the spherical coordinate system. The random point coordinate value includes azimuth angle θ, polar angle φ and radius r, and is expressed as: ; In the above formula, is a function that can generate a uniformly distributed random number in the interval [0, 1]; Then, the spherical coordinate is transformed into Cartesian coordinate, and the spherical coordinate value is converted into Cartesian coordinate value to obtain the random point coordinate value located inside the unit sphere, and the formula is: ; S204, coordinate transformation is performed on the random point located inside the unit sphere. First, a local coordinate system is established: ; In the above formula, u, v and w are all basis vectors, and vector w is obtained by the cross product of vector u and vector v, and vector u is obtained by normalizing the vector from the starting point init to the vector pointing to the ending point goal, denotes the Euclidean distance, u x is the component of u in the x-axis direction, u y is the component of u in the y-axis direction, u xy is the projection of u on the xy plane, and ε is a threshold value, which is a positive number; Then a rotation matrix is constructed by vectors u, v, and w: ; In the above formula, denotes the three-dimensional special orthogonal group; Then the sampling points are generated by using the rotation matrix: ; In the above formula, is the translation center of the translation vector, a is the semi-major axis obtained by S202 in the last iteration, b is the semi-minor axis obtained by S202 in the last iteration, and c is the semi-axis length of the ellipsoid in the direction perpendicular to the plane in which the semi-major axis a and the semi-minor axis b are located; S3, node expansion is performed by using a dynamic target direct connection strategy S301, the start point init and the end point goal are taken as root nodes respectively, and two random trees are generated by performing node expansion multiple times, and the process of node expansion is represented as: ; ; ; In the above formula, d is a direction vector, q goal is a target point of node expansion, and the sampling points generated by S2 are selected, q near is the node in the random tree closest to q goal , q new is a new node after the expansion of the random tree, d unit is an expansion step length, and η is a preset step length, which is determined based on the scale of the environment and the density of obstacles; S302, after each node expansion, the direction vector d is compared with a threshold value d min , when d < d min , step S4 is performed; When d > d min , collision detection is performed on the random tree and the obstacles, when collision occurs, the node expansion is cancelled, and step S4 is performed; When no collision occurs, the parent node is reconnected and rewired, and then step S6 is performed; S4, node expansion is performed by using a random sampling expansion strategy S401, random points q rand: q rand = [ x rand , y rand , z rand ] ; x rand : U(0, x max ) ; y rand : U(0, y max ) ; z rand : U(0, z max ) ; In the above formula, U(a, b) represents a uniform distribution in the interval [a, b], x max , y max , z max represent the spatial dimensions of the environment; S402, select the node q rand closest to the random point q nearest from the random tree, then generate a direction vector d from q nearest to q rand , and expand the node: ; When , directly take q rand as the new node q new ; S403, after each node expansion, compare the direction vector d with the threshold d min , when d < d min , proceed to step S5; When d > d min , collision detection is performed between the random tree and the obstacle, when collision occurs, the node expansion is revoked, and step S5 is performed; When there is no collision, reconnect the parent node and rewire, then proceed to step S6; S5, use the improved artificial potential field strategy to expand the node S501, obtain the total direction vector by synthesis , and take it as the direction vector d of node expansion, the formula is: ; In the above formula, F alt is the synthetic attractive force of the target point and the random point, F rep is the repulsive force generated by the obstacle, and F near represents the repulsive force generated by the surrounding nodes; The calculation formula of the attractive force F alt is: ; In the above formula, q goal is the target point selected by S301, q rand is the random point selected by S401, q current is the current node obtained after the last node expansion of the random tree, a is the attraction force weight of the target point to the current node, and b is the attraction force weight of the random point to the current node; repulsive force F rep of the obstacle; ; In the above formula, F rep1 is the basic repulsive force, and F rep2 is the additional repulsive force, and the calculation formulas are respectively: ; ; In the above formula, k rep is the repulsive force constant of the obstacle, n1 is the control repulsive force coefficient, is the distance from the current node to the surface of the obstacle, is the repulsive force action range, is the distance from the current node to the target, is the direction vector from the surface of the obstacle to the current node; repulsive force F near of the node; ; In the above formula, η repel is the repulsive force constant of the node, is the distance from the current node to the surrounding other nodes, r1 is the repulsive force action radius, is the unit direction vector from the surrounding other nodes to the current node; S502, merge the normalized direction to generate a new node and perform node expansion: ; ; In the above formula, is the total direction vector module; S503, after each node expansion, compare the direction vector d with the threshold d min , when d < d min , return to step S2; when d > d min , perform collision detection between the random tree and the obstacle, when collision occurs, return to step S2; when no collision occurs, reconnect the parent node and rewire, and then proceed to step S6; S6, when the distance between the new nodes of the two random trees is less than the threshold value, and the connection line between the two new nodes does not collide with the obstacle, it is determined that the two new nodes meet, and the two new nodes are connected to form an obstacle avoidance path; When it is determined that the two new nodes do not meet, return to step S2; S7, the path pruning strategy is used to perform path optimization processing on the obstacle avoidance path From the starting point of the obstacle avoidance path, the connection line is sequentially connected with each subsequent trajectory point, and collision detection is performed after each trajectory point connection, until the connection line of a certain trajectory point collides, and then the starting point is connected with the last trajectory point before the trajectory point where the collision occurs without collision; Then, the newly connected trajectory point is taken as the starting point, and the connection line and collision detection of the subsequent trajectory point are continued until the last trajectory point is connected, and the optimized path is obtained; S8, the path pruning strategy optimized path is smoothed by a cubic B-spline curve, and the curve equation is: ; In the above formula, t is a node vector, B0, B1, B2, and B3 are coordinate values of four control points, N 0,3 , N 1,3 , N 2,3 , N 3,3 are B-spline basis functions, and are expressed as: ; The final path point after smoothing is the obstacle avoidance path of the mechanical arm.

[0006] Preferably, the method for reselecting the parent node and rewiring is that a new node q new is taken as the center of a circle, all nodes with a distance less than a radius r2 from q new are searched to obtain a neighbor point set, the node q new with the smallest distance from q min is found from the neighbor point set, and q new is reconnected to the node with the shortest path in the neighbor point set except q min . Then, the cost of each node in the neighbor point set is calculated, the candidate path cost of the neighbor node through the reconnected node to the starting point is compared with the original path cost of the neighbor node, when the path cost of the reconnected node is smaller, the parent node is updated and the path is reconnected.

[0007] According to the above technical scheme, the beneficial effects of the present application are: The application firstly controls the generation of sampling points through a dynamic ellipsoid sampling strategy, and then sequentially attempts a dynamic target direct connection strategy, a random sampling expansion strategy and an improved artificial potential field strategy. When the node expansion of the previous sequence expansion strategy does not meet the requirements of the vector threshold and collision detection, the subsequent expansion strategy is performed. The order of the three strategies belongs to the logic chain of "efficiency first (direct connection investigation) → feasibility guarantee (sampling exploration) → optimality optimization (artificial potential field smoothing)". The previous strategy provides necessary input for the subsequent strategy, and the subsequent strategy solves the problems not covered by the previous strategy. The order of the three strategies realizes the optimal selection of function complementation and risk avoidance in dynamic environment path planning, can improve the path planning efficiency, significantly shorten the search time, reduce the search difficulty, avoid the occurrence of local optimal trap or path failure, achieve path smoothing processing while simplifying the path, and make the end of the mechanical arm smoothly avoid obstacles during operation, thereby realizing efficient and safe path planning. BRIEF DESCRIPTION OF DRAWINGS

[0008] Figure 1 The algorithm flowchart of the application is shown in Figure Figure 2 The path segment optimization schematic diagram is shown in Figure Figure 3 The process schematic diagram of path search in the embodiment is shown in Figure Figure 4 The initial obstacle avoidance path schematic diagram not subjected to optimization processing in the embodiment is shown in Figure Figure 5 The obstacle avoidance path schematic diagram subjected to B-spline curve smoothing processing in the embodiment is shown in Figure DETAILED DESCRIPTION

[0009] Referring to the drawings, the specific implementation is as follows: As shown in Figure Figure 1 , a mechanical arm obstacle avoidance path planning method based on an improved RRT* algorithm comprises the following steps: S1, a mechanical arm collision motion model is established and parameters are initialized, the obstacle distribution of the mechanical arm working space is obtained, a three-dimensional space map model is established, and the starting point init and the terminal point goal of the mechanical arm motion path are obtained.

[0010] S2, a dynamic ellipsoid sampling strategy is used to control the generation of sampling points S201, the semi-major axis and semi-minor axis of the ellipsoid are initialized, and the formula is: ; In the above formula, a0 is the initial value of the semi-major axis, and the value of a0 is not less than the distance between the starting point qinit and the target point q goal ; b0 is the initial value of the semi-minor axis; is the total number of obstacles in the three-dimensional space map model; The number of obstacles that the connecting path of the starting point q init and the target point q goal passes through; The proportion of the total volume of the obstacles in the total volume of the space.

[0011] S202, multiple iterations are performed on the semi-major axis and semi-minor axis of the ellipsoid, and the formula is: ; ; In the above formula, is the environmental complexity factor selected according to the obstacles.

[0012] Substitute a0 and b0 into the iteration formula, and after the number of iterations reaches the threshold value, the semi-major axis and semi-minor axis obtained by the last iteration are used to establish the ellipsoid space.

[0013] S203, a spherical coordinate system based on a unit sphere is established in the ellipsoid space, and a random point inside the unit sphere is generated in the spherical coordinate system. The random point coordinate values include azimuth angle θ, polar angle φ and radius r, and are expressed as: ; In the above formula, is a function that can generate a uniformly distributed random number in the interval [0, 1].

[0014] Then, the spherical coordinate value is converted into Cartesian coordinate value through spherical coordinate to Cartesian coordinate transformation, and the random point coordinate value inside the unit sphere is obtained, and the formula is: .

[0015] S204, coordinate transformation is performed on the random point inside the unit sphere. First, a local coordinate system is established: ; In the above formula, u, v and w are all basis vectors, vector w is obtained by the cross product of vector u and vector v, and vector u is obtained by normalizing the vector from the starting point init and the vector pointing to the end point goal . represents the Euclidean distance, u x is the component of u in the x-axis direction, u y is the component of u in the y-axis direction, u xy is the projection of u in the xy plane, and ε is a threshold value, which is a positive number.

[0016] Then, a rotation matrix is constructed by vectors u, v and w: ; In the above formula, represents a three-dimensional special orthogonal group.

[0017] Then the sampling points are generated by using the rotation matrix: ; In the above formula, is the translation center of the translation vector, a is the semi-major axis obtained by the last iteration of S202, b is the semi-minor axis obtained by the last iteration of S202, and c is the semi-axis length of the ellipsoid in the direction perpendicular to the plane in which the semi-major axis a and the semi-minor axis b are located.

[0018] S3, node expansion is performed by using a dynamic target direct connection strategy S301, the start point init and the end point goal are taken as root nodes respectively, two random trees are generated by performing node expansion multiple times, and the process of node expansion is represented as: ; ; ; In the above formula, d is a direction vector, q goal is a target point of node expansion, the sampling point generated by S2 is selected, q near is a node in the random tree closest to q goal , q new is a new node after the expansion of the random tree, d unit is an expansion step, and η is a preset step length, which is determined based on the scale of the environment and the density of obstacles.

[0019] The value range and principle of the preset step length η are mainly related to the following factors: Matching the scale of the environment: the overall size of the planning environment needs to be considered. If the environment is large, η should be appropriately large, so as to improve the search efficiency and quickly cover a large space; if the environment is small and the obstacles are dense, η should be small, so as to search the path between the obstacles more finely and avoid being unable to find a feasible path due to too large step length.

[0020] Obstacle density: when the obstacle density is high, a small η helps to find a path in a narrow channel or a complex obstacle layout; when the obstacle density is low, η can be appropriately increased to speed up the search.

[0021] Balance between calculation efficiency and path accuracy: the smaller η is, the more path nodes are generated, the higher the path accuracy is, but the larger the calculation amount is; the larger η is, the higher the calculation efficiency is, but the path may not be fine enough, or even may not be able to accurately bypass the obstacles.

[0022] S302, after each node expansion, the direction vector d is compared with a threshold value d min , and when d < d min , step S4 is performed.

[0023] When d>d min , collision detection is performed between the random tree and the obstacle, when collision occurs, the node expansion is revoked, and step S4 is performed.

[0024] When no collision occurs, the parent node is reconnected and rewired, and then step S6 is performed.

[0025] S4, node expansion is performed by using a random sampling expansion strategy S401, uniform random sampling in the configuration space C is performed to generate a random point q rand : q rand =[ x rand , y rand , z rand ] ; x rand :U(0,x max ) ; y rand :U(0,y max ) ; z rand :U(0,z max ) ; In the above formula, U(a,b) represents uniform distribution in the interval [a,b], x max , y max , z max represent the spatial dimensions of the environment.

[0026] S402, the node q rand closest to the random point q nearest is selected from the random tree, and a direction vector d is generated from q nearest to q rand for node expansion: ; When , q rand is directly taken as the new node q new .

[0027] S403, after each node expansion, the direction vector d is compared with the threshold value d min , when d min , step S5 is performed.

[0028] When d>d min , collision detection is performed between the random tree and the obstacle, when collision occurs, the node expansion is revoked, and step S5 is performed.

[0029] When no collision occurs, reconnect the parent node and rewire, and then proceed to step S6.

[0030] S5, node expansion using improved artificial potential field strategy S501, total direction vector is obtained by synthesis , and it is used as the direction vector d of node expansion, the formula is: ; In the above formula, F alt is the synthetic attractive force of the target point and the random point, F rep is the repulsive force generated by the obstacle, and F near represents the repulsive force generated by the surrounding nodes.

[0031] The calculation formula of the attractive force F alt is: ; In the above formula, q goal is the target point selected in S301, q rand is the random point selected in S401, q current is the current node obtained after the last node expansion of the random tree, and α is the attractive force weight of the target point and the current node, and β is the attractive force weight of the random point and the current node.

[0032] The calculation formula of the obstacle repulsive force F rep is: ; In the above formula, F rep1 is the basic repulsive force, and F rep2 is the additional repulsive force, and the calculation formulas are respectively: ; ; In the above formula, k rep is the obstacle repulsive force constant, n1 is the control repulsive force coefficient, is the distance from the current node to the surface of the obstacle, is the repulsive force action range, is the distance from the current node to the target, is the direction vector from the surface of the obstacle to the current node.

[0033] The calculation formula of the node repulsive force F near is: ; In the above formula, η repel is the node repulsive force constant, is the distance between the current node and other nodes, and r1 is the repulsive force action radius, is a unit direction vector pointing from the current node to the surrounding other nodes.

[0034] S502, merge the direction normalization to generate a new node and expand the node: ; ; In the above formula, is the total direction vector module.

[0035] S503, after each node expansion, compare the direction vector d with the threshold value d min When d < d min , return to step S2.

[0036] When d > d min , perform collision detection between the random tree and the obstacle, and when a collision occurs, return to step S2.

[0037] When no collision occurs, reconnect the parent node and rewire, and then proceed to step S6.

[0038] S6, when the distance between the new nodes of the two random trees is less than the threshold value, and the connection between the two new nodes does not collide with the obstacle, it is determined that the two new nodes meet, and the two new nodes are connected to form an obstacle avoidance path.

[0039] When it is determined that the two new nodes do not meet, return to step S2.

[0040] S7, path optimization processing is performed on the obstacle avoidance path using a path pruning strategy Starting from the starting point of the obstacle avoidance path, sequentially connect each subsequent trajectory point, and perform collision detection after each connection, until a collision occurs when connecting a certain trajectory point. The last trajectory point before the collision that does not collide is connected to the starting point and the trajectory point that collides.

[0041] Then take the newly connected trajectory point as the starting point, continue to connect the subsequent trajectory points and perform collision detection, until the last trajectory point is connected, and the optimized path is obtained.

[0042] S8, smooth the path optimized by the path pruning strategy by a cubic B-spline curve, and the curve equation is: ; In the above formula, t is the node vector, B0, B1, B2, B3 are the coordinate values of the four control points, N 0,3 , N 1,3 , N 2,3 , N 3,3 are B-spline basis functions, which are represented as: .

[0043] The final path point after smoothing is the obstacle avoidance path of the robot arm.

[0044] The method of reselecting the parent node and rewiring is to connect the new node q new to all nodes within a radius r2 of q new to obtain a set of neighboring nodes, and to find the node q new with the smallest distance from q min , and to reconnect q new to the node with the shortest path in the set of neighboring nodes other than q min .

[0045] Then, the cost of each node in the set of neighboring nodes is calculated, and the candidate path cost of the neighboring node through the reconnected node to the starting point is compared with the original path cost of the neighboring node. When the path cost of the reconnected node is smaller, the parent node is updated and the path is reconnected.

[0046] As shown in Figure 2 , the original path is the solid line part, point 1 is connected to point 5 to collide, point 1 is connected to point 6 to collide, point 1 is connected to point 4 to not collide, and point 4 is connected to point 6 to not collide, and the final optimized path is the dotted line part, and points 2, 3, and 5 are redundant nodes.

[0047] In this embodiment, the sampling points are first generated by a dynamic ellipsoid sampling strategy, and then the dynamic target direct connection strategy, the random sampling expansion strategy, and the improved artificial potential field strategy are tried in sequence. When the node expansion of the current sequence expansion strategy does not meet the requirements of the vector threshold and collision detection, the subsequent expansion strategy is performed.

[0048] The dynamic ellipsoid sampling strategy, the dynamic target direct connection strategy, the random sampling expansion strategy, and the improved artificial potential field strategy belong to the progressive relationship of "the previous strategy provides necessary input for the subsequent strategy, and the subsequent strategy iteratively optimizes the previous result". Once this dependency chain is broken, the subsequent strategy will not take effect due to the lack of processing objects. The specific dependency logic is as follows: The dynamic ellipsoid sampling strategy generates sampling points, providing necessary input for subsequent strategies. The size of the ellipsoid will be dynamically adjusted according to the search progress and the complexity of the environment space.

[0049] 1. The first step must be the dynamic target direct connection strategy to avoid invalid work of subsequent strategies. The core value of the dynamic target direct connection strategy is "preferentially checking the optimal shortcut" - if there is no obstacle between the current position and the dynamic target, the direct connection path can be output directly, without the need to start the subsequent complex sampling and optimization process, saving computing resources.

[0050] If the dynamic target direct connection strategy is skipped and directly enters the random sampling expansion strategy, it will cause a lot of time to be spent on sampling and generating redundant paths, although there is a clear unobstructed direct connection path, and the planning efficiency will be severely reduced.

[0051] If the improved artificial potential field strategy is directly entered, the artificial potential field will guide the path to move towards the target, but since the direct connection feasibility is not judged in advance, unnecessary potential field optimization may still be performed in the absence of obstacles, wasting resources.

[0052] 2. The second step must be the random sampling expansion strategy, which provides an initial feasible path for the improved artificial potential field strategy to avoid falling into local optimum.

[0053] The improved artificial potential field strategy still has a "local optimum trap" - if the "start point - target" is directly used as the initial guide, when the obstacle distribution is complex (such as "concave obstacle"), the artificial potential field's attractive force and repulsive force may cancel each other out, causing the robot to stop at a local optimal position and unable to reach the target.

[0054] The core role of the random sampling expansion strategy is to "explore the global feasible region and generate an initial path", which breaks the local spatial limit through random sampling to ensure that the generated initial path can bypass complex obstacles and connect the start point and the target. The improved artificial potential field strategy only needs to optimize (such as smoothing the path and shortening the distance) based on the initial feasible path, without the need to explore whether it can reach the target, fundamentally avoiding the local optimum problem.

[0055] If the random sampling expansion strategy is skipped and the improved artificial potential field strategy is directly used to connect the dynamic target direct connection strategy, when the direct connection path has a collision, the artificial potential field loses the initial path guidance and is easily trapped in a local optimum, leading to planning failure.

[0056] 3. The third step must be the improved artificial potential field strategy, which serves as an optimization link and cannot replace the previous exploration link. The essence of the improved artificial potential field strategy is a path optimizer, not a path explorer - it needs to smooth and optimize based on an existing feasible path, and it does not have the ability to explore unknown feasible regions.

[0057] If the improved artificial potential field strategy is placed in the second step to replace the random sampling expansion, the artificial potential field will not be able to generate an initial path that bypasses complex obstacles, and can only adjust within a local space, either colliding with the path or falling into a local optimum.

[0058] If the improved artificial potential field strategy is placed in the first step to replace the dynamic target direct connection strategy, since there is no initial path guidance, the artificial potential field has no clear direction and cannot generate an effective path.

[0059] Therefore, the order of the three strategies belongs to the logical chain of "efficiency first (direct connection troubleshooting) → feasibility guarantee (sampling exploration) → optimality optimization (artificial potential field smoothing)", the previous strategy provides necessary input for the subsequent strategy, and the subsequent strategy solves the problems not covered by the previous strategy, realizing the optimal choice of complementary functions and risk avoidance in dynamic environment path planning.

[0060] The simulation experiment is performed in Matlab2024b, the obstacles on the map are composed of cubes, cuboids, spheres and cylinders with different shapes and sizes, the starting point coordinates are (10, 10, 20), the ending point coordinates are (140, 140, 80), the search range size is 150*150*100, the step size is 13, and the threshold value is 10, Figure 3 That is, the process diagram of path search.

[0061] Figure 4 is the initial obstacle avoidance path without optimization processing, Figure 5 is the obstacle avoidance path after B-spline curve smoothing processing, by comparing Figure 4 and Figure 5 It can be found that Figure 5 the degree of curvature and length of the path are significantly reduced, the smoothness is higher, there is no corner in the path, and the planning result is more concise. In addition, the path after smoothing processing is still a collision-free path, which can avoid collision between the robot arm and the obstacles in the workspace.

[0062] Practice verification shows that the embodiment can improve the path planning efficiency, significantly shorten the search time, reduce the search difficulty, avoid the occurrence of local optimal trap or path failure, achieve path smoothing while simplifying the path, make the robot arm end smoothly avoid obstacles during operation, and thus realize efficient and safe path planning.

Claims

1. A mechanical arm obstacle avoidance path planning method based on an improved RRT* algorithm, characterized in that, The method comprises the following steps: S1, a mechanical arm collision motion model is established and parameters are initialized, an obstacle distribution of a mechanical arm workspace is obtained, a three-dimensional space map model is established, and a starting point init and an ending point goal of a mechanical arm motion path are obtained; S2, a dynamic ellipsoid sampling strategy is used to control the generation of sampling points S201, the semi-major axis and semi-minor axis of the ellipsoid are initialized, and the formula is: ； In the above formula, a0 is an iterative initial value of the semi-major axis, and the value a of a0 is not less than the distance between the starting point qinit and the target point q goal ; b0 is an iterative initial value of the semi-minor axis; is the total number of obstacles in the three-dimensional space map model; is the number of obstacles through which the connecting path of the starting point q init and the target point q goal passes; is the proportion of the total volume of obstacles in the total volume of space. S202, the semi-major axis and semi-minor axis of the ellipsoid are iterated for multiple times, and the formula is: ； ； In the above formula, is an environmental complexity factor selected according to the obstacle; a0 and b0 are substituted into the iteration formula, and the semi-major axis and semi-minor axis obtained through the last iteration are used to establish the ellipsoid space after the iteration number reaches a threshold value; S203, a spherical coordinate system based on a unit sphere is established in the ellipsoid space, and a random point inside the unit sphere is generated in the spherical coordinate system, and the random point coordinate value includes an azimuth angle θ, a polar angle φ and a radius r, and is expressed as: ； In the above formula, is a function that generates a uniformly distributed random number in the interval [0, 1]. Then, the spherical coordinate to Cartesian coordinate transformation is performed, the spherical coordinate value is converted into Cartesian coordinate value, and the random point coordinate value inside the unit sphere is obtained, and the formula is: ； S204, coordinate transformation is performed on the random point inside the unit sphere, a local coordinate system is first established: ； In the above formula, u, v, w are basis vectors, vector w is the cross product of vector u and vector v, vector u is the vector from the starting point init and the vector pointing to the end point goal is normalized to get, represents the Euclidean distance, u x is the component of u in the x-axis direction, u y is the component of u in the y-axis direction, u xy is the projection of u on the xy plane, and ε is a threshold value, which is a positive number; Then, a rotation matrix is constructed through vectors u, v and w: ； In the above formulae, denotes the three-dimensional special orthogonal group; Then, the rotation matrix is used to generate a sampling point: ； In the above formula, is the translation center of the translation vector, a is the semi-major axis obtained by the last iteration of S202, b is the semi-minor axis obtained by the last iteration of S202, and c is the semi-axis length of the ellipsoid in the direction perpendicular to the plane in which the semi-major axis a and the semi-minor axis b lie. S3, a dynamic target direct connection strategy is used for node expansion S301, the starting point init and the ending point goal are taken as root nodes, two random trees are generated through multiple node expansions, and the node expansion process is expressed as: ； ； ； In the above formula, d is a direction vector, q goal is a target point of node expansion, and the sampling point generated by S2 is selected, q near为随机树中与qgoal距离最近的节点，qnew is a new node after random tree expansion, d unit为扩展步长，η为预设步长，基于环境尺度和障碍物密度进行取值； S302. After each node expansion, the direction vector d and the threshold d are... min Compare the results; when d < d min Then, proceed to step S4; When d > d min When a collision occurs, the node expansion is cancelled and step S4 is performed. When no collision occurs, the parent node is reconnected and rewired, and then step S6 is performed; S4, a random sampling expansion strategy is used for node expansion S401. Uniformly random sampling in the configuration space C to generate a random point q rand : q rand = [ x rand , y rand , z rand ] ; x rand : U(0, x max ) ; y rand : U(0,y max ) ; z rand : U(0,z max ) ; In the above formula, U(a,b) represents a uniform distribution in the interval [a,b], x max , y max , z max represent the spatial dimensions of the environment; S402、Select a random point q from the random tree rand The nearest node q nearest Then generate a direction vector d from q nearest To q rand Direction generates a direction vector d, and expands the node: ； When q is directly taken rand as a new node q new ; S403. After each node expansion, the direction vector d and the threshold d are... min Compare the results; when d < d min Then proceed to step S5; When d > d min When d > d min When d > d min When d > d min When d > d min When d > d min When d > d min When d > d min When d > d min When d > d < When no collision occurs, the parent node is reconnected and rewired, and then step S6 is performed; S5, an improved artificial potential field strategy is used for node expansion S501, obtain the total direction vector by synthesis and take it as the direction vector d of the node expansion, formula: ； In the above formula, F alt is the resultant attractive force of the goal point and the random point, F rep is the repulsive force generated by the obstacle, F near represents the repulsive force generated by the surrounding nodes; Attractive force F alt The calculation formula is: ； In the above formula, q goal is the target point selected by S301, q rand is the random point selected by S401, q current is the current node obtained after the last node expansion of the random tree, a is the attraction weight of the target point and the current node, and β is the attraction weight of the random point and the current node. Obstacle repulsion force F rep The calculation formula is: ； In the above formula, F rep1 is the basic repulsive force, F rep2 is the additional repulsive force, and the calculation formulas are respectively: ； ； In the above formula, k rep is the obstacle repulsion constant, n1 is the control repulsion coefficient, is the distance from the current node to the obstacle surface, is the repulsion range, is the distance from the current node to the target, is the direction vector from the obstacle surface to the current node; Node repulsive force F near The calculation formula is: ； In the above formula, η repel is the repulsion constant of the node, is the distance between the current node and the surrounding other nodes, r1 is the repulsion radius, is the unit direction vector from the surrounding other nodes to the current node; S502, the direction is normalized after merging, a new node is generated, and node expansion is performed: ； ； In the above formula, is the total directional vector modulus; S503. After each node expansion, the direction vector d and the threshold d are... min Compare the results; when d < d min Then, return to step S2; When d > d min a collision detection is performed between the random tree and the obstacle, and when a collision occurs, the process returns to step S2. When no collision occurs, the parent node is reconnected and rewired, and then step S6 is performed; S6, when the distance between the new nodes of the two random trees is less than a threshold value, and the connection line of the two new nodes does not collide with the obstacle, it is determined that the two new nodes meet, and the two new nodes are connected to form an obstacle avoidance path; When it is determined that the two new nodes do not meet, return to step S2; S7, a path pruning strategy is used to optimize the obstacle avoidance path From the starting point of the obstacle avoidance path, each subsequent trajectory point is connected in turn, and collision detection is performed after each trajectory point connection, until the connection line of a certain trajectory point collides, and the last trajectory point without collision before the starting point and the trajectory point with collision is connected; Then, the newly connected trajectory point is taken as the starting point, and the connection and collision detection of the subsequent trajectory points are continued until the last trajectory point is connected, and the optimized path is obtained; S8, the path optimized by the path pruning strategy is smoothed through a cubic B-spline curve, and the curve equation is: ； In the above formula, t is a node vector, B0, B1, B2, B3 are coordinate values of four control points, respectively, N 0,3 , N 1,3 , N 2,3 , N 3,3 are B-spline base functions, and are expressed as: ； The final path point after smoothing is the mechanical arm obstacle avoidance path.

2. The robot obstacle avoidance path planning method based on the improved RRT* algorithm according to claim 1, characterized in that: The method of reselecting a parent node and rewiring is to select a new node q new As the center of a circle, search for all nodes within a distance less than a radius r2 and obtain a set of near nodes, and find the node q new with the minimum distance from q new from the set of near nodes; q min is connected to the node with the shortest path from q new except q min ; Then, for each node in the set of near-neighbor points, a cost calculation is performed, comparing the candidate path cost of the near-neighbor node through the reconnected node to the start point with the original path cost of the near-neighbor node. When the reconnected node path cost is lower, the parent node is updated and the path is reconnected.

Images