Mechanical arm path planning method and system based on adaptive ellipsoid sampling

Abstract

The invention discloses a mechanical arm path planning method and system based on self-adaptive ellipsoid sampling, and the method combines ellipsoid contraction sampling and dynamic step length adjustment, and uniformly samples in an ellipsoid and eliminates collision points by constructing an ellipsoid sampling domain taking a connecting line of a starting point and an end point as a main axis; meanwhile, the expansion step length is adaptively adjusted according to the local obstacle density, and the ellipsoid is dynamically shrunk and expanded in the searching process so as to continuously focus the feasible channel. After an initial path is obtained, path smoothing optimization is carried out by adopting a non-uniform B spline guided by an ellipsoid, and the curvature of the path and the safety distance between the path and an obstacle are comprehensively considered. Verification of the method in a typical complex environment shows that the planning efficiency and the success rate can be remarkably improved, the path smoothness and the safety distance are considered at the same time, and the method is suitable for mechanical arm online obstacle avoidance path planning in the real-time complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a method and system for manipulator path planning based on adaptive ellipsoid sampling. Background Art

[0002] As the dimensionality of the robotic arm's motion space increases, the computational complexity of traditional graph-based search algorithms increases dramatically. For high-dimensional configuration spaces, the RRT (Rapidly Expanding Random Tree) algorithm has gradually become a research focus due to its simple structure and rapid expansion, and has been widely used in robotic arm path planning. However, while RRT's random search strategy helps avoid local optima, it also makes the search process blind, and the planned path is often suboptimal.

[0003] However, the traditional RRT algorithm still has many shortcomings in practical applications. On the one hand, global uniform sampling often produces a large number of redundant samples. In high-dimensional space, most samples fall into obstacles or blank areas, resulting in low expansion efficiency and an increase in the number of iterations. On the other hand, in order to speed up convergence, a fixed probability target sampling bias (Bias-RRT) is often used. However, the fixed bias lacks perception of obstacle distribution and can easily cause the algorithm to fall into local optimality in areas with dense obstacles. In addition, the fixed step size expansion strategy expands slowly in areas with sparse obstacles, and is prone to frequent collisions in areas with dense obstacles, resulting in expansion failure, making it difficult to balance search speed and obstacle avoidance safety. If the traditional multi-stage planning of global fast search and local fine obstacle avoidance is adopted, it is often necessary to manually set the conversion threshold or switching strategy, and the degree of automation is low.

[0004] In summary, there is an urgent need for a new path planning method that can adaptively focus on the sampling area and dynamically adjust the step size according to the density of environmental obstacles to significantly improve planning efficiency and success rate. Summary of the Invention

[0005] The object of the present invention is to provide a robot arm path planning method and system based on adaptive ellipsoid sampling.

[0006] In a first aspect, the present invention provides a method for manipulator path planning based on adaptive ellipsoid sampling, which comprises the following steps: Step 1: Obtain the starting point, target point, and obstacle location information of the robot arm, and construct a search tree with the starting point as the root node; construct an ellipsoid sampling area with the starting point and target point as two reference points; Step 2: Get a random point in the ellipsoid sampling area; take the node closest to the random point in the search tree as the node to be expanded; Step 3: Obtain the moving step length based on the obstacle density; generate candidate new nodes according to the step length along the direction from the node to be expanded to the random point; if the line connecting the candidate new node and the node to be expanded does not intersect with any obstacles, then add the candidate new node to the search tree; otherwise, resample in the ellipsoid sampling area until the line connecting the obtained candidate new node and the node to be expanded does not intersect with any obstacles; Step 4: The midpoint between the candidate new node and the target point is used as the center of the ellipsoid sampling area in the next iteration; if the candidate new node is closer to the target point than the node to be expanded, the ellipsoid sampling area is shrunk, and the major axis and minor axis of the ellipsoid sampling area are multiplied by the shrinkage factor and shrinkage factor ; Step 5: Repeat steps 2 to 4 until the initial path from the starting point to the target point is obtained; Step 6: Use non-uniform B-spline to optimize the initial path and obtain the final smooth obstacle avoidance trajectory.

[0007] Preferably, the method for obtaining the obstacle density is as follows: With the node to be expanded as the center, offset points are generated in multiple equiangular directions. The distance between each offset point and the node to be expanded is the maximum distance allowed for the end of the robotic arm to move. Obstacle density is obtained based on the connection between each offset point and the node to be expanded. , whose expression is: in, is the number of offset points; is the indicator function value; Jordi If the line connecting the offset point and the node to be expanded collides with an obstacle, the indicator function value is 1; otherwise, the indicator function value is 0.

