A mechanical arm path planning method based on space-interval dichotomy

Abstract

The application belongs to the field of mechanical arm path planning, and particularly relates to a mechanical arm path planning method based on space-interval bisection, steps of which include obtaining the size of a three-dimensional space map, the positions of a starting point and an ending point, sampling parameters, the shape and position of an obstacle; constructing an initial collision-free path based on a bidirectional expansion octree strategy, and improving the success rate of path planning by using an adaptive step strategy, a branch backtracking strategy and a collision detection strategy; pruning and optimizing the path based on a bisection pruning strategy; and establishing a forward kinematics model of the mechanical arm according to the D-H parameter method, and obtaining the final joint angle sequence of the mechanical arm by calculating the collision-free inverse solution of each path node. The application solves the problem of high computational complexity of global path pruning caused by regional redundant search and random sampling in the traditional algorithm based on sampling in mechanical arm path planning, and can effectively improve the efficiency and reliability of the mechanical arm in industrial automation, thus having important theoretical significance and engineering value.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mechanical arm path planning, and particularly relates to a mechanical arm path planning method based on space-interval bisection. BACKGROUND

[0002] With the rapid advancement of the new generation of intelligent manufacturing, mechanical arms are widely used in manufacturing, logistics, medical treatment, aerospace and other industries. Mechanical arms effectively improve production efficiency and work precision, promote industrial upgrading and intelligent transformation, by virtue of the advantages of high-precision motion control and flexible operation capability. In a complex three-dimensional operation environment, mechanical arms need to complete precise control of end effectors while avoiding obstacles through path planning technology, and the path planning quality directly affects the safety of equipment operation, operation efficiency and energy economy. Therefore, it has important theoretical significance and engineering value to study efficient and reliable path planning algorithms for mechanical arms.

[0003] In human-machine cooperation scenarios, mechanical arms have a certain degree of autonomy. Mechanical arm path planning aims to generate a collision-free path connecting the starting point and the ending point in three-dimensional space without the help of artificial teaching, and its output is a sequence of spatial positions or joint angles without time information. Existing path planning methods can be mainly divided into four categories: ① graph search-based methods: A* algorithm, Dijkstra algorithm, etc., generally combined with probabilistic roadmap (PRM); ② artificial potential field (APF)-based methods; ③ bionic algorithm-based methods: genetic algorithm, particle swarm algorithm, ant colony algorithm, etc.; and ④ random sampling-based methods: Rapidly-exploring Random Tree (RRT), RRT-Connect, RRT*, etc. Methods ① and ③ have slow running speed, method ② has the problem of being easy to fall into local minimum traps, and method ④ has the problems of many redundant points in the obtained path and sampling waste in repeated areas.

[0004] Therefore, it is necessary to provide a mechanical arm path planning method based on space-interval bisection, aiming at the above defects in the prior art. SUMMARY

[0005] Therefore, the application provides a mechanical arm path planning method based on space-interval bisection, to realize efficient path planning of mechanical arms.

[0006] Specifically, the application is realized by the following technical solutions:

[0007] The application provides a mechanical arm path planning method based on space-interval bisection, and the method comprises the following steps:

[0008] S1, obtaining three-dimensional space map size and path planning parameters, including map side length , starting point position , end point position , obstacle shape and position , sampling step , upper limit of sampling times ;

[0009] S2, adopting bidirectional expansion octree algorithm according to the three-dimensional space map size and the path planning parameters to obtain a collision-free initial path node sequence ;

[0010] S3, adopting a halving pruning algorithm to prune the initial path node sequence to obtain a collision-free optimized path node sequence ;

[0011] S4, establishing a mechanical arm model according to the D-H parameter method, taking the optimized path node sequence as the mechanical arm end effector path node sequence, obtaining the collision-free inverse kinematics solution corresponding to each path node according to the mechanical arm model to obtain a final mechanical arm joint angle sequence .

[0012] Among them, the three-dimensional space map size and the path planning parameters are obtained, specifically including:

[0013] obstacle shape and position The center position of the bounding sphere is recorded in the list , wherein a single element is denoted as , and the radius of the bounding sphere is recorded in the list , wherein a single element is denoted as .

[0014] Among them, the bidirectional expansion octree algorithm is adopted according to the three-dimensional space map size and the path planning parameters, specifically including:

[0015] S21, octree bidirectional expansion strategy

[0016] S22, adaptive step strategy

[0017] S23, branch backtracking strategy

[0018] S24, path segment and obstacle collision detection strategy

[0019] Among them, the S21 step specifically includes:

[0020] S211, with and Initialize two independent octrees for the root node, called the start tree and the end tree. and All are set as active nodes ;

[0021] S212, Each octree uses the currently active node The most recently added node becomes the parent node, which extends continuously in eight preset directions in three-dimensional space until it encounters an obstacle and stops, generating candidate child nodes. These eight preset directions are composed of a set of orthogonal bases and their negative vectors in three-dimensional space. Furthermore, to avoid generating redundant paths, each active node... During expansion, directions pointing to its parent node will be excluded;

[0022] S213. Generate candidate child nodes from the starting tree and the ending tree. Two nodes are paired up to form a candidate node pair. Iterate through and check all candidate node pairs;

[0023] S214. If there exists a pair of candidate nodes whose paths do not collide, then the starting tree and the ending tree are considered successfully connected, and the algorithm ends. Otherwise, select the pair with the smallest distance among all candidate node pairs and use it as the active node for the next round of expansion of its tree. Continue expanding until the starting tree and the ending tree are connected or the maximum number of samplings is reached;

[0024] S215. If the starting tree and the ending tree are connected, return the initial path node sequence. If the starting tree and the ending tree are still not connected after reaching the upper limit of the number of samplings, then continue to try step S22.

[0025] Specifically, step S22 includes:

[0026] S221. When the algorithm detects an active node If extension fails in all eight preset directions, multiply the step size by the decay factor. And with a new step size, try to expand in the original preset direction again;

[0027] S222. If a new candidate child node can be successfully generated in a certain direction. Then Add to the tree and resume expansion at the original step size; if expansion is still impossible in any direction, continue multiplying the step size by the decay factor. Repeat the above steps until the starting tree and the ending tree are connected or the set minimum step size threshold is reached. ;

[0028] S223. If the starting tree and the ending tree are connected, return the initial path node sequence. If the set minimum step size threshold is reached... If the starting tree and the ending tree are still not connected, the adaptive step size strategy fails, and the S23 step is tried again.

[0029] Specifically, step S23 includes:

[0030] S231, If ​​the currently active node If both the bidirectional expansion strategy and the adaptive step-size strategy for expanding the octree fail, then it is determined that... It is a "dead end" that cannot be further expanded;

[0031] S232, Return to The parent node is identified, and the system attempts to extend along other directions using an adaptive step-size strategy; if extension is still impossible in any direction, the system retreats to the parent node. Repeat the above operation on the parent node of the parent node until the starting tree and the ending tree are connected or back to the root node.

[0032] S233. If the starting tree and the ending tree are connected, return the initial path node sequence. If the starting tree and the ending tree are still not connected after returning to the root node, then the path planning fails.

[0033] Specifically, step S24 includes:

[0034] S241. Obtain the endpoints of the path segment. , The path segment formed;

[0035] S242, Traversal Check , The ball's center is surrounded by various obstacles. (recorded as) distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection;

[0036] S243, Calculate the projection coefficient The calculation formula is:

[0037] ;

[0038] S244. Calculate the coordinates of the nearest point H between the line segment and the center of the sphere. The calculation formula is:

[0039] ;

[0040] S245, calculate the distance between the line segment and the center of the sphere , if and , the line segment must collide with the obstacle, otherwise no collision occurs.

[0041] wherein the initial path node sequence is pruned by using a bisection pruning algorithm, specifically including:

[0042] S31, taking the complete path node list as input, and determining the starting node and the terminal node of the path, denoted as and , the corresponding node numbers are denoted as and , let the left end pointer be , the right end pointer be , and the path segment from to is called an interval;

[0043] S32, if the number of nodes in the interval is not less than 4, i.e. , take , wherein is a rounding function, find the middle node with the node number , and divide the entire interval into two sub-intervals, the left half interval is composed of the starting node to the middle node , and the right half interval is composed of the middle node to the terminal node ; for each sub-interval, if the number of nodes in the interval is still not less than 4, continue to repeat the S3 step to further subdivide the interval;

