Mechanical arm path planning method based on space-interval double division and conquer

Abstract

The invention belongs to the field of mechanical arm path planning, and particularly relates to a mechanical arm path planning method based on space-interval double division and conquer, which comprises the following steps: acquiring a three-dimensional space map size, starting point and ending point positions, sampling parameters, and obstacle shapes and positions; an initial collision-free path is constructed based on a bidirectional expansion octree strategy, and the path planning success rate is improved by adopting an adaptive step length strategy, a branch backtracking strategy and a collision detection strategy; performing pruning optimization on the path based on a bipartition pruning strategy; a mechanical arm forward kinematics model is established according to a D-H parameter method, and a final mechanical arm joint angle sequence is obtained by calculating a collision-free inverse solution of each path node. According to the method, the problems of regional redundancy search and high global path pruning calculation complexity caused by random sampling of a sampling-based traditional algorithm in mechanical arm path planning are solved, the high efficiency and reliability of the mechanical arm in industrial automation can be effectively improved, and the method has 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 new generation intelligent manufacturing and industry 5.0, mechanical arms are widely used in manufacturing, logistics, medical treatment, aerospace and other industries. Mechanical arms effectively improve production efficiency and work precision, and promote industrial upgrading and intelligent transformation, by virtue of the advantages of high-precision motion control and flexible operation. 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 consumption economy. Therefore, it has important theoretical significance and engineering value to research a high-efficiency and reliable path planning algorithm for mechanical arms.

[0003] In a human-machine cooperation scene, a mechanical arm has a certain autonomy. Mechanical arm path planning aims to generate a collision-free path connecting a starting point and an ending point in a three-dimensional space without the help of artificial teaching, and the 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 a 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. The above methods ① and ③ have slow running speed, the method ② has the problem of being easy to fall into a local minimum trap, and the method ④ has the problems of a large number of 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 a mechanical arm.

[0006] Specifically, the application is realized by the following technical scheme: The application provides a mechanical arm path planning method based on space-interval bisection, which comprises: S1, obtaining a three-dimensional space map size and path planning parameters, including a map side length , a start point position , an end point position , an obstacle shape and position , a sampling step length , and an upper limit of sampling times ; S2, using a 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 ; S3, using a halving pruning algorithm to prune the initial path node sequence to obtain a collision-free optimized path node sequence ; S4, establishing a robot arm model according to a D-H parameter method, taking the optimized path node sequence as a robot arm end effector path node sequence, and calculating a collision-free inverse kinematics solution corresponding to each path node according to the robot arm model to obtain a final robot arm joint angle sequence .

[0007] The three-dimensional space map size and the path planning parameters are obtained, specifically including: obstacle shape and position The position of the sphere center of the bounding sphere is recorded in a list , where a single element is denoted as , and the radius of the bounding sphere is recorded in a list , where a single element is denoted as .

[0008] The bidirectional expansion octree algorithm is used according to the three-dimensional space map size and the path planning parameters, specifically including: S21, an octree bidirectional expansion strategy S22, an adaptive step length strategy S23, a branch backtracking strategy S24, a path segment and obstacle collision detection strategy

[0009] The S21 step specifically includes: S211, initializing two independent octrees with and as root nodes, called start point tree and end point tree, and setting and as active nodes ; S212, each octree takes the current active node The newly added node (i.e. the parent node) is continuously extended along eight preset directions in three-dimensional space until it encounters an obstacle and stops and generates a candidate child node The eight preset directions are composed of a set of orthogonal bases and their negative vectors in three-dimensional space, and to avoid generating redundant paths, each active node excludes the direction pointing to its parent node when expanding; S213, the candidate child nodes generated by the start point tree and the end point tree are paired to form a pair of candidate nodes , and all the pairs of candidate nodes are traversed and checked; S214, if there is a path between a pair of candidate nodes without collision, it is considered that the start point tree and the end point tree are successfully connected, and the algorithm ends, otherwise, the pair of candidate nodes with the smallest distance is selected from all the pairs of candidate nodes as the active nodes for the next round of expansion of the tree , and the expansion continues until the start point tree and the end point tree are connected or the upper limit of the sampling number is reached; S215, if the start point tree and the end point tree are connected, the initial path node sequence is returned ; if the start point tree and the end point tree are still not connected after reaching the upper limit of the sampling number, the step S22 is continued to be tried.

