Autonomous Motion Planning Strategies in Opportunistic Navigation Environments
Abed AlRahman Al Makdah, Joshua Morales, and Zak (Zaher) M. Kassas; University of California, Riverside
Future autonomous vehicles (AVs) will be deployed in poorly known environments with minimal human interaction, where they will be tasked to complete a certain mission. In order to achieve this mission, they need to sufficiently learn the environment to possess situational awareness. For accurate navigation, AVs need to be equipped with accurate navigation systems, such as Global Navigation Satellite System (GNSS). However, GNSS will not handle the demand of the future autonomous systems, since GNSS signals are very weak/unavailable in indoor environment and in deep urban canyons, and are susceptible to jamming and interference. Hence, additional sources of information are needed for the AVs to exploit in order to possess situational awareness. This gives rise to the need for equipping the future AVs with sensors to be able to look for additional sources of information within the unknown environment (e.g. landmarks, radio signals), where they map these landmarks/signals while localizing themselves within that environment. This process is known in the literature as Simultaneous Localization and Mapping (SLAM) , . The case where the AV uses radio signals (e.g. signals from cellular towers, digital video broadcasting) to extract spatial and temporal information is referred to as radio SLAM.
In traditional SLAM problem, AVs move passively while collecting information to achieve situational awareness without acing deliberately to collect this information. Extensive re- search has been done on developing the SLAM algorithms in order to result in more accurate maps. Recently, researchers have been focusing on finding motion planning strategies for au- tonomous systems, which maximize information gathering. This type of SLAM is called active SLAM. In this type of SLAM the AV is actively controlled to follow a planned strat- egy that results in maximum information gathering, which leads to more accurate situational awareness with less uncertainty , .
In practice, the main interest when deploying an AV in a certain stochastic environment, is to perform a specified mission. However, to be able to complete this mission, sufficiently enough information about the environment needs to be collected. So, for the AV to complete its main mission in a stochastic environment, it has to consider pursuing a low-level mission which corresponds to information gathering and mapping this environment.
This paper addresses the problem of optimal motion planning strategies that optimizes the performance of completing the main objective assigned to an AV, which is deployed in a stochastic environment. The main objective cannot be achieved without gathering suffi- ciently enough information about this environment. So, this work investigates the decision
on the quantity of the trade-offs between both objectives (i.e. main objective and low-level objective) in order to find an optimal trajectory that optimizes the performance of the main objective.
Finding an optimal trajectory that optimizes multi-objectives is considered a typical op- timal control problem, where no single solution that optimizes each objective simultaneously exists. Optimal solutions change depending on the decision on the quantity of trade-offs between the two objectives. However, in SLAM the optimal motion planning problem is more complex, and optimal control framework cannot be applied. This complexity stems from the fact that AV’s observations and dynamics are stochastic, where its states and the environment’s are estimated after applying a motion command, then plan for a new opti- mized input based on the updated states and the updated map. So, the motion planning strategy is directly influenced by the accuracy of the estimated states, making the estimation and planning coupled.
Extensive framework approaches were applied for active SLAM in the literature. In  and , the authors used Model Predictive Control (MPC) approach for active SLAM. In , the authors used multi-step look-ahead (receding horizon) for active SLAM in stochastic environments using radio signals a source of information. Greedy motion planning optimiza- tion approach is used in  and  for optimizing situational awareness in environments using radio signals as landmarks. Recent works in  and , the authors consider apply- ing continuous-space planning approaches under uncertainty. Also in , the initial optimal trajectory is assumed to be known and optimization where performed locally around that initial trajectory. So, convergence of the approaches used is not guaranteed outside the re- gion around the initial trajectory. Several more approaches are mentioned in . In this work, multi-step look-ahead (receding horizon) strategy is adopted.
Consider an AV deployed in a stochastic environment with radio signals as the source of information. The AV has minimal a priori knowledge about its own states and the environment’s. And it is assigned to complete a specific objective, which is to move from its initial position to a specified final destination. This paper studies three motion planning strategies that optimizes the cost function corresponding to the main objective, which for the sake of simplicity, is considered to be the norm of the error between the estimated states of the AV and the states of the target. The first strategy considers minimizing the cost function that corresponds to the main objective. An optimum solution is solved for using a traditional optimal control approach assuming that the AV’s estimated states coincide with its true states. This strategy results in a catastrophic failure of the system, where the uncertainty increases at an enormous rate and the observer diverges. The second strategy considers adding a constraint to the previous strategy which corresponds to adding a bound to the uncertainty of the estimated states. In this strategy, multi-step look ahead (receding horizon) approach is used to solve for the optimal solution that minimizes the cost function subject to the previously mentioned constraint. This strategy results in a convergence of the system. In the third strategy, two cost functions are considered to be optimized simultaneously: (i) a cost function that corresponds to the main objective, (ii) a cost function that corresponds to the low-level objective (learning the environment). Also in this strategy, multi-step look ahead (receding horizon) approach is used to solve for the optimal solution that minimizes the convex combination of both cost functions. Similar to the second strategy, this strategy results in a convergence of the system. However, given that the second strategy resulted in
a different optimal solution than the third strategy, the main question is, are these the most optimized solutions that can be solved for? This work addresses the following questions: 1) What is the optimal value of the uncertainty bound in the second strategy’s constraint that will result in the most optimal solution? 2) Does changing this value adaptively as the AV gathers more information, results in a more optimized solution? 3) What is the is the optimal weights’ values of the convex combination of the third strategy’s cost functions that results in the most optimal solution? 4) Does changing the values of these weights adaptively as the AV reduces the uncertainty of its situational awareness, results in a more optimized solution? 5) Considering the most optimal solutions are solved for using the second and the third strategies, which solution provides a better overall performance? Do both solutions result in a similar performance?
This work will address the questions in simulation environment. And the resulting opti- mal motion planning strategies will be studied analytically and in simulation.
 S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics. MIT press, 2005.
 H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part i,”
