[Paper Review] Can't Touch This: Real-Time, Safe Motion Planning and Control for Manipulators Under Uncertainty

Jonathan Michaux, Patrick Holmes+8 more|arXiv (Cornell University)|Jan 30, 2023

Formal Methods in Verification4 citations

TL;DR

This paper proposes ARMOUR, a real-time, receding-horizon motion planning and control framework for robotic manipulators that guarantees continuous-time safety under set-based uncertainty in inertial parameters. By combining robust passivity-based control, polynomial zonotope-based reachability analysis, and optimization with overapproximated swept volumes, ARMOUR ensures collision avoidance, joint limit satisfaction, and torque constraint compliance, achieving 92% success in simulation and safe real-world operation on a Kinova Gen3 with uncertain payloads.

ABSTRACT

Ensuring safe, real-time motion planning in arbitrary environments requires a robotic manipulator to avoid collisions, obey joint limits, and account for uncertainties in the mass and inertia of objects and the robot itself. This paper proposes Autonomous Robust Manipulation via Optimization with Uncertainty-aware Reachability (ARMOUR), a provably-safe, receding-horizon trajectory planner and tracking controller framework for robotic manipulators to address these challenges. ARMOUR first constructs a robust controller that tracks desired trajectories with bounded error despite uncertain dynamics. ARMOUR then uses a novel recursive Newton-Euler method to compute all inputs required to track any trajectory within a continuum of desired trajectories. Finally, ARMOUR over-approximates the swept volume of the manipulator; this enables one to formulate an optimization problem that can be solved in real-time to synthesize provably-safe motions. This paper compares ARMOUR to state of the art methods on a set of challenging manipulation examples in simulation and demonstrates its ability to ensure safety on real hardware in the presence of model uncertainty without sacrificing performance. Project page: https://roahmlab.github.io/armour/.

Motivation & Objective

• Address the challenge of ensuring continuous-time safety in robotic manipulators operating under uncertain inertial dynamics, such as unknown payload masses and inertias.
• Overcome the limitations of existing methods that fail to guarantee safety in real-time when dynamics are uncertain or model inaccuracies exist.
• Develop a framework that integrates real-time planning with robust control to ensure safety without sacrificing performance or requiring perfect dynamic models.
• Enable safe operation in complex, cluttered environments where traditional planners may fail due to non-convex constraints or unsafe trajectory convergence.
• Guarantee that all safety constraints—collision avoidance, joint limits, torque limits—are satisfied continuously in time, not just at discrete steps.

Proposed method

• Formulate a robust passivity-based controller that uniformly bounds the worst-case tracking error across all possible uncertainties in the robot’s and object’s inertial parameters.
• Introduce a novel recursive Newton-Euler algorithm (PZRNEA) to compute the set of all possible control inputs required to track any desired trajectory within a continuum of trajectories.
• Use polynomial zonotopes to represent and over-approximate the forward reachable set (swept volume) of the manipulator under uncertainty, enabling efficient and conservative collision checking.
• Construct a nonlinear optimization problem that incorporates continuous-time constraints for obstacle avoidance, joint position, velocity, and torque limits using overapproximated reachable sets.
• Implement a receding-horizon framework where the planner re-optimizes at each time step, incorporating a fail-safe braking maneuver in every planned trajectory to ensure long-term safety.
• Leverage polynomial zonotope arithmetic to efficiently propagate uncertainty through the dynamics and compute conservative bounds on the robot’s motion envelope.

Experimental results

Research questions

• RQ1 Can a real-time motion planning and control framework guarantee continuous-time safety for manipulators under set-based uncertainty in inertial parameters?
• RQ2 How can robust control be combined with reachability analysis to ensure bounded tracking error despite uncertain dynamics?
• RQ3 Can polynomial zonotopes be effectively used to over-approximate the swept volume of a manipulator in real-time while preserving computational tractability?
• RQ4 To what extent can this framework outperform state-of-the-art methods in terms of safety and success rate under model uncertainty?
• RQ5 Is it possible to embed a fail-safe braking maneuver into every planned trajectory to ensure long-term safety without compromising performance?

Key findings

• ARMOUR achieved 92% success rate in 100 random obstacle scenarios with a straight-line high-level planner, with zero crashes and no safety violations.
• In the challenging Hard Scenarios set, ARMOUR with RRT* high-level planning completed 6 out of 7 tasks safely, outperforming ARMTD, which completed only 5/7 under the same conditions.
• ARMOUR successfully operated in real-world experiments on a Kinova Gen3 robot manipulating a dumbbell with uncertain mass, demonstrating safe, real-time control without crashes.
• The average planning iteration time was 364.9 ms, confirming real-time feasibility on standard hardware.
• CHOMP, while always generating dynamically feasible trajectories, failed in 13% of cases due to collisions, highlighting the lack of built-in collision guarantees.
• The use of polynomial zonotopes enabled tighter overapproximation of the reachable set than prior methods, allowing for closer navigation around obstacles without violating safety constraints.