Visual Servoing of a Moving Target by an Unmanned Aerial Vehicle

Abstract

To track moving targets undergoing unknown translational and rotational motions, a tracking controller is developed for unmanned aerial vehicles (UAVs). The main challenges are to control both the relative position and orientation between the target and the UAV to within desired values, and to guarantee that the generated control input to the UAV is feasible (i.e., below its motion capability). Moreover, the UAV is controlled to ensure that the target always remains within the field of view of the onboard camera. These control objectives were achieved by developing a nonlinear-model predictive controller, in which the future motion of the target is predicted by quadratic programming (QP). Since constraints of the feature vector and the control input are considered when solving the optimal control problem, the control inputs can be bounded and the target can remain inside the image. Three simulations were performed to compare the efficacy and performance of the developed controller with a traditional image-based visual servoing controller.

1. Introduction

Unmanned aerial vehicles (UAVs) have attracted much attention since the agileness made them capable of adapting to diverse terrains and executing various tasks such as monitoring, rescue, and target tracking [1,2]. Studies of UAVs have focused on localizing a target from sensing data of cameras [3], radars [4], and sensor networks [5,6,7]. Due to the advantageous availability and cost of cameras, many approaches can reconstruct the environment by 2D image features. Algorithms for estimating the position of objects based on image features captured by stationary cameras [8] and moving cameras [9] have been presented. Methods have been developed to reconstruct the 3D model of an object from a perspective camera [10,11]. Other features such as lines [12], cylinders and spheres [13], or planes [14,15] are also applied to a structure-from-motion (SfM) task. However, the aforementioned SfM methods rely on triangulation to recover the depth, and extension of these works to the motion estimation of moving targets is still challenging.

Stationary targets: Image-based visual servoing (IBVS) is a system control method that guarantees a series of visual features of a target will converge to the desired setpoints in the image [16]. However, IBVS approaches have potential problems such as causing larger tracking errors or losing the tracking altogether, especially when the target motion is time-varying or not predicted correctly [17]. Visual predictive control (VPC) [18,19] is a method that combines model predictive control constraints such as in the field of view (FOV), output limitations of the actuator, and workspace. A nonlinear model predictive controller was applied to an underwater vehicle to generate a desired velocity while satisfying the constraints of the visibility [20]. Similarly, MPC has been used to control a mobile robot while tracking a stationary feature point [21], to keep a visual feature of the target in the curtain position of the image [22,23], and to maximize the visibility of a target and minimize the velocity of its image feature when utilizing quadrotors. Artificial patterns have been used to predict all of the missing feature points due to occlusion problems and to ensure the execution of IBVS for navigation [24]. The aforementioned MPC approaches all considered only stationary targets, and so the present work designed an MPC to track a moving target, and moreover incorporated its motion estimation into the control sequence as a feedforward term in order to generate smooth control inputs.

Dynamic targets: Knowledge of motion of targets is crucial in tracking control; however, the knowledge is unknown and difficult to predict, and so various forms of image features are used for feedback control as to keep the target in FOV [25,26]. Many image processing methods have been developed for feature extraction and matching, such as RGB-based methods [27,28], scale-invariant feature-transform (SIFT) [29,30], and speeded-up robust-features (SURF) [31,32]. However, there are some drawbacks for these methods in terms of object detection and estimation of the relative motion between the camera and the target. Visual-based optimization methods have been applied a quadrotor [33], and it aimed to track a moving target while avoiding obstacles and the target position is assumed to be known in advance. Model-based optimization was applied to achieve perception and action objectives for robust sensing of a UAV [22]; however, the image features were extracted for indoor localization, rather than target tracking. Bounding boxes and feature points have been utilized as image features for tracking human beings [34,35,36,37], but the targets were moving at speeds that were so low that their motions could be ignored during tracking. Moreover, the orientation of the target is unavailable [38]. MPC has been applied to track a target moving along a periodic path, with this motion constraint reducing the complexity of the controller design [39]. However, the aforementioned works did not take the interaction between the target and the UAV into account, and the relative angle between the camera and the target was not modeled or measured.

This work models the interaction between a UAV and target and includes it in the developed controller, so that the tracking performance can be improved given certain motion, control, and FOV constraints. A DNN (e.g., YOLO [40]) is designed and trained to generate a bounding box in the image that encloses the target and predicts the angle of the target as viewed by the UAV, since both types of information are required to estimate target motion in an unscented Kalman filter (UKF). Finally, a high-order polynomial that emulates the target motion is obtained using quadratic programming (QP), which is used in the controller to predict the future motion of the target. The contributions of this work can be summarized as follows:

• An optimal tracking controller is developed to track a moving target undergoing unknown motion while meeting motion, control, and sensing constraints, where the relative motion between the target and the UAV is estimated.
• In contrast to previous approaches [34,35,36,37,38], this work models the dynamics of the relative rotation between the target and the UAV, and the relative angle can be controlled to a predefined desired value, which can be applied in applications such as the automatic searching, detection, and recognition of car license plate.
• The controller is designed to ensure that the target remains within the FOV.
• Compared to the IBVS controller, the developed controller can ensure smooth control input, less energy, and smaller tracking error.
• The developed control architecture can be applied to track other moving targets as long as it can be detected by the YOLO network.

The organization of this work is as follows: Section 2 formulates the interaction between the target and the UAV as well as the control objectives. Section 3 describes how the target velocity is estimated based on the UKF and the bounding box in the image. Section 4 designs a controller to meet the control objectives. Simulations are presented in Section 5 to verify the efficacy of the developed controllers.

2. Preliminaries

Control Objectives

Figure 1 depicts the kinematics of a dynamic target and a camera fixed on a UAV, and the superscript $\mathcal{G}$ and the subscript C denote the inertial frame and the camera frame, respectively. The vector between the target and the camera can be expressed as

$$\begin{array}{cc}\hfill r_{q/c}& =r_q-r_c,\hfill \end{array}$$

(1)

where $r_q={\left[\begin{array}{ccc}{x}_q,& y_q,& z_q\end{array}\right]}^T$ is defined as the target position, $r_c={\left[\begin{array}{ccc}{x}_c& y_c& z_c\end{array}\right]}^T$ is the camera position, $r_{q/c}=\left[\begin{array}{ccc}X,& Y,& Z\end{array}\right]$ is defined in the camera frame in order to facilitate the subsequent analysis, and Z defined in $r_{q/c}$ is the depth $d\in \mathbb{R}.$ Based on (1), the relative velocity ${\dot{r}}_{q/c}$ can be expressed as

$$\begin{array}{ccc}\hfill {\dot{r}}_{q/c}& =& V_q-V_c-{\omega }_c\times r_{q/c},\hfill \end{array}$$

(2)

where $V_c\triangleq {\left[\begin{array}{ccc}{v}_{cx}& v_{cy}& v_{cz}\end{array}\right]}^T$ and ${\omega }_c\triangleq {\left[\begin{array}{ccc}{\omega }_{cx}& {\omega }_{cy}& {\omega }_{cz}\end{array}\right]}^T$ are defined as the translational and angular velocities of the camera. In (2), $V_q={\left[\begin{array}{ccc}{v}_{qx}& v_{qy}& v_{qz}\end{array}\right]}^T$ denoted the translational velocity of the target is unknown and will be estimated, and $V_c$ and ${\omega }_c$ are the control input to be designed in the subsequent sections.

The orientation of the target denoted as $\psi \in \mathbb{R}$ shown in Figure 2 can be expressed as:

$$\psi =arctan\left(\frac{y_c^g-y_q^g}{x_c^g-x_q^g}\right)-arctan\left(\frac{V_{qy}^g}{V_{qx}^g}\right),$$

(3)

where the superscript denotes the value defined in the global frame. Taking the time derivative on both sides of (3) yields the relative angular velocity between the target and the UAV:

$$\begin{array}{cc}\hfill \dot{\psi }=& \frac{(v_{cy}^g-v_{qy}^g)(x_c^g-x_q^g)-(v_{cx}^g-v_{qx}^g)(y_c^g-y_q^g)}{{(x_c^g-x_q^g)}^2+{(y_c^g-y_q^g)}^2}\hfill \\ \hfill & -\frac{a_{qy}^g{v}_{qx}^g-a_{qx}^g{v}_{qy}^g}{{\left(v_{qx}^g\right)}^2+{\left(v_{qy}^g\right)}^2}.\hfill \end{array}$$

(4)

The control objective is to achieve

$$r_{q/c}\to {[0,0,d_{des}]}^T,\psi \to {\psi }_{des},$$

(5)

where implies that the target remains at the center of the image, and $d_{des}\mathrm{and}{\psi }_{des}\in \mathbb{R}$ are the desired depth and angle, with respect to the target. The limitation of existing results is relaxed by making Assumption 1.

Assumption 1.

$r_q,{\dot{r}}_q,{\ddot{r}}_q\in {\mathcal{L}}_{\infty }$.

3. Estimating the Motion of the Moving Target

3.1. Kinematics Model

To model the tightly coupled motion between the camera and the target, the states of the system are defined as

$$\mathbf{x}={\left[x_1,x_2,x_3,\psi ,r_q^T,V_q^T\right]}^T,$$

(6)

where

