Pragmatic-Pedagogic Value Alignment

Springer Proceedings in Advanced Robotics
Series Editors: Bruno Siciliano · Oussama Khatib10
Nancy M. Amato
Greg HagerShawna ThomasMiguel Torres-Torriti Editors
Robotics
Research
The 18th International Symposium ISRR
Springer Proceedings in Advanced Robotics
Advisory Editors
Gianluca Antonelli, Department of Electrical and Information Engineering,
University of Cassino and Southern Lazio, Cassino, Italy
Dieter Fox, Department of Computer Science and Engineering, University ofWashington, Seattle, WA, USAKensuke Harada, Engineering Science, Osaka University Engineering Science,Toyonaka, JapanM. Ani Hsieh, GRASP Laboratory, University of Pennsylvania, Philadelphia,PA, USATorsten Kr öger, Karlsruhe Institute of Technology, Karlsruhe, Germany
Dana Kulic, University of Waterloo, Waterloo, ON, CanadaJaeheung Park, Department of Transdisciplinary Studies, Seoul NationalUniversity, Suwon, Korea (Republic of)10
Series Editors
Bruno Siciliano
Dipartimento di Ingegneria Elettrica
e Tecnologie dell ’Informazione
Universit /C18a degli Studi di Napoli
Federico II
Napoli, NapoliItalyE-mail: siciliano@unina.itOussama KhatibRobotics LaboratoryDepartment of Computer ScienceStanford UniversityStanford, CAUSAE-mail: khatib@cs.stanford.edu
The Springer Proceedings in Advanced Robotics (SPAR) publishes new develop-
ments and advances in the ﬁelds of robotics research, rapidly and informally but
with a high quality.
The intent is to cover all the technical contents, applications, and multidisci-
plinary aspects of robotics, embedded in the ﬁelds of Mechanical Engineering,
Computer Science, Electrical Engineering, Mechatronics, Control, and LifeSciences, as well as the methodologies behind them.
The publications within the “Springer Proceedings in Advanced Robotics ”are
primarily proceedings and post-proceedings of important conferences, symposiaand congresses. They cover signi ﬁcant recent developments in the ﬁeld, both of a
foundational and applicable character. Also considered for publication are editedmonographs, contributed volumes and lecture notes of exceptionally high quality
and interest.
An important characteristic feature of the series is the short publication time and
world-wide distribution. This permits a rapid and broad dissemination of research
results.
More information about this series at http://www.springer.com/series/15556

Nancy M. Amato Greg Hager
Shawna Thomas Miguel Torres-Torriti
Editors
Robotics Research
The 18th International Symposium ISRR
123
Editors
Nancy M. Amato
Department of Computer ScienceUniversity of Illinois at Urbana-Champaign
Urbana, IL, USAGreg Hager
Department of Computer ScienceJohns Hopkins University
Baltimore, MD, USA
Shawna Thomas
Department of Computer Science andEngineering
Texas A&M University
College Station, TX, USAMiguel Torres-TorritiDepartment of Electrical EngineeringPonti ﬁcia Universidad Cat /C19olica de Chile
Santiago, Chile
ISSN 2511-1256 ISSN 2511-1264 (electronic)
Springer Proceedings in Advanced Robotics
ISBN 978-3-030-28618-7 ISBN 978-3-030-28619-4 (eBook)https://doi.org/10.1007/978-3-030-28619-4
©Springer Nature Switzerland AG 2020
This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part
of the material is concerned, speci ﬁcally the rights of translation, reprinting, reuse of illustrations,
recitation, broadcasting, reproduction on micro ﬁlms or in any other physical way, and transmission
or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilarmethodology now known or hereafter developed.The use of general descriptive names, registered names, trademarks, service marks, etc. in thispublication does not imply, even in the absence of a speci ﬁc statement, that such names are exempt from
the relevant protective laws and regulations and therefore free for general use.The publisher, the authors and the editors are safe to assume that the advice and information in this
book are believed to be true and accurate at the date of publication. Neither the publisher nor the
authors or the editors give a warranty, expressed or implied, with respect to the material containedherein or for any errors or omissions that may have been made. The publisher remains neutral with regardto jurisdictional claims in published maps and institutional af ﬁliations.
This Springer imprint is published by the registered company Springer Nature Switzerland AG
The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland
Organization
ISRR 2017 is sponsored by the International Foundation of Robotics Research
(IFRR).
Organizing Committee
General Chairs: Nancy M. Amato, University of Illinois,
Urbana-Champaign, IL, USA
Greg Hager, Johns Hopkins University, MD, USA
Review Chair: Shawna Thomas, Texas A&M University, TX, USA
Local Arrangements Chair: Miguel Torres-Torriti, Ponti ﬁcia Universidad
Cat/C19olica de Chile, Chile
IFRR Of ﬁcers
IFRR President: Oussama Khatib, Stanford University, CA, USA
IFRR Secretary: Henrik Christensen, UC San Diego, CA, USA
Program Committee
Antonio Bicchi, Universita di Pisa e Instituto Italiano di Technologia, Italy
Aude Billard, /C19Ecole Polytechnique F /C19ed/C19erale de Lausanne, Switzerland
Ben Kuipers, University of Michigan, MI, USA
Daniela Rus, MIT, MA, USADarius Burschka, Technische Universit ätMünchen, Germany
David Hsu, National University of Singapore, Singapore
v
Dieter Fox, University of Washington, WA, USA
Eiichi Yoshida, AIST, JapanFrancois Conti, Stanford University, CA, USAGaurav Sukhatme, University of Southern California, CA, USA
Jana Kosecka, George Mason University, VA, USA
Jean-Paul Laumond, LAAS— Laboratoire d ’Analyse et d ’Architecture des
Syst/C19emes, France
Jing Xiao, UNC Charlotte, NC, USAJulie Shah, MIT, MA, USALydia Tapia, University of New Mexico, NM, USAMarcia K. O ’Malley, Rice University, TX, USA
Masayuki Inaba, University of Tokyo, Japan
Sami Haddadin, Leibniz Universit ät Hannover & FRANKA EMIKA, Germany
Tamim Asfour, Karlsruher Institut f ür Technologie, Germany
Yoshihiko Nakamura, University of Tokyo, Japanvi Organization
Foreword
At the dawn of the century ’s third decade, robotics is reaching an elevated level of
maturity and continues to bene ﬁt from the advances and innovations in its enabling
technologies. These all are contributing to an unprecedented effort to bringing
robots to human environment in hospitals, homes, factories, and schools; in the ﬁeld
for robots ﬁghting ﬁres, making goods and products, picking fruits and watering the
farmland, and saving time and lives. Robots today hold the promise for making aconsiderable impact on a wide range of real-world applications from industrialmanufacturing to healthcare, transportation, and exploration of the deep space andsea. Tomorrow, robots will become pervasive and touch upon many aspects of
modern life.
The Springer Tracts in Advanced Robotics (STAR) was launched in 2002 with
the goal of bringing to the research community the latest advances in the robotics
ﬁeld based on their signi ﬁcance and quality. During the latest 15 years, the STAR
series has featured publication of both monographs and edited collections. Amongthe latter, the proceedings of thematic symposia devoted to excellence in roboticsresearch, such as ISRR, ISER, FSR, and WAFR, have been regularly included inSTAR.
The expansion of our ﬁeld as well as the emergence of new research areas has
motivated us to enlarge the pool of proceedings in the STAR series in the past fewyears. This has ultimately led to launching a sister series in parallel to STAR. TheSpringer Proceedings in Advanced Robotics (SPAR) is dedicated to the timely
dissemination of the latest research results presented in selected symposia andworkshops.
This volume of the SPAR series brings the proceedings of the 18th edition of the
International Symposium of Robotics Research (ISRR). This symposium took place
in Puerto Varas, Chile from December 11th to December 14th, 2017. The
seven-part volume edited by Nancy M. Amato, Greg Hager, Shawna Thomas, andMiguel Torres-Torriti is a collection of a broad range of topics in robotics rangingfrom control to human ‒robot interaction, from mobility to manipulation, from
perception to planning, along with Blue Sky articles describing new early-stageideas that inspired discussion and debate. The content of these contributions
vii
provides a wide coverage of the current state of robotics research: the advances and
challenges in its theoretical foundation and technology basis, and the developmentsin its traditional and new emerging areas of applications. The diversity, novelty, andspan of the work unfolding in these areas reveal the ﬁeld’s increased pace of
development and expanded scope.
From its beautiful venue to its excellent program, the 13th edition of ISRR
culminates with this important reference on the current developments and newdirections in the ﬁeld of robotics —a true tribute to its contributors and organizers!
Naples, Italy Bruno Siciliano
Stanford, CA, USA Oussama Khatib
July 2019 SPAR Editorsviii Foreword
Preface
The 2017 International Symposium on Robotics Research (ISRR) took place in
beautiful Puerto Varas, Chile from December 11 –14, 2017. Consistent with the
history of ISRR, our goal was to design a meeting in an out-of-the-ordinary settingwhich would be a forum for debate and discussion with an emphasis on futurevisions for robotics research.
In a change from the past, the program featured papers of two different formats:
full-length papers of up to sixteen pages reporting on recent results, and a new BlueSky format that were six pages in length. The latter were introduced to describe newearly-stage ideas that would inspire discussion and debate. A prize for the bestpaper was supported by the CRA Computing Community Consortium. The ﬁnal
program featured 53 full-length papers organized into ten sessions, and 16 blue skypapers organized into three sessions. As is usual for ISRR, all papers were pre-sented in a single-track format to enhance interaction, debate, and discussion. Thepapers were presented in panel format, with each panel including a short intro-
duction, three 15-minute oral presentations, 3-minute spotlight talks for authors
presenting posters, and a substantial discussion period including authors from allpapers in the session.
In addition, the program featured four invited talks by leading researchers in the
ﬁeld. These included a perspective on manipulation by Matthew Mason, a per-spective on interaction and medical robotics by Marcia O ’Malley, a perspective on
biomechanics in the light of robotics computation by Yoshihiko Nakamura, and aperspective on service robotics by Henrik Christensen. The program also included
an industry panel which discussed current and future trends for robotics from the
industry perspective. We also organized a doctoral consortium for students fromaround the world with support from the National Science Foundation and severalcompanies.
Theﬁnal program provided some interesting perspectives on the topics and
themes that are receiving the highest interest in the ﬁeld. Planning was the topic
which had the most submissions; it was allocated three sessions in the ﬁnal pro-
gram. The topics discussed ranged from motion planning for hopping rovers to
trajectory optimization with contacts to planning for muscle-actuated bodies. It also
ix
included discussions on planning for soft robots, multi-robot coordination, and
improved methods for mapping and motion planning.
Learning and perception were also popular topics, with two sessions devoted to
each. In learning, topics ranged from adaptive policy transfer for stochastic
dynamical systems to topometric localization and reinforcement learning for
visual –inertial calibration. The perception session addressed a wide range of topics
including exploration and mapping, perception for grasping, object pose estimation,and sparse point registration.
Three topics were addressed with a single session: human –robot interaction,
manipulation, and control. Human –robot interaction addressed topics such as
viewing navigation as a cooperative activity, learning groundings for natural lan-guage interaction, and communicating robot arm motion intent with head-mounted
displays. Manipulation included presentations on sampling-based planning of
in-hand manipulation, a deterministic sampling-based veri ﬁcation algorithm for
path non-existence, and trajectory planning and stabilization for pushing. Controlpresentations included multi-objective optimal control with temporal logic models,reactive control of transitional legged robot maneuvers, and control of quadrotorsusing the Hopf ﬁbration.
As noted above, the program included the three blue sky sessions. The ﬁrst
session focused on cognitive topics —how to align values between a human and a
robot, a computational theory of pain, an axiomatic theory of risk, human-assistedrobot control, and human interaction challenges for autonomous agents in the wild.The second panel discussed planning and learning, touching on task representationsfor coupling perception to action in dynamic scenes, data-driven motion synthesis,self-directed lifelong learning for robot vision, deep multimodal learning, andBayesian active learning for adaptive motion planning. The ﬁnal session included
several papers on multi-robot systems —covering the value of diversity in robot
teams, controlling microrobot swarms using rotating magnetic dipole ﬁelds, and
large sensors with adaptive shape using micro-aerial vehicles. The session alsoincluded a paper on smart materials for robotics and the economics of cloud-basedrobot motion planning.
Theﬁnal awards for the blue sky papers were as follows:
First Place : Materials that Make Robots Smart by Nikolaus Correll and Christoffer
Heckman, University of Colorado Boulder
Second Place : Pragmatic-Pedagogic Value Alignment by Jaime F. Fisac, Monica
Gates, Jessica Hamrick, Chang Liu, Dylan Had ﬁeld-Menell, Malayandi Palaniappan,
Dhruv Malik, Shankar Sastry, Tom Grif ﬁths, and Anca Dragan, University of
California BerkeleyThird Place : DART: Diversity-enhanced Autonomy in Robot Teams by Nora
Ayanian, University of Southern California Autonomous Agents in the Wild:Human Interaction Challenges by Laura Major and Caroline E Harriott, DraperLabsx Preface
After the symposium, a one-day workshop was organized in Santiago on the
campus of the Ponti ﬁcia Universidad Cat /C19olica de Chile for all researchers, graduate,
and undergraduate students of the local universities. The day was divided into four
blocks of talks by leading experts and two blocks of spotlight presentations made
on recent developments followed by interactive sessions. The ﬁrst block of talks
covered topics related to human –robot collaboration and interaction including the
talks on interaction control by Sami Haddadin, engagement of patients in therapy
with wearable technologies by Marcie O ’Malley, a comparative overview of robot
and human manipulation by Matthew Mason, and a look at the evolution andongoing developments on human –robot collaboration by Oussama Khatib. The
second group of talks revolved around robot design and application challenges witha presentation on uni ﬁed digital human and humanoid motion synthesis for product
design and evaluation, ongoing humanoid and aerial robot development andapplications by Masayuki Inaba, and recent developments and trends in agriculturalrobotics by George Kantor. The third group of talks focused on perception andsensing topics. It included a talk by Gregory Hager on teaching robots usingcomputer vision techniques, a presentation on face recognition in challenginglow-quality images by Domingo Mery, and a talk on recent advances in randomﬁnite sets applied to robotic navigation and tracking by Martin Adams. The fourthblock of talks was concerned with dynamics and planning. The talks covered topics
on non-Cartesian task representation in dynamic environments by Darius Burschka,
motion planning for many arms and objects by Kostas Bekris, mobile manipulatorchallenges in agriculture and mining by Miguel Torres and applications ofsampling-based motion planning by Nancy Amato. The spotlights covered topicsranging from medical applications and learning to manipulation and control. With aparticipation of more than 100 undergraduate students from the different localuniversities, and 20% of the talks given by the Chilean researchers, the workshopwas an intense day that covered current challenges and a diversity of existing ideas.
In closing, we would like to acknowledge the help and support of everyone who
contributed to the organization of ISRR. We would like to especially acknowledgeour sponsors which included: Siemens, Honda Research Institute, Franka Emika,Automatica, Godelius, the National Science Foundation, the Computing CommunityConsortium, and the International Federation of Robotics Research.
Urbana, USA Nancy M. Amato
General Chair
Baltimore, USA Greg Hager
General Chair
College Station, USA Shawna Thomas
Review Chair
Santiago, Chile Miguel Torres-Torriti
May 2019 Local Arrangements ChairPreface xi
Sponsoring Institutions
automatica, Germany
FRANKA EMIKA, Germany
SK Godelius, Chile
Honda Research Institute USA, CA, USA
Siemens, Germany
Computing Community Consortium, DC, USA
xiii
Laboratory for Computational Sensing + Robotics, MD, USA
Parasol Laboratory, TX, USA
Robotics & Automation Laboratory, Chile
National Science Foundation, VA, USAxiv Sponsoring Institutions
Contents
Blue Sky Ideas
Controlling Homogeneous Microrobot Swarms In Vivo
Using Rotating Magnetic Dipole Fields ......................... 3
Jake J. Abbott and Henry C. Fu
DART: Diversity-Enhanced Autonomy in Robot Teams ............ 9
Nora Ayanian
Perspectives on Deep Multimodel Robot Learning ................ 17
Wolfram Burgard, Abhinav Valada, Noha Radwan, Tayyab Naseer,
Jingwei Zhang, Johan Vertens, Oier Mees, Andreas Eiteland Gabriel Oliveira
Task Representation in Robots for Robust Coupling of Perception
to Action in Dynamic Scenes
................................ 25
Darius Burschka
A Bayesian Active Learning Approach to Adaptive Motion
Planning ................................................ 33
Sanjiban Choudhury and Siddhartha S. Srinivasa
Materials That Make Robots Smart ........................... 41
Nikolaus Correll and Christoffer HeckmanPragmatic-Pedagogic Value Alignment
......................... 49
Jaime F. Fisac, Monica A. Gates, Jessica B. Hamrick, Chang Liu,
Dylan Had ﬁeld-Menell, Malayandi Palaniappan, Dhruv Malik,
S. Shankar Sastry, Thomas L. Grif ﬁths and Anca D. Dragan
The Economic Case for Cloud-Based Computation for RobotMotion Planning
.......................................... 59
Jeffrey Ichnowski, Jan Prins and Ron Alterovitz
xv
Autonomous Agents in the Wild: Human Interaction Challenges ..... 67
Laura Major and Caroline Harriott
How Should a Robot Assess Risk? Towards an Axiomatic Theory
of Risk in Robotics ........................................ 75
Anirudha Majumdar and Marco Pavone
Human-Assisted Humanoid Robot Control ...................... 85
Jaeheung Park, Yisoo Lee, Mingon Kim, Soonwook Hwang, Jaesug Jung
and Junhyung Kim
“Should Robots Feel Pain? ”—Towards a Computational Theory
of Pain in Autonomous Systems .............................. 91
Trevor Richardson, Indranil Sur and Heni Ben AmorLarge Sensors with Adaptive Shape Realised by Self-stabilised
Compact Groups of Micro Aerial Vehicles
...................... 101
Martin Saska
Self-directed Lifelong Learning for Robot Vision ................. 109
Tanner Schmidt and Dieter FoxDeep Coverage: Motion Synthesis in the Data-Driven Era
.......... 115
David A. Surovik and Kostas E. Bekris
Control
Multi-objective Optimal Control for Proactive Decision Making
with Temporal Logic Models ................................ 127
Sandeep P. Chinchali, Scott C. Livingston and Marco Pavone
Towards Reactive Control of Transitional Legged Robot
Maneuvers .............................................. 145
Jeffrey Duperret and Daniel E. Koditschek
Compositional and Contract-Based Veri ﬁcation for Autonomous
Driving on Road Networks .................................. 163
Lucas Liebenwein, Wilko Schwarting, Cristian-Ioan Vasile,
Jonathan DeCastro, Javier Alonso-Mora, Sertac Karaman and Daniela Rus
Towards Interaction, Disturbance and Fault Aware Flying
Robot Swarms ........................................... 183
Teodor Tomi ćand Sami Haddadin
Control of Quadrotors Using the Hopf Fibration on SO(3)......... 199
Michael Watterson and Vijay Kumar
Towards Uni ﬁed Framework for Trajectory Optimization
Using General Differential Kinematics and Dynamics ............. 217
Eiichi Yoshida and Ko Ayusawaxvi Contents
Toward a Human(oid) Motion Planner ........................ 233
Eiichi Yoshida, Ko Ayusawa, Yusuke Yoshiyasu, Adrien Escande
and Abderrahmane Kheddar
Human-Robot Interaction
Accelerating Motion Planning for Learned Mobile Manipulation
Tasks Using Task-Guided Gibbs Sampling ...................... 251
Chris Bowen and Ron Alterovitz
Synchronicity Trumps Mischief in Rhythmic Human-Robot
Social-Physical Interaction .................................. 269
Naomi T. Fitter and Katherine J. Kuchenbecker
Viewing Robot Navigation in Human Environment
as a Cooperative Activity ................................... 285
Harmish Khambhaita and Rachid Alami
Communicating Robot Arm Motion Intent Through Mixed Reality
Head-Mounted Displays .................................... 301
Eric Rosen, David Whitney, Elizabeth Phillips, Gary Chien,James Tompkin, George Konidaris and Stefanie Tellex
Learning Unknown Groundings for Natural Language Interaction
with Mobile Robots
....................................... 317
Mycal Tucker, Derya Aksaray, Rohan Paul, Gregory J. Steinand Nicholas Roy
Comparing Robot Grasping Teleoperation Across Desktop
and Virtual Reality with ROS Reality
......................... 335
David Whitney, Eric Rosen, Elizabeth Phillips, George Konidarisand Stefanie Tellex
Learning
Situated Bayesian Reasoning Framework for Robots Operating
in Diverse Everyday Environments
........................... 353
Sonia Chernova, Vivian Chu, Angel Daruna, Haley Garrison, Meera Hahn,
Priyanka Khante, Weiyu Liu and Andrea Thomaz
A Multiview Approach to Learning Articulated Motion Models ..... 371
Andrea F. Daniele, Thomas M. Howard and Matthew R. WalterReachability and Differential Based Heuristics for Solving Markov
Decision Processes
........................................ 387
Shoubhik Debnath, Lantao Liu and Gaurav Sukhatme
Learning to Singulate Objects Using a Push Proposal Network ...... 405
Andreas Eitel, Nico Hauff and Wolfram BurgardContents xvii
Variational Hilbert Regression with Applications to Terrain
Modeling ............................................... 421
Vitor Campanholo Guizilini and Fabio Tozeto Ramos
ADAPT: Zero-Shot Adaptive Policy Transfer for Stochastic
Dynamical Systems ........................................ 437
James Harrison, Animesh Garg, Boris Ivanovic, Yuke Zhu,
Silvio Savarese, Li Fei-Fei and Marco Pavone
Planning Multi- ﬁngered Grasps as Probabilistic Inference
in a Learned Deep Network ................................. 455
Qingkai Lu, Kautilya Chenna, Balakumar Sundaralingam
and Tucker Hermans
Reinforcement Learning for Assisted Visual-Inertial Robotic
Calibration .............................................. 473
Fernando Nobre and Christoffer Heckman
Bayesian Optimisation for Safe Navigation Under Localisation
Uncertainty .............................................. 489
Rafael Oliveira, Lionel Ott, Vitor Guizilini and Fabio Ramos
Topometric Localization with Deep Learning .................... 505
Gabriel L. Oliveira, Noha Radwan, Wolfram Burgard and Thomas Brox
Manipulation
Sampling-Based Planning of In-Hand Manipulation with External
Pushes ................................................. 523
Nikhil Chavan-Da ﬂe and Alberto Rodriguez
Simply Grasping Simple Shapes: Commanding a Humanoid Hand
with a Shape-Based Synergy ................................. 541
Logan C. Farrell, Troy A. Dennis, Julia Badger and Marcia K. O ’Malley
Fundamental Limitations in Performance and Interpretabilityof Common Planar Rigid-Body Contact Models
.................. 555
Nima Fazeli, Samuel Zapolsky, Evan Drumwright
and Alberto Rodriguez
Object Shape Estimation Through Touch-Based Continuum
Manipulation ............................................ 573
Huitan Mao and Jing Xiao
Caging and Path Non-existence: A Deterministic Sampling-Based
Veriﬁcation Algorithm ..................................... 589
Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokornyand Danica Kragicxviii Contents
Pushing Revisited: Differential Flatness, Trajectory Planning
and Stabilization .......................................... 605
Jiaji Zhou and Matthew T. Mason
Perception
Conﬁdence-Rich Grid Mapping .............................. 623
Ali-akbar Agha-mohammadi, Eric Heiden, Karol Hausman
and Gaurav Sukhatme
Multirobot Cooperative Localization Algorithm with Explicit
Communication and Its Topology Analysis ..................... 643
Tsang-Kai Chang, Shengkang Chen and Ankur Mehta
Localization in Inconsistent WiFi Environments ................. 661
Hsin-Min Cheng and Dezhen SongTime-of-Flight Depth Datasets for Indoor Semantic SLAM
......... 679
Vijaya K. Ghorpade, Dorit Borrmann, Paul Checchin, Laurent Malaterre
and Laurent Trassoudaine
Globally Optimal Object Pose Estimation in Point Clouds
with Mixed-Integer Programming ............................ 695
Gregory Izatt, Hongkai Dai and Russ Tedrake
Probabilistic Boundary Coverage for Unknown Target Fields
with Large Perception Uncertainty and Limited Sensing Range ...... 711
Binbin Li and Dezhen Song
Sharing Heterogeneous Spatial Knowledge: Map Fusion
Between Asynchronous Monocular Vision and Lidaror Other Prior Inputs
...................................... 727
Yan Lu, Joseph Lee, Shu-Hao Yeh, Hsin-Min Cheng, Baifan Chenand Dezhen Song
Sparse Point Registration
.................................. 743
Rangaprasad Arun Srivatsan, Prasad Vagdargi and Howie ChosetAutonomous Exploration with Expectation-Maximization
.......... 759
Jinkun Wang and Brendan EnglotMap-Based Localization Under Adversarial Attacks
.............. 775
Yulin Yang and Guoquan Huang
Planning
Safety, Challenges, and Performance of Motion Planners
in Dynamic Environments .................................. 793
Hao-Tien (Lewis) Chiang, Baisravan HomChaudhuri, Lee Smithand Lydia TapiaContents xix
Global Inverse Kinematics via Mixed-Integer Convex
Optimization ............................................. 809
Hongkai Dai, Gregory Izatt and Russ Tedrake
Parallelizing Synthesis from Temporal Logic Speci ﬁcations
by Identifying Equicontrollable States ......................... 827
Sumanth Dathathri, Ioannis Filippidis and Richard M. MurrayFast Action Elimination for Ef ﬁcient Decision Making and Belief
Space Planning Using Bounded Approximations
................. 843
Khen Elimelech and Vadim IndelmanFunctional Path Optimisation for Exploration in Continuous
Occupancy Maps
......................................... 859
Gilad Francis, Lionel Ott and Fabio Ramos
Stochastic Motion Planning for Hopping Rovers on Small Solar
System Bodies ............................................ 877
Benjamin Hockman and Marco Pavone
Perception-Aware Motion Planning via Multiobjective Search
on GPUs ................................................ 895
Brian Ichter, Benoit Landry, Edward Schmerling and Marco Pavone
Decoupling Constraints from Sampling-Based Planners ............ 913
Zachary Kingston, Mark Moll and Lydia E. KavrakiFast Anytime Motion Planning in Point Clouds by Interleaving
Sampling and Interior Point Optimization
...................... 929
Alan Kuntz, Chris Bowen and Ron Alterovitz
From Quasi-static to Kinodynamic Planning for Spherical Tensegrity
Locomotion .............................................. 947
Zakary Little ﬁeld, David Surovik, Weifu Wang and Kostas E. Bekris
Conﬁguration Recognition with Distributed Information
for Modular Robots ....................................... 967
Chao Liu and Mark Yim
Variational Contact-Implicit Trajectory Optimization ............. 985
Zachary Manchester and Scott Kuindersma
Parametric Trajectory Libraries for Online Motion Planning
with Application to Soft Robots .............................. 1001
Tobia Marcucci, Manolo Garabini, Gian Maria Gasparri, Alessio Artoni,Marco Gabiccini and Antonio Bicchi
Interleaving Planning and Control for Deformable Object
Manipulation
............................................ 1019
Dale McConachie, Mengyao Ruan and Dmitry Berensonxx Contents
Controlling Muscle-Actuated Articulated Bodies in Operational
Space .................................................. 1037
Samir Menon, Takatoki Migimatsu and Oussama Khatib
Multi-robot Trajectory Generation for an Aerial Payload
Transport System ......................................... 1055
Sarah Tang, Koushil Sreenath and Vijay KumarContents xxi
Blue Sky Ideas
Controlling Homogeneous Microrobot
Swarms In Vivo Using Rotating MagneticDipole Fields
Jake J. Abbott and Henry C. Fu
Biomedical “microrobots”, which are typically conceived as simple microstructures
with no actual computational intelligence on board, can be functionalized to performtargeted therapy in the body such as chemotherapy or hyperthermia [ 10,14]. The
majority of the work on biomedical microrobots has focused on magnetic swimmers
and screws that use a chiral structure (e.g., a helix) to convert magnetic torque gen-erated by a rotating magnetic ﬁeld into forward propulsion, although we have shown
that achiral structures can also be propelled in the same fashion [ 3]. This method of
propulsion compares favorably to other methods of magnetic propulsion [ 1].
Biomedical applications will likely require the control of a large number of micro-
robots (i.e., a swarm) to accomplish a therapeutic task. However, this is difﬁcult for
two reasons. First, the entire swarm will be subject to some globally applied magneticﬁeld, and the distances between individual microrobots will be small compared to
their distances from the ﬁeld-generation source, resulting in them experiencing very
similar magnetic ﬁelds to each other. Second, for clinical use it may be unrealistic toassume that each microrobot can be individually localized; rather, a medical image
will show a swarm of microrobots as a blob in the image [ 13].
To date, research on the control of multiple magnetic microrobots has either con-
sidered a small set that are individually localized [ 5,6], or a swarm that is controlled
as an aggregate unit with no ability to differentiate microrobots [ 13], or a swarm in
which microrobot heterogeneity is required for differentiation [ 2,15]. No prior work
This work was supported by the National Science Foundation under awards #1435827 and
#1650968.
J. J. Abbott ( B)·H. C. Fu
Department of Mechanical Engineering and Robotics Center, University of Utah,Salt Lake City, UT, USAe-mail: jake.abbott@utah.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_13
4 J. J. Abbott and H. C. Fu
has proposed a solution for the comprehensive control of a swarm of batch-fabricated
homogeneous microrobots, a likely scenario for practical realization.
In this paper, we propose two ways to think about controlling a swarm of homo-
geneous microrobots in vivo. We can treat the swarm as an object to be manipulated,
and perform basic manipulation primitives on the swarm such as “move the aggregate
swarm to a new location,” “spread out the swarm,” “gather the swarm together,” or“split the swarm into smaller swarms and move them to separate locations.” Alterna-
tively, we can directly control the concentration ﬁeld of the microrobots throughout a
volume of interest. We will describe how the unique properties of rotating magneticdipole ﬁelds can be utilized to make both of these strategies possible.
Our group has put signiﬁcant effort into characterizing and utilizing magnetic
dipole ﬁelds due to their numerous desirable properties, ﬁrst and foremost beingthat they have a simple analytic representation that lends itself to analysis and real-
time computation. Dipole ﬁelds are generated by spherical permanent magnets,
1and
the ﬁelds generated by certain other permanent-magnet geometries2and specialized
electromagnetic sources3can be accurately approximated by the dipole model at
clinically realistic distances. It is easy to conceive of a clinical scenario in which
the patient is surrounded by one or more relatively small dipole sources in close
proximity to the location of interest, as opposed to designing a large one-size-ﬁts-all
system into which the patient is placed (which is typical in prior work).
A magnetic dipole moment mgenerates a ﬁeld hat each point p(with respect to
the dipole), which is described by the point-dipole equation:
h=1
4π/bardblp/bardbl3/bracketleftBig
3ˆpˆpT−I/bracketrightBig
m=1
4π/bardblp/bardbl3Hm (1)
We see that the magnetic ﬁeld is nonlinear with respect to position, with a strength
that decays rapidly from the source as ∼/bardblp/bardbl−3, but the ﬁeld is linear with respect
to the dipole itself. We can use Hto capture the shape of the dipole ﬁeld, which is
invariant to distance from the source.4
We showed in [ 8] that if a dipole moment is rotated about, and orthogonal to, some
axisˆωm, then the ﬁeld at any given point in space will rotate about, and orthogonal to,
some axis ˆωh, with the same period. The inverse problem was also solved (i.e., How
should we rotate the dipole to achieve some desired ˆωhat some desired location?):
1We have developed a spherical-permanent-magnet robotic end-effector capable of continuous
singularity-free rotation of the spherical magnet about any axis [ 17].
2We show in [ 11] that the ﬁelds of cubic and certain cylindrical permanent magnets—which are easy
to fabricate (and purchase in variety of sizes), ﬁxture, and manipulate—are accurately approximatedby the dipole model not far outside of their minimum bounding sphere.
3We developed an electromagnetic source called the Omnimagnet, comprising three mutually or-
thogonal coils with a common soft-magnetic spherical core, all in a cubic package [ 12]. The Omni-
magnet was optimized such that its ﬁeld is accurately approximated by the dipole model just outsideof its minimum bounding sphere.
4We use the “hat” notation to describe unit-normalized vectors (e.g., ˆp≡p//bardblp/bardbl), as well as
pointing-direction vectors that are inherently unit length (e.g., ˆω).
Controlling Homogeneous Microrobot Swarms … 5
N
SN
SmN
SN
SN
SN
SN
S
N
SN
SN
SN
S
N
S(a) (b)(c)
(d)
N
S
N
SN
S
N
SN
S
N
SNS
Fig. 1 A magnetic dipole moment m, with instantaneous ﬁeld lines shown, is rotated about axis
ˆωm, with swarms of microrobots shown at different locations. The microrobots are shown simply as
rotating magnets without any chiral structure, with their respective ˆωhvectors shown. At locations
along the axis of ˆωm, swarms are driven straight while either agathering or bspreading the swarm.
At a location that is orthogonal to ˆωmsuch as ( c), the swarm does not gather/spread, but it is steered.
At general locations, such as shown in ( d), the swarm will experience both gathering/spreading and
steering
ˆωh=/hatwiderH−1ˆωm⇐⇒ ˆωm=/hatwideHˆωh (2)
where H−1=(H−I)/2 is always well conditioned. The body of a microrobot
located at pwill tend to align with ˆωhas its magnetic element synchronously rotates
with h, andˆωhwill become the microrobot’s “forward” direction.
If we consider a swarm of microrobots at some nominal position (e.g., the centroid
of the swarm), we observe that each microrobot will be at a different pand will thus
experience a different ˆωh. As shown in Fig. 1, there will be locations in the rotating
dipole ﬁeld in which we can conceive of basic swarm manipulation primitives, such as
spreading out or gathering together while moving forward, or steering while moving
forward. If the patient is surrounded by multiple sources, or a single moving source,
such motions will be possible in arbitrary directions.
The phenomena that we have discussed become less pronounced as we consider
locations with increasing distance from the dipole source. It is likely that we will
need to utilize nonholonomic control techniques to amplify the phenomena. For
example, consider the scenario depicted in Fig. 2in which two dipole sources are
on opposite sides of the swarm. By alternating between each source performing the
manipulation primitive of Fig. 1b, the swarm can be made to effectively spread out in
6 J. J. Abbott and H. C. Fu
N
SN
SN
S
Fig. 2 As the rotating dipole source is alternated (images going from left to right), the swarm can
be made to spread out without signiﬁcant net motion of the centroid
Average Microrobot
Forward Velocity v
Dipole Rotation Frequency, || ωm||12N
SN
SN
S1 2ωm^
Fig. 3 In [9] we showed how the step-out regime can be exploited to differentiate heterogeneous
microrobots in a rotating uniform ﬁeld. The same concepts can be applied to the homogeneous
microrobots in a nonuniform dipole ﬁeld of interest here. Note: the step-out frequencies are not
shown to scale with the microrobot locations depicted
place, without a net movement of the centroid. An analogous gathering of the swarm
can be visualized by considering the microrobots in Fig. 1a.
Until this point, we have been assuming that all of the microrobots are rotating
synchronously with the applied ﬁeld. However, that need not be the case. Consider
the two microrobots swimming along the ˆωmaxis in Fig. 1b. If they are both close
enough to the dipole source that the magnetic ﬁeld is sufﬁciently strong to keep
them both rotating synchronously with the ﬁeld, then they will move forward at the
same average velocity. However, the farther microrobot will be the ﬁrst to reach the
“step-out” regime in which the ﬁeld is too weak to generate synchronous rotation,
at which point the microrobot’s average forward velocity will decrease. As shown
in Fig. 3, this yields a forward velocity vin the ˆωhdirection that transitions between
linear and nonlinear dependence on the ﬁeld rotation frequency /bardblωm/bardbl(in general, the
forward velocity is a function of both /bardblωm/bardblandp). Consider the inset in Fig. 3:t h i s
phenomenon will enable the closer microrobot to catch up with the farther, effectively
creating another means to gather the swarm. An analogous spreading of the swarm
can be visualized by considering the microrobots in Fig. 1a.
A general description of the behavior of a population of microrobots can be con-
structed as follows. Consider a concentration (density) ﬁeld of microrobots ρat some
time. Then, since at each position p, microrobots move with velocity magnitude vin
direction ˆωh, we can interpret vˆωhas the standard velocity ﬁeld for microrobots used
in continuum ﬂuid mechanics [ 7]. Therefore, for example, by number conservation
Controlling Homogeneous Microrobot Swarms … 7
the density proﬁle evolves in time as
∂ρ
∂t=− ∇· (ρvˆωh). (3)
This formalism can be directly related to the primitives in Fig. 1. To determine
if a local population of microrobots is spreading or gathering, one would examine
the rate of change in density moving with that local population , i.e., the material
derivative of the density:
Dρ
Dt≡∂ρ
∂t+(vˆωh·∇)ρ=−ρ∇·(vˆωh). (4)
A positive/negative value ofDρ
Dtindicates an increasing/decreasing population con-
centration and corresponds to gathering/spreading of microrobots. Steering of mi-crorobots is determined by the rate of change of their orientation ( ˆω
h) moving with
the local population:
Dˆωh
Dt≡∂ˆωh
∂t+/parenleftbig
vˆωh·∇/parenrightbig
ˆωh. (5)
The measures in Eqs. 4and5can be combined to describe all of the scenarios depicted
in Fig. 1.
Alternatively, time integration of Eq. 3sufﬁces to solve the forward problem of
how a swarm described by an initial density ρwould behave for a given time-
sequence of dipole strengths and rotation rates, which also enables motion planners
that do not rely on primitives. We can conceptualize such a motion planner as follows:(1) Voxelize the given volume, with each of the voxels having a desired concentration
of microrobots. From these values, compute the desired centroid and variance of the
swarm. (2) Using medical images, estimate the current concentration in each voxel,as well as the current centroid and variance. (3) Construct an objective function that
penalizes a combination of the errors in the quantities found in Step 2. This objective
function would likely work by driving the centroid and variance of the swarm in thecorrect direction initially, and then ﬁne-tuning the individual voxel concentrations.
(4) For each of the dipole sources, determine the value of ˆω
mthat would minimize
the objective function locally. (5) From the set of ˆωmvectors found in Step 4, choose
the one that minimizes the objective function and implement it for a short period of
time. (6) Go back to Step 2 and iterate until convergence.
In this paper we have described kinematic models for how rotating dipole ﬁelds
can be utilized in the control of in vivo microrobot swarms. However, we did not
model the transient as a microrobot aligns itself with a rapid change in ˆωh, nor did
we model other magnetic and ﬂuidic interactions that will certainly exist [ 4,16,
18]. In light of this fact, the swarm-manipulation techniques that we have described
should be thought of as feedforward models for the purpose of control, and as pro-cess models for the purpose of estimation, but with the knowledge that closed-loop
feedback of the swarm via medical imaging will be required to ensure the swarm
keeps evolving as desired. As we learn more about the unmodeled effects, it may be
8 J. J. Abbott and H. C. Fu
possible to incorporate them into improved kinematic models. Assuming microrobots
are not deployed in a ﬂowing environment (e.g., the bloodstream), we anticipate inter-
microrobot magnetic interactions to be the most signiﬁcant disturbance to our model.When might these magnetic interactions ruin our model of control by a dipole ﬁeld?
In typical cases we estimate that a microrobot’s magnetic ﬁeld is comparable to the
external ﬁeld at ∼2 magnetic-element lengths, i.e., for a quite dense swarm. Although
even small magnetic attraction can lead to (irreversible) aggregation, for less dense
swarms our methods might be used to prevent such aggregation.
References
1. Abbott, J.J., Peyer, K.E., Cosentino Lagomarsino, M., Zhang, L., Dong, L., Kaliakatsos, I.K.,
Nelson, B.J.: How should microrobots swim? Int. J. Robot. Res. 28(11–12), 3663–3667 (2009)
2. Cheang, U.K., Lee, K., Julius, A.A., Kim, M.J.: Multiple-robot drug delivery strategy through
coordinated teams of microswimmers. Appl. Phys. Lett. 105(8), 083705 (2014)
3. Cheang, U.K., Meshkati, F., Kim, D., Kim, M.J., Fu, H.C.: Minimal geometric requirements
for micropropulsion via magnetic rotation. Phys. Rev. E 90, 033007 (2014)
4. Cheang, U.K., Meshkati, F., Kim, H., Lee, K., Fu, H.C., Kim, M.J.: Versatile microrobotics
using simple modular subunits. Sci. Rep. 6, 30472 (2016)
5. Chowdhury, S., Jing, W., Cappelleri, D.J.: Controlling multiple microrobots: recent progress
and future challenges. J. Micro-Bio Robot. 10(1–4), 1–11 (2015)
6. Diller, E., Giltinan, J., Sitti, M.: Independent control of multiple magnetic microrobots in three
dimensions. Int. J. Robot. Res. 32(5), 614–631 (2013)
7. Kunda, P., Cohen, I., Dowling, D.: Fluid Mechanics, 6th edn. Academic Press, New York
(2015)
8. Mahoney, A.W., Abbott, J.J.: Generating rotating magnetic ﬁelds with a single permanent
magnet for propulsion of untethered magnetic devices in a lumen. IEEE Trans. Robot. 30(2),
411–420 (2014)
9. Mahoney, A.W., Nelson, N.D., Peyer, K.E., Nelson, B.J., Abbott, J.J.: Behavior of rotating mag-
netic microrobots above the step-out frequency with application to control of multi-microrobotsystems. Appl. Phys. Lett. 104(14), 144101 (2014)
10. Nelson, B.J., Kaliakatsos, I.K., Abbott, J.J.: Microrobots for minimally invasive medicine. Ann.
Rev. Biomed. Eng. 12, 55–85 (2010)
11. Petruska, A.J., Abbott, J.J.: Optimal permanent-magnet geometries for dipole ﬁeld approxi-
mation. IEEE Trans. Magn. 2, 811–819 (2013)
12. Petruska, A.J., Abbott, J.J.: Omnimagnet: an omnidirectional electromagnet for controlled
dipole-ﬁeld generation. IEEE Trans. Magn. 50(7), 8400810 (2014)
13. Servant, A., Qiu, F., Mazza, M., Kostarelos, K., Nelson, B.J.: Controlled in vivo swimming of
a swarm of bacteria-like microrobotic ﬂagella. Adv. Mater. 27(19), 2981–2988 (2015)
14. Sitti, M., Ceylan, H., Hu, W., Giltinan, J., Turan, M., Yim, S., Diller, E.: Biomedical applications
of untethered mobile milli/microrobots. Proc. IEEE 103(2), 205–224 (2015)
15. Tottori, S., Sugita, N., Kometani, R., Ishihara, S., Mitsuishi, M.: Selective control method for
multiple magnetic helical microrobots. J. Micro-Nano Mechatronics 6(3–4), 89–95 (2011)
16. Tottori, S., Zhang, L., Peyer, K.E., Nelson, B.J.: Assembly, disassembly, and anomalous propul-
sion of microscopic helices. Nano Lett. 13(9), 4263–4268 (2013)
17. Wright, S.E., Mahoney, A.W., Popek, K.M., Abbott, J.J.: The spherical-actuator-magnet ma-
nipulator: a permanent-magnet robotic end-effector. IEEE Trans. Robot. 33(5), 1013–1024
(2017)
18. Yu, J., Xu, T., Lu, Z., Vong, C.I., Zhang, L.: On-demand disassembly of paramagnetic nanopar-
ticle chains for microrobotic cargo delivery. IEEE Trans. Robot. 99, 1–13 (2017)
DART: Diversity-Enhanced Autonomy in
Robot T eams
Nora Ayanian
1 Introduction
The ﬁeld of multi-robot systems (MRS) is growing at a rapid pace. Research in MRS
spans many different areas, including automated delivery [ 1–3], surveillance [ 4],
and disaster response [ 5,6]. There have also been many successful demonstra-
tions of increasing numbers of robots [ 7–12]. MRS have also been successfully
deployed in the ﬁeld including in warehousing [ 13], manufacturing [ 14], and enter-
tainment [ 15]. While these outcomes show the promise of MRS, the environments
in which MRS have been successful are highly controlled, and some are highlyinstrumented, enabling precise tuning of controllers and nearly perfect knowledge
of environmental conditions.
Many environments where MRS could be beneﬁcial are not highly controlled or
equipped with the extensive infrastructure often necessary to coordinate large teams
of robots with state-of-the-art algorithms. For example, containing wildﬁres, search-
ing collapsed buildings, patrolling borders, monitoring infrastructure, and containingoil spills all occur in highly dynamic and unique environments (no two collapsed
buildings are the same), with high uncertainty and little control over other non-robot
agents in the environment. One of the most desirable beneﬁts of MRS is robustness ,
wherein robots can compensate for loss of capabilities by relying on other robots in
the team. However, the uncertainty of many real-world environments renders current
algorithms, even those designed for robustness, ineffectual. The reason for this is not
due to limitations in robot hardware, but in how multi-robot problems are solved.
Many controllers are so specialized and optimized for speciﬁc capabilities and con-
ditions that they cannot cope with uncertainty. Thus, the true beneﬁts of robustnessin teams of robots have yet to be achieved.
N. Ayanian ( B)
University of Southern California, Los Angeles, CA, USAe-mail: ayanian@usc.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_29
10 N. Ayanian
2 Motivation
In disaster response alone, the potential impact of autonomous MRS is substantial:
60,000 people die each year in natural disasters [ 16]. The company DJI announced
that one properly equipped drone can ﬁnd a missing person more than ﬁve times
faster than traditional search methods [ 17]. This makes robots an ideal tool for
disaster response.
However, most robots used in search and rescue today are teleoperated [ 18], requir-
ing trained operators which may not be nearby. Disaster response that is autonomous,
without the need for an expert operator, can reduce response time and save more lives,
especially when a trained operator may be hours away.
The potential applications of autonomous MRS go well beyond disaster response,
including military, agriculture, transportation, manufacturing, and fulﬁllment appli-
cations. However, current solutions for MRS have not successfully transitioned from
controlled environments such as laboratories or warehouse facilities to the inher-
ently high uncertainty in these complex environments. Without infrastructure that
provides communication and localization, and without knowledge of or control over
the environment, current state-of-the-art methods fail.
While the ﬁeld of MRS has advanced signiﬁcantly, the same problem-solving
paradigm has remained. First, the problem is deﬁned. Next, complexity is reduced
by making several assumptions to simplify the problem, such as terrain and commu-
nication range. Finally, an optimal solution to that speciﬁc problem is designed and
applied to all the robots in the team. This paradigm (Fig. 1a) limits the capability of
MRS to cope with real-world environments. The solutions are brittle, as the assump-
tions made are easily invalidated and the optimized controller is not designed for real
environments. In the best case, the controller is able to overcome these challenges,
but it is not a good solution to the problem. In the worst case, the controller cannot
cope, causing mission failure, loss of high-value assets, and casualties; after all, if
the same failed controller is applied to all robots, all of them will fail.
Problem
SolutionAssumptions
(a) Curent MRS ParadigmProblem
Solution
3Solution
1
Solution
2Solution
4Solution
5Different
Assumptions
(b) Proposed MRS Paradigm
Fig. 1 a The current MRS problem-solving paradigm is linear, applying the same solution to all
robots. bThe proposed novel paradigm takes advantage of diversity in controllers to handle various
scenarios
DART: Diversity-Enhanced Autonomy in Robot Teams 11
3 A Potential Solution
Instead of applying the same controller to all robots, a new approach leveraging
diversity in policies within the robot team can allow MRS to better cope with uncer-tain environments. Using an ensemble of diverse control policies to accomplish a
coordinated task within a single team of robots can enable the team to adjust to dif-
ferent conditions. For example, with two unmanned aerial vehicles (UA V) on a largesecurity task, a natural result of using an ensemble of controllers is for one UA V to
position itself high, to view the entire area, while the other UA V takes a closer look
at areas of interest.
Diversity is well established as a way to improve the performance of human work-
groups: studies have shown repeatedly that diverse groups outperform homogeneous
groups [ 19–22]. Thus, the current problem-solving paradigm in MRS does not reﬂect
an effective approach to working in groups . Instead of the current paradigm of solv-
ing problems and uniformly applying the solution to all robots as in Fig. 1a, several
solutions to the problem under different assumptions and different styles of inter-
action should be developed and the best approaches combined to take advantage of
their strengths under different conditions, as in Fig. 1b.
4 Current State of the Art
While diversity of robots with different physical embodiment or capabilities has pre-
viously been studied [ 23–26], there has been relatively little exploration into diversity
in control policies within a single team of robots. Most research in this area is a result
of studying ants that take different roles in foraging and house hunting [ 27–29]o r
collective transport [ 30], and applied to similar problems in robotics. Unfortunately,
in trying to model ant algorithms closely, these works do not take advantage of robot
capabilities, including communication, sensing, and computation.
In Tang and Parker’s ASyMTRe, robots take different roles depending on environ-
mental conditions [ 31,32], but the robots are all programmed to react the same. This
leaves them vulnerable to unforeseen changes in capabilities or the environment, and
does not enable robots to individually adjust their approaches.
A majority of work exploring control diversity in robots exists in behavior-based
systems, most notably Balch’s work in learning behavioral specialization for robot
teams [ 33,34]. Goldberg and Matari´ c evaluate multi-robot controllers based on the
amount of interference and describe caste arbitration, where all robots have the samecapabilities, but have different conditions for activating behaviors [ 35].
More recently, evolutionary robotics and agent-based systems have been appear-
ing as a method for encouraging behavioral diversity and plasticity (individualschanging roles over time). Mouret and Doncieux review and benchmark published
approaches to behavioral diversity, and show that fostering behavioral diversity
substantially improves the evolutionary process in the investigated experiments,
12 N. Ayanian
regardless of task [ 36]. Pugh et al. review quality diversity algorithms, which have
resulted in a new class of algorithms that return an archive of diverse, high-quality
behaviors in a single run [ 37]. V assiliades and Christodoulou design behaviorally
plastic agents (capable of switching between different behaviors in response to envi-
ronmental changes) [ 38] and Umedachi et al. attempt to understand the underlying
mechanism of the behavioral diversity of animals, then use the ﬁndings to buildtruly adaptive robots [ 39]. However, all of these approaches focus on training agents
to act independently in the environment, and thus are not directly applicable to
multi-robot problems where task completion relies on tight coordination, such asbox-pushing, shape formation, wildﬁre containment, cooperative transport, etc. Fur-
thermore, agents are trained in the environments where they will be used, which,
especially in natural disasters, may not be possible.
Heterogeneity has also been studied extensively in insect and animal behavior.
Jandt et al. study personality at multiple levels with regard to behavioral syndromes
and insect societies, discussing ﬁtness consequences of intra-colony behavioralvariation [ 40]. Speciﬁcally, under varying environmental conditions, maintaining
a mixture of individuals with different behavioral types may be more effective than
individuals switching between behavioral types, which might be costly and inefﬁ-
cient. Slower, more accurate individuals can bring large quantities of food back to
the colony when good abundance is constant, whereas faster “sloppier” individualsmight be more efﬁcient at exploiting resources in more frequently changing envi-
ronments [ 41]. Burns and Dyer [ 42] found that ant colonies that maintain a mixture
of different foraging types within a group allow colonies to respond more quicklyto environmental ﬂuctuation. On the other hand, maintaining a mixture of inﬂexible
behavioral types can incur costs to the colony, such as overly aggressive types being
aggressive to their own nestmates [ 43].
These results in insect and animal behavior studies point strongly to behaviorally
heterogeneous teams having higher ﬁtness in uncertain and dynamic environments,
which has inspired many multi-robot approaches. However, there is a need to furtherthe use of diversity as a tool for MRS, especially in tightly-coordinated tasks.
5 Some Open Problems in Diversity-Enhanced Autonomy
for Robot T eams
Much as human workgroups, as well as insects and animals, beneﬁt from diversity in
composition of the group, such variation of behavior would be beneﬁcial for teams of
robots operating in uncertain and unstructured environments. There exist many openproblems in DART; some of the challenging open problems that must be addressed
by the community are described here.
1Learning from Humans : Humans provide a pool of diverse resources that can be
tapped to develop diverse controllers that work well together. However, due to dif-
ferences in human and robot capabilities (communication, locomotion, sensing,
DART: Diversity-Enhanced Autonomy in Robot Teams 13
etc.), it is difﬁcult to learn controllers by observing human in-person interac-
tion. By limiting interaction to an interface (such as a mobile phone, tablet, or
laptop), communication, locomotion, and sensing can be restricted to robot-likecapabilities [ 44]. A major beneﬁt of human-inspired controllers is the ability to
communicate with and easily motivate study participants, as opposed to animal-
inspired controllers. However, learning from human cooperation requires multi-agent learning tools for many agents. This is an area that is not yet well represented
in the literature, save for several works [ 45–47].
2Deep Learning for MRS : In order to learn from humans, or to learn directly
from simulations, new machine learning tools must be developed for multi-agent
systems. While some solutions exist in multi-agent learning, some focus on tasks
that can be learned and completed alone [ 48,49]; those that are suitable for tight
coordination for a few (2–3) agents are intractable for large numbers of agents
that must tightly coordinate [ 50–52]. Tight coordination between a large team
of agents, for example in wildﬁre containment, currently presents a signiﬁcantcomputational challenge for existing multi-agent learning tools.
3Measures of Diversity and Fitness : Taking inspiration from the study of behav-
ioral diversity in social insect colonies, there is a need for understanding the
impact of behavioral diversity on MRS in tightly coordinated tasks. To that end,
measures of diversity and ﬁtness must be developed that apply to MRS, such asBalch’s Hierarchic Social Entropy [ 34]. Such tools will likely be task-speciﬁc at
ﬁrst, while the science of diversity-enhanced autonomy is established.
4Adjusting Policies Online : To successfully utilize a diverse set of controllers,
the team of robots must collectively reason about the role that each team member
plays and automatically adjust their own roles to achieve an appropriately diverse
team with an effective skill set. To do so, they must have the ability to measurethe success of individual agents on a coordinated task, learning from their own
and others’ shortcomings and successes.
6 Discussion
This paper proposes a new research thrust that represents a paradigm-shift in problem-
solving for multi-robot systems from a linear paradigm, where policies are optimizedfor a speciﬁc set of assumptions and applied to the entire team, to one where policies
are developed with multiple sets of assumptions and exist synergistically within a
team of robots. Such diversity in control policies will better prepare the team of robots
for challenging environments, much like diversity in the knowledge base in human
workgroups leads to higher quality solutions. Adoption of this new paradigm maylead to expanded success of multi-robot systems in the ﬁeld, especially in unstruc-
tured and uncertain environments.
A small sample of open problems were discussed, but there exist many open
problems in this space. By explicitly deﬁning Diversity-enhanced Autonomy for
Robot Teams, we hope to inspire the development of new tools for coping with
14 N. Ayanian
uncertain, unstructured environments such as ﬁrst response, precision agriculture,
surveillance, and others.
Acknowledgements This work was supported by NSF CAREER (IIS-1553726) and the Okawa
Foundation Research Award. Special thanks to Gaurav Sukhatme, M. Ani Hsieh, and Fei Sha forconversations and guidance in formulating this line of research.
References
1. Agha-mohammadi, A., Ure, N.K., How, J.P ., Vian, J.: Health aware stochastic planning for
persistent package delivery missions using quadrotors. In: IEEE/RSJ International Conferenceon Intelligent Robots and Systems, pp. 3389–3396 (2014)
2. Lonsdorf, K.: Hungry? Call your neighborhood delivery robot. NPR Morning Edition.
http://www.npr.org/sections/alltechconsidered/2017/03/23/520848983/hungry-call-your-neighborhood-delivery-robot (2017)
3. Sung, C., Ayanian, N., Rus, D.: Improving the performance of multi-robot systems by task
switching. In: IEEE International Conference on Robotics and Automation, pp. 2999–3006(2013)
4. Glaser, A.: These surveillance robots will work together to chase down suspects. Recode. https://
www.recode.net/2017/4/18/15264908/surveillance-robots-network-cornell-suspects (2017)
5. Schurr, N., Marecki, J., Tambe, M., Scerri, P ., Kasinadhuni, N., Lewis, J.: The future of dis-
aster response: humans working with multiagent teams using DEFACTO. In: AAAI SpringSymposium AI Technologies for Homeland Security (2005)
6. Jennings, J.S., Whelan, G., Evans, W.F.: Cooperative search and rescue with a team of mobile
robots. In: International Conference on Advanced Robotics, pp. 193–200 (1997)
7. Chung, T.H., Clement, M.R., Day, M.A., Jones, K.D., Davis, D., Jones, M.: Live-ﬂy, large-scale
ﬁeld experimentation for large numbers of ﬁxed-wing UA Vs. In: International Conference onRobotics and Automation, pp. 1255–1262 (2016)
8. Glaser, A.: Intel invented a way for a single operator to ﬂy hundreds of drones
at once. Recode. https://www.recode.net/2016/11/4/13517550/intel-single-operator-ﬂy-
hundreds-drones-shooting-star (2016)
9. Hauert, S., Leven, S., V arga, M., Ruini, F., Cangelosi, A., Zufferey, J., Floreano, D.: Reynolds
ﬂocking in reality with ﬁxed-wing robots: communication range vs. maximum turning rate. In:IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5015–5020. IEEE(2011)
10. Kushleyev, A., Mellinger, D., Powers, C., Kumar, V .: Towards a swarm of agile micro quadro-
tors. Auton. Robot. 35(4), 287–300 (2013)
11. Preiss, J.A., Hoenig, W., Sukhatme, G.S., Ayanian, N.: Crazyswarm: a large nano-quadcopter
swarm. In: IEEE International Conference on Robotics and Automation (2017)
12. Rubenstein, M., Ahler, C., Nagpal, R.: Kilobot: a low cost scalable robot system for collective
behaviors. In: IEEE International Conference on Robotics and Automation, pp. 3293–3298(2012)
13. D’Andrea, R., Wurman, P .: Future challenges of coordinating hundreds of autonomous vehicles
in distribution facilities. In: IEEE International Conference on Technologies for Practical RobotApplications, pp. 80–83 (2008)
14. Hagerty J.R.: Meet the new generation of robots for manufacturing. Wall Street J. (2015). Last
accessed 2 June 2015
15. Barret, B.: Disney’s latest attraction? 300 drones ﬂying in formation. Wired. https://www.
wired.com/2016/11/disneys-latest-attraction-300-drones-ﬂying-formation/ (2016)
16. Kenny, C.: Why Do People Die in Earthquakes? The Costs, Beneﬁts and Institutions of Disaster
Risk Reduction in Developing Countries. The World Bank (2009)
DART: Diversity-Enhanced Autonomy in Robot Teams 15
17. DJI: DJI documents faster search and rescue responses with drones. DJI Newsroom.
http://www.dji.com/newsroom/news/dji-documents-faster-search-and-rescue-responses-with-drones (2016)
18. Liu, Y ., Nejat, G.: Robotic urban search and rescue: a survey from the control perspective. J.
Intel. Robot. Syst. 72(2), 147–165 (2013)
19. Hoffman, L.R.: The group problem-solving process. In: Berkowitz, L. (ed.) Group Processes,
pp. 101–114. Academic Press, New Y ork (1978)
20. Hoffman, L.R., Maier, N.R.F.: Quality and acceptance of problem solutions by members of
homogeneous and heterogeneous groups. J. Abnorm. Soc. Psychol. 62, 401–407 (1961)
21. Nemeth, C.: Differential contributions of majority and minority inﬂuence. Psychol. Rev. 93,
23–32 (1986)
22. Jackson, S.: Team composition in organizations. In: Worchel, S., Wood, W., Simpson, J. (eds.)
Group Process and Productivity. Sage, London (1992)
23. Parker, L.E.: Alliance: an architecture for fault tolerant multirobot cooperation. IEEE Trans.
Robot. Autom. 14(2), 220–240 (1998)
24. Pimenta, L.C.A., Kumar, V ., Mesquita, R.C., Pereira, G.A.S.: Sensing and coverage for a
network of heterogeneous robots. In: IEEE Conference on Decision and Control, pp. 3947–3952 (2008)
25. Huang, J., Farritor, S.M., Qadi, A., Goddard, S.: Localization and follow-the-leader control
of a heterogeneous group of mobile robots. IEEE/ASME Trans. Mechatronics 11(2), 205–215
(2006)
26. Prorok, A., Hsieh, M.A., Kumar, V .: The impact of diversity on optimal control policies for
heterogeneous robot swarms. IEEE Trans. Robot. 33(2), 346–358 (2017)
27. Dorigo, M., Birattari, M., Stutzle, T.: Ant colony optimization. IEEE Comput. Intell. Mag.
1(4), 28–39 (2006)
28. Sugawara, K., Kazama, T., Watanabe, T.: Foraging behavior of interacting robots with virtual
pheromone. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 3,pp. 3074–3079 (2004)
29. Berman, S., Halasz, A., Kumar, V ., Pratt, S.: Bio-inspired group behaviors for the deployment
of a swarm of robots to multiple destinations. In: IEEE International Conference on Roboticsand Automation, pp. 2318–2323 (2007)
30. Kumar, G.P ., Bufﬁn, A., Pavlic, T.P ., Pratt, S.C., Berman, S.M.: A stochastic hybrid system
model of collective transport in the desert ant aphaenogaster cockerelli. In: International Con-ference on Hybrid Systems: Computation and Control, pp. 119–124, New Y ork, NY , USA.ACM (2013)
31. Tang, F., Parker, L.E.: A complete methodology for generating multi-robot task solutions using
asymtre-d and market-based task allocation. In: IEEE International Conference on Roboticsand Automation, pp. 3351–3358 (2007)
32. Parker, L.E., Tang, F.: Building multirobot coalitions through automated task solution synthesis.
Proc. IEEE 94(7), 1289–1305 (2006)
33. Balch, T.: Learning roles: Behavioral diversity in robot teams. In: AAAI Workshop on Multi-
agent Learning (1997)
34. Balch, T.: Hierarchic social entropy: an information theoretic measure of robot group diversity.
Auton. Robot. 8(3), 209–238 (2000)
35. Goldberg, D., Matari´ c, M.J.: Interference as a tool for designing and evaluating multi-robot
controllers. In: Proceedings AAAI, pp. 637–642, Providence, Rhode Island (1997)
36. Mouret, J.B., Doncieux, S.: Encouraging behavioral diversity in evolutionary robotics: an
empirical study. Evol. Comput. 20(1), 91–133 (2011). Last accessed 03 Nov 2016
37. Pugh, J.K., Soros, L.B., Stanley, K.O.: Quality diversity: a new frontier for evolutionary com-
putation. Front. Robot. AI 3, 40 (2016)
38. V assiliades, V ., Christodoulou, C.: Behavioral plasticity through the modulation of switch
neurons. Neural Netw. 74, 35–51 (2016)
39. Umedachi, T., Ito, K., Ishiguro, A.: Soft-bodied amoeba-inspired robot that switches between
qualitatively different behaviors with decentralized stiffness control. Adapt. Behav. 23(2), 97–
108 (2015)
16 N. Ayanian
40. Jandt, J.M., Bengston, S., Pinter-Wollman, N., Pruitt, J.N., Raine, N.E., Dornhaus, A., Sih, A.:
Behavioural syndromes and social insects: personality at multiple levels. Biol. Rev. 89, 48–67
(2014)
41. Chittka, L., Skorupski, P ., Raine, N.E.: Speed-accuracy tradeoffs in animal decision making.
Trends Ecol. Evol. 24, 400–407 (2009)
42. Burns, J.G., Dyer, A.G.: Diversity of speed-accuracy strategies beneﬁts social insects. Curr.
Biol. 18, R953–R954 (2008)
43. Crosland, M.W.J.: V ariation in ant aggression and kin discrimination ability within and between
colonies. J. Insect Behav. 3, 359–379 (1990)
44. Tavakoli, A., Nalbandian, H., Ayanian, N.: Crowdsourced coordination through online games
(Late Breaking Report). In: ACM/IEEE International Conference on Human-Robot Interaction,Christchurch, New Zealand (2016)
45. Matignon, L., Laurent, G.J., Le Fort-Piat, N.: Independent reinforcement learners in cooperative
markov games: a survey regarding coordination problems. Knowl. Eng. Rev. 27(1), 1–31 (2012)
46. Foerster, J., Assael, Y .M., de Freitas, N., Whiteson, S.: Learning to communicate with deep
multi-agent reinforcement learning. In: Advances in Neural Information Processing Systems,pp. 2137–2145 (2016)
47. Lowe, R., Wu, Y ., Tamar, A., Harb, J., Abbeel, P ., Mordatch, I.: Multi-agent actor-critic for
mixed cooperative-competitive environments. In: Ulrike, V .L., Isabelle, G., Samy B., HannaW., and Rob F. (Eds.) Proceedings of the 31 st International Conference on Neural InformationProcessing System (NIPS 17), pp. 6382–6393, Curran Associates Inc., USA (2017)
48. Matari´ c, M.J.: Learning to behave socially. Int. Conf. Simul. Adapt. Behav. 617, 453–462
(1994)
49. Recchia, T., Chung, J., Pochiraju, K.: Improving learning in robot teams through personality
assignment. Biol. Inspired Cogn. Arch. 3, 51–63 (2013)
50. Buffet, O., Dutech, A., Charpillet, F.: Shaping multi-agent systems with gradient reinforcement
learning. Auton. Agents Multi-Agent Syst. 15(2), 197–220 (2007)
51. Amato, C., Konidaris, G.D., Kaelbling, L.P .: Planning with macro-actions in decentralized
POMDPs. In: International Conference on Autonomous Agents and and multi-agent systems,pp. 1273–1280 (2014)
52. Amato, C., Konidaris, G.D., Cruz, G., Maynor, C.A., How, J.P ., Kaelbling, L.P .: Planning for
decentralized control of multiple robots under uncertainty. In: IEEE International Conferenceon Robotics and Automation, pp. 1241–1248 (2015)
Perspectives on Deep Multimodel Robot
Learning
Wolfram Burgard, Abhinav V alada, Noha Radwan, T ayyab Naseer,
Jingwei Zhang, Johan V ertens, Oier Mees, Andreas Eitel and Gabriel Oliveira
1 Introduction
Robots today have evolved from being able to perform only structured actions to being
able to act re-actively based on sensing their environment. Robot learning has playeda crucial role in enabling this capability. The classical paradigm involves a pipeline
containing modules for perception, world modelling, planning and control, each of
which are carefully engineered, incorporating handcrafted features and task-speciﬁc
structures. A typical modern robot control system is an ensemble of modules, which
often contain learning based models, and that are designed to perform dedicated tasksaimed at accomplishing a speciﬁc goal. In the last decade, Deep Convolutional Neu-
ral Network (DCNN) architectures have achieved remarkable results across several
robotic problems. However, the focus has been on designing individual networks forspeciﬁc problems including perception, localization, navigation and manipulation. In
addition, several disjoint models have been used in conjunction. This limits the over-
all learning ability of the robot as most models are trained in a supervised fashion andindependently, therefore they have no ability to share cross-domain information using
training signals from auxiliary tasks. Our vision is a uniﬁed multimodel deep learning
framework that jointly learns multiple robot tasks across multiple domains includingperception, planning and control. We propose a multimodel framework that incor-
porates soft parameter sharing thereby enabling the network to decide what layers
from auxiliary tasks to share and which sub-models can beneﬁt from representationslearned by layers in other sub-models. We believe that this will enable robots to learn
tasks with limited amount of data by leveraging transfer learning across sub-models
and equipping it with the capability to continuously learn from what it experiencesand perceives in the real-world.
W. Burgard ( B)·A. V alada ·N. Radwan ·T. Naseer ·J. Zhang ·J. V ertens ·O. Mees ·A. Eitel ·
G. OliveiraDepartment of Computer Science, University of Freiburg, Freiburg im Breisgau, Germanye-mail: burgard@informatik.uni-freiburg.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_317
18 W. Burgard et al.
In the following sections, we ﬁrst describe the classical pipeline that is commonly
employed to enable robots to perform autonomous actions. We then give an overview
of deep learning approaches that have demonstrated substantial progress in relevantbasic modules for perception, localization and navigation. Finally we discuss our
perspectives on how to enable robots to more proﬁciency learn from the world around
them using multimodel frameworks.
2 Classical Paradigms
The classical deﬁnition of an agent is anything that can perceive its surroundingsand act upon it and in the context of robotics, autonomous agents inhabit a complex
dynamic environment. In order to achieve their predeﬁned goals, they ﬁrst need to
sense their surroundings before they can plan actions. Robots are often equipped withmultiple sensors that provide complementary information. Extracting information
from raw sensor data is in itself a challenging task which requires expert knowledge
of both the environment and characteristics of the data produced by the sensor. Over
time, several approaches have been developed for feature extraction, some intricately
handcrafted and recently even learned from sensor data. The extracted features arethen used to infer information about the environment.
Complementary to the perception module, the robot also needs accumulated
knowledge about the world in which it is placed. Accordingly, a world module needsto be carefully designed such that static persistent landmarks in the environment, for
example walls, poles and trees, are well represented. Furthermore, it deﬁnes the pos-
sible set of actions for the agent along with the state transition function which deﬁnesthe state of the world after each action. Instead of providing the world model to the
agent, we let the system built its own model using information gathered from the per-
ception module. However, relying solely on the perception module to build the modelis a challenging task as the entire environment is not visible in one sensor observation.
Hence to build a complete model, the agent needs to explore the environment over
time and correct any inconsistencies occurring during observation. For the agent toachieve its goal, it relies on the output of the perception module along with the world
model to formulate a plan. The planning module is responsible to provide a plan that
can be executed in the current state space and successfully complete the required task.To this end, the planning module needs to not only formulate the plan but also have the
ability to recover from failure and replan in the event of an unexpected situation. The
control module is responsible for providing the proper control commands to the actua-
tors of the robot in a way that follows the plan provided to perform a predeﬁned action.
Perspectives on Deep Multimodel Robot Learning 19
3 Emergence of Deep Models
In the last few years, convolutional neural networks has revolutionized several core
components that constitute an autonomous robotic system. They have brought abouta signiﬁcant change in the traditional pipelines employed. We brieﬂy discuss some
of these advances in the following sections.
Scene Understanding Scene understanding is an essential component of any robotic
system as robots need to ﬁrst know what and where the elements of the scene are
before they can act on them. The advent of DCNNs have brought about several
state-of-the-art models for a variety of perception tasks including object recognition[3,4], detection [ 4,9,13] and semantic segmentation [ 10,20,25]. However, robotic
perception models have different requisites than those in computer vision. Robots are
often equipped with multiple sensors such as cameras, lidars and radars to perceivetheir surroundings. Therefore, deep architectures need to efﬁciently learn a combined
representation of the world utilizing these sensors. To this end, multi-stream networks
are often used to train each stream on speciﬁc modality and fuse them towards the endof the network [ 3,23]. Alternatively, architectures have also been designed that fuse
feature maps from modality speciﬁc streams at intermediate points in the network and
converge to a single stream towards the end [ 7]. As sensor noise is a major hindrance in
the real-world, noise augmentation strategies can be employed, either before feeding
data to the network [ 3] or while training [ 24]. One of the major challenges in real-
world robot perception is the ability of models to adapt to changes in appearance dueto weather and seasons. In such conditions, incorporating adaptive fusion strategies
such as mixture of deep experts has substantially improved the performance and
robustness of models for semantic segmentation [ 25] and pedestrian detection [ 13].
DCNNs have also been used for specialized classiﬁcation tasks such of terrains and
with unconventional sensors including microphones [ 24]. Advances such as new
pooling strategies that learn statistics of temporal features in the signal enable these
approaches to outperform classiﬁers learned on traditional audio features.
The introduction of fully convolutional neural networks [ 10] has brought about
several state-of-the-art architectures for various robotic applications from segmenting
roads [ 18] to human body parts [ 20]. Incorporating advances such as residual learning
and dilated convolutions to learn deep multi-scale features have further pushed theboundaries of achievable performance while maintaining fast inference times [ 25].
Often in robotics it is also necessary to estimate motion of objects in order to plan
future actions. Complementary tasks such as segmentation and motion estimation canbe learned using a joint formulation in a uniﬁed deep framework [ 27]. Such networks
not only reduce the model complexity but also enable interactive frame rates.
Localization and Odometry Robust place recognition and visual localization of
autonomous systems is of paramount importance for relevant robotic applications.
Visual localization largely depends on robust and repeatable feature descriptions
over large variety of environmental changes. The feature descriptions from deepnetworks have outperformed the traditional hand-crafted features in this domain
due to their ability to learn feature correspondences under different appearances.
20 W. Burgard et al.
A model designed for visual localization can also leverage vital information from
a model trained on a different task e. g. segmentation [ 17] or visual similarity. This
aspect of joint learning of different tasks enables us to learn a heterogeneous modelwhere subtasks beneﬁt from each other’s data. Recently, deep architectures for metric
localization have emerged that provide an efﬁcient map representation in addition to
demonstrating considerable robustness in challenging perceptual conditions [ 8,28].
In contrast to traditional methods, deep models provide a ﬁxed map size and a constant
time complexity for camera-based metric localization.
Recently, end-to-end DCNN approaches that estimate visual odometry have also
been proposed [ 14,29]. Most of these approaches employ a Siamese-type network
architecture that take two consecutive images as input and regress the relative trans-
formation between them. In [ 29], the authors use a AlexNet-based Siamese archi-
tecture and an L2-loss layer with equal weights for the translational and rotational
components, while in [ 14] the authors propose a weighting term to balance these
components. In order to exploit the advantages of both metric and topological local-ization, while concurrently reducing the error caused by the accumulation of drift in
visual odometry, an optimization technique was proposed that fuses the output of a
odometry and topological DCNN [ 19]. Utilizing the topological information helps
in bounding the accumulated drift within consecutive topological nodes, thereby
improving the accuracies of such systems by an order of magnitude.
Navigation In the area of navigation, reinforcement learning has been used to inves-
tigate the possibility of enabling intelligent agents to learn to navigate through envi-
ronments without the need for labelled data and without the requirement for explicitlocalization, mapping or planning procedures as in traditional methods [ 32]. Deep
reinforcement learning methods [ 16,26] which originated from solving control prob-
lems for playing Atari games are utilized along with deep neural nets as functionapproximators to represent the Q-value function. In order to ensure that the learned
navigation policy can be effectively transferred to new navigation goals and environ-
ments, the problem can be framed as a sequence of related reinforcement learning
tasks and successor feature based reinforcement learning procedures are embedded
into the network architecture. Unlike the original deep reinforcement learning algo-rithms that usually result in a black-box function approximator, the successor feature
representation of the Q-value function gives us a natural way to transfer learned task
solutions to new task instances, while making sure that the solutions to old tasks arepreserved after the transfer. Results have demonstrated that the agent is able to learn
successful navigation strategies even with sparse supervision from the reward signal
it receives and more importantly without any need for human intervention.
4 T owards Predictive Multimodel Learning
While we have seen tremendous amount of progress in robot learning these recentyears, robots are still far away from being able to self sufﬁciently learn and execute
tasks as efﬁciently as humans. This is perhaps because research thus far has been
Perspectives on Deep Multimodel Robot Learning 21
focused on learning models for small subtasks without considering that each of
these subtasks might have complex interactions that our conjoint supervised system
fails to capture. For example, consider the task of semantic segmentation and visuallocalization, the model trained for segmentation has strong priors about objects and
structures in the scene, which can provide an inductive bias to the model being trained
for visual localization. Thereby enabling the localization model to generalize betterdue to the inductive transfer. If these models are trained in a disjoint fashion, it not only
affects the scalability but also restricts the transfer of cues and parameter sharing that
could potentially occur. Each of these subtasks have been studied for several decadesand numerous deep learning architectures have emerged after months of crafting
and tuning. Often this effort is reiterated for different subtasks, limiting the overall
learning capability of a robotic system as a whole. While this modular paradigm maybe effective in accomplishing a task, it will often break down in unforeseen scenarios
that occur in this complex real-world. Moreover, in terms of feasibility, individual
models require a large amount of specialized labelled training data, deploying arobot with multiple models demands a substantial amount of GPU hardware and
the inability of these models to interact and update their weights online based on
current observations, make this impractical to use over longer periods. In contrast, a
complete end-to-end approach to a multitask problem such as robot autonomy, forces
the model to squeeze enormous amount of information about disjoint tasks into thesame parameter space, which is infeasible.
In order for robots to be able to effectively learn, they should be able to perceive the
states of the world, plan and perform actions based on these observed states, remem-ber outcomes and be able to make predictions based on these for future actions. At
present, robots have some of these components in them but they are disjoint and are
not learned in a coherent framework. We think that in a multitask learning scenario,models can not only beneﬁt from the transfer of inductive bias from models in mul-
tiple domains but in addition, models trained for tasks with a large amount of exam-
ples can self supervise training of models with a small number of training examples.Multitask learning can be deﬁned as a transfer learning mechanism that improves
generalization by using domain speciﬁc information contained in the training signals
of related tasks [ 1]. Speciﬁcally in convolutional neural networks, multitask learning
is generally achieved using either hard or soft parameter sharing. In hard parameter
sharing, the hidden layers are shared between all the subtasks, while having task-
speciﬁc output layers. Hard parameter sharing has recently been used for severaltasks including facial landmark detection [ 22,34], grasping [ 21] and face recog-
nition [ 31]. In the aforementioned works, a core DCNN architecture is employed,
followed by task-speciﬁc inner-product layers. The advantage of incorporating hard
parameter sharing is the reduced risk of overﬁtting, while the disadvantage being
the potential corruption of low-level features in the core architecture due to noisefrom a related subtask. Soft parameter sharing on the other hand, overcomes this
drawback by having task-speciﬁc sub-networks with separate hidden layers, while
using a sharing mechanism such as regularizing the distance between the param-eters of the sub-networks using the l
2norm [ 2], trace norm [ 30] or tensor normal
priors [ 11]. However, the main challenge in soft parameter sharing is developing
22 W. Burgard et al.
an appropriate sharing mechanism for the tasks at hand. Recently, Misra et al. pro-
posed cross-stitch units [ 15] for multitask networks that are in soft parameter sharing
conﬁguration. These units learn a combination of shared and task-speciﬁc represen-tations from multiple sub-networks and has demonstrated improved performance
for tasks with limited training data. However, the placement of these cross-stitch
units still remains on open research problem. Some of the mechanisms that enablethe aforementioned multitask networks to generalize better include regularization,
representation bias, eavesdropping, attribute selection and data augmentation. The
effect of these mechanisms in multitask networks are discussed in detail in the workof Caruna et al. [ 1].
Multimodality is another important characteristic that can enable models to learn
the most comprehensive information about the scene or situation which can in turnhelp them reason more effectively. Multimodal learning can be deﬁned as learning
from multiple sensory modes such as cameras, lasers, sound and etc. Learning from
multimodal sensory data can help robots enhance their perception of the environmentand reduce perceptual ambiguity in challenging conditions. Consider a robot that has
to grasp an object; if equipped with only a monocular camera, the robot will have
to perform millions of grasps to identify the properties of the object and the right
strategy, but if the robot is also equipped with a tactile sensor then robot can more
efﬁciently learn the properties and thus correlate this tactile sense to visual featuresand use it for future inference, even in a different domain. Moreover, by utilizing
multimodal data in a multimodel framework, we also enable better representational
learning, as task speciﬁc sub-models learn a particular noise pattern with respect toa modality and by inductive transfer, all the sub-models implicitly learn a combined
representation of several noise patterns.
What we envision is a uniﬁed neural network architecture that is able to perform
perception, localization, planning and control, not in a completely end-to-end fash-
ion going from visual input to action, but having individual learnable sub-models in
a soft parameter sharing conﬁguration for each of these tasks that enable continu-ous update of their weights from each others experiences. This requires each of the
sub-models to have a network memory, for example, neural turing machines [ 5]o r
differential neural computers [ 6] so that they can quickly store information and rea-
son from it when required. By joint learning of tasks across these multiple domains,
we can not only improve the performance of models by more coherent under-
standing but also in domains with limited amount of data. In our work of NeuralSLAM [ 33], we give intelligent agents long-term memory capabilities, through the
integration of an external memory architecture with a deep reinforcement learning
framework. Identifying that cognitive mapping is essential for agents to make com-
prehensive navigation and exploration decisions, we embed the procedures mimicing
that of traditional SLAM algorithms, into a completely differentiable deep neuralnetwork. The proposed agent is able to learn to map into its external memory and
perform effective exploration behaviors. Finally, a critical trait that is important
to our proposed multimodel framework is for models to be able to predict futurestates. Recent work with adversarial training enables models to predict intermedi-
ate actions or future frames of a video sequence using unsupervised learning [ 12].
Perspectives on Deep Multimodel Robot Learning 23
While this is only the initial stages of being able to create models that can predict
what is unknown, instilling this into the multimodel framework will enable us to
create robots that can self-sufﬁciently learn across domains with limited amount oflabelled data.
References
1. Caruana, R.: Multitask learning. Mach. Learn. 28(1), 41–75 (1997)
2. Duong, L., Cohn, T., Bird, S., Cook, P .: Low resource dependency parsing: cross-lingual param-
eter sharing in a neural network parser. In: 53rd Annual Meeting of the Association for Compu-tational Linguistics and the 7th International Joint Conference on Natural Language Processing(2015)
3. Eitel, A., Springenberg, J.T., Spinello, L., Riedmiller, M., Burgard, W.: Multimodal deep learn-
ing for robust RGB-D object recognition. In: International Conference on Intelligent Robotsand Systems (2015)
4. Girshick, R., Donahue, J., Darrell, T., Malik, J.: Rich feature hierarchies for accurate object
detection and semantic segmentation. In: IEEE Conference on Computer Vision and PatternRecognition (2014)
5. Graves, A., Wayne, G., Danihelka, I.: Neural turing machines. arXiv:1410.5401 (2014)
6. Graves, A., Wayne, G., Reynolds, M., Harley, T., Danihelka, I., Grabska-Barwi´ nska, A., Col-
menarejo, S.G., Grefenstette, E., Ramalho, T., Agapiou, J., et al.: Hybrid computing using aneural network with dynamic external memory. Nature 538(7626), 471–476 (2016)
7. Hazirbas, C., Ma, L., Domokos, C., Cremers, D.: Fusenet: Incorporating depth into semantic
segmentation via fusion-based cnn architecture. In: Asian Conference on Computer Vision(2016)
8. Kendall, A., Grimes, M., Cipolla, R.: Posenet: A convolutional network for real-time 6-DOF
camera relocalization. In: International Conference on Computer Vision (2015)
9. Liu, W., Anguelov, D., Erhan, D., Szegedy, C., Reed, S., Fu, C.Y ., Berg, A.C.: SSD: single shot
multibox detector. In: European Conference on Computer Vision (2016)
10. Long, J., Shelhamer, E., Darrell, T.: Fully convolutional networks for semantic segmentation.
In: IEEE Conference on Computer Vision and Pattern Recognition (2015)
11. Long, M., Wang, J.: Learning multiple tasks with deep relationship networks. arXiv:1506.02117
(2015)
12. Lotter, W., Kreiman, G., Cox, D.D.: Deep predictive coding networks for video prediction and
unsupervised learning. arXiv:1605.08104 (2016)
13. Mees, O., Eitel, A., Burgard, W.: Choosing smartly: adaptive multimodal fusion for object
detection in changing environments. In: International Conference on Intelligent Robots andSystems (2016)
14. Melekhov, I., Kannala, J., Rahtu, E.: Relative camera pose estimation using convolutional
neural networks. arXiv:1702.01381 (2017)
15. Misra, I., Shrivastava, A., Gupta, A., Hebert, M.: Cross-stitch networks for multi-task learning.
In: IEEE Conference on Computer Vision and Pattern Recognition (2016)
16. Mnih, V ., et al.: Human-level control through deep reinforcement learning. Nature 518 (2015)
17. Naseer, T., Oliveira, G., Brox, T., Burgard, W.: Semantics-aware visual localization under
challenging perceptual conditions. In: International Conference on Robotics and Automation
(2017)
18. Oliveira, G., Burgard, W., Brox, T.: Efﬁcient deep models for monocular road segmentation.
In: International Conference on Intelligent Robots and Systems (2016)
19. Oliveira, G., Radwan, N., Burgard, W., Brox, T.: Topometric localization with deep learning.
arXiv:1706.08775 (2017)
24 W. Burgard et al.
20. Oliveira, G., V alada, A., Bollen, C., Burgard, W., Brox, T.: Deep learning for human part
discovery in images. In: International Conference on Robotics and Automation (2016)
21. Pinto, L., Gupta, A.: Learning to push by grasping: Using multiple tasks for effective learning.
In: International Conference on Robotics and Automation (2017)
22. Ranjan, R., Patel, V .M., Chellappa, R.: Hyperface: A deep multi-task learning frame-
work for face detection, landmark localization, pose estimation, and gender recognition.arXiv:1603.01249 (2016)
23. V alada, A., Oliveira, G., Brox, T., Burgard, W.: Deep multispectral semantic scene under-
standing of forested environments using multimodal fusion. In: International Symposium onExperimental Robotics (2016)
24. V alada, A., Spinello, L., Burgard, W.: Deep feature learning for acoustic-based terrain classi-
ﬁcation. In: International Symposium on Robotics Research (2015)
25. V alada, A., V ertens, J., Dhall, A., Burgard, W.: Adapnet: adaptive semantic segmentation in
adverse environmental conditions. In: International Conference on Robotics and Automation(2017)
26. van Hasselt, H., Guez, A., Silver, D.: Deep reinforcement learning with double q-learning. In:
Proceedings of the Thirtieth AAAI Conference on Artiﬁcial Intelligence (2016)
27. V ertens, J., V alada, A., Burgard, W.: Smsnet: semantic motion segmentation using deep con-
volutional neural networks. In: International Conference on Intelligent Robots and Systems(2017)
28. Walch, F., Hazirbas, C., Leal-Taix, L., Sattler, T., Hilsenbeck, S., Cremers, D.: Image-based
localization using lstms for structured feature correlation. In: International Conference onComputer Vision (2017)
29. Wang, S., Clark, R., Wen, H., Trigoni, N.: Deepvo: Towards end-to-end visual odometry with
deep recurrent convolutional neural networks. In: International Confonference on Robotics andAutomation (2017)
30. Yang, Y ., Hospedales, T.M.: Trace norm regularised deep multi-task learning.
arXiv:1606.04038 (2016)
31. Yin, X., Liu, X.: Multi-task convolutional neural network for face recognition.
arXiv:1702.04710 (2017)
32. Zhang, J., Springenberg, J.T., Boedecker, J., Burgard, W.: Deep reinforcement learning with
successor features for navigation across similar environments. In: International Conference onIntelligent Robots and Systems (2017)
33. Zhang, J., Tai, L., Boedecker, J., Burgard, W., Liu, M.: Neural slam. arXiv:1706.09520 (2017)
34. Zhang, Z., Luo, P ., Loy, C.C., Tang, X.: Facial landmark detection by deep multi-task learning.
In: European Conference on Computer Vision (2014)
T ask Representation in Robots
for Robust Coupling of Perceptionto Action in Dynamic Scenes
Darius Burschka
1 Motivation
The robotic perception is undergoing a transformation from navigation of service
robots in static indoor environments to operation in large scale dynamic environ-ments. This process is rapidly evolving due to the availability of cheap and robust
sensors and computing units and because of the current focus on navigation tasks
in automotive and ﬂying applications. It is very tempting to make this transition
using tools from the well established service robotics domain and apply them to the
outdoor applications. While this process allows a fast deployment of systems in thenew domain, multiple new problems are created. The advantage of being able to rely
on well-understood and -tested software seem to outweigh the problems, which are
created through this migration.
Most of the current service robotics research focuses on efﬁcient modeling of
static environments with focus shifting towards abstracting and labeling of objects
in the scene (Fig. 1). The typical tasks in this domain are identiﬁcation and transport
of objects in the environment, where the dynamic changes are caused exclusively
by the robot itself. Any other dynamic changes in the scene are treated often as
outliers and are neglected in the data processing, e.g. [ 4]. Since the collision with
an object in the scene depends here entirely on the own velocity, the distance to the
static object gives the correct ordering of information for planning. In this type of
environments, the system indexes the information through its geometric position. Theresulting underlying model representation is usually organized based on the Cartesian
position of objects in the world to support this type of queries. The perception on a
service robot is also tuned to operate in indoor environments, which becomes obviouslooking at the signiﬁcant number of systems relying on active sensors, like Kinect,
which operate reliably only in indoor environments due to the limited power of the
D. Burschka ( B)
Department of Computer Science, Technische Universität München, Munich, Germanye-mail: burschka@tum.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_425
26 D. Burschka
Fig. 1 Most current service robotics applications focus on static representations of the scene and
abstract the sensor data to 3D (left branch), while the task deﬁnition in dynamic scenes usuallydepends on dynamic motion parameters of objects(right branch)
projectors used to illuminate the scene. The perception capability needs to match the
requirement of the task deﬁnition. Since the tasks are mostly deﬁned as changes in
the static 3D structure in case of static scenes, an ideal sensor is a stereo camera oran active sensor for reconstruction. The 3D data is often not the native representation
of the sensor and it requires additional calibration parameters, which are sensitive to
changes in the intrinsic and extrinsic geometry of the camera system due to vibrationsduring operation or errors in the calibration process.
New application domains for robotic systems deﬁned often by industrial require-
ments try to develop perception for outdoor applications, where the scale of the envi-
ronment in which they operate is a lot larger and the robot is not the only dynamic
agent operating in the scene. It is interesting to observe that often conventionalapproaches from indoor service robotics are directly applied to dynamic large scale
outdoor scenarios. This step makes many of the existing well-tested tools available
to the new domain, but the geometric representation often removes important signalproperties in camera images which become lost in noise or that are very difﬁcult
to reconstruct later from the geometric representation of the task. Often, the task in
such environments is not a unique identiﬁcation of objects but a safe operation underchanging dynamic conditions of the environment, like collision avoidance to name a
very basic important task in the domain. In many cases, it is not the geometric relation
of objects to each other but their relative dynamic state of great importance for thetask. An example depicting such a case is the situation of a moving agent acting in
the neighborhood of 2 other dynamic (or static) agents, where the task is to prioritize
Task Representation in Robots for Robust Coupling of Perception to Action … 27
Fig. 2 Geometric indexing to the scene representation may result in incorrect prioritization of
objects for dynamic analysis
immediate collisions with the goal to avoid objects (Fig. 2). Here, a representation
scheme in dynamic or temporal properties of the object could better preserve the
information about possible collision relations in the environment, which exist in theoriginal camera information in form of time-to-collision calculations and epipolar
analysis of optical ﬂow in the image sequence [ 1]. The closest object in a metric
sense does not necessarily pose any problems on the task of the obstacle avoidance.
Another reason requiring a modiﬁcation in the task representation is the limited
sensing accuracy. While the sensors can cope correctly with the 3D information in a
typical indoor environment, the signal-to-noise ratio makes any 3D reconstruction in
large scale environments useless. The error due to detection and matching accuracy
of the stereo system is larger than the displacement information between the cameraimages used for reconstruction. A dynamic state estimation from changes in the 3D
Fig. 3 The relative error in the estimated position for a binocular setup with the distance between
the cameras =10 cm under the assumption of matching accuracy of 0.5 pixel
28 D. Burschka
position of vehicles becomes unreliable (Fig. 3). It is apparent that the information
is useful for the original applications inclose range of an indoor scenario, but shall
not be applied for larger distances.
2 Proposed Research Direction
The question that needs to be answered for most of the tasks that go beyond simplepick and place actions is: what is a sufﬁcient abstraction level of the sensor data to
provide enough information to monitor and control the completion of a task and what
is the robust control strategy for a given task? We propose to focus on formalization
of the exchange layer between the perception and execution by combining ﬁndings
in the perception and control domain. The goal is to ﬁnd representations, which do
not require additional calibration parameters that are error-prone.
2.1 Examples of Representation-Alternatives
Perception for Grasping
For the simple case of obstacle avoidance can be shown, that the task can be deﬁned
in a Cartesian, metric map or directly as a response to the signal variations in the
camera images (Fig. 4). A good example for adaptation of the task description is
the typical implementation of the grasping task, where the system can apply a look-then-move or visual servoing approach. In the ﬁrst case, the system lifts the sensor
information to the 3D representation introducing errors resulting in grasp failures or
it deﬁnes the task directly in the sensor image for a robust processing. The idea of taskrepresentation outside of the 3D domain is not new. Researchers have been posing
the question about possible task representation [ 5], but the current rapid development
of the sensor technology triggered by industrial and indoor application let the current
application neglect this previously explored theoretical ﬁelds.
Navigation MapsIn previous work, we have shown that the navigation of a robot in an environment does
not necessarily require a metric map of the environment [ 6,7]. A robust navigation
on ﬁxed path can be also achieved by deﬁning the task directly in the image space as
an extension of the visual servoing approach to a sequence of reference images that
are used for navigation, e.g. [ 2]. The construction and maintenance of these visual
maps is a lot simpler than the construction of fused 3D models for navigation. The
visual approach is also more robust against wrong camera parameters. For example,a wrong focal length results mostly in a scaling error. It just changes the gains in
the control derived from the visual information. On the other hand, image-based
approach is less ﬂexible in re-planning than a geometric approach.
The selection of the appropriate strategy is dependent on the task but can be
signiﬁcantly robustiﬁed if the geometric representation is not used.
Task Representation in Robots for Robust Coupling of Perception to Action … 29
Fig. 4 Typical planning approaches operate in 3D Cartesian space that makes the plan easier to
understand for a human. Planning directly in the sensor space or close to its representation showsincreased robustness to errors
Dynamic Obstacle Avoidance
Our preliminary work shows that if a task can be deﬁned closer to a sensor repre-
sentation then the system becomes robust against calibration parameters and often
monocular approaches can be used instead of error-sensitive binocular setups. Once
the 3D information in the sensor is not necessary, other image properties of the sensorsignal can be used directly to monitor the task. Suddenly, the missing scale in the
monocular setups does not pose a problem anymore, because in case of a collision
time representation, the collision time information can be calculated directly from
the pixel changes in monocular sequences. This makes navigation approaches with
monocular systems possible.
In [3], an example of state estimation for single points in the images from monoc-
ular images is presented. It estimates collision relations directly for any single point
in the image. It is just one possible way, how such exchange between the sensor andthe actuator may be accomplished. Here, the pixel information is used directly to
estimate the collision property.
A three-dimensional point P
itravels with an arbitrary constant velocity vector vgi
in Fig. 5. This motion results in a trace of tracked points piin consecutive image
frames for time steps t ={0,1,2}. The vector vgideﬁnes a plane containing Pi.W e
call this a collision plane which sweeps through the focal point of the camera after
some. The collision time can be estimated from the trace of the point [ 3].
30 D. Burschka
Fig. 5 A 3D point Pitravels in three frames t =0,1,2 with a velocity vgi. The vector vgiwith the
point Pideﬁnes a gray collision plane with vgias its normal vector
Planning without Scale Information
In case of dynamic obstacles, the car can be navigated completely in the pixel-space
of the camera using time-to-collision calculations that cancel the unknown scale in
the distance and velocity calculations. Both identical scales cannot be observed in
monocular systems, which renders the monocular system useless for Cartesian task
representation.
A planning in a map represented by the time-to-collision allows to control the
vehicle with uncalibrated monocular camera (Fig. 6). While the estimation of the
collision relations does not require a scale, the control requires a strategy to ﬁnd
correct compensation gains to respond to detected dangerous conditions [ 8].
Fig. 6 Obstacle avoidance planning in a map represented by dynamic state properties allows control
with uncalibrated cameras
Task Representation in Robots for Robust Coupling of Perception to Action … 31
The idea helps also to robustify the stereo reconstruction especially in large dis-
tances, where the signal to noise level makes the reconstruction close to impossible.
It can be shown that the separation of the estimation of direction from magnitude ofthe motion helps to get good results for planning of collision-free paths [ 9].
3 Conclusion
The initial studies show that a deﬁnition of tasks in spaces others than the 3D geometry
of the environment often robustiﬁes the execution and monitoring of the tasks. An
appropriate solution to many problems can only be achieved if also the controlconsiders new ways of passing the information to the vehicle. Often, the Cartesian
interface for task execution leads to error accumulation on both sides: perception
and control. The question about the necessary level of data abstraction in controlinterfaces should be addressed as well.
References
1. Ancona, N., Poggio, T.: Optical ﬂow from 1d correlation: application to a simple time-to-crash
detector. In: 1993 4th International Conference on Computer Vision, pp. 209–214 (1993)
2. Burschka, D., Hager, G., Vision-based control of mobile robots. In: IEEE International Confer-
ence on Robotics and Automation. Proceedings 2001 ICRA, vol. 2, pp. 1707–1713 (2001)
3. Burschka, D.: Monocular navigation in large scale dynamic environments. In: BMVC (2017)4. Engel, J., Schöps, T., Cremers, D.: LSD-SLAM: Large-Scale Direct Monocular SLAM, pp.
834–849. Springer International Publishing, Cham (2014)
5. Hespanha, J.P ., Dodds, Z., Hager, G.D., Morse, A.S.: What tasks can be performed with an
uncalibrated stereo vision system? Int. J. Comput. Vis. 35(1), 65–85 (1999)
6. Hornung, A., Wurm, K.M., Bennewitz, M., Stachniss, C., Burgard, W.: OctoMap: an efﬁcient
probabilistic 3D mapping framework based on octrees. Auton. Robot. (2013). Software availableathttp://octomap.github.com
7. Magin, G., Ru, A., Burschka, D., Frber, G.: A dynamic 3D environmental model with real-time
access functions for use in autonomous mobile robots. Robot. Auton. Syst. 14(2), 119–131
(1995). Research on Autonomous Mobile Robots
8. Schaub, A., Burschka, D.: Reactive avoidance of dynamic obstacles through optimization of
their epipoles. Best paper in ICSTCC (2017)
9. Schaub, A., Burschka, D.: Single point velocity estimation in dynamic scenes from optical ﬂow
in binocular systems. In: AIM (2017)
A Bayesian Active Learning Approach
to Adaptive Motion Planning
Sanjiban Choudhury and Siddhartha S. Srinivasa
1 Introduction
Motion planning, the task of computing collision-free motions for a robotic system
from a start to a goal conﬁguration, has a rich and varied history [ 17]. Up until
now, the bulk of the prominent research has focused on the development of tractable
planning algorithms with provable worst-case performance guarantees such as com-
putational complexity [ 3], probabilistic completeness [ 18] or asymptotic optimality
[16]. In contrast, analysis of the expected performance of these algorithms on the real
world planning problems a robot encounters has received considerably less atten-tion, primarily due to the lack of standardized datasets or robotic platforms. However,
recent advances in affordable sensors and actuators have enabled mass deployment
of robots that navigate, interact and collect real data. This motivates us to examinenew algorithmic questions such as: “How can we design planning algorithms that,
subject to on-board computation constraints, maximize their expected performance
on the actual distribution of problems that a robot encounters?”
We formalize the problem as follows—given a probability distribution over
worlds, we want to optimize expected planning effort of ﬁnding a good solution.
The main planning effort is in collision checking as it requires expensive geomet-ric intersection computations [ 1,9,10,20]. Moreover, the uncertainty over worlds
induces a complex uncertainty over the validity of edges. To solve such problems, we
advocated in [ 24] for an adaptive motion planning system that learns which potential
solutions to evaluate using features extracted from the world. However, a signiﬁcant
drawback is that feature extraction requires extensively probing the environment
S. Choudhury ( B)
The Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USAe-mail: sanjiban@cmu.edu
S. S. Srinivasa
School of Computer Science and Engineering, University of Washington, Seattle, WA, USAe-mail: siddh@cs.uw.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_533
34 S. Choudhury and S. S. Srinivasa
(a) (b)
Fig. 1 Real world planning problems where edges are correlated. In such cases, a white-box adap-
tiveplanner can infer the structure of the world from outcomes of edge evaluations. aPresence of
a table in robotic arm planning correlates neighbouring edges (courtesy Dellin [ 9]).bPresence of
wires and guide-towers in helicopter planning correlates corresponding edges
before any planning can commence. Moreover, the planners are treated as a “black
box” wherein no degree of intervention during the planning cycle is allowed. This
motivates us to examine the “white box” paradigm where a planner adapts during
planning . Often a large degree of inference can be made about the world in the inter-
mediate stages of search as shown in Fig. 1. Based on this information, the planning
algorithm itself can choose to adapt its search strategy and make the most out of its
planning effort.
While the white box paradigm encompasses a large class of problems, in this
paper we focus on one such subclass—adaptive edge evaluation during search on
explicit graphs. Consider a graph whose vertices represent robot conﬁgurations and
edges represent potentially valid movements between these conﬁgurations. In order
to minimize edge evaluation effort, prior information about the validity of edges can
be leveraged [ 2,6]. This is illustrated in Fig. 2where a robot navigates in a world with
a narrow gap in one of two places. If an edge on gap 1 is blocked, the planner infers
that gap 2 is free. In other words, the posterior distribution over worlds collapses on a
set of worlds, all of which admit a particular path to be feasible. The planner proceeds
Fig. 2 Collision checking informative edges can lead to reduced search effort. aA prior distribution
over worlds shows a narrow gap in one of two places. Hence the collision checker checks the most
informative edge and ﬁnds it to be in collision. bThe posterior collapses on one path being free
(but many possible worlds). The algorithm proceeds to check these edges for collision
A Bayesian Active Learning Approach to Adaptive Motion Planning 35
to check only that path, leaving all other edges untouched. We wish to compute such
a policy that judiciously chooses edges to evaluate by reasoning about likely worlds
in which the robot operates.
Our key insight is that this problem is equivalent to the Bayesian active learn-
ing problem of decision region determination (DRD) [4,15]—given a set of tests
(edges), hypotheses (worlds), and regions (potential paths), the objective is to selecta sequence of tests that drive uncertainty into a single decision region. Drawing
this analogy enables us to leverage existing methods in Bayesian active learning
[12] for robot motion planning. Note that the DRD problem has one key distinc-
tion from the general active learning problem—the uncertainty only needs to be
driven down enough to ascertain if a path is free, and it is not necessary to iden-
tify the underlying world [ 8]. This makes it very applicable for motion planning
where we only need to know enough about the world to compute a path. Solving
the DRD problem in general is NP-hard [ 15]. Fortunately, Chen et al.[ 4] provide
a method to solve this problem by maximizing an objective function that satisﬁesadaptive submodularity [11]—a natural diminishing returns property that endows
greedy policies with near-optimality guarantees. We are able to directly adopt this
algorithm to compute a policy that would select which edges to evaluate for our
problem.
We are excited to make this connection between two disparate disciplines. We have
made some preliminary inroads and demonstrated great empirical results compared to
several state of the art baseline algorithms in real world settings [ 5]. Interestingly, we
found that this connection leads to new subproblems that necessitate attention fromthe active learning community. One such problem, as discussed in Sect. 3,i st h eD R D
problem when test outcomes are independent Bernoulli random variables. We show
that under such a setting the computational complexity for edge evaluation is reducedfrom exponential in number of edges to linear while still retaining near-optimality
guarantees. We describe other such problem variants, insights and potential future
research directions in detail in Sect. 5.
2 Problem Formulation
We now describe the adaptive edge evaluation, drawing the equivalence with the
DRD problem along the way. Let G=(V,E)be an explicit graph that consists of a
set of vertices Vand edges E. Given a pair of start and goal vertices,/parenleftbig
vs,vg/parenrightbig
∈V,
a search algorithm computes a path ξ⊆E—a connected sequence of valid edges.
We have a set of worlds H={h1,..., hn}, each of which are analogous to a
“hypothesis”. We have a prior distribution P(h)on this set. A “test” is performed
by querying an edge e∈Efor evaluation which returns a binary outcome x∈{0,1}
denoting if an edge is valid or not. Thus each world h∈Hcan be considered a
function h:E→{0,1}mapping edges to corresponding outcomes. We address
applications where edge evaluation is expensive, i.e., the computational cost c(e)of
computing h(e)is signiﬁcantly higher than regular search operations.
36 S. Choudhury and S. S. Srinivasa
We make a second simpliﬁcation to the problem—from that of search to that
of identiﬁcation. Instead of searching Gonline for a path, we frame the problem as
identifying a valid path from a library of ‘good’ candidate paths Ξ=(ξ1,ξ2,...,ξ m).
Each path can be thought of as carving out a “decision region” over the space of
worlds. By abuse of notation, we write ξ⊆Hto denote that each path corresponds
to a set of worlds for which it would be feasible.
If a set of edge evaluations S⊆Eare performed, let the observed outcome vector
be denoted by xS. Let the version space H(xS)be the set of worlds consistent with
observation vector xS, i.e. H(xS)={h∈H|∀e∈S,h(e)=xS(h)}.
We deﬁne a policy πas a mapping from observation vector xSto edges. A policy
terminates when it shows that at least one path is valid, or all paths are invalid. Let
hbe the underlying world on which it is evaluated. Denote the observation vector of
a policy πasxS(π,h). The expected cost of a policy πisc(π)=Eh[c(xS(π,h)]
where c(xS)is the cost of all edge evaluations e∈S. The objective is to compute a
policy π∗with minimum cost that ensures at least one path is valid, i.e.
π∗∈arg min
πc(π)s.t∀h,∃ξd:P(ξd|xS(π,h))=1( 1 )
3 Approach
We adopt the framework of Decision Region Edge Cutting ( DiRECt )[4] which we
describe brieﬂy in our context. DiRECt creates a graph where nodes are hypotheses
(worlds) and edges are between hypotheses belonging to different regions. Perform-
ing a test ‘cuts’ edges if any one of the hypotheses is inconsistent with the testoutcome. Once all the edges are cut, the uncertainty collapses into atleast one of the
regions. A key difﬁculty arises from the fact that regions overlap. DiRECt solves this
problem by deﬁning a set of subproblems containing disjoint regions with the caveat
that solving any one subproblem sufﬁces, and combining subproblems with a Noisy-
OR operator. Interestingly, DiRECt can directly solve our edge evaluation problem!
A concern is that DiRECt requires |H|computation per sub-problem, which can
beO/parenleftbig
2
E/parenrightbig
. We circumvent this problem by computing a decision tree ofﬂine using
DiRECt which has a runtime of O(1). The nodes of the tree encode which edge to
evaluate. The tree branches on the outcome of the evaluation. The tree terminates
on a leaf node when the uncertainty has been pushed onto one region. The tree also
terminates if there are no consistent worlds in its database that matches the outcomevector.
Hence, if the world at test time is not in the training data, it may lead to a situation
where the ofﬂine decision tree terminates without ﬁnding a solution. We need apolicy to execute online under such situations. However, we would still like the
policy to be informed by some prior. Since assuming independent edges is a common
simpliﬁcation [ 2,6,9,17,19], we assume edges are independent Bernoulli random
variables. This leads to a new active learning problem deﬁnition. While naively
applying DiRECt requires O/parenleftbig
2
E/parenrightbig
per iteration, we present a more efﬁcient Bernoulli
A Bayesian Active Learning Approach to Adaptive Motion Planning 37
Subregion Edge Cutting ( BiSECt )algorithm [ 5], which computes each subproblem
inO(E)time.
4 Preliminary Results
Figure 3shows some preliminary results of our approach. We evaluated our approach
on a collection of datasets spanning a spectrum of motion planning applications thatrange from simplistic yet insightful 2D problems to more realistic high dimension
problems as encountered by a helicopter or a robot arm. The autonomous helicopter
Fig. 3 Comparison of LazySP ,LazySPSet ,BiSECt andDiRECt +BiSECt on a selection of
datasets. 4 samples from each dataset is shown. The ﬁnal performance of all algorithms on a testproblem is shown: valid edges checked (green) and invalid edges checked (red)
38 S. Choudhury and S. S. Srinivasa
dataset in particular is our target application. A typical dataset is constructed as
follows. The robot dynamics information is used to create an explicit graph G=
(V,E)and a start and goal vertex. A dataset of nworlds is sampled from a designed
generative model. Each edge is evaluated on each world to create a test outcome
matrix. A library of paths is created by solving for k−shortest paths on the dataset
and sub-sampling it to maintain a size of m. This is then used to create a binary
membership matrix encoding the validity of a path on a world. 10% of the data
is used for test, remainder for training. The algorithms work with these abstract
representations and do not need access to application speciﬁc details.We see that theactive edge evaluation methods vastly outperform the passive baselines LazySP and
LazySPSet .
5 Discussion and Future Directions
We empirically evaluated BiSECt on a spectrum of synthetic and real world planning
problems [ 5]. These results demonstrate the efﬁcacy of leveraging prior data to
signiﬁcantly reduce collision checking effort. Interestingly, they raise a lot of research
questions which we discuss below.
Q1 We need to relax assumptions in the framework in (Sect. 2)—the prior is speciﬁed
only via a ﬁnite database of worlds and selection is limited to a ﬁxed library of paths.
(a) Can we better model how collision information propagates through the graph?
(b) Can we circumvent explicitly computing a candidate set of paths?
(a)Specifying the prior as a ﬁnite database of worlds is memory inefﬁcient and
can lead to overﬁtting. A better way is to build and update belief distributions over
conﬁguration space using techniques such as KDE [ 6], mixture of Gaussians [ 14],
RKHS [ 23] or even customized models [ 22]. The efﬁcacy of these models depends
on how accurately they can represent the world, how efﬁciently they can be updated
and how efﬁciently they can be projected on the graph. The active learning not only
needs to reason about the current belief of the world, but belief posteriors conditionedon possible outcomes of edge evaluation.
(b)Explicitly reasoning about a set of paths is expensive as the size of the set can
be exponential in the number of edges in the graph. An alternate method is to directlyreason about a distribution over all possible paths between two vertices implicitly,
however, this can be intractable. Tractable approximations to such functions have
been explored in the context of edge selection [ 9]. Adopting such techniques in the
active learning setting would be interesting to pursue.
Q2 The active learning algorithms we use are restrictive and expensive.
(a) Are there alternatives to solving DRD that are less restrictive?(b) Are there more efﬁcient approaches that do not require enumerating worlds?
(a)The restriction in DiRECt arises from the need to show adaptive submodularity
for the surrogate objective. This is difﬁcult to show in general as it depends not
A Bayesian Active Learning Approach to Adaptive Motion Planning 39
only on the objective but also on the set of possible realizations and the probability
distribution over these realizations. In contrast, interactive submodularity [13], which
only requires the objective be submodular for a ﬁxed hypothesis, is easier to showand can lead to simpler surrogate functions.
(b)While DiRECt solves the exploration-exploitation problem in a principled
fashion, it requires enumerating all plausible worlds and reasoning about them jointly.An alternative approach to efﬁcient exploration-exploitation is via posterior sampling
for reinforcement learning (PSRL) [21]. PSRL samples a single world from the
posterior at the start of the episode, solves for an optimal policy and executes it.Hence PSRL efﬁciently exploits while exploration is automatically obtained by the
variance of sampled worlds. Such policies also enjoy Bayesian regret bounds.
Q3 We have so far been concerned with ﬁnding a feasible path
(a) Can we extend our framework to the optimal path identiﬁcation problem?
(b) Furthermore, can we achieve asymptotic optimality via incremental densiﬁca-tion?
(a)Introducing an additional criteria of minimizing path cost creates a tension
between producing high quality paths and expending more evaluation effort. A desir-able behaviour is to have an anytime algorithm that traverses the Pareto-frontier [ 6].
We can tweak our algorithm to display such behavior—we ﬁrst solve the feasible
path identiﬁcation problem, prune all costlier paths (including this) from the library,prune worlds which belonged only to those paths, and then solve the feasible path
problem again. However, while this will eventually converge to the optimal path, we
can not necessarily control the speed of convergence.
(b)A naive approach to achieve asymptotic optimality is to add a batch of edges
(densify) to the current graph, ﬁnd the optimal path and repeat [ 7]. An key question
is—where should we add samples? We would like to use the current belief about the
world to actively add edges in promising areas that lead to discovery of better paths
with little evaluation effort. Additionally, we would also like to interleave planningand densiﬁcation such that the output of one constantly informs the other.
References
1. Bohlin, R., Kavraki, L.E.: Path planning using lazy PRM. In: ICRA (2000)
2. Burns, B., Brock, O.: Sampling-based motion planning using predictive models. In: ICRA
(2005)
3. Canny, J.: The Complexity of Robot Motion Planning. MIT Press, Cambridge (1988)4. Chen, Y., Javdani, S., Karbasi, A., Bagnell, D., Srinivasa, S., Krause, A.: Submodular surrogates
for value of information. In: AAAI (2015)
5. Choudhury, S., Javdani, S., Srinivasa, S., Scherer, S.: Near-optimal edge evaluation in explicit
generalized binomial graphs, Arxiv (2017)
6. Choudhury, S., Dellin, C.M., Srinivasa, S.S.: Pareto-optimal search over conﬁguration space
beliefs for anytime motion planning. In: IROS (2016)
7. Choudhury, S., Salzman, O., Choudhury, S., Srinivasa, S.S.: Densiﬁcation strategies for anytime
motion planning over large dense roadmaps. In: ICRA (2017)
40 S. Choudhury and S. S. Srinivasa
8. Dasgupta, S.: Analysis of a greedy active learning strategy. In: NIPS (2004)
9. Dellin, C.M., Srinivasa, S.S.: A unifying formalism for shortest path problems with expensive
edge evaluations via lazy best-ﬁrst search over paths with edge selectors. In: ICAPS (2016)
10. Gammell, J.D., Srinivasa, S.S., Barfoot, T.D.: Batch informed trees: sampling-based optimal
planning via heuristically guided search of random geometric graphs. In: ICRA (2015)
11. Golovin, D., Krause, A.: Adaptive submodularity: theory and applications in active learning
and stochastic optimization. J. Artif. Intell. Res. (2011)
12. Golovin, D., Krause, A., Ray, D.: Near-optimal bayesian active learning with noisy observa-
tions. In: NIPS (2010)
13. Guillory, A., Bilmes, J.A.: Interactive submodular set cover. In: Proceedings of the 27th Inter-
national Conference on Machine Learning (ICML-10) (2010)
14. Huh, J., Lee, D.D.: Learning high-dimensional mixture models for fast collision detection in
rapidly-exploring random trees. In: ICRA (2016)
15. Javdani, S., Chen, Y., Karbasi, A., Krause, A., Bagnell, D., Srinivasa, S.: Near optimal bayesian
active learning for decision making. In: AISTATS (2014)
16. Karaman, Sertac, Frazzoli, Emilio: Sampling-based algorithms for optimal motion planning.
Int. J. Robot. Res. 30(7), 846–894 (2011)
17. LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)
18. LaValle, S.M., Kuffner Jr, J.J.: Randomized kinodynamic planning. IJRR (2001)19. Narayanan, V., Likhachev, M.: Heuristic search on graphs with existence priors for expensive-
to-evaluate edges. In: ICAPS (2017)
20. Nielsen, C.L., Kavraki, L.E.: A 2 level fuzzy prm for manipulation planning. In: IROS (2000)21. Osband, I., Russo, D., Van Roy, B.: (More) efﬁcient reinforcement learning via posterior sam-
pling. In: NIPS (2013)
22. Pan, J., Chitta, S., Manocha, D.: Faster sample-based motion planning using instance-based
learning. In: WAFR. Springer (2012)
23. Ramos, F., Ott, L.: Hilbert maps: scalable continuous occupancy mapping with stochastic
gradient descent. IJRR (2016)
24. Tallavajhula, A., Choudhury, S., Scherer, S., Kelly, A.: List prediction applied to motion plan-
ning. In: ICRA (2016)
Materials That Make Robots Smart
Nikolaus Correll and Christoffer Heckman
1 Introduction
The impressive functionality of natural systems such as the camouﬂage skin of a
cuttleﬁsh, morphing wings of an eagle, structural adaptation of a mammalian boneor the Banyan tree, or the many sensor modalities in the human skin are enabled
by tight integration of sensing, actuation, computation and communication in these
systems [ 22]. Figure 1shows schematic drawings of some of these natural systems,
illustrating how function arises from sensors, actuators, nerves and vascular systems
being co-located.
This is fundamentally different from how we construct robotic systems, which
consists of homogeneous, hierarchical components such as structures, gears, links
and joints that are interfaced by sensing and computer systems, but lack the tightintegration of sensing and computation that biological systems exhibit (Fig. 2).
This paradigm has begun to change with the emergence of soft robotics and multi-
material manufacturing techniques [ 9], which has led to fully self-contained robotic
systems [ 3] and components [ 2,18] that blur the distinction between a robot and a
material [ 8]. While soft robotics provides radically novel ways for locomotion [ 20,
31] and manipulation [ 11], this ﬁeld has led to new manufacturing approaches that
allow us to get much closer to the integrated nature of biological systems [ 9,29].
We believe it is this integration that will allow us to create robotic components that
are in some sense and by themselves autonomous, thereby facilitating the creationof more complex and adaptable autonomous robotic systems. We argue that such
N. Correll (B)·C. Heckman
Department of Computer Science, University of Colorado, Boulder, CO, USAe-mail: nikolaus.correll@colorado.edu
C. Heckman
e-mail: christoffer.heckman@colorado.edu
N. Correll
Material Science Engineering, University of Colorado, Boulder, CO, USA
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_641
42 N. Correll and C. Heckman
Fig. 1 Biological tissue that tightly integrates sensing, actuation, computation and communica-
tion. From left to right: chromatophore in an octopus skin (©Springer V erlag, from [ 7]), human
skin (©Public Domain), octopus suckers. Computation and communication are implemented by anervous system, power is provided by a vascular system
Fig. 2 Most robotic systems clearly separate between perception and control. Here, the role of the
material is to make the transition from a computational model to real-world dynamics as smooth aspossible. This is not the case in biological systems that are sensor rich, dynamic and over-actuated
components, which necessarily implement design trade-offs and are therefore more
suitable for one robot design than another, reach their true potential once they are
available in the form of a material .
A deﬁnition of “material” that extends to these devices is a critical element to this
newly developing ﬁeld. This quandary permeated the halls of the ﬁrst “Workshop
on Robotic Materials”, which was held March 10–12, 2017, in Boulder, CO. The
consensus reached by that workshop were that materials have the properties of:
1.Functionality independent of size , i.e., performance is unchanged when cut in
half (up to a reasonable discretization);
2.Self-similarlity and bulk reconﬁgurability , i.e., consisting of homogeneous
elements that can be arranged in either an amorphous or a discrete, grid-like
fashion; and
Materials That Make Robots Smart 43
3.Robustness , i.e. the material does not lose its capabilities should failure of any
constituent elements occur.
These properties align well with the contemporary material science perspective
[6]. Here, “robotic materials” are composites with structural, sensing, actuation, and
computational “phases.” Such phases can be either dispersed, such as in compos-
ites that are reinforced with granular particles, or placed in an anisotropic manner,
such as in ﬁber reinforced composites. While providing the material with improvedfunctionality, additional phases typically introduce challenges in terms of structural
integrity and manufacturing at the interfacial bond between the matrix and the dis-
persed phases. Here, robotic materials pose hard problems by requiring the integra-tion of hard elements into soft materials, as well as materials and devices that are not
designed to create strong bonds with others.
Once the conditions that deﬁne a material are met and manufacturing challenges
are solved, such materials could be manufactured and distributed in volumetric units
and used to enhance the functionality of a robotic system. Section 2describes a series
of such systems, not necessarily all with robotic applications, that emphasize the
distributed algorithms, material science, communication, power, and manufacturing
challenges. Section 3then describes what kind of materials we envision to transform
how we make robots.
2 State-of-the-Art
Figure 3shows a series of systems which, although crude, adhere to the necessary
conditions for making them a material as laid out in the deﬁnition of materials
enunciated above.
The camouﬂage system (top-left) consists of a swarm of “Droplets” miniature
robots, which perform consensus and distributed pattern generation algorithms to
match dominant patterns they perceive in the environment [ 19]. As the algorithm is
fully local, the system works no matter what the shape or size of the arrangement is,making an integration of color-changing particles into rubber sheets conceivable.
The texture sensing skin (top-center) is able to localize objects as well as differ-
entiate up to 15 different textures by measuring the vibrations that are induced whenrubbing against the skin [ 14]. The algorithm is fully distributed, and a network of
microcontrollers has been integrated into a rubber skin.
The shape-changing beam (top-right) consists of six identical variable stiffness
elements [ 21], which can calculate the required stiffness to reach an arbitrary shape
after bending in a fully distributed way [ 23]. The proposed algorithm scales lin-
early with the length of the beam, albeit allows for instantaneous motion, reduc-
ing the computation to a constant time operation with respect to the system’s
motion.
The gesture-detecting skin (bottom-left) consists of 8 ×8 proximity sensors inte-
grated into transparent polymer and is capable to distinguish between a variety of
social touch gestures [ 15]. Sensor information is accessed using a hierarchical bus
44 N. Correll and C. Heckman
Fig. 3 From top-left to bottom-right: active camouﬂage [ 19], texture-detecting skin [ 14], a shape-
changing beam [ 21], gesture-detecting skin [ 15], a sound localizing dress [ 30], and an interactive
fa´cade [ 13]
system and processed for an area of 8 ×8 by a deep-learning network that ﬁts onto a
microcontroller. This system fulﬁlls the material requirements above with the shown
patch as individual unit, making a large deployment conceivable in which high-levelgesture information are communicated in a hop-by-hop fashion.
The dress (bottom-center) is the prototype of a smart fabric that uses a reg-
ular arrangement of computing elements to triangulate the direction incomingsounds [ 30].
The facade system in the lower-right is created from identical building blocks
that are arranged in a regular structure and are each equipped with the ability tosense a human hand and change their color [ 10]. Each block shares power and local
communication with its neighbors. By locally exchanging information about touch
events, the system is able to recognize a series of letters drawn across its surface [ 13],
functions no matter how many elements the surface has, and is able to interpolate
across broken elements.
3 Vision and Challenges
The examples above demonstrate the overall feasibility of obtaining complex signal
processing and control ability from fully distributed systems. All of these materialscould be made with smaller components and integrated at higher density, and all
of them rely only on local information, making them scalable. Some of the pre-
sented materials have already been created with robotic applications in mind. Forexample, skins that localize and recognize gestures and textures could dramatically
Materials That Make Robots Smart 45
facilitate the robot design problem in highly asymmetric ways. If embedded into an
end effector, grasp selection and reactive control would be available based on local
information from the skin [ 27] rather than through exteroceptive sensors perceiving
geometry. Not only does local processing take the burden of a central processing
unit, but also directly addresses the problems of registering the location of indi-
vidual sensors on the robotic skin and routing the information. Similarly, a shapechanging beam would allow to control the structure of a robotic system such as run-
ning and walking robots, exo-skeletons, but also airfoils of aircraft or autonomous
vehicles.
Once we are ready to delegate to a material the functionality that has previously
only been available at the device level, many other applications become conceivable.
For example, smart rubber embedded with light-emitting computational elementscoated over a robot body would allow it to camouﬂage, act as a display for arbitrary
information, or indicate material fatigue. Embedding a polymer with proximity sen-
sors could not only create robotic skins, but smart wheels that can measure the groundt h e ya r eo n[ 16] or the tire proﬁle from the inside to detect skid, and turn any solid
into an input device that is aware of its surroundings. Using accelerometers and gyro-
scopes that can compute and communicate with their local neighbors would allow
a material to ascertain proprioceptive state with unprecedented spatial resolution.
Functionality can also come from the polymer itself. For example, polymers mightcontract, expand, or change their stiffness, viscosity or color when locally activated
by light, electric current or magnetic ﬁelds [ 22], shape-changing structures, or tires
that adapt their proﬁle to driving conditions.
Interestingly, many of the key technologies to enable these materials already
exist. Computational elements have been dramatically reduced in size, making the
operation of thousands of wireless devices in close proximity feasible. This brings usclose to the original vision of distributed MEMS and “smart dust” [ 5] and challenges
speciﬁc to the wireless networking community are described in [ 12].
Given a small enough computational element with wireless communication [ 26],
many conceivable robotic materials could be produced using existing manufacturing
techniques for composite materials such as vacuum forming, shape deposition man-
ufacturing and overmolding. A key challenge of such highly-functioning materialsis the wide range of length scales, ranging from microscale sensors and compu-
tational elements to the meter-scale, to which the ﬁnal material must extend. It is
this scaling property that leads most conventional manufacturing techniques to theirlimitations. One possible solution to this problem is to assemble robotic materials
using autonomous robots or multi-material 3D printers [ 25]. Given the ability to
provide power wirelessly [ 1] and to localize the computational elements inside the
structure after manufacturing, either using external ﬁelds or using self-localization
technologies [ 24], it is also conceivable that it may soon be possible to simply mix
small pellets that sense, communicate and compute and trigger actuation into a liquid
material and let it cure in the desired shape.
Reaching the desired homogeneous distribution of dispersed particles in a poly-
mer matrix is a problem that is well studied in the composite materials community
46 N. Correll and C. Heckman
for nanoscale elements [ 32], but more difﬁcult to achieve for objects at the millime-
ter scale. Robotic materials will also beneﬁt from advances in polymer themselves
ranging from electro-active polymer muscles [ 17] with improved performance to a
wide range of smart polymers that can double as sensors or actuators [ 4].
Challenges lie not only in developing the different components of robotic mate-
rials, however, but also their interaction. Integrating sensors, for example, offers theopportunity of “morphological computation” [ 28], that is the placement of sensors in
a way that minimizes the computations required to process them. Similarly, choos-
ing material properties carefully will affect the way the systems into which they areintegrated are controlled, a challenge unto itself.
It is clear that the vision of robotic material requires the close collaboration of
researchers in disparate ﬁelds like wireless networking, robotics, and chemistry. Yet,bringing those groups together to create systems that go beyond individual contribu-
tions at device level remains a major challenge. In particular, isolated contributions
from any individual discipline are often difﬁcult to transfer as challenges of systemintegration have been neglected, possibly leading to designs that are fundamentally
infeasible.
4 Conclusion
Materials that make robots smart have the potential to dramatically simplify robot
design by off-loading signal processing and control into the material, thereby
abstracting some traditionally high-level functions. Creating such materials poses
many challenges in distributed algorithms, manufacturing, as well as platform tech-nologies such as wireless power transfer, directional RF communication, localization
techniques, and polymers with interesting capabilities that can enhance sensing and
actuation.
Albeit somewhat functional, current robotic materials are too crude to be prac-
tical and will beneﬁt from the continued development of these techniques and an
improved understanding of how to integrate them. It is ultimately the robotic appli-cations enabled by such materials that need to be compelling, leading then to interdis-
ciplinary efforts that combine wireless sensor networking experts, material scientists
and roboticists. From these ﬁrst conversations, materials that enable perception byperforming distributed computation seem to be more immediate candidates than
materials that perform actuation. While the application of such materials are not
limited to robotics, it is the robotics community that is well-positioned to appreciate
the challenges that span from material to computer science and contribute to the
multi-modal problem solving required.
Acknowledgements This work was sponsored by ARO under grant number W911NF-16-1-0476,
program manager S. Stanton, by AFOSR, program manager B. “Les” Lee, and DARPA award no.N65236–16–1–1000. We are grateful for this support.
Materials That Make Robots Smart 47
References
1. Agbinya, J.I.: Wireless Power Transfer, vol. 45. River Publishers, Aalborg (2015)
2. Asbeck, A.T., Kim, S., Cutkosky, M.R., Provancher, W.R., Lanzetta, M.: Scaling hard vertical
surfaces with compliant microspine arrays. Int. J. Robot. Res. 25(12), 1165–1179 (2006)
3. Bartlett, N.W., Tolley, M.T., Overvelde, J.T., Weaver, J.C., Mosadegh, B., Bertoldi, K., White-
sides, G.M., Wood, R.J.: A 3d-printed, functionally graded soft robot powered by combustion.Science 349(6244), 161–165 (2015)
4. Bauer, S., Bauer-Gogonea, S., Graz, I., Kaltenbrunner, M., Keplinger, C., Schwödiauer, R.:
25th anniversary article: a soft future: from robots and sensor skin to energy harvesters. Adv.Mater. 26(1), 149–162 (2014)
5. Berlin, A.A., Gabriel, K.J.: Distributed MEMS: new challenges for computation. IEEE Comput.
Sci. Eng. 4(1), 12–16 (1997)
6. Brostow, W., Lobland, H.E.H.: Materials: Introduction and Applications. Wiley, New Y ork
(2016)
7. Cloney, R.A., Florey, E.: Ultrastructure of cephalopod chromatophore organs. Cell Tissue Res.
89(2), 250–280 (1968)
8. Correll, N., Önal, Ç.D., Liang, H., Schoenfeld, E., Rus, D.: Soft autonomous materials using
active elasticity and embedded distributed computation. Experimental Robotics, pp. 227–240.Springer, Berlin (2014)
9. Cutkosky, M.R., Kim, S.: Design and fabrication of multi-material structures for bioinspired
robots. Philos. Trans. R. Soc. Lond. A: Math. Phys. Eng. Sci. 367(1894), 1799–1813 (2009)
10. Farrow, N., Sivagnanadasan, N., Correll, N.: Gesture based distributed user interaction sys-
tem for a reconﬁgurable self-organizing smart wall. In: Proceedings of the 8th InternationalConference on Tangible, Embedded and Embodied Interaction, pp. 245–246. ACM (2014)
11. Farrow, N., Li, Y ., Correll, N.: Morphological and embedded computation in a self-contained
soft robotic hand (2016). arXiv:1605.00354
12. Han, R., Correll, N., Pister, K., Dutta, P .: New directions: wireless robotic materials. In: 15th
ACM Conference on Embedded Networked Sensor Systems (SenSys), Delft, The Netherlands(2017)
13. Hosseinmardi, H., Mysore, A., Farrow, N., Correll, N., Han, R.: Distributed spatiotemporal
gesture recognition in sensor arrays. ACM Trans. Auton. Adapt. Syst. (TAAS) 10(3), 17 (2015)
14. Hughes, D., Correll, N.: Texture recognition and localization in amorphous robotic skin. Bioin-
spiration Biomim. 10(5), 055002 (2015)
15. Hughes, D., Farrow, N., Proﬁta, H., Correll, N.: Detecting and identifying tactile gestures using
deep autoencoders, geometric moments and gesture level features. In: Proceedings of the 2015ACM on International Conference on Multimodal Interaction, pp. 415–422. ACM (2015)
16. Hughes, D., Heckman, C., Correll, N.: Terrain sensitive tires for autonomous driving. In:
Workshop on Material Robotics at Robotics, Science and Systems (RSS), Cambridge, MA(2017)
17. Keplinger, C., Sun, J.-Y ., Foo, C.C., Rothemund, P ., Whitesides, G.M., Suo, Z.: Stretchable,
transparent, ionic conductors. Science 341(6149), 984–987 (2013)
18. Kim, S., Clark, J.E., Cutkosky, M.R.: iSprawl: design and tuning for high-speed autonomous
open-loop running. Int. J. Robot. Res. 25(9), 903–912 (2006)
19. Li, Y ., Klingner, J., Correll, N.: Distributed camouﬂage for swarm robotics and smart materials.
In: Distributed Autonomous Robotic Systems (DARS) (2016)
20. Marchese, A.D., Onal, C.D., Rus, D.: Autonomous soft robotic ﬁsh capable of escape maneu-
vers using ﬂuidic elastomer actuators. Soft Robot. 1(1), 75–87 (2014)
21. McEvoy, M., Correll, N.: Shape change through programmable stiffness. Experimental
Robotics, pp. 893–907. Springer, Berlin (2016)
22. McEvoy, M.A., Correll, N.: Materials that couple sensing, actuation, computation and com-
munication. Science 347(6228) (2015)
23. McEvoy, M.A., Correll, N.: Distributed inverse kinematics for shape-changing robotic mate-
rials. Procedia Technol. 26, 4–11 (2016)
48 N. Correll and C. Heckman
24. Moffo, D., Canalda, P ., Spies, F.: Relative and absolute positioning in ultra dense MEMS
system. In: 2016 IEEE International Conference on Computer and Information Technology(CIT), pp. 140–147. IEEE (2016)
25. O’Donnell, J., Kim, M., Y oon, H.-S.: A review on electromechanical devices fabricated by
additive manufacturing. J. Manuf. Sci. Eng. 139(1), 010801 (2017)
26. Pannuto, P ., Andersen, M.P ., Bauer, T., Campbell, B., Levy, A., Culler, D., Levis, P ., Dutta,
P .: A networked embedded system platform for the post-mote era. In: Proceedings of the 12thACM Conference on Embedded Network Sensor Systems, pp. 354–355. ACM (2014)
27. Patel, R., Cox, R., Romero, B., Correll, N.: Improving grasp performance using in-hand prox-
imity and contact sensing (2017). arXiv:1701.06071
28. Pfeifer, R., Iida, F., Gómez, G.: Morphological computation for adaptive behavior and cogni-
tion. International Congress Series, vol. 1291, pp. 22–29. Elsevier, Amsterdam (2006)
29. Polygerinos, P ., Correll, N., Morin, S.A., Mosadegh, B., Onal, C.D., Petersen, K., Cianchetti,
M., Tolley, M.T., Shepherd, R.F.: Soft robotics: review of ﬂuid-driven intrinsically soft devices;manufacturing, sensing, control, and applications in human-robot interaction. Adv. Eng. Mate-rials
30. Proﬁta, H., Farrow, N., Correll, N.: Flutter: an exploration of an assistive garment using dis-
tributed sensing, computation and actuation. In: Proceedings of the Ninth International Con-
ference on Tangible, Embedded, and Embodied Interaction, pp. 359–362. ACM (2015)
31. Tolley, M.T., Shepherd, R.F., Karpelson, M., Bartlett, N.W., Galloway, K.C., Wehner, M.,
Nunes, R., Whitesides, G.M., Wood, R.J.: An untethered jumping soft robot. In: 2014 IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS 2014), pp. 561–566. IEEE(2014)
32. Xie, X.-L., Mai, Y .-W., Zhou, X.-P .: Dispersion and alignment of carbon nanotubes in polymer
matrix: a review. Mater. Sci. Eng.: R: Rep. 49(4), 89–112 (2005)
Pragmatic-Pedagogic Value Alignment
Jaime F. Fisac, Monica A. Gates, Jessica B. Hamrick, Chang Liu,
Dylan Hadﬁeld-Menell, Malayandi Palaniappan, Dhruv Malik,S. Shankar Sastry, Thomas L. Grifﬁths and Anca D. Dragan
1 Introduction
The accelerating progress in artiﬁcial intelligence (AI) and robotics is bound to have asubstantial impact in society, simultaneously unlocking new potential in augmentingand transcending human capabilities while also posing signiﬁcant challenges to safe
and effective human-robot interaction. In the short term, integrating robotic systems
J. F. Fisac (B)·M. A. Gates ·J. B. Hamrick ·C. Liu ·D. Hadﬁeld-Menell ·M. Palaniappan ·
D. Malik ·S. S. Sastry ·T. L. Grifﬁths ·A. D. Dragan
University of California, Berkeley, CA, USAe-mail: jﬁsac@berkeley.edu
M. A. Gates
e-mail: mgates@berkeley.edu
J. B. Hamrick
e-mail: jhamrick@berkeley.edu
C. Liu
e-mail: changliu@berkeley.edu
D. Hadﬁeld-Menell
e-mail: dhm@berkeley.edu
M. Palaniappan
e-mail: malayandi@berkeley.edu
D. Malik
e-mail: dhruvmalik@berkeley.edu
S. S. Sastry
e-mail: shankar\_sastry@berkeley.edu
T. L. Grifﬁths
e-mail: tom\_grifﬁths@berkeley.edu
A. D. Dragan
e-mail: anca@berkeley.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_749
50 J. F. Fisac et al.
into human-dominated environments will require them to assess the intentions and
preferences of their users in order to assist them effectively, while avoiding fail-
ures due to poor coordination. In the long term, ensuring that advanced and highlyautonomous AI systems will be beneﬁcial to individuals and society will hinge on
their ability to correctly assimilate human values and objectives [ 1]. We envision
the short- and long-term challenges as being inherently coupled, and predict thatimproving the ability of robots to understand and coordinate with their human users
will inform solutions to the general AI value-alignment problem .
Successful value alignment requires moving from typical single-agent AI formu-
lations to robots that account for a second agent—the human—who determines what
the objective is. In other words, value alignment is fundamentally a multi-agent prob-
lem. Cooperative Inverse Reinforcement Learning (CIRL) formulates value align-ment as a two-player game in which a human and a robot share a common reward
function, but only the human has knowledge of this reward [ 2]. In practice, solving
a CIRL game requires more than multi-agent decision theory: we are not dealingwith anymulti-agent system, but with a human-robot system. This poses a unique
challenge in that humans do not behave like idealized rational agents [ 3]. However,
humans do excel at social interaction and are extremely perceptive of the mental
states of others [ 4,5]. They will naturally project mental states such as beliefs and
intentions onto their robotic collaborators, becoming invaluable allies in our robots’quest for value alignment.
In the coming decades, tackling the value-alignment problem will be crucial to
building collaborative robots that know what their human users want. In this paper,we show that value alignment is possible not just in theory, but also in practice. We
introduce a solution for CIRL based on a model of the human agent that is grounded
in cognitive science ﬁndings regarding human decision making [ 6] and pedagogical
reasoning [ 7]. Our solution leverages two closely related insights to facilitate value
alignment. First, to the extent that improving their collaborator’s understanding of
their goals may be conducive to success, people will tend to behave pedagogically ,
deliberately choosing their actions to be informative about these goals. Second, the
robot should anticipate this pedagogical reasoning in interpreting the actions of its
human users, akin to how a pragmatic listener interprets a speaker’s utterance in
natural language. Jointly, pedagogical actions and pragmatic interpretations enable
stronger and faster inferences among people [ 7]. Our result suggests that it is possible
for robots to partake in this naturally-emerging equilibrium, ultimately becomingmore perceptive and competent collaborators.
2 Solving Value Alignment Using Cognitive Models
2.1 Cooperative Inverse Reinforcement Learning (CIRL)
Cooperative Inverse Reinforcement Learning (CIRL) [ 2] formalizes value alignment
as a two-player game, which we brieﬂy present here. Consider two agents, a human
Pragmatic-Pedagogic Value Alignment 51
Hand a robot R, engaged in a dynamic collaborative task involving a (possibly
inﬁnite) sequence of steps. The goal of both agents is to achieve the best possible
outcome according to some objective θ∈Θ. However, this objective is only known
toH. In order to contribute to the objective, Rwill need to make inferences about
θfrom the actions of H(an Inverse Reinforcement Learning (IRL) problem), and
Hwill have an incentive to behave informatively so that Rbecomes more helpful,
hence the term cooperative IRL.
Formally, a CIRL game is a dynamic (Markov) game of two players ( Hand R),
described by a tuple /angbracketleftS,{AH,AR},T,{Θ,r},P0,γ/angbracketright, where Sis the set of possible
states of the world; AH,ARare the sets of actions available to Hand Rrespectively;
T:S×S×AH×AR→[0,1]a discrete transition measure1over the next state,
conditioned on the previous state and the actions of Hand R:T(s/prime|s,aH,aR);Θis
the set of possible objectives; r:S×AH×AR×Θ→ Ris a cumulative reward
function assigning a real value to every tuple of state and actions for a given objective:
r(s,aH,aR;θ);P0:S×Θ→[0,1]is a probability measure on the initial state and
the objective; γ∈[0,1]is a geometric time discount factor making future rewards
gradually less valuable.
2.2 Pragmatic Robots for Pedagogic Humans
Asymmetric information structures in games (even static ones) generally induce an
inﬁnite hierarchy of beliefs : our robot will need to maintain a Bayesian belief over
the human’s objectives to decide on its actions. To reason about the robot’s decisions,
the human would in principle need to maintain a belief on the robot’s belief, whichwill in turn inform her decisions, thereby requiring the robot to maintain a belief on
the human’s belief about its own belief, and so on [ 8]. In [ 2], it was shown that an
optimal pair of strategies can be found for any CIRL game by solving a partially
observed Markov decision process (POMDP). This avoids this bottomless recursion
as long as both agents are rational and can coordinate perfectly before the start of
the game.
Unfortunately, when dealing with human agents, rationality and prior coordination
are nontrivial assumptions. Finding an equivalent tractability result for more realistic
human models is therefore crucial in using the CIRL formulation to solve real-world value-alignment problems. We discover the key insight in cognitive studies of
human pedagogical reasoning [7], in which a teacher chooses actions or utterances
to inﬂuence the beliefs of a learner who is aware of the teacher’s intention. The
teacher can then exploit the fact that the learner can interpret utterances pragmatically.
Inﬁnite recursion is averted by ﬁnding a ﬁxed-point relation between the teacher’sbest utterance and the learner’s best interpretation, exploiting a common modeling
assumption in Bayesian theory of mind: the learner models the teacher as a noisily
1Note that the theoretical formulation is easily extended to arbitrary measurable sets; we limit our
analysis to ﬁnite state and objective sets for computational tractability and clarity of exposition.
52 J. F. Fisac et al.
rational decision maker [ 9], who will be likelier to choose utterances causing the
learner to place a high posterior belief on the correct hypothesis, given the learner’s
current belief. While in reality, the teacher cannot exactly compute the learner’s belief,the model supposes that she estimates it (from the learner’s previous responses to her
utterances), then introduces noise in her decisions to capture estimation inaccuracies.
This framework can predict complex behaviors observed in human teaching-learninginteractions, in which pedagogical utterances and pragmatic interpretations permit
efﬁcient communication [ 7].
We adopt an analogous modeling framework to that in [ 7] for value alignment, with
a critical difference: the ultimate objective of the human is not to explicitly improve
the robot’s understanding of the true objective, but to optimize the team’s expected
performance towards this objective. Pedagogic behavior thus emerges implicitly to
the extent that a well-informed robot becomes a better collaborator.
2.3 Pragmatic-Pedagogic Equilibrium Solution to CIRL
The robot does not have access to the true objective θ, but rather estimates a belief bR
over θ. We assume that this belief on θcan be expressed parametrically (this is always
true if Θis a ﬁnite set), and deﬁne /triangleΘto be the corresponding (ﬁnite-dimensional)
parameter space, denoting R’s belief by bR∈/triangle Θ. While in reality the human cannot
directly observe bR, we assume, as in [ 7], that she can compute it or infer it from
the robot’s behavior (and model estimation inaccuracies as noise in her policy).
We can then let Q:S×/triangle Θ×AH×AR×Θ→ Rrepresent the state-action value
function of the CIRL game for a given objective θ, which we are seeking to compute:
ifθ∈Θis the true objective known to H, then Q(s,bR,aH,aR;θ)represents the
best performance the team can expect to achieve if Hchooses aHand Rchooses aR
from state s, with R’s current belief being bR.
In order to solve for Q, we seek to establish an appropriate dynamic programming
relation for the game, given a well-deﬁned information structure and a model of the
human’s decision making. Since it is typically possible for people to predict a robot’snext action if they see its beginning [ 10], we assume that Hcan observe a
Rat each
turn before committing to aH. A well-established model of human decision making
in psychology and econometrics is the Luce choice rule, which models people’sdecisions probabilistically, making high-utility choices more likely than those with
lower utility [ 9]. In particular, we employ a common case of the Luce choice rule,
the Boltzmann (or soft-max ) noisy rationality model [ 6], in which the probability of
a choice decays exponentially as its utility decreases in comparison to competing
options. The relevant utility metric in our case is the sought Q(which captures H’s
best expected outcome for each of her available actions a
H). Therefore the probability
that Hwill choose action aHhas the form
π⊙
H(aH|s,bR,aR;θ)∝exp/parenleftbig
βQ(s,bR,aH,aR;θ)/parenrightbig
, (1)
Pragmatic-Pedagogic Value Alignment 53
where β>0 is termed the rationality coefﬁcient ofHand quantiﬁes the concentra-
tion of H’s choices around the optimum; as β→∞ ,Hbecomes a perfect rational
agent, while, as β→0,Hbecomes indifferent to Q. The above expression can be
interpreted by Ras the likelihood of action aHgiven a particular θ. The evolution of
R’s belief bRis then given (deterministically) by the Bayesian update
b/prime
R(θ|s,bR,aR,aH)∝π⊙
H(aH|s,bR,aR;θ)bR(θ) , (2)
Jointly, ( 1) and ( 2) deﬁne a ﬁxed-point equation analogous to the one in [ 7],
which states how Rshould pragmatically update bRbased on a noisily rational
pedagogic aH. This amounts to a deterministic transition function for R’s belief,
b/prime
R=fb(s,bR,aH,aR). Crucially, however, the ﬁxed-point relation derived here
involves Qitself, which we have yet to compute.
Unlike H,Ris modeled as a rational agent; however, not knowing the true θ,t h e
best Rcan do is to maximize2the expectation of Qbased on its current belief3bR:
π∗
R(s,bR):=arg max
aR/summationdisplay
aH,θQ(s,bR,aH,aR;θ)·π⊙
H(aH|s,bR,aR;θ)bR(θ) . (3)
Combining ( 2) with the state transition measure T(s/prime|s,aH,aR), we can deﬁne
the Bellman equation for Hunder the noisily rational policy π⊙
Hfor any given θ∈Θ:
Q(s,bR,aH,aR;θ)=r(s,aH,aR;θ)+Es/prime,a/prime
H/bracketleftBig
γ·Q/prime/parenleftBig
s/prime,b/prime
R,a/prime
H,π∗
R(s/prime,b/prime
R);θ/parenrightBig/bracketrightBig
,
(4)
where s/prime∼T(s/prime|s,aH,aR);b/prime
R=fb(s,bR,aH,aR);a/prime
H∼π⊙
H(aH|s/prime,b/prime
R,π∗
R
(s/prime,b/prime
R);θ). Note that H’s next action a/prime
Himplicitly depends on R’s action at the
next turn.
Substituting ( 1)–(3)i n t o( 4), we obtain the sought dynamic programming relation
for the CIRL problem under a noisily rational-pedagogic human and a pragmatic
robot. The human is pedagogic because she takes actions according to ( 1), which
takes into account how her actions will inﬂuence the robot’s belief about the objective.
The robot is pragmatic because it assumes the human is actively aware of how her
actions convey the objective, and interprets them accordingly.
The resulting problem is similar to a POMDP (in this case formulated in belief-
state MDP form), with the important difference that the belief transition depends on
the value function itself. In spite of this complication, the problem can be solved inbackward time through dynamic programming: each Bellman update will be based
on a pragmatic-pedagogic ﬁxed point that encodes an equilibrium between the Q
function (and therefore H’s policy for choosing her action) and the belief transition
2We assume for simplicity that the optimum is unique or a well-deﬁned disambiguation rule exists.
3Note that this does not imply certainty equivalence , nor do we assume separation of estimation
and control: Ris fully reasoning about how its actions and those of Hmay affect its future beliefs.
54 J. F. Fisac et al.
(that is, R’s rule for interpreting H’s actions). Evidence in [ 7] suggests that people
are proﬁcient at ﬁnding such equilibria, even though uniqueness is not guaranteed in
general; study of disambiguation is an open research direction.
3 A Proof-of-Concept
We introduce the benchmark domain ChefWorld, a household collaboration setting
in which a human Hseeks to prepare a meal with the help of an intelligent robotic
manipulator R. There are multiple possible meals that Hmay want to prepare using
the available ingredients, and Rdoes not know beforehand which one she has chosen
(we assume Hcannot or will not tell Rexplicitly). The team obtains a reward only
ifH’s intended recipe is successfully cooked. If His aware of R’s uncertainty, she
should take actions that give Ractionable information, particularly the information
that she expects will allow Rto be as helpful as possible as the task progresses.
Our problem has 3 ingredients, each with 2 or 3 states: spinach (absent, chopped),
tomatoes (absent, chopped, puréed), and bread (absent, sliced, toasted). Recipes
correspond to (joint) target states for the food. Soup requires the tomatoes to be
chopped then puréed, the bread to be sliced then toasted, and no spinach. Salad
requires the spinach and tomatoes to be chopped, and the bread to be sliced then
toasted. HandRcan slice or chop any of the foods, while only Rcan purée tomatoes
or toast bread.
A simple scenario with the above two recipes is solved using discretized belief-
state value iteration and presented as an illustrative example in Fig. 1.Rhas a wrong
initial belief about H’s intended recipe. Under standard IRL, Hfails to communicate
Expert Human
Pragmatic RobotPedagogic HumanLiteral Robot
Reward: 1Reward: 0R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9
Initial ConditionsR Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9
R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9
R Belief
.1 .2 .3 .4 .5 .6 .7 .8 .9
Fig. 1 Simple collaborative scenario with two possible objectives. The human wants to make soup
but the robot is initially conﬁdent ( P=0.9) that her goal is salad. Even under a full POMDP
formulation, if the robot reasons “literally” about the human’s actions using IRL (assuming the
human behaves as an expert planning the optimal centralized team strategy) it fails to infer her
true objective. Conversely, under the pragmatic-pedagogic CIRL equilibrium, the robot expects the
human to choose informative actions when needed, which leads it to interpret the human’s wait
action in turn 2 as signaling soup (since a pedagogic human who had wanted salad could have
instead added spinach, fully disambiguating the goal). While the human’s actions are the same in
both solutions, only the pragmatic robot aligns itself to the human and completes the recipe
Pragmatic-Pedagogic Value Alignment 55
Table 1 A comparison of the expected value (or equivalently here, the probability of success)
a c h i e v e db yC I R La n dI R Lo nt h e ChefWorld domain with four recipes when the robot begins
with a uniform belief over the set of recipes. We ran each algorithm across different models of thehuman’s behavior, namely a rational model and a Boltzmann-rational model with various values ofβ(a higher βcorresponds to a more rational human). When the human is highly irrational ( β=1),
both CIRL and IRL unsurprisingly perform rather poorly. However, as the human becomes lessnoisy ( β=2.5,β=5), CIRL outperforms IRL by a signiﬁcant margin; in fact, the pragmatic-
pedagogic CIRL strategy with a Boltzmann-rational human performs comparably ( β=2.5) or even
substantially outperforms ( β=5) the IRL result achieved when the human is perfectly rational
Boltzmann
(β=1)Boltzmann
(β=2.5)Boltzmann
(β=5)Rational
IRL 0.2351 0.3783 0.4555 0.7083
CIRL 0.2916 0.7026 0.9727 1.0000
her recipe. But if Ris pragmatic and His pedagogic, His able to change R’s belief
and they successfully collaborate to make the meal.
In addition, we computed the solution to games with 4 recipes through a modiﬁ-
cation of POMDP value iteration (Table 1). In the pragmatic-pedagogic CIRL equi-
librium with β=5,Hand Rsuccessfully cook the correct recipe 97% of the time,
whereas under the standard IRL framework (with Hacting as an expert disregarding
R’s inferences) they only succeed 46% of the time—less than half as often.
4 Discussion
We have presented here an analysis of the AI value alignment problem that incorpo-
rates a well-established model of human decision making and theory of mind into
the game-theoretic framework of cooperative inverse reinforcement learning (CIRL).Using this analysis, we derive a Bellman backup that allows solving the dynamic
game through dynamic programming. At every instant, the backup rule is based on
a pragmatic-pedagogic equilibrium between the robot and the human: the robot is
uncertain about the objective and therefore incentivized to learn it from the human,
whereas the human has an incentive to help the robot infer the objective so that itcan become more helpful.
We note that this type of pragmatic-pedagogic equilibrium, recently studied in the
cognitive science literature for human teaching and learning [ 7], may not be unique in
general: there may exist two actions for Hand two corresponding interpretations for
Rleading to different ﬁxed points. For example, Hcould press a blue or a red button
which Rcould then interpret as asking it to pick up a blue or a red object. Although
we might feel that blue-blue/red-red is a more intuitive pairing, blue-red/red-blue is
valid as well: that is, if Hthinks that Rwill interpret pressing the blue button as
asking for the red object then she will certainly be incentivized to press blue whenshe wants red; and in this case R’s policy should consistently be to pick up the red
56 J. F. Fisac et al.
object upon H’s press of the blue button. When multiple conventions are possible,
human beings tend to naturally disambiguate between them, converging on salient
equilibria or “focal points” [ 11]. Accounting for this phenomenon is likely to be
instrumental for developing competent human-centered robots.
On the other hand, it is important to point out that, although they are compu-
tationally simpler than more general multi-agent planning problems, POMDPs arestill PSPACE-complete [ 12], so reducing pragmatic-pedagogic equilibrium compu-
tation to solving a modiﬁed POMDP falls short of rendering the problem tractable in
general. However, ﬁnding a POMDP-like Bellman backup does open the door to efﬁ-cient CIRL solution methods that leverage and beneﬁt from the extensive research on
practical algorithms for approximate planning in large POMDPs [ 13]. Some results
in this direction have recently been presented in [ 14].
We ﬁnd the results in this work promising for two reasons. First, they provide
insight into how CIRL games can be not only theoretically formulated but also
practically solved. Second, they demonstrate, for the ﬁrst time, formal solutions tovalue alignment that depart from the ideal assumption of a rational human agent and
instead beneﬁt from modern studies of human cognition. We predict that developing
efﬁcient solution approaches and incorporating more realistic human models will
constitute important and fruitful research directions for value alignment.
Acknowledgements This work is supported by ONR under the Embedded Humans MURI
(N00014-13-1-0341), by AFOSR under Implicit Communication (16RT0676), and by the Centerfor Human-Compatible AI.
References
1. Amodei, D., Steinhardt, J., Man, D., Christiano, P.: Concrete problems in AI safety (2017)2. Hadﬁeld-Menell, D., Dragan, A., Abbeel, P., Russell, S.: Cooperative inverse reinforcement
learning. NIPS (2016)
3. Tversky, A., Kahneman, D.: Judgment under uncertainty: heuristics and biases. Science
185(4157) (1974)
4. Heider, F., Simmel, M.: An experimental study of apparent behavior. Am. J. Psychol. 57(2)
(1944)
5. Meltzoff, A.N.: Understanding the intentions of others: re-enactment of intended acts by 18-
month-old children. Dev. Psychol. 31(5) (1995)
6. Baker, C.L., Tenenbaum, J.B.: Modeling human plan recognition using Bayesian theory of
mind. Plan Act. Intent Recognit. (2014)
7. Shafto, P., Goodman, N.D., Grifﬁths, T.L.: A rational account of pedagogical reasoning: teach-
ing by, and learning from, examples. Cogn. Psychol. 71(2014)
8. Zamir, S.: Bayesian games: games with incomplete information. Computational Complexity:
Theory, Techniques, and Applications (2012)
9. Luce, R.D.: Individual Choice Behavior: A Theoretical Analysis. Wiley, New York (1959)
10. Dragan, A.D., Srinivasa, S.: Integrating human observer inferences into robot motion planning.
Auton. Robot. (2014)
11. Schelling, T.C.: The Strategy of Conﬂict. Harvard University Press, Harvard (1960)12. Mundhenk, M., Goldsmith, J., Lusena, C., Allender, E.: Complexity of ﬁnite-horizon Markov
decision process problems. J. ACM 47(4) (2000)
Pragmatic-Pedagogic Value Alignment 57
13. Silver, D., Veness, J.: Monte-Carlo planning in large POMDPs. NIPS (2010)
14. Malik, D., Palaniappan, M., Fisac, J.F., Hadﬁeld-Menell, D., Russell, S., Dragan, A. D.: An
efﬁcient, generalized Bellman update for cooperative inverse reinforcement learning. In: DyJ., Krause A. (eds.) Proceedings of the 35th International Conference on Machine Learning,vol.80, pp. 3394–3402. PMLR (2018). http://proceedings.mlr.press/v80/malik18a.html
The Economic Case for Cloud-Based
Computation for Robot Motion Planning
Jeffrey Ichnowski, Jan Prins and Ron Alterovitz
1 Introduction
Consider a robot that needs to solve very challenging motion planning problems
quickly. For high degree of freedom robots, a typical setup requires a capital expenseof thousands of dollars to purchase a high-end computer capable of computing timely
solutions. As an alternative, would you prefer gaining access to the latest compu-
tational hardware on demand, and for cents per task? That is the promise of cloud
computing for robots—a potential to lower costs and improve efﬁciency for a variety
of robotics applications.
Cloud computing has the potential to change the way we design, use, and pay
for robotic systems. Unlike traditional robots which are purchased upfront, cloud
computers are billed in units of usage time. Thus, when using cloud computing onecan and should approach solving problems in the most cost-effective way possible.
To illustrate, for $10 000 one could purchase a high-end computer, or one could get
117 647 h on a compute-optimized 1-core cloud computer, 3 267 h on an 36-core cloudcomputer, or 1 h of 117 647 cores.
1With an embarrassingly parallel algorithm such
as a sampling-based motion planner [ 1] and externalizing the robot’s computation
(e.g., [ 9]) to a cloud-based computer, one could dramatically reduce the time to solve
a complex task. New robotics algorithms that leverage this computing power may
1As of March 2018, Amazon offers 1-core servers at $0.085/h, and 36-core at $3.06/h.
J. Ichnowski ( B)·J. Prins ·R. Alterovitz
University of North Carolina, Chapel Hill, NC 27599-3175, USAe-mail: jefﬁ@cs.unc.edu
J. Prins
e-mail: prins@cs.unc.edu
R. Alterovitz
e-mail: ron@cs.unc.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_859
60 J. Ichnowski et al.
extend a robot’s service life and battery-based operation time, and reduce its initial
and operating costs.
The cloud is already changing the way we think about computing for robots, but its
full potential has not been tapped. To date, many data-centric, and pre-computation
approaches leverage the cloud [ 12]. What about solving complex tasks with near-term
deadlines by using the cloud to add computing power in response to the changingdemands of a problem? This will be particularly valuable for network-connected
robots that face challenging motion planning problems that involve high degree of
freedom systems, dense cluttered environments, learning complex task models, ormanaging high levels of uncertainty. In this paper, we present an economic motiva-
tion for, and the research challenges posed by, leveraging cloud-based computation
in online and interactive robotic algorithms. Bringing the beneﬁts of cloud-basedcomputing to robots poses multiple open research challenges, such as: how to cost-
effectively allocate computing, how to design algorithms around network bottle-
necks, and how to split computation between a robot and the cloud.
2 The Economic Potential of Robot Cloud Computing
The cloud changes the cost model of computing by shifting it from a capital expense
(CapEx) to an operational expense (OpEx). Typically, robots require a large upfront
CapEx, driven in part by the cost of the robot’s computer. Using the cloud makescomputing become an OpEx over the service life of the robot. With the right algo-
rithms and utilization, an increase in a robot’s OpEx will be offset by, not only a
reduced CapEx, but also an increased service life, increased battery-based operationtime, and a net improvement in operational efﬁciency.
Lower CapEx by extending a robot’s service . A robot’s service life may be
extended through the use of cloud computing. The service life starts at purchase
and ends when the robot’s utility decreases to the point it is removed from service.
Increasing the service life reduces the number of robots purchased over time, lead-ing to a reduced CapEx. Consider a home assistance robot that aids someone with
variety of daily tasks of living. Such a robot could gain additional functionality by
following a process similar to that of installing applications and updates to a smart-phone or tablet. In this scenario, the robot becomes obsolete and needs replacement
due to either physical component wear or due to advances in software exceeding the
capabilities of the robot’s computing hardware.
Historically, computing hardware has become obsolete much more quickly than
non-computing hardware (e.g., motors, sensors). Smartphones, as a proxy for a
robot’s computing platform, have a life expectancy in the range of 3–4.7 years [ 2,4].
Cars, as a proxy for a robot’s non-computing hardware, have an average age in the
US of 11.1 years [ 6]. The short service life of mobile computing devices is unsur-
prising when considering Moore’s Law, which observes an 18-month doubling incomputation power as measured by transistor count. At the end of a 4.7 year service
The Economic Case for Cloud-Based Computation for Robot Motion Planning 61
life, a robot will have almost 9 times less computing power than its replacement. At
the end of a car’s 11.1 years, a robot will have almost 170 times less. The computing
platform on a robot is ﬁxed, but cloud services offer computers that are routinelyupgraded.
Cheaper robots with longer battery life . Incorporating a reliance on cloud com-
puting into the physical design of a mobile robot will allow for cheaper robots withlonger battery-based operation time. The computing platform in a robot is necessarily
limited by economic factors, including price and, for mobile robots, physical size and
battery capacity. Embedding high-end CPUs and GPUs enables higher performance
computing, but comes at a cost of dramatically increased price and energy drain
for the robot. Higher energy drain either requires increased battery size and weight,or results in reduced battery-based operation time. If instead, a robot’s designers
look to lower-power computing platforms sufﬁcient to running baseline algorithms,
while ofﬂoading intensive computation tasks to the cloud, their robot design can offerdecreased battery size or allow for an increased battery-based operation time, all for
a reduced upfront cost.
Robots that learn from their environment and from humans are examples that
could naturally beneﬁt from such a cloud-enabled robot design [ 15]. Cloud-based
computation accelerates the learning of a model, while the robot need only use the
learned model with low-powered computation. Such a system could be used in robotsthat are deployed to unfamiliar environments and expected to adapt rapidly to them
as they operate. Novel cloud-based learning solutions [ 10], and low-powered fast
convolutional network processors [ 5] and FPGAs [ 14], are making this closer to
reality.
Improved operational efﬁciency . Cloud computation can not only reduce the initial
purchase costs of a robot, but it can also increase a robot’s operational efﬁciency,
potentially increasing associated revenue and reducing the need to purchase more
robots. Motion planning can be computationally intensive, whether attempting toﬁnd a feasible solution in a complex space, maximizing a task’s success rate in the
presence of uncertainty, or minimizing a motion’s cost (e.g., path length, time to com-
pletion, energy required). Asymptotically optimal [ 11] and near-optimal [ 13] motion
planners work to minimize a motion’s cost by converging towards optimality. They
converge faster when given more computing power or computational parallelism [ 7].
By leveraging parallelism of cloud computers for motion planning [ 3,8], robots can
complete tasks faster. When accelerating robot motions results in more revenue, the
OpEx associated with cloud computing could be justiﬁed by net improved proﬁts (see
Fig.1). When a ﬁxed number of tasks are required per unit time, faster completion
times means fewer robots are required, thus lowering CapEx.
62 J. Ichnowski et al.
(a) Robot picking from a shelf 35 40 45
 55 60 65motion time
(seconds)
tasks/hourtasks/hour
motion time
$260$280$300$320$340
 2 16 32 48 64 96 128$0$20maxincome/hour
cost/hour
Computing power (number of cores)revenue/hour
profit/hour
cost/hour
(b) Eﬃciency gained with cloud computers
Fig. 1 Example of maximizing proﬁt with cloud computing. A warehouse packs packages using
robots ( a). The robots avoid collision with an ever-changing inventory by using motion plan-
ning algorithms. In b, the warehouse wishes to maximize proﬁt/hour , which here is computed
as(revenue/hour )−(cost/hour ). Each task the robot completes results in revenue for the company,
thus more tasks/hour means more revenue/hour The robot uses cloud computation of an asymptot-
ically optimal motion planner to reduce motion time and thus pack more boxes per hour. When the
cost of adding more computing outweighs the gain in revenue due to higher quality motion plans,
proﬁt is maximized
3 Research Challenges
Cloud computing offers many potential beneﬁts, but realizing them presents several
open research problems. Cloud computing services offer the scaling of computing
power and a wide variety of conﬁgurations, from a single core virtualized on a server,
to all cores on a high-end multi-core computer, to arrays of GPUs, to networked
combinations of these. Motion planning algorithms that beneﬁt from parallelism
typically run with ﬁxed parallelism conﬁgured a priori. With cloud computing, the
amount of parallelism to allocate to a problem becomes a question of balancing
beneﬁt to the cost (instead of availability) of computing. Robots also must interact
with a changing world, and in order to respond to changes (e.g., to sense and avoid
collisions with obstacles) they must take into account the network latency (i.e., round-
trip time) and bandwidth limits. One option is mixing or splitting computing between
multiple sites: the robot’s onboard and the cloud-based computers, ideally gaining
the beneﬁts of each site’s strengths while avoiding the weaknesses. The research
challenges are thus: how to allocate parallelism afforded by cloud computing, how
to model the costs of it, and how to adapt algorithms to work around limitations of
the network.
Allocating computing resources . How does one best allocate parallel computing
power when implementing an algorithm on a robot? That is the question often left as
The Economic Case for Cloud-Based Computation for Robot Motion Planning 63
an engineering exercise to the reader of parallel computing research. The answer is
typically: allocate as much computing power as you have available—e.g., all cores
on a computer, all warps on a GPU, and/or all computers in a cluster. But with cloudcomputing, “as much computing power as you have available” is insufﬁcient as an
answer—since the amount of computing power available is typically beyond the
ﬁnancial limits of reasonability. This suggests the beneﬁt associated with the amountof computing power must be balanced against the costs associated with using that
computing power. For example, increasing the motion efﬁciency of a warehouse robot
will have an associated beneﬁt to the bottom-line of the company (money saved),so at what level of computing parallelism (money expended) does the company
maximize proﬁt? Thus the question of how to best allocate computing power depends
on modeling economic costs. And while the exact answer is still application speciﬁc,as researchers we can look to supplying the tools to make the decisions. Such tools
might include models of convergence rates and speedup associated with parallelism,
and determining minimum or expected computational requirements associated withtasks.
Network bottlenecks and deadlines . Robot algorithms that rely on cloud comput-
ing must consider and address the limitations imposed by the network. Advances in
networking technology may improve the latency and bandwidth to an extent, but com-
munication networks will always be slower than the interconnect between the robotand its onboard or co-located computer. This limit is fundamentally insurmountable,
since it is a direct result of the speed of light. As such, network limitations vary by
domain, and the challenges imposed by the network bottlenecks for robots in homeand warehouse environments signiﬁcantly differ from robots tasked with deep-sea
and space exploration. For the class of algorithms and scenarios in which the results
can be pre-computed, the network might not warrant concern. However, robots oper-ate in the real world, and they must be able to sense and respond quickly to changes
in the environment in order to avoid undesirable or harmful outcomes, especially
in safety-critical scenarios, such as warehouse robots operating in close proximity
to humans or with medical robots working with, or operating on, humans. To avoid
undesirable outcomes, we pose the research challenge by borrowing language fromthe real-time computing community, and considering computing tasks with hard
deadlines andsoft deadlines . For robotic tasks with hard deadlines (ones that cannot
be missed), how can we ensure that a robotic algorithm will meet the deadline (orat worst minimize the chance of missing the deadline)? For robotic tasks with soft
deadlines (ones for which a miss results in reduced beneﬁt or increased cost), how
can a robotic algorithm maximize the beneﬁt or minimize the cost of these tasks?More parallel processing can speed up computation to get ahead of the deadline,
but the network remains a bottleneck of fundamental importance to these research
challenges.
Splitting computation between multiple sites . Algorithms can potentially address
the resource allocation and network concerns by splitting computation betweenthe robot’s on-board computer, a co-located computer, and cloud-based comput-
ers. The robot’s computer has the lowest latency and highest bandwidth access to its
64 J. Ichnowski et al.
environment via its sensors and actuators. A cloud-based computer has relatively high
latency and low bandwidth. Depending on the scenario in which the robot operates,
some portion of the robot’s computation can be split between the different sites. As anexample, vision processing and motion tracking require a large amount of bandwidth
and low latency in order to react to changes in the environment—matching the char-
acteristics and (hopefully) capabilities of the robot’s onboard or co-located computer.On the other hand, an intensive pre-computation of a robot’s path through its envi-
ronment (barring dynamic obstacles) can be rapidly computed by high-performance
parallel computing in the cloud. As a general research challenge, can we design robotalgorithms that split portions of computation between multiple computing sites and
thus gain the beneﬁts of the cloud’s massive computing power while meeting the
demands of a problem that requires low-latency computation?
4 Conclusion
Cloud computing has a great potential to beneﬁt robot motion planning and related
computationally intensive problems in robotics. New algorithms that leverage cloud
computing can increase a robot’s service life and improve its operational efﬁciency.Incorporating cloud computing into a robot’s design can decrease initial costs while
reducing size and weight and extending battery-based operation time. These potential
gains motivate new open areas of research in cloud computing for robotics, includinghow to address the network bottlenecks, exploit the cloud’s computing parallelism,
model the costs of cloud-based robotics algorithms, and meet task deadlines. Finally,
while our examples focused on motion planning as a particularly computationallyintensive problem, we hope to inspire the robotics community to apply cloud com-
puting to entirely new aspects of research not considered here.
Acknowledgements We thank Jonathan Lynn for useful discussions regarding economic models.
This research was supported in part by the U.S. National Science Foundation (NSF) under awardsCCF-1533844 and IIS-1149965 and the U.S. National Institutes of Health (NIH) under awardR01EB024864.
References
1. Amato, N.M., Dale, L.K.: Probabilistic roadmap methods are embarrassingly parallel. In: Pro-
ceedings of the IEEE International Conference Robotics and Automation, pp. 688–694 (1999)
2. Apple: environment - answers. http://web .archive .org/web/20170612120817 ,https://www .
apple .com/environment/answers/ . Accessed 12 June 2017
3. Bekris, K., Shome, R., Krontiris, A., Dobson, A.: Cloud automation: precomputing roadmaps
for ﬂexible manipulation. IEEE Robot. Autom. 22(2), 41–50 (2015)
4. Ely, C.: The life expectancy of electronics (2014). https://www .cta.tech/News/Articles/2014/
September/The-Life-Expectancy-of-Electronics .aspc
The Economic Case for Cloud-Based Computation for Robot Motion Planning 65
5. Esser, S.K., Merolla, P.A., Arthur, J.V ., Cassidy, A.S., Appuswamy, R., Andreopoulos, A.,
Berg, D.J., McKinstry, J.L., Melano, T., Barch, D.R., et al.: Convolutional networks for fast,energy-efﬁcient neuromorphic computing. Proc. Natl. Acad. Sci. 11,441–11,446 (2016)
6. Ford, D.: As cars are kept longer, 200,000 is new 100,000. New York Times (2012). http://www .
nytimes .com/2012/03/18/automobiles/as-cars-are-kept-longer-200000-is-new-100000 .html
7. Ichnowski, J., Alterovitz, R.: Scalable multicore motion planning using lock-free concurrency.
IEEE Trans. Robot. 30(5), 1123–1136 (2014)
8. Ichnowski, J., Prins, J., Alterovitz, R.: Cloud-based motion plan computation for power-
constrained robots. In: Algorthmic Foundations of Robotics (Proceedings of the WAFR 2016).Springer (2016)
9. Inaba, M., Kagami, S., Kanehiro, F., Hoshino, Y ., Inoue, H.: A platform for robotics research
based on the remote-brained robot approach. Int. J. Robot. Res. 19(10), 933–954 (2000)
10. Jouppi, N.P., Young, C., Patil, N., Patterson, D., Agrawal, G., Bajwa, R., Bates, S., Bhatia, S.,
Boden, N., Borchers, A., et al.: In-datacenter performance analysis of a tensor processing unit.In: Proceedings of the 44th Annual International Symposium on Computer Architecture, pp.1–12. ACM (2017)
11. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J.
Robot. Res. 30(7), 846–894 (2011)
12. Kehoe, B., Patil, S., Abbeel, P., Goldberg, K.: A survey of research on cloud robotics and
automation. IEEE Trans. Autom. Sci. Eng. 12(2), 398–409 (2015)
13. Marble, J.D., Bekris, K.E.: Asymptotically near optimal planning with probabilistic roadmap
spanners. IEEE Trans. Robot. 29(2), 432–444 (2013)
14. Murray, S., Floyd-Jones, W., Qi, Y ., Sorin, D.J., Konidaris, G.: Robot motion planning on a
chip. In: Robotics: Science and Systems (2016)
15. Waibel, M., Beetz, M., Civera, J., d’Andrea, R., Elfring, J., Galvez-Lopez, D., Häussermann,
K., Janssen, R., Montiel, J., Perzylo, A., et al.: Roboearth. IEEE Robot. Autom. Mag. 18(2),
69–82 (2011)
Autonomous Agents in the Wild: Human
Interaction Challenges
Laura Major and Caroline Harriott
1 Introduction
Autonomous agents are moving out of research labs and entering our everyday lives.
Completing complex tasks within the conﬁnes of industrial applications has bene-ﬁted from the addition of autonomous systems for decades. For example, commercial
airline cockpits began incorporating collision detection and avoidance systems start-
ing in 1986 [ 1]. The human factors research area was developed in order to study
and enable humans to control complex machines and safely incorporate autonomous
capabilities into industrial operations.
Now we are seeing autonomous agents enter the cars we drive, the sidewalks we
walk on, and our homes. Developing autonomous systems for commercial products
comes with new challenges, such as responding to dynamic environments and operat-ing in the existing world (e.g., processing dynamic construction information during
road work and adapting accordingly). Additionally, commercial products involve
users from the general population, as opposed to specialized professionals, whichintroduces a broad knowledge level. For example, operators of self-driving cars will
range from teenagers to responsible professionals. This concept of understanding
autonomous systems “in the wild” has been discussed before (e.g., [ 2,3]), but this
paper relates our burgeoning challenges with demonstrated strategies for improve-
ment.
There are many beneﬁts to introducing autonomy into commercial products.
Automating repetitive tasks has shown to increase accuracy and safety. For example,
comparisons of outcomes of laparoscopic surgery and use of the Da Vinci surgical
robot indicated faster patient recovery time and shorter hospital stays, due to the
L. Major ·C. Harriott ( B)
Charles Stark Draper Laboratory, Inc, 555 Technology Sq, Cambridge, MA 02139, USA
e-mail: charriott@draper.com
L. Major
e-mail: lmajor@draper.com
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_967
68 L. Major and C. Harriott
capacity to operate in much tighter spaces than a human hand [ 4]. Additionally,
advanced driver assistance systems (ADAS) have increased safety by automating
speciﬁc parts of the driving task, such as braking for collision avoidance [ 5].
However, there are also known behavioral pitfalls that occur when people interact
with highly autonomous agents [ 6], such as distraction [ 7], and automation bias
[8]. Managing a user’s concurrent tasks [ 9], creating adaptive automation [ 10], and
introducing training strategies [ 11] have demonstrated mitigations for these pitfalls,
but everyday interaction will include untrained users and observers who will not know
the capabilities and limitations of the system. Introducing automation can providegreat beneﬁts, but as we see the presence of autonomous agents expanding, we
must account for the unexpected interaction effects between people and autonomous
agents.
2 Decomposition of Application Space
This paper presents a framework created to explore the impact the expansion of
autonomous agents into commercial products will have as adoption increases. As
presented earlier, autonomous capabilities have long been in use in industrial appli-cations, such as automated ﬂight controls in commercial aircraft. Autonomous capa-
bilities have been growing in the commercial space as well, with examples including
intelligent agents (e.g., Amazon Echo) and robotic vacuums (e.g., iRobot Roomba).We are now seeing a growth of products in between the required-use systems in indus-
trial applications, and low-risk commercial systems, designed to make every-day life
easier. This new area of growth includes domains such as self-driving vehicles anddelivery robots. Because these domains have commercial applications, training time
is minimal; however, because these devices participate in safety-critical activities
such as transportation, there is a bigger risk.
Two primary factors that characterize the applications are the nature of the opera-
tions, which we quantify in terms of risk and user expertise. Risk is calculated as the
cost of failure and expertise is calculated as the amount of training required. Figure 1
presents sixteen example systems. Cost of failure is estimated by summing the sys-
tem cost, lost time, and potential injury or death as valued by insurance companies.
Operator expertise is calculated by estimating the number of hours taken to trainbefore use (e.g., time to become a doctor before using the Da Vinci system, time to
connect an Amazon Echo for the ﬁrst time).
Commercial products typically sit in the lower left area of the graph presented in
Fig. 1, seen in the points represented by blue diamonds. The cost of a failure during
use of one of these systems is low; the risk usually is the cost of the device or a minorinjury to the single user of the system. Industrial applications, shown with an orange
square, typically have systems that have a high cost of failure and require thousands
of hours of training. For example, a commercial ﬂight requires the expertise of apilot and co-pilot. The recent expansion of autonomous agents into new applications
Autonomous Agents in the Wild: Human Interaction Challenges 69
(seen in the green triangles in Fig. 1) comes with a higher cost of failure. The users
of these systems can cause injury to self or others.
3 Autonomous Agents Merging with People
Now that autonomous agents are interacting directly with the general population,
the design needs of these commercial products are merging with mission-criticalapplications. Commercial products have traditionally focused on user experience
and preference, optimizing the design around what people like, while industrial
applications focus on designing to optimize total system performance because of thehigh cost of a failure. Peoples’ preferences often conﬂict with the design that leads to
the best performance [ 17]. Thus, with increasing autonomous capabilities entering
into commercial products, it is no longer enough to focus design efforts on delightingusers—we must also design for optimal performance to mitigate safety risks.
3.1 Interaction with Autonomous Agents
Most interaction with autonomous agents involves supervising actions that areautomatically conducted by the autonomous agent. This is referred to as human-supervisory control, where a closed loop system is processing sensory inputs, deter-
mining actions and executing those actions. In nominal conditions, the person only
monitors these activities. The challenge arises when something unexpected happensand the user must override the autonomous agent and retake some form of control.
Well-studied challenges with this type of interaction include:
Vigilance decrement: signiﬁcant decline in detection performance for rare event
tasks [ 18].
Mode confusion: inability to maintain adequate awareness of the behavior of the
automation and determine correct interventions in a timely manner [ 19].
Cognitive skill degradation: the reduction in retention of cognitive skills due to
the use of automation [ 20]. When automation fails or enters into a state it can no
longer use and a person must resume control, effective control of the system will
be reduced if the person’s skills have degraded.
We have seen the catastrophic effect of these challenges in aviation. One example
is the Air France 447 accident in 2009. The aircraft was at a cruising altitude and
the autopilot was in control. Then ice crystals formed on the pitot tubes, causingthe airspeed measurements to be invalid; thus, autopilot disconnected. The pilot was
confused about the mode of the automation and ultimately could not recover control
of the aircraft. The accident investigation determined the pilot was at fault and it wasdeemed that there was an over-dependence on automated ﬂight control.
70 L. Major and C. Harriott

Autonomous Agents in the Wild: Human Interaction Challenges 71
/triangleleftsldFig. 1 Framework presenting commercial products (blue diamonds) and required-use systems
(orange squares) in comparison to emerging commercial applications for autonomous agents in the
wild (green triangles) by cost of failure and operator expertise. (1Nuclear engineer expertise is esti-
mated to include bachelor’s degree and ﬁve years’ work experience (~13,000 h). Three Mile Islandaccident represents a benchmark for cost of failure of a nuclear power plant accident’s cleanup at$973 M [ 12].
2Commercial aircraft pilot expertise is estimated using 22,000 h. Costs are estimated
based on American Airlines Flight 191 crash in 1979 (deadliest in U.S. history), estimated to havecost ~$400 M, inﬂated to 2017 [ 13].
3NASA estimates the space shuttle costs $450 M per launch [ 14].
Operator expertise is estimated using time to receive a Master’s degree in an engineering discipline.4
Use of Intuitive Surgical’s Da Vinci surgical robot requires a medical degree and a training course(described at https://www.intuitivesurgical.com/training/ , accessed 31 May 2017). Cost of failure is
estimated using the cost of a lawsuit (e.g., Michelle Zarick’s $30 M award in 2016).
5The cost of fail-
ure of a self-driving car is estimated using average cost of an accident on U.S. roads [ 15]. Operator
training time is estimated using time to receive a driver’s license (45 h).6Medication delivery robot
operator expertise is estimated for setup and training time for a hospital employee at approximately2–3 workdays. Cost of failure is estimated at $650 M, the cost of a payout by CVS to the state of NewJersey after distributing incorrect medication (Chiesa, J.S., Warhaftig, J.L. (2013). Assurance ofV oluntary Compliance, State of New Jersey and CVS).
7Personal Drone expertise estimated using
length of training videos available for a Phantom 2 on Lynda.com (approximately 8 h). Cost offailure estimated using potential for head injury and previous lawsuits (approximately $500,000 forcar accident involving head injury http://www.alllaw.com/articles/nolo/personal-injury/settlement-
value-head-injury-claim.html ).
8Operator expertise for a ground delivery robot based on one work-
day (6–8 h). Cost of failure based on potential for a bystander to fall after colliding with a groundrobot (e.g., Savioke Botlr Hotel Robot). As an example of a hotel fall suit, Carol Wilson of Denverwas awarded over $240,000 for a slip and fall in 2010.
9Operator expertise for an ofﬁce telepresence
robot is estimated to take 15 min to learn basic functions, an d2ht og a i ne x p e r t i s et h r ough trial
and error. Cost of failure estimated using the potential to injure a bystander (e.g., leg injury seen
in “Little Chubby” incident in China in 2016, http://www.hufﬁngtonpost.com/entry/robot-attack-
china\_us\_5833e645e4b058ce7aacc025 ) and potential to sue for a leg injury (e.g., $85,000 estimated
recovery cost for broken leg by Miller & Zois personal injury lawyers - https://www.millerandzois.
com/value-leg-broken-fractured.html ).10Operator expertise for a smartphone is estimated using
training time for an Apple iPhone available on Lynda.com. Cost of failure calculated using cost ofdevice ($769 for an iPhone 7 Plus, as of 31 May 2017).
11Operator expertise for a robotic vacuum
is estimated for setup time and basic troubleshooting, calculated using available training videos forthe iRobot Roomba on www.irobot.com . Cost of failure calculated as cost of device ($899.99 for
Roomba 980, as of 31 May 2017).
12A smart home appliance (e.g., Nest thermostat) takes approxi-
mately 1.5 h to set up and learn, as estimated by 30 min setup time ( https://nest.com/support/article/
How-to-install-your-Nest-Learning-Thermostat ) and 1 h of trial and error and online tutorials. Cost
of failure estimated by the cost of device ($249.99 for Nest Learning Thermostat as of 31 May2017).
13A digital single reﬂex lens camera may take around 20 h to become an expert, as estimated
by the length of training courses available on Lynda.com. The cost of failure is estimated using thecost of the device (approximately $400 for a Canon EOS Rebel camera).
14An intelligent assistant
(e.g., Amazon Echo) requires very little operator expertise, estimated using 1 h of setup time andtrial and error. The cost of failure is also low and is estimated by the cost of the device ($179.99 foran Echo as of 31 May 2017).
15Time to become an expert is presented as 10,000 h. [ 16]16Loss of
human life is estimated using the military death gratuity of $100,000 ( http://militarypay.defense.
gov/Beneﬁts/Death-Gratuity/ ).)
72 L. Major and C. Harriott
We are now seeing a similar pattern in self-driving cars. The ﬁrst death occurred
with a car in autopilot mode and the driver was found at fault. While in autopilot, a
tractor-trailer turned in front of the car. The camera system did not detect the tractor-trailer because of the lack of contrast between the white truck and the bright sky and
the car crashed into the tractor-trailer at full speed. NHTSA found the driver at fault
because “autopilot requires full driver attention at all times.” However, research intovigilance decrement has shown that peoples’ performance in detecting rare event
occurrences, such as a failure in the camera detection system, is poor.
In commercial products, there may be a user experience beneﬁt achieved by
removing the user from the control loop, such as the autopilot mode in self-driving
cars. But an unexpected consequence of this is that the user is unable to detect when
they need to resume control and do so effectively within the time frame needed.
3.2 Negotiation with an Autonomous Agent
As autonomous agents enter our everyday lives we now see the negotiation of people
with agents that are not under their control. For example, Google’s ﬁrst accident
blamed on one of its self-driving cars occurred in 2016 [ 21]. The self-driving car
stopped in the right line because of an obstruction and then attempted to merge into
the left lane. There was a city bus approaching from the rear. The driving behavior of
the bus was interpreted as the bus yielding to the self-driving car, however the intentof the bus driver was not to yield, and the car struck the bus.
The challenge of negotiation between autonomous agents and people has been
studied for decades in industrial applications. An example is in the developmentof the Trafﬁc Collision and Avoidance System (TCAS) [ 22]. TCAS was developed
to reduce the incidence of mid-air collisions by detecting when aircraft are on a
collision course, warn the pilots and provide instructions on a maneuver to preventthe collision. In 2002 a mid-air collision occurred over Germany when one pilot
followed the instructions TCAS provided (descend to avoid the collision) and the
other pilot followed the instructions provided by the air trafﬁc controller provided(descend to avoid the collision). The two aircraft descended into each other.
Following the accident new procedures were created to prioritize the instruc-
tions provided by TCAS over the air trafﬁc controller. As we begin to experienceautonomous agents moving around us in our everyday lives, there is not the equiv-
alent of an air trafﬁc controller and we will need to create the means for effective
negotiations to happen across the agents and the people with whom they come into
contact.
Autonomous Agents in the Wild: Human Interaction Challenges 73
4 Conclusion
The introduction of autonomous agents into transportation and delivery systems into
widely used commercial products has the potential to alleviate many of the errors andaccidents we see with human controlled systems. However, new human-automation
interaction challenges are emerging and will continue to do so unless capabilities are
developed to extend the mitigations developed in industrial applications to untrainedusers.
References
1. Seipel, J.D.: Airwothiness approval of trafﬁc alert and collision avoidance systems (TCAS II),
V ersions 7.0 & 7.1 and associated Mode S Transponders. Advisory Circluar from the U.S.Department of Transportation FAA. AC No: 20-151B (2014)
2. Sabanovic, S., Michalowski, M. P ., Simmons, R.: Robots in the wild: Observing human-robot
social interaction outside the lab. In: 9th IEEE International Workshop on Advanced MotionControl, 2006, pp. 596–601. IEEE (2006)
3. Sung, J., Christensen, H.I., Grinter, R.E.: Robots in the wild: understanding long-term use.
In: Proceedings of the 4th ACM/IEEE International Conference on Human Robot Interaction,pp. 45–52. ACM (2009)
4. Maeso, S., Reza, M., Mayol, J.A., Blasco, J.A., Guerra, M., Andradas, E., Plana, M.N.: Efﬁcacy
of the Da Vinci surgical system in abdominal surgery compared with that of laparoscopy. Ann.Surg. 252(2), 254–262 (2010)
5. Wada, T., Sonoda, K., Tada, S.: Simultaneous achievement of supporting human drivers and
improving driving skills by shared and cooperative control. IFAC-PapersOnLine 49(19), 90–95
(2016)
6. Parasuraman, R., Riley, V .: Humans and automation: use, misuse, disuse, abuse. Hum. Factors:
J. Hum. Factors Erg. Soc. 39(2), 230–253 (1997)
7. Blanco, M., Atwood, J., V asquez, H.M., Trimble, T.E., Fitchett, V .L., Radlbeck, J., Morgan,
J.F.: Human factors evaluation of level 2 and level 3 automated driving concepts. (No. DOTHS 812 182) (2015)
8. Cummings, M.L.: Automation bias in intelligent time critical decision support systems. In:
AIAA 1st Intelligent Systems Technical Conference , vol. 2, pp. 557–562 (2004)
9. Horberry, T., Anderson, J., Regan, M.A., Triggs, T.J., Brown, J.: Driver distraction: The effects
of concurrent in-vehicle tasks, road environment complexity and age on driving performance.Accid. Anal. Prev. 38(1), 185–191 (2006)
10. Miller, C.A.: Trust in adaptive automation: the role of etiquette in tuning trust via analogic
and affective methods. In: Proceedings of the 1st International Conference On AugmentedCognition, pp. 22–27 (2005)
11. Bahner, J.E., Hüper, A.D., Manzey, D.: Misuse of automated decision aids: Complacency,
automation bias and the impact of training experience. Int. J. Hum Comput Stud. 66(9), 688–
699 (2008)
12. Pedraza, J.M.: World major nuclear accidents and their negative impact in the environment,
human health and public opinion. Int. J. Energy Environ. Econ. 21(2), 139 (2013)
13. Chalk, A.: Market forces and aircraft safety: The case of the DC-10. Econ. Inq. 24(1), 43–60
(1986)
14. Grinter, K.: Kennedy Space Center FAQ Accessed 31 May 2017 at https://science.ksc.nasa.
gov/pao/faq/faqanswers.htm (2000)
74 L. Major and C. Harriott
15. Blincoe L, Miller T.R., Zaloshnja, E., Lawrence, B.A.: The Economic and Societal Impact of
Motor V ehicle Crashes, 2010 (Revised). Washington, DC: NHTSA. DOT HS 812 013 (2015)
16. Gladwell, M.: Outliers: The Story of Success. Hachette UK (2008)17. Bailey, R.W.: Performance vs. preference. In: Proceedings of the HFES Annual Meeting, vol.
37, Issue 4, pp. 282–286 (1993)
18. Gier, R.A., Warm, J.S., Dember, W.N., Matthews G, Galinsky, T.L., Parasuramon, R.: The Vig-
ilance Decrement Reﬂects Limitations in Effortful Attention, No Mindlessness. Hum. Factors45(3), 349–339 (2003)
19. Rodriguez, M., Zimmerman, M., Katahira, M., de Villepi, M., Ingram, B., Leveson, N.: Iden-
tifying mode confusion potential in software design. In: The 19th Digital Avionics SystemsConference (2000)
20. V olz, K., Euijung, Y ., Dudley, R., Lynch, E., Dropps, M., Dorneich, M.C. (2016). An Evaluation
of Cognitive Skill Degradation in Information Automation, Human Factors (V ol. 60, Issue 1)
21. Davies, A.: Google’s Self-driving car caused its ﬁrst crash. Wired (2016). https://www.wired.
com/2016/02/googles-self-driving-car-may-caused-ﬁrst-crash/
22. Williamson, T., Spencer, N.A.: Development and operation of the trafﬁc alert and collision
avoidance system (TCAS). Proc. IEEE 77(11), 1735–1744 (1989)
How Should a Robot Assess Risk?
T owards an Axiomatic Theory of Riskin Robotics
Anirudha Majumdar and Marco Pavone
1 Introduction
Safe planning and decision making under uncertainty are widely regarded as central
challenges in enabling robots to successfully operate in real-world environments.By far the most common conceptual framework for addressing these challenges is
to assign costs to stochastic outcomes and then to use the expected value of the
resulting cost distribution as a quantity that “summarizes” the value of a decision.
Such a quantity can then be optimized, or bounded within a constrained formulation.
However, in settings where risk has to be accounted for, this choice is rarely welljustiﬁed beyond the mathematical convenience it affords. For example, imagine a
safety-critical application such as autonomous driving; would a passenger riding
in an autonomous car be happy to do so if she was told that the average behaviorof the car is not to crash? While one can introduce some degree of risk sensitivity
(i.e., sensitivity to the tails of the cost distribution) in the expected cost framework
by simply shaping the cost function, this can quickly turn into an exercise in “costfunction hacking”. Unless one is careful about the way one shapes the cost function,
this can lead to the robot behaving in an irrational manner [ 1]. The common alternative
approach, aimed at promoting risk sensitivity, is to consider a worst-case assessment
of the distribution of stochastic outcomes. In practice, however, such an assessment
Anirudha Majumdar—Work performed as a postdoctoral scholar at Stanford University.
A. Majumdar ( B)
Department of Mechanical and Aerospace Engineering, Princeton University, Princeton, NJ08544, USAe-mail: ani.majumdar@princeton.edu
M. Pavone
Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305, USAe-mail: pavone@stanford.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_1075
76 A. Majumdar and M. Pavone
can often be quite conservative: an autonomous car whose goal is to never crash
would never leave the garage.
The expected value operator and the worst-case assessment are examples of risk
metrics . Informally, a risk metric is a mapping from a random variable corresponding
to costs to a real number. The expected cost corresponds to risk neutrality while the
worst-case assessment corresponds to extreme risk aversion. For practical applica-tions, we would like to explore risk metrics that lie in between these extremes. This
raises the following question: is there a class of risk metrics that lie between these
extremes while still ensuring that the robot quantiﬁes risk (and hence safety) in arational and trustworthy manner? This question is central to the problem of decision
making under uncertainty since the choice of a risk metric is one that must be made in
anyframework that assigns costs to outcomes. Yet, despite the role of safe decision
making under uncertainty as a core theme in practically all areas of robotics, this
question has received very little attention within the robotics community. As a result,
there is arguably no ﬁrm theoretical foundation for making an informed decisionabout what risk metric to use for a given robotics application.
The goal of this paper is to provide a ﬁrst step towards such a principled framework.
More precisely, we describe axioms (properties) that a risk metric employed by a
robot should satisfy in order to be considered sensible. To our knowledge, this is the
ﬁrst attempt to provide such an axiomatic framework for evaluating risk in roboticsapplications. Our effort is inspired by a similar effort in the ﬁnance community that
led to the identiﬁcation of coherent risk metrics [2,13] as a class of risk metrics
that have desirable properties for assessing the risk associated with a ﬁnancial asset(e.g., a portfolio of stocks). The inﬂuence that these ideas have had can be gauged
by the fact that in 2014 the Basel Committee on Banking Supervision changed its
guidelines for banks to replace the Value at Risk (VaR) (a non- coherent risk metric)
with the Conditional Value at Risk (CVaR) (a coherent risk metric) for assessing
market risk [ 11]. We refer the reader to the extended version of our paper [ 8]f o ra
more thorough discussion of approaches for quantifying risk in ﬁnance, along withrelated approaches in other ﬁelds.
We believe that the question of what properties a risk metric should satisfy in
robotics applications is a fundamental one: a robot’s inability to assess risks in arational way could lead to behavior that is harmful both to itself and humans or
other autonomous agents around it. Our hope is that the ideas presented in this paper
can help the community converge upon a set of properties that risk metrics mustsatisfy in order for the robot’s decision-making system to be considered rational
and trustworthy, paralleling a similar effort in the ﬁnancial industry. Indeed, it is
conceivable that in the not-so-distant future, robots such as autonomous cars or
unmanned aerial vehicles (UA Vs) deployed in safety-critical scenarios will be subject
to regulatory frameworks that mandate the use of “ofﬁcially-approved” risk metrics.
Outline : The outline of this paper is as follows. Section 2formally introduces the
notion of a risk metric (Sect. 2.1) and proposes an interpretation of risk in robotics
applications (Sect. 2.2). In Sect. 3we advocate axioms that risk metrics in robotics
applications should satisfy in order to be considered sensible (Sects. 3.1), provide
How Should a Robot Assess Risk? Towards an Axiomatic Theory … 77
examples of metrics that satisfy them, and discuss pitfalls stemming from using risk
metrics that do not satisfy these axioms (Sect. 3.2). Section 4concludes the paper
with a number of points for discussion and directions for future research.
2 Assessing Risk: Preliminaries
In this section, we formally introduce risk metrics and propose an intuitive interpre-tation of risk quantiﬁcation in robotics contexts. This interpretation will form the
basis for the axioms we advocate in Sect. 3.
2.1 Risk Metrics
We denote the set of possible outcomes that may occur when a robot operates in
uncertain settings as Ω. In order to avoid heavy use of measure theoretic notions, we
takeΩto be ﬁnite. Denote by Pa probability mass function that assigns probabilities
P(ω)to outcomes ω∈Ω. Consider a cost function Z:Ω→ Rthat assigns costs
Z(ω)to outcomes. The cost Zis then a random variable, namely the cost random
variable. Let Zdenote the set of all random variables on Ω.Arisk metric is a mapping
ρ:Z→ R, i.e., a risk metric maps a cost random variable to a real number.
2.2 Interpretation of Risk in Robotics Applications
Imagine a ﬁctional government agency known as the Robot Certiﬁcation Agency
(RCA) that is responsible for certifying if a given robot is safe to operate in the real
world. How should the RCA quantify the risk faced by this robot? As an example,
consider an autonomous car driving from one city to another. While performingthis task, the robot will incur random costs Z(e.g., due to fuel consumption, time,
crashes, mechanical wear and tear, etc.). In order to provide clear interpretations of
the axioms for risk metrics discussed below, we will take the following axiom as akey starting point.
A0. Monetary costs . The costs Zare expressed in monetary terms.
This axiom ensures that the costs assigned to outcomes have a tangible and inter-
pretable value, which will be instrumental in deﬁning a meaningful notion of riskbelow. Such an axiom may also provide a handle on reasoning about insurance poli-
cies for safety-critical robots (e.g., autonomous cars). We note that our starting point
contrasts with one where one considers a more abstract and subjective notion of cost(e.g., quadratic state and control costs for Linear Quadratic Regulator problems).
78 A. Majumdar and M. Pavone
Given this assumption, suppose that the RCA demands that the robot’s owner must
deposit an amount of money ρ(Z)before the robot is deployed such that the RCA
is satisﬁed that the owner will be able to cover the potential costs incurred duringoperations (e.g., making repairs to the robot due to an accident) with the amount
ρ. We deﬁne the amount ρ(Z)as the perceived risk from operating the robot. The
particular risk metric ρ:Z→ Rthe RCA uses will depend on its attitude towards
risk and may depend on the application under consideration. For example, the RCA
may ask for a deposit ρ(Z)=E[Z]if it is risk neutral. If it wants to be highly
conservative, the RCA may demand a deposit equal to the worst-case cost outcome.The question we will pursue in Sect. 3is the following: what properties must ρsatisfy
in order for it to be considered sensible?
We note that we are using the RCA here as a pedagogical tool to provide an
interpretation of risk in robotics applications and to motivate the axioms described
in Sect. 3. In reality, the robot’s decision-making system will assess risks and make
decisions based on those assessments.
3 An Axiomatization of Risk Metrics for Robotics
Applications
3.1 Axioms and Their Interpretations
We now parallel results in ﬁnance [ 3, Chap. 4] [ 13] and propose six axioms for risk
metrics. Speciﬁcally, we make the case that these axioms should be fulﬁlled by any
risk metric used in a robotics application in order for it to be considered a sensible
assessment of risk. For each axiom, we ﬁrst provide a formal statement and then an
intuitive interpretation based on the interpretation of risk from Sect. 2.2.
A1. Monotonicity .L e t Z,Z/prime∈Zbe two cost random variables. Suppose Z(ω)≤
Z/prime(ω)for all ω∈Ω. Then ρ(Z)≤ρ(Z/prime).
Interpretation : If a random cost Z/primeis guaranteed to be greater than or equal to a
random cost Z no matter what random outcome occurs , then Z/primemust be deemed
at least as risky as Z. One can think of the random costs as corresponding to two
different robots, or the same robot performing different tasks, or executing different
controllers. Given our interpretation of risk, this axiom states that the RCA must
demand at least as large a deposit for covering costs for the robot (or task) cor-responding to Z
/primeas for the robot (or task) corresponding to Z. This is a sensible
requirement since we are guaranteed to incur at least as high a cost in the second
scenario as the ﬁrst no matter which outcome ω∈Ωis realized.
A2. Translation invariance .L e t Z∈Zandc∈R. Then ρ(Z+c)=ρ(Z)+c.
Interpretation : If one is charged a deterministic cost c(in addition to random costs
incurred when the robot is operated), then the RCA should demand that this amount
cbe set aside in addition to money for covering the other costs from operating the
How Should a Robot Assess Risk? Towards an Axiomatic Theory … 79
robot. Note that this axiom also implies that ρ(Z−ρ(Z))=0. Thus, ρ(Z)is the
smallest amount that must be deducted from the costs to make the task risk-free.
A3. Positive homogeneity .L e t Z∈Zandβ≥0 be a scalar. Then ρ(βZ)=βρ(Z).
Interpretation : If all the costs incurred by the robot (regardless of the random out-
come) are scaled by β, the RCA demands that the deposit is scaled commensurately.
This is reasonable since this corresponds to simply changing the units of cost (recallthat we assumed that the costs are expressed in monetary terms).
A4. Subadditivity .L e t Z,Z
/prime∈Z. Then ρ(Z+Z/prime)≤ρ(Z)+ρ(Z/prime).
Interpretation : This axiom encourages diversiﬁcation of risk. For example, imagine
a system with two robots. Suppose that ZandZ/primeare costs incurred by Robot 1 and
Robot 2 respectively. The left-hand side (LHS) of the inequality corresponds to thedeposit that the RCA demands when both robots are run simultaneously, while the
right-hand side (RHS) corresponds to the sum of the deposits when the robots are
deployed separately. Axiom A4 then states that deploying both robots simultaneouslyis at most as risky as deploying them separately.
This captures the intuition that one robot acts as a hedge against the other robot
failing (i.e., if one of the robot fails in some way, then the other will make up for this
loss). Another interpretation is that this axiom promotes redundancy in the system.
The exact interpretation of the LHS and RHS of the inequality in A4 will dependon the particular application under consideration. For example, the two sub-costs Z
andZ
/primemay correspond to two separate sub-tasks that the robot must perform. In this
case, the LHS corresponds to the robot performing both tasks simultaneously whilethe RHS corresponds to performing them independently. A4 encodes the intuition
that performing both tasks simultaneously is less risky since one sub-task can act as
a hedge against the other.
We note that A3 and A4 together imply convexity:
ρ(λZ+(1−λ)Z
/prime)≤λρ(Z)+(1−λ)ρ( Z/prime),for all λ∈[0,1].
A5. Comonotone additivity . Suppose Zand Z/primeare comonotone, i.e., (Z(ω)−
Z(ω/prime))(Z/prime(ω)−Z/prime(ω/prime))≥0,∀(ω,ω/prime)∈Ω×Ω. Then ρ(Z+Z/prime)=ρ(Z)+
ρ(Z/prime).
Interpretation : This axiom supplements A4. In particular, A5 states that if two costs
rise and fall together, then there is no beneﬁt from diversifying (e.g., if one robot
always performs poorly at a task when the other does or when a robot performs
poorly at a sub-task when it also performs poorly at another one).
A6. Law invariance .I fZandZ/primeare identically distributed, then ρ(Z)=ρ(Z/prime).
Interpretation : If two tasks have the same distribution of costs, then the RCA
demands an equal deposit in both cases. For example, suppose Ω={ω,ω/prime}with
both outcomes having probability 0.5. Further, suppose Z(ω)=1,Z(ω/prime)=10,
Z/prime(ω)=10,Z/prime(ω/prime)=1. The two situations must be considered equally risky even
though the assignment of costs to events is different.
80 A. Majumdar and M. Pavone
Taken together, Axioms A1–A6 capture a fairly exhaustive set of essential prop-
erties that we believe any reasonable quantiﬁcation of risk in robotics should obey
given our interpretation of risk in Sect. 2.2. A hypothetical RCA that quantiﬁes risk
in a manner that is consistent with these axioms would be considered a sensible
one. Moreover, robots that assess risks according to risk metrics that fail to satisfy
some of these axioms can behave in a manner that would be considered extremelyunreasonable and arguably very unsafe, as illustrated in Sect. 3.2. We thus advocate
risk metrics that satisfy Axioms A1–A6 for use in robotics applications. We note
that risk metrics satisfying Axioms A1–A6 have also been studied in the context ofportfolio optimization in ﬁnance and are known as distortion risk metrics [3,14].
These metrics enjoy an elegant characterization in terms of the CVaR metric. We
refer the reader to [ 8] for a discussion of this characterization.
We note, however, that the axioms highlighted here only form a starting point for
a complete axiomatization of risk metrics in robotics applications. For example, the
sequential nature of many decision-making tasks in robotics gives rise to additional
important considerations beyond the ones discussed here. We refer the reader to [ 8]
for a discussion of these additional axioms.
3.2 Examples and Pitfalls of Commonly Used Risk Metrics
In this section, we ﬁrst discuss examples of existing risk metrics that fulﬁll Axioms
A1–A6. We then discuss commonly used risk metrics that do not fulﬁll some of these
axioms, along with pitfalls stemming from their use.
An important risk metric that satisﬁes Axioms A1–A6 is the Conditional Value
at Risk (CVaR) [ 12]. The CVaR αfor a random cost Zat level αis deﬁned as:
CVaR α(Z):=1
α/integraldisplay1
1−αVaR 1−τ(Z)dτ, (1)
where VaR α(Z)is the Value at Risk (VaR) at level α, i.e., simply the (1−α)-quantile
of the cost random variable Z:
VaRα(Z):=min{z|P[Z>z]≤α}. (2)
Intuitively, CVaR αis the expected value of Zin the conditional distribution of Z’s
upper (1−α)-tail. It can thus be interpreted as a risk metric that quantiﬁes “how
bad is bad.” We note that the expected cost and worst-case assessment also satisfyA1–A6. Figure 1provides a visualization of the expected cost, worst case, VaR, and
CVaR. Axioms A1–A6 thus deﬁne a broad class of risk metrics that capture a wide
spectrum of risk assessments from risk-neutral to worst-case.
However, there are many examples of popular risk metrics that do not fulﬁll the
axioms we advocate. For example, a very popular metric to quantify risk in robotics
applications is the mean-variance risk metric: E[Z]+βVariance [Z](see, e.g.,
How Should a Robot Assess Risk? Towards an Axiomatic Theory … 81
Fig. 1 An illustration of
four important risk metrics,namely: expected cost, worst
case, Value at Risk (VaR),
and Conditional Value atRisk (CVaR)
[4,6,10]). The mean-variance metric satisﬁes A6 but fails to satisfy the other axioms.
This can lead to a robot that utilizes the mean-variance metric making decisions that
would be considered unreasonable. Consider the setup in Table 1(based on [ 7]),
where ω1,ω2,ω3,ω4are disturbance outcomes, and Zand Z/primeare the costs result-
ing from executing two different controllers πandπ/prime. Which controller should the
robot execute? Controller πresults in lower costs no matter what the disturbance
outcome is and hence should be preferred by any sensible decision maker. However,
computing the mean-variance risk metric with β=1, we see that:
E[Z]+Variance [Z]=3.75>E[Z/prime]+Variance [Z/prime]=3.4375.
The robot would hence strictly prefer π/prime. This unreasonable behavior is a result of
the mean-variance risk metric failing to satisfy Axiom A1 (monotonicity).
We note that other commonly used risk metrics in robotics also suffer from similar
issues. We refer the reader to [ 8] for a discussion of the pitfalls of using the Value at
Risk (VaR) (deﬁned in Eq. ( 2)), which fails to satisfy the subadditivity axiom (A4).
Table 2lists the axioms satisﬁed by popular risk metrics in the literature (we refer
the reader to [ 13] for deﬁnitions). We note that the standard semi-deviation is widely
used in ﬁnance [ 13], while the entropic risk metric has been popular in control theory
for risk-averse control [ 5,15].
To summarize our discussion, based on our arguments in this section, we advocate
the use of risk metrics satisfying Axioms A1–A6 (i.e., distortion risk metrics) in
robotics applications. This is in contrast to popular risk metrics used in the robotics
literature (e.g., mean-variance, or VaR).
Ta b l e 1 Issues with the mean-variance risk metric. Any rational agent would choose controller π
(with associated costs Z) since it always results in lower costs. But, using the mean-variance risk
metric results in choosing π/prime(with associated costs Z/prime)
Outcome ω1 ω2 ω3 ω4
Probability 0.25 0.25 0.25 0.25
Z 1 2 3 4
Z/prime2 2 3 4
82 A. Majumdar and M. Pavone
Ta b l e 2 Axioms satisﬁed by
popular risk metricsAxioms
Conditional Value at Risk(CVaR) A1–A6
Expected Cost A1–A6
Worst case A1–A6
Mean–Variance A6
Entropic risk A1, A2, A6
Value at Risk (VaR) A1–A3, A5, A6
Standard semi-deviation A1–A4, A6
4 Discussion and Conclusions
Our goal in this paper has been to provide preliminary directions towards an axiomatic
theory of risk for robotics applications. We have advocated properties that risk metrics
employed by robots should satisfy in order for them to be considered sensible. Theseaxioms deﬁne a class of risk metrics, known as distortion risk metrics, which have
been previously used in ﬁnance. We end with some questions that highlight areas for
future thought.
Discussion 1 (Axioms A1–A6 ) The axioms of monotonicity (A1), translation invari-
ance (A2), positive homogeneity (A3), and law invariance (A6) should arguably be
applicable in any robotics application. Subadditivity (A4) and comonotone addi-
tivity (A5) are also intuitively appealing, particularly for applications that involvesome degree of high-level decision making (since a high-level decision making sys-
tem should ideally diversify risks). However, the interpretation of diversiﬁcation is
somewhat unclear in certain applications. For example, imagine a humanoid robotwhose goal is to minimize a cost function that is a sum of two components ZandZ
/prime,
where Zpenalizes one aspect of the robot’s motion (e.g., deviations of the robot’s
torso from the vertical orientation) while Z/primepenalizes another aspect (e.g., deviation
of the robot’s gaze from a target). For such low-level control tasks, the interpretation
ofρ(Z)+ρ(Z/prime)is not entirely clear since it is not possible to perform the different
subtasks corresponding to Zand Z/primeindependently of each other. The following is
thus a question for discussion and future work: should A4 and A5 be abandoned (or
replaced by other axioms) for such tasks?
Discussion 2 (Further axioms ) While we have highlighted a number of axioms
that we believe are particularly important, the identiﬁcation of other axioms is an
important direction for future work. These may depend on the particular domain ofapplication. Moreover, for certain applications it may not be necessary to impose
all the axioms described here. For example, A4 and A5 (concerning diversiﬁcation
of risks) will generally be relevant to high-level decision making tasks where it ispossible to diversify risks and may not be relevant for low-level control tasks where
diversiﬁcation may not be possible.
How Should a Robot Assess Risk? Towards an Axiomatic Theory … 83
Discussion 3 (Choosing a particular risk metric ) For a given application, we may
wish to choose a particular risk metric from the class of metrics described here. How
should such a metric be chosen? One possibility is to learn a distortion risk metricthat explains how humans evaluate risk in the given application domain and then
employ the learned risk metric. We describe ﬁrst steps towards this in [ 9], where we
have introduced a framework for risk-sensitive inverse reinforcement learning for
learning humans’ risk preferences from the class of coherent risk metrics.
Discussion 4 (Legal frameworks ) The question of safety for AI systems has received
signiﬁcant attention recently (see, e.g., [ 1] for a recent review). An important compo-
nent of this discussion has been the consideration of legal frameworks and guidelines
that must be placed on AI systems to ensure that they do not pose a threat to our
safety. Such considerations for robots such as unmanned aerial vehicles are alreadyextremely pressing for government agencies such as the Federal Aviation Admin-
istration (FAA). It is not difﬁcult to imagine a future where the Robot Certiﬁcation
Agency (RCA) is in fact a real entity that certiﬁes the safety of new robotic systems.How can we effectively engage lawmakers and government ofﬁcials in discussions
on how to evaluate risks in robotic applications?
Our hope is that the ideas presented in this paper will spur further work on this
topic and eventually lead to a convergence upon a particular class of risk metrics that
form the standard for assessing risk in robotics.
Acknowledgements The authors were partially supported by the Ofﬁce of Naval Research, Science
of Autonomy Program, under Contract N00014-15-1-2673.
References
1. Amodei, D., Olah, C., Steinhardt, J., Christiano, P., Schulman, J., Mané, D.: Concrete problems
in AI safety (2016). arXiv:1606.06565
2. Artzner, P., Delbaen, F., Eber, J.-M., Heath, D.: Coherent measures of risk. Math. Financ. 9(3),
203–228 (1999)
3. Föllmer, H., Schied, A.: Stochastic Finance: An Introduction in Discrete Time. Walter de
Gruyter (2011)
4. García, J., Fernández, F.: A comprehensive survey on safe reinforcement learning. J. Mach.
Learn. Res. 16(1), 1437–1480 (2015)
5. Glover, K., Doyle, J.C.: Relations between H ∞and risk sensitive controllers. Analysis and
Optimization of Systems. Springer, Berlin (1987)
6. Kuindersma, S., Grupen, R., Barto, A.: Variable risk control via stochastic optimization. Int. J.
Robot. Res. 32(7), 806–825 (2013)
7. Maccheroni, F., Marinacci, M., Rustichini, A., Taboga, M.: Portfolio selection with monotone
mean-variance preferences. Math. Finan. 19(3), 487–521 (2009)
8. Majumdar, A., Pavone, M.: How should a robot assess risk? Towards an axiomatic theory of
risk in robotics. Extended version of ISRR 2017 paper (2017). arXiv:1710.11040
9. Majumdar, A., Singh, S., Mandlekar, A., Pavone, M.: Risk-sensitive inverse reinforcement
learning via coherent risk models. Robotics Science and Systems (2017)
10. Mannor, S., Tsitsiklis, J.N.: Mean-variance optimization in Markov decision processes. In:
International Conference on Machine Learning (2011)
84 A. Majumdar and M. Pavone
11. B.C. on Banking Supervision: Fundamental Review of the Trading Book: A Revised Market
Risk Framework. Bank for International Settlements (2014)
12. Rockafellar, R.T., Uryasev, S.: Optimization of conditional value-at-risk. J. Risk 2, 21–41
(2000)
13. Shapiro, A., Dentcheva, D., Ruszczy´ nski, A.: Lectures on stochastic programming: Modeling
and theory. SIAM, 2nd edn. (2014)
14. Wang, S.: A class of distortion operators for pricing ﬁnancial and insurance risks. J. Risk Insur.
67(1), 15–36 (2000)
15. Whittle, P.: Risk-sensitive linear/quadratic/gaussian control. Adv. Appl. Probab. 13(4), 764–
777 (1981)
Human-Assisted Humanoid Robot
Control
Jaeheung Park, Yisoo Lee, Mingon Kim, Soonwook Hwang, Jaesug Jung
and Junhyung Kim
1 Introduction
Recently, humanoid robots have been actively developed for various purposes, such
as disaster response [ 1,2] and manufacturing [ 3,4]. Although there can be many
aspects in the development of humanoid robots, locomotion and manipulation are
the most important aspects in terms of functionality. Especially, biped locomotion,
walking, can provide the mobility of the robot over various terrains and spaces
compared to other types of mobility.
Humanoid robots are now capable of walking very well in certain situations due
to very active research [ 5]. However, there are still many cases and terrains that the
robot may fail to traverse over. It is believed that this limited mobility prevents the
humanoid robot from being used in more general cases. For example, humanoidrobots are always being hanged from a crane until they operate. This is due to the
fact that we cannot trust the walking and standing capability, in addition to the power
consumption when they are standing. From these observations, we speculate that the
J. Park (B)·Y. L e e ·M. Kim ·S. Hwang ·J. Jung ·J. Kim
Department of Transdisciplinary Studies, Graduate School of Convergence Science andTechnology, Seoul National University, Seoul, Republic of Koreae-mail: park73@snu.ac.kr
Y. L e e
e-mail: howcan11@snu.ac.kr
M. Kim
e-mail: mingonkim@snu.ac.kr
S. Hwang
e-mail: jbs4104@snu.ac.kr
J. Jung
e-mail: jjs916@snu.ac.kr
J. Kim
e-mail: john3.16@snu.ac.kr
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_1185
86 J. Park et al.
perfect locomotion capability of humanoid robots can be achieved in quite a long
time scale. Until then, the locomotion capability may be supported or assisted by
human, so that the humanoid robot can provide useful services in many situations.This is especially true for the cases when the robot can move and perform tasks very
well in certain places but it is a little bit dangerous or difﬁcult to move from one place
to other service places. In between the places, there can be stairs, uncertain terrains,and cluttered obstacles. If we do not have to use a crane to move the robot to pass
through these speciﬁc small areas, the robot can be more useful and very convenient
to be utilized. Therefore, there can be more demands of humanoids. As an example,when the robot is asked for some performance on a stage, moving onto the stage is
always one of the difﬁcult things that we need to consider. Or, we can put the robot
on a passenger seat to move from one site to another site.
Mobility and locomotion of humanoid robots have similar forms to humans. In
addition to walking, there are motions such as sitting on a chair, getting in and out
of a car, and climbing stairs. In these scenarios, it is believed that the robot can bephysically assisted by human for much better stability. In the case of robot assistance
to human such as wearable or exoskeleton robots, there are two major problems [ 6].
The ﬁrst one is understanding the intention of human, and the second is the limited
capability of robots. However, if human is to assist a robot, human can be much
better in these two aspects. The intention of the robot can be expected directly fromthe control program. Also, human can be very much capable of assisting the robot
as long as it is within physical limits.
In this paper, we propose to think about the robot control that can utilize human
physical assistance in humanoid robot control, especially involving mobility as shown
in Fig. 1. In Sect. 2, recent work is summarized in relation to this topic. In Sect. 3,
possible strategies are described for implementing robot control that can be assistedby human. Finally, the paper is concluded in Sect. 4.
Fig. 1 Examples of human physical assistance for a humanoid robot to awalk and bsit down on
ac h a i r
Human-Assisted Humanoid Robot Control 87
2 Related Work
In a broad sense, the proposed concept of human-assisted motion control may belong
to Human-Robot Interaction (HRI). Therefore, in this chapter, a few recent works inphysical HRI are introduced in relation to the possible use in human-assisted robot
control. Recent studies in HRI have mainly focused on two issues: stability in phys-
ical contact between human and robot, and algorithm to improve the human-robotinteraction. Firstly, intervention of humans to operating robots may cause an accident
such as physical collision. For this reason, safety in physical HRI has been studied
in [7,8]. Secondly, for the performance improvement of HRI, human intention is
recognized through exterior perception [ 9,10] or probabilistically predicted using
Gaussian process [ 11,12]. Machine learning has also been exploited in prepared sce-
narios to increase the effectiveness of interaction [ 11,13]. Furthermore, appropriate
task allocation based on task planning has been veriﬁed to increase the efﬁciency of
cooperation between human and robot [ 14,15].
However, since motions of humanoid robots are very complex, a comprehensive
approach is necessary. That is, starting from the simple approach that human helps the
robot while running existing controllers, the effectiveness of HRI should be enhancedby implementing cooperation-based task planning and estimation of human intention.
Finally, further enhancement of performance and stability of the robot mobility can
be achieved by the active HRI based on the capability of robots by understanding thesituation and request for the assist from human.
3 Control Strategies for Human-Assisted Robot Mobility
The robot walking motion in lateral plane can take advantage of human assistance.In most of the cases, the Linear Inverted Pendulum Model (LIPM) [ 16]i su s e dt o
plan Center of Mass (CoM) motion. The CoM in the lateral plane has a frequent
change in velocity because of its constant stop and moving motion. The robot can
easily lose balance in this plane because of relatively short foot-width comparedto its foot length when there is disturbance either from ground or by external force.
Human assistance may prevent it from losing balance in this case. The stability of the
robot may be improved by simple human external force against its falling direction,without any modiﬁcation on the controller. This is especially true when the robot
controller is simple enough for human to expect its motion to help the robot, such as
an open-loop motion execution based on a pattern.
However, if the robot executes a planned motion without recognizing or utilizing
the physical assistance from human, the robot controller and human assistance may
conﬂict and the cooperation may not be most efﬁcient. One way to improve on thisis to create an agreed or expected pattern between the robot and human. Human
can assist the robot whenever the assistance is required in this planned pattern. For
88 J. Park et al.
example, an agreed walking motion is to have least motion in the lateral plane. Then,
human can hold the robot whenever the robot happens to move in the lateral direction
so that the lateral motion of the robot does not occur. Another example is to use humanforce as a means to extend supporting polygon area. The robot may plan CoM motion
with an expectation to have support on one hand on the other side of supporting foot.
Human should support the robot as the expected pattern. The motion of the robotshould be planned with the assumption of the human support but within a certain
range of magnitude and direction of force.
This kind of motion plan with arranged patterns between human and robot will
have high performance only in certain situations. Therefore, if we want to utilize
human assistance in more various situations, the interaction force from human to
robot can be estimated for better motion planning of the robot. For example, theexternal force on CoM can be estimated using the measured Zero Moment Point
(ZMP) and CoM state. Real-time motion planning of CoM can be implemented by
using the estimated external force on LIPM model. In consequence, CoM motion inlateral plane decreases as human applies more force in the lateral direction, and vice
versa. Therefore, human may assist the robot better when the robot can estimate and
utilize the human force as accurately as possible.
The human assisted robot control can perform very well if the programmed pat-
tern for the robot and human is very well understood by human. However, thismay be possible only for programmers themselves, or very well trained opera-
tors. For general use, it could be very helpful if the robot can recognize its status
and ask assistance to human when needed. That is, the robot can realize its cur-rent incapability of executing a certain task or motion, and create a certain signal
to human such as gesture. For example, during walking, it can be assessed if the
robot can stabilize itself from the kinematic and dynamic information of the robotsuch as position and velocity of CoM, contact forces and moments, and foot posi-
tion and orientation [ 17]. In this case, the robot can express itself to ask physical
assistance in certain ways. Another example is to stand up from a chair [ 18]. The
robot can assess the possibility of standing up by itself using the relative position
between CoM and feet, and contact state of the feet. If it cannot execute to stand
up by itself, the robot can extend its arms as a gesture for asking pulling forces.This kind of interaction may require high level of physical human robot interaction
technologies.
Compliant motion control can be an option to provide robots with the capability
of working or moving together with human. The robot can be programmed to be
compliant to human force, or to be guided to move to human force, so that it can
move as human desires. For example, when a robot gets into a car, it will be quite
difﬁcult to have balance and the motion to get in the car. Human can assist some
parts of the body for balance, while some of the parts are guided by human force incompliant motion. This kind of motion capability can be implemented using existing
compliant motion control technologies with or without tactile feedback. In this case,
again, human intention or the expected range of assisted force can be important forrobot programming.
Human-Assisted Humanoid Robot Control 89
4 Conclusion
In this paper, we have considered the possibility of human assisted robot control,
especially for humanoid mobility. This may greatly help overcome the current limi-tations of humanoid mobility to increase the usability of humanoid robots in wider
applications. Considered scenarios in this paper included walking, getting in a car,
and standing up from a chair. In this paper, we have thought about possible strategiesto assist humanoid robots by human. Simple physical support of human may help the
stability of the robot even without any change on existing controllers. However, it is
believed that it can be more efﬁciently achieved if we can develop certain technolo-gies such as recognition of human intention, and the controller that can account for
the help from human. Compliant motion control can also be very useful for guided
motion of humanoid robots for certain situations. This kind of humanoid control
strategy with the help of human assistance will be useful before the humanoid robot
can fully autonomously navigate by itself, or even after.
References
1. Negrello, F., Garabini, M., Catalano, M.G., Malzahn, J., Caldwell, D.G., Bicchi, A.,
Tsagarakis, N.G.: A modular compliant actuator for emerging high performance and fall-resilient humanoids. In: 2015 IEEE-RAS 15th International Conference on Humanoid Robots
(Humanoids), pp. 414–420 (2015)
2. Pratt, G., Manzo, J.: The DARPA robotics challenge [competitions]. IEEE Robot. Autom. Mag.
20(2), 10–12 (2013)
3. COMANOID project. http://comanoid.cnrs.fr (2017). Accessed 25 June 2017
4. Space robotics challenge. https://www.nasa.gov/feature/space-robotics-challenge (2017).
Accessed 23 June 2017
5. Koolen, T., Bertrand, S., Thomas, G., De Boer, T., Wu, T., Smith, J., Englsberger, J., Pratt, J.:
Design of a momentum-based control framework and application to the humanoid robot Atlas.Int. J. Humanoid Robot. 13(01), 1650007 (2016)
6. Viteckova, S., Kutilek, P ., Jirina, M.: Wearable lower limb robotics: a review. Biocybern.
Biomed. Eng. 33(2), 96–105 (2013)
7. Lee, H., Hogan, N.: Essential considerations for design and control of human-interactive robots.
In: IEEE International Conference on Robotics and Automation (ICRA), vol. 2016, pp. 3069–3074 (2016)
8. Navarro, B., Cherubini, A., Fonte, A., Passama, R., Poisson, G., Fraisse, P .: An ISO10218-
compliant adaptive damping controller for safe physical human-robot interaction. In: IEEEInternational Conference on Robotics and Automation (ICRA), vol. 2016, pp. 3043–3048(2016)
9. Holtzen, S., Zhao, Y ., Gao, T., Tenenbaum, J.B., Zhu, S.-C.: Inferring human intent from video
by sampling hierarchical plans. In: IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), vol. 2016, pp. 1489–1496 (2016)
10. Hu, N., Bestick, A., Englebienne, G., Bajscy, R., Kröse, B.: Human intent forecasting using
intrinsic kinematic constraints. In: IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), vol. 2016, pp. 787–793 (2016)
11. Medina, J.R., Endo, S., Hirche, S.: Impedance-based gaussian processes for predicting human
behavior during physical interaction. In: IEEE International Conference on Robotics andAutomation (ICRA), vol. 2016, pp. 3055–3061 (2016)
90 J. Park et al.
12. Pellegrinelli, S., Admoni, H., Javdani, S., Srinivasa, S.: Human-robot shared workspace col-
laboration via hindsight optimization. In: IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), vol. 2016, pp. 831–838 (2016)
13. Berger, E., V ogt, D., Haji-Ghassemi, N., Jung, B., Amor, H.B.: Inferring guidance informa-
tion in cooperative human-robot tasks. In: 2013 13th IEEE-RAS International Conference onHumanoid Robots (Humanoids), pp. 124–129 (2013)
14. Gams, A., Ude, A.: On-line coaching of robots through visual and physical interaction: Analysis
of effectiveness of human-robot interaction strategies. In: IEEE International Conference onRobotics and Automation (ICRA), vol. 2016, pp. 3028–3034 (2016)
15. Johannsmeier, L., Haddadin, S.: A hierarchical human-robot interaction-planning framework
for task allocation in collaborative industrial assembly processes. IEEE Robot. Autom. Lett.2(1), 41–48 (2017)
16. Kajita, S., Morisawa, M., Miura, K., Nakaoka, S., Harada, K., Kaneko, K., Kanehiro, F., Y okoi,
K.: Biped walking stabilization based on linear inverted pendulum tracking. In: IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS), vol. 2010, pp. 4489–4496(2010)
17. Hof, A., Gazendam, M., Sinke, W.: The condition for dynamic stability. J. Biomech. 38(1),
1–8 (2005)
18. Padois, V ., Ivaldi, S., Babiˇ c, J., Mistry, M., Peters, J., Nori, F.: Whole-body multi-contact
motion in humans and humanoids: advances of the CoDyCo european project. Robot. Auton.Syst. 90, 97–117 (2017)
“Should Robots Feel Pain?”—Towards
a Computational Theory of Painin Autonomous Systems
Trevor Richardson, Indranil Sur and Heni Ben Amor
1 Introduction
Pain plays a central role in our lives and is of paramount importance to many brain
and body mechanisms such as cognition, social interaction, motor control, memory,learning, autonomy and, most importantly, self-preservation. It acts as a critical sig-
nal, which guides our decision-making processes and shapes the choices we make.
A long-standing theory, as articulated by Descartes [ 1], describes pain as bodily per-
turbations that are detected by nerve ﬁbers and communicated to the brain. Cartesian
theory limits the role of pain to the sensation of bodily harm, failing to acknowledgethe many other functions involved in complex biological pain systems. More recent
scientiﬁc evidence suggests that pain is generated through a complex interplay of a
variety of signals and predictions involving multiple areas of the brain [ 2–4].
Despite its central role in many functions of the human brain, to date, pain has
attracted relatively little interest in the robotics community. Pain and its relation-
ship to robotics, however, has not been completely overlooked [ 5,6]. Researchers
have attempted to formalize pain for robotics and one promising result has been the
development of an “artiﬁcial Robot Nervous System” that can react to multi-modal
stimuli much like a biological organism’s pain-reﬂex [ 7]. This system is similar in
nature to the nociceptive Cartesian view and does not encompass the various other
roles and interactions of complex biological pain systems. In contrast to the Carte-
sian approach, Sur and Ben Amor have shown that perturbations and their causes can
T. Richardson ( B)·H. B. Amor
Arizona State University, Tempe 85281, AZ, USAe-mail: trevor.richardson@asu.edu
H. B. Amor
e-mail: hbenamor@asu.edu
I. Sur
SRI International, Princeton 08540, NJ, USAe-mail: indranil.sur@sri.com
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_1291
92 T. Richardson et al.
be learned and anticipated [ 6]. Pain is also prominently featured in reinforcement
learning (RL) algorithms [ 8] as a negative reward. RL algorithms, however, require
a human expert to specify how this negative reward is calculated, typically resultingin extremely task-speciﬁc deﬁnitions of pain.
In this paper, we argue in favor of a general, computational theory of pain
in autonomous systems that goes beyond the simple Cartesian model typicallyemployed today. Translating the concept of pain into a bio-inspired computational
framework allows for resilient machines and systems that can learn to anticipate and
avoid harmful sensations, with a concomitant increase in longevity and autonomy.We will ﬁrst discuss the evolutionary origin of pain, as well as the complex web of
underlying mechanisms and functions of biological pain systems. Next, we discuss
the opportunities and challenges that arise from studying computational frameworksthat mimic nociceptive pathways. Further, we propose two benchmark tasks that
can be leveraged to accelerate such research. Our main objectives are to highlight a
critical knowledge gap in our understanding of intelligent, physical systems and, toidentify a new, promising avenue for further research by the robotics community.
2 The Evolutionary Role of Pain
Pain is a sensation that many species experience. It is not unique to humans and hasbeen observed in vertebrates as well as invertebrates such as cephalopods. Pain isa dominant neurobiological process that is essential to the survival of our species;
its inﬂuence is felt in almost all functional areas of the human brain [ 9]. The central
nervous system (CNS) generates pain signals that inﬂuence our behavior and guideour learning within the contexts of self-preservation and reproduction.
The CNS has proven to be evolutionarily advantageous, having arisen as a result
of natural selection. Accordingly, pain pathways are heritable traits that promotethe ﬁtness of biological populations. Individuals with congenital insensitivity to pain
frequently die at a relatively young age due to tissue damage, infections, or both [ 10].
Dawkins [ 11] offers a persuasive thought experiment to illustrate the importance of
pain’s role in evolution. He asks his audience to consider the potential ﬁtness of a
gazelle population with genes that cause analgesic states when ﬂeeing predators. He
concludes that this gene pool of gazelles would not be favored by natural selectionunless tranquilizing a gazelle evading predation improved that gazelle’s probability
of reproducing [ 11]. He asserts that one must infer that gazelles experience extreme
agony before death, because this system promotes self-preservation and the likeli-
hood of reproduction. His thought experiment reinforces the principle that pain is a
product of natural selection.
In addition to its evolutionary advantage, pain functions as a guiding signal by
which a biological organism learns to navigate its environment safely, interact with
living beings and inanimate objects, and promote its own well-being. It is there-fore central to the behaviors learned and exhibited by a species within a lifetime.
According to Craig [ 12], pain is not only a sensation, but also a motivation that is
“Should Robots Feel Pain?”—Towards a Computational … 93
rooted in an emotional drive that results in homeostatic behavior. Not only does pain
impose evolutionarily signiﬁcant inﬂuences on behavior, it also serves as an educa-
tional feedback signal. The use and maintenance of a CNS of sufﬁcient complexityto experience pain requires the expenditure of considerable energy. Such a CNS
would be wasteful unless it served an evolutionarily advantageous purpose. Current
research indicates that most insects do not experience pain [ 13]. A prevalent scien-
tiﬁc theory suggests that this is evidence that pain is more advantageous to organisms
with longer life spans because learning complex relationships is more important to
organisms that live longer. Pain is beneﬁcial to complex learning within the scopeof self-preservation. Even more important may be the relationship between pain and
emotional learning. Apkarian found that the representation of acute pain is related
to the areas of the brain primarily responsible for emotional learning, memory andreward/addictive behavior [ 14]. Scientiﬁc evidence indicates that as we navigate our
lives, pain consistently inﬂuences our behavior; it plays a central role in our ability
to safely learn complex relationships while engaging with our environment [ 2].
3 Learning, Empathy, Memory and Fear
The neurobiological processes that produce pain signiﬁcantly inﬂuence other human
and animal functions. In particular, our ability to learn, empathize, remember and
fear are all mechanisms that are affected by pain pathways in the central nervoussystem [ 3,4,15–19]. With regard to learning, this impact is realized in two distinct
ways. First, fear-based conditioning leads to associative and avoidance learning [ 16,
20–23]. Second, pain impacts learning through the bidirectional relationship between
the formation of an individual’s motivations and the pain that is experienced when
those motivational goals are pursued [ 16,20–23]. Contemporary research has shown
that personal experiences of pain are altered based on an individual’s motivations andconditioned fear-based associations, as well as social factors that are unique to that
individual [ 14,22,23]. Because the impact of experiencing pain is bidirectional, it
functions as a fear-based conditioner. Fear-based conditioning that results from paindirects an organism’s motivations and, as a result, affects how that organism learns
[20–22] and how quickly it learns [ 3].
Another function that bears a close relationship to pain is empathy . There is a
considerable overlap of brain activation between individuals experiencing pain and
those experiencing empathy [ 4,17,19]. Research has shown that the brain’s signa-
ture for empathy overlaps speciﬁcally with the brain’s signature for pain in areas that
are associated with pain’s affective as opposed to sensory qualities [ 4]. Some results
suggest that empathy is not exclusively a human emotive state, but instead is onethat also exists in other living organisms such as rats [ 17]. The attentional resource
needs of pain systems are considerable and parts of the brain outside the pain matrix
can be altered by CNS pain networks. For example, an individual’s memories canbe altered by a painful experience [ 15,18,24–26]. In many cases, experiencing
intense pain results in an enhanced ability to accurately recall a memory or to recall
94 T. Richardson et al.
the emotions experienced during the painful event [ 15,18,26]. In contrast, due to
pain’s attentional requirements, painful experiences can also limit an individual’s
capability to remember information about his or her environment, especially whenthe information is not directly related to the cause of the painful experience [ 15]. In
one study, subjects were found to remember their emotions with high accuracy after
experiencing intense, acute pain. The same subjects, however, were much less accu-rate in recalling an unrelated stimulus present during the painful event [ 15]. Pain’s
functional role for an organism impacts that organism’s ability to learn, empathize,
experience, fear and remember.
4 Pain Maps to Robotics and the Beneﬁt of Robotic Pain
Systems
The development of robotic systems with the capacity to perceive pain would further
the dream of creating a fully autonomous robot that could safely explore dangerous
environments. With advances in hardware and software, the aspects of pain described
in Sects. 2and 3have the potential to be realized in robotic systems. Using biology
as inspiration, robotic software and hardware can incorporate systems that, to a
considerable extent, mirror the functionality, complexity and interplay of biological
pain systems. Such developments would provide immense beneﬁts to functioningrobots.
Figure 1displays speciﬁc interrelated and essential properties of pain-based sys-
tems. Hardware and software research has the potential to map these properties to therobotic domain. The construction of systems that improve a robot’s self-awareness
and promote self-preservation have far reaching applications. Such systems would
beneﬁt robotics in general by enhancing various learning capabilities, reducing theﬁnancial cost of replacing robotic parts and ensuring the longevity of robotic sys-
tems as a whole. Self-preservation systems for robots have the potential to map
to empathy, enhanced learning, augmented memory and emotional fear-based con-ditioning. A robotic system that learns to associate certain scenes with negative
internal states has the potential to understand the behaviors of other robots and
Fig. 1 Critical functions that
are interconnected to pain
“Should Robots Feel Pain?”—Towards a Computational … 95
organisms in the same environment. Empathy involves the comprehension of another
organism’s internal states. If a robot can predict a potentially precarious situation fac-
ing another robot, then not only can that robot learn from the other robot’s experience,but also potentially provide assistance to the endangered robot. A robot’s ability to
understand another damaged robot’s behavior promotes the well being of both the
observing and damaged robots. A robot that understands negative internal harm andevents that may cause harm also has the potential to help protect human beings and
other organisms in dangerous situations. It is infeasible to preprogram all possible
noxious states. In many experiments, robots only learn how to interact with their envi-ronment over a small period of time within ﬁxed boundaries. In the future, robots
will need to learn how to accomplish signiﬁcantly more complex tasks that require
them to reason about and interact with their environment over much longer periodsof time and in multiple and varying locations. One could imagine giving a robot the
tasks of shopping at a grocery store that it has never been to before and then cooking
a meal for you and your family. These tasks present many perilous possibilities forthe robot as it interacts with new situations. Learning how to accomplish these tasks
and overcome the obstacles it will face (such as navigating new doorways in high
trafﬁc areas, transporting the required ingredients and overcoming food preparation
dangers) can best be addressed through a system that promotes learning that involves
concerns about internal safety and sustainability. Such a system would need to beable to reevaluate its own goals and motivations based on new data coming from
nociceptive pain systems. Any learned information about noxious experiences with
the environment would need to be stored, reused and, most importantly, generalizedto new contexts.
It is crucial that intelligent, autonomous robots with ﬁnite capacity for storing
memories retain the most relevant and important experiences. A fully integratedpain system can assist in grading the relative importance of speciﬁc memories. For
a fully autonomous robot to be realized, these types of Bayesian conditioning asso-
ciations must be learned in real time. A pain-based robotics system would provideimportant feedback that would enable probabilistic conditioning. Nociceptive feed-
back would provide valuable insight regarding whether a given action or state would
be beneﬁcial or harmful to the robot. In general, robotic pain systems will promoterobotic autonomy, system lifespan and robotic altruism—a robot’s ability to assist
other robots, humans and other living organisms.
5 Experimental Frameworks to Test Robotic Pain Systems
The creation of robotic pain systems requires experimental platforms that enable
scientists to collaborate in this promising new area of research. We propose two
scenarios to encourage research on robot pain perception—the Rotating Bar task and
the Dodge Ball task. These scenarios allow the exploration of fundamental questionsregarding computational theories of pain.
96 T. Richardson et al.
Fig. 2 Rotating bar
benchmark
Rotating Bar : In the Rotating Bar task displayed in Fig. 2, a ﬁxed-position robot
must learn how to extend its arms and end effectors, while at the same time avoiding
damage from a rotating bar. In this scenario, the robot uses an RGB vision camera to
observe its environment. The robot can query about its own internal states such as theposition and orientation of its end effectors and arms. Parameters that can be varied
include the angular velocity of the bar, angular acceleration of the bar, size of the bar,
length of the robotic arms, and complexity of the task assigned. The robot’s position,however, is ﬁxed. A proper pain solution requires the robot to rapidly learn to avoid
painful negative noxious states and to balance these nociceptive stimuli against its
desire to complete its assigned task. The robot must learn to balance its goals andmotivations with its own well-being. Note that any solution should not include any
hard-coded reward function, e.g., if(arm\_torque >thresh) pain =1.
This scenario tests the bi-directionality of competing interests and avoidance learning
and provides the ability to analyze various nociceptive software models. The robot
must learn to understand where negative feedback is occurring and how to respondto possible harm such as a damaged arm or end effector.
Dodge Ball : In the Dodge Ball scenario, see Fig. 3, N balls with Gaussian dis-
tributed initial positions are sent into projectile motion with randomly distributedinitial velocities in the x, y, z planes toward a robot that can move along one dimen-
sion only. The robot is conﬁned to a limited space. It accesses information about
its environment using an RGB camera. It also can obtain information about its owninternal state such as its velocity, orientation, and position. If the robot chooses not
“Should Robots Feel Pain?”—Towards a Computational … 97
Fig. 3 Dodge ball scenario
to move, or moves randomly, it will eventually be hit by some of the projectiles.
Movement guided by intelligent anticipation is necessary to minimize the number ofcollisions with incoming projectiles. The robot needs to learn from experience what
visual information is predictive of impending harm. This platform provides multiple
parameters of interest, such as the rate and speed at which balls are ﬁred, the distancefrom the robot to the balls, and the damage incurred to the robot from each collision.
The collisions provided by this simulation offer a way to test and build software
that learns to predict negative future internal states. The robot must prioritize somenoxious states over others. In these situations, the robot will need to endure some
painful stimuli in order to develop an effective long term self-preservation strategy.
The robot needs to reason about future states in order to maximize its longevity byincurring minimal damage over time.
In order to approach the development of a fully integrated pain system that pro-
motes robotic well-being and autonomy, scenarios such as the two described aboveare required for comparison and testing purposes.
6 Conclusion
Pain is an adaptive trait produced by natural selection that promotes homeostaticbehavior by inﬂuencing the way organisms learn, empathize, remember, and fear.Pain’s feedback is essential to the biological ﬁtness of many organisms. The bio-
logical motivation and the two scenarios outlined in this paper illuminate what we
propose as a new direction of research that aspires to develop a computational the-ory of pain. Any such theory and mechanism has the potential to unify and integrate
many critical functions of an intelligent system. In turn, this may lead to fundamental
breakthroughs in smart, autonomous and resilient robots that require minimal if anyhuman intervention.
98 T. Richardson et al.
References
1. Descartes, R.: Treatise on man. The Nature of Life: Classical and Contemporary Perspectives
from Philosophy and Science, pp. 15–20 (2010)
2. Legrain, V ., Iannetti, G.D., Plaghki, L., Mouraux, A.: The pain matrix reloaded: a
salience detection system for the body. Prog. Neurobiol. 93(1), 111–124 (2011). ISSN
0301-0082. https://doi.org/10.1016/j.pneurobio.2010.10.005 .http://www.sciencedirect.com/
science/article/pii/S0301008210001759
3. Gramsch, C., Kattoor, J., Icenhour, A., Forsting, M., Schedlowski, M., Gizewski, E.R., Elsen-
bruch, S.: Learning pain-related fear: neural mechanisms mediating rapid differential condi-tioning, extinction and reinstatement processes in human visceral pain. Neurobiol. Learn. Mem.116, 36–45 (2014)
4. Singer, T., Seymour, B., O’doherty, J., Kaube, H., Dolan, R.J., Frith, C.D.: Empathy for pain
involves the affective but not sensory components of pain. Science 303(5661), 1157–1162
(2004)
5. Haddadin, S.: Towards Safe Robots: Approaching Asimovs 1st Law, vol. 90. Springer (2013)
6. Sur, I., Amor, H.B.: Robots that anticipate pain: anticipating physical perturbations from visual
cues through deep predictive models. In: 2017 IEEE/RSJ International Conference on Intelli-gent Robots and Systems (IROS), pp. 5541–5548. IEEE (2017)
7. Kuehn, J., Haddadin, S.: An artiﬁcial robot nervous system to teach robots how to feel pain
and reﬂexively react to potentially damaging contacts. IEEE Robot. Autom. Lett. 2(1), 72–79
(2017)
8. Sutton, R.S., Barto, A.G.: Introduction to Reinforcement Learning, 1st edn. MIT Press, Cam-
bridge, MA, USA (1998)
9. Ranney, D.: Anatomy of pain. In: Ontario Inter-Urban Pain Conference, Waterloo, vol. 29
(1996)
10. Nagasako, E.M., Oaklander, A.L., Dworkin, R.H.: Congenital insensitivity to pain: an update.
Pain 101(3), 213–219 (2003)
11. Dawkins, R.: River Out of Eden: A Darwinian View of Life. Basic Books (2008)
12. Craig, A.D.: A new view of pain as a homeostatic emotion. Trends Neurosci. 26(6), 303–307
(2003)
13. Skibinsky, C.G.: Changes in store for the livestock industry-canada’s recurring proposed animal
cruelty amendments. Sask. L. Rev. 68, 173 (2005)
14. Apkarian, A.V .: Pain perception in relation to emotional learning. Curr. Opin. Neurobiol.
18(4):464–468 (2008)
15. Beese, A., Morley, S.: Memory for acute pain experience is speciﬁcally inaccurate but generally
reliable. Pain 53(2), 183–189 (1993)
16. Broom, D.M.: Evolution of pain. Vlaam. Diergeneeskund. Tijdschr. 70(1), 17–21 (2001)
17. Chen, J., Li, Z., Lv, Y .F., Li, C.L., Wang, Y ., Wang, R.R., Geng, K.W., He, T.: Empathy for
pain: a novel bio-psychosocial-behavioral laboratory animal model. Sheng li xue bao:(ActaPhysiologica Sinica) 67(6), 561–570 (2015)
18. Hunter, M., Philips, C., Rachman, S.: Memory for pain. Pain 6(1), 35–46 (1979)
19. Lamm, C., Decety, J., Singer, T.: Meta-analytic evidence for common and distinct neural
networks associated with directly experienced pain and empathy for pain. Neuroimage 54(3),
2492–2502 (2011)
20. Schwartz, N., Miller, C., Fields, H.L.: Cortico-accumbens regulation of approach-avoidance
behavior is modiﬁed by experience and chronic pain. Cell Rep. 19(8), 1522–1531 (2017)
21. Talmi, D., Dayan, P ., Kiebel, S.J., Frith, C.D., Dolan, R.J.: How humans integrate the prospects
of pain and reward during choice. J. Neurosci. 29(46), 14617–14626 (2009)
22. Wiech, K., Tracey, I.: Pain, decisions, and actions: a motivational perspective. Front. Neurosci.
7, 46 (2013)
23. Zaman, J., Madden, V .J., Iven, J., Wiech, K., Weltens, N., Ly, H.G., Vlaeyen, J.W.S., V an Ouden-
hove, L., V an Diest, I.: Biased visceral perception through fear learning biased intensity judge-ments of visceral sensations after learning to fear visceral stimuli: a drift diffusion approach.J. Pain (2017)
“Should Robots Feel Pain?”—Towards a Computational … 99
24. Craig, A.D.: Opinion: how do you feel? Interoception: the sense of the physiological condition
of the body. Nat. Rev. Neurosci. 3(8), 655 (2002)
25. Pearce, S.A., Isherwood, S., Hrouda, D., Richardson, P .H., Erskine, A., Skinner, J.: Memory
and pain: tests of mood congruity and state dependent learning in experimentally induced andclinical pain. Pain 43(2), 187–193 (1990)
26. Roche, P .A., Gijsbers, K.: A comparison of memory for induced ischaemic pain and chronic
rheumatoid pain. Pain 25(3), 337–343 (1986)
27. Bonavita, V ., De Simone, R.: Pain as an evolutionary necessity. Neurol. Sci. 32(1), 61–66
(2011)
Large Sensors with Adaptive Shape
Realised by Self-stabilised CompactGroups of Micro Aerial V ehicles
Martin Saska
1 Introduction
As examples of applications of such distributed sensor arrays let us discuss
electromagnetic ﬁeld (EMF) measurement and radiation mapping, which may resultin localization of sources of radiation and EMF transmitters, for motivation in this
paper. For the task of radiation mapping and sources of radiation detection, huge
radiation detection sensors (often carried by a truck) have to be used to measure
the intensity of radiation and also to detect direction of the emitted radiation parti-
cles nowadays. Besides the great weight and limited operability, the shape of theseconventional sensors cannot be optimally adjusted to the actual situation mainly
due to the large device dimensions. In the second application, large antennas need
to be deployed to achieve required sensitivity of EMF ﬁeld mapping and sourcesof transmission localization, where the size and shape of the antenna have to be
optimized prior the mission with limited possibility of their alternation during its
deployment. Using MA Vs with available light-weight sensors in a distributed modeovercomes these drawbacks and enables to solve the mentioned tasks in an optimal
way. Moreover, it enables new perspectives and approaches of analysing properties
of the EMF and radiation ﬁelds, such as interference of multiple ﬁelds from multiplesources, attenuation in different environments, and reﬂections, in real-world scenar-
ios, while now this research is limited to laboratory conditions. From the multi-MA V
research perspective, this is a challenging example of using large groups of self-stabilized MA Vs, whose shape is directly determined by the measured properties
of the surrounding environment. It is one of the few examples, where the deploy-
ment of multi-MA V team is really necessary and which cannot be solved by a singleplatform only, as it is the case of most of the multi robot systems motivated only
by reduction of overall time of the mission. The proposed approach differs from
M. Saska (B)
Czech Technical University in Prague, Prague, Czech Republice-mail: martin.saska@fel.cvut.cz
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_13101
102 M. Saska
the available distributed sensor systems that enable to obtain sensory information
in multiple locations. It goes beyond these systems by considering all measuring
devices as a single distributed sensor in a similar way as static sensory arrays usedfor RFID (Radio Frequency Identiﬁcation) localisation. See https://comtel.fel.cvut.
cz/en/projects/rﬁd-locator for an example of such static array (a test of a similar
technology, where each MA V of the group carries one of the small antennas of thesensor, is shown in Sect. 3).
2 Challenges and Research Opportunities
The most important ability of the system of multiple MA Vs deployed in these scenar-
ios is precise position control and localization of group members. Mainly a precise
information on mutual positions of the MA Vs is crucial for correct fusion of infor-mation gathered by multiple distributed sensors onboard of helicopters. For outdoor
deployment, precision and reliability of GNSS (Global Navigation Satellite System)
is insufﬁcient in most of these applications. In some special cases, Differential Global
Positioning System (DGPS) may be applied for MA V localization with 1–2 cm pre-
cision, which provides an important tool mainly for initial testing of basic principles.Nevertheless, to provide applicability also in GPS denied environment, i.e. in indoor
applications, in workspace with high density of obstacles, in urban environment with
tall buildings, and independently to external localization systems (such as motioncapture systems or the DGPS) that have to be install prior the mission, an onboard
relative localization needs to be applied to provide precise mutual position of dis-
tributed sensors. To be able to reach the high precision (in order of centimetres) andreliability in environments with obstacles (possible occlusions and interruption of
direct visibility between MA Vs), a cooperative approach fusing information from
multiple sensors needs to be employed.
As mentioned, one of the main advantages of the distributed sensory array is
the possibility to change the shape of the group in order to maximize the gained
information. To be able to realize the desired shape based on currently gatheredsensory information, the system needs to be capable of real-time onboard processing
of scanned data, optimization of the shape of the group (desired relative positions of
MA Vs), and also precise MA V control to be able to reach these positions. To obtainedmeaningful measurements and to surpass actual approaches using large and heavy
sensors (e.g. the big antennas and radiation detectors), the position controller of
individual MA Vs must be able to keep the desired state with position error in units
of centimetres.
Moreover, the shape optimization as well as the process of re-shaping based on
currently gathered data by all team members have to be realized in a cooperative way.
This makes additional requirements on communication, which needs to be realized
with minimal possible latency, on coordination, which is challenging mainly in caseof large compact groups, and on synchronisation. The precise synchronisation of
multiple distributed sensors is crucial for example for detection of single events
Large Sensors with Adaptive Shape … 103
of radiation decay and for time-of-ﬂight and time difference of arrival measuring
systems employed for transmitters localization. Another interesting challenge is to
identify constraints of the mutual localization with respect to obstacles present inenvironment, and to integrate them into the high level motion control and shape
optimization.
In addition to these special requirements of swarm robotics, numerous additional
tools have to be integrated into the system to be able to solve given tasks in the
full complexity. Although high precision of global localization of the group in the
environment is not so critical in these multi-robot applications and standard availablerobotic solutions can be applied (see e.g. the summary in [ 2] with sufﬁcient results, an
interesting problem is fusing of information on global position obtained by different
robots of the group with different precision and reliability. Let us consider a compactgroup of multiple MA Vs equipped with GNSS ﬂying in forest or in an urban envi-
ronment, where GNSS signal is available only in some locations (forest glades, city
parks far from tall buildings, etc.) and with different precision (number of connectedsatellites differs). Therefore, the information on global position is available only for
few MA Vs at the moment and often a temporary drop out of global localization for
all MA Vs may happen. The main challenge is to distribute the information on the
global localization in the group applying the results of the mutual relative localization
and overcoming the temporary drop outs using a dead reckoning by onboard sensors(such as IMU and optical ﬂow from cameras) and known MA V model and control
inputs.
3 Preliminary Results
Being motivated by the proposed idea of using multiple self-localised and self-
stabilized MA Vs of variable relative positions (ﬂexible shape of the group) as asingle distributed sensor in a workspace with high density of obstacles (e.g. urban
environment and forest), we have designed an onboard vision-based relative local-
ization method [ 3,4], which was used for control of MA V formations in numerous
applications, such as cooperative surveillance [ 7,14], cooperative transport of large
payload ( https://youtu.be/nVWqOCK6x24 ), formation ﬂying [ 6,9–11,15,17], bio-
inspired swarming [ 5,12,13](https://youtu.be/hqHW6jYTBEY ), etc.
The precise position control of particular MA Vs has been achieved using a unique
onboard model predictive control (MPC) with possibility to compensate external
forces (e.g. applied by wind) and change of the MA V model [ 1]. The reliability and
precision of the controller have been veriﬁed in numerous multi-MA V experiments
[7,8,16] and mainly in the international competition MBZIRC in Abu Dhabi http://
mbzirc.com/ . The multi-MA V task of the competition was to cooperatively locate
and grasp a set of cylindrical object of 30 cm diameter by a magnetic gripper. In
the competition, the MPC based controller achieved the best performance (precisionand reliability) among all ﬁnalists selected in four qualiﬁcation rounds out of 150
applicants from worldwide robotic laboratories http://mrs.felk.cvut.cz/mbzirc .O u r
104 M. Saska
Fig. 1 Examples of the MA V deployment showing precision and reliability of controllers (the
cooperative grasping and landing on moving car tasks during the MBZIRC competition http://mrs.
felk.cvut.cz/mbzirc ) and relative localization (the cooperative carrying of large objects)
Fig. 2 An example of autonomous source of radiation detection and radiation mapping by a com-
posed radiation sensor consisting of multiple simulated Timepix sensors carried by MA Vs. Thepictures on the left side shows the Timepix sensor and a snapshot from Gazebo simulator, wherewe have implemented a model of the Timepix sensor and sources of radiation to be able to verifythe proposed principles with large teams of robots
system was able to fully autonomously grasp objects in strong wind conditions of
the arena located in between of ocean and desert. To be able to grasp the objects,the required precision was less than 10 cm in horizontal direction and less than 4 cm
in vertical direction (see Fig. 1and video https://youtu.be/ogmQSjkqqp0 for exam-
ples of successful grasping manoeuvres). In addition, the MPC controller achievedgood performance in high speeds, which is important for the fast transition between
different desired shapes of the group and for measuring a dynamic phenomena,
such as moving transmitters and sources of radiation, forest ﬁres, etc. In the task ofautonomous landing on a moving car, only our system was able to land three times
during the ﬁnal rounds on a car going 15 km/h and it achieved the fastest time from all
participants. The technical description of the MPC controller and the overall systemhas to be omitted due to page constraints of this paper and see [ 1] for these details
(Fig. 2).
The synchronisation, communication, ability of transition between different
desired shapes, and onboard sensor data processing and analysing were tested in
real scenarios of multiple RFID transmitters detection (including moving transmit-
ters) and localization of a source of radiation, in which the previously mentioned tools
have been also integrated. The concept of distributed measurement of the EMF ﬁeld
by multiple MA Vs was tested using omnidirectional and directive onboard antennas.
Large Sensors with Adaptive Shape … 105
Fig. 3 Snapshots from experiments with a formation of MA Vs carrying directive (left) and rotating
omnidirectional (middle) onboard antennas. Picture on the right side demonstrates inﬂuence of thelocalization error on the shape and position of the formation
It was shown that the ability of measurement of Received Signal Strength Indication
(RSSI) in multiple locations simultaneously is crucial mainly in case of a variable
EMF ﬁeld (e.g. in case of the moving transmitters), where the approach with a singleantenna achieved signiﬁcantly lower performance and often cannot be used. Even
more promising was the tests of Angle of Arrival (AOA), Time of Arrival (TOA),
Time Difference of Arrival (TDOA), and Adaptive Power Multilateration (APM)
approaches. Using a single antenna, the measurement in these principles is nega-
tively inﬂuenced by the phase composition (especially additions and subtractions)of electromagnetic waves, multiplexed deﬂects and other interferences, which are
time and point of observation dependent. It was experimentally shown (see Fig. 3for
examples of snapshots of the experiments) that the dynamic space arrangement ofthe location of multiple measurement (receiving) stations eliminates this effect and
signiﬁcantly increases precision of RFID localisation in comparison with systems
using static radar arrays.
The tested approach of the radiation source localization relies on the ability to
determine a direction of incoming particles by Timepix sensors in different positions
at the same time (see Fig. 2and links https://youtu.be/aZLEAHczIBw and https://
youtu.be/6GEYo5El-zQ for examples of simulations and experiments with simulated
sensors and a source of radiation). The possibility of measuring particles emitted in
one event of radiation decay in different positions enables to localize the source ofradiation with better precision in comparison with large compact detectors and even
estimate the type of radioactive material. In addition, the precisely synchronised
distributed measurement enables to localise position of multiple sources of radiationusing the TOA technique.
4 Conclusion and a Note for the Reviewers
The motivation of this paper is to introduce the complex problem of realizationof antenna-like sensing devices (sensor arrays) with a ﬂexible shape using largecompact groups of Micro Aerial V ehicles (MA Vs) and to highlight challenges and
106 M. Saska
research opportunities in this problem. We brieﬂy summarised our achievements
in this research stream to clarify particular tasks that have to be investigated and
to demonstrate their difﬁculties. Although, we have touched most of the addressedchallenges that are crucial for demonstrating the basic functionalities of the composed
distributed sensor, most of the research problems remain opened, mainly to achieve
sufﬁcient reliability and applicability in various indoor and outdoor environments.We hope that this paper could attract robotic community to solve the particular
problems and to speed up research in this challenging task.
Acknowledgements The work has been supported by the Czech Science Foundation (GACR)
under research project No. 17-16900Y . Also the support of the CESNET association is greatlyappreciated.
References
1. Baca, T., Loianno, G., Saska, M.: Embedded model predictive control of unmanned micro
aerial vehicles. MMAR (2016)
2. Cadena, C., Carlone, L., Carrillo, H., Latif, Y ., Scaramuzza, D., Neira, J., Reid, I., Leonard,
J.J.: Past, present, and future of simultaneous localization and mapping: toward the robust-
perception age. IEEE Trans. Robot. 32(6), 1309–1332 (2016)
3. Faigl, J., Krajnik, T., Chudoba, J., Preucil, L., Saska, M.: Low-cost embedded system for
relative localization in robotic swarms. In: IEEE ICRA (2013)
4. Krajnik, T., Nitsche, M., Faigl, J., V anek, P ., Saska, M., Preucil, L., Duckett, T., Mejail, M.: A
practical multirobot localization system. J. Intell. Robot. Syst. 76(3–4), 539–562 (2014)
5. Saska, M.: MA V-swarms: unmanned aerial vehicles stabilized along a given path using onboard
relative localization. In: International Conference on Unmanned Aircraft Systems (ICUAS)(2015)
6. Saska, M., Baca, T., Hert, D.: Formations of unmanned micro aerial vehicles led by migrating
virtual leader. In: 14th International Conference on Control, Automation, Robotics and Vision(ICARCV) (2016)
7. Saska, M., Baca, T., Thomas, J., Preucil, L., Krajnik, T., Loianno, G., Kumar, V .: System for
deployment of groups of unmanned micro aerial vehicles in GPS-denied environments usingonboard visual relative localization. Auton. Robot. 41(4), 919–944 (2017)
8. Saska, M., Chudoba, J., Preucil, L., Thomas, J., Loianno, G., Kumar, V .: Autonomous deploy-
ment of swarms of micro-aerial vehicles in cooperative surveillance. In: ICUAS (2014)
9. Saska, M., Kasl, Z., Preucil, L.: Motion planning and control of formations of micro aerial
vehicles. In: 19th World Congress of the International Federation of Automatic Control (IFAC).IFAC (2014)
10. Saska, M., Krajnik, T., V onasek, V ., Kasl, Z., Spurny, V ., Preucil, L.: Fault-tolerant formation
driving mechanism designed for heterogeneous MA Vs-UGVs groups. J. Intell. Robot. Syst.73(1–4), 603–622 (2014)
11. Saska, M., Krajnik, T., V onasek, V ., V anek, P ., Preucil, L.: Navigation, localization and stabi-
lization of formations of unmanned aerial and ground vehicles. In: International Conference
on Unmanned Aircraft Systems (ICUAS) (2013)
12. Saska, M., Langr, J., Preucil, L.: Plume tracking by a self-stabilized group of micro aerial
vehicles. In: Modelling and Simulation for Autonomous Systems (2014)
13. Saska, M., V akula, J., Preucil, L.: Swarms of micro aerial vehicles stabilized under a visual
relative localization. In: IEEE ICRA (2014)
Large Sensors with Adaptive Shape … 107
14. Saska, M., V onásek, V ., Chudoba, J., Thomas, J., Loianno, G., Kumar, V .: Swarm distribution
and deployment for cooperative surveillance by micro-aerial vehicles. J. Intell. Robot. Syst.84(1), 469–492 (2016)
15. Saska, M., V onasek, V ., Krajnik, T., Preucil, L.: Coordination and navigation of heterogeneous
UA Vs-UGVs teams localized by a hawk-eye approach. In: IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS) (2012)
16. Saska, M., V onasek, V ., Krajnik, T., Preucil, L.: Coordination and navigation of heterogeneous
MA V&UGV formations localized by a ‘hawk-eye’-like approach under a model predictivecontrol scheme. Int. J. Robot. Res. 33(10), 1393–1412 (2014)
17. Spurny, V ., Baca, T., Saska, M.: Complex manoeuvres of heterogeneous MA V-UGV formations
using a model predictive control. In: 21st International Conference on Methods and Models inAutomation and Robotics (MMAR) (2016)
Self-directed Lifelong Learning
for Robot Vision
T anner Schmidt and Dieter Fox
1 Visual Information Extraction Systems
General-purpose robots of the future will need to robustly perceive and understand
the local environment in order to attain various goals. Currently, computer visionresearch often focusses narrowly on particular tasks, such as object detection and
recognition, semantic segmentation, caption generation, or pose estimation. How-
ever, any particular task put to a general-purpose robot could require different infor-
mation to be gleaned from a visual sensor, necessitating a general-purpose vision
system. We can envision such a system as a generic visual information extraction
system , which is able to process an image and produce a representation of its con-
tent. This representation should be sufﬁcient for completing a wide array of potential
tasks. The speciﬁc, purely visual tasks listed above could be solved by selectivelyextracting only the task-relevant information (e.g. the class of an object or semantic
label of a particular pixel).
In order to deal with the vast quantity of information in a high-resolution image
and limited computational budgets, visual information extraction systems will need
attention mechanisms. This includes attention as traditionally understood in computer
vision (i.e. attention to an image sub-region) as well as attention to only task-relevantinformation. This would allow the robot to avoid incurring the cost of extracting
all possibly relevant information when only a small fraction is needed. Note that
task-dependent attention requires either multiplexing an array of task-speciﬁc visualsystems or, perhaps more realistically, top-down feedback that changes the behavior
of the information extraction system depending on the task.
T. Schmidt ( B)·D. Fox
Paul G Allen School of Computer Science and Engineering, Seattle, W A, USA
e-mail: tws10@cs.washington.edu
D. Fox
e-mail: fox@cs.washington.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_14109
110 T. Schmidt and D. Fox
Robots embodied in the physical world will observe temporally consistent visual
scenes. It would be wasteful to extract all possibly relevant information from every
frame of video input, as the information content of neighboring frames is highlycorrelated. It would therefore be prudent for the visual information extraction system
to integrate information over time and focus on novel or changed aspects of incoming
imagery. This approach complicates things, as the robot’s moving vantage pointmeans some model of the world is needed to do integration. However, the gains in
efﬁciency when only incremental updates are needed will likely outweigh the cost
of maintaining such a model.
2 A Path to Robust Visual Information Extraction
Now that we have loosely outlined the desiderata of a robot visual system, we’llexplore how it might be realized. Given recent developments in the ﬁeld of computer
vision, it seems highly unlikely that signiﬁcant portions of future robot vision systems
will be hand-engineered. This is not to say that we expect human engineering to play
no role whatsoever. There simply seems to be no way that any human engineer
will ever manually design a fully performant system that requires no learning. Wetherefore believe robot vision systems in the future will be mostly learned with some
manually-designed common-sense and physics-based priors.
Under the assumption that future robot vision systems will rely heavily on learn-
ing, we now consider the question of supervision. Given the immense scope of
information that may be needed from images (i.e. all potentially task-relevant infor-
mation), training with full supervision immediately appears to be untenable. As itstands, datasets for tasks such as semantic segmentation are already limited in size
by the need for a human to indicate the class of every pixel in every image.
1Addi-
tionally, semantically labelled datasets typically allow for on the order of 103or
fewer classes, which is probably orders of magnitude fewer than would be needed
for general-purpose robotics applications. To complicate things even further, a single
class label is not always sufﬁcient considering hierarchical nesting of classes (musta Schnauser also be labelled as a dog? a mammal?) or even trickier problems like
reﬂections (should an image of a book as seen reﬂected in a mirror be labelled ‘book’
or ‘mirror’?). It could also be useful to recognize variations on the same class. Forexample, a robot seeking to place a foot should know the difference between ‘wood
ﬂoor’ and ‘wet wood ﬂoor’. Should these be separate classes? Are both road ‘road’
and ‘wet road’ required as well? What about ‘icy road?’. Given all these difﬁculties,
we argue that full human supervision is not a viable path to robust visual information
extraction systems.
Another possible path to robust visual information extraction, which we will
consider brieﬂy, is to learn to extract task-relevant information using only task-
level supervision (i.e. a reward function) in the context of reinforcement learning.
1This is true at least in the case of publicly available datasets.
Self-directed Lifelong Learning for Robot Vision 111
There has already been some exciting work in this direction by Levine et al. [ 3]
and Mnih et al. [ 4], just to name a few. While this approach is plausible, it does
seem somewhat unrealistic to expect an agent to invent visual information extraction
from scratch using only (potentially much delayed) rewards as a training signal.
One could imagine adding additional rewards which encourage the development of
a visual information extraction submodule, but this would likely end up looking very
much like our preferred approach: robots that teach themselves to see as part of a
life-long and self-directed learning process.
2.1 First Steps T owards Self-directed Lifelong Learning
Work has already begun on leveraging self-supervision to solve computer vision
problems. Godard et al. used stereo pairs and a reconstruction objective to train a
depth estimation network [ 2]. Unsupervised learning of image representations has
been demonstrated by Agrawal et al. [ 1], using egomotion cues, by Wang and Gupta
[6], using tracked patches from Y ouTube videos, and by Zeng et al. [ 7]u s i n ga
SLAM system to identify and learn from keypoint correspondences in depth videos.
In prior work, we explored using SLAM systems to label dense correspondences
in RGB-D video [ 5]. After tracking many points through many videos, a network
was trained to produce representations such that images of corresponding points are
represented in the same way (see illustration in Fig. 1). Due to the need for continuous
tracking for automated labelling, we were only able to use correspondences between
points in the same video for training. However, we showed that the network learned
a descriptor that generalized to cross-video correspondences despite the lack of any
Different Objects
Same Object
Fig. 1 An illustration of the self-supervised approach to learning about objects. Two objects appear-
ing at the same time are obviously different, and if the motion of an object can be tracked from one
frame to another, it is known that the frames are two observations of the same object
112 T. Schmidt and D. Fox
Fig. 2 A deep network was trained to output dense three-dimensional descriptors, visualized here
by mapping the descriptor values to the RGB cube. The network learns ﬁne-grained recognitionwith no supervision. The network that produced these representations was trained with 59 videosof the same person, but was not given correspondences across videos. See [ 5] for more details
training signal encouraging this property. A visualization of descriptors learned by
our network can be found in Fig. 2.
Image representations learned without supervision often take the form of a visual
descriptor or feature vector which represents an image or part of an image with a
vector of ﬁxed size. While this might not ultimately prove to be the optimal outputrepresentation, it is currently the dominant paradigm. Therefore, we will frame the
discussion with the goal of producing feature vectors which represent all information
extracted from an image in mind.
2
2Solving a speciﬁc task, such as estimating the semantic class of a particular pixel, might then
involve a further processing step which takes the full feature vector as input and produces onlythe relevant information, which might be as simple as projection onto a lower-dimensional vectorwhich preserves the relevant information and discards everything else. If it is possible that this furtherprocessing step for all tasks can indeed be implemented as a projection, and furthermore that theprojections are orthogonal and axis-aligned, then we can achieve the same effect by computing onlythe relevant part of the feature vector to begin with and avoiding the expense of computing featuredimensions that fall in the null space of the projection.
Self-directed Lifelong Learning for Robot Vision 113
2.2 Next Steps
The systems mentioned above represent a ﬁrst step along the self-directed learning
path, as they use hand-engineered visual tracking methods based only on physicalpriors to learn to represent images. If a robot running such a system also maintained an
indexed database of previously seen objects, it should be able to detect re-appearances
of objects and link new appearances with previous experiences—an ability very muchlike what is termed ‘object permanence’ in infants. Much like a human child, the
robot could be told the name of each object a few times, and we might reasonably
expect it to apply the name to all past experiences of the same object.
In this way we could derive a powerful object classiﬁcation system from a massive
amount of data and a comparatively tiny set of labels. Of course, object permanence
is not the end goal of robot vision, so further steps will be needed.
We believe that the systems mentioned above can be greatly improved using
bootstrapping techniques. Once an initial system is trained, it can be used to enhancethe abilities of currently existing model-based trackers, e.g. by enabling better and
faster loop closure detection, providing better data association for use within pose
optimization, or allowing for more expressive objective functions. These enhancedtracking systems will allow the system to automatically label a richer and more varied
dataset, leading to improved features. Iterating this procedure should continue to
improve reoccurrence recognition rates and decrease the incidence of false positivesfor some time until diminishing returns kick in. In the end, the system will be capable
of matching known objects despite very large changes in lighting, viewpoint, and
pose. Once the system has seen a sufﬁciently large set of objects, it should also beable to produce meaningful representations of entirely novel objects. This will enable
the robot to reason about new objects by, for example, inferring potential grasp points
or estimating its semantic relationship to previously known object classes.
Recognizing reoccurences is a powerful ability of a visual information extraction
system and will help solve problems like loop closure and object recognition, but
many robotics applications demand more. The ideal vision system would be able to
determine what, if anything, differs between two appearances of what is otherwise the
same object, such as lighting or state changes. In other words, to determine an object’sclass or instance identity, the system should use features that are invariant to lighting,
viewpoint, pose, and state. However, these properties are useful for accomplishing
some tasks, so we’d like to preserve this information in some form. For example,to return to a previous example, the reﬂectance changes induced by the presence of
water on a ﬂoor is informative for locomotion. The end goal is thus to learn rich,
disentangled representations.
We argue that visual identiﬁcation of object state can also be learned with self-
supervision, using the same technique as before: by observing changes. Observing
an object as it moves from one pose to another allows a system to automaticallylearn pose-invariant representations. Similarly, observing an object transition from
one state to another will allow the system to learn a representation which enables
recognition of the object in both states as well as identiﬁcation of its current state.
114 T. Schmidt and D. Fox
For example, if the robot is able to observe an onion being chopped, it is obvious
given basic physical priors that the chopped onion is the same matter as the original
onion. Given pre-training to the ‘object performance’ level of performance and thesigniﬁcant appearance change, it should also be obvious that the onion has undergone
a signiﬁcant state change. Such an observation can be used to produce a label identi-
fying the observations before and after chopping as two different states of the sameobject. We might expect the system to cluster many instances of chopped vegeta-
bles without supervision given the appearance similarities. Then it will again require
very few external labels to teach the robot that the new state it had learned is called‘chopped.’
3 Conclusion
We presented a vision of a plausible path from vision systems of today to visual
information extraction systems of the future that will enable general-purpose robots
to interact with the world robustly, safely, and efﬁciently. The key idea is to combine
self-directed learning on unlabelled data, which can be done at vast scale, with a
very sparse set of externally-labelled data points. The robot will ﬁrst teach itself tosensibly organize visual inputs using only a small set of physics-based priors, and
then propagate whatever labels it receives through the self-taught relations between
inputs. A system can start with nothing more than a visual tracking system and teachitself to recognize recurring objects, then use its ‘object permanence’-like abilities to
observe state changes and learn to identify various states of objects. By bootstrapping
in this way, a robot can continuously improve its perceptual capabilities over time.In the end, the robot will be equipped with a visual system that can extract all
information from a video stream that might be relevant to some generic task.
References
1. Agrawal, P ., Carreira, J., Malik, J.: Learning to see by moving. In: Proceedings of the IEEE
International Conference on Computer Vision, pp. 37–45 (2015)
2. Godard, C., Mac Aodha, O., Brostow, G.J.: Unsupervised monocular depth estimation with
left-right consistency (2016). arXiv preprint arXiv:1609.03677
3. Levine, S., Finn, C., Darrell, T., Abbeel, P .: End-to-end training of deep visuomotor policies. J.
Mach. Learn. Res. 17(39), 1–40 (2016)
4. Mnih, V ., Kavukcuoglu, K., Silver, D., Graves, A., Antonoglou, I., Wierstra, D., Riedmiller, M.:
Playing atari with deep reinforcement learning (2013). arXiv preprint arXiv:1312.5602
5. Schmidt, T., Newcombe, R., Fox, D.: Self-supervised visual descriptor learning for dense cor-
respondence. IEEE Robot. Autom. Lett. 2(2), 420–427 (2017)
6. Wang, X., Gupta, A.: Unsupervised learning of visual representations using videos. In Proceed-
ings of the IEEE International Conference on Computer Vision, pp. 2794–2802 (2015)
7. Zeng, A., Song, S., Nießner, M., Fisher, M., Xiao, J.: 3D match: learning the matching of local
3D geometry in range scans (2016). arXiv preprint arXiv:1603.08182
Deep Coverage: Motion Synthesis
in the Data-Driven Era
David A. Surovik and Kostas E. Bekris
1 Modern Robotic Capabilities and Challenges
Robots have thrived in highly structured, accurately known, and safely enclosed
industrial settings, such as those shown in Fig. 1(left). Typically, these platforms
consist of rigid elements connected by a few degrees of freedom ( DoF ) that can be
controlled directly. This allows strict enforcement of motion for high speed, accuracy,
and consistency of repeated tasks. In contrast, modern robotics aspires to more gen-
eral and ambitious operational goals involving highly unstructured environments,
arbitrary terrains, unknown objects, or the company of humans. This means thattraditional objectives are superseded by a fundamental need for adaptability.
These altered priorities motivate the use of robots with versatile physical traits,
such as deformation and heightened articulation, which can be achieved by usingsoft components or a high number of compliant DoF ,a si nF i g . 1(center/right).
These features allow for adaptive contact geometry and storage or dissipation of
energy for purposes such as efﬁcient mobility on rough terrain and manipulationof arbitrary objects. Examples include soft [ 8] and adaptive hands [ 9], bio-inspired
modular robots [ 19], underactuated legged systems [ 28], as well as tensegrity rovers,
dynamic truss structures that incorporate length-actuated members [ 23].
While such platforms provide increased safety and adaptability, they pose many
complications to the prediction and regulation of motion. Due to higher system
dimensionality and imperfect sensing of environmental variation, natural dynamicalinﬂuences such as momentum and contact forces become both more signiﬁcant and
more complex to describe. Nevertheless, a pivotal requirement is to achieve motion
D. A. Surovik ·K. E. Bekris ( B)
Computer Science Department of Rutgers, The State University of New Jersey,New Brunswick, USA
e-mail: kostas.bekris@cs.rutgers.edu
D. A. Surovik
e-mail: david.surovik@cs.rutgers.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_15115
116 D. A. Surovik and K. E. Bekris
Fig. 1 Left: Rigid industrial robots for manufacturing [KUKA]. Center: a soft robotic hand con-
forming to a grasped object [ 8]. Right: NASA ’s terrain-compliant tensegrity rover, SUPERball [ 23]
coverage with such robots. This expresses the need to effectively command motion
for a wide range of operational bounds. This work aims to outline a pathway toward
this goal in light of ongoing progress in motion synthesis and data-driven design.
2 Computing Motion for Operational Coverage
Discussion of the concept of motion coverage will start from nominal coverage, which
identiﬁes useful paths within a region, but quickly proceed to focus on robust cover-
age, which ensures intended motion under a range of circumstances. Model-Based
Motion Coverage : Given geometric and kinematic models, nominal coverage can
be provided via tools for validating conﬁgurations (e.g., collision checkers [ 18]) and
for charting connectivity, such as sampling-based roadmaps [ 14]. These principles
can also account for dynamics [ 17], uncertainty [ 1,4], changing environments [ 27]
and robustness, if given stabilizing controllers [ 26].
Existing motion coverage tools depend on a tractable system model.
Many systems, however, cannot easily return to a nominal path after signiﬁcant
deviation. Control synthesis , the composition of multiple control laws established
prior to execution, can provide robust coverage. One composition method is to design
controllers so that each region of attraction ( RoA ) is “funneled” into another [ 6], as in
Fig. 2(left). LQR -trees integrate this idea with sampling to provide nonlinear robust
coverage via linear quadratic regulators ( LQR )[25]. Alternatively, Explicit Model
Predictive Control tiles the operational space with light-weight localized equivalents
of an expensive predictive controller [ 3] (center of Fig. 2). Composition can also
address discontinuities, as in Sliding Mode Control [ 11].
Deep Coverage: Motion Synthesis in the Data-Driven Era 117
x1xn
pipjpk...xn
x1pa,ipa,j
pb,ipb,j
pa,k...p1
p2p3
π
x1xn
Fig. 2 Examples of controller composition: sequential (left), tiling (center), and merging (right)
Combining multiple controllers boosts robustness with minimal online effort.
Data-Driven Control : The above methods are not easily applied to systems with
many compliant DoF and strong natural dynamics. Even with a descriptive model,
it may be challenging to determine if a conﬁguration of a deformable robot is valid.
Contact forces, energy, and momentum hinder the identiﬁcation of good dynamical
paths and the derivation of stabilizing controllers. Such complications have spurred
data-driven approaches for interpreting sensor data, predicting state evolution, or
imposing actuation [ 12,24]. They have also led into modeling all three as a single
“end-to-end” process [ 16]. These methods ﬁt generic parametrized models, such as
polynomial functions, Gaussian processes, or artiﬁcial neural networks ( ANN ), to
match experimental data that represent the intended operational regime.
Learned models can adequately represent complex behaviors,
but typically require signiﬁcant training effort for each application scenario.
The risks and demands of obtaining necessary data have motivated many inno-
vations with relevance to controller synthesis. To improve sample efﬁciency via
optimal control principles, Guided Policy Search merges an evolving set of LQR
controllers into one ANN feedback policy [ 16]. Although this approach has been
applied end-to-end, it can also be viewed as a controller composition scheme (see
right of Fig. 2). Meanwhile, other data-driven techniques have been developed for
determining nonlinear controller RoA s in the context of partially modeled dynamics
and safety-constrained sampling [ 2,5]. A model-free variant of LQR -Trees similarly
establishes RoA s via sampling to validate the synthesized control set [ 21].
Recent work has begun to reveal how data-driven control can be made more
efﬁcient and veriﬁable, potentially for use in composition schemes.
118 D. A. Surovik and K. E. Bekris
3 Data-Driven Synthesis for Deep Coverage
An emerging possibility for motion computation on modern robotic platforms is to
(a) establish modular “motion elements” through an ofﬂine data-driven process and(b) employ them online in an architecture that exploits their suitability for compo-
sition and synthesis. This framework, summarized in Fig. 3, will be referred to as
“deep coverage” to emphasize the goal of effectiveness under highly variable cir-
cumstances (hence coverage) through hierarchical, modular processes that enforce
robust behavior at all scopes of the problem (hence depth). Motion elements at var-
ious levels of the proposed hierarchy—which are no longer clearly delineated by anexplicit, analytical model but are instead data-driven—can also be seen as equivalent
to different layers in deep learning architectures for perception challenges.
Successful employment of “deep coverage” will require careful consideration of
numerous trade-offs and design principles that follow from the combined use of
data-driven design and motion composition. The following discussion of some ofthese key aspects will frequently refer to an example scenario of deploying NASA ’s
tensegrity rover over an unstructured landscape, as shown in Fig. 4.
Robust Motion Elements : The motion elements for deep coverage must meet needs
at varying physical and temporal scope. For instance, they should be able to govern
the pressure response of a ﬁnger or leg, reach for or in-hand manipulate an object, or
achieve locomotion of an entire platform. One way to provide this is to increase theapplicability of individual elements so as to cover broader circumstances, represented
as increased funnel width in Fig. 3(center). The recent methods for verifying and
expanding RoA s may help fulﬁll this desire through increased use of data. Training
controllers to match commanded values for steering, dampening, etc., would further
boost their individual usefulness.
Nonetheless, the approach of broadening the RoA through increased data use
will eventually suffer diminishing returns and weakened veriﬁability if taken to its
Fig. 3 Three different paradigms for computing robust motion for nonlinear, nonconvex systems.
Given computational resources, motion elements are produced to provide motion coverage. Leftcolumn: model-based synthesis, center: end-to-end, right: deep coverage
Deep Coverage: Motion Synthesis in the Data-Driven Era 119
Fig. 4 A mock-up of NASA ’s tensegrity rover on a rugged Martian landscape [NASA/JPL Caltech]
extreme. A contrasting desire is for each element to be modular , with a
fundamental, distinct and well interpretable purpose. This eases the matching of
elements to speciﬁc roles in different problem instances, improving adaptability [ 7].
If complete solutions are available as reference, modularity might be achieved bydecomposing them into elements of the desired scope [ 13].
In the context of the tensegrity rover example, these two possibilities can be
illustrated in terms of different terrain types to be deﬁned, such as sandy or rubble-ﬁlled areas with different slopes. As depicted by the region-expanding arrows in
Fig. 5, a motion element for one terrain class could potentially be broadened to
cover related domains. Yet, natural limits to this practice are to be anticipated; itmay be infeasible or unreasonable to formulate a single element for use, e.g., on
both rubble and sand, due to fundamental differences in contact mechanics. Similar
considerations could arise for a compliant end-effector, with individual elements
being effective for wide varieties of grasped object shapes but naturally delineated
in terms of other factors, such as object fragility or deformability. Limitations suchas this would need to be recognized within reference data that span multiple types
of scenarios, to give rise to appropriate boundaries of specialization.
Each motion element should be broad enough to quickly cover the space,
but focused enough so that it is veriﬁable, modular and reusable.
Comprehensive Libraries : Equally important to the effectiveness of individual ele-
ments is that they collectively cover the underlying operational space and enable
good solutions to be discovered efﬁciently. Covering the problem space, illustrated
via tiling in the bottom of Fig. 3, requires that each class of environmental feature
(e.g., terrain type in locomotion or object type in manipulation) can be handled by at
least one element. Allowing overlapping applicability among elements can be beneﬁ-
cial for safety and performance, since the failure of one controller may be recoverable
120 D. A. Surovik and K. E. Bekris
Fig. 5 Classiﬁcations of different terrains that may require different motion elements. Arrows
indicate expansion of an individual element’s coverage
by a neighboring one. Such redundancy can also bode well for adaptability to new
domains. At the same time, an over-abundance of options should be avoided in order
to moderate the size of the search and decision-making process.
Approaches for designing a good library can borrow ideas from the principle of
cooperative coevolution [ 20], where each individual element’s ﬁtness is measured in
the context of group performance. The data-efﬁciency of library design might also
be improved by selectively sharing information between the training processes for
different elements, possibly based on a learned classiﬁcation [ 22].
In terms of the tensegrity rover example, this aspect suggests a requirement for
both rubble-traversing and sand-traversing gaits. Less obviously, it also motivates the
previously discussed broadening of individual elements — if each of these gaits canfeasibly handle an intermediate terrain type containing a mixture of rubble and sand,
then it may or may not be warranted to further inﬂate the motion element library by
including an element speciﬁcally tailored to such a scenario. This is illustrated bythe arrows in the “sandy rubble” region of Fig. 5, implying mutual coverage by the
sand gait and the rubble gait.
An element library should be minimal but comprehensive so as to cover the
operational space, while achieving efﬁcient and robust solutions.
Composition of Elements : When efﬁciently piecing together a complete solution,
the various interactions between motion elements, objectives, and environmental
features must be considered. The traversal scenario of Fig. 6, for example, involves
both different types of terrain and different demands for movement precision. For
the relatively open expanse of rubble, it should quickly be recognized by the motion
planner that a general-purpose low-precision gait is sufﬁcient for progressing. Only
Deep Coverage: Motion Synthesis in the Data-Driven Era 121
Fig. 6 A terrain navigation plan using motion elements with different scopes and properties. Call-
out: Synthesis of a new motion element to address an unforeseen terrain feature
once the cluster of sharp rocks has been reached will it become worthwhile to consider
the use of more precise and specialized maneuvers, which are necessary for handling
more formidable obstacles but are slower and more difﬁcult to sequence. Other typesof systems, conversely, may involve independent motion elements, such as an arm
movement primitive and an end-effector grasping action. Given the holistic nature of
the dynamics for compliant and deformable systems, integration of behaviors usedin parallel must also be handled with care.
This process could amount to an informed search of the space of element com-
positions, as in traditional planning, with reasoning based upon RoA s and other
motion element characterizations. Alternatively, decisions could be governed by a
data-driven process, aimed to handle subtle interactions. Composability of human
motions, for example, can be established through segmentation of reference solu-
tions [ 15]. This use of a data-driven approach for composition can lead to hierarchical
schemes, in which intermediate-level elements can be responsible for the composi-tion of low-level ones, without rigid and restrictive rules.
The motion element composition process should account for the elements’
applicability and compatibility, whether determined via models or data.
Continued Learning Online : Real-world deployment of robots in natural and
human-occupied workspaces inevitably leads into unforeseen setups, where a validsolution must be improvised quickly. This entails identifying a gap in coverage, safely
exploring the corresponding region of the system’s dynamics, and incorporating the
results into a new motion element (far right of Fig. 3). These steps should beneﬁt
from ofﬂine training and effectively utilize the existing library of motion elements
so that the online adaptation can be light-weight.
122 D. A. Surovik and K. E. Bekris
A beneﬁt of data-driven approaches is that they can incorporate additional,
highly-relevant data gathered in the ﬁeld during deployment. Again, the challenge
lies in the amount of data that need to be collected. Model-based reinforcement learn-ing brings the hope of adaptation to new domains in a sample-efﬁcient manner as
long as the model of the robot’s dynamics are appropriately adapted on the ﬂy [ 10].
This type of adaptation would be most effective with a highly modular library, whereelements represent the system dynamics in a principled manner.
The call-out in Fig. 6demonstrates these principles for the tensegrity example.
Traversal of the rock ﬁeld would provide real-world data on the performance ofsome of the precise motion elements. When facing an unforeseen obstacle, such as
the narrow gap between two large boulders, this information could be leveraged to
guide discovery of a new motion element to ﬁll the gap in operational coverage.
New elements should be generated on the ﬂy to deal with unforeseen setups.
4 Discussion
Many modern robots experience complex inﬂuences from the environment and their
natural dynamics, which are difﬁcult to adequately model. This motivates end-to-end control architectures that solve motion problems at sufﬁcient depth , but depend
on a lot of data for generalization. Meanwhile, controller synthesis and composi-
tion enable greater breadth of motion coverage, but depend on accurate models. A
successful reconciliation of these two paradigms can enable deep coverage: the efﬁ-
cient use of data-derived motion elements for generating robust solutions in general
problem spaces in the context of exciting, frontier applications of robotics.
References
1. Agha-mohammadi, A.a., Chakravorty, S., Amato, N.M.: FIRM: Sampling-based feedback
motion-planning under uncertainty and imperfect measurements. IJRR 33(2), 268–304 (2014)
2. Akametalu, A.K., Fisac, J.F., Gillula, J.H., Kaynama, S., Zeilinger, M.N., Tomlin, C.J.:
Reachability-based safe learning with Gaussian processes. In: CDC, pp. 1424–1431 (2014)
3. Alessio, A., Bemporad, A.: A Survey on Explicit Model Predictive Control (2009)4. Bai, H., Hsu, D., Lee, W.S.: Integrated perception and planning in the continuous space: a
POMDP approach. IJRR 33(9), 1288–1302 (2014)
5. Berkenkamp, F., Moriconi, R., Schoellig, A.P ., Krause, A.: Safe learning of regions of attraction
for uncertain systems with Gaussian processes. In: CDC, pp. 4661–4666 (2016)
6. Burridge, R.R., Rizzi, A.A., Koditschek, D.E.: Sequential composition of dynamically dexter-
ous robot behaviors. IJRR 18(6), 534–555 (1999)
7. Clune, J., Mouret, J.B., Lipson, H.: The evolutionary origins of modularity. Proc. R. Soc. B
280(1755), 2012–2863 (2013)
Deep Coverage: Motion Synthesis in the Data-Driven Era 123
8. Deimel, R., Brock, O.: A novel type of compliant and underactuated robotic hand for dexterous
grasping. IJRR 35(1–3), 161–185 (2016)
9. Dollar, A.M., Howe, R.D.: The highly adaptive SDM hand: design and performance evaluation.
IJRR 29(5), 585–597 (2010)
10. Fu, J., Levine, S., Abbeel, P .: one-shot learning of manipulation skills with online dynamics
adaptation and neural network priors. In: IROS (2016)
11. Guldner, J., Utkin, V .I.: Sliding mode control for gradient tracking and robot navigation using
artiﬁcial potential ﬁelds. IEEE TRA 11(2), 247–254 (1995)
12. Hou, Z.S., Wang, Z.: From model-based control to data-driven control: survey, classiﬁcation
and perspective. Inf. Sci. 235, 3–35 (2013)
13. Ijspeert, A.J., Nakanishi, J., Hoffmann, H., Pastor, P ., Schaal, S.: Dynamical movement prim-
itives: learning attractor models for motor behaviors. Neural Comput. 25(2), 328–373 (2013)
14. Kavraki, L.E., Svestka, P ., Latombe, J.C., Overmars, M.H.: Probabilistic roadmaps for path
planning in high-dimensional conﬁguration spaces. IEEE TRA 12(4), 566–580 (1996)
15. Lee, C.S., Elgammal, A.: Human motion synthesis by motion manifold learning and motion
primitive segmentation. In: Articulated Motion and Deformable Objects, pp. 464–473 (2006)
16. Levine, S., Finn, C., Darrell, T., Abbeel, P .: End-to-end training of deep visuomotor policies.
JMLR 17(39), 1–40 (2016)
17. Li, Y ., Littleﬁeld, Z., Bekris, K.E.: Asymptotically optimal sampling-based kinodynamic plan-
ning. IJRR 35(5), 528–564 (2016)
18. Lin, M., Manocha, D.: Efﬁcient contact determination in dynamic environments. Int. J. Comput.
Geom. Appl. 07(01n02), 123–151 (1997)
19. Onal, C.D., Rus, D.: Autonomous undulatory serpentine locomotion utilizing body dynamics
of a ﬂuidic soft robot. Bioinspiration Biomimetics 8(2), 3–26 (2013)
20. Pollack, J.B., Lipson, H., Ficici, S., Funes, P ., Hornby, G., Watson, R.A.: Evolutionary tech-
niques in physical robotics. In: Evolvable Systems: From Biology to Hardware (2000)
21. Reist, P ., Preiswerk, P ., Tedrake, R.: Feedback-motion-planning with simulation-based LQR-
trees. IJRR 35(11), 1393–1416 (2016)
22. Rennie, C., Bekris, K.E.: Discovering a library of rhythmic gaits for spherical tensegrity loco-
motion. In: IEEE ICRA (2018)
23. Sabelhaus, A.P ., Bruce, J., Caluwaerts, K., Manovi, P ., Firoozi, R.F., Dobi, S., Agogino, A.M.,
SunSpiral, V .: System design and locomotion of SUPERball, an untethered tensegrity robot.In: IEEE ICRA, pp. 2867–2873 (2015)
24. Tai, L., Liu, M.: Deep-learning in mobile robotics - from perception to control systems: a survey
on why and why not (2016). arXiv:1612.07139 [cs]
25. Tedrake, R., Manchester, I.R., Tobenkin, M., Roberts, J.W.: LQR-trees: feedback motion plan-
ning via sums-of-squares veriﬁcation. IJRR 29(8), 1038–1052 (2010)
26. Walsh, G., Tilbury, D., Sastry, S., Murray, R., Laumond, J.P .: Stabilization of trajectories for
systems with nonholonomic constraints. IEEE Trans. Autom. Control 39(1), 216–222 (1994)
27. Yang, Y ., Brock, O.: Elastic roadmaps–motion generation for autonomous mobile manipulation.
Auton. Robot. 28(1), 113 (2010)
28. Zhou, X., Bi, S.: A survey of bio-inspired compliant legged robot designs. Bioinspiration
Biomimetics 7(4), (2012)
Control
Multi-objective Optimal Control
for Proactive Decision Makingwith Temporal Logic Models
Sandeep P. Chinchali, Scott C. Livingston and Marco Pavone
1 Introduction
Data-driven learning and proactive decision making are key ingredients of modern
autonomous systems (AS). Robots, ranging from surgical assistants to autonomouscars, must seamlessly interact with other agents, which requires understanding their
intents and behavioral models. While most current strategies used by a robot to
understand the plan of a human rely on passive observations, recent work has focused
signiﬁcant attention on proactive intent decoding and decision making [ 8,15]. Exam-
ples include autonomous cars that gently nudge into adjacent lanes to discern thedriving style of nearby drivers for lane-merging [ 15] or use large signs to proactively
signal when pedestrians can safely cross at intersections [ 8].
A principal challenge of proactive decision making coupled with concurrent
robotic control is that the resulting decision-making problem is computationally
intractable. A robot must optimize over several plausible models of human behavior,
which is especially complex if we consider a high-dimensional set of trajectories thatan agent may enact to accomplish its goals. In this context, the principal aim of this
paper is to provide a tractable approach for proactive decision making that exploits
algorithmic and data mining techniques for dimensionality reduction.
The authors were partially supported by the Ofﬁce of Naval Research, ONR YIP Program, under
Contract N00014-17-1-2433.
S. P . Chinchali ( B)·M. Pavone
Stanford University, Stanford, CA, USAe-mail: csandeep@stanford.edu
M. Pavone
e-mail: pavone@stanford.edu
S. C. Livingston
California Institute of Technology, Pasadena, CA, USAe-mail: slivingston@cds.caltech.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_16127
128 S. P . Chinchali et al.
Related work : Prior work has typically separately treated the problems of intent
decoding and strategy synthesis, which describes how to best use learned information
for planning future actions. In [ 16], this gap is partially bridged by modeling inter-
dependency of human-robot planning using Gaussian processes. Recently, Sadigh
et al. [ 15] show how a robotic car can identify whether nearby human drivers are
aggressive or cautious by nudging into adjacent lanes for information gain. Thoughpromising, the scheme does not account for safety constraints in probe selection and
assumes a static human driving style. A key motivation of our work is to incorpo-
rate safety constraints and anticipate a rich variety of human behaviors that maycontextually change based on robot interaction to enhance autonomy.
Proactive decision making can be cast as a Partially Observable Markov Decision
Process (POMDP) where the hidden mode of a human must be estimated by a robot,but POMDPs can only be solved efﬁciently for small problems [ 10,12]. Relevant
prior work using POMDPs includes hindsight optimization for grasping [ 6], inter-
active POMDPs (I-POMDPs) [ 5], and goal decomposition approaches [ 11]. Recent
work on temporal logic models highlights their ability to capture safety constraints
and high-level interactions [ 7,13,17]. In particular, [ 17] is especially relevant since
it casts human-robot interaction as an adversarial Markov Decision Process (aMDP)
where a robot must synthesize a strategy to maximize the probability of satisfying
a formal interaction speciﬁcation by reasoning over several possible environment(human) behavioral modes, which quickly becomes intractable for long-interaction
horizons with a plethora of plausible human models. Unlike [ 17], which simply
maximizes speciﬁcation satisfaction probabilities, we instead use an explicit rewardsignal in the aMDP formulation to incentivize information-seeking, proactive deci-
sion making. Further, we use temporal logic as a tool for dimensionality reduction
by distilling complex human-robot interactions into succint behavioral templates,which we learn from real driving data.
Statement of contributions : We signiﬁcantly reduce the computational complexity
of proactive decision making using a novel combination of formal methods and datamining. First, we show how to ﬁlter high-dimensional state trajectories into a concise
set of behavioral models expressed using temporal logic formulae. We construct
concise states as belief distributions over formal, symbolic models, as opposed tobeliefs over a much higher-dimensional set of state trajectories. Leveraging real
human driving data from the Stanford Drone Dataset (SDD) [ 14], we mine parameters
for temporal logic formulae that we select to be representative of key lane-mergingbehavior. Our framework, however, is general and can be extended to formulae that
are automatically learned from data. Based on these symbolic models, we synthesize
value iteration and reinforcement learning (RL) controllers that proactively probe
human intent for information gain while minimizing control cost. Simulated studies
of two robotic car scenarios, ranging from adversarial to cooperative, validate ourapproach, which we also characterize theoretically.
Paper organization : The rest of the paper is organized as follows. In Sect. 2,w e
introduce two motivating examples of proactive decision making. We then introducetemporal logic and our solution framework in Sect. 3. Next, we show how to reduce
problem complexity and provide a theoretical analysis of our solution framework in
Multi-objective Optimal Control for Proactive Decision Making … 129
Sect. 4. Sections 5and 6provide simulations from data-driven models from the SDD
and control strategies generated by both value iteration and reinforcement learning.
Finally, we provide concluding remarks in Sect. 7.
2 Examples of Proactive Decision Making
Throughout this paper, we refer to the following two examples of proactive decision
making to illustrate key technical concepts.
Example 1 (Cooperative lane-merging ) A robotic car must merge into a crowded
roundabout with pedestrians and bikers, such as in Fig. 1a from the SDD [ 14]. Such
examples of human-robot interaction are already starting to be addressed in industry.
For example, autonomous car startup drive.AI [8] has proposed to equip vehicles
with large signs that indicate when pedestrians can safely cross at intersections.
Inspired by drive.AI ’s proposal, the robotic car in our example can proactively instruct
pedestrians to wait, safely cross, or choose not to signal. Pedestrians obey or disobey
the robot’s safety indication and cross based on their observations of trafﬁc and
internal risk proﬁle (cautious or daring). The robot balances the cost of signaling,
which represents a risk probability of erroneously indicating safe conditions, with
exploitation of its current pedestrian model. Notably, we mine key temporal logic
formulae for this scenario from the SDD.
Example 2 (Adversarial car-pursuit )I nF i g . 1b, a robotic aid vehicle (green) is trans-
porting medical supplies in an urban warzone, where it might be followed by benign
civilian vehicles, an enemy surveillance car, or be directly chased by an enemy pur-
suer (red). If the robot proactively makes subtle route changes, it can differentiate
benign civilian cars from surveillance vehicles since it is highly improbable civilians
systematically follow the robot. Thus, the robot must balance exploration of follower
Fig. 1 Examples of Proactive Decision Making : (Left) A cooperative scenario, from the Stanford
Drone Dataset [ 14], shows how cars must nudge into crowded roundabouts. (Right) An adversarial
scenario, inspired by [ 4], shows how a green robotic car must safely swerve lanes to determine if it
is being pursued by a red adversarial car
130 S. P . Chinchali et al.
intent, which comes with a control cost of extra travel time and fuel, with exploitation
of its currently assumed model for safe delivery of supplies.
3 Proactive Decision Making Framework
In this section, we formulate the problem of proactive decision making with formal
methods. First, a deﬁnition is presented for sequences of interaction between multipleagents. Next, the speciﬁcation language used throughout the paper is introduced,
f o l l o w e db ya n adversarial Markov Decision Process (MDP) with labelings that
allow it to be evaluated with respect to a speciﬁcation. Finally, a problem is formulatedfor strategy selection and optimality of the adversarial MDP .
Consider a set of magents operating in discrete time. Let A={1,..., m}denote
the set of agents, Act =Act
1×···× Act mdenote their joint action space, and S
denote a joint state space.
Deﬁnition 1 Aninteraction sequence is a sequence of state-action pairs indexed by
time and denoted by
Ht(τ)=/bracketleftbig
(sτ,aτ), . . . , ( sτ+t−1,aτ+t−1),sτ+t/bracketrightbig
,
where Ht(τ)is said to begin at discrete time τand have duration t, and where
states are given by s∈Sand actions by a=(a1,..., am)∈Act. An interaction
sequence Ht(0)is called an interaction history and is also written Ht.
In the car-following example, the set of agents comprises the robot and follower,
states are the lane occupancies, and actions are probing route deviations.
3.1 Bounded Linear-Time Temporal Logic (BLTL)
In this paper, formal speciﬁcations of interaction sequences are deﬁned for ﬁnite
durations that are well-suited to information gathering tasks. We employ a fragment
of metric temporal logic (MTL) named bounded linear-time temporal logic (BLTL),
which was introduced in [ 4] and summarized below. The crux of BLTL is to ﬁrst
deﬁne the operator UI, where I=[a,b]is a bounded interval on the nonnegative
integers N, to express constrained reachability over ﬁnite durations of non-dense time.
For Boolean formulae fand gthat do not contain temporal operators, fU[a,b]gis
satisﬁed by a sequence σiffis true at each state beginning at time a, until a state
is reached where gis satisﬁed or the time bis reached. A key feature of all BLTL
formulae is that they can be decided using a ﬁnite sequence of timesteps, since allsuch intervals Iare bounded.
To reason about interaction sequences, we need a mechanism to check if high-
level logical formulae, constructed from a set of atomic propositions , are satisﬁed at
individual states. We denote a ﬁnite set of atomic propositions by/Pi1, where elements
of/Pi1are Boolean-valued variables that, at each discrete time, evaluate to either True
Multi-objective Optimal Control for Proactive Decision Making … 131
orFalse . Atomic propositions associated with a self-driving car might include
Y={Cx}X
x=1to indicate the robot occupies lane xofXtotal lanes.
BLTL syntax over interval Iis given by the context-free grammar
ϕ:: =True |p|¬ϕ|ϕ∧ϕ|/circlecopyrtϕ|ϕUIϕ, (1)
where pis an atomic proposition p∈/Pi1. Here, atomic propositions pcan be com-
bined to describe logical formulae ϕby using standard logical connectives such as
conjunction ( ∧), disjunction ( ∨), negation ( ¬), and implication ( =⇒), coupled with
temporal operators such as eventually ( /squareI), always ( /squareI), and until ( UI). The con-
nective /squareIϕmeans that ϕis true at all positions of the word in the interval Iof time
steps; the connective /squareIϕmeans that ϕeventually becomes true within a ﬁnite time;
the connective ϕ1Uϕ2means that ϕ1has to hold at each position in the word, at least
until ϕ2is true in interval I. Signiﬁcant expressivity can be achieved by combining
temporal and Boolean operators in BLTL.
For a search and rescue mission triggered by a ﬂare, the operational behavior
“once a ﬂare is lighted, always a drone is dispatched until a human-operated rescue
helicopter arrives within interval Th” can be expressed as:
flare =⇒ (drone U[0,Th]helicopter ).
3.2 High-Level Agent Intent Models
Henceforth, we treat the case of two interacting agents—an autonomous system that
we control (denoted by AS), and an adversary denoted by adv . To reason about
their interaction, we use labelled adversarial Markov Decision Processes (aMDPs) ,
which are deﬁned similarly as in [ 17].
Deﬁnition 2 Alabelled adversarial MDP M is a tuple (S,Init,Actc,Actu,P,
/Pi1,L), where Sis a ﬁnite set of states, Init ⊆Sis a set of possible initial states,
Actcis a mapping from states into sets of controlled actions ,A c tuis a mapping
from states into sets of uncontrolled actions (oradversarial actions ),/Pi1is a ﬁnite
set of atomic propositions, the labelling function L:S→2/Pi1maps states to atomic
propositions, and P:S×Actc×Actu×S→[ 0,1]deﬁnes transition probabilities
where for each state s∈S,a∈Actc(s), and b∈Actu(s),/summationtext
s/prime∈SP(s,a,b,s/prime)=1.
Assumption 1 For every state s∈S,A c tc(s)/negationslash=∅ and Actu(s)/negationslash=∅.
Intuitively, this assumption stipulates that no dead-ends exist.
3.3 Probability of Satisfaction and Agent-Intent Models
Let M be a labelled adversarial MDP . A strategy is a partial function from
Hist(M,T)to exactly one of the two action sets associated with M:A c tc,A c tu.
132 S. P . Chinchali et al.
Letπbe a strategy mapping into Actc, and let μbe a strategy mapping into Actu.
The set of interaction histories consistent with these strategies is deﬁned by
Hist(M,T,π,μ ) =/braceleftbig
HT|s0∈Init∧
∀τ< T:P(sτ,π(Hτ),μ(Hτ),sτ+1)> 0/bracerightbig
. (2)
Each interaction history HT∈Hist(M,T,π,μ ) deﬁnes a ﬁnite sequence of
states and actions. From ( 2), it follows that HTis equivalent to a ﬁnite sequence
of state-action-state triples, each of which is a transition of M. Recall that the prob-
ability of each transition is deﬁned by P. Computing the product of these probabilities
for each element of Hist (M,T,π,μ ) and normalizing provides a probability mass
function over Hist (M,T,π,μ ) .
For sufﬁciently large T, it can be decided for each element of Hist (M,T,π,μ )
whether it satisﬁes the BLTL formula ϕ, hence we deﬁne
PM,π,μ(Init|=ϕ) (3)
as the probability of satisfying ϕwhen ASstrategies and adversarial strategies are
applied to M. When Init is a singleton, we write PM,π,μ(s|=ϕ).
Deﬁnition 3 Anagent-intent model is a pair (M,ϕ), where Mis a labelled adver-
sarial MDP and ϕis a BLTL formula.
The following assumption states that the adversary, if following model (M,ϕ), will
select a strategy that maximizes the probability of satisfying ϕ. Such an assumption
is natural since the adversary’s own strategy must be as concordant as possible withits true speciﬁcation, but we allow for unbiased measurement uncertainty etc.
Assumption 2 Let(M,ϕ) be an agent-intent model. For any s∈Init, the adver-
sarial strategy is from the set
arg max
μmin
πPM,π,μ(s|=ϕ),
where PM,π,μ(s|=ϕ)is the probability that the labeled Markov chain induced by
ﬁnite-memory strategies πandμand with initial state ssatisﬁes ϕ.
3.4 Proactive Decision Making with BLTL Constraints
We now introduce the problem of Proactive Decision Making with BLTL formulae
and adversarial MDPs. It involves interaction between an autonomous system (AS)and an adversary, referred to as adv . The AS must ﬁnd a control policy π
ASthat max-
imizes reward Rdespite possible interference from the adversary enacting controller
πadv. Reward function R:Ht/mapsto→ Rdeﬁnes a scalar reward over interaction history
Multi-objective Optimal Control for Proactive Decision Making … 133
Htwhich incentivizes the AS to learn about the true model (Mj,ϕj)of agent adv
while minimizing the cost of information gain. Interaction histories must satisfy goal
and safety speciﬁcations encoded in BLTL formula ϕ.
Problem 1 (ProDM-BLTL: Proactive Decision Making with BLTL ) Given a ﬁnite
set of agent-intent models {(M1,ϕ1), . . . , ( MN,ϕN)}, ﬁnd strategy π∗
ASsuch that
π∗
AS∈arg maxπAS/parenleftbigg
min
πadvEM j,πAS,πadv(R(HT))/parenrightbigg
where ground-truth model jis ﬁxed and known to the agent adv but is unknown to
the AS, and where πadv∈arg maxμPM j,πAS,μ(Init|=ϕj).
In terms of the car-following example, Ris a weighted sum of fuel and lane devia-
tion cost coupled with a metric of information gain. The cost in Rcould also include
distance to a destination, incentivizing goal-driven behavior. Speciﬁcations ϕgovern
how different follower car models pursue the robot, while interaction sequences HT
are joint robot-follower lane trajectories.
4 Dimensionality Reduction and Solution Algorithm
Problem 1is intractable because, as a consequence of not knowing the ground-
truth index j, we must optimize over all possible agent-intent models (Mi,ϕi)and
interaction sequences Ht. In this section, we present, solve, and theoretically analyze
a new problem based on complexity reductions from Problem 1.
4.1 Request-Response Formulae and Satisfaction Bitvectors
For a special form of BLTL formulae that is deﬁned below, we are able to reduce the
size of the action space of the abstract system. Let {(M1,ϕ1) ,...,( MN,ϕN)}be a
set of agent-intent models (recall Deﬁnition 3in Sect. 3.3).
Deﬁnition 4 (BLTL request-response formula ) The speciﬁcation ϕifor agent-intent
model iis said to be a BLTL request-response formula if it has the form
ϕi=/logicalanddisplay
p∈{1,...,pi}/parenleftBig
ψreq,p
AS,i=⇒/square[0,Tp]ψres,p
adv,i/parenrightBig
=/logicalanddisplay
p∈{1,...,pi}ϕp
i (4)
where each mode prepresents an AS (robot) probe to agent igiven by ψreq,p
AS,i,
ψres,p
adv,iis the adversary’s informative response , and Tpis the maximum duration to
wait for the response.
BLTL request-response formulae are practically-motivated because many real-life
information-gathering tasks generate a response within a bounded time. Autonomous
134 S. P . Chinchali et al.
cars must wait a ﬁnite duration for pedestrians or anomalous trafﬁc patterns to reach
normal states. Deadlock situations, such as a pedestrian waiting inﬁnitely long at
an intersection, rarely persist indeﬁnitely. Even if they do, AS, such as search andrescue drones, can employ a ﬂexible approach where they move to a new search
location after a timeout but can revisit target sites. In the car-following example, a
request is a lane-change and an informative response is for an adversary to followthe robot within a bounded interval T
p. In the deﬁnition of belief MDP given later
in this section, request-response formulae allow us to deﬁne an abstract “impulse”
action, thus reducing the effective space of possible actions for policy selection.
Given our goal of reasoning about likelihood among agent-intent models, the
effective state space for planning can be reduced by abstracting it into outcomes of
interaction, which we now deﬁne.
Deﬁnition 5 (Satisfaction bitvector observation ) Consider Nhigh-level agent intent
models Mi=(Mi,ϕi)(Deﬁnition 3in Sect. 3.3) where each speciﬁcation ϕihas
piBLTL request-response formulae. Deﬁne Q=/summationtext
ipi, and T=max iT(ϕi).T h e
satisfaction bitvector observation o is a function that maps an interaction history
HTto a vector of dimension Qwhere the qth element is 1 if HTsatisﬁes the qth
interaction formula ϕq=ϕp
iand is otherwise 0. Observation space O={o|o∈
{0,1}Q}comprises 2Qpossible bitvectors. Written in terms of the indicator function
1,
o(HT)=[1(HT|=ϕ1
1), . . . , 1(HT|=ϕpN
N)]. (5)
4.2 Reduced ProDM-BLTL Problem with Belief MDPs
Leveraging the previous deﬁnitions, we reduce the complexity of Problem 1by
constructing a new reward function and belief MDP (deﬁned below) where statesare belief distributions over the Ncandidate agent-intent models, actions are BLTL
requests, and transitions depend on satisfaction bitvector observations.
The belief MDP of a set of candidate agent-intent models is deﬁned as ˜M=
(˜B
0,˜B,˜A,Pb,τb)with the following components:
1.˜B={B∈RN|B(i)≥0∧/summationtext
iB(i)=1}.
Belief states are probability distributions over the Ncandidate models.
2.˜A={ψreq,p
AS,i|i∈{1,..., N},p∈{1,..., pi}}.
Abstract (impulse) actions are controlled requests for any BLTL request-response
formula.
3.Pb:˜B×˜A×˜B→ R
Belief transition function Pbis deﬁned as follows, where ok=o(HT(tk))is a
satisfaction bitvector observation (Deﬁnition 5in Sect. 4.1).
Multi-objective Optimal Control for Proactive Decision Making … 135
Pb(Bk,ak,Bk+1)=Pr(Bk+1|Bk,ak)
=/summationdisplay
ok∈OPr(Bk+1|Bk,ak,ok)N/summationdisplay
i=1Pr(ok|ak,Mi)Bk(i)
where
Pr(ok|ak,Mi)=Q/productdisplay
q=1/parenleftBig
oq
kPr(HT(tk)|=ϕq|ak,Mi)
+(1−oq
k)Pr(HT(tk)/negationslash|=ϕq|ak,Mi)/parenrightBig
, (6)
where ok=(o1
k,..., oQ
k)andϕqwas in Deﬁnition 5. Notice that Pr (ok|ak,Mi)
is the joint probability of satisfaction or not, depending on respective elements of
the bitvector ok, for each agent-intent model, given that the true model is (Mi,ϕi).
4. The belief update function τb:˜B×˜A×O→˜Bmaps belief state Bk, action ak,
and resulting observation okto new belief vector Bk+1by Bayes’ Rule. The belief
update depends on the probability of observation okgiven an informative impulse
akunder each competing model Mi, i.e., Bk+1=τb(Bk,ak,ok)where
Bk+1(i)=ηBk(i)Pr(ok|ak,Mi), (7)
andηis a normalization factor.
5. The initial state is a uniform probability distribution over Npossible models, i.e.,
˜B0=B0=[1
N,...,1
N].
6. The stage reward is a weighted sum of control cost and information gain, where
information gain is measured as entropy reduction in the belief vector
RH
k(Bk,ak,Bk+1)=−αc(ak,Bk)+β[H(Bk)−H(Bk+1)]. (8)
Here, c:˜A×˜B→ Ris the control cost function, His the Shannon entropy, and
α> 0,β > 0 weight the control cost and information gain.
A transition of ˜Moccurs in two parts starting at an iteration k. First, we start at
a belief state Bkat time tkwhen the fully observable aMDP state is stk. An action
is selected as an impulse atk, corresponding to a speciﬁc BLTL request-response
formula for some model iand high-level mode p. By selecting this action, we can
assume model iis the true model and synthesize a strategy corresponding to aMDP
Miand interaction formula ϕi, using Assumption 2in Sect. 3.3.
Second, a ﬁnite-horizon play occurs, resulting in a ﬁnal state stk+T(ϕ)of the aMDP
and an interaction sequence from tkof length T(ϕ),HT(tk). The interaction sequence
can then be mapped to a satisfaction bitvector observation ok=o(HT(tk)).G i v e n
current belief state Bk, action ak, and satisfaction bitvector observation ok, we can
construct a new belief state Bk+1by Bayes’ Rule. From new belief state Bk+1and
underyling aMDP state stk+T(ϕ), the process repeats.
We can now present the ProDM-BLTL problem solved in this paper.
136 S. P . Chinchali et al.
Problem 2 (Belief-ProDM: ProDM-BLTL with Belief MDPs )L e t{(M1,ϕ1) ,...,
(MN,ϕN)}be a set of agent-intent models, and let (˜B0,˜B,˜A,Pb,τb)be the belief
MDP corresponding to it. Given the reward function RH
kin (8) and discounting factor
γ< 1, and denoting AS control policies by ξ,s o l v e
max
ξlim
K→∞E/parenleftBiggK/summationdisplay
k=0γkRH
k(Bk,ak,Bk+1)/parenrightBigg
such that ok=o(HT(tk))
Bk+1=τb(Bk,ak,ok)
for all k=0,..., K−1
4.3 V alue Iteration on BLTL Trees
A key challenge in solving Problem 2is that there are inﬁnite belief states Bk.
However, initial belief B0is always uniform, and there are a ﬁnite number of impulses
ak∈˜A. Notably, there are 2Qpossible observations ok∈O, which is much smaller
than the number of interaction sequences of length T(ϕ).T h u s ,a ta n yi t e r a t i o n k,
there are a ﬁnite number of possible next belief states Bk+1.
Hence, we solve Problem 2in a receding horizon fashion, where at iteration k
we construct a policy tree [ 2] starting at initial belief Bk. Each node in the tree is
an impulse ak, leaves are observations ok, and subsequent nodes are actions ak+1
etc. Using value iteration on such policy trees with a horizon of Hactions, we can
select an optimal sequence of impulses aktoak+H, and enact the plan depending on
observations ok. Finally, at belief node Bk+Hwe construct a new tree and optimal
policy, which avoids optimizing over all possible belief vectors in ˜B.
4.4 Size of BLTL Trees
Satisfaction bitvectors provide signiﬁcant reduction in the number of BLTL trees.
The set of policy trees with |Actc|actions, |O|observations, and a horizon Hhas
size|Actc||O|H−1
|O|−1trees [ 2]. We ﬁrst calculate the number of BLTL trees. The size of
˜A, the set of all impulses, is |˜A|=Q=/summationtext
ipiconsisting of all possible modes pi
for each model i. There are also 2Qtotal observation bitvectors in O, leading to
|˜A||O|H−1
|O|−1=Q2QH−1
2Q−1 (9)
trees. We now consider the number of trees if we used impulse actions, but observa-
tions were individual interaction histories of length T(ϕ). Since the AS only acts with
an impulse, we need not look at all possible actions in the interaction sequence, but
Multi-objective Optimal Control for Proactive Decision Making … 137
rather just look at all possible state trajectories. Let the set of all observation histories
of duration T(ϕ)to be ˜HT(ϕ), where |˜HT(ϕ)|=| S|T(ϕ). Then, we must optimize over
|˜A|(|S|T(ϕ))H−1
|S|T(ϕ)−1=Q(|S|T(ϕ))H−1
|S|T(ϕ)−1 (10)
trees, which scales with the size of state space |S|, impulse bound T(ϕ), and horizon
H. Notably, the BLTL tree size with bitvectors only scales with the size of a smaller
set of temporal logic formulae and horizon H. In Sect. 6, we show how a concise set
of BLTL trees allows us to solve otherwise intractable problems.
4.5 Analysis
Theorem 1 (convergence) Suppose that (M1,ϕ1)is the ground-truth agent-intent
model (without loss of generality, modulo permutation of indices of candidate mod-
els). Furthermore, suppose there is some a ∈˜A such that each j /negationslash=1,
Pr(o|a,M1)/negationslash=Pr(o|a,Mj) (11)
for some observation o. Then, for each optimal policy ξthat solves Problem 2with
α=0, there is a time τsuch that for all time k ≥τ, the belief vector B khas maximum
value in the ﬁrst position and no others, i.e., B k(1)> Bk(j)for j/negationslash=1.
Due to space constraints, we only provide a proof sketch here. Full proofs are in an
online supplement.1Because the agent-intent model (M1,ϕ1)is the ground-truth by
hypothesis, the true probability density function according to which observations are
sampled is Pr (o|a,M1)for each abstract action a. As such, ( 11) implies that there
is an abstract action such that every other candidate agent-intent model would yieldobservation vectors that differ in their expected frequency compared to the ground-
truth (i.e., model 1). If α=0i n( 8) (zero control cost), then from ( 7) it follows that
the belief will tend to accumulate mass in position 1, i.e., for the ground-truth.
When the ground-truth stochastic dynamics or behavior speciﬁcation is not in the
set of candidate agent models, we may want the belief distribution to indicate when
there is no match. Intuitively, the ﬁtting process of belief states attempts to ﬁt thedistributions of satisfaction bitvectors per agent-intent model to the data. Thus, if
none of the candidates is a good match, the belief will remain with high entropy and
the AS can enact a conservative control policy.
1Available at https://asl.stanford.edu/publications or directly, https://asl.stanford.edu/wp-content/
papercite-data/pdf/Chinchali.Livingston.Pavone.ISRR17.pdf .
138 S. P . Chinchali et al.
5 Example Scenarios
In Sect. 5.1, we provide BLTL formulae for adversarial car-following. Then, in
Sect. 5.2, we show our approach extends to real SDD driving data. Finally, in
Sect. 5.3.1 , we introduce control costs that decrease as a robot gains certainty over
human models, serving as a key contribution for modeling dynamic human-robot
cooperation.
5.1 Robotic Car-Following BLTL Speciﬁcations
A robot transporting medical supplies must make subtle, costly route deviations todiscern the nature of followers (Fig. 1). The robot’s lane occupancy in one of X
total lanes is denoted by system variables Y={C
1,..., CX}. Likewise, the fol-
lower’s lane is denoted by environment variables X={F1,..., FX}, leading to
overall state st=[Cx,tFx,t]at time t. Follower models are referred to as pursuant ,
surveil z(for a surveillance car), or benign for a civilian. The surveil zcar
represents surveillance behavior since it must always stay within zlanes of the robot,
yet is allowed the ﬂexibility of Tztime steps to do so. We expand the BLTL formulae
ϕfrom [ 4] by adding control costs and more surveillance cars:
1.ϕbenign =True , i.e., civilian cars have no temporal logic constraint.
2.ϕsurveil z=/logicalandtext
x∈{1,...X}/square/parenleftbig
Cx=⇒/square[0,Tz]Fx−z∨Fx−z+1∨···∨ Fx+z/parenrightbig
3.ϕpursuant =/logicalandtext
x∈{1,...,X}/square/parenleftbig
Cx=⇒/square[0,Tpursuant ]Fx/parenrightbig
with time bound Tpursuant
The robot can probe the follower by changing lanes which incurs a lane-deviation
and fuel cost of c(at)=1. Staying in the same lane incurs no cost. By proactively
changing lanes, the robot learns if adversarial cars will eventually follow it to satisfyformulae of the form 2 and 3.
5.2 Stanford Drone Dataset (SDD)
The SDD [ 14] is a series of trajectories of bikers, pedestrians, and low-speed ser-
vice carts that navigate crowded scenes on Stanford University’s campus. Speed
limits and restrictions on outside trafﬁc make the campus a prime testing ground for
autonomous vehicles. Though the dataset features entirely human-human interac-tions of vehicles and pedestrians, we use it to mine speciﬁcation parameters on safe
interaction between agents to guide future autonomous vehicle design. Speciﬁcally,
we consider how low-speed robotic carts that transport supplies across campus mightmerge into crowded roundabouts such as in Fig. 1if they were allowed to proactively
signal their merging intent. We note that potential cart-pedestrian accidents are only
minorly injurious due to speed restrictions. Indeed, the SDD shows carts aggressively
approaching pedestrians and the annotations and students alike jokingly refer to the
Multi-objective Optimal Control for Proactive Decision Making … 139
roundabout in Fig. 1aa st h e death circle only since it has notcaused major accidents.
Thus, a robot can plan proactive merging strategies.
We performed extensive quality-control of over 69 GB of SDD data to identify
trajectories where pedestrians and carts respond to rapidly changing trafﬁc densities
and intra-agent distances to merge or cross at opportune times (Fig. 2).
5.3 Car-Merging BLTL Speciﬁcations
Let˜A={s,u,n}denote the cart’s safe,unsafe , and no signal control actions,
respectively. Cross trafﬁc density ρ∈Ris deﬁned as the number of pedestrians per
frame that travel perpendicular to the cart’s merging direction. State S=[ρ,d,v]
incorporates trafﬁc density ρ, the cart’s speed v∈R, and its distance to a clos-
est pedestrian d∈R, as exempliﬁed in Fig. 2b. Pedestrian models are denoted by
m∈{cautious ,daring }. Boolean variables ˜ρ∈{h,l}indicate if trafﬁc is heavy
(h=ρ>ρ 0) or light ( l=ρ<ρ 0), where ρ0is a density threshold.
Given a trafﬁc density ˜ρand safety indication a, a pedestrian of model mwill not
cross for a time Tm
˜ρ,ato safely assess their surroundings, and after may cross based on
their internal risk proﬁle. Such behavior is captured by formula ϕm
˜ρ,aand an example
for a daring pedestrian during heavy trafﬁc ( ρ>ρ 0) after the robot indicates safe is:
ϕdaring
h,s=/square[(ρ > ρ 0)/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
heavy∧safe/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
impulse]= ⇒¬ cross U[0,Tdaring
h,s]True
/bracehtipupleft /bracehtipdownright/bracehtipdownleft /bracehtipupright
can only cross after wait-time(12)
The formula for a model mhuman covers all trafﬁc scenarios ˜ρand signals a:
ϕm=/logicalanddisplay
a∈˜A/logicalanddisplay
˜ρ∈{h,l}ϕm
˜ρ,a (13)
Crucially, BLTL time-bounds Tm
˜ρ,aallow the robot to differentiate models based
on their crossing probabilities. For example, Tdaring Tm
˜ρ,afor any model m, trafﬁc condition ˜ρ, and safety signal
ato assess the pedestrian’s response. Suppose the agent indicated safe during very
heavy trafﬁc since it could not merge, yet it observed ¬cross after T(ϕ). Since
the daring pedestrian is only constrained to not cross for Tdaring
h,s<< T(ϕ),t h e
probability of observing ¬cross in a long interval might indicate a cautious model.
5.3.1 Robot Behavior and Dynamic Control Costs
In the following speciﬁcations, density and distance thresholds ρ0,d0, and speed
multipliers M,Lare mined from data.2We can learn such parameters automatically
2SDD provides annotations in terms of video frames and pixel distances, without calibration data.
As such, recovering metric distances was infeasible, so we omit the values of these parameters.
140 S. P . Chinchali et al.
0200 400 600 800 1000 1200 14000500100015002000
Biker, Pedestrian
Car02468ρ
024681012speed v
6500 6550 6600 6650 6700 6750 6800 6850
video frame0100200300400d
Fig. 2 (Left) Aerial view of an SDD scene where a car (bold) rapidly accelerates to merge into a
roundabout. (Right) The car rapidly accelerates, reﬂected in speed v,w h e nt r a f ﬁ c ρsubsides after
video frame 6700. Video frames are used as a proxy for continuous time, which is not reported
by contrasting velocity and trafﬁc distributions during merging and steady-driving
scenarios to ﬁnd separating thresholds. Formulae 1 and 2 capture scenarios like Fig. 2,
where a cart cannot signal safe as it attempts to merge:
1.ϕslow=/square[(ρ > ρ 0)/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
cross-trafﬁc∧(dd0)/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
biker far∧(v=v0)]= ⇒ /square[0,Tmerge][(v≥Lv0/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
accelerate)∧ ¬safe/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
disallow cross]
Control costs capture the risk of a worst-case scenario where the cart causes an
accident after indicating safe ,s oc(s,Bk)>c(u,Bk)>c(n,Bk)for all k. Since the
risk may decrease as the agent is more certain about the true pedestrian model, we
also have c(a,Bk)∝H(Bk)for any action a.
6 Simulation Results
In Sect. 6.1, we show how dimensionality reduction allows us to solve an otherwise
intractable belief MDP using value iteration. Then, in Sect. 6.2, we leverage advances
in deep RL to scale our framework to larger problems with several competing models
or cases where the ground truth model is not in the candidate set.
6.1 V alue Iteration Tree Results
Figure 3shows a proactive car-merging strategy solved by BLTL tree value iteration.
As introduced in Sect. 5.3.1 , control costs capture model uncertainty and risk proba-
bilities, so c(a,Bk)=c0(a)
2(1+|H(Bk)
H(B0)|)where co(safe)=0.40,c0(unsafe )=0.30,
Multi-objective Optimal Control for Proactive Decision Making … 141
0 20 40 60 80 100
Iteration k0.00.51.01.52.0Action ak, Trafﬁc ˜ρtrue = cautious, α=0 .7, β=1 .0
noSignal: 0 , unsafe: 1 , safe: 2
ρ>ρ 0:1, ρ<ρ 0:0
0 20 40 60 80 100
Iteration k0.00.20.40.60.81.0Cost c(ak,B k), Belief Bktrue = cautious, α=0 .7, β=1 .0
c(safe ,B k)
c(noSignal ,B k)
c(unsafe ,B k)
Bk(cautious)
Bk(daring)
Fig. 3 (Left) In car-merging, the cart repeatedly signals unsafe to merge only after trafﬁc ρ
subsides. (Right) Control costs decrease as the cart quickly identiﬁes the true cautious pedestrian
andc0(no signal )=0.3The cart initially chooses the high-cost, but most informative
saf e action when trafﬁc ρis heavy and precludes merging. As the trafﬁc subsides,
the cart chooses the lower cost, but still informative unsaf e signal since it can now
merge. Belief Bkindicates we correctly identify the true cautious pedestrian model
and cost of informative controls decays over time as we increase model conﬁdence
(Fig. 3b). Eventually, the agent chooses the zero cost no signal action once it has
high model conﬁdence and has already merged.
Using bitvector observations instead of individual interaction histories allows
tractable value iteration. Even a simple car-following scenario with 4 lanes and
time-bound of T(ϕ)=5 steps would have |S|=42=16 joint robot-follower lane
occupancies and 45=1024 possible trajectories for the robot alone. If we consider
only 3 BLTL formulae for pursuit, surveillance, or civilian behavior, 3 possible con-
trol actions ˜A={left,right,stay}, and a horizon of H=2 impulses, there would be
an integer overﬂow number of trees using interaction histories on a 64-bit computer.
Notably, with bitvector observations, we only have a tractable 243 trees, since the
observation space does notscale with the lane count or T(ϕ).
6.2 Reinforcement Learning (RL) Results
For complex problems with several candidates or an unanticipated model, enumerat-
ing even high-level observations and their probabilities is infeasible. Thus, we train
an AS to tradeoff costly exploration with exploitation of the most likely model, with-
outexplicitly knowing model observation probabilities. Such a setting is a hallmark
of RL, so we train an RL agent in a series of training episodes of Kiterations. Each
episode starts from a uniform belief B0and at step k, the agent chooses informative
probe akbased on current belief state Bkusing parametrized control policy πθ(Bk).
The environment generates observations okunder true model M1and provides a new
3This is just one representative example of control costs allowed by our general framework.
142 S. P . Chinchali et al.
0 5 10 15 20
Iteration k0.00.20.40.60.81.0Belief Bktrue = z1,α=0 .15 , β=1 .0
benign
pursuer
0 2 4 6 8 10 12 14 16
Training episodes0.00.20.40.60.81.01.2Scaled rewardtrue = z1,α=0 .15, β=1 .0
KL
entropy
Fig. 4 (Left) In the car-following scenario solved with RL, both KL and entropy rewards lead to
correct model identiﬁcation, but the KL trained policy (solid) identiﬁes true model surveil z1
with higher certainty. (Right) Normalized RL learning curves for both reward functions indicates
KL converges faster with lower variance
belief vector Bk+1and reward rk+1to the agent, since the agent cannot compute the
belief update itself without model probabilities.
We developed simulators for both examples using the openAI gym framework
[3]. We used Google’s Tensorﬂow [ 1] to learn a stochastic control policy using the
Actor-Critic (AC) RL learning algorithm [ 9], where the policy πθ(Bk)is encoded
in a neural network with parameters θof 1 hidden layer of 50 units. Our general
framework allows for deeper neural networks to be used for problems with more
complex human-robot interaction models. Figure 4b illustrates model convergence,
where the shaded area shows the variance of test episode rewards when the network
policy is paused periodically to evaluate learning.
6.3 RL Reward Structures
In addition to the entropy based reward from the belief MDP setting (Eq. 8), we
can formulate a reward that penalizes the KL divergence between the true model
“one-hot” vector ¯B=[1,0,... 0]and the current belief. The following reward is
only appropriate in the RL scenario where the environment simulator knows the true
model and incentivizes the agent to learn the ground-truth:
rKL
k(Bk,ak,Bk+1)=−αc(ak,Bk)−βKL(Bk+1,¯B), (14)
where KL is the Kullback–Leibler divergence, c:˜A×˜B→ Ris the control cost
function, and α> 0,β > 0. Since the KL divergence is always positive, we weight
by−βto penalize excessive differences between Bkand¯B.
Interestingly, for a wide spectrum of weights α, β , the policy learned under the KL
reward converged faster than the entropy reward for the same experimental settings
(Fig. 4b). Further, in a single test episode of Kiterations, the KL-learned policy
Multi-objective Optimal Control for Proactive Decision Making … 143
led the agent to identify the true model with more certainty (Fig. 4a). Intuitively, if
the expected future entropy reduction is lower than the cost of informative probes,
the agent will stop probing but incur zero future reward since the belief vector willsaturate. However, the KL reward converges better since it continually penalizes KL
divergence between the current belief and true distribution throughout the episode,
incentivizing longer exploration to reduce uncertainty. As often seen in practicalRL, many trials of the entropy policy showed it does not learn for several initial
episodes as it explores sub-optimal policies, but sees a sudden performance jump as
it discovers a better set of policy parameters (Fig. 4b).
7 Conclusion
In this paper, we couple formal methods with data-driven learning to provide
a tractable framework for proactive decision making. Formal methods are used
to extract meaningful symbolic interaction templates from complex interaction
sequences, such as traces of real human driving data in the SDD. Leveraging advances
in deep RL, we then synthesize information-seeking controllers and provide a theo-
retical analysis of their ability to distinguish models.
Future work centers on conducting experimental user studies where a simulated
autonomous cart signals its merging intent to human pedestrian subjects using a
ProDM scheme, allowing us to explicitly probe for human risk proﬁles. Such experi-
ments will likely require us to expand our speciﬁcation mining approach to automati-
cally derive both the speciﬁcations and associated parameters from high-dimensional
datasets. In a real deployment, we would also need to couple such automaticallyderived speciﬁcations with a safety mechanism that defaults to a conservative AS
policy if experimental data deviate signiﬁcantly from learned models. To solve prob-
lems with a larger spectrum of agent types, we will use shared generative models,such as pre-trained neural networks that capture general driver behavior.
As robots cooperate with humans on increasingly complex tasks, techniques that
distill a continuum of high-dimensional interaction sequences into core essentialtemplates of interaction will be evermore indispensable. Such a holistic approach to
robot task planning may one day allow robots to effectively cooperate with humans
in diverse settings ranging from factory assembly lines to freeways.
References
1. Abadi, M., Barham, P ., Chen, J., Chen, Z., Davis, A., Dean, J., Devin, M., Ghemawat, S., Irving,
G., Isard, M., et al.: Tensorﬂow: A system for large-scale machine learning. In: Proceedings ofthe OSDI (2016)
2. Braziunas, D.: POMDP solution methods. Technical report, Department of Computer Science,
University of Toronto (2003)
144 S. P . Chinchali et al.
3. Brockman, G., Cheung, V ., Pettersson, L., Schneider, J., Schulman, J., Tang, J., Zaremba, W.:
OpenAI gym. https://arxiv.org/abs/1606.01540 (2016)
4. Chinchali, S.P ., Livingston, S.C., Pavone, M., Burdick, J.W.: Simultaneous model identiﬁcation
and task satisfaction in the presence of temporal logic constraints. In: Proceedings of the IEEEConference on Robotics and Automation (2016)
5. Gmytrasiewicz, P .J., Doshi, P .: A framework for sequential planning in multi-agent settings. J.
Artif. Intell. Res. 24, 49–79 (2005)
6. Javdani, S., Srinivasa, S.S., Bagnell, J.A.: Shared autonomy via hindsight optimization. In:
Robotics, Science and Systems (2015)
7. Jones, A., Schwager, M., Belta, C.: Information-guided persistent monitoring under temporal
logic constraints. In: American Control Conference, pp. 1911–1916 (2015)
8. Knight, W.: New self-driving car tells pedestrians when it’s safe to cross the street. MIT
Technology Review (2016)
9. Konda, V ., Tsitsiklis, J.: Actor-critic algorithms. In: Advances in Neural Information Processing
Systems (2000)
10. Madani, O., Hanks, S., Condon, A.: On the undecidability of probabilistic planning and inﬁnite-
horizon partially observable markov decision problems. In: Proceedings of the AAAI Confer-
ence on Artiﬁcial Intelligence (1999)
11. Nguyen, T.-H.D., Hsu, D., Lee, W.-S., Leong, T.-Y ., Kaelbling, L.P ., Lozano-Perez, T., Grant,
A.H.: Capir: collaborative action planning with intention recognition. In: Seventh ArtiﬁcialIntelligence and Interactive Digital Entertainment Conference (2011)
12. Papadimitriou, C.H., Tsitsiklis, J.N.: The complexity of markov decision processes. Math.
Oper. Res. 12(3), 441–450 (1987)
13. Raman, V ., Donzé, A., Sadigh, D., Murray, R.M., Seshia, S.A.: Reactive synthesis from signal
temporal logic speciﬁcations. In: Hybrid Systems, Computation and Control (2015)
14. Robicquet, A., Sadeghian, A., Alahi, A., Savarese, S.: Learning social etiquette: Human tra-
jectory understanding in crowded scenes. In: European Control Conference (2016)
15. Sadigh, D., Sastry, S.S., Seshia, S.A., Dragan, A.: Information gathering actions over human
internal state. In: IEEE/RSJ International Conference on Intelligent Robots & Systems (2016)
16. Trautman, P ., Krause, A.: Unfreezing the robot: Navigation in dense, interacting crowds. In:
IEEE/RSJ International Conference on Intelligent Robots & Systems (2010)
17. Wongpiromsarn, T., Frazzoli, E.: Control of probabilistic systems under dynamic, partially
known environments with temporal logic speciﬁcations. In: Proceedings of the IEEE Confer-ence on Decision and Control (2012)
T owards Reactive Control of Transitional
Legged Robot Maneuvers
Jeffrey Duperret and Daniel E. Koditschek
1 Introduction
Roboticists are becoming increasingly successful at both implementing [ 1–3] and
analyzing [ 4] dynamic steady-state legged robotic tasks such as running. Underlying
this success is the mathematical understanding that many useful steady-state behav-
iors can be encoded as stable limit cycles of the controlled hybrid dynamics of the
system. The dynamical systems literature [ 5] has given engineers a wealth of tools
for encoding and controlling the asymptotic properties of such systems—for exam-
ple, to recast the problem of generating stable locomotion as ﬁnding a control whichbrings the Jacobian of an appropriate Poincaré map of the system to have eigenvalues
with magnitude less than unity at a designated ﬁxed point.
In contrast, tasks such as leaping, turning, and dodging have no well-agreed upon
mathematical description that lends itself to synthesis. These tasks—which we refer
to as transitional and intuitively think of as exhibiting agility—are inherently not
steady-state but represent, nevertheless, a canonical motivating setting for leggedlocomotion, for example in operation over unstructured or irregular terrain such as
leaping from foothold to foothold in rubble during disaster response. The inher-
ently transient nature of these maneuvers does not seem well-suited to straightfor-ward encoding via asymptotic dynamical properties and seems to call for alternative
formulations.
Several techniques exist in the literature for achieving transitional tasks in hybrid
systems, albeit for limited classes of models. Our focus on hybrid systems is moti-
vated by the intrinsically multifarious and abruptly changing leg-ground mechanics
associated with making and breaking contact during stance and recirculation. Implicitmodel predictive control [ 6,7] has been used to great effect, for example, by recourse
J. Duperret ( B)·D. E. Koditschek
Department of Electrical and Systems Engineering, University of Pennsylvania,200 South 33rd Street, Philadelphia, PA 19104, USAe-mail: jdup@seas.upenn.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_17145
146 J. Duperret and D. E. Koditschek
to quadratic programming [ 8] over a ﬁnite horizon for leaping over obstacles. Such
methods recompute a solution at every step even in a static environment and are
generally sensitive to their cost functions, which often are constrained to take anartiﬁcial form due to the optimization methods available and may not readily express
the designer’s underlying intent or handle gracefully the inherent nonlinearity of a
given problem. Exciting work in sum-of-squares veriﬁcation [ 9,10] allows reactive
local controllers via Lyapunov functions but thus far appear to be fragile in the face
of model uncertainty as they can incur vanishingly small basins of attraction and it
remains to be demonstrated that actuator saturation—perhaps the predominant con-straint in legged locomotion—can be elegantly incorporated. Reactive techniques
taking the form of sequential composition [ 11] such as [ 12,13] are often very robust
and can incorporate actuator constraints, however known methods are conservativeand require the by-hand deployment of local controllers in the environment instead
of in an automated fashion. Classical sequential composition techniques also pro-
vide guarantees over an inﬁnite time horizon, which can be overly restrictive fortransitional tasks that may be as quick in duration as a single stride.
In this paper we explore application of the reachability ideas presented in [ 14]
and the sizable literature it generated ([ 15–17] to name a few) that construct con-
trollers for discrete-time systems to reach some target set in a minimum number of
steps, a framework that intersects the similarly longstanding tradition of pre-imagebackchaining in the LMT literature [ 18–20] as well as explicit model predictive con-
trol [ 21,22]. Particularly, we directly employ the techniques of [ 23] which gives
the complete class of minimum-time feedback laws to reach a target set, a modiﬁedformulation of which is presented in Theorem 1. To our knowledge, such techniques
have not heretofore been used for reactive control on legged robots.
We connect this theory to legged locomotion by proposing a formal deﬁnition of a
class of transitional tasks that we term a discrete navigation problem , which consists
of controlling the state of a discrete-time control system to reach a goal set while in
the interim avoiding a set of obstacle states. Here the passage from the continuoushybrid dynamics of a physical plant to the discrete-time system is achieved through
the imposition of a suitable stance map via an appropriate continuous-time within-
stance controller. Our main contribution is summarized in Fig. 1. We empirically
demonstrate the efﬁcacy of the control relation given in Theorem 1implemented on
a physical hopping robot afﬁxed to a boom (controlled in continuous time in stance to
exhibit a linear stance map) for the encoding and execution of a reactive leap over anobstacle. We formulate this task as a discrete navigation problem such that—if a task
solution exists—application of Theorem 1is necessary and sufﬁcient for its solution
in a minimum number of steps. The real-time deployment of this reactive control
relation is completely automated, however our understanding of how to effectively
compute it is limited to afﬁne control systems with polyhedral obstacles, goal sets,and control constraints.
This paper is organized as follows. Section 2introduces the discrete navigation
problem. Section 3describes the reactive control relation and Sect. 4approximates
the task of a legged robot leaping over an obstacle as a discrete navigation problem
in a manner suitable for computing the associated reactive control relation. Section 5
Towards Reactive Control of Transitional Legged Robot Maneuvers 147
Fig. 1 Empirical data from two experiments using the hopper introduced in Fig. 3to perform a
leap across a gap obstacle subject to a continuous-time within-stance controller that enforces thelinear stance map described in Sect. 5. The ﬁrst experiment—left—uses our new scheme to suggest
a degree of behavioral autonomy by forcing the robot to back up so as to get a sufﬁcient running startto clear the gap. The second experiment—middle—demonstrates that simply applying a maximumforwards acceleration is not sufﬁcient to cross the gap. Our algorithm generates a hopping controllerguaranteeing that the abstract representation of the mass center will leap over but not cross throughthe the red line segment—a coarse approximate obstacle set derived from the actual terrain (depictedin black in the two plots on the left). The apex states of both experiments—right—are plotted ontop of the goal set of apices that will cross the gap (green), the obstacle set of apices will fall into thegap (red), and the set of apex states that can reach the goal in ksteps for some integer k>0 (light
blue) as introduced in Sect. 3, while the remaining white set will enter the obstacle regardless of the
applied control input and can be considered “as good as lost.” These sets are explicitly constructedin Sect. 4and [ 24]. The k-step goal-reachable blue sets “funnel” into the goal set and illustrate that
reversing to get a longer running start to clear the gap is required from the robot’s starting state
empirically demonstrates the control relation deployment on a physical hopper to
leap over an obstacle and is followed by concluding remarks in Sect. 6. This paper
has a technical report companion [ 24] that provides proofs for the statements in
Sect. 3and calculates the control relation for a class of linear discrete-time control
systems that describes the simpliﬁed hopper dynamics.
2 Discrete Navigation Problems
Consider a discrete-time control system qn+1=f(qn,un), where qn∈D⊂Rm,
un∈U⊂Rp, and the continuous map f:D×U→Dis a homeomorphism of
Dfor each ﬁxed value of the second argument. We are interested in the task of
controlling the state of such system to reach a nonempty goal set G⊂Dwhile in the
interim avoiding a set of obstacle states O⊂Dwhere O∩G=∅. We call this task
adiscrete navigation problem since it is the discrete-time analogue of a continuous-
148 J. Duperret and D. E. Koditschek
time navigation problem where the state needs to reach (but not necessarily stay in)
the goal set.1
To provide a framework for deriving a controller for this task we mathematically
represent the discrete navigation problem as executing (if it exists) a K -step naviga-
tion plan starting from q∈D,q/∈O∪Gfor some unspeciﬁed K∈N+.2We deﬁne a
K-step navigation plan ( K∈N+) from q0∈D,q0/∈O∪Gas the ordered pair (ˆq,ˆu)
where ˆuis the length Kcontrol sequence ˆu={ u0,u1,..., uK−1},ui∈Usuch that
the length (K+1)sequence of states ˆq={ q0,q1,..., qK}given by the execution
of the control sequence ˆuvia qi+1=f(f(. . . f(q0,u0),u1) ,..., ui)has the prop-
erties (which we term admissible in the sequel) qi/∈O∪Gfor i∈{0,..., K−1}
and qK∈G.
We additionally introduce the notion of a candidate K -step navigation plan to
assist in the later proofs, which we deﬁne as the ordered pair (ˆq,ˆu)that satisﬁes
all the requirements of a K-step navigation plan except for possibly admissibility.
Once admissibility of a candidate K-step navigation plan has been veriﬁed, it is a
K-step navigation plan and we say that it is a solution to the discrete navigation prob-
lem, where by deﬁnition a candidate K-step navigation plan completes the discrete
navigation task (is a solution) if and only if it is admissible.
3 Reactive Control for Discrete Navigation T asks
This section derives a reactive control relation (given in Theorem 1) that—if a solu-
tion to the task exists—is necessary and sufﬁcient to solve a discrete navigation
problem, and concludes with remarks about its computability. Proofs of Proposition1, Corollary 1, and Theorem 1are provided in the technical report companion [ 24,
Sect. 1].
We inductively deﬁne the set of all states R
kfor which a control action exists that
ﬁrst completes the discrete navigation problem in kiterations as
Rk+1:= f−1(Rk,U)\(O∪G),R0:=G,
1To give the reader some intuition for how this task differs from a more traditional task encoded with
a limit-set goal—using an example well beyond the scope of this paper’s experiments—consider arunning leap to grab a vine or tree branch extending over a deep gorge so as to swing over it, wherewe separate the running leap to the vine from the brachiation task of grasping and swinging from it.The goal state of the running leap might be designated as requiring an apex state of ballistic ﬂightto be within reaching distance of the vine while the obstacle would be the gorge. This task differsfrom encoding limit sets in that there is no notion of continuing the task indeﬁnitely to reduce error(as missing the vine goal set over the gorge would be catastrophic) or even remaining in the goalset for an arbitrary amount of time. Instead, the behavior of the modeled dynamics is irrelevant(in the encoding) after the goal is reached. In our example the forward ﬂow would enter the gorgeobstacle after task is ﬁnished, however phenomena outside the scope of the model take over. Thevine is grasped and used to brachiate across the chasm. The brachiation itself could be considereda discrete navigation problem which could be composed with its predecessor.
2N+denotes the positive integers.
Towards Reactive Control of Transitional Legged Robot Maneuvers 149
where we deﬁne f−1(S,V):= { q∈D|f(q,¯u)=¯q,¯q∈S,¯u∈V}on sets S⊂
D,V⊂U, noting that the set R=/uniontext
kRkgives the set of all states which are able to
complete the discrete navigation problem.
Proposition 1 A candidate K -step navigation plan (ˆq,ˆu)=({q0,..., qK},
{u0,..., uK−1})is admissible if and only if for every i ∈{0,..., K−1}it holds
that qi∈RK−iand f (qi,ui)∈RK−(i+1)(proof given in [ 24, Sect. 1]).
Deﬁne the index set IqasIq:= { i∈N+|q∈Ri}.
Corollary 1 There exists an admissible K -step navigation plan from qif and only if
K∈Iq. If a solution to the discrete navigation problem exists, the minimum number
of steps that it can be completed in from qismin(Iq)(proof given in [ 24, Sect. 1]).
We note that the sets Pi={ q∈R|i=min(Iq)}for i=1,2,..., together with
P0=R0form a partition of R, where q∈Piimplies that an i-step solution from
qexists and that iis the minimum number of steps that the navigation task can be
solved in.
For ease of notation deﬁne Uq,k:= { u∈U|f(q,u)∈Rk−1},k∈N+.
Theorem 1 If a solution to the discrete navigation problem exists, then the discrete
navigation problem is solved in the minimum number of possible steps if and only if
the following reactive control relation is observed at every step:
u∈/braceleftBigg
Uq,min(Iq)Iq/negationslash=∅,
U else,(1)
where qis the state at any given iteration and uis the chosen control action at that
iteration (proof given in [ 24, Sect. 1]).
Solving the discrete navigation problem with this strategy has the practical utility
that it is reactive. It reduces the problem of forming a full navigation plan—whichmight need to be re-planned in the case of state disturbance or uncertainty—to that
of only choosing the next step from wherever the current state is.
In general there is no known method for computing R
k, without which computing
the control relation is infeasible and the user is relegated to techniques such as
computing conservative approximations of these sets [ 20]. Even if computing a single
Rkset is possible for a particular problem, the set of relevant Rk’s might have inﬁnite
cardinality in which case the algorithm is not guaranteed to terminate.
However, speciﬁc cases can admit readily computable Rksets and there are sit-
uations in which the cardinality of the set of all relevant Rksets is ﬁnite. We show
in [24, Sect. 2.2] a Rkcomputation method for a linear form of the system dynamics
with polyhedral control constraints which is directly generalizable to afﬁne system
dynamics. Regarding the cardinality problem, if the region of operation Dis compact
(for example, if the state space is restricted to be a closed set sufﬁciently local to the
robot) then there are problems where—for a sufﬁciently large K—the set/uniontextK
i=0Ri
150 J. Duperret and D. E. Koditschek
has the property that for any j∈N,Rj⊂/uniontextK
i=0Ri, allowing the computation to
terminate. Even if Dis not compact, in some applications the user is only interested
in solutions that complete the task within a maximum number of steps K(for exam-
ple, if the task must be performed quickly) in which case specifying a maximum K
guarantees termination. Since transitional maneuvers are typically local to a robot
and must be performed quickly we could in theory adopt either of these methods butfor simplicity we choose to specify a maximum Kin our implementation described
in Sect. 5.
4 Leaping Over an Obstacle with a Legged Hopper:
Formulation as a Transitional Discrete NavigationProblem
This section presents our method for applying the control relation of Theorem 1
to the generation of autonomous leaping behavior in a simple sagittal-plane legged
hopping model. It entails the interaction of three distinct abstracted representationsof the task, the environment, and the robot. The terrain height and robot template [ 25]
dynamics representative of the physical machine are described in what we term the
sagittal-hopper model . We make a local approximation of the sagittal-hopper model
called the local ballistic-approach (LBA) approximation that gives a physically useful
criterion for leaping over an obstacle in a way that is agnostic to the particular stance
dynamics at the expense of making solutions slightly conservative and allowing asmall but well-characterized (and rarely encountered) class of low-speed obstacle
collisions as described more carefully in the third paragraph of this section’s Local
Ballistic-Approach Approximation . From this approximation we form the task space
that forms the basis of a discrete navigation problem suitable for the application of the
control relation in Theorem 1and consists of the set of ballistic ﬂight apex states—
subsets of which form goal and obstacle sets—along with a control apex map. The
control relation of Theorem 1is explicitly calculated in [ 24, Eq. 3] to complete the
leap in the task space under the simpliﬁed dynamics presented in this section. Weuse these calculations as the basis for the physical experiments of Sect. 5.
Note that in this section we will write all set boundaries as closed to avoid the
cumbersome notation of keeping track of which set boundaries are open and closed.
Sagittal-hopper model Consider a reduced-order point-mass sagittal-plane hopper
tasked with leaping over without falling in or impacting a simple obstacle such as a
gap, wall, or ledge from relatively ﬂat ground. Denote the location of the hopper mass
center by x=(x,y), where xis forward position and yis vertical height. The robot
locomotes over ground represented by some terrain height function h(x):R→ R.
Suppose that the hopper’s template dynamics together with the terrain height function
hadmit an apex map q
n+1=f(qn,un),f:R3×U→ R3mapping the apex state
qn=(xn,yn,˙xn)∈R3at the nth step to the (n+1)th step according to the control
Towards Reactive Control of Transitional Legged Robot Maneuvers 151
vector un∈U⊂Rmchosen at step n, where unis representative of some member
of a parametrized family of continuous stance-controllers.
We assume a single obstacle that is some user-speciﬁed or pre-sensed feature in
the height function which is encoded with p∈N+closed connected line-segments
which the robot should leap over but not cross through. These line segments are indi-
vidually denoted L(xi,O1,xi,O2)in(x,y)space with endpoints xi,O1=(xi,O1,yi,O1)
and xi,O2=(xi,O2,yi,O2), where i∈{1,..., p}and by convention we assume
xi,O1
max(y
i,O1,yi,O2)∀i∈{1,..., p}, so that g−
qand g+
qare well-deﬁned (if the robot
starts below this threshold height we assume it is able to safely leap to above this
threshold on the next leap). Let the goal set Gbe the set of apex states which complete
the leaping task according to the LBA approximation and similarly let the obstaclesetObe the set of apex states which immediately fail the leaping task. Explicitly,
they are given by
4:
G=⎧
⎪⎪⎪⎪⎪⎪⎨
⎪⎪⎪⎪⎪⎪⎩(x,˙x,y)∈R
3|∀i∈{1,..., p}:⎡
⎢⎢⎢⎢⎢⎢⎣1σ/radicalBig
2
g(y−yi,O1)
−1σ/radicalBig
2
g(y−yi,O1)
1σ/radicalBig
2
g(y−yi,O2)
−1σ/radicalBig
2
g(y−yi,O2)⎤
⎥⎥⎥⎥⎥⎥⎦/bracketleftbiggx
˙x/bracketrightbigg
>⎡
⎢⎢⎣x
i,O1
−xi,O1
xi,O2
−xi,O2⎤
⎥⎥⎦⎫
⎪⎪⎪⎪⎪⎪⎬
⎪⎪⎪⎪⎪⎪⎭,
and
Oi=/braceleftBig
(x,˙x,y)∈R3|
⎛
⎝⎡
⎣1/radicalBig
2
g(y−yi,O1)
−1−/radicalBig
2
g(y−yi,O2)⎤
⎦/bracketleftbiggx
˙x/bracketrightbigg
≥/bracketleftbiggxi,O1
−xi,O2/bracketrightbigg⎞
⎠∨⎛
⎝⎡
⎣−1−/radicalBig
2
g(y−yi,O1)
1/radicalBig
2
g(y−yi,O2)⎤
⎦/bracketleftbiggx
˙x/bracketrightbigg
≥/bracketleftbigg−xi,O1
xi,O2/bracketrightbigg⎞
⎠∨
3We believe that one of the fundamental constraints of legged robot technology is limited actuation
power, implying a limited affordance to change kinetic energy over the course of a stride.
4These sets are calculated by solving the inequalities resulting from the hypograph/epigraph of
fq,g−
q,o r g−
qcontaining obstacle line segment endpoints, as explained in this section’s Local
Ballistic-Approach (LBA) Approximation .
154 J. Duperret and D. E. Koditschek
⎛
⎝⎡
⎣−1/radicalBig
2
g(y−yi,O1)
1−/radicalBig
2
g(y−yi,O2)⎤
⎦/bracketleftbiggx
˙x/bracketrightbigg
≥/bracketleftbigg−xi,O1
xi,O2/bracketrightbigg⎞
⎠∨⎛
⎝⎡
⎣1−/radicalBig
2
g(y−yi,O1)
−1/radicalBig
2
g(y−yi,O2)⎤
⎦/bracketleftbiggx
˙x/bracketrightbigg
≥/bracketleftbiggxi,O1
−xi,O2/bracketrightbigg⎞
⎠∨
/parenleftbigg/bracketleftBig
σ−σ/radicalBig
2
g(y−yi,O1)/bracketrightBig/bracketleftbiggx
˙x/bracketrightbigg
≥σxi,O1/parenrightbigg
∨/parenleftbigg/bracketleftBig
σ−σ/radicalBig
2
g(y−yi,O2)/bracketrightBig/bracketleftbiggx
˙x/bracketrightbigg
≥σxi,O2/parenrightbigg/bracerightBig
,
where O=∪p
i=1Oi, and where σ=1 if the obstacle is to be traversed forwards in
xandσ=− 1 if the obstacle is to be traversed backwards in x. We note that for
a constant y,GandOibecome polyhedra, a fact we use in the following section.
When holding yconstant, the goal set is described by four halfspace constraints for
a given obstacle line segment. The ﬁrst two halfspace constraints of the goal set
require that M(q)pass over xi,O1while the second two require that M(q)pass over
xi,O2. The obstacle set is described by six polyhedra for a ﬁxed y. The ﬁrst four
obstacle set polyhedra correspond to φα(q)intersecting L(xi,O1,xi,O2). There are
four to account for the cases of intersections through either direction of L(xi,O1,xi,O2)
and for intersecting with positive or negative values of αinφα(q). The point 2 in
Fig.2gives an example of such a case. The last two polyhedra encode states whose
evolution φα(q)with negative αpass under either of the obstacle endpoints to account
for all apex states that have already passed the obstacle. The point 3 in Fig. 2gives
an example of such a case.
Restriction to Linear Apex Map and Polyhedra Goal and Obstacles in Sagittal-
Hopper Model We restrict our attention to a linear form of the apex map fpredicated
on the desire for iterated dynamics simple enough for analysis but complex enough
to give physically relevant sagittal-plane behavior. To achieve such a map, we pre-
scribe our point-mass hopper model a constant stance time uTduring which we
apply an average horizontal stance acceleration u¨xon the mass center—where u¨xis
constrained to a closed interval containing zero—and some vertical force to achieve
the (artiﬁcially imposed) constant ballistic ﬂight apex height yn=y. For simplicity
we require that the beginning of stance occur when the point mass drops below a
height of yTDand that the hopper lift off from stance into ﬂight at this height as well.
While these assumptions (mainly the assumption of a constant apex height) severelyrestricts the behavior of the template, they still yield a reduced-order model capable
of relevant sagittal plane behaviors such as hopping over gaps and onto ledges from a
running start. These restrictions also endow the apex map with a linear form, makingit suitable for the linear programming computations given in [ 24, Sect. 2]. We hope
in future work to investigate restricted dynamics with more expressive behaviors that
are also simple enough to be used with a similar algorithm.
Let u=u
¨x∈U=[ u¨x,min,u¨x,max]⊂ R. We treat the constant apex height yand
constant touchdown/liftoff height yTDas parameters, along with the vertical accel-
eration due to gravity g. The linear controlled apex map qn+1=f(qn,un)of such a
system is given by:
Towards Reactive Control of Transitional Legged Robot Maneuvers 155
xn+1=xn+uT˙xn+/parenleftbig
2˙xn+u¨x,nuT/parenrightbig/radicalBigg
2
g(y−yTD)+/parenleftbiggu2
T
2/parenrightbigg
u¨x,n, (2)
yn+1=y
˙xn+1=˙xn+u¨x,nuT,
While many continuous stance dynamics could satisfy such an apex map, they can
be as simple as applying a constant horizontal force and piecewise constant verticalforce in stance for a time duration u
Tto satisfy the desired forwards acceleration and
vertical apex height.
5 Experiments
A series of running leap experiments using a robotic hopper on a boom were per-
formed to demonstrate the utility of the control relation given in Theorem 1under
the linear dynamics assumption of Sect. 4.
The hopper, shown in Fig. 3, consists of a two degree-of-freedom 3.2 kg leg ﬁxed to
a circular boom, constrained to allow translation but not rotation in the sagittal plane.
The hopper leg is a parallel ﬁve-bar mechanism [ 26] actuated by two T-Motor U10-
Plus 80KV motors5using Ghost Robotics motor controllers.6An STM-32 F303VC
micro-controller7performs the control algorithm and sends commands to the motor
controllers at 1 kHz.8The only sensors used by the machine are encoders at the motor
shafts to sense the leg kinematics and two encoders on the boom that sense the polarangle of the boom (used to calculate forward distance in the sagittal plane) as well
as the azimuth angle of the boom (used to calculate the vertical height in the sagittal
plane). No sensing of the obstacle is necessary as its location is preprogrammedinto the algorithm, although future work will involve its sensing. An off-board 4-cell
lithium polymer battery was used to power the hopper.
The template dynamics given in Eq. 2were achieved in stance by applying a
constant feed-forward horizontal force u
¨x, a feed-forward piecewise constant vertical
force to achieve the desired deadbeat apex height y, and limiting the stance duration
time to uT. Simple proportional derivative control was used as feedback to track
the stance trajectory expected from the feed-forward signals. This anchoring [ 25]
was achieved by mapping force commands to torque commands through the inversetranspose of the kinematic Jacobian, and saturating the inﬁnity norm of the required
torque according to the motor’s torque limits. A singularity-avoidance controller that
applies increasing amounts of torque as the leg approaches a singular conﬁguration
5http://store-en.tmotor.com/goods.php?id=362 .
6https://www.ghostrobotics.io/ .
7http://www.st.com/en/microcontrollers/stm32-32-bit-arm-cortex-mcus.html?querycriteria=productId=SC1169 .
8The computations of [ 24] are not constrained to a 1 kHz update rate and are only computed once
per stride.
156 J. Duperret and D. E. Koditschek
Fig. 3 The legged hopper ( a) is used to empirically demonstrate the utility of the control relation
given in Theorem 1on a microcontroller to perform a leaping maneuver with a running start. The
fully actuated two degree-of-freedom legs allow the command of arbitrary-direction sagittal-planestance forces so as to anchor the linear apex map template model given in Eq. 2for use in the
experiments. The experimental setup ( b) shows the hopper constrained to the sagittal plane by a
boom in front of the gap obstacle consisting of pink tape
guarantees that the leg won’t travel through a kinematic singularity during stance.
We demonstrate the efﬁcacy of this anchoring on the hopper at low speeds in Fig. 4
and high speeds in Fig. 5, although we save an in-depth description and experimental
statistical analysis for future work on the more interesting case of a free-running
untethered robot.
Figure 4shows 30 instances of anchoring a low-speed stride on the hopper. The
correspondence of the desired and actual trajectories at the apex events indicate
that the linear apex-map template of Eq. 2is approximately achieved, however work
remains to improve the in-stance anchoring as indicated by the the y-trace in which a
variety of undesired phenomena are evident. We believe that the larger-than-expected
vertical acceleration early in stance is due to the effect of a singularity avoidancecontroller activating as the leg approaches the edge of its workspace. We are also
unclear exactly as to why the correct apex is achieved despite liftoff occurring at too
great a height but suspect that some friction-like effect from the boom is responsible.For now we allow these effects to roughly cancel each other out but they deserve
a more careful treatment in future work when we implement this controller on an
untethered robot unhindered by the dynamics of the boom.
Figure 5shows 28 runs of attempting to anchor a higher-speed stride on the
hopper, demonstrating that at higher speeds the hopper is unable to anchor the linear
apex-map template of Eq. 2. This appears to be due to the fact that at higher speeds
the kinematic conﬁguration of the leg is such that in the second half of stance one of
the two motors is saturated and the other one is doing almost no work, as shown in themotor plots on the right. This manifests itself in a sharp drop-off in vertical velocity
in the second half of stance so that the hopper is unable to generate enough liftoff
Towards Reactive Control of Transitional Legged Robot Maneuvers 157
Fig. 4 Boom encoder readings and motor commands (black) from 30 runs of experiments of
lower-speed hopper strides, superimposed with the desired trajectory of the robot (red), show thatat slower speeds the linear apex-map template of Eq. 2is approximately achieved—as indicated
by the correspondence of the desired and actual trajectories at the apex events occurring at times
0 and 0.7. However, work remains to improve the in-stance anchoring as indicated by the y-trace
in which a variety of undesired phenomena are evident as discussed in Sect. 5. Note that the blue
background indicates stance while the green background indicates ballistic ﬂight. The data has beenshifted in xso that every run starts from x=0 as well as in time so that detected stance onset occurs
simultaneously in every run
velocity to attain the commanded apex height, indicating that the implementation
of this algorithm on an untethered robot would beneﬁt from a leg conﬁguration that
more evenly shares the burden of the motor affordance while running, bearing inmind that the requirement that the motor-leg kinematics be designed to support this
class of templates will likely decrease the performance of some other task.
We implement the algorithm on-board the robot by computing the R
ksets up
to a kof 20 when the robot initializes and then using the estimated apex state to
calculate the control relation Uq,min(Iq)via [ 24, Eq. 3] once per stride. The hopper
then chooses the forward acceleration u¨xthat is both consistent with the control
relation and maximizes the distance of the next apex state to the boundary of the
relevant Rkset containing it, achieving a degree of robustness in an informal sense
that is elaborated on in [ 24, Sect. 3]. The obstacle location (represented by features
in the black line-segment terrain in the right-hand-side column of Fig. 6) is known
a-priori to the control algorithm and encoded in the sagittal-hopper model using
a single line segment (represented by the red line segments in the right-hand-sidecolumn of Fig. 6), which is user-speciﬁed to span the horizontal width of the physical
obstacle and to vertically extend beyond the physical obstacle by the length of the
hopper’s leg so as to allow room for the hopper’s leg to clear the physical obstacle in
158 J. Duperret and D. E. Koditschek
Fig. 5 Boom encoder readings and motor-commands (black) from 28 runs of experiments of
higher-speed hopper strides, superimposed with the desired trajectory of the robot (red), show thatat higher speeds the hopper is unable to anchor the linear apex-map template of Eq. 2. This appears
to be due to the fact that at higher speeds the kinematic conﬁguration of the leg is such that, in thesecond half of stance, one of the two motors is saturated and the other one is doing almost no workas shown in the motor plots on the right as discussed in Sect. 5. Note that at these higher speeds the
boom dynamics become more pronounced; ringing oscillations are evident in both the forward andvertical velocity traces later in the stride
ﬂight. Future work will seek to automate this obstacle detection process from sensor
data such as LiDAR readings.
Three experiments were performed to demonstrate the applicability of Theorem
1to a physical machine using embedded hardware. In the ﬁrst, the hopper is com-
manded to repeatedly leap over a ledge with a gap in front of it from various initialconditions to demonstrate repeatability. In the second and third, the hopper was
commanded to cross other types of obstacles (a hurdle and a simple gap) to show-
case a few of the different obstacle types that can be traversed and give a senseof their corresponding sets GandO. In each case the hopping apex height ywas
set to 0.65 m, the touchdown and liftoff height y
TDwas set to 0.32 m, the stance
time uTwas set to 200 ms, and the forward acceleration was limited to the modest
u¨x,max=− u¨x,min=0.5m / s2due actuator saturation constraints.
The results of the experiments are shown in Fig. 6. Figure 6a depicts 15 runs of the
hopper leaping onto a ledge immediately preceded by a gap, and demonstrates that thetask can be repeatedly completed from various initial conditions. Figure 6b depicts
5 runs of the hopper leaping over a simple gap obstacle, and Fig. 6c depicts 6 runs
of the hopper leaping over a hurdle obstacle. The variation in apex trajectories from
similar initial conditions indicates our anchoring scheme of the linear apex dynamics
given in Eq. 2remains to be improved (as depicted in Figs. 4and5), however the task
is still completed.
Towards Reactive Control of Transitional Legged Robot Maneuvers 159
Fig. 6 (Continued)
160 J. Duperret and D. E. Koditschek
/triangleleftsldFig. 6 Experimental results of the robotic hopper introduced in Fig. 3using the control relation of
Theorem 1to reactively leap over various obstacles from a running start. Row adepicts 15 instances
of the hopper leaping onto a ledge immediately preceded by a gap from various initial conditions,
bdepicts 6 instances of leaping over a simple gap obstacle, and cdepicts 5 instances of leaping
over a hurdle obstacle. The right column of the ﬁgure shows the sagittal-plane representation of theenvironment with the terrain depicted in black and an example controlled robot trajectory from theexperiments in blue, smoothed to ﬁlter out measurement noise. The yellow trajectory demonstratesfailure when the robot simply accelerates at maximum. The red line segment is the encoded obstacleto be avoided by the mass-center. This line segment was user-speciﬁed to span the horizontal widthof the physical obstacle and to vertically extend beyond the physical obstacle by the length ofthe hopper’s leg so as to allow room for the hopper’s leg to clear the physical obstacle in ﬂight.Future work will seek to automate this obstacle detection process from sensor data such as LiDARreadings. The left column of the ﬁgure shows the apex-state representation of the leaping taskwhere the robot’s apex trajectory is depicted by dark blue dots sequentially connected by dark bluelines. The set of apex states which result in immediately passing through the obstacle as-per theapproximations of Sect. 4are shown in red and comprise the obstacle set O, while the set of apex
states which pass over the obstacle are shown in green and comprise the goal set G. The lighter blue
sets represent the set of states R
kwhich can reach the goal in ksteps and which “funnel” into G,
while the remaining white area is the set of apex states which ﬂow into Oregardless of the applied
control input
An additional experiment is shown in Fig. 1which demonstrates a degree of
behavioral autonomy which this reactive control relation can provide. In this set ofexperiments the maximum acceleration was set to u
¨x,max=− u¨x,min=1.0m / s2.T h e
robot is initialized too close to a gap to directly leap over it and—when using the
proposed control scheme—reverses to gain enough of a running start to clear thegap. Without this direction reversal (when simply accelerating forwards at u
¨x,max)
the robot is observed falling into the gap.
6 Conclusions and Future Work
We propose the notion of a discrete navigation problem—consisting of controlling
the state of a discrete-time control system to reach a goal set while in the interim
avoiding a set of obstacle states—to approximate a class of tasks useful for legged
robotic applications such as leaping. We demonstrated the efﬁcacy of the controlrelation given in Theorem 1, which is (assuming a task solution exists) necessary
and sufﬁcient to solve a discrete navigation problem in a minimum number of steps,
on a physical hopping robot afﬁxed to a boom to reactively leap over an obstaclewith a running start, controlling the continuous stance dynamics to exhibit a linear
stance map.
Future work will focus on implementing this algorithm on a pronking quadruped
robot autonomously sensing obstacles with LiDAR. We also plan to apply this algo-
rithm to multiple sequential obstacles in an “obstacle course”-like environment which
we expect will raise interesting issues such as avoiding deadlock. Finally, we hopeto be able to make formal claims on forming various control laws from the proposed
Towards Reactive Control of Transitional Legged Robot Maneuvers 161
control relation as it relates to criteria such as robustness and aggressiveness now that
we have an exact representation of the solution space of discrete navigation problems
as opposed to a conservative representation of it.
Acknowledgements This work was supported in part by the National Science Foundation Graduate
Research Fellowship under Grant No. DGE-0822 held by the ﬁrst author and in part by the ArmyResearch Ofﬁce under Grant No. W911NF-17-1-0229.
References
1. Seok, S., Wang, A., Chuah, M.Y ., Hyun, D.J., Lee, J., Otten, D.M., Lang, J.H., Kim, S.: Design
principles for energy-efﬁcient legged locomotion and implementation on the mit cheetah robot.IEEE/ASME Trans. Mechatron. 20(3), 1117–1129 (2015)
2. Hyun, D.J., Seok, S., Lee, J., Kim, S.: High speed trot-running: implementation of a hierarchical
controller using proprioceptive impedance control on the mit cheetah. Int. J. Robot. Res. 33(11),
1417–1445 (2014)
3. Boston dynamics, http://www.bostondynamics.com
4. De, A., Koditschek, D.E.: Vertical hopper compositions for preﬂexive and feedback-stabilized
quadrupedal bounding, pacing, pronking and trotting, (under review) (2016)
5. Guckenheimer, J., Holmes, P.J.: Nonlinear Oscillations, Dynamical Systems, and Bifurcations
of Vector Fields, vol. 42. Springer Science & Business Media, Berlin (2013)
6. Mayne, D.Q.: Model predictive control: recent developments and future promise. Automatica
50(12), 2967–2986 (2014)
7. Lee, J.H.: Model predictive control: review of the three decades of development. Int. J. Control.
Autom. Syst. 9(3), 415–424 (2011)
8. Park, H., Wensing, P.M., Kim, S.: Online planning for autonomous running jumps over obstacles
in high-speed quadrupeds. Robot. Sci. Syst. 11(2015)
9. Tedrake, R., Manchester, I.R., Tobenkin, M., Roberts, J.W.: Lqr-trees: Feedback motion plan-
ning via sums-of-squares veriﬁcation. Int. J. Robot. Res. 29(8), 1038–1052 (2010)
10. Majumdar, A., Ahmadi, A.A., Tedrake, R.: Control design along trajectories with sums of
squares programming. In: Proceedings of the IEEE International Conference on Robotics andAutomation, pp. 4054–4061 (2013)
11. Burridge, R., Rizzi, A., Koditschek, D.: Sequential composition of dynamically dexterous robot
behaviors. Int. J. Robot. Res. 18, 534–555 (1999)
12. Arslan, O., Saranli, U.: Reactive planning and control of planar spring-mass running on rough
terrain. IEEE Trans. Robot. 28(3), 567–579 (2012)
13. Conner, D.C., Choset, H., Rizzi, A.A.: Integrated planning and control for convex-bodied
nonholonomic systems using local feedback control policies. Robot. Sci. Syst. 2, 57–64 (2007)
14. Bertsekas, D.P., Rhodes, I.B.: On the minimax reachability of target sets and target tubes.
Automatica 7(2), 233–247 (1971)
15. Mayne, D.Q., Schroeder, W.R.: Robust time-optimal control of constrained linear systems.
Automatica 33(12), 2103–2118 (1997)
16. Rakovi, S.V ., Mayne, D.Q.: Robust time optimal obstacle avoidance problem for constrained
discrete time systems. In: Proceedings of the 44th IEEE Conference on Decision and Control,
and the European Control Conference, CDC-ECC ’05, vol. 2005, pp. 981–986 (2005)
17. Rakovi, S.V ., Blanchini, F., Crck, E., Morari, M.: Robust obstacle avoidance for constrained
linear discrete time systems: a set-theoretic approach. In: Proceedings of the IEEE Conferenceon Decision and Control, pp. 188–193 ( 2007)
18. Lozano-Prez, T., Mason, M.T., Taylor, R.H.: Automatic synthesis of ﬁne-motion strategies for
robots. Int. J. Robot. Res. 3(1), 3–24 (1984)
162 J. Duperret and D. E. Koditschek
19. Mason, M.: Automatic planning of ﬁne motions: correctness and completeness. In: Proceedings
of the 1984 IEEE International Conference on Robotics and Automation, vol. 1, pp. 492–503.IEEE (1984)
20. Erdmann, M.: Using backprojections for ﬁne motion planning with uncertainty. Int. J. Robot.
Res. 5(1), 19–45 (1986)
21. Alessio, A., Bemporad, A.: A Survey on Explicit Model Predictive Control. Lecture Notes in
Control and Information Sciences, vol. 384 (2009)
22. Bemporad, A., Borrelli, F., Morari, M.: Model predictive control based on linear programming
- the explicit solution. IEEE Trans. Autom. Control. 47(12), 1974–1985 (2002)
23. Keerthi, S.S., Gilbert, E.G.: Computation of minimum-time feedback control laws for discrete-
time systems with state-control constraints. IEEE Trans. Autom. Control. 32(5), 432–435
(1987)
24. Duperret, J., Koditschek, D.E.: Technical report on: towards reactive control of simpliﬁed
legged robotics maneuvers. Technical report, University of Pennsylvania (2017)
25. Full, R.J., Koditschek, D.E.: Templates and anchors: neuromechanical hypotheses of legged
locomotion on land. J. Exp. Biol. 202(23), 3325–3332 (1999)
26. Avik De, Kenneally, G., Koditschek, D.E.: Design principles for a family of direct-drive legged
robots. In: Robotics: Science and Systems, Workshop (2015)
Compositional and Contract-Based
Veriﬁcation for Autonomous Drivingon Road Networks
Lucas Liebenwein, Wilko Schwarting, Cristian-Ioan Vasile, Jonathan
DeCastro, Javier Alonso-Mora, Sertac Karaman and Daniela Rus
1 Introduction
The way we use and think about mobility and transportation has changedsigniﬁcantly in the last years due to the developments in autonomous driving. Amajor emerging challenge is to provide safety guarantees to gain customers’ trust.
Current driver-assisting technologies have mostly advisory roles with limited auton-
omy and decision making responsibilities [ 8,25]. V ehicles with full or shared auton-
omy have the potential to reduce the high incidence of vehicle-related deaths (over
Toyota Research Institute (“TRI”) provided funds to assist the authors with their research, but this
article solely reﬂects the opinions and conclusions of its authors, and not TRI or any other Toyotaentity.
L. Liebenwein ( B)·W. Schwarting ·C.-I. V asile ·S. Karaman ·D. Rus
Massachusetts Institute of Technology, Cambridge, MA 02139, USAe-mail: lucasl@mit.edu
W. Schwarting
e-mail: wilkos@mit.edu
C.-I. V asile
e-mail: cvasile@mit.edu
S. Karaman
e-mail: sertac@mit.edu
D. Rus
e-mail: rus@csail.mit.edu
J. DeCastro
Toyota Research Institute, Cambridge, MA 02139, USAe-mail: jonathan.decastro@tri.global
J. Alonso-Mora
Delft University of Technology, 2628 CD Delft, The Netherlandse-mail: J.AlonsoMora@tudelft.nl
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_18163
164 L. Liebenwein et al.
Fig. 1 The network shows the Manhattan street grid. We veriﬁed a part of Manhattan consisting of
ca. 130 blocks with 180 intersections and 330 straight road segments (marked black) using a libraryof 22 veriﬁed segments. This underlines the gain in efﬁciency of the compositional-based approachcompared to that of a direct naive approach
3000 each month [ 22]), most of them caused by driver error (over 70% [ 23]). Thus,
safety standards , which can be used for development by companies, and certiﬁcation
by legislators and administrators, can be very beneﬁcial for autonomous driving.
Formal veriﬁcation can ﬁll the gap of certiﬁcation by providing a platform to
assess safety with clear assumptions and guarantees. While simulation and testing
are undoubtedly essential tools for deployment of complex systems, they lack the
completeness, and therefore the guarantees, of veriﬁcation, potentially missing outon rare and hard-to-characterize events. Recent studies [ 18] have indicated that the
requirement to demonstrate safety for an autonomous car is hundreds of millions
of miles of testing taking possibly tens of years to complete. To meet these proof-
of-safety demands, testing and simulation, which provide very detailed insights for
speciﬁc events, can be supplemented with veriﬁcation frameworks, which provide
insights for an entire set of events, though often less detailed. In such a sense, ver-iﬁcation provides a way to check over an inﬁnite number of simulated trajectories
as opposed to straightforward case-based simulation at the cost of reduced model
complexity.
Despite its appeal, veriﬁcation can become computationally intractable for large
complex systems. Informally speaking, veriﬁcation is a tool, where a system is
checked over all possible executions with respect to a model to ensure a speciﬁ-
cation [32]. Checking for safety of a given controller over a city, e.g. Fig. 1, might
take years to process. Moreover, the process must be repeated for each controller wewant to verify, thus heavily limiting re-usability. In this paper, we propose a method
Compositional and Contract-Based V eriﬁcation … 165
to verify a broad class of controllers for autonomous vehicles navigating in urban
environments. We leverage compositional veriﬁcation ,assume-guarantee contracts ,
and reachability theory to verify space-time properties of controllers. Instead of
performing the veriﬁcation over entire road networks, we verify local road mod-
els, such as road segments and intersections, against trafﬁc models for other cars.
Concurrently, assume-guarantee contracts , i.e., pairs of safe entry and exit sets, are
synthesized that guarantee safe traversal of road models (reaching safe exit sets) if
the vehicle enters the components in the safe entry sets. A controller is certiﬁed safe
over a road network if all of its local road models can be composed together usingtheir associated contracts. An important feature of the veriﬁcation framework is that
it applies to all controllers that abide by a controller contract , i.e., a set of safety
constraints. The key idea is to propagate reachable sets that guarantee safety throughthe contract, without explicitly considering the controller itself. The framework can
be used to verify entire networks ofﬂine, or as a roll-out procedure to sequentially
check routes as these are traversed.
As an example of a veriﬁcation task, consider the road network in Fig. 1. Checking
the entire network is intractable. We therefore decompose it into intersection and road
segments such as the ones shown in Fig. 2. Thus, the veriﬁcation reduces to checking
a limited number of smaller models over short time horizons. The assume-guarantee
contracts are computed during veriﬁcation (Fig. 2b and e), and are used to certify the
road network through composition (Fig. 2f).
Contributions and Impact We propose a safety standard for full or shared
autonomous driving controllers in the form of a veriﬁcation framework based oncompositional and contract-based principles. The beneﬁts of the approach are: (1)
reusability of road models across road networks, (2) extendibility of the library to
additional local road models without re-veriﬁcation of existing models, and (3) abilityto perform computations in a parallel and distributed way.
Thus, we are able to efﬁciently leverage computational resources over the entire
life cycle of the standard: the initial construction of the library, its use for check-
ing road networks, and its incremental extension. Furthermore, controller contracts
ensure that any controller abiding by them can be certiﬁed safe, regardless of whethermotion primitive, sampling, or receding horizon based. For shared autonomy [ 30],
safety is assured under human input as long as the vehicle satisﬁes the controller
contract. The contributions of the paper are:
1. we formalize the vehicle safety veriﬁcation problem;
2. we design a compositional framework that deﬁnes local road models, controller
contracts, assume-guarantee contracts, and composition;
3. we develop the veriﬁcation and contract-synthesis procedure, and the composi-
tional veriﬁcation algorithm;
4. we provide domain-speciﬁc methods and implementation to overcome tractability
issues;
5. we demonstrate effectiveness of our approach on a case study of the Manhattan
road network.
166 L. Liebenwein et al.
(a)Four-way intersection (b) Forward propagation (c)Unsafe set pruning
(d) Safe exit set (e)Backward propagation (f)Composition
Fig. 2 Consider the library M={m1,m2}composed of a straight road m1and a four-way inter-
section m2.aThe four-way intersection m2is shown. bThe entry set is propagated forward, and,
c, concurrently pruned of unsafe states induced by other cars. dThe safe exit set is the intersection
of the safe reachable set at time step H, the veriﬁcation horizon, and the exit set. eThe safe entry
set is computed by backward propagating the safe exit set. fLastly, the composition of the models
using the associated assume-guarantee contracts enables us to certify road networks
The work presented in this paper may impact: (1) car companies, providing them
with a safety standard for developed controllers; (2) road administrators and mapping
businesses, providing road safety certiﬁcates; and (3) legislators, policy makers and
insurers, enabling them to assess and certify safety guarantees for autonomous cars.
Related works Challenges such as DARPA-sponsored competitions [ 6] have pushed
automated driving to near real-world conditions, but the question of safety and reli-
ability remains, preventing widespread adoption up to now.
Local motion planning : Methods to compute safe trajectories for autonomous
vehicles in dynamic environments [ 27] include input space discretization [ 14,33],
rapidly exploring random trees [ 19], and receding horizon control [ 12,31]. The latter
can be applied for shared-control of highly automated vehicles [ 30]. These methods
work well in practice, but usually compute valid and safe trajectories only up to a
pre-deﬁned time horizon, with no global and long term guarantees. In this work,
we aim to obtain guarantees via road certiﬁcates, which are employed by the local
planner.
Compositional and Contract-Based V eriﬁcation … 167
Reachability and Safety Games : Safety can be guaranteed by avoiding sets of
states that inevitably lead to collisions. These sets are referred to as the capture
set [ 17], the inevitable collision states (ICS) [ 5,13], the region of inevitable col-
lision (RIC) [ 7], and the target set [ 21]. Such sets are expensive to compute, thus
they are mostly applied to simpliﬁed systems, such as highway driving. Numerical
reachability analysis was used to ensure safety in reach-avoid control problems [ 11],
though only over limited domains, in symbolic abstractions of system models [ 35],
and for overtaking maneuvers in autonomous driving [ 2], though for speciﬁc ones
only. We use reachability analysis to concurrently solve local road model veriﬁcationand contract synthesis problems to synthesize guarantees over larger domains.
Formal Veriﬁcation : V eriﬁcation has been employed in a variety of safety-critical
domains such as aerospace [ 4], and automotive [ 24,26,34]. In the automotive
ﬁeld, prior work has focused on time-bounded behaviors and simple motion primi-
tives rather than veriﬁcation of controlled systems over complex, structured environ-
ments. Recent veriﬁcation and synthesis techniques exploit composition [ 3,9,20,
28], assume-guarantee contracts [ 15,29], and reachability [ 10]. These approaches
are limited in the scope of the system and/or model considered. We leverage com-
positional and contract-based veriﬁcation to simultaneously achieve scalability and
computational tractability in the model (large networks) and the system (control
system of car).
Overview In Sect. 2, we formalize the veriﬁcation problem of a controller for a
(semi-)autonomous vehicle. The veriﬁcation framework is presented in Sect. 3, its
implementation is described in Sect. 4, and a case study involving parts of the Man-
hattan road network in Sect. 5. Conclusions and future work are discussed in Sect. 6.
2 Problem Formulation
In this section, we introduce the safety veriﬁcation problem of controllers for
autonomous cars. We deﬁne models for the ego-car, trafﬁc, road network, controller,
and safety that form the context of the veriﬁcation problem.
Ego-car and road network The ego-car is deﬁned as a dynamical system V=
(Z,R,U,f,h)evolving according to zk+1=f(zk,uk), where Zis the state space,
R⊂R2the workspace, Uthe control space, C⊂SE(2)the conﬁguration space
(pose) of the car, and zk,pk=h(zk)and qk=g(zk)are the state, location and
conﬁguration of the car at time k. Further, let f:Z×U→Z,h:Z→R, and
g:Z→Cbe the Lipschitz continuous (invertible) dynamics, observation function,
and conﬁguration space submersion, respectively. When it is clear from the context,we denote the ego-car’s state by z
0
kinstead. The workspace R, which is a planar
compact connected region, corresponds to the roadway of the road network the car
operates in, see Fig. 1.L e t B(zk)⊂Rbe the ego-car’s footprint.
Trafﬁc The road network associated with the ego-car is also populated by other
trafﬁc participants, e.g., pedestrians, bikes, and cars. For brevity, we only consider
168 L. Liebenwein et al.
Table 1 Symbols table
V,Z,U,R,C V ehicle model and its state space, control space, workspace, conﬁguration
space
f,h,g Dynamics, observation, conﬁguration map
zi
kState of car iat time k,i=0 denotes the ego-car
Bi(zi
k),Ok Footprint of car iat state zi
k, footprint of static and dynamic obstacles
V(k),N(k) Set and number of cars present in Rat time k
T,I,O,S Trafﬁc model, regions where cars enter and exit a road model, trafﬁc scheduler
CiController of car i
Zk,/tildewideZk,/hatwideZk Reachable, safe reachable, and backward safe reachable set of ego-car at time
k
¬R Exterior (complement) of road network
G=(I,R) Topological graph of the road network
M,m,A The set of veriﬁed road models, road model and associated parameters
S,(entry,exit)Controller contract, pair of assume-guarantee contracts
other cars. We denote the state of car ipresent in the road network Rat time kby
zi
k,i∈{1,..., N(k)}, where N(k)is the number of cars in Rat time k. We consider
a trafﬁc model T=(V(0),I,O,S), where V(0)={Vi}N(0)
i=1is the set of vehicles
present in Rat initial time k=0,I⊂ZandO⊂Zare sets of states/regions for
entering and leaving the road network, and Sis a scheduler that generates cars at I
and destroys them at O, see Sect. 5for an example in the form of a hybrid system.
Controllers and driving behaviors The behaviors of all the cars are deﬁned
by controllers (feedback or open-loop). Formally, a controller for car iis a map
from all the car states z0:N
kto a control value ui
k, i.e., Ci:ZN+1→Usuch that
zi
k+1=fi(zi
k,Ci(z0:N
k)),∀i∈{0,..., N}, where fideﬁnes the dynamics of car i.
Throughout the paper, we will tacitly assume that the other cars’ models are given
together with the controllers that deﬁne their behavior and are known a priori for
veriﬁcation.
Safety The controller for the ego-car is said to be safe at time kif it does not collide
with environment obstacles, the road boundary, and other vehicles. Formally, theego-car is safe at time kif inf
z∈B(zk),o∈Ok/bardblz−o/bardbl>π saf ety , where Ok=¬ R∪/uniontextN
i=1Bi(zi
k)is the footprint of static and dynamic obstacles, ¬Ris the exterior
(complement) of the road network, Bi(zi
k)is the footprint of car iin the workspace
at state zi
k, andπsaf ety∈R≥0is the safety margin. Similarly, the ego-car is safe on
{0,..., K},K∈N∪{0,∞}, if it is safe for all times k∈{0,..., K}.T h es e to f
constraints representing safety are hereby said to be the controller contract S,s e e
Sect. 5for more details.
The problem that we address in this paper is checking the safety of executing
a controller Con the ego-car with respect to given car, road network, and trafﬁc
models. The controller Cis hereby represented by the controller contract S, which
Compositional and Contract-Based V eriﬁcation … 169
it is supposed to enforce. This not only allows for an abstract representation of speciﬁc
controllers, but also enables to concurrently verify a broad class of controllers.
Problem 1 (Controller safety ) Given a ego-car model Voperating in road network
R, using controller Cthat abides by the controller contract S, under the assumption
of a trafﬁc model T, determine whether the ego-car is safe under control of Cin time
interval {0,..., K}starting from some subset of the initial states Z0⊆Z(Table 1).
3 Composition-Based Veriﬁcation Framework
In this section, we propose a veriﬁcation framework based on decomposition of the
problem into smaller veriﬁcation tasks corresponding to topological features of road
networks, mainly road segments and intersections. V eriﬁcation over entire networks
is achieved by composition of models using synthesized assume-guarantee contracts.
The framework has two steps: (1) local veriﬁcation of the controller contract
Swith synthesis of assume-guarantee contracts, and (2) ﬁtting local models and
composition with the assume-guarantee contracts. First, the parameterized local road
models are veriﬁed, i.e., Sis veriﬁed, and a pair of safe entry and exit states is
synthesized for each model, which forms the assume-guarantee contract associated
with each model. These tasks may be performed off-line in parallel. The collection
of local models is called a library . Second, given a road network, locally veriﬁed
models from the library are ﬁtted to the roads and intersections of the network. Theassume-guarantee contracts are used to check the composition of the models based
on the topology of the road network.
Library of Parameterized Models The veriﬁcation process is decomposed into
smaller local problems to enable computational tractability as well as facilitate par-
allel and distributed solutions. Moreover, we want to reuse the local computationsboth within and across road networks. Thus, we propose constructing a library of
parameterized models that can be veriﬁed a priori and used in any road network
to decide the safety of a controller. Local veriﬁcation of models is valid for any
controller that implements the controller contract S.
Formally, each road element model is a tuple m=(Z,I,O,S,A), where Zand
R=h(Z)⊂R
2are the local state space and workspace (roadway) of an ego-car,
I={Ij}nI
j=1⊂Zis the set of the nIpossible entry regions, O={Oj}nO
j=1⊂Z
is the set of the nOpossible exit regions, Sis the trafﬁc scheduler that dictates
when cars are generated into R, and Ais the set of parameters associated with
the model. The parameters of a road model can be related to its geometry, such
as the width of the lanes, the angles of an intersection’s branches, and the poseof the model within a global road network. Note that, depending on the parameter
(e.g. lane width), the safety guarantee does not automatically hold for the entire set of
parameters, implying that the veriﬁcation procedure has to be conducted for a (ﬁnite)set of potential parameter values. The library of all available road element models
is denoted by M={m
p}p, where upper indices are used to distinguish between
170 L. Liebenwein et al.
multiple road models if necessary. An example of a four-way intersection is shown
in Fig. 2a. Composition of road models is done such that the exit region of the current
model overlaps with the entry region of the next one, see Fig. 2f for an example.
Veriﬁcation of Controller Contract and Assume-Guarantee Contracts V eriﬁca-
tion of the local road models in the library is based on reachability analysis, which
allows concurrent synthesis of the assume-guarantee contracts.
An assume-guarantee contract of a road model m, traversed from entry region j
to exit region j/prime, is a pair of safe entry and exit sets (entry(m,j,j/prime),exit(m,j,j/prime)),
where entry(m,j,j/prime)⊆Ij⊂Zand exit(m,j,j/prime)⊆Oj/prime⊂Z. The contract is
interpreted as follows: if the system starts in entry(m,j,j/prime), then it is guaranteed
that the controller can drive the ego-car safely to exit(m,j,j/prime)within the exit region
Oj/prime.
Algorithm 1: V eriﬁcation of Road Models
Input :m=(Z,I,O,S,A)– road model, Ij∈I,Oj/prime∈O– pair of entry/exit regions, H
– veriﬁcation horizon
Output :entry(m,j,j/prime),exit(m,j,j/prime)– pair of safe entry/exit regions (assume-guarantee
contract)
1/tildewideZ0←Ij
2{Zi
0}N(0)
i=1←S.init()
3fork∈{0,..., H−1}do // forward propagation
4 Zi
k+1←fi(Zi
k,Ci(/tildewideZk,Z1:N(k)
k)),∀i∈{1,..., N(k)}
5 N(k+1)←S.update ()
6 Ok+1←¬ R∪/uniontextN(k+1)
i=1Bi(Zi
k+1)
7 Zk+1←f(/tildewideZk,U)
8/tildewideZk+1←Zk+1\Ok+1 // enforcing the controller contract
9exit(m,j,j/prime)←/tildewideZH∩Oj/prime
10/hatwideZH←exit(m,j,j/prime)
11fork∈{H,..., 1}do // backward propagation
12 /hatwideZk−1←f†(/hatwideZk,U)∩/tildewideZk−1
13entry(m,j,j/prime)←/hatwideZ0∩Ij
14return entry(m, j, j’), exit(m,j,j’)
The veriﬁcation procedure for a road model mis shown in Algorithm 1 for given
entry and exit regions. Overall, all entry-exit pairs need to be veriﬁed. The algorithm
has two parts. In the ﬁrst part, the entry set is propagated forward over the given time
horizon H, starting from the initial entry set Ij(line 1). The initial state sets of the
other cars are initialized by the scheduler using its init()method (line 2). At each step
the other vehicles’ state sets are propagated using their controllers Ci(line 4). Next,
the scheduler’s update ()method is called (line 5) that spawns and removes vehicles,
initializes new vehicles, and returns the number of vehicles, according to the trafﬁc
model T. The unsafe states Ok+1, which arise from S, are the union over the road
complement ¬R, i.e., the exterior, and the union of possible footprints Bi(Zi
k+1)of
Compositional and Contract-Based V eriﬁcation … 171
each of the other cars (line 6). Then, the ego-car’s reachable set Zk+1is computed
(line 7) and the unsafe states Ok+1are pruned from Zk+1(line 8) to obtain the safe
reachable set /tildewideZk+1. Note that the reachable set Zk+1is computed over all possible
control inputs u∈U, since the controller is only implicitly represented via S, i.e.,
the safety constraints. In other words, we check for all solutions Zk+1and then prune
them accordingly to obtain the possible solutions /tildewideZk+1under S. The safe exit set
exit(m,j,j/prime)is the set of safe reachable solutions within the exit set Oj/prime(line 9).
Next, to compute the safe entry set for exit(m,j,j/prime)⊂Oj/prime, backwards propagation
is employed starting from the safe exit set (line 10). The safe exit set is backpropagatedvia the inverse dynamics z
k−1=f†(zk,uk−1)of the ego-car and intersected with the
safe forward reachable set /tildewideZk−1, since these are the only relevant solutions (line 12).
The controller contract Sis enforced at all times since /tildewideZk−1abides by S. The safe
entry set is thus /hatwideZ0(line 13). The necessity for this additional step arises from the
fact that, although we can infer the set of initial conditions ( Ij) of the safe reachable
set/tildewideZH, we cannot infer the set of safe initial conditions ( entry(m,j,j/prime)) of the safe
exit set exit(m,j,j/prime). A graphic representation of the procedure is shown in Fig. 2.
The forward propagation procedure (line 7) is shown in Fig. 2b, and the pruning step
(line 8) is shown in Fig. 2c. Once the safe-reachable set at step His computed, it
is trimmed (line 9) to lie within the exit region, see Fig. 2d. The second part of the
procedure, the backward propagation (line 10–13) shown in Fig. 2e, computes the
safe entry set.
Algorithm 2: V eriﬁcation with Library of V eriﬁed Road Models
Input :V=(Z,R,U,f,h),M– library of veriﬁed parameterized models
Output : Boolean value indicating safety
1extract topology graph G=(I,R)of road network R
2ﬁt each node ι∈I(intersection) to mι∈Mwith parameters Aι
3ﬁt each edge r∈R(road) to mr∈Mwith parameters Ar
4forr1=(ι1,ι2),r2=(ι2,ι3)∈Rdo
5 if¬/parenleftbig
exit(mr1,1,1)⊆entry(mι2,r1,r2)∧exit(mι2,r1,r2)⊆entry(mr2,1,1)/parenrightbig
then
6 return ⊥
7return /latticetop
Road Network Veriﬁcation Given a library of veriﬁed road models M, we can verify
road networks via composition using the models’ assume-guarantee contracts. The
procedure is summarized in Algorithm 2. First, we extract the topology graph Gof
network R(line 2), and then ﬁt models to all intersections ι∈I(line 2) and road seg-
ments r∈R(line 3) corresponding to the graph’s nodes and edges. Finally, we check
for each two incident road segments r1,r2∈Rif: (a) the safe exit set exit(mr1,1,1)
of the ingoing road r1is included in the safe entry set entry(mι2,r1,r2)of the com-
mon intersection mι2, and (b) the safe exit set of the intersection exit(mι2,r1,r2)is
included in the safe entry set entry(mr2,1,1)of the outgoing road r2. If all checks
pass, then the network is certiﬁed safe. In other words, if the safe set remains non-
172 L. Liebenwein et al.
empty during the propagation throughout the network, the network is certiﬁed safe.
In the case that a (pairwise) composition is deemed unsafe, Algorithm 1 can be run
for the composition.
4 Implementation
In the following, we introduce the tools for reachability analysis and set operations.Relevant details for the implementation of Algorithm 1 are mentioned as well.
Reachability Tool We employ CORA [ 1] to compute the reachable set Z
k⊂Zfor
each time k(line 7 of Algorithm 1). CORA is a reachability tool for linear, nonlinear
and hybrid dynamical systems. Starting from some set /tildewideZk, represented as zonotope,
it computes the reachable set Zk+1at time k+1 by solving Zk+1=f(/tildewideZk,u=0),
and, subsequently, appropriately enlarging Zk+1to account for the variable control
input u∈U. Nonlinear systems, such as the dynamics model used in our case study
in Sect. 5, are abstracted to polynomial systems and the abstraction error is accounted
for through an additional enlargement of Zk+1to obtain an overapproximation.
Set Representation The road segments Rand the collision constraints Oare
described via polytopes. Since polytopes, as opposed to zonotopes, are closed underintersection, they can be used to prune unsafe solutions from the reachable set. We
use the MPT toolbox [ 16] for polytope operations. To be able to convert sets back
between zonotopes (for reachability) and polytopes (for other purposes) representa-tion, we use CORA to compute an overapproximation, i.e., an encompassing convex
hull of the polytope. Note that non-convex regions are stored as an array of convex
regions. Thus, any error caused by the conversion can be neglected.
Set Pruning As expressed in Algorithm 1, we check for unsafe states and prune solu-
tions that are in collision with either road boundaries or other cars, which generally
yields non-convex sets (line 8). These operations are performed using polytopic repre-
sentations. Note that any non-convex safe reachable set /tildewideZ
k=/uniontextLk
/lscript=1/tildewideZk,/lscriptis segmented
into Lkconvex regions /tildewideZk,/lscriptand stored accordingly. This is crucial as CORA requires
convex input sets. The next reachable set is computed as Zk+1=/uniontextLk
/lscript=1f(/tildewideZk,/lscript,U)
applying CORA separately to each convex region /tildewideZk,/lscript, see Fig. 5for an example.
Reduction of Complexity Representing sets as a union of convex sets, however,
leads to an exponential growth in the number of convex segments Lk, because: (1)
each pruning Zk,/lscript\Okcan lead to a split into more convex regions, and (2) the
reachability tool splits large partial sets /tildewideZk,/lscriptinto further subregions to minimize the
error associated with the underlying approximation procedure. Exponential growth
inLkwill inevitably yield exponential growth in runtime as we must compute the
reachable set individually for each of the Lksubsets. Thus, this calls for an efﬁcient
and effective reduction method in order for the procedure to maintain computational
tractability.
Compositional and Contract-Based V eriﬁcation … 173
Due to the constraint checks during each time k, many subsets /tildewideZk,/lscriptare reduced to
a negligible size, e.g. subsets that overlap the roadway, i.e., Zk,/lscript∩¬ R/negationslash=∅. This can
be exploited by omitting any subsets for which μ(/tildewideZk,/lscript)/lessmuchmax/lscriptμ(/tildewideZk,/lscript)at each time
k(1). Moreover, neighboring subsets /tildewideZk,/lscript,/tildewideZk,/lscript/primethat will be propagated separately,
are likely to overlap at time k+1, i.e., /tildewideZk+1,/lscript∩/tildewideZk+1,/lscript/prime/negationslash=∅. A simple iteration over
all possible pairwise combinations of sets can be used to check if the one set is asubset of the other; If so, that set may be removed from the list. The downside of
this approach is that it runs in O(L
2
k)time. Since we may expect a large reduction in
the number of sets, a divide and conquer approach is applied, where we recursively
divide the overall set {/tildewideZk,/lscript}/lscriptto perform pairwise checks on smaller sets of subsets (2).
Our empirical results show that the runtime can be signiﬁcantly reduced (up to
99%) when methods (1) and (2) are applied at the end of each time step. The sameprinciples are applied during backpropagation to maintain a tractable representation
of the relevant sets.
We also note that the runtime is dependent on the number of vehicles, since each
additional obstacle increases the complexity of O
k. Empirically, we found that up to
5–7 vehicles can be added before noticing signiﬁcant increases in the runtime.
5 Case Study
In this section, we will instantiate the veriﬁcation framework to verify a speciﬁc
receding horizon controller, also referred to as model predictive controller (MPC) that
abides by the controller contract S. In the following, we specify the dynamic motion
model and the vehicle’s dynamic limitations, as well as, the collision constraintsfrom other vehicles and the drivable space limitations are formulated, resulting in
the controller contract S. At the end of the section, the Manhattan road library and
trafﬁc model, which are to be veriﬁed in the case study, together with the results, arepresented. In the following, the Minkowski sum is denoted by ⊕.
Dynamic Motion Model and Dynamic Constraints We follow the nonlinear MPC
f o r m u l a t i o ni n[ 30] and employ a car model with a ﬁxed rear wheel and a steerable
front wheel with state zand controls u.A tt i m e k, we denote the state of the ego-
vehicle, typically position p
k=[xk,yk]∈ R⊆R2, linear velocity vk, orientation
θkand steering angle δk,b y zk=[pk,θk,δk,vk]∈ Z⊆R5, and the conﬁguration
byqk=[pk,θk]∈ C⊆SE(2). Its control input, typically steering velocity ˙δkand
acceleration ak, is labeled uk=[uδ
k,ua
k]∈ U⊂R2. The rear-wheel driven vehicle
with inter-axle distance Land continuous kinematic model
˙z=⎡
⎢⎢⎢⎢⎣˙x
˙y
˙θ
˙δ
˙v⎤
⎥⎥⎥⎥⎦=⎡
⎢⎢⎢⎢⎣vcos(θ)
vsin(θ)
v
Ltan(δ)
0
0⎤
⎥⎥⎥⎥⎦+⎡
⎢⎢⎢⎢⎣00
00
00
10
01⎤
⎥⎥⎥⎥⎦/bracketleftbiggu
δ
ua/bracketrightbigg
/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
u, (1)
174 L. Liebenwein et al.
is described by a discrete time model by integration zk+1=zk+/integraltext(k+1)/Delta1t
k/Delta1t˙zdt=
f(zk,uk), where /Delta1tis the sampling period.
We limit the steering angle, |δ|≤δmax, steering speed, |uδ|≤˙δmax, longitudinal
speed, |v|≤vmax, breaking and accelerations amin≤ua≤amax, such that they con-
form to the dynamical limitations and the rules of the road. The yaw-rate is limited to
|˙θ|≤˙θmax, allowing to neglect slip. The modiﬁcation is in line with our main goal:
driver safety. While our choice of motion model considers a more conservative yaw-
rate constraint, the veriﬁcation framework allows for straight-forward integration of
more advanced motion models including slip and load-transfers. Uncertainty in thedynamical model may be accounted for through proper enlargement of the reachable
set.
Other vehicles In the following, we will derive the representation of the safety
constraints with respect to the other vehicles. To ensure real-time operation, motion
planners frequently approximate their own or other cars’ footprint B
i(zi
k)by simpler
geometries such as rotation invariant bounding boxes, enclosing ellipses, or poly-
gons. For the reachability analysis, we approximate the shape of other vehicles by a
polygon, enclosing the ellipse used by the MPC of [ 30]. Note that accounting for the
ego-car’s shape in the reachable set can become intractable due to the non-convex,
disjoint nature of the set. We propose an approach, where we instead compute the
Minkowski sum of the other vehicle’s polygon and the ego vehicle’s rectangularshape for each possible difference in conﬁguration /Delta1q
i
k=qk−qi
kto form a single
representation of the collision region Ci
k(qk)=Bi(zi
k)⊕B(zk),Ci
k(qk)⊂R.T h e
resulting volume Ci
k(SE(2))⊂Ccan be represented as a single, invariant shape in
the/Delta1q-space, where /Delta1q=qi−q, see Fig. 3a. This volume is translated and rotated
according to each of the other vehicles’ poses qi
kto obtain the actual constraint in
the conﬁguration space of the ego-car. To reduce the computational complexity inthe implementation, a coarser overapproximation is chosen instead.
Drivable space We take a similar approach to obtain the drivable space, where we
compute the Minkowski sum between the ego-car’s footprint, and the non-road sur-
face over all conﬁgurations qof the ego-car. The complement, i.e., the drivable space
D(q)=¬(¬R⊕B(q)),can then be lifted into a conﬁguration volume D(SE(2))
containing the allowed ego-conﬁgurations, see Fig. 3b. An underapproximation with
fewer θ-slices is used in the implementation to reduce the computational effort.
Admissible conﬁgurations An intuitive visualization of the ﬁnal admissible con-
ﬁguration volume A
k⊂Ccan be obtained by subtracting the other vehicle’s swept
volume from the drivable conﬁguration volume: Ak=D(SE(2))/backslashBig/uniontext
i∈{1,...,N(k)}
Ci
k(SE(2)),see Fig. 4. After lifting the admissible conﬁguration volume into the
state space Z, we now have the unsafe states Ok=g−1(¬Ak)in a simple form,
which enables us to efﬁciently compute the collision constraints once and enforcethem by simply applying a set difference operation to the forward propagated state
setZ
k, as shown in Algorithm 1, line 8.
Compositional and Contract-Based V eriﬁcation … 175
(a)Car-collision con ﬁgurations (b) Drivable space
Fig. 3 a The exact Minkowski swept volume between the ego-car and another vehicle, which
represents conﬁgurations in collision, is shown. bDrivable space in a four-way intersection left-
turn scenario, obtained by taking the Minkowski sum between the rectangular ego-car and the road
over/Delta1q, i.e.,D(SE(2)). This is used to trim the admissible state space. Because of symmetry and
for brevity θis only shown for [0,π]. Approximations to 10 θ-slices are used in the implementation
to reduce computational complexity
10
Fig. 4 The dynamic obstacles (black) and the drivable space (grey) are shown for time kon the
left. Subtraction yields the admissible conﬁguration volume Ak(right)
Controller Contract SWe summarize the speciﬁc instantiation of the controller
contract Sto be veriﬁed below:
•Safety (obstacle avoidance): Ok∩/tildewideZk=∅,∀k,
•Speed limit (trafﬁc rules): |v|≤vmax,
•Dynamic limits: |δ|≤δmax,|uδ|≤˙δmax,amin≤ua≤amax.
Road Library and Network We showcase the framework for a Manhattan-style
library consisting of 17 intersections and 5 straight road segments. Roads can have
one or more lanes and incoming roads into an intersection may be tilted with respect
to each other, see Fig. 7for a selection of veriﬁed models. The library is used to verify
the network shown in Fig. 1, which consists of ca. 130 blocks in Mid-Manhattan with
ca. 180 intersections and ca. 330 straight road segments.
Trafﬁc Model We describe other trafﬁc participants as a hybrid system, i.e., a set of
trafﬁc ﬂows. Each trafﬁc ﬂow is speciﬁed by a predeﬁned path, including possible
lane changes, connecting regions for entering ( Ij) and leaving ( Oj/prime) road model mas
well as a velocity proﬁle, together forming a trajectory. The trafﬁc scheduler Sspawns
176 L. Liebenwein et al.
Fig. 5 The forward propagation of the reachable set is shown for a left turning maneuver. Blue sets indicate the safely reachable conﬁguration set g(/tildewideZk)(top
row), respectively the position set h(/tildewideZk)(bottom row) of the ego-car for various times k. Black sets mark the swept volume Ci
k(SE(2))(top row), respectively
the footprint Bi(zi
k)(bottom row) of other trafﬁc participants. The entry and exit sets are shown in green and red, respectively. Note how the ego-car maintains
a safe distance to the other cars and the road boundaries at all times
Compositional and Contract-Based V eriﬁcation … 177
Fig. 6 Backward propagation on the four-way intersection mfor various times k.A tt=6.0s ,w e
start out at /hatwideZH=exit(m,1,4)=O4and compute the backward reachable set /hatwideZk(marked blue)
for each time kto obtain the safe entry set entry(m,1,4)
vehicles at some region Ijwith a ﬁxed frequency (0.1–0 .4s−1)1and removes them
once they reach Oj/prime. Trajectories are deﬁned as arc-length parametrized, continuously
differentiable clothoid spline paths and velocity proﬁles generated by cubic Hermite
spline interpolation. For simplicity, we restrict the class of controllers associated
with the other trafﬁc cars to open-loop.
Results The proposed procedure was successfully applied to the aforementioned
library, including the four-way intersection min Fig. 5, where the veriﬁed left-turn
maneuver is shown. The considered time horizon is 6 .0 s with a discrete timestep
/Delta1t=0.05 s resulting in 120 iterations. In the shown example there are two streams of
cars occupying the intersection, both of which the ego-car avoids. When the second
car crosses the intersection all states in collision are pruned causing the reachable set
to become disjoint ( t=3.6s ,t=4.6 s). However, as the ego-car controller has to
abide by the controller contract Ssafety remains veriﬁed. Upon the exit of the second
car, the safe reachable set is expanding again reﬂecting the fact that the intersection
has become available ( t=5.8s ) .
As visible from Fig. 5the ego-car reaches the entire exit set, i.e., exit(m,1,4)=
/tildewideZH∩O4=O4. Next, the safe entry set entry(m,1,4)is computed by means of
backward propagation, see Algorithm 1 for details. The results of the backward
propagation are shown in Fig. 6.
The remaining library was veriﬁed analogously. Note that for intersections, where
there are multiple entry and exit regions, all combinations must be tested in order for
the model to be deemed safe. This results in a total of 83 experiments for the 22 road
models considered. In Fig. 7, a selection of models with various geometries is shown
together with the reachable set and trafﬁc for some time step of the veriﬁcation.
Consequently, the library was matched with the topology graph of Mid-Manhattan,
see Fig. 1, and the compositions were tested for safety according to Algorithm 2. All
compositions have been deemed safe. Henceforth, any car, abiding by the controller
1This range covers a large variety of situations, from occasional vehicles to dense car streams.
178 L. Liebenwein et al.
t = 2.55s t = 6.00s t = 1.25s t = 4.35s
t = 4.55s t = 1.00s t = 4.00s t = 4.50s
t = 5.40s t = 2.22s t = 3.85s t = 3.55s
Fig. 7 A selection of veriﬁed road models, comprised of various intersections and straight roads,
is shown together with the reachable set (blue) and other trafﬁc participants (black) at the indicated
timestep. The initial and ﬁnal set are marked green, respectively, red
Forward propagation Backward propagationReachability Road Other cars Reduce Reachability Constraints Reduce0100200300400500Computation time [s]
Fig. 8 The box plot indicates computation times for various parts of one iteration with a ﬁxed
timestep of /Delta1t=0.05 s averaged over all conducted experiments
contract and the assume-guarantee contracts, can safely transit through the network
under the assumption of the used trafﬁc model.2
In Fig. 8, average computation times are shown for one iteration. We observe that
the largest cost comes from the reachability analysis itself. We also observe large
variations in the computation times, arising from the varying level of complexity, i.e.,
2Further details of the case study and the library may be found in the supplementary material.
Compositional and Contract-Based V eriﬁcation … 179
level of fragmentation of the reachable sets. The backward propagation shows a sig-
niﬁcantly higher computational demand due to the generally more fragmented sets
caused by the computationally more intense constraints. We conducted our experi-ments on an Intel Xeon E5-2680 2.8 GHz 16 Core CPU with each experiment running
on a separate core. On average one experiment took 21 h.
6 Conclusions
We studied the problem of safety veriﬁcation of controllers for autonomous vehicles
and proposed a novel framework for synthesizing safety guarantees for entire roadnetworks building upon compositional veriﬁcation and assume-guarantee contracts.
Our framework is based on verifying a library of local road models against given ego-
car and trafﬁc models, concurrently with synthesizing assume-guarantee contractsused for composition. The library can then be used to certify the safety of executing
ego-car controllers satisfying a controller contract over road networks. We further
demonstrated the effectiveness of our approach on a case study involving a library of
local road models, which enabled us to verify a substantial part of Mid-Manhattan.
Avenues for future work include extending the library of locally veriﬁed road
models to capture a wider range of road geometries. We plan to use the extended
library to certify larger, more complex road networks. At the trafﬁc level, we want to
extend the framework to handle more realistic behaviors of other vehicles and trafﬁcsituations, as well as relax the assumptions about the knowledge of other trafﬁc
participants. Finally, we are interested in complementing the veriﬁcation framework
with a roll-out strategy for controllers that leverages the local road models’ safetycertiﬁcates. The goal is to provide online constraints that ensure the long-term safety
of the ego-car.
References
1. Althoff, M.: An Introduction to CORA. In: ARCH, CPSWeek. https://cps-vo.org/node/20302
(2015)
2. Althoff, M., Dolan, J.M.: Online veriﬁcation of automated road vehicles using reachability
analysis. IEEE T-RO 30(4). https://doi.org/10.1109/TRO.2014.2312453 (2014)
3. Alur, R., Moarref, S., Topcu, U.: Compositional synthesis with parametric reactive controllers.
In: ACM HSCC. https://doi.org/10.1145/2883817.2883842 (2016)
4. Balachandran, S., Ozay, N., Atkins, E.M.: V eriﬁcation guided reﬁnement of ﬂight safety assess-
ment and management system for takeoff. J. Aerosp. Inf. Syst. 13.https://doi.org/10.2514/1.
I010408 (2016)
5. Bautin, A., Martinez-Gomez, L., Fraichard, T.: Inevitable collision states: a probabilistic per-
spective. In: IEEE ICRA. https://doi.org/10.1109/ROBOT.2010.5509233 (2010)
6. Buehler, M., Iagnemma, K., Singh, S.: The DARPA Urban Challenge: Autonomous V ehicles
in City Trafﬁc, vol. 56. Springer, Berlin. https://doi.org/10.1007/978-3-642-03991-1 (2009)
7. Chan, N., Kuffner, J., Zucker, M.: Improved motion planning speed and safety using regions
of inevitable collision. In: 17th CISM-IFToMM Symposium on Robot Design, Dynamics, and
180 L. Liebenwein et al.
Control. http://ri.cmu.edu/pub\_ﬁles/pub4/chan\_nicholas\_2008\_1/chan\_nicholas\_2008\_1.pdf
(2008)
8. Dagan, E., Mano, O., Stein, G.P ., Shashua, A.: Forward Collision Warning with a Single
Camera. In: IEEE IV . https://doi.org/10.1109/IVS.2004.1336352 (2004)
9. Dallal, E., Tabuada, P .: Decomposing controller synthesis for safety speciﬁcations. In: IEEE
CDC. https://doi.org/10.1109/CDC.2016.7799148 (2016)
10. DeCastro, J.A., Kress-Gazit, H.: Synthesis of nonlinear continuous controllers for veriﬁably-
correct high-level, reactive behaviors. IJRR 34(3).https://doi.org/10.1177/0278364914557736
(2015)
11. Ding, J., et al.: Hybrid Systems in Robotics: Toward reachability-based controller design. IEEE
Robot. Autom. Mag. 18(3). https://doi.org/10.1109/MRA.2011.942113 (2011)
12. Erlien, S.M., Fujita, S., Gerdes, J.C.: Shared steering control using safe envelopes for obstacle
avoidance and vehicle stability. IEEE T-ITS 17(2).https://doi.org/10.1109/TITS.2015.2453404
(2016)
13. Fraichard, T., Asama, H.: Inevitable collision states. A step towards safer robots? In: IEEE/RSJ
IROS. https://doi.org/10.1109/IROS.2003.1250659 (2003)
14. Furgale, P ., et al.: Toward automated driving in cities using close-to-market sensors: An
overview of the V-Charge Project. In: IEEE IV . https://doi.org/10.1109/IVS.2013.6629566
(2013)
15. Henzinger, T.A., Qadeer, S., Rajamani, S.K.: Y ou assume, we guarantee: Methodology and
case studies. Springer, Berlin. https://doi.org/10.1007/BFb0028765 (1998)
16. Herceg, M., Kvasnica, M., Jones, C., Morari, M.: Multi-Parametric Toolbox 3.0. In: Proceedings
of the European Control Conference. http://ieeexplore.ieee.org/document/6669862/ ,http://
control.ee.ethz.ch/~mpt (2013)
17. Hoehener, D., Huang, G., V ecchio, D.D.: Design of a lane departure driver-assist system under
safety speciﬁcations. In: IEEE CDC. https://doi.org/10.1109/CDC.2016.7798632 (2016)
18. Kalra, N., Paddock, S.M.: Driving to Safety: How Many Miles of Driving Would It Take to
Demonstrate Autonomous V ehicle Reliability? Transp. Res. Part A: Policy Pract. 94.http://
www.rand.org/pubs/research\_reports/RR1478.html (2016)
19. Karaman, S., Frazzoli, E.: Optimal kinodynamic motion planning using incremental sampling-
based methods. In: IEEE CDC. https://doi.org/10.1109/CDC.2010.5717430 (2010)
20. Kim, E.S., Arcak, M., Seshia, S.A.: Compositional controller synthesis for vehicular trafﬁc
networks. In: IEEE CDC. https://doi.org/10.1109/CDC.2015.7403189 (2015)
21. Mitchell, I.M., Bayen, A.M., Tomlin, C.J.: A time-dependent Hamilton-Jacobi formulation of
reachable sets for continuous dynamic games. IEEE Trans. Autom. Control. 50(7).https://doi.
org/10.1109/TAC.2005.851439 (2005)
22. National Safety Council: NSC motor vehicle fatality estimates (2012–2015)23. NHTSA trafﬁc safety facts. https://crashstats.nhtsa.dot.gov/ (2015)
24. Nilsson, P ., et al.: Correct-by-construction adaptive cruise control: two approaches. IEEE Trans.
Control. Syst. Technol. 24(4). https://doi.org/10.1109/TCST.2015.2501351 (2016)
25. Nohmi, M., Fujikura, N., Ueda, C., Toyota, E.: Automatic braking or acceleration control system
for a vehicle. https://www.google.com/patents/US4066230 (1978). US Patent 4,066,230
26. O’Kelly, M., et al.: APEX: A tool for autonomous vehicle plan veriﬁcation and execution. In:
SAE World Congress and Exhibition. http://papers.sae.org/2016-01-0019/ (2016)
27. Paden, B., Cap, M., Y ong, S.Z., Yershov, D., Frazzoli, E.: A survey of motion planning and
control techniques for self-driving urban vehicles. IEEE T-IV 1(1). https://doi.org/10.1109/
TIV .2016.2578706 (2016)
28. Rungger, M., Zamani, M.: Compositional Construction of Approximate Abstractions of Inter-
connected Control Systems. IEEE Trans. Control. Netw. Syst.
https://doi.org/10.1109/TCNS.
2016.2583063 (2016)
29. Sadraddini, S., Rudan, J., Belta, C.: Formal synthesis of distributed optimal trafﬁc control
policies. In: International Conference on Cyber-Physical Systems. https://doi.org/10.1145/
3055004.3055011 (2017)
Compositional and Contract-Based V eriﬁcation … 181
30. Schwarting, W., et al.: Parallel autonomy in automated vehicles: safe motion generation with
minimal intervention. In: IEEE ICRA. http://hdl.handle.net/1721.1/110365 (2017)
31. Shia, V .A., Gao, Y ., V asudevan, R., Campbell, K.D., Lin, T., Borrelli, F., Bajcsy, R.: Semiau-
tonomous vehicular control using driver modeling. IEEE T-ITS 15(6).https://doi.org/10.1109/
TITS.2014.2325776 (2014)
32. Tabuada, P .: V eriﬁcation and control of hybrid systems: a symbolic approach. Springer Science
& Business Media, Berlin (2009)
33. Urmson, C., et al.: Autonomous driving in urban environments: boss and the urban challenge.
J. Field Robot. 25(8). https://doi.org/10.1002/rob.v25:8 (2008)
34. Wongpiromsarn, T., Topcu, U., Murray, R.: Receding horizon temporal logic planning. IEEE
Trans. Autom. Control. 57.https://doi.org/10.1109/TAC.2012.2195811 (2012)
35. Zamani, M., Pola, G., Mazo, M., Tabuada, P .: Symbolic models for nonlinear control systems
without stability assumptions. IEEE Trans. Autom. Control. 57(7). https://doi.org/10.1109/
TAC.2011.2176409 (2012)
Towards Interaction, Disturbance
and Fault Aware Flying Robot Swarms
Teodor Tomi´ c and Sami Haddadin
1 Introduction
With the maturing of onboard visual navigation and perception for ﬂying robots,
research has partly focused on developing physical interaction and manipulationcapabilities for these machines. In order to deploy such capabilities at a large scale, a
high level of robustness against faults and disturbances is required. With the majority
of the literature focusing on basic capabilities such as interaction control and (coop-
erative) aerial manipulation, handling general classes of faults and disturbances has
not been covered in great detail and is still in its infancy.
In this paper, we provide a synopsis of recent developments in physically interact-
ing ﬂying robots, put them in a larger taxonomical context, and develop a generalized
concept of interacting ﬂying robots at scale, such as in a swarm. A main contributionis the the introduction of the awareness pipeline that uniﬁes the representation of
interaction, disturbance, and fault awareness for a single ﬂying robot as well as ﬂy-
ing robot swarms. We provide a synopsis of already developed individual elementsof the pipeline from the literature, and speculate about future research directions that
would validate our proposed approach.
Interaction, disturbance, and fault aware ﬂying robots . Figure 1depicts a ﬂy-
ing robot system (single robot, robot group or robot swarm) that is simultaneously
subject to (possibly multiple) physical interactions (e.g. from a human, or manipu-
lation), wind disturbances, and external faults such as collisions. An interaction is a
T. Tomi´ c(B)
Skydio, 114 Hazel Ave, Redwood City, CA 94061, USAe-mail: teo@skydio.com
S. Haddadin
Chair of Robotics and Systems Intelligence, Munich School of Robotics and MachineIntelligence, Technical University of Munich (TUM), Heßstr. 134,80797 Munich, Germanye-mail: sami.haddadin@irt.uni-hannover.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_19183
184 T. Tomi´ c and S. Haddadin
Interaction [42] Disturbance [44] Fault [42]
Flying Robot
System+τiτwcollision fault
τe
impedance control
reactioncollision re ﬂex
reactioncompensate τw
Single
RobotRobot group Robot swarmmember of member of
Fig. 1 An interaction, disturbance, and fault aware ﬂying robot system understands the source and
context of its simultaneous inputs and reacts accordingly
desired dynamic behavior of the robot w.r.t. some input quantity. A disturbance is an
uncontrolled input to the system, while a fault is an unpermitted deviation of at least
one characteristic property or parameter of the system from the nominal operating
condition [ 40]. A hypothetical interaction, disturbance and fault aware ﬂying robot
system will be able to sense these inputs and safely react consistent to a task-speciﬁc
context. Furthermore, by exploiting contextual information from disturbance mod-
els we argue that fundamentally new interaction paradigms may be achieved that
allow e.g. physical interaction under wind inﬂuence, or following the wind speed
gradient. Figure 2depicts a speculative application of such a system. The task is to
cooperatively slide an object along a compliant surface, under wind inﬂuence, while
generating an accessible 3D map of the surface. A robot in the swarm serves as haptic
input device for a human user. The system is composed of three robot groups. The
blue group preforms cooperative slung load transport [ 10]. The red group grasps [ 19]
for controlling the horizontal motion of the object. The green group is maintaining
visual line of sight. To achieve complex interaction tasks, ﬂying robots swarms must
beaware of all involved interactions, in particular physical ones. In Sect. 3we further
reﬁne the considered types of awareness.
Swarm hierarchy . The ﬂying robot swarm can be subdivided into multiple hier-
archical subsystems: propulsion ,ﬂying robot ,robot group , and robot swarm . Each
subsystem subsumes multiple units of the lower level subsystem, and is subject to
speciﬁc disturbances and faults (e.g. wind, latency, packet loss). Faults may occur at
each hierarchy level and propagate upwards. Each subsystem is assigned prioritized
tasks (e.g. trajectory tracking, formation, obstacle avoidance). Obviously, higher
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 185
Interaction controlled
flying robotsVirtual spring
Line of sight, mapping,
visual feedback
Slung load transportFormationRigid surface
Virtual object
setpointCooperatively
transported
object
Wind
Human
Fig. 2 Exemplary hypothetical future scenario of cooperative ﬂying robot swarms (top view). So
far, no uniﬁed framework for dealing with such problems exists
Table 1 Examples of ﬂying robots physically interacting with the environment
Application Related work
Individual robots
Interaction control Impedance control [ 26,35,36,43,45], admittance control [ 4,37,
43,45], force control [ 9,33], contact inspection [ 2]
Load transport Slung load transport [ 10,41], aerial manipulators [ 1,18]
Reconﬁguration Whole-body manipulation [ 51], full controllability [ 11,37,38]
Groups of robots
Manipulation Transport [ 25,27], grasping [ 19], aerial Manipulators [ 6,29]
Assembly/Construction Building structures [ 5]
Formation control Energy tank based [ 17]
Physical docking Distributed ﬂight array [ 32]
level tasks should not destabilize presumably safety critical lower level tasks—for
example, formation control should not destabilize vehicle trajectory tracking or local
collision avoidance. Lower hierarchy level tasks therefore have higher priority. Dis-
turbances may be compensated and potentially exploited, and faults must be handled
in a hierarchical manner due to propagation. In the paper, we explore methods to
handle interaction, disturbances, and faults at multiple hierarchy levels.
Physically interacting ﬂying robots . Table 1lists some examples of state of the
art developments in physical interaction control of ﬂying robots. Note that so far
aerodynamic effects due to wind are largely ignored in the literature, or they are
simply compensated as a lumped disturbance. As of today, this limits the applica-
bility of physical interaction methods in harsh environments. We aim to develop a
framework that also provides a roadmap and guidelines for future developments to
enhance existing methods with disturbance and fault awareness.
186 T. Tomi´ c and S. Haddadin
2 System Dynamics
System dynamics with disturbances and faults . In this paper we consider inter-
action ,disturbance andfault aware ﬂying robots, which should be reﬂected in their
dynamics formulation. We start with the ideal system dynamics of an individual
ﬂying robot, which may be described by the Lagrangian dynamics
M(q)¨q+g(q)+C(q,˙q)˙q=τ, (1)
where M(q)is the generalized inertia matrix, qare generalized coordinates, g(q)
is the generalized gravity vector, C(q,˙q)is the matrix of Coriolis and centripetal
terms, and τis the control input. For an individual ﬂying robot without an articulated
structure, qis the position and orientation of the robot, and τis the control wrench.
The actual system dynamics is subject to disturbances, faults, and parameter and
structural changes [ 16,40]. Dropping arguments, we may write
(M+ΔM)¨q+Δg+g+(ΔC+C)˙q=τ+Δτ+τd+τf,e, (2)
where τdis the generalized disturbance (e.g. aerodynamics), τf,eare the additive
external faults . The preﬁx Δdenotes a change in the respective parameter or wrench,
for example Δτis the change of propeller thrust due to airspeed. External faults
violate the free-ﬂight assumption, and are caused by external factors. Examples
are collisions, contacts, undesired constrained motion, etc. The proper reaction is
to change the control objective, and/or escalate to higher level planning. We alsoconsider multiplicative model faults τ
f,mdue to parameter uncertainty or a change
in the system dynamics, such as loss of motor actuation, change of aerodynamic
coefﬁcients, a shifting center of mass, or structural failure. The reaction is e.g. astructural change or adaptation of the controller. We make this distinction because
the two fault types differ in effect and required recovery, see Sect. 3.3. By deﬁning
τ
f,mfrom ( 2)a s
τf,m(q,˙q,τ,t)=Δτ−ΔM(q)¨q−Δg(q)−ΔC(q,˙q)˙q, (3)
the dynamics of a single robot with disturbances and faults may be written as
M(q)¨q+g(q)+C(q,˙q)˙q=τ+τd+τf, (4)
where faults are lumped into a single fault wrench τf=τf,m+τf,e.
System dynamics of a ﬂying robot swarm . In the swarm, we assume that g(q)and
C(q,˙q)˙qare compensated by the local controllers at individual robot level, and a set
of controllable coordinates qcis exposed to the swarm level. Quadrotors, for example,
are underactuated and their local controller ensures tracking of a Cartesian position
and orientation about the body z−axis. For ﬂying robots with full controllability
(e.g. [ 34,37]),qcwill contain the full 6D pose. For a swarm of robots, let the swarm
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 187
generalized coordinates qscontain the controllable coordinates qcof each robot. We
may then simplify the dynamics equations of the swarm to
Ms¨qs=τs, (5)
where Msis the generalized swarm inertia matrix, and τsis the swarm control input
in the generalized coordinates. The dynamics with swarm-level disturbances and
faults become
Ms¨qs=τs+τd,s+τf,m,s+τf,e,s, (6)
where τd,s,τf,m,s, andτf,e,sare swarm-level disturbances, modeling, and external
faults, respectively. Note that these equations have been presented in order to deﬁne
the nomenclature and apply model-based concepts to the problem. They present the
basic equations for the awareness pipeline. In the following sections, we discuss
speciﬁcs of disturbance, interaction control, and fault awareness.
3 Awareness Pipeline and Interaction Control
To deal with complex tasks, ﬂying robot swarms must be aware of physical interac-
tions. We conceptualize this awareness through the novel concept awareness pipeline ,
depicted in Fig. 3a. It is derived from a deductive sequential question-driven process
to generate knowledge. This is a generalization of a fault handling pipeline introduced
in Haddadin et al. [ 21] to interactions and disturbances.
The choice of pipeline deﬁnes the scope of the awareness. First, the robot must
know about the presence of an object (anything from a real physical object, a data-
driven signal input to the arising of a monitoring signal). Second, an isolation step
Presence Isolation Identi ﬁcation Classi ﬁcation ResponseContext
Is the object there? What is its origin? How does it
evolve over time?What is its
purpose?How should I
respond to it?
(a) The awareness pipeline for structural handling of faults, disturbances, and interaction.
Input
discriminationContext
External wrench τe
Fault pipelineInteraction pipelineDisturbance pipelineˆτd
ˆτfˆτi
Robot behaviorModels Fault inputs τf
Disturbance inputs τd
Interaction inputs τi Sensors
(b) Interaction, disturbance and fault awareness pipelines run in parallel.
Fig. 3 The awareness pipeline and its parallel instantiation
188 T. Tomi´ c and S. Haddadin
determines its origin (e.g. location). The identiﬁcation step determines the time evo-
lution of the object, enabling the classiﬁcation of its purpose. Once this is known, a
proper response may be taken by the robot.
The external wrench τeacting on the robot is a sum of all interaction, disturbance,
and fault inputs. Most control schemes operate under the assumption that only the
interaction wrench acts on the robot [ 9,26,43]. Consider the interaction in Fig. 1,
where wind is added to the interaction wrench. In order to simultaneously compensate
for the aerodynamic disturbance, perform impedance control towards the human, and
react to collisions, the ﬂying robot must be able to discriminate between the separateforces acting on it. Input discrimination is therefore a basic enabling component for
running awareness pipelines in parallel, see Fig. 3. It may use sensors (e.g. IMU, force
sensing), models (e.g. dynamics, geometry), estimates and context (e.g. environmentmaps, task deﬁnitions, proximity of humans). We discuss it in detail in Sect. 3.2.
Discriminated wrenches are used as inputs to the respective pipelines. The overall
system behavior is a composition of the pipeline responses. Awareness pipelines runat each swarm hierarchy level. Note that contextual input can be provided by other
levels in the hierarchy. In the following sections, we discuss each pipeline in more
detail.
3.1 Disturbance Awareness Pipeline
Disturbance presence, isolation and classiﬁcation are usually obtained from context
information, during the controller design stage. Table 2lists the most common distur-
bances in ﬂying robots, and sensors that may be used to discriminate between them.Each disturbance type (the what ) requires a separate awareness pipeline, because
their interpretation and responses will differ. Identiﬁcation (i.e. time evolution of the
signal) may be used to adapt the response online. In the following, we discuss twodisturbance response strategies: compensation andexploitation .
Disturbance compensation . In most situations, it is desireable that a single
interacting ﬂying robot under the inﬂuence of disturbances maintains the simpli-ﬁed dynamics ( 5). The control law may be decomopsed as τ=τ
task+τcomp, where
τtaskis a nominal controller that executes the desired task(s), e.g. impedance con-
trol, while τcomp compensates the disturbances. A common approach is to lump all
disturbances and faults into one unknown input wrench, which is interpreted as a
disturbance and compensated. The lumped disturbance estimation is obtained by
an external wrench estimator [ 15,36,43], which is based on IMU measurements, a
dynamics model, and the control input. Then, methods such as disturbance observers
[23,36,42], or integral control may be applied for compensation. A compensated
impedance controller that reacts to the interaction force and compensates wind forces
has been proposed in Tomi´ ce ta l .[ 45].
Exploitation of disturbances . In some applications it may be beneﬁcial to exploit
a known model of the disturbance. Undoubtedly, there is large potential in exploiting
wind information, which can be obtained from the aerodynamic disturbance using
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 189
Table 2 A list of the most common disturbances acting on ﬂying robots
Type Description Sensing
Aerodynamic wrench Aerodynamic drag wrench due
to relative airspeed of the
ﬂying robot, caused by form
drag and blade ﬂapping of the
propellersModel [ 46,48], pitot tubes
[50], anemometer [ 13]
Propeller thrust Change of propeller thrust (i.e.
control input) with airspeed [ 7,
8]Motor power [ 8], force sensing
[14]
Payload Payload inertial parameters,
aerodynamic drag wrench on
the payloadOnline identiﬁcation [ 24]
Payload aerodynamic wrench Aerodynamic drag wrench on
the payload, during
cooperative transportOnline identiﬁcation ( open
problem )
Aerodynamic
wrenchFlying robot, ext.
wrench estimatorForce
discriminationAerodynamics
modelVehicle tasks Vehicle controller Vehicle plannerMap of
wind speedsGroup tasks Group controller Group plannerShared map of
wind speedsSwarm tasks Swarm controller Swarm planner
Wind vw τaero ˆτe ˆτaeroˆvw(x)ˆvw(x)ˆvw(x)ˆvw(xg)
ˆvw(xg)
xdxd,gxd,s
τf1f2
Fig. 4 A framework for exploiting an aerodynamics model to map the wind speed and use the
resulting wind map for planning on multiple levels of the hierarchy
model-based methods [ 44,46,48]. Initial advancements have been made toward
motion planning using known wind ﬁelds [ 47,49]. In Ware and Roy [ 47], motion
planning in an urban wind ﬁeld is performed that minimizes the required motor
power. The most promising direction seems to be building a shared map, as depicted
in Fig. 4. Instead of interpreting wind as a disturbance, disturbance context may be
shared among robots. For example, in Yao et al. [ 49] the authors learned a map of
disturbance wrenches to improve planning in the proximity of structures. Figure 4
depicts a generalized scheme that exploits a known aerodynamics model. Here, the
wind vwis causing an aerodynamics wrench τaeroon a ﬂying robot. The estimated
external wrench ˆτeis then used for discriminating between wrenches acting on the
robot, see Sect. 3.2. The result is an estimated aerodynamics wrench ˆτaero, which is
inverted in a learned aerodynamics model to obtain an estimate ˆvw.T h i si ss t o r e di n
190 T. Tomi´ c and S. Haddadin
a local map of wind speeds, and shared among multiple robots. The wind map may
then be used for planning e.g. energy-optimal trajectories xd, or to follow the wind
gradient. Lastly, the controller at each level commands a lower-priority task fito the
respective lower level controller, according to ( 8).
3.2 Interaction Aware Control and Sensing
Handling of aerodynamic effects during interaction . Discrimination between
interaction, disturbance, and fault wrenches is possible in a localized fashion, using
sensors for speciﬁc applications [ 2,12,35]. The interaction input may further be
discriminated from other inputs with the help of sensors as the ones listed in Table 3.
In Rajappa et al. [ 35] the authors use an array of force sensors to isolate and identify
the interaction wrench. However, force sensors are discretely localized, and a reli-able, lightweight wind sensor for multirotor UA V does not yet exist. In Tomi´ c and
Haddadin [ 44], collisions are discriminated from aerodynamic forces based on the
frequency characteristics of the signals. However, slow interaction forces and aero-
dynamics forces lie in the same frequency region. Approaching this from a different
perspective, we may use the known wind speed and an aerodynamics model to obtainthe aerodynamic wrench. This requires a wind sensing method that is independent of
estimating the external wrench, such as onboard pitot tubes [ 50] anemometers [ 13],
Table 3 Most typical sensors and models that enable the discrimination of contact forces acting
on ﬂying robots
Sensor Measures Description
IMU Acceleration, angularrate Commonly used for navigation (strapdown).May be used for external wrench estimation[43]
Force/Torque sensor Force/Torque Directly measures the wrench at certain
locations. Provides direct measurement of theinteraction wrench at the sensor’s position[35]. Useful e.g. for contact inspection tasks
[2], measuring payload properties [ 10], or
forces generated by propellers [ 14]
Tactile sensor Touch Can be installed on the robot hull as input
device for physical human-robot interaction
Bumper Touch Detect collisions and contacts at speciﬁc hullpoints [ 12]
Surface humidity
sensorHumidity Can detect and isolate environmentalconditions that are potentially harmful for therobot, for example a water avoidance reﬂex incaves
Strain gauge Deformation Measures bending of ﬂexible structures. Maybe used to directly measure the propellerwrench, or detect structural failure
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 191
or motor power and an aerodynamics model [ 46]. However, purely model based dis-
crimination between slow aerodynamic and contact forces is still an open research
problem. Lastly, uncertainty of the control wrench Δτdue to the propeller force
varying with airspeed will result in a model fault, reducing the accuracy of input
discrimination. Recent advances have been made to estimate [ 8] or directly measure
[14] the control input in ﬂying robots.
Model-based contact wrench isolation with minimal additional sensing, or at
least obtaining the contact position on the robot hull in a continuous fashion is
therefore an open problem. Figure 5shows preliminary results of an residual-based
force discrimination method that serves this purpose. It is based on the insight that
the aerodynamic torque can be expressed in terms of the aerodynamic force as a
function md:=md(fd). Detection of wrenches other than the aerodynamics is then
possible by evaluating the aerodynamic torque residual ˜md=md(fe)−me, which
will be close to zero (up to modeling errors) when only aerodynamics is acting on
the robot. Note that feis used for evaluation of the torque model. A contact force
fcis acting at location rcon the robot’s convex hull will therefore cause a nonzero
residual, which can be used for contact detection . This scheme is not able to detect
pure force contacts. We trained the model md(fd)from wind tunnel data obtained
in [46]. Figure 5shows the drag force fddue to constant 5 m/s wind with noise
for a hexacopter ﬂying to a waypoint and yawing. The contact force fcis virtually
injected and acts on the robot hull. Discrimination is obtained by slowing down
wind estimation when contact is detected. The aerodynamic force is obtained from a
forward airspeed model as ˆfd:=fd(ˆvr). Contact detection is fast, and discrimination
of the estimated contact force ˆfcand drag force ˆfdclosely matches ground truth. In
general, the quality of discrimination will depend on the aerodynamic models, wind
conditions and contact type.
−505fx[N]
fdˆfd fcˆfc
0 2 4 6 8 100.000.050.10˜md[N m]residual
threshold
Fig. 5 Preliminary results of discrimination between aerodynamic and contact forces in simulation,
based on models identiﬁed using wind tunnel data [ 46]. Note that the other axes show analogous
behavior
192 T. Tomi´ c and S. Haddadin
Multi-priority task deﬁnition . After the interaction input has been discriminated,
the particular response of the interaction pipeline in Fig. 3is deﬁned by one or more
prioritized tasks . The response of the pipeline is then a generalized control force
obtained from a multi-priority task controller. Our belief is that multi-priority task
control for swarms will become an important research topic. Tasks may be deﬁned
w.r.t an operational space O[22]. The operational space task xcan be mapped to the
generalized coordinates qvia the Jacobian J(q):=∂x
∂q. For example, the operational
space of a quadrotor may be its position and yaw, while its generalized coordinates are
its full 6-DoF pose. A controller in the operational space will command a generalizedwrench f(x), which may then be translated to the generalized coordinates by the map
τ=J
T(q)f(x). (7)
Due to redundancy, multiple tasks may be imposed on a robot or a swarm simul-
taneously. In particular, redundancy resolution methods from multi-priority controlof redundant robot manipulators and humanoids may be extended to ﬂying robot
swarms. The basic idea is to project lower-priority tasks into the nullspace of higher-
priority tasks so they do not interfere with each other [ 1,3,6]. Doing this at the swarm
level dynamics ( 5) would result in a hierearchical multi-priority wrench swarm con-
troller. Note that this is still an open research problem. Alternatively, consensus algo-rithms can be used, see Olfati-Saber et al. [ 31] and references therein. The control
input for task with priority iwould then be given by
τ
i=τi−1+(JiPi−1)−T(fi−Jiτi−1), (8)
where fiis the desired operational space wrench of task i, andτ0=0. The matrix Pi
is the projection operator onto the nullspace of matrix Ji(i.e.Pi=I−J+
iJi)[39].
This state-of-the art approach decouples tasks and ensures that the higher priority
tasks are not perturbed by lower priority tasks, at the cost of reduced perfomance ofthe latter. Of course, it is important to have good models for all levels of the swarm
hierarchy. Well-researched simultaneous tasks for swarms include maintaining a
formation, obstacle avoidance, and tracking desired positions [ 30].
Operational space interaction behaviors . In the context of interaction, we may
also include desired physical interaction with the environment and/or objects to be
manipulated, such as admittance [ 4,37,43,45], impedance [ 26,35,36,43,45], or
force control [ 2,9,33]. Typically, impedance control for the tracking case is deﬁned
through the desired error dynamics
M¨x+D˙x+K(x−x
∗)=fi, (9)
where x∗is the desired equilibrium state, and DandKare appropriate damping
and stiffness gain matrices, respectively. Operational space controllers f(x)may be
deﬁned on the robot, group [ 6,51] or swarm level, see Fig. 6. Only the task Jacobian
must be changed. Note that using this framework, also perception-based tasks on all
hierarchy levels may be included.
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 193
x∗
(a) Swarm formation ﬂight of virtual
springs in a leader-follower con ﬁgura-
tion.xox∗
o
(b) Cooperative grasping with ﬂying
robots through intermediate frames.
Fig. 6 Operational space behaviors on swarm level imposed via virtual springs
Physical interaction with swarms . There have been signiﬁcant advancements in
cooperative aerial manipulation, e.g. [ 6,25,51]. However, physical interaction with
a ﬂying robot swarm is still and unresearched area. External wrench estimation for
robot swarms would enable novel physical interaction paradigms, such as moving the
swarm through physical interaction with its agents. Furthermore, input discrimination
may be applied at higher levels of the hierarchy by sharing context between the robots.
Each robot may be understood as a particle of a rigid, elastic or articulated body.
Then, physical interaction properties on this body may be deﬁned on swarm level
analogous to the single robot or manipulator case, respectively.
3.3 F ault Handling Pipeline
Figure 7depicts a fault handling pipeline [ 16,21,40] on a collision detection example
[44]. The pipeline starts with a residual generator , which is a fault indicator based
on a deviation between measurements and a system model (practically also a data-
driven model). The residual is used for fault detection , which outputs a binary signal
(true/false), i.e. whether the fault is present, as well as the time of detection. Fault
isolation deals with determination of the kind and location of the fault. The fault is
then identiﬁed to determine the size and time-variant behavior of the fault. This can
further be classiﬁed , e.g. by severity, intensity, intent, etc. The appropriate reaction
depends on the result of the fault diagnosis , which consists of isolation, identiﬁcation
and classiﬁcation. The last step in the pipeline is fault recovery .
Hierarchical fault handling . Faults may occur at multiple levels of the robot
or robot swarm, as depicted in Fig. 7b. In this hierarchical scheme, multiple fault
handling pipelines run in parallel. Residual generators may use signals from lower
levels in the hierarchy as well. The diagnosis is similarly connected, where identi-
ﬁcation may use lower level isolation and identiﬁcation signals, and classiﬁcation
may use lower level identiﬁcation signals. The reaction depends on the fault type,
as explained below. Note that there is a inherent and possibly time-varying latency
between the levels in the fault handling pipeline (Fig. 7).
194 T. Tomi´ c and S. Haddadin
Residual
generationDetection Isolation Identi ﬁcation Classi ﬁcation Reaction RecoveryPresence Diagnosis Response
r=H(/bardblˆτe/bardbl)r>rdetection
{true, false }Contact
point pcDirection nand
timely evolutionIntensity, Type Collision re ﬂex Vehicle
stabilized
(a) Single level of a fault handling pipeline [21] on a collision detection example [43].
Motor Residual Detection Isolation Identi ﬁcation Classi ﬁcation Reaction RecoveryVehicle Residual Detection Isolation Identi ﬁcation Classi ﬁcation Reaction RecoveryGroup Residual Detection Isolation Identi ﬁcation Classi ﬁcation Reaction RecoverySwarm Residual Detection Isolation Identi ﬁcation Classi ﬁcation Reaction Recovery
(b) Hierarchical fault handling. Higher level diagnosis stages may use signals from other
levels of the hierarchy. A low level fault may also escalate the recovery to a higher level.
Fig. 7 Hierarchical model-based fault handling pipeline
Model faults cause an internal reconﬁguration of the system. This may be adap-
tation orstructural (morphological) reconﬁguration. Adaptation is, for example,
changing parameters of an adaptive controller, or changing the spring stiffness in
a formation of robots. For example, the loss of a motor in a quadrotor requires a
different control strategy than in the nominal case because the dynamics changes
considerably [ 28]. For a group of robots, losing one robot may require reconﬁg-
uring the morphology of the formation [ 17]. Similarly, when the individual robot
cannot maintain ( 5) anymore (e.g. due to actuator failure), the robot may safely land,
while removing itself from the swarm, in turn necessitating the swarm to react with
according awareness (swarm-level reﬂexes). Picking up a payload with a single robot
can also be handled as a model fault, detected e.g. by a change detection algorithm
[20]. The proper reaction would be parameter adaptation [ 25], e.g. by resetting the
covariance of estimated parameters. The recovery is complete once the parameter
covariance converges below a threshold. In this case, no escalation to a higher level
in the hierarchy is necessary, as the local fault-aware controller is ensuring that the
robot behaves as ( 5). However, if multiple robots grasp a payload simultaneously,
this may trigger a group-level model fault, leading to distributed estimation of the
payload parameters (Table 4).
External faults trigger a change in the task objectives. This might be a reﬂex
reaction, or a switch to a different interaction task. In the hierarchical fault handling
pipeline, re-planning at the group or swarm level may also be triggered. Unwanted
collisions with the environment are an example of external faults [ 44], and may be
handled according to the pipeline from Fig. 7as follows. On motor level, a residual
may be used to detect collisions with the propeller. An example reaction is slowing
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 195
Table 4 Common faults for single ﬂying robots and robot swarms
Type Description Reaction
Model faults τf,m
Motor loss Loss of motor or propeller,
reducing vehiclecontrollabilityChange controller [ 28]a n d
land (underactuated vehicle),adapt control allocation matrix(redundant propellers) [ 38]
Parameter change Parameter uncertainty or
change of parameters due toe.g. picking up payloadReset covariance of aparameter estimationalgorithm [ 20]
Contact with environment Contact is achieved, changing
sytem dynamics [ 2,33,37]Switch to contact controller
Loss of robot A robot has failed critically Remove the robot from the
swarm and reconﬁgureformation
External faults τf,e
Hull collision Unwanted collision of robot
hull with environmentStabilize vehicle at safedistance from collision pose,replan trajectory [ 45]
Motor collision Objects in propellers Stop or slow down the motor,
escalate to hull collisionreaction
Contact One or more robots come intocontact with environment Engage force closure
down or stopping the motor to prevent further damage. Going up to the vehicle level,
the motor residual may be used for detection, together with the highpass ﬁltered
external force as depicted in Fig. 7b. After the collision direction and location are
isolated, the classiﬁcation may use information from the motor level to determinewhether the collision was on the hull or within the propeller. Contextual information
such as vehicle velocity at the collision time may be used. An example reaction is
stopping the vehicle away from the collision pose, as to “bounce off”. Inclusion inthe environment map and replanning is also possible in this case [ 45]. The vehicle
is recovered once the reﬂex is complete. Going further up in the hierarchy, in the
group and swarm levels, this may trigger a change in the formation parameters, likechanging the spring stiffness between the robots, or changing the desired formation.
Disturbance and fault estimation for robot groups .Disturbance estimation
exist for individual robots, however this area is unexplored for groups of ﬂyingrobots. Furthermore, ﬂying robot swarm literature is mostly concerned with motion
planning. We see general fault disturbance handling as an enabling factor in robust
operation of swarms. Therefore, fault detection at the group and swarm level seem
to be important underrepresented research topics. Parameter and aerodynamic dis-
turbance estimation during cooperative load carrying seem to be topics that have not
196 T. Tomi´ c and S. Haddadin
yet been handled in the literature. However, this may change as aerial manipulation
systems mature and start executing missions under wind inﬂuence.
4 Conclusions
Signiﬁcant advancements in ﬂying robots physically interacting with the environ-
ment were made. Still, in comparison to well established disciplines like industrialrobotics the entire ﬁeld is still rather young. Based on the state of the art and by
taking signiﬁcant speculative steps we assembled a synopsis and high-level control
framework for concretizing a fully elaborated vision of interaction aware ﬂying robotsystems. This was possible since on the one hand we isolated the major technological
challenges when aiming for general robust interaction capabilities, high-performance
disturbance compensation and exploitation, and fault handling in ﬂying single robotsand swarms. On the other hand, we proposed a unifying methodology inspired from
deductive awareness concepts in order to deﬁne and develop awareness in ﬂying
systems. The resulting awareness pipelines aim to generalize methods from fault
detection and isolation literature to interactions and disturbances. Lastly, we specu-
late about future research that is needed to achieve truly interactive, disturbance andfault aware ﬂying robot swarms in the wild.
References
1. Acosta, J.A., de Cos, C.R., Ollero, A.: A robust decentralised strategy for multi-task con-
trol of unmanned aerial systems. Application on underactuated aerial manipulator. In: 2016International Conference on Unmanned Aircraft Systems (ICUAS), pp. 1075–1084 (2016)
2. Alexis, K., Darivianakis, G., Burri, M., Siegwart, R.: Aerial robotic contact-based inspection:
planning and control. Auton. Robot. 40(4), 631–655 (2016)
3. Antonelli, G., Arrichiello, F., Chiaverini, S.: The null-space-based behavioral control for
autonomous robotic systems. Intell. Serv. Robot. 1(1), 27–39 (2008)
4. Augugliaro, F., D’Andrea, R.: Admittance control for physical human-quadrocopter inter-
action. In: Control Conference (ECC). 2013 European, pp. 1805–1810. Zürich, Switzerland(2013)
5. Augugliaro, F., Lupashin, S., Hamer, M., Male, C., Hehn, M., Mueller, M.W., Willmann,
J.S., Gramazio, F., Kohler, M., D’Andrea, R.: The ﬂight assembled architecture installation:
cooperative construction with ﬂying machines. IEEE Control Syst. 34(4), 46–64 (2014)
6. Baizid, K., Giglio, G., Pierri, F., Trujillo, M.A., Antonelli, G., Caccavale, F., Viguria, A.,
Chiaverini, S., Ollero, A.: Behavioral control of unmanned aerial vehicle manipulator systems.Auton. Robot. 41(5), 1203–1220 (2017)
7. Bangura, M., Lim, H., Kim, H.J., Mahony, R.: Aerodynamic Power Control for Multirotor
Aerial V ehicles. In: ICRA 2014, pp. 529–536. Hong Kong, China (2014)
8. Bangura, M., Mahony, R.: Thrust control for multirotor aerial vehicles. IEEE Trans. Robot. 33,
accepted (2017)
9. Bellens, S., De Schutter, J., Bruyninckx, H.: A hybrid pose/wrench control framework for
quadrotor helicopters. ICRA 2012 , 2269–2274 (2012)
Towards Interaction, Disturbance and Fault Aware Flying Robot Swarms 197
10. Bernard, M., Kondak, K.: Generic slung load transportation system using small size helicopters.
In: IEEE International Conference on Robotics and Automation, ICRA ’09, pp. 3258–3264.IEEE (2009)
11. Brescianini, D., D’Andrea, R.: Design, modeling and control of an omni-directional aerial
vehicle. In: 2016 IEEE International Conference on Robotics and Automation (ICRA), pp.
3261–3266. IEEE (2016)
12. Briod, A., Kornatowski, P ., Klaptocz, A., Garnier, A., Pagnamenta, M., Zufferey, J.C., Floreano,
D.: Contact-based navigation for an autonomous ﬂying robot. IROS 2013 , 3987–3992 (2013)
13. Bruschi, P ., Piotto, M., Dell’Agnello, F., Ware, J., Roy, N.: Wind speed and direction detection
by means of solid-state anemometers embedded on small quadcopters. Procedia Eng. 168,
802–805 (2016)
14. Davis, E., Pounds, P .E.I.: Direct sensing of thrust and velocity for a quadrotor rotor array. IEEE
Robot. Autom. Lett. 2(3), 1360–1366 (2017)
15. De Luca, A., Albu-Schaffer, A., Haddadin, S., Hirzinger, G.: Collision detection and safe
reaction with the dlr-iii lightweight manipulator arm. IROS 2006 , 1623–1630 (2006)
16. Ding, S.: Model-based fault diagnosis techniques: design schemes, algorithms, and tools.
Springer Science & Business Media, Berlin (2008)
17. Franchi, A., Secchi, C., Ryll, M., Bülthoff, H., Giordano, P .R.: Shared control: Balancing
autonomy and human assistance with a group of quadrotor uavs. IEEE Robot. Autom. Mag.19(3) (2012)
18. Fumagalli, M., Naldi, R., Macchelli, A., Forte, F., Keemink, A.Q.L., Stramigioli, S., Carloni,
R., Marconi, L.: Developing an aerial manipulator prototype: physical interaction with theenvironment. IEEE Robot. Autom. Mag. 21(3), 41–50 (2014)
19. Gioioso, G., Franchi, A., Salvietti, G., Scheggi, S., Prattichizzo, D.: The ﬂying hand: a formation
of uavs for cooperative aerial tele-manipulation. In: 2014 IEEE International Conference onRobotics and Automation (ICRA), pp. 4335–4341 (2014)
20. Gustafsson, F., Gustafsson, F.: Adaptive ﬁltering and change detection, vol. 1. Wiley, New Y ork
(2000)
21. Haddadin, S., Luca, A.D., Albu-Schäffer, A.: Robot collisions: a survey on detection, isolation,
and identiﬁcation. IEEE Trans. Robot. PP(99), 1–21 (2017)
22. Khatib, O.: A uniﬁed approach for motion and force control of robot manipulators: the opera-
tional space formulation. IEEE J. Robot. Autom. 3(1), 43–53 (1987)
23. Li, S., Yang, J., Chen, W.H., Chen, X.: Disturbance Observer-Based Control: Methods and
Applications, 1st edn. CRC Press Inc, Boca Raton (2014)
24. Mellinger, D., Lindsey, Q., Shomin, M., Kumar, V .: Design, modeling, estimation and control
for aerial grasping and manipulation. In: 2011 IEEE/RSJ International Conference on Intelli-gent Robots and Systems. pp. 2668–2673 (2011)
25. Mellinger, D., Shomin, M., Michael, N., Kumar, V .: Cooperative grasping and transport using
multiple quadrotors. In: Distributed autonomous robotic systems, pp. 545–558. Springer (2013)
26. Mersha, A.Y ., Stramigioli, S., Carloni, R.: V ariable impedance control for aerial interaction.
In: IROS2014, pp. 3435–3440. IEEE (2014)
27. Michael, N., Fink, J., Kumar, V .: Cooperative manipulation and transportation with aerial
robots. Auton. Robot. 30(1), 73–86 (2011)
28. Mueller, M.W., D’Andrea, R.: Stability and control of a quadrocopter despite the complete
loss of one, two, or three propellers. In: 2014 IEEE International Conference on Robotics andAutomation (ICRA), pp. 45–52. IEEE (2014)
29. Muscio, G., Pierri, F., Trujillo, M.A., Cataldi, E., Giglio, G., Antonelli, G., Caccavale, F.,
Viguria, A., Chiaverini, S., Ollero, A.: Experiments on coordinated motion of aerial roboticmanipulators. In: 2016 IEEE International Conference on Robotics and Automation (ICRA),pp. 1224–1229 (2016)
30. Olfati-Saber, R.: Flocking for multi-agent dynamic systems: algorithms and theory. IEEE Trans.
Autom. Control. 51(3), 401–420 (2006)
31. Olfati-Saber, R., Fax, J.A., Murray, R.M.: Consensus and cooperation in networked multi-agent
systems. Proc. IEEE 95(1), 215–233 (2007)
198 T. Tomi´ c and S. Haddadin
32. Oung, R., D’Andrea, R.: The distributed ﬂight array: design, implementation, and analysis of
a modular vertical take-off and landing vehicle. Int. J. Robot. Res. 33(3), 375–400 (2014)
33. Papachristos, C., Alexis, K., Tzes, A.: Efﬁcient force exertion for aerial robotic manipula-
tion: exploiting the thrust-vectoring authority of a tri-tiltrotor uav. In: 2014 IEEE InternationalConference on Robotics and Automation (ICRA), pp. 4500–4505 (2014)
34. Rajappa, S., Ryll, M., Bülthoff, H.H., Franchi, A.: Modeling, control and design optimization
for a fully-actuated hexarotor aerial vehicle with tilted propellers. In: 2015 IEEE InternationalConference on Robotics and Automation (ICRA), pp. 4006–4013 (2015)
35. Rajappa, S., Bülthoff, H., Stegagno, P .: Design and implementation of a novel architecture for
physical human-uav interaction. Int. J. Robot. Res. 0278364917708038 (2017)
36. Ruggiero, F., Cacace, J., Sadeghian, H., Lippiello, V .: Impedance Control of VToL UA Vs with
a Momentum-based External Generalized Forces Estimator. In: ICRA 2014, pp. 2093–2099.Hong Kong, China (2014)
37. Ryll, M., Muscio, G., Pierri, F., Cataldi, E., Antonelli, G, C., F, Franchi, A.: 6d physical inter-
action with a fully actuated aerial robot. In: 2017 IEEE International Conference on Roboticsand Automation (ICRA) (2017)
38. Ryll, M., Bicego, D., Franchi, A.: Modeling and Control of FAST-Hex: a Fully-Actuated by
Synchronized-Tilting Hexarotor. In: IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS) 2016. Daejeon, South Korea (2016)
39. Siciliano, B., Khatib, O.: Springer Handbook of Robotics. Springer, Berlin (2016)40. Simani, S., Fantuzzi, C., Patton, R.J.: Model-based fault diagnosis in dynamic systems using
identiﬁcation techniques. Springer Science & Business Media, Berlin (2013)
41. Sreenath, K., Lee, T., Kumar, V .: Geometric control and differential ﬂatness of a quadrotor uav
with a cable-suspended load. In: 2013 IEEE 52nd Annual Conference on Decision and Control(CDC), pp. 2269–2274. IEEE (2013)
42. Tomi´ c, T.: Evaluation of acceleration-based disturbance observation for multicopter control.
In: 2014 European Control Conference (ECC), pp. 2937–2944. Strasbourg, France (2014)
43. Tomi´ c, T., Haddadin, S.: A uniﬁed framework for external wrench estimation, interaction
control and collision reﬂexes for ﬂying robots. In: IROS 2014, pp. 5290–5296. IEEE, Chicago,IL, USA (2014)
44. Tomi´ c, T., Haddadin, S.: Simultaneous estimation of aerodynamic and contact forces in ﬂying
robots: applications to metric wind estimation and collision detection. In: ICRA 2015, pp.5290–5296. Seattle, W A, USA (2015)
45. Tomi´ c, T., Ott, C., Haddadin, S.: External wrench estimation, collision detection, and reﬂex
reaction for ﬂying robots. IEEE Trans. Robot. PP(99), 1–17 (2017)
46. Tomi´ c, T., Schmid, K., Lutz, P ., Mathers, A., Haddadin, S.: The ﬂying anemometer: uniﬁed
estimation of wind velocity from aerodynamic power and wrenches. In: IROS 2016, pp. 1637–1644. IEEE, Daejeon, Korea (2016)
47. Ware, J., Roy, N.: An analysis of wind ﬁeld estimation and exploitation for quadrotor ﬂight in
the urban canopy layer. In: ICRA 2016, pp. 1507–1514. IEEE (2016)
48. Waslander, S.L., Wang, C.: Wind disturbance estimation and rejection for quadrotor position
control. In: AIAA Infotech@Aerospace Conference. Seattle, W A, USA (2009)
49. Yao, J.W., Desaraju, V .R., Michael, N.: Experience-based models of surface proximal aerial
robot ﬂight performance in wind. In: International Symposium on Experimental Robotics, pp.563–573. Springer (2016)
50. Yeo, D.W., Sydney, N., Paley, D.A.: Onboard Flow Sensing for Rotary-Wing UA V Pitch Control
in Wind. In: Proceedings of the AIAA GNC Conference, p. 1386 (2016)
51. Zhao, M., Kawasaki, K., Chen, X., Kakiuchi, Y ., Okada, K., Inaba, M.: Transformable Multi-
rotor with Two-Dimensional Multilinks: Modeling, Control, and Whole-Body Aerial Manip-ulation, pp. 515–524. Springer International Publishing, Cham (2017)
Control of Quadrotors Using the Hopf
Fibration on SO(3)
Michael Watterson and Vijay Kumar
1 Introduction
Quadrotors and other multi-rotor autonomous micro aerial vehicles have been studied
extensively in recent years. The control literature is mostly focused on model basedapproaches using either linear control or nonlinear control derived from geometric
approaches [ 8,14]. There has been some research on learning based control [ 24], but
the model based approaches are more mature and can provide provable guarantees
on stability.
Linearized control approaches express the nonlinear dynamics as a ﬁrst order
approximation around a ﬁxed set point or the current state [ 7]. Model predictive
methods [ 3,17,19] compute this linearization around a window of control states
and optimize a sequence of control inputs in a receding horizon manner. Thesemethods are adaptable to many types of quadrotor based dynamical systems such as
tethered ﬂight [ 19], attached active or passive manipulators [ 3] and many others [ 10].
Geometric techniques directly use the nonlinear structure of the dynamics of the
robot. [ 8] uses an error metric on the rotation group SO(3)to control the attitude of
the robot. The desired attitude is calculated from errors in 3D position and velocity
to create a controller which can control on SE(3). This controller is stable almost
everywhere which is better than the local stability results guaranteed by linearized
methods. Reference [ 14] showed how this controller could be applied to a physical
Michael Watterson is supported with a NASA space technology research fellowship. Infrastructure
was supported by grants HR001151626/HR0011516850, W911NF-08-2-0004, and N00014-17-1-2437.
M. Watterson ( B)·V . Kumar
University of Pennsylvania, 3330 Walnut St, Philadelphia, PA 19104, USAe-mail: wami@seas.upenn.edu
V . Kumar
e-mail: kumar@seas.upenn.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_20199
200 M. Watterson and V . Kumar
quadrotor and be used to plan trajectories. That work has been extended by [ 1]t o
only need control over attitude rates as opposed to attitude and attitude rates. This
geometric approach can also be extended to other models such as hybrid systems[21].
Inverted acrobatics with a quadrotor has been performed with a myriad of tech-
niques [ 4,9,11,15]. All of these techniques use open loop rate control for some part
of the trajectory. [ 15] uses a hybrid transition between geometric control and rate
control. Reference [ 4] uses an approach where the quadrotor has an inverted open
loop phase in which the robot drifts during the inversion. References [ 9,11] use open
loop rate input for the entire trajectory. They correct the control inputs based on past
performance of the maneuver to reduce error over successive trials.
The control of just the attitude of a quadrotor without any position feedback as
been done with a general SO(3)formulation as part of [ 8,14]. Without position
feedback, there are no singularity issues and these approaches has been shown to
be asymptomatically stable almost everywhere on SO(3). This resulted stabilization
from an inverted initial condition in [ 8]. A mediation of numerical stability issues
near the position feedback singularity in [ 14] was proposed in that work to limit
instantaneous desired attitude changes from 180
◦to 90◦, but does not guarantee
continuity of the desired attitude.
One example of the Hopf ﬁbration in application to robotics is in [ 23] which uses
the Hopf ﬁbration to generate samples on SO(3)with an application to robot motion
planning. In their work, they use spherical coordinates to parameterize S2, which
produces a different function than in our work which uses an input element of S2
directly.
In this work we propose a geometric controller for a quadrotor using the Hopf
ﬁbration which is valid for almost all of SO(3)and has a singularity further from
hover than [ 8,14]. The Hopf ﬁbration can be used to generate a map S2×S1→
SO(3). We propose a new Hopf Fibration Control Algorithm (HFCA) which uses
2 charts on S2to describe the full manifold. Since it takes 2 charts to cover S1,w e
end up needing 2 ×2=4 charts to cover SO(3), which is the theoretical minimum
number [ 5]. With this parameterization of SO(3)we are able to control a quadrotor
everywhere on SE(3)using a geometric feedback controller. While any state in
SE(3)is valid for our controller, we also must have a feasible trajectory in this
space which respects the non-holonomic constraints of the platform which only has
4 inputs. We use the ﬂatness transform of [ 14] and use the ﬂat variables to generate
a control input. We demonstrate the performance of this controller in simulation and
compare it to the other geometric controller [ 14]. On an experimental platform we
show aggressive maneuvers which would hit the singularities of other controllers,
but work with our approach.
The advantages of our approach over the traditional geometric controller for
quadrotors, is that 1: the Hopf ﬁbration directly represents the rotation of the robot
without needing to perform a Gram–Schmidt decomposition, 2: ensures continuous
control inputs for position feedback for all orientations on SO(3), and 3: the repre-
sentation easily allows a choice of yaw angle to make the body frame yaw angular
Control of Quadrotors Using the Hopf Fibration on SO(3) 201
Chart 1 ( q)
HFCAChart 2 (¯ q)
Fig. 1 The Hopf Fibration Control Algorithm (HFCA) switches between two geometric construc-
tions of SO(3)based on the desired direction of thrust of the robot. One of these is singular when
this direction is directly up (orange) and the other when the direction is directly down (blue). The
set of feasible directions for each construction is plotted as a subset of the sphere, with the point
singularity on each expanded for visualization. By switching between these two maps we insure
our controller is valid for all orinetations in SO(3), and thus has feeedback for all of SE(3)
velocity zero, which is the redundant degree of freedom for controlling the position
of the robot (Fig. 1).
We structure the paper starting with an overview of the dynamic model and the
differential ﬂatness property applied to quadrotors in Sect. 2. We introduce Hopf
ﬁbration and the use of quaternions in Sect. 3. The Hopf Fibration Control Algorithm
(HFCA) is detailed in Sect. 4along with the necessary second Hopf ﬁbration need for
complete coverage of SO(3). In Sect. 5, we show our experimental setup, trajectory
generation and results in both simulation and with hardware.
2 Modeling
2.1 Dynamic Model
We deﬁne an inertial reference frame Wto describe the motion of the robot’s rigid
body frame B. The world frame e3vector is aligned against gravity making gravi-
tational acceleration g=− g·e3. The robot’s rotors each produce a thrust parallel
to the b3vector and b1and b2are aligned with the arms of the robot. The motors
produce a body frame moment ( [m1,m2,m3]T) and thrust fin the b3direction. This
wrench is dependent on the propeller speeds si, thrust properties kf,km, and the
characteristic length of the robot L[14].
⎡
⎢⎢⎣m1
m2
m3
f⎤
⎥⎥⎦=⎡
⎢⎢⎣0 kfL0−kfL
−kfL0 kfL 0
km−kmkm−km
kf kf kf kf⎤
⎥⎥⎦⎡
⎢⎢⎣s2
1
s2
2
s2
3
s2
4⎤
⎥⎥⎦(1)
202 M. Watterson and V . Kumar
Fig. 2 The quadrotor’s body frame B(vectors b1,b2,a n d b3) shown in relation to an inertial frame
Wwith vectors e1,e2,a n d e3. For a geometric controller, the yaw of the robot is determined by a
rotated frame ( a1,a2,a3) about the world e3direction
From this wrench acting on the system, the equations of motion can be found
from classical dynamics. We use the convention where all angular velocities ωare
represented in the robot’s body frame and all linear positions and velocities are
expressed in the world frame. The mass of the robot is mand the angular inertia
tensor is H. The differential relation between ωand the rotation matrix Rdepend
on the coordinates in which Ris expressed. In Sect. 4we will introduce this relation
using the quaternion representation and it can be described in terms of the 9 elements
of the rotation matrix in Eq. 4(Fig. 2).
m¨r=− m·g+R·fe3
H˙ω=⎡
⎣m1
m2
m3⎤
⎦−ω×(Hω)(2)
The full state and control inputs of the robot is shown in Eq. 3. Underneath the
horizontal line, we clearly state the domain of each variable. Since the state contains
elements in both Rnand SO(3), we denote it as a set instead of a state vector. The
angular velocity and the rotational dynamics are related by Eq. 4where the hat ( ˆ.)
operator takes an R3vector and returns the corresponding skew symmetric matrix
such that ˆv1·v2=v1×v2for any two vectors v1,v2∈R3.
Control of Quadrotors Using the Hopf Fibration on SO(3) 203
x={ r,R,˙r,ω}
u={ s2
1,s2
2,s2
3,s2
4}
r,˙r∈R3
R∈SO(3)
ω∈R3
s2
1,s2
2,s2
3,s2
4∈[0,∞)(3)
ˆω=RT˙R (4)
2.2 Differential Flatness
The dynamics of a quadrotor are well known to be differentially ﬂat [ 14] which
means the state and control inputs (Eq. 3) can be mapped to ﬂat outputs (Eq. 5)
through a change of coordinates φ:(x,u)→γ.γis the collection of ﬂat outputs
of the system, which consist of the R3location of the quadrotor, the derivatives of
this locations, and yaw orientation ψand its derivative.
γ={ r,˙r,¨r,...r,ψ,˙ψ}
r,˙r,¨r,...r∈R3
ψ∈SO(2)
˙ψ∈R(5)
One map is not sufﬁcient for the entire domain between these two spaces (Eqs. 3
and 5) because SO(3)needs at least 4 charts [ 5]. Previous geometric approaches use
one map, which can cover a open subset of SO(3). For example, this open subset
excludes orientations where a2is parallel to the desired thrust. We will use multiple
diffeomorphisms for φ, so we can get full coverage of SO(3). We show the charts
in Sect. 4.2.
For a quadrotor which can only produce positive forces with each propeller, there
is an intrinsic singularity of the system for a trajectory which requires all the motors
to be simultaneously off s2
1=s2
2=s2
3=s2
4=0. This corresponds to the situation
where the robot is exerting zero force and is thus in free fall. We cannot compute a
differentially ﬂat relationship for this case because this maneuver is not in a control-
lable region of the dynamics. This is because without reversible motors or variablepitched propellers, each propeller cannot produce a force in some ball of radius δ> 0
around producing zero force. So we assume the robot is never in complete free fall.
2.3 Geometric Control on S E (3)
Geometric control for a quadrotor operates on the principle that the thrust of the
robot is in the body frame b3direction. This can be stated with a constraint which
204 M. Watterson and V . Kumar
determines two dimensions of R:
¨r+g
||¨r+g||=R(γ)·e3 (6)
The traditional geometric control technique described in [ 8,14]u s e saG r a m –
Schmidt process to construct an orthonormal basis of R3from two vectors. This
basis is constructed to be right handed and thus describes an element of SO(3).T h e
second axis a1describes the yaw of the robot. This leaves a singularity whenever
¨r+gis parallel to a1. To keep this singularity far away from normal operation it
is often chosen to be perpendicular to gravity (ex. a1=[cos(ψ), sin(ψ), 0]T). This
results in a singularity when the robot is pitched by 90◦with respect to hover.
R(γ)=/bracketleftbigbw
1bw
2bw
3/bracketrightbig
,bw
3=¨r+g
||¨r+g||,bw
2=bw
3×a1
||bw
3×a1||,bw
1=bw
2×bw
3 (7)
From a desired angle we can compute a feedback term for the rotation using the
formula from [ 14] which has a proportional and derivative term on the rotation.
⎡
⎣m1
m2
m3⎤
⎦=− kR1
2(RT
dR−RTRd)∨−kω(ω−ωd) (8)
The desired force is calculated with a feedback error on the position and velocity of
the robot.
f=(−kx(r−rd)−kv(˙r−˙rd)+¨rd)·Re3 (9)
Here we note that any possible parameterization of SO(3)can be used inside
the function R(γ) as long Eq. 6holds and we can invert this function to have a
diffeomorphism between the ﬂat variables and SE(3)state in Eq. 5.I fw eu s eE u l e r
angles, R(γ)becomes a complicated function because Eq. 6is a nonlinear constraint
in the Euler angles. We will show in Sect. 3that the Hopf ﬁbration provides a simple
way of enforcing Eq. 6.
We can show that the geometric control paradigm of constructing Rfrom a desired
thrust direction is not possible without introducing a singularity. For an arbitrary state
γ, it is possible to construct a non-singular R(γ)which is smooth and diffeomorphic
everywhere. For example, if γcontains an element of SO(3), the identity function
works for R(γ). With quadrotor geometric control, part of the state constrains a
desired direction for the thrust ( ¨r+g) and part deﬁnes the rest of the rotation ( ψ).
For the proof that any construction causes a singularity, we can just look at thisdirection and hold the rest of the state constant. Let us deﬁne this direction as s=
¨r+g
||¨r+g||.L e t F(s):S2→ SO(3)be the function that results from R(γ) holding all
of the state γconstant except s. Thus F(s)·e3=sfor all s. Let us assume Ris
continuous and differentiable everywhere on SO(3), and thus Fmust be continuous
and differentiable too. Now if we take a non-zero vector yperpendicular to e3,
their inner product is zero ( y·e3=0). Since rotations preserve length, we also
Control of Quadrotors Using the Hopf Fibration on SO(3) 205
have(F(s)·y)·(F(s)·e3)=0, which is (F(s)·y)·s=0. Thus we have found a
continuous vector ﬁeld ( F(s)·y) which is tangent to S2. By the Hairy ball theorem
[16], this must be zero somewhere, but ||F(s)·y|| = || y||>0 because y/negationslash=0. Thus
F(s)cannot be continuous everywhere on S2by contradiction and thus R(γ)cannot
be continuous everywhere.
3 Quaternions and the Hopf Fibration
To develop a better function R(γ), we introduce the use of unit quaternions to rep-
resent rotations. We will write a quaternion using the Hamilton convention [ 20]. For
computation we will use the representation S3≡{ x∈R4|xTx=1}and assume
the following standard operations are known:
R(q):S3→ SO(3)⊂R3×3
q1⊗q2:S3×S3→ S3|R(q1⊗q2)=R(q1)·R(q2)
q−1:S3→ S3|q−1⊗q=I(10)
The function R(q)takes a unit quaternion and produces the corresponding rota-
tion matrix. This map is two to one as both qand−qmap to the same rotation.
The quaternion product q1⊗q2composes quaternions in such a way that the corre-
sponding rotations are also composed. This operation is non-commutative just like
the composition of rotation matrices. The ﬁnal operation we need is the inverse. Theinverse of an element left or right multiplied with an element produces the identity
element I=[1,0,0,0]
T.
The Hopf ﬁbration is a mapping from S3toS2×S1[2]. If we take the inverse map
and use the quaternion mapping to SO(3), we get a map from S2×S1toSO(3).
This is shown explicitly in [ 12], but for a different ordering of variables we can get
other valid Hopf ﬁbrations. With a properly ordered geometric construction of theHopf ﬁbration, we are able to have this map represent the orientation of the quadrotor
in a manner which is convenient to expressing the dynamics of the system. The S
2
component of the map will directly be the body frame b3vector expressed in the
word frame and the S1component will be a body frame yaw angle.
For the 2D sphere, we have the point [a,b,c]T∈S2if[a,b,c]T∈R3and is
distance 1 from the origin. By taking the time derivative, we can also write an
expression which is needed for simplifying equations:
a2+b2+c2=1
a˙a+b˙b+c˙c=0(11)
We can write the following unit quaternion as a function of S2. We note that with
the properties in Eq. 11, it can be shown that the magnitude is 1:
206 M. Watterson and V . Kumar
qabc=1√2(1+c)⎡
⎢⎢⎣1+c
−b
a
0⎤
⎥⎥⎦(12)
An application of the quaternion to rotation matrix map shows that this quaternion
indeed has the property that the body frame b3direction is the elements of S2:
R(qabc)e3=/bracketleftbigabc/bracketrightbigT(13)
qabcalone cannot parameterize SO(3)because S2is a 2 dimensional manifold and
thus this map describes a 2D subset of SO(3). However we can get a 3 Dparameter-
ization by right multiplying by a quaternion which describes a rotation about an e3
axis by and angle ψ∈SO(2). We note that SO(2)and S1have the same topological
structure. Using qψ=[cosψ
2,0,0,sinψ
2], we get the quaternion:
q=qabc⊗qψ (14)
Since R(qψ)is a rotation about b3,E q . 13also hold for qand thus can be used to
describe Rconsistently with Eq. 6.
4 The Hopf Fibration Control Algorithm
Our control algorithm consists of calculating the state and control inputs from the ﬂat
variables using our representation of the desired rotation. To get around the SO(3)
parameterization singularity we show how a second ﬁbration can be used to cover
the singularity when the quadrotor is fully inverted. Using an extension to SE(3),
we can control the full ﬂat state of the robot.
4.1 Differential Flatness with Hopf Fibration
To calculate the control inputs, we need to transform from the ﬂat variables to thestate variables. To use qas a parameterization of R, we need the following quater-
nion property which expresses the body frame angular velocity in terms of the time
derivative of the quaternion.
ω=2q
−1⊗˙q (15)
Using Eqs. 10,11, and 15, we can derive expressions for the angular velocity as
a function of a,b,c,andψ. Where ω1,ω 2,ω 3are the components of the angular
velocity expressed about the b1,b2,b3body axes.
Control of Quadrotors Using the Hopf Fibration on SO(3) 207
ω1=sin(ψ)˙a−cos(ψ)˙b−(asin(ψ)−bcos(ψ))/parenleftbig˙c
c+1/parenrightbig
ω2=cos(ψ)˙a+sin(ψ)˙b−(acos(ψ)+bsin(ψ))/parenleftbig˙c
c+1/parenrightbig
ω3=b˙a−a˙b
1+c+˙ψ(16)
The magnitude of the ﬁrst two components of the angular velocity is the mag-
nitude of [˙a,˙b,˙c]T. Since this is independent of ψ, we can ﬁnd a trajectory for ψ
which minimizes angular velocity by setting ω3=0. This is convenient because the
dynamics of a quadrotor have a better response in producing moments in the b1and
b2axes than the b3since the torque produced by the force of a propeller is stronger
than its rotation moment.
||ω12||2=ω2
1+ω2
2=˙a2+˙b2+˙c2(17)
Up until now we have assumed our rotation is a function as a point on the sphere
S2, but to use the quadrotor we need to express this point in terms of the ﬂat variables.
We do this by making the same assumption as the traditional geometric control that
the thrust of the quadrotor is aligned with its body axis.
ζ=f·b3=¨r+g (18)
/bracketleftbigabc/bracketrightbigT=normalize (ζ)=ζ
||ζ||(19)
The function normalize (ζ)computes the unit vector in the direction ζby normalizing
it. Since we need the derivatives of the points on the sphere, we take the time derivativeof the normalization function. Equating this will give us a full expression in term of
the ﬂat variables for the angular velocities and attitude.
/bracketleftbig
˙a˙b˙c/bracketrightbig
T=d
dtnormalize (ζ)=ζTζ·I−ζζT
||ζ||3·˙ζ (20)
This relation gives a simple form for calculating the magnitude of the angular
velocity. Reference [ 6] uses a bound on this formula for computing trajectories for a
robot.
||ω12|| =/vextendsingle/vextendsinglesin(∠ζ˙ζ)/vextendsingle/vextendsingle||˙ζ||
||ζ||(21)
4.2 Control on SO(3)
With the known dynamics, a desired path through acceleration space ( ¨r) can be
converted into a trajectory of rotations and angular velocities. Since the attitude
controller from [ 14] operates on SO(3)and the angular velocities, we can use it to
control the attitude of our robot without modiﬁcation. Reference [ 8] provides a proof
208 M. Watterson and V . Kumar
of the stability properties of the attitude controller which is exponentially stable for
rotational error less than 90◦and attractive for error less than 180◦.
The important difference of our approach is that our singularity is further from
hover than that of [ 8,14]. In addition we have the beneﬁt that we never need to
explicitly compute Rto ﬁnd control inputs because we have a direct parameterization
of the desired quaternion. This also simpliﬁes the calculation of the desired ω.
The one case this approach will not work is for a ﬂip which requires full inversion
of the platform. To be able to use all possible orientations, we need a secondary map
to cover orientations around full inversion. We can write a second quaternion whichis singular about c=1 instead of c=− 1 like in our ﬁrst map:
¯q
abc=1√2(1−c)⎡
⎢⎢⎣−b
1−c
0
a⎤
⎥⎥⎦(22)
We again need to rotate about b3by an angle ¯ψto get a 3 dimensional parameterization
ofSO(3). We denote this angle ¯ψbecause [a,b,c]Tneeds a different angle ψand
¯ψin each chart to represent the same rotation.
¯q=¯qabc⊗q¯ψ (23)
It can be veriﬁed that Eq. 13also holds for this quaternion and that these two param-
eterizations overlap on [a,b,c]T/negationslash=[0,0,±1]T. When we transition from one map
to the other, we need to calculate ¯ψin terms of ψwhich we can get by equating the
two rotations:
¯ψ=arctan 2 (a,b)+ψ (24)
For our second chart ( ¯q), we can ﬁnd the angular velocity using the same method
as with the ﬁrst ﬁbration:
ω1=sin(ψ)˙a+cos(ψ)˙b−(asin(ψ)+bcos(ψ))/parenleftbig˙c
c−1/parenrightbig
ω2=cos(ψ)˙a−sin(ψ)˙b−(acos(ψ)−bsin(ψ))/parenleftbig˙c
c−1/parenrightbig
ω3=b˙a−a˙b
−1+c+˙¯ψ(25)
For this chart, the magnitude of ||ω12||i st h es a m ea si nE q . 17but rotated as the
parameter ψdescribes yaw in a different way on the second chart. We simply need
to ensure that the desired proﬁle for ψrespects this when the quadrotor transitions
between charts. For example, the different between ˙ψand˙¯ψat the transition point
can be found by making ω3t h es a m ei nE q s . 16and 25. Equivalently, the change
from ˙ψto˙¯ψcan be found by differentiating equation 23. Since SO(3)is a smooth
manifold, we can insure that differentiating ( 23) further will results in all derivatives
onSO(3)will be continuous during the chart transition [ 22].
Control of Quadrotors Using the Hopf Fibration on SO(3) 209
The HFCA switches between charts whenever c=0. As the robot’s desired ori-
entation rotates past 90◦we compute ψin the other chart using Eq. 24. For trajec-
tories which do not care about yaw, we can update ψby assuming ω3=0. Since
ψ∈SO(2), we actually need two charts representing ψfor each chart on S2. Thus
our approach uses 4 charts to cover SO(3), since SO(2)is a one parameter group,
switching charts representing ψis just done by adding or subtracting 2 πfrom an
angular representation of ψ.
4.3 Extension to S E (3)
To be able to control the quadrotor in 3D space, we need to have feedback on the posi-
tion and velocity of the system, not just the orientation. We take the same approach
as [8,14], and use a proportional-derivative control term along with a feed forward
term on the desired acceleration. When we evaluate this on the differentially ﬂat
equations, we need to take the derivative of this equation to ﬁnd the angular veloci-
ties. Thus our feedback ends up being on position, velocity, and acceleration in ﬂat
space and has a feed forward term calculated from the desired jerk.
ζ=− kx(r−rd)−kv(˙r−˙rd)+¨rd+g (26)
5 Simulations and Experimental Results
We performed tests of our controller in both simulation and on a hardware platform.
For both, we used the circular trajectories described in Sect. 5.2with a rotation rate of
ωc=4 rad/s. We let the controller fully stabilize at a hover position and then slowly
increased the radius ρover 10 s in a minimum snap proﬁle. Whenever we transition
between different circle trajectories we also take 10 s with minimum snap proﬁle on
the exponential coordinates.
5.1 Hardware Platform
Our testbed is an stock Lumenier QA V250 racing quadrotor with a pixracer autopilot
running the PX4 stack [ 13]. We choose this platform for its high thrust to weight
ratio and small size. It is 34 cm when measured between the furthest two tips of thepropellers. It weighs 400 g and can produce a maximum thrust of 1.2 kg. This thrust
to weight ratio is comparable to larger platforms with on-board pose estimation. A
external motion capture system provides pose information at 100 hz (Fig. 3).
210 M. Watterson and V . Kumar
Fig. 3 The hardware platform we use is a commercial racing quadrotor and autopilot. The platform
measures 34 cm prop tip to prop tip and weights 400 g
5.2 Trajectory Generation
To test the performance of our controller, we use circular trajectories for the quadrotor
because they can demonstrate highly aggressive behavior in tightly conﬁned spaces.
Also, they can be varied continuously between horizontal and vertical loops in such away to show all possible orientations achievable by our controller. These trajectories
are purposefully non-optimal as to stress the controller. Trajectories that start and
stop from hover at the bottom of an inverted loop [ 9] take much less force because
they do not need to maintain centripetal acceleration at the bottom.
We parameterize a circle with radius ρand angular velocity ω
c. To smoothly enter
and exit these trajectories we generate a minimum snap polynomial for ρ, but assume
ωcis a constant. By varying ρfrom 0 to some ﬁxed radius, we can start a circular
trajectory smoothly from a stopped state.
p(t)=⎡
⎣ρcos(ω ct)
ρsin(ω ct)
0⎤
⎦ (27)
To transform between trajectories of different orientations, we rotate using a rota-
tion parameterized by exponential coordinates:
r(t)=expˆξ(t)·p(t) (28)
To input these trajectories into the controller, we need up to the third derivative
ofrbecause of the ˙ζterm in the angular velocity. Taking the time derivative of
Control of Quadrotors Using the Hopf Fibration on SO(3) 211
exponential coordinates can be simpliﬁed if the coordinates are a function along one
axis. We assume ξ=α(t)vfor some ﬁxed v∈R3. We write the derivatives of ξ
as˙ξto save horizontal space, but note these expressions are not true for arbitrary
functions ξ(t).
˙r(t)=exp(ˆξ(t))/parenleftBig
˙p+ˆ˙ξp/parenrightBig
(29)
¨r(t)=exp(ˆξ(t))/parenleftBig
¨p+2ˆ˙ξ˙p+2ˆ¨ξp+(ˆ˙ξ)2p/parenrightBig
(30)
...r(t)=exp(ˆξ(t))·/parenleftBig...p+3ˆ¨ξ˙p+3ˆ˙ξ¨p+3ˆ¨ξˆ˙ξp+3ˆ˙ξ2˙p+ˆ...
ξp+(ˆ˙ξ)3p/parenrightBig(31)
We also need to calculate the derivatives of p. We can choose ωcbased on the
thrust needed in the most aggressive trajectory we execute. When the quadrotor isfully upside down, it needs to produce some thrust ε> 0t os t a ya w a yf r o mt h e
zero singularity in dynamics. During the top of a circular trajectory, the centripetal
acceleration will be this thrust plus the gravitational acceleration. At the bottom theneeded thrust will be maximal at 2 ε+g. So we can calculate a feasible ω
cbased on
the radius of the circular trajectory, εand the maximum acceleration achievable by
our platform. The other dynamic constraints such as thrust limits are constrained byevaluating the nominal thrust on the trajectory and choosing a different ω
candρif
they violate the limits.
˙p=⎡
⎣˙ρcos(ω ct)−ρω csin(ω ct)
˙ρsin(ω ct)+ρω ccos(ω ct)
0⎤
⎦ (32)
¨p=⎡
⎣¨ρcos(ω ct)−2ωc˙ρsin(ω ct)−ω2
cρcos(ω ct)
¨ρsin(ω ct)+2ωc˙ρcos(ω ct)−ω2
cρsin(ω ct)
0⎤
⎦ (33)
...p=⎡
⎣(...ρ−3ω2
c˙ρ)cos(ω ct)−(3ωc¨ρ−ω3
cρ)sin(ω ct)
(...ρ−3ω2
c˙ρ)sin(ω ct)+(3ωc¨ρ−ω3
cρ)cos(ω ct)
0⎤
⎦ (34)
5.3 Simulation Results
In simulation, we compared our controller to that of [ 14] with identical simulated
robots. Since the controllers are similar, we used gains tuned for the traditional
geometric controller directly on our controller without any modiﬁcation. Since ψis
deﬁned differently between the controllers, there is not a direct way to ensure the
212 M. Watterson and V . Kumar
0 5 10 15 20
t (s)-10010x (m)
0 5 10 15 20
t (s)-50050y (m)
0 5 10 15 20
t (s)0510z (m)05 1 0 1 5 2 0
t (s)-202x (m)
05 1 0 1 5 2 0
t (s)-0.500.5y (m)
05 1 0 1 5 2 0
t (s)246z (m)
Actual Position
Desired Position
Fig. 4 Comparing to the traditional geometric controller [ 14] with ﬁxed ψ. The HFCA is on
the right. On the left is the traditional controller. In this highly aggressive trajectory, [ 14]h i t sa
singularity and fails to control the robot
trajectory of the robot is the same in the yaw orientation. As an example, we can give
both of the controllers a commanded ψof zero.
When we execute a circular trajectory of radius 1 m with both controllers, we
ﬁnd that our controller executes well, while the traditional controller saturates the
actuators and goes unstable at this radius as shown in Fig. 4. Since the trajectory is
not a ﬁxed point, the stability guarantees in [ 8] are not violated by this failure and a
soon as we stop the trajectory, both controller fully stabilize to hover.
We can actually improve the performance of Mellinger and Kumar’s controller
[14] in the circular trajectories by rotating the yaw at the same rate as the circle. This
keeps the ﬁrst body axis further away from the centripetal acceleration vector, thus
avoiding the singularity. In the video we show both controllers working well with a
horizontal circular trajectory of radius 1 m.
In the video and Fig. 5, we then rotated the horizontal circle about the world e1
axis by 1.2 radians with a minimum snap rotational proﬁle. Our HFCA successfully
tracks this trajectory, while the other controller fails once the maximum angle goes
near the 90◦singularity.
In simulation, we execute a continuous sequence of inverted loops which demon-
strates the need of using both ﬁbrations. Whenever the quadrotor is sideways c=0,
we switch between using the ﬁrst and second ﬁbration. Again using ωc=4, and
ρ=1, we get a trajectory which requires a thrust of 0 .6 g at the top and 2 .6g a t t h e
bottom.
The simulated quadrotor is able to fully invert as shown in Fig. 6. The feedback
controller is able to complete ﬂips for as long as we leave it running despite having
Control of Quadrotors Using the Hopf Fibration on SO(3) 213
05 1 0
t (s)-10010x (m)
05 1 0
t (s)-50050y (m)
05 1 0
t (s)05z (m)05 1 0
t (s)-202x (m)
05 1 0
t (s)-0.500.5y (m)
05 1 0
t (s)246z (m)
Actual Position
Desired Position
Fig. 5 Controller comparison with changing ψ. Again, our controller is on the right and performs
better once the quadrotors start to invert. Video is at https://www.youtube.com/watch?v=ycdmfKB-
Ps0
-5 0 5
x(m/s)-4-3-2-1012345z(m/s)Actual Velocity
Desired Velocity
Fig. 6 X-Z velocity plot of inverting the quadrotor in simulation
only proportional and derivative terms. Since the trajectories are periodic, small
errors in velocity persist over multiple loops. Other works have found similar offsets
in their controller and have reduced them by changing model parameters [ 18].
We are able to demonstrate aggressive maneuvers in which the quadrotor is
inclined up to 1.0 radians from hover. We start a circle trajectory and then rotate
it by exp (/hatwider[1,0,0])and then smoothly transition to exp (/hatwider[0,1,0]). The positional
tracking of our controller had a larger error than simulation during the trajectory.
214 M. Watterson and V . Kumar
We believe this can be attributed to several sources of error which were not present
in simulation. Firstly, there is no integral term in the controller to compensate for
model errors. We do not have an exact model of the inertia tensor of the robot or thepropeller force. We estimated the inertia and ﬁt a quadratic model of motor veloc-
ity to thrust on the propellers. At our speeds, we believe there are signiﬁcant drag
effects which are unmodeled. Despite these issues, the robot is stable doing highlyaggressive maneuvers shown in the attached video.
6 Conclusion
We propose the Hopf Fibration Control Algorithm, a geometric tracking algorithm
that uses the Hopf ﬁbration on SO(3). Our algorithm is superior to previous methods
in that it allows the control of quadrotors through all conﬁgurations including onesin which the orientation of the vehicle is inverted. We demonstrate improved results
in aggressive maneuvering using a family of circular trajectories on a sphere, similar
to those used in acrobatics.
References
1. Achtelik, M.W., Lynen, S., Chli, M., Siegwart, R.: Inversion based direct position control and
trajectory following for micro aerial vehicles. In: IROS, IEEE (2013)
2. Gallier, J., Quaintance, J.: Notes on Differential Geometry and Lie Groups. Springer, Berlin
(2017)
3. Geisert, M., Mansard, N.: Trajectory generation for quadrotor based systems using numerical
optimal control. In ICRA, IEEE (2016)
4. Gillula, J.H., Huang, H., Vitus, M.P ., Tomlin, C.J.: Design of guaranteed safe maneuvers using
reachable sets: Autonomous quadrotor aerobatics in theory and practice. In ICRA, IEEE (2010)
5. Grafarend, E.W., Khnel, W.: A minimal atlas for the rotation group SO (3). GEM-Int. J. Geo-
math. (2011)
6. Hehn, M., D’Andrea, R.: Real-Time Trajectory Generation for Quadrocopters. IEEE Trans.
Robot. (2015)
7. How, J., Behihke, B., Frank, A., Dale, D., Vian, J.: Real-time indoor autonomous vehicle test
environment. IEEE Control. Syst. Mag. (2008)
8. Lee, T., Leoky, M., McClamroch, N.: Geometric tracking control of a quadrotor UA V on SE(3).
In: CDC (2010)
9. Lupashin, S., D’Andrea, R.: Adaptive fast open-loop maneuvers for quadrocopters. Auton
Robot. (2012)
10. Lupashin, S., Hehn, M., Mueller, M.W., Schoellig, A.P ., Sherback, M., DAndrea, R.: A platform
for aerial robotics research and demonstration: the ﬂying machine arena. Mechatronics (2014)
11. Lupashin, S., Schllig, A., Sherback, M., D’Andrea, R.: A simple learning strategy for high-
speed quadrocopter multi-ﬂips. In: ICRA , IEEE (2010)
12. Lyons, D.W.: An elementary introduction to the hopf ﬁbration. Math. Mag. (2003)13. Meier, L., Honegger, D., Pollefeys, M.: PX4: A node-based multithreaded open source robotics
framework for deeply embedded platforms. In: ICRA, IEEE (2015)
14. Mellinger, D., Kumar, V .: Minimum snap trajectory generation and control for quadrotors. In:
ICRA, IEEE (2011)
Control of Quadrotors Using the Hopf Fibration on SO(3) 215
15. Mellinger, D., Michael, N., Kumar, V .: Trajectory generation and control for precise aggressive
maneuvers with quadrotors. IJRR (2012)
16. Milnor, J.: Analytic proofs of the “Hairy Ball Theorem” and the brouwer ﬁxed point theorem.
Am. Math. Mon. (1978)
17. Neunert, M., de Crousaz, C., Furrer, F., Kamel, M., Farshidian, F., Siegwart, R., Buchli, J.: Fast
nonlinear model predictive control for uniﬁed trajectory optimization and tracking. In: ICRA,IEEE (2016)
18. Schoellig, A.P ., Wiltsche, C., D’Andrea, R.: Feed-forward parameter identiﬁcation for precise
periodic quadrocopter motions. In: ACC, IEEE (2012)
19. Schulz, M., Augugliaro, F., Ritz, R., D’Andrea, R.: High-speed, steady ﬂight with a quadro-
copter in a conﬁned environment using a tether. In: IROS, IEEE (2015)
20. Sola, J.: Quaternion kinematics for the error-state KF. Technical Report, Laboratoire dAnalyse
et dArchitecture des Systemes-Centre national de la recherche scientiﬁque (LAAS-CNRS),Toulouse, France (2012)
21. Tang, S., Kumar, V .: Mixed integer quadratic program trajectory generation for a quadrotor
with a cable-suspended payload. In: ICRA, IEEE (2015)
22. Whitney, H.: Differentiable manifolds. Ann. Math. (1936)
23. Yershova, A., Jain, S., Lavalle, S.M., Mitchell, J.C.: Generating uniform incremental grids on
SO (3) using the Hopf ﬁbration. IJRR (2010)
24. Zhang, T., Kahn, G., Levine, S., Abbeel, P .: Learning deep control policies for autonomous
aerial vehicles with mpc-guided policy search. In: ICRA, IEEE (2016)
Towards Uniﬁed Framework
for Trajectory OptimizationUsing General Differential
Kinematics and Dynamics
Eiichi Yoshida and Ko Ayusawa
1 Introduction
Optimization technique is becoming more and more important in many research ﬁelds
of robotics to plan and control the motion of various robots. Especially, anthropo-morphic systems including humanoid robots and digital human models that we have
been working on, are attracting interests because of its widespread application areas.
Together with a motion planning method where geometric and kinematic path is ﬁrst
planned and then converted into whole-body trajectory by using dynamic locomotion
pattern generator and inverse kinematics (IK) for a loco-manipulation task [ 36,37],
our group also tackled whole-body motion optimization problem [ 18,30,38] that
was still challenging at that time as it should take into account many mechanical
constraints such as joint or torque limit, as well as constraints like dynamic stabilityand unilateral contacts at the same time for a large number of degrees of freedom
(DOFs).
Whole-body motion optimization technique for anthropomorphic systems keeps
evolving for various purposes: dynamic multi-contact motions [ 8,9,16,32], human
motion retargeting [ 1,23,29,35], and dual-arm manipulation of a ﬂexible object
with minimum deformation [ 25] by humanoid robots. The usage of trajectory opti-
mization for anthropomorphic systems is not limited to humanoid robots, but can be
exploited for human motion analysis and understanding. Human motion simulation is
one of the typical examples. Self-stable dynamic running motion for a human ﬁgurehas been generated by determining model parameters and actuation input based on
efﬁcient optimal control techniques and stability optimization [ 19]. Musculoskele-
E. Y oshida ( B)·K. Ayusawa
CNRS-AIST JRL (Joint Robotics Laboratory), UMI3218/RL Tsukuba Central 1,1-1-1 Umezono, Tsukuba, Ibaraki 305-8560, Japane-mail: e.yoshida@aist.go.jp
K. Ayusawa
e-mail: k.ayusawa@aist.go.jp
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_21217
218 E. Y oshida and K. Ayusawa
tal analysis [ 11,21] of a digital human model is another typical application. This
study models the muscles as massless wires connecting links and computes muscle
tensions by minimizing the total energy and biologically modeled muscle activitiesunder dynamic constraints. Real-time visual feedback of muscular activity was also
proposed by introducing fast computation [ 2,20].
According to recent progress in robotics and increasing requirements for diverse
applications, we are facing more and more complex optimization problems includ-
ing dynamics, system identiﬁcation, and motion generation and control. For instance,
optimization-based motion planning like CHOMP [ 26] and trajopt [ 27] allow con-
tinuous path reﬁnement to obtain a collision-free path even the initial path is in
collision, based on covariant gradient techniques. The cost function can include such
indices like path smoothness, obstacle avoidance and other physical quantities suchas object orientation depending on the required task. Another illustrative example is
the generation of optimal persistent exciting (PE) trajectory for dynamics parameter
identiﬁcation for humanoid robots [ 3,4,12]. The robot needs to move sufﬁciently
each of its links in a dynamic manner for better identiﬁcation performance, which
is called the persistent exciting motion. It is necessary to minimize the condition
number of “regressor matrix” computed from the trajectory while satisfying other
constraints such as dynamic balance and mechanical limitations. Here also, gradients
of several physical quantities should be computed and it will become intractable withnumerical differentiation.
In those complex trajectory optimization problems, the cost function and the
constraints often involve combined physical quantities. The partial derivative of anyphysical quantities with respect to the joint coordinates is therefore indispensable
when evaluating the cost and constraints represented by the coordinates and their
derivatives, as well as forces in both Cartesian and joint space. Previous work [ 24,
28,29] can deal with part of those problems, but still needs improvements to cope
with free-ﬂoating and spherical joints for humans and humanoids, and to reduce
computational cost as well.
Concerning control issues, the optimization technique can be used to build a
controller to achieve the generated trajectory. Recently, control frameworks based
on quadratic programming (QP) for digital humans and humanoid including multiplecontacts with the environments [ 7,8,17,31,32], extending the model predictive
control (MCP) applied to simpliﬁed model of bipedal locomotion [ 13,34]. The basic
approach is to compute the control input to reduce the errors between the desiredtasks and their current state from sensors, anticipating the future for a ﬁnite horizon.
For those controllers to be applied to full whole-body model in real-time motion
generation, efﬁcient analytical gradient computation is a critical issue as well.
In this paper, we present a new framework of trajectory optimization and its appli-
cations for anthropomorphic systems based on a comprehensive differential kinemat-ics and dynamics [ 5,6]. After introducing the comprehensive motion transformation
matrix (CMTM), we show some useful formulae allowing computation of Jacobians
of physical quantities with respect to joint angles and its derivatives. Examples ofoptimization for redundant manipulator with spherical joints and PE trajectory for a
humanoid are demonstrated to validate the effectiveness of the proposed framework.
Towards Uniﬁed Framework for Trajectory Optimization … 219
Especially, the analytical gradient of zero moment point (ZMP) [ 33] with respect
to joint angles and their derivatives has never been shown and used for dynamic
humanoid motion optimization in previous work. Utilization of the framework foroptimal control is then discussed before concluding the paper.
2 New Optimization Framework Using CMTM
This chapter outlines the new optimization framework using CMTM [ 5]. We will
brieﬂy introduce its deﬁnition and derivation of useful analytical Jacobians for vari-
ous physical quantities with respect to joint angles and its derivatives.
2.1 Optimization Formulation
Here the trajectories of the generalized coordinates of a robot qare parameterized
by a vector aand time instance t, namely q(a,t). This parameterization can be
for instance Fourier series, polynomial interpolation, or B-splines, which allowscomputation of their derivatives ˙qand¨qwith respect to aand tdepending on the
implemented trajectory parameterization.
By deﬁning a vector Q=[q
T˙qT¨qT]T, physical quantities yare expressed by
using Q, such as the position, orientation, linear/angular velocity and acceleration
of each link coordinate, the joint torques and the constraint forces acting on the joint
coordinates. By combining those quantities we can compute the center of mass (CoM)and their derivatives, or the ZMP that is a point where the horizontal components
of moments from the ground reaction force are zero. For jth time instance t
j,l e t
Qjdenote the coordinates together with their derivatives and yi,jthe ith physical
quantity at tj, where yi,jconstituting Y, the whole set of the evaluated quantities.
Based on this notation, the trajectory optimization problem can be formulated:
min
ac(Y) (1)
subject to ∀kg k(Y)≤0
where cis the cost function to be evaluated and gkiskth inequality constraint.
Equality constraints are omitted as they can be represented by two inequalities.
The optimization needs the gradient of the cost function and the constraints, which
is computationally expensive if we compute it numerically. The following analyticalgradient computation is therefore necessary for efﬁcient optimization:
∂h
∂a=/summationdisplay
i/summationdisplay
j∂h
∂yi,j∂yi,j
∂Qj∂Qj
∂a(h=co r g k) (2)
220 E. Y oshida and K. Ayusawa
The gradient ∂h/∂yi,jis derived according to the physical quantities involved in the
cost function or the constraints, and the partial derivative ∂Qj/∂acan be computed
from the implemented trajectory parameterization. The term ∂yi,j/∂Qjis the partial
derivative of physical quantities of multi-body system with respect to the joint coor-
dinates and their derivatives. Its typical example is the basic Jacobian [ 15], which is
the partial derivative of the position and orientation of each link with respect to thejoint coordinates. In general, the derivatives of the velocities, the accelerations and
the joint torques with respect to q,˙q,¨qare required in order to compute
∂yi,j/∂Qj.
Although previous work [ 24,28] formulate the analytical partial derivative of
Cartesian coordinates, their derivative, and joint torques with respect to the joint
coordinates and their derivatives, it should be extended for spherical joints, free-
ﬂoating base or other types of joints in order to be applied to anthropomorphicsystems, because the formulations are originally designed for ﬁxed manipulators.
The computation complexity should also be improved for the partial derivative of
the variables of a link using recursive formula, as it is almost O ( N
2
J), where NJis
the number of DOF.
2.2 Comprehensive Motion Transformation Matrix (CMTM)
Before introducing the new framework, we provide deﬁnitions of variables and nota-tions. The position and orientation matrix of the coordinate system of a rigid bodyarepand Rrespectively. Let ωandνbe the linear and angular velocity represented
by the local coordinate respectively, then the following relationship holds.
˙R=R[ω×] (3)
ν=R
T˙p (4)
where skew operator is represented as follows:
[x×]/defines/bracketleftBigg0−x(3)x(2)
x(3) 0−x(1)
−x(2)x(1) 0/bracketrightBigg
(5)
The 6×6 transformation matrix of position pand orientation Ris deﬁned as follows:
A(p,R)/defines/bracketleftbigg
R[p×]R
O3 R/bracketrightbigg
(6)
where Onand Enare n×nzero and identity matrices respectively. Linear and
angular velocities are concatenated and deﬁned as the following vector:
υ/defines/bracketleftbigνTωT/bracketrightbigT(7)
Towards Uniﬁed Framework for Trajectory Optimization … 221
Here, we introduce the operator for linear and angular velocities as follows:
[υ•]/defines/bracketleftbigg
[ω×][ν×]
O3[ω×]/bracketrightbigg
,υ1•υ2/defines[υ1•]υ2 (8)
This operator satisﬁes several important axioms like bilinearity, alternativity, and
Jacobi identity. For the sake of explanation, we utilize variation δαinstead of υin
the following formulations. The variation of Acan be written as:
δA=A[(δα)•] (9)
Let us now deﬁne the following new 18 ×18 “comprehensive motion transfor-
mation matrix (CMTM)” Xas follows:
X(A,υ,˙υ)/defines/bracketleftBiggAO 6O6
A[υ•] AO 6
1
2A/parenleftbig
[˙υ•]+[υ•]2/parenrightbig
A[υ•]A/bracketrightBigg
(10)
We here deﬁne the following variation of 18 dimensional vector:
δx/defines/bracketleftbig
δαTδυTδ˙υT/bracketrightbigT(11)
V ector δxis the concatenated vector of the variation of standard 6 dimensional
coordinates, velocities, and accelerations. Although δxis usually utilized in the
standard formulations of differential kinematics, the differential operation of CMTM
Xwill be discussed by newly deﬁning the variation of 18 dimensional vector:
δξ/defines/bracketleftbigδαTδζTδηT/bracketrightbigT/definesSδx (12)
where,
S(υ,˙υ)/defines⎡
⎣E6 O6 O6
−[υ•] E6 O6
−1
2/parenleftbig
[˙υ•]−[υ•]2/parenrightbig
−1
2[υ•]1
2E6⎤
⎦ (13)
Matrix Stransforms variation δxinto that of new vector δξ, and note that the inverse
matrix of Salways exists.
Let us also deﬁne the following new matrix and operator:
[δξ•]/defines/bracketleftBigg[δα•]O6 O6
[δζ•][δα•]O6
[δη•][δζ•][δα•]/bracketrightBigg
,δξ1•δξ2/defines[δξ1•]δξ2 (14)
Operator [•]also shows the similar features as operator [×] and[•]in Eqs. ( 5) and
(8). By introducing the new vector and operator, the variation of Xcan be written in
the similar form as Eq. ( 9), and we can have:
222 E. Y oshida and K. Ayusawa
δX=X[(δξ)•] (15)
The similarity between Eqs. ( 9) and ( 15) plays an important role when introducing
the Jacobians of physical quantities with respect to joint angles and its derivatives,
which is detailed in [ 5].
Concerning the robot dynamics, inertial properties of a rigid body consist of mass
m,C o M c, and inertia tensor Ic. They can be summarized as the following 6 ×6
matrix:
M/defines/bracketleftbigg
mE3 m[c×]T
m[c×]Ic+m[c×][c×]T/bracketrightbigg
(16)
Deﬁning the inertial forces of a rigid body /hatwidefand the moment around its coordinate
nrepresented in global frame, let us deﬁne 6-axis force frepresented by the local
coordinate as follows:
f/defines/bracketleftBig
R/hatwidefTRnT/bracketrightBigT
(17)
The equations of motion of a rigid body, and its variation can be written as:
f=M˙υ−[υ•]TMυ (18)
δf=Mδ˙υ−Dδυ=Hδx (19)
H/defines/bracketleftbigO6DM/bracketrightbig
(20)
by introducing a matrix D[5].
Here we also deﬁne the variables of kinematic chain: qiisnJiset of joint variables
(angles) where nJiis the number of DOF of joint i;i nt h es a m ew a y , ψidenotes the
joint velocity variables. We can deﬁne the following variation of 3 nJidimensional
vector consisting of the variations of joint variable, velocity, and its derivative:
δχ/defines/bracketleftBig
δqTδψTδ˙ψT/bracketrightBigT
(21)
and the relationship between δxp(i)
iandδχiis summarized as follows [ 5]:
δxp(i)
i=Giδχi=/bracketleftBig
KiTδqTKiTδψTKiTδ˙ψT/bracketrightBigT
(22)
where p(i)and Kiare the index of a root-side link connected to link i, and 6 ×nJi
constant selection matrix specifying the axes of the DOF of the joint irespectively.
For example, if joint i1has z-axis rotational joint and joint i2has a spherical joint,
the corresponding matrices are followings:
Ki1=/bracketleftbig000001/bracketrightbigT,Ki2=/bracketleftbigO3E3/bracketrightbigT(23)
Towards Uniﬁed Framework for Trajectory Optimization … 223
2.3 Analysis on Arbitrary Jacobian
As a result of differential analysis [ 5], we can derive the following differential rela-
tionship between the variation δξjof link jand those of root-side links δξp(k)
k as
follows:
δξj=/summationdisplay
k∈P(j)Xj
kδξp(k)
k (24)
where P(i)is the set of all root-side links recursively connected to link i, including
i, and Xj
kdenotes the relative matrix Xof link jwith respect to k. The coefﬁcient
matrix of Eq. ( 24) means the Jacobian matrix with respect to δξ, and each block
matrix is equal to relative CMTM Xp(k)
k.
By utilizing Eq. ( 12), the following equation is obtained.
δxj=/summationdisplay
k∈P(j)Sj−1Xj
kSp(k)
kδxp(k)
k=/summationdisplay
k∈P(j)/hatwideXj
kδxp(k)
k (25)
The next transformation can be performed with Eq. ( 22)a sf o l l o w s :
δxj=/summationdisplay
kJ(j,k)δχp(k)
k (26)
J(j,k)/definess(j,k)/hatwideXj
kGk (27)
where, s(j,k)is equal to 1 when k∈P(j), and otherwise is equal to 0.
This is analogous to the basic Jacobian [ 15] as shown in the Fig. 1:
δαj=/summationdisplay
k∈P(j)Aj
kδαp(k)
k (28)
The Jacobian of link internal forces can also be derived as follows:
δfj=Ljδχall=/summationdisplay
kL(j,k)δχk (29)
where
L(j,k)=H jJ(j,k) (30)
In the same way, the variation of the joint constraint force fp(j)
jof link jcan be
computed as:
δfp(j)
j=Njδχall=/summationdisplay
kN(j,k)δχk (31)
224 E. Y oshida and K. Ayusawa




   
Fig. 1 The relationship between basic Jacobian and the variance deﬁned with CMTM. The new
framework can also be applied to free-ﬂoating and spherical joints
where
N(j,k)=⎧
⎪⎨
⎪⎩/hatwideH jJ(j,k) (k∈P(j))
Aj
k−T/parenleftBig
/hatwideHkJ(k,k)−[fp(k)
k/hatwide•]TG k/parenrightBig
(k∈C(j))
O6×18 (other s )(32)
/hatwideH j=H j+/summationdisplay
k∈C(j)Aj
k−T/hatwideHkSk−1Xk
jSj (33)
T/defines[E6O6O6] (34)
operation [/hatwide•]/defines/bracketleftbigg
O3/bracketleftbig/hatwidef×/bracketrightbig
/bracketleftbig/hatwidef×/bracketrightbig
[n×]/bracketrightbigg
,[/hatwidef1•]T/hatwidef2=[/hatwidef2/hatwide•]/hatwidef1 (35)
where C(k)is the set of leaves-side links connected to link k, and C(k)is the set of
all leaves-side links recursively connected to link k.
Before computing Jacobian N(j,k),m a t r i x /hatwideH jfor all the links needs to be com-
puted in advance according to recursive formula Eq. ( 33). After updating /hatwideH jfor all
links, matrix N(j,k)for any jand kcan be directly computed by Eq. ( 32). Those
computation can be performed with the computational cost O(NJ)[5].
3 Applications
This section presents examples of application of the proposed framework, ﬁrst opti-
mization of dynamic collision-free motion of redundant robot arm composed of
spherical joints and next whole-body PE trajectory for dynamic parameter identiﬁ-
cation of a humanoid robot [ 6].
Towards Uniﬁed Framework for Trajectory Optimization … 225
3.1 Dynamic Collision-Free Motion for Redundant Robot
We applied the proposed optimization framework to a redundant serial robot manip-
ulator composed of ﬁve spherical joints in a complex environment cluttered withnon-convex obstacles and no gravity. Each link has the identical physical prop-
erties: for link i,p
p(i)
i=[000.2][m],mi=3.0[kg],ci=[000.1][kgm],
Ici=diag([11.21 1.22.4])∗10−3[kgm2]with its base spherical joint is ﬁxed at
[000]. As shown on the bottom-right in Fig. 2, starting from p0
e=[001]att=0.0,
the end-effector of the manipulator should pass through the points p1
e=[010]at
t=1.0 [s], before stopping at p2
e=[100]att=2.0 [s] with zero joint velocities
and accelerations. Six square-shape hollow obstacles composed of four cylinders
whose diameter and length are 0.1 [m] and 0.4 [m] are placed such that their center
is at the distance of 0.4 and 0.8 from the origin along each of x,yand zaxes.
The trajectory of spherical joints is parameterized by using B-splines as in [ 29],
expressing their joint angles with an angle-axis vector. During the movement, the
joint torque limitations are considered; |τ1,k|<0.1 [Nm] and |τj,k|<20.0[ N ](2≤
j≤5), which means the torque of the base spherical joint is limited to a very low
(0, 0, 1)0.00[s] 0.16[s] 0.33[s] 0.50[s]
0.67[s] 0.83[s] 1.00[s] 1.16[s]
1.33[s] 1.50[s] 1.67[s] 1.83[s]
2.00[s]
(0, 1, 0) (1, 0, 0)z
x y
Fig. 2 Snapshots of the generated motion of the redundant robot in a cluttered environment. Starting
from the initial position [001], the target end-effector positions (bottom-right) at [010]and[100]
were achieved without collision to obstacles (video in the supplementary material)
226 E. Y oshida and K. Ayusawa
Fig. 3 Resultant joint torque
trajectories of the base
spherical joint. The torque
limit of small value of ±0.1
[Nm] is satisﬁed throughout
the motion-0.100.1
-0.100.1
0 0.5 1 1.5 2-0.100.1X
Y
Z
Time [s]Joint torque [Nm]
value. In order to avoid collisions and self-collisions, constraints on the distance from
joint kto the closest obstacle dkand to the link lshould also be satisﬁed; dk≥0.15
and||pk−pl|| ≥ 0.1(k/negationslash=l)so that a sphere of diameter 0.1[m] around the joint
does not collide.
All the constraint conditions were taken into account according to penalty func-
tion method. The optimization itself was solved by quasi-Newton method [ 10]b y
utilizing the proposed framework. As shown in Fig. 2, the end-effector successfully
passed through the targeted positions without colliding obstacles. As can be seen in
Fig. 3showing the resultant joint torque of the base spherical joint, the torque of all
the axes satisfy the desired limitations, which has been difﬁcult with conventional
optimization methods.
3.2 Optimization of Humanoid PE Trajectory
for Identiﬁcation
In order to optimize the PE trajectory for dynamic parameter identiﬁcation of a
humanoid robot, the condition number of the “regressor matrix” obtained from joint
trajectory should be minimized while maintaining dynamic constraints. Although we
showed an analytical framework to optimize the condition number in our previous
work [ 3], the stability was considered only statically by using the CoM, which is
conservative and may limit the identiﬁcation performance. In the resultant motions,
both feet were placed to the ground as the dynamic stability condition becomes too
severe to be satisﬁed only by the CoM condition. This limitation makes it difﬁcult
to generate dynamic leg motions by standing on one leg for better identiﬁcation.
To solve this issue, we here derive analytical computation of Jacobian of the ZMP
based on the proposed framework to use it as the dynamic stability constraint in
the trajectory optimization. The joint torque τj,a nJidimensional vector, can be
Towards Uniﬁed Framework for Trajectory Optimization … 227
obtained as:
τj=KjTfp(j)
j (36)
Its variance can be be extracted from the corresponding component of N(j,i)as
follows:
δτj=/hatwideNjδχall=/summationdisplay
i/hatwideN(j,i)δχi (37)
/hatwideN(j,i)/definesKjTN(j,i) (38)
The total external forces acting on the kinematic chain are equivalent to fp(W)
W
where index Wmeans the world coordinate is connected to ﬂoating base link 0. By
redeﬁning Fex/definesfp(w)
w, we can obtain ZMP of Fexprojected, for example, on x-y
plane.
Fex/defines/bracketleftbigFex(1)··· Fex(6)/bracketrightbigT(39)
pZMP/defines/bracketleftbig−Fex(5)/Fex(3)Fex(4)/Fex(3)0/bracketrightbigT(40)
Jacobian matrix of pZMP can therefore be easily computed by utilizing N(W,i)as
follows:
δpZMP=/summationdisplay
i/parenleftbig
ZN(W,i)/parenrightbig
δχi (41)
Z/defines⎡
⎢⎣00Fex(5)
Fex(3)2 0−1
Fex(3)0
00−Fex(4)
Fex(3)21
Fex(3)00
00 0 0 0 0⎤
⎥⎦ (42)
We have derived a dynamically stable optimal PE trajectory on one leg by con-
straining the ZMP inside the area of 4 [cm] and 1 [cm] around the center of the stand-ing foot in front and lateral direction. The trajectory is parameterized by B-Spline by
using physical properties from the robot CAD model. We also added constraints on
forces and torque applied on the ankle so that the horizontal forces F
ex(1),Fex(2)and
torque Fex(6)stay within ±20[N], ±20[N], ±4[Nm] respectively to avoid slipping.
As shown in snapshots in Fig. 4, the humanoid HRP-4 [ 14] successfully performed
the optimized PE trajectory on dynamic simulator Choreonoid [ 22], which validates
the feasibility. Note that this one-leg dynamic motion cannot be generated with the
previous method [ 3] considering only CoM for stability.
228 E. Y oshida and K. Ayusawa
0.0[s] 0.4[s] 0.8[s] 1.2[s] 1.6[s] 2.0[s]
2.4[s] 2.8[s] 3.2[s] 3.6[s] 4.0[s] 4.4[s]
Fig. 4 Snapshots of optimized one-leg PE trajectory for dynamic parameter identiﬁcation. The
humanoid robot HRP-4 can successfully perform resultant whole-body motion by also exciting thefree leg (video in the supplemental material)
4 Discussions: Controller Based on Proposed Framework
The formulation using CMTM can potentially extend the arguments on conven-
tional generic trajectory tracking control in such a way that they include velocity and
acceleration. We believe that various Jacobians derived by CMTM can be applied towhole-body cooperative control currently using the basic Jacobian in future devel-
opments. The proposed framework of trajectory optimization has high afﬁnity to
Model Predictive Control (MPC) or Receding Horizon Control that determines con-trol input for a ﬁnite horizon. It optimizes the integrated evaluation function of the
state variables or control inputs on the ﬁnite horizon, based on discrete equations
describing system equation and its time integral.
MPC has been applied to walking motion control for humanoid robots by
researchers as Kajita and Wieber [ 8,13,34]. By approximating the whole-body
dynamics as a concentrated mass model, those studies have been investigating a
linear system equation whose state variables are the CoM position, velocity and
acceleration using the CoM jerk as the system input. Constraints related to ZMP andother state variables are also taken into account in case of humanoid to guarantee
its balance condition. In above studies on locomotion, the input jerk is determined
based on the evaluation function to minimize the tracking error of state and the jerkitself.
Just like mapping Cartesian states into joint angles through basic Jacobian as
is often the case in position-based tracking control, the state variables on velocity,acceleration and ZMP can also be mapped in the same way by using the proposed
framework. Since the jerk is regarded as the variance of acceleration, it may be used as
trajectory parameter a, or the optimization problem of MPC may be formulated as the
evaluation function Eq. ( 1) itself in the presented trajectory optimization framework.
Towards Uniﬁed Framework for Trajectory Optimization … 229
In general, the solution of the evaluation function should satisfy the necessary
condition that the gradient of the extended Lagrangian function should be equal
to zero. The derivation of analytical gradient therefore plays an important role todetermine the control inputs and to investigate the system stability [ 34]. The most
advantageous feature of the proposed comprehensive framework is its capability
to analytically formulate the gradient of arbitrary physical quantities with respectto motion trajectory. Having started studying this new framework, we intend to
come up with a new theoretical basis on determination of input or stability analysis
for various control problems of anthropomorphic systems including humanoids anddigital humans.
5 Conclusion
In this paper, we addressed a new framework for trajectory optimization based on
comprehensive differential kinematics and dynamics that allows dealing with the
gradient of any physical quantities with respect to position and its derivatives in the
same way as conventional differential kinematics. We introduced a matrix called
CMTM, an extended version of transformation matrix by adding velocity and accel-eration, and showed that it has the same property as the transformation matrix to
apply the chain rules and Jacobian computations. It was also shown that the gradient
of physical quantities such as forces applied to links and joints can be computedanalytically. Another advantage is that the proposed method can deal with spherical
joint, which is critical to analyze human models.
A couple of application examples were presented to demonstrate the usefulness
of the proposed framework. We ﬁrst showed dynamic trajectory optimization of a
redundant serial robot manipulator composed of spherical joints. A collision-free
dynamic motion was successfully generated in a cluttered environment with non-convex obstacles, imposing a strong torque limit at the same time. This validates
the basic trajectory optimization capacity of the proposed framework under severe
constraints. Another application is optimization of PE trajectory for identiﬁcationof dynamic parameters of a humanoid robot. In this example, analytical gradient
of ZMP with respect to joint angle and its derivatives was utilized to guarantee the
balance. Dynamic one-leg PE motions were generated whose validity was conﬁrmedby dynamic simulator.
Finally, we discussed the potential of the proposed framework for building con-
trollers. Combining with MPC, we believe its ability of analytical computation of
gradient of any physical quantities with respect to joint trajectory can bring a new
perspective to the optimal control of anthropomorphic systems. Especially, MPC isheavily utilized for humanoid walking and whole-body control on a simpliﬁed model,
we expect to contribute to development of such a controller for full anthropomorphic
model. We will also push forward the research to apply the proposed framework tohuman motion modeling and analysis through efﬁcient and accurate computation.
230 E. Y oshida and K. Ayusawa
Acknowledgements This research has been partly supported by Japan Society for the Promotion
of Science (JSPS) Grant-in-Aid for Scientiﬁc Research (A) Number 17H00768.
References
1. Ayusawa, K., Morisawa, M., Y oshida, E.: Motion retargeting for humanoid robots based on
identiﬁcation to preserve and reproduce human motion features. In: Proceedings of the 2015IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2774–2779 (2015)
2. Ayusawa, K., Nakamura, Y .: Fast inverse kinematics algorithm for large dof system with decom-
posed gradient computation based on recursive formulation of equilibrium. In: Proceedings ofthe IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3447–3452(2012)
3. Ayusawa, K., Rioux, A., Y oshida, E., V enture, G., Gautier, M.: Generating persistently excit-
ing trajectory based on condition number optimization. In: Proceedings of the 2017 IEEE
International Conference Robotics and Automation, pp. 6518–6524 (2017)
4. Ayusawa, K., V enture, G., Nakamura, Y .: Identiﬁcation of humanoid robots dynamics using
ﬂoating-base motion dynamics. In: Proceedings of the 2008 IEEE/RSJ International Conferenceon Intelligent Robots and Systems, pp. 2854–2859 (2008)
5. Ayusawa, K., Y oshida, E.: Comprehensive theory of differential kinematics and dynamics for
motion optimization. In: Robotics: Science and Systems XIII (2017)
6. Ayusawa, K., Y oshida, E.: Comprehensive theory of differential kinematics and dynamics
towards extensive motion optimization framework. Int. J. Rob. Res. (2018). (Under review)
7. Bouyarmane, K., Kheddar, A.: On the dynamics modeling of free-ﬂoating-base articulated
mechanisms and applications to humanoid whole-body dynamics and control. In: Proceedingsof the 2012 IEEE-RAS International Conference on Humanoid Robots, pp. 36–42 (2012)
8. Caron, S., Kheddar, A.: Multi-contact walking pattern generation based on model preview con-
trol of 3D com accelerations. In: Proceedings of the 2016 IEEE-RAS International Conferenceon Humanoid Robots, pp. 550–557 (2016)
9. Dai, H., V alenzuela, A., Tedrake, R.: Whole-body motion planning with centroidal dynam-
ics and full kinematics. In: Proceedings of the 2014 IEEE-RAS International Conference onHumanoid Robots, pp. 295–302 (2014)
10. Fletcher, R.: Practical Methods of Optimization, 2nd edn. Wiley-Interscience, New Y ork (1897)11. Imamura, Y ., Ayusawa, K., Y oshida, E.: Risk estimation for intervertebral disc pressure through
musculoskeletal joint reaction force simulation. In: Proceedings of the 39th IEEE AnnualInternational Conference Engineering in Medicine and Biology Society (2017). To appear
12. Jovic, J., Escande, A., Ayusawa, K., Y oshida, E., Kheddar, A., V enture, G.: Humanoid and
human inertia parameter identiﬁcation using hierarchical optimization. IEEE Trans. Robot.32(3), 726–735 (2016). https://doi.org/10.1109/TRO.2016.2558190
13. Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Harada, K., Y okoi, K., Hirukawa, H.: Biped
walking pattern generation by using preview control of zero-moment point. In: Proceedings of
the 2003 IEEE International Conference on Robotics and Automation, pp. 1620–1626 (2003)
14. Kaneko, K., Kanehiro, F., Morisawa, M., Akachi, K., Miyamori, G., Hayashi, A., Kanehira,
N.: Humanoid robot HRP-4 - humanoid robotics platform with lightweight and slim body.In: Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots andSystems, pp. 4400–4407 (2011)
15. Khatib, O.: A uniﬁed approach for motion and force control of robot manipulators: the opera-
tional space formulation. Int. J. Robot. Res. 3(1), 43–53 (1987)
16. Lengagne, S., V aillant, J., Y oshida, E., Kheddar, A.: Generation of whole-body optimal dynamic
multi-contact motions. Int. J. Robot. Res. 32(9–10), 1104–1119 (2013). https://doi.org/10.
1177/0278364913478990
Towards Uniﬁed Framework for Trajectory Optimization … 231
17. Liu, M., Micaelli, A., Evrard, P ., Escande, A., Andriot, C.: Interactive virtual humans: a two-
level prioritized control framework with wrench bounds. IEEE Trans. Robot. 28(6), 1309–1322
(2012)
18. Miossec, S., Y okoi, K., Kheddar, A.: Development of a software for motion optimization of
robots - application to the kick motion of the hrp-2 robot. In: Proceedings of the 2006 IEEEInternational Conference on Robotics and Biomimetics, pp. 299–304 (2006)
19. Mombaur, K.: Using optimization to create self-stable human-like running. Robotica 27, 321–
330 (2009)
20. Murai, A., Kurosaki, K., Yamane, K., Nakamura, Y .: Computationally fast estimation of muscle
tension for realtime bio-feedback. In: Proceedings of the 31st Annual International Conferenceof the IEEE EMBS, pp. 6546–6549 (2009)
21. Nakamura, Y ., Yamane, K., Fujita, Y ., Suzuki, I.: Somatosensory computation for man-machine
interface from motion-capture data and musculoskeletal human model. IEEE Trans. Robot.
21(1), 58–66 (2005)
22. Nakaoka, S.: Choreonoid: extensible virtual robot environment built on an integrated gui frame-
work. In: Proceedings of the 2012 IEEE/SICE International Symposium on System Integration,pp. 79–85 (2012)
23. Nakaoka, S., Komura, T.: Interaction mesh based motion adaptation for biped humanoid robots.
In: Proceedings of the 2012 IEEE-RAS International Conference on Humanoid Robots, pp.625–631 (2012)
24. Park, F.C., Bobrow, J.E., Ploen, S.R.: A lie group formulation of robot dynamics. Int. J. Robot.
Res. 14(6), 609–618 (1995)
25. Ramirez-Alpizar, I.G., Harada, K., Y oshida, E.: Motion planning for dual-arm assembly of
ring-shaped elastic objects. In: Proceedings of the 2014 IEEE-RAS International Conferenceon Humanoid Robots, pp. 594–600 (2014)
26. Ratliff, N., Zucker, M., Bagnell, J.A., Srinivasa, S.: CHOMP: gradient optimization techniques
for efﬁcient motion planning. In: Proceedings 2009 IEEE International Conference on Roboicsand Automation, pp. 489–494 (2009)
27. Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., Abbeel, P .: Finding locally optimal,
collision-free trajectories with sequential convex optimization. In: Proceedings of the Robotics:Science and Systems IX (2013)
28. Sohl, G.A., Bobrow, J.E.: A recursive multibody dynamics and sensitivity algorithm for
branched kinematic chains. J. Dyn. Syst. Meas. Control 123(3), 391–399 (2001)
29. Suleiman, W., Y oshida, E., Kanehiro, F., Laumond, J.P ., Monin, A.: On human motion imitation
by humanoid robot. In: Proceedings of the 2008 IEEE International Conference Robotics andAutomation, pp. 2697–2704 (2008)
30. Suleiman, W., Y oshida, E., Laumond, J.P ., Monin, A.: On humanoid motion optimization. In:
Proceedings of 7th IEEE-RAS International Conference on Humanoid Robots, pp. 180–187(2007)
31. Tassa, Y ., Erez, T., Todorov, E.: Synthesis and stabilization of complex behaviors through online
trajectory optimization. In: Proceedings of the 2012 IEEE/RSJ International Conference onIntelligent Robots and Systems, pp. 4906–4913 (2012)
32. V aillant, J., Kheddar, A., Audren, H., Keith, F., Brossette, S., Escande, A., Bouyarman, K.,
Kaneko, K., Morisawa, M., Gergondet, P ., Y oshida, E., Kajita, S., Kanehiro, F.: Multi-contactvertical ladder climbing with an hrp-2 humanoid. Auton. Robot. 40(3), 561580 (2016). https://
doi.org/10.1007/s10514-016-9546-4
33. Vukobratovi´ c, M., Borovac, B.: Zero-moment point - thirty-ﬁve years of its life. Int. J.
Humanoid Robot. 1(1), 157–174 (2004)
34. Wieber, P .B.: Trajectory free linear model predictive control for stable walking in the presence
of strong perturbations. In: Proceedings of the 2006 IEEE-RAS International Conference onHumanoid Robots, pp. 137–142 (2006)
35. Yamane, K., Anderson, S.O., Hodgins, J.K.: Controlling humanoid robots with human motion
data: Experimental validation. In: Proceedings 2010 IEEE-RAS International Conference onHumanoid Robots, pp. 504–510 (2010)
232 E. Y oshida and K. Ayusawa
36. Y oshida, E., Belousov, I., Esteves, C., Laumond, J.P .: Humanoid motion planning for dynamic
tasks. In: Proceedings of 5th IEEE-RAS International Conference on Humanoid Robots, pp.1–6 (2005)
37. Y oshida, E., Esteves, C., Belousov, I., Laumond, J.P ., Sakaguchi, T., Y okoi, K.: Planning 3D
collision-free dynamic robotic motion through iterative reshaping. IEEE Trans. Robot. 24(5),
1186–1198 (2008). https://doi.org/10.1109/TRO.2008.2002312
38. Y oshida, E., Kanoun, O., Esteves, C., Laumond, J.P ., Y okoi, K.: Task-driven support poly-
gon reshaping for humanoids. In: Proceedings of 6th IEEE-RAS International Conference onHumanoid Robots, pp. 827–832 (2006)
T oward a Human(oid) Motion Planner
Eiichi Y oshida, Ko Ayusawa, Y usuke Y oshiyasu, Adrien Escande
and Abderrahmane Kheddar
1 Introduction
When a robot moves to one position to another, it needs to change its conﬁguration byavoiding collision with the given environment. Research on this problem of planningrobot motion or path from initial to ﬁnal positions became active from 1980’s when
the notion of conﬁguration space was introduced and deterministic methods such
as potential methods were proposed [ 14]. During 1990’s, sampling-based motion
planning methods, which are still actively studied, exhibited remarkable progress in
their theory and applications [ 3,15]. In 2000’s, along the signiﬁcant improvement
in capacity of computers, the motion planning techniques, mainly sampling-based
methods, has been utilized in various applications with many DOFs, such as trajectory
planning of robots in production lines and medical and bioinformatic computations.
On the other hand, motion planning technology and humanoid robotics started
being integrated in early 2000s [ 13] along with the rapid progress of sampling-
based methods that has afﬁnity with problems with many DOFs. At the same time,
E. Y oshida ( B)·K. Ayusawa ·Y . Y oshiyasu ·A. Escande ·A. Kheddar
CNRS-AIST JRL (Joint Robotics Laboratory),UMI3218/RL, Tsukuba Central 1, 1-1-1 Umezono,Tsukuba, Ibaraki 305-8560, Japane-mail: e.yoshida@aist.go.jp
K. Ayusawa
e-mail: k.ayusawa@aist.go.jp
Y . Y oshiyasu
e-mail: yusuke-yoshiyasu@aist.go.jp
A. Escande
e-mail: adrien.escande@gmail.com
A. Kheddar
e-mail: kheddar@lirmm.fr
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_22233
234 E. Y oshida et al.
motion planning was used to generate the movement of animation characters in
computer graphics [ 8]. The authors joined this exciting research ﬁeld of humanoid
motion planning, focusing on whole-body motion generation. Starting from two-stage methods where geometric and kinematic motion planning ﬁrst provides the path
of humanoid base that is converted into whole-body trajectory with dynamic motion
pattern generator and inverse kinematics (IK) allowing task execution in workspace.As a result, we have demonstrated its feasibility through manipulation while walk-
ing [ 27] and pivoting manipulation of bulky objects [ 29] by full-size humanoid
HRP-2. Optimization was also applied to dynamic whole-body motion and multi-contact motion generation [ 16]. A collection of advanced work on humanoid motion
planning was published in 2010 [ 11].
Till then we concentrated on motion planning by regarding humanoid as just robots
with many DOFs without really its characteristics of human-like shape. However,
while planning and generating a variety of humanoid motions, we got interested in
“natural” humanoid motions, which correspond to the motions human should make.Our ﬁrst human-aware motion is walking path planning [ 19] that can generate paths
including lateral motions when the goal is close and aside, not only the normal
walking path whose velocity is in the front direction, modeled in the same way as
non-holonomic motion for car-like robots. We also noticed that some multi-contact
motions with dynamic constraints look human-like. These observations brought usto further investigation of human motion: is human optimizing something when
they move? If yes, what are the optimization criteria? Although these are still not
completely solved, we ﬁrst tried to approach by analyzing and imitating the humanmotion and to see what we can get.
From 2013 we joined “METI Robotic Devices for Nursing Care Project” [ 6] that
is a national project to support companies developing assistive devices that reduceburden of the elderly as well as caregivers using robotics technology. We addressed a
challenging problem of using a humanoid robot as a subject that evaluates the devices
by wearing assistive devices instead of human. We showed that this approach is usefulbecause we can evaluate the supportive effects in a quantitative manner by measuring
motor torques with and without the wearable device, whereas it is difﬁcult to directly
measure forces applied to human body [ 18]. We also enhanced the digital human that
models surface shape, musculo-skeletal structure and interactions with humans and
environments [ 30]. We continue addressing human motion understanding to better
reproduce human motions with a humanoid robot.
Human-based humanoid motion planning is not only useful for evaluation of
assistive products, but also designing motions of disaster response and heavy-duty
robots. In case of disasters in artiﬁcial structure like nuclear plants, humanoid robot
could be advantageous because it can easily access to environments made for human.
In 2015 DARPA Robotics Challenge (DRC) [ 4] was held as a competition of capacity
of doing tasks in the real world. One of the examples of heavy-duty task is assembly
for airplane manufacturing. Human workers are often forced to do such tasks as
cabling and device installation under very difﬁcult posture. This is another typicalﬁeld where humanoid robots can serve, as they have afﬁnity to human workers’
workspace and possibly execute tasks in their place to reduce their load. In both
Toward a Human(oid) Motion Planner 235
applications, human motions can offer many suggestions about how the humanoid
adapt the environment and execute the tasks. If we can model human behaviors and
interactions with products as motion primitives will help product design as we can testthe digital mockup using the human model, without building hardware prototypes.
We expect that those behaviors can be modeled as basic motion primitives that
can be tuned and combined to reproduce higher-level complex behaviors. Human-based motion generation is also valuable for graphics animation, for instance crowd
simulation and motions in games.
2 Humanoids Meet Planning
The early important work of humanoid motion planning was made by Kuffner
et al. [ 13] where sampling-based motion planning was ﬁrst applied to human-size
humanoid robot. In the computer graphics ﬁeld, motion planning techniques are
used to deal with motions of digital characters as it is easier to generate motions of
multi-DOF structure like [ 8].
2.1 Whole-Body Dynamic Motion Planning
We have proposed a two-stage method [ 27] that plans the path of robot base at pelvis
in the ﬁrst stage using a well-established method [ 24] for car-like robot, by taking
into account the feature of walking that it is always easy to move in the direction thatrobot is facing. Then in the second stage the planned path is transformed into 3D
dynamically stable walking motion based on Zero Moment Point (ZMP) [ 12] while
ensuring the upper body does not collide with obstacles during manipulation of along bar Fig. 1shows the obtained experimental results.
In the above work we dealt with the walking and upper-body separately, which
means the walking motion was planned ﬁrst and then the apply adjustments by shift-ing pelvis position to absorb the “disturbance” caused by the upper-body motion for
maintain stability. We came to a question about whether we can generate a motion
incorporating the upper and lower body at the same time. The approach taken herewas to integrate the walking pattern generator into the priority-based generalized IK
[23]. We have taken advantage that the ZMP-based walking pattern generator using
preview control [ 12] outputs the velocity of Center of Mass (CoM) for dynamically
stable walking motion for speciﬁed footsteps. The desired CoM velocity can be easily
integrated in the task-priority generalized IK to generate stable whole-body motionssmoothly combining those of arms and legs as shown in Fig. 2[28]. Although no
particular tasks were assigned to the upper-body, some people remarked that the
resultant motion was human-like: bringing the other arm backward as the conse-quence of IK that is a local optimization keeping the CoM around the center of the
support polygon. From this period we start being interested in human-like motions.
236 E. Y oshida et al.
Fig. 1 Experiment of 3D collision-free motion for bar-carrying task
Fig. 2 Whole-body reaching motion integrating upper-body motion and stepping
2.2 Multi-contact Dynamic Motion
Human daily activities include many motions with multiple contacts with environ-
ments and objects, not only walking. Therefore, collisions or contacts should not be
just avoided but exploited to achieve complex tasks. Multi-contact humanoid motionplanning was pioneered by Kheddar, Escande et al. since mid-2000’s, and this is still
a very active research ﬁeld. Multi-contact motion planning and generation is chal-
lenging in several aspects: it requires coordinated whole-body motion, ﬂat-planestability criteria like ZMP is not valid any more, and contact space between the robot
and the environments is volume zero for planning.
Toward a Human(oid) Motion Planner 237
Fig. 3 With the right arm, the HRP-2 robot takes an additional support on the desk, which allows
it to stably lean and put a ball into the box located under the desk
After developing multi-contact motion planning methods [ 2,7], we have investi-
gated dynamic motion generation through optimization [ 16]. We have modeled the
problem of optimal trajectory generation as a Semi Inﬁnite Programming (SIP) by
parameterizing the joint trajectories with uniform cubic B-Splines curves. The motionis decomposed into several contact phases and we formulate the multi-contact motion
generation as an optimization problem that minimizes the following cost function C,
under the constraints of mechanical limitation of the robot and multi-contact stability.
As the objective function, we consider a weighted sum of the motion duration,
square torques and jerks as follows:
C
r(qr)=Np/summationdisplay
i=1aT i+Ndof/summationdisplay
i=1/integraldisplayT
0/parenleftBig
bΓi2+c...qr,i2/parenrightBig
dt (1)
where Tiis the time duration of phase iand Npthe number of phases ( T=/summationtext Np
iTi)
in the ﬁrst term. In the second term, Ndofdenotes the number of DOF, Γiand qr,i
are the torque and joint angle of ith joint of the robot. Optimization is performed by
taking into account constraints
g(qr)≤0( 2 )
including equalities and inequalities that correspond to constraints such as robot
dynamics, contact conditions and stabilities, where the vector qrandΓdenote joint
angles and torques for whole robot. These constraints are considered when the opti-
mization is solved by solvers like IPOPT [ 26]. It is always not easy to determine
the weights, and here we set a=5,b=1e−2,c=1e−6that are empirically deter-
mined through several simulations. Recently, “inverse optimal control problem” is
intensively studied to systematically compute those weights [ 20], which could help
determining the composition of objective functions.
Figure 3shows a typical multi-contact dynamic motion that supports the body by
putting on a hand to reach a position under the desk. By introducing the time durationand dynamic multi-contact constraints for global trajectory optimization, we could
obtain natural-looking smooth motion that could not be possible with only static or
planar stability constraints.
238 E. Y oshida et al.
3 Reproducing Human Motion by Humanoid Using
Optimization
The motion planning and generation methods with local and global optimization
presented in Sect. 2.1did not explicitly imitate human motions although they often
produce motions that are natural-looking and human-like motion. These results sug-gest that humans may optimize some of criteria that we have introduced previously. In
this section, we introduce some research on human-like humanoid motion generation
based on optimization.
3.1 Human Motion Reproduction Through Optimization
An alternative direct approach for human-like humanoid motion generation is a
process called “retargeting” that converts human motions, often measured as marker
positions by motion capture systems, into motions of digital actors and humanoids[10,17,22]. Retargeting has been intensively studied envisaging such applications as
not only natural human-robot interfaces and entertainment use, but also evaluation of
assistive devises by using a humanoid as an active dummy [ 18]. In the last application,
we can exploit the anthropomorphic shape and structure of humanoids by making
it use or wear the assistive devices to provide quantitative evaluation by measuring
joint torques, which is difﬁcult with humans.
A standard way of retargeting is a sequential processing. The marker positions
are ﬁrst converted into joint angles, and then the motion is adjusted in order to meet
the mechanical constraints such as joint limits and stability conditions. Althoughcloseness to the original human motions can be used as an objective function of
optimization of the ﬁrst step, modiﬁcation of trajectory in the second step may induce
inconsistency with respect to the original human motions.
In order to overcome those issues, we have introduced a simultaneous optimization
for criteria for both robots and humans [ 1]. In this optimization, we optimize not only
the objective function C
r(qr)in Eq. ( 1) for the robot, but also a combined one:
min
qr,qh,φCr,h(qr,qh,pref
h,φ)
s.t. g(qr)≤0
h(qr,qh,φ)=0(3)
where qh,pref
handφare joint angle of the human model, marker positions, and
human geometric parameters respectively. Some part of qrand qhare “shared” by the
robot and the human to ensure the motion compatibility. The geometric parameters
φmainly represents the body link length to absorb the geometric difference between
the robot and the human. Here the equality constraint h(qr,qh,φ)=0 implies how
to map the coordinate of each body segments between the robot and the human. The
Toward a Human(oid) Motion Planner 239
φ
φφφ
Fig. 4 The framework of human motion retargeting based on simultaneous optimization for the
human and the robot
constraints g(qr)≤0 can be taken into account during the optimization in the same
manner as in Sect. 2.
This method allows generating robot motions as close as original human motion
within a uniﬁed framework of simultaneous optimization, while keeping track of
how the features of human motion, such as joint trajectory, the dynamics of CoM
and ZMP , are preserved or modiﬁed as a result of retargeting. Since the human motion
qhis also included in the optimization, the resulting motion can be regarded as the
motion the human would make if he had the same constraints as the robot. We can
take advantage of this property of this method by estimating what kind of human
motions can be reproduced by the robot. The method is illustrated in Fig. 4[1].
As mentioned later, we can use this method as a part of human-in-the-loop plan-
ning to utilize human motion as the basic path for complex humanoid motion in con-
strained environments. Figure 5shows the snapshot of a captured caregiving motion
by the humanoid robot HRP-4. We can observe that the humanoid robot repro-
duces whole-body motion by maintaining its stability although the motion speed
was reduced by three times due to its mechanical limitation.
3.2 Constraints Shape Motions
The resultant motions from optimization depend not only on its objective function
but also constraints. Together with the objective function that is supposed to govern
240 E. Y oshida et al.
Fig. 5 Snapshots of a retargeted assisting motion. The upper row shows the markers and skeleton
captured human motion. The retargeted humanoid motion with HRP-4 is shown in the lower row
Fig. 6 Walking motion generated from multi-contact optimization
human motions, we expect that constraints also “shape” the generated motions. For
instance, in applications of assistive device evaluation by a humanoid robot, it may be
necessary to reproduce some constrained motions of the elderly or the handicapped.As it is difﬁcult to test or evaluate the device by the subjects with the disability,
humanoids can be helpful if they can replace those speciﬁc subjects using the device.
We have veriﬁed this possibility by some basic experiments in our previous work
[16] based on the framework of optimization in Sect. 2.2. Figure 6shows the results
of walking motions generated based on the optimization in Eq. ( 1) by providing the
contact sequence of footsteps. Although it is not still a natural walking motion, weregard this as the basic walking motion without any constraints.
In order to see the difference of motions with additional constraints, we introduced
a desired upper limit of the contact force of the (left) foot. The observed average
ground reaction force is 590N during standby standing posture and even larger during
walking. We here added an inequality constraint limiting the contact force of the leftfoot less than 500N during the walking motion. The resultant robot motion is shown
in Fig. 7. When we have a sore foot, we try to minimize the force on the foot by
Toward a Human(oid) Motion Planner 241
Fig. 7 Walking motion generated with constraints of sore foot
 0 100 200 300 400 500 600 700 800
 0 2 4 6 8 10 12Foot forces (N)
time(s)walking motion (broken leg) forces
right foot
left foot
preparation phase
Fig. 8 Foot forces during the broken leg walking. The double support phase is used to accumulate
kinetic energy to cope the broken leg
leaning on the other foot. In this experiment, we can see similar behavior with the
right knee more deeply bent than the left one. Figure 8shows the vertical force
measured by the force-torque sensors at the ankles. Except the peak due to the error
of open-loop trajectory control, the force at the left foot is maintained around 500N.
It is also noteworthy that a preparation phase appears before the left foot stance phase
(during the double support phase) used to accumulate kinetic energy.
In this section, we demonstrated that the optimization process can possibly repro-
duce human motions with humanoid robots. Inverse optimal control is one of the
promising methods to model human motions to investigate on what criteria humans
adapt or optimize their motions. We also showed that human motions with disability
can be reproduced by identifying the corresponding constraints and applying to the
242 E. Y oshida et al.
optimization process. This method will open a new perspective of simulations and
humanoid imitation of disabled motions.
4 T oward Human Planning
We have tried to analyze and understand human motions through their reproduction
by humanoid robots. Those ﬁndings can also be beneﬁcial to predict, or to planhuman motions including interactions with environments and devices using a more
detailed body model.
In this section, we introduce some of ongoing research and future perspective
toward “human planning”.
4.1 Reproducing Interactive Human Motions
When we design a device for human, especially an assistive device, it is important toknow the physical interactions between the device and a human. If a simulator canreproduce human behaviors precisely, it allows estimating the positive and negative
effects of the device without making hardware prototypes that is time-consuming.
The design cycle time can be reduced by performing several design reﬁnements witha simulated human and the device before making a semi-ﬁnal product prototype.
In “METI Robotic Devices for Nursing Care Project” [ 6], the AIST is working
in the consortium of device standardization and evaluation, and has been developinga “digital human model” including skin surface and interaction modeling, physical
simulation with joint torques, and musculo-skeletal structure.
As the ﬁrst step of device evaluation with simulation, we have developed the skin
deformation and interaction model [ 30]. The simulation consists in modeling skin
deformation of a human body and physical simulation coming from interaction with
the assistive device for transfer.
The skin deformation is computed by linear blend skinning (skinning in short) that
is integrated into a rigid-body articulated dynamics model. Here, the surface of the
human body and the device are both approximated by triangle meshes. The interactionforce applied to the human from the device is computed at the contact points of both
surfaces. The component of the device supporting the human is modeled as a rigid
body or mass-spring model. The contact force fis then transformed into joint torques
τby inverse mapping based on the skinning equation that relates joint angles with
skin vertices by:
τ=J
Tf (4)
where Jis the Jacobian representing the relationship between contact point and the
joint angle.
Toward a Human(oid) Motion Planner 243
Fig. 9 Simulations of behavior of a human supported by a robotic lift device. In the upper images,
the human cannot be lifted with a ﬂat support part. The successful lift with inclined support is shownin the lower images
Figure 9shows an example of simulation of a robotic lift device used to hold
the subject to be transferred between different places. When the supporting part
is horizontal, the body fell from the support. In contrast, the successful result wasgenerated with the inclined (7
◦) support. Although the human active motion is not
yet simulated, this result shows possibility of “human motion planning” evaluating
the supportive effect for design improvement of such devices.
4.2 Human-in-the-Loop Planning
As mentioned earlier, the motion planning technique, represented by sampling-basedmethods, has made tremendous progress as a powerful tool. Nevertheless, it is still
a challenging problem to plan a natural motion of human ﬁgures or humanoids
that require consideration of many mechanical constraints and dynamics stability, aswell as measured human motions to reproduce. Recently, a planning scheme called
“human-in-the-loop planning” is drawing attention in the planning ﬁeld. Especially
for narrow passage problems and motions involving contacts, the planning efﬁciency
is drastically improved by using an initial path planned by the human with some
interfaces with intuitive control [ 5,25].
We consider this observation also applies to humanoid motion planning for com-
plex tasks. In DARPA Robotics Challenge (DRC) [ 4], several manipulation tasks are
required such as door opening, turning valves, making a hole on the wall with tools,
244 E. Y oshida et al.
Fig. 10 HRP-2 humanoid robot of AIST-NEDO team executing the valve turning task, and its
teleoperation GUI on Choreonoid during DARPA Robotics Challenge
pushing a button and plugging a socket. The difﬁculty lies in the variance of tasks
whose parameters like the valve diameter or manipulation positions are changed
over time. The robot should perform measurements in the world to localize and to
recognize the target objects.
In this competition, the robots are basically teleoperated with certain autonomy in
such a way that the robot generates necessary reaching or manipulating motions byproviding the target object from a graphical user interface (GUI) based on Choreonoid
humanoid simulation framework [ 21] as shown in Fig. 10. However, some of the
teams encounter difﬁculties in generating coordinated whole-body motions due tokinematic singularities or constrained environments. This problem can be dealt with
by testing several end-effector orientations or giving many intermediate waypoints,
but this severely slows down task execution because of limited bandwidth to getinformation from the environment. In addition, even advanced motion planning or
optimization methods need long computation time to derive a solution path due to
the complexity of the problem.
The retargeting scheme presented in Sect. 3.1could be useful to solve the above
problems. This retargeting framework can backproject the retargeted robot motions
into human motions, which means that it can generate human motions as if the humanhad the physical or mechanical constraints of the robot. By integrating an immersive
tele-presence device with virtual reality and physical feedbacks like TELESAR [ 9],
the human operator can feel “embodiment” into robots in order to input the robotmotion by considering its constraints.
We may use these motions from human under robot constraints as the initial path of
motion planning, as well as motion primitives of the task that can be parameterized
to adapt to task variations. By taking advantage of human intuition as an initial
guess, the efﬁciency of motion planning could be much more improved than startingplanning from scratch. Figure 11illustrates this human-in-the-loop motion planning
combined with immersive interface.
Toward a Human(oid) Motion Planner 245
Fig. 11 A future framework of human-in-the-loop planning that allows human to generate motions
under robot constraints. The resultant motions can be used as motion primitives for adaptive task
execution
5 Summary
This paper discussed new perspectives in the motion planning related to humans
and humanoid robots. At the early research stage our primary interest was how to
generate humanoid motions that exploit many DOFs including arms and legs. Inten-
sive study on whole-body humanoid motion planning and optimization, including
multiple contacts, brought us to interest in human-like motion. Driven by projects
including evaluation of assistive devices using a humanoid robot as a subject, we
have developed a motion retargeting method based on simultaneous optimization for
human and humanoid motions.
Finally, some current work is introduced: “planning humans” and “human-in-
the-loop” planning. As an important factor of simulation-based assistive device
246 E. Y oshida et al.
evaluation, we have also developed a simulator that reproduces human behaviors
when interacting with those devices. Here, upcoming challenges include usage of
inverse optimal control to investigate what the humans optimize during their motion.Another challenge is application of motion retargeting to human-in-the-loop plan-
ning, inspired by DRC where complex humanoid motion planning should be done
efﬁciently. Together with an immersive interface, the human operator is expected toinput motions as if he/she had the robot constraints, which can be used the initial
path for complex motion planning and motion primitives for adaptive task execution.
Acknowledgements This research has been partially supported by Japan Society for the Pro-
motion of Science (JSPS) Grant-in-Aid for JSPS Fellows P13796 and for Scientiﬁc Research22300071/25820082, and also by METI Robotic Devices for Nursing Care Project.
References
1. Ayusawa, K., Y oshida, E.: Motion retargeting for humanoid robots based on simultaneous
morphing parameter identiﬁcation and motion optimization. IEEE Trans. Robot. 33(6), 1343–
1357 (2017). https://doi.org/10.1109/TRO.2017.2752711
2. Bouyarmane, K., Kheddar, A.: Multi-contact stances planning for multiple agents. In: Pro-
ceedings of the 2011 IEEE International Conference Robotics and Automation, pp. 5246–5253(2011)
3. Choset, H., Lynch, K., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L., Thrun, S.: Princi-
ples of Robot Motion: Theory, Algorithms, and Implementation. MIT Press, Cambridge (2006)
4. Defense Advanced Resaerch Projects Agency, “DARPA robotics challenge”. http://www.
theroboticschallenge.org/ (2015)
5. Denny, J., Sandstrm, R., Julian, N., Amato, N.M.: A region-based strategy for collaborative
roadmap construction. In: Akin, H.L., Amato, N.M., Isler, V ., van der Stappen, A.F. (eds.)Algorithmic Foundations of Robotics XI, Springer Tracts in Advanced Robotics, pp. 125–141.Springer, Berlin (2014)
6. Devices for Nursing Care Project, R. http://robotcare.jp/ (2013)
7. Escande, A., Kheddar, A.: Multi-contact acyclic motion planning and experiments on HRP-2
humanoid. In: Harada, K., Y oshida, E., Y okoi, K. (eds.) Motion Planning for Humanoid Robots,pp. 161–179. Springer, Berlin (2010)
8. Esteves, C., Arechavaleta, G., Pettré, J., Laumond, J.P .: Animation planning for virtual char-
acters cooperation. ACM Trans. Graph. 25(2), 319–339 (2006)
9. Fernando, C.L., Furukawa, M., Kurogi, T., Kamuro, S., Sato, K., Minamizawa, K., Tachi, S.:
Design of TELESAR v for transferring bodily consciousness in telexistence. In: Proceedingsof the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5112–5118 (2012)
10. Gleicher, M.: Retargetting motion to new characters. In: Proceedings of the SIGGRAPH ’98,
pp. 33–42 (1998)
11. Harada, K., Y oshida, E., Y okoi, K.: Motion Planning for Humanoid Robots. Springer, Berlin.
http://www.springer.com/engineering/robotics/book/978-1-84996-219-3 (2010)
12. Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Harada, K., Y okoi, K., Hirukawa, H.: Biped
walking pattern generation by using preview control of zero-moment point. In: Proceedings ofthe 2003 IEEE International Conference on Robotics and Automation, pp. 1620–1626 (2003)
13. Kuffner, J., Kagami, S., Nishiwaki, K., Inaba, M., Inoue, H.: Dynamically-stable motion plan-
ning for humanoid robots. Auton. Robot. 12(1), 105–118 (2002)
14. Latombe, J.C.: Robot motion planning. Kluwer Academic Publishers, Boston (1991)
Toward a Human(oid) Motion Planner 247
15. LaV alle, S.: Planning Algorithm. Cambridge University Press, Cambridge (2006)
16. Lengagne, S., V aillant, J., Y oshida, E., Kheddar, A.: Generation of whole-body optimal dynamic
multi-contact motions. Int. J. Robot. Res. 32(9–10), 1104–1119 (2013). https://doi.org/10.
1177/0278364913478990
17. Miura, K., Morisawa, M., Nakaoka, S., Harada, K., Kaneko, K., Kajita, S.: Robot Motion Remix
based on Motion Capture Data - Towards Human-like Locomotion of Humanoid Robots . In:Proceedings of the 2009 IEEE-RAS International Conference on Humanoid Robots, pp. 596–603 (2009)
18. Miura, K., Y oshida, E., Kobayashi, Y ., Endo, Y ., Kanehiro, F., Homma, K., Kajitani, I., Mat-
sumoto, Y ., Tanaka, T.: Humanoid robot as an evaluator of assistive devices. In: Proceedingsof the 2013 IEEE International Conference Robotics and Automation, pp. 671–677 (2013)
19. Mombaur, K., Laumond, J.P ., Y oshida, E.: An optimal control-based formulation to determine
natural locomotor paths for humanoid robots. Adv. Robot. 24(4), 515–535 (2010). https://doi.
org/10.1163/016918610X487090
20. Mombaur, K., Truong, A., Laumond, J.P .: From human to humanoid locomotion - an inverse
optimal control approach. Auton. Robot. 28, 369–383 (2010)
21. Nakaoka, S.: Choreonoid: extensible virtual robot environment built on an integrated gui frame-
work. In: Proceedings of the 2012 IEEE/SICE International Symposium on System Integration,
pp. 79–85 (2012)
22. Nakaoka, S., Nakazawa, A., Y okoi, K., Ikeuchi, K.: Leg motion primitives for a dancing
humanoid robot. In: Proceedings of the IEEE International Conference on Robotics andAutomation, pp. 610–615 (2004)
23. Siciliano, B., Slotine, J.J.E.: A general framework for managing multiple tasks in highly redun-
dant robotic systems. In: Proceedings of the IEEE International Conference on AdvancedRobotics, pp. 1211–1216 (1991)
24. Soueres, P ., Laumond, J.P .: Shortest paths synthesis for a car-like robot. IEEE Trans. Autom.
Control. 41(5), 672–688 (1996)
25. Tax, M., Flavign, D., Ferr, E.: Human interaction with motion planning algorithm. J. Intell.
Robot. Syst., Springer 67(3–4), 285–306 (2012)
26. Wächter, A., Biegler, L.T.: On the implementation of a primal-dual interior point ﬁlter line
search algorithm for large-scale nonlinear programming. Math. Program. 106, 22–57 (2006)
27. Y oshida, E., Esteves, C., Belousov, I., Laumond, J.P ., Sakaguchi, T., Y okoi, K.: Planning 3D
collision-free dynamic robotic motion through iterative reshaping. IEEE Trans. Robot. 24(5),
1186–1198 (2008). https://doi.org/10.1109/TRO.2008.2002312
28. Y oshida, E., Kanoun, O., Esteves, C., Laumond, J.P ., Y okoi, K.: Task-driven support poly-
gon reshaping for humanoids. In: Proceedings of 6th IEEE-RAS International Conference onHumanoid Robots, pp. 827–832 (2006)
29. Y oshida, E., Poirier, M., Laumond, J.P ., Kanoun, O., Lamiraux, F., Alami, R., Y okoi, K.:
Pivoting based manipulation by a humanoid robot. Auton. Robot. 28(1), 77–88. http://
www.springerlink.com/content/d47r262762502912/ (2010). https://doi.org/10.1007/s10514-
009-9143-x
30. Y oshiyasu, Y ., Ayusawa, K., Y oshida, E., Matsumoto, Y ., Endo, Y .: Forward dynamics sim-
ulation of human ﬁgures on assistive devices using geometric skin deformation model. In:
Proceedings of the 37th IEEE Annual International Conference Engineering in Medicine andBiology Society, pp. 2442–2445 (2015)
Human-Robot Interaction
Accelerating Motion Planning for
Learned Mobile Manipulation TasksUsing Task-Guided Gibbs Sampling
Chris Bowen and Ron Alterovitz
1 Introduction
For a mobile manipulator performing a task, the problems of navigation and
manipulation are inherently coupled. On the one hand, the manipulator may need tobe repositioned to navigate through a narrow passage (e.g., moving between furniture
in Fig. 1) while adhering to task constraints (e.g., levelness of the grasped pitcher
to avoid spilling). On the other, the robot may need to reposition itself to enable
the manipulator to accomplish a given task (e.g., pouring liquid from the pitcher
into a bowl). This coupling presents multiple challenges, mostly stemming from theassociated increase in dimensionality. While a traditional robot arm might have only
six degrees of freedom, a mobile manipulator like the Fetch robot [ 32] might have
eleven or more.
Sampling-based motion planners have previously been applied to the execution of
tasks based on a learned model. We introduce Task-Guided Gibbs Sampling (TGGS)
to accelerate such a motion planner in the context of mobile manipulation tasks byutilizing the same learned task model. TGGS enables the motion planner to more
quickly sample conﬁgurations that are likely to be important for task success without
relying on manual heuristics, thereby producing successful plans faster than samplingmethods that do not explicitly consider the task and environment together, like goal-
biasing or obstacle-biasing.
Our method depends on a model of the task learned from demonstrations per-
formed in varying environments. This same model, which is used by the motion
planner to generate plans that successfully perform the task, also informs the sam-
C. Bowen (B)·R. Alterovitz
University of North Carolina at Chapel Hill, Chapel Hill, NC 27599-3175, USAe-mail: cbbowen@cs.unc.edu
R. Alterovitz
e-mail: ron@cs.unc.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_23251
252 C. Bowen and R. Alterovitz
Fig. 1 a The Fetch robot’s task is to pour liquid from the grasped pitcher into the green bowl on the
table. bRoadmap of base motions biased towards motions most likely to be useful (yellow) while
avoiding the obstacle point cloud (white). cEnd-effector positions for sampled arm conﬁgurations
biased towards conﬁgurations most likely to be useful (blue). dEnd-effector positions for conﬁg-
urations in the hybrid roadmap (green) with the base (yellow) and end-effector (red) motion of theﬁnal plan
pling strategy. Speciﬁcally, the method we propose combines local optimization with
a Markov Chain Monte Carlo (MCMC) approach to effectively project the task space
distribution learned from the demonstrations into the robot’s conﬁguration space.This sampling strategy is applicable to both the bounded subspace of the manipula-
tor as well as the unbounded subspace of the mobile base. While the task model we
consider is based on prior work [ 6] and the Fetch robot is used in the experiments,
we believe TGGS could also be adapted to a number of other models and robots.
Like many low-cost mobile manipulators, the Fetch robot’s mobile base uses dif-
ferential drive, which presents the challenge of nonholonomy. Fast motion planningfor nonholonomic robots with 10 or more degrees of freedom remains computa-
tionally difﬁcult, especially when asymptotic optimality is desirable, as when soft
task constraints are present. Fortunately, the conﬁguration spaces of many mobilemanipulators, including the Fetch robot, have a large subspace that isholonomic:
that of the manipulator arm. TGGS leverages this property by partitioning the sam-
pling across holonomic and nonholonomic degrees of freedom, which acceleratessampling-based motion planners for learned mobile manipulation tasks while pre-
serving their asymptotic optimality guarantees.
We validate our new approach analytically and then empirically using the Fetch
robot to perform two household tasks in randomly-generated scenarios.
2 Related Work
Motion planning for mobile manipulation is challenging because of the high numberof degrees of freedom. V arious methods that lack completeness address this problem,
including potential ﬁelds [ 13,30] and local optimization [ 20]. However, complete-
ness guarantees are important in many applications involving complex environments
Accelerating Motion Planning for Learned Mobile Manipulation Tasks … 253
and higher-dimensional spaces. Probabilistically complete sampling-based motion
planners that build roadmaps or trees in the robot’s conﬁguration space are effective
in such spaces [ 1,25] and can incorporate a class of task constraints [ 11,28], even
at reactive rates using elastic roadmaps [ 34].
For some applications, we desire not just a feasible path, but also optimality with
respect to some task-speciﬁc cost function, either locally using a controller [ 31]o r
globally using a motion planner [ 6]. But in high-dimensional spaces, the convergence
of asymptotically optimal motion planners may be impractically slow. Even when
only completeness is required, biased sampling may improve performance [ 2], par-
ticularly for challenging problems with narrow passages [ 7,15,24,29]. Similarly,
cost-based task constraints [ 23] may induce narrow passages in cost space that make
planning more challenging, and biased sampling is again effective [ 4,5,12,17].
In this paper, we introduce a new sampling strategy that can be used for efﬁ-
ciently constructing motion planning roadmaps for learned mobile manipulation
tasks while retaining asymptotic optimality of the motion planner. We apply thisapproach to one task model [ 6] here, although it could be adapted to motion plan-
ning for tasks represented by other generative models that may be learned from
demonstrations [ 8,14].
Additionally, our approach relies on decomposing the conﬁguration space of the
robot. Related approaches have been considered in the contexts of hierarchical plan-ning [ 10,26] and hybrid planning [ 27], but without asymptotic optimality guarantees.
We also note that high-level methods have considered this problem in a more tradi-
tional task planning framework [ 9,19,33], but these approaches depend on methods
that can compute motion plans for low-level actions, which is precisely the domain
of our method.
3 Problem Deﬁnition
We consider a mobile manipulator that consists of a holonomic arm and a mobile base
(which may be holonomic or nonholonomic). Given a set of arm conﬁgurations Qarm
and a set of base conﬁgurations Qbase,l e t Farm⊆Qarmdenote those arm conﬁgura-
tions which are potentially free for some base conﬁguration (i.e., those that are not in
self-collision). Similarly, let Fbase⊆Qbasedenote those base conﬁgurations which
are potentially free for some arm conﬁguration (i.e., those for which the base itself
does not collide with the environment). Finally, let Q=Qarm×Qbasedenote the
combined conﬁguration space and F⊆Farm×Fbasedenote the free conﬁguration
space.
In addition to the above sets, we assume as input to the method an initial con-
ﬁguration q0∈Fand a task model. The task model depends on a feature function
fa(q)∈Y⊆Rbwhere ais an environment description that encodes the positions
of salient objects (e.g., the bowl in Fig. 1). We call Ythe feature space, and the
task model consists of a ﬁnite sequence of feature space multivariate Gaussian dis-
tributions {µ1,/Sigma1 1,µ2,/Sigma1 2,..., µT,/Sigma1 T}. One can think of these feature space dis-
254 C. Bowen and R. Alterovitz
tributions as deﬁning distributions over conﬁgurations when pulled back along f,
conditional on a given environment described by a. Task models of this form can be
estimated from expert demonstrations for a variety of useful tasks [ 6,8,14].
The output of the motion planner is a feasible conﬁguration space path πcompris-
ing a sequence of local plans which maximizes similarity to this task model. In this
paper, we use a speciﬁc deﬁnition of similarity [ 6] which we summarize in Sect. 4.2.
4 Motion Planning Using the Learned Model
The problem of planning motions for mobile manipulation tasks presents a number
of challenges but also a degree of structure. Notably, any mobile base that uses dif-
ferential drive (e.g. the Fetch robot) is nonholonomic and thus so is the robot as a
whole. This precludes a large class of motion planners [ 21], including probabilis-
tic roadmaps with linear edges. However, the subspace of conﬁguration space that
represents only the arm isholonomic.
In Sect. 4.1, we ﬁrst describe and analyze a hybrid roadmap that leverages this
inherent structure by using different roadmap constructions for different subspaces
of the problem as appropriate. Next, in Sect. 4.2, we apply a learned task model to this
hybrid roadmap. Then, in Sect. 4.3, we describe how the nature of this construction
lends itself to efﬁcient sampling of high-quality conﬁgurations in both roadmaps
using Gibbs sampling.
4.1 Mobile Manipulation Hybrid Roadmap
Mobile manipulators are generally characterized by low-dimensional base motion,
usually SE (2), combined with a high-dimensional manipulator, R5(S1)3in the case
Fig. 2 Hybrid roadmap
(green) formed by the
Cartesian product of an arm
PRM (blue) and a base RRG
(yellow) while avoiding
obstacles (red)
Arm PRM
Base RRG
Accelerating Motion Planning for Learned Mobile Manipulation Tasks … 255
of the Fetch robot. The combined conﬁguration space is often 11-dimensional or
greater. Additionally, we are not interested in merely feasible plans but ones which
also have low costs, ideally approaching the optimal solution in the limit.
Asymptotically-optimal variants of rapidly-exploring random graphs, e.g., RRG\*
[22], are applicable to such motion planning problems. However, in high-dimensional
spaces, these planners can exhibit very slow convergence to optimality. On the otherhand, less general methods like probabilistic roadmaps (e.g., PRM\*) may exhibit
faster convergence for multiple queries but often rely on additional structure, like
an optimal local planner. Furthermore, applying PRM\* to unbounded spaces, likeSE(2), necessitates additional heuristics because new conﬁgurations will be added
from the entire space unlike RRG\* which only extends to conﬁgurations near those
already explored.
We show that these planners can be combined to plan for mobile manipulation
tasks in such a way that their relative strengths are exploited in the subspaces where
they are most applicable. Speciﬁcally, we propose to build an RRG\* of pure basemotions and a separate PRM\* of pure manipulator motions. We then search in the
Cartesian product of the resulting graphs to ﬁnd asymptotically-optimal motion plans
(Fig. 2). Additionally, this approach allows for eager collision checking on the base
during roadmap construction but lazily checking for arm collisions during the shortest
path search.
Let C
base(ebase,qarm)denote the cost of traversing edge ebasein the base roadmap
with arm conﬁguration qarmand Carm(qbase,earm)denote the cost of traversing edge
earmin the arm roadmap at base conﬁguration qbase. Every edge in the Cartesian
product graph is composed of a vertex from one graph and an edge from the other.
This implies that the arm and base are never actuated simultaneously. Let C(e)be
either CbaseorCarmas applicable. We assume Cbaseand Carmare non-negative and
Lipschitz continuous. We extend this cost to a path πin the obvious way:
C(π)=/summationdisplay
e∈πC(e).
Algorithm 1 BuildHybridRoadmap (q0,nbase,narm)
Require: Initial conﬁguration q0, number of samples n
Gbase←BuildBaseRoadmap (Fbase,q0,nbase)
Garm←BuildArmRoadmap (Farm,q0,narm)
return Gbase×Garm// graph Cartesian product
Theorem 1 If both BuildBaseRoadmap and BuildArmRoadmap are
asymptotically-optimal with respect to C base and C arm for all qarm andqbase respec-
tively, then BuildHybridRoadmap (see Algorithm 1) is asymptotically-optimal with
respect to C with the restriction that only the base or arm are actuated at any given
time.
256 C. Bowen and R. Alterovitz
Proof For any ε> 0, consider a feasible path πsuch that C(π) < C∗+ε
2where C∗
is the inﬁmum of costs among feasible paths. Such a path must exist by deﬁnition.
Letπbasedenote the set of edges in πwhich correspond to base motions and πarm
those which correspond to arm motions. Observe that, by associativity, C(π)=
C(πbase)+C(πarm).
We next note that because the base and arm do not actuate simultaneously, πbase
can be divided up into no more than |π|sub-paths, each with qarmconstant in each
sub-path. Let Kbasedenote the Lipschitz constant of Cbase. By assumption, for all
δ> 0 there exists marmsuch that BuildArmRoadmap (Farm,q0,marm) includes a
reachable conﬁguration q/prime
i,armthat isε
8Kbase|π|-close to a given qi,armwith probability
1−δ
4|π|. Thus, with probability at least (1−δ
4|π|)|π|≥1−δ
4this holds for each such
qarm.
Now, consider one of these sub-paths πi,basewith shared qi,arm. Again by assump-
tion, there exists nbasesuch that with probability at least 1 −δ
4|π|there is a path π/prime
i,base
inBuildBaseRoadmap (Fbase,q0,nbase) with C(π/prime
i,base,qi,arm)< C(π i,base,qi,arm)+
ε
8|π|. By Lipschitz continuity of Cbase,w eh a v e C(π/prime
i,base,q/prime
i,arm)< C(π/prime
i,base,qi,arm)+
ε
8|π|. Together, these sub-paths form π/prime
basewith C(π/prime
base)< C(πbase)+ε
4with prob-
ability at least (1−δ
4)2≥1−δ
2.
The above argument applies similarly to πarm, with π/prime
arm,mbase, and narm. So, for
n=max(mbase,nbase,marm,narm), with probability at least (1−δ
2)2≥1−δthere
exists a path π/primeinBuildHybridRoadmap (q0,n,n)such that C(π/prime)=C(π/prime
base)+
C(π/prime
arm)< C(π)+ε
2, where dis an ambiguous word and cis disambiguated,
we generate the Word Sense Proﬁle, WSP (di)=w1,w2,... for each sense, di,o f
the word d. Each wjin the WSP is a word from one of the following sources in
WordNet:
1. All synonyms of di
2. All words (excluding stop words) in the gloss/deﬁnition for di
3. All direct hypernyms (parent nodes) and hyponyms (child nodes) of diin WordNet
4. All meronyms/holonyms (has part or part of) relations in WordNet
5. All words (excluding stop words) in the glosses of the direct hyponyms of di.
Once we generate the WSP for each sense, we compute a score for each WSP equal
to the sum of the semantic relatedness between the non-ambiguous word, c, and each
word in WSP (di). The relatedness measure is calculated using Explicit Semantic
Analysis (ESA) [ 6] based on word frequency count of Wikipedia entries. We choose
the sense of dithat corresponds with the maximal relatedness value, and add to the
BLN only the relations corresponding to the selected senses of each word to avoid
contextually irrelevant information from being added to the network. Additionally,
we exclude relations, , where dconsists of more than one word.
Since ConceptNet is not hand-coded, it contains a signiﬁcant amount of noisy or
erroneous relations, and excluding multi-word relations signiﬁcantly reduced thesize of the network without removing a large number of correct relations.
The ﬁnal step in BLN construction is to learn the conditional probability function
for each fragment in the network. To do so, we generate training evidence with alikelihood equal to a linear combination of the weights assigned to each relation
in ConceptNet and the Explicit Semantic Analysis relatedness measure between the
two concepts in the relation. This approach provides an initial estimate for real-worldprobabilities and enable inference results to be ranked according to their relative
likelihoods. Once we collect the evidence, the conditional probability functions can
be learned via maximum likelihood by counting the frequency of each child nodebeing true for each conﬁguration of the parent nodes.
An example resulting BLN is shown in Fig. 3. We chose the IsA,HasProperty ,
AtLocation , and UsedF or relations because together they support a wide range of
inference queries that can be used across many domain-independent applications.
Understanding of object categorization, properties and uses can enable a robot to
Fig. 3 An example of high-level knowledge representation using a BLN
Situated Bayesian Reasoning Framework for Robots Operating … 359
identify objects that can perform a certain function, or act as a substitute for a missing
item. Information about likely object locations can be used in locating objects or act
as a prior for semantic mapping. Similarly to the way in which humans use prior
knowledge when visiting a new location, we view the presented approach as a means
of providing a situated prior to the robot. In this work, we examine the performance
of the ﬁxed prior; in future work, we will explore how the resulting model can be
reﬁned over time to match a given environment.
4 Evaluation of Semantic Inference in Abstract Domains
The knowledge framework described so far has the ability to mine abstract semantic
information, but does not yet integrate data from situated robot interactions with
the environment. In this section, we brieﬂy evaluate the quality of the abstract data
mined, before discussing application and reﬁnement of this knowledge in a robotic
setting. We evaluate the performance of our approach on 13 abstract domains using
seed words obtained from two different sources:
•Ten segmented and labeled images of kitchen scenes obtained from the SUN
database [ 23] as an analog for object recognition output from a robot’s vision
system. Figure 4shows each of the images with the number of seed words from each
image. Example seed words obtained for image SUN10 include basket, cabinet,
ceiling, ceiling lamp, clock, coffee maker , cup, curtain, desk lamp, etc .
•Three task descriptions related to typical household chores, including cooking a
recipe (spaghetti), doing laundry, and cleaning the house. Each of the manipulable
objects in the three tasks was used as a seed word. For example, the recipe task
included objects such as spaghetti ,tomato sauce , and saucepan , doing laundry
included shirt ,laundry detergent , and washer , and cleaning included vacuum ,
mop, and soap . The three tasks contained 19, 15, and 11 seed words, respectively.
The size of the BLN generated from each of these data sets varied from 69 (cleaning
house) to 195 (SUN10) predicates, with an average size of 129.
For each set of seed words, we tested the word sense disambiguation accuracy
by computing the percentage of correctly disambiguated seed words. The mean
SUN1 (20) SUN2 (38) SUN3 (17)
...SUN10 (35) SUN10 labels
Fig. 4 Four of the ten images of kitchens taken from the SUN database, and an example of the
accompanying object label information. Number of seed words generated per image is listed in
parenthesis
360 S. Chernova et al.
Table 1 Forward and reverse (in parenthesis) inference accuracy (%) over the seed words from
each source when compared to the gold standard
Source IsA AtLocation HasProperty UsedFor
Recipe 97.6 86.8 (71.4) 82.0 (84.4) 88.1 (88.0)
Laundry 98.3 77.3 (82.1) 88.9 (87.5) 89.5 (90.1)
Cleaning 98.6 72.7 (67.1) 94.7 (90.9) 79.2 (80.0)
SUN1 95.0 81.1 (93.9) 78.0 (85.3) 91.3 (93.5)
SUN2 94.1 84.4 (85.7) 90.4 (91.6) 93.7 (96.3)
SUN3 94.5 76.5 (85.9) 72.4 (81.5) 80.3 (86.9)
SUN4 95.1 78.7 (88.9) 65.2 (78.8) 87.7 (91.7)
SUN5 97.4 83.8 (89.3) 85.0 (87.3) 90.8 (93.7)
SUN6 96.6 73.2 (80.4) 87.2 (90.6) 88.8 (93.6)
SUN7 93.5 74.0 (81.0) 83.6 (90.8) 85.5 (92.1)
SUN8 94.2 77.3 (78.3) 84.9 (85.8) 87.8 (90.2)
SUN9 96.2 78.6 (78.6) 88.8 (89.8) 87.9 (91.4)
SUN10 97.0 75.7 (81.3) 88.2 (90.0) 91.4 (93.3)
Mean 96.0 78.5 (81.8) 83.8 (87.1) 87.9 (90.8)
disambiguation accuracy for the three task descriptions was 78 .5%±3.5, while
the SUN images averaged at 83 .9%±6.8. Overall, the disambiguation algorithm
performs better with larger numbers of seed words because the algorithm ﬁnds the
set of senses that maximizes the similarity between the seed words. This is reﬂected
in the three task description experiments, which had at most 19 seed words, comparedto as many as 38 seed words in SUN images. Thus, in a real-world setting, word sense
disambiguation will improve as the robot observes more objects.
Next, we evaluate the forward and reverse inference accuracy of the model result-
ing from each independent set of seed words by comparing the BLN output to a gold
standard.
1For forward inference, we perform inference over the network where the
evidence variable is IsA(Object i,x)with a value set to true . We then query each
of the relations, IsA(Object i,si),AtLocation (Object i,si),HasProperty(Obj ect i,si),
and UsedF or(Obj ect i,si). These queries return the probability of x’s categorization,
likely location, properties, and affordances; a query result is considered true if the
associated probability is greater than 0 .5, and false otherwise. Reverse inference
uses IsAas a query variable (instead of as an evidence variable), enabling the robot
to query for a list of objects that can be found at a particular location, have certain
properties, or can be used for a desired purpose. Table 1shows the results across all
thirteen data sets, with reverse inference results listed in parenthesis.
1The gold standard was generated by hand based on commonsense information (e.g., Used-
F or(Knife,Cut) istrue ), and then validated by comparing to crowd-generated labels from ﬁve
crowd workers (0 .8 agreement threshold). A comparison between hand-labeled and crowd-labeled
data resulted in accuracy values within 1% for all tested instances.
Situated Bayesian Reasoning Framework for Robots Operating … 361
Inference results for both the task descriptions and the SUN images showed high
accuracies for the IsArelation, with an average of 96%. This is due to the fact that
WordNet, the source of the IsArelation, is hand coded and thus contains highly
accurate data. The accuracies for the AtLocation ,HasProperty , and UsedF or rela-
tions average at 78.5%, 83.8% and 87.9%, respectively for forward inference, and
81.5, 87.1 and 90.8% for reverse inference. All three query types show an increasein performance as the number of seed words increases, in part due to the improve-
ments in the underlying disambiguation rate, and in part because of the richer set of
connections present with more data.
Critically, this promising result shows that as few as 20 words obtained from a
single image of the robot’s environment already allows the robot to mine sufﬁcient
semantic information to predict many commonsense facts. Examples include identi-fying a table, dishwasher, and cabinet as likely locations for a bowl; predicting that
an apple has the properties red, round, and healthy; knowing that a knife can be used
to pare, scratch, and separate; and inferring that a drawer can be used to contain,organize, and hide. In the next section we discuss how this general knowledge can
be adapted to a speciﬁc robot environment.
5 Leveraging Semantic Knowledge in Situated Robotic
System
To validate our Bayesian reasoning framework on a physical robot, we utilize the
robot architecture shown in Fig. 5. The architecture consists of three main com-
ponents: Seed Word Generation, Abstract Knowledge Base, and Local Knowledge
Base. Below, we present the technical details of each component, including improve-
ments made to the BLN generation process to improve location data processing. Inthe next section, we report results from two trials with a mobile robot.
Object Recognition : The pipeline begins with object recognition, assigning
object class labels (e.g., cup, bowl, etc.) to objects detected in the robot’s environment.The generated class names become seed words that are used to extract information
for the BLN. For object detection, we use the open source real-time object detection
system YOLOv2 [ 14]. YOLOv2 uses a convolutional neural network and computes
the location and classiﬁcation of each object in an image in a single pass by dividing
the image into cells, calculating an objectness score and then object classiﬁcationprobabilities over the individual cells, and then using anchor boxes to predict the
object bounding boxes. For our robot experiments, we trained YOLOv2 on a subset
of COCO [ 11] object classes, consisting of the following 31 objects commonly found
in a home environment: apple, banana, book, bottle, bowl, broccoli, cake, carrot,
chair , clock, couch, cup, donut, fork, glass, knife, laptop, microwave, orange, oven,
phone, pizza, plant, refrigerator , sandwich, sink, spoon, table, toaster , tv, vase . Each
time the system recognizes the object, the object label, bounding box of the object,
362 S. Chernova et al.
Fig. 5 Robot architecture overview
and raw rectangle segment of the object is sent to the Local Knowledge Base, and
the object labels are passed to the Abstract Knowledge Base.
Abstract Knowledge Base (AKB) : The Abstract Knowledge Base consists of the
pipeline described in Sect. 3, with two modiﬁcations. First, we use the SUN image
database as an additional source of AtLocation data, providing us with a richer set
of room-level locations than available in ConceptNet. Locations of an object are
imported based on probability of occurrence within the database. To maintain scope,
we add only locations related to the high level environment the robot is operating in(i.e., related to house , eliminating park ,grocery store , etc). Second, to ensure correct
probability distribution of hierarchical location information (e.g., counter-kitchen-
apartment ), we encode transitivity properties using the ﬁrst-order logic rules of the
BLN.
Local Knowledge Base (LKB) : We represent the robot’s local environment
through a collection of object instances, forming a memory of encountered items,
and their locations and properties. For each object class o,w es t o r e iinstances of
that object within the LKB, where each instance corresponds to a unique object ofthat class. The LKB is implemented using PyTables and HDF5; each object class
ois stored as a database, with a table generated for each object instance. We dis-
tinguish instances using multi-object tracking that identiﬁes instances using localfeatures. For each instance, we store the object label, previously seen locations (pose
and semantic label), image region corresponding to the bounding box from object
recognition, visual information (RGB-D values), and properties of the instance (e.g,color, material), if known. The resulting representation provides a scalable memory
system that allows for efﬁcient retrieval of all of its recent memories of instances.
Situated Bayesian Reasoning Framework for Robots Operating … 363
Metric map Topological map
Fig. 6 Topological and metric maps of the robot’s environment
To provide a semantic location for an object, we utilize a hybrid map [ 2], which
links a topological map, consisting of a tree graph representing human domain knowl-
edge, with a metric map of spatial locations in the environment. Figure 6shows the
topological and metric maps used in this work. The links between the topological
map and metric map are expressed directly in the topological map nodes; association
of each node with a volume in the metric map. This map structure enables the robot
to obtain a semantic label for any 3D point that is hierarchical (e.g., object ois in a
drawer in the kitchen in the apartment ).
6 Evaluation in Robot Experiments
We evaluate our situated reasoning framework using two real-world case studies in
a small-scale apartment setting (Fig. 6) using a mobile robot equipped with a laser
rangeﬁnder and RGB-D camera.
Case study 1 : In the ﬁrst case study, we demonstrate the use of abstract semantic
knowledge to enable the robot to locate objects never before seen in the environment.
In this experiment, the robot begins by randomly exploring the environment shown in
Fig.7, in which food items are visible on the table. The robot obtains the following 11
seed words from object recognition: apple, banana, book, bottle, block, cup, donut,
fork, orange, spoon, vase , using which it constructs a BLN consisting of 97 nodes
encoding the abstract knowledge of the environment. Next, we request the robot to
ﬁnd a bowl within the environment. Note that we provide the robot with the semantic
map, shown in Fig. 6, but no other information beyond the seed words listed, which
do not include the bowl . To complete this task, the robot must ﬁrst add the word
bowl to the BLN, using the words already present in the network to perform word
sense disambiguation. The result deﬁnes bowl as “a round vessel that is open at
the top; used chieﬂy for holding food or liquids” as opposed to “a large structure
for open-air sports or entertainments” or “a small round container that is open at
the top for holding tobacco”. Given this deﬁnition, appropriate relations are mined
from WordNet and ConceptNet following previously described processes. Once the
BLN has been updated, the robot performs a reverse inference query to locate the
364 S. Chernova et al.
Fig. 7 Robot performing case study 1. ascanning environment, blooking for bowl in refrigerator,
clooking for and ﬁnding bowl in cabinet
Table 2 Ranked list of likely
bowl locationsLocation Prob.
Refrigerator 1.000
Cabinet 1.000
Cupboard 1.000
Kitchen 0.933
Dishwasher 0.786
Closet 0.773
Food 0.724
Sink 0.723
Kitchen\_table 0.722
Diningroom 0.273
Bathroom 0.225
bowl , which results in the list of possible locations, ranked by probability value,
shown in Table 2. Using the semantic map provided, the robot is able to ground the
bolded words above to known locations in its environment. Using the resulting sorted
list of locations ( refrigerator , cabinet, cupboard, sink, table ), the robot explores the
environment until it successfully locates the bowl in the cabinet . This case study
validates an important use case for the semantic reasoning framework in which
situated and abstract knowledge is combined to enable the robot to reason about apreviously unknown object.
Case study 2 : In the second case study, we examine the relative tradeoffs of
using abstract versus local knowledge in locating a known object given an increasing
number of observations. We utilize a bowl and a potted plant as representatives of
classes of objects that have a high and low location distribution, respectively. Forthis experiment, we allowed the robot to explore the environment and observe the
location of the two items in 20 independent runs (Fig. 8). During each run, the plant
and bowl were placed in a random location sampled from the following distributions:
Situated Bayesian Reasoning Framework for Robots Operating … 365
Fig. 8 The robot recording
location of the bowl
Fig. 9 Average accuracy
(AKB vs. LKB) across 5000permutations to predict thelocations of potted plant and
bowl
•plant : ofﬁce desk: 50%, coffee table: 25%, counter: 25%
•bowl : dining table: 20%, kitchen table: 20%, counter: 12%, coffee table: 12%,
ofﬁce desk: 12%, drawer1: 12%, drawer2: 12%.
Leveraging this data, we conducted 5000 simulated trials, splitting the dataset of the
above observations into 75–25% randomized training-test set per trial. For each trial,
we present our system with an increasing number of object observations and use theAKB and LKB to predict the location of the bowl and plant , comparing the result to
the hold-out test set. We consider the trial to be a success if the true object location
of the object is within the top 3 ranked locations returned by the algorithm.
Figure 9presents the results of the experiment; solid lines represent inference
using only local observations (instances stored in the LKB) and dashed lines representinference performed using the Bayesian Logic Network based on abstract knowledge.
The two knowledge bases share no information to illustrate the relative strengths and
weaknesses of each. For both objects, the BLN provides the robot with the abilityto make an “intelligent guess” when no other information is available, allowing the
robot to predict the correct location of both plant and bowl with 60% accuracy.
The robot’s local observations, by comparison, become highly valuable as the robot
366 S. Chernova et al.
gains information about the environment, surpassing BLN performance after 2 and
11 observations for the plant and bowl , respectively.
As this experiment demonstrates, the incorporation of semantic knowledge into
the robot architecture provides a way to supplement or guide local observations. Just
as a person entering an unfamiliar house can leverage prior semantic knowledge to
guess likely item locations (e.g., search kitchen drawers for a fork), our robot is ableto infer likely object locations. Over time, a person would stop relying on abstract
information and instead utilize local knowledge, going directly to the needed drawer
without searching. This process is equivalent to enabling the robot to increasinglyrely on the LKB. Our future work will explore techniques for merging abstract and
local data sources, as well as ways in which the robot is able to leverage abstract
knowledge to guide exploration, semantic mapping, and task execution in unfamiliarenvironments.
7 Human Reﬁnement Using Actively Situated Knowledge
(ASK)
The ﬁnal component of our work, is to consider reﬁnement of the robot’s abstract
knowledge in the BLN from another situated knowledge source—a co-present
human. Our prior case studies demonstrated the effectiveness of the BLN in predict-ing location information. However, certain other types of object information, such as
material properties and affordances, are more difﬁcult for the robot to acquire inde-
pendently. Additionally, the BLN contains a certain level of noise, due to both wordsense disambiguation errors and noise within ConceptNet. We, therefore, introduce
Actively Situating Knowledge (ASK), a technique which situates the general knowl-
edge in the BLN with the help of a human user,
2which we apply to the HasProperty
and UsedF or relations in the BLN.
Note that the BLN contains too many property edges to make it practical to verify
each one with the human user. Thus, our goal is to intelligently query a subset ofproperty relations for validation while still obtaining veriﬁcation of all the relations
within the BLN. For this, we ﬁrst modify the BLN to add inter-property edges. For
all pairs of properties in the BLN, we add an edge if a relation exists between them in
ConceptNet. For example, since an IsArelation exists between Metal and Aluminum
in ConceptNet, and a HasProperty relation exists between Glass and Recyclable,
inter-property edges are added between these pairs of relations, respectively.
Next, given N, the number of known objects, and P, the number of known prop-
erties, we deﬁne:
2This approach could also be combined with crowdsourcing, although we relied on a co-present
expert for all experiments described here.
Situated Bayesian Reasoning Framework for Robots Operating … 367
•Tmate rial :aPx1 table listing all material properties present in our BLN (i.e., nodes
that hold a relation with material in the ConceptNet, e.g. metal, aluminum, plastic ,
etc.).
•TO
assoc:aNxP table storing the ConceptNet association values between object O
and every property belonging to O, where the the association value is a measure
between 0 to 1 of how related two words are.3
•TO
inte rp rop :a PxP table storing the ConceptNet inter-property association values
between any two properties, PO, associated with object O.
Given the above tables, we select properties to verify with a human expert based on
the inter-property association value. For each object, we alternate between querying
the expert about property p∈POwith the highest association value and the lowest
association value in TO
assoc. We alternate queries between maximum and minimum
inter-property association value in order to maximize information gain by askingabout properties that are least likely to occur together.
If a property is conﬁrmed as true by the user, and exists in T
mate rial , then all
other material properties belonging to that object are assumed to be false and are
not queried (we make the simplifying assumption that each object only has a single
material property). Given this information, we can also infer that the predecessors of
that property are true for O(e.g., if aluminum is true, then wood is false but metal is
true). For the successors of O, we assume their hasProperty relations are true (e.g., if
metal true, then opaque true), but that IsArelations must still be veriﬁed (e.g., if metal
true, still need to verify aluminum ). If a node in this isAset is veriﬁed to be true ,t h e
r e s ta r ea s s u m e dt ob e false . However, if an attribute is veriﬁed to be false , all its IsA
successors are assumed to be false but its HasProperty successors are still queried
(e.g., if metal isfalse , then tinis assumed to be false but recyclable is still
queried). We repeat this process until all the properties are veriﬁed as true/false
and create an updated, expert-veriﬁed BLN ( vBLN ) using the updated true veriﬁed
property relations.
We evaluate ASK by comparing the accuracy of the resulting vBLN to a ground
truth BLN, gBLN, generated by an expert. The gBLN differs from vBLN in that everyedge was expert-veriﬁed instead of just the subset explored by ASK. To evaluate ASK
performance we calculate the dissimilarity index:
I
dis=Uncommon edges between ground truth, gBLN and vBLN
Total number of unique edges in gBLN and vBLN
To evaluate ASK, we trained a BLN using the 31 seed words from the COCO
dataset, resulting in a network consisting of 195 property edges. Using ASK, we
performed 84 clariﬁcations, resulting in 50 pruned property edges in the vBLN.While this is a large number of clariﬁcations, during a deployment such queries
could occur over a length of time (multiple days) as the robot spends time learning
about its environment. The algorithm obtained a ﬁnal dissimilarity score I
dis=0.11,
indicating that the vBLN contained only 6 extraneous edges in comparison to the
3We ignore properties with value <0.07.
368 S. Chernova et al.
ground truth. In future work, this result can be further improved through selective
ﬁltering of data imported from ConceptNet, or through the use of more reliable data
sources. The above experiment demonstrates that even with highly noisy data sources,we are able to intelligently reﬁne the robot’s semantic knowledge. Furthermore, the
inference techniques used in ASK can be leveraged in combination with the robot’s
own sensing in addition to human input, such as if material information were tobecome available.
8 Conclusion
In this paper, we explore the ability of a robot to use limited local information, some-
times as little as the set of object labels recognized in a single image, to automatically
mine semantic information about its environment. We embed the mined informationwithin a statistical relational model, and demonstrate its use on both abstract tasks
and as part of a robot architecture, as well as introduce a technique for interactively
reﬁning the semantic knowledge with the help of a human expert. Our results show
that, just as for humans, semantic knowledge can provide valuable guidance in the
absence of extensive familiarity with the operating environment. In future, we willexplore how local and abstract information can be merged over time to enable the
knowledge framework to further adapt to the robot’s current environment.
Acknowledgements This work is supported in part by NSF IIS 1564080 and ONR N000141612835.
References
1. Boteanu, A., Chernova, S.: Solving and explaining analogy questions using semantic networks.
In: AAAI Conference on Artiﬁcial Intelligence, pp. 1–8 (2015)
2. Buschka, P., Safﬁotti, A.: Some notes on the use of hybrid maps for mobile robots. In: Inter-
national Conference on Intelligent Autonomous Systems, pp. 547–556 (2004)
3. Chen, J., Liu, J.: Combining conceptnet andwordnet forword sense disambiguation. In: Inter-
national Joint Conference on Natural Language Processing, pp. 686–694 (2011)
4. Deng, J., et al.: Imagenet: a large-scale hierarchical image database. In: Computer Vision and
Pattern Recognition, pp. 248–255. IEEE (2009)
5. Fung, R.M., Chang, K-C.: Weighing and integrating evidence for stochastic simulation in
Bayesian networks. In: Conference on Uncertainty in Artiﬁcial Intelligence, pp. 209–220(1990)
6. Gabrilovich, E., Markovitch, S.: Computing semantic relatedness using wikipedia-based
explicit semantic analysis. In: International Joint Conference on Artiﬁcial Intelligence, pp.1606–1611 (2007)
7. Jain, D., Kirchlechner, B., Beetz, M.: Extending markov logic to model probability distributions
in relational domains. In: KI 2007: Advances in Artiﬁcial Intelligence, pp. 129–143. Springer(2007)
8. Jain, D., Waldherr, S., Beetz, M.: Bayesian logic networks. Technical report, Technische Uni-
versität München, pp. 1–21 (2009)
Situated Bayesian Reasoning Framework for Robots Operating … 369
9. Kucera, H., Francis, W.N.: Computational Analysis of Present-Day American English. Brown
University Press, Providence (1967)
10. Lenat, D.B.: CYC: a large-scale investment in knowledge infrastructure. Commun. ACM
38(11), 33–38 (1995)
11. Lin, T-Y ., et al. Microsoft COCO: common objects in context. In: CoRR arXiv:1405.0312
(2014)
12. Matuszek, C., et al.: An introduction to the syntax and content of Cyc. In: AAAI Spring Sympo-
sium: Formalizing and Compiling Background Knowledge and Its Applications to KnowledgeRepresentation and Question Answering, pp. 44–49 (2006)
13. Miller, George A.: WordNet: a lexical database for English. Commun. ACM 38(11), 39–41
(1995)
14. Redmon, J., et al.: You only look once: uniﬁed, real-time object detection. In: Conference on
Computer Vision and Pattern Recognition, pp. 779–788. IEEE (2016)
15. Richardson, M., Domingos, P.: Markov logic networks. Mach. Learn. 62(1–2), 107–136 (2006)
16. Saxena, A., et al.: Robobrain: large-scale knowledge engine for robots. arXiv:1412.0691 (2014)
17. Seco, N., et al.: An intrinsic information content metric for semantic similarity in wordnet
(2004)
18. Speer, R., Havasi, C.: Representing general relational knowledge in conceptnet 5. In: Interna-
tional Conference on Language Resources and Evaluation. Istanbul (2012)
19. Stoica, E., Hearst, M.A.: Nearly-automated metadata hierarchy creation. In: North American
Chapter of the Association for Computational Linguistics: Human Language Technologies,pp. 117–120 (2004)
20. Tenorth, M., Beetz, M.: KnowRob-knowledge processing for autonomous personal robots. In:
IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4261–4266. IEEE(2009)
21. Tsatsaronis, G., Varlamis, I., Vazirgiannis, M.: Word sense disambiguation with semantic net-
works". In: Sojka, P., et al. (eds.) Text, Speech, and Dialogue, pp. 219–226. Springer, Berlin(2008)
22. Waibel, Markus, et al.: Roboearth. IEEE Robot. Autom. Mag. 18(2), 69–82 (2011)
23. Xiao, J., et al.: SUN database: large-scale scene recognition from abbey to zoo. In: IEEE
Conference on Computer Vision and Pattern Recognition, pp. 3485–3492. IEEE (2010)
24. Zhu, Y ., Fathi, A., Fei-Fei, L.: Reasoning about object affordances in a knowledge base repre-
sentation. In: European Conference on Computer Vision, pp. 408–424. Springer (2014)
A Multiview Approach to Learning
Articulated Motion Models
Andrea F . Daniele, Thomas M. Howard and Matthew R. Walter
1 Introduction
As robots move off factory ﬂoors and into our homes and workplaces, they face the
challenge of interacting with the articulated objects frequently found in environmentsbuilt by and for humans (e.g., drawers, ovens, refrigerators, and faucets). Typically,
this interaction is predeﬁned in the form of a manipulation policy that must be
(manually) speciﬁed for each object that the robot is expected to interact with. Such
an approach may be reasonable for robots that interact with a small number of objects,
but human environments contain a large number of diverse objects. In an effort toimprove efﬁciency and generalizability, recent work employs visual demonstrations
to learn representations that describe the motion of these parts in the form of kinematic
models that express the rotational, prismatic, and rigid relationships between objectparts [ 6,18,23,39,45]. These structured object-relative models, which constrain
the object’s motion manifold, are suitable for trajectory controllers [ 19,45], provide
a common representation amenable to transfer between objects [ 46], and allow for
manipulation policies that are more efﬁcient and deliberate than reactive policies
(Fig. 1). However, such visual cues may be too time-consuming to provide or may
not be readily available, such as when a user is remotely commanding a robot overa bandwidth-limited channel (e.g., for disaster relief). Further, reliance solely on
vision makes these methods sensitive to common errors in data association, object
segmentation, and tracking (e.g., tracking features over time and associating them
A. F. Daniele ( B)·M. R. Walter
Toyota Technological Institute at Chicago, Chicago, IL, USAe-mail: afdaniele@ttic.edu
M. R. Walter
e-mail: mwalter@ttic.edu
T. M. Howard
University of Rochester, Rochester, NY, USAe-mail: thoward@ece.rochester.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_30371
372 A. F. Daniele et al.
Fig. 1 A robot autonomously opens a drawer by recalling a learned kinematic model of the object,
which is visualized in the inset
with the correct object part) that occur as a result of clutter, occlusions, and a dearth
of visual features. Consequently, most existing systems require scenes to be free of
distractors and that object parts be labeled with ﬁducial markers.
Natural language utterances offer a ﬂexible, bandwidth-efﬁcient medium that
humans can readily use to convey knowledge of an object’s operation [ 46]. When
paired with visual observations, free-form descriptions of an articulated motion alsoprovide a source of information that is complementary to visual input. In similar fash-
ion to existing work on learning manipulation policies from Internet videos [ 54], the
ability to learn kinematic models from narrated demonstrations, either given in situ or
via instructional videos [ 29,43,55], provides an efﬁcient, intuitive means for people
to convey information to robots, as the authors have shown in the context of maplearning [ 49]. Natural language descriptions that accompany these demonstrations
can be used to overcome some of the limitations of using visual-only observations,
e.g., by providing cues regarding the number of parts that comprise the object orthe motion type (e.g., rotational) between a pair of parts. However, fusing visual
and linguistic observations is challenging. For one, language and vision provide dis-
parate observations of motion and exhibit different statistical properties. Secondly,the two are often prone to uncertainty. RGB-D observations are subject to occlusions
(e.g., as the human interacts with the object) and the objects often lack texture (e.g.,
the drawers in Fig. 2), which makes feature detection challenging and feature corre-
spondences subject to noise. Meanwhile, free-form descriptions exhibit variability
and are prone to errors (e.g., confusing “left” and “right”). Further, language also
tends to be ambiguous with respect to the corresponding referents (i.e., object partsand their motion) in the scene. For example “open” can suggest both rotational and
prismatic motion, and inferring the correct grounding requires reasoning over the
full description (e.g., “open the door” vs. “open the drawers”).
A Multiview Approach to Learning Articulated Motion Models 373
Input : Narrated video
“A man opens and closes the cabinet drawers”
Output : Kinematic model & predicted motion
Fig. 2 Our framework learns the kinematic model that governs the motion of articulated objects
(lower-left) from narrated RGB-D videos. The method can then use this learned model to subse-
quently predict the motion of an object’s parts (lower-right)
In order to overcome these challenges, we present a multimodal learning frame-
work that estimates the kinematic structure and parameters of complex multi-part
objects using both vision and language input. We address the challenges associated
with language understanding through a probabilistic language model that captures
the compositional and hierarchical structure of natural language descriptions. Addi-
tionally, our method maintains a distribution over a sparse, structured model of an
object’s kinematics, which provides a common representation with which to fuse
disparate linguistic and visual observations.
Our effort is inspired by the recent attention that has been paid to the joint use of
vision and language as complementary signals for multiview learning in robotics [ 15,
25,40,46,49,56] and scene understanding [ 1,8,13,21,27,41,44,48]. We
leverage the joint advantages of these two modalities in order to estimate the structure
and parameters that deﬁne kinematic models of complex, multi-part objects such as
doors, desks, chairs, and appliances from narrated examples such as those conveyed
in instructional videos or through demonstrations [ 3] in the form of a “guided tour
of manipulation” (Fig. 2), which provides an efﬁcient and ﬂexible means for humans
to share information with robots.
Our multimodal learning framework ﬁrst extracts noisy observations of the object
parts and their motion separately from the vision- and language-based observations.
It then fuses these observations to learn a probabilistic model over the kinematic
structure and model parameters that best explain the motion observed in the vision
374 A. F. Daniele et al.
and language streams. Integral to this process is an appropriate means of representing
the ambiguous nature of observations gleaned from natural language descriptions.
We treat language understanding as a symbol grounding problem and employ a prob-abilistic language model [ 17] that captures the uncertainty in the mapping between
words in the description and their corresponding referents in the scene, namely the
object parts and their relative motion. We fuse these language-based observationswith those extracted from vision to estimate a joint distribution over the structure
and parameters that deﬁne the kinematics of each object.
The contributions of this work include a multimodal approach to learning kine-
matic models from vision and language signals and the integration of a probabilistic
language model that grounds natural language descriptions into a structured repre-
sentation of an object’s articulation manifold. By jointly reasoning over vision andlanguage cues, our framework is able to formulate a complete object model with-
out the need for an expressed environment model. Our method requires no prior
knowledge about the objects and operates in situ, without the need for environmentpreparation (i.e., ﬁducials). Evaluations on a dataset of video-text pairs demonstrate
improvements over the previous state-of-the-art, which only uses visual information.
2 Related Work
Recent work considers the problem of learning articulated models based upon visual
observations of demonstrated motion. Several methods formulate this problem as
bundle adjustment, using structure-from-motion methods to ﬁrst segment an articu-
lated object into its compositional parts and to then estimate the parameters of therotational and prismatic degrees-of-freedom that describe inter-part motion [ 18,53].
These methods are prone to erroneous estimates of the pose of the object’s parts and of
the inter-part models as a result of outliers in visual feature matching. Alternatively,Katz et al. [ 22] propose an active learning framework that allows a robot to interact
with articulated objects to induce motion. This method operates in a deterministic
manner, ﬁrst assuming that each part-to-part motion is prismatic. Only when theresidual error exceeds a threshold does it consider the alternative rotational model.
Further, they estimate the models based upon interactive observations acquired in a
structured environment free of clutter, with the object occupying a signiﬁcant portionof the RGB-D sensor’s ﬁeld-of-view. Katz et al. [ 23] improve upon the complexity
of this method while preserving the accuracy of the inferred models. This method
is prone to over-ﬁtting to the observed motion and may result in overly complex
models to match the observations. Hausman et al. [ 14] similarly enable a robot to
interact with the object and describe a probabilistic model that integrates observa-tions of ﬁducials with manipulator feedback. Meanwhile, Sturm et al. [ 45] propose
a probabilistic approach that simultaneously reasons over the likelihood of observa-
tions while accounting for the learned model complexity. Their method requires thatthe number of parts that compose the object be known in advance and that ﬁducials
be placed on each part to enable the visual observation of motion. More recently,
A Multiview Approach to Learning Articulated Motion Models 375
Pillai et al. [ 39] propose an extension to this work that uses novel vision-based motion
segmentation and tracking that enables model learning in situ, without prior knowl-
edge of the number of parts or the need for ﬁducial markers. Our approach builds uponthis method with the addition of natural language descriptions of motion as an addi-
tional observation mode in a multimodal learning framework. Meanwhile, Schmidt
et al. [ 42] propose a framework that addresses the similar problem of tracking artic-
ulated models that uses an articulated variation of the signed distance function to
identify the model that best ﬁts observed depth data. Related, Martín-Martín and
Brock [ 30] consider the problem of perceiving articulated objects and demonstrate
how an estimate of kinematic structure facilitates tracking and manipulation. The
authors improve upon this work by integrating shape reconstruction, pose estima-
tion, and kinematic estimation [ 31].
Related, there has been renewed attention to enabling robots to interpret natural
language instructions that command navigation [ 4,7,24,32,34] and manipula-
tion [ 17,35,38,47] through symbol grounding and semantic parsing methods.
While most existing grounded language acquisition methods abstract away percep-
tion by assuming a known symbolic world model, other work jointly reasons over
language and sensing [ 9,12,16,33] for instruction following. Meanwhile, multi-
modal learning methods have been proposed that use language and vision to formulate
spatial-semantic maps of a robot’s environment [ 15,40,49,56] and to learn object
manipulation policies [ 46]. Particularly relevant to our work, Sung et al. [ 46] learn
a neural embedding of text, vision, and motion trajectories to transfer manipulation
plans between similarly operating objects. Kollar et al. [ 25] extend their framework
that jointly learns a semantic parsing of language and vision [ 27] to enable robots
to learn object and spatial relation classiﬁers from textual descriptions paired with
images. We similarly use language and vision in a joint learning framework, but forthe challenging task of learning object articulation in terms of kinematic motion mod-
els. Beyond robotics, there is a long history of work that exploits the complementary
nature of vision and language in the context of multiview learning, dating back tothe seminal SHRDLU program [ 51]. This includes work for such tasks as image and
video caption synthesis [ 8,21,37,44,48,52], large-vocabulary object retrieval [ 13],
visual coreference resolution [ 26,41], and visual question-answering [ 2]. Particu-
larly related to our work are methods that use instructional videos paired with lan-
guage (text or speech) for weakly supervised learning [ 29,55], extracting procedural
knowledge [ 43], and identifying manipulating actions [ 1,46].
3 Multimodal Learning Framework
Given an RGB-D video paired with the corresponding natural language description
(alternatively, an instruction or caption) of an articulated object’s motion, our goal is
to infer the structure and parameters of the object’s kinematic model. Adopting theformulation proposed by [ 45], we represent this model as a graph, where each vertex
denotes a different part of the object (or the stationary background) and edges denote
376 A. F. Daniele et al.
Fig. 3 Our multimodal articulation learning framework ﬁrst identiﬁes clusters of visual features
that correspond to individual object parts. It then uses these feature trajectories to estimate the
model parameters, assuming an initial estimate of the kinematic type associated with each edge in
the graph. The method grounds natural language descriptions of the motion to their corresponding
referents in the kinematic model and parameters through a probabilistic language model, visualized
as a factor graph. The vision and language observations are then fused to learn a distribution over
the object’s kinematic model
the existence of constrained motion (e.g., a linkage) between two parts (Fig. 2). More
formally, we estimate a kinematic graph G =(VG,EG)that consists of vertices VG
for each object part and edges EG⊂VG×VGbetween parts whose relative motion
is kinematically constrained. Associated with each edge (ij)∈EGis its kinematic
type Mij∈{rotational ,prismatic ,rigid}as well as the corresponding parameters θij,
such as the axis of rotation and the range of motion (see Fig. 3, lower-right). We
take as input vision Dvand language Dlobservations of the type and parameters
of the edges in the graph. Our method then uses this vision-language observation
pair Dz={ Dv,Dl}to infer the maximum a posteriori kinematic structure and model
parameters that constitute the kinematic graph:
ˆG=arg max
Gp(G|Dz) (1a)
=arg max
Gp({Mij,θij|(ij)∈EG}|Dz) (1b)
=arg max
G/productdisplay
(ij)∈EGp(Mij,θij|Dz) (1c)
Due to the complexity of joint inference, we adopt the procedure described by
[45] and use a two-step inference procedure that alternates between model param-
eter ﬁtting and model structure selection steps (Fig. 3). In the ﬁrst step, we assume
a particular kinematic model type between each object iand j(e.g., prismatic), and
then estimate the kinematic parameters based on the vision data (relative transforma-
tion between the two objects) and the assumed model type Mij. We make one such
assumption for each possible model type for each object pair.
In the model selection step, we then use the natural language description to infer
the kinematic graph structure that best expresses the observation. While our previous
work [ 39] provides visual observations of motion without the need for ﬁducials, it
A Multiview Approach to Learning Articulated Motion Models 377
relies upon feature tracking and segmentation that can fail when the object parts
lack texture (e.g., metal door handles) or when the scene is cluttered. Our system
incorporates language as an additional, complementary observation of the motion,in order to improve the robustness and accuracy of model selection.
3.1 Vision-Guided Model Fitting
We parse a given RGB-D video of the objects’ motion (either performed by a human
or the robot via teleoperation) to arrive at a visual observation of the trajectory of
each object part [ 39]. The method (Fig. 3, “Construct Trajectories”) ﬁrst identiﬁes a
set of 3D feature trajectories {(f1
1,f2
1,..., ft
1) ,...( f1
n,f2
n,..., ft
n)}that correspond
to different elements in the scene, including the object parts, background, and clutter.
Importantly, both the number of elements and the assignment of points in the RGB-Dvideo to these elements are assumed to be unknown a priori. Further, many of the
objects that we encounter lack the amount of texture typically required of SIFT [ 28]
and KLT [ 5] features. Consequently, we utilize dense trajectories [ 50] through a strat-
egy that involves dense sampling (via the Shi-Tomasi criterion) for feature extraction
followed by dense optical ﬂow for propagation. We prune trajectories after a ﬁxedlength and subsequently sample new features in order to reduce feature drift.
Having extracted a set of features trajectories, the next step is then to group features
that correspond to the same scene element via motion segmentation (Fig. 3, “Motion
Segmentation”). For this purpose, we evaluate the relative displacement between
pairs of feature trajectories along with the angle between their normals. We model
the relative displacement and angle as Gaussian in order to account for measurementnoise. We then employ density-based clustering [ 10] to identify rigidly associated
feature trajectories. These clusters {C
1,C2,..., Cm}denote the parsing of the scene
into its requisite elements, namely the inferred object parts and background.
Next, we estimate the 6-DOF pose xt
iof each cluster at each point in time according
to the set of features Zt
iassigned to each cluster Ciat time t(Fig. 3, “Pose Estima-
tion”). We treat this as a pose graph estimation problem, whereby we consider therelative transformation Δ
t−1,t
i between successive time steps for each cluster based
on the known correspondences between features Zt−1
iand Zt
i.1We optimize the pose
graph using iSAM [ 20], which models the relative transformations as observations
(constraints) in a factor graph with nodes that denote cluster poses.
The resulting 6-DOF pose trajectories constitute the visual observation of the
motion Dv. Our framework uses these trajectories to estimate the parameters of a
candidate kinematic model during the model ﬁtting step. Speciﬁcally, we ﬁnd the
kinematic parameters that best explain the visual data given the assumed model
1We employ RANSAC [ 11] to improve robustness to erroneous corresponences.
378 A. F. Daniele et al.
ˆθij=arg max
θijp(Dv|ˆMij,θij), (2)
where Dv=(Δ1
ij,...,Δtij),∀(ij)∈EGis the sequence of observed relative transfor-
mations between the poses of two object parts iand j, andˆMijis the current estimate
of their model type. We perform this optimization over the joint kinematic structure
deﬁned by the edges in the graph [ 45].
3.2 Language-Guided Model Selection
Methods that solely rely on visual input are sensitive to the effects of scene clutter
and the lack of texture, which can result in erroneous estimates for the structure
and parameters of the kinematic model [ 39]. An alternative is to exploit audial
information, provided in the form of utterances provided by the operator, to helpguide the process for inferring the relationships between objects in the environment.
Speciﬁcally, we consider a natural language description D
lthat describes the motion
observed in the video. Given this description, we infer the maximum a posteriori
set of affordances or relationships between pairwise objects in the utterance. Note
that we do not assume that valid captions provide an unambiguous description of allaffordances, but rather consider a distribution over the language observation, which
provides robustness to noisy, incomplete, or incorrect descriptions.
Following the notation from [ 38], we formulate this problem as one of inferring
a distribution of symbols ( Γ) representing objects ( Γ
O), relationships ( ΓR), and
affordances ( ΓA) in the absence of an environment model for each utterance. Object
groundings are deﬁned by an object type oifrom a space of object types O, relation-
ship groundings are deﬁned by a relationship type rkfrom a space of relationship
types R. Affordance groundings are deﬁned by a pair of object types oiand oj, and
relationship type rk:
ΓO={γoi,oi∈O} (3a)
ΓR={γrk,rk∈R} (3b)
ΓA={γoi,oj,rk,oi,oj∈O,rk∈R} (3c)
Examples of object types include “chair”, “desk”, and “door” which represent
semantic classes of random variables inferred by visual perception. Examples of
relationship types include “prismatic” and “revolute” that represent translational androtational motion. The set of all groundings is deﬁned as the union of these symbols:
Γ={Γ
O∪ΓD∪ΓA} (4)
Extracting the most probable set of groundings from language is challenging
due to the diversity inherent in free-form language and the complex relationships
A Multiview Approach to Learning Articulated Motion Models 379
λ0φ01φ02φ0n
γ01γ02...γ0n
am a nλ1φ11φ12φ1n
γ11γ12...γ1n
andλ2φ21φ22φ2n
γ21γ22...γ2n
opensλ3φ31φ32φ3n
γ31γ32...γ3n
closesλ4φ41φ42φ4n
γ41γ42...γ4n
the cabinet drawersλ5φ51φ52φ5n
γ51γ52...γ5n
Fig. 4 The DCG for the utterance “a man opens and closes the cabinet drawers” constructed from
the parse tree illustrated in Fig. 3. This model enumerates all possible groundings for each phrase
and performs inference by searching over unknown correspondences. The expressed groundings
(groundings for factors with True -valued correspondence variables) of factors connected to λ0are
used as language-based observations that are fused with the visual observation. In this example, the
model should infer a “prismatic” relationship between objects of semantic classes “cabinet” and
“drawer”
between the articulation of different objects. For example, the verb “open” can be
used to describe a person’s interaction with both a drawer and a door, but the motion
described in the former case is prismatic with a cabinet, while it is rotational with a
wall in the latter. We address these challenges by adapting the Distributed Correspon-
dence Graph (DCG) [ 17] to the problem of affordance inference which formulates
a probabilistic graphical model according to the parse structure of the sentence that
is searched for the most likely binary correspondence variable φi,j∈{ True,False }
between linguistic elements in the command λi, groundings γi,j∈Γ, and expressed
groundings of child phrases Γci∈Γ. The DCG encodes the factors fi,jin the graph
using log-linear models whose weights are learned from a corpus of annotated exam-
ples. We then perform inference over this model in a space of correspondence vari-
ables to arrive at a distribution over the kinematic model structure and parameters.
Note that since inference is conducted without an expressed environment model, the
symbols express inferred relationships between semantic classes of pairwise objects.
These are interleaved with the model constructed from visual perception to form a
probabilistic model of the environment. An example of the structure of the DCG for
the utterance “a man opens and closes the cabinet drawers” is illustrated in Fig. 4.
3.3 Combining Vision and Language Observations
The ﬁnal step in our framework selects the kinematic graph structure ˆM=
{ˆMij,∀(ij)∈EG}that best explains the vision and language observations Dz=
380 A. F. Daniele et al.
{Dv,Dl}from the space of all possible kinematic graphs. We do so by maximiz-
ing the conditional posterior over the model type associated with each edge in the
graph (ij)∈EG:
ˆMij=arg max
Mijp(Mij|Dz) (5a)
=arg max
Mij/integraldisplay
p(Mij,θij|Dz)dθij (5b)
Evaluating this likelihood is computationally prohibitive, so we use the Bayesian
Information Criterion (BIC) score as an approximation
BIC(Mij)=− 2l o g p(Dz|Mij,ˆθij)+klog n, (6)
where ˆθijis the maximum likelihood parameter estimate (Eq. 2),kis the number
of parameters of the current model and nis the number of vision and language
observations. We choose the model with the lowest BIC score
ˆMij=arg min
MijBIC(Mij) (7)
as that which speciﬁes the kinematics of the object.
While our previous method [ 39] only considers visual measurements, our new
framework performs this optimization over the joint space of vision and languageobservations. Consequently, the BIC score becomes
BIC(M
ij)=− 2/parenleftBig
log p(Dv|Mij,ˆθij)+log p(Dl|Mij,ˆθij)/parenrightBig
+klog n, (8)
where we have made the assumption that the language and vision observations are
conditionally independent given the model and parameter estimates. We formulate
the conditional likelihood of the linguistic observation according to the grounding
likelihood P(Φ=True|γ1,...,γn,Λ) from the DCG language model. The ground-
ing variables γidenote affordances that express different kinematic structures that
encode the articulation of the object, namely the relationship between its individual
parts (Eq. 3). For each candidate model, we use the likelihood of the corresponding
groundings under the learned DCG language model to compute the BIC score for
the corresponding affordance. We then estimate the overall kinematic structure by
solving for the minimum spanning tree of the graph, where we deﬁne the cost of eachedge as cost
ij=− log p(Mij,θij|Dz). Such a spanning tree constitutes the kinematic
graph that best describes the vision and language observations.
A Multiview Approach to Learning Articulated Motion Models 381
4 Results
We evaluate our framework using a dataset of 78 RGB-D videos in which a user
manipulates a variety of common household and ofﬁce objects (e.g., a microwave,refrigerator, and ﬁling cabinet). Each video is accompanied with 5 textual descrip-
tions provided by different human subjects using a web-based crowd-sourcing plat-
form. We split the dataset into separate training and test sets consisting of 22 and 56videos, respectively. AprilTags [ 36] were placed on each of the object parts in the test
set to determine ground-truth motion. We train our language grounding model on a
corpus of 50 video descriptions relative to the training set composed of 28 uniquesymbols composed of different object and/or relation types. Our language ground-
ing model requires that every word in a given training sentence is aligned with the
corresponding symbol. Such alignment is not required at test time.
Of the 56 test videos, 25 involve single-part objects and 31 involve multi-part
objects. The single-part object videos are used to demonstrate that the addition oflanguage observations can only improve the accuracy of the learned kinematic mod-
els. The extent of these improvements on single-part objects is limited by the rela-
tive ease of inference of single degree-of-freedom motion. In the case of multi-partobjects, the larger space of candidate kinematic graphs makes vision-only inference
challenging, as feature tracking errors may result in erroneous estimates of the graph
structure.
4.1 Evaluation Metrics
We estimate the ground-truth kinematic models by performing MAP inference basedupon the motion trajectories observed using AprilTags. We denote the resulting kine-
matic graph as G
∗. The kinematic type and parameters for each object part pair are
denoted as M∗
ijandθ∗
ij, respectively. Let ˆG,ˆMij,ˆθijbe the estimated kinematic graph,
kinematic type, and parameters for each object pair from the RGB-D video, respec-
tively.
The ﬁrst metric that we consider evaluates whether the vision component estimates
the correct number of parts. We determine the ground-truth number of parts as the
number of AprilTags observed in each video, which we denote as N∗. We indicate the
number of parts (motion clusters) identiﬁed by the visual pipeline as Nv. We report the
average success rate when using only visual observations as Sv=1
K/summationtextK
k=11(Nk
v=
Nk∗), where Kis the number of videos for each object type.
Next, we consider two metrics that assess the ability of each method to estimate a
graph with the same kinematic model as the ground truth G∗. The ﬁrst metric requires
that the two graphs have the same structure, i.e., ˆMij=M∗
ij,∀(ij)∈EˆG=EG∗.T h i s
equivalence requires that vision-only inference yields the correct number of object
parts and that the model selection framework selects the correct kinematic edge type
for each pair of object parts. We report this “hard” success rate Shin terms of the
382 A. F. Daniele et al.
fraction of demonstrations for which the model estimate agrees with the ground-truth.
Note that this is bounded from above by fraction for which the vision component
estimates the correct number of parts. The second “soft” success rate (denoted by Ss)
employs a relaxed requirement whereby we only consider the inter-part relationships
identiﬁed from vision, i.e., ˆMij=M∗
ij,∀(ij)∈EˆG⊂EG∗. In this way, we consider
scenarios for which the visual system detects fewer parts than are in the ground-truthmodel. In our experiments, we found that ˆGis a sub-graph of G
∗, so we only require
that the model type of the edges in this sub-graph agree between both graphs. The
metric reports the fraction of total demonstrations for which the estimated kinematicgraph is a correct sub-graph of the ground-truth kinematic graph.
Once we have the same kinematic models for both ˆGand G
∗, we can compare
the kinematic parameters ˆθijto the ground-truth values θ∗
ijfor each inter-part model
ˆMij. Note that for the soft metric, we only compare kinematic parameters for edges
in the sub-graph, i.e., ∀(ij)∈EˆG⊂EG∗. We deﬁne the parameter estimation error
for a particular part pair as the angle between the two kinematic parameter axes
eij=arccosˆθij·θ∗
ij
/bardblˆθij/bardbl/bardblθ∗
ij/bardbl, (9)
where we use the directional and rotational axes for prismatic and rotational degrees-
of-freedom, respectively. We measure the overall parameter estimation error eparam
for an object as the average parameter estimation error over each edge in the object’s
kinematic graph. We report this error further averaged over the number of demonstra-tions. We do not report the estimation error for the object classes Drawer (Single-Part)
and Door (Multi-Part) because the demonstrations for those classes do not contain
the ﬁducial markers needed to compute the ground-truth.
4.2 Results and Analysis
Table 1provides a summary that compares the performance of our multimodal learn-
ing method against that of the vision-only baseline [ 39], which the authors have
previously compared favorably to alternative state-of-the-art formulations [ 23]. We
limit our comparison to methods that similarly do not assume that the number of
parts is known a priori and that do not require the use of ﬁducial markers. The table
indicates the number of demonstrations K, the ground-truth number of parts for each
object N∗, a list of the number of parts identiﬁed using visual trajectory clustering
for each demonstration Nv, and the fraction of videos for which the correct number
of parts was identiﬁed Sv. The number in parenthesis under Nvindicates the number
of demonstrations for which a speciﬁc number of parts Nvwas identiﬁed. We then
present the hard Shand soft Ssmodel selection rates for our method as well as for the
baseline. The denominators listed under Shand Ssindicate the number of parts in each
demonstration. In the case of a single-part object, this matches the number of demon-
A Multiview Approach to Learning Articulated Motion Models 383
Ta b l e 1 Overall performance of our framework on video-description pairs
Vision-only Our framework
Object K N∗Nv Sv Sh Ss Sh Ss eparam
Single-
partDoor 9 1 1(9) 9/9 5/9 5/9 9/9 9/9 1.86◦
Chair 5 1 1(4),
3(1)4/5 1/5 2/5 4/5 5/5 3.34◦
Refrigerator 5 1 1(5) 5/5 5/5 5/5 5/5 5/5 5.74◦
Microwave 4 1 1(3),
2(1)3/4 3/4 4/4 3/4 4/4 2.02◦
Drawer 2 1 1(2) 2/2 0/2 0/2 2/2 2/2 –
Multi-
partChair 4 2 1(2),2(2) 2/4 1/4 5/8 2/4 6/8 3.05◦
Monitor 7 2 1(7) 0/7 0/7 6/14 0/7 7/14 7.27◦
Bicycle 7 3 1(1),
2(4),3(2)2/7 0/7 13/21 0/7 13/21 11.33◦
Drawer 11 2 1(6),
2(4),3(1)4/11 3/11 10/22 4/11 15/22 0.11◦
Door 2 2 2(2) 2/2 0/2 2/4 2/2 4/4 –
strations Kfor that object. In the case of a multi-part object, while the denominator
for Shmatches the number of demonstrations K, the denominator for Ssindicates
the total number of parts (i.e., K∗N∗). Our method bests the vision-only baseline
in estimating the full kinematic graph for ﬁve of the eight object classes, matching
its performance on the remaining three objects. Speciﬁcally, our framework yieldsaccurate estimates of the full kinematic graphs for thirteen more demonstrations
than the vision-only baseline, nine more for single-part objects and four more for
multi-part objects, corresponding to a 23% absolute improvement. Similarly, we areable to estimate a valid sub-graph of the ground-truth kinematic graph for eighteen
more demonstrations than the vision-only baseline (eleven for single-part and seven
for multi-part objects), corresponding to a 19% absolute improvement. One notable
object on which both methods have difﬁculty is the bicycle for which the trajectory
clustering method was unable to identify the presence of the third part (the wheel)due to the sparsity of visual features. Consequently, neither method estimated the full
kinematic graph for any video. Similarly, clustering failed to identify the three parts
comprising the monitor in all videos, however our framework exploits language toestimate an accurate sub-graph for one more video.
We then evaluate the accuracy of the parameters estimated by our method by
reporting the parameter estimation error for each object, averaged over the set ofvideos. Note that it is difﬁcult to compare against the error of the vision-only baseline
since it does not yield accurate kinematic graphs for several of the videos. When
384 A. F. Daniele et al.
the kinematic graph estimates agree, however, the parameter estimation errors are
identical for the two methods, since they both estimate the parameters from the visual
data (Eq. 2).
5 Conclusion and Future Work
We have described a method that uses a joint combination of vision- and language-
based observations to learn accurate probabilistic models that deﬁne the structure and
parameters of articulated objects. Our framework treats linguistic descriptions of a
demonstrated motion as a complementary observation of the structure of kinematiclinkages. We evaluate our framework on a series of RGB-D video-description pairs
involving the manipulation of common household and ofﬁce objects. The results
demonstrate that exploiting language as a form of weak supervision improves theaccuracy of the inferred model structure and parameters. While this evaluation con-
siders videos paired with free-form descriptions, the method does not rely on any
assumptions that preclude its application to robotic systems. Future work includes
incorporating semantic segmentation as a means of assigning perceived labels to
inferred clusters, using the description to mitigate noise in the visual recognition.Additionally, kinematic models provide a common representation that is suited to
generalization across different object instances. We are extending our model to pre-
dict kinematic models of novel (i.e., unseen) articulated objects, using natural lan-guage as a means of knowledge transfer.
Acknowledgements This work was supported in part by the National Science Foundation under
grants IIS-1638072 and IIS-1637813, and by the Robotics Consortium of the U.S. Army Research
Laboratory under the Collaborative Technology Alliance Program Cooperative Agreement W911
NF-10-2-0016.
References
1. Alayrac, J.B., Sivic, J., Laptev, I., Lacoste-Julien, S.: Joint discovery of object states and
manipulation actions (2017). arXiv:170202738
2. Antol, S., Agrawal, A., Lu, J., Mitchell, M., Batra, D., Zitnick, C.L., Parikh, D.: VQA: visual
question answering (2015). arXiv:150500468
3. Argall, B.D., Chernova, S., V eloso, M., Browning, B.: A survey of robot learning from demon-
stration. Robot. Auton. Syst. (2009)
4. Artzi, Y ., Zettlemoyer, L.: Weakly supervised learning of semantic parsers for mapping instruc-
tions to actions. Trans. Assoc. Comput. Linguist. (2013)
5. Bouguet, J.Y .: Pyramidal implementation of the afﬁne Lucas-Kanade feature tracker description
of the algorithm. Intel Corp. (2001)
6. Byravan, A., Fox, D.: SE3-Nets: learning rigid body motion using deep neural networks. In:
Proceedings of ICRA (2017)
7. Chen, D.L., Mooney, R.J.: Learning to interpret natural language navigation instructions from
observations. In: Proceedings of AAAI (2011)
A Multiview Approach to Learning Articulated Motion Models 385
8. Donahue, J., Hendricks, L.A., Guadarrama, S., Rohrbach, M., V enugopalan, S., Saenko, K.,
Darrell, T.: Long-term recurrent convolutional networks for visual recognition and description(2014). arXiv:14114389
9. Duvallet, F., Walter, M.R., Howard, T., Hemachandra, S., Oh, J., Teller, S., Roy, N., Stentz,
A.: Inferring maps and behaviors from natural language instructions. In: Proceedings of ISER(2014)
10. Ester, M., Kriegel, H.P ., Sander, J., Xu, X.: A density-based algorithm for discovering clusters
in large spatial databases with noise. In: Proceedings of KDD (1996)
11. Fischler, M.A., Bolles, R.C.: Random sample consensus: a paradigm for model ﬁtting with
applications to image analysis and automated cartography. Commun. ACM (1981)
12. Guadarrama, S., Riano, L., Golland, D., Gohring, D., Jia, Y ., Klein, D., Abbeel, P ., Darrell, T.:
Grounding spatial relations for human-robot interaction. In: Proceedings of IROS (2013)
13. Guadarrama, S., Rodner, E., Saenko, K., Zhang, N., Farrell, R., Donahue, J., Darrell, T.: Open-
vocabulary object retrieval. In: Proceedings of RSS (2014)
14. Hausman, K., Niekum, S., Ostenoski, S., Sukhatme, G.S.: Active articulation model estimation
through interactive perception. In: Proceedings of ICRA (2015)
15. Hemachandra, S., Walter, M.R., Tellex, S., Teller, S.: Learning spatially-semantic representa-
tions from natural language descriptions and scene classiﬁcations. In: Proceedings of ICRA(2014)
16. Hemachandra, S., Duvallet, F., Howard, T.M., Roy, N., Stentz, A., Walter, M.R.: Learning
models for following natural language directions in unknown environments. In: Proceedingsof ICRA (2015)
17. Howard, T.M., Tellex, S., Roy, N.: A natural language planner interface for mobile manipulators.
In: Proceedings of ICRA (2014)
18. Huang, X., Walker, I., Birchﬁeld, S.: Occlusion-aware reconstruction and manipulation of 3D
articulated objects. In: Proceedings of ICRA (2012)
19. Jain, A., Kemp, C.C.: Pulling open doors and drawers: coordinating an omni-directional base
and a compliant arm with equilibrium point control. In: Proceedings of ICRA (2010)
20. Kaess, M., Ranganathan, A., Dellaert, F.: iSAM: incremental smoothing and mapping. Trans.
Robot. (2008)
21. Karpathy, A., Fei-Fei, L.: Deep visual-semantic alignments for generating image descriptions.
In: Proceedings of CVPR (2015)
22. Katz, D., Orthey, A., Brock, O.: Interactive perception of articulated objects. In: Proceedings
of ISER (2010)
23. Katz, D., Kazemi, M., Andrew Bagnell, J., Stentz, A.: Interactive segmentation, tracking, and
kinematic modeling of unknown 3D articulated objects. In: Proceedings of ICRA (2013)
24. Kollar, T., Tellex, S., Roy, D., Roy, N.: Toward understanding natural language directions. In:
Proceedings of HRI (2010)
25. Kollar, T., Krishnamurthy. J., Strimel. G.: Toward interactive grounded language acquisition.
In: Proceedings of RSS (2013)
26. Kong, C., Lin, D., Bansal, M., Urtasun, R., Fidler, S.: What are you talking about? text-to-image
coreference. In: Proceedings of CVPR (2014)
27. Krishnamurthy, J., Kollar, T.: Jointly learning to parse and perceive: connecting natural lan-
guage to the physical world. Trans. Assoc. Comput. Linguist. (2013)
28. Lowe, D.G.: Distinctive image features from scale-invariant keypoints. Int. J. Comput. Vis.
(2004)
29. Malmaud, J., Huang, J., Rathod, V ., Johnston, N., Rabinovich, A., Murphy, K.: What’s cookin’?
Interpreting cooking videos using text, speech and vision (2015). arXiv:150301558
30. Martín-Martín, R., Brock, O.: Online interactive perception of articulated objects with multi-
level recursive estimation based on task-speciﬁc priors. In: Proceedings of IROS (2014)
31. Martín-Martín, R., Höfer, S., Brock, O.: An integrated approach to visual perception of artic-
ulated objects. In: Proceedings of ICRA (2016)
32. Matuszek, C., Fox, D., Koscher, K.: Following directions using statistical machine translation.
In: Proceedings of HRI (2010)
386 A. F. Daniele et al.
33. Matuszek, C., Herbst, E., Zettlemoyer, L., Fox, D.: Learning to parse natural language com-
mands to a robot control system. In: Proceedings of ISER (2012)
34. Mei, H., Bansal, M., Walter, M.R.: Listen, attend, and walk: neural mapping of navigational
instructions to action sequences. In: Proceedings of AAAI (2016)
35. Misra, D.K., Sung, J., Lee, K., Saxena, A.: Tell me Dave: Context-sensitive grounding of natural
language to manipulation instructions. Int. J. Robot. Res. (2016)
36. Olson, E.: AprilTag: a robust and ﬂexible visual ﬁducial system. In: Proceedings of ICRA
(2011)
37. Ordonez, V ., Kulkarni, G., Berg, T.L.: Im2Text: describing images using 1 million captioned
photographs. In: Proceedings of NIPS (2011)
38. Paul, R., Arkin, J., Roy, N., Howard, T.M.: Efﬁcient grounding of abstract spatial concepts for
natural language interaction with robot manipulators. In: Proceedings of RSS (2016)
39. Pillai, S., Walter. M.R., Teller. S.: Learning articulated motions from visual demonstration. In:
Proceedings of RSS (2014)
40. Pronobis, A., Jensfelt, P .: Large-scale semantic mapping and reasoning with heterogeneous
modalities. In: Proceedings of ICRA (2012)
41. Ramanathan, V ., Joulin, A., Liang, P ., Fei-Fei, L.: Linking people in videos with their names
using coreference resolution. In: Proceedings of ECCV (2014)
42. Schmidt, T., Newcombe, R., Fox, D.: DART: dense articulated real-time tracking. In: Proceed-
ings of RSS (2014)
43. Sener, O., Zamir, A.R., Savarese, S., Saxena, A.: Unsupervised semantic parsing of video
collections. In: Proceedings of ICCV (2015)
44. Srivastava, N., Mansimov, E., Salakhutdinov, R.: Unsupervised learning of video representa-
tions using LSTMs. In: Proceedings of ICML (2015)
45. Sturm, J., Stachniss. C., Burgard, W.: A probabilistic framework for learning kinematic models
of articulated objects. J. Artif. Intell. Res. (2011)
46. Sung, J., Jin, S.H., Saxena, A.: Robobarista: object part-based transfer of manipulation trajec-
tories from crowd-sourcing in 3D pointclouds. In: Proceedings of ISRR (2015)
47. Tellex, S., Kollar, T., Dickerson, S., Walter, M.R., Banerjee, A.G., Teller, S., Roy, N.: Under-
standing natural language commands for robotic navigation and mobile manipulation. In: Pro-ceedings of AAAI (2011)
48. Vinyals, O., Toshev, A., Bengio, S., Erhan, D.: Show and tell: a neural image caption generator.
In: Proceedings of CVPR (2015)
49. Walter, M.R., Hemachandra, S., Homberg, B., Tellex, S., Teller, S.: Learning semantic maps
from natural language descriptions. In: Proceedings of RSS (2013)
50. Wang, H., Klaser, A., Schmid, C., Liu, C.L.: Action recognition by dense trajectories. In:
Proceedings of CVPR (2011)
51. Winograd, T.: Understanding natural language. Cogn. Psychol. (1972)52. Xu, K., Ba, J., Kiros, R., Cho, K., Courville, A., Salakhutdinov, R., Zemel, R., Bengio, Y .:
Show, attend and tell: neural image caption generation with visual attention. In: Proceedingsof ICML (2015)
53. Yan, J., Pollefeys, M.: A general framework for motion segmentation: independent, articulated,
rigid, non-rigid, degenerate and non-degenerate. In: Proceedings ECCV (2006)
54. Yang, Y ., Li, Y ., Fermüller, C., Aloimonos, Y .: Robot learning manipulation action plans by
“watching” unconstrained videos from the world wide web. In: Proceedings of AAAI (2015)
55. Y u, S.I., Jiang, L., Hauptmann, A.: Instructional videos for unsupervised harvesting and learning
of action examples. In: Proceedings of International Conference on Multimedia (2014)
56. Zender, H., Martínez Mozos, O., Jensfelt, P ., Kruijff, G., Burgard, W.: Conceptual spatial
representations for indoor mobile robots. Robot. Auton. Syst. (2008)
Reachability and Differential Based
Heuristics for Solving Markov DecisionProcesses
Shoubhik Debnath, Lantao Liu and Gaurav Sukhatme
1 Introduction
Decision-making in uncertain environments is a basic problem in the area
of artiﬁcial intelligence [ 18, 19], and Markov decision processes (MDPs) have
become very popular for modeling non-deterministic planning problems with full
observability [17 ,22]. Speciﬁcally, an MDP assumes discrete states and discrete
actions, and can be viewed as stochastic automata where an agent’s actions haveuncertain effects. Such uncertain action outcomes induce stochastic transitions
between states. The expected value of a chosen action is a function of the transi-tions it induces. On executing the action, the agent receives a reward and also causes
a change in the state of the environment. The objective of the agent is to perform
actions in order to maximize the cumulative future reward over a period of time. Inpractice, the V alue Iteration (VI) is probably the most famous and most widely used
method for solving the MDPs [ 5,13].
We are interested in the exact solution methods and our objective is to further
accelerate the convergence of the MDPs’ solving mechanism. Different from manypopular state-space search based heuristics which essentially exploit MDP’s prob-
abilistic transition models and search for a path leading from the start state to thegoal state based on some search tree/graph structures, our method evaluates the
global feature of the entire state space and quantiﬁes the level of reachability of each
S. Debnath ( B)·G. Sukhatme
NVIDIA Corporation, Santa Clara, CA 95051, USA
e-mail: sdebnath@nvidia.com
G. Sukhatmee-mail: gaurav@usc.edu
L. LiuIntelligent Systems Engineering Department, Indiana University,Bloomington, IN 47408, USAe-mail: lantao@iu.edu
© Springer Nature Switzerland AG 2020N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_31387
388 S. Debnath et al.
states using ﬁrst passage time information. This enables us to assess the relevance or
importance of states, built on which other completely new heuristics such as backup
differentials can be designed.
In greater detail, in this paper we introduce the notion of reachability landscape
which characterizes how hard it is for the agent to transit from any state to the given
goal state. To compute the reachability landscape, we use the Mean First PassageTime (MFPT) which can be formulated into a simple linear system. We show that such
reachability characterization of each state reﬂects the importance of this state, and
thus provides a natural basis that can be used for prioritizing states for the standardvalue iteration process. With that, we also propose the differential backup based
heuristic where the potential impacts of states can be more accurately captured. Inaddition, we extend the classic prioritized sweeping to the temporal process and re-allocate the sweeping efforts during different stages, so that only partial sweeping can
be performed during certain intermediate value iteration stages and the convergence
performance can be further improved.
2 Related Work
The basic computational mechanisms and techniques for MDPs have been well-
understood and widely applied to solve many decision-theoretic planning [ 8,20]
and reinforcement learning problems [ 10,21]. V alue iteration and policy iteration
are two of the most famous algorithms to solve the MDPs and particularly the valueiteration might be the most widely used mechanism due to its easy implementation
and fast convergence [5, 13].
One of the most widely-used frameworks for speeding up the convergence is a
rich class of heuristics based on state-space search [ 4,12]. Such heuristics usually
take advantage of MDPs’ probabilistic transition models directly, and use a tree orgraph structure to search for a solution in the form of a sequence of actions, leading
to a path from the start state to a goal state. For instance, the most well known
method in this category is probably the real-time dynamic programming (RTDP) [4]
where states are not treated uniformly. In each DP iteration of the RTDP , only a
subset of most important states might be explored, and the selection of the subset
of states are usually built on, and related to, the agent’s exploration experience.For a single backup iteration, the RTDP typically requires less computation time
in comparison to the classic DP where all states need to be swept, and thus can
be extended as an online process and integrated into the real-time reinforcement
learning framework [ 6]. Similar strategies also include the state abstraction [2,15],
where states with similar characteristics are hierarchically and/or adaptively grouped
together, either in ofﬂine static or online dynamic aggregation style. Different from
all these methods, the proposed framework is not based on state-space search, instead
it characterizes the entire state space into a reachability landscape using the MFPTinformation.
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 389
Another important heuristic for efﬁciently solving MDPs is the prioritized
sweeping [16], which has been broadly employed to further speed up the value
iteration process. This heuristic evaluates each state and obtains a score based on the
state’s contribution to the convergence, and then prioritizes/sorts all states based on
their scores (e.g., those states with larger difference in value between two consecu-
tive iterations will get higher scores) [ 1,23]. Then in the immediately next dynamic
programming iteration, evaluating the states follows the newly prioritized order. The
prioritized sweeping heuristic is also leveraged in our MFPT based value iteration
procedure, and comparisons with baseline approaches have been conducted in ourexperimental section.
The reachability of state space has been investigated in existing works. For exam-
ple, the structured reachability analysis [7] of MDPs has been proposed to evaluate
whether a state is reachable or not, so that one can restrict the dynamic programming
to only reachable states, reducing the computational burden of solving an MDP . Note,
the reachability in that work is deﬁned as a binary state, and if a state is eventuallyreachable from a given starting state, then it is deﬁned as reachable, otherwise it
is unreachable. This is different from our reachability landscape where each state’s
reachability is measured with a real-valued number.
Important related frameworks for solving MDPs also include compact represen-
tations such as linear function representation and approximation [ 13, 17]u s e di n
the policy iteration algorithms. The linear equation based techniques do not exploit
regions of uniformity in value functions associated with states, but rather a compact
form of state features that can somewhat reﬂect values [ 9]. Our method for comput-
ing the MFPT can also be formulated into a linear system. However, the intermediate
results generated from MFPT are more direct: the produced reachability landscape
represented by a “grid map” very well capture—and also allow us to visualize—therelevance or importance of states, and can lead to a faster convergence speed which
is demonstrated in the experiments.
3 Preliminaries
3.1 Markov Decision Processes
Deﬁnition 1 An Markov Decision Process (MDP) is a tuple M=(S,A,T,R),
where
•S={s1,..., sn}is a set of states;
•A={a1,..., an}is a set of actions;
•T:S×A×S→[ 0,1]is a state transition function such that Ta(s1,s2)is the
probability that action ain state s1will lead to state s2;
•R:S×A→ Ris a reward function where Ra(s,s/prime)returns the immediate reward
received on taking action ain state sthat will lead to state s/prime.
390 S. Debnath et al.
If every non-terminal state can eventually enter a terminal state such as a
goal/destination state, then such a Markov system is absorbing by nature [9]. In
this work, we restrict our attention to absorbing Markov systems so that the agent
can arrive and stop at a goal.
Apolicy is of the form π={s1→a1,s2→a2,..., sn→an}. We denote π[s]
as the action associated to state s. If the policy of a MDP is ﬁxed, then the MDP
behaves as a Markov chain [14 ].
3.1.1 Value Iteration
The V alue Iteration (VI) is probably the most widely employed approach to solve
MDPs. It is an iterative procedure that calculates the value (or utility in some litera-
ture) of each state based on the values of the neighbouring states until it converges.
The value V(s)of a state sat each iteration can be calculated by the Bellman equation
shown below
V(s)=max
a∈A/summationdisplay
s/prime∈STa(s,s/prime)/parenleftBig
Ra(s,s/prime)+γV(s/prime)/parenrightBig
, (1)
where γis a reward discounting parameter. The stopping criteria for the algorithm
is when the values calculated on two consecutive iterations are close enough, i.e.,
max s∈S|V(s)−V/prime(s)|≤ε , where εis an optimization tolerance/threshold value,
which determines the level of convergence accuracy. We call one such iterative update
aBellman backup.
3.1.2 Prioritized Sweeping
The standard Bellman backup evaluates values of all states in a sweeping style, fol-
lowing the index of the states stored in the memory. To speed up the convergence, a
heuristic called prioritized sweeping has been proposed and widely used as a bench-
mark framework for non-domain-speciﬁc applications. The algorithm labels states
as more “relevant” or more “important” during a particular iteration, if the change
in the state value is higher when compared to its previous iteration. The essential
idea is that, the larger the value changes, the higher impact that updating that statewill change its dependent states, thereby taking a larger step towards convergence.
Algorithm 1 shows how the value iteration proceeds in prioritized sweeping (VI-PS).
3.2 Mean First Passage Times
Starting from state si, the number of state transitions involved in reaching states sj
for the ﬁrst time is referred as ﬁrst passage time (FPT) ,Tij.T h emean ﬁrst passage
time (MFPT), μijfrom state sitosjis the expected number of steps required to
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 391
Algorithm 1: Prioritized Sweeping (VI-PS)
1Initialize a priority queue, PQ as empty
2s:= state with change δinV(s)
3Insert sinto PQ
4while PQ is not empty do
5 s:= pop highest priority entry from PQ
6 Update V(s)as per Bellman backup
7 foreach predecessor state s/primeof sdo
8 Calculate change δ/primeinV(s/prime)
9 ifδ/prime≥threshold then
10 priority of s/prime:= max (current priority, max a(δ·Ta(s/prime,s))
11 Insert or update PQ with s/primeand the calculated priority
transit from state sito an absorbing state sj[3]. The MFPT analysis is built on the
Markov chain and give us information about the short range behavior of the chain.
Moreover, it has nothing to do with the agent’s actions. Remember that, when an
MDP is associated to a policy, it then behaves as a Markov chain [14 ].
Formally, let us deﬁne a Markov chain with nstates and transition probability
matrix, p∈IRn,nwhere pikrepresents the transition probability from state sito
sk. If the transition probability matrix is regular, then we can refer each MFPT,
μij=E(Tij). Given, Bkis an event where the ﬁrst state transition happens from
state sitosk, MFPT satisﬁes the below conditional expectation formula:
E(Tij)=/summationdisplay
kE(Tij|Bk)pik (2)
From the deﬁnition of mean ﬁrst passage times, we have, E(Tij|Bk)=1+
E(Tkj). So, we can rewrite Eq. (2)a sf o l l o w s .
E(Tij)=/summationdisplay
kpik+/summationdisplay
k/negationslash=jE(Tkj)pik (3)
Since,/summationtext
kpik=1 ,E q . ( 3) can be formulated as per the below equation:
μij=1+/summationdisplay
k/negationslash=jpik∗μkj (4)
Equation (4) can be rewritten as:
/summationdisplay
k/negationslash=jpik∗μkj−μij=− 1, (5)
Solving all MFPT variables can be viewed as solving a system of linear equations
392 S. Debnath et al.
⎛
⎜⎜⎜⎜⎝p
11−1 p12.. .. p1n
p21 p22−1.. .. p2n
.. .. .. .. ..
.. .. .. .. ..
pn1 pn2.. .. pnn−1⎞
⎟⎟⎟⎟⎠⎛
⎜⎜⎜⎜⎝μ
1j
μ2j
..
..
μnj⎞
⎟⎟⎟⎟⎠=⎛
⎜⎜⎜⎜⎝−1
−1
....
−1⎞
⎟⎟⎟⎟⎠. (6)
The values μ
1j,μ2j,...,μ njrepresent the MFPTs calculated for state transitions
from states s1,s2,..., sntosj. To solve above equation, efﬁcient decomposition
methods [ 11] may help to avoid a direct matrix inversion.
4 Reachability and Differential Based Solution
Our objective is to accelerate the convergence process of solving an MDP and we
propose a reachability and differential based solution. Speciﬁcally, we ﬁrst quantifythe level of reachability of every state using the Mean First Passage Time (MFPT);
then, we introduce the notion of backup differential as an extension to capture the
state’s potential impact to other states at an even ﬁner level. Finally, the reachabilityand differential based mechanism is integrated with the temporally prioritized partial-
space sweeping heuristics.
4.1 Construction of Reachability Landscape
By “reachability of a state” we mean that based on current ﬁxed policy, how hard
it is for the agent to transit from this state to the given goal/absorbing state. Asmaller reachability value is deﬁned as more reachable. Thus, a state’s reachability
in this context is in fact a real-valued number instead of a binary value (reachable
vs. unreachable), and for all states we can construct a reachability landscape, which
can be visualized as a grid map if the states have some spatial structure, see Fig. 1
for an illustration.
Speciﬁcally, assume the goal state is s
j, we use the MFPT value μijto quantify
the reachability from an arbitrary state sito the goal state sj. For all states, we can
then construct a reachability landscape represented as a surface. Figure 1b shows an
example represented as heatmap in our simulated environment. The values in the
heatmap range from 0 (cold color) to 100 (warm color). Intuitively, one may imagine
the landscape to be an “energy surface” where the minimum energy state is themost stabilized, and an agent situating at a non-minimum state will be unstable and
eventually moves towards to the minimum state, which is the absorbing state where
the goal is located. It is worth mentioning that, in order to better analyze the lowMFPT spectrum that we are most interested, values in reachability landscape may
be “clipped”. For example, in Fig. 1b any value greater than 100 has been clipped
to 100.
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 393
Fig. 1 Illustration of reachability landscape. aDemonstration of a simple simulation scenario
with dark blocks as obstacles, and the goal state as a red circle; bFinal reachability landscape
corresponding to the converged MDP
We observe that the reachability conveys very useful information on potential
impacts to other states. More speciﬁcally, a state with a better reachability (smaller
MFPT value) is more likely to make a larger change during the MDP convergence
procedure, leading to a bigger convergence step.
4.2 Mean First Passage Time Based V alue Iteration
(MFPT -VI)
The prioritized sweeping mechanism has further improved the convergence rate
over the classic Bellman backup by exploiting and ranking the states based on their
potential contributions, where the metric is based on comparing the difference ofvalues for each state between two consecutive iterations.
We propose a new method called Mean First Passage Time based V alue Iteration
(MFPT-VI) which is also built on the prioritized sweeping mechanism, but using a
different prioritizing metric. Formally, we propose to prioritize the states using the
reachability values, because as aforementioned, the reachability characterization ofeach state reﬂects the potential impact/contribution of this state, and thus provides
a natural basis for prioritization. Our reachability based metric is distinct from the
existing value-difference metric as follows, the reachability landscape can very wellcapture the degree of importance for all states from a global viewpoint, whereas the
classic value-difference strategy evaluates states locally, and may fail to grasp the
correct global “convergence direction” due to the local viewpoint.
Note that, since the MFPT computation is relatively expensive and it is particularly
good at capturing high-level feature, it is not necessary to compute the MFPT at everyiteration, but rather after every few iterations. It is also worth mentioning that, if there
394 S. Debnath et al.
are multiple goal/absorbing states, then each goal state will require to compute its
own MFPT landscape, and the ﬁnal reachability landscape will be normalized across
all obtained landscapes. The computational process of MFPT-VI is pseudo-coded inAlgorithm 2.
Algorithm 2: Mean First Passage Time based V alue Iteration (MFPT-VI)
1Given states S, actions A, transition probability Ta(s,s/prime)and reward Ra(s,s/prime). Assume goal
state s∗, calculate the optimal policy π
2while true do
3 V=V/prime
4 Calculate MFPT values μ1s∗,μ2s∗,···,μ|S|s∗by solving the linear system as shown in
Eq. (6 )
5 List L:= Sorted states with increasing order of MFPT values
6 foreach state s in L do
7 Compute value update at each state sgiven policy π:
V/prime(s)=max a∈A/summationtext
∀s/prime∈STa(s,s/prime)/parenleftBig
Ra(s,s/prime)+γV(s/prime)/parenrightBig
8 ifmax si|V(si)−V/prime(si)|≤ε then
9 break
4.3 A Heuristic Using Backup Differentials
Since the reachability of a state implies how hard it is to reach the given goal statefrom that state, and during the value iteration process the reachability landscape is
re-computed periodically representing a more reﬁned overview of the reachability
characterization, therefore the difference from the old landscape to an updated versioncan indicate the “changing direction of the reachability” to some extent. Intuitively,
a larger error (reduction of the reachability value) on a state implies more signiﬁcant
potential that this state can impact the convergence. Since the error is the differencebetween two subsequent landscapes and it occurs during the Bellman backup, we
call such error computation a backup differential .
We propose to extend the MFPT-VI to use the backup differential as a prioritizing
metric, and term this method D-MFPT-VI, where “D” means differential. Based on
above observation, the potential impact of a state can also be better captured bytherate of error changes, which is essentially the higher order of differentials. For
example, the next order of the backup differential is D2-MFPT-VI, accounting forthe possible future trends of the error, based on its current rate of change. This isanalogous to the kinematics analysis of displacement, velocity and acceleration, etc.
Obviously, the standard VI-PS, that only measures the error between two itera-
tions is a special case of such differential framework. In addition to that, we alsoimplemented a one-order higher differential version of VI-PS and we refer it to as
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 395
D-VI-PS, and compared its performance with the reachability based variant of the
equivalent order of backup differential. The computational process of D-MFPT-VI
is pseudo-coded in Algorithm 3.
Algorithm 3: Differential Mean First Passage Time based V alue Iteration (D-
MFPT-VI)
1Given states S, actions A, transition probability Ta(s,s/prime)and reward Ra(s,s/prime). Assume goal
state s∗, calculate the optimal policy π
2while true do
3 V=V/prime
4 MFPT Old = Current MFPT values μ1s∗,μ2s∗,···,μ|S|s∗
5 Calculate MFPT values μ1s∗,μ2s∗,···,μ|S|s∗by solving the linear system as shown in
Eq. (6 )
6 MFPT New= Newly calculated MFPT values μ1s∗,μ2s∗,···,μ|S|s∗
7ΔMFPT =MFPT New-MFPT Old
8 List L:= Sorted states with decreasing order of ΔMFPT values
9 foreach state s in L do
10 Compute value update at each state sgiven policy π:
V/prime(s)=max a∈A/summationtext
∀s/prime∈STa(s,s/prime)/parenleftBig
Ra(s,s/prime)+γV(s/prime)/parenrightBig
11 ifmax si|V(si)−V/prime(si)|≤ε then
12 break
4.4 Solving MDPs in Partial State Space
Until here, what we discussed are for prioritized sweeping in full state space, andour experimental results in Sect. 5will show that the proposed framework is superior
to the baseline popular methods. In this section, we show that by extending the
framework to partial space sweeping, the convergence rate can be further enhanced.
Our heuristics are based on the allocation of sweeping efforts. Speciﬁcally, the
prioritized sweeping mechanism has re-allocated the state sweeping from an arbitraryset to a sorted list, but such re-allocation is still limited to one iteration at a particular
time moment. Because the solving process involves many iterations (and moments),
we take advantage of the temporal process and re-allocate the sweeping efforts dur-
ing different temporal stages. In greater detail, as aforementioned, the reachabilitycharacterization at earlier stages can only capture a big picture of the problem and it
begins to reﬁne local details in later phases, thus, at earlier value iterations we opt to
sweep a small subset of states of most impact, and gradually enlarge the sweepingset by including more states of smaller impact, until all states are included in the
later stages. One challenge here is how to divide the space from the prioritized list.
We provide two heuristics as follows:
396 S. Debnath et al.
(1) The ﬁrst heuristic is to uniformly divide the prioritized states into sub-lists of
equal size (length). If the number of partitions is p, and the sweeping method is
D-MFPT-VI (for example), then each partitioned sub-list contains |S|/pstates.
We refer to this heuristic as D-MFPT-VI-H1.
(2) The second heuristic is to uniformly divide the “impact”, but in this way the
lengths of sub-lists can be non-uniform. Formally, given the number of parti-
tion pand the errors obtained in D-MFPT-VI, and assume the maximum error
(corresponding to largest impact) and minimum error (corresponding to smallestimpact) are e
maxandemin, respectively. Then the error range can be calculated as
r=emax-eminand we wish each partitioned sub-list to have an error range ofr
p.
Consequently, for partitioning index i=1,..., p, a state with an error between
emin+(i−1)∗r/pand emin+i∗r/pbelongs to partition i. We refer to this
heuristic as D-MFPT-VI-H2.
4.5 Time Complexity Analysis
Each value iteration has a time complexity of O(|A||S|2)where |S|denotes the
number of states and |A|represents the number of actions. Calculation of the
MFPT needs to solve a linear system that involves matrix inversion (the matrix
decomposition has a time complexity of O(|S|2.3)if state-of-the-art algorithms are
employed [ 11], given that the size of matrix is the number of states |S|). There-
fore, for each iteration, both the MFPT-VI and D-MFPT-VI algorithms have a timecomplexity O(|A||S|
2+|S|2.3). However, experimental results show that our reach-
ability characterization based framework practically requires much fewer iterations
to converge.
5 Experimental Results
We validated our method through numerical evaluations with a simulator written in
C++ running on a Linux machine.
We consider the generic MDP problem where each action can lead to transitions
to all other states with certain transition probabilities. However, in many practicalscenarios, the probability of transiting from a state to another state that is “weakly
connected” can be small, even close to 0. This can potentially result in non-dense
transition matrix. For example, in the robotic motion planning scenario, the state
transition probability from state s
ito state sjcan be correlated with the time or
distance of traveling from sitosj, and it is more likely for a state to transit to some
states within certain vicinity. To obtain the discrete MDP states, we tessellate theagent’s workspace into a grid map, and represent the center of each grid as a state. Inthis way, the state hopping between two states represents the corresponding motion in
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 397
Fig. 2 a Demonstration of simulation environment, with the agent’s initial state (blue) and the
goal state (red). Grey blocks are obstacles; b–eEvolution of reachability landscapes; fConverged
optimal policy (red arrows) and a trajectory completed by the agent to reach the goal
the workspace. A demonstration is shown in Fig. 2. All experiments were performed
on a laptop computer with 8GB RAM and 2.6 GHz quad-core Intel i7 processor.
5.1 Solution with Full Space Sweeping
To assess the advantages introduced by the MFPT reachability characterization, we
ﬁrst investigate the solution convergence using this method in the full state space, by
comparing with other popular full space sweeping mechanisms.
We start by comparing the practical runtime performance as it is the most basic
algorithmic evaluation metric. The time taken by VI, VI-PS and MFPT-VI algorithmsto converge to the optimal solution (with the same convergence error threshold) are
shown in Fig. 3a. The results reveal that VI is the slowest amongst all the three
algorithms. Because of the prioritized sweeping heuristic, VI-PS is faster than VI. It is
obvious to see that our proposed MFPT-VI is much faster than the other two methods.
It is worth noting that in our implementation of MFPT-VI, the MFPT component iscomputed every three iterations instead of every single iteration, because the the
purpose of using MFPT is to characterize global reachability feature which requires
less frequent computations.
398 S. Debnath et al.
Fig. 3 Time comparisons between the baseline methods and our proposed algorithms, with chang-
ing numbers of states ( x-axis). aV ariants of value iteration methods. bBackup differential variants.
The thick green curve in two ﬁgures is the result of MFPT-VI, which can be used as a common
baseline to compare
Figure 3b compares the time taken by MFPT-VI, D-VI-PS, D-MFPT-VI, and D2-
MFPT-PI. Firstly, D-VI-PS is faster than the state-of-the-art VI-PS. Moreover, our
proposed algorithms based on the reachability abstraction are faster than D-VI-PS.
The differential versions of MFPT-VI i.e. D-MFPT-VI and D2-MFPT-VI are faster
compared to MFPT-VI. This clearly establishes the notion of backup differentialas an effective mechanism towards runtime improvement. The results also show
that the D2-MFPT-VI is the fastest compared to the other algorithms. Although the
improvement in performance is signiﬁcant on comparing D-MFPT-VI and MFPT-VI,but its not that signiﬁcant on comparing D-MFPT-VI and D2-MFPT-VI. We believe
that the differential of second order is sufﬁcient in capturing a state’s potential impact,
and therefore, we do not analyze beyond the differential of second order.
Next, we evaluate the number of iterations taken by the algorithms to converge to
the optimal policy as the number of states changes. Figure 4a compares the number
of iterations taken by VI, VI-PS and MFPT-VI, respectively. Again, the advantage
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 399
Fig. 4 Number of iterations required to converge. The x-axis denotes the numbers of states. a
V ariants of value iteration methods. bBackup differential variants. The thick green curve in both
ﬁgures is the result of MFPT-VI
of our algorithm is obvious, and the results show that the MFPT-VI requires much
smaller number of iterations compared to the other two algorithms.
Figure 4b compares the number of iterations taken by MFPT-VI, D-VI-PS, D-
MFPT-VI, and D2-MFPT-VI, from which we can observe that the D2-MFPT-PIconverges the fastest amongst all four algorithms. Similar to the runtime behavior
shown above, the differential variants—D-MFPT-VI and D-VI-PS—converge faster
than the non-differential MFPT-VI and VI-PS. This also implies the remarkable merit
of backup differentials as means for faster convergence.
Then, we look into the detailed time taken by the critical components of each
algorithm. A good understanding for the cost of individual component will allow usto better design state sweeping heuristics. It can be observed from Fig. 5a that most
time is used for Bellman backup operations in the case of VI; there is an additional
step involved for sorting the states for the purpose of states prioritization in case of
VI-PS, which is negligible in comparison to the cost of Bellman backup, as presentedin Fig. 5b. In contrast, majority of time is used for computing the MFPT values in
400 S. Debnath et al.
Fig. 5 Time taken by individual components of algorithms: aVIbVI-PS cMFPT-VI. In the
ﬁgures, VI/BE, VI-PS/BE and MFPT-VI/BE represent the Bellman Equation component of VI, VI-
PS and MFPT-VI respectively. VI-PS/S denotes the sorting component of VI-PS and MFPT-VI/FPTrepresents the component that computes MFPT values in the MFPT-VI algorithm
our MFPT-VI algorithm, as shown in Fig. 5c. Consequently, for the MFPT-VI related
algorithms we suggest to update the MFPT result every few iterations instead of re-
computing it every iteration.
Finally, we are interested in the convergence curve of each algorithm, through
which we can learn and compare the detailed convergence behavior. An importantcriterion to judge the convergence is to see if the difference between two consecutiveiterations is small enough. Thus, we utilize the maximal error Δ
Sacross all states
as an evaluation metric. Speciﬁcally, ΔS=max si∈S|V(si)−V/prime(si)|where V,V/prime
represent the values of states at iteration iandi+1.
Figure 6a shows that initially VI, VI-PS and MFPT-VI start with the same ΔS
value. In the ﬁrst few iterations, our MFPT-VI approach achieves an extremely steepdescent compared to the non-reachability based VI and VI-PS. For example, in oursimulation scenario (with around 2500 states and a threshold value of 0.1), the MFPT-
VI converges using only 19 iterations; in contrast, VI takes 55 iterations and VI-PS
takes 45.
Fig. 6 a The progress of VI, VI-PS and MFPT-VI across iterations. bThe progress of D-VI-PS,
D-MFPT-VI and D2-MFPT-VI across iterations
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 401
Figure 6b shows proﬁles for D-VI-PS, D-MFPT-VI and D2-MFPT-PI. Initially, all
the three algorithms start with the same ΔSvalue. However, both D2-MFPT-VI and
D-MFPT-VI have extremely steep descents in the ﬁrst few iterations as compared
D-VI-PS. For instance, in the same simulation example mentioned above, our D2-
MFPT-VI method converges in 12 iterations whereas D-MFPT-VI takes 13 iterations;
in contrast, D-VI-PS takes 40 iterations.
5.2 Solution with Partial Space Sweeping
As discussed in Sect. 4.4, exploring state space and sweeping in partial space can
further accelerate the convergence. Since the results in full state space reveal that the
reachability based algorithms are faster than other algorithms, we only evaluate the
performances for the reachability based variants including MFPT-VI, D-MFPT-VI,D-MFPT-VI-H1, D-MFPT-VI-H2.
Speciﬁcally, based on the reachability value computed for each state, we sort
all states and partition the space into nsub-spaces (partitions), using the partition
heuristics of either D-MFPT-VI-H1 or D-MFPT-VI-H2. In this way, states with sim-ilar reachability values are clustered together, e.g., in our simulation scenario thestates of the highest reachability are grouped around the goal state. And, the smaller
the reachability of the states, the farther they are from the goal state. See Fig. 7for
an illustration.
Figure 8shows the time taken by the algorithms as the number of partitions n
changes. The results show that, given the same number of partitions, the D-MFPT-
VI-H2 algorithm is in general the fastest among the three algorithms, indicating that
Fig. 7 Illustration of partitioned sub-spaces using the example of Fig. 2. There are four partitions
with different colors and a colder color represents better reachability. The black area in each ﬁgure
denotes obstacle. aCaptured partitions at an earlier iteration; bCaptured partitions at a later iteration
402 S. Debnath et al.
Fig. 8 Time comparisons across various variants under different size of partitions with the number
of states set to 2500
Fig. 9 Time cost surface of
D-MFPT-VI-H2 under
different numbers ofpartitions and states
partitioning the state space measured by states’ potential impacts can achieve better
performance in comparison to partitioning measured by the sizes of sub-spaces. In
contrast, the advantage of D-MFPT-VI-H1 is not obvious as it improves over the
basic MFPT-VI algorithm with a very small margin.
Note, although partitioning the space and exploring the partial space can acceler-
ate the convergence, we have not decided the ideal number of partitions. For exam-ple, in Fig. 8we can see that initially the time taken by the algorithms reduces as
the number of partitions increases. However, after a certain point, the time taken
begins to increase as the number of partitions grows; we can also observe that all the
three algorithms reach the minimal time almost simultaneously when the number
of partitions is at around 5. Finding the optimal partitioning number is difﬁcult as
the state (reachability) values can be very unstructured. Consequently, we use theexperimental method to ﬁnd such number.
We have plotted a clearer proﬁle to capture the minimal-time partition number, as
shown in Fig. 9. In the ﬁgure, the x,y,zaxes represent the number of states, number
Reachability and Differential Based Heuristics for Solving Markov Decision Processes 403
Table 1 The number of partitions when the time taken by D-MFPT-VI-H2 is minimum to calculate
the optimal policy across different state-size
State-size 900 1600 2500 3600 4900
Partitions 2 5 5 2 3
of partitions, and practical runtime, respectively. Note that, there is a “crease” at the
left side the 3D surface, and the bottom of the crease represent the optimal numberof partitions in our heuristics. Table 1shows the partition numbers that result in the
minimum time taken to calculate the optimal policy for differing state-size. We canconclude that, the state space partition number need not be very large, and a partitionnumber of around 2 ∼5 can result in near-optimal runtime.
6 Conclusions
In this paper, we propose new heuristics for efﬁciently solving the MDPs. Our pro-posed framework explores reachability of states using MFPT values, which char-acterizes the degree of difﬁculty of reaching given goal states. We then introduced
the notion of backup differentials as an extension to the reachability characteriza-
tion to capture more accurate impacts of states so that the prioritized sweeping canbe ameliorated. Also, we demonstrated that our reachability and differential based
framework can be further improved using partial-space sweeping strategies. The
experimental results show that in comparison with other state-of-the-art methods,our algorithms converge much faster, with less running time and fewer iterations.
References
1. Andre, D., Friedman, N., Parr, R.: Generalized prioritized sweeping. Advances in Neural Infor-
mation Processing Systems (1998)
2. Andre, D., Russell, S.J.: State abstraction for programmable reinforcement learning agents. In:
AAAI/IAAI, pp. 119–125 (2002)
3. Assaf, D., Shared, M., Shanthikumar, J.G.: First-passage times with PFr densities. J. Appl.
Probab. 22(1), 185–196 (1985)
4. Barto, A.G., Bradtke, S.J., Singh, S.P .: Learning to act using real-time dynamic programming.
Artif. Intell. 72(1–2), 81–138 (1995)
5. Bertsekas, D.P .: Dynamic Programming: Deterministic and Stochastic Models. Prentice-Hall,
Englewood Cliffs (1987)
6. Bonet, B., Geffner, H.: Labeled RTDP: improving the convergence of real-time dynamic pro-
gramming. ICAPS 3, 12–21 (2003)
7. Boutilier, C., Brafman, R.I., Geib, C.: Structured reachability analysis for Markov decision pro-
cesses. In: Proceedings of the Fourteenth Conference on Uncertainty in Artiﬁcial Intelligence,
pp. 24–32. Morgan Kaufmann Publishers Inc., (1998)
404 S. Debnath et al.
8. Boutilier, C., Dean, T., Hanks, S.: Decision-theoretic planning: structural assumptions and
computational leverage. J. Artif. Intell. Res. 11, 1–94 (1999)
9. Boutilier, C., Dearden, R., Goldszmidt, M.: Stochastic dynamic programming with factored
representations. Artif. Intell. 121(1), 49–107 (2000)
10. Busoniu, L., Babuska, R., De Schutter, B., Ernst, D.: Reinforcement Learning and Dynamic
Programming Using Function Approximators, vol. 39. CRC Press, Boca Raton (2010)
11. Golub, G.H., V an Loan, C.F.: Matrix Computations, 3rd edn. Johns Hopkins University Press,
Baltimore (1996)
12. Hansen, E.A., Zilberstein, S.: Lao: a heuristic search algorithm that ﬁnds solutions with loops.
Artif. Intell. 129(1–2), 35–62 (2001)
13. Howard, R.A.: Dynamic Programming and Markov Processes. MIT Press, Cambridge (1960)
14. Kemeny, J.G., Mirkill, H., Snell, J.L., Thompson, G.L.: Finite Mathematical Structures.
Prentice-Hall, Upper Saddle River (1959)
15. Li, L., Walsh, T.J., Littman, M.L.: Towards a uniﬁed theory of state abstraction for MDPs. In:
ISAIM (2006)
16. Moore, A.W., Atkeson, C.G.: Prioritized sweeping: reinforcement learning with less data and
less time. Mach. Learn., 103–130 (1993)
17. Puterman, M.L.: Markov Decision Processes: Discrete Stochastic Dynamic Programming.
Wiley, Hoboken (2014)
18. Russell, S., Norvig, P .: Artiﬁcal intelligence: a modern approach. http://aima.cs.berkeley.edu/
(2002). Accessed 22 Oct 2004
19. Sigaud, O., Buffet, O.: Markov Decision Processes in Artiﬁcial Intelligence. Wiley, Hoboken
(2013)
20. Sutton, R.S.: Integrated architectures for learning, planning, and reacting based on approx-
imating dynamic programming. In: Proceedings of the Seventh International Conference onMachine Learning, pp. 216–224 (1990)
21. van Otterlo, M., Wiering, M.: Reinforcement learning and Markov decision processes. Rein-
forcement Learning, pp. 3–42. Springer, Berlin (2012)
22. White, D.J.: A survey of applications of Markov decision processes. J. Oper. Res. Soc. 44(11),
1073–1096 (1993)
23. Wingate, D., Seppi, K.D.: Prioritization methods for accelerating MDP solvers. J. Mach. Learn.
Res. 6, 851–881 (2005)
Learning to Singulate Objects
Using a Push Proposal Network
Andreas Eitel, Nico Hauff and Wolfram Burgard
1 Introduction
Robot manipulation tasks such as tidying up a room or sorting piles of objects are a
substantial challenge for robots, especially in scenarios with unknown objects. Theobjective of object singulation is to physically separate a set of cluttered objects
through manipulation, a capability regarded as relevant for service robots operating
in unstructured household environments. Further, the ability to separate unknown
objects from surrounding objects provides great beneﬁt for object detection, which
still remains an open problem for overlapping objects. A key challenge of objectsingulation is the required interaction of multiple capabilities such as perception,
manipulation and motion planning. Perception may fail because of occlusions from
objects but also from occlusion by the manipulator itself. Manipulation actions suchas grasping may also fail when attempting to manipulate objects that the perception
systems fails to correctly segment. Motion planning is particularly prone to errors
when applied in unknown, unstructured environments.
Previous approaches for object singulation strongly incorporate the concept of
object segments or even complete objects. Push action strategies are selected based
on the current belief of the conﬁguration of all segments or objects in the scene,an assumption that is not robust, since segments might merge and object detection
might fail. We apply a different strategy that aims to relax the concept of segments and
objects for solving the task at hand, making task execution less prone to modellingerrors from the perception modules. Similar to recent approaches that operate directly
on image inputs [ 19,22] we aim for a learning-based action selection approach that
takes as input an RGB-D image of a tabletop scene and proposes a set of meaningfulpush actions using a convolutional neural network, as shown in Fig. 1.
A. Eitel (B)·N. Hauff ·W. Burgard
Department of Computer Science, University of Freiburg, Freiburg, Germanye-mail: eitel@cs.uni-freiburg.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_32405
406 A. Eitel et al.
Fig. 1 Our approach consists of a convolutional neural network that ranks a set of potential push
actions based on an over-segmented input image in order to clear objects in clutter. The proposed
pushes are executed using motion planning. Our push proposal network is trained in an iterative
manner from autonomous interaction with cluttered object scenes
Our primary contributions are: (1) a push proposal network (Push-CNN), trained
in an iterative manner to detect push candidates from RGB-D images in order to
separate cluttered objects, (2) a method that samples potential push candidates based
on depth data, which are ranked with our Push-CNN, and (3) real-world experiments
with a PR2 robot in which we compare the performance of our method against a
strong manually-designed baseline.
We quantitatively evaluate our approach on 4 sets of real-robot experiments.
These experiments involve singulation tasks of increasing difﬁculty ranging from
4 unknown objects up to conﬁgurations with 8 unknown objects. In total the robot
executed over 400 push actions during the evaluation and achieved an object separa-
tion success rate of up to 70%, which shows that our Push-CNN is able to generalize to
previously unseen object conﬁgurations and object shapes. Our Freiburg Robotpush
dataset is publicly available at http://robotpush.cs.uni-freiburg.de .
2 Related Work
Standard model-based approaches for object singulation require knowledge of object
properties to perform motion planning in a physics simulator and to choose push
actions accordingly [ 6,7]. However, estimating the location of objects and other
properties of the physical environment in order to plan push actions can be subject to
errors, especially for unknown objects and cluttered scenes [ 21,30,31]. Interactive
model-free methods have been applied to solve the task by accumulating evidence of
singulated items over a history of interactions including push and grasp primitives [ 5].
Learning to Singulate Objects Using a Push Proposal Network 407
Difﬁculties arise when objects have to be tracked after each interaction step, requiring
small motion pushes due to partial observations and occlusions. We follow a model-
free approach that encodes evidence of singulated objects in the learned feature spaceof the network. Hermans et al. [ 13] perform object singulation using several push
primitives similar to ours. Their method is based on object edges to detect splitting
locations between potential objects to apply pushes in those regions respectively,but does not include learned features and does not take into account stacked objects.
Katz et al. [ 14] present an interactive segmentation algorithm to singulate cluttered
objects using pushing and grasping. Similar to them, we also create object hypothesisfrom over-segmenting objects into surface facets but use a different method based on
t h ew o r ko f[ 26]. They perform supervised learning with manual features to detect
good manipulation actions such as push, pull and grasp. We take the idea of learning astep further by directly learning from over-segmented images and therefore removing
the need for manual feature design. Boularias et al. [ 3] learn favourable push actions
to improve grasping of objects in clutter in a reinforcement learning setting, but usemanual features and do not show the applicability of their method for more than 2
objects. Gupta et al. [ 10] present an approach to sort small cluttered objects using a
set of motion primitives, but do not show experiments with objects of various sizes
and shapes. Laskey et al. [ 18] leverage learning from human demonstrations to learn
control policies for object singulation, but thus far only considered singulating asingle object from a cluttered scene, while our method can singulate up to 6 objects
and achieve a higher success rate.
Our approach is related to robot learning from physical interaction [ 24], includ-
ing approaches that learn to predict how rigid objects behave if manipulated [ 12,17,
32] and self-supervised learning methods for grasping [ 20,25]. Gualtieri et al. [ 9]
present a grasp pose detection method in which they detect grasp candidates using aconvolutional neural network. They follow a similar methodology but for a different
task and goal. Recent methods learn the dynamics of robot-object interaction for the
task of pushing objects to a target location [ 1,8], but do not evaluate on scenes with
many touching objects and do not follow an active singulation strategy. Byravan et
al. [4] learn to predict rigid body motions of pushed objects using a neural network,
but to not demonstrate results for multiple objects in a real-world scenario. Objectseparation is used as a source to leverage perceptual information from interaction
following the paradigm of interactive perception [ 2]. As demonstrated in our exper-
iments, our singulation method can be used to generate informative sensory signalsthat lower the scene complexity for interactive object segmentation [ 11,16,27,29].
3 Learning to Singulate Objects
We consider the problem of learning good push actions to singulate objects in clutter.
Let obe an image with height Hand width Wtaken from an RGB-D camera with
a known camera intrinsics matrix K. Further, let a=(c,α) be a push proposal with
start position c=(x,y)and push angle αboth speciﬁed in the image plane. We aim
408 A. Eitel et al.
to learn a function p=F(o,a;θ)with learned parameters θthat takes as input the
image together with a push proposal and outputs a probability of singulation success.
3.1 Data
The prediction function Fis trained in an iterative manner (we use 2 training iterations
F1,F2) on experiences that the robot collects trough interaction with the environment.
At the ﬁrst iteration, we gather a dataset Dof push proposals together with images
from randomly pushing objects in simulation. Then, we train a classiﬁer F1that
best mimics the labels from an expert user in terms of successful and unsuccessful
singulation actions. In the next iteration, we use F1to collect more data and add this
data to D. The next classiﬁer F2is trained to mimic the expert labels on the whole
dataset D. We refer to F1as a vanilla network that is trained solely on experience
collected from random interaction and F2as an aggregated network. The training
examples D={(o1,a1,y1),... ,( oN,aN,yN)}are labeled by an expert user, where
yiis a corresponding binary label. Figure 2depicts the labels given by an expert user
for one exemplary singulation trial. The function is trained in a supervised manner
using a negative log likelihood loss min θ/summationtextN
i=1L/parenleftbig
p,yi/parenrightbig
and a sigmoid function.
Fig. 2 For training the push proposal CNN each push action (small arrow) from the collected
dataset is labeled as positive or negative by a user who assesses the outcome of the action. The
criteria for a positive label are: the object got singulated, the push did not move multiple objects,
the object was pushed close to its center of mass and the pushed object was not already singulated
Learning to Singulate Objects Using a Push Proposal Network 409
3.2 Approach
Our approach is divided into three modules, as shown in Fig. 1: (1) a sampling module
that generates a set of push proposals, (2) a neural network module that classiﬁesthe set of push proposals and ranks them accordingly, and (3) an action execution
module that computes arm motion plans for the ranked pushes and then executes
the ﬁrst push proposal that has both a high probability of singulation success and asuccessful motion plan.
3.2.1 Push Proposal Sampling
Our push proposal sampling method is designed to generate a set of push pro-
posals {a
1,..., aM}. First, we sample 3D push handles {h1,..., hM}represented
as point normals pointing parallel to the xy-plane of the robot’s odometry frameh
m=(x,y,z,nh),nh=(−nx,−ny,0). Speciﬁcally, given the raw depth image,
we ﬁrst apply a surface-based segmentation method [ 26]( w eo n l yu s et h ep r e -
segmentation and surface-detection steps of the approach) to obtain a set of seg-ments {s
1,..., sL}together with a surface normals map and a table plane. Second,
we sample for each segment sla ﬁxed number of push handles and remove push
handles below the table plane. We assume a ﬁxed push length la=0.2 for all han-
dles and compute the convex hull for the table in order to ﬁlter out push handles that
have push endpoints outside of the table. Finally, we obtain a set of push proposals
by transforming the push handles into the camera frame using the transformationmatrix Cfrom the odometry to the camera frame together with the camera intrinsics
a=KCh .
3.2.2 Push Proposal Network and Input
We parametrize the predictor F(o,a;θ)using a deep convolutional neural network,
denoted as a push proposal CNN in Fig. 1. The distinction between oand aas input
is somewhat artiﬁcial, because both inputs are combined in a new image o
reswhich
constitutes the ﬁnal input of the network. To predict the probability of success of a
push proposal for the given singulation task the most important feature is the relationof the candidate segment that we want to push and its relation to neighbouring
segments. We propose to fuse the segmented camera image with a push proposal
using rigid image transformations T
t,Tr, see Fig. 3. First, we transform the image
by a rigid translation Tt(c)according to the start position cof the push proposal.
Second, we apply image rotation Tr(α)according to the push angle α. The resulting
image is ores=TrTto, as depicted in the right image of Fig. 3.A tt e s tt i m ew ef o r w a r d
the set of transformed input images {o1
res,..., oM
res}into the push proposal network to
predict a probability of success for each push proposal in the set {a1,..., aM}.T h e
network architecture is depicted in Fig. 4.
410 A. Eitel et al.
Fig. 3 We encode oand atogether in a push proposal image oresthat maps both the conﬁguration
of the objects in the scene and the push proposal direction into a ‘push centric’ frame
Fig. 4 Our push proposal network architecture to predict a probability of singulation success for
a push proposal image ores. The input to the network is a single channel grayscale version of the
transformed image oreswith a resolution of 64 ×64 pixels. The network consists of 5 convolutional
layers and 2 fully connected layers
We train the network from scratch using random uniform weight initialization
and Adam [ 15]. We performed network architecture search with 10 different con-
ﬁgurations and found that for our task the depicted network architecture yielded
best performance on 2 separate validation sets. We experienced a drop in classiﬁca-
tion performance when increasing the image size, introducing dropout, or removing
pooling operations, and did not experience improvement for adding xavier weight
initialization or other activation functions besides ReLUs. Finally, we train our model
for 25 epochs with a batch size of 64, using the default parameters for Adam (learning
rate=0.001,β1=0.9,β2=0.999,/epsilon1=1e−08, decay =0.0).
3.2.3 Push Motion Planning
To ﬁnd a motion plan for each push proposal we use the LBKPIECE motion planning
algorithm provided by an out-of-the-box framework [ 28]. Our arm motion strategy
consists of two steps. In the ﬁrst step, we plan to reach the target push proposal am.
In a second step, given that the gripper reached the desired push start position, the
robot performs a straight line push with ﬁxed push length la. We compute motion
Learning to Singulate Objects Using a Push Proposal Network 411
plans for both steps before execution to avoid that the robot executes the reaching
plan and then fails to ﬁnd a plan for the straight line push. We found our two-step
procedure to be more stable than executing a reach and push trajectory at once, dueto arm controller errors.
4 Experiments
We conduct both qualitative and quantitative experiments on a real PR2 robot to
benchmark the performance of our overall approach. Hereby, we aim to answer the
following questions: (1) Can our push proposal network predict meaningful pushactions for object shapes and conﬁgurations it has not seen during training? (2) Can
our network trained in simulation generalize to a real-world scenario? (3) Can our
model reason about scene ambiguities including multiple object clusters and focus itsattention to objects that are more cluttered than others? In order to answer question
(1) we perform real-world experiments with objects that the network has not seen
during training and compare against a manual model-free baseline that reasons about
available free space in the scene and keeps a history of previous actions. To answer
question (2) we test a network that we train solely with data from simulation (denotedas vanilla network). Finally, three challenging experiments with 6 and 8 objects aim
to answer question (3), where the learned model has to trade-off between further
separating isolated objects to create more space and split isolated object clusters tosolve the task. The overall intent of our experiments is to show that a learned model
is able to directly map from sensor input to a set of possible actions, without prior
knowledge about objects, spatial relations or physics simulation.
4.1 Experimental Setup
Our PR2 robot is equipped with a Kinect 2 camera mounted on its head that providesRGB-D images with a resolution of 960 ×540. We use both arms of the robot, each
with 7-DOF to perform the pushing actions. The task is designed as follows: a trial
consists of a ﬁxed number of objects from a test set of everyday objects with differentshapes that the robot has never seen before. In the starting conﬁguration for a trial all
objects are placed or stacked on a table to be in collision. The starting conﬁgurations
are saved and reproduced for all methods in a manual manner using point cloud
alignment. We conduct different experiments of increasing difﬁculty ranging from 4
to 8 objects, in which for each experiment the number of objects stays ﬁxed. The 25starting conﬁgurations for our quantitative evaluation with 4 objects are depicted in
Fig. 5.
We report the number of successful singulation trials. A singulation trial is con-
sidered successful if all objects are separated by a minimum distance of 3cm. Further,
we report results for singulation success after every action, to show which methods
412 A. Eitel et al.
Fig. 5 Starting conﬁgurations of the 25 test trials with 4 objects
can successfully execute the task with less actions. For each trial the robot performs a
ﬁxed set of actions. Given the number of objects nofor each experiment the maximum
number of pushes is npushes=⌊1.3·no⌋+ 1. The robot is allowed to stop before, if
it reasons that all objects are singulated. This is implicitly encoded into our method.If the robot does not ﬁnd a motion plan for the set of push proposals that the network
ranked as positive it automatically terminates the trial.
To train our push proposal network we use an iterative training procedure. First,
the vanilla network is trained on labeled data from a total of 2,486 (243 positives,
2,243 negatives) random interactions performed in simulation. Then the aggregated
network is trained on additional 970 interactions (271 positives, 699 negatives), whichwe collected using the vanilla network in both simulation and real-world, resulting
in a training dataset size of 3,456 push interactions. We use object meshes from the
Freiburg Spatial Relations Dataset for our simulation experiments [ 23].
4.2 Baseline Method
We provide a quantitative comparison against a manually designed baseline method,
to evaluate whether or not the predictions of the push proposal network lead to
improved object singulation results. We do not provide knowledge about the objectsand their physical properties to our model, correspondingly our baseline method is
model-free and follows an interactive singulation strategy that is reasonably effective.
The method which we denote as ‘free space+tracking’ works as follows:
Learning to Singulate Objects Using a Push Proposal Network 413
1. Given a set of segments {s1,..., sL}from our object over-segmentation approach
we construct a graph that includes all segments, where each segment is a node
in the graph and the edges correspond to distances between the segments. Werepresent each segment as an axis-aligned bounding box (AABB) and compute
an edge between 2 segments by means of the Manhattan distance of the 2 AABBs.
2. We compute 2 features for scoring push proposals with the baseline method.
The ﬁrst feature is a predictive feature that reasons about the resulting free space
if a segment would be pushed to some target location. To compute the feature,
we predict a straight line motion of the respective segment to which the pushproposal is assigned to, according to the push direction and the length. Next, we
compute the Manhattan distance between the resulting transformed AABB and all
other segments in the scene, which we assume will remain static. The free spacefeature f
sis the minimum distance between the transformed AABB and the other
AABBs. If the predicted ﬁnal position of a segment would lead to collision with
another segment the free space feature is zero.
3. The second feature includes the push history that we store for each segment.
It follows the intuition that the same segment should not be pushed too often,
regardless of the predicted free space around it. To retrieve the push history over all
segments, we follow a segment-based tracking approach, which aligns the centroid
and the principal components of two segments from the current set of segments andthe set of segments from the last interaction. We match a set of segments using a
weighted average of the principal components and the segment centroid distances
d(s
l,sm)=0.6·dpca(sl,sm)+0.4·dc(sl,sm). To punish multiple pushes rof a
segment throughout a trial we map the push motion history into a normalized
feature using an exponential decay function fh=exp(−r).
4. Accordingly, a push proposal amreceives the score pm=0.5·fs+0.5·fh.
4.3 Quantitative Comparisons
We extensively evaluate our method in real-robot object singulation experiments with
4, 6 and 8 unknown objects. Additionally we perform an experiment with 6 identical
objects to further challenge our approach. Our results with 4 objects (experiment 1),
s h o w ni nT a b l e 1, indicate that our method is able to improve over the performance of
the manual baseline. The success rate of our vanilla network is 68%, which suggests
that the model is making meaningful predictions about which push actions to perform
in order to singulate all 4 objects. Figure 6provides a more ﬁne-grained evaluation,
showing the success rate with respect to the number of pushes. Note that the network
requires less push actions to complete the task. Interestingly, the baseline methodperforms on par with the network after the robot has executed ﬁve push actions, but
then it does not further improve after 6 executed pushes. We noted that the baseline
method sometimes fails to singulate the last 2 objects of the scene and instead willchoose an object that might already be singulated because it lacks a good attention
mechanism.
414 A. Eitel et al.
Ta b l e 1 Object singulation results for our four experiments of increasing difﬁculty
Experiment
no.Number of
objectsMethod Success/total
trialsSuccess rate
(%)Mean number
of pushes
1 4 Free space +
tracking14/25 56 5.4±1.68
4 Va n i l l a
network17/25 68 4.96±1.84
2 6 Free space +
tracking4/10 40 7.8±1.61
6 Va n i l l a
network4/10 40 8.1±1.37
6 Aggregated
network7/10 70 6.7±2.0
3 6 (identical
objects)Aggregated
network10/20 50 7.6±1.76
4 8 Aggregated
network4/10 40 11.1±1.60
1 2 3 4 5 6
number of pushes0.00.20.40.60.8success ratevanilla network
free space+tracking
Fig. 6 Success rate with respect to number of pushes (max. 6) required to clear 4 objects
(experiment 1)
The task is more complex with 6 or 8 objects, due to additional space constraints
and formation of multiple object clusters. When looking at the scene in Fig. 1one
sees that 4 out of the 6 objects are located very closely on the table (green drink
bottle, blue box, white round bowl, white coffee bag). Although there are many
possible push actions that we would consider reasonable, only pushing the coffee
bag or the round bowl to the left side of the table can clear the scene. Accordingly, we
ﬁnd that the performance of the vanilla network drops with respect to the previous
experiment with 4 objects. During training it has only seen a small amount of scenes
where a random baseline would have cleared all but 2 objects and even less likely
has seen examples where the robot by chance chose the right action to clear the 2
remaining objects. Therefore, in this scenario the vanilla network and the baseline
Learning to Singulate Objects Using a Push Proposal Network 415
1 2 3 4 5 6 7 8
number of pushes0.00.20.40.60.8success rateaggregated network
free space+tracking
vanilla network
Fig. 7 Success rate with respect to number of pushes (max. 8) required to clear 6 objects
(experiment 2)
Fig. 8 On the left, we see a successful trial. Note that the robot tries several times to clear a small
plastic bottle which is stuck under a blue box and manages to singulate the objects in the very last
action. On the right we see a run that fails because of two typical reasons: First, the robot is not able
to perform a save push action (fourth column, ﬁfth row), which results in 2 objects moving more
closely together (blue box, cereal box). Second, the network is not able to draw its attention to the
same 2 objects that are now touching and instead proposes an action that moves the green bottle
further away from the scene (lower right image)
416 A. Eitel et al.
Fig. 9 The ﬁrst row depicts a successful singulation run with six identical objects (experiment 3).
The left image depicts the starting conﬁguration and the right image the end result after manipulation.The second row shows a failed trial. The third row shows a trial where the robot managed to singulate8 objects (experiment 4). The last row depicts a trial that failed because the object cluster at thebottom is not cleared
method perform on par with a success rate of 40%. When comparing the average
number of pushes the baseline performs slightly better, see Fig. 7. Results show that
the aggregated network clearly outperforms the other two methods, winning sevenout of ten trials with an average of 6 .7 push actions needed to singulate all 6 objects,
as shown in Table 1(experiment 2). To get an intuition about the numerical results we
refer the reader to the performance reported by Hermans et al. [ 13], who evaluated
Learning to Singulate Objects Using a Push Proposal Network 417
a very similar task with 6 objects. They report a success rate of 20% with twelve
average number of push actions.
We perform an additional experiment with 6 identical objects (sponges) using
the aggregated network and reach a success rate of 50%. The most challenging
experiment is conducted with 8 objects and even though the network was not trained
with such large amount of objects on the table our approach performs well and isable to generalize. Both experiments are depicted in Table 1(experiment 3 and 4).
4.4 Qualitative Results
Figure 8gives insights about the singulation trials from the perspective of the Kinect
camera for experiment 1. The ﬁrst and the third column show the current scene, while
the second and fourth column show the robot performing a push action. The trialevolves from top to bottom of the ﬁgure. Figure 9shows qualitative end results of
singulation runs with 6 identical objects (experiment 3) and 8 objects (experiment
4).
5 Conclusions
We presented a novel neural network-based approach to singulate unknown objects
in clutter by means of pushing actions and showed that it is possible to perform
challenging perceptual manipulation tasks by learning rich feature representationsfrom visual input. Unlike traditional methods we do not manually design features for
the task at hand. Instead, we train a high-capacity convolutional neural network that
we incorporate into a novel system of motion planning and action selection, leadingto good generalization performance, while requiring a reasonable amount of labeled
training data. We tested our method in extensive real-robot experiments using a PR2
robot and showed the ability of our method to achieve good performance for thechallenging task of singulating up to 8 objects. In the future it would be interesting to
train the network in a self-supervised manner, which poses the challenge to automat-
ically generate labels from ambiguous visual data. Furthermore, the network couldbe extended to a multi-class model that predicts different push lengths, which would
require collecting additional training data with varying push lengths.
Acknowledgements This work was partially funded by the German Research Foundation under
the priority program Autonomous Learning SPP 1527 and under grant number EXC 108. We thankSeongyong Koo for advice with the baseline method. We further thank Sudhanshu Mittal, OierMees and Tim Welschehold for their help and ideas.
418 A. Eitel et al.
References
1. Agrawal, P ., Nair, A.V ., Abbeel, P ., Malik, J., Levine, S.: Learning to poke by poking: Expe-
riential learning of intuitive physics. In: Advances in Neural Information Processing Systems(NIPS) (2016)
2. Bohg, J., Hausman, K., Sankaran, B., Brock, O., Kragic, D., Schaal, S., Sukhatme, G.: Inter-
active perception: leveraging action in perception and perception in action. arXiv:1604.03670
(2016)
3. Boularias, A., Bagnell, J.A.D., Stentz, A.T.: Learning to manipulate unknown objects in clut-
ter by reinforcement. In: Proceedings of the Twenty-Ninth AAAI Conference on ArtiﬁcialIntelligence (AAAI) (2015)
4. Byravan, A., Fox, D.: Se3-nets: learning rigid body motion using deep neural networks.
arXiv:1606.02378 (2016)
5. Chang, L., Smith, J.R., Fox, D.: Interactive singulation of objects from a pile. In: Proceedings
of the IEEE International Conference on Robotics and Automation (ICRA) (2012)
6. Cosgun, A., Hermans, T., Emeli, V ., Stilman, M.: Push planning for object placement on clut-
tered table surfaces. In: Proceedings of the IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS) (2011)
7. Dogar, M., Srinivasa, S.: A framework for push-grasping in clutter. In: Robotics: Science and
Systems VII, vol. 1 (2011)
8. Finn, C., Levine, S.: Deep visual foresight for planning robot motion. arXiv:1610.00696 (2016)
9. Gualtieri, M., ten Pas, A., Saenko, K., Platt, R.: High precision grasp pose detection in dense
clutter. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS). IEEE (2016)
10. Gupta, M., Müller, J., Sukhatme, G.S.: Using manipulation primitives for object sorting in
cluttered environments. IEEE Trans. Autom. Sci. Eng. 12(2) (2015)
11. Hausman, K., Balint-Benczedi, F., Pangercic, D., Marton, Z.C., Ueda, R., Okada, K., Beetz,
M.: Tracking-based interactive segmentation of textureless objects. In: Proceedings of the IEEE
International Conference on Robotics and Automation (ICRA) (2013)
12. Hermans, T., Li, F., Rehg, J.M., Bobick, A.F.: Learning contact locations for pushing and
orienting unknown objects. In: Proceedings of the IEEE-RAS International Conference onHumanoid Robots (Humanoids) (2013)
13. Hermans, T., Rehg, J.M., Bobick, A.: Guided pushing for object singulation. In: Proceedings
of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2012)
14. Katz, D., V enkatraman, A., Kazemi, M., Bagnell, J.A., Stentz, A.: Perceiving, learning, and
exploiting object affordances for autonomous pile manipulation. Auton. Robot. (2014)
15. Kingma, D., Ba, J.: Adam: a method for stochastic optimization. arXiv:1412.6980 (2014)
16. Koo, S., Lee, D., Kwon, D.S.: Unsupervised object individuation from rgb-d image sequences.
In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS) (2014)
17. Kopicki, M., Zurek, S., Stolkin, R., Mörwald, T., Wyatt, J.: Learning to predict how rigid objects
behave under simple manipulation. In: Proceedings of the IEEE International Conference onRobotics and Automation (ICRA) (2011)
18. Laskey, M., Chuck, C., Lee, J., Mahler, J., Krishnan, S., Jamieson, K., Dragan, A., Goldberg,
K.: Comparing human-centric and robot-centric sampling for robot deep learning from demon-strations. In: Proceedings of the IEEE International Conference on Robotics and Automation(ICRA) (2017)
19. Levine, S., Finn, C., Darrell, T., Abbeel, P .: End-to-end training of deep visuomotor policies.
J. Mach. Learn. Res. 17(39) (2016)
20. Levine, S., Pastor, P ., Krizhevsky, A., Quillen, D.: Learning hand-eye coordination for robotic
grasping with large-scale data collection. In: International Symposium on ExperimentalRobotics (ISER) (2016)
21. Lynch, K.M., Mason, M.T.: Stable pushing: mechanics, controllability, and planning. Int. J.
Robot. Res. 15(6) (1996)
Learning to Singulate Objects Using a Push Proposal Network 419
22. Mahler, J., Liang, J., Niyaz, S., Laskey, M., Doan, R., Liu, X., Ojea, J.A., Goldberg, K.: Dex-net
2.0: deep learning to plan robust grasps with synthetic point clouds and analytic grasp metrics.arXiv:1703.09312 (2017)
23. Mees, O., Abdo, N., Mazuran, M., Burgard, W.: Metric learning for generalizing spatial rela-
tions to new objects. In: Proceedings of the IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS) (2017)
24. Pinto, L., Gandhi, D., Han, Y ., Park, Y .L., Gupta, A.: The curious robot: learning visual repre-
sentations via physical interactions. arXiv:1604.01360 (2016)
25. Pinto, L., Gupta, A.: Supersizing self-supervision: Learning to grasp from 50k tries and 700
robot hours. In: Proceedings of the IEEE International Conference on Robotics and Automation(ICRA) (2016)
26. Richtsfeld, A., Mörwald, T., Prankl, J., Zillich, M., Vincze, M.: Segmentation of unknown
objects in indoor environments. In: Proceedings of the IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS) (2012)
27. Schiebener, D., Ude, A., Morimoto, J., Asfour, T., Dillmann, R.: Segmentation and learning of
unknown objects through physical interaction. In: Proceedings of the IEEE-RAS InternationalConference on Humanoid Robots (Humanoids) (2011)
28. Sucan, I.A., Moll, M., Kavraki, L.E.: The open motion planning library. IEEE Robot. Autom.
Mag. 19(4), 72–82 (2012)
29. V an Hoof, H., Kroemer, O., Peters, J.: Probabilistic segmentation and targeted exploration of
objects in cluttered environments. IEEE Trans. Robot. 30(5) (2014)
30. Y u, K.T., Bauza, M., Fazeli, N., Rodriguez, A.: More than a million ways to be pushed. a high-
ﬁdelity experimental dataset of planar pushing. In: Proceedings of the IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS) (2016)
31. Zhou, J., Paolini, R., Bagnell, J.A., Mason, M.T.: A convex polynomial force-motion model
for planar sliding: identiﬁcation and application. In: Proceedings of the IEEE InternationalConference on Robotics and Automation (ICRA) (2016)
32. Zhu, S., Kimmel, A., Bekris, K.E., Boularias, A.: Model identiﬁcation via physics engines for
improved policy search. arXiv:1710.08893 (2017)
V ariational Hilbert Regression
with Applications to T errain Modeling
Vitor Campanholo Guizilini and Fabio T ozeto Ramos
1 Introduction
Any sort of autonomous task is predicated on a system’s ability to sense its
surroundings, as a way to understand its current state before deciding what its nextaction should be in order to achieve a determined goal. For mobile robot navigation,
an accurate model for terrain representation is crucial, since it dictates which areas
the vehicle can traverse safely and which ones would result in a collision, or some
other sort of unfavorable outcome. The main challenges in creating an accurate ter-
rain model lie in the presence of measurement uncertainties, data incompletenessand handling details in unstructured areas, which becomes even more challenging
in large-scale scenarios, which is the case for most current applications. Any sensor
is inherently inaccurate, and these inaccuracies are compounded by uncertainty invehicle localization, as new data is collected during navigation to produce a global
representation of observed areas. Data point density rapidly decays as we move
away from the sensor, creating low-resolution areas in which large portions of theenvironment within range remain unobserved. Terrain details (i.e. sharpness and sud-
den discontinuities) should be correctly captured for an accurate representation of
observed data, however if the model is too complex it might overﬁt and produce poorestimates under testing conditions.
In robotics, state-of-the-art terrain representations were originally limited to tools
such as elevation maps (EMs) and triangulated irregular networks (TINs). EMs [ 15,
34] are 2D grid-based structures that model 3D space in terms of a scalar estimate
corresponding to the height of each cell, an arrangement also known as a 2.5D
representation. Their main advantage is simplicity, however this comes at the cost
V . C. Guizilini ( B)·F. T. Ramos
School of Information Technologies, The University of Sydney, Sydney, Australia
e-mail: vitor.guizilini@sydney.edu.au
F. T. Ramos
e-mail: fabio.ramos@sydney.edu.au
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_33421
422 V . C. Guizilini and F. T. Ramos
of drawbacks such as: inability to handle abrupt elevation changes [ 35]; lack of a
statistically proper way to handle spatial correlation or measurement uncertainty [ 16];
and scalability issues to larger datasets [ 9]. Over the years, these drawbacks have
been partially addressed in several ways, such as [ 34], that proposes an extension
capable of handling multiple surfaces and hanging objects, and [ 7,16], that attempts
to incorporate uncertainty estimates in the EM model. TINs [ 28], on the other hand,
sample a set of points that capture important aspects of the observed terrain surface,
which are then connected to their nearest neighbors to produce a triangular network
model. While better able to capture sudden elevation changes and more efﬁcientwhen dealing with ﬂat areas, TINs struggle with dense sensor data [ 34], due to the
huge memory footprint produced by highly textured surfaces, and are still unable to
formally incorporate spatial dependencies and measurement uncertainty.
The use of Gaussian Processes (GPs) [ 27], a powerful Bayesian non-parametric
regression learning technique, for terrain modeling has become popular in recent
years, because they are able to handle most of the above mentioned issues. Theyprovide a continuous elevation function that can be sampled at arbitrary resolutions,
incorporate uncertainty in an statistically sound way and represent spatially corre-
lated data in an appropriate manner. Essentially, a GP implements an interpolation
technique commonly used in statistics, called Krigging [18], which is the best linear
unbiased estimate based on the underlying stochastic model of spatial correlationbetween data points (further discussion on other interpolation techniques for grid-
based methods can be found in [ 14]). The standard GP framework, however, scales
cubically with the number of training points, thus rendering it computationally tooexpensive for large-scale datasets, and most kernel covariance functions are too
smooth to correctly model terrain details and sudden discontinuities. Non-stationary
covariance functions [ 23,35] can be used to produce variable smoothness throughout
the input space, and sparse approximations [ 24,25,33] reduce computational com-
plexity by projecting data into a subset of inducing points, or using GP ensembles.
In this paper we propose a novel regression methodology for terrain modeling that
operates on a Reproducing Kernel Hilbert Space (RKHS) [ 30], generated by the pro-
jection of input data into a high-dimensional feature vector. In this high-dimensional
space, real world complexity can be represented using very simple models, whichresults in efﬁcient training and query times. Although this approach has already been
explored before, most notably in [ 36] for a functional regression setting and in [ 12]
for rough terrain estimation using space-carving kernels, here we develop a fullyprobabilistic sparse approximation that is suitable for online learning and can be
readily applied to large-scale datasets. Variational Bayesian inference is used for
the probabilistic treatment of variables, thus providing a measure of uncertainty that
quantiﬁes the model’s conﬁdence in relation to its own estimates. The compact sup-
port kernel introduced in [ 10] serves to decouple different dimensions of the RKHS,
which enforces non-stationarity in the input space and allows the optimization of
different localized regression models, while still maximizing a single global varia-
tional bound. Furthermore, we show how kernel parameters can be jointly learnedalongside regression model parameters, to achieve a better variational approximation
Variational Hilbert Regression with Applications … 423
of the underlying function. Finally, we test the proposed methodology in two large-
scale unstructured real datasets, and show that it outperforms other commonly used
terrain modeling techniques both in terms of accuracy and computational speed.
2 Methodology
We assume a training dataset D={ X,y}composed of Nobserved input points
X={ xn}N
n=1, with xn∈RD, and their corresponding output values y={ yn}N
n=1,
with yn∈R. The objective is to produce a continuous function g(.)that approximates
a hypothetical underlying latent function f(.), from which Dwas sampled. This latent
function can be deﬁned as:
yn=f(xn)+ε=wTxn+ε,( 1 )
where εis the system noise and wis the parameter vector, that is optimized based
on the information contained in Dand deﬁnes the regression model. For a standard
linear regression model, wis a D+1 column vector and xnis augmented so that ˜xn=
{1,x1
n,x2
n,..., xD
n}. The use of basis functions can further improve its predictive
powers [ 2], however for notation simplicity here we assume a non-biased linear
regression model.
2.1 Bayesian Inference
We start by modeling εas a zero-mean Gaussian distribution with variance σ2
ε, i.e.
ε∼N(0,σ2
ε). The probability of an output y, given the corresponding input x, model
parameters wand noise variance σε, is given by:
p(y|x,w,σ2
ε)=N(y|wTxn,σ2
ε)=1/radicalbig
2πσ2εexp/parenleftbigg1
2σ2ε(yn−wTxn)2/parenrightbigg
.( 2 )
Furthermore, if we consider the entire training dataset Dand assume they are
sampled independently from f(.), the log-likelihood function can be expressed as:
lnp(y|X,w,σ2
ε)=N/productdisplay
n=1lnN(y|wTxn,σ2
ε)=N
2lnβ−N
2ln(2π)−βE(w),
(3)
where β=σ−2
εis the noise precision and E(w)=1
2/summationtextN
n=1/parenleftbig
yn−wTxn/parenrightbig2is the sum
of squared errors loss function. Based on the above equation, we can conclude thatmaximizing this log-likelihood is equivalent to minimizing the sum of squared errors
424 V . C. Guizilini and F. T. Ramos
between model estimates wTxnand ground-truth information yn. The gradient of
Eq.3is given by:
∇lnp(y|X,w,σ2
ε)=N/summationdisplay
n=1/parenleftbig
yn−wTxn/parenrightbig
xT
n.( 4 )
Setting this gradient to zero and solving for w, we obtain the optimal parameters
aswML=(XTX)−1XTy. However, this direct optimization has the known effect of
inducing over-ﬁtting if model complexity is not carefully selected [ 2], since sim-
ply maximizing the likelihood function will always lead to excessively complexmodels. Because of that, usually a Bayesian formulation is used [ 4], that is able
to automatically determine model complexity based on training data alone. This
is done by placing a prior distribution on the model parameters themselves, suchthat p(w)=N(w|μ
0,S0). To facilitate the treatment, we can assume a zero-mean
distribution and a single precision parameter α, so that p(w|α)=N(w|0,α−1I)
The posterior distribution p(w|y)=N(w|μt,St), calculated using the information
contained in D, has the following mean and variance values:
μt=St/parenleftbig
S−1
t−1μt−1+βXTy/parenrightbig
=βStXTy (5)
S−1
t=S−1
t−1+βXTX=αI+βXTX.( 6 )
It can be shown [ 2] that maximizing this new formulation with respect to wis
equivalent to minimizing the sum of squared errors with the addition of a quadratic
regularization termλ
2wTw, with λ=α/β.
2.2 V ariational Bayesian Approximation
The explicit calculation of these distributions quickly becomes intractable as models
get more complex, particularly when hyper-parameters such as αandβare integrated
alongside model parameters w. Because of that, considerable work has been done in
approximation techniques, such as Markov Chain Monte Carlo [ 8] and Variational
Bayesian [ 2,6,21] methods. Here we will focus on Variational Bayesian methods,
that introduce a new distribution q(.)that is optimized to approximate p(.), such
that p(Z|X)≈q(Z)according to a distance metric D(q,p), usually the Kullback-
Leibler (KL) divergence, deﬁned as:
DKL(q||p)=/summationdisplay
Zq(Z)logq(Z)
p(Z|X).( 7 )
The prior on wandβis a conjugate normal inverse-gamma distribution param-
eterized on α, deﬁned as p(w,β|α)=N(w|0,(α β)−1I)Gam(β|a0,b0). Similarly,
Variational Hilbert Regression with Applications … 425
αreceives the hyper-prior p(α)=Gam(α|c0,d0). There is no analytic solution to
these posteriors, due to the introduction of hyper-priors, so a variational treatment
becomes necessary. The variational bound to be maximized is:
L(q)=/integraldisplay/integraldisplay/integraldisplay
q(w,α,β) lnp(y|X,w,β) p(w,β|α)p(α)
q(w,α,β)dwdαdβ≤lnp(D),
(8)
where p(D)is the model evidence. To maximize this bound, we assume the varia-
tional distribution q(w,α,β) , which approximates the posterior p(w,α,β|D), can
be factored as q(w,β) q(α). The variational posterior for wandβthat maximizes
this variational bound, while keeping q(α)ﬁxed, is given by:
lnq∗(w,β)=lnN(w|wt,β−1Vt)Gam(β|at,bt),( 9 )
with:
at=at−1+N
2,bt=bt−1+1
2/parenleftBigg/summationdisplay
n(yn−wT
nxn)2+ct
dtwT
twt/parenrightBigg
,
V−1
t=ct
dtI+/summationdisplay
nxnxT
n,wt=Vt/summationdisplay
nxnyn.( 1 0 )
Similarly, the variational posterior for α, while keeping q(w,β) ﬁxed, is given
by:
lnq∗(α)=ln Gam (α|ct,dt), (11)
with:
ct=ct−1+D
2,dt=dt−1+1
2/parenleftbiggat
btwT
tw+Tr(Vt)/parenrightbigg
.( 1 2 )
The complete variational bound can then be rearranged to assume the form:
L(q)t=−N
2ln 2π−1
2/summationdisplay
n/parenleftbiggat
bt(yn−wT
txn)2+xT
nVtxn/parenrightbigg
+1
2ln|Vt|+D
2
−lnΓ(at−1)+at−1lnbt−1−bt−1at
bt+lnΓ(at)−atlnbt+at
−lnΓ(ct−1)+ct−1lndt−1+lnΓ(ct)−ctlndt. (13)
This bound is maximized by iteratively updating over Vt,wt,at,bt,ctand dt
until L(q)converges. The predictive distribution is evaluated by approximating the
posterior p(w,β|D)by its variational counterpart q(w,β), such that:
p(y|x,D)≈St/parenleftbig
y|wT
tx,(1+xTVtx)−1,2at/parenrightbig
,( 1 4 )
426 V . C. Guizilini and F. T. Ramos
which is a Student’s t-distribution with mean wT
tx, precision (1+xTVtx)−1at/btand
2atdegrees of freedom. This variational Bayesian formulation lends itself naturally
to online learning, since new information can be incorporated as the iterative processdescribed previously is performed. Several extensions that further improve online
learning have also been proposed, such as a forgetting scheme that allows its use in
dynamic environments [ 13,29] and asynchronous streaming support [ 3].
2.3 V ariational Hilbert Regression
The Hilbert Maps (HM) framework, introduced in [ 26], proposes the modeling of
arbitrarily complex functions by projecting input coordinates xinto a Reproducing
Kernel Hilbert Space (RKHS) [ 31], using a feature vector Φ(x). The dot product
of these feature vectors can be used to approximate popular kernels found in theliterature [ 27], and by operating only in terms of kernel evaluations it is possible
to avoid calculations in this high-dimensional (and possibly inﬁnite) feature space.
The original implementation, alongside subsequent extensions [ 5,10,11], focus on
classiﬁcation tasks, such as occupancy mapping, and here we propose what is to
the best of our knowledge the ﬁrst application of the Hilbert Maps framework forregression tasks.
In [10] a sparse derivation was developed, which used clustering to place induc-
ing points throughout observed areas of the input space, that served as anchors fordifferent dimensions of the feature vector. Sparseness is achieved using the kernel
proposed in [ 19], which vanishes exactly to zero after a certain distance threshold.
The feature vector Φ(x)and sparse kernel k(x,M)are deﬁned as:
Φ(x,M)=⎡
⎢⎢⎢⎣k(x,M
1)
k(x,M2)
...
k(x,M M)⎤
⎥⎥⎥⎦,k(x,M
i)=/braceleftBigg
2+cos(2πri)
3(1−ri)+1
2πsin(2πri)ifri<1
0i f ri≥1,
(15)
where M={ M i}M
i=1={μi,Σ i}M
i=1is the cluster set extracted from D, each one con-
taining mean μand covariance Σvalues calculated from their corresponding input
points subset and used to obtain the distance threshold ri=/radicalBig
(x−μi)TΣ−1
i(x−μi).
Efﬁcient data storage structures, such as kd-trees [ 20], can be used to quickly search
for clusters within this threshold, thus eliminating irrelevant kernel calculations.
For the proposed regression methodology, each cluster is augmented to include a
parameter vector θ={ w,V,a,b,c,d}, such that M i={μ,Σ,θ}i. Note that each
cluster has its own regression model, that is initialized empty (i.e. with zeros) and
updated as new information is observed. Assuming that each new observation {x,y}∗
is drawn independently from the underlying function f(.)and belongs to a single
cluster, the global variational bound can be expressed as the sum L(M)=/summationtext
iL(qi)
(i.e. the feature vector Φ(x∗,M)is orthogonal), with each local component described
Variational Hilbert Regression with Applications … 427
by Eq. 13. To enforce sparsity during updates, and maintain non-stationarity in dif-
ferent areas of the input space, these components are scaled by the corresponding
feature vector Φ(x∗,M), so that:
L(M)t+1=L(M)t+max
Θ/bracketleftBiggM/summationdisplay
i=1Φi(x∗,M)/parenleftBig
L(M i)t+1−L(M i)t/parenrightBig/bracketrightBigg
, (16)
where Θrepresents all model parameters {θ}M
i=1, that are optimized to maximize
the increase in the global variational bound from timestep t(prior to new data
incorporation) to t+1 (after new data incorporation). The intuition is that points
closer to a cluster should contribute with a larger rate of change for that particular
model. Similarly, parameters of clusters with no points within its distance threshold(i.e.Φ
i(x,M)=0) remain unchanged, since there is no relevant information to be
incorporated. Note that multiple points can be incorporated simultaneously, by opti-
mizing Eq. 16with the summed contribution of each new observation scaled by their
corresponding feature vector.
Prediction is performed in a similar way, using the approximated distribution in
Eq.14to generate mean μ∗and variance ν2
∗estimates for any query point x∗. Each
cluster contributes with one estimate, that is scaled according to its corresponding
feature vector Φ(x,M)dimension and normalized by the total sum of the feature
vector, so that nearby clusters contribute more strongly to the ﬁnal estimate:
μ∗=1
||Φ(x∗,M)||1M/summationdisplay
i=1Φ(x∗,M i)wT
ix∗ (17)
ν2
∗=1
||Φ(x∗,M)||1M/summationdisplay
i=1Φ(x∗,M i)bi
ai(1+xT
∗Vix∗) (18)
A consequence of the sparseness enforced by Φis that query points outside the
distance threshold of all clusters will not be considered by any model, which results
in an ill-deﬁned behavior in Eqs. 17and18. A solution to this scenario is to set a
minimum number of nearest clusters whose models will be considered, regardless of
their distance thresholds. In the experiments we used a simple heuristic in which ri/¯r
nearest neighbors are considered, where riis the distance threshold to the nearest
cluster and ¯ris the average distance between clusters. This formulation ensures that
more clusters are used to estimate areas further away, thus producing smoother results
that do not depend on any model in particular, while nearby regions still retain theirsharpness, since fewer clusters are used for estimates.
428 V . C. Guizilini and F. T. Ramos
2.4 Joint Kernel Learning
Given an initial dataset D, several different clustering techniques [ 1,17,32] can be
used to generate cluster centers, and by extension covariance values. In this workwe employ the ASK-Means algorithm described in [ 11], because it produces similar
cluster densities and can automatically determine how many clusters are necessary to
properly describe input data. Once the kernel parameters {μ,Σ}
M
i=1are generated, the
methodology described previously is used to separately learn the regression model
parameters Θ={θ}M
i=1that together compose the Variational Hilbert Regression
model.
In this section we brieﬂy discuss a few techniques that enable the joint learning
of kernel and regression model parameters {μ,Σ,θ}M
i=1, as a way to further increase
L(M)values during the training process or decrease model complexity without
signiﬁcantly compromising results.
2.4.1 Switching
Switching refers to changing the index of a given training point xk, so it belongs
to a new cluster. By extension, this change also modiﬁes the kernel parameters of
both clusters, which can be performed efﬁciently using routines for incrementalcalculation of mean and variance [ 22]. The maximization process for the global
variational bound now includes the cluster selection for x
k, and is of the form:
L(C)t+1=L(C)t+max
Θ,C⎡
⎣C/summationdisplay
j=1Φj(x,C)/parenleftBig
L(Cj)t+1−L(Cj)t/parenrightBig⎤
⎦, (19)
where Crepresents the subset of clusters considered to receive xk(note that the
remainder of Mis not considered here, since these clusters are not affected by the
switching process). As a way to avoid excessive computational costs, switching is
triggered by calculating the residual error ζi
k=(μi
k−yk)2ofxkin relation to its
current cluster M i. If this value is above a certain error threshold, its nearby clusters
(i.e. within distance threshold) are considered as the subset Cin Eq. 19.
2.4.2 Splitting
Splitting refers to separating one cluster into two or more, each one receiving a
subset of its corresponding points. This is particularly useful when dealing with more
complex areas, with shapes that cannot be accurately captured by the regression
Variational Hilbert Regression with Applications … 429
model.1The splitting process does not affect any other clusters, and therefore its
maximization process can be simpliﬁed to:
L(M k)t+1=L(M k)t+max
Θ,C⎡
⎣C/summationdisplay
j=1Φj(x,C)/parenleftBig
L(Cj)t+1−L(M k)t/parenrightBig⎤
⎦,( 2 0 )
where M kdenotes the cluster to be split, and Cis the subset of clusters produced
from its corresponding points. Note that this process introduces a new dimension to
the feature vector Φ, thus increasing the overall complexity of the underlying model.
To avoid over-ﬁtting, new clusters are only generated if assigned a minimum number
of points, which can be calculated based on average point density for different areas
of the input space. As a way to avoid excessive computational costs, splitting istriggered by calculating the average residual value ¯ζ
i=1
k/summationtext
kζi
kof all kpoints in
M iin relation to its own regression model. If this value is above a certain threshold,
its points are considered for the generation of sub-clusters Cin Eq. 20.
2.4.3 Merging
Merging refers to combining two or more clusters into a single one, with the same
kernel and regression parameters. Differently from previous techniques, merging
does not attempt to maximize the global variational bound, since it decreases the
complexity of the underlying model. However, it can determine which clusters canbe safely removed without degrading estimates above a given error threshold, thus
promoting a trade-off between accuracy and speed that can be useful for certain
applications. Pseudo-code for the checking and merging of a cluster M
i, given an
error threshold ζmax, is shown in Algorithm 1, and its effects on terrain mapping
results can be seen in Fig. 3.
3 Experimental Results
In this section the proposed Variational Hilbert Regression (VHR) is used to address
the problem of terrain modeling, in which 2D inputs x={ x,y}N
n=1are mapped into a
probability distribution f(x)=N(μ,ν2)for elevation values. Two different large-
scale publicly available datasets were considered, with VHR results being comparedto both standard linear (LI) and cubic (CI) interpolation techniques and also a GP
framework for the modeling of large-scale terrains, as shown in [ 35].
1The proposed framework can be trivially extended to support different regression models for each
cluster, including the learning of which regression models should be used, however this was notexplored in this paper.
430 V . C. Guizilini and F. T. Ramos
Algorithm 1: Pseudo-Code for cluster merging within the VHR framework.
input :M: Cluster Set
i: Index of cluster considered for merging
output :M: Updated cluster set
1C← nearest (M i)% Find nearest neighbors
2for CjinCdo
3ζj
i=1
U/radicalbigg
/summationtext
u/parenleftBig
μj
u−yu/parenrightBig2
,{x,y}u∈M i% Residual error of M iin relation to Cj
4ζi
j=1
V/radicalBig/summationtext
v/parenleftbig
μiv−yv/parenrightbig2,{x,y}v∈Cj % Residual error of Cjin relation to M i
5 if/parenleftBig
ζj
i+ζi
j/parenrightBig
/2<ζ max then
6 M i← Cj% Merge clusters
7 end
8end
9{μ,Σ,θ}← M i% Update cluster kernel and model parameters
The ﬁrst dataset considered here was obtained from the website http://asrl.
utias.utoronto.ca/datasets/3dmap/a100\_dome.html , and is entitled Rover . It contains
252616 points obtained from a laser rangeﬁnder mounted on top of a panning unit,
to produce 3D slices of the surrounding environment. This dataset covers an area of
roughly 30 m ×30 m, however due to terrain obstructions a large portion remains
unobserved, as it can be seen in Fig. 1a, where the 1589 clusters generated from
the original pointcloud are depicted. Figure 1b presents the terrain modeling results
obtained using a GP framework, based on the work of [ 35]. Due to the number of
training points, a sparse approximation was necessary [ 33], in which cluster cen-
ters were used as inducing points to decrease computational cost during training
and inference. Similarly, terrain modeling results obtained using the proposed VHR
framework are depicted in Fig. 2, also colored by variance.
As expected, areas close to training points have lower variance values (shades
of blue), and as we move away from these observed areas variance values steadily
(a) 1589 Clusters generated from the original
pointcloud (252616 points).
(b) Sparse GP results (mean estimates, colored
by variance).
Fig. 1 Terrain modeling results for the Rover dataset using a Sparse GP framework. White dots
indicate the points used to train the regression model
Variational Hilbert Regression with Applications … 431
Fig. 2 Terrain modeling results for the Rover dataset using the proposed framework (colored by
variance). White dots indicate the points used to train the regression model
increases (shades of red, until saturation). Interestingly, in the VHR framework vari-
ance values vary even in areas close to training points, according to the accuracy ofnearby local regression models that contribute to the feature vector. As observed data
diverges from current estimates, the resulting variance values increase (i.e. different
shades of blue on the bottom left image in Fig. 2), which might lead to switching
and splitting, as detailed in Sects. 2.4.1 and2.4.2 respectively. It is also worth noting
that the model produced by the Sparse GP (SGP) framework is much smoother, and
therefore does not capture ﬁne details in well-deﬁned areas of the input space. Thelocalized aspect of the VHR framework, on the other hand, produces variable smooth-
ness levels, that maintains the sharpness in well-deﬁned areas while smoothing out
areas with higher variance. Additionally, the VHR framework does not converge
back to a single mean value as it moves away from observed data, but maintains
the behavior of nearby structures as dictated by their corresponding local regres-sion models, which tends to produce a visually better representation of unobserved
terrains.
The second dataset, entitled Aerial , was obtained from the website http://www.
pointcab-software.com/en/downloads/ . It contains 39460480 points collected by an
UA V ﬂying over a construction site, using a camera sensor. It covers an area of
432 V . C. Guizilini and F. T. Ramos
roughly 140 m ×140 m, with well-distributed training points throughout the entire
input space (Fig. 3a). The residual error for each training point, in relation to the
trained VHR model, is depicted in Fig. 4a, where we can see that most of this residual
error is concentrated in a few points (in fact, 90% of it is attributed to 47854 points,
only 1 .34% of the total). Figure 3also depicts VHR terrain modeling results using
different number of clusters, which is achieved by varying the error threshold ζmax
(see Algorithm 1). If ζmax=0 all clusters are considered, and as we increase this
value more clusters are merged together to decrease the complexity of the resulting
regression model. The corresponding degradation in terms of accuracy is shown inFig.4b, where we can see that the merging technique proposed in Sect. 2.4.3 is able
to detect and remove clusters with lower impact on the overall regression model
(a decrease of 30% in the number of clusters produces an RMSE increase of only3.88%). Naturally, at some point there are no more low impact clusters left, and further
removals will start to have a more signiﬁcant effect on overall model accuracy.
Table 1presents quantitative results for different terrain modeling techniques when
applied to both datasets, as data becomes sparser.
2For 100%, all points are used for
training and residual error calculation. For other levels, that percentage of points
(determined randomly) is used solely for training purposes and the remaining ones
only for residual error calculation (computational times for inference were calculated
based on a 0.1 m resolution grid). The RMSE results testify to each algorithm’s abilityto extrapolate over available data to infer the state of unobserved areas of the input
space. As expected, standard interpolation techniques are unable to deal with sparser
data, with the residual error rapidly increasing as more training points are removed.This increase is signiﬁcantly smaller for the SGP and VHR frameworks, indicating
better interpolative powers, however VHR consistently outperforms SGP in terms of
residual error.
The computational times presented in Table 1validate the VHR framework’s abil-
ity to scale to larger datasets, especially when compared to the non-parametric SGP
framework. As expected, interpolation techniques are much faster, since no train-ing is required, only linear and cubic calculations based on nearest neighbors. The
SGP framework, even when using a sparse approximation, required substantially
more training time (by several orders of magnitude) to generate optimized modelparameters, and inference was also noticeably slower. We attribute this increase in
performance provided by the VHR framework to the decoupling between different
local regression models, which produces efﬁcient parameter updates during varia-tional training. In [ 35] a similar local approximation using kd-trees is proposed as a
way to maintain computational complexity low and bounded, which would further
decrease SGP computational times, however this was not explored in this paper.
Regardless, RMSE results for the VHR framework are consistently smaller, which
makes it a more attractive terrain modeling technique both in terms of accuracyand computational power. Interestingly, for the VHR framework, as the number of
2All computations were performed on a i7/2.60 x 8 GHz notebook, with multi-threading enabled
wherever possible. Due to lack of memory, training data was downsampled by 5 in the Rover dataset
a n db y2 5i nt h e Aerial dataset for tests using the SGP framework.
Variational Hilbert Regression with Applications … 433
(a) Original pointcloud
(39460480 points)(b) 30895 clusters
(max =0.000)
(c) 15131 clusters
(max =0.005)(d) 9580 clusters
(max =0.010)
(e) 1082 clusters
(max =0.050)(f) 537 clusters
(max =0.100)
Fig. 3 Terrain modeling results for the Aerial dataset using the proposed VHR framework (colored
by height), with different number of clusters. Black dots indicate cluster centers
434 V . C. Guizilini and F. T. Ramos
(a) L1 error for training points, in descending
order.(b) RMSE with different number of clusters.
Fig. 4 Training error results under different circumstances using the proposed VHR framework,
for the Aerial dataset (see Fig. 3)
Ta b l e 1 Quantitative results for different terrain modeling techniques, with varying levels of data
sparsity
Metric Sparsity (%) Rover Aerial
LI CI SGP VHR LI CI SGP VHR
RMSE (m) 100 0.201 0.211 0.186 0.149 0.101 0.113 0.087 0.054
90 0.211 0.217 0.194 0.152 0.122 0.144 0.095 0.057
70 0.234 0.263 0.217 0.162 0.165 0.198 0.104 0.059
50 0.298 0.324 0.239 0.171 0.203 0.249 0.115 0.065
Training time (s) 100 – – 4705.2 0.975 – – 31936.3 9.266
90 – – 3573.8 0.825 – – 23216.7 7.172
70 – – 1985.1 0.760 – – 16759.1 5.500
50 – – 1001.7 0.495 – – 10845.8 3.938
Inference time (s) 100 0.602 0.643 46.340 1.587 2.748 3.382 807.432 8.694
90 0.361 0.490 42.945 1.423 2.470 2.865 498.374 8.019
70 0.303 0.368 37.296 1.234 1.578 1.978 259.128 7.273
50 0.217 0.276 28.254 1.092 0.934 1.107 168.109 6.558
training points decreases inference times become longer when compared to training
times, due to the large number of query points necessary to produce a reconstructed
model of the surveyed area, given the requested resolution.
Variational Hilbert Regression with Applications … 435
4 Conclusion
This paper introduces a novel regression methodology based on sparse projections to
a Reproducing Kernel Hilbert Space. Observed data is clustered and different localregression models are attributed to each cluster, thus allowing non-stationary behav-
iors throughout the input space and decreasing overall computational complexity.
We employ a Variational Bayesian approach to uncertainty estimation, and train-ing is conducted via the maximization of a single global variational bound that is
dependent on the parameters for each local regression model. Furthermore, we show
how kernel and model parameters can be jointly optimized, to produce a more accu-rate approximation to the underlying function or to further decrease computational
costs by removing unnecessary clusters. The proposed VHR framework is tested in
two different large-scale terrain modeling datasets, with results that surpass other
commonly used terrain modeling techniques. Future work will focus on the use of
different regression models and kernels, such as the space-carving kernel proposedin [12], and the active selection of regression models given local point distribution.
Acknowledgements This research was supported by funding from the Faculty of Engineering and
Information Technologies, The University of Sydney, under the Faculty Research Cluster Program.
References
1. Arthur, D., Vassilvitskii, S.: K-means++: the advantages of careful seeding. In: Proceedings of
the ACM-SIAM Symposium on Discrete Algorithms (SODA), pp. 1027–1035 (2007)
2. Bishop, C.: Pattern Recognition and Machine Learning (Information and Statistics). Springer,
New York (2006)
3. Broderick, T., Boyd, N., Wibisono, A., Wilson, A., Jordan, M.: Streaming variational Bayes.
In: Advances in Neural Information Processing Systems (NIPS), pp. 1727–1735 (2013)
4. Carlin, B., Louis, T.: Bayesian Methods for Data Analysis, 3rd edn. Chapman and Hall, London
(2008)
5. Doherty, K., Wang, J., Englot, B.: Probabilistic map fusion for fast, incremental occupancy
mapping with 3d Hilbert maps. In: Proceedings of the IEEE International Conference onRobotics and Automation (ICRA) (2016)
6. Drugowitsch, J.: Variational Bayesian inference for linear and logistic regression. Technical
report (2013). arXiv:1310.5438
7. Frankhauser, P., Bloesch, M., Gehring, C., Hutter, M., Siegwart, R.: Robot-centric elevation
mapping with uncertainty estimates. In: Proceedings of the International Conference on Climb-ing and Walking Robots (CLAW AR) (2014)
8. Gilks, W., Richardson, S., Spiegelhalter, D.: Markov Chain Monte Carlo in Practice, 1st edn.
Chapman and Hall, London (1995)
9. Gu, J., Cao, Q., Huang, Y .: Rapid traversability assessment in 2.5d grid-based map on rough
terrain. Int. J. Adv. Robot. Syst. 5(4) (2008)
10. Guizilini, V ., Ramos, F.: Large-scale 3d scene reconstruction with Hilbert maps. In: Proceedings
of the IEEE International Conference on Intelligent Robots and Systems (IROS) (2016)
11. Guizilini, V ., Ramos, F.: Unsupervised feature learning for 3d scene reconstruction with occu-
pancy maps. In: Proceedings of the AAAI Conference on Artiﬁcial Intelligence (2017)
12. Hadsell, R., Bagnell, J., Huber, D., Hebert, M.: Space-carving kernels for accurate rough terrain
estimation. Int. J. Robot. Res. (IJRR) 29(8), 981–996 (2010)
436 V . C. Guizilini and F. T. Ramos
13. Honkela, A., Valpola, H.: On-line variational bayesian learning. In: International Symposium
on Independent Component Analysis and Blind Signal Separation (ICA) (2003)
14. Kidner, D., Dorey, M., Smith, D.: What’s the point? interpolation and extrapolation with a
regular grid dem. In: Proceedings of the International Conference on Geocomputation (1999)
15. Krotkov, E., Hoffman, R.: Terrain mapping for a walking planetary rover. IEEE Trans. Robot.
Autom. 10(6), 278–292 (1994)
16. Lacroix, S., Mallet, A., Bonnafous, D., Bauzil, G., Fleury, S., Herrb, M., Chatila, R.:
Autonomous rover navigation on unknown terrains: functions and integration. Int. J. Robot.Res. (IJRR) 21(10), 917–942 (2002)
17. Lloyd, S.: Least-squares quantization in PCM. IEEE Trans. Inf. Theory 28, 129–136 (1982)
18. Matheron, G.: Principles of geostatistics. Econ. Geol. 58, 1246–1266 (1963)
19. Melkumyan, A., Ramos, F.: A sparse covariance function for exact Gaussian process inference
in large datasets. In: Proceedings of the International Joint Conference on Artiﬁcial Intelligence(IJCAI), pp. 1936–1942 (2009)
20. Muja, M., Lowe, D.: Fast approximate nearest neighbours with automatic algorithm con-
ﬁguration. In: Proceedings of the International Conference on Computer Vision Theory andApplications (VISAPP), vol. 4, pp. 331–340 (2009)
21. Murphy, K.: Machine Learning: A Probabilistic Perspective, 1st edn. The MIT Press, Cam-
bridge (2012)
22. Pébay, P.: Formulas for robust, one-pass parallel computation of covariances and arbitrary-order
statistical moments. Technical report, Sandia National Laboratories (2008)
23. Plagemann, C., Kersting, K., Burgard, W.: Nonstationary Gaussian process regression using
point estimates of local smoothness. In: Proceedings of the European Conference on Machine
Learning (ECML) (2008)
24. Plagemann, C., Mischke, S., Prentice, S., Kersting, K., Roy, N., Burgard, W.: Learning pre-
dictive terrain models for legged robot locomotion. In: Proceedings of the IEEE InternationalConference on Intelligent Robots and Systems (IROS) (2008)
25. Quinonero-Candelas, J., Rasmussen, C.: An unifying view of sparse approximate Gaussian
process regression. J. Mach. Learn. Res. (JMLR) 6, 1939–1959 (2005)
26. Ramos, F., Ott, L.: Hilbert maps: scalable continuous occupancy mapping with stochastic
gradient descent. In: Proceedings of Robotics: Science and Systems (RSS) (2015)
27. Rasmussen, C., Williams, C.: Gaussian Processes for Machine Learning. The MIT Press,
Cambridge (2005)
28. Rekleitis, I., Bedwani, J., Gingras, D., Dupuis, E.: Experimental results for over-the-horizon
planetary exploration using a LIDAR sensor (2008)
29. Sato, M.: Online model selection based on the variational Bayes. Neural Comput. 13(7), 1649–
1681 (2001)
30. Schölkopf, B., Muandet, K., Fukumizu, K., Harmeling, S., Peters, J.: Computing functions of
random variables via reproducing kernel Hilbert space representations. Stat. Comput. 25(4),
755–766 (2015)
31. Schölkopf, B., Smola, A.: Learning with Kernels: Support Vector Machines, Regularization,
Optimization, and Beyond. The MIT Press, Cambridge (2001)
32. Sculley, D.: Web-scale k-means clustering. In: Proceedings of the International Conference on
World Wide Web (WWW), vol. 19, pp. 1177–1178 (2010)
33. Snelson, E., Ghahramani, Z.: Sparse Gaussian processes using pseudo-inputs. In: Advances in
Neural Information Processing Systems (NIPS), pp. 1257–1264 (2006)
34. Triebel, R., Pfaff, P., Burgard, W.: Multi-level surface maps for outdoor terrain mapping and
loop closing. In: Proceedings of the IEEE International Conference on Intelligent Robots andSystems (IROS) (2006)
35. Vasudevan, S., Ramos, F., Nettleton, E., Durrant-Whyte, H.: Gaussian process modeling of
large-scale terrain. J. Field Robot. (JFR) 26(10), 812–840 (2010)
36. Yuan, M., Cai, T.: A reproducing kernel Hilbert space approach to functional linear regression.
Ann. Stat. 38(6), 3412–3444 (2010)
ADAPT: Zero-Shot Adaptive Policy
Transfer for Stochastic Dynamical
Systems
James Harrison, Animesh Garg, Boris Ivanovic, Y uke Zhu, Silvio Savarese,
Li Fei-Fei and Marco Pavone
1 Introduction
Deep reinforcement learning (RL) has achieved remarkable advances in sequentialdecision making in recent years, often outperforming humans on tasks such as Atarigames [ 17]. However, model-free variants of deep RL are not directly applicable
to physical systems because they exhibit poor sample complexity, often requiring
millions of training examples on an accurate model of the environment. One approach
to using model-free RL methods on robotic systems is thus to train in a relatively
accurate simulator (a source domain), and transfer the policy to the physical robot
J. Harrison ( B)
Department of Mechanical Engineering, Stanford University,Stanford, CA 94305, USAe-mail: jharrison@stanford.edu
A. Garg ·B. Ivanovic ·Y. Z h u ·S. Savarese ·L. Fei-Fei
Department of Computer Science, Stanford University,Stanford, CA 94305, USAe-mail: garg@cs.stanford.edu
B. Ivanovic
e-mail: borisi@cs.stanford.edu
Y. Z h u
e-mail: yukez@cs.stanford.edu
S. Savarese
e-mail: ssilvio@cs.stanford.edu
L. Fei-Fei
e-mail: feifeili@cs.stanford.edu
M. Pavone
Department of Aeronautics and Astronautics, Stanford University,Stanford, CA 94305, USAe-mail: pavone@stanford.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_34437
438 J. Harrison et al.
(a target domain). This naive transfer may, in practice, perform arbitrarily badly and
so online ﬁne-tuning may be performed [ 1]. During this ﬁne-tuning, the robot may
behave unsafely however, and so it is desirable for a system to be able to train ina simulator with slight model inaccuracies but still be able to perform well on the
target system on the ﬁrst iteration. We refer to this as the zero-shot policy transfer
problem .
The zero-shot transfer problem involves training a policy on a system possess-
ing different dynamics than the target system, and evaluating performance as the
average initial return in target domain without training in the target domain. This
problem is challenging for robotic systems since simpliﬁed simulated models may
not always accurately capture all relevant dynamics phenomena, such as friction,
structural compliance, turbulence and so on, as well as parametric uncertainty in themodel. In spite of the renewed focus on this problem, few studies in deep policy
adaptation offer insightful analysis or guarantees regarding feasibility, safety, and
robustness in policy transfer.
In this paper, we introduce a new algorithm which we refer to as AdaPT , that
achieves provably safe and robust, dynamically-feasible zero-shot direct transfer of
RL policies to new domains with dynamics mismatch. The key insight here is to
leverage the global optimality of learned policy with local stabilization from MPC
based methods to enable dynamic feasibility, thereby building on strengths of twodifferent methods. In the ofﬂine stage, AdaPT ﬁrst computes a nominal trajectory
(without disturbance) by executing the learned policy on the simulator dynamics.
Then in the online stage, AdaPT adapts the nominal trajectory to the target dynamics
with an auxiliary MPC controller.
Statement of Contributions
1. We develop the AdaPT algorithm, which allows online transfer of policy trained
solely in a simulation ofﬂine, to a family of unknown targets without ﬁne-tuning.
2. We also formally show that (i) AdaPT guarantees state and control safety through
state-action tubes under the assumption of Lipschitz continuity of the divergence
in dynamics and, (ii) AdaPT results in a bounded loss of reward accumulation
in case of direct transfer with AdaPT as compared to a policy trained only on
target.
3. We evaluate AdaPT on two continuous, non-holonomic simulated dynamical
systems with four different disturbance models, and ﬁnd that AdaPT performs
between 50 and 300% better on mean reward accrual than direct policy transfer
as compared to mean reward.
Organization This paper is structured as follows. In Sect. 2we review related work
in robust control, robust reinforcement learning, and transfer learning. In Sect. 3we
formally state the policy transfer problem. In Sect. 4we present AdaPT and discuss
algorithmic design features. In Sect. 5we prove the accrued reward for AdaPT
is lower bounded. In Sect. 6we present experimental results on a simulated car
environment and a two-link robotic manipulator, as well as present results for AdaPT
with robust policy learning methods. Finally, in Sect. 7we draw conclusions and
discuss future directions.
AdaPT : Zero-Shot Adaptive Policy Transfer … 439
2 Related Work and Background
A plethora of work in both learning and control theory has addressed the problem of
varying system dynamics, especially in the context of safe policy transfer and robustcontrol.
Transfer in reinforcement learning The problem of high sample complexity in
reinforcement learning has generated considerable interest in policy transfer. Taylor
et al. provide an excellent review of approaches to the transfer learning problem [ 28].
A series of approaches focused on reducing the number of rollouts performed on aphysical robot, by alternating between policy improvement in simulation and physical
rollouts [ 1,13]. In those works, a time-dependent term is added to the dynamics after
each physical rollout to account for unmodeled error. This approach, however, doesnot address robustness in the initial transfer, and the system could sustain or cause
damage before the online learning model converges.
The EPOpt algorithm [ 23] randomly samples dynamics parameters from a Gaus-
sian distribution prior to each training run, and optimizes the reward for the worst-
performing ε-fraction of dynamics parameters. However, it is not clear how robust
it is against disturbances not explicitly experienced in training. This approach isconceptually similar to that in [ 19], in which more traditional trajectory optimiza-
tion methods are used with an ensemble of models to increase robustness. Similarly,
[14] and [ 22] use adversarial disturbances instead of random dynamics parameters
for robust policy training. Tobin et al. [ 29] and Peng et al. [ 21] randomize visual
inputs and dynamics parameters respectively. Bousmalis et al. [ 2] meanwhile adapt
rendered visual inputs to reality using a framework based on generative adversarial
networks, as opposed to strictly randomizing them. While this may improve adap-
tation to a target environment in which these parameters are varied, this may notimprove performance on dynamics changes outside of those varied; in effect, it does
not mitigate errors due to the “unknown unknowns”.
Christiano et al. [ 4] approach the transfer problem by training an inverse dynamics
model on the target system and generating a nominal trajectory of states. The inverse
dynamics model then generates actions to connect these states. However, there are no
guarantees that an action exists in the target dynamics to connect two learned adjacentstates. Moreover, this requires training on the target environment; in this work we
consider zero-shot learning where this is not possible. Recently, the problem of
transfer has been addressed in part by rapid test adaptation [ 6,24]. These approaches
have focused on training modular networks that have both “task-speciﬁc” and “robot-
speciﬁc” modules. This then allows the task-speciﬁc module to be efﬁciently swapped
out and retrained. However, it is unclear how error in the learned model affects these
methods.
In this work we aim to perform zero-shot policy transfer, and thus efﬁcient model-
based approaches are not directly applicable. However, our approach uses an auxiliary
control scheme that leverages model learning for an approximate dynamics model.
When online learning is possible, sample-efﬁcient model-based reinforcement learn-ing approaches can dramatically improve sample complexity, largely by leveraging
440 J. Harrison et al.
tools from planning and optimal control [ 11]. However, these models require an
accurate estimate of the true system dynamics in order to learn an effective policy.
A variety of model classes have been used to represent system dynamics, such asneural networks [ 9], Gaussian processes [ 5], and local linear models [ 8,13].
Robust control Trajectory optimization methods have been widely used for robotic
control [ 27]. Among these optimization methods, model predictive control (MPC)
is a class of online methods that perform trajectory optimization in a receding-horizon fashion [ 20]. This receding-horizon approach, in which a ﬁnite-horizon,
open-loop trajectory optimization problem is continuously re-solved, results in an
online control algorithm that is robust to disturbances. Several works have attemptedto combine trajectory optimization methods with dynamics learning [ 16] and policy
learning [ 10]. In this work, we develop an auxiliary robust MPC-based controller to
guarantee robustness and performance for learned policies. Our method combines thestrengths of deep policy networks [ 25] and tube-based MPC [ 15] to offer a controller
with good performance as well as robustness guarantees.
3 Problem Setup and Preliminaries
Consider a ﬁnite-horizon Markov Decision Process ( M) deﬁned as a tuple M:
/angbracketleftS,A,p,r,T/angbracketright.H e r e Sand Arepresent continuous, bounded state and action spaces
for the agent, r:S×A→ Ris the reward function that maps a state-action tuple
to a scalar, and Tis the problem horizon. Finally, p:S×S×A→[ 0,1]is the
transition distribution that captures the state transition dynamics in the environment
and is a distribution over states conditioned on the previous state and action. The
goal is to ﬁnd a policy π:S→ Athat maximizes the expected cumulative reward
over the choice of policy:
π∗(s)=argmax
π(s)E/bracketleftBiggT/summationdisplay
t=0r(st,at)/bracketrightBigg
. (1)
The above reﬂects a standard setup for policy optimization in continuous state
and action spaces. In this work, we are interested in the case in which we only havean approximately correct environment, which we refer to as the source environment
(e.g. a physics simulator). We may sample this simulator an unlimited number of
times, but we wish to maximize performance on the ﬁrst execution in a target envi-
ronment. Without any assumptions on the correctness of the simulator, this problem
is of course intractable as the two sets of dynamics may be arbitrarily different.However, relatively loose assumptions about the correctness of the simulator are
very reasonable, based on the modeling ﬁdelity of the simulator. We assume the sim-
ulator (denoted M
S) has deterministic, twice continuously-differentiable dynamics
st+1=f(st,at). Then, let the dynamics of the target environment (denoted M T)
be denoted st+1=f(st,at)+wt, for iid additive noise wtwith compact, convex
AdaPT : Zero-Shot Adaptive Policy Transfer … 441
support Wthat contains the origin. Generally, the noise distribution may be state
and action dependent, so this formulation reduces to standard formulations in both
robust and stochastic control [ 32]. We assume all other components of the MDPs
deﬁning the source and target environments are the same (e.g. reward function).
Finally, we assume the reward function ris Lipschitz continuous, an assumption that
we discuss in more detail in Sect. 5. Based on the above deﬁnitions, we can now state
the problem we aim to solve.
Problem Statement Given the simulator dynamics and the problem deﬁned by the
MDP M S, we wish to learn a policy to maximize the reward accrued during operation
in the target system, M T. Formally, if we write the realization of the disturbance at
time tas˜wt, we wish to solve the problem:
max
{at}T
t=0E/bracketleftBiggT/summationdisplay
t=0r(st,at)/bracketrightBigg
s.t. st+1=f(st,at)+˜wt,and st∈S,at∈A∀t∈[0,T],(2)
while only having access to the simulator, M S, for training.
4A DAPT: Adaptive Policy Transfer for Stochastic
Dynamics
In this section we present the AdaPT algorithm for zero-shot transfer. A high level
view of the algorithm is presented in Algorithm 1. First, we assume that a policy is
trained in simulation. Our approach is to ﬁrst compute a nominal trajectory (without
disturbance) by continuously executing the learned policy on the simulator dynamics.Then, when transferred to the target environment, we use an auxiliary model pre-
dictive control-based (MPC) controller to stabilize around this nominal trajectory.
In this work, we use a reward formulation for operation in the primary environment(i.e, the aim is to maximize reward), and a cost formulation for the auxiliary con-
troller (i.e., the aim is to minimize cost to thus minimize deviation from the nominal
trajectory). This is in part to disambiguate the distinction between the primary andauxiliary optimization problems.
Policy Training We use model-free policy optimization on the black-box simulated
model. Our theoretical guarantees rely on the auxiliary controller avoiding saturation.
Therefore, if a policy operates near the limits of its control authority and thus the
auxiliary controller saturates when used on the target environment, this policy istrained using restricted state and action spaces S
/prime⊆S,A/prime⊆A.W el e t M/primedenote
an MDP with restricted state and action spaces. This follows the approach of [ 15],
where it is used to prevent auxiliary controller saturation. Intuitively, restricting thestate and action space ensures any nominal trajectory in those spaces can be stabilized
442 J. Harrison et al.
by the auxiliary controller. Therefore, if saturation is rare, restricting these sets is
unnecessary.
AdaPT is invariant to the choice of policy optimization method. During online
operation, a nominal trajectory τ={(¯st,¯at)}T
t=0is generated by rolling out the policy
on the simulator dynamics, M S. The auxiliary controller then tracks this trajectory
in the target environment.
Approximate Dynamics Model Because the model of the simulator is treated as a
black-box, it is impractical to use for the auxiliary controller in an optimal control
framework. As such, we rely on an approximate model of the dynamics, separate
from the simulator dynamics f, which we refer to as ˆf. The speciﬁc representation of
the model (e.g. linear model, feedforward neural network, etc.) depends on both theaccuracy required as well as the method used to solve the auxiliary control problem.
This model may be either learned from the simulator, or based on prior knowledge.
A substantial body of literature exists on dynamics model learning from black-boxsystems [ 18]. Alternatively, this model may be based on external knowledge, either
from learning a dynamics model in advance from the target system or from, for
example, a physical model of the system.
Auxiliary MPC Controller Our auxiliary nonlinear MPC controller is based on that
of [15]. Speciﬁcally, we write the auxiliary control problem:
min
{ak}t+N
k=tt+N/summationdisplay
k=t(sk−¯sk)TQk(sk−¯sk)+(ak−¯ak)TRk(ak−¯ak)
s.t. sk+1=ˆf(sk,ak),and sk∈S,ak∈A∀k∈[t,t+N],(3)
where Nis the MPC horizon, Qkand Rkare positive deﬁnite cost matrices for the
state deviation and control deviation respectively, and ˆfis the approximate dynamics
model. In some cases, this problem is convex, but generally it may not be. In ourexperiments, this optimization problem is solved with iterative relinearization based
on [ 30]. However, whereas they iteratively linearize the nonlinear optimal control
problem and solve an LQR problem over the full horizon of the problem, we explic-itly solve the problem over the MPC horizon. We do not consider terminal state
costs or constraints. This formulation of the auxiliary controller by [ 15] allows us
to guarantee, under our assumptions, that our true state stays in a tube around thenominal trajectory, where the tube is deﬁned by level sets of the value function (the
details of this are addressed in Sect. 5).
The solution to the MPC problem is iterative. First, we linearize around the nom-
inal trajectory τ. We introduce the notation {(ˆs
k,ˆak)}k=t+N
k=t, which is the solution for
the last iteration. These are initialized as ˆst←¯ standˆat←¯ at. Then, we introduce
the deviations from this solution as
δst=st−ˆst,δ at=at−ˆat. (4)
Then, taking the linearization of our dynamics
AdaPT : Zero-Shot Adaptive Policy Transfer … 443
Algorithm 1 Adaptive Policy Transfer for Stochastic Dynamics ( AdaPT )
Input: Source Env: M S,T a r g e tE n v : M T, Initial State: s0
Ofﬂine:
1:A/prime,S/prime←bound\_set (A,S) // Calculate constrained state & action space
2:π←policy\_opt/parenleftbig
M/prime
S/parenrightbig
// Train a policy for M/prime
Susing constrained S/prime,A/prime
3:ˆf←fit\_dynamics/parenleftbig
M S/parenrightbig
// Fit Dynamics for M S
Online:
4:τ←rollout/parenleftbig
s0,π, M S,T/parenrightbig
// Roll out πon M Sto get nominal trajectory
5:s← s0
6:for t∈[0,T]do
7: a←aux\_MPC/parenleftbig
s,τ,ˆf,τ, N/parenrightbig
// NMPC with iterative linearization
8: s← f(s,a)+w // Rollout the ﬁrst step of action seq. on M T
9:end for
At=∂ˆf
∂st/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsingle
st=ˆst,at=ˆatBt=∂ˆf
∂at/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsingle
st=ˆst,at=ˆat, (5)
we can rewrite the MPC problem as:
min
{δak}t+N
k=tt+N/summationdisplay
k=t(δsk+ˆsk−¯sk)TQk(δsk+ˆsk−¯sk)+(δak+ˆak−¯ak)TRk(δak+ˆak−¯ak)
s.t. δsk+1=Akδsk+Bkδak,andδsk+ˆsk∈S,δak+ˆak∈A,∀k∈[t,t+N].
(6)
Note that the optimization is over the action deviations {δak}t+N
k=t. Once this problem
is solved, we use the update rule ˆst←ˆ st+δst,ˆat←ˆ at+δat. The dynamics are
then relinearized, and this is iterated until convergence. Because we use iterative
linearization to solve the nonlinear program, it is necessary to choose a dynamicsrepresentation ˆfthat is efﬁciently linearizable. In our experiments, we use an analyt-
ical nonlinear dynamics representation for which the linearization can be computed
analytically (see [ 31] for details), as well as ﬁt a time-varying linear model. Choices
such as, e.g., a Gaussian process representation, may be expensive to linearize.
5A DAPT: Analysis
The following section develops the main theoretical analysis of this study. We willﬁrst show that AdaPT results in bounded deviation from the nominal trajectory
τunder a set of technical assumptions. This result is then used to show that the
deviation between cumulative reward of the realized rollout on the target system and
the cumulative reward of the nominal trajectory on the source environment, is upperbounded. This is to say, the decrease in performance below the ideal case is bounded.
444 J. Harrison et al.
5.1 Safety Analysis in ADAPT
Using the notation from Eq. ( 3), let us denote the solution at time kasC∗
N(sk,k)
for MPC horizon N. This is the minimum cost associated with the ﬁnite horizon
problem that is solved iteratively in the MPC framework. Note that this problem is
solved with the approximate dynamics model; in the case where the approximate
dynamics model exactly matches the target environment model, the solution to thisproblem would have value zero as the trajectory would be tracked exactly. We denote
byκ
N(sk)the action at time kfrom the solution to the MPC problem. Then, let
Ld(k)/defines{s|C∗
N(s,k)≤d}denote the level set of the cost function for some value
d∈(0,c)(for some constant c;s e e[ 15]) at time k.
We assume the error between approximate dynamics representation ˆfand the
simulator dynamics fis outer approximated by a compact, convex set Dthat contains
the origin. Therefore, for all state, action pairs (s,a)∈S×A,f(s,a)−ˆf(s,a)∈
D. In the case where the state and action spaces are bounded, there always exists
an outer approximation which satisﬁes this assumption. However, in practice, it is
likely considerably smaller than this worst case.
Let Ts(s0)/defines{Ld(k)|k∈Z≥0}denote a state tube deﬁned by the time-dependent
level sets of the auxiliary cost function. We may now state our ﬁrst result, noting
that the auxiliary stabilizing policy κNis the result of the MPC optimization problem
relying solely on the approximate dynamics ˆf.
Theorem 1 Every state trajectory {st}T
t=0generated by the target dynamics s t+1=
f(st,κ N(st))+wtwith initial state s 0, lies in the state tube Ts(s0).
Proof Note that W+D, where the addition denotes a Minkowski sum, is compact,
convex, and contains the origin. Then, the result follows from Theorem 1of [15]b y
replacing the set of disturbances (which the authors refer to as W) with W+D./intersectionsq/unionsq
The above result combined with Proposition 2i of [ 15], which shows that for some
constant c1,C∗
N(sk,k)≥c1/bardblsk−¯sk/bardbl2, gives insight into the safety of AdaPT . In par-
ticular, note that for an arbitrarily long trajectory, the realized trajectory stays in a
region around the nominal trajectory despite using an inaccurate dynamics represen-tation in the MPC optimization problem. While this result shows that the deviation
from the nominal trajectory is bounded, it does not allow construction of explicit
tubes in the state space, and thus can not be used directly for guarantees on obstacleavoidance. Recent work by Singh et al. [ 26] establishes tubes of this form, and this
is thus a promising extension of the AdaPT framework.
5.2 Robustness Analysis in ADAPT
We will now show that due to the boundedness of state deviation, the deviation in
the total accrued reward over a rollout on the target system is bounded. Let Vπ
S(s)
AdaPT : Zero-Shot Adaptive Policy Transfer … 445
and Vκ
T(s)denote the value functions associated with some state sand the primary
policy executed on the source environment, and the AdaPT policy on the secondary
environment respectively.
Theorem 2 Under the technical assumptions made in Sects. 3and 5.1,|Vκ
T(s0)−
Vπ
S(s0)|≤ c2/summationtextT
t=0/radicalbig
C∗
N(st,t), where c 2is some constant and s t+1=f(st,κ N(st))
+wt.
Proof First, note |Vκ(s0)−Vπ(s0)|≤/summationtextT
t=0|r(st,κ N(st))−r(¯st,π(¯st))|, where
st+1=f(st,κ N(st))+wtand¯st+1=f(¯st,π(¯st)). Additionally, letting a=κN(s)
and¯a=π(¯s), note that similarly to Proposition 2i of [ 15], we can establish a bound
on the action deviation from the nominal trajectory in terms of the auxiliary cost func-tion, C
∗(st,t)≥c3/bardblat−¯at/bardbl2for all t(where the norm is in the Euclidean sense),
by taking c3as the minimum eigenvalue of Rt. By the Lipschitz continuity of the
reward function, and writing the Lipschitz constant of the reward function Lr,w e
have
|r(s,a)−r(¯s,¯a)|≤ Lr(/bardbls−¯s/bardbl+/bardbl a−¯a/bardbl). (7)
Then, noting that the quadratic auxiliary cost function C∗
Nis always positive, the
result is proved by applying Proposition 2i of [ 15] and the bound on action deviation
from the nominal to the right hand side of Equation 7. /intersectionsq/unionsq
This result may then be restated in terms of the disturbance sets. Let /bardblW+D/bardbl/defines
max w∈W,d∈D/bardblw+d/bardbl.
Theorem 3 Under the same technical assumptions as Theorem 2, the following
inequality holds for some constant c 4>0:
|Vκ
T(s0)−Vπ
S(s0)|≤ c4T/radicalbig
/bardblW+D/bardbl (8)
Proof The result follows from combining Theorem 2with Proposition 4ii of [ 15].
/intersectionsq/unionsq
These results shows that along with guarantees on spatial deviation from the
nominal trajectory, we may also establish bounds on the accrued reward relative
to what is received with the nominal policy in the source environment, in effect
demonstrating that zero-shot transfer is possible. The Lipschitz continuity of the
reward function is essential to this result, and this illustrates several aspects of thepolicy transfer problem.
The AdaPT algorithm is based on tracking a nominal rollout in simulation. Critical
in the success of this approach is gradual variation of the reward function. Sparsereward structures are likely to fail with this approach to transfer, as tracking the
nominal trajectory, even relatively closely, may result in poor reward. On the other
hand, a slowly varying reward function, even if tracked relatively roughly may resultin accrued reward close to the nominal rollout on the source environment.
446 J. Harrison et al.
6 Experimental Evaluation
We implemented AdaPT on a nonlinear, non-holonomic kinematic car model with
a 5-dimensional state space as well as on the Reacher environment in OpenAI’s
Gym [ 3]. We train policies using Trust Region Policy Optimization (TRPO) [ 25].
The policy is parameterized as a neural network with two hidden layers, each with
64 units and ReLU nonlinearities. In all of our experiments, we report normalized
cost. This is the cost (negative reward) realized by a trial in the target environment,
divided by the cost of the nominal policy rolled out on the simulated environment
from the same initial state. This allows more direct comparison between episodesfor environments with stochastic initial states. We generally compare the naive trial,
which is the nominal policy rolled out on the target environment (e.g., standard
transfer with no adaptation) to AdaPT .
6.1 Environment I: 5-D Car
We implemented AdaPT on a nonlinear, nonholonomic 5-dimensional kinematic car
model that has been used previously in the motion planning literature [ 31]. Speciﬁ-
cally, the car has state s=[ x,y,θ, v,κ]T, where xand ydenote coordinates in the
plane, θdenotes heading angle, vdenotes speed, and κdenotes trajectory curva-
ture. The system has dynamics ˙s=[ vcosθ,vsinθ,vκ,av,aκ], where av∈[ − 2,2]
and aκ∈[ − 0.5,0.5]are the controlled acceleration and curvature derivative. The
policy is trained to minimize the quadratic cost L(s,a)=/summationtextT
t=0/lscript(st,at), where
/lscript(st,at)=x2
t+y2
t+a2
v,t+a2
κ,t, which results in policies that drive to the origin. In
each trial, the vehicle is initialized in a random state, with position x,y∈[ − 5,5],
with random heading and zero velocity and curvature.
Our auxiliary controller used an MPC horizon of 2 s (20 timesteps). Our state
deviation penalty matrix, Q, has value 1 along the diagonal for the position terms,
and zero elsewhere. Thus, the MPC controller penalizes only deviation in position.
The matrix Rhad small terms (10−3) along the diagonal to slightly penalize control
deviations. In practice, this mostly acts as a small regularizing term to prevent large
oscillatory control inputs by the auxiliary controller. The behavior of the auxiliary
controller is dependent on the matrices Qand R, but in practice good performance
may be achieved across environments with ﬁxed values. Because of the relatively high
quadratic penalty on control in policy training, the nominal policy rarely approaches
the control limits. Thus, we can set A/prime=A, and we set S/prime=S. For our dynamics
model, we use the linearization reported in [ 31].
AdaPT : Zero-Shot Adaptive Policy Transfer … 447
6.2 Disturbance Models
We investigate four disturbance types:
1. Environmental Uncertainty: We add randomly-generated hills to the target envi-
ronment such that the car experiences accelerations due to gravity. This noise
is therefore state-dependent. Figure 2a shows a randomly generated landscape.
We randomly sample 20 hills in the workspace, each of which is circular and
has varying radius and height. The vehicle experiences an additive longitudinal
acceleration proportional to the landscape slope at its current location, and nolateral acceleration.
2. Control noise: Nonzero-mean additive control error drawn from a uniform distri-
bution.
3. Process noise: Additive, zero-mean noise added to the state. Disturbances are
drawn from a uniform distribution.
Fig. 1 Mean cumulative cost over the length of an episode for 50 episodes on the kinematic car
environment. The conﬁdence intervals are standard error. The costs are normalized to the cost ofthe naive policy being rolled out on the simulated environment from the same initial state, to allowmore direct comparison across episodes. The naive rollout is the nominal policy executed on the
target environment. The disturbances tested are aa hill landscape, badditive control error, cprocess
noise, and ddynamics parameter error
448 J. Harrison et al.
Fig. 2 a The car environment with the paths for the ideal case (nominal policy on simulated envi-
ronment), the naive case (nominal policy on the target environment), and the AdaPT case ( AdaPT
on the target environment). The contour plot shows the height of the added hills. Figures band
cshow the normalized cost for varying disturbances due to additive control error and dynamics
parameter error for bthe naive case and cAdaPT (lower is better). In addition to the listed dis-
turbances, disturbances due to hills are also added for all trials. Each grid cell is the mean of 50trials
4. Dynamics parameter error: We add a scaling factor γto the control of ˙κ, such
that˙κ=γaκ.
For the last three, the noise terms were drawn i.i.d. from a uniform distribution at
each time t. These disturbances were investigated both independently (Fig. 1) and
simultaneously (Fig. 2). Figure 1shows the normalized cost of the naive transfer and
AdaPT for each of the four disturbances individually.
In our experiments, AdaPT substantially outperforms naive transfer, achieving
normalized costs 1.5–5x smaller. Additionally, the variance of the naive transfer
is considerably higher, whereas the realized cost for AdaPT is clustered relatively
tightly around one (e.g., approximately equal cost to the ideal case). In Fig. 1d, the
normalized cost of AdaPT is actually below one, implying that the transferred policy
performs better than the ideal policy. In fact, this is because the dynamics parameter
error in this trial results in oversteer, and so the agent accumulates less cost to turnto face the goal than in the nominal environment. Thus, pointing toward the goal is
more “cost-efﬁcient” in the target environment. The performance of direct transfer
and AdaPT with varying parameter error may be seen in Fig. 2b, c. In Fig. 2a, a case
is presented where the direct policy transfer fails to make it up a hill, whereas the
AdaPT policy tracks the nominal trajectory well.
6.3 ADAPTwith Robust Ofﬂine Policy
Whereas AdaPT ’s approach to policy transfer relies primarily on stabilization in
the target environment, recent work has focused on training robust policies in the
source domain, and then performing direct transfer. In the EPOpt policy train-
ing framework [ 23], an agent is trained over a family of MDPs in which model
AdaPT : Zero-Shot Adaptive Policy Transfer … 449
parameters are drawn from distributions before each training rollout. Then, a
Conditional V alue-at-Risk (CV aR) objective function is optimized as opposed to
an expectation over all training runs. We apply AdaPT on top of an EPOpt -1 policy
(equivalent to optimizing expected reward, with model parameters varying), and ﬁnd
that for disturbances explicitly varied during training, the performance of EPOpt -
only transfer and AdaPT are comparable. We add parameters γito the state deriva-
tive as follows: ˙s=[γ1vcosγ2θ,γ 1vsinγ2θ,γ 1vγ3κ,γ 4av,γ5aκ]. Each of these γi
are drawn from Gaussian distributions before each training run, and are ﬁxed during
the training run. Although some of these parameters do not have a physical inter-pretation, the resulting policies are still robust to both parametric error, as well as
process noise. In these experiments, an MPC horizon of 1 s was used (10 timesteps).
The matrices Qand Rwere set as in Sect. 6.1.
In Fig. 3, the comparison between the direct transfer of EPOpt policies and
AdaPT policies is presented. We can see that, for disturbances that are explicitly
considered in training (speciﬁcally, model parameter error), naive transfer performsslightly better, albeit with higher variance. For other disturbances, like the addition
of hills or control noise, AdaPT signiﬁcantly outperforms the directly-transferred
Fig. 3 Mean cumulative cost over the length of an episode for 50 episodes on the 5-D car environ-
ment, using an EPOpt -1 robust policy. The conﬁdence intervals are standard error. The disturbances
tested are aa hill landscape, badditive control error, cprocess noise, and ddynamics parameter
error. The details of each noise source is presented in the supplementary materials
450 J. Harrison et al.
policy. Indeed, while the performance of the AdaPT policy is comparable to direct
transfer for disturbances directly considered in training, unmodelled disturbances
are handled substantially better by AdaPT . Thus, to extract the best performance,
we recommend applying the two approaches in tandem.
6.4 Environment II: 2-Link Planar Robot Arm
We next evaluate the performance of AdaPT on the Reacher environment of Gym
[3]. This environment is a two link robotic arm that receives reward for proximity to
a goal in the workspace, and is penalized for control effort. The state is a vector of the
sin and cos of the joint angles, as well as joint angular velocities, the goal position,
and the distance from the arm end-effector to the goal. In our tests, we ﬁx one goal
location and one starting state for all tests to more directly compare between trials.
As such, the variance in normalized cost in experiments is much smaller than in thecar experiments. For these experiments, the same noise models were used as in the
previous section, with the exception of the “hills” disturbance.
As an approximate dynamics model used for the auxiliary controller, we use the
time-varying linear dynamics from [ 12]. This model is ﬁt from rollouts in simula-
tion. Since this model is linear, the MPC problem is convex, and the iterative MPC
converges in one iteration. These dynamics are only valid in a local region, and thusmust be ﬁt for each desired policy rollout in the target environment. However, since
the model is ﬁt from simulation data, it is generated quickly and inexpensively.
The results for normalized cost comparisons between naive transfer and AdaPT
are presented in Fig. 4. We note that AdaPT achieves signiﬁcantly lower cost for
additive control error and process noise, but achieves comparable cost for parameter
error. The parameter varied in these experiments was the mass of the links of thearm. The effect of this change is to increase the inertia of the manipulator as a
whole. In fact, this can be seen in the Fig. 4c. While the cost of the naive transfer
Fig. 4 Mean cumulative cost over the length of an episode for 50 episodes on the reacher environ-
ment. The conﬁdence intervals are standard error. The costs are normalized to the cost of the naivepolicy being rolled out on the simulated environment from the same initial state, to allow moredirect comparison across episodes. The naive rollout is the nominal policy executed on the target
environment. The disturbances tested are aadditive control error, bprocess noise, and cdynamics
parameter error
AdaPT : Zero-Shot Adaptive Policy Transfer … 451
increases slowly, the cost of the AdaPT trials spikes at approximately time t=0.25.
As AdaPT is tracking the nominal trajectory, it increases the torque applied, thus
suffering a penalty for the increased control action, but resulting in better trackingof the nominal trajectory.
A similar effect can be observed in Fig. 4a. The added control error actually drives
the manipulator toward the goal, resulting in the dip in the normalized cost for bothtrajectories. However, the naive policy overshoots the goal substantially, and thus
accrues substantially higher normalized cost than the AdaPT experiments.
7 Conclusion and Outlook
We have presented the AdaPT algorithm for robust transfer of learned policies to
target environments with unmodeled disturbances or model parameters. We have
also provided guarantees on the lower bounds of the accrued reward in the targetenvironment for a policy transferred with AdaPT . Our results were demonstrated on
two different environments with four disturbance models investigated. We addition-
ally discuss usage of robust policies with AdaPT . The results presented demonstrate
that this method improves performance on unmodeled disturbances by 50–300%.
In this work, we construct our analysis on the Lipschitz continuity of the dynamics.
Indeed, the smoothness of the deviation in dynamics is fundamental to the guaranteeswe establish. An immediate avenue of future investigation is, therefore, expanding
the work presented here to environments with discrete and discontinuous dynamics
such as contact. Recently, Farshidian et al. [ 7] have extended an iteratively linearized
nonlinear MPC, similar to ours, to switching linear systems, which may have poten-
tial as a foundation on which to develop a capable contact formulation of AdaPT .
Additionally, recent work has developed robust, receding horizon tube controllersthat allow the establishment of explicit tubes in the state space [ 26]. This approach
has the potential to establish explicit safety constraints for operation in cluttered
environments. Finally, these methods will also be evaluated on a physical systems.
Acknowledgements This work was supported by the Ofﬁce of Naval Research (Grant N00014-
15-1-2673) and by the Toyota Research Institute (“TRI”). This article solely reﬂects the opinionsand conclusions of its authors and not ONR, TRI, or any other Toyota entity. James Harrison wassupported in part by the Stanford Graduate Fellowship and the National Sciences and EngineeringResearch Council of Canada (NSERC).
References
1. Abbeel, P ., Quigley, M., Ng, A.Y .: Using inaccurate models in reinforcement learning. In:
Proceedings of the 23rd International Conference on Machine Learning. ACM (2006)
2. Bousmalis, K., Irpan, A., Wohlhart, P ., Bai, Y ., Kelcey, M., Kalakrishnan, M., Downs, L., Ibarz,
J., Pastor, P ., Konolige, K., et al.: Using simulation and domain adaptation to improve efﬁciencyof deep robotic grasping (2017). arXiv:1709.07857
452 J. Harrison et al.
3. Brockman, G., Cheung, V ., Pettersson, L., Schneider, J., Schulman, J., Tang, J., Zaremba, W.:
Openai gym (2016). arXiv:1606.01540
4. Christiano, P ., Shah, Z., Mordatch, I., Schneider, J., Blackwell, T., Tobin, J., Abbeel, P .,
Zaremba, W.: Transfer from simulation to real world through learning deep inverse dynamicsmodel (2016). arXiv:1610.03518
5. Deisenroth, M., Rasmussen, C.E.: Pilco: a model-based and data-efﬁcient approach to policy
search. In: Proceedings of the 28th International Conference on Machine Learning (ICML-11)(2011)
6. Devin, C., Gupta, A., Darrell, T., Abbeel, P ., Levine, S.: Learning modular neural network
policies for multi-task and multi-robot transfer (2016). arXiv:1609.07088
7. Farshidian, F., Pardo, D., Buchli, J.: Sequential linear quadratic optimal control for nonlinear
switched systems (2016). arXiv:1609.02198
8. Gu, S., Lillicrap, T., Sutskever, I., Levine, S.: Continuous deep Q-learning with modelbased
acceleration. ICML (2016)
9. Heess, N., Wayne, G., Silver, D., Lillicrap, T., Erez, T., Tassa, Y .: Learning continuous control
policies by stochastic value gradients. In: NIPS (2015)
10. Kahn, G., Zhang, T., Levine, S., Abbeel, P .: Plato: policy learning using adaptive trajectory
optimization (2016). arXiv:1603.00622
11. Kober, J., Bagnell, J.A., Peters, J.: Reinforcement learning in robotics: a survey. Int. J. Robot.
Res. 0 278 364 913 495 721 (2013)
12. Levine, S., Abbeel, P .: Learning neural network policies with guided policy search under
unknown dynamics. In: Advances in Neural Information Processing Systems (2014)
13. Levine, S., Finn, C., Darrell, T., Abbeel, P .: End-to-end training of deep visuomotor policies.
J. Mach. Learn. Res. 17(39), 1–40 (2016)
14. Mandlekar\*, A., Zhu\*, Y ., Garg\*, A., Fei-Fei, L., Savarese, S.: Adversarially robust policy
learning through active construction of physically-plausible perturbations. In: IEEE Interna-tional Conference on Intelligent Robots and Systems (IROS) (2017) (\* equal contribution)
15. Mayne, D.Q., Kerrigan, E.C., V an Wyk, E., Falugi, P .: Tube-based robust nonlinear model
predictive control. Int. J. Robust Nonlinear Control (2011)
16. Mitrovic, D., Klanke, S., Vijayakumar, S.: Adaptive optimal feedback control with learned
internal dynamics models. In: From Motor Learning to Interaction Learning in Robots, pp.65–84. Springer, New Y ork (2010)
17. Mnih, V ., Kavukcuoglu, K., Silver, D., Rusu, A.A., V eness, J., Bellemare, M.G., Graves, A.,
Riedmiller, M., Fidjeland, A.K., Ostrovski, G., et al.: Human-level control through deep rein-forcement learning. Nature 518(7540), 529–533 (2015)
18. Moerland, T., Broekens, J., Jonker, C.: Learning multimodal transition dynamics for model-
based reinforcement learning (2017). arXiv:1705.00470
19. Mordatch, I., Lowrey, K., Todorov, E.: Ensemble-cio: full-body dynamic motion planning that
transfers to physical humanoids. In: 2015 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), pp. 5307–5314. IEEE (2015)
20. Neunert, M., de Crousaz, C., Furrer, F., Kamel, M., Farshidian, F., Siegwart, R., Buchli, J.:
Fast nonlinear model predictive control for uniﬁed trajectory optimization and tracking. In:
Proceedings of the IEEE Conference on Robotics and Automation (2016)
21. Peng, X.B., Andrychowicz, M., Zaremba, W., Abbeel, P .: Sim-to-real transfer of robotic control
with dynamics randomization (2017). arXiv:1710.06537
22. Pinto, L., Davidson, J., Sukthankar, R., Gupta, A.: Robust adversarial reinforcement learning
(2017). arXiv:1703.02702
23. Rajeswaran, A., Ghotra, S., Levine, S., Ravindran, B.: EPOpt: learning robust neural network
policies using model ensembles (2016). arXiv:1610.01283
24. Rusu, A.A., Rabinowitz, N.C., Desjardins, G., Soyer, H., Kirkpatrick, J., Kavukcuoglu, K.,
Pascanu, R., Hadsell, R.: Progressive neural networks (2016). arXiv:1606.04671
25. Schulman, J., Levine, S., Moritz, P ., Jordan, M., Abbeel, P .: Trust region policy optimization.
In: ICML (2015)
AdaPT : Zero-Shot Adaptive Policy Transfer … 453
26. Singh, S., Majumdar, A., Slotine, J.-J., Pavone, M.: Robust online motion planning via con-
traction theory and convex optimization. In: Proceedings of the IEEE Conference on Roboticsand Automation (2017)
27. Tassa, Y ., Erez, T., Todorov, E.: Synthesis and stabilization of complex behaviors through online
trajectory optimization. In: 2012 IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), pp. 4906–4913. IEEE (2012)
28. Taylor, M.E., Stone, P .: Transfer learning for reinforcement learning domains: a survey. J.
Mach. Learn. Res. 10 (2009)
29. Tobin, J., Fong, R., Ray, A., Schneider, J., Zaremba, W., Abbeel, P .: Domain randomization for
transferring deep neural networks from simulation to the real world (2017). arXiv:1703.06907
30. Todorov, E., Li, W.: A generalized iterative LQG method for locally-optimal feedback control
of constrained nonlinear stochastic systems. In: Proceedings of the 2005 American ControlConference, pp. 300-306. IEEE (2005)
31. Webb, D.J., van den Berg, J.: Kinodynamic RRT\*: asymptotically optimal motion planning for
robots with linear dynamics. In: IEEE International Conference on Robotics and Automation(ICRA) (2013)
32. Zhou, K., Doyle, J.C., Glover, K., et al.: Robust and Optimal Control, vol. 40 (1996)
Planning Multi-ﬁngered Grasps as
Probabilistic Inference in a LearnedDeep Network
Qingkai Lu, Kautilya Chenna, Balakumar Sundaralingam
and Tucker Hermans
1 Introduction and Motivation
Learning-based approaches to grasping [ 11,13,17,20,21] have become a popular
alternative to geometric [ 1,3,4,19] and model-based planning [ 6,16] over the
past decade. In particular grasp learning has shown to generalize well to previously
unseen objects where only partial-view visual information is available. More recently,
researchers have looked to capitalize on the success of deep neural networks to
improve grasp learning. Broadly speaking deep neural network methods for grasp
learning can be split into two approaches: predicting grasp success for an imagepatch associated with a gripper conﬁguration [ 8,9,13–15,17,23] and directly
predicting a grasp conﬁguration from an image or image patch using regression [ 12,
18,24]. While these deep learning approaches have shown impressive performance
for parallel jaw grippers (e.g. [ 17]) relatively little work has focused on the more
difﬁcult problem of multi-ﬁngered grasping [ 10,23,24]. We believe two primary
difﬁculties restrict the use of deep learning for multi-ﬁngered grasping (1) the inputrepresentation used for grasp conﬁgurations in neural networks and (2) the reliance
on external planners for generating candidate grasps.
In order to combat these two problems, we propose an alternative approach to
grasp planning with deep neural networks, where we directly use the learned net-
Q. Lu (B)·K. Chenna ·B. Sundaralingam ·T. Hermans
Utah Robotics Center, School of Computing, University of Utah,Salt Lake City, UT, USAe-mail: qklu@cs.utah.edu
K. Chenna
e-mail: kautilya.chenna@utah.edu
B. Sundaralingam
e-mail: bala@cs.utah.edu
T. Hermans
e-mail: thermans@cs.utah.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_35455
456 Q. Lu et al.
work for planning. In our work, we train a network to predict grasp success; however,
we use the trained network in a substantially different and novel way from currently
employed sampling methods for grasp planning. The grasp representation we usefundamentally enables our approaches’ success. In addition of giving the neural net-
work the image patch, z, as input, we also provide the grasp conﬁguration parameters,
θ(e.g. joint (preshape) angles, wrist pose, etc.).
Once trained, given a new object patch, z, we perform inference over the grasp
parameters, θ, in order to maximize the probability of grasp success P(Y=1|z,θ)
learned by our convolutional neural network (CNN). We perform this probabilisticinference as a direct optimization over θusing constrained gradient-ascent, which
leverages the efﬁcient computation of gradients in neural networks, while ensuring
joint angles remain within their limits. Thus, our approach can quickly plan reliablemulti-ﬁngered grasps given an image of an object and an initial grasp conﬁguration.
Our planner offers a number of beneﬁts over previous deep-learning approaches
to multi-ﬁngered grasping. Kappler and colleagues [ 10] learn to predict if a given
palm pose will be successful for multi-ﬁngered grasps using a ﬁxed preshape and
perform planning by evaluating a number of sampled grasp poses. V arley et al. [ 23]
present a deep learning approach to effectively predict a grasp quality metric for
multi-ﬁngered grasps, but rely on an external grasp planner to provide candidate
grasps. In contrast, our method learns to predict grasp success as a function of boththe palm location and preshape conﬁguration and plans grasps directly using the
learned network. Saxena et al. [ 21] also perform grasp planning as inference using
learned probabilistic models; however they use separate classiﬁers for both the imageand range data, using hand selected models instead of a uniﬁed deep model. Zhou
and Hauser [ 25] concurrently propose a similar optimization-based grasp planning
approach to ours using a similar CNN architecture. In contrast to our work, they donot interpret planning as probabilistic inference; they optimize only for hand pose,
ignoring hand joint conﬁgurations; and they validate only in simulation.
Ve r e s e t a l . [ 24] train a conditional variational auto-encoder (CV AE) deep network
to predict the contact locations and normals for a multi-ﬁngered grasp given an RGBD
image of an object. In order to perform grasping an external inverse kinematics solver
must be used for the hand to try and reach the desired contact poses as best as possible.Implicit in such a regression method as proposed in [ 24] lies the assumption that there
exists a unique best grasp for a given object view. In contrast, our method can plan
multiple high quality grasps for a given object using different initial conﬁgurations.This offers the robot the option of selecting a grasp best suited for its current task.
Additionally, we show that our classiﬁcation-based network can effectively learn
with a smaller dataset compared with a regression network, which can not leverage
negative grasp examples.
We propose two novel CNN architectures to encode grasp conﬁgurations for our
planner, but many alternatives would surely also work. Our proposed multi-channel
network (c.f.Sect. 3.1) has a similar architecture to that of [ 14], but we train on joint
conﬁgurations instead of motor commands and use only a single image as input, aswe evaluate grasps and do not learn a controller to perform grasping. Our alternative
patch-based network described in Sect. 3.2 was inspired by the ﬁngertip patches
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 457
used in [ 23]; however, we construct our patches as a function of the grasp parameters
differently in order to improve learning and efﬁciently perform gradient ascent.
The contributions of this paper are as follows:
•We present a method for performing grasp planning as probabilistic inference in
a deep neural network, that
– is the ﬁrst work to directly optimize over grasp conﬁgurations inside a learned
deep network;
– is data-efﬁcient compared with direct regression CNNs;– can naturally predict a variable number of high quality grasps;
– can improve initial grasp conﬁgurations which would fail to result in successful
grasps;
– requires far fewer grasps as initializations than sampling-based approaches.
•We propose two novel CNN architectures for use with our planning algorithm.
•We provide a multi-ﬁnger grasp dataset from simulation with more realistic data
than currently existing simulation datasets.
•We experimentally validate the effectiveness and efﬁciency of our inference
method for both multi-ﬁnger and two-ﬁnger grasp planning on real robots.
In the next section we provide a formal description of our grasp planning approach.
We follow this in Sect. 3with an overview of our approach to multi-ﬁnger grasp
learning and the novel CNN architectures for predicting grasp success. We then give
a thorough account of our experiments and results in Sect. 4. We conclude with a
brief discussion in Sect. 5.
2 Grasp Planning as Probabilistic Inference
Following [ 3] we deﬁne the grasp planning problem as ﬁnding a grasp pre-shape
conﬁguration (hand joint angles and palm pose). The robot then moves to this pose
and executes a controller to close the hand forming the grasp on the object. Wefocus on scenarios where a single object of interest in isolation exists in the scene.
Importantly, we assume no explicit knowledge of the object beyond this sensor
reading. The problem we address states, given such a grasp scenario, plan a grasppreshape conﬁguration that allows the robot to successfully grasp and lift the object
without dropping it.
We propose planning grasps by ﬁnding the grasp conﬁguration which maximizes
the grasp success probability. We use a deep neural network to predict the probability
of grasp success, Y, as a function of the sensor readings, z, and hand conﬁguration,
θ. We formalize probabilistic grasp inference as an optimization problem:
argmax
θp(Y=1|θ,z,w)=f(θ, z,w)
subject to θmin≤θ≤θmax.(1)
458 Q. Lu et al.
In Eq. 1f(θ, z,w) deﬁnes a neural network classiﬁer with logistic output trained to
predict the grasp success probability as a Bernoulli distribution over Y. The parame-
terswdeﬁne the neural network parameters. We encode joint limits and the reachable
workspace of the robot hand as constraints on the decision variables. We can now
directly optimize over θin order to infer the maximum likelihood grasp estimate, θ⋆.
In order to efﬁciently solve this optimization we use a gradient-ascent approach,
which leverages the structure of the neural network to efﬁciently compute gradients.
The gradients are computed using the well-known backpropogation algorithm with
respect to the grasp input, θ, instead of the more typical optimization of network
parameters, w, during learning. The complete algorithm takes as input the current
object image, z, and an initial grasp conﬁguration, θ◦. We compute the success
probability by evaluating a forward pass of the neural network, then update thegrasp parameters, θ, using backpropogation in backwards pass through the network.
We iterate these forward and backward passes until convergence. We handle the
linear constraints using gradient projection and apply a backtracking line search todetermine the gradient step length at each iteration. Initial hand conﬁgurations could
be generated in a number of different ways, we describe the heuristic approach we
use in Sect. 4.1.
Our formulation allows for a number of straightforward extensions, which we
do not consider in this work. First, adding a prior over θwould allow one to infer
the Maximum a posteriori (MAP) grasp estimate, which could also be learned from
data. This could encode, for example, generally effective preshapes independent of
the object. Additionally other constraints could be added to the optimization suchas collision avoidance constraints or the full forward kinematics of the robot arm.
Finally, other sensing modalities could be given as input to the neural network if
available such as tactile or haptic information. In the next section we deﬁne twoneural network architectures we use in evaluating our approach. However, many
other networks would likely work well within our framework, as long as they use
the same inputs and outputs required by our planner.
3 Deep Networks for Multi-ﬁngered Grasp Learning
We present two novel neural network architectures designed to predict the probabilityof grasp success for multi-ﬁngered hands. Importantly, the networks operate as a
function of both a grasp conﬁguration and RGBD image of the object of interest.
For a given RGBD image we compute the surface normals and curvature from the
associated point cloud giving an 8-channel representation (i.e. RGB (3), depth (1),
normal (3), and curvature (1)). In the remainder of this section we ﬁrst detail thespeciﬁcs of the two networks. We then explain the training algorithm used, followed
by a brief discussion of the gradient computation necessary to perform the inference
described in the previous section.
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 459
3.1 A Multi-channel Grasp Conﬁguration and Image CNN
We deﬁne a multi-channel deep neural network which takes as input a grasp con-
ﬁguration and RGBD image grasp patch and predicts as output the probability of
grasp success. We extract a 400 ×400 pixel object patch from the 8-channel RGBD
representation. In our experiments we examine both keeping this object patch ﬁxed
and moving it as a function of the palm pose of the grasp. The conﬁguration RGBD
CNN takes the grasp patch and the grasp preshape conﬁguration as the input to learn
to predict the grasp success probability. We pass the image patch through two convo-
lution layers and one max pooling layer. We ﬁrst process the grasp conﬁguration, θ,
through one fully connected layer. We tile the resulting grasp conﬁguration features
point-wise across the spatial dimensions of the response map of the pool1 features
output from the image feature channel (c.f. [ 14]). This generates a concatenation
of the grasp conﬁguration and image features which pass through one convolution
layer, then one max pooling layer, followed by two-fully connected layers, and a
ﬁnal logistic regression output layer. Figure 1shows this “conﬁg-CNN” structure in
detail. The conﬁg-CNN contains 61k parameters in total.
3.2 An Image Patch Based CNN for Grasping
As an alternative to the conﬁg-CNN, we deﬁne an image based network inspired
by [23]. We extract image patches related to the palm pose and ﬁngertip locations for
a given grasp conﬁguration, θ. Each image patch is passed through two convolution
Fig. 1 The conﬁg-CNN architecture. Top left: the RGBD representation, z, extracted grasp patch, p,
and image patch network channel m(·). Bottom: the Allegro hand and grasp conﬁguration network
channel n(·). Right: the combined feature network layers and output k(·)
460 Q. Lu et al.
Fig. 2 The patches-CNN architecture and patch input. Finger and palm patches are extracted from
the 8-channel RGBD representation on the left. The red bounding box represents the palm patch.
The blue and green boxes deﬁne the patches for the two Baxter gripper ﬁngers
layers, one max pooling layer, and one fully connected layer fc1. We concatenate the
features from each ﬁnger and palm fc1and feed these into one fully connected layer
followed by a logistic regression output. We show the structure of the “patches-CNN”
in Fig. 2implemented for the Baxter parallel jaw gripper as an example. However,
extending it for use on a multi-ﬁnger hand is straightforward.
We use the same 8 channel image representation as with the conﬁg-CNN. We
extract a 200 ×200 pixel patch centered at the projected palm location and oriented
to align with the project palm orientation. We extract 100 ×100 pixel patches for
each ﬁnger centered at the projected ﬁngertip location in the image rotated to align
with the projected palm orientation. Our patches-CNN has 316k parameters in total.
3.3 Training
We train our classiﬁers using the common cross entropy loss function:
argmin
wM/summationdisplay
i=1−yilog(f(θ, z,w))−(1−yi)log(1−f(θ, z,w)) (2)
where yideﬁnes the ground truth grasp label as 1 if successful and 0 if failed. We
optimize for training by stochastic gradient descent (SGD) using Adam optimizer
with mini-batches of size 8 for 6,000 iterations. The learning rate starts at 0 .001 and
decreases by 10 ×every 2,000 iterations. We apply dropout to prevent overﬁtting
keeping weights with a probability of 0 .75 and apply Xavier initialization [ 5]f o r
the weights. For the multi-ﬁnger grasp CNN training, we oversample positive grasps
making sure at least one positive grasp exist in each mini-batch. The same training
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 461
set up is used for the patches-CNN, except we train for 60 ,000 iterations with the
learning rate decreasing by 10 ×every 20 ,000 iterations. When training on the two-
ﬁngered grasps dataset from [ 13] we do not oversample, but mirror each grasp patch
to double the number of training examples. We implemented and trained all of our
neural network models using TensorFlow ( http://tensorﬂow.org/ ).
3.4 Computing Gradients for Image Patches in Inference
Since, we deﬁne the patches used as input to our CNNs, as a function of the grasp
conﬁguration, θ, we must compute the gradient of the patch with respect to the
conﬁguration parameters for use in inference. In the patches-CNN the gradient takes
the form:
∂f
∂θ=N/summationdisplay
i=0∂f
∂pi∂pi
∂θ(3)
where we deﬁne the sum over the derivatives with respect to the image patches pi
associated with the N−1 ﬁngers and palm.
We can decompose the conﬁg-CNN f(·)into sub-modules m,k, and nso that
f=k(m(p(z,θ)), n(θ)) as shown in Fig. 1. This gives the following equation in
computing the gradient:
∂f
∂θ=∂k
∂n∂n
∂θ+∂k
∂m∂m
∂p∂p
∂θ(4)
We use backpropogation to compute most of the terms in Eqs. 3and4; however,
for both networks we use ﬁnite differences to estimate the patch gradient with respect
to the hand conﬁguration:
∂p
∂θ=p(z,θ+ε)−p(z,θ−ε)
2ε(5)
4 Experimental Validation
In this section, we describe our simulation-based training data collection process
and the experimental evaluation of our grasp inference approach. We conduct multi-
ﬁnger grasp experiments using the same four-ﬁngered Allegro hand mounted on a
Kuka LBR4 arm in simulation and on the real robot. We compare to a heuristicgrasp procedure, as well as the two dominant approaches to deep-learning based
grasp planning: sampling and regression. Finally, we show the applicability of our
inference method to two-ﬁnger grippers by preforming real-world experiments onthe Baxter robot. All data and software used in this paper are available for use at:
https://robot-learning.cs.utah.edu/project/grasp\_inference .
462 Q. Lu et al.
(a)The RGB image.
(b)The depth image.
Fig. 3 The RGB and depth image generated by Blensor for one grasp trial of the “detergent” object.
The bottom part of the orange robot arm can be seen in the left side of the image
4.1 Multi-ﬁnger Grasping Data Collection
We developed a grasping system using ROS that runs both in simulation and on the
real robot. We collected simulated grasps data using the Allegro hand mounted on the
Kuka LBR4 arm inside the Gazebo simulator with the DART physics engine ( https://
dartsim.github.io/ ). We use Blensor [ 7] to generate RGB and depth images simulating
a Kinect camera we use in real-world experiments. Example images generated by
blensor can be seen in Fig. 3.
For a given trial we place the selected object, with a predetermined support surface
facing down, at a location chosen uniformly at random from a 0 .2×0.2 m rectangle
area with a uniformly random orientation. We then perform object segmentation by
ﬁtting a plane to the table using RANSAC and extract the points above the table. We
compute a 3D bounding box of the segmented object using PCA used to generate
heuristic grasps associated with the bounding box’s top face and its two faces closer
to the camera.
We generate a grasp for a given face by setting the palm to be a ﬁxed distance from
the face center (2 cm for top grasps and 3 cm for side grasps) in the direction of its
surface normal. For data collection we add zero mean Gaussian noise with a standard
deviation of 1 mm along the normal to get a more diverse set of grasps. We rotate
the palm to align with the bounding box face. For side grasps we have the thumb
pointing towards the top, for top grasps we randomly select to align with either the
major or minor axis of the face and add an additional small amount of random noise.
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 463
We generate a preshape by randomly sampling joint angles for the ﬁrst two joints of
all ﬁngers within a reasonable range, ﬁxing the last two joints of each ﬁnger to be
zero. The preshape of the Allegro hand has 14 parameters: 6 for the palm pose and8 for joint angles. In data collection, each preshape reachable for the arm using the
RRT-connect motion planner in Moveit is executed and recorded. In experiments, all
3 preshapes are used as initializations for inference.
For grasping the object we ﬁrst use a position controller to move the ﬁnger pre-
shape joints to the desired preshape, followed by moving the arm to reach the desired
palm preshape pose. We then use a simple grasp controller to close the hand. Thecontroller closes the ﬁngers at a constant velocity stopping each ﬁnger independently
when contact is detected by the measured joints velocities being close to zero. We
found slightly different controllers to work well for top grasps versus side grasps. Formost grasps the controller closes the last three joints of non-thumb ﬁngers and the
last two joints of the thumb. Importantly, the last three joints includes both preshape
and non-preshape joints. However, for overhead grasps the controller closes only thesecond joint of non-thumb ﬁngers and only the third joint of the thumb.
If after closing the hand the robot can lift the object to a height 0.15 m without
dropping it, the system labels the grasp as successful. We used this grasp system to
collect a dataset containing 1507 grasp trials. The dataset covers all 125 objects of
the Bigbird [ 22] dataset with an average of 12 grasps per object, depending on how
many grasps were reachable by the planner. Of the collected grasps attempted 159
were successful.
4.2 Grasp Classiﬁcation CNN Evaluation
We ﬁrst compare the performance of our two proposed network architectures conﬁg-
CNN and patches-CNN. We train the networks using a random 80% of the collecteddata and test with the remaining 20%. We repeat this procedure to perform a ﬁve-fold
cross validation. We compare two different cross validation scenarios. In the ﬁrst the
training set contains examples of all objects in the test set. In the second scenarioobjects in the testing set are held out from the training set to simulate observing
novel objects. We refer to these scenarios as “seen” and “unseen” respectively. In
both scenarios, each fold contains approximately the same percentage of successfulgrasps as the complete data-set.
The patches-CNN performs poorly for the classiﬁcation of multi-ﬁnger grasps for
both seen and unseen scenarios. The network predicts all test grasps as failure cases.
Different training parameters (e.g. learning rate, dropout probability, the number
of batches, the number of training iterations, and the oversampling of successfulgrasps) were attempted to improve learning, including using the successful settings
from the conﬁg-CNN. As such we do not report detailed results from the patches-
CNN. The comparative success of the conﬁg-CNN shows that having direct access tothe grasp conﬁguration parameters allows the network to learn a more useful feature
representation than providing this information indirectly through the image patch.
464 Q. Lu et al.
Fig. 4 The average ROC curve of ﬁve-fold cross validation for seen and unseen objects. For seen
objects, the AUC mean is: 0.83 the AUC std: 0.02. For unseen objects, the AUC mean is: 0.82, theAUC std: 0.06. For random guessing, the AUC mean is: 0.5
Table 1 Accuracy and F1 score of the RGBD conﬁg-CNN of the 5-fold cross validation for seen
and unseen objects. We list the mean with standard deviation in parentheses
Experiment Accuracy F1
Seen 0.766 (0.038) 0.405 (0.016)
Unseen 0.756 (0.05) 0.385 (0.096)
Random 0.5 (–) 0.172 (0.02)
The average receive operator characteristic (ROC) curve and the area under the
ROC curve (AUC) for the conﬁg-CNN can be seen in Fig. 4along with the perfor-
mance of selecting a label at 50% probability for comparison. Table 1shows the
mean and standard deviation of the accuracy and F1 score across all validation foldsfor both seen and unseen objects. We treat predictions with grasp probability above
0.4 as positive, based on the ROC curve and our desire for high recall. As we can see,
the conﬁg-CNN predicts multi-ﬁnger grasp success reasonably well for both seenand unseen objects, signiﬁcantly outperforming chance.
As an additional comparison to our proposed networks we implemented a regres-
sion network to directly predict the grasp conﬁguration parameters, θ, as a function
of the 8-channel RGBD object patch. We implemented a network architecture similar
to the RGBD channel of our conﬁg-CNN, except that we removed the third convo-
lution layer used for concatenating the conﬁguration and grasp patch features. Theoutput layer of the regression CNN consists of separate linear regression outputs for
each grasp parameter. We apply ridge regression to regularize the network with a
strength of 0.5 in addition to the standard least-squares loss function.
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 465
In order to predict good grasps, the regression CNN can only learn from the
159 successful grasp examples in our dataset and makes no use of the remaining
1348 samples. Unsurprisingly this is far too few samples to reasonably learn in ourregression CNN with 90k parameters. The regression CNNs in [ 12,18]h a v ea t
least 60M parameters and the CV AE deep learning model in [ 24] has more than 2M
parameters, thus we ﬁnd it highly unlikely that they will work with our data-set.
We further validate the regressors performance by examining the distance between
the predicted grasp and the ground truth example for a given object pose. The mean
Euclidean distance between the predicted preshape palm location and the successfulground truth preshape palm location was above 0.4 m and the minimum distance was
above 0.2 m. Thus the grasp regression CNN cannot predict palm locations within
an effective distance of the object for grasping. The predicted palm orientations andjoint angles were also far from the ground truth preshapes. We examined alternative
deeper and shallower network structures for regression, but found none which could
effectively learn from such a limited amount of data.
4.3 Multi-ﬁnger Grasp Inference in Simulation
We ﬁrst evaluate our inference procedure for multi-ﬁnger grasp planning by per-
forming grasping experiments on 10 objects of daily life from the Bigbird dataset.
We test grasps for each object at 5 different random poses on the table. For each posewe generate three initial grasp conﬁgurations following the heuristic described in
Sect. 4.1. We apply our CNN gradient ascent planner to each initialization and select
the grasp preshape conﬁguration with highest predicted grasp success probability forexecution on the robot. The robot automatically selects to close with the top or side
grasp controller depending on which bounding box face was used for initialization.
If the planned grasp is not reachable by the arm motion planner, then we generate anew random object pose for evaluation.
We used the following parameters in performing inference. We constrained the
palm location to be within a 0 .1×0.1×0.1 m cube around the palm location given
as initialization. We restrict the palm orientation to stay within 0.3 rad of the initial
pose expressed in XYZ Euler angles. All preshape joint angles were restricted to the
same limits used with the heuristic grasp. The maximum number of iterations for theinference is 100. We set the initial step size for backtracking line search to 0.001 and
allow it a maximum of 10 iterations for each gradient ascent step. We set the control
parameter of backtracking line search to 0.5.
We found that changing the object image patch as a function of the palm pose
did not signiﬁcantly change the grasps resulting from inference. As such, we onlycompute the change in grasp conﬁguration using the gradient
∂k
∂n∂n
∂θ(c.f. Sect. 3.1).
This approximation sped up the inference procedure signiﬁcantly without noticeably
changing the grasps generated by the planner.
We examine our grasp planner on both seen and unseen objects. For the seen
scenario we train the learner on the entire training set. In the unseen case we only hold
466 Q. Lu et al.
020406080100Heuristic Max-eval Sampling Inference
Coffee MatePringlesSyrup
DetergentCanonBox
Body washHotsauce
Momto mom
Volumizing ShampooSpray AdhesiveAll020406080100Seen Success Rate (%) Unseen Success Rate (%)
Fig. 5 Simulation Success Rate Results : Success rate for executed grasps on previously seen
objects (top) and previously unseen objects (bottom) in simulation
out the test object from the training set to simulate the robot viewing a novel object.
We compare our planner to three baselines. First we perform the three heuristic grasps
for each object pose used by the planner. We ignore cases where the arm planner
could not ﬁnd a plan to reach the grasp pose. This gives a maximum of 15 heuristic
grasps per object. The second approach evaluates the three heuristic grasps using
the learned network and selects the one with highest predicted success probability to
execute. We term this approach “max-eval” and executed it on an additional 5 random
object poses per object. Our ﬁnal comparison performs a sampling approach where
we generate 150 random grasp conﬁgurations. We evaluate all 150 generated grasps
with our learned network and execute the grasp with highest predicted probability.
Unlike the other three methods, the heuristic approach produced a variable number
of reachable grasps for each object pose. In total we collected 121 heuristic grasp
attempts for seen and 110 for unseen.
Figure 5shows the grasping success rates for our method and the baselines com-
parisons. We report the rate of successful grasps for each object in both the previously
seen and unseen cases. We note that our inference planner performs signiﬁcantly bet-
ter than the alternative approaches for both scenarios. Most importantly, for grasp
initializations which would fail, the gradient-based optimization can reﬁne these
failure grasps to be successful. For example for the unseen scenario, a side grasp ini-
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 467
00 .20 .4 0.6 0.81Coffee MatePringlesSyrupDetergentCanon BoxBody washHot sauceMom to momVolumizing ShampooSpray AdhesiveAll
0.290.330.310.30.360.340.290.280.290.340.31
0.490.490.480.490.490.490.490.490.490.50.49
Avg. Predicted Success Prob. (Seen)
Heuristic Inference00 .20 .4 0.6 0.810.30.420.390.310.320.30.30.410.350.30.34
0.580.560.50.560.590.530.490.510.550.470.53
Avg. Predicted Success Prob. (Unseen)
Fig. 6 Simulation Predicted Grasp Success Probability Results : Probability of grasp success
predicted by the network for multi-ﬁngered grasps for previously seen (left) and unseen (right)
objects in simulation
tialized for the “spray adhesive” object failed, but succeed after inference at the same
object pose. We report the average success probability predicted by the network in
Fig. 6for the initial heuristic grasps and the grasps found after inference for both the
seen and unseen cases. We see that the inference is clearly improving the predicted
probability, but has fairly low conﬁdence in its prediction.
Our planner performs best with objects that can be easily enveloped from the
side. It never selects precision grasps. For larger objects such as boxes the inference
planner generally selects top grasps. For shorter objects, our arm planner had a
difﬁcult time ﬁnding plans which would not collide with the table, especially for
side grasps. In terms of timing the inference-based planner takes approximately 2 –
3 s for each initialization, while sampling takes around 6 s to evaluate the 150 sampled
grasp preshapes, comparable to the time to perform inference with 3 initializations.
Max-eval takes 0.1 – 0.2 s for 3 initializations. The computer used to run the training
and simulation experiment has an Intel-i74790k with 64 GB RAM and an Nvidia
GeForce GTX 970 graphics card (1.1 GHz base clock rate and 4G memory) running
Ubuntu 14.04 with ROS Indigo.
4.4 Real Robot Multi-ﬁnger Grasp Inference
We evaluate our conﬁg-CNN trained on the simulated grasp data on the real robot.
We compare our proposed CNN gradient ascent inference to the heuristic grasp and
the “max-eval” grasp planner using the same heuristic initializations. We do not
compare to the regression network on the real robot, because of its poor performance
in simulation. We perform inference and max-eval following the same procedures
468 Q. Lu et al.
PringlesPitcherLego
MustardSoftscrubAll020406080100Success Rate (%)Heuristic Max-eval Inference
Fig. 7 Comparison of success rates for multi-ﬁngered grasping on the real robot. Pringles was seen
in training, all other objects are previously unseen
Fig. 8 Examples of successful grasps generated by our inference approach to grasp planning
used with the simulated data, except for one minor modiﬁcation. In order to overcome
the issue of planned grasps being in collision with the table, we limit the minimum
z-axis value of the hand position to remain above the table. We perform experiments
on 5 objects from the YCB dataset [ 2], only the “pringles” can objects was present
in the training data; however, it was a different ﬂavor. We evaluate each object at 5
random poses.
Figure 7summarizes the results of these experiments. We note that the inference
results perform better than our comparison methods in all cases. Similar with simula-
tion, the gradient-based optimization can reﬁne these failure grasps to be successful.
For example, a side grasp initialized for the “pitcher” object failed, but succeed after
inference at the same object pose. In Fig. 8we show several example grasps generated
by our grasp inference planner. We see that while most grasps are enveloping, dif-
ferent ﬁnger spreads are used to accommodate the different object geometries. The
inference-based planner and max-eval have similar running time with simulation
experiments run on similar hardware.
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 469
4.5 Experiments for Two-Finger Gripper Grasp Inference
As a ﬁnal evaluation we apply our grasp inference approach to planning two-ﬁngered
griper grasps on the Baxter robot. We train our patches-CNN (c.f. Fig. 2) to predict
two-ﬁnger gripper grasp success using the Cornell grasp dataset from [ 13]. Our
patches-CNN network has similar classiﬁcation performance on an ofﬂine validation
dataset as the deep network results reported in [ 13]. To highlight the role of the
inference procedure, and not the network, we chose to evaluate the patches-CNN,
and not the conﬁg-CNN, as it is more similar to previous approaches tested on this
dataset than our conﬁg-CNN.
We initialize our gradient ascent inference using four different conﬁgurations
(grasp rectangle center, orientation, and gripper width) for a given object location.
For all initializations we set the bounding box center to the estimated center of the
segmented object and provide a random orientation. We initialize the gripper width to
be 30 pixels. We perform inference initialized with all four conﬁgurations and select
the conﬁguration with highest predicted success probability to execute. We set the
maximum number of iterations for gradient ascent to 10 and set the initial step size
of the backtracking line search to 0.05. We set the maximum number of iterations
for the backtracking line search to 5 per ascent step and the control parameter of
backtracking line search to 0.5. We transform the rectangle found through inference
to a 3D grasp pose using the same method as [ 13]. If the robot can then lift the object
without dropping it to a height 0.2 m, we label it successful.
We selected 10 objects, some coming from the YCB dataset, with varying shape,
texture, and visual properties comparable to the variability of the items used in [ 13].
Among these 10 objects different examples of the stapler, screw driver, and umbrella
all appear in the Cornell dataset; however, the other 7 objects are entirely novel.
02 0 4 0 60 80 100PringlesAsusXtionScrew DriverUmbrellaFoam CubeLego BlocksT-ShirtPaint CanJelloStaplerAll
Success Rate (%)
Initialization Inference00 .20 .4 0.6 0.810.090.470.240.620.020.660.490.010.230.050.29
0.970.990.990.990.980.960.960.990.990.990.98
Unseen Avg. Predicted Success Probability
Fig. 9 Success rate for executed grasps and the associated probability of grasp success predicted
by the network for Baxter 2-ﬁnger grasps
470 Q. Lu et al.
For each object we evaluate grasps at three random poses. As a comparison to our
inference approach we test all four random initial conﬁgurations for each object.
Figure 9shows the success rate and success probability predicted by the patches-
CNN for both inference and the initial heuristic grasps for all experiments. The
inference signiﬁcantly improves upon the initializations with an average success
rate of 84% across all objects. This performance is comparable to the success rateshown in [ 13]. However, our inference takes only 4 initializations to achieve the same
success rate, far fewer than the number of grasp samples required in [ 13]. Inline with
our multi-ﬁnger experiments, we see that the CNN gradient inference can improveinitial conﬁgurations which would fail to generate successful grasps. However, the
trained network is over conﬁdent in its prediction of success probability predicting
at lest 96% conﬁdence, while only being successful 84% of the time.
5 Conclusions
We presented a novel approach for multi-ﬁngered grasp planning formulated as prob-
abilistic inference in a learned deep neural network. Our planning algorithm generally
achieves higher grasp success rates compared with sampling-based approaches cur-rently used for grasping with neural networks, while still running fast enough for
use in a deployed robotic system. We showed the successful application of our grasp
optimization approach on two novel neural network architectures. Additionally, ourCNN classiﬁcation approach to learning grasp success allows for more data-efﬁcient
learning of grasps compared to directly predicting grasps as output of a neural net-
work regression model.
Our multi-ﬁngered grasping results leave room for signiﬁcant improvement.
Importantly any improvement to the underlying classiﬁer will immediately be lever-
aged by our planner. As immediate extensions we are examining the use of learnedpriors over grasp parameters, θ, as well as using priors over the neural network
weights w, to have better-calibrated predictions of the probability of grasp success,
for reliable use within task-level planners. Finally, we wish to explore active datacollection to further improve the data efﬁciency of our learning algorithm while also
learning a wider variety of grasps.
Acknowledgements Q. Lu and B. Sundaralingam were supported in part by NSF Award #1657596.
References
1. Bohg, J., Morales, A., Asfour, T., Kragic, D.: Data-driven grasp synthesis-a survey. IEEE Trans.
Robot. 30(2), 289–309 (2014)
2. Calli, B., Singh, A., Walsman, A., Srinivasa, S., Abbeel, P ., Dollar, A. M.: The YCB object and
model set: towards common benchmarks for manipulation research. In: International Confer-ence on Advanced Robotics (ICAR), pp. 510–517 (2015)
Planning Multi-ﬁngered Grasps as Probabilistic Inference in a Learned Deep Network 471
3. Ciocarlie, M., Goldfeder, C., Allen, P .K.: Dimensionality reduction for hand-independent dex-
terous robotic grasping. In: IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), pp. 3270–3275 (2007)
4. Dragiev, S., Toussaint, M., Gienger, M.: Gaussian process implicit surfaces for shape estimation
and grasping. In: IEEE International Conference on Robotics and Automation (ICRA), pp.2845–2850 (2011)
5. Glorot, X., Bengio, Y .: Understanding the difﬁculty of training deep feedforward neural net-
works. In: International Conference on Artiﬁcial Intelligence and Statistics, pp. 249–256 (2010)
6. Grupen, R.A.: Planning grasp strategies for multiﬁngered robot hands. In: IEEE International
Conference on Robotics and Automation (ICRA), pp. 646–651 (1991)
7. Gschwandtner, M., Kwitt, R., Uhl, A., Pree, W.: BlenSor: blender sensor simulation toolbox.
In: Advances in visual computing, pp. 199–208 (2011)
8. Gualtieri, M., Ten Pas, A., Saenko, K., Platt, R.: High precision grasp pose detection in dense
clutter. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.598–605 (2016)
9. Johns, E., Leutenegger, S., Davison. A.J.: Deep learning a grasp function for grasping under
gripper pose uncertainty. In: IEEE/RSJ International Conference on Intelligent Robots and
Systems (IROS), pp. 4461–4468 (2016)
10. Kappler, D., Bohg, J., Schaal, S.: Leveraging big data for grasp planning. In: IEEE International
Conference on Robotics and Automation (ICRA), pp. 4304–4311 (2015)
11. Kopicki, M., Detry, R., Adjigble, M., Stolkin, R., Leonardis, A., Wyatt, J.L.: One-shot learning
and generation of dexterous grasps for novel objects. Int. J. Robot. Res. (IJRR) 35(8), 959–976
(2016)
12. Kumra, S., Kanan, C.: Robotic grasp detection using deep convolutional neural networks. In:
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2017)
13. Lenz, I., Lee, H., Saxena, A.: Deep learning for detecting robotic grasps. Int. J. Robot. Res.
(IJRR) 34(4–5), 705–724 (2015)
14. Levine, S., Pastor, P ., Krizhevsky, A., Ibarz, J., Quillen, D.: Learning hand-eye coordination
for robotic grasping with deep learning and large-scale data collection. Int. J. Robot. Res. p.0278364917710318 (2016)
15. Mahler, J., Liang, J., Niyaz, S., Laskey, M., Doan, R., Liu, X., Goldberg, K.: Dex-net 2.0:
deep learning to plan robust grasps with synthetic point clouds and analytic grasp metrics. In:Robotics Science and Systems (RSS) (2017)
16. Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Robotic Manipulation. CRC
Press, Boca Raton (1994)
17. Pinto, L., Gupta, A.: Supersizing self-supervision: learning to grasp from 50K tries and 700
robot hours. In: IEEE International Conference on Robotics and Automation (ICRA), pp.3406–3413 (2016)
18. Redmon, J., Angelova, A.: Real-time grasp detection using convolutional neural networks. In:
IEEE International Conference on Robotics and Automation (ICRA), pp. 1316–1322 (2015)
19. Sahbani, A., El-Khoury, S., Bidaud, P .: An overview of 3D object grasp synthesis algorithms.
Robot. Auton. Syst. 60(3), 326–336 (2012)
20. Saxena, A., Driemeyer, J., Ng, A.Y .: Robotic grasping of novel objects using vision. Int. J.
Robot. Res.(IJRR), 27(2), 157–173 (2008)
21. Saxena, S., Wong, L.L.S., Ng, A.Y .: Learning grasp strategies with partial shape information.
In: AAAI National Conference on Artiﬁcial Intelligence, pp. 1491–1494 (2008)
22. Singh, A., Sha, J., Narayan, K.S., Achim, T., Abbeel, P .: Bigbird: a large-scale 3D database of
object instances. In: IEEE International Conference on Robotics and Automation (ICRA), pp.509–516 (2014)
23. V arley, J., Weisz, J., Weiss, J., Allen, P .: Generating multi-ﬁngered robotic grasps via deep
learning. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),pp. 4415–4420 (2015)
472 Q. Lu et al.
24. V eres, M., Moussa, M., Taylor, G.W.: Modeling grasp motor imagery through deep conditional
generative models. IEEE Robot. Autom. Lett. 2(2), 757–764 (2017)
25. Zhou, Y ., Hauser, K.: 6DOF grasp planning by optimizing a deep learning scoring function. In:
Robotics: Science and Systems (RSS) Workshop on Revisiting Contact - Turning a Probleminto a Solution (2017)
Reinforcement Learning for Assisted
Visual-Inertial Robotic Calibration
Fernando Nobre and Christoffer Heckman
1 Introduction
Common to all robotic applications are some set of parameters—camera intrinsics,
sensor extrinsics, biases, scale factors, model parameters, etc.—that are essential forhigher level robotic tasks such as state estimation, planning and control. These param-
eters are called calibration parameters and are usually obtained ofﬂine with spe-
ciﬁc and sometimes sophisticated calibration routines, or online in a self-calibrating
framework that relies on sufﬁciently exciting motions and naturally occurring data.
“Sufﬁciently exciting motions” is a recurring phrase in almost all expositions ofself-calibration in the literature [ 13]. While there has been work on determining
the observability of different motions [ 13], the full observability of the calibration
parameters may not be guaranteed for an arbitrary measurement sequence. This ren-ders the calibration procedure non-trivial for an inexperienced operator, especially
in non-holonomic platforms such as ground vehicles; it is simply not obvious how
to physically move the platform so as to collect measurements that allow the desiredparameters to be inferred.
In the context of life-long autonomous operation, self-calibrating systems are a
necessity since they allow for compensation of errors induced over time, such asafter a collision or due to sensor changes. Robust self-calibration also allows for
using the same algorithm on multiple platforms and foregoing the ofﬂine calibration
routine entirely. The downside of self-calibration is that it considerably increases thedimensionality of the state space while providing no additional measurements. Thus
the task of collecting measurements that render the full state-space observable is non-
trivial. In practice this means that self-calibrating systems require speciﬁc motionsthat are executed by an expert operator who excites the platform until the desired
states have converged to acceptable values, or when no operator is available, hoping
F. Nobre ·C. Heckman ( B)
University of Colorado, Boulder, CO 80309, USAe-mail: christoffer.heckman@colorado.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_36473
474 F. Nobre and C. Heckman
the platform will undergo sufﬁcient excitation so as to render the states observable.
This problem can be addressed in two ways:
1Optimization : optimize over trajectories in order to generate motions which ren-
der observable parameters.
2Learning : determine which motions render the system observable through expe-
riences.
The ﬁrst option tackles the problem by optimizing over some measure of observ-
ability [ 9], such as the condition number of the linearized system, in order to produce
trajectories that maximize the observability of the state space. While appealing in
its elegance, this requires knowledge of the motion model of the platform, and can
be computationally demanding. The second option, which is the approach taken inthis paper, forgoes the process model and uses a model-free reinforcement learning
technique to learn which sequence of motions render the desired states observable.
The appeal of this approach is that it can be applied to any platform, holonomic ornon-holonomic, without the simplifying assumption of a process and environment
model. A use case is a non-expert operator needing to calibrate a rig (physical assem-
bly with sensors mounted) with a camera and an IMU (Inertial Measurement Unit).
In currently-available calibration libraries [ 2] the user is tasked with waving the rig
around until the calibration parameters are obtained with satisfactory uncertainty.Our approach allows for the system to learn which sequences of motions contain
useful segments that render the desired state-space observable. Once the informative
sequence of motions is learned, these can be suggested to any user, removing therandom “hope-that-this-motion-is-sufﬁciently-exciting” approach in favor of a series
of suggested motions that will obtain the desired calibration parameters determinis-
tically.
Unsurprisingly, self-calibration has received considerable attention in the robotics
community, and has proven to be a hard problem due to the slow time-varying
nature (drift over time) and inference over naturally occurring data. The ﬁrst of theseproblems has been addressed in part by existing self-calibrating algorithms [ 21], the
second gives rise to a series of problems that are less commonly considered:
1Observability during normal operation : normal operation may not render cali-
bration parameters observable if e.g. two cameras do not share an overlapping ﬁeld
of view. In this case, planar motion renders the problem of ﬁnding the camera-to-camera extrinsic parameter degenerate.
2Parameters that appear observable, but in fact are not were noise not present :
related to the general observability of calibration parameters, it is possible that asolution is numerically obtained even in degenerate cases due to noisy measure-
ments, leading to a physically uninterpretable solution. An example of this is a
rank-deﬁcient matrix which is made numerically full-rank due to noise. Whilea solution is possible, it has no physical meaning. This is rarely addressed in
calibration systems.
3Which motions will render unobservable dimensions in parameter space
observable : calibration experiments are usually hand-engineered to guarantee
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 475
that all parameters become observable, especially for platforms for which a
process model is not available or desirable, this makes calibration a very tedious
and error-prone activity for the average operator, since it is not obvious how thesensor has to be excited.
Existing algorithms handle (1) by hoping sufﬁciently exciting motions are pro-
vided. Some work has been done to address (2) but it is largely unexplored in mostcalibration systems. To the best of our knowledge no published self-calibration algo-
rithm is able to cope with issue (3) without requiring a process model, some work
on this direction has been done by [ 22] but it is limited to camera intrinsics and our
approach does not require the user to follow the given instructions.
In this paper we propose an algorithm to deal with all of the presented difﬁculties
in calibration. Observability is handled by exploiting the link between the Fischer
Information matrix and nonlinear observability [ 11]. Treating numerically unobserv-
able parameters is performed by detecting directions in the parameter space that areunobservable through singular value thresholding of the scaled information matrix
and avoiding updating the current state estimate in those directions. Deciding which
motions will generate measurements that allow for inference over the desired param-eters is done by incorporating reinforcement learning for empirically learning which
sequence of motions maximizes the inference of the desired parameters.
The calibration procedure is modeled as a Markov Decision Process (MDP) and
Q-learning is employed to learn the optimal action-selection policy. The action-space
is discretized into motions that can be easily performed by a human operator, and the
state-space is deﬁned as the combination of possible parameter states. For example,the desired ﬁnal state is when every direction of the parameter space can be inferred,
intermediate states are composed of the subsets of the parameter space that may be
independently observed—when calibrating camera-to-IMU extrinsics, a set of mea-surements may render the rotation observable, but not the translation, then another
set of measurements may provide information on the translation (see Fig. 1). Thus
the only requirement of our system is that the sensors be equipped on a platform that
iscapable of performing motions that render the parameters observable, and that
the human-operator is able to loosely follow instructions on moving the platform.The main question we aim to answer is: can a MDP with delayed reward regress a
convergent policy for the calibration problem? The remainder of this paper will cover
related work in Sect. 2, the mathematical background in Sect. 3, the theoretical foun-
dation of our method is explained in Sects. 4,5validates our approach with simulated
and real-world experiments. Finally Sect. 6provides a discussion on our ﬁndings.
2 Related Work
The use of a known calibration pattern such as a checkerboard coupled with non-
linear regression has become the most popular method for camera calibration incomputer vision during the last decade; it has been deployed both for intrinsic cam-
476 F. Nobre and C. Heckman
(a) Suggested motions
0 5 10 15 20 25 30 35 40 45 50 55 6000.050.10.150.2error [m]x
y
z
0 5 10 15 20 25 30 35 40 45 50 55 60024·10−2
keyframeerror [rad]roll
pitch
yaw
(b) Extrinsics calibration with suggested motions
Fig. 1 a Motions suggested to an inexperienced operator for camera-to-IMU extrinsic calibration,
drawn from the learned policy. bConvergence of translation and rotation over the motions suggested
ina. Note how the ﬁrst suggested movements provide little to no information on translation, but allow
rotation to converge. This follows our practical knowledge that estimating rotation ﬁrst improves
convergence on translation
era calibration [ 25] and extrinsic calibration between heterogeneous sensors [ 26].
While being relatively efﬁcient, this procedure still requires expert knowledge to
reach a discerning level of accuracy. It can also be quite inconvenient on a mobile
platform requiring frequent recalibration (e.g experimental platforms which undergo
constant sensor changes). In an effort to automate the process in the context of mobile
robotics, several authors have included the calibration problem in a state-space esti-
mation framework, either with ﬁltering [ 19] or smoothing [ 14] techniques. Filtering
techniques based on the Kalman ﬁlter are appealing due to their inherently online
nature. However, in case of nonlinear systems, smoothing techniques based on iter-
ative optimization can be superior in terms of accuracy [ 24].
Our approach does not rely on formal observability analyses to identify degen-
erate paths of the calibration run as in [ 3], since these approaches still expect non-
degenerate excitations.
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 477
A last class of methods relies on an energy function to be minimized. For instance,
Levinson and Thrun [ 17] have deﬁned an energy function based on surfaces and
Sheehan et al. [ 23] on an information theoretic quantity measuring point cloud quality.
Despite considerable work in the ﬁeld of calibration, very little is known regard-
ing how to efﬁciently and actively deal with degenerate cases frequently occurring
during a calibration routine. The current state of the literature frequently assumesthat optimization routines are executed on well-behaved data. As demonstrated in
the next sections, this can be a critically ﬂawed assumption in real-world scenarios.
3 Problem Formulation
In the following exposition, we borrow the formalism of the probabilistic discrete-
time Simultaneous Localization and Mapping (SLAM) model [ 4]. For the sake of
clarity, we consider here a robot with a single camera observing a known number
of landmarks at each timestep and a single inertial measurement unit sampling lin-
ear acceleration and angular velocity. A front-end inspired by [ 16]i st a s k e dw i t h
establishing correspondences between sensor’s measurements and landmarks.1
4 Methodology
Let X={ x0:K}be a set of latent random variables (LRV) representing robot states
up to timestep K,L={ l1:N}a set of LRV representing Nlandmark positions, Z=
{z11:N:K1:N}a set of LRV representing K×Nlandmark measurements, and Θan
LRV representing the calibration parameters of the robot’s sensor. The goal of the
calibration procedure is to compute the posterior marginal distribution of Θgiven
all the measurements up to timestep K,
p(Θ|Z)=/integraldisplay
X,Lp(Θ, X,L|Z). (1)
The full joint posterior on the right-hand side may be factorized into:
p(Θ, X,L|Z)∝p(Θ, x0,L)K/productdisplay
k=1p(xk|xk−1,uk)K/productdisplay
k=1N/productdisplay
i=1p(zki|xk,li,Θ). (2)
By making the common assumption that Eq. ( 2) is normally distributed with mean
μΘXLand covariance /Sigma1ΘXLwe can derive a Maximum a Posteriori (MAP) solution
for the mean and covariance,
1The details of this implementation are omitted due to space limitations.
478 F. Nobre and C. Heckman
ˆμΘXL=arg max
ΘXLp(Θ, X,L|Z)=arg min
ΘXL−log p(Θ, X,L|Z); (3)
we further deﬁne our model by deﬁning an observation model zki=g(xk,li,Θ, nk)
where nk∼N(0,Nk)is a normally distributed observation noise variable, with
known covariance Nk. Given that the observation model is usually nonlinear, such
as a camera projection with lens distortion, we resort to a nonlinear least squares
method that iteratively solve a linearized version of the problem. We employ the
Gauss-Newton algorithm for this purpose.
Directly from Eq. ( 3) and the assumption of normally distributed measurements,
we can turn the MAP problem into the minimization of a sum of squared error
terms. This is covered in detail in [ 4], so we will brieﬂy cover only the aspects that
are relevant to our approach. The Gauss-Newton method only requires the Jacobian
matrix of error terms, J. In block matrix form the update is
(JTG−1J)δˆμΘXL=− JTG−1r(ˆμΘXL), (4)
where Gis the error covariance matrix built from diagonal blocks of Nkand r(ˆμΘXL)
is the error evaluated at the current state estimate. At convergence the quantity
JTG−1Jis the Fischer Information Matrix (FIM) and also the inverse of the esti-
mate covariance matrix, ˆ/Sigma1ΘXL. This is a key aspect of this work, since as will
be described below, the numerical rank of the FIM provides information about the
numerical observability of the parameters for a given batch of data. Speciﬁcally forcomputing the posterior distribution Θwe use the cost terms and Jacobians for both
camera intrinsics and camera-to-IMU extrinsics as deﬁned in [ 12,21].
4.1 Observability
There exists a solution to Eq. ( 4) if the FIM is invertible, i.e. it is of full rank. The
link between the rank of the FIM and observability of the parameters being esti-mated is well established in [ 11]. A singular FIM corresponds to some unobservable
directions in the parameter space given the current set of observations. Classical
observability analysis, for example the method of Hermann and Krener [ 10], proves
structural observability—that there exists some dataset for which the parameters are
observable—but it does not guarantee that the parameters are observable for anydataset.
Using singular-value decomposition (SVD) on the FIM we can identify a numer-
ically rank-deﬁcient matrix by analyzing its singular values and consequently thenumerical observability of the system [ 6,8,18]. The numerical rank r of a matrix is
deﬁned as the index of the smallest singular value σ
rwhich is larger than a pre-deﬁned
tolerance ε,
r=arg max
iσi≥ε. (5)
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 479
It is important to note that the numerical rank corresponds to the algebraic rank of the
unperturbed matrix within a neighborhood deﬁned by the parameter εproportional
to the magnitude of the perturbation matrix, which in this case can be interpreted asthe noise affecting the measurements. When the noise affecting the matrix entries
has the same scale (by using column or row scaling) then the numerical rank can be
determined by the singular values.
Speciﬁcally we decompose the error covariance matrix Gfrom Eq. ( 4) into its
square root form by using Cholesky decomposition, G
−1=LTL, we can re-write
Eq. ( 4) in standard form:
(LJ)T(LJ)δˆμΘXL=−(LJ)TLr(ˆμΘXL), (6)
which are the normal equations for the linear system (LH)δˆμΘXL=− Lr(ˆμΘXL).
Thus we can directly use a rank-revealing decomposition to estimate the numeri-
cal rank of the FIM, and consequently the numerical observability of the system.Both SVD [ 7] and QR decomposition [ 7] are rank-revealing; here we use SVD
to demonstrate the method, though we use the more computationally efﬁcient QR
decomposition in practice. Let (LJ)be a m×nmatrix with the following SVD
decomposition:
LJ=USV
T, (7)
where Uism×nand orthogonal, S=diag(σ1,...,σ n)the singular values and V
ann×nmatrix, also orthogonal. From Eq. ( 7) and the orthogonality of Uand Vwe
can solve ( 4)a s
δˆμΘXL=− VS−1ULr(ˆμΘXL) (8)
In order to only update the observable directions of the parameter space, we apply
the truncated SVD (or truncated QR decomposition) which establishes the rank of
the system by analyzing its singular values [ 8], only using the ﬁrst rrows of S,a s
deﬁned in Eq. ( 5). This allows us to only update the observable directions of the
parameter space and maintain the other directions at their initial value. Establishing
the value of εto use is speciﬁc to the amount of noise expected in the measurements
and is treated in Sect. 5. Using the method described in this section we are able to
identify which subset of the calibration parameters are observable, and when the full
parameters space is observable, which is a key element of the algorithm describedin the following section.
4.2 Learning Motions
The methodology described up to this point allows us to optimize for the calibrationparameters while identifying unobservable directions in the parameter space. Wewill now describe using that result in order to learn which sequence of motions lead
to the regression of the full parameter space.
480 F. Nobre and C. Heckman
We consider a robot moving through space (or manipulated by an operator) as
anagent situated in some feature-rich environment comprised of both the position
of the robot in the environment and the latent calibration variables. The agent can
perform certain actions in the environment (e.g. move to a new location, make new
measurements). These actions may result in a reward (e.g. information on a latent
variable). Actions can thus transform the environment (e.g. new conﬁguration of
latent variables) and lead to a new state, which the agent can perform another action
on, and so forth. The rules for how to chose which action to take given the current
state is called a policy , which must consider the stochasticity of the environment
(e.g. the choice of action, such as moving the robot forward, may or may not obtain
information about a latent variable depending on the structure of the world). The set
of states and actions along with the rules for transitioning make up a Markov decision
process, and one episode of this process corresponds to a sequence of state, action,
rewards. The episode ends when the terminal state is reached.
The calibration problem can be interpreted as an MDP in which each state repre-
sents the regression of each direction of the calibration parameter space (see Fig. 2),
and the actions are a discretized set of movements that can be performed by the
operator. Drawing a parallel to a game, we wish to discover the optimal policy, and
therefore sequence of actions, for reaching our ﬁnal state taking into account not
only the immediate reward of an action (i.e. learning about a speciﬁc parameter) but
the future rewards. To illustrate how this applies to calibration, consider a simpli-
ﬁed example: If the robot knows nothing about its camera-to-IMU calibration, but it
knows that given a certain movement it will learn the camera-IMU translation along
the xdirection, and given another movement, it will learn the rotation about the
x-axis. Note that we assume here that no feasible movement will learn both simul-
taneously. From an immediate reward standpoint it may seem arbitrary, but by ﬁrst
regressing rotation we are able to reach the full ﬁnal calibration in fewer movements.
We wish to learn the optimal policy for calibration. The state discretization is based
on the choice of εas described in Sect. 4.1which is the threshold that determines
∅f, c+25
fx,y
cx,ya00.4
0.6a00.7
0.1
+100a1
+250.50.5a10.3
0.9+100
a1a0+100
0.6
0.40.80.2+100
Fig. 2 Example of discretized state space and actions for camera intrinsic calibration (pinhole
camera model). Dark circles correspond to possible stochastic actions {a1,a2}. Blue arrows indicate
a reward for that transition. We have grouped {fx,fy}and{cx,cy}for simplicity. All possible
transitions are not shown for readability
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 481
the rank deﬁciency and therefore the observable directions of the parameter space.
We have empirically set ε=0.015, however it does need to be adjusted if we were
to use a system with a different noise proﬁle.
Q-Learning is well suited for the class of problem. Brieﬂy, within reinforcement
learning, Q-Learning is a model-free technique which can be used to ﬁnd an optimal
action-selection policy for a ﬁnite MDP. The basic principle is to maximize thediscounted future reward. The discounted aspect is due to the stochastic nature of
the environment and this to down-weigh the uncertain future rewards. This method
essentially consists of establishing a discrete set of actions (Sect. 4.2) and states, a
reward table for state transitions, and a Q-table which contains one row for each
state and a column for each possible action, which encodes the “quality” of a certain
action in a given state. Given our ﬁnite state space and discretized action space weare able to iteratively approximate the Q function using the Bellman equation :
Q(s,a)=r+γmax
a/primeQ(s/prime,a/prime), (9)
Which is the reward for the state transition plus the maximum possible future reward
for the next state. The idea behind Q-learning is that we can iteratively converge
on the Q-table using the Bellman equation, see Algorithm 1. In practice we also
use a learning rate parameter α=0.1 which essentially limits the step size for each
iteration. Once the Q-table has converged we can simply follow the learned policy
π, given the current state: π(s)=arg maxaQ(s,a)and suggest that action to the
user.
Algorithm 1: Observability-aware calibration Q-Learning
Data : sensor measurements, γ,α, Reward matrix
Result : converged Q-table
Initialize Q-table [states ,actions ]←0;
while Q-table not converged do
select random initial state;
while goal state not reached do
select possible action, a, given current state;
display action ato be carried out to user;
compute δˆμΘXL←− VS−1ULr(ˆμΘXL)according to ( 8);
compute observable directions according to 5;
ifat least one calibration direction is observable then
go to corresponding state s/primeand observe reward r;
else
stay in current state and set r←0;
end
update Q-table:
Q[s,a]← Q[s,a]+α/parenleftbig
r+γmax a/primeQ[s/prime,a/prime]− Q[s,a]/parenrightbig
;
end
end
482 F. Nobre and C. Heckman
We apply this to both regressing camera intrinsics and extrinsics. Note that the
important data association, outlier rejection, residual and Jacobian calculation steps
are delegated to a sparse visual-inertial keyframe-based SLAM system which wewill not go into detail here. In both intrinsic and extrinsic cases the reward table is
straightforward: a reward of 25 is given for non-ﬁnal state transitions and a reward
of 100 is given for any transition to the ﬁnal state (full calibration). Figure 2shows
a simple example where only two actions are possible, and the blue arrows indicate
the reward value for that state transition. These values were obtained empirically.
We found that using a constant α=0.1 works well, although experimenting with
variable step sizes could lead to quicker convergence. We initialize the Q-table to
zero and use γ=0.9. We experimented with a variable discount factor [ 5] but found
no practical beneﬁt. The outer loop is executed until the convergence criteria is met.We use a relative change metric to quit the learning process, with a maximum number
of iterations max \_ite r=1000.
Action Discretization Given that this work is focused on providing intuitive and
simple calibration instructions to a inexperienced operator, we discretize the initially
continuous action space (possible movements performed by the rig) into a set ofmovements that can be easily conveyed and performed by the operator. This consists
of translation along one degree of freedom combined with rotation around a single
axis. Empirically we found this to be the optimal trade-off for basis motions thatare both sufﬁciently informative and simple enough for the operator to execute.
Degenerate motions such as pure rotation or translation were not included. This
results in a tractable set of only 18 actions. The motions are also limited to therange of motion of the average human arm, between 60 and 80 cm. Given the goal
of providing easy-to-understand motion suggestions we chose simpler basis motions
which may lead to more actions being performed (i.e. a more complex motion couldcontain more information than the simple motions suggested here, but would be
harder to execute). Figure 3shows a few learned motions from the discrete set of
actions. The motion discretization described requires that the platform execute those
motions when training. The training procedure consists of either simulations, as
shown in Sect. 5.2or the the rig being moved around the environment (either by an
operator or autonomously) in the latter case we suggest the discretized motions to
be executed by the operator.
5 Experiments
We evaluate the proposed framework on several fronts. First we describe results
from our simulated experiments, using synthetic visual and inertial data to train oursystem and evaluate the performance of calibration with the synthetically trained
motions vs. trained with real data. We then demonstrate that the system can be
successfully applied to both camera intrinsics and camera-to-IMU extrinsics, andthat our method performs equally or better than publicly available calibration tools.
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 483
−0.500.5−0.5
0
0.5
1−0.5
0
0.5
xyz
00.5−0.500.51−0.5
0
xyz
Fig. 3 Left image shows learned policy for regressing camera intrinsics for a radial-tangential
distortion model, with a narrow ﬁeld of view lens ( f=460px) and central point in the middle of
the image. Changing to a ﬁsheye lens and a much wider ﬁeld of view ( f=220px) changes the
learned optimal action sequence (right ﬁgure). Each graph depicts the three suggested motions for
that camera
Finally we demonstrate that this system can be used for life-long learning by adapting
to changes in hardware conﬁguration that impact the calibration routine.
5.1 Experimental Setup
The proposed method was implemented in C++ and integrated into our existing
sparse keyframe-based visual-inertial SLAM pipeline which uses BRISK [15] feature
descriptors and ceres-solver [ 1] as the non-linear solver. In order to evaluate the
proposed method, experiments were run on a sensor platform known as “rig.” The rig
was equipped with a monocular camera and a commercial grade MEMS-based IMU.
The camera is equipped with a wide ﬁeld-of-view lens at 2040 ×1080 resolution
downsampled to 640 ×480 coupled with a MEMS IMU, sampled at 200 Hz. The
camera captures images at 30 frames per second.
In order to generate simulated measurements visual and inertial data correspond-
ing to each of the 18 basis motions was generated. A differentiable quaternion spline
through the SO(3) element of each pose was used to obtain smooth gyroscope mea-
surements, and a cubic spline was run through the euclidean positions for accelerom-
eter measurements. Simulated visual data was generated by creating a virtual world
in OpenGL and simulating movement corresponding to the accelerometer measure-
ments. Visual data was captured at 15 fps with IMU updates at 100 Hz. For each pro-
jected feature point from the simulated images (640 ×480 resolution), independent
zero-mean Gaussian noise with σ=0.5 was added to the (u,v)pixel coordinates.
Zero-mean Gaussian noise with σ=10−3was also added to the IMU accelerometer
and gyroscope measurements and biases.
484 F. Nobre and C. Heckman
5.2 Simulations
We generate noisy simulated measurements corresponding to the 18 basis functions
and run through the training procedure in Algorithm 1 until the relative change invalues for the Q-table is <1e
−3.
An experiment was run to determine if different calibration parameters would
suggest different motions. To that end we generated synthetic data to regress theQ-table for narrow (radial-tangential distortion) and wide ﬁeld of view ﬁsheye (fov
distortion) camera, which resulted in three considerably distinct set motions (see
Fig.3). We then used the suggested motions to execute the calibration procedure
with simulated measurements for both cameras. Using the suggested motions corre-
sponding to the camera which was used for training yielded on average a 22% better
mean and 8% lower variance, suggesting a correlation between the learned motions
and the camera distortion model and ﬁeld of view.
5.3 Real-World Data
Experiments with the rig described in Sect. 5require an interface for suggesting
motions to the user. Due to the simplicity of the motions we resort to a textualrepresentation, indicating what direction the rig should be moved in and rotation
about which axis. A graphical interface with visual feedback is in development. The
main objectives of this experiment are to verify that an inexperienced user can use thesuggested motions to easily and reliably calibrate a robotic system and obtain lower
variance compared to a publicly available standard calibration pipeline. To that end
we manually trained algorithm, then calibrated the same camera with a ﬁsheye lensby following the motions suggested by our system and also by waving a calibration
target in front of the camera and using vicalib [ 2], a publicly available calibration
library, to optimize over the camera intrinsics. This experiment was repeated 50
times for each calibration method in order to obtain statistically signiﬁcant results.
The users selected to perform the calibration ranged from lab members to randomvolunteers that did not have experience calibrating cameras. The results can be seen
in Fig. 6which clearly show that both methods converge to the same mean, but the
distribution is tighter around our method. Furthermore both extrinsic (Fig. 4) and
intrinsic (Fig. 5) calibration using motion suggestions converges to within 3 σof the
ground truth in as few as 40 keyframes. Finally Fig. 1shows an example of a sequence
of motions suggested for extrinsics calibration and the corresponding convergenceof translation and rotation. It is important to note that a feature-rich environment
assumption is made: there must be enough salient features for visual tracking. This
is the most common scenario so we are not sacriﬁcing much by forgoing an analysison the effects of the structure of the environment on calibration.
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 485
02 0 4 00.050.10.15[m]translation -x
02 0 4 0−0.1−0.050[m]translation -y
20 400.10.150.2
keyframe[m]translation -z02 0 4 00.780.80.820.84[rad]rotation - roll
20 400.480.50.520.540.56[rad]rotation - pitch
02 0 4 00.770.780.790.80.81
keyframe[rad]rotation - yaw
Fig. 4 Convergence of extrinsic parameters by an inexperienced operator following suggested
motions. The solid black line represents the ground truth as obtained by a batch optimization with
a target-based routine, the blue line represents the estimate as the rig is moved and the dotted red
line the 3 σbounds. The jumps in values correspond to the moment a new segment is processed. A
total of four motions were suggested for this calibration run
0 1 02 03 04 0300320340360380[px]focal length - fu
0 1 02 03 04 0250300350400[px]focal length - fv
0 1 02 03 04 0300320340360380
keyframes[px]central point - cu
0 1 02 03 04 0220230240250260
keyframes[px]central point - cv
Fig. 5 Convergence of camera intrinsic calibration parameters with real-data by following motion
suggestions. Dotted red line is the 3 σbound, solid black line is the ground truth value
486 F. Nobre and C. Heckman
100 200 300 400 500 6000510152025303540
Focal length (x)Occurencesvicalib
ourmethod
200 300 400 500010203040
Central Point (x)Occurencesvicalib
ourmethod
Fig. 6 Distribution of focal lengths and central point for all trials. The mean parameter values
between our method and vicalib are similar (focal length: 332.1px for vicalib and 334.8px for our
method), the standard deviations are much higher (32.1 vs. 21.2) indicating more consistent and
repeatable results
6 Discussion
In this paper, we have presented a novel calibration tool that uses reinforcement
learning to provide live feedback on the state of calibration and produces accurate
calibration parameters even when used by inexperienced operators. We have lever-
aged truncated SVD/QR decomposition to deal with unobservable directions and
reinforcement learning for motion suggestions to perform model-free calibration for
both camera intrinsics and camera-to-IMU extrinsics. We have evaluated the pro-
posed system in a variety of scenarios and shown that it can be used as a replacement
for currently available calibration toolkits. Our principal question was to assess the
suitability of reinforcement learning when applied to the calibration problem. We
have shown that through the discretization of both the action and state we are able
to leverage Q-learning to improve the calibration experience. An argument could
be made that a much larger state space would be possible when using a deep net-
work [ 20] instead of the Q-table, but we argue that for the calibration problem as
stated here, there is little practical advantage. A considerable challenge in the pro-
posed framework is designing the human interface so that the user will follow the
instructed motions. This is an area in which warrants further development, however
in the projected fully autonomous mode where motion suggestions are the input to
a controller, the human interface can be removed from the pipeline. An interesting
result that lends itself to long term autonomy is the ability to capture different sensor
conﬁgurations. As shown in Fig. 3having radically different calibration parameters
results in a different set of optimal motions, reinforcing the point that the “SLAM
wobble” or any pre-determined calibration maneuver is not guaranteed to provide
Reinforcement Learning for Assisted Visual-Inertial Robotic Calibration 487
acceptable parameter inference. If a robot is expected to calibrate itself autonomously
it cannot have a pre-determined routine or hope that random navigation will render
its calibration parameters observable. A system that is robust to sensor conﬁgura-tion changes by re-learning how to calibrate itself is a step in the direction of both
“power-on-and-go” robotics and long term autonomy.
Acknowledgements This work was supported by DARPA DSO “Ninja Car” award no. N65236-
16–1–1000.
References
1. Agarwal, S., Mierle, K.: Others: ceres solver. http://ceres-solver.org
2. Autonomous Robotics and Perception Group (ARPG): VICalib visual-inertial calibration suite.
https://github.com/arpg/vicalib (2016)
3. Brookshire, J., Teller, S.: Extrinsic calibration from per-sensor egomotion. Robot.: Sci. Syst.
VIII 5, 504–512 (2013)
4. Durrant-Whyte, H., Bailey, T.: Simultaneous localization and mapping: part I. IEEE Robot.
Autom. Mag. 13(2), 99–110 (2006)
5. François-Lavet, V ., Fonteneau, R., Ernst, D.: How to discount deep reinforcement learning:
towards new dynamic strategies. arXiv:1512.02011 (2015)
6. Gibbs, B.P.: Advanced Kalman Filtering, Least-squares and Modeling: A Practical Handbook.
Wiley, New York (2011)
7. Golub, G.H., Van Loan, C.F.: Matrix Computations, vol. 3. JHU Press (2012)8. Hansen, P.C.: Rank-Deﬁcient and Discrete Ill-Posed Problems: Numerical Aspects of Linear
Inversion. SIAM (1998)
9. Hausman, K., Preiss, J., Sukhatme, G.S., Weiss, S.: Observability-Aware Trajectory Optimiza-
tion for Self-Calibration with Application to UA Vs. arxiv.org (2016)
10. Hermann, R., Krener, A.: Nonlinear controllability and observability. IEEE Trans. Autom.
Control 22(5), 728–740 (1977)
11. Jauffret, C.: Observability and ﬁsher information matrix in nonlinear regression. IEEE Trans.
Aerosp. Electron. Syst. 43(2), 756–759 (2007)
12. Keivan, N., Sibley, G.: Constant-time monocular self-calibration. In: Robotics and Biomimetics
(ROBIO), pp. 1590–1595 (2014)
13. Kelly, J., Sukhatme, G.S.: Visual-inertial sensor fusion: localization, mapping and sensor-to-
sensor self-calibration. Int. J. Robot. Res. 30(1), 56–79 (2011)
14. Kümmerle, R., Grisetti, G., Burgard, W.: Simultaneous calibration, localization, and mapping.
In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.3716–3721. IEEE (2011)
15. Leutenegger, S., Chli, M., Siegwart, R.Y .: BRISK: binary robust invariant scalable keypoints.
In: 2011 IEEE International Conference on Computer Vision (ICCV), pp. 2548–2555. IEEE(2011)
16. Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., Furgale, P.: Keyframe-based visual-inertial
odometry using nonlinear optimization. Int. J. Robot. Res. 34(3), 314–334 (2015)
17. Levinson, J., Thrun, S.: Unsupervised calibration for multi-beam lasers. In: Experimental
Robotics, pp. 179–193. Springer, Berlin (2014)
18. Low, K.H.: Industrial robotics: programming, simulation and applications. I-Tech (2007)19. Martinelli, A., Scaramuzza, D., Siegwart, R.: Automatic self-calibration of a vision system
during robot motion. In: Proceedings 2006 IEEE International Conference on Robotics andAutomation, 2006. ICRA 2006, pp. 43–48. IEEE (2006)
488 F. Nobre and C. Heckman
20. Mnih, V ., Kavukcuoglu, K., Silver, D., Rusu, A.A., Veness, J., Bellemare, M.G., Graves, A.,
Riedmiller, M., Fidjeland, A.K., Ostrovski, G., et al.: Human-level control through deep rein-forcement learning. Nature 518(7540), 529–533 (2015)
21. Nobre, F., Heckman, C., Sibley, G.: Multi-sensor slam with online self-calibration and change
detection. In: International Symposium on Experimental Robotics (ISER) (2016)
22. Richardson, A., Strom, J., Olson, E.: Aprilcal: Assisted and repeatable camera calibration.
In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.1814–1821. IEEE (2013)
23. Sheehan, M., Harrison, A., Newman, P.: Self-calibration for a 3d laser. Int. J. Robot. Res. 31(5),
675–687 (2012)
24. Strasdat, H., Montiel, J., Davison, A.J.: Real-time monocular slam: why ﬁlter? In: 2010 IEEE
International Conference on Robotics and Automation (ICRA), pp. 2657–2664. IEEE (2010)
25. Sturm, P.F., Maybank, S.J.: On plane-based camera calibration: a general algorithm, singular-
ities, applications. In: IEEE Computer Society Conference on Computer Vision and PatternRecognition, 1999, vol. 1, pp. 432–437. IEEE (1999)
26. Zhang, Q., Pless, R.: Extrinsic calibration of a camera and laser range ﬁnder (improves camera
calibration). In: Proceedings. 2004 IEEE/RSJ International Conference on Intelligent Robots
and Systems, 2004. (IROS 2004), vol. 3, pp. 2301–2306. IEEE (2004)
Bayesian Optimisation for Safe
Navigation Under LocalisationUncertainty
Rafael Oliveira, Lionel Ott, Vitor Guizilini and Fabio Ramos
1 Introduction
Mobile robots have been successfully applied to many ﬁeld applications, such as
mining [ 9], planetary exploration [ 8], agriculture [ 25], and environmental monitoring
[10], to name a few. In all these applications, robots face environments with physical
characteristics that are a priori unknown and can heavily affect performance. In the
case of ground robots, terrain roughness can affect the ability of a robot to navigate
and even cause damage to its on-board hardware due to excessive vibration [ 23].
To aid in these problems, methods to enable the robot to automatically learn terrainproperties from its sensory data have been presented in the literature [ 8,18,23].
However, such methods usually assume that localisation is accurate enough, without
dealing with its inherent uncertainty.
Uncertainty in localisation can mislead learning algorithms with noise-corrupted
estimates of the location where measurements are taken. In addition, localisation
accuracy also affects navigation, since local path execution heavily depends on know-ing where the robot is with respect to a given reference path. As a result, localisation
inaccuracy can lead a robot into areas that are unsafe for navigation.
In this paper, we propose a method that allows a robot to actively search for,
and learn a model of, traversable areas while keeping itself safe by considering both
model and localisation uncertainty. In particular, we propose:
– a framework to account for input uncertainty in Bayesian optimisation [ 1]f o r
mission planning; and
– an adaptation of the DUCB [ 10] acquisition function to the context of exploration
under uncertain localisation.
In our method, we apply an extension of the conventional Gaussian process (GP) [ 20]
regression models to contexts involving input uncertainty [ 2]a sp r i o r sf o rt h eB O
R. Oliveira ( B)·L. Ott ·V . Guizilini ·F. Ramos
School of Computer Science, The University of Sydney, Sydney, Australiae-mail: rdos6788@uni.sydney.edu.au
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_37489
490 R. Oliveira et al.
framework. Such GP models allow BO to take into account noise in both the execution
of a query and in the observation’s location estimate. We apply the proposed method
to the task of learning a model of terrain roughness from experienced vibration data[23].
The remainder of this paper is organised as follows. In the next section, we review
relevant prior work in the areas of terrain modelling and Bayesian optimisation.Section 3revises the general GP-based BO framework, which does not consider
uncertain inputs. In Sect. 4we present our method for Bayesian optimisation under
localisation uncertainty. Then, in Sect. 5, we present experimental results in simula-
tion and on a physical robot to evaluate our approach. Finally, in Sect. 6, we conclude
and propose some directions for future work.
2 Related Work
Traversability metrics estimate how hard it is for a vehicle to traverse a certain ter-
rain. In robotics, terrain traversability is usually estimated from either LIDAR range
measurements [ 16] or stereo vision information [ 6]. Among the different kinds of
metrics, terrain roughness can play an important role in planning how a robot shouldnavigate on the terrain [ 16]. Methods to learn terrain roughness using image data
usually rely on learning image classiﬁcation models [ 7], which generally do not
provide a measure of how uncertain they are about their estimates and are computa-tionally intensive. Martin et al. [ 12], however, propose using vehicle experience data
to learn a Gaussian process (GP) regression model [ 20] to predict terrain traversabil-
ity with uncertainty estimates. Though restrictive when compared to exteroceptivesensing information [ 6], proprioceptive sensing can still be a very viable option for
traversability estimation with robots that are not equipped with stereo vision or 3D
LIDAR sensors, or do not possess the computational resources to process this kindof information on-board online.
Souza et al. [ 23] presented a method to learn a GP model for terrain roughness
from vehicle experience. The authors applied Bayesian optimisation (BO) [ 1]i n
an active perception approach to reduce experienced vibration during navigation
while learning the model from IMU measurements. In the BO framework, the terrain
roughness model is learnt online as the algorithm drives the robot around selectinglocations to visit balancing a trade-off between exploration and exploitation [ 1,23].
Nevertheless, BO usually considers deterministic query locations within its search
space, as BO is typically used in problems where a ﬁxed number of parameters have
to be optimised [ 22,27].
Recent work [ 15] presented a method to apply BO to problems where the execution
of a query is uncertain, such as robotic grasping. The authors propose querying BO’s
surrogate model using a Gaussian distribution by applying the unscented transform
[26]. In this, even with uncertainty in the execution of the query, the algorithm chooses
to sample the objective function at locations where interesting values are more likely
to be observed, instead of trying to reach a narrow peak. Despite that, Nogueira et al.
Bayesian Optimisation for Safe Navigation … 491
[15] still apply a conventional GP model as a prior for BO. In navigation problems,
the robot is usually able to obtain a probability distribution estimating its location,
which is given by a localisation system. Making use of a GP model that takes intoaccount such distributions as inputs should then allow BO to learn a better model of
the true underlying objective function.
To deal with uncertain location estimates in observations, besides the usual noisy
outputs, BO needs a statistical model that considers partial observations of the inputs
when sampling the function being modelled, usually called the objective function. In
BO the most common approach to modelling the objective function is using Gaussianprocess (GP) regression [ 20]. In the case of observations at uncertain inputs, Girard
[4] proposed methods to propagate input uncertainty through a GP model, developing
analytical approximations to compute covariance function values between inputsrepresented as Gaussian distributions. Another approach is to assume that location
estimates are corrupted by i.i.d. Gaussian noise, similar to the output observations in
standard GP regression. Then a method to propagate input uncertainty to the outputof the model using a ﬁrst-order Taylor approximation can be formulated [ 13]. Finally,
GP latent variable models [ 3], where the true unknown of the GP model are considered
as latent variables, apply a variational inference framework to compute the posterior
of this GP model under a set of assumptions about the input distributions. None of
these methods, however, have been applied to the BO context, where the GP modelis built in an online fashion as the algorithm proceeds.
3 Preliminaries
In this section, we review the concepts that form the basis of the method we propose.
We start with a general introduction to Gaussian process (GP) regression, which is
used to model terrain properties. Then we follow with a short review of Bayesianoptimisation (BO) using GP priors, which will be our planning technique.
3.1 Gaussian Process Regression
Gaussian process regression [ 20] is a Bayesian non-parametric framework that places
a Gaussian distribution as a prior over the space of functions f, mapping inputs
x∈Rdto outputs y∈R. Considering a mean function μ0:Rd→ Rand a positive-
deﬁnite covariance function k:Rd×Rd→ R, a GP prior models the distribution
of the function values y=[ y1,..., yn]Tat a given set of input locations X={ xi}n
i=1
as:
y|X∼N(μ0(X),k(X,X)) , (1)
where μ0(X)=[μ0(x1) ,...,μ 0(xn)]Tand[k(X,X)]ij=k(xi,xj). In many practi-
cal applications the prior mean μ0is set to zero or a constant that can be learnt from
492 R. Oliveira et al.
a dataset. For the covariance function there are a few popular choices, one of them
is the squared exponential kernel:
k(x,x/prime)=σ2
fexp/parenleftbigg
−1
2(x−x/prime)TW−1(x−x/prime)/parenrightbigg
, (2)
where σ2
fis a signal variance parameter and Wis a d×ddiagonal matrix with
[W]ii=λ2
i, and each λiis a length-scale parameter indicating how much the function
can vary along the ith dimension.
Given a set of tobservations Dt={ xi,zi}t
i=1, containing both noise-corrupted
outputs zi=f(xi)+εiwithεi∼N(0,σ2
n)and deterministic input locations xi,t h e
values for y∗=f(x∗)at a given query location x∗are then distributed according to
a Gaussian posterior:
y∗|Dt,x∼N(μ( x∗),σ2(x∗)) , (3)
where:
μt(x∗)=μ0(x∗)+k(x∗,X)K−1
X(z−μ0(X)) (4)
σ2
t(x∗)=k(x∗,x∗)−k(x∗,X)K−1
Xk(X,x∗), (5)
using KX=k(X,X)+σ2
nI. Therefore, GP models can be used as priors to model
functions that are not directly observable during an optimisation process. Partialobservations z
tcan be collected from the function being optimised to incrementally
update the GP model. If the GP prior is appropriately speciﬁed, as more observations
are collected the variance in the posterior decreases, leading the mean to convergeon top of the true f.
3.2 Bayesian Optimisation
Consider the problem of searching for the global optimum of a function f:Rd→ R
in a certain compact region S⊂Rd, i.e.:
x∗=argmin
x∈Sf(x). (6)
Assume that fis unknown to us and we have only access to noisy observations of
its output z=f(x)+εwithε∼N(0,σ2
n)and that we can only sample the function
Ntimes.
Bayesian optimisation [ 1]a s s u m e st h a t fis a random variable itself and applies
a probability distribution over it using a statistical model. Using a Gaussian process
(GP) model, BO makes some prior assumptions about fencoded by the mean μ0(x)
and covariance function k.
Bayesian Optimisation for Safe Navigation … 493
Rather than directly searching over f,B Ou s e sa n acquisition function h (x)as a
guide to sequentially select input locations at which to observe f. The acquisition
function uses the information provided by the GP prior and the observations of fto
estimate an utility value for sampling fat a given x. So at each iteration, BO queries
the objective function fat the location of highest utility according to the acquisition
function, considering the data observed so far Dt−1, which is given by:
xt=argmax
x∈Sh(x|Dt−1). (7)
After observing fatxt, BO updates the GP observations dataset with the new pair
{xt,zt}, which improves the belief about f. After the GP update the algorithm pro-
ceeds to the next iteration, choosing an xt+1. The BO loop runs until a stopping
criterion is satisﬁed, which is usually deﬁned by a maximum budget of iterations. Inthe end of this process, BO obtains a model that approximates the objective function
fand an estimate of its optimum’s location x
∗.
One example of acquisition function that can be applied to problems involving
robotics navigation is the distance-penalised upper conﬁdence bound (DUCB) [ 10]:
hDUCB(x|Dt−1)=μt−1(x)+κσ t−1(x)−γd(xt−1,x), (8)
where d(xt−1,x)corresponds to the distance between the last sampled location and
the candidate x,μt−1andσt−1are given by the GP posterior with the observations
inDt−1, andκ> 0,γ> 0 are parameters to be set. κcontrols the exploration-
exploitation trade off, with higher values favouring areas of high uncertainty, whileγpenalises large jumps, allowing shorter paths between observations. In addition,
notice that for minimisation objectives, as in ( 6), DUCB can be applied by simply
ﬂipping the sign of the GP posterior mean to negative instead.
When the hyper-parameters of the GP model are unknown a priori, a few methods
can also be applied to adapt them online as BO runs. One of the simplest, but usually
effective, methods is to optimise the log-marginal likelihood of the GP [ 20]a f t e r
each observation, or after collecting a batch of observations. This optimisation can
be done by performing a few steps of gradient descent on the hyper-parameters after
the GP is updated.
4 Bayesian Optimisation Under Localisation Uncertainty
In this section we present our framework for Bayesian optimisation under uncertain
inputs. By inputs, we refer to points in the objective function f(Eq. 6) input space,
i.e. any x∈Rd. Input noise can affect both the execution of a query to the objective
function and the estimates of the locations where these observations are taken, as
we further explain in Sect. 4.2. In this sense, we ﬁrst need to extend BO’s surrogate
model to incorporate uncertainty in the inputs. We propose to use a Gaussian process
494 R. Oliveira et al.
regression model with uncertain inputs [ 2] as a prior, which we then incorporate
into BO to obtain a framework for optimisation under uncertain inputs. We start by
explaining this GP model before going into details about the proposed BO approach.
4.1 Gaussian Process Priors with Uncertain Inputs
Consider that, due to measurement and execution noise, when evaluating a function f
at a desired input x∗∈Rd, the actual input location ˜x∗|x∗∼P∗where the observation
is collected at is not directly observable, but we have access to some estimate of its
probability distribution, here indicated by P∗. Assuming that f∼GP(μ0,k),t h e
distribution over ˜y∗=f(˜x∗)under input noise is no longer Gaussian due to the non-
linear input dependencies in the distribution of y|x, given by Eq. 1, and x. Therefore,
the resulting stochastic process that represents funder random inputs is no longer
Gaussian and lacks an analytic formulation [ 3,4]. However, as demonstrated by
Girard [ 4], we can still recover a Gaussian process approximation for fby using
the mean and the covariance of the resulting stochastic process under the inﬂuence
of input noise. In particular, in the case of a constant deterministic mean function
μ0(x)=m0, the mean and the covariance of this noisy process are:
E˜x∗∼P∗[f(˜x∗)]= m0 (9)
CovPi,Pj[˜yi,˜yj]= EPi,Pj[k(˜xi,˜xj)], (10)
where it is assumed that ˜xiand˜xjare independent. Here Edenotes expected value
and Cov stands for covariance . Therefore, the expected covariance function is given
by:
EPi,Pj[k(˜xi,˜xj)]=/integraldisplay
xi∈Rd/integraldisplay
xj∈Rdk(xi,xj)dPi(xi)dPj(xj)=kp(Pi,Pj). (11)
Case˜xiand˜xjrepresent the same location, the above equation becomes a single
expectation under the input distribution. Depending on the type of input distribu-tions and the original kernel for deterministic inputs k, approximate and analytical
solutions for Eq. 11are available in the literature [ 2,4,17]. An example is the squared
exponential kernel in Eq. 2. In this case, if the input distributions are Gaussian, the
resulting covariance function, according to Dallaire et al. [ 2], is given by:
k
p(N(ui,Σ i),N(uj,Σ j))=σ2
fexp/parenleftbig
−1
2(ui−uj)T(W+Σi+Σj)−1(ui−uj)/parenrightbig
|I+W−1(Σ i+Σj)(1−δij)|1/2,
(12)
where σ2
fand Ware the same hyper-parameters as described for the standard squared
exponential kernel in Eq. 2andδijdenotes the Kronecker delta.
Bayesian Optimisation for Safe Navigation … 495
The posterior of the stochastic process representing f∼GP(μ0,k)under input
noise is not Gaussian due to the non-linear form of Eqs. 4and 5with respect to x
and X. Yet we can obtain a suitable approximation [ 2] for the original fin the noisy
input setting by doing inference over a GP with mean m0and covariance function
kp, as deﬁned in Eq. 11.G i v e n tobservations, this approximation is obtained as:
˜y∗|Dt,P∗∼N(mt(P∗),vt(P∗)) (13)
with:
mt(P∗)=m0+kt(P∗)TK−1
t(z−m0) (14)
vt(P∗)=kp(P∗,P∗)−kt(P∗)TK−1
tkt(P∗), (15)
where m0is a t-dimensional mean vector with the constant m0as elements, kt(P∗)=
[kp(P∗,P1) ,..., kp(P∗,Pt)]T, and[Kt]ij=kp(Pi,Pj)+δijσ2
n.
4.2 Bayesian Optimisation Under Localisation Uncertainty
To extend BO to the context of uncertain inputs in robotics, we ﬁrst have to consider
how input noise affects the BO process. In robotics problems involving location
estimation, we can split input noise into two categories: localisation noise and exe-
cution noise. The ﬁrst refers to noise affecting the location estimate provided by the
robot’s localisation system, usually due to imperfections in motion sensing and in
other kinds of sensors, such as GPS devices. The second type, execution noise, isthe combined effect of everything affecting the execution of the robot’s path to a
given target location, such as localisation noise, uncertain motion dynamics, etc. As
an illustration, consider the example in Fig. 1.
In the BO context, execution noise determines the actual location at which an
observation will be taken, while localisation noise affects the estimation of that
location. Using an uncertain-inputs GP model, as deﬁned in Sect. 4.1, allows us to
take into account both execution and localisation noise in the BO algorithm. For
instance, assuming additive zero-mean noise, given a target x, the actual location
where the observation will be taken is:
˜x=x+ε
e, (16)
where εe∼Perepresents execution noise. The query location can then be modelled
as a random variable ˜x∼Px. In practice, we usually don’t know the execution noise
distribution Peexactly, and consequently don’t know Px. However, we can use an
approximate querying distribution ˆPx≈Pxto account for this uncertainty in the
query process. Considering that, at each iteration t, the resulting BO loop should
select as target location:
496 R. Oliveira et al.
Pt−1Pxt
xtPt
˜xt−1˜xt
Fig. 1 At time t−1, the robot is estimated to be at some ˜xt−1∼Pt−1by the localisation system.
It is then sent to target location xt. However, due to execution noise, the robot will actually end up
at another location ˜xt|xt∼Pxt. After moving, the localisation system provides an updated estimate
on˜xt, represented by Pt. The robot’s true locations and true path are indicated by the dashed lines
xt=argmax
x∈Sh(ˆPx|Dt−1), (17)
which is computed over the uncertain-inputs GP posterior. Under this formulation,
ˆPxprovides BO with a measure of how the true location distribution spreads around
a given target, due to execution noise. Therefore, any ˆPxthat can upper-bound the
variance of Pxshould be sufﬁcient for the algorithm to work.
After observing the objective function, we assume that BO is provided with
another distribution estimating the robot’s new location ˜xt∼Pt, provided by locali-
sation, which is a better estimate than ˆPx, but only available after the query execution.
The mean location estimate given by Ptis:
ˆxt=EPt[˜xt]=˜x∗
t+εl,t, (18)
where εl,trepresents localisation noise. Standard BO would utilise ˆxtas the input
to be added with the corresponding outcome ztinto the observations dataset Dt.
However, if the localisation noise level is signiﬁcant or if Ptis multi-modal, ˆxtmight
not be a good estimator. Fortunately, the GP model in Sect. 4.1allows us to use the
pair{Pt,zt}directly in the observations dataset, mitigating these effects.
As Eq. 17requires, the acquisition function needs to be able to handle probability
distributions as inputs by using the uncertain-inputs GP model. For some acquisition
functions, such as UCB [ 24], it can be as straight forward as replacing xby Px
and using the corresponding GP posterior mean m(Px)(Eq. 14) and variance v(Px)
(Eq. 15). For others, some modiﬁcations need to be made. In the case of DUCB, a
Bayesian Optimisation for Safe Navigation … 497
simple but effective modiﬁcation to the distance-penalty term is to use the distance
between the target location and the mean of the distribution of the last sampled
location ( EPt−1[˜xt−1]=ˆxt−1), i.e.:
hDUCB(ˆPx|Dt−1)=mt−1(ˆPx)+κv1/2
t−1(ˆPx)−γd(ˆxt−1,x). (19)
5 Experiments
In this section, we present experimental results obtained with our method for
uncertain-inputs Bayesian optimisation (UIBO) in the presence of localisation uncer-tainty. In all cases, we used the DUCB acquisition function [ 10] to guide the explo-
ration process. We compare our method against two other approaches. The ﬁrst is
standard BO, which does not consider any uncertainty in the location estimates. Thesecond is unscented Bayesian optimisation (UBO) [ 15], which considers execution
noise by means of the unscented transform [ 26], but uses a point estimate for the
location of each observation.
5.1 Simulations
We performed simulations using randomly-generated 2D functions as a model for
terrain roughness to be learnt by BO. The task is to ﬁnd areas of low terrain roughness
while staying away from areas of high vibration, which can cause damage to the robot.In addition, the learnt GP model should then be more accurate over areas of lower
terrain roughness, which are more interesting in practice.
For each test trial, a function is drawn from a reproducing kernel Hilbert space [ 21]
with kernel deﬁned as the input-noise-free covariance function and combined with a
constant mean so that vibration values positive within the search space. In our case, we
used the squared exponential (see Eq. 2) and its uncertain-inputs equivalent (Eq. 12)
as covariance functions. To simulate input noise, we sample execution noise from
a Gaussian N(0,σ
2
eI)and localisation noise from another Gaussian N(0,σ2
lI).T h e
querying distribution ˆPxapplied by UIBO is set to also be Gaussian N(x,σ2
xI)and is
the same distribution as applied by unscented BO (UBO). We set σe=σl=0.07 and
σx=0.1. Besides input noise, observation noise was set to σn=0.1. For DUCB’s
parameters, we set κ=10 and γ=1 for the three versions of BO, which were
manually tuned. The hyper-parameters for the GP models were ﬁxed and identical
for each BO method.
In the simulations, we also assessed the performance of maximum entropy search
(ES) as heuristics for BO, not to be confused with Hennig and Schuler’s method [ 5].
By maximum entropy search, we mean heuristics seeking only to reduce the entropyof the corresponding GP posterior, choosing to visit areas of high uncertainty to gain
498 R. Oliveira et al.
(a) Paths and underlying vibration map
(b) Mean experienced vibration
Fig. 2 Simulation results for standard BO, Unscented BO (UBO) and Uncertain-Inputs BO (UIBO)
methods for random functions modelling terrain-induced vibration. aPresents the true paths taken
by each BO method using DUCB over the true vibration map in one of the test trials. The markers
along the paths indicate the locations where observations were taken. The big “X” mark indicates
the starting location, which is the same for the three methods at each trial. The plots in bpresent
performance results in terms of mean intensity of the experienced vibration. The results were
averaged over 30 trials, each with different maps, and the shaded areas correspond to one standard
deviation
information. ES methods should provide a baseline for comparisons, since they do
not consider any estimate of the expected vibration in their search. Following ES
guidance, the robot should experience high amounts of vibration and long paths. For
fair comparisons, the methods based on deterministic-inputs GP models used the
equivalent noise-free version of the uncertain-inputs GP covariance function and the
same DUCB parameters, since these do not affect the GP model.
Figure 2presents results obtained in simulation. As seen in the test case in
Fig. 2a, standard BO and UBO using DUCB end up having a much more exploratory
behaviour due to noise, which leads the robot to execute long paths over the terrain.
UIBO, on the other hand, is able to focus its exploration on areas of lower vibra-
tion intensity. Overall, Fig. 2b shows that UIBO is able to signiﬁcantly improve over
iterations, ﬁnding areas of low vibration for the robot to navigate through. Both the
standard and the unscented versions of BO ended up having an average performance
Bayesian Optimisation for Safe Navigation … 499
Ta b l e 1 Simulation results: average performance comparisons for each BO approach under differ-
ent metrics with the corresponding standard deviation
Method RMSE WRMSE Distance travelled Relative vibration
BO-DUCB 0.59±0.09 0.31±0.05 31.25±1.98 0.95±0.04
UBO-DUCB 0.64±0.12 0.34±0.08 35.49±1.23 0.95±0.04
UIBO-DUCB 0.59±0.15 0.26±0.08 16.12±2.04 0.61±0.17
BO-ES 0.58±0.10 0.32±0.07 57.07±2.73 1.00
UBO-ES 0.64±0.12 0.34±0.08 67.15±2.56 1.00±0.03
UIBO-ES 0.38±0.05 0.22±0.05 62.85±2.06 0.99±0.03
much similar to pure entropy search (ES). As entropy-based methods are purely
exploratory, there is no improvement in terms of mean experienced vibration, since
the algorithm is not optimising for that. In the case of BO-DUCB and UBO-DUCB,
the uncertainty in localisation corrupts the location estimates in the observationsdataset, and this effect is not taken into account by the GP model. As a result, both
methods are misled into areas close to locations that they previously observed, ending
up into this more exploratory behaviour, instead of exploiting previous information.
Table 1presents a summary of the ﬁnal performance in terms of different met-
rics for each method. The root mean square error (RMSE) and weighted RMSE
(WRMSE) [ 10] values are computed from the corresponding GP posterior mean ( μ)
and the ground-truth values of fdeterministically queried over a uniform grid cover-
ing the entire search space. In our case, the WRMSE emphasises the reconstruction
error in areas of lower vibration, being computed as:
WR MS E =/radicaltp/radicalvertex/radicalvertex/radicalbt
1
nn/summationdisplay
i=1/parenleftbigg
(μ( xi)−f(xi))max jf(xj)−f(xi)
max jf(xj)−min jf(xj)/parenrightbigg2
(20)
This metric is a better performance indicator for this experiment’s task than the plain
RMSE, since we are more interested in ﬁnding areas of lower vibration.
As seen in Table 1, UIBO is able to outperform the other methods when using
DUCB, obtaining a good model, in terms of WRMSE, of the terrain roughness
while travelling the shortest distance and experiencing the least amount of vibration.
Entropy search using the uncertain-inputs GP was able to obtain the smallest RMSE
and WRMSE values, but required a much longer distance to be travelled under thecost of high vibration. In general, in terms of relative experienced vibration, BO and
UBO methods with DUCB performed worse due to the high amounts of localisation
noise, which led them to behave similar to BO-ES. These results indicate that UIBOis able to effectively take into account the uncertainty in localisation, keeping the
robot safe while exploring an unknown environment.
500 R. Oliveira et al.
5.2 Experiments with a Real Robot
We performed experiments with a physical robot outdoors to test the performance of
the proposed uncertain-inputs BO approach against standard BO with DUCB as in
[23]. The purpose of this experiment is to highlight the differences in performance and
in behaviour between both BO approaches when faced with localisation uncertainty
in a real-world scenario, where the robot is tasked with learning terrain roughness.
Experimental Setup Our test platform was a small four-wheeled skid-steer robot,
depicted in Fig. 3a. The robot is equipped with an on-board computer running ROS.1
The tests were performed in an area with terrain covered by grass and with some
portions of harder ground exposed. The goal of the robot is to ﬁnd areas of low terrain
roughness, while avoiding areas that induce excessive vibration to the platform. In
this kind of scenario, exposure to high amounts of vibration can cause damage to
the robot’s hardware, while interfering with readings from a few sensors, such as
cameras and LIDAR’s.
Our robot was equipped with an IMU sensor, placed on top of its chassis. With
the robot moving, vibration shows up mostly as linear acceleration along the robot’s
vertical axis, bouncing the robot up and down off the ground. We considered different
methods to measure vibration from a ﬁxed moving window of raw acceleration
measurements. In particular, we considered the mean and the root mean square values.
As we want the robot to avoid areas of high vibration, the RMS value demonstrated
itself to be a better estimator, since it grows with the amplitude of the vibration, as
(a) Robot platform (b) Vibration measurements
Fig. 3 Robot experiment details. aPresents our experimental platform. bCompares different
methods to measure vibration using an IMU: raw corresponds to the raw vertical acceleration
readings (discounting gravity), mean corresponds to the mean value of the raw measurements in
a moving window of size 200, and RMS corresponds to the root-mean-square value of the raw
measurements in the same window
1The Robot Operating System: www.ros.org .
Bayesian Optimisation for Safe Navigation … 501
pictured in Fig. 3b. The mean, on the other hand, should be always around zero, as
each upwards acceleration is immediately compensated by a subsequent fall.
As observations for the BO algorithms, the RMS vibration estimates were com-
puted over a sliding window of 100 IMU measurements and updated at a rate of 2 Hz
as the robot is driving. To reduce the effect of different driving speeds on the read-
ings, the vibration estimates were posted whenever the robot was driving at speedsbetween 0.4 and 0.7 m/s, where 0.7 m/s was the maximum speed allowed for the
path following control. These observations were combined with location estimates
from an extended Kalman ﬁlter (EKF) [ 14], which was conﬁgured to fuse wheel
odometry, IMU and GPS estimates.
We ran both plain BO and UIBO for a ﬁxed budget of 30 iterations, i.e. each
algorithm was allowed to choose 30 target locations to take the robot to. At eachiteration, the robot drove autonomously attempting to follow straight paths from
its previous goal to the next goal, as given by the BO planner. The iteration was
signalled as ﬁnished whenever the robot arrived within a given radius from the goal.After that, the GP model is updated with the new observations, the hyper-parameters
are re-learnt by maximising the GP log-marginal likelihood using COBYLA [ 19],
and the planner selects a new target. As the sample rate of observations is relatively
low, both BO methods were able to repeatedly execute this process online in close
to real time. The DUCB acquisition function was conﬁgured with an uncertaintyfactor κ=10 and a distance factor γ=0.5. UIBO applied as query distribution an
isotropic Gaussian with variance σ
2
x=4 for each coordinate. The choice of this value
forσxwas mainly based on a minimum distance-to-goal tolerance for the robot’s path
follower, which was set to 1.3 m to cope with the effects of localisation noise on each
path’s execution. Ideally, however, σxcould be estimated from data, which is left for
future work.
Performance Figure 4presents the GP model obtained by each BO method and
their paths, according to noisy EKF location estimates. For both methods, the robot
was placed at an initial location at the bottom left area of the search space. As Fig. 4
shows, UIBO is able to concentrate its search over areas of low vibration. On theother hand, standard BO ends up performing too much exploration passing more
often over areas that cause excessive vibration to the robot.
For performance comparisons, we collected a set of vibration measurements dur-
ing the experiments using a real time kinematic (RTK) GPS
2device, which was
employed to obtain high-precision location estimates. These measurements were
only collected for the purposes of validation, and not passed on to the BO planners,which relied solely on a conventional GPS device fused into the EKF estimates. The
magnitude of the error in the conventional GPS location estimates varied around 3 m
with respect to the RTK GPS estimates, which is able to provide centimetre accuracy.The posterior mean of a GP model built directly from the validation data is shown in
Fig. 5. This model is presented only for visualisation purposes and was not used to
compute performance metrics, which were solely based on raw validation data. The
2Emlid Reach RTK: https://emlid.com/reach/ .
502 R. Oliveira et al.
Fig. 4 Posterior mean of the GP model built by each BO method overlaid with their respective
paths according to locations estimated by an EKF fusing conventional GPS, IMU and odometry
Fig. 5 For visualisation purposes, posterior mean of a GP model built from the validation data
overlaid with the true paths taken by the robot, according to RTK GPS location estimates
Ta b l e 2 Field results: performance comparisons for each BO approach under different metrics.
The WRMSE was computed between the posterior mean of the ﬁnal GP model at the locations inthe validation dataset and the corresponding vibration measurements at those locations
Method WRMSE [m/s2] RMSE [m/s2]Distance travelled [m] Mean vibration [m/s2]
BO 1.04 1.31 247 3.0
UIBO 0.83 1.33 92 2.1
model’s hyper-parameters were determined by maximising its log-marginal likeli-
hood as {m0,l,σ f,σ n}={ 2.7,2.8,0.8,1.0}, where lis the kernel length-scale.
Table 2presents a performance summary. The WRMSE value for this experiment
was computed between the posterior mean from each method’s ﬁnal GP model, builtfrom noisy EKF location estimates, and the raw vibration measurements present in
the validation dataset, collected using precise RTK GPS location estimates. We can
see that, for this experiment, UIBO is able to outperform standard BO by producing
a better model over areas of low roughness while experiencing less vibration and
following a much shorter path, conﬁrming facts previously observed in simulations.
Bayesian Optimisation for Safe Navigation … 503
6 Conclusion
In this paper, we presented a new Bayesian optimisation method for active learning
problems in robotics in the cases where uncertainty in the location estimates issigniﬁcant. The proposed method provides a principled approach to account for this
uncertainty affecting the inputs of a Gaussian process model, which is applied as a
prior by the BO algorithm. Execution noise is also considered by means of a querydistribution when optimising BO’s acquisition function. The method was proved to
outperform other BO approaches in simulation and in an experiment with a physical
robot navigating over rough terrain. Therefore, the proposed method can be appliedto problems where we are interested in mapping the traversability of a terrain, but
need to keep the robot safe in the midst of localisation and model uncertainty.
Some topics were not addressed by this paper, but are worthy of future research
work. One of them is the estimation of execution noise, which could be done in an
online way by other statistical methods, for example, using maximum likelihoodestimates for the parameters of a given distribution. Another topic is the extension
of the DUCB-based exploration to informative continuous path planning [ 11]o rt o
trajectory optimisation in goal-directed navigation [ 18]. For that, however, a method
to propagate execution noise through a candidate path would have to be developed.
This requires taking into account hard-to-model factors from both the robot and the
environment, such as stochastic motion dynamics and imperfect sensor information.
Acknowledgements This work was supported by CAPES, Brazil (scholarship BEX 13224/13-1),
and by Data61/CSIRO, Australia.
References
1. Brochu, E., Cora, V .M., de Freitas, N.: A tutorial on Bayesian optimization of expensive cost
functions, with application to active user modeling and hierarchical reinforcement learning.Technical report, University of British Columbia (2010)
2. Dallaire, P ., Besse, C., Chaib-Draa, B.: An approximate inference with Gaussian process to
latent functions from uncertain data. Neurocomputing 74, 1945–1955 (2011)
3. Damianou, A.C., Titsias, M.K., Lawrence, N.D.: V ariational inference for latent variables and
uncertain inputs in Gaussian processes. J. Mach. Learn. Res. 17(1), 1–62 (2016)
4. Girard, A.: Approximate methods for propagation of uncertainty with Gaussian process models.
University of Glasgow, Ph.D (2004)
5. Hennig, P ., Schuler, C.J.: Entropy search for information-efﬁcient global optimization. J. Mach.
Learn. Res. (JMLR) 13, 1809–1837 (2012)
6. Ho, K., Peynot, T., Sukkarieh, S.: Traversability estimation for a planetary rover via experi-
mental kernel learning in a Gaussian process framework. In: IEEE International Conference
on Robotics and Automation (ICRA), pp. 3475–3482. IEEE, Karlsruhe, Germany (2013)
7. Komma, P ., Weiss, C., Zell, A.: Adaptive Bayesian ﬁltering for vibration-based terrain clas-
siﬁcation. In: Proceedings - IEEE International Conference on Robotics and Automation, pp.3307–3313. IEEE, Kobe, Japan (2009)
504 R. Oliveira et al.
8. Lang, T., Plagemann, C., Burgard, W.: Adaptive non-stationary kernel regression for terrain
modelling. In: Burgard, W., Brock, O., Stachniss, C. (eds.) Proceedings of the Robotics: Scienceand Systems Conference (RSS). Atlanta, GA (2007)
9. Maekawa, T., Noda, T., Tamura, S., Ozaki, T., Machida, K.I.: Curvature continuous path gen-
eration for autonomous vehicle using B-spline curves. Comput.-Aided Des. 42(4), 350–359
(2010)
10. Marchant, R., Ramos, F.: Bayesian optimisation for intelligent environmental monitoring. In:
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE (2012)
11. Marchant, R., Ramos, F.: Bayesian optimisation for informative continuous path planning. In:
IEEE International Conference on Robotics and Automation (ICRA), pp. 6136–6143 (2014)
12. Martin, S., Murphy, L., Corke, P .: Building large scale traversability maps using vehicle expe-
rience. In: The 13th International Symposium on Experimental Robotics (ISER), vol. 88, pp.891–905. Springer (2013)
13. Mchutchon, A., Rasmussen, C.E.: Gaussian process training with input noise. In: Advances in
Neural Information Processing Systems, pp. 1341–1349 (2011)
14. Moore, T., Stouch, D.: A generalized extended Kalman ﬁlter implementation for the robot oper-
ating system. In: Proceedings of the 13th International Conference on Intelligent Autonomous
Systems (IAS-13). Springer (2014)
15. Nogueira, J., Martinez-Cantin, R., Bernardino, A., Jamone, L.: Unscented Bayesian optimiza-
tion for safe robot grasping. In: IEEE International Conference on Robotics and Automation(ICRA), pp. 1967–1972. Daejeon, Korea (2016)
16. Nordin, P .: Mobile robot traversability mapping. Licentiate thesis, Linköping University (2012)17. O’Callaghan, S.T., Ramos, F.T.: Gaussian process occupancy maps. Int. J. Robot. Res. (IJRR)
31(1), 42–62 (2012)
18. Oliveira, R., Ott, L., Ramos, F.: Active perception for modelling energy consumption in off-
road navigation. In: Australasian Conference on Robotics and Automation (ACRA). Brisbane,QLD, Australia (2016)
19. Powell, M.: A View of Algorithms for Optimization Without Derivatives. Technical report,
Cambridge University DAMTP , Cambridge, United Kingdom (2007)
20. Rasmussen, C.E., Williams, C.K.I.: Gaussian Processes for Machine Learning. The MIT Press,
Cambridge, MA (2006)
21. Schölkopf, B., Smola, A.J.: Learning with Kernels: Support V ector Machines, Regularization,
Optimization, and Beyond. MIT Press, Cambridge (2002)
22. Snoek, J., Larochelle, H., Adams, R.P .: Practical Bayesian optimization of machine learning
algorithms. In: Pereira, F., Burges, C.J.C., Bottou, L., Weinberger, K.Q. (eds.) Advances inNeural Information Processing Systems, vol. 25, pp. 2951–2959. Curran Associates, Inc. (2012)
23. Souza, J.R., Marchant, R., Ott, L., Wolf, D.F., Ramos, F.: Bayesian optimisation for active per-
ception and smooth navigation. In: IEEE International Conference on Robotics and Automation(ICRA) (2014)
24. Srinivas, N., Krause, A., Kakade, S.M., Seeger, M.W.: Information-theoretic regret bounds
for Gaussian process optimization in the bandit setting. IEEE Trans. Inf. Theory 58(5), 1–16
(2012)
25. Underwood, J., Wendel, A., Schoﬁeld, B., McMurray, L., Kimber, R.: Efﬁcient in-ﬁeld plant
phenomics for row-crops with an autonomous ground vehicle. J. Field Robot. 34, 1061–1083
(2017)
26. Wan, E.A., van der Merwe, R.: The unscented Kalman ﬁlter for nonlinear estimation. In: Adap-
tive Systems for Signal Processing, Communications, and Control Symposium (AS-SPCC),pp. 153–158 (2000)
27. Wilson, A., Fern, A., Tadepalli, P .: Using trajectory data to improve Bayesian optimization for
reinforcement learning. J. Mach. Learn. Res. 15, 253–282 (2014)
T opometric Localization with Deep
Learning
Gabriel L. Oliveira, Noha Radwan, Wolfram Burgard and Thomas Brox
1 Introduction
Robot localization is essential for navigation, planning, and autonomous operation.
There are vision-based approaches addressing the robot localization and mappingproblem in various environments [ 8]. Constant changes in the environment, such as
varying weather conditions and seasonal changes, and the need of expert knowledge
to properly lay out a set of domain speciﬁc features makes it hard to develop a robust
and generic solution for the problem.
Visual localization can be distinguished according to two main types: metric and
topological localization. Metric localization consists of computing the coordinates of
the location of the observer. The coordinates of the vehicle pose are usually obtained
by visual odometry methods [ 16,21,22,24]. Visual metric approaches can provide
accurate localization values, but suffer from drift accumulation as the trajectory
length increases. In general, they do not reach the same accuracy as LiDAR-based
localization techniques. Topological localization detects the observer’s approximateposition from a ﬁnite set of possible locations [ 2,3,19]. This class of localization
methods provides coarse localization results, e.g. if a robot is in front of a speciﬁc
building or room. Due to its limited state space, topological approaches providereliable localization results without drift, but only rough position estimates.
Accurate, drift-free localization can be obtained by combining both approaches,
which is known as topometric localization [ 20]. In this paper, we present a novel
topometric localization technique that formulates metric and topological mapping
as learning problems via a deep network. The ﬁrst network computes the relative
visual odometry between consecutive images, while the second network estimates
G. L. Oliveira and N. Radwan—These authors contributed equally.
G. L. Oliveira ( B)·N. Radwan ·W. Burgard ·T. Brox
Department of Computer Science, University of Freiburg,Freiburg im Breisgau, Germanye-mail: oliveira@informatik.uni-freiburg.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_38505
506 G. L. Oliveira et al.
Fig. 1 The three main modules of our approach: metric ,topological and topometric . The metric part
called VONet estimates odometry values given two consecutive images. The topological module
denoted as LocNet provides the closest topological location of an input image, mapping it back
to a pose through a hash table. VONet and LocNet are trained independently of each other. The
optimization part fuses metric and topological predictions to produce the corrected topometric pose
the topological location. Both networks are trained independently and their outputs
are provided to our topometric fusion technique. By fusing the output of the visual
odometry network with the predicted topological location, we are able to produce
an accurate estimate that is robust to the trajectory length and has minimum drift
accumulation. An overview of the proposed method is shown in Fig. 1. We introduce
a real-world dataset collected from the Freiburg University Campus captured in a high
noise environment. We compare the proposed approach on the dataset, with state-of-
the-art visual odometry methods. The experimental evaluation shows minimum drift
accumulation using the proposed approach, while yielding comparable accuracy to
LiDAR-based localization methods. The dataset will be made publicly available to
simplify comparisons and foster research in visual localization.
2 Related Work
One of the seminal deep learning approaches for visual odometry was proposed by
Konda et al. [ 16]. They proposed a CNN architecture which infers odometry based
on classiﬁcation. A set of prior velocities and directions are classiﬁed through a
softmax layer to infer the transformation between images from a stereo camera. A
major drawback of this approach lies in modeling a regression problem as a clas-
siﬁcation one which reduces the representational capabilities of the learned model.
Other approaches [ 21,22,24] tackle the problem of visual odometry as a regres-
sion problem. Nicolai et al. [ 24] proposed a CNN architecture for depth images
based on LiDAR scans. Mohanty et al. [ 22] proposed a Siamese AlexNet [ 17] based
approach called DeepVO, where the translation and rotation outputs of the network
Topometric Localization with Deep Learning 507
are regressed through an L2-loss layer with equal weight values. Choosing weight
values for the translational and rotational components of the regression output is
explored by Melekhov et al. [ 21]. They propose a Siamese AlexNet [ 17] network
similar to DeepVO [ 22], where they add a weight term to balance the translational and
rotational errors. They additionally use a spatial pyramid pooling (SPP) layer [ 13]
which allows for arbitrary input image resolutions. Our metric localization approachshares similarities with [ 21], since we use SPP and a loss function which balances
translation and rotation losses. Our contribution to this problem is a new densely
connected architecture along with a different angular representation.
Another part of our approach is topological localization. With increasing focus on
the long-term autonomy of mobile agents in challenging environments, the need for
life-long visual place recognition has become more crucial than before [ 19]. In [ 2],
the authors present an end-to-end approach for large-scale visual place recognition.
Their network aggregates mid-level convolutional features extracted from the entire
image into a compact vector representation using VLAD [ 12], resulting in a compact
and robust image descriptor. Chen et al. [ 5] combine a CNN with spatial and sequen-
tial ﬁltering. Using spatio-temporal ﬁltering and spatial continuity checks ensures
consecutive ﬁrst ranked hypotheses to occur in close indices to the query image. In
the context of this work, the problem of visual place recognition can be considered
as a generalized form of topological localization. Similar to the visual place recogni-tion approaches presented above, the authors in [ 3] present a topological localization
approach that is robust to seasonal changes using a CNN. They fuse information
from convolutional layers at several depths, ﬁnally compressing the output into asingle feature vector. Image matching is done by computing the Hamming distance
between the feature vectors after binarization, thus improving the speed of the whole
approach. Similar to these approaches, we use a CNN architecture that aggregatesinformation from convolutional layers to learn a compact feature representation.
However, instead of using some distance heuristic, the output of our network is a
probability distribution over a discretized set of locations.
Topometric localization was explored by the works of Badino et al. [ 4] and Mazu-
ran et al. [ 20]. Badino et al. [ 4] proposed one of the ﬁrst methods to topometric
localization. They represent visual features using a topometric map and localizethem using a discrete Bayes ﬁlter. The topometric representation is a topological
map where each node is linked to a pose in the environment. Mazuran et al. [ 20]
extended the previous method to relative topometric localization. They introduceda topometric approach which does not assume the graph to be the result of an opti-
mization algorithm and relaxed the assumption of a globally consistent metric map.
While Mazuran et al. [ 20] use LiDAR measurements and Badino et al. [ 4]r e l yo n
a multi-sensory approach, which employs camera, GPS, and an inertial sensor unit,
our approach is based on two CNNs for metric and topological estimation and onlyrequires visual data.
508 G. L. Oliveira et al.
3 Methodology
This paper proposes a topometric localization method using image sequences from a
camera with a deep learning approach. The topometric localization problem consistsof estimating the robot pose x
t∈SE(2)and its topological node nt∈SE(2),g i v e n
a map of globally referenced nodes equipped with sensor readings [ 20,25].
We propose a deep CNN to estimate the relative motion between consecutive
images. In order to reduce the drift often encountered by these approaches we propose
a second deep CNN for visual place recognition. The outputs of the two networks are
fused to an accurate location estimate. In the remainder of this section, we describethe two networks and the fusion approach in detail.
3.1 Metric Localization
The goal of our metric localization system is to estimate the relative camera posefrom images. We design a novel architecture using Dense-blocks [10] as base, which
given a pair of images in a sequence (I
t,It−1)will predict a 4-dimensional relative
camera pose pt:
pt:= [Δxtr
t,Δrt]
where Δxtr
t:=xtr
t−xtr
t−1∈R2is the relative translation values between the position
xtr
tand its previous value xtr
t−1andΔrt:= [ sin(xθ
t−xθ
t−1),cos(xθ
t−xθ
t−1)]∈ R2
is the relative rotation estimation between xθ
tand the previous rotation xθ
t−1.W e
represent the rotation using Euler6 notation, i.e., the rotation angle θis represented
by two components [sin(θ),cos(θ)][27].
3.1.1 Loss Function
The proposed metric localization network is designed to regress the relative trans-
lation and orientation of an input set (It,It−1). We train the network based on the
Euclidean loss between the estimated vectors and the ground truth. Having a loss
function that deals with both the translation and orientation in the same manner wasfound inadequate, due to the difference in the scale between them [ 14]. Instead, we
deﬁne the following loss function:
L:=/vextenddouble/vextenddoubleΔˆx
tr−Δxtr/vextenddouble/vextenddouble
2+β/vextenddouble/vextenddoubleΔˆr−Δr/vextenddouble/vextenddouble
2, (1)
where ΔxtrandΔrare respectively the relative ground-truth translation and rotation
vectors and ΔˆxtrandΔˆrtheir estimated counterparts. We use the parameter β> 0
to balance the loss for the translation and orientation error.
Topometric Localization with Deep Learning 509
Fig. 2 Depiction of the proposed VONet architecture. Each Dense stream is fused after three
Dense Blocks. The fusion layer is responsible to concatenate the time dependent features into one
representation that is fed to another Dense Block and to a SPP layer. The SPP features serve as
input to a fully connected layer that is consequently split into speciﬁc layers, for translation and
orientation prediction
3.1.2 Network Architecture
To estimate the visual odometry or relative camera pose we propose a Siamese archi-
tecture built upon Dense-blocks [10] and spatial pyramid pooling (SPP) [ 13]. The
direct connections between multiple layers of a Dense-block yielded state-of-the-art
results on other tasks, and as we show in this paper, also yields excellent performance
for the task at hand. Figure 2shows the proposed VONet architecture. The network
consist of two parts: time feature representation and regression, respectively. The
time representation streams are built upon Dense-blocks with intermediate transition
blocks. Each Dense-block contains multiple dense layers with direct connections
from each layer to all subsequent layers. Consequently, each dense layer receives as
input the feature maps of all preceding layers. A dense layer is composed of four con-
secutive operations; namely batch normalization (Batch Norm) [ 11], rectiﬁed linear
unit (ReLU) [ 23], a 3 ×3 convolution (conv) and a drop-out. The structure of the
transition layer is very similar to that of a dense layer, with the addition of a 2 ×2
pooling operation after the drop-out and a 1 ×1 convolution instead of the 3 ×3
one. Furthermore, we alter the ﬁrst two convolutional operations in the time repre-
sentation streams by increasing their kernel sizes to 7 ×7 and 5 ×5, respectively.
These layers serve the purpose of observing larger image areas, thus providing better
motion prediction. We also modify the Dense-blocks by replacing ReLUs with expo-
nential linear units (ELUs), which proved to speed up training and provided better
results [ 7].
We fuse both branches from VONet through concatenation. We also tried fusion at
the fully connected layer level, however features from convolutional layers produced
better results. The fused features are passed to another Dense-block , which is followed
by a Spatial Pyramid Pooling (SPP) layer. SPPs are another main building block of
510 G. L. Oliveira et al.
our approach. Using SPP layers has two main advantages for the speciﬁc task we are
tackling. First, it allows the use of the presented architecture with arbitrary image
resolutions. The second advantage is the layer’s ability to maintain part of the spatialinformation by pooling within local spatial bins. The ﬁnal layers of our network are
fully connected layers to regress the two 2-dimensional vectors.
3.2 T opological Localization
Given a set of images acquired on a path during navigation, the task of topological
localization can be considered as the problem of identifying the location among a setof previously visited scenes. To this end, we ﬁrst pre-process the acquired images
to create the distinct key-frames, then we train a CNN that learns the probability
distribution over the likelihood of the key-frames given the input image.
In order to extract visually distinct locations from a given path, we cluster poses by
computing an image-to-image correlation score, so that similar images are grouped
together in one cluster. More precisely, we discretize the state space into cells of
size s
x×syand cluster the poses of the robot such that poses falling within the same
grid cell having similar orientation within a threshold αthform one cluster. We then
select clusters within a certain distance threshold dth, and generate the key-frames
by taking the mean pose of the individual poses within this cluster. We use a cell size
of 1×1m2andαth=10 deg. We experimented with the effect of different values
ofdthon the overall localization accuracy as shown in Sect. 4.7.
Similar to our VONet, we introduce a network architecture based on Dense-
blocks [10], namely LocNet. Our core network consists of four Dense-blocks with
intermediate transition blocks. The proposed architecture differs from the DenseNet
architecture [ 10] by the addition of an extra fully connected layer before the pre-
diction, extra connections between the Dense-blocks fusing information from earlier
layers to later ones. Similar to the experiments of Huang et al. [ 10] on ImageNet, we
experiment with the different depths and growth rates for the proposed architecture,
using the same conﬁgurations as the ones reported by Huang et al. Figure 3illus-
trates the network architecture for LocNet-121, where 121 refers to the depth of the
network in terms of number of layers. Given an input image, the network estimates
the probability distribution over the discrete set of locations.
3.3 T opometric Localization
Our topometric approach aims to reﬁne the metric localization given topological
priors. The topological localization network provides a set of values corresponding
to locations and the probability of prediction conﬁdence. For this purpose we needto fuse metric and topological network predictions into a single representation C.
The proposed topometric fusion approach is optimized as below:
Topometric Localization with Deep Learning 511
Fig. 3 Proposed LocNet-121 architecture. At each Dense Block feature maps from previous layers
are fused together as input. The lower legend shows the constituting parts for each block
C:= F+B+λS (2)
where Fis the forward drift correction, Bthe backward path optimization, and λ> 0
the smooth parameter of the Ssmoothness term. At time t≥0,xt∈SE(2)is the
pose and nt∈SE(2)is the matched topological node with probability higher than a
threshold δ.
F:=/vextenddouble/vextenddoublextr
t−ntr
t/vextenddouble/vextenddouble2
2+|xθ
t−nθ
t|2, (3)
B:=/summationdisplay
t−tw≤τ≤t−1e−λtr(t−τ)/vextenddouble/vextenddoublextr
τ−ntr
τ/vextenddouble/vextenddouble2
2+e−λθ(t−τ)|xθ
τ−nθ
τ|2, (4)
S:=argmin
α∈R3/summationdisplay
0≤τ≤t/vextenddouble/vextenddoublextr
τ−Pα(τ)/vextenddouble/vextenddouble2
2W(τ), (5)
where Pα(τ)is a quadratic polynomial and W(τ)is the weight function described
in [6].
Equation ( 3) presents our forward drift correction approach, which we model in the
following manner: Given a high probability topological node matching, we compute
the translation and rotation errors and propagate it through the next metric local-
ization predictions until another topological node is detected. Whereas the forward
drift correction component is responsible for mitigating future error accumulation,
the backward and smoothness terms are designed to further correct the obtained
trajectory. The backward path optimization is introduced in Eq. 4. The backward
optimization term works as follows: given a conﬁdent topological node it calculates
its error to it and, using an exponential decay, corrects previous predictions in terms
of translation and rotation values, until it reaches a predeﬁned time window tw.W e
also treat the exponential decay functions separately for translation and orientation
because of their different scale values.
The ﬁnal term, which compromises smoothing the trajectory, is presented in Eq. 5.
It corresponds to a local regression approach, similar to that used by Cleveland [ 6].
Using a quadratic polynomial model with α∈R3we locally ﬁt a smooth surface to
our current trajectory. One difference from this term to the others is that such term
512 G. L. Oliveira et al.
is only applied to translation. Rotation is not optimized in this term given the angles
are normalized. We choose smoothing using local regression due to the ﬂexibility of
the technique, which does not require any speciﬁcation of a function to ﬁt the model,only requiring a smoothing parameter and the degree of the local polynomial.
4 Experiments
We evaluated our topometric localization approach on a dataset collected from
Freiburg campus. The dataset is split into two parts; RGB data and RGB-D data.
We perform a separate evaluation for each the proposed Visual Odometry and Topo-logical Localization networks, as well as the fused approach. The implementation
was based on the publicly available Tensorﬂow learning toolbox [ 1].
4.1 Experimental Setup - Dataset
In order to evaluate the performance of the suggested approach, we introduce
the Freiburg Localization (FLOC) Dataset, where we use our robotic platform,
Obelix [ 18] for the data collection along with the SLAM system in [ 18] for ground-
truth estimation. The sensors necessary for SLAM are three laser scanners, a V elo-dyne HDL-32E scanner, and a vertically mounted SICK scanner. In addition to the
previously mentioned sensors, we relied on the Bumblebee camera, and a ZED stereo
camera to obtain the images used by both networks. As previously mentioned, thedataset was split into two parts; RGB and RGB-D data. We used images from the
Bumblebee camera to collect the former, and the ZED camera for the latter. The
dataset collection procedure went as follows; we navigate the robot along a chosentrajectory multiple times using different driving patterns. Out of those multiple runs,
one is randomly selected for testing, while the remainder are used for training. We
collected the data for each trajectory at different times of the day, over an extendedtime period. Overall, the dataset has a high degree of noise incurred by pedestrians
and cyclists walking by in different directions rendering it more challenging to esti-
mate the relative change in motion between frames. We use the output of the SLAMsystem of Obelix [ 18] as a source of ground-truth information for the traversed trajec-
tory. The output of the SLAM system is a set of nodes, where each node provides the
3Dposition of the robot, and the rotation in the form of a quaternion. Furthermore,
we disregard translation motion along the z-axis, and rotations along the x- and y-
axes, as they are very unlikely in our setup.
For the remainder of this section, we focus our attention on two sequences of the
FLOC dataset, namely Seq-1 and Seq-2. Seq-1 is an RGB-D sequence with 262 m
of total length captured by the ZED camera, while Seq-2 is comprised of a longertrajectory of 446 m of RGB only data captured by the Bumblebee camera. We favored
those two sequences from the dataset as they are representative of the challenges faced
Topometric Localization with Deep Learning 513
by vision-based localization approaches. Seq-1 represents a case where most vision-
based localization systems are likely to perform well as the trajectory length is short.
Moreover the presence of depth information facilitates the translation estimationfrom the input images. On the other hand, Seq-2 is more challenging with almost
double the trajectory length and no depth information.
4.2 Performance Experiments
The proposed networks are dependent on resolution and hardware used, in terms
of overall execution times. In this section we present the average execution timesfor the metric and topological networks. The performance values of the topological
metric fusion module are not discussed given they introduce a minimum overhead,
inferior to a millisecond. The GPU used is a GTX TITAN X and resolutions rangefrom 224 ×224 to 512 ×384, respectively for the topological and metric networks.
Table 1presents our forward pass times, where our metric network can process, on
average, 17 frames per second. Our topological approach can provide an average of
9 frames per second. While the input of LocNet is smaller in size compared to that
of VONet, the number of layers in the third and forth dense blocks of LocNet is 2 ×
and 1.2×bigger than their corresponding blocks in VONet. Our full approach can
then process each input every 166 .7m s .
4.3 Network Training
The networks were training on a single stage manner. VONet was trained using
Adam solver [ 15], with a mini-batch of 2 for 100 epochs. The initial learning rate is
set to 0 .001, and is multiplied by 0 .99 every two epochs. We adopt the same weight
initialization as in [ 10]. The loss function balance variable βis set to 10. The training
time for VONet for 100 epochs took around 12 h on a single GPU. LocNet was trainedusing Nesterov Momentum [ 26], with a ﬁxed learning rate of 0 .01 for 10 epochs
with a batch size of 10.
Ta b l e 1 Runtimes of VONet,
LocNet and our combinedapproachNetwork Forward pass time (ms)
VONet 56.5±4.0
LocNet 110.2±2.5
Full Approach 166.7
514 G. L. Oliveira et al.
4.4 Metric Localization
We evaluate our VO based metric localization approach over multiple sequences of
FLOC. For these experiments we compared our approach with Nicolai et al. [ 24],
DeepVO [ 22] and cnnBspp [ 21]. For each sequence, two metrics are provided: aver-
age translation error and average rotation error as a function of the sequence length.
Figure 4shows the computed trajectories of the compared methods for Seq-1.
Table 2depicts the average translation and rotation error as a function of Seq-1
length. Our approach outperforms the compared methods with almost 2 ×smaller
error for translation and rotation inference making it the closest to the ground-truth.
Seq-1 shows that our VO approach can achieve state-of-the-art performance. How-
ever the cumulative error characteristic of the problem makes it harder for longer
trajectories. Figure 5presents results for Seq-2. For this experiment the trajectories
have a bigger error, especially for translation. Table 3quantiﬁes the obtained values,
conﬁrming the difﬁculty of this sequence for all tested techniques. The results show
that our approach is still capable of largely outperforming the compared methods,
with a translation and rotation error almost twice as low as the other methods. Despite
the performance of our approach, it is still far from being competitive with LiDAR
Fig. 4 Predicted trajectories
versus ground truth for Seq-1
20 m
DeepVO Nicolai et al. Ground Truth cnnBspp Ours
Ta b l e 2 Average translation
and rotation as a function of
sequence length (Seq-1)Method Translation (%) Rot (deg/m)
Nicolai [ 24] 3.12 0.6518
DeepVO [ 22] 2.41 0.4471
cnnBspp [ 21] 3.09 0.4171
Ours 1.54 0.2919
Topometric Localization with Deep Learning 515
Fig. 5 Predicted trajectories
versus ground truth for Seq-2
20 m
DeepVO Nicolai et al. Ground Truth cnnBspp Ours
Ta b l e 3 Average translation
and rotation as a function of
sequence length (Seq-2)Method Translation (%) Rot (deg/m)
Nicolai [ 24] 6.1 0.4407
DeepVO [ 22] 9.13 0.2701
cnnBspp [ 21] 6.11 0.2083
Ours 3.82 0.1137
based approaches, like the one used to generate our ground-truth [ 18]. With this goal
in mind, we exploit the topological localization method to reﬁne our metric approach
providing an even more precise topometric approach.
4.5 T opological Localization
In this section, we evaluate the performance of the proposed LocNet architecture. To
get an estimate of the suitability of the proposed architecture to the problem at hand,
we used the Places2 dataset [ 28] for scene recognition. The dataset contains over
ten million scene images divided into 365 classes. We use the pretrained DenseNet
model on ImageNet to initialize the weights for our LocNet-121 architecture, as our
architecture is quite similar to DenseNet aside from using a different activation func-
tion. Using the pretrained model, we are able to achieve a T op-1 accuracy of 54 .34%,
and a T op-5 accuracy of 83 .94%, which is comparable to the Places365-GoogLeNet
architecture as reported by the authors. While our aim was not to outperform state-of-
the-art methods for scene recognition, the performance of our LocNet architecture
on the Places2 dataset validates its aptness for topological localization problems.
For the remaining experiments, we did not use the pretrained model to initialize the
516 G. L. Oliveira et al.
Ta b l e 4 Classiﬁcation accuracy of the different networks on the Freiburg Campus Dataset (Seq-1)
Classiﬁer Depth Params (M) Accuracy (%)
ResNet 50 26 84.7
ResNet 101 44 85.1
LocNet 121 9 90.0
LocNet 169 15 90.4
LocNet 201 20 88.99
Ta b l e 5 Classiﬁcation accuracy of the different networks on the PoseNet Dataset
Scene Classes RN-50 (%) RN-101 (%) LN-121 (%) LN-169 (%) LN-201 (%)
Shop facade 9 82.8 77.8 80.3 85.7 86.6
King’s
college20 87.7 85.1 87.1 90.4 89.5
Great court 109 67.4 65.6 68.0 68.8 71.44
Old hospital 14 86.0 84.0 88.6 90.1 90.6
St.Mary’schurch 30 79.0 79.1 87.7 88.4 88.8
Street 241 73.4 72.7 67.1 73.3 75.6
RN: ResNet, LN: LocNet
values of LocNet, rather trained the network from scratch on both the FLOC and
Cambridge Landmarks [ 14] datasets. We compare the performance of our LocNet
architecture with that of Residual Networks (ResNet) [ 9] given its recent perfor-
mance in image recognition. We evaluate the performance of both architectures overmultiple sequences of the FLOC dataset and the Cambridge Landmarks dataset. For
both datasets, we report the accuracy in terms of the number of images where the
predicted location is within a 1 m radius of the ground-truth pose. Table 4illustrates
the performance results on Seq-1 of the FLOC dataset. We investigate the effect of
the depth of the network on the accuracy of the predicted poses, while comparing the
number of parameters. The best performance was achieved using LocNet-169 withan accuracy of 90 .4% with approximately 3 ×less parameters than its best perform-
ing counterpart in ResNet. Table 5illustrates the performance on the different scenes
from the Cambridge Landmarks dataset. On this dataset, LocNet-201 achieves the
best performance with the exception of King’s College scene. It is worth noting that
LocNet-169 achieves the second highest accuracy in four out of the ﬁve remainingscenes, providing further evidence to the suitability of this architecture to the prob-
lem at hand. For the remainder of the experimental evaluation, we use the prediction
output from LocNet-201.
Topometric Localization with Deep Learning 517
4.6 T opometric Localization
This section presents the results of fusing both topological and metric localization
techniques. Figure 6presents both the metric and topometric results for Seq-1. As
can be noticed the trajectory difference between the ground truth and our topometric
approach is almost not visually distinguishable. Table 6shows an improvement of
7×in the translation inference and superior up to 6 ×for orientation. Such values
provide competitive results even to the LiDAR SLAM system utilized to provide
ground-truth to FLOC.
We also evaluated our approach using Seq-2. Figure 7depicts the obtained results.
While the results for this sequence are not as accurate as those of Seq-1, the gain
Fig. 6 Seq-1, metric versus
topometric
Ground Truth Ours-Metric Ours-Topometric
Fig. 7 Seq-2, metric versus
topometric
Ground Truth Ours-Metric Ours-Topometric
518 G. L. Oliveira et al.
Ta b l e 6 Metric versus topometric results
Method Translation (%) Rot (deg/m)
Seq. 1 Metric 1.54 0.2919
Seq. 1 Topometric 0.21 0.0464
Seq. 2 Metric 3.82 0.1137
Seq. 2 Topometric 0.38 0.0634
in translation is more than 10 ×the metric counterpart. For orientation, even though
our metric approach already presents good results, the error is reduced by half using
our topometric technique, as shown in Table 6.
4.7 Generalization Experiments
In this section, we evaluate the effect of varying number of key-frames on the accuracy
of our topometric localization method. Using the clustering approach in Sect. 3.2,
we generated key-frames of varying distances, by selecting different values for the
parameter dth; 10, 20, and 40 m. We then trained multiple versions of LocNet on
each of the generated key-frames, along with the fusion approach. Table 7presents
the effect of varying dthon the localization accuracy using the aforementioned values.
Note that increasing the value of dthresults in a fewer number of key-frames with
minimum visual similarity between the generated classes. We refer to this as Sparse ,
while having a small value of dthresults in having Dense key-frames. In this table,
T opometric refers to the standard value of dthwhich was used in all evaluations.
The results show that, for both sequences, using sparse key-frames produces worse
localization accuracy in comparison to using a denser value. We further observethat while using sparse key-frames causes a deterioration in translation accuracy, the
effect is not observed on the rotational accuracy as observed in both sequences. Using
denser key-frames resulted in having lower conﬁdence probabilities from LocNet,which is a consequence of the close visual similarity between the classes. The results
Ta b l e 7 Generalization results
Method Translation (%) Rot (deg/m)
Seq. 1 Sparse (40 m) 0.4273 0.0571
Seq. 1 Dense (10 m) 0.2280 0.0659
Seq. 1 T opometric (20 m) 0.21 0.0464
Seq. 2 Sparse (40 m) 0.7598 0.0556
Seq. 2 Dense (10 m) 0.4341 0.1062
Seq. 2 T opometric (20 m) 0.38 0.0634
Topometric Localization with Deep Learning 519
indicate that while having more dense key-frames is more favorable to a sparse
conﬁguration, nonetheless, ﬁnding a good balance between both provides the best
trade-off between translation and rotational accuracies.
5 Conclusions
In this paper, we presented a novel deep learning based topometric localizationapproach. We proposed a new Siamese architecture, which we refer to as VONet,
to regress the translational and rotational relative motion between two consecutive
camera images along a traversed path. The output of the proposed network providesthe visual odometry information along the traversed path. We additionally proposed a
novel architecture, LocNet, targeted towards topological localization. Using a fusion
approach, we combine the output of both networks to probabilistically reduce theaccumulated drift in the visual odometry. We evaluated our approach on the new
Freiburg Localization (FLOC) dataset, which we collected in adverse weather con-
ditions and which we will provide to the research community. The extensive experi-
mental evaluation shows that our proposed VONet and LocNet architectures surpass
current state-of-the-art methods for their respective problem domain. Furthermore,using the proposed topometric approach we improve the localization accuracy by
one order of magnitude.
Acknowledgements This work has been partially supported by the European Commission under
the grant numbers H2020-645403-ROBDREAM, ERC-StG-PE7-279401-VideoLearn, the FreiburgGraduate School of Robotics.
References
1. Abadi, M., Agarwal, A., Barham, P ., et al.: Tensorﬂow: large-scale machine learning on het-
erogeneous distributed systems (2016). arXiv:1603.04467
2. Arandjelovic, R., Gronat, P ., Torii, A., Pajdla, T., Sivic, J.: NetVLAD: CNN architecture for
weakly supervised place recognition. In: IEEE Conference on Computer Vision and Pattern
Recognition (2016)
3. Arroyo, R., Alcantarilla, P .F., Bergasa, L.M., Romera, E.: Fusion and binarization of CNN
features for robust topological localization across seasons. In: Proceedings of the IEEE/RSJ
International Conference on Intelligent Robots and Systems (2016)
4. Badino, H., Huber, D., Kanade, T.: Visual topometric localization. In: IEEE Intelligent V ehicles
Symposium (IV) (2011)
5. Chen, Z., Lam, O., Jacobson, A., Milford, M.: Convolutional neural network-based place
recognition (2014). arXiv:1411.1509
6. Cleveland, W.S.: Robust locally weighted regression and smoothing scatterplots. J. Am. Stat.
Assoc. 74(368), 829–836 (1979)
7. Clevert, D., Unterthiner, T., Hochreiter, S.: Fast and accurate deep network learning by expo-
nential linear units (elus) (2015). arXiv:1511.07289
8. Garcia-Fidalgo, E., Ortiz, A.: Vision-based topological mapping and localization methods: a
survey. Robot. Auton. Syst. 64, 1–20 (2015)
520 G. L. Oliveira et al.
9. He, K., Zhang, X., Ren, R., Sun, J.: Deep residual learning for image recognition. In: Proceed-
ings of the IEEE Conference on Computer Vision and Pattern Recognition (2016)
10. Huang, G., Liu, Z., Weinberger, K.Q., van der Maaten, L.: Densely connected convolutional
networks (2016). arXiv:1608.06993
11. Ioffe, S., Szegedy, C.: Batch normalization: accelerating deep network training by reducing
internal covariate shift (2015). arXiv:1502.03167
12. Jégou, H., Douze, M., Schmid, C., Pérez, P .: Aggregating local descriptors into a compact image
representation. In: IEEE Conference on Computer Vision and Pattern Recognition (2010)
13. Kaiming, H., Xiangyu, Z., Shaoqing, R., Sun, J.: Spatial pyramid pooling in deep convolutional
networks for visual recognition. In: European Conference on Computer Vision (2014)
14. Kendall, A., Grimes, M., Cipolla, R.: Posenet: a convolutional network for real-time 6-dof
camera relocalization. In: Proceedings of the IEEE International Conference on ComputerVision (2015)
15. Kingma, D., Ba, J.: Adam: a method for stochastic optimization (2014). arXiv:1412.6980
16. Konda, K., Memisevic, R.: Learning visual odometry with a convolutional network. In: Pro-
ceedings of the 10th International Conference on Computer Vision Theory and Applications(2015)
17. Krizhevsky, A., Sutskever, I., Hinton, G.E.: Imagenet classiﬁcation with deep convolutional
neural networks. In: Pereira, F., Burges, C.J.C., Bottou, L., Weinberger, K.Q. (eds.) Advancesin Neural Information Processing Systems (2012)
18. Kümmerle, R., Ruhnke, M., Steder, B., Stachniss, C., Burgard, W.: Autonomous robot naviga-
tion in highly populated pedestrian zones. J. Field Robot. 32(4), 565–589 (2015)
19. Lowry, S., Sünderhauf, N., Newman, P ., Leonard, J., Cox, D., Corke, P ., Milford, M.J.: Visual
place recognition: a survey. IEEE Trans. Robot. 32(1), 1–19 (2016)
20. Mazuran, M., Boniardi, F., Burgard, W., Tipaldi, G.D.: Relative topometric localization in glob-
ally inconsistent maps. In: Proceedings of the International Symposium on Robotics Research(2015)
21. Melekhov, I., Kannala, J., Rahtu, E.: Relative camera pose estimation using convolutional
neural networks (2017). arXiv:1702.01381
22. Mohanty, V ., Agrawal, S., et al.: DeepVO: a deep learning approach for monocular visual
odometry (2016). arXiv:1611.06069
23. Nair, V ., Hinton, G.E.: Rectiﬁed linear units improve restricted Boltzmann machines. In: Pro-
ceedings of the International Conference on Machine Learning (2010)
24. Nicolai, A., Skeele, R., Eriksen, C., Hollinger, G.A.: Deep learning for laser based odometry
estimation. In: RSS Workshop Limits and Potentials of Deep Learning in Robotics (2016)
25. Sprunk, C., Tipaldi, G.D., Cherubini, A., Burgard, W.: Lidar-based teach-and-repeat of mobile
robot trajectories. In: Proceedings of the IEEE/RSJ International Conference on IntelligentRobots and Systems (2013)
26. Sutskever, I., Martens, J., Dahl, G.E., Hinton, G.E.: On the importance of initialization and
momentum in deep learning. In: Proceedings of the International Conference on MachineLearning (2013)
27. Wu, J., Ma, L., Hu, X.: Delving deeper into convolutional neural networks for camera relo-
calization. In: Proceedings of the IEEE International Conference on Robotics and Automation
(2017)
28. Zhou, B., Khosla, A., Lapedriza, A., Torralba, A., Oliva, A.: Places: an image database for
deep scene understanding (2016). arXiv:1610.02055
Manipulation
Sampling-Based Planning of In-Hand
Manipulation with External Pushes
Nikhil Chavan-Daﬂe and Alberto Rodriguez
1 Introduction
In-hand manipulation, understood as the capability to adapt a grasp on an object,
facilitates the complex process involved in picking and using an object. Robots,especially those with simple grippers, lack the necessary dexterity to do so, which
strains their manipulation capabilities.
In this paper, we propose a planner to manipulate grasped objects through a
sequence of external pushes, such as those in Fig. 1, a.k.a., prehensile pushing [ 2].
Given a pair of start and goal object grasps, the planner outputs a sequence of pushes,possibly from different sides of the object, to reconﬁgure the object in the grasp.
Planning these push sequences presents two main challenges:
•Continuous contact dynamics of the frictional interaction between gripper, object
and their environment.
•Discrete contact switch-overs between continuous pushes.
To address them both, we combine a low-level optimization-based approach to
solve the inverse dynamics of prehensile pushing with a high-level sampling-based
planning approach to build long sequences of pushes.
Low-level optimization-based inverse dynamics For prehensile pushing, solving
for a unit step control to propagate a planning-tree refers to solving the inverse
dynamics problem, i.e., ﬁnding the external pusher motion that yields the objectmotion as close to the desired one as possible. We develop an optimization-based
dynamics formulation capturing the contact dynamics between gripper, object, and
N. Chavan-Daﬂe ( B)·A. Rodriguez
Department of Mechanical Engineering, MIT, Cambridge, MA, USA
e-mail: nikhilcd@mit.edu
A. Rodriguez
e-mail: albertor@mit.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_39523
524 N. Chavan-Daﬂe and A. Rodriguez
Fig. 1 An example of prehensile pushing—an aluminum object is reconﬁgured in a grasp by
pushing it against the environment from different sides
external pusher, which in practice takes the form of a mixed nonlinear complemen-
tarity problem (MNCP).
High-level sampling-based planning The higher level planning architecture fol-
lows a transition-based RRT\* (T-RRT\*) formulation which takes advantage of the
optimality convergence properties of typical RRT\* technique and efﬁcient explo-
ration of conﬁguration space using transition tests [ 6,9,11]. We use the optimal
connections feature of RRT\* to minimize the number of pusher contact switch-overs
along a pushing strategy. The transition tests allow us to loosely conﬁne the stochas-
tic exploration towards the goal grasp, while allowing the ﬂexibility to explore inother directions if it’s necessary to get the object ﬁnally to the goal.
The planning architecture and the dynamics solver work together to build a tree
of grasp poses. A path in this tree provides a pushing strategy to change a grasp poseto another.
We evaluate the performance of the planner for the case of a parallel jaw-gripper
manipulating different objects. We validate the pushing sequences with real experi-ments in a robotic manipulation platform which is equipped to track the motion of
the robot and pose of the object.
To summarize, the main contributions of this paper are:
•an optimization-based inverse dynamics formulation for full three dimensional
in-hand manipulations using external pushes,
•a planning framework to combine low-level contact dynamics with high-level
reasoning for long pushing strategies with discrete contact changes,
•application and experimental validation of the proposed planner to prehensile
pushing.
2 Related Work
Early work on dexterous manipulation focused on providing a gripper with enough
degrees of freedom to give full controllability over a grasped object and furtherallowing ﬁnger contacts to either roll or slide [ 1,5,10,17,18,22]. It assumed the
intrinsic capability of the gripper to control these interactions.
Sampling-Based Planning of In-Hand Manipulation … 525
Diverging from this assumption, in a recent work, we demonstrated the use of
gravity, dynamic motions, and contacts with the environment to regrasp objects using
a library of hand-scripted motions [ 4]. In [ 2], we studied in-hand manipulations with
external contacts. We referred to it as prehensile pushing and presented a quasi-
dynamic formulation to predict the instantaneous motion of a grasped object for a
given pusher motion—forward dynamics problem [ 2]. The inverse dynamics solver
we use in this paper shares a similar dynamics formulation underneath, but solves
for the required pusher motion for a desired object motion.
Planning for prehensile pushing requires an understanding of how forces and
motions evolve at contact interactions. There is a large array of work on trajectory
optimization techniques for planning and control through contact. In most cases,
these make assumptions of point contact interactions modelled with polyhedral fric-tion cones or patch contacts modelled as soft point contacts to alleviate the compu-
tational complexity of contact modeling [ 7,16,21]. Shi et al. [ 19], Viña B et al. [ 23]
and Hou et al. [ 8] demonstrate application of such approach to in-hand manipula-
tion, particularly for in-hand sliding and pivoting. For computational efﬁciency, Erez
and Todorov [ 7], Todorov et al. [ 21] relax complementarity constraints required to
impose non-penetration condition at contacts and to model sticking/sliding transi-
tions. This leads to fast algorithms, but with limited success in modeling situations
of interest to this work, i.e., beneﬁting from hard line and patch contacts [ 12]. The
contact modelling approach in this paper resembles to that presented in [ 2], but with a
quadratic Coulomb friction cone instead of a polyhedral approximation. The polyhe-
dral approximation introduces artiﬁcial anisotropy in friction and “preferred” slidingdirections.
Sampling-based techniques for planning are key to the presented approach.
Rapidly exploring random tree (RRT) derives it’s strength from fast and randomexploration of the conﬁguration space [ 13,14]. RRT\* introduces the concept of
optimality for connecting the nodes in a tree and provides conditions under which
it can lead to asymptotically optimal solutions [ 11]. One of the variants of RRT\*
that we particularly ﬁnd useful in this work is T-RRT\*, which is developed for path
planning on conﬁguration-space cost maps [ 6,9]. By employing a transition test to
accept/reject nodes, it guides the exploration to follow low-cost valleys of the costmap with a provision to traverse across high-cost regions whenever required. This
provides a more controlled and efﬁcient exploration of the conﬁguration space.
While sampling based methods have not been thoroughly explored for contact-
rich applications and may not seem an immediate choice for problems with complex
and computationally expensive dynamics, in the coming sections we discuss in detail
the ﬁt of T-RRT\* based approach for such problems and demonstrate its effectiveness
at practical in-hand manipulations.
526 N. Chavan-Daﬂe and A. Rodriguez
3 Problem Formulation
This paper focuses on planning in-hand manipulations using external pushes. In our
implementation, the external pushes are executed by a robot forcing a grasped objectagainst a rigid environment. More generally, such external pushes could also abstract
the interactions with a second robot arm or extra ﬁngers of a multi-ﬁnger gripper.
Equivalently, in this paper, we assume the gripper is ﬁxed in the world and grasps
an object, while a virtual pusher with full 6 DOF mobility executes the external
pushes. In this case, planning for external pushes is equivalent to planning the motion
of the virtual pusher. For the problem setup, we assume the following informationabout the manipulation system:
•Object geometry and mass.
•Initial and goal grasp on the object, speciﬁed by the locations and geometries of
each ﬁngers contacts.
•Gripping force.
•Discrete set of pusher contacts, speciﬁed by initial locations and geometries.
•Coefﬁcient of friction at all contacts.
As described in Sect. 1, the proposed planner works at two levels—a high level
planning architecture (Sect. 5) that explores the conﬁguration space of reachable
grasps and builds a tree of optimally connected conﬁgurations, and a low level inversedynamics solver (Sect. 4) that controls the unit-step propagation in the tree. In short,
the decision ﬂow of the planner is follows:
i. Sample a random object conﬁguration in a grasp.
ii. Check if moving toward the sampled conﬁguration satisﬁes a “beneﬁt” criteria.
If not, return to step i.
iii. Solve inverse dynamics for a valid pusher location and pusher motion to move
the object in the direction of the sampled pose. If not possible, return to step i.
iv. Check for other ways to reach the newly added conﬁguration with lower cost,
from existing nodes in the tree.
v. Iterate until reaching the goal grasp within a given resolution and cost threshold.
4 Low-Level: Inverse Dynamics Solver
Sampling-based planners are built on top of a unit-step algorithm that, when possible,
steers the system along a sampled direction. In this paper, we refer to that unit step
as the inverse dynamics problem: given the pose of the object in a grasp, the position
of a pusher on the object and a desired instantaneous object motion in the grasp, ﬁndan instantaneous motion of the pusher that forces the object in the direction as close
to the desired direction as possible.
The following sections discuss our approach to model contact interactions and
kinetic-kinematic constraints governing the object motion in the grasp.
Sampling-Based Planning of In-Hand Manipulation … 527
Fig. 2 Different contact geometries: point, line and circular patch, modeled as sets of rigidly
connected point contacts
4.1 Contact Modelling
Our contact modelling approach is similar to that proposed in [ 2]. We model a patch
contact as a rigid array of point contacts as shown in Fig. 2. Each of these constituent
point contacts, is modeled as a hard point contact with quadratic Coulomb friction
cone.
We represent a point contact between two bodies by a local coordinate frame
with, ˆnnormal to the contact plane and and ˆtandˆospanning the contact plane.
Let f=[ fn,ft,fo]/latticetopand v=[ vn,vt,vo]/latticetopbe a net force and a relative velocity at a
contact in the local contact frame. For a given coefﬁcient of friction ( μ), Coulomb’s
friction cone at the contact is deﬁned as the following set:
FC={ fnˆn+ftˆt+foˆo|fn≥0,f2
t+f2
o≤μf2
n} (1)
By Coulomb’s law, when a contact slides, the contact force is on the boundary of
the friction cone and the direction of the friction force is opposite to that of the
sliding velocity at the contact. We can formalize this constraint using the standard
complementarity and nonlinear equations:
[(μfn)2−f2
t−f2
o]/bardbl[vt,vo]/bardbl=0,( μ fn)2−f2
t−f2
o≥0( 2 )
μfnvi+fi/bardbl[vt,vo]/bardbl=0 i=t,o (3)
For a contact with ﬁnite area, modelled as an array of points, we impose ( 2) and
(3) at each constituent point, along with constraints on the relative velocities at them
to make sure that the array moves as a rigid body. See [ 2] for more details.
4.2 Dynamics of Prehensile Pushing
The ﬁctional forces involved in prehensile pushing are much more dominant than
the object inertia, so we will limit ourselves to a quasi-dynamic model of pushing.
528 N. Chavan-Daﬂe and A. Rodriguez
We deﬁne this model in the space of local contact impulses, relative velocities at
the contacts, velocity of the object, and velocity of the pusher. The solution space is
constrained by the following kinematic and kinetic constraints.
Newton Euler Equation :L e t Gimaps local contact forces at contact ito the cor-
responding wrench in the object frame. Gis deﬁned as diagonal concatenation of
Gi’s for all the contacts on the object. As we are interested in a quasi-dynamic for-
mulation, for a single time step with zero initial velocity of the object, we can write
the time-integrated Newton’s law for an object with mass mand generalized inertia
matrix Mas:
G·P+/vectorPmg=M·/vectorvobj (4)
where Pis an array collecting impulses equivalent to all the contact forces ( f1,.., fn),
/vectorPmgis the gravitational impulse and /vectorvobjis the resultant object velocity in the object
frame.
Rigid Body Motion Constraints :L e t Jbe the jacobian matrix that maps the veloc-
ities of the pusher and gripper actuators ( ˙θ) to the input velocities at all the contacts
in the local contact frames. We can write V=[ v1,v2,..., vn]/latticetop, the array collecting
the relative velocities at all contacts, as difference between the input velocities and
the reﬂection of the object velocity at those contacts points:
V=G/latticetop·/vectorvobj−J·˙θ (5)
Unilateral Contact Constraints : There can not be interpenetration at contacts
between two rigid bodies. Contacts can only push and not pull, and only when
there in no separation at them. We write it as a complementarity constraint at each
point contact.
vn·pn=0,vn≥0,pn≥0( 6 )
Contact Modelling Constraints : We model the force-motion interactions at every
contact as explained in Sect. 4.1.L e t p=[ pn,pt,po]/latticetopbe an impulse at a contact,
then rewriting Eqs. 2and 3in the space of impulse-velocity:
[(μpn)2−p2
t−p2
o]/bardbl[vt,vo]/bardbl=0,( μ pn)2−p2
t−p2
o≥0( 7 )
μpnvi+pi/bardbl[vt,vo]/bardbl=0 i=t,o (8)
Further, for contacts with ﬁnite area modelled with arrays of point contacts, we
impose constraints on the relative velocities at them to make sure that each array
moves as a rigid body.
Sampling-Based Planning of In-Hand Manipulation … 529
4.3 Numerical Solver for the Dynamics Problem
In our problem, solving inverse dynamics means ﬁnding a pusher velocity that pro-
duces a desired object velocity while satisfying all the constraints listed above. It hasthe form of a mixed nonlinear complementarity problem (MNCP), which we solve as
a nonlinear constrained optimization problem using interior point method in MA T-
LAB. We deﬁne the objective function as a weighted sum of the complementarityconstraints and the difference between the desired object velocity and that actually
achieved. We try to minimize the objective function subject to the constraints detailed
in Sect. 4.2. A feasible solution exists when the objective goes close to zero while
meeting the constraints. In practice, it helps to give a relatively larger weight on
complementarity constraints, yielding more accurate satisfaction of contact dynam-
ics and compromising on the desired object velocity if necessary. The ratio of weights
we used is 10
4.
5 High-Level: Long Horizon Planning with Contact
Switch-overs
An effective regrasp skill requires exploiting contact switch-overs. A continuous and
greedy approach based on pushing iteratively towards the goal grasp has limited
success in a problem as constrained and underactuated as in-hand manipulation. Theproblem beneﬁts from a long-horizon planning technique that allows the regrasp
strategy to deviate from goal momentarily if necessary and sequence different discrete
pushes.
Trajectory optimization has been studied to capture the effects of a long-horizon
cost, but has difﬁculty with the hybridness of discrete contact switch-overs. On the
other hand, sampling based methods are naturally suited to search over continuous
plans intertwined with discrete changes along the plan. Being able to change the
pusher contact from one side of the object to another can be pivotal. In practice,minimizing the number of contact switch-overs yields beneﬁts in the form of: time
savings, and avoiding uncertainty introduced by engaging and disengaging contacts.
The higher level architecture of our planner is based on a T-RRT\* formulation.
We exploit the optimality convergence properties of the underlying RRT\* method
to reduce the number of pusher contact switch-overs and the efﬁciency of transition
tests to direct the exploration of conﬁguration space towards the goal.
Algorithm 1 presents our in-hand manipulation planner, starting from the assump-
tions listed in Sect. 3.L e t qdenote a conﬁguration of an object, i.e., the pose of the
object with respect to a gripper frame which is assumed to be ﬁxed in the world.Though the conﬁguration space Cis six dimensional, different types of grasps con-
ﬁne it to lower dimensions. We use scaled euclidean distance, where 1 mm is treated
equivalent to 3 degree, as a metric between two object conﬁgurations.
530 N. Chavan-Daﬂe and A. Rodriguez
Algorithm 1 : In-Hand Manipulation Planner
input :qinit,qgoal
output :tree T
1:T←initialize tree (qinit)
2:while qgoal/∈Tdo
3: qrand←sample random conﬁguration (C)
4: qparent ←ﬁnd nearest neighbor (T,qrand)
5: qideal←take unit step (qparent,qrand)
6: iftransition test (qparent,qideal,T)and grasp maintained (qideal)then
7: qnew,˙θpusher ←InvDynamics (qparent,qrand)
8: ifqnew/negationslash=null and transition test (qparent,qnew,T)and grasp maintained (qnew)then
9: (q\*parent)←optimal connection (T,qnew,qparent)
10: add new node (T,qnew)
11: add new edge (q\*parent,qnew)
12: rewire tree (T,qnew,q\*parent)
13: end if
14: end if
15:end while
Let qinitand qgoalbe an initial and desired conﬁguration of the object respectively.
The planner initiates a tree Twith qinit. While the desired object pose is not reached,
it samples a random conﬁguration ( qrand) and ﬁnds the nearest conﬁguration to qrand
in the tree T.
Controlled exploration : A transition test decides if the propagation of the tree
towards the newly sampled conﬁguration is acceptable or not. Let Cqbe a cost
deﬁned on the object conﬁguration q, as the distance between qand qgoal.I fm o v i n g
the object from the nearest neighbor towards the newly sampled pose can reduce theconﬁguration cost, the sample is accepted. If such an object motion will increase
the conﬁguration cost, but still keep it lower than some maximum bound set, the
sample is accepted with a certain probability. Following [ 9], we deﬁne the transition
probability for a transition from q
atoqbas:
p(qa,qb)=exp/parenleftbigg−ΔC(qa,qb)
KT/parenrightbigg
where,
•ΔC(qa,qb)=Cqb−Cqa
dist(qa,qb)is the rate of cost variation per unit distance.
•Kis a normalization factor deﬁned as average of costs Cqband Cqa.
•T, is temperature parameter which controls the difﬁculty of a transition. We adjust
it as the planner progresses. It is increased if the tree is getting stuck locally to
allow transitions of high cost, and decreased otherwise to allow transitions of onlylow cost.
In practice, the transition test with our conﬁguration cost deﬁnition loosely con-
ﬁnes the propagation of the tree towards the goal pose, while allowing the ﬂexibility
to steer away from it momentarily if necessary.
Sampling-Based Planning of In-Hand Manipulation … 531
If the transition test succeeds, we query the inverse dynamics solver to predict
the motion of a pusher required to move the object from ( qparent ) by a unit step
as much as possible towards ( qrand). The dynamics solver limits its choice for the
pushers to a ﬁxed set of pusher locations on the object and the evolved pusher location
corresponding to qparent . Here, by evolution we mean the new location of the pusher
contact if it slides on the object. This makes sure that we account for the pusher slipwhen sequencing multiple instantaneous pushes to generate a smooth continuous
push using the same pusher.
Algorithm 2 : optimal connection
input :T,qnew,qparent
output :q\*parent
1:Jqnew←ﬁndNodeCost (qnew,qparent)
2:J\*qnew← Jqnew;q\*parent ← qparent
3:Qnear←nodesInBall (T,qnew,Rball)
4:while Qnear/negationslash=∅ do
5: qparent ← q∈Qnear
6: ifInvDynamics (qparent,qnew)/negationslash=null then
7: Jqnew←ﬁndNodeCost (qnew,qparent)
8: ifJqnewμ
s
and a ﬁxed ε, the predicted impulse from the model will remain the same and the
cost function will not have a gradient, making the optimization ill-posed numerically.This implies that pre and post contact conditions exist which do not provide sufﬁcient
information to estimate μcorrectly and render this parameter unobservable, more
on this in Sect. 3.2.2.
To perform system identiﬁcation for a batch of impacts, we simply sum up the
cost function over the set of impacts (Sect. 3.2.1).
Fundamental Limitations in Performance … 563
Fig. 5 Experimental setup: autonomous data collection for planar impact data. A planar object
is constrained to move between two lubricated vertical glass planes. The robot uses a directional
magnetic latch to pick up the object across the glass and provide control over position and velocityof the object. The Vicon motion capture system is used to track the states of the object
3.1 Experimental Data Collection
In order to evaluate and compare the contact models we conceived the experimental
setup depicted in Fig. 5[6,7]. The purpose of the apparatus is to autonomously collect
measurements of the trajectory of falling planar objects initialized from a speciﬁableset of initial conditions. We chose this experiment because it captures key featuresof a dynamic frictional interaction while being amenable to careful monitoring and
instrumentation resulting in high quality measurements; for further details see [ 6].
The experimental setup is composed of a “dropping arena” designed to constrain
the motion of planar objects to a plane using lubricated glass planes. The planarobjects are equipped with markers to be tracked using the Vicon motion capture
system at 250 Hz, and directional magnets allowing the robot to latch onto the objectand prescribe desired initial state to the object for each drop.
We collected a total of 2,000 drops. In some instances the tracking system may lose
frames or suffer spurious errors due to reﬂections; to detect and remove these effectseach experimental run was subjected to a series of tests to validate the correctness of
the measurements. The three most important tests were: (i) the energy test: veriﬁes
constant energy during ballistic motion and energy loss during times of contact, (ii)a frame drop test, and (iii) a deviation test: checks deviations of trajectories during
ballistic motion from parabolic paths. A total of 1,718 of the 2,000 drops passed
all tests, and we used this set for the rest of this study. An example of the recorded
conﬁgurations for one experiment is shown in Fig. 6.
To detect the time of contact we use the fact that the object undergoes ballistic
motion, consequently the acceleration of the object is smooth. Any impact eventinduces a spikes in the second time derivative of the state vector, and we use a
threshold on these spikes to detect events. We further use the ballistic motion of theobject to ﬁt parabola to the pre- and post-contact trajectories, thereby mitigating the
564 N. Fazeli et al.
0.8 0.9 1 1.1 1.2 1.3 1.4
Time (seconds)-30-20-10010203040Position (cm)
Fig. 6 The conﬁgurations of the object as a function of time during an experimental run. θis in
radians but has been scaled by the radius of gyration of the object
effects of noise on the state measurements and yielding better approximations to
velocities of the object; for more details see [ 6].
3.2 Results of System ID and Model Prediction Performance
We conduct the identiﬁcation in two scenarios, once in batch (Sect. 3.2.1 ensemble
parameters), and once for individual impacts (Sect. 3.2.2 individual best parameters).
The former scenario will allow us to evaluate the performance of the models in
predicting outcomes on previously unseen data, and we will use the latter to show
the large variation in parameters across impacts and limitations in the predictionsmade by the models.
3.2.1 Ensemble Parameter Identiﬁcation
Ensemble parameters were identiﬁed from a randomly selected subset of the data as
per the procedure described in Sect. 3. Figure 7a shows the shape of the cost function.
This function is convex, which is to be expected since the region of admissible
impulses predicted by all models is convex and the values of the predicted impulsesgrow proportionally to the magnitudes of the parameters. Intuitively, if there exists
a pair of parameters for a given impact that perfectly predicts the impulse, then the
Fundamental Limitations in Performance … 565
0
0.40.0050.01
0.450.0150.02
0.5
0.55
0.20.150.1 0.60.0500.0150.0160.0170.0180.0190.02
(a) Objective function (Whittaker model).10 20 30 40 50 60 70 80 90 100 110 12000.10.20.30.40.50.60.7Parameter Value
Number of Contact Events
(b) Model parameters Convergence.
Fig. 7 a Cost function plotted as a function of the parameters for the Whittaker model for 120
drops. bConvergence behavior of the model parameters as a function of number of contacts used
for the identiﬁcation, the solid colored lines are the average parameter values over a random set of20 samples and the dashed black lines depict 1 standard deviation
Table 1 Average-best parameter identiﬁed values and the percentage a model was chosen as the
best and worst model
Model μ ε Best (%) Worst (%)
DrumShell 0.081 ±0.008 0.516 ±0.009 4 11
AP Poisson 0.101 ±0.007 0.526 ±0.008 7 6
AP Newton 0.084 ±0.007 0.547 ±0.009 25 0
Mirtich 0.062 ±0.009 0.558 ±0.010 17 43
Wang–Mason 0.120 ±0.008 0.537 ±0.010 16 10
Whittaker 0.111 ±0.008 0.484 ±0.011 31 30
cost associated with that pair would be zero and all other points within the convex set
would have error values that grow proportional to deviations from the optimal pair.
For a point outside the set of admissible impulses—due to the imposed constraintμ≤μ
s—then there is a point that lies on the boundary of the set for which the
distance takes on a minimum.
Figure 7b shows the convergence of the parameters as a function of the number of
data used for the Whittaker model; for every kcontact events we selected 20 sets of
size kat random without replacement from the experiments. Table 1lists the results
of the identiﬁcation for all 6 models for k=120.
Table 1shows that the values for the coefﬁcient of friction found for each model
are much lower than the value (0.3) estimated from an inclined surface experiment
between PLA (material of the object used) and Aluminium (material of the impactplate). We believe that the major contributing factor to the discrepancy is the com-
plex interaction that the coefﬁcient of friction tries to model in the highly dynamic
dropping experiments. The object can undergo a number of rich and difﬁcult to
model interactions in a very short period of time, such as small local deformations,
566 N. Fazeli et al.
ﬁnite-time impact, vibrations, and impulsive torsional frictional forces (in conjunc-
tion with the modeled linear ones). The use of just two parameters, which in this
case are state independent, is not sufﬁcient to describe the resulting behavior, and theidentiﬁed parameters lose meaning as they are compensating for all the unmodelled
dynamics. In contrast, the inclined surface experiment is slow and controlled, and one
parameter is more descriptive of the observations. This is a known phenomenon, [
2]
discusses the simplicity of impact models, in that they attempt to capture a compli-
cated phenomenon in a minimalistic way and with intuitive parameters. This intuition
is valuable: it gives a direct interpretation to the way these parameters affect the out-come of a prediction, but it is unreasonable to expect the values found for these
parameters to match those found from other experiments. The coefﬁcient of friction
in this context is not necessarily the same as that measured from the inclined surfaceexperiment, though they have the same name; this thought will be elaborated upon in
Sect. 3.2.2 . It is important to note that using the inclined surface experimental value
forμleads to signiﬁcantly poorer predictive accuracy. Figure 8depicts the estimated
PDF of the /lscript
2-norm error of predicted vo(we normalize the angular velocity by the
radius of gyration so that the linear and angular terms are of the same order). The
Best Post Hoc model is also plotted to show a performance upper bound on the ana-
lytical model; that is to say, if, for each trial, we knew a priori which model would
perform best. Table 1shows the number of times each model was chosen as the best
and worst performing for the dataset. We can see that the Whittaker model is quite
erratic, while the most consistent model is the AP Newton model, with a signiﬁcant
number of best predictions and no worst.
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 200.511.522.53PDF EstimateDrumShell
APPoisson
APNewton
Mirtich
Wang-Mason
Whittaker
Best Post-Hoc
IRB Bound
Fig. 8 Estimated probability density functions (PDFs) of the /lscript2-norm error in post-contact velocity
of the center of mass
Fundamental Limitations in Performance … 567
The IRB Bound model is the result of directly searching for the best impulse
that explains the difference between pre- and post-contact velocities subject to the
constraints of rigid-body point contact. This post hoc model quantiﬁes the absolutebest possible performance for any rigid-body contact model under the assumption
of point contact.
It is important to note that the error introduced by the rigid body assumptions is not
distinguishable from error resulting from uncertainty in sensor measurements, but oursetup was designed to minimize the latter errors. The implication of this ambiguity is
that it may be impossible to construct a zero error model without violating the rigidbody or rigid contact models, meaning that the best we can hope to achieve in terms
of performance is the “Ideal” line.
We can further examine the errors made in the models’ predictions to quantify
regions of the input space in which we expect models to perform comparativelybetter and worse. This information is valuable if we are interested in selecting initial
conditions that are easier to predict or rejecting initial conditions that may be moredifﬁcult to predict (for purposes of control). Figure 9depicts the performance of the
AP Poisson model in predicting the outcome of a contact (error in predicting thevelocity of the COM) as a function of the normal and tangential components of the
pre-contact system momentum measured at the contact point. The region in which
the tangential momentum at the contact point is near zero or changes direction provesto be the most difﬁcult to predict; we will discuss this phenomenon in Sect. 3.2.2.
-0.02 -0.01 0 0.01 0.02 0.03 0.04-0.11-0.105-0.1-0.095-0.09-0.085-0.08-0.075-0.07
0.20.40.60.811.21.4
Fig. 9 Performance of AP Newton model depicted in the momentum space as a function of the
normal and tangential components of the pre-contact momentum of the system measured at the
contact point. Blue regions represent high accuracy, yellow regions predict high error
568 N. Fazeli et al.
0 0.05 0.1 0.15 0.2 0.25 0.3051015PDE
0.2 0.3 0.4 0.5 0.6 0.7 0.8012345PDE
0 0.05 0.1 0.15 0.2 0.250.20.30.40.50.60.70.8
0.010.020.030.040.050.060.070.08(a) (b)
Fig. 10 Identiﬁed values of (μ,ε) for each drop, where ( a) parameter values colored by the ability
of the identiﬁed pair to predict the impulse, and (b) distribution of the parameters
The optimization in Sect. 3.2.1 to ﬁnd the ensemble parameters hides some of the
details of individual trials. In this section we ﬁnd the values of (μ,ε) that best explain
each drop, for example Fig. 10shows the identiﬁed parameters for the AP Newton
model, and we make the following observations:
1. We observe a sharp peak at μ=0.
2. The lower left region in Fig. 10a shows a loss in performance.
3. The distribution of εis uni-modal.
4. The means of the distributions of μandεare very close to the “optimal” values
computed for the ensemble parameters.
5. Neither distribution is tightly localized about a single value. This is perhaps the
most important feature to be noted since it is common practice to select a single
value of the parameters.
The fact that such variation in parameter values exists, particularly for the value
ofμ, begins to highlight issues of interpretation of the parameters. The intuitive
expectation for a coefﬁcient of friction is that it should be constant for all impacts, but
these experiments demonstrate that this is not the case. The coefﬁcient of friction in
these models acts more similarly to a restitution variable that regulates the magnitude
of the tangential impulse as a function of the pre-contact conditions rather than asthe typical notion of the coefﬁcient of friction (e.g., that used in the inclined surface
experiments).
Part of the challenge in identifying μis that not all impacts are informative.
Impacts that stick provide no information other than a lower bound for μ. Effectively,
any coefﬁcient of friction above the value found by the identiﬁcation is also able
to explain the interaction equally well which partially explains the dispersion in
identiﬁed μ. To prevent parameter drift and an ill-posed optimization, we use μ
sas
a bound on the coefﬁcient of friction.
We now look at the anomalous cases for which μ=0 in isolation; Fig. 11as h o w s
such an example. When the contact point lies directly below the center of mass of
Fundamental Limitations in Performance … 569
-0.1 -0.05 0 0.05 0.100.020.040.060.080.10.120.140.160.180.2
0.010.0150.020.0250.030.0350.040.0450.050.0550.06
(a) Conﬁ guration 1-0.1 -0.05 0 0.05 0.100.020.040.060.080.10.120.140.160.180.2
0.010.020.030.040.050.06
(b) Conﬁ guration 2
Fig. 11 a An example of μ=0, this impact is characterized by the point of contact being located
directly under the center of mass of the object. In this situation, the Energy Ellipse’s axis is aligned to
the normal impulse direction. bExample of a measured impulse outside the convex set of predictions
made by the model due to “spin-reversal”
Table 2 Fraction of drops that lie within the predictive range of each model
DrumShell AP Poisson AP Newton Mirtich Wang–Mason Whittaker
Fraction 0.355 0.357 0.458 0.413 0.455 0.456
the object, the Energy ellipse’s major axis aligns with the normal impulse direction
and the convex set of predictions for the outcome of the contact collapses to a very
narrow band, resulting in a very small predictive range. A small prediction rangecoupled with sensor noise the predictive range of the models. In the case shown
in Fig. 11, the space spans to the left, but the measured impulse lies to the right;
consequently, the value of the coefﬁcient of friction tends to zero to get as close as
possible to the target value. This issue highlights the difﬁculties in interpreting μ
as the coefﬁcient of friction within the context of these impact models. The secondset of anomalous cases is characterized by the inability to predict “spin-reversal”, anexample of which can be seen in Fig. 11b. In these scenarios, the measured impulse
does not fall within the predictive range of the model and a residual error will alwaysbe present, thereby explaining the region in Fig. 10where the parameters take on
values with non-zero errors. Table 2quantiﬁes the fraction of data that lies within the
convex set of each model. The relative ratios are quite low which indicates that themodels suffer from potentially restrictive prediction ranges. Note that models with
similar regions in the energy ellipse exhibit similar ratios, exempliﬁed by the AP
Poisson and the Drumwright–Shell models.
4 Conclusion and Summary
Rigid contact models are popular due to their simplicity and computational efﬁciency
yet they exhibit clear limitations in predictive power and accuracy. In this paper, we
empirically demonstrated the inability to predict back-spin and challenges with both
system identiﬁcation and interpretation of model parameters.
570 N. Fazeli et al.
The limitations of these analytical models can partially be attributed to the param-
eterization of a subset of the admissible impulse space. The independence of the
parameters from the inputs to the models and the limited regions for prediction(Table 2) contribute to the errors in predictions. Two approaches can remedy these
shortcomings, we may either learn the mapping directly from the impulses and doaway with the parameterization all together (data-driven approach) or use the modelpredictions as a prior and correct the impulse predictions using a data-driven model
(data-augmented approach). In [8] we explore these approaches and compare per-
formances across analytical and learned models.
The rigid models studied assume a single point contact and instantaneous impact.
This assumption is only an approximation and the impact event occurs over a ﬁnite
time and involves a small amount of local deformation leading to a distributed impactimpulse over a patch. In [8] we develop a data-driven contact model that accounts for
this deformation using an instantaneous wrench and demonstrate that the resulting
model can signiﬁcantly outperform the rigid contact models.
This study demonstrates the importance of data in model validation and highlights
the importance of experimentation in parameter selection, and its potential role indeveloping data-driven contact models [ 8,31]. We hope that this study serves as
motivation to experimentally validate other models frequently leveraged in roboticsand leads to their better understanding.
References
1. Anitescu, M., Potra, F.A.: Formulating dynamic multi-rigid-body contact problems with friction
as solvable linear complementarity problems. Nonlinear Dyn. 14, 231–247 (1997)
2. Chatterjee, A., Ruina, A.: A new algebraic rigid body collision law based on impulse space
considerations. ASME J. Appl. Mech. 65(4), 939–951 (1998)
3. Chavan Daﬂe, N., Rodriguez, A .: Prehensile pushing: in-hand manipulation with push-
primitives. In: IEEE/RSJ International Conference on Intelligent Robots Systems (IROS), pp.
6215 – 6222 (2015)
4. Drumwright, E., Shell, D.A.: Modeling contact friction and joint friction in dynamic robotic
simulation using the principle of maximum dissipation. In: Proceedings of Workshop on theAlgorithmic Foundations of Robotics (W AFR), pp. 249–266. Springer, Berlin (2010)
5. Fazeli, N., Tedrake, R., Rodriguez, A.: Identiﬁability analysis of planart rigid-body frictional
contact. In: International Symposium Robotics Research (ISRR), pp. 665–682. Springer, Cham(2016)
6. Fazeli, N., Donlon, E., Drumwright, E., Rodriguez, A.: Empirical evaluation of common contact
models for planar impact. In: 2017 IEEE International Conference on Robotics and Automation(ICRA), pp. 3418–3425 (2017)
7. Fazeli, N., Kolbert, R., Tedrake, R., Rodriguez, A.: Parameter and contact force estimation of
planar rigid-bodies undergoing frictional contact. Int. J. Robot. Res. 36(13–14), 1437–1454
(2017)
8. Fazeli, N., Zapolsky, S., Drumwright, E., Rodriguez, A.: Learning data-efﬁcient rigid-body
contact models: case study of planar impact (2017). arXiv:171005947
9. Gautier, M., Khalil, W.: On the identiﬁcation of the inertial parameters of robots. In: IEEE
Conference on Decision and Control, pp 2264–2269. Austin (1988)
10. Hertz, H.: On the contact of elastic solids. J. Reine Angew. Math. 92, 156–171 (1881)
Fundamental Limitations in Performance … 571
11. Hogan, F., Rodriguez, A.: Feedback control of the pusher-slider system: a story of hybrid and
underactuated contact dynamics. In: Proceedings of the Workshop on Algorithmic Foundation
Robotics (W AFR). San Francisco (2016)
12. Kane, T.R., Levinson, D.A.: Dynamics: Theory and Applications. McGraw-Hill, New Y ork
(1985)
13. Khosla, P ., Kanade, T.: Parameter identiﬁcation of robot dynamics. In: IEEE Conference on
Decision and Control, pp. 1754–1760 (1985)
14. Koval, M., Pollard, N., Srinivasa, S.: Pose estimation for planar contact manipulation with
manifold particle ﬁlters. Int. J. Robot. Res. 7(34), 922–945 (2015)
15. Kraus, P .R., Kumar, V .: Compliant contact models for rigid body collisions. In: Proceedings
of International Conference on Robotics and Automation, vol. 2, pp. 1382–1387 (1997)
16. Lacoursiére, C.: Ghosts and machines: regularized variational methods for interactive simula-
tions of multibodies with dry frictional contacts. Ph.D. thesis, Umeå University, Umeå (2007)
17. Lankarani, H., Nikravesh, P .: A contact force model with hysteresis damping for impact analysis
of multibody systems. J. Mech. Design 112(3), 369–376 (1990)
18. Li, Z., Kota, S.: Virtual prototyping and motion simulation with adams. J. Comput. Inf. Sci.
Eng. 1(3), 276–279 (2001)
19. Marhefka, D.W., Orin, D.E.: A compliant contact model with nonlinear damping for simulation
of robotic systems. IEEE Trans. Syst. Man Cybern. 29(6), 566–572 (1999)
20. Mirtich, B.: Impulse-based dynamic simulation of rigid body systems. Ph.D. thesis, University
of California, Berkeley (1996)
21. Nubiola, A., Bonev, I.A.: Absolute calibration of an abb irb 1600 robot using a laser tracker.
Robot. Comput. Integr. Manuf. 29(1), 236–245 (2013)
22. Posa, M., Cantu, C., Tedrake, R.: A direct method for trajectory optimization of rigid bodies
through contact. Int. J. Robot. Res. 33(1), 69–81 (2013)
23. Shapiro, A., Faloutsos, P ., Ng-Thow-Hing, V .: Dynamic animation and control environment.
In: Proceedings of Graphics Interface 2005, Canadian Human-Computer CommunicationsSociety, pp. 61–70 (2005)
24. Slotine, J.J.E.: On the adaptive control of robot manipulators. Int. J. Robot. Res. 6(3), 49–59
(1987)
25. Song, P ., Kraus, P ., Kumar, V ., Dupont, P .: Analysis of rigid body dynamic models for simulation
of systems with frictional contacts. J. Appl. Mech. 68 (2000)
26. Stewart, D.E.: Rigid-body dynamics with friction and impact. SIAM 42, 3–39 (2000)
27. Stewart, D.E., Trinkle, J.C.: An implicit time-stepping scheme for rigid body dynamics with
inelastic collisions and coulomb friction. Int. J. Numer. Methods Eng. 39(15), 2673–2691
(1996)
28. Stronge, W.J.: Rigid body collisions with friction. Proc. R. Soc. Lond. A 431, 169–181 (1990)
29. Wang, Y ., Mason, M.T.: Two-dimensional rigid-body collisions with friction. ASME J. Appl.
Mech. 59, 635–642 (1992)
30. Whittaker, E.T.: A Treatise on the Analytical Dynamics of Particles and Rigid Bodies, 4th edn.
Dover, Mineola (1944)
31. Y u, K.T., Bauza. M., Fazeli. N., Rodriguez, A.: More than a million ways to be pushed. a high-
ﬁdelity experimental data set of planar pushing. In: Proceedings of the IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), Daejeon (2016)
Object Shape Estimation Through
T ouch-Based Continuum Manipulation
Huitan Mao and Jing Xiao
1 Introduction
Information of object shape is crucial for many robotic tasks. A grasp can be planned
to fetch an object with a known shape. When planning a path, collision can be checkedagainst an object using its shape and conﬁguration. Information of object shape also
facilitates object detection, recognition, and pose estimation. Usually, the shape of an
object is either provided as a priori knowledge or acquired by object model building
through sensing, especially vision and tactile sensing.
Vision sensing has been widely used for object model building. An object appear-
ance model can be built by either moving an RGB-D camera around the target object
[21] or moving the object with a turntable [ 4,25]. Through robotic manipulation to
change the bottom surface of a table-top object, an automatic approach interleav-ing perception and manipulation [ 27] is able to build the entire surface model of
the object. However, vision sensing can be ineffective for transparent objects or in
environments with poor illumination and specularity conditions.
Tactile sensing is useful for exploring object shapes and building object models
when vision is ineffective. By equiping a robotic end-effector with tactile sensors,
the surface of an object can be explored with guarded moves [ 3] and compliant
motion [ 12,22]. Object recognition [ 32] and grasping of unknown objects [ 26] can
also be achieved. In [ 8], recognition of curved surfaces through touch is achieved
by matching contact points to principal-curvature-based local geometry features.Similar features are also shown to be useful in reconstructing the local surface patch
by ﬁtting a high-order polynomial [ 10].
H. Mao (B)·J. Xiao
Department of Computer Science, University of North Carolina at Charlotte,9201 University City Blvd, Charlotte, NC 28223, USA
e-mail: hmao4@uncc.edu
J. Xiao
e-mail: xiao@uncc.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_42573
574 H. Mao and J. Xiao
In order to guide touch-based exploration, i.e., to decide where to touch next to
collect contact points, there are several methods. One method uses a dynamic poten-
tial ﬁeld [ 1], where a uniformly attractive potential ﬁeld is updated as more contacts
are made and generates repulsive forces to push the touch-enabled hand to visit
unexplored areas. Gaussian Process (GP) [ 23] is used to drive active exploration into
uncertain areas. In [ 31], discrete touch probings of the end-effector are progressively
generated to reduce the uncertainty of the interest area using GP regression. In [ 9], it
is shown that GP classiﬁcation can also effectively bias the exploration towards the
boundaries of the objects, which are more informative of the object shape. Extendedfrom GP , a probabilistic model of uncertainty based on Gaussian Process Implicit
Surfaces (GPIS) [ 30] is used to guide the active exploration and modeling of an object
[3,5] or serve as a framework of data fusion from sensors with different modalities
[7].
However, using only the robot end-effector to touch a target object usually only
makes a couple contact points per probing, and thus it is a slow process to collectsufﬁcient contact points to capture the global shape of the object by changing the
end-effector pose after each probe. The process also does not explicitly utilize the
adjacency information of nearby contact points.
Unlike conventional robotic manipulators, continuum robots [ 2,6,11,24,28,29]
are suitable for whole-arm manipulation to wrap around an object and for maneuver-ing in cluttered space. Autonomous motion planning algorithms are introduced for
generating whole-arm wrapping conﬁgurations [ 16], force-closure wraps [ 15,17],
and manipulator motion constrained by the target object surface [ 14], for a known
object. More recent work addressed object wrapping [ 13] and object modeling using
RGBD sensing [ 19] by a continuum manipulator in an unknown environment.
In this paper, we address the problem of estimating the shape of an unknown object
through obtaining object shape information from touch-based continuum manipula-
tion, which has not been studied before. By wrapping around an object (see Fig. 1
for an example), a continuum manipulator makes many more contacts with an object
Fig. 1 A continuum
manipulator wraps around abox

Object Shape Estimation Through Touch-Based Continuum Manipulation 575
than a conventional robot end-effector. Moreover, the manipulator shape itself in a
wrapping conﬁguration is also indicative of the object shape. Our approach, called
progressive object shape estimation through continuum manipulation (POSE-CoM),
extends the GPIS method by explicitly incorporating the continuum robot arm shape
in addition to the contact points made between the robot and the object in each wrap
to estimate the overall object shape. The approach is shown to be both more efﬁcientand more accurate over existing methods for touch-based object shape estimation.
The rest of the paper is organized as follows. Section 2presents the manipulator
model considered in this paper. Section 3presents the proposed approach. Section 4
reports experimental results on objects with various shapes to demonstrate the advan-
tages of this approach. Section 5concludes the paper.
2 Manipulator Model
We brieﬂy review the manipulator kinematic model used in this work. A continuum
manipulator is considered as a concatenation of ncircular sections when each section
is not in contact or under external loading [ 11]. Each section sec i,i=1,..., n,i s
described by three controllable variables: length si, curvature κi, and orientation φi,
and bounded by its base point pi−1and end point pi. A local coordinate system is
attached to the base point of sec iwith the zaxis tangential to sec ias shown in Fig. 2.
The arm conﬁguration C of an n-section continuum manipulator can be represented
asC={(s1,κ1,φ1),....,( sn,κn,φn)}.
In this work, we assume the continuum manipulator is covered with tactile sensors,
which are able to detect, localize and estimate the normal and tangential directionof the contact points when the robot is in touch with the object. In simulation,
contacts between a continuum robot and an object can be efﬁciently detected using
the collision checking algorithm in [ 15].
Fig. 2 Illustration of the
frame of sec i,c i r c u l a ra r c
center ciand the control
variables si,κiandφi[13]

576 H. Mao and J. Xiao
3 Progressive Object Shape Estimation Through
Continuum Manipulation (POSE-CoM)
An overview of our approach is presented in Fig. 3.F i r s t , touch-based continuum
wrapping progressively moves the continuum robot to a wrapping conﬁguration
based on contact points made between the robot and the object. Next, object shape
data collection gathers contact points and the shape information from the continuum
wrap. Shape estimation uses the data collected so far in a probabilistic framework
based on Gaussian Process Implicit Surfaces (GPIS) to estimate the overall shape
of the object. Active guidance uses the result of estimation to decide the next con-
tinuum wrap to cover the most uncertain region of the object and collect more data
for estimation. The process repeats until either the estimated object shape has low
uncertainty or the robot has exhausted the possible wraps.
3.1 Touch-Based Continuum Wrapping
A continuum wrap of an object here is deﬁned as a planar whole-arm grasp by the
continuum manipulator around a cross section of the object, which further deﬁnes a
wrapping plane .
Initialization
Touch-based con-
tinuum wrapping
Object shape data collection
Shape estimation based
on the accumulated data
EndActive guidance
Low uncertainty or No more wraps
Fig. 3 Overview of the POSE-CoM approach
Object Shape Estimation Through Touch-Based Continuum Manipulation 577
Fig. 4 The wrapping planes are systematically enumerated between the table plane (grey back-
ground) and the black planes orthogonal to the table plane. Left : each red plane is used to generate
a clockwise continuum wrap. Right : each green plane is used to generate a counter-clockwise con-
tinuum wrap. Note that the initial conﬁgurations of the robot in Left and Right are different. The
frame at the robot base is the global coordinate system
Fig. 5 A few examples of the wrapping planes (grey) and their corresponding continuum wraps.
These planes also uniquely cut through the object (a bottle) and pass through the ﬁxed robot base.The frame at the robot base is the global coordinate system
Given a continuum manipulator with a ﬁxed base, its workspace can be decom-
posed into discrete wrapping planes, as shown in Fig. 4. We denote the set of wrapping
planes as WP={wp 1,..., wp c}. On each wrapping plane, the continuum robot can
generate a wrap of an object around the cross section on the plane. See Fig. 5for
some example wraps on different wrapping planes of an object.
For initialization, an initial set of wrapping planes WP 0⊂WP are randomly
selected, and the continuum robot generates wraps on those wrapping planes one by
one, while accumulating shape data of the object from each wrap. The data are thenused to conduct shape estimation. The result is further used to guide the selection of
the next wrapping plane by Active guidance (see Sect. 3.4), and so on.
Now, given a wrapping plane and an initial arm conﬁguration, we use a motion
planning strategy to generate a continuum wrap of an object progressively as guided
by the contact points made along the way [ 20]. The continuum manipulator alternates
between the enclosing motion step and the advancing motion step on the wrapping
plane until a wrap is achieved.
Enclosing motion step brings the robot into contact with the object as much as
possible to create contact points. This is achieved by having the robot curve its
578 H. Mao and J. Xiao
Fig. 6 Illustration of an enclosing motion. Left : Robot at the initial conﬁguration. Middle :T h e
robot makes ﬁrst contact by curving Sect. 1(black). Right : The robot makes the second contact by
curving Sect. 3(green) without penetrating into the object
Fig. 7 Illustration of an advancing motion. Left : the robot section end points are indicated in
yellow, the contact points are in red, and the new endpoint positions are indicated in green. Right :
the new arm conﬁguration solved using constrained inverse kinematics based on the new endpointpositions (green)
sections one by one to make contact with the object until no further contact points
can be made. Figure 6shows an example of such an enclosing motion.
Advancing motion step takes advantage of the contacts made in the enclosing
motion step to move the robot forward to a new arm conﬁguration towards wrap-
ping around the object. The new arm conﬁguration is achieved by (1) moving theendpoint of each robot section a small distance from its current position along the
direction n+tof the closest contact point, where nand tdenote the normal and the
tangential unit vectors of the contact point respectively
1; (2) solving the resulting
arm conﬁguration by the constrained inverse kinematics [ 14] corresponding to the
new endpoint positions from (1).
Figure 7shows an example of advancing motion. The robot backbone(dashed line)
is colored using black, red and green for Sects. 1,2and 3respectively. As the result
1The direction of tis ﬂipped if the dot product between tand the z-axis of the local frame on the
section endpoint closest to the contact point is negative, to ensure that the robot moves towards awrapping conﬁguration.
Object Shape Estimation Through Touch-Based Continuum Manipulation 579
of the previous enclosing motion step, the robot is in contact with the object at the
red points on Sects. 2and 3. Now, for the end point of each section in contact, its new
position (green) is determined by a translation of a small distance from its currentposition (yellow) along n+tof the closest contact point. For Sect. 1that is not in
contact, its new endpoint position is obtained by a small translation from the current
endpoint position along the zaxis of its local frame.
3.2 Object Shape Data Collection and Generation
Two types of shape data are collected in our approach (see Fig. 8(a)):
(i) contact points made between the robot and the object during wraps, and
(ii) arm points sampled along the backbone of the robot when the robot is in the
conﬁguration of a complete wrap, i.e., points indicative of the robot shape.
We denote the collected points from all wraps made (both the contact points and
the arm points) as Xcol={ xi}, where xi∈R3,i=1,2,... A potential function
value yi∈Y⊂Rassociated with xiis deﬁned as follows, based on GPIS [ 30].
yi=⎧
⎪⎨
⎪⎩1i f xiis above the surface
0i f xiis on the surface
−1i f xiis below the surface(1)
The contact points that happen on the object surfaces have y=0, while the arm
points sampled on the backbone of the continuum robot have y=1( s i n c et h ea r m
does not penetrate into the object).
We further generate the above-surface and below-surface points for each contact
point along the contact normal (see Fig. 8b for an illustration) and denote the set of
Fig. 8 Left : the dashed line is the backbone of the continuum robot. The blue dots are the arm
points systematically sampled on the arm backbone. The red dots are the contact points betweenthe robot and the object. Right : the interpolated above-surface and below-surface points from the
contact point along the contact normal
580 H. Mao and J. Xiao
such generated points Xgen⊂R3.N o wl e t Tbe the set of all points that are collected
and generated so far: T={xi,yi},xi∈Xcol∪Xgen,yi∈Y,i=1,2,...
Note that initially Tonly contains points collected and generated from the few ini-
tial wraps. As each new wrap is conducted (based on active guidance – see Sect. 3.4),
more points are added to Tto facilitate more accurate shape estimation.
3.3 GPIS-Based Shape Estimation
Estimation of an object’s shape is done by ﬁnding the points with zero yvalue (i.e.,
the isosurface) in a 3D region of interest. GPIS is used to learn such a mapping f(x):
x∈R3toy∈Rbased on the data in T. It is fully deﬁned by a Gaussian Process
(GP) [ 23] with a mean function μ(x)and a covariance function k(xi,xj), where
j=1,2,... The prior μ(x)is zero. k(xi,xj)is chosen to be the commonly used
squared exponential kernel, and a noise ε∼N(0,σ2
n)is also included:
k(xi,xj)=σ2
fexp(−(xi−xj)2
2l2)+σ2
nδij (2)
where δijis the Kronecker delta, which is 1 iff i=jand 0 otherwise.
The hyper parameters/braceleftbig
σf,l,σn/bracerightbig
are then optimized by maximizing the log-
marginal likelihood [ 23] using the data in T, i.e., the training data.
Next, the zero-mean isosurface is extracted as the current estimation of the object
shape by querying the GPIS model with τtesting points from a 3D region, which is
known to contain the object or is within the reachable region of the robot.
For a testing point x⋆, the predicted distribution is a Gaussian with the mean μ(x⋆)
in Eq. ( 3) and the variance σ2(x⋆)in Eq. ( 4)
μ(x⋆)=kT
⋆K−1Y (3)
σ2(x⋆)=k⋆⋆−kT
⋆K−1k⋆ (4)
where k⋆is a covariance matrix between mtraining points and τtesting points
[k⋆]i=1...m,j=1...τ=k(xi,x⋆j),Kis a covariance matrix between training points
[K]i,j=1...m=k(xi,xj)and k⋆⋆is a covariance matrix between testing points
[k⋆⋆]i,j=1...τ=k(x⋆i,x⋆j). Note that the number of training points mincreases after
new wraps are conducted.
Each testing point corresponding to a zero mean of Eq. ( 3) is a point on the
estimated object shape, and the associated posterior variance of Eq. ( 4) deﬁnes the
uncertainty of the point due to few data points nearby or large shape change.
Object Shape Estimation Through Touch-Based Continuum Manipulation 581
3.4 Active Object Exploration
Our approach actively determines the next wrapping plane to conduct another touch-
based continuum wrap based on a measure of uncertainty. Denote the set of theavailable wrapping planes WP
aasWP a=WP\WP 0.
For each available plane wp i∈WP a,l e t Pibe the set of points on wp ithat are
also on the zero-mean isosurface, then the uncertainty measure uiofwp iis computed
as the average standard deviation σ(q),∀q∈Pi.
Letwp jbe the wrapping plane with uj=max i(ui), then wp jis chosen as the
next wrapping plane to conduct a continuum wrap, and a new iteration in the POSE-CoM process (Fig. 3) starts. The newly collected and generated data from the new
wrap are added to the training set T, and the GPIS model is in turn updated. WP
ais
also updated by removing wp j(after it has been used).
The POSE-CoM process (Fig. 3) is repeated until the maximum uncertainty ujis
below a predeﬁned threshold or WP a=∅.
4 Experiments
We implemented our approach in C ++ and Python under ROS and tested it on a
3.4GHz CPU with 16GB RAM. Objects of various shapes are used (Fig. 9).
To initialize the POSE-CoM process, we randomly select 4 wrapping planes in the
robot workspace for the continuum robot to generate touch-based wraps and collectdata. Next, a GPIS model is trained and queried in the region of [(0 .0−1.0), (0.0−
1.0), (0.0−0.8)](dm). The simulated continuum robot has the following parameters:
the width of each section is 0 .5 (cm), the length of each section can vary from 2 to 15
(cm), the curvature of each section can vary from 0 to 0 .1( c m
−1), and the orientation
of each section can vary from −πtoπ. It typically takes 3 – 15 min to train a
GPIS model. The next wrapping plane is selected according to the active strategy
in Sect. 3.4, then touch-based continuum wrapping is conducted, and the POSE-
CoM process repeats. The process is terminated when 1) the maximum uncertaintymeasure u
jis below a predeﬁned threshold 0 .15, or 2) all the wrapping planes in
the robot workspace have been used at least once to generate continuum wraps.
Figure 10shows the snapshots of robot motion for example continuum wraps. The
accompanying video shows the animated motion of the example continuum wraps.
Fig. 9 Object mesh models: (left to right) bottle, apple, sphere, bunny, pentagon, vase
582 H. Mao and J. Xiao
(a) bottle (b) bunny
(c) apple (d) vase
Fig. 10 Snapshots of the motion of example continuum wraps. Each subﬁgure is a wrap from the
initial conﬁguration (left) to the wrapping conﬁguration (right)
(a) 4 wraps (b) 8 wraps (c) 12 wraps
(d) Left to right: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 11 Shape estimation results for the bottle , colored by uncertainty
4.1 Shape Estimation Results
For objects with simple shape geometry, such as the bottle (Fig. 11) and the sphere
(Fig. 12), with just a few wraps, the estimated shape is already similar to the actual
shape. In Fig. 11, the object shape uncertainty keeps decreasing (i.e., the blue parts
in Fig. 11keep increasing) as more wraps are conducted until all available wraps are
exhausted. The maximum uncertainty measure ujstarts being 0 .38 and reaches 0 .32
at the end. Note that the part near the origin (where the robot arm base is) is more
uncertain (colored mostly red), which is due to that the robot cannot reach this area
as it is too close to the robot base. In Fig. 12, note how the ﬁfth wrap (counted from
left) in Fig. 12(d) helps reduce the uncertain (red) area in Fig. 12(a). The POSE-CoM
process is terminated after 7 wraps as ujreaches 0 .14 (below threshold 0 .15).
For objects with more complex shapes, such as the apple (Fig. 13), bunny (Fig. 14),
pentagon (Fig. 15), and hollow vase (Fig. 18), more varied wraps are needed to better
capture certain details for more accurate shape estimation results. For example, wraps
Object Shape Estimation Through Touch-Based Continuum Manipulation 583
(a) 4 wraps (b) 5 wraps (c) 7 wraps
(d) Left to right: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 12 Shape estimation results for the sphere , colored by uncertainty
(a) 4 wraps (b) 8 wraps (c) 12 wraps
(d) Left to ri ght: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 13 Shape estimation results for the apple , colored by uncertainty
(a) 6 wraps (b) 9 wraps (c) 12 wraps
(d) Left to right: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 14 Shape estimation results for the bunny , colored by uncertainty
584 H. Mao and J. Xiao
(a) 4 wraps (b) 8 wraps (c) 12 wraps
(d) Left to ri ght: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 15 Shape estimation results for the pentagon , colored by uncertainty
Ta b l e 1 The total number of contact points and arm points collected on each object, the total
number of wraps, and the average time Twrap per wrap for (motion planning + collision detection)
Object # contact points # arm points #w r a p s Twrap (ms)
Bottle 143 150 12 135
Apple 259 172 12 425
Sphere 267 113 7 246
Bunny 264 172 12 432
Pentagon 141 167 12 132
Va s e 153 160 12 278
are needed around the stalk of the apple, the ears of the bunny, and the entrance of
the hollow vase, to better capture those details. However, the GPIS modeling tends
to blur the connection between a detail and the main shape because the prediction
assumes smooth connection. Thus, the estimation results of a complex shape tend to
resemble certain bounding envelopes of the actual shape.
Table 1shows that a total of 500 to 900 points are used to estimate the shape
of an object.2It would be very time-consuming if those points were collected by
a conventional manipulator making point contacts with the object [ 3,9,31,32].
However, our approach is very efﬁcient by using continuum wraps automatically
generated from our motion planning algorithm. Table 1also shows that it is very fast
to plan a touch-based continuum wrap.
Figure 16shows the reconstructed mesh models from the shape estimation results
using the Marching Cubes’ algorithm [ 18]. As explained earlier, those models capture
enveloping contours of the actual shapes because sharp details were blurred by the
GPIS modeling under noisy observations.
2Recall that one above-surface point and one below-surface point are generated for each contact
point along the contact normal.
Object Shape Estimation Through Touch-Based Continuum Manipulation 585
(a) bottle (b) apple (c) sphere (d) bunny (e) pentagon (f) vase
Fig. 16 Object mesh models reconstructed from shape estimation results using Marching Cubes’
algorithm [ 18], colored by elevation
Ta b l e 2 Average distances between the wrapping conﬁgurations of a reconstructed model and the
original mesh model. # wraps indicates the number of wraps used to reconstruct the model
Object Intermediate model 1 Intermediate model 2 Final model
#w r a p s average
distance#w r a p s average
distance# wraps average
distance
Bottle 4 0.31 8 0.27 12 0.27
Apple 4 0.44 8 0.36 12 0.28
Sphere 4 0.28 5 0.26 7 0.24
Bunny 6 0.57 9 0.47 12 0.31
Pentagon 4 0.34 8 0.19 12 0.16
Va s e 6 0.48 9 0.41 12 0.31
Fig. 17 Comparing example wraps on the original mesh model and the reconstructed model. a
wrap on the original apple model bwrap on a reconstructed apple model cwrap on the original
vase model dwrap on a reconstructed vase model
If the reconstructed model is identical to the original mesh model of an object,
given the same arm initial conﬁguration, model location, and the wrapping plane,
the ﬁnal wrapping conﬁguration Crof the reconstructed model should be the same
as the ﬁnal wrapping conﬁguration Coof the original mesh model. Therefore, the
distance of normalized conﬁgurations /bardblCr/prime−Co/prime/bardbl, where Cr/primeandCo/primeare obtained
by normalizing the component values in CrandCo, is indicative of how accurate the
shape estimation is. We use the average distance of multiple wraps as a parameter
to measure the accuracy of shape estimation indirectly, as shown in Table 2.I ti s
clear that model quality improves as more wraps are used to reconstruct the model.
Figure 17compares example wraps visually.
586 H. Mao and J. Xiao
(a) 6 wraps (b) 9 wraps (c) 10 wraps (d) 12 wraps
(e) Left to ri ght: the ﬁrst conducted wrap to the last conducted wrap.
Fig. 18 Shape estimation results of the vase , colored by uncertainty
(a) 6 wraps (b) 9 wraps (c) 10 wraps (d) 12 wraps
Fig. 19 Shape estimation results of the vase without using arm points, colored by uncertainty
4.2 Signiﬁcance of the Arm Points
Figure 19shows the shape estimation results of the hollow vase using only contact
points. Since the object is hollow, continuum wraps cannot make a large number of
contacts, and thus the shape estimation result is inaccurate. However, the arm shape
at a wrapping conﬁguration itself is indicative of the object shape, and the arm points
can be used to compensate for the lack of sufﬁcient contact points and determine
the object geometry. As shown in Fig. 18, our approach using both arm points (robot
proprioception) and contact points better captured the vase shape and also captured
the hollowness of the vase using as few as 6 wraps. Whereas, without using the arm
points, as shown in Fig. 19, the hollowness of the vase could not be captured even
with 12 wraps.
5 Conclusions
We have presented an approach POSE-CoM of progressive object shape estimation
through touch-based continuum manipulation and a GPIS-based probabilistic model.
Unlike shape estimation based on contact points collected through the end-effector of
Object Shape Estimation Through Touch-Based Continuum Manipulation 587
a conventional, articulated manipulator, shape estimation through continuum manip-
ulation is both more efﬁcient and effective by deliberate use of the robot propriocep-
tion data in addition to contact information, as demonstrated in the experiments onobjects of various shapes.
Acknowledgements This work is supported by the US NSF grant IIP-1439695.
References
1. Bierbaum, A., Rambow, M., Asfour, T., Dillmann, R.: A potential ﬁeld approach to dexterous
tactile exploration of unknown objects. In: Proceedings of IEEE International Conference onHumanoid Robots (2008)
2. Burgner-Kahrs, J., Rucker, D.C., and Choset, H.: Continuum robots for medical applications:
a survey. IEEE TRO 31(6) (2015)
3. Caccamo, S., Bekiroglu, Y ., Ek, C.H., Kragic, D.: Active exploration using gaussian random
ﬁelds and gaussian process implicit surfaces. In: Proceedings of IEEE/RSJ IROS (2016)
4. Choi, C., Christensen, H.I.: 3D pose estimation of daily objects using an RGB-D camera. In:
Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(2012)
5. Dragiev, S., Toussaint, M., Gienger, M.: Gaussian process implicit surfaces for shape estimation
and grasping. In: Proceedings of IEEE ICRA (2011)
6. Frazelle, C.G., Kapadia, A.D., Fry, K.E., Walker, I.D.: Teleoperation mappings from rigid link
robots to their extensible continuum counterparts. In: Proceedings of IEEE ICRA (2016)
7. Gerardo-Castro, M.P ., Peynot, T., Ramos, F.: Laser-radar data fusion with gaussian process
implicit surfaces In: Field and Service Robotics. Springer, Cham (2015)
8. Ibrayev, R., Jia, Y .-B.: Recognition of curved surfaces from one-dimensional tactile data. IEEE
Trans. Autom. Sci. Eng. 9(3) (2012)
9. Jamali, N., Ciliberto, C., Rosasco, L., Natale, L.: Active perception: building objects’ models
using tactile exploration. In: Proceedings of IEEE International Conference on Humanoid
Robots (Humanoids) (2016)
10. Jia, Y .-B., Tian, J.: Surface patch reconstruction from one-dimensional tactile data. IEEE Trans.
Autom. Sci. Eng. 7(2) (2010)
11. Jones, B.A., Walker, I.D.: Kinematics for multisection continuum robots. IEEE TRO 22(1)
(2006)
12. Li, Q., Schürmann, C., Haschke, R., Ritter, H.J.: A control framework for tactile servoing. In:
Robotics: Science and systems (2013)
13. Li, J., Teng, Z., Xiao, J.: Can a continuum manipulator fetch an object in an unknown cluttered
space? IEEE Robot. Autom. Lett. (2017)
14. Li, J., Xiao, J.: A general formulation and approach to constrained, continuum manipulation.
Adv. Robot. 29(13) (2015)
15. Li, J., Xiao, J.: An efﬁcient algorithm for real time collision detection involving a continuum
manipulator with multiple uniform-curvature sections. Robotica 34(7) (2016)
16. Li, J., Xiao, J.: Determining grasping conﬁgurations for a spatial continuum manipulator. In:
Proceedings of IEEE/RSJ IROS (2011)
17. Li, J., Xiao, J.: Progressive planning of continuum grasping in cluttered space. IEEE TRO
32(3) (2016)
18. Lorensen, W.E., Cline, H.E.: Marching cubes: a high resolution 3d surface construction algo-
rithm. In: Proceedings of ACM Siggraph Computer Graphics (1987)
19. Mao, H., Teng, Z., Xiao, J.: Progressive object modeling with a continuum manipulator in
unknown environments. In: Proceedings of IEEE ICRA (2017)
588 H. Mao and J. Xiao
20. Mao, H., Zhang, M.M., Xiao, J., Daniilidis, K.: Shape-based object classiﬁcation and recog-
nition through continuum manipulation. In: Proceedings of IEEE/RSJ IROS (2017)
21. Newcombe, R.A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison, A.J., Kohi, P .,
Shotton, J., Hodges, S., Fitzgibbon, A.: Kinectfusion: real-time dense surface mapping andtracking. In: Proceedings of IEEE International Symposium on Mixed and Augmented Reality(ISMAR) (2011)
22. Okamura, A.M., Curkosky, M.: Feature-guided exploration with a robotic ﬁnger. In: Proceed-
ings of IEEE ICRA (2001)
23. Rasmussen, C.E.: Gaussian Processes for Machine Learning. MIT Press, Cambridge (2006)24. Robinson, G., Davies, J.B.C.: Continuum robots-a state of the art. In: Proceedings of IEEE
ICRA (1999)
25. Singh, A., Sha, J., Narayan, K.S., Achim, T., Abbeel, P .: Bigbird: a large-scale 3D database of
object instances. In: Proceedings of IEEE International Conference on Robotics and Automa-tion (ICRA) (2014)
26. Sommer, N., Li, M., Billard, A.: Bimanual compliant tactile exploration for grasping unknown
objects. In: Proceedings of IEEE ICRA (2014)
27. Teng, Z., Mao, H., Xiao, J.: Automatic object modeling through integrating perception and
robotic manipulation. In: Proceedings of International Symposium on Experimental Robotics
(ISER) (2016)
28. Walker, I.D.: Continuous backbone “continuum” robot manipulators. ISRN Robot. 2013 (2013)
29. Webster III, R.J., Romano, J.M., Cowan, N.J.: Mechanics of precurved-tube continuum robots.
IEEE Trans. Robot.(TRO) (2009)
30. Williams, O., Fitzgibbon, A.: Gaussian process implicit surfaces. Gaussian Proc. Pract. (2007)31. Yi, Z., Calandra, R., V eiga, F., van Hoof, H., Hermans, T., Zhang, Y ., Peters, J.: Active tactile
object exploration using gaussian processes. In: Proceedings of IEEE/RSJ IROS (2016)
32. Zhang, M.M., Kennedy, M., Hsieh, M., Daniilidis, K.: A triangle histogram for object classi-
ﬁcation by tactile sensing. In: Proceedings of IEEE/RSJ IROS (2016)
Caging and Path Non-existence: A
Deterministic Sampling-BasedVeriﬁcation Algorithm
Anastasiia Varava, J. Frederico Carvalho, Florian T. Pokorny
and Danica Kragic
1 Introduction
Acage is an arrangement of obstacles that restricts the mobility of the object in
such a way that it cannot escape arbitrarily far from its initial position. In robotics,caging relates to grasping, where it is used either as an alternative to contact-based
grasping or as a pregrasping condition [ 8,9,14,18–20]. Apart from that, caging has
applications in multi-agent manipulation [ 11].
In terms of the object’s conﬁguration space, the object is caged if its conﬁguration
lies in a bounded connected component of its free space. Therefore, a cage and apath can be seen as two opposite concepts: while the existence of a path ensures
reachability, the presence of a cage means that the object’s mobility is restricted to
the current bounded connected component.
The problem of proving path non-existence can be applied to path planning.
Most of the sampling-based planning algorithms perform iterative random sampling
and incrementally build paths between conﬁgurations. Usually these algorithms areeither probabilistically complete [ 1] or resolution complete [ 6], but they are not
guaranteed to ﬁnd a path in ﬁnite time if one exists. Therefore, one of the practical
questions is how long the planner should search for a path before concluding that itdoes not exists. Instead of using stopping heuristics, one could avoid this problem
A. Varava (B)·J. F. Carvalho ·F. T. Pokorny ·D. Kragic
KTH Royal Institute of Technology, Stockholm, Swedene-mail: varava@kth.se
J. F. Carvalho
e-mail: jfpbdc@kth.se
F. T. Pokorny
e-mail: fpokorny@kth.se
D. Kragic
e-mail: dani@kth.se
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_43589
590 A. Varava et al.
by proving path non-existence. The latter is a challenging problem as it requires
retrieving information about the connectivity of the entire conﬁguration space rather
than ﬁnding a single path which has been addressed in [ 2,10,23].
Essentially, computing the connected components of the free space would solve
two problems: (i)proving or disproving caging and (ii)checking if the object can be
moved from one conﬁguration to another. In this paper, we address both problemsfor rigid objects in 2D and 3D workspaces.
To compute connected components, we construct an approximation of the free
space of the object consisting of a collection of lower dimensional ‘slices’ corre-sponding to small intervals of the object’s orientation value. Each slice is constructed
as the collision space of a ‘core’ of the object with ﬁxed orientation; this ‘core’ is
chosen in such a way that its collision space is a proper subset of the collision spaceof the initial object for each orientation value in the considered interval.
In this paper, we present our approach at a conceptual level. Further theoretical
and experimental evaluation of the 2D case can be found in [ 21].
2 Related Work
The notion of a planar cage was initially introduced by Kuperberg in 1990 [ 5]a sa
set of ncoplanar points lying in the complement of a polygon and preventing it from
escaping arbitrarily far from its initial position. This problem can be directly appliedto robotics if we represent the ﬁngertips or mobile robots as points, and the object
as a polygon. Point-based caging of polygons and polytopes has received a lot of
attention in the context of grasping and multi-agent manipulation. Rimon and Blake[16] proposed an algorithm computing a set of caging conﬁgurations of a two-ﬁngered
hand for planar non-convex objects. In [ 12], Pipattanasomporn and Sudsang proposed
an algorithm reporting all two-ﬁnger caging sets for a given concave polygon. Vahediand van der Stappen extended this result in [ 22] by proposing an algorithm that returns
all caging placements of a third ﬁnger given a polygonal object and a placement of
two other ﬁngers. A similar approach has also been adopted for caging 3D objects.Pipattanasomporn and Sudsang [ 13] proposed an algorithm for computing all two-
ﬁnger cages for non-convex polytopes. Rodriguez et al. introduced a notion of a
pregrasping cage in their work [ 17]. Starting from a pregrasping cage, a manipulator
can move to a form closure grasp without breaking the cage.
In the above mentioned works ﬁngertips are represented as points or spheres,
and more complex shapes of caging tools usually are not taken into account. Al-
ternatively, one can derive sufﬁcient caging conditions based on speciﬁc geometric
and topological features of the object and the caging tools. In particular, Stork etal. proposed an approach towards caging objects with ‘holes’ in their works [ 14,
18,19]. Subsequently, Varava et al. considered loop-based caging of objects with
narrow parts—so-called ‘necks’ and ‘double forks’ [ 20]. Sufﬁcient conditions for
caging objects of particular shapes have also been studied in [ 8,9].
Caging and Path Non-existence: A Deterministic Sampling-Based … 591
The advantage of the above mentioned approaches is that they utilize global geo-
metric properties of the objects. Therefore, one does not have to explicitly reconstruct
the free space of the object, which is a challenging task due to its high dimensional-ity. However, these methods require the objects to exhibit particular shape features,
which prevents us from applying them to caging objects of arbitrary shape. A more
general approach is to directly analyze the topological properties of the conﬁgurationspace of the object.
In this context, caging is closely related to the problem of proving path non-
existence. Motivated by motion planning applications, in their pioneering work [ 2]
Basch et al. propose an approach towards proving that two conﬁgurations are discon-
nected because the object is ‘too big’ or ‘too long’ to pass through a ‘gate’ between
them. The authors apply their algorithm to several simple objects and integrate it witha PRM motion planner. In [ 23], Zhang et al. use approximate cell decomposition and
prove path non-existence. They decompose a conﬁguration space into a set of cells
and for each cell decide if it lies in the collision space. In [ 10] McCarthy et al. propose
a somewhat similar approach. There, they randomly sample the conﬁguration space
and reconstruct its approximation as an alpha complex. They later use it to check the
connectivity between pairs of conﬁgurations.
In this paper, we also aim to study the connectivity of the free space of the object.
However, unlike [ 10], we do not construct the collision space directly. Instead we
decompose it into a ﬁnite set of lower dimensional ‘slices’. We compute the connected
components of the latter and analyze the possible transitions between different slices.
This allows us to overcome the dimensionality problem without losing any necessaryinformation about the topology of the conﬁguration space.
3A n O v e r v i e w o f O u r M e t h o d
In this section we state the problem we are going to address, introduce the necessary
notation, and provide an intuitive description of our approach while avoiding going
into technical details.
3.1 Problem Formulation
Let us start with the necessary deﬁnitions. A rigid object is a compact connected sub-
set of Rd, where d∈{2,3}.Aset of obstacles is a compact subset of Rd. We assume
that both the obstacles and the object can be approximated as unions of balls lyingin their interior, S={B
R1(X1) ,..., BRn(Xn)}and O={Or1(Y1),... , Orm(Ym)}of
radii R1,..., Rnandr1,..., rmrespectively.
Let C=SE(d)denote the conﬁguration space of the object. We deﬁne its col-
lision space as the set of the objects conﬁgurations in which the object penetrates
the obstacles: Ccol={c∈C|int(c(O))∩int(S)/negationslash=∅ } , where c(O)denotes the ob-
592 A. Varava et al.
ject in a conﬁguration c. Note that this deﬁnition allows the object to be in contact
with the obstacles. The free space Cfre eis the complement of the collision space:
Cfre e=C−Ccol.
Our goal is to provide an algorithm which: (i)takes the approximations of the
object and the obstacles as input; (ii)constructs an approximation of the collision
space; (iii) for a given conﬁguration, checks whether it is caged; returns “True” if
it is guaranteed to be caged, and “Undecided” otherwise; (iv)for a given pair of
conﬁgurations, checks whether there is a collision-free path between them; returns
“False” if they are guaranteed to be disconnected, and “Undecided” otherwise.
3.2 Our Approach: The General Idea
To address our problem we compute an approximation of Ccol, and then analyse
the connectivity of its complement—the free space. Let us think of Cas a product
C=Rd×SO(d)where d∈{2,3}.
Note that as long as the object is caged by a subset of the collision space, it is
guaranteed to be caged by the entire collision space. Therefore, to solve our problem,
it is not necessary to reconstruct the exact collision space Ccol. Instead, it is enough
to approximate it by its sufﬁciently large (i.e., capturing the necessary connected
components) subset Ccol
a⊂Ccol, see Fig. 1.
Instead of reconstructing Ccoldirectly, we work with its projections Ccol(Oφ)
onto Rdcorresponding to ﬁxed orientation φof the object in SO(d).
Since the number of all possible orientation values is inﬁnite, we divide Cinto a
ﬁnite number of slices:
Deﬁnition 1 A slice of the conﬁguration space Cis a subset of Cdeﬁned as follows:
SlU=Rd×U,
where Uis an open subset of SO(d).
Fig. 1 Consider collision
space of the simplestobject—a point. The realcollision space
Ccolis
depicted in light grey, whileits approximation
Ccol
ais
depicted in dark grey. Theapproximation lies strictlyinside the real collisionspace, and captures all thethree (two bounded and oneunbounded) connectedcomponents of the free space

Caging and Path Non-existence: A Deterministic Sampling-Based … 593
Fig. 2 Anε-core Oφ
εof the
object Oφis depicted in dark
grey; it is contained inside of
the rotated object Oθ
Similarly, we denote a slice of the collision (free) space by Slcol
U(Slfre e
U). For each
slice we build an approximation of the collision space Slcol
U, so that the union of all
of them represents the entire collision space:
Ccol=/uniondisplay
U⊂SO(d)Slcol
U
Let us now discuss how we construct the slices. Note that to show that the object
collides with an obstacle it is enough to show that some part of the object is incollision. We deﬁne an ε−core of the object as follows:
Deﬁnition 2 Anε-core of an object Ois a subset O
εof the object such that any
point of Oεlies at least at a distance1εfrom the boundary of O:
Oε={p∈O|d(p,∂O)≥ε}
Let Oφand Oφ
εdenote an object Oand its ε-core with a ﬁxed orientation φ∈
SO(d), respectively. Note that the collision space of an object with a ﬁxed rotation
is a subset of Rd.
In Sect. 4, we show that for an object Oφand its ε-core Oφ
εwith a ﬁxed orientation
φwe can ﬁnd an open neighbourhood U(ε,φ) ⊂SO(d)ofφsuch that for any
θ∈U(ε,φ) theε-core Oφ
εis fully contained within a slightly rotated object Oθ,
see Fig. 2. Since SO(d)is compact, we choose a ﬁnite set of rotations {φ1,...,φ s}
so that the corresponding family of sets U(ε,φ i)is a ﬁnite open cover of SO(d):
SO(d)=/uniontext
i∈{1,...,s}U(ε,φ i).
Now note that if Oφ
ε⊂Oθ, then Ccol(Oφ
ε)⊂Ccol(Oθ). Thus, we can approximate
the collision space of a slice by aSlcol
U(ε,φ) deﬁned as follows:
Slcol
U(ε,φ)≈aSlcol
U(ε,φ)=Ccol(Oφ
ε)×U(ε,φ),
and then Ccol≈Ccol
a=/uniontext
i∈{1,...,s}aSlcol
U(ε,φ i).
Given an object Oφand an ε> 0, we compute the corresponding ε-core Oφ
ε.
We then compute an approximation of the collision space for Oφ
ε. Recall that the
set of obstacles is represented as a set of balls S={BR1(X1) ,..., BRn(Xn)}and
1By distance here we mean Euclidean distance in Rd.
594 A. Varava et al.
Fig. 3 Two approximations of the collision space corresponding to the non-disjoint orientation
intervals U1andU2are depicted in grey. The free space is depicted in white. The arrows between
different connected components indicate the possible transitions between slices
O={Or1(Y1),... , Orm(Ym)}.T h eε-core can be represented as a set of balls with
centres at the same points and smaller radii: Oφ
ε={Or1−ε(Y1),... , Orm−ε(Ym)}.
However, to understand the connectivity of the free space it is not enough to
consider different orientation intervals separately. Consider an example, see Fig. 3.
Here we have two slices of the free space corresponding to two orientation in-
tervals U1∩U2/negationslash=∅. Both slices have 3 connected components: Q1,Q2,Q3, and
Q/prime
1,Q/prime
2,Q/prime
3respectively. The object can move between the connected components
Qiand Q/prime
jfrom different slices if (i)the corresponding orientation intervals have a
non-empty intersection and (ii) Q iand Qjhave non-empty intersection in Rd.
Therefore, the object is contained within a bounded connected component of
the free space if (i)for the corresponding slice SlUof the conﬁguration space it is
contained within a bounded connected component of Cfre e, and (ii)there is no path
between slices leading the object to an unbounded connected component of Cfre e.
4 Methodology
In this section, we describe the technical details of our approach. We start with the
derivation of the possible slice size depending on the size of the chosen ε-core of the
object.
4.1 The Object Core and the Size of the Slices
Consider an ε-core of the object in a given orientation for a ﬁxed ε, and let us ﬁx its
orientation φ∈SO(d). We now want to explicitly ﬁnd an open set U(ε,φ) ⊂SO(d)
such that for any θ∈U(ε,φ) theε-core Oφ
εis fully contained within any Oθ.W e
represent a motion in SO(d)as a rotation matrix R. First, we make the following
observation:
Caging and Path Non-existence: A Deterministic Sampling-Based … 595
Observation 1 Consider an object Oφand an ε-core Oφ
εinside it. Let us ﬁx the
orientation φ∈SO(d)ofOφ
ε, and apply a rotation R to Oφ. Then Oφ
εis fully
contained inside the rotated object ORφif the displacement ||Ry−y||2of a point y
after applying the rotation is smaller than εfor any point y ∈Oφ.
The proof is trivial, as by construction any point of Oφ
εis separated at least at a
distance εfrom the boundary of Oφ. Thus, if Oφ
εis not lying inside of ORφ, then
there exists a point y∈Oφsuch that the distance from it to its image RyinORφis
not smaller than ε,||Ry−y||2≥ε.
Note that in fact the form of U(ε,φ) does not depend on φ, and U(ε,Rφ)can be
obtained by rotating the former: U(ε,Rφ)=RU(ε,φ) for any φ∈SO(d)and any
rotation R.
We now want to estimate the maximum displacement ||Ry−y||2of a point
y∈Rdafter applying a rotation R.L e t Ibe the identity matrix. We call R−I
a displacement matrix associated to the rotation R. To estimate ||(R−I)y||2,w e
compute the spectral norm of the displacement matrix:
||R−I||2= sup
y∈R3,||y||2/negationslash=0||(R−I)y||2
||y||2,
which gives us an upper bound for the displacement:
∀y∈Oφ:| |R−I||2max
x∈Oφ(||x||2)≥| |(R−I)y||2
4.1.1 2D Workspace
Consider a two-dimensional workspace. In this case, the conﬁguration space (and
hence the collision space) of the object is a three-dimensional subset of SE(2)=
R2×SO(2), where each conﬁguration is described by a position of the object x∈R2
and its orientation φ∈SO(2).
In this case rotation matrix can be written as
Rθ=/bracketleftbiggcos(θ)−sin(θ)
sin(θ) cos(θ)/bracketrightbigg
,
Recall that the spectral norm of R−Iis deﬁned as the square root of the maximum
eigenvalue of (R−I)T(R−I).
This matrix has a single eigenvalue of algebraic multiplicity two:
λ=(cos(θ)−1)2+sin2(θ)=2−2 cos(θ)=4s i n2(θ/2),
which gives us an upper bound for the maximum displacement of yafter applying a
rotation Rθ:
||Rθy−y||2≤2|sin(θ/2)|·| |y||2
596 A. Varava et al.
4.1.2 3D Workspace
We represent motions in SO(3)as a composition of the pitch, yaw and roll rotation
matrices:
Rφ=⎡
⎣cos(φ) 0s i n(φ)
01 0
−sin(φ)0c o s(φ)⎤
⎦,Rσ=⎡
⎣cos(σ)−sin(σ)0
sin(σ) cos(σ) 0
00 1⎤
⎦,Rθ=⎡
⎣10 0
0c o s(θ)−sin(θ)
0s i n(θ) cos(θ)⎤
⎦
As before, we want to estimate the displacement incurred by applying a given
rotation. Consider a ﬁxed triplet (θ,σ,φ) we deﬁne R(θ,σ,φ) =Rθ·Rσ·Rφ, and
once again compute the norm of the displacement matrix as the square root of the
maximum eigenvalue of (R(θ,σ,φ)−I)T(R(θ,σ,φ)−I).
Direct computations show that this matrix has a single non-zero eigenvalue of
algebraic multiplicity two:
λ=3−cos(σ)cos(φ)−cos(θ)cos(φ)−cos(σ)cos(θ)−sin(σ)sin(φ)sin(θ)
We can simplify this expression by rewriting the products of trigonometric func-
tions as their sums and get:
λ=3−cos(σ−φ)
2−cos(σ+φ)
2−cos(σ−θ)
2−cos(σ+θ)
2−cos(θ−φ)
2
−cos(θ+φ)
2−sin(σ+θ−φ)
4−sin(σ−θ+φ)
4+sin(σ+θ+φ)
4+sin(σ−θ−φ)
4
For simplicity, instead of treating our parameters θ,σ, and φseparately, we
consider those triples which lie inside of a ball Br(0)of radius r>0.
Now for a ﬁxed r>0 we observe that for any (θ,σ,φ) ∈Br(0), each of the
arguments σ±θ,σ±φ,θ±φ,σ±φ±θis contained in [−r,r].
This means that for any rin the interval [−π/2,π/2]we have:
cos(σ+θ),cos(σ−θ),cos(σ+p),cos(σ−φ),cos(θ+φ),cos(θ−φ)≥cos(r)
sin(σ+φ+θ),sin(σ−θ−φ)≥sin(−r)=− sin(r)
sin(σ+θ−φ),sin(σ−θ+φ)≥− sin(r),
And hence
λ≤3−3 cos(r)+sin(r).
So we get
||R(θ,σ,φ)−I||2≤/radicalbig
3−3 cos(r)+sin(r)∀(θ,σ,φ) ∈Br(0),
and the maximum displacement of yafter applying a rotation R(θ,σ,φ) can be estimated
as follows:
Caging and Path Non-existence: A Deterministic Sampling-Based … 597
Fig. 4 Graph of the
maximum displacementestimate for a ball of radius r
||(R(θ,σ,φ)−I)y||2≤/radicalbig
3−3 cos(r)+sin(r)·| |y||2
Figure 4illustrates how the displacement grows with the radius r.
4.2 Collision Space Reconstruction
Let us now discuss how we construct the collision space for each slice. Recall that both
the object and the obstacles are represented as unions of balls, S=/uniontext
i∈{1,...,n}BRi(Xi)
and O=/uniontext
i∈{1,...,m}Ori(Yi)of radii R1,..., Rnandr1,..., rmrespectively. Let the
frame of the object be centred in its centroid G,GY idenote the position vector of
the center of each ball, i∈{1..m}, and T−GY idenote a translation by a vector −GY i.
The object represented as a union of balls collides with the obstacles if at least
one of the balls is in collision, so the collision space of the object is a union of the
collision spaces of the balls shifted with respect to the position of the balls centers:
Ccol(Oφ
ε)=/uniondisplay
i∈{1..m}T−GY i(Ccol(Ori−ε))
Note also that a ball collides with the obstacles when the distance from the ob-
stacles to its center is not greater than the radius of the ball, so the collision space of
a single ball of radius ri−εcan be expressed as follows:
598 A. Varava et al.
Ccol(Ori−ε)={x∈Rd|d(x,S)≤ri−ε}=/uniondisplay
j∈{1..n}BRj+ri−ε(Xj)
So, the collision space of the ε-core can be written as
Ccol(Oφ
ε)=/uniondisplay
i∈{1..m},j∈{1..n}T−GY i(BRj+ri−ε(Xj))
Algorithm 1: Construct-Slice
Data :A nε-core Oφ
ε={Or1−ε(Y1),... , Orm−ε(Ym)}, a set of obstacles
S={BR1(X1),... , BRn(Xn)}
Result : A union of balls representing the collision space Ccol(Oφ
ε)
1G←Centroid( O)
2Ccol(Oφ
ε)←∅
3foreach i∈{1..m},j∈{1..n}do
4 GY i←Yi−G
5 Ccol(Oφ
ε)←Ccol(Oφ
ε)∪T−GY i(BRj+ri−ε(Xj))
6end
7return Ccol(Oφ
ε)
The construction of the collision space of the ε-core of the object is straightfor-
ward, see Algorithm 1. Recall now that for a certain U(ε,φ) the collision space
Ccol(Oφ
ε)is contained within Ccol(Oθ)for each θ∈U(ε,φ) , which gives us an
approximation of the slice of CcolasCcol(Oφ
ε)×U(ε,φ) .
4.3 Gluing the Slices T ogether
Once we have computed the collision space for each slice, we proceed by ﬁnding the
connected components of the free space of each of them. As a result, for each slice
we want to get a d−dimensional simplicial complex approximating its free space.
For that, we use the approach proposed by McCarthy et al. in [ 10]. Since Ccol(Oφ
ε)
is a union of balls, it is convenient to represent it as an alpha complex—a discrete
representation of the space. Alpha complexes are simplicial complexes which are
particularly convenient for approximating unions of balls, as by the nerve theoremthey preserve the homotopy type (and hence the necessary topological properties) of
the corresponding union of balls. They have been used to approximate conﬁguration
spaces, for instance, in [ 7,10]. We refer the reader to [ 4] for a complete and self-
contained treatment of alpha complexes and the theory behind them. To approximate
the free space, we compute a regular triangulation, which is a convex superset of the
computed alpha complex, see [ 3]. We mark tetrahedra in the regular triangulation as
“free” or “in collision”, and compute a set of connected components of the free space.
We then mark the connected components as “bounded” and “unbounded”. Note that
since both the object and the set of obstacles are bounded, the collision space is
Caging and Path Non-existence: A Deterministic Sampling-Based … 599
Algorithm 2: Glue-Slices
Data : A set of slices Ccol
a={aSlcol
U1,..., aSlcol
Us}; their connected components
{{C1
1,..., C1
n1},...,{Cs
1,..., Cs
ns}}
Result : A connectivity graph G
1G←∅
2AddVertices( G,{C1
1,..., C1
n1,..., Cs
1,..., Cs
ns}
3foreach aSlcol
Ui∈Ccol
ado
4 foreach aSlcol
Uj∈Neighbours(aSlcol
Ui)do
5 foreach Ci
k∈Components(aSlcol
Ui)do
6 foreach Cj
l∈Components(aSlcol
Uj)do
7 ifCi
k∩Cj
l/negationslash=∅ then
8 AddEdge( G,(Ci
k,Cj
l)
9 end
10 end
11 end
12 end
13end
14return G
also bounded, and therefore the free space always has one unbounded connected
component. We represent it by artiﬁcially adding an “inﬁnite” tetrahedron.
Now when we have an approximation of the collision space as a union of slices
Ccol
a=/uniondisplay
Ui,i∈{1,...,s}aSlcol
Ui
and an alpha complex representing each slice aSlcol
U, we can study the relationship
between slices. More precisely, we would like to build a connectivity graph G, where
the vertices represent the connected components {Ci
1,..., Ci
ni}of each slice Uifor
i∈{1,..s}, see Algorithm 2.
Two vertices representing components Cp⊂aSlcol
UiandCq⊂aSlcol
Uj,i/negationslash=j,a r e
connected by an edge if a direct transition between them is possible. That is, if
(i)the sets Uiand Ujoverlap, Ui∩Uj/negationslash=∅, and (ii)the respective projections of
the connected components to Rdintersect: Pr Rd(Cq)∩Pr Rd(Cp)/negationslash=∅, where the
projection operator is deﬁned as Pr Rd(x,φ)=x. So, for each slice aSlcol
Ui, we consider
its neighbours—i.e., slices aSlcol
Ujsuch that Ui∩Uj/negationslash=∅, and check whether the
connected components of the free space of these pairs of slices (represented as d-
dimensional simplicial complexes) overlap.
Once the connectivity graph is constructed, we can easily check if there is no path
between two given conﬁgurations, and if a given conﬁguration is caged. We run a
depth-ﬁrst search in Gto compute its connected components, and then given two
conﬁgurations csandcgwe simply check if the corresponding vertices of Gbelong
to different connected components. If that is the case, there is no path between cs
andcg.
600 A. Varava et al.
Algorithm 3: Query-Connectivity
Data : Two conﬁgurations, ciandcj; a set of slices
Ccol
a={aSlcol
U1,..., aSlcol
Us}; their connected components
{{C1
1,..., C1
n1},...,{Cs
1,..., Cs
ns}}; the connectivity graph G
Result : Boolean, path existence
1aSl i←Slice-Containing-Conﬁguration( ci,{aSlcol
U1,..., aSlcol
Us})
2aSl j←Slice-Containing-Conﬁguration( cj,{aSlcol
U1,..., aSlcol
Us})
3Ci←Component-Containing-Conﬁguration( ci,aSl i,{Ci
1,..., Ci
n1})
4Cj←Component-Containing-Conﬁguration( cj,aSl j,{Cj
1,..., Cj
n1})
5ifAre-Connected(C i,C j,G)then
6 return Undecided
7end
8else
9 return False
10end
For caging, we need to check whether we can reach an unbounded connected
component starting from a given conﬁguration, see Algorithm 4. For each connected
component of each of the slices, we know if it is bounded or not, each vertex of Ghas
a corresponding mark. To prove caging, we check if the vertex of Gcorresponding
to a certain conﬁguration cis connected to an “unbounded” vertex.
Algorithm 4: Check-Caging
Data : A conﬁguration ci;as e to fs l i c e s Ccol
a={aSlcol
U1,..., aSlcol
Us}; their
connected components {{C1
1,..., C1
n1},...,{Cs
1,..., Cs
ns}};t h e
connectivity graph G
Result : Boolean, the conﬁguration is caged
1aSl i←Slice-Containing-Conﬁguration( ci,{aSlcol
U1,..., aSlcol
Us})
2Ci←Component-Containing-Conﬁguration( ci,aSl i,{Ci
1,..., Ci
n1})
3ifConnected-To-Unbounded(C, G)then
4 return Undecided
5end
6else
7 return True
8end
5 Paths Construction
In this section, we discuss how our approximation of the conﬁguration space can be
used for path planning. Modern path planning algorithms, such as PRM, RRT, and
their modiﬁcations, are guaranteed to ﬁnd a path if one exists. However, they cannotprove that two conﬁgurations are disconnected. On the contrary, our approach is not
Caging and Path Non-existence: A Deterministic Sampling-Based … 601
Fig. 5 The arcs (pi−1,pi)and(pi,pi+1)of the projection πare completely contained within a
ballωof radius ε/2
guaranteed to ﬁnd a path if one exists, as our approximation of the collision space is
just a subset of the actual collision space. However, we can prove path non-existence.
Thus, path planning algorithms and our method can be considered as complementary
techniques and can be run in parallel to solve path planning problem more efﬁciently.
Furthermore, our approximation of the collision space can be used to construct path
candidates which are not guaranteed to be collision-free, but can be used as initialapproximations and to be further improved using some path optimization technique,
such as CHOMP [ 15]. This is especially useful in cluttered environments, when
we need to ﬁnd a path through a narrow passage. Our approach can be seen as acontinuation of the pioneering work by Basch et al., [ 2].
Since our approximation of the collision space is not complete, we are limited
to a certain class of paths which we can construct. We call them decomposable
paths :—collision-free paths that can be decomposed into ﬁnite sequence of alter-
nating motions in SO(d)(pure rotations) and motions in R
d(pure translations).
Formally, we deﬁne it as follows:
Deﬁnition 3 A path γ:[0,1]→ SO(d)is decomposable if there exists a family of
pathsγ1,γ/prime
1,...,γ n,γ/prime
nsuch that Pr Rd(Im(γi))and Pr SO d(Im(γ/prime
i))are constant paths
inRdand SO(d)respectively,2andγ=γ1∗γ/prime
1∗..∗γn∗γ/prime
n, where by γi∗γjwe
mean a composition of paths γiandγj.
However, this class is nevertheless useful as we can show that if there exists some
collision-free path in the free space, our approximation of the conﬁguration spaceallows us to construct a decomposable approximation. Namely, instead of considering
the original object, we construct a decomposable path for its ε-core, and we show
that we can always do that providing there is a collision-free path (Fig. 5):
Lemma 1 If two conﬁgurations c and c
/primeof the original object Oare connected, then
there is a decomposable path between the respective conﬁgurations of its ε-core Oε.
Proof. If there exists a decomposable path between cand c/primefor the original ob-
ject O, then the same path is valid for Oε⊂O. Otherwise, let us pick any non-
decomposable path γbetween cand c/prime. Note that along this path Oεis always
at least at a distance εfrom the obstacles. Let us now explicitly decompose
2Here PrRd(.)and Pr SO(d)(.)denote the projections to Rdand SO(d)respectively, and Im (γ)
denotes the image of γinSO(d).
602 A. Varava et al.
γby choosing waypoints c=(p0,r0),...,( pn,rn)=c/primefromγand construct-
ingγ/prime={(p0,r0),(p1,r0),(p1,r1) ,...,( pn−1,rn−1),(pn,rn−1),(pn,rn)}, where
piare the coordinates of the Rdcomponent of SE(d)andriare the coordinates of
theSO(d)component. The resulting decomposition will consist of alternating pure
translation and pure rotation paths sending the point (pi,ri)to(pi+1,ri)through a
translation-only segment and then to (pi+1,ri+1)through a rotation-only segment.
Letπ=Pr Rd(γ)be the projection of γonto Rd. We choose the next point pi+1as
the second endpoint of the longest arc (pi,y)of the path πwhich is fully contained
within an ε/2-ball ωcentered at pi(or as the Rd-component of c/prime, in case the part of
πconnecting it to pilies inside ω), and make sure that pi+1does not coincide with
pi−1.
Let us show that a pure translation of Oεfrom(pi,ri)to(pi+1,ri)is collision-
free. Since πis a projection of a collision-free path, Oin conﬁguration (pi,ri)does
not collide with obstacles. Then any point of Oεin(pi,ri)is separated at least at a
distance εfrom the obstacles. Pure translation to (pi+1,ri)will move each point of
Oεat distance ε/2<ε, so it will not collide with the obstacles.
Now consider a pure rotation of Oεfrom(pi+1,ri)to(pi+1,ri+1). Assume that
there is some r/prime∈[ri,ri+1]such that Oεin conﬁguration (pi+1,r/prime)collides with
obstacles. Given the continuity of the path between (pi,ri)and(pi+1,ri+1), there is
some conﬁguration (p/prime,r/prime)such that p/prime∈[pi,pi+1]. Since pi+1is chosen in a way
that the arc (pi,pi+1)lies inside ω,p/primealso lies inside ω. Then the distance between
p/primeand pi+1is less than ε. Therefore, since (p/prime,r/prime)is collision-free for the original
object O,(pi+1,r/prime)is collision-free for Oε, which contradicts our assumption.
Finally, we note that using Lemma 1we can extend Algorithm 3 to provide an
approximate path between two conﬁgurations (p,r)and(p/prime,r/prime)if one exists. To
do this, we can have the algorithm output a sequence of pairs (C1,S1),... ,( Cn,Sn)
which comprise the path in the connectivity graph G, where the Ciare the components
and the Si. To produce a path we start from the conﬁguration (p,r)∈C1×S1and
use a local planner to ﬁnd a path to some point (p2,r)∈(C1∩C2)×S1and then
we rotate this conﬁguration to (p2,r2)where r2∈S2. We proceed in this manner
until we reach (pn,rn)∈Cn×Sn, since by construction (p/prime,r/prime)∈Cn×Sn, we can
use a local planner to ﬁnd a path from (pn,rn)to(p/prime,rn)and ﬁnally rotate the ﬁnal
conﬁguration to (p/prime,r/prime).
6 Discussion and Possible Extensions
In this paper, we propose an approach towards proving caging and path non-existence
for rigid objects in 2D and 3D workspaces. We compute an approximation of the
collision space of the object, represent it as a collection of lower dimensional pro-
jections, and analyze the connectivity of the free space of the object. We also discusshow our approach can be generalized to arbitrary conﬁguration spaces.
Caging and Path Non-existence: A Deterministic Sampling-Based … 603
Our approach can be potentially extended to a broader class of conﬁguration
spaces, for example, in order to prove path non-existence for an articulated robot.
Indeed, as long as the conﬁguration space can be represented as a product C=
C1×C2, where C1⊂Rdand C2is a compact set, we can construct a ﬁnite open
cover C2=/uniontext
Ui⊂C2Ui, and for each slice Sli=C1×Uiconstruct collision space
approximations using an ε-core of the robot.
References
1. Barraquand, J., Kavraki, L., Latombe, J.-C., Motwani, R., Li, T.-Y ., Raghavan, P.: A random
sampling scheme for path planning. Int. J. Robot. Res. 16(6), 759–774 (1997)
2. Basch, J., Guibas, L.J., Hsu, D., Nguyen, A.T.: Disconnection proofs for motion planning. In:
IEEE International Conference on Robotics and Automation, pp. 1765–1772 (2001)
3. Edelsbrunner, H.: Weighted alpha shapes. University of Illinois at Urbana-Champaign, Depart-
ment of Computer Science, Champaign, IL, USA (1992)
4. Edelsbrunner, H., Harer, J.: Computational topology: an introduction. American Mathematical
Society (2010)
5. Kuperberg, W.: Problems on polytopes and convex sets. In: DIMACS Workshop on Polytopes,
pp. 584–589 (1990)
6. Latombe, J.-C.: Robot Motion Planning. Kluwer Academic Publishers, Norwell, MA, USA
(1991)
7. Mahler, J., Pokorny, F.T., McCarthy, Z., van der Stappen, A.F., Goldberg, K.: Energy-bounded
caging: formal deﬁnition and 2-D energy lower bound algorithm based on weighted alphashapes. IEEE Robot. Autom. Lett. 1(1), 508–515 (2016)
8. Makita, S., Maeda, Y .: 3D multiﬁngered caging: basic formulation and planning. In: IEEE
Intelligent Robots and Systems, pp. 2697–2702 (2008)
9. Makita, S., Okita, K., Maeda, Y .: 3D two-ﬁngered caging for two types of objects: sufﬁcient
conditions and planning. Int. J. Mechatron. Autom. 3(4), 263–277 (2013)
10. McCarthy, Z., Bretl, T., Hutchinson, S.: Proving path non-existence using sampling and alpha
shapes. In: IEEE International Conference on Robotics and Automation, pp. 2563–2569 (2012)
11. Pereira, G.A.S., Campos, M.F.M., Kumar, V .: Decentralized algorithms for multi-robot manip-
ulation via caging. Int. J. Robot. Res. 23(7–8), 783–795 (2004)
12. Pipattanasomporn, P., Sudsang, A.: Two-ﬁnger caging of concave polygon. In: IEEE Interna-
tional Conference on Robotics and Automation, pp. 2137–2142 (2006)
13. Pipattanasomporn, P., Sudsang, A.: Two-ﬁnger caging of nonconvex polytopes. IEEE Trans.
Robot. 27(2), 324–333 (2011)
14. Pokorny, F.T., Stork, J.A., Kragic, D.: Grasping objects with holes: a topological approach. In:
IEEE International Conference on Robotics and Automation, pp. 1100–1107 (2013)
15. Ratliff, N., Zucker, M., Bagnell, J.A., Srinivasa, S.: CHOMP: gradient optimization techniques
for efﬁcient motion planning. In: IEEE International Conference on Robotics and Automation,pp. 489–494 (2009)
16. Rimon, E., Blake, A.: Caging planar bodies by one-parameter two-ﬁngered gripping systems.
Int. J. Robot. Res. 18(3), 299–318 (1999)
17. Rodriguez, A., Mason, M.T., Ferry, S.: From caging to grasping. Int. J. Robot. Res. 31(7),
886–900 (2012)
18. Stork, J.A., Pokorny, F.T., Kragic, D.: A topology-based object representation for clasping,
latching and hooking. In: IEEE-RAS International Conference on Humanoid Robots, pp. 138–145 (2013)
19. Stork, J.A., Pokorny, F.T., Kragic, D.: Integrated motion and clasp planning with virtual linking.
In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3007–3014(2013)
604 A. Varava et al.
20. Varava, A., Kragic, D., Pokorny, F.T.: Caging grasps of rigid and partially deformable 3-D
objects with double fork and neck features. IEEE Trans. Robot. 32(6), 1479–1497 (2016)
21. Varava, A., Carvalho, J.F., Kragic, D., Pokorny, F.T.: A decomposition-based approach to
reasoning about free space path-connectivity for rigid objects in 2D (2017). arXiv:1710.10089
[cs.RO]
22. Vahedi, M., van der Stappen, A.F.: Caging polygons with two and three ﬁngers. Int. J. Robot.
Res. 27(11–12), 1308–1324 (2008)
23. Zhang, L., Young, J.K., Manocha, D.: Efﬁcient cell labelling and path non-existence compu-
tation using C-obstacle query. Int. J. Robot. Res. 27(11–12), 1246–1257 (2008)
Pushing Revisited: Differential Flatness,
Trajectory Planning and Stabilization
Jiaji Zhou and Matthew T. Mason
1 Introduction
Robots can increase dexterity via harnessing task mechanics including inertia force,
gravity and external contacts. Pushing is an example of using the arm motion andsupporting surface to change the planar pose of objects through sliding. The quasi-
static pusher-slider system is a canonical hybrid system with model uncertainty due
to indeterminate and stochastic friction distribution. In this paper, we mainly focus
on pushing with rolling/sticking contact where the object is constrained to a two
dimensional space embedded in SE(2). The nonholonomic constraint appears to bechallenging but is fortunately not. Intuitively, the applied force through the contact
point is bounded inside the friction cone and hence the turning rate of the object
must be bounded, indicating similarity to the steering car system with bounded frontwheel turning angle.
We use differential ﬂatness techniques from nonlinear control theory [ 18], which
offers some advantage for trajectory generation and control of underactuated me-chanical systems. A particular interesting system is the kinematic steering car whose
ﬂat output is the center of the rear axle. The problem is well-studied: the time-optimal
motion planning solution is given by Dubins curve [ 7] and globally stable controller
synthesized with dynamic feedback linearization [ 20]. In this paper, we show that the
pusher-slider system with single sticking contact is differential ﬂat, which opens new
avenue for trajectory planning and stabilization. We ﬁrst give an intuitive graphicalanalysis and continue with algebraic derivation.
J. Zhou (B)·M. T. Mason
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USAe-mail: jiajiz@cs.cmu.edu
M. T. Mason
e-mail: matt.mason@cs.cmu.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_44605
606 J. Zhou and M. T. Mason
The literature has addressed dealing with inherent model uncertainty in sliding
manipulation, often in the form of worst-case guarantee. The ﬁrst result is the V oting
Theorem [ 17] that dictates the sense of rotation given a single contact knowing only
the center of pressure without the exact distribution. Peshkin and Sanderson [ 21]
give further bounds on the possible instantaneous motion. Goyal et al. [ 10] noted
that all the possible static and sliding frictional wrenches form a convex set whoseboundary is called as the limit surface. Howe and Cutkosky [ 12] proposed using
ellipsoid approximation of the limit surface for a given pressure distribution. Zhou et
al. [25,26] proposed a framework of representing limit surfaces using homogeneous
even-degree convex polynomials. A purely data-driven method is presented in [ 2].
The use of multiple contacts can be open-loop stable. Lynch and Mason [ 15]g i v e
results on controllability and stability for open loop edge pushing. A discretizedsearch-based planner is also given in [ 15]. The multiple constraints imposed by an
edge can reduce uncertainty. Examples include push-grasps [ 3,6], squeezing [ 8,26]
and parts feeding [ 24].
Lynch et al. [ 16] achieves stable translation through a round ﬁnger with only
tactile sensing that gives the contact point position and normal feedback. Hogan and
Rodriguez [ 11] recently proposed using online hybrid model predictive control for
tracking a pushing trajectory. The contact modes are treated as integer variables.
To avoid excessive combinatorial mode enumeration, prescribed phases of sequencewith a single mode are used. Trajectory optimization and local feedback control
synthesis through rigid body contacts with complementarity constraints are studied
in [22,23].
2 Background on Quasi-static Pushing
We describe the single point pusher quasi-static motion model assuming rigid body
mechanics with Coulomb friction. The following notations are used:
•q: the object pose ( x,y,θ) in the world frame W.
•p: the contact point ( px,py) in the local frame O.
•np: the inward normal in the local frame at p.
•f: the applied force by the pusher.
•fl,fr: the left and right edges of the friction cone.
•u: the pushing velocity of the pusher in the object local frame O.
•ul,ur: the left and right edges of the motion cone.
•F,V: the applied wrench and resultant twist in the object local frame.
The force-motion model for quasi-static pushing given certain pressure distribu-
tion can be efﬁciently established through limit surface representation [ 9,12,25].
Points inside the surface correspond to static friction wrenches. Points on the surface
correspond to friction wrenches with normals parallel to sliding twist directions,forming a mapping between friction wrench and sliding twist.
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 607
Using a homogeneous even-degree convex polynomial H(F)representation for
the limit surface [ 25], the resultant object twist Vfollows the same direction as the
gradient evaluated at the applied wrench F:
V=k∇H(F), k>0, (1)
A global diagonal ellipsoid approximation [ 11,16] is often adopted as a convenient
representation by existing literature on model-based pushing, i.e., assuming H(F)=
FTAF, where A=⎡
⎣a00
0a0
00b⎤
⎦. In this paper, we ﬁnd the closest vector of form [a,a,b]
to the diagonal vector of ∇2H(Fnp)as a local approximation, where Fnpis the normal
contact wrench at the contact point p, i.e., the wrench applied as if the contact is
frictionless.
a=(∇2H(Fnp)11+∇2H(Fnp)22)/2( 2 )
b=∇2H(Fnp)33 (3)
Choose the positive yaxis of the local body frame to align with the vector pointing
from the contact point to the center of mass O. In doing so, px=0. After absorbing
the scalar kinto f, we have that in local frame
Vx=afx (4)
Vy=afy (5)
Vθ=bτ=− bpyfx. (6)
The quasi-static motion equations in global frame are written as:
˙x=a(fxcosθ−fysinθ) (7)
˙y=a(fxsinθ+fycosθ) (8)
˙θ=− bpyfx, (9)
and the friction cone constraint is given by
/bardblfx/bardbl≤μfy (10)
fy≥0 (11)
Denote by Jp=/bracketleftbigg10−py
01 px/bracketrightbigg
the Jacobian matrix at pandR(θ)=⎡
⎣cosθ−sinθ0
sinθcosθ0
00 1⎤
⎦,
Eqs. 7–9is equivalent to:
608 J. Zhou and M. T. Mason
Fig. 1 A rectangle pushed by a round ﬁnger. Blue arrows correspond to the friction cone K(fl,fr)
edges and red arrows correspond to the motion cone K(ul,ur)edges. The instantaneous clockwise
center of rotation (COR) is marked as a circle with negative sign. The contact sticks since thepushing direction uis inside the motion cone
˙q=R(θ)AJT
pf (12)
Note that the magnitude of fdoes not have physical meaning and is proportional to
the magnitude of the input velocity. Since the pusher is position controlled, we will
need to relate the applied force to pushing velocity. It can be shown [ 25] that for
sticking contact the applied force fand pushing velocity uare linearly related, and
have a one-to-one mapping:
u=JpAJT
pf. (13)
Further, the friction cone constraint is translated into motion cone K(ul,ur)where
the left edge uland right edge urof the cone are given by:
ul=JpAJT
pfl, ur=JpAJT
pfr. (14)
Ifuis within the motion cone, then the contact sticks as shown in Fig. 1. Left sliding
occurs if uis to the left of uland right sliding occurs if uis to the right of ur.T h e
planner and controller described in the rest of the paper assumes the control input is
applied force, which will be converted to pusher velocity using Eq. 13.
3 Differential Flatness
The rough idea of differential ﬂatness is to ﬁnd ﬂat output states (of the same di-
mension as control input) as a function of the original states and control inputs. Such
mapping also admits an inverse function such that the original states and control
inputs can be recovered from the ﬂat output states and their high order derivativeswithout any integration step.
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 609
3.1 Graphical Derivation
From Eqs. 4–6, an applied body wrench F=[ fx,fy,τ]is mapped to a body twist V.
A twist in a plane can be further represented as a center of rotation (Vx/Vθ,Vy/Vθ)
with the same sign as Vθ).L e t r=|py|be the distance from the line of force to the
local frame origin. The distance ˜rfrom the center of rotation to the origin is inverse
proportion to r:
˜r=/radicalBigg
V2x+V2y
V2
θ=a
br. (15)
If we deﬁne the unit length as a/b, then the center of rotation lies on the opposite
side along the perpendicular line to the line of force through the contact point, withdistance to the origin equals 1 /r. As in projective geometry, the dual of a line is a
point. Here, the line of applied force is mapped to the resultant center of rotation
point.
Proposition 1 The instantaneous rotation centers corresponding to applied fric-
tional forces through the contact point form a line perpendicular to the vector from
the origin to the contact point. The distance from the line to the origin equals
a
br,
where r is the distance from the contact point to the origin.
This is similar to the force-dual method [ 4] that maps a line of force to the
acceleration center. The matrix Acan be treated as a damping matrix that connects
force to velocity, analogous to the inertia matrix in [ 4] that maps force to acceleration.
In Fig. 2a, the friction cone is symmetric with respect to the origin as the contact
point’s normal passes through the origin. The friction cone constraints are represented
using the force dual graphical method. Denote by zlandzrthe instantaneous rotation
centers given applied forces on the left edge fland the right edge frof the friction
cone respectively. The allowable motion of zcan be characterized by a ray (blue) of
counter-clockwise rotation center starting from zror a ray (magenta) of clockwise
rotation center starting from zl. Figure 2b illustrates the general asymmetric case.
Choose the positive y axis to be aligned with the vector pointing from the contactpoint to the center of mass, the trajectory of the pushed object can be exactly recovered
from the trajectory of such point (ﬂat output).
Theorem 1 Any point on the line of center of rotations is a differential ﬂat output.
Proof Section 3.2provides an algebraic proof. We also give a geometrical proof
sketch here.
1. Since the line of center of rotation is perpendicular to the positive y axis, the tan-
gents along the trajectory point in the directions of body positive y axis (heading)
of the object.
2. After knowing the orientation of the body frame, we can compute the position of
the body frame since the point is ﬁxed in the body frame.
610 J. Zhou and M. T. Mason
Y
XO
Y
XWfl frzlzrz
(a) S ymmetric case.Y
XO
Y
XWflfrzr
zlz
(b) As ymmetric case.
Fig. 2 Graphical analysis
3. The instantaneous center of rotation can be further determined from the curvature
along the trajectory. Therefore the velocity control input is also known.
A key observation is that if we choose the mid point (in red color) between zlandzr,
then the instantaneous motion constraints from the sticking contact are simply mini-
mum turning radius constraints. We now have a reduction to a Dubins car model [ 7,
13] where the heading aligns with the positive body y axis and the mid point (red)
ofzlandzris the center of rear axle.
3.2 Algebraic Derivation
This section derives the function mappings between cartesian pose and control to
ﬂat outputs.
3.2.1 Symmetric Case
The symmetric case as shown in Fig. 2a is when the pushing point’s normal aligns
with the vector pointing from the point pto the center of mass O. Hence we have
px=0 and r=− py. cosθ×(7)+sinθ×(8) and−sinθ×(7)+cosθ×(8) yield
fx=1
a(˙xcosθ+˙ysinθ) (16)
fy=1
a(−˙xsinθ+˙ycosθ). (17)
Together with Eq. ( 9) yields
−a
br˙θ+˙xcosθ+˙ysinθ=0. (18)
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 611
A choice of ﬂat outputs are given by
z1=x−a
brsinθ (19)
z2=y+a
brcosθ, (20)
whose derivative are
˙z1=˙x−a
brcosθ˙θ (21)
˙z2=˙y−a
brsinθ˙θ (22)
Rewrite Eq. ( 18)u s i n g z,˙zwe get
˙z1cosθ+˙z2sinθ=0. (23)
Therefore,
θ=arctan/parenleftbigg−˙z1
˙z2/parenrightbigg
(24)
x=z1−a˙z1
br/radicalBig
˙z2
1+˙z2
2(25)
y=z2−a˙z2
br/radicalBig
˙z2
1+˙z2
2(26)
fx=˙z1¨z2−˙z2¨z1
br(˙z2
1+˙z2
2)(27)
fy=/radicalBig
˙z2
1+˙z2
2
a(28)
The friction cone constraints represented in ﬂat output space can be written as
|˙z1¨z2−˙z2¨z1
(˙z2
1+˙z2
2)3
2|≤brμ
a(29)
Note that constraints ( 29) is exactly the curvature of the trajectory of z(t). We can
now conclude that pushing with sticking constraint is equivalent to ﬁnding curves
with bounded curvature that connects two 2D oriented points. In particular, Dubinscurve [ 7] is the time-optimal solution.
Figure 3demonstrate two examples of trajectory planning with a single point
sticking contact. The friction cone constraint is converted to minimum turning radiusconstraint. Dubins curve is generated in ﬂat output space. Then the SE(2) poses of
612 J. Zhou and M. T. Mason
-0.15 -0.1 -0.05 0-0.1-0.050
-0.15 -0.1 -0.05 0-0.0500.05
Fig. 3 Example planned trajectories with the initial pose in black and the ﬁnal pose in blue
the object and control actions are mapped back to the cartesian space as given in
Eqs. 24and 25–28.
3.2.2 General Case
We also derive the general case when the contact point’s normal is not aligned with
the vector as shown in Fig. 2b. Let the local frame origin be the COM and the positive
y-axis aligned with the vector pointing from the contact point pto center of mass
O. We show that with this choice of reference frame, any point on the dual line of
the friction cone is differential ﬂat, and conveniently we can simply choose the mid
point between the two extreme center of rotations that correspond to left and right
edges of the friction cone. Denote by cxthexcomponent along the line of center of
rotations, and ρ=a
b. The ﬂat outputs are given by
/bracketleftbiggz1
z2/bracketrightbigg
=/bracketleftbiggx
y/bracketrightbigg
+/bracketleftbiggcosθ−sinθ
sinθcosθ/bracketrightbigg/bracketleftbiggc
ρ/r/bracketrightbigg
, (30)
where the vector/bracketleftbiggc
ρ/r/bracketrightbigg
represents a point on the line of center of rotations (dual line
of friction cone) in body frame.
Following similar steps in Sect. 3.2.1 , we can map the ﬂat outputs to cartesian
pose and applied force:
θ=arctan/parenleftbigg−˙z1
˙z2/parenrightbigg
(31)
/bracketleftbiggx
y/bracketrightbigg
=/bracketleftbiggz1
z2/bracketrightbigg
−1/radicalBig
˙z2
1+˙z2
2/bracketleftbigg˙z2˙z1
−˙z1˙z2/bracketrightbigg/bracketleftbiggc
ρ/r/bracketrightbigg
(32)
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 613
/bracketleftbiggfx
fy/bracketrightbigg
=1
˙z2
1+˙z2
2⎡
⎣˙z1¨z2−¨z1˙z2
br
(˙z2
1+˙z2
2)3
2−c(˙z2
1+˙z2
2)
a⎤
⎦. (33)
Further, let cland crbe the xcomponents of the center of rotations corresponding
tofland fr.I fw es e t c=(cl+cr)/2, then the friction cone constraint is turned into
a curvature upper bound of 2 /|cl−cr|(or minimum turning radius of |cl−cr|/2.)
We refer the readers to the appendix for detailed derivation.
4 Stabilization
Uncertainty for robotic pushing mainly come from two sources: (1) perception uncer-
tainty for the initial object pose and (2) model uncertainty due to changing friction
distribution. Single contact pushing cannot be open-loop stable and needs activefeedback control strategy. Section 4.1derives a linear tracking controller in ﬂat out-
put space through dynamic feedback linearization. Section 4.2addresses improving
robustness against model uncertainty through open-loop stable two-points push thatnaturally induces mechanical feedback.
4.1 Dynamic F eedback Linearization Control
Equation 12is in the form of driftless underactuated system with three degrees of
freedom state and two degrees of freedom control input:
˙q=G(q)f. (34)
For such systems, dynamic feedback linearization ﬁnds a feedback compensator of
the form:
˙ζ=α(q,ζ)+β(q,ζ) w (35)
f=γ(q,ζ)+δ(q,ζ) w, (36)
where the kth derivative of ﬂat output zcan be directly controllable via w
zk=w. (37)
Differentiating the ﬂat output zwith respect to time yields
˙z=/bracketleftbigg0−asinθ
0acosθ/bracketrightbigg/bracketleftbiggfx
fy/bracketrightbigg
. (38)
614 J. Zhou and M. T. Mason
We need to keep taking derivative since only fyaffects ˙z. Denote by ζ=fythe
dynamic feedback compensator and s=˙ζ.
¨z=a/bracketleftbigg−sinθ−ζbrcosθ
cosθ−ζbrsinθ/bracketrightbigg/bracketleftbiggs
fx/bracketrightbigg
(39)
Let
/bracketleftbiggs
fx/bracketrightbigg
=/parenleftbigg
a/bracketleftbigg−sinθ−cosθ
cosθ−sinθ/bracketrightbigg/bracketleftbigg10
0ζbr/bracketrightbigg/parenrightbigg−1/bracketleftbiggw1
w2/bracketrightbigg
(40)
=/bracketleftbigg−sinθ/a cosθ/a
−cosθ/(aζbr)−sinθ/(aζbr)/bracketrightbigg/bracketleftbiggw1
w2/bracketrightbigg
, (41)
which leads to
¨z=/bracketleftbiggw1
w2/bracketrightbigg
=w (42)
The dynamic feedback compensator is of the following form:
˙ζ=− w1sinθ/a+w2cosθ/a (43)
fy=ζ (44)
fx=− w1cosθ/(aζbr)−w2sinθ/(aζbr) (45)
We can therefore design a simple PD controller to track a planned trajectory zd(t).
w=¨zd−kp(z−zd)−kd(˙z−˙zd). (46)
This PD controller is globally exponentially stable assuming the model does not
change. A robustness analysis for a changing model is beyond the scope of this
paper. The manipulator velocity control input can be further determined via Eqs. 13
and 43–45.
A simulation experiment using a high-ﬁdelity simulator [ 25] is shown in Fig. 4b.
The initial state is perturbed by −1m m i n x,−2.5 mm in yand 3.6◦inθ.T h e
system model parameter Ain Eq. 12is perturbed from⎡
⎣1.0537 0 0
01.0537 0
00 1 .5087⎤
⎦to
⎡⎣1.0719 −0.0177−0.1782
−0.0177 1 .0417 0 .1599
−0.1782 0 .1599 1 .5104⎤
⎦.
The gain for position error term is kp=[2.0,0.5]and
the gain for velocity error term is kv=[0.1,0.05]. The controller runs at 60 Hz for
30 s and the ﬁnal pose error is 0.0034 mm in x, 0.0012 mm in yand 2.55◦inθ.
The ABB robot is currently not suitable for closed-loop control due to low position
control input frequency. In the future, we will conduct robotic experiments withrecently released externally guided motion package [ 1].
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 615
-0.05 0 0.05-0.04-0.0200.020.040.060.08
(a) Planned reference trajectory.-0.05 0 0.05-0.04-0.0200.020.040.060.08
(b) Trajectory tracking with dynamic feed-
back linearization.
Fig. 4 Example trajectory stabilization with initial pose and system model parameter perturbations
4.2 Open Loop Stabilization with Kinematic Constraints
Lynch and Mason [ 15] showed that a two-points push action against an edge of the
object can be stable such that the object will remain attached to the pusher without
slipping or breaking contact, despite the presence of uncertainty.
This can be seen as natural mechanical feedback that tolerates model uncertainty.
The object will follow a body twist motion Vas long as the corresponding frictional
wrench Fis inside the composite wrench cone Fc=K(F1
c,F2
c)formed by the two
wrench cones F1
c,F2
cat the contact points, i.e., ∃Fsuch that ∇H(F)=Vand F∈Fc.
The span of the composite wrench cone provides redundancy to balance uncertain
frictional wrench between the object and the supporting surface.
Throughout our experiments, we use the mid point of the two points as a virtual
contact point and the average normal as the contact normal to plan reference trajec-
tory. Perception uncertainty is not addressed for this form of mechanical feedback
although a sequence of designed open-loop translational pushes can reduce the initial
perception uncertainty [ 3].
5 Experiments
5.1 Pushing Using Multiple Actions
A pushing point in the body frame deﬁnes an action associated with a Dubins car
reduction. If we allow switching between multiple pushing points (actions), the object
can be moved faster to the goal state. It is also natural to specify a switching cost
between actions. This section presents a simple planner that gives the near-optimal
path for a given query initial pose to the goal pose at the origin. We ﬁrst construct a
graph using the following steps:
616 J. Zhou and M. T. Mason
Fig. 5 Experimental setup
1. Sample SE(2) poses within the boundary as graph nodes.
2. For each node, split into kcopies tagged with action id, where kequals the number
of actions.
3. Run Dijkstra’s algorithm [ 5] and add switching cost to the edge weight if the two
nodes are tagged with different action ids.
The graph is organized as a tree structure whose paths to the goal are the shortest
path subject to the sampling resolution. Then for any new query pose, we treat it as
a new node to connect to the goal through either direct connection to the goal using
one action or paths in the graph using multiple actions.
The experimental setup is shown in Fig. 5. We use the ABB-120 robot mounted
with a two-points pusher. The object bottoms are attached with AprilTags [ 19]. The
supporting surface is a transparent acrylic table with a camera underneath to acquirethe initial and ﬁnal poses. We use four objects with different pressure distributions,
material and shapes. Trajectories are generated using the mid point of the two points
as a single point pusher and executed open-loop. Each object is given three or fourpushing points (actions). The triangular object has actions of asymmetric push point.
Three different initial locations that require difﬁcult maneuvers are chosen for each
object with the same target location such that the local frame exactly aligns withtable frame at the center. Each initial condition is executed ﬁve times. Trajectories
generated from experimental logs are shown in Figs. 6,7,8and 9. The object initial
poses (in sequence) for each action are ﬁlled with red, purple and blue colors. The
ﬁnal pose is ﬁlled with black color. The average error is within 1.67 mm in translation
and 0.5
◦in orientation over the 60 experiments.1
1All 60 runs videos are available at https://www.dropbox.com/sh/2t6cwqwv3w95iji/
AABLHdlnRhSQzHKhcmg2zOT4a?dl=0 .
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 617
Fig. 6 Rectangle with three-point pressure. The average error (mm, mm, degree) with 95%
conﬁdence interval from left to right are [0.03±0.02,−3.19±0.29,0.53±0.11],[0.50±
0.13,−0.96±0.6,−0.48±0.61],[−0.23±0.11,−4.17±0.87,−1.29±0.5]
Fig. 7 Rectangle with boundary pressure. The average error (mm, mm, degree) with 95%
conﬁdence interval from left to right are [−0.26±0.12,−3.31±0.75,−0.46±0.19],[0.42±
0.12,−1.49±1.63,−0.14±0.27],[−0.27±0.21,−4.56±0.43,−0.93±0.76]
Fig. 8 The butterﬂy object with boundary pressure. The average error (mm, mm, degree) with 95%
conﬁdence interval from left to right are [−0.69±0.17,−1.46±0.06,4.40±1.24],[−0.65±
0.17,−1.38±0.07,5.89±2.37],[−0.96±0.08,−0.09±0.73,0.83±1.00]
Fig. 9 Triangle with uniform pressure. The ﬁnal error (mm, mm, degree) with 95% conﬁ-
dence interval are [0.64±0.05,1.04±0.63,0.11±0.31],[0.11±0.65,−0.50±0.30,−0.42±
0.44],[2.34±0.23,0.12±0.06,−1.06±0.42].
618 J. Zhou and M. T. Mason
Fig. 10 Planned pushing actions among obstacles using RRT with exact steering
5.2 Pushing Among Obstacles
The proposed reduction to Dubins curve beneﬁts randomized motion planners since
the two point boundary value problem can be solved exactly via the reduction, i.e.,
the steering is exact. We use a RRT [ 14] planner to generate a collision free pushing
path shown in Figs. 10and 11. The triangular object and the two-points pusher are
not allowed to touch the red obstacle nor the blue boundary of the map. The goal is
to align the center of the triangle with the red point in an up-right orientation.
6 Conclusion and Future Work
This paper studies the geometrical structure for the pusher-slider system with onerolling/sticking constraint: there exists body-ﬁxed point whose trajectory completely
determines the system behavior. Planning and control in this differential ﬂat (carefully
chosen reduced minimal coordinates) space is signiﬁcantly easier. In the future, weplan to conduct robotic experiments for the dynamic feedback linearization control
with single point contact in a receding horizon fashion since unexpected slip may
Pushing Revisited: Differential Flatness, Trajectory Planning and Stabilization 619
Fig. 11 Snapshots of the robot executing the plan
occur and the control needs to reveal the change of contact positions. Extension to
three-dimensional space where out-of-plane moments cannot be neglected remains
as a challenging problem.
Acknowledgements The authors would like to thank Guofan Wu, Devin Balkcom, Kevin Lynch
and Sanjiban Choudhury for thoughtful discussions.
References
1. ABB.: Abb externally guided motion (egm). http://new.abb.com/products/ABB.PARTS.
SEROP3HAC054376-001 (2017). Accessed 14 June 2017
2. Bauza, M., Rodriguez, A.: A probabilistic data-driven model for planar pushing (2017).
arXiv:1704.03033
3. Brost, R.C.: Automatic grasp planning in the presence of uncertainty. Int. J. Robot. Res. 7(1),
3–17 (1988)
4. Brost, R.C., Mason, M.T.: Graphical analysis of planar rigid-body dynamics with multiple
frictional contacts. In: The Fifth International Symposium on Robotics Research, pp. 293–300(1991). MIT Press
5. Dijkstra, E.W.: A note on two problems in connexion with graphs. Numerische mathematik
1(1), 269–271 (1959)
6. Dogar, M.R., Srinivasa, S.S.: Push-grasping with dexterous hands: mechanics and a method.
In: 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.2123–2130. IEEE (2010)
7. Dubins, L.E.: On curves of minimal length with a constraint on average curvature, and with
prescribed initial and terminal positions and tangents. Am. J. Math. 79(3), 497–516 (1957)
8. Goldberg, K., Mason, M.T.: Bayesian grasping. In: IEEE International Conference on Robotics
and Automation (ICRA), pp. 1264–1269 (1990)
9. Goyal, S.: Planar Sliding of a Rigid Body With Dry Friction: Limit Surfaces and Dynamics of
Motion. Ph.D. thesis, Cornell University, Department of Mechanical Engineering (1989)
10. Goyal, S., Ruina, A., Papadopoulos, J.: Planar sliding with dry friction. Part 1. Limit surface
and moment function. Wear 143, 307–330 (1991)
11. Hogan, F.R., Rodriguez, A.: Feedback control of the pusher-slider system: a story of hybrid
and underactuated contact dynamics (2016). arXiv:1611.08268
620 J. Zhou and M. T. Mason
12. Howe, R.D., Cutkosky, M.R.: Practical force-motion models for sliding manipulation. Int. J.
Robot. Res. 15(6), 557–572 (1996)
13. LaV alle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)14. LaV alle, S.M., Kuffner, J.J.Jr.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5),
378–400 (2001)
15. Lynch. K.M., Mason, M.T.: Stable pushing: mechanics, controllability, and planning. Int. J.
Robot. Res. 15(6), 533–556 (1996)
16. Lynch, K.M. Maekawa, H., Tanie, K.: Manipulation and active sensing by pushing using tactile
feedback. In: Proceedings of the 1992 lEEE/RSJ International Conference on Intelligent Robotsand Systems, 1992, vol. 1, pp. 416–421. IEEE (1992)
17. Mason, M.T.: Mechanics and planning of manipulator pushing operations. Int. J. Robot. Res.
5(3), 53–71 (1986)
18. Murray, R.M., Rathinam, M., Sluis, W.: Differential ﬂatness of mechanical control systems: a
catalog of prototype systems. In: ASME International Mechanical Engineering Congress andExposition (1995)
19. Olson, E.: Apriltag: a robust and ﬂexible visual ﬁducial system. In: 2011 IEEE International
Conference on Robotics and Automation (ICRA), pp. 3400–3407. IEEE (2011)
20. Oriolo, G., De Luca, A., V endittelli, M.: Wmr control via dynamic feedback linearization:
design, implementation, and experimental validation. IEEE Trans. Control Syst. Technol. 10(6),
835–852 (2002)
21. Peshkin, M.A., Sanderson, A.C.: Planning robotic manipulation strategies for workpieces that
slide. IEEE J. Robot. Autom. 4(5), 524–531 (1988)
22. Posa, M., Cantu, C., Tedrake, R.: A direct method for trajectory optimization of rigid bodies
through contact. Int. J. Robot. Res. 33(1), 69–81 (2014)
23. Posa, M., Kuindersma, S., Tedrake, R.: Optimization and stabilization of trajectories for con-
strained dynamical systems. In: 2016 IEEE International Conference on Robotics and Automa-tion (ICRA), pp. 1366–1373. IEEE (2016)
24. Wiegley, J., Goldberg, K., Peshkin, M., Brokowski, M.: A complete algorithm for design-
ing passive fences to orient parts. In: 1996 IEEE International Conference on Robotics andAutomation, 1996. Proceedings, vol. 2, pp. 1133–1139. IEEE (1996)
25. Zhou, J., Paolini, R., Bagnell, J.A., Mason, M.T.: A convex polynomial force-motion model
for planar sliding: identiﬁcation and application. In: 2016 IEEE International Conference onRobotics and Automation (ICRA), pp. 372–377 (2016)
26. Zhou, J., Paolini, R., Johnson, A., Bagnell, J.A., Mason, M.T.: A probabilistic planning frame-
work for planar grasping under uncertainty. In: 2017 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS) (2017)
Perception
Conﬁdence-Rich Grid Mapping
Ali-akbar Agha-mohammadi, Eric Heiden, Karol Hausman
and Gaurav Sukhatme
1 Introduction
Consider a quadrotor equipped with a forward-facing stereo camera ﬂying in anobstacle-laden environment tasked to reach a goal. In order to ensure the safety of thesystem and avoid collisions, the robot needs to create a representation of obstacles,
which we refer to as the map, and incorporate it in the planning framework. Due
to the noise caused by using imperfect sensors and models, the robot requires a
probabilistic representation of the map that is able to capture the uncertainty of the
environment. In order to plan trajectories using such a probabilistic map, the plannerneeds to be aware of not only the occupancy value estimates but also of how much
these values can be trusted. In particular, this trust (or conﬁdence) is important when
considering sensors with high noise, such as stereo cameras with small baselines.This paper presents an algorithm that creates a map which, in addition to the most
likely occupancy values, encodes the conﬁdence (trust) associated with these values.
Grid-based structures are among the most common representations of the envi-
ronment when dealing with range sensors [ 25]. Typically, each grid voxel contains
binary information indicating whether the voxel is free or occupied. In a slightly
richer format, each voxel contains the probability of being occupied. In the main
A. Agha-mohammadi ( B)
Jet Propulsion Laboratory, Caltech, Pasadena, CA, USAe-mail: aliagha@jpl.nasa.gov
E. Heiden ·G. Sukhatme
University of Southern California, Los Angeles, CA, USAe-mail: heiden@usc.edu
G. Sukhatme
e-mail: gaurav@usc.edu
K. Hausman
Google Brain, Mountain View, CA, USAe-mail: hausmankarol@gmail.com
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_45623
624 A. Agha-mohammadi et al.
body of literature, occupancy grids are used to store binary occupancies updated by
the log-odds method [ 21,22], which is discussed in detail in Sect. 3. Even though the
log-odds-based occupancy grids have enjoyed success in a variety of applications,these methods, especially when coping with noisy sensors such as sonar and stereo
cameras, suffer from three main issues:
(A) The occupancy of each voxel is updated independently of the rest of the map.
This is a well-known problem [ 21] which has been shown to lead to conﬂicts
between the map and measurement data. In particular, when the sensor is noisy
or has a large ﬁeld of view, there is a clear coupling between voxels that fall into
the ﬁeld of view of the sensor.
(B) The log-odds methods rely on the “inverse sensor model” (ISM), which needs
to be hand-engineered for each sensor and a given environment.
(C) In order to represent the voxel occupancy, each voxel stores a single number.
As a result, there is no consistent conﬁdence or trust value to help the plannerreason about the reliability of the estimated occupancy.
In this paper, we propose a method that partially relaxes these assumptions, gen-
erates more accurate maps, and provided a more consistent ﬁltering mechanism than
prior approaches. The highlights and contributions of this work are as follows:
1. The main assumption in traditional occupancy grid mapping is (partially) relaxed.
We take into account the interdependence between voxels within the same mea-
surement cone at every step. Further, the proposed method relaxes the binary
assumption on the occupancy level and is able to cope with maps where eachvoxel is only partially occupied by obstacles.
2. We replace the ad-hoc inverse sensor model by a novel “sensor cause model”
which is computed based on the forward sensor model in a principled manner.
3. In addition to the most likely occupancy value for each voxel, the proposed map
representation contains conﬁdence values (e.g. variance) of voxel occupancies.
The conﬁdence information is crucial for planning over grid maps. We incorporate
the sensor model and its uncertainties in characterizing the map accuracy.
4. While the majority of approaches that relax the voxel-independence assumption
are batch methods, our method does not require logging of the data in an ofﬂine
phase. Instead, the map can be updated online as the sensory data are received.
In our experiments, our method achieves an improvement over traditional
approaches of up to 30% in absolute error and up to two orders of magnitudes better
variance consistency, according to the proposed inconsistency measure. We believe
that the map representation introduced here and the probabilistic map-update algo-
rithm provide a signiﬁcant step towards uncertainty-aware safe planning, a crucial
component to enabling fast navigation in uncertain environments.
Conﬁdence-Rich Grid Mapping 625
2 Related Work
The ﬁrst application of occupancy grids in robotics dates back to Moravec [ 11] and
Elfes [ 2] and has since been widely used in robotics. Thrun et al. [ 21], Stachniss
[18], and Thrun et al. [ 22] discuss many variants of these methods.
Grid-based maps have been constructed using a variety of ranging sensors, that
include stereo-cameras [ 9], sonars [ 26], laser range ﬁnders [ 19], and fusion of
thereof [ 11]. Their structure has been extended to achieve more memory-efﬁcient
maps [ 25]. There have also been methods that extend grid-based mapping to store
richer forms of data, including the distance to obstacle surfaces [ 13], reﬂective prop-
erties of environment [ 4], and color/texture [ 12]. The main method used to updating
the occupancy values of the voxels was presented by Thrun et al. [ 22] and is based on
the log-odds approach. We provide a detailed description of this method in Sect. 3.
Another class of mapping methods that have shown great success is the class of Gaus-
sian Process-based mapping methods (e.g., [ 6,7,14,15,17,24]). These methods
do not rely on voxel grids but model the occupied spaces continuously. Furthermore,
they take into account the spatial correlations between occupancy of different regions
of the map.
Different researchers have studied the drawbacks of the log-odds approach in
occupancy grids and proposed methods to alleviate them [ 3,8,10,16,20,23].
All these methods attempt to mitigate the negative effects caused by the incorrectvoxel-independence assumption in mapping. In particular, Thrun [ 20] proposes a
grid-mapping method using forward sensor models, which takes into account all
voxel dependencies and generates maps of higher quality compared to maps resultingfrom ISM. However, this method requires the measurement data to be collected
ofﬂine. Then, it runs the expectation-maximization (EM) algorithm on the full data
to compute the most likely map. Hähnel et al. [ 3] extend the grid-based mapping
methods to dynamic environments using a similar sensor model to the one used
in this paper. However, this method assumes accurate measurements (e.g. coming
from a laser range ﬁnder). It also uses EM to compute the map, which limits the
result to the most likely values and does not provide any conﬁdence measure on
the reported values. In this paper, we propose a mapping method that is online andcan cope with high-noise range measurements by incorporating the noise into the
model. More importantly, the proposed method computes a conﬁdence value for the
estimate which can be very beneﬁcial for planning purposes.
3 Occupancy Grid Mapping Using Inverse Sensor Models
Most occupancy grid mapping methods decompose the full mapping problem intomany binary estimation problems on individual voxels assuming full independence
between voxels. This assumption leads to inconsistencies in the resulting map. In
this section, we discuss the method and these assumptions.
626 A. Agha-mohammadi et al.
Let G=[ G1,..., Gn]be an n-voxel grid overlaid on the 3D (or 2D) environment,
where Gi∈R3is a 3D point representing the center of the ith voxel of the grid in the
global coordinate frame. An occupancy map m=[ m1,..., mn]is deﬁned as a set
of values over this grid. We start with a more general deﬁnition of occupancy where
mi∈[0,1]denotes what percentage of a voxel is occupied. mi=1 when the ith
voxel is fully occupied and mi=0 when it is free. For maps where occupancy can
only be 0 or 1, we use the notationbinmifor the occupancy of voxel ito explicitly
show that the occupancy is binary, i.e.,binmi∈{0,1}.
The full mapping problem is deﬁned as estimating map mbased on obtained
measurements and robot poses. We denote the sensor measurement at the kth time
step by zkand the sensor conﬁguration at the kth time step by xk. As we will discuss
further in Sect. 5,xkcharacterizes the ﬁeld of view (e.g., a pixel cone or a measurement
ray when dealing with a single pixel sensor). Formulating the problem in a Bayesian
framework, we compress the information obtained from past measurements z0:k=
{z0,..., zk}and x0:k={ x0,..., xk}to create a probability distribution (belief) ¯bm
k
on the map m.
¯bm
k=p(m|z0:k,x0:k) (1)
However, due to challenges in storing and updating such a high-dimensional
belief, grid mapping methods start from individual cells (marginal distributions).
Assumption 1 (Collection of marginals ) The map pdf is represented by the collec-
tion of individual voxel pdfs (marginal pdfs), instead of the full joint pdf.
bm
k≡(bmi
k)n
i=1, bmi
k=p(mi|z0:k,x0:k) (2)
where ndenotes the number of voxels in the map.
To compute the marginal bmiin a recursive manner, the method starts with applying
Bayes’ rule.
bmi
k=p(mi|z0:k,x0:k)=p(zk|mi,z0:k−1,x0:k)p(mi|z0:k−1,x0:k)
p(zk|z0:k−1,x0:k)(3)
The main incorrect assumption is applied here:
Assumption 2 ((Incorrect) Measurement independence ) Standard approaches
assume that a single voxel is sufﬁcient (independent of other voxels) to characterize
the measurement. Mathematically,
p(zk|mi,z0:k−1,x0:k)≈p(zk|mi,xk) (4)
Remark 1 Note that Assumption 2(Eq. 4) would be precise if conditioning was over
the whole map. In other words,
Conﬁdence-Rich Grid Mapping 627
p(zk|m,z0:k−1,x0:k)=p(zk|m,xk) (5)
is correct. But, when conditioning on a single voxel, this approximation could be
very inaccurate, because a single voxel miis not enough to generate the likelihood
of observation z. For example, there might even be a wall between miand the sensor,
and clearly mialone cannot tell what range will be measured by the sensor in that
case.
When dealing with noisy sensors (such as stereo cameras) or with noise-free
sensors with large measurement cones (such as sonar), this assumption leads to map
conﬂicts and estimation inconsistencies.
Inverse sensor model : Following Assumption 2, one can apply Bayes’ rule to
Eq.4
p(zk|mi,xk)=p(mi|zk,xk)p(zk|xk)
p(mi|xk)(6)
which gives rise to the concept of the inverse sensor model (ISM), i.e., p(mi|zk,xk).
The inverse sensor model describes the occupancy probability given a single mea-
surement. The model cannot be derived from a sensor model. However, depending
on the application and the utilized sensor, ad-hoc models can be hand-engineered.
The reason to create this model is that it leads to an elegant mapping scheme on
binary maps as follows.
Plugging ( 4) and ( 6)i n t o( 3), we obtain:
bmi
k=p(mi|z0:k,x0:k)=p(mi|zk,xk)p(zk|xk)p(mi|z0:k−1,x0:k)
p(mi|xk)p(zk|z0:k−1,x0:k)(7)
Given that the robot’s motion does not affect the map:
bmi
k=p(mi|z0:k,x0:k)=p(mi|zk,xk)p(zk|xk)p(mi|z0:k−1,x0:k−1)
p(mi)p(zk|z0:k−1,x0:k)(8)
Assumption 3 (Binary occupancy ) To complete the recursion, traditional grid-
based methods further assume that the occupancy of voxels are binary,
i.e.,binmi∈{0,1}. Thus, p(binmi=1)=1−p(binmi=0).
According to Assumption 3, one can deﬁne odds ri
kof occupancy and compute it
using Eq.( 8):
ri
k:=p(binmi=1|z0:k,x0:k)
p(binmi=0|z0:k,x0:k)=p(binmi=1|zk,xk)p(binmi=0)
p(binmi=0|zk,xk)p(binmi=1)ri
k−1 (9)
Remark 2 Relying on Assumption 3and using odds, removes the terms which are
difﬁcult to compute from the recursion in Eq. ( 8).
628 A. Agha-mohammadi et al.
Fig. 1 Typical inverse
sensor model for a rangesensor. It returns the
occupancy probability for
voxels on the measurementray/cone based on theirdistance to the camera
Further, denoting log-odds as li
k=log ri
k, we can simplify the recursion as:
li
k=li
k−1+li
ISM−li
pri or (10)
where, li
ISM=log(p(binmi=1|zk,xk)p(binmi=0|zk,xk)−1)are the log-odds of
ISM at voxel i, and li
pri or=log(p(binmi=1)p(binmi=0)−1)are the log-odds of
prior. ISM is often hand-engineered for a given sensor/environment. Figure 1shows
the typical form of the ISM function.
4 Conﬁdence-Rich Representation
In our map representation we store the probability distribution of miin each voxel
i. V ariable miin this paper can be interpreted in two ways:
1. In the more general setting, mi∈[0,1]directly represents the occupancy level
(the fraction of voxel ithat is occupied by obstacles.). The proposed method can
model continuous occupancy and Assumption 3in traditional occupancy mapping
can be relaxed.
2. If the underlying true map is assumed to be a binary map (denoted bybinm), the
occupancy of the ith voxelbinmi∈{0,1}is distributed as Bernoulli distribution
binmi∼Bernoulli (mi). In other words, in this case mirefers to the parameter
of the Bernoulli distribution. While ISM-based mapping methods store mias a
deterministic value, we estimate miprobabilistically based on measurements and
store its pdf at each voxel. Note that in this setting,binmi∈{0,1}, where mi∈
[0,1]represents the occupancy probability, i.e., mi
k=p(binmi=1|z0:k,x0:k).
Problem description : Given the above-mentioned representation, we aim at
estimating mbased on noisy measurements by computing its posterior distribution.
We deﬁne three beliefs over the map: (1) full map belief ¯bm
k=p(m|z0:k,x0:k),( 2 )
marginal cell beliefs bmi
k=p(mi|z0:k,x0:k), and (3) the collection of marginals bm
k=
Conﬁdence-Rich Grid Mapping 629
(bmi
k)M
i=1.1Similar to ISM-based methods, for mapping we maintain and update the
collection of marginals bm
k. To do so, we derive the following items:
1.Ranging sensor model : Given that the obstacles are described by a stochastic
map, we derive a ranging sensor model, i.e., the probability of obtaining mea-
surement zgiven a stochastic map and robot location: p(zk|xk,bm
k). This model
will be used in the map update module.
2.Recursive density mapping : We derive a recursive mapping scheme τthat
updates the current density map based on the last measurements
bmi
k+1=τmi(bm
k,zk+1,xk+1). (11)
The fundamental difference with ISM-mapping is that the evolution of the ith
voxel depends on other voxels as well. Note that the input argument to τmiis the
collection of all voxel beliefs bm, not just the ith voxel belief bmi.
3. While planning is beyond the scope of this paper, we brieﬂy discuss how planning
can beneﬁt from this enriched map data and consistent estimation mechanism to
generate actions that actively reduce mapping uncertainty and lead to safer paths.
Overall, this method relaxes Assumptions 2and 3of the ISM-based mapping.
5 Range-Sensor Modeling
In this section, we model a range sensor when the environment representation is a
stochastic map. We focus on passive ranging sensors like stereo cameras, but the
discussion easily translates to active sensors as well.
Ranging pixel : Let us consider an array of ranging sensors (e.g., disparity pixels).
We denote the camera center by xcam, the 3D location of the ith pixel by v, and the
ray emanating from xcamand passing through vbyx=(xcam,v).L e t rdenote the
distance between the camera center and the closest obstacle to the camera along ray
x. For a stereo camera, the range ris related to the measured disparity zas:
z=r−1fd b (12)
where fis the camera’s focal length and dbis the baseline between two cameras on
the stereo rig. In the following, we focus on a single pixel vand derive the forward
sensor model p(z|x,bm).
Pixel cone : Consider the ﬁeld-of-view of pixel v. More precisely, it is a narrow
3D cone with apex at xand boundaries deﬁned by pixel v. Also, for simplicity, one
can consider just a ray xgoing through camera center xand the center of pixel v.
1More precisely, in these deﬁnitions the variable brefers to the set of parameters that characterize
the probability distributions. So, we will treat bas a vector (deterministic or random depending on
the context) in the rest of the paper.
630 A. Agha-mohammadi et al.
Fig. 2 The cone formed by
two red lines depicts theﬁeld-of-view of pixel v.T h e
disparity measurement on
pixel vcan be caused by
light bouncing off any voxel(e.g., the red one) in the pixelcone and reaching the imageplane
The pixel cone Cone(x)refers to the set of voxels in map mthat fall into this cone
(or lie on ray x). We denote this set by C=Cone(x).
Local versus global indices : For a given ray x, we order the voxels along the ray
from closest to farthest from the camera. Notation-wise, il∈{1,...,/bardblC/bardbl}represents
the local index of a voxel on ray x. Function ig=g(il,x)returns the global index
igof this voxel in the map.
Cause variables : The disparity measurement on pixel vcould be the result of
light bouncing off any of voxels in the cone C=Cone(x)(see Fig. 2). Therefore,
any of these voxels are a potential cause for a given measurement. In the case that the
environment map is perfectly known, one can pinpoint the exact cause by ﬁnding theclosest obstacle to the camera center. But, when the knowledge about the environment
is partial and probabilistic, the best one can deduce about causes is a probability
distribution over all possible causes in the pixel cone C=Cone(x). These causes
will play an important role (as hidden variables) in deriving the sensor model for
stochastic maps.
Cause probability : To derive the full sensor model, we need to reason about
which voxel was the cause for a given measurement. For a voxel c∈C(x)to be the
cause, two events need to happen: (i) B
c, which indicates the event of light bouncing
off voxel cand (ii) Rc, which indicates the event of light reaching the camera from
voxel c.
p(c|bm)=Pr(Bc,Rc|bm)=Pr(Rc|Bc,bm)Pr(Bc|bm) (13)
Bouncing probability : To compute the bouncing probability, we rely on the fact
that Pr (Bc|mc)=mc(by the deﬁnition). Note that Pr (Bc|mc,bm)=Pr(Bc|mc).
Pr(Bc|bm)=/integraldisplay1
0Pr(Bc|mc,bm)p(mc|bm)dmc=/integraldisplay1
0mcbmcdmc=Emc=/hatwidemc
Conﬁdence-Rich Grid Mapping 631
Reaching probability : For the ray emanating from voxel cto reach the image
plane, it has to go through all voxels on ray xbetween cand the sensor. Let cldenotes
local index of voxel calong the ray x, i.e., cl=g−1(c,x), then we have:
Pr(Rc|Bc,bm)=(1−Pr(Bg(cl−1,x)|bm))Pr(Rg(cl−1,x)|Bg(cl−1,x),bm) (14)
=cl−1/productdisplay
l=1(1−Pr(Bg(l,x)|bm))=cl−1/productdisplay
l=1(1−/hatwidemg(l,x))
Sensor model with known cause : Assuming the cause voxel for measurement
zis known, the forward sensor is typically modeled as:
z=h(x,c,nz)=/bardbl Gc−xcam/bardbl−1fd b+nz, (15)
where, nz∼N(0,V)denotes the observation noise, drawn from a zero-mean Gaus-
sian with variance V. We can alternatively describe the observation model in terms
of pdfs as follows:
p(z|x,c)=N(/bardblGc−xcam/bardbl−1fd b,V) (16)
Sensor model with stochastic maps : The sensor model given a stochastic map
can be computed by incorporating hidden cause variables into the formulation:
p(z|x,bm)=/summationdisplay
c∈C(x)p(z|x,c;bm)p(c|bm) (17)
=/summationdisplay
c∈C(x)N(/bardblGc−xcam/bardbl−1fd b,V)/hatwidemccl−1/productdisplay
l=1(1−/hatwidemg(l,x))
6 Conﬁdence-Augmented Grid Map
In this section, we derive the recursive mapping algorithm described in Eq. ( 11). This
mapping algorithm τcan not only reason about the occupancy at each cell but also
about the conﬁdence level of this value. As a result, it enables efﬁcient prediction of
the map that can be embedded in planning, resulting in safer plans.
To compute the belief of the ith voxel, denoted by bmi
k=p(mi|z0:k,x0:k),w eb r i n g
the cause variables into the formulation.
632 A. Agha-mohammadi et al.
bmi
k=p(mi|z0:k,x0:k)=/summationdisplay
ck∈C(x)p(mi|ck,z0:k,x0:k)p(ck|z0:k,x0:k) (18)
=/summationdisplay
ck∈C(x)p(mi|ck,z0:k−1,x0:k)p(ck|z0:k,x0:k)
=/summationdisplay
ck∈C(x)p(ck|mi,z0:k−1,x0:k)
p(ck|z0:k−1,x0:k)p(ck|z0:k,x0:k)bmi
k−1
It can be shown that bm
k−1is a sufﬁcient statistic [ 5] for the data (z0:k−1,x0:k−1)in
the above terms. Thus, we can rewrite ( 18)a s :
bmi
k=/summationdisplay
ck∈C(x)p(ck|mi,bm
k−1,xk)
p(ck|bm
k−1,xk)p(ck|bm
k−1,zk,xk)bmi
k−1 (19)
In the following, we make the assumption that the map pdf is sufﬁcient for com-
puting the bouncing probability from voxel c(i.e., one can ignore voxel igiven the
rest of the map.) Mathematically, for ck/negationslash=i, we assume:
Pr(Bck|mi,bm
k−1,xk)/approxorequalPr(Bck|bm
k−1,xk)=/hatwidemck
Note that we still preserve a strong dependence between voxels via the reaching
probability. To see this clearly, we expand the numerator p(ck|mi,bm
k−1,xk)in (19)
as (we drop xto unclutter the equations):
p(ck|mi,bm
k−1)=Pr(Bck,Rck|mi,bm
k−1)=Pr(Bck|mi,bm
k−1)Pr(Rck|Bck,mi,bm
k−1)
=⎧
⎪⎪⎪⎨
⎪⎪⎪⎩/hatwidemck/producttextcl
k−1
l=1(1−/hatwidemg(l,x)) ifcl
kil(20)
The denominator is p(ck|bm
k−1,xk)=/hatwidemck/producttext cl
k−1
l=1(1−/hatwidemg(l,x))for all ck∈C(x).I n
these equations, cl
k=g−1(ck,xk)and il=g−1(i,xk)are the corresponding indices
ofckand iin the local frame. Therefore, the ratio in ( 19) is simpliﬁed to:
p(ck|mi,bm
k−1,xk)
p(ck|bm
k−1,xk)=I(cl
kil)
where I(A)is one if event Ais true, and is zero otherwise. Plugging the ratio back
into the ( 19), and collecting linear and constant terms, we can show that:
p(mi|z0:k,x0:k)=(αimi+βi)p(mi|z0:k−1,x0:k−1) (21)
Conﬁdence-Rich Grid Mapping 633
where
αi=(/hatwidemi)−1p(ck|bm
k−1,zk,xk)−(1−/hatwidemi)−1|C(x)|/summationdisplay
cl
k=il+1p(ck|bm
k−1,zk,xk) (22)
βi=il−1/summationdisplay
cl
k=1p(ck|bm
k−1,zk,xk)+(1−/hatwidemi)−1|C(x)|/summationdisplay
cl
k=il+1p(ck|bm
k−1,zk,xk) (23)
In a more compact form, we can rewrite Eq. ( 21)a s :
bmi
k+1=τi(bm
k,zk+1,xk+1). (24)
Sensor cause model : The proposed machinery gives rise to the term
p(ck|z0:k,x0:k)=p(ck|bm
k−1,zk,xk), which is referred to as “Sensor Cause Model
(SCM)” in this paper. As opposed to the inverse sensor model in traditional mapping
that needs to be hand-engineered, the SCM can be derived from the forward sensor
model in a principled way as follows. η/primeis the normalization constant.
p(ck|z0:k,x0:k)=p(ck|bm
k−1,zk,xk)=p(zk|ck,xk)p(ck|bm
k−1,xk)
p(zk|bm
k−1,xk)(25)
=η/primep(zk|ck,xk)p(ck|bm
k−1,xk)=η/primep(zk|ck,xk)/hatwidemck
k−1cl
k−1/productdisplay
j=1(1−/hatwidemg(j,x)
k−1),∀ck∈C(xk)
The complete mapping algorithm is recapped in Algorithm 1. Using a KD-tree for
storing the map, the complexity of the proposed algorithm is O(|Vray|·log(|V|)·
|Vray|)=O(|Vray|2·log(|V|)), where Vand Vrayis the set of voxels in the map and
the set of voxels on the ray. The complexity of the standard log-odds approach with
KD-trees is O(|Vray|·log(|V|)). For details on these results, see [ 1].
Algorithm 1: Conﬁdence-rich grid mapping
input : Current map belief bk, observation zk, measurement ray xk
output : Updated map belief bk+1
Procedure :bk+1=Update (bk,zk,xk)
1Find Vrayvoxels on the ray xk;
2Compute SCM using Eq. ( 25);
3foreach vi∈Vraydo
4 Compute αi,βi(Eqs. ( 22), (23));
5 Update voxel belief (Eq. ( 21));
6return bk+1;
634 A. Agha-mohammadi et al.
7 Conﬁdence in Map and Safe Exploration for Planning
The proposed method not only provides a more accurate map estimate, but more
importantly, the uncertainty associated with the returned value. In doing so, it incor-porates the full forward sensor model into the mapping process. As an example,
it can distinguish between two voxels, when they are both reported as almost free
(e.g.ˆm
1=ˆm2=0.1), but they have different conﬁdence levels (e.g. σm1=0.01 and
σm2=0.2). This conﬁdence level is a crucial piece of information for the planner.
Obviously, the planner should either try to avoid m2since the robot is not sure if
m2is actually risk free (due to high variance), or it needs to take active perceptual
actions to take more measurements from m2before passing through that part of the
map.
In the log-odds-based method only one number is stored in the map, namely the
parameter of the Bernoulli distribution. One might try to utilize the variance of the
Bernoulli distribution to infer the conﬁdence in an log-odds-based map, but dueto the incorrect assumptions made in the mapping process and since the Bernoulli
distribution is a single parameter distribution (mean and variance are dependent), the
computed variance is not a reliable conﬁdence source.
It is important to note that generally a planner is able to cope with large errors if
there is a high variance associated with them. However, if the error is high and the
ﬁlter is conﬁdent about its wrong estimate, planning becomes very risky, and proneto failures.
Consider a simple planning scenario in an unknown environment, where a quadro-
tor has to traverse the environment as fast as possible while ensuring a reasonablelevel of safety. Using our conﬁdence-rich mapping (CRM) approach, one can predict
the future variance of the occupancy of the path. Since our method yields consistent
variance estimates, it enables uncertainty-aware planner to reliably infer about theinformation gain of future perceptual measurements. Having this information opens
new possibilities for uncertainty-aware planners such as incorporating safety-critical
exploratory behavior into the fast navigation task. We plan to thoroughly investigate
the planning component of the problem in future work.
8 Results
In this section, we demonstrate the performance of the proposed method and compare
it with commonly used mapping methods. Figure 3shows a 2D ground truth map
which serves as a simulation environment for the following scenario. Each voxel is
assumed to be a cube with s=0.06 m side length. The environment size is 2 ×2m ,
consisting of 1089 voxels. Each voxel is either fully occupied (shown in black) orempty (white). The robot follows a trajectory (red arrows), as shown in Fig. 3, and
takes one measurement at every arrow position.
Conﬁdence-Rich Grid Mapping 635
Fig. 3 Ground truth map with sampling trajectory where red arrows represent measurement posi-
tions
Table 1 Mean Absolute Error (MAE) and inconsistency Icwithγ=1( E q . 27) of log-odds map-
ping, GPOM and the proposed method under different sensor model noise std. deviations (right)
where sis the voxel size (0 .06 m)
Sensor std dev 0.25 s 0.5s s 2s 3s
Log-odds
MAE0.346 0.348 0.351 0.358 0.376
GPOM MAE 0.421 0.468 0.468 0.468 0.468
CRM MAE 0.289 0.299 0.323 0.355 0.365
Log-odds Ic 17.340 18.609 17.665 18.862 19.243
GPOM Ic 10.167 10.172 12.964 10.212 10.490
CRM Ic 0.056 2.040 6.100 5.418 4.575
For the sensing system, we simulate a ranging sensor with 60 omnidirectional
depth sensors spanning over a ﬁeld-of-view of 360◦and reaching up to a range of
1 m. Measurements are perturbed by zero-mean Gaussian noise with 0 .06 m std.
deviation, i.e. one voxel length.
For the log-odds mapping, we use a typical inverse sensor model (Fig. 1) with
parameters rramp=0.1,rtop=0.1,ql=0.45, and qh=0.55. The Gaussian Pro-
cesses occupancy mapping (GPOM) uses a combination of a Matérn kernel with a
Gaussian noise:
k(x,x/prime)=21−v
Γ(v)/parenleftBigg√
2v|x−x/prime|
l/parenrightBiggv
Kv/parenleftBigg√
2v|x−x/prime|
l/parenrightBigg
+σ2δx,x/prime (26)
where Kvis the modiﬁed Bessel function of order vandδx,x/primeis the Kronecker delta.
This kernel has three parameters, v,landσ, which have been determined empirically
(Fig. 7(b)) to achieve comparable results to log-odds mapping and our method.
636 A. Agha-mohammadi et al.
Fig. 4 Mapping results: CRM (left column), Log Odds (center column), GPOM (right column).
Mean occupancies (top row), estimated standard deviation (middle row), inconsistencies |ec|>σ c
for absolute voxel error |ec|and std σc(lower row). The mean occupancy GP map (top right) shows
the GP sample set where blue dots represent samples of free space and red dots represent occupiedspace. Measurement rays are discretized on the voxel grid for GPOM, and appear with a randomoffset only for visualization purposes
The occupancy maps resulting from the log-odds mapping, GPOM and the pro-
posed method are shown in the upper row of Fig. 4, respectively.
First, we study the sensitivity of our method to different sensor noises. The top
rows of Table 1show the map mean absolute error (MAE) over time for different
sensor noise std deviations. The MAE is averaged over all the voxels that were
updated throughout the mapping process to best show the improvement of the affected
parts of the map. For the same noise intensity, the proposed CRM method shows a
smaller error compared to the log-odds and Gaussian Processes method.
Conﬁdence-Rich Grid Mapping 637
0 20 40 60 80 100 120 1401.00.50.00.51.0CRM
True Error
±2
±
0 20 40 60 80 100 120 1401.00.50.00.51.0Log Odds
True Error
±2
±
0 20 40 60 80 100 120 1401.00.50.00.51.0GPOM
True Error
±2
±
Fig. 5 Mapping error (red) and algorithmically computed std deviation (blue shades for 2 σ(light)
andσ(dark) conﬁdence bounds) over a set of voxels, with highlighted severe inconsistencies: errors
outside 2 σ-interval (black squares) and outside 1 σ-interval (orange diamonds)
Fig. 6 Evaluation of inconsistency (Eq. 27) over threshold values γ(left). Pearson correlation
coefﬁcient between the reported std deviation and the absolute error over all voxels at a given step
(right)
It is worth emphasizing that reducing the map error is only an ancillary beneﬁt
of our method. The main objective of CRM is to provide a consistent conﬁdence
measure. This consistency is particularly important for planning purposes. A planner
might be able to handle large errors as long as the ﬁlter indicates that the estimates
are unreliable (e.g. via their variance). However, if the ﬁlter returns a wrong estimate
with low variance (i.e. it is conﬁdent that its estimate is correct, while it is not), then
the planner is prone to yield unsafe results.
To quantify the inconsistency between the error and reported variances, we utilize
the following measure:
638 A. Agha-mohammadi et al.
Ic=/summationdisplay
cramp(|ec|−γσ c) (27)
where, ecandσcdenote the estimation error and the std. deviation of voxel
c, respectively. γdecides what level of error is acceptable. The ramp function
ramp(x):=max(0,x)ensures that only inconsistent voxels (with respect to γσ c)
contribute to the summation. Accordingly, Icindicates how much of the error signal
is out of bound (i.e., how unreliable the estimate is) over the whole map. As can be
seen in the middle row of Fig. 4, where voxels of high inconsistency appear brighter,
the log-odds based approach tends to be overly conﬁdent in false estimates, com-pared to the other methods. GP-based maps appear less clear compared to traditional
occupancy grids with the advantage of a higher consistency. However, as can be seen
in the bottom right map of Fig. 4, smaller obstacles are not mapped accurately and
pose dangerous areas of inconsistency.
The last two rows of Table 1show the described inconsistency measure for dif-
ferent sensor noise levels. The proposed CRM method is able to signiﬁcantly out-
perform the log-odds and the Gaussian Processes methods for all of the tested noise
std. deviations.
Figure 5shows the error value corresponding to the groundtruth map in Fig. 3
along with the estimated σ-bound over
1/7of all voxels. As can be seen from the top
row, the σ-bound from CRM grows and shrinks in a consistent manner with the error,
and behaves as a reliable conﬁdence interval that can be used in planning. However,
the log-odds mapping (middle row), there exist many voxels (highlighted by square
and diamond markers) where the error is high (close to −1 or 1, i.e., a free voxel is
estimated as fully occupied or vice versa) at which the variance is very low , which
pose a signiﬁcant challenge to the planner. Similarly to our method, GPOM rarely
exhibits inconsistencies. However, the σ-bound is not as pronounced (bottom row),
compared to CRM, making it difﬁcult to assess the conﬁdence of the estimates since
most of them appear highly uncertain ( σclose to 0 .5).
Figure 6(left) compares the inconsistency measure (Eq. 27) over different γ
thresholds for the presented mapping algorithms. The log-odds approach overes-
timates the conﬁdence signiﬁcantly more while GPOM and our method yield much
safer estimates.
While GPOM and the proposed approach yield similar results in the ramp incon-
sistency comparison, it should be noted that GPOM is computationally much moreexpensive than occupancy grids [ 15]. Gaussian Processes rely on the full set of mea-
surement samples in order to predict the map occupancy. Over the course of the
trajectory, it takes more than two minutes to update the Gaussian Processes with1380 depth measurements and to query the GP at all every voxel. In comparison, on
the same hardware, our method ﬁnishes both tasks in less than ﬁve seconds.
As an alternative measure of consistency, we compute the Pearson correlation
coefﬁcient between the true mapping error and the reported std. deviation (see Fig. 6
on the right), which indicates that the std. deviation computed by the proposed method
is highly correlated with the error, and hence can reliably describe the error’s behavior.
Conﬁdence-Rich Grid Mapping 639
Fig. 7 Comparison of cumulative mapping error between log-odds using different ISM parameters
and the proposed method. The whiskers depict the inconsistency Ic(Eq. 27), the striped areas
represent the portion of errors |ec|that are inconsistent with the reported std (greater than γσ cfor
γ=1.25). Our method results in a seven times more consistent mapping with one third of the
critical errors due to overconﬁdence, compared to the best log-odds conﬁguration. While some
GPOM conﬁgurations yield zero inconsistencies for γ=1.25, the absolute error is much higher
(>20%) compared to the proposed method, rendering the overall occupancy information less useful
Since the inverse sensor model (ISM) is typically hand-engineered for a given sen-
sor and a given environment, we sweep over a set of ISM parameters to compare the
performance with the proposed method. Following the generic form of ISM in Fig. 1,
we create 27 ISM’s by (i)setting the qh−0.5=0.5−qlto(0.05,0.2,0.4),(ii)set-
ting rramp to(0.05,0.1,0.3), and (iii) setting rtopto(0.05,0.1,0.3). Accordingly,
we draw the Gaussian Process (GP) parameters v,landσfrom the sets {2,2.5,3},
{2.5,3.5,4.5}and{0.5,1,1.5}, respectively, yielding 27 different GPOM conﬁgura-
tions. The results reported in Fig. 7show that CRM consistently produces a smaller
absolute error than log-odds and GPOM over a variety of ISM and GP parameters,
as well as a smaller portion of error that is inconsistent according to the Icmeasure.
9 Conclusion
This paper proposes an alternative algorithm for occupancy grid mapping, by storing
richer data for every voxel. It extends traditional grid mapping in three ways: ﬁrst,
it relaxes the full-independence assumption and incorporates dependence between
voxels in the measurement cone into the mapping scheme. Second, it relaxes the need
for hand-engineering an inverse sensor model and proposes the sensor cause model
that can be derived in a principled manner from the forward sensor model. Third, and
640 A. Agha-mohammadi et al.
most importantly, it provides consistent conﬁdence values over occupancy estima-
tion that can be reliably used in planning. The method runs online as measurements
are received and it enables mapping environments where voxels might be partiallyoccupied. The results on real-world laser measurements show that the mapping accu-
racy is up to three times better than the ISM-based method and, in simulation, 30%
better than Gaussian Processes maps. More importantly, according to the proposedconsistency measure, the conﬁdence values are up to two orders of magnitude more
reliable.
References
1. Agha-mohammadi, A., Heiden, E., Hausman, K., Sukhatme, G.: Conﬁdence-rich grid map-
ping: Extended version. Technical Report, http://people.lids.mit.edu/aliagha/Web/pubpdfs/
CRM.pdf (2017)
2. Elfes, A.: Occupancy grids: a probabilistic framework for robot perception and navigation.
Ph.D thesis, Carnegie Mellon University (1989)
3. Hähnel, D., Triebel, R., Burgard, W., Thrun, S.: Map building with mobile robots in dynamic
environments. In: IEEE International Conference on Robotics and Automation, vol. 2, pp.1557–1563. IEEE (2003)
4. Howard, A., Kitchen, L.: Generating sonar maps in highly specular environments. In: Pro-
ceedings of the Fourth International Conference on Control Automation Robotics and Vision(1996)
5. Kay, S.M.: Fundamentals of Statistical Signal Processing, V olume I: Estimation Theory. Pren-
tice Hall (1993)
6. Kim, Soohwan, Kim, Jonghyuk: Occupancy mapping and surface reconstruction using local
gaussian processes with kinect sensors. IEEE Trans. Cybernetics. 43(5), 1335–1346 (2013)
7. Kim, S., Kim, J. et al.: Recursive bayesian updates for occupancy mapping and surface recon-
struction. In: Proceedings of the Australasian Conference on Robotics and Automation (2014)
8. Konolige, Kurt: Improved occupancy grids for map building. Auton. Robots 4(4), 351–367
(1997)
9. Konolige, K., Agrawal, M., Bolles, R.C., Cowan, C., Fischler, M., Gerkey, B.: Outdoor mapping
and navigation using stereo vision. In: Experimental Robotics, pp. 179–190. Springer (2008)
10. Paskin, M., Thrun, S.: Robotic mapping with polygonal random ﬁelds. In: Conference on
Uncertainty in Artiﬁcial Intelligence, pp. 450–458 (2005)
11. Moravec, H.P .: Sensor fusion in certainty grids for mobile robots. AI Mag. 9(2), 61 (1988)
12. Moravec, H.P .: Robot spatial perception by stereoscopic vision and 3D evidence grids. In:
Technical Report CMU-RI-TR-96-34, Carnegie Mellon University (1996)
13. Newcombe, R.A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison, A.J., Kohi, P .,
Shotton, J., Hodges, S., Fitzgibbon, A.: Kinectfusion: real-time dense surface mapping andtracking. In: 2011 10th IEEE International Symposium on Mixed and Augmented Reality(ISMAR), pp. 127–136 (2011)
14. OCallaghan, S., Ramos, F.T., and Durrant-Whyte, H.: Contextual occupancy maps using
Gaussian processes. In: IEEE International Conference on Robotics and Automation, 2009.
ICRA ’09, pp. 1054–1060 (2009)
15. OCallaghan, S.T., Ramos, F.T.: Gaussian process occupancy maps. Int. J. Robot. Res. 31(1),
42–62 (2012)
16. Pagac, D., Nebot, E.M., Durrant-Whyte, H.: An evidential approach to probabilistic map-
building. In: Reasoning with Uncertainty in Robotics, pp. 164–170. Springer (1996)
17. Ramos, Fabio, Ott, Lionel: Hilbert maps: scalable continuous occupancy mapping with stochas-
tic gradient descent. Int. J. Robot. Res. 35(14), 1717–1730 (2016)
Conﬁdence-Rich Grid Mapping 641
18. Stachniss, C.: Robotic Mapping and Exploration, vol. 55. Springer (2009)
19. Thrun, Sebastian: Learning metric-topological maps for indoor mobile robot navigation. Artif.
Intell. 99(1), 21–71 (1998)
20. Thrun, Sebastian: Learning occupancy grid maps with forward sensor models. Auton. Robots
15(2), 111–127 (2003)
21. Thrun, Sebastian, Burgard, Wolfram, Fox, Dieter: Probabilistic Robotics. MIT Press, Cam-
bridge (2005)
22. Thrun, Sebastian, et al.: Robotic mapping: a survey. Explor. Artif. Intell. New Millenn. 1, 1–35
(2002)
23. V eeck, Michael, Burgard, Wolfram: Learning polyline maps from range scan data acquired
with mobile robots. IEEE/RSJ Int. Conf. Intell. Robots Syst. 2, 1065–1070 (2004)
24. Wang, J., Englot, B.: Fast, accurate Gaussian process occupancy maps via test-data octrees
and nested bayesian fusion. In: IEEE International Conference on Robotics and Automation(ICRA), pp. 1003–1010 (2016)
25. Wurm, K.M., Hornung, A., Bennewitz, M., Stachniss, C., Burgard, W.: Octomap: a probabilis-
tic, ﬂexible, and compact 3d map representation for robotic systems. In: ICRA 2010 Workshopon Best Practice in 3D Perception and Modeling for Mobile Manipulation, vol. 2 (2010)
26. Yamauchi, B.: A frontier-based approach for autonomous exploration. In: IEEE International
Symposium on Computational Intelligence in Robotics and Automation, pp. 146–151 (1997)
Multirobot Cooperative Localization
Algorithm with Explicit Communicationand Its Topology Analysis
Tsang-Kai Chang, Shengkang Chen and Ankur Mehta
1 Introduction
Localization is one of the most fundamental tasks for mobile robots [ 18]. In mul-
tirobot systems, robots can observe and communicate with one another to achievelocalization cooperatively, even in uncharted places or GPS-denied environments.
This scheme is called cooperative localization (CL), and is considered a promising
approach for better autonomy. Among all approaches for CL, we focus on those
based on the extended Kalman ﬁlter (EKF) due to its computational efﬁciency.
The success of CL algorithms primarily lies in tracking the correlations between
robots. If the correlations between robots are underestimated, that is, two estimates
are considered more independent than they really are, the uncertainty in the resulting
localization estimation may be too optimistic. This is exactly the double-countingproblem, or the over-conﬁdence problem , which impairs the localization reliability.
Consequently, extensive communication between the distributed robots is required
to track those correlations across all localization estimates.
Besides the communication overheads in these algorithms, there is a more funda-
mental concern: the communication is often implicitly assumed to be perfect after
relative observations. In fact, observation and communication are performed by dif-ferent hardware modules, and communication failures are unavoidable in real sys-
tems. That is, the availabilities of observation and of communication are not identi-
cal, and under this assumption the observation step is vulnerable to communicationfailure.
T.-K. Chang ( B)·S. Chen ·A. Mehta
University of California, Los Angeles, CA 90095, USAe-mail: tsangkaichang@ucla.edu
S. Chen
e-mail: billyskc@ucla.edu
A. Mehta
e-mail: mehtank@ucla.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_46643
644 T.-K. Chang et al.
In this paper, we propose the Global State–Covariance Intersection (GS-CI) mul-
tirobot cooperative localization algorithm that separates communication and obser-
vation as two independent information contributors. As a consequence, the proposedalgorithm needs lower communication cost, and is robust to communication failure.
To avoid tracking the exact interdependency between distributive estimations, we
apply covariance intersection (CI) [ 1,5,16] during the communication update to get
a conservative but consistent result, which guarantees that the computed covariance
is always not smaller than the true covariance in a positive-deﬁnite sense.
While the framework of the proposed algorithm is similar to the ones discussed
in vehicular technology [ 9,13], we furthermore provide the theoretical treatment for
the proposed algorithm. It has been shown in [ 15] that in a centralized-equivalent
localization algorithm, the covariance will be bounded if at least one landmark areobserved. The veriﬁcation should also be given for the proposed algorithm, con-
cerning the disagreement of observation and communication topologies, and the
conservative fusion result by CI as well.
The contributions of this paper include:
•The CL algorithm that separates out communications explicitly and also preserves
estimation consistency to avoid over-convergence
•An analysis of the boundedness properties of the algorithm with respect to obser-
vation and communication topologies
•Comparisons of our algorithm versus other state-of-the-art algorithms based on
simulated and measured data demonstrating comparable estimation error with less
communication cost
This paper is organized as follow: Related work is presented in the following
section. We setup the system model in Sect. 3. The main algorithm is proposed in
Sect. 4, while the associative topology analysis is given in Sect. 5. Simulations are
delivered in Sect. 6, followed by the conclusions of this paper.
2 Related Work
The benchmark of EKF-CL is established in [ 17], together with the theoretical analy-
sis in [ 15] .T h ef o r m u l a t i o ni n[ 17] begins with a strict requirement on the availability
of the local information in each robots, which can only be satisﬁed in a centralized
system. A distributive version is then designed in the same paper, where each robot
holds its own estimation and correlation parameters regarding rest of the robots.
However, to keep those correlation parameters in each robot updated, the communi-
cation complexity of this algorithm after one relative observation is O(N2), where N
is the number of the robots. The communication complexity is lowered to O(N)in
[11] by introducing the interim master, who is in charge of calculating and broadcast-
ing the required updating information for the rest of the robots. Following the samemodeling, a partially-decentralized scheme is discussed in [ 10], mainly to combat
Multirobot Cooperative Localization Algorithm with Explicit … 645
information dropouts in communication failure. However, a central process unit in
charge of information exchange is indispensable in this scheme.
In [3,14], the communication cost is only O(1)after the relative observation. In
both papers, only the estimation information in the robots involved in relative obser-
vation is utilized, which means that the exact correlation between the fused estimates
is not available but can only be approximate. In other words, the communication costis lowered with the cost of estimation accuracy. In [ 3], CI is applied to get a con-
servative but consistent estimation result. While our algorithm applies CI as well, it
outperforms the one proposed in [ 3] in estimation accuracy since our algorithm can
utilize the correlations of robots not involved in the relative observation. In [ 14], the
exact correlation between the fused estimates is approximated without the guarantee
of estimation consistency. As mentioned in [ 14], the approximated covariance in
their algorithm may be smaller than the real covariance in positive-deﬁnite sense,
which may lead to over-convergence problem. For all the aforementioned algorithms
[3,14,17], the communication takes place directly after relative observation. On the
contrary, we separate communication and observation for the robustness against
communication failure.
Similar to the algorithm we propose, the CL algorithm for vehicle networks stores
the estimation for the whole system in each agent, called state exchange scheme [8,9,
13]. The main challenge of the state exchange scheme is to avoid over convergence
problem without the knowledge of correlation between state estimates. In [ 8,9],
only independent estimates are fused. However, to keep the required independence,
historical information is discarded if there are new-coming independent information.The split covariance intersection (SCI) is applied in [ 13] in information fusion. One
of the main drawbacks of this approach is that SCI is not applicable in relative
observation due to the dependency of the information, as mentioned in [ 3]. Instead
of applying SCI, we will consider CI for generalization without imposing further
requirement. Furthermore, we are the only one that provides theoretical argument
to guarantee the consistency and boundedness of the uncertainty, to the best of ourknowledge.
As for the estimation algorithm, our framework is similar to the diffusion Kalman
ﬁlter with CI [ 6]. In the diffusion step of the original diffusion Kalman ﬁlter [ 4],
the combination of estimations is based on heuristic convex combination without
updating the corresponding covariance matrix. In [ 6], the diffusion step is replaced
by CI, which guarantees the consistency of the resulting estimation. The major dif-ference between our algorithm and diffusion-based KF is that we do not implement
incremental update, where the raw measurement data is exchanged and incorporated.
However, whether to exchange state estimates or measurement results or both should
be further investigated concerning the accuracy and efﬁciency jointly.
646 T.-K. Chang et al.
3 System Model
We consider a 2D multirobot system indexed by Ω={1,..., N}, together with
several landmarks whose locations are known by the robots in advance. Landmarksare denoted as Δ, andΩ
∗=Ω∪{Δ}. The position of robot iat time tis regarded
as the state, denoted as si,t=[xi,t,yi,t]T, where Tdenotes matrix transpose. The
orientation of robot iat time tis denoted by θi,t, and we do not incorporate θi,tin
the estimation state due to the linearization issue [ 2]. The state of the whole system
is denoted by st=[sT
1,t,..., sT
N,t]T. In EKF, each robot ikeeps an estimate of st,
denoted by ˆsi
t, together with its covariance Σsi,t.
3.1 Motion Model
The motion model describes the spatial displacement of robots due to odometry
inputs. While the framework is not limited to any speciﬁc models, we mainly consider
the velocity input vi,tin this paper. Let δtbe the time interval between two consecutive
observation points, the state of robot iat the next time is given by
si,t+1=fi(si,t,vi,t)=/bracketleftbiggxi,t+vi,tδtcos(θi,t)
yi,t+vi,tδtsin(θi,t)/bracketrightbigg
. (1)
3.2 Observation Model
If robot iobserves an object j, either a robot or a landmark, the relative position
obtained by robot iis
oij=CT(θi,t)/parenleftbigg/bracketleftbiggxj,t
yj,t/bracketrightbigg
−/bracketleftbiggxi,t
yi,t/bracketrightbigg/parenrightbigg
=Hoijst, (2)
where C(θ)is the rotation matrix with argument θ.
Most of the time, the relative positions can not be obtained directly, but they
are general enough to incorporate different kinds of sensing result. The observation
is often accomplished by distance and bearing sensors. Consequently, the relative
position can also be expressed as
oij=dij/bracketleftbiggcos(φij)
sin(φij)/bracketrightbigg
, (3)
based on the relative distance dijand relative bearing φij.
Multirobot Cooperative Localization Algorithm with Explicit … 647
4 Cooperative Localization Algorithm with Explicit
Communication
As mentioned, the CL algorithm should consist of three parts based on the hardware
module. In this section, we present an algorithm according to these three actions:
motion propagation, observation and communication. Motion propagation and obser-vation steps are standard EKF updates, but we have to handle the unavailable odome-
try inputs of other robots in motion propagation. In communication step, it is nothing
more than a direct application of CI, which will be introduced in the following.
4.1 Motion Propagation
We consider robot ifor example. While the velocity inputs of each robot are uncor-
related, the covariance update of motion propagation is
Σsi,t+1=Σsi,t+Σqi=Σsi,t+Diag(Σu1,...,Σ uN). (4)
The determination of Σujdepends on the velocity inputs vjfor all j. For robot
i, the velocity input viis available, and it is disturbed by a noise nvmodeled as
zero-mean Gaussian random variable with variance σ2
nv. By linearizing ( 1), the error
propagation equation of robot iitself is
˜si,t+1≈/bracketleftbigg˜xi,t
˜yi,t/bracketrightbigg
+δt/bracketleftbiggcos(θi,t)−visin(θi,t)
sin(θi,t)vicos(θi,t)/bracketrightbigg/bracketleftbiggnv
˜θi,t/bracketrightbigg
. (5)
The errors in the orientation estimates ˜θi,t=θi,t−ˆθi,tare modeled by a zero-mean
Gaussian random variable, whose variance σ2
˜θi,t=E[˜θ2
i,t]is bounded by σ2
˜θi.T h e
increment of covariance matrix Σuican then be obtained as
Σui=(δt)2C(θi,t)/bracketleftbiggσ2
nv0
0v2
iσ2
˜θi/bracketrightbigg
CT(θi,t). (6)
The velocity inputs vj,j/negationslash=i, are not available for robot i. Without the exact value,
we than model the input itself as a Gaussian random variable vj, whose variance σ2
v
can be determined by the maximum input value vmaxandσ2
nv<σ2
v. While the exact
velocity value is unknown, we can still get upper bound covariance increment by
Σuj=(δt)2max(σ2
v,v2
maxσ2
˜θi)I2,j/negationslash=i, (7)
where Inis the n×nidentity matrix.
648 T.-K. Chang et al.
4.2 Observation
The observation step updates the estimates based on the exteroceptive measurements
oij, which is standard in EKF procedure. The subscript iis omitted in the following
derivation since the observation update occurs only in the robot itself. Based on ( 2),
the innovation ˜oij=oij−ˆoijcan be approximated as
˜oij≈Hoij˜s+CT(ˆθi)JH ijˆst−˜θi+noij, (8)
to distinguish the estimation error ˜s, the orientation estimation error ˜θi, and the
measurement noise noij, where Hoij=CT(θi)Hijand J=/bracketleftbigg01
−10/bracketrightbigg
. If the object j
is a landmark,
Hij=⎡
⎣02×2··· − I2/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
i···02×2⎤
⎦
2×2N;
while the object is robot j, then
Hij=⎡
⎣02×2··· − I2/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
i··· I2/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
j···02×2⎤
⎦
2×2N.
Furthermore, if the observed relative position is obtained by distance and bearing
sensors as in ( 3), the errors in observed relative position noijcan be expressed as
noij=/bracketleftbiggcosφij−dijsinφij
sinφijdijcosφij/bracketrightbigg/bracketleftbigg˜di
˜φi/bracketrightbigg
,
with the corresponding covariance Roij. Then the covariance of innovation ˜oij
isΣoij=E[˜oij˜oT
ij]= HoijΣs,t−HT
oij+Rθij+Roij, where Rθij=Gijσ2
θiGT
ij,Gij=
CT(ˆθi)JH ijˆst−.
Robots can observe more than one object at the same time, and the observation
results are then correlated. Therefore, we should process the results in the same
observation step together. We ﬁrst deﬁne the observation topology Oi,t={Ω∗,EOi,t}
as a directed graph with edges (i,j)∈EOi,twhere jis the object observed by iat time
t. For simplicity, O∗
i,t={j∈Ω∗|(i,j)∈EOi,t}is the index set of objects observed
by robot i, and Oi,t=O∗
i,t∩Ω. The overall observation result can be presented by
the stacked vector
oi=[oij]j∈O∗
i,t=[oT
ij1,..., oT
ijk]T,j1,..., jk∈O∗
i,t.
Multirobot Cooperative Localization Algorithm with Explicit … 649
By separating state error and the rest terms, the covariance of the innovation ˜oi
isΣoi=HoiΣs,t−HT
oi+Σri, where Hoi=[Hoij]j∈O∗
i,t. Explicitly, Σri=Giσ2
θiGT
i+
Diag(Roij), where Gi=[Gij]j∈O∗
i,t.
4.3 Communication
When robot jsends its estimation information, in particular ˆsjandΣsj, to robot
i, robot ican use this information to update its own estimation. However, the cor-
relation between ˆsiandˆsjis not recorded at all. Without knowing the correlation
among estimates, the fusion of estimates becomes challenging. In particular, if oneunderestimates the correlation between two fused estimates, or the two estimates are
considered more independent than they really are, one encounter the over-conﬁdence
problem. However, tracking the correlations imposes excessive communication over-head as in aforementioned algorithms [ 3,14,17]. Here, we apply CI to fuse two
estimates without knowing the correlation between them, and to guarantee the con-
sistency of estimates.
Deﬁnition 1 (Consistent fusion [1]) An estimate zis a Gaussian random vector with
mean E[z]=¯zand covariance Σ
z. The pair (ˆz,Σˆz)is called consistent ifE[ˆz]=¯z
andΣˆz≥Σz.
A consistent estimate can be regarded as another estimate which is mean-preserving
but conservative in terms of covariance matrix.
We can similarly deﬁne the the communication topology Ci,t={Ω∗,ECi,t}as a
directed graph with edges as (j,i)∈ECi,twhere jsends its information to iat time
t.A l s o , Ci,t={j∈Ω|(j,i)∈ECi,t}is the index set of the information sources.
By directly applying CI, we have the update equation for robot iwhen receiving
information from j∈Ci,t, the index set of the robots sending information to robot
i,a si n( 13) and ( 14).
The coefﬁcients {c0,cj,j∈Ci,t}are all nonnegative and sum to 1. The deter-
mination of the coefﬁcient is usually based on the minimization of det(Σsi,t+)or
that of tr(Σsi,t+). In implementation, the communication update normally involves
two estimates only, since the inverse of convex combination parameters may lead to
undesirably conservative result once the number of parameters is large.
4.4 Summary
The overall proposed algorithm is summarized in Algorithm 1, Global State Covari-
ance Intersection (GS-CI) Multirobot Cooperative Localization Algorithm. The term
global state indicates that each robot keeps an estimate on the whole system, in orderto distinguish other algorithms in which each robot only keeps an estimate on its
own state.
650 T.-K. Chang et al.
Algorithm 1 Global State Covariance Intersection (GS-CI) Multirobot Cooperative
Localization Algorithm
Initialization
Setˆsi
0andΣsi,0for robot i.
Motion Propagation
input: odometry input vi,t
ˆsi
t+1=[fT
1(ˆsi
1,t,E[v1,t]) ,..., fT
i(ˆsi
i,t,vi,t) ,..., fT
N(ˆsi
N,t,E[vN,t])]T, (9)
Σsi,t+1=Σsi,t+Diag(Σu1,...,Σ uN). (10)
Observation
input: observation result oi
ˆsi
t+=ˆsi
t−+Σsi,t−HT
oiΣ−1
oi(oi−Hoiˆsi
t−), (11)
Σsi,t+=Σsi,t−−Σsi,t−HT
oiΣ−1
oiHoiΣsi,t−. (12)
Communication
input: ˆsj
t−,Σsj,t−from robot j∈Ci,t
ˆsi
t+=Σsi,t+⎡
⎣c0Σ−1
si,t−ˆsi
t−+/summationdisplay
j∈Ci,tcjΣ−1
sj,t−ˆsj
t−⎤
⎦, (13)
Σ−1
si,t+=c0Σ−1
si,t−+/summationdisplay
j∈Ci,tcjΣ−1
sj,t−. (14)
5 Topological Analysis of Global State Covariance
Intersection Multirobot Cooperative Localization
Algorithm
In EKF setup, the uncertainty of the estimate ˆsiis represented by the state covariance
Σsi. While the observation and communication topologies are no longer required to
be identical in the proposed algorithm, together with conservative fusion scheme in
CI, the uncertainty of the proposed algorithm should be guaranteed non-divergent
as a prerequisite for further application, as in [ 15]. Therefore, we determine the
boundedness criteria for Σsiof the proposed algorithm in this section, and it mainly
depends on the observation and communication topology (Oi,t,Ci,t). To proceed, we
use the notation convention that for P>0 and an index set I, we deﬁne [P]Ias the
submatrix of the index set I.
We consider a time-slotted scenario for theoretical analysis. In each time slot,
every robot executes motion propagation, observation and communication in Algo-
rithm 1 consecutively. We assume that the observation topology Oiand communi-
cation conﬁguration Ciare time-invariant for each robot. The parameters in convex
combination are also constant w.r.t. time. We then obtain the covariance update in
each time slot as
Multirobot Cooperative Localization Algorithm with Explicit … 651
Σsi,t+1=⎡
⎣c0Σ−1
si,t+c0HT
oiΣ−1
riHoi+/summationdisplay
j∈CicjΣ−1
sj,t⎤
⎦−1
+Σqi. (15)
With the deﬁnition Hi=[Hij]j∈O∗
i,i tf o l l o w st h a t Hoi=ΞT(ˆθi)Hi,Ξ(ˆθi)=IN⊗
C(ˆθi), where ⊗stands for Kronecker multiplication. The update Eq. ( 16) can then
be rewritten as
Σsi,t+1=⎡
⎣c0Σ−1
si,t+c0HT
iΞ(ˆθi)Σ−1
riΞT(ˆθi)Hi+/summationdisplay
j∈CicjΣ−1
sj,t⎤
⎦−1
+Σqi.(16)
We can ﬁnd the covariance bound of the error in observation and in motion prop-
agation, ΣrandΣqrespectively, such that Σr≥ΞTΣriΞandΣq≥Σqiby some
fairly common physical constraints. The detail can be found in [ 15]. By substituting
the corresponding terms in ( 16), we can ﬁnd another sequence Πsi,twith recursion
Πsi,t+1=⎡
⎣c0Π−1
si,t+c0HT
iΣ−1
rHi+/summationdisplay
j∈CicjΠ−1
sj,t⎤
⎦−1
+Σq. (17)
such that Πsi,t≥Σsi,tfor all twith the same initial condition Πsi,0=Σsi,0. Fur-
thermore, in ( 17) every term besides Πsi,tis constant and independent of the state of
the system.
Proposition 1 IfΔ∈O∗
i, then [Πsi,t]Oiis bounded with positive deﬁnite initial
condition.
Proof We ﬁrst rearrange the order in Πsi,t+1to pick ias the ﬁrst, followed by other
terms in Oi, while the rest are all indices not in Oi. Thus, [Πsi,t+1]Oiis now the
upper-left diagonal submatrix of Πsi,t+1.
In fact, the elements in HT
iΣ−1
rHiare zero outside Oi. Explicitly,
[HT
iΣ−1
rHi]Oi=/bracketleftbigg
n−1T
n−1
−1n−1In−1/bracketrightbigg
⊗Σ−1
r,
where n=|Oi|. With the special structure, the matrix is nonsingular, with the inver-
sion
R=[HT
iΣ−1
rHi]Oi−1=/parenleftbigg
1n1T
n+/bracketleftbigg00
0In−1/bracketrightbigg/parenrightbigg
⊗Σr.
By dropping positive deﬁnite/summationtext
j∈CicjΠ−1
sj,tin the inversion, we have
Πt+1≤/bracketleftbig
c0Π−1
t+c0HT
iΣ−1
rHi/bracketrightbig−1+Σq. (18)
652 T.-K. Chang et al.
Applying Lemma 1in Appendix, the upper-left diagonal term in the inversion satisﬁes
c0[Π−1
t+HT
iΣ−1
rHi]Oi=c0[Π−1
t]Oi+c0[HT
iΣ−1
rHi]Oi
≥c0[Πt]Oi−1+c0R−1.
We can choose B=[c0Π−1
t](Oi)C, then by Lemma 2
c0Π−1
t+c0HT
iΣ−1
rHi≥1
2Diag/parenleftbig
c0[Πt]Oi−1+c0R−1,B/parenrightbig
.
The original recursion can then be bounded by
Πt+1≤2Diag/parenleftBig/bracketleftbig
c0[Πt]Oi−1+c0R−1/bracketrightbig−1,B−1/parenrightBig
+Σq,
which implies the solution Ptof the recursion
Pt+1=2/bracketleftbig
c0P−1
t+c0R−1/bracketrightbig−1+[Σq]Oi
=2
c0/bracketleftbig
Pt−Pt(R+Pt)−1Pt/bracketrightbig
+[Σq]Oi
is always larger than [Πt]Oi, and Ptconverges by [ 7, Chap. 14.5].
Proposition 1indicates that whenever one robot observes a landmark, the error
in its estimates of those observed robot together with that of the robot itself will be
bounded.
Proposition 2 Suppose that Δ∈O∗
i∪O∗
jand O∗
i∩O∗
j/negationslash=∅.I f j ∈Ci, then
[Πsi,t]Oi∪Ojwill be bounded given positive deﬁnite initial condition.
Proof By dropping the communication terms other than robot jin (17), we have
Πsi,t+1≤/bracketleftBig
c0Π−1
si,t+c0HT
iΣ−1
qHi+cjΠ−1
sj,t/bracketrightBig−1
+Σu. (19)
IfΔ∈O∗
iandΔ∈O∗
j, we can easily obtain the conclusion from Proposition 1and
Lemma 2. Therefore, we only consider that Δlies in one set.
Given Δ∈O∗
jandΔ/∈O∗
i, we now consider a submatrix of Πsi,twith index in
Oi∪Oj. From Proposition 1, we know that [Πsj,t]Ojis bounded, then so is [Πsi,t]Oj.
To ﬁnd the bound for [Πsi,t]Oi, we ﬁrst note that with n=|Oi|,
[HT
iΣ−1
rHi]Oi=/bracketleftbigg
n−1T
n
−1nIn/bracketrightbigg
⊗Σ−1
r,
Multirobot Cooperative Localization Algorithm with Explicit … 653
which is singular. We then ﬁnd one overlapping index k∈Oi∩Oj, and[Πsi,t]{k}
is bounded since k∈Oj. By deleting the row and the column associated with k,
[HT
iΣ−1
rHi]Oi\{k}is now nonsingular, which leads to the boundedness of [Πsi,t]Oi\{k}.
By Lemma 2, the conclusion follows.
The case Δ∈O∗
iandΔ/∈O∗
jcan be proved in the similar way by replacing the
communication term Πsj,tin (19) with the covariance update equation of j.
Proposition 2can be easily extended to [Πsi,t]Oi/uniontext
j∈CiOj,i fΔ∈O∗
i/uniontext
j∈CiO∗
j
and O∗
i∩O∗
j/negationslash=∅ for every pair of robots.
We can restate Propositions 1and 2in graph perspective to understand the com-
munication effect on the estimation performance. In a single robot, those indices with
bounded covariance can be regarded as connected to the landmark Δin the graph Oi,
according to Proposition 1. Considering the incoming communication information
in Proposition 2, the bounded indices are those weakly connected to Δin the graph
(Ω∗,/uniontext
j∈Ci∪{i}EOj), regardless of edge direction.
In fact, this is not all the indices having bounded covariance, since robot j∈Ci
can also receive other incoming information. We deﬁne a graph Tifor each robot
i∈Ωby
Ti=⎛
⎝Ω∗,EOi∪⎛
⎝/uniondisplay
j∈CiETj⎞
⎠⎞
⎠, (20)
/uniondisplay
i∈ΩETj=/uniondisplay
i∈ΩEOj. (21)
Tican be regarded as the collective information obtained by robot ifrom observation
and communication. To be more precise, an edge in Tirepresents an observation
relationship, either direct or received from other robots. However, the equation ( 20)
has a trivial solution in which all Tiis fully-connected. We have to impose one
more constraint to avoid such case. The additional equation ( 21) states that the
communication can not invent new observation. Therefore, the total observationpairs in all T
ishould be the same as those in all Oi. For simplicity, we deﬁne Tito
be the set of robots weakly connected to ΔinTi.
Proposition 3 [Πsi,t]{j}is bounded given positive deﬁnite initial condition if and
only if j ∈Ti.
Proof Ifj∈Ti, we can arrive the conclusion by the direct application of Propositions
1 and 2. For the inverse statement, the proof relies on Lemma 3 in [ 15]. One can refer
to it for details while the complete proof is omitted here.
As Proposition 3implies, [Σsi,t]Tiis also bounded.
654 T.-K. Chang et al.
6 Simulations
In this section, we simulate the proposed algorithm to verify the convergent analysis
as well as to compare the performance with other algorithms, including:
•local state centralized equivalent (LS-Cen) [ 11,17]
•local state CI (LS-CI) [ 3]
•local state block diagonal approximation (LS-BDA) [ 14]
•global state split CI (GS-SCI) [ 13]
•proposed global state CI (GS-CI)
In the ﬁrst part, we use generated data to substantiate the theoretical treatment of
the proposed algorithm. In the second part, we apply dataset collected in real-world
experimental setup to investigate the localization performance for the whole system.
6.1 Generated Data
We simulate a system with N=5 robots to verify the main propositions of this
paper. Robots are moving in a circle with radius 25 m. The velocity input is take
uniformly between [−0.25,0.25]m/s, with σnv=0.0125 m/s and σv=0.25 m/s.
The variances of observation are taken as σnd=0.1 m and σnφ=2◦. For LS algo-
rithms, only observation topology is considered as in Fig. 1, and the communication
topology is implicitly assumed fully connected, which means that the communica-
tion after relative observation is assumed perfect. For GS algorithms that separates
communication and observation, both topologies follow Fig. 1.
Fig. 1 The observation and
communication topologies
52
3
41
communicationlandmark
observation
Multirobot Cooperative Localization Algorithm with Explicit … 655
0 2 4 6 8 1000.050.10.150.20.25
time [s]estimation error [m]LS−Cen
LS−CI
LS−BDA
GS−SCI
GS−CI
0 2 4 6 8 1000.10.20.3
time [s]trace error [m]GS−CI (anly.)
Fig. 2 Simulation result of generated data
Table 1 Number of
communication per slot with
generated dataAlgorithms Number of communication
LS-Cen 23
LS-CI 6
LS-BDA 6
GS-SCI 2
GS-CI 2
We focus on the estimation in robot 1 and consider two error performance metrics.
The estimation error is given by/radicalbig
||ˆs1−s||2/N, which indicates the error comparing
to the ground truth. On the other hand, the trace error is deﬁned as√tr(Σs1)/N, which
signiﬁes the convergence of the covariance matrix in EKF.
Based on the simulation result in Fig. 2, LS-CI and GS-SCI are divergent in
terms of estimation error, since these two algorithms have no investigation on the
boundedness of state estimation covariance matrices. The rest can lead to reasonable
estimation results around 0 .05 m in estimation error, but the communication cost of
each algorithm varies from 23 for LS-Cen to 2 for the proposed GS-CI, as in Table 1.
In other words, the proposed GS-CI achieves comparable accuracy with far less
communication. The trace error of GS-CI also substantiates the analysis in Sect. 5:
T1=Ω, andΣˆs1is therefore bounded as a result according to Proposition 3.
656 T.-K. Chang et al.
6.2 Real-World Data
We use UTIAS Multi-Robot Cooperative Localization and Mapping Dataset [ 12]t o
compare the performance of algorithms. The UTIAS dataset is a 2D indoor dataset
collection consists of 9 individual datasets, and each dataset contains odometry and
measurement data from 5 robots, as well as accurate position groundtruth data. We
plot the dataset 6 in the UTIAS dataset in Fig. 3, while the results of other datasets
have no signiﬁcant difference.
In this simulation, we implement a centralized localization as in [ 17]a sap e r f o r -
mance benchmark, and focus on the topology constraints on LS and GS algorithms.
Speciﬁcally, we set that only robots 1 ,2,3 can observe landmarks, and the commu-
nication only takes place between robots 1 ,2,3 and 4 ,5 for all algorithms of interest.
In other words, robots 4 ,5 can only rely on relative observation and communication
for localization. To mitigate linearization error, we use orientation data from ground
truth. The parameters of propagation and observation errors in the algorithms follow
the previous simulation.
Instead of concentrating on the behavior of a single robot as in the previous part,
we consider the overall localization performance of all N=5 robots. Two metrics
are applied to present the estimation error: the root mean square error (RMSE) is
deﬁned as RMSE =/radicalBig/summationtextN
i=1||ˆsi
i−si||2/N, and RMTE =/radicalBig/summationtextN
i=1tr([Σsi]i)/Nas
the deﬁnition of root mean trace error (RMTE).
Since only robots 1, 2, 3 can observe landmarks, only LS-BDA and proposed
GS-CI can localize the rest robots in a distributed sense, where the RMSEs of both
0 100 200 300 400 500 60000.511.52
time [s]RMSE [m]Cen
LS−CI
LS−BDA
GS−SCI
GS−CI
0 100 200 300 400 500 60000.10.20.30.40.5
time [s]RMTE [m]
Fig. 3 Simulation result of UTIAS dataset
Multirobot Cooperative Localization Algorithm with Explicit … 657
algorithms are less than 1 m. However, one should notice that the number of commu-
nication is half in GS-CI than in LS-BDA, where the communication is bi-directional
after relative observation.
7 Conclusions
In this paper, we propose a cooperative localization algorithm for multirobot systemwith explicit communication mechanism. In the proposed algorithm the covariance
intersection is applied in communication update to preserve the consistency of the
estimate. We also give the boundedness criteria on covariance matrix of individualrobot, which heavily depends on the observation and communication topologies.
The work in this paper is inspiring in several perspectives. In most robotic systems,
localization is not the main goal but the underlying requirement for high-level tasks,exploration or self-driving for example. While the proposed algorithm has actions
corresponding to the hardware mechanism, one can relate the underlying physical
actions to the performance of the targeted tasks relied on localization for further
optimization. In addition, while the estimation is one of the fundamental problem in
robotics, the estimation scheme used in this paper can be applied to other distributiveestimation scenarios beyond localization, such as SLAM problems.
Appendix: Auxiliary Lemmas
Lemma 1 For P >0and the index set I , we have [P−1]I>[P]I−1.
Proof For convenience, we partition Pinto blocks where the upper-left submatrix
is exactly [P]I. We denote A=[P]Ifor simplicity, which gives P=/bracketleftbiggAB
BTC/bracketrightbigg
.B y
matrix inversion lemma, we have
P−1=/bracketleftbiggA−1+A−1B(C−BTA−1B)−1BTA−1−A−1B(C−BTA−1B)−1
−(C−BTA−1B)−1BTA−1(C−BTA−1B)−1/bracketrightbigg
.
By assumption, P−1>0, and thus the diagonal submatrix (C−BTA−1B)−1>0.
In conclusion, we arrive [P−1]I=A−1+A−1B(C−BTA−1B)−1BTA−1>A−1=
[P]−1
I.
Lemma 2 Suppose that P >0and P =/bracketleftbiggAm×mB
BTCn×n/bracketrightbigg
, then P 0,vTPv>0, or
658 T.-K. Chang et al.
(1−c)2xTAx+c2yTCy+c(1−c)yTBTx+c(1−c)xTBy>0.
For arbitrary uT=[xT,yT], we then know that
Q=/bracketleftbigg(1−c)2Ac(1−c)B
c(1−c)BTc2C/bracketrightbigg
>0.
The result follows by P/prime−P=1
c(1−c)JQJ>0, where J=Diag(Im,−In).
References
1. Arambel, P .O., Rago, C., Mehra, R.K.: Covariance intersection algorithm for distributed space-
craft state estimation. In: Proceedings of the 2001 American Control Conference, vol. 6, pp.4398–4403 (2001)
2. Bailey, T., Nieto, J., Guivant, J., Stevens, M., Nebot, E.: Consistency of the EKF-SLAM Algo-
rithm. In: 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.3562–3568 (2006)
3. Carrillo-Arce, L.C., Nerurkar, E.D., Gordillo, J.L., Roumeliotis, S.I.: Decentralized multi-
robot cooperative localization using covariance intersection. In: 2013 IEEE/RSJ InternationalConference on Intelligent Robots and Systems, pp. 1412–1417 (2013)
4. Cattivelli, F.S., Sayed, A.H.: Diffusion strategies for distributed Kalman ﬁltering and smooth-
ing. IEEE Trans. Autom. Control 55(9), 2069–2084 (2010)
5. Chen, L., Arambel, P .O., Mehra, R.K.: Estimation under unknown correlation: Covariance
intersection revisited. IEEE Trans. Autom. Control 47(11), 1879–1882 (2002)
6. Hu, J., Xie, L., Zhang, C.: Diffusion Kalman ﬁltering based on covariance intersection. IEEE
Trans. Signal Process. 60(2), 891–902 (2012)
7. Kailath, T., Sayed, A.H., Hassibi, B.: Linear Estimation. Pearson (2000)
8. Karam, N., Chausse, F., Aufrere, R., Chapuis, R.: Cooperative multi-vehicle localization. In:
2006 IEEE Intelligent V ehicles Symposium, pp. 564–570 (2006)
9. Karam, N., Chausse, F., Aufrere, R., Chapuis, R.: Localization of a group of communicating
vehicles by state exchange. In: 2006 IEEE/RSJ International Conference on Intelligent Robotsand Systems, pp. 519–524 (2006)
10. Kia, S.S., Rounds, S., Martanez, S.: Cooperative localization under message dropouts via a
partially decentralized EKF scheme. In: 2015 IEEE International Conference on Robotics andAutomation, pp. 5977–5982 (2015)
11. Kia, S.S., Rounds, S.F., Martnez, S.: A centralized-equivalent decentralized implementation
of Extended Kalman Filters for cooperative localization. In: 2014 IEEE/RSJ InternationalConference on Intelligent Robots and Systems, pp. 3761–3766 (2014)
12. Leung, K.Y ., Halpern, Y ., Barfoot, T.D., Liu, H.H.: The UTIAS multi-robot cooperative local-
ization and mapping dataset. Int. J. Robot. Res. 30(8), 969–974 (2011)
13. Li, H., Nashashibi, F., Yang, M.: Split covariance intersection ﬁlter: Theory and its application
to vehicle localization. IEEE Trans. Intell. Transp. Syst. 14(4), 1860–1871 (2013)
14. Luft, L., Schubert, T., Roumeliotis, S.I., Burgard, W.: Recursive decentralized collaborative
localization for sparsely communicating robots. In: Proceedings of Robotics: Science andSystems (2016)
15. Mourikis, A.I., Roumeliotis, S.I.: Performance analysis of multirobot cooperative localization.
IEEE Trans. Robot. 22(4), 666–681 (2006)
16. Reinhardt, M., Noack, B., Arambel, P .O., Hanebeck, U.D.: Minimum covariance bounds for
the fusion under unknown correlations. IEEE Signal Process. Lett. 22(9), 1210–1214 (2015)
Multirobot Cooperative Localization Algorithm with Explicit … 659
17. Roumeliotis, S.I., Bekey, G.A.: Distributed multirobot localization. IEEE Trans. Robot. Autom.
18(5), 781–795 (2002)
18. Saeedi, S., Trentini, M., Seto, M., Li, H.: Multiple-robot simultaneous localization and map-
ping: A review. J. Field Robot. 33(1), 3–46 (2016)
Localization in Inconsistent WiFi
Environments
Hsin-Min Cheng and Dezhen Song
1 Introduction
Indoor localization has become more important in recent years as mobile robots and
mobile device users often need to ﬁnd their locations where global positioning system(GPS) signals are unavailable. One low-cost solution is to utilize WiFi signals in the
environment. The number of WiFi access points (APs) has dramatically increased in
the past few years. A large number of APs are temporarily generated by cellphones
and other mobile devices. Moreover, more and more infrastructural APs are equipped
with beamforming capabilities which adjust radiation patterns according to clientlocations. These APs have large variation in their signal ﬁelds. We name those APs as
inconsistent APs. Figure 1shows an example of a WiFi environment with inconsistent
APs which dramatically change received signal strength (RSS) patterns. When theclient cannot interrogate APs for their whereabouts and signal pattern changes, the
existing WiFi localization approaches cannot handle inconsistent WiFi environments
well. Their assumption of small variations in RSS spatial distribution is brokenbecause a majority of APs may be inconsistent.
Building on existing WiFi ﬁngerprinting approach, our method also employs
Gaussian processes (GPs) to establish belief functions from priorly collected WiFireference data. However, our approach utilizes two important designs to handle incon-
sistent APs. First, majority voting is introduced to the initial matching phase which
allows us to develop a statistical hypothesis test to ﬁlter out inconsistent APs that are
This work was supported in part by National Science Foundation under IIS-1318638, NRI-1426752,
NRI-1526200, and NRI-1748161.
H.-M. Cheng ·D. Song (B)
Department of Computer Science and Engineering, Texas A&M University, College Station77843, TX, USAe-mail: dzsong@cse.tamu.edu
H.-M. Cheng
e-mail: hmcheng@cse.tamu.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_47661
662 H.-M. Cheng and D. Song
1
32
456
Xnq X1
Xj1
32
45
6
Xnq X1
Xj
Fig. 1 Example of localization scenarios. Left: APs and their RSS readings at reference collection
time. Right: APs and their RSS readings at localization time. Even though the robot’s trajectory is
identical, the RSS pattern may be very different which leads to unsatisfying localization results
obvious out of places. Second, we use a windowed approach by employing a window
of recent RSS readings along with relative motion information provided by inertial
measurement units (IMU) to develop posterior distribution of location. We formally
derive the conditional distribution and determine the length of the time window by
minimizing Shannon entropy. At last, we apply the maximum likelihood estimation
(MLE) method to obtain reﬁned localization results. We have implemented our algo-
rithm and compared it with the state of the art k-Nearest-Neighbor ( k-NN) approach.
The experimental results show that our method outperforms its counterpart in incon-
sistent WiFi environments. Speciﬁcally, our algorithm achieves a mean localization
error of less than 3.7 m when 70% of APs are inconsistent.
2 Related Work
Our work is related to the simultaneous localization and mapping (SLAM) [ 45]u s i n g
on-board sensors. SLAM using a lidar [ 13] and/or a camera [ 4,7,30–32] can be
more accurate but is computation intensive and suffers from reliability issues and
speciﬁc requirements for environments. WiFi localization has its own advantages
when considering sensor size, power, and cost. Recently, researchers have applied
the graph-SLAM structure using WiFi and other wireless signals [ 10,16,34]. These
methods provide good localization results, but the inconsistent WiFi environments
have not been considered as SLAM in dynamic environments remains a difﬁcult prob-
lem [ 21,54]. SLAM approaches usually assume stationary environment/landmark
locations. The underlying assumption that WiFi APs or RSS patterns can be treated
as stationary landmarks is no longer true under inconsistent WiFi environments. In
fact, inconsistent WiFi environments are highly dynamic instead of just containing
a few moving landmarks in a largely stationary background.
WiFi localization has been a popular research area [ 12,14,29]. We can classify
the existing methods into four types: angle based, time based, RSS modeling based,
Localization in Inconsistent WiFi Environments 663
Ta b l e 1 Overview of WiFi indoor localization methods
Method Hardware cost Known AP
locationsCommunicationsb/w AP andclient System/Solution
Angle based (AoA) High Y Y [11,22,39,50]
Time based
(ToA/TDoA)High Y Y [12,18,26,33,
46]
RSS modeling based Low Y N [1,6,19,20,27,
40,42,43,48]
Fingerprint based Low N N [2,3,10,14,15,
23,28,35,36,
38,47,49]
and ﬁngerprinting based approaches (see Table 1). Angle based approaches use AP
with multiple antennas to compute Angle of Arrival (AoA) of the multi-path sig-
nals received at each AP and localize through triangulation. Time based approaches
include time of arrival (ToA) or time difference of arrival (TDoA) relies on the propa-gation time of signals traveling from a transmitter to a receiver. However, precise time
synchronization is required which is not available using commodity WiFi. Both the
two methods relies on known AP locations, and the client needs to interrogate APsfor their locations or precise synchronization. Although more accurate in general,
these two methods bear high cost in infrastructure and are difﬁcult to be deployed.
The RSS modeling based approaches model WiFi RSS signal propagation in thespace assuming known AP locations, which have advantages of low cost and easy
deployment. These methods often suffer from low accuracy because signal attenu-
ation is often complex and hard to be predicted. Fingerprinting based approaches
require priorly-mapped WiFi RSSs in the working space to construct a database
for localization purpose which does not require communication between APs andthe client or known AP locations. Our approach inherits the beneﬁts with focus on
addressing the dynamic signal pattern issues.
In order to improve WiFi localization accuracy, auxiliary sensors are combined
into the above approaches, such as cameras, which are used to recognize landmarks,
and IMUs for motion estimation [ 3,17,25,37]. The sensor conﬁguration in our
approach is the same as the latter. Existing work in the IMU-assisted WiFi localizationsystems [ 8,53] localize pedestrians by utilizing step count information to mitigate
IMU drifting issue. However, the step count information is not available for robots
and these methods have not explicitly consider inconsistent WiFi environments.
To deal with uncertainties in WiFi environment, existing works detect outliers
using k-NN [ 24,41], penalize RSS readings from uncertain APs, and signify APs
with strong RSS readings in the k-NN method. Laoudias et al. [ 24] assume the
fault model of AP with on and off status and set threshold on sum of distance of k-
NN method to mitigate errors from faulty APs. These works have demonstrated the
advantages of removing outliers. However, they use only RSS at the current moment
664 H.-M. Cheng and D. Song
which may be challenging for moving users who experience signal ﬂuctuations.
We take a windowed approach by using sequence matching which greatly improves
the robustness to inconsistent APs. AP selection is not a new technique in WiFilocalization. However, the main focus is to reduce the computation cost and improve
accuracy instead of handling inconsistent WiFi environments. Existing approaches
choose APs with the highest RSS observation [ 52] or select APs based on entropy-
based information gain criterion [ 5]. They cannot be guaranteed the selected APs are
reliable in inconsistent WiFi environments due to different design purposes.
3 Problem Formulation
3.1 Application Scenario and Assumptions
Our system extends the aforementioned ﬁngerprinting methods, which assume that
there is a mobile robot equipped with SLAM ability to pre-scan the environment toestablish a database of WiFi RSS readings and their corresponding listening locations.
The database is referred to as the WiFi reference data thereafter. It is clear that the
reference data contain inconsistent AP RSSs.
At client side, the robot also perceives WiFi signal strengths and accumulates
them over time which are referred to as WiFi query data. It is true that WiFi query
data may also contain noisy data from inconsistent APs. The focus here is the clientside localization problem in the presence of inconsistent AP signals in both query
and reference data. Each localization client is equipped with an IMU. The client may
be a mobile robot or a cellphone user. To focus on the most relevant issues, we havethe following assumptions:
1. WiFi and IMU readings have been synchronized and time-stamped.
2. The RSS reception noises are Gaussian and IMU signal noises are white.
It is also worthing noting that we do not assume that we start with a known ini-
tial position and hence our localization is a global location problem instead of an
incremental localization problem.
3.2 Notations and Problem Deﬁnition
Common notations are deﬁned as follows,
•Reference data: D={(zr,i,Xr,i)|i=1,..., n}is composed of ninput-output pairs
where iis the index variable, Xr,iis the position in 2 Dor 3 DCartesian coordinate
system, and vector zr,icontains the WiFi RSS readings at Xr,i. Subscript rrefers
to the reference data. zr,i’s dimension is determined by number of APs in the
Localization in Inconsistent WiFi Environments 665
environment. Let us deﬁne AP index m∈Mwhere M:= { 1,..., mmax}is the AP
index set. There are mmax=| M|distinct APs.
•Query data: subscript qrefers to query data. Let jbe the time index where j≥0.
V ector zq,jcontains all WiFi RSS readings at time j. The sequence of WiFi query
data from the beginning to time jis denoted as
zq,0:j={ zq,0,..., zq,j}. (1)
•IMU readings from the beginning to time jare denoted as a0:jandω0:jfor accel-
erations and angular velocities, respectively. Note that a0:jandω0:jcontain a lot
more entries than that of query set in ( 1) due to high sampling frequency.
With important notations deﬁned, let us formulate the localization problem,
Problem 1 Given D,zq,0:j,a0:j, andω0:jat time j, estimate location Xj.
4 System Design and Algorithm
Our system architecture is illustrated in Fig. 2which contains four main blocks shaded
in gray: (1) Reference pre-processing where we construct RSS-location belief modelusing the reference data, (2) Query pre-processing where we match a time window of
query APs with reference data and reconstruct the corresponding window of relative
motion of the client using IMU inputs, (3) Query AP selection where we matchquery WiFi with those in the reference data to remove unmatched query APs, and
(4) Location estimation where we fuse the recent relative motion with historical RSS
to localize the client and determine the optimal window size for next time frame.We begin with reference pre-processing (Box 1 of Fig. 2) where we construct an
RSS-location belief model from the WiFi reference data using GPs.
4.1 Reference Pre-processing
Let Xr,ibeith location where reference data is collected and zr,i,mbe the perceived
RSS for the mth AP at this location. Xr,ican be either 2D or 3D depending on
the environment. Let Xr:= [ Xr,1,..., Xr,n]Tbe a location matrix containing all
collection locations in the reference data D. Deﬁne zr∗,m:= [ zr,1,m..., zr,n,m]as
all RSSs for the mth AP in the reference data. Then the combined data set Dm:=
{zr∗,m,Xr}be the training set of the mth AP to instantiate a GP to characterize a
regression model fm(·)between Xrand zr∗,m. For each element in zr∗,m,
zr,i,m=fm(Xr,i)+ε, (2)
666 H.-M. Cheng and D. Song
Fig. 2 System Diagram: the light gray region needs only one time computation and all the dark
gray regions are computed for each frame
where ε∼N(0,σ2
nm)is the observation noise and σ2
nmis the variance. For two
different locations Xr,iand Xr,i/prime, we employ the kernel function km(·)in GP to
characterize the correlation between their function values, namely, fm(Xr,i)and
fm(Xr,i/prime):
km(Xr,i,Xr,i/prime)=σ2
fmexp/parenleftbigg
−1
2λ2m|Xr,i−Xr,i/prime|2/parenrightbigg
, (3)
where σ2
fmis the signal variance and λmis the length scale. The covariance matrix
Kmis an n×nmatrix with the (i,i/prime)th element Km(i,i/prime)=km(Xr,i,Xr,i/prime)where
i,i/prime∈{1,..., n}.F o rt h e mth AP , the predicted function value ˜zfor an arbitrary
location Xaconditioned on Xrand zr∗,mis
˜z=KT
a(Km+σ2
nmI)−1zr∗,m, (4)
where Iis an n-dimensional identity matrix and Kais an n×1 vector which cap-
tures the relationship between Xaand Xrusing ( 3),Ka(i,1)=km(Xr,i,Xa)where
i∈{1,..., n}. The values of parameters σ2
fm,σ2
nmandλmare learned using hyperpa-
Localization in Inconsistent WiFi Environments 667
rameter estimation mentioned in [ 10]. Since GP is a zero mean process, we subtract
the mean of zr∗,mbefore training and add it back after to get the required ˜z.
We can do this for each AP and hence we can obtain a location belief model based
on all APs in the reference data.
4.2 Query Pre-processing
Before we match query data with the reference data, we need to remove query
APs that do not exist in the reference data. It is clear that their RSSs would not
assist localization. Subsequently, we reorganize zq,jin (1)t o zjby trimming out the
useless RSSs. Deﬁne the surviving AP index set as M j. Similarly, we also update
the historic query data set z0:j:= { z0,..., zj}from zq,0:j. In fact, it is not necessary
to employ the entire historic query data set for localization computation. We onlyneed to backtrack a window of length wreadings, which allow us to establish the
windowed query data set z
j−w:jfrom time j−wto time j. We will discuss how to
determine the optimal window length in Sect. 4.5.
Next, we associate the relative motion from time j−wtojfor the query data
zj−w:j. Although we do not know the absolute position, we can utilize IMU data to
obtain the relative motion in the time window. We deﬁne ΔXa:b=Xb−Xaas the
travel distance between time aand time b. To get ΔXa:b, we employ an EKF-based
approach for IMUs [ 9,44,51]. From the EKF, ΔXa:bfollows Gaussian distribu-
tion with a mean of ΔXa:band a covariance of ΣΔXa:b. Deﬁne the relative motion
setΔXj−w:j:= {ΔXj−k+1:j−k|k=1,···,w}, which captures the relative motion
within the time window.
4.3 Query AP Selection
With zj−w:jandΔXj−w:jobtained, we can perform statistical testing to remove
inconsistent APs which refers to APs that may have changed locations or have sig-
niﬁcant signal strength changes. If we do not remove inconsistent APs, localization
results may suffer. In fact, we can visualize this issue. We can obtain posterior dis-tribution of X
jdirectly using GPs in ( 4) based on zjwhich is shown in Fig. 3a. A
desirable outcome is supposed to be a unimodal distribution with its peak close to or
overlapping with the actual location. Unfortunately, the inconsistent APs have lead
to a multimodal distribution and the ground truth position does not correspond to a
peak position which indicated localization would fail if such belief model is naivelyused.
668 H.-M. Cheng and D. Song
Fig. 3 Sample cases of posterior condition probability of robot/client location. The color changing
from yellow to blue corresponds to high to low probability regions, respectively. The red staris the ground truth location. The x-axis and y-axis that span horizontal plane are 2D Cartesian
coordinates, and vertical z-axis represents probability. aDirectly computed from GPs without
removing inconsistent APs and using windowed inputs. bAfter inconsistent AP removal. cAfter
inconsistent AP removal and location reﬁnement
4.3.1 Initial Localization Using Majority V oting
To remove inconsistent APs, we perform an initial low resolution localization by
simply assuming current location Xjis co-located at each location Xr,iin reference
set and then we can compare the sequence of RSSs. Relative motion set ΔXj−w:j
allow us to generate assumed reference location information ˜Xr,i={ Xr,i}∪{ Xr,i−/summationtextk
p=1ΔXj−p:j−p+1|k=1,···,w}for the entire window. Plug ˜Xr,iinto ( 4), we
obtain the assumed reference RSSs ˜zr,i. Note that ˜zr,iis of the same length as zj−w:j.
Now we ﬁnd the most possible location by matching query zj−w:jto reference ˜zr,i.
We ﬁnd best match location from initial location candidates using majority voting
over best location candidates identiﬁed by each AP .
For the mth AP at time j−k, its query entry in zj−w:jiszj−k,mand the cor-
responding reference entry in ˜zr,iis˜zi,m,k. The matching metric function fEis the
summation of the squared l2−norm over the window,
Localization in Inconsistent WiFi Environments 669
fE(m,i)=w/summationdisplay
k=0(˜zi,m,k−zj−k,m)2. (5)
The mth AP can predict best location imby comparing fEvalues over the entire
reference set
im=arg min
i∈{...n}fE(m,i).
Combining the best solutions for all APs, we have a candidate solution set Im:=
{im|m=1,... mmax}. We employ majority voting to ﬁnd the most agreed location
index i∗
jas the solution. Speciﬁcally, we deﬁne a binary ballot function bb:
bb(im,i)=/braceleftBigg
1,i=im,
0,otherwise .
The location with the most votes wins,
i∗
j=arg max
i∈Immmax/summationdisplay
m=1bb(im,i). (6)
Now we know that the actual location is close to the location at reference index i∗
j.
This information can be exploited to ﬁlter out inconsistent APs.
4.3.2 Inconsistent AP Removal
We develop statistical hypothesis testing to remove inconsistent APs. To perform the
statistics testing, we begin with analyzing fE(m,i∗
j)as a distribution. We would like
to derive the probability that fE(m,i∗
j)is abnormally large. For the mth AP , have
two hypotheses:
H0:The mth AP is an inconsistent AP vs. H1:The mth AP is a consistent AP
The signiﬁcance level is α. We reject H0ifp−value is less than α.
Let˜z∗
m,kbe the reference entry for the mth AP corresponds to location i∗
j.
From the GP model, we know (˜z∗
m,k−zj−k,m)/σ nm∼N(0,1)is a random vari-
able following the normal distribution with zero mean and a variance of 1. Weknow that f
E(m,i∗
j)/σ2
nmis the sum of squares of multiple normal distributions
according to ( 5),fE(m,i∗
j)/σ2
nmhas to follow χ2-distribution with w+1 degrees of
freedom. Its cumulative probability function is F(x,w+1)=γ(( w+1)/2,x/2)
/Gamma1(( w+1)/2)
where γ(a,x)=/integraltextx
0ta−1e−tdtand/Gamma1(a)=/integraltext∞
0ta−1e−tdt. The probability of a value
fromχ2-distribution distribution is larger than xis
670 H.-M. Cheng and D. Song
P{fE(m,i∗
j)/σ2
nm≥x}= 1−F(x,w+1)=α. (7)
Setting F(x,w+1)=1−αallows us to ﬁnd threshold x=F−1(1−α)where
F−1(·)is the quantile function deﬁned as F−1(a)=inf{y:F(y)≥a}. Thus we
reject H0iffE(m,i∗
j)≤σ2
nmF−1(1−α).After the statistical testing, we remove
inconsistent APs. The remaining AP index set is deﬁned as M∗
j.W et r i m zj−w:j
accordingly. Figure 3b illustrates how inconsistent AP removal help reshape the pos-
terior location distribution which has its highest peak closer to the actual AP location.However, inconsistent AP removal cannot distinguish APs which change their posi-
tions slightly. Therefore, we still have a multi-modal distribution with many peaks,
which limits the localization accuracy. To handle this issue, we introduce localizationreﬁnement.
4.4 Location Reﬁnement Using Windowed Sequence
Our idea is to derive the posterior probability of Xjfor present time jgiven RSS read-
ings zj−w:jand relative motion information ΔXj−w:jand then apply MLE method
to obtain the location estimation (Fig. 2Box 4).
Let P(Xj|zj−w:j,ΔXj−w:j)be the posterior probability of Xjgiven zj−w:jand
ΔXj−w:j. With ΔXj−w:j, the robot/client position Xj−kcan be obtained (see Fig. 4),
Xj−k=Xj−ΔXj−k=Xj−k/summationdisplay
p=1ΔXj−p+1:j−p. (8)
Assuming Xj=xj, then the conditional distribution of Xj−kgiven ( xj,ΔXj−k)
is,
Xj−k|xj,Δ Xj−k=xj−ΔXj−k, (9)
where operator ‘ ·|·’ represents the condition for a random variable with conditions
at the right side of ‘ |’. Since ΔXj−kis obtained from EKF based on IMU inputs,
it follows Gaussian distribution with a mean of ΔXj−k, and a covariance matrix of
ΣΔXj−k:
Xj−k|xj,Δ Xj−k∼N(xj−ΔXj−k,ΣΔXj−k). (10)
Now let us derive the posterior probability using Bayes theorem,
P(Xj|zj−w:j,ΔXj−w:j)=P(Xj,zj−w:j,ΔXj−w:j)
P(zj−w:j,ΔXj−w:j). (11)
We decompose P(Xj,zj−w:j,ΔXj−w:j)by taking zjto the front,
Localization in Inconsistent WiFi Environments 671
Fig. 4 Relationships between locations, displacements, and RSS readings in the window of prior
locations and RSSs. Each prior location can be associated to its RSS readings using GPs
P(Xj,zj−w:j,ΔXj−w:j) (12)
=P(zj|Xj,ΔXj−w:j,zj−w:j−1)P(Xj,ΔXj−w:j,zj−w:j−1)
=P(zj|Xj)P(Xj,ΔXj−w:j,zj−w:j−1).
This is because the current observation zjonly depends on the current location.
Again, we decompose P(Xj,ΔXj−w:j,zj−w:j−1)
P(Xj,ΔXj−w:j,zj−w:j−1) (13)
=P(zj−1|Xj,ΔXj−w:j,zj−w:j−2)P(Xj,ΔXj−w:j,zj−w:j−2)
=P(zj−1|Xj−1=Xj−ΔXj,j−1)P(Xj,ΔXj−w:j,zj−w:j−2).
This is because zj−1is independent of zj−w;j−2given Xj−1.
For P(Xj,ΔXj−w:j,zj−w:j−2),w eh a v e
P(Xj,ΔXj−w:j,zj−w:j−2) (14)
=P(zj−2|Xj,ΔXj−w:j,zj−w:j−3)P(Xj,ΔXj−w:j,zj−w:j−3)
=P⎛
⎝zj−2|Xj−2=Xj−2/summationdisplay
p=1ΔXj−p+1,j−p⎞
⎠P(Xj,ΔXj−w:j,zj−w:j−3).
Xj−2and Xjare related using ( 8). By decomposing P(Xj|zj−w:j,ΔXj−w:j)itera-
tively, we rewrite ( 11)a s
P(Xj|zj−w:j,ΔXj−w:j)=P⎛
⎝zj|Xj)w/productdisplay
k=1P(zj−k|Xj−k=Xj−k/summationdisplay
p=1ΔXj−p+1,j−p⎞
⎠
P(zj−w:j,ΔXj−w:j).(15)
We integrate displacement ΔXj−k:jfor each term:
672 H.-M. Cheng and D. Song
P⎛
⎝zj−k|Xj−k=Xj−k/summationdisplay
p=1ΔXj−p+1,j−p⎞
⎠=/integraldisplay
P(zj−k|xj−k)f(Δxj−k)d(Δxj−k),(16)
where f(Δxj−k)is a Gaussian distribution function ( 10) obtained from EKF result.
Under the GP model, the probability distribution for the mth AP’s RSS conditioned
on Xj=xjis
zj,m|Xj∼N(μ m(xj,D),Σ m(xj,D)), (17)
where μm(xj,D)=kT
Xj(Km+σ2
nmI)−1zT
r,∗mandΣm(xj,D)=k(xj,xj)−kT
Xj
(Km+σ2
nmI)−1kXjwith k(xj,xj)=σ2
fmobtained from ( 3). Since RSSs of all APs
are independent, we have
f(zj|Xj=xj)=/productdisplay
m∈M∗
jf(zj,m|Xj=xj). (18)
Similarly, we can obtain a probability distribution function for zj−k|Xj−k. With
results from ( 16) and ( 18) and the fact that zj−w:jandΔXj−w:jare independent, we
can compute ( 15) to obtain P(Xj|zj−w:j,ΔXj−w:j). Figure 3c illustrates the posterior
distribution for the windowed inputs which appears as a unimodal distribution with
its peak located close to the actual location. This signiﬁcantly increases the accuracy
of the localization algorithm.
Finally, we estimate Xjby applying MLE to the posterior probability in ( 15),
ˆXj=arg max
XjP(zj|Xj)/integraldisplay w/productdisplay
k=1P⎛
⎝zj−k|Xj−k=Xj−k/summationdisplay
p=1ΔXj−p+1,j−p⎞
⎠d(Δxj−k).
(19)
Note that we dropped the denominator in ( 15) because it is not a function of Xj.
4.5 Determine Optimal Window Size
The remaining issue is how to determine the optimal window size wof RSS sequence.
It is worth noting that increasing window size wsigniﬁcantly increases computation
load. Also, the relative motion information ΔXj−w:jhas its variance increasing over
time due to IMU drifting and eventually becomes useless due to its large spatial
uncertainty. To choose an appropriate window size, we minimize the Shannon entropy
over window size. Deﬁne Aas a latticed set of the localization space. The lattice
resolution is 0.1 meters in each dimension in our settings. Deﬁne H(w,j)as the
Shannon entropy over the probability distribution P(Xj|zj−w:j,ΔXj−w:j),
H(w,j)=−/summationdisplay
Xj∈AP(Xj|zj−w:j,ΔXj−w:j)log P(Xj|zj−w:j,ΔXj−w:j).
Localization in Inconsistent WiFi Environments 673
We choose the optimal wthat minimizes H(w,j)over all possible window length
set w:= { 0,1,..., wmax}where wmaxis the maximum allowable window size that
covers the entire A. Then we ﬁnd the optimal solution w∗,
w∗=arg min
w∈wH(w,j). (20)
The resulting w∗will be used as the window size for time frame j+1.
5 Experiments
We have implemented our algorithm using MA TLAB under a PC with Windows
7. We evaluate our method using real world data from physical experiments. Three
algorithms are compared in our experiment.
•MLE-S: Our complete method including both AP removal and MLE reﬁnement,
•MLE-NS: our algorithm without AP removal in Sect. 4.3.2 . We test this variant of
our method to show the beneﬁt of inconsistent AP removal, and
•k-NN [ 17]: This method is chosen as the counterpart because it has ability to
handle some degrees of inconsistent WiFi environments despite it only targets at
handling RSS ﬂuctuations. It also employs IMU to assist WiFi localization. For
brevity, we name it as k-NN method here.
Dataset : We have collected datasets from three different buildings: H. R. Bright
building (HRBB), Scoates Hall (SCTS), and Wisenbaker Engineering Building
(WEB) at Texas A&M University using a Nexus 7 tablet and a mobile robot (see
Fig. 5). Our robot is equipped with a Hokuyo UTM-30LX lidar and provides 2D lidar
map (see Fig. 5b) with timestamps. The location from lidar-based SLAM is used as
a ground truth with an error of less than 10 cm. The dataset consists of WiFi RSS
readings collected at 1 Hz and IMU readings at 100 Hz. Table 2describes details of
each dataset including the overall trajectory, the number of APs in reference data,
and query data. We have collected the reference data and query data in different days.
Nexus 7
tablet
lidar(a) (b) (c) (d)
Fig. 5 a Our data collection robot and sensor conﬁguration. 2D lidar maps are used as ground
truth: bHRBB, cSCTS, and dWEB
674 H.-M. Cheng and D. Song
Ta b l e 2 Dataset description
Building HRBB SCTS WEB
Trajectory (meters) 96 50 80
Reference data # of AP 252 132 159
Query data AP # of AP 246 135 171
Shared APs between two data sets 195 130 158
Ta b l e 3 Mean localization error (meters)
Dataset Method Inconsistent APs
0% 10% 20% 30% 40% 50% 60% 70% 80% 90%
HRBB k-NN 3.3 3.5 3.8 4.2 4.4 5.5 6.5 8.6 9.7 15.1
MLE-NS 3.2 3.4 3.9 4.2 4.3 5.6 7.0 6.7 7.3 10.0
MLE-S 3.2 3.2 3.2 3.2 3.2 3.2 3.2 3.5 4.6 9.1
SCTC k-NN 1.6 1.8 1.9 2.6 3.0 3.8 5.8 7.1 8.0 10.4
MLE-NS 2.1 2.1 2.1 2.6 2.9 3.6 6.9 6.6 6.4 6.4
MLE-S 1.7 1.6 1.7 1.7 2.1 2.0 2.4 3.1 4.9 10.1
WSB k-NN 1.2 1.3 1.3 1.5 2.1 2.9 6.2 8.9 12.7 16.3
MLE-NS 1.6 1.5 1.6 1.7 1.9 2.3 4.4 6.4 8.9 14.5
MLE-S 1.4 1.5 1.4 1.4 1.4 1.6 2.3 3.7 10.3 19.4
Therefore, the two data sets do not share the exact same number of APs. The shared
AP number are shown in the last row of the Table 2.
Evaluation Metric : The localization error at time jis deﬁned as the Euclidean
distance between the estimation ˆXjand ground truth Xg,j:εj=| |ˆXj−Xg,j||.T h e
error is measured in meters.
Table 3shown the test results. The best results are highlighted in bold fonts. We
have tested all three algorithms under different percentage of inconsistent APs rang-
ing from 0 to 90% in the environment. To generate inconsistent APs, we ﬁrst usesystematic random sampling on the shared APs between reference data and query
data and shift AP RSS patterns randomly to form inconsistent APs. It is clear that our
method is more robust than the counterpart under inconsistent WiFi environments.Also, the inconsistent AP removal process does help in maintain consistent localiza-
tion accuracy up to 70% inconsistent APs. Our methods achieve the best or close to
the best mean localization error as long as the inconsistent AP ratio is less than 70%.At 80 or 90% inconsistent AP ratio, our proposed method does not work well, this
is because signal to noise ratio is too low and inconsistent AP removal process may
fail.
Figure 6shows the effectiveness of inconsistent AP removal scheme by illustrating
true positive (TP) rates, which is deﬁned as
Localization in Inconsistent WiFi Environments 675
Fig. 6 Average TP rates of
selected APs after the AP
removal process
10 20 30 40 50 60 70 80 90
Inconsistent AP percentage (%)0.10.20.30.40.50.60.70.80.91Average TPHRBB
SCTS
WSB
TP=#consistent AP selected
#All selected APs,
over different inconsistent AP ratios. Our method ensures that TP rates remain above
0.5 under 70% inconsistent APs environment among different datasets with a mean
localization error less than 3.7 m.
The last evaluation is runtime of our approach. We report the average runtime for
each time frame are 0 .30 s on average for SCTS dataset, 0 .89 s for HRBB dataset, and
0.63 s for WEB dataset. From alogithm analysis, the most time consuming operation
is computing location posterior probability which is proportional to the number of
APs (Table 2). Our runtime result conﬁrm it. Recall that the time interval for each
time frame is 1 s in experiment set up, the runtime for localization is tolerable.
6 Conclusion and Future Work
We reported a WiFi-based localization method designed to handle inconsistent WiFi
environments where mobile APs and APs with beamforming capabilities cause sig-
niﬁcant changes in radiation patterns in RSSs. Building on the WiFi ﬁngerpint-
ing method that utilizes GPs to establish a belief model from reference data, our
method employed majority voting along with embedded statistical hypothesis test-
ing to remove inconsistent APs. Instead of using RSS pattern at a single time instance,
we used a historical sequence to further improve the robustness to inconsistent APs.
We derived a posterior location distribution function for the sequence and applied
MLE to reﬁne localization results. Our method was tested and compared to an exist-
ing method. The results showed that our method outperforms the counterpart and our
676 H.-M. Cheng and D. Song
design is effective. In the future, we will conduct more experiments and comparison
studies. We will also focus on analysis and improving computation speed.
Acknowledgements We would like to thank C. Chou, B. Li, S. Yeh, A. Kingery, A. Angert,
Y . Sun, M. Jin, D. Wang, Y . Y ou, M. Momin, T. Sun, and H. Li for their input and contributions tothe NetBot Lab at Texas A&M University.
References
1. Bahl, P ., Padmanabhan, V .N.: RADAR: an in-building RF-based user location and tracking
system. In: Proceedings of the IEEE International Conference on Computer Communica-tions(INFOCOM), Tel-Aviv, Israel (2000)
2. Balaguer, B., Erinc, G., Carpin, S.: Combining classiﬁcation and regression for wiﬁ localization
of heterogeneous robot teams in unknown environments. In: The 2012 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS) (2012)
3. Biswas, J., V eloso, M.: WiFi localization and navigation for autonomous indoor mobile robots.
In: IEEE International Conference on Robotics and Automation (ICRA), Anchorage, USA(2010)
4. Carlevaris-Bianco, N., Eustice, R.M.: Learning visual feature descriptors for dynamic lighting
conditions. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS),pp. 2769–2776 (2014)
5. Chen, Y ., Yang, Q., Yin, J., Chai, X.: Power-efﬁcient access-point selection for indoor location
estimation. IEEE Trans. Knowl. Data Eng. 18(7), 877–888 (2006). July
6. Chintalapudi, K., Padmanabha Iyer, A., Padmanabhan, V .N.: Indoor localization without the
pain. In: Proceedings of the Sixteenth Annual International Conference on Mobile Computingand Networking (MobiCom) (2010)
7. Civera, J., Grasa, O.G., Davison, A.J., Montiel, J.M.M.: 1-point RANSAC for extended Kalman
ﬁltering: application to real-time structure from motion and visual odometry. J. Field Robot.27(5), 609–631 (2010)
8. Deng, Z.-A., Ying, H., Jianguo, Y ., Na, Z.: Extended kalman ﬁlter for real time indoor local-
ization by fusing wiﬁ and smartphone inertial sensors. Micromachines 6(4), 523–543 (2015)
9. Dissanayake, G., Sukkarieh, S., Nebot, E., Durrant-Whyte, H.: The aiding of a low-cost strap-
down inertial measurement unit using vehicle model constraints for land vehicle applications.IEEE Trans. Robot. Autom. 17(5), 731–747 (2001). Oct
10. Ferris, B., Fox, D., Lawrence, N.: WiFi-SLAM using Gaussian process latent variable models.
In: Proceedings of the 20th International Joint Conference on Artiﬁcial Intelligence, Hyderabad,India (2007)
11. Gjengset, J., Xiong, J., McPhillips, G., Jamieson, K.: Phaser: enabling phased array signal
processing on commodity wiﬁ access points. In: Proceedings of the 20th Annual InternationalConference on Mobile Computing and Networking, pp. 153–164. ACM (2014)
12. Guvenc, I., Chong, C-C.: A survey on toa based wireless localization and nlos mitigation
techniques. IEEE Commun. Surv. Tutor. 11(3) (2009)
13. Hahnel, D., Burgard, W., Fox, D., Thrun, S.: An efﬁcient FastSLAM algorithm for generating
maps of large-scale cyclic environments from raw laser range measurements. IEEE/RSJ Int.Conf. Intell. Robots Syst. (IROS) 1, 206–211 (2003)
14. He, S., Chan, S.H.G.: Wi-ﬁ ﬁngerprint-based indoor positioning: recent advances and compar-
isons. IEEE Commun. Surv. Tutor. 18(1), 466–490 (2016)
15. Howard, A., Siddiqi, S., Sukhatme, G.S.: An experimental study of localization using wireless
ethernet. In: Field and Service Robotics, pp. 145–153. Springer (2003)
Localization in Inconsistent WiFi Environments 677
16. Huang, J., Millman, D., Quigley, M., Stavens, D., Thrun, S., Aggarwal, A.: Efﬁcient, general-
ized indoor wiﬁ graphslam. In: IEEE International Conference on Robotics and Automation(ICRA), Shanghai, China, pp. 1038–1043 (2011)
17. Jin, M., Koo, B., Lee, S., Park, C., Lee, M.J., Kim, S.: IMU-assisted nearest neighbor selection
for real-time WiFi ﬁngerprinting positioning. In: International Conference onIndoor Position-ing and Indoor Navigation (IPIN), Zurich, Switzerland (2014)
18. Kiran Raj Joshi: Steven Siying Hong, and Sachin Katti. Localizing interfering radios. In NSDI.
USENIX, Pinpoint (2013)
19. Kim, C., Song, D., Xu, Y ., Yi, J., Wu, X.: Cooperative search of multiple unknown transient radio
sources using multiple paired mobile robots. IEEE Trans. Robot. 30(5), 1161–1173 (2014).
Oct
20. Kim, C., Song, D., Yi, J., Wu, X.: Decentralized searching of multiple unknown and transient
radio sources with paired robots. Engineering 1(1), 058–065 (2015)
21. Kim, D.H., Kim, J.H.: Effective background model-based rgb-d dense visual odometry in a
dynamic environment. IEEE Trans. Robot. 32(6), 1565–1573 (2016). Dec
22. Kotaru, M., Joshi, K., Bharadia, D., Katti, S.: Spotﬁ: decimeter level localization using wiﬁ.
SIGCOMM Comput. Commun. Rev. 45(4), 269–282 (2015). Aug
23. Ladd, A.M., Bekris, K.E., Rudys, A., Kavraki, L.E., Wallach, D.S.: Robotics-based location
sensing using wireless ethernet. Wirel. Netw. 11(1–2), 189–204 (2005)
24. Laoudias, C., Michaelides, M.P ., Panayiotou, C.G.: Fault detection and mitigation in wlan rss
ﬁngerprint-based positioning. J. Locat. Based Serv. 6(2), 101–116 (2012)
25. Li, W.W.L., Iltis, R.A., Win, M.Z.: A smartphone localization algorithm using rssi and inertial
sensor measurement fusion. In: 2013 IEEE Global Communications Conference (GLOBE-COM) (2013)
26. Li, X., Pahlavan, K.: Super-resolution toa estimation with diversity for indoor geolocation.
IEEE Trans. Wirel. Commun. 3(1), 224–234 (2004)
27. Lim, H., Kung, L-C., Hou, J.C., Luo, H.: Zero-conﬁguration indoor localization over IEEE
802.11 wireless infrastructure. Wirel Netw 16(2) (2010)
28. Liu, H., Gan, Y ., Yang, J., Sidhom, S., Wang, Y ., Chen, Y ., Ye, F.: Push the limit of wiﬁ based
localization for smartphones. In: Proceedings of the 18th Annual International Conference onMobile Computing and Networking (MobiCom) (2012)
29. Liu, H., Darabi, H., Banerjee, P ., Liu, J.: Survey of wireless indoor positioning techniques and
systems. IEEE Trans. Syst. Man Cybern. Part C (Appl. Rev.) 37(6), 1067–1080 (2007)
30. Lu, Y ., Song, D.: Visual navigation using heterogeneous landmarks and unsupervised geometric
constraints. In: IEEE Transactions on Robotics (T-RO), vol. 31, pp. 736–749 (2015)
31. Lu, Y ., Song, D.: Robust RGB-D odometry using point and line features. In: IEEE International
Conference on Computer Vision (ICCV), Santiago, Chile (2015)
32. Majdik, A., Gálvez-López, D., Lazea, G., Castellanos, J.A.: Adaptive appearance based loop-
closing in heterogeneous environments. In: IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), pp. 1256–1263 (2011)
33. Mariakakis, A.T., Sen, S., Lee, J., Kim, K-H.: Sail: single access point-based indoor localization.
In: Proceedings of the 12th Annual International Conference on Mobile Systems, Applications,and Services, pp. 315–328. ACM (2014)
34. Mirowski, P ., Ho, T.K., Yi, S., MacDonald, M.: Signalslam: simultaneous localization and
mapping with mixed wiﬁ, bluetooth, lte and magnetic signals. In: International Conference onIndoor Positioning and Indoor Navigation, pp. 1–10 (2013)
35. Nandakumar, R., Chintalapudi, K.K., Padmanabhan, V .N.: Centaur: locating devices in an
ofﬁce environment. In: Proceedings of the 18th Annual International Conference on MobileComputing and Networking (2012)
36. Ocana, M., Bergasa, L.M., Sotelo, M.A., Nuevo, J., Flores, R.: Indoor robot localization system
using wiﬁ signal measure and minimizing calibration effort. In: Proceedings of the IEEEInternational Symposium on Industrial Electronics, Dubrovnik, Croatia (2005)
37. Quigley, M., Stavens, D., Coates, A., Thrun, S., Sub-meter indoor localization in unmodi-
ﬁed environments with inexpensive sensors. In: The IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), p. 2010. Taipei, Taiwan (2010)
678 H.-M. Cheng and D. Song
38. Saha, S., Chaudhuri, K., Sanghi, D., Bhagwat, P .: Location determination of a mobile device
using IEEE 802.11b access point signals. In: IEEE Wireless Communications and NetworkingConference(WCNC), New Orleans, LA (2003)
39. Sen, S., Lee, J., Kim, K-H., Congdon, P .: Avoiding multipath to revive inbuilding wiﬁ localiza-
tion. In: Proceeding of the 11th Annual International Conference on Mobile Systems, Appli-cations, and Services, MobiSys ’13. ACM (2013)
40. Serrano, O., Marıa Canas, J., Matellán, V ., Rodero, L.: Robot localization using WiFi signal
without intensity map, In: International Workshop on Algorithmic Foundationsof Robotics(W AFR), Utrecht/Zeist, The Netherlands (2004)
41. So, J., Lee, J.-Y ., Y oon, C.-H., Park, H.: An improved location estimation method for wiﬁ
ﬁngerprint-based indoor localization. Int. J. Softw. Eng. Appl. 7(3), 77–86 (2013)
42. Song, D., Kim, C., Yi, J.: Simultaneous localization of multiple unknown CSMA-based wireless
sensor network nodes using a mobile robot with a directional antenna. J. Intell. Serv. Robots2(4), 219–233 (2009). Oct
43. Song, D., Kim, C., Yi, J.: Simultaneous localization of multiple unknown and transient radio
sources using a mobile robot. IEEE Trans. Robot. 28(3), 668–680 (2012). June
44. Sukkarieh, S., Gibbens, P ., Grocholsky, B., Willis, K., Durrant-Whyte, H.F. A low-cost, redun-
dant inertial measurement unit for unmanned air vehicles. Int. J. Robot. Res. 19(11), 1089–1103
(2000)
45. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press (2005)46. V asisht, D., Kumar, S., Katabi, D.: Decimeter-level localization with a single wiﬁ access point.
In: NSDI (2016)
47. Wang, H., Sen, S., Elgohary, A., Farid, M., Y oussef, M., Roy Choudhury, R.: No need to war-
drive: Unsupervised indoor localization. In: Proceedings of the 10th International Conferenceon Mobile Systems, Applications, and Services, MobiSys ’12 (2012)
48. Whitehouse, K., Karlof, C., Culler, D.: A practical evaluation of radio signal strength for
ranging-based localization. SIGMOBILE Mob. Comput. Commun. Rev. 11(1) (2007)
49. Wu, C., Yang, Z., Liu, Y .: Smartphones based crowdsourcing for indoor localization. IEEE
Trans. Mob. Comput. 14(2), 444–457 (2015). Feb
50. Xiong, J., Jamieson, K.: Arraytrack: a ﬁne-grained indoor location system. In: (NSDI), pp.
71–84. USENIX (2013)
51. Yi, J., Wang, H., Zhang, J., Song, D., Jayasuriya, S., Liu, J.: Modeling and analysis of skid-
steered mobile robots with applications to low-cost inertial measurement unit-based motionestimation. IEEE Trans. Robot. 25(5), 1087–1097 (2009). Oct
52. Y oussef, M.A., Agrawala, A., Udaya Shankar, A.: Wlan location determination via clustering
and probability distributions. In: Proceedings of the First IEEE International Conference onPervasive Computing and Communications, 2003. (PerCom 2003) (2003)
53. Zhuang, Y ., El-Sheimy, N.: Tightly-coupled integration of wiﬁ and mems sensors on handheld
devices for indoor pedestrian navigation. IEEE Sens. J. 16(1), 224–234 (2016)
54. Zou, D., Tan, P .: Coslam: collaborative visual slam in dynamic environments. IEEE Trans.
Pattern Anal. Mach. Intell. 35(2), 354–366 (2013). Feb
Time-of-Flight Depth Datasets for Indoor
Semantic SLAM
Vijaya K. Ghorpade, Dorit Borrmann, Paul Checchin, Laurent Malaterre
and Laurent Trassoudaine
1 Introduction
Publicly available and benchmark datasets help to push forward the state-of-the-art
techniques in Computer Vision, Image Processing, Machine Learning, Robotics andseveral other scientiﬁc domains. They support the scientiﬁc evaluation and objective
comparison of algorithms with clear evaluation metrics. SLAM is one of the problems
in robotics which has been investigated using a variety of image and time-of-ﬂight
sensors that use radar, sonar and LiDAR [ 3].
A good source of SLAM datasets acquired with different types of sensors can be
found at [ 42]. Since the last decade, cameras that use the time-of-ﬂight technique have
become ubiquitous in robotic applications. They are commercially cheap, simple and
provide data at video frame rates. The ﬁrst SLAM systems that used RGB-D sensorshave appeared in [ 8,10,23], though these sensors use structured light rather than
the time-of-ﬂight technique. Sturm [ 51] presented a RGB-D benchmark dataset for
This work is supported by the Labex IMobS3.
V . K. Ghorpade ·P. Checchin ( B)·L. Malaterre ·L. Trassoudaine
Institut Pascal, SIGMA Clermont, CNRS, Université Clermont Auvergne,63000 Clermont-Ferrand, Francee-mail: paul.checchin@uca.fr
V . K. Ghorpade
e-mail: vijaya.saarland@gmail.com
L. Malaterre
e-mail: laurent.malaterre@uca.fr
L. Trassoudaine
e-mail: laurent.trassoudaine@uca.fr
D. Borrmann
Robotics and Telematics Group, University of Würzburg, Würzburg, Germanye-mail: borrmann@informatik.uni-wuerzburg.de
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_48679
680 V . K. Ghorpade et al.
SLAM using Kinect (v 1.0) RGB-D camera. However, presently, the data from even
the best RGB-D cameras (Kinect 2.0, Asus Xtion PRO live) are extremely noisy with
limited maximum range (4.5 m) and larger measurement accuracy ( ±0.03 m for 3 m
range). And for mapping with these sensors the data should be acquired within 13 m
distance from the surroundings [ 28]. On the other hand, the SwissRangers time-of-
ﬂight camera is although noisy (with measurement accuracy ±0.01 m for 5 and
10 m range) but has better and well studied noise characteristics [ 4,5,11–15,19,
21,22,26,27,34,45,46,53,56] with a maximum range of 10 m and higher
frequency than Kinect, but without RGB channel instead it provides conﬁdence andamplitude images. Also, the data from it is much smoother than Kinect like sensors
(as it uses time-of-ﬂight principle not structured light, unlike Kinect). The phase shift
principle of it helps to acquire the data at longer range more accurately than Kinect.Well studied noise characteristics of the SwissRangers camera helps to easily remove
both systematic and non-systematic errors.
Future service robots largely depend on time-of-ﬂight cameras for mapping, nav-
igation, manipulation and semantic interpretation of their surroundings and will
certainly use only the robust, light-weighted and simple sensors for this purpose.
SwissRangers time-of-ﬂight camera fulﬁl these criteria compared to Kinect or any
other state-of-the-art sensor presently available [ 6,9,16,24,25,30,37,38,57].
Moreover, they are improved constantly when compared to Kinect family of sensorswhich are explicitly developed for video-gaming not for robotics. To the best of our
knowledge, colorless point cloud dataset from a time-of-ﬂight camera with ground
truth pose information for a medium-scale indoor environment do not exist. Withthese motivations, in this paper we propose a depth dataset for 3D semantic SLAM
for evaluation and objective comparison of SLAM algorithms. In [ 48], a colorless
depth dataset is available, but of a single room, made by mounting the sensor on aKUKA robot arm, making a 360
◦rotation, acquiring images at every 2◦interval.
The IP-ToF (Institut Pascal Time-of-Flight) dataset comprises both a SLAM
dataset and household objects. The goal is to achieve semantic mapping of the envi-ronment, recognizing the objects after 3D reconstruction. The SLAM dataset again
consists of two different types of range data: 5 m and 10 m, each of them consisting
of 484 scans of the indoor environment (see Fig. 1).
The SLAM problem has been explored using different sensor modalities, includ-
ing 2D laser scanners [ 20,39], 3D scanners [ 2,36,41,47], monocular cameras [ 29,
32,40,43,49,50], stereo systems [ 7,31], radar sensors [ 54,55], time-of-ﬂight sen-
sors [ 8,10,23,51]. This paper is most closely related to the work of [ 1,44,48,51,
52]. A collection of several datasets for 6D-SLAM can be found at [ 42]. To the best
of our knowledge, IP-ToF dataset is the ﬁrst of its kind which provides depth data for
SLAM and semantics from time-of-ﬂight camera. Another important contribution
in this paper being, introducing a novel SLAM evaluation metric from InformationTheory, a context-based similarity score. This paper extensively depends on 3DTK
developed by Borrmann et al. [ 2] for evaluation, 3D reconstruction, plane detection
and visualization.
The structure of the paper follows as a brief introduction to the importance and
necessity for benchmark datasets for SLAM in Sect. 1. In Sect. 2, the description
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 681
Fig. 1 Institut Pascal Semantic SLAM dataset. aMap of the indoor environment, two ofﬁces
connected with a long corridor. The dataset consists of 484 scans taken at pre-arranged locations.The map is reconstructed using 3DTK [ 2], the heat-map representation is proportional to the distance
above the ground. bThe path of the indoor environment across which scans are acquired. The pose
ﬁles have the x,y,z,θ
zinformation. A ﬂoor plan of the environment is overlaid on the path for
reference only. Scale and representation are not accurate. Chairs and tables are positioned in thetwo rooms to serve as objects for semantic interpretation. Several household objects are placed onthe desk in the right room (towards the north, see Fig. 9). These are the same objects represented in
c, for semantic labelling of the scene. cDifferent household objects have been placed in the indoor
environment to be labelled after the 3D mapping. The object dataset consists of 2.5D views ofdifferent objects, taken at 5
◦intervals on a turntable equipped with an electronic device to measure
the rotation
682 V . K. Ghorpade et al.
of the dataset creation is discussed. In Sect. 3, 3D reconstruction, metric deﬁnition,
plane detection and semantic labelling results are presented.
2 Creating IP-ToF Dataset
The SwissRanger SR4000 comes with two different ﬂavours: 5 and 10 m maximum
range (though they look physically similar in appearance (see left Fig. 2)). The camera
is mounted on an angular-graded tripod, and the whole system is made mobile by
a triangular wooden board attached with wheels. The wooden board with the tripod
system enables capturing of images at pre-deﬁned locations in the environment (seeright Fig. 2). A virtual coordinate system is created, in which xpoints to the north,
ypoints to the west and zpoints upwards. The acquisition procedure starts from the
origin that lies at the center of the left room (Room 1) and terminates at the right room(Room 2) (Fig. 1b). At the origin and the end position 360
◦rotations are performed
with data collected every 5◦which is marked on the tripod for ease. The scans are
saved in both PCD (original camera data) and 3D format (in accordance with the
left-handed coordinate system from 3DTK). The change in the pose ﬁles is limited
to 4 degrees of freedom, as there is no roll and pitch rotation of the camera. The 3Dﬁles of the scans along with pose ﬁles are compatible with 3DTK. 3DTK supports a
variety of ﬁle formats for input, but uses a left-handed coordinate system internally,
i.e. thumb =x, index ﬁnger =yand middle ﬁnger =zaxes, where zrepresents the
depth axis and ythe lookup vector of the SR4000 internal coordinate system. As the
ﬁrst pose deﬁnes the coordinate system zis pointing to the north and xpointing to the
east. All data (including both 5 and 10 m) is freely available for research purposesat:ftp://ftp.ip.univ-bpclermont.fr/iptof-d .
3 Results
3.1 The 3D T oolkit: 3DTK
The 3D Toolkit is a publicly available software collection which provides algorithms
to process 3D point clouds, including 6DSLAM, plane detection, a 3D viewer, map-
ping of RGB and thermal images and others.1The toolkit is simple to use with a
GUI for visualization. It is open source thus providing options to adapt the existing
algorithms according to the user’s speciﬁc needs. The 6DSLAM algorithm provided
in 3DTK solves the SLAM problem in 6 degrees of freedom for data obtained from awide range of sensors.
2Lu and Milios [ 35] proposed a globally consistent solution to
1http://slam6d.sourceforge.net/ .
2http://kos.informatik.uni-osnabrueck.de/3Dscans/ .
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 683
Fig. 2 Mobile
Tripod-Camera system.(Left) SwissRanger SR4000
Time-of-Flight camera.
(Right) Sketch of thecamera-tripod system. Thetripod mounted with thecamera is ﬁxed to a triangularwooden board (with a hole inthe center) having trolleywheels. The whole systemhelps to scan the indoorenvironment at ease. A laserpointer is ﬁxed to the tripodfor accurate positioning onthe prearranged locations
the SLAM problem for 2D range scanners with 3 DoF. Borrmann et al. [ 2] extended
the approach to 6 DoF by solving the non-linearities using Taylor expansion and
Cholesky decomposition (Fig. 3). This paper extensively depends on 3DTK for scan
registration, reconstruction, plane detection and visualizing the obtained results.
3.2 SLAM Results Using only T oF Scans
The algorithmic process to obtain a consistent map from the scans is shown inFig. 3[2]. The scan matching is done at two stages (during pairwise ICP and graph
optimization). Faster results (with very few ICP iterations) can be obtained by using
initial pose estimates for ICP and by improving the matching later by a global graph
optimization. The graph is automatically generated based on point pairs. A link is
added for a scan pair if the ﬁxed range nearest neighbor search ﬁnds more than tenpoints pairs. The results obtained using 6DSLAM for the 5 m dataset are shown
in Fig. 6. The pose estimated by 3DTK (in 6 DoF) compared to the ground truth
are plotted in Figs. 4and 5. A new metric, which evaluates the similarity of the
estimated poses by 3DTK and the actual poses is designed. The metric is similar to
cosine similarity in NLP (Natural Language Processing), DNA-sequence alignment
and in data mining, in that, the similarity (or dissimilarity) is calculated using a
684 V . K. Ghorpade et al.
Fig. 3 Globally consistent 3D mapping, an extension to 6 DoF (image taken from [ 2]w i t hp e r -
mission)
vector space model. The raw pose difference between the estimated and actual poses
(Figs. 4and 5) let’s assume that the camera locations/positions are stochastically
independent. There is higher chance of the error being accumulated and propagatedwithout considering the relations between consecutive poses, and thus leads to false
evaluation of SLAM algorithms. In order to solve the problem that arises due to
the assumption of statistical independence of camera positions, “context” of thescan acquisition is taken into account. The “context” considers the spatial camera
viewpoint relation between two or three consecutive scans.
3.3 Context-Based Similarity Measurement
Consider three scans which are consecutively acquired ( i,j,k), hereafter i,jand k
represent consecutive scans ( j=i+1;k=j+1) throughout the paper. A camera
position (or ground truth pose, differentiated with a∗superscript from those estimated
with SLAM) in the space can be described with its position and direction vectors:
pose i=(t∗
i,r∗
i), (1)
where,
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 685
Fig. 4 Comparison of the translation from SLAM results with ground truth. The blue data is the
estimated pose of the tripod-camera-board system at each location by 6DSLAM from 3DTK. The
red data is the actual pose on the virtual coordinate system. It should be noted that the red datais prone to human error: the mobile system may not be placed accurately on each and every pre-arranged position. Also, the ﬁrst pose deﬁnes the coordinate system for SLAM. Pose errors at thisposition are propagated to the following positions (see also Fig. 6)
Fig. 5 Angular differences
between the poses fromSLAM results with groundtruth based on unitquaternions [ 33]

686 V . K. Ghorpade et al.
Fig. 6 6DSLAM using 3DTK. a3DTK uses the pre-arranged locations as the initial pose-estimate
for ICP and solves the SLAM problem in 6 DoF. No IMU (Inertial Measurement Unit) or visualodometry or explicit loop-closure has been used. The data from the SwissRanger camera is noisy, ascommon for time-of-ﬂight cameras, variant to illumination and has void data in case of transparent
surfaces like glass windows. The results demonstrate this effect. bThe path obtained by 6DSLAM.
The small positional inaccuracies as compared to the ground truth during the 360
◦rotation at the
beginning and at the end of the path are caused by the noisy data
t∗
i=(t∗
ix,t∗
iy,t∗
iz)T,
r∗
i=(r∗
ix,r∗
iy,r∗
iz)T(2)
are position (translation) vector and direction (lookat) vector respectively, and ˆt∗,ˆr∗
are their unit vectors. The resultant position ( T∗=(ˆt∗
i×ˆt∗
j)×ˆt∗
k)and direction vector
(R∗=(ˆr∗
i×ˆr∗
j)×ˆr∗
k)of three consecutive poses is obtained by a vector triple product.
Similarly, the same operation is applied to the poses estimated from 6DSLAM to
obtain RandT. Then the inner-product (T·T∗)and(R·R∗)between them reveals,
how similar the estimated poses are to the ground truth with a quantitative value in
the range of [0,2](0: dissimilar, 1: similar, 2: similar but opposite).
The quality of similarity can be further augmented using the context, weighted
inner-product. The weighted inner-product has been extensively used in information
retrieval:
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 687
Fig. 7 Rotational and translational variance with two and three consecutive scans. a–c Rotational
variance. d–f Translational variance. cIt can be seen, by considering three scans instead of two,
the rotational variance is better adjusted and there is higher similarity between the calculated posesand the ground truth. eAs expected the translational variance is not affected by the number of scans
considered, as it is a position vector with no direction, but with direction vector the effect can beseen in a–c. The huge peak in the beginning of d–f corresponds to the accumulated error in Fig. 4
(left)
688 V . K. Ghorpade et al.
Fig. 8 Context-based similarity between estimated pose and ground truth. aTranslational variance
with prior rotation information. bComparison of context-based and context-free similarity. For
better understanding, the physical change in the camera pose is depicted as colored rectangularboxes. Yellow: represents the positions where there is only rotation, like making 360
◦view in
Room 1 and Room 2 and while entering and leaving the corridor. Red: represents only translation,
like in the long corridor where there is no rotation. Turquoise: represents where there is rotationand translation. It can be seen in the dotted circle, how context corrects the translational variance.The pose variance changes from being dissimilar to similar in opposite magnitude. The small peakbetween position 320–350 corresponds to the bend seen in Figs. 4(left), 5,6b,7d–f. It should be
noted that, the data acquisition is prone to human-error, which might have caused the translationalvariance in the Room 1
Fig. 9 Single scene semantic labelling. The objects are placed in the right room, on the desk towards
the north. A semantic interpretation system based on Keypoint distributions is used to recognizethe objects in the scene with some certainty. (Left) Semantic labelling with certainty of scene pointcloud. (Right Top) Reﬂectance image of scene. (Right Bottom) Semantic parsing of reﬂectanceimage
T·T∗=u×Tx×T∗
x+v×Ty×T∗
y+w×Tz×T∗
z (3)
where, u,v&ware the context information from the direction vector rx,ry&rz
respectively. To simplify, the translational dot products magnitude is ampliﬁed with
a priori information from the individual direction vector components. This helps to
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 689
Fig. 10 Plane detection using the Randomized Hough Transform. (Left) Point cloud with detected
planes. (Right) Only planes. Each plane is represented with distinct color (19 planes representingwalls, doors, ceiling and ﬂoor). 3DTK also provides tools for extracting planes in point clouds. Thecomplete point cloud is down-sampled using octree based method (one point in 10 cm voxel) forfaster and efﬁcient calculation
correct the translational pose difference with the context of rotational similarity. This
is shown in Fig. 8b, the corridor pose variance is corrected with rotational context
information: “there is no rotation in the corridor, only translation”. Figure 8ai st h e
context-based pose similarity. Figures in 7show the rotational and translational pose
variance without context, by considering two and three consecutive poses.
It should be noted that, the acquisition might be prone to human judgement error
of positioning the camera system perfectly on the pre-deﬁned locations. However,3DTK is able to use these initial estimates for ICP and solve the SLAM problem
and give a globally consistent map after optimization using 6D GraphSLAM. The
single scene semantic labelling (North of Room 2) is also depicted in Fig. 9,t h e
semantic interpretation is achieved using a novel descriptor based on PDF (Probabil-
ity Distribution Functions) distribution of key points [ 17,18]. Plane detection is also
performed using Randomized Hough Transform (RHT) from 3DTK (see Fig. 10).
4 Conclusion
In this paper, a dataset for semantic SLAM using only depth data has been proposed.
The dataset along with pose information is publicly available. SLAM in 6 DoF is
solved using a toolkit which is freely available. A novel way of evaluation of SLAM
algorithm is also discussed. Semantic interpretation of single scene along with plane
detection results are also presented in this work. The future work would involve plane
classiﬁcation into walls, ceiling, ﬂoor and desks; at the same time highlighting theobjects from the dataset in the map with semantic information.
Acknowledgements This work is supported by the French government research program Investisse-
ments d’Avenir through the RobotEx Equipment of Excellence (ANR-10-EQPX-44) and the
IMobS3 Laboratory of Excellence (ANR-10-LABX-16-01), by the European Union through the
program Regional competitiveness and employment 2007–2013 (ERDF - Auvergne region), bythe Auvergne region.
690 V . K. Ghorpade et al.
References
1. Bao, S.Y ., Bagra, M., Chao, Y .W., Savarese, S.: Semantic structure from motion with points,
regions, and objects. In: 2012 IEEE Conference on Computer Vision and Pattern Recognition,pp. 2703–2710 (2012). https://doi.org/10.1109/CVPR.2012.6247992
2. Borrmann, D., Elseberg, J., Lingemann, K., Nüchter, A., Hertzberg, J.: Globally
consistent 3D mapping with scan matching. Robot. Auton. Syst. 56(2), 130–142
(2008). https://doi.org/10.1016/j.robot.2007.07.002 ,http://www.sciencedirect.com/science/
article/pii/S0921889007000863
3. Cadena, C., Carlone, L., Carrillo, H., Latif, Y ., Scaramuzza, D., Neira, J., Reid, I., Leonard,
J.J.: Past, present, and future of simultaneous localization and mapping: toward the robust-perception age. IEEE Trans. Robot. 32(6), 1309–1332 (2016). https://doi.org/10.1109/TRO.
2016.2624754
4. Cazorla, M., Viejo, D., Pomares, C.: Study of the sr4000 camera. In: Proceedings of XI Work-
shop of Physical Agents Fısicos, Valencia, Spain (2010)
5. Chiabrando, F., Chiabrando, R., Piatti, D., Rinaudo, F.: Sensors for 3d imaging: metric evalua-
tion and calibration of a ccd/cmos time-of-ﬂight camera. Sensors 9(12), 10080–10096 (2009)
6. Chiabrando, F., Piatti, D., Rinaudo, F.: SR-4000 Tof camera: further experimental tests and
ﬁrst applications to metric surveys. Int. Arch. Photogramm. Remote Sens. Spatial Inf. Sci. 38,
149–154 (2010)
7. Comport, A., Malis, E., Rives, P.: Real-time quadrifocal visual odometry. Int. J. Rob. Res.
29(2–3), 245–266 (2010). https://doi.org/10.1177/0278364909356601
8. Comport, A.I., Meill, M., Rives, P.: Real-time dense appearance-based SLAM for RGB-D
sensors. In: Proceedings of the 2011 Australasian Conference on Robotics and Automation,pp. 100–109 (2011)
9. Cui, Y ., Schuon, S., Chan, D., Thrun, S., Theobalt, C.: 3D shape scanning with a time-of-
ﬂight camera. In: 2010 IEEE Computer Society Conference on Computer Vision and PatternRecognition, pp. 1173–1180 (2010). https://doi.org/10.1109/CVPR.2010.5540082
10. Dib, A., Beaufort, N., Charpillet, F.: A real time visual SLAM for RGB-D cameras based
on chamfer distance and occupancy grid. In: 2014 IEEE/ASME International Conference onAdvanced Intelligent Mechatronics, pp. 652–657 (2014). https://doi.org/10.1109/AIM.2014.
6878153
11. Diebel, J., Thrun, S.: An application of Markov random ﬁelds to range sensing. In: Paper
presented on NIPS, pp. 291–298. MIT Press, Cambridge (2005)
12. Donoho, D.L.: Denoising by soft-thresholding. IEEE Trans. Inf. Theory 41, 613–627 (1995).
https://doi.org/10.1109/18.382009
13. Dopfer, A., Wang, H.H., Wang, C.C.: 3d active appearance model alignment using intensity
and range data. Robot. Auton. Syst. 62(2), 168–176 (2014). https://doi.org/10.1016/j.robot.
2013.11.002 ,http://www.sciencedirect.com/science/article/pii/S0921889013002194
14. Falie, D., Buzuloiu, V .: Noise characteristics of 3d time-of-ﬂight cameras. In: 2007 International
Symposium on Signals, Circuits and Systems, vol. 1, pp. 1–4 (2007). https://doi.org/10.1109/
ISSCS.2007.4292693
15. Foix, S., Alenya, G., Torras, C.: Lock-in time-of-ﬂight (tof) cameras: a survey. IEEE Sens. J.
11(9), 1917–1926 (2011). https://doi.org/10.1109/JSEN.2010.2101060
16. Fuchs, S., May, S.: Calibration and registration for precise surface reconstruction with time-
of-ﬂight cameras. Int. J. Intell. Syst. Technol. Appl. 5(3/4), 274–284 (2008). https://doi.org/
10.1504/IJISTA.2008.021290
17. Ghorpade, V .K., Checchin, P., Malaterre, L., Trassoudaine, L.: Performance evaluation of 3d
keypoint detectors for time-of-ﬂight depth data. In: 2016 14th International Conference onControl, Automation, Robotics and Vision (ICARCV), pp. 1–6 (2016). https://doi.org/10.1109/
ICARCV .2016.7838686
18. Ghorpade, V .K., Checchin, P., Malaterre, L., Trassoudaine, L.: 3d shape representation with
spatial probabilistic distribution of intrinsic shape keypoints. EURASIP J. Adv. Signal Process.2017 (1), 52 (2017). https://doi.org/10.1186/s13634-017-0483-y
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 691
19. Ghorpade, V .K., Checchin, P., Trassoudaine, L.: Line-of-sight-based tof camera’s range image
ﬁltering for precise 3d scene reconstruction. In: 2015 European Conference on Mobile Robots(ECMR), pp. 1–6 (2015). https://doi.org/10.1109/ECMR.2015.7324208
20. Grisetti, G., Stachniss, C., Burgard, W.: Improved techniques for grid mapping with rao-
blackwellized particle ﬁlters. IEEE Trans. Robot. 23(1), 34–46 (2007). https://doi.org/10.1109/
TRO.2006.889486
21. Hansard, M., Lee, S., Choi, O., Horaud, R.P.: Time-of-ﬂight cameras: principles, methods and
applications. Springer Science & Business Media, London (2012)
22. He, Y ., Liang, B., Zou, Y ., He, J., Yang, J.: Depth errors analysis and correction for time-of-ﬂight
(tof) cameras. Sensors 17(1), 92 (2017)
23. Henry, P., Krainin, M., Herbst, E., Ren, X., Fox, D.: RGB-D Mapping: Using Depth Cameras
for Dense 3D Modeling of Indoor Environments, pp. 477–491. Springer, Berlin (2014). https://
doi.org/10.1007/978-3-642-28572-1\_33
24. Hong, S., Ye, C., Bruch, M., Halterman, R.: Performance evaluation of a pose estimation method
based on the swissranger sr4000. In: 2012 IEEE International Conference on Mechatronics andAutomation, pp. 499–504 (2012). https://doi.org/10.1109/ICMA.2012.6283123
25. Iddan, G.J., Yahav, G.: Three-dimensional imaging in the studio and elsewhere. In: Proceedings
of the SPIE Three-Dimensional Image Capture and Applications IV . vol. 4298 (2001). https://
doi.org/10.1117/12.424913
26. Jovanov, L., Pižurica, A., Philips, W.: Fuzzy logic-based approach to wavelet denoising of 3d
images produced by time-of-ﬂight cameras. Opt. Exp. 18, :22651–22676 (2010). https://doi.
org/10.1364/OE.18.022651
27. Kahlmann, T., Remondino, F., Ingensand, H.: Calibration for increased accuracy of the range
imaging camera SwissRangerTM. In: Maas, H.G. (ed.) Proceedings of the ISPRS Commis-sion V Symposium, International archives of photogrammetry, remote sensing and spatialinformation sciences, vol. 36, pp. 136–141. Institute of Photogrammetry and Remote Sensing,University of Technology, Dresden (2006)
28. Khoshelham, K., Elberink, S.O.: Accuracy and resolution of kinect depth data for indoor
mapping applications. Sensors 12(2), 1437–1454 (2012). https://doi.org/10.3390/s120201437 ,
http://www.mdpi.com/1424-8220/12/2/1437
29. Klein, G., Murray, D.: Parallel tracking and mapping for small AR workspaces. In: Proceedings
of the 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality,ISMAR ’07, pp. 1–10. IEEE Computer Society, Washington, DC, USA (2007). https://doi.org/
10.1109/ISMAR.2007.4538852
30. Kolb, A., Barth, E., Koch, R., Larsen, R.: Time-of-ﬂight sensors in computer graphics. In:
Pauly, M., Greiner, G. (eds.) Eurographics 2009 - State of the Art Reports. The EurographicsAssociation, Aire-la-Ville (2009). https://doi.org/10.2312/egst.20091064
31. Konolige, K., Agrawal, M., Bolles, R.C., Cowan, C., Fischler, M., Gerkey, B.: Outdoor Mapping
and Navigation Using Stereo Vision, pp. 179–190. Springer, Berlin (2008). https://doi.org/10.
1007/978-3-540-77457-0\_17
32. Konolige, K., Bowman, J.: Towards lifelong visual maps. In: 2009 IEEE/RSJ International
Conference on Intelligent Robots and Systems, pp. 1156–1163 (2009). https://doi.org/10.1109/
IROS.2009.5354121
33. Kuffner, J.J.: Effective sampling and distance metrics for 3D rigid body path planning. In:
2004 IEEE International Conference on Robotics and Automation (ICRA), pp. 3993–3998.New Orleans, United States (2004)
34. Lange, R.: 3d time-of-ﬂight distance measurement with custom solid-state image sensors in
cmos/ccd-technology (2000). http://dokumentix.ub.uni-siegen.de/opus/volltexte/2006/178
35. Lu, F., Milios, E.: Globally consistent range scan alignment for environment mapping. Auton.
Robot. 4(4), 333–349 (1997). https://doi.org/10.1023/A:1008854305733
36. Magnusson, M., Andreasson, H., Nüchter, A., Lilienthal, A.J.: Automatic appearance-based
loop detection from three-dimensional laser data using the normal distributions transform. J.Field Robot. 26(11–12), 892–914 (2009). https://doi.org/10.1002/rob.20314
692 V . K. Ghorpade et al.
37. May, S., Droeschel, D., Fuchs, S., Holz, D., Nüchter, A.: Robust 3d-mapping with time-of-ﬂight
cameras. In: 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.1673–1678 (2009). https://doi.org/10.1109/IROS.2009.5354684
38. May, S., Droeschel, D., Holz, D., Fuchs, S., Malis, E., Nüchter, A., Hertzberg, J.: Three-
dimensional mapping with time-of-ﬂight cameras. J. Field Robot. 26(11–12), 934–965 (2009).
https://doi.org/10.1002/rob.20321
39. Montemerlo, M., Thrun, S., Koller, D., Wegbreit, B.: FastSLAM: a factored solution to the
simultaneous localization and mapping problem. In: 18th National Conference on ArtiﬁcialIntelligence, pp. 593–598. American Association for Artiﬁcial Intelligence, Menlo Park, CA,USA (2002). http://dl.acm.org/citation.cfm?id=777092.777184
40. Nister, D.: Preemptive RANSAC for live structure and motion estimation. In: Proceedings
Ninth IEEE International Conference on Computer Vision, vol. 1, pp. 199–206 (2003). https://
doi.org/10.1109/ICCV .2003.1238341
41. Nüchter, A., Lingemann, K., Hertzberg, J., Surmann, H.: 6D SLAM - 3D mapping outdoor
environments: research articles. J. Field Robot. 24(8–9), 699–722 (2007). https://doi.org/10.
1002/rob.v24:8/9
42. Nüchter, A., Lingemann, K.: Robotic 3D Scan Repository. http://kos.informatik.uos.de/
3Dscans/ (2017)
43. Pollefeys, M., Gool, L.V .: From images to 3D models. Commun. ACM 45(7), 50–55 (2002).
https://doi.org/10.1145/514236.514263
44. Pomerleau, F., Magnenat, S., Colas, F., Liu, M., Siegwart, R.: Tracking a depth camera: param-
eter exploration for fast ICP. In: 2011 IEEE/RSJ International Conference on Intelligent Robotsand Systems, pp. 3824–3829 (2011). https://doi.org/10.1109/IROS.2011.6094861
45. Reynolds, M., Dobo, J., Peel, L., Weyrich, T., Brostow, G.J.: Capturing time-of-ﬂight data with
conﬁdence. CVPR 2011 , 945–952 (2011). https://doi.org/10.1109/CVPR.2011.5995550
46. Robbins, S., Schroeder, B., Murawski, B., Heckman, N., Leung, J.: Photogrammetric calibration
of the SwissRanger 3D range imaging sensor. In: Proceedings of the SPIE Optical Sensors,vol. 7003, p. 700320 (2008). https://doi.org/10.1117/12.781551
47. Segal, A., Haehnel, D., Thrun, S.: Generalized-ICP. In: Proceedings of Robotics: Science and
Systems. Seattle, USA (2009). https://doi.org/10.15607/RSS.2009.V .021
48. Fuchs, S.: DLR institute of robotics and mechatronics: 3D mapping with ToF cameras. http://
kos.informatik.uos.de/3Dscans/ (2017)
49. Strasdat, H., Montiel, J., Davison, A.J.: Scale drift-aware large scale monocular SLAM. In:
Robotics: Science and Systems VI. The MIT Press, Cambridge (2010)
50. Stühmer, J., Gumhold, S., Cremers, D.: Real-Time Dense Geometry from a Handheld Camera,
pp. 11–20. Springer, Berlin (2010). https://doi.org/10.1007/978-3-642-15986-2\_2
51. Sturm, J., Engelhard, N., Endres, F., Burgard, W., Cremers, D.: A benchmark for the evaluation
of RGB-D SLAM systems. In: 2012 IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), pp. 573–580. IEEE, Algarve (2012)
52. Sturm, J., Magnenat, S., Engelhard, N., Pomerleau, F., Colas, F., Cremers, D., Siegwart, R.,
Burgard, W.: Towards a benchmark for RGB-D SLAM evaluation. In: RGB-D Workshop onAdvanced Reasoning with Depth Cameras at Robotics: Science and Systems Conference (RSS).
Los Angeles, United States (2011). https://hal.archives-ouvertes.fr/hal-01142608
53. Tamas, L., Jensen, B.: Robustness analysis of 3d feature descriptors for object recognition using
a time-of-ﬂight camera. In: 22nd Mediterranean Conference on Control and Automation, pp.1020–1025 (2014). https://doi.org/10.1109/MED.2014.6961508
54. Vivet, D., Gérossier, F., Checchin, P., Trassoudaine, L., Chapuis, R.: Mobile ground-based
radar sensor for localization and mapping: an evaluation of two approaches. Int. J. Adv. Robot.Syst. 10(307), 12 (2013). https://doi.org/10.5772/56636
55. Vivet, D., Checchin, P., Chapuis, R.: Localization and mapping using only a rotating FMCW
radar sensor. Sensors 13(4), 4527–4552 (2013). https://doi.org/10.3390/s130404527 ,http://
www.mdpi.com/1424-8220/13/4/4527
Time-of-Flight Depth Datasets for Indoor Semantic SLAM 693
56. Weingarten, J.W., Gruener, G., Siegwart, R.: A state-of-the-art 3d sensor for robot navigation.
In: 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), vol.3, pp. 2155–2160 (2004). https://doi.org/10.1109/IROS.2004.1389728
57. Ye, C., Bruch, M.: A visual odometry method based on the swissranger sr4000. In: Technical
report, Arkansas University at Little rock, Little Rock (2010)
Globally Optimal Object Pose Estimation
in Point Clouds with Mixed-IntegerProgramming
Gregory Izatt, Hongkai Dai and Russ T edrake
1 Introduction
The robotic perception community has placed signiﬁcant emphasis on designing and
improving perception modules for estimating the poses of objects in a scene. Thesemodules have enormous value for autonomous systems, in that they reduce extremely
high-dimensional sensor inputs to compact and semantically loaded object state that
can be consumed by a broad range of motion planners and robot controllers.
Here, we are concerned with systems for estimating object poses from point cloud
information, e.g. from increasingly ubiquitous RGB-D cameras. Myriad techniquesperform pose estimation without an initial guess, e.g. via sampling [ 11,24,28],
feature extraction [ 7,17,34,35,38], template matching [ 15], shape descriptors
[1], and direct machine learning [ 36]. However, because of the scale of the sensory
data and the difﬁculty of the global optimization, few of these techniques run in real
time, and those that do can not make claims concerning reliability and convergence
to global optimality. Given a reasonable initial guess, another broad class of posetracking techniques perform real-time object tracking. These techniques have grown
extremely mature, boasting support for multiple articulated objects [ 31], deformable
objects [ 32], contact [ 20], and tactile sensing [ 16,18,30].
In this work, we present a study of the core optimization problem that underlies
many of these techniques. We show that the pose estimation problem for point clouds
G. Izatt (B)·R. Tedrake
Computer Science and Artiﬁcial Intelligence Lab, MIT, Cambridge, MA, USAe-mail: gizatt@mit.edu
R. Tedrake
e-mail: russt@mit.edu
H. Dai ·R. Tedrake
Toyota Research Institute, Ann Arbor, MI, USAe-mail: hongkai.dai@tri.global
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_49695
696 G. Izatt et al.
can be viewed through the lens of mixed-integer programming, and that doing so leads
to a problem formulation permitting optimization to certiﬁable approximate global
optimality via branch and bound search. This formulation is written in a general formthat is extensible to handle explicit outlier rejection and multiple models, and can
consume the output of other local and global pose estimation algorithms as seeds to
accelerate the global search—and in doing so, verify the optimality of the output ofthose other algorithms.
2 Problem Formulation
We focus on an instance of the point-cloud object pose estimation problem that
involves ﬁnding the best conﬁguration of a rigid-body model to explain the data
available from a sensed point cloud. The model conﬁguration is parameterized bythe rotations R∈SO (3)and translations T∈R
3of each rigid body. The point cloud
sensor samples a set of Nspoints S={si}from the geometry of the world. Many
techniques represent the model as a collection of Nmpoint features, which leads to
an optimization penalizing a norm (shown here in the single-object case):
min
R,T,C/summationdisplay
i∈[1,Ns]/vextenddouble/vextenddoubleRsi+T−mC(i)/vextenddouble/vextenddouble, (1)
C(i)=argmin
j∈[1,Nm]/vextenddouble/vextenddoubleRsi+T−mj/vextenddouble/vextenddouble,
where C(i)corresponds each scene point to the closest model feature according to
the desired norm.
Objectives like this one are reﬂected in many of the pose estimation techniques in
the literature. Key differences between these techniques lie in the model representa-
tion and distance function used, the method of optimization, and further additions to
the objective beyond optimization of just a distance function.
A critical feature of this problem is that the correspondences Cand transformation
{R,T}are each independently sufﬁcient to specify a solution to this problem. Given
the correspondences, the optimal transformation can be computed in closed form [ 9].
Given the transformation, correspondences can be backed out if desired via, e.g., clos-
est point lookups on the model. The famous Iterative Closest Point (ICP) algorithm,
from which many object trackers are inspired, performs Expectation Maximizationby alternating between solving these two problems [ 3,5].
3 Related Work
As described in Sect. 1, numerous techniques have been developed to estimate object
poses without an initial guess. However, in order to make the global optimization
more tractable, most of these techniques rely on methods like stochastic sampling
Globally Optimal Object Pose Estimation … 697
and downsampling via feature extraction, and additionally may make assumptions
concerning the quantity of outliers and prior knowledge of object segmentations. At
a high level, several broad classes of approaches have been used to attempt globaloptimization of the point cloud pose estimation objective directly on the complete
raw point cloud, including semideﬁnite programming (SDP) relaxation, and branch
and bound search.
SDP relaxation of the rotation and correspondence constraints constrain Rto be
within a convex hull of SO (3), and allow a continuous relaxation of C[2]. This
relaxation transforms the difﬁcult nonlinear problem to a much easier convex one.This technique has proven very powerful for solving the Procrustes Matching (PM)
problem, [ 22], and similar SDP relaxation can be applied to the pose estimation
problem with ﬁxed correspondences (i.e. an alignment problem) [ 4]. Their method
boasts tightness up to a quantiﬁed noise threshold, and is demonstrated aligning 800
points across 30 overlapping point clouds. SDP relaxations have also proven powerful
for performing large-scale, noisy pose graph optimization over SE (3)[29]. However,
we have found that these relaxations are difﬁcult to apply directly to the point-cloud
pose estimation problem, because the SDP relaxation of R∈SO (3)is very loose
and introduces trivial solutions to Eq. 1.
Other methods perform branch and bound over the space of rotations [ 14,19,27],
or rotations and translations [ 37]. The latter—GO-ICP—accomplishes our broad goal
of providing globally-optimal pose estimates, but it does not explicitly reason about
correspondences. This manifests itself most clearly in the handling of outliers: a
user of GO-ICP must specify the expected fraction of outliers ahead of time, andsetting this parameter incorrectly can lead to invalid results. Other techniques take
direct advantage of the property that it is easy to detect inconsistencies in small sets
of correspondences in order to prune branches in the search tree [ 10,12]. Recent
work has also demonstrated a custom search strategy over object arrangements that
is aware of the order of object occlusion and its effect on the independence of objects
an alignment objective [ 25]. This work explicitly reasons about outliers, but has not
been scaled beyond planar object arrangements.
The transform and correspondence information are tightly coupled in the pose
estimation problem. Thus, a formulation that reasons about point correspondencesand model transformations simultaneously stands to beneﬁt from this interplay. We
present such a formulation based on mixed-integer programming [ 26]. While even the
restricted class of mixed-integer linear programs (MILPs) is itself NP-hard, mixed-integer programs (MIPS) that are convex in their continuous and integer variables are
amenable to branch and bound search that can be very efﬁcient, given the right prob-
lem structure. These algorithms are implemented by powerful off-the-shelf solvers
capable of solving problems with millions of variables and constraints [ 13].
698 G. Izatt et al.
4 Mixed-Integer Problem Formulation
Our formulation of this problem uses a generalized mesh model to represent the
objects, for the reason that the mesh model is signiﬁcantly more compact than atraditional sampled point model. Given a model deﬁned by N
mvertices and Nf
faces, where each face is deﬁned as an afﬁne combination of a subset of coplanar
vertices, as well as
•scene points S={si},i∈[1,Ns],
•model vertices M={mj},j∈[1,Nm],
•a binary face membership map F∈{0,1}Nm×Nf,
the generic pose estimation problem is equivalent to ﬁnding a rotation matrix R,a
translation matrix T, a combination matrix C∈RNs×Nm, and a face correspondence
matrix f∈{0,1}Ns×Nfthat satisfy the following.
minimize
R,T,C,f1
Ns/summationdisplay
i∈[1,Ns]/vextenddouble/vextenddoubleRsi+T−MCT
i/vextenddouble/vextenddouble2
2
subject to R∈SO (3),
/summationdisplay
j∈[1,Nm]Ci,j=1,∀i,
/summationdisplay
k∈[1,Nf]fi,k=1,∀i,
0≤Ci,j≤FjfT
i,∀i,j,
fi,j∈[0,1],∀i,j.
Ciand fiare the ith rows of Cand f, and Fjis the jth row of F. The afﬁne
combination coefﬁcient for the ith scene point and jth model point Ci,jis constrained
to be inactive unless one of the faces for model point jis active according to its face
map Fjand the face selection fi. Scene points can only correspond to a single
model face. Note that, save the constraint R∈SO (3), this problem is already a
mixed-integer convex (quadratic) program. In the next section, we describe how
to approximate the constraint R∈SO (3)to completely express this problem in a
mixed-integer convex way.
4.1 Mixed-Integer Linear Approximation of R ∈SO (3)
We approximate the R∈SO (3)constraint with a piecewise-convex outer approx-
imation in the spirit of the McCormick Envelope [ 23]. For each member of the
rotation matrix Ri,j, we introduce new binary variables to assign Ri,jto one of
Globally Optimal Object Pose Estimation … 699
Fig. 1 We cut the ﬁrst octant
of the sphere with regularly
spaced planes normal to each
axis. The intersections
between the planes and the
sphere (red arcs) partition the
surface of the sphere into
smaller regions
Fig. 2 The ﬁrst octant of the
sphere surface after partition
in Fig. 1. The shaded surface
region is one of the partitions
Nkpartitions of [−1,1]. These binary variables are used to activate region-speciﬁc
constraints approximating the constraints implied by R∈SO (3), based on the for-
mulation in [ 6]. The constraints approximated are RTR=IandR1×R2=R3⇐=
=⇒ det (R)=+ 1 for the sets of rows and columns {R1,R2,R3}of R.
For example, if we denote a column of Ras[x,y,z]T∈R3,RTR=Iimplies
that this vector should have a unit length and hence lie on the surface of the unit
sphere. The partitioning of x,y, and zcan be geometrically interpreted as cutting the
surface of the unit sphere by planes parallel to either xy,xzoryzplanes, as shown in
Fig. 1. The intersection between the planes and the sphere partitions the surface of the
sphere into small regions (Fig. 2). For each surface region, we can readily compute a
convex polytope A[xyz]T≤bcontaining the region (Fig. 3). This linear constraint
is activated by the binary variables denoting in which interval the variables x,y,z
lie. Similar constructions are used to approximate the remaining orthogonality and
cross-product constraints.
700 G. Izatt et al.
Fig. 3 The polytope A[xyz]T≤b(light blue region) containing the shaded surface region in
Fig. 2, viewed from two perspectives. We relax the constraint that the vector is on the shaded
surface area, to that the vector lies within the polytope
5 Extensions
5.1 Handling Outliers
Correct outlier handling is critical for object pose estimation algorithms, as point
clouds in the wild invariably include unmodeled points from nearby objects and
support surfaces in the scene. We extend this formulation to allow scene points to be
explicitly classiﬁed as outliers.
We ﬁrst switch from the L-2 to the L-1 norm in our error metric, so that we can
include the distance to each point in the set of linear constraints. We introduce an
intermediate variable φifor each scene point sistoring the L-1 distance from sito the
matched point on the model. Additional slack variables αi∈R3are introduced to
implement 3 absolute values within the L-1 norm for each scene index i. We bound
φiwith a constant maximum allowed L-1 distance φmaxas a threshold (and penalty)
for classifying points as outliers. Finally, we add a new binary variable oifor each
scene point indicating that scene point is being considered an outlier.
Extending the mesh-model MIP formulation from Sect. 4,w en o ws o l v et h eM I L P
(for large M):
minimize
R,T,C,f,φ,α,omin1
Ns/summationdisplay
i∈[1,Ns]φi
subject to Relaxed R ∈SO (3),
φi≥1Tαi,
φi≥φmaxoi,
αi≥+/parenleftbig
Rsi+T−MCT
i/parenrightbig
−Moi, (2)
Globally Optimal Object Pose Estimation … 701
αi≥−/parenleftbig
Rsi+T−MCT
i/parenrightbig
−Moi, (3)
/summationdisplay
j∈NmCi,j+oi=1,
/summationdisplay
k∈Nffi,k+oi=1,
0≤Ci,j≤FjfT
i,
φi,αi≥0,
fi,j,oi∈{0,1}.
5.2 Handling Multiple Objects
Using similar machinery to that employed to correspond to outliers, we can extend
our formulation further to support multiple objects. We can extend the formulationto simultaneously optimize over multiple rotations and translations {R
1,T1},...,
{RNb,TNb}forNbseparate bodies. Given a map B∈{0,1}Nb×Nf, where the (i,j)th
entry indicates if face jis a member of body i, we can replace constraints ( 2) and
(3) with the disjunction
∀i∈Nm,k∈Nb:
αi≥+/parenleftbig
Rksi+Tk−MCT
i,:/parenrightbig
−M(1−Bk,:fT
i,:),
αi≥−/parenleftbig
Rksi+Tk−MCT
i,:/parenrightbig
−M(1−Bk,:fT
i,:).
where the expression M(1−Bk,:fT
i,:)deactivates the constraint if the current assign-
ment fdoes not assign scene point ito a face on body k.
5.3 Using Other Pose Estimation Methods as a Heuristic
A beneﬁt of optimizing directly over the fundamental problem addressed by a wide
class of pose estimation methods is that we can take advantage of solutions generated
by those other methods by consuming them as candidate feasible solutions. Thebranch and bound algorithm (and solvers that implement it) is able to asynchronously
consume feasible solution guesses as nodes in the search tree. These new feasible
solutions provide upper bounds on the global optimal cost, which are used to prunebad nodes. Because a signiﬁcant amount of search time is spent ﬁnding better feasible
solutions (as can be seen in the results in e.g. Fig. 4), getting better feasible solutions
from faster but less-consistent pose estimation methods can improve the runtime ofthe global optimization. This ability also means that this formulation can be used to
702 G. Izatt et al.
post-process the output of methods that trade consistency for efﬁciency in order to
guarantee stable results without completely discarding the efﬁciency of the original
method.
Given the MIP formulation described above and a candidate pose {ˆR,ˆT}generated
by any method, one can extract C,f,φ, andαvia closest-point queries against the
mesh models. The value of ˆRdirectly determines which binary variables should be
active in the piecewise-linear relaxation of R∈SO (3).
6 Results
We executed several experiments to verify our formulation on synthetic data. In addi-
tion, we performed exploratory experiments on real data to probe the practicality of
this and competing approaches to global pose estimation. To perform these exper-iments, we implemented both formulations in C++ and Julia, relying on the Drake
[33] and JuMP [ 8] symbolic optimization libraries respectively. No performance dif-
ference between the libraries was observed. We used Gurobi 7.0.2 [ 13] as a backend
to solve the resulting MIPs.
6.1 Experiments on Synthetic Data
6.1.1 General Performance
We generated a synthetic point cloud from a cube model with a side length of 1 unit.
We generated 15 scene points, with 10 sampled randomly from the surface of the
cube at its ground truth pose, and 5 more generated randomly in the area around thecube. We included outliers in this test case to illuminate how the solver performs in
terms of progress of the upper and lower bounds—without outliers, the optimal error
would be close to the trivial lower bound of 0. An optimal ﬁt in this conﬁguration hasan optimal average saturated L-1 error of 0 .033: the
2
3of points that are inliers have
L - 1e r r o ro f0 ,a n dt h e1
3of points that are outliers have L-1 error of ≥φmax=0.1
by construction.
Our MIP formulation converged to the optimal solution and certiﬁed its global
optimality to within a MIP gap of 5% (Fig. 4). This desired MIP gap is tunable, and
trades off with runtime. This gap of 5% was chosen arbitrarily, and corresponds to an
optimality gap of 0 .033×0.05=0.00165 average L-1 error over the scene points.
We used 4 binary variables per element of R. The largest elementwise infeasibil-
ity of RTR=Ibeing 0 .020, and det (R)was 1.002. These values reﬂect that the
approximation of R∈SO (3)is reasonably tight.
Globally Optimal Object Pose Estimation … 703
Fig. 4 Pose estimate produced by our MIP formulation with a cube model of 12 triangular faces,
given 15 scene points with 5 outliers. The solution shown here has optimal cost that matches theoptimal cost of the ground truth solution. Optimality of this solution was certiﬁed to a MIP gap of5%. To p l e f t : Ground truth pose. To p r i g h t : Pose estimate using our MIP formulation. Bottom :
Convergence time of upper and lower bounds across time for the MIP solution
6.1.2 Characterization of Runtime
To characterize the time scaling of this algorithm with respect to model complexity,
we recorded the runtime of the solver across a set of synthetic scene-model pairs
with varying scene, model, and outlier complexities (Fig. 5). Intriguingly, the char-
acterization example had little noticeable runtime variation when model complexity
was increased, even though the total problem size and number of binary variables
scales linearly with the model complexity. It is possible that, since only the repre-sentation of the underlying geometry is changing (and not the underlying geometry
itself), either the post-presolve MIP or its LP relaxations are not signiﬁcantly dif-
ferent between these cases, in such a way to lead to similar runtimes. The case ofzero outliers showed signiﬁcantly lower runtime than the case of nonzero outliers,
as the case of zero outliers has a trivial lower bound of 0 and thus an easier branch
and bound search. In the case of nonzero outliers, however, there was no clear rela-tionship between outlier ratio and runtime. These two properties make scaling this
method to larger examples more hopeful, though it is unclear if the relationships will
704 G. Izatt et al.
Fig. 5 The time scaling of the convergence time to a MIP gap of 5% as scene, model, and outlier
complexity is varied. Each point represents a complete solve of a problem instance. Scene points
were sampled uniformly from the surface of a cube described by varying numbers of triangularfaces. The vertical red bar indicates the condition that is shared between each experiment (i.e. 18scene points, 12 model faces, 33% outliers). Left : Solve times across a varying number of scene
points, with 12 model faces and 33% intentionally injected outliers. V ertical blue bars indicatesolutions that reached a maximum timeout of 1200 s without converging to a MIP gap. Middle :
Solve times across a varying number of model faces, with 18 scene points and 33% outliers. Right :
Solve times across a varying set of outlier ratios, with 18 scene points and 12 model faces
continue to hold for higher numbers of scene points and signiﬁcantly more complex
models.
6.1.3 Outlier Rejection and Multiple Models
To highlight the extensions of our formulation, we generated similar synthetic point
clouds to test the outlier rejection and multiple model cases, with results shown in
Figs. 6and7. To avoid unreasonably long runtimes, we had to constrain rotations and
limit the search to be over translations and correspondences. Rwas thus constrained
to take the value of the ground truth rotation.
6.1.4 Upper Bounds from ICP
To demonstrate that solutions generated from other efﬁcient but non-global meth-
ods can be leveraged to make our global optimization faster, we implemented an
ICP-based heuristic for generating candidate feasible solutions online during theoptimization. This procedure is directly inspired by GO-ICP [ 37]. Our solver main-
tains a queue of feasible solutions found by the branch and bound algorithm, and
runs point-to-plane ICP with proportional outlier rejection on each feasible solutionin a parallel thread alongside the global optimization solver. If the ICP produces a
solution better than the best currently held by the solver, the ICP solution is handed
to the solver as a heuristically-derived feasible solution. This procedure signiﬁcantlyimproves runtime, as is shown in Fig. 8.
Globally Optimal Object Pose Estimation … 705
Fig. 6 Pose estimates produced by our MIP formulation for a cube model of 12 triangular faces,
given 100 scene points, with a varying number of them being outliers: Left : 50% outliers, con-
verged in 50 s. Middle : 80% outliers, converged in 400 s. Right : 90% outliers, converged in 2000 s.
Rotations were frozen to the ground truth rotation in order to produce these solutions in reasonabletime. All solutions shown here have optimal cost that matches the optimal cost of the ground truthsolution and align with the ground truth pose. Optimality of these solutions were certiﬁed to a MIPgap of 5%
Fig. 7 Pose estimates
produced by our MIPformulation simultaneouslyﬁtting two box models to 100scene points with no outliers.Rotations were frozen to theground truth rotations inorder to produce thesesolutions in reasonable time.Convergence took 1100 s
Fig. 8 Comparison of the
upper bound convergencebehavior of the MIPformulation with 50 scenepoints and 0 outliers ﬁtting abox model, with and withoutan ICP algorithm generatingnovel feasible solutions inparallel. The lower bound isomitted, as it is trivially 0 forthe 0 outlier case

706 G. Izatt et al.
6.2 Experiments on Real-World Data
To illustrate how this method compares to other global pose estimation methods
when applied to real-world data, we provide experiments comparing the performanceof GO-ICP [ 37], Super4PCS [ 24], and our method (MIP) on a set of examples
drawn from point clouds collected from ofﬁce and laboratory environments [ 21].
Examples were generated by cropping each complete point cloud (Fig. 9) around
the object of interest. No additional cleanup was performed to remove outliers or
other clutter, and each method was supplied with only this raw cropped point cloud.
GO-ICP and Super4PCS were chosen as representative methods from the approachesof branch-and-bound search and sampling methods respectively. The parameters of
each method were hand-tuned to improve performance, but held constant across the
entire dataset. In order to keep timings reasonable, we terminated our MIP solver
after 60 s and took as its answer its current best feasible solution. We used ICP to
provide candidate feasible solutions as described in Sect. 6.1.4 , resulting in the MIP
system functioning as a hybrid method that used ICP to make local progress, while
using MIP heuristics and branch-and-bound search to generate new seeds for ICP to
explore.
While each method was successful on a number of examples (e.g. Fig. 10), this
dataset proved extremely difﬁcult for all methods (Fig. 11). In almost all cases, incor-
rect solutions were the consequence of clutter and incorrect outlier handling.
Fig. 9 An example pointcloud from our testing dataset. The ground-truth location of the object
of interest in the pointcloud (in this case, the drill on the table) is known precisely. Test cases aredrawn by cropping the point cloud around the known object location with varying crop sizes
Globally Optimal Object Pose Estimation … 707
Fig. 10 Example performance of GOICP ( Left ), our solver’s best solution after 60 s ( Middle ), and
Super4PCS ( Right ), given 25 cm crops around the true object location without additional outlier
removal. The ground-truth object location is shown in purple
GOICP MIP Super4PCS
% correct 13.0% 12.6% 8.9%
Fig. 11 Histogram of translation and angle error of pose estimate of each method across a set of
246 test instances. The MIP results presented here were generated by taking the best solution found
by the branch-and-bound search after 60 s, and thus are not certiﬁed /epsilon1-optimal. While the object-
centered cropping made the translation easy to estimate, the extreme clutter of the examples greatly
injured all methods. Approximate rotational symmetries in the most common object (a drill) caused
many erroneous solutions with 180-degree error, emphasizing the vulnerability of even these global
methods to confusing clutter. Using a threshold of 5 cm translation error and 10◦angle error, only
approximately 10% of examples were estimated correctly by a given method
708 G. Izatt et al.
7 Conclusion
The formulations we present can be used to ﬁnd certiﬁed /epsilon1-globally-optimal solu-
tions for small numbers of scene points and outliers, even in the face of combinatorialcomplexity. The solver is capable of ﬁnding and certifying the right solution, even in
very high outlier ratios, and can optimize with multiple objects seamlessly. That this
technique can so easily incorporate hypotheses from other methods makes it a candi-date for being an ofﬂine veriﬁcation technique for the results from other efﬁcient but
inconsistent pose estimation methods. This functionality is critical when considering
the kinds of highly ambiguous point clouds that result from highly cluttered scenes,and from tactile sensing. However, pilot experiments on real data from natural clut-
tered scenes results underline how elusive reliable global object pose estimation in
complex, practical data remains. A truncated form of our method can hold its own
in terms of performance against competing global pose estimation techniques, when
compared on difﬁcult, unsegmented, colorless point clouds. Further work is requiredto remove the early-stopped requirement and make it tractable to perform the branch
and bound search to convergence for practically sized point clouds and models, as
well as to incorporate RGB and other feature cues.
By framing the core point-cloud object pose estimation problem as a mixed-integer
convex program, our solution method makes clear what it means to examine partial
relaxations of the problem; and in doing so, makes it possible to verify that solutionsare approximately globally optimal, or provide a search region that may contain better
solutions. Further, the class of mixed integer convex programs seems sufﬁciently rich
to capture many of the nuances of this problem—[ 6] shows that, in a similar mixed-
integer convex framework with maximal coordinate rigid body conﬁgurations, it
is possible to enforce complex kinematic constraints including revolute joints and
nonpenetration with an environment. While object tracking algorithms (e.g. [ 30])
have had great success taking advantage of articulations, nonpenetration, and free-
space information to combat ambiguity and reﬁne their results, there remains a great
opportunity for global pose estimation algorithms to utilize these additional sources
of information. We hope that the tools and techniques we have presented will lead to
more reliable and informed algorithms in these directions that can tackle object poseestimation in the complex and occluded scenes that are unavoidable as our robots
move into the natural world.
Acknowledgements This material is based upon work supported by NSF Contract IIS-1427050,
a National Science Foundation Graduate Research Fellowship under Grant No. 1122374, as wellas support from ABB and Draper Laboratory.
Globally Optimal Object Pose Estimation … 709
References
1. Akgül, C.B., Sankur, B., Yemez, Y ., Schmitt, F.: 3d model retrieval using probability density-
based shape descriptors. IEEE Trans. Pattern Anal. Mach. Intell. 31(6), 1117–1133 (2009)
2. Arie-Nachimson, M., Kovalsky, S.Z., Kemelmacher-Shlizerman, I., Singer, A., Basri, R.:
Global motion estimation from point matches. In 2012 second international conference on
3D imaging, modeling, processing, visualization and transmission (3DIMPVT) , pp. 81–88.
IEEE (2012)
3. Besl, P .J., McKay, N.D.: Method for registration of 3-d shapes. In: Robotics-DL tentative, pp.
586–606. International Society for Optics and Photonics (1992)
4. Chaudhury, K.N., Khoo, Y ., Singer, A.: Global registration of multiple point clouds using
semideﬁnite programming. SIAM J. Optim. 25(1), 468–501 (2015)
5. Chen, Y ., Medioni, G.: Object modelling by registration of multiple range images. Image Vis.
Comput. 10(3), 145–155 (1992)
6. Dai, H., Izatt, G., Tedrake, R.: Global inverse kinematics via mixed-integer convex optimization
(2017)
7. Drost, B., Ulrich, M., Navab, N., Ilic, S.: Model globally, match locally: efﬁcient and robust
3d object recognition. In: 2010 IEEE conference on computer vision and pattern recognition(CVPR), pp. 998–1005. IEEE (2010)
8. Dunning, I., Huchette, J., Lubin, M.: Jump: a modeling language for mathematical optimization.
SIAM Rev. 59(2), 295–320 (2017)
9. Eggert, D.W., Lorusso, A., Fisher, R.B.: Estimating 3-d rigid body transformations: a compar-
ison of four major algorithms. Mach. Vis. Appl. 9(5–6), 272–290 (1997)
10. Enqvist, O., Josephson, K., Kahl, F.: Optimal correspondences from pairwise constraints. In:
2009 IEEE 12th International Conference on Computer Vision, pp. 1295–1302. IEEE (2009)
11. Fischler, M.A., Bolles, R.C.: Random sample consensus: a paradigm for model ﬁtting with
applications to image analysis and automated cartography. Commun. ACM 24(6), 381–395
(1981)
12. Gelfand, N., Mitra, N.J., Guibas, L.J., Pottmann, H.: Robust global registration. In: Symposium
on Geometry Processing, vol. 2, p. 5 (2005)
13. G.O. Inc. Gurobi optimizer reference manual (2016)14. Hartley, R.I., Kahl, F.: Global optimization through rotation space search. Int. J. Comput. Vis.
82(1), 64–79 (2009)
15. Hinterstoisser, S., Lepetit, V ., Ilic, S., Holzer, S., Bradski, G., Konolige, K., Navab, N.: Model
based training, detection and pose estimation of texture-less 3d objects in heavily clutteredscenes. In: Asian Conference on Computer Vision, pp. 548–562. Springer (2012)
16. Izatt, G., Mirano, G., Adelson, E., Tedrake, R.: Tracking objects with point clouds from vision
and touch. In: 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE(2017)
17. Johnson, A.E., Hebert, M.: Using spin images for efﬁcient object recognition in cluttered 3d
scenes. IEEE Trans. Pattern Anal. Mach. Intell. 21(5), 433–449 (1999)
18. Klingensmith, M., Koval, M.C., Srinivasa, S.S., Pollard, N.S., Kaess, M.: The manifold particle
ﬁlter for state estimation on high-dimensional implicit manifolds (2016). arXiv:1604.07224
19. Li, H., Hartley, R.: The 3d-3d registration problem revisited. In: IEEE 11th International Con-
ference on Computer Vision, 2007. ICCV 2007, pp. 1–8. IEEE (2007)
20. Li, S., Lyu, S., Trinkle, J.: State estimation for dynamic systems with intermittent contact. In:
2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 3709–3715.IEEE (2015)
21. Marion, P ., Florence, P .R., Manuelli, L., Tedrake, R.: A pipeline for generating ground truth
labels for real rgbd data of cluttered scenes. Under Review (2017)
22. Maron, H., Dym, N., Kezurer, I., Kovalsky, S., Lipman, Y .: Point registration via efﬁcient
convex relaxation. ACM Trans. Gr. (TOG) 35(4), 73 (2016)
23. McCormick, G.P .: Computability of global solutions to factorable nonconvex programs: part
i-convex underestimating problems. Math. Program. 10(1), 147–175 (1976)
710 G. Izatt et al.
24. Mellado, N., Aiger, D., Mitra, N.J.: Super 4pcs fast global pointcloud registration via smart
indexing. In: Computer Graphics Forum, vol. 33, pp. 205–215. Wiley Online Library (2014)
25. Narayanan, V ., Likhachev, M.: Deliberative object pose estimation in clutter. In: 2017 IEEE
International Conference on Robotics and Automation (ICRA), pp. 3125–3130. IEEE (2017)
26. Nemhauser, G.L., Wolsey, L.A., Integer programming and combinatorial optimization. Wiley,
Chichester. Nemhauser, G.L., Savelsbergh, M.W.P ., Sigismondi, G.S.: Constraint classiﬁcationfor mixed integer programming formulations. COAL Bull. 20(8–12) (1988) (1992)
27. Olsson, C., Kahl, F., Oskarsson, M.: Branch-and-bound methods for euclidean registration
problems. IEEE Trans. Pattern Anal. Mach. Intell. 31(5), 783–794 (2009)
28. Papazov, C., Burschka, D.: An efﬁcient ransac for 3d object recognition in noisy and occluded
scenes. Comput. Vis.-ACCV 2010 , 135–148 (2011)
29. Rosen, D.M., Carlone, L., Bandeira, A.S., Leonard, J.J.: Se-sync: a certiﬁably correct algorithm
for synchronization over the special Euclidean group (2016)
30. Schmidt, T., Hertkorn, K., Newcombe, R., Marton, Z., Suppa, M., Fox, D.: Depth-based tracking
with physical constraints for robot manipulation. In: 2015 IEEE International Conference onRobotics and Automation (ICRA), pp. 119–126. IEEE (2015)
31. Schmidt, T., Newcombe, R., Fox, D.: Dart: dense articulated real-time tracking with consumer
depth cameras. Auton. Robots 39(3), 239–258 (2015)
32. Schulman, J., Lee, A., Ho, J., Abbeel, P .: Tracking deformable objects with point clouds. In:
2013 IEEE International Conference on Robotics and Automation (ICRA), pp. 1130–1137.IEEE (2013)
33. Tedrake, R., The Drake Development Team: Drake: A planning, control, and analysis toolbox
for nonlinear dynamical systems (2016)
34. Tombari, F., Salti, S., Di Stefano, L.: Unique signatures of histograms for local surface descrip-
tion. In: European Conference on Computer Vision, pp. 356–369. Springer (2010)
35. Wohlhart, P ., Lepetit, V .: Learning descriptors for object recognition and 3d pose estimation.
In: The IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2015)
36. Wong, J.M., Kee, V ., Le, T., Wagner, S., Mariottini, G.-L., Schneider, A., Hamilton, L.,
Chipalkatty, R., Hebert, M., Johnson, D. et al.: Segicp: integrated deep semantic segmenta-tion and pose estimation (2017). arXiv:1703.01661
37. Yang, J., Li, H., Campbell, D., Jia, Y .: Go-icp: a globally optimal solution to 3d icp point-set
registration. IEEE Trans. Pattern Anal. Mach. Intell. 38(11), 2241–2254 (2016)
38. Zhou, Q.-Y ., Park, J., Koltun, V .: Fast global registration. In: European Conference on Computer
Vision, pp. 766–782. Springer (2016)
Probabilistic Boundary Coverage
for Unknown T arget Fields with LargePerception Uncertainty and Limited
Sensing Range
Binbin Li and Dezhen Song
1 Introduction
Imagine that an unmanned aerial vehicle (UA V) is dispatched to map boundaries of
excessive wind shear or low pressure regions in storm cells (see Fig. 1). The UA V
has to plan its motion based on its on-board sensor readings to quickly enclose
the unknown target ﬁeld (UTF), which is a form of boundary coverage problem.
However, UTFs often do not have a clear boundary or a priorly known dispersion
function and the UA V has to get closer to the ﬁeld to take multiple readings to predict
ﬁeld dispersion for boundary coverage. Moreover, the sensor readings often containlarge uncertainties due to variations of the ﬁeld itself or difﬁcult sensing conditions. It
is clear that regular boundary traversing techniques are not applicable. Such problems
are not unusual. Another example is that an inspection robot is tasked to ﬁnd thinhairline cracks on airport runway. These applications propose a new problem: how
can we design a principled approach to ensure the robot can effectively cage UTFs
under large perception uncertainty and limited sensing range.
We present this new boundary coverage problem and propose an algorithm to
solve the problem. At each step, the robot accumulates sufﬁcient sensory readings to
instantiate Gaussian processes (GPs) as a local belief function to approximate ﬁelddispersion in an ellipse-shaped local region. The local belief function allows us to
predict UTF boundary trends and establish adjacent ellipses for further exploration
in next step. The process is governed by a depth-ﬁrst search process until UTF
This work was supported in part by National Science Foundation under IIS-1318638,
NRI-1426752, NRI-1526200, and NRI-1748161.
B. Li·D. Song (B)
Department of Computer Science and Engineering, Texas A&M University,College Station, TX 77843, USAe-mail: dzsong@cse.tamu.edu
B. Li
e-mail: binbinli@cse.tamu.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_50711
712 B. Li and D. Song
(a) (b)
AqAg
(xp,yp)(xg,yg)
AqAg
(xq,yq)
A0
x0x1
Fig. 1 Problem illustration. aWind shear region boundary coverage application: The green and
orange clouds represent potential regions of interest that may contain UTFs. To map each UTF, we
need to send an UA V to cage the UTF. bOutput of our boundary coverage algorithm is to cover the
boundary using a sequence of connected ellipses
is enclosed by connected ellipses (see Fig. 1b) with probability guarantees, as we
formally prove that our boundary coverage process guarantees that the enclosed
UTF is above a given coverage ratio with a preset probability threshold. We have
implemented our method and tested with different types of UTFs (1D vs. 2D, smooth
vs. non-smooth boundary, and convex vs. non-convex) in simulation. The results
show that the algorithm always guarantees that the coverage ratio is above the given
threshold for all testing cases, which is conformal to our analysis.
2 Related Work
This new UTF boundary coverage problem relates to many existing works including
coverage in continuous ﬁelds, discrete space search, robotic caging in manipulation
and grasping, and GPs.
Boundary coverage of UTFs is related to well-studied coverage problems because
the latter also need to identify target ﬁeld boundary before planning for coverage.
However, identifying boundary might not be an issue if target ﬁeld information is
known whereas our problems focus on identifying boundaries. In coverage prob-
lems, the focus is to calculate optimal trajectories with respect to a given objective
function for known ﬁeld functions. Most existing methods depend on gradients or
other information from the known ﬁeld functions. Y un et al. [ 32] present the decen-
tralized algorithms for the coverage with mobile robots on a graph. Miller et al.
[16] use an ergodic control algorithm for the coverage with respect to the expected
information density. Shnaps et al. [ 24] perform online tethered coverage in planar
unknown environments using position and local obstacle detection sensors. Bekris
et al. [ 2] apply cloud computing to efﬁciently plan the motion of new robot manipu-
lators designed for ﬂexible manufacturing ﬂoors. In our boundary coverage problem,
we have to approximate ﬁeld functions based on noisy and local/partial observations
before planning for robot motion which further complicates the problem.
Probabilistic Boundary Coverage for Unknown Target Fields … 713
Searching for point/small objects without ﬁeld functions can be viewed as a dis-
crete search problem. The original search space could be either continuous or discrete
but it is often discretized into grids or graphs in the searching process [ 5]. Acar et al.
[1] introduce a hierarchical decomposition that combines the Morse decompositions
and the generalized V oronoi diagram to ensure that the robot covers the searching
domain. Paull et al. [ 17] present a sensor driven on-line approach for seabed cover-
age for mine countermeasure using grid-based coverage. Xu et al. [ 30] address the
problem of effective graph coverage with environmental constraints and incomplete
prior map information. Mannadiar et al. [ 14] guarantee the complete coverage of
the free space based on the Boustrophedon cellular decomposition. Although these
methods can be applied to UTF searching problem, their efﬁciency is problematic
because the existing methods do not exploit the continuity of UTF structure.
Boundary coverage of UTFs is related to the caging problem in grasping which
focuses on using geometric information of the manipulated object to generate stable
grasps [ 6,13,19,23,29]. V ongmasa et al. [ 28] compute coverage parameters for
2D polygons to form a cage to transport an object. Pereira et al. [ 18] enable a team
of robots with limited sensing range to achieve a condition of object closure, and
move toward a goal position while maintaining the object closure condition. Ivan
et al. [ 7] compute coverage without the reliance to geometrical detail but capture
the topology of punctured euclidean spaces. Zarubin et al. [ 33] use geodesic balls
to estimate object’s surface in the presence of noise. These methods compute the
waypoints for the object to generate a set of caging grasps. One issue that separates
UTF caging from object caging is the issue of limited sensing range because objectsare often small and fully covered by the sensor in the grasping process.
GPs become a powerful tool to represent unknown environments with limited
observations [ 21]. GPs are widely used in geographical terrains [ 3,27], nonlinear
systems [ 31], complex environments [ 9,12,15], autonomous robotic vehicles [ 4],
solar map constructions [ 20], and sensor networks [ 8] etc. Furthermore, GPs are
capable to deal with the sensor information with uncertainties. In this case, we utilizeGPs to approximate the distribution of the UTFs based on existing observations.
Our group has experiences in robotic searching problems. Our previous work
develops algorithms and systems to detect an unknown wireless sensor network thatintermittently transmits signals [ 10,11,25,26]. The focus of our previous work is
the modeling of stochastic properties of point targets where this work focuses on
ﬁeld targets.
3 Problem Formulation
3.1 Scenario and Assumptions
A mobile robot or UA V is dispatched to ﬁnd an efﬁcient path to enclose UTFs in
an obstacle-free 2D space. This robot is equipped of sensors with a limited sensing
714 B. Li and D. Song
range. The robot observation noise follows a Gaussian distribution with zero mean
and variance σ2. To formulate the UTF boundary coverage problem, we have the
following assumptions:
1. We assume that the UTF is much larger than the sensing capabilities of the robot,
and the moving speed of the robot is much faster than that of the UTF. This is
common for large scale coverage occurring on the surface of the Earth.
2. The robot knows its current position using global positioning system and has a
memory of where the robot has visited before.
3. We have no knowledge about UTF shapes.
4. GPs are capable of approximating the UTF boundary distribution.
3.2 UTF Properties and Modeling Perception
To further clarify our problem, let us deﬁne UTF and its key properties. Denote ztto be
the sensor readings at time twhen the robot is at position xt=[x(t)y(t)]T∈R2, and
setZT={ z1,z2,..., zT}as all observations sensed from the beginning of localization
process up to time T. Denote Tto be the UTF region and x=[x,y]T∈R2to be a
point in the 2d space. Tis usually obtained by thresholding ﬁeld boundary distribution
function or geometric constraints which are described as follows,
T:=⎧
⎨
⎩x/vextendsingle/vextendsingle/vextendsingle/braceleftBig
I
IN=/logicalanddisplay
zt∈ZT/parenleftbig
f(zt,xt,σ)≥ft/parenrightbig/bracerightBig
=1( i sT R U E )⎫
⎬
⎭, (1)
where indicator variable IIN∈{0,1}is binary (boolean) variable depending on if the
thresholding criteria are satisﬁed,/logicalandtextis logic AND operator, fis a nonnegative ﬁeld
boundary distribution function, σis the standard deviation of observation noise, and
thresholding value ftfor ﬁeld value is predetermined by application.
Unfortunately, there is often no prior knowledge about shape and position of UTFs.
Instead of computing from geometric constraints, indicator variable IINvalues are
often obtained by thresholding sensory readings. We assume the robot is equipped
with an omni-directional sensor with the maximum sensing distance ds.I fds=0, the
sensor becomes a point sensor such as a barometer measuring air pressure changes
at its current position; if ds>0, the sensor may have measurable coverage like a
camera covering the UTF. Let ∂Tbe the T’s boundary. ∂Twould be unmeasurable
if there were no uncertainty in the sensing process. Due to the fact that ztis normally
distributed, we have,
∂T:=/braceleftBig
x/vextendsingle/vextendsingle/vextendsingleIUTF=1,x∈T/bracerightBig
, (2)
where IUTF∈{0,1}is a binary indicator variable describing if the point is a boundary
point and
Probabilistic Boundary Coverage for Unknown Target Fields … 715
IUTF=/parenleftBig/logicalordisplay
zt∈ZT/parenleftbig
f(zt,xt,σ)=ft/parenrightbig/parenrightBig/logicalanddisplay/parenleftbig
IIN=1/parenrightbig
. (3)
It is worth noting that ( 3) usually cannot be directly computed since we do not know
f. With observations from multiple sensor readings, it can be predicted by a GP
based on observations (zt,xt). To ensure the coverage of a UTF, we just need to
cover its boundary ∂T. The coverage of UTF interior is trivial if ∂Tis covered.
3.3 Problem Deﬁnition
To quantify the boundary coverage performance, we need to deﬁne a performance
metric to determine the trade-off between quality and effort: let β∈(0,1]be the
coverage ratio threshold for UTF boundary. Denote STto be the area of the UTF’s
boundary ∂T. Noted the UTF’s boundary is a 2D region due to robot sensing uncer-
tainties. We have the following boundary coverage success metric.
Condition 1 (Quality Metric) The boundary coverage task for the UTF is considered
to be accomplished if the covered boundary is no less than βSTwhere S Tis the area
of the UTF’s boundary ∂T.
With inputs and quality metric deﬁned, our problem is described as follows,
Problem 1 Given the observation set ZT, plan robot trajectory xT+1based on xTto
generate ellipses to cover ∂Twith Condition 1satisﬁed.
4 System Modeling
The robot starts the boundary coverage process when it encounters an UTF region.
The boundary coverage process generates a sequence of ellipses to enclose the UTF,which are treated as nodes for a depth-ﬁrst search of a tree. Employing a sequence of
ellipses allows us to break down a long boundary traversing problem into a sequence
of local problems to reduce problem scale. In each local problem, we can handlechallenges associated with limited sensing range and observation uncertainty. We
arrange each ellipse to have its long axis aligned with the boundary of the UTF
to speed up the boundary converge process. At each ellipse, the robot accumulates
observations to instantiate a Gaussian Process (GP) to predict the position of next
ellipse along the boundary. The depth-ﬁrst search ensures that the robot traversesthe entire UTF boundary and coordinates ellipse generation as tree expansion which
will be detailed later.
Let us clarify the use of index variables in the depth-ﬁrst search progress. Ellipse
A
qis where the robot is currently located. Index grefers to the total number of
ellipses. Since we start the index with its root at A0,Agis also the new ellipse during
716 B. Li and D. Song
new node generation. Aprefers to which neighboring ellipse the robot uses to enter
Aq. The “neighboring ellipse” refers to either parent or child nodes of qon the search
tree.
The whole process consists of two main steps: initialization and boundary travers-
ing. We start with system initialization.
4.1 Ellipses, Robot Trajectory, Observation Set, and
Initialization of the Depth-First Search
The boundary coverage process relies on a sequence of ellipses to track UTF bound-
aries. Deﬁne Aqas the qth ellipse,
Aq=/braceleftBig
x/vextendsingle/vextendsingle/vextendsingle/parenleftbig
x−xq/parenrightbigTCq/parenleftbig
x−xq/parenrightbig
≤1/bracerightBig
, (4)
where xq=[xq,yq]Tis its center point and Cqis a 2×2 positive deﬁnite matrix.
When the robot enters Aqfrom a neighboring ellipse center xpto current ellipse
center xqalong the shortest path, the robot trajectory set uqinAqcan be deﬁned as,
uq={ x|x=ρxp+(1−ρ)xq,ρ∈[0,1]} (5)
if ignoring obstacles. Denote tj∈Ras the exact continuous time at the moment of
the discrete time jwhen the robot with constant velocity traverses from xptoxq.
Lettj−tj−1=c0forj>0 where c0∈R++is a constant variable. The index j
is reset to zero every time when the robot reaches xq. During the travel, the robot
accumulates observations from its on-board sensor to establish its observation set
Oq,
Oq=/braceleftbig
(x,ztj)/vextendsingle/vextendsingle|x−xtj|≤ds,xtj∈uq/bracerightbig
, (6)
and denote x∈Xq. Figure 2a illustrates the observation set Oqcoverage in Aq.
The ellipse generation process initializes at the moment when the robot ﬁrst
encounters an UTF at point x0. It immediately generates A0which is chosen to
Aqxp xq
(a) Accumulate obs. in AqAqxgLq(λ) xp
Ag
xq
(b) Compute Lq()and obtain Ag
Fig. 2 The robot accumulates observations in Aqin blue shaded area in a, establishes belief
functions in Aqusing a GP based on observation set Oq, which assists in determining Aginb
Probabilistic Boundary Coverage for Unknown Target Fields … 717
be a circle because it is likely that we do not have enough information to determine
boundary direction yet. For A0,w eh a v e
C0=/bracketleftbigg1/4d2
s 0
01/4d2
s/bracketrightbigg
. (7)
On the other hand, u0is slightly different from ( 5) because we do not have a neigh-
boring ellipse. Alternatively, we substitute xpwith xenter in (5) to obtain u0where
xenter is the point of entry to A0during the global search process. Consequently, we
have a non-empty observation set O0.
4.2 Depth-First Search-Based Boundary Traversing
We employ a depth-ﬁrst search over a tree, which contains all ellipses as tree nodes.
Ellipses A0is the root node of the ellipse tree. Each node qstores its uqand Oq.
Oqis updated as the robot travels inside the ellipse. As mentioned before, the robot
only moves between ellipse centers xqof neighboring tree nodes along a linear path
because we ignore the obstacle in the process. This also yields a piecewise linear
trajectory for the robot.
4.2.1 Branching Method
At each ellipse Aq, the robot uses Oqto instantiate a GP [ 21] approximating the
belief function for ∂TinAqwhich provides information to determine Ag.M o r e
speciﬁcally, for an x∈Aq, the GP provides posterior distribution P(IUTF|x,Oq)for
the UTF region to stand for the ﬁeld function. It is worth noting that Oqtheoretically
contains inﬁnite number of observations. To facilitate computation, we sample Oq
using a local lattice according to sensor spatial resolution or task needs to accelerate
GP training time. Recall Oqis continuously updated according to robot trajectory
set. This leads to a recursive Bayesian estimation process, which can be computed
using a two-phase approach [ 22]:
1. Update Phase: For an x∗∈Oq,
P(I∗
UTF|x∗,Oq)=bel(I∗
UTF|f∗(x∗),σ2), (8)
where the latent function f∗is represented by GP , an approximation of ( 3).
2. Prediction Phase: The GP provides posterior distribution for the UTF boundary
for a given x/∈Oq,
P(IUTF|x,Oq)=bel(IUTF|μI,σ2
I). (9)
718 B. Li and D. Song
Here, μIandσ2
Iare the expectation and variance of the posterior distribution
related to kernel function, which characterizes the correlation between the func-
tion values at different locations, namely, f∗(xi)and f∗(xj).W ee m p l o yah i s -
togram intersection kernel Kas
K(xi,xj)=2/summationdisplay
d=1min(xi(d),xj(d)). (10)
for xi,xj∈R2withμI=kT
∗(K+σ2I)−1I∗
UTFandσ2
I=k∗∗kT
∗(K+σ2I)−1k∗
where k∗=K(Xq,x),Kis the kernel matrix of the training data Xq,k∗∗=
K(x,x), and Iis an identity matrix.
To approximate UTF boundary ∂T, we calculate the level set Lq(λ)where thresh-
oldλ> 0, to cover regions with high probability of containing the boundary. This is
done by thresholding on P(IUTF|x,Oq).
Lq(λ)=/braceleftbig
x|P(IUTF|x,Oq)≥λ,x∈Aq/bracerightbig
. (11)
The value of threshold λis determined by coverage ratio threshold βin Condition 1
and will be discussed in Sect. 5.2.
Now let us show how to determine xg, center of the new node on the ellipse tree.
The boundary of Lq(λ)intercepts the boundary of Aqand generates a set of points
XL
q. As illustrated Fig. 2b, we evenly divide Aqintokd=12 sectors with each sector
spans 2 π/kd=π/6. For each sector, we identify a middle angle boundary point
xs
qby intercepting Aqboundary with the ray shooting from the ellipse center along
the middle angle ( π/kdfrom the sector side). We add xs
qto the candidate solution
set X∗
qfor xgif we can ﬁnd a solution in set XL
qlocated on the corresponding
sector boundary. This means that we use XL
qto ﬁlter out less likely candidate center
locations. To avoid repeated search, we remove xs
qof the sector where the robot enters
Aqfrom the candidate solution set X∗
q. Therefore, set X∗
qonly contains branches that
robot has not visited.
4.2.2 T ermination Scenarios
With X∗
qintroduced, let us explain the termination condition of the search, which has
two scenarios. When X∗
qis empty, it means that the robot reaches the extreme end
of the UTF which is the leaf of the depth-ﬁrst search tree. Now the only choice is to
let the robot traverse back on the tree to the parent ellipse and check if the candidate
solution set of the parent ellipse is non-empty. If a non-empty node is found, weenter the tree expansion case as described in the sub-section. Otherwise, we keep
back-traversing the robot and repeat this process along the tree to upper level parent
node. We update pandqin the process.
If we return to the root and ﬁnd X
∗
0is empty, it means the robot has covered the
entire search tree in the boundary traversing. This concludes the ﬁrst termination
Probabilistic Boundary Coverage for Unknown Target Fields … 719
scenario and the depth-ﬁrst search ends. This scenario occurs a lot with curves, lines
or other thin UTFs.
For a compact and sizable UTF, it is likely that the robot loops around the UTF
(see Fig. 1b), which is the second termination scenario. Because it leads the robot to
travel back to A0, we can identify this by verifying if xq∈A0is true. We also need
to remove the candidate solution from the sector that contains xqinX∗
0.A g a i n ,i f X∗
0
is empty, the search ends.
4.2.3 Node/Ellipse Generation
Next is about tree expansion or node generation process. It happens if the candidate
solution set X∗
qis non-empty for the original qor the updated qfrom the back-
traversing process, then we choose the xgas follows,
xg=arg max
x∈X∗q/parenleftbigg
arccos/angbracketleftxq−,x∗,q/angbracketright
/bardblxq−/bardbl·/bardbl x∗,q/bardbl/parenrightbigg
. (12)
where xq−=xq−xprepresents the vector describing how the robot enters Aq,x∗,q=
x−xq,/bardbl·/bardblis vector l-2 norm, and /angbracketleft·,·/angbracketrightis vector inner product. This means that the
candidate is the closest to the direction that the robot enters Aq. Once xgis chosen, we
remove it from X∗
q=X∗
q\{xg}.This is to avoid repeated search when we return to
Aqin the depth-ﬁrst search process. Again, set X∗
qkeeps track of the visited branch of
the search tree. New node gis added to the tree with its parent to be qandg=g+1.
After obtaining new center xg, we place Aginto its position by determining Cg.
Set the long and short axes of Agto be 4 dsand 2 ds, respectively. To approximate
the UTF boundary, we want the long axis of Agto be aligned with xg−xqas
follows,
Cg=/bracketleftbiggcosθ−sinθ
sinθcosθ/bracketrightbigg/bracketleftBigg1
16d2s0
01
4d2s/bracketrightBigg/bracketleftbiggcosθsinθ
−sinθcosθ/bracketrightbigg
, (13)
where θis determined by
θ=arctan/parenleftbiggyg−yq
xg−xq/parenrightbigg
. (14)
After Agis determined, the robot motion is also obtained as ug. As the robot
moves toward xg, we update X∗
q,p, and qaccordingly.
5 Boundary Coverage Performance Analysis
The remaining question is how good the boundary coverage quality is and how to
guarantee Condition 1. We ﬁrst analyze the coverage quality for a point xinAqand
then aggregate it into the entire boundary.
720 B. Li and D. Song
5.1 Probability Bounds for a Point xinAq
The UTF boundary ∂Tis covered by the level set Lq(λ)which is generated by the
thresholding on P(IUTF|x,Oq)in (11). It is important to understand P(IUTF|x,Oq).
P(IUTF|x,Oq)is a function of its condition (x,Oq)which means it is still a random
variable because (x,Oq)are random variables. Since it is a random variable, it has
an expectation E/parenleftbig
P(IUTF|x,Oq)/parenrightbig
.E/parenleftbig
P(IUTF|x,Oq)/parenrightbig
can be estimated by averaging
P(IUTF|x,Oq)across all points using the output from GPs. E/parenleftbig
P(IUTF|x,Oq)/parenrightbig
can
be viewed as an observation of the unconditional distribution P(IUTF).P(IUTF)is the
mean value of P(IUTF|x,Oq).P(IUTF)is important because it can help us to determine
if we miss any UTF boundary points when performing thresholding in ( 11). We do
not know P(IUTF)but we know its low bound in probability as follows.
Lemma 1 With 1 −τprobability, P(IUTF)which is the unconditional probability
that a point in Aqbelongs to UTF boundary has the following lower bound B−
q,
P(IUTF)≥B−
q=inf
η>0/braceleftbigg
E(P(IUTF|x,Oq))−η
2−1
lmaxηlog1
τ/bracerightbigg
, (15)
where τ∈(0,1)is a chosen small number, lmaxis the set cardinality of Lq(λ), and
nonnegative variable ηis determined by the inf computation.
Proof The lower bound of P(IUTF)can be proved by relating it to its estimator
E(P(IUTF|x,Oq)). We construct the following probability event E0fort>−1,
P(E0)=P/parenleftBig
P(IUTF)−η
2−E/parenleftbig
P(IUTF|x,Oq)/parenrightbig
≤−(t+1)/parenrightBig
, (16)
This is equivalent to,
P(E0)=P(e−ηlmax(P(IUTF)−η
2−E(P(IUTF|x,Oq)))≥eηlmax(t+1)), (17)
and apply Markov’s inequality, we have,
P(E0)≤e−ηlmax(t+1)E/parenleftBig
e−ηlmax(P(IUTF)−η
2−E(P(IUTF|x,Oq)))/parenrightBig
. (18)
Since eηlmaxη
2≥1, we multiply it to the right hand side of ( 18) and move the constant
terms outside the expectation E(·),w eh a v e
P(E0)≤e−ηlmax(t−η
2)−ηlmax(P(IUTF)−η
2)
×E/parenleftBig
eηlmax(E(P(IUTF|x,Oq))−1)/parenrightBig
(19)
Using the fact that E(P(IUTF|x,Oq))≤1, we know
Probabilistic Boundary Coverage for Unknown Target Fields … 721
P(E0)≤e−ηlmax(t−η
2)−ηlmaxP(IUTF)+η2lmax
2E/parenleftbig
e0/parenrightbig
, (20)
which leads to
P(E0)≤e−ηlmax(t−η
2)+η2lmax
2=τ. (21)
We can solve tfrom ( 21) and plug it back to ( 16) and the lemma is proved.
5.2 Probability of Covering an UTF Boundary Point in Level
Set Construction
Lower bound B−
qcan help us determine the probability that the UTF boundary
is captured by the GP in level set construction ( 11). It also helps determine how to
choose λ. It is clear that a reasonable λshould be smaller than B−
q. Deﬁning F(μ,σ2)(x)
as the cumulative probability function of the Gaussian distribution N(μ,σ2),w eh a v e
the following lemma,
Lemma 2 For a given lower bound B−
qandλ≤B−
qat point x∈Aqwhen computing
the level set Lq(λ), the probability that an UTF boundary point satisﬁes ( 11)i sn o
less than (1−F(B−q,σ2
I)(λ))( 1−τ)where σ2
Iis the variance of P(IUTF|x,Oq).
Proof From GP model, we know P(IUTF|x,Oq)is a Gaussian distribution with mean
μI=P(IUTF)and variance σ2
I. For a given λandB−
q, the fact that an UTF boundary
point is captured means the following conditional event P(IUTF|x,Oq)≥λ|μI≥B−
q
occurs. We now compute its probability by further conditioning on E0and applying
the fact that B−
qis not a deterministic bound,
P(P(IUTF|x,Oq)≥λ|μI≥B−
q)
=P(P(IUTF|x,Oq)≥λ|μI≥B−
q,E0)(1−τ)
+P(P(IUTF|x,Oq)≥λ|μI≥B−
q,E0)τ
≥P(P(IUTF|x,Oq)≥λ|μI≥B−
q,E0)(1−τ)
≥P(P(IUTF|x,Oq)≥λ|μI=B−
q,E0)(1−τ). (22)
Therefore, we have
P(P(IUTF|x,Oq)≥λ|μI≥B−
q))≥(1−F(B−q,σ2
I)(λ))( 1−τ). (23)
Note that σ2
Ican be obtained using GP outputs.
722 B. Li and D. Song
5.3 Ensure Boundary Coverage Quality
Recall that we have coverage ratio threshold βin boundary coverage quality metric
deﬁned in Condition 1in our problem deﬁnition in Sect. 3.3. To satisfy the condition,
we choose λandτaccordingly. This means that we can set
(1−F(B−q,σ2
I)(λ))( 1−τ)=β, (24)
to help obtain correct threshold λ. We do have some freedom in choosing τto assist
the selection of λ. It is not difﬁcult to perform a binary search to obtain it.
5.4 Algorithm and Complexity Analysis
Algorithm 1: Boundary Coverage of an UTF
Input : robot observations Oq
Output : robot trajectory in set G
1Generate A0atx0; ⊿O(1)
2Stack S={ } ; ⊿O(1)
3Push(S,x0); ⊿O(1)
4while S is not empty do ⊿O(ν)
5 xq=Pop(S); ⊿O(1)
6 ifVisited (xq):=FALSE then
7 Visited (xq):=TRUE; ⊿O(1)
8 Update pandqaccording to trajectory; ⊿O(1)
9 P(IUTF|x,Oq)using GPs; ⊿O(nqlognq)
10 Obtain B−
qaccording to ( 15); ⊿O(logns)
11 Calculate λthrough ( 24); ⊿O(1)
12 X∗
q=/uniontextxs
q\{xp}; ⊿O(1)
13 if X∗
q=∅ orxq∈A0for q/negationslash=0then
14 ifRobot at A0then
15 Break; ⊿O(1)
16 else
17 Travese back to parent node; ⊿O(1)
18 else
19 Obtain Agand move to xg; ⊿O(1)
20 Push(S,xg); ⊿O(1)
21 G=G/uniontext{xg}; ⊿O(1)
Algorithm 1 summarizes the proposed method. To overcome the computational
limitations of naive GPs with time complexity O(n3
q), we employ a Gaussian pro-
cess with generalized histogram intersection kernels to speed up the naive GPs to
Probabilistic Boundary Coverage for Unknown Target Fields … 723
O(nqlognq)where nqis the set cardinality of Oq[22]. Recall that νrefers the
number of ellipse centers, and we initially set Oq=∅,X∗
q=∅, and G=∅.F o rt h e
coverage process, Algorithm 1 details the pseudocode, which leads to the followingcomplexity result.
Lemma 3 Boundary coverage algorithm for an UTF runs in O(νn
qlognq)time,
where nqis the set cardinality of Oq, andνis the number of ellipses to enclose the
UTF.
6 Experimental Result
We have implemented the proposed method in Matlab on a Laptop PC with an Intel(R)
CoreTM2 i7-3517U CPU@1.90GHz and 8 GB memory. To verify the proposed local
coverage method, we simulate different ﬁeld shapes to test our approach (see Table 1).
It includes both simple geometric shapes such as lines, circles, squares, and two
complex shapes including storm cells and an island. Figure 3shows the images of
the two complex shapes. Each image has a resolution of 720 ×480 pixels.
To measure our algorithm’s boundary coverage capability, we deﬁne ALas the
boundary area covered using our method, and Aas the actual area the UTF boundary
occupies. We evaluate the coverage ratio βrby using:
βr=|AL∩A|
|A|. (25)
We set the value ds=1 for the simple geometric shapes and dsequal to 20 pixels
for the two image-based case.
The experimental settings and results are shown in Table 1. The last two rows
show that for a given different threshold β, our algorithm has guaranteed that the
actual coverage is no less than βfor all testing cases, which is conformal to our
Ta b l e 1 Local coverage experiment settings and results
UTF Type fj(x) Dimension Boundary Shape βr(%) β(%)
Line 2x−y+5=0
with 3 ≤x≤121D Smooth Convex 98.12 95.00
Circle 25−(x−6)2−
(y−6)2≥02D Smooth Convex 97.56 95.00
Square |x−5|≤
6,|y−5|≤62D Non-
smoothConvex 93.34 90.00
Storm cell See Fig. 3a 2D Non-
smoothNon-convex 87.32 85.00
Island See Fig. 3b 2D Non-
smoothNon-convex 88.59 85.00
724 B. Li and D. Song
Fig. 3 Local coverage testing with real image data and robot coverage path. aA weather radar
map showing a storm cell. bAn island map
analysis. Also, the sample robot trajectories for boundary traversing are illustrated
as green piecewise linear curves in the right side of Fig. 3. It is clear that the robot
successfully covers the both testing cases.
7 Conclusion and Future Work
We introduced a new UTF boundary coverage problem with many applications. We
reported a probabilistic boundary coverage method for addressing UTF problems.We generated a sequence of ellipses to cover UTF boundary. The introduction of
ellipse sequence also allowed us to decompose the long trajectory traversing prob-
lem to multiple local problems with each ellipse represented a local problem. In eachlocal problem, we employed Gaussian processes (GPs) as a local belief function to
approximate ﬁeld distribution. The local belief function allows us to predict UTF
boundary trends and establish adjacent ellipses for further exploration. The process
was governed by a depth-ﬁrst search process until UTF is approximately enclosed by
connected ellipses. We formally proved that our boundary coverage process guaran-tees the enclosure above a given coverage ratio with a preset probability threshold. We
Probabilistic Boundary Coverage for Unknown Target Fields … 725
implemented our algorithm and successfully tested it in experiments with different
ﬁeld types.
In the future, we will perform physical experiments using bridge deck scanning
and storm cell mapping applications. We will provide overall trajectory length pre-
diction for the algorithm. We will consider a multiple robot team and moving targets.
We will also consider robots/UA Vs with kinodynamic constraints in the trajectorygeneration.
Acknowledgements Thanks for C. Chou, H. Cheng, S. Yeh, A. Kingery, A. Angert, H. Li, and
T. Sun for their inputs and Y . Sun, M. Jin, D. Wang, and Y . Y u for their contributions to the NetBotLaboratory, Texas A&M University.
References
1. Acar, E.U., Choset, H., Lee, J.Y .: Sensor-based coverage with extended range detectors. IEEE
Trans. Robot. 22(1), 189–198 (2006)
2. Bekris, K., Shome, R., Krontiris, A., Dobson, A.: Reducing roadmap size for network trans-
mission in support of cloud automation. IEEE Robot. Autom. Mag. (2016)
3. Bryan, B., Nichol, R.C., Genovese, C.R., Schneider, J., Miller, C.J., Wasserman, L.: Active
learning for identifying function threshold boundaries. In: Advances in Neural InformationProcessing Systems, pp. 163–170 (2006)
4. Chen, J., Low, K.H., Yao, Y ., Jaillet, P .: Gaussian process decentralized data fusion and active
sensing for spatiotemporal trafﬁc modeling and prediction in mobility-on-demand systems.IEEE Trans. Autom. Sci. Eng. 12(3), 901–921 (2015)
5. Chung, T.H., Hollinger, G.A., Isler, V .: Search and pursuit-evasion in mobile robotics. Auton.
Robots 31(4), 299 (2011)
6. Fink, J., Hsieh, M.A., Kumar, V .: Multi-robot manipulation via caging in environments with
obstacles. In: IEEE International Conference on Robotics and Automation, pp. 1471–1476(2008)
7. Ivan, V ., Vijayakumar, S.: Space-time area coverage control for robot motion synthesis. In:
International Conference on Advanced Robotics (ICAR), pp. 207–212. IEEE (2015)
8. Jadaliha, M., Xu, Y ., Choi, J., Johnson, N., Li, W.: Gaussian process regression for sensor
networks under localization uncertainty. IEEE Trans. Signal Process. 61(2), 223–237 (2013).
https://doi.org/10.1109/TSP .2012.2223695
9. Jing, W., Newman, W.: Improving robotic assembly performance through autonomous explo-
ration. IEEE International Conference on Robotics and Automation, p. 3303 (2002)
10. Kim, C.Y ., Song, D., Xu, Y ., Yi, J., Wu, X.: Cooperative search of multiple unknown transient
radio sources using multiple paired mobile robots. IEEE Trans. Robot. 30(5), 1161–1173 (2014)
11. Kim, C.Y ., Song, D., Yi, J., Wu, X.: Decentralized searching of multiple unknown and transient
radio sources with paired robots. Engineering 1(1), 058–065 (2015)
12. Low, K.H., Chen, J., Dolan, J.M., Chien, S., Thompson, D.R.: Decentralized active robotic
exploration and mapping for probabilistic ﬁeld classiﬁcation in environmental sensing. In:Proceedings of the 11th International Conference on Autonomous Agents and MultiagentSystems-V olume 1, International Foundation for Autonomous Agents and Multiagent Systems,pp. 105–112 (2012)
13. Maeda, Y ., Kodera, N., Egawa, T.: Caging-based grasping by a robot hand with rigid and soft
parts. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 5150–5155(2012)
14. Mannadiar, R., Rekleitis, I.: Optimal coverage of a known arbitrary environment. In: IEEE
International Conference on Robotics and Automation, pp. 5525–5530 (2010)
726 B. Li and D. Song
15. Marchant, R., Ramos, F.: Bayesian optimisation for intelligent environmental monitoring. In:
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2242–2249(2012)
16. Miller, L.M., Silverman, Y ., MacIver, M.A., Murphey, T.D.: Ergodic exploration of distributed
information. IEEE Trans. Robot. 32(1), 36–52 (2016)
17. Paull, L., Saeedi, S., Seto, M., Li, H.: Sensor-driven online coverage planning for autonomous
underwater vehicles. IEEE/ASME Trans. Mechatron. 18(6), 1827–1838 (2013)
18. Pereira, G.A., Campos, M.F., Kumar, V .: Decentralized algorithms for multi-robot manipulation
via caging. Int. J. Robot. Res. 23(7–8), 783–795 (2004)
19. Pipattanasomporn, P ., Makapunyo, T., Sudsang, A.: Multiﬁnger caging using dispersion con-
straints. IEEE Trans. Robot. 32(4), 1033–1041 (2016)
20. Plonski, P .A., V ander Hook, J., Isler, V .: Environment and solar map construction for solar-
powered mobile systems. IEEE Trans. Robot. 32(1), 70–82 (2016)
21. Rasmussen, C.E.: Gaussian Processes for Machine Learning. The MIT Press (2006)22. Rodner, E., Freytag, A., Bodesheim, P ., Denzler, J.: Large-scale Gaussian process classiﬁcation
with ﬂexible adaptive histogram kernels. In: European Conference on Computer Vision, pp.85–98. Springer (2012)
23. Rodriguez, A., Mason, M.T., Ferry, S.: From caging to grasping. Int. J. Robot. Res. 31(7),
886–900 (2012)
24. Shnaps, I., Rimon, E.: Online coverage by a tethered autonomous mobile robot in planar
unknown environments. IEEE Trans. Robot. 30(4), 966–974 (2014)
25. Song, D., Kim, C.Y ., Yi, J.: On the time to search for an intermittent signal source under a
limited sensing range. IEEE Trans. Robot. 27(2), 313–323 (2011)
26. Song, D., Kim, C.Y ., Yi, J.: Simultaneous localization of multiple unknown and transient radio
sources using a mobile robot. IEEE Trans. Robot. 28(3), 668–680 (2012). https://doi.org/10.
1109/TRO.2012.2183069
27. V asudevan, S., Ramos, F., Nettleton, E., Durrant-Whyte, H.: Gaussian process modeling of
large-scale terrain. J. Field Robot. 26(10), 812–840 (2009). https://doi.org/10.1002/rob.20309
28. V ongmasa, P ., Sudsang, A.: Coverage diameters of polygons. In: IEEE/RSJ International Con-
ference on Intelligent Robots and Systems, pp. 4036–4041. IEEE (2006)
29. Wan, W., Fukui, R.: Efﬁcient planar caging test using space mapping. IEEE Trans. Autom. Sci.
Eng. (2016)
30. Xu, L., Stentz, A.: An efﬁcient algorithm for environmental coverage with multiple robots. In:
IEEE International Conference on Robotics and Automation (ICRA), pp. 4950–4955 (2011)
31. Yang, K., Keat Gan, S., Sukkarieh, S.: A gaussian process-based RRT planner for the exploration
of an unknown and cluttered environment with a UA V . Adv. Robot. 27(6), 431–443 (2013).
https://doi.org/10.1080/01691864.2013.756386
32. Sk, Y un, Rus, D.: Distributed coverage with mobile robots on a graph: locational optimization
and equal-mass partitioning. Robotica 32(02), 257–277 (2014)
33. Zarubin, D., Pokorny, F.T., Toussaint, M., Kragic, D.: Caging complex objects with geodesic
balls. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.2999–3006. IEEE (2013)
Sharing Heterogeneous Spatial
Knowledge: Map Fusion BetweenAsynchronous Monocular Vision
and Lidar or Other Prior Inputs
Yan Lu, Joseph Lee, Shu-Hao Yeh, Hsin-Min Cheng, Baifan Chen
and Dezhen Song
1 Introduction
Since GPS signals are often challenged in indoor or urban environments, many
researchers focus on developing simultaneous localization and mapping (SLAM)algorithms using onboard sensors for robots or mobile devices. Nowadays regular
cameras are the most available and inexpensive sensor which relies on visual SLAM
algorithms. RGB-D cameras become more available but are unreliable in strongsunlight. A lidar is often considered as the most reliable mapping sensor, but it is too
power hungry, bulky, and expensive for many mobile robots or devices. A mobile
device or a small robot often can only rely on a monocular camera due to power andsize constraints.
However, monocular visual SLAM algorithms often generate low-quality maps
due to scale and angular drift, as illustrated in Fig. 1. To address these problems,
we propose to pre-scan the environment with a high-precision 2D or 3D lidar. The
lidar generated map (see Fig. 1) will be shared among mobile devices or other robots
with only monocular cameras. If we can rectify the low-quality visual SLAM map to
Y. L u (B)
Honda Research Institute USA, Mountain View, CA, USAe-mail: sinoluyan@gmail.com
J. Lee
U.S. Army TARDEC, Warren, MI, USAe-mail: joseph.s.lee34.civ@mail.mil
S.-H. Yeh ·H.-M. Cheng ·D. Song
Texas A&M University, College Station, TX, USAe-mail: ericex1015@tamu.edu
H.-M. Cheng
e-mail: dora90474@gmail.com
D. Song
e-mail: dzsong@cse.tamu.edu
B. Chen
Central South University, Hunan, Chinae-mail: chenbaifan@csu.edu.cn
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_51727
728 Y . Lu et al.
Lidar
SLAMMonocular
SLAM
Rectified Map
Fusion
Fig. 1 The inputs of our map fusion include a low-quality 3D map produced by a monocular visual
SLAM, and a high-precision prior map generated by lidar SLAM other methods. The map fusion
rectiﬁes the 3D map by leveraging vertical planes commonly available in both maps and outputs a
more accurate 3D map
the lidar-level accuracy (see Fig. 1), mobile device users and robots will enjoy high
localization accuracy by just using low-cost cameras, which has a large advantage in
many applications. In fact, we can also take other prior maps as inputs to rectify visual
SLAM results instead of lidar inputs. The only requirement is that we can extract
vertical planes from these prior mapping inputs. For example, Google maps™ often
contains building exterior planes.
Since lidar point clouds or prior maps and image feature points cannot be directly
registered to each other, this requires us to fuse the spatial knowledge (i.e. landmarks
of maps) asynchronously captured from different perspectives and platforms by the
heterogeneous sensors, which presents a new research problem. We detect high-level
landmarks such as vertical planes that can be identiﬁed in both sensing modalities.
We formulate a constrained global bundle adjustment using coplanarity constraints
and solve the problem using a penalty-barrier approach. By error analysis we prove
that the coplanarity constraints help reduce the estimation uncertainties. We have
implemented the system and tested it with real data. The initial results show that our
algorithm signiﬁcantly reduces the absolute trajectory error of visual SLAM by as
much as 68.3%.
2 Related Work
The map fusion problem can be viewed as a post processing step in visual SLAM,
which is a fast developing area. More speciﬁcally, this paper builds upon recent works
including optimization techniques, different sensor and feature conﬁgurations, and
collaborative map merging for multiple robots.
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 729
An SLAM algorithm simultaneously estimates camera/robot poses and landmark
positions, which is a fundamental problem in robotics [ 33] and computer vision. A
full ﬂedged SLAM framework consists of subproblems such as tracking, mapping,and loop closing. For efﬁciency, recent systems [ 9,16,23,29,31] implement a
front-end and back-end system running in parallel, where the front-end performs
tracking in real-time while the back-end reﬁnes both the resulting trajectory and themap, and closes loops occasionally. To increase the speed of the back-end, different
optimization techniques are proposed. Recent optimization methods take advantage
of sparse matrix structures in the SLAM problem. In [ 19], an implementation of a
hypergraph-based sparse non-linear optimization framework called g2o is presented.
An optimization method known as iSAM2 [ 15] uses a Bayes tree to incrementally
update the sparse matrix for an on-line SLAM system. Building on the g2o frameworkto exploit sparse matrix computation, our proposed map fusion can be viewed as a
back-end system which runs asynchronously from a front-end tracking and mapping
process.
SLAM can be performed with different exteroceptive sensors or their combi-
nations including regular cameras, lidars, and RGB-D cameras. Regular camera-
based SLAM is referred as visual SLAM, where two main approaches exist: ﬁltering
and structure-from-motion (SFM) using an optimization approach. For ﬁltering, the
extended Kalman ﬁlters (EKF) or its variants have been used extensively [ 5,7,22],
whereas the dominating method for SFM is bundle adjustment (BA) [ 18,27]. The
BA method is an optimization-based method which minimizes the reprojection error
over multiple image frames. Strasdat et al. [ 30] have compared EKF against BA and
pointed out that unless in high uncertainty situations, BA has a better performance
and accuracy than EKF. Our proposed method uses a batch optimization to estimate
the fused map. However, monocular vision-based SLAM is the most difﬁcult prob-lem due to the infamous scale drifting issue. Stereo vision can relieve the issue but
is often limited by the baseline and power constraints of small devices.
Depth sensors such as RGB-D cameras or lidars [ 8,13,24] can help address the
scale drifting issue in monocular vision. While RGB-D cameras are widely used for
indoor environments, lidars are still the most favorable sensor due to longer sensing
range, wide ﬁeld of view, and robustness to lighting conditions, making it suitablefor both indoor and outdoor applications [ 12]. Kohlbrecher et al. [ 17] register 2D
lidar scans to a 2D occupancy grid map based on an image registration technique.
Zhang and Singh [ 34] use a 2-axis lidar and develop a real-time 3D mapping method
with low-drift by separating the odometry and mapping task. In these methods, the
lidar map is accurate enough so that post-processing is not required.
Combining both vision and lidar inputs to utilize the beneﬁt of each sensor has
been proposed as well. Newman et al. [ 25] employ a 3D lidar to map buildings
and use vision to detect loop closure. Zhang and Singh [ 35] use vision to handle
rapid motion while the lidar warrants low-drift and robustness to lighting changes.
However, these methods require that the vision and lidar data are synchronized and
captured by the same robot. The resulting map is also limited to the hosting robotfor the localization usage. Our method is intended to fuse the vision and lidar maps
acquired by different robots and generate maps that can help different cameras or
730 Y . Lu et al.
mobile device users. Caselitz et al. [ 4] study localizing a monocular camera within
a lidar map, but do not investigate the map fusion problem.
Map merging has been studied for multi-robot systems in indoor environments.
Dedeoglu and Sukhatme [ 6] combine topological maps using landmarks detected by
sonar, vision, and laser. Fox et al. [ 11] merge lidar maps with robots actively detecting
each other to estimate their relative positions. Carpin [ 3] proposes a map merging
technique based on Hough transforms to merge occupancy grid maps. Baudouin
et al. [ 1] propose a method that merges robot paths with different scales generated by
multi-modal vision sensors such as perspective, ﬁsh-eye, or omnidirectional cameras.These prior works shed light on our problem but they focus on map building with
the same type of sensors.
In a lidar map or other prior map, the most visible features are wall planes.
Many studies make use of the fact that indoor/urban environments have planar struc-
tures [ 2,21,26]. Lu and Song introduce a multilayer feature graph (MFG) [ 20]f o ra
visual SLAM approach using heterogeneous landmarks including planes. Taguchi etal. [32] use an RGB-D camera and propose a method that uses combinations of prim-
itives, i.e. points and planes, for faster pose estimation. More recently, Salas-Moreno
et al. [ 28] have proposed a dense planar SLAM using an RGB-D camera.
3 Problem Deﬁnition
A robot equipped with a monocular camera navigates in an area for which a lidar
map or other prior map is given. From the prior/lidar map ML, a set of line segments
are extracted to represent major vertical planes from a top down perspective. Weassume (1) the camera is calibrated, and (2) the vertical direction is known which
can be obtained through either analyzing vanishing points from images or sensing
gravity direction from inertial sensors.
Let the camera pose at time k∈Nbe T
k∈SE(3). Deﬁne Tk={T1,..., Tk}as
the collection of camera poses up to time k.L e t MVdenote the 3D map built by
visual SLAM, which consists of a set of 3D points pi=[xi,yi,zi]T∈R3.
The problem is: Given MVand Tkup to time k, rectify MVand Tkwith ML.
4 Map Fusion and Uncertainty Analysis
The map built by visual SLAM is inevitably subject to drift. To rectify it, we period-
ically detect vertical planes from MVand associate them with those in ML. We then
incorporate the plane information from MLas additional constraints into the bundle
adjustment of visual SLAM to rectify MVand Tk. Next we ﬁrst present our map
fusion algorithm including plane detection and bundle adjustment, and then performuncertainty analysis to show the beneﬁt of including additional plane information in
visual SLAM.
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 731
4.1 Map Fusion
4.1.1 Plane Detection and Matching
In visual SLAM we periodically detect vertical planes from 3D points using
sequential RANSAC [ 10]. To keep the problem manageable, we only consider a
ﬁxed number of recent points that are visible in at least three adjacent keyframes. At
each iteration of RANSAC, we randomly pick three points from the pool and com-
pute a candidate plane. As we know the vertical direction, we can ﬁlter out candidate
planes in the ﬁrst place. Then we check how many points support this candidate
plane by computing a consensus score for each point in the pool. If enough support
points are found, we add the plane to MVand remove the support points from the
pool. The RANSAC process keeps going until the maximum number of iterations is
reached.
In this process, the consensus score is key to properly evaluate whether a point
supports a candidate plane. An intuitive way is to compute the perpendicular point-
to-plane distance in 3D as the consensus score. However, the map is only up-to-scale
in monocular SLAM, which poses difﬁculty to choosing a proper threshold for the
scores. Here we propose a novel consensus score in image space. Given a 3D point
pand a 3D plane π, suppose pis observed by camera at times k∈K. As illustrated
in Fig. 2, we ﬁrst ﬁnd the point p/primelying on πthat is closest to p, then deﬁne
dIMG(p,π)=max
k∈K/bardblφ(p,Tk)−φ(p/prime,Tk)/bardbl, (1)
where φ(·,·)is the camera projection function. Deﬁned in image space, this metric
is independent of the absolute map scale and eases the task of setting thresholds.
Fig. 2 Consensus score for
evaluating whether a 3D
point supports a plane.
Deﬁned in image space, it
avoids the scale ambiguity in
monocular visual SLAM
732 Y . Lu et al.
Once a vertical plane is detected from MV, we need to match it against ML.
This problem can be solved by topology graph matching, or using manual inputs if
the number of planes is small. We ignore planes in MVthat do not correspond to a
physical plane, which can be achieved by detecting appearance consistency.
4.1.2 Optimization
Since prior/lidar map MLis much more accurate than MV, we assume MLto be
error-free. Our goal is to register MVagainst MLso that the mapping error in MVis
minimized. This is achieved by leveraging plane correspondences. Suppose by time
kwe have found correspondences for planes πj,j∈Jk. We formulate the following
optimization problem to rectify MVand Tk.
min
M V,Tkk/summationdisplay
κ=1/summationdisplay
i∈I(κ)/bardblφ(pi,Tκ)−mi,κ/bardbl2
Σi,κ(2)
s.t.d⊥(p,π j)=0,∀p∈πj,j∈Jk
where I(κ)collects all indexes of points visible by camera at time κ,mi,κis the
image observation for piat time κ,Σi,κis the covariance of the measurement noise of
mi,κ,/bardbl·/bardblΣdenotes Mahalanobis distance, and d⊥(·,·)represents the perpendicular
Euclidean distance between a point and a plane in 3D. Here we abuse the notationp∈π
jto indicate the relation that pis supposed to reside on plane πj. The exact
position of πjin (2) is retrieved from ML.
Equation ( 2) is a constrained optimization problem. To solve it, we convert it to
an unconstrained optimization problem by adding a penalty function as follows
min
M V,Tkk/summationdisplay
κ=1/summationdisplay
i∈I(κ)/bardblφ(pi,Tκ)−mi,κ/bardbl2
Σi,κ+w/summationdisplay
j∈Jk/summationdisplay
p∈πjd⊥(p,π j)2(3)
The ﬁrst term is the same re-projection errors as in ( 2), and the second term is a
penalty term. We ﬁrst solve ( 3) with a relatively small weight w, and use the solution
as initial point to solve ( 3) again with an increased w. We repeat this process until
the change in the solution is negligible. It is worth noting that we use d⊥(·,·)instead
ofdIMG(·,·)in (2) and ( 3) because the scale ambiguity does not cause problems in
optimization.
4.2 Uncertainty Analysis
In this section we show the effect of incorporating known plane information in visual
SLAM by analyzing the estimation uncertainties. To simplify the analysis, we assume
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 733
the ﬁrst two camera poses are given, which essentially ﬁxes the absolute scale for
visual SLAM. Upon time k, we want to estimate all k−2 camera poses and all n
map points. Let y:=/bracketleftbig
TT
3,..., TT
k/bracketrightbigTbe the vector comprising camera poses, where
Tkis minimally parameterized. Without loss of generality, we assume that the last m
points are from a plane π. Furthermore, we choose a world coordinate system such
that its X-Y plane is parallel to π. This leads to the following coplanarity constraint.
Deﬁnition 1 (Coplanarity ) Points {pi|i=n−m+1,..., n}reside on a known
3D plane π, which is parallel to the X-Y plane. Thus the Z-coordinates of all points
onπare a constant, denoted by zπ.
We next analyze the estimation uncertainties of the camera poses and points
without and with using the coplanarity constraint, respectively.
4.2.1 No Coplanarity
In this case, we do not consider the coplanarity in Deﬁnition 1by treating each point
as independent. Let z1:=/bracketleftbig
pT
1,..., pT
n−m/bracketrightbigTcomprise points not residing on π, and
z2:=/bracketleftbig
pT
n−m+1,..., pT
n/bracketrightbigTcomprise points on π. We reorder the elements of z2such
that
z2=[xn−m+1,yn−m+1,..., xn,yn/bracehtipupleft /bracehtipdownright/bracehtipdownleft /bracehtipupright
:=zT
XY,zn−m+1,..., zn/bracehtipupleft /bracehtipdownright/bracehtipdownleft /bracehtipupright
:=zT
Z]T=/bracketleftbig
zT
XY,zT
Z/bracketrightbigT.
Letx=/bracketleftbig
yT,zT
1,zT
2/bracketrightbigT=/bracketleftbig
yT,zT
1,zT
XY/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
:=xT
1,zT
Z/bracketrightbigT=/bracketleftbig
xT
1,zT
Z/bracketrightbigT. Deﬁne a measurement
error function
Φ(x)=⎡
⎢⎢⎣...
φ(pi,Tκ)−mi,κ
...⎤
⎥⎥⎦(4)
∀κ=1,···,k,∀i∈I(κ).
Recall that mi,κis the image observation of piat time κ, and I(κ)collects the point
indexes visible at time κ.
We estimate xby
min
xΦ(x)TΣ−1Φ(x) (5)
where Σ=diag/parenleftbig
...,Σ i,κ,.../parenrightbig
, with Σi,κbeing the covariance of mi,κ.
734 Y . Lu et al.
Lemma 1 Letx∗=/bracketleftbig
x∗T
1,z∗T
Z/bracketrightbigTbe the solution of the problem in ( 5). Under
Gaussian noise assumption, the ﬁrst order approximation of the covariance of x∗
1
is
Cov(x∗
1)=/parenleftBig
JT
x1Σ−1Jx1−JT
x1Σ−1JzZ/parenleftbig
JT
zZΣ−1JzZ/parenrightbig−1JT
zZΣ−1Jx1/parenrightBig−1
(6)
where Jx1=∂Φ
∂x1/vextendsingle/vextendsingle/vextendsingle/vextendsingle
x1=x∗
1,JzZ=∂Φ
∂zZ/vextendsingle/vextendsingle/vextendsingle/vextendsingle
zZ=z∗
Z.
4.2.2 Apply Coplanarity
In this case, we explicitly model coplanarity in Deﬁnition 1. As a result, ∀i∈[n−
m+1,..., n],w eh a v e pi=[xi,yi,zπ]T, and thus φ(pi,Tκ)=φ/parenleftbig
[xi,yi,zπ]T,
Tκ).Recall x1=/bracketleftbig
yT,zT
1,zT
XY/bracketrightbigT.In this case, x1consists of all the parameters we
need to estimate. Deﬁne a measurement error function
Φ/prime(x1)=⎡
⎢⎢⎣...
φ(pi,Tκ)−mi,κ
...⎤
⎥⎥⎦(7)
∀κ=1,..., k,∀i∈I(κ).
We estimate x1by
min
x1Φ/prime(x1)TΣ−1Φ/prime(x1). (8)
Lemma 2 Letx/prime∗
1be the solution of the problem in ( 8). Under Gaussian noise
assumption, the ﬁrst order approximation of the covariance of x/prime∗
1is
Cov(x/prime∗
1)=/parenleftBig
J/primeT
x1Σ−1J/prime
x1/parenrightBig−1
(9)
where J/primex1=∂Φ
∂x1/vextendsingle/vextendsingle/vextendsingle/vextendsingle
x1=x/prime∗
1.
4.2.3 Uncertainty Reduction
Now we have estimated x1under two scenarios and its respective covariances
Cov(x∗
1)andCov(x/prime∗
1). The following theorem reveals how the coplanarity con-
straint affects the estimation uncertainty.
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 735
Theorem 1 The coplanarity in Deﬁnition 1reduces the estimation uncertainty of
the parameters in x1. Speciﬁcally,
λj/parenleftbig
Cov/parenleftbig
x/prime∗
1/parenrightbig/parenrightbig
<λ j/parenleftbig
Cov/parenleftbig
x∗
1/parenrightbig/parenrightbig
,0≤j≤len(x1) (10)
where λj(·)denotes the jth largest eigenvalue, len (·)is the length of a vector, and
x/prime∗
1andx∗
1are the optimal estimation of x1with and without knowing the coplanarity,
respectively.
Proof Let us write M1/followsM2if matrices M1andM2are real symmetric and M1−M2
is positive deﬁnite.
Asx∗andx/prime∗
1are the global optimal solution to ( 5) and ( 8), respectively, it is easy
to see x∗
1=x/prime∗
1, and z∗
Z=[zπ,..., zπ]T. Consequently, Jx1=J/primex1. From Lemmas 1
and 2, we derive that
Cov/parenleftbig
x/prime∗
1/parenrightbig−1−Cov/parenleftbig
x∗
1/parenrightbig−1=JT
x1Σ−1JzZ/parenleftbig
JT
zZΣ−1JzZ/parenrightbig−1JT
zZΣ−1Jx1.
Since we use minimal parameterization for x1andzZ, it holds that
JT
x1Σ−1JzZ/parenleftbig
JT
zZΣ−1JzZ/parenrightbig−1JT
zZΣ−1Jx1/follows0,
which leads to Cov/parenleftbig
x/prime∗
1/parenrightbig−1/followsCov/parenleftbig
x∗
1/parenrightbig−1.According to Theorem 7.7.3 in [ 14], it
holds
Cov/parenleftbig
x/prime∗
1/parenrightbig−1/followsCov/parenleftbig
x∗
1/parenrightbig−1⇔Cov/parenleftbig
x∗
1/parenrightbig
/followsCov/parenleftbig
x/prime∗
1/parenrightbig
,
which further leads to ( 10) per Corollary 7.7.4 in [ 14]. /square
It is worth noting that Theorem 1can be easily generalized when the plane is not
parallel to X-Y plane, but an arbitrary plane. Furthermore, when more planes areknown, it is intuitive to see that the estimation uncertainties can be further reduced.
5 Experiments
In this section, we ﬁrst verify Theorem 1by simulated experiments, and then evaluate
our proposed method using real world data.
5.1 Simulation
In this simulation the robot navigates through an L-shaped corridor as illustrated in
Fig. 3a. The corridor consists of 4 walls: Plane A, B, C, and D, and each wall has
around 50 randomly distributed points. The robot starts at the origin, ﬁrst moves
736 Y . Lu et al.
Plane C
Plane D(a) (b)
(c) (d)
Fig. 3 a The simulation setup. An L-shaped corridor comprises four walls, Plane A, B, C and D,
each consisting of around 50 points. The camera starts from the origin, moves forward ﬁrst and
turns right at the corner. b–dThe uncertainties of camera positions and points in the case of b
no coplanarity being considered, cthe coplanarity constraint for Plane B being applied, and dthe
coplanarity constraint for Plane C being applied
forward and then turns right at the corner. The image resolution is 640 ×480 and
the camera focal length is 500. As assumed in Sect. 4.2the ﬁrst two camera poses
are given.
We ﬁrst compute the uncertainties using ( 6) which takes no coplanarity informa-
tion into account. Figure 3b shows the uncertainties of all camera positions and 3D
points. As expected the uncertainties gradually increase as the robot moves forward.
Now consider the constraint that a set of points reside on Plane B whose position is
known. The uncertainties of map points and camera poses are computed using ( 9) and
illustrated in Fig. 3c. Compared with Fig. 3b, it is obvious that all the uncertainties
are reduced.
To illustrate how the plane position affects the uncertainty reduction, we consider
the coplanarity of Plane C instead and show the resulted uncertainties in Fig. 3b.
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 737
Fig. 4 Determinants of
camera pose covariancesunder different scenarios.
Note how the coplanarity
reduces the uncertainties.Note that the vertical axisuses log scale
Compared with Fig. 3b, it is also obvious that the uncertainties are reduced. However,
by comparing Fig. 3c, d, we see the different effects. Figure 4further compares the
determinants of all camera pose covariances for the three scenarios described above.
5.2 Real World T est
We have implemented our system based on ORB-SLAM [ 23], a state-of-the-art
monocular visual SLAM system, though our method is applicable to any other
monocular visual SLAM. We evaluate our system on one indoor and two outdoordatasets, as described below.
•HRBB4 : This is a sequence of 12,000 images collected in an ofﬁce corridor envi-
ronment with the ground truth of camera trajectory provided [ 20]. To evaluate
our method, we collected lidar data using a Hokuyo (UTM-30LX) in the same
environment two years after the images were acquired. We generate a lidar map
using Hector-SLAM [ 17] and extract major line segments (i.e. vertical planes) as
illustrated in Fig. 5a.
•KITTI-07 : This is an outdoor image sequence from the KITTI dataset [ 12], con-
sisting of 1101 images recorded by an autonomous driving platform. The groundtruth of camera trajectory is provided by a high-grade GPS-INS system. We gen-
erate a 3D lidar map from the accompanied velodyne data using LOAM [ 34] and
treat it as a 2D map by projecting it to the ground plane as illustrated in Fig. 6a.
•KITTI-00 : This is another outdoor image sequence from the KITTI dataset [ 12],
consisting of 4541 images. We use Google satellite map of the corresponding area
as our prior map, as shown in Fig. 7a.
To evaluate the SLAM accuracy, we compute the statistics of absolute
trajectory errors (A TE), including RMS (root-mean-square) error, SD (standard devi-
738 Y . Lu et al.
(a) (b)
Fig. 5 a V ertical planes (red and green lines) overlaid on the HRBB4 lidar map (black). Red lines
indicate the planes that are matched and thus used for map fusion. bCamera trajectory ground truth
and estimates for the HRBB4 dataset
(a) (b)
Fig. 6 a V ertical planes (red and green lines) overlaid on the KITTI-07 lidar map (gray). Red lines
indicate the planes that are matched and thus used for map fusion. bCamera trajectory ground truth
and estimates for the KITTI-07 dataset
ation), Max(maximum), and the ratio of RMS error over the trajectory length for my
method and ORB-SLAM. Note that loop closing is disabled in ORB-SLAM for fair
comparison. In Table 1we see that our algorithm signiﬁcantly reduces the A TE by
leveraging the prior 2D maps. The estimated camera trajectories versus the ground
t r u t ha r es h o w ni nF i g s . 5b,6b, and 7b. Compared with ORB-SLAM, our method
reduces the RMS error by 60 .6% for the HRBB4 dataset, by 69 .8% for the KITTI-07
dataset, and by 74 .4% for the KITTI-00 dataset. On average our method reduces the
RMS error by 68 .3%.
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 739
(a)(b)
Fig. 7 a V ertical planes (red and green lines) overlaid on the Google satellite map for the KITTI-
00 dataset. Red lines indicate the planes that are matched and thus used for map fusion. bCamera
trajectory ground truth and estimates for the KITTI-00 dataset
Table 1 Absolute Trajectory Errors (m)
Dataset ORB-SLAM Our method
RMS(m) SD(m) Max(m) RMS/Traj (%) RMS(m) SD(m) Max(m) RMS/Traj (%)
HRBB4 1.32 0.68 3.2 1.89 0.52 0.27 1.37 0.74
KITTI-07 15.06 8.61 36.99 2.17 4.55 2.82 15.04 0.65
KITTI-00 62.08 30.04 144.68 1.67 15.92 8.04 34.04 0.43
6 Conclusions
We presented a method that fuses two maps generated from different sensory modal-
ities, i.e. monocular vision and prior/lidar data, to assist low-cost devices/robots to
obtain high quality localization information. The prior/lidar data can be either maps
constructed from lidar inputs or other prior map inputs as along as we can extract ver-
tical planes from them. We exploited the planar structure extracted from both vision
and prior/lidar data and use it as the anchoring information to fuse the heterogeneous
maps. We formulated a constrained nonlinear optimization problem under global
bundle adjustment framework using coplanarity constraints. We solved the problem
using a penalty-barrier approach. By error analysis we proved that the coplanarity
constraints help reduce the estimation uncertainties. We implemented the system and
tested it with real data. The results showed that our algorithm signiﬁcantly reduced
the absolute trajectory error of visual SLAM by as much as 68.3%.
For future work, we will test our method on different indoor environments. One
shortcoming of the proposed method is the reliance on vertical plane distribution.
Although most indoor environments are dominated by vertical planes, this method
740 Y . Lu et al.
would fail in an environment with very few vertical planes. We are interested in
extracting other signatures such as object boundary points to handle the problem.
We will also explore distributed implementations for the optimization framework.Finally, we are interested in developing cloud-based schemes to further allow algo-
rithms and positional information to be shared among low-cost devices.
Acknowledgements This work was supported in part by National Science Foundation under NRI-
1426752, NRI-1526200 and NRI-1748161, and in part by National Science Foundation of Chinaunder 61403423.
References
1. Baudouin, L., Mezouar, Y ., Ait-Aider, O., Araújo, H.: Multi-modal sensors path merging. In:
Intelligent Autonomous Systems, vol. 13, pp. 191–201. Springer (2016)
2. Bódis-Szomorú, A., Riemenschneider, H., V an Gool, L.: Fast, approximate piecewise-planar
modeling based on sparse structure-from-motion and superpixels. In: Proceedings of the IEEEConference on Computer Vision and Pattern Recognition, pp. 469–476 (2014)
3. Carpin, S.: Merging maps via hough transform. In: IEEE/RSJ International Conference on
Intelligent Robots and Systems, 2008. IROS 2008, pp. 1878–1883. IEEE (2008)
4. Caselitz, T., Steder, B., Ruhnke, M., Burgard, W.: Monocular camera localization in 3d lidar
maps. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and
Systems (IROS) (2016)
5. Civera, J., Grasa, O.G., Davison, A.J., Montiel, J.M.M.: 1-point RANSAC for extended Kalman
ﬁltering: Application to real-time structure from motion and visual odometry. J. Field Robot.27(5), 609–631 (2010)
6. Dedeoglu, G., Sukhatme, G.S.: Landmark-based matching algorithm for cooperative mapping
by autonomous robots. In: Distributed Autonomous Robotic Systems vol. 4, pp. 251–260.Springer (2000)
7. Durrant-Whyte, H., Bailey, T.: Simultaneous localization and mapping: part I. Robot. Autom.
Mag. IEEE 13(2), 99–110 (2006)
8. Endres, F., Hess, J., Sturm, J., Cremers, D., Burgard, W.: 3-D mapping with an RGB-D camera.
IEEE Trans. Robot. 30(1), 177–187 (2014)
9. Engel, J., Schöps, T., Cremers, D.: Lsd-slam: Large-scale direct monocular slam. In: European
Conference on Computer Vision, pp. 834–849. Springer (2014)
10. Fischler, M.A., Bolles, R.C.: Random sample consensus: a paradigm for model ﬁtting with
applications to image analysis and automated cartography. Commun. ACM 24(6), 381–395
(1981)
11. Fox, D., Ko, J., Konolige, K., Limketkai, B., Schulz, D., Stewart, B.: Distributed multirobot
exploration and mapping. Proc. IEEE 94(7), 1325–1339 (2006)
12. Geiger, A., Lenz, P ., Urtasun, R.: Are we ready for autonomous driving? the KITTI vision
benchmark suite. In: IEEE Conference on Computer Vision and Pattern Recognition (CVPR),pp. 3354–3361 (2012)
13. Henry, P ., Krainin, M., Herbst, E., Ren, X., Fox, D.: RGB-D mapping: using kinect-style depth
cameras for dense 3D modeling of indoor environments. Int. J. Robot. Res. 31(5), 647–663
(2012)
14. Horn, R.A., Johnson, C.R.: Matrix Analysis. Cambridge University Press, Cambridge (2012)15. Kaess, M., Johannsson, H., Roberts, R., Ila, V ., Leonard, J.J., Dellaert, F.: isam2: incremental
smoothing and mapping using the bayes tree. Int. J. Robot. Res. 31(2), 216–235 (2012)
16. Klein, G., Murray, D.: Parallel tracking and mapping for small AR workspaces. In: IEEE and
ACM International Symposium on Mixed and Augmented Reality (ISMAR), pp. 225–234(2007)
Sharing Heterogeneous Spatial Knowledge: Map Fusion Between Asynchronous … 741
17. Kohlbrecher, S., V on Stryk, O., Meyer, J., Klingauf, U.: A ﬂexible and scalable slam system
with full 3d motion estimation. In: 2011 IEEE International Symposium on Safety, Security,and Rescue Robotics (SSRR), pp. 155–160. IEEE (2011)
18. Konolige, K., Agrawal, M.: Frameslam: from bundle adjustment to real-time visual mapping.
IEEE Trans. Robot. 24(5), 1066–1077 (2008)
19. Kummerle, R., Grisetti, G., Strasdat, H., Konolige, K., Burgard, W.: g 2 o: a general frame-
work for graph optimization. In: IEEE International Conference on Robotics and Automation(ICRA), pp. 3607–3613 (2011)
20. Lu, Y ., Song, D.: Visual navigation using heterogeneous landmarks and unsupervised geometric
constraints. IEEE Trans. Robot. (T-RO) 31(3), 736–749 (2015)
21. Ma, L., Kerl, C., Stückler, J., Cremers, D.: CPA-SLAM: consistent plane-model alignment for
direct RGB-D SLAM. In: 2016 IEEE International Conference on Robotics and Automation(ICRA), pp. 1285–1291. IEEE (2016)
22. María Paz, L., Piniés, P ., Tardós, J.D., Neira, J.: Large-scale 6-DOF SLAM with stereo-in-hand.
IEEE Trans. Robot. 24(5), 946–957 (2008)
23. Mur-Artal, R., Montiel, J.M.M., Tardós, J.D.: ORB-SLAM: a versatile and accurate monocular
SLAM system. IEEE Trans. Robot. 31(5), 1147–1163 (2015)
24. Newcombe, R.A., Fox, D., Seitz, S.M.: Dynamicfusion: reconstruction and tracking of non-
rigid scenes in real-time. In: Proceedings of the IEEE Conference on Computer Vision andPattern Recognition, pp. 343–352 (2015)
25. Newman, P ., Cole, D., Ho, K.: Outdoor SLAM using visual appearance and laser ranging. In:
Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA2006, pp. 1180–1187. IEEE (2006)
26. Pollefeys, M., Lee, G.H., Fraundorfer, F.: MA V visual SLAM with plane constraint. In: 2011
IEEE International Conference on Robotics and Automation (ICRA), pp. 3139–3144 (2011)
27. Royer, E., Lhuillier, M., Dhome, M., Lavest, J.-M.: Monocular vision for mobile robot local-
ization and autonomous navigation. Int. J. Comput. Vis. 74(3), 237–260 (2007)
28. Salas-Moreno, R.F., Glocker, B., Kelly, P .H.J., Davison, A.J.: Dense planar SLAM. In: IEEE
and ACM International Symposium on Mixed and Augmented Reality (ISMAR), pp. 157–164(2014)
29. Salas-Moreno, R.F., Newcombe, R.A., Strasdat, H., Kelly, P .H.J., Davison, A.J.: Slam++: simul-
taneous localisation and mapping at the level of objects. In: Proceedings of the IEEE Conferenceon Computer Vision and Pattern Recognition, pp. 1352–1359 (2013)
30. Strasdat, H., Montiel, J.M.M., Davison, A.J.: Real-time monocular SLAM: why ﬁlter? In: IEEE
International Conference on Robotics and Automation (ICRA), pp. 2657–2664 (2010)
31. Strasdat, H., Montiel, J.M.M., Davison, A.J.: Scale drift-aware large scale monocular SLAM.
In: Robotics: Science and Systems (RSS), vol. 1, p. 4 (2010)
32. Taguchi, Y ., Jian, Y -D., Ramalingam, S., Feng, C.: Point-plane SLAM for hand-held 3D sensors.
In: IEEE Int. Conf. Robot. Autom. (ICRA), pp. 5182–5189 (2013)
33. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press (2005)34. Zhang, J., Singh, S.: Loam: lidar odometry and mapping in real-time. In: Robotics: Science
and Systems, vol. 2. Citeseer (2014)
35. Zhang, J., Singh, S.: Visual-lidar odometry and mapping: Low-drift, robust, and fast. In: 2015
IEEE International Conference on Robotics and Automation (ICRA), pp. 2174–2181. IEEE(2015)
Sparse Point Registration
Rangaprasad Arun Srivatsan, Prasad V agdargi and Howie Choset
1 Introduction
In several applications of engineering, medicine and especially robotics, one often
encounters the need to perform registration. In a typical registration problem, the spa-tial transformation between the geometric model of the object-of-interest and point
measurements of the object’s surface, needs to be estimated. In most applications,
the point clouds obtained from sensors such as LIDAR, Kinect, feature rich stereo-images, etc, contain hundreds of points. Several methods have been developed to
perform registration when dense point measurements are obtained [ 1–5]. However,
these methods do not perform well when only a small number of point measurementsare available, and hence in this work we develop a method for robust sparse point
registration (SPR).
Sparse point registration is of critical importance in surgical applications, where a
surgeon probes the visible anatomy using a robot in order to register the anatomy to
its preoperative model obtained from CT scan or MRI. In such applications, there isa cost associated with probing more points and the goal is to quickly and accurately
register with a fewer number of measurements. Prior work either uses greater than
100 measurement points for reliable registration [ 4–6], or uses a priori knowledge
of anatomical landmarks to hand-pick a small number of probing locations [ 7–9]. In
an attempt to keep the formulation general, in this work we do not assume any prior
knowledge of anatomical segments.
Our approach to SPR is developed as an iterative procedure and in each itera-
tion, the current best pose estimate is perturbed to obtain several poses. Each of the
R. A. Srivatsan ( B)·P. Vagdargi ·H. Choset
Robotics Institute at Carnegie Mellon University, 5000 Forbes Avenue, Pittsburgh, PA 15213,USAe-mail: rarunsrivatsan@cmu.edu
P. Vagdargi
e-mail: prasadv@cmu.edu
H. Choset
e-mail: choset@cscmu.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_52743
744 R. A. Srivatsan et al.
obtained pose will hereby be referred to as a ‘pose particle’. The amount of per-
turbation is reduced in each iteration to balance exploration and exploitation. By
evaluating a cost function, the best pose particle is selected and used as initial seedfor an optimization problem that computes a locally optimal pose. This process is
then repeated for a ﬁxed number of iterations or until convergence. The optimizer
used for computing the locally optimal pose can be deterministic or probabilistic anddepending on the requirement of the problem, two variants have been developed:
deterministic SPR (dSPR) and probabilistic SPR (pSPR). The dSPR uses iterative
closest point algorithm (ICP) [ 1], while the pSPR uses dual quaternion ﬁltering
(DQF) [ 10] to estimate the pose. Note that this approach may appear similar to the
stochastic ICP [ 11]. However, a key difference is that stochastic ICP perturbs each
point by a random noise, while our approach perturbs the pose which results in eachpoint being perturbed in a structured manner.
The dSPR is computationally faster than pSPR, but requires the perturbation
related parameters to be set manually. The pSPR on the other hand uses uncertaintyinformation to automatically set these parameters. An additional contribution of this
work is extending the online formulation of DQF in [ 10] to process batches of point
measurements.
In Sect. 5, the dSPR and pSPR are evaluated in simulation over a number of
standard data sets and compared against popular registration methods. The resultsshow that the SPR typically takes less than 20 points to accurately and robustly
estimate the registration and is more robust to initial registration errors ( ≈30
◦orien-
tation and ≈30 mm translation). In Sect. 5, a general guideline is provided on how
to automatically probe the object to get a good spread of sparse points.
2 Related Work
Registration is the process of ﬁnding the spatial transformation that aligns a point
cloud to a geometric model, deﬁned in different reference frames. When the cor-
respondences between the points in the two reference frames are known, Horn etal. developed a least squares based approach to ﬁnd the spatial transformation [ 12].
However, in most practical applications, the correspondence is unknown. By iter-
atively ﬁnding the best correspondence and the optimal transformation given thatcorrespondence, Besl and McKay developed the iterative closest point (ICP) [ 1].
Several variants of the ICP have since been developed that use surface normal
information [ 2,13], incorporate uncertainty in measurements [ 3], incorporate un-
certainty in ﬁnding correspondence [ 5], are robust to outliers [ 14,15], are globally
optimal [ 16–18], etc. The computation time for most of the ICP-based methods is
high and in order to address this, Kalman ﬁltering-based variants have been devel-
oped [ 4,19–22].
Sparse Point Registration 745
In applications where only a small number of sparse point measurements are
available due to high cost of measurement, conventional registration approaches
do not perform reliably. For example, in a surgical application of probing-basedregistration using any of the Kalman ﬁltering variants, the computation time might
be low [ 4,10,23], but the time taken to obtain greater than 100 point measurements
can be high.
Prior work such as [ 7,8,24,25], register using a small number of carefully chosen
probing points. These points are selected based on optimization of a stiffness-based
quality metric [ 24]. Since, these works were inspired by surgical applications, they
assume a priori knowledge of the location of anatomical features in the robot-frame.
Such an assumption is very limiting in its nature and reduces the scope of approach.
Firstly, the workspace constraints of the robot might restrict the robot from locating allthe anatomical segments. Secondly, when applied to a non-surgical domain, deﬁning
an equivalent of anatomical feature is non-trivial. There is also a vast literature on
touch-based localization for robot manipulation applications. These approaches areeither meant for planar localization [ 26–28], or time consuming [ 29], or assume good
initial alignment [ 30] or assume multiple simultaneous contacts [ 31,32].
T h ew o r ko fM ae ta l .[ 9] comes closest to our approach. In [ 9], an unscented
particle ﬁlter (UPF) is used to register an object using a small number of point
measurements without relying on prior knowledge of any landmarks or segments.While the UPF uses a small number of measurements, it uses a large number of pose
particles ≈2000 in each iteration, resulting in a large computation time.
3 Mathematical Background
3.1 Iterative Closest Point
The ICP algorithm introduced by Besl and McKay [ 1] has two important steps that
are repeated until convergence
1. Finding correspondences between a point set and a geometric representation of
a surface.
2. Computing the transformation which minimizes the distance between correspond-
ing points.
Consider two point clouds, A={ ai},ai∈R3,i=1,..., nare npoints on the
geometric model of the object and B={ bj},bj∈R3,j=1,..., mare mpoints
obtained using sensor measurements. Let T∈SE(3)be the transformation that
aligns Aand B. The ICP algorithm is listed in Algorithm 1, typically uses Horn’s
method [ 12] to perform the minimization; although there are other optimization
variants as well [ 13].
746 R. A. Srivatsan et al.
Algorithm 1: Iterative Closest Point Update
Input :
A={ ai∈R3},i=1,2,..., n
B={ bj∈R3},j=1,2,..., m
Initial transformation: T0∈SE(3)
Output :T∈SE(3)that aligns Aand B
1Initialize :T← T0
2while not converged do
3 Correspondence :cj=FindClosest Point (T(bj)),cj∈A
4 Minimization :T=argmin
Tm/summationtext
j=1/vextenddouble/vextenddoublecj−T(bj)/vextenddouble/vextenddouble2
3.2 Dual Quaternion Filtering
Dual quaternion ﬁltering (DQF) is a linear Kalman ﬁltering based approach for
online pose estimation [ 10]. Unlike ICP, DQF is not a batch processing algorithm.
It is more similar to a Kalman ﬁlter since it uses measurement information as it
becomes available, but with one small difference—the DQF updates the registration
once for every pair of measurements obtained. Compared to other ﬁltering basedregistration methods such as [ 4,19,20], the DQF is preferred because it is a truly
linear ﬁlter without any approximations or linearizations, resulting in quick and
accurate estimates. The transformation T∈SE(3)is parameterized using the unit
dual quaternion x=(q,d)
T, where q∈R4is a unit quaternion that parameterizes
the rotation, d=/parenleftbig
(0,t)T⊙q/parenrightbig
/2, where ⊙is the quaternion multiplication operator,
and t∈R3is the translation vector.
Srivatsan et al. [ 10] use a pair of measurements {(cj,bj),(cj+1,bj+1)}, to derive
Hq=0, H∈R4×4(1)
H=/bracketleftbigg0 −(cj−cj+1−bj+bj+1)T
(cj−cj+1−bj+bj+1)( cj−cj+1+bj−bj+1)×/bracketrightbigg
, (2)
t=cj−cj+1
2−Real/parenleftbigg
q⊙bj−bj+1
2⊙q∗/parenrightbigg
, (3)
where j=1,3,..., 2α−1,α=⌊m
2⌋,q∗is the conjugate quaternion and []×is the
operator that converts a vector to a skew-symmetric matrix.
The update equations for the Kalman ﬁlter are:
qk=qk−1−KkHkqk−1,
Σq
k=(I−KkHk)Σq
k−1,where,Kk=Σq
k−1HT
k(HkΣq
k−1HT
k+Qk)−1
Sparse Point Registration 747
whereΣq
kis the uncertainty in the quaternion qkand Qkis the pseudo-measurement
uncertainty. The translation vector tkis obtained from qkusing Eq. 3. For the sake
of brevity, the derivation for Qk=g(Σq
k,Σbj
k,Σcj
k)andΣt
k=f(Σq
k,Σbj
k,Σcj
k)
representing the uncertainty in the translation, are omitted here. The expressions
along with their derivation can be obtained from Sect. IV .C of [ 10].
The standard implementation of DQF requires ≈100 measurements for reliable
registration estimation [ 10]. In this work, the DQF is modiﬁed to a batch processing
variant, which updates using all the mmeasurements collected, instead of a single
pair per iteration. We shall henceforth refer to this variant as batch-DQF (bDQF).
As shown in Sect. 5, bDQF requires fewer measurements for accurate estimation
compared to the standard DQF. We modify Eq. 1as
Gq=0,G∈Rα×4,where G=[ H1,..., Hα].
Hi∈R4×4is as deﬁned in Eq. 2. The update equations of the ﬁlter remain the same.
The algorithm for bDQF is shown in Algorithm 2.
Algorithm 2: Batch Dual quaternion ﬁltering
Input :
A={ ai∈R3},i=1,2,..., n
B={ bj∈R3},j=1,2,..., m
Initial transformation: q0∈R4,t0∈R3,
Output :
q∈R4,t∈R3that aligns Aand B
Σq∈R4×4,Σt∈R3×3
1Initialize :k=1
2while not converged do
3 Correspondence :
4 Tk−1(bj)=tk−1+Real(qk−1⊙bj⊙q∗
k−1)
5 cj=FindClosest Point (Tk−1(bj)),cj∈A,
6 State Update :
7 qk=(I−KkGk)qk−1
8Σq
k=(I−KkGk)Σq
k−1
9 tk=1
m/parenleftBig/summationtextm
j=1cj−Real/parenleftBig
qk⊙/summationtextm
j=1bj⊙q∗
k/parenrightBig/parenrightBig
10 Σt
k=f(Σq
k,Σbj
k,Σcj
k)
11 k=k+1
4 Modeling
Algorithm 3 and Fig. 1show the basic framework and the steps involved in our SPR
approach.
748 R. A. Srivatsan et al.
Algorithm 3: Sparse Point Registration
Input :
A={ ai∈R3},i=1,2,..., n
B={ bj∈R3},j=1,2,..., m
Initial transformation: T0∈SE(3)
Output :T∈SE(3)that aligns Aand B
1Initialize :T← T0,k=0,ε=inf
2while k Threshold do
3 Perturbation :˜Tj=Tk+N(0,Σk),j=1,..., p
4 Evaluate Cost Function :Oj=/summationtextm
i=1||˜Tj(bi)−ci||
5 Locally Optimal estimate :
6 ˆTk=argmin ˜TjOj
7 Tk=ICP(A,B,ˆTk)orDQF(A,B,ˆTk)
8εk=/summationtextm
i=1||Tk(bi)−ci||
9 ifεk<ε then
10 T=Tk,ε=εk
11 k=k+1
1. The algorithm 3 is initialized using an initial pose (see Fig. 1b).
2. The current best pose is perturbed and pperturbed poses are obtained. In Fig. 1,
p=3 is chosen. The amount of perturbation is reduced over the iterations.
Refer Sects. 4.1and 4.2for more information on how to choose the amount
of perturbation.
3. A cost function is evaluated for each of the perturbed poses. The cost function is
the sum of the closest distance between the point measurements and the geometric
model: Oj=/summationtextm
i=1||˜Tj(bi)−ci||,j=1,..., p, where ˜Tj∈SE(3)and ci∈A
is the closest point in the A. In this step, we use an approximate geometric model
A∗instead of A, to quickly evaluate the cost function. Depending on the format of
the geometric model, several existing simpliﬁcation techniques can be applied,
such as [ 25,33,34]. For example, in this work we work with a triangulated
mesh model and use a quadric mesh simpliﬁcation [ 35] as shown in Fig. 1c. The
more approximate the geometry, the lesser the time taken to compute Ojbut
greater number of iterations for the overall algorithm. In our experiment, we do
not observe the level of approximation affecting the quality of the results.
4. The pose ˆT=argmin ˜TjOj, is chosen as the initial guess for a locally optimal
pose estimation using ICP or bDQF. In Sect. 5we discuss the advantages and
limitations of using ICP over bDQF.
5. Steps 2–4 are repeated until convergence or up to a ﬁxed number of iterations.
Sparse Point Registration 749
Fig. 1 The steps involved in two iterations of the sparse point registration algorithm. The best
pose estimate in each iteration is perturbed to obtain three pose particles. aPoint measurements.
bGeometric model of the object. cPerturbed poses of a lower resolution geometric model. The
number of triangle vertices in the original model is 259,896, in comparison with 88 in the approx-imate model. dThe best pose from the perturbed poses is selected and a locally optimal pose is
obtained by using ICP or bDQF and the original geometric model. eThe current best pose is per-
turbed to obtain three new poses. The perturbation in this step is lower than the previous iteration.fThe locally optimal pose obtained and the steps are repeated until convergence
750 R. A. Srivatsan et al.
4.1 Deterministic Sparse Point Registration (dSPR)
In the dSPR, ICP as described in Sect. 3.1is used to ﬁnd the locally optimal pose
(Step 4 of SPR). There are three tunable parameters:
1. Number of perturbations: Perturbations help the optimizer move out of a local
minima. The higher the number of perturbations, the faster the convergence to
the optimal estimate is. But higher perturbations also imply higher computationtimes. In this work we choose the number of perturbations p=10.
2. Amount of perturbation: The amount of perturbation helps balance exploration
and exploitation. Higher perturbation encourages exploration while lower per-turbation encourages exploitation. We start with a high perturbation amount and
decrease the perturbation over iterations. In this work we set the initial perturba-
tion in orientation to be drawn from a normal distribution with zero mean and astandard deviation of 10
◦. The initial perturbation in translation is drawn from a
normal distribution with zero mean and a standard deviation of 10% of the size of
the object. The perturbation is decreased linearly until it is reduced to zero aftera maximum of 30 iterations.
3. Termination criteria: The procedure can be terminated when the number of iter-
ations reaches a set limit or when the RMS error between Aand Bis lower than
a set threshold. In this work, we set the maximum number of iterations to be 30
and the RMS error threshold to be 0 .5% of the size of the object. In addition the
maximum number of iterations of each ICP step is set to 20.
Using a large number of perturbations and iterations, while decreasing the RMS
error threshold improves the accuracy of the ﬁnal results; at the cost of increasedcomputation time. The choices of parameters made in this work for dSPR are based
on manual tuning over several standard data sets; and are not meant to exhibit any
optimal behavior.
4.2 Probabilistic Sparse Point Registration (pSPR)
In the pSPR, bDQF as described in Sect. 3.2is used to ﬁnd the locally optimal pose.
There are two tunable parameters:
1. Number of perturbations: Since we use a Gaussian distribution-based ﬁlter, the
number of perturbations are be chosen to be equal to the number of sigma-points.In the case of bDQF, there are 15 sigma points. Unlike the dSPR, the amount of
perturbation need not be set manually, but can be chosen from a normal distribu-
tion with zero mean and standard deviation matching the standard deviation ofthe current state estimate.
2. Termination criteria: In this work, we set the maximum number of iterations to be
12 and the RMS error threshold to be 0 .5% of the size of the object. The number
of iterations in each bDQF step is set to 50.
Sparse Point Registration 751
Fig. 2 Four different poses of the liver contain the same set of four green points
5 Simulation Experiments
We perform a number of simulation experiments on standard shape data sets to
systematically study the dSPR and pSPR. The results are compared with ICP, bDQFand UPF [ 9]. For all the simulation experiments, the objects are scaled to ﬁt in a cube
of edge length 100 mm, for a fair comparison of the registration errors.
1
5.1 Minimum Number of Points Required
Different shapes need different number of points for reliable registration estimates.
In theory, if the point correspondences are known, four points not lying on a planeare sufﬁcient to unambiguously ﬁnd the pose [ 12]. If the correspondences are un-
known, there may exist multiple valid solutions for the pose when only a small
number of points are available (see Fig. 2). While the authors are not aware of
any prior work that describes the lower limit on the number of random points re-
quired to reliably estimate the pose, the works of Simon et al. [ 7] (later extended by
Ellis and Ma [ 24]) comes closest to answering this question. Given the geometry
of the object, Simon et al. ﬁnd a small number of feature points in the frame of the
geometric model, which when probed helps provide reliable registration estimates.But in order to probe these points, their locations need to be known in the robot
frame. Thus their approach produces good results only when the initial registration
guess is close to the true registration.
In an attempt to empirically ﬁnd the minimum number of point measure-
ments required for reliable registration, we perform an experiment where pran-
dom points from the model are selected. A known transformation is applied tothese ppoints. The applied transformation and RMS error are estimated us-
ing dSPR. The applied transformation is parameterized by Cartesian coordinates
(x,y,z)∈R
3and Euler angles (θx,θ y,θ z)∈R3. Each translation parameter is
1All the computations are carried using MATLAB R2015a software from MathWorks, running on
a ThinkPad T450s computer with 8 GB RAM and intel i7 processor.
752 R. A. Srivatsan et al.
Fig. 3 Plot of RMS error
versus number of points used
for registration, when using
dSPR. For each integer
element on the X axis, mean
error is computed over 100
experiments. Most of the
shapes considered need ≈20
measurements for accurate
registration
uniformly drawn from [−30,30]mm and each orientation parameter is uni-
formly drawn from [−30,30]◦. The experiment is repeated 100 times and the
mean error is calculated. This process is repeated for different values of p, where
p∈{4,5,..., 36}. We perform this experiment for several shapes namely, Bunny,
Armadillo, Dragon, Happy Buddha, Lucy, Thai Statue—obtained from the Stanford
Point Cloud library [ 36], Fertility obtained from the AIM shape repository [ 37],
femur bone ,liver obtained from https://grabcad.com andpelvis bone obtained from
https://www.thingiverse.com/ .
It is observed that some shapes like pelvis and Bunny require only 16 points, while
others like the Dragon require 36 points. Most of the shapes need ≈20 points. Given
a new shape, similar experiments can be run to empirically ﬁnd out the minimum
number of random points required for reliably registration estimate (Fig. 3).
5.2 Robustness to Noise
We repeat the experiment from the previous section for a femur bone , with different
amount of noise added to the points. The dashed lines in Fig. 4show the mean error for
dSPR and pSPR over 100 experiments versus the number of measurements used. The
RMS error for both dSPR and pSPR decreases to zero after 12 and 18 measurements
respectively.
The experiments were repeated under identical conditions, with a noise uniformly
drawn from [−2,2]mm added to each coordinate of the point measurements. Fig-
ure4shows that the mean error for dSPR and pSPR both converge to less than 2 mm
after 20 measurements. The performance of both dSPR and pSPR are very similar—
Sparse Point Registration 753
Fig. 4 Plot of the RMS error versus number of measurements used for dSPR and pSPR, with a
without noise in the measurements. In the absence of noise, dSPR takes 12 measurements and pSPR
takes 18 measurements to converge to zero RMS error. In the presence of a uniform noise of 2 mm,
both pSPR and dSPR converge to an RMS error of <2 mm after 20 measurements
with and without measurement noise. Table 1shows the RMS error for dSPR and
pSPR as well as the estimation time, for varying levels of noise, using 20 point
measurements and 100 measurements. When using 100 measurements, as expected,
dSPR, pSPR, standard ICP, bDQF and UPF accurately estimate the registration. How-
ever, when using 20 measurements, dSPR and pSPR outperform the other methods.
UPF takes the most computation time compared to all the other methods because of
the need to iterate over ≈2000 pose particles (other researchers have also noticed the
high computation time of UPF [ 4]).
In this work, we do not present comparisons with other popular registration meth-
ods such as generalized ICP [ 3], UKF-based registration [ 4], DQF [ 10], iterative
most likely point [ 5], etc. as those methods are not designed to work with less than
100 measurements and hence the comparison would be unfair.
Even though the RMS error is similar, the time taken by pSPR is greater than
dSPR. This is because each ICP evaluation in dSPR internally takes ≈20 iterations,
while each bDQF evaluation in pSPR internally takes ≈50 iterations for convergence
to a local minima. The beneﬁt of using pSPR however, lies in the fewer parameters
that require tuning.
5.3 Point Selection Criteria
If an operator has visual information about the environment and telemanipulates the
robot, then it is trivial to pick points on the object spread across the surface of the
object. But if the robot is autonomously collecting point measurements then it is
critical to ensure that the points are randomly distributed over the surface. To this
754 R. A. Srivatsan et al.
Ta b l e 1 Femur bone: registration in the presence of noise
No noise 2 mm noise 5 mm noise
RMS (mm) Time (s) RMS (mm) Time (s) RMS (mm) Time (s)
20 points
dSPR 0 0.08 1.08 0.44 1.79 0.46
pSPR 0 0.07 1.63 1.00 1.66 0.99
ICP 4.72 0.01 2.92 0.01 6.69 0.01
bDQF 2.84 0.05 2.19 0.09 7.83 0.09
UPF 7.13 161.77 22.12 159.83 24.23 242.38
100 points
dSPR 0 0.03 0.59 0.51 1.12 0.53
pSPR 0 0.13 0.64 2.53 1.07 3.10
ICP 0 0.01 0.72 0.01 0.93 0.01
bDQF 0 0.08 0.38 0.19 1.55 0.24
UPF 4.82 1891.1 8.06 1843.3 14.29 1594.0
effect, two strategies that have been developed in this work. For both the strategies,
ﬁrst ﬁnd the location and dimensions of a cuboid in the workspace of the robot,
within which the object lies. The object can be probed from 5 faces of the cuboid(Assuming the object rests on a table and cannot be probed from the bottom face).
The guidelines for probing the object to obtain point measurements are:
1. Choose a face of the cuboid at random, pick a point on this face at random and
probe along the direction joining the chosen point and the center of the bottom
face of the cuboid (as shown in Fig. 5a). Stop moving the robot, once it makes
contact with the object.
2. If the object is relatively ﬂat (the smallest face of the cuboid is <30% of the
largest face), then the previous strategy would result in most of the probed pointslying on the face with the largest area. So an alternate strategy is followed, where
a random point is chosen on the face with largest area. The robot is moved in the
direction of the surface normal of this face, until contact is made with the object
(see Fig. 5b).
It is observed that such a strategy ensures that the points probed are spread evenly
over the surface of the object. It is also observed that sometimes the robot might pass
through holes in the object and make contact with the environment instead of the
object. Such points are not considered in the computations in this work. Note that theprobing strategies discussed here are only a general guideline, and the presented SPR
approach would work ﬁne if measurements are obtained using alternate strategies
such as [ 31], or manually by a teleoperator [ 38].
Sparse Point Registration 755
Fig. 5 A cuboid is selected in the workspace of the robot that conservatively estimates the location
of the object. aDifferent probing paths for the robot are selected such that the probed points are
spread across the surface of the object. The colors of the path show the face of the cuboid that thepaths originate from. bPoint collection strategy for relatively ﬂat object. Some paths do not produce
a point on the object. If the robot does not make contact with the object during the course of itspath, then the point is not included in the registration
6 Robot Experiments
In order to test our approach to SPR with real data, an experimental setup as shown
in Fig. 6a is used. The setup consists of a 6-DOF robot Foxbot®equipped with an
ATI Nano17 force sensor at the end-effector. The object of interest is clamped in
front of the robot and is probed using the strategies described in Sect. 5.3.T h e
objects chosen for this experiment are: Femur bone, pelvis bone and Stanford bunny.
The largest dimension for each of these objects is 100 mm. Using the information
obtained from Fig. 3, we collect 18, 20 and 20 points respectively for the femur,
pelvis and bunny respectively. The following video shows the experiment in progress:
https://youtu.be/xnkGRFBDE0Q .
The blue circles in Fig. 6b–d show the initial guess for the location of the point
measurements and the red circles show the location as estimated by dSPR. Note that
the estimated location of the points lie on the model of the objects. The RMS error
for the location estimated by dSPR and pSPR are shown in Table 2. For 20 point
measurements, the RMS error for bunny is higher than the other two models as is
consistent from our simulation experiments (see Fig. 3).
756 R. A. Srivatsan et al.
Fig. 6 a Experimental set up consists of a robot manipulator equipped with a force sensor. The
object that is to be registered is clamped and held in place. b–dBlue circles represent the initial
location of the point measurements and red circles represent the registered location of the points.Pelvis bone, femur bone and bunny are probed at 18, 20 and 20 points respectively
Ta b l e 2 Experimental results
Pelvis Femur Bunny
RMS (mm) Time (s) RMS (mm) Time (s) RMS (mm) Time (s)
dSPR 2.17 1.5 1.38 1.56 5.00 1.76
pSPR 2.21 3.38 2.33 3.01 4.91 3.95
7 Conclusion
In this work, a sparse point registration (SPR) method for robust registration using
a small number of sparse point measurements was developed. The approach can be
implemented in a deterministic manner (dSPR) or a probabilistic manner (pSPR).The dSPR is faster but has more parameters to tune than pSPR. pSPR has the added
advantage over dSPR of providing the uncertainty in the registration estimate. We also
provide guidelines for choosing the tuning parameters for each algorithm. Anothercontribution of this work is the development of a batch processing variant of the dual
quaternion ﬁlter [ 10], which is used in the pSPR.
Through simulations and robot experiments, both dSPR and pSPR are found to
be robust and accurate compared to state-of-the-art methods. Even in the presence
of noise, our approach accurately estimates the registration compared to populardeterministic and probabilistic approaches for registration. The computation time
was≈1 s for dSPR and ≈3 s for pSPR. A C++ implementation would greatly reduce
the computation time. Through a number of simulations, it is empirically found thatmost shapes require ≈20 points for reliable registration. Future work would explore
a more theoretical approach for ﬁnding the lower bound on the number of random
points required for registration.
Sparse Point Registration 757
In the future, we plan to extend the formulation to ﬂexible objects that can deform
upon contact. Prior work such as [ 23] can be used to estimate the local deformation
introduced by forceful contact. Future work will also explore using surface-normalin addition to point measurements for registration.
Acknowledgements This work was supported by NRI large grant IIS-1426655. The authors would
also like to thank Mr. Nicolas Zevallos, Mr. Ky Woodard and the Center for Machine Learning and
Health, Carnegie Mellon University.
References
1. Besl, P.J., McKay, N.D.: Method for registration of 3-D shapes. In: Robotics-DL Tentative, pp.
586–606. International Society for Optics and Photonics (1992)
2. Rusinkiewicz, S., Levoy, M.: Efﬁcient variants of the ICP algorithm. In: Proceedings of 3rd
International Conference on 3-D Digital Imaging and Modeling, pp. 145–152. IEEE (2001)
3. Segal, A., Haehnel, D., Thrun, S.: Generalized-ICP. In: Robotics: Science and Systems, vol. 2,
no. 4 (2009)
4. Moghari, M.H., Abolmaesumi, P.: Point-based rigid-body registration using an unscented
Kalman ﬁlter. IEEE Trans. Med. Imaging 26(12), 1708–1728 (2007)
5. Billings, S.D., Boctor, E.M., Taylor, R.H.: Iterative most-likely point registration (IMLP): a
robust algorithm for computing optimal shape alignment. PLoS ONE 10(3) (2015)
6. Audette, M.A., Ferrie, F.P., Peters, T.M.: An algorithmic overview of surface registration tech-
niques for medical imaging. Med. Image Anal. 4(3), 201–217 (2000)
7. Simon, D.A., Hebert, M., Kanade, T.: Techniques for fast and accurate intrasurgical registration.
J. Image Guided Surg. 1(1), 17–29 (1995)
8. Ma, B., Ellis, R.E.: Robust registration for computer-integrated orthopedic surgery: laboratory
validation and clinical experience. Medical Image Anal. 7, 237–250 (2003)
9. Ma, B., Ellis, R.E.: Surface-based registration with a particle ﬁlter. In: International Conference
on Medical Image Computing and Computer-Assisted Intervention, pp. 566–573. Springer
(2004)
10. Srivatsan, R.A., Rosen, G.T., Naina, F.D., Choset, H.: Estimating SE(3) elements using a dual
quaternion based linear Kalman ﬁlter. In: Robotics: Science and Systems (2016)
11. Penney, G.P., Edwards, P.J., King, A.P., Blackall, J.M., Batchelor, P.G., Hawkes, D.J.: A stochas-
tic iterative closest point algorithm (stochastICP). In: MICCAI, pp. 762–769 (2001)
12. Horn, B.K.: Closed-form solution of absolute orientation using unit quaternions. JOSA A 4(4),
629–642 (1987)
13. Tam, G.K., Cheng, Z.-Q., Lai, Y .-K., Langbein, F.C., Liu, Y ., Marshall, D., Martin, R.R., Sun,
X.-F., Rosin, P.L.: Registration of 3d point clouds and meshes: a survey from rigid to nonrigid.IEEE Trans. Vis. Comput. Graph. 19(7), 1199–1217 (2013)
14. Tsin, Y ., Kanade, T.: A correlation-based approach to robust point set registration. In: European
Conference on Computer Vision, pp. 558–569. Springer (2004)
15. Phillips, J.M., Liu, R., Tomasi, C.: Outlier robust ICP for minimizing fractional RMSD. In:
Sixth International Conference on 3-D Digital Imaging and Modeling, 2007: 3DIM’07, pp.427–434. IEEE (2007)
16. Gelfand, N., Mitra, N.J., Guibas, L.J., Pottmann, H.: Robust global registration. In: Symposium
on Geometry Processing, vol. 2, no. 3, p. 5 (2005)
17. Makadia, A., Patterson, A., Daniilidis, K.: Fully automatic registration of 3D point clouds. In:
CVPR, vol. 1, pp. 1297–1304. IEEE (2006)
18. Yang, J., Li, H., Jia, Y .: Go-ICP: solving 3D registration efﬁciently and globally optimally. In:
IEEE International Conference on Computer Vision, pp. 1457–1464 (2013)
758 R. A. Srivatsan et al.
19. Pennec, X., Thirion, J.-P.: A framework for uncertainty and validation of 3-D registration
methods based on points and frames. Int. J. Comput. Vis. 25(3), 203–229 (1997)
20. Hauberg, S., Lauze, F., Pedersen, K.S.: Unscented Kalman ﬁltering on Riemannian manifolds.
J. Math. Imaging Vis. 46(1), 103–120 (2013)
21. Srivatsan, R.A., Xu, M., Zevallos, N., Choset, H.: Bingham distribution-based linear ﬁlter for
online pose estimation. In: Robotics: Science and Systems (2017)
22. Srivatsan, R.A., Choset, H.: Multiple start branch and prune ﬁltering algorithm for nonconvex
optimization. In: W AFR (2016)
23. Srivatsan, R.A., Ayvali, E., Wang, L., Roy, R., Simaan, N., Choset, H.: Complementary model
update: a method for simultaneous registration and stiffness mapping in ﬂexible environments.In: International Conference on Robotics and Automation, pp. 924–930 (2016)
24. Ma, B., Ellis, R.E.: Spatial-stiffness analysis of surface-based registration. In: MICCAI, pp.
623–630 (2004)
25. Glozman, D., Shoham, M., Fischer, A.: A surface-matching technique for robot-assisted reg-
istration. Comput. Aided Surg. 6(5), 259–269 (2001)
26. Gadeyne, K., Bruyninckx, H.: Markov techniques for object localization with force-controlled
robots. In: International Conference on Advanced Robotics (2001)
27. Chhatpar, S.R., Branicky, M.S.: Particle ﬁltering for localization in robotic assemblies with
position uncertainty. In: IROS, pp. 3610–3617 (2005)
28. Hsiao, K., Kaelbling, L.P.: Task-driven tactile exploration. In: Robotics: Science and Systems
(2010)
29. Petrovskaya, A., Khatib, O.: Global localization of objects via touch. IEEE Trans. Robot. 27(3),
569–585 (2011)
30. Saund, B., Chen, S., Simmons, R.: Touch based localization of parts for high precision manu-
facturing. In: International Conference on Robotics and Automation. IEEE (2017)
31. Javdani, S., Klingensmith, M., Bagnell, J.A., Pollard, N.S., Srinivasa, S.S.: Efﬁcient touch based
localization through submodularity. In: International Conference on Robotics and Automation,pp. 1828–1835. IEEE (2013)
32. Hebert, P., Howard, T., Hudson, N., Ma, J., Burdick, J.W.: The next best touch for model-based
localization. In: ICRA, pp. 99–106 (2013)
33. Pauly, M., Gross, M., Kobbelt, L.P.: Efﬁcient simpliﬁcation of point-sampled surfaces. In:
Conference on Visualization’02, pp. 163–170 (2002)
34. Cignoni, P., Montani, C., Scopigno, R.: A comparison of mesh simpliﬁcation algorithms. Com-
put. Graph. 22(1), 37–54 (1998)
35. Garland, M., Heckbert, P.S.: Surface simpliﬁcation using quadric error metrics. In: Proceedings
of the 24th Annual Conference on Computer Graphics and Interactive Techniques, pp. 209–216.ACM Press/Addison-Wesley Publishing Co. (1997)
36. Turk, G., Levoy, M.: The Stanford 3D Scanning Repository. Stanford University Computer
Graphics Laboratory. http://graphics.stanford.edu/data/3Dscanrep
37. AIM@SHAPE Model Repository. Fertility Point Cloud Scan. http://visionair.ge.imati.cnr.it/
ontologies/shapes/releases.jsp
38. Srivatsan, R.A., Vagdargi, P., Zevallos, N., Choset, H.: Multimodal registration using stereo
imaging and contact sensing. In: Robotics: Science and Systems, Workshop on ‘Revisiting
Contact—Turning a Problem into a Solution’ (2017)
Autonomous Exploration
with Expectation-Maximization
Jinkun Wang and Brendan Englot
1 Introduction
While simultaneous localization and mapping (SLAM) has been applied successfully
for state estimation and map-building with data collected passively on sensing plat-forms, it’s still a challenge for an autonomous vehicle to actively explore an unknown
environment and manage the quality of its state estimate and map. The capability
of autonomous exploration may be especially impactful in scenarios where teleop-
eration is limited or infeasible due to constrained communication, e.g. in unknown
subsea environments with underwater robots.
In general, the autonomous exploration problem for mobile robots comprises three
stages: (1) the robot identiﬁes candidate locations to explore or paths to follow; (2) a
utility function is evaluated for every candidate and the optimal one is selected; (3)the robot executes the optimal action and updates its current knowledge of the envi-
ronment. While the ﬁrst task is commonly achieved by enumerating frontier locations
or by employing sampling-based methods and the third task is simply performed byfeedback controllers, the remaining question is how to properly formulate a utility
function that effectively captures the exploration-exploitation dilemma, i.e., a bal-
ancing of visiting unknown areas to reduce map uncertainty and revisiting knownareas to seek better localization.
J. Wang ·B. Englot (B)
Department of Mechanical Engineering, Stevens Institute of Technology,Castle Point on Hudson, Hoboken, NJ 07030, USA
e-mail: benglot@stevens.edu
J. Wang
e-mail: jwang92@stevens.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_53759
760 J. Wang and B. Englot
1.1 Related Work
Without considering the robot’s localization uncertainty, the problem has been
approached by using Cauchy-Schwarz quadratic mutual information (CSQMI) toreduce computation time [ 6], by combining global planning with local motion prim-
itives and also reﬁning a trajectory using optimization methods [ 7], and by exploring
on continuous Gaussian process frontier maps [ 11]. The simpliﬁed problem, plan-
ning with a priori maps, has also been discussed [ 9,17,19] to actively minimize the
uncertainty of known landmarks.
Most of the existing research on exploration in unknown environments takes
advantage of occupancy grid maps, considering utility functions of map entropy and
of the uncertainty of robot poses. An integrated exploration strategy was proposed
to combine different utilities [ 2,16], e.g. the utility of information gain over the
occupancy grid maps, the utility of travel distance to the goal, and the utility of local-
izability, which incorporates the uncertainty of robot poses and landmarks. However,these methods ignore the correlation between localization and information gain.
High-uncertainty poses are likely to result in inaccurate occupancy grid maps,
limiting the usefulness of the information gained by exploring unknown regions.The correlation between localization and information gain was taken into account
by integrating over a map’s entropy weighted by trajectory probability [ 20,22,23].
The trajectory probability was transformed into particle weight in the particle ﬁlterin [20], and the integral was simpliﬁed to use only the mean trajectory in [ 23].
Instead of leveraging the entropy of occupancy grid maps, in this paper, we pro-
pose the concept of a virtual map comprised of virtual landmarks , and acting to
minimize the uncertainty of these landmarks. In occupancy grid mapping, grid cells
inside the sensor observation cone are less affected by the uncertainty of robot poses,
which gives rise to overconﬁdence in the information gained by exploring unknownmaps. In contrast to that, every virtual landmark is deeply connected with robot
poses that can observe it in our proposed approach. The proposed metrics contain
the uncertainty from potential landmarks that may be observed in the future, and
therefore, more accurate landmark positions are obtained in the course of exploring
an a priori unknown environment.
1.2 Paper Organization
We describe in Sect. 2the formal deﬁnition of the exploration problem within a
feature-based SLAM framework, and the proposed utility function is formulated
by considering virtual landmarks. In Sect. 3, the optimization problem with latent
variables is solved through expectation-maximization, where the E-step involvesupdating the occupancy probabilities (Sect. 3.3) and the M-step searches for the
best path among motion primitives (Sect. 3.4). Experimental results are presented in
Sect. 4, with conclusions and discussion of future work in Sect. 5.
Autonomous Exploration with Expectation-Maximization 761
2 The Exploration Problem with Feature-Based SLAM
In this paper, we consider the autonomous mobile robot exploration problem in
unknown environments, where’s the robot’s objective guiding exploration is to pro-duce a feature-based map of its surroundings accurately and efﬁciently. Here we
make the following assumptions for simplicity:
1. There are a certain number of static landmarks in the environment which can be
used for localization.
2. Landmarks are identiﬁable without considering the data association problem.3. The movement of the robot is conﬁned in a limited space, the border of which
doesn’t have any landmarks.
2.1 The SLAM Framework
We use the smoothing -based approach rather than the ﬁltering -based approach, adopt-
ing incremental smoothing and mapping [ 14] to repeatedly estimate the entire robot
trajectory. The beneﬁt of that will be discussed in Sect. 3.
Let a mobile robot’s motion model be deﬁned as
xi=fi(xi−1,ui)+wi,wi∼N(0,Qi), (1)
and let the measurement model be deﬁned as
zij=gij(xi,lj)+vij,vij∼N(0,Rij), (2)
where we assume the data association between xi,ljis known.
Given measurements Z={ zk}, we can obtain the best estimate of the entire
trajectory X={ xi}and observed landmarks Lo={ lj},
X∗,L∗
o|Z=argmin
X,LoP(X,L|Z). (3)
The maximum a posteriori (MAP) estimate can be used by maximizing the joint
probability, which afterwards leads to a nonlinear least-squares problem. By con-
structing a graph representation and linearizing nonlinear functions, the marginal
distributions and joint marginal distributions, both of which are Gaussian, can beextracted using graphical model-based inference algorithms [ 14].
762 J. Wang and B. Englot
2.2 Utility Function
Let L={ li}be the set of landmarks in the environment. We have an estimate for
each of them li∈R2, which is distributed as N(ˆli,Σ li). The deﬁnition of the utility
function for the exploration problem is as follows,
U=/summationdisplay
li∈Lφ(Σ li), (4)
where φ:Σ→ Rrepresents the uncertainty criterion for covariance matrices (see
Sect. 3.1). In addition to the uncertainty of landmarks, it is beneﬁcial to add a cost-
to-go C(a)to favor a shorter path [ 20]. Thus the optimal action sequence is the one
minimizing the uncertainty with a weighted smaller cost:
a∗=argmin
aU+αC. (5)
Since there are landmarks that haven’t been observed yet, in most works, the
common strategy is to omit the values of unknown landmarks and replace them
with the entropy of occupancy grid maps. In this paper, we introduce the concept ofvirtual landmarks and treat the landmarks as latent variables. The objective is then
to minimize the uncertainty of possible landmarks that would be observed when
following the planned path. This uncertainty is deﬁned
U≈/summationdisplay
vk∈Vφ(Σ vk), (6)
where V={ vk}arevirtual landmarks (see Fig. 2), which represent all the possible
locations of actual landmarks. They won’t be incorporated into a robot’s SLAMoptimization unless they are revealed to be true landmarks, but the potential to reduce
their uncertainty is considered throughout the course of autonomous exploration.
Intuitively, the approximation computes the expected uncertainty of actual land-
marks after taking into account the observations collected while following a can-
didate path. The function provides a trade-off between exploration and localization
internally: exploration will decrease the uncertainty of virtual landmarks, which areinitialized to have large covariance matrices, but poor-quality localization will lead
to higher cost, since the covariance matrices of landmarks are estimated based on
the robot poses that can observe them.
Autonomous Exploration with Expectation-Maximization 763
3 The EM Exploration Algorithm
Because of the introduction of latent variables, it is intuitive to approach the explo-
ration problem using an EM algorithm. Such an approach encompasses the updateof the probability of a landmark’s existence, represented by virtual landmarks, and
also the prediction of its uncertainty. Here we show that iteratively the probability
can be calculated from an occupancy grid map maintained along the landmarks inthe Expectation step, and afterwards it is utilized to evaluate the covariance criteria
in the Maximization step. Correspondingly, the E-step provides the expected land-
mark locations, and the M-step is merely to maximize the utility function, whichessentially is an exploration problem with prior knowledge of the environment.
A mathematical formulation is as follows. We wish to maximize the marginal
log-likelihood of observations Zgiven an action sequence awith latent variables V:
lnp(Z|a)=ln/braceleftBig/summationdisplay
Vp(Z,V|a)/bracerightBig
. (7)
Different from traditional EM applications, the exploration problem involves col-
lecting measurements after each iteration. For that reason, we assume a maximum
likelihood measurement model and add measurements of all virtual landmarks intoZin Eq. 7. For virtual landmarks beyond the robot’s maximum sensing range, we
use priors with large uncertainty.
According to the EM algorithm, in the E-step, we use the current a
oldto evaluate
the posterior distribution of latent variables p(V|Z,aold). In the M-step, we determine
anewby maximizing the function below:
anew=argmax
a/summationdisplay
Vp(V|Z,aold)lnp(Z,V|a). (8)
The equation above poses a challenge for efﬁcient solution due to the exponential
growth of virtual landmark conﬁgurations with respect to the number of virtual
landmarks. Inspired by classiﬁcation EM algorithms, an alternative solution wouldadd a classiﬁcation step (C-step) [ 5] before the M-step to provide the maximum
posterior probability estimate of the virtual landmark conﬁguration,
V
∗=argmax
Vp(V|Z,aold). (9)
Afterwards, the M-step can be solved given the deterministic variable V∗as follows,
anew=argmax
alnp(Z,V∗|a) (10)
=argmin
alnΣV∗=argmin
aφ(Σ V∗). (11)
764 J. Wang and B. Englot
Here, we choose to replace the log-likelihood function with the logarithm of the
determinant because we assume Gaussian distributions of the virtual landmarks and
maximum likelihood observations. We also use φto generalize the log-determinant
covariance criterion as shown in Sect. 3.1.
3.1 Uncertainty Criteria
In this section, we discuss the choice of uncertainty criteria φ(Σ V)in Eq. 11.I n
general, there are two commonly used uncertainty criteria in the exploration problem,
namely A-Optimality and D-Optimality [ 4,19], formulated respectively as:
φA=n/summationdisplay
i=1λi=tr(Σ V) (12)
φD=n/productdisplay
i=1λi=det(Σ V). (13)
Assuming all of the virtual landmarks are independent, the computation of deter-
minant and trace can be written as tr (Σ V)=/summationtext
vk∈Vtr(Σ vk)and det (Σ V)=/producttext
vk∈V
det(Σ vk). As pointed out in [ 19], we can drive the determinant of the entire covari-
ance matrix to zero by minimizing the eigenvalue of one feature to zero. Speciﬁcally,D-optimality doesn’t capture the global information gain and hence we choose A-
optimality in the utility function, and also in the evaluation of all of the algorithms
examined in this paper. Now we can derive the utility function introduced in Eq. 6.
3.2 Covariance Intersection
To obtain the covariance estimate of a virtual landmark from poses where the robot
is able to observe it, it is desirable to add these “virtual measurement factors” into
the graphical model and then compute its marginalization. However, two problems
arise due to this addition. First, due to the large number of virtual landmarks, thevirtual factors increase the burden of optimization. Second, some virtual landmarks
will become actual landmarks, providing localization information for the system.
For a virtual landmark v
k, suppose that there are nrelated robot poses ˘xk=
[x1x2...xn]T, each of which corresponds to a measurement term zjk=gjk(xj,vk)+
vjk. Then the predicted covariance estimate per an EKF is HΣ˘xkHT, where H=
diag(H1,H2,..., Hn)is the Jacobian matrix with Hi=∂gjk
∂xj|ˆxj, andΣ˘xkis the marginal
covariance of poses observing the virtual landmark. Both [ 13,21] present an efﬁcient
algorithm to recover the parts of the covariance matrix associated with non-zero
factor entries, which correspond to variables in the same clique of a Bayes tree [ 14].
Autonomous Exploration with Expectation-Maximization 765
However, the condition is unlikely to be satisﬁed, and the covariance estimate must
be approximated to avoid the expensive recovery of full pose uncertainty.
The Covariance Intersection (CI) algorithm [ 8] was presented to estimate the
covariance matrix when the correlation between two sources are unknown. Let Σ1=
H1Σx1HT
1,Σ2=H2Σx2HT
2be two independent estimates from the ﬁrst two poses.
Then the CI combines these two estimates to provide an upper bound based on thefollowing equation:
Σ
−1=ωΣ−1
1+(1−ω)Σ−1
2. (14)
The weight ωcan be optimized to achieve the minimal determinant of Σas follows:
ω=2b−ac
2(a+b−ac), (15)
where a=|Σ1|−1,b=|Σ2|−1, and c=Tr(Σ1Σ−1
2). By iteratively fusing estimates
from different poses, we derive an optimal upper bound on the actual covariance in
terms of the minimal determinant, given only the block diagonal of Σ˘xk.
3.3 Expectation Step
The E-step is used to compute qk/definesp(vk∈L), the probability of a potential land-
mark given the current measurements. By discretizing the map into grid cells, these
probabilities are equivalent to the occupancy probabilities used in occupancy gridmaps, that is q
k=p(mk|X,Z)[18].
Generally, occupancy grid maps are constructed based on the most likely hypoth-
esis of the robot path X∗. As a result, the uncertainty of poses is not expressed
in grid maps, which, under circumstances with severe drift, would falsely exclude
some hypotheses that observe actual landmarks by reducing their probabilities close
to zero. Therefore, we adopt the concept of an expected map [1] to take a weighted
average of occupancy grid maps computed from all path hypotheses. Different from
the method in the original paper which relies on samples generated by particle ﬁlter-
ing, we employ Monte Carlo sampling to approximate the integral over the continuous
space of poses,
qk=E[p(mk|X,Z)] (16)
=/integraldisplay
Xp(mk|X,Z)p(X,Lo|Z) (17)
≈1
NN/summationdisplay
n=1p(m[n]
k|X[n],Z), (18)
766 J. Wang and B. Englot
Algorithm 1 Qt+1,Xt+1,Lt+1
o=Expectation (Xt,Lt
o,Zt)
1:Qt+1←∅
2:Xt+1,Lt+1
o←Optimize (Xt,Lt
o,Zt)// iSAM2
3:{X[n]}← Sample (Xt+1)
4:for v k∈Vdo
5: for X[n]∈{ X[n]}do
6: qt+1
k=p(mk|X[n],Zt)/#samples
7: end for
8: Qt+1← Qt+1∪qt+1
k
9:end for
10: return Qt+1,Xt+1,Lt+1
o
where Nis the number of samples. The expectation step is performed as shown in
Algorithm 1, in which the occupancy probabilities computed from posterior samples(line 3) are averaged (line 4–9) and after that the distributions of virtual landmarks
are updated.
3.4 Maximization Step
In the M-step, given the distribution of virtual landmarks, the path candidates are
selected and the utility function is evaluated for each of them. The global paths
over a long period must take into account two types of actions, exploration actions
and place-revisiting actions [ 20]. Usually exploration actions have destinations near
frontier locations where explored cells meet unknown cells, and place-revisiting
actions choose to go back to locations the robot has visited. The prevalence of these
particular locations requires us to examine a large amount of free grid cells in orderto obtain a near-optimal solution. After identifying destinations, it is also of great
beneﬁt to reﬁne the path to a destination rather than take the shortest one [ 7].
In consideration of these factors, we employ a sampling-based routine for gen-
erating action primitives, in which exploration and revisiting actions, possibly with
varying paths, are considered simultaneously. The rapidly-exploring random tree
(RRT) [ 15] is a randomized algorithm designed to quickly solve motion planning
problems in high-dimensional spaces with constraints such as obstacles and dynam-
ics. RRTs have the property of probabilistic completeness, ensuring that a solution
will be found in the limit, if a feasible solution exists. Also due to their randomizedconstruction, a tree might end up with a variety of paths to the same goal region.
RRT presents a simple and efﬁcient method for building our action primitives.
An optimal variant of the RRT algorithm, RRT\* [ 12], guarantees asymptotic
optimality, converging in the limit toward the shortest path. And RRT\* has been
successfully applied to active SLAM to generate a robot’s action set [ 23]u s i n gt w o
“distance functions”, one is the traditional Euclidean travel distance and the other one
is cumulative map entropy reduction. As shown in [ 23], generating candidate paths
Autonomous Exploration with Expectation-Maximization 767
Algorithm 2 Zt=Maximization (Qt,Xt,Lt
o)
1: Tree ←BuildRRT ()
2:for a∈Tree do
3: Va←∅
4: Za←Predict (Xt,a)
5: Xa,La
o←Optimize (Xt,Lt
o,Za)
6: for qt
k∈Qtdo
7: Σa
vk=CI(˘xa
k,block-diagonal (Σa
˘xk))
8: Va← Va∪(qt
k,Σa
vk)
9: end for
10: end for
11: a∗=argmina/summationtext
vk∈VaI(qk≥poccupied )Σvk
12: Zt=Execute (a∗,#steps )
13: return Zt
using the robot’s travel distance as a cost metric still provides a sufﬁcient diversity
of primitives to achieve a good compromise between computational complexity and
entropy reduction. Since our computational task is even more expensive, we also
adopt the Euclidean travel distance in the Nearest function of RRT.
The M-step is summarized in Algorithm 2, in which the details of building RRTs
are omitted for the sake of brevity. The utility function is evaluated for every path
afrom the tree root to the leaves. First, the measurements following the path are
predicted assuming maximum likelihood observations (line 4). Second, the whole
trajectory is re-optimized to incorporate future measurements (line 5). Third, the
virtual landmarks are updated using the optimized trajectory (line 6–9). Finally thebest action sequence is selected with the minimum utility value, which is computed
from the updated virtual landmarks (line 11). To account for the deviation from the
nominal trajectory during execution, model predictive control (MPC) performs onlythe ﬁrst few steps [ 10] (line 6–12). One example of the M-step is shown in Fig. 2.
3.5 Algorithm Analysis
The proof that the EM exploration algorithm maximizes the likelihood of measure-
ments is essentially to explain why the function in Eq. 8is increased after executing
action anew. This will be satisﬁed if the prior uncertainties of virtual landmarks are
large enough to encourage exploration. However, this also implies that the algorithm
can fail to explore if it does not have a properly initialized prior—in such a case, arobot may conclude that there isn’t a viable path that explores an unknown area with
a guarantee of acceptable estimates of the surrounding landmarks.
Next we analyze the computational complexity of the algorithm. The computation
in Algorithm 1 involves an iSAM2 update, sampling from the robot trajectory, and
an occupancy probability update. The complexity of the iSAM2 update generally is
bounded by O(n
3), where nis the number of variables [ 14]. The most expensive part
768 J. Wang and B. Englot
in the sampling phase is to extract the joint covariance, which depends on the sparsity
of the factor matrix. For the case of exploration, we force the robot to perform loop-
closure occasionally, and the complexity of covariance recovery could deteriorate toO(s
3), where sis the size of the dense block in the factor matrix [ 13]. The update of
occupancy probability has the computational complexity O(Nsamples ·Nvl), in which
Nsamples is the number of samples in Eq. 18, and generally a small set of samples
can well approximate the expected map. Nvlis the number of virtual landmarks; an
appropriate resolution may vary depending on the application.
Algorithm 2 has the computational complexity of O(Nnodes+Nleaves(C1+C2)),
where the ﬁrst term describes the construction of an RRT, and the second term
describes the evaluation of our utility function ( C1for the iSAM2 update and C2for
the covariance update for virtual landmarks) for each path to a tree leaf. To alleviatecostly computation, we propose a simpliﬁed process to handle situations without
loop-closures. First, EKF propagation is used to predict the covariance instead of
using iSAM2. Second, the virtual landmarks are cached in every node, and thecovariances of a child node inherit from its parent with a few updates after incorpo-
rating the latest robot poses. Overall, by limiting the number of nodes in the RRT,
we are able to achieve nearly real-time decision making (see Sect. 4for details).
4 Experiments and Results
We analyze the performance of the proposed algorithm in a simulated environment
(see Fig. 1as an example). The simulation employs an environment with point fea-
tures uniformly distributed in its inner region (the square [−20 m,20 m]in Fig. 1).
The robot is equipped with a range sensor with a limited ﬁeld of view (FOV) that
is capable of measuring the relative range and bearing to a landmark. Zero-mean
Gaussian noise is added to both measurements. Our mobile robot is conﬁgured as aDubins car, which we assume has constant translational and rotational speed during
one simulation period T. The assumed vehicle dynamics are as follows:
x
t=xt−1+vT1dtcosθt−1 (19)
yt=yt−1+vT1dtsinθt−1 (20)
θt=θt−1+ωT1dt, (21)
where vT1∈[vmin,vmax],ωT1∈[ωmin,ωmax],t∈[Tmin,Tmax]. Similarly, zero-mean
Gaussian noise is added to the state propagation equations above. We also place
a circumscribed obstacle radius around landmarks to avoid collision. The robot is
initialized with low uncertainty to perform the exploration task. The conﬁgurationdetails of our simulation environment are listed in Table 1.
The RRT planner only takes samples within the free space, which is deﬁned by
an occupancy probability less than 0 .4. To take into account the kinodynamic con-
Autonomous Exploration with Expectation-Maximization 769
Fig. 1 A few representative steps of planning and execution using our proposed algorithm, over
advancing, but non-consecutive time instants during a single exploration process. Virtual landmarks
are embedded in the underlying grid cell environment map with occupancy probabilities shown in
grayscale color. Error ellipses (2 standard deviations) of observed landmarks (orange) and robot
poses (green) are overlaid on top of the corresponding best estimates. An RRT rooted at the current
robot pose is constructed (purple). The trajectory from the RRT offering the expected minimum
cost is marked by a red line. From top left to bottom right, one ﬁgure showing the expected best
trajectory is followed by a ﬁgure showing its execution, and the ﬁnal ﬁgure simply represents the
ﬁnal step of the exploration task
straints, we construct a motion library in advance by forward simulating the vehi-
cle with all possible combinations of simulation period, translational and rotational
velocity. In the Steer function, the connectivity of nodes is checked through searching
for the nearest end point in the motion library given a certain radius (Fig. 2).
770 J. Wang and B. Englot
Ta b l e 1 Simulation parameters
Bearing: stddev (deg) 0.5 Translational speed (m/s) [0.5, 1.0]
Range: stddev (m) 0.002 Rotational speed (rad/s) [−0.5, 0.5]
Rotation: stddev (deg) 0.2 Simulation step (s) 0.2
Translation: stddev (m) 0.01 Simulation duration (s) [1.0, 4.0]
Bearing FOV (deg) 120 Safe distance (m) 1.5
Range FOV (m) [1.0, 8.0] Number of landmarks 20
Initial position: ([m, m]) [10.0, 0.0] Initial sigmas ([m, m, deg]) [0.05, 0.05, 0.01]
Fig. 2 Gaussian error ellipses (0.5 standard deviations) of virtual landmarks. Left: The uncertainty
of virtual landmarks at the bottom of the map isn’t reduced signiﬁcantly after exploration, as the
robot hasn’t met a loop-closure, which means the possible landmarks observed in the future would
have high uncertainty. The proposed algorithm makes a decision to travel upwards in order to
revisit a landmark for better localization. Right: The execution leads to an accurate estimate of the
trajectory, and the error ellipses of virtual landmarks shrink signiﬁcantly
4.1 Comparison
We analyze the performance of our proposed algorithm by comparing it with two
variants of entropy-based exploration algorithms.
1. SLAM-OG [ 2]. The utility function consists of a normalized localization com-
ponent (SLAM) and a normalized occupancy grid (OG) mapping component:
USLAM \_OG=αISLAM(X,L|a)
ISLAM max+(1−α)HOG(m|a)
HOGmax, (22)
where ISLAM≈/summationtext|Lo|
i=1/radicalbig
det(Σ li),HOG=/summationtext
mi∈mH(mi).
2. OG [ 1,20]. The utility function computes the map entropy weighted by the
likelihood of robot poses:
Autonomous Exploration with Expectation-Maximization 771
UOG=/integraldisplay
XH(m|X,Z)p(X,Lo|Z) (23)
≈1
NN/summationdisplay
n=1H(m[n]|X[n],Z). (24)
In the ﬁrst utility function (SLAM-OG), the uncertainty of continuous pose vari-
ables is expressed using differential entropy, unlike occupancy grid maps with adiscrete probability distribution. As a consequence, the scale of pose uncertainty is
much smaller than that of map entropy [ 3]. The second utility function (OG) priori-
tizes grid cells that lie inside the robot’s current sensor observation cone. In addition,the potential impact of loop closures on previously observed portions of the map
(and their accuracy) is ignored, due to the utility function’s emphasis on the entropy
of occupancy maps.
4.2 Results
The simulated experiments were conducted such that all the parameters are the same
for all algorithms except for the weight of the distance cost. Here we used a dis-
tance weight with a constant rate of decay with respect to the area discovered, orα=w
max#freecells
#totalcells. For each algorithm, the maximum weight wmaxwas chosen to
achieve the best performance through exhaustive search. Landmarks were uniformly
sampled in the environment and 50 environments were generated. We also tested allalgorithms using two levels of resolution for the virtual landmarks and occupancy
grids: 2 .0(m)and 0.5(m)to see the effect of using a high resolution map. One exam-
ple of exploration using the EM algorithm with a low resolution is demonstrated inFig.1. The performance of the algorithms was averaged among the 50 experiments
performed, as listed in Fig. 3.
Figure 3a shows the uncertainty reduction of landmarks, where the uncertainty
is computed by/summationtext
l∈Ltr(Σ l), in which unobserved landmarks have initial estimates
σ0=3.0. On one hand, this metric takes into account localization and mapping
uncertainty through observed landmarks; on the other hand, it also measures theexploration rate by incorporating unknown landmarks. Figure 3b shows the evolu-
tion of localization error by evaluating max
x∈Xtr(Σ x). We also compare different
algorithms with respect to average map entropy (/summationtextH(mi)/Nm) reduction, shown
in Fig. 3c. The planning time is shown in Fig. 3d under Ubuntu 14.04 on an i7-
6950X CPU, although these Python-based implementations are capable of futheroptimization.
We have the following observations from the comparisons. The OG algorithm,
weighing map entropy by localization uncertainty, has a comparable exploration ratewith the proposed algorithm (EM) in the beginning (Fig. 3a, c). However, landmark
uncertainty is reduced at a lower rate when most of the landmarks are observed
(Fig. 3a). In addition, by exploring with the EM algorithm, we end up with more
772 J. Wang and B. Englot
(a) Sum of landmarks’ uncertainty (b) Max uncertainty of the trajectory
(c) Avera ge entrop yreduction (d) Plannin gtime
Fig. 3 The results of 50 exploration trials with the same randomly initialized landmarks for every
algorithm
Fig. 4 An exploration
example in a structured
environment

Autonomous Exploration with Expectation-Maximization 773
accurate feature-based maps—their curves are closer to zero in the ﬁnal stage of
Fig.3a. The SLAM-OG algorithm places constant relative weights on SLAM and
OG uncertainty, and thus its exploration is the most conservative, but it has thelowest localization error as shown in Fig. 3b. In contrast, the EM algorithm is able to
maintain low trajectory uncertainty and it achieves a superior exploration rate until
landmark uncertainty is close to zero. More importantly, the planning time of theEM algorithm is much cheaper because it doesn’t need to calculate the map entropy
for every candidate by sampling from the posterior trajectory distributions (Eq. 24).
Overall, by using a ﬁner resolution, the performance of all algorithms improves.However, for the EM algorithm, the small gains in performance do not appear to
merit the sacriﬁce in computation time (Fig. 3d).
5 Conclusions and Future Work
In this paper, we proposed the concept of virtual landmarks, which are latent vari-
ables representing the possible locations of actual landmarks in the environment. We
presented a novel utility function for the autonomous exploration problem in feature-
based maps, which essentially computes the covariance criteria at virtual landmarks.The direct modeling of landmarks potentially observed in the future enables more
accurate mapping and also a comparable exploration rate with respect to traditional
methods. However, the evaluation of the proposed utility function relies on the recov-ery of a full trajectory, which is expensive for real-time applications. In addition, a
more memory-efﬁcient approach to storing and updating virtual landmarks is a sub-
ject of ongoing and future work. We hope to apply this algorithm to the exploration ofunderwater environments by a sonar-equipped marine robot, where structures such
as piers will be similar in appearance to the scenario depicted in Fig. 4.
Acknowledgements This research has been supported in part by the National Science Foundation,
grant number IIS-1551391.
References
1. Blanco, J.L., Fernandez-Madrigal, J.A., González, J.: A novel measure of uncertainty for mobile
robot slam with Rao-Blackwellized particle ﬁlters. Int. J. Robot. Res. 27(1), 73–89 (2008)
2. Bourgault, F., Makarenko, A.A., Williams, S.B., Grocholsky, B., Durrant-Whyte, H.F.: Infor-
mation based adaptive robotic exploration. In: Proceedings of IEEE/RSJ International Confer-ence on Intelligent Robots and Systems, pp. 540–545 (2002)
3. Carrillo, H., Dames, P., Kumar, V ., Castellanos, J.A.: Autonomous robotic exploration using
occupancy grid maps and graph slam based on shannon and rényi entropy. In: Proceedings ofIEEE International Conference on Robotics and Automation, pp. 487–494 (2015)
4. Carrillo, H., Reid, I., Castellanos, J.A.: On the comparison of uncertainty criteria for active
SLAM. In: Proceedings of IEEE International Conference on Robotics and Automation, pp.2080–2087 (2012)
774 J. Wang and B. Englot
5. Celeux, G., Govaert, G.: A classiﬁcation EM algorithm for clustering and two stochastic ver-
sions. Comput. Stat. Data Anal. 14(3), 315–332 (1992)
6. Charrow, B., Liu, S., Kumar, V ., Michael, N.: Information-theoretic mapping using Cauchy-
Schwarz quadratic mutual information. In: Proceedings of IEEE International Conference onRobotics and Automation, pp. 4791–4798 (2015)
7. Charrow, B., Kahn, G., Patil, S., Liu, S., Goldberg, K., Abbeel, P., Michael, N., Kumar, V .:
Information-theoretic planning with trajectory optimization for dense 3D mapping. In: Pro-ceedings of Robotics Science and Systems (2015)
8. Chen, L., Arambel, P.O., Mehra, R.K.: Estimation under unknown correlation: covariance
intersection revisited. IEEE Trans. Autom. Control 47(11), 1879–1882 (2002)
9. Feder, H.J.S., Leonard, J.J., Smith, C.M.: Adaptive mobile robot navigation and mapping. Int.
J. Robot. Res. 18(7), 650–668 (1999)
10. Huang, S., Kwok, N.M., Dissanayake, G., Ha, Q.P., Fang, G.: Multi-step look-ahead trajectory
planning in SLAM: possibility and necessity. In: Proceedings of IEEE International Conferenceon Robotics and Automation, pp. 1091–1096 (2005)
11. Jadidi, M.G., Miró, J.V ., Valencia, R., Andrade-Cetto, J.: Exploration on continuous Gaussian
process frontier maps. In: Proceedings of IEEE International Conference on Robotics and
Automation, pp. 6077–6082 (2014)
12. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J.
Robot. Res. 30(7), 846–894 (2011)
13. Kaess, M., Dellaert, F.: Covariance recovery from a square root information matrix for data
association. Robot. Auton. Syst. 57(12), 1198–1210 (2009)
14. Kaess, M., Johannsson, H., Roberts, R., Ila, V ., Leonard, J.J., Dellaert, F.: iSAM2: Incremental
smoothing and mapping using the Bayes tree. Int. J. Robot. Res. 31(2), 216–235 (2012)
15. LaValle, S.M.: Rapidly-Exploring Random Trees: A New Tool for Path Planning (1998)16. Makarenko, A.A., Williams, S.B., Bourgault, F., Durrant-Whyte, H.F.: An experiment in inte-
grated exploration. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robotsand Systems, pp. 534–539 (2002)
17. Martinez-Cantin, R., de Freitas, N., Doucet, A., Castellanos, J.A.: Active policy learning for
robot planning and exploration under uncertainty. In: Proceedings of Robotics: Science andSystems, pp. 321–328 (2007)
18. Moravec, H.., Elfes, A.: High resolution maps from wide angle sonar. In: Proceedings of IEEE
International Conference on Robotics and Automation, pp. 116–121 (1985)
19. Sim, R., Roy, N.: Global A-optimal robot exploration in slam. In: Proceedings of IEEE Inter-
national Conference on Robotics and Automation, pp. 1050–4729 (2005)
20. Stachniss, C., Grisetti, G., Burgard, W.: Information gain-based exploration using Rao-
Blackwellized particle ﬁlters. In: Proceedings of Robotics: Science and Systems, pp. 65–72(2005)
21. Triggs, B., McLauchlan, P.F., Hartley, R.I., Fitzgibbon, A.W.: Bundle adjustment—a modern
synthesis. In: International Workshop on Vision Algorithms, pp. 298–372 (1999)
22. Valencia, R., Miró, J.V ., Dissanayake, G., Andrade-Cetto, J.: Active pose SLAM. In: Proceed-
ings of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1885–1891
(2012)
23. Vallvé, J., Andrade-Cetto, J.: Active pose SLAM with RRT\*. In: Proceedings of IEEE Inter-
national Conference on Robotics and Automation, pp. 2167–2173 (2015)
Map-Based Localization Under
Adversarial Attacks
Y ulin Yang and Guoquan Huang
1 Introduction and Related Work
It is conceivable that thousands of autonomous vehicles will be operated in a
wide range of civilian and military application domains, such as self-driving cars,unmanned aerial vehicles (UA Vs), and autonomous underwater vehicles (AUVs).
However, current onboard navigation systems for these vehicles are often vulnerable
to malicious attacks—that is, terrorists and criminals may easily hijack vehicles to
attack the public. While the study of secure control has made important advances
over the past few years, the vast majority of this literature focuses on cyber attacks.However, sensor attacks—manipulating physical ﬁelds such as electromagnetic and
pressure which are measured by sensors and/or directly compromising measurements
even if communication is secure (e.g. see [ 1,2])—pose a more menacing threat to
autonomous navigation systems.
In particular, secure state estimation and control in cyber-physical systems has
gained signiﬁcant attention (e.g., [ 3–8]), because it was realized that adversarial
attacks on sensors truly occur in real life. For example, the ﬁrst-time-ever attack
(Stuxnet) on the Supervisory Control And Data Acquisition (SCADA) system was
found in 2010 [ 9], where sensor measurements were replaced by previously recorded
data and fed to the controller, thus leading to possible catastrophic damages; false
data can be injected into smart power grids [ 10]; and an attacker can spoof the GPS
to misguide an $80 million yacht off route [ 11].
To secure state estimation in linear dynamical systems, one can formulate a non-
convex /lscript
0-minimization problem when sensor measurements are either noise-free
[3,4] or being corrupted by noise [ 6], which is then relaxed into a convex /lscriptr//lscript1(sum
Y. Ya n g (B)·G. Huang
Department of Mechanical Engineering, University of Delaware, Newark, DE 19716, USA
e-mail: yuyang@udel.edu
G. Huang
e-mail: ghuang@udel.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_54775
776 Y . Yang and G. Huang
of/lscriptrnorms) problem. In particular, Fawzi et al. [ 4] studied the secure estimation
problem for a noiseless linear time invariant (LTI) system with a ﬁxed set of attacked
sensors which are less than one half of the total number of sensors, but the attack sig-nals can be arbitrary. Pajic et al. [ 12,13] extended [ 4] to noisy system with bounded
noise assumption, and proved that the worst-case estimation error of their algorithms
is linear with the bound of the noise. If there is no (processing) resource constraint, aminimax optimization can be formulated to construct an optimal estimator by min-
imizing the worst-case mean square error against all possible attacked sensors and
all possible sensor noise [ 5,8]. Moreover, in [ 3,14]acomplete set of fault-monitor
ﬁlters are generated to detect the existence of an attack. However, if only an upper
bound on the number of the attacked sensors is available, this method is notscalable
since the number of monitors is combinatorial in the size of the attacked sensors.In [14] observability analysis was also performed for a linear system under attacks,
showing that the system is observable if and only if less than a half of the sensors are
attacked. In robotics, Bezzo et al. [ 15] introduced a secure Kalman ﬁlter (KF) for the
LTI system by inﬂating the covariance of attacked sensors’ measurements. Recently,
Hu et al. [ 16] addressed secure localization for UA Vs by using error correction tech-
niques [ 17] to identify the attack signals based on the sparse attack assumption but
relaxing the assumption of a ﬁxed set of attack sensors and allowing different sets
of sensors to be attacked each time. Additionally, in noise-free cases, Satisﬁability
Modulo Theory (SMT)-based algorithms can also be employed to detect and iso-
late the compromised sensors for both linear dynamical systems [ 7] and nonlinear
differentially ﬂat systems [ 18].
In this paper, we seek to secure state estimation for stochastic nonlinear sys-
tems with the particular application to map-based localization. In particular, based
on the MCC-KF [ 19], we ﬁrst perform in-depth analysis of the maximum corren-
tropy criterion (MCC)-based EKF. Then, we analytically derive the weighted MCC-
EKF (WMCC-EKF) that shows to improve accuracy and robustness to unbounded
attacks as compared to the state-of-the-art methods. Different with [ 20], the proposed
WMCC-EKF is derived for nonlinear measurement model and the weights are deter-
mined partially according to the known noise level. Furthermore, as a conservative
solution, we generalize the secure estimation algorithm [ 16] to nonlinear systems
and develop the Secure Estimation (SE)-EKF that integrates the attack detection
within a sliding-window ﬁltering framework. The proposed secure EKFs are vali-
dated through both Monte-Carlo simulations and experiments on real datasets.
2 Problem Statement
Consider a nonlinear system with measurements possibly attacked by adversaries:
xk+1=f(xk,wk) (1)
yk+1=h(xk+1)+nk+1+ak+1 (2)
zk+1=yk+1−ak+1=h(xk+1)+nk+1 (3)
Map-Based Localization Under Adversarial Attacks 777
where xk∈Rm×1represents the system states at the time step k,frepresents the
system dynamic model and wis the input white Gaussian noise with covariance
Q.y∈Rp×1denotes the measurements from psensors, hrepresents the nonlinear
measurement model function. a∈Rp×1denotes the attack signals and is assumed to
be sparse vector that at least one sensor cannot be attacked. We also deﬁne z∈Rp×1
as the un-attacked output. n∈Rp×1represents zero-mean Gaussian white noises
with covariance R=diag{σ2
1...σ2
i...σ2
p}, where σi,i=1...prepresents the i-
th sensor’s noise variance and diag{·}is the diagonal matrix form. If the Ris a full
(not diagonal or block diagonal) matrix, a noise pre-whitening operation (see [ 21])
can be performed to transform Rinto diagonal form. The corresponding linearized
system can be computed as follows:
˜xk+1/similarequalFk˜xk+Gkwk (4)
˜yk+1/similarequalHk+1˜xk+1+nk+1+ak+1 (5)
˜zk+1/similarequalHk+1˜xk+1+nk+1 (6)
where ˜x=x−ˆxdenotes the error states, the Fkand Gkrepresent the Jacobians
regarding to the state xkand the noise wkrespectively. ˜ydenotes the measurement
residual, while ˜zdescribes the un-attacked measurement residual. Hk+1represents
the measurement Jacobian with respect to the state xk+1.
2.1 Map-Based Localization with Malicious Attacks
While this paper particularly focuses on 2D map-based localization as an example to
illustrate the key ideas of our proposed secure estimators, the methodology is general
and readily applicable to other systems. Speciﬁcally, in map-based localization, the
dynamic motion model of the robot pose is given by:
˙x=/bracketleftbigg˙pR
˙φ/bracketrightbigg
=⎡
⎣˙x
˙y
˙φ⎤
⎦=⎡
⎣vcos(φ)
vsin(φ)
ω⎤
⎦=⎡
⎣cos(φ)
sin(φ)
0⎤
⎦v+⎡
⎣0
0
1⎤
⎦ω (7)
where vis the linear velocity and ωis the angular velocity of the robot. pRandφ
denote the position and orientation of the robot, respectively. Note that we assume
a more challenging localization scenario than [ 13,16] that the robot does not have
access to GPS signals. Instead, only the relative range and bearing measurements of
the features are available for localization, and the measurements can be described
as:
h(x)=/bracketleftbiggh(r)(x)
h(b)(x)/bracketrightbigg
+a=/bracketleftBigg/radicalbig
spf/latticetopspf
arctan/parenleftBigsyf
sxf/parenrightBig/bracketrightBigg
+a (8)
778 Y . Yang and G. Huang
where h(r)and h(b)represent the range and bearing measurements respectively. Given
the rotation matrix C(φ)between global and sensor frames,spf=[sxfsyf]/latticetop=
C(φ)(pf−pR)represents the map feature in the sensor frame of reference.
It is important to note that, instead of assuming a ﬁxed set of attacked sensors [ 4,
12], we consider that the attacker can attack different sensors randomly at different
time steps [see ( 54)]. Note also that as compared to [ 15,16], instead of assuming
that less than a half of the sensors can be attacked, we only assume that at least
one bearing or range sensor is not attacked. Moreover, attack signals can even go
unbounded—that is, some of the sensor attacks ai(i∈{1...p})might go unbounded,
i.e.,/bardblai/bardbl→∞ .
3 Maximum Correntropy Criterion (MCC)-Based Filters
In this section, we present in detail our secure ﬁlters based on the maximum corren-
tropy criterion. The correntropy can be deﬁned as a statistical metric of similarity
between two random variables [ 19], and one can pose a cost function Jmfor robust
ﬁlters based on the correntropy with Gaussian kernels as follows:
Jm(xk+1)=Gσ/parenleftBig
/bardblyk+1−h(xk+1)/bardblR−1
k+1/parenrightBig
+Gσ/parenleftBig
/bardblxk+1−f(xk,0)/bardblP−1
k+1|k/parenrightBig
(9)
where Gσis the Gaussian kernel in the form of Gσ(/bardblxi−yi/bardbl)=exp(−/bardblxi−yi/bardbl2
2σ2)
withσas bandwidth, Pk+1|kis the propagated covariance [see ( 11)]. Minimization
of the cost function ( 9) can lead to the derivation of correntropy based ﬁlters [ 19].
Correntropy based ﬁlter is proved to be robust when having large disturbances or
outliers and can work well with non-Gaussian noise.
3.1 MCC-EKF
Based on [ 19,22], we analytically derive the MCC-EKF for the case of nonlinear
systems such as map-based localization. In particular, given the initial state in the
form of Gaussian distribution, N(ˆx0|0,P0), state estimate and covariance propaga-
tion based on the motion model ( 1) from time step ktok+1i s :
ˆxk+1|k=f(ˆxk|k,0) (10)
Pk+1|k=FkPk|kF/latticetop
k+GkQkG/latticetop
k (11)
Then, EKF-like update based on the measurement model ( 2) can be written as:
ˆyk+1|k=ˆzk+1|k=h(ˆxk+1|k) (12)
Map-Based Localization Under Adversarial Attacks 779
dk+1=Gσ/parenleftBig/vextenddouble/vextenddoubleyk+1−h(ˆxk+1|k)/vextenddouble/vextenddouble
R−1
k+1/parenrightBig
Gσ/parenleftBig/vextenddouble/vextenddoubleˆxk+1|k−f(ˆxk|k,0)/vextenddouble/vextenddouble
P−1
k+1|k/parenrightBig (13)
Kk+1|k=/parenleftBig
P−1
k+1|k+H/latticetop
k+1(dk+1R−1
k+1)Hk+1/parenrightBig−1
H/latticetop
k+1(dk+1R−1
k+1) (14)
=Pk+1|kH/latticetop
k+1/parenleftbig
Hk+1Pk+1|kH/latticetop
k+1+d−1
k+1Rk+1/parenrightbig−1(15)
ˆxk+1|k+1=ˆxk+1|k+Kk+1|k(yk+1−ˆyk+1|k) (16)
Pk+1|k+1=/parenleftBig
P−1
k+1|k+H/latticetop
k+1(dk+1R−1
k+1)Hk+1/parenrightBig−1
(17)
where dk+1is a ratio scalar computed from Gaussian kernel. Based on these deriva-
tions, the detailed MCC-EKF algorithm can be found in the companion technical
report [ 21]. With an in-depth inspection of the MCC-EKF, the updated covariance
(17) can also be written as:
Pk+1|k+1=Pk+1|k−Pk+1|kH/latticetop
k+1S−1
k+1|kHk+1Pk+1|k (18)
with the innovation covariance Sk+1|kdeﬁned as:
Sk+1|k=Hk+1Pk+1|kH/latticetop
k+1/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
S1+d−1
k+1Rk+1/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
S2(19)
where S1and S2denote the covariance contribution from the motion ( 1) and mea-
surement ( 2), respectively. Note that the MCC-EKF can be viewed as using the scalar
dk+1to control the covariance inﬂation from the attacked measurements. As shown in
(13),dk+1decreases if system has been attacked, and the covariance contribution S2
will be increased [see( 19)], implying that the measurement becomes more uncertain.
As a result, Sk+1|kand thus the updated state covariance Pk+1|k+1, will be inﬂated
due to ( 18). Lemma 1summarizes our analysis:
Lemma 1 For the MCC-EKF , if the attack ak+1goes unbounded, the ﬁlter will not
perform measurement update.
Proof If the attack goes unbounded, that is /bardblak+1/bardbl→∞ , then /bardblyk+1−
h(ˆxk+1|k)/bardblR−1
k+1→∞ , and hence dk→0. According to ( 14) and ( 16),Kk+1→ 0
andˆxk+1|k+1→ˆxk+1|k. Finally, with ( 17),Pk+1|k+1→ Pk+1|k.
This result essentially shows that the scalar dk+1will dismiss allthe observation
updates even if only one measurement is attacked at time step k+1, which clearly
is too conservative. In order to enable the MCC-EKF to utilize the information
contained in un-attacked measurements, we propose the weighted MCC-EKF derived
from multiple Gaussian kernels.
780 Y . Yang and G. Huang
3.2 W eighted MCC-EKF
Compared to ( 9), we deﬁne the cost function for the maximum correntropy criterion
with multiple Gaussian kernels as:
J(xk+1)=p/summationdisplay
i=1Gˆσi,k+1/parenleftbig/vextenddouble/vextenddoubleyi,k+1−hi,k+1(xk+1)/vextenddouble/vextenddouble/parenrightbig
+Gˆσ0,k+1/parenleftBig/vextenddouble/vextenddoublexk+1−f(ˆxk|k,0)/vextenddouble/vextenddouble
P−1
k+1|k/parenrightBig
(20)
where we have deﬁned the Gaussian kernel Gˆσi,k+1and Gˆσ0,k+1according to [ 19]:
Gˆσi,k+1/parenleftbig/vextenddouble/vextenddoubleyi,k+1−hi,k+1(xk+1)/vextenddouble/vextenddouble/parenrightbig
=exp/parenleftBigg
−/vextenddouble/vextenddoubleyi,k+1−hi,k+1(xk+1)/vextenddouble/vextenddouble2
2ˆσ2
i,k+1/parenrightBigg
(21)
Gˆσ0,k+1/parenleftBig/vextenddouble/vextenddoublexk+1−f(ˆxk|k,0)/vextenddouble/vextenddouble
P−1
k+1|k/parenrightBig
=exp⎛
⎝−/vextenddouble/vextenddoublexk+1−f(ˆxk|k,0)/vextenddouble/vextenddouble2
P−1
k+1|k
2ˆσ2
0,k+1⎞
⎠(22)
where ˆσi,k+1,i=1...pdenotes the Gaussian kernel bandwidth of the i-th measure-
ment at time step k+1, and ˆσ0,k+1denotes the Gaussian kernel bandwidth of the
motion model. yi,k+1and hi,k+1(xk+1)represents the i-th row of yk+1and hk+1.A i m -
ing to meet the maximum correntropy criterion, we linearize and take the derivativesof the cost function J(x
k+1)as:
∂J(xk+1)
∂˜xk+1/similarequal−1
2p/summationdisplay
i=1Gˆσi,k+1
ˆσ2
i,k+1∂/parenleftBig/vextenddouble/vextenddouble˜yi,k+1−Hi,k+1˜xk+1/vextenddouble/vextenddouble2/parenrightBig
∂˜xk+1−1
2Gˆσ0,k+1
ˆσ2
0,k+1∂/parenleftbigg/vextenddouble/vextenddouble˜xk+1/vextenddouble/vextenddouble2
P−1
k+1|k/parenrightbigg
∂˜xk+1=0
(23)
where Hi,k+1,i=1...p, represents each row of the Jacobian Hk+1=∂h
∂xk+1/vextendsingle/vextendsingle
xk+1=ˆxk+1
and˜xk+1=xk+1|k−f(ˆxk|k,0)=xk+1−ˆxk+1|k. Then we can arrive at:
p/summationdisplay
i=1Gˆσi,k+1
Gˆσ0,k+1H/latticetop
i,k+1Hi,k+1
ˆσ2
i,k+1
ˆσ2
0,k+1˜xk+1−p/summationdisplay
i=1Gˆσi,k+1
Gˆσ0,k+1H/latticetop
i,k+1
ˆσ2
i,k+1
ˆσ2
0,k+1˜yi,k+1+P−1
k+1|k˜xk+1=0(24)
⇒⎡
⎢⎣p/summationdisplay
i=1Gˆσi,k+1
Gˆσ0,k+1H/latticetop
i,k+1Hi,k+1
ˆσ2
i,k+1
ˆσ2
0,k+1+P−1
k+1|k⎤
⎥⎦˜xk+1=p/summationdisplay
i=1Gˆσi,k+1
Gˆσ0,k+1H/latticetop
i,k+1
ˆσ2
i,k+1
ˆσ2
0,k+1˜yi,k+1(25)
Then ( 25) can be written in matrix form as:
/bracketleftBig
H/latticetop
k+1Dk+1ˆR−1
k+1Hk+1+P−1
k+1|k/bracketrightBig
˜xk+1=H/latticetop
k+1Dk+1ˆR−1
k+1˜yk+1 (26)
where we have deﬁned di,k+1,Dk+1andˆRk+1as:
Map-Based Localization Under Adversarial Attacks 781
di,k+1=Gˆσi,k+1/parenleftbig/vextenddouble/vextenddoubleyi,k+1−hi,k+1(xk+1)/vextenddouble/vextenddouble/parenrightbig
Gˆσ0,k+1/parenleftBig/vextenddouble/vextenddoublexk+1−f(ˆxk|k,0)/vextenddouble/vextenddouble
P−1
k+1|k/parenrightBig (27)
Dk+1=diag{d1,k+1,..., di,k+1,..., dp,k+1} (28)
ˆRk+1=diag{ˆσ2
1,k+1
ˆσ2
0,k+1,...,ˆσ2
i,k+1
ˆσ2
0,k+1,...,ˆσ2
p,k+1
ˆσ2
0,k+1} (29)
Hence, the new state and covariance update can be expressed as:
ˆxk+1|k+1=ˆxk+1|k+/bracketleftBig
H/latticetop
k+1Dk+1ˆR−1
k+1Hk+1+P−1
k+1|k/bracketrightBig−1
H/latticetop
k+1Dk+1ˆR−1
k+1/parenleftbig
yk+1−ˆyk+1|k/parenrightbig
(30)
Pk+1|k+1=/bracketleftBig
H/latticetop
k+1Dk+1ˆR−1
k+1Hk+1+P−1
k+1|k/bracketrightBig−1(31)
Up to this step, we have the new state update as ( 30), which is highly similar to ( 16).
Now comes how to choose appropriate bandwidths. We ﬁxed the ratio ofˆσ2
i
ˆσ2
0asσ2
i,
where σidenotes the standard deviation of the i-th measurement obtained from noise
covariance Rk+1. Therefore, ˆRk+1=Rk+1, and Dk+1can just be seen as a weight
matrix for the measurement noise. During the implementation of the WMCC-EKF
[21], we choose σ2
i=λσˆσ2
i, with λσ∈(0.125,0.5)which are shown to work well
in our simulation and experiments. Upon this choice, the state and covariance updateof the proposed WMCC-EKF can be ﬁnally described as:
K
k+1|k=/bracketleftBig
H/latticetop
k+1Dk+1R−1
k+1Hk+1+P−1
k+1|k/bracketrightBig−1
H/latticetop
k+1Dk+1R−1
k+1 (32)
=Pk+1|kH/latticetop
k+1/parenleftbig
Hk+1Pk+1|kH/latticetop
k+1+Rk+1D−1
k+1/parenrightbig−1(33)
ˆxk+1|k+1=ˆxk+1|k+Kk+1|k/parenleftbig
yk+1−ˆyk+1|k/parenrightbig
(34)
Pk+1|k+1=/bracketleftBig
H/latticetop
k+1Dk+1R−1
k+1Hk+1+P−1
k+1|k/bracketrightBig−1
(35)
Now we will inspect WMCC-EKF from an information perspective. Compared
to the MCC-EKF, the information matrix for the WMCC-EKF can be written as:
P−1
k+1|k+1=P−1
k+1|k+H/latticetop
k+1(Dk+1R−1
k+1)Hk+1=P−1
k+1|k/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
Σw1+p/summationdisplay
i=1di,k+1H/latticetop
i,k+1Hi,k+1
σ2
i,k+1/bracehtipupleft /bracehtipdownright/bracehtipdownleft /bracehtipupright
Σw2
(36)
where Σw1andΣw2denote the information from motion model ( 1) and the measure-
ment model ( 2), respectively. Note that di,k+1H/latticetop
i,k+1Hi,k+1
σ2
i,k+1represents the information
contribution from the i-th sensor’s measurement, and thus, Σw2in (36) can be seen
as the sum of single information matrix from all the psensors. If the i-th sensor is
782 Y . Yang and G. Huang
attacked, di,k+1will decrease exponentially and the corresponding information con-
tribution di,k+1H/latticetop
i,k+1Hi,k+1
σ2
i,k+1will be dramatically reduced. However, this process will
not affect the information contribution from other sensors. Therefore, different from
the MCC-EKF, the WMCC-EKF is able to utilize the information from un-attackedsensor measurements.
3.3 Convergence Analysis Under Unbounded Attacks
Inspired by [ 15], to further understand the proposed WMCC-EKF, we perform the
convergence analysis when the system is suffering from unbounded attacks. Weﬁrst deﬁne ¯x
k+1as the state estimate with un-attacked measurement zk+1, and the
predicted measurement based on ¯xk+1can be denoted as:
¯zk+1=h(¯xk+1) (37)
Hence, with ( 2) and ( 3), the update Eq. ( 34) can be rewritten as:
ˆxk+1|k+1=ˆxk+1|k+Kk+1|k(zk+1−¯zk+1+h(¯xk+1)−h(ˆxk+1|k)+ak+1)(38)
=ˆxk+1|k+Kk+1|k(zk+1−¯zk+1)+Kk+1|ksk+1 (39)
where sk+1=h(¯xk+1)−h(ˆxk+1|k)+ak+1describes the difference of measurement
estimates from un-attacked and attacked measurements. Since sk+1also includes the
attack vector ak+1,t h et e r m Kk+1|ksk+1can be seen as Attack Innovation . We would
like to shrink this term, so that the attacked estimate ˆxk+1|k+1will approach the ideal
estimate ¯xk+1as close as possible. Interestingly, the WMCC-EKF can constrain the
attack innovation to a small bound even under unbounded attacks.
Lemma 2 Given an unbounded attack ak+1and an arbitrarily small positive con-
stant value ξ, there exists a correntropy weight matrix Dk+1for the WMCC-EKF
such that:
Pr/parenleftBig/vextenddouble/vextenddoubleKk+1|ksk+1/vextenddouble/vextenddouble2≤ξ/parenrightBig
>99.7% (40)
Proof From ( 33), we can write attack innovation Kk+1|ksk+1as:
/vextenddouble/vextenddoubleKk+1|ksk+1/vextenddouble/vextenddouble2=/vextenddouble/vextenddoublePk+1|kH/latticetop
k+1/vextenddouble/vextenddouble2/bardblτ/bardbl2(41)
where we deﬁne τ=/parenleftbig
Hk+1Pk+1|kH/latticetop
k+1+D−1
k+1Rk+1/parenrightbig−1sk+1. We can observe that in
oder to show bounded attack innovation, we only need to show that /bardblτ/bardblis bounded.
We consider the worst case and compute the boundary for /bardblτ/bardblas:
Map-Based Localization Under Adversarial Attacks 783
/bardblτ/bardbl2≤/vextenddouble/vextenddouble/vextenddouble/parenleftbig
σ2
minI+D−1
k+1Rk+1/parenrightbig−1sk+1/vextenddouble/vextenddouble/vextenddouble2
=p/summationdisplay
j=1/parenleftBigg
sj
σ2
min+d−1
jσ2
j/parenrightBigg2
(42)
We deﬁne the ideal estimate residual as ˜zj,k+1=zj,k+1−ˆzj,k+1, and ˜zj,k+1∼
N(0,¯σ2
j,k+1). Based on Gaussian distribution, we have:
Pr/parenleftbig/vextenddouble/vextenddouble˜zj,k+1/vextenddouble/vextenddouble≤3¯σj,k+1/parenrightbig
=99.7% (43)
Equation ( 43) indicates that/vextenddouble/vextenddouble˜zj,k+1/vextenddouble/vextenddoubleis almost bounded by 3 ¯σj,k+1.I ft h e j-th sensor
attack ajgoes unbounded,/vextenddouble/vextenddoublesj/vextenddouble/vextenddouble→∞ and hence/vextenddouble/vextenddoublesj/vextenddouble/vextenddouble>3¯σj. Then, we drop the
timestamps for simplicity and arrive at:
/bracketleftBigg
sj
σ2
min+d−1
jσ2
j/bracketrightBigg2
≤⎡
⎢⎣sj
σ2
min+exp/parenleftBig(/bardblsj/bardbl−/bardbl˜zj/bardbl)2
2ˆσ2
j/parenrightBig
σ2
j⎤
⎥⎦2
<ˆσ2
j
σ4
jζ2
/bracketleftbig
exp/parenleftbig1
2(ζ−μ)2/parenrightbig/bracketrightbig2
(44)
where ζ=/bardblsj/bardbl
ˆσj, andμ=3¯σj
ˆσj. Obviously, as/vextenddouble/vextenddoublesj/vextenddouble/vextenddouble→∞ ,ζ→∞ , and the right side
of (44) will ﬁnally approach 0. Besides, if we take derivative of the right side of ( 44)
regarding to ζ, we can have the maximum value of ( 44) when ζ/prime=μ+√
μ2+4
2, that
is:/bracketleftBigg
sj
σ2
min+d−1
jσ2
j/bracketrightBigg2
≤ˆσ2
j
σ4
jζ/prime2
/bracketleftbig
exp/parenleftbig1
2(ζ/prime−μ)2/parenrightbig/bracketrightbig2(45)
Since ζ/primeis independent of the attack innovation sj, thus we can bound ( 45)b y
appropriate design of bandwidth ˆσj. According to ( 42) and ( 43),/bardblτ/bardbl2is the summa-
tion of ( 45) and is bounded by the design of Dk+1with probability 99 .7%. In ( 41),/vextenddouble/vextenddoublePk+1|kH/latticetop
k+1/vextenddouble/vextenddouble2is independent from the ak+1, and thus it is bounded. Therefore, we
can easily ﬁnd a ξthat satisﬁes ( 40).
4 Secure Estimation (SE)-EKF
Ideally, we would like to identify the attacked measurements so that we can ensure
estimation security by excluding them from the EKF update. To this end, we introduce
the Secure-estimation (SE)-EKF by generalizing the SE-KF [ 16,23] to the nonlin-
ear system under consideration. In particular, in order to detect sensor attacks, we
adopt the sliding-window strategy. Speciﬁcally, we construct a ﬁxed-sized window
within EKF framework by stochastic cloning [ 24]. All the accumulated measure-
ments within the window are used for update at certain time step. After update, the
window will be cleared and start to accumulate new measurements again. We deﬁne
the state vector with window size Nat time step kas:
784 Y . Yang and G. Huang
xck=/bracketleftbig
x/latticetop
kx/latticetop
k−1··· x/latticetop
k−N+1x/latticetop
k−N/bracketrightbig/latticetop(46)
where xkrepresents the current robot state, xk−irepresents the cloned robot state at
time step k−i,i∈{1...N}. Thus, xk−Nis the oldest cloned state. Similar to SE in
[16], after we have cloned Nrobot states in the state vector and accumulated their
measurements, we can linearize and stack all the measurements together as:
⎡
⎢⎢⎢⎣˜z
k
˜zk−1
...
˜zk−N⎤
⎥⎥⎥⎦/similarequal⎡
⎢⎢⎢⎣H
k
Hk−1
...
Hk−N⎤
⎥⎥⎥⎦˜x
ck+⎡
⎢⎢⎢⎣n
k
nk−1
...
nk−N⎤
⎥⎥⎥⎦+⎡
⎢⎢⎢⎣a
k
ak−1
...
ak−N⎤
⎥⎥⎥⎦(47)
According to the linearized motion model ( 4), within the sliding-window, we have
˜x
k=Fk−1···Fk−N˜xk−N=Fk−1,k−N˜xk−N (48)
where Fk−1,k−N=Fk−1···Fk−Nrepresents the state transition matrix from cloned
state˜xk−Nto the current robot state ˜xk.T h u s ,( 47) can be written as:
⎡
⎢⎢⎢⎣˜z
k
˜zk−1
...
˜zk−N⎤
⎥⎥⎥⎦
/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
˜Z/similarequal⎡
⎢⎢⎢⎣H
0
Hk−1
...
Hk−N⎤
⎥⎥⎥⎦⎡
⎢⎢⎢⎣F
k,k−N
Fk−1,k−N
...
I⎤
⎥⎥⎥⎦
/bracehtipupleft/bracehtipdownright/bracehtipdownleft /bracehtipupright
Φ˜xk−N+⎡
⎢⎢⎢⎣n
0
nk−1
...
nk−N⎤
⎥⎥⎥⎦+⎡
⎢⎢⎢⎣a
0
ak−1
...
ak−N⎤
⎥⎥⎥⎦
/bracehtipupleft/bracehtipdownright/bracehtipdownleft /bracehtipupright
E(49)
where ˜Zrepresents the stacked measurement residuals, and Edenotes the sum of
stacked noise and attack vectors, Φdenotes the stacked state transition matrix from
˜xk−Nto each state in the window. Similar to [ 16,23] we apply left null space operation
toΦto simplify ( 49). Let Unbe the left null space of Φ, that is U/latticetop
nΦ=0, then we
can have:
Zo=U/latticetop
nZ=U/latticetop
nE (50)
where Uncan be computed from the QRdecomposition of Φas:
Φ=UeRΔ=/bracketleftbigUeUn/bracketrightbig/bracketleftbiggRΔ
0/bracketrightbigg
(51)
Given the strong sparse attack assumption that less than a half of the all the sensors
can be attacked, Ecan be solved by formulating the following optimization problem
with/lscript1norm regularization [ 25]a s :
ˆE=arg min
E/bracketleftBig/vextenddouble/vextenddoubleZo−U/latticetop
nE/vextenddouble/vextenddouble2
2+λ/bardblE/bardbl/lscript1/bracketrightBig
(52)
where λis the regularization parameter.
Map-Based Localization Under Adversarial Attacks 785
Different from [ 16], we here consider a nonlinear model and thus, the sparsity
ofEwill be contaminated by linearization errors and noises. Therefore, the /lscript1-
optimization solution ˆEfrom ( 52) will not be perfectly sparse. In order to minimize
this side effect, we propose to set a threshold tforˆEto enforce the sparsity. Let ei
denotes the i-th element in E, and if ei0 produces
a higher success rate. But, collisions occur since the collision probability of feasible
paths can be non-zero. Note that Pthresho ld is the threshold of collision probability of
nodes, not the entire trajectory.
No tested method achieves a 100% success rate in presence of obstacle motion
uncertainty. This includes state of the art methods (DRT and SES) designed to work in
such environments and ST-A\* FSR that employs exact obstacle prediction. Therefore,
obstacle motion uncertainty remains a difﬁcult challenge and an open problem for
motion planning.
804 H.-T. L. Chiang et al.
Ta b l e 2 Performance of ST-A\* FSR with various collision probability thresholds Pthresho ld
Pthresho ld 0% 1% 5% 10%
Lanes Success rate (%) 49±13 96±4 99±1 98±2
Finish time (m) 30.3 ±12.6 21.6 ±7.3 18.5 ±4.9 17.0 ±4.3
Random Success rate (%) 62±12 95±5 94±6 96±4
Finish time 45.3 ±7.3 35.6 ±4.3 33.4 ±2.9 32.9 ±2.9
4.6 Impact of Number of Moving Obstacles
This section investigates the impact of increasing number of obstacles, with and
without increasing obstacle density. Since the number of obstacles directly impacts
computation load, we also compare the computation time per planning step of plan-
ning algorithms. To vary the obstacle density, we vary the number of obstacles from50 to 150 in Random and the number of obstacles per lane from 2 to 7 in Lanes. A
snapshot of 7 obstacles per lane and 150 obstacles in Lanes and Random are shown
in Figs. 1a and 1b, respectively. We also investigated the effect of varying the number
of obstacles, without increasing obstacle density, in Lanes. This is done by varying
the number of lanes from 2 to 10 without increasing the number of obstacles per
lane. All other settings are identical to Sect. 4.3.
Figure 5a, c show that increasing obstacle density decreases success rate for all
methods except for ST-A\* and DRT. This is expected as obstacle density greatlydecreases the X
C
ICSratio. For example, in the environment in which 34% of the area
is occupied by obstacles, roughly 20% of the states are in XC
ICS. As discussed in
Sect. 4.4, this poses a signiﬁcant challenge for planning algorithms.
Increasing the number of obstacles while keeping the density constant decreases
the success rate of all methods except for ST-A\* and DRT, as shown in Fig. 6a. How-
ever, this is not due to decrease of XC
ICSratio, since the XC
ICSratio (magenta solid
line in Fig. 6a) remains roughly the same. This is likely due to the robot needing
to traverse through more regions of possible collision (more lanes), thus increas-
ing the probability of collision along the trajectory. The decrease in success ratesalso indicates that the difﬁculty of planning in dynamic environments is very hard
to quantify. We utilized X
C
ICS ratio to explain the performance of many planning
algorithms. However, XC
ICSratio does not fully quantify the difﬁculty of planning in
dynamic environments as it failed to explain the success rate decrease as the number
of lanes increase.
Comparing Fig. 5a, c, we can see the density of Random is lower than that of
the Lanes. However, even though the obstacles have the same (relative) speed, it
can be seen that success rate of most methods in the highest density in Random(13% occupied) is lower than the a similar density in Lanes (13% occupied). This
indicates that the structure of the environment may impact the success rate of planning
algorithms. For example, the robot can safely stay outside the lanes to wait until anopening occurs. Such safe zones do not exist in Random.
Safety, Challenges, and Performance of Motion Planners … 805
(a) (b)
(c) (d)
Fig. 5 Success rate and computation time per planning step with varying obstacle density in Lanes
(a,b) and Random ( c,d). The black dotted line is the real-time limit (200 ms)
Computation time per planning step is typically sensitive to the number of obsta-
cles. Figures 5b, d and 6b show the computation time per online planning step
has three groupings. First, reactive methods (APF methods, VO, and APFSR) are
extremely fast, in the order of 10 µs in the 150 obstacle environment. Since these
methods return an action every time step (10 ms), they are real-time capable. Next,
the computation time of state-time sampling-based methods (D. Window, ST-RRT,
PMP , SES and DRT) ranges from 0.1 ms to 35.2 ms, while returning actions every
200 ms. Since 200 ms is generally considered as the requirement for real-time plan-
ning algorithms [ 23], these methods are also real-time capable. Lastly, Dynamic
RRT, A\* and ST-A\* are no longer real-time capable in these environments with
more than 100 obstacles. In particular, ST-A\* is not real-time capable except for the
environment with less than 12 obstacles. This is due to ST-A\* explicitly discretizing
806 H.-T. L. Chiang et al.
(a) (b)
Fig. 6 Success rate and computation time per planning step as a function of number of lanes in
Lanes. Note that the obstacle density remains constant but the number of obstacles increases with
number of lanes. The black dotted line is the real-time limit (200 ms)
state and time, resulting in a graph with O(1/Δ3)number of nodes (in contrast, A\*
has O(1/Δ2)number of nodes), where Δis the temporal discretization size.
5 Conclusion
In this paper, we classify, implement and compare thirteen planning algorithms for
dynamic environments in two tunable environments. Using success rates, ﬁnish times,
and computation times per planning step, we identiﬁed and analyzed the impact of
environmental challenges on these algorithms. These challenges include max speed
ratio (obstacle speed/robot max speed), obstacle motion uncertainty, and number
of obstacles. We show that planning in dynamic environments remains unsolved as
state of the art planning algorithms failed to consistently identify collision-free paths
even in simple geometric planning problems. Our detailed analysis demonstrates an
initial attempt at obtaining insights about the speciﬁc challenges of motion planning
algorithms, and we hope may motivate new solutions to this challenging problem.
Acknowledgements This material is based upon work supported by the National Science Founda-
tion (NSF) under Grant Numbers IIS-1528047 and IIS-1553266 (Tapia, CAREER). Any opinions,
ﬁndings, and conclusions or recommendations expressed in this material are those of the authors
and do not necessarily reﬂect the views of the NSF.
Safety, Challenges, and Performance of Motion Planners … 807
References
1. Abate, A., Prandini, M., Lygeros, J., Sastry, S.: Probabilistic reachability and safety for con-
trolled discrete time stochastic hybrid systems. Automatica 44(11), 2724–2734 (2008)
2. Bacha, A., Bauman, C., Faruque, R., Fleming, M., Terwelp, C., Reinholtz, C., Hong, D.,
Wicks, A., Alberi, T., Anderson, D., et al.: Odin: team victortango’s entry in the DARPA urbanchallenge. J. Field Robot. 25(8), 467–492 (2008)
3. Benenson, R., Petti, S., Fraichard, T., Parent, M.: Integrating perception and planning for
autonomous navigation of urban vehicles. In: Proceedings of IEEE International Conference
on Intelligent Robots System (IROS), pp. 98–104 (2006)
4. Canny, J.: Some algebraic and geometric computations in pspace. In: Proceedings of the Twen-
tieth Annual ACM Symposium on Theory of Computing, pp. 460–467 (1988)
5. Canny, J., Reif, J.: New lower bound techniques for robot motion planning problems. In: 28th
Annual Symposium on Foundations of Computer Science, pp. 49–60 (1987)
6. Chiang, H.-T., HomChaudhuri, B., Vinod, A.P ., Oishi, M., Tapia, L.: Dynamic risk tolerance:
motion planning by balancing short-term and long-term stochastic dynamic predictions. In:Proceedings IEEE International Conference Robotics Automation (ICRA), pp. 3762–3769(2017)
7. Chiang, H.-T., Malone, N., Lesser, K., Oishi, M., Tapia, L.: Aggressive moving obstacle avoid-
ance using a stochastic reachable set based potential ﬁeld. In: Proceedings International Work-shop on Algorithmic Foundations of Robotics (W AFR), pp. 73–89 (2014)
8. Chiang, H.-T., Malone, N., Lesser, K., Oishi, M., Tapia, L.: Path-guided artiﬁcial potential
ﬁelds with stochastic reachable sets for motion planning in highly dynamic environments.
In: Proceedings IEEE International Conference Robotics Automation (ICRA), pp. 2347–2354
(2015)
9. Chiang, H.-T., Rackley, N., Tapia, L.: Stochastic ensemble simulation motion planning in
stochastic dynamic environments. In: Proceedings IEEE International Conference on IntelligentRobots and Systems (IROS), pp. 3836–3843 (2015)
10. Ferguson, D., Kalra, N., Stentz, A.: Replanning with RRTs. In: Proceedings IEEE International
Conference on Robotics and Automation (ICRA), pp. 1243–1248 (2006)
11. Fiorini, P ., Shiller, Z.: Motion planning in dynamic environments using velocity obstacles. Int.
J. Robot. Res. 17(7), 760–772 (1998)
12. Fraichard, T.: Trajectory planning in a dynamic workspace: a state-time space approach. Adv.
Robot. 13(1), 75–94 (1998)
13. Fraichard, T., Asama, H.: Inevitable collision states-a step towards safer robots? Adv. Robot.
18(10), 1001–1024 (2004)
14. Fujimura, K.: Motion Planning in Dynamic Environments. Springer (1991)
15. Ge, S.S., Cui, Y .J.: Dynamic motion planning for mobile robots using potential ﬁeld method.
Auto. Robot. 13(3), 207–222 (2002)
16. Goerzen, C., Kong, Z., Mettler, B.: A survey of motion planning algorithms from the perspective
of autonomous UA V guidance. J. Intell. Robot. Syst. 57(1), 65–100 (2010)
17. Kamil, F., Tang, S., Khaksar, W., Zulkiﬂi, N., Ahmad, S.: A review on motion planning and
obstacle avoidance approaches in dynamic environments. Adv. Robot. Autom. 4(2), 134–142
(2015)
18. Khatib, O.: Real-time obstacle avoidance for manipulators and mobile robots. In: Proceedings
IEEE International Conference Robotics Automation (ICRA), pp. 500–505 (1985)
19. Lam, C.-P ., Chou, C.-T., Chiang, K.-H., Fu, L.-C.: Human-centered robot navigation towards a
harmoniously human-robot coexisting environment. IEEE Trans. Robot. 27(1), 99–112 (2011)
20. LaV alle, S.M., Sharma, R.: A framework for motion planning in stochastic environments: mod-
eling and analysis. In: Proceedings IEEE International Conference on Robotics and Automation(ICRA), pp. 3057–3062 (1995)
21. Malone, N., Chiang, H.-T., Lesser, K., Oishi, M., Tapia, L.: Hybrid dynamic moving obstacle
avoidance using a stochastic reachable set-based potential ﬁeld. IEEE Transactions on Robotics,pp. 1124–1138 (2017)
808 H.-T. L. Chiang et al.
22. Miura, J., Shirai, Y .: Modeling motion uncertainty of moving obstacles for robot motion plan-
ning. In: Proceedings IEEE International Conference Robotics Automation (ICRA), pp. 2258–2263 (2000)
23. Paden, B., ˇCáp, M., Y ong, S.Z., Yershov, D., Frazzoli, E.: A survey of motion planning and
control techniques for self-driving urban vehicles. IEEE Trans. Intell. V eh. 1(1), 33–55 (2016)
24. Seder, M., Petrovic, I.: Dynamic window based approach to mobile robot motion control in
the presence of moving obstacles. In: Proceedings IEEE International Conference on Roboticsand Automation (ICRA), pp. 1986–1991 (2007)
25. Short, A., Pan, Z., Larkin, N., van Duin, S.: Recent progress on sampling based dynamic motion
planning algorithms. In: Proceedings IEEE/ASME International Conference on AdvancedIntelligent Mechatronics (AIM), pp. 1305–1311 (2016)
26. Trautman, P ., Krause, A.: Unfreezing the robot: Navigation in dense, interacting crowds. In:
Proceedings IEEE International Conference on Intelligent Robots and Systems (IROS), pp.797–803 (2010)
27. Triebel, R., Arras, K., Alami, R., Beyer, L., Breuers, S., Chatila, R., Chetouani, M., Cremers,
D., Evers, V ., Fiore, M., et al.: Spencer: a socially aware service robot for passenger guidanceand help in busy airports. In: Field and Service Robotics, pp. 607–622 (2016)
28. Tzafestas, S.: Research on autonomous robotic wheelchairs in europe. Robot. Automat. Mag.
8(1), 4–6 (2001)
29. Urmson, C., Anhalt, J., Bagnell, D., Baker, C., Bittner, R., Clark, M., Dolan, J., Duggins, D.,
Galatali, T., Geyer, C., et al.: Autonomous driving in urban environments: boss and the urbanchallenge. J. Field Robot. 25(8), 425–466 (2008)
30. van den Berg, J., Guy, S.J., Lin, M.C., Manocha, D.: Reciprocal n-body collision avoidance.
Int. J. Robot. Res. pp. 3–19 (2009)
31. van den Berg, J., Guy, J.S., Snape, J., Lin, M.C., Manocha, D.: RVO2 library: collision avoidance
for real-time multi-agent simulation. http://gamma.cs.unc.edu/RVO2/
32. V annoy, J., Xiao, J.: Real-time adaptive motion planning (RAMP) of mobile manipulators in
dynamic environments with unforeseen changes. IEEE Trans. Robot. 24(5), 1199–1212 (2008)
33. V atcha, R., Xiao, J.: Detection of robustly collision-free trajectories in unpredictable environ-
ments in real-time. Auto. Robot. 37(1), 81–96 (2014)
34. Wu, A., How, J.P .: Guaranteed inﬁnite horizon avoidance of unpredictable, dynamically con-
strained obstacles. Auto. Robot. 32(3), 227–242 (2012)
Global Inverse Kinematics via
Mixed-Integer Convex Optimization
Hongkai Dai, Gregory Izatt and Russ Tedrake
1 Introduction
The inverse kinematics (IK) problem is one of the most fundamental problems in
robotics. The problem is to ﬁnd robot postures, so as to satisfy certain kinematicconstraints. Traditionally the kinematic constraints are on the end-effectors, such as
hand reaching a certain location with a given orientation. More recently applications
have required more complex constraints in the task space, such as grasping a mug
on the table while keeping the robot free from collision (Fig. 1a).
Solving the IK problem is quite challenging, as it requires solving a set of nonlinear
equations involving products of trigonometric functions (sin/cos). There have been
tremendous efforts to solve the inverse kinematics problem [ 4,10,21,24], and
they can be coarsely categorized as analytical or numerical. The analytical approachsolves the kinematics equations as polynomials of sine and cosine and can produce
closed-form solutions. It is widely known that many manipulators with 6 Degree of
Freedoms (DoFs) allow analytical solution for the end effector to reach a speciﬁedposition with a given orientation [ 20,26]. More generally, Diankov et al. introduced
IKfast , which can ﬁnd inverse kinematics solutions for more complicated robots, but
the set of kinematics constraints it can handle is still limited [ 15]. Since the analytical
approach solves the inverse kinematics problem by computing roots of polynomial
equations, it does not permit inequality constraints on the link poses [ 1], which occur
frequently when planning postures for certain tasks. This shortcoming makes it hard
H. Dai (B)·R. Tedrake
Toyota Research Institute, Los Altos, USAe-mail: hongkai.dai@tri.global
R. Tedrake
e-mail: russ.tedrake@tri.global
G. Izatt ·R. Tedrake
Computer Science and Artiﬁcial Intelligence Lab, MIT, Cambridge, USAe-mail: gizatt@mit.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_56809
810 H. Dai et al.
Fig. 1 Our IK solver ﬁnds a collision free grasping posture for the KUKA IIW A arm (left), and a
posture for Little Dog standing on stepping stones (right)
to handle general task space constraints involving inequalities, such as “putting the
hand between the two boxes on the table”.
On the other hand, the numerical approach can solve inverse kinematics problems
for complicated robots with generic constraints. This approach formulates an inverse
kinematics problem as a general non-convex nonlinear optimization problem, and
calls gradient-based nonlinear solvers to handle these non-convex constraints [ 3,8,
12,16,25]. The drawback of this approach is that the solution heavily relies on
the user-supplied initial seed. Because the nonlinear optimization typically solves
the problem locally around the initial seed, hence it can get trapped in some locally
infeasible region and never reach the solution far away. As a result, when thesenon-convex nonlinear solver reports the problem is infeasible, it only asserts local
infeasibility, providing no guarantee on the global solution [ 5]. We will show that our
approach can signiﬁcantly improve the robustness and success rate of the IK solver,over the gradient-based nonlinear optimization approach.
We shall use a numerical approach to the IK problem with generic constraints,
while also obtain the global solution. Instead of formulating the problem throughnon-convex nonlinear optimization, we instead consider a mixed-integer convex
optimization formulation, which does not require an initial seed, and can provide
a global solution [ 7], such as a certiﬁcate of global infeasibility. The non-convexity
of the IK problem originates from the non-convixity of SO(3)constraints on rota-
tion. If we choose to represent rotation with a matrix R=[u
1u2u3]∈ R3×3,t h i s
rotation matrix should satisfy the following constraints
uT
iui=1 (Unit length) (1a)
uT
iuj=0i fi/negationslash=j(Orthogonality) (1b)
ui×uj=uk, (1c)
Global Inverse Kinematics via Mixed-Integer … 811
where (i,j,k)=(1,2,3),(2,3,1)or(3,1,2)in (1c). We choose the rotation matrix
as the orientation representation, since the position of a point attached to a body is a
linear expression of the body rotation matrix; on the other hand, the position would
be a nonlinear expression, if we were to represent orientation using other forms,
such as unit quaternions or angle-axis.
Obviously the orientation constraint SO(3)(1a)–(1c) is non-convex. Take con-
straint ( 1a) as an example: geometrically, the convex combination of two points on the
surface of a unit sphere, lies strictly in the interior of the unit sphere. Algebraically,
an equality constraint is non-convex, if it includes quadratic terms such as ui(1)2in
(1a), or bilinear terms like ui(1)uj(1)in (1b).
V arious convex relaxations for SO(3)constraints have been proposed. Saunderson
et al. proved that the convex hull of the set of rotation matrices can be describedby a positive semideﬁnite constraint , a special type of convex constraint [ 27]. In
[11] this convex relaxation is exploited to solve the IK problem through a sequence
of convex optimizations. The drawback of this approach [ 11] is that the convex
hull of the rotation matrix is a rather loose relaxation of SO(3). For example, the
matrix/bracketleftbigu
1,0,0/bracketrightbig
=0.5/bracketleftbigu1,u2,u3/bracketrightbig
+0.5/bracketleftbigu1,−u2,−u3/bracketrightbig
is in the convex hull for
any rotation matrix/bracketleftbigu1,u2,u3/bracketrightbig
. Hence quite often the relaxed convex program is
too loose to detect global infeasibility of the original IK problem. To overcome this,
we seek a tighter mixed-integer convex relaxation of the SO(3)constraint.
Our intuition is that instead of considering the convex hull of allrotation matrices
as in [ 27], we divide the range of the rotation matrices into smaller intervals, and
compute the convex hull of each small interval to obtain a tighter approximation. Wecan then constrain the approximated rotation matrix to be within one of the convex
hulls. This approach is inspired by Deits et al. [ 13], in which the orientation con-
straint SO(2)in 2D is approximated by mixed-integer linear constraints. In a similar
fashion, we replace ( 1a)–(1c) with mixed-integer convex (quadratic) constraints, as
a relaxation of the original non-convex SO(3)constraints. This relaxation allows the
inverse kinematics problem to be formulated as a mixed-integer convex optimizationprogram, which is amenable to the branch-and-bound algorithm by branching on
integer variables, and can be solved efﬁciently by modern solvers [ 17]. Like convex
optimization, mixed-integer convex optimization also does not rely on the initialseed, and warrants global solution [ 6,28]. With the rapid advancement in numerical
solvers, mixed integer convex optimization becomes increasingly popular in robotics
[14,22,30]. In this paper we show that our approach can either produce an approx-
imate solution to the inverse kinematics problem, or prove the problem is globally
infeasible for generic constraints.
2 Background
In this section we brieﬂy review how to relax a non-convex quadratic constraint as
mixed-integer convex constraints, which will be used to relax the non-convex SO(3)
constraint on the rotation matrix ( 1a)–(1c).
812 H. Dai et al.
Consider a non-convex quadratic constraint
{z|a≤zTQz+cTz≤b}, (2)
where the matrix Qis not necessarily positive semideﬁnite. This quadratic constraint
contains bilinear product terms like xy, visualized in Fig. 2a, b. It is NP-hard to
obtain an exact global optimal solution to this non-convex problem, but there has
been a lot of research on obtaining an approximated one through mixed-integer-
convex optimization [ 19,23]. The key idea is to partition the range of each variable
into smaller intervals, and replace the bilinear term xywith a linear term which
approximates xyinside each interval. To see this geometrically, we draw the surface
ofw=xyin Fig. 2within x∈[0,1],y∈[0,1]. We then partition the range [0,1]
into two intervals, with interval ias[φi,φi+1]. The convex hull of w=xyin interval
x∈[φi,φi+1],y∈[φj,φj+1]is a tetrahedron. We can then relax the constraint w=
xy, with the constraint that the point (x,y,w) lies within one of the tetrahedrons.
We will use some binary variables to determine within which tetrahedron the point
lies.
To enforce that (x,y,w) falls into one of the convex hulls, we rely on the special
ordered set of type 2 (sos2) constraint [ 2], deﬁned as follows
Deﬁnition 1 Special ordered set of type 2: λ=(λ0,...,λ n)∈Rn+1is in sos2, if
/summationdisplay
iλi=1,λi≥0( 3 a )
∃j∈0,..., n−1,s.tλi=0∀i/negationslash=j,i/negationslash=j+1. (3b)
Namely at most two entries in λcan be strictly positive, these two entries must
be consecutive in their ordering.
The sos2 constraint is often used to formulate a piecewise linear approximation
to a nonlinear function. For example in Fig. 3, we approximate f(x)w i t han e w
variable wsatisfying
/bracketleftbiggx
w/bracketrightbigg
=/summationdisplay
iλi/bracketleftbiggφi
f(φi)/bracketrightbigg
,λ is in sos2 (4)
(4) forces the point (x,w) to be on the black lines in Fig. 3.
We can formulate the sos2 ( 3) as mixed-integer linear constraints by introducing
auxiliary binary variables. In this paper we adopt the formulation proposed in [ 31],
which introduces log2nbinary variables. We refer the readers to [ 31] for more details
on sos2 constraint.
To impose the constraint that (x,y,w) is in one of the tetrahedron in Fig. 2c, d,
we introduce auxiliary continuous variables γ∈R(n+1)×(n+1),α∈Rn+1,β∈Rn+1
with the additional constraints
Global Inverse Kinematics via Mixed-Integer … 813
Fig. 2 a ,bw=xysurface in the range x∈[0,1],y∈[0,1]from two perspectives. In c,dwe
divide the range [0,1]into two intervals, with (φ0,φ1,φ2)=(0,0.5,1), and compute the convex
hull of the surface in each interval x∈[φi,φi+1],y∈[φj,φj+1]. Each convex hull is a tetrahedron
drawn in distinct colors
Fig. 3 A piecwise linear
function (black lines)
approximating the nonlinear
function f(x)(red curve)
x=/summationdisplay
iαiφi,y=/summationdisplay
jβjφj (5a)
w=/summationdisplay
i/summationdisplay
jγi,jφiφj (5b)
814 H. Dai et al.
/summationdisplay
jγi,j=αi,/summationdisplay
iγi,j=βj,γ i,j≥0 (5c)
α,βa r ei ns o s 2 . (5d)
Constraint ( 5d) is formulated as mixed-integer linear constraints with auxiliary
binary variables.
To summarize, for a non-convex quadratic constraint ( 2), we can replace the
bilinear term xywithwsatisfying ( 5), and obtain mixed-integer convex constraints
as a relaxation of the original non-convex constraint.
3 Approach
As explained in Sect. 1, the non-convexity of the inverse kinematics problem orig-
inates from the non-convexity of the SO(3)constraint. In order to obtain a global
solution to this non-convex problem, in Sect. 3.1, we ﬁrst relax the SO(3)con-
straint ( 1a)–(1c), to a set of mixed-integer convex constraints, using the technique
described in Sect. 2. Then in Sect. 3.2, we formulate the inverse kinematics problem
by searching over the link poses (position/orientation) satisfying both the kinematic
constraints, and the relaxed SO(3)constraint introduced in Sect. 3.1. Finally, we
project the approximated rotation matrices to the SO(3)manifold to obtain the angle
of each joint within the joint limits.
3.1 Rotation Constraint Relaxation
We aim to ﬁnd a matrix ¯R∈R3×3satisfying a relaxation of the SO(3)constraint
(1a)–(1c). To this end, we cut the range [−1,1]into nsmall intervals. The quadratic
constraints ( 1b)–(1c) can be relaxed by replacing each bilinear term with a new
variable approximating the bilinear term within each interval, described by ( 5).
To relax the unit length constraint uT
iui=1(1a), we ﬁrst introduce auxiliary
variables wi∈R3,λi,j∈Rn+1satisfying
/bracketleftbiggui(j)
wi(j)/bracketrightbigg
=n/summationdisplay
k=0λi,j
k/bracketleftbiggφk
φ2
k/bracketrightbigg
,λi,jis in sos2. (6)
Constraint ( 6) forces the point (ui(j),wi(j))to be on the black lines in Fig. 4,f o r
n=4 case. Namely wi(j)is an upper-bound of ui(j)2. We can thus relax the unit
length constraint ( 1a)a s
Global Inverse Kinematics via Mixed-Integer … 815
Fig. 4 The range of ui(j)is cut into 4 intervals, with (φ0,φ1,φ2,φ3,φ4)=(−1,−0.5,0,0.5,1).
The adjacent two points (φi,φ2
i)and(φi+1,φ2
i+1)are connected with the black straight line. We
introduce an auxiliary variable wi(j)to be on the black lines, as an upper bound of the red curve
ui(j)2. The unit length constraint/summationtext
jui(j)2=1 is relaxed as/summationtext
jzi(j)=1, where zi(j)lies
within the shaded cyan region, as a relaxation of the red curve ui(j)2
uT
iui≤1( 7 a )
wi(1)+wi(2)+wi(3)≥1. (7b)
Geometrically, constraint ( 7a) forces the rows/columns of the rotation matrix to
be within the unit sphere, while constraint ( 7b) pushes the row/column vector from
inside the unit sphere, such that the Euclidean length of the row/column vector is
bounded from below.
While the generic technique introduced in Sect. 2can already approximate the
orthogonality constraint, it is desirable to introduce an additional relaxation, because
enforcing the solution to live in the intersection of different relaxations can only
improve its approximation quality. We thus impose the following convex (quadratic)
constraints as a relaxation to the orthorgonal constraint ( 1b)
|ui±uj|2
2≤2, (8)
where ui,ujare two distinctive rows/columns of the matrix ¯R,|•| 2is the l2norm
of a vector. Constraint ( 8) is equivalent to |uT
iuj|≤1
2(2−uT
iui−uT
juj). When
the vectors ui,ujare “close to” the unit sphere (due to the constraint ( 7a)–(7b)),
constraint ( 8) approximates the orthogonal constraint uT
iuj=0. Similarly we can
impose the convex constraint |u1±u2±u3|2
2≤3 as an relaxation of the orthogonal
constraint, where u1,u2,u3are all three rows/columns of ¯R.
The non-convex SO(3)constraint is thus relaxed to a set of mixed-integer convex
quadratic constraints. These mixed-integer convex constraints are readily handled by
modern numerical solvers such as Gurobi [ 17].
816 H. Dai et al.
Fig. 5 The kinematics relation between adjacent link i−1a n d i[10]. The frame with axes ˆxi,ˆyi,ˆzi
is rigidly attached to the child link i, the position of the frame origin is pi. Without loss of generality
we can assume the rotation axis iis along the zaxisˆzi. Similarly for the parent link i−1. The joint
frame Jwith axes ˆxJ,ˆyJ,ˆzJis rigidly attached to the parent link i−1; this joint frame coincides
with the child link frame when the joint angle qi=0
3.2 Inverse Kinematics Formulation
We aim to solve the inverse kinematics problem by ﬁnding the pose of each link. The
pose of link ican be represented by rigidly attaching a frame to the link, with the
positionWpi∈R3and the rotation matrixWRi, where the right subscript idenotes
it is link i’s frame we are interested in, the left superscriptWindicates the quantity
is expressed in the world frame. In Fig. 5, we illustrate the kinematics relationship
between two links connected by a revolute joint. We will express this kinematics
relation as constraints on the positionWpiand orientationWRiof link irelative to
and expressed in the world frame, and the position/orientation of its parent link i−1.
A joint frame Jis rigidly attached to the parent link i−1, and coincides with the
child link frame, when the joint angle qi=0. We denote the ﬁxed translation of joint
frame to the parent link frame asi−1pJ. The joint axis is expressed asi−1ˆziin the
parent link (i−1)’s frame, andiˆziin the child link i’s frame. Since the position and
direction of joint axis is invariant under rotation about this axis, we can impose the
following constraints on the poses of the parent and child links
Wpi=Wpi−1+WRi−1i−1pJ (9a)
WRi−1i−1ˆzi=WRiiˆzi (9b)
(9) are linear constraints on the decision variablesWRi,WRi−1,Wpi,Wpi−1.
We also handle the angle limits on the revolute joint. Without loss of generality,
we assume that angle limits for joint iare−α≤qi≤α. In order to enforce this joint
limit constraint with only link poses as decision variables, we consider a unit length
vector ˆxJ, perpendicular to the joint axis ˆzi, and how it relates to itself before and
after joint rotation. For example in Fig. 5, rotating the i’th joint by qitransforms the
unit length vector ˆxJtoˆxi. The joint limit constraint |qi|≤αcan be imposed as
Global Inverse Kinematics via Mixed-Integer … 817
Fig. 6 The visual illustration of the joint limit constraint ( 10). The red chord is the difference
between ˆxJand another vector, obtained by rotating ˆxJabout an axis by angle α. The chord’s length
is 2 sin α/2.ˆxJ−ˆxishould have shorter length than the red chord, since the angle qibetween these
two vectors is smaller than α
|∠(WˆxJ,Wˆxi)|≤α⇔WˆxT
JWˆxi≥cosα⇔/vextendsingle/vextendsingleWˆxJ−Wˆxi/vextendsingle/vextendsingle
2≤2s i nα
2
⇔/vextendsingle/vextendsingleWRi−1i−1ˆxJ−WRiiˆxi/vextendsingle/vextendsingle
2≤2s i nα
2
(10)
where ∠(·,·)is the angle between two vectors.i−1ˆxJis the given unit length vector
ﬁxed and expressed in the parent i−1 frame, andiˆxiis the given unit length vector
ﬁxed and expressed in the child iframe. Eq. ( 10) is a convex (quadratic) constraint on
the decision variables (parent and child link orientations). The joint limit constraint
(10) would be tight ifWRi,WRi−1satisﬁed SO(3)constraints exactly (Fig. 6).
To impose collision avoidance constraints, we use a similar formulation as [ 14].
We segment the free space into convex polytopes Pi,i=1,..., Np, with the vertices
ofPidenoted as vi,1,..., vi,mi. A point is collision free if it is in one of the polytopes.
We impose the following mixed-integer convex constraints to enforce the point Q,
on link kwith coordinateskpQin the link frame, to be collision free
Wpk+WRkkpQ=Np/summationdisplay
i=1mi/summationdisplay
j=1λi,jvi,j (11a)
zi=mi/summationdisplay
j=1λi,j,Np/summationdisplay
i=1zi=1 (11b)
λi,j≥0,ziis binary (11c)
ziis the binary variable indicating within which polytope the link point Qstays.
With the mixed-integer convex relaxation on SO(3)(Sect. 3.1), and the kinematic
constraints in Sect. 3.2, we can solve the IK problem through mixed-integer convex
(quadratic) optimization, to ﬁnd the body positions and an approximated solution to
818 H. Dai et al.
the body orientations. If the mixed-integer convex program is infeasible, it proves
that the original unrelaxed IK problem is globally infeasible.
3.3 Reconstruct Joint Angle by Projecting Back to S O (3)
After obtaining the approximated solution to the body orientations through the mixed-
integer convex optimization, we need to recover the value of each joint angle. Sincethe solution to the body orientation does not satisfy the SO(3)constraint exactly, we
project the approximated solution to the rotation constraint, starting from the root
link. If the link is ﬂoating, then the optimal projected solution is obtained as UV
T,
where ¯R=U/Sigma1VTis the SVD of ¯R, as proved in [ 18]. On the other hand, if the link
is connected to its parent link through a revolute joint, we projectWRT
i−1W¯Rionto
SO(3)with the given rotation axis and angle limits, whereWRi−1is the rotation
matrix of the parent i−1 link, computed by doing forward kinematics using the
recovered posture from the root to the i−1 link. To projectWRT
i−1W¯Ri, we ﬁnd the
joint angle qisuch that the joint rotation matrix R(i−1ˆzi,qi)has the minimal error
toWRT
i−1W¯Riunder the joint limits (Fig. 5).R(i−1ˆzi,qi)∈SO(3)means rotating
by angle qiabout the axisi−1ˆzi. Algebraically, the optimal qiis the solution to the
following program
min
qi/vextendsingle/vextendsingleR(i−1ˆzi,qi)−WRT
i−1W¯Ri/vextendsingle/vextendsingle
F(12a)
s.t−α≤qi≤α. (12b)
In the objective function |·| Fis the matrix Frobenius norm, |X|F=/radicalbig
trace(XTX).R(i−1ˆzi,qi)can be computed from the Rodriguez Formula below
R(i−1ˆzi,qi)=I3×3+sinqi⌊i−1ˆzi⌋×+(1−cosqi)⌊i−1ˆzi⌋2
×, (13)
where ⌊i−1ˆzi⌋×∈R3×3is the skew-symmetric matrix, representing the cross product
with the vectori−1ˆzi.
Substituting Eq. ( 13) into the optimization objective ( 12a), we can analytically
obtain the optimal qias
⎧
⎪⎪⎪⎨
⎪⎪⎪⎩if∃k∈Z,−α+β≤π
2+2kπ≤α+β,qi=π
2+2kπ−β
else:
if sin(−α+β)≥sin(α+β), qi=−α
if sin(−α+β) < sin(α+β), qi=α.(14)
where β=atan2/parenleftbig
−trace(MTA2),trace(ATM)/parenrightbig
,A=⌊i−1ˆzi⌋×,M=WRT
i−1W¯Ri.
Note that at value π/2+2kπ−βthe derivative of the objective ( 12a) is zero, i.e.,
it is the optimal solution to ( 12a) without the joint limits ( 12b). The optimal solution
Global Inverse Kinematics via Mixed-Integer … 819
is truncated to the joint bounds if π/2+2kπ−βis outside of the angle limits. qiin
(14) is the optimal joint angle within the joint limits, that minimizes the projection
error from the mixed-integer convex optimization solution, to SO(3)with a given
rotation axis and angle bounds. We thus obtain a posture as the approximated solution
to the inverse kinematics problem.
4 Results
We present the results on the SO(3)relaxation and the inverse kinematics algorithm.
The programs run on 64 bit Intel Xeon CPUs with Gurobi 7.5 [ 17] as the solver.
4.1 Tightness of Rotation Matrix Relaxation
We ﬁrst show the tightness of our mixed-integer convex relaxation of the SO(3)
constraint on the rotation matrix. If ¯R∈R3×3satisﬁes the relaxed constraints, then
we know that |¯Rv|2≈|v|2∀v∈R3, and∠(¯Rv1,¯Rv2)≈∠(v1,v2). Geometrically
the relaxation allows the transformation v→¯Rvto change the length of a vector,
and to perturb the angle between two vectors. In this section we show how much
shorter the transformed vector can be, and also derive a bound on the angle error.
We ﬁnd the global optimum to two mixed-integer convex optimization programs,
to determine the tightness of the relaxation. First we compute the global minimum of
min ¯R,i|¯Rei|2, where eiis the unit length vector with ei(i)=1. To determine the angle
error, we compute the global minimum min ¯Rin relaxation ,i/negationslash=j|¯Rei+¯Rej|2. If we denote
this global minimum as d, we obtain loose bounds on the angle after transformation
as 180◦−arccos (d2/2−1)≤∠(¯Rei,¯Rej)≤arccos (d2/2−1). We compute the
error bounds on the relaxation, by partitioning the range [−1,1]into N=2,4o r6
intervals, with φi=− 1+2i/N. The results are in Table 1. The last column with
inﬁnite number of binary variables is the ideal ¯R∈SO(3)case without any relax-
ation. The bounds on |¯Rei|2and|¯Rei+¯Rej|2are tight, but the bound on the angle
in the last row of the table is not tight.
As the number of intervals increases, the convex hull of xyin each interval is closer
to the bilinear product. As a result in Table 1, the minimal length of ¯Reiincreases,
Table 1 The tightness on the mixed-integer convex relaxation on SO(3)
#o fi n t e r v a l s 2 4 6 ∞
min¯R,i/vextendsingle/vextendsingle¯Rei/vextendsingle/vextendsingle
20.57735 0.90453 0.95784 1
min¯R,i,j/vextendsingle/vextendsingle¯Rei+¯Rej/vextendsingle/vextendsingle
20.60302 1.22474 1.32667 1.414
∠(¯Rei,¯Rej)−90◦[−54.90◦,54.90◦][−14.47◦,14.47◦][−6.89◦,6.89◦][0◦,0◦]
820 H. Dai et al.
Fig. 7 KUKA IIW A arm grasping a mug on the table with two different postures. The green line
is the yaxis of the hand which should stay horizontal. The mesh ﬁles of the objects are obtained
from Shapenet [ 9]
and the bound on the angle error shrinks, leading to a tighter relaxation of SO(3).
In the inverse kinematics problem, we choose to cut [−1,1]into 4 intervals, as a
compromise between computation speed and relaxation accuracy.
4.2 Inverse Kinematics
We ﬁrst show that our inverse kinematics approach can ﬁnd postures for compli-cated kinematics constraints including inequalities, such as those arising from the
task “grasping a mug on the table in the cluttered environment” in Fig. 7.W es h o w
two distinct postures found by Gurobi on the mixed-integer convex optimization
programs. These two postures have different sets of active binary variables. We use a
KUKA IIW A arm and a Schunk gripper with 7 total joints. The kinematics constraintsare
•The middle point between the two ﬁngers should be on the axis of the mug. The
middle point should be above the table, and lower than the top rim of the mug.
•The yaxis on the hand should be horizontal.
•The links and the gripper should be collision free.
We also show that our approach can prove the global infeasibility of some kine-
matics constraints. In Fig. 8, we impose the task constraint that the hand should grasp
the wine bottle, by keeping its center on the green line segment, and its orientationaligned with the bottle’s longitudinal axis. In the left plot the mixed-integer program
is solved successfully, while in the right plot with the fridge on the table, the addi-
Global Inverse Kinematics via Mixed-Integer … 821
Fig. 8 KUKA IIW A arm grasping the wine bottle. On the left, the IK problem is solved successfully.
The center of the hand is constrained to be on the green line segment. On the right, with the fridge
on the table, the mixed-integer convex program proves there is no posture that can satisfy the same
grasping constraint, while keeping the robot from colliding with the fridge
(a) LittleDog stands on stepping stones. Com-
putation takes 15s for this 18 DoF quadruped.
(b) Mixed-Integer Convex IK proves it infeasi-
ble to put each foot on one of the stepping stone
(violet), while avoiding the red obstacles.
Fig. 9 Find standing postures for LittleDog using mixed-integer convex IK
tional collision avoidance constraint causes the mixed-integer convex program to be
globally infeasible. The solver takes 0.75 s to detect the global infeasibility.
For more complicated robots like LittleDog (Fig. 9), we show that our approach
can ﬁnd a posture of the robot with each leg on one of the stepping stones. This
constraint is imposed similar to the collision avoidance constraint ( 11a)–(11c), but
replacing the vertices in each polytope with the top corners of each stepping stone.
In Fig. 9b the mixed-integer convex IK spends 0.25 s to determine that there does
not exist a posture for the robot to put each of its toes on one of the violet stepping
stones, while avoiding the red obstacles.
822 H. Dai et al.
Fig. 10 The results of running both analytical and mixed-integer convex IK on an ABB IRB140
arm. We take 9321 sample points in an 1 m3cube. The robot end effector is required to reach each
sampled location with two different given orientations. The desired orientation of the end effectoris shown in silver color next to the coordinate axes in the bottom left of the ﬁgure. The green dotscorrespond to positions for which both IK approaches obtain the solution. The blue dots correspondto positions for which both IK approaches guarantee global infeasibility. The red dots correspondto the gap for which the analytical IK proves that the problem is infeasible, while the mixed-integerconvex IK thinks the problem is feasible under relaxation. In the ﬁrst row of the ﬁgures we showall three colors of dots. In the last two rows we highlight each color of the dots separately
Global Inverse Kinematics via Mixed-Integer … 823
Fig. 11 Histogram on
mixed-integer IK
computation time, for all the
green dots in Fig. 10a, b
0 5 10 15 20 25 30
time (s)050100150200250count
Fig. 12 The end effector
pose error for the
reconstructed posture from
mixed-integer convex IK
solver, for all the green dots
in Fig. 10a, b
02468 1 0 1 2
position error (cm)0123456789orientation angle error (degree)Joint limits active
Joint limits inactive
To get some statistics on our mixed-integer convex IK approach, we test its per-
formance on an ABB IRB140 arm with 6 DoF, for which the IK problem can be
solved analytically [ 10]. In Fig. 10, we show the results of running both analytical
and mixed-integer convex IK on the robot. In each column of the ﬁgures, an IK prob-
lem is solved to determine whether the end effector will reach a sampled location
with the given orientation. There are three categories of the solutions
•Green dot. Both analytical and mixed-integer convex IK ﬁnd the solution.
•Blue dot. Both analytical and mixed-integer convex IK prove global infeasiblity.
•Red dot. Analytical IK proves the problem is infeasible, but mixed-integer convex
IK thinks the problem is feasible, due to the relaxation.
We ﬁrst analyze the tightness of the relaxation. The red dots, which imply loose
relaxation on the SO(3), form a thin layer between the unreachable blue dots, and the
reachable green dots. Only 2 .43% of all the samples are red. For all the kinematically
unreachable points (red and blue), 94 .72% of them are detected as global infeasible
824 H. Dai et al.
time (s)0100020003000400050006000count
(a) Histogram on mixed-integer IK computation
time, when it proves global infeasibility.0 5 10 15 20 25 30 0 0.02 0.04 0.06 0.08 0.1
time (s)0500100015002000250030003500count
(b) A zoomed in view of the histogram in Fig
13a for samples with computation time ≤0.1s.
Fig. 13 Histogram on computation time when mixed-integer IK detects global infeasibility
in our mixed-integer IK solver; on the other hand, SDP relaxation of the SO(3)
constraint, proposed in [ 11,27], can detect only 42 .23% of infeasibility. This huge
difference demonstrates that our mixed-integer convex relaxation of SO(3)constraint
is a lot tighter than the SDP relaxation.
We also examine the computation time of the mixed-integer convex IK, together
with the quality of the approximated solution. In Fig. 11we show the histogram of the
mixed-integer convex IK computation time for the green dots (both IK approaches
can ﬁnd the solution). 95 .1% of samples are solved within 10s, and the average
computation time is 4.2 s. In Fig. 12after reconstructing the posture from the mixed-
integer convex IK, we compute the end effector pose error from the reconstructed
posture to the sampled pose, to demonstrate how much relaxation error is brought
into the approximated IK solution. Most of the solutions have less than 5 cm of
position error, and less than 2◦of orientation error. The solutions with large pose
error all have some joints with active joint limits. This large error occurs because
the joint limit constraint ( 10) would be tight only if the link orientation matrices
WRi−1,WRiwere on SO(3). With our mixed-integer convex relaxation on SO(3),
the angle π/2+2kπ−βin (14), where the derivative of the objective function ( 12a)
would be zero, falls outside of the joint limits. So the reconstructed joint angle is
instead truncated to its angle limits ±α. This truncation can cause large cost in the
projection error ( 12a), resulting in signiﬁcant deviation in the end effector pose.
In Fig. 13a, b we draw the histograms of the computation time, when the mixed-
integer convex IK proves global infeasibility. In 86 .05% cases the computation takes
less than 0.1 s.
Finally, we solve the inverse kinematics program for the samples in Figs. 10a, b
through nonlinear optimization [ 16]. We ﬁrst set the initial seed as the postures in
Fig. 10a, b, and then solve the nonlinear optimization again, but replace the initial
seed with the reconstructed posture from the mixed-integer convex IK. The success
rate of the nonlinear IK solver increases from 85 .67% to 100%. This improvement
Global Inverse Kinematics via Mixed-Integer … 825
demonstrates that the approximate solution from the mixed-integer convex IK can
always serve as a good initial seed for the nonlinear optimization program.
5 Conclusion and Discussion
In this paper we propose a mixed-integer convex relaxation of the non-convex SO(3)
constraint, and formulate a mixed-integer convex optimization program to solve theinverse kinematics problem globally with generic constraints. We show that this
relaxation is relatively tight, and our IK approach can either produce an approximate
solution, or prove that the solution does not exist globally. We demonstrate resultswhen applying our approach on manipulators and quadruped robots.
Currently the computation time is a lot slower than the gradient-based nonlinear
optimization approach, and the reconstructed posture can violate the kinematic con-straints with large errors. We are working on improving the accuracy of the solution,
and reducing the computation time.
We would like to thank our colleagues at Toyota Research Institute and Robot
Locomotion Group for the valuable discussions.
References
1. Bates, D.J., Hauenstein, J.D., Sommese, A.J., Wampler, C.W.: Numerically Solving Polynomial
Systems With Bertini, vol. 25. SIAM, Philadelphia (2013)
2. Beale, E.M.L., Tomlin, J.A.: Special facilities in a general mathematical programming system
for non-convex problems using ordered sets of variables. Oper. Res. 69, 447–454 (1970)
3. Beeson, P ., Ames, B.: Trac-ik: an open-source library for improved solving of generic
inverse kinematics. In: 2015 IEEE-RAS 15th International Conference on Humanoid Robots(Humanoids), pp. 928–935. IEEE, Seoul (2015)
4. Berenson, D., Srinivasa, S., Kuffner, J.: Task space regions: a framework for pose-constrained
manipulation planning. Int. J. Robot. Res. 30(12), 1435–1460 (2011)
5. Bertsekas, D.P .: Nonlinear Programming. Athena scientiﬁc Belmont, Belmont (1999)6. Bertsimas, D., Weismantel, R.: Optimization Over Integers, volume 13. Dynamic Ideas Bel-
mont, Belmont (2005)
7. Boyd, S., V andenberghe, L.: Convex Optimization. Cambridge University Press, Cambridge
(2004)
8. Buss, S.R.: Introduction to inverse kinematics with jacobian transpose, pseudoinverse and
damped least squares methods. IEEE J. Robot. Autom. 17(1–19), 16 (2004)
9. Chang, A.X., Funkhouser, T., Guibas, L., Hanrahan, P ., Huang, Q., Li, Z., Savarese, S.,
Savva, M., Song, S., Su, H., et al.: Shapenet: an information-rich 3d model repository (2015).arXiv:1512.03012
10. Craig, J.J.: Introduction to Robotics: Mechanics and Control, vol. 3. Pearson/Prentice Hall,
Upper Saddle River (2005)
11. Dai, H., Majumdar, A., Tedrake, R.: Synthesis and optimization of force closure grasps via
sequential semideﬁnite programming. In: International Symposium on Robotics Research(2015)
826 H. Dai et al.
12. Dai, H., V alenzuela, A., Tedrake, R.: Whole-body motion planning with centroidal dynamics
and full kinematics. In: 2014 14th IEEE-RAS International Conference on Humanoid Robots(Humanoids), pp. 295–302. IEEE, Madrid (2014)
13. Deits, R., Tedrake, R.: Footstep planning on uneven terrain with mixed-integer convex optimiza-
tion. In: 2014 14th IEEE-RAS International Conference on Humanoid Robots (Humanoids),IEEE, Madrid (2014)
14. Deits, R., Tedrake, R.: Efﬁcient mixed-integer planning for uavs in cluttered environments.
In: 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, Seattle(2015)
15. Diankov, R.: Automated construction of robotic manipulation programs. Ph.D. thesis, Carnegie
Mellon University, Pittsburgh (2010)
16. Fallon, M., Kuindersma, S., Karumanchi, S., Antone, M., Schneider, T., Dai, H., D’Arpino,
C.P ., Deits, R., DiCicco, M., Fourie, D., et al.: An architecture for online affordance-basedperception and whole-body planning. J. Field Robot. 32(2), 229–254 (2015)
17. Gurobi optimizer reference manual, vol. 2, pp. 1–3 (2012). http://www.gurobi.com
18. Haralick, R.M., Joo, H., Lee, C.-N., Zhuang, X., V aidya, V .G., Kim, M.B.: Pose estimation
from corresponding point data. IEEE Trans. Syst. Man Cybern. 19(6), 1426–1446 (1989)
19. Huchette, J., Vielma, J.P .: Small independent branching formulations for unions of v-polyhedra
(2016). arXiv:1607.04803
20. Manocha, D., Canny, J.F.: Real time inverse kinematics for general 6r manipulators. In: Pro-
ceedings of the 1992 IEEE International Conference on Robotics and Automation, pp. 383–389.IEEE, Nice (1992)
21. Mason, M.T.: Mechanics of Robotic Manipulation. MIT press, Cambridge (2001)22. Mellinger, D., Kushleyev, A., Kumar, V .: Mixed-integer quadratic program trajectory genera-
tion for heterogeneous quadrotor teams. In: 2012 IEEE International Conference on Roboticsand Automation (ICRA), pp. 477–483. IEEE, St. Paul (2012)
23. Misener, R., Floudas, C.A.: Global optimization of mixed-integer quadratically-constrained
quadratic programs (MIQCQP) through piecewise-linear and edge-concave relaxations. Math.Program. 136, 155–182 (2012)
24. Murray, R.M., Li, Z., Sastry, S.S., Sastry, S.S.: A mathematical introduction to robotic manip-
ulation. CRC Press, Boca Raton (1994)
25. Peiper, D.L.: The kinematics of manipulators under computer control. Technical report, Ph.D.
thesis, Stanford University, Stanford (1968)
26. Raghavan, M., Roth, B.: Kinematic analysis of the 6r manipulator of general geometry. In:
International Symposium on Robotics Research, pp. 314–320 (1990)
27. Saunderson, J., Parrilo, P .A., Willsky, A.S.: Semideﬁnite descriptions of the convex hull of
rotation matrices. SIAM J. Optim. 25(3), 1314–1343 (2015)
28. Schrijver, A.: Theory of Linear and Integer Programming. Wiley, Hoboken (1998)29. Tedrake, R., The Drake Development Team: Drake: a planning, control, and analysis toolbox
for nonlinear dynamical systems (2016)
30. V alenzuela, A.K.: Mixed-integer convex optimization for planning aggressive motions of
legged robots over rough terrain. Ph.D. thesis, Massachusetts Institute of Technology, Cam-
bridge (2016)
31. Vielma, J.P ., Nemhauser, G.L.: Modeling disjunctive constraints with a logarithmic number of
binary variables and constraints. Math. Program. 128(1), 49–72 (2011)
Parallelizing Synthesis from Temporal
Logic Speciﬁcations by IdentifyingEquicontrollable States
Sumanth Dathathri, Ioannis Filippidis and Richard M. Murray
1 Introduction
As robotic systems get more complex, logic speciﬁcations assist in precisely specify-
ing desired behavior for a system and constructing controllers that provably guaranteesatisfaction of the speciﬁcation. In our work, we focus on reactive synthesis from
temporal logic speciﬁcations. This involves reasoning about all admissible behaviors
for the environment and synthesizing a policy for the controlled agent. This makes the
algorithms for synthesis difﬁcult to scale and synthesis can be prohibitively expensive
when applied to problems with large state spaces.
In this paper, we focus on reactive planning for speciﬁcations in linear temporal
logic (LTL) and, in particular, the Generalized Reactivity (1) (GR(1)) fragment. The
complexity for synthesis for general LTL speciﬁcations is doubly exponential in thelength of the formula [ 25]. For LTL formulas in the GR(1) fragment, synthesis can
be performed in time polynomial in the size of the state space, which is typically
exponential in the number of variables that describe the problem. The complexityof synthesis for the GR(1) fragment scales as cubic or quadratic depending on the
algorithms used for synthesis [ 2]. Tractability has enabled the use of speciﬁcations
in the GR(1) fragment for robotics applications [ 7,11,18,22].
Scalability of synthesis aids in the application of formal techniques to more chal-
lenging robotics applications [ 21]. Symbolic model checking based on local invari-
ants has been parallelized with substantial speed-ups [ 8,9]. Performing computations
S. Dathathri ( B)
CMS, California Institute of Technology, Pasadena, CA, USAe-mail: sdathath@caltech.edu
I. Filippidis ·R. M. Murray
CDS, California Institute of Technology, Pasadena, CA, USA
e-mail: iﬁlippi@caltech.edu
R. M. Murray
e-mail: murray@cds.caltech.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_57827
828 S. Dathathri et al.
directly on the original system can accelerate policy synthesis for a fragment of LTL
more restricted than GR(1) [ 28]. In [ 1], a compositional approach is taken where ﬁrst
a parametrized controller is synthesized, and then a controller is synthesized overthe parameters to compose the parametric controllers for reachability and safety
speciﬁcations. In [ 10], an approach to parallelize synthesis is presented for the case
when the liveness guarantees correspond to singleton sets. The work here presentsan approach for parallelizing synthesis in a more general setting.
In our work, we adopt a compositional approach where the objective is to allow
for the synthesis procedure to be parallelized. Our approach applies to recurrenceproperties, in addition to safety and reachability. The approach relies on identifying
equivalence classes of states that “look the same” from a control perspective, in that
we can controllably steer the system from any state to any other state within thesame class (thus the name equicontrollable ). The parametric controllers for individ-
ual reachability games are synthesized in parallel to abstract the states corresponding
to the liveness guarantees into sets of equicontrollable states. Following the decom-position into such sets, we abstract away the local transitions to construct a compos-
ite synthesis problem. The performance gain comes from solving the parametrized
reachability games in parallel during the identiﬁcation of the equicontrollable sets.
Each process has its own BDD manager, thus a different kind of parallelization than
a single BDD manager with parallelized operations [ 27].
The main contribution of this paper is an approach to decompose and parallelize
synthesis for the GR(1) fragment. The approach is sound but as a limitation we
do lose completeness as a result of the decomposition. The paper is organized asfollows. Section 2provides background and introduces notation and Sect. 3develops
an example used to illustrate the ideas presented in the paper. Section 4describes
the approach for partitioning a set into subsets of equicontrollable states. Section 5
describes a procedure for setting up the synthesis problem for the composite controller
and employs the synthesized controller to compose the reachability games. This
ensures that the liveness properties are satisﬁed (soundness). Section 6summarizes
the results from benchmarking experiments on planning problems for robots with
mutually exclusive access to critical areas of the workspace.
2 Preliminaries
We use syntax from the raw temporal logic of actions (raw TLA+)[19,20], with
semantics restricted to only declared variables, and variable values restricted to a
ﬁnite set of values Va lu e s (for example {false,true,−5,−4,..., 35}). Temporal
semantics resembles LTL [ 24], from where we borrow one past temporal operator.
Let Va rs denote a ﬁnite set of variable identiﬁers (for example {“x”,“y”}). A state
is a function with domain Va rs and values in Va lu e s , i.e., an assignment of values
to variables. For example, the function state/defines[var∈Va rs /mapsto→3]assigns to every
variable varthe same value, 3. For every “ x”∈Va rs ,state[var]= 3. The set
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 829
of states Σ/defines[Va rs → Va lu e s ]contains all assignments from Va lu e s .L e t Nat
denote the set of natural numbers {0,1,...}.
We write s/llbracketϕ/rrbracketif the formula ϕis true at state s∈Σ(also denoted by s|=ϕ).
For example, the formula (x<2)∧(y=false)is true at the state [x/mapsto→1,y/mapsto→
false]. We use the Boolean operators for conjunction ∧, disjunction ∨, implication
⇒, and equivalence ≡, and the temporal operators eventually 3and always 2.
Let/llbracketϕ/rrbracket/defines{state ∈Σ:state/llbracketϕ/rrbracket}denote the states in Σthat satisfy formula ϕ.
Abehavior is an inﬁnite sequence of states σ∈Behavio rs/defines[Nat→Σ], and
astep is a pair of consecutive states within a behavior. Temporal formulas are
interpreted over behaviors, as follows. A primed variable identiﬁer denotes the
value of a variable in the second state of a step (for example, x/prime), whereas an
unprimed identiﬁer denotes the variable value at the ﬁrst state of a step. A for-mula that does not contain temporal operators (but may contain primes) is called
anaction . For an action Aand a behavior σ,σ,n|=Ameans /angbracketleftσ[n],σ[n+1]/angbracketright/llbracketA/rrbracket,
andσ,0|=2Ameans ∀n∈Nat:σ,n|=A, i.e., every step of σsatisﬁes A.F o r
example, /angbracketleft[x/mapsto→3],[x/mapsto→4]/angbracketright/llbracketx
/prime=x+1/rrbracket. For a formula ϕthat contains tempo-
ral operators, σ,0|=2ϕmeans that every sufﬁx of σsatisﬁes formula ϕ. Dually,
σ|=3Pmeans ¬(σ|=2¬P).
Reactive synthesis can be modeled as a “game” between two players [ 2]. The com-
ponent we design is required to behave as prescribed only as long as its environmentdoes not violate certain assumptions. Different variables represent the component
and the environment. Partition the variables into the sets:
EnvVa rs,SysVars ∈subset Va rs
that represent the environment and component, with EnvVa rs∩SysVars =∅ and
Va rs =EnvVa rs∪SysVars (subset Sdenotes the powerset of S, i.e., all its
subsets). Let the restrictions of assignments be Σ
e/defines[EnvVa rs → Va lu e s ]and
Σa/defines[SysVars → Va lu e s ].
Synthesis of a Mealy implementation is the problem of ﬁnding a function
f∈[(Σ e×Σ×M)→(Σ a×M)]
such that the sequences of states generated by this strategy satisfy a given speciﬁcation
ϕ.M⊆Nat is a ﬁnite set of memory values with a unique initial memory value m0.
For a ﬁnite-memory strategy f, the inﬁnite sequences that occur when using fare
referred to as plays :
Plays (f)/Delta1={σ∈Behavio rs:∃m∈[ Nat→ M]:
∧m[0]= m0
∧∀ k∈Nat:let r/Delta1= k+1args/Delta1=/angbracketleftσ[r]|EnvVa rs,σ[k],m[k]/angbracketright
in/angbracketleftσ[r]|SysVars ,m[r]/angbracketright = f[args]}
where f|Sis the function [x∈S/mapsto→ f[x]].
830 S. Dathathri et al.
As t r a t e g yi s winning for a formula ϕif and only if all plays of fsatisfy the
formula. A state s∈Σis winning if there exists a strategy that is winning if we
replace the initial condition of ϕwith the requirement to start at s.
2.1 Assume-Guarantee Speciﬁcations with GR(1) Liveness
We consider temporal speciﬁcations of an assume-guarantee form, with one general-
ized Streett pair as liveness (i.e., an implication of recurrence properties). For Mealy
implementations, assume-guarantee speciﬁcations are written using the operator [ 17]
Strict Impl (EnvIn it,EnvNext,EnvRecur ,
SysInit ,SysNext ,Sys Recur )/Delta1=
EnvIn it⇒∧ SysInit
∧2(UpToNo w(EnvNext)⇒ SysNext )
∧(2EnvNext)⇒(EnvRecur ⇒ Sys Recur )
The operators EnvIn it,EnvNext,EnvRecur describe the assumption we make
about how the environment behaves, and SysInit ,SysNext ,Sys Recur the require-
ment from the component under design. The operator UpToNo wis borrowed from
LTL, where it is known as historically [23]. This form of speciﬁcation ensures that the
component does not violate its safety constraint by forcing the environment to violateits assumption in the future. The operators EnvIn it,SysInit are state predicates
(initial conditions), and EnvNext,SysNext are actions. For Mealy speciﬁcations,
primed SysVars should not occur in EnvNext .
Let Q(j),R(i)denote state predicates for all indices jand i.I f
EnvRecur
/Delta1=∀ j∈1..m:23 Q(j)
Sys Recur/Delta1=∀ i∈1..n:23 R(i)
then the liveness formula EnvRecur ⇒ Sys Recur is a GR(1) formula. The liveness
formula in an assume-guarantee speciﬁcation of the form Strict Impl determines
the complexity of synthesis [ 2].
2.2 Complexity of GR(1) Synthesis
Polynomial-time algorithms are known for the synthesis of strategies that implement
assume-guarantee GR(1) speciﬁcations. This favorable complexity is why GR(1)
synthesis is often used for high-level reasoning in engineering applications. Symbolic
algorithms allow for reasoning about problems with very large state spaces, because
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 831
they construct strategies by manipulating sets of states, as opposed to enumerative
approaches. Symbolic algorithms for GR(1) synthesis represent sets of states as
(ordered) binary decision diagrams (BDDs) [ 2].
A BDD is a graph-based data structure with a speciﬁc order of Boolean-valued
variables (bits) associated with it. The variable ordering can have a signiﬁcant effect
on the size of a BDD [ 5,6]. Finding a variable order that minimizes the size of a
BDD is an NP-complete problem [ 3,26]. The cost for reordering of BDDs is often
neglected in the complexity analysis of symbolic synthesis algorithms.
A cubic-time symbolic algorithm for GR(1) synthesis is known and commonly
used [ 2]. Memoization can reduce the run time to quadratic, but at the cost of storing
and reordering (in the worst case) nm|Σ|2BDDs (roots) [ 4] (for an implementation
see [ 12]). Reordering these stored BDDs can negate the gains from memoization.
Problem Statement 1 Synthesize a strategy that is winning for a GR(1) formula
ϕin a scalable manner , computing sub-strategies in parallel, and compose those
sub-strategies in a way that implements ϕ.
Besides the obvious gain from parallelization, the decomposition-based approach
presented here also allows us to compute, reorder, and store the BDDs for the sub-
strategies separately, leading to gains in performance.
3 Example
In this section we introduce a simple instance of planning for a domestic mobile
robot that we use to illustrate and develop ideas presented in the paper. Consider the
workspace shown in Fig. 1. The door is controlled by the environment. Rooms with
charging stations have a lightning sign to indicate the same.
Door Open is a proposition that indicates whether the door is open. The robot’s
position is controlled by us and the robot can transition from its current position
through an opening to any of the adjacent rooms. Movement between the Co rrido r
Fig. 1 Workspace for
example in Sect. 3. Slits
represent pathways for the
robot. A door guards the path
between the corridor and the
living room. Lightning sign
in a room indicates the
presence of a charging
station
832 S. Dathathri et al.
and the Living Room requires that the door be open, modeled by Door Open ,a s
follows:
∧{ Door Open ,Co rrido r ,Living Room ,Garage ,Co rrido r }⊆ boolean
∧(Door Open ∧Co rrido r )⇒(Living Room/prime∨Garage/prime∨Co rrido r/prime)
∧(¬Door Open ∧Co rrido r )⇒(Garage/prime∨Co rrido r/prime)
∧(Door Open ∧Living Room )⇒(Living Room/prime∨Co rrido r/prime)
∧(¬Door Open ∧Living Room )⇒(Living Room/prime).
4 Partitioning into Sets of Equicontrollable States
The arguments EnvRecur and Sys Recur of the operator Strict Impl need not
be GR(1) formulas. The Strict Impl operator is useful for deﬁning other kinds
of games too, for example reachability games. When the formula Sys Recur is of
the form 3Ta rg e t where Ta rg e t a state predicate, and EnvRecur istrue , then
the speciﬁcation describes a reachability game. A reachability game takes O(|Σ|2)
symbolic controllable steps ( ∃∀)t os o l v e .
Let G/defines{s∈Σ:s/llbracketTa rg e t /rrbracket}be the set of states that satisfy the predicate
Ta rg e t . Suppose that EnvRecur is a conjunction of recurrence formulas ( 23). Let
WinSe t (G)be the set of states from where a strategy fexists that implements the
formula Strict Impl (EnvIn it,EnvNext,EnvRecur ,SysInit ,SysNext ,SysL)
with3Ta rg e t asSysL . In other words, WinSe t (Ta rg e t )is the set of states from
where the component under design can force the system to transition to Ta rg e t
for any admissible environment behavior. Computing the WinSe t takes O(m|Σ|2)
symbolic steps [ 16].
The winning set can contain states where EnvNext is false, because that case
violates the assumption. In contrast to the winning set, the set of reachable states
contains those states that the assembled system (component plus environment) can
reach. Formally, those states that occur along behaviors that satisfy the formula
EnvIn it∧SysInit ∧2(EnvNext∧SysNext )(if liveness is included, then this
set can shrink). The reachable states can be computed in at most O(|Σ|)symbolic
successor steps ( ∃). For the example, in Sect. 3, states where Of fic e =true∧
Garage =true are unreachable, because the robot can be present in either the
ofﬁce or the garage, but not both.
4.1 Parametrized Games
By computing which states can be controllably reached from which other states, wecan form equivalence classes of states that “look the same” from a control viewpoint.
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 833
Let Prm denote a ﬁnite set of constant identiﬁers, distinct from variables [ 20,p .2 5 ] .
The value of a constant remains unchanged throughout a behavior [ 19,N o t e1 6o n
p. 920]. In BDD computations, bits that correspond to constants are representedby variables that are constrained to remain unchanged. An alternative is to leave
these bits unquantiﬁed. Each variable is mapped to a parameter by the function
f
pr m∈[ Va rs → Prm], which is a bijection. We use the parameters to deﬁne a
speciﬁc state as goal, schematically (with simpliﬁed notation)
Goal/defines/logicalanddisplay
var∈SomeV ars (var=fpr m[var]) (1)
This goal state is not ﬁxed, but depends on the parameter values. For example, if
SomeV ars /defines{“x”,“y”}, and Prm/defines{“px”,“py”}, then Goal/defines(x=px)∧(y=
py). To map parameter assignments to states with same values for the corre-
sponding variables, let PrmT oSt (p,V)/defines[var∈V/mapsto→ p[fpr m[var]]], and the
inverse from state to parameter assignments StT oPrm (s,P)/defines[pr m∈Prm/mapsto→
s[f−1
pr m[pr m]]]. We use these mappings also for assignments to subsets of vari-
ables and parameters, by passing V⊂Va rs and P⊂Prm . For simplicity, we dis-
cuss in terms of the Cartesian product of Σwith the set of parameter assignments
/Pi1/defines[Prm→ Va lu e s ].
Solving a game with the parametric liveness objective 3Goal yields a parametric
set of winning states, WinSe t (Goal)⊆Σ×/Pi1. For each valuation of parameters
p∈[ Prm→ Va lu e s ], for each state s∈Σ,i ti s/angbracketlefts,p/angbracketright∈ WinSe t (Goal)if, and
only if, we can start from sand controllably reach the state t/definesPrmT oSt (p,Va rs).
The parametric set WinSe t (Goal)can be computed in O(m|Σ|2)symbolic steps,
as follows from the corresponding μ-calculus formula [ 16, Lemma 9]. For each
assignment of values to the parameters in Prm , the parametrized game corresponds
to solving a non-parametric game with at most Σreachable states. Thus, the symbolic
set operations can be viewed as operating on copies of the same transition system, for
different valuations of the parameters. So the number of symbolic steps needed forsolving each non-parametric game is limited by the same upper bound. However, the
symbolic steps themselves are more expensive due to the added parameters, which
increases the size of BDDs. In other words, the parametric game corresponds to
solving multiple non-parametric games side-by-side . The parametrization we use
is similar to previous work on reachability games [ 1], with the difference that we
include liveness for the environment, as recurrence properties in EnvRecur .
4.2 Partitioning the States into Equicontrollable Sets
We use the parametric winning set WinSe t (Goal)from Sect. 4.1to group states into
sets that are controllably reachable from each other. Let SomeV ars ⊆Va rs .A par-
tial state is a function s∈[ SomeV ars → Va lu e s ].L e t SomePrms /defines
{fpr m[var]:var∈SomeV ars }, and
834 S. Dathathri et al.
CanGoTo (a,b)/defines∀s∈Σ:∨(s|SomeV ars /negationslash=a)
∨/angbracketleft s,StT oPrm (b,SomePrms )/angbracketright∈ WinSe t (Goal).
We call a pair of partial states /angbracketlefts1,s2/angbracketrightequicontrollable if and only if, from any state
sthat agrees with s1onSomeV ars , the agent can force the execution to some state
that agrees with s2onSomeV ars , and vice versa, i.e.,
Ar e Equi contr ollable (s1,s2)/definesCanGoTo (s1,s2)∧CanGoTo (s2,s1).
Problem Statement 2 Given a state predicate ξ, partition the set of states {s∈Σ:
s/llbracketξ/rrbracket}into equivalence classes of equicontrollable states over the variables in
SomeV ars ⊆Va rs .
In this paper, we let SomeV ars be the set of variables that the formula ξdepends
on, also known as the support variables (in BDD terminology). We restrict attention
to only the subset of variables SomeV ars for two reasons. Firstly, formulas of
recurrence goals in Sys Recur typically depend on only a few variables. So the
satisfaction of those goals is insensitive to the values of other variables. For example,if the liveness guarantee is 23(Living Room ∨Garage )in our running example,
and we declare a variable We athe r , the value of this variable is irrelevant to this
goal. The second reason is that too ﬁne-grained distinction between states leadsto unnecessary fragmentation of equivalence classes. By omitting We athe r from
the set SomeV ars , states with the robot in the living room belong to the same
equivalence class, even though they may differ in what the weather conditions are
outside the house. The robot has no control over the weather conditions, but this
fact can be ignored when considering whether the goal Living Room ∨Garage has
been reached. So viewed over Va rs (instead of SomeV ars ), not every pair of states
need be equicontrollable. For a speciﬁc application, domain knowledge could guide
the selection of the set SomeV ars to be different from the support variables of ξ.
The algorithm of Fig. 2describes how the partitioning problem is solved. As live-
ness guarantee, we use the formula NewGoal/definesξ∧Goal . This goal is the objective
of the parametric game deﬁned by Strict Impl (... ,3NewGoal). The algorithm
splits the set of states into equicontrollable classes by iterating through assignments
of values to parameters in the set SomePrms /defines{f
pr m[var]:var∈SomeV ars },
i.e., only those parameters that correspond to variables in SomeV ars . For simplicity,
we use Va lu e s for all variables and parameters, though the implementation has dif-
ferent ranges of values for each variable. This partitioning requires O(k|Σ SomeV ars |)
evaluations, where kis the number of classes. Furthermore, while iterating over sets
of states, we can eliminate spurious classes by ignoring those sets that are disjoint
from the set of reachable states.
Example 1 For the workspace in Sect. 3, we want to partition the set of states where
the robot is in a room with a charging station into equivalence classes. ξhas the form
ξ=Of fic e ∨Living Room ∨Garage .This formula speciﬁes whether the robot
is in a room with a charging station. Denote the set of variables that ξdepends on by
X/defines{Of fic e ,Living Room ,Garage }.
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 835
Fig. 2 Separating into equicontrollable classes
Suppose that the behavior of the door is unconstrained. This yields that the garage
and ofﬁce are in the same equicontrollability class, whereas the living room is in
a different class. If we assume that the door opens inﬁnitely often EnvRecur /defines
23 Door Open ), then the living room is in the same equicontrollability class as the
ofﬁce and garage.
5 Synthesizing a Composite Controller
This section describes an approach to build a transition system and a speciﬁcation
such that the winning strategy for this system can be used to compose the sub-
strategies that are synthesized and stored in parallel to ﬁnd a strategy winning against
a GR(1) speciﬁcation.
Example 2 For the workspace from Sect. 3, consider a synthesis problem where the
robot has to patrol the dining room and the music room inﬁnitely often, while making
sure to visit a room with a charging station inﬁnitely often and the robot is initially
in the dining room. The liveness guarantees to be satisﬁed are:
23 Dining Room ,23(Of fic e ∨Garage ∨Living Room ),23 Music Room .
836 S. Dathathri et al.
Fig. 3 Supervisory transition system for different assumptions about the environment
We ﬁx the ordering of liveness guarantees ( Dining Room ,Of fic e ∨Garage ∨
Living Room ,Music Room ) and build a new transition system, shown in Fig. 3.F o r
each liveness guarantee, there is a node in the transition system that corresponds
to a subset of classes arising from decomposition of the set of states that satisfy
the liveness guarantee. We add transitions between these states if every state in the
predecessor node can reach some state in the successor node.
If the liveness behavior of the environment is modeled as EnvRecur /definesTRUE ,
we get the abstracted supervisory transition system shown in Fig. 3a. For the
liveness guarantee Of fic e ∨Garage ∨Living Room , the classes are {Of fic e ∨
Garage ,Living Room }. The transition system in Fig. 3a has states corresponding
to all non-empty subsets of these classes. If we assume that the door opens inﬁnitely
often, EnvRecur /defines23 Door Open , then we obtain the supervisory transition sys-
tem shown in Fig. 3b. Both transition systems have a cycle that can be used to compose
the sub-strategies. The construction of the transition system and the composition of
the sub-strategies is described below.
5.1 Synthesizing a Supervisory Controller
Consider a GR(1) formula Strict Impl . The states corresponding to the liveness
guarantees ψaifor the agent (within Sys Recur ) are partitioned into equicon-
trollable classes as described in Sect. 4. For each i∈1..n,l e t kibe the num-
ber of classes ψaiis partitioned into. Let Λi/defines{αi,1,αi,2,...,αi,ki}be the set of
classes associated with ψai.L e tΩi/defines(subset Λi)\∅. Note that |Ωi|=2ki−1.
Without loss of generality, we ﬁx some ordering of the elements in Ωisuch
thatΩi={Ωi,1,Ω i,2,...,Ω i,2ki−1}.L e t AΩi,jdenote the WinSe t forΩi,j, i.e.
AΩi,j=WinSe t (Ω i,j). For the case in Example 2where the door is not assumed
to open inﬁnitely often, the classes that correspond to the speciﬁcation where the
robot has to inﬁnitely often visit a room with a charging station are Λ2={ Of fic e ∨
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 837
Garage ,Living Room }andΩ2=/braceleftBig
{Of fic e ∨Garage },{Living Room },{Of fic e
∨Garage ,Living Room }/bracerightBig
.
Note that if/vextendsingle/vextendsingleΩi,j/vextendsingle/vextendsingle=1 we can use the parametric WinSe t computed for decom-
posing [[ψai]]into equicontrollable classes to obtain the WinSe t forΩi,j, by setting
values to the parameters appropriately. For Ωi,jwith|Ωi,j|>1, we compute Win-
Set(AΩi,j) by solving a reachability game with Ωi,jas the goal to be reached. As
these computations are independent, they can be performed in parallel. The symbolicsteps here are less expensive than that for ﬁnding the WinSet set of [[ψ
ai]], because
we are only considering a subset of the set of states satisfying the liveness constraint
[[ψai]]and hence there are fewer transitions to be accounted for in each symbolic
step.
Following this setup, a hierarchical game is constructed as follows. The set of
additional Boolean-valued variables is AP/defines{ρi,ji:i∈1..n,ji∈1..2ki−1}.T h e
transition rule is speciﬁed as:
ρcmp/defines/logicalordisplay
v
v∈AP∧/logicalanddisplay
i∈1..n,
j∈1..2ki−1/parenleftBig
ρi,j⇒/logicalordisplay
Ωi,j⊆AΩk,l,k∈{i,i⊕n1},
l∈1..2ki−1ρ/prime
k,l/parenrightBig
.( 2)
Here/logicalortextis the exclusive-or operator. In Eq. ( 2), we allow for a transitions between
the states {ρi,j}and{ρk,l}only if the set Ωi,jis contained in the WinSe t of the
setΩk,l. For the transition system in Fig. 3a, the Dining Room is in the WinSe t of
Living Room ∨Of fic e ∨Garage but Living Room ∨Of fic e ∨Garage is not
in the WinSet ofMusic Room . The transition relations reﬂect the same. Similarly,
transition relations are constructed between the other states.
Note that we restrict k∈{ i,i⊕ n1}, ensuring that only those transitions are cho-
sen that either stay in the same liveness guarantee or lead to the next liveness guar-
antee. Thus, we avoid cycling back to a liveness guarantee that was visited earlier inthe current cycle.
The liveness guarantees ensure that repeatedly, for each i∈1..n,ρ
i,jis satisﬁed
for some j, and can be written as ψcmp
i/defines/logicalortext
j∈1..2ki−1ρi,j. This ensures that at least
one of the classes corresponding to a liveness guarantee is visited in each cyclethrough the liveness guarantees. For a particular i, satisfying ψcmp
iis equivalent to
satisfying ψaiin the original system.
Note that there is no environment here, and we only need to search for a cycle
passing through all the liveness guarantees for a given set of initial states. Consider
some i∈1..n. The set of valid initial states are those nodes that correspond to the
elements in Ωifor which EnvIn it∧SysInit lies in their WinSe t . The initial con-
dition can be written as (for some i)θcmp/defines∨
θ∈AΩi,jρi,j. Conjoining the speciﬁcations
above, we need to ﬁnd a controller that implements the formula
θcmp∧/squareρcmp∧/logicalanddisplay
i∈1..n/square♦ψcmp
i.( 3)
838 S. Dathathri et al.
Finding a winning strategy fcmp∈[ΣAP×Msup→ΣAP×Msup]for the above
speciﬁcation gives a supervisory controller for composing the strategies for the reach-
ability games.
5.2 Assembling the Sub-strategies
Deﬁne fk,l
i,l∈[Σe×Σ×Mk,l
i,l→Σa×Mk,l
i,l]to be the strategy that takes the
agent from a state in Ωi,ltoΩk,l.L e t mk,l
i,jbe the initial memory value for
Mk,l
i,l. Without loss of generality, assume Mk1,l1
i1,l1∩Mk2,l2
i2,l2=∅ when/angbracketlefti1,j1,k1,l1/angbracketright/negationslash=
/angbracketlefti2,j2,k2,l2/angbracketright.L e t kmax/definesmax({ki:i∈1..n}), i.e., kmaxis the size of the largest
number of equicontrollable classes for any of the liveness classes. Let M/definesMsup×
∩
i,j,k,lMk,l
i,jandξcomp/defines1..N×1..kmax×1..N×1..kmax. We construct a strategy
fcompose∈[Σe×Σ×ξcomp×M→Σa×ξcomp×M]that uses fcmpto compose
the strategies for the reachability games ( fk,l
i,l)
fcompose[x,s,/angbracketlefti,j,k,l/angbracketright,/angbracketleftw,w sup/angbracketright] = /angbracketleft y,/angbracketlefti/prime,j/prime,k/prime,l/prime/angbracketright,/angbracketleftw/prime,w/prime
sup/angbracketright/angbracketright
where if s∈Ωk,l, then
∧/angbracketleft y,w/prime/angbracketright= fk,l
i,j[x,s,mk,l
i,j],
∧/angbracketleft {ρk/prime,l/prime},w/prime
sup/angbracketright= fcmp[{ρk,l},w sup],
∧/angbracketlefti/prime,j/prime/angbracketright=/angbracketleft k,l/angbracketright.else∧/angbracketleft y,w/prime/angbracketright= fk,l
i,j[x,s,w]
∧/angbracketlefti/prime,j/prime,k/prime,l/prime/angbracketright=/angbracketleft i,j,k,l/angbracketright
∧w/prime
sup=wsup(4)
For the case s/∈Ωk,l, while we are moving towards Ωk,l, the values are updated
according to the strategy fk,l
l,j. Once we reach Ωk,l(s∈Ωk,l), the next goal is updated
according to fcmpand we continue towards the next goal, switching goals again once
the next goal is reached.
Theorem 1 Strategy fcomposeis sound. Finding an implementation of Eq. ( 3) takes
O((2kmax)2n3)symbolic steps.
Proof Sketch: By construction, a winning strategy in the original system was com-
puted corresponding to every transition in the abstracted system, i.e. the agent caneither force the execution to the next liveness guarantee or block the environment from
satisfying the assumption on its behavior. A winning strategy for the abstracted sys-
tem ﬁnds an execution that cycles through the liveness guarantees. Cycling throughthe liveness guarantees in the abstracted system corresponds to cycling through live-
ness guarantees in the original system. Hence, composing the strategies from the
reachability games in accordance with the composite controller ensures satisfactionof the original GR(1) formula.
Equation ( 3) is a GR(1) formula in absence of an environment, hence the innermost
ﬁxpoint associated with blocking the environment from satisfying its recurrence
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 839
objectives is absent. The total number of states is (n2kmax), and there are nliveness
guarantees, resulting in O((2kmax)2n3)symbolic steps [ 13].
For applications where the number of liveness guarantees and the number of
equicontrollable classes are much smaller than the total number of states, i.e. n/lessmuch|Σ|
and k/lessmuch|Σ|, the parallelized approach presented here is well-suited and should result
in performance gains in term of computation time.
Limitations
The presented algorithm is not complete, because transitions can be lost during the
abstraction into a supervisory transition system. Another limitation of the approach
is that if we end up with a large number of equicontrollable classes, computing the
compositional strategy can become intractable.
6 Experiments
We consider a multi-agent robot motion planning problem where the objective is to,
for a set of robots, schedule access to critical sections of a given workspace in a safe
manner. The environment consists of an uncontrolled adversarial mobile robot that
is functioning in the same workspace as the controlled robots and requires access to
certain critical sections of the workspace. An example instance is shown in Fig. 4.
Both the controlled robots and the uncontrolled robot have to visit cells shaded with
each of the three colors (red and green) inﬁnitely often. The problem instances are
Fig. 4 Workspace with
shaded obstacles (black) and
critical sections (green and
red)

840 S. Dathathri et al.
Fig. 5 Performance on benchmark experiments—mean runtimes over 50 randomized problem
instances are reported, error bars indicate standard deviation
parametrized in terms of the size of the workspace, the number of critical sections
and the number of controlled robots. The colored cells represent critical sections
of the workspace that must be accessed in a mutually exclusive manner and each
color represents a critical resource of a type. While the adversarial robot is accessing
a critical section, the controlled robots must not access the same critical section.
Similarly, no two controlled robots can access a critical section at the same time. The
adversarial robot’s access to the critical sections is prioritized over the controlled
robots. When the adversarial robot attempts to access a critical space, the controlled
robots as a part of their safety requirement must allow the adversarial robot to gain
access by vacating the critical section. The regions shaded black (density =5%)
represent static obstacles and both the uncontrolled and controlled robots must avoid
the obstacles. The robots are allowed to transition to any of their non-diagonally
adjacent cells in a single step. The uncontrolled robot is allowed to pursue a trajectory
of its choice and the only assumption on its behavior, in addition to the constraints
on its motion, is that the uncontrolled robot will access cells shaded with each of the
colors inﬁnitely often.
Figure 5a and b report performance over problem instances of varying size. The
mean time over 50 problem instances is reported. For each of these instances the
initial positions of the robots, the positions of the obstacles and the critical sections are
randomized. The computations were performed on a 32 core AMD Opteron machine
at 2.4 GHz with 96 GB of RAM, and we see considerable gains in computation time
for the parallelized approach. The implementation is written in Python using the
packages dd,omega [15], and tulip [14].1A possible explanation for the large
variance is the cost of reordering the BDDs, which can show large variance depending
on the structure of the formula for which the reordering is being done. For the case
1Available from PyPI: https://pypi.org/project/dd/ ,https://pypi.org/project/omega/ ,a n d https://
pypi.org/project/tulip/ .
Parallelizing Synthesis from Temporal Logic Speciﬁcations … 841
of the varying number of critical sections, the number of equivalence classes roughly
increases linearly with the number of critical sections. Hence, for the parallelized
approach, the dependence on the number of critical sections is smaller.
7 Conclusions and Future Work
A major challenge to the widespread adoption of formal methods is their scalabil-ity. As systems get larger and complex, scalable algorithms that can deal with the
size and complexity of these systems are necessary. In this regard, we present an
approach that allows us to decompose and parallelize the synthesis algorithm for theGR(1) fragment of linear temporal logic. The approach relies on the construction of
a composite strategy that is used to compose local strategies to ensure satisfaction
of the GR(1) speciﬁcation. However, the approach comes with certain drawbacksas outlined earlier. Empirical evidence demonstrating the resulting gains in perfor-
mance is presented for a robot motion planning problem, where in addition to path
planning, safe access to certain critical sections of the workspace is scheduled.
Future work would be to explore if a similar approach can be used to synthesize
policies that can handle uncertainty, because some local uncertainty can be toleratedwithout a resynthesis of the entire strategy by applying a local correction. This way
certain unexpected disturbances can be handled locally, without the need for global
synthesis. A hierarchical framework as presented here could be used in such settings.Another direction for future work includes exploring the possibility of a symbolic
approach for decomposition of sets into equicontrollable classes as opposed to the
enumerative approach considered here.
Acknowledgements This work is sponsored in part by TerraSwarm, a funded center of STARnet,
a Semiconductor Research Corporation (SRC) program sponsored by MARCO and DARPA.
References
1. Alur, R., Moarref, S., Topcu, U.: Compositional synthesis with parametric reactive controllers.
In: Proceedings of HSCC, pp. 215–224 (2016)
2. Bloem, R., Jobstmann, B., Piterman, N., Pnueli, A., Sa’ar, Y .: Synthesis of reactive(1) designs.
J. Comput. Syst. Sci. 78, 911–938 (2012)
3. Bollig, B., Wegener, I.: Improving the variable ordering of OBDDs is NP-complete. IEEE
Trans. Comput. 45(9), 993–1002 (1996)
4. Browne, A., Clarke, E., Jha, S., Long, D., Marrero, W.: An improved algorithm for the evaluation
of ﬁxpoint expressions. Theor. Comput. Sci. 178(1), 237–255 (1997)
5. Bryant, R.E.: Graph-based algorithms for Boolean function manipulation. IEEE Trans. Comput.
35(8), 677–691 (1986)
6. Bryant, R.E.: On the complexity of VLSI implementations and graph representations of Boolean
functions with application to integer multiplication. TOC 40(2), 205–213 (1991)
842 S. Dathathri et al.
7. Chinchali, S., Livingston, S.C., Topcu, U., Burdick, J.W., Murray, R.M.: Towards formal syn-
thesis of reactive controllers for dexterous robotic manipulation. In: Proceedings of ICRA, pp.5183–5189 (2012)
8. Cohen, A., Namjoshi, K.S., Sa’ar, Y .: SPLIT: a compositional LTL veriﬁer. In: CA V , pp. 558–
561 (2010)
9. Cohen, A., Namjoshi, K.S., Sa’ar, Y ., Zuck, L.D., Kisyova, K.I.: Parallelizing a symbolic
compositional model-checking algorithm. In: Proceedings of HVC, pp. 46–59 (2010)
10. Dathathri, S., Murray, R.M.: Decomposing GR(1) games with singleton liveness guarantees
for efﬁcient synthesis. In: Proceedings of CDC, pp. 911–917 (2017)
11. DeCastro, J.A., Alonso-Mora, J., Raman, V ., Rus, D., Kress-Gazit, H.: Collision-free reactive
mission and motion planning for multi-robot systems. In: Proceedings of ISRR, pp. 459–476(2015)
12. Ehlers, R., Raman, V .: Slugs: extensible GR(1) synthesis. In: Proceedings of CA V , pp. 333–339
(2016)
13. Emerson, E.A., Lei, C.: Efﬁcient model checking in fragments of the propositional mu-calculus.
In: IEEE Computer Society Press LICS, pp. 267–278 (1986)
14. Filippidis, I., Dathathri, S., Livingston, S.C., Ozay, N., Murray, R.M.: Control design for hybrid
systems with TuLiP: the temporal logic planning toolbox. In: Proceedings of CCA, pp. 1030–
1041 (2016)
15. Filippidis, I., Murray, R.M., Holzmann, G.J.: A multi-paradigm language for reactive synthesis.
In: 4
thwork. on synthesis, SYNT, pp. 73–97 (2015)
16. Kesten, Y ., Piterman, N., Pnueli, A.: Bridging the gap between fair simulation and trace inclu-
sion. Inf. Comput. 200, 35–61 (2005)
17. Klein, U., Pnueli, A.: Revisiting synthesis of GR(1) speciﬁcations. In: Proceedings of HVC,
pp. 161–181 (2010)
18. Kress-Gazit, H., Fainekos, G.E., Pappas, G.J.: Where’s Waldo? sensor-based temporal logic
motion planning. In: Proceedings of ICRA, pp. 3116–3121 (2007)
19. Lamport, L.: The temporal logic of actions. TOPLAS 16(3), 872–923 (1994)
20. Lamport, L.: Specifying Systems. Addison-Wesley, Boston (2002)21. Majumdar, R.: Robots at the edge of the cloud. In: Proceedings of TACAS, pp. 3–13 (2016)22. Maniatopoulos, S., Schillinger, P ., Pong, V ., Conner, D.C., Kress-Gazit, H.: Reactive high-level
behavior synthesis for an Atlas humanoid robot. In: Proceedings of ICRA, pp. 4192–4199(2016)
23. Manna, Z., Pnueli, A.: A hierarchy of temporal properties. In: Proceedings of PODC, pp.
377–408 (1990)
24. Pnueli, A.: The temporal logic of programs. In: Proceedings of FOCS, pp. 46–57 (1977)25. Pnueli, A., Rosner, R.: On the synthesis of a reactive module. In: Proceedings of POPL, pp.
179–190 (1989)
26. Rudell, R.: Dynamic variable ordering for ordered binary decision diagrams. In: Proceedings
of ICCAD, pp. 42–47 (1993)
27. van Dijk, T., Laarman, A., van de Pol, J.: Multi-core BDD operations for symbolic reachability.
ENTCS (PDMC’12) 296, 127–143 (2013)
28. Wolff, E.M., Murray, R.M.: Optimal control of nonlinear systems with temporal logic speciﬁ-
cations. In: Proceedings of ISRR, pp. 21–37 (2013)
Fast Action Elimination for Efﬁcient
Decision Making and Belief SpacePlanning Using Bounded Approximations
Khen Elimelech and Vadim Indelman
1 Introduction
Autonomous decision making is a fundamental problem in artiﬁcial intelligence and
robotics, in which one wishes to select the action which maximizes some objectivefunction. When solved iteratively, in a context of a speciﬁc problem with a goal
deﬁnition, the problem turns into a planning problem. A solution to this problem
must take into consideration many aspects such as goal setting, deﬁnition of can-
didate actions, accounting to different planning horizons and future developments,
coordination of agents and so on. While these aspects can be optimized in the con-text of the discussed problem, basic decision making, the examination of a group
of candidate actions under the objective measure, is still the common ground to all
planning problems. Unfortunately, little work has been done on trying to optimizethis elementary process, and even if the planning aspects are optimized, its solution
is still usually done naively with an exhaustive calculation of the objective function
for all candidates.
In our previous work [ 1–3], we raised the idea that the basic decision making
process can be done more efﬁciently. There, we deﬁned a set of fundamental notions
to allow us to compare states in the context of the decision making. As far as weknow, there were no prior attempts to do so beforehand. There we introduced the
concept of action consistent state approximations, in order to to solely and directly
reduce the complexity of the decision making, while maintaining the same action
K. Elimelech ( B)
Robotics and Autonomous Systems Program, Technion - Israel Institute of Technology,32000 Haifa, Israel
e-mail: khen@campus.technion.ac.il
URL: https://www.khen.io
V . Indelman
Department of Aerospace Engineering, Technion - Israel Institute of Technology,
32000 Haifa, Israel
e-mail: vadim.indelman@technion.ac.il
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_58843
844 K. Elimelech and V . Indelman
a\* a'a\*
s
Action05101520RevenueJ(b,a) - Real objective values
J(bs,a) - Approximated objective values
(b,bs,a) - Simplification offset (of an action)
Fig. 1 This conceptual ﬁgure demonstrates a relation between two states b,bsin the context of
decision making. The dark blue graph shows the real objective values, based on the original state b,
while the light blue graph shows the approximation of these values, based on the approximated state
bs. The states are not action consistent , since the graphs do not preserve the same trend. Hence, a∗
is the real best candidate action, and a∗
sis the best candidate action based on the approximation.
The green bars stand for the simpliﬁcation offset between the states, for each action. The maximal
offset, the one of action a/prime, is the overall simpliﬁcation offset between the two states. Obviously,
without action elimination, in order to solve the problem, all the real objective values (the entire
dark blue graph) have to be calculated
selection (Fig. 1). Despite involving state approximations, no dilution of information
is done from the maintained state in the inference process, as the approximations
are only used in candidate examination. In this work we wish to further demonstrate
that this framework of deﬁnitions and qualitative analysis is worthwhile, by using
it to create a new candidate-elimination paradigm to efﬁciently solve the decision
making problem, in a general context. In contrast to our fundamental papers, where
we solved an approximated problem, and then analyzed its consequences, this time
we utilize state approximation to create a tool to be used in the solution of the original
problem.
Here, we depend on the intuitive conclusion that in most cases, we can easily
realize when an action is clearly “bad”. For example, in the context of navigation to
a given goal, backtracking away from the goal is (usually) undesired, and there is
no need to invest in the calculation of how “bad” it is, when there are clearly better
options. Thus, we should thoroughly examine only the subset of candidates which
seems to be “good”. Our method suggests using easy-to-calculate approximation of
the objective function to identify unﬁt candidates, which, using our previous notions,
can be guaranteed not to be the desired one, and eliminate them. The problem can
then be efﬁciently solved considering a smaller subset of candidates, while sparing
Fast Action Elimination for Efﬁcient Decision Making 845
a\*a\*
s
Action05101520Revenue
J(b,a) - Real objective values
J(bs,a) - Approximated objective values
Elimination interval limits
Fig. 2 This ﬁgure demonstrates the application of our action elimination method on the scenario
described in Fig. 1. The light blue graph of the approximated objective values is easily calculated,
and the approximated best action a∗
sis found. Using the simpliﬁcation offset (known in this case,
or bounded in the general case) and Eq. 8from Theorem 1, the elimination interval, bounded by the
two red lines, is calculated. J(bs,a∗), the approximated objective value of the real best candidate
action, must be in this interval. Thus, all the actions for which the objective approximation J(bs,a)
(in light blue) is below the lower red line, can be eliminated (the dashed section). Now, to solve
the problem and ﬁnd a∗, we should only calculate the real values J(b,a)of the remaining actions
(solid dark blue)
the need to calculate the exact objective values for each one (Fig. 2). By doing so,
under real-time constraints, we are able to examine more complicated candidate
actions, or use a higher resolution to discretize a continuous action domain, leading
to an indirect improvement of solution quality as well. Moreover, since the method
does not depend on any speciﬁc planning problem, it can be added “on top” of other
methods, after the planning aspects were optimized, with no contradiction.
As a highly relevant case study, we extend the discussion to the sub-problem
of decision making under uncertainty and belief space planning. Relevant planning
problems include sensor deployment, for which we demonstrate the approach, and
also robotic arm manipulation, autonomus navigation and SLAM. For the last one,
improving the computational cost is especially important, as it is often required to
solve this problem online and on limited hardware (such as quadcopters) [ 4]. Also,
as the state size tends to grow over time in such problems, long-term autonomy is
another challenge. In this context, several methods achieve indirect improvement in
planning efﬁciency by using active sparsiﬁcation of the maintained state [ 5,6], but
thus sacriﬁce the quality of solution of the state inference and by such also of the
planning. Our method turns out superior in that matter, by being independent of the
inference process, in addition to not affecting the quality of action selection.
846 K. Elimelech and V . Indelman
Numerous studies have also attempted to directly optimize planning processes,
although almost always in the context of speciﬁc problems, by adjusting and opti-
mizing aspects in the representation of the problem; e.g., in the context of decisionmaking under uncertainty and belief space planning [ 7–9], and speciﬁcally SLAM
[10], path planning [ 11,12], sensor deployment [ 13] and robotic arm manipulation
[14]. Some papers have examined action elimination in the context of planning prob-
lems. In [ 15] actions are eliminated from a constructed plan, comprised of a sequence
of actions, in order to reduce its execution complexity, and not the planning complex-
ity. For learning problems, a few papers [ 16–18] have attempted to remove actions
from consideration by transferring previous learned knowledge across learning tasks.
Our method requires no previous knowledge, and not even for the decision making
to be sequential. Several studies [ 19,20] have utilized bounds for action elimination,
in a similar way our approach, yet these bounds are tightly coupled with their for-
mulation for planning under the limited MDP framework. In the context of motion
planning in the belief space, pruning of the planning tree has been applied in orderto remove unpromising candidate paths [ 21]. Overall, none of these approaches rely
solely on the general decision problem, making these applications limited to the dis-
cussed scenario. Being applicable for different problems is the main advantage of
our method, which also allows, as previously stated, to apply it with no contradiction
alongside other optimizations or pruning methods.
2 Problem Formulation
2.1 Decision Making
Let us ﬁrst formally deﬁne the general decision problem. Starting from an initial state
b, we measure the value of an action a, using an objective function J(b,a).T h i s
function can measure either the expected value of the posterior state, or the value of
the action itself, or a combination of these values. Given a set of candidate actionsA, we look for the action which maximizes this function. Thus, the problem can be
formulated as
a
∗=argmax
a∈AJ(b,a). (1)
According to this basic problem deﬁnition, we are required to calculate the value
of the objective function per action. Yet, in many real world problems, especially
when discussing a discretization of a continuous action domain, as common to doin navigation, or object manipulation, the set of candidate actions can grow very
large. Also, in some cases, such as decision making under uncertainty and belief
space planning in high dimensional states, calculation of this objective functionis computationally expensive, as it requires calculating information measures (e.g.
entropy or information gain) and by such, handling high dimensional covariance or
information matrices.
Fast Action Elimination for Efﬁcient Decision Making 847
When planning in the belief space, we assume the states, actions, and observations
are stochastic. Hence, to describe a sequential planning process (POMDP) at time k,
we use the posterior distribution over the state vector Xk
bk.=P(Xk|a0:k−1,z0:k)∼N(ˆXk,Λ−1
k), (2)
where a0:k−1.={a0,..., ak−1}and z0:k.={z0,..., zk}are the actions and observa-
tions taken until time k, respectively. To describe such a distribution we use the term
belief . The transition and observation models can be described probabilistically as
P(xk+1|xk,ak)and P(zk|Xk), respectively. We assume that both models and the
beliefs are normally distributed.
When applying a new action akover belief bk(and then taking a new observation),
its information matrix Λkis updated according to the corresponding transition and
observation models. Due to the additive quality of the information form, such update
can be compactly described as
Λk+1=Λk+AT
kAk, (3)
where the collective Jacobian Akencapsulates all the new information in a single
term. This Jacobian can hold information (or new factors, in a factor graph represen-tation) regarding the action, or the sequence of actions, for the nonmyopic scenario,
and the following observations. Representation of an update in such form can be done
for every candidate action (for more details see [ 1]). We will use this representation
throughout the paper.
In this context, we wish to minimize the uncertainty in the future belief, or equiv-
alently maximize the information gain in the posterior belief. Thus, we deﬁne thefollowing objective function (although other objective deﬁnitions may also be appli-
cable):
J(b
k,ak).=|Λk+1|=|Λk+AT
kAk|, (4)
Calculation of the objective values requires computing the determinant of the poste-
rior information for each candidate action. We recall that a single determinant eval-uation of a matrix of size n×nis valued at O(n
3), for the general case. Although
this computation can be done efﬁciently for sparse matrices (a quality which is often
utilized by state of the art code frameworks), it is still considerable when discussinghigh dimensional states.
The variables in the state vector are deﬁned with relation to the discussed problem,
e.g. for a sensor deployment problem, it can include the measured values from the
placed sensors; for a SLAM problem it can include previous poses of the agent and
the locations of observed landmarks. In that case, the dimension of the state vectorand information matrix can rapidly grow such that revenue calculation becomes
challenging to do in real time.
848 K. Elimelech and V . Indelman
2.2 Goal
As demonstrated, solving the decision problem may encapsulates a high computa-
tional cost, caused by an expensive objective function and/or numerous candidateactions. We wish to select the best candidate action without the explicit and exhaus-
tive calculation of all of the objective values. Of course, we want to minimize any
effect over the action selection, in order to maintain the original quality of solution,as our focus is on reducing the computational cost of the problem.
3 Approach
3.1 Action Elimination
As stated, solution of the decision making problem requires calculation of the revenue
function for each candidate action, given an initial state b. Alternatively, say we were
to calculate the revenues of the same set of candidate actions, while considering adifferent initial state b
s(sis used to mark a simpliﬁed or sparse version of b).
Considering these two initial states, if all the respective pairs of revenues were equal,
then the selected candidate action, the one with the highest revenue, would be the
same in both cases. To describe this tight correlation, we say that bandbsareaction
consistent . When this situation is guaranteed, we could equivalently examine the
actions using either of these states. Formally:
Deﬁnition 1 Consider a set of actions Aand an objective function J(state,action )
(these notations will also be relevant for the deﬁnitions to follow). Two states b,bs
areaction consistent , in relation to Jand A, and marked b/similarequalbs, if the following
applies ∀ai,aj∈A:
J(b,ai)< J(b,aj)⇐⇒ J(bs,ai)< J(bs,aj) (5)
In other cases, when there is a difference in the objective values of some actions, the
action selection might be affected. To examine this situation, we suggest the simpli-
ﬁcation offset as a “metric” between such states, in the context of decision making.
Naturally, when the offset between the states is zero, they are action consistent.
Deﬁnition 2 The simpliﬁcation offset of an action a∈Ais deﬁned as:
γ(b,bs,a).=|J(b,a)−J(bs,a)| (6)
Fast Action Elimination for Efﬁcient Decision Making 849
The simpliﬁcation offset between the two states is deﬁned as:
γ(b,bs).=max
a∈Aγ(b,bs,a) (7)
Please note, Deﬁnitions 1and 2, were initially deﬁnied in our previous work [ 1], but
due to their extensive use throughout the paper, are brought here again, in order to
allow ﬂuent reading. These deﬁnitions are demonstrated in Fig. 1.
While it appears that calculating this offset essentially requires calculation of all
objective values, and thus actually solving the problem, in many cases it can still be
bounded without actually calculating them, but by considering the structure of the
state, actions, and objective. An example of calculating such a bound is to appearahead.
Ifb
srepresents an approximation or simpliﬁcation of the original state b, then for
each action a, we can say that J(bs,a)represents an approximation of the exact value
of this action. By using a simpliﬁed, or sparse bs, the approximated values should be
easier to calculate than the real ones. For intuition, when the offset between states is
small enough, then the action selection is “similar” in both cases. Therefore, since
our goal is to improve the computational complexity of the problem, with minimal
effect on the action selection, utilizing such an approximation may be worthwhile.
When approximating the objective using bs(which is not action consistent),
although they are expected to be faster to calculate than the values, we might not
achieve the highest potential value, due to selection of a sub optimal action. To makeany valuable conclusion, we should seek a formal connection between the approxi-
mated objective values and the exact ones.
Theorem 1 (Elimination Interval) For two states b ,b
s, and an objective function
J(state,action ):
J(bs,a∗
s)−2·γ(b,bs)≤ J(bs,a∗)≤ J(bs,a∗
s)wherea∗=argmax
aJ(b,a)
a∗
s=argmax
aJ(bs,a),
(8)
Proof According to [ 2], given a state and its approximation, the error in the objective
value can be bounded in the following manner:
/vextendsingle/vextendsingleJ(b,a∗)−J(b,a∗
s)/vextendsingle/vextendsingle≤2·γ(b,bs). (9)
Since this statement is true for every selection of b,bs, it is also true when switching
between the two beliefs, as if bis the approximation of bs, giving the following
equation:/vextendsingle/vextendsingleJ(bs,a∗
s)−J(bs,a∗)/vextendsingle/vextendsingle≤2·γ(b,bs). (10)
850 K. Elimelech and V . Indelman
By deﬁnition a∗
syields higher value than any other action, considering bsas the prior.
Hence, we can omit the absolute value,
0≤J(bs,a∗
s)−J(bs,a∗)≤2·γ(b,bs). (11)
Finally, we can rearrange the equation into
J(bs,a∗
s)−2·γ(b,bs)≤J(bs,a∗)≤J(bs,a∗
s). (12)
/square
Note that despite its short proof, Theorem 1is not trivial. It transforms a statement
over the exact values (Eq. 9), which are unknown to us, into a statement based solely
on the approximation, and allows us to infer about the desired best candidate action.
This important conclusion allows us to learn about the desired best candidate
action a∗, without having to know what it is, nor to exactly calculate the objective
for the actions, but only using the easy to calculate approximated values, and the rev-
enue offset of the approximation (which, as previously stated, can often be bounded
without having to solve the problem, and such an example appears ahead). Eq. 8
deﬁnes an interval of size 2 ·γ(b,bs), in which the approximated value of the real
best candidate action J(bs,a∗)must lie. Thus, without having to know the exact
values, we can eliminate actions which cannot possibly be the best candidate a∗,
since the approximation is not in this range.
Of course, when using a bound of the offset, and not the real value, the interval
grows larger in size. Hence, when bounding the offset, we should balance between
a tight bound, and keeping it easy to calculate. We recall again that we aspire to use
a simpliﬁed approximated state bs, such that by using it, the approximated objective
values are easy to calculate. Overall, when an approximation of the initial state is
given, considering its offset from the original state can be calculated or bounded,
we can easily eliminate unﬁt actions, resulting in a smaller subset of candidates, tobe used for solving the problem. This action elimination method is summarized and
demonstrated in Fig. 2.
Again, this discussion is not limited to a speciﬁc class of problems, not even
decision making under uncertainty, and is general to any decision problem. Finding
such b
scan be done in any way for which the offset from the real state γ(b,bs)can be
calculated. Overall, an action elimination process can either be applied once beforethe action selection, or recursively, when the approximation is scalable, as described
in the next section.
Fast Action Elimination for Efﬁcient Decision Making 851
3.2 Recursive Elimination
As a rule of thumb, we consider that calculating a rough approximation of the objec-
tive (high offset) is faster than calculating a more reﬁned one (smaller offset). Forexample, a factor graph can be approximated using node sparsiﬁcation. Removal of
more nodes from it is expected to increase the offset from the original graph, as the
approximation becomes more “rough”. Hence, given state approximations of differ-ent scale (with different offsets from the original), one can balance between investing
in minimizing the offset and by such the size of the interval, potentially leading to
elimination of more actions, and an easier calculation of the approximation, with amore lenient elimination.
Recursively reﬁning the approximation (decreasing offset) can be the most bene-
ﬁcial. After each iteration of eliminating actions, we can try to reduce the size of the
interval, using a more reﬁned approximation, in order to perform another elimination
iteration. Each iteration leaves us with a smaller subset of candidate actions to selectfrom. This way actions which are clearly non beneﬁcial are eliminated ﬁrst, with
minimal computational investment, and the reﬁned approximation is only used for
the top candidates. Ultimately, all the candidates but one would be eliminated, withconvergence of the size of the interval into zero, and the approximated best candidate
into the real one. Alternatively, considering the time constraints, if more than one
candidate still remains, we can either solve the problem exactly for those remainingactions, or return the best choice based on the current approximation. This recursive
method is summarized in the form of “anytime” Algorithm 1.
Note that whenever breaking on this anytime algorithm, the loss in potential value,
induced by selecting the approximated best action, is still ensured to be bounded
(according to Eq. 9).
Also, every iteration requires recalculation of the values according to the new
scale of approximation. This can be optimized if the approximation method allows
to update and reﬁne them, instead of calculating the values all over again. We still
assume that applying the elimination can be done fast enough, such that the saved time
in objective calculation is greater than the run time of the elimination itself. In our
experiments we show that this claim is indeed feasible. We note, however, that if theconvergence of the elimination interval is very slow, then the overall computational
cost may increase, due to the recalculation of the objective. Therefore, when applying
sequential elimination, the reﬁnement of the approximations should be consideredin accordance to the problem, and the magnitude of the objective cost.
852 K. Elimelech and V . Indelman
Algorithm 1: Recursive action elimination—an “anytime” algorithm
Inputs:
Initial state b
A set of candidate actions A
Output:
Best candidate action a∗
1Calculate an approximation of the initial state bs
2Calculate an approximation of the objective values using bsfor each candidate action in A
3Calculate or bound the simpliﬁcation offset γ(b,bs)
4Eliminate actions for which the approximated value is not in the interval from Theorem 1.
For the remaining subset of candidate actions:
5ifonly a single action a∗
sis in the interval then
6 return a∗
s(the real best candidate action)
7else if timeout then
8 return a∗
s(the best current choice)
9else
10 Reﬁne the approximation bsandgo to 3
11 or
12 Solve the problem exactly, for the remaining candidate actions
4 Practical Application
4.1 Decision Making Under Uncertainty
As stated earlier, the elimination process is general and independent of the problem
structure. Here we extend the discussion to the highly relevant case of decisionmaking under uncertainty and belief space planning, and specify practical tools for
applying the method.
When discussing decision making under uncertainty, the states, known as beliefs,
are stochastic, and described using a state vector and an information (or covariance)
matrix. Thus, state approximation here translates to sparsiﬁcation of the the under-
lying information matrices, as sparse matrices can be efﬁciently processed. In [ 2]
we describe a scalable sparsiﬁcation algorithm (brought here as Algorithm 2), which
yields a sparse approximation of an information matrix, for which the offset canbe bounded. Since the algorithm is scalable, and allows creating approximations
of different degrees, it is an appropriate match to the recursive action elimination
process.
According to the algorithm, we are able to select which variables, or rows, to
sparsify from the matrix (the indices of the sparsiﬁed rows are collected to a set
of indices S). Sparsiﬁcation of each row contributes somewhat to the offset from
the original belief, with variables which are uninvolved in the current decision (see
[1]) having zero contribution. One can freely select which and how many rows to
sparsify, and by such control the scale of the sparsiﬁcation, and its offset from theoriginal belief.
Fast Action Elimination for Efﬁcient Decision Making 853
Fig. 3 Sparsiﬁcation example using Algorithm 2. On the left—an information matrix taken from a
SLAM process; In the middle—a sparse version, after sparsifying all the uninvolved variables; Onthe right—a diagonal approximation, after sparsifying all variables. Note the signiﬁcant differencein the number of non zero elements
Recall the objective function for this problem includes an evaluation of a deter-
minant (Eq. 4). The order of magnitude of calculation of a determinant of a general
matrix of size n×nisO(n3), while for a fully sparsiﬁed matrix, a diagonal one, it
is linear. Meaning, the calculation of the approximated objective can be done much
faster, making the elimination process here very cost-effective. This progression also
matches the intuition we previously got while discussing a recursive elimination. A
sparser approximation leads to a faster calculation, yet a bigger offset and a more
lenient elimination (Fig. 3).
The usage of the sparsiﬁcation Algorithm 2 for the recursive elimination Algo-
rithm 1, is done by selecting a growing number of rows to sparsify in each iteration.
Otherwise, for a single elimination process, one can use any row selection Sfor the
sparsiﬁcation.
Algorithm 2: Scalable sparsiﬁcation of a belief
Inputs:
Ab e l i e f b∼N(x,Λ−1)
As e t Sof row indexes to sparsify
Output:
A sparse approximated belief bs
1Use Cholesky decomposition to ﬁnd Rsuch that Λ=RTR
2Calculate M=R−1
3Generate a sparse Msaccording to:
(Ms)ij=/braceleftbigg
0 i∈Sandi/negationslash=j
Mijelse
4Calculate Rs=M−1
s
5Calculate Λs=RT
sRs
6return bs∼N(x,Λ−1
s)
854 K. Elimelech and V . Indelman
According to Deﬁnition 2and Eq. 3, the offset of an action ain this case is
γ(b,bs,a).=/vextendsingle/vextendsingle|Λ+AAT|−|Λs+AAT|/vextendsingle/vextendsingle. (13)
For an approximation generated using Algorithm 2, given a speciﬁc selection of
rows S, and considering a single-row collective Jacobian A of an action a, the offset
can be developed into (see [ 2])
γ(b,bs,a)=1
|Σ|·/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsinglen/summationdisplay
i=1n/summationdisplay
j=1Ai·(Σ−Σs)ij·Aj/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsingle/vextendsinglewhere Λ=Σ
−1,Λ s=Σ−1
s
(14)
Again, the overall offset of the approximation is the maximal offset among all
actions. Instead of explicitly calculating the offset of all the actions, we reduce the
cost of calculating γ(b,bs)by settling on a bound over this value. This can be done
by using an upper bound over the elements of A:
γ(b,bs,a)=1
|Σ|·/summationdisplay
i,j/vextendsingle/vextendsingleAiAj/vextendsingle/vextendsingle·/vextendsingle/vextendsingle(Σ−Σs)ij/vextendsingle/vextendsingle≤α
|Σ|·/summationdisplay
i,j/vextendsingle/vextendsingle(Σ−Σs)ij/vextendsingle/vextendsingle,(15)
where the scalar αis an upper bound over the coefﬁcients/vextendsingle/vextendsingleAiAj/vextendsingle/vextendsingle,f o r1 ≤i,j≤
n. When αbounds the coefﬁcients ∀a∈A, this bound over the offset becomes
independent of a speciﬁc action, and only a single value needs to be calculated
(instead of calculating the offset per action, and then choosing the maximal value).Another “middle solution” could be ﬁnding similar upper bounds for subsets of
similar actions, and then taking the maximal value among the bounds of each of
these subsets. We denote that despite what is implied from its deﬁnition, we wereable to bound the offset without actually having to calculate the objective values of
the actions, as previously promised.
In the more general scenario, where Ais a multi-row matrix, representing a non-
myopic action sequence, the offset can be bounded using various determinant bounds.
This scenario is left to be investigated as a part of our future research.
4.2 Simulation
In this simulation, we demonstrate the application of the method on a sensor deploy-
ment problem, based on the previously presented analysis for decision making under
uncertainty. We wish to place 10 sensors on the junctions of a 5 ×5 grid, in order
to measure its temperature in the most informative way. This is a demonstration ofplanning in the belief space, hence the grid states are stochastic, and the information
matrix (sized 25 ×25) represents the uncertainty of the current temperature mea-
surement in the 25 junctions of the grid. At each step we greedily choose a position
Fast Action Elimination for Efﬁcient Decision Making 855
Fig. 4 Uncertainty levels in the temperature grid. Left—initial state, right—after sensor deploy-
ment. Higher values indicated higher uncertainty levels in that junction
from those available, and place a sensor on it (meaning, taking a measurement of the
temperature from that position), as shown in Fig. 4. The problem is initialized with
random uncertainty over the grid, leading to a dense prior information matrix.
At each iteration, we performed an elimination of unﬁt candidates, according to the
previously described approach. The approximated objective values were calculatedusing a sparse approximation of the information matrix, generated using Algorithm 2,
with Scontaining all rows, leaving only a diagonal matrix. Here the collective
Jacobians of the actions contain only 0’s and 1’s, indicating placement of a sensor,
Fig. 5 A single elimination process. The approximated objective values (revenues) and the
(unknown) exact values are shown in turquoise and green, respectively. Actions for which theapproximated value is not between the blue lines can be eliminated
856 K. Elimelech and V . Indelman
Fig. 6 Number of candidates action to examine, before and after the elimination process
and therefore in order to calculate the elimination interval (Eq. 8), we were able to
calculate a single bound of the overall offset using Eq. 15.
Figure 5describes a single elimination process from one of the iterations. The
approximated objective values (revenues) and the (unknown) exact ones are shown
in turquoise and green, respectively. The horizontal blue lines stand for the limits of
the elimination interval—actions for which the approximated value is not betweenthese lines, can be eliminated. In that iteration, only 3 actions passed the elimination.
We could then continue and solve the decision problem to this minimal subset of
candidates, without having to calculate the exact value of the eliminated ones.
Figure 6shows the number of remaining candidate actions after the elimination,
in each iteration, in comparison to the original number of candidates. In a few cases,
all candidates besides one were eliminated, hence leaving us with the best candidate,without even calculating any value exactly. In the ﬁnal iterations, the most informative
sensors were already placed, and the remaining candidates yield similar value (no
dominant actions), therefore more candidates passed the elimination.
5 Discussion and Conclusions
This paper introduced an approach to reduce the computational complexity of the
basic decision problem. Instead of exhaustively calculating the exact objective val-
ues of the candidate actions, in order to select the best one, we can use a roughapproximation of these values to quickly eliminate unﬁt candidates.
Fast Action Elimination for Efﬁcient Decision Making 857
The main concept of this paper is not tied to a speciﬁc problem. Yet, since this
approach is highly relevant to problems in which the action domain is large, and the
objective is hard to calculate, we extended the discussion to the challenging caseof decision making under uncertainty and belief space planning. There we used a
dedicated scalable approximation algorithm, to form a practical set of tools alongside
the elimination algorithm. We ﬁnally demonstrated the beneﬁts of using the methodwhile solving a sensor deployment problem.
This work is a conceptual introduction to the general case, and acts as a good
basis to many future research directions. The simple simulation is brought in orderto demonstrate how, in practice, the approach can be used, and to indicate its pos-
sible beneﬁts. Applications to advanced scenarios require a thorough examination.
Future extensions include, for example, in the context of belief space planning: set-ting bounds over multi-row Jacobians, to support non myopic cases; upgrading the
sparsiﬁcation algorithm, such that it becomes updatable—sparing the recalculation
of the objective values in each iteration, which would dramatically improve the per-formance; or creating an entirely new sparsiﬁcation method. Similar analysis can be
developed to other types of problems as well.
A subsequent goal of this paper is to break the current nativity in approaching
decision problems. To the best of our knowledge, our continuous work on the subject
is the ﬁrst attempt to analyze and directly improve the computational complexity ofthe fundamental problem. With this work we also wish to encourage the robotics and
AI community to extend its focus on the algorithmic basis of autonomous systems,
and speciﬁcally towards efﬁcient decision making and planning. Despite the growthin academic research in this ﬁeld, a great portion of recent work has been on adjusting
speciﬁc properties in order to improve or upgrade existing methods. This paper proves
that tackling the basics can be fruitful, and have a wider impact.
References
1. Elimelech, K., Indelman, V .: Consistent sparsiﬁcation for efﬁcient decision making under
uncertainty in high dimensional state spaces. In: IEEE International Conference on Roboticsand Automation (ICRA) (2017)
2. Elimelech, K., Indelman, V .: Scalable sparsiﬁcation for efﬁcient decision making under uncer-
tainty in high dimensional state spaces. In: IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS) (2017)
3. Indelman, V .: No correlations involved: decision making under uncertainty in a conservative
sparse information space. IEEE Robot. Autom. Lett. (RA-L) 1(1), 407–414 (2016)
4. He, R., Prentice, S., Roy, N.: Planning in information space for a quadrotor helicopter in a gps-
denied environment. In: IEEE International Conference on Robotics and Automation (ICRA),pp. 1814–1820 (2008)
5. Huang, G., Kaess, M., Leonard, J.J.: Consistent sparsiﬁcation for graph optimization. In: Pro-
ceedings of the European Conference on Mobile Robots (ECMR), pp. 150–157 (2012)
6. Carlevaris-Bianco, N., Kaess, M., Eustice, R.M.: Generic node removal for factor-graph SLAM.
IEEE Trans. Robot. 30(6), 1371–1385 (2014)
7. Roy, N., Gordon, G., Thrun, S.: Finding approximate pomdp solutions through belief compres-
sion. J. Artif. Intell. Res. 23, 1–40 (2005)
858 K. Elimelech and V . Indelman
8. Patil, S., Kahn, G., Laskey, M., Schulman, J., Goldberg, K., Abbeel, P .: Scaling up gaussian
belief space planning through covariance-free trajectory optimization and automatic differen-tiation. In International Workshop on the Algorithmic Foundations of Robotics (2014)
9. Indelman, V ., Carlone, L., Dellaert, F.: Planning in the continuous domain: a generalized belief
space approach for autonomous navigation in unknown environments. Int. J. Robot. Res. 34(7),
849–882 (2015)
10. Eustice, R., Walter, M., Leonard, J.: Sparse extended information ﬁlters: insights into sparsiﬁ-
cation. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp.3281–3288, Aug 2005
11. Prentice, S., Roy, N.: The belief roadmap: efﬁcient planning in belief space by factoring the
covariance. Int. J. Robot. Res. (2009)
12. V an Den Berg, J., Abbeel, P ., Goldberg, K.: Lqg-mp: optimized path planning for robots with
motion uncertainty and imperfect state information. Intl. J. Robot. Res. 30(7), 895–913 (2011)
13. Krause, A., Singh, A., Guestrin, C.: Near-optimal sensor placements in gaussian processes:
theory, efﬁcient algorithms and empirical studies. J. Mach. Learn. Res. 9, 235–284 (2008)
14. Platt, R., Tedrake, R., Kaelbling, L.P ., Lozano-Pérez, T.: Belief space planning assuming max-
imum likelihood observations. In: Robotics: Science and Systems (RSS), Zaragoza, Spain, pp.
587–593 (2010)
15. Nakhost, H., Müller, M.: Action elimination and plan neighborhood graph search: two algo-
rithms for plan improvement. In: Twentieth International Conference on Automated Planningand Scheduling (ICAPS), pp. 121–128 (2010)
16. Rosman, B., Ramamoorthy, S.: What good are actions? Accelerating learning using learned
action priors. In: 2012 IEEE International Conference on Development and Learning andEpigenetic Robotics (ICDL), pp. 1–6. IEEE (2012)
17. Sherstov, A., Stone, P .: Action-space knowledge transfer in mdps: Formalism, suboptimality
bounds, and algorithms. In: Conference on Learning Theory (2005)
18. Abel, D., Hershkowitz, D.E., Barth-Maron, G., Brawner, S., O’Farrell, K., MacGlashan, J.,
Tellex, S.: Goal-based action priors. In: Twenty-Fifth International Conference on AutomatedPlanning and Scheduling(ICAPS), pp. 306–314 (2015)
19. Kuter, U., Hu, J.: Computing and using lower and upper bounds for action elimination in mdp
planning. In: International Symposium on Abstraction, Reformulation, and Approximation,pp. 243–257. Springer, Heidelberg (2007)
20. Even-Dar, E., Mannor, S., Mansour, Y .: Action elimination and stopping conditions for the
multi-armed bandit and reinforcement learning problems. J. Mach. Learn. Res. 7(Jun), 1079–
1105 (2006)
21. Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty.
In: IEEE International Conference on Robotics and Automation (ICRA), pp. 723–730 (2011)
Functional Path Optimisation
for Exploration in ContinuousOccupancy Maps
Gilad Francis, Lionel Ott and Fabio Ramos
1 Introduction
The objective of autonomous exploration is to produce a consistent representation
of the environment. It involves complex decision-making, selecting the trajecto-ries a robot should take in order to minimise the overall uncertainty in the model.
Essentially, exploration is a path optimisation procedure for ﬁnding trajectories that
efﬁciently learn the environment. The difﬁculty lies in the dimensionality and shape
of the search space which prohibits a closed-form solution to the general exploration
problem, making autonomous exploration an active ﬁeld of research. The plethoraof exploration methods in the literature offer different strategies for relaxing this
intractable problem, most commonly by reducing the search space dimensionality,
for example by discretising the path.
In this work, we formalise exploration as a variational problem. We present a
novel approach based on functional gradient descent (FGD) to efﬁciently optimise
exploratory paths over continuous occupancy maps. We use stochastic FGD to over-come the limitations of standard FGD methods in order to ensure convergence. This
process enables optimisation over the entire path, resulting in continuous smooth
paths that maximise the overall map quality while keeping the robot safe from col-lisions. Our contributions are:
1. A Next Best Path method. An information-driven variational framework for safe
autonomous exploration in continuous occupancy maps. Path optimisation is
performed directly in the space of trajectories using a combined objective; which
considers safety, efﬁciency and information. The method is invariant to the choiceof path representation as it uses stochastic functional gradient descent to optimise
the objective along the entire path.
G. Francis (B)·L. Ott ·F. Ramos
School of Computer Science, The University of Sydney, Sydney, Australiae-mail: gilad.francis@sydney.edu.au
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_59859
860 G. Francis et al.
2. Developing a mutual information (MI) variational objective for continuous occu-
pancy maps. This replaces the common and expensive approach of computing
MI explicitly over the entire map for each evaluated path. Instead, our methodmodiﬁes the path using MI functional gradients without the need to compute
MI explicitly. These gradients are obtained from local perturbations on the map
model which are derived in closed-form.
The remainder of this paper is organised as follows. Literature on autonomous
exploration is surveyed in Sect. 2. The basic building blocks, such as Hilbert maps and
FGD are reviewed in Sect. 3. Section 4describes in detail the functional exploration
algorithm. Experimental results and analysis are presented in Sect. 5. Finally, Sect. 6
draws conclusions on the proposed method.
2 Related Work
The goal of autonomous exploration is to produce a consistent environment modelby minimising any uncertainties. In a mapping context, exploration is the process
of producing high-ﬁdelity maps [ 21]. This is a complex problem mainly due the
dimensionality of the solution space. Most exploration methods use occupancy gridmaps in their planning [ 22], rather than continuous occupancy maps. Regardless of
the type of occupancy map, exploration methods take one of two forms; frontier-
driven or information-theoretic. Juliá et al. [ 12] provide a quantitative comparison
between these exploration methods.
Frontier-based exploration methods drive the robot toward the borders of the
known space [ 25]. In a grid map, frontiers are clusters of free cells neighbouring
unknown cells. Once frontiers are identiﬁed, a separate path planner ﬁnds a safe path
toward a selected frontier. V arious utility functions can be used when choosing the
most desirable frontier. The simplest form considers only the travelling costs [ 25].
González-Baños and Latombe [ 8] choose a goal point based on a score of expected
coverage penalised by the travelling costs. A generalised approach for goal pointselection given several criteria were suggested by Holz et al. [ 9].
Information-theoretic exploration methods optimise a utility function associated
with the uncertainty of the map. Early work optimised the selection of a discretegoal point rather than optimising an entire path. Elfes [ 4] suggested MI as an infor-
mation metric for exploration, while Whaite and Ferrie [ 24] proposed a next best
view (NBV) approach using the entire map entropy. V allvé and Andrade-Cetto [ 23]
use a potential ﬁeld computed over the entire conﬁguration space to ﬁnd exploration
candidates. However, this method assumes discrete steps, disregarding the reduction
of entropy between consecutive robot poses. Charrow et al. [ 1] combined the fron-
tier and information-based methods. While they optimised the information heuristic
over a continuous control input space, in effect the path consisted of a ﬁxed number
of time steps. Lauri and Ritala [ 14] formulate the exploration problem as partially
observable Markov decision process (POMDP) and used sample-based approach to
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 861
solve the POMDP . Similarly to the work of Charrow etr al., the action space is con-
tinuous, however, the path consists of a ﬁnite set of time steps, which is in contrast
to the proposed method where optimisation is performed in the space of trajectories.
Only a handful of algorithms tackle exploration in continuous occupancy maps.
Yang et al. [ 26] employed rapidly-exploring random tree (RRT) to sample a set of
feasible path candidates for a Gaussian Processes (GPs) maps. An adaptation of
the frontier method for continuous occupancy maps was introduced by Jadidi et al.
[10], where a discretised frontier map was built from the continuous map. More
recently, Jadidi et al. [ 11] employed MI to rank frontiers. Bayesian optimisation
(BO) has also been used for exploration. Marchant and Ramos [ 15] utilised BO to
optimise the selection of continuous informative paths over a continuous environ-
mental model. Francis et al. [ 5] used constrained BO for safe exploration to learn an
MI objective. While BO optimises continuous paths, in practice it uses only a limited
parameterisation such as quadratic or cubic splines.
In summary, the exploration method proposed in this paper uses an information-
based utility to optimise path selection. Employing calculus of variations, the opti-
misation procedure is invariant to the path representation. This is a major difference
from existing methods that typically depend on a ﬁnite path parametrisation, such
as ﬁnite sets of time steps or waypoints, or employ simple representations such as
quadratic or cubic splines. Instead, our method can utilise a highly expressive pathrepresentation; such as non-parametric [ 6] or approximate kernel paths [ 7]. As our
method uses continuous occupancy maps, the MI utility can be derived directly from
the map model in closed form, which simpliﬁes computations.
3 Preliminaries
In this section, we review the basic building blocks of the functional explorationmethod; Hilbert maps and the functional gradient path planning method. In Sect. 4,
we extend these components to support an information-driven safe exploration algo-
rithm.
3.1 Hilbert Maps
Hilbert maps [ 19] are continuous discriminative models that predict occupancy based
on sensors observations. Unlike grid maps [ 3] that discretise space into a set of inde-
pendent cells, Hilbert maps maintain neighbourhood information. Other continuousmapping methods based on Gaussian Processes (GPs) [ 10,18] hold similar prop-
erties. However, these methods are limited by the scalability of the associated GPs.
Hilbert maps, on the other hand, use an approximate kernel logistic regression model,which renders updating and querying the map independent of the dataset size. In addi-
tion, stochastic gradient descent (SGD) is used to enable real-time performance.
862 G. Francis et al.
Formally, a Hilbert map is a logistic regression (LR) classiﬁer model that predicts
occupancy anywhere in the map. It uses nonlinear projection into a reproducing
kernel Hilbert space (RKHS) in order to represent a real environment. Given a set
of weights w, the predictive occupancy posterior transforms into:
p(y=+ 1|x,w)=1
1+exp(−wTˆΦ(x))=σ(wTˆΦ(x)). (1)
In an occupancy map context x∈RDrepresents either a 2 Dor 3 Dlocation, while
y∈{ − 1,+1}denotes two possible binary outputs, unoccupied and occupied. We
deﬁne σas the logistic sigmoid function. ˆΦ(·)is a set of features that, in expectation,
approximate the inner product deﬁned by the kernel function k(·,·). This approxi-
mation enables fast training of a map model, suitable for use in a robotics setting.There are several methods to approximate the kernel matrix [ 19]. Given the desired
approximation, the weights vector wcan be then trained by minimising the regu-
larised negative log-likelihood (NLL) as commonly performed in logistic regression
methods [ 19].
3.2 Functional Gradient Descent
Functional gradient descent (FGD) is a variational framework to optimise nonlin-
ear models. It has been successfully applied to motion planning problem in recent
years with the main objective of producing safe, collision-free paths. It was recentlysuggested as an alternative approach to sampling-based methods for path planning
using occupancy maps [ 6]. In this section, the general method is discussed, before
the extension for autonomous exploration is described in Sect. 4.
We ﬁrst introduce notation. A path, ξ:[0,1]→ C∈R
D, is a function that maps
a time-like parameter t∈[0,1]into conﬁguration space C. The objective functional
U(ξ):Ξ→ Rreturns a real number for each path ξ∈Ξ, corresponding to a cost
or loss associated with ξ. FGD path planners capture path features such as smooth-
ness and safety in U(ξ). The goal of the optimisation process is to ﬁnd a path that
minimises the overall costs:
ξoptimal =argmin
ξU(ξ) (2)
Finding the optimal path is performed by following the functional gradient of
the objective. This is an iterative process where the functional gradient update ruleis derived from a linear approximation of the cost functional around the current
trajectory, ξ
n:
U(ξ)≈U(ξn)+∇ξU(ξn)(ξ−ξn). (3)
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 863
We enforce small updates by adding a regularisation term based on the norm of the
update:
ξn+1=argmin
ξU(ξn)+(ξ−ξn)T∇ξU(ξn)+1
2ηn/bardblξ−ξn/bardbl2
Λ. (4)
The regularisation term /bardblξ−ξn/bardbl2
Λ=(ξ−ξn)TΛ(ξ−ξn)is the squared norm with
respect to a metric tensor Λandηnis a user-deﬁned learning rate. By differentiating
the right hand side of ( 4) with respect to ξ, we obtain the iterative update rule:
ξn+1(·)=ξn(·)−ηnΛ−1∇ξU(ξn). (5)
We note that ( 5) forms a general update rule, regardless of the choice of the objective
function or the path representation. The only requirements are that Λis invertible
and the gradient ∇ξU(ξn)exists.
The general rule for computing the objective functional gradient ∇ξUstems from
the calculus of variations. As a variational method, the objective functional must takethe form of an integral or sum. Generally, the objective functional Ftakes the form
F(ξ)=/integraltext
b
av(t,ξ,ξ/prime)dt, therefore the functional gradient can be computed using the
Euler-Lagrange equation [ 27]:
∇Fξ(ξ)=∂v
∂ξ−d
dt∂v
∂ξ/prime. (6)
These gradients are then used to compute the iterative update rule of ( 5). Constraints
are incorporated using KKT conditions, similar to [ 16], and are an explicit part of
the path representation.
4 Functional Exploration
The following section introduces our proposed functional exploration method. The
use of functional gradient descent on an information-based objective results in tra-
jectories which are highly expressive and safe while optimising the amount of infor-mation gained along the path.
4.1 Notation
We ﬁrst introduce the notation used throughout the following sections. The workspace
of the robot W∈R3deﬁnes the space where obstacles lie and the map is queried.
In addition, to account for the robot’s ﬁnite size or its pose uncertainty, a set of body
864 G. Francis et al.
points, B∈R3are deﬁned. As the trajectory ξlies in conﬁguration space C,w e
set a forward kinematic transform gthat maps a robot conﬁguration, ξ(t)and body
point b∈Bto a point in the workspace g:C×B→ W. To simplify notation, we
deﬁne xt=g(ξ(t),b)as the workspace location for the pair (t,b). We assume that
the robot is equipped with a sensor, such as laser range ﬁnder, with a maximum range
Rmaxand an angular ﬁeld of view Ω.
For a given function ξ, a functional returns a single value U:RD→ R. Function-
als are usually represented by an integral. However, whenever we compute the safety
or MI functionals, the cost is computed over W. As a result, the functional must be
approximated by a reduce operator, e.g., average or maximum, that aggregates the
cost along ξ(·). Given a workspace cost function c/parenleftBig
g/parenleftbig
ξ(t),b/parenrightbig/parenrightBig
:R3→ R, we can
approximate the functional by a sum over a ﬁnite set T(ξ)={ t,b}iof time and
body points:
Uobs,MI(ξ)≈/summationdisplay
(t,b)∈T(ξ)cobs,MI/parenleftBig
g/parenleftbig
ξ(t),b/parenrightbig/parenrightBig
≡/summationdisplay
(t,b)∈T(ξ)cobs,MI(xt) (7)
In addition, we note the difference between gradient operators. We deﬁne ∇as a
gradient with respect to ξ, while ∇xis the workspace gradient.
4.2 Exploration Functional Objective
The goal of autonomous exploration is to safely reduce the uncertainty of the envi-
ronment model. Some exploration methods compute a ﬁnite set of go-to points that
locally maximise information gain. Other methods optimise an information-basedobjective over the entire path, however, these are highly dependent on the path param-
eterisation. Formulating exploration as a variational problem and solving it using
functional gradient descent, provides a general optimisation framework which isinvariant to the choice of path parameterisation and can even take a non-parametric
form as shown in [ 6].
The approach taken in our work relies on maximising mutual information along
the entire path while keeping the trajectory safe. This is attained by constructing an
objective functional Uwhich is a weighted sum of three components:
–U
obswhich maintains path safety by penalising proximity to obstacles,
–Udynwhich penalises based on the shape of the trajectory, keeping path smooth
and short and,
–UMIwhich rewards the mutual information gained along the path.
The overall objective takes a form of a weighted sum, as is also shown schematically
in Fig. 1;
U(ξ)=βobsUobs(ξ)+βdynUdyn(ξ)+βMIUMI(ξ). (8)
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 865
Fig. 1 Given a sample (t,b), the functional Uis the weighted sum of the various objectives;
obstacle, dynamic and MI. Mandξare global variables used to compute these objectives
Hereβobs,βdyn,βMIare user-deﬁned coefﬁcients. In the following sections, we
will introduce the different components of the functional objective; Uobs(ξ),Udyn(ξ)
and UMI(ξ). For each component we will derive its functional gradient assuming a
Hilbert map as the environment model.
Obstacle Functional Uobs(ξ)Following our previous work [ 6], we deﬁne the
workspace cost function cobsas the map occupancy of ( 1), i.e. cobs(xt)=p(y=
+1|xt).G i v e n( 7), the obstacle functional can be approximated by Uobs(ξ)≈/summationtext
xt∈T(ξ)cobs(xt). The functional gradient can then be computed as
∇Uobs=/summationdisplay
xt∈T(ξ)J(xt)∇xcobs(xt). (9)
Here, J(xt)is the workspace Jacobian. Since the map model is continuous and at
least twice differentiable [ 19], the spatial gradient of occupancy can be computed in
closed-form from ( 1)a s :
∇xcobs(xt)=∇ xp(y=+ 1|xt)=σ/parenleftBig
wTˆΦ(xt)/parenrightBig/parenleftBig
1−σ/parenleftbig
wTˆΦ(xt)/parenrightbig/parenrightBig
wT∇xˆΦ(xt).
(10)
Path Dynamics Functional Udy n(ξ)Udyn(ξ) penalises kinematic costs associ-
ated with ξ. The straightforward approach is to regularise on the trajectory length,
which can be attained by optimising the integral over the squared velocity norm:
Udyn(ξ)=1
2/integraltext1
0/vextendsingle/vextendsingle/vextendsingle/vextendsingled
dtξ(t)/vextendsingle/vextendsingle/vextendsingle/vextendsingle2dt. Following the Euler-Lagrange equation ( 6), the func-
tional gradient of Udynis
∇ξUdyn(ξ(t))=−d2
dt2ξ(t). (11)
Mutual Information Functional UMI(ξ)Mutual information (MI) is used in many
autonomous exploration algorithms as an information-based objective function [ 13].
In this context, MI is deﬁned as the reduction in entropy conditioned on expected
866 G. Francis et al.
observations. Given an occupancy map Mand a set of expected observations ˆz,w e
can deﬁne MI as:
MI(M;ˆz)=H(M)−H(M|ˆz) (12)
where Hdenotes Shannon’s entropy. The main computational challenge of ( 12)
is resolving the expected observations ˆz. These are emulated observations that are
produced by ray casting based on the sensor model. Determining ˆzand MI over an
entire path is computationally intensive, leading most exploration methods to solve a
relaxed MI optimisation problem, by either discretising or parameterising the paths.
The approach taken in this work uses the MI functional UMIand its gradient to
maximise MI efﬁciently over the entire trajectory. To compute UMI,a nM Ir e w a r d
function cMI(xt)≈MI(M;ˆz|xt)is computed over the robot’s workspace W,in a
similar fashion to the computation of Uobs. However, computation of the conditional
entropy H(M|ˆz)entails changes to the Hilbert map model.
In the following section, we will describe the stages involved in computing the
MI functional gradient. The three stages executed when computing ∇UMIare:
– Simulating expected observations
– Creating a perturbed Hilbert map model M|ˆz
– Obtaining MI functional gradient ∇UMI.
Figure 2shows the steps required for MI gradient computations at a given time t.
Simulating Expected Observations
Similarly to the obstacle functional, the MI functional is approximated by a sum overa ﬁnite set of points x
t;UMI(ξ)≈/summationtext
xt∈T(ξ)cMI(xt).cMIis chosen in a way that will
estimate the inﬁnitesimal change in MI at xt. To compute cMIwe emulate observa-
tions by ray casting, as done in other information-driven exploration techniques [ 1,
4,11]. However, while these methods evaluate the MI reward over the entire ﬁeld
of view of the sensor, our method is only interested in the expected observations at
the sensor’s limits (maximum range). The rationale behind this approach is that new
information about the environment will be mainly obtained at the sensor’s sensing
limits Rmax. Figure 3illustrates the difference between the two approach to compute
MI.
The output of the ray casting process is a set of unoccupied expected observations
ˆzffor any time and body point xt.ˆzfdiffers from the expected observations of
(12), as we are only interested in unoccupied (no obstacle) observation at the sensor
maximum range. Figure 2b depicts ˆzfas cyan diamonds.
Creating a Perturbed Hilbert Map Model M|ˆz
In this step, we generate M|ˆzf, the modiﬁed Hilbert map conditioned on the expected
observations. The straightforward approach is to train a new map model based on ˆz.
However the computational costs of such an approach are high, as new maps must
be generated along the entire trajectory during optimisation. Instead, we propose the
use of a perturbed Hilbert map model.
The perturbed Hilbert map model modiﬁes the predictive map model σ/parenleftbig
wTˆΦ(x)/parenrightbig
(1) with a perturbed model σ/parenleftbig
Ψ(x,ˆzf)/parenrightbig
. This model uses the expected observations
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 867
Fig. 2 MI Functional gradient. The MI gradient is computed for a time sample tgiven a continuous
occupancy map OM (a). The current path estimate ξis depicted in blue, while the expected pose
at time tis shown in green. The modiﬁed OM ( b) is generated using hypothetical observations,
shown as cyan diamonds, based on a robot’s conﬁguration at time t. Only observations at the edge
of sensor range are considered for t. The entropy of the OM ( c) and the modiﬁed OM ( d) can be
computed from the occupancy values (high entropy shown in white), which produces MI ( e). The
MI gradient is estimated by samples around observations (shown as black arrows). Accumulating
gradient samples results in the overall MI gradient for t, shown by the yellow arrow on the robot.
Note that the images of occupancy, entropy and MI are only given here for presentation purposes
and full maps are never computed. The planner only accesses occupancy, entropy and gradient
values through random samples
(a) Standard Exploration (b) Functional Exploration
Fig. 3 Difference in MI calculation. xtis the robot’s pose for which MI is computed, the black
lines depict the sensor ﬁeld of view and the blue area is the region where MI is computed. aMI is
computed over the entire ﬁeld of view of sensor, producing ˆz.bMI is computed only at the sensor’s
range limits, producing ˆzf
as a dataset ˆzf=/braceleftbig
xi,ri/bracerightbigN
i=1, where xi∈RDis a point in 2D or 3D space and riis
the log odds of the desired predictive occupancy posterior at xi,t oﬁ ta Gaussian
process (GP):
Ψ(x,ˆzf)∼GP/parenleftbig
wTˆΦ(x),k(xi,xi)/parenrightbig
. (13)
868 G. Francis et al.
We note that wTˆΦ(x)of the current occupancy map is the mean function of the GP .
The kernel function kis the same function approximated by the Hilbert map features
ˆΦ(·). The predictive probability of the perturbed map ˆpis given by:
ˆp(x,ˆzf)=σ/parenleftbig
Ψ(x,ˆzf)/parenrightbig
(14)
Figure 2b depicts the resulting occupancy map following the embedding of the
expected observations in an existing map.
The computational cost of the perturbed Hilbert map is cubic in the number of
expected observations |ˆzf|. As we are only modifying the map in a small region, a
small set of observations is required to generate perturbation, keeping the computa-
tion load low.
Obtaining MI Functional Gradient ∇UMI
The workspace MI cost function for xtis deﬁned as the MI summed over the
entire map, i.e. cMI(xt)=/integraltext
MMI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
dm. However, as we are only inter-
ested in the change at the limits of the sensor range, cMIcan be replaced by;
cMI(xt)=/integraltext
m∈mmax(xt)MI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
dm. Where mmax(xt)denotes workspace
locations which lie on the arc given by the maximum sensing range Rmax and the
sensor’s ﬁeld of view Ω, as shown in Fig. 3.cMIcan be approximated with a sum
using either a deterministic or a Monte-Carlo schedule:
cMI(xt)≈/summationdisplay
m∈mmax(xt)MI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
. (15)
Given the approximations ( 7) and ( 15), the MI functional can be represented as
UMI(ξ)≈/summationdisplay
xt∈T(ξ)/summationdisplay
m∈mmax(xt)MI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
. (16)
The MI functional gradient follows the same form as ( 9):
∇UMI(xt)=/summationdisplay
xt∈T(ξ)/summationdisplay
m∈mmax(xt)J(xt)∇xMI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
. (17)
The spatial gradient of MI, ∇xMI/parenleftbig
M(m)/parenrightbig
, can be expressed in closed-form using
the Hilbert maps’ continuous model. Using the MI deﬁnition from ( 12), the gradi-
ent is deﬁned as ∇xMI(M;ˆz)=∇ xH(M)−∇ xH(M|ˆz), where ∇xHis the spatial
gradient of the entropy. Using the chain rule we rewrite the spatial gradient of H
around a query point xas:
∇xH(x)=dH
dp∇xp(x), (18)
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 869
where pis the probability of occupancy at xgiven by ( 1). As pis a Bernoulli
random variable,dH
dpis simply;dH
dp=log21−p
p. The occupancy gradient ∇xp(x)
depends on the occupancy map used. For the unperturbed map, ∇xp(x)is given by
(10). The spatial gradient of the perturbed map ( 14) can be computed similarly to
the unperturbed map;
∇xˆp(x,ˆzf)=∇ xσ/parenleftbig
Ψ(x,ˆzf)/parenrightbig
=σ/parenleftbig
Ψ(x,ˆzf)/parenrightbig/parenleftBig
1−σ/parenleftbig
Ψ(x,ˆzf)/parenrightbig/parenrightBig
∇xΨ(x,ˆzf)(19)
AsΨis a GP , its gradient can be computed in closed form.
Figure 2e shows the computation of MI gradient by sampling along the arc deﬁned
by the sensor range. Each sample produces an MI gradient, schematically shownby the black arrows. The overall MI gradient, which pushes optimisation toward
exploratory trajectories is computed from the sum of all samples and is shown in
Fig. 2e as a yellow arrow.
4.3 Functional Exploration Algorithm
In this section, we describe the functional exploration algorithm, which aims to ﬁnd
a safe path that maximises MI over its entire course.
Functional exploration is a general optimisation framework, meaning it is invariant
to the choice of path representation. Functional optimisation methods have used
waypoint parameterisation [ 27], Gaussian process representation [ 2,17], or deﬁned
trajectories in RKHS [ 16]. However, in all methods the objective is sampled via
a deterministic schedule. Such approaches have proved unsatisfactory for planning
using occupancy maps [ 6]. To ensure convergence to a safe solution, the path is
stochastically sampled in the entire t∈[0,1]domain and any uninformative gradient
updates, such as those coming from unsafe parts of the map, are rejected.
The path representation used in this work is based on kernel matrix approximations
[7]. The path is essentially a weighted sum of nonlinear features, similar to a nonlinear
regression. This path model is highly expressive, yet compact. Most importantly, the
path can be optimised by SGD [ 20]. Given a set of weights wξ, the path is deﬁned
as:
ξ(t)=(wξ)TˆΥ(t)+ξo(t)+ξb(t). (20)
ξois an initial path, which can be randomly chosen or can be computed by a simple
and fast planner. ξbis a term used to adjust boundary conditions, such as the start and
goal pose. ˆΥ(·)are nonlinear features that approximates the inner product deﬁned
by a kernel function kpby the following dot product [ 7];
kp(t,t/prime)=/angbracketleftΥ(t),Υ ( t/prime)/angbracketright≈ˆΥ(t)TˆΥ(t/prime). (21)
The kernel kp(t,t/prime)maintains the correlation between various time points.
870 G. Francis et al.
Algorithm 1: Stochastic Functional Exploration
Input :M: Occupancy Map.
ξ(0): Start state.
Psa f e: No obstacle threshold.
Optional: boundary conditions ξb, initial guess ξo.
Output :woptimal
n=0
while solution not converged do
//Stochastic sampling:
S∼U[0,1]Draw mini-batch
foreach xt∈Sdo
Pocc← M(xt))Eq. ( 1)
ifPocc≤PSa f e then
ˆzf(xt)←simulated observations
∇Uobs←∂
∂ξxt/parenleftbig
σ/parenleftBig
wTˆΦ(xt)/parenrightBig/parenleftBig
1−σ/parenleftbig
wTˆΦ(xt)/parenrightbig/parenrightBig
wT∇xˆΦ(xt)/parenrightbig
Eq.( 9)
∇Udyn←−d2
dt2ξ(t)Eq.( 11)
∇UMI←/summationtext
m∈mmax(xt)∂
∂ξxt∇xMI/parenleftbig
M(m);ˆzf(xt)/parenrightbig
Eq.( 17)
∇U←Combine using Eq.( 8)
wn+1← wξ
n−ηnΛ−1ˆΥ(ti)TˆΥ(ti)∇ξU(ξn)(ti)update rule Eq. 22
end
end
n=n+1
end
Using ( 20), the general functional optimisation given in ( 2) is transformed into
an optimisation of the weights, wξ
optimal =argminwξU(ξ). The iterative update rule
of (5) takes the following form:
wξ
n+1=wξ
n−ηnΛ−1ˆΥ(ti)TˆΥ(ti)∇ξU(ξn)(ti), (22)
where ti∈T.
The algorithm for functional exploration is given in Algorithm 1. The essential
inputs are the initial robot state and the occupancy map. Boundary conditions or
an initial solution are optional inputs. In each iteration, a mini-batch is drawn. The
safety of each sample is checked, and if it is below Psa f e, the sample is used to update
the path. The gradient of the various components (obs, dyn, MI) of the objectivefunctional are computed, and summed according to ( 8). Once the overall functional
gradient is computed, the weights w
ξ
n+1are updated according to ( 22).
Figure 4provides an insight into the optimisation process of a single planning
iteration. Each iteration starts with an initial guess, which is depicted in blue. The
obstacle functional gradient repels the path from obstacles and unexplored space,
while the MI objective pulls the path toward unexplored space. The intermediatepath updates, following the functional gradient, are shown in grey. The ﬁnal optimal
path is shown in green (Fig. 5).
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 871
Fig. 4 Functional planning iteration. Functional gradients deform the initial solution in blue. The
obstacles gradient repels path from obstacles and unknown space, while the MI gradient pulls path
toward unexplored space. The resulting path, in green, is the optimised solution
Fig. 5 Comparison of exploration methods—2 repetitions (each with 20 planning instances); the
proposed functional exploration method (red), RRT-based exploration (black) and frontier-based
exploration (green). RRT and the propose method converge in a similar fashion as both optimise
MI over entire path. The goal-based approach of frontier converges slower as it does not explicitly
optimise on MI
5 Experimental Results
In this section, we evaluate the performance of the stochastic functional gradient path
planner and compare it to other exploration methods for continuous occupancy maps.
As a benchmark, we chose to compare our method to the RRT-based exploration
method of Yang et al. [ 26] which we modiﬁed to use Hilbert maps. This method
optimises MI during path selection, and while its bottleneck lies in computational
complexity , it takes advantage of the fact the RRTs are probabilistically complete.
Another method used for comparison is based on frontier exploration [ 25], which
takes a grid approximation of the continuous map in a similar approach to [ 10]. The
path is then constructed by a smooth RRT planner which reasons only about the path
872 G. Francis et al.
Fig. 6 Comparison of exploration methods at various planning iterations using continuous occu-
pancy maps. The hexagonal markers are the planning poses. Triangle and star represent start andend points, respectively. The grey lines are either candidate paths (RRT) or intermediate solutions(functional exploration). The pink line depicts the optimised path and the green line is the traversedpath. The path is assessed during execution, and a re-plan step is invoked if a path is no longer safe
safety. All methods, including Hilbert maps, are implemented in Python and tested
on an Intel i5-6200U with 8 GB RAM.
Figure 6shows a qualitative comparison at various planning iterations. While all
exploration methods successfully build the map, there are some clear differences.
The frontier-based method produces jerky paths and tends to move towards the edges
and corners of the map. Path optimisation methods, on the other hand, stay closer
to obstacles. As the RRT-based method maximises MI only, its path moves closerto the edges of obstacles, while the proposed method keeps a bigger distance, as
its objective includes obstacle safety explicitly. We note that increasing the safety
margin by applying a blurring ﬁlter, as done with grid maps, is not applicable incontinuous maps, as it requires expensive discretisation of the map.
Quantitative comparisons are shown in Fig. 5and in Table 1. Figure 5depicts the
reduction in the entropy of the map. The rate of reduction is similar for both ourmethod and the RRT-based exploration, albeit slightly better for the latter, mainly
due the fact that the paths generated by the RRT move closer to obstacles. As a
result, the RRT planner covers the map faster, however with a higher probability of
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 873
Ta b l e 1 Performance comparison (40 planning iterations)
Proposed method RRT [ 26] Frontier [ 25]
Mean occupancy 1.2 5.1 3.0
Max. occupancy 26.3 75.2 47.9
Median iter. plan time [s] 65.4 200.6 13.4
Mean iter. plan time [s] 73.7 196.6 84.3
Max. iter. plan time [s] 174.4 239.6 1345.2
collision, as shown in Table 1by the maximum occupancy values exceeding the 50%
occupancy threshold. The convergence of the frontier planner is slower, as a result of
the over-estimation of the path utility by the choice of a single goal at each iteration.
In addition, paths are jerky, leading to longer time to cover the same area. As thefrontier planner does not explicitly minimise collision risk, the maximum occupancy
over the path is high. In contrast, the maximum occupancy of the proposed method
along the path is signiﬁcantly below the 50% occupied threshold.
Comparing the median runtime results shown in Table 1reveals that the RRT
planner is signiﬁcantly slower than our method, mainly because MI is computedover the entire map. Since frontier only queries the map for occupancy, its runtime is
signiﬁcantly smaller. However, its average runtime is similar to our method as occa-
sionally resolving longer path is required. It is worth noting that the runtime resultsare limited by the Python implementation of the Hilbert map. C++ implementation
of the Hilbert map proved to be two to three orders of magnitude faster, which makes
it suitable for online applications.
6 Conclusions
This paper introduces a novel method for exploration over continuous occupancymaps using stochastic functional gradient descent. This approach formalises explo-
ration as a variational problem, where optimisation is performed directly in the space
of trajectories. The functional objective of the proposed method explicitly optimisesboth safety and information collection over the entire path, ﬁnding the Next Best
Path . While this approach can be used with any type of occupancy map, it is highly
effective with Hilbert maps, where the introduced MI objective and its gradient can
be computed from a perturbed model of the map. Our proposed approach eliminates
the need for computing MI over the entire map as done in other exploration tech-niques. Rather, it computes variations to the path based on functional gradient of MI
which are efﬁciently derived in closed-form from the map model.
Comparisons with other exploration methods show that the proposed method
improves on both safety and MI. Point exploration methods, such as frontier, which
do not optimise the path selection, exhibit slower exploration rate. On the other
874 G. Francis et al.
hand, sampling-based exploration methods, such as [ 26], do not include safety in their
objective, hence the resulting paths tends to move closer to obstacles. Moreover, these
methods are computationally expensive due to the need to repeatedly sample the MIobjective over the entire path. In comparison our proposed method achieves similar
exploration rates to [ 26] while taking less time to compute and still maximising
safety.
References
1. Charrow, B., Kahn, G., Patil, S., Liu, S., Goldberg, K., Abbeel, P ., Michael, N., Kumar, V .:
Information-theoretic planning with trajectory optimization for dense 3D mapping. In: Pro-ceeding of Robotics: Science and Systems (2015)
2. Dong, J., Mukadam, M., Dellaert, F., Boots, B.: Motion planning as probabilistic inference
using gaussian processes and factor graphs. In: Proceeding of Robotics: Science and Systems(2016)
3. Elfes, A.: Using occupancy grids for mobile robot perception and navigation. Computer 22(6),
46–57 (1989)
4. Elfes, A.: Robot navigation: integrating perception, environmental constraints and task execu-
tion within a probabilistic framework. In: Reasoning with Uncertainty in Robotics (1996)
5. Francis, G., Ott, L., Marchant, R., Ramos, F.: Occupancy map building through bayesian
exploration. Int. J. Robot. Res. 38(7), 769–792 (2019)
6. Francis, G., Ott, L., Ramos, F.: Stochastic functional gradient for motion planning in contin-
uous occupancy maps. In: Proceeding of the IEEE International Conference on Robotics andAutomation (2017)
7. Francis, G., Ott, L., Ramos, F.: Stochastic Functional Gradient Path Planning in Occupancy
Maps (2017). arXiv:1703.00227
8. González-Baños, H.H., Latombe, J.-C.: Navigation strategies for exploring indoor environ-
ments. Int. J. Robot. Res. 21(10–11), 829–848 (2002)
9. Holz, D., Basilico, N., Amigoni, F., Behnke, S.: A comparative evaluation of exploration
strategies and heuristics to improve them. In: Proceedings of the European Conference onMobile Robots (2011)
10. Jadidi, M.G., Miro, J.V ., V alencia, R., Andrade-Cetto, J.: Exploration on continuous gaussian
process frontier maps. In: Proceedings of the IEEE International Conference on Robotics andAutomation (2014)
11. Jadidi, M.G., Miro, J.V ., Dissanayake, G.: Mutual information-based exploration on continuous
occupancy maps. In: IEEE/RSJ International Conference on Intelligent Robots and Systems(2015)
12. Juliá, M., Gil, A., Reinoso, O.: A comparison of path planning strategies for autonomous
exploration and mapping of unknown environments. Auton. Robot. 33(4), 427–444 (2012)
13. Julian, B.J., Karaman, S., Rus, D.: On mutual information-based control of range sensing robots
for mapping applications. Int. J. Robot. Res. 33(10), 1375–1392 (2014)
14. Lauri, M., Ritala, R.: Planning for Robotic Exploration based on Forward Simulation (2015).
arXiv:1502.02474
15. Marchant, R., Ramos, F.: Bayesian optimisation for informative continuous path planning. In:
Proceedings of the IEEE International Conference on Robotics and Automation (2014)
16. Marinho, Z., Boots, B., Dragan, A., Byravan, A., Gordon, G.J., Srinivasa, S.: Functional gradi-
ent motion planning in reproducing kernel hilbert spaces. In: Proceedings of Robotics Scienceand Systems (2016)
17. Mukadam, M., Yan, X., Boots, B.: Gaussian process motion planning. In: Proceedings of the
IEEE International Conference on Robotics and Automation (2016)
Functional Path Optimisation for Exploration in Continuous Occupancy Maps 875
18. O’Callaghan, S.T., Ramos, F.T.: Gaussian process occupancy maps. Int. J. Robot. Res. 31(1),
42–62 (2012)
19. Ramos, F., Ott, L.: Hilbert maps: scalable continuous occupancy mapping with stochastic
gradient descent. In: Proceedings of Robotics: Science and Systems (2015)
20. Robbins, H., Monro, S.: A stochastic approximation method. Ann. Math. Stat. 22(3), 400–407
(1951)
21. Stachniss, C.: Robotic Mapping and Exploration. Springer (2009)22. Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics. MIT Press, Cambridge (2005)23. V allvé, J., Andrade-Cetto, J.: Potential information ﬁelds for mobile robot exploration. Robot.
Auton. Syst. 69, 68–79 (2015)
24. Whaite, P ., Ferrie, F.P .: Autonomous exploration: driven by uncertainty. IEEE Trans. Pattern
Anal. Mach. Intell. 19(3), 193–205 (1997)
25. Yamauchi, B.: A frontier-based approach for autonomous exploration. In: Proceedings of the
IEEE International Symposium on Computational Intelligence in Robotics and Automation(1997)
26. Yang, K., Keat Gan, S., Sukkarieh, S.: A gaussian process-based RRT planner for the exploration
of an unknown and cluttered environment with a UA V . Adv. Robot. 27(6), 431–443 (2013)
27. Zucker, M., Ratliff, N., Dragan, A.D., Pivtoraiko, M., Klingensmith, M., Dellin, C.M., Bagnell,
J.A., Srinivasa, S.S.: CHOMP: covariant hamiltonian optimization for motion planning. Int. J.Robot. Res. 32(9–10), 1164–1193 (2013)
Stochastic Motion Planning for Hopping
Rovers on Small Solar System Bodies
Benjamin Hockman and Marco Pavone
1 Introduction
Small Solar System bodies, such as asteroids, comets, and irregular moons, have
become a key target for exploration due to their scientiﬁc interest, potential forresource extraction, and for studying impact mitigation strategies. While some infor-
mation about their chemical and structural properties can be obtained remotely, mea-
surements that constrain composition and physical properties require direct contact
with the surface at multiple locations and over extended periods of time [ 1]. Accord-
ingly, NASA and space agencies worldwide have recognized the need for roverscapable of controlled surface mobility [ 2–4].
However, the surface environment of small bodies presents many unique chal-
lenges for mobility, including their highly irregular shape and extremely reducedgravity (10–1000 µg), which prohibit conventional wheeled rovers that rely on sur-
face traction. Instead, hopping systems are more naturally suited for such environ-
ments, as they can traverse large distances over arbitrarily rough terrain with littleenergy. In fact, four small hoppers are currently en route to asteroid Ryugu aboard
JAXA ’s Hayabusa 2 spacecraft—a MASCOT lander developed by DLR [ 3] and three
MINERV A landers [ 4], which are equipped with internal momentum devices to pro-
vide a “kick” for hopping, albeit with minimal control.
To extend the idea of hopping to an architecture capable of targeted science, NASA
has invested in various new technologies to enable controlled mobility in micrograv-
ity. As part of the Evolvable Mars Campaign, Howe and Gernhardt et. al investigated
a pressurized, six-legged hopping excursion vehicle for human exploration of Mars’
B. Hockman ( B)
Robotic Mobility Group, Jet Propulsion Laboratory, California Institute of Technology, Pasadena,CA, USAe-mail: Benjamin.j.hockman@jpl.nasa.gov
M. Pavone
Department of Aeronautics and Astronautics, Stanford University, Stanford, CA, USAe-mail: pavone@stanford.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_60877
878 B. Hockman and M. Pavone
Fig. 1 Rovers designed for precise mobility on small bodies. Left : Hedgehog: a small (15–
25 cm), internally-actuated rover that uses three internal ﬂywheels to hop and tumble in micro-gravity. Right : A THLETE hopper: a pressurized excursion vehicle that uses six spring-loaded legs
to hop and transport humans on the surface of Phobos
Fig. 2 The high-level autonomy architecture for hopping rovers consists of four phases: (1) local-
ization, (2) trajectory planning, (3) hop execution, and (4) uncontrolled, ballistic dynamics
moon, Phobos [ 5]( s e eF i g . 1, right). The authors have been developing a small,
internally-actuated hopping rover called “Hedgehog,” which can perform various
motion primitives by applying torques to three orthogonal ﬂywheels, including long-
range hops as well as short, precise tumbling (see Fig. 1, left). The dynamics and
control of these motion primitives has been studied in detail, from analytical models
to high ﬁdelity simulations [ 6–8]. Experiments in various test beds have also vali-
dated these control laws, including a custom-built 6 DoF gravity-ofﬂoading test bed
[8] and a parabolic ﬂight campaign [ 9].
In addition to controllability, hopping rovers operating on distant bodies require a
high degree of autonomy , as communication suffers from long light-speed delays and
relies on a mothership relay, which may be infrequent. On Mars, wheeled rovers areequipped with visual perception, terrain classiﬁcation, and path planning algorithms
for autonomous mobility [ 10]. However, in contrast to wheeled rovers, which operate
through continuous interaction with the environment, hoppers can only apply forcesfrom rest on the surface and have no control of their trajectory mid-ﬂight. Thus,
autonomy for hopping systems requires a more discrete and sequential structure,
which can be decomposed into four phases: (1) localization, (2) trajectory planning,(3) hop execution, and (4) ballistic dynamics (see Fig. 2).
Localization on the surface of small bodies is an open area of research, which has
been addressed from two perspectives: (1) assuming an “eye-in-the-sky” mothership[11], and (2) mothership-independent, on-board perception, primarily through visual
odometry [ 12]. While each method has its strengths and weaknesses, any approach
is likely to produce pose estimates with some level of uncertainty.
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 879
Fig. 3 High-resolution images and shape models of two small bodies. Left : Asteroid 25143 Itokawa
(535 m, 6–9 µg), Right : Comet 67P Churyumov-Gerasimenko (4.3 km, 140–300 µg)
Hop execution addresses the problem of pushing off from the surface to achieve
some desired velocity vector and, as discussed for Hedgehog in [ 8], is highly
dependent on the rover architecture (e.g. actuation mechanisms, surface interac-tion, etc.). Generally, for a given hopper, the target velocity can only be executed
approximately—due to environmental uncertainty and control errors—and must obey
certain constraints, such as speed and direction limitations.
Dynamics about small bodies has a rich body of literature for orbiting spacecraft
[13], and to a lesser extent for surface interaction [ 14,15]. The key challenges lie
in accurate modeling of the gravity ﬁeld about irregularly shaped bodies and thephysical interactions with the surface. However, even the highest ﬁdelity gravity and
contact models rely on information that may not be available a priori via remote
observations, such as internal mass distribution and surface structure.
The focus of this paper is on phase II, planning , which is by far the least addressed
element in the autonomy stack. Planning seeks to answer the question:
What is the next best hop to perform, given a set of mission objectives, an estimate of the
rover’s location, an understanding of its capabilities, and a model of the world?
In other words, planning represents the rover’s decision-making module and is tightly
intertwined with all other elements in the mobility pipeline. Previous studies of
planning for hopping rovers have assumed highly simpliﬁed and deterministic modelsof the dynamics, localization, and the hopper itself. Bellerose, et al. derive analytical
control laws for a spherical hopper on a smooth, spherical asteroid with a coulomb
friction contact model, and exact localization [ 16]. This work is extended to the
case of smooth, ellipsoidal asteroids in [ 17], which also derives approximate speed
constraints to prevent escape trajectories. However, these results are founded on
oversimpliﬁcations of the body shape, gravity, and contact models, as evidenced byremote observations of highly uneven and rocky surfaces (see Fig. 3), and the chaotic
bouncing pattern of the Philae lander on comet 67P [ 18].
Statement of Contributions : In contrast to the approaches of [ 16,17], the goal of
this paper is not to derive closed-form analytical expressions for optimal control
laws, which requires a number of unrealistic simpliﬁcations. Instead, we proposea planning architecture that directly accounts for various sources of uncertainty,
and thus, produces control policies that are more robust to modeling, control, and
localization errors. The key idea is to shift from a “ﬁrst-principles” approach, to adata-driven approach, whereby high-ﬁdelity dynamics models can be used to simulate
instances of hopping trajectories from various uncertainty distributions.
880 B. Hockman and M. Pavone
Speciﬁcally, the contributions of this paper are twofold: First, we solve the prob-
lem of planning a single hop under uncertainty and assuming minimal bouncing
(Sect. 3). We develop a robust and efﬁcient algorithm for solving “Lambert’s orbital
boundary value problem”—the problem of ﬁnding an initial velocity to intercept a
target—in highly irregular gravity ﬁelds (Sect. 3.1). We then forward propagate con-
trol and gravity uncertainty (modeled as Gaussian mixtures) through the dynamicsto compute trajectory funnels and landing distributions for a given nominal trajec-
tory (Sect. 3.2). Then, we compute myopic policy gradients based on these landing
distributions to derive time/energy-optimal single-hop control policies (Sect. 3.3).
Second, in Sect. 4we extend the planning problem to the case where multiple hops
are required and structure the problem as an MDP . We apply a variant of Least Squares
Policy Iteration (LSPI) to derive approximately optimal control policies that are safe,efﬁcient, and amenable to real-time implementation on computationally-constrained
rover hardware. The performance of these policies is evaluated on a high ﬁdelity
dynamics simulator and compared to that of a greedy heuristic policy. Collectively,the methods presented in this paper constitute the ﬁrst ever study of uncertainty-
aware motion planning for hopping rovers—a crucial component of autonomy for
future exploration missions to small Solar System bodies.
2 Preliminaries
In this section, we present the dynamics models used to simulate the trajectories of
a hopping rover. Section 2.1details the forces acting on the rover during ballistic
ﬂight, and Sect. 2.2discusses the model used for the contact dynamics of bouncing
on the surface.
2.1 Ballistic Dynamics
A rover, R, at position rwith mass mand velocity vrelative to the asteroid body, B,
hops from rest at rt0with velocity vt0and impacts at rtfwith velocity vtf. The body
is represented as a closed polygonal mesh with ktriangular facets, where facet Fihas
outward normal Ni. The asteroid rotates at a constant angular velocity ωB=Ωˆbz
(Fig. 4).
The external forces ( Fe) acting on a hopping rover include gravitational forces (of
the primary and possibly tertiary bodies), solar radiation pressure (SRP), electrostatic
forces, and contact forces. It will also be convenient to represent the rover’s dynamicsin the rotating body frame, thus introducing effective centrifugal and Coriolis forces.
The total effective force is expressed as,
F=F
e−mωB×(ωB×r)−2mωB×v. (1)
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 881
Fig. 4 Dynamic model of rover Rhopping on body B, which is rotating at ωB=Ωˆbzand is
represented by a closed surface mesh consisting of ktriangular facets, Fi
In general, Fe(expressed in ˆb) is a function of not only position and velocity, but
also time. In this paper, we focus on the stationary case to derive time-invariant
control policies, which excludes forces that are periodic when expressed in the rotat-
ing body frame such as SRP and third body perturbations. This assumption is not
overly restrictive since SRP is typically three to six orders of magnitude weaker
than gravity in close proximity to small bodies, and most small bodies of interest
for exploration are either gravitationally isolated (e.g., Itokawa, Bennu, and Psyche)
or tidally locked secondaries (e.g., Phobos and Deimos). However, if periodic force
models are important, the methods in this paper can be generalized by augmenting
the rover’s “state” with a temporal state (e.g., body phase). Nutation of the body’s
spin axis can also be accounted for in this way, although most asteroids are believed
to be in stable spin about their major axis [ 19].
Gravity on small bodies may be orders of magnitude weaker than on Earth, but it
still represents the dominant force on rovers, so accurate modeling is essential. With
only shape information, the most accurate gravity model is a polyhedral model [ 20],
which leverages the divergence theorem to exactly model the gravitational potential
(U), attraction ( g=∇ U), gradient ( ∇∇U), and Laplacian ( ∇2U) of a constant den-
sity polyhedron as a summation over all facets and edges of the surface mesh. This
representation, while highly computational, is especially critical in close proximity
to irregular bodies (see, e.g., Fig. 3), where conventional models such as harmonic
expansions and mascons yield large errors [ 20]. We alleviate the computational bur-
den of evaluating the surface integral at each time step by precomputing the gravity
ﬁeld at regular grid points within the vicinity of the body ofﬂine , and then interpolat-
ing within this precomputed ﬁeld online. V alidation tests comparing this approximate
interpolation to exact evaluation suggest errors on the order of 0.01% for a 5 m ﬁeld
discretization on Itokawa using a 5000-facet shape model.
However, most small bodies are likely notconstant density, and internal density
variations are, in general, not know a priori. The polyhedral model may be super-
imposed with harmonics or mascons if in situ measurements are taken, but a model
882 B. Hockman and M. Pavone
ofuncertainty is perhaps more import for robust policy generation. We consider an
arbitrary parametric uncertainty model,
g=¯g+δg,δ g∼Pθ,θ∈Rk, (2)
where ¯g∈R3is the nominal modeled gravity vector, and δgis a random perturbation
from distribution Pθ. As a simple example, Gaussian uncertainty on the total mass
of the body could be encoded with a single parameter, δg∼N(0,σ2
M)¯g/|¯g|.
2.2 Contact Model
Finally, we require a model for the dynamics of the rover upon impact with the
surface. In some cases, it may be sufﬁcient to assume that the rover can achievea dead-stick landing (e.g., if it is equipped with a damping mechanism), but in
general, uncontrolled impact will cause the rover to rebound somewhat randomly. The
dynamic response of an impact event is a complex function of the physical properties
of both the surface and rover, and the speed and orientation of the rover upon contact.
Tardivel and V an wal, et al. developed high-ﬁdelity small body lander simulations tomodel the rebound and settling behavior of landers using rigid-body contact models
[14,15], and Murdock, et al. have studied low-velocity impact dynamics in granular
media [ 21]. However, for the motion planning problem, we only need a model of the
rebound distribution ,
v
t+
f∼Pζ(vt−
f,ζ( rtf)), (3)
where the rebound velocity, vt+
f, is a random variable dependent on the pre-impact
velocity, vt−
f, and a parametrized description of the surface properties, ζ(rtf)(e.g.
surface friction and elasticity). This allows us to abstract away the detailed contact
physics in a general way, whereby the rover can be modeled as a dimensionlessparticle. In practice, P
ζshould be ﬁt to the rebound dynamics observed on a higher
ﬁdelity model, including surface shape uncertainty. For the data used in Sect. 4.2,w e
use a kernel density estimator to ﬁt Pζto the rebound dynamics of a cube impacting
a ﬂat surface with some friction and elasticity. The simulation is stopped when
||vt−
f||τ i,g r a v i t y ﬁ e l d ¯g(r)
1: initialize guess for vt0(τ1)(e.g., as Lambert solution in spherical gravity ﬁeld)
2:for i=1,..., ndo
3: Use shooting solver to solve for vt0(τi)and Jacobian, J(τi):
vt0(τi),vtf(τi),J(τi)← Solve/parenleftbig
rt0,rtf,¯g(r),vt0(τi)/parenrightbig
4: Compute ﬁrst-order estimate of partial vt0w.r.t.τ:∂vt0
∂τ← J(τi)−1vtf(τi)
5: Initial guess for τi+1:vt0(τi+1)← vt0(τi)+∂vt0
∂τ(τi+1−τi)
6: isV alid (i)← Check for trajectory surface penetration
7:end for
Output: v t0,vtf∈R3×n,J∈R3×3×n, isV alid ∈{0,1}n
3.2 Uncertainty Propagation
Computing the nominal Lambert solutions for planning a single hop helps to inform
what trajectories may be beneﬁcial for targeting a speciﬁc impact location, but it
assumes a perfect gravity model, perfect control accuracy, and perfect state informa-tion. It is also important to understand how sensitive these solutions are to various
sources of uncertainty. In other words, we would like to predict the impact distri-
bution by treating the gravity ﬁeld, control accuracy, and state estimate as random
variables rather than known quantities.
In general, there are two approaches to density estimation for nonlinear functions
of random variables: (1) analytical propagation of simpliﬁed uncertainty modelsthrough linearized dynamics, and (2) sampling-based techniques. While sampling-
based techniques (e.g., kernel methods) are amenable to arbitrarily complex dynamics
and uncertainty models, they typically assume some measure of “local smoothness”
and do not scale well to higher dimensions. On the other hand, analytical methods
can be much more sample efﬁcient for high-dimensional uncertainty models (e.g., by
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 885
approximating gradients), but are often restricted to simple uncertainty models and
locally linear dynamics. The ballistic dynamics of hopping are indeed smooth and
linearizable, but also depend on the collision with a highly irregular surface. Accord-ingly, we decompose the density estimation problem into two phases: (1) Gaussian
error propagation through the linearized ballistic dynamics, and (2) projection onto
the irregular surface mesh.
For error propagation through the ballistic dynamics, we assume Gaussian uncer-
tainty on the gravity, g, according to Eq. ( 2) and Gaussian uncertainty on the control
(v
0) and initial state ( r0) according to v0∼N(μ v0,Σ v0), and r0∼N(μ r0,Σ r0).
More generally, the joint uncertainty of g,v0, and r0may be expressed as,
⎡
⎣δg
v0
r0⎤
⎦∼N(μδg,v0,r0,Σδg,v0,r0), μ δg,v0,r0=⎡
⎣0
μv0
μr0⎤
⎦,Σδg,v0,r0=/bracketleftbiggΣδg 0
0Σv0,r0/bracketrightbigg
,
(4)
where Σδg∈Sk
+is the covariance of the gravity model, and Σv0,r0∈S6
+is the joint
state-control covariance. A two-sided ﬁnite difference approximation of the Jacobian,
Jv0,r0=[∂rf/∂θ ∂ rf/∂v0∂rf/∂r0]∈ R3×(k+6), can be approximated with 2 (k+
6)simulations (note that ∂rf/∂v0is obtained for free from Algorithm 1). With this
linear approximation, the impact covariance about rfcan be computed as Σrf=
Jv0,r0Σδg,v0,r0JT
v0,r0.
To get an impact distribution over the surface , we then project this covariance
along vf, whereby the probability of impact on any facet can be computed as,
P(rtf∈Fi)≈ai/parenleftBigg
e−1
2rT
FiΣ−1
uwrFi
−2π√|Σuw|/parenrightBigg
max/braceleftBig
ˆNi·ˆcv,0/bracerightBig
, (5)
where Σuwv=cRbΣrfcRbT=⎡
⎣Σuwσuσv
σwσv
σvσuσvσwσ2
v⎤
⎦. (6)
Here, Rb
cis the rotation matrix from ˆbtoˆc,rFi∈R2is the vector from rtfto the
center of facet Fi(in/angbracketleftˆcu,ˆcw/angbracketright), and aiis the area of facet Fi(see Fig. 7,l e f t ) .
Figure 7(right) illustrates this two-phase error propagation method on three trajec-
tories from the example in Fig. 6, where trajectory funnels bound a 90% conﬁdence
envelope, and surface color denotes projected uncertainty distribution, Σrf. Inter-
estingly, these funnels are not always divergent along the trajectory as one might
expect (e.g. the blue trajectory). While the total magnitude of the error ( |Σuwv|) does
generally grow, its projection along the trajectory (i.e., |Σuw|) can shrink, indicating
that errors can grow in time while shrinking in the cross-track directions. Also, while
this analysis assumed Gaussian uncertainty, it can be trivially extended to Gaussianmixture models by simply taking a weighted sum of the components.
886 B. Hockman and M. Pavone
Fig. 7 Left : Landing distributions are computed using covariance propagation and projection.
Right : Error propagation for three trajectories on Itokawa, corresponding to position uncer-
tainty, Σr0=Iσ2
r0,σr0=1 m, velocity error Σv0=Iσ2
v0,σv0=0.03||v0||, and gravity error σg=
0.03||¯g||
One drawback of the projection in Eq. 5is that it does not capture “shadowing”
effects, which is the case whenever ˆNi·ˆcv>0. For surfaces that are highly irregular
in the vicinity of rtfor for shallow, glancing impacts, Eq. 5may be augmented with a
“ray-tracing” collision checker to accurately project shadows—an established, albeitmore computational technique commonly used for graphics rendering.
3.3 Optimal Hop Selection
Equipped with an algorithm to compute exact solutions for Lambert’s two-pointboundary value problem (Sect. 3.1) and a method for propagating uncertainty to
compute approximate landing distributions (Sect. 3.2), we can now formulate an
optimization problem for selecting the best (nominal) hop velocity, μ
∗
v0. We consider
the following optimization problem:
minimize
μv0J(μ v0)=E/bracketleftbig
λuvT
0Suv0+λTτ(r0,v0)−V(rf|r0,v0)/bracketrightbig
subject to μv0∈A(r0)
where v0∼N(μ v0,Σ v0), r0∼N(μ r0,Σ r0)(7)
where the additive cost function, J, represents an expectation of the control effort
(weighted by λu), ﬂight time, τ(weighted by λτ), and the negative “value,” V,o f
impacting at location rf. The surface value map, V, may encode various mission
objectives including the distance to the goal, possible hazards, or even the expected
future rewards in the context of sequential hopping (to be addressed in Sect. 4). The
constraint on μv0belonging to the action space of the rover, A, at position estimate,
(μ r0,Σ v0), can be quite naturally constructed as the intersection of two convex sets
representing the speed ,||μv0|| ≤ vmax, and direction , cos−1(μ v0·ˆN(r0)/||μv0||)≤
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 887
π/2−θmin, which is a cone about the local surface normal vector, ˆN(r0), bounded
by the minimum elevation angle, θmin.
Assuming that Vprimarily encodes a “distance-to-goal” metric, choosing one
solution from each homotopy class of trajectories (e.g., the blue and pink solution
families in Fig. 6) can serve as good initializations for a gradient descent solver.
An estimate for the global minimum can then be obtained by comparing the localminimum obtained from each homotopy class. For example, in the case where λ
τ/greatermuch
λu,μ∗
v0belongs to the most direct class (blue trajectories in Fig. 6), whereas for
λu/greatermuchλτ,μ∗
v0belongs to the longer, albeit less energy intensive solution class (pink
trajectories in Fig. 6). The details of the gradient descent solver are omitted for brevity,
b u ta tah i g hl e v e l , E(V)can be derived from Eq. ( 5) and ﬁnite difference estimates
of the cost gradient, ∇μv0J, can be obtained in a similar fashion to the Jacobian in
Sect. 3.2.
4 Sequential Hop Planning
Recall the sequential autonomy architecture illustrated in Fig. 2. For mobility tasks
that require multiple hops (e.g., traversing long distances or correcting for unfa-vorable bouncing), we must extend the myopic strategies developed in Sect. 3for
planning over a longer horizon. This requires some notion of how immediate actions
facilitate future actions and how this sequence of actions achieves certain missionobjectives. A natural framework for modeling this inherently discrete and stochastic
planning problem is a Markov Decision Process (MDP). In contrast to “classical”
open-loop motion planning algorithms (e.g., combinatorial and sampling-based) thatsearch for feasible (or even “optimal”) reference trajectories, MDPs provide a more
explicit representation of uncertainty and a powerful reward structure for encod-
ing more complex mission objectives (i.e., not just “steering towards the goal”).Section 4.1outlines how the planning problem can be structured as an MDP , Sect. 4.2
discusses a sample-efﬁcient reinforcement learning method for learning approxi-
mate state-action value (Q-) functions and implicitly, approximately optimal policies.Finally, Sect. 4.4compares the performance of learned control policies to heuristics
proposed in [ 6], and evaluates performance robustness to modeling errors.
4.1 Hopping as an MDP
In accordance with the “classic” inﬁnite horizon MDP formulation, we cast the
sequential hop planning problem as the ﬁve-tuple, (S,A,T,R,γ)—the state space,
action space, transition model, reward model, and discount factor. However, unlike
most planning problems in robotics that force an MDP structure by temporally dis-cretizing an inherently continuous-in-time process, hopping has a natural sequential
decomposition, where transitions are marked by the eventual settling of each hop.
888 B. Hockman and M. Pavone
State Space : At rest on the surface, the rover’s state is simply its position and ori-
entation. However, assuming that a lower level controller can reorient the rover as
needed (as is the case for Hedgehog), we can collapse the state to just the surfaceposition—an irregular manifold in R
3. For nearly-spherical bodies, spherical coordi-
nates (i.e., latitude/longitude) may be sufﬁcient to uniquely parametrize the surface,
but for highly irregular bodies such as those in Fig. 3, more elaborate map projec-
tions may be required. More generally, other state formulations might also include
the rover’s internal state (e.g., battery charge), its state history (e.g., in the context of
a coverage problem), or its belief state (in the case of partial observability).
Action Space : In its most fundamental form, the action space of a hopping rover
can be described by raw actuators (e.g., three motors and brakes for Hedgehog).However, for motion planning it is more convenient to consider the rover’s action
as its velocity immediately after hopping—a higher level abstraction of the action
space that leverages a lower level hopping controller (e.g., the controller discussedin [8]) and is amenable to the ballistic particle simulator presented in Sect. 2.
Moreover, it is critical that this velocity vector be expressed in a global reference
frame—rather than a local surface frame—to maintain continuity in the transitiondynamics; informally, T(·|s,a)≈T(·|s+δs,a). In other words, action descriptions
that depend on the local surface slope (e.g., “spin ﬂywheel number 2” or “hop left”)
can yield sharp changes in Tfor small changes in state on irregular terrain, whereas
global descriptions (e.g, “hop north”) are unaffected by local changes in topography.
However, local surface properties impose critical constraints on the feasible action
space of the rover, A(s)(e.g., that the velocity vector must lie within some “fric-
tion cone” about the local surface normal). Thus, expressing actions in a global
frame helps to “smooth” the dynamics (and consequently, the Q-function) but comes
at the cost of requiring sharp discontinuities in A(s), suggesting that it may be
advantageous to store a policy implicitly through a Q-function approximator (i.e.
π(s)=argmax
a∈A(s)ˆQ(s,a)) rather than explicit function approximators on π.
Reward Model : The reward model is a mission designer’s tool for encoding various
mission objectives, such as “visit sites A, B, and C under time and energy constraints
while avoiding hazards D and E.” Thus, a reward function can take many forms and,in general, may be updated based on new information gathered or new objectives.
We consider a general formulation that penalizes the time and energy required for
each hop, incentivizes a set of n
ggoal regions, and penalizes a set of nhhazardous
regions. In summary, the state, action and reward models we consider here are:
s∈S2,a∈A(s)⊂R3,
R(s,a)=− E[τ(s,a)]/τmax−λuE[u(s,a)],R(sgi)=ri,R(shj)=rj,(8)
where E[τ(s,a)]and E[u(s,a)]are the expected time and energy required to execute
action aat state s. States sgi∈Sgiandshj∈Shjare states within the goal and hazard
regions, with associated rewards ri>0 and rj<0, respectively. τmaxis a maximum
travel time, and λuweights the control effort.
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 889
4.2 Reinforcement Learning Method
The transition model, T, is unknown. In the case of minimal bouncing, approxima-
tions of the dynamics such as those discussed in Sect. 3.2may work, but in general,
a series of elastic bounces (e.g., Fig. 5) induces chaotic, highly non-Gaussian, mul-
timodal transition dynamics. Without discretization of the state or action spaces, T
is extremely difﬁcult to approximate. Accordingly, model-free methods are bettersuited for this domain, whereby simulations (discussed in Sect. 2) can be used to
generate large sets of transition data ofﬂine.
One popular technique for such batch, off-line, off-policy, model-free RL is Least
Squares Policy Iteration (LSPI), which, as originally described in [ 24], uses a linear
function approximator for the Q-function, and an exact, ﬁxed-point projection for
policy evaluation (LSTD-Q):
solve: (ˆA
n−γˆBn)θ=ˆbn,where ˆbn=1
nn/summationdisplay
i=1ρi[φ(si,ai)ri],
ˆAn−γˆBn=1
nn/summationdisplay
i=1ρiφ(si,ai)/bracketleftbig
φT(si,ai)−γφT(s/prime
i,π(s/prime
i))/bracketrightbig
,(9)
π(s)=argmax
a∈A(s)/bracketleftbig
φT(s,a)θ/bracketrightbig
. (10)
Here,φ(s,a)is the state-action feature vector, and ρis a weight vector that sums to
one. Like most approximate RL methods, LSPI is not guaranteed to yield optimal
policies, but it is a stable algorithm. That is, it will either converge or it will oscillatein an area of the policy space where policies have suboptimality bounded by some
approximation error, ε, which is highly dependent on the richness of the feature space
and the coverage of the sampling distribution [ 24]. In practice, this bound is often
quite conservative and LSPI typically converges in very few iterations.
One of the most important features of LSPI for deriving hopping policies is its
amenability to off-policy exploration strategies, which provides the ability to reuselarge data sets, and thus, relearn a policy for a new reward structure on the ﬂy.
The weight vector, ρ, provides a convenient way to preferentially bias previously
collected samples via importance weighting (i.e.,ρ
i=p(si,ai)/q(si,ai), where p
is the desired distribution and qis the sample distribution); qmay be approximated
directly from samples via kernel density estimation, and pmay be chosen arbitrarily
(e.g., a uniform distribution).
890 B. Hockman and M. Pavone
4.3 Practical Considerations
Data Collection : An off-line simulator provides large ﬂexibility for data collection.
For a given initial state, s0, and policy, π, state-action samples can be biased towards
more likely regions (e.g., through direct Monte Carlo sampling, or importance sam-
pling/variance reduction techniques). However, in the more general case when s0is
uncertain, or the reward structure may change (and thus, π), we would like a good
ﬁt of ˆQover a much broader range of the state-action space. Thus, a mostly “pure
exploration” strategy is preferred, with a combination of full episode rollouts and
periodic restarts, perhaps with some bias towards “hard-to-reach” regions.
Feature Engineering : Linear function approximation relies on a rich set of features
over the state-action space to produce good estimates for the Q-function (i.e., small||φ
T(s,a)θ∗−Q∗||2). At the same time, the feature set must be amenable for com-
puting the argmax in Eq. ( 10) for policy extraction. Accordingly, we decouple state
and action features such that φ(s,a)=φs(s)φa(a), where φscan be arbitrarily com-
plex in the state while φaremains “simple” enough for optimization. We construct
φsfrom a set of “hand-crafted” features that leverage domain knowledge (e.g. local
geopotential and surface slope) and a set of distributed basis functions—namely,a Fourier basis (similar to [ 25]), which is more naturally suited to spherical state
domains than say, polynomials or RBFs. The action features are simple monomi-
a l so ft h ef o r m φ
a(v)=vi
xvj
yvk
z, where i+j+k≤m, such that a polynomial root
solver can compute all local minima exactly.
4.4 Evaluation of Learned Policies
As a preliminary case study, we consider a notional mission scenario on Asteroid
Itokawa in which the hopper must reach a target location in minimum time. Thereward model (Eq. ( 8)) is deﬁned as R(s
g)=+ 1 and R(s,a,s/prime)=−τ/τ max, where
τmax=10 h, and γ=11. Approximately ﬁve million trajectories were simulated over
a broad range of the state-action space, sampling from a Gaussian distribution on thegravity ﬁeld ( σ
g∼5%) and a rebound distribution as discussed in Sect. 2.2, with a
mean restitution of 0.65. Uniformly random mini-batches of size 100,000 were used
in each iteration of LSPI. The action space, A(s), is speed constrained by vmax=
10 cm/s and direction constrained by 40◦≤β≤50◦, where the elevation angle, β,
must lie within an annular cone about the local surface normal (in accordance with
the hopping constraints for Hedgehog, derived in [ 8]). With this problem deﬁnition,
LSPI was able to converge to a small error, ||θi−θi−1||<10−6, in only a few tens
of iterations and within tens of minutes on a laptop (though, signiﬁcant speedupsmay be achieved with a more efﬁcient implementation).
1Non-discounting is stable since the reward is always negative and each episode must terminate.
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 891
Fig. 8 T op : Three rollouts of the learned policy. The surface color map shows the optimal value
function under π∗.Bottom : Two rollouts of the “hop-to-the-goal” heuristic policy, where the color
map shows the difference between the learned and heuristic value functions ( /Delta1Q=Qπ∗−Qπh)
Figure 8shows a few example trajectories comparing the performance of the
learned policy, π∗, with a “hop-towards-the-goal” heuristic policy, πh, that attempts
to take the most direct path to the goal. From 1000 policy rollouts, the mean time
to reach the goal from deployment was 5.1 h for the learned policy, and 7.6 h forthe heuristic policy. The color map in the top ﬁgure shows the optimal value, Q
∗,
at each point on the surface, and due to the reward structure deﬁned above, it also
represents the expected time to reach the goal (as a function of τmax). Not surprisingly,
Q∗decays away from the goal region. The color map in the bottom ﬁgure shows
the value margin of the optimal policy over the heuristic policy, suggesting that the
heuristic policy has difﬁculties on sloped surfaces and states farther from the goal.The second hop of the blue trajectory in the top ﬁgure illustrates how the learned
policy enables the hopper to perform local adjustments to better position itself for
future hops—in this case, by performing a small backwards hop off of a slopedregion. The last hop of the green trajectory highlights another interesting learned
behavior: the rover hops uphill from the goal region, “understanding” that it is likely
to tumble downhill, thereby leveraging the dynamics of the environment without
ever having explicitly learned a model.
An important consideration when learning in simulation and executing in the real
world is robustness to modeling errors. This “transfer learning” problem for asteroid
environments may have three major types of modeling errors: (1) the asteroid’s shape,
(2) its surface properties, and (3) its density/gravity. While this learning methoddoes require an accurate shape model at a global scale, it is insensitive to smaller
scale variations due to the global-frame representation of the action space. That is,
unanticipated deviations in local topography only affect the constraints for policyextraction, not the optimal value function itself. As a preliminary study of robustness
892 B. Hockman and M. Pavone
to contact modeling errors, we rolled out the learned policy in an environment with a
different contact model—speciﬁcally, one with higher surface elasticity and one with
lower elasticity. For the more elastic case, 1000 policy rollouts exhibited signiﬁcantly
longer traverse times, with a mean of 8.2 h. These trajectories often bounce off course
or overshoot the goal, requiring major corrections. On the other hand, policy rollouts
in an environment with reduced elasticity actually exhibit better performance, with a
mean traverse time of only 4.7 h. This result suggests that one should err on the side of
overestimating the surface elasticity for simulations. Finally, although density/gravity
models are likely to be fairly accurate from preliminary surveying by the mothership,simulations can use a conservative under approximation of gravity for safe policy
transfer (i.e., so that the hopper does not overshoot it’s target or reach escape velocity).
5 Conclusions
In this paper, we presented an uncertainty-aware approach to motion planning for
hopping rovers on small Solar System bodies. We ﬁrst examined the problem of
planning a single hopping trajectory using a model-based approach, computing exact
solutions for impact targeting, propagating uncertainty, and deriving optimal hopsfrom a myopic policy gradient. We then cast the sequential planning problem as an
MDP , proposed an ofﬂine, off-policy, model-free RL method, and evaluated learned
policies against heuristic policies and robustness to modeling errors.
This paper leaves numerous important extensions open for further study. First, we
would like to explore various additional mission scenarios that include expert-deﬁned
hazards (e.g. pits and caves), multiple target sites, and constraints on mothershipcommunication and solar recharge. Localization errors, or state uncertainty, is another
important aspect to consider. Future work will consider extensions of this approach
that are amenable to partial observability (POMDPs). Finally, learned policies shouldbe validated in a higher ﬁdelity simulation environment that captures cm-scale contact
interactions as well as simulated visual odometry.
Acknowledgements This work is supported by NASA under the Innovative Advanced Concepts
program.
References
1. Castillo, J., Pavone, M., Nesnas, I., Hoffman, J.A.: Expected science return of spatially-
extended in-situ exploration at small Solar System bodies. In: IEEE Aerospace Conference(2012)
2. Ambrose, R., Nesnas, I.A.D., Chandler, F., Allen, B.D., Fong, T., Matthies, L., Mueller, R.:
NASA technology roadmaps: TA 4: robotics and autonomous systems. Technical report, NASA(2015)
Stochastic Motion Planning for Hopping Rovers on Small Solar System Bodies 893
3. Dietze, C., Herrmann, F., Kuß, S., Lange, C., Scharringhausen, M., Witte, L., van Zoest, T.,
Yano, H.: Landing and mobility concept for the small asteroid lander MASCOT on asteroid1999 JU3. In: International Astronautical Congress (2010)
4. Y oshimitsu, T., Kubota, T., Nakatani, I., Adachi, T., Saito, H.: Micro-hopping robot for asteroid
exploration. Acta Astronaut. 52(2–6), 441–446 (2003)
5. Abercromby, A.F.J., Gernhardt, M.L., Chappell, S.P ., Lee, D.E., Howe, A.S.: Human explo-
ration of phobos. In: IEEE Aerospace Conference (2015)
6. Allen, R., Pavone, M., McQuin, C., Nesnas, I.A., Castillo-Rogez, J.C., Nguyen, T.N., Hoffman,
J.A.: Internally-actuated rovers for all-access surface mobility: theory and experimentation. In:Proceedings IEEE Conference on Robotics and Automation (2013)
7. Reid, R.G., Roveda, L., Nesnas, I.A.D., Pavone, M.: Contact dynamics of internally-actuated
platforms for the exploration of small solar system bodies. In: Proceedings of i-SAIRAS (2014)
8. Hockman, B., Frick, A., Nesnas, I.A.D., Pavone, M.: Design, control, and experimentation of
internally-actuated rovers for the exploration of low-gravity planetary bodies. J. Field Robot.34(1), 5–24 (2016)
9. Hockman, B., Reid, R.G., Nesnas, I.A.D., Pavone, M.: Experimental methods for mobility
and surface operations of microgravity robots. In: International Symposium on Experimental
Robotics (2016)
10. Bajracharya, M., Maimone, M.W., Helmick, D.: Autonomy for mars rovers: past, present, and
future. IEEE Comput. 41(12), 44–50 (2008)
11. Higo, S. Nakatani, I., Y oshimitsu, T.: Localization over small body surface by radio ranging.
In: Proceedings of Space Sciences and Technology Conference (2005)
12. So, E.W.Y ., Y oshimitsu, T., Kubota, T.: Relative localization of a hopping rover on an asteroid
surface using optical ﬂow. In: SICE Anual Conference (2008)
13. Scheeres, D.J.: Orbit mechanics about asteroids and comets. AIAA J. Guid. Control. Dyn.
35(3), 987–997 (2012)
14. Tardivel, S., Scheeres, D.J., Michel, P ., V an wal, S., Ánchez, P .S.: Contact motion on surface
of asteroid. AIAA J. Spacecr. Rocket. 51(6), 1857–1871 (2014)
15. V an wal, S., Tardivel, S., Scheeres, D.J.: High-ﬁdelity small body lander simulations. In: Inter-
national Conference on Astrodynamics Tools and Techniques (2016)
16. Bellerose, J., Scheeres, D.J.: Dynamics and control for surface exploration of small bodies. In:
AIAA/AAS Astrodynamics Specialist Conference and Exhibit (2008)
17. Klesh, A., Bellerose, J., Kubota, T.: Guidance and control of hoppers for small body exploration.
In: International Astronautical Congress (2010)
18. Hand, E.: Philae probe makes bumpy touchdown on a comet. Science 346(6212), 900–901
(2014)
19. Sharma, I., Burns, J.A., Hui, C.-Y .: Nutational damping times in solids of revolution. Mon.
Not. R. Astron. Soc. 359(1), 79–92 (2005)
20. Werner, R.A., Scheeres, D.J.: Exterior gravitation of a polyhedron derived and compared with
harmonic and mascon gravitation representations of asteroid 4769 castalia. Celest. Mech. Dyn.Astron. 65(3), 313–344 (1996)
21. Murdoch, N., Martinez, I.A., Sunday, C., Zenou, E., Cherrier, O., Cadu, A., Gourinat, Y .: An
experimental study of low-velocity impacts into granular material in reduced gravity. Mon.
Not. R. Astron. Soc. 468(2), 1259–1272 (2017)
22. Gooding, R.H.: A procedure for the solution of lambert’s orbital boundary-value problem.
Celest. Mech. Dyn. Astron. 48(2), 145–165 (1990)
23. Woollands, R.M., Y ounes, A.B., Junkins, J.L.: New solutions for the perturbed lambert problem
using regularization and picard iteration. AIAA J. Guid. Control. Dyn. 38(9), 1548–1562 (2015)
24. Lagoudakis, M.G. Parr, R.: Least-squares policy iteration. J. Mach. Learn. Res. 4(Dec), 1107–
1149 (2003)
25. Konidaris, G., Osentoski, S., Thomas, P .: V alue function approximation in reinforcement learn-
ing using the Fourier basis. In: Proceedings AAAI Conference on Artiﬁcial Intelligence (2011)
Perception-Aware Motion Planning
via Multiobjective Search on GPUs
Brian Ichter, Benoit Landry, Edward Schmerling and Marco Pavone
1 Introduction
The ability of an autonomous robotic system to localize itself in an environment is
fundamental to robust operation in the ﬁeld. While recent advances in perceptionhave enabled this capability in a wide range of settings, the associated uncertainty
can still vary signiﬁcantly during operation. Often this is due to variations in envi-
ronmental knowledge, particularly in environments derived through simultaneous
localization and mapping, or due to intrinsic environmental qualities, e.g., texture or
lighting. Despite this, navigation in these environments is often posed as a motionplanning problem agnostic to localization, operating under the assumption that the
environment is known yet the ability of a robot to localize itself against and track a
trajectory is independent of its context.
In this paper, we approach the perception-aware motion planning problem,
whereby we seek low-cost motion plans subject to a constraint on perception local-
ization quality, in order to identify robust, well-localized plans. Unfortunately, thecomplexity of modern perception techniques and their history dependence make it
difﬁcult to quantify localization uncertainty, both computationally and theoretically,
B. Ichter (B)·B. Landry ·M. Pavone
Department of Aeronautics and Astronautics, Stanford University,Stanford, CA 94305, USAe-mail: ichter@stanford.edu
B. Landry
e-mail: blandry@stanford.edu
M. Pavone
e-mail: pavone@stanford.edu
E. Schmerling
Institute for Computational and Mathematical Engineering, Stanford University,Stanford, CA 94305, USAe-mail: schmrlng@stanford.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_61895
896 B. Ichter et al.
and thus a key contribution of this work is to perform perception-aware motion
planning with a multiobjective search on both cost and a perception heuristic to
identify promising motion plans. By considering trajectory cost and perception inde-pendently, both the problem formulation and algorithm can admit a wide range of
perception heuristics applicable to modern closed-loop SLAM methods, including
heuristics learned from empirical measurements of perception error. We propose asampling-based algorithm to approximate globally optimal plans in complex envi-
ronments, the additional burden of considering perception and planning in tandem
is offset through algorithmic design for massive parallelization on GPUs. We termthis algorithm the Multiobjective Perception-Aware Planning (MPAP) algorithm.
Although MPAP leverages a heuristic to estimate localization uncertainty, we note
that, if an accurate generative model of perception error is available, the robustnessof plans may be fully certiﬁed using Monte Carlo (MC) methods.
1.1 Related Work
While research has traditionally focused on improving methods for planning and per-ception, without much thought towards their later coupling, several methods to fusethe two have been put forward. One possible approach is to formally combine them
in a partially observable Markov Decision Process (POMDP) framework [ 1,2]. This
approach, however, has been shown to quickly become intractable as the complex-ity or dimension of the problem increases. Many works have sought to reduce this
computational load by applying search techniques to graphical-representations of the
belief space. For example, the Belief Roadmap (BRM) was introduced in [ 3], which
applies ideas from probabilistic roadmap methods to planning in the belief space. The
Rapidly-exploring Random Belief Trees algorithm [ 4] similarly adapts the rapidly-
exploring random trees algorithm to planning in the belief space. Along with the useof sampling-based approaches, there exists a large body of work (including the above)
that lessens the computational load of belief space planning through local approx-
imations, receding horizon techniques, and simplifying assumptions like linearity,Gaussian belief spaces, stabilization phases, and maximum likelihood observations
[3–10]. While these works approach the problem of perception-aware planning in
a principled manner, the required assumptions and computational load of trackingbelief states throughout the search can limit their applicability on high-dimensional
problems or problems with very complex, difﬁcult-to-quantify perception, as is the
case with many modern perception methods. Our approach simpliﬁes the problem by
avoiding the computation of the belief state during the graph search through instead
only considering a perception heuristic and verifying the ﬁnal result with MC. MonteCarlo veriﬁcation is done using the best available perception system model, and as
an MC method it is asymptotically exact up to the model ﬁdelity.
The ﬁeld of active perception also considers the coupling of planning and percep-
tion, where the research problem is focused on computing robot motions that help
minimize the uncertainty of localization and mapping [ 11–13]. In contrast to these
Perception-Aware Motion Planning via Multiobjective Search … 897
works, the problem addressed in this paper adds an additional objective of ensuring
high quality motion plans with respect to a dynamics-based cost objective. Com-
pared to the active perception literature, we exchange modeling ﬁdelity in a robot’sperception systems for computational tractability in directly considering the tradeoff
between perception quality and motion plan cost.
Some of the most similar works to our own have approached the problem as
perception-aware motion planning. In [ 14] a perception heuristic to capture the
feature-richness of an environment model is introduced and used within a rapidly-
exploring random tree (RRT) framework as a bound on individual edge, i.e., localconnection, quality. Another work, [ 15], proposes an approach to incorporate pho-
tometric information into the planning process, which is used as part of a weighted
cost metric with path cost in an RRT search. Lastly, [ 16] considers an uncertainty-
constrained planning problem within a receding horizon framework to compute fea-
sible plans with mixed-integer linear programming. In contrast to these works, we
pose the problem as a bound on localization uncertainty (through a heuristic) of theentire trajectory. This shifts the approach more towards global motion plans and
allows the use of more general heuristic formulations that can capture the history
dependence of localization and trajectory deviation, though the proposed heuristics
in these works too can be used in this framework. As a brief example, consider a
robot executing a trajectory in a learned environment that relocalizes (i.e., performsloop closure) given enough learned features in view. A weighted cost and localization
uncertainty metric is unable to capture the possibility of future decreases in local-
ization uncertainty and an edge-wise or limited time horizon approach may rejecttrajectories that would soon relocalize or conversely allow trajectories created from
semi-localized edges that is never capable of full relocalization and continues to drift.
Finally, we note that the concept of multiobjective search to balance uncertainty
with plan cost has been considered previously in motion planning, notably in [ 17]. We
utilize the same multiobjective search accelerated through massively parallel GPU
implementation put forth by the Parallel Uncertainty-aware Multiobjective Planning(PUMP) algorithm [ 17]. However, while PUMP considers only uncertainties internal
to the system, this work considers external uncertainties through perception. Addi-
tionally, in order to handle the added complexity of modern perception solutions,such as the Google Tango we experimentally demonstrate on, we instead employ a
perception-heuristic constraint, and consider it in the objective search.
1.2 Statement of Contributions
In this work, we present the Multiobjective Perception-Aware Planning (MPAP)
algorithm, which computes low-cost, well-localized, robust motion plans through
perception-aware planning. Built on a sampling-based graphical representation of the
state space, the algorithm performs a multiobjective search to identify a set of Paretooptimal motion plans (considering cost and a perception heuristic). By considering
cost and perception separately, the perception heuristic can consider the history-
898 B. Ichter et al.
dependence of the problem while being general enough to mirror modern perception
techniques. We present one such heuristic, in this case based on a trained neural
network, that maps sensor and estimation outputs of a robotic system to distributionsof localization error rates. The lowest-cost trajectory contained in the Pareto optimal
set can then be certiﬁed to be well-localized and robust through asymptotically exact
Monte Carlo methods. The entire algorithm is designed speciﬁcally to be massivelyparallel and thus offset the burden of perception-awareness through the computational
breadth of GPUs.
We demonstrate the efﬁcacy of MPAP through both numerical and physical exper-
iments in mapped environments. The numerical experiments demonstrate both that
perception-aware plans can be generated on the order of seconds and that these
plans are signiﬁcantly more robust than their perception-agnostic counterparts. Thephysical experiments were performed by a quadrotor ﬂying with a Google Tango
smartphone performing localization in a previously mapped room. The resulting tri-
als compare perception-agnostic and perception-aware trajectories, ﬁnding that theperception-agnostic ﬂights crash several times due to loss of localization while the
perception-aware ﬂights complete all trajectories safely.
1.3 Organization
The remainder of this work is organized as follows. Section 2details the problem
formulations considered herein. Section 3formally describes the MPAP algorithm.
Section 4demonstrates the algorithm’s efﬁcacy and timing over a series of simula-
tions. Section 5demonstrates the algorithm and a learned heuristic through physi-
cal experiments, further motivating the need for perception-aware planning. Lastly,
Sect. 6summarizes our ﬁndings and offers directions for future work.
2 Problem Statement
This work seeks to compute a trajectory from a start state to a goal region that
is low-cost, dynamically-feasible, collision-free, and well-localized throughout itsexecution. We consider a robot with state space X⊂R
dand dynamics ˙x=f(x,u),
where x∈Xis the state and u∈R/lscriptis the control input. Let Xobsbe the obstacle
space and Xfree:=X\Xobsbe the free space, which the robot must navigate through
to get from an initial state xinit∈Xfreeto a goal region Xgoal⊂Xfree. The robot
tracks a planned trajectory x∗:[0,T]→ Rdusing a perception-based state estimate
ˆx∈Rdand closed-loop controller. In the process of tracking this trajectory, the
robot is subject to state estimate noise due to imperfect measurements, resulting in
a localization error ||ˆx(t)−x(t)||at time t. Given this, we approach the perception-
aware motion planning problem by seeking a minimum cost trajectory (with cost
measure c) to be localized within a bound δwith high probability.
Perception-Aware Motion Planning via Multiobjective Search … 899
Perception-Aware Motion Planning (PMP) Problem:
min
u∗(·)c(x∗(·))
s.t.˙x∗=f(x∗,u∗)
x∗∩Xobs=∅
x∗(0)=xinit,x∗(T)∈Xgoal
P({||ˆx(t)−x(t)|| | t∈[0,T]} ≥δ)≤α.(1)
Unfortunately, perceptual localization error can be difﬁcult to quantify, partic-
ularly for entire trajectories. Instead, we propose the use of a perception heuristic
conditioned on the nominal trajectory and the environment as a proxy to the true local-ization error. We deﬁne the heuristic as h(x
∗([0,t]),E), where x∗([0,t])denotes the
trajectory over the time range [0,t]and Erepresents environmental information used
for perception (e.g., visual features). We thus wish to solve the
Perception-Heuristic-Aware Motion Planning (PHMP) Problem:
min
u∗(·)c(x∗(·))
s.t.˙x∗=f(x∗,u∗)
x∗∩Xobs=∅
x∗(0)=xinit,x∗(T)∈Xgoal
h(x∗([0,t]),E)≤β∀t∈[0,T].(2)
By considering PHMP as a cost optimization with a separate perception con-
straint, this formulation allows the heuristic to capture the history dependent natureof trajectory and localization drift and can be tailored to the available sensors and
algorithmic approaches. This heuristic need not be linear or monotonic, and in gen-
eral should be tuned to the system’s perception method and empirical results. InSect. 5.2we provide an additive, learned heuristic based on state and visual features
to approximate the localization error rate, but many other possible heuristics exist.
Additional examples include estimated uncertainty covariance, observability, mapentropy, or likelihood of loop closure in an area, and in general may incorporate
complex evolution over state history as needed.
Ultimately, the goal of the perception-aware motion planning problem is to ﬁnd
a robust trajectory—a trajectory that can be tracked closely with high probability.
Formally, we state the robust motion planning problem asP({||x
∗(t)−x(t)|| | t∈
[0,T]} ≥δ)≤α, where xis the executed trajectory, δxis the deviation bound,
andαis the probability bound. While this deviation may result from many sources,
including dynamics modeling errors, environmental disturbances, and process noise,we focus on localization error due to perception as one major source of failure in
modern robotic systems. In scenarios where localization error is the primary source
of deviation, we may indirectly address the robust motion planning problem.
900 B. Ichter et al.
3 The Multiobjective Perception-Aware Planning
Algorithm
In this section we introduce the Multiobjective Perception-Aware Planning (MPAP)
algorithm. The algorithm assumes an a priori mapped environment, including both
obstacles and some sense of perceptual capabilities (e.g., locations of features),though we do present a methodology for use within a SLAM framework at the end of
this section. MPAP computes well-localized, robust motion plans initially through
the perception-heuristic-aware motion planning problem via a multiobjective search
of the state space to compute the Pareto optimal front of cost and a perception heuris-
tic. This search removes any Pareto dominated plans, as well as any plans in violationof the perception-heuristic constraint. The lowest-cost feasible plan resulting from
this search is then used to inform robustness through a Monte Carlo (MC) veriﬁca-
tion of the full system. The resulting simulations can be used to iteratively updatethe perception-heuristic bound (either loosening or tightening) until a robust solu-
tion is found. The additional computational load of a multiobjective search and a
MC veriﬁcation are offset through algorithmic design for massive parallelization toleverage the computational power of GPUs. This approach is outlined in Algorithm 1
and thoroughly detailed below. We note this algorithm uses the same multiobjective
search approach as well as massive parallelization strategy introduced in [ 17]. In
the interest of completeness, we detail the algorithm in its entirety here, while also
providing new discussion relevant to perception-aware motion planning.
Algorithm 1 MPAP: Outline
1 Massively parallel sampling-based graph building (Algorithm 2)
2 Massively parallel perception heuristic computation3 Multiobjective search to ﬁnd optimal plan subject to a perception-heuristic constraint (Algo-
rithm 3)
4 Monte Carlo veriﬁcation of localization and robustness, and reﬁnement of bound for Algorithm 3
We now present formal notation and algorithmic primitives to aid in the elucidation
of MPAP. We deﬁne sampled nodes (samples) in Rdwhere dis the dimension of the
state space. We represent plans as structs (head,path,cost,h), where each respective
ﬁeld represents the terminal trajectory node from which we may extend other plans,
the list of previous nodes, the cost, and the perception heuristic. In the following
deﬁnitions let u,v∈Xfreebe samples and let V⊂Xfreebe a set of samples. Deﬁne
the following standard algorithmic primitives: SampleFree (n)returns a set of
n∈Npoints sampled from Xfree,Cost(u,v)returns the cost of the optimal trajectory
from utov,Near(V,u,r)returns the set of samples {v∈V:Cost(u,v)< r}, and
Collision (u,v)returns a boolean value indicating whether the optimal trajectory
from utovintersects Xobs.
Further deﬁne the following functions speciﬁc to the MPAP algorithm. Given
a set of motion plans Pand Popen⊂P,RemoveDominated (P,Popen)denotes
Perception-Aware Motion Planning via Multiobjective Search … 901
a function that removes all p∈Popen from Popen and Pthat are dominated by a
motion plan in {pdom∈P:pdom.head=p.head}; we say that pdomdominates pif
(p.cost>pdom.cost)∧(p.h≥pdom.h). Deﬁne PH(v,p)as a function that returns
the value of the perception heuristic for the motion plan pconcatenated with the
optimal connection p.head to v.
With these algorithmic primitives and notation in hand, we now present the full
MPAP algorithm. Note that for brevity, as well as clarity, any necessary tuning
parameters or variables not explicitly passed to functions are assumed to be global.
Given a mapped environment, the algorithm begins with a graph building stage(Algorithm 2) to construct a representation of available motions through the state
space, similar to probabilistic roadmap methods [ 18]. The graph is initialized with
x
initand nsamples from Xfree,v i aSampleFree (n), to form the set V. This set of
samples must include at least one from Xgoal.A n rn-disc graph is then formed by
computing the neighbors of each sample within an rncost radius and checking their
connection is collision free. The value of rncan be considered a tuning parameter in
line with standard practices in sampling-based planning. For kinodynamic systems,
these edges should consider the respective kinematics and dynamics. This ﬁrst stage
represents an embarrassingly parallel process following the discussion in [ 19].
Algorithm 2 BuildGraph
1V←{ xinit}∪SampleFree (n)
2for all v∈Vdo{massively parallel graph building}
3 N(v)←Near(V\{v},v,rn)
4 for all u∈N(v)do
5 ifCollision (v,u)then N(v)← N(v)\{u}end if
6 end for
7end for
8return (V,N)
With the graph complete, the second phase performs any computation of the
perception heuristic that does not require full trajectories. Key to the computa-
tional tractability of this algorithmic approach is the choice of a perception heuristicamenable to signiﬁcant, massively parallel computation before the full trajectory is
determined in the graph search. This may be a heuristic that is derived through the
combination of nodes visited or edges checked, allowing the value of each individualsection to be computed independently on the GPU. We present two such heuristics
in Sects. 4and5, but note that the heuristic can be quite general and should be set to
the given perception method.
Following the graph construction and any massively parallel computation for the
perception heuristic, a massively parallel multiobjective search is performed over thestate space, considering both the trajectory cost and perception heuristic, as detailed in
Algorithm 3. The exploration proceeds by iteratively expanding groups, G, in parallel
to their nearest neighbors. These groups are formed from the set of open motion plans,
P
open, below an increasing cost threshold deﬁned for each iteration iasiλrn, where
902 B. Ichter et al.
λ∈(0,1]denotes the group cost factor. The value of λis a tuning parameter that
represents a tradeoff between parallelism and the potential for discarding promising
plans or early termination. For higher values of λthe number of samples in each
group increases, and thus the computation is made more parallel, but this comes at
the cost of spreading the set Popen(as plans are left behind the cost threshold), which
potentially allows the search to terminate before the optimal motion plan has reachedX
goal. At the end of each iteration any dominated plans are removed, as are any plans
found in violation of the perception-heuristic constraint ( p.h>β). The exploration
terminates when a feasible plan reaches XgoalorPopen=∅. If multiple plans are
found in the Pareto front that satisfy the constraint bound, the minimum cost plan
is selected. We note that in addition to the parallelism introduced over individual
plans, the use of a cost bound, iλrn, allows for plans to be stored in cost thresholded
buckets with O(1)insertion and removal.
Algorithm 3 Explore
1Popen←{(xinit,∅,0,0)} // plans ready to be expanded
2P(xinit)← Popen // plans with head at xinit
3G← Popen // plans considered for expansion
4i=0
5while Popen/negationslash=∅∧{ g∈G:(g.head∈Xgoal)∧(g.h≤β)}=∅ do
6 for all p∈Gdo
7 for all x∈N(p.head)do
8 q←(x,p.path+{ p.head},p.cost+Cost(p.head,x),PH(x,p))
9 ifq.h≤βthen {Perception-heuristic cutoff }
10 P(x)← P(x)∪{q}
11 Popen← Popen∪{q}
12 end if
13 end for
14 end for
15(P,Popen)←RemoveDominated (P,Popen)
16 Popen← Popen\G
17 i← i+1
18 G←{ p∈Popen:p.cost≤iλrn}
19 end while
20 Pcandidates ←{ p∈P(v):v∈Xgoal}
21 return PXgoal←min p∈Pcandidates {p.cost}
As discussed, we employ a multiobjective search to solve the PHMP problem,
but it may be natural to ask why incur the computational burden of a multiobjective
search instead of a combined cost function. Since we are approaching a problem
that considers an objective and constraint, we should consider both independentlyas we do not know how they will progress in the future. For example, consider the
two potential plans expanding through a narrow passageway in Fig. 1.P l a n p
Ahas
a low cost, medium constraint value (but has not yet violated the constraint), andlow combined cost function, while plan p
Bhas a high cost, low constraint value, and
high combined cost function. With our combined cost function, pAclearly dominates
Perception-Aware Motion Planning via Multiobjective Search … 903
Fig. 1 An uncertainty-constrained planning problem in which a multiobjective formulation should
be used over a weighted cost metric. Features to localize against are shown as black diamonds.While the partial plan p
Amay outperform pBin a combined cost metric, it eventually exceeds the
constraint. A multiobjective search will consider continuations of both plans, while a combinedformulation may extend only one
and is expanded through the narrow passageway, spawning several other plans as
concatenations with pA. If all of these new plans incur high additional penalties on the
constraint value, none will be valid. Unfortunately, because the narrow passageway
is already ﬁlled by pA, the potentially feasible plans that begin with pBare never
realized. The multiobjective search has the further beneﬁt of admitting a wide range ofperception heuristic functions. In this work we consider additive heuristics intended
to emulate the variance in a localization estimate (indeed, heuristics of this form
could be addressed by state augmentation in a non-sampling-based context) but,e.g., more complicated particle-based heuristics have also been applied within a
similar multiobjective search framework [ 17].
The ﬁnal phase of MPAP computes an asymptotically exact probability of motion
plan psatisfying a localization error or deviation bound through MC sampling [ 20].
Given the output of this search, the perception-heuristic bound may be updated
(incremented or decremented) and the exploration rerun. Once a ﬁnal feasible planhas been determined, this has the additional beneﬁt of giving a certiﬁcate of constraint
satisfaction. This ﬁnal phase too can be augmented by a smoothing algorithm to
adjust the pose along the trajectory, subject to feasibility, to maximize perception
(one potential approach is outlined in [ 21]).
Brieﬂy, we note that this ﬁnal phase can be considered optional, dependent on
the ability to accurately model the system’s trajectories and perception as well as the
quality of the perception heuristic used. Despite the ﬂexibility of the MC approach,
in some cases due to complex dynamics, modeling the system’s trajectories maybe computationally intractable or the onboard perception may not be well known,
rendering a MC step inaccurate. We demonstrate this case in Sect. 5of this work,
where a Google Tango smartphone is used for localization, which, to the authors,may be considered a black box localization framework. Without the MC veriﬁcation,
signiﬁcant gains are still observed by simply considering the perception heuristic.
Anytime implementation : Finally, we discuss here implementing MPAP for real-
time planning in conjunction with SLAM. The computational load of the MPAP algo-
904 B. Ichter et al.
rithm is primarily in the graph building and computation of the perception heuris-
tic (for a wide range of choices), whereas the multiobjective search is relatively
lightweight. Thankfully, both of these intensive algorithms can easily be iterativelyupdated as new information becomes available. For the graph building, if new obsta-
cles are discovered or regions are updated, the number of new edge collision checks
can be limited. For the perception heuristic, if updates are local (such as a new regionor feature’s discovery), again only small changes need occur. The exploration loop,
being separate algorithmically from the other two, can then be rerun as needed.
4 Numerical Experiments
4.1 Simulation Setup
In this section we demonstrate the performance of the MPAP algorithm through
numerical simulations with a quadrotor-inspired dynamical system operating in aknown 3D workspace with visual-inertial odometry (VIO) localization. We model
our system as a 6D double integrator ( ¨x=u) with an additional single integrator yaw
state. The system’s dynamics are propagated by discretized approximate dynamicsand the nominal trajectory is tracked by an LQR trajectory tracking controller. For
simplicity, the yaw state is considered to be tracked exactly.
The system is localized in MC through a learned-feature-based VIO method. The
inertial estimate is obtained through a simulated accelerator with noise. The visual
localization estimate is based on matching viewed features with known 3D locations
(up to a level of noise), as to emulate a learned environment. At each timestep thesystem identiﬁes the features in the ﬁeld of view (unobstructed by an obstacle and
within a ﬁeld of view angle from the system’s yaw direction) and is given a relative
position with noise, as may be estimated from motion and successive images (for
simplicity is assumed that features are identiﬁed perfectly if in the ﬁeld of view). The
position is then found from these features using a 3D-to-3D correspondence methoddescribed in [ 22]. Finally the estimates are fused via a Kalman ﬁlter.
For the numerical simulations in this section we introduce a simple feature-based
perception heuristic, matching our system’s localization method (in Sect. 5we present
a more generally applicable learned heuristic). At each timestep dtalong a given
trajectory, an additive penalty of dtis incurred to represent increasing drift. This
penalty is offset when features are in view (i.e., in the ﬁeld of view and unobstructed)by decrementing the heuristic by dt/n
f, where nfrepresents the number of features
required to offset any drift. If the number of features in view exceeds nf, the heuristic
will decrease in time, representing possible relocalization (loop closure). We setn
f=12 for the remainder of this work. If the heuristic becomes negative, it is set to
zero to represent the concept that a robot can at most be fully localized. This heuristic
is able to capture both the character of our problem and the history-dependence oflocalization drift, and demonstrates the capability of the MPAP framework.
Perception-Aware Motion Planning via Multiobjective Search … 905
4.2 Simulation Results
The simulations were implemented in CUDA C and run on an NVIDIA GeForce
GTX 1080 Ti GPU on a Unix system with a 3.0 GHz CPU. Our implementationsamples the state space using the deterministic, low-dispersion Halton sequence.
As this set of samples is deterministic, no variance results are presented. Leveraging
this determinism, we perform an ofﬂine precomputation phase to compute both near-est neighbors and edge discretizations. We also note that the obstacles used in our
simulations represent axis-aligned bounding boxes, as often used in a broadphase
collision checking phase [ 23]. This methodology can provide increasingly accurate
representations of obstacles as needed. Finally, we set λ=0.5, which we have found
represents a good tradeoff between parallelism, ease of implementation, and potential
early termination.
The ﬁrst simulation environment, shown in Fig. 2, is rather simple, but demon-
strates the beneﬁts of perception-aware planning. The obstacle set was generated toallow for low-cost trajectories around the outside of the workspace, but with a fea-
ture distribution favoring the inside of the workspace. Three plans were generated for
varying levels of perception heuristic (from highly perception-aware to perception-agnostic). As can be seen, the perception-agnostic and low perception-aware plans
utilize the same solution homotopy, varying primarily in yaw direction. The highly
perception-aware plan however changes homotopy class. Figure 2b, c and Table 1
show the MC simulation results, which demonstrate the robustness beneﬁts both in
terms of mean, but also variance. The distributions vary most signiﬁcantly in the
length of the right tails, which is the regime in which robustness is most impacted.The errors for the top 1% of plans are shown in Table 1.
The second simulation environment, shown in Fig. 3, represents a more complex,
realistic environment generated from [ 24], where individual objects are bounded by
axis-aligned boxes. The problem’s initial state and goal region require the planner
Fig. 2 a MPAP motion plans with varying levels of perception heuristic bounds. Mapped features
are shown in black, but only visible for localization if within the ﬁeld of view and unobstructed. Thelow aware trajectory improves over the agnostic trajectory primarily with changes in yaw, whereasfor high awareness the solution homotopy class must change. b,cThe histograms of maximum
localization error and trajectory deviation from MC simulations demonstrates perception-awarenessresults in both lower mean deviations and less variance
906 B. Ichter et al.
Ta b l e 1 Simulation results over varying levels of perception-awareness. Note the max error and
deviation are normalized by the max, max error and deviation observed over all 1000 trials
Figure 2 Figure 3
Perception-awareness High Low Agnostic High Agnostic
Max localization error in Top 1% of Plans 0.30 0.37 0.83 0.21 0.79
Max deviation in top 1% of Plans 0.21 0.59 0.84 0.21 0.79
Cost 288 211 201 263 193
Fig. 3 a The point cloud representing the environment [ 24].bThe obstacle representation of the
environment as well as perception-aware and perception-agnostic trajectories. Mapped featuresare shown in black, but only visible to a plan if within the ﬁeld of view and unobstructed. cThe
histograms of the distribution of localization error and deviation for each trajectory, generated viaMC simulations. The results show both lower mean deviations and signiﬁcantly lower variances forthe perception-aware algorithm. The distributions vary especially in the right-tails
to compute trajectories for the full size of the building. The features were generated
in this environment to favor the high-cost hallway, representing an incomplete map-ping in the low-cost hallway. The resulting trajectories show the perception-agnostic
formulation plans a more direct trajectory, while the perception-aware formulation
plans a more robust trajectory at a higher cost. Corroborating the results in the otherobstacle set, the mean and variance of the perception-aware trajectory deviations are
lower, but the largest difference occurs in the right tails of the distributions (Table 1).
Lastly, Table 2details the computation time for each phase and the cost over a
range of selected sample counts. On average over all runs, the computation time is
allocated as 1% graph building, 65% perception heuristic computation, 7% explo-
ration, and 27% MC. This further motivates the use of an anytime implementationdiscussed at the end of Sect. 3to offset the computation cost of the perception heuristic
computation. For other cases when the environment is known a priori (as is assumed
in this work), the computation time reduces to the multiobjective search and poten-
tial MC veriﬁcation. The MC trials too can be computationally intensive due to the
Perception-Aware Motion Planning via Multiobjective Search … 907
Ta b l e 2 Perception-aware plan costs and computation times over varying samples. In practice, since
the environment can be mapped a priori, the full run time can be as little as just the explorationstep. Note the trajectory in Fig. 3must traverse the entire building and represents an upper bound
on what we expect the planner to compute
Figure 2 Figure 3
Sample count 2k 4k 2k 4k
Cost 296 288 302 263
Total time (ms) 49 147 812 1200
Graph building (Algorithm 2) 1 2 6 20
Perception heuristic 34 116 276 919
Exploration (Algorithm 3) 6 19 2 14
Monte Carlo 8 10 528 247
Fig. 4 a The framework of our learned heuristic, the localization error rate distribution is predicted
from the system’s sensor output Sand estimated rates ˆ˙x.bRate of the position error magnitude
predicted by the learned heuristic over test data. The distribution at each timestep is predictedindependently. The data was generated by ﬂying Google Tango inside of our motion capture systemand evaluating the deviation between the two
repeated computations of ﬁeld of view, though we note our current implementation
is completely unoptimized and considering a trajectory assumed fully recomputingfeatures at 50 Hz (Fig. 4).
5 Physical Experiments
5.1 Experimental T estbed
The quadrotor used for the experiment was based on the DJI F330 frame (Fig. 5a). A
Pixhawk autopilot running the PX4 software stack [ 25] was used for ﬂight control.
Additionally, the platform was augmented with a Linux computer, the Odroid XU4,responsible for communicating with the Pixhawk and bridging the autopilot with
the rest of the network through the Robot Operating System (ROS). Using ROS,
908 B. Ichter et al.
Fig. 5 a Quadrotor with mounted Google Tango used in experiments. bExperimental environment
setup, with white sheets on the left half of the room to reduce features. cNearest neighbor contour
plot from the learned heuristic (each position is averaged over a suite of yaw directions and zerovelocity), where yellow represents poor localization and blue represents good. Note the regionsurrounded by white sheets is predicted to perform poorly. dPlanned trajectories, where known
feature rich regions are represented with black diamonds. eDesired, ground truth (Vicon), and
onboard (Tango) estimate trajectories for the quadrotor over 26 ﬂights, including 3 crashes forthe perception-agnostic trajectory. fHistograms of maximum localization error and deviation in
experiments
Perception-Aware Motion Planning via Multiobjective Search … 909
the platform exposes an interface for a ground-station to send position waypoints to
the quadrotor. Trajectories are ﬂown using this waypoint interface. The quadrotor
was also equipped with a Google Tango smartphone. Tango performs a feature-based visual-inertial odometry along with a feature-based loop closure (termed “area
learning”); note the exact features and algorithmic methods are not known to the
authors and thus the MC step was not performed. The localization computed byTango was broadcasted over the network using ROS, transformed appropriately, and
then used as a sensor measurement in the autopilot’s Extended Kalman Filter.
The quadrotor was ﬂown in a room measuring approximately 3 m ×3m×4m
equipped with motion tracking cameras to provide ground-truth measurements, i.e.,
not used by the quadrotor for navigation. Instead, they are only used to evaluate and
learn the performance of the Tango position estimate and train our heuristic. Theroom was learned with Tango’s area learning feature with a large obstacle made up
of a white table cloth in the middle of the room along with two additional sheets on
the walls on one side of the room (Fig. 5b). The presence of white uniform sheets
reduces the number of visual features available to Tango in those regions. Moreover,
the wind disturbances generated by the propellers tend to make the sheets move,
resulting in non-stationary features. This makes it even more difﬁcult for Tango to
accurately track features that it can reliably use for pose estimation.
5.2 Learned Heuristic
We consider localization through Google Tango to be a black box, due to its soft-
ware complexity and the uncertain nature of its loop-closure update, and proposea learning-based approach in order to derive the perception heuristic used in our
experiments. This framework is outlined in Fig. 4a. We acquired data on the percep-
tion quality of Google Tango by ﬂying the quadrotor inside a motion capture systemand computing the error between the two state estimates. We make the modeling
assumption that the position error rate produced by Google Tango (i.e., the rate of
error magnitude between the onboard estimate and ground truth) is drawn from atime-varying Gaussian distribution, and ﬁt the parameters of this distribution at each
timestep/image frame using a maximum likelihood objective.
We trained a deep neural network for this distributional prediction, with learning
inputs the linear and angular velocities of the quadrotor as well as the number of
SURF features visible by the camera. We did not include position as a training feature,
because it would prevent the heuristic from generalizing outside of the motion capture
room. The neural network is a feedforward network with two hidden layers. The
hidden layers contain eight nodes each and rectiﬁed linear unit activation functions. Aﬁnal projection layer produces the mean and standard deviation of the instantaneous
error rate distribution. Example test output of the prediction network is shown in
Fig. 4b; we note that the error rate model performs quite well within the conﬁnes
of our motion capture arena. We leave to future work the validation of this learned
heuristic in other environments with ground truth state measurement.
910 B. Ichter et al.
When mapping a new environment, the neural network is evaluated along the way
using the number of SURF features visible as well as the current linear and angular
velocities of the quadrotor, resulting in a mapping of the heuristic throughout thestate space. This map is then interpolated using weighted nearest-neighbors (over
the position, velocity, and yaw of the quadrotor) in order to produce the value of
the heuristic for unvisited states used during the graph building phase of MPAP. Acontour plot of this procedure applied to the experimental environment is shown
in Fig. 5c, where the region surrounded by white sheets predicts poor localization
performance, as expected.
5.3 Experimental Results
Two trajectories with the same initial and goal states (across the room from each other)were generated with the mapped environment (Fig. 5d). One trajectory was computed
using the full MPAP algorithm, with λ=0.5, while the other was computed as a
baseline perception-agnostic trajectory. Each trajectory was ﬂown to the goal region
and then back to the initial position.
The motion plans were ﬂown 13 times each with the resulting trajectories shown
in Fig. 5e. The perception-aware trajectory was able to ﬂy all successfully, while
the perception-agnostic trajectory crashed three times, all due to loss of localization
events. These events are comparable to the long right tails observed in the numericalsimulations in Sect. 4,F i g s . 2b and 3c. As seen in Fig. 5f, outside of these localization
losses, the two trajectories track the nominal approximately equally, despite the
perception-aware being better localized. This is a result of the perception-awareplan incorporating more difﬁcult turns for the controller to track. Videos of the
two different plans being ﬂown are available on the Stanford Autonomous Systems
Laboratory Youtube channel.
1
6 Conclusions
In this work we presented the Multiobjective Perception-Aware Planning algorithm tocompute well-localized, robust motion plans. The algorithm leverages GPU massive
parallelization to perform a multiobjective search (considering cost and a perception
heuristic) over a graph representation of the state space, ultimately identifying a
motion plan to be certiﬁed robust via Monte Carlo methods. We demonstrated through
numerical experiments that MPAP identiﬁes motion plans in under a few secondsand that the objectives of perception and cost must be considered simultaneously to
obtain truly robust plans. Finally, we have shown through physical experiments that
1https://www.youtube.com/watch?v=YKXf4iuj8-k .
Perception-Aware Motion Planning via Multiobjective Search … 911
these perception-aware plans are indeed more robust and that the use of a learned
perception heuristic, leveraging neural networks, can be effective in generating them.
This work leaves many avenues for further investigation. First, we plan to imple-
ment the algorithm in a real-time onboard framework with simultaneous localization
and mapping. Second, we plan to extend our work on learned heuristics for planning,
particularly towards heuristics that take the full sensor input directly. Third, we plan todemonstrate this algorithm in more heterogeneous environments, e.g., outdoors, with
varied lighting conditions. Finally, while this work considered robustness through
perception uncertainty, we plan to additionally consider environmental uncertaintiesand the ability of the controller to track trajectories while planning.
References
1. Kaelbling, L.P., Littman, M.L., Cassandra, A.R.: Planning and acting in partially observable
stochastic domains. Artif. Intell. (1998)
2. Kurniawati, H., Hsu, D., Lee, W.S.: SARSOP: efﬁcient point-based POMDP planning by
approximating optimally reachable belief spaces. In: Robotics: Science and Systems (2008)
3. Prentice, S., Roy, N.: The belief roadmap: efﬁcient planning in linear POMDPs by factoring
the covariance. Int. J. Robot. Res. (2009)
4. Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty.
In: Proceedings of the IEEE Conference on Robotics and Automation (2011)
5. van den Berg, J., Patil, S., Alterovitz, R.: Motion planning under uncertainty using iterative
local optimization in belief space. Int. J. Robot. Res. (2012)
6. Patil, S., Kahn, G., Laskeym, M., Schulman, J., Goldberg, K., Abbeel, P.: Scaling up gaussian
belief space planning through covariance-free trajectory optimization and automatic differen-tiation. In: Workshop on Algorithmic Foundations of Robotics (2014)
7. Indelman, V ., Carlone, L., Dellaert, F.: Planning in the continuous domain: a generalized belief
space approach for autonomous navigation in unknown environments. Int. J. Robot. Res. (2015)
8. van den Berg, J., Abbeel, P., Goldberg, K.: LQG-MP: optimized path planning for robots with
motion uncertainty and imperfect state information. Int. J. Robot. Res. (2011)
9. Agha-mohammadi, A., Agarwal, S., Chakravorty, S., Amato, N.M.: Simultaneous localization
and planning for physical mobile robots via enabling dynamic replanning in belief space. IEEETrans. Robot. (2016). arXiv:1510.07380 (submitted)
10. Platt, R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maxi-
mum likelihood observations. In: Robotics: Science and Systems (2010)
11. Aloimonos, J., Weiss, I., Brandyopadhyay, A.: Active vision. Int. J. Comput. Vis. (1988)12. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press, Cambridge (2005)13. Cadena, C., Carlone, L., Carrillo, H., Latif, Y ., Scaramuzza, D., Neira, J., Reid, I., Leonard,
J.: Past, present, and future of simultaneous localization and mapping: towards the robust-perception age. IEEE Trans. Robot. (2016)
14. Sadat, S.A., Chutskoff, K., Jungic, D., Wawerla, J., Vaughan, R.: Feature-rich path planning
for robust navigation of MA Vs with Mono-SLAM. In: Proceedings of the IEEE Conference onRobotics and Automation (2014)
15. Costante, G., Forster, C., Delmerico, J., Valigi, P., Scaramuzza, D.: Perception-aware path
planning. IEEE Trans. Robot. (2017). arXiv:1605.04151 (submitted)
16. Carlone, L., Lyons, D.: Uncertainty-constrained robot exploration: a mixed-integer linear pro-
gramming approach. In: Proceedings of the IEEE Conference on Robotics and Automation(2014)
912 B. Ichter et al.
17. Ichter, B., Schmerling, E., Agha-mohammadi, A., Pavone, M.: Real-time stochastic kinody-
namic motion planning via multiobjective search on GPUs. In: Proceedings of the IEEE Con-ference on Robotics and Automation (2017)
18. Kavraki, L.E., Švestka, P., Latombe, J.-C., Overmars, M.H.: Probabilistic roadmaps for path
planning in high-dimensional spaces. IEEE Trans. Robot. Autom. (1996)
19. Amato, N.M., Dale, L.K.: Probabilistic roadmap methods are embarrassingly parallel. In: Pro-
ceedings of the IEEE Conference on Robotics and Automation (1999)
20. Janson, L., Schmerling, E., Pavone, M.: Monte Carlo motion planning for robot trajectory
optimization under uncertainty. In: International Symposium on Robotics Research (2015)
21. Davison, A.J., Murray, D.W.: Simultaneous localization and map-building using active vision.
IEEE Trans. Pattern Anal. Mach. Intell. (2002)
22. Scaramuzza, D., Fraundorfer, F.: Visual odometry part I: the ﬁrst 30 years and fundamentals.
IEEE Robot. Autom. Mag. (2011)
23. LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)24. Armeni, I., Sener, O., Zamir, A.R., Jiang, H., Brilakis, I., Fischer, M., Savarese, S.: 3D semantic
parsing of large-scale indoor spaces. In: IEEE Conference on Computer Vision and PatternRecognition (2016)
25. PX4 Development Team: PX4 autopilot. http://px4.io/
Decoupling Constraints
from Sampling-Based Planners
Zachary Kingston, Mark Moll and Lydia E. Kavraki
1 Introduction
Motion planning is an essential tool for a robotic system with any level of autonomy.
With planning, a robot’s movements can be speciﬁed with start and goal conﬁgura-tions, rather than a full prescription of intermediate states [ 7].Task constraints are an
important mechanism to concisely specify complex motions for a robot. For example,
a robot tasked with transferring a glass of water may have to be constrained to keep
the glass level. Another example of common task constraints are loop-closure con-
straints, such as in parallel manipulators or a bi-manual system carrying a tray [ 25]
(shown in Fig. 1). Recently, there has been rapid development in creating robotic
systems that are high-dimensional such as humanoid robots, mobile manipulators,
and redundant arms, but planning for these high-dimensional systems is hard dueto the inherent difﬁculty of the motion planning problem [ 25]. Planning for these
systems with constraints is even more important and relevant as complex systems
are tasked with more complex objectives.
In general, sampling-based planners have been effective at planning motions for
high-dimensional systems [ 7]. These planners randomly explore the robot’s conﬁg-
uration space and build a discrete representation of valid motions. Many sampling-based planners have been developed with different methods to explore and exploit the
valid motions of a robot. However, incorporating constraints in planning is still difﬁ-
cult, as ﬁnding conﬁgurations that satisfy constraints is a challenging task. Recently,several algorithms have been developed for planning with constraints that are
Z. Kingston ( B)·M. Moll ·L. E. Kavraki
Rice University, 6100 Main Street, Houston, TX 77005, USAe-mail: zak@rice.edu
M. Moll
e-mail: mmoll@rice.edu
L. E. Kavraki
e-mail: kavraki@rice.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_62913
914 Z. Kingston et al.
Fig. 1 An plan in the “implicit parallel manipulator” environment. The goal is to move from a
ﬂat, rotated conﬁguration to upright. This path was computed using kpiece in a projection-based
constrained space in a median time of 14 .5s
effective for realistic problems [ 2,18,23]. These algorithms are somewhat
limited in the sense that they adapt a speciﬁc sampling-based algorithm to also satisfy
task constraints, convolving constraint satisfaction with planning methodology.
This paper presents a solution to the design of constrained sampling-based algo-
rithms for more complex systems by means of a framework that decouples constraint
satisfaction from space exploration in the planner. With this framework, a broad class
of sampling-based planners can utilize many previously proposed constraint satis-
faction methods and leverage the tools developed by the community, such as asymp-totically optimal planners [ 20], path optimization [ 12], or domain speciﬁc planners
for high-dimensional problems [ 37]. The conceptual framework encapsulates and
extends previous approaches in the literature. We show that within our frameworkdifferent constraint satisfaction methodologies can all use the same underlying con-
straint representation. Furthermore, we show that different problems can be solved
more successfully using novel combinations of planning algorithms and implicitlydeﬁned constrained spaces.
This paper is organized as follows. Section 2contains a survey of related work
for constrained motion planning. Section 3deﬁnes constraints and the constrained
planning problem. Our framework which decouples constraints from sampling-based
algorithms is presented in Sect. 4. Empirical results are shown and discussed in
Sect. 5. Section 6contains concluding remarks and directions for future work.
2 Related Work
In this work we are studying sampling-based planning with geometric task con-
straints [ 25], which has a wide breadth of literature concerning both techniques to
plan motions and represent constraints. Using task constraints to specify the motionof a robotic system is not a new problem, and has its roots within industrial con-
trol [ 22,27]. Task constraints on robot motion can be used to specify many useful
Decoupling Constraints from Sampling-Based Planners 915
manipulation tasks [ 34], parallel manipulators and closed chains [ 25,41] and even
structural biology [ 46]. Most early work with task constraints did not focus on geo-
metric constraints, and was directed at non-holonomic constraints, such as differentialdrive cars [ 1]. However, as planners were applied to more complex, high-dimensional
systems, geometric constraints were revisited as a difﬁcult addition to the motion
planning problem.
Non-Sampling Methods While not the focus of this paper, a short survey of non-
sampling-based methods for planning with constraints is given for completeness.
One approach is to plan in the robot’s workspace, so geometric constraints can
be directly evaluated and satisfying poses can be sampled. Post-planning a path in
the robot’s conﬁguration space is generated using inverse kinematics ( ik)[19,33].
However, these methods may not be efﬁcient as re-planning is required if a found
path cannot be mapped into the conﬁguration space of the robot. Completeness is also
not guaranteed unless all feasible iksolutions can be generated given the constraints.
Another approach that operates within the robot’s workspace is reactive control,
which uses convex optimization to ﬁnd local satisfying motions, such as those used at
the darpa Robotics Challenge (e.g., [ 10]). While effective with operator supervision,
these controllers are usually incomplete and risk local minima. As local controllers
are optimization-based methods, hard constraints are relaxed into soft constraints,
and invalid motions can be generated. Trajectory optimization approaches (e.g., [ 32,
47]) optimize within trajectory space and are effective for everyday manipulation
tasks, but suffer from many of the same shortfalls as reactive control. Comprehensive
comparison of constrained non-sampling-based methods to sampling-based plannershas not been done, and a thorough analysis is left as future work.
Sampling-Based Planning Sampling-based planners fall broadly into two cate-
gories: graph-based methods such as prm [21] and tree-based methods such as
rrt [26]. Graph-based methods construct a “roadmap” within the conﬁguration
space that can be queried multiple times. Tree-based methods build a tree of motionsrooted from the start or goal. Many techniques perform a bidirectional search for
efﬁciency (e.g., [ 24]) or use coverage estimates to bias search towards unexplored
space (e.g., [ 37]). While sampling-based planners have been shown to be very efﬁ-
cient in ﬁnding feasible paths, paths are often far from optimal. One approach to
improve path quality is to post-process and locally optimize paths [ 12]. Sampling-
based algorithms can also provide asymptotic optimality guarantees [ 20] such that
the solution path converges to a globally optimal path for a cost function, or “soft”
constraint.
The ﬁrst methods capable of solving constrained problems dealt with specialized
cases of constraints, speciﬁcally loop closures in parallel manipulators. Planning with
loop closures is very relevant in structural biology [ 43], and complex loop closure
problems in robotics were solved with prm variants using active/passive chain meth-
ods [ 8,13,44]. Key to loop closure methods is a projection operator , which maps an
unsatisfying conﬁguration into a satisfying one. Active/passive chain methods use ik
as a projection operator to join the passive chain to the active chain, closing the loop
and creating a satisfying conﬁguration. Cyclic-Coordinate Descent [ 5] is another
916 Z. Kingston et al.
loop closure method that, unlike numerical ik, does not require the computation of
Jacobian (pseudo-)inverses.
The idea of projection to satisfy constraints was applied to general end-effector
constraints in [ 45]. Task Constrained rrt [36] further generalized the idea of con-
straints and utilized Jacobian gradient descent [ 4] for projection. More recently,
cbirrt2 [2] and the motion planner implemented for the Humanoid Path Planner
System [ 29] utilize projection with general constraints and can solve complex com-
binations of constraints. Additionally, the projection methodology has been extended
to handle “soft” constraints with Gradien t- rrt [2]. We show our framework can
emulate cbirrt2 and other previous approaches in Sect. 4.2.
Projection, while effective at satisfying constraints, utilizes very little information
from the constraint. A constraint function deﬁnes an implicit manifold within the
robot’s conﬁguration space composed of all constraint satisfying conﬁgurations. This
manifold is typically of lower dimension than the conﬁguration space. The projection
operator described above projects a point from the ambient space to this manifold. Asthe constraint deﬁnes a manifold, it is possible to locally approximate the manifold
using a tangent space of a satisfying conﬁguration. The tangent space can be used to
generate new conﬁgurations that are close to the manifold. As the complexity of the
constraint manifold approximation increases, sampling in the tangent space becomes
more accurate at the price of increased computational cost per sample. References[6,30,36] use local tangent space approximations to generate new samples.
Furthering the idea of local parameterization with tangent spaces is the concept of
building an atlas of the manifold, a concept borrowed from the deﬁnition of differen-
tiable manifolds [ 35]. Here, the atlas is deﬁned as a piece-wise linear approximation
of the manifold using tangent spaces, which fully cover and approximate the man-
ifold [ 15].tb- rrt [23] and Atlas rrt [18] both construct an atlas, incrementally
building a set of tangent spaces that approximate the manifold. tb- rrt evaluates
the manifold lazily and does not separate tangent spaces, leading to overlap and
potential problems with invalid points. Atlas rrt computes halfspaces to separate
tangent spaces into tangent polytopes to guarantee uniform coverage in the limit at
additional computational cost. Atlas rrt has been extended to asymptotically opti-
mal (Atlas rrt
∗[17]) and kinodynamic planning [ 3]. Like cbirrt2 , both tb- rrt
and Atlas rrt are emulated within our framework, as shown in Sect. 4.3.
Carrying the concept of approximation even farther, recent works delve into com-
plete parameterizations of the manifold. The constraint function is used to build arepresentation that only contains conﬁgurations that satisfy or very nearly satisfy the
constraint. “Deformation space” [ 14] and “Reachable volume space” [ 28] are both
reparameterization-based approaches.
The techniques discussed above cover a spectrum of methods to compute satis-
fying conﬁgurations for constrained motion planning. The spectrum describes theamount of effort the constraint methodology is using to more closely plan using the
true implicit manifold. On one end of the spectrum are projection-based methods,
which use little information about the constraint. On the other end lie approachessuch as atlas-based methods, which compute considerable information about the
constraint in order to approximate the implicit manifold.
Decoupling Constraints from Sampling-Based Planners 917
3 Constrained Sampling-Based Planning
Aconﬁguration of the robot is denoted by q∈Q, where Qis the conﬁguration space ,
a metric space. The number of degrees of freedom of a robot, i.e., the dimensionalityof its conﬁguration space, is denoted by n. The motion planning problem is deﬁned
as ﬁnding a continuous, collision-free path from q
start toqgoalin conﬁguration space
τ:[0,1]→ Q,τ(0)=qstart,τ(1)=qgoal. In many cases, avoiding collisions is the
only concern for computing a valid path. For constrained motion planning, we also
want to satisfy a constraint function F (q):Q→ Rkover the conﬁguration space,
which evaluates F(q)=0 when qsatisﬁes the constraint. Here, kis the number of
equality constraints imposed. The constraint function deﬁnes an (n−k)-dimensional
implicit constrained conﬁguration space within the ambient conﬁguration space:
X={q∈Q|F(q)=0}
For this work, we assume Fis continuous and differentiable everywhere, and there-
fore Xis a manifold. This assumption is stronger than strictly necessary for much of
this work, but is imposed for ease of presentation. The constrained motion planningproblem, with a constraint function Fand conﬁguration space Q, is a problem of
ﬁnding τ:[0,1]→ X. For example, consider a point robot with a conﬁguration
space Q⊂R
3.G i v e n F(q)=/bardblq/bardbl−1, the robot is constrained to the surface of a
unit sphere, a two-dimensional manifold in R3. Operations on Xare described in
Sects. 4.2and4.3, which require additional deﬁnitions to be understood.
Aprojection operator is a continuous idempotent mapping P(q):Q→X, where
ifq∈X,P(q)=q. Projection takes a conﬁguration and projects it onto the surface
of the implicit manifold, solving for a root of the constraint function. Typically this
is implemented using Jacobian gradient descent, using the Jacobian of the constraintfunction J(q). The descent stops when F(q)=0. A more comprehensive look at
projection for constrained motion planning is found in [ 2].
Local parameterization of the implicit manifold can be accomplished by a tangent
space (alternatively, a chart from [ 18]). The tangent space is constructed by ﬁnding
the basis for the nullspace of J(q), which can be computed through a matrix decom-
position. The tangent space T
qis a(n−k)-dimensional space with its origin at a
conﬁguration q∈X, with an n×(n−k)orthonormal basis Φq. A point t∈Tqcan
be mapped into qt∈Qbyqt=q+Φqt. To map the conﬁguration qtonto the man-
ifold (an exponential map), an orthonormal projection can be computed by solving
the system of equations:
F(q)=0 and ΦT
q(q−qt)=0
The opposite mapping from the manifold to Tqis much simpler: t=ΦT
q(q−qt).
Tangent spaces are composed into an approximation of the manifold by the Atlas rrt
and tb- rrt constrained spaces within the framework, and are discussed in Sect. 4.3.
918 Z. Kingston et al.
A more comprehensive look at manifold approximation for planning can be found
in [18], with many operations on implicit manifold discussed in [ 31]. The concepts
of differential manifolds outlined here are covered in depth in [ 35].
4 Representing Implicit Spaces
Despite their differences, sampling-based planners have similar requirements from
the robot’s conﬁguration space [ 25]. The primary capabilities we are concerned with
are the following:
•Computation of distance between states, to select nearby states in the motion graph
to either extend from or connect to.
•“Projection” for estimating conﬁguration space coverage in relation to a task,
so that the planner can measure progress and sampling can be directed towards
uncovered regions (Note this is not a projection operator as described before).
•Linear interpolation on a geodesic, or moving between two states, so that new
states can be created or validated through extension or connection.
•Sampling “uniformly” over the space or nearby known states to generate new
conﬁgurations, which can be grown towards or connected to the motion graph.
These can be deﬁned as operations on the space itself and need not be speciﬁc
to any planner. The contribution of this paper is a conceptual framework, outlinedwithin Sect. 4.1, that enables a broad class of motion planners to plan in many con-
strained spaces by exploiting the commonality of the spaces’ primitive operations.
This decouples constraints from a planner by augmenting the space with primitivesthat automatically satisfy imposed constraints.
This section is organized as follows. First we discuss the framework at an abstract
level in Sect. 4.1and describe how each of the space primitives utilized by a sampling-
based planner are conceptualized. Then, we show our emulations of three successful
and widely known methodologies within our framework: cbirrt2 [2] in Sect. 4.2,
and Tangent Bundle rrt [23] and Atlas rrt [18] in Sect. 4.3.
4.1 Conceptual Framework
A sampling-based planning algorithm plans within a conﬁguration space Q, and
generates a collision-free path by using a validity checker along with properties
of the conﬁguration space, shown in Fig. 2a. Prior works augmented the planning
algorithm with a means to ﬁnd constraint satisfying motions, shown in Fig. 2c. In
contrast, our framework is a layer of abstraction that lies between the representation
of the robot’s conﬁguration space and the sampling-based planner used to ﬁnd validmotions, shown in Fig. 2d. The framework can be thought of as a representation of
Decoupling Constraints from Sampling-Based Planners 919
(a) (b)
(c) ( d)
Fig. 2 A depiction of the framework and its relation to sampling-based planners. aA box conﬁg-
uration space Qis shown in black. A sampling-based planner (purple) plans in Qusing primitives
afforded by the space. bA constraint function F(q)=0 deﬁnes a implicit manifold X(green).
cAn augmented constrained sampling-based planner (yellow) (e.g., cbirrt2 ,e t c . )p l a n so n X,
using its constraint methodology. dThe framework enables any sampling-based planner (such as
the unaugmented planner) to plan on Xby incorporating Qand the constraint function F(q)=0
the implicit manifold Xdeﬁned by the constraint function F, and a means for a
sampling-based planner to plan within this space.
Normally, the distance metric utilized by a sampling-based planner is deﬁned by
the conﬁguration space. This metric is primarily for nearest-neighbor computation, by
which states nearby novel states can be found (e.g., Select andSelectNghbrs
in Fig. 3). For example, a point robot in R3and a manipulator arm with Q⊆Rn
commonly use the Euclidean norm. However, the notion of distance in the ambient
space has little meaning on the implicit manifold, as the manifold can twist and
curve relative to the ambient space [ 40]. As the framework represents the implicit
manifold of a constraint, a natural metric to use would be the Riemannian metric, or
the length of the geodesic between points. However, this is computationally infeasi-
Fig. 3 Prototypical examples of tree- and graph-based sampling-based planners. Many sampling-
based planners can be cast into this mold, such as rrt for the tree-based planner, and prm for the
graph-based planner. This illustrates the underlying similarities in sampling-based planners, as they
use the same primitive operations. Note that both Connect andExtend interpolate a geodesic
920 Z. Kingston et al.
ble, as nearest-neighbor computations would require many geodesic computations.
As such, the metric from the conﬁguration space is used unless otherwise speci-
ﬁed, deﬁning a semi-metric on the manifold, as the triangle inequality may not hold
given sufﬁcient curvature. This is still “good enough” for most motion planning algo-
rithms in practice, but some theoretical guarantees may not hold, such as asymptotic
optimality.
Similarly, “projection” for coverage estimates is left unaffected by the framework.
As they are heuristics to bias sampling, projections are problem speciﬁc and are
typically hard to devise. Random linear projections perform well in many cases, but
do not incorporate constraint information [ 39]. Interesting future work would be to
utilize information about the implicit manifold as a projection for coverage estimates.
Computing geodesics from conﬁguration qatoqbnormally has an analytic form,
such as linear interpolation in Rn. In sampling-based planners, geodesic movement
underlies Extend andConnect , as shown in Fig. 3. For implicit manifolds, travers-
ing geodesics is one of the biggest hurdles to cross. Traversing a geodesic in conﬁgu-
ration space and attempting to “ﬁx-up” the new conﬁguration ignores the manifold’s
curvature and can generate invalid motions. Thus, geodesic interpolation within the
framework is akin to a local motion planner, computing a discretized geodesic by
growing from one state to another, taking small enough steps to accurately traverse
the manifold’s curvature. The way this traversal is accomplished is up to the instan-
tiation of the methodology behind the framework, and is one of the deﬁning traits
of a constrained space. Figure 4shows three local planning methodologies to com-
pute discretized geodesics used by the three spaces in the framework. Once the
discretized geodesic is computed, an interpolated state can be computed along the
found geodesic, by doing piece-wise interpolation.
Critical to sampling-based planners is the ability to sample new conﬁgurations in
the conﬁguration space ( Sample in Fig. 3). This is normally as simple as drawing
uniformly random values from Q. However, with an implicit manifold, the structure
of the manifold is not known a priori, and is thus hard to sample uniformly without
careful consideration or pre-processing. How this sampling is done is contingent on
the speciﬁc constrained space, but we do not guarantee that it will produce uniform
(a)( c ) (b)
Fig. 4 Projection-, tangent bundle-, and atlas-based geodesic interpolation. Between points (large
black) on the implicit manifold (green), the discretized geodesic is computed (black). aProjection-
based ( cbirrt2 ). Small extensions are taken (grey) and projected using a projection operator (arrow).
bTangent bundle-based ( tb- rrt ). The manifold is lazily evaluated with tangent spaces (grey),
projecting when necessary. cAtlas-based (Atlas rrt). Tangent spaces are traversed, projecting at
every step
Decoupling Constraints from Sampling-Based Planners 921
samples. Instead, we simply guarantee that any instantiation of the framework will
almost-surely sample any volume of non-zero measure within the manifold.
In summary, the key idea of our framework is to imbue the implicit constraint
manifold with primitives that closely approximate those that exist for regular con-
ﬁguration spaces. This allows any sampling-based planner to plan with constraints
without any special consideration. The next two sections describe two approachesto sampling and interpolation that are on opposite ends of the spectrum in terms of
amount of information they maintain about the constraint manifold.
4.2 Projection-Based Space
One of the simplest methods to sample the constraint manifold is to sample from
the conﬁguration space and use the projection operator to retract samples onto themanifold. It was shown in [ 2] that sampling with projection will eventually cover
the manifold, albeit with no guarantees on uniformity. Interpolation on the manifold
using projection is achieved using a method similar to the extension method of the
cbirrt2 algorithm [ 2]( s h o w ni nF i g . 4a). We emulate the projection-based space
within our framework using the aforementioned methodology. We conjecture theframework retains the probabilistic completeness of the overlying planners, following
the proof of probabilistic completeness of projection-based rrt-like planners in [ 2].
4.3 T angent Space-Based Spaces
As discussed in Sect. 3, an implicit manifold Xcan be approximated by a set of
tangent spaces. A few recent planners use tangent space approximations for efﬁcientsampling nearby the manifold, such as tb- rrt [23] and Atlas rrt[18]. The planners
both sample new points by sampling within tangent spaces and projecting these
points onto the manifold. Although at ﬁrst biased towards explored areas, in thelimit once the manifold has been fully explored sampling can approach uniform
sampling [ 18]. These methods can sample within hard to project areas, such as the
interior surface of a highly curved manifold. Geodesic interpolation is accomplishedby walking along the tangent spaces of the approximation, switching tangent spaces
once certain criteria are met. tb- rrt takes a lazy approach to interpolation, projecting
to the manifold only when necessary to switch tangent spaces (shown in Fig. 4b).
This has the beneﬁt of performing less work computing projections, but it is harder
to do correctly. Extra consideration is needed when performing collision checking aslazy evaluation generates a relaxed geodesic, which might miss obstacles. Atlas rrt
projects at every step along the approximation, and generates separating half-spaces
to create polytopes of the tangent space for more accurate sampling and interpolation,at the cost of additional computation (shown in Fig. 4c). Both of these methods are
emulated within our framework.
922 Z. Kingston et al.
5 Empirical Results
The framework was implemented within the Open Motion Planning Library [ 38]
(OMPL), which has implementations of many popular sampling-based planningalgorithms. Our framework ﬁts neatly within OMPL’s notion of a state space , and
no modiﬁcation was necessary to the implementation of any of the planning algo-
rithms for them to work with the constrained planning framework. Moreover, allbenchmarks were done with a single set of parameters for each constrained space
and planning algorithm, to preserve fairness across multiple environments. More
performance could have been gained by tuning these for each problem, but a set ofreasonable defaults is desirable especially from a naïve user’s perspective. All bench-
marks were performed on workstations with an Intel
®Core™i7-6700K processor
and 32GB of DDR4 RAM at 2400 MHz. The experiments shown here are meant
to both demonstrate the effectiveness of the planning system as well as illustrate
concepts that help put the work in context.
Within the literature of constrained motion planning, most planners are adap-
tations of sampling-based planners augmented with a constraint methodology.
cbirrt2 [2],tb- rrt [23], and Atlas rrt [18], the planners that we have chosen to
emulate within the framework, all are augmentations of rrt-Connect [ 24]. Figure 5
shows the “sphere” environment, a two-dimensional manifold embedded within R3,
Fig. 5 The “sphere” environment. aThe sphere constraint manifold (grey) with obstacles (black).
The solution path (yellow) runs from the south to north pole. bProjection-based rrt∗[20] motion
graph (green) (Sect. 4.2).cTangent bundle-based bit∗[11] motion graph and tangent spaces (grey)
(Sect. 4.3).dAtlas-based spars [9] motion graph and tangent polytopes (Sect. 4.3)
Decoupling Constraints from Sampling-Based Planners 923
deﬁned by the constraint function F(q)=/bardblq/bardbl−1. The planner must traverse three
longitudinal obstacles each with a narrow passage to move from the south to the
north pole. We show the results of 100 runs of various motion planners within the
framework in Fig. 6. As shown in the ﬁgure, combinations of planners and constrained
spaces within the framework have dramatically different outcomes on planning time.
Previous approaches in the literature are emulated by rrt-Connect within the frame-
work, which is shown to have the poorest performance overall within the “sphere”
environment. For this problem, any of the other tested planners would be a better
selection of planner if speed was the primary concern.
More so, it is not just the planner that matters when approaching a constrained
problem, the ambient conﬁguration space can dramatically effect performance. Con-
sider a “torus” environment (Fig. 7), which is a two-dimensional manifold embedded
within R3, with a constraint function F(q)=(3−/radicalbig
x2+y2)2+z2−2. The plan-
ning problem is to traverse from one end of the torus to the other. There are obstacles
BIEST BKPIECE KPIECE RRT EST PRM RRT-Connect0.00.51.01.5Time (s)Time to Solve within the “Sphere” Environment
Constraint Methodology
Projection
Tangent Bundle
Atlas
Fig. 6 Timing results from 100 runs of each planner in the “sphere” environment (Fig. 5)u s i n g
the three constrained spaces in the framework. Planners tested are est,kpiece , their bidirectional
variants biest and bkpiece [16,37],rrt [26]a n d rrt-Connect [ 24], and prm [21].cbirrt2 ,tb-
rrt,a n dA t l a s rrtare emulated by rrt-Connect in their respective constrained space, and perform
the worst overall
5 1 01 52 02 53 0
Bounds0.00.51.01.52.02.5Time (s)Time to Solve vs. X-,Y-axis Bounds
Projection
Atlas
Fig. 7 The “torus” environment (grey) with obstacles (black) and timing results from 100 runs of
prm using the atlas- and projection-based constrained space versus the size of the x-a n d y-a x e s
of the conﬁguration space. On the left is a prm motion graph (green) using the atlas-based space
(tangent polytopes in grey). Projection-based prm performs orders of magnitude worse than its
atlas-based counterpart
924 Z. Kingston et al.
bound around the outer surface of the torus, allowing passage only through the inner
hole to traverse from one end to the other. Timing results for the prm planner using
the projection- and atlas-based methodologies is also shown in Fig. 7, where the
total volume of the conﬁguration space is varied while the size of the torus remains
constant. As shown by the results, projection-based planning performs orders of mag-
nitude worse than its atlas-based counterpart and worsens as the volume of the spaceexpands, due to the inefﬁciency of sampling conﬁgurations that mostly project to the
outer surface of the torus. The atlas-based methodology, which samples directly off
of an approximation of the manifold, is unaffected by changes in the ambient conﬁg-uration space. Projection to the inner surface of the torus requires sampling inside of
the hole of the torus, which becomes less likely as ambient space expands. The torus
example is illustrative of a problem that might arise on real robotic manipulators,as conﬁguration spaces with revolute joints are toroidal in topology. It is unknown
a priori how obstacles in the environment will interact with constraints, and no one
constraint methodology is equipped to handle every case. Therefore, the ability tocombine and change constraint methodology with a planner is essential to efﬁciently
planning within different environments.
A general trend observed by the authors is that as a planning problem becomes
more constrained and the implicit manifold more curved with respect to the ambient
space, atlas- and tangent-bundle-based methods perform better as the extra compu-tation to maintain the approximation pays off. However, as the dimensionality of the
problem grows, the approximation is less helpful and requires a similar, amortized
amount of work as projection does, and projection-based methods do well. Theseare not rules written in stone, and there are many problems which belie their guid-
ance. Take for example the problem of an “implicit chain”, shown in Fig. 8. Here, the
kinematics are modeled as distance constraints, one for each link, on a chain with 5spherical joints. The conﬁguration space is thus R
3×5. To further increase problem
complexity, we impose the additional constraints: (a) end-effector is constrained to
the surface of a sphere of radius three, (b) joint 1 and 2 must have the same z-value,
(c) joint 2 and 3 must have the same x-value, and (d) joint 3 and 4 must have the
same z-value. This gives an implicit manifold dimension of six. Timing results for
this problem are shown in Fig. 8. When there are no obstacles in this scene, tangent
bundle-based methods perform the best, while projection- and atlas-based methods
perform equally less. Lazy evaluation of states works in favor of this problem, as the
planner can quickly traverse the constraint manifold. However, as obstacles are addedto the surface of the outer sphere, tangent-bundle performs worse, as the projection
and atlas methods improve relative performance drastically.
One motivating factor of this work was extending constrained planning to high-
dimensional spaces, taking advantage of previous approaches in high-dimensional
planning without any additional cost. In Fig. 1, we show the “implicit parallel manip-
ulator” environment, a parallel manipulator deﬁned with a set of the “implicit chains,”
deﬁned analogously to the previous example. The end-effectors of the chains are con-
strained to remain attached to a shared disk, creating dependencies in their motion.The environment shown has eight chains with seven links each, for a total ambient
space dimensionality of 168. The constraint manifold is of dimension 99. Emulated
Decoupling Constraints from Sampling-Based Planners 925
0.000.250.500.751.00Prob. of SuccessZero Obstacles
Time (s)One Obstacle
0.00 .51 .01 .5 0 2 4 6 8 0 1 02 03 0Two Obstacles
Projection
Tangent Bundle
Atlas
Fig. 8 A sample solution path with one obstacle is shown in the top ﬁgure. The bottom graphs show
the cumulative probability of ﬁnding a path versus time for kpiece using each constrained space
with no surface obstacles, one obstacle, and two obstacles with antipodal narrow passages. 100
runs were used for each cumulative probability curve. Qualitatively similar results were obtained
for rrt-Connect and prm. Note that the X-axis on each plot is different
prior works (with rrt-Connect) were unable to successfully solve this system given
10 min of planning time. Using the kpiece planner designed for high-dimensional
spaces, we can quickly solve (median 14.5 s over 100 runs) this problem while
satisfying constraints.
There is little work in the literature on satisfying “soft” constraints in tandem with
kinematic constraints. Atlas rrt∗[17] and Gradien t- rrt [2] both respect “soft” con-
straints, but require specialized implementation and integration with the constraint
methodology to work. Within our framework, no additional overhead is necessary for
asymptotically optimal planning, as shown in Fig. 5, which shows motion graphs for
three asymptotically optimal and near-optimal planners. Additionally, path smooth-
ing, shortening, and interpolation algorithms work with no knowledge of constraints,
as all operations are handled by the framework.
6 Conclusion
We have introduced a novel framework for constrained sampling-based planning
that decouples constraint satisfaction from a motion planner’s exploration of a con-
ﬁguration space. We have demonstrated the framework’s capability by showing our
emulations of the constraint satisfaction methodology employed by three constrained
planners, cbirrt2 ,tb- rrt , and Atlas rrt. Additionally, we have tested a broad
range of sampling-based planners within the framework for a set of constrained
problems and shown that each planner can operate within the framework’s con-
straint spaces. The framework is easily extended to new planners, and new constraint
926 Z. Kingston et al.
spaces can be adapted to the framework as its concepts are general to constrained
planning. Although there are rough guidelines on when different constrained plan-
ning approaches tend to work better than others, for speciﬁc problems it is difﬁcultto predict which combination of constraint space and planner will work the best.
This further highlights the beneﬁt of decoupling constraints from planning. Future
work for the framework is the implementation of other constraint spaces, such as localtangent space sampling, adapting the framework for kinodynamic planning with con-
straints, and addressing proofs of completeness in light of the framework. Finally,
we are in the process of integrating the framework with real robotic platforms.
Acknowledgements We would like to thank Caleb V oss for his preliminary work [ 42]. ZK, MM,
and LEK are supported by NSF IIS 1317849 and Rice University funds. ZK is also supported by aNASA Space Technology Research Fellowship 80NSSC17K0163.
References
1. Barraquand, J., Latombe, J.C.: Nonholonomic multibody mobile robots: controllability and
motion planning in the presence of obstacles. Algorithmica 10(2), 121–155 (1993)
2. Berenson, D.: Constrained manipulation planning. Ph.D. thesis, CMU (2011)3. Bordalba, R., Ros, L., Porta, J.M.: Kinodynamic planning on constraint manifolds. ArXiv
e-prints (2017). http://arxiv.org/abs/1705.07637
4. Buss, S.R., Kim, J.S.: Selectively damped least squares for inverse kinematics. J. Graph. GPU,
Game Tools 10(3), 37–49 (2005)
5. Canutescu, A.A., Dunbrack, R.L.: Cyclic coordinate descent: a robotics algorithm for protein
loop closure. Protein Sci. 12(5), 963–972 (2003)
6. Cefalo, M., Oriolo, G., Vendittelli, M.: Task-constrained motion planning with moving obsta-
cles. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5758–5763
(2013)
7. Choset, H., Lynch, K., Hutchinson, S., et al.: Principles of Robot Motion: Theory, Algorithms,
and Implementation. MIT Press, Cambridge (2005)
8. Cortés, J., Siméon, T., Laumond, J.P.: A random loop generator for planning the motions of
closed kinematic chains using PRM methods. IEEE Int. Conf. Robot. Autom. 2, 2141–2146
(2002)
9. Dobson, A., Krontiris, A., Bekris, K.E.: Sparse roadmap spanners. In: International Workshop
on the Algorithmic Foundations of Robotics (2013)
10. Fallon, M., Kuindersma, S., Karumanchi, S., et al.: An architecture for online affordance-based
perception and whole-body planning. J. Field Robot. 32(2), 229–254 (2015)
11. Gammell, J., Srinivasa, S.S., Barfoot, T.D.: Batch informed trees (BIT\*): Sampling-based
optimal planning via the heuristically guided search of implicit random geometric graphs. In:IEEE International Conference on Robotics and Automation (2015)
12. Geraerts, R.J., Overmars, M.H.: Creating high-quality paths for motion planning. Int. J. Robot.
Res. 26(8), 845–863 (2007)
13. Han, L., Amato, N.M.: A kinematics-based probabilistic roadmap method for closed chain
systems. In: International Workshop on the Algorithmic Foundations of Robotics (2000)
14. Han, L., Rudolph, L., Blumenthal, J., Valodzin, I.: Convexly stratiﬁed deformation spaces
and efﬁcient path planning for planar closed chains with revolute joints. Int. J. Robot. Res.27(11–12), 1189–1212 (2008)
15. Henderson, M.E.: Multiple parameter continuation: computing implicitly deﬁned k-manifolds.
Int. J. Bifurc. Chaos 12(3), 451–476 (2002)
Decoupling Constraints from Sampling-Based Planners 927
16. Hsu, D., Latombe, J.C., Motwani, R.: Path planning in expansive conﬁguration spaces. Int. J.
Comput. Geom. Appl. 9(4n5), 495–512 (1999)
17. Jaillet, L., Porta, J.M.: Efﬁcient asymptotically-optimal path planning on manifolds. Robot.
Auton. Syst. 61(8), 797–807 (2013)
18. Jaillet, L., Porta, J.M.: Path planning under kinematic constraints by rapidly exploring mani-
folds. IEEE Trans. Robot. 29(1), 105–117 (2013)
19. James, J., Weng, Y ., Hart, S., et al.: Prophetic goal-space planning for human-in-the-loop mobile
manipulation. In: IEEE-RAS International Conference on Humanoid Robots, pp. 1185–1192(2015)
20. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J.
Robot. Res. 30(7), 846–894 (2011)
21. Kavraki, L.E., Švestka, P., Latombe, J.C., Overmars, M.: Probabilistic roadmaps for path plan-
ning in high-dimensional conﬁguration spaces. IEEE Trans. Robot. Autom. 12(4), 566–580
(1996)
22. Khatib, O.: A uniﬁed approach for motion and force control of robot manipulators: The oper-
ational space formulation. IEEE J. Robot. Autom. 3(1), 43–53 (1987)
23. Kim, B., Um, T.T., Suh, C., Park, F.C.: Tangent bundle RRT: a randomized algorithm for
constrained motion planning. Robotica 34(1), 202–225 (2016)
24. Kuffner, J.J., LaValle, S.M.: RRT-connect: an efﬁcient approach to single-query path planning.
IEEE Int. Conf. Robot. Autom. 2, 995–1001 (2000)
25. LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)26. LaValle, S.M., Kuffner, J.J.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5),
378–400 (2001)
27. Mason, M.T.: Compliance and force control for computer controlled manipulators. IEEE Trans.
Syst., Man, Cybern. (1981)
28. McMahon, T.: Sampling based motion planning with reachable volumes. Ph.D. thesis, Texas
A&M University (2016)
29. Mirabel, J., Tonneau, S., Fernbach, P., et al.: HPP: A new software for constrained motion
planning. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 383–389 (2016)
30. Pachov, D.V ., van den Bedem, H.: Nullspace sampling with holonomic constraints reveals
molecular mechanisms of protein G αs. PLoS Comput. Biol. 11(7) (2015)
31. Rheinboldt, W.C.: MANPAK: a set of algorithms for computations on implicitly deﬁned man-
ifolds. Comput. Math. Appl. 32(12), 15–28 (1996)
32. Schulman, J., Duan, Y ., Ho, J., et al.: Motion planning with sequential convex optimization
and convex collision checking. Int. J. Robot. Res. 33(9), 1251–1270 (2014)
33. Sentis, L., Khatib, O.: Synthesis of whole-body behaviors through hierarchical control of
behavioral primitives. Int. J. Hum. Robot. 2(4), 505–518 (2005)
34. Siméon, T., Laumond, J.C., Cortés, J., Sahbani, A.: Manipulation planning with probabilistic
roadmaps. Int. J. Robot. Res. 32(7–8), 729–746 (2004)
35. Spivak, M.: A Comprehensive Introduction to Differential Geometry. Publish or Perish (1999)36. Stilman, M.: Global manipulation planning in robot joint space with task constraints. IEEE
Trans. Robot. 26(3), 576–584 (2010)
37. ¸ Sucan, I., Kavraki, L.E.: Kinodynamic motion planning by interior-exterior cell exploration.
In: International Workshop on the Algorithmic Foundations of Robotics (2008)
38. ¸ Sucan, I., Moll, M., Kavraki, L.E.: The open motion planning library. IEEE Robot. Autom.
Mag. 19(4), 72–82 (2012)
39. ¸ Sucan, I.A., Kavraki, L.E.: On the performance of random linear projections for sampling-based
motion planning. In: IEEE/RSJ International Conference on Intelligent Robots and Systems(2009)
40. Tenenbaum, J.B., de Silva, V ., Langford, J.C.: A global geometric framework for nonlinear
dimensionality reduction. Science (2000)
41. Tsai, L.W.: Robot Analysis: The Mechanics of Serial and Parallel Manipulators. Wiley, New
York (1999)
928 Z. Kingston et al.
42. V oss, C., Moll, M., Kavraki, L.E.: Atlas + X: Sampling-based planners on constraint manifolds.
Technical report. 17-02, Department of Computer Science, Rice University, Houston (2017)
43. Wedemeyer, W.J., Scheraga, H.A.: Exact analytical loop closure in proteins using polynomial
equations. J. Comput. Chem. 20(8), 819–844 (1999)
44. Yakey, J.H., LaValle, S.M., Kavraki, L.E.: Randomized path planning for linkages with closed
kinematic chains. IEEE Trans. Robot. Autom. 17(6), 951–958 (2001)
45. Yao, Z., Gupta, K.: Path planning with general end-effector constraints: using task space to
guide conﬁguration space search. In: IEEE/RSJ International Conference on Intelligent Robotsand Systems, pp. 1875–1880 (2005)
46. Zhang, Y ., Hauser, K.: Unbiased, scalable sampling of protein loop conformations from prob-
abilistic priors. BMC Struct. Biol. 13(1), S9 (2013)
47. Zucker, M., Ratliff, N., Dragan, A., et al.: CHOMP: Covariant hamiltonian optimization for
motion planning. Int. J. Robot. Res. (2013)
Fast Anytime Motion Planning in Point
Clouds by Interleaving Samplingand Interior Point Optimization
Alan Kuntz, Chris Bowen and Ron Alterovitz
1 Introduction
Robotic manipulators are entering unstructured environments, such as homes, ofﬁces,
hospitals, and restaurants, where robots need to plan motions quickly while ensuringsafety via obstacle avoidance. Motion planning in such settings is challenging in part
because the robot must rely on real-world sensors such as laser scanners, RGBD
sensors, or stereo reconstruction, which typically produce point clouds. In addition,
enabling intuitive, interactive, and reactive user experiences requires that the robot
generate plans of high quality as quickly as possible, without necessarily knowingin advance the maximum time allocatable to motion planning. Hence, motion plan-
ning in such settings should be implemented as an anytime algorithm, meaning the
algorithm progressively improves its solution and can be interrupted at any time andreturn a valid solution.
To enable fast, anytime, asymptotically-optimal motion planning in point clouds,
we propose to blend two popular motion planning paradigms: (1) sampling-basedmotion planning and (2) optimization-based motion planning. Sampling-based
motion planners can directly use point cloud data by incorporating appropriate col-
lision detection algorithms [ 19] and can optimize a cost metric in an asymptotically
optimal manner [ 12]. However, sampling-based motion planners in practice can be
This research was supported in part by the U.S. National Science Foundation (NSF) under awards
CCF-1533844 and IIS-1149965 and the U.S. National Institutes of Health (NIH) under awardR01EB024864.
A. Kuntz (B)·C. Bowen ·R. Alterovitz
University of North Carolina at Chapel Hill, Chapel Hill, NC 27599-3175, USAe-mail: adkuntz@cs.unc.edu
C. Bowen
e-mail: cbbowen@cs.unc.edu
R. Alterovitz
e-mail: ron@cs.unc.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_63929
930 A. Kuntz et al.
Fig. 1 ISIMP interleaves sampling-based and optimization-based motion planning. aISIMP ﬁrst
performs sampling-based motion planning until a feasible motion plan is found from start to goal.
bIt then uses interior point optimization to locally optimize the plan. cSampling-based motion
planning resumes until a shorter plan is found. dThe shorter plan is then optimized. The method
iterates in this fashion and can be interrupted at any time after step aand return a feasible solution
that gets better over time. eAn example sampling-based plan found for a Baxter robot in a point
cloud sensed by Microsoft Kinect. fThat plan locally optimized
slow to converge and frequently return paths in ﬁnite time that are far from optimal,
especially for problems with higher dimensional conﬁguration spaces [ 10]. In con-
trast, optimization-based motion planners are often very fast [ 11,20,21,23], but
typically do not converge to globally optimal solutions, do not operate in an any-
time manner (since, even when initialized with a feasible solution, their intermediate
iterations may consider robot conﬁgurations that are in collision with obstacles),
and are inefﬁcient when using large point clouds. In this paper, we introduce a new
optimization approach to motion planning based on interior point optimization [ 24,
25], which has unique properties that enable anytime motion planning and efﬁcient
handling of large point clouds. By integrating our new interior point optimization
formulation with a sampling-based method, our approach quickly computes locally
optimized plans in an anytime fashion, and continues to reﬁne the solution toward
global optimality for as long as time allows.
Our new method, Interleaved Sampling and Interior point optimization Motion
Planning (ISIMP), interleaves global exploration with local optimization (see Fig. 1).
The method starts with global exploration by building a graph using an asymptotically
optimal motion planner, such as k-nearest Probabilistic Roadmap (PRM\*) [ 12], until
it ﬁnds a collision-free path. It then uses our lazy interior point optimization formula-
tion to reﬁne the path found by the sampling-based method. The algorithm then iter-
ates between (1) resuming the sampling-based motion planner until a new better path
Fast Anytime Motion Planning in Point Clouds … 931
is found and (2) running interior point optimization on this new path. The sampling-
based motion planning phase of each iteration explores globally, discovering other
homotopy classes in conﬁguration space, as well as escaping local minima in thepath cost landscape. The interior point optimization phase in turn allows the method
to provide a high quality locally-optimized motion plan based on the best path found
by the sampling-based method. Either phase can be interrupted at any time and stillreturn a valid, collision-free result. Interleaving these phases provides higher quality
motion plans earlier than asymptotically optimal sampling-based motion planning
algorithms alone. In this way, our method preserves guarantees which are not pro-vided by most optimization-based motion planning algorithms—namely complete-
ness and asymptotic optimality—all in an anytime fashion and designed speciﬁcally
to work efﬁciently on point cloud data.
ISIMP is based on interior point optimization, a type of local optimization that
has properties critical to achieving fast, anytime motion planning for robotic manip-
ulators. Interior point methods solve optimization problems by only consideringfeasible points in each iteration of the method. These methods typically incorpo-
rate constraints into the objective function by adding barrier functions, continuous
functions whose value on a point increases to inﬁnity as the point approaches the
boundary of the feasible region. These methods adjust weights on the barrier func-
tions over time so the solution converges to a locally optimal solution of the originalobjective function. We apply interior point optimization to reﬁning motion plans
and show that it is fast (like other motion plan optimizers, e.g., [ 11,13,18,20,21,
23]) while providing three additional important properties. First, our interior point
optimization implementation is an anytime algorithm: each intermediate iteration
of the optimization algorithm considers only collision-free robot conﬁgurations, so
the optimizer always returns a feasible motion plan even when interrupted. Second,interior point optimization can be formulated to efﬁciently and directly handle large
point clouds, which we accomplish using a lazy evaluation of constraints. This elimi-
nates the time-consuming process of transforming the point cloud into more complexgeometric primitives or meshes, which is often required by other optimization-based
motion planners to be efﬁcient. Third, our interior point optimization is designed to
optimize path length, a commonly desired metric which is often used by sampling-based motion planners. Many existing optimization-based motion planners [ 11,20,
21,23] are designed to optimize metrics such as smoothness, which do not sat-
isfy the triangle inequality and hence are incompatible with cost requirements ofasymptotically-optimal sampling-based motion planners.
We evaluate the efﬁcacy of ISIMP for a Baxter robot’s manipulator arm using a
Microsoft Kinect to sense point cloud data in a cluttered environment. We demon-
strate ISIMP’s fast, anytime, and asymptotically optimal performance in comparison
to other motion planners that only use either sampling or local optimization.
932 A. Kuntz et al.
2 Related Work
In sampling-based motion planning, a graph data structure is constructed incremen-
tally via random sampling providing a collision-free tree or roadmap in the robot’sconﬁguration space. These algorithms provide probabilistic completeness, i.e., the
probability of ﬁnding a path, if one exists, approaches one as the number of samples
approaches inﬁnity. Examples include the Rapidly-exploring Random Tree (RRT)[16] and the Probabilistic Roadmap (PRM) [ 13] methods. These have been adapted
in many ways, such as taking advantage of structure in the robot’s conﬁguration
space or modifying the sampling strategy [ 6,15].
Adaptations of these algorithms can provide asymptotic optimality guarantees
in which the path cost (e.g., path length) will approach the global optimum as the
number of algorithm iterations increases. Examples include RRT\* and PRM\* [ 12]
where the underlying motion planning graph is either rewired or has asymptotically
changing connection strategies. Other asymptotically optimal algorithms grow a treein cost-to-arrive space [ 10], identify vertices likely to be a part of an optimal path [ 1],
or investigate the distributions from which samples and trajectories are taken [ 14].
Lazy collision checking has been shown to substantially improve the speed of thesealgorithms [ 9], and in some cases, near optimality can be achieved while improving
speed [ 7,22].
Optimization-based motion planning algorithms perform numerical optimization
in a high dimensional trajectory space. Each trajectory is typically encoded as a vec-
tor of parameters representing a sequence of robot conﬁgurations or controls. A cost
can be computed for each trajectory, and the motion planner’s goal is to computea trajectory that minimizes cost. In the presence of obstacles and other constraints,
the problem can be formulated and solved as a numerical optimization problem.
Examples include taking an initial trajectory and performing gradient descent [ 21],
using sequential quadratic programming with inequality constraints to locally opti-
mize trajectories [ 23], and combining optimization with re-planning to account for
dynamic obstacles [ 20]. These methods typically produce high quality paths but are
frequently unable to escape local minima, and as such are subject to initialization
concerns. To avoid local minima, some methods inject randomness [ 3,11]. The solu-
tions of these existing optimization-based motion planners may converge to locally
optimal motion plans that include robot conﬁgurations that are in collision with
obstacles. This limitation can be partially circumvented through techniques such asrestarting the optimization with multiple different initial paths (e.g., [ 23]), but such
approaches are heuristic and provide no guarantee that a collision-free trajectory will
be found in general.
Several methods aim to bridge the gap between sampling-based methods and
optimization. Some methods use paths generated by global planners and reﬁne them
using shortcutting or smoothing methods adaptively or in post processing [ 8,18].
GradienT-RRT moves vertices to lower cost regions using gradient descent during
the construction of an RRT [ 2]. More recently, local optimization has been used
on in-collision edges during sampling-based planning to bring them out of colli-
Fast Anytime Motion Planning in Point Clouds … 933
sion and effectively ﬁnd narrow passages [ 4]. In contrast, our method is using the
local optimizer not to ﬁnd narrow passages, but to improve the overall quality of
the paths found by the sampling-based planner, while relying on the sampling-basedplanner’s completeness property to discover narrow passages. BiRRT-Opt [ 17] uti-
lizes a bi-directional RRT to generate an initial trajectory for trajectory optimization,
demonstrating the efﬁcacy of a collision-free initial solution for local optimization.Our method differs in that interior point optimization provides collision-free iterates
allowing it to work in an anytime fashion, and our method continues beyond a single
local optimization to provide global asymptotic optimality.
3 Problem Deﬁnition
Let Cbe the conﬁguration space of the robot. Let q∈Crepresent a single robot con-
ﬁguration of dimensionality dandΠ={q0,q1,..., qn−1}represent a continuous
path in conﬁguration space in a piecewise linear manner by a sequence of nconﬁgura-
tions. Such paths may need to satisfy generalized, user-deﬁned inequality constraints.
These constraints could include joint limits, end effector orientation requirements,
etc., where each constraint may be represented by the inequality g(Π)≥0f o rs o m e
constraint speciﬁc function g. Let the set of all such user-deﬁned constraints be J.
In the robot’s workspace are obstacles that must be avoided, and which are being
represented by a point cloud. We consider each point in the point cloud an obstacleand let the set of all such points be O. We also deﬁne the robot’s geometry as a
set of geometric primitives S. An individual geometric primitive s∈Scould be
represented as a mesh, bounding sphere, capsule, etc.
A path is collision-free if the robot’s geometry Sover each edge (q
i,qi+1),i=
0,..., n−2, does not intersect an obstacle o∈O. Formally, we require a func-
tionclearance (qi,qi+1,s,o)which is a function parameterized by a path edge
(qi,qi+1), a geometric primitive s, and an obstacle o. This function, deﬁned in
Sec. 4, is continuous and monotonically increasing, has positive value when ois not
in collision with sover the edge, negative value when openetrates into son the
edge, and 0 at the boundary. A collision-free path is then one for which each edge
has non-negative clearance for all o∈Oand s∈S. This requirement can be repre-
sented as an additional set of inequality constraints, which we deﬁne as K, wherein
clearance (qi,qi+1,s,o)≥0, for all o∈Oand s∈S.
Our objective is to ﬁnd a collision-free path for the robot from the robot’s start
conﬁguration qstartto a user speciﬁed goal conﬁguration qgoalthat satisﬁes all the
constraints and minimizes path length. We deﬁne path length by length (Π) ,t h e
sum of Euclidean distances in conﬁguration space along the path. We use the sumof Euclidean distances because it is a commonly used metric for path length and
because it satisﬁes the triangle inequality property required by asymptotically opti-
mal sampling-based motion planners (unlike other metrics such as sum of squareddistances). This optimal motion planning problem can be formulated as a nonlinear,
constrained optimization problem:
934 A. Kuntz et al.
Π∗=argmin
Πlength (Π)
Subject to:
clearance (qi,qi+1,s,o)≥0, 0≤i0, as a “look-ahead”
distance. Let
d=min\_dist (s,qi,qi+1,o), (2)
clearance (d,r)=⎧
⎪⎨
⎪⎩2/parenleftbigd
r/parenrightbig/parenleftbigd
r/parenrightbig
<0/parenleftbigd
r/parenrightbig4−2/parenleftbigd
r/parenrightbig3+2/parenleftbigd
r/parenrightbig
0≤/parenleftbigd
r/parenrightbig
≤1
1/parenleftbigd
r/parenrightbig
>1,(3)
where min\_dist is the minimum distance between the point oand the geometric
primitive swhen sis swept along the space curve deﬁned by the transition between
conﬁgurations qiandqi+1, i.e. as the robot arm is moving from conﬁguration qi
toqi+1. Equation 3sends the clearance function’s derivative to zero at r, allowing
us to sparsify the constraint Jacobian and only consider points in Kenab which are
within distance rfrom the robot’s geometry at a given time. This allows even greater
speedup to the optimization, as the constraint set Kenab, which is already greatly
reduced in size from the full point cloud, is still larger than needs to be considered
in many entries of the constraint Jacobian.
Another challenge of our constraint formulation is that the minimum clearance
between a given robot geometric primitive along an edge and a point in the point
cloud is not guaranteed to be differentiable. This is because the robot’s arm is tracing
a nonlinear, complex curve through the workspace, even though the edge is linear
in conﬁguration space. This means that in some places, the closest position on that
curve to an obstacle point can be discontinuous as the edge is perturbed (see Fig.3b), i.e., there are multiple closest positions on the curve which are equidistant to
the point cloud point. Using distance over the nonlinear edge allows us to ensure
obstacle avoidance, but can cause numerical problems during the optimization. Toaddress this issue, when we are evaluating the derivative of a constraint and ﬁnd it to
be discontinuous over an edge, we bisect that edge and start the optimization anew
Fast Anytime Motion Planning in Point Clouds … 939
Fig. 3 a The cutoff function used as the clearance function for our obstacle avoidance constraints.
The function ﬂattens out after a certain distance so as to allow us to only consider points in Kenab
which are within a speciﬁed radius r.b,cAn example with a manipulator (shown in red). bIf
a sphere bounding the robot geometry is swept between two conﬁgurations and produces a curve
such as the blue curve here, then when d1and d2are equal, the minimum distance between sphere
s1swept from qktoqk+1and point cloud point ois not necessarily differentiable. cWhen such a
case is detected, an intermediate conﬁguration is added into the path with its own constraint. In this
way, both d1and d2are used in different constraints and are both locally differentiable
(see Fig. 3c, and Algorithm 2 lines 9–12). In this way, after enough bisections the
discontinuity is avoided. In practice, we observed relatively few bisections.
4.3 Asymptotic Optimality
Asymptotic optimality of our method falls naturally from the use of an asymptotically
optimal motion planner as the underlying sampling-based motion planner, as long
as three conditions are met. First, the sampling-based motion planner adds at least
one node to its roadmap between adjacent interior point optimization calls. Second,
the interior point optimization does not make the roadmap’s solution worse. Third,
the interior point optimization completes in ﬁnite time.
The ﬁrst condition is dependent on the implementation of the sampling-based
motion planner. The implementations we use guarantee this property. The second
condition can be handled by discarding any solutions which are worse than the best
that has been found at any point in time. The third condition can be guaranteed with
an external time limiter on the optimization.
5 Results
We implement the sampling-based motion planning portion of our method using the
OMPL framework [ 5], including OMPL’s PRM\* and RRT#implementations. In this
section, we refer to ISIMP with PRM\* as ISIMP , and ISIMP with RRT#as ISIMP-#.
940 A. Kuntz et al.
For the black box local optimizer, we use the IPOPT software framework, an open
source interior point optimization library1[24].
We consider a scenario involving motion planning for the left arm of a Baxter
robot (see Fig. 6). For the purposes of computing constraints in the optimization and
obstacle avoidance in the sampling-based motion planner, we represent the geometry
of the robotic manipulator arm as a set of 9 bounding spheres along the links of therobot’s arm. The choice of bounding spheres allows us to conservatively represent
the robot’s geometry while enabling fast distance calculations between the geometric
primitives and the points in the point cloud.
We evaluate ISIMP’s performance in a real-world point cloud obtained from a
Microsoft Kinect sensor (see Fig. 6). We evaluate our method utilizing point clouds
of differing sizes, ≈10,000 points, ≈25,000 points, and ≈100,000 points, the smaller
of which were generated via downsampling the original point cloud. We generated
50 motion planning scenarios at random in the scene using random start and goal
conﬁgurations, using rejection sampling to remove trivial scenarios for which astraight-line naive trajectory would not collide with the point cloud.
We compare our method to the popular anytime asymptotically-optimal sampling
based motion planners PRM\* [ 12] and RRT
#[1] using OMPL [ 5], and to a popular
trajectory optimization method, Traj-Opt [ 23], using their distributed source code.
All results were generated on an 3.40 GHz Intel Xeon E5-1680 CPU with 64 GB ofRAM.
5.1 Comparison to Asymptotically-Optimal Sampling-Based
Motion Planners
To evaluate how well ISIMP performs in an anytime manner, we compare our method
to PRM\* and RRT#with the three point cloud sizes. In Fig. 4a, we compare the best
path found by ISIMP , up until a given time, to the best path found by PRM\* at that
same time. We average this over 50 runs, and plot the ratio of the PRM\*’s path lengthto our method’s path length. A value greater than one indicates that our method has
a shorter path, and a value less than one indicates that PRM\* has a shorter path.
Similarly, in Fig. 4c, we compare ISIMP-# with RRT
#.
As another way of evaluating anytime performance, we stop ISIMP’s execution
at 1 s and then measure how long PRM\* requires to produce a solution of equal or
better cost. In the majority of cases, PRM\* in 5 min failed to produce a path of shorter
length than ISIMP’s solution at 1 s. For the cases where PRM\* did produce a better
solution in less than 5 min, we average the results and show them in Fig. 4b.
1IPOPT requires deﬁnitions of a cost function to be minimized (path length in our case), constraint
functions ( Jand KorKenab), and cost and constraint Jacobians. It then handles the optimization
iterations itself. Note that default settings will allow intermediate solutions to potentially exist ininfeasible space. We set the settings to prevent infeasible intermediate solutions.
Fast Anytime Motion Planning in Point Clouds … 941
Fig. 4 a The path length ratio over time between PRM\* and ISIMP , averaged over 50 runs with
random start and goal conﬁgurations. Results are shown for various sizes of point clouds. A value
greater than one indicates ISIMP has a shorter path than PRM\*. bAnother way to visualize anytime
results. ISIMP is stopped after 1 s, and displayed is how many times longer PRM\* ran to achieve
a comparable result. In many cases, PRM\* had still not found a comparable result after 5 min,
in which case we did not include that run in this analysis. The included runs were averaged. The
number of included runs is shown in white overlaid on each bar. cThe path length ratio over time
between RRT#and ISIMP-#
As can be seen, ISIMP outperforms PRM\* on path length by a large percentage
for a long duration (Fig. 4a), and ISIMP-# outperforms RRT#similarly, but by a
smaller percentage (Fig. 4c).
5.2 Comparison to an Optimization-Based Motion Planner
We also compare ISIMP’s performance to a state-of-the-art optimization-based
motion planner, Traj-Opt [ 23], for which open-source code compatible with the
Baxter robot is available. The Traj-Opt distribution recommends several ways to
handle point cloud data input, including: (1) converting the points in the point cloud
into down-sampled cubic boxes, and (2) constructing a polygonal mesh out of the
point cloud, and then simplifying the mesh through decimation.
As with most trajectory optimization methods, the question arises of how to gen-
erate an initial trajectory. We evaluate Traj-Opt using two initialization approaches
recommended in the optimization-based motion planning literature. First, we initial-
ize with a straight line trajectory in conﬁguration space with 10 conﬁgurations (the
number of conﬁgurations used for initialization in the example code), which starts
in collision. Second, we seed Traj-Opt with the ﬁrst path found by our method from
the sampling-based motion planner, which is augmented with duplicate conﬁgura-
tions if it starts with fewer than 10 total. This initialization starts out collision-free.
TO-BS refers to Traj-Opt with a box environment representation and a straight line
initialization, TO-MS refers to a mesh environment with a straight line initialization,
TO-BR refers to a box environment with a sampling-based motion planner initial-
ization, and TO-MR refers to a mesh environment with a sampling-based motion
planner initialization.
942 A. Kuntz et al.
(a) (b)
02468
Time (s)0%20%40%60%80%100%Feasible Paths FoundISIMP 10k
ISIMP 25k
ISIMP 100k
TO-BS
TO-MS
Fig. 5 a The percent of trials for which a feasible path has been found over 50 trials. bThe percent of trials for which feasible, collision-free, paths have
been found over time. The ISIMP variations ﬁnd feasible paths for 98% of the trials within 0.5 s (and 100% of trials within 30 s). The Traj-Opt variations take
signiﬁcantly longer ( ≈5.5–7 s) to ﬁnd feasible paths for some trials, and fail to ﬁnd feasible paths at all for the others. (TO-BR and TO-MR are not shown in b
because they are seeded with already feasible solutions from a sampling-based motion planner.)
Fast Anytime Motion Planning in Point Clouds … 943
(a) PRM\* Path (b) Optimized PRM\* Path (c) Alternate View
(d) Traj-Opt Initialization (e) Traj-Opt Converged (f) Alternate View
Fig. 6 a ISIMP’s collision-free initialization found by the sampling based motion planning algo-
rithm. b,cISIMP’s collision-free trajectory after interior point optimization, which is still collision-
free and passes behind the obstacles. The path length has been reduced by more than 36%. dTraj-Opt
is initialized with a straight line trajectory, passing through the point cloud obstacles. e,fTraj-Opt’s
converged trajectory which is still in collision due to being trapped in a poor quality local minimum
The comparison between our method and Traj-Opt reveals three things. First,
Traj-Opt can fail to converge to a collision-free path in a signiﬁcant percentage of
cases in our scenario. See Fig. 6for an example. Traj-Opt returned locally optimal
paths that were infeasible likely due to the complex objective function landscape
with many in-collision local minima induced by the real-world point cloud data.
Figure 5a shows the percentage of problems for which each method found a feasible
solution. Our method found feasible solutions to 100% of planning problems within
30 s, while variations on Traj-Opt returned locally optimal paths that were infeasible
in a signiﬁcant percentage of planning problems. This is even true in the case where
the initialization was collision-free (TO-BR and TO-MR).
Second, Traj-Opt, when it did produce a valid collision-free solution, takes
between 5.3 and 7.1 s to initialize the problem, load the scene, and optimize the
trajectory. By contrast, our method initializes the problem, loads the scene, and pro-
duces its ﬁrst valid collision-free solution in an average of 0.41 s for 10 k points,
0.43 s for 25 k, and 0.63 s for 100 k (See Fig. 5b). Traj-Opt’s slower relative speed is
mostly due to the time required to initialize the problem and pre-process the point
cloud data, with Traj-Opt’s optimization itself taking on average less than 1 s.
Third, when Traj-Opt converged to a collision-free solution it produced high
quality paths. For the cases where Traj-Opt produced a collision-free solution, the
944 A. Kuntz et al.
path lengths of its solutions were comparable to the path lengths produced by ISIMP ,
with path lengths varying by less than 5% across the methods.
6 Conclusion
We presented a method designed to achieve the beneﬁts of both local optimization
and global sampling-based motion planning when planning motions for a robotic
manipulator arm with point cloud sensor data. ISIMP interleaves asymptotically-
optimal sampling-based motion planning with anytime interior point local optimiza-tion. Designed to work with point cloud data directly, our novel lazy interior point
optimization formulation brings the path quality beneﬁts of local optimization to the
anytime performance of sampling-based methods, while providing completeness and
global asymptotic optimality guarantees not present in current optimization-based
motion planning methods. The results demonstrate that our method outperformsasymptotically-optimal sampling-based motion planning alone, producing higher
quality motion plans earlier. Our method also outperforms an optimization-based
method, Traj-Opt, in the percentage of collision-free solutions found and in time toﬁrst valid solution.
In the future, we plan to evaluate ISIMP on other robotic systems of varying
dimensionality. We intend to incorporate more complex constraints, such as taskconstraints, into the method and to explore other optimization objectives, such as
clearance from obstacles. We will also investigate the interesting question of how to
best balance the time spent on sampling versus optimizing.
References
1. Arslan, O., Tsiotras, P .: Use of relaxation methods in sampling-based algorithms for optimal
motion planning. In: Proceedings of IEEE International Conference on Robotics and Automa-tion (ICRA), pp. 2421–2428 (2013)
2. Berenson, D., Simeon, T., Srinivasa, S.S.: Addressing cost-space chasms in manipulation plan-
ning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA),pp. 4561–4568 (2011)
3. Carriker, W., Khosla, P ., Krogh, B.: The use of simulated annealing to solve the mobile manip-
ulator path planning problem. In: Proceedings of IEEE International Conference on Robotics
and Automation (ICRA), pp. 204–209 (1990)
4. Choudhury, S., Gammell, J.D., Barfoot, T.D., Srinivasa, S.S., Scherer, S.: Regionally acceler-
ated batch informed trees (RABIT\*): A framework to integrate local information into optimalpath planning. In: Proceedings of IEEE International Conference on Robotics and Automation(ICRA), pp. 4207–4214 (2016)
5. ¸Sucan, I.A., Moll, M., Kavraki, L.E.: The open motion planning library. IEEE Robot. Autom.
Mag. 19(4), 72–82 (2012). http://ompl.kavrakilab.org
6. Denny, J., Amato, N.M.: Toggle PRM: simultaneous mapping of C-free and C-obstacle - a
study in 2D. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), pp. 2632–2639 (2011)
Fast Anytime Motion Planning in Point Clouds … 945
7. Dobson, A., Bekris, K.E.: Sparse roadmap spanners for asymptotically near-optimal motion
planning. Int. J. Robot. Res. 33(1), 18–47 (2014)
8. Geraerts, R., Overmars, M.H.: Creating high-quality paths for motion planning. Int. J. Robot.
Res. 26(8), 845–863 (2007)
9. Hauser, K.: Lazy collision checking in asymptotically-optimal motion planning. In: Proceed-
ings of IEEE International Conference on Robotics and Automation (ICRA), pp. 2951–2957(2015)
10. Janson, L., Schmerling, E., Clark, A., Pavone, M.: Fast marching trees: a fast marching
sampling-based method for optimal motion planning in many dimensions. Int. J. Robot. Res.34(7), 883–921 (2015)
11. Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P ., Schaal, S.: STOMP: Stochastic trajec-
tory optimization for motion planning. In: Proceedings of IEEE International Conference onRobotics and Automation (ICRA), pp. 4569–4574 (2011)
12. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J.
Robot. Res. 30(7), 846–894 (2011)
13. Kavraki, L.E., Svestka, P ., Latombe, J.C., Overmars, M.: Probabilistic roadmaps for path plan-
ning in high dimensional conﬁguration spaces. IEEE Trans. Robot. Autom. 12(4), 566–580
(1996)
14. Kobilarov, M.: Cross-entropy motion planning. Int. J. Robot. Res. 31(7), 855–871 (2012)
15. Kurniawati, H., Hsu, D.: Workspace importance sampling for probabilistic roadmap planning.
In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), vol. 2, pp. 1618–1623 (2004)
16. LaV alle, S.M., Kuffner, J.J.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5),
378–400 (2001)
17. Li, L., Long, X., Gennert, M.A.: BiRRTOpt: A combined sampling and optimizing motion plan-
ner for humanoid robots. In: IEEE-RAS 16th International Conference on Humanoid Robots(Humanoids), pp. 469–476 (2016)
18. Luna, R., ¸ Sucan, I.A., Moll, M., Kavraki, L.E.: Anytime solution optimization for sampling-
based motion planning. In: Proceedings of IEEE International Conference on Robotics andAutomation (ICRA), pp. 5068–5074 (2013)
19. Pan, J., Chitta, S., Manocha, D.: FCL: A general purpose library for collision and proxim-
ity queries. In: Proceedings of IEEE International Conference on Robotics and Automation(ICRA), pp. 3859–3866 (2012)
20. Park, C., Pan, J., Manocha, D.: ITOMP: incremental trajectory optimization for real-time
replanning in dynamic environments. In: International Conference on Automated Planningand Scheduling (ICAPS) (2012)
21. Ratliff, N.D., Zucker, M., Bagnell, J.A., Srinivasa, S.: CHOMP: gradient optimization tech-
niques for efﬁcient motion planning. In: Proceedings of IEEE International Conference onRobotics and Automation (ICRA), pp. 489–494 (2009)
22. Salzman, O., Halperin, D.: Asymptotically near-optimal RRT for fast, high-quality motion
planning. IEEE Trans. Robot. 32(3), 473–483 (2016)
23. 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 Sys-
tems (RSS) (2013)
24. Wächter, A., Biegler, L.T.: On the implementation of an interior-point ﬁlter line-search algo-
rithm for large-scale nonlinear programming. Mathematical Programming 106(1), 25–57
(2006)
25. Wright, S., Nocedal, J.: Numerical Optimization. Springer Science, Berlin (1999)
From Quasi-static to Kinodynamic
Planning for Spherical TensegrityLocomotion
Zakary Littleﬁeld, David Surovik, Weifu Wang and Kostas E. Bekris
1 Introduction
A versatile class of mobility platforms has emerged from the structural principle of
tensegrity [ 31], which distribute stresses across a compliant network of linear ele-
ments, such as rods and cables, as shown in Fig. 1. The concept was ﬁrst explored in
architecture [ 9] and biology, as it relates to spine structures [ 17] and cytoskeletons
of single-celled moving organisms [ 12]. Tensegrity structures are able to physically
change their structural expression based on external forces, while minimizing bend-
ing and shearing [ 31].
Tensegrity Robotics : Tensegrity robots actively control cable lengths and can
adapt to contact dynamics. Given their compliant nature, they are ideally suited for
unstructured terrains [ 13,22], where contact forces impact the motion of the system
and traditional mechanisms are not as adaptive [ 36]. These beneﬁts have motivated
ongoing work on the design, simulation, and hardware prototyping of tensegrity
robots [ 6,31]. Multiple behaviors have already been demonstrated: crawling [ 22,24],
swimming [ 3], rolling locomotion [ 13,14], and deployment as compliant many-DoF
joints, as in Fig. 1(b). Example uses include search and rescue in disaster areas, the
exploration of natural environments, exploration and cleaning of pipes and ducts [ 7],
and space-related applications [ 8]. Early prototypes were focused on form ﬁnding or
Z. Littleﬁeld ·D. Surovik ·K. E. Bekris ( B)
Department of Computer Science, Rutgers University, Piscataway, NJ, USAe-mail: kostas.bekris@cs.rutgers.edu
Z. Littleﬁeld
e-mail: zwl2@cs.rutgers.edu
D. Surovik
e-mail: david.surovik@cs.rutgers.edu
W. Wang
Department of Computer Engineering, University of Albany, SUNY, Albany, USAe-mail: wwang8@albany.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_64947
948 Z. Littleﬁeld et al.
Fig. 1 Tensegrity robots: aNASA SUPERball: a robotic icosahedron with 6 rods and 24 cables [ 29].
bA duct climbing robot: 2 tetrahedral frames with 8 actuated cables [ 7]
Fig. 2 SUPERball tensegrity robot prototype: aduring a test at NASA Ames. bactive crouching
by contracting cables (background and cables removed). cdeformation during droptest. drolling
from face to face
basic motion primitives and were limited to no sensor feedback [ 4,21,24,28,30].
More recently, ReCTeR is an un-tethered tensegrity prototype that allows for closed-loop locomotion [ 6].
A similar geometry tensegrity robot is SUPERball, which is a prototype planetary
exploration rover for NASA to study dynamic locomotion and path planning [ 5,29].
Figure 2illustrates the capabilities of SUPERball. This is the focus platform for
the current paper, given a model of the robot in a physics engine [ 1], which has
From Quasi-static to Kinodynamic Planning … 949
been veriﬁed against the physical prototype [ 23]. Motion planning for such systems
is needed to perform tasks with long horizons, such as goal-directed navigation,
obstacle-avoiding locomotion or purposeful deformation of a tensegrity robot.
Solving such problems, however, is challenging because both the state and con-
trol spaces of tensegrity robots are high-dimensional and they are highly-dynamic
systems. These traits arise from the several elements that comprise the robot and theircomplex interactions and contacts with the environment. Furthermore, providing a
predictive model for a tensegrity robot requires a physics-based simulator [ 1], which
is a computationally expensive primitive. This implies that a motion planner needsto minimize calls to the physics engine to achieve efﬁcient computation.
Prior Planning Approaches for Tensegrity Robots : Some early planners pro-
vided deployment and shape changes through optimization to generate staticallystable conﬁgurations [ 25,33]. Further work accounted for self-collisions [ 10,35].
More recent approaches plan paths for tensegrity mobility but assume a control pro-
cess slow enough to eliminate dynamic effects [ 26,35]. This quasi-static approach
is popular in civil engineering [ 27] but is not desirable in robotics. Methods have
been developed for utilizing the dynamics of tensegrity structures at a local level,
either along static equilibrium manifolds [ 32] or through feedback linearization con-
trol laws [ 2] and Lyapunov-based controllers for 3D systems [ 34]. These approaches
generally have not accounted for self-collisions or environmental contacts, limitingreal-world applicability.
Preliminary work by the authors has provided a proof of concept application of
sampling-based kinodynamic planning to a tensegrity-based robot [ 20]. The initial
attempt was fairly computationally expensive as it exclusively depended on random
controls, which did not result in meaningful tensegrity locomotion. This prior work
did not consider quasi-static solutions and did not provide a comparative evaluation.
Contribution : This paper investigates motion planners for spherical tensegrity
locomotion, which span the spectrum of quasi-static to fully dynamic. First, it pro-
poses a kinematic reasoning for generating highly controllable but slow gaits for thepurposeful movement of SUPERball. The number of gaits needed is minimized by
identifying the platform’s topological symmetries. An informed search process, such
asA
∗, that uses these kinematic gaits can then solve some problems effectively in a
quasi-static manner. More challenging problems, however, may require signiﬁcant
deformations or the excitement of the platform’s dynamics.
The current work shows that sampling-based kinodynamic planners, such Expan-
sive Space Trees [ 11] andRRT [16], can generate solutions in these more challenging
setups, as long as an appropriate control sampling process is employed. The proposed
sampling scheme simultaneously accounts for constraints that arise due to couplings
between different control inputs, and biases toward values that have a high probabil-
ity of effecting useful motion. The overall control selection process, which includesquasi-static gaits, provides a full range of capabilities from slow, purposeful motions
to fast, dynamic motions.
The early sampling-based kinodynamic planners, however, do not improve solu-
tion quality over time. The current paper builds on top of recent contributions by the
authors [ 18,19] to develop an anytime kinodynamic planner for spherical tensegrity
950 Z. Littleﬁeld et al.
locomotion that is informed by the proposed quasi-static reasoning. In particular,
the proposed method is initially biased along a quasi-static solution by using the
generated kinematic gaits. When these gaits do not make progress or once an initialsolution has been found, the method utilizes the control sampling process to explore
high-quality dynamic solutions. Experiments using the physically simulated model
of SUPERball show the beneﬁts of the planner. It achieves improved solution qualityand good computational performance relative to alternatives, while working in a
variety of environments and adapting its performance to the scene’s characteristics.
2 Setup: Target Platform and Identiﬁcation of Symmetries
Platform’s Structure : SUPERball is an approximately spherical tensegrity platform.
Its convex hull is pseudo-icosahedral as true icosahedrons have equal triangles. Allcompressive elements (bars) cross its interior and all tensile elements (cables) lie
on its exterior. The bars end up on nodes where the cables link to. The following
discussion reasons about the faces of the structure as follows:
•Δ
i,i∈[0,7]: These are eight triangular exterior faces, which have cables along
all three edges and never share an edge with another Δi.
•Λi,i∈[8,19],triangular faces: The remaining 12 faces have cables on only two
edges. They occur in pairs with a shared cable-free “virtual” edge.
The state xof SUPERball can be expressed as a collection of six rigid body states,
expressing the translational and rotational position and velocity of each bar, combined
with the rest lengths of the cables. These lengths may differ from the actual cablelengths, which are derivable from bar states, due to elastic deformation. Motion can
be induced by commanding cable rest lengths uwithin a feasible range of u
−=0.35b
tou+=0.65bwith a standard value u∗=0.57b, where bis the length of a bar. If
all the cable lengths have the standard value, then this gives rise to the standard
cable point u∗, seen in Fig. 3(left), where the Δifaces are equilateral and the Λiare
isosceles. An intermediate PID controller governs the behavior of the cable spool
motors to attempt to fulﬁll these commands within limitations on actuation speed
of about ω=0.17b/sec. Given the use of a PID controller, the available controls
for planning purposes correspond to desired cable lengths. The physical model ofthe platform used during planning assumes the cables to always have tension and no
slack between the two connected rods. While this is an approximation of reality, the
real platform aims to keep the cables close to tension. The motion model for these
cables is modeled as torques on a spooling motor.
Topological Mapping : In its standard conﬁguration, SUPERball exhibits pyri-
tohedral symmetry : there are 24 unique combinations of rotations and reﬂections
that result in an equivalent conﬁguration. This regularity is also expressed topolog-
ically: all nodes are connected to four cables, all Δ
ifaces are bordered by three Λi
faces with the same relative handedness, etc. Figure 3presents a 2D illustration of
the system topology in relation to its 3D expression. Upon actuation, the platform
From Quasi-static to Kinodynamic Planning … 951
Δ Λ
Λ
Fig. 3 SUPERball topology ﬂattened onto the plane after making a cut, shown as gray boundaries.
Nodes are represented by circles, actuators by black lines, virtual edges by dotted lines, and Δi
equilateral faces are shaded. The thick, colored curves correspond to rigid bars that connect the
corresponding nodes. The bars are in the interior of SUPERball and do not deﬁne exterior faces.
Matching bars are parallel in physical space
becomes physically asymmetric, but the structural connectivity of its various ele-
ments remains constant. While the 24 rotation and reﬂection operations no longer
preserve conﬁguration, there remain 24 unique permutations of element ID labels
that preserve topology, referred to as topological mappings .
Motion Primitives : The basic locomotive operation for SUPERball is to transi-
tion from one face on the ground to an adjacent face on the ground instead. This is
illustrated by the green arrows in Fig. 4. Since the surface geometry of SUPERball
contains 24 edges occupied by cables and six additional virtual edges, 60 such transi-
tions are possible when accounting for directionality. Nevertheless, reduction under
topological equivalence reduces this set of possibilities to three options: (i) Λ→Δ,
Fig. 4 Two equivalent
instances of the ˜uΔΛmotion
primitive are depicted on a
subset of SUPERball’s
topology with element ID
labels
95
11
7Λ14Δ2
Δ322
38
1˜uΔΛ
17Λ13Λ122
˜uΔΛ
952 Z. Littleﬁeld et al.
(ii)Δ→Λ, and (iii) Λ→Λ. An additional, longer transition Δ→Λ→Δis also
considered as it can easily be achieved in a single ﬂuid motion. Consequently, design-
ing motion primitives for the following four transitions is sufﬁcient to allow the robotto switch between any pair of faces:
U={˜u
ΛΔ,˜uΔΛ,˜uΛΛ,˜uΔΛΔ} (1)
Consider Fig. 4where, for instance, motion primitive ˜uΔΛwas designed to transi-
tion from face Δ2over edge 22 toΛ14. If the robot is resting upon face Δ3,t h es a m e
primitive can be reused to transition to Λ13as long as two steps are applied to adapt
˜uΔΛ. First, the topological mappings need to be applied that map face Δ2ontoΔ3
andΛ14ontoΛ13. Then, this same mapping is used to permute the dimensions of
˜uΔΛin accordance with the re-labeling of cable elements. Note that when beginning
on aΔface, both uΔΛanduΔΛΔ primitives can be executed, totaling six options.
From a Λface, uΛΔcan be executed over either long edge or uΛΛover the short
edge, for three total options.
Time-Parameterized Control : The simplest option for deﬁning ˜uis a single
point uin the control space U, i.e., a set of target cable lengths, and a duration
T. The underlying PID controller will then attempt to satisfy u. Achieving u,
however, may not be feasible under geometric constraints or may require signif-icant actuation time. V alues of Tcan be deﬁned in proportion κto the actuation
rateωand the total actuation necessary based on the previous cable point, i.e.,
T=/bardblu−u
0/bardbl∞(κ/ω), where uis the commanded cable lengths and u0is the pre-
vious set of cable lengths.
Another parameterization is a sequence ˜u={u1,u2,..., uM}of cable lengths
used as targets one after the other. More generally, ˜uis a continuous path through the
24-dim. domain of cable points U⊂R24. Time is discretized at 10 Hz, in line with
processing limitations on the real robot. Simple locomotive actions typically last in a
range of around 10–30 time steps or 1 to 3 s. The cost of exploring these possibilitiesmotivates the ofﬂine determination of good motion primitives.
A compromise between constant inputs and 30-step sequences is to deﬁne only a
few ( M/lessmuch30) cable points and interpolate linear transitions between them, which
makes the response of the physical platform more stable. Beginning from the stan-dard cable point u
∗, the series ˜ucan then be parameterized as {ui,δti}1:Mwith
lin/parenleftbig
ui,uj,δt/parenrightbig
denoting a linear transition from uitoujoverδttime steps:
˜u=concatenate (lin(u0,u1,δt1),lin(u1,u2,δt2),···,lin(uM−1,uM,δtM)) (2)
Using uM=u0(=u∗)makes these primitives easier to apply in sequence, but
requires time spent reconﬁguring without achieving forward movement. In the fol-lowing discussion, both control parameterizations will be used: (a) constant inputs
will be used when randomly sampling control online and (b) the set of longer
sequences listed in Eq. 1will be pre-established ofﬂine as useful motion primitives.
Problem Summary : The target task is to move the center-of-mass of SUPER-
ball through an environment with obstacles to reach a target location. The path
From Quasi-static to Kinodynamic Planning … 953
cost is the distance traveled by the center of mass. Because of the platform’s
rugged nature, collisions with the environment are permissible and may even
be beneﬁcial for providing traction or helping the robot compress itself at anarrow gap.
3 Kinematic Primitives
The motion of SUPERball is inherently dynamic, due to nontrivial inﬂuences from
gravity, friction, momentum, etc. Physically accurate simulation of the motion
requires signiﬁcant computation. Strictly kinematic approaches for generatingmotion, though less realistic, are very light-weight. They can provide commands
˜ubased on simple reasoning about static stability for the center of mass ¯q, which is
the mean of all nodes: ¯q=Σq
i/12, i.e., the endpoints of rigid bars, where the cables
are attached.
Kinematic Objective : The process assumes that the platform starts at the standard
cable point u∗, where a single triangular face is in contact with the ground. In such
conﬁgurations, the downward projection of the center of mass ¯qlies within that “base
face”. The objective is to generate a motion primitive that causes a neighboring faceto become the base relative to the current one, i.e., to cause SUPERball to roll. The
roll occurs when the downward projection of ¯qis moved across any edge of the
current base face. Once the roll occurs, SUPERball should attain again the standardcable point u
∗, from where these primitives can be applied sequentially.
One way to compute the necessary deformation would be to compute the geo-
metric effect of uupon¯q. This is computationally prohibitive even under coarse
discretizations given the high-dimensionality of U. Alternatively, a method is fol-
lowed that numerically evaluates the sensitivity of uto a change of ¯qso as to ﬁnd
the control that produces a desired shift.
Constraints : Ignoring elastic effects, SUPERball’s 6 rigid body states provide
36 degrees of freedom ( DoFs ), corresponding to 12 node positions qi. Relative
distance constraints account for 30 DoFs , with 24 corresponding to cable lengths
ukand six more to bar lengths b, which are constant and equal. The remaining
free 6 DoFs correspond to the absolute position and orientation of the platform.
The relationship ¯q(u)requires solving a system of distance constraint equations
dk=/bardblqi−qj/bardblwhere each k’th structural element connects nodes iand j, with
dk=ukfork∈(0,23)and dk=bfork∈(24,29). No analytical approach exists
for solving such a large set of constraints. Furthermore, many cable points umay not
be valid for the system, such as when a series of two cables linking opposite ends of
one bar do not satisfy the triangle inequality.
Solution Technique : A constraint Jacobian Jcan be computed to numerically
approximate the sensitivity of ¯qtouthrough linearization about a reference value
ofu. Denoting the set of node coordinates as Q=/bracketleftbigqT
1,qT
2,..., qT
12/bracketrightbigT, then:
954 Z. Littleﬁeld et al.
Ji,j=δui
δqj,δ u=JδQ (3)
where individual terms Ji,jreﬂect small shifts along the manifold that satisfy the
distance constraints imposed by other elements. If any node qidrifts below the
ground plane during this variation, an operation is applied to the entire set such that
the lowest three nodes are precisely at ground level.
The local sensitivity of the center of mass can similarly be posed as δ¯q=JδQ.
Then, the cable length change that fulﬁlls a desired movement of ¯qis:
δu=J·J−1·δ¯q,where J−1is a pseudo-inverse. (4)
Using this linearized information to produce large changes, however, can result
in constraint violations. To satisfy the constraints a 2-step process is followed. Thecable lengths are ﬁrst satisﬁed, which is an easier problem, regardless of the bar
constraints. The second step is to ensure that all bars have length bby appropriately
moving the nodes, while minimizing the changes to the discovered cable lengths.
These two steps can be applied iteratively until the nodes move less than a predeﬁned
threshold. A Python implementation of the process requires about 0.2 s of runtimein a single threaded implementation on a commodity laptop computer.
Designing Primitives : The above process is used ofﬂine to deﬁne the motion
primitives of Eq. 1. Each primitive is initiated at u
∗, the reference cable lengths, and
δuis computed to move the planar projection of ¯qto the edge between the current base
face and the targeted one. This shift is divided into a series of 3 targeted cable points
to help accommodate constraints at intermediate stages. Once ¯qis moved beyond
the edge of the current face, the system begins to “ﬂop” over due to gravity. Then, a
single linear transition back to u∗is commanded to occur and u∗is held constant until
the system stabilizes. Thus, the overall motion primitive corresponds to the sequenceof cable lengths (u
∗,u1,u2,u3,u∗,u∗), where the transitions between cable lengths
are found using the described, numerical kinematic solver. The double face transition
˜uΔΛΔ uses the same center-of-mass shifts as ˜uΔΛand achieves its second transition
due to strong dynamic effects.
For example, consider again Fig. 4. If the robot begins at u∗and is resting upon
faceΔ2, its center of mass lies directly above the center of that face. In order to
design a command uthat causes a transition onto face Λ14, three incremental center-
of-mass shifts δ¯qare speciﬁed that would relocate ¯qso that its planar projection lies
on the opposite side of cable 22. Equation 3is then evaluated incrementally using this
input and the new control value u∗+δuis post-processed to ensure that all nodes
connected by bars, e.g., 8 and 9, are exactly distance bapart.
The motion primitives also require the timing parameterizations {δt1,...,δ t5}of
Eq.2to be computed. For this purpose, an exhaustive search is performed ofﬂine
over a range of possible δtivalues using the physics engine. The goal of the search is
to minimize the total duration of the motion primitive while maintaining robustness
to noise in the initial state. In particular, the process deﬁnes a success ratio for each 4-
tuple of time steps {δt1,δt2,δt3,δt4}by simulating it from 50 perturbed initial states
From Quasi-static to Kinodynamic Planning … 955
Fig. 5 Analysis for ˜uΔΛand
˜uΛΛprimitives
in a ﬂat environment. The success ratio counts the number of outcomes that resulted
in stably resting upon the targeted face. The ﬁnal step δt5is set equal to the time
needed for the center of mass to reach the middle of the targeted base face within a
threshold, although additional time would be needed to fully dissipate kinetic energy.
The timing search process revealed that the platform’s natural stability character-
istics allowed transition onto Δfaces to be achieved very easily, meaning that the
intermediate control points u1andu2were not needed. The more stable transitions
˜uΛΔand˜uΔΛΔ were robustly achieved with values δt1=δt2=δt4=0, where only
δt3=1. For the motion primitives ˜uΔΛand˜uΛΛit was necessary to make use of
intermediate center of mass points, i.e., δt1=δt2=δt4/negationslash=0. Figure 5shows the
relationship between actuation duration and the highest success ratio for the less
stable transitions ˜uΔΛand˜uΛΛ. The least trivial transition, uΛΛ, required 18 time
steps to reach 90% consistency. Snapshots of all transitions are shown in Fig. 6.
Fig. 6 Snapshots of executions of the three single face transitions and the natural double-transition
956 Z. Littleﬁeld et al.
4 Algorithms
This section provides an overview of motion planners for spherical tensegrity loco-
motion, which span the spectrum of quasi-static to fully dynamic solutions.
4.1 Quasi-static Solution via Informed Search
A standard informed search method, such as weighted A∗, can be used in conjunction
with the kinematic motion primitives Uof Eq. 1. A queue maintains the states that
can be reached and which have not been expanded yet, which is prioritized based on
an evaluation function f=g+w·h, where gis the path cost from the initial state,
his an optimistic heuristic estimate of the cost-to-go and wis a weight. The standard
A∗uses w=1 and for w>1, an increasingly greedier algorithm is achieved, which
sacriﬁces solution quality for getting a solution faster.
When the top state xin the queue is considered for expansion, all available motion
primitives ˜u∈Uforxare physically propagated. This set depends on the base face at
xand whether it is of type ΔorΛ. Furthermore, the applicable topological mappings
are selected to determine the edge of the current base face that will be traversed for
each motion primitive. The available motion primitives are as follows:
•If the base face is Λ: conduct a single-face transition over any of the 3 edges.
•If the base face is Δ: conduct a single-face or a two-face transition over any of the
3 edges, for a total of six options.
The heuristic value hat each state xis computed as the shortest path for a simpliﬁed
spherical rigid body to reach the goal, while avoiding obstacles. This spherical rigidbody has a radius value such that it ﬁts at the interior of the tensegrity platform
without touching the bars. At each state x, the sphere is centered at the center of
mass at that state. Then, the shortest path to the center of mass at the goal state is
computed. The computation of the heuristic is sped up online given a probabilistic
roadmap constructed ofﬂine for the spherical approximation.
The search tree given a weighted A
∗and the designed motion primitives is shown in
Fig. 7. The center-of-mass path roughly corresponds to an unraveling of the edges of
thedual icosahedron, whose nodes are the robot’s face centers. For open, ﬂat environ-
ments, this quasi-static solution solves problems reliably and fast. There are concerns,
however, relating to completeness, computational efﬁciency and path quality.
The method’s ability to ﬁnd a solution depends on the traits and the number of
motion primitives. If the terrain becomes uneven, other primitives may be needed
to achieve rolling. Other types of motion may be needed if there are obstacles that
require signiﬁcant deformation not considered during the design of the primitives.The kinematic solver can take such constraints into account to generate more prim-
itives. Nevertheless, as the number of primitives increases, a computational concern
arises. Each propagation corresponds to a call to a physics engine, and having to
From Quasi-static to Kinodynamic Planning … 957
Fig. 7 Weighted A∗search tree in a ﬂat open environment for w=2. In blue: the solution path
evaluate multiple primitives at every state xresults in a slow down. Furthermore, this
approach is strictly limited to quasi-static paths, where after every ﬂop the platform
has to settle back to a stable conﬁguration. These are low quality solutions that make
simplistic assumptions and do not leverage the platform’s dynamics.
4.2 Sampling-Based Kinodynamic Planning
An alternative approach is to use sampling-based kinodynamic planners, such as
Expansive Space Trees [ 11]o rRRT [16]. These methods select states xin so as to
promote the quick exploration of the underlying space. In the context of RRT and for
the accompanying implementation, this is achieved by sampling a random ¯qrand and
ﬁnding the state xnear in the search tree that has the closest ¯q(xnear)to¯qrand. Then, a
single random control is propagated beginning at xnear. Strictly random controls have
been shown to be effective for kinodynamic planning [ 15], instead of controls that
aim to extend toward the random samples, which is desirable in kinematic challenges.
Sampling Generic Controls : Sampling random controls, instead of consider-
ing only the predesigned primitives, allows more comprehensive exploration of the
kinodynamic behaviors of SUPERball. Nevertheless, the control sampling process
must be carefully considered. A naive sampling scheme is to independently select
each i’th cable length within the permissible range ui∈/parenleftbig
u−,u+/parenrightbig
using a uniform
probability distribution. This method, however, frequently results in impermissiblecable values during execution. The most common cause of this is violation of the
triangle inequality for three-element loops consisting of one bar and two cables, i.e.,
when the sum of the cable lengths is less than the bar length. In practice, issuing such
958 Z. Littleﬁeld et al.
Fig. 8 V ariation of the lengths of several cables in response to control inputs. Every two seconds,
an e wv a l u eo f uis sampled (black). Actual cable values (red) attempt to track the commands, but
often differ signiﬁcantly due to the complex coupling of the structure
Fig. 9 Cable length variation under a sampling scheme that is biased toward extreme values
commands actually causes violation of the upper bound on length for other cables
in the platform.
A straightforward ﬁx is to scale up the lengths of each such cable pair (e.g., cables
2 and 17 in Fig. 4) until their sum is greater than the bar length b. While this greatly
reduces the incidence of invalid lengths, performance in terms of effecting motion
remains poor. As is suggested by the cable length time series of Fig. 8, uniform
sampling returns many middle-ground values for cable lengths. Only rarely are the
cumulative effects sufﬁcient to cause useful motion. Experimental trials indicated
that less than 5% of these commands resulted in face transitions.
Thus, a different scheme is needed for producing effective inputs more frequently.
This is achieved by pairing cables that link opposite ends of a bar and jointly sampling
their values from the following set:
/braceleftbig
ui,uj/bracerightbig
∈/braceleftbig/braceleftbig
u+,u+−b/bracerightbig
,/braceleftbig
u+−b,u+/bracerightbig
,/braceleftbig
u+,u+/bracerightbig/bracerightbig
(5)
The triangle inequality is thus satisﬁed for every bar-cable-cable loop, while oth-
erwise maximizing the deviation of each uifrom u∗in one direction or another.
Figure 9shows the much more sharply varying behavior that arises with this method.
It was found that about 30% of control inputs sampled in this way produce a transi-
tion to another face. Notably, as seen in the ﬁrst plot of Fig. 9, invalid cable lengths
remain an occurrence. While this cannot be completely avoided, the validity rate of
85% was found to be sufﬁcient for effective planning.
Table 1summarizes the beneﬁts of the proposed sampling. More than 90% of
propagations drawn violate the safe bounds of the platform under naïve uniform
sampling. Respecting fundamental constraints ensured that nearly all samples pro-
duced safe motion, but few of these resulted in ﬂops. An increase in the rate ofuseful motion generation is achieved by biasing the cable values to their extremes.
From Quasi-static to Kinodynamic Planning … 959
Table 1 Rates of motion production and validity for control sampling schemes (1000 samples)
V alid ﬂop (%) V alid no-ﬂop (%) Invalid (%)
Uniform, unconstrained 0.6 6.0 93.4
Uniform, constrained 0.3 95.0 4.7
Extreme, constrained 30.2 55.4 14.4
Experiments show that a method like RRT equipped with the above sampling process
can discover solutions for more complex challenges relative to A∗applied over the
motion primitives. This requires signiﬁcant computational effort, however, and theresulting path quality can remain low even when allowed additional runtime.
4.3 Anytime Kinodynamic Planning Informed by Kinematic
Gaits
The original sampling-based kinodynamic planners aimed to quickly ﬁnd a feasible
solution and did not provide “anytime” properties, i.e., the capability of improving
solution quality over time. Furthermore, the basic versions of these methods did notutilize heuristics in order to guide the search process. The authors’ previous work
has focused on anytime sampling-based kinodynamic planners, like SST [18] and
its informed variant iSST [19]. The current work adopts iSST and adapts it so
as to utilize: (a) the effective sampling scheme for spherical tensegrity locomotion
provided in the previous section and (b) the kinematic motion primitives, which can
provide effective guidance for ﬁnding a quick, low-quality solution through the easyparts of an environment, which can be improved with additional computation.
Algorithm 1 provides an outline of the proposed process. The method receives as
input the initial system state x
◦, the goal condition G, the motion primitives U,t h e
heuristic function h, the total number of iterations Nand a pruning radius δw.T h e
method maintains a search tree T, which upon initialization includes only the initial
state x◦. It aims to construct a solution path π, which is initiated to be empty.
At every iteration, the algorithm selects a state xselected from the tree for prop-
agation (lines 4–8). If the latest state added in the tree xnewhas a better heuristic
cost than its parent, then this state is immediately selected. This is a greedy step of
the algorithm that aims to exploit heuristic information as much as possible. If no
new state was generated in the last iteration or no improvement in the heuristic costwas achieved, the selection process is close to that of an informed search algorithm.
In particular, it prioritizes states according to an evaluation function f=g+w·h,
similar to that of a weighted A
∗, and selects the best among them according to a
priority queue. In traditional informed search, however, once a state is selected, it
is removed from the queue and is never re-examined as all available (discrete and
ﬁnite) controls will be propagated from it. Given the continuous nature of the under-
960 Z. Littleﬁeld et al.
Algorithm 1: Informed\_Kinodynamic\_Planning( x◦,G,U,h(·),N,δw)
1T←{x◦},π←∅ ;
2xnew←x◦;
3forN iterations do
4 if x new!=∅ and h(xnew) 0,N(v,v0)/negationslash=∅ and Cd(v)/negationslash=∅. We can
compute CNv(c)when c∈Cd(v)using Eq. ( 3a) and also compute CNˆvc(ˆc/prime)where
h(ˆvc)=h(v)−1u s i n gE q . ( 3b). In this iteration, CNv(c)where c/∈Cd(v)is not
computed which will be computed when visiting its parent vertex. After this iter-ation, move to the modules with higher height and the algorithm ends until the
predeﬁned root v
0is visited. It is clear that the root module can be found in time
O(|V|).
Algorithm 2: Root Module Search
Input : Graph representation G=(V,E)
Output : Root module τ
1Root G=(V,E)with respect to a module v0with height of Hand the height
ofv∈Vish(v);
2Initialize CNv(c)forv∈Vandc∈Cto be zero;
3Initialize h←1;
4while h ≤Hdo
5 forEach module v with h (v)=hd o
6 forEach module ˆvc∈N(v,ˆτ)do
7 CNv(c)=/summationtext
ˆc∈C−ˆc/primeCNˆvc(ˆc),c∈Cd(v);
8 CNˆvc(ˆc/prime)=n−1−/summationtext
ˆc∈Cd(ˆv)CNˆvc(ˆc);
9 h←h+1;
10return τsuch that CNτ(c)≤1
2n∀c∈C
5.3 Matching and Mapping
The conﬁguration graph G=(V,E)can be rooted with respected to the root module
deﬁned from Algorithm 2. Then given two conﬁgurations, we want to verify if they
are isomorphic in terms of modular robotic system topology and, if true, map each
module from one conﬁguration to that in another conﬁguration. Intuitively, this can be
solved by traverse the trees from the root to the leaves and check all the edges and the
corresponding CNv(c)values and, if the connections are equivalent and CNv(c)are
equal, then these two connections share the same topology. However, since modular
robots are usually symmetric in their geometry, for example, for SMORES system,
the left side is a mirror image of the right side for each module, there will be multiplecandidates for some connection when trying to ﬁnd the equivalent one in the other
conﬁguration. It is a heavy computation burden to track all the options and the number
of cases to track can quickly grow.
If two modular robot conﬁgurations G
1=(V1,E1)andG2=(V2,E2)are iso-
morphic, then each module v1∈V1must be able to be mapped to a unique module
Conﬁguration Recognition with Distributed Information for Modular Robots 977
(a) (b)
Fig. 5 SMORES Conﬁgurations. The subgraphs of aand bcircled by “ --” is an example of
common subconﬁguration with mapping 1 →1a n d0 →2. The subgraphs of aandbcircled by
“—”i sM C S (1,1)with mapping 1 →1, 2→0a n d0 →2
v2∈V2who shares the same topology and the number of mapped pairs of modules
are the number of modules in G1orG2. Hence, if there exists this bijective mapping
f:V1→V2, then G1andG2are isomorphic and fis the mapping we are looking
for. For each module v1∈V1, there may be multiple modules in G2that share the
same topology with v1inG1. The idea of our algorithm is to try to ﬁnd some possible
mappings which result in multiple common subgraphs of both conﬁgurations, and, if
these two conﬁgurations are isomorphic, then there must be one common subgraph
containing all the modules that is also the isomorphism mapping.
Given two modular robot conﬁgurations G1=(V1,E1)andG2=(V2,E2), there
may be some common parts between them. Two deﬁnitions are introduced:
Deﬁnition 3 Given two modular robot conﬁgurations G1=(V1,E1)and G2=
(V2,E2),acommon subconﬁguration is a set of connected graphs {G/prime
1,G/prime
2}where
G/prime
1=(V/prime
1,E/prime
1)⊆G1,G/prime
2=(V/prime
2,E/prime
2)⊆G2such that G/prime
1andG/prime
2are isomorphic.
The corresponding bijective common subconﬁguration mapping is deﬁned as
f/prime:V/prime
1→V/prime
2.
Deﬁnition 4 Given the condition that module v1∈V1ofG1=(V1,E1)must be
mapped to module v2∈V2ofG2=(V2,E2), the common subconﬁguration with
maximum common connections is called maximum common subconﬁguration
with respect to v1andv2denoted as MCS (v1,v2)with mapping f:ˆV1→ˆV2where
ˆV1⊆V1andˆV2⊆V2. If a module pair (v/prime
1,v/prime
2)satisﬁes v/prime
1∈ˆV1,v/prime
2∈ˆV2andf(v/prime
1)=
v/prime
2, then(v/prime
1,v/prime
2)∈MCS(v1,v2)under f:ˆV1→ˆV2.
Given two graphs G1and G2rooted with respect to τ1∈V1andτ2∈V2
respectively, for module v1∈V1andv2∈V2, we can construct a common sub-
conﬁguration {G/prime
1,G/prime
2}where V/prime
1={v1,ˆvc1
1},V/prime
2={v2,ˆvc2
2}under a subconﬁgura-
tion mapping f/prime:V/prime
1→V/prime
2such that f/prime(v1)=v2and f/prime(ˆvc1
1)=ˆvc2
2if and only if
connect (v1,ˆvc1
1)∼=connect (v2,ˆvc2
2)which is called feasibility rule . We can make
a stronger feasibility rule by adding CNv1(c1)=CNv2(c2)then the rule becomes a
sufﬁcient condition which is expressed as a form of a function F((v1,c1),(v2,c2)).
c1is not necessarily to be equal to c2because of the symmetry property of modules
(Fig. 5).
978 C. Liu and M. Yim
Theorem 1 Given MCS(v1,v2)under mapping f :V1→V2, for any module pair
(v/prime
1,v/prime
2)∈MCS(v1,v2)under f :V1→V2,MCS(v/prime
1,v/prime
2)is equal to MCS(v1,v2).
According to Theorem 1and previous analysis, if F((v1,c1),(v2,c2))is true,
then(ˆvc1
1,ˆvc2
2)∈MCS(v1,v2)under f:V1→V2and MCS (ˆvc1
1,ˆvc2
2)is equal to
MCS(v1,v2). Hence, whether computing MCS (v1,v2)is equivalent to computing
MCS(ˆvc1
1,ˆvc2
2)depends only on F(ˆvc1
1,ˆvc2
2). If two modular robot conﬁgurations
G1=(V1,E1)andG2=(V2,E2)are isomorphic, τ1andτ2are the only root mod-
ule of G1andG2respectively, then a common subconﬁguration {G1,G2}under
mapping f:V1→V2must exist and this common subconﬁguration is actually
MCS(τ1,τ2). There may be multiple options of f:V1→V2because of the symme-
try property of the modules. The problem to compute MCS (τ1,τ2)can be converted
into the problem to compute MCS (ˆτc1
1,ˆτc2
2)just by checking F((τ 1,c1),(τ 2,c2)).
This gives us the recursive solution to compute MCS (τ1,τ2)and the subproblem
is to solve MCS (ˆτc1
1,ˆτc2
2)where c1∈Cd(τ1)andc2∈Cd(τ2). Whichever of these
MCSs is equivalent to MCS (τ1,τ2)is the solution and the corresponding mapping
f:V1→V2is generated accordingly. For some conﬁgurations that may have a pair
of root modules while each conﬁguration is only rooted with respect to one root inthe library, we just need to root this conﬁguration with respect to both of the root
modules and compare both of them with that in the library. With this, we bring up a
bottom-up algorithm to check the isomorphism and map modules simultaneously.
The algorithm takes two rooted conﬁguration graphs as input. Some preliminary
checks can be added here, like the height of trees, the number of modules in each
level and so on. If they fail these tests, they cannot be isomorphic because there can
be two root modules at most and, if two graphs are isomorphic, then they should
have the same root modules. We start this bottom-up algorithm from the modulesthat are leaves of two given conﬁgurations and move generally to their roots. During
the process, all possible MCS which may be equivalent to MCS (τ
1,τ2)are computed
and the corresponding mapping is generated. In the end, the one that contains all themodules is the solution and the common subconﬁguration mapping tells us how to
map each module. The pseudocode is presented in Algorithm 3. For any vertex v∈V
with its depth d(v)> 0, we denote its parent connected via its connector cas˜v
cand
the mating connector of ˜vcas˜c/prime.
Line 8—9 : It is possible that (v1,v2)or(˜vc1
1,˜vc2
2)has been added to some MCSs
with respect to some pair of modules. MCS Pis the set of all MCSs containing
(˜vc1
1,˜vc2
2)with corresponding mapping fp. Similarly MCS Cis the set of all MCSs
containing (v1,v2)with corresponding mapping fc.
Line 10—24 :I f F((˜vc1
1,˜c/prime
1),(˜vc2
2,˜c/prime
2))is true, then MCS (v1,v2)is equal to
MCS(˜vc1
1,˜vc2
2). When MCS Pis empty but MCS Cis not empty, then any
MCS(u1,u2)∈MCS Cis equal to MCS (v1,v2)from Theorem 1. Hence, as long
as˜vc1
1/∈U1∧˜vc2
2/∈U2, each MCS (u1,u2)is also equal to MCS (˜vc1
1,˜vc2
2)andu1
andu2are updated accordingly. Then MCS is also updated which is to remove
the old MCS (u1,u2)if existing and add the new MCS (u1,u2). Similar to the pre-
vious case, when MCS Pis not empty and MCS Cis empty, then, as long as
v1/∈P1∧v2/∈P2,M C S (p1,p2)is equal to MCS (v1,v2)and p1and p2are also
Conﬁguration Recognition with Distributed Information for Modular Robots 979
Algorithm 3: Matching and Mapping
Input : Graph representations G1=(V1,E1)with root τ1andG2=(V2,E2)with root τ2where
h(G1)=h(G2)=H
Output :M C S (τ1,τ2)with f:V/prime
1→V/prime
2
1Create a set MCS ={MCS(null,null)}with a trivial mapping f:∅→∅ ;
2Initialize h←H;
3while h >0do
4 forv1∈V1with d (v1)=hd o
5 MCS/prime←MCS ;
6 forv2∈V2with d (v2)=hd o
7 ifF((˜vc1
1,˜c/prime
1),(˜vc2
2,˜c/prime
2))then
8 MCS P={MCS(p1,p2)∈MCS/primefp:P1→P2|(˜vc1
1,˜vc2
2)∈MCS(p1,p2)};
9 MCS C={MCS(u1,u2)∈MCS/primefc:U1→U2|(v1,v2)∈MCS(u1,u2)};
10 ifMCS P=∅∧ MCS C/negationslash=∅ then
11 forMCS(u1,u2)∈MCS Cdo
12 if˜vc1
1/∈U1∧˜vc2
2/∈U2then
13 Add(˜vc1
1,˜vc2
2)to MCS (u1,u2)such that fc(˜vc1
1)=˜vc2
2;
14 u1←˜vc1
1andu2←˜vc2
2;
15 Update MCS ;
16 else if MCS P/negationslash=∅∧ MCS C=∅ then
17 forMCS(p1,p2)∈MCS Pdo
18 ifv1/∈P1∧v2/∈P2then
19 Add(v1,v2)to MCS (p1,p2)such that fp(v1)=v2;
20 p1←v1andp2←v2;
21 Update MCS ;
22 else if (v1∈P1∧v2/∈P2)∨(v1/∈P1∧v2∈P2)then
23 Construct MCS (v1,v2)with f:{v1,˜vc1
1}→{ v2,˜vc2
2}such that f(v1)=v2and
f(˜vc1
1)=˜vc2
2;
24 Add MCS (v1,v2)toMCS ;
25 else if MCS P=∅∧ MCS C=∅ then
26 Construct MCS (v1,v2)with f:{v1,˜vc1
1}→{ v2,˜vc2
2}such that f(v1)=v2and f(˜vc1
1)=˜vc2
2;
27 Add MCS (v1,v2)toMCS ;
28 else
29 MCS/prime
P={MCS(p1,p2)∈MCS P|v1/∈P1∧v2/∈P2};
30 MCS/prime
C={MCS(u1,u2)∈MCS C|˜vc1
1/∈U1∧˜vc2
2/∈U2};
31 ifMCS/prime
P=∅∧ MCS/prime
C/negationslash=∅ then
32 forMCS(u1,u2)∈MCS/prime
Cdo
33 Add(˜vc1
1,˜vc2
2)to MCS (u1,u2)such that fc(˜vc1
1)=˜vc2
2;
34 u1←˜vc1
1andu2←˜vc2
2;
35 Update MCS ;
36 else if MCS/prime
P/negationslash=∅∧ MCS/prime
C=∅ then
37 forMCS(p1,p2)∈MCS/prime
Pdo
38 Add(v1,v2)to MCS (p1,p2)such that fp(v1)=v2;
39 p1←v1andp2←v2;
40 Update MCS ;
41 else if MCS/prime
P/negationslash=∅∧ MCS/prime
C/negationslash=∅ then
42 MCS(p1,p2)←MCS(u1,u2)∪MCS(p1,p2),∀MCS(p1,p2)∈MCS/prime
Pand
∀MCS(u1,u2)∈MCS/prime
C;
43 Update MCS ;
44 h←h−1;
45return MCS(τ1,τ2)with f:V/prime
1→V/prime
2
980 C. Liu and M. Yim
updated. In addition, it is possible that v1∈P1∧v2/∈P2orv1/∈P1∧v2∈P2, then
there is no MCS ∈MCS equal to MCS (v1,v2)or MCS (˜vc1
1,˜vc2
2).S oM C S (v1,v2)
or MCS (˜vc1
1,˜vc2
2)is a new possible MCS which may be equal to MCS (τ1,τ2). Thus,
we add MCS (v1,v2)with mapping f:{v1,˜vc1
1}→{ v2,˜vc2
2}toMCS .
Line 25—27 : If both MCS Pand MCS Care empty, then MCS (v1,v2)or
MCS(˜vc1
1,˜vc2
2)is a new possible MCS which may be equal to MCS (τ1,τ2). Thus, we
add MCS (v1,v2)with mapping f:{v1,˜vc1
1}→{ v2,˜vc2
2}toMCS .
Line 28—43 : This is the last case when F((˜vc1
1,˜c/prime
1),(˜vc2
2,˜c/prime
2))is true that both
MCS Pand MCS Care not empty. MCS (u1,u2)∈MCS Ccannot be equal to
any MCS (p1,p2)∈MCS Pfor the reason that the algorithm starts from leaves
of two given trees and multiple modules may share a parent but multiple modules
cannot have the same children. In Line 29 andLine 30 the algorithm checks if (v1,v2)
can be added to any MCS (p1,p2)or(˜vc1
1,˜vc2
2)can be added to any MCS (u1,u2).
MCS is updated according to different conditions and the special one is that when
both MCS/prime
Pand MCS/prime
Care not empty, we have to merge MCS (u1,u2)and
MCS(p1,p2)with all possible combinations.
Line 44 : Update hand, in the next iteration, the one-level higher modules will be
compared to update MCS . The last group of modules the algorithm compares are
those with depth equal to one.
Line 45 : In the end, there will be multiple MCS (τ1,τ2)∈MCS and multiple
MCS(q1,q2)∈MCS such that (τ1,τ2)∈MCS(q1,q2). Among all these candi-
dates, the one covering maximum number of modules is the solution of MCS (τ1,τ2)
with corresponding mapping f:V/prime
1→V/prime
2. If the roots of both two given conﬁgura-
tions are unique, then they are isomorphic if V/prime
1=V1(obviously V/prime
2=V2) and not
isomorphic if V/prime
1/negationslash=V1(namely V/prime
2/negationslash=V2).
This algorithm can be improved to be more efﬁcient for matching and map-
ping task. When checking v1∈V1with d(v1)=h,i f∀v2∈V2with d(v2)=h,
F((˜vc1
1,˜c/prime
1),(˜vc2
2,˜c/prime
2))is not true, then these two conﬁgurations cannot be isomor-
phic and the algorithm can stop or continue with next candidate. This algorithm cansolve the matching and mapping problem in time O(|E
1|2)orO(|V1|2)for the worst
case. In reality, the number of connectors a module has is usually small, so the time
for a large number of modules should be much smaller than the worst case.
6 Experiments
The integral algorithm is implemented in Python. The SMORES conﬁguration usedin this experiment is a walker with a manipulator shown in Fig. 6. The corresponding
graph representations of them are shown in Fig. 7. This is a special conﬁguration that
has a lot of symmetric parts. For example, the left legs and right legs are symmetric
and the front legs and the back legs are also symmetric.
We ﬁrst run Algorithm 1 to discover all the connections in this conﬁguration
shown in Fig. 6b. The sequence to discover all the connections is connect (0,1)→
connect (0,4)→connect (0,12)→connect (1,11)→connect (1,3)→connect (4,8)
Conﬁguration Recognition with Distributed Information for Modular Robots 981
→connect (12,5)→connect (11,6)→connect (3,7)→connect (3,2)→connect
(6,13)→connect (2,9)→connect (7,10).
Then the graph G=(V,E)can be built based on these connections. With G=
(V,E), the root and all CNv(c)forv∈Vandc∈Ccan be computed by Algorithm 2
and the root module is Module 1. Some preliminary tests can be implemented to
ﬁlter most of the conﬁgurations in the library and the one shown in Fig. 6ai st h e
closest one. The graph representation ˆG=(ˆV,ˆE)(Fig. 7a) rooted with respect to
the corresponding root module (Module 0), all connection information and all CN
values are stored in the library. Then run Algorithm 3 to match this new conﬁguration
G=(V,E)toˆG=(ˆV,ˆE)and map each module v∈Vtoˆv∈ˆV. Since there are
many symmetric parts, the mapping between GandˆGis not unique. By our algorithm,
we can get all isomorphic mappings f:V→ˆV. For this example, there are eight
different mappings in total and one of them is {10→5,7→4,3→1,9→7,2→
6,1→0,8→9,4→8,0→2,5→11,12→10,13→13,6→12,11→3}.
Fig. 6 Walker conﬁguration
with different labels. ais the
conﬁguration in our library,
andbis the new
conﬁguration to recognize
Fig. 7 Walker conﬁguration
graphs. ais conﬁguration in
our library, and bis the new
design(a) (b)
982 C. Liu and M. Yim
7 Conclusion
We develop and implement an efﬁcient algorithm for modular robot systems to
do conﬁguration recognition automatically. A new conﬁguration can be discoveredby use of local communication among modules and a graph representation can be
generated. Then its root module(s) and all CN values are computed using dynamic
programming. Each conﬁguration in the library contains its rooted graph with respectto its root module, all connections and all CN values. Matching and mapping problem
is solved by searching MCS and, if this new conﬁguration is isomorphic to some
conﬁguration in the library, all the mapping results will be computed. The algorithmcan be adapted to other modular systems easily as long as local communication
among modules is supported and equivalent connections can be deﬁned.
The future work will focus on the optimal mapping for modular robots since mul-
tiple mappings may exist. In addition, this algorithm should be helpful for reconﬁg-
uration planning to ﬁnd the maximum overlapping between the initial conﬁgurationand the goal conﬁguration. This will also be part of our future work.
Acknowledgements The authors would like to acknowledge the support of NSF grant CCF-
1138847.
References
1. Yim, M., Shen, W.M., Salemi, B., Rus, D., Moll, M., Lipson, H., Klavins, E., Chirikjian, G.:
Modular self-reconﬁgurable robot systems: grand challenges of robotics. IEEE Robot. Autom.Mag. 14(1), 43–52 (2007)
2. Stoy, K., Brandt, D., Christensen, D.J.: Self-Reconﬁgurable Robots. The MIT Press, Cam-
bridge, MA, (2010)
3. Park, M., Chitta, S., Teichman, A., Yim, M.: Automatic conﬁguration recognition methods in
modular robots. Int. J. Robot. Res. 27(3–4), 403–421 (2008)
4. Chen, I.-M., Burdick, J.W.: Enumerating the nonisomorphic assembly conﬁgurations of modu-
lar robotic systems. In: Proceedings of 1993 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS’93), vol. 3, pp. 1985–1992. Y okohama, Japan (1993)
5. Shiu, M.-C., Fu, L.-C., Chia, Y .-J.: Graph isomorphism testing method in a self-recognition
velcro strap modular robot. In: Proceedings of the 2010 5th IEEE Conference on IndustrialElectronics and Applications, pp. 222–227. Taichung, Taiwan (2010)
6. Zhu, Y ., Li, G., Wang, X., Cui, X..: Automatic function-isomorphic conﬁguration recognition
and control for UBot modular self-reconﬁgurable robot. In: Proceedings of the 2012 IEEEInternational Conference on Mechatronics and Automation, pp. 451–456. Chengdu, China(2012)
7. Butler, Z., Fitch, R., Rus, D., Wang, Y .: Distributed goal recognition algorithms for modular
robots. In: Proceedings of the IEEE International Conference on Robotics and Automation(Cat. No.02CH37292), vol. 1, pp. 110–116. Washington DC (2002)
8. Castano, A., Will, P .: Representing and discovering the conﬁguration of conro robots. In:
Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat.
No.01CH37164), vol. 4, pp. 3503–3509. Seoul, South Korea (2001)
9. Baca, J., Woosley, B., Dasgupta, P ., Nelson, C.A.: Conﬁguration discovery of modular self-
reconﬁgurable robots: real-time, distributed, IR +XBee communication method. Robot. Auton.
Syst. 91, 284–298 (2017)
Conﬁguration Recognition with Distributed Information for Modular Robots 983
10. Tosun, T., Davey, J., Liu, C., Yim, M.: Design and characterization of the EP-face connector.
In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), pp. 45–51. Daejeon, South Korea (2016)
11. McColm, G.L.: On the structure of random unlabelled acyclic graphs. Discret. Math. 277(1),
147–170 (2004)
12. Hou, F.: Self-reconﬁguration planning for modular robots. University of Southern California,
Los Angeles (2011)
V ariational Contact-Implicit Trajectory
Optimization
Zachary Manchester and Scott Kuindersma
1 Introduction
Trajectory optimization algorithms comprise a powerful collection of methods for
planning motions of nonlinear dynamical systems [ 3]. Generally speaking, these
algorithms aim to ﬁnd an input trajectory that minimizes a cost function subject to a
set of constraints on the system’s states and inputs. Trajectory optimization has a long
history of successful application to systems with smooth dynamics. However, manyrobotic systems experience discontinuous frictional contact with the environment as
an essential part of their routine operation. The non-smooth dynamics encountered
in these situations pose signiﬁcant challenges.
A popular approach for handling contact events is to use a hybrid system model
that explicitly accounts for discontinuities [ 20]. However, contact mode sequences
must then be pre-speciﬁed by the user or generated by a higher-level heuristic planner.
This approach can work quite well for systems with a small number of contacts [ 6,
12,23,24,28]. Unfortunately, for more complex systems, the number of modes
grows exponentially with the number of contact constraints, making mode sequence
pre-speciﬁcation computationally impractical.
Recently, an alternative approach has emerged in which state, input, and con-
tact force trajectories are simultaneously optimized [ 13,21,26]. These so-called
contact-implicit trajectory optimization methods can synthesize motions without a
priori speciﬁcation of the contact mode sequence. However, current state-of-the-artalgorithms rely on ﬁrst-order discretizations of the dynamics constraints, severely
limiting accuracy and closed-loop trajectory tracking performance [ 20,28].
To overcome the accuracy limitations of current algorithms, we propose a new
family of variational contact-implicit methods that combine ideas from discrete
mechanics with the complementarity formulation of rigid body contact to achieve
Z. Manchester ( B)
Harvard University, 60 Oxford St., Cambridge, MA 02138, USAe-mail: zacmanchester@stanford.edu
S. Kuindersma
Harvard University, 33 Oxford St., Cambridge, MA 02138, USAe-mail: scottk@seas.harvard.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_66985
986 Z. Manchester and S. Kuindersma
higher-order integration accuracy. For simplicity and concreteness, we provide an
explicit derivation of a second-order method. However, the mathematical tools used
are general, and can be applied to derive integrators of arbitrary order.
The remainder of the paper is organized as follows: Section 2provides a summary
of related work on trajectory optimization through contact. Section 3then gives a brief
review of some important concepts from both classical variational mechanics anddiscrete mechanics. Section 4derives the new variational rigid body time-stepping
scheme, and Sect. 5introduces a direct trajectory optimization algorithm built around
these dynamics. Several examples that demonstrate the performance of the newalgorithm are then presented in Sect. 6. Finally, we summarize our ﬁndings in Sect. 7.
2 Related Work
In spite of their limitations, hybrid trajectory optimization algorithms that rely on
a pre-speciﬁed contact mode sequence have had a number of notable successes.
For example, hybrid multiple-shooting algorithms have been used to ﬁnd open-loop
stable walking trajectories for a two-dimensional model of a humanoid [ 12] and to
study the energetics of quadrupedal locomotion [ 23]. Hybrid collocation methods
with third-order integration accuracy have also been demonstrated on a full-body
model of a humanoid [ 20].
Much of the recent work on contact-implicit methods has focused on approx-
imation schemes to smooth discontinuities. Several authors have developed indi-
rect trajectory optimization algorithms based on differential dynamic programming
(DDP) [ 11] that apply a smoothing function to the contact constraints [ 26], model
contact forces with nonlinear springs and dampers [ 16], or penalize constraint viola-
tions in the objective function [ 15,27]. The penalty approach has also been applied
in direct trajectory optimization methods [ 13]. These algorithms have been used
to plan motions for quadrupeds [ 15,16] and simpliﬁed humanoids [ 13,26,27]i n
simulation.
The previous work most closely related to the present paper is that of Posa, Cantu,
and Tedrake [ 21]. Their algorithm attempts to accurately capture discontinuous rigid-
body physics by relying on the “time-stepping” linear complementarity formulation
of Stewart and Trinkle [ 2,25]. The essential idea behind time-stepping methods is
to apply a ﬁrst-order semi-implicit Euler discretization to the dynamics,
M(qk)(vk+1−vk)=h/parenleftbig
B(qk)uk+N(qk)Tγk−C(qk,vk+1)/parenrightbig
qk+1=qk+hv k+1, (1)
where kis a time index; q,v,u, andγare conﬁgurations, velocities, control inputs,
and normal contact impulses acting over a timestep of length h, respectively; M
is the system’s mass matrix; Band NTare the Jacobians mapping control inputs
and normal contact forces into generalized coordinates; and Cincludes Coriolis and
V ariational Contact-Implicit Trajectory Optimization 987
potential terms. We have temporarily ignored the tangental (friction) component of
the contact force for clarity, but it is discussed extensively in Sect. 4.2. For the normal
component, we have the following constraints:
γk≥0
φ(qk+1)≥0
γkφ(qk+1)=0, (2)
where φ(q)is a function that returns the signed distance between closest points on
bodies. In words, the conditions in Eq. ( 2) specify that normal forces can only push
bodies apart (not pull them together), that bodies cannot interpenetrate, and thatcontact forces can only be non-zero when bodies are in contact. The combination
of (1) and ( 2) forms a linear complementarity problem (LCP) that can be solved
efﬁciently [ 2]. However, this formulation depends crucially on the particular choice
of ﬁrst-order discretization used in ( 1).
While it may be possible to apply a higher-order discretization scheme in an ad
hocway, it is not obvious how to do so while still satisfying the contact constraints. To
overcome this, the next few sections introduce a set of mathematical tools based on
discrete mechanics [ 10] for systematically deriving time-stepping methods with any
desired order of integration accuracy. Previous authors have used discrete mechan-ics, together with trajectory optimization, to plan motions for systems with smooth
dynamics — an idea known as Discrete Mechanics and Optimal Control (DMOC) [ 8].
Hybrid DMOC methods have also been used to optimize bipedal walking gaits withpre-speciﬁed contact mode sequences [ 19].
3 Preliminaries
This section reviews some classical results from variational mechanics, as well as
some more recent results from discrete mechanics.
3.1 Lagrange-D’Alembert Principle
Our starting point is the Lagrange-D’Alembert principle, which is the integral formof D’Alembert’s principle of virtual work [ 9], and can also be thought of as a modi-
ﬁcation of Hamilton’s principle of least action to accommodate external forces:
δ/integraldisplay
tf
t0L(q,˙q)dt+/integraldisplaytf
t0F·δqd t=0. (3)
988 Z. Manchester and S. Kuindersma
We use Lto denote the system’s Lagrangian, Fto denote a generalized force, and
δto indicate a variation. Equation ( 3) describes a boundary-value problem in which
a trajectory q(t)is sought given ﬁxed end points q(t0)and q(tf).
We now review the steps used to derive the classical forced Euler-Lagrange equa-
tion from ( 3). Carrying out the variational derivative results in,
/integraldisplaytf
t0D1L(q,˙q)·δq+D2L(q,˙q)·δ˙qd t+/integraldisplaytf
t0F·δqd t=0, (4)
w h e r ew eh a v eu s e dt h e slot derivative D ito indicate partial differentiation with
respect to a function’s ith argument. The next step is to eliminate δ˙qby performing
an integration by parts:
/integraldisplaytf
t0D1L(q,˙q)·δq−d
dtD2L(q,˙q)·δqd t+/integraldisplaytf
t0F·δqd t
+D2L(q(tf),˙q(tf))·δq(tf)−D2L(q(t0),˙q(t0))·δq(t0)=0.
(5)
The fact that the end points q(t0)and q(tf)of the boundary value problem are ﬁxed,
and thus δq(t0)=δq(tf)=0, can be used to eliminate the last two terms in ( 5):
/integraldisplaytf
t0D1L(q,˙q)·δq−d
dtD2L(q,˙q)·δqd t+/integraldisplaytf
t0F·δqd t=0. (6)
Finally, recognizing that Eq. ( 6) must hold for all variations δq, we arrive at the forced
Euler-Lagrange equation:
d
dtD2L(q,˙q)−D1L(q,˙q)=F. (7)
3.2 Discrete Mechanics
Discrete mechanics encompasses a set of mathematical tools for deriving specialized
numerical integrators for mechanical systems. These so-called variational integra-
tors have many advantages over traditional Runge–Kutta schemes, including realis-
tic long-term energy and momentum behavior [ 10] (although this is not our primary
focus).
The general strategy behind discrete mechanics is to approximate the integrals
in (3) with a quadrature rule before taking variations. We begin by breaking those
integrals into Nsmaller pieces,
V ariational Contact-Implicit Trajectory Optimization 989
δN−1/summationdisplay
k=0/integraldisplaytk+1
tkL(q,˙q)dt+N−1/summationdisplay
k=0/integraldisplaytk+1
tkF(q,˙q)·δqd t=0, (8)
where tk=t0+khand his a small timestep. Each short integral in Eq. ( 8) is then
approximated. For simplicity and concreteness, we will use the midpoint rule:
δN−1/summationdisplay
k=0hL/parenleftbiggqk+qk+1
2,qk+1−qk
h/parenrightbigg
+N−1/summationdisplay
k=0h
2F/parenleftbiggqk+qk+1
2,qk+1−qk
h/parenrightbigg
·(δqk+δqk+1)=0.
(9)
Equation ( 9) can be written more compactly as,
δN−1/summationdisplay
k=0Ld(qk,qk+1)+N−1/summationdisplay
k=01
2Fd(qk,qk+1)·(δqk+δqk+1)=0, (10)
where Ldis the discrete Lagrangian,
Ld(qk,qk+1)=hL/parenleftbiggqk+qk+1
2,qk+1−qk
h/parenrightbigg
, (11)
and Fdis the discrete generalized force,
Fd(qk,qk+1)=hF/parenleftbiggqk+qk+1
2,qk+1−qk
h/parenrightbigg
. (12)
Note that the deﬁnitions of the discrete Lagrangian and discrete generalized force
depend on our choice of quadrature rule [ 10].
We now carry out the variational derivative in ( 10):
N−1/summationdisplay
k=0D1Ld(qk,qk+1)·δqk+D2Ld(qk,qk+1)·δqk+1
+N−1/summationdisplay
k=01
2Fd(qk,qk+1)·(δqk+δqk+1)=0.(13)
Paralleling the derivation of the classical Euler–Lagrange equation in the previous
section, we perform a “discrete integration by parts” to line up the time indices of
theδqterms. This amounts to a simple index manipulation trick:
990 Z. Manchester and S. Kuindersma
D1Ld(q0,q1)δq0+1
2Fd(q0,q1)·δq0
+N−1/summationdisplay
k=1/parenleftbigg
D2Ld(qk−1,qk)+D1Ld(qk,qk+1)+1
2Fd(qk−1,qk)+1
2Fd(qk,qk+1)/parenrightbigg
·δqk
+D2Ld(qN−1,qN)δqN+1
2Fd(qN−1,qN)·δqN=0.(14)
As in the continuous case, the endpoints q0and qNare ﬁxed. As a result, δq0=
δqN=0, and the ﬁrst and last terms in ( 14) can be eliminated:
N−1/summationdisplay
k=1/parenleftbigg
D2Ld(qk−1,qk)+D1Ld(qk,qk+1)
+1
2Fd(qk−1,qk)+1
2Fd(qk,qk+1)/parenrightbigg
·δqk=0. (15)
Finally, using the fact that Eq. ( 15) must hold for all variations δqk,w ea r r i v ea tt h e
following discrete-time version of the forced Euler-Lagrange equation:
D2Ld(qk−1,qk)+D1Ld(qk,qk+1)+1
2Fd(qk−1,qk)+1
2Fd(qk,qk+1)=0.(16)
Equation ( 16) can be used to simulate the dynamics of a mechanical system by
inserting values for qk−1and qkand solving for qk+1. In fact, it is equivalent to the
implicit midpoint method. In general, the order of accuracy of a variational integrator
is equal to the order of accuracy of the quadrature rule used in its construction [ 10].
Since the midpoint rule has a global error of O(h2), an integrator based on ( 16)
inherits this second-order accuracy. V ariational integrators of any desired order can
be derived by simply choosing an appropriate quadrature rule [ 18].
4 V ariational Time-Stepping Methods
Time-stepping methods for simulating rigid body dynamics with contact were ﬁrst
proposed by Stewart and Trinkle [ 25]. The essential idea is to deal with the disconti-
nuities that occur during rigid body impacts by formulating the dynamics at the levelof impulses and velocities, rather than forces and accelerations. The contact impulse
produced during a timestep is computed, together with the next state, by solving a
constrained optimization problem.
Since variational integrators like ( 16) are also formulated in terms of impulses
and avoid direct computation of forces and accelerations, they are a natural choice
for handling rigid body contact dynamics. In this section, we derive a time-steppingmethod with second-order integration accuracy using ideas from discrete mechanics.
We treat only the case of perfectly inelastic collisions, however extension to the elastic
case is possible along the same lines used in existing time-stepping schemes [ 2].
V ariational Contact-Implicit Trajectory Optimization 991
4.1 Interpenetration and Complementarity
Interpenetration must not occur between rigid bodies. Mathematically, this constraint
can be expressed as an inequality,
φ(q)≥0, (17)
where φ(q)is a vector-valued function that evaluates the signed distance between
closest points on all pairs of bodies.
To build a variational integrator that respects the interpenetration constraint, we
add it to ( 10) with a corresponding Lagrange multiplier, γ[10]:
δN−1/summationdisplay
k=0Ld(qk,qk+1)+γT
kφ(qk+1)+N−1/summationdisplay
k=01
2Fd(qk,qk+1)·(δqk+δqk+1)=0.(18)
Note the choice of time indices in the constraint term γT
kφ(qk+1)to indicate that a
contact impulse during timestep kmust be generated such that the next state satisﬁes
the inequality φ(qk+1)≥0.
Following the same steps used to derive equation ( 16) in the previous section, we
ﬁnd,
D2Ld(qk−1,qk)+D1Ld(qk,qk+1)
+1
2Fd(qk−1,qk)+1
2Fd(qk,qk+1)+N(qk+1)Tγk=0,(19)
where N(q)T=(∂φ/∂ q)Tis the Jacobian mapping normal contact forces into gen-
eralized coordinates. In addition, solutions to ( 19) must satisfy the following condi-
tions:
γk≥0
φ(qk+1)≥0
γT
kφ(qk+1)=0.(20)
The combination of ( 19) and ( 20) form the ﬁrst-order necessary conditions, known as
Karush–Kuhn–Tucker (KKT) conditions, for an inequality constrained optimization
problem [ 4].
Physically, the Lagrange multiplier γktakes on the magnitude of the contact
impulse in the normal direction. The three conditions in ( 20) are collectively known
as a complementarity constraint . In addition to preventing interpenetration, they
ensure that contact forces can only push bodies apart (not pull them together), andthat contact forces can only act when bodies are in contact. Such constraints are
commonly denoted using the following shorthand notation:
0≤γ
k⊥φ(qk+1)≥0. (21)
992 Z. Manchester and S. Kuindersma
Intuitively, complementarity constraints express discontinuous “switching” behav-
ior: only one variable or the other is allowed to be non-zero at a time. They are an
inherent feature in many models of contact physics.
4.2 Coulomb Friction
Coulomb friction exerts forces in the plane tangent to the contact surface betweentwo bodies. It can be described by the Maximum Dissipation Principle [14], which
states that friction forces instantaneously maximize the dissipation of kinetic energy.
Mathematically, this can be written as,
minimize
b˙qTDTb
subject to /bardblb/bardbl≤μγ,(22)
where bis the friction force, μis the friction coefﬁcient, DTis the Jacobian mapping
tangential contact forces into generalized forces, and the constraint ensures that forces
remain within the Coulomb friction cone.
For computational reasons, it is standard in time-stepping methods to form an
inner approximation to the friction cone using a polyhedron [ 2,25]. For the special
case of a pyramidal approximation, this is accomplished by deﬁning a new frictionvector βwith twice as many elements as b, enforcing the constraint β≥0, and
forming a new Jacobian matrix:
P=/bracketleftbiggD
−D/bracketrightbigg
. (23)
With this approximation, the optimization problem ( 22) becomes,
minimize
β˙qTPTβ
subject to μγ−eTβ≥0
β≥0(24)
where eis a vector of ones.
The set of ﬁrst-order necessary conditions (KKT conditions) for an optimum of
(24)a r e ,
P˙q+ψe−η=0
β,ψ,η ≥0
μγ−eTβ≥0
ψT/parenleftbig
μγ−eTβ/parenrightbig
=0
ηTβ=0,(25)
V ariational Contact-Implicit Trajectory Optimization 993
where ψandηare Lagrange multipliers. In the more compact shorthand notation
introduced in the previous subsection, these conditions can be rewritten as:
P˙q+ψe−η=0
0≤ψ⊥/parenleftbig
μγ−eTβ/parenrightbig
≥0
0≤η⊥β≥0.(26)
Physically, the Lagrange multiplier ψapproximates the projection of the system’s
velocity onto the plane tangent to the contact manifold. The conditions in ( 26) ensure
that the friction force will assume whatever value is necessary to prevent sliding when
ψ=0, up to the boundary of the friction cone. In the sliding case, when ψ/negationslash=0, the
friction force will lie on the boundary of the friction cone.
4.3 A Second-Order V ariational Time-Stepping Method
We now build a complete time-stepping method by combining the results of Sects. 4.1
and 4.2. We ﬁrst deﬁne a vector λthat combines the normal and friction components
of the contact impulse,
λ=/bracketleftbiggγ
β/bracketrightbigg
, (27)
and the corresponding Jacobian matrix to map λinto generalized coordinates:
J=/bracketleftbiggN
P/bracketrightbigg
. (28)
The discrete Euler-Lagrange dynamics can then be written as follows:
D2Ld(qk−1,qk)+D1Ld(qk,qk+1)
+1
2Fd(qk−1,qk)+1
2Fd(qk,qk+1)+J(qk+1)Tλk=0.
(29)
The set of complementarity conditions derived in the previous subsections are
used to determine λkin (29). Given qk−1and qk, the following feasibility problem
can be solved to ﬁnd λkand qk+1,
r(h,qk−1,qk,qk+1,λ k)=0
P(qk+1)/parenleftbiggqk+1−qk
h/parenrightbigg
+ψke−ηk=0
0≤(μγ k−eTβk)⊥ψk≥0(30)
994 Z. Manchester and S. Kuindersma
0≤φ(qk+1)⊥γk≥0
0≤βk⊥ηk≥0,
where r(h,qk−1,qk,qk+1,λ k)=0 refers to Eq. ( 29).
5 Direct Trajectory Optimization
We now propose a direct trajectory optimization algorithm that uses the variational
time-stepping scheme developed in the previous section as a set of dynamics con-
straints. Our strategy is to formulate the trajectory optimization problem as a nonlin-
ear program (NLP) and to solve it using standard constrained optimization software.
To ease the numerical difﬁculties associated with complementarity constraints,
we apply a smoothing scheme similar to that used in [ 7]. The key idea is to relax
the equality constraints in the three complementarity conditions in ( 30) by replacing
them with inequalities and introducing the slack variables sk:
r(h,qk−1,qk,qk+1,λ k)=0
P(qk+1)/parenleftbiggqk+1−qk
h/parenrightbigg
+ψke−ηk=0
λk,ψ k,η k,sk≥0
φ(qk+1)≥0
(μγ k−eTβk)≥0
sk−ηT
kβk≥0
sk−γT
kφ(qk+1)≥0
sk−ψT
k(μγ k−eTβk)≥0.(31)
Figure 1illustrates the feasible regions for both the original “strict” complementarity
constraints and the new relaxed complementarity constraints in ( 31). If the slack
variables are reduced to zero, the two regions coincide.
Fig. 1 Illustration of
feasible regions for relaxed
and strict complementarity
constraintsRelaxed Feasible Region
Strict Feasible Region
V ariational Contact-Implicit Trajectory Optimization 995
Physically, the relaxed complementarity constraints allow contact forces to act at
a non-zero distance from the contact manifold. This aids convergence, but we would
ultimately like solutions that closely respect the true constraints. To encourage con-vergence of solutions towards strict satisfaction of the complementarity constraints,
we augment the cost function with a term that penalizes s
k. The trajectory optimiza-
tion algorithm can now be stated as the following NLP:
minimize
h,Q,U,CJ(h,Q,U)+αN−1/summationdisplay
k=1sk
subject to f(h,qk−1,qk,qk+1,λ k,ψ k,η k)=0
g(qk+1,λ k,ψ k,η k,sk)≥0
umin≤uk≤umax
hmin≤h≤hmax,(32)
where Jis a cost function; αis a positive scalar weighting parameter; fand gare
the equality and inequality constraints in ( 31);Qis the set of all conﬁguration knot
points, qk;Uis the set of all control inputs, uk; and Cis the set of all contact-related
variables, λk,ψk,ηk, and sk.
The penalty on the slack variables in the the cost function of ( 32) is a so-called
“exact penalty” that has theoretical convergence guarantees with ﬁnite values of α[1].
In practice, we have observed good convergence behavior with modest values of α.
Problem ( 32) can be solved with standard nonlinear programming algorithms like
sequential quadratic programming (SQP) and interior-point methods [ 17]. It is also
straight-forward to include additional constraints on the system’s state and inputs.
6 Examples
To evaluate the proposed trajectory optimization algorithm, we demonstrate its abil-ity to generate complex, multi-contact motions by optimizing walking trajectories
for two simulated legged robots: Spring Flamingo and LittleDog. We also com-
pare its accuracy to the ﬁrst-order method used by Posa et al. [ 21] in both open-
and closed-loop simulations. In all cases, the optimizer was initialized with dynami-
cally infeasible trajectories consisting of linear interpolation between initial and goalposes. No a priori information about contact forces or mode sequences was used.
6.1 Spring Flamingo
Spring Flamingo is an 18-state planar bipedal robot with actuated hips and knees and
a passive spring ankle joint [ 22]. A trajectory optimization problem was deﬁned in
which the robot was required to move from an initial standing pose to a ﬁnal standing
996 Z. Manchester and S. Kuindersma
Fig. 2 Spring Flamingo optimized walking gait
pose translated to the left. The following cost function was minimized,
J=N−1/summationdisplay
i=10.1(xi−xg)T(xi−xg)+uT
iui, (33)
where xgis the goal state. Figure 2shows a sequence of frames taken from the
optimized walking gait. The algorithm discovered an energetically efﬁcient heel-toe
gait that exploits the passive dynamics of the leg and ankle.
6.2 LittleDog
LittleDog is a 36-state quadrupedal robot designed by Boston Dynamics to enable
research on legged locomotion [ 5]. A trajectory optimization problem was deﬁned in
which the robot was required to climb up an 11 cm step. Once again, initial and ﬁnalstate constraints were enforced, and a simple quadratic cost function was minimized.
Figure 3shows an example climbing strategy and Fig. 4shows the corresponding
sequence of modes (combinations of foot contacts) that were generated implicitlyby the solver.
6.3 Simulation Accuracy
We compare the ﬁrst-order and variational time-stepping methods in open-loop sim-
ulations of a tumbling brick hitting the ground and closed-loop simulations of the
V ariational Contact-Implicit Trajectory Optimization 997
Fig. 3 LittleDog climbing a step
00 .20 .40.60.811 .21 .41.61.82Back LeftBack RightFront LeftFront Right
Time (s)
Fig. 4 Contact mode sequence for each foot from LittleDog step-climbing example
LittleDog robot tracking a walking trajectory with proportional-derivative (PD) feed-
back control applied to its joints.
Tumbling-brick simulations were initialized with twenty different randomly cho-
sen initial conditions while varying the number of knot points. A reference solution
was computed using the ﬁrst-order method at a sample rate of 2 kHz. Figure 5shows
the root mean square (RMS) error (compared to the reference solution) as a function
of the number of knot points. The variational method achieves better accuracy with
fewer knot points than the ﬁrst-order time-stepping method.
To test closed-loop tracking performance, a set of walking trajectories was ﬁrst
optimized using both ﬁrst-order and variational time-stepping dynamics as con-
straints. The number of knot points used to parameterize the trajectories was varied
between 10 and 40. Simple PD control was applied to each joint of the robot, and
simulations were performed using the ﬁrst-order method at a sample rate of 2 kHz.
Figure 6shows the RMS tracking error in the robot’s state. Once again, better accu-
racy is achieved with fewer knot points using the variational method.
998 Z. Manchester and S. Kuindersma
24 6 8 1 01 21 4 16012
Knot PointsRMS ErrorFirst Order
Variational
Fig. 5 Falling brick simulation RMS error and standard deviation
10 15 20 25 30 35 400.20.220.240.260.280.3
Knot PointsRMS Error First Order
Variational
Fig. 6 RMS tracking error for LittleDog walking with PD tracking controller
7 Conclusions
We have presented a new family of variational time-stepping algorithms that gen-
eralize previous methods to higher orders of integration accuracy. We derived a
second-order method and incorporated it into a direct trajectory optimization algo-
rithm that solves for contact forces along with state and input trajectories. Our
numerical tests suggest that the method offers improvements over existing ﬁrst-
order contact-implicit trajectory optimization algorithms, allowing smaller NLPs to
be solved while maintaining reasonable accuracy. We also demonstrated the algo-
rithm’s ability to generate walking trajectories for simulated underactuated robots
with minimal problem-speciﬁcation effort.
There are several directions for future work. First, a more extensive numeri-
cal comparison including 3rd and 4th-order time-stepping methods would allow us
to better understand the trade-off between accuracy and computational cost. Sec-
V ariational Contact-Implicit Trajectory Optimization 999
ond, our current MA TLAB implementation requires several minutes to compute the
SpringFlamingo and LittleDog plans described in the previous section. Signiﬁcant
speed improvements could be made with a careful C++ implementation that exploitssparsity structure of the problem. Finally, as other authors have observed [ 13], the
contact mode trajectory often changes over longer timescales than the state and input
trajectories. Explicitly encoding this into the NLP formulation could aid convergenceand avoid unnecessary contact transitions.
Acknowledgements This work was supported by a Draper Internal Research and Development
grant and a Google Faculty Research Award. The authors would like to thank Michael Posa, InnaSharf, and the members of the Harvard Agile Robotics Lab for their helpful comments and discus-
sions.
References
1. Anitescu, M.: On using the elastic mode in nonlinear programming approaches to mathematical
programs with complementarity constraints. SIAM J. Optim. 15(4), 1203–1236 (2005). https://
doi.org/10.1137/S1052623402401221
2. Anitescu, M., Potra, F.A.: Formulating dynamic multi-rigid-body contact problems with friction
as solvable linear complementarity problems. Nonlinear Dyn. 14(3), 231–247 (1997)
3. Betts, J.T.: Survey of numerical methods for trajectory optimization. J. Guid. Control Dyn.
21(2), 193–207 (1998). https://doi.org/10.2514/2.4231
4. Boyd, S., V andenberghe, L.: Convex Optimization. Cambridge University Press, Cambridge
(2004)
5. Buchli, J., Pratt, J., Roy, N., Murphy, M.P ., Saunders, A., Moreira, C., Rizzi, A.A., Raibert,
M.: The littledog robot. Int. J. Robot. Res. 30(2), 145–149 (2010). https://doi.org/10.1177/
0278364910387457
6. Buss, M., Hardt, M., Kiener, J., Sobotka, M., Stelzer, M., von Stryk, O., Wollherr, D.: Towards an
autonomous, humanoid, and dynamically walking robot: modeling, optimal trajectory planning,
hardware architecture, and experiments. In: Proceedings of the 3rd International Conferenceon Humanoid Robotics, pp. 2491–2496 (2003)
7. Fletcher, R., Leyffer, S.: Solving mathematical programs with complementarity constraints as
nonlinear programs. Optim. Methods Softw. 19(1), 15–40 (2004)
8. Junge, O., Marsden, J.E., Ober-Blöbaum, S.: Discrete mechanics and optimal control. IFAC
Proc. V ol. 38(1), 538–543 (2005)
9. Marsden, J., Ratiu, T.: Introduction to Mechanics and Symmetry. Texts in Applied Mathematics,
2nd edn. Springer, New Y ork (1999)
10. Marsden, J.E., West, M.: Discrete mechanics and variational integrators. Acta Numer. 10,
357–514 (2001)
11. Mayne, D.Q.: A second-order gradient method of optimizing non- linear discrete time systems.
Int. J. Control 3, 8595 (1966)
12. Mombaur, K.D.: Using optimization to create self-stable human-like running. Robotica 27(3),
321–330 (2009). https://doi.org/10.1017/s0263574708004724
13. Mordatch, I., Todorov, E., Popovi´ c, Z.: Discovery of complex behaviors through contact-
invariant optimization. ACM Trans. Graph. 31(4), 43 (2012). https://doi.org/10.1145/2185520.
2185539
14. Moreau, J.: On unilateral constraints, friction and plasticity. In: New V ariational Techniques
in Mathematical Physics, pp. 172–322. Springer, Berlin (1973)
15. Neunert, M., Farshidian, F., Buchli, J.: Efﬁcient whole-body trajectory optimization
using contact constraint relaxation. In: 2016 IEEE-RAS 16th International Conference on
1000 Z. Manchester and S. Kuindersma
Humanoid Robots (Humanoids), pp. 43–48 (2016). https://doi.org/10.1109/HUMANOIDS.
2016.7803252
16. Neunert, M., Farshidian, F., Winkler, A.W., Buchli, J.: Trajectory optimization through contacts
and automatic gait discovery for quadrupeds. IEEE Robot. Autom. Lett. 2(3), 1502–1509
(2017). https://doi.org/10.1109/LRA.2017.2665685
17. Nocedal, J., Wright, S.J.: Numerical Optimization, 2nd edn. Springer, New Y ork (2006)18. Ober-Blobaum, S., Saake, N.: Construction and analysis of higher order Galerkin variational
integrators. Adv. Comput. Math. 41, 955–986 (2015)
19. Pekarek, D., Ames, A.D., Marsden, J.E.: Discrete mechanics and optimal control applied to the
compass gait biped. In: 2007 46th IEEE Conference on Decision and Control, pp. 5376–5382(2007). https://doi.org/10.1109/CDC.2007.4434296
20. Posa, M., Kuindersma, S., Tedrake, R.: Optimization and stabilization of trajectories for con-
strained dynamical systems. In: Proceedings of the International Conference on Robotics andAutomation (ICRA), pp. 1366–1373. Stockholm, Sweden (2016)
21. Posa, M., Cantu, C., Tedrake, R.: A direct method for trajectory optimization of rigid bodies
through contact. Int. J. Robot. Res. 33(1), 69–81 (2014)
22. Pratt, J., Pratt, G.: Intuitive control of a planar bipedal walking robot. In: Proceedings of the
1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146), vol.
3, pp. 2014–2021 (1998). https://doi.org/10.1109/ROBOT.1998.680611
23. Remy, C.D.: Optimal exploitation of natural dynamics in legged locomotion. Ph.D. thesis, ETH
ZURICH (2011)
24. Schultz, G., Mombaur, K.: Modeling and optimal control of human-like running. IEEE/ASME
Trans. Mechatron. 15(5), 783–792 (2010)
25. Stewart, D.E., Trinkle, J.C.: An implicit time-stepping scheme for rigid body dynamics with
inelastic collisions and coulomb friction. Int. J. Numer. Methods Eng. 39(15), 2673–2691
(1996)
26. Tassa, Y ., Erez, T., Todorov, E.: Synthesis and stabilization of complex behaviors through
online trajectory optimization. In: IEEE/RSJ International Conference on Intelligent Robotsand Systems, Algarve, Portugal (2012)
27. Todorov, E.: A convex, smooth and invertible contact model for trajectory optimization. In:
Proceedings of the International Conference on Robotics and Automation (ICRA), Shanghai,China (2011)
28. Xi, W., Remy, C.D.: Optimal gaits and motions for legged robots. In: Proceedings of the
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Chicago, IL,USA (2014)
Parametric Trajectory Libraries for
Online Motion Planning with Applicationto Soft Robots
T obia Marcucci, Manolo Garabini, Gian Maria Gasparri, Alessio Artoni,
Marco Gabiccini and Antonio Bicchi
1 Introduction
Trajectory planning is a problem of critical importance in robotics. Among the many,two fundamental motivations are: increase robot autonomy towards automated dis-covery of complex behaviors [ 13], and maximize performances such as energy efﬁ-
ciency or execution speed (critical in production cycles [ 21] and in mobile robotics
[16]).
Trajectory planning is even more important for the novel generation of soft robots.
These systems, thanks to the introduction of continuous or lumped elasticity in theirdesign (see [ 11,19] for two recent reviews), present richer dynamics with respect
to their rigid counterparts. A suitable exploitation of such characteristic leads to
substantial improvements in efﬁciency [ 20], and maximum speed [ 9]. However, in
order to achieve these results, two ingredients are fundamental for the control scheme:
optimality and feedforward. Optimal control theory is one of the very few tools able to
exploit elastic potential energy of soft robots [ 9,10], whereas closed-loop controllers
may alter the natural dynamics of the system [ 4]. Additionally, in a great variety of
practical situations, problem parameters may vary and be known only at the very
beginning of the task, posing the challenging requirement of generating the motionplanning in a negligible time with respect to the smallest time constant of the system
dynamics.
In this context, due to the dynamic nonlinearities, nonlinear programming is one
of the only viable options to both exploit the system dynamics and reduce the need
T. Marcucci ( B)·M. Garabini ·G. M. Gasparri ·M. Gabiccini ·A. Bicchi
Research Center “E. Piaggio”, Università di Pisa, Pisa, Italye-mail: tobia.marcucci@gmail.com
T. Marcucci ·M. Gabiccini ·A. Bicchi
Department of Advanced Robotics, Istituto Italiano di Tecnologia, Genoa, Italy
A. Artoni ·M. Gabiccini
Dipartimento di Ingegneria Civile e Industriale, Università di Pisa, Pisa, Italy
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_671001
1002 T. Marcucci et al.
for feedback. However, its direct application is not sufﬁciently fast, and the need for
alternative methods for real-time computation of (near-) optimal trajectories arises.
A classical answer to this problem is Dynamic Programming (DP) where all the
computational effort is shifted ofﬂine, but the curse of dimensionality makes this
method inapplicable to large scale systems. Similar in concept to DP , but different
in practice, is the trajectory library approach [ 15]; here the ofﬂine phase consists in
the computation of a set of optimal motions and the synthesis of optimal controllers
able to steer the system to one of the stored paths. Such libraries can be built with
various techniques, e.g. randomized motion planning [ 17]; in this paper we opt for the
mature ﬁeld of Direct Trajectory Optimization (DTO) [ 2]. A combination of DTO and
differential DP for the trajectory generation was proposed by [ 12], where neighboring
optimal controllers were also stored in the library. In [ 18] a feedback motion planning
algorithm, which uses sum-of-squares veriﬁcation for the computation of stability
regions along the trajectories in the library, was presented.
A different line to approach real-time optimization comes from the ﬁeld of Non-
linear Model Predictive Control (NMPC) where, in some schemes, only the solution
of a computationally lighter approximation of the nonlinear Optimal Control Prob-
lem (OCP) is performed. For example, [ 5] perform only one Sequential Quadratic
Program (SQP) iteration per sampling time, whereas [ 23] use sensitivity analysis to
anticipate the online solution of the OCP and subsequently correct the result. Theseare, however, control techniques and their applicability is limited to local corrections
of a nominal trajectory computed at the previous sampling time. From our side, we
are interested in optimal feedforward actions without restrictions on the values ofthe OCP parameters.
In this paper we aim to merge the previous points of view, using NMPC correction
techniques for the online reﬁnement of trajectories stored in a library. Furthermore,unlike the works listed above, our goal is not the system stabilization, which would
implicitly identify the system initial state as the OCP parameter, but rather to consider
a generic deﬁnition of a parametric NonLinear Program (NLP). In this framework,the optimal trajectory might be a function of any parameter in the underlying NLP ,
such as the system ﬁnal state or the cost function weights.
The presented method is composed of three phases: the ofﬂine generation of a
library of parametric trajectories via DTO, the online search in the library for the
best candidate solution to the OCP we aim to solve, and the online reﬁnement of
this trajectory. The last step consists in a real-time correction based on a sensitivityanalysis or in the solution of a Quadratic Program (QP), depending on the magnitude
of the parameter variation. Also the selection of the candidate trajectory is based on
sensitivity information, which, together with an efﬁcient statement of the QP , are
stored in the library.
The main contribution of this work is the development of an algorithm for real-
time optimal motion planning; nonetheless, to the best of the authors’ knowledge,
the experimental results presented in this work are the ﬁrst successful application of
open-loop optimal control of ﬂexible-joint robots, strategy that recently proved to bethe most adequate to the control of this type of systems [ 4].
Parametric Trajectory Libraries … 1003
In Sect. 2we expose a motivational pick-and-place problem with parametric ob-
ject mass, initial and terminal pose. We show how soft robots could in principle save
a considerable amount of energy (approximately 30%) but, assuming the parametervalues to be known only at the very beginning of the task, this saving can be achieved
only with real-time motion planning. After a brief review of parametric nonlinear
optimization (Sect. 3), in Sect. 4we present an efﬁcient strategy for the online reﬁne-
ment of optimal trajectories. Section 5describes the method of parametric trajectory
libraries, and shows how it overcomes the difﬁculties outlined in Sect. 2. Experimen-
tal validation is presented in Sect. 6(and in the accompanying video), pointing out the
further beneﬁt of this method of not requiring settling times caused by model-plant
mismatches. Finally, conclusions are presented in Sect. 7.
2 Motivational Example and Problem Statement
Pick and place is probably the most striking example of soft robots efﬁciency; how-
ever, even for this simple task, if we allow object positions and masses to vary, bring-
ing the robot to resonance becomes a non-trivial operation which requires advanced
planning techniques. In this section we analyze energy efﬁciency of a pick-and-placeproblem for the planar manipulator shown in Fig. 1and described in Table 1.W es h o w
how, in principle, soft actuation could outperform rigid one but excessive planning
times make these savings out of reach for common DTO techniques and the need ofnovel methods for online motion planning arises.
The task is to move n
o=100 objects with random masses mo∈[0.3,0.5]kg
between random positions which are assumed to be known only at the completionof the previous task. We compare rigid actuation (R), soft actuation with variable
stiffness (VS), and soft actuation with constant stiffness (CS); including returns, we
obtain 2 n
oOCPs per actuation type. Preliminarily, we will not take computation
times into account assuming that optimal trajectories are computed instantaneously.
Fig. 1 Planar RR manipulator powered by series elastic actuators with the fast version of the
Pisa-IIT SoftHand with a closure time of 0.1 s
1004 T. Marcucci et al.
Ta b l e 1 Parameters of the planar manipulator (identiﬁed from a point-mass model)
Mass (kg) Friction (Nms
rad) Center of mass
position (m)Length (m)
Link 1 0.75 0.040 0.20 0.22
Link 2 0.88 0.030 0.19 0.22
Inertia (mgm2) Friction (μNms
rad) Reduction ratio
Motor 0.50 0.22 205
Denoting with ϕ∈Rnϕthe angular position of the links and with θ∈Rnϕthe
link-side angular position of the motors, the soft robot dynamics is modeled as
M(ϕ)¨ϕ+c(ϕ,˙ϕ)+K(ϕ−θ)=0,I¨θ+F˙θ+K(θ−ϕ)=τ, (1)
where M∈Rnϕ×nϕis the link inertia matrix, cincludes Coriolis, centrifugal, fric-
tional, and gravitational terms, Kis the diagonal stiffness matrix, Iis the diagonal
inertia matrix of the motors, Fis the diagonal friction matrix of the motors, and τ
represents motor torques. The dynamics of the corresponding rigid robot is simply
(M(ϕ)+I)¨ϕ+c(ϕ,˙ϕ)+F˙ϕ=τ. (2)
Fork=1,..., 2no, each OCP has the following structure (case-speciﬁc equations
or variables are explicitly indicated):
min
τ(t), K/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
VSJk:=1
tk
f−tk
i/integraldisplaytk
f
tk
iτ2(t)dts.t. (OCP)
{(1)/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
CS, VS,(2)/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
R,(dynamic
model)
ϕ(tk
i)=ϕk
i,˙ϕ(tk
i)=0,θ (tk
i)=θk
i,˙θ(tk
i)=0/bracehtipupleft /bracehtipdownright/bracehtipdownleft /bracehtipupright
CS, VS,(initial
conditions)
ξ(ϕ(tk
f))=ξk
f,˙ϕ(tk
f)=0,˙θ(tk
f)=0/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
CS, VS,(terminal
constraints)
ϕ≤ϕ(t)≤ϕ, τ≤τ(t)≤τ,v≤˙ϕ(t)≤v/bracehtipupleft/bracehtipdownright/bracehtipdownleft /bracehtipupright
R,v≤˙θ(t)≤v/bracehtipupleft/bracehtipdownright/bracehtipdownleft /bracehtipupright
CS, VS,K/follows0/bracehtipupleft/bracehtipdownright/bracehtipdownleft/bracehtipupright
VS},(variable
bounds)
where ξ(ϕ)∈Rnξdenotes the position of the end-effector, the subscripts i and f rep-
resent initial and ﬁnal values, respectively, whereas underlines and overlines denotelower and upper bounds, respectively. The time horizon of each plan is t
k
f−tk
i=2s
which, overall, demonstrated to be the shortest horizon allowed by actuator velocity
limits for this positioning of the objects. The ( OCP ) is discretized with a multiple
Parametric Trajectory Libraries … 1005
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.400.10.20.30.4
-3-2-1012
02468 1 0 1 2-2-101
(a) Rigid robot.-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.400.10.20.30.4
-3-2-1012
02468 1 0 1 2-1.5-1-0.500.51
(b) Robot with constant stiffness.
Fig. 2 First three pick-and-place tasks. From top to bottom: end-effector trajectory, conﬁguration
angles as functions of time, input torques as functions of time
shooting approach: 30 shooting intervals per plan with piecewise constant inputs
and 5 integration steps per shooting. The integration scheme is the 4th order ex-
plicit Runge–Kutta. Bounds on the optimization variables are: τ=−τ=5 Nm,
ϕ=−ϕ=/bracketleftbig∞π/2/bracketrightbig/latticetoprad,v=−v=4 rad/s. NLPs are solved with IPOPT [22]
and implemented in the highly efﬁcient framework CasADi [1]. Energy efﬁciency
is compared analyzing the mean cost, deﬁned as ¯J∗:=1
2no/summationtext2no
k=1Jk∗, with Jk∗de-
noting the solution of the kth (OCP ).
The mean cost set by the rigid robot (R) is ¯J∗
R=0.184(Nm)2; by way of illustra-
tion, Fig. 2a shows the optimal trajectories for the ﬁrst three movements. Optimizing
the stiffness of each actuator at every movement (VS), we reduce the energy con-
sumption by 31 .0% setting ¯J∗
VS=0.127(Nm)2. However, this analysis neglects
energy consumption due to the change (and the retention) of the actuator stiffness
that, depending on the motor design, might be signiﬁcant. To make the comparison
fair, we ﬁx motor stiffnesses (CS) to the mean optimal values obtained from the
previous analysis ¯K∗=diag(0.316,1.772)Nm/rad. The resulting average cost is
¯J∗
CS=0.137(Nm)2, with a satisfactory saving with respect to the rigid case (25 .8%)
and a limited loss with respect to the variable stiffness case ( −7.6%). Figure 2bs h o w s
the optimal trajectories for the latter case.
Taking computation times into account, however, these conclusions change dras-
tically. Since the ﬁnal state of the robot is not an equilibrium ( ϕ(tk
f)/negationslash=θ(tk
f)), delays
in the computations and, consequently, in the application of the open-loop control
might have catastrophic consequences. NLP solutions, in fact, required on average
0.853 s for the constant stiffness case and 1 .658 s for variable stiffness case, making
1006 T. Marcucci et al.
these optimal trajectories useless in practice. (Computations are performed with a
notebook computer with the 2.4 GHz Intel Core i7 processor and a 16 GB 1867 MHz
LPDDR3 memory.)
The problem address by this work is to extend the scope of DTO methods to the
real-time motion planning of dynamical systems. The goal is not to deﬁne a stabilizing
policy for the system under analysis, but rather to derive a general-purpose strategyable to react online to environment changes and variations in the task parameters. To
this end, we decide to leverage on ofﬂine computations and we consider a limited loss
in optimality an admissible compromise but, on the other hand, we are as intransigentas possible towards the feasibility of the synthesized motions. The algorithm we
propose in this paper is able to generate nearly optimal (and ﬁrst-order feasible)
trajectories in an amount of time that, for the case study presented in this section, istwo orders of magnitude lower than the one obtained with the state-of-the-art NLP
techniques mentioned above.
3 Notions of Nonlinear Optimization
In this section we review some basic concepts of parametric nonlinear optimiza-
tion. Even if these are well known (see, e.g., [ 7,14]), they are reviewed here for
convenience as they are the basis for the developments in the next sections. In the
following, we will denote with d xf(x)(∂xf(x)) the total (partial) derivative of f(x)
with respect to x.
Direct trajectory optimization techniques translate the continuous time ( OCP )i n t o
a ﬁnite dimensional NLP . Including the explicit dependence on a set of parameters
p, we get a multiparametric NLP , which can be stated as
min
xf(x,p)s.t.{g(x,p)=0,h(x,p)≥0}, (mpNLP)
where x∈Rnx,p∈Rnp,f∈R,g∈Rng, and h∈Rnh. For a feasible x, we call
A(x,p):= {k∈{1,..., nh}|hk(x,p)=0}active set, and we distinguish
active inequalities ha(x,p):= {hk(x,p)|k∈A(x,p)}∈ Rnafrom inactive inequal-
ities hi(x,p):= {hk(x,p)|k∈{1,..., nh}\A(x,p)}∈ Rnh−na. Moreover, denot-
ing with x∗(p)a local solution of ( mpNLP ), we deﬁne the optimal active set
A∗(p):=A(x∗(p),p).
The Lagrangian function for ( mpNLP )i sl(x,λ,μ, p):=f(x,p)−λ/latticetopg(x,p)−
μ/latticetoph(x,p),with Lagrange multipliers λ∈Rngandμ∈Rnh. The following theorem
states necessary conditions for xto be a local solution of ( mpNLP ).
Theorem 1 (First-Order Necessary Conditions (FONC)) Consider the set of
Karush–Kuhn–Tucker (KKT) equations
Parametric Trajectory Libraries … 1007
∂/latticetop
xl(x,λ,μ, p)=0, (stationarity) (3a)
g(x,p)=0, (primal feasibility) (3b)
h(x,p)≥0, (3c)
μ≥0, (dual feasibility) (3d)
μ/latticetoph(x,p)=0, (complementarity slackness) (3e)
where (3e)is equivalent to μkhk(x,p)=0for k=1,..., nh. Assume both the objec-
tive f(x,p)and the constraints g (x,p),h(x,p)to be C1with respect to x and Linear
Independence Constraint Qualiﬁcation (LICQ) to hold at x∗(p). Then there exist La-
grange multipliers λ∗(p),μ∗(p)such that ( 3) are satisﬁed at (x∗(p),λ∗(p),μ∗(p)).
Collecting primal and dual variables in a single vector y:=/bracketleftbigx/latticetopλ/latticetopμ/latticetop/bracketrightbig/latticetop,w e
are interested in determining how the solution y∗(p)changes as a result of small
variations of p. The following theorem sets the basis for this analysis.
Theorem 2 (Optimal Sensitivity) Suppose both the objective f (x,p)and the con-
straints g (x,p),h(x,p)to be C2with respect to x and p. For p ≡¯p, if FONC,
Second Order Sufﬁcient Conditions (SOSC), and Strict Complementarity Slackness(SCS) hold at y
∗(¯p), then: i) x∗(¯p)is an isolated local minimum of (mpNLP )for
p≡¯p and multipliers λ∗(¯p),μ∗(¯p)are uniquely determined; ii) for p in a neigh-
borhood of ¯p, there exists a unique C1function y∗(p)satisfying FONC, SOSC, and
SCS for (mpNLP ).
Given the existence and the differentiability of y∗(p), we can derive the sensitivity
matrix d py∗(¯p). Let us consider the equalities ( 3a), (3b), (3e) of the FONC; requiring
their total derivative with respect to pto vanish at the solution, we obtain the linear
system of equations
⎡
⎣∂xxl−∂/latticetop
xg−∂/latticetop
xh
∂xg 00
μ∗/latticetop∂xh 0 h/latticetop⎤
⎦dpy∗=−⎡
⎢⎣∂xpl
∂pg
μ∗/latticetop∂ph⎤
⎥⎦, (4)
where all the functions are evaluated at ¯pandy∗(¯p). Under the hypotheses of Theo-
rem 2, the KKT matrix in the left hand side of ( 4) is always invertible and the linear
system can be solved for d py∗(¯p).
Given a solution of ( mpNLP )f o rs o m e p≡¯p, we are now able to approximate
the solution of ( mpNLP ) for a new value of the parameter p≡¯p+Δpas
y∗(¯p+Δp)≈y∗(¯p)+dpy∗(¯p)Δp. (5)
1008 T. Marcucci et al.
4 Active Set Changes in Sensitivity Analysis
While the quality of approximation ( 5) is generally satisfactory when the perturbed
parameters belong to a neighborhood of their nominal values ¯p, we assist to a severe
accuracy loss whenever the magnitude of Δpis high enough to cause a change in the
optimal active set (i.e., A∗(¯p)/negationslash=A∗(¯p+Δp)). This change, in fact, is associated
with: i) a discontinuity in the sensitivity function d py∗(p), that makes the use of a
linear approximation inconsistent; ii) a high risk of unfeasibility of the approximated
solution which would result in a control input inapplicable to the real system.
In order to overcome this problem, in Sect. 4.1we take advantage of a procedure
(originally proposed in [ 8] and, more recently, used in NMPC [ 6]) that, through the
deﬁnition of a tangent Quadratic Program (tQP), naturally extends the sensitivity
analysis to take into account active set changes and inequality constraints ( 3c), (3d).
In Sect. 4.2we show how the structure of this tQP can be exploited to reduce ofﬂine the
dimension of the problem, with a great decrease in the online solution times. Finally,in Sect. 4.3, we address the issue of the nonconvexity of the tQP and we propose a
tailored convexiﬁcation strategy which has a minimal impact on the solution. This
leads to the statement of the problem we actually solve online.
4.1 Sensitivity Analysis via Quadratic Programming
In order to include active set changes in the sensitivity analysis, we consider the tQP
min
Δx1
2Δ/latticetop
x∂xxlΔx+/parenleftbig
Δ/latticetop
p∂pxl+∂xf/parenrightbig
Δx (tQP)
s.t./braceleftbig
∂xgΔx+∂pgΔp=0,h+∂xhΔx+∂phΔp≥0/bracerightbig
,
where all the functions are evaluated at ¯pandy∗(¯p)andΔxrepresents the correction
tox∗(¯p)induced by the parameter variation Δp. In this subsection we show that the
solution of ( tQP):
•always veriﬁes the ﬁrst order approximation at ¯pand y∗(¯p)of the feasibility
conditions ( 3c) and ( 3d),
•coincides with the sensitivity approximation ( 5) whenever the latter does not vio-
late the ﬁrst order approximation of ( 3c)o r( 3d).
These two properties candidate ( tQP) as the natural extension of ( 5) whenever active
set changes are detected.
We start considering the Lagrangian function for ( tQP) with multipliers λ∗(¯p)+
Δλfor the equalities and μ∗(¯p)+Δμfor the inequalities. The related FONC are
Parametric Trajectory Libraries … 1009
∂xxlΔx−∂/latticetop
xgΔλ−∂/latticetop
xhΔμ+∂xplΔp=0,(stationarity) (6a)
∂xgΔx+∂pgΔp=0,(primal feasibility) (6b)
h+∂xhΔx+∂phΔp≥0, (6c)
μ∗+Δμ≥0,(dual feasibility) (6d)
(μ∗
k+Δμk)(hk+∂xhkΔx+∂phkΔp)=0,(complementarity slackness) (6e)
with k=1,..., nh. Analyzing this set of equations we note that: ( 6a) and ( 6b)
coincide with the ﬁrst two equations in ( 4) multiplied by Δp,(6c) and ( 6d)a r et h e
linearization of ( 3c) and ( 3d)a t¯pandy∗(¯p), and ( 6e) is equivalent to the third
equation in ( 4) except for second order terms. Focusing on ( 6e) we also notice that,
as long as the solution of ( 6) veriﬁes ( 3c) and ( 3d) strictly, second order terms are
irrelevant and the solutions of ( 6) and ( 4) coincide. This claim can be easily veriﬁed;
in fact, only one of the following alternatives is possible: either the kth constraint is
active ( hk>0,μ∗
k=0), then, since ( 6c) holds strictly, ( 6e) implies Δμ k=0, which
agrees with the result from the third equation in ( 4); or the kth constraint is inactive
(hk=0,μ∗
k>0), then, since ( 6d) holds strictly, ( 6e) implies ∂xhkΔx+∂phkΔp=0,
which agrees with the result from the third equation in ( 4). On the other hand, second
order terms in ( 6e) become relevant when some of the inequalities ( 6c), (6d) are active,
ensuring feasibility of ( 6).
4.2 Condensation of the Tangent Quadratic Program
Generally speaking, there are two main approaches to the solution of ( tQP)[6]: solve
the problem in the actual form, using a sparse QP solver; or use the equality constraints
to remove dependent variables (process called condensing) and solve the resulting
dense QP . Since in our case the entire condensing process can be performed ofﬂine,the second approach is certainly more convenient. tQPs are typically condensed
exploiting the structure of the control problem: with the initial state as a parameter,
a forward simulation of the system dynamics is performed to express state variablesas functions of the inputs and remove them from the optimization. This procedure
however is not easily extendible to our problem because of the generic nature of the
parameters we consider. In this subsection we propose a method to condense ( tQP)
that does not require any hypothesis on the typology of parameters.
Denoting with N(M)and R(M)the null and range space of a matrix M
and deﬁning c
/latticetop
a:=/bracketleftbigg/latticetoph/latticetop
a/bracketrightbig
, we consider: Z1∈Rnx×(nx−ng−na)orthogonal basis of
N(∂xca),Z2∈Rnx×naorthogonal complement to Z1to form a basis of N(∂xg),
andZ3∈Rnx×ngorthogonal complement to/bracketleftbigZ1Z2/bracketrightbig
to form a basis of Rnx. Further-
more, we deﬁne ˜Z2:=Z2(∂xhaZ2)−1, where ∂xhaZ2∈Rna×nais invertible since
N(∂xha)∩R(Z2)=∅ . We then consider the new set of variables/bracketleftbigz/latticetop
1˜z/latticetop
2z/latticetop
3/bracketrightbig/latticetop:=/bracketleftbig
Z1˜Z2Z3/bracketrightbig−1Δx. In the following we show that, with this change of variables, ( tQP)
assumes the condensed form
1010 T. Marcucci et al.
min
˜z121
2˜z/latticetop
12H˜z12+d˜z12s.t.{A˜z12≥b,˜z2≥c}. (ctQP)
where ˜z/latticetop
12:=/bracketleftbigz/latticetop
1˜z/latticetop
2/bracketrightbig
. This reformulation entails two main advantages:
•the decrease in the number of optimization variables from nxtonx−ng,
•the transformation of active inequalities into lower bounds on the variables ˜z2.
Changing variables in the equalities of ( tQP), we can express the dependent vari-
ables as explicit functions of the parameter variation z3=−(∂xgZ 3)−1∂pgΔp,where
again∂xgZ 3∈Rng×ngis invertible since N(∂xg)∩R(Z3)=∅ . Active inequalities
of (tQP) are transformed into lower bounds ˜z2≥−∂xhaZ3z3−∂phaΔp=:c.Deﬁn-
ing˜Z12:=/bracketleftbig
Z1˜Z2/bracketrightbig
andA:=∂xhi˜Z12, we see that inactive inequalities do not take
any advantage from this change of variables, in fact we obtain A˜z12≥−∂xhiZ3z3−
∂phiΔp−hi=:b.Finally, changing variables in the cost function, after some ma-
nipulations we arrive to the form presented in ( ctQP ) with H:=˜Z/latticetop
12∂xxl˜Z12and
d:=z/latticetop
3Z/latticetop
3∂xxl˜Z12+Δ/latticetop
p∂pxl˜Z12+/bracketleftbig
01×(nx−ng−na)μ∗
a/latticetop∂xha˜Z2/bracketrightbig
, where optimality
condition ( 3a) have been used, constant terms (including z3) have been removed,
andμ∗
a∈Rnadenotes the optimal value of the active inequality multipliers.
4.3 Tangent Quadratic Program Convexiﬁcation
The ﬁnal issue in the solution of ( tQP) is its potential nonconvexity, which prevents
a direct application of the most efﬁcient optimization algorithms. In fact, assum-
ing SOSC and LICQ to hold, only the reduced Hessian matrix R:=Z/latticetop
1∂xxlZ1is
guaranteed to be positive deﬁnite, while ∂xxlmight have negative eigenvalues.
Commonly, ∂xxlis convexiﬁed with the addition of a correction matrix ΔL:=
∂/latticetop
xcaΛ∂xcawith symmetric Λ. This choice is particularly appealing since:
•there always exists a ﬁnite Λsuch that ∂xxl+ΔL/follows0,
•this convexiﬁcation does not alter the solution of ( tQP) whenever the correction
from ( tQP) does not require any active constraint to become inactive (i.e., whenever
in (6c)w eh a v e ∂xhaΔ∗
x+∂phaΔp=0, with Δ∗
xsolution of ( tQP)).
However, since we are going to solve the problem in its condensed form ( ctQP ), to
our purpose is sufﬁcient to make only Hpositive deﬁnite and not the entire ∂xxl.T od o
that we consider the simpler correction matrix ΔL:=∂/latticetop
xhaΛ∂xha; in the following
we prove that the previous claims still hold.
We deﬁne the corrected Hessian matrix L:=∂xxl+ΔLand we replace it to ∂xxl
in the deﬁnition of H. We get
˜Z/latticetop
12L˜Z12=/bracketleftbiggRZ/latticetop
1∂xxl˜Z2
˜Z/latticetop
2∂xxlZ1˜Z/latticetop
2∂xxl˜Z2+Λ/bracketrightbigg
.
Parametric Trajectory Libraries … 1011
For the Schur complement lemma, requiring ˜Z/latticetop
12L˜Z12to be positive deﬁnite is equiva-
lent to require R/follows0 andΛ/follows˜Z/latticetop
2/parenleftbig
∂xxlZ1R−1Z/latticetop
1∂xxl−∂xxl/parenrightbig˜Z2=:S. Being Rthe
reduced Hessian, the ﬁrst condition is veriﬁed whenever SOSC and LICQ hold, and
being Sa bounded matrix, a Λsuch that the second relation holds can always be found.
Moving to the second claim, we note that, as long as ∂xhaΔ∗
x+∂phaΔp=0 the con-
tribution to the cost function of the correction term Δ∗
x/latticetopΔLΔ∗
xassumes the constant
valueΔ/latticetop
p∂/latticetopphaΛ∂ phaΔp,hence it does not affect the solution of ( tQP). Finally, con-
cerning the choice of Λ, we decide to minimize /bardblΛ/bardblFsubject to min (eig(Λ−S)) > ε
for a small ε> 0, whose closed-form solution consists in a clipping of the eigenvalues
ofΛ−Stoε[14].
4.4 Sensitivity Analysis Summary
In conclusion we brieﬂy summarize the results from this section. We have shown
how to take into account the ﬁrst order approximation of inequality constraints ( 3c),
(3d) in the sensitivity analysis. This process results in the quadratic program ( tQP),
whose solution coincides with the linear sensitivity correction ( 5) as long as the
feasibility conditions ( 6c), (6d) are satisﬁed, but allows to improve the quality of the
approximation for parameter variations Δpbig enough to change the optimal active
set. We have proposed a condensing strategy that (without any hypothesis on thenature of the parameters p) allows to remove (ofﬂine) all the dependent variables from
(tQP), resulting in the dense, but substantially smaller, quadratic program ( ctQP ).
Finally, we have proposed a convexiﬁcation method tailored for ( ctQP ) which does
not alter its solution as long as “constraint deactivations” do not occur, ensuring that
also the solution of the convexiﬁed ( ctQP ) coincides with the linear correction ( 5)
when the active set is unchanged.
5 Parametric Trajectory Libraries
We now describe how parametric trajectory libraries are built and how they are used
for the online generation of suboptimal trajectories.
Having parameterized the OCP with the vector p, we discretize the continuous
time problem into the ﬁnite dimensional ( mpNLP ). Algorithm 1 is then applied for the
construction of the trajectory library. This consists in the generation of random OCPs;
for each OCP we ﬁrst ﬁnd the nearest trajectory in the library (using a metric deﬁnedin the following). Then, starting from this trajectory, an approximated solution is
derived. The approximation accuracy is tested through a problem-dependent accuracy
index: if the accuracy is high enough, the solution of ( mpNLP ) is not required,
otherwise ( mpNLP ) is solved and its solution is added (together with sensitivity
1012 T. Marcucci et al.
Algorithm 1: Parametric Trajectory Library Generation
Result : library
nsucc=0
while nsucc/bardblξ(q)−ξk
f/bardblis veriﬁed, with k=1,..., 6.
We start presenting in Fig. 4a the results achievable when each movement is
planned through the solution of ( mpNLP ). The ﬁrst thing to note is that, because
of the high computation times, in this case a settling time is necessary to stabilize the
system at the end of each planning, ensuring that the position read by the sensors (and
used as initial condition in ( OCP )) is consistent with the state of the system when the
feedforward is actually applied. (Settling times can be easily recognized from Fig. 4a
as time intervals of constant motor angle.) This phenomenon makes the storage of
potential energy between consecutive tasks impossible therefore, in order to facili-
tate the stabilization, we add the constraint ϕ(tk
f)=θ(tk
f)to (OCP ). Considering the
absence of a feedback action on the end-effector position, the overall accuracy of the
movements is satisfactory (even if not sufﬁcient to guarantee the success of every
grasp): each trajectory reaches a circle of radius 40 mm centered at the nominal ﬁnal
position. Nonetheless, stabilization phases make the system behavior and the execu-
tion time (25.1 s) unacceptable. Figure 4b shows the experiment in case a trajectory
library is used. Here it can be seen that, with similar accuracy, the method based on
the trajectory libraries is able to perform the same task in a considerably shorter time
(7.1 s) and without requiring any settling time.
1The accompanying video can be found at https://www.youtube.com/watch?v=AEDXAZmoPuw .
Parametric Trajectory Libraries … 1015
0.20.30.40.5
0.20.30.40.5
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.40.20.30.40.5
0 5 10 15 20 25-2-101
(a) Nonlinear trajectory optimization.0.20.30.40.5
0.20.30.40.5
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.40.20.30.40.5
01234567-2-101
(b) Parametric trajectory library.
Fig. 4 Experimental results. From top to bottom: end-effector trajectory during the three pick-and-
place actions, reference and real motor positions with the PID controller
7 Conclusions and Future Works
In this paper we have illustrated a procedure for online generation of optimal mo-
tion plans for constrained nonlinear systems. Combining a trajectory library with a
sensitivity analysis, we are able to reﬁne online the trajectories stored in the library,
making them suitable for the open-loop control of the system. Analyzing this prob-
lem as a generic parametric NLP , we can manage a wider class of parameters with
respect to common applications of trajectory libraries, which are mainly focused
on system stabilization. We motivated the necessity of such a planning technique
analyzing a pick-and-place task, showing how the combination of soft actuation and
trajectory optimization can drastically reduce energy consumptions but it is not fea-
sible because of the slow responsiveness of NLP-based planners. In this context, we
have experimentally validated our approach demonstrating its ability to overcome
computation time issues for the analyzed system. Nonetheless, we underline that the
presented method can be applied to the trajectory optimization of generic dynamical
systems and is not in any way restricted to soft robots or robots in general.
Future works will be focused on the analysis of the complexity of the presented
approach in case of higher dimensional systems, such as bipeds and humanoids.
1016 T. Marcucci et al.
Moreover, we plan to conduct a deeper analysis of the sampling process for the
library generation in order to derive theoretical guarantees about its completeness.
Finally, we also aim to conduct more detailed experimental analyses on the energybeneﬁts that result from the application of this method to soft robots.
References
1. Andersson, J.: A general-purpose software framework for dynamic optimization. Ph.D. thesis,
Arenberg Doctoral School, KU Leuven (2013)
2. Betts, J.T.: Practical Methods for Optimal Control and Estimation Using Nonlinear Program-
ming, vol. 19. Siam (2010)
3. Catalano, M.G., Grioli, G., Farnioli, E., Serio, A., Piazza, C., Bicchi, A.: Adaptive synergies
for the design and control of the pisa/iit softhand. Int. J. Robot. Res. 33, 768–782 (2014)
4. Della Santina, C., Bianchi, M., Grioli, G., Angelini, F., Catalano, M., Garabini, M., Bicchi, A.:
Controlling soft robots. IEEE Robot. Autom. Mag. 24, 75–83 (2017)
5. Diehl, M., Bock, H.G., Schlöder, J.P ., Findeisen, R., Nagy, Z., Allgöwer, F.: Real-time opti-
mization and nonlinear model predictive control of processes governed by differential-algebraicequations. J. Process Control 12(4), 577–585 (2002)
6. Diehl, M., Gros, S.: Numerical optimization of dynamic systems (draft) (2016). https://www.
syscop.de/ﬁles/2015ws/noc-dae/00-book.pdf
7. Fiacco, A.V .: Introduction to Sensitivity and Stability Analysis in Nonlinear Programming.
Academic Press, Inc., by Springer, Berlin (1984)
8. Ganesh, N., Biegler, L.T.: A reduced hessian strategy for sensitivity analysis of optimal ﬂow-
sheets. AIChE J. 33(2), 282–296 (1987)
9. Garabini, M., Passaglia, A., Belo, F., Salaris, P ., Bicchi, A.: Optimality principles in variable
stiffness control: the VSA hammer. In: 2011 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), pp. 3770–3775 (2011)
10. Haddadin, S., Huber, F., Albu-Schäffer, A.: Optimal control for exploiting the natural dynamics
of variable stiffness robots. In: IEEE International Conference on Robotics and Automation(ICRA), pp. 3347–3354. IEEE, St. Paul (2012)
11. Kim, S., Laschi, C., Trimmer, B.: Soft robotics: a bioinspired evolution in robotics. Trends
Biotechnol. 31(5), 287–294 (2013)
12. Liu, C., Atkeson, C.G.: Standing balance control using a trajectory library. In: 2009 IEEE/RSJ
International Conference on Intelligent Robots and Systems, pp. 3031–3036. IEEE, ST. Louis(2009)
13. Mordatch, I., Todorov, E., Popovi´ c, Z.: Discovery of complex behaviors through contact-
invariant optimization. ACM Trans. Graph. 31(4), 43 (2012)
14. Nocedal, J., Wright, S.: Numerical Optimization. Springer Science and Business Media, New
Y ork (2006)
15. Schaal, S., Atkeson, C.G.: Learning control in robotics. IEEE Robot. Autom. Mag. 17(2),
20–29 (2010)
16. Silva, M.F., Machado, J.T.: A literature review on the optimization of legged robots. J. Vib.
Control 18(12), 1753–1767 (2012)
17. Stolle, M., Atkeson, C.G.: Policies based on trajectory libraries. In: IEEE International Con-
ference on Robotics and Automation (ICRA), pp. 3344–3349 (2006)
18. Tedrake, R., Manchester, I.R., Tobenkin, M., Roberts, J.W.: LQR-trees: feedback motion plan-
ning via sums-of-squares veriﬁcation. Int. J. Robot. Res. 29(8), 1038–1052 (2010)
19. V anderborght, B., Albu-Schäffer, A., Bicchi, A., Burdet, E., Caldwell, D.G., Carloni, R., Cata-
lano, M., Eiberger, O., Friedl, W., Ganesh, G., et al.: V ariable impedance actuators: a review.Robot. Auton. Syst. 61(12), 1601–1614 (2013)
Parametric Trajectory Libraries … 1017
20. V anderborght, B., V errelst, B., V an Ham, R., V an Damme, M., Lefeber, D., Duran, B.M.Y ., Beyl,
P .: Exploiting natural dynamics to reduce energy consumption by controlling the complianceof soft actuators. Int. J. Robot. Res. 25, 343–358 (2006)
21. V erscheure, D., Demeulenaere, B., Swevers, J., De Schutter, J., Diehl, M.: Practical time-
optimal trajectory planning for robots: a convex optimization approach. IEEE Trans. Autom.Control (2008)
22. Wächter, A., Biegler, L.T.: On the implementation of an interior-point ﬁlter line-search algo-
rithm for large-scale nonlinear programming. Math. Program. 106, 25–57 (2006)
23. Zavala, V .M., Biegler, L.T.: The advanced-step nmpc controller: optimality, stability and ro-
bustness. Automatica 45(1), 86–93 (2009)
Interleaving Planning and Control
for Deformable Object Manipulation
Dale McConachie, Mengyao Ruan and Dmitry Berenson
1 Introduction
Examples of deformable object manipulation range from domestic tasks like folding
clothes to time and safety critical tasks such as robotic surgery. One of the challengesin planning for deformable object manipulation is the high number of degrees of
freedom involved; even approximating the conﬁguration of a piece of cloth with a
4×4 grid results in a 48 degree of freedom conﬁguration space. In addition, the
dynamics of the deformable object are difﬁcult to model [ 8]; even with high-ﬁdelity
modeling and simulation, planning for an individual task can take hours [ 2]. Local
controllers on the other hand are able to very efﬁciently generate motion, however,they are only able to successfully complete a task when the initial conﬁguration is
favorable [ 3,19].
The central question we address in this work is how can we combine the strengths
of global planning with the strengths of local control while mitigating the weak-
ness of each? We propose a framework for interleaving planning and control whichuses global planning to generate gross motion of the deformable object, and a local
controller to reﬁne the conﬁguration of the deformable object within the local neigh-
borhood. By separating planning from control we are able to use different repre-sentations of the deformable object, each suited to efﬁcient computation for their
respective roles. In order to determine when to use each component, we introduce a
novel deadlock prediction algorithm that is inspired by topologically based motionplanning methods [ 4,13]. By answering the question “Will the local controller get
stuck?” we can predict if the local controller will be unable to achieve the task from
This work was supported in part by NSF grant IIS-1656101 and ONR grant N000141712050.
D. McConachie ( B)·M. Ruan ·D. Berenson
University of Michigan, Ann Arbor, MI 48109, USAe-mail: dmcconac@umich.edu
M. Ruan
e-mail: ruanmiao@umich.edu
D. Berenson
e-mail: berenson@eecs.umich.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_681019
1020 D. McConachie et al.
Fig. 1 Three example manipulation tasks for our framework. In the ﬁrst two tasks, the objective is
to cover the surface of the table (indicated by the red lines) with the cloth (shown in green). In theﬁrst task, the grippers (shown in blue) can freely move however the cloth is obstructed by a pillar.In the second task, the grippers must pass through a narrow passage before the table can be covered.In the third task, the robot must navigate a rope (shown in green in the top left corner) through athree-dimensional maze before covering the red points in the top right corner. The maze consistsof top and bottom layers (purple and green, respectively). The rope starts in the bottom layer andmust move to the target on the top layer through an opening (bottom left or bottom right)
the current conﬁguration. If we predict that the controller will get stuck we can then
invoke the global planner, moving the deformable object into a new neighbourhood
from which the local controller may be able to succeed. The key to our efﬁcient pre-
diction is forward-propagating only the stretching constraint, assuming the objectwill comply to contact.
We seek to solve problems where we need to arrange the object in a particular way
(e.g. covering a table with a tablecloth) but where there is also complex environmentgeometry preventing us from directly completing the task. While we can’t claim to
solve all problems in this class (in particular in environments where the deformable
object can get snagged), we can solve an important sub-class; in this work we restrictour focus to controllers of the form described in Sect. 4.1, and tasks suited to these
controllers. Examples of these types of tasks are shown in Fig. 1. In our experiments
we show that this iterative method of interleaving planning and control is able to suc-cessfully perform several interesting tasks for one-dimensional and two-dimensional
deformable objects (e.g. rope and cloth) where our planner or controller alone are
unable to succeed.
2 Related Work
Robotic manipulation of deformable objects has been studied in many contexts rang-
ing from surgery to industrial manipulation (see [ 16] for an extensive survey).
Motion planning for manipulation of deformable objects is an active area of
research [ 14]. Saha et al. [ 27] present a Probabilistic Roadmap (PRM [ 15]) that plans
for knot-tying tasks with rope. Rodriguez et al. [ 25] study motion planning in fully
deformable simulation environments. Their method, based on Rapidly-exploringRandom Trees (RRTs [ 18]), applies forces directly to an object to move it through
Interleaving Planning and Control for Deformable Object Manipulation 1021
narrow spaces while using the simulator to compute the resulting deformations.
Recently Frank et al. [ 9] presented a method that pre-computes deformation simu-
lations in a given environment to enable fast multi-query planning. Other sampling-based approaches [ 1,6,10,17,20,26] rely on being able to explicitly deﬁne the
goal region for the deformable object, whereas our approach uses an error-function-
based task deﬁnition. Also, the above methods rely on potentially time-consumingphysical simulation of the deformable object, while our method does not.
Model-based visual servoing approaches [ 11,29,31] bypass planning entirely,
using a local controller to determine how to move the robot end-effector for agiven task. Our recent work [ 3,19]a sw e l la s[ 22] bypass the need for an explicit
deformable object model, instead using Jacobian approximation controllers to drive
the deformable object to the attractor of the starting state. Rather than using onlya planner or only a controller, our framework uses both components, each when
appropriate. Park et al. [ 23] also interleave planning and control; but their controller
follows a trajectory generated by the planner rather than operating independently.
Approaches based on learning from demonstration [ 12,28] avoid planning and
deformable object modelling challenges entirely by using ofﬂine demonstrations to
teach the robot speciﬁc manipulation tasks; however, when a new task is attempted
a new training set needs to be generated. In our application we are interested in a
way to manipulate a deformable object without a high-ﬁdelity model or training setavailable a priori. For instance, imagine a robot encountering a new piece of clothing
for a new task. While it may have models for previously-seen clothes or training
sets for previous tasks, there is no guarantee that those models or training sets areappropriate for the new task.
Our planning method has some similarity to topological [ 4,13] and tethered
robot [ 5,30] planning techniques; these methods use the topological structure of the
space to deﬁne homotopy classes, either as a direct planning goal, or as a way to
help inform planning in the case of tethered robots. Planning for some deformable
objects, in particular rope or string, can be viewed as an extension of the tetheredrobot case where the base of the tether can move. This extension, however, requires
a very different approach to homotopy than is traditionally used, particularly when
working in three-dimensional space instead of a planar environment. In our work weuse visiblity deformations from [ 13] as a way to encode homotopy-like classes of
conﬁgurations.
3 Problem Statement
Let the robot be represented by a set of Ggrippers with conﬁguration q∈SE(3)G.
We assume that the robot conﬁguration can be measured exactly. In this work we
assume the robot to be a set of free ﬂoating grippers; in practice we can track the
motion of these with inverse kinematics. We use the Lie algebra [ 21]o f SE(3)to
represent robot gripper velocities. This is the tangent space of SE(3), denoted as
1022 D. McConachie et al.
se(3). The velocity of a single gripper gis then ˙qg=/bracketleftbigvT
gωT
g/bracketrightbigT∈se(3)where vg
andωgare the translational and rotational components of the gripper velocity. We
deﬁne the velocity of the entire robot to be ˙q=/bracketleftbig˙qT
1...˙qT
G/bracketrightbigT∈se(3)G.
The conﬁguration of a deformable object is a set P⊂R3ofP=|P|points. We
assume that we have a method of sensing P. We deﬁne a task based on a set of Ttarget
points T⊂R3, a function ρwhich measures the alignment error between Pand T,
and a termination condition Ω. Let a robot controller be a function C(q,P,T,ρ,Ω )
which maps the system state (q,P)and task (T,ρ,Ω )to a desired robot motion
˙qcmd. In this work we restrict our discussion to tasks and controllers of the form
introduced in our previous work [ 3,19]; these controllers are local, i.e. at each time
tthey choose an incremental movement ˙qcmdwhich reduces the alignment error as
much as possible at time t+1.
Let E(˙qcmd,q,P)=˙qactbe the true motion of the robot when ˙qcmdis executed
for unit time; in this work we will be predicting the future state of the system, thus
it is not sufﬁcient to consider only ˙qcmd, we must also consider ˙qact. Modelling
inaccuracies as well as manipulation in contact can lead to meaningful differences
between ˙qcmdand˙qact.
The problem we address in this work is how to ﬁnd a sequence of Nerobot
commands {˙qcmd,1,...,˙qcmd,Ne}=˙Qcmd such that each motion is feasible, i.e. it
should not bring the grippers into collision with obstacles, should not cause the
object to stretch excessively, and should not exceed the robot’s maximum velocity
˙qmax. Let these feasibility constraints be represented by A(˙q)=0. Then the problem
we seek to solve is:
ﬁnd ˙Qcmd
s.t. Ω(PNe)=true
A(˙qact,t)=0,t=1,..., Ne(1)
where PNeis the conﬁguration of the deformable object after executing ˙Qcmd.
Solving this problem directly is impractical in the general case for two major
reasons. First, modeling a deformable object accurately is very difﬁcult in the general
case, especially if it contacts other objects or self-collides. Second, even given a
perfect model, computing precise motion of the deformable object requires FEMsimulation and is very time consuming. We seek a method which does not rely on
high-ﬁdelity modelling and simulation; instead we present an incremental solution
which leverages the strengths of both global planning and local control in order toefﬁciently perform the task. This paper focuses on the case of using two grippers,
but our method could be extended to the multi-gripper case.
4 Interleaving Planning and Control
Global planners are effective at ﬁnding paths through complex conﬁguration spaces,but for highly underactuated systems such as deformable objects achieving a spe-
ciﬁc conﬁguration is very difﬁcult even with high-ﬁdelity models; this means that
Interleaving Planning and Control for Deformable Object Manipulation 1023
we cannot rely on them to complete a task independent of a local controller. In order
for the local controller to complete the task, the system must be in the correct basin
of attraction. From this point of view it is not the planner’s responsibility to completea task but rather to move the system into the right basin for the local controller to
ﬁnish the task. By explicitly separating planning from control we can use different
representations of the deformable object for each component; this allows us to usea highly-simpliﬁed model of the deformable object for global planning to generate
gross motion of the deformable object, while using an independent local approxima-
tion for the controller. The key question then is when should we use global planningversus local control? We answer this question by considering the limitations of a
given controller.
4.1 Local Control
For our local controller we use a controller of the form introduced in [ 3,19]. These
controllers locally minimize error ρwhile obeying the constraints of the system. For
every target point Ti∈Twe deﬁne a potential ﬁeld pointing towards Tiusing Dijk-
stra’s algorithm. This gives us the shortest collision-free path between any point in theworkspace and the target point, as well as the distance travelled along that path. These
potential ﬁelds are used to deﬁne the best direction to manipulate the deformable
object in order to locally reduce error as much as possible at each timestep. Thiserror reduction term is then combined with stretching avoidance and gripper colli-
sion avoidance methods to generate robot motion commands while obeying velocity
constraints. Full details can be found in [ 19].
One limitation of this approach is that the individual potential ﬁelds are deﬁned
and applied independently of each other; this means that the potential ﬁelds that
are combined to deﬁne the direction to move the deformable object can cause thecontroller to move the grippers on opposite sides of an obstacle, leading to poor
local minima. Examples of this scenario are shown in Figs. 2,4, and 5. In addition,
while this local controller can avoid obstacles, it does not explicitly have any abilitytogo around obstacles as shown in Fig. 3. In order to address these limitations we
introduce a novel deadlock prediction algorithm to detect when the system (q
t,Pt)
is in a state that will lead to deadlock if we continue to use the local controller.
4.2 Predicting Deadlock
Predicting deadlock is important for two reasons; ﬁrst we don’t want to waste time
executing motions that will not achieve the task. Second, we want to avoid the com-
putational expense of planning our way out of a cul-de-sac. By predicting deadlockbefore it happens we address both of these concerns.
1024 D. McConachie et al.
Algorithm 1 PredictDeadlock (qt,Pt,Bt,Path,T,Lmax,Np)
1: ConﬁgurationHistory ←[ ConﬁgurationHistory ,qt]
2: ErrorHistory ←[ ErrorHistory ,ρ( Pt)]
3:for n=0,..., Np−1do
4: ifPath/negationslash=∅ then
5: Pt+n+1←FollowV ectorField (Pt+n,T)
6: ˙qcmd,t+n+1← Csimple(qt+n,Pt+n,T,ρ,Ω )
7: qt+n+1← qt+n+˙qcmd,t+n
8: else
9: qt+n+1← qt+n+FollowPath(Path)
10: end if
11: Bt+n+1←ForwardPropagateBand (Bt+n,qt+n+1)
12: end for
13: ifPredictOverstretch (Lmax)orNoProgress ()then
14: return true
15: else
16: return false
17: end if
We consider a controller to be deadlocked if the commanded motion produces
(nearly) no actual motion, and the task termination condition is not met:
˙qact,t≈0 Ω(Pt)=false . (2)
In general we cannot predict if the system will get stuck in the limit; our controllers
and the system itself are complex, and as such we do not have a good way to evaluate
the attractor of the current state. Instead we predict if the system will get stuck within
a prediction horizon Nptimesteps. We divide our deadlock prediction algorithm into
three parts and discuss each in turn: (1) estimating gross motion; (2) predicting
overstretch; and (3) progress detection.
4.2.1 Estimating Gross Motion
The idea central to our prediction (Algorithm 1) is that while we may not be able to
determine precisely how a given controller will steer the system, we can capture the
gross motion of the system and estimate if the controller will be deadlocked. We splitthe prediction into two parts; ﬁrst we assume that controller Cis able to manipulate the
deformable object with a reasonable degree of accuracy within a local neighbourhood
of the current state. This allows us to approximate the motion of the deformableobject by following the task deﬁned potential ﬁelds for each P
i∈P. Examples of
this approximation are shown in Figs. 2,3,4and 5. Next we use a simpliﬁed version
ofCwhich omits the stretching avoidance term to predict the commands sent to the
robot. This term is omitted as it can be sensitive to the exact conﬁguration of the
deformable object, which is not considered in this approximation. If we are executing
a path then we can use the planned path directly to predict overstretch.
Interleaving Planning and Control for Deformable Object Manipulation 1025
Fig. 2 Sequence of snapshots showing the execution of the ﬁrst experiment. The cloth is shown
in green, the grippers are shown in blue, and the target points are shown as red lines. (1) The
approximate integration of the potential ﬁelds from error reduction over Nptimesteps, shown in
magenta, pull the cloth to opposite sides of the pillar. (2) A sequence of virtual elastic bands
between the grippers is shown in black and teal, indicating the predicted gripper conﬁguration over
the prediction horizon as the local controller follows the potential ﬁelds. The elastic band changes
to teal as the predicted motion of the grippers moves the cloth into an infeasible conﬁguration.
(3–5) The resulting plan by the RRT, shown in magenta and teal, moves the system into a new
neighbourhood. (6) Final system state when the task is ﬁnished by the local controller
4.2.2 Predicting Overstretch
Next we introduce the notion of a virtual elastic band between the grippers. This
virtual elastic band is based on Quinlan’s path deformation algorithm [ 24] and is
used both in deadlock prediction as well as global planning (Sect. 4.3) to determine
if a given gripper trajectory will stretch the deformable object beyond a task speciﬁed
maximum stretching factor λ. This is designed to detect when the deformable object
can get caught on an obstacle, preventing forward motion of the grippers. At each
timestep the elastic band is initialized with the shortest path between the grippers
through the deformable object, and then “pulled” tight using the internal contraction
force described in [ 24] and a hard constraint for collision avoidance. The endpoints
of the band track the predicted translation of the grippers. This band represents the
constraint that must be satisﬁed for the object not to tear. By considering only this
constraint on the object in prediction, we are implicitly relying on the object to
1026 D. McConachie et al.
Fig. 3 Sequence of snapshots showing the execution of the second experiment. We use the same
colors as the previous experiment (Fig. 2), but in this example instead of detecting future overstretch
in panel (2), we detect that the system is stuck in a bad local minimum and unable to make progress
comply to the contacts as it is moved by the grippers. We discuss the limitations of
this assumption in the discussion (Sect. 6).
Let Lt+nbe the length of the path deﬁned by the virtual elastic band Bt+nat
timestep nin the future, and Lmaxbe the longest allowable band length. To use this
length sequence to predict if the controller will overstretch the deformable object, we
perform three ﬁltering steps: an annealing low-pass ﬁlter, a ﬁlter to eliminate cases
where the band is in freespace, and the detector itself which predicts overstretch.
We use a low-pass annealing ﬁlter with annealing constant α∈[0,1)to mitigate
the effect of numerical and approximation errors which could otherwise lead to
unnecessary planning:
˜Lt+1=Lt+1
˜Lt+n=α˜Lt+n−1+(1−α)Lt+n,n=2,..., Np.(3)
Second, we discard from consideration any bands which are not in contact with
an obstacle; we can eliminate these cases because our local controller includes an
overstretch avoidance term which will prevent this case in general. Last we compare
the ﬁltered length of any remaining band predictions to Lmax; if after ﬁltering, there
is a estimated band length ˜Lthat is larger than Lmax then we predict that the local
Interleaving Planning and Control for Deformable Object Manipulation 1027
1: IniƟal state
7: Path execu Ɵon2: Deadlock predic Ɵon
8: Path ﬁnished3: Planned path
9: State reached by controller4: Path execu Ɵon 5: Path execu Ɵon 6: Path execu Ɵon
Fig. 4 Sequence of snapshots showing the execution of the third experiment. The rope is shown
in green starting in the top left corner, the grippers are shown in blue, and the target points are
shown in red in the top right corner. The maze consists of top and bottom layers (green and purple,
respectively). The rope starts in the bottom layer and must move to the target on the top layer
through an opening (bottom left or bottom right)
controller will be stuck. An example of this type of detection is shown in Fig. 2,
where the local controller will wrap the cloth around the pole, eventually becoming
deadlocked in the process.
4.2.3 Progress Detection
Last, we track the progress of the grippers and task error to estimate if the controller
Cis making progress towards the task goal. This is designed to detect cases when
the grippers themselves are trapped against an obstacle. Naively we could look for
instances when ˙qact=0 however due to sensor noise, actuation error, and using
discrete math in a computer, we need to use some threshold instead. At the same time
we want to avoid false positives, where the grippers are moving slowly however task
error is decreasing. To address these concerns we introduce three parameters: history
window Nh, error improvement threshold βe, and conﬁguration distance threshold
1028 D. McConachie et al.
2: PotenƟal ﬁelds 1: IniƟal state 3: Deadlock predic Ɵon 4: Planned path 5: Path ﬁnished
7: Planned path 6: Deadlock predic Ɵon 8: Path execu Ɵon 9: Path ﬁnished 10: Task ﬁnished
Fig. 5 Sequence of snapshots for the fourth experiment. We use the same colors as the previous
experiment (Fig. 4), but in this example the local controller gets stuck twice, in panels 3 and 6.
In panel 7 the global planner ﬁnds a new neighbourhood that is distinct from previously-tried
neighbourhoods
βm. If over the last Nhtimesteps, the improvement in error is less than βe, and the
grippers have moved less than βm, then we predict that the controller will not be able
to reach the goal from the current state and trigger global planning.
4.3 Global Planning
In order to enable efﬁcient planning, we need to approximate the conﬁguration of
the deformable object in a way that captures the gross motion of the deformable
object without being prohibitively expensive to use. We use the same approach from
Sect. 4.2.2 , but the interpretation in this use is slightly different; the virtual elastic
band is a proxy for the leading edge of the deformable object. In this way we can plan
to move the deformable object to a different part of the workspace without needing
to simulate the entire deformable object, instead the deformable object conforms to
the environment naturally.
We also do not consider the orientation of the grippers while planning, instead
treating the grippers as spheres. The resulting space that we are planning in is R3G×
V, where Vrepresents the space of virtual elastic band conﬁgurations. For our
global planner we use a conventional RRT-Connect approach as the central planning
mechanism (Algorithm 2). We cannot explicitly sample Vas the state of the band
depends on the path the grippers take, hence we sample only the position of the
grippers, forward-propagating the virtual elastic band from the nearest neighbor
towards the sampled gripper positions until the grippers collide with an obstacle, or
the length of the virtual elastic band Lexceeds Lmax.
Interleaving Planning and Control for Deformable Object Manipulation 1029
In order to make progress towards achieving the task, we want to set the goal for
the RRT to be a conﬁguration that we have not explored with the local controller.
We do so in two parts; we ﬁnd the set of all target points TUwhich are contributing
to task error, split these points into Gclusters, and use the cluster centers to deﬁne
the goal position of the grippers, qxyz,goal. Second, we set the goal conﬁguration of
the virtual elastic band to be any conﬁguration that is not similar to a blacklist of
virtual rubber bands. This blacklist is the set of all band conﬁgurations from which
we predicted that the local controller would be deadlocked in the future (Sect. 4.2).
To deﬁne similarity we use Jaillet and Siméon’s visibility deformation [13] def-
inition to compare two virtual elastic bands. Intuitively two virtual elastic bands
are similar if you can sweep a straight line connecting the two bands from the start
points to the end points of the two bands without intersecting an obstacle. Unlikethe original use, we do not constrain the start and end points of each path to match,
but the algorithm is identical. We use this as a heuristic to ﬁnd states that are dis-
similar from states where we have already predicted that the local controller wouldbe deadlocked. Let VisCheck (B,Blacklist )→{ 0,1}denote this visibility deforma-
tion check, returning 1 if Bis similar to a band in the blacklist and 0 otherwise.
Let R
goal={(qxyz,goal,Bgoal)|VisCheck (Bgoal,Blacklist )=0}be the goal region
deﬁned by the gripper target above.
Given that we are only sampling the position of the grippers and the use of
VisCheck to deﬁne the goal region, we need to carefully deﬁne the distance and
NearestNeighbour (Algorithm 3) functions. Let qxyz,rand be the sampled gripper
conﬁguration. Let Rtreebe the set of all nodes ri=(qxyz,i,Bi)in the tree. Then the
distance between the sample and a node in the tree is the Euclidean distance between
gripper positions:
d(qxyz,rand,ri)=/bardbl qxyz,rand−qxyz,i/bardbl. (4)
Let Rcomp={(qxyz,B)|qxyz=qxyz,goal}\ Rgoal be the set of all RRT conﬁgura-
tions whose gripper positions match the goal conﬁguration, but whose virtual elasticbands are similar to the Blacklist. Then the distance between any conﬁguration
r
comp∈Rcomp and any rgoal∈Rgoal is zero. If Rtree∩ Rcomp/negationslash=∅ then when the
goal is sampled, the closest node in the tree cannot be extended towards the goal.To address this issue, we maintain a set of nodes that we have extended towards
the goal R
nnbl. When qxyz,goal is selected as qxyz,rand, we ﬁrst consider any nodes
r∈Rtree\Rnnbl which we have not yet extended towards the goal, returning the
closest such node. If Rtree\Rnnbl=∅ then we have extended every node in the
tree towards the goal already. In this case, we default to standard nearest neighbor
behavior. While this possibility is remote it can happen, particularly near the start
of planning when there are few nodes in the tree. If the random sample is not the
goal, then we use standard nearest neighbour behavior, setting rnear to the closest
conﬁguration in Rtree.
The combination of local control, deadlock prediction, and global planning are
shown in the MainLoop function (Algorithm 4). Because the virtual elastic band isan approximation we need to predict deadlock while executing the planned path. We
use the same prediction method for path execution as for the local controller.
1030 D. McConachie et al.
Algorithm 2 PlanPath (qt,Pt,Bt,T,Lmax,Blacklist )
1:TU←UncoveredTargetPoints (Pt,T)
2:qxyz,goal←ClusterCenters (TU)
3:qxyz,goal←ProjectOutOfCollision (qxyz,goal)
4:rstart←(qxyz,t,Bt)
5:Rgoal←{(qxyz,goal,Bgoal)|VisCheck (Bgoal,Blacklist )=0}
6:Rnnbl←∅
7: Path ←RRTPlan (rstart,Rgoal,Rnnbl,Lmax)
8:ifPath/negationslash=Failure then
9: return ShortcutSmooth(Path)
10: else
11: return Failure
12: end if
Algorithm 3 NearestNeighbor (qrand,qxyz,goal,Rtree,Rnnbl)
1:if/bardblqrand−qxyz,goal/bardbl= 0and Rtree\Rnnbl/negationslash=∅ then
2: rnear←argminr∈Rtree\Rnnbld(qrand,r)
3: Rnnbl← Rnnbl∪{rnear}
4:else
5: rnear←argminr∈Rtreed(qrand,r)
6:end if
7:return (rnear,Rnnbl)
To set the maximum band length Lmaxused by the global planner and the dead-
lock prediction algorithms, we calculate the geodesic distance between the grippers
through the deformable object when it is relaxed and scale it by the task speciﬁed
maximum stretching factor λ.
5 Experiments and Results
We now present four example tasks to demonstrate our algorithm, two with cloth,
and two with rope. In the ﬁrst and second tasks, two grippers manipulate the cloth
so that it covers a table. In the ﬁrst task the cloth is obstructed by a pillar while in the
second task the grippers must pass through a narrow passage before the table can becovered. The third and fourth scenarios require the robot to navigate a rope through
three-dimensional maze before aligning the rope with a line traced on the ﬂoor (see
Fig. 1).
1
All experiments were conducted in the open-source Bullet simulator [ 7], with
additional wrapper code developed at UC Berkeley. The cloth is modeled as a triangle
mesh using 1500 vertices with a total size of 0 .3m×0.5 m. The rope is modeled as
a series of small capsules linked together by springs. In the ﬁrst rope experiment we
use 39 capsules for a 0.78 m long rope, and 47 capsules for a 0.94 m rope in the last
1The video accompanying this paper ( https://youtu.be/548ypyLOgPA ) shows the task executions.
Interleaving Planning and Control for Deformable Object Manipulation 1031
Ta b l e 1 Deadlock prediction
parametersPrediction horizon Np 10
Band annealing factor α 0.3
History window Nh 100
Error improvement threshold βe 1
Conﬁguration distance threshold βm 0.03
experiment. We emphasize that our method does not have access to the model of the
deformable object or the simulation parameters. The simulator is used as a “blackbox” for testing. We set the maximum stretching factor λto 1.17 for the cloth and
1.1 for the rope. All tests are performed using an i7-7700K processor. We use the
same deadlock prediction parameters for all tasks, shown in Table 1.
Algorithm 4 MainLoop (λ, Np)
1:D←GeodesicDistanceBetweenGrippers (Pre laxed ) //
2:Lmax←λD;t←0 // Initialization
3: Blacklist ←∅ ;P a t h ←∅ //
4:q0←SenseRobotConﬁg ();P0←SensePoints () //
5:while ¬Ω(Pt)do
6: T←GetTargets (Pt)
7: Bt←InitializeBand (Pt)
8: ifPredictDeadlock (qt,Pt,Bt,Path,T,Lmax,Np)then
9: Blacklist ←Blacklist ∪{Bt} //
10: Path ←PlanPath (Pt,Bt,T,Lmax,Blacklist ) //
11: ifPath = Failure then // Global planning
12: return Failure //
13: end if //
14: end if
15: ifPath/negationslash=∅ then
16: ˙qcmd←FollowPath(Path)
17: ifPathFinished(Path) then
18: Path ←∅
19: end if
20: else
21: ˙qcmd← C(qt,Pt,T,ρ,Ω) // Local controller
22: end if
23: CommandConﬁguration (qt+˙qcmd) //
24: qt+1←SenseRobotConﬁg ();Pt+1←SensePoints () // Update
25: t← t+1/ /
26: end while
27: return Success
To smooth the path returned by the RRT, at each iteration we randomly select
either a single gripper or both grippers and two conﬁgurations in the path. To smooth
between the conﬁgurations we use the same forward-propagation method for the
virtual elastic band as used in the planning process. If we have selected only one
1032 D. McConachie et al.
gripper for smoothing, we do not change the conﬁguration of the second gripper
during that smoothing iteration. We also forward-propagate the virtual elastic band
to the end of the path to ensure that the band at the end of the smoothed path isdissimilar from the blacklist. We perform 500 smoothing iterations for experiments
1, 2, and 4; and 1500 for experiment 3 due to the larger environment.
5.1 Single Pillar
In the ﬁrst example task, the objective is to spread the cloth across a table that is
on the far side of a pillar (see Fig. 2). We uniformly discretize the surface of the
table to create the target points T, with each discretized point creating a potential
ﬁeld that pulls the closest point on the deformable object towards the target. These
target points are set slightly above the surface to allow for collision margins withinthe simulator. A single point on the cloth can have multiple “pulls” or none. Task
errorρis deﬁned as the sum of the Dijkstra’s distances from each target point to the
closest point on the cloth. If a target point in Tis within a small-enough threshold of
their nearest neighbors in P, then these points are considered “covered” and do not
inﬂuence task error or any other calculation. Our results show that even though theglobal planner is only planning using the gripper positions and a virtual elastic band
between them, it is able to ﬁnd the correct neighbourhood for the local controller to
complete the task. On average we are able to ﬁnd and smooth a path in well under10 s (Table 3), with the majority of the time spent on forward propagation of the
virtual elastic band as part of the validity check for a potential movement of the
grippers. In all 10 trials the global planner is only invoked once, with the localcontroller completing the task after the plan ﬁnishes. The average times for the local
controller and deadlock prediction algorithms are shown in Table 2, averaged across
all trials of all experiments.
5.2 Double Slit
The second experiment uses the same setup as the ﬁrst, with the only change beingreplacing the single pillar obstacle with a wide wall with two narrow slits (Fig. 3).
Ta b l e 2 Local controller and deadlock prediction avg. computation time per iteration for each type
of deformable object, averaged across all trials
GetTargets() time (s) Predict Deadlock() time (s) Local controller time (s)
Cloth 0.0108 0.0136 0.0056
Rope 0 0.0216 0.0029
Interleaving Planning and Control for Deformable Object Manipulation 1033
Ta b l e 3 Planning statistics for the ﬁrst plan for each example task, averaged across 10 trials
RRT planning Smoothing
Samples States NN time
(s)V aliditychecking
time (s)Total
time (s)Iterations V aliditychecking
time (s)Visibility
deforma-
tion time
(s)Totaltime (s)
Singlepillar 369 2401 ∼0.0 2.9 3.0 500 2.4 ∼0.0 2.5
Double
slit723 3204 ∼0.0 3.0 3.1 500 6.2 ∼0.0 6.2
Rope
maze5099 9886 0.2 9.7 10.1 1500 12.6 ∼0.0 12.8
Repeated
planning578 1626 0.1 2.9 3.0 500 4.7 ∼0.0 4.7
This adds a narrow passage problem and also demonstrates the utility of the progress
detection ﬁlter. In this example the local controller is trying to move the deformable
object straight forward, but with the wall in the way it is unable to make progress; thelocal controller cannot explicitly go around obstacles. This experiment shows com-
parable planning time, but it takes longer to smooth the resulting path (as expected
given that the virtual elastic band forward propagation takes longer near obstacles).The local controller is again able to complete the task after invoking the planer a
single time on all 10 trials.
5.3 Moving a Rope Through a Maze
In the third task, the robot must navigate a rope through a three-dimensional maze
before aligning the rope with a line traced on the ﬂoor (Fig. 4). In this task, the
correspondences between the target points Tand the deformable object points Pare
ﬁxed in advance, thus the GetTargets function does not have to do any work, as shownin Table 2. Task error ρis deﬁned in the same way as in the ﬁrst two experiments.
Again the planner is invoked a single time per trial, and planning time is comparable
to the previous tasks, but we do spend more time smoothing for this example. Thisis a function of the size of the environment rather than any particular difference
in the difﬁculty of performing the smoothing. This task has the most potential for a
planned path to move the deformable object into a conﬁguration from which the localcontroller cannot ﬁnish the task (by wrapping the rope around an obstacle near the
goal), however our framework is able to address this as shown in the next example.
1034 D. McConachie et al.
5.4 Repeated Planning
The fourth task is a variant of the third, with the start point for the rope moved near
the goal region on the top layer of the maze. For this experiment we reduce the sizeof the planning arena to only the goal area, and the immediate surroundings on the
top layer (Fig. 5). From this starting position, the planner is more likely to ﬁnd the
incorrect neighbourhood for the local controller on the ﬁrst attempt. In 4 of the 10trials, the planner was invoked twice, in 4 other trials it was invoked three times,
and in one trial it was invoked four times. These additional planning and smoothing
stages took on average an additional 40.9 s, but the task was completed successfullyin all 10 trials.
6 Discussion and Conclusion
We have presented a method to interleave global planning and local control fordeformable object manipulation that does not rely on high-ﬁdelity modeling or sim-ulation of the object. Our method combines techniques from topologically-based
motion planning with a standard RRT implementation in order to generate gross
motion of the deformable object. The purpose of this gross motion is not to achievethe task alone, but rather to move the object into a position from which the local con-
troller is able to complete the task. This division of labour enables each component
to focus on their strengths rather than attempt to solve the entire problem directly.As part of our framework, we introduced a novel deadlock prediction algorithm
to determine when to use the local controller and when to use the global planner.
Our experiments demonstrate that our framework is able to be applied to severalinteresting tasks for rope and cloth, including an adversarial case where we setup
the planner to fail on the ﬁrst attempt. In principle, these techniques can be used to
extend our work beyond two grippers by using a virtual rubber band between every
pair of grippers, however we have not experimented with such a task.
While we have included elements to handle the need to plan multiple times,
there are several issues left unaddressed. In particular environments with “hooks”
can cause problems due to our approximation methods; the virtual elastic band we
use for constraint checking and planning assumes that there is no minimum lengthof the deformable object. This assumption means that our planner cannot detect
cases where the excess material can get snagged on corners or hooks, preventing the
motion plan from being executed. In addition we have no explicit method to avoidtwisting or knot-tying behaviour. While shortcut smoothing can potentially mitigate
the worst effects, this is not something that is within the scope of this work. Last, we
cannot guarantee that we can achieve any given task in general; our sampling andnearest neighbour functions do not guarantee probabilistic completeness, and while
our blacklisting method is designed to encourage exploration of the state space,
it also has the potential to block regions of the state space from which the local
Interleaving Planning and Control for Deformable Object Manipulation 1035
controller can achieve the task. Despite these limitations we ﬁnd that our framework
is able to reliably perform complex tasks where neither planning nor control alone
are sufﬁcient.
In future work we plan to address these weaknesses, in particular the snagging
and twisting limitations which are artifacts of our approximation methods. We also
seek to extend our framework to a broader range of tasks, beyond coverage and pointmatching applications.
References
1. Anshelevich, E., Owens, S., Lamiraux, F., Kavraki, L.: Deformable volumes in path planning
applications. In: ICRA (2000)
2. Bai, Y ., Y u, W., Liu, C.K.: Dexterous manipulation of cloth. Comput. Graph. Forum 35(2),
523–532 (2016)
3. Berenson, D.: Manipulation of deformable objects without modeling and simulating deforma-
tion. In: IROS (2013)
4. Bhattacharya, S., Likhachev, M., Kumar, V .: Topological constraints in search-based robot path
planning. Auton. Robot. 33(3), 273–290 (2012)
5. Brass, P ., Vigan, I., Xu, N.: Shortest path planning for a tethered robot. Comput. Geom. 48(9),
732–742 (2015)
6. Burchan Bayazit, O., Jyh-Ming, L., Amato, N.: Probabilistic roadmap motion planning for
deformable objects. In: ICRA (2002)
7. Coumans, E.: Bullet physics library. Open source: https://www.bulletphysics.org (2010)
8. Essahbi, N., Bouzgarrou, B.C., Gogu, G.: Soft material modeling for robotic manipulation. In:
Applied Mechanics and Materials (2012)
9. Frank, B., Stachniss, C., Abdo, N., Burgard, W.: Efﬁcient motion planning for manipulation
robots in environments with deformable objects. In: IROS (2011)
10. Gayle, R., Lin, M., Manocha, D.: Constraint-based motion planning of deformable robots. In:
ICRA (2005)
11. Hirai, S., Wada, T.: Indirect simultaneous positioning of deformable objects with multi-pinching
ﬁngers based on an uncertain model. Robotica 18(1), 3–11 (2000)
12. Huang, S.H., Pan, J., Mulcaire, G., Abbeel, P .: Leveraging appearance priors in non-rigid
registration, with application to manipulation of deformable objects. In: IROS (2015)
13. Jaillet, L., Siméon, T.: Path deformation roadmaps: compact graphs with useful cycles for
motion planning. Int. J. Robot. Res. 27(11–12), 1175–1188 (2008)
14. Jiménez, P .: Survey on model-based manipulation planning of deformable objects. Robot.
Comput.-Integr. Manuf. 28(2), 154–163 (2012)
15. Kavraki, L.E., Svestka, P ., Latombe, J.C., Overmars, M.H.: Probabilistic roadmaps for path
planning in high-dimensional conﬁguration spaces. IEEE Trans. Robot. Autom. 12(4), 566–580
(1996)
16. Khalil, F., Payeur, P .: Dexterous robotic manipulation of deformable objects with multi-sensory
feedback – a review. In: In-Teh (ed.) Robot Manipulators, Trends and Development, chap. 28,pp. 587–621 (2010)
17. Lamiraux, F., Kavraki, L.E.: Planning paths for elastic objects under manipulation constraints.
Int. J. Robot. Res. 20(3), 188–208 (2001)
18. LaV alle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)
19. McConachie, D., Berenson, D.: Bandit-Based Model Selection for Deformable Object Manip-
ulation. W AFR (2016)
20. Moll, M., Kavraki, L.E.: Path planning for deformable linear objects. IEEE Trans. Robot. 22(4),
625–636 (2006)
1036 D. McConachie et al.
21. Murray, R.M., Li, Z., Sastry, S.S.: A Mathematical Introduction to Robotic Manipulation, vol.
29. CRC Press, Boca Raton (1994)
22. Navarro-Alarcon, D., Liu, Y .h., Romero, J.G., Li, P .: On the visual deformation servoing of
compliant objects: uncalibrated control methods and experiments. Int. J. Robot. Res. 33(11),
1462–1480 (2014)
23. Park, D., Kapusta, A., Hawke, J., Kemp, C.C.: Interleaving planning and control for efﬁcient
haptically-guided reaching in unknown environments. In: Humanoids (2014)
24. Quinlan, S.: Real-time modiﬁcation of collision-free paths. Ph.D. thesis, Department of Com-
puter Science, Stanford University (1994)
25. Rodriguez, S., Amato, N.: An obstacle-based rapidly-exploring random tree. In: ICRA (2006)26. Roussel, O., Borum, A., Taïx, M., Bretl, T.: Manipulation planning with contacts for an exten-
sible elastic rod by sampling on the submanifold of static equilibrium conﬁgurations. In: ICRA(2015)
27. Saha, M., Isto, P ., Latombe, J.C.: Motion planning for robotic manipulation of deformable linear
objects. In: Proceedings of the International Symposium On Experimental Robotics (ISER)(2006)
28. Schulman, J., Ho, J., Lee, C., Abbeel, P .: Learning from demonstrations through the use of non-
rigid registration. In: Springer Tracts in Advanced Robotics, vol. 114, pp. 339–354. Springer
International Publishing, Berlin (2016)
29. Smolen, J., Patriciu, A.: Deformation planning for robotic soft tissue manipulation. In: 2009
Second International Conferences on Advances in Computer-Human Interactions, pp. 199–204(2009)
30. Soonkyum Kim, Likhachev, M.: Path planning for a tethered robot using Multi-Heuristic A\*
with topology-based heuristics. In: IROS (2015)
31. Wada, T., Hirai, S., Kawarnura, S., Karniji, N.: Robust manipulation of deformable objects by
a simple PID feedback. In: ICRA (2001)
Controlling Muscle-Actuated Articulated
Bodies in Operational Space
Samir Menon, Takatoki Migimatsu and Oussama Khatib
1 Introduction
Muscle-actuated control of articulated body systems in operational space remains
challenging due to the lack of analytical methods for kinematics and dynamics reso-lution. A fundamental problem is resolving the redundancy of complex articulation
mechanisms in the context of non-negative actuation by (even more redundant) mus-
cle actuators. A second fundamental problem is to maintain continuity of the kine-
matics and dynamics redundancy resolution over a given time-window—a ‘good’
solution must resolve redundancy in a manner that limits noise in higher deriva-tives. Such mechanisms include muscle-actuated robots, as well as models of the
human musculoskeletal system. Using the latter as an exemplar, the fundamental
problems we discussed can resolve to: developing a differentiable model of musclearticulation; computing the muscle-to-generalized coordinate Jacobian; computing
muscle-to-operational space Jacobians; and computing task-to-muscle Jacobians.
Finally, we note that it is important to do the previous computations in a mannerthat the re-computation of local linearization from time-step to time-step does not
introduce large changes. The last point is important to develop stable control and
simulation.
Past research in linear (muscle) actuation has used a variety of methods to resolve
kinematics and dynamics. A few approaches attempted explicit scalar resolution of
the geometry [ 11,21]. Other approaches used learning [ 14] or optimization [ 15,28,
29]. Some approaches used hand engineered solutions [ 6]. Others designed (robotic)
S. Menon (B)·T. Migimatsu ·O. Khatib
Stanford Robotics Lab, Stanford University, Stanford, CA, USAe-mail: smenon@cs.stanford.edu
T. Migimatsu
e-mail: takatoki@stanford.edu
O. Khatib
e-mail: ok@cs.stanford.edu
© Springer Nature Switzerland AG 2020
N. M. Amato et al. (eds.), Robotics Research , Springer Proceedings
in Advanced Robotics 10, https://doi.org/10.1007/978-3-030-28619-4\_691037
1038 S. Menon et al.
systems that had tractable analytic solutions [ 8,21,25,27]. While many models
deconstruct muscles to ﬁber-like line segments [ 15,29,30], some attempt to capture
muscle dynamics in the continuum setting [ 13]. In addition to models, there has also
been much research in muscle actuated control, particularly of position and stiffness
[19,27]. A particularly interesting set of work studied somatosensory estimation in
complex human biomechanical models; its contributions include a muscle Jacobianspeciﬁcation along with quadratic and linear programming optimization formulations
[20].
In this paper, we outline a modeling approach that locally linearizes nonlinear
muscle-actuation, which enables us to include muscle-actuated control in the opera-
tional space control framework. We start by assuming that muscles in any system of
interest—robotic or biomechanical—can be modeled with polynomial functions.Since polynomials can be approximated with piece-wise linear segments within
bounded error [ 12], we developed a segmented muscle model that is differentiable
and admits an analytical Jacobian (similar to Jacobians derived in [ 20]). Using the
muscle Jacobian within the operational space control framework, we obtain dynam-
ics and control expressions for muscle-actuated articulated bodies. In particular, we
use acceleration consistency of kinematics and dynamics to ﬁnd closed form solu-
tions for muscle-force to task-force (‘forward’) maps. We then outline an iterative
algorithm that uses an initial seed to solve the KKT conditions for constrained non-negative optimization, and apply this algorithm to compute task-force to muscle-force
(‘inverse’) maps. Finally, we discuss the convergence of our non-negative muscle
Jacobian inversion algorithm with randomized Jacobians as well as a set of detailedhuman upper-body musculoskeletal models.
2 Modeling Priorities
Our modeling aims are twofold: (i)to develop a precise muscle actuation model for
robots where the articulation mechanism is well known and can be modeled using
available kinematics and dynamics methods; and (ii)to develop computationally efﬁ-
cient and numerically stable models for human musculoskeletal actuation where the
actual articulation mechanism is unknown. In the latter case, our goal is for the models
to work well even with indirect kinematic measurements like physically measurabletask-space limb positions. Since such measurements cannot observe variables like
dynamic bone-to-bone joint changes, muscle warping, deformation, and sliding, our
goal is to ensure that models do not require these measurements in order to be robust.
Our models thus aim to avoid overﬁtting, and to enable analyses without requiring
complex (human muscle) state estimation methods.
Given our goals, we prioritize the following model and computation characteris-
tics:
1. Determinism
2. Analytical solutions with efﬁcient algorithms
Controlling Muscle-Actuated Articulated Bodies in Operational Space 1039
3. Bounded uniform error
4. Differential maps from actuators to task space
5. Controllable numerical errors across simulation time-steps
3 Modeling Musculoskeletal Kinematics
Modeling muscle actuated articulated bodies with line-segment ﬁber-like muscleactuators has been a standard practice in biomechanics [ 7,15,20,28,29], yet
the dynamic properties of such models can be complicated to analyze. Existing
models often have many parameters and are either are nondeterministic, expensive tosimulate, or potentially numerically unstable. Simpliﬁed models, in contrast, often
do not capture the richness of muscle actuation. We will now attempt to strike a
balance.
3.1 Articulated Body Model : Articulated Links with Joints
We brieﬂy outline our notation for modeling the skeleton as an articulated body
structure.
Deﬁnition 1 An articulated body with ndegrees-of-freedom is a set of links, /lscript=
/lscript−1,/lscript0,...,/lscript nwhere any two connected links, /lscriptaand/lscriptbare separated by a one degree-
of-freedom joint,ajb. The ﬁrst or root link, /lscript−1, is assumed to be ﬁxed with respect
to an inertial frame. All links are connected to the root with joints in an undirected
chain, or a tree topology.1Each joint thus has a pre-link, which is separated by fewer
degrees-of-freedom from the root, and a post-link (the other).
Next, we assume that the articulated body’s state, its conﬁguration, and its
velocity, can be completely described by a suitable set of generalized coordinates,
q=q0,q1,..., qn, and generalized velocities, ˙q=˙q0,˙q1,...,˙qn, which are mono-
tonic functions of parameters associated with the joint degree-of-freedom. As such,
the articulated body dynamics may be resolved using a variety of algorithmicmethods. To introduce muscle actuation, we will analytically compute the muscu-
lar kinematics, dynamics, and actuation, and then project them into the generalized
coordinates. Thus the existing algorithmic dynamics methods will resolve the effectof muscles implicitly.
1We ignore graph topologies at the moment. Accommodating those is feasible using our methods
in combination with constraint solving dynamics algorithms. See [ 9] for an overview.
1040 S. Menon et al.
3.2 Muscle Model: Line-Segment Actuators
We now develop a framework for muscle actuation that is computationally efﬁcient,
deterministic, and yet allows arbitrary precision while modeling biological muscu-
loskeletal systems. We begin with the notion of a muscle system, or a set of muscles
that actuate an articulated body:
Deﬁnition 2 Let{M0,M1,...,Mm}be a set of muscles that actuate an articulated
body. Each muscle is modeled as a force generating element. Muscles only apply
contractile forces.
To model such muscles, we now characterize a method that approximates muscles
with one or more segments arranged in a piece-wise linear geometry (Fig. 1):
Deﬁnition 3 A k-segmented muscle is a continuous piecewise linear muscle func-
tion,ˆfk, that approximates a given muscle function fusing at most ksegments with
a bounded

Jaime F. Fisac
Monica A. Gates
Jessica B. Hamrick
Chang Liu
Dylan Hadfield-Menell
Malayandi Palaniappan
Dhruv Malik
S. Shankar Sastry
Thomas L. Griffiths
Anca D. Dragan