IEEE robotics & automation magazine, vol. 13, no. 2, pp. 99–110, 2006.
 C. Leung, S. Huang, N. Kwok, and G. Dissanayake, “Planning under uncertainty using model predictive control for information gathering,” Robotics and Autonomous Systems, vol. 54, no. 11, pp. 898–910, 2006.
 C. Leung, S. Huang, and G. Dissanayake, “Active slam using model predictive control and attractor based exploration,” in Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pp. 5026–5031, IEEE, 2006.
 Z. M. Kassas, “Collaborative opportunistic navigation [student research highlight],” IEEE Aerospace and Electronic Systems Magazine, vol. 28, no. 6, pp. 38–41, 2013.
 Z. M. Kassas and T. E. Humphreys, “Receding horizon trajectory optimization in op- portunistic navigation environments,” IEEE Transactions on Aerospace and Electronic Systems, vol. 51, no. 2, pp. 866–877, 2015.
 Z. M. Kassas, A. Arapostathis, and T. E. Humphreys, “Greedy motion planning for simultaneous signal landscape mapping and receiver localization,” IEEE Journal of Selected Topics in Signal Processing, vol. 9, no. 2, pp. 247–258, 2015.
 Z. Kassas and T. E. Humphreys, “Motion planning for optimal information gathering in opportunistic navigation systems,” in AIAA Guidance, Navigation, and Control (GNC) Conference, p. 4551, 2013.
 J. Van Den Berg, S. Patil, and R. Alterovitz, “Motion planning under uncertainty using iterative local optimization in belief space,” The International Journal of Robotics Research, vol. 31, no. 11, pp. 1263–1278, 2012.
 V. Indelman, L. Carlone, and F. Dellaert, “Planning in the continuous domain: A generalized belief space approach for autonomous navigation in unknown environments,” The International Journal of Robotics Research, vol. 34, no. 7, pp. 849–882, 2015.
 C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Transactions on Robotics, vol. 32, no. 6, pp. 1309– 1332, 2016.