Distributed Safety-Filtered Optimal Formation Control With Executed-Input-Matched Disturbance Estimation

Abstract

This paper addresses the distributed optimal formation control problem for second-order multi-agent systems subject to external disturbances and collision-avoidance constraints. The desired formation is formulated as a convex optimization problem with time-varying specifications driven by rigid-motion scheduling. We develop a unified control architecture combining a primal-dual nominal law with an optimization-based safety filter. To estimate structured disturbances without acceleration measurements, an executed-input-matched filter constructs an online disturbance proxy whose dynamics are independent of the applied control input. A filter-assisted internal-model observer then provides exponential convergence under the exact exosystem model and an explicit ultimate bound under bounded model mismatch. Pairwise collision constraints are incorporated through a distributed safety-filtering QP. The closed-loop analysis provides an input-to-state practical-tracking bound that explicitly accounts for disturbance-estimation error, safety-filter corrections, and time variation of the selected optimal multiplier; asymptotic tracking is recovered when these inputs vanish. Under the stated observer-envelope initialization, safe-initialization, QP-feasibility, and regularity conditions, the protected-pair safe set is forward invariant along the corresponding closed-loop solutions. Note to Practitioners—Multi-robot teams used in inspection, monitoring, and logistics must maintain safe separation while executing coordinated motions under disturbances such as wind, uneven terrain, or payload variation. In a safety-filtered implementation, the applied input may differ sharply from the nominal command when collision constraints are active. This paper filters the applied input with the same dynamics used to construct the acceleration proxy, so that these control modifications cancel directly from the proxy dynamics at the signal-construction level. Under exact exosystem matching, the resulting internal-model observer converges exponentially to the modeled disturbance; under bounded model mismatch, it admits an explicit ultimate estimation bound. Practical implementation requires access to the input actually applied to the plant and an exosystem model that captures the dominant disturbance modes. Any downstream actuator modification must also be supplied to the matched filter.