Research on obstacle avoidance path planning of spatial manipulator based on improved RRT-Connect

Yi CAO, Ya-bin ZHANG， Yi ZHOU， Xiang-wei BAO， Lei LI, Jing-tao ZHANG, Lin-feng ZHANG

(1College of Electrical Engineering, Henan University of Technology, Zhengzhou 450000, China)(2Yonyou Seentao Technology Co.,Ltd., Beijing 100000, China)

Abstract: An improved RRT-Connect algorithm, which synthesizes the merit of Dijkstra policy, is proposed to boost its prior one in view of the disadvantages of node extension inef?ciency, poor safety and multiple invalid path points in the obstacle-avoidance path planning of spatial manipulators. Firstly, the path search efficiency and safety of the traditional RRT-Connect algorithm are improved by the presenting of dual trees to extend the target change strategy, extreme greed strategy and new collision detection method. Secondly, the optimization feature of the Dijkstra algorithm is utilized to eliminate redundant invalid nodes in the planned path to achieve the effects of high search speed, short path length and high security in path planning. Finally, algorithmic simulation experiments were performed through the MATLAB Robotics Toolbox, which showed the effectiveness and feasibility of this method.

Key words: RRT-Connect， Path planning， Obstacle avoidance， Dijkstra

1 Introduction

Nowadays, with the rapid development of artificial intelligence technology, it has also provided new impetus for the development of robots. Multi-degree-of-freedom space manipulator, as a typical representative industrial robot, have been widely used in aerospace, logistics, catering, medical services, industrial production and other fields. However, due to the complexity of manipulator task space environment, the research on collision-free path planning is becoming more and more urgent. The purpose of path planning is to find a continuous, safe and non-collision path from the initial position of the manipulator to the position of the target in the task space, as well as to meet certain optimization indexes as far as possible in the search process, such as the shortest path, the least time or the optimal energy [1-6].

Many scholars had carried out researches recently. The RRT algorithm was formally proposed by Lavalle in 1998 [8], which has the characteristics of probabilistic completeness, high success rate of path search, a long time, large randomness of path, and different length of the path. Dijkstra algorithm was proposed by Dutch computer scientist Dijkstra in 1959. It is a single-source shortest path algorithm, which is used to calculate the shortest path from one node to all other nodes. Many scholars have improved these two algorithms. Wang et al. [9] proposed that the GB_RRT algorithm uses gaussian sampling to reduce the blindness of the classical algorithm. Chen et al. [10] proposed a hybrid algorithm of low oscillation artificial potential field and Adaptive Rapidly Randomly Exploring Tree (ARRT) which is used to escape the robot arm from the local minimum. Liu et al. [11] proposed combining Dijkstra algorithm with particle swarm optimization to solve the problem of local planning path length of a mobile robot.

To sum up, the previous obstacle model is simple, however, it is difficult to recognize the characteristics of the obstacle model entirely in the actual working environment, and the safety performance of the obstacle avoidance path is ignored in the planning process. In this paper, the Dobot robot arm with three degrees of freedom is taken as the research object to explore the path in Cartesian space. A cuboid envelope model is used to describe obstacles. In the cluster obstacle environment, multiple cuboids can also be selected for approximate superposition. In order to simplify the collision detection model, a cylinder is adopted to describe the link of the robot arm. Taking advantage of the fast search speed of RRT-Connect algorithm, the greedy strategy is introduced to expand to the new target point with a certain probability each time, and the relative target point is updated in the process of alternating two-tree expansion, which can not only ensure the safety of path search, but also speed up the speed of path search [14-15]. Then the Dijkstra algorithm is used for secondary optimization to remove redundant path points and achieve the goal of reducing the path length. The combination of the two algorithms can optimize the path quality.

2 Kinematic model of manipulator

Dobot is a typical publicized robot, which is mainly composed of a base, upper arm, lower arm and hand. The robot is a 3-DOF open-chain manipulator with resolute joints. In order to describe the spatial position of the end-effector of the manipulator, it is necessary to determine the relative relationship of the links.

In this paper, the D-H(Denavit-Hartenberg) method is applied to describe the topology of the manipulator by D-H parameters. The coordinate system of each link is established, and the D-H parameters are shown in Table 1. The three-dimensional model of the manipulator is established through the MATLAB Robot Toolbox, as shown in Fig.1.

Fig.1 3D model of spatial manipulator

Table 1 D-H parameters of spatial manipulator

2.1 Forward kinematic equations of the manipulator

