A Visual Predictive Control Framework for Robust and Constrained Multi-Agent Formation Control

Abstract

In this study, the problem of leader-follower position-based formation control is considered. Each agent in the multi-agent network is equipped with a perspective camera (assumed to be pinhole-type) and is servoed visually. A depth-based visual predictive controller is proposed. The framework optimizes the planned trajectory with a prediction horizon, while taking image- and physical-space constraints into account. Furthermore, the presented control scheme provides robustness against the camera occlusion, modeling errors and uncertainties. The performance of the proposed tracking algorithm is validated via numerous simulations.

Appendix

The motion dynamics of the UAV [41], equipped by camera can be described by the following equations,

$$\begin{array}{@{}rcl@{}} \dot{x_{1}} &=&x_{2}\\ \dot{x_{2}} &=&\left[\begin{array}{c} 0 \\ 0 \\ -g \end{array}\right]+\frac{1}{m} \left[\begin{array}{ccc} c_{\phi} c_{\psi}-c_{\theta} s_{\phi} s_{\psi} & -c_{\psi} s_{\phi}-c_{\phi} c_{\theta} s_{\psi} & s_{\theta} s_{\psi} \\ c_{\theta} c_{\psi} s_{\phi}+c_{\phi} s_{\psi} & c_{\phi} c_{\theta} c_{\psi}-s_{\phi} s_{\psi} & -c_{\psi} s_{\theta} \\ s_{\phi} s_{\theta} & c_{\phi} s_{\theta} & c_{\theta} \end{array}\right] \left[\begin{array}{c} f_{x} \\ f_{y} \\ f_{z} \end{array}\right]\\ \dot{x_{3}} &=&\left[\begin{array}{ccc} 1 & 0 & -s_{\theta} \\ 0 & c_{\phi} & c_{\theta} s_{\phi} \\ 0 & -s_{\phi} & c_{\theta} c_{\phi} \end{array}\right]^{-1} x_{4}\\ \dot{x_{4}} &=&\left[\begin{array}{c} \tau_{\phi} I_{x x}-1 \\ \tau_{\theta} I_{y y}-1 \\ \tau_{\psi} I_{z z}-1 \end{array}\right]-\left[\begin{array}{l} \frac{I_{y y}-I_{z z}}{I_{x x}} \omega_{y} \omega_{z} \\ \frac{I_{z z}-I_{x x}}{I_{y y}} \omega_{x} \omega_{z} \\ \frac{I_{x x}-I_{y y}}{I_{z z}} \omega_{x} \omega_{y} \end{array}\right] \end{array}$$

(18)

where, s and c refer to $\sin \limits ()$ and $\cos \limits {()}$ operators and x = (x₁,x₂,x₃,x₄) represents the state vector of the vehicle. Also, x₁ = (x,y,z) and $x_{2}=(\dot {x},\dot {y},\dot {z})$ are position and velocity of the UAV. The attitude of the UAV in the reference frame is denoted by x₃ = (𝜃,ϕ,ψ), where 𝜃, ϕ, ψ denote the roll, pitch and yaw in the global frame. In addition, the angular velocity of UAV in the body frame is described by x₄ = (ω_x,ω_y,ω_z). It worths mentioning that the angular velocity vector $\omega \neq \dot {\theta }$. In fact, the angular velocity is a vector pointing along the axis of rotation in the body frame, while $\dot {\theta }$ represents time derivative of pitch, yaw and roll. The manipulated variable for controlling the pose of the vehicle is the force, $\mathbf {F}=\left [\begin {array}{lll} f_{x} & f_{y} & f_{z} \end {array}\right ]^{\mathrm {T}}$, and torque, $\mathbf {M}=\left [\begin {array}{lll} \tau _{\phi } & \tau _{\theta } & \tau _{\psi } \end {array}\right ]^{\mathrm {T}}$ vector generated by the propellers. A linear model can be formulated to relate each propeller’s angular velocity to generated force f and torque τ.

$$\begin{aligned} f &=K_{f} \cdot \omega^{2} \\ \tau &=K_{\tau} \cdot \omega^{2} \end{aligned}$$

(19)

where, K_f and K_τ are the force and torque coefficients, ω represent the propellers rotation velocity. The trasnfer function, T can be used to calculate the velocity of each propeller based on the required F and M vector calculated the controller.

$$\boldsymbol{\Omega}=T^{-1}\left[\begin{array}{l} \mathbf{F} \\ \mathbf{M} \end{array}\right]$$

$$\begin{aligned} T=\operatorname{diag}\left( \frac{\sqrt{3} K_{f} s_{\gamma}}{2}, \frac{K_{f} s_{\gamma}}{2}, K_{f} c_{\gamma}, \frac{L K_{f} c_{\gamma}}{2}, \frac{\sqrt{3} L K_{f} s_{\gamma}}{2},1 \right)\\ \left[\begin{array}{cccccc} 1 & 0 & -1 & 1 & 0 & -1 \\ -1 & 2 & -1 & -1 & 2 & -1 \\ -1 & -1 & -1 & -1 & -1 & -1 \\ 1 & 2 & 1 & -1 & -2 & -1 \\ 1 & 0 & -1 & -1 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] \end{aligned}$$

$$\begin{aligned}\\ +\operatorname{diag}\left( 1,1,1, \frac{\sqrt{3} K_{\tau} s_{\gamma}}{2}, \frac{K_{\tau} s_{\gamma}}{2}, K_{\tau} c_{\gamma}\right) \left[\begin{array}{cccccc} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ -1 & 0 & 1 & 1 & 0 & -1 \\ 1 & 2 & 1 & -1 & -2 & -1 \\ 1 & -1 & 1 & -1 & 1 & -1 \end{array}\right] \end{aligned}$$

where, the vector $\boldsymbol {\Omega }=\left [\begin {array}{llllll} {\omega _{1}^{2}} & {\omega _{2}^{2}} & {\omega _{3}^{2}} & {\omega _{4}^{2}} & {\omega _{5}^{2}} & {\omega _{6}^{2}} \end {array}\right ]^{\mathrm {T}}$ can be formed by square of spinning velocity of each propeller.

Table 1 Dynamics and control parameters of the UAV

For simulations of the paper, an LQR controller was design by successively linearizing system dynamics using Jacobian in each control step. The successive linearization model-based LQR controller found to be effective in stabilizing and controlling the position and orientation of the proposed UAV. The rest of the dynamics and controller parameters used in the simulation can be found in Table 1.