Redundant snakelike mechanical arm path planning method based on AC-RRT algorithm

Abstract

The invention relates to a redundant snakelike mechanical arm path planning method based on an AC-RRT (Angle Constraint Rapidly-Exploring Random Tree) algorithm, and provides an AC-RRT (Angle Constraint Rapidly-Exploring Random Tree) algorithm. A step length and target offset probability determination method is provided according to the obstacle distribution relation in the space, and the adaptive capacity of the algorithm is improved; a random tree extension angle constraint model is provided according to the mechanical structure of the snakelike mechanical arm, so that a planned path better conforms to the spatial motion characteristics of the mechanical arm; and finally, a target point greedy query method is introduced, the path search time is shortened, and redundant nodes are reduced. Matlab simulation is carried out, performance differences between RRT and RRT-connect algorithms are compared, V-REP software is combined with a mechanical arm model to carry out analog simulation on the algorithms, and the feasibility of the algorithms is verified.

Description

Technical Field

[0001] The invention relates to the field of redundant serpentine robot arm path planning, and in particular to a redundant serpentine robot arm path planning method based on an AC-RRT algorithm. Background Art

[0002] As robot work scenarios and tasks become more and more diverse, such as the emergence of various in-cavity maintenance operations, the robot's workspace has been compressed, which makes it difficult for traditional robots with fewer degrees of freedom to meet task requirements. [1-2] To this end, a type of multifunctional special robot based on bionics and suitable for working in narrow spaces has gradually emerged. [3-6] This type of emerging robot is gradually active in scientific research, work and other aspects. In order to adapt to special task requirements and complex working environments, the robot has been transformed into a variety of intelligent robots, expanding its application scope to the fields of medicine, military, industry, etc.

[0003] The movement mode of the snake robot arm is mainly to control the end effector to reach the target position or complete the target posture through the joint movement of multiple joints. Due to the strong plasticity of the snake robot arm, different mechanical structures can be designed under different environmental requirements. For the robot arm, path planning is one of the important steps in its motion planning. Commonly used path planning algorithms include artificial potential field method, probability map method, Dijkstra algorithm, genetic algorithm, etc. Although the above algorithms have their own characteristics, most of them are used in path planning of two-dimensional space and mobile robots; because the working scene of redundant robots is three-dimensional, when using the probability map method for path planning, even if the randomness of the algorithm has a certain search advantage, its calculation amount will increase significantly with the increase of dimension; Dijkstra algorithm and genetic algorithm have the same problem as the probability map method when facing three-dimensional space; the artificial potential field method needs to calculate different gravitational functions and repulsive functions according to the situation of the exploration space, and a large number of constraints are required to prevent the generation of local minimum points. The RRT algorithm is based on spatial random sampling and is suitable for two-dimensional and three-dimensional spaces. It is favored by Chinese and foreign scholars for its fast spatial exploration speed and few parameters.

[0004] Wang Shuo et al. studied the improvement of the rapid expansion of random tree algorithm in robot path planning, and proposed an improved RRT path planning algorithm with dynamic weight coefficient and adaptive step size, and enhanced the adaptability to the environment and path search efficiency through iterative pruning. Zhang Jiandong et al. used obstacle factors and variable step size strategies to speed up the exploration time of RRT and optimize the algorithm performance in the mobile manipulator path planning based on the improved RRT algorithm. Li Yang et al. proposed an adaptive RRT algorithm based on active and passive extension of random trees to achieve the path search purpose of adaptive step size by iterating the size of step size in the search path, which increased the algorithm's adaptability to the environment. However, more constraints need to be added in the inverse kinematics analysis to ensure the operation of the manipulator. In a multi-obstacle environment, Zhang Libin and Deng. Adaptive GB_RRT* manipulator path planning based on grid space combines the artificial potential field method and RRT algorithm to complement the advantages of the two algorithms, while avoiding the minimum point, strengthening the asymptotic optimality and guidance of the path, and combining the shortest stroke inverse solution algorithm to improve the success rate and computational efficiency of the algorithm. Xu J, Wang J. Effective motion planning of manipulator based on SDPS-RRT-Connect, by introducing a sparse dead point preservation strategy in the RRT-connect algorithm, the number of collision detections is reduced, the convergence speed of the random tree path is accelerated, and the local minimum is avoided, ensuring the completeness of the search space. Other existing technologies are based on the RRT* algorithm to optimize path planning from the aspects of extension strategy, backtracking algorithm and step size selection to increase the practicality of the algorithm. However, the progressive iteration process of RRT* requires a lot of time to calculate, and although the above-mentioned literature has made multi-angle improvements to RRT and its derivative algorithms from different angles, none of them has made improvements to the problem of difficult control of the angles between tree nodes during the random tree extension process. Too large an angle between random points will cause the computational complexity to increase exponentially and the success rate to drop significantly when performing inverse kinematics decomposition. Appropriate angle constraints are conducive to the efficient calculation of inverse kinematics analysis and the stable operation of highly redundant manipulators.

