A simultaneous trajectory generation method for quadcopter intercepting ground mobile vehicle

Abstract

This article proposes a trajectory generator for quadcopter to intercept moving ground vehicle. For this air–ground interaction problem, we formulate the trajectory generation problem as quadratic dynamic programming in a moving-horizon scheme based on the quadcopter kinematics and observation to ground vehicle. The closed-form solution of quadratic dynamic programming in each iteration enables this algorithm a real-time replanning performance. Thereafter, segmented trajectory rule, inspired from commercial flight landing regular, is implemented to guarantee smoothness in approaching and interception to moving ground target from comparably far origin. Our established algorithm is verified through both simulations and experiments.

Introduction

Nowadays, micro-aerial vehicle (MAV) is becoming a hot research topic. In military missions, MAV would be widely used in scouting, communication, and transportation in the near future. In civilian aspect, some E-commerce companies have made attempts using MAV to finish “the last mile” delivery to customer. MAV with multi-copter structure has special advantage compared with traditional unmanned aerial vehicle, leading to an easy implementation in indoor environment. Also, the development in micro-electro-mechanical system and smart sensor extends the range of MAV applications. Improvement in computation power makes it possible to execute complex tasks such as real-time searching and tracking on moving target.

In an air-to-ground interaction task, computer vision system and trajectory generation algorithm are equally important, but facing different aspects. The major concerning of vision system in the tracking problem is the reliability in a complex dynamic background. Vision-based tracking and landing is proposed in the study by Wenzel et al. ¹ with a basic PID controller.[Please provide expansions for “PID, PD, and GPS.”] On the other hand, air–ground interaction requires a fast and accurate aggressive approaching maneuver based on trajectory generation and control. However, major part of such interaction is finished by height-keeping tracking flight followed by vertical landing ² or simply track a moving target without interception. ^3,4

Aggressive maneuver and trajectory generation of a quadrotor have been widely investigated. General Tau theory is introduced in quadrotor perching movement ⁵ and four-dimensional trajectory planning. ⁶ Minimum snap metric (e.g. two-norm of the fourth derivative of position) is proposed in the studies by Mellinger and Kumar ⁷ and Richter et al., ⁸ which generates an aggressive trajectory within a tight constraint. Being motivated by Mellinger and Kumar, ⁷ an avian-inspired approach is proposed in the study by Thomas et al. ⁹ for quadrotor grasping. In the literature above, polynomial curves are used to approximate a trajectory, where coefficients are then determined by numerical optimization with respect to a given cost functional. To the best of our knowledge, the minimum snap method is not yet been used in dynamic scenario which needs a real-time replanning capability.

The main contribution of this article is twofold: (1) the dynamic programming (DP) trajectory generation approach for a moving ground target interception and (2) the segmented trajectory rule (STR). Trajectory on DP does not require an explicit dynamic model when generating the trajectory. All steps are calculated analytically to reduce the computational burden. STR is an empirical scheme rather than a single algorithm, which performs well in the experiments. To the best of our knowledge, this kind of scheme is new to the air–ground interception problem of MAV.

Compared to the tracking-landing scheme of a quadcopter toward a moving ground target, which focuses mainly on the error-feedback control, this hierarchical structure avoids the stabilization and convergence problems in the single control problem. Also, a smooth trajectory is much easier for quadcopter to follow, especially when the starting point is comparably far from the target. The major difference between minimum snap approach and this DP approach is that the former generates a one-time optimal trajectory through multiple predefined waypoints, while our proposed DP approach generates trajectory iteratively with the new target states given in an updated way. Therefore, the DP approach in this article is capable of real-time replanning in a dynamic environment. Also, the polynomial trajectory with the minimum snap metric requires high-quality modeling of a quadcotper, because the control sequence is directly depending on the motors speed. While our approach is independent of the controller, which is more suitable for practical implementation.

We organize the rest of this article as follows. The section “Problem formulation” briefly formulates this problem as trajectory intersection of quadcopter and ground vehicle. The section “Dynamic programming” gives the derivation of quadratic DP. The section “Multi-level implementation of DP on quadcopter” gives a detailed description of trajectory implementation on quadrotor and STR. The sections “Simulation ” and “Experiments ” introduce simulation and experiment results, respectively. We close the article by drawing a conclusion and discussing some potential research topics in the section “Conclusion.”

Problem formulation

