Automation and Robots for Disaster Response

Online Predictive Visual Servo Control for Constrained Target Tracking of Fixed-Wing Unmanned Aerial Vehicles

By inergency On Apr 5, 2024

[ad_1]

3.1. MPC Optimization Problem Formulation

This part aims to solve Problem 1.

Based on our previous work [7], a reference state named the Ideal State was proposed to construct the relationship between the image feature vector and

u_{ψ}

. As for the reference state, it has the following constant state:

$θ_{p} = - \frac{π}{2}, θ_{t} = - α, θ = 0, ϕ = 0 .$

(7)

Note that

α \in (0, \frac{π}{2}]

is a given constant. When given the desired circling radius

R_{d}

α

can be expressed as

α = arctan (\frac{H}{R_{d}})

. Denote the coordinates of the feature point in

F_{i}

(u_{1}, v_{1})

, and in the reference state by

(u_{2}, v_{2})

; then, the relationship can be written as:

$[\begin{matrix} {\dot{u}}_{2} \\ {\dot{v}}_{2} \end{matrix}] = [\begin{matrix} \frac{f^{2} + {u_{2}}^{2}}{f} \cdot c o s α - v_{2} \cdot s i n α & \frac{f}{z_{c}} \\ \frac{u_{2} \cdot v_{2}}{f} \cdot c o s α + u_{2} \cdot s i n α & 0 \end{matrix}] \cdot [\begin{matrix} u_{ψ} \\ V_{t} \end{matrix}] .$

(8)

Simultaneously,

u_{p}

and

u_{t}

can be obtained from (20) in [7], which is

