Predictive Control for Chasing a Ground Vehicle using a UAV

TL;DR

This paper presents a model predictive control-based high-level planner enabling a UAV to chase a ground vehicle while respecting constraints, demonstrated through simulations and real-world experiments.

Contribution

It introduces a novel predictive control approach that integrates online data for real-time planning in UAV ground vehicle chasing scenarios.

Findings

Successful simulation results showing effective chasing behavior.

Real-world experiments validating the approach on a quadcopter.

The method respects state and input constraints during operation.

Abstract

We propose a high-level planner for a multirotor to chase a ground vehicle, while simultaneously respecting various state and input constraints. Assuming a minimal kinematic model for the ground vehicle, we use data collected online to generate predictions for our planner within a model predictive control framework. Our solution is demonstrated, both via simulations and experiments on a stable quadcopter platform.

Equations

$$G_{roll}(s)=\frac{a_r}{s^2+b_{r1}s+b_{r0}}$$

$$G_{pitch}(s)=\frac{a_p}{s^2+b_{p1}s+b_{p0}}$$

$$T=\frac{mg}{\cos \gamma }\gamma =\theta ,\varphi$$

$$\ddot{x}=g\theta$$

$$\ddot{y}=-g\varphi$$

$$\ddot{z}=\frac{T_z}{m}-g$$

$$\mathbf{X}=[x\dot{x}\theta \dot{\theta }y\dot{y}\varphi \dot{\varphi }z\dot{z}{]}^T$$

$$\mathbf{U}=[{\theta }_{cmd}{\varphi }_{cmd}{T}_z{]}^T$$

$$\dot{\mathbf{X}}=A\mathbf{X}+B\mathbf{U}+G$$

$$A=\left[\begin{array}{cccccccccc}0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& g& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& -b_{p0}& -b_{p1}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& -g& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& -b_{r0}& -b_{r1}& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$

$$B=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ -a_p& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& -a_r& 0\\ 0& 0& 0\\ 0& 0& \frac{1}{m}\end{array}\right]$$

$$G=[000000000-g{]}^T$$

$${\mathbf{X}}_{k+1}=A_T{\mathbf{X}}_k+B_T{\mathbf{U}}_k+G_T$$

$$A_T=e^{A\Delta T}{B}_T=({\int }_0^{\Delta T}{e}^{A(\Delta T-\tau )}d\tau )B$$

$$G_T=({\int }_0^{\Delta T}{e}^{A(\Delta T-\tau )}d\tau )G$$

$${\dot{\mathbf{X}}}_v={\mathbf{U}}_v$$

$${\mathbf{X}}_{v,k+1}={\mathbf{X}}_{v,k}+\Delta T{\mathbf{U}}_{v,k}$$

$${\mathbf{X}}_{k+1}=A_T{\mathbf{X}}_k+B_T{\mathbf{U}}_k+G_T$$

$${\mathbf{X}}_k\in \mathcal{X},{\mathbf{X}}_N\in {\mathcal{X}}_f,{\mathbf{U}}_k\in \mathcal{U}$$

$${\mathbf{X}}_0=\mathbf{X}(t)$$

$${\mathcal{B}}_f=X_c\oplus \{x\in {\mathbb{R}}^2:||x||\le \bar{V}N\Delta T\}$$

$${\mathbf{X}}_{vk}-{\mathbf{X}}_{vk-1}$$

$$\Rightarrow ||{\mathbf{X}}_{vk}$$

$$||X_{vk}-X_c||$$

$$\Rightarrow X_{vk}$$

$${\mathcal{X}}_{ft}={\mathcal{B}}_f\times [0,H]\times [e_2{e}_6{e}_{10}{]}^T\circ \mathcal{X}$$