[0044] S33, if the number of nodes in the interval is 3, i.e. or or , determine whether the line connecting the left and right endpoints collides; if no collision occurs, connect the left and right endpoints and delete the middle node; if collision occurs, do not perform pruning operation;

[0045] S34, combine the pruning results of each interval to form a complete optimized path .

[0046] wherein the collision-free kinematic inverse solution corresponding to each path node is solved according to the mechanical arm model, specifically including:

[0047] ​S41. Establish the forward kinematics model of the robotic arm based on the DH parameter method;

[0048] S42. Using the connecting rod obtained by the DH parameter method as the axis of the cylinder and the connecting rod radius as the cylinder radius, establish the mechanical arm connecting rod bounding box model using a cylindrical bounding box.

[0049] S43, Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm;

[0050] S44. Based on the robotic arm model, the collision-free inverse kinematics solution corresponding to each path node is obtained through the collision detection strategy between the robotic arm link bounding box and the obstacle bounding sphere, thus obtaining the final robotic arm joint angle sequence. .

[0051] Specifically, step S44 includes:

[0052] S431. Obtain the endpoints of the path segment. , The path segment is used to obtain the radius of the cylindrical bounding box of the robotic arm link. ;

[0053] S432, Traversal Check , The ball's center is surrounded by various obstacles. (recorded as) distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection;

[0054] S433, Calculate the projection coefficient The calculation formula is:

[0055] ;

[0056] S434. Calculate the coordinates of the nearest point H between the line segment and the center of the sphere. The calculation formula is:

[0057] ;

[0058] S435, Calculate the distance between the line segment and the center of the sphere. ,like and If the line segment collides with the obstacle, then the line segment will inevitably collide with the obstacle; otherwise, no collision will occur.

[0059] This invention belongs to the field of robotic arm path planning, specifically relating to a spatial-interval dual divide-and-conquer method for robotic arm path planning. The steps include: acquiring the dimensions of a 3D spatial map, the starting and ending points, sampling parameters, and the shape and position of obstacles; constructing an initial collision-free path based on a bidirectional expanding octree strategy, employing an adaptive step size strategy, a branch backtracking strategy, and a collision detection strategy to improve the success rate of path planning; pruning and optimizing the path based on a bipartite pruning strategy; establishing a forward kinematics model of the robotic arm using the DH parameter method, and obtaining the final robotic arm joint angle sequence by calculating the collision-free inverse kinematics of each path node. This invention solves the problems of path redundancy, wasteful repeated exploration, and low solution efficiency in traditional path planning methods in complex 3D environments, effectively improving the efficiency and reliability of robotic arms in industrial automation, and has significant theoretical and engineering value. Attached Figure Description

[0060] Figure 1 This is a flowchart of the steps of a robotic arm path planning method based on space-interval double divide-and-conquer according to the present invention;

[0061] Figure 2 This is an octree structure diagram of the present invention;

[0062] Figure 3 This is a schematic diagram of the collision detection between a three-dimensional path segment and an obstacle-enclosed sphere according to the present invention;

[0063] Figure 4 This is a schematic diagram of the DH model of the robotic arm of the present invention;

[0064] Figure 5 This is a schematic diagram of the collision detection between the robotic arm link and the obstacle-surrounded ball according to the present invention. Detailed Implementation

[0065] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numerals in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Rather, they are merely examples of apparatuses and methods consistent with some aspects of the invention as detailed in the appended claims.

[0066] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The singular forms “a,” “the,” and “the” used in this invention and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0067] It should be understood that although the terms first, second, third, etc., may be used in this invention to describe various information, this information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, first information may also be referred to as second information without departing from the scope of this invention, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to a determination."

[0068] This invention provides a robotic arm path planning method based on spatial-interval dual divide-and-conquer, the method comprising:

[0069] Obtain the dimensions of the 3D spatial map and path planning parameters, including map side lengths. Starting point Finish line Obstacle shape and location Sampling step size Maximum number of sampling times ;

[0070] Based on the dimensions of the 3D spatial map and the path planning parameters, a bidirectional extended octree algorithm is used to obtain a collision-free initial path node sequence. ;

[0071] The initial path node sequence is processed using a branching pruning algorithm. Pruning is performed to obtain a collision-free optimized path node sequence. ;