To acquire a trajectory approaching and intercepting mobile ground vehicle, the algorithm requires replanning capability. The trajectory tracking controller also needs to keep up with the planner. The trajectory can be a sequence of frames (attitude, velocity, or waypoint). Acceleration control (attitude mode) ^7,8 is the fastest-responding approach, since the rotation speed of propeller is directly ordered. But this type of trajectory planning requires high-quality modeling of the physical system. Position control ¹⁰ has independent path planning and tracking part but needs the tracking controller responding fast. Otherwise the reaction to target position update will be very slow. Velocity mode ¹¹ provides smart respondence to dynamic target, while an outer-loop controller guarantees the position convergence.

Quadrotor dynamics and control

As a typical structure of MAV, quadrotor has great advantages in mechanical simplicity and application extension. Four propellers distributing around the quadrotor provide thrust to quadrotor flight. The orientation of total thrust is always vertical to the quadrotor body. This property is important for deducing the Euler angle set point, after the trajectory is generated as accelerations at three axial directions in the section “Multi-level implementation of DP on quadcopter.”

To describe the kinematics and dynamics of a quadrotor, two right-handed coordinates, earth frame E and body frame B, are set. The earth frame is fixed on the ground, considered as an inertial frame. And the body frame is fixed on the quadrotor. Twelve variables in total are used to describe the full states (Figure 1):

• position on the ground frame (p_x, p_y, p_z);

• Euler angle roll, pitch, and yaw (φ, θ, ψ);

• velocity refers to the earth frame (v_x, v_y, v_z); and

• angular rate toward body axis (ω_x, ω_y, ω_z).

Figure 1.

Dynamics of quadrotor is highly nonlinear. Except basic body dynamics, motor, propeller, and battery also have complicated relations.

By neglecting the nonlinear relationship between the lift force and the rotation speed, we derive the following simplified translation equations

