# Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments

^{*}

## Abstract

**:**

## 1. Introduction

## 2. The Kinematic Model and Dual-Tree RRT

#### 2.1. Kinematic Model of a Car-Like Robot

#### 2.2. Dual-Tree RRT

## 3. Car-like DT-RRT* (CDT-RRT*)

#### 3.1. Motion Controller for Trajectory Generations

#### 3.2. Dual-Tree Structure of CDT-RRT*

Algorithm 1 BuildTree*$({q}_{0},{x}_{0},goal)$ |

$\mathbf{TW}.\mathrm{INIT}({q}_{0},goal);$ $\mathbf{TX}.\mathrm{INIT}({x}_{0},goal);$ while ${t}_{limit}$ do${q}_{rand}\leftarrow $ RandomSample(); $[{Q}_{new},{Q}_{near},type]\leftarrow $ ExtWorkSpace$(\mathbf{TW},{q}_{rand});$ ${\mathbb{Q}}_{nearlist}\leftarrow $ NearNodeList$({Q}_{new},{B}_{r})$ $[{X}_{p},{X}_{min}]\leftarrow $ FindParentState*$({\mathbb{Q}}_{nearlist},{q}_{new},type);$ if ${X}_{p}=nil$ then$\mathbf{continue}$; end if$\mathbf{TW}.\mathrm{Add}({Q}_{near},{Q}_{new});$ $\mathbf{TX}.\mathrm{Add}({X}_{p},{X}_{min});$ ReConnectTrees$({Q}_{new},{X}_{min});$ end while$\mathbf{return};$ |

Algorithm 2 FindingParentState*$\{{\mathbb{Q}}_{nearlist},{q}_{new},type\}$ |

${X}_{p},{X}_{min}\leftarrow nil;$ $cos{t}_{min}\leftarrow inf;$ for all ${Q}_{near}$ ∈ ${\mathbb{Q}}_{nearlist}$ do${X}_{cur}\leftarrow $ GetStateNode$({Q}_{near});$ $[{X}_{new},{e}_{r},cost]\leftarrow $ ExtendState$({\mathrm{q}}_{\mathrm{new}},{\mathrm{X}}_{\mathrm{cur}},\mathrm{type})$ if ${e}_{r}\le {B}_{r}\phantom{\rule{3.33333pt}{0ex}}\mathbf{and}\phantom{\rule{3.33333pt}{0ex}}cost\le cos{t}_{min}$ then$[{X}_{p},{X}_{min},cos{t}_{min}]\leftarrow [{X}_{cur},{X}_{new},cost];$ end ifend for$\mathbf{return}\phantom{\rule{1.33333pt}{0ex}}{X}_{p},{X}_{min};$ |

## 4. Simulation Results

## 5. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Conflicts of Interest

## References