[0072] A robotic arm model is established based on the DH parameter method, and the optimal path node sequence is determined. As the path node sequence of the robotic arm's end effector, the collision-free inverse kinematics solution corresponding to each path node is obtained based on the robotic arm model, resulting in the final robotic arm joint angle sequence. .

[0073] Specifically, obtaining the 3D spatial map size and path planning parameters includes:

[0074] Obstacle shape and location Represented by the bounding sphere, record the position of the center of the bounding sphere in a list. In the middle, a single element is denoted as Simultaneously record the radius of the enclosing sphere in the list. In the middle, a single element is denoted as .

[0075] Specifically, the step of employing a bidirectional extended octree algorithm based on the 3D spatial map size and path planning parameters includes:

[0076] Octree bidirectional expansion strategy;

[0077] Adaptive step size strategy;

[0078] Branch backtracking strategy;

[0079] Path segment and obstacle collision detection strategy.

[0080] Specifically, the octree bidirectional expansion strategy includes:

[0081] by and Initialize two independent octrees for the root node, called the start tree and the end tree. and All are set as active nodes ;

[0082] Each octree uses the currently active node as its basis. The most recently added node becomes the parent node, which extends continuously in eight preset directions in three-dimensional space until it encounters an obstacle and stops, generating candidate child nodes. These eight preset directions are composed of a set of orthogonal bases and their negative vectors in three-dimensional space. Furthermore, to avoid generating redundant paths, each active node... During expansion, directions pointing to its parent node will be excluded;

[0083] Candidate child nodes generated from the starting tree and the ending tree Two nodes are paired up to form a candidate node pair. Iterate through and check all candidate node pairs;

[0084] If there exists a pair of candidate nodes whose paths do not collide, the starting tree and the ending tree are considered successfully connected, and the algorithm ends. Otherwise, the pair with the smallest distance among all candidate node pairs is selected as the active node for the next round of expansion of its tree. Continue expanding until the starting tree and the ending tree are connected or the maximum number of samplings is reached;

[0085] If the starting tree and the ending tree are connected, return the initial path node sequence. If the starting tree and the ending tree are still not connected after reaching the upper limit of the number of samplings, continue to try the above steps.

[0086] Specifically, the adaptive step size strategy includes:

[0087] When the algorithm detects an active node If extension fails in all eight preset directions, multiply the step size by the decay factor. And with a new step size, try to expand in the original preset direction again;

[0088] If new candidate child nodes can be successfully generated in a certain direction Then Add to the tree and resume expansion at the original step size; if expansion is still impossible in any direction, continue multiplying the step size by the decay factor. Repeat the above steps until the starting tree and the ending tree are connected or the set minimum step size threshold is reached. ;

[0089] If the starting tree and the ending tree are connected, return the initial path node sequence. If the set minimum step size threshold is reached... If the starting tree and the ending tree are still not connected, the adaptive step size strategy fails, and the S23 step is tried again.

[0090] Specifically, the branch backtracking strategy includes:

[0091] If the current active node If both the bidirectional expansion strategy and the adaptive step-size strategy for expanding the octree fail, then it is determined that... It is a "dead end" that cannot be further expanded;

[0092] Return to The parent node is identified, and the system attempts to extend along other directions using an adaptive step-size strategy; if extension is still impossible in any direction, the system retreats to the parent node. Repeat the above operation on the parent node of the parent node until the starting tree and the ending tree are connected or back to the root node.

[0093] If the starting tree and the ending tree are connected, return the initial path node sequence. If the starting tree and the ending tree are still not connected after returning to the root node, then the path planning fails.

[0094] The path segment and obstacle collision detection strategy specifically includes:

[0095] Get the two endpoints of the path segment , The path segment formed;

[0096] Traversal and inspection , The ball's center is surrounded by various obstacles. (recorded as) distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection;

[0097] Calculate projection coefficients The calculation formula is:

[0098] ;

[0099] The formula for calculating the coordinates of the nearest point H between the line segment and the center of the sphere is as follows:

[0100] ;

[0101] Calculate the distance between the line segment and the center of the sphere. ,like and If the line segment collides with the obstacle, then the line segment will inevitably collide with the obstacle; otherwise, no collision will occur.

[0102] The method described above employs a branch-and-prune algorithm for the initial path node sequence. Pruning includes:

[0103] Take the complete list of path nodes as input and determine the start and end nodes of the path, denoted as . and The corresponding node number is denoted as and Let the left endpoint pointer Right endpoint pointer , said from arrive The path segment is an interval;

[0104] If the number of nodes in the interval is not less than 4, that is Then take ,in For the rounding function, find the node number as intermediate node Divide the entire interval into two sub-intervals, left and right. The left sub-interval starts from the starting node. To intermediate node Composed of, the right half of the interval is composed of the middle node To the terminal node Composition; For each sub-interval, if the number of nodes in the interval is still not less than 4, then continue to repeat step S3 to further subdivide the interval;

[0105] If the number of nodes in the interval is 3, that is or or If a collision occurs, check if the lines connecting the left and right endpoints collide. If no collision occurs, connect the left and right endpoints and delete the middle node. If a collision occurs, do not perform pruning.

[0106] The pruning results from each interval are merged to form a complete optimization path. .

[0107] Specifically, the step of obtaining the collision-free inverse kinematics solution for each path node based on the robotic arm model includes:

[0108] A forward kinematics model of the robotic arm is established based on the DH parameter method;

[0109] Using the DH parameter method, the connecting rod is taken as the axis of the cylinder, and the connecting rod radius is taken as the cylinder radius. A cylindrical bounding box model of the robotic arm connecting rod is established.

[0110] Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm;

[0111] Based on the robotic arm model, the collision-free inverse kinematics solution for each path node is obtained through a collision detection strategy between the robotic arm link bounding box and the obstacle bounding sphere, resulting in the final robotic arm joint angle sequence. .

[0112] Specifically, the collision detection strategy between the robotic arm linkage enclosure box and the obstacle enclosure ball includes:

[0113] Get the two endpoints of the path segment , The path segment is used to obtain the radius of the cylindrical bounding box of the robotic arm link. ;

[0114] Traversal and inspection , The ball's center is surrounded by various obstacles. (recorded as) distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection;

[0115] Calculate projection coefficients The calculation formula is:

[0116] ;

[0117] The formula for calculating the coordinates of the nearest point H between the line segment and the center of the sphere is as follows:

[0118] ;

[0119] Calculate the distance between the line segment and the center of the sphere. ,like and If the line segment collides with the obstacle, then the line segment will inevitably collide with the obstacle; otherwise, no collision will occur.

[0120] The following specific embodiments are provided to illustrate the technical solution of the present invention in detail.

[0121] Figure 1 This invention provides a flowchart of a robotic arm path planning method based on spatial-interval dual divide-and-conquer. Please refer to... Figure 1 The method provided in this embodiment may include:

[0122] S1. Obtain the dimensions of the 3D spatial map and path planning parameters, including map side lengths. Starting point Finish line Obstacle shape and location Sampling step size Maximum number of sampling times ;

[0123] It should be noted that three-dimensional space can be represented in various forms; this embodiment uses a cube representation, i.e., represented by the map's side length. The dimensions of the cube in three-dimensional space are determined as follows: .

[0124] In specific implementation, for example in this embodiment, a starting position is set. Set the endpoint position to (0.005m, 0.005m, 0.005m). Given (0.495m, 0.495m, 0.495m), take... Generate a set of spheres of random size and position. The spherical bounding box used to represent obstacles is set to have 10 obstacles. ;Pick .

[0125] S2. Based on the dimensions of the 3D spatial map and the path planning parameters, a bidirectional extended octree algorithm is used to obtain a collision-free initial path node sequence. ;

[0126] In a specific implementation, for example, in this embodiment, the pseudocode for the bidirectional extended octree function (Octree-Connect) is as follows:

[0127] FUNCTION Octree-Connect ( , , MaxSampleNumber, StepLength,Map)

[0128] StartTree ← [] / / Initialize the starting point tree

[0129] GoalTree ← [] / / Initialize the endpoint tree