$$\left[x_1,x_2,x_3\right]=\left[\frac{X}{Z},\frac{Y}{Z},\frac{1}{Z}\right]$$

(7)

denotes the state related to the image feature. By taking the time derivative of (6), the dynamics of the visual servoing system can be obtained as

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}& =\left[\begin{array}{c}{v}_{qx}{x}_3-v_{qz}{x}_1{x}_3+{\zeta }_1+{\eta }_1\\ v_{qy}{x}_3-v_{qz}{x}_2{x}_3+{\zeta }_2+{\eta }_2\\ -v_{qz}{x}_3^2+v_{cz}{x}_3^2-({\omega }_{cy}{x}_1-{\omega }_{cx}{x}_2)x_3\\ \frac{(v_{cy}^g-v_{qy}^g)(x_c^g-x_q^g)-(v_{cx}^g-v_{qx}^g)(y_c^g-y_q^g)}{{(x_c^g-x_q^g)}^2+{(y_c^g-y_q^g)}^2}-\frac{a_{qy}^g{v}_{qx}^g-a_{qx}^g{v}_{qy}^g}{{\left(v_{qx}^g\right)}^2+{\left(v_{qy}^g\right)}^2}\\ V_q\\ \begin{array}{c}{0}_{3\times 1}\end{array}\end{array}\right],\hfill \end{array}$$

(8)

where (2) is used and ${\zeta }_1$, ${\zeta }_2$, ${\eta }_1$, ${\eta }_2,x_{c/q},y_{c/q}\in \mathbb{R}$ are defined as

$$\begin{array}{cc}\hfill {\zeta }_1& ={\omega }_{cz}{x}_2-{\omega }_{cy}-{\omega }_{cy}{x}_1^2+{\omega }_{cx}{x}_1{x}_2\hfill \\ \hfill {\zeta }_2& =-{\omega }_{cz}{x}_1+{\omega }_{cx}+{\omega }_{cx}{x}_2^2-{\omega }_{cy}{x}_1{x}_2\hfill \\ \hfill {\eta }_1& =(v_{cz}{x}_1-v_{cx})x_3\hfill \\ \hfill {\eta }_2& =(v_{cz}{x}_2-v_{cy})x_3\hfill \\ \hfill x_{c/q}& =x_c-x_q\hfill \\ \hfill y_{c/q}& =y_c-y_q.\hfill \end{array}$$

3.2. Measurements: Bounding Box and Orientation

This section defines two measurements performed using a YOLO DNN as depicted in Figure 3. The bounding box and the orientation measurement provide information to correct the states in the UKF, and the detailed description of the DNN can be found in [41].

