# Dynamical Analysis of a Navigation Algorithm

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

#### 1.1. Localization

#### 1.2. Path Following

#### 1.3. Path Planning

## 2. Materials and Methods

_{path}(rad). The positioning error noted as e (m), refers to the minimum distance between the autonomous vehicle and the closest point of the trajectory in reference to the vehicle as represented in Figure 2.

_{setpoint}(rad) is necessary, to adapt the range of values of φ. Its value is defined in Equation (2):

_{1}(s

^{−1}) and K

_{2}(1/m), which are to be determined. It can be appreciated that the constant K

_{1}is the one related to the alignment error φ and K

_{2}controls the value of the positioning error e.

_{path}.

_{1}and K

_{2}in Equations (1) and (2). The Lyapunov energy equation noted as L (m

^{2}) is proposed based on the previous equations in Equation (5).

_{1}and K

_{2}are applied to the lateral control, analysing how the AGV behaves as a function of these coefficients. To simplify the analysis, a trajectory of the form Ax + By + C = 0 will be considered.

#### Distinction of the Positioning Error Sign

## 3. Proposal Explanation

#### 3.1. Necessary Data Acquisition

#### 3.2. Concurrency in the Approach

_{optimun}(rad) for the autonomous vehicle. This value is not the same as θ, and this is where K

_{1}comes in. θ

_{optimun}refers to the sum between φ

_{path}and φ

_{setpoint}(e). Accordingly, because of the fast dynamic, the AGV orients itself on the way of the trajectory rapidly, being able to consider that the path has no inclination. This is depicted in Figure 6. In the case of the positioning error, it is not possible to make an approximation, and the path will be considered to be angled. In conclusion, it can be declared that the time constants of the orientation loop are smaller than those of the displacement. Ergo, two situations are envisaged as represented in Figure 6a,b.

_{1}and K

_{2}values.

## 4. Value of K_{1}

#### 4.1. System Stability Study

_{path}= 0, as shown in Equation (13).

_{2}will always be positive, so the following remarks can be made.

#### 4.2. Procurement of Value of K_{1}

_{path}. Therefore, one can formulate the integral of Equation (20), which remains a linear system.

_{1}can be known. In Equations (24) and (25) the integral is noted as I.

_{1}are related. Analysing Equations (23) and (25), the following conclusions can be drawn. If the value of K

_{1}is very aggressive, the exponential tends to 0 quickly obtaining a sinusoidal function with value 0 and making the expression vanish very briefly. This makes it independent of the value V that is set. On the contrary, if K

_{1}is small, the velocity is limited. Otherwise, an undesired oscillation system would appear.

_{1}= 1 s

^{−1}are set for plotting. It can be visualized how the value of the integral (red line) takes time to fade out, in the order of seconds. In this case, the value of y admits an extremely significant positive value which does not ensure stability as it cannot be guaranteed to be lower than y(0). In the plot of Figure 7b, V = 10 m/s and K

_{1}= 1000 s

^{−1}are set. The value of the integral disappears instantly, ensuring the stability of the system and regardless of the velocity value set.

_{1}is set to 1000 s

^{−1}. This value refers to the gain of the alignment error control loop.

## 5. Value of K_{2}

#### 5.1. System Stability Study

_{1}value, once again a non-linear system is observed, so Lyapunov is employed in order to ensure stability.

_{1}is already defined, it can be ignored.

_{path}must also attend a function that depends on the positioning error, due to the position (x, y) of the AGV. Depending on this, the most adjacent point of the path will vary. In this case, the positioning error is calculated as in Equations (27) and (28).

_{2}is varied, allowing its value to be dictated. The following system is considered at instant t.

_{2}, it is possible to determine some expression at t + dt, taking into account Equations (3) and (4).

^{2}) value. The derivative of L in Figure 8b is negative throughout the space considered, coinciding with the value of the variation of the positioning error. Therefore, the stability of the system is confirmed.

#### 5.2. Optimization of Value of K_{2}

_{2}in a range for the whole amount $\overrightarrow{p}$. By setting a value of K

