Provably Safe Tolerance Estimation for Robot Arms via Sum-of-Squares Programming

Weiye Zhao, Suqin He, Changliu Liu

TL;DR

This paper introduces a novel algorithm for estimating the "joint tolerance" of robot arms—the maximum amount a robot's joints can deviate from a target pose without hitting an obstacle. By leveraging Sum-of-Squares (SOS) programming and the Positivstellensatz theorem, the authors transform a complex problem with infinite constraints into a solvable nonlinear program that provides a provably safe lower bound for robot operations.

What Problem Does This Paper Address?

In robotics, precision is rarely perfect. Sensors have noise, and controllers have tracking errors. To operate safely, a robot needs to know its tolerance: how much can its joint angles ($\theta$) deviate from a reference trajectory before a collision occurs?

Mathematically, this is a "local positiveness" problem. We want to find the largest radius $\lambda$ such that every possible state within that radius satisfies a safety constraint (e.g., distance to a wall $> 0$). This is notoriously difficult because:

  1. Infinite Constraints: You must ensure safety for every point in the neighborhood, not just a few samples.
  2. Non-linearity: Robot kinematics involve trigonometric functions (sines and cosines), making the distance functions highly non-linear and non-convex.
  3. Conservatism vs. Speed: Existing methods like reachability analysis are often too slow for real-time use or too conservative (providing bounds that are much smaller than necessary).

Methods

The authors propose the Joint Tolerance Estimation (JTE) algorithm, which follows a four-step pipeline:

  1. Polynomial Approximation: Since SOS programming requires polynomial inputs, the authors use Taylor expansion (small-angle approximation) to convert the trigonometric forward kinematics of the robot into a multivariate polynomial.
  2. Refute Certification: Instead of trying to prove a set is safe, they define a "refute set" (the set of all unsafe points) and aim to prove it is empty.
  3. Positivstellensatz Theorem: This is the "secret sauce." It’s a powerful tool from algebraic geometry that provides an equivalent condition for a set of polynomial inequalities to have no solution. It allows the authors to turn the "empty set" condition into a feasibility problem involving Sum-of-Squares polynomials.
  4. Nonlinear Programming (NLP) Conversion: To make the problem computationally tractable, they simplify the SOS search into a standard nonlinear program. This allows them to use off-the-shelf solvers like MATLAB’s fmincon to find the optimal tolerance $\lambda$ quickly.

Datasets

The paper does not use standard machine learning datasets. Instead, it evaluates the algorithm on two simulated robotic platforms:

  • 2-DOF Planar Robot: A simple two-link arm working in a 2D plane.
  • 6-DOF YASKAWA GP50: A standard industrial robot arm working in 3D Cartesian space.
  • Constraints: The robot was tested against various "half-plane" obstacles (walls aligned with axes or at general angles).

Key Insights and Ideas

  • Provable Safety: Unlike sampling-based methods that might miss a collision point, this approach provides a lower bound. If the algorithm returns a tolerance of $0.03$ radians, any deviation smaller than that is mathematically guaranteed to be safe (within the limits of the polynomial approximation).
  • Local vs. Global: Most SOS applications look for global properties (like Lyapunov stability). This paper specifically adapts the framework for local tolerance, which is more practical for collision avoidance.
  • Linear Scalability: The authors prove (Theorem 3) that if there are multiple obstacles, you can calculate the tolerance for each one independently and simply take the minimum. This means the computational cost grows linearly with the number of obstacles, rather than exponentially.

Key Results

  • Zero Violations: In 10,000 random samples taken within the calculated tolerance bounds, there were 0% safety violations.
  • Tight Bounds: The "smallest distance" to obstacles in the 3D tests was often in the millimeter range (e.g., 0.0014m), indicating that the algorithm finds a very "tight" bound that isn't overly conservative.
  • Speed:
    • The 2D problem solved in ~0.6 to 0.8 seconds.
  • The 6-DOF 3D problem solved in ~1.5 to 6.8 seconds. While not yet "high-frequency" real-time, it is fast enough for many trajectory planning and verification tasks.

Limitations and Future Work

  • Small Angle Assumption: The method relies on Taylor expansion. While the authors show the error is $<1\%$ for the tolerances found in their tests ($<0.244$ rad), the approximation could break down if the required tolerance is very large.
  • Differentiability: The current framework assumes safety constraints are differentiable. The authors note that investigating non-differentiable constraints (like those involving complex mesh geometries) is a direction for future research.
  • Approximation Proof: While the algorithm is shown to be safe empirically, a rigorous proof that the polynomial approximation $g(x)$ always remains a lower bound of the true kinematics $f(x)$ was left for future work.

Summary updated

Abstract

Tolerance estimation problems are prevailing in engineering applications. For example, in modern robotics, it remains challenging to efficiently estimate joint tolerance, i.e., the maximal allowable deviation from a reference robot state such that safety constraints are still satisfied. This letter presents an efficient algorithm to estimate the joint tolerance using sum-of-squares programming. It is theoretically proved that the algorithm provides a lower bound of the joint tolerance. Extensive numerical studies demonstrate that the proposed method is computationally efficient and near optimal.
BibTeX
@inproceedings{zhao2022provably,
  author = {Weiye Zhao and Suqin He and Changliu Liu},
  title = {{Provably Safe Tolerance Estimation for Robot Arms via Sum-of-Squares Programming}},
  booktitle = {IEEE Control. Syst. Lett},
  year = {2022},
  url = {https://openreview.net/forum?id=0evkQmonVMQ},
}

Loading related content...

Figure