Configuration planning method for moving mechanical arm tail end path following

Abstract

The invention discloses a configuration planning method for moving mechanical arm tail end path following, which comprises the following steps: S1, generating a reachability graph of a mechanical arm by adopting an inverse solution method, and projecting the reachability graph to a mechanical arm base plane to generate an inverse reachability graph of the mechanical arm; s2, a collision detection method is adopted, and a feasible configuration set of the mobile chassis is solved according to the inverse reachability graph and the tail end target path of the mechanical arm; s3, performing initial configuration planning on the feasible configuration of the mobile chassis by adopting a graph search method according to the feasible configuration set of the mobile chassis to obtain an initial configuration sequence of the mobile chassis; s4, performing secondary configuration planning on the initial configuration sequence of the mobile chassis by adopting a numerical optimization method to obtain an optimal configuration sequence of the mobile chassis; and S5, according to the optimal configuration sequence of the mobile chassis, the tail end target path and the motion relation between the mobile chassis and the tail end of the mechanical arm, a complete configuration sequence of the mobile mechanical arm is obtained through calculation.

Description

Technical Field

[0001] The present invention relates to the technical field of path following of a robot end, and in particular to a configuration planning method for path following of a mobile robot end. Background Art

[0002] Mobile manipulation robots (consisting of a mobile chassis and a robotic arm) are widely used in industrial automation, intelligent manufacturing, and other fields. By integrating a robotic arm onto a mobile chassis to form a composite mobile manipulation robot, they can achieve greater operational flexibility and greater task accessibility. In applications requiring precise machining of large, complex curved surfaces, the end-end of the mobile robotic arm is often required to follow a specific machining path to complete the specified surface machining task. Therefore, reliable path tracking by the end-end of the mobile robotic arm is essential.

[0003] Mobile manipulators are high-degree-of-freedom systems with complex constraints and system dynamics. This makes them more flexible when manipulating complex environments, but it also complicates their motion planning. Furthermore, the mobile chassis and manipulator arm exhibit significant dynamic differences and a strong coupling relationship, which causes their motion to influence each other. Acceleration and velocity changes of the mobile base affect the trajectory of the manipulator arm, while conversely, the manipulator arm's movements alter the base's stability and posture. This interaction makes the overall motion of the system highly complex, difficult to predict, and difficult to control. The high degrees of freedom and heterogeneity of mobile manipulator systems further complicate collaborative planning for mobile manipulators. Traditional configuration planning methods often struggle to balance computational efficiency and configuration path quality when faced with mobile manipulator path-following tasks.

[0004] Therefore, facing the problem that current planning methods are difficult to balance computational efficiency and configuration path quality, it is urgent to propose a configuration planning method for the path following of the end of a mobile robot arm. Summary of the Invention

[0005] The purpose of the present invention is to provide a configuration planning method for path following of the end of a mobile robot arm, so as to solve the problem that the existing planning methods are difficult to take into account both computational efficiency and configuration path quality at the same time.

[0006] To solve the above technical problems, the present invention provides a configuration planning method for path following of a mobile robot end, comprising the steps of:

[0007] S1: Generate the reachability graph RM of the robot arm using the inverse solution method, and project the reachability graph RM onto the base plane of the robot arm to generate the inverse reachability graph IRM of the robot arm;

[0008] S2: Using the collision detection method, according to the inverse reachability graph IRM and the end target path of the manipulator, the feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each end target path point is solved;

[0009] S3: Using the graph search method to perform initial configuration planning on the feasible configuration of the mobile chassis according to the feasible configuration set of the mobile chassis feasible_base_IRM, the initial configuration sequence of the mobile chassis is obtained.

[0010] S4: Initial configuration sequence of the mobile chassis using numerical optimization method Perform secondary configuration planning to obtain the optimal configuration sequence of the mobile chassis

[0011] S5: Optimal configuration sequence based on mobile chassis The end target path and the motion relationship between the mobile chassis and the end of the manipulator are calculated to obtain the complete mobile manipulator configuration sequence path q .

[0012] Furthermore, the method for generating the reachability graph RM of the robotic arm includes:

[0013] S11: Assume that the reachability graph RM has a grid spacing δ at position [x, y, z] p , with grid spacing δ on pose [α,β,γ] r ; At the same time, the initial range of the reachability graph RM is set to a sphere with the base of the robot arm as the center and the total arm length R as the radius;

[0014] S12: By using a grid spacing δ on the six-dimensional pose space [x, y, z, α, β, γ] p and grid spacing δ r Uniform sampling is performed, and the spherical space with a radius of R is used as the sampling constraint boundary. The voxelized working space can be expressed as:

[0015]

[0016] S13: Traverse each pose voxel in the workspace, find the inverse solution for the actuator pose at the end of the manipulator corresponding to the pose voxel, and calculate the corresponding manipulator configuration q r ; If there is an inverse solution, further calculate the operability μ k , and the joint configuration and operability μ k Stored in the corresponding pose voxel Then each pose voxel in the reachability graph RM Both include robotic arm configuration q r and operability μ k:

[0017]

[0018] q r =[q1,…q6]

[0019]

[0020] Among them, J(q r ) is the Jacobian matrix.

[0021] Furthermore, the method for generating the inverse reachability graph IRM of the robotic arm includes:

[0022] S14: Input the reachability graph RM, divide the posture space according to the hierarchical parameters, and build a four-layer tree index structure For each pose voxel Preallocate a storage container and initialize it to an empty set.

[0023] S15: Traverse each pose voxel in the reachability graph RM Extract the related manipulator end posture parameters EEF (x, y, z, α, γ, β) and manipulator joint configuration q r , and obtain the unit voxel in the inverse reachability graph RM according to the relative position relationship between the manipulator base and the manipulator end effector

[0024]

[0025] Four-layer tree index structure Index into the inverse reachability subgraph IRM in the inverse reachability graph IRM i , and Stored in the reachability subgraph IRM i middle.

[0026] S16: Serialize and save the hierarchical structure of the inverse reachability graph IRM.

[0027] Furthermore, step S2 specifically includes:

[0028] S21: Discretize the end target path to obtain the end target path point sequence path EEF ;

[0029] S22: Extract the end target path point sequence path from the inverse reachability map IRM EEF Each path point T in EEF The corresponding inverse reachability subgraph IRM i ;

[0030] S23: Transform each inverse reachability subgraph IRM through posture transformationi Each unit voxel The base position of the robot arm in x r ,y r Convert to mobile chassis position x b ,y b , get each path point T EEF The corresponding reachable configuration set reachable_IRM i ={q p (x b ,y b ,q r )};

[0031] S24: Filter out the reachable configuration set reachable_IRM through collision detection method i The configuration that may collide with the environment is obtained for each path point T EEF The corresponding feasible configuration set feasible_IRM i , from the feasible configuration q p Extract the feasible configuration q of the mobile chassis bp (x b ,y b ), so that each path point T EEF The corresponding feasible configuration set of the mobile chassis feasible_base_IRM i ={q bp (x b ,y b )}; Each path point T EEF The corresponding feasible configuration set of the mobile chassis is expressed as feasible_base_IRM = [feasible_base_IRM1, ..., feasible_base_IRM i ,…]; where feasible_base_IRM i is the set of feasible configurations of the mobile chassis corresponding to the i-th terminal target path point.

[0032] Furthermore, step S3 specifically includes:

[0033] S31: Possible configuration of mobile chassis q bp (x b ,y b ) as a node, with the feasible configuration q of the mobile chassis bp (x b ,y b ) are connected as edges to construct a graph structure; the edge weight of the graph structure is one of the feasible configurations q bp (x b ,y b ) to another feasible configuration qbp (x b ,y b ) between the costs;

[0034] S32: Combine dynamic programming and Dijkstra method to perform initial configuration planning on the feasible configuration set feasible_base_IRM of the mobile chassis according to the graph structure, and obtain the initial configuration sequence of the mobile chassis

[0035] Furthermore, step S32 includes:

[0036] S321: Input graph structure and starting configuration q of mobile chassis bp0 ; Initialize the cost matrix cost to infinity, initialize the predecessor matrix prev to -1, and initialize the priority queue pq to empty;

[0037] S322: Perform configuration sequence expansion, and if the priority queue pq is not empty during the configuration sequence expansion process, pop out the configuration p with the minimum cost from the priority queue pq head , and calculate the cost from the current configuration to all candidate configurations in the next stage; if a smaller cost is found, the configuration sequence matrix cost is updated and the new configuration is added to the priority queue;

[0038] S323: When the configuration sequence is extended to the last stage, the sequence with the minimum cost is selected from the candidate sequence of the last stage as the end point, and the configuration sequence index of all stages is backtracked, and the predecessor point of each stage is tracked through the predecessor matrix prev, so as to obtain the starting configuration q from the mobile chassis bp0 Sequence of initial configurations to the final configuration of the mobile chassis

[0039] S324: Initial configuration sequence Perform smoothing to obtain the initial configuration sequence of the smooth moving chassis

[0040] Furthermore, step S4 specifically includes:

[0041] S41: Each feasible configuration set feasible_base_IRM that will move the chassis i Transformed into a continuous feasible configuration region represented by a convex hull;

[0042] S42: Convert the quadratic configuration programming into an unconstrained optimization problem and obtain the cost function and its gradient;

[0043] S43: The L-BFGS numerical optimization algorithm is used to solve the optimal configuration sequence of the mobile chassis that minimizes the cost function.

[0044] Furthermore, step S41 specifically includes:

[0045] S411: The feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each target path point i As the initial point set P ini ; Set the grid spacing δ and the minimum cluster size threshold N cmin ; Initialize the list of convex polygons without holes Polygons←{};

[0046] S412: Initial point set P ini Perform point set filtering to obtain the filtered initial point set P filt ={p i ∈P ini |N(p i ,δ)>2};

[0047] S413: Combine K-Means and DBSCAN algorithms to find the initial point set P filt Perform clustering to generate sub-cluster set C=C1,C2,…,C k ;

[0048] S414: For each sub-cluster C i ∈C, the QuickHull convex hull algorithm based on the divide-and-conquer method is used to generate the convex hull H for each cluster area i ;

[0049] S415: Detecting the convex hull H i Is there a hole inside? If not, then the convex hull H i Add Polygons; if yes, repeat steps S412-S414 until subcluster C i Number of points N within Ci are all less than the set threshold N cmin ;

[0050] S416: Output the final convex polygon list Polygons, that is, obtain the continuous feasible configuration area represented by the convex hull;