[0010] The S22 step specifically includes: S221, when the algorithm detects that a certain active node cannot be successfully extended in any of the eight preset directions, the step length is multiplied by a decay coefficient , and the expansion in the original preset direction is reattempted with the new step length; S222, if a new candidate child node can be successfully generated in a certain direction, the is added to the tree, and the expansion continues with the original step length; if the extension cannot be performed in any direction, the step length is multiplied by the decay coefficient and the above steps are repeated until the start point tree and the end point tree are connected or the minimum step length threshold is reached ; S223, if the start point tree and the end point tree are connected, the initial path node sequence is returned ; if the start point tree and the end point tree are still not connected after reaching the minimum step length threshold , the adaptive step length strategy fails, and the step S23 is continued to be tried.

[0011] The S23 step specifically includes: S231, if the octree bidirectional expansion strategy and the adaptive step length strategy of the current active node both fail, it is determined that it is a "dead end" that cannot be further expanded; 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. 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.

[0012] Specifically, step S24 includes: S241. Obtain the endpoints of the path segment. , The path segment formed; 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, Calculate the projection coefficient The calculation formula is: ; S244. Calculate the coordinates of the nearest point H between the line segment and the center of the sphere. The calculation formula is: ; S245, 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.

[0013] The method described above employs a branch-and-prune algorithm for the initial path node sequence. Pruning includes: S31. 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; S32. 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; S33. 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. S34. Merge the pruning results of each interval to form a complete optimization path. .

[0014] Specifically, the step of obtaining the collision-free inverse kinematics solution for each path node based on the robotic arm model includes: S41. Establish the forward kinematics model of the robotic arm based on the DH parameter method; 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. S43, Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm; 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. .

[0015] Specifically, step S44 includes: 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. ; S432, Traversal Check , The center of the ball is surrounded by various obstacles. (recorded as) distance whether less than the obstacle surrounding sphere radius , if , the end point is located on the surface of the sphere or inside the sphere, indicating that the line segment must collide with the obstacle; if , the next step is checked; S433, calculate the projection coefficient , the calculation formula is: ; S434, calculate the coordinates of the nearest point H of the line segment and the sphere center, and the calculation formula is: ; S435, calculate the distance between the line segment and the sphere center , if and , the line segment must collide with the obstacle, otherwise no collision occurs.

[0016] 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 binary partition, which comprises the following steps: 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 path planning success rate by using an adaptive step strategy, a branch backtracking strategy and a collision detection strategy; pruning and optimizing the path based on a halving pruning strategy; and establishing a mechanical arm forward kinematics model according to the D-H parameter method, and obtaining the final mechanical arm joint angle sequence by calculating the collision-free inverse solution of each path node. The application solves the problems of path redundancy, repeated exploration waste and low solving efficiency of the traditional path planning method in a complex three-dimensional environment, can effectively improve the efficiency and reliability of the mechanical arm in industrial automation, and has important theoretical significance and engineering value. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 is a step flow chart of the mechanical arm path planning method based on space-interval binary partition of the application; Figure 2 is an octree structure diagram of the application; Figure 3 is a three-dimensional path segment and obstacle surrounding sphere collision detection schematic diagram of the application; Figure 4 is a mechanical arm D-H modeling schematic diagram of the application; Figure 5 is a mechanical arm connecting rod and obstacle surrounding sphere collision detection schematic diagram of the application. DETAILED DESCRIPTION

