Aerial Robot Control in Close Proximity to Ceiling: A Force Estimation-based Nonlinear MPC

TL;DR

This paper presents a novel control framework for UAVs to perform safe, precise, and power-efficient close proximity flights near ceilings, using force estimation and nonlinear model predictive control.

Contribution

A new control approach combining force estimation and nonlinear MPC enables UAVs to fly safely within 1 cm of ceilings, addressing complex nonlinear dynamics and safety constraints.

Findings

UAV stabilizes at 1 cm from ceiling

Power consumption reduces by up to 12.5% near ceiling

Effective in complex nonlinear proximity environments

Abstract

Being motivated by ceiling inspection applications via unmanned aerial vehicles (UAVs) which require close proximity flight to surfaces, a systematic control approach enabling safe and accurate close proximity flight is proposed in this work. There are two main challenges for close proximity flights: (i) the trust characteristics varies drastically for the different distance from the ceiling which results in a complex nonlinear dynamics; (ii) the system needs to consider physical and environmental constraints to safely fly in close proximity. To address these challenges, a novel framework consisting of a constrained optimization-based force estimation and an optimization-based nonlinear controller is proposed. Experimental results illustrate that the performance of the proposed control approach can stabilize UAV down to 1 cm distance to the ceiling. Furthermore, we report that the UAV consumes up to 12.5% less power when it is operated 1 cm distance to ceiling, which is promising potential for more battery-efficient inspection flights.

Tables

Table 1. TABLE I: Specification of NMHE.

Parameter	Value
Time step $\Delta t$	$0.01$ (s)
Estimation window $N$	$40$
Measurement noise weight $V$	${10}^{-4}\cdot \mathrm{bdiag}\left(\{25\cdot {𝑰}_6,\mathrm{\hspace{0.33em}0.01},\mathrm{\hspace{0.33em}25}\cdot {𝑰}_3\}\right)$
Process noise weight $W$	$\mathrm{bdiag}\left(\{1/30\cdot {𝑰}_6,\mathrm{\hspace{0.33em}1}/8\cdot {𝑰}_3\}\right)$
Arrival cost weight $P_L$	$\mathrm{bdiag}\left(\{0.01\cdot {𝑰}_6,\mathrm{\hspace{0.33em}0.001}\cdot {𝑰}_3\}\right)$
Constraints	$-6⩽F_z^{}{}_{ext}{}^{}⩽2$ (N)

Table 2. TABLE II: Specification of NMPC.

Parameter	Value
Time step $\Delta t$	$0.01$ (s)
Prediction and control horizon $N$	$40$
Stage cost weight $Q$	$diag(30,30,10,1,1,2.5)$
Input weight $R$	$diag(30,30,80,4\times {10}^{-2})$
Terminal cost weight $S$	$diag(60,60,20,2,2,5)$
Constraints	$0.5⩽F_z⩽1.5$ (mg)

Equations

$${}^W\dot{\mathbf{x}}=R_{WB}{}^B\dot{\mathbf{x}},$$

$$\mathbf{M}{}^B\ddot{\mathbf{x}}-\mathbf{\omega }\times {}^B\dot{\mathbf{x}}+\mathbf{G}=\mathbf{F}+{\mathbf{F}}_{\mathrm{ext}},$$

$$\dot{\mathrm{x}}(t)$$

$$f_n$$

$$f_a$$

$$\mathrm{x}=[x,y,z,u,v,w,{F_{\mathrm{ext}}}_x,{F_{\mathrm{ext}}}_y,{F_{\mathrm{ext}}}_z{]}^{\mathrm{T}},$$

$$\mathrm{u}=[F_z,\varphi ,\theta ,\psi {]}^{\mathrm{T}},$$

$${\mathrm{x}}_{k+1}$$

$${\mathrm{y}}_k$$

$$h(\cdot )={\left[x,y,z,u,v,w,F_z,\varphi ,\theta ,\psi \right]}^{\mathrm{T}}.$$

$$\underset{{\mathrm{x}}_k,\text{mathpzc}{w}_k}{\min }$$

$${\mathrm{x}}_{k+1}=f({\mathrm{x}}_k,{\mathrm{u}}_k)+\text{mathpzc}{w}_k,\forall k\in [0,\dots ,j-1]$$

