# Optimal Maneuvering for Autonomous Vehicle Self-Localization

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Related Work

#### 2.1. Measures of Uncertainty

#### 2.2. Path Optimization

#### 2.2.1. Waypoint Selection

#### 2.2.2. Optimization Algorithms

#### 2.3. Objective Function

#### 2.4. Comparison of the Proposed Algorithm with Existing Literature

## 3. Problem Setup

## 4. Optimal Maneuvering

#### 4.1. State Estimation

#### 4.2. Information Content and Objective Function

#### 4.3. Motion and Trajectory Constraints

**Forward speed constraint**Given a fixed forward speed V,$$\left|\right|{\left[\begin{array}{c}{x}_{v,k+1}-{x}_{v,k}\\ {y}_{v,k+1}-{y}_{v,k}\end{array}\right]\left|\right|}_{2}=VT.$$Above, $\left|\right|\xb7{\left|\right|}_{2}$ denotes the Euclidean norm.**Turn-rate constraint**Physical limitations of any real-world vehicle constrain the vehicle to a maximum turn rate, denoted by ${\omega}_{\mathrm{max}}$. Change in heading between time steps k and $k+1$ is limited by$$|{\varphi}_{k+1}-{\varphi}_{k}|\le T{\omega}_{\mathrm{max}}.$$**Proximity constraints to the beacons**Proximity constraints arise from the need to prevent numerical instability which occurs when the vehicle gets too close to a beacon. ${n}_{b}$ proximity constraints are implemented as radii from the beacons according to the inequalities$$\left|\right|{\left[\begin{array}{c}{x}_{v,k+1}-{x}_{bi}\\ {y}_{v,k+1}-{y}_{bi}\end{array}\right]\left|\right|}_{2}\ge {d}_{\mathrm{min}},\phantom{\rule{2.em}{0ex}}\mathrm{for}\phantom{\rule{4.pt}{0ex}}i=1,\cdots ,{n}_{b}.$$

#### 4.4. Optimal Waypoint Selection

## 5. Simulation Results

#### 5.1. Comparison of the LSLA Algorithm for Different Values of l and m

#### 5.2. Comparison between LSLA and RIG Algorithms

#### 5.3. Mobile Beacons and Beacon Communication Loss

#### 5.4. Stationary Beacons Far from Initial Vehicle Position

#### 5.5. Optimal Maneuvering with an Undesirable Vehicle/Beacons Geometry

#### 5.6. Computational Complexity

## 6. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Acknowledgments

## Conflicts of Interest

## References

