An obstacle avoidance path planning method for a vector adjusting mechanism based on RRT principle

Abstract

The application relates to a kind of obstacle avoidance path planning methods of vector adjusting mechanism based on RRT principle, belong to satellite control technical field.This method carries out inverse kinematics calculation and path obstacle avoidance planning simultaneously, uses pattern search method to replace traditional numerical iteration inverse algorithm, solves the problem that traditional RRT method can only carry out path planning after inverse solution is calculated in advance and joint angle corresponding to target pose is obtained, avoids the problems of matrix inversion, partial derivative calculation and initial value selection caused by inverse solution calculation of common numerical iteration method.

Description

TECHNICAL FIELD

[0001] The application relates to an obstacle avoidance path planning method of a vector adjusting mechanism based on an 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

[0002] The structure of the vector adjusting mechanism is similar to that of a traditional serial type mechanical arm, and the obstacle avoidance path planning refers to planning a path from a current time joint angle vector sequence to a target joint angle vector sequence, each path point (joint angle vector sequence) on the path being capable of guaranteeing that the mechanism does not collide with the environment. Traditional mechanical arm obstacle avoidance planning methods are all to calculate the inverse solution of the target and then plan, and for the classic RRT algorithm, the target joint angle vector sequence of the path planning must be known in advance or the joint angle vector sequence corresponding to the target point pose must be calculated in advance, and then planning is performed.

[0003] Patent CN201910715625.9 provides a mechanical arm motion planning method for improving a bidirectional RRT algorithm. The method fuses a traditional bidirectional RRT algorithm, introduces an optimal parent node searching mode and an updated node connecting mode, can reduce the mechanical arm path planning cost and plan a relatively optimal path, but the method is only a path planning algorithm, and the prerequisite for realizing the "bidirectional" is to calculate the inverse solution corresponding to the target point in advance, and the present application realizes 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. The method is also a robot path planning method based on an improved RRT algorithm. A random sampling point is generated by using a target-oriented strategy-based random sampling function, then a weight value of a random sampling point direction and a target point direction is obtained, and a new expansion point is generated by weight value weighted operation. The method can ensure that the new expansion point is close to the target point with the maximum probability, avoid random sampling using a fixed step length, and solve the problems of many turning points and large turning angles in the path generated by the algorithm, and can significantly improve the search speed of the algorithm. However, the variable step length strategy of the patent is used for planning the path, and the random step length of the present application is used for searching the inverse solution in the planning process. SUMMARY

[0005] The technical problem solved by the present application is to overcome the shortcomings of the prior art and provide an obstacle avoidance path planning method of a vector adjusting mechanism based on an RRT principle, which solves the problem that the traditional RRT method can only plan a path after the inverse solution is calculated and the joint angle corresponding to the target pose is obtained.

[0006] The technical scheme of the present application is an obstacle avoidance path planning method of a vector adjusting mechanism based on an RRT principle, which comprises the following steps:

[0007] S1, acquiring a pose of a target, a joint angle sequence of a vector adjusting mechanism at a current time, and storing the joint angle sequence of the vector adjusting mechanism at the current time as a root node of an RRT tree storage structure;

[0008] S2, generating a random number, when the random number is less than a threshold value, entering step S3 to perform an inverse solution calculation step, otherwise, entering step S4 to perform an obstacle avoidance planning step;

[0009] S3, the inverse solution calculation step is as follows:

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

[0011] S3.2, judging whether the new node generated in step S3.1 is the target, if yes, ending, otherwise, returning to step S2;

[0012] S4, the obstacle avoidance planning step based on the RRT principle is as follows:

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

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

[0015] S4.3, judging whether the new leaf node generated in step S4.2 is reachable, if yes, entering step S4.4, otherwise, performing a pruning operation, the pruning operation deletes the new leaf node and all branches connected with the parent node of the new leaf node from the RRT tree storage structure, and returning to step S2;

[0016] S4.4, judging whether the new node is the target, if no, returning to step S2, otherwise, ending.

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

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