$$\displaystyle\mathrm{y}_{k}=h\big{(}\mathrm{x}_{k},\mathrm{u}_{k}\big{)}+\nu_{k},\quad\quad\forall k\in[0,\ldots,j]$$

$${\mathrm{x}}_{\min }⩽{\mathrm{x}}_k⩽{\mathrm{x}}_{\max }$$

$$\underset{{\mathrm{x}}_k,\text{mathpzc}{w}_k}{\min }$$

$${\mathrm{x}}_{k+1}=f({\mathrm{x}}_k,{\mathrm{u}}_k)+\text{mathpzc}{w}_k,\forall k\in [0,\dots ,j-1]$$

$$\displaystyle\mathrm{y}_{k}=h\big{(}\mathrm{x}_{k},\mathrm{u}_{k}\big{)}+\nu_{k},\quad\quad\forall k\in[0,\ldots,j]$$

$${\mathrm{x}}_{\min }⩽{\mathrm{x}}_k⩽{\mathrm{x}}_{\max }$$

$$\underset{{\mathrm{x}}_k,\text{mathpzc}{w}_k}{\mathrm{argmin}}$$

$${\mathrm{x}}_{k+1}=f({\mathrm{x}}_k,{\mathrm{u}}_k)+\text{mathpzc}{w}_k,\forall k\in [0,\dots ,j-1]$$

$$\displaystyle\mathrm{y}_{k}=h\big{(}\mathrm{x}_{k},\mathrm{u}_{k}\big{)}+\nu_{k},\quad\forall k\in[0,\ldots,j]$$

$$\underset{{\mathrm{x}}_k,\text{mathpzc}{u}_k}{\min }$$

$${\hat{\mathrm{x}}}_k={\mathrm{x}}_k$$

$${\mathrm{x}}_{k+1}=f({\mathrm{x}}_k,{\mathrm{u}}_k)$$

$${\mathrm{x}}_k=({\mathrm{x}}_0,{\mathrm{x}}_1,\dots ,{\mathrm{x}}_{k-1},{\mathrm{x}}_k,\dots )$$

$${\mathrm{u}}_k=({\mathrm{u}}_0,{\mathrm{u}}_1,\dots ,{\mathrm{u}}_{k-2},{\mathrm{u}}_{k-1},\dots )$$

$${\mathrm{x}}_{\min }⩽{\mathrm{x}}_k⩽{\mathrm{x}}_{\max }$$

$${\mathrm{u}}_{\min }⩽{\mathrm{u}}_k⩽{\mathrm{u}}_{\max }$$

$$\underset{{\mathrm{x}}_k,\text{mathpzc}{u}_k}{\min }$$

$$\text{eqref}sube{q}_{e}stconst,\text{eqref}sube{q}_{m}odelmpc,\text{eqref}sube{q}_{s}tates,\text{eqref}sube{q}_{c}ontrols,\text{eqref}sube{q}_{c}onstmpc,\text{eqref}sube{q}_{c}onstmpc2$$

$$P_{ave}=\frac{1}{T}{\int }_{t=0}^{t=T}v(t)i(t)dt,$$

Full text

I Introduction

In recent years, in-site UAV inspection has gained momentum as an application area in robotic research [1]. Typical inspection application requires a robot to achieve accurate motions in close proximity to the environment for long periods of measurement [2]. This is a challenging control task for conventional controllers such as PID controller, because of the strong cross-coupling between the UAV and surroundings. Although nonlinear controllers for such operating conditions are extensively studied for ground-effects, to our knowledge, there is not any controller systematically using force estimation in predictive control framework to achieve accurate control in close proximity to the ground as well as the ceiling.

In this study, the system is modeled as (i) a baseline model, which is composed of second-order translational dynamics of UAV system; and (ii) an additive model, which summarizes the interactions of the UAV with its surrounding. The additive model is constructed using lumped external forces. In our proposed approach, these external forces are estimated using nonlinear moving horizon estimation (NMHE) and fed into the nonlinear model predictive controller (NMPC) to fully capture the effect of interaction on the system. Later, the proposed controller is tested in close proximity to the ceiling. In summary, the following novelties are proposed in this work:

• •

For the first time, an optimization-based framework consisting of force estimation-based nonlinear model predictive controller is investigated for the ceiling effect.

• •

The power consumption of the aerial robot is analyzed for the proposed system in close proximity to the ceiling.

Leveraging the above-listed key findings of this study, it is possible to prolong the flight duration as well as the battery life. Interested readers may refer to the work on the reduced current effect on the battery life for UAVs in [3].

II Related Work

The ground effect observation is reported in landing problems, e.g. in [4], and addressed in some implementations. As it can be visualized, the ground effect stands as a repulsive force which still provides a safety margin in a normal flight. For the modeling of the ground effect, the interested reader can refer to [5, 6]. On the other hand, the ceiling effect creates an attractive force which pulls the aerial robot through the surroundings. Without a proper control strategy, the flight of the aerial robot might end up with a crash. Moreover, the near wall effect is numerically investigated in [7], and experimentally tested in [8]; therefore, some control techniques are suggested. It is noted that this effect is milder as compared to the ground and ceiling effects.

The numerical and experimental evaluation of the ceiling effect received less attention than that of ground and near-wall effects so far. However, recent attempts intent to explore its modeling such as [9, 10]. In terms of the control, from a passive point of view, it aims to avoid when a downwash effect (aerodynamic effect on the system surrounded by air flow) on the system is detected [11, 12]. In this context, the path planner generates an alternative trajectory to minimize the downwash effect. A counterintuitive approach to those of avoiding ones is analyzed in [13] and the experimental results are presented in [14], where the rotors are covered by a protective case to be in contact with the ceiling. Similarly, a system is designed to be hooked on some anchor points on the ceiling in [15]. However, its active compensation is required when the system needs to operate in close proximity, e.g., picking an object and inspection of the ceiling. A numerical investigation for variable aerodynamic effect while the system flies close to the ceiling is given in [16]. In a standard control allocation matrix, it is assumed that the change in rotor velocity is proportional to the generated thrust. In order to compensate for the ceiling effect, the real-time data is used in [17] for rotor velocity-thrust equations instead of a standard control allocation. This resulted in a better performance as compared to the conventional case. In summary, due to the significant changes in aerodynamic characteristics with the ceiling effect, there has been not enough successful flight demonstration in close proximity mode with the exception of our previous results [18], which was based on the nominal conditions.

The available approaches can handle the control of the flying robot when it does not engage with an interaction. However, the challenges associated with the aerodynamic interaction require the system to be more responsive, adaptive and resilient [19, 20, 21, 22]. This operation also brings system and environment based constraints including the level of the interaction. The available approaches that consider the constraints leverage individual multi-models for generic interaction problems which bring additional complexity [23]. Moreover, nominal optimization-based approaches are considered in the UAV control for the interaction tasks, wherein the system lacks the ability to take external forces, changing parameters and unmodeled dynamics into account [24, 25, 26].

III Problem Formulation

III-A Modeling

A snapshot of the aerial robot in close proximity is presented in Fig. 1. In order to define the position of the aerial robot with respect to the world frame, the following transformation can be used

[TABLE]

where ${}^{W}\boldsymbol{\dot{x}}$ and ${}^{B}\boldsymbol{\dot{x}}$ are the translational states in the world frame and the body frame, respectively. $R_{WB}$ is the rotation matrix from the body to the world frame. The second order nonlinear translational dynamics of the aerial robot including external forces is expressed as

[TABLE]

where $\boldsymbol{M}=m\cdot\boldsymbol{I}_{3}$ is the diagonal mass matrix and $\boldsymbol{\omega}$ is the vector of angular rates. $\boldsymbol{G}$ is gravitational force acting on system in $z$ direction [27]. The input vector is the force generated by the blades on quadrotor in the body frame, $\boldsymbol{F}=[0,0,F_{z}]^{\rm T}$. Furthermore, $\boldsymbol{F}_{\rm ext}\in\mathbb{R}^{3}$ is the external force vector acting on the system.

III-B Augmented Formulation

Consider a nonlinear system, where the system is represented by the sum of a nominal and additive models

[TABLE]

