Real-Time Terrain-Following of an Autonomous Quadrotor by Multi-Sensor Fusion and Control

Abstract

For the application of the autonomous guidance of a quadrotor from confined undulant ground, terrain-following is the major issue for flying at a low altitude. This study has modified the open-source autopilot based on the integration of a multi-sensor receiver (a Global Navigation Satellite System (GNSS)), a Lidar-lite (a laser-range-finder device), a barometer and a low-cost inertial navigation system (INS)). These automatically control the position, attitude and height (a constant clearance above the ground) to allow terrain-following and avoid obstacles based on multi-sensors that maintain a constant height above flat ground or with obstacles. The INS/Lidar-lite integration is applied for the attitude and the height stabilization, respectively. The height control is made by the combination of an extended Kalman filter (EKF) estimator and a cascade proportional-integral-derivative (PID) controller that is designed appropriately for the noise characteristics of low accuracy sensors. The proposed terrain-following is tested by both simulations and real-world experiments. The results indicate that the quadrotor can continuously navigate and avoid obstacles at a real-time response of reliable height control with the adjustment time of the cascade PID controller improving over 50% than that of the PID controller.

1. Introduction

In recent years, advances in constructions, electronics, sensors and control algorithms have fueled a growth in the development of unmanned aerial vehicles (UAVs) [1]. The automatic applications of UAVs rely on the continuous control of the position, attitude, speed and other high level commands [2,3]. Specifically, the attitude and height (the clearance from the ground) controllers allow terrain-following of an autonomous quadrotor, which is useful for topography prediction, navigation and obstacle avoidance [4,5,6]. An integrated navigation framework is an autonomous system used for determining the position, attitude and velocity of UAVs using multi-sensors [7,8], i.e., a global positioning system (GPS), an inertial measurement unit ((IMU) e.g., an accelerometer, a gyroscope and a magnetometer) [9] and radar or distance sensors [10,11,12,13]. To achieve a more reliable travel in real environments, fruitful research efforts have focused on the height control problem [14].

The height control of common UAVs applies a proportional-integral-derivative (PID) control structure for the altitude that handles high impact and changed mass [15,16,17]. These controllers compare the behavior of the UAVs with a model and adapt the output of the controller to minimize the error between the model and the system. The design, implementation, simulation and evaluation of this controller will be presented in this paper divided into algorithms based on a classic control theory and a modern control theory. Control algorithms based on a modern control theory include a dynamic inverse control [18], a linear-quadratic regulator (LQR) control [19], an intelligent control (such as a neural network control [20,21]) and an adaptive control [22,23]. Ngo used the dynamic inversion based adaptive control method to control the height of the UAVs in the climbing stage and verified that the method had good effects through simulation [24]. However, the long-term flight of UAVs is a time-varying nonlinear system [25,26] whose external environment, speed, load and other parameters change continuously. For dynamic inverse control strategies, the control effect is often not satisfactory because there is no effective method to obtain the precise parameters of the model. Due to the requirement of the rapidity of response, the intelligent control algorithm, which needs complex calculations, is less used in practical engineering applications though it has a strong environmental adaptability [16]. Thus, in engineering practice, methods based on a classic control (such as the PID control) are mostly used because of the uncertainty of system parameters and the requirement of real-time control. The PID control is an effective and reliable flight control strategy that has been studied for some time and has a mature design strategy. For the UAVs’ terrain-following system, the problem to be solved is that the traditional single-stage PID control cannot meet the requirements in terms of response speed and stability [27]. In order to improve the quality of control, Idres used the cascade PID control method to control the UAVs and verified the efficiency of the cascade PID controlled UAVs in tracking slopes, circles and sinusoidal trajectories through a simulation [28]. Ren analyzed the system based on the classical control theory and proved that the cascade PID was better than the traditional single-stage PID control from an amplitude margin and a phase angle margin [29]. A cascade PID is one of the effective methods used to improve the quality of traditional PID control and it has been widely used in the field of process control [30,31,32]. For the cascade PID control of terrain-following UAVs, how to set the control feedback state quantity and how to adjust its parameters are important issues. The Ziegler–Nichols method [33], the critical proportionality method [34] and the attenuation curve method [35] are commonly used in engineering for tuning. Choosing a suitable method for PID tuning can also help improve the quality of control [36,37].