However, it is hard to defined the accuracy of the estimation since the output of the DNN network is classified into 24 classes. For example, as shown in the video (https://www.youtube.com/watch?v=KMQD7KzsnPE&list=PLrxYXaxBXgRqAUZyX5TsTsPaBtfSYFpHL&index=15) (accessed on 5 June 2021), the upper window demonstrates the estimation of the angle of the target, which is classified into 24 classes from 0 to 360 degree in order to reduce computational efforts. Therefore, when the camera moved from the left to the front, the angle only experienced 6 classes, and the accuracy calculated by comparing the differences between the 6 classes and the continuous variation of angles is not rigorously defined in the existing literature. Nevertheless, the estimation is affordable to be used for sensing the angle of the target in our application.

3.2.1. Bounding Box

Based on the pinhole model, states $x_1$ and $x_2$ can be obtained as

$$\begin{array}{cc}\hfill x_1& =\frac{\overline{u}-c_u}{f_x}\hfill \end{array}$$

$$\begin{array}{cc}\hfill x_2& =\frac{\overline{v}-c_v}{f_y}\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill x_3& =\frac{1}{d},\hfill \end{array}$$

(10)

where $f_x$ and $f_y$ are the focal lengths of the camera, $\left[c_u,c_v\right]$ represents the center of the image frame, $\overline{u}$ and $\overline{v}$ represent the center of bounding box that encloses the detected moving target in the image frame as depicted in Figure 1, and d is the depth obtained from the pinhole model

$$\begin{array}{ccc}\hfill d& =& \sqrt{\frac{A{f}_x{f}_y}{a}},\hfill \end{array}$$

(11)

where a is the area of the bounding box, and A is the ground truth side area of the target (i.e., typical size for a sedan is 4.6 m × 1.5 m).

Remark 1.

The estimate of distance to the vehicle is obtained by (11) and is not directly generated from the DNN. That is, given the generated bounding box, its area A (i.e., unit in pixel) can be calculated and used to calculate the depth by (11). Therefore, as long as the generated bounding box can accurately enclose the target in the image and and the focal lengths are accurately obtained by calibration, the estimated depth will be accurate.

Based on the camera measurement, pinhole model, orientation of the target, and the location of the UAV, the measurement model can be derived as

$$h\left(\mathbf{x}\right)=\left[\begin{array}{c}{f}_x{x}_1+c_u\\ f_y{x}_2+c_v\\ 1/x_3\\ \psi \\ r_c\end{array}\right],$$

(12)

where (1), (7), and (9) are used, and ${\psi }_m$ is $\psi$ measured as described in Section 3.2.2, and d is the relative distance between the target and the UAV. Measurements d and $\varphi$ are defined in detail in Section 3.2.

Remark 2.

Changes in the target velocity can result in inaccurate state estimations, and therefore a UKF [42] is applied to address this issue. The model mismatch between the real and the model defined in (8) is considered as process noise, and the states can be estimated and updated to approach the ground truth in the UKF.

3.2.2. Orientation Measurement

As shown in Figure 2, ${\psi }_m$ can be obtained from a YOLO network with the captured image from the onboard camera and is used to measure where the UAV is located observed by the camera, so that the controller developed in Section 4 can ensure the control objective $\psi \to {\psi }_{des}$ defined in (5) can be achieved.

3.3. Observability

Since the system dynamics defined in (8) is nonlinear, the approach to evaluate the observability of the system developed in (13) of [43] is applied to establish Theorem 1.

Theorem 1.

The system defined in (8) is observable.

Proof. 

Given an observability matrix O defined as

$$O\triangleq \frac{\partial \varphi }{\partial \mathbf{x}}=\left[\begin{array}{ccc}\frac{\partial {\mathcal{L}}_f^0\left(h\left(\mathbf{x}\right)\right)}{\partial x_1}& \cdots & \frac{\partial {\mathcal{L}}_f^0\left(h\left(\mathbf{x}\right)\right)}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial {\mathcal{L}}_f^{n-1}\left(h\left(\mathbf{x}\right)\right)}{\partial x_1}& \cdots & \frac{\partial {\mathcal{L}}_f^{n-1}\left(h\left(\mathbf{x}\right)\right)}{\partial x_n}\end{array}\right],$$

(13)

where $x_1,\dots ,x_n$ are the elements of state $\mathbf{x}$ defined in (6), $h\left(\mathbf{x}\right)$ is the measurement defined in (12), and ${\mathcal{L}}_f^i\left(h\left(\mathbf{x}\right)\right)$ denoted the ith order Lie derivative of $h\left(\mathbf{x}\right)$ is defined as

$$\begin{array}{cc}\hfill {\mathcal{L}}_f^0\left(h\left(\mathbf{x}\right)\right)& \triangleq h\left(\mathbf{x}\right)\hfill \\ \hfill {\mathcal{L}}_f^1\left(h\left(\mathbf{x}\right)\right)& \triangleq \frac{\partial h\left(\mathbf{x}\right)}{\partial \mathbf{x}}·f\hfill \\ \hfill {\mathcal{L}}_f^2\left(h\left(\mathbf{x}\right)\right)& \triangleq \frac{\partial }{\partial \mathbf{x}}\left[{\mathcal{L}}_f^1\left(h\left(\mathbf{x}\right)\right)\right]·f\hfill \\ \hfill ⋮\\ \hfill {\mathcal{L}}_f^k\left(h\left(\mathbf{x}\right)\right)& \triangleq \frac{\partial }{\partial \mathbf{x}}\left[{\mathcal{L}}_f^{k-1}\left(h\left(\mathbf{x}\right)\right)\right]·f,\hfill \end{array}$$

where $f=\dot{\mathbf{x}}$ is the vector field, and the submatrix of observability matrix O denoted as $O_{\mathrm{s}}\in {\mathbb{R}}^{14\times 10}$ is defined as

$$\begin{array}{cc}\hfill O_{\mathrm{s}}& \triangleq \left[\begin{array}{c}\frac{\partial {\mathcal{L}}_f^0\left(h\left(\mathbf{x}\right)\right)}{\partial \mathbf{x}}\\ \frac{\partial {\mathcal{L}}_f^1\left(h\left(\mathbf{x}\right)\right)}{\partial \mathbf{x}}\end{array}\right],\hfill \end{array}$$

and can be rewritten as

