# Obstacle Avoidance Path Planning for the Dual-Arm Robot Based on an Improved RRT Algorithm
## 3 Algorithm Improvement
### Path Planning of Dual-Arm Based on GA_RRT
#### 3.2.3 Collision Detection of Dual-Arm Robots  
In order to simplify the calculation of the 6-DOF manipulator, the method of the bounding sphere is used for obstacle avoidance detection. It is the smallest sphere that surrounds three-dimensional objects. In order to reduce the complex calculation of collision detection during the movement of the arms, this method is selected in this paper. Its advantages are convenient calculation, simple structure, and it is not affected by spin. This algorithm has better security while generating partial redundant spaces. When considering whether there is a collision between the manipulator and the obstacle, or a collision between the main manipulator and the slave manipulator, it is only necessary to consider the distance and radius of the object center.  
The base point of the main manipulator is denoted as \(O_{10}\), and the center points of the six joints are denoted as {\(O_{11}\), \(O_{12}\), \(O_{13}\), \(O_{14}\), \(O_{15}\), \(O_{16}\)}. Then, the center points of the connecting rod are denoted as {\(A_{1}\), \(A_{2}\), \(A_{3}\), \(A_{4}\), \(A_{5}\), \(A_{6}\)}. These values are determined by the parameters of the manipulator body. Taking the joint bounding sphere as an example, the joint bounding sphere can be expressed as:  
\[R=\left\{(x,y,z)\Big{|}(x-o_{jix})^{2}+(y-o_{jiy})^{2}+(z-o_{jiz})^{2}<r^{2}\right\} \tag{5}\]  
The radius of the joint bounding sphere for the main manipulator is set as {\(R_{11}\), \(R_{12}\), \(R_{13}\), \(R_{14}\), \(R_{15}\), \(R_{16}\)}, and the bounding sphere radius of the connecting rod is set as {\(R_{a1}\), \(R_{a2}\), \(R_{a3}\), \(R_{a4}\), \(R_{a5}\), \(R_{a6}\)}. The slave arm parameters are represented by \(O_{2i}\), \(B_{i}\), \(R_{2i}\), \(R_{bi}\). The radius can be calculated using Equation (6)  
\[R_{ij}=\frac{1}{2}\sqrt{(x_{ij\max}-x_{ij\min})^{2}+(y_{ij\max}-y_{ij\min})^{2 }+(z_{ij\max}-z_{ij\min})^{2}} \tag{6}\]  
where \(x_{ij\max}\), \(x_{ij\min}\), \(y_{ij\max}\), \(y_{ij\min}\), \(z_{ij\max}\), respectively, represent the maximum and minimum values of the object projected on the XYZ coordinate axes. Bounding sphere model of the dual-arm robot is shown in Figure 4.  
The conditions for collision detection of an enclosing ball are shown in Equations (5)-(7).  
1. No collision between each connecting rod and the joint in the main manipulator: \[\left\{\begin{array}{l}\left\|O_{1i}-O_{1j}\right\|<(R_{1i}+R_{1j})\\ \left\|O_{1i}-A_{j}\right\|<(R_{1i}+R_{aj})\end{array}\right.,i\
eq j,i=1,2, \ldots,6,j=1,2,\ldots,6\] (7)
2. No collision between each connecting rod and the joint in the slave manipulator: \[\left\{\begin{array}{l}\left\|O_{2i}-O_{2j}\right\|<(R_{2i}+R_{2j})\\ \left\|O_{2i}-B_{j}\right\|<(R_{2i}+R_{bj})\end{array}\right.,i\
eq j,i=1,2, \ldots,6,j=1,2,\ldots,6\] (8)
3. No collision between the links of the main manipulator and the links of the slave manipulator:\[\left\{\begin{array}{l}\|O_{1i}-O_{2j}\|<(R_{1i}+R_{2j})\\ \|O_{1i}-B_{j}\|<(R_{1i}+R_{bj})\\ \|A_{i}-O_{2j}\|<(R_{ai}+R_{2j})\\ \|A_{i}-B_{j}\|<(R_{ai}+R_{bj})\end{array}\right.,i=1,2,\ldots,6,j=1,2,\ldots,6 \tag{9}\]  
The static obstacle collision detection in the environment mainly calculates the distance, D, from the center of the manipulator to the center of the obstacle ball. The collision is judged by comparing the distance, D, with the sum of the radius of the enclosing ball.