- McNaughton, M.; Urmson, C.; Dolan, J.M.; Lee, J.W. Motion planning for autonomous driving with a conformal spatiotemporal lattice. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011; pp. 4889–4895. [Google Scholar]
- Hart, P.E.; Nilsson, N.J.; Raphael, B. A formal basis for the heuristic determination of minimum cost paths. IEEE Trans. Syst. Sci. Cybern.
**1968**, 4, 100–107. [Google Scholar] [CrossRef] - Stentz, A. The focussed d^* algorithm for real-time replanning. IJCAI
**1995**, 95, 1652–1659. [Google Scholar] - Ding, W.; Gao, W.; Wang, K.; Shen, S. An Efficient B-Spline-Based Kinodynamic Replanning Framework for Quadrotors. IEEE Trans. Robot.
**2019**, 35, 1287–1306. [Google Scholar] [CrossRef][Green Version] - Artuñedo, A.; Villagra, J.; Godoy, J. Real-Time Motion Planning Approach for Automated Driving in Urban Environments. IEEE Access
**2019**, 7, 180039–180053. [Google Scholar] [CrossRef] - Lin, J.; Zhou, T.; Zhu, D.; Liu, J.; Meng, M.Q.H. Search-Based Online Trajectory Planning for Car-like Robots in Highly Dynamic Environments. arXiv
**2020**, arXiv:2011.03664. [Google Scholar] - Schleich, D.; Behnke, S. Search-based planning of dynamic MAV trajectories using local multiresolution state lattices. arXiv
**2021**, arXiv:2103.14607. [Google Scholar] - Yang, S.; Lin, Y. Development of an Improved Rapidly Exploring Random Trees Algorithm for Static Obstacle Avoidance in Autonomous Vehicles. Sensors
**2021**, 21, 2244. [Google Scholar] [CrossRef] [PubMed] - Shome, R.; Kavraki, L.E. Asymptotically Optimal Kinodynamic Planning Using Bundles of Edges. In Proceedings of the 2021 International Conference on Robotics and Automation (ICRA), Xian, China, 30 May–5 June 2021. [Google Scholar]
- LaValle, S.M. Rapidly-Exploring Random Trees: A New Tool for Path Planning; TR 98-11; Computer Science Department, Iowa State University: Ames, IA, USA, 1998; Available online: http://janowiec.cs.iastate.edu/papers/rrt.ps (accessed on 11 May 2021).
- Hauser, K.; Zhou, Y. Asymptotically optimal planning by feasible kinodynamic planning in a state–cost space. IEEE Trans. Robot.
**2016**, 32, 1431–1443. [Google Scholar] [CrossRef] - Verginis, C.K.; Dimarogonas, D.V.; Kavraki, L.E. Sampling-Based Motion Planning for Uncertain High-Dimensional Systems via Adaptive Control. In Algorithmic Foundations of Robotics XIV, Proceedings of the Fourteenth Workshop on the Algorithmic Foundations of Robotics 14, Oulu, Finland, 21–23 June 2021; Springer: Basingstoke, UK, 2021; pp. 159–175. [Google Scholar]
- Kingston, Z.; Moll, M.; Kavraki, L.E. Sampling-based methods for motion planning with constraints. Annu. Rev. Control. Robot. Auton. Syst.
**2018**, 1, 159–185. [Google Scholar] [CrossRef][Green Version] - Vasile, C.I.; Li, X.; Belta, C. Reactive sampling-based path planning with temporal logic specifications. Int. J. Robot. Res.
**2020**, 39, 1002–1028. [Google Scholar] [CrossRef] - Ghosh, D.; Nandakumar, G.; Narayanan, K.; Honkote, V.; Sharma, S. Kinematic constraints based Bi-directional RRT (KB-RRT) with parameterized trajectories for robot path planning in cluttered environment. In Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; pp. 8627–8633. [Google Scholar]
- Webb, D.J.; van den Berg, J. Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany, 6–10 May 2013; pp. 5054–5061. [Google Scholar]
- Hu, B.; Cao, Z.; Zhou, M. An Efficient RRT-Based Framework for Planning Short and Smooth Wheeled Robot Motion Under Kinodynamic Constraints. IEEE Trans. Ind. Electron.
**2021**, 68, 3292–3302. [Google Scholar] [CrossRef] - Westbrook, M.G.; Ruml, W. Anytime Kinodynamic Motion Planning using Region-Guided Search. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 24 October 2020–24 January 2021; pp. 6789–6796. [Google Scholar]
- Schmerling, E.; Janson, L.; Pavone, M. Optimal sampling-based motion planning under differential constraints: The driftless case. In Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 2368–2375. [Google Scholar]
- Thakar, S.; Rajendran, P.; Kim, H.; Kabir, A.M.; Gupta, S.K. Accelerating Bi-Directional Sampling-Based Search for Motion Planning of Non-Holonomic Mobile Manipulators. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 24 October 2020–24 January 2021; pp. 6711–6717. [Google Scholar] [CrossRef]
- Joshi, S.S.; Tsiotras, P. Relevant Region Exploration On General Cost-maps For Sampling-Based Motion Planning. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 24 October 2020–24 January 2021; pp. 6689–6695. [Google Scholar] [CrossRef]
- LaValle, S.M. From dynamic programming to RRTs: Algorithmic design of feasible trajectories. In Control Problems in Robotics; Springer: Berlin/Heidelberg, Germany, 2003; pp. 19–37. [Google Scholar]
- hwan Jeon, J.; Cowlagi, R.V.; Peters, S.C.; Karaman, S.; Frazzoli, E.; Tsiotras, P.; Iagnemma, K. Optimal motion planning with the half-car dynamical model for autonomous high-speed driving. In Proceedings of the 2013 American Control Conference, Washington, DC, USA, 17–19 June 2013; pp. 188–193. [Google Scholar]
- Armstrong, D.; Jonasson, A. AM-RRT*: Informed Sampling-based Planning with Assisting Metric. arXiv
**2020**, arXiv:2010.14693. [Google Scholar] - Luders, B.D.; Karaman, S.; Frazzoli, E.; How, J.P. Bounds on tracking error using closed-loop rapidly-exploring random trees. In Proceedings of the 2010 American Control Conference, Baltimore, MD, USA, 30 June–2 July 2010; pp. 5406–5412. [Google Scholar]
- Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res.
**2011**, 30, 846–894. [Google Scholar] [CrossRef] - Samaniego, R.; Rodríguez, R.; Vázquez, F.; López, J. Efficient Path Planing for Articulated Vehicles in Cluttered Environments. Sensors
**2020**, 20, 6821. [Google Scholar] [CrossRef] [PubMed] - Schmid, L.; Pantic, M.; Khanna, R.; Ott, L.; Siegwart, R.; Nieto, J. An Efficient Sampling-Based Method for Online Informative Path Planning in Unknown Environments. IEEE Robot. Autom. Lett.
**2020**, 5, 1500–1507. [Google Scholar] [CrossRef][Green Version] - Moon, C.; Chung, W. Kinodynamic Planner Dual-Tree RRT (DT-RRT) for Two-Wheeled Mobile Robots Using the Rapidly Exploring Random Tree. IEEE Trans. Ind. Electron.
**2015**, 62, 1080–1090. [Google Scholar] [CrossRef] - Kanayama, Y.; Kimura, Y.; Miyazaki, F.; Noguchi, T. A stable tracking control method for an autonomous mobile robot. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Cincinnati, OH, USA, 13–18 May 1990; pp. 384–389. [Google Scholar]
- De Luca, A.; Oriolo, G.; Vendittelli, M. Control of wheeled mobile robots: An experimental overview. Ramsete
**2001**, 270, 181–226. [Google Scholar] - Amidi, O.; Thorpe, C.E. Integrated mobile robot control. In Proceedings of the SPIE, The International Society for Optical Engineering, Boston, MA, USA, 1 March 1991; Volume 1388, pp. 504–523. [Google Scholar]