$\{\begin{array}{c}{\dot{p}}_x=v_x\\ {\dot{p}}_y=v_y\\ {\dot{p}}_z=v_z\end{array}$

1

and the dynamics of Euler angles

$\{\begin{array}{l}\dot{\phi }={\mathit{\omega }}_x+{\mathit{\omega }}_y\cdot \text{sin}\phi \cdot \text{tan}\mathit{\theta }+{\mathit{\omega }}_z\cdot \text{cos}\phi \cdot \text{tan}\mathit{\theta }\hfill \\ \dot{\mathit{\theta }}={\mathit{\omega }}_y\cdot \text{cos}\phi +{\mathit{\omega }}_z\cdot \text{sin}\phi \hfill \\ \dot{\psi }=({\mathit{\omega }}_y\cdot \text{sin}\phi +{\mathit{\omega }}_z\cdot \text{cos}\phi )/\text{cos}\mathit{\theta }\hfill \end{array}$

2

where ω_x, ω_y, and ω_z are angular velocities.

Define control variable U₁ as the total thrust, the acceleration on earth frame can be defined as

$\{\begin{array}{l}{\dot{v}}_x=(\text{sin}\psi \cdot \text{sin}\phi +\text{cos}\psi \cdot \text{sin}\mathit{\theta }\cdot \text{cos}\phi )U_1/m\hfill \\ {\dot{v}}_y=(-\text{cos}\psi \cdot \text{sin}\phi +\text{sin}\psi \cdot \text{sin}\mathit{\theta }\cdot \text{cos}\phi )U_1/m\hfill \\ {\dot{v}}_z=(U_1\cdot \text{cos}\phi \cdot \text{cos}\mathit{\theta }-m\cdot g)/m\hfill \end{array}$

3

For the derivative of angular rate, three parts are taken into consideration: the torque generated by propeller’s lift, the angular momentum of body, and gyroscopic effect of propellers. Combine all three parts together, and then the differential equations to the angular velocity are as follows

$\{\begin{array}{l}{\dot{\mathit{\omega }}}_x=[\sqrt{2}\cdot l\cdot U_2+{\mathit{\omega }}_y\cdot {\mathit{\omega }}_z\cdot (I_y-I_z)-J_{TP}\cdot {\mathit{\omega }}_y\cdot \Omega ]/I_x\hfill \\ {\dot{\mathit{\omega }}}_y=[\sqrt{2}\cdot l\cdot U_3+{\mathit{\omega }}_x\cdot {\mathit{\omega }}_z\cdot (I_z-I_x)-J_{TP}\cdot {\mathit{\omega }}_x\cdot \Omega ]/I_y\hfill \\ {\dot{\mathit{\omega }}}_z=[\sqrt{2}\cdot U_4+{\mathit{\omega }}_y\cdot {\mathit{\omega }}_x\cdot (I_x-I_y)]/I_z\hfill \end{array}$

4

In the set of equations above, U₁, U₂, U₃, and U₄ are the controller outputs of four feedback loops for z, φ, θ, and ψ. They are manipulated by the rotation rate of four propellers. The control outputs are decoupled to rotation speed of each motor ω_i using the equations below in real experiments. This is a typical roll–pitch–height controller used in the section “Fixed-point one-step approaching”

$\{\begin{array}{l}{U}_1=k_{p1}\cdot (z_t-z(t))-k_{d1}\cdot \dot{z}+k_{i1}{\displaystyle {\int }_0^t}[z_t-z(s)]ds\hfill \\ U_2=k_{p2}\cdot ({\phi }_t-\phi (t))-k_{d2}\cdot \dot{\phi }\hfill \\ U_3=k_{p3}\cdot ({\mathit{\theta }}_t-\mathit{\theta }(t))-k_{d2}\cdot \dot{\mathit{\theta }}\hfill \\ U_4=k_{p4}\cdot ({\psi }_t-\psi (t))-k_{d4}\cdot \dot{\psi }\hfill \end{array}$

5

where k represent the coefficients of PID controllers.

For all the control output, single-loop PID is used for four states, z, φ, θ, and ψ. Conventionally, a simple PD controller is good enough to keep a quadrotor hovering. But in purpose of eliminating the steady-state error in height control, integral part is added to the controller. There are different kinds of flight controllers which have been proposed and widely used.

Ground vehicle interception

Assumption 1

The basic motion behaviors of ground vehicle are straight-line motion and turning. The motion is along with random small changes in direction. The time and degree turn is prior unknown.

We use a kinematic model to describe the motion of each ground target as equations (6) and (7). $\mathbf{q}=[x,y,\mathit{\theta }]$ are the states of the ground vehicle. This model is commonly used in path-planning algorithms such as Dubin’s path ¹² for wheeled robots

${\dot{q}}_s=\{\begin{array}{l}\dot{x}=v_0\text{cos}\mathit{\theta }\hfill \\ \dot{y}=v_0\text{sin}\mathit{\theta }\hfill \\ \dot{\mathit{\theta }}=\nu +\eta \hfill \end{array}\text{for straight-line motion}$

6

$\text{or} {\dot{q}}_t=\{\begin{array}{l}\dot{x}=0\hfill \\ \dot{y}=0\hfill \\ \dot{\mathit{\theta }}={\mathit{\omega }}_0\hfill \end{array}\text{for steep}\hspace{0.17em}\text{turn}$

7

where the linear motion velocity, heading angle, and angular velocity are denoted as v₀, θ, and ω. The random noise and constant unknown disturbance are denoted as ν and η, respectively. Thus, the trajectory of ground vehicle during a period of time with steep turning is

$q(a,t_a)={\displaystyle {\int }_{t_0}^{t_a}}{\dot{q}}_s\hspace{0.17em}\text{d}t+{\displaystyle {\int }_{t_a}^{t_a+t_t}}{\dot{q}}_t\hspace{0.17em}\text{d}t+{\displaystyle {\int }_{t_a+t_t}^{t_f}}{\dot{q}}_s\hspace{0.17em}\text{d}t$

8

where t₀ is the initial time, t_a is the initial turning time, t_t is the turning duration, and t_f is the terminal time.

To stop the ground vehicle, the trajectory of quadcopter and ground vehicle should intersect at the end

$x(t_f)=q(t_f)$

9

where t_f is the final time.

Assumption 2

Because turning time and degree are unpredictable, we allow the quadcopter hold the approaching process on the air if the ground vehicle begins a turning. The quadcopter starts planning until the ground vehicle starting another straight-line motion.

Dynamic programming

Basic quadratic DP

DP ¹³ is a method to break an optimization problem into easy-to-solve subproblems. Consider a general form of time-varying discrete system

$x_{k+1}=A_k{x}_k+B_k{u}_k$

10

and the objective function

$J(x,u)={\displaystyle \sum _{k=1}^N}{J}_k(x_k,u_k)$

11

where x and u represent the system state and control input, respectively. The notation J_k denotes the immediate cost of each time step. Define the optimal cost-to-go function C_i from current time step i as

$C_i=\underset{\left\{u_k\right\}}{{\displaystyle \text{min}}}{\displaystyle \sum _{k=i}^N}{J}_k(x_k,u_k)$

12

where C_i is a minimal value of J. This derivation leads to the Hamilton–Jacobi–Bellman (HJB) equation, which is generally a nonlinear partial differential equation. The HJB equation is usually solved backward in time. We derive Bellman equation and show the backward recursion of the cost-to-go value function

$C_i=\underset{u_i}{{\displaystyle \text{min}}}(J_i+C_{i+1})$

13

To make this problem easy to solve, we describe J_i as a general quadratic form

$J_i={\mathit{\gamma }}_i+x_i^T{\text{m}}_i+u_i^T{\text{h}}_i+\frac{1}{2}(x_i^T{Q}_i{x}_i+2x_i^T{R}_i{u}_i+u_i^T{S}_i{u}_i)$

14

where $\mathit{\gamma },\mu ,\eta ,Q,R,\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\text{and}\hspace{0.17em}\hspace{0.17em}S$ are predefined coefficient vectors and weight matrices. Since the objective function is quadratic, the optimal cost-to-go function should also be in general quadratic form

$C_i(x_i)={\mathit{\zeta }}_i+x_i^T{\nu }_i+\frac{1}{2}{x}_i^T{W}_i{x}_i$

15

According to the studies by Starr et al. and Palunko et al., ^13,14 the backward recursion of the coefficients in C_i are as follows

$\begin{array}{c}{\mathit{\zeta }}_i={\mathit{\zeta }}_{i+1}+{\mathit{\gamma }}_i-\frac{1}{2}{({\eta }_i+B_i^T{\nu }_{i+1})}^T\\ {(S_i+B_i^T{W}_{i+1}{B}_i)}^{-1}({\eta }_i+B_i^T{\nu }_{i+1})\\ {\nu }_i=\hspace{0.17em}(\hspace{0.17em}{\mu }_i+A_i^T{\nu }_{i+1})-(R_i+A_i^T{W}_{i+1}{B}_i)\\ {(S_i+B_i^T{W}_{i+1}{B}_i)}^{-1}({\eta }_i+B_i^T{\nu }_{i+1})\\ W_i=(Q_i+A_i^T{W}_{i+1}{A}_i)-2(R_i+A_i^T{W}_{i+1}{B}_i)\\ {(S_i+B_i^T{W}_{i+1}{B}_i)}^{-1}({\eta }_i+B_i^T{\nu }_{i+1})\end{array}$

16

The optimal control sequence to minimize J can be obtained by solving the first-order optimality condition

$\begin{array}{c}{u}_i=-{(S_i+B_i^T{W}_{i+1}{B}_i)}^{-1}[(R_i+A_i^T\\ W_{i+1}{B}_i{)}^T{x}_i+({\eta }_i+B_i^T{\nu }_{i+1})]\end{array}$

17

Online quadratic DP

A one-step optimal trajectory is not adequate in the dynamic scenario which both target vehicles and (possible suddenly appeared) obstacles keep moving. The requirement of the DP method here is to provide trajectory with real-time replanning capability. Concept of receding horizon optimization is introduced as a combination of DP, called quadratic DP. Denote j as the jth recursion. When the target is moving, the target state $x_{G_j}$ is updated in time.

Denote N as the planning step length and M < N as the applied step length during which the quadrotor really tracks this trajectory (e.g. N = 400, M = 100 in Figure 2). After M steps, the quadrotor generates a new trajectory segment aiming at new target position $x_{G_j}$ with a new initial state $x_{t_{j+1}}$ . As $x_{M_j}$ keeps the position and velocity data of the last recursion, we can choose to transmit final velocity information in $x_{M_j}$ to the initial state $x_{t_{j+1}}$ in the next recursion. This makes the trajectory smooth as in the moving blue boxes in Figure 2. Denote $f_{\text{dp}}(x_0,x_G)$ as one recursion in quadratic DP to generate a trajectory section, the recursive implementation can be referred as follows

$\begin{array}{l}u(t)=\{\begin{array}{cc}{f}_{\text{dp}}(x_0,x_{G_0}),& t_0\le t<t_1\\ f_{\text{dp}}(x_{t_1},x_{G_1}),& t_1\le t<t_2\\ ⋮& \\ f_{\text{dp}}(x_{t_{F-1}},x_{G_F}),& t_{F-1}\le t<t_F\end{array}\hfill \end{array}$

18

Figure 2.

Comparison between one-step DP and QDP. Blue box is the window of each recursion in x-axis. DP: dynamic programming; QDP: quadratic dynamic programming.

Figure 2 shows the comparison between one-step DP (dash line) and quadratic DP (solid line) toward a fixed-point target. One-step DP generates an S-shape trajectory with clear acceleration and deceleration process with small overshoot, while quadratic DP shows a similar tendency but be slower to arrive the set point.

Multi-level implementation of DP on quadcopter

Trajectory to attitude control sequence

To reduce the computational burden of DP, we use the kinematic model instead of dynamic model. The kinematic model is a double integrator, which takes only translation of the quadrotor into consideration as a mass dot

$\begin{array}{l}\left(\begin{array}{c}{p}_k\\ v_k\end{array}\right)=\left(\begin{array}{cc}{I}_{3\times 3}& dT\cdot I_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}\end{array}\right)\left(\begin{array}{c}{p}_{k-1}\\ v_{k-1}\end{array}\right)+\left(\begin{array}{c}\frac{d{T}^2}{2}\cdot I_{3\times 3}\\ dT\cdot I_{3\times 3}\end{array}\right)a_k\hfill \end{array}$

19

where p, v, and a are position, velocity, and acceleration, respectively. Therefore, we obtain a time-invariant state space (A, B) instead of time varying (A_k, B_k). We define quadrotor states as $x=[p_x,p_y,p_z,v_x,v_y,v_z{]}^T$ , containing position and linear velocity of quadrotor on earth frame. $u=[a_x,a_y,a_z{]}^T$ refers to the linear acceleration in the earth frame.

We also define the objective function (14) as a minimization problem

$J_i=\frac{1}{2}{x}_i^{T}Q{x}_i+\frac{1}{2}{u}_i^{T}S{u}_i+\frac{1}{2}{(x_i-x_G)}^{T}P(x_i-x_G)$

20

where x _G is the target state. Q, S, and P are adjustable coefficient matrices. Therefore, the coefficients in equation (15) can be initialized as ${\mathit{\zeta }}_N={\mathit{\gamma }}_N$ , ${\nu }_N={\mu }_N$ , and $W_N=Q_N$ .

As u is the linear acceleration vector of a quadrotor on the earth frame, we must transfer it to control commands that the quadrotor can understand. A quadrotor has two common flight modes: roll–pitch–throttle controller (attitude mode) and x–y–z controller (position mode). Position mode can keep catching up with waypoints but needs external equipment such as GPS. Therefore, we consider the attitude mode first, which is more direct and fast responding. In the attitude mode, quadrotor follows control command on throttle s_u, roll s_φ, and pitch s_θ.

Considering the Z–Y–X Euler angle, the rotation matrix from earth frame to the body frame is derived as follows ¹⁵

$\begin{array}{l}{M}_{be}=M_{eb}^T\\ =\left(\begin{array}{ccc}c\mathit{\theta }c\psi & c\mathit{\theta }s\psi & -s\mathit{\theta }\\ s\phi s\mathit{\theta }c\psi -c\phi s\psi & s\phi s\mathit{\theta }s\psi +c\phi c\psi & s\phi c\mathit{\theta }\\ c\phi s\mathit{\theta }c\psi & c\phi s\mathit{\theta }s\psi -s\phi c\psi & c\phi c\mathit{\theta }\end{array}\right)\end{array}$

21

where s and c represents sine and cosine functions, respectively.

Not losing the generality, we lock ψ = 0 in equation (18), and add gravity coefficient g on u as $g+u=[a_x,a_y,a_z+g{]}^T$ , then normalize it to σ. Since the thrust of a quadrotor should always be on the z_b of its body frame, we derive this relation

$\begin{array}{ll}\mathit{\sigma }\hfill & =\frac{g+u}{\left|\right|g+u\left|\right|}=\left(\begin{array}{ccc}c\mathit{\theta }& s\phi s\mathit{\theta }& c\phi s\mathit{\theta }\\ 0& c\phi & -s\phi \\ -s\mathit{\theta }& s\phi c\mathit{\theta }& c\phi c\mathit{\theta }\end{array}\right)\hspace{0.17em}\hspace{0.17em}\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)\hfill \end{array}$

22

and control set point $s_u,s_{\phi },\hspace{0.17em}\hspace{0.17em}\text{and}\hspace{0.17em}\hspace{0.17em}{s}_{\theta }$ (throttle, roll, and pitch, resp.) as follows

$\begin{array}{l}{s}_u=\parallel u+g\parallel \hfill \\ s_{\phi }=\text{arcsin}(-{\mathit{\sigma }}_y)\hfill \\ s_{\theta }=\text{arcsin}\left(\frac{{\mathit{\sigma }}_x}{\text{cos}(s_{\phi })}\right)\hfill \end{array}$

23

We can put s_u, s_φ, and s_θ as the set point command sequences for a quadrotor in the attitude mode to follow.

Position feedback

For a quadrotor navigating in the environment, though acceleration control signal is more straightforward, the difference between the PID output and the set point may lead to a position drift. For a nonlinear system, it could be linearized as

$\begin{array}{c}{\dot{x}}^{*}+\dot{\tilde{x}}=f(x^{*}+\tilde{x},u^{*}+\tilde{u})\\ \approx f(x^{*},u^{*})+{\frac{\mathit{\partial }f}{\mathit{\partial }x}|}_{(x^{*},u^{*})}\tilde{x}+{\frac{\mathit{\partial }f}{\mathit{\partial }u}|}_{({\text{x}}^{*},u^{*})}\tilde{u}\end{array}$

24

where x* refers to the optimal trajectory and $\tilde{x}$ refers to as the deviation. The state drift can be updated by state space

$\dot{\tilde{x}}\approx A(x^{*}(t),u^{*}(t))\tilde{x}+B(x^{*}(t),u^{*}(t))\tilde{u}$

25

and then position feedback is introduced by a simple feedback gain K as

$\begin{array}{ll}u(t)\hfill & =u^{*}(t)+\tilde{u}(t)=u^{*}(t)-K[x(t)-x^{*}(t)]\hfill \end{array}$

26

The above equations give a general scheme of feedback control on position drift in tracking the trajectory. The trajectory tracking control is not discussed in detail since there are various methods on this topic. In our implementation, we use a cascade control scheme on the horizontal plain, with PD for inner loop and PID for outer loop, and a single-loop PID controller on height/throttle.

For the controller’s design, the parameters are handcrafted. This exactly goes specific to the aerial platform itself. However, we use the parameter set which performs better in attitude stabilization, hovering, and fixed waypoint flight, no special treatment to this DP trajectory is implemented.

Segmented trajectory rule

To handle the complexity of the approaching and interception process (e.g. sudden turning, obstacles, and distant origin), we are inspired by the regular commonly used in air traffic control. ¹⁶ Airfield traffic pattern is a typically rectangular standard path that commercial flights commonly follow before landing for flight safety. We use a similar scheme called STR, which divides the whole process into three segments (commonly using term “leg” in air traffic control): cruising, tracking, and final approaching.

The cruising segment is a long and high-speed path aiming at the target. Since the maximum speed of the quadrotor platform in our experiment is 10 times larger than the ground vehicle, the movement of target vehicle during this time is small. Therefore, the path is approximately linear as in Figure 3. The target state x _G is updated by the attitude of ground vehicle x _U with an additive safety offset x _S .

Figure 3.

Segmented trajectory pattern: cruising, tracking, and approaching.

In the tracking segment, the quadrotor follows the target vehicle with the same velocity and heading direction. During this period, quadrotor recovers from the aggressive attitude and large absolute velocity in the cruising segment, catches up with the target, and prepares for the final approaching. The velocity of sideslip should be reduced before starting to approach.

Unlike both the cruising and the tracking segment, the approaching segment is more likely to be a problem of prediction. The target in the approaching segment is a “fixed” interception point. But this fixed point is also updated by the current vehicle state x _U and the remaining time T_r. A simple extrapolating algorithm is used to predict interception point f_pre.

Simulation

In this section, we use MATLAB simulation to verify the effectiveness of our method. Our simulation system has two major packages: trajectory generator to give a command sequence as algorithm 1 and dynamic model of quadrotor as in section “Problem formulation” to track the trajectory. The dynamic model part is implemented with the solver ode45. The physical parameters in the dynamic model are identified from a real quadrotor platform accordingly.

As a real-world implementation for air–ground interception, shepherd mission in the International Aerial Robotics Competition (IARC) Mission-7 is a challenging task which have in its fourth year. The ground vehicle’s behavior in simulations and experiments are modeled the same as IARC Mission-7 rule ¹⁷ for testing. The statement goes specific to competition for the purpose of demonstrating a real implementation performance on a real hardware platform. The behavior of the ground robot also imitates a typical ground target (straight line, turning with noise, sudden stop, and U-turn). We use no prior knowledge of ground vehicle motion rule (velocity, turning time, disturbance, etc.) for trajectory generation in simulations and experiments.

Fixed-point one-step approaching

First simulation aims to verify the basic performance when the target is fixed. In this simulation, DP is implemented for only one-step, while position feedback as in section “Position feedback” is only implemented on height, which makes this quadrotor in the roll–pitch–height mode. In the simulation, the initial state of quadrotor is set as $x_0{=[1,1,1]}^T$ , and the approaching target is fixed at $x_G{=[5,3,0.3]}^T$ . The discretization is dT = 0.01 s.

Figure 4(a) shows the trajectory in the earth frame. The dashed line is the trajectory generated by DP, while the solid line is the position of quadrotor tracking of the trajectory. The quadrotor gets close to the target through a relatively smooth curve within a short period of time (300 steps, 3 s). There is a lag between the planned path and the real one, due to the coarse single-loop PID/PD controller. This kind of controller can hardly catch up a rapid change of the set point.

Figure 4.

Simulation results of fixed-point target approaching. (a) Position trajectory. (b) Direct command sequence.

Figure 4(b) shows value of throttle, roll, and pitch. There is a difference at the beginning because the set point can be nonzero initially, while states have to begin from zero initial value and transfer through a two-stage dynamic process. This difference coincides with the tracking delay in xy position in Figure 4(a).

However, the simple roll–pitch–height controller is verified to be not qualified in a more complex scenario, due to the accumulation of lag error. x–y–z controller (waypoint mode) is implemented in all the following simulations and experiments.

Moving target tracking

In IARC shepherd problem, the moving ground targets are iRobot Create. The goal of quadrotor approaching in this problem is to intercepting and triggering the sensor of the ground vehicle. Figure 5 is a close look of ground vehicle trajectory used in the coming simulation.

Figure 5.

A detailed description of the ground vehicle’s behavior in simulation.

Without any external interference, the ground vehicle remains to move along a noisy straight line within a time period T_l, then keeps rotating (resulting U-turn) in T_u. We denote $T=T_l+T_u$ as a cycle. Denote T_s as the starting time within a cycle and T_f as the time of the whole process. Within a cycle of T = 20 s, we solve the approaching problem at different T_s instances as shown in Figure 6. Apparently, the ground vehicle makes a U-turn in this process, if $T_s+T_f>T$ . The parameter value used in the simulation is defined as Table 1. The quadrotor in this simulation has no prior knowledge about these rules. The algorithm works only based on the real-time observations of the ground target states.

Figure 6.

Comprehensive simulation results. Simulation video link is listed in the section “Conclusion.” (a) T_s = 2: 3D plot. (b) T_s = 2: colormap of acceleration. (c) T_s = 2: xyz-plot. (d) T_s = 14: 3D plot. (e) T_s = 14: colormap of acceleration. (f) T_s = 14: xyz-plot.

Table 1.

Parameter description.

Parameter	Value	Description
T	20 s	Period of an entire loop
Tl	17 s	Period of linear motion
Tu	3 s	Period of U-turn
v 0	0.33 m/s	Velocity of linear motion
ω	1.04 rad/s	Angular velocity of U-turn
ν	$\mathcal{N}(0,1)$	Artificial noise
η	Const.	Natural disturbance

Figure 6(a) to (c) shows the case $T_s+T_f<T$ (no turning till the interception). In the long and straight cruising segment, the quadrotor first accelerates then decelerates, resulting almost no overshoot. There is a deviation between red line (planned trajectory) and blue line (real quadrotor trajectory), due to the shortage of tracking ability of PID controller. According to the STRs, the quadrotor determines when to start approaching based on the behavior of the ground vehicle. If the ground vehicle is turning at that moment, then quadrotor holds in tracking and waits for the next straight-line motion.

Figure 6(d) to (f) shows the case $T_s+T_f>T$ (turning during approaching process). The quadrotor recovers from the aggressive maneuver in cruising, while dynamically predicts the interception points. The final approaching is implemented while these preparations are done. This simulation verifies the effectiveness of this method to satisfy different conditions with expected performance. The trajectory can track and intercept the target within an acceptable time cost.

Obstacle avoidance

In a flight operation, the priority of obstacle avoidance is always higher than navigation and guidance. The algorithm may override the controller for avoidance when the obstacle is detected unexpectedly. Our algorithm should be able to recover to interception after the obstacle is passing by. Especially in the high-speed cruising segment, the reaction time is very short since the range of laser scanner is limited. We do not discuss advanced obstacle avoidance path planning here. In the basic control level, an obstacle avoidance command always overrides all planning levels and implements the most direct command to get far away from the moving obstacle. Therefore, we implement basic maneuver-like “hard aport” or “change lane” when detecting obstacle and recovery to cruising after passing by.

Figure 7(a) to (f) shows two similar cases with the interference of obstacles. Quadrotor detects the obstacle and begins to evade by implementing a “hard-aport” maneuver. The PID controller tracks the trajectory with a significant delay. After passing by the obstacle, there is a dense recovery acceleration as shown in Figure 7(b) and (e).

Figure 7.

Comprehensive simulation results. Simulation video link is listed in the section “Conclusion.” (a) T_s = 2 with obstacle: 3D plot. (b) T_s = 2 with obstacle: colormap of acceleration. (c) T_s = 2 with obstacle: xyz-plot. (d) T_s = 14 with obstacle: 3D plot. (e) T_s = 14 with obstacle: colormap of acceleration. (f) T_s = 14 with obstacle: xyz-plot.

Figure 7 shows the quadrotor can intercept a ground vehicle with the interference of obstacle. However, this kind of obstacle avoidance maneuver is really coarse and cannot guarantee every success. Empirically, obstacle avoidance is mainly concerned in a higher level path planning with a proficient knowledge for optimal path.

Experiments

Experiments in this article are all conducted in the ZMART IARC Testbed as shown in Figure 8(a), which is a MAV task test environment majorly for the IARC shepherd problem. This system can also test other functions such as obstacle avoidance, exploring, mapping, and formation. OptiTrack vision capture system provides an accurate position and attitude data at a rate of 120 Hz with an accuracy of millimeter. The test quadrotor is equipped with minimum system hardware consists of inertial measurement unit, flight controller, Zigbee communication module, a frame size of 330 mm, and electric propulsion system.

Figure 8.

Comprehensive experiment results. Experiments video link is listed in the section “Conclusion.” (a) ZMART IARC testbed. (b) Fixed point approaching. (c) Moving target approaching.

In this experiment, the quadrotor and the target are marked by infrared reflectors. The motion capture camera is sensitive to infrared ray. The flight controller unit is DJI NAZA, a widely used commercial flight controller, working in the attitude control mode. An outer loop position controller is running on the ground station computer. The communication between quadrotor and ground station is through Zigbee.

Basic functional test

In the basic test as Figure 8(b) and (c), basic functional segments, fixed point, and moving target are tested independently.

Figure 8(b) shows the quadrotor approaches the target rapidly with a smooth trajectory, which is for final approach segment. Compared to the simple single-loop PID controller in simulation, the DJI NAZA flight controller performs better in trajectory tracking. Because DP does not provide strong constraint on final time T_f, this experiment also needs to measure the approaching time T_r, which is a basic parameter to predict the interception point in approaching function.

In Figure 8(c), an iRobot Create, coded exactly the same as (6,7) and Table 1, carries reflective balls and moves on the ground as a target. The result shows that the quadrotor can catch up and track the target steadily.

Competition scenario test

To test the performance of this method comprehensively, we use all elements in the testbed to imitate the real competition scenario. The black box available in Figure 9(f) is to trigger the sudden turning. Ground vehicle starts a U-turn when hitting this box. The white bar in Figure 9(i) is an obstacle. This test scenario is proficient but simple for basic probable condition with unpredicted target turning and suddenly appeared obstacle.

Figure 9.

Comprehensive experiment results. Experiments video link is listed in the section “Conclusion.” (a) Common condition: 3D plot. (b) Common condition: xyz-axis. (c) Common condition: experiment snapshot. (d) Double-turn condition: 3D plot. (e) Double-turn condition: xyz-axis. (f) Double-turn condition: experiment snapshot. (g) Obstacle avoidance: 3D plot. (h) Obstacle avoidance: xyz-axis. (i) Obstacle avoidance: experiment snapshot.

Figure 9(a) to (c) is a common entire process without any interference. It shows a similar pattern as the simulation in Figure 6(a) to (c), thus verifies the accuracy of the simulation system in the section “Simulation.” In the z-axis of Figure 9(b), (e), and (h), there is a constant gap between set point and system output because the NAZA flight controller has a dead zone in height. The dead zone leaves huge effect in the final approaching segment, for inaccuracy in height control may lead to wrong interception point. Therefore, we use a mixture of direct throttle command and height controller to solve this problem.

With the existence of a black box in Figure 9(f), it becomes more complex since the ground vehicle may turn twice during the process, one for head collision and another for 20 s count. Turning behaviors are well addressed by tracking segment, since the quadrotor can keep tracking until the ground vehicle finishes turning. Tracking segment is the most steady segment among three. No matter how complex the target behavior is, the quadrotor can track and prepare for the final approaching.

Figure 9(g) to (i) shows the experiment with an obstacle. This obstacle is located by reflective balls. The quadrotor successfully evades then recovers to track the target. For the equipment limit, the effective area of this testbed is less than 4 m × 4 m. We cannot implement some experiments in longer time or larger distance because the quadrotor and ground vehicle will easily get out of the bound. Those experiments above have verified the basic functional effectiveness.

Conclusion

In this article, we propose a simultaneous tracking and approaching method for quadrotor toward mobile ground vehicle. Trajectory generation method based on DP is used. STR is implemented to divide the whole process into three parts to guarantee smooth and robust control sequence till the final interception under different conditions. The quadrotor achieves aggressive approaching toward moving ground targets with smooth trajectory in both simulations and experiments. Simulation and experiment video is in link (http://v.youku.com/v_show/id_XMTU0NzM5ODE2MA==.html?from=y1.7-1.2).

The future work could be as the following two aspects: (1) terminal time constrain should be taken into concern as a strong constrain in trajectory generation and (2) the quadrotor should be able to locate itself and target with onboard payloads, vision for example, instead of external supports. Furthermore, this approach will be improved to a learning-based structure, such that it can be trained to touch ground vehicles, without human adjustment for algorithm parameters.