[0008] Preferably, the moving step length The method to obtain is as follows: in, is the basic step length; is the adjustment coefficient; and are the minimum and maximum distances allowed for the end of the robotic arm to move; is the obstacle density.

[0009] As a preference, during the resampling process, if the number of consecutive expansion failures is greater than the expansion failure threshold, the major axis and minor axis of the ellipsoid sampling area are multiplied by the expansion factor and expansion factor , and sample random points in the updated ellipsoid sampling area.

[0010] Preferably, the trajectory optimization process is as follows: Sampling is performed at equal arc length intervals along the initial path to obtain a set of sampling points; high-complexity and low-complexity sections are obtained from the initial path; the maximum and minimum numbers of control points are set respectively, and cubic B-splines with the maximum number of control points are used to fit the high-complexity sections, while quadratic B-splines with the minimum number of control points are used to fit the low-complexity sections; smoothness objectives, safety distance constraints, and path conformality constraints are established for the control points, forming a standard quadratic programming problem, which is solved to obtain a set of control points; the B-spline curves of each section are spliced ​​together to generate the final smooth obstacle avoidance trajectory.

[0011] Preferably, the method for obtaining the high-complexity segment and the low-complexity segment is as follows: The number of ellipsoid contractions at each sampling point is obtained, and the difference between the ellipsoid contraction numbers of adjacent sampling points is used as the contraction number increment; if multiple consecutive contraction number increments are less than the increment threshold, the segment is a high-complexity segment; if multiple consecutive contraction number increments are greater than the increment threshold, the segment is a low-complexity segment.

[0012] Preferably, the standard quadratic programming problem is expressed as: in, is the vector of all control points of the optimized B-spline; is the second-order derivative matrix of the smoothness objective term; is the linear term coefficient vector; is the optimized B-spline control point; is the boundary of obstacles in the environment; is the minimum safety distance value; is the node on the initial path; is the maximum allowable deviation; The smoothness target item J smooth for: in, is the total number of control points.

[0013] Preferably, the center of the ellipsoid sampling area is the midpoint of the two reference points; the major axis is the line connecting the two reference points; and the length of the minor axis is the length of the major axis multiplied by the compression factor.

[0014] Preferably, the method for obtaining random points is: using the Marsaglia method to uniformly sample within the unit sphere to generate a random vector; mapping the random vector to the ellipsoid sampling area in the current iteration process through a linear transformation formula to obtain a random point located within the ellipsoid sampling area; if the random point is located inside the obstacle, the random point is resampled in the ellipsoid sampling area.

[0015] In the second aspect, the present invention provides a robotic arm path planning system based on adaptive ellipsoid sampling, which is used to execute the above-mentioned robotic arm path planning method; the robotic arm path planning system includes a sensor, a path generation module and a path optimization module; the sensor is used to collect environmental information; the path generation module is used to generate an initial path from the starting point to the target point according to the environmental information; the path optimization module smoothes the initial path to generate a final smooth obstacle avoidance trajectory.

[0016] The present invention has the following beneficial effects: 1. The present invention dynamically reduces the sampling domain by shrinking the ellipsoid, concentrating the sampling on feasible paths leading to the target. Simultaneously, the present invention automatically adjusts the moving step size according to obstacle density, balancing efficiency and accuracy in path search. It exhibits high robustness under different obstacle distributions (sparse, clustered, and narrow channels), enabling path search to be concentrated in the target area, effectively improving search efficiency.

[0017] 2. The present invention uses a larger step size for rapid expansion in open areas and a smaller step size for precise obstacle avoidance in areas with dense obstacles, taking into account both search speed and safety, and significantly reducing the number of invalid expansions and collisions. At the same time, the present invention supports constrained projection of the robot arm joint space and is suitable for real-time path planning applications in dynamic or complex industrial environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a flow chart for generating a smooth obstacle avoidance trajectory in the present invention.

[0019] Figure 2 Schematic diagram of the initial ellipsoid in the present invention.

[0020] Figure 3 Schematic diagram of adaptive adjustment step size in the present invention.

[0021] Figure 4 Schematic diagram of the dynamic adjustment process of the elliptical sampling area in the present invention.