[0130] StartTree ← AddNode(StartTree, / / Add the starting point to the starting point tree

[0131] GoalTree ← AddNode(GoalTree, / / Add the endpoint to the endpoint tree

[0132] Directions ← [1,1,1;1,1,-1;1,-1,1;1,-1,-1;-1,1,1;-1,1,-1;-1,-1,1;-1,-1,-1] / / Preset exploration directions

[0133] i ← 1

[0134] WHILE i <MaxSampleNumber DO

[0135] StartCandidateNodes ← GetCandidateNodes( (Directions, Map) / / Octagonal bidirectional expansion strategy

[0136] GoalCandidateNodes ← GetCandidateNodes( (Directions, Map) / / Octagonal bidirectional expansion strategy

[0137] WHILE CandidateNodes == [] AND TempStepLength ≥ MinStepLength DO / / Adaptive step size strategy

[0138] TempStepLength ← HalfStepLength(TempStepLength) / / Halve the step size

[0139] CandidateNodes ← GetCandidateNodes( (Directions, Map) / / Regenerate candidate nodes

[0140] END WHILE

[0141] WHILE CandidateNodes == [] AND ≠ OR DO / / Branch backtracking strategy

[0142] ← BackToParentNode( / / Backtrack to the parent node of the current node

[0143] CandidateNodes ← GetCandidateNodes( (Directions, Map) / / Regenerate candidate nodes

[0144] END WHILE

[0145] BestPairNodes ← FindBestPairNodes(StartCandidateNodes,GoalCandidateNodes, Map) / / Finds the best candidate node pairs

[0146] StartTree ← AddNode(StartTree, BestPairNodes[1]) / / Adds the candidate nodes from the start side to the start tree.

[0147] GoalTree ← AddNode(GoalTree, BestPairNodes[2]) / / Adds the candidate nodes on the endpoint side to the endpoint tree.

[0148] IF CollisionFree(BestPairNodes[1], BestPairNodes[2], Map) == TRUETHEN / / Check for collisions between candidate node pairs

[0149] InitialPath ← Combine(StartTree, GoalTree) / / Connects the starting tree and the ending tree

[0150] RETURN InitialPath / / Returns the complete tree

[0151] END IF

[0152] i ← i + 1

[0153] END WHILE

[0154] END FUNCTION

[0155] S3. Use a branching pruning algorithm to process the initial path node sequence. Pruning is performed to obtain a collision-free optimized path node sequence. ;

[0156] In a specific implementation, for example, in this embodiment, the pseudocode for the BinaryPruning function is as follows:

[0157] FUNCTION BinaryPruning(InitialPath, , Map)

[0158] PrunedPath ← InitialPath / / Initialize the path to be pruned

[0159] ← / / Initialize the left node as the starting node

[0160] ← / / Initialize the right node as the end node

[0161] left ← Index( / / Initialize left to the left node number

[0162] right ← Index( / / Initialize right to the right node number

[0163] IF right - left ≥ 3 THEN

[0164] middle ← Round((left + right) / 2)

[0165] IF middle - left ≥ 3 THEN / / Prune the left half of the data.

[0166] LeftPrunedPath ← BinaryPruning(PrunedPath, , Map) / / Recursively call the branch pruning algorithm on the left half of the partition

[0167] ELSE IF middle - left == 2 AND CollisionFree( , Map) THEN

[0168] DeleteRedundantNode() / / Deletes redundant nodes

[0169] END IF

[0170] IF right - middle ≥ 3 THEN / / Prune the right half of the data.

[0171] RightPrunedPath ← BinaryPruning(PrunedPath, , Map) / / Recursively call the branch pruning algorithm on the right half of the partition

[0172] ELSE IF right - middle == 2 AND CollisionFree( , Map)THEN

[0173] DeleteRedundantNode() / / Deletes redundant nodes

[0174] END IF

[0175] ELSE IF right - left == 2 AND CollisionFree( , Map) THEN

[0176] DeleteRedundantNode() / / Deletes redundant nodes

[0177] END IF

[0178] PrunedPath ← Combine(LeftPrunedPath, RightPrunedPath) / / Reconnect the left and right halves of the graph.

[0179] RETURN PrunedPath / / Returns the pruned path

[0180] END FUNCTION

[0181] S4. Establish a robotic arm model based on the DH parameter method, and optimize the path node sequence. As the path node sequence of the robotic arm's end effector, the collision-free inverse kinematics solution corresponding to each path node is obtained based on the robotic arm model, resulting in the final robotic arm joint angle sequence. .

[0182] In specific implementation, for example, the method provided in this embodiment may include:

[0183] S41. Establish the forward kinematics model of the robotic arm based on the DH parameter method.

[0184] It should be noted that the robotic arm used in this embodiment is the Universal Robots UR5e robotic arm, and the standard DH method from the DH parameter method is used to model the UR5e robotic arm. The standard DH method is based on joint angles... Linkage offset Linkage length Twist angle The homogeneous transformation matrix of adjacent joints is constructed in the following order, as shown in the following formula:

[0185]

[0186] By multiplying the homogeneous transformation matrices of adjacent joints in sequence, the expression for the UR5e robotic arm end effector relative to the base coordinates is obtained as follows:

[0187] ;

[0188] Figure 4 This invention provides an example of standard DH (Hyperdynamic Dexterity) modeling for the UR5e robotic arm. The DH parameters of the UR5e robotic arm are shown in Table 1.

[0189] Table 1 DH parameters of the UR5e robotic arm

[0190]

[0191] S42. Using the connecting rod obtained by the DH parameter method as the axis of the cylinder and the connecting rod radius as the cylinder radius, a bounding box model of the robotic arm connecting rod is established using a cylindrical bounding box.

[0192] It should be noted that, in practice, the task of creating the bounding box model of the robotic arm linkage can be accomplished by setting up a robot model with collision geometry in MATLAB.

[0193] S43, Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm.

[0194] It should be noted that PrunedPath comes from the output of step S3.

[0195] S44. Based on the robotic arm model, the collision-free inverse kinematics solution corresponding to each path node is obtained through the collision detection strategy between the robotic arm link bounding box and the obstacle bounding sphere, thus obtaining the final robotic arm joint angle sequence. .

[0196] It should be noted that when performing inverse kinematics calculations on the path points in the end effector's path, collision detection is required based on position and attitude constraints, and the final sequence of robot arm joint angles is obtained through path interpolation. .

[0197] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A method for robot path planning based on space-interval binary divide-and-conquer, characterized in that, The method comprises the following steps: S1, acquire three-dimensional space map size and path planning parameters, including map side length , start point position , end point position , obstacle shape and position , sampling step length , upper limit of sampling times ; S2, using bidirectional expansion octree algorithm according to the three-dimensional space map size and path planning parameters, obtaining a collision-free initial path node sequence ; S3, using a binary pruning algorithm to prune the initial path node sequence to obtain a collision-free optimized path node sequence ; S4, according to the D-H parameter method, the mechanical arm model is established, and the optimized path node sequence As the mechanical arm end effector path node sequence, the collision-free inverse kinematics solution corresponding to each path node is obtained according to the mechanical arm model, and the final mechanical arm joint angle sequence is obtained .

2. The method of claim 1, wherein, The S1 step comprises the following steps: Obstacle shape and position The enclosing sphere center position is recorded in a list where individual elements are denoted by The enclosing sphere radius is recorded in a list where individual elements are denoted by .

3. The method of claim 1, wherein, The S2 step comprises the following steps: S21, an octree bidirectional expansion strategy; S22, an adaptive step strategy; S23, a branch backtracking strategy; S24, a path segment and obstacle collision detection strategy.

4. The method of claim 3, wherein, The S21 step comprises the following steps: S211、with and Initialize two independent octrees as root nodes, called the start tree and the end tree, and set and as active nodes ; S212, each octree takes the current active node , i.e., the most recently added node, as the parent node, and keeps extending along eight preset directions in the three-dimensional space until it encounters an obstacle and generates a candidate child node , the eight preset directions are composed of a set of orthogonal bases and their negative vectors in the three-dimensional space, and to avoid generating redundant paths, each active node excludes the direction pointing to its parent node when expanding; S213, generating candidate child nodes from the start tree and the end tree form a pair of candidate nodes , traversing and checking all candidate node pairs S214, if there is no collision between the paths of a pair of candidate nodes, it is considered that the start tree and the end tree are successfully connected, the algorithm ends, otherwise, select the pair with the smallest distance from all candidate node pairs as the active nodes for the next round of expansion of the tree , continue to expand until the start tree and the end tree are connected or the upper limit of the sampling number is reached; S215、if the start tree and the end tree are connected, return the initial path node sequence ; if the start tree and the end tree are still not connected after reaching the upper limit of the sampling number, continue to try the step S22.

5. The method of claim 3, wherein, The S22 step comprises the following steps: S221、When the algorithm detects that a certain active node When the step cannot be extended successfully in all eight preset directions, multiply the step by the attenuation coefficient And try to expand in the original preset direction with a new step. S222、if a new candidate child node can be successfully generated in a certain direction , then the new candidate child node is added to the tree, and the step size is restored to the original step size to continue the expansion; if the expansion cannot be continued in all directions, then the step size is multiplied by the decay coefficient , and the above steps are repeated until the start tree and the end tree are connected or a minimum step size threshold is reached . ; S223, if the start tree and the end tree are connected, return the initial path node sequence ; if a set minimum step threshold is reached if the start tree and the end tree are still not connected, the adaptive step strategy fails and the procedure continues with step S23.

6. The method of claim 3, wherein, The S23 step comprises the following steps: S231、If the current active node fails to expand by both the octree bidirectional expansion strategy and the adaptive step strategy, it is determined as a "dead end" that cannot be expanded further. ​ S232, back to the parent node of the parent node and try to continue expanding in other directions from the parent node by adaptive step strategy; if it is still impossible to extend in each direction, continue to back to the parent node of the parent node of the parent node and repeat the above operation until the start tree and the end tree are connected or back to the root node; the parent node of the parent node and try to continue expanding in other directions from the parent node by adaptive step strategy; if it is still impossible to extend in each direction, continue to back to the parent node of the parent node of the parent node and repeat the above operation until the start tree and the end tree are connected or back to the root node; S233, if the start tree and the end tree are connected, return the initial path node sequence If the start tree and the end tree are still not connected after returning to the root node, the path planning fails.

7. The method of claim 3, wherein, The S24 step comprises the following steps: S241, acquiring a path segment consisting of two end points of the path segment , ​ S242, Traversal Check , The center of the ball is surrounded by various obstacles. , recorded as distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection; S243、calculating the projection coefficient The calculation formula is: ； S244, calculating the H coordinate of the closest point between the line segment and the sphere center, and the calculation formula is: ； S245, calculate the distance between the line segment and the sphere center If and , the line segment will collide with the obstacle, otherwise not.

8. The method of claim 1, wherein, The S3 step comprises the following steps: S31, take the complete list of path nodes as input, and determine the start node and end node of the path, denoted as and , the corresponding node numbers are denoted as and , let the left end pointer be , the right end pointer be , and call the path segment from to an interval; S32, if the number of nodes in the interval is not less than 4, i.e. then take where is a rounding function, find the middle node with node number and divide the whole interval into two sub-intervals, the left half interval consists of the start node to the middle node and the right half interval consists of the middle node to the end node ; for each sub-interval, if the number of nodes in the interval is still not less than 4, continue to repeat the step S3 to further subdivide the interval; S33, if the number of nodes in the interval is 3, i.e. or or whether the line connecting the left and right end points collides; if not, connecting the left and right end points and deleting the middle node; if it collides, no pruning operation is performed; S34, merge the pruning results of each interval to form a complete optimized path .

9. The method of claim 1, wherein, The S4 step comprises the following steps: S41, establishing a forward kinematics model of the mechanical arm according to a D-H parameter method; S42, taking the link obtained by the D-H parameter method as a cylinder axis and the link radius as a cylinder radius, and establishing a bounding box model of the link of the mechanical arm by using a cylinder bounding box; S43, the optimized path node sequence as the mechanical arm end effector path node sequence; S44、According to the robot arm model, the collision-free inverse kinematics solution corresponding to each path node is obtained by the collision detection strategy of the robot arm link bounding box and the obstacle bounding sphere, and the final robot arm joint angle sequence is obtained .

10. The method of claim 9, wherein, The S44 step comprises the following steps: S441、acquire the path segment composed of the two end points of the path segment , acquire the radius of the cylinder bounding box of the mechanical arm connecting rod ; S442, Traversal Check , The center of the ball is surrounded by various obstacles. , recorded as distance Is it smaller than the radius of the sphere surrounded by obstacles? ,like If the endpoint is located on the surface or inside the sphere, it means the line segment must have collided with the obstacle; if If so, proceed to the next step of the inspection; S443、calculating the projection coefficient The calculation formula is: ； S444, calculating the H coordinate of the closest point between the line segment and the sphere center, and the calculation formula is: ； S445, calculate the distance between the line segment and the sphere center If and , the line segment will collide with the obstacle, otherwise not.

Images