A two-way extended robot arm path planning method and system under pose constraint

Abstract

The application discloses a kind of bidirectional expansion mechanical arm path planning method and system under pose constraint, specific steps are as follows: S1 obtains the initial position and final position coordinate of mechanical arm;S2 is improved to BT-RRT* algorithm using the bidirectional expansion strategy guided by target bias strategy to RRT* algorithm, to the initial position and final position of mechanical arm as the starting point and target point of BT-RRT* algorithm carries out mechanical arm path planning, obtains initial planning path T;S3 is smoothed using pose constraint path optimization strategy to initial planning path T, obtains the final motion path of mechanical arm.The method of the application is based on progressive optimal quick random search tree RRT, designs target bias strategy guided bidirectional expansion strategy BT-RRT* and pose constraint path optimization strategy PCO, to the generated path pruning eliminates redundancy, shortens the length of feasible path, simultaneously to the redundant node is eliminated in the form of iterative adjustment Smooth optimization, improve path generation quality.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm path planning technology, specifically to a bidirectional extended robotic arm path planning method and system under pose constraints. Background Technology

[0002] Robotic arm path planning refers to planning a collision-free path from the starting point to the target point for the robotic arm in a short time, given obstacles in the workspace. Existing path planning methods are mainly divided into three categories: The first category is grid segmentation algorithms, which traverse all neighboring points around the starting point in a grid map environment, expanding outwards to find the destination. These algorithms have strong search capabilities, but the number of nodes increases with spatial complexity, leading to high memory consumption and low computational efficiency. The second category is biomimetic algorithms, which simulate collective intelligence behavior using individual algorithms. They explore the global optimal solution in high-performance areas through parallel search across the entire solution space, exhibiting strong adaptability and learning capabilities. Examples include particle swarm optimization, genetic algorithms, and ant colony optimization. However, these algorithms require a large amount of memory and their optimization speed is easily affected by parameters. The third category is random sampling algorithms, which rapidly explore random trees. RandomTree (RRT) is a representative algorithm that can solve tasks with high path planning complexity due to the high-dimensional linkage structure of robotic arms without preprocessing the state space. It has the characteristics of fast search speed and easy handling of constraints, so it is widely used in the research of robotic arm path planning problems. However, the blind expansion of RRT has led to the algorithm spending a lot of time unnecessarily, generating too many invalid paths, and making it difficult to plan a smooth execution path for the robotic arm.

[0003] Current improvements to the RRT algorithm mainly focus on improving single performance metrics such as convergence speed in different ways. In specific application scenarios, more attention is paid to the planning of the end effector path of the robotic arm. This results in low planning efficiency, poor link passability, and less consideration of the impact of the robotic arm's physical structure, leading to wasted new nodes, coarse paths, and a tendency for the robotic arm to collide or become unreachable, thus affecting the path planning results. Summary of the Invention

[0004] To address the problems of low planning efficiency, poor link passability, and coarse paths in existing robotic arm path planning methods, this invention provides a bidirectional extended robotic arm path planning method and system under pose constraints. Based on the Progressive Optimal Fast Random Search Tree (RRT), a bidirectional extended strategy BT-RRT* guided by a target bias strategy and a pose-constrained path optimization strategy (PCO) are designed. The generated paths are pruned and redundant nodes are removed to shorten the length of feasible paths. At the same time, redundant nodes are iteratively adjusted to perform smooth optimization, thereby improving the quality of path generation.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a method for path planning of a bidirectional extended robotic arm under pose constraints, the specific steps of which are as follows:

[0006] S1 obtains the initial and final position coordinates of the robotic arm;

[0007] S2 improves the RRT* algorithm by using a bidirectional expansion strategy guided by the target bias strategy to obtain the BT-RRT* algorithm. The initial position and final position of the robotic arm are used as the starting point and target point of the BT-RRT* algorithm to perform path planning for the robotic arm and obtain the initial planned path T.

[0008] S3 uses a pose-constrained path optimization strategy to smooth the initial planned path T, thus obtaining the final motion path of the robotic arm.

[0009] Furthermore, in S2, the bidirectional expansion strategy involves constructing two fast exploration random trees T1 and T2 during the initialization process of the RRT* algorithm, and using the starting point and target point in the RRT* algorithm as the initial nodes of the two random trees T1 and T2 respectively to perform a simultaneous search in opposite directions.

[0010] The target bias strategy is to select the target point x with a certain probability during each expansion sampling of the random tree. goal For a random point x rand Specifically:

[0011]

[0012] In the formula, P target The target bias threshold is defined by P, P1, P2, and P3, which are probability values ​​randomly selected from a uniform probability distribution and ranging from 0 to 1. If P is greater than P... target Then the expansion direction of the random tree is towards the target point x. goal Growth; if P is less than P target Then, the random tree expands random points (P1·x, P2·y, P3·z) as sampling nodes in the three-dimensional exploration space and grows in that direction until it is connected to another random tree and stops. The exploration principle of the two random trees is the same.

[0013] Furthermore, in S2, the BT-RRT* algorithm is used for robotic arm path planning, and the specific requirements for the initial planned path T are as follows:

[0014] 1) Initialize two random trees T1 and T2 in a predefined three-dimensional spatial region M, and set x init and x goal Add them as the initial nodes for T1 and T2 respectively, where x init and x goal These are the initial and final positions of the robotic arm, respectively.

[0015] 2) Determine the new nodes of the random tree T1;

[0016] 3) Calculate the Euclidean distance between the new node and all nodes in the random tree T2. Select the minimum Euclidean distance and compare it with a preset threshold. If the minimum distance is less than the preset threshold, and the new node moves to x... init Backtracking, the node that generates the minimum distance in random tree T2 is x goal Backtracking yields the initial planned path T; if the minimum distance is greater than or equal to the preset threshold, then a new node in the random tree T2 is determined.

[0017] 4) Calculate the Euclidean distance between the new node in random tree T2 and all nodes in random tree T1. Select the minimum Euclidean distance and compare it with a preset threshold. If the minimum distance is less than the preset threshold, and the new node moves to x... goal Backtracking, the node that generates the minimum distance in random tree T1 is x init Backtrack to obtain the initial planned path T; if the minimum distance is not greater than or equal to the preset threshold, repeat step 2 to determine the new node of the random tree T1.

[0018] Furthermore, in S3, the pose constraint path optimization strategy includes a pose constraint strategy and a path optimization strategy. The pose constraint strategy is embedded in the BT-RRT* algorithm to constrain the pose of the robotic arm so that the link structure of the robotic arm does not collide with obstacles, thereby constraining the generation of new nodes. The path optimization strategy is connected after the BT-RRT* algorithm to smooth and optimize the initial planned path T obtained by the BT-RRT* algorithm, so as to obtain the final motion path of the robotic arm.