[0018] The exemplary embodiments will be described in detail herein with reference to a few examples. The following description is not intended to represent all embodiments in accordance with the present application. Rather, the following description presents several examples of devices and methods in accordance with some aspects of the present application, which are described in detail in the claims that follow below.

[0019] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0020] It is to be understood that the singular forms "a", "an", and "the" include plural referents unless the context clearly dictates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0021] The present application provides a mechanical arm path planning method based on space-interval bisection, the method comprising: acquiring 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 , upper limit of sampling times ; adopting bidirectional expansion octree algorithm according to three-dimensional space map size and path planning parameters to obtain a collision-free initial path node sequence ; adopting bisection pruning algorithm to prune the initial path node sequence to obtain a collision-free optimized path node sequence ; establishing a mechanical arm model according to D-H parameter method, taking the optimized path node sequence as the mechanical arm end effector path node sequence, and according to the mechanical arm model, obtaining the collision-free inverse kinematics solution corresponding to each path node to obtain a final mechanical arm joint angle sequence .

[0022] The obtaining of the three-dimensional space map size and the path planning parameter specifically comprises: Obstacle shape and position The position of the bounding sphere center is recorded in a list , where a single element is denoted as The radius of the bounding sphere is recorded in a list , where a single element is denoted as .

[0023] The bidirectional expansion octree algorithm is adopted according to the three-dimensional space map size and the path planning parameter, and specifically comprises: An octree bidirectional expansion strategy; An adaptive step length strategy; A branch backtracking strategy; A path segment and obstacle collision detection strategy.

[0024] The octree bidirectional expansion strategy specifically comprises: Two independent octrees are initialized with and as root nodes, which are called the start point tree and the end point tree, and and are set as active nodes ; Each octree continuously extends in three-dimensional space along eight preset directions with the current active node , i.e., the most recently added node, as the parent node, until it encounters an obstacle and generates a candidate child node , and the eight preset directions are composed of a set of orthogonal bases and their negative vectors in three-dimensional space, and to avoid generating redundant paths, each active node will exclude the direction pointing to its parent node when expanding; The candidate child nodes generated by the start point tree and the end point tree are paired to form a pair of candidate nodes , and all candidate node pairs are traversed and checked; If there is a pair of candidate nodes whose path between them has no collision, it is considered that the start point tree and the end point tree are successfully connected, and the algorithm ends, otherwise, the pair of candidate nodes with the smallest distance is selected from all candidate node pairs as the active nodes for the next round of expansion of the tree , and the expansion continues until the start point tree and the end point tree are connected or the upper limit of the sampling number is reached; If the start point tree and the end point tree are connected, the initial path node sequence is returned; if the start point tree and the end point tree are still not connected after reaching the upper limit of the sampling number, the above steps are continued to be tried.

[0025] Specifically, the adaptive step size strategy includes: 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; 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 still fails 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. ; 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.

[0026] Specifically, the branch backtracking strategy includes: 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; 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. 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.

[0027] The path segment and obstacle collision detection strategy specifically includes: Get the two endpoints of the path segment , The path segment formed; 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; Calculate projection coefficients The calculation formula is: ;

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

[0029] 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.

[0030] The method described above employs a branch-and-prune algorithm for the initial path node sequence. Pruning includes: 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; 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; 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. The pruning results from each interval are merged to form a complete optimization path. .

[0031] Specifically, the step of obtaining the collision-free inverse kinematics solution for each path node based on the robotic arm model includes: A forward kinematics model of the robotic arm is established based on the DH parameter method; 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. Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm; 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. .

[0032] Specifically, the collision detection strategy between the robotic arm linkage enclosure box and the obstacle enclosure ball includes: 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. ; 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; Calculate projection coefficients The calculation formula is: ;

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

[0034] 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.

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

[0036] 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 1The method provided by the embodiment can include: S1, acquiring a three-dimensional space map size and path planning parameters, including a map edge length , a start point position , an end point position , an obstacle shape and position , a sampling step length , and an upper limit of a sampling number .