[0019] S3.11, taking the joint angle sequence q k =[q1,q2,...q i ,...,q N ] of the vector adjusting mechanism corresponding to the pattern search starting point as the initial value of the new node. iplus a random step size Δ j denoted as temporary sequence q', i = 1 ~ n, N is the degree of freedom of the vector adjusting mechanism, j is initialized as 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 size of the random step size Δ j belongs to [0, βΔ max ], the random step size Δ j covers positive and negative directions, and β is a random step size adjustment coefficient; Δ max is a random step size threshold; the value range of the random step size adjustment coefficient β is: 0.9 < β < 1;

[0021] S3.13, constructing an inverse solution function f(q inv , T) = f z (q inv ) - T, such that f(q inv , T) = 0, q inv is the inverse solution corresponding to the target pose T, and the forward kinematics equation is f z (q) = T q , f z (q) is a forward kinematics function, and T q is the end pose of the vector adjusting mechanism calculated based on the joint angle vector q;

[0022] S3.14, substituting each temporary sequence in the temporary sequence set into the constructed inverse solution function, comparing the absolute values of the inverse solution function values corresponding to each temporary sequence, and recording the joint angle sequence corresponding to the temporary sequence corresponding to the minimum value as q - k+1 , and recording the corresponding random step size Δ min ; preferably, the random step size threshold Δ max is 0.025 radian;

[0023] S3.15, substituting the joint angle sequence q k of the vector adjusting mechanism corresponding to the mode search starting point into the forward kinematics equation to obtain a first end pose matrix T1;

[0024] S3.16, substituting q - k+1 into the forward kinematics equation to obtain a second end pose matrix T2;

[0025] S3.17, if the second end pose matrix T2 is closer to the target pose matrix T than the first end pose matrix T1, let q - = q - k+1 + a(q - k+1 - q k ), a is a search acceleration coefficient; q - is the accelerated joint angle, let q k+1 = q - , and adjust the random step adjustment coefficient β downward; otherwise, q k+1 = q - k+1 , the random step adjustment coefficient β remains unchanged; preferably, the search acceleration coefficient a is in the range of 0.5 < a < 1;

[0026] S3.18, determine whether q k+1 is reachable, if q k+1 is reachable, store q k+1 as a new node of the RRT tree structure, if q k+1 is not reachable, go to S3.11 to recalculate.

[0027] Preferably, the closeness of the second end pose matrix T2 or the first end pose matrix T1 to the target pose matrix T is determined by the following method:

[0028] Calculate the maximum value of the absolute value of the difference between the corresponding elements of the first end pose matrix T1 and the target pose T, denoted as the first pose difference C1;

[0029] Calculate the maximum value of the absolute value of the difference between the corresponding elements of the second end pose matrix T2 and the target pose T, denoted as the second pose 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 end 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 end pose matrix T2 is closer to the target pose T.

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

[0033] The node joint angle vector calculates the configuration of the vector adjusting mechanism, which will not collide with the environment, and the node contains joint angles that do not exceed the motion range of each joint, then it is considered that the node is reachable, otherwise it is considered that the node is not reachable.

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

[0035] The joint angle sequence corresponding to the new node of the RRT tree is substituted into the forward kinematics motion equation to obtain a predicted end position matrix T y ;

[0036] The predicted end position matrix T y is substituted into the forward kinematics motion equation to obtain a predicted end position matrix T

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

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

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

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

[0041] (2) The present application adds a pattern search algorithm to the basic algorithm framework of the RRT algorithm "random sampling search", and can simultaneously complete the calculation of the safe reachable inverse solution of the vector adjustment mechanism.

[0042] (3) The present application uses a pattern search method to replace the traditional numerical iterative inverse solution algorithm, solves the problem that the traditional RRT method can only perform path planning after the inverse solution is calculated and the joint angle corresponding to the target position is obtained, and avoids the problems of matrix inversion, partial derivative calculation and initial value selection caused by the inverse solution calculation of the common numerical iterative method.