[0019] Furthermore, in S3, the pose constraint strategy is specifically as follows:

[0020] Step 1: Obtain the new node x obtained by the BT-RRT* algorithm new (x,y,z);

[0021] Step 2: Set the position parameter (p) of the robotic arm's end effector x ,p y ,p z ) equals the new node x new (x,y,z) Perform inverse kinematics analysis on the end effector position of the robotic arm to determine if there is an inverse kinematic solution for the new node. If there is an inverse kinematic solution, continue to Step 3; otherwise, proceed to Step 1.

[0022] Step 3: Perform optimal constraint determination on the obtained inverse kinematics solution of the robotic arm as shown in formula (8), and add the joint angles of the j-th inverse kinematics solution that satisfy the optimal constraint conditions to obtain the sum of the joint angles S. j According to minS j Obtain the order of selection of inverse solutions;

[0023]

[0024] In the formula, θ i S represents the radian of the i-th joint in a certain inverse solution. j This represents the sum of joint angles of the j-th inverse solution that satisfies the optimal constraints;

[0025] Step 4: Following the order of inverse kinematics selection, each inverse kinematics solution is used as the current joint radian value. The spatial coordinates of each joint are obtained through forward kinematics analysis. The spatial linear equation of each link i of the robotic arm is calculated. Collision detection is performed on the new node and obstacles in space using collision rules. If no collision occurs, the generated new node is added to the random tree planned by the BT-RRT* algorithm as the parent node for the next exploration. If a collision occurs, the next set of inverse kinematics solutions is selected as the current joint radian value and collision detection is performed. If no inverse kinematics solution without collision is found in the entire solution set, the new node is deleted and Step 1 is continued.

[0026] Furthermore, in S3, the path optimization strategy is specifically as follows:

[0027] 1) Remove redundant nodes from the node sequence N on the initial planned path T to obtain the updated path N', and use the updated path N' as the path to be optimized for smooth optimization;

[0028] 2) Define the sequence of path nodes to be optimized as C, and the final sequence of smoothed paths as Y. The specific steps for smoothing optimization are as follows:

[0029] Step 1: Set the sequence of nodes in the path to be optimized, C, to be equal to the updated path, N'.

[0030] Step 2: Let the initial value of the smooth path sequence Y be C, and establish the smoothness objective function as shown in Equation (10). Continuously adjust the smoothness objective function of each node Y in the sequence Y through iteration. q The position yields a smooth path;

[0031]

[0032] Step 3: Perform collision detection on the smoothed path sequence Y obtained in Step 2. Assume that newly generated nodes in the sequence can be used as new nodes. If the result is true, the smoothed path sequence Y is obtained; if the result is false, C is removed from the path node sequence to be optimized. q With C q+1 Insert median C between l Update sequence C to [C1,…,C q C l C q+1 ,…,C 2m-1Continue with Step 2.

[0033] Furthermore, in S3, the start and end nodes of the initially planned path T are not optimized.

[0034] This invention provides a bidirectional extended robotic arm path planning system under pose constraints, applying the above-described path planning method, comprising:

[0035] The data acquisition module is used to acquire the initial and final position coordinates of the robotic arm;

[0036] The initial path planning module is used to improve the RRT* algorithm by using a bidirectional expansion strategy guided by the target bias strategy to obtain the BT-RRT* algorithm. The initial position and final position of the robotic arm are used as the starting point and target point of the BT-RRT* algorithm to perform path planning for the robotic arm and obtain the initial planned path T.

[0037] The initial path optimization module is used to smooth the initial planned path T using a pose-constrained path optimization strategy to obtain the final motion path of the robotic arm.

[0038] The present invention provides a terminal device, comprising: a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the above-described path planning method, or, when the processor executes the computer program, it implements the functions of each module in the above-described system.

[0039] The present invention provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps in the path planning method described above.

[0040] Compared with the prior art, the present invention has at least the following beneficial effects:

[0041] This invention provides a bidirectional extended robotic arm path planning method under pose constraints. Based on a target bias strategy that guides a random tree to grow towards the target point with a certain probability, the method improves the RRT* algorithm structure by using the starting point and target point as the initial nodes of two random trees and simultaneously performing a head-to-head search. This effectively accelerates path generation and significantly improves the efficiency of robotic arm end-effector path planning. Furthermore, the method utilizes pose constraints to prune and de-reduce the generated initial planned path T, shortening the feasible path length. It also fully considers collision issues caused by the robotic arm's physical structure by constraining new nodes x. newThe generation process ensures that the new nodes added to the two fast-exploration random trees are within the reachable space of the robotic arm, satisfying the purpose of preventing the robotic arm's linkage structure from colliding with obstacles, improving the quality of path generation, and obtaining the final motion path of the robotic arm that is not only smooth but also executable by the robotic arm, without any collisions or unreachable paths.

[0042] Furthermore, the path planning method of this invention optimizes new nodes multiple times by combining factors such as the robot arm pose and workspace, thus avoiding the waste of new nodes. Attached Figure Description

[0043] Figure 1 This is a schematic diagram of RRT algorithm node expansion;

[0044] Figure 2 The RRT* algorithm resets the parent node and rewires the process.

[0045] Figure 3 A schematic diagram of the extended BT-RRT* algorithm;

[0046] Figure 4 Here is the overall flowchart of the BT-RRT* algorithm;

[0047] Figure 5 Model of SNR3-C30 six-axis robotic arm and improved DH parameter model;

[0048] Figure 6 A simplified model of obstacles and robotic arms;

[0049] Figure 7 The structure is PCO-BT-RRT* algorithm;

[0050] Figure 8 This is a schematic diagram for pruning redundant nodes;

[0051] Figure 9 This is a schematic diagram of the node position update process;

[0052] Figure 10 This is a schematic diagram of the path smoothing process;

[0053] Figure 11 This diagram illustrates the algorithm expansion process in a 3D map.

[0054] Figure 12 This is a simulation experiment scenario for a robotic arm;

[0055] Figure 13 Simulation process of robot arm path with and without pose constraints in a simple scenario;

[0056] Figure 14 Simulation process of robot arm path in complex scenarios with unconstrained pose;

[0057] Figure 15 Simulation process of robot arm path under pose constraints in complex scenarios;

[0058] Figure 16 This refers to the path planning process of the PCO-BT-RRT* algorithm.

[0059] Figure 17 Kinematic simulation for robotic arm trajectory planning;

[0060] Figure 18 Results of real-machine experiments for obstacle scenarios in building construction tasks. Detailed Implementation

[0061] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0062] This invention provides a path planning method for a bidirectional extended robotic arm under pose constraints, the specific steps of which are as follows:

[0063] S1. Based on the shortcomings of the current RRT algorithm in robotic arm path planning, this invention proposes a new robotic arm path planning algorithm. Based on the RRT* algorithm, during the initialization process of the RRT* algorithm, two fast exploration random trees are constructed to obtain a bidirectional expansion strategy BT-RRT* guided by a target bias policy. The initial planned path T is then planned using the bidirectional expansion strategy guided by the target bias policy, ultimately obtaining the initial planned path T of the robotic arm. The specific implementation steps are as follows:

[0064] 1) Design a bidirectional extended RRT* algorithm guided by target bias (BT-RRT*). Based on introducing a target bias strategy to guide the random tree to grow towards the target point with a certain probability, improve the RRT* algorithm structure by using the starting point and target point as the initial nodes of two random trees and simultaneously performing a mutual search, effectively accelerating path generation. Specifically:

[0065] 1.1) RRT* Algorithm

[0066] The RRT algorithm is an incremental growth global programming algorithm based on random sampling. After determining the starting point x... start and target point x goal Then, a sampling point x is obtained by randomly sampling in the workspace. rand Find the nearest neighbor x on the random tree to the sampling point. near and at neighboring point x near With sampling point x randBy using a certain step size in the direction of the connection, a new node x is obtained that does not collide with the obstacle. new Determine the new node x new With target point x goal Check if the Euclidean distance *dist* between the nodes is less than the set threshold. If it is, the path has been successfully found; otherwise, repeat the above process. A schematic diagram of the node expansion in the RRT algorithm is shown below. Figure 1 As shown.

[0067] To address the issue that the RRT algorithm does not consider whether the generated path is optimal, the RRT* algorithm expands to obtain a new node x. new Perform parent node reset and rewiring operations to optimize the search path and reduce cost losses.

[0068] like Figure 2 As shown, the node labels indicate the order in which nodes are generated in the tree; ① represents the starting node x. start , Indicates the new node x new ⑩ represents the nearest neighbor x in the random tree to the new node. near The default value is x. new The parent node is the node whose path is connected to another node. The numbers on the lines connecting the nodes represent the path costs between the two nodes. Resetting the parent node means finding a path from ① to ①. The feasible path with the minimum path cost. Figure 2 In (a), with Centered on a random tree, all nodes within a given threshold range are considered candidate nodes. The relationship between each candidate node and the target node is calculated. The path cost after connection is used to determine the node with the lowest cost. The final parent node. Compared to other candidate nodes, ⑥ and The resulting path after connection is ①-②-⑤-⑥- The minimum cost is 3 + 7 + 3 + 3 = 16, therefore... The parent node is changed to ⑥, and the result of resetting the parent node is as follows. Figure 2 As shown in (b).

[0069] To further reduce the connection cost between nodes in the random tree, a rewiring operation is performed on the random tree. Figure 2 In (c), if the parent node of the candidate node is changed to the new node. If the path cost can be reduced, then the change is effective. It can be seen that if the parent node of ⑧ is reset from ⑦ to... The path has been changed to ①-②-⑤-⑥- -⑧, the path cost 3+7+3+3+4=20 is less than the original path cost 7+9+9=25; at the same time, if the parent node of ⑨ is reset from ⑥ to The path is ①-②-⑤-⑥- The path cost of ⑨ (3+7+3+3+5=21) is also less than the original path cost of 3+7+3+9=22. Therefore, the parent nodes of ⑧ and ⑨ should be rerouted. The random tree after rewiring is as follows Figure 2 As shown in (d).

[0070] 1.2) Target bias strategy-guided bidirectional expansion strategy

[0071] Analysis of the RRT* principle reveals that each sampling point is a random sample within the workspace, ensuring the algorithm can effectively explore the entire space. However, the blind nature of path exploration leads to problems such as long search times and high randomness of sampling points in complex environments. To improve the algorithm's search speed by expanding the two random trees as far towards the target as possible, a common approach is to introduce a target bias strategy. This strategy selects the target point x with a certain probability during each expansion sampling of a single random tree. goal For a random point x rand The calculation method is as follows:

[0072]

[0073] In the formula, P target Let P be the target bias threshold, where P1, P2, and P3 are probability values ​​randomly selected from a uniform probability distribution, ranging from 0 to 1. If P is greater than P... target Then the expansion direction of the random tree is towards the target point x. goal Growth; if P is less than P target Then, the random tree expands random points (P1·x, P2·y, P3·z) as sampling nodes in the three-dimensional exploration space and grows in that direction until it is connected to another random tree and stops. The exploration principle of the two random trees is the same, and the specific principle is shown in the above formula.

[0074] Based on this, such as Figure 3 As shown, this invention constructs two fast exploration random trees during the initialization process of the RRT* algorithm. One random tree is generated from the initial point and performs target bias selection on the target point with a certain probability. The other random tree is generated from the target point and performs target bias selection on the initial point with a certain probability. The two trees grow alternately towards each other. When the Euclidean distance (dist) between a new node generated on either random tree and a node on the other random tree is less than the threshold, the two trees are connected, and the bidirectional expansion process ends. After the path search is completed, the connecting point on the two random trees is taken and backtracked to the starting point and the target point respectively to obtain the planned path.

[0075] The BT-RRT* algorithm naturally overcomes the blindness of RRT* pathfinding by using a search method where the starting and target points attract each other and move towards each other, thus reducing search time and ensuring path quality. The overall process of the BT-RRT* algorithm is as follows: Figure 4 As shown, the specific steps for path generation are as follows:

[0076] Step 1: Initialize two random trees T1 and T2 in a predefined three-dimensional spatial region M, and set x init and x goal Add them as the initial nodes (x) of T1 and T2 respectively. init and x goal (This refers to the initial and final positions of the robotic arm).

[0077] Step 3: Determine if the new node is in the obstacle area. If it is not in the obstacle area, determine if the robotic arm collides with the obstacle area at the new node. If the robotic arm does not collide with the obstacle area, proceed to the next step.

[0078] Calculate the nearest point x near and new node x new The Euclidean distance between the spatial line connecting the obstacle and the geometric center point of the obstacle region is calculated, and the calculated Euclidean distance value is compared with the radius of the obstacle region to determine x. near →x new Collision detection is performed on the path.

[0079] If the Euclidean distance is less than the radius of the obstacle area, a collision will occur, and Step 6 will be executed.

[0080] No collision occurs when the Euclidean distance is greater than the radius of the obstacle region. The distance to x is calculated by traversing T1. new The set of candidate parent nodes X whose distance is less than the R threshold near ;

[0081] Get the set of candidate parent nodes X near Each point in the middle and the new node x new If there exists a path cost between the two paths that is less than the original path cost, then the new node x will be added. new The parent node is modified to the point x that implements the cost of this path. min If there exists a path cost greater than or equal to the original path cost, then the new node x... new The parent node remains the original neighbor node x. near .

[0082] Step 4: Connect the new node x above new Add it to T1, and perform rewiring operations on nodes within the R threshold range. For nodes that satisfy the path cost reduction requirement, change their parent node to x. new .

[0083] Step5: Calculate x new Calculate the Euclidean distance dist between x and all nodes in T2, select the minimum distance and compare it with the preset threshold threshold. If the minimum distance ≥ threshold, continue to execute Step6;

[0084] If the minimum distance < threshold, and at the same time x new Backtrack towards x init Backtrack from the node with the minimum distance towards x goal Backtrack to obtain the initial planned path T.

[0085] Step6: In the random tree T2, use the goal biasing strategy to select a random point x' with a certain probability rand , if P > P target , then x' rand = x init ; if P < P target , then x' rand Randomly sample a point with coordinates (P1·x, P2·y, P3·z) within the three-dimensional space region M as the random point x' rand ;

[0086] Obtain the neighboring point x' closest to the random point x' rand , near , Calculate and generate a new node x' with a fixed step size stepsize new .

[0087] Step7: Calculate the Euclidean distance between the spatial connection line of the neighboring point x' near and the new node x' new and the geometric center point of the obstacle region, and compare the calculated Euclidean distance value with the radius of the obstacle region to perform collision detection on the path of x' near →x' new .

[0088] When the Euclidean distance is greater than the radius of the obstacle region, there is no collision. Calculate the set of candidate parent nodes X' whose distances from x' new are less than the R threshold by traversing T2 near ;

[0089] Obtain the path costs between each point in the set of candidate parent nodes X' near and the new node x' new . When there exists a path cost less than the original path cost, modify the parent node of the new node x' new to the point x' that achieves this path cost min ;

[0090] When there exists a path cost greater than or equal to the original path cost, the new node x'new The parent node of it remains the original adjacent point x'. near ;

[0091] If the Euclidean distance is less than the radius of the obstacle area, a collision occurs, and Step 2 is executed.

[0092] Step 8: Add the newly generated node x' new to T2, and perform re - wiring operations on the nodes within the R threshold range. For the nodes whose path cost decreases, change their parent node to x'. new .

[0093] Step 9: Calculate the Euclidean distances dist' between x' new and all nodes in T1, and select the minimum distance among the Euclidean distances dist' for comparison with the preset threshold threshold.

[0094] If the minimum distance ≥ threshold, continue to execute Step 2.

[0095] If the minimum distance < threshold, and at the same time x' new backs to x goal and the node that generates the minimum distance backs to x init to backtrack, and the initial planned path T is obtained.

[0096] S2 To verify the feasibility of the above algorithm, we need to build an algorithm verification simulation environment, which consists of a robotic arm model and obstacles in space.

[0097] Before establishing the geometric model of the robotic arm, we need to establish the kinematic model of the robotic arm in advance, obtain the inverse kinematic equations of each joint of the robotic arm, provide a theoretical basis for subsequent collision detection and pose constraints, and conduct simulation experiments for verification when verifying the feasibility of the algorithm.

[0098] At the same time, establish the geometric model of the robotic arm, use the actual geometric parameters of the robotic arm to perform scaled modeling in the software, verify the correctness of the pose constraint strategy, and limit the working space of the robotic arm itself to perform collision detection between the new node and the robotic arm.

[0099] This invention takes Figure 5 the SNR3 - C30 six - axis industrial robotic arm shown as an example, and uses the improved DH parameter method to establish the kinematic model of the robotic arm. The transformation matrix of the robotic arm link {i} relative to the link {i - 1} in the joint coordinate system is shown in formula (1).

[0100]

[0101] A six-DOF robotic arm consists of links connected by six joints. Based on the DH parameters and joint limits in Table 1, in the robot coordinate system, when the parameters of each link are known, given the joint radian values ​​θ1, θ2, θ3, θ4, θ5, and θ6 of the robotic arm, a homogeneous transformation equation is applied. By multiplying the results sequentially, the positive kinematic relationship of the robotic arm end effector relative to the base position is obtained as shown in formula (4), where attitude = [noa](n = [n x ,n y ,n z ] T , o = [o x ,o y ,o z ] T , a = [a x ,a y ,a z ] T ) represents the attitude vector of the robotic arm's end effector coordinate system; posture = [p x ,p y ,p z ] T This represents the position vector of the end effector coordinate system of the robotic arm.

[0102] Table 1 Improved DH parameters and joint range of the SNR3-C30 robotic arm

[0103]

[0104]

[0105] However, in general, the end pose of a robotic arm is expressed by position parameters and RPY angles. In order to solve the joint radian value of a robotic arm given the end pose parameter pos=[x,y,z,R,P,Y], this invention uses formula (4) to establish the relationship between pos and the end pose vector position of the robotic arm, thereby obtaining the inverse kinematic equations of each joint of the SNR3-C30 six-axis industrial robotic arm, as shown in formula (5).

[0106]

[0107]

[0108] In the formula, k2=-d1-a z d6+p z , A=k1-a2cosθ2, B=k2-a2 sinθ2.

[0109] S3 utilizes the motion model parameters of the robotic arm. This invention employs a combination of envelope boxes and envelope spheres to establish the geometric model of the robotic arm and obstacles, simplifying the geometric model of the robotic arm and obstacles to transform collision detection between the robotic arm and obstacles into a problem of judging geometric interactions, as detailed below:

[0110] like Figure 6 As shown, firstly, based on the AABB envelope method in the envelope box, the obstacle is enveloped with a hexahedron of minimum volume. Secondly, the hexahedron is wrapped by a cylinder, and the maximum radial radius *r* of the robotic arm link is superimposed on the radius of the cylinder. A sphere is then extended from the base of the cylinder with the superimposed radius, thus representing the robotic arm link as an equivalent line. Finally, the collision detection between the robotic arm and the obstacle is transformed into a problem of determining geometric interactions. Although this method sacrifices some of the robotic arm's free space, it simplifies the collision model and improves computational efficiency.

[0111] Figure 2 In the middle, O j For the hexahedron that encloses the obstacle, d j Let R be the circumradius of the hexahedron, and R be the envelope radius of the cylinder and sphere. j =d j +r. After representing the robotic arm links as equivalent lines, the spatial linear equation of each link i is obtained as follows:

[0112]

[0113] Where (x) i ,y i ,z i ) and (x i+1 ,y i+1 ,z i+1 ) represents the spatial coordinates of two adjacent joints of the robotic arm.

[0114] S4 Figure 7 As shown, considering the pose constraints of the robotic arm during operation, this invention proposes a pose constraint path optimization strategy (PCO) before verifying the trajectory planning algorithm. This strategy combines the pose constraints of the robotic arm with the initial planned path T obtained by the bidirectional extension strategy BT-RRT* guided by the target bias strategy, pruning and smoothing to improve path quality and obtain a smooth path that ultimately satisfies the robotic arm's motion. Specifically, the pose constraint path optimization strategy PCO includes a pose constraint strategy (checkLink) and a path optimization strategy (pathOptimization). The pose constraint strategy is embedded in the BT-RRT* algorithm, while the path optimization strategy performs post-processing on the initial planned path T obtained by the BT-RRT* algorithm.

[0115] The pose constraint strategy (checkLink) is used to constrain the new node x.new The generation process ensures that new nodes added to the two fast-exploration random trees are within the reachable space of the robotic arm, and that the robotic arm's linkage structure does not collide with obstacles. Specifically:

[0116] Step 1: Obtain the new node x obtained by the bidirectional expansion strategy BT-RRT guided by the target bias strategy. new (x,y,z).

[0117] Step 2: Set the position parameter (p) of the robotic arm's end effector x ,p y ,p z ) equals the new node x new Substitute the coordinates (x, y, z) into formula (5) to perform inverse kinematics analysis and determine whether the new node has an inverse kinematic solution for the robotic arm. If an inverse kinematic solution exists, continue to Step 3; if no inverse kinematic solution exists, proceed to Step 1.

[0118] Step 3: Due to the limitations of joint positioning and the inherent unreachable space of the robotic arm (e.g., the joint positioning of the SNR3-C30 robotic arm shown in Table 1), the optimal constraint determination is performed on the inverse solution of the robotic arm obtained in Step 2 according to the optimal constraint expression (8).

[0119] Calculate the sum of joint angles S of the j-th inverse solution that satisfies the above constraints. j According to minS j Obtain the order in which the inverse solution is selected.

[0120]

[0121] In the formula, θ i S represents the radian of the i-th joint in a certain inverse solution. j This represents the sum of joint angles of the j-th inverse solution that satisfies the optimal constraints.

[0122] Step 4: Following the order of inverse kinematics selection, each inverse kinematics solution is used as the current joint radian value. The spatial coordinates of each joint are calculated using the forward kinematics expression (8). Then, the spatial linear equation of each link i of the robotic arm is calculated according to equation (6). Collision detection is performed on the new node and obstacles in space using collision rules. If no collision occurs, the new node generated by the previous algorithm is added to the random tree planned by the BT-RRT* algorithm as the parent node for the next exploration. If a collision occurs, the next set of inverse kinematics solutions is selected as the current joint radian value, and collision detection is performed. If no inverse kinematics solution that does not cause a collision is found in the entire solution set, the new node is deleted, and Step 1 is continued.

[0123]

[0124] Step 5: If dist < threshold or the number of iterations reaches the upper limit, the entire expansion process ends.

[0125] S5 combines the BT-RRT* trajectory planning algorithm with the pose constraint strategy to obtain the two-way expansion strategy guided by the target offset strategy under the pose constraint of the robotic arm. The initial planned path T of the robotic arm planned by BT-RRT* is smoothed and optimized using the path optimization strategy to obtain the final motion path of the robotic arm. The optimization includes redundant node deletion and smoothing optimization, as follows:

[0126] Since the updated path obtained by pruning the initial planned path T, removing redundant nodes, and shortening the path length is not smooth, it will not only affect the stability of the robotic arm operation but also cause the robot to increase unnecessary turning time during turning. Therefore, it is necessary to perform path optimization.

[0127] Redundant nodes in the sequence N of nodes of the initial planned path T are removed to obtain the updated path N'. The updated path N' is used as the path to be optimized for smoothing optimization. During smoothing optimization, the sequence of nodes of the path to be optimized is defined as C, and the sequence of the final smoothed path is Y;

[0128] Redundant node deletion: As Figure 8 shown, assume that the sequence N of nodes of the initial planned path T planned by the BT-RRT* algorithm under the pose constraint of the robotic arm is [N1, N2, N3,..., N n , where n is the number of path nodes. First, starting from the starting node N1, connect each node backward in turn and perform collision detection. When a collision occurs, record the current node as N k (1 < k < n), and all nodes between N1 and N k-1 are redundant nodes. Therefore, the path node sequence N is updated to [N1, N k-1 , N k ,..., N n . Then, starting from the node N k-1 , search for collision-free nodes backward in turn. Repeat the above steps to traverse the entire sequence N of nodes of the initial planned path T and remove all redundant nodes to obtain the updated path N' with a smaller path cost as [N'1, N'2, N'3,..., N' m , where 0 < m < n.

[0129] Smoothing optimization: Then, the updated path N' after removing redundant points is smoothed and optimized. To facilitate the description of the path smoothing process, the sequence C of nodes of the path to be optimized and the sequence Y of the smoothed path are defined. As Figure 9 shown, the process of implementing node position update essentially needs to meet the following three conditions:

[0130] 1) Node Y after path smoothing q With path node C to be optimized q The positional deviation is minimal. That is, for Y... q Continue solving until ||C is satisfied q –Y q The value of || is the smallest, where 0 <q<m。

[0131] 2) Node Y after path smoothing q Its neighboring node Y q+1 The relative distance is minimized. That is, solving for Y yields the result. q Satisfy ||Y q –Y q+1 The value of || is the smallest.

[0132] 3) Node Y after path smoothing q Its neighboring node Y q+1 The lines connecting the obstacles do not collide with them. In other words, the smoothed path is as close as possible to the original path.

[0133] To satisfy the three conditions, this invention proposes a path smoothing method, the specific path smoothing steps of which are as follows:

[0134] Step 1: For conditions 1) and 2), initialize the path node sequence C to be optimized to equal P'.

[0135] Step 2: Let the initial value of the smooth path sequence Y be C, and establish the smoothness objective function as shown in Equation (10). Continuously adjust the smoothness objective function of each node Y in the sequence Y through iteration. q The position yields a smooth path, where the start and end nodes are not optimized.

[0136]

[0137] Step 3: After removing redundant points, the number of path nodes decreases, resulting in fewer path nodes to be optimized. During the iteration process, the smoothed path is prone to deviating from the initial planned path T. Consequently, the robotic arm is not only more likely to collide with obstacles, but the path is also more likely to appear in spaces inaccessible to the robotic arm, causing condition 3) to be unsatisfactory. Therefore, as... Figure 10 As shown, collision detection is performed on the smooth path sequence Y obtained in Step 2. It is assumed that newly generated nodes in the sequence can be used as new nodes, let x... new =Y q If the result is true, return the smoothed path sequence Y; if the result is false, return the path node sequence C to be optimized. q With C q+1 Insert median C between l Update sequence C to [C1,…,Cq C l C q+1 ,…,C 2m-1 Continue with Step 2.

[0138] This invention also provides a bidirectional extended robotic arm path planning system under pose constraints that applies the above-described path planning method, comprising:

[0139] The data acquisition module is used to acquire the initial and final position coordinates of the robotic arm;

[0140] The initial path planning module is used to improve the RRT* algorithm by using a bidirectional expansion strategy guided by the target bias strategy to obtain the BT-RRT* algorithm. The initial position and final position of the robotic arm are used as the starting point and target point of the BT-RRT* algorithm to perform path planning for the robotic arm and obtain the initial planned path T.

[0141] The initial path optimization module is used to smooth the initial planned path T using a pose-constrained path optimization strategy to obtain the final motion path of the robotic arm.

[0142] The present invention also provides a terminal device, comprising: a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the above-described method. Alternatively, when the processor executes the computer program, it implements the functions of each module / unit in the above-described system.

[0143] The computer program described above can be divided into one or more modules / units. One or more modules / units are stored in the memory described above and executed by the processor to complete the present invention.

[0144] Terminal devices can be computing devices such as desktop computers, laptops, handheld computers, and cloud servers.

[0145] Terminal devices may include, but are not limited to, processors and memory.

[0146] The processor can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0147] The memory can be used to store the aforementioned computer programs and / or modules. The processor implements the various functions of the aforementioned terminal device by running or executing the computer programs and / or modules stored in the memory, and by calling the data stored in the memory.

[0148] If the modules / units integrated into the terminal device are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium.

[0149] Based on this understanding, all or part of the processes in the above-described method can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the above-described method steps. The computer program includes computer program code, which can be in the form of source code, object code, executable file, or some intermediate form.

[0150] The aforementioned computer-readable media may include: any entity or device capable of carrying the aforementioned computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the aforementioned computer-readable media may be appropriately added to or subtracted from the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media may not include electrical carrier signals and telecommunication signals.

[0151] 4. Experimental Analysis

[0152] This invention verifies the proposed method through simulation and real-machine experiments. The computers used for both simulation and real-machine experiments were configured with an Intel(R) Core i5-8400 CPU (2.81GHz) and 16GB of RAM. The simulation experiments were conducted using MATLAB R2019a software and mainly included three parts: BT-RRT* algorithm experiments, robotic arm pose constraint process experiments, and robotic arm path optimization process experiments. The real-machine experiments were carried out using a self-developed building masonry system.

[0153] 4.1 Experimental Analysis of BT-RRT* Algorithm

[0154] To verify the effectiveness and superiority of the algorithm, the BT-RRT* algorithm was compared with RRT*, T-RRT* (target bias strategy-guided RRT* algorithm), and B-RRT* (bidirectional extended RRT* algorithm) on a 3D map. The upper limit of the number of experimental iterations was set to 10,000, and the target bias threshold P was set to... target The algorithm was set to 0.5, the map range was set to [160, 160, 160], the starting point was set to [10, 10, 10], the target point was set to [150, 150, 150], and the search step size and distance threshold were set to three groups: [5, 10], [10, 10], and [10, 20]. Considering the randomness of the sampling algorithm, each experiment was run 100 times, with obstacles randomly set each time.

[0155] Figure 11 (a) The search process for one instance of RRT*, T-RRT*, B-RRT*, and BT-RRT* when the search step size and distance threshold are set to [10,10]; the search results are as follows: Figure 11 As shown in (b), each algorithm yields a feasible path from the starting point to the target point; Figure 11 (c) shows the relationship between the number of iterations and the distance during the search process for the four algorithms. Since RRT* and T-RRT* are single-tree searches, the distance represents the distance between the newly added node in the random tree and the endpoint during the search process. B-RRT* and BT-RRT* are dual-tree searches, so the distance represents the shortest distance between the two random trees during the search process.

[0156] from Figure 11 (a) It can be intuitively seen that RRT* is a random search of the space, so the search path fills the entire space, resulting in low search efficiency; T-RRT* overcomes the blindness of the path exploration in the RRT* algorithm, searching in the direction of the target with a certain probability, thus speeding up the search; the B-RRT* algorithm uses two trees to alternately search towards each other, which improves the convergence efficiency to a certain extent, but from Figure 11 (b) The search results displayed show that, due to the lack of goal orientation, the random trees cannot attract each other, sacrificing path cost and resulting in an uneven path outcome; Figure 11 (c) As shown in the relationship between the number of iterations and the distance, the BT-RRT* algorithm designed in this invention, under the feature of local asymptotic optimality brought about by the parent node reset and rewiring operation of the RRT* algorithm itself, is compatible with the advantages of the target bias strategy and the bidirectional search strategy, reducing the number of iterations of the RRT* algorithm from 3395 to only 78 iterations to converge to the local optimum, and generating feasible paths faster.

[0157] Three different search step sizes and distance thresholds were set up, and the average results of 100 experiments in each group are shown in Table 2. It can be seen that compared to the RRT* algorithm, the T-RRT* algorithm not only significantly improves the average planning time by approximately 73.12%, but also has the ability to optimize path cost. The B-RRT* algorithm, at the expense of path cost, shows an even more significant improvement in average planning time, with an average improvement of approximately 79.48%. The BT-RRT* algorithm, which combines the two strategies, still maintains good performance in terms of average planning time, with an average improvement of approximately 80.92%, and the negative impact of the path cost caused by the bidirectional expansion strategy is reduced due to the guidance of the target bias strategy. Therefore, the algorithm proposed in this paper has a significant advantage when comprehensively considering planning time and path cost, and can quickly and effectively achieve path planning for robotic arms.

[0158] Table 2 Comparison of search performance of each algorithm

[0159]

[0160] 4.2 Experimental Analysis of Robotic Arm Pose Constraint Process

[0161] To verify the effectiveness of the proposed robotic arm pose constraint strategy, two experimental scenarios—simple and complex—were built in MATLAB software, depicting the robotic arm in a Cartesian coordinate system. For example... Figure 12 As shown, the origin of the robotic arm's coordinate system is set as the origin of a Cartesian coordinate system, the starting point is set to [100, -400, 100], the target point is set to [0, 450, 450], and the exploration range is set to a 1000×1000×1000 hexahedron. Considering the impact of the unreachable space of the robotic arm, corresponding obstacles are set to constrain the robotic arm's activity space.

[0162] Based on the established simulation experimental scenario, using the Robotic System Toolbox and the URDF format SNR3-C30 robotic arm model, kinematic simulations were performed on the paths planned by the BT-RRT* algorithm with and without robotic arm pose constraints, with the search step size and distance threshold set to [50,50].

[0163] Simulation results in simple scenarios are as follows Figure 13As shown, (a) and (e) are the paths planned by the BT-RRT* algorithm without and with the robot arm pose constraint strategy, respectively; (b) and (f) are the robot arm's initial poses in the two ways; (d) and (h) are the robot arm's target poses in the two ways; (c) shows the path planned without robot arm pose constraints. Although the planned robot arm end-effector path did not collide with the obstacle, the link structure collided with the obstacle during the robot arm's operation, so the planned path is not feasible; (g) shows the path planned by the BT-RRT* algorithm with robot arm pose constraints. It can be seen that not only did the planned robot arm end-effector path not collide with the obstacle, but the robot arm link structure also did not collide with the obstacle, so the planned path is effective and feasible.

[0164] Simulation results in complex scenarios without robotic arm pose constraint strategies are as follows: Figure 14 As shown, where Figure 14 (a) demonstrates the process of the BT-RRT* algorithm planning an obstacle avoidance path for the robotic arm's end effector in a complex scene without robotic arm pose constraints. Similar to simple scenes, the planned path can effectively avoid obstacles, providing a collision-free path from the starting point to the target point for the robotic arm's end effector. However, as the environmental complexity increases, it is more likely that the robotic arm's mechanical structure will experience obstacles during movement, such as... Figure 14 The scenarios shown in (b) and (c) involve the link colliding with the obstacle area. Figure 15 As shown, by constraining the new node generation process of the BT-RRT* algorithm with the robot arm pose, it is ensured that the robot arm pose of each node on the planned path does not collide with obstacles. This verifies that the robot arm pose constraint strategy proposed in this paper has excellent constraint ability on the path planning algorithm when the robot arm is in a scenario with different complexity.

[0165] The BT-RRT* algorithm with and without the robotic arm pose constraint strategy was simulated 10 times in different scenarios, and the average computation time was calculated. Table 3 shows that in simple scenarios, the average computation time of the BT-RRT* algorithm with pose constraints increased by approximately 3.37% compared to the BT-RRT* algorithm for obstacle avoidance path search. Compared to simple scenarios, the computation time for obstacle avoidance path search increased in complex scenarios, but the average computation time of the BT-RRT* algorithm with pose constraints only increased by approximately 3.52% compared to the BT-RRT* algorithm. Therefore, as the complexity of the obstacle space increases, the pose constraint strategy, while improving the adaptability of the robotic arm's linkage structure to the planned path, can effectively ensure the convergence efficiency of the path planning algorithm, enabling rapid obstacle avoidance operation of the robotic arm.

[0166] Table 3 Comparison of computation time of the two algorithms in different scenarios

[0167]

[0168] 4.3 Experimental Analysis of Robotic Arm Path Optimization Process

[0169] To verify the significant effectiveness of the PCO-BT-RRT* algorithm in the path planning problem of robotic arms, path planning simulation was performed in the complex scenario established in Section 4.2.

[0170] like Figure 16 As shown, under the pose constraint strategy, the BT-RRT* algorithm first plans an initial planned path T in the reachable space of the robotic arm to avoid obstacles. Then, it removes all redundant nodes from the initial planned path T to obtain an updated path with a smaller path cost. Finally, through three iterations, it adjusts the path nodes and finds a smooth path that avoids collisions and allows the robotic arm to operate stably.

[0171] To further quantitatively verify the advantages of the PCO-BT-RRT* algorithm, this invention utilizes the B-spline trajectory planning function provided by the Robotic System Toolbox to generate initial trajectories and PCO trajectories from the initial planned path T (i.e., the path obtained by pose constraint + BT-RRT* algorithm) and smoothed path (i.e., the path obtained by PCO-BT-RRT* algorithm). Simultaneously, the initial trajectory is solved using the Particle Swarm Optimization (PSO) algorithm to obtain the PSO trajectory, and the running time between adjacent nodes is specified as 1 second, thereby realizing the kinematic simulation experiment of the joint angle, joint angular velocity, and joint angular acceleration of the robotic arm.

[0172] Depend on Figure 17 As shown in (a), the robotic arm simulation process clearly demonstrates that both the PSO and PCO trajectories significantly optimize the initial trajectory, making it smoother and more stable. Figure 17 (b) to Figure 17 The experimental results in (d) show that, compared with the initial trajectory, the smooth trajectory solved by the PSO algorithm and the smooth trajectory processed by the PCO strategy both reduce the fluctuations of joint angles, angular velocities and angular accelerations to a certain extent, and the joint angle changes are stable and meet the constraints of joint angles, joint angular velocities and joint angular accelerations. However, compared with the PSO algorithm, the smooth trajectory processed by the PCO strategy has smoother and more continuous fluctuations of joint angles, angular velocities and angular accelerations, and can more effectively reduce mechanical vibration and motion impact.

[0173] Table 4 Comparison of Experimental Results for the Improved Algorithm (B-splines)

[0174]

[0175]

[0176] The experimental results shown in Table 4 indicate that the number of path nodes optimized by the PCO strategy is less than that of the initial planned path T, a reduction of approximately 19.05%, thus optimizing the simulation runtime of the robotic arm. While both the PCO strategy and the PSO algorithm increase the overall runtime of the algorithm during post-processing of the initial planned path T obtained from the BT-RRT* algorithm using pose constraints, the PCO strategy only increases the runtime by 694 ms compared to the 2755 ms of the PSO algorithm. The PCO strategy demonstrates a more significant ability to optimize path costs compared to the PSO algorithm, reducing the cost by approximately 30.25% compared to the initial planned path T.

[0177] In summary, the algorithm proposed in this invention can plan a smooth and stable path for the robotic arm while effectively ensuring its safe obstacle avoidance. It is a simple and effective obstacle avoidance path planning method for robotic arms.

[0178] 4.4 Real Machine Experiment

[0179] To verify the practicality of the proposed algorithm, a real-machine experiment was conducted. The PCO-BT-RRT* algorithm was compiled using C++ and embedded into the system software platform as the core path planning algorithm. A system was built... Figure 18 The diagram shows an obstacle course in a building construction task. Orange and yellow cuboids represent obstacles, while blue cuboids represent supports for the yellow obstacles. An SNR3-C30 robotic arm is used. As can be seen in Cartesian coordinates, the PCO-BT-RRT* algorithm, starting from the left pose, plans a collision-free path to the right target pose. In a real-world environment, the robotic arm achieves obstacle avoidance while reaching the designated target point, demonstrating the good practicality of the proposed method.

Claims

1. A path planning method for a bidirectional extended robotic arm under pose constraints, characterized in that, The specific steps are as follows: S1 obtains the initial and final position coordinates of the robotic arm; S2 improves the RRT* algorithm by using a bidirectional expansion strategy guided by the target bias strategy to obtain the BT-RRT* algorithm. The initial position and final position of the robotic arm are used as the starting point and target point of the BT-RRT* algorithm to perform path planning for the robotic arm and obtain the initial planned path T. S3 uses a pose-constrained path optimization strategy to smooth the initial planned path T, thus obtaining the final motion path of the robotic arm. In S3, the pose constraint path optimization strategy includes a pose constraint strategy and a path optimization strategy. The pose constraint strategy is embedded in the BT-RRT* algorithm to constrain the pose of the robotic arm so that the link structure of the robotic arm does not collide with obstacles, thereby constraining the generation of new nodes. The path optimization strategy is connected after the BT-RRT* algorithm to smooth and optimize the initial planned path T obtained by the BT-RRT* algorithm, so as to obtain the final motion path of the robotic arm. In S3, the pose constraint strategy is specifically as follows: S3.1: Obtain the new node obtained by the BT-RRT* algorithm x new ( x , y , z ); S3.2: Set the position parameters of the robotic arm end effector ( p x , p y , p z ) equals new node x new ( x , y , z Perform inverse kinematics analysis on the end effector position of the robotic arm to determine if there is an inverse kinematic solution for the new node. If there is an inverse kinematic solution, continue to execute S3.3; if there is no inverse kinematic solution, execute S3.

1. S3.3: The optimal constraint determination of the obtained inverse solution of the robotic arm is as follows: the first one that satisfies the optimal constraint condition is determined by the following formula. j The sum of the joint angles obtained by adding the joint angles of the inverse solution is given. S j According to min S j Obtain the order of selection of inverse solutions; In the formula, θ i In a set of inverse solutions, the i-th i Each joint curvature, S j The first one that satisfies the optimal constraints j The sum of the joint angles of the inverse solution; S3.4: Following the selection order of the inverse kinematics, the current joint radian values ​​are used sequentially. The spatial coordinates of each joint are obtained through forward kinematics analysis, and the values ​​of each link of the robotic arm are calculated. i The spatial linear equation is used to perform collision detection between new nodes and obstacles in space using collision rules. If no collision occurs, the generated new node is added to the random tree planned by the BT-RRT* algorithm as the parent node for the next exploration. If a collision occurs, the next set of inverse solutions is selected sequentially as the current joint radian value for collision detection. If no inverse solution that does not cause a collision is found in the entire solution set, the new node is deleted and S3.1 is continued.

2. The method for path planning of a bidirectional extended robotic arm under pose constraints according to claim 1, characterized in that, In S2, the bidirectional expansion strategy involves constructing two fast exploration random trees during the initialization process of the RRT* algorithm. T 1 and T 2. Treat the starting point and target point in the RRT* algorithm as two random trees respectively. T 1 and T The initial nodes of 2 simultaneously perform opposite-direction searches; The target bias strategy involves selecting a target point with a certain probability during each expansion sampling of the random tree. x goal For random points x rand Specifically: In the formula, P target The target bias threshold, P , P 1. P 2. P 3 represents a probability value between 0 and 1, randomly obtained according to a uniform probability distribution. if P Greater than P target Then the expansion direction of the random tree is towards the target point. x goal Growth; if P Less than P target Then the random tree expands random points in the three-dimensional exploration space. P 1 ·x , P 2 ·y , P 3 ·z The node is used as a sampling node and grows in that direction until it is connected to another random tree and stops. The exploration principle of the two random trees is the same.

3. The method for path planning of a bidirectional extended robotic arm under pose constraints according to claim 2, characterized in that, In S2, the BT-RRT* algorithm is used for robotic arm path planning. The specific requirements for the initial planned path T are as follows: 1) In a pre-defined three-dimensional spatial region M Initialize two random trees T 1 and T 2. x init and x goal Add them separately T 1 and T The initial node of 2, where x init and x goal These are the initial and final positions of the robotic arm, respectively. 2) Determine the random tree T 1 new node; 3) Calculate the new node and the random tree T 2. The Euclidean distances of all nodes are calculated, and the minimum Euclidean distance is compared with a preset threshold. If the minimum distance is less than the preset threshold, and the new node moves towards... x init Backtracking, Random Tree T The node that generates the minimum distance in 2 x goal Backtracking yields the initial planned path T; if the minimum distance is ≥ a preset threshold, then a random tree is determined. T 2 new nodes; 4) Calculate the random tree T 2 New Nodes and Random Trees T 1. The Euclidean distances of all nodes are calculated, and the minimum distance among these distances is compared to a preset threshold. If the minimum distance is less than the preset threshold, and the new node moves towards... x goal Backtracking, Random Tree T The node that generates the minimum distance in 1 x init Backtracking yields the initial planned path T; if the minimum distance is ≥ the preset threshold, repeat step 2 to determine the random tree. T A new node.

4. The method for path planning of a bidirectional extended robotic arm under pose constraints according to claim 1, characterized in that, In S3, the path optimization strategy is specifically as follows: 1) Remove the node sequence from the initial planned path T N Redundant nodes in the path are updated N' The path will be updated. N' Perform smooth optimization on the path to be optimized; 2) Define the sequence of path nodes to be optimized as follows: C The final sequence of smooth paths is Y The specific steps for smoothing optimization are as follows: 2.1): Let the sequence of path nodes to be optimized be... C Equals updating path N' ; 2.2): Make the path sequence smooth. Y The initial value is C A smoothness objective function is established as shown in the following formula, and the sequence is continuously adjusted iteratively. Y Each node Y q The position yields a smooth path; 2.3): The smooth path sequence obtained in 2.2). Y Collision detection is performed, assuming that newly generated nodes in the sequence can be used as new nodes. If the result is true, a smooth path sequence is obtained. Y If the result is false, in the sequence of path nodes to be optimized... C q and C q+1 Insert median between C l Update sequence C for[ C 1,…, C q , C l , C q+1 ,…, C 2m-1 Continue executing 2.2).

5. The method for path planning of a bidirectional extended robotic arm under pose constraints according to claim 4, characterized in that, In S3, the start and end nodes of the initially planned path T are not optimized.

6. A bidirectional extended robotic arm path planning system under pose constraints, applying the path planning method according to any one of claims 1 to 5, characterized in that, include: The data acquisition module is used to acquire the initial and final position coordinates of the robotic arm; The initial path planning module is used to improve the RRT* algorithm by using a bidirectional expansion strategy guided by the target bias strategy to obtain the BT-RRT* algorithm. The initial position and final position of the robotic arm are used as the starting point and target point of the BT-RRT* algorithm to perform path planning for the robotic arm and obtain the initial planned path T. The initial path optimization module is used to smooth the initial planned path T using a pose-constrained path optimization strategy to obtain the final motion path of the robotic arm.

7. A terminal device, characterized in that, include: A processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the path planning method according to any one of claims 1 to 5, or, when the processor executes the computer program, to implement the functions of each module in the system according to claim 6.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the path planning method according to any one of claims 1 to 5.

Images