- Bulusu, N.; Heidemann, J.; Estrin, D. GPS-less low-cost outdoor localization for very small devices. IEEE Pers. Commun.
**2000**, 7, 28–34. [Google Scholar] [CrossRef] [Green Version] - Caballero, F.; Merino, L.; Ferruz, J.; Ollero, A. Vision-based odometry and SLAM for medium and high altitude flying UAVs. J. Intell. Robot. Syst.
**2009**, 54, 137–161. [Google Scholar] [CrossRef] - Langelaan, J.; Rock, S. Passive GPS-free navigation for small UAVs. In Proceedings of the 2005 IEEE Aerospace Conference, Big Sky, MT, USA, 5–12 March 2005; pp. 1–9. [Google Scholar]
- Loevsky, I.; Shimshoni, I. Reliable and efficient landmark-based localization for mobile robots. Robot. Auton. Syst.
**2010**, 58, 520–528. [Google Scholar] [CrossRef] - Ogiso, S.; Kawagishi, T.; Mizutani, K.; Wakatsuki, N.; Zempo, K. Self-localization method for mobile robot using acoustic beacons. ROBOMECH J.
**2015**, 2, 12. [Google Scholar] [CrossRef] [Green Version] - Jais, M.; Ehkan, P.; Ahmad, R.; Ismail, I.; Sabapathy, T.; Jusoh, M. Review of angle of arrival (AOA) estimations through received signal strength indication (RSSI) for wireless sensors network (WSN). In Proceedings of the 2015 International Conference on Computer, Communications, and Control Technology (I4CT), Kuching, Malaysia, 21–23 April 2015; pp. 354–359. [Google Scholar]
- Niculescu, D.; Nath, B. Ad hoc positioning system (APS) using AOA. In Proceedings of the IEEE INFOCOM 2003. Twenty-second Annual Joint Conference of the IEEE Computer and Communications Societies (IEEE Cat. No.03CH37428), San Francisco, CA, USA, 30 March–3 April 2003; Volume 3, pp. 1734–1743. [Google Scholar]
- Betke, M.; Gurvits, L. Mobile robot localization using landmarks. IEEE Trans. Robot. Autom.
**1997**, 13, 251–263. [Google Scholar] [CrossRef] [Green Version] - Esteves, J.S.; Carvalho, A.; Couto, C. Generalized geometric triangulation algorithm for mobile robot absolute self-localization. In Proceedings of the 2003 IEEE International Symposium on Industrial Electronics (Cat. No.03TH8692), Rio de Janeiro, Brazil, 9–11 June 2003; Volume 1, pp. 346–351. [Google Scholar]
- Lin, Y.; Tao, H.; Tu, Y.; Liu, T. A node self-localization algorithm with a mobile anchor node in underwater acoustic sensor networks. IEEE Access
**2019**, 7, 43773–43780. [Google Scholar] [CrossRef] - Sabale, K.; Mini, S. Anchor node path planning for localization in wireless sensor networks. Wirel. Netw.
**2019**, 25, 49–61. [Google Scholar] [CrossRef] - McGuire, J.; Law, Y.W.; Chahl, J.; Doğançay, K. Optimal Beacon Placement for Self-Localization Using Three Beacon Bearings. Symmetry
**2021**, 13, 56. [Google Scholar] [CrossRef] - Bishop, A.N.; Fidan, B.; Anderson, B.D.; Doğançay, K.; Pathirana, P.N. Optimality analysis of sensor-target localization geometries. Automatica
**2010**, 46, 479–492. [Google Scholar] [CrossRef] - Peliti, P.; Rosa, L.; Oriolo, G.; Vendittelli, M. Vision-based loitering over a target for a fixed-wing UAV. IFAC Proc. Vol.
**2012**, 45, 51–57. [Google Scholar] [CrossRef] [Green Version] - Boström-Rost, P.; Axehill, D.; Hendeby, G. On Global Optimization for Informative Path Planning. IEEE Control Syst. Lett.
**2018**, 2, 833–838. [Google Scholar] [CrossRef] - Boström-Rost, P.; Axehill, D.; Hendeby, G. Informative Path Planning for Active Tracking of Agile Targets. In Proceedings of the 2019 IEEE Aerospace Conference, Big Sky, MT, USA, 2–9 March 2019; pp. 1–11. [Google Scholar] [CrossRef]
- Popović, M.; Vidal-Calleja, T.; Hitz, G.; Chung, J.J.; Sa, I.; Siegwart, R.; Nieto, J. An informative path planning framework for UAV-based terrain monitoring. Auton. Robot.
**2020**, 44, 889–911. [Google Scholar] [CrossRef] [Green Version] - 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] - Huber, M.F.; Bailey, T.; Durrant-Whyte, H.; Hanebeck, U.D. On entropy approximation for Gaussian mixture random vectors. In Proceedings of the 2008 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, Seoul, Korea, 20–22 August 2008; pp. 181–188. [Google Scholar] [CrossRef] [Green Version]
- Michalowicz, J.V.; Nichols, J.M.; Bucholtz, F. Handbook of Differential Entropy; CRC Press: Boca Raton, FL, USA, 2013. [Google Scholar]
- Wei, Y.; Zheng, R. Informative Path Planning for Mobile Sensing with Reinforcement Learning. In Proceedings of the IEEE INFOCOM 2020-IEEE Conference on Computer Communications, Toronto, ON, Canada, 6–9 July 2020; pp. 864–873. [Google Scholar] [CrossRef]
- Hollinger, G.A.; Sukhatme, G.S. Sampling-based robotic information gathering algorithms. Int. J. Robot. Res.
**2014**, 33, 1271–1287. [Google Scholar] [CrossRef] - Doğançay, K.; Hmam, H. Optimal angular sensor separation for AOA localization. Signal Process.
**2008**, 88, 1248–1260. [Google Scholar] [CrossRef] - Cheng, Y.; Wang, X.; Morelande, M.; Moran, B. Information geometry of target tracking sensor networks. Inf. Fusion
**2013**, 14, 311–326. [Google Scholar] [CrossRef] - Oshman, Y.; Davidson, P. Optimization of observer trajectories for bearings-only target localization. IEEE Trans. Aerosp. Electron. Syst.
**1999**, 35, 892–902. [Google Scholar] [CrossRef] - Hoffmann, G.M.; Tomlin, C.J. Mobile sensor network control using mutual information methods and particle filters. IEEE Trans. Autom. Control
**2010**, 55, 32–47. [Google Scholar] [CrossRef] - Grocholsky, B.; Makarenko, A.; Durrant-Whyte, H. Information-theoretic coordinated control of multiple sensor platforms. In Proceedings of the 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422), Taipei, Taiwan, 14–19 September 2003; Volume 1, pp. 1521–1526. [Google Scholar]
- Passerieux, J.M.; Van Cappel, D. Optimal observer maneuver for bearings-only tracking. IEEE Trans. Aerosp. Electron. Syst.
**1998**, 34, 777–788. [Google Scholar] [CrossRef] - Martínez, S.; Bullo, F. Optimal sensor placement and motion coordination for target tracking. Automatica
**2006**, 42, 661–668. [Google Scholar] [CrossRef] - Bishop, A.N.; Pathirana, P.N. Optimal Trajectories for Homing Navigation with Bearing Measurements. IFAC Proc. Vol.
**2008**, 41, 12117–12123. [Google Scholar] [CrossRef] [Green Version] - Doğançay, K. Single- and multi-platform constrained sensor path optimization for angle-of-arrival target tracking. In Proceedings of the 2010 18th European Signal Processing Conference, Aalborg, Denmark, 23–27 August 2010; pp. 835–839. [Google Scholar]
- Schlotfeldt, B.; Thakur, D.; Atanasov, N.; Kumar, V.; Pappas, G.J. Anytime Planning for Decentralized Multirobot Active Information Gathering. IEEE Robot. Autom. Lett.
**2018**, 3, 1025–1032. [Google Scholar] [CrossRef] - Andreev, K.V.; Rubinovich, E.Y. Moving observer trajectory control by angular measurements in tracking problem. Autom. Remote Control
**2016**, 77, 106–129. [Google Scholar] [CrossRef] - Galyaev, A.A.; Lysenko, P.V.; Rubinovich, E.Y. Optimal stochastic control in the interception problem of a randomly tacking vehicle. Mathematics
**2021**, 9, 2386. [Google Scholar] [CrossRef] - Zhang, H.; Zhang, Y.; Zhang, P. Optimal guidance law for intercepting the active defense aircraft with terminal angle constraint. J. Phys. Conf. Ser.
**2021**, 1828, 012160. [Google Scholar] [CrossRef] - Papachristos, C.; Khattak, S.; Alexis, K. Uncertainty-aware receding horizon exploration and mapping using aerial robots. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 29 May–3 June 2017; pp. 4568–4575. [Google Scholar] [CrossRef]
- Kassas, Z.M.; Arapostathis, A.; Humphreys, T.E. Greedy Motion Planning for Simultaneous Signal Landscape Mapping and Receiver Localization. IEEE J. Sel. Top. Signal Process.
**2015**, 9, 247–258. [Google Scholar] [CrossRef] - Xu, S.; Doğançay, K.; Hmam, H. Distributed path optimization of multiple UAVs for AOA target localization. In Proceedings of the 2016 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), Shanghai, China, 20–25 March 2016; pp. 3141–3145. [Google Scholar]
- Hernandez, M.L. Optimal sensor trajectories in bearings-only tracking. In Proceedings of the Seventh International Conference on Information Fusion, Stockholm, Sweden, 28 June–1 July 2004; Volume 2, pp. 893–900. [Google Scholar]
- Pivtoraiko, M.; Knepper, R.A.; Kelly, A. Differentially constrained mobile robot motion planning in state lattices. J. Field Robot.
**2009**, 26, 308–333. [Google Scholar] [CrossRef] - Papadimitriou, C.H.; Steiglitz, K. Combinatorial Optimization: Algorithms and Complexity; Dover Publications: Mineola, NY, USA, 1998. [Google Scholar]
- Vitus, M.P.; Zhang, W.; Abate, A.; Hu, J.; Tomlin, C.J. On efficient sensor scheduling for linear dynamical systems. Automatica
**2012**, 48, 2482–2493. [Google Scholar] [CrossRef] - Wang, W.; Bai, P.; Zhou, Y.; Liang, X.; Wang, Y. Optimal configuration analysis of AOA localization and optimal heading angles generation method for UAV swarms. IEEE Access
**2019**, 7, 70117–70129. [Google Scholar] [CrossRef] - Hansen, N. The CMA Evolution Strategy: A Comparing Review. In Towards a New Evolutionary Computation: Advances in the Estimation of Distribution Algorithms; Lozano, J.A., Larrañaga, P., Inza, I., Bengoetxea, E., Eds.; Springer: Berlin/Heidelberg, Germany, 2006; pp. 75–102. [Google Scholar] [CrossRef]
- Sabet, M.; Fathi, A.; Daniali, H.M. Optimal design of the own ship maneuver in the bearing-only target motion analysis problem using a heuristically supervised extended Kalman filter. Ocean Eng.
**2016**, 123, 146–153. [Google Scholar] [CrossRef] - Patle, B.; Pandey, A.; Parhi, D.; Jagadeesh, A. A review: On path planning strategies for navigation of mobile robot. Def. Technol.
**2019**, 15, 582–606. [Google Scholar] [CrossRef] - Ucinski, D. Optimal sensor location for parameter estimation of distributed processes. Int. J. Control
**2000**, 73, 1235–1248. [Google Scholar] [CrossRef] - Zhang, H.; Dufour, F.; Anselmi, J.; Laneuville, D.; Nègre, A. Piecewise optimal trajectories of observer for bearings-only tracking of maneuvering target. In Proceedings of the 2018 IEEE Aerospace Conference, Big Sky, MT, USA, 3–10 March 2018; pp. 1–7. [Google Scholar]
- McGuire, J.; Law, Y.W.; Chahl, J. Mobile beacon path planning for optimal unmanned aerial vehicle self-localization. In Proceedings of the AIAC 2021: 19th Australian International Aerospace Congress. Engineers Australia, Melbourne, VIC, Australia, 29 November–1 December 2021; pp. 198–203. [Google Scholar]
- Roh, H.; Cho, M.H.; Tahk, M.J. Trajectory optimization using Cramér-Rao lower bound for bearings-only target tracking. In Proceedings of the 2018 AIAA Guidance, Navigation, and Control Conference, Kissimmee, FL, USA, 8–12 January 2018; p. 1591. [Google Scholar]
- Moreno-Salinas, D.; Pascoal, A.; Aranda, J. Sensor networks for optimal target localization with bearings-only measurements in constrained three-dimensional scenarios. Sensors
**2013**, 13, 10386–10417. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Xu, S.; Doğançay, K. Optimal sensor placement for 3-D angle-of-arrival target localization. IEEE Trans. Aerosp. Electron. Syst.
**2017**, 53, 1196–1211. [Google Scholar] [CrossRef] - Ucinski, D. Optimal Measurement Methods for Distributed Parameter System Identification; CRC Press: Boca Raton, FL, USA, 2004. [Google Scholar]
- Kalman, R.E. A new approach to linear filtering and prediction problems. J. Basic Eng.
**1960**, 82, 35–45. [Google Scholar] [CrossRef] [Green Version] - Julier, S.J.; Uhlmann, J.K. New extension of the Kalman filter to nonlinear systems. In Proceedings of the Signal Processing, Sensor Fusion, and Target Recognition VI, Orlando, FL, USA, 21–25 April 1997; Volume 3068, pp. 182–193. [Google Scholar]
- Kay, S.M. Fundamentals of Statistical Signal Processing; Prentice Hall PTR: Hoboken, NJ, USA, 1993. [Google Scholar]
- Niu, R.; Vempaty, A.; Varshney, P.K. Received-signal-strength-based localization in wireless sensor networks. Proc. IEEE
**2018**, 106, 1166–1182. [Google Scholar] [CrossRef] - Spagnolini, U. Statistical Signal Processing in Engineering; John Wiley & Sons Ltd.: Hoboken, NJ, USA, 2018. [Google Scholar]
- Tichavsky, P.; Muravchik, C.H.; Nehorai, A. Posterior Cramér-Rao bounds for discrete-time nonlinear filtering. IEEE Trans. Signal Process.
**1998**, 46, 1386–1396. [Google Scholar] [CrossRef] [Green Version] - Van Trees, H.L. Detection, Estimation, and Modulation Theory, Part I: Detection, Estimation, and Linear Modulation Theory; John Wiley & Sons: Hoboken, NJ, USA, 2004. [Google Scholar]
- Zuo, L.; Niu, R.; Varshney, P.K. Conditional posterior Cramér–Rao lower bounds for nonlinear sequential Bayesian estimation. IEEE Trans. Signal Process.
**2010**, 59, 1–14. [Google Scholar] [CrossRef] [Green Version] - Lu, S.; Cai, L.; Ding, L.; Chen, J. Two Efficient Implementation Forms of Unscented Kalman Filter. In Proceedings of the 2007 IEEE International Conference on Control and Automation, Guangzhou, China, 30 May–1 June 2007; pp. 761–764. [Google Scholar] [CrossRef]

