# A Method on Dynamic Path Planning for Robotic Manipulator Autonomous Obstacle Avoidance Based on an Improved RRT Algorithm

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Improved RRT Algorithm

#### 2.1. Traditional RRT Algorithm

_{init})

- T.init(q
_{init}); - for k = 1 to k do
- q
_{rand}← Random_State() - Extend(T, q
_{rand}); - Return T

- q
_{near}←Nearest_Neighbor(q, T); - If New_State(q, q
_{near}, q_{new}, u_{new}) then - T.add_vertex(q
_{new}); - T.add_edge(q
_{near,}q_{new,}u_{new}); - if q
_{new}= q then - Return Reached;
- else
- Return Advanced;
- Return Trapped;

#### 2.2. Node Extension

- p←Random(0, 1.0)
- if p < P
_{goal}; - Return goal;
- else
- Return RandomNode();

- result←Extend(T, goal)
- if Trapped = result
- q
_{rand}←Random_Node() - while Trapped = Extend(T, goal)
- Random_Extend(T, q
_{goal}); - else
- Improve_Extend(T)

- p←Random(0, 1.0)
- if p < P
_{best} - Extend(T, q
_{nearest}, q); - else
- Extend(T, q);

#### 2.3. Collision Inspection

- Forward_Kinematics(x);
- for k = 1 to 6 do
- C
_{k}←FCL_Cylinder_Create(x, k) - if FCL_Cylinder_Collision(C
_{k}) - Return Trapped;
- Return Advanced;

#### 2.4. Trajectory Optimization

- Q←Pruning(T)
- Q←Inser_MidNode(Q)
- S←Cubic_Bspline(Q)
- Return S

- 5.
- T←obtained from S-RRT
- 6.
- Var Q
_{1}, Q_{2}: path - 7.
- Q
_{1}(q_{0}, q_{1}, q_{2}, ⋯, q_{n}) = Path(T) - 8.
- q
_{temp}←q_{0}; Q_{2}.Add_Node(q_{0}) - 9.
- while q
_{temp}! = q_{n}do - 10.
- for each node qi ∈ Q
_{1} - 11.
- if Collision(q
_{temp}, q_{i}) - 12.
- q
_{temp}←q_{i}; - 13.
- Q
_{2}.Add_Node(q_{temp});break - 14.
- end if
- 15.
- end for
- 16.
- Q
_{2}.Add_Node(q_{n}) - 17.
- end while
- 18.
- for each node q
_{k}∈ Q_{2} - 19.
- if Angle $(\overrightarrow{{q}_{k+1}{q}_{k}},\overrightarrow{{q}_{k+1}{q}_{k+2}})<{\alpha}_{\mathrm{min}}$
- 20.
- Q
_{2}Insert_Node(q_{k}, q_{insert}, q_{k+1}) - 21.
- end if
- 22.
- end for
- 23.
- Return Q
_{2}

## 3. Simulation and Experiment

#### 3.1. Simulation in a Static Environment Based on MATLAB

#### 3.2. Simulation Validation in a Dynamic Environment Based on an ROS

#### 3.3. Experiment of Static Global Autonomous Obstacle Avoidance Path Planning

#### 3.4. Experiment of Dynamic Local Autonomous Obstacle Avoidance Path Planning

