# Vector Field Following for Quadrotors using Differential Flatness
###### tags: `Paper`

[TOC]

## Introduction
### Scenario
<div style="text-align: justify">In tasks like navigation, obstacle avoidance, collision avoidance, swarming and flocking, controllers often provide a vector field for the robots to follow. </div>

### Problem
<div style="text-align: justify">This potential vector field provides spatial information as inputs for an open-loop controller which is effective with pseudo-static regime where dynamics doesn't play a major role, like slow moving robot or ground vehicles. In a complex regime where inertial take place, like an agile flight of UAV, open-loop controller with vector alone is not sufficient. Thus, how to effectively compensate the dynamics of an UAV so that it can follow the vector field becomes a problem.</div>

### Solution
<div style="text-align: justify">Exploiting the differential flatness property of UAV to obtain the 12D states of quatrotor from a desired position and yaw angle, which are from the vector field, and their time derivatives. The obtain states are then be used to produce a closed-loop feedback controller instead of a open-loop controller commonly seen in trajectory tracking.</div>

### Main Contribution
<div style="text-align: justify">Derivation of the instantaneous velocity, acceleration, jerk, and
snap required in the quadrotors endogenous transformation analytically from a given vector field. Then, using these derived flat outputs to reproduce the ideal states and control inputs at a specified location as reference signal. </div>

把vector field作微分算出理想的無人機state做feedback來達到更穩定的路徑追蹤?

## Related Work

## Methodology

### Differential Flatness

* Concept:
Property of differential flatness allows a system to have length N vector of states and inputs be mapped to a length M vector of 'Flat Outputs', where M < N. In trajectory planning, the flat outputs and their time derivatives can be used to reproduce the trajectory for the UAV to follow.

* Definition:
A nonlinear control system
$$
\dot{\xi} = f(\xi, \mu), where \ \xi\ is\ state\ and\ \mu\ is\ control\ input
$$
is differentially flat if there exists an invertible function
$$
\sigma = \alpha(\xi, \mu, \dot{\mu}...,\mu^{(p)}),\ where\ p\ is\ finite\ and\ \sigma\ is\ the\ flat\ output
$$

### Quadrotors Dynamics
* Coordinate frame in NED world frame

* Dynamics
$$
\upsilon = [\upsilon_x, \upsilon_y, \upsilon_z]^{T}\ in\ world\ frame.\\
g:\ acceleration\ due\ to\ gravity,\quad m:\ mass,\quad f_z:\ the\ total\ thrust,\quad e_3 = [0,0,1]^{T}\\
\omega_b = [p,q,r]^{T}:\ angular\ velocity\ in\ F_b,\quad \Omega = \hat{\omega_b} = \begin{bmatrix}
0 & -r & q \\
r & 0 & -p \\
-q & p & 0
\end{bmatrix}\\
\tau = [\tau_x,\tau_y,\tau_z]^{T}:\ torque\ in\ F_b,\quad J:\ inertia\ matrix,\quad h = [x,y,z]^{T}:\ position\ in\ F_w
$$

* Control Inputs and States
$$
\mu = [f_z, \tau_x, \tau_y, \tau_z]^{T}\\
\xi = [x,y,z,\upsilon_x,\upsilon_y,\upsilon_z,\psi,\theta,\phi,p,q,r]^{T}
$$

### Endogenous Transformation

* Definition
The inverse of &alpha; function in differential flatness gives the trajectory &xi; and &mu; as functions of the flat outputs &sigma; and its q time derivatives.
$$
\xi = \beta(\sigma, \dot{\sigma}, ..., \sigma^{(q)})\\
\mu = \gamma(\sigma, \dot{\sigma}, ..., \sigma^{(q)})\\
(\beta,\gamma)\ is\ called\ endogenous\ transformation
$$