[0043] (4) The present application combines the inverse solution calculation and the RRT obstacle avoidance planning, solves the problem that after a single inverse solution calculation under the traditional planning algorithm framework, it is found that the target is unreachable, and the inverse solution needs to be recalculated.

[0044] (5) The present application simultaneously sets the inverse solution search branch and the obstacle avoidance planning branch, and alternately completes the inverse solution calculation and the trajectory generation.

[0045] (6) The present application uses a random step mechanism in the pattern search algorithm, which facilitates the approximation of the accurate inverse solution.

[0046] (7) The present application uses a pruning strategy when planning the trajectory, which constantly removes the inverse solution values that will collide and the "branches" connected to the inverse solution in the rapidly expanding random tree, thereby reducing the size of the RRT tree and the calculation amount of each iteration. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1The flow chart of the obstacle avoidance path planning method of the vector adjusting mechanism based on the RRT principle is shown in the algorithm of the embodiment of the present application.

[0048] Fig. 2(a) is a new node 03 generated by the inverse solution calculation branch in the embodiment of the present application, which is unreachable.

[0049] Fig. 2(b) is a new node generated in the embodiment of the present application, which is unreachable, and pruning is performed.

[0050] Fig. 2(c) is the embodiment of the present application, in which the inverse solution calculation branch and the obstacle avoidance planning branch are alternately performed, and reachable 01 and 01 nodes are generated.

[0051] Fig. 2(d) is the embodiment of the present application, in which a new node 011 is generated, which is unreachable, pruning is performed, and a new reachable 021 node is generated.

[0052] Fig. 2(e) is the embodiment of the present application, in which new reachable nodes 0211 and 0212 are generated.

[0053] Fig. 2(f) is the embodiment of the present application, in which the inverse solution calculation branch finds the target (inverse solution 2) through the node 0211, and the obstacle avoidance planning is completed at the same time. DETAILED DESCRIPTION

[0054] The present application will be described in detail below in combination with the drawings and specific embodiments.

[0055] The present application improves the RRT planning algorithm, and sets inverse solution calculation branches and obstacle avoidance planning branches respectively. Figure 1 As shown in the figure, the method of the present application comprises the following steps:

[0056] S1, the pose of the target, the joint angle sequence of the vector adjusting mechanism at the current time are obtained, and the joint angle sequence of the vector adjusting mechanism at the current time is stored as the root node of the RRT tree type storage structure;

[0057] S2, a random number is generated, when the random number is less than the threshold value, step S3 is entered, the inverse solution calculation step is performed, otherwise, step S4 is entered, the obstacle avoidance planning step is performed; in a specific embodiment of the present application, the random number is between 0 and 1, and the threshold value matching the threshold value is 0.5.

[0058] S3, the inverse solution calculation step is as follows:

[0059] S3.1, the leaf node closest to the target in the RRT tree type storage structure is taken as the starting point of pattern search, a reachable new node is generated by using the pattern search method, and is appended to the RRT tree type storage structure, and S3.2 is entered;

[0060] S3.2, it is judged whether the new node generated in step S3.1 is the target, if yes, the process is ended, otherwise, step S2 is returned;

[0061] S4, the obstacle avoidance planning step based on RRT principle is as follows:

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

[0063] S4.2, a new leaf node is grown between the random search starting point and the reachable sampling point at a fixed step length;

[0064] S4.3, judging whether the new leaf node generated in step S4.2 is reachable, if yes, entering step S4.4, otherwise, performing a pruning operation, the pruning operation deletes all branches connected by the new leaf node and its parent node from the RRT tree type storage structure, and returning to step S2;

[0065] S4.4, judging whether the new node is the target, if no, returning to step S2, otherwise, ending.

[0066] The obstacle avoidance path planning method of the vector adjusting mechanism based on the RRT principle provided in the application can search a collision-free safe path while performing inverse solution search calculation, so that the high precision of inverse solution calculation and the trajectory safety of the vector adjusting mechanism operation can be ensured. In the process of iterative calculation, if it is found that the calculated inverse solution is not safe, the algorithm can also automatically search the next safe inverse solution by using the random sampling idea in the RRT algorithm.