[0051] S417: Loop steps S411-S416 to finally obtain the convex hull corresponding to the terminal target path to represent the continuous feasible configuration area.

[0052] Furthermore, in step S42, the specific expression of the unconstrained optimization problem is:

[0053]

[0054]

[0055] Among them, Length(qbi ) is the i-th node q bi The length cost, is the gradient of the length cost; Smooth(q bi ) is the i-th node q bi The smoothness cost, is the gradient of the smoothing cost; Reach(q bi ) is the i-th node q bi feasibility cost, is the gradient of the feasibility cost, dist min For the i-th node q bi The shortest distance to the nearest convex polygon; α is a constant that controls the rate at which the cost increases.

[0056] Furthermore, step S43 specifically includes:

[0057] S431: Initial configuration sequence of the mobile chassis As the initial value u0 of the iteration, the cost function As the objective function f(u), the gradient of the feasibility cost As the gradient g(u) of the objective function f(u), the inverse of the Hessian matrix is initialized as At the same time, set the convergence tolerance ε and the maximum number of iterations iter max ;

[0058] S432: According to the gradient g(u i ) and the inverse of the approximate Hessian matrix Calculate the search direction d i :

[0059]

[0060] At the same time, the Lewis & Overton line search is used to determine the objective function f(u i +α i d i )The smallest optimal step size α i , and use the optimal step size α i Update the current path:

[0061] u i+1 =u i +α i ·d i

[0062] S433: Calculate the gradient g(u i+1 ), according to the step difference Δu i =u i+1 -u i and gradient difference Δgi =g(u i+1 )-g(u i ) Update the inverse of the approximate Hessian matrix

[0063]

[0064] S434: Repeat S432 and S433 above, iteratively calculate the path, and determine whether the norm of the gradient is less than the set tolerance in each iteration:

[0065] ‖g(u i+1 )‖<ε

[0066] If the conditions are met, the iteration is terminated and a sequence of better mobile chassis configurations with both accessibility, feasibility and smooth stability is returned. And the optimal objective function value f(u * ). In addition, to avoid infinite iterations, if the number of iterations exceeds the maximum number of iterations iter max Returns False.

[0067] The beneficial effect of the present invention is that by introducing the inverse reachability graph, the terminal target path and the mobile chassis configuration sequence are established under the premise of ensuring the reachability of the robot arm. The 8-dimensional mobile manipulator configuration planning problem is effectively simplified into a 2-dimensional mobile chassis configuration planning problem. By adopting a dimensionality reduction strategy, the computational complexity is significantly reduced, improving the efficiency of configuration planning. By planning the feasible configurations of the mobile chassis twice, the quality of the configuration sequence can be effectively improved, thereby further enhancing the operating efficiency of the mobile manipulation robot and strengthening its adaptability to real-world environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The same reference numerals are used in these drawings to represent the same or similar parts. The exemplary embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0069] Figure 1 A flow chart of a method according to an embodiment of the present invention;

[0070] Figure 2 A distribution diagram of the reachability of a robotic arm according to an embodiment of the present invention;

[0071] Figure 3 This is a schematic diagram of the initial configuration planning of an embodiment of the present invention;

[0072] Figure 4 This is a schematic diagram of an initial point set according to an embodiment of the present invention;

[0073] Figure 5 The convex hull generated by one embodiment of the present invention;

[0074] Figure 6 A continuous feasible configuration region represented by a convex hull according to an embodiment of the present invention;

[0075] Figure 7 This is a schematic diagram of secondary configuration planning according to an embodiment of the present invention. DETAILED DESCRIPTION

[0076] The present invention discloses a configuration planning method for the path following of the end of a mobile manipulator, such as Figure 1 As shown, the steps include:

[0077] S1: Generate the reachability graph RM of the robot arm using the inverse solution method, and project the reachability graph RM onto the base plane of the robot arm to generate the inverse reachability graph IRM of the robot arm;

[0078] S2: Using the collision detection method, according to the inverse reachability graph IRM and the end target path of the manipulator, the feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each end target path point is solved;

[0079] S3: Using the graph search method to perform initial configuration planning on the feasible configuration of the mobile chassis according to the feasible configuration set of the mobile chassis feasible_base_IRM, the initial configuration sequence of the mobile chassis is obtained.

[0080] S4: Initial configuration sequence of the mobile chassis using numerical optimization method Perform secondary configuration planning to obtain the optimal configuration sequence of the mobile chassis

[0081] S5: Optimal configuration sequence based on mobile chassis The end target path and the motion relationship between the mobile chassis and the end of the manipulator are calculated to obtain the complete mobile manipulator configuration sequence path q .

[0082] The present invention introduces the inverse reachability graph to establish the terminal target path and the mobile chassis configuration sequence under the premise of ensuring the reachability of the robot arm. The 8-dimensional mobile manipulator configuration planning problem is effectively simplified into a 2-dimensional mobile chassis configuration planning problem. By adopting a dimensionality reduction strategy, the computational complexity is significantly reduced, improving the efficiency of configuration planning. By planning the feasible configurations of the mobile chassis twice, the quality of the configuration sequence can be effectively improved, thereby further enhancing the operating efficiency of the mobile manipulation robot and strengthening its adaptability to real-world environments.

[0083] According to one embodiment of the present application, a method for generating a reachability map RM of a robotic arm includes:

[0084] S11: Assume that the reachability graph RM has a grid spacing δ at position [x, y, z] p , with grid spacing δ on pose [α,β,γ] r ; At the same time, the initial range of the reachability graph RM is set to a sphere with the base of the robot arm as the center and the total arm length R as the radius;

[0085] S12: By using a grid spacing δ on the six-dimensional pose space [x, y, z, α, β, γ] p and grid spacing δ r Uniform sampling is performed, and the spherical space with a radius of R is used as the sampling constraint boundary. The voxelized working space can be expressed as:

[0086]

[0087] S13: Traverse each pose voxel in the workspace, find the inverse solution for the actuator pose at the end of the manipulator corresponding to the pose voxel, and calculate the corresponding manipulator configuration q r ; If there is an inverse solution, further calculate the operability μ k , and the joint configuration and operability μ k Stored in the corresponding pose voxel Then each pose voxel in the reachability graph RM Both include robotic arm configuration q r and operability μ k :

[0088]

[0089] q r =[q1,…q6]

[0090]

[0091] Among them, J(q r ) is the Jacobian matrix.

[0092] In order to intuitively display the reachability map of the manipulator generated by the above steps, the 6D reachability map can be projected into a 3D position grid, and the color of each grid point is determined according to the number of reachable poses in the grid. Red represents voxels with a large number of poses, and blue represents voxels with a small number of poses, thereby generating a reachability hotspot map, such as Figure 2 shown.

[0093] According to one embodiment of the present application, a method for generating an inverse reachability graph IRM of a robotic arm includes:

[0094] S14: Input the reachability graph RM, divide the posture space according to the hierarchical parameters, and build a four-layer tree index structure For each pose voxel The storage containers are pre-allocated and initialized to an empty set. The grid structure of the inverse reachability graph is strictly aligned with the reachability graph, and its grid spacing δ p , δ r Keep consistent with the reachability graph to ensure data compatibility. The inverse reachability graph adopts a four-layer tree index structure, and the inverse reachability graph is organized hierarchically according to the end-effector posture to facilitate configuration query in subsequent configuration planning: IRM = T(z, α, β, γ);

[0095] S15: Traverse each pose voxel in the reachability graph RM Extract the related manipulator end posture parameters EEF (x, y, z, α, β, γ) and manipulator joint configuration q r , and obtain the unit voxel in the inverse reachability graph RM according to the relative position relationship between the manipulator base and the manipulator end effector

[0096]

[0097] Four-layer tree index structure Index into the inverse reachability subgraph IRM in the inverse reachability graph IRM i , and Stored in the reachability subgraph IRM i middle.

[0098] Each subgraph IRM i ∈IRM stores the current end effector pose T EEF The set of feasible mobile manipulator configurations Each unit voxel Can be defined as:

[0099]

[0100] where x r ,y r is the plane coordinate of the robot base, q r is the corresponding robotic arm configuration;

[0101] S16: Serialize and save the hierarchical structure of the inverse reachability graph IRM.

[0102] According to one embodiment of the present application, step S2 specifically includes:

[0103] S21: Discretize the end target path to obtain the end target path point sequence path EEF;

[0104] S22: Extract the end target path point sequence path from the inverse reachability map IRM EEF Each path point T in EEF The corresponding inverse reachability subgraph IRM i ;

[0105] S23: Transform each inverse reachability subgraph IRM through posture transformation i Each unit voxel The base position of the robot arm in x r ,y r Convert to mobile chassis position x b ,y b , get each path point T EEF The corresponding reachable configuration set reachable_IRM i ={q p (x b ,y b ,q r )};

[0106] S24: Filter out the reachable configuration set reachable_IRM through collision detection method i The configuration that may collide with the environment is obtained for each path point T EEF The corresponding feasible configuration set feasible_IRM i , from the feasible configuration q p Extract the feasible configuration q of the mobile chassis bp (x b ,y b ), so that each path point T EEF The corresponding feasible configuration set of the mobile chassis feasible_base_IRM i ={q bp (x b ,y b )}; Each path point T EEF The corresponding feasible configuration set of the mobile chassis is expressed as feasible_base_IRM = [feasible_base_IRM1, ..., feasible_base_IRM i ,…]; where feasible_base_IRM i is the set of feasible configurations of the mobile chassis corresponding to the i-th terminal target path point.

[0107] This embodiment links coordinate transformation with collision detection to ensure that all configurations simultaneously meet the requirements of terminal accessibility (success rate 100%) and no collision with the environment (collision rate reduced to 0.1%).

[0108] According to one embodiment of the present application, step S3 specifically includes:

[0109] S31: Possible configuration of mobile chassis q bp (x b ,y b ) as a node, with the feasible configuration q of the mobile chassis bp (x b ,y b ) are connected as edges to construct a graph structure; the edge weight of the graph structure is one of the feasible configurations q bp (x b ,y b ) to another feasible configuration q bp (x b ,y b ) between the cost; feasible configuration q bp (x b ,y b ) includes two parts: length cost and smoothness cost:

[0110] (1) Length cost

[0111] The length is measured by the Euclidean distance between the adjacent feasible configurations of the moving chassis. bi (x i ,y i ) and q bi+1 (x i+1 ,y i+1 ), the length cost of its edge Length(q bi )for:

[0112]

[0113] (2) Smoothness cost

