Obstacle avoidance path planning method of vector adjusting mechanism based on RRT principle

Abstract

The invention relates to an obstacle avoidance path planning method of a vector adjusting mechanism based on an RRT principle, and belongs to the technical field of satellite control. According to the method, inverse kinematics calculation and path obstacle avoidance planning are carried out at the same time, a mode search method is used for replacing a traditional numerical iteration inverse solution algorithm, and the problem that path planning can only be carried out after an inverse solution is calculated in advance and a joint angle corresponding to a target pose is obtained in a traditional RRT method is solved; the problems of matrix inversion, partial derivative calculation and initial value selection caused by inverse solution calculation of a common numerical iteration method are avoided.

Description

Technical Field

[0001] The invention relates to an obstacle avoidance path planning method of a vector regulating mechanism based on the RRT principle, which can simultaneously complete inverse solution calculation and obstacle avoidance planning in one algorithm and belongs to the technical field of satellite control. Background Art

[0002] The structure of the vector adjustment mechanism is similar to that of a traditional serial robot arm. Obstacle avoidance path planning refers to planning a path from the current joint angle vector sequence to the target joint angle vector sequence. Each path point (joint angle vector sequence) on the path can ensure that the mechanism does not collide with the environment. Traditional robot arm obstacle avoidance planning methods first calculate the inverse solution of the target and then plan. For the classic RRT algorithm, the joint angle vector sequence of the target point of path planning must be known in advance or the joint angle vector sequence corresponding to the target point posture must be calculated in advance before planning.

[0003] Patent CN201910715625.9 provides a robot arm motion planning method that improves the bidirectional RRT algorithm. This method integrates the traditional bidirectional RRT algorithm, introduces a method for finding the optimal parent node and a method for updating the connection of nodes, which can reduce the cost of robot arm path planning and plan a better path. However, this method is only a path planning algorithm. The premise for achieving "bidirectionality" is to calculate the inverse solution corresponding to the target point in advance. This patent realizes the parallel development of the inverse solution process and the planning process.

[0004] Patent CN202110662621.6 provides a robot path planning method and device based on an improved RRT algorithm. This method is also a robot path planning method based on an improved RRT algorithm. It uses a random sampling function based on a goal-oriented strategy to generate random sampling points, and then obtains the weight values ​​of the random sampling point directions and the target point directions, and generates new extension points based on weighted operations of the weight values. This method can ensure that the new extension point approaches the target point with the maximum probability, avoids the use of fixed step sizes for random sampling, and avoids the problem of many turning points and large corners in the path generated by the algorithm, which can significantly improve the search speed of the algorithm. However, the variable step size strategy of this patent is used to plan the path, while the random step size of this patent is used to search for inverse solutions during the planning process. Summary of the invention

[0005] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art and provide an obstacle avoidance path planning method for a vector adjustment mechanism based on the RRT principle, which solves the problem that the traditional RRT method can only perform path planning after the inverse solution is calculated in advance and the joint angles corresponding to the target posture are obtained.

[0006] The technical solution of the present invention is: an obstacle avoidance path planning method for a vector control mechanism based on the RRT principle, the method comprising the following steps:

[0007] S1, obtaining the position and posture of the target and the joint angle sequence of the vector adjustment mechanism at the current moment, and storing the joint angle sequence of the vector adjustment mechanism at the current moment as the root node of the RRT tree storage structure;

[0008] S2, generate a random number. When the random number is less than the threshold, go to step S3 and perform the inverse solution calculation step. Otherwise, go to step S4 and perform the obstacle avoidance planning step.

[0009] S3. The inverse solution calculation steps are as follows:

[0010] S3.1, taking the leaf node closest to the target in the RRT tree storage structure as the starting point of pattern search, using the pattern search method to generate a reachable new node, appending it to the RRT tree storage structure, and entering S3.2;

[0011] S3.2, determine whether the new node generated in step S3.1 is the target, if it is the target, then end, otherwise, return to step S2;