_{2}it is possible to visualize the evolution of Δe, and the optimal value can be acquired. In Equations (36) and (37), a cost function is proposed that depends on the mean square error, denoted as J.

_{2}, formulated in Equation (39).

_{2}values presented in Figure 9.

_{2}attends the parameter that acts on the rate of evolution of the φ

_{setpoint}. Accordingly, this value is set as K

_{2}= 1.21 (1/m).

## 6. Longitudinal Control Algorithm

_{T}(m/s

^{2}) and a normal acceleration, denoted as a

_{N}(m/s

^{2}). The latter can be defined as in Equation (40), from which $\rho $ (m) can be known.

_{N}needs to be under maximum speeding up, called a

_{Nmax}(m/s

^{2}), as in Equation (41).

_{1}and K

_{2}, so a maximum velocity can be fixed as well, as in Equation (43).

^{2}).

_{T}, as seen in Equation (40).

_{max}″. Taking into account Equation (44), Equation (48) is defined.

_{max}′ is provided by the AGV, representing the maximum nominal velocity.

## 7. Results

_{nom}of the AGV. In the case of a curve in the path, it can be observed that the autonomous vehicle reduces its speed to prevent deviating from the predetermined path. Simultaneously, it is making a constant redirection to adhere to the reduced value of alignment error.

^{®}Core™ i9-9880H CPU @ 2.30 GHz 3.30 GHz. The RAM memory of the computer on which the algorithm is executed is 16 GB and it has an NVIDIA Quadro T1000 graphics card.

## 8. Discussion

## 9. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Acknowledgments

## Conflicts of Interest

## References