[0114] The smoothness is measured by the second-order derivative of the configuration path. For discretized nodes, their second-order derivatives can be approximated by the difference between adjacent points. Therefore, for three adjacent feasible configurations q bi-1 (x i-1 ,y i-1 ),q bi (x i ,y i ) and q bi+1 (x i+1 ,y i+1 ), its smoothness cost Smooth(q bi ) can be expressed as:

[0115] Smooth(q bi )=(|q bi+1 -2qbi +q bi-1 |) 2

[0116] =(x i+1 +x i-1 -2x i ) 2 +(y i+1 +y i-1 -2y i ) 2 ;

[0117] The second-order derivative measures the curvature change of the path by calculating the degree of deviation of the current position relative to the two previous and next points, thereby evaluating its smoothness. A smaller smoothness cost means that the configuration path changes more smoothly, which helps improve the motion stability and execution accuracy of the mobile manipulation robot;

[0118] S32: Combine dynamic programming and Dijkstra method to perform initial configuration planning on the feasible configuration set feasible_base_IRM of the mobile chassis according to the graph structure, and obtain the initial configuration sequence of the mobile chassis

[0119] This embodiment is achieved by converting the initial configuration sequence The planning process can be regarded as the feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each terminal target path point i A configuration is selected and connected in series to form a complete configuration sequence. This configuration planning problem can be viewed as a multi-stage shortest path problem, where the length and smoothness of the mobile chassis configuration sequence are optimized. By employing a configuration planning method that combines dynamic programming with the Dijkstra algorithm, this method effectively solves this multi-stage shortest path problem by leveraging the advantages of dynamic programming in multi-stage decision-making problems and combining it with the Dijkstra algorithm's ability to efficiently calculate shortest paths.

[0120] According to one embodiment of the present application, step S32 includes:

[0121] S321: Input graph structure and starting configuration q of mobile chassis bp0 Initialize the cost matrix cost to infinity, which is used to store the cost from a configuration in the current layer (stage) to each candidate configuration in the next layer (stage); initialize the predecessor matrix prev to -1, which is used to record the configuration sequence backtracking information; initialize the priority queue pq to empty;

[0122] S322: Perform configuration sequence expansion, and if the priority queue pq is not empty during the configuration sequence expansion process, pop out the configuration p with the minimum cost from the priority queue pq head, and calculate the cost from the current configuration to all candidate configurations in the next stage; if a smaller cost is found, the configuration sequence matrix cost is updated and the new configuration is added to the priority queue;

[0123] S323: When the configuration sequence is extended to the last stage, the sequence with the minimum cost is selected from the candidate sequence of the last stage as the end point, and the configuration sequence index of all stages is backtracked, and the predecessor point of each stage is tracked through the predecessor matrix prev, so as to obtain the starting configuration q from the mobile chassis bp0 Sequence of initial configurations to the final configuration of the mobile chassis

[0124] S324: Initial configuration sequence Perform smoothing to obtain the initial configuration sequence of the smooth moving chassis

[0125] By adopting the initial configuration planning method based on graph search, the initial optimal configuration sequence can be effectively solved in a complex environment.

[0126] According to one embodiment of the present application, step S4 specifically includes:

[0127] S41: Each feasible configuration set feasible_base_IRM that will move the chassis i Transformed into a continuous feasible configuration region represented by a convex hull; the convex hull is the smallest convex polygonal region formed around a point set, which contains the points in the point set;

[0128] S42: Convert the quadratic configuration programming into an unconstrained optimization problem and obtain the cost function and its gradient;

[0129] S43: The L-BFGS numerical optimization algorithm is used to solve the optimal configuration sequence of the mobile chassis that minimizes the cost function.

[0130] Although the initial configuration planning method based on graph search can effectively solve the initial optimal configuration sequence in a complex environment, it often has poor smoothness. Because it ignores the limitations of the manipulator's reachability and environmental collisions, it may cause the path point to be unreachable or collide with the environment, thus causing serious accidents in actual applications. To address this problem, this embodiment adopts a secondary configuration planning method based on a numerical optimization method. This numerical optimization method can comprehensively optimize the smoothness, length and other indicators of the chassis configuration sequence under the premise of considering the reachability constraints of the manipulator and the environmental collision limitations, reduce the range of configuration changes of the robot during driving, and effectively improve the quality of the configuration sequence, thereby further improving the operating efficiency of the mobile operating robot and enhancing its adaptability to the real environment.

[0131] According to one embodiment of the present application, step S41 specifically includes:

[0132] S411: The feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each target path point i As the initial point set P ini ,like Figure 5 As shown; set the grid spacing δ and the minimum cluster size threshold N cmin ; Initialize the list of convex polygons without holes Polygons←{};

[0133] S412: Initial point set P ini Perform point set filtering to obtain the filtered initial point set P filt ={p i ∈P ini |N(p i ,δ)>2};

[0134] S413: Combine K-Means and DBSCAN algorithms to find the initial point set P filt Perform clustering to generate sub-cluster set C=C1,C2,…,C k ;

[0135] S414: For each sub-cluster C i ∈C, the QuickHull convex hull algorithm based on the divide-and-conquer method is used to generate the convex hull H for each cluster area i ,like Figure 5 As shown;

[0136] S415: Detecting the convex hull H i Is there a hole inside? If not, then the convex hull H i Add Polygons; if yes, repeat steps S412-S414 until subcluster C i Number of points N within Ci are all less than the set threshold N cmin ;

