This paper proposes a Model Predictive Control (MPC) algorithm for target tracking amongst static and dynamic obstacles. Our main contribution lies in improving the computational tractability and reliability of the underlying non-convex trajectory optimization. The result is an MPC algorithm that runs real-time on laptops and embedded hardware devices such as Jetson TX2. Our approach relies on novel reformulations for the tracking, collision, and occlusion constraints that induce a multi-convex structure in the resulting trajectory optimization. We exploit these mathematical structures using the split Bregman Iteration technique, eventually reducing our MPC to a series of convex Quadratic Programs solvable in a few milliseconds. The fast re-planning of our MPC allows for occlusion and collision-free tracking in complex environments even while considering a simple constant-velocity prediction for the target trajectory and dynamic obstacles. We perform extensive bench-marking in a realistic physics engine and show that our MPC outperforms the state-of-the-art algorithms in visibility, smoothness, and computation-time metrics.
View on arXiv