In this representation, the external forces, $\boldsymbol{F}_{\rm ext}=[{F_{\rm ext}}_{x},{F_{\rm ext}}_{y},{F_{\rm ext}}_{z}]^{\rm T}$, represent unmodeled dynamics, disturbances, changing parameters as well as the external forces arising during the interaction phase. In this context, we can consider the following problem:

Problem 1

In order to fly in close proximities, how can the system identify external forces precisely? If the system can accurately explore the external forces, how can this data be used within the controller?

The state vector in the nominal case can be given in terms of the translational positions $(x,y,z)$ and the velocities ($u,v,w$). However, to address the defined problem, we have augmented the nominal case as

[TABLE]

where $\mathrm{x}_{k}\in\mathbb{X}$. The state constraint set $\mathbb{X}$ is closed, compact and includes the origin. In the augmented model representation, the external force vector is assumed to be a constant disturbance in the form of $\dot{\boldsymbol{F}}_{ext}=0$. It is also assumed that the origin is included in the feasible set. Therefore, differentially flat states can be driven by the following input vector

[TABLE]

where $\mathrm{u}_{k}\in\mathbb{U}$. The control constraint set $\mathbb{U}$ has the same properties with the $\mathbb{X}$. The control vector includes angular positions ($\phi,\theta,\psi$). Therefore, the estimation problem can be formulated in discrete time as

[TABLE]

where the subscript $k$ is the sample taken at time $t_{k}$, where $\forall k\geqslant 0$. The function $f(\cdot)$ is composed of discretized versions of (1) and (2). In order to obtain (6a), a direct multiple shooting method is utilized based on [28]. For this operation, Gauss-Legendre integrator of order 4 is preferred with 2 steps per shooting interval and the grid size is chosen 10 ms. Moreover, the physical system parameters are adopted from [29]. The process noise is indicated by $\mathpzc{w}_{k}\in\mathbb{R}^{N_{\rm x}\times 1}$, where its covariance can be formulated by $\mathbb{E}(\mathpzc{w}\mathpzc{w}^{\rm T})=Q_{\hat{\mathrm{x}}}\in\mathbb{R}^{N_{\rm x}\times N_{\rm x}}$. In order to identify the external forces online, the following measurement function is used

[TABLE]

The measurement noise is represented by $\nu_{k}\in\mathbb{R}^{N_{\rm y}\times 1}$, where its covariance can be formulated by $\mathbb{E}(\nu\nu^{\rm T})=R_{\hat{\mathrm{x}}}\in\mathbb{R}^{N_{\rm y}\times N_{\rm y}}$.

Assumption 1

The noise vectors ($\mathpzc{w}_{k}$ and $\nu_{k}$) are independent and normally-distributed random variables.

IV Force Estimation

Consider a constrained state estimation problem in the form of a squared norm using the data collected until the jth time step:

[TABLE]

where $P_{L}$ is a positive definite weight matrix to find a balance between initial guess $\mathrm{\hat{x}}_{0}$ and the initial state $\mathrm{x}_{0}$. The other positive definite matrices are the inverse of covariance matrices, where $V=Q_{\hat{\mathrm{x}}}^{-1/2}$ and $W=R_{\hat{\mathrm{x}}}^{-1/2}$. Unfortunately, in this generic formulation, the problem may become intractable when the data size increases within time. In order to avoid the curse of dimensionality problem, we can impose a moving window by limiting the number of last measurements. In this context, the estimation window size $N$ is considered, where $L=j-N+1$. The problem in (8) can be reformulated as follows

[TABLE]

where $\mathrm{\bar{x}}$ is the estimation value given by the arrival cost to approximate the past values until the first sample of the estimation window. In terms of the estimation problem, this term is similar to the initial guess $\mathrm{\hat{x}}_{0}$ in (8) since it is the first term of the estimation window. Similarly, $\mathrm{\hat{x}}$ is the estimation value given by the moving horizon estimation. The arrival cost can be defined by Eq. (10)

[TABLE]

where it can be solved with linearity assumptions, e.g., a Kalman filter. The solution of (10) is adopted from [30]. The output of (10) will be $\mathrm{\bar{x}}_{L+1}$ and $P_{L+1}$ for the next iteration in (9). The specified parameters for the estimation problem are summarized in Table I.