$\{\begin{matrix} u_{p} = arctan (\frac{{\dot{u}}_{1}}{f}), \\ u_{t} = arctan (\frac{{\dot{v}}_{1}}{f}) . \end{matrix}$

(9)

Define

s_{2} = {[u_{2}, v_{2}]}^{⊤}

and based on (8), the discrete time state-space model can be expressed as

$s_{2} (k + 1) = s_{2} (k) + {\dot{s}}_{2} (k) \cdot Δ t,$

(10)

where $Δ t$ is the sampling time.

For convenience of subsequent analysis, the discrete form is rewritten as

$s_{2} (k + 1) = f_{s} (s_{2}, u_{ψ}, k),$

(11)

Since the yaw rate of the fixed-wing UAV is restricted, then the following constraint needs to be satisfied:

$u_{ψ} \in U_{s e t}, U_{s e t} = [- u_{max}, u_{max}] .$

(12)

In order to guarantee that the change in

u_{ψ}

can be smooth, the acceleration constraint is also given:

$Δ u_{ψ} \in a_{s e t}, a_{s e t} = [- a_{max}, a_{max}] .$

(13)

where

$Δ u_{ψ} (k + i) = u_{ψ} (k + i) - u_{ψ} (k + i - 1) .$

(14)

Furthermore, the perception capability of the camera is taken into consideration as well. Under the condition of invariant camera parameters, whether the target can be detected correctly usually depends on the amount of feature information contained in the image. More specifically, it is related to the maximum perceived distance of the camera, which is denoted by

L_{M}

. Therefore, the clear perception range of the camera is equivalent to a cone, as shown in Figure 3.

In order to determine whether the target is within the cone, the feature point in the current image plane is first mapped into that in the horizontal image plane. Define the feature point in the latter plane as

(u_{3}, v_{3})

, considering that

θ_{t} = - α

of (7) in the Ideal State; then,

(u_{3}, v_{3})

can be obtained from

(u_{2}, v_{2})

based on the following transformation:

$[\begin{matrix} u_{3} \\ v_{3} \\ f \end{matrix}] = k_{3} [\begin{matrix} 1 & 0 & 0 \\ 0 & sin α & - cos α \\ 0 & cos α & sin α \end{matrix}] [\begin{matrix} u_{2} \\ v_{2} \\ f \end{matrix}],$

(15)

where $k_{3} \in R^{+}$ . After solving the above three equations, the value of $u_{3}$ and $v_{3}$ can be obtained.

Since the flight altitude H of the UAV is constant in this paper, the maximum perceived distance can be converted into the maximum horizontal distance, which is denoted by

R_{M}

. Therefore, the target is recognized to be within the perception range if, and only if, the following condition is met:

$\frac{\sqrt{{u_{3}}^{2} + {v_{3}}^{2}}}{f} \leq \frac{R_{M}}{H} .$

(16)

After that, in order to keep the target within the perception range when the UAV circles around it,

α

in (7) needs to satisfy the following condition:

$tan (\frac{π}{2} - α) \leq \frac{R_{M}}{H},$

(17)

Simultaneously, considering the constraint of

u_{ψ}

in (12), there exists

$tan (\frac{π}{2} - α) \geq \frac{V_{t}}{u_{max} \cdot H} .$

(18)

Define

η_{1} = arctan \frac{R_{M}}{H}

η_{2} = arctan \frac{V_{t}}{u_{max} \cdot H}

, then

α \in [\frac{π}{2} - η_{1}, \frac{π}{2} - η_{2}]

. Based on (16), the perception constraint at time-step k can be defined as

where

$g_{1} : = \frac{{u_{3}}^{2} + {v_{3}}^{2}}{f^{2}} - \frac{R_{M}^{2}}{H^{2}}$

(20)

Denote the error of the feature point by

$Δ s_{2} (i | k) = s_{d} - s_{2} (i | k),$

(21)

where $s_{d} = {[0, 0]}^{⊤}$ is the desired value of $s_{2}$ , and $s_{2} (0 | k) = s_{2} (k)$ is the observed value at time-step k. Since $Δ u_{ψ} (0 | k) = u_{ψ} (0 | k) - u_{ψ} (k - 1)$ , then the cost function can be defined as follows:

$J_{1} (k) = \sum_{i = 1}^{N_{p}} (∥ Δ s_{2}_{(i | k)}^{∥} + {∥ Δ u_{ψ}}_{(i - 1 | k)}^{∥}) = \sum_{i = 1}^{N_{p}} J_{r} (i | k),$

(22)

where $∥ Δ s_{2} ∥_{Q_{s}}^{2} = Δ s_{2}^{⊤} \cdot Q_{s} \cdot Δ s_{2}$ and similar to $∥ Δ u_{ψ} ∥_{Q_{u}}^{2}$ , $Q_{s} = d i a g {q_{1}, q_{2}}$ is positive definite and $Q_{u} \in R^{+}$ . The control sequence is

$U = {u_{ψ} (0 | k), u_{ψ} (1 | k), \dots, u_{ψ} (N_{p} - 1 | k)},$

(23)

$N_{p} \in R^{+}$ is the prediction horizon.

Note that $J_{1} = 0$ if, and only if, $Δ s_{2} (i | k) = 0$ and $Δ u_{ψ} (i - 1 | k)$ for $i \in {1, \dots, N_{p}}$ . That is, the constant control input of the UAV makes it circle around the static target.

Due to the fact that only the first control input of

U

in (23) is used, the following contraction constraint is provided to ensure the stability of the system (which will be proven in Section 3.3):

$C C_{1} (k) = ∥ Δ s_{2} (1 | k) ∥ - δ \cdot ∥ Δ s_{2} (k) ∥ \leq 0,$

(24)

where $∥ Δ s_{2} (k) ∥$ represents the 2-norm of $s_{2} (k)$ , $δ \in (0, 1)$ .

Therefore, based on Equations (11)–(13), (19), (22), and (24), the MPC optimization problem can be formulated as

$\begin{matrix} P_{1} : & arg min_{U (k)} J_{1} (k) \end{matrix}$

(25a)

$\begin{matrix} s . t . & s_{2} (i | k) = f_{s} (s_{2}, u_{ψ}, i - 1 | k), \end{matrix}$

(25b)

$\begin{array}{l} u_{ψ} (i - 1 | k) \in U_{s e t}, \end{array}$

(25c)

$\begin{array}{l} Δ u_{ψ} (i - 1 | k) \in a_{s e t}, \end{array}$

(25d)

$\begin{array}{l} g_{1} (i | k) \leq 0, \end{array}$

(25e)

$\begin{array}{l} C C_{1} (k) \leq 0, \end{array}$

(25f)

where $i = 1, 2, \dots, N_{p}$ .

3.2. Switching-Based Optimization Control

Problem 2.

With the increase in $N_{p}$ and limited by the computing capability of the processors, it is time-consuming to solve the optimization problem of (25) with nonlinear hard constraints (25e) and (25f) online. Even worse, they may also lead to the problem of an infeasible solution within the predefined iterations. Then, the question arises of how to design an online MPC strategy for practical application?

To solve the problem, this paper considers adding the hard constraints into the cost function, so that these constraints need not be strictly satisfied.

Firstly, add the constraint (25e) into

J_{r}

; then, we have

$J_{r, 1} (i | k) = J_{r} (i | k) + β_{1} \cdot max {g_{1} (i | k), 0},$

(26)

where $β_{1} \in R^{+}$ is a constant.

Remark 1.

In the context of discrete systems as addressed in this work, the discontinuous nature of the sampling points renders the exact occurrence at $g_{1} (i | k) = 0$ highly improbable. Furthermore, it is essential to acknowledge that the cost function focuses on the overall optimization objective, thus rendering the non-differentiability of specific points inconsequential to the overall optimization.

Furthermore, considering the constraints of (12), (13), and the unknown motion of the target, it may be unable to make

g_{1} (k + 1) \leq 0

when

g_{1} (k) > 0

. Therefore,

g_{1}

in (20) is redefined as follows to make

β_{1} \cdot max {\cdot}

in (26) function earlier:

$g_{2} : = \frac{{u_{3}}^{2} + {v_{3}}^{2}}{f^{2}} - \frac{{(R_{M} - Δ R)}^{2}}{H^{2}},$

(27)

where $Δ R > 0$ . Then, (26) is rewritten as

$J_{r, 1} (i | k) = J_{r} (i | k) + β_{1} \cdot max {g_{2} (i | k), 0},$

(28)

After that, turn (24) into a soft constraint as well; then, the cost function can be represented as

$J_{1, s o f t} (k) = \sum_{i = 1}^{N_{p}} J_{r, 1} (i | k) + β_{2} \cdot max {C C_{1} (k), 0} .$

(29)

Compared with the soft constraints-based method that introduces slack variables like [26], the method that uses the max function is more efficient. When

g_{2} (i | k) \leq 0

is satisfied, the nonlinear function does not participate in solving the gradient in each iteration when using the interior point method. However, the gradients of all the

g_{2} (i | k)

still need to be calculated in each iteration for the former method, which is more time-consuming.

Therefore, the MPC optimization problem can be reformulated as

$\begin{matrix} P_{1, s o f t} : & arg min_{U (k)} J_{1, s o f t} (k) \end{matrix}$

(30a)

$\begin{matrix} s . t . & s_{2} (t | k) = f_{s} (s_{2}, u_{ψ}, i - 1 | k), \end{matrix}$

(30b)

$\begin{matrix} u_{ψ} (i - 1 | k) \in U_{s e t}, \end{matrix}$

(30c)

$\begin{matrix} Δ u_{ψ} (i - 1 | k) \in a_{s e t}, \end{matrix}$

(30d)

where $i = 1, 2, \dots, N_{p}$ .

A warm start is also used in this work. Based on our previous work [7], the control input for the UAV can be obtained with the aid of LSM and (11), which is denoted by

u_{ψ} (i | 0), i = 0, \dots, N_{p} - 1

. Note that

u ψ_{i} \in U_{s e t}

is required; then, the initial control sequence can be chosen as

$U_{0} = {u_{ψ} (0 | 0), u_{ψ} (1 | 0), \dots, u_{ψ} (N_{p} - 1 | 0)} .$

(31)

Obviously, $U_{0}$ satisfies the inequality constraints of (30c) and (30d). This indicates that $U_{0}$ is the feasible solution of the optimization problem.

Furthermore, denote the optimal solution at time-step k by

U^{*} (k)

; then, the feasible solution at time-step

k + 1

, denoted by

U (k + 1)

, can be chosen as

$u_{ψ} (i | k + 1) = \{\begin{matrix} u_{ψ}^{*} (i + 1 | k), i = 0, \dots, N_{p} - 2 \\ u_{ψ}^{*} (N_{p} - 1 | k), i = N_{p} - 1 \end{matrix}$

(32)

With increase in the target speed, the relative distance between the fixed-wing UAV and the target may exceed the maximum perceived distance limited by the constraints of (12) and (13). Although the target may still be recognized with the aid of a detection algorithm, the accuracy of detection will decline and the details of the target cannot be clearly distinguished. It is noted that investigating the details of the target is a basic need for UAV target tracking applications.

The items of

β_{1} \cdot max {g_{2} (i | k), 0} (i = 1, \dots, N_{p})

in the cost function

J_{r, 1}

of (28) help to avoid the relative distance from exceeding

R_{M}

. However, the optimization problem of (30) is still more concerned with the convergence of

s_{2}

. Moreover, both the gradients about

(u_{2}, v_{2})

and

(u_{3}, v_{3})

need to be calculated online, which increases the workload of computation.

To solve the problem, a switching-based control method is proposed. When the relative distance keeps within the maximum perceived distance, the optimization problem (30) is considered to enable the UAV to fly around the target. However, if the condition is unsatisfied, the other optimization problem is designed to return the target back to the perception range quickly.

According to (16), the perception constraint is directly affected by

(u_{3}, v_{3})

. However, the optimization problem

P_{1, s o f t}

focuses on the convergence of

(u_{2}, v_{2})

s_{d}

. Therefore, in order to accelerate the convergence of

g_{1} (k)

to 0 once the perception constraint is unsatisfied, a new cost function is defined as follows:

$J_{g} (k) = \sum_{i = 1}^{N_{p}} (∥ Δ s_{3}_{(i | k)}^{∥} + {∥ Δ u_{ψ}}_{(i - 1 | k)}^{∥}) = \sum_{i = 1}^{N_{p}} l_{g} (i | k),$

(33)

where $Q_{s, g} = d i a g {q_{1, g}, q_{2, g}}$ is positive definite, $Q_{u} \in R^{+}$ and

$\{\begin{matrix} s_{3} = {[u_{3}, v_{3}]}^{⊤}, \\ Δ s_{3} (i | k) = s_{d} - s_{3} (i | k) . \end{matrix}$

(34)

In addition, to ensure the convergence of

Δ s_{3}

, the following contraction constraint is also given:

$C C_{g} (t | k) = ∥ Δ s_{3} (1 | k) ∥ - δ \cdot ∥ Δ s_{3} (k) ∥ \leq 0,$

(35)

where $∥ Δ s_{3} (k) ∥$ represents the 2-norm of $Δ s_{3} (k)$ . Similar to (29), in order to avoid the nonlinear constraint of (35), the cost function of $J_{g} (k)$ in (33) is rewritten as

$J_{g, s o f t} (k) = \sum_{i = 1}^{N_{p}} l_{g} (i | k) + β_{g} \cdot max {C C_{g} (k), 0},$

(36)

where $β_{g} \in R^{+}$ .

Remark 2.

The convergence of

C C_{1}

in (24) does not theoretically guarantee the convergence of

C C_{g}

in (35) around

g_{1} (k) = 0

. Only when

{u_{2}}^{2}

and

{v_{2}}^{2}

converge simultaneously can the convergence of (35) be ensured (refer to Section 3.3 for the proof), thereby ensuring the stability of the switching process.

To this end, a new contractive constraint is defined as follows:

$\{\begin{matrix} C C_{2 a} = | Δ u_{3} (1 | k) | - δ \cdot | Δ u_{3} (k) ∥ \leq 0, \\ C C_{2 b} = | Δ v_{3} (1 | k) | - δ \cdot | Δ v_{3} (k) ∥ \leq 0 . \end{matrix}$

(37)

Referring to (36), the corresponding cost function can be written as

$J_{2, s o f t} (k) = \sum_{t = 1}^{N_{p}} l_{g} (t | k) + a_{2} \cdot (max {C C_{2 a} (k), 0} + max {C C_{2 b} (k), 0}) .$

(38)

Considering that (37) necessitates the simultaneous convergence of both

| u_{2} |

and

| v_{2} |

, whereas (35) only mandates the convergence of

s_{3}

, it is evident that the solution requirements for the former are more stringent. By comprehensively considering the stability of the switching process and alleviating the solution constraints, we reformulate the control optimization problem based on Equations (30), (36), and (38) as

$\begin{matrix} P_{s w i t c h} : & arg min_{U (k)} J_{s w i t c h} (k) \end{matrix}$

(39a)

$\begin{matrix} s . t . & s_{2} (t | k) = f_{s} (s_{2}, u_{ψ}, t - 1 | k), \end{matrix}$

(39b)

$\begin{matrix} u_{ψ} (t - 1 | k) \in U_{s e t}, \end{matrix}$

(39c)

$\begin{matrix} Δ u_{ψ} (t - 1 | k) \in a_{s e t}, \end{matrix}$

(39d)

where $t = 1, 2, \dots, N_{p}$ , and

$J_{s w i t c h} = \{\begin{cases} J_{1, s o f t} (k), n_{1} = 0, \end{cases} J_{2, s o f t} (k), 0 < n_{1} < n_{max}, J_{g, s o f t} (k), n_{1} = n_{max}$

(40)

Note that

n_{1} \in Z^{+}

functions as a counter that increases by one when

g_{1} (k) \leq 0

and resets to 0 when

g_{1} (k) > 0

. Moreover,

n_{max} \in Z^{+}

represents the counting threshold. The constraint (35) is employed to relax the solution limit only after

n_{1}

reaches

n_{max}

, thereby mitigating the potential issue of an unstable switching process.

The update of

n_{1}

is formulated as

$\{\begin{matrix} n_{1} = sat (n_{1} + 1), & g_{1} (k) \leq 0, \\ n_{1} = 0, & g_{1} (k) > 0, \end{matrix}$

(41)

where the saturation function $s a t (\cdot)$ is defined as

$sat (x) = \{\begin{matrix} n_{max}, & x > n_{max}, \\ x, & e l s e . \end{matrix}$

(42)

The initialization of

n_{1}

is as follows:

$\{\begin{matrix} n_{1} = n_{max}, & g_{1} (0) < 0, \\ n_{1} = 0, & e l s e, \end{matrix}$

(43)

The implementation of the proposed method is shown as Algorithm 1.

Algorithm 1:Switching-based optimization control for target tracking

Require: The image captured by camera

Ensure:

(u_{1}, v_{1}) \to 0

(u_{2}, v_{2}) \to 0

1:: Let $k = 0$ , calculate $g_{1} (0)$ , and initialize $n_{1}$ based on (43).
2:: while Discover the target do
3:: Detect the centroid coordinates $(u_{1}, v_{1})$ ;
4:: Calculate $(u_{2}, v_{2})$ of the reference state (7);
5:: if $k = 0$ then
6:: Obtain initial control sequence $U (k)$ based on (31).
7:: else
8:: Obtain initial control sequence $U (k)$ based on (32).
9:: end if
10:: Solve the optimization problem (39) online to obtain $U^{*} (k)$ .
11:: Choose the first element of $U^{*} (k)$ to be the control input.
12:: $k = k + 1$ .
13:: Obtain the control input of the pan-tilt based on (9).
14:: Calculate $g_{1} (k)$ and update $n_{1}$ based on (41).
15:: end while

[ad_2]