Integrated Task and Motion Planning for Safe Legged Navigation in Partially Observable Environments
Authors: 
Abdulaziz Shamsah, Zhaoyuan Gu, Jonas Warnke, Seth Hutchinson, Ye ZhaoAuthors Info & Claims
Abdulaziz Shamsah, Zhaoyuan Gu, Jonas Warnke, Seth Hutchinson, Ye ZhaoAuthors Info & ClaimsPages 4913 - 4934
Abstract
This study proposes a hierarchically integrated framework for safe task and motion planning (TAMP) of bipedal locomotion in a partially observable environment with dynamic obstacles and uneven terrain. The high-level task planner employs linear temporal logic for a reactive game synthesis between the robot and its environment and provides a formal guarantee on navigation safety and task completion. To address environmental partial observability, a belief abstraction model is designed by partitioning the environment into multiple belief regions and employed at the high-level navigation planner to estimate the dynamic obstacles' location. This additional location information of dynamic obstacles offered by belief abstraction enables less conservative long-horizon navigation actions beyond guaranteeing immediate collision avoidance. Accordingly, a synthesized action planner sends a set of locomotion actions to the middle-level motion planner while incorporating safe locomotion specifications extracted from safety theorems based on a reduced-order model (ROM) of the locomotion process. The motion planner employs the ROM to design safety criteria and a sampling algorithm to generate nonperiodic motion plans that accurately track high-level actions. At the low level, a foot placement controller based on an angular-momentum linear inverted pendulum model is implemented and integrated with an ankle-actuated passivity-based controller for full-body trajectory tracking. To address external perturbations, this study also investigates the safe sequential composition of the keyframe locomotion state and achieves robust transitions against external perturbations through reachability analysis. The overall TAMP framework is validated with extensive simulations and hardware experiments on bipedal walking robots Cassie and Digit designed by Agility Robotics.
References
[1]
N. Bohórquez, A. Sherikov, D. Dimitrov, and P.-B. Wieber, “Safe navigation strategies for a biped robot walking in a crowd,” in Proc. IEEE 16th Int. Conf. Humanoid Robots, 2016, pp. 379–386.
[2]
A. Pajon and P.-B. Wieber, “Safe 3D bipedal walking through linear MPC with 3D capturability,” in Proc. IEEE Int. Conf. Robot. Automat., 2019, pp. 1404–1409.
[3]
N. Scianca, P. Ferrari, D. De Simone, L. Lanari, and G. Oriolo, “A behavior-based framework for safe deployment of humanoid robots,” Auton. Robots, vol. 45, no. 4, pp. 435–456, 2021.
[4]
M. Srinivasan and S. Coogan, “Control of mobile robots using barrier functions under temporal logic specifications,” IEEE Trans. Robot., vol. 37, no. 2, pp. 363–374, Apr. 2021.
[5]
H. Kress-Gazit, M. Lahijanian, and V. Raman, “Synthesis for robots: Guarantees and feedback for robot behavior,” Annu. Rev. Control, Robot., Auton. Syst., vol. 1, pp. 211–236, 2018.
[6]
J. Jiang, Y. Zhao, and S. Coogan, “Safe learning for uncertainty-aware planning via interval MDP abstraction,” IEEE Contr. Syst. Lett., vol. 6, pp. 2641–2646, Jan. 1, 2022.
[7]
C.-I. Vasile, J. Tumova, S. Karaman, C. Belta, and D. Rus, “Minimum-violation scLTL motion planning for mobility-on-demand,” in Proc. IEEE Int. Conf. Robot. Automat., 2017, pp. 1481–1488.
[8]
V. Vasilopoulos et al., “Reactive semantic planning in unexplored semantic environments using deep perceptual feedback,” IEEE Robot. Autom. Lett., vol. 5, no. 3, pp. 4455–4462, Jul. 2020.
[9]
S. Feng, Z. Zhou, J. S. Smith, M. Asselmeier, Y. Zhao, and P. A. Vela, “GPF-BG: A hierarchical vision-based planning framework for safe quadrupedal navigation,” in Proc. IEEE Int. Conf. Robot. Automat., 2023, pp. 1968–1975.
[10]
S. Bharadwaj, R. Dimitrova, and U. Topcu, “Synthesis of surveillance strategies via belief abstraction,” in Proc. IEEE Conf. Decis. Control, 2018, pp. 4159–4166.
[11]
Y. Gong and J. W. Grizzle, “Zero dynamics, pendulum models, and angular momentum in feedback control of bipedal locomotion,” J. Dyn. Syst., Meas., Control, vol. 144, no. 12, 2022, Art. no.
[12]
H. Sadeghian, C. Ott, G. Garofalo, and G. Cheng, “Passivity-based control of underactuated biped robots within hybrid zero dynamics approach,” in Proc. IEEE Int. Conf. Robot. Automat., 2017, pp. 4096–4101.
[13]
A. Robotics, “Digit robot,” 2023. [Online]. Available: https://agilityrobotics.com/robots
[14]
S. Heim and A. Spröwitz, “Beyond basins of attraction: Quantifying robustness of natural dynamics,” IEEE Trans. Robot., vol. 35, no. 4, pp. 939–952, Aug. 2019.
[15]
P. Zaytsev, W. Wolfslag, and A. Ruina, “The boundaries of walking stability: Viability and controllability of simple models,” IEEE Trans. Robot., vol. 34, no. 2, pp. 336–352, Apr. 2018.
[16]
R. R. Burridge, A. A. Rizzi, and D. E. Koditschek, “Sequential composition of dynamically dexterous robot behaviors,” Int. J. Robot. Res., vol. 18, no. 6, pp. 534–555, 1999.
[17]
J. Warnke, A. Shamsah, Y. Li, and Y. Zhao, “Towards safe locomotion navigation in partially observable environments with uneven terrain,” in Proc. IEEE Conf. Decis. Control, 2020, pp. 958–965.
[18]
S. Teng, Y. Gong, J. W. Grizzle, and M. Ghaffari, “Toward safety-aware informative motion planning for legged robots,” 2021, arXiv:2103.14252.
[19]
K. V. Alwala and M. Mukadam, “Joint sampling and trajectory optimization over graphs for online motion planning,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2021.
[20]
S. Kulgod, W. Chen, J. Huang, Y. Zhao, and N. Atanasov, “Temporal logic guided locomotion planning and control in cluttered environments,” in Proc. IEEE Amer. Control Conf., 2020, pp. 5425–5432.
[21]
M. E. Cao, X. Ni, J. Warnke, Y. Han, S. Coogan, and Y. Zhao, “Leveraging heterogeneous capabilities in multi-agent systems for environmental conflict resolution,” in Proc. IEEE Int. Symp. Saf., Secur., Rescue Robot, 2022, pp. 94–101.
[22]
S. Tonneau, P. Fernbach, A. D. Prete, J. Pettré, and N. Mansard, “2PAC: Two-point attractors for center of mass trajectories in multi-contact scenarios,” ACM Trans. Graph., vol. 37, no. 5, pp. 1–14, 2018.
[23]
Z. Zhou, D. J. Lee, Y. Yoshinaga, S. Balakirsky, D. Guo, and Y. Zhao, “Reactive task allocation and planning for quadrupedal and wheeled robot teaming,” in Proc. IEEE Int. Conf. Automat. Sci. Eng., 2022, pp. 2110–2117.
[24]
J. Jiang, S. Coogan, and Y. Zhao, “Abstraction-based planning for uncertainty-aware legged navigation,” IEEE Open J. Control Syst., to be published.
[25]
L. Wang, A. D. Ames, and M. Egerstedt, “Safe certificate-based maneuvers for teams of quadrotors using differential flatness,” in Proc. IEEE Int. Conf. Robot. Automat., 2017, pp. 3293–3298.
[26]
E. Rimon, “Exact robot navigation using artificial potential functions,” Ph.D. dissertation, Dept. Elect. Eng., Yale Univ., New Haven, CT, USA, 1990.
[27]
J.-K. Huang and J. W. Grizzle, “Efficient anytime CLF reactive planning system for a bipedal robot on undulating terrain,” IEEE Trans. Robot., 2023.
[28]
S. Kajita et al., “Biped walking pattern generation by using preview control of zero-moment point,” in Proc. IEEE Int. Conf. Robot. Automat., vol. 2, 2003, pp. 1620–1626.
[29]
J. Englsberger, C. Ott, and A. Albu-Schäffer, “Three-dimensional bipedal walking control based on divergent component of motion,” IEEE Trans. Robot., vol. 31, no. 2, pp. 355–368, Apr. 2015.
[30]
J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in Proc. Int. Conf. Humanoid Robots, 2006, pp. 200–207.
[31]
J.-P. Aubin, A. M. Bayen, and P. Saint-Pierre, Viability Theory: New Directions. Berlin, Germany: Springer, 2011.
[32]
Z. Li, J. Zeng, S. Chen, and K. Sreenath, “Autonomous navigation of underactuated bipedal robots in height-constrained environments,” Int. J. Robot. Res., to be published.
[33]
S. Kuindersma et al., “Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot,” Auton. Robots, vol. 40, no. 3, pp. 429–455, 2016.
[34]
A. Herdt, H. Diedam, P.-B. Wieber, D. Dimitrov, K. Mombaur, and M. Diehl, “Online walking motion generation with automatic footstep placement,” Adv. Robot., vol. 24, no. 5/6, pp. 719–737, 2010.
[35]
Y. Ding, C. Khazoom, M. Chignoli, and S. Kim, “Orientation-aware model predictive control with footstep adaptation for dynamic humanoid walking,” in Proc. Int. Conf. Humanoid Robots, 2022, pp. 299–305.
[36]
G. Romualdi, S. Dafarra, G. L'Erario, I. Sorrentino, S. Traversaro, and D. Pucci, “Online non-linear centroidal MPC for humanoid robot locomotion with step adjustment,” in Proc. Int. Conf. Robot. Automat., 2022, pp. 10412–10419.
[37]
C. Brasseur, A. Sherikov, C. Collette, D. Dimitrov, and P.-B. Wieber, “A robust linear MPC approach to online generation of 3D biped walking motion,” in Proc. Int. Conf. Humanoid Robots, 2015, pp. 595–601.
[38]
G. Gibson, O. Dosunmu-Ogunbi, Y. Gong, and J. Grizzle, “Terrain-adaptive, ALIP-based bipedal locomotion controller via model predictive control and virtual constraints,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots. Syst., 2022, pp. 6724–6731.
[39]
R. Grandia, F. Jenelten, S. Yang, F. Farshidian, and M. Hutter, “Perceptive locomotion through nonlinear model-predictive control,” IEEE Trans. Robot., to be published.
[40]
J. Shim, C. Mastalli, T. Corberes, S. Tonneau, V. Ivan, and S. Vijayakumar, “Topology-based MPC for automatic footstep placement and contact surface selection,” in Proc. IEEE Int. Conf. Robot. Automat., London, United Kingdom, 2023, pp. 12226–12232.
[41]
K. S. Narkhede, A. M. Kulkarni, D. A. Thanki, and I. Poulakakis, “A sequential MPC approach to reactive planning for bipedal robots using safe corridors in highly cluttered environments,” IEEE Robot. Autom. Lett., vol. 7, no. 4, pp. 11831–11838, Oct. 2022.
[42]
L. P. Kaelbling and T. Lozano-Pérez, “Integrated task and motion planning in belief space,” Int. J. Robot. Res., vol. 32, no. 9/10, pp. 1194–1227, 2013.
[43]
H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based reactive mission and motion planning,” IEEE Trans. Robot., vol. 25, no. 6, pp. 1370–1381, Dec. 2009.
[44]
J. A. DeCastro, J. Alonso-Mora, V. Raman, D. Rus, and H. Kress-Gazit, “Collision-free reactive mission and motion planning for multi-robot systems,” in Robotics Research. Berlin, Germany: Springer, 2018, pp. 459–476.
[45]
E. Plaku and S. Karaman, “Motion planning with temporal-logic specifications: Progress and challenges,” AI Commun., vol. 29, no. 1, pp. 151–162, 2016.
[46]
S. Sarid, B. Xu, and H. Kress-Gazit, “Guaranteeing high-level behaviors while exploring partially known maps,” Robot.: Sci. Syst. VIII, pp. 377–384, 2013.
[47]
M. R. Maly, M. Lahijanian, L. E. Kavraki, H. Kress-Gazit, and M. Y. Vardi, “Iterative temporal motion planning for hybrid systems in partially unknown environments,” in Proc. Int. Conf. Hybrid Syst.: Comput. Control, 2013, pp. 353–362.
[48]
S. C. Livingston, R. M. Murray, and J. W. Burdick, “Backtracking temporal logic synthesis for uncertain environments,” in Proc. IEEE Int. Conf. Robot. Automat., 2012, pp. 5163–5170.
[49]
U. Rosolia, A. Singletary, and A. D. Ames, “Unified multirate control: From low-level actuation to high-level planning,” IEEE Trans. Autom. Control, vol. 67, no. 12, pp. 6627–6640, Dec. 2022.
[50]
S. Ragi and E. K. Chong, “UAV path planning in a dynamic environment via partially observable Markov decision process,” IEEE Trans. Aerosp. Electron. Syst., vol. 49, no. 4, pp. 2397–2412, Oct. 2013.
[51]
C. Fulgenzi, A. Spalanzani, and C. Laugier, “Dynamic obstacle avoidance in uncertain environment combining PVOs and occupancy grid,” in Proc. IEEE Int. Conf. Robot. Automat., 2007, pp. 1610–1616.
[52]
W. Chung et al., “Safe navigation of a mobile robot considering visibility of environment,” IEEE Trans. Ind. Electron., vol. 56, no. 10, pp. 3941–3950, Oct. 2009.
[53]
L. Yang, Z. Li, J. Zeng, and K. Sreenath, “Bayesian optimization meets hybrid zero dynamics: Safe parameter learning for bipedal locomotion control,” in Proc. Int. Conf. Robot. Automat., Philadelphia, PA, USA, 2022, pp. 10456–10462.
[54]
H. Dai and R. Tedrake, “Planning robust walking motion on uneven terrain via convex optimization,” in Proc. Int. Conf. Humanoid Robots, 2016, pp. 579–586.
[55]
S. Caron, Q.-C. Pham, and Y. Nakamura, “Leveraging cone double description for multi-contact stability of humanoids with applications to statics and dynamics,” in Proc. Robot., Sci. Syst., 2015, pp. 1–9.
[56]
N. Smit-Anseeuw, C. D. Remy, and R. Vasudevan, “Walking with confidence: Safety regulation for full order biped models,” IEEE Robot. Autom. Lett., vol. 4, no. 4, pp. 4177–4184, Oct. 2019.
[57]
R. Grandia, A. J. Taylor, A. D. Ames, and M. Hutter, “Multi-layered safety for legged robots via control barrier functions and model predictive control,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 8352–8358.
[58]
S.-C. Hsu, X. Xu, and A. D. Ames, “Control barrier function based quadratic programs with application to bipedal robotic walking,” in Proc. IEEE Amer. Control Conf., 2015, pp. 4542–4548.
[59]
M. Dai, X. Xiong, and A. D. Ames, “Data-driven step-to-step dynamics based adaptive control for robust and versatile underactuated bipedal robotic walking,” 2022.
[60]
Z. Li et al., “Reinforcement learning for robust parameterized locomotion control of bipedal robots,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 2811–2817.
[61]
J. Siekmann, K. Green, J. Warila, A. Fern, and J. Hurst, “Blind bipedal stair traversal via sim-to-real reinforcement learning,” 2021, arXiv:2105.08328.
[62]
Y. Zhao and L. Sentis, “A three dimensional foot placement planner for locomotion in very rough terrains,” in Proc. Int. Conf. Humanoid Robots, 2012, pp. 726–733.
[63]
Y. Zhao, B. R. Fernandez, and L. Sentis, “Robust optimal planning and control of non-periodic bipedal locomotion with a centroidal momentum model,” Int. J. Robot. Res., vol. 36, no. 11, pp. 1211–1242, 2017.
[64]
A. Shamsah, Z. Gu, J. Warnke, S. Hutchinson, and Y. Zhao, “Integrated task and motion planning for safe legged navigation in partially observable environments,” 2023, arXiv:2110.12097.
[65]
Y. Li and J. Liu, “ROCS: A robustly complete control synthesis tool for nonlinear dynamical systems,” in Proc. Int. Conf. Hybrid Syst.: Comput. Control, 2018, pp. 130–135.
[66]
P. Holmes et al., “Reachable sets for safe, real-time manipulator trajectory design,” 2020, arXiv:2002.01591.
[67]
S. Bansal, M. Chen, S. Herbert, and C. J. Tomlin, “Hamilton-Jacobi reachability: A brief overview and recent advances,” in Proc. IEEE Conf. Decis. Control, 2017, pp. 2242–2253.
[68]
J. Liu, “Robust abstractions for control synthesis: Completeness via robustness for linear-time properties,” in Proc. Int. Conf. Hybrid Syst.: Comput. Control, 2017, pp. 101–110.
[69]
L. Jaulin, M. Kieffer, O. Didrit, and E. Walter, Applied Interval Analysis With Examples in Parameter and State Estimation, Robust Control and Robotics. London, U.K.: Springer London Ltd., Aug. 2001.
[70]
Y. Zhao, Y. Li, L. Sentis, U. Topcu, and J. Liu, “Reactive task and motion planning for robust whole-body dynamic locomotion in constrained environments,” Int. J. Robot. Res., vol. 41, no. 8, pp. 812–847, 2022.
[71]
N. Piterman, A. Pnueli, and Y. Sa'ar, “Synthesis of reactive(1) designs,” in Verification, Model Checking, and Abstract Interpretation. Berlin, Germany: Springer, 2006, pp. 364–380.
[72]
R. Ehlers and V. Raman, “Slugs: Extensible GR (1) synthesis,” in Proc. Int. Conf. Comput. Aided Verification, 2016, pp. 333–339.
[73]
J. Alonso-Mora, J. A. DeCastro, V. Raman, D. Rus, and H. Kress-Gazit, “Reactive mission and motion planning with deadlock resolution avoiding dynamic obstacles,” Auton. Robots, vol. 42, no. 4, pp. 801–824, 2018.
[74]
J. Englsberger, C. Ott, M. A. Roa, A. Albu-Schäffer, and G. Hirzinger, “Bipedal walking control based on capture point dynamics,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots. Syst., 2011, pp. 4420–4427.
[75]
R. Tedrake and the Drake Development Team, “Drake: Model-based design and verification for robotics,” 2019. [Online]. Available: https://drake.mit.edu
[76]
B. Morris and J. W. Grizzle, “Hybrid invariant manifolds in systems with impulse effects with application to periodic locomotion in bipedal robots,” IEEE Trans. Autom. Control, vol. 54, no. 8, pp. 1751–1764, Aug. 2009.
[77]
Y. Gong et al., “Feedback control of a Cassie bipedal robot: Walking, standing, and riding a Segway,” in Proc. IEEE Amer. Control Conf., 2019, pp. 4559–4566.
[78]
X. Xiong and A. Ames, “3-D underactuated bipedal walking via H-LIP based gait synthesis and stepping stabilization,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2405–2425, Aug. 2022.
[79]
S. B. Akers, “Binary decision diagrams,” IEEE Trans. Comput, vol. TC-27, no. 6, pp. 509–516, Jun. 1978.
Index Terms
- Integrated Task and Motion Planning for Safe Legged Navigation in Partially Observable Environments
Index terms have been assigned to the content through auto-classification.
Recommendations
Towards Safe Locomotion Navigation in Partially Observable Environments with Uneven Terrain
2020 59th IEEE Conference on Decision and Control (CDC)This study proposes an integrated task and motion planning method for dynamic locomotion in partially observable environments with multi-level safety guarantees. This layered planning framework is composed of a high-level symbolic task planner and a low-...
Comments
Information & Contributors
Information
Published In
1552-3098 © 2023 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See https://www.ieee.org/publications/rights/index.html for more information.
Publisher
IEEE Press
Publication History
Published: 01 December 2023
Qualifiers
- Research-article
Contributors
Other Metrics
Bibliometrics & Citations
Bibliometrics
Article Metrics
- 0Total Citations
- 0Total Downloads
- Downloads (Last 12 months)0
- Downloads (Last 6 weeks)0
Reflects downloads up to 08 Feb 2025