[0005] The redundancy and spatiality of the movement of the serpentine robot enable it to better adapt to narrow space working environments, and the multiple constraints brought by high redundancy need to be fully combined with path planning. In view of the problem that the RRT algorithm path cannot ensure a better result and the route search constraints are tortuous, which is not conducive to the operation of the serpentine robot, the present invention is based on the mechanical characteristics of a 12-joint 24-degree-of-freedom curled-up mobile base serpentine robot, combined with the RRT algorithm, and improved from the following three aspects: determining the maximum step length and target offset probability through environmental modeling; limiting the random point extension and constraint angle by the mechanical structure of the robot joint; adding target points and greedy algorithms in the random tree extension process. Based on the above problems, the present invention proposes an AC-RRT algorithm based on angle constraints to optimize path planning. Summary of the invention

[0006] In order to solve the above problems existing in the prior art, the object of the present invention is to provide a redundant serpentine robot arm path planning method based on the AC-RRT algorithm.

[0007] To achieve the above object, the technical solution of the present invention is:

[0008] The redundant snake robot path planning method based on AC-RRT algorithm consists of the following steps:

[0009] Step 1: Snake-shaped robotic arm structure and parameter selection:

[0010] 1) Standard DH model

[0011] The physical model of the snake-like robot arm has a disk diameter of 1000mm. The disk is connected to a joint-link structure with 1 degree of freedom and 11 joint-link structures with 2 degrees of freedom. The links are connected by universal joints. The parameters are as follows:

[0012] Parameters of the joint-link 1-2 structure

[0013]

[0014] Parameters of the joint-link 3-13 structure

[0015]

[0016] Based on the physical model parameters of the snake-like robot arm, the standard DH model of the snake-like robot arm is obtained; for the convenience of modeling, the curled-up mobile base is approximated as a joint link; and the DH model between the joints of the snake-like robot arm is established;

[0017] For the convenience of analysis, the DH model is idealized and the universal joint with 2 degrees of freedom is regarded as two intersecting and perpendicular rotation axes: the rotation axes with a distance of 0 in the X-axis direction and an angle of 90° along the Z-axis form the direction of the Y-axis, which can be determined by the right-hand rule; the joint change matrix T of the standard DH model is obtained i :

[0018]

[0019] Among them, Rot(Z i-1 ,θ i ) represents the winding Z i-1 Axis rotation angle θ i 、Trans(Z i-1 ,d i ) indicates that along Z i-1 Axis movement d i 、Trans(X i ,a i ) indicates that along X i Axis movement ai, Rot(X i ,α i ) indicates winding around X i Axis rotation angle α i ;

[0020] Get the single-joint homogeneous transformation matrix:

[0021]

[0022] The forward kinematic model based on the standard DH model is:

[0023]

[0024] The angle change of each joint of the snake robot is mapped to the end effector through equation (3); 2) The working space of the snake robot

[0025] According to the structural characteristics of the snake-like robot, the rotation angle range of each joint is mainly affected by the physical model. To further evaluate the working performance of the robot, the Monte Carlo method is used to obtain the working space of the robot based on formula (3). The sampling times are 4000 times, and the working space of the curled-up snake-like robot is obtained. At the same time, the working space of the advancing snake-like robot with the same degree of freedom and size is compared.

[0026] Step 2: AC-RRT algorithm

[0027] 1) Target bias probability and step size design

[0028] The probability target bias function R and step length T are introduced, random points and target points are set as the extension direction of the random tree, the target bias probability is introduced, and the adaptive determination method of the target bias probability and step length of the working scene is combined. According to the relationship between obstacles and step length in the three-dimensional scene, the distance between obstacles and the distance between obstacles and the starting point and the end point is analyzed, and finally the step length after adaptive selection is obtained; the formula for determining the step length is as follows:

[0029]

[0030] in is the distance between obstacle boundaries in the scene, is the distance between the boundary of each obstacle in the scene and the target point, is the distance between each obstacle in the scene and the starting point;

[0031] In order to facilitate spatial modeling and calculation, the obstacle in the present invention adopts a ball collision model; thus,