[0037] It should be noted that the three-dimensional space can be represented in various forms, and in the embodiment, a cubic representation is adopted, that is, the size of the cubic three-dimensional space is determined by the map edge length . .

[0038] In specific implementation, for example, in the embodiment, the start point position is set as (0.005 m, 0.005 m, 0.005 m), the end point position is set as (0.495 m, 0.495 m, 0.495 m), the map edge length is taken as 0.5 m, a group of randomly sized and positioned spheres are generated to represent the spherical bounding boxes of obstacles, the number of obstacles is taken as 10, and the sampling step length is taken as 0.05 m. .

[0039] S2, acquiring a collision-free initial path node sequence by using a bidirectional expansion octree algorithm according to the three-dimensional space map size and the path planning parameters . In specific implementation, for example, in the embodiment, the pseudo code of the bidirectional expansion octree function (Octree-Connect) is as follows: FUNCTION Octree-Connect( , , MaxSampleNumber, StepLength,Map) StartTree ← [] / / initialize the start point tree GoalTree ← [] / / initialize the end point tree StartTree ← AddNode(StartTree, ) / / add the start point to the start point tree GoalTree ← AddNode(GoalTree, ) / / add the end point to the end point tree 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 i ← 1 WHILE i<MaxSampleNumber DO StartCandidateNodes ← GetCandidateNodes( , Directions, Map) / / octree bidirectional expansion strategy GoalCandidateNodes ← GetCandidateNodes( , Directions, Map) / / octree bidirectional expansion strategy WHILE CandidateNodes == [] AND TempStepLength ≥ MinStepLength DO / / adaptive step length strategy TempStepLength ← HalfStepLength(TempStepLength) / / halve the step length CandidateNodes ← GetCandidateNodes( , Directions, Map) / / regenerate candidate nodes END WHILE WHILE CandidateNodes == [] AND ≠ OR DO / / branch backtracking strategy ← BackToParentNode( ) / / backtrack to the parent node of the current node CandidateNodes ← GetCandidateNodes( , Directions, Map) / / regenerate candidate nodes END WHILE BestPairNodes ← FindBestPairNodes(StartCandidateNodes,GoalCandidateNodes, Map) / / find the best candidate node pair StartTree ← AddNode(StartTree, BestPairNodes[1]) / / Adds the candidate nodes from the start side to the start tree. GoalTree ← AddNode(GoalTree, BestPairNodes[2]) / / Adds the candidate nodes on the endpoint side to the endpoint tree. IF CollisionFree(BestPairNodes[1], BestPairNodes[2], Map) == TRUETHEN / / Check for collisions between candidate node pairs InitialPath ← Combine(StartTree, GoalTree) / / Connects the starting tree and the ending tree RETURN InitialPath / / Returns the complete tree END IF i ← i + 1 END WHILE END FUNCTION 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. ; In a specific implementation, for example, in this embodiment, the pseudocode for the BinaryPruning function is as follows: FUNCTION BinaryPruning(InitialPath, , Map) PrunedPath ← InitialPath / / Initialize the path to be pruned ← / / Initialize the left node as the starting node ← / / Initialize the right node as the end node left ← Index( / / Initialize left to the left node number right ← Index( / / Initialize right to the right node number IF right - left ≥ 3 THEN middle ← Round((left + right) / 2) IF middle - left ≥ 3 THEN / / Prune the left half of the data. LeftPrunedPath ← BinaryPruning(PrunedPath, , Map) / / Recursively call the branch pruning algorithm on the left half of the partition ELSE IF middle - left == 2 AND CollisionFree( , Map) THEN DeleteRedundantNode() / / Deletes redundant nodes END IF IF right - middle ≥ 3 THEN / / Prune the right half of the data. RightPrunedPath ← BinaryPruning(PrunedPath, , Map) / / Recursively call the branch pruning algorithm on the right half of the partition ELSE IF right - middle == 2 AND CollisionFree( , Map)THEN DeleteRedundantNode() / / Deletes redundant nodes END IF ELSE IF right - left == 2 AND CollisionFree( , Map) THEN DeleteRedundantNode() / / Deletes redundant nodes END IF PrunedPath ← Combine(LeftPrunedPath, RightPrunedPath) / / Reconnect the left and right halves of the graph. RETURN PrunedPath / / Returns the pruned path END FUNCTION S4. Establish a robotic arm model based on the DH parameter method, and optimize the path node sequence. As the sequence of path nodes of the end effector of the mechanical arm, the inverse kinematics solution without collision corresponding to each path node is obtained according to the mechanical arm model, and a sequence of final mechanical arm joint angles is obtained .

[0040] In a specific implementation, for example, the method provided in the embodiment can include: S41, a forward kinematics model of the mechanical arm is established according to the D-H parameter method.

[0041] It should be noted that the mechanical arm model selected in the embodiment is UR5e mechanical arm of the company, and the standard D-H method in the D-H parameter method is used to model the UR5e mechanical arm. The standard D-H method constructs the homogeneous transformation matrix of adjacent joints in the order of joint angle Link offset Link length Torsion angle , please refer to the following formula:

[0042] Multiply the homogeneous transformation matrices of adjacent joints in order to obtain the expression of the end effector of the UR5e mechanical arm relative to the base coordinate: ;

[0043] Figure 4 An example of modeling the UR5e mechanical arm by the standard D-H method is provided in the embodiment. The D-H parameters of the UR5e mechanical arm are shown in Table 1: Table 1 D-H parameters of UR5e mechanical arm

[0044] S42, the link obtained by the D-H parameter method is taken as the axis of the cylindrical body, the link radius is taken as the radius of the cylindrical body, and the cylindrical bounding box is used to establish the mechanical arm link bounding box model.

[0045] It should be noted that in a specific implementation, the task of establishing the mechanical arm link bounding box model can be completed by setting the robot model loaded with the collision geometry in MATLAB.

[0046] S43, the optimized path node sequence as the sequence of path nodes of the end effector of the mechanical arm.

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

[0048] 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. .

[0049] 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. .

[0050] 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 robotic arm path planning method based on spatial-interval dual divide-and-conquer, characterized in that, Includes the following steps: 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 ; 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. ; 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. ; 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. .

2. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 1, characterized in that, In step S1: 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 .

3. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 1, characterized in that, The S2 step consists of the following strategies: S21, Octree bidirectional expansion strategy; S22, Adaptive step size strategy; S23, Branch backtracking strategy; S24. Path segment and obstacle collision detection strategy.

4. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 3, characterized in that, The S21 step includes: 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 ; 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; 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; 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; 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.

5. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 3, characterized in that, The S22 step includes: 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; 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 still fails 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. ; 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.

6. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 3, characterized in that, Step S23 includes: 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; 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. 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.

7. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 3, characterized in that, Step S24 includes: S241. Obtain the endpoints of the path segment. , The path segment formed; 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; S243, Calculate the projection coefficient The calculation formula is: ； S244. Calculate the coordinates of the nearest point H between the line segment and the center of the sphere. The calculation formula is: ； S245, 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.

8. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 1, characterized in that, The S3 step includes: S31. 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; S32. 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; S33. 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. S34. Merge the pruning results of each interval to form a complete optimization path. .

9. The robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 1, characterized in that, The S4 step includes: S41. Establish the forward kinematics model of the robotic arm based on the DH parameter method; 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. S43, Optimize the path node sequence As a sequence of path nodes for the end effector of a robotic arm; 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. .

10. A robotic arm path planning method based on spatial-interval dual divide-and-conquer as described in claim 9, characterized in that, The S44 step includes: 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. ; 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; S433, Calculate the projection coefficient The calculation formula is: ； S434. Calculate the coordinates of the nearest point H between the line segment and the center of the sphere. The calculation formula is: ； 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.

Images