Abstract
In the pursuit of real-time motion planning, a commonly adopted practice is to compute trajectories by running a planning algorithm on a simplified, low-dimensional dynamical model, and then employ a feedback tracking controller that tracks such a trajectory by accounting for the full, high-dimensional system dynamics. While this strategy of planning with model mismatch generally yields fast computation times, there are no guarantees of dynamic feasibility, which hampers application to safety-critical systems. Building upon recent work that addressed this problem through the lens of Hamilton-Jacobi (HJ) reachability, we devise an algorithmic framework whereby one computes, offline, for a pair of “planner” (i.e., low-dimensional) and “tracking” (i.e., high-dimensional) models, a feedback tracking controller and associated tracking bound. This bound is then used as a safety margin when generating motion plans via the low-dimensional model. Specifically, we harness the computational tool of sum-of-squares (SOS) programming to design a bilinear optimization algorithm for the computation of the feedback tracking controller and associated tracking bound. The algorithm is demonstrated via numerical experiments, with an emphasis on investigating the trade-off between the increased computational scalability afforded by SOS and its intrinsic conservativeness. Collectively, our results enable scaling the appealing strategy of planning with model mismatch to systems that are beyond the reach of HJ analysis, while maintaining safety guarantees.
Singh, Chen, and Pavone were supported by NASA under the Space Technology Research Grants Program, Grant NNX12AQ43G, and by the King Abdulaziz City for Science and Technology (KACST). Herbert and Tomlin were supported by SRC under the CONIX Center and by ONR under the BRC program in Multibody Systems.
This is a preview of subscription content, log in via an institution to check access.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
References
Ahmadi, A.A., Majumdar, A.: DSOS and SDSOS optimization: LP and SOCP-based alternatives to sum of squares optimization. In: IEEE Annual Conference on Information Sciences and Systems (2014)
Ahmadi, A.A., Majumdar, A.: Some applications of polynomial optimization in operations research and real-time decision making. Optim. Lett. 20(4), 709–729 (2016)
ApS, M.: MOSEK optimization software (2017). https://mosek.com/
Ben-Tal, A., Nemirovski, A.: Lectures on Modern Convex Optimization: Analysis, Algorithms, and Engineering Applications. SIAM, Philadelphia (2001)
Burridge, R.R., Rizzi, A.A., Koditschek, D.E.: Sequential composition of dynamically dexterous robot behaviors. Int. J. Robot. Res. 18(6), 534–555 (1999)
Chen, M., Bansal, S., Fisac, J.F., Tomlin, C.J.: Robust sequential path planning under disturbances and adversarial intruder. IEEE Trans. Control Syst. Technol. (2018, in press)
Chen, M., Herbert, S., Tomlin, C.J.: Fast reachable set approximations via state decoupling disturbances. In: Proceedings of the IEEE Conference on Decision and Control (2016)
Chen, M., Herbert, S.L., Vashishtha, M.S., Bansal, S., Tomlin, C.J.: Decomposition of reachable sets and tubes for a class of nonlinear systems. IEEE Trans. Autom. Control (2018, in press)
Chitsaz, H., LaValle, S.M.: Time-optimal paths for a Dubins airplane. In: Proceedings of the IEEE Conference on Decision and Control (2007)
Coddington, E.A., Levinson, N.: Theory of Ordinary Differential Equations. McGraw-Hill, New York (1955)
Herbert, S.L., Chen, M., Han, S., Bansal, S., Fisac, J.F., Tomlin, C.J.: FaSTrack: a modular framework for fast and guaranteed safe motion planning. In: Proceedings of the IEEE Conference on Decision and Control (2017)
Iwasaki, T., Hara, S.: Generalized KYP lemma: unified frequency domain inequalities with design applications. IEEE Trans. Autom. Control 50(1), 41–59 (2005)
Janson, L., Schmerling, E., Clark, A., Pavone, M.: Fast marching tree: a fast marching sampling-based method for optimal motion planning in many dimensions. Int. J. Robot. Res. 34(7), 883–921 (2015)
Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30(7), 846–894 (2011)
Kousik, S., Vaskov, S., Johnson-Roberson, M., Vasudevan, R.: Safe trajectory synthesis for autonomous driving in unforeseen environments. In: Proceedings of the ASME Dynamic Systems and Control Conference (2017)
LaValle, S.M.: Motion planning: wild frontiers. IEEE Robot. Autom. Mag. 18(2), 108–118 (2011)
LaValle, S.M., Kuffner, J.J.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5), 378–400 (2001)
Li, Y., Littlefield, Z., Bekris, K.E.: Asymptotically optimal sampling-based kinodynamic planning. Int. J. Robot. Res. 35(5), 528–564 (2016)
Majumdar, A., Ahmadi, A.A., Tedrake, R.: Control design along trajectories with sums of squares programming. In: Proceedings of the IEEE Conference on Robotics and Automation (2013)
Majumdar, A., Tedrake, R.: Robust online motion planning with regions of finite time invariance. In: Algorithmic Foundations of Robotics X. Springer (2013)
Majumdar, A., Tedrake, R.: Funnel libraries for real-time robust feedback motion planning. Int. J. Robot. Res. 36(8), 947–982 (2017)
Owen, M., Beard, R.W., McLain, T.W.: Implementing Dubins airplane paths on fixed-wing UAVs. In: Handbook of Unmanned Aerial Vehicles. Springer, Dordrecht (2015)
Parrilo, P.A.: Structured semidefinite programs and semialgebraic geometry methods in robustness and optimization. Ph.D. thesis, Massachusetts Institute of Technology (2000)
Posa, M., Koolen, T., Tedrake, R.: Balancing and step recovery capturability via sums-of-squares optimization. In: Robotics: Science and Systems (2017)
Putinar, M.: Positive polynomials on compact semi-algebraic sets. Indiana Univ. Math. J. 42(3), 969–984 (1993)
Rajamani, R.: Vehicle Dynamics and Control, 2nd edn. Springer, Boston (2012)
Ratliff, N., Zucker, M., Bagnell, J.A., Srinivasa, S.: CHOMP: gradient optimization techniques for efficient motion planning. In: Proceedings of the IEEE Conference on Robotics and Automation (2009)
Schmerling, E., Janson, L., Pavone, M.: Optimal sampling-based motion planning under differential constraints: the drift case with linear affine dynamics. In: Proceedings of the IEEE Conference on Decision and Control (2015)
Schmerling, E., Janson, L., Pavone, M.: Optimal sampling-based motion planning under differential constraints: the driftless case. In: Proceedingd of the IEEE Conference on Robotics and Automation (2015). Extended version http://arxiv.org/abs/1403.2483/
Schmerling, E., Pavone, M.: Evaluating trajectory collision probability through adaptive importance sampling for safe motion planning. In: Robotics: Science and Systems (2017)
Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., Abbeel, P.: Finding locally optimal, collision-free trajectories with sequential convex optimization. In: Robotics: Science and Systems (2013)
Singh, S., Chen, M., Herbert, S.L., Tomlin, C.J., Pavone, M.: Robust tracking with model mismatch for fast and safe planning: an SOS optimization approach. In: Workshop on Algorithmic Foundations of Robotics (2018). https://arxiv.org/abs/1808.00649
Singh, S., Majumdar, A., Slotine, J.J.E., Pavone, M.: Robust online motion planning via contraction theory and convex optimization. In: Proceedings of the IEEE Conference on Robotics and Automation (2017). Extended Version http://asl.stanford.edu/wp-content/papercite-data/pdf/Singh.Majumdar.Slotine.Pavone.ICRA17.pdf
Steinhardt, J., Tedrake, R.: Finite-time regional verification of stochastic non-linear systems. Int. J. Robot. Res. 31(7), 901–923 (2012)
Tanabe, K., Chen, M.: Beacls Library. https://github.com/HJReachability/beacls
Tedrake, R., Manchester, I.R., Tobenkin, M., Roberts, J.W.: LQR-trees: feedback motion planning via sums-of-squares verification. Int. J. Robot. Res. 29(8), 1038–1052 (2010)
Tobenkin, M.M., Permenter, F., Megretski, A.: SPOTLESS polynomial and conic optimization (2013). https://github.com/spottoolbox/spotless
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Rights and permissions
Copyright information
© 2020 Springer Nature Switzerland AG
About this paper
Cite this paper
Singh, S., Chen, M., Herbert, S.L., Tomlin, C.J., Pavone, M. (2020). Robust Tracking with Model Mismatch for Fast and Safe Planning: An SOS Optimization Approach. In: Morales, M., Tapia, L., Sánchez-Ante, G., Hutchinson, S. (eds) Algorithmic Foundations of Robotics XIII. WAFR 2018. Springer Proceedings in Advanced Robotics, vol 14. Springer, Cham. https://doi.org/10.1007/978-3-030-44051-0_32
Download citation
DOI: https://doi.org/10.1007/978-3-030-44051-0_32
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-030-44050-3
Online ISBN: 978-3-030-44051-0
eBook Packages: Intelligent Technologies and RoboticsIntelligent Technologies and Robotics (R0)