[0032]

[0033] Among them, i is the center of obstacle i in the ball collision model, r i is the radius of obstacle i, P s and P g are the starting point and the end point respectively;

[0034] When the total volume of obstacles in the space increases, the target bias probability must be reduced to fully explore the space. The size of the target bias probability and the size of the step size are related to the ratio of the obstacle volume to the space volume, as shown in formula (6):

[0035]

[0036] Where V is the volume of the exploration space, V O is the volume of the obstacle in space;

[0037] There is a certain coupling relationship between the target bias probability and the step size. In most cases, the size of the target bias probability is positively correlated with the step size, but the degree of change of the two is not the same. The coupling relationship between the two is expressed by formula (7):

[0038]

[0039] Where λ is the weight, the ratio of step size, target bias probability and the ratio of exploration space and obstacle volume are exponentially distributed, and λ is mainly determined by the complexity of space and obstacle distribution; pseudo code is shown in Algorithm 1:

[0040]

[0041] Where p is the ratio of the explored space volume to the total obstacle volume;

[0042] 2) Joint angle constraints

[0043] In order to further adapt the mechanical structure of the snake-like robot, an angle constraint model is added to the random tree extension during the path exploration process to limit the angle between the nodes as much as possible, so as to avoid the snake-like robot being unable to complete the inverse kinematics analysis of the path during the movement. The main idea is: the random point P and the nearest tree node A form a space vector Tree node A and its parent node B form a space vector judge and Whether the angle between them meets the angle constraint is as shown in formula (8):

[0044]

[0045] Where θ is the angle constraint of the snake-like robot joint, which is determined by the mechanical structure of the joint;

[0046] like Figure 5 As shown, when the random point P2 and its nearest tree node A form a vector With vector When the angle is greater than the specified angle constraint, the random point P2 is abandoned and a new random point is selected; when the random point P1 falls within the conical area with the nearest node A as the vertex in the graph, this node is selected as the extension node of the random tree for subsequent collision judgment; the pseudo code is shown in Algorithm 2:

[0047]

[0048] 3) Greedy Algorithm

[0049] The target bias probability characteristics of the random tree are further optimized. A greedy algorithm is added after each random point is included in the random tree. That is, by judging whether the target point can be directly connected to the new random tree node, the exploration time is shortened and the oscillation phenomenon that occurs when the random tree grows near the target point is avoided. At the same time, an adaptive step size algorithm is added. The step size obtained by Algorithm 1 is used as the maximum step size. When the random point touches an obstacle, the known vector is proportionally reduced according to Formula (9) to maximize the use of the spatial value of iterative exploration. Where L is the longest boundary and i is the number of adaptive times.

[0050]

[0051] The circular shadow part is an obstacle. When the tree node expands the random points to A, B, and C, in order to avoid redundant nodes similar to path 1 and path 2, the latest tree node and the target point P are judged before each iteration. g Are there any obstacles between A, B and P? g There is an obstacle between C and P g If there is no obstacle between them, connect C and P directly. g , complete the exploration of the entire path, reduce redundant nodes and shorten the exploration time and number of iterations.

[0052] Compared with the prior art, the beneficial effects of the present invention are:

[0053] The redundant serpentine robot path planning method based on AC-RRT algorithm takes the 12-joint 24-DOF serpentine robot as the analysis object, and proposes an improved path planning method based on RRT algorithm, namely AC-RRT algorithm. It improves the problem of parameter determination and the randomness of angles between random tree nodes in the traditional RRT algorithm, and has the following advantages:

[0054] 1. Based on the coupling relationship between step size and target bias probability, a method for determining step size and target bias probability is proposed to improve the algorithm's adaptability.

[0055] 2. Aiming at the randomness problem of the angle between random tree nodes, an angle constraint model is proposed. By judging the angle between the connection point between the random point and the random tree during the extension process, the random point is selectively discarded to reduce the redundancy of the random tree.

[0056] 3. In order to reduce the large number of redundant points in the algorithm's planning path, a target point greedy query algorithm is added in each random tree iteration process to shorten the random tree path planning time.

[0057] Matlab simulation was used to compare the performance differences in three-dimensional space path exploration between the RRT algorithm, RRT-connect algorithm, and AC-RRT algorithm. It was verified that the AC-RRT algorithm has a faster path search time, a lower path cost, and fewer iterations. Finally, the physical model of the snake robot and the narrow space were established by V-REP software for model simulation verification. The results show that the AC-RRT algorithm has the path constraint capability that conforms to the motion characteristics of the robot, which is more conducive to the operation of the snake robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 Snake-like robotic arm model;