## 4. Conclusions and Future Work

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- Han, D.; Nie, H.; Chen, J. Dynamic obstacle avoidance for manipulators using distance calculation and discrete detection. Robot. Comput. Integr. Manuf.
**2018**, 49, 98–104. [Google Scholar] [CrossRef] - Park, C.; Pan, J.; Manocha, D. Real-time optimization-based planning in dynamic environments using GPUs. In Proceedings of the IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 4090–4097. [Google Scholar]
- Luo, R.C.; Kuo, C. Intelligent Seven-Dof Robot with dynamic obstacle avoidance and 3-D object recognition for industrial cyber-physical systems in manufacturing automation. Proc. IEEE
**2016**, 104, 1102–1113. [Google Scholar] [CrossRef] - Indri, M.; Trapani, S.; Lazzero, I. Development of a virtual collision sensor for industrial robots. Sensors
**2017**, 17, 1148. [Google Scholar] [CrossRef] [PubMed] - Ji, W.; Cheng, F.Y.; Zhao, D. Obstacle avoidance method of apple harvesting robot manipulator. Trans. Chin. Soc. Agric. Mach.
**2013**, 44, 253–259. [Google Scholar] - He, Z.C.; He, Y.L.; Zeng, B. Obstacle avoidance path planning for robot arm based on mixed algorithm of artificial potential field method and RRT. Ind. Eng. J.
**2017**, 20, 56–63. [Google Scholar] - Kivelä, T.; Mattila, J.; Puura, J. Redundant Robotic Manipulator Path Planning for Real-Time Obstacle and Self-Collision Avoidance. In Advances in Service and Industrial Robotics; Springer: Cham, Switzerland, 2017; pp. 208–216. [Google Scholar]
- Feng, L.; Jia, J.H. Improved algorithm of RRT path planning based on comparison optimization. Comput. Eng. Appl.
**2011**, 47, 210–213. [Google Scholar] - Lavalle, S.M.; Kuffner, J.J. Rapidly-Exploring Random Trees: Progress and Prospects. In Algorithmic & Computational Robotics New Directions; CRC Press: Boca Raton, FL, USA, 2000; pp. 293–308. [Google Scholar]
- Kuffner, J.J.; Lavalle, S.M. RRT-connect: An efficient approach to single-query path planning. In Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, CA, USA, 24–28 April 2000; pp. 995–1001. [Google Scholar]
- Ge, J.; Sun, F.; Liu, C. RRT-GD: An efficient rapidly-exploring random tree approach with goal directionality for redundant manipulator path planning. In Proceedings of the IEEE International Conference on Robotics and Biomimetics, Qingdao, China, 3–7 December 2017; pp. 1983–1988. [Google Scholar]
- Yang, H.J.; Li, L.J.; Gao, Z.C. Obstacle avoidance path planning of hybrid harvesting manipulator based on joint configuration space. Trans. Chin. Soc. Agric. Eng.
**2017**, 33, 55–62. [Google Scholar] - Li, X.C.; Zhao, D.B.; Yi, J.Q. A dynamic path planning approach for mobile manipulators along given end effector paths. Control Decis.
**2007**, 22, 184–188. [Google Scholar] - Kuwata, Y.; Teo, J.; Fiore, G. Real-time motion planning with applications to autonomous urban driving. IEEE Trans. Control Syst. Technol.
**2009**, 17, 1105–1118. [Google Scholar] [CrossRef] - Fraichard, T.; Scheuer, A. From Reeds and Shepp’s to continuous-curvature paths. IEEE Trans. Robot.
**2004**, 20, 1025–1035. [Google Scholar] [CrossRef] - Lau, B.; Sprunk, C.; Burgard, W. Kinodynamic motion planning for mobile robots using splines. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, USA, 10–15 October 2009; pp. 2427–2433. [Google Scholar]
- Elbanhawi, M.; Simic, M. Continuous-Curvature Bounded Trajectory Planning Using Parametric Splines. In Proceedings of the Frontiers in Artificial Intelligence and Applications, Chania, Greece, 26–28 June 2014; pp. 513–522. [Google Scholar]
- Pan, J.; Chitta, S.; Manocha, D. FCL: A general purpose library for collision and proximity queries. In Proceedings of the IEEE International Conference on Robotics and Automation, Saint Paul, MN, USA, 14–18 May 2012; pp. 3859–3866. [Google Scholar]
- Gómez-Bravo, F.; Cuesta, F.; Ollero, A. Continuous curvature path generation based on β-spline curves for parking manoeuvres. Robot. Auton. Syst.
**2008**, 56, 360–372. [Google Scholar] [CrossRef] - Koyuncu, E.; Inalhan, G. A probabilistic B-spline motion planning algorithm for unmanned helicopters flying in dense 3D environments. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 815–821. [Google Scholar]

**Figure 1.**Pruning algorithm diagram. (

**a**) Initial paths generated by the S-RRT; (

**b**) Smooth paths using Function Pruning(T).

**Figure 2.**Comparison of the three algorithms and the result of the Smoothly Rapidly Exploring Random Tree (S-RRT) algorithm. (

**a**) Random tree with Basic-RRT; (

**b**) Random tree with Bi-RRT; (

**c**) Random tree with S-RRT; (

**d**) Random tree with optimized S-RRT; (

**e**) Curvature variation map of the path generated by S-RRT.

**Figure 6.**Path sequence with the S-RRT algorithm in static obstacle avoidance. (

**a**–

**f**) represent different states of the manipulator and static obstacle at each moment.

**Figure 7.**Comparison of the three algorithms and curvature variation with S-RRT. (

**a**) Comparison of path planning with the three algorithms; (

**b**) Variation of path curvature with path length.

**Figure 9.**Path sequence with S-RRT algorithm during dynamic obstacle avoidance. (

**a**–

**f**) represent different states of the manipulator and dynamic obstacle at each moment

50 Times Planning Experiments | Average Planning Time/ms | Average Sampling Nodes | Successful Times |
---|---|---|---|

Basic-RRT | 403.5 | 752.6 | 42 |

Bi-RRT | 186.75 | 351.8 | 50 |

S-RRT | 79.4 | 172.3 | 50 |

20 Planning Experiments | Average Planning Time/ms | Average Sampling Nodes | Successful Times |
---|---|---|---|

Basic-RRT | 986.5 | 1203.5 | 12 |

Bi-RRT | 523.6 | 632.5 | 16 |

S-RRT | 242.2 | 209.4 | 20 |

© 2018 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 (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

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.
https://doi.org/10.3390/s18020571

**AMA Style**

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(2):571.
https://doi.org/10.3390/s18020571

**Chicago/Turabian Style**

Wei, Kun, and Bingyin Ren.
2018. "A Method on Dynamic Path Planning for Robotic Manipulator Autonomous Obstacle Avoidance Based on an Improved RRT Algorithm" *Sensors* 18, no. 2: 571.
https://doi.org/10.3390/s18020571