$$\begin{array}{cc}\hfill O_{\mathrm{s}}& =\left[\begin{array}{cccccc}{f}_x& 0& 0& 0& 0& 0\\ 0& f_y& 0& 0& 0& 0\\ 0& 0& -x_3^{-2}& 0& 0& 0\\ & & & 1& & 0\\ 0& 0& 0& 0& I_3& 0_{3\times 3}\\ -& -& -& -& -\\ & & \frac{\partial f_x{\dot{x}}_1}{\partial \mathbf{x}}\\ & & \frac{\partial f_y{\dot{x}}_2}{\partial \mathbf{x}}\\ & & \frac{\partial \left(-x_3^{-2}{\dot{x}}_3\right)}{\partial \mathbf{x}}\\ & & \frac{\partial \dot{\psi }}{\partial \mathbf{x}}\\ 0& 0& 0& 0& 0& I_3\end{array}\right].\hfill \end{array}$$

(14)

Based on (14), $O_{\mathrm{s}}$ is full-rank since rank($O_{\mathrm{s}}$) = 10 based on rows 1~7 and rows 12~14, which implies O is full-rank matrix and the system is observable.    □

4. Controller Architecture

Compared to the existing IBVS control methods [16], the controller developed in this work considers the constraints of visual features and control input in order to improve the tracking performance. The control input is calculated by minimizing the cost function designed in this section that considers not only the error of the feature vector but also the velocity of the moving target. Using the controller can lead to less-aggressive flying behavior and also decrease the energy consumption by choosing appropriate gain matrices in the cost function.

Controller Design

The dynamics model defined in (8) is derived for the process step in the UKF so that the target state can be estimated. The dynamics model of $s_m\left(t\right)={\left[x_1,x_2,x_3,\psi \right]}^T\in {\mathbb{R}}^4$ used for prediction in the controller can be written as

$${\dot{s}}_m=\left[\begin{array}{c}-x_3(v_{cx}-v_{qx})+x_1{x}_3(v_{cz}-v_{qz})-(1+x_1^2){\omega }_{cy}\\ -x_3(v_{cy}-v_{qy})+x_2{x}_3(v_{cz}-v_{qz})-{\omega }_{cy}{x}_1{x}_2\\ x_3^2(v_{cz}-v_{qz})-{\omega }_{cy}{x}_1{x}_3\\ \frac{(v_{cy}^g-v_{qy}^g)(x_c^g-x_q^g)-(v_{cx}^g-v_{qx}^g)(y_c^g-y_q^g)}{{(x_c^g-x_q^g)}^2+{(y_c^g-y_q^g)}^2}-\frac{a_{qy}^g{v}_{qx}^g-a_{qx}^g{v}_{qy}^g}{{\left(v_{qx}^g\right)}^2+{\left(v_{qy}^g\right)}^2}\end{array}\right],$$

(15)

where the control input can be reduced to $u\triangleq {\left[v_{qx},v_{qy},v_{qz},{\omega }_{cy}\right]}^T\in {\mathbb{R}}^4.$ To deal with the modeling errors associated with the use of the dynamics model and the imperfect actuation of the UAV, an error signal ${ϵ}_k$ is designed as:

$$\begin{array}{ccc}\hfill {ϵ}_k& =& s_k-s_{m,k}\hfill \end{array}$$

(16)

where $s_k$ is estimated using the UKF and $s_{m,k}$ is predicted using the model (15) at time k. The desired feature vector utilized in the controller can be defined as

$$\begin{array}{ccc}\hfill s_{d,k}& =& s^{*}-{ϵ}_k,\hfill \end{array}$$

(17)

where $s^{*}\triangleq {\left[x_1^{*},x_2^{*},x_3^{*},{\psi }^{*}\right]}^T\in {\mathbb{R}}^4$ is the reference feature vector prescribed by the user.

With (15) and (17), the optimal control problem (OCP) of the controller can be expressed as

$$\begin{array}{cc}\hfill \underset{s_m,u}{min}& \sum _{i=0}^{N_p-1}\left[{∥s_{m,k+i/k}-s_{d,k+i/k}∥}_{Q_s}^2+{∥u_{k+i/k}-u_{d,k+i/k}∥}_{R_u}^2\right]\hfill \\ \hfill & +{∥s_{m,k+N_p/k}-s_{d,k+N_p/k}∥}_{W_s}^2,\hfill \end{array}$$

(18)

s.t.

$$\begin{array}{cc}\hfill & s_{m,k+i+1/k}=F_m(s_{m,k+i/k},u_{k+i/k}),i=0,\dots ,N_p-1\hfill \\ \hfill & u_{d,k+i+1/k}=u_{d,k+i/k}+{\left[\begin{array}{cc}\ddot{\widehat{{\Gamma }_i}},& 0\end{array}\right]}^{T}dt,i=0,\dots ,N_p-1\hfill \\ \hfill & s_{m,k+i/k}\in \mathbb{S},i=1,\dots ,N_p\hfill \\ \hfill & u_{k+i/k}\in \mathbb{U},i=0,\dots ,N_p-1\hfill \\ \hfill & s_{d,k+i/k}=s^{*}-{ϵ}_{k+i/k},i=1,\dots ,N_p\hfill \\ \hfill & s_{m,k/k}=s_k\hfill \\ \hfill & s_{d,k/k}=s_{d,k}\hfill \end{array}$$

