Fusing Phase Map Servoing and MPC for High-Precision Robotic Tracking of Dynamic Objects

This paper makes the structured-light phase map a control signal rather than only a reconstruction intermediate. The robot observes dense phase information from fringe projection, converts phase error into end-effector velocity through the phase interaction model, and uses MPC to plan tracking actions under motion and timing constraints.

The difficult part is scale. A full phase map contains thousands of visual features, which makes direct predictive control too slow for a moving target. The paper solves this by selecting informative phase-gradient regions, embedding the remaining phase features into a low-dimensional PCA subspace, and adapting the MPC horizon according to phase variation. An EKF predicts phase evolution so that the controller can compensate sensing and processing delay.

This work is published as an open-access article in Actuators under the MDPI CC BY license.

Algorithm principle

The pipeline starts from fringe-projection profilometry. A projector and camera produce a wrapped and then unwrapped phase map, and the vectorized phase map becomes the visual feature for control. Instead of extracting sparse image keypoints, the controller uses dense phase differences between the desired phase map and the observed phase map.

The control law is built from a phase-map interaction matrix that relates phase variation to 6D end-effector velocity. For real-time MPC, the method reduces this dense model in two steps: gradient-guided sparsification keeps informative phase pixels, and PCA maps the remaining phase features into a compact control subspace. The M6 configuration combines PCA dimensionality reduction, a sparse Jacobian, and an adaptive prediction horizon.

An EKF is integrated into the loop to predict phase evolution under target motion. Those predicted phase maps become the MPC reference trajectory, helping the robot account for image-to-control latency during dynamic tracking.

Representative figures

Results and impact

The experiments use a 640 x 480 camera at 120 Hz with 3.0 ms exposure, an Intel Core i7-12700 workstation, and qpOASES with warm start for the MPC subproblem. In the per-cycle computation breakdown, the proposed M6 strategy reduces MPC/QP solving time from 6.8/12.4 ms to 1.9/3.2 ms at median/95th percentile. Total closed-loop latency drops from 12.9/19.2 ms to 8.3/10.4 ms, keeping the loop near the 120 Hz sensing rate.

In simulation, M6 converges in about 20 iterations and shows lower RMS error and tighter solver-time distribution than the other tested strategies. In physical experiments, the stationary tracking accuracy remains around 0.3 mm while convergence improves from roughly 30 iterations to 20 iterations. At 0.05 m/s, both baseline and M6 succeed, but M6 reports lower final error (0.04 +/- 0.01 mm) and faster convergence (290 +/- 40 ms). At 0.1 m/s, the baseline succeeds in only 33% of trials, while M6 succeeds in all trials with 0.05 +/- 0.02 mm final error and 340 +/- 60 ms convergence time.

The main limitation is sensing throughput. The paper reports stable tracking up to about 0.12 m/s in the current setup; faster targets increase phase noise because phase-map generation cannot keep up. The current implementation also focuses on a single target under controlled visual conditions, leaving multi-object tracking and faster phase generation as future work.