RRT [10] | CL-RRT [25] | High Speed RRT* [23] | DT-RRT [29] | CDT-RRT* | |
---|---|---|---|---|---|

Car-like model | None | Good | Good | None | Good |

Piecewise motion | None | None | None | Good | Good |

Goal-biased sampling | Good | Fair | Fair | Good | Good |

Cluttered Environments | Good | Fair | Fair | Fair | Good |

Replanning | None | Fair | Fair | Good | Good |

Saving orphan nodes | None | None | None | Good | Good |

Parameters | Values | Specifications | |
---|---|---|---|

${v}_{max}$ | 2.7 m/s | CPU | Intel i7-2620M 2.7 Ghz |

${a}_{max}$ | 1.8 m/s${}^{2}$ | RAM | 4 GB (1333 MHz) |

$\mathbf{L}$ | 2.8 m | OS | Ubuntu 11.04 |

$[{\delta}_{min},{\delta}_{max}]$ | $[-{30}^{\circ},{30}^{\circ}]$ | Middleware | Robot Operating System |

${\dot{\delta}}_{max}$ | ${20}^{\circ}/$s${}^{2}$ | Language | C++ |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 8.62 | 7.93 | 0.02 | 27.59 | 66 |

Travel time | 35.01 | 7.15 | 19.20 | 49.80 | ||