[0022] Figure 5 This is the trajectory optimization flow chart of the present invention. DETAILED DESCRIPTION

[0023] The present invention will be further described below with reference to the accompanying drawings.

[0024] In order to better understand the technical solution of the present invention, the present invention is now described in detail with reference to the accompanying drawings.

[0025] The present invention provides a robot arm obstacle avoidance path planning method combining adaptive ellipsoid shrinkage sampling and dynamic step size adjustment, which can improve sampling efficiency and obstacle avoidance success rate, and is suitable for robot arm path search tasks in complex environments.

[0026] like Figure 1 As shown, a robotic arm path planning method based on adaptive ellipsoid sampling adopts a robotic arm path planning system including a sensor, a path generation module and a path optimization module; the sensor is used to collect environmental information; the path generation module is used to generate an initial path from a starting point to a target point according to the environmental information; the path optimization module smoothes the initial path to generate a final smooth obstacle avoidance trajectory.

[0027] The robot arm path planning method includes the following steps: Step 1: Initialization Get the starting point of the robotic arm , target point And the location information of the obstacle. The search tree with root node is and target point Use two reference points to construct the initial ellipsoid sampling area , which is constructed as follows: the initial ellipsoid sampling area Center , long axis , short axis ( is the compression factor). With unit vector As the main axis direction of the ellipsoid, select the auxiliary axis that is not collinear with the main axis direction of the ellipsoid, and remove its axis in the main axis unit vector of the ellipsoid by projection. τ The direction component, get an orthogonal unit vector , and then generate another orthogonal unit vector by cross product , and finally construct the standard orthogonal basis , and use this to construct the ellipsoidal coordinate system rotation matrix .

[0028] Step 2: Get random points The Marsaglia method is used to uniformly sample within the unit sphere to generate random vectors. , the specific process is as follows: Independently generate uniformly distributed random numbers in the interval [-1,1] and random numbers , and calculate the sum of squares ;like , then discard the pair of random numbers and regenerate them until According to the sum of squares Get a random vector in the unit sphere u= , which is expressed as: (1) in, are mutually orthogonal vectors.

[0029] The random vector is transformed into u Mapped to the ellipsoid sampling area during the current iteration The sampling area of ​​the ellipsoid is obtained Random points within , whose expression is: (2) in, The ellipsoid sampling area in the current iteration process The center position vector of is the rotation matrix, which can be used to transform a random vector in the unit sphere Rotate to the appropriate orientation to match the main axis direction of the ellipsoid, etc. It is a diagonal matrix, and the elements on the diagonal determine the scaling ratio in different coordinate axis directions.

[0030] Determine random points Is it inside the obstacle? If the random point Located inside the obstacle, then in the ellipsoid sampling area Resample random points.

[0031] Step 3: Adaptively adjust the moving step length To search for random points in the tree The nearest node is used as the node to be expanded ; Nodes to be expanded As the center, generate offset points in multiple equiangular directions, and each offset point is close to the node to be expanded. The distance between L = ;in, The maximum distance the end of the robot arm is allowed to move; ensure that even if the moving step size is adaptively reduced, the detection range is still sufficient to cover the obstacles corresponding to the maximum possible moving distance. Figure 3 As shown, based on the offset points and the nodes to be expanded Obstacle density of the connection , whose expression is: (3) in, is the number of offset points, in this embodiment, m=8; is the indicator function value.

[0032] Jordi offset points and nodes to be expanded If the line collides with an obstacle, the indicator function value is 1; otherwise, the indicator function value is 0. Adaptive adjustment of moving step length , whose expression is: (4) in, is the preset basic step length; is the adjustment coefficient; and These are the minimum and maximum distances allowed for the end of the robotic arm to move, respectively, to ensure that the moving step length is within an appropriate range.

[0033] Step 4: Expand the search tree Along the node to be expanded Point to a random point direction, according to the moving step Generate candidate new nodes , which is expressed as: (5) If the candidate new node Node to be expanded If the line of the node does not intersect with any obstacle, the candidate new node Add to the search tree; otherwise, in the ellipsoid sampling area Resample to obtain a random point, and obtain a candidate new node based on the random point until the line connecting the candidate new node and the node to be expanded does not intersect with any obstacles.