$$\Phi =\left[\begin{array}{cccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$

$${\mathcal{X}}_{fr}=\Phi \circ \mathcal{X}$$

$${\mathcal{X}}_f=({\mathcal{X}}_{ft}\times {\mathcal{X}}_{fr})\cap \mathcal{X}$$

$$U_f({\mathbf{X}}_k)$$

$$\frac{[e_1{e}_5{]}^T(A_T-I){\mathbf{X}}_k+B_T^{†}u}{||[e_1{e}_5{]}^T(A_T-I){\mathbf{X}}_k+B_T^{†}u||}=\frac{X_c-[e_1{e}_5{]}^T{\mathbf{X}}_k}{||X_c-[e_1{e}_5{]}^T{\mathbf{X}}_k||}\}$$

$$\Delta x=e_1^T\circ ((A_T-I)\circ \mathcal{X}\oplus B_T\circ \mathcal{U})$$

$$\Delta y=e_5^T\circ ((A_T-I)\circ \mathcal{X}\oplus B_T\circ \mathcal{U})$$

Full text

I Introduction

Multirotor UAVs are expected to be applied widely in the realm of urban air mobility[1]. An interesting application in particular is the use of multirotors for chasing (or more formally, tracking) dynamic targets, owing to their agility and ubiquity. However, flying such vehicles in the city necessitates precise trajectory tracking of the vehicle and a framework that respects constraints– those on the vehicle itself, as well as those imposed by the environment. Solutions to certain aspects of the problem have been proposed in the literature; [2] and [3] focus on vision-based strategies to localize and track the target. More recently, [4] proposes a hybrid decision planner for docking UAVs on ground vehicles. We address the problem of trajectory and control design for UAVs to chase ground vehicles while adhering to constraints.

In this paper, we present a modular solution to the problem that can be applied to any commercial UAV platform with minimal experimental tuning- a high-level planner that

1. a)

incorporates data from the past to predict the position of the ground vehicle and, 2. b)

use the same data to generate waypoints that the quadcopter can follow without violating state and input constraints.

For (a), the vehicle position is predicted by assuming a kinematic model with bounded states, and updating the bounds from past data using exponential moving average. These continually updated bounds are used to construct a set of predicted positions, of which, the Chebyshev center ([5]) is used as the estimate of the vehicle’s future position. In the context of (b), we use model predictive control (MPC), a popular strategy for synthesizing controls for systems with constraints, for generating the waypoints by solving a constrained finite time optimal control problem online [5]. While the latter, in general, can be intractable for online computation owing to the often nonlinear dynamical behaviour of UAVs, we model our UAV in closed-loop with a low-level planner as a simple LTI system, thus making real-time, online computations feasible. We also present the control and communication architecture that enabled us to deploy our algorithm.

The rest of the paper is organized as follows. Section II formally states the problem along with the working assumptions before we present the theoretical framework for our solution in section III. Sections IV and V present simulations and experiments respectively that corroborate the effectiveness of our solution.

II Problem Formulation

We work with a quadcopter equipped with a low-level controller that linearizes the dynamics around the hover condition, and is capable of driving the quadcopter to specified way-points. To chase the ground vehicle, we aim to generate way-points for the quadcopter that can be reasonably tracked using the low-level controller. Towards this goal, we list the blanket assumptions and models we plan to use for this work, and conclude this section with a formal statement of the problem.

II-A Assumptions

• •

Accurate measurements of the states of the quadcopter are available.

• •

Accurate measurements of the position and velocity of the ground vehicle are available.

• •

Quadcopter is stable in hover with a low-level controller in loop.

• •

External disturbances are ignored owing to experiments being conducted in a spacious indoor testing arena.

• •

The velocity of the ground vehicle is bounded, and these bounds are known.

II-B Problem Statement

Assuming that the conditions in II-A hold, design a control strategy for a quadcopter UAV to chase an evasive ground vehicle while adhering to specified state and input constraints.

III Predictive Control for Trajectory Generation

We adopt a hierarchical control strategy:

• •

Off-board Control System: Use MPC to synthesize trajectories and send way-points to the quadcopter

• •

Onboard Control System: Drive the quadcopter towards the commanded way-points