DT-RRT | Computing time | 0.36 | 0.20 | 0.06 | 1.04 | 0 |

Travel time | 18.67 | 0.62 | 17.30 | 19.30 | ||

CDT-RRT* | Computing time | 0.36 | 0.52 | 0.07 | 5.00 | 0 |

Travel time | 18.87 | 0.82 | 17.30 | 21.70 |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 29.89 | 1.11 | 18.90 | 30.00 | 99 |

Travel time | 85.50 | 0.00 | 85.50 | 85.50 | ||

T-RRT | Computing time | 12.08 | 12.19 | 0.60 | 30.00 | 26 |

Travel time | 58.79 | 2.22 | 55.70 | 73.30 | ||

CDT-RRT* | Computing time | 2.79 | 1.47 | 0.70 | 8.60 | 0 |

Travel time | 57.91 | 1.07 | 55.70 | 61.30 |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 29.81 | 1.93 | 10.71 | 30.00 | 99 |

Travel time | 62.70 | 0.00 | 62.70 | 62.70 | ||

DT-RRT | Computing time | 10.20 | 12.30 | 0.20 | 30.00 | 23 |

Travel time | 40.56 | 1.91 | 35.70 | 45.20 | ||

CDT-RRT* | Computing time | 6.78 | 7.03 | 0.32 | 30.00 | 3 |

Travel time | 40.71 | 4.71 | 35.20 | 61.30 |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 30.00 | - | 30.00 | 30.00 | 100 |

Travel time | - | - | - | - | ||

DT-RRT | Computing time | 14.50 | 12.39 | 0.98 | 30.00 | 36 |

Travel time | 72.83 | 11.60 | 58.30 | 102.70 | ||

CDT-RRT* | Computing time | 6.46 | 3.44 | 1.60 | 17.34 | 0 |

Travel time | 66.54 | 8.52 | 57.60 | 86.00 |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 30.00 | - | 30.00 | 30.00 | 100 |

Travel time | - | - | - | - | ||

DT-RRT | Computing time | 7.61 | 10.03 | 0.90 | 30.00 | 14 |

Travel time | 62.40 | 3.37 | 57.50 | 74.60 | ||

CDT-RRT* | Computing time | 5.25 | 2.06 | 2.20 | 11.10 | 0 |

Travel time | 61.33 | 4.22 | 56.00 | 75.70 |

Mean (s) | $\mathit{\sigma}$ | Min (s) | Max (s) | Failure Count | ||
---|---|---|---|---|---|---|

RRT | Computing time | 30.00 | - | 30.00 | 30.00 | 100 |

Travel time | - | - | - | - | ||

DT-RRT | Computing time | 3.53 | 3.78 | 1.23 | 30.00 | 1 |

Travel time | 47.10 | 5.19 | 42.60 | 66.90 | ||

CDT-RRT* | Computing time | 1.74 | 0.69 | 0.60 | 4.45 | 0 |

Travel time | 46.43 | 4.29 | 42.60 | 61.80 |

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

© 2021 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**

Kwon, H.; Cha, D.; Seong, J.; Lee, J.; Chung, W. Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments. *Sensors* **2021**, *21*, 4828.
https://doi.org/10.3390/s21144828

**AMA Style**

Kwon H, Cha D, Seong J, Lee J, Chung W. Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments. *Sensors*. 2021; 21(14):4828.
https://doi.org/10.3390/s21144828

**Chicago/Turabian Style**

Kwon, Hyunki, Donggeun Cha, Jihoon Seong, Jinwon Lee, and Woojin Chung. 2021. "Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments" *Sensors* 21, no. 14: 4828.
https://doi.org/10.3390/s21144828