[0067] The inverse solution calculation branch of the application uses a standard pattern search algorithm framework, and the search step length is changed from a fixed value to a random step length. Specifically, in the inverse solution search process, two exploration modes set by the classical pattern search method are used, one is to move a step length in the positive and negative directions of each joint, and in the application, the step length is changed from a fixed value to a random value, and the other is to accelerate along the direction of the connecting line of the adjacent two detection points. By alternately using the two detection modes, a direction close to the inverse solution joint angle sequence is found. Compared with the classical inverse solution solving method, the calculation amount is reduced. The significance of the random step length is to ensure the gradual approximation of the accurate inverse solution value. Because in the process of approximating the accurate inverse solution value, the change of the step length of each joint in the traditional pattern search method is consistent, while the random step length can produce inconsistent step length changes, so the accurate inverse solution can be better approximated.

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

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

[0070] S3.12, traversing M different random step sizes Δ j , repeating 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 size Δ j size belongs to [0, βΔ max ] random number, the random step size Δ j covers both positive and negative directions, β is a random step size adjustment coefficient; Δ max is a random step size threshold; the value range of the random step size adjustment coefficient β is: 0.9 < β < 1;

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

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

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

[0074] S3.16, substituting q - k+1 into the forward kinematics motion equation to obtain a second end pose matrix T2;

[0075] S3.17, if the second end pose matrix T2 is closer to the target pose matrix T than the first end pose matrix T1, let q - = q - k+1 + a(q - k+1 - q k ), a is a search acceleration coefficient; q - is the accelerated joint angle, let q k+1 = q - , and adjust the random step length adjustment coefficient β down; otherwise, q k+1 = q - k+1 , the random step length adjustment coefficient β remains unchanged;

[0076] S3.18, determine whether q k+1 is reachable, if q k+1 is reachable, store q k+1 as a new node of the RRT tree structure, if q k+1 is not reachable, go to S3.11 to recalculate.

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

[0078] Calculate the maximum value of the absolute value of the difference of each element of the first end pose matrix T1 and the target pose T, denoted as the first pose difference C1;

[0079] Calculate the maximum value of the absolute value of the difference of each element of the second end pose matrix T2 and the target pose T, denoted as the second pose 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 end 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 end pose matrix T2 is closer to the target pose T.

[0082] In the method, the obstacle avoidance planning branch gradually guides the searched region to a safe region after searching a joint angle of the vector adjusting mechanism that causes collision, and theoretically, if the time is sufficient and the iteration number is sufficient, a safe inverse solution can be calculated through the inverse solution branch, but the calculation amount of a single search of the traditional RRT algorithm increases with the increase of the size of the tree type storage structure, and the size of the tree increases in each cycle, if the size of the tree is not controlled, the iteration speed becomes slower as the inverse solution value is closer to the accurate value. Therefore, in the search process, if it is found that a new node is unreachable, a pruning strategy is executed. The pruning means that all branches connected with the unreachable node and the parent node of the node are recorded, and then the branches are all deleted in the tree type storage structure (the parent node of the node does not need to be deleted). The branches belong to useless branches for inverse solution calculation and path planning because they are connected with the unreachable node, and deleting the branches makes the resources consumed by the obstacle avoidance planning calculation smaller. Here, the useless branches are defined as branches along which the path will go to the unreachable node.

[0083] The method for judging whether the new node is reachable is as follows:

[0084] If the joint angle vector of the node calculates the configuration of the vector adjusting mechanism and no collision occurs between the configuration and the environment, and the joint angles contained in the node do not exceed the motion ranges of the joints, it is considered that the node is reachable, otherwise, it is considered that the node is unreachable.

[0085] The method for judging whether the new node is the target is as follows:

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