The two parts are interconnected with radio/telemetry. In this section, we focus on the former and describe our approach to solve the problem.

III-A Quadcopter Model

We model the quadcopter with the on-board, inner control loop along the lines of [6] and [7]. The yaw angle is assumed fixed while the closed-loop roll and pitch dynamics are modelled as decoupled, second-order systems with input being a commanded angle (roll or pitch) and output being the actual angle (roll or pitch).

[TABLE]

[TABLE]

The parameters $a_{r},a_{p},b_{r1},b_{r0},b_{p1},b_{p0}$ are obtained by system identification.

Translational motion along the x axis is assumed to be coupled only with pitch and the same along the y axis is assumed to be coupled only to roll, and are thus, decoupled from each other. Moreover, we assume that the attitude dynamics in closed loop are significantly faster than the translational (specifically altitude) dynamics. So the thrust at any instant balances the weight and is given by

[TABLE]

So the lateral translational dynamics are given by

[TABLE]

[TABLE]

Finally, the altitude dynamics are simply given by

[TABLE]

To consolidate, the quadcopter is described by the states and inputs

[TABLE]

[TABLE]

and the state space model in continuous time is expressed as

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]

Assuming ZOH, we compute the exact discretization of these equations.

[TABLE]

where the matrices are defined below for discretization step $\Delta T$

[TABLE]

[TABLE]

III-B Vehicle Model

We assume a point mass kinematic model for the vehicle and we measure the position and velocity of the same. The states and inputs of the vehicle are given by $\mathbf{X}_{v}=[x_{v}\ y_{v}]^{T}$ and $\mathbf{U}_{v}=[v_{x}\ v_{y}]^{T}$ dynamics are written as

[TABLE]

In discrete time,

[TABLE]

At any instant, we assume that the velocity of the vehicle in its body-fixed frame lies in the set $\mathcal{B}_{v}=\{v\ :\ v\in\mathbb{R}^{2},\ ||v||\leq\bar{V},\ \arcsin(\frac{e^{T}_{1}v}{||v||})\in[\underline{\Theta},\bar{\Theta}]\}$ The bounds $\bar{V},\underline{\Theta}$ and $\bar{\Theta}$ are assumed to be known beforehand.

III-C Receding Horizon Control Problem for Tracking MPC

With design parameters $N$ (prediction horizon), $Q$ (state cost matrix) and $R$ (input cost matrix), we wish to solve the following optimization problem $(P)$ at time $t$,

minimize $\sum_{k=0}^{N}||\mathbf{X}_{k}-\mathbf{X}_{k}^{ref}||^{2}_{Q}+||\mathbf{U}_{k}||^{2}_{R}$

subject to

[TABLE]

$\mathcal{X}$ and $\mathcal{U}$ are the sets of feasible states and control inputs respectively. We assume that these sets are convex and preferably, polyhedral. $\mathcal{X}_{f}$ is the terminal constraint set and will be constructed in the sequel, along with a heuristic for designing the reference trajectory $\mathbf{X}^{ref}$ to make the quadcopter chase the ground vehicle.

The MPC algorithm prescribes using $U^{*}_{0}$ yielded by solving $(P)$ to obtain the next state $\mathbf{X}(t+\Delta T)$ using dynamics (1), and then solving $(P)$ again with $X_{0}=\mathbf{X}(t+\Delta T)$.

Construction of Terminal Set $\mathcal{X}_{f}$

Let $X_{c}$ be the current state of the ground vehicle. Define the set

[TABLE]

Lemma 1

For a vehicle governed by dynamics (3) and for the set $\mathcal{B}_{f}$ constructed as in (4), the following are true:

Set $\mathcal{B}_{f}$ is convex. 2. 2.

The vehicle positions lie within this set for all instants over the prediction horizon $N$.

**Proof

**Part 1)

$\mathcal{B}_{f}$ is a circle centered at $X_{c}$, which is convex.

Part 2)

At any instant $k$,

[TABLE]

Using a telescopic sum and the triangle inequality, we get