[0137] S416: Output the final convex polygon list Polygons, that is, obtain the continuous feasible configuration area represented by the convex hull;

[0138] S417: loop through steps S411-S416, and finally obtain the convex hull corresponding to the terminal target path to represent the continuous feasible configuration area, such as Figure 6 shown.

[0139] Numerical optimization methods require that the path cost can be represented by a continuous and differentiable function to accurately describe the configuration sequence cost during the quadratic configuration planning process. Therefore, this embodiment first converts the discretized set of feasible configurations of the mobile chassis into a continuous geometric region representation.

[0140] According to one embodiment of the present application, in step S42, the goal of the secondary configuration planning is to optimize the initial optimal configuration sequence to make it smoother and to ensure that all configurations are within the feasible chassis configuration region. For ease of understanding, the problem is first constructed as a constrained optimization problem, where the optimization object is a series of discrete chassis configuration path nodes q bi (x i ,y i ), where the position of each node is constrained by the feasible configuration region. Specifically, the quadratic configuration planning problem with N nodes can be described as follows:

[0141]

[0142] Among them, Length(q bi ) and Smooth(q bi ) are the i-th node q bi Length cost and smoothness cost; q bi The feasibility constraint, q bi It needs to be within the continuous feasible region represented by the convex hull;

[0143] The length cost and its gradient are:

[0144]

[0145] The smoothness cost and its gradient are:

[0146] Smooth(q bi )=(|q bi+1 -2q bi +q bi-1 |) 2

[0147] =(x i+1 +x i-1 -2x i ) 2 +(y i+1 +y i-1 -2y i ) 2

[0148]

[0149] The feasibility constraints are converted into cost functions Reach(q bi ), represents the i-th node q bi The feasibility cost; then the constrained optimization problem can be transformed into an unconstrained optimization problem, the specific form is as follows:

[0150]

[0151] To obtain the calculation formula of feasibility cost, the node q bi The distance dist to the nearest convex polygon min As a cost indicator to measure feasibility. Calculate dist min The specific steps are as follows:

[0152] (1) Use the ray method to determine the positional relationship between the node and each polygon. That is, draw a ray horizontally through the node and determine the positional relationship between the node and the polygon based on the number of intersections between the ray and the polygon. If the number of intersections is odd, the point is inside the convex polygon, and the distance dist is negative. If the number of intersections is even, the point is outside the convex polygon, and the distance dist is positive. If the node is on the polygon boundary, the distance dist is 0.

[0153] (2) Calculate the shortest distance dist between a node and a single polygon; describe a polygon with m vertices by a set of linear equations, and for each of its edges E i (p k ,p j ), can be expressed as:

[0154] A Q (k,0)=y k -y j ,A Q (k,1)=x j -x k

[0155] b Q (k) = A Q (k,0)·x j +A Q (k,1)·y j

[0156] where p k (x k ,y k ) and p j (x j ,y j ) are the edges E i The coordinates of two vertices.

[0157] (2.1) If node q bi Outside the polygon, the distance node q can be calculated by solving the low-dimensional quadratic programming problem (QP) bi (x i ,y i)The nearest point p(x p ,y p ), the QP problem can be expressed as:

[0158]

[0159] stA Q p≤b Q

[0160] Among them, M Q =2I 2×2 , c Q =[-2x i -2y i ], the distance node q is calculated using the sdqp solver bi (x i ,y i )The nearest point p(x p ,y p ), thus calculating the node q bi (x i ,y i ) to a single polygon and its gradient is:

[0161]

[0162] (2.1) If node q bi In a polygon, the node q can be obtained by solving the perpendicular distance from the node to each edge of the polygon and then taking the minimum perpendicular distance. bi (x i ,y i ) to a single polygon and its gradient is:

[0163]

[0164] in,

[0165]

[0166] (3) Calculate the distance from the node to all polygons, and then take the minimum to get the node q bi (x i ,y i ) to the nearest convex polygon distance dist min ;

[0167] In order to ensure that the mobile chassis is located in the chassis feasible area at each node, it is necessary to constrain its position to always be within any polygon in the chassis feasible area. Therefore, the reachability cost function needs to be continuous and differentiable and change with the distance dist minIn addition to the basic requirement of monotonically increasing growth, it is also hoped that the cost function dist min When ≤0, it grows slowly, which means that each location has almost the same accessibility within the feasible range; in the cost function dist min When it is >0, it rises rapidly to pull the node back to the feasible area. In order to meet the above characteristics, an exponential function is selected as the reachability cost function, which is as follows:

[0168]

[0169] Among them, α is a constant that controls the cost growth rate.

[0170] According to one embodiment of the present application, step S43 specifically includes:

[0171] S431: Initial configuration sequence of the mobile chassis As the initial value u0 of the iteration, the cost function As the objective function f(u), the gradient of the feasibility cost As the gradient g(u) of the objective function f(u), the inverse of the Hessian matrix is initialized as At the same time, set the convergence tolerance ε and the maximum number of iterations iter max ;

[0172] S432: According to the gradient g(u i ) and the inverse of the approximate Hessian matrix Calculate the search direction d i :

[0173]

[0174] At the same time, the Lewis & Overton line search is used to determine the objective function f(u i +α i d i )The smallest optimal step size α i , and use the optimal step size α i Update the current path:

[0175] u i+1 =u i +α i ·d i

[0176] S433: Calculate the gradient g(u i+1 ), according to the step difference Δu i =u i+1 -u i and gradient difference Δg i =g(u i+1 )-g(ui ) Update the inverse of the approximate Hessian matrix

[0177]

[0178] S434: Repeat S432 and S433 above, iteratively calculate the path, and determine whether the norm of the gradient is less than the set tolerance in each iteration:

[0179] ‖g(u i+1 )‖<ε

[0180] If the conditions are met, the iteration is terminated and a sequence of better mobile chassis configurations with both accessibility, feasibility and smooth stability is returned. And the optimal objective function value f(u * ). In addition, to avoid infinite iterations, if the number of iterations exceeds the maximum number of iterations iter max Returns False.

[0181] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A configuration planning method for path following of a mobile robot end, characterized in that: include S1: Generate a reachability graph RM of the robot arm using an inverse solution method, and project the reachability graph RM onto the base plane of the robot arm to generate an inverse reachability graph IRM of the robot arm; S2: using a collision detection method, solving the feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each end target path point according to the inverse reachability graph IRM and the end target path of the manipulator; S3: Using the graph search method to perform initial configuration planning on the feasible configuration of the mobile chassis according to the feasible configuration set feasible_base_IRM of the mobile chassis, and obtain the initial configuration sequence of the mobile chassis S4: Using numerical optimization method to calculate the initial configuration sequence of the mobile chassis Perform secondary configuration planning to obtain the optimal configuration sequence of the mobile chassis S5: According to the optimal configuration sequence of the mobile chassis The end target path and the motion relationship between the mobile chassis and the end of the manipulator are calculated to obtain the complete mobile manipulator configuration sequence path q .

2. The configuration planning method for path following of a mobile robot end according to claim 1, characterized in that: The method for generating the reachability graph RM of the robotic arm includes: S11: Assume that the reachability graph RM has a grid spacing δ at position [x, y, z] p , with grid spacing δ on pose [α,β,γ] r ; At the same time, the initial range of the reachability graph RM is set to a sphere with the base of the robot arm as the center and the total arm length R as the radius; S12: By using a grid spacing δ on the six-dimensional pose space [x, y, z, α, β, γ] p and grid spacing δ r Uniform sampling is performed, and the spherical space with a radius of R is used as the sampling constraint boundary. The voxelized working space can be expressed as: S13: traverse each pose voxel in the workspace, find the inverse solution for the actuator pose at the end of the manipulator corresponding to the pose voxel, and calculate the corresponding manipulator configuration q r ; If there is an inverse solution, further calculate the operability μ k , and the joint configuration and operability μ k Stored in the corresponding pose voxel Then each pose voxel in the reachability graph RM Both include robotic arm configuration q r and operability μ k : q r =[q1,…q6] Among them, J(q r ) is the Jacobian matrix.

3. The configuration planning method for path following of a mobile robot end according to claim 2, characterized in that: The method for generating the inverse reachability graph IRM of the robotic arm includes: S14: Input the reachability graph RM, divide the posture space according to the hierarchical parameters, and construct a four-layer tree index structure For each of the pose voxels Preallocate a storage container and initialize it to an empty set. S15: traverse each pose voxel in the reachability graph RM Extract the related manipulator end posture parameters EEF (x, y, z, α, β, γ) and manipulator joint configuration q r , and obtain the unit voxel in the inverse reachability graph IRM according to the relative position relationship between the manipulator base and the manipulator end effector The four-layer tree index structure Index into the inverse reachability subgraph IRM in the inverse reachability graph IRM i , and Stored to the reachability subgraph IRM i middle. S16: Serialize and save the hierarchical structure of the inverse reachability graph IRM.

4. The configuration planning method for path following of a mobile robot end according to claim 3, characterized in that: The step S2 specifically includes: S21: Discretize the end target path to obtain the end target path point sequence path EEF ; S22: Extracting the end target path point sequence path from the inverse reachability map IRM EEF Each path point T in EEF The corresponding inverse reachability subgraph IRM i ; S23: By moving the relative position relationship between the chassis and the robot base, each inverse reachability subgraph IRM i Each unit voxel The base position of the robot arm in x r ,y r Convert to mobile chassis position x b ,y b , get each path point T EEF The corresponding reachable configuration set reachable_IRm i ={q p (x b ,y b ,q r )}; S24: Filter out the reachable configuration set reachable_IRM by collision detection method i The configuration that may collide with the environment is obtained for each path point T EEF The corresponding feasible configuration set feasible_IRM i , from the feasible configuration q p Extract the feasible configuration q of the mobile chassis bp (x b ,y b ), so that each path point T EEF The corresponding feasible configuration set of the mobile chassis feasible_base_IRM i ={q bp (x b ,y b )}; Each path point T EEF The corresponding feasible configuration set of the mobile chassis is expressed as feasible_base_IRM = [feasible_base_IRM1, ..., feasible_base_IRM i ,…]; where feasible_base_IRM i is the set of feasible configurations of the mobile chassis corresponding to the i-th terminal target path point.