[0059] Figure 2 Snake-shaped robotic arm DH model;

[0060] Figure 3The working space of snake-like manipulators with different configurations, among which, Figure 3 (a) The workspace of the curled-up snake-like robot arm; Figure 3 (b) The working space of the forward-moving snake-like robot arm;

[0061] Figure 4 AC-RRT improvement method diagram;

[0062] Figure 5 Angle constraint diagram;

[0063] Figure 6 Greedy algorithm angle constraint graph;

[0064] Figure 7 AC-RRT algorithm flow chart;

[0065] Figure 8 Environmental modeling;

[0066] Fig. 9 The operation effect diagram of three path planning schemes in three-dimensional space, among which,

[0067] Fig. 9 (a) Original RRT path planning algorithm;

[0068] Fig. 9 (b) RRT-connect path planning algorithm;

[0069] Fig. 9 (c) AC-RRT path planning algorithm;

[0070] Fig.10 The initial state of the narrow space and the snake-like robot arm;

[0071] Fig.11 The motion process with Target1 as the target, where

[0072] Fig.11 (a) Operation process; Fig.11 (b) End of operation;

[0073] Fig.12 The motion process with Target2 as the target, where

[0074] Fig.12 (a) Operation process Fig.12 (b) End of the Movement

[0075] Fig.13 The joints of the robot arm with Target1 as the target point change, among which,

[0076] Fig.13 (a) Joint changes in joints 1-6;

[0077] Fig.13 (b) Joint changes in joints 7-12;

[0078] Fig.13 (c) joint changes in joints 13-18;

[0079] Fig.13 (d) Joint changes in joints 19-24. DETAILED DESCRIPTION

[0080] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0081] Experimental example:

[0082] like Figure 1-13 As shown in the figure, the redundant snake robot arm path planning method based on the AC-RRT algorithm is as follows:

[0083] 1Snake-like robotic arm

[0084] 1.1 Standard DH model

[0085] The physical model of the snake-like mechanical arm of the present invention is as follows: Figure 1 As shown in Figure 1, the disk has a diameter of 1000 mm, and is connected to the disk by a joint-link structure with 1 degree of freedom and 11 joint-link structures with 2 degrees of freedom. The links are connected by universal joints. The parameters are shown in Tables 1 and 2.

[0086] Table 1 Main parameters of joint-link 1-2 structure

[0087]

[0088] Table 2 Main parameters of joint-link 3-13 structure

[0089]

[0090] Based on the physical model parameters of the snake-like manipulator in Table 1 and Table 2, the standard DH model of the snake-like manipulator can be obtained. For the convenience of modeling, the curled-up mobile base is approximated as a joint link. The DH model between the joints of the snake-like manipulator is as follows: Figure 2 shown.

[0091] To facilitate analysis, Figure 2 An idealized model is processed, and the universal joint with 2 degrees of freedom is regarded as two intersecting and perpendicular rotation axes (the rotation axes with a distance of 0 in the X-axis direction and an angle of 90° along the Z-axis form the direction of the Y-axis, which can be determined by the right-hand rule). The change matrix T of the joint based on the standard DH model is i for:

[0092]

[0093] Among them, Rot(Z i-1 ,θ i ) represents the winding Z i-1 Axis rotation angle θ i 、Trans(Z i-1 ,d i ) indicates that along Z i-1 Axis movement d i 、Trans(X i ,a i ) indicates that along X i Axis movement a i 、Rot(X i ,α i ) indicates winding around X i Axis rotation angle α i .

[0094] Get the single-joint homogeneous transformation matrix:

[0095]

[0096] The forward kinematic model based on the standard DH model is:

[0097]

[0098] Formula (3) can be used to map the angle change of each joint of the snake robot to the end effector, and it is also one of the important bases for calculating the change angle of each joint by analyzing the trajectory of the end effector.

[0099] 1.2 Working space of the snake-like robot

[0100] According to the structural characteristics of the snake-like robot, the rotation angle range of each joint is mainly affected by the physical model. To further evaluate the working performance of the robot, the Monte Carlo method is used to obtain the working space of the robot based on formula (3). The sampling times are 4000 times, and the working space of the curled-up snake-like robot is obtained. At the same time, the working space of the advancing snake-like robot with the same degree of freedom and size is compared. Figure 3 shown.

[0101] It can be seen that the curled-up serpentine robotic arm has stronger lateral extension and exploration capabilities and a larger exploration space than the forward-moving serpentine robotic arm. The movement mode of the curled-up serpentine robotic arm ensures that it occupies a smaller area while expanding the movable space.