[TABLE]

$\blacksquare$

Construct the translational part of the terminal constraint set as

[TABLE]

where the $e_{i}$s are among the standard basis vectors of $\mathbb{R}^{10}$ and $H$ is the height from the ground within which we consider the quadcopter to have ”captured” the vehicle.

Now define following matrix which plucks out the rotational states from state vector $X$

[TABLE]

We use this to define the rotational part of the terminal constraint set

[TABLE]

and finally, use cartesian product to construct the terminal constraint set

[TABLE]

Lemma 2

The terminal constraint set given by (5) is control invariant for the quadcopter dynamics described by (1) with the terminal controller

[TABLE]

where $B^{\dagger}_{T}=[e_{1}\ e_{5}]^{T}B_{T}[e_{1}\ e_{2}]$, if the following conditions are met

$[-\bar{V}\Delta T,\bar{V}\Delta T]\subseteq\Delta x\cap\Delta y$* where*

[TABLE]

[TABLE] 2. 2.

The quadcopter platform has large enough $\mathcal{X}$ such that

[TABLE] 3. 3.

$\mathcal{U}$* contains an open ball around the origin $\mathbf{0}$*

**Proof

**Suppose that the state of the quadcopter at the $k$th instant belongs to the terminal set at the $k$th instant, i.e., $\mathbf{X}_{k}\in\mathcal{X}_{f}$. We want to show that $\mathbf{X}_{k+1}\in\mathcal{X}_{f}$ on applying control (2).

First, observe that the subsystem of the quadcopter comprising the $x,\ y$ translational dynamics and the pitch, roll dynamics is controllable with inputs $\theta_{cmd},\phi_{cmd}$. This implies that $B^{\dagger}_{T}$ is invertible, and moreover with condition (3), this ensures that the set $U_{f}(\mathbf{X}_{k})\neq\emptyset$. On applying this input, the quadcopter moves in the direction $\frac{X_{c}-[e_{1}\ e_{5}]^{T}\mathbf{X}_{k}}{||X_{c}-[e_{1}\ e_{5}]^{T}\mathbf{X}_{k}||}$.

At instant $k+1$, the horizontal distance between the quadcopter and the vehicle’s new position $X^{\prime}_{c}$ is given by

[TABLE]

Condition (1) ensures that quadcopter’s displacement in the direction $\frac{X_{c}-[e_{1}\ e_{5}]^{T}\mathbf{X}_{k}}{||X_{c}-[e_{1}\ e_{5}]^{T}\mathbf{X}_{k}||}$ exceeds $\bar{V}\Delta T$ and thus,

[TABLE]

[TABLE]

Note that the altitude dynamics is decoupled from the other state dynamics and $\mathbf{X}_{k}\in\mathcal{X}_{f}$. So, applying [math] thrust as prescribed by $U_{f}(\mathbf{X}_{k})$ results in

[TABLE]

(A), (B) and condition (2) together give

[TABLE]

$\blacksquare$

Construction of Set of Feasible Initial States $\mathcal{X}_{0}$

We construct $\mathcal{X}_{0}$ as the $N$-step reachable set to $\mathcal{X}_{f}$. For ease of computations, we construct the polyhedron that circumscribes the set $\mathcal{X}_{f}$ to get a (consequently) conservative estimate $\tilde{\mathcal{X}}_{0}\subset\mathcal{X}_{0}$.

Theorem 1

The MPC described in section (III-C) is recursively feasible if $X(0)\in\mathcal{X}_{0}$ and the conditions of lemma 2 hold.

**Proof

**Since $X(0)\in\mathcal{X}_{0}$, the RHC optimization problem $(P)$ is feasible over the first prediction horizon. Now suppose that $X(k\Delta T)\in\mathcal{X}_{0}$ and let $\{u^{*}_{0},u^{*}_{1},..,u^{*}_{N}\}$ be the optimal sequence obtained by solving $(P)$. Using the terminal controller $U_{f}(\mathbf{X}^{*}_{N})$ from lemma 2, we construct the sequence $\{u^{*}_{1},u^{*}_{2},..,U_{f}(\mathbf{X}^{*}_{N})\}$ which clearly yields a feasible solution to $(P)$ with $X_{0}=\mathbf{X}^{*}_{1}=X((k+1)\Delta T)$. Using induction on $k$, this shows that the MPC strategy is recursively feasible.