[0034] To avoid the ellipsoidal sampling area Due to excessive contraction, the search range becomes too small, and the expansion failure threshold N is preset. If continuous expansion fails (that is, the candidate new node obtained Node to be expanded The number of times the line intersects with the obstacle is greater than the expansion failure threshold , then increase the major axis of the ellipsoid sampling area and short axis (i.e., long axis and short axis Multiply by the expansion factor and , ), and perform random point sampling in the updated ellipsoid sampling area.

[0035] Step 5: Update the ellipsoid sampling area like Figure 4 As shown, based on the candidate new nodes obtained during the current iteration Update the ellipsoid sampling area during the next iteration , the specific process is as follows: Candidate new node and target point Update ellipsoid sampling area Center and the rotation matrix , ellipsoid sampling area The major axis and candidate new nodes and target point If the candidate new node Nodes to be expanded Closer to the target point ,Right now: (6) It is determined to be a valid expansion, and the ellipsoid sampling area contraction operation is triggered to reduce the long axis of the ellipsoid sampling area. and short axis (i.e., long axis and short axis Multiply by the shrinkage factor and , ).

[0036] Step 6: Repeat steps 2 to 5 until the distance between the candidate new node and the target point in the current iteration is less than the threshold Or iterate here to reach the preset maximum number of iterations. If the distance between the candidate new node and the target point is less than the threshold , then backtracking from the candidate new node can obtain a smooth obstacle avoidance trajectory (initial path); if the preset maximum number of iterations is reached, the path planning is determined to have failed, and the search tree is rebuilt for path planning.

[0037] Step 7: Sampling along the initial path at equal arc length intervals to obtain a set of sampling points . Count the number of ellipsoid contractions at each sampling point (the number of times the ellipsoid sampling area contraction operation is triggered) , as a local space complexity indicator; the evaluation of path complexity is not only based on the number of ellipsoid contractions at each path sampling point The absolute value of the contraction times between adjacent sampling points is also introduced. To more accurately characterize the local complexity of the path area, it is expressed as: (7) If the number of consecutive contractions increases Less than the increment threshold , it means that the segment has multiple expansion failures or contraction stagnations during the path search process, indicating that the area is narrow and has dense obstacles, that is, it is a high-complexity segment; if the number of consecutive contraction increments is Greater than the increment threshold , it means that the expansion is continuously successful and the number of contractions increases rapidly, which belongs to a low-complexity segment with open space and unobstructed channels.

[0038] For high-complexity sections, use the maximum number of control points Use cubic B-spline for fitting; for low complexity segments, use the minimum number of control points Use quadratic B-spline fitting, , the specific formula is as follows: (8) in, m i is the number of control points.

[0039] Step 8: Establish smoothness target items, safety distance constraints, and path conformality constraints for the control points, which are expressed as follows: Smoothness objective term J smooth : (9) in, is the optimized B-spline control point; is the total number of control points.

[0040] The safety distance constraint is: (10) in, is the boundary of obstacles in the environment; The minimum safe distance value.

[0041] The path conformality constraint is: (11) in, is the node on the initial path; is the maximum allowable deviation.

[0042] Combining the above objectives with constraints forms a standard quadratic programming (QP) problem: (12) in, is the vector of all control points of the optimized B-spline; is the second-order derivative matrix of the smoothness objective term; is the linear term coefficient vector, which is the linear term coefficient of the smoothness target term.

[0043] Due to the local support characteristics of B-spline, the second-order derivative matrix The Jacobian matrices involved in both constraints are sparse banded structures, allowing for accelerated solution through pre-sparse coding and Schur elimination. After obtaining the control point set, the B-spline curves of each segment are concatenated to generate the final smooth obstacle avoidance trajectory. This optimization method balances smoothness, safety, and shape preservation, making it suitable for real-time online path optimization scenarios.

Claims

1. A robotic arm path planning method based on adaptive ellipsoid sampling, characterized by: The following steps are involved: Step 1: Get the starting point, target point, and obstacle location information of the robot arm, and build a search tree with the starting point as the root node; The starting point and the target point are used as two reference points to construct the ellipsoid sampling area; Step 2: Obtain random points within the ellipsoid sampling area; The node closest to the random point in the search tree is used as the node to be expanded; Step 3: Obtain the moving step length based on the obstacle density; generate candidate new nodes according to the step length along the direction from the node to be expanded to the random point; if the line connecting the candidate new node and the node to be expanded does not intersect with any obstacles, then add the candidate new node to the search tree; Otherwise, re-sampling is performed in the ellipsoid sampling area until the line connecting the obtained candidate new node and the node to be expanded does not intersect with any obstacle; Step 4: The midpoint between the candidate new node and the target point is used as the center of the ellipsoid sampling area in the next iteration; if the candidate new node is closer to the target point than the node to be expanded, the ellipsoid sampling area is shrunk, and the major axis and minor axis of the ellipsoid sampling area are multiplied by the shrinkage factor and shrinkage factor ; Step 5: Repeat steps 2 to 4 until the initial path from the starting point to the target point is obtained; Step 6: Use non-uniform B-spline to optimize the initial path and obtain the final smooth obstacle avoidance trajectory.

2. A robotic arm path planning method based on adaptive ellipsoid sampling according to claim 1, characterized in that: The method for obtaining the obstacle density is as follows: With the node to be expanded as the center, offset points are generated in multiple equiangular directions. The distance between each offset point and the node to be expanded is the maximum distance allowed for the end of the robotic arm to move. Obstacle density is obtained based on the connection between each offset point and the node to be expanded. , whose expression is: in, is the number of offset points; is the indicator function value; Jordi If the line connecting the offset point and the node to be expanded collides with an obstacle, the indicator function value is 1; otherwise, the indicator function value is 0.

3. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 1, characterized in that: The moving step length The method to obtain is as follows: in, is the basic step length; is the adjustment coefficient; and are the minimum and maximum distances allowed for the end of the robotic arm to move; is the obstacle density.

4. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 1, characterized in that: During the resampling process, if the number of consecutive expansion failures is greater than the expansion failure threshold, the major axis and minor axis of the ellipsoid sampling area are multiplied by the expansion factor and expansion factor , and sample random points in the updated ellipsoid sampling area.

5. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 1, characterized in that: The process of trajectory optimization is as follows: Sampling is performed along the initial path at equal arc length intervals to obtain a set of sampling points; obtaining high-complexity segments and low-complexity segments in the initial path; setting the maximum and minimum number of control points respectively, and using the maximum number of control points to fit the high-complexity segments using cubic B-splines, and using the minimum number of control points to fit the low-complexity segments using quadratic B-splines; Smoothness objectives, safety distance constraints, and path conformality constraints are established for the control points, forming a standard quadratic programming problem. The standard quadratic programming problem is solved to obtain a set of control points; the B-spline curves of each segment are spliced ​​to generate the final smooth obstacle avoidance trajectory.

6. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 5, characterized in that: The method for obtaining high-complexity segments and low-complexity segments is as follows: Obtain the number of ellipsoid contractions at each sampling point, and use the difference between the ellipsoid contraction times of adjacent sampling points as the contraction number increment; if multiple consecutive contraction number increments are less than the increment threshold, the segment is a high-complexity segment; If the increment of multiple consecutive contraction times is greater than the increment threshold, the segment is a low-complexity segment.

7. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 5, characterized in that: The standard quadratic programming problem is expressed as: in, is the vector of all control points of the optimized B-spline; is the second-order derivative matrix of the smoothness objective term; is the linear term coefficient vector; is the optimized B-spline control point; is the boundary of obstacles in the environment; is the minimum safety distance value; is the node on the initial path; is the maximum allowable deviation; The smoothness target item J smooth for: in, is the total number of control points.

8. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 1, characterized in that: The center of the ellipsoid sampling area is the midpoint of the two reference points; the major axis is the line connecting the two reference points; and the length of the minor axis is the length of the major axis multiplied by the compression factor.

9. The method for manipulator path planning based on adaptive ellipsoid sampling according to claim 1, characterized in that: The method for obtaining random points is as follows: uniform sampling is performed within the unit sphere using the Marsaglia method to generate random vectors; the random vectors are mapped to the ellipsoid sampling area in the current iteration process using a linear transformation formula to obtain random points located within the ellipsoid sampling area; if the random point is located inside an obstacle, the random point is resampled within the ellipsoid sampling area.

10. A robotic arm path planning system based on adaptive ellipsoid sampling, characterized by: Used to execute the manipulator path planning method based on adaptive ellipsoid sampling according to claim 1; the manipulator path planning system includes a sensor, a path generation module and a path optimization module; the sensor is used to collect environmental information; The path generation module is used to generate the initial path from the starting point to the target point based on environmental information; the path optimization module smoothes the initial path to generate the final smooth obstacle avoidance trajectory.

Images