A Motion Planning Method for Unmanned Surface Vehicle Based on Improved RRT Algorithm
Abstract
:1. Introduction
2. Preliminaries
2.1. USV Motion Mathematical Model
- (1)
- USV is symmetric about the longitudinal section amidships. The rigid body mass is constant and uniformly distributed.
- (2)
- Ignore the higher order damping terms and assume that the restoring force matrix is a zero array.
2.2. Motion Planning Formulation
3. Main Algorithms
3.1. Extension Rule Based on State Prediction
Algorithm 1 Extend function |
1. Function |
2. |
3. |
4. if then |
5. for each moment in do |
6. |
7. if then |
8. |
9. 10. return |
11. End Function |
3.2. Sampling Performance Enhancement
3.3. Algorithm Summary
Algorithm 2 spRRT-Informed |
Input: , , , , map, |
Output: , |
1. |
2. |
3. Flag |
4. while () |
5. if Flag == TRUE then |
6. ; |
7. ; |
8. |
9. ; |
10. if Flag == FALSE then |
11. |
12. ; |
13. |
14. ; |
15. return; |
- (1)
- Initialization Stage, Line 1–2, Algorithm 2: The spRRT-Informed algorithm maintains both the spatial search tree and the auxiliary extension tree . The root nodes of both trees, and , are initialized at the start point.
- (2)
- Sampling Stage, Line 3, 5–7 or Line 3, 10–12, Algorithm 2: For the purpose of conducting official sampling, it is necessary to determine whether obstacles prevent access to the goal position and the start point. As described in Section 3.2, if the concatenation is blocked, is initialized and the double elliptic sampling domain is used. Conversely, a single elliptic sampling domain is used. Different types of elliptic domains follow similar sampling steps: The shortest distance (the two distances in the case of the double elliptic sampling domain, and ) is output by the basic RRT algorithm and saved in the database. After the elliptic sampling domain is constructed, the random position is sampled and output. The value of (the two distances in the case of the double elliptic sampling domain, and ) is updated only when the basic RRT outputs a shorter distance.
- (3)
- Extension Stage, Line 8 or Line 13, Algorithm 2: This stage has been described in Section 3.1 in more detail. As shown in Figure 6, the blue dashed line indicates the predicted states. If falls within the goal region, then the path will be backtracked to the start point. According to Figure 6, the path produced by the backtracking is valid.
- (4)
- Optimization Stage, Line 9 or Line 14, Algorithm 2: Because of the inclusion of flow field disturbances, the path cost is replaced by the time cost [37]. In order to reduce time costs, the structure of the two trees is optimized at the end of each iteration. If a child node is unfeasible, it can be repropagated or deleted [38]. During each iteration, the success rate of extending the predicted states can only be increased by as many child nodes as possible. Figure 7 illustrates the process of optimizing the tree structure: As shown in Figure 7a, an optimization region is defined with the node as its center and as its radius. A traversal of the tree is performed in order to identify all nodes within the optimization region except for the parent node of . Next, the tree mapping of is used as the initial state to predict states. If the cost of the replacement state point is less than the original cost, as shown in Figure 7b,c, the wiring of the two trees is updated. In the end, the parents of the updated node in both trees are passed to the child as new parents, thus ending this optimization.
4. Simulation Results
4.1. Performance Analysis of Algorithms
4.1.1. Scenario 1
4.1.2. Scenario 2
4.1.3. Scenario 3
4.1.4. Dishui Lake scenario
5. Conclusions
Author Contributions
Funding
Institutional Review Board Statement
Informed Consent Statement
Data Availability Statement
Conflicts of Interest
References
- Liu, Y.; Bucknall, R.; Zhang, X. The fast marching method based intelligent navigation of an unmanned surface vehicle. Ocean Eng. 2017, 142, 363–376. [Google Scholar] [CrossRef] [Green Version]
- Wang, N.; Sun, Z.; Yin, J.; Zou, Z.; Su, S.-F. Fuzzy unknown observer-based robust adaptive path following control of underactuated surface vehicles subject to multiple unknowns. Ocean Eng. 2019, 176, 57–64. [Google Scholar] [CrossRef]
- Guan, G.; Wang, L.; Geng, J.; Zhuang, Z.; Yang, Q. Parametric automatic optimal design of usv hull form with respect to wave resistance and seakeeping. Ocean Eng. 2021, 235, 109462. [Google Scholar] [CrossRef]
- Feng, Z.; Pan, Z.; Chen, W.; Liu, Y.; Leng, J. Usv application scenario expansion based on motion control, path following and velocity planning. Machines 2022, 10, 310. [Google Scholar] [CrossRef]
- Yu, K.; Liang, X.-F.; Li, M.-Z.; Chen, Z.; Yao, Y.-L.; Li, X.; Zhao, Z.-X.; Teng, Y. Usv path planning method with velocity variation and global optimisation based on ais service platform. Ocean Eng. 2021, 236, 109560. [Google Scholar] [CrossRef]
- Zhou, C.H.; Gu, S.D.; Wen, Y.Q.; Du, Z.; Xiao, C.S.; Huang, L.; Zhu, M. The review unmanned surface vehicle path planning: Based on multi-modality constraint. Ocean Eng. 2020, 200, 14. [Google Scholar] [CrossRef]
- Aggarwal, S.; Kumar, N. Path planning techniques for unmanned aerial vehicles: A review, solutions, and challenges. Comput. Commun. 2020, 149, 270–299. [Google Scholar] [CrossRef]
- Majeed, A.; Lee, S. A fast global flight path planning algorithm based on space circumscription and sparse visibility graph for unmanned aerial vehicle. Electronics 2018, 7, 375. [Google Scholar] [CrossRef] [Green Version]
- Mao, S.; Yang, P.; Gao, D.; Liu, Z. Global path planning for unmanned surface vehicle based on bacterial foraging-improved ant colony hybrid algorithm. Control Eng. China 2022, 1, 1–9. [Google Scholar] [CrossRef]
- Song, R.; Liu, Y.; Bucknall, R. Smoothed a* algorithm for practical unmanned surface vehicle path planning. Appl. Ocean Res. 2019, 83, 9–20. [Google Scholar] [CrossRef]
- Niu, H.; Lu, Y.; Savvaris, A.; Tsourdos, A. Efficient path planning algorithms for unmanned surface vehicle. IFAC-PapersOnLine 2016, 49, 121–126. [Google Scholar] [CrossRef]
- Wu, G.; Atilla, I.; Tahsin, T.; Terziev, M.; Wang, L. Long-voyage route planning method based on multi-scale visibility graph for autonomous ships. Ocean Eng. 2021, 219, 108242. [Google Scholar] [CrossRef]
- Yao, P.; Zhao, R.; Zhu, Q. A hierarchical architecture using biased min-consensus for usv path planning. IEEE Trans. Veh. Technol. 2020, 69, 9518–9527. [Google Scholar] [CrossRef]
- Zhao, L.; Wang, F.; Bai, Y. Route planning for autonomous vessels based on improved artificial fish swarm algorithm. Ships Offshore Struct. 2022. [Google Scholar] [CrossRef]
- Wang, N.; Jin, X.Z.; Er, M.J. A multilayer path planner for a usv under complex marine environments. Ocean Eng. 2019, 184, 1–10. [Google Scholar] [CrossRef]
- Svec, P.; Schwartz, M.; Thakur, A.; Gupta, S.K. Trajectory planning with look-ahead for unmanned sea surface vehicles to handle environmental disturbances. In Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, CA, USA, 25–30 September 2011; pp. 1154–1159. [Google Scholar]
- Gao, D.; Zhou, P.; Shi, W.; Wang, T.; Wang, Y. A Dynamic Obstacle Avoidance Method for Unmanned Surface Vehicle under the International Regulations for Preventing Collisions at Sea. J. Mar. Sci. Eng. 2022, 10, 901. [Google Scholar] [CrossRef]
- Du, Z.; Wen, Y.; Xiao, C.; Zhang, F.; Huang, L.; Zhou, C. Motion planning for Unmanned Surface Vehicle based on Trajectory Unit. Ocean Eng. 2018, 151, 46–56. [Google Scholar] [CrossRef]
- Du, Z.; Wen, Y.; Xiao, C.; Huang, L.; Zhou, C.; Zhang, F. Trajectory-cell based method for the unmanned surface vehicle motion planning. Appl. Ocean Res. 2019, 86, 207–221. [Google Scholar] [CrossRef]
- La Valle, S.M. Motion Planning. IEEE Robot. Autom. Mag. 2011, 18, 108–118. [Google Scholar] [CrossRef]
- Moon, C.-B.; Chung, W. Kinodynamic Planner Dual-Tree RRT (DT-RRT) for Two-Wheeled Mobile Robots Using the Rapidly Exploring Random Tree. IEEE Trans. Ind. Electron. 2014, 62, 1080–1090. [Google Scholar] [CrossRef]
- Wang, J.; Chi, W.; Li, C.; Meng, M.Q.-H. Efficient Robot Motion Planning Using Bidirectional-Unidirectional RRT Extend Function. IEEE Trans. Autom. Sci. Eng. 2021, 19, 1859–1868. [Google Scholar] [CrossRef]
- Chiang, H.-T.L.; Hsu, J.; Fiser, M.; Tapia, L.; Faust, A. RL-RRT: Kinodynamic Motion Planning via Learning Reachability Estimators from RL Policies. IEEE Robot. Autom. Lett. 2019, 4, 4298–4305. [Google Scholar] [CrossRef] [Green Version]
- Li, Y.; Cui, R.; Li, Z.; Xu, D. Neural Network Approximation Based Near-Optimal Motion Planning with Kinodynamic Constraints Using RRT. IEEE Trans. Ind. Electron. 2018, 65, 8718–8729. [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] [CrossRef]
- Blanco, J.L.; Bellone, M.; Gimenez, A. TP-Space RRT—Kinematic Path Planning of Non-Holonomic Any-Shape Vehicles. Int. J. Adv. Robot. Syst. 2015, 12, 55. [Google Scholar] [CrossRef]
- Wang, J.; Li, B.; Meng, M.Q.-H. Kinematic Constrained Bi-directional RRT with Efficient Branch Pruning for robot path planning. Expert Syst. Appl. 2020, 170, 114541. [Google Scholar] [CrossRef]
- Han, S.; Wang, L.; Wang, Y.; He, H. An efficient motion planning based on grid map: Predicted Trajectory Approach with global path guiding. Ocean Eng. 2021, 238, 109696. [Google Scholar] [CrossRef]
- Chu, Z.; Wang, F.; Lei, T.; Luo, C. Path Planning Based on Deep Reinforcement Learning for Autonomous Underwater Vehicles Under Ocean Current Disturbance. IEEE Trans. Intell. Veh. 2022, 8, 108–120. [Google Scholar] [CrossRef]
- MahmoudZadeh, S.; Abbasi, A.; Yazdani, A.; Wang, H.; Liu, Y. Uninterrupted path planning system for Multi-USV sampling mission in a cluttered ocean environment. Ocean Eng. 2022, 254, 111328. [Google Scholar] [CrossRef]
- Tan, G.; Zhuang, J.; Zou, J.; Wan, L. Adaptive adjustable fast marching square method based path planning for the swarm of heterogeneous unmanned surface vehicles (USVs). Ocean Eng. 2023, 268, 113432. [Google Scholar] [CrossRef]
- Breivik, M. Nonlinear Maneuvering Control of Underactuated Ships. Master’s Thesis, Norwegian University of Science and Technology, Trondheim, Norway, 2003; pp. 10–30. [Google Scholar]
- Fossen, T.I. Handbook of Marine Craft Hydrodynamics and Motion Control; John Wiley & Sons: New York, NY, USA, 2011. [Google Scholar]
- Ma, Y.; Hu, M.; Yan, X. Multi-objective path planning for unmanned surface vehicle with currents effects. ISA Trans. 2018, 75, 137–156. [Google Scholar] [CrossRef]
- Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 2011, 30, 846–894. [Google Scholar] [CrossRef] [Green Version]
- Gammell, J.D.; Srinivasa, S.S.; Barfoot, T.D. Informed rrt: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 2997–3004. [Google Scholar]
- Guo, X.; Ji, M.; Zhao, Z.; Wen, D.; Zhang, W. Global path planning and multi-objective path control for unmanned surface vehicle based on modified particle swarm optimization (PSO) algorithm. Ocean Eng. 2020, 216, 107693. [Google Scholar] [CrossRef]
- Jeon, J.H.; Karaman, E.S.; Frazzoli, E. Anytime computation of time-optimal off-road vehicle maneuvers using the rrt. In Proceedings of the 2011 50th IEEE Conference on Decision and Control and European Control Conference, Orlando, FL, USA, 12–15 December 2011; pp. 3276–3282.
- Skjetne, R.; Smogeli, Ø.N.; Fossen, T.I. A Nonlinear Ship Manoeuvering Model: Identification and adaptive control with experiments for a model ship. Model. Identif. Control 2004, 25, 3–27. [Google Scholar] [CrossRef] [Green Version]
Description | Value |
---|---|
Unit predicted time | 1 s |
Period predicted time | 10 s |
Resolution of yaw moment | 0.02 N × m |
Maximum values of yaw moment | 0.2 N × m |
Minimum values of yaw momen | −0.2 N × m |
Surge force | 2 N × m |
Longitudinal current field velocity | 0.2 m/s |
Transverse current field velocity | 0.1 m/s |
Methods | Performances | Mean | Min | Max | σ |
---|---|---|---|---|---|
RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 57.559 | 55.4969 | 58.972 | 0.908 | |
Steering angle (rad) | 0.284 | 0.002 | 0.957 | 0.202 | |
Informed-RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 55.099 | 54.818 | 56.385 | 0.485 | |
Steering angle (rad) | 0.155 | 4.795 × 10−4 | 0.937 | 0.149 | |
spRRT | Sailing time (s) | 124.40 | 106.2 | 138.6 | 8.916 |
Distance (m) | 71.618 | 61.120 | 92.576 | 8.795 | |
Steering angle (rad) | 0.0920 | 3.338 × 10−4 | 2.264 | 0.335 | |
spRRT- Informed | Sailing time (s) | 106.92 | 103.8 | 110.0 | 0.885 |
Distance (m) | 62.212 | 60.114 | 64.301 | 1.169 | |
Steering angle (rad) | 0.0696 | 1.030 × 10−4 | 1.038 | 0.229 |
Methods | Performances | Mean | Min | Max | σ |
---|---|---|---|---|---|
RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 74.946 | 70.407 | 82.222 | 2.098 | |
Steering angle (rad) | 0.382 | 8.553 × 10−4 | 1.941 | 0.367 | |
Informed-RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 71.372 | 69.058 | 72.989 | 0.564 | |
Steering angle (rad) | 0.320 | 1.850 × 10−4 | 1.766 | 0.347 | |
spRRT | Sailing time (s) | 176.3 | 150.0 | 258.0 | 29.570 |
Distance (m) | 100.563 | 80.869 | 139.871 | 15.988 | |
Steering angle (rad) | 0.0878 | 3.103 × 10−4 | 1.968 | 0.287 | |
spRRT- Informed | Sailing time (s) | 154.48 | 138.0 | 170.4 | 4.469 |
Distance (m) | 93.890 | 79.636 | 108.512 | 8.385 | |
Steering angle (rad) | 0.0786 | 4.671 × 10−4 | 1.343 | 0.259 |
Methods | Performances | Mean | Min | Max | σ |
---|---|---|---|---|---|
RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 53.020 | 51.759 | 54.216 | 0.661 | |
Steering angle (rad) | 0.165 | 4.130 × 10−4 | 1.010 | 0.143 | |
Informed-RRT* | Sailing time (s) | - | - | - | - |
Distance (m) | 52.098 | 51.608 | 52.791 | 0.329 | |
Steering angle (rad) | 0.038 | 2.230 × 10−4 | 0.226 | 0.041 | |
spRRT | Sailing time (s) | 99.54 | 91.6 | 112.0 | 5.397 |
Distance (m) | 57.224 | 52.756 | 64.453 | 3.119 | |
Steering angle (rad) | 0.0562 | 2.110 × 10−4 | 1.327 | 0.188 | |
spRRT- Informed | Sailing time (s) | 92.2 | 89.0 | 96.4 | 1.778 |
Distance (m) | 53.214 | 51.644 | 56.464 | 1.027 | |
Steering angle (rad) | 0.0571 | 1.055 × 10−4 | 0.899 | 0.167 |
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content. |
© 2023 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
Mao, S.; Yang, P.; Gao, D.; Bao, C.; Wang, Z. A Motion Planning Method for Unmanned Surface Vehicle Based on Improved RRT Algorithm. J. Mar. Sci. Eng. 2023, 11, 687. https://doi.org/10.3390/jmse11040687
Mao S, Yang P, Gao D, Bao C, Wang Z. A Motion Planning Method for Unmanned Surface Vehicle Based on Improved RRT Algorithm. Journal of Marine Science and Engineering. 2023; 11(4):687. https://doi.org/10.3390/jmse11040687
Chicago/Turabian StyleMao, Shouqi, Ping Yang, Diju Gao, Chunteng Bao, and Zhenyang Wang. 2023. "A Motion Planning Method for Unmanned Surface Vehicle Based on Improved RRT Algorithm" Journal of Marine Science and Engineering 11, no. 4: 687. https://doi.org/10.3390/jmse11040687