[0102] 2 Path Planning Algorithm

[0103] 2.1 RRT and RRT-connect algorithm

[0104] The RRT algorithm has the advantages of fast search speed, strong spatial search capability, few parameters, simple algorithm, good inclusiveness and easy combination with other algorithms. It is suitable for solving complex problems in three-dimensional space and with multiple constraints.

[0105] The RRT-connect algorithm introduces a greedy strategy and uses a bidirectional random tree. The end random tree grows greedily with the current end node of the start random tree as the target to achieve the purpose of bidirectional search and speed up the path search planning process.

[0106] RRT* obtains a random tree through the RRT or RRT-connect algorithm and continuously selects the parent node by backtracking from the end point, and finally completes the optimal convergence of the planned path. It and RRT-connect are different branches of the RRT improvement method. They represent two different improvement ideas: shortest time and optimal path.

[0107] Although the randomness and efficient scalability of the RRT and RRT-connect algorithms bring the ability of rapid spatial exploration, their randomness also leads to the need for a large number of random point iterations in a simple exploration space, resulting in node redundancy and waste of computing resources. At the same time, the path obtained by the algorithm will be difficult to perform inverse kinematics analysis because the path is too tortuous. Although RRT* can obtain an optimal path, it requires a large number of iterative calculations for the optimal selection of the parent node, and the backtracking premise for RRT* to obtain the optimal path is that the random tree has completed the search of the entire space, which requires a lot of time and computing resources. For this reason, the present invention will improve and optimize RRT in terms of step size selection and random point screening while ensuring the shortest time.

[0108] 2.2AC-RRT Algorithm

[0109] The present invention adds a method for determining the target bias probability and step length according to the environment adaptively on the basis of ensuring the shortest running time, adds angle constraints according to the mechanical structure of the serpentine robot arm, and adds a greedy algorithm based on the optimization strategy to obtain a planning path that conforms to the movement of the serpentine arm. The specific implementation is as follows: Figure 4 As shown in the figure, the optimization scheme is simulated using Matlab.

[0110] 2.2.1 Target bias probability and step size design

[0111] The probabilistic target bias function R and step size T are introduced, and the random points and target points are set as the extension direction of the random tree. The introduction of the target bias probability can further constrain the search range of the random tree, and the guided selection of the extension direction not only speeds up the search speed of the available paths, but also increases the asymptotic optimality and feasibility of the random tree. For the step size, different target scenarios have different requirements. If the step size is too small, the random tree space exploration will be slow, the node redundancy will increase, and resources will be wasted; if the step size is too large, local singular points will be generated, resulting in the failure of space exploration. The traditional RRT algorithm requires manual determination of the step size and target bias probability, which requires additional experiments or the operator's experience judgment, which increases the workload and experimental difficulty.

[0112] Since the optimal values ​​of the target bias probability and the step length are related to the working scene, and there is a certain coupling relationship between the two, the present invention proposes an adaptive determination method of the target bias probability and the step length in combination with the working scene, and finally obtains the adaptively selected step length by analyzing the distance between obstacles and the distance between obstacles and the starting point and the end point in view of the relationship between obstacles and the step length in the three-dimensional scene. The formula for determining the step length is as follows:

[0113]

[0114] in is the distance between obstacle boundaries in the scene, is the distance between the boundary of each obstacle in the scene and the target point, is the distance between each obstacle in the scene and the starting point.

[0115] In order to facilitate spatial modeling and calculation, the obstacle in the present invention adopts a ball collision model.

[0116]

[0117] Among them, i is the center of obstacle i in the ball collision model, r i is the radius of obstacle i, P s and P g The starting point and the end point respectively.

[0118] When the total volume of obstacles in the space increases, the target bias probability must be reduced to fully explore the space. The size of the target bias probability and the size of the step size are related to the ratio of the obstacle volume to the space volume, as shown in formula (6):

[0119]

[0120] Where V is the volume of the exploration space, V Ois the volume of the obstacle in space.

[0121] There is a certain coupling relationship between the target bias probability and the step size. In most cases, the size of the target bias probability is positively correlated with the step size, but the degree of change of the two is not the same. The coupling relationship between the two is expressed by formula (7):

[0122]

[0123] Where λ is the weight, the ratio of step size, target bias probability and the ratio of exploration space and obstacle volume are exponentially distributed, and λ is mainly determined by the complexity of space and obstacle distribution. Pseudo code is shown in Algorithm 1:

[0124]

[0125] Where p is the ratio of the explored space volume to the total obstacle volume.

[0126] 2.2.2 Joint Angle Constraints

[0127] In order to further adapt the mechanical structure of the snake-like robot, an angle constraint model is added to the random tree extension during the path exploration process to limit the angle between nodes as much as possible, so as to avoid the snake-like robot being unable to complete the inverse kinematics analysis of the path during movement. The main idea is: the random point P and the nearest tree node A form a space vector Tree node A and its parent node B form a space vector judge and Whether the angle between them meets the angle constraint is as shown in formula (8):

[0128]

[0129] Where θ is the angle constraint of the snake robot joint, which is determined by the mechanical structure of the joint.

[0130] like Figure 5 As shown, when the random point P2 and its nearest tree node A form a vector With vector When the angle is greater than the specified angle constraint, the random point P2 is abandoned and a new random point selection is performed. When the random point P1 falls within the conical area with the nearest node A as the vertex in the graph, this node is selected as the expansion node of the random tree for subsequent collision judgment. See Algorithm 2 for pseudo code:

[0131]

[0132]

[0133] 2.2.3 Greedy Algorithm

[0134] The target bias probability characteristics of the random tree are further optimized. A greedy algorithm is added after each random point is included in the random tree. That is, by judging whether the target point can be directly connected to the new random tree node, the exploration time is shortened and the oscillation phenomenon that occurs when the random tree grows near the target point is avoided. At the same time, an adaptive step size algorithm is added, and the step size obtained by Algorithm 1 is used as the maximum step size. When the random point touches an obstacle, the known vector is proportionally reduced according to Formula (9) to maximize the use of the spatial value of iterative exploration. Where L is the longest boundary and i is the number of adaptive times.

[0135]

[0136] like Figure 6 As shown, the circular shadow part is an obstacle. When the tree node expands the random points to A, B, and C, in order to avoid redundant nodes similar to path 1 and path 2, the latest tree node and the target point P are judged before each iteration. g Is there an obstacle between them? As shown in the figure, points A, B, and C are g There is an obstacle between C and P g If there is no obstacle between them, connect C and P directly. g , complete the exploration of the entire path, reduce redundant nodes and shorten the exploration time and number of iterations.

[0137] 2.3 Algorithm comparison and simulation

[0138] In order to explore the performance of the algorithm as comprehensively as possible, improvements in target bias probability and step size, joint angle constraints, and target point greedy query are introduced into three-dimensional space for comparative verification of different algorithms. The computer configuration is: 64-bit Windows 11 Professional Edition, 12th generation Intel i7-12700H processor. The specific random tree expansion flow chart is as follows Figure 7 shown.

[0139] like Figure 8 The exploration space is shown in the figure. The size is 100*100*100. The obstacles are 9 spherical obstacles with a radius of 20 evenly distributed in the space. The random point growth angle of the random tree during the exploration process is constrained to 45°.

[0140] After calculation, according to the maximum step size of 16 and the target bias probability of 0.02, the same step size and target bias probability coupling model were used to compare the RRT, RRT-connect, and AC-RRT algorithms in three-dimensional space on Matlab. The simulation results are as follows Fig. 9 As shown in (a), (b) and (c).

[0141] In Figures (a) and (c), the red line is a random expansion tree, and the black line is the shortest path. In Figure (b), the red line and blue line are bidirectional random expansion trees, and the black line is the shortest path. From the path trajectory, the paths of RRT and RRT-connect are relatively tortuous, while the addition of angle constraints and greedy query of target points in AC-RRT greatly improves the tortuosity of the path and maximizes the use of iteration point resources. The three path planning indicators are shown in Table 3.

[0142] Table 3 Average indicators of three path planning algorithms running 1000 times

[0143]

[0144] Compared with Table 3, the step-size-target bias probability coupling model does not directly lead to a decrease in the success rate of path planning. The average number of iterations of the AC-RRT algorithm when planning a path in three-dimensional space is much smaller than that of the RRT algorithm and the RRT-connect algorithm, the average number of spatial exploration nodes is much smaller than that of the other two algorithms, and the average path cost is optimized by 11.67% and 19.18% respectively compared with the other two algorithms. The average time consumption is shortened by 32.47% and 14.05% respectively compared with the other two algorithms, and the average shortest path contains nodes that are reduced by 38.95% and 40.68% respectively compared with the other two algorithms. In the performance comparison with the RRT algorithm and the RRT-connect algorithm, it can be seen that AC-RRT has improved the path exploration time, path cost, number of iterations and iteration nodes to varying degrees while ensuring the angle between the nodes of the constrained random tree.