- Kilic, C.; Ohi, N.; Gu, Y.; Gross, J. Slip-Based Autonomous ZUPT through Gaussian Process to Improve Planetary Rover Localization. IEEE Robot. Autom. Lett.
**2021**, 6, 4782–4789. [Google Scholar] [CrossRef] [PubMed] - Chen, S.Y. Kalman filter for robot vision: A survey. IEEE Trans. Ind. Electron.
**2012**, 59, 4409–4420. [Google Scholar] [CrossRef] - Cho, H.; Kim, E.K.; Kim, S. Indoor SLAM application using geometric and ICP matching methods based on line features. Rob. Auton. Syst.
**2018**, 100, 206–224. [Google Scholar] [CrossRef] - Shamsfakhr, F.; Sadeghi Bigham, B. GSR: Geometrical scan registration algorithm for robust and fast robot pose estimation. Assem. Autom.
**2020**, 40, 801–817. [Google Scholar] [CrossRef] - Naus, K.; Marchel, Ł. Use of a weighted ICP algorithm to precisely determine USV movement parameters. Appl. Sci.
**2019**, 9, 3530. [Google Scholar] [CrossRef] [Green Version] - Wang, Y.T.; Peng, C.C.; Ravankar, A.A.; Ravankar, A. A single LiDAR-based feature fusion indoor localization algorithm. Sensors
**2018**, 18, 1294. [Google Scholar] [CrossRef] [Green Version] - Senin, N.; Colosimo, B.M.; Pacella, M. Point set augmentation through fitting for enhanced ICP registration of point clouds in multisensor coordinate metrology. Robot. Comput. Integr. Manuf.
**2013**, 29, 39–52. [Google Scholar] [CrossRef] - Gao, Y.; Liu, S.; Atia, M.M.; Noureldin, A. INS/GPS/LiDAR integrated navigation system for urban and indoor environments using hybrid scan matching algorithm. Sensors
**2015**, 15, 23286–23302. [Google Scholar] [CrossRef] [Green Version] - Kim, H.; Song, S.; Myung, H. GP-ICP: Ground Plane ICP for Mobile Robots. IEEE Access
**2019**, 7, 76599–76610. [Google Scholar] [CrossRef] - Sadeghi Bigham, B.; Dolatikalan, S.; Khastan, A. Minimum landmarks for robot localization in orthogonal environments. Evol. Intell.
**2021**, 1, 1–4. [Google Scholar] [CrossRef] - Yap, Y.Y.; Khoo, B.E. Landmark-based Automated Guided Vehicle Localization Algorithm for Warehouse Application. Pervasive Health Pervasive Comput. Technol. Healthc.
**2019**, 47–54. [Google Scholar] [CrossRef] - Gao, X.; Zhang, T. Robust RGB-D simultaneous localization and mapping using planar point features. Rob. Auton. Syst.
**2015**, 72, 1–14. [Google Scholar] [CrossRef] - Clemens, J.; Kluth, T.; Reineking, T. β-SLAM: Simultaneous localization and grid mapping with beta distributions. Inf. Fusion
**2019**, 52, 62–75. [Google Scholar] [CrossRef] - Gentner, C.; Jost, T.; Wang, W.; Zhang, S.; Dammann, A.; Fiebig, U.C. Multipath Assisted Positioning with Simultaneous Localization and Mapping. IEEE Trans. Wirel. Commun.
**2016**, 15, 6104–6117. [Google Scholar] [CrossRef] [Green Version] - Yang, P.; Wu, W. Efficient particle filter localization algorithm in dense passive RFID tag environment. IEEE Trans. Ind. Electron.
**2014**, 61, 5641–5651. [Google Scholar] [CrossRef] - Zhang, Q.B.; Wang, P.; Chen, Z.H. An improved particle filter for mobile robot localization based on particle swarm optimization. Expert Syst. Appl.
**2019**, 135, 181–193. [Google Scholar] [CrossRef] - Carrera Villacres, J.L.; Zhao, Z.; Braun, T.; Li, Z. A Particle Filter-Based Reinforcement Learning Approach for Reliable Wireless Indoor Positioning. IEEE J. Sel. Areas Commun.
**2019**, 37, 2457–2473. [Google Scholar] [CrossRef] - Wang, L. Automatic control of mobile robot based on autonomous navigation algorithm. Artif. Life Robot.
**2019**, 24, 494–498. [Google Scholar] [CrossRef] - Tao, B.; Wu, H.; Gong, Z.; Yin, Z.; Ding, H. An RFID-Based Mobile Robot Localization Method Combining Phase Difference and Readability. IEEE Trans. Autom. Sci. Eng.
**2021**, 18, 1406–1416. [Google Scholar] [CrossRef] - Lu, S.; Xu, C.; Zhong, R.Y. An Active RFID Tag-Enabled Locating Approach with Multipath Effect Elimination in AGV. IEEE Trans. Autom. Sci. Eng.
**2016**, 13, 1333–1342. [Google Scholar] [CrossRef] - Kashyap, A.K.; Parhi, D.R.; Muni, M.K.; Pandey, K.K. A hybrid technique for path planning of humanoid robot NAO in static and dynamic terrains. Appl. Soft Comput. J.
**2020**, 96, 106581. [Google Scholar] [CrossRef] - Brock, O.; Khatib, O. High-speed navigation using the global dynamic window approach. In Proceedings of the 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), Detroit, MI, USA, 10–15 May 1999; Volume 1, pp. 341–346. [Google Scholar] [CrossRef]
- Liu, L.S.; Lin, J.F.; Yao, J.X.; He, D.W.; Zheng, J.S.; Huang, J.; Shi, P. Path Planning for Smart Car Based on Dijkstra Algorithm and Dynamic Window Approach. Wirel. Commun. Mob. Comput.
**2021**, 2021, 8881684. [Google Scholar] [CrossRef] - Dobrevski, M.; Skocaj, D. Adaptive dynamic window approach for local navigation. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 24 October–24 January 2021; pp. 6930–6936. [Google Scholar] [CrossRef]
- Fox, D.; Burgard, W.; Thrun, S. The dynamic window approach to collision avoidance. IEEE Robot. Autom. Mag.
**1997**, 4, 23–33. [Google Scholar] [CrossRef] [Green Version] - Wang, T.; Yan, X.; Wang, Y.; Wu, Q. A distributed model predictive control using virtual field force for multi-ship collision avoidance under COLREGs. In Proceedings of the 2017 4th International Conference on Transportation Information and Safety (ICTIS), Banff, AB, Canada, 8–10 August 2017; pp. 296–305. [Google Scholar] [CrossRef]
- Burgos, E.; Bhandari, S. Potential flow field navigation with virtual force field for UAS collision avoidance. In Proceedings of the 2016 International Conference on Unmanned Aircraft Systems ICUAS 2016, Arlington, VA, USA, 7–10 June 2016; pp. 505–513. [Google Scholar] [CrossRef]
- Borenstein, J.; Koren, Y. The Vector Field Histogram—Fast obstacle avoidance for mobile robots. IEEE J. Robot. Autom.
**1991**, 7, 278–288. [Google Scholar] [CrossRef] [Green Version] - Ye, C. Navigating a mobile robot by a traversability field histogram. IEEE Trans. Syst. Man, Cybern. Part B Cybern.
**2007**, 37, 361–372. [Google Scholar] [CrossRef] - AbdElmoniem, A.; Osama, A.; Abdelaziz, M.; Maged, S.A. A path-tracking algorithm using predictive Stanley lateral controller. Int. J. Adv. Robot. Syst.
**2020**, 17, 1–11. [Google Scholar] [CrossRef] - Fei, W.; Ziwei, W.; Meijin, L. Robot Path Planning Based on Improved Particle Swarm Optimization. In Proceedings of the 2021 IEEE 2nd International Conference on Big Data, Artificial Intelligence and Internet of Things Engineering ICBAIE 2021, Nanchang, China, 26–28 March 2021; Volume 21, pp. 887–891. [Google Scholar] [CrossRef]
- Liu, Z.; Liu, H.; Lu, Z.; Zeng, Q. A Dynamic Fusion Pathfinding Algorithm Using Delaunay Triangulation and Improved A-Star for Mobile Robots. IEEE Access
**2021**, 9, 20602–20621. [Google Scholar] [CrossRef] - Cheng, L.; Liu, C.; Yan, B. Improved hierarchical A-star algorithm for optimal parking path planning of the large parking lot. In Proceedings of the ICIA 2014—IEEE International Conference on Information and Automation, Hailar, China, 28–30 July 2014; pp. 695–698. [Google Scholar] [CrossRef]
- da Silva Costa, L.; Tonidandel, F. DVG+A* and RRT Path-Planners: A Comparison in a Highly Dynamic Environment. J. Intell. Robot. Syst.
**2021**, 101, 1–20. [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.
**2021**, 170, 114541. [Google Scholar] [CrossRef] - 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] [Green Version] - Tang, C.; Sun, R.; Yu, S.; Chen, L.; Zheng, J. Autonomous Indoor Mobile Robot Exploration Based on Wavefront Algorithm. Lect. Notes Comput. Sci.
**2019**, 11744, 338–348. [Google Scholar] - Wu, S.; Du, Y.; Zhang, Y. Mobile Robot Path Planning Based on a Generalized Wavefront Algorithm. Math. Probl. Eng.
**2020**, 2020, 6798798. [Google Scholar] [CrossRef] [Green Version] - Sdwk, U.; Edvhg, S.; Ohduqlqj, R.Q.; Dqj, L.; Lqj, L.X.; Hiilflhqf, F.; Wkh, R.I.; Dojrulwkp, S.; Ri, J.; Vlqfh, U.; et al. Mobile robot path planning based on Q-learnig algorithm*. In Proceedings of the 2019 WRC Symposium on Advanced Robotics and Automation (WRC SARA), Beijing, China, 21–22 August 2019; pp. 160–165. [Google Scholar]
- Gao, J.; Ye, W.; Guo, J.; Li, Z. Deep reinforcement learning for indoor mobile robot path planning. Sensors
**2020**, 20, 5493. [Google Scholar] [CrossRef] - Zheng, K.; Gao, J.; Shen, L. UCAV Path Planning Algorithm Based on Deep Reinforcement Learning. Lect. Notes Comput. Sci.
**2019**, 11902, 702–714. [Google Scholar] - Teso-Fz-Betoño, D.; Zulueta, E.; Sánchez-Chica, A.; Fernandez-Gamiz, U.; Saenz-Aguirre, A. Semantic segmentation to develop an indoor navigation system for an autonomous mobile robot. Mathematics
**2020**, 8, 855. [Google Scholar] [CrossRef] - Olgun, M.C.; Baytar, Z.; Akpolat, K.M.; Koray Sahingoz, O. Autonomous vehicle control for lane and vehicle tracking by using deep learning via vision. In Proceedings of the 2018 6th International Conference on Control Engineering and Information Technology, CEIT 2018, Istanbul, Turkey, 25–27 October 2018; pp. 25–27. [Google Scholar] [CrossRef]
- Azimi, S.M.; Fischer, P.; Korner, M.; Reinartz, P. Aerial LaneNet: Lane-Marking Semantic Segmentation in Aerial Imagery Using Wavelet-Enhanced Cost-Sensitive Symmetric Fully Convolutional Neural Networks. IEEE Trans. Geosci. Remote Sens.
**2019**, 57, 2920–2938. [Google Scholar] [CrossRef] [Green Version] - Hoffmann, G.M.; Tomlin, C.J.; Montemerlo, M.; Thrun, S. Autonomous automobile trajectory tracking for off-road driving: Controller design, experimental validation and racing. In Proceedings of the 2007 American Control Conference, New York, NY, USA, 9–13 July 2007; pp. 2296–2301. [Google Scholar] [CrossRef]
- Quan, S.; Chen, J. AGV localization based on odometry and LiDAR. In Proceedings of the 2019 2nd World Conference on Mechanical Engineering and Intelligent Manufacturing (WCMEIM), Shanghai, China, 22–24 November 2019; pp. 483–486. [Google Scholar] [CrossRef]
- Chen, X.; Lin, W.; Liu, J.; Guan, L.; Zheng, Y.; Gao, F. Electromagnetic Guided Factory Intelligent AGV. In Proceedings of the 2016 3rd International Conference on Mechatronics and Information Technology, Shenzhen, China, 9–10 April 2016; pp. 1–6. [Google Scholar] [CrossRef] [Green Version]