The main goal of this study is to improve the autonomous terrain-following and navigation performance of a quadrotor system. We address the detailed mathematical modeling of the quadrotor’s kinematics and dynamics of the quadrotor. The autonomous controller comprises of an integrated control for the quadrotor’s position, attitude and height control with the multi-sensor based control loops for terrain-following depicted in Figure 1. The control methodologies are conducted in MATLAB Simulink and the experiments on a quadrotor test-bed where interference such as wind conditions and sensor noise are incorporated in both the simulation and real-world experiments. A quadcopter measures its altitude relatively to the ground by using a range-finder. However, when the surface of the ground is fluctuating, a traditional PID control algorithm cannot meet the requirements of accurate terrain-following. Work in this innovation is the adaptive control function using the integration of a Global Navigation Satellite System (GNSS) (GPS) and a BeiDou Navigation Satellite System (BDS) [38], an inertial navigation system (INS) [39], a barometer [40] and a Lidar-lite sensor [41], especially the combination of an extended Kalman filter (EKF) and the cascade PID controller. In this paper, a dynamic model of the quadcopter is built and then linearized for simplification. The cascade PID is modeled and simulated in Simulink based on a tuning method according to the transfer function. Finally, an experiment is performed based on an open-source flight controller Pixhawk. Both the simulation and the real flight show improvements with this controller.

The rest of this paper is organized as follows. In II, dynamic models of the quadrotor are described. In III, we design the integration of the position and height control based on the combination of an extended Kalman filter and a cascade PID. IV verifies the reliability of the proposed models by MATLAB Simulink and real-world quadrotor flying avoiding a box above the ground. Conclusions are given in V.

2. Quadrotor System Models

2.1. Quadrotor Dynamic Models

The quadrotor changes its position and attitude by modifying the speed of the four motors [42,43]. The movements of back and forth and left and right are produced by pitch and roll, respectively, and the movements of up and down are achieved by the four motors increasing or decreasing the speed at the same time.

In order to study the kinematic law of aircraft, two coordinate systems [44] depicted in Figure 2 are defined as follows:

• The geographic coordinate system ($O_E{X}_E{Y}_E{Z}_E$) is used to study the motion state of aircraft relative to the earth and the space position coordinates in which $Y_E\text{-}{X}_E\text{-}{Z}_E$ point to East-North-Up, respectively.
• The airframe coordinate system ($O_b{X}_b{Y}_b{Z}_b$) is used to study the rotational motion of aircraft relative to the center of gravity in which $Y_b\text{-}{X}_b\text{-}{Z}_b$ point to Right-Front-Up, respectively.

According to the conversion relationship between the Euler angle and the attitude matrix, the attitude transfer matrix $R_E^b$ from the geographic coordinate system to the airframe coordinate system can be obtained:

$$R_E^b=\left[\begin{array}{ccc}\cos \theta \sin \psi & \sin \psi \cos \theta & -\sin \theta \\ \sin \varphi \sin \theta \cos \psi -\cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi +\cos \varphi \cos \psi & \sin \varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi & \cos \varphi \sin \psi \sin \theta -\sin \varphi \cos \psi & \cos \varphi \cos \theta \end{array}\right]$$

(1)

The vector of ${\left[\begin{array}{cccccc}x& y& z& \varphi & \theta & \psi \end{array}\right]}^T$ represents the position and angle of the quadrotor in the geographic coordinate system and the vector of ${\left[\begin{array}{cccccc}u& v& w& p& q& r\end{array}\right]}^T$ represents the linear velocity and angular velocity of the quadrotor in the airframe coordinate system. The conversion relationship of these twelve-dimensional variables in the two coordinate systems is:

$$\{\begin{array}{l}{v}_E=R_b^E{v}_b\\ \omega =T{\omega }_b\end{array}.$$

(2)

In Equation (2), $v_E={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{z}\end{array}\right]}^T\in R^3$, $\omega ={\left[\begin{array}{ccc}\dot{\varphi }& \dot{\theta }& \dot{\psi }\end{array}\right]}^T\in R^3$, $v_b={[\begin{array}{ccc}u& v& w\end{array} ]}^T\in R^3$, ${\omega }_b={[\begin{array}{ccc}p& q& r\end{array} ]}^T\in R^3$, the $R_b^E$ is the direction cosine matrix from the airframe coordinate system to the geographic coordinate system and $T$ is the conversion matrix between the airframe rotation angular velocity and the attitude angle change rate. The dynamic model of the quadrotor is:

$$\{\begin{array}{l}\dot{x}=w(\sin \varphi \sin \psi +\cos \varphi \cos \psi \sin \theta )-v(\cos \varphi \sin \psi -\cos \psi \sin \varphi )\\ \dot{y}=v(\cos \varphi \cos \psi +\sin \varphi \sin \psi \sin \theta )-w(\cos \psi \sin \varphi -\cos \varphi \sin \psi )\\ \dot{z}=w(\cos \varphi \cos \theta )-u(\sin \theta )+v(\cos \theta \sin \varphi )\\ \dot{\varphi }=p+r(\cos \varphi \tan \theta )+q(\sin \varphi \tan \theta )\\ \dot{\theta }=q(\cos \varphi )-r(\sin \varphi )\\ \dot{\psi }=r\frac{\cos \varphi }{\cos \theta }+q\frac{\sin \varphi }{\cos \theta }\end{array}.$$

(3)

According to Newton’s law of motion, the total force acting on the quadrotor is:

$$m\left({\omega }_b\otimes v_b+{\dot{v}}_b\right)=f_b,$$

(4)

where $m$ is the mass of the quadcopter, $\otimes$ represents the cross product operation and $f_b=\left[\begin{array}{ccc}{f}_x& f_y& f_z\end{array}\right]\in R^3$ is the total force acting on the quadrotor. According to Euler’s equation, the total torque produced by the four motors is:

$$I\cdot {\dot{\omega }}_b+{\omega }_b\otimes \left(I\cdot {\omega }_b\right)=m_b.$$

(5)

In (5), ${\tau }_b=\left[\begin{array}{ccc}{\tau }_x& {\tau }_y& {\tau }_z\end{array}\right]\in R^3$ represents the total torque, $I$ is the inertia matrix of the quadrotor, which can be simplified as follows according to the symmetry of the quadrotor:

$$I=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right],$$

(6)

where $I_{xx}, I_{yy}, I_{zz}$ are called the central principal moments of inertia and can be measured through experiments. Therefore, the dynamic model of the quadrotor in the airframe coordinate system is:

$$\{\begin{array}{c}{f}_x=m(\dot{u}+qw-rv)\\ f_y=m(\dot{v}-pw+ru)\\ f_z=m(\dot{w}+pv-qu)\\ {\tau }_x=\dot{p}{I}_{xx}-qr{I}_{yy}+qr{I}_{zz}\\ {\tau }_y=\dot{q}{I}_{yy}+pr{I}_{xx}-pr{I}_{zz}\\ {\tau }_z=\dot{r}{I}_{zz}-pq{I}_{xx}+pq{I}_{yy}\end{array}.$$

(7)

The above equation is established under the condition that the origin and axis of the airframe coordinate system coincide with the centroid and main axis of the quadrotor.

The total external force received by the quadrotor in the airframe coordinate system is:

$$f_b=mg{R}^T\cdot {\widehat{e}}_z-f_t{\widehat{e}}_3+f_w,$$

(8)

where ${\widehat{e}}_z$ is the unit vector of the z-axis of the geographic coordinate system, ${\widehat{e}}_3$ is the unit vector of the z-axis of the airframe coordinate system, $g$ is the acceleration of gravity, $f_t$ is the total pulling force generated by motors and $f_w$ is the force generated by the wind acting on the quadrotor.

Ignoring the rotational torque in the control equation, the total external torque ${\mathrm{{\rm T}}}_b$ in the airframe coordinate system can be expressed as:

$${\mathrm{{\rm T}}}_b={\tau }_b+{\tau }_w,$$

(9)

where ${\tau }_b={\left[\begin{array}{ccc}{\tau }_x& {\tau }_y& {\tau }_z\end{array}\right]}^T\in R^3$,${\tau }_w={\left[\begin{array}{ccc}{\tau }_{wx}& {\tau }_{wy}& {\tau }_{wz}\end{array}\right]}^T\in R^3$ are the control torques generated by the difference in motor speed and the movement generated by the wind acting on the quadrotor.

Substituting Equations (8) and (9) into Equation (7), the complete dynamic model of the quadrotor in the airframe coordinate system is obtained as:

$$\{\begin{array}{l}-mg(\sin \theta )+f_{wx}=m(\dot{u}+qw-rv)\\ mg(\cos \theta \sin \varphi )+f_{wy}=m(\dot{v}-pw+ru)\\ mg(\cos \theta \cos \varphi )+f_{wz}-f_t=m(\dot{w}+pv-qu)\\ {\tau }_x+{\tau }_{wx}=\dot{p}{I}_{xx}-qr{I}_{yy}+qr{I}_{zz}\\ {\tau }_y+{\tau }_{wy}=\dot{q}{I}_{yy}+pr{I}_{xx}-pr{I}_{zz}\\ {\tau }_z+{\tau }_{wz}=\dot{r}{I}_{zz}-pq{I}_{xx}+pq{I}_{yy}\end{array}$$

(10)

Considering that the force and torque input by the control system are proportional to the square of the motor speed, the parameters in (10) are as follows:

$$\{\begin{array}{l}{f}_t=c_T\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\\ {\tau }_x=\frac{\sqrt{2}}{2}{c}_{T}l\left(-{\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)\\ {\tau }_y=\frac{\sqrt{2}}{2}{c}_{T}l\left(-{\mathsf{\Omega }}_1^2-{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\\ {\tau }_z=c_M\left(-{\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\end{array},$$

(11)

where $l$ is the distance between the center of the motor and the center of the airframe, $c_T$ is the tensile coefficient of the motor, and $c_M$ is the torque coefficient. All of the above parameters can be measured through experiments. Substituting (11) into (10), the complete dynamic model of the quadrotor in the airframe coordinate system can be further expressed as:

$$\{\begin{array}{l}m(\dot{u}+qw-rv)=-mg(\sin \theta )+f_{wx}\\ m(\dot{v}-pw+ru)=mg(\cos \theta \sin \varphi )+f_{wy}\\ m(\dot{w}+pv-qu)=mg(\cos \theta \cos \varphi )+f_{wz}\\ -c_T\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\\ \dot{p}{I}_{xx}-qr{I}_{yy}+qr{I}_{zz}=\frac{\sqrt{2}}{2}{c}_{T}l\left(-{\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)\\ +{\tau }_{wx}\\ \dot{q}{I}_{yy}+pr{I}_{xx}-pr{I}_{zz}=\frac{\sqrt{2}}{2}{c}_{T}l\left(-{\mathsf{\Omega }}_1^2-{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\\ +{\tau }_{wy}\\ \dot{r}{I}_{zz}-pq{I}_{xx}+pq{I}_{yy}=c_M\left(-{\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\\ +{\tau }_{wz}\end{array}.$$

(12)

The parameter setting of the quadrotor models is listed in

Table 1. Parameter list of the quadrotor.

Parameter	Unit	Measurements
g	m/s2	9.81
m	kg	1.12
l	m	0.225
cT	N s2/rad2	1.394 × 10−7
cM	N m s2/rad2	2.857 × 10−9
Ixx	kg m2	0.0093
Iyy	kg m2	0.0088
Izz	kg m2	0.0189

.

The state vector is expressed as follows:

$$x={[\begin{array}{cccccccccccc}\varphi & \theta & \psi & p& q& r& u& v& w& x& y& z\end{array}]}^T.$$

(13)

According to Equations (3) and (10), the dynamic equation of the quadrotor can be rewritten as:

$$\{\begin{array}{l}\dot{\varphi }=p+r(\cos \varphi \tan \theta )+q(\sin \varphi \tan \theta )\\ \dot{\theta }=q(\cos \varphi )-r(\sin \varphi )\\ \dot{\psi }=r\frac{\cos \varphi }{\cos \theta }+q\frac{\sin \varphi }{\cos \theta }\\ \dot{p}=\frac{I_{yy}-I_{zz}}{I_{xx}}rq+\frac{{\tau }_x+{\tau }_{wx}}{I_{xx}}\\ \dot{q}=\frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{{\tau }_y+{\tau }_{wy}}{I_{yy}}\\ \dot{r}=\frac{I_{xx}-I_{yy}}{I_{zz}}pq+\frac{{\tau }_z+{\tau }_{wz}}{I_{zz}}\\ \dot{u}=rv-qw-g(\sin \theta )+\frac{f_{wx}}{m}\\ \dot{v}=pw-ru+g(\sin \varphi \cos \theta )+\frac{f_{wy}}{m}\\ \dot{w}=qu-pv+g(\cos \theta \cos \varphi )+\frac{f_{wz}-f_t}{m}\\ \dot{x}=w(\sin \varphi \sin \psi +\cos \varphi \cos \psi \sin \theta )\\ \hspace{1em}\hspace{1em}-v(\cos \varphi \sin \psi -\cos \psi \sin \varphi \sin \theta )+u(\cos \psi \cos \theta )\\ \dot{y}=v(\cos \varphi \cos \psi +\sin \varphi \sin \psi \sin \theta )\\ \hspace{1em}\hspace{1em}-w(\cos \psi \sin \varphi -\cos \varphi \sin \psi in\theta )+u(\cos \theta sin\psi )\\ \dot{z}=w(\cos \varphi \cos \theta )-u(\sin \theta )+v(\cos \theta \sin \varphi )\end{array}$$

(14)

In hovering mode, the attitude of the aircraft changes very little. It can be made that $\cos \alpha =1, \sin \alpha =\tan \alpha =0$ and the state Equation (14) can be approximately expressed as:

$$\{\begin{array}{l}\dot{\varphi }\approx p+r\theta +q\varphi \theta \hfill \\ \dot{\theta }\approx q-r\varphi \hfill \\ \dot{\psi }\approx r+q\varphi \hfill \\ \dot{p}\approx \frac{I_{yy}-I_{zz}}{I_{xx}}rq+\frac{{\tau }_x+{\tau }_{wx}}{I_{xx}}\hfill \\ \begin{array}{l}\dot{q}\approx \frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{{\tau }_y+{\tau }_{wy}}{I_{yy}}\\ \begin{array}{l}\dot{r}\approx \frac{I_{xx}-I_{yy}}{I_{zz}}pq+\frac{{\tau }_z+{\tau }_{wz}}{I_{zz}}\hfill \\ \dot{u}\approx rv-qw-g\theta +\frac{f_{wx}}{m}\hfill \\ \dot{v}\approx pw-ru+g\varphi +\frac{f_{wy}}{m}\hfill \\ \dot{w}\approx qu-pv+g+\frac{f_{wz}-f_t}{m}\hfill \\ \dot{x}\approx w(\varphi \psi +\theta )-v(\psi -\varphi \theta )+u\hfill \\ \dot{y}\approx v(1+\varphi \psi \theta )-w(\varphi -\psi \theta )+u\psi \hfill \\ \dot{z}\approx w-u\theta +v\varphi \hfill \end{array}\end{array}\hfill \end{array}.$$

(15)

The above equation can be written in the form of a differential equation:

$$\dot{x}=f(x,u).$$

(16)

In Equation (16), $u={\left[\begin{array}{llll}{f}_t\hfill & {\tau }_x\hfill & {\tau }_y\hfill & {\tau }_z\hfill \end{array}\right]}^T\in R^4$, $x={[\begin{array}{cccccccccccc}\varphi & \theta & \psi & p& q& r& u& v& w& x& y& z\end{array}]}^T\in R^{12}$.

The balance point of the system can be set as follows:

$$\overline{x}={\left[\begin{array}{llllllllllll}0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & \overline{x}\hfill & \overline{y}\hfill & \overline{z}\hfill \end{array}\right]}^T\in R^{12}.$$

(17)

When the system is in equilibrium, the input of the system is:

$$\overline{u}={\left[\begin{array}{llll}mg\hfill & 0\hfill & 0\hfill & 0\hfill \end{array}\right]}^T\in R^4.$$

(18)

The state equation is further written as:

$$\{\begin{array}{c}\dot{\varphi }=p\\ \dot{\theta }=q\\ \dot{\psi }=r\\ \dot{p}=\frac{{\tau }_x+{\tau }_{wx}}{I_{xx}}\\ \dot{q}=\frac{{\tau }_y+{\tau }_{wy}}{l_{yy}}\\ \dot{r}=\frac{{\tau }_z+{\tau }_{wz}}{I_{zz}}\\ \dot{u}=-g\theta +\frac{f_{wx}}{m}\\ \dot{v}=g\varphi +\frac{f_{wy}}{m}\\ \dot{w}=\frac{f_{wz}-f_t}{m}\\ \dot{x}=u\\ \dot{y}=v\\ \dot{z}=w\end{array}.$$

(19)

Ignoring the influence of wind, it can be written as:

$$\{\begin{array}{c}\dot{\varphi }=p\\ \dot{\theta }=q\\ \psi =r\\ \dot{p}=\frac{{\tau }_x}{I_{xx}}\\ \dot{q}=\frac{{\tau }_y}{l_{yy}}\\ \dot{r}=\frac{{\tau }_z}{I_{zz}}\\ \dot{u}=-g\theta \\ \dot{v}=g\varphi \\ \dot{w}=\frac{-f_t}{m}\\ \dot{x}=u\\ \dot{y}=v\\ \dot{z}=w\end{array}.$$

(20)

That is, the linearized model of the hovering state equation can be written as:

$$\dot{x}=A\cdot x+B\cdot u,$$

(21)

where A and B can be expressed as follows:

$$\begin{array}{cc}A=& {\frac{\partial f(x,u)}{\partial x}|}_{\begin{array}{c}x=\overline{x}\\ u=\overline{u}\end{array}}\hfill \\ =& \left[\begin{array}{cccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 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& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& -g& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ g& 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& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\end{array}\right],\hfill \end{array}$$

(22)

$$B={\frac{\partial f(x,u)}{\partial x}|}_{\begin{array}{c}x=\overline{x}\\ u=\overline{u}\end{array}}=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& \frac{1}{I_{xx}}& 0& 0\\ 0& 0& \frac{1}{I_{yy}}& 0\\ 0& 0& 0& \frac{1}{I_{zz}}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ \frac{1}{m}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right].$$

(23)

2.2. Quadrotor State Estimations

When the laser-range-finder is used for distance measurement in terrain-following flight, the large vertical drop of the terrain will cause the delay of altitude control and the decrease of the stability of the quadrotor. Therefore, it is necessary to ameliorate the altitude control channel of the quadrotor to improve the stability and real-time performance of the quadrotor in a large maneuvering and large overload terrain-following flight.

Combined with the linearized dynamics model of the quadrotor in the hovering state in Section 2.2, the state equation of the height channel control model can be written as:

$$\{\begin{array}{l}\dot{x}=A\cdot x+B\cdot u\hfill \\ w=C\cdot x\hfill \end{array},$$

(24)

where $A, B, x$ and $u$ are the same as those defined in section A, $C=\left[\begin{array}{cccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\end{array}\right]$. Using a Laplace transform on both sides of Equation (24), the transfer function of the vertical speed control can be obtained as:

$$G_I(s)=\frac{1}{sm}.$$

(25)

According to the voltage balance equation of the motor armature circuit, the motor model can be approximated as a first-order inertia link and the transfer function of the motor torque is:

$$G_{\mathsf{\Pi }}(s)=\frac{K}{T_{m}s+1},$$

(26)

where T_m is the inertia time constant and K is the open loop gain. Multiplying Equation (25) by Equation (26), the final transfer function of vertical velocity is:

$${\mathrm{G}}_{\mathrm{v}}(\mathrm{s})=\frac{K/m}{s\left(T_{m}s+1\right)}.$$

(27)

Adding the integral link to Equation (27) to obtain the transfer function of the UAVs’ height control channel gives:

$${\mathrm{G}}_{\mathrm{h}}(\mathrm{s})=\frac{K/m}{s^2\left(T_{m}s+1\right)}.$$

(28)

Using the parameter measurement method proposed in the literature [45], the inertial time constant $T_m=0.153$ and the open-loop gain $K=1542$ of the 2212KV910 brushless motor were measured by experiment. Substituting $T_m$ and $K$ $K$ into Equations (27) and (28), Equation (29) can be obtained:

$$\{\begin{array}{l}{\mathrm{G}}_{\mathrm{v}}(\mathrm{s})=\frac{8398.7}{\mathrm{s}(\mathrm{s}+6.5)}\hfill \\ {\mathrm{G}}_{\mathrm{h}}(\mathrm{s})=\frac{8398.7}{{\mathrm{s}}^2(\mathrm{s}+6.5)}\hfill \end{array}.$$

(29)

3. Quadrotor Integrated Control

3.1. Height and Vertical Velocity Estimations

As the error of the accelerometer will accumulate with the integration, it is not reliable to obtain the vertical velocity by using the accelerometer integration alone. Therefore, the optimal vertical velocity is estimated by integrating the differential data of the range-finder and the integral data of the accelerometer. The sampling period is set as T = 1 and the block diagram of the discretized fusion speed measurement system is shown in Figure 3.

In Figure 3, $a_k$ is the speed increment from time $t_{k-1}$ to time $t_k$, $\widehat{v}(k-1)$ is the optimal estimation of vertical velocity. $Z_k$ is the observations of multi-sensor increment from time $t_{k-1}$ to time $t_k$, which are used as the estimated value of the observation correction of the speed measurement system.

Assuming that both the system and the measurement noise are Gaussian white noise with an expectation of zero, the discretized extended Kalman filter equation of the velocity measurement system [46,47] is:

$$\{\begin{array}{c}{X}_k={\widehat{X}}_{k-1}+a_k\\ P_k=\mathsf{\Phi }{\widehat{P}}_{k-1}{\mathsf{\Phi }}^{\mathrm{T}}+Q_{k-1}\\ K_k=P_k{H}_k^{\mathrm{T}}{\left(H_k{P}_k{H}_k^{\mathrm{T}}+R_k\right)}^{-1}\\ {\widehat{X}}_k=X_k+K_k\left(Z_k-H_k{X}_k\right)\\ {\widehat{P}}_k=\left(I-K_k{H}_k\right)P_k\end{array},$$

(30)

where $X_k$ is the estimated value before correction at $t_k$, ${\widehat{X}}_{k-1}$ is the best estimate at $t_{k-1}$, $P_k$ is the mean square error before correction, $K_k$ is the Kalman gain, $H_k$ is the observation matrix, $Q_{k-1}$ and $R_k$ are the noise variance of the system and the measurement process, respectively [48].

3.2. Height and Vertical Velocity Based on a PID Controller

The multi-sensor control method is a classic control method mainly designed by the PID control principle. The differential equation of the PID controller is as follows:

• Continuous form

  $$u(t)=k_{p}e(t)+k_i{\displaystyle {\int }_0^{t}e}(\tau )d\tau +k_d\frac{de(t)}{dt},$$

  (31)

  where $u(t)$ is the output of the control system, $k_p$, $k_i$ and $k_d$ is the proportional, integral and the differential gain. And $e$ is the error between the set point and the feedback value, $t$ represents the current time, and $\tau$ is an integral variable of which the value is from 0 to the current time.
• Discrete form

  $$u(t)=k_{p}e(t)+k_i{\displaystyle \sum _{i=0}^{k}e}(i)+k_d[e(k)-e(k-1)].$$

  (32)

The PID control system of the quadrotor is based on a hierarchical control of inner and outer loops [49,50] and it can be further subdivided into four levels including position control, attitude control, control distribution and power distribution as shown in Figure 4.

As the height control of the quadrotor has nothing to do with the attitude of the body, there is an independent feedback input z in the height control system and the height correction is output after the height PID controller. Due to the balance between gravity and lift when the quadrotor is hovering, it is necessary to add gravity compensation to the height controller. When there is no error in the height, the controller can still output a stable value to control the motor rotation to maintain the hovering state. The simulation structure of the height controller is shown in Figure 5.

3.3. Height and Vertical Velocity Based on a Cascade PID Controller

The cascade control system is one of the effective methods to improve the control quality and it has been widely used in process controls. The cascade PID controller is composed of a single loop PID regulator (as the main regulator) and an external set regulator (as a secondary regulator) connected in series to form a dual loop regulation system. The control output of the main regulator is used as the external set regulator and the control output of the external set regulator is sent to the control regulation structure.

In order to improve the quality of height control effectively, a cascade PID based height controller system is designed as follows. The error between the expected height and the height measured by the range-finder is set as the outer loop feedback of the cascade PID controller, the expected vertical speed is obtained after the outer loop PID controller and then the difference between the expected vertical velocity and the actual vertical speed is set as the inner loop feedback input to accomplish the adjustment of the vertical speed, which finally realizes the dual loop control of the height and vertical speed. The control structure and Simulink are shown in Figure 6 and Figure 7.

4. Simulation and Experiment Evaluations

4.1. Simulink Parameter Tuning by Attenuation Curve

In order to obtain the optimal control effect, the parameters of the PID regulator need to be tuned. There are three common methods for PID parameter tuning in engineering: the Ziegler–Nichols tuning method, the critical proportionality method and the attenuation curve method. These three methods have their own characteristics: the Ziegler–Nichols tuning method is suitable for systems with a delay link in their open loop transfer function while the critical proportionality method requires the order of the system to be third-order or above and the attenuation curve method adjusts controller parameters by attenuation frequency characteristics [28]. If we suppose that the initial state of the quadrotor is $x={[\begin{array}{cccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}0]}^T$ and ${\mathsf{\Omega }}_i={\left[\begin{array}{cccc}4100& 4100& 4100& 4100\end{array}\right]}^T$, this means that the drone hovers at a height of 10 m and the simulation duration is set to 15 s. After 2 s, a position control command is given to make the aircraft rise to a height of 20 m and remain hovering. The simulation results are shown in Figure 8.

The rising speed shown in the left picture (a) first increases and then decreases after 2 s and then oscillates and stabilizes at about 0 m/s. The right picture shows that the height overshoots after reaching 20 m and finally stabilizes at 20 m.

According to the previously derived open loop transfer function Equation (31) of the height control system, the attenuation curve method is selected to tune the PID controller parameters and build the single-stage/cascade PID controller simulation block in Figure 8.

After setting up the control system structure in Simulink, first set the regulator parameters in the control system to a pure proportional action (Ti = ∞, τ = 0) put the system into operation and then gradually adjust the proportionality δ from large to small until the ratio of the amplitude of the two adjacent periodic curves is 4:1 attenuation.

At this time, the ratio is 4:1, the attenuation ratio is δs, the rise time is tr and the time interval between two adjacent peaks is Ts. According to δs, tr or Ts, use the empirical formula in

Table 2. PID regulator parameter tuning based on the attenuation curve method.

Controller	Proportional Band $\mathit{\delta }$	Integration Time $\mathit{T}\mathit{i}$	Derivative Time $\mathit{\tau }$
P	${\delta }_s$	$\infty$	0
PI	1.2 ${\delta }_s$	$2t_r/0.5T_s$	0
PID	0.8 ${\delta }_s$	$1.2t_r/0.3T_s$	$0.4t_r/0.1T_s$

to calculate each parameter value of the regulator [28].

The sequence of the PID controller parameter tuning is a proportional link first, then an integral link and then a derivative link. The parameters after tuning are:

$$K_p=0.009267,T_i=12.5,\tau =2.7398.$$

(33)

In addition to the former requirements, the cascade PID controller must follow the inner first and then the outer setting rule. The parameters after tuning are:

Inner loop:

$$K_{p1}=0.037975,T_{i1}=0.117,{\tau }_1=0.039.$$

(34)

Outer loop:

$$K_{p2}=3.8235,T_{i2}=5.1282,{\tau }_2=0.04875.$$

(35)

Set the expected value of the height to 3 m and then input the tuned parameters into the simulation system. After the system has run, the response curves of the single-stage PID controller and the cascade PID controller are shown in Figure 9.

It can be analyzed from Figure 9 that after the parameters are set by the attenuation curve method, the time it takes for the cascade PID controller to reach the expected height is reduced by 70% compared with the PID controller. The comparison of specific indicators is shown in

Table 3. Dynamic performance indicators of single-stage/cascade PID.

Dynamic Performance Index	PID	Cascade PID
Overshoot $M_p$	12.6%	4.6%
Settling time $t_s(s)$	4.427	1.352
Rise time $t_r(s)$	0.807	0.540
Peak time $t_p(s)$	1.338	0.190

below.

4.2. Carton Box Avoidance Experiment

To simulate the drastic changes of relative height to the ground, we carried out a carton box avoidance experiment to validate the proposed control algorithm. The assembled quadrotor as the experimental platform used a core controller stm32f427 cortex M4 and output inertial data with an mpu9250 nine axis IMU. Lidar-lite V3, a high-precision laser-range-finder, was used in the ranging sensor. Its data update frequency can reach 400 Hz, which can meet the requirements of high dynamic ranging. The cascade PID control algorithm was transplanted to flight control. The laser-range-finder height was taken as the output of the outer loop and the change rate of the measured altitude with time was taken as the input of the inner loop. The control performance test was carried out in an open outdoor field in Southeast University, China. The wheelbase of our quadrotor was 400 mm and the parameters of its brushless motor are shown in

Table 4. Parameters of the brushless motor.

Parameter	Value
$\mathrm{m}/\mathrm{kg}$	1.2
$\mathrm{K}$	1542
$T_m$	0.153

.

In order to simulate the height mutation of ground obstacles, a 0.48 m high carton was placed on flat ground as shown in Figure 10. Considering the great influence of high winds before take-off, the expected height of the UAV was set as 1.9 m, the horizontal flight speed was set as 0.5 m/s and the UAV was manipulated to fly over the carton repeatedly for 10 times. The controlled variables and experimental scenarios in the experiment are shown in

Table 5. Variable setting of the experiment.

Variables	Values/m
Height of the obstacle (the box)	0.48
Width of the obstacle (the box)	1.2
Travel speed of the quadrotor (m/s)	0.5

and Figure 10, respectively. When the quadrotor was holding its altitude, the proposed controller should have detected the edge of the box and manipulated it to fly over the box.

The resulting height data of the multi-sensor based controller can be seen in Figure 11 with the target height of 10 m above flat terrain. The difference in the performance of the height control clearly illustrates a better stability and sensitivity of the multi-sensor control. Therefore, the multi-sensor based control can react quickly especially with a reliable position and the velocity control in the z-axis.

For the height control of the quadrotor, we implemented with a PID and a cascade PID controller, respectively. The laser-range-finder measurements for the two controllers and the target height (1.9 m) are shown in Figure 12.

From the comparison of the fixed height flight results in Figure 12a,b, it can be seen that the adjustment time of the cascade PID controller was much less than that of the PID controller in the case of sudden changes of the ground altitude. In addition, the stability of the cascade PID control was better than that of the PID. After considering the statistics of all of the height measurement data, the shortest time from detecting a height change to reaching the desired height was 1.13 s and the longest time was 1.66 s when using the PID controller; when using the cascade PID controller, the shortest time was 0.43 s and the longest time was 0.75 s. The adjustment time of the cascade PID controller improved over 50% than that of the PID controller.

5. Conclusions

This study investigated the position, attitude, velocity and height estimators based on GPS, accelerometers, gyroscopes and Lidar-lite onboard sensors, which were operated on a quadrotor of the Pixhawk open-source autopilot. For the terrain-following of the quadrotor, we have presented real-time height control, navigation and obstacle avoidance above outdoor undulant ground. In our system, GNSS and INS information were used to verify the position, velocity and attitude estimates with a constant height control for autonomous flights. The navigation and estimations depended on an available multi-sensor fusion. Apart from the absolute altitude estimation, the terrain-following required relative distance measurements from the Lidar-lite sensor. The proposed control algorithm was tested extensively in MATLAB Simulink simulations and then implemented on the real-world quadrotor vehicle. The results confirmed that the proposed combination of the multi-sensor fusion and cascade PID controller could ensure autonomous navigation, obstacle avoidance and height control, achieving better accuracy, response time and sensitivity. Future developments will fuse trajectory and velocity smoothing that enable the balance between control efficiency and terrain-following criteria.