**Figure 3.**An example of a tree-like search space used by the optimal maneuvering algorithm. Here, $m=3$ and $l=3$. Suppose evaluation of ${J}_{k}$ along the thick branch gives the lowest value, then the thick branch is the optimal branch.

**Figure 4.**(

**a**) The average value of the objective function for the last 200 s of simulation using the LSLA and RIG algorithms with moving beacons. (

**b**) Settling time of the objective function.

**Figure 5.**The average value of the objective function for the last 200 s of simulation using the LSLA and RIG algorithms after communications with (

**a**) one beacon and (

**b**) two beacons are dropped.

**Figure 6.**Average vehicle trajectory with position uncertainty ellipses placed every 100 s using (

**a**) LSLA and (

**c**) RIG algorithms in the optimal maneuvering scheme. Actual and average theoretical objective function value using (

**b**) LSLA and (

**d**) RIG algorithms.

**Figure 7.**Average vehicle trajectory with position uncertainty ellipses placed every 100 s using (

**a**) LSLA and (

**c**) RIG algorithms in the optimal maneuvering scheme with a less informative beacon formation. Actual and average theoretical objective function value using (

**b**) LSLA and (

**d**) RIG algorithms.

**Figure 8.**The average time for the LSLA and RIG algorithms to produce an optimal waypoint as the total number of waypoints is increased.