**Figure 4.**Process of obtaining trajectory: (

**a**) Image that AGV takes of the path; (

**b**) Semantic segmentation of the path; (

**c**) Medium point of the navigable mask; (

**d**) Obtained path.

**Figure 6.**Path approach: (

**a**) Situation where φ

_{path}is zero; (

**b**) Situation where φ

_{path}is not zero.

**Figure 7.**Results of simulation: (

**a**) Plot when V is high and K

_{1}low; (

**b**) Plot when V is low and K

_{1}high.

**Figure 8.**Results of the simulation: (

**a**) The values of L in all the space; (

**b**) The values of ΔL in all the space.

**Figure 10.**Different path simulation: (

**a**) Sinusoidal with AGV at the right side; (

**b**) Sinusoidal with AGV at the left side; (

**c**) Linear with AGV at the right side; (

**d**) Linear with AGV at the left side; (

**e**) Circular with AGV outside; (

**f**) Circular with AGV inside.

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

Cabezas-Olivenza, M.; Zulueta, E.; Sánchez-Chica, A.; Teso-Fz-Betoño, A.; Fernandez-Gamiz, U.
Dynamical Analysis of a Navigation Algorithm. *Mathematics* **2021**, *9*, 3139.
https://doi.org/10.3390/math9233139

**AMA Style**

Cabezas-Olivenza M, Zulueta E, Sánchez-Chica A, Teso-Fz-Betoño A, Fernandez-Gamiz U.
Dynamical Analysis of a Navigation Algorithm. *Mathematics*. 2021; 9(23):3139.
https://doi.org/10.3390/math9233139

**Chicago/Turabian Style**

Cabezas-Olivenza, Mireya, Ekaitz Zulueta, Ander Sánchez-Chica, Adrian Teso-Fz-Betoño, and Unai Fernandez-Gamiz.
2021. "Dynamical Analysis of a Navigation Algorithm" *Mathematics* 9, no. 23: 3139.
https://doi.org/10.3390/math9233139