$\blacksquare$

III-D Reference Trajectory Generation for Tracking MPC

For constructing the reference trajectory $X^{ref}$ that is passed to the MPC algorithm, we fit a minimum jerk trajectory between the quadcopter’s current state and a point $H$ above an estimate of the vehicle’s future position. The roll, pitch and yaw angles are set to [math] at the terminal point. We now present a heuristic to get an estimate the vehicle’s future position.

Updating $\mathcal{B}_{v}$

We measure and store time series data for $X_{vk}=[x_{vk}\ y_{vk}]^{T}$ and $U_{vk}=[v_{xk}\ v_{yk}]^{T}$ over time instants in $[t-L\Delta T,t]$ where $t$ is the current time and $L$ is a design parameter. The velocity of the vehicle in the body-fixed frame is obtained as follows

[TABLE]

where

[TABLE]

and $\phi_{k}$ is the measured heading of the car.

Compute the average of the time-series data as $\tilde{v}$,$\tilde{\delta}$ and update the bounds $\bar{v}$, $\underline{\delta}$ and $\bar{\delta}$ as follows

[TABLE]

$\bar{V},\underline{\Theta}$ and $\bar{\Theta}$ are bounds that are known beforehand while $\beta_{v},\underline{\beta}$ and $\bar{\beta}$ are tuning parameters.

Prediction of the Vehicle’s Position

Obtain a sequence of car states $\{\underline{X}_{vk}\}_{k=0}^{N}$ using dynamics (3) initialized with the current measured state of the car, over the prediction horizon $N$, using inputs $\underline{U}_{k}$ given by

[TABLE]

Similarly, obtain a sequence of car states $\{\overline{X}_{vk}\}_{k=0}^{N}$ using dynamics (3) initialized with the current measured position of the car, over the prediction horizon $N$, using inputs $\overline{U}_{k}$ given by

[TABLE]

Let $\underline{X}_{v0}=\overline{X}_{v0}=X_{c}$ and compute

[TABLE]

[TABLE]

Define the set

[TABLE]

where

[TABLE]

Lemma 3

The set $\mathcal{C}_{f}$ constructed as in (11), is bounded and convex

**Proof

**Observe that by construction, $\mathcal{C}_{f}\subset\mathcal{B}_{f}$ and is hence, bounded.

$\mathcal{C}_{f1}$ is a sector of a circle that subtends an angle $\bar{\theta}-\underline{\theta}$ at the centre $X_{c}$ whereas $\mathcal{C}_{f2}$ is a triangle with the same vertices as that of the sector $\mathcal{C}_{f1}$.

Case 1: $\bar{\theta}-\underline{\theta}\leq\pi$

$\mathcal{C}_{f1}$ is convex and $\mathcal{C}_{f2}\subset\mathcal{C}_{f1}$.

Thus $\mathcal{C}_{f}=\mathcal{C}_{f1}\cup\mathcal{C}_{f2}=\mathcal{C}_{f1}$

$\Rightarrow\mathcal{C}_{f}$ is convex

Case 2: $\bar{\theta}-\underline{\theta}>\pi$

In this case, $\mathcal{C}_{f2}\not\subset\mathcal{C}_{f1}$ and the set $\mathcal{C}_{f}=\mathcal{C}_{f1}\cup\mathcal{C}_{f2}$ forms a segment of the circle centered at $X_{c}$, which is convex.

$\Rightarrow\mathcal{C}_{f}$ is convex

Cases 1 and 2 are exhaustive and hence, $\mathcal{C}_{f}$ is indeed convex.

$\blacksquare$