* Flat outputs to Quadrotor's state and control input
:::success
The detailed derivation is from [Minimum Snap Trajectory Generation and Control for Quadrotors](https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=5980409&casa_token=4ATiWNNMysQAAAAA:pkJs-0faajbNDGTlEtBfGj56aw62yX9Q5v0jXCaoxdXBa4Pnw40QhRr1rRbdPZdHW9y19Wb1&tag=1)
:::
$$
\sigma = [\sigma_1,\sigma_2,\sigma_3,\sigma_4]^{T} := [x,y,z,\psi]^{T}\\ s.t\\
$$
$$
\xi = \beta(\sigma_1,\sigma_2,\sigma_3,\sigma_4),\\
with\
\begin{cases}
[x,y,z,\upsilon_x,\upsilon_y,\upsilon_z,\psi]^{T}=\beta_{1:7}(\sigma,\dot{\sigma})=[\sigma_1,\sigma_2,\sigma_3,\dot{\sigma_1},\dot{\sigma_2},\dot{\sigma_3},\sigma_4]^{T}\\
\theta=\beta_8(\sigma,\dot{\sigma},\ddot{\sigma})=atan2(\beta_a,\beta_b)\\
\phi=\beta_9(\sigma,\dot{\sigma},\ddot{\sigma})=atan2(\beta_c,\sqrt{\beta_a^{2}+\beta_b^{2}})\\
[p,q,r]^{T}=\beta_{10:12}(\sigma,\dot{\sigma},\ddot{\sigma}, \dddot{\sigma})=(R^{T}\dot{R})^{V}\\
\end{cases} \\
where\
\begin{cases}
\beta_a = -cos\sigma_4\ddot{\sigma_1}-sin\sigma_4\ddot{\sigma_2}\\
\beta_b=-\ddot{\sigma_3}+g\\
\beta_c=-sin\sigma_4\ddot{\sigma_1}+cos\sigma_4\ddot{\sigma_2}
\end{cases} \\
$$
$$
\mu = \gamma(\dot{\sigma},\ddot{\sigma},\dddot{\sigma},\ddddot{\sigma}),\\
with\
\begin{cases}
f_z = \gamma_1(\sigma, \dot{\sigma}, \ddot{\sigma}) = -m||\ddot{\sigma_{1:3}}-ge_3||\\
[\tau_x, \tau_y, \tau_z]^{T}=\gamma_{2:4}(\dot{\sigma},\ddot{\sigma},\dddot{\sigma},\ddddot{\sigma})=J(\dot{R}^{T}\dot{R}+R^{T}\ddot{R})^{V}+R^{T}\dot{R}J(R^{T}\dot{R})^{V}
\end{cases} \\
$$

### Vector Field Following (Main Contribution)
Suppose
$$
\sigma_4 = 0
$$
At any given point in vector field x,
J(f(x),x) denotes the Jacobian matrix of the function f(x)

### Controller (SE(3) controller)

&xi; is composed of [x, &upsilon;, R, &Omega;], and the reference &xi; can be derived from the vector field.
## Simulation and Experiments

### Experiment Setup

### Limit Cycle
* Simulation

* Experiment
[video](https://www.youtube.com/watch?v=eOT7Rli_P-c)

### Logarithmic Spiral
* Simulation

* Experiment
[video](https://www.youtube.com/watch?v=z8TK1dNHSDk)

### Obstacle Avoidance
* Simulation

* Experiment
[video1](https://www.youtube.com/watch?v=fhVwrt4QDQk)
[video2](https://www.youtube.com/watch?v=omgquupTz3A)

## Thoughts and Questions

1. This paper provides a method to utilize vector field for controlling an agile quadrotors without computing an explicit trajectory to follow.

2. Where is the "closed-loop"?
The computed state from flat output can act as a reference signal for compensation.

3. No detailed controller design diagram is provided. Only mentioned a closed-loop controller with an inner-loop is implemented.

4. The actual contribution is combining vector field directly with the SE(3) controller with the ability of compensating the nonlinear dynamics, without having to do explicit trajectory planning again to obtain control signal. ???

5. Experiment doesn't show the result without their 'inner-loop', which is directly utilize the vector field on an aggressive quadrotor.

6. Normally we need to calculate an explicit trajectory from methods like QP then use differential flatness property to obtain the control inputs and states to control the quadrotor. In this paper, QP for trajectory planning is neglected. Instead, calculation of states and inputs is directly from the provided vector field.