(19)

where $u_d\in {\mathbb{R}}^4$ defined as the desired velocity that control input u needs to achieve and can be updated by $\ddot{\widehat{{\Gamma }_i}}$ over the prediction horizon, where ${\widehat{\mathsf{\Gamma }}}_i\left(t\right)\in {\mathbb{R}}^3$ is an $n\mathrm{th}$-order polynomial obtained by the QP and defined in the interval $[t_{i-1},t_i)$ expressed as

$${\widehat{\mathsf{\Gamma }}}_i\left(t\right)=\sum _{j=0}^n{c}_{ij}{(t-t_{i-1})}^j,\mathrm{for}{t}_{i-1}\le t<t_i,$$

(20)

where $c_{ij}\in {\mathbb{R}}^3$ denotes the $j\mathrm{th}$-order coefficient, $S_m\triangleq \left\{s_{m,k/k},s_{m,k+1/k},\dots ,s_{m,k+N_p/k}\right\}$ denotes the predicted features at sampling time k based on the control sequence $U=\left\{u_{k/k},u_{k+1/k},\dots ,u_{k+N_p-1/k}\right\}$ and the estimation $s_k$ obtained using the UKF. $Q_s$, $R_u$, and $W_s$ defined in (18) are positive weighting matrices, and $N_p\in \mathbb{N}$ is the prediction horizon. Increasing $N_p$ results in less-aggressive control inputs but also increases the computational effort. $F_m(·)$ is the model defined in (15). Since only the linear velocity of the target can be estimated (i.e., $\widehat{{\Gamma }_i}$), control input ${\omega }_{cy}$ only depends on the cost of feature errors $s_m-s_d$. $\mathbb{S}$ and $\mathbb{U}$ are the sets of constraints of the feature vector and the control input given by

$$\begin{array}{ccc}\hfill \mathbb{S}& \triangleq & \left\{s\in {\mathbb{R}}^4|\left[\begin{array}{c}{x}_{1,min}\\ x_{2,min}\\ x_{3,min}\\ {\psi }_{min}\end{array}\right]\le s\le \left[\begin{array}{c}{x}_{1,max}\\ x_{2,max}\\ x_{3,max}\\ {\psi }_{max}\end{array}\right]\right\}\hfill \end{array}$$

$$\begin{array}{ccc}\hfill \mathbb{U}& \triangleq & \left\{u\in {\mathbb{R}}^4|\left[\begin{array}{c}{v}_{cx,min}\\ v_{cy,min}\\ v_{cz,min}\\ {\omega }_{cy,min}\end{array}\right]\le u\le \left[\begin{array}{c}{v}_{cx,max}\\ v_{cy,max}\\ v_{cz,max}\\ {\omega }_{cy,max}\end{array}\right]\right\}.\hfill \end{array}$$

When solving the OCP at time k, ${ϵ}_k$ would be calculated by (16) using the feature vector estimated using the UKF at time k and the feature vector predicted by the model (15) at time $k-1$. This error signal is assumed to be constant over the prediction horizon; $s_{d,k+i/k},i=0,\dots ,N_p$ would be constant over the prediction horizon.

The direct multiple shooting method is employed to solve the problem, since “lifting” the OCP to a higher dimension can usually speed up the rate of convergence. The OCP is repeated at every sampling time, with only the first vector $u_{k/k}$ in control set U being adopted in the system. Several software libraries can be used to solve this kind of nonlinear programming problem (NLP), and this study used CasADi for formulating the NLP and Ipopt for solving it.

Remark 3.

The control architecture depicted in Figure 4 can be applied to general target tracking. That is, the feature vector and orientation generated from YOLO can be used to predict the motion of target $v_q,$ which can facilitate tracking performance and, in turn, enhance the detection of bounding boxes.

5. Simulations

5.1. Environment Setup