[0087] The maximum value of the absolute values of the element difference between the predicted end pose matrix T y and the target pose T is recorded as the distance d between the forward kinematics pose of the new node and the target pose T.

[0088] If d is less than a threshold, it is considered that the new node is the target, otherwise, it is considered that the new node is not the target.

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

[0090] Embodiment:

[0091] Figures 2(a) to 2(f) A simple example is given to show how the algorithm proposed in the application executes the inverse solution calculation branch and the obstacle avoidance planning branch alternately under the RRT algorithm framework, continuously eliminates unsafe inverse solutions and executes the pruning operation, and finally finds a new safe inverse solution by using the idea of random sampling search in the RRT algorithm.

[0092] In Fig. 2(a), in a certain iteration, the current examined leaf node is numbered 0, after 2 collision avoidance planning, leaf nodes 01 and 02 are generated, when entering the inverse solution calculation branch for the third time to produce leaf node 03, it is detected that the corresponding mechanism will cause collision, and it is considered that leaf node 03 is unreachable, then pruning is performed, except that the new node numbered 03 will not be added, the existing 01 and 02 nodes will also be removed, as shown in Fig. 2(b).

[0093] From the state of Fig. 2(b), first perform inverse solution calculation to generate 01 node, then perform collision avoidance planning to generate 02 node, as shown in Fig. 2(c), then perform inverse solution calculation, when 011 node is generated, collision is detected, but at this time there is no other branch on the 01 node, so only 011 node is discarded, and a new round of search is started. From the current state, first perform inverse solution calculation to generate leaf node 021, at this time, in Fig. 2(d), the leaf node closest to the target has changed from 01 node to 021 node, so the newly generated leaf node of the inverse solution calculation will take 021 node as the parent node to generate nodes 0211 and 0212, as shown in Fig. 2(e), finally, perform inverse solution search to directly find inverse solution 2, and the search process ends, as shown in Fig. 2(f). The algorithm will output the final safe inverse solution 2 and the motion trajectory of the vector adjusting mechanism.

[0094] Although the present application has been disclosed with the above preferred embodiments, it is not intended to limit the present application, any person skilled in the art can make possible changes and modifications to the technical solutions of the present application by using the disclosed methods and technical contents without departing from the spirit and scope of the present application, therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, which does not deviate from the technical solutions of the present application, all belong to the protection scope of the technical solutions of the present application.

Claims

1. A method for obstacle avoidance path planning of a vector adjustment mechanism based on RRT principle, characterized in that The method comprises the following steps: S1, acquiring a pose of a target, a joint angle sequence of a vector adjusting mechanism at a current time, and storing the joint angle sequence of the vector adjusting mechanism at the current time as a root node of an RRT tree type storage structure; S2, generating a random number, and when the random number is less than a threshold value, entering step S3 to perform an inverse solution calculation step, otherwise, entering step S4 to perform an obstacle avoidance planning step; S3, the inverse solution calculation step is as follows: S3.1, taking a leaf node closest to the target in the RRT tree type storage structure as a pattern search starting point, generating a reachable new node by using a pattern search method, and appending the new node to the RRT tree type storage structure, and entering S3.2; S3.2, judging whether the new node generated in step S3.1 is the target, and if so, ending, otherwise, returning to step S2; S4, the obstacle avoidance planning step based on the RRT principle is as follows: S4.1, taking a leaf node closest to the target in the RRT tree type storage structure as a random search starting point, and randomly generating a reachable sampling point; S4.2, growing a new leaf node between the random search starting point and the reachable sampling point at a fixed step length; S4.3, judging whether the new leaf node generated in step S4.2 is reachable, and if so, entering step S4.4, otherwise, performing a pruning operation, and the pruning operation deletes all branches connected by the new leaf node and its parent node from the RRT tree type storage structure, and returning to step S2; S4.4, judging whether the new node is the target, and if not, returning to step S2, otherwise, ending.

2. The obstacle avoidance path planning method for a vector adjustment mechanism based on the RRT principle according to claim 1, characterized in that, In step S2, the random number is between 0 and 1.

