Distributed Geometric Optimization and Control of Rigid Differential-Drive Formations from Relative-Position Measurements

Abstract

Distance based formation control fixes the shape of a robot team but leaves its position and orientation unconstrained. We regulate all three for differential drive robots that have only one absolute position anchor, absolute headings, body frame relative position measurements, and communication with their neighbors. We derive a two layer estimation control scheme directly from optimization. The first half of the result is a set of graph-proximal estimators that reconstruct the centroid, the anchor vector, and the centered coordinates. Exact proximal updates reach consensus in finitely many steps, and a finite number of primal dual iterations leaves a bounded reconstruction error. The controller combines proximal centroid and orientation steps with distance gradient shape regulation, and a finite rotation removes the orientation drift caused by the shape step. A discrete Lyapunov analysis gives local exponential convergence once the estimators have converged. We validate the method in ROS 2 Gazebo and in experiments with five physical robots.