Proposition 1

The vehicle’s future position is estimated as the Chebyshev center of $\mathcal{C}_{f}$ (Figure 3) and this estimate belongs to $\mathcal{C}_{f}\subset\mathcal{B}_{f}$.

**Proof

**Lemma 3 directly gives the desired result.

$\blacksquare$

IV Simulations

Two simulated chasing experiments were carried out with a ROS-based quadcopter Simulation tool. The tool takes control commands via ROS communication, simulates the flight via propagating through a high fidelity quad-copter vehicle dynamics model, and returns full vehicle states. For the first simulated test, quad-copter was ordered to chase a ground vehicle moving in a circular trajectory. For the second simulated experiment, the quadcopter chased a car that randomly drove in a bounded square virtual test field.

IV-A Simulation-1 Result

Figure 4 shows the trajectory of a simulated quadcopter chasing a ground vehicle simulated to run in a circular path. Figure 5 shows a plot for the tracking distance error against time. We observe that the distance error quickly converges after MPC controller starts working and that we manage to contain the steady state error within about 0.25 m.

IV-B Simulation-2 Result

Figure 6 shows the trajectory of a simulated quadcopter chasing a ground vehicle simulated to randomly drive in a bounded square field. Figure 7 shows a plot for the tracking distance error against time. From the graph, we observe that the tracking error would increase when the vehicle make sharp turns, but the MPC controller manages to keep that value under 0.25 m.

V Experiments

V-A Experiment Setup

In this experiment, a RC car is manually controlled by a human driver and a quadcopter flies autonomously to chase the RC car. The motion of the car and the quad-copter is collected by the motion capture system and sent to a real-time MPC program, which generates a trajectory in terms of desired way-points and corresponding way-point velocities. The trajectory is then interpreted as radio commands and sent to the quadcopter to close the control loop. We work with a crazyflie-based quadcopter platform, which is equipped with a robust, low-level trajectory tracking controller. Figure 9 depicts the low-level control architecture of the miniquad platform.

V-B Experiment Result

Figure 10 shows the trajectory of an actual quadcopter chasing a remotely controlled ground vehicle that is driven by an operator in our laboratory space. Figure 11 shows a plot for the tracking distance error against time. From the graph, we observe that the tracking error increases when the vehicle accelerates, resulting in peaks. As time progresses, the predictive controller manages to reduce the error to under 1.0 m.

The video of the experiment can be seen here: https://goo.gl/eUEq2W

We can clearly observe the difference in performance between experiments and simulations. The experimental tracking error is about an order of magnitude higher than that of simulations. Following are the reasons for the inadequate performance of the experimental chase:

Model mismatch due to linearization of dynamics of the quadcopter in our control algorithm. 2. 2.

Time delays in sending and receiving radio messages to and from the quadcopter during experiments. 3. 3.

Measurement noise in sensors which leads to imperfect waypoint tracking.

VI Conclusions and Future Work

In this paper, we focused on designing a predictive controller for an aerial vehicle to chase a ground vehicle. We assume a model for the ground vehicle for estimating its position in future time instants. These estimates are used to generate waypoints for our aerial vehicle so that it can track the ground vehicle with an objective of minimizing the tracking error in distance.

We proved feasibility of the optimization problem that we solve. We then implemented this control algorithm for simulations and experiments.

In simulations, we achieved good tracking performance with tracking errors smaller than 0.25 m. However, in the case of experiments, we had much larger tracking errors owing to non-idealities such as model mismatch, time delays, and sensor noise.

In future work, we will characterize the tracking error bound, and incorporate probabilistic beliefs into our prediction strategy. Furthermore, while we have incorporated slacks variables in our optimization problems for this work, we could also tackle model mismatch via a robust MPC strategy. Lastly, waypoint tracking will be improved by commanding velocities and accelerations in addition to positions.

Acknowledgement

The experimental test bed at the HiPeRLab is the result of contributions of many people, a full list of which can be found at hiperlab.berkeley.edu/members/.