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 paper presented an efficient algorithm to estimate the joint tolerance using sum-of-squares programming. It is theoretically proved that the algorithm provides a tight lower bound of the joint tolerance. Extensive numerical studies demonstrate that the proposed method is computationally efficient and near optimal. The algorithm is implemented in the JTE toolbox and is available at https://github.com/intelligent-control-lab/Sum-of-Square-Safety-Optimization.
Introduction: The Challenge of Joint Tolerance
In robotics, precision is rarely absolute. Sensors are subject to noise, and controllers inevitably exhibit tracking errors. To operate safely in environments with obstacles, a robot must determine its joint tolerance: the maximum amount its joint angles ($\theta$) can deviate from a reference pose without causing a collision.
Mathematically, this is framed as a "local positiveness" problem. The goal is to find the largest radius $\lambda$ such that every possible state within that neighborhood satisfies a safety constraint, such as maintaining a positive distance from an obstacle. This problem is difficult due to infinite constraints (safety must be guaranteed for every point in the set), non-linear kinematics involving trigonometric functions, and the trade-off between computational speed and conservatism.
The Joint Tolerance Estimation (JTE) Algorithm
The authors propose the Joint Tolerance Estimation (JTE) algorithm, which leverages algebraic geometry to transform this complex problem into a solvable nonlinear program. The pipeline consists of four key steps:
- Polynomial Approximation: SOS programming requires polynomial inputs. The authors use Taylor expansion to convert trigonometric forward kinematics into multivariate polynomials.
- Refute Certification: Rather than proving safety directly, the algorithm defines a "refute set" of unsafe points and aims to prove this set is empty.
- Positivstellensatz Theorem: This theorem from algebraic geometry provides an equivalent condition for a set of polynomial inequalities to have no solution. It allows the "empty set" condition to be reformulated as a feasibility problem involving Sum-of-Squares (SOS) polynomials.
- Nonlinear Programming (NLP) Conversion: The SOS search is simplified into a standard nonlinear program, allowing the use of efficient solvers like MATLAB’s
fminconto find the optimal tolerance $\lambda$ quickly.
Experimental Validation
The JTE algorithm was evaluated on two simulated platforms: a 2-DOF planar robot and a 6-DOF YASKAWA GP50 industrial arm. These robots were tested against various "half-plane" obstacles, including walls at different orientations.
A key theoretical insight is the algorithm's linear scalability. The authors prove that for multiple obstacles, one can calculate the tolerance for each independently and take the minimum. This prevents the exponential computational growth typically associated with complex environments.
Key Results and Safety Guarantees
The proposed method provides a provably safe lower bound. If the algorithm returns a tolerance, any deviation within that bound is mathematically guaranteed to be safe (within the limits of the polynomial approximation).
Empirical results support these theoretical claims:
- Zero Violations: In 10,000 random samples taken within the calculated tolerance, there were zero safety violations.
- Tightness: The algorithm finds "tight" bounds; the minimum distance to obstacles in 3D tests was often in the millimeter range (e.g., 0.0014m), showing it is not overly conservative.
- Efficiency: The 2D problem solved in under 0.8 seconds, while the 6-DOF 3D problem took between 1.5 and 6.8 seconds. This performance is suitable for many trajectory planning and verification tasks.
Limitations and Future Work
While effective, the method relies on a small-angle assumption for Taylor expansion. Although errors remained below 1% in these tests, the approximation may degrade for very large tolerances. Future work includes extending the framework to handle non-differentiable constraints (like those involving complex mesh geometries) and providing a rigorous proof that the polynomial approximation always remains a conservative lower bound of the true kinematics.
Summary updated