The D-H method is used to establish each link coordinate system. The homogeneous transformation of the link coordinates system {i} relative to {i-1} is a link transformation. Link transformation has four fixed parameters, in turn, called connecting rod coupler-angleai-1, connecting rod lengthαi-1, connecting rod deflection distancedi, and joints angleθi. In each joint of the robot, four parameters of the DH parameters represent different characteristics of the joint connecting rod or a different transformation. At some point, from the {i-1} joint to {i} joint, it will traverse a series of transformations: theZaxis rotation (θi),Zaxis shift (di), theXaxis translation (ai-1),Xaxis rotation (αi-1). The following Eq1 can be obtained by connecting the transformation matrixTand DH parameters.

DX(ai-1)×RX(αi-1)

(1)

Through expansion, the general formula of variation between adjacent connecting rods is obtained:

(2)

According to Eq (2), the homogeneous transformation matrix of each link of the manipulator arm can be calculated and the homogeneous transformation matrix of Dobot end-effector relative to the base coordinate {0} can be obtained by multiplying in turn. The forward kinematic equation of the manipulator is obtained as follows.

(3)

wheresstands for sine,s1=sinθ1,cfor cosine,c1=cosθ1,c23=cos(θ2+θ3), and so on.

2.2 Inverse kinematics equations of the manipulator

The inverse kinematics problem of the manipulator is that the position of the tool coordinate system of the manipulator end actuator is known relative to the coordinate system of the base, and the joint angle is calculated by the inverse position. In this paper, the inverse kinematics equation of Dobot robot topology is solved by the mathematical geometry method. The top view and side view of the operating arm are shown in the Fig.2.

Fig.2 Schematic diagram of top and side view of the manipulator

In Fig.2, the coordinate of the C-terminal actuator in space is composed ofα,βandγangle. The inverse kinematics solution is to find the joint angle when the end actuator position is known. Given that the position of terminal endpointCis (xc,yc,zc), the following results can be obtained.

(4)

From Eq (4), the deflection angle of the base γ can be obtained, that is, joint angleθ1; Wherek1is the distance from the terminal end point to joint A.

With the knowledge of trigonometric function, the angle between the straight line connecting the end point and joint A and the boom can be obtained, as shown in Eq (5).

(5)

From Eq (5), A joint angle α, that is, joint angleθ2; In the same way, it can be obtained.

(6)

From Eq (6), B joint angle β that is, joint angleθ3.

3 Improved algorithm design

3.1 Establish collision detection model

The collision detection model is a description model which that is established when the robot and obstacle collide. The practical application environment of manipulator is complex. It has the advantage of a simple description to describe the barriers with the ball covering method. However, it is difficult to describe the complex obstacle model. Using the spherical envelope will lose the obstacle avoidance workspace and reduce the path search speed. Therefore, this paper uses the cuboid envelope method to describe the obstacle model. Based on the principle of the integral method, many cuboids are added to approach the complex obstacle scene in the real environment. The robot arm connecting rod is simplified by using a uniform cylinder, which can simplify the collision detection model and reduce the collision detection time. The two-dimensional plane simplified model is shown in Fig.3.

Fig.3 Model simplification diagram

The spatial coordinates of the two joints can be calculated by the forward kinematics equation of the manipulator, and the coordinates of any point on the connecting line can be obtained by using the knowledge of spatial geometry. This line is the simplified mathematical model of the manipulator link. The side length direction of the envelope model of the space obstacle box must be parallel to the coordinate axis. Such a mathematical box model can be formulated as.

V={x1≤x≤x2，y1≤y≤y2,z1≤z≤z2}

(7)

To detect whether the connecting rod collides within an obstacle is to determine whether the discrete points of the simplified segment of the connecting rod of the manipulator are in the box envelope model after expansion. If it is in this range,

it indicates that the connecting rod collides with the obstacle, and the path point will be discarded to continue to find a new path point. Otherwise, the waypoint is considered safe and added to the tree.

3.2 RRT-Connect algorithm

RRT (rapidly exploring random tree) based path planning algorithm avoids spatial modeling in the process of collision detection of state-space sampling points, and effectively solves the path planning problems of high-dimensional space and complex constraints. The characteristic of this method is that it can search the high-dimensional space quickly and effectively, and guide the search to the blank area through the random sampling points of the state space, so as to find a planning path from the starting point to the target point. Merit is suitable for solving the path planning of multi-degree-of-freedom robots in the complex environment and the dynamic environment. Similar to PRM, the method is probabilistically complete and not optimal. In order to improve the search speed of this algorithm, Kuffner and Lavalle proposed RRT-Connect algorithm[14]. Based on the basic algorithm, the start point and the target point were selected as the root nodes of the two initial trees. In each search process, each tree will be alternately extended to the corresponding target point, and the pseudo code of the algorithm is as follows.