Assumption 2

The state function $f(\cdot)$, and the associated costs are continuous and differentiable.

V Controller Design

In this work, two staged feedback controller is implemented. In the first stage; the NMPC, which generates the force in $z$ direction and attitude reference for the feedback controller, is used. In the second stage, a cascaded P and PID controller is used to generate desired momentum to be applied by rotors. Finally, the rotor speeds are calculated by control allocation matrix. The proposed approach is illustrated in Fig. 2.

Consider an optimization-based control problem in the form of a squared norm:

[TABLE]

where $e_{\mathrm{x}}=(\mathrm{x}_{k}^{r}-\mathrm{x}_{k})$ and $e_{\mathrm{u}}=(\mathrm{u}_{k}^{n}-\mathrm{u}_{k})$ in which $\mathrm{x}_{k}^{r}$ is the trajectory reference for the system and $\mathrm{u}_{k}^{n}$ is the nominal control signal. The weight matrix $Q$ is positive semidefinite and the weight matrix $R$ is positive definite. These weight matrices might affect the performance of the system. However, for this set of controlled states by the infinite sequences of control actions, the problem may not be applicable due to the potential infinite dimensional optimization problem [31]. Similar to the NMHE case, the infinite dimensional problem can be defined in a receding horizon manner:

[TABLE]

In this representation, the contribution of the states and control actions beyond the finite and moving horizon is approximated by the terminal cost. Similar to the stage cost weight, the weight matrix $S$ is also positive semidefinite. The specified parameters for the controller are given in Table II.

The proposed optimization-based approach is set using real-time iteration scheme in ACADO [32] and solved by qpOASES [33]. First, the self-contained C codes are generated by ACADO for the NMHE and NMPC. Afterward, these codes are integrated into the ROS-Kinetic environment. With the evaluations in the simulation environment (Gazebo), the system is tested in OptiTrack motion capture system, which localizes the aerial robot at 240 Hz over the wifi network.

VI Experimental Results

In order to show the effectiveness of our proposed approach, we performed a set of experiments. By leveraging the optimization-based force estimation and the optimization-based controller, the quadrotor system is tested while approaching the ceiling.

VI-A Experimental Setup

We used a quadrotor platform for the experiments and the scheme for the test procedure can be seen in Fig. 3. It is a small scale quadrotor (DJI F450) and its subcomponents can be seen in Fig. 4. This experimental setup is equipped with a PX4 FMU and a Raspberry Pi 3 onboard computer unit. While the Raspberry Pi 3 is responsible for the higher-level tasks (commanding generated throttle and angular positions), the PX4 FMU (Firmware v1.6.5) handles the attitude setpoint tracking as well as reaching required vertical force values. For the serial connection between onboard computer and PX4, an FTDI cable is used. In the experiments, the PX4 unit’s attitude controller is used.

VI-B Close Proximity Flight Performance Comparison

In order to evaluate the performance of the proposed controller statistically, we have compared its performance with PID and NMPC without force estimation in a scenario, where the robot is commanded to stabilize itself under a ceiling in different proximities. The deviation from the desired distance to ceiling is measured for three controllers and then is plotted in Fig. 5. The results show that the level of interaction starts affecting the performance of the PID and NMPC while approaching the ceiling. They could not bring the robot back to the reference point. The performance of these controllers further deteriorates as the robot approaches the ceiling and eventually they fail to overcome the suction force and the robot stick on the ceiling. On the other hand, the proposed force estimation-based nonlinear MPC approach manages to keep the robot even under 1 cm below the ceiling without experiencing any sticking.

VI-C Evaluation of the Battery Performance in Close Proximity

We investigated battery currents and voltages measured by Pixhawk to evaluate the power efficiency of the flight. In this analysis, we excluded PID and NMPC since they already fail to stay in close proximity interaction zones. The average power consumption is calculated as

[TABLE]

where $T$ is the duration of measurement for each distance, and $v(t)$ and $i(t)$ are battery voltage and drawn current at time $t$, respectively. The experimental average power consumption results are summarized in Fig. 6 for different proximities. It is observed that the power consumption of the system decreases up to $12.5\%$ in close proximity flight. Considering the declined power demand of the UAV, we expect that the flight duration can be longer when the system flies below the ceiling.