5. The configuration planning method for path following of a mobile robot end according to claim 4, characterized in that: The step S3 specifically includes: S31: With the feasible configuration q of the mobile chassis bp (x b ,y b ) as a node, with the feasible configuration q of the mobile chassis bp (x b ,y b ) are connected as edges to construct a graph structure; the edge weight of the graph structure is one of the feasible configurations q bp (x b ,y b ) to another feasible configuration q bp (x b ,y b ) between the costs; S32: Combine dynamic programming and Dijkstra method to perform initial configuration planning on the feasible configuration set feasible_base_IRM of the mobile chassis according to the graph structure to obtain the initial configuration sequence of the mobile chassis 6. The configuration planning method for path following of a mobile robot end according to claim 5, characterized in that: The step S32 includes: S321: Input the graph structure and the initial configuration q of the mobile chassis bp0 ; Initialize the cost matrix cost to infinity, initialize the predecessor matrix prev to -1, and initialize the priority queue pq to empty; S322: Perform configuration sequence expansion, and if the priority queue pq is not empty during the configuration sequence expansion process, pop out the configuration p with the minimum cost from the priority queue pq head , and calculate the cost from the current configuration to all candidate configurations in the next stage; if a smaller cost is found, the configuration sequence matrix cost is updated and the new configuration is added to the priority queue; S323: When the configuration sequence is extended to the last stage, the sequence with the minimum cost is selected from the candidate sequence of the last stage as the end point, and the configuration sequence index of all stages is backtracked, and the predecessor point of each stage is tracked through the predecessor matrix prev, so as to obtain the starting configuration q from the mobile chassis bp0 Sequence of initial configurations to the final configuration of the mobile chassis S324: The initial configuration sequence Perform smoothing to obtain the initial configuration sequence of the smooth moving chassis 7. The configuration planning method for path following of a mobile robot end according to claim 6, characterized in that: The step S4 specifically includes: S41: Set the feasible configuration set feasible_base_IRM of the mobile chassis i Transformed into a continuous feasible configuration region represented by a convex hull; S42: Convert the quadratic configuration programming into an unconstrained optimization problem, and obtain a cost function and its gradient; S43: The L-BFGS numerical optimization algorithm is used to solve the optimal configuration sequence of the mobile chassis that minimizes the cost function.

8. The configuration planning method for path following of a mobile robot end according to claim 7, characterized in that: The step S41 specifically includes: S411: The feasible configuration set feasible_base_IRM of the mobile chassis corresponding to each target path point i As the initial point set P ini ; Set the grid spacing δ and the minimum cluster size threshold N cmin ; Initialize the list of convex polygons without holes Polygons←{}; S412: The initial point set P ini Perform point set filtering to obtain the filtered initial point set P filt ={p i ∈P ini |N(p i ,δ)>2}; S413: Combine K-Means and DBSCAN algorithms to find the initial point set P filt Perform clustering to generate sub-cluster set C=C1,C2,…,C k ; S414: For each sub-cluster C i ∈C, the QuickHull convex hull algorithm based on the divide-and-conquer method is used to generate the convex hull H for each cluster area i ; S415: Detecting the convex hull H i Is there a hole inside? If not, then the convex hull H i Add Polygons; if so, repeat steps S412-S414 until the sub-cluster C i Number of points N within Ci are all less than the set threshold N cmin ; S416: Output the final convex polygon list Polygons, that is, obtain the continuous feasible configuration area represented by the convex hull; S417: Loop steps S411-S416 to finally obtain the convex hull corresponding to the terminal target path to represent the continuous feasible configuration area.

9. The configuration planning method for path following of a mobile robot end according to claim 8, characterized in that: In step S42, the specific expression of the unconstrained optimization problem is: Among them, Length(q bi ) is the i-th node q bi The length cost, is the gradient of the length cost; Smooth(q bi ) is the i-th node q bi The smoothness cost, is the gradient of the smoothing cost; Reach(q bi ) is the i-th node q bi feasibility cost, is the gradient of the feasibility cost, dist min For the i-th node q bi The shortest distance to the nearest convex polygon; α is a constant that controls the rate at which the cost increases.

10. The configuration planning method for path following of a mobile robot end according to claim 9, characterized in that: The step S43 specifically includes: S431: Sequence the initial configuration of the mobile chassis As the initial value u0 of the iteration, the cost function As the objective function f(u), the gradient of the feasibility cost As the gradient g(u) of the objective function f(u), the inverse of the Hessian matrix is initialized as At the same time, set the convergence tolerance ε and the maximum number of iterations iter max ; S432: According to the gradient g(u i ) and the inverse of the approximate Hessian matrix Calculate the search direction d i : At the same time, the Lewis & Overton line search is used to determine the objective function f(u i +α i d i )The smallest optimal step size α i , and use the optimal step size α i Update the current path: u i+1 =u i +α i ·d i S433: Calculate the gradient g(u i+1 ), according to the step difference Δu i =u i+1 -u i and gradient difference Δg i =g(u i+1 )-g(u i ) Update the inverse of the approximate Hessian matrix S434: Repeat S432 and S433 above, iteratively calculate the path, and determine whether the norm of the gradient is less than the set tolerance in each iteration: ‖g(u i+1 )‖<ε If the conditions are met, the iteration is terminated and a sequence of better mobile chassis configurations with both accessibility, feasibility and smooth stability is returned. And the optimal objective function value f(u * ). In addition, to avoid infinite iterations, if the number of iterations exceeds the maximum number of iterations iter max Returns False.

Images