[0012] S4. The steps of obstacle avoidance planning based on the RRT principle are as follows:

[0013] S4.1. Take the leaf node closest to the target in the RRT tree storage structure as the random search starting point and randomly generate a reachable sampling point; the sampling point is a joint angle vector between [-π,π].

[0014] S4.2, grow a new leaf node with a fixed step size between the random search starting point and the reachable sampling point;

[0015] S4.3, determine whether the new leaf node generated in step S4.2 is reachable, if it is reachable, proceed to step S4.4, otherwise, perform a pruning operation, the pruning operation deletes the new leaf node and all branches connected to its parent node from the RRT tree storage structure, and return to step S2;

[0016] S4.4. Determine whether the new node is the target. If not, return to step S2. Otherwise, end.

[0017] Preferably, in step S2, the random number is between 0 and 1.

[0018] Preferably, the specific steps of generating a reachable new node using the pattern search method are as follows:

[0019] S3.11, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k =[q1,q2,...q i ,...,q N Each element q in ] iAdd a random step size Δ j It is denoted as temporary sequence q', i = 1 to n, N is the degree of freedom of the vector control mechanism, and j is initialized to 1;

[0020] S3.12. Traverse M different random step sizes Δ j Repeat step S3.11 to obtain a temporary sequence set {q'1,q'2,...,q' j ,...,q' M}, j = 1 ~ M, M = 2 N , the random step length Δ j The size belongs to [0,βΔ max ] Random number, random step length Δ j Covering both positive and negative directions, β is the random step adjustment coefficient; Δ max is the random step length threshold; the value range of the random step length adjustment coefficient β is: 0.9<β<1;

[0021] S3.13, according to the forward kinematics equation of motion and the target pose matrix T, construct the inverse solution function f(q inv , T) = f z (q inv )-T, so that f(q inv ,T)=0,q inv is the inverse solution corresponding to the target posture T, and the forward kinematic equation is: z (q) = T q , f z (q) is the positive kinematic function, T q is the end pose of the vector-adjusted mechanism calculated based on the joint angle vector q;

[0022] S3.14. Substitute each temporary sequence in the temporary sequence set into the constructed inverse solution function, compare the absolute values ​​of the inverse solution function values ​​corresponding to each temporary sequence, and record the joint angle sequence corresponding to the temporary sequence corresponding to the minimum value as q - k+1 , and record the corresponding random step length Δ min Preferably, the random step threshold Δ max is 0.025 radians;

[0023] S3.15, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k Substituting into the forward kinematics equation of motion, we obtain the first terminal pose matrix T1;

[0024] S3.16, q - k+1 Substituting into the forward kinematics equation of motion, we obtain the second terminal pose matrix T2;

[0025] S3.17. If the second terminal pose matrix T2 is closer to the target pose matrix T than the first terminal pose matrix T1, let q - =q - k+1 +α(q - k+1 -q k ), α is the search acceleration factor; q - is the joint angle after acceleration, let q k+1 =q - , and reduce the random step adjustment coefficient β; otherwise, q k+1 =q - k+1 , the random step adjustment coefficient β remains unchanged; preferably, the value range of the search acceleration coefficient α is 0.5<α<1;

[0026] S3.18, judge q k+1 Is it reachable? If q k+1 can be reached, q k+1 As a new node of the RRT tree storage structure, if q k+1 If it is unreachable, enter S3.11 and recalculate.

[0027] Preferably, the degree of proximity between the second terminal pose matrix T2 or the first terminal pose matrix T1 and the target pose matrix T is determined by the following method:

[0028] Calculate the maximum absolute value of the difference between the first terminal pose matrix T1 and the target pose T, and record it as the first pose difference C1;

[0029] Calculate the maximum absolute value of the difference between each element of the second terminal posture matrix T2 and the target posture T, and record it as the second posture difference C2;

[0030] When the first pose difference C1 is less than or equal to the second pose difference C2, it is considered that the first terminal pose matrix T1 is closer to the target pose T;