RRT-Connect algorithm

1 Input(Xstart,Xgoal,stepsize)

2 Init(Ta={ Xstart}, Tb={ Xgoal },obstacle)

3 While((0 to N) OR Flag)

Xrand=Rand()

Xnear=Nearest(Xrand,Ta)

Xnew =NewExtend(Xnear,Xrand,stepsize)

If ～EdgeCollision(Xnew,Xnear)

Ta={Ta， Xnew}

Else

Continue

End

If Connect(Ta, Tb)

Flag=false

Return(Path)

Break

Else

Swap(Ta, Tb)

Continue

end

End

4 Output(Path)

3.3 Improved RRT-Connect algorithm

RRT connect algorithm increases the number of trees, introduces a greedy strategy to control the tree to expand to the target point with a certain probability, and improves the efficiency of the path search. Based on the innovative algorithm, IM-RRT-Connect is proposed to change the extended target point, replace the original algorithm with expanding to the original root node, and select the shortest Euclidean distance between the node and the two trees to speed up the connection path. To increase the expansion rate and reduce the number of nodes, the extreme greed strategy is adopted. If there is no obstacle in the expansion process to the target point, continue to expand in this direction until a collision occurs or the tree is connected to stop the expansion. Therefore, the third step of the traditional algorithm is modified as follows.

IM-RRT-Connect algorithm

3 While((0 to N) OR Flag)

If(prand>p)

Xrand=Rand()

Else

Xrand=Nearest((Ta,Xgoal)or(Tb, Xstart))

End If

Xnear=Nearest(Xrand,Ta)

Xnew =NewExtend(Xnear,Xrand,stepsize)

While ～EdgeCollision(Xnew,Xnear)

Xnew=GreedyExtend(Xnew,Xrand, stepsize)

End While

Ta={Ta， Xnew}

If Connect(Ta, Tb)

Flag=false

Return(Path)

Break

Else

Swap(Ta, Tb)

Continue

End If

End While

3.4 Dijkstra for path optimization

Dijkstra algorithm uses the breadth first search to solve the problem of single-source shortest path of a weighted digraph or undirected graph. Finally, the shortest path tree is obtained by this algorithm. This algorithm is often used in the routing algorithm or as a sub module of other graph data algorithms. Dijkstra algorithm is implemented by greedy thinking. First, save the distance from the starting point to all points to find the shortest one, then relax once to find the shortest one. The so-called relaxation operation is to traverse the shortest point just located at a transit station to see if it will be closer. If it is closer, update the distance, so that after all points are searched, the starting point will be saved the shortest distance to all other points.

3.5 Simulation experiments

The start point of the manipulator is (20,-10, 18), the target point is (25, 10, 20), and a number of box enveloped obstacles are set in the 3D environment to simulate the complex obstacle environment and the goal is to verify the feasibility and effectiveness of the improved algorithm’s obstacle avoidance path planning under the complex obstacle environment. The simulation is shown in the Fig.4.

Fig.4 Path side view and face view

In the case of 3D environment, the improved algorithm, RRT-Connect algorithm and RRT* algorithm were compared with simulation experiments under the same conditions. To facilitate the demonstration of the simulation results, the top of obstacles should be removed. The comparison of obstacle avoidance planning paths of the three algorithms is shown in the Fig.5.

Fig.5 Path face view and top view

The blue path in the figure is not optimized by the Dijkstra algorithm. It can be seen from the Fig.5 that the improved algorithm is better than the other two algorithms in path planning.

The simulation environment was MATLAB2018a, and each group was executed 50 times, and the results are shown in the Table 2.

Table 2 Simulation results

4 Conclusion

Combining the advantages of the RRT-Connect algorithm and Dijkstra algorithm, this paper proposes an IM-RRT-connect algorithm to optimize the path planning of the operation arm. In addition, the effectiveness of the traditional RRT-Connect algorithm, RRT* algorithm and IM-RRT-Connect algorithm is compared In the 3D environments, the improved algorithm can quickly and efficiently complete the obstacle avoidance path planning of the manipulator and guide the manipulator to complete the task safely and without collision. Through the comparison of simulation experiments, the RRT-Connect algorithm can be improved, which has the advantages of fast search speed, fewer nodes and shorter paths, and the effect of obstacle avoidance path planning has been significantly improved.