[0145] 3 Path Planning Simulation Experiment

[0146] A model of the snake-like robotic arm was established in the V-REP software, and the movement of the snake-like robotic arm was controlled by an embedded script file, and simulation experiments on the actual model were carried out.

[0147] 3.1 Narrow Space Modeling

[0148] The size of the narrow space is 3m*1m*2m. Add Target1 and Target2 as target points for path planning. Target points Target1 and Target2 can detect the effectiveness of the obstacle avoidance strategy from two different extension angles, upward and downward. The detection scene and the initial state of the snake robot are as follows: Fig.10 shown.

[0149] 3.2 Simulation Verification

[0150] In order to better observe the changes in the joints of the snake-like robot arm during movement, the movement speed of the end effector of the snake-like robot arm is set to 0.1m / s. There are two target points set in this simulation, at different locations in the same narrow environment, to verify the exploration ability of the snake-like robot arm in narrow spaces. The snake-like robot arm is planned as follows in different experiments: Fig.11 As shown, from the initial state to "Target1" and Fig.12 As shown, from the initial state to "Target2".

[0151] The trajectory of the robot arm moving toward the two target points is relatively smooth, and the joint angles of the snake robot arm do not change suddenly during the movement. The bending degree of the snake robot arm itself is shared by each joint of the snake robot arm, which greatly enhances the stability of the snake robot arm during operation. It can be seen that the snake robot arm has extremely high joint flexibility. With the AC-RRT path planning algorithm, it can efficiently complete narrow space tasks. The joint angle changes from joint 1 (j1) to joint 24 (j24) during the movement with Target1 as the target point are shown in the figure below. Fig.13 shown.

[0152] Depend on Fig.13 It can be seen that the planned route is smooth, and there is no obvious oscillation in the joints during the movement of the snake-like robot arm. At the same time, each joint does not exceed the constraint angle during the movement, which verifies the feasibility of the AC-RRT algorithm to plan the path.

[0153] This paper takes a 12-joint 24-DOF snake-like robot as the analysis object and proposes an improved path planning method based on the RRT algorithm, the AC-RRT algorithm. It improves the problem of parameter determination in the traditional RRT algorithm and the problem of random angles between random tree nodes. The main contents include:

[0154] 1. Based on the coupling relationship between step size and target bias probability, a method for determining step size and target bias probability is proposed to improve the algorithm's adaptability.

[0155] 2. Aiming at the randomness problem of the angle between random tree nodes, an angle constraint model is proposed. By judging the angle between the connection point between the random point and the random tree during the extension process, the random point is selectively discarded to reduce the redundancy of the random tree.

[0156] 3. In order to reduce the large number of redundant points in the algorithm's planning path, a target point greedy query algorithm is added in each random tree iteration process to shorten the random tree path planning time.

[0157] Matlab simulation was used to compare the performance differences in three-dimensional space path exploration between the RRT algorithm, RRT-connect algorithm, and AC-RRT algorithm. It was verified that the AC-RRT algorithm has a faster path search time, a lower path cost, and fewer iterations. Finally, the physical model of the snake robot and the narrow space were established by V-REP software for model simulation verification. The results show that the AC-RRT algorithm has the path constraint capability that conforms to the motion characteristics of the robot, which is more conducive to the operation of the snake robot.

[0158] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that are not conceived through creative work should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope defined in the claims.

Claims

