# Obstacle Avoidance Path Planning for the Dual-Arm Robot Based on an Improved RRT Algorithm

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Model Building

## 3. Algorithm Improvement

#### 3.1. Basic RRT

Algorithm 1: RRT algorithm |

1: T = init Tree(); // Initialize the random tree 2: R = init R(); // Initialize the step size 3: T[0] = Node(${X}_{init}$); 4: for i = 1 to N: 5: ${X}_{rand}$= Random sampling(); // Random sampling 6: ${X}_{nearest}$= min the distance (${X}_{rand}$)$Nod{e}_{tree});$// Find the nearest node 7: ${X}_{new}$ = extend (${X}_{nearest},{X}_{rand}$, R); // Expand towards random points 8: if not obstacle(${X}_{nearest}$, ${X}_{new}$): 9: T[i] = add Node(${X}_{new}$); // Add a new node to the tree 10: end if 11: end for 12: return T |

#### 3.2. Path Planning of Dual-Arm Based on GA_RRT

#### 3.2.1. The Goal Probability Bias

#### 3.2.2. A* Cost Function

#### 3.2.3. Collision Detection of Dual-Arm Robots

- 1.
- No collision between each connecting rod and the joint in the main manipulator:$$\{\begin{array}{l}\Vert {O}_{1l}-{O}_{1j}\Vert <({R}_{1l}+{R}_{1j})\\ \Vert {O}_{1i}-{A}_{j}\Vert <({R}_{1i}+{R}_{aj})\end{array},i\ne j,i=1,2,\dots ,6,j=1,2,\dots ,6$$
- 2.
- No collision between each connecting rod and the joint in the slave manipulator:$$\{\begin{array}{l}\Vert {O}_{2i}-{O}_{2j}\Vert <({R}_{2i}+{R}_{2j})\\ \Vert {O}_{2i}-{B}_{j}\Vert <({R}_{2i}+{R}_{bj})\end{array},i\ne j,i=1,2,\dots ,6,j=1,2,\dots ,6$$
- 3.
- No collision between the links of the main manipulator and the links of the slave manipulator:$$\{\begin{array}{l}\Vert {O}_{1i}-{O}_{2j}\Vert <({R}_{1i}+{R}_{2j})\\ \Vert {O}_{1i}-{\mathrm{B}}_{j}\Vert <({R}_{1i}+{R}_{\mathrm{b}j})\\ \Vert {{\displaystyle A}}_{i}-{O}_{2j}\Vert <({R}_{ai}+{R}_{2j})\\ \Vert {{\displaystyle A}}_{i}-{\mathrm{B}}_{j}\Vert <({R}_{ai}+{R}_{\mathrm{b}j})\end{array},i=1,2,\dots ,6,j=1,2,\dots ,6$$

#### 3.2.4. Overall Process of GA_RRT

Algorithm 2: GA_RRT algorithm |

1: T = init Tree(); // Initialize the random tree 2: R = init R(); // Initialize the step size 3: T[0] = Node(${X}_{init}$); 4: for i = 1 to N: 5: for j = 1 to M: // M random states are generated, recommended value 4 6: if ${P}_{rand}$ < ${P}_{a}$: 7: ${X}_{direction}$ = ${X}_{goal}$;// Select random points as target points 8: else: 9: ${X}_{direction}$ = Random sampling(); // Random sampling 10: ${X}_{nearest}$ = min the distance ${X}_{direction},Nod{e}_{tree});$ 10: ${X}_{select}$ = extend (${X}_{nearest},{X}_{direction}$, R); // Expand 11: ${X}_{path}$(j) = add ${X}_{select}$ // Add random states to cost function group 12: end for 13: F(j) = the distance(${X}_{path}$(j), ${X}_{init}$) + the distance(${X}_{path}$(j), ${X}_{goal}$); // cost function 14: ${X}_{new}$ = min F(j); // The minimum cost node is taken in tree 15: if not obstacle(${X}_{nearest}$, ${X}_{new}$): 16: T[i] = add Node(${X}_{new}$); // Add new nodes 17: end if 18: end for 19: return T |

## 4. Simulation

#### 4.1. Obstacle Avoidance Path Planning for Static Plane

#### 4.2. Obstacle Avoidance Path Planning for Dynamic Dual-Arm Robots

## 5. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Bohren, J.; Rusu, R.B.; Jones, E.G.; Marder-Eppstein, E.; Pantofaru, C.; Wise, M.; Mösenlechner, L.; Meeussen, W.; Holzer, S. Towards autonomous robotic butlers: Lessons learned with the PR2. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 5568–5575. [Google Scholar]
- Matthias, B.; Kock, S.; Jerregard, H.; Kallman, M.; Lundberg, I.; Mellander, R. Safety of collaborative industrial robots: Certification possibilities for a collaborative assembly robot concept. In Proceedings of the 2011 IEEE International Symposium on Assembly and Manufacturing (ISAM), Tampere, Finland, 25–27 May 2011; pp. 1–6. [Google Scholar]
- Connolly, C. Motoman markets co-operative and humanoid industrial robots. Ind. Robot: Int. J.
**2009**, 36, 417–420. [Google Scholar] [CrossRef] - Fuchs, M.; Borst, C.; Giordano, P.R.; Baumann, A.; Kraemer, E.; Langwald, J.; Gruber, R.; Seitz, N.; Plank, G.; Kunze, K.; et al. Rollin’Justin-Design considerations and realization of a mobile platform for a humanoid upper body. In Proceedings of the 2009 IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009; pp. 4131–4137. [Google Scholar]
- Wang, M. Research on Collaborative Motion Planning and Control Methods of Dual-Arm Robots for Rescue Tasks; University of Science and Technology of China: Hefei, China, 2015. [Google Scholar]
- Peng, Y.C.; Carabis, D.S.; Wen, J.T. Collaborative manipulation with multiple dual arm robots under human guidance. Int. J. Intell. Robot. Appl.
**2018**, 2, 252–266. [Google Scholar] [CrossRef] - Khatib, O. Real-time obstacle avoidance for manipulators and mobile robots. In Autonomous Robot Vehicles; Springer: New York, NY, USA, 1986; pp. 396–404. [Google Scholar]
- Abhishek, T.S.; Schilberg, D.; Doss, A.S.A. Obstacle Avoidance Algorithms: A Review. In Proceedings of the International Conference on Robotics, Intelligent Automation and Control Technologies (RIACT 2020), Chennai, India, 2–3 October 2020; Volume 1012, p. 012052. [Google Scholar]
- Palmieri, G.; Scoccia, C.; Palpacelli, M.C.; Callegari, M. Motion planning and control of redundant manipulators for dynamical obstacle avoidance. Machines
**2021**, 9, 121. [Google Scholar] [CrossRef] - Duchoň, F.; Babinec, A.; Kajan, M.; Beňo, P.; Florek, M.; Fico, T.; Jurišica, L. Path planning with modified a star algorithm for a mobile robot. Procedia Eng.
**2014**, 96, 59–69. [Google Scholar] [CrossRef] [Green Version] - Guruji, A.K.; Agarwal, H.; Parsediya, D.K. Time-efficient A* algorithm for robot path planning. Procedia Technol.
**2016**, 23, 144–149. [Google Scholar] [CrossRef] [Green Version] - Han, B.; Luo, X.; Luo, Q.; Zhao, Y.; Lin, B. Research on Obstacle Avoidance Motion Planning Technology of 6-DOF Manipulator. International Conference on Geometry and Graphics; Springer: Berlin/Heidelberg, Germany, 2021; pp. 604–614. [Google Scholar]
- Wang, J.; Liu, S.; Zhang, B.; Yu, C. Manipulation Planning with Soft Constraints by Randomized Exploration of the Composite Configuration Space. Int. J. Control. Autom. Syst.
**2021**, 19, 1340–1351. [Google Scholar] [CrossRef] - Glasius, R.; Komoda, A.; Gielen, S.C.A.M. Neural network dynamics for path planning and obstacle avoidance. Neural Netw.
**1995**, 8, 125–133. [Google Scholar] [CrossRef] - Sung, I.; Choi, B.; Nielsen, P. On the training of a neural network for online path planning with offline path planning algorithms. Int. J. Inf. Manag.
**2021**, 57, 102142. [Google Scholar] [CrossRef] - Gerke, M. Genetic path planning for mobile robots. In Proceedings of the American Control Conference, San Diego, CA, USA, 2–4 June 1999; pp. 596–601. [Google Scholar]
- Kang, H.B. Mechanical Dual-Arms: Collision-Free Path Planning of Robot Based on Improved Ant Colony Algorithms; Northeastern University: Shenyang, China, 2010. [Google Scholar]
- Tuncer, A.; Yildirim, M. Dynamic path planning of mobile robots with improved genetic algorithm. Comput. Electr. Eng.
**2012**, 38, 1564–1572. [Google Scholar] [CrossRef] - Dorigo, M.; Maniezzo, V.; Colorni, A. Ant system: Optimization by a colony of cooperating agents. IEEE Trans. Syst. Man Cybern. Part B (Cybern.)
**1996**, 26, 29–41. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Silvia, M.; Irene, L. An ant colony system for responsive dynamic vehicle routing. Eur. J. Oper. Res.
**2015**, 245, 704–718. [Google Scholar] - Zong, C.; Lu, L.; Lei, X.; Zhao, P. A path planning approach for multi-dof spatial manipulator via A* algorithm. J. Hefei Univ. Technol. (Nat. Sci.)
**2017**, 40, 164–168. [Google Scholar] - Dijkstra, E.W. A note on two problems in connexion with graphs. Numer. Math.
**1959**, 1, 269–271. [Google Scholar] [CrossRef] [Green Version] - Sanchez, G.; Latombe, J.C. Using a PRM planner to compare centralized and decoupled planning for multi-robot systems. In Proceedings of the IEEE International Conference on Robotics and Automation, Washington, DC, USA, 11–15 May 2002; pp. 2112–2119. [Google Scholar]
- Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res.
**2011**, 30, 846–894. [Google Scholar] [CrossRef] - Jaillet, L.; Cortés, J.; Siméon, T. ‘Sampling-based path planning on configuration-space costmaps. IEEE Trans. Robot.
**2010**, 26, 635–646. [Google Scholar] [CrossRef] [Green Version] - Pepy, R.; Lambert, A. Safe path planning in an uncertain-configuration space using RRT. In Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 5376–5381. [Google Scholar]
- LaValle, S.M.; Kuffner, J.J.; James, J. Randomized Kino-dynamic Planning. Int. J. Robot. Res.
**1999**, 15, 378–400. [Google Scholar] - Urmson, C.; Simmons, R. Approaches for heuristically biasing RRT growth. In Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No. 03CH37453), Las Vegas, NV, USA, 27–31 October 2003; Volume 2, pp. 1178–1183. [Google Scholar]
- Li, Z.; Ma, H.; Zhang, X.; Fei, Q. Path planning of the dual-arm robot based on VT-RRT algorithm. In Proceedings of the 2019 Chinese Control Conference (CCC), Guangzhou, China, 27–30 July 2019; pp. 4359–4364. [Google Scholar]
- Jiang, H.; Jiang, X. An improved path planning algorithm based on RRT. J. Chongqing Univ. Technol. (Nat. Sci.)
**2021**, 35, 10–16. [Google Scholar] - Shao, L.; Liu, H.; Chen, C.; Du, D.; Li, J.; Liu, H. Path Planning for Mobile Robots Based on Improved RRT Algorithm. In Proceedings of the 2020 IEEE International Conference on Mechatronics and Automation (ICMA), Beijing, China, 13–16 October 2020; pp. 1240–1244. [Google Scholar]
- Wei, K.; Ren, B. A method on dynamic path planning for robotic manipulator autonomous obstacle avoidance based on an improved RRT algorithm. Sensors
**2018**, 18, 571. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Andreas, V.; Knut, G. An optimization-based approach to dual-arm motion planning with closed kinematics. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, 1–5 October 2018; pp. 8346–8351. [Google Scholar]
- Kim, D.H.; Lim, S.J.; Lee, D.H.; Lee, J.; Han, C. A RRT-based motion planning of dual-arm robot for (Dis)assembly tasks. In Proceedings of the IEEE Conference on Intelligence and Safety for Robotics, Seoul, Korea, 24–26 October 2013. 6p. [Google Scholar]
- Li, Y.; Xu, D. Cooperative path planning of dual-arm robot based on gravitational adaptive step RRT. Robot
**2020**, 42, 606–616. [Google Scholar]

**Figure 7.**The obstacle avoidance path planning result of scenario B: (

**a**) RRT; (

**b**) G_RRT; (

**c**) GA_RRT.

**Figure 8.**Motion state decomposition without obstacle avoidance path planning: (

**a**) initial state; (

**b**) motion state 1, collide with the environment; (

**c**) motion state 2, self-collision; (

**d**) side view of final state.

**Figure 9.**Exploration process of the main arm joint angle: (

**a**) the first three joint angles of the main arm; (

**b**) the posterior three joint angles of the main arm.

**Figure 11.**Exploration process of the salve arm: (

**a**) the first three joint angles of the slave arm; (

**b**) the posterior three joint angles of the slave arm.

**Figure 14.**GA_RRT obstacle avoidance trajectory: (

**a**) self-collision avoidance; (

**b**) side view of final state.

Joint Number | α | a | θ | d |
---|---|---|---|---|

1 | 90 | 0 | ${\mathsf{\theta}}_{1}$ | 89.2 |

2 | 0 | −425 | ${\mathsf{\theta}}_{2}$ | 0 |

3 | 0 | −392 | ${\mathsf{\theta}}_{3}$ | 0 |

4 | 90 | 0 | ${\mathsf{\theta}}_{4}$ | 109.3 |

5 | −90 | 0 | ${\mathsf{\theta}}_{5}$ | 94.75 |

6 | 0 | 0 | ${\mathsf{\theta}}_{6}$ | 82.5 |

RRT | G_RRT | GA_RRT | |
---|---|---|---|

Total number of nodes | 256 | 105 | 49 |

Path length | 1539 | 1360 | 1373 |

Average time/s | 8.35 | 4.5 | 3.38 |

RRT | G_RRT | GA_RRT | |
---|---|---|---|

Total number of nodes | 348 | 129 | 75 |

Path length | 683.53 | 465.42 | 459.3 |

Average time/s | 13.1 | 5.5 | 3.6 |

RRT | Bi-RRT | S-RRT | GA_RRT | |
---|---|---|---|---|

Average Planning Time/s | 9.86 | 5.23 | 2.42 | 2.04 |

Robotic Arm | Path Length | Average Planning Time/s | Successful Times | |
---|---|---|---|---|

Scenario 0 | The main arm | 142.5 | 1.73 | 10 |

The slave arm | 195.2 | 2.35 | 9 | |

Scenario 1 | The main arm | 154.0 | 1.87 | 10 |

The slave arm | 234.3 | 2.90 | 10 | |

Scenario 2 | The main arm | 153.9 | 1.67 | 10 |

The slave arm | 163.8 | 2.23 | 10 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Shi, W.; Wang, K.; Zhao, C.; Tian, M.
Obstacle Avoidance Path Planning for the Dual-Arm Robot Based on an Improved RRT Algorithm. *Appl. Sci.* **2022**, *12*, 4087.
https://doi.org/10.3390/app12084087

**AMA Style**

Shi W, Wang K, Zhao C, Tian M.
Obstacle Avoidance Path Planning for the Dual-Arm Robot Based on an Improved RRT Algorithm. *Applied Sciences*. 2022; 12(8):4087.
https://doi.org/10.3390/app12084087

**Chicago/Turabian Style**

Shi, Wubin, Ke Wang, Chong Zhao, and Mengqi Tian.
2022. "Obstacle Avoidance Path Planning for the Dual-Arm Robot Based on an Improved RRT Algorithm" *Applied Sciences* 12, no. 8: 4087.
https://doi.org/10.3390/app12084087