Symbols | Definitions |
---|---|

${x}_{v,k}$, ${y}_{v,k}$, ${\varphi}_{k}$ | Vehicle’s 2D coordinates and heading at time k |

${z}_{k}$ | AOA measurement vector at time k |

V | Vehicle’s forward speed |

T | Sample period |

${\omega}_{k}$ | Vehicle’s rotational speed at time k |

n, ${n}_{b}$ | Number of states and beacons |

${x}_{bi}$, ${y}_{bi}$ | 2D coordinates of the ith beacon |

${J}_{k}$ | Objective function defined in Equation (11) |

m | Number of candidate waypoints in one time step |

l | Number of look-ahead steps when determining the optimal waypoint |

**Table 2.**Average objective function value for different m and l. Results are in the order of ${10}^{-11}$.

m = 3 | m = 4 | m = 5 | m = 6 | m = 7 | |
---|---|---|---|---|---|

l = 1 | 2.89 | 2.73 | 3.27 | 2.97 | 2.71 |

l = 2 | 2.60 | 2.50 | 2.98 | 2.97 | 2.48 |

l = 3 | 3.07 | 3.02 | 2.80 | 2.62 | 2.22 |

l = 4 | 3.26 | 2.48 | 2.81 | 2.77 | 2.42 |

m = 3 | m = 4 | m = 5 | m = 6 | m = 7 | |
---|---|---|---|---|---|

l = 1 | 184 | 190 | 200 | 194 | 214 |

l = 2 | 187 | 164 | 214 | 230 | 174 |

l = 3 | 166 | 188 | 165 | 173 | 171 |

l = 4 | 183 | 201 | 173 | 170 | 185 |

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

McGuire, J.L.; Law, Y.W.; Doğançay, K.; Ho, S.-Y.; Chahl, J.
Optimal Maneuvering for Autonomous Vehicle Self-Localization. *Entropy* **2022**, *24*, 1169.
https://doi.org/10.3390/e24081169

**AMA Style**

McGuire JL, Law YW, Doğançay K, Ho S-Y, Chahl J.
Optimal Maneuvering for Autonomous Vehicle Self-Localization. *Entropy*. 2022; 24(8):1169.
https://doi.org/10.3390/e24081169

**Chicago/Turabian Style**

McGuire, John L., Yee Wei Law, Kutluyıl Doğançay, Sook-Ying Ho, and Javaan Chahl.
2022. "Optimal Maneuvering for Autonomous Vehicle Self-Localization" *Entropy* 24, no. 8: 1169.
https://doi.org/10.3390/e24081169