1. The redundant snake robot path planning method based on AC-RRT algorithm consists of the following steps: Step 1: Snake-shaped robotic arm structure and parameter selection: 1) Standard DH model The physical model of the snake-like robot arm has a disk diameter of 1000mm. The disk is connected to a joint-link structure with 1 degree of freedom and 11 joint-link structures with 2 degrees of freedom. The links are connected by universal joints. The parameters are as follows: Parameters of the joint-link 1-2 structure Parameters of the joint-link 3-13 structure Based on the physical model parameters of the snake-like robot arm, the standard DH model of the snake-like robot arm is obtained; for the convenience of modeling, the curled-up mobile base is approximated as a joint link; and the DH model between the joints of the snake-like robot arm is established; For the convenience of analysis, the DH model is idealized and the universal joint with 2 degrees of freedom is regarded as two intersecting and perpendicular rotation axes: the rotation axes with a distance of 0 in the X-axis direction and an angle of 90° along the Z-axis form the direction of the Y-axis, which can be determined by the right-hand rule; the joint change matrix T of the standard DH model is obtained i : Among them, Rot(Z i-1 ,θ i ) represents the winding Z i-1 Axis rotation angle θ i 、Trans(Z i-1 ,d i ) indicates that along Z i-1 Axis movement d i 、Trans(X i ,a i ) indicates that along X i Axis movement a i 、Rot(X i ,α i ) indicates winding around X i Axis rotation angle α i ; Get the single-joint homogeneous transformation matrix: The forward kinematic model based on the standard DH model is: The angle change of each joint of the snake robot is mapped to the end effector through equation (3); 2) The working space of the snake robot; According to the structural characteristics of the snake-like robot, the rotation angle range of each joint is mainly affected by the physical model. To further evaluate the working performance of the robot, the Monte Carlo method is used to obtain the working space of the robot based on formula (3). The sampling times are 4000 times, and the working space of the curled-up snake-like robot is obtained. At the same time, the working space of the advancing snake-like robot with the same degree of freedom and size is compared. Step 2: AC-RRT algorithm 1) Target bias probability and step size design The probability target bias function R and step length T are introduced, random points and target points are set as the extension direction of the random tree, the target bias probability is introduced, and the adaptive determination method of the target bias probability and step length of the working scene is combined. According to the relationship between obstacles and step length in the three-dimensional scene, the distance between obstacles and the distance between obstacles and the starting point and the end point is analyzed, and finally the step length after adaptive selection is obtained; the formula for determining the step length is as follows: in is the distance between obstacle boundaries in the scene, is the distance between the boundary of each obstacle in the scene and the target point, is the distance between each obstacle in the scene and the starting point; In order to facilitate spatial modeling and calculation, the obstacle in the present invention adopts a ball collision model; thus, Among them, i is the center of obstacle i in the ball collision model, r i is the radius of obstacle i, P s and P g are the starting point and the end point respectively; When the total volume of obstacles in the space increases, the target bias probability must be reduced to fully explore the space. The size of the target bias probability and the size of the step size are related to the ratio of the obstacle volume to the space volume, as shown in formula (6): Where V is the volume of the exploration space, V O is the volume of the obstacle in space; There is a certain coupling relationship between the target bias probability and the step size. In most cases, the size of the target bias probability is positively correlated with the step size, but the degree of change of the two is not the same. The coupling relationship between the two is expressed by formula (7): Where λ is the weight, the ratio of step size, target bias probability and the ratio of exploration space and obstacle volume are exponentially distributed, and λ is mainly determined by the complexity of space and obstacle distribution; pseudo code is shown in Algorithm 1: Where p is the ratio of the explored space volume to the total obstacle volume; 2) Joint angle constraints In order to further adapt the mechanical structure of the snake-like robot, an angle constraint model is added to the random tree extension during the path exploration process to limit the angle between the nodes as much as possible, so as to avoid the snake-like robot being unable to complete the inverse kinematics analysis of the path during the movement. The main idea is: the random point P and the nearest tree node A form a space vector Tree node A and its parent node B form a space vector judge and Whether the angle between them meets the angle constraint is as shown in formula (8): Where θ is the angle constraint of the snake-like robot joint, which is determined by the mechanical structure of the joint; As shown in Figure 5, when the random point P2 and its nearest tree node A form a vector With vector When the angle is greater than the specified angle constraint, the random point P2 is abandoned and a new random point is selected; when the random point P1 falls within the conical area with the nearest node A as the vertex in the graph, this node is selected as the extension node of the random tree for subsequent collision judgment; the pseudo code is shown in Algorithm 2: 3) Greedy Algorithm The target bias probability characteristics of the random tree are further optimized. A greedy algorithm is added after each random point is included in the random tree. That is, by judging whether the target point can be directly connected to the new random tree node, the exploration time is shortened and the oscillation phenomenon that occurs when the random tree grows near the target point is avoided. At the same time, an adaptive step size algorithm is added, with the step size obtained by Algorithm 1 as the maximum step size. When the random point touches an obstacle, the known vector is proportionally reduced according to Formula (9) to maximize the spatial value of iterative exploration. Where L is the longest boundary, i is the number of adaptive times; The circular shadow part is an obstacle. When the tree node expands the random points to A, B, and C, in order to avoid redundant nodes similar to path 1 and path 2, the latest tree node and the target point P are judged before each iteration. g Are there any obstacles between A, B and P? g There is an obstacle between C and P g If there is no obstacle between them, connect C and P directly. g , complete the exploration of the entire path, reduce redundant nodes and shorten the exploration time and number of iterations.

Images