VI-D Further Controller Performance Analysis

In the flight tests, a random point in the air is set for the aerial robot. After this hovering phase, a reference is generated to go below the ceiling. In order to evaluate system performance, different proximities are defined. For this set of tests, the distance is measured from the top of the Raspberry Pi, as it is illustrated in Fig. 3. The distance between the propellers and the Raspberry Pi is 6 cm. In the first experiment, the system needs to stay by keeping its orientation below 16 cm from the ceiling. In Fig. 7, the system response on the tracking (Fig. 7a), controller action (Fig. 7b) and the force estimation (Fig. 7c) can be seen. The first instant of the green region indicates the switching mechanism, where the force estimation-based NMPC is activated. In the first part of the figures, the nominal model-based NMPC is used. As it can be seen, the controller can bring the system to the defined reference when the additive model is leveraged.

Similar to the first case, the system is also tested below 11 cm from the ceiling. When the switching mechanism is activated, the proposed approach actively suppress the ceiling effect. It is noted that the disturbance on the system becomes more dominant while it flies within 10 cm range as can be seen from Fig. 8.

The system performance below 6 cm from the ceiling is given in Fig. 9. The active force estimation-based NMPC mitigates the ceiling effect. As compared to Fig. 7b and Fig. 8b, the controller effort is decreased. Since the ceiling effect increases the rotor wake which results in an increase in the thrust; to stay in close proximities to the ceiling, the system does not need to generate the same thrust when hovering in the free flight case

One of the extreme cases, i.e., staying 1 cm below the ceiling, is tested in this implementation as can be seen in Fig. 10. The system still handles the ceiling effect in order not to be in permanent physical contact with the ceiling. Since the battery state is observed online in this implementation, it is explored that the current drawn from the battery can be decreased significantly during the flight in very close proximities (up to 15.8 %).

The average computation time of the NMPC is approximately 1.98 ms. When it is switched to the Force-NMPC, there is an increasing trend while the system approaches the ceiling (from 1.93 ms to 2 ms). The average computation time of the NMHE is around 3.35 ms. A similar rise is observed in the NMHE case, where the computation time changed between 3.29 ms to 3.38 ms while approaching the ceiling.

VII Discussion and Conclusion

In this work, we presented a force estimation based nonlinear MPC approach for operating quadrotors within close proximity to the surrounding. Our framework generates attitude angles and vertical force values that satisfy the dynamic behavior of the system together with the physical limits. Our algorithm is applicable in real-time and all the computations stay below 10 ms. We validated our approach experimentally in real-time using a small-scale quadrotor platform.

In the dual problem, the proposed estimation approach explores the defects (external forces, disturbances, unmodelled dynamics, and modeling mismatch) in the model leveraged within the controller. To this end, a suitable dynamical model for both free-flying and the interaction cases has been presented, together with an optimization framework for generating optimal motion reactions in close proximity. Our approach is agnostic to the information of the environment (e.g., distance to the ceiling), and hence eliminates the need for dedicated proximity or wind sensors and precise mathematical model.

One main limitation of this work is to use the motion capture system, which provides precise motion information. The estimation-approach identifies the external forces online after each new pose measurement is available. For the in-situ inspection operation, we plan to adopt visual-inertial odometry methods for pose estimation in our future work.

There are several potential extensions of the proposed work. First of all, we intend to test our controller algorithm in more challenging environments with more complex interactions such as side walls and ceilings with stalactite-like structures. Second, the proposed approach is similar to the data-driven perspective. A ceiling effect model may be further explored with data collection. Third, the energy-aspects can be included in the cost function by mapping the current drawn from the batteries and the generated vertical forces to create a controller that is capable of prolonging flight duration and battery aging awareness [34]. This can allow to plan and execute efficient task-based trajectories to increase the flight envelope.

ACKNOWLEDGMENT

The authors wish to thank for conducting the research work with support from the Energy Research Institute @ NTU (ERI@N) and NTU internal grant for the project on Large Vertical Take-Off and Landing (VTOL) Research Platform: Prototype development and demonstration (NTU internal funding).