The Gazebo simulator under the ROS framework (version 18.04, Melodic) is used to conduct the simulations to verify the efficacy of the developed control algorithm, and the source code is posted on GitHub for public access (https://github.com/Networked-Control-Robotics-Lab/uav_nmpc_tracking_task.git) (accessed on 5 August 2021). A quadrotor implemented with the developed controller is deployed to track a moving vehicle. The initial locations of the target and the UAV are ${\left[\begin{array}{ccc}0,& 0,& 0\end{array}\right]}^T$ and ${\left[\begin{array}{ccc}2.18,& 6.65,& 1.75\end{array}\right]}^T$ and the initial orientations are ${\left[\begin{array}{ccc}0,& 0,& {90}^{\circ }\end{array}\right]}^T$ and ${\left[\begin{array}{ccc}0,& 0,& -{90}^{\circ }\end{array}\right]}^T,$ respectively, in the global frame as shown in Figure 5. The target moves on the $XY$ plane following the nonholonomic dynamics, and its velocity is controlled by the user commands with a loop rate of 80 Hz. The resolution of the captured image is $640\times 480$, and the intrinsic parameters of the camera are

$$\begin{array}{ccc}\hfill K& =& \left[\begin{array}{ccc}381.36& 0& 320\\ 0& 381.36& 240\\ 0& 0& 1\end{array}\right].\hfill \end{array}$$

The control goal in the tracking task is to keep the moving target within the FOV of the camera at the desired depth and angle relative to the target. The reference feature vector was

$$s^{*}={\left[\frac{\left(320-320\right)}{381.36},\frac{\left(312-240\right)}{381.36},1/7,{90}^{\circ }\right]}^T\in {\mathbb{R}}^4$$

(21)

in (17) which defines the desired feature vector, as shown in Figure 6.

In order to maintain the visibility and address the limitations of the actuator while tracking the moving target, the constraints of the states and constraints of the control inputs were considered in the numerical simulations, as listed in

Table 1. State Constraints.

States	Min.	Max.
$x_1$	$-0.84$	$0.84$
$x_2$	$-0.63$	$0.63$
$x_3$	$0.07$	1
$\psi$	$0^{\circ }$	${180}^{\circ }$

and

Table 2. Control Constraints.

Inputs	Min.	Max.
$v_{cx},v_{cy},v_{cz}$$\left(\mathrm{m}/\mathrm{s}\right)$	$-10$	10
$w_{cy}$$\left(\mathrm{rad}/\mathrm{s}\right)$	$-0.59$	$0.59$

. The parameters for tuning the sampling time, predicted horizons, and the weighting matrices in the cost function are presented in

Table 3. Control Parameter Settings.

Parameter	Value
Sampling Time ($▵k$)	$0.025\mathrm{s}$
Horizon Length ($N_p$)	50
$Q_s$	$\left[\begin{array}{cccc}1& 1& 1& 1\\ 1& 1& 1& 1\\ 1& 1& 100& 1\\ 1& 1& 1& 1\end{array}\right]$
$R_u$	$\left[\begin{array}{cccc}0.02& 1& 1& 1\\ 1& 0.03& 1& 1\\ 1& 1& 0.01& 1\\ 1& 1& 1& 0.3\end{array}\right]$

.

5.2. Simulation Results

To demonstrate the practicability and efficiency of the developed controller, three simulations are described that compared the controller and the traditional IBVS controller. In the first simulation, the UAV tracked a target that was at a relative angle that changed over time in order to show the benefit of including the angle dynamics in the dynamics model. In the second simulation, the UAV tracked a target moving over a z-shaped path in order to compare the tracking performance of different designs of controllers in an aggressive-motion condition. In the third simulation, the IBVS controller and the controller were utilized to demonstrate the tracking efficiency in an aggressive-motion condition.

5.2.1. Simulation 1: Controller with Relative Rotational Dynamics

The scenario of Simulation 1 is depicted in Figure 7. The controller designed in Section 4 was implemented to track a target at a time-varying angle. In Case 1, the rotational dynamics was not considered in the dynamics model, and the relative orientation was measured directly using a YOLO network. In Case 2, the angle dynamics was taken into consideration in the UKF, which made the relative orientation more robust to noisy or intermittent measurements from the YOLO network.

Comparison of State Feature Vectors

Figure 8 and Figure 9 show the trajectories of the features (i.e., the center of the bounding box) in the image frame over time.

The performances are quantified as the root mean square (RMS) tracking errors defined in

Table 4. Simulation 1: RMS errors.

Case	U-Axis	V-Axis
1	$29.75$	$28.93$
2	$25.00$	$18.18$

.

Figure 10 compares the state vector tracking errors in Cases 1 and 2. The trends in the state vectors are consistent in the two cases.

Comparison of Control Inputs

Figure 11 and Figure 12 show the velocity control inputs for Case 1 and Case 2 in Simulation 1, respectively, and the upper and lower bounds on the control inputs defined in Table 2 are highlighted.

Figure 13 compares the total control inputs, which shows that they were lower in Case 2 than in Case 1. “The total control inputs are defined as the summation of $u_{k+i/k}$ solved from the cost problem defined in (18) over the simulation time period (i.e., 50 s). The higher the value, the higher the energy consumption since the energy consumption of the UAV is proportional to the cube of its speed $u_{k+i/k}$ defined below (15) based on [44]”.

5.2.2. Simulation 2: Controller with Target Motion Pattern

The scenario of Simulation 2 is depicted in Figure 14, where the UAV is controlled to track a target moving over a z-shaped path. The controllers for Cases 1 and 2 are compared. In Case 1, the difference in the contiguous control inputs to the UAV (i.e., differential control cost) is considered in the controller. In Case 2, by leveraging the motion pattern estimated using the UKF, the difference between the velocities of the UAV and the target can be considered in the controller designed in Section 4.

Comparison of State Feature Vectors

Figure 15 and Figure 16 show the trajectories of the target features in the image frame over time, which indicate that the feature trajectory in Case 2 moves over a smaller range compared to that in Case 1.

The performances are quantified based on the RMS tracking errors defined in

Table 5. Simulation 2: RMS errors.

Case	U-Axis	V-Axis
1	$40.92$	$16.60$
2	$18.80$	$20.42$

.

Figure 17 compares the depth and relative angle tracking errors in Case 1 and Case 2, which also shows that Case 2 performed better than Case 1. Thanks to the motion pattern estimated using the UKF, the controller developed in Section 4 performed better than the traditional optimal controller with differential control cost.

Comparison of Control Inputs

Figure 18 and Figure 19 show the velocity control inputs for Cases 1 and 2, respectively, and the upper and lower bounds on the control inputs defined in Table 2 are highlighted. The control inputs were much more stable and smoother for Case 2 than for Case 1.

Figure 20 compares the total control inputs in Cases 1 and 2, and shows that these inputs were lower for Case 2 than for Case 1.

5.2.3. Simulation 3: IBVS vs. the Developed Controller

The scenario in this simulation is the same as that depicted in Figure 14, but the performances of the IBVS controller and the controller designed in Section 4 are compared to track a target moving over a z-shaped path in Cases 1 and 2.

Comparison of State Feature Vectors

Figure 16 and Figure 21 show the trajectories of the features in the image frame over time. The stability of the IBVS controller and the controller is also evident from

Table 6. Simulation 3: RMS errors.

Case	U-Axis	V-Axis
1	$34.92$	$35.23$
2	$18.80$	$20.42$

, which lists the RMS tracking errors. Figure 22 compares the state vector tracking errors in Cases 1 and 2.

The performances quantified based on the RMS tracking errors are presented in Table 6.

Comparison of Control Inputs

Figure 19 and Figure 23 show the velocity control inputs to the IBVS controller and the developed controller, respectively, and the upper and lower bounds on the control inputs defined in Table 2 are highlighted. Figure 23 shows that the linear and angular velocity control input generated for the IBVS controller keeps fluctuating while tracking. In contrast, Figure 19 shows that the linear and angular velocity control inputs of the controller are stable and smooth.

Figure 24 shows that compared with the IBVS controller, the controller needs much less energy to track the moving target. In other words, the controller is much more efficient than the IBVS controller since it saves energy by not generating unnecessary motion.

IBVS Controller vs. the Developed Controller

The IBVS controller relies on the image-feature error of the target for feedback, which results in the motion of the UAV becoming unstable and oscillating with the inclusion of a large error (i.e., large displacement and rotation) as shown in Figure 24. In contrast, the developed controller is designed to minimize the cost function that considers not only the current feature error but also predicts the future states of the image features in the predicted horizons according to the dynamics model. Additionally, the constraints of the control inputs are taken into consideration in order to avoid excessive and unreasonable motions. Figure 23 shows that the angular velocity exceeds the limitation of the control inputs defined in Table 2; however, the angular velocity generated by the controller in Figure 19 prevents unreasonable motion from occurring.

6. Conclusions

A tracking controller has been developed to track a moving target, and it relies on bounding box features generated by a YOLO network and a target motion pattern obtained by QP, in which the target states are estimated using a UKF. The main features of the developed controller ensure the following characteristics:

• The control effort is less than for traditional controllers, while the tracking error is quantified by the RMS error and is less than that for a traditional controller.
• The value of the control input to the controller always remains within the UAV motion capabilities.
• The target can be tracked, and its image features always remain within the image, which is not guaranteed for a traditional IBVS controller.

All of the above results were verified in three simulations. The first simulation illustrated that the controller, which considers the relative rotational dynamics, can perform better in tracking a target at a specified angle. The second simulation indicated that using the predicted motion pattern of a target in the controller can improve the tracking performance compared to the traditional optimal controller, and the controller also requires less control effort for tracking. The third simulation compared the controller with a traditional IBVS controller, and showed that the controller requires much less control effort and achieves a smaller tracking error. This work has demonstrated both the control efficacy and high performance of the developed controller.