3. The obstacle avoidance path planning method for a vector adjustment mechanism based on the RRT principle according to claim 1, characterized in that, The specific steps of generating the reachable new node by using the pattern search method are as follows: S3.11, adjust the joint angle sequence q of the vector adjustment mechanism corresponding to the mode search starting point k = [q1, q2,... q i N-1] N ] by adding a random step size Δ i to each element q j , denoted as temporary sequence q', i = 1 ~ n, N is the degree of freedom of the vector adjustment mechanism, j is initialized to 1; S3.12, traversing M different random steps Δ j , repeating step S3.11 to obtain a temporary sequence set {q'1, q'2,..., q' j ,...,q' M}, j = 1 ~ M, M = 2 N , the size of the random step Δ j belongs to [0, βΔ max ] random number, the random step Δ j covers both positive and negative directions, and β is a random step adjustment coefficient; Δ max is a random step threshold value; the value range of the random step adjustment coefficient β is: 0.9 < β < 1; S3.13, constructing an inverse solution function f(q inv , T) = f z (q inv ) - T, such that f(q inv , T) = 0, q inv is an inverse solution corresponding to the target pose T, the forward kinematics motion equation is: f z (q) = T q , f z (q) is a forward kinematics function, and T q is a vector adjustment mechanism end pose calculated based on a joint angle vector q; S3.14, substituting each temporary sequence in the temporary sequence set into the constructed inverse solution function, comparing the absolute values of the inverse solution function values corresponding to each temporary sequence, and recording the joint angle sequence corresponding to the temporary sequence with the minimum value as q - k+1 , and record the corresponding random step size Δ min ; S3.15, adjust the joint angle sequence q of the vector adjusting mechanism corresponding to the mode search starting point k Substitute the forward kinematics motion equation to obtain the first end position matrix T1; S3.16, q - k+1 Substituting the forward kinematics motion equation, the second end position matrix T2 is obtained; S3.17, if the second end pose matrix T2 is closer to the target pose matrix T than the first end pose matrix T1, let q - = q - k+1 + a(q - k+1 - q k ), a is a search acceleration coefficient; q - is an accelerated joint angle, let q k+1 = q - , and adjust the random step length adjustment coefficient β downward; otherwise, q k+1 = q - k+1 , the random step length adjustment coefficient β remains unchanged; S3.18, determine q k+1 if q is reachable, go to S3.19 k+1 if q is reachable, go to S3.19 k+1 as a new node of the RRT tree structure, if q k +1 if q is not reachable, go to S3.11 to recalculate.

4. The obstacle avoidance path planning method for a vector adjustment mechanism based on the RRT principle according to claim 1, characterized in that, The closeness of the second end pose matrix T2 or the first end pose matrix T1 to the target pose matrix T is judged by the following method: calculating a maximum value of absolute values of difference values of corresponding elements of the first end pose matrix T1 and the target pose T, and denoting the maximum value as a first pose difference C1; calculating a maximum value of absolute values of difference values of corresponding elements of the second end pose matrix T2 and the target pose T, and denoting the maximum value as a second pose 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 end 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 end pose matrix T2 is closer to the target pose T.

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

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

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

8. The obstacle avoidance path planning method for a vector adjustment 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 for a vector adjustment mechanism based on the RRT principle according to claim 1, characterized in that, The method for judging whether the new node is the target is as follows: The joint angle sequence corresponding to the new node of the RRT tree-shaped storage structure is substituted into the forward kinematics motion equation to obtain a predicted end position matrix T y ; Compute the predicted end pose matrix T y The maximum value of the absolute value of the difference of each element corresponding to the target pose T is recorded as the distance d between the positive kinematic pose of the new node and the target pose T. if d is less than a threshold, it is considered that the new node is the target, otherwise, it is considered that the new node is not the target.

10. The obstacle avoidance path planning method for a vector adjustment mechanism based on the RRT principle according to claim 9, wherein, The threshold is less than or equal to 0.001.

Images