[0031] When the second pose difference C2 is less than the first pose difference C1, it is considered that the second terminal pose matrix T2 is closer to the target pose T.

[0032] Preferably, the method for determining whether the new node is reachable is:

[0033] If the node joint angle vector calculates the configuration of the vector adjustment mechanism and does not collide with the environment, and the joint angles contained in the node do not exceed the motion range of each joint, the node is considered reachable, otherwise the node is considered unreachable.

[0034] Preferably, the method for determining whether a new node is a target is:

[0035] Substitute the joint angle sequence corresponding to the new node of the RRT tree storage structure into the forward kinematics equation to obtain the predicted end pose matrix T y ;

[0036] Calculate the predicted terminal pose matrix T y The maximum absolute value of the difference between each element corresponding to the target posture T is recorded as the distance d between the positive kinematic posture of the new node and the target posture T;

[0037] If d is less than the threshold, the new node is considered to be the target; otherwise, the new node is considered not to be the target.

[0038] Preferably, the threshold is less than or equal to 0.001.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] (1) The present invention simultaneously performs inverse kinematics calculation and path obstacle avoidance planning, and has the advantage of dynamically switching unreachable inverse solution results during the calculation process.

[0041] (2) The present invention adds a pattern search algorithm to the basic algorithm framework of the RRT algorithm "random sampling search", which can simultaneously complete the calculation of the safe and achievable inverse solution of the vector control mechanism;

[0042] (3) The present invention uses a pattern search method to replace the traditional numerical iterative inverse solution algorithm, which solves the problem that the traditional RRT method can only perform path planning after the inverse solution is calculated in advance and the joint angles corresponding to the target posture are obtained, and avoids the matrix inversion, partial derivative calculation and initial value selection problems caused by the common numerical iterative method for inverse solution calculation;

[0043] (4) The present invention integrates the inverse solution calculation and RRT obstacle avoidance planning, which solves the problem that in the framework of traditional planning algorithms, after a single inverse solution calculation, the target is found to be unreachable and the inverse solution needs to be recalculated;

[0044] (5) The present invention simultaneously sets up an inverse solution search branch and an obstacle avoidance planning branch to alternately complete inverse solution calculation and trajectory generation;

[0045] (6) The present invention uses a random step size mechanism in the pattern search algorithm to facilitate approximation of the exact inverse solution;

[0046] (7) When planning the trajectory, the present invention uses a pruning strategy to continuously remove the inverse solution values ​​that may collide and the "branches" connected to the inverse solution in the rapid expansion random tree, thereby reducing the size of the RRT tree and the amount of calculation for each iteration. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1This is a flow chart of an obstacle avoidance path planning method of a vector adjustment mechanism based on the RRT principle according to an algorithm of an embodiment of the present invention;

[0048] FIG2( a ) is an embodiment of the present invention, where the inverse solution calculation branch generates a new node 03 which is unreachable;

[0049] FIG2( b ) is an embodiment of the present invention, in which a new node is generated to be unreachable and pruning is performed;

[0050] FIG2(c) is an embodiment of the present invention, in which the inverse solution calculation branch and the obstacle avoidance planning branch are performed alternately to generate reachable 01 and 01 nodes;

[0051] Figure 2(d) is an embodiment of the present invention, where a new node 011 is generated to be unreachable, and the node is pruned to generate a new reachable node 021;

[0052] FIG2(e) is an embodiment of the present invention, generating new reachable nodes 0211 and 0212;

[0053] FIG2( f ) is an embodiment of the present invention, where the inverse solution calculation branch finds the target (inverse solution 2) through node 0211 and completes the obstacle avoidance planning at the same time. DETAILED DESCRIPTION

[0054] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0055] The present invention improves the RRT planning algorithm by setting up an inverse solution calculation branch and an obstacle avoidance planning branch. Figure 1 As shown, the method of the present invention comprises the following steps:

[0056] S1, obtaining the position and posture of the target and the joint angle sequence of the vector adjustment mechanism at the current moment, and storing the joint angle sequence of the vector adjustment mechanism at the current moment as the root node of the RRT tree storage structure;

[0057] S2. Generate a random number. When the random number is less than a threshold, proceed to step S3 to perform an inverse solution calculation step. Otherwise, proceed to step S4 to perform an obstacle avoidance planning step. In a specific embodiment of the present invention, the random number is between 0 and 1, and the threshold value of the threshold matching is 0.5.

[0058] S3. The inverse solution calculation steps are as follows:

[0059] S3.1, taking the leaf node closest to the target in the RRT tree storage structure as the starting point of pattern search, using the pattern search method to generate a reachable new node, appending it to the RRT tree storage structure, and entering S3.2;

[0060] S3.2, determine whether the new node generated in step S3.1 is the target, if it is the target, then end, otherwise, return to step S2;

[0061] S4. The steps of obstacle avoidance planning based on the RRT principle are as follows:

[0062] S4.1. Take the leaf node closest to the target in the RRT tree storage structure as the random search starting point and randomly generate a reachable sampling point; the sampling point is a joint angle vector between [-π,π].

[0063] S4.2, grow a new leaf node with a fixed step size between the random search starting point and the reachable sampling point;

[0064] S4.3, determine whether the new leaf node generated in step S4.2 is reachable, if it is reachable, proceed to step S4.4, otherwise, perform a pruning operation, the pruning operation deletes the new leaf node and all branches connected to its parent node from the RRT tree storage structure, and return to step S2;

[0065] S4.4. Determine whether the new node is the target. If not, return to step S2. Otherwise, end.

[0066] The obstacle avoidance path planning method of the vector regulating mechanism based on the RRT principle proposed by the present invention performs inverse solution search calculation while searching for a collision-free safe path, which can ensure the high accuracy of the inverse solution calculation and the safety of the trajectory of the vector regulating mechanism. In the process of iterative calculation, if it is found that the calculated inverse solution is unsafe, the algorithm can also use the random sampling idea in the RRT algorithm to continue to automatically search for the next safe inverse solution.

[0067] The inverse solution calculation branch of the present invention uses a standard pattern search algorithm framework, and changes the search step length from a fixed value to a random step length. Specifically, in the inverse solution search process, two exploration modes set by the classic pattern search method are used. One is to move a step length along the positive and negative directions of each joint. In the present invention, the step length is changed from a fixed value to a random value. The other is to accelerate the movement along the direction of the line connecting two adjacent detection points. By alternating between the two detection methods, a direction close to the inverse solution joint angle sequence is found. This strategy reduces the amount of calculation compared to the classic inverse solution solution method. The significance of the random step length is to ensure that the precise inverse solution value is gradually approached. Because in the process of approximating the precise value of the inverse solution, the changes in the step lengths of each joint in the traditional pattern search method are consistent, and the random step length can produce inconsistent step length changes, so it can better approximate the precise inverse solution.

[0068] In the inverse solution algorithm, the specific steps of using the pattern search method to generate a reachable new node are as follows:

[0069] S3.11, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k =[q1,q2,...q i ,...,q NEach element q in ] i Add a random step size Δ j It is denoted as temporary sequence q', i = 1 to n, N is the degree of freedom of the vector control mechanism, and j is initialized to 1;

[0070] S3.12. Traverse M different random step sizes Δ j Repeat step S3.11 to obtain a temporary sequence set {q'1,q'2,...,q' j ,...,q' M}, j = 1 ~ M, M = 2 N , the random step length Δ j The size belongs to [0,βΔ max ] Random number, random step length Δ j Covering both positive and negative directions, β is the random step adjustment coefficient; Δ max is the random step length threshold; the value range of the random step length adjustment coefficient β is: 0.9<β<1;

[0071] S3.13, according to the forward kinematics equation of motion and the target pose matrix T, construct the inverse solution function f(q inv , T) = f z (q inv )-T, so that f(q inv ,T)=0,q inv is the inverse solution corresponding to the target posture T, and the forward kinematic equation is: z (q) = T q , f z (q) is the positive kinematic function, T q is the end pose of the vector-adjusted mechanism calculated based on the joint angle vector q;

[0072] S3.14. Substitute each temporary sequence in the temporary sequence set into the constructed inverse solution function, compare the absolute values ​​of the inverse solution function values ​​corresponding to each temporary sequence, and record the joint angle sequence corresponding to the temporary sequence corresponding to the minimum value as q - k+1 , and record the corresponding random step length Δ min ;

[0073] S3.15, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k Substituting into the forward kinematics equation of motion, we obtain the first terminal pose matrix T1;

[0074] S3.16, q - k+1 Substituting into the forward kinematics equation of motion, we obtain the second terminal pose matrix T2;

[0075] S3.17. If the second terminal pose matrix T2 is closer to the target pose matrix T than the first terminal pose matrix T1, let q - =q - k+1 +α(q - k+1 -q k ), α is the search acceleration factor; q - is the joint angle after acceleration, let q k+1 =q - , and reduce the random step adjustment coefficient β; otherwise, q k+1 =q - k+1 , the random step adjustment coefficient β remains unchanged;

[0076] S3.18, judge q k+1 Is it reachable? If q k+1 can be reached, q k+1 As a new node of the RRT tree storage structure, if q k+1 If it is unreachable, enter S3.11 and recalculate.

[0077] The closeness of the second terminal pose matrix T2 or the first terminal pose matrix T1 to the target pose matrix T is determined by the following method:

[0078] Calculate the maximum absolute value of the difference between the first terminal pose matrix T1 and the target pose T, and record it as the first pose difference C1;

[0079] Calculate the maximum absolute value of the difference between each element of the second terminal posture matrix T2 and the target posture T, and record it as the second posture difference C2;

[0080] When the first pose difference C1 is less than or equal to the second pose difference C2, it is considered that the first terminal pose matrix T1 is closer to the target pose T;

[0081] When the second pose difference C2 is less than the first pose difference C1, it is considered that the second terminal pose matrix T2 is closer to the target pose T.

[0082] In this method, after the obstacle avoidance planning branch searches for the joint angle of the vector adjustment mechanism that causes the collision, it will gradually guide the search area to the safe area. Theoretically, if there is enough time and enough iterations, it is possible to continue to calculate a safe inverse solution through the inverse solution branch. However, the computational complexity of a single search of the traditional RRT algorithm increases with the increase in the scale of the tree storage structure. The scale of each cycle tree is increasing. If the scale of the tree is not controlled, the closer it is to the accurate value of the inverse solution, the slower the iteration speed. Therefore, in the search process of the present invention, if it is found that the generated new node is unreachable, then a pruning strategy is executed. The meaning of pruning is: first record all branches connected to the unreachable node and the parent node of the node, and then delete all these branches in the tree storage structure (the parent node of the node does not need to be deleted). These branches are useless for inverse solution calculation and path planning because they are connected to unreachable nodes. Deleting them will make the resources consumed by obstacle avoidance planning calculation smaller. The definition of useless branches here is: the path along the branch will reach the unreachable node.

[0083] The method for determining whether the new node is reachable is:

[0084] If the node joint angle vector calculates the configuration of the vector adjustment mechanism and does not collide with the environment, and the joint angles contained in the node do not exceed the motion range of each joint, the node is considered reachable, otherwise the node is considered unreachable.

[0085] The method to determine whether a new node is a target is:

[0086] Substitute the joint angle sequence corresponding to the new node of the RRT tree storage structure into the forward kinematics equation to obtain the predicted end pose matrix T y ;

[0087] Calculate the predicted terminal pose matrix T y The maximum absolute value of the difference between each element corresponding to the target posture T is recorded as the distance d between the positive kinematic posture of the new node and the target posture T;

[0088] If d is less than the threshold, the new node is considered to be the target; otherwise, the new node is considered not to be the target.

[0089] Preferably, the threshold is less than or equal to 0.001.

[0090] Example:

[0091] Figure 2(a) to Figure 2(f) A simple example is given to demonstrate how the algorithm proposed in the present invention executes the inverse solution calculation branch and the obstacle avoidance planning branch alternately under the RRT algorithm framework, continuously eliminates unsafe inverse solutions and performs pruning operations, and finally uses the idea of ​​random sampling search in the RRT algorithm to find a new safe inverse solution.

[0092] In Figure 2(a), in a certain round of iteration, the leaf node currently under investigation is numbered 0. After two obstacle avoidance plans, leaf nodes 01 and 02 are generated. When the inverse solution calculation branch is entered for the third time to produce leaf node 03, it is detected that the corresponding mechanism will collide, and it is considered that leaf node 03 is unreachable. Then a pruning is performed. In addition to not adding a new node numbered 03, the existing nodes 01 and 02 are also subtracted, as shown in Figure 2(b).

[0093] Starting from the state of Figure 2(b), first perform an inverse solution calculation to generate node 01, then perform an obstacle avoidance plan to generate node 02, as shown in Figure 2(c), then perform an inverse solution calculation. When generating node 011, a collision is detected, but there is no other branch on node 01 at this time, so node 011 is simply discarded and a new round of search begins. Starting from the current state, first perform an inverse solution calculation to generate leaf node 021. At this time, in Figure 2(d), the leaf node closest to the target has changed from node 01 to node 021, so the newly generated leaf node of the inverse solution calculation will use node 021 as the parent node to generate nodes 0211 and 0212, as shown in Figure 2(e). Finally, perform an inverse solution search to directly find inverse solution 2, and the search process ends, as shown in Figure 2(f). The algorithm will simultaneously output the final safe inverse solution 2 and the motion trajectory of the vector control mechanism.

[0094] Although the present invention has been disclosed as above in the form of a preferred embodiment, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications to the technical solution of the present invention by using the methods and technical contents disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall fall within the protection scope of the technical solution of the present invention.

Claims

1. An obstacle avoidance path planning method for a vector control mechanism based on the RRT principle, characterized in that The steps include: S1, obtaining the position and posture of the target and the joint angle sequence of the vector adjustment mechanism at the current moment, and storing the joint angle sequence of the vector adjustment mechanism at the current moment as the root node of the RRT tree storage structure; S2, generate a random number. When the random number is less than the threshold, go to step S3 and perform the inverse solution calculation step. Otherwise, go to step S4 and perform the obstacle avoidance planning step. S3, the inverse solution calculation steps are as follows: S3.1, taking the leaf node closest to the target in the RRT tree storage structure as the starting point of pattern search, using the pattern search method to generate a reachable new node, appending it to the RRT tree storage structure, and entering S3.2; S3.2, determine whether the new node generated in step S3.1 is the target, if it is the target, then end, otherwise, return to step S2; S4. The steps of obstacle avoidance planning based on the RRT principle are as follows: S4.1, taking the leaf node closest to the target in the RRT tree storage structure as the random search starting point, randomly generate a reachable sampling point; S4.2, grow a new leaf node with a fixed step size between the random search starting point and the reachable sampling point; S4.3, determine whether the new leaf node generated in step S4.2 is reachable, if it is reachable, proceed to step S4.4, otherwise, perform a pruning operation, the pruning operation deletes the new leaf node and all branches connected to its parent node from the RRT tree storage structure, and return to step S2; S4.

4. Determine whether the new node is the target. If not, return to step S2. Otherwise, end.

2. According to claim 1, the obstacle avoidance path planning method of the vector control mechanism based on the RRT principle is characterized in that: In step S2, the random number is between 0 and 1.

3. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The specific steps of using the pattern search method to generate a reachable new node are as follows: S3.11, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k =[q1,q2,...q i ,...,q N Each element q in ] i Add a random step size Δ j It is denoted as temporary sequence q', i = 1 to n, N is the degree of freedom of the vector control mechanism, and j is initialized to 1; S3.

12. Traverse M different random step sizes Δ j Repeat step S3.11 to obtain a temporary sequence set {q'1,q'2,...,q' j ,...,q' M }, j = 1 ~ M, M = 2 N , the random step length Δ j The size belongs to [0,βΔ max ] Random number, random step length Δ j Covering both positive and negative directions, β is the random step adjustment coefficient; Δ max is the random step length threshold; the value range of the random step length adjustment coefficient β is: 0.9<β<1; S3.13, according to the forward kinematics equation of motion and the target pose matrix T, construct the inverse solution function f(q inv , T) = f z (q inv )-T, so that f(q inv ,T)=0,q inv is the inverse solution corresponding to the target posture T, and the forward kinematic equation is: z (q) = T q , f z (q) is the positive kinematic function, T q is the end pose of the vector-adjusted mechanism calculated based on the joint angle vector q; S3.

14. Substitute each temporary sequence in the temporary sequence set into the constructed inverse solution function, compare the absolute values ​​of the inverse solution function values ​​corresponding to each temporary sequence, and record the joint angle sequence corresponding to the temporary sequence corresponding to the minimum value as q - k+1 , and record the corresponding random step length Δ min ; S3.15, the joint angle sequence q of the vector adjustment mechanism corresponding to the pattern search starting point k Substituting into the forward kinematics equation of motion, we obtain the first terminal pose matrix T1; S3.16, q - k+1 Substituting into the forward kinematics equation of motion, we obtain the second terminal pose matrix T2; S3.

17. If the second terminal pose matrix T2 is closer to the target pose matrix T than the first terminal pose matrix T1, let q - =q - k+1 +α(q - k+1 -q k ), α is the search acceleration factor; q - is the joint angle after acceleration, let q k+1 =q - , and reduce the random step adjustment coefficient β; otherwise, q k+1 =q - k+1 , the random step adjustment coefficient β remains unchanged; S3.18, judge q k+1 Is it reachable? If q k+1 can be reached, k+1 As a new node of the RRT tree storage structure, if q k +1 If it is unreachable, enter S3.11 and recalculate.

4. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The closeness of the second terminal pose matrix T2 or the first terminal pose matrix T1 to the target pose matrix T is determined by the following method: Calculate the maximum absolute value of the difference between the first terminal pose matrix T1 and the target pose T, and record it as the first pose difference C1; Calculate the maximum absolute value of the difference between each element of the second terminal posture matrix T2 and the target posture T, and record it as the second posture difference C2; When the first pose difference C1 is less than or equal to the second pose difference C2, it is considered that the first terminal pose matrix T1 is closer to the target pose T; When the second pose difference C2 is less than the first pose difference C1, it is considered that the second terminal pose matrix T2 is closer to the target pose T.

5. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The random step threshold Δ max is 0.025 radians.

6. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The value range of the search acceleration coefficient α is 0.5<α<1.

7. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The method for determining whether the new node is reachable is: If the node joint angle vector calculates the configuration of the vector adjustment mechanism and does not collide with the environment, and the joint angles contained in the node do not exceed the motion range of each joint, the node is considered reachable, otherwise the node is considered unreachable.

8. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: In step S4.1, the sampling point is a joint angle vector between [-π, π].

9. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 1, characterized in that: The method to determine whether a new node is a target is: Substitute the joint angle sequence corresponding to the new node of the RRT tree storage structure into the forward kinematics equation to obtain the predicted end pose matrix T y ; Calculate the predicted terminal pose matrix T y The maximum absolute value of the difference between each element corresponding to the target posture T is recorded as the distance d between the positive kinematic posture of the new node and the target posture T; If d is less than the threshold, the new node is considered to be the target; otherwise, the new node is considered not to be the target.

10. The obstacle avoidance path planning method of a vector control mechanism based on the RRT principle according to claim 9, characterized in that: The threshold is less than or equal to 0.001.

Images