Nonlinear Model Predictive Impedance Control of a Fully Actuated Hexarotor for Physical Interaction

Abstract

In this paper, the problem of a fully actuated hexarotor performing a physical interaction with the environment through a rigidly attached tool is considered. A nonlinear model predictive impedance control (NMPIC) method is proposed to achieve the goal in which the controller is able to simultaneously handle the constraints and maintain the compliant behavior. The design of NMPIC is the combination of a nonlinear model predictive control and impedance control based on the dynamics of the system. A disturbance observer is exploited to estimate the external wrench and then provide compensation for the model which was employed in the controller. Moreover, a weight adaptive strategy is proposed to perform the online tuning of the weighting matrix of the cost function within the optimal problem of NMPIC to improve the performance and stability. The effectiveness and advantages of the proposed method are validated by several simulations in different scenarios compared with the general impedance controller. The results also indicate that the proposed method opens a novel way for interaction force regulation.

1. Introduction

Unmanned aerial vehicles (UAVs) have attracted increasing attention in recent years for their wide range of applications, including pesticide spraying [1], aerial photography [2], environmental monitoring [3], reconnaissance [4], and transportation [5]. In addition to these non-contact tasks, there are increasing demands on contact-based operations such as industrial contact inspection [6], window-cleaning [7], peg-in-hole assembly [8], and aerial grasping [9]. Physical interactions between the UAV and environments is usually required in contact-based tasks, which is quite challenging for the controller design since more constraints should be considered and the environments may be undesirable. Additionally, the complexity of aerodynamics and the intrinsic instability further increase the difficulties.

To perform physical interaction with the environment, there have been several studies on the classical under-actuated UAV (UUAV) equipped with either a fixed tool [10] or a degree-of-freedom (DoF) robot arm [11,12]. They have reached remarkable performances when dealing with simple contact tasks. However, they lack dexterity due to the underactuation of the platform. Additionally, the coupled rotational and translational dynamics result in the limited lateral force magnitudes and disturbance rejection capabilities. Moreover, they may not be able to maintain stability when unexpected situation occurs (e.g., when the contact force suddenly disappears). For the aforementioned reasons, the fully actuated UAVs (FUAVs) for aerial interactions have emerged to address the problems in recent years. Compared with the UUAVs, the rotational and translational dynamics of FUAVs can be decoupled, which brings convenience to the controller design. A broad class of mechanical structures of FUAVs were reviewed in [13]. The discussion of the mechanical structures is beyond the scope of this paper, so the control strategies will be focused on the following.

To achieve the aerial physical interaction with the environments, and in contrast to the traditional proportional–integral–derivative (PID) controller [14] used for UAV attitude stability, the fractional order PID (FOPID) [15,16] built for dealing with actuator faults, wind effects, and some other disturbances, the controllers have to simultaneously deal with both force and motion control. In the work by [17], a hybrid controller is proposed with force control in 1 DoF for interactions and motion control for the other 5 DoFs, which is a straightforward and effective means of physically interacting. However, it is not compatible with unpredictable interactions. Therefore, a more suitable strategy is to design the controllers as inherently compatible with the environmental interactions. Among the existing interactive control methods, impedance and admittance control [18,19,20] are the most popular and widely used in various fields, particularly in robots interacting with humans/environments, which are more suitable for physical interactive control than the PID class method that is not model-based. The impedance method is exploited in [21] for a tethered UAV exerting physical human–robot interaction. Lovro et al. in [22] proposed using an impedance controller coupled with an online stiffness estimation algorithm for UAV to exert aerial inspection, applying regulated force via the end-effector to the contact surface. However, these methods are particularly designed for UUAVs, which are difficult to generalize to FUAVs. For FUAVs, a flying end-effector paradigm by means of the admittance filter is proposed in [23] to achieve 6D interaction, and the paradigm is able to control the full 6D pose and exert a full-wrench with a rigidly attached end-effector. Moreover, an impedance control (IC) scheme is proposed in [24] for aerial robotic manipulators based on a multi-layer architecture. To allow compliance in certain degrees of freedom while maintaining accurate trajectory tracking and disturbance rejection abilities in others, an axis-selective impedance controller was proposed in [25] and extended to a variable axis-selective impedance controller in [26]. The aforementioned methods may all obtain satisfactory performances under normal situations; however, they lack consideration of the constraints in their controller design. In real robotic systems, constraints always exist, such as actuator saturation, speed limits, force limits, etc. These limits are the key parts for guaranteeing safety and stability, especially in the physical interaction process.

To deal with constraints, model predictive control (MPC), as a model-based method that works for both linear and nonlinear systems, and even hybrid systems, is widely employed in many applications. The MPC can simultaneously optimize towards the optimal tracking of the reference trajectory, satisfy the input and state constraints, and maintain robust performance during a finite horizon. Manzoor et al. in [27] proposed a robust MPC framework for the trajectory tracking of a ducted fan aircraft subject to different uncertainties and external disturbances. A novel nonlinear information fusion model predictive control (NIFMPC) approach is proposed in [28] for a UUAV under external wind disturbance. The nonlinear model predictive control (NMPC) methods were extended to FUAVs or overactuated UAVs recently. In the work of [29], an NMPC method is applied for an overactuated UAV to solve trajectory tracking problems under constraints. A NMPC method is applied to multi-rotor aerial systems in [30] with arbitrarily positioned and oriented rotors considering the actuator constraints. However, the aforementioned MPC is not designed for interaction control. There have been some other research works on the MPC for physical interactions recently. In [31,32], the NMPC is combined with nonlinear moving horizon estimation (NMHE) for an aerial physical interaction task. However, the compliant behavior is obtained by the mechanical structure of the attached tool. Thus, the compliant characteristics are not feasible to adjust compared with software methods. In the work of [33], a model predictive impedance controller (MPIC) is presented to allow compliant behavior while respecting practical robotic constraints for physical interaction between the robot arm and environment. However, this method is only applied to a base-fixed robot arm system. In the flying system with a floating base, the situation is more complicated. Moreover, Nava et al. in [34] presented an optimization-based method to control aerial manipulators in physical contact with the environment considering hard constraints, although this idea is very similar to the one in this paper, the desired dynamics are provided by a PID controller which cannot obtain compliant behavior.

One critical problem for MPC is the requirement for an accurate dynamic model. There are various methods to address this problem now, e.g., HMHE [31], extended Kalman filter (EKF) [29], and disturbance observer (DO) [35]. In fact, these methods can also be employed for the measurement of an external wrench (force and torque) for interaction control without force sensors. These estimators are also called wrench estimators in some literature. The wrench estimators used for interaction control in [23,25] have the same structures as DOs; however, in general, the estimated wrench provided by DO is the sum of the physical interaction wrench, the aerodynamic wrench, a fault wrench, and other disturbances [36]. The lumped estimated wrench can be used in anti-disturbance control such as [37]. In the field of the physical interaction control, separating the physical interaction wrench from other disturbances is another popular topic which is beyond the scope of this paper. Thus, in this work, the DO will be employed for both wrench estimation and dynamic model compensation for MPC under the assumption that the physical interaction wrench covers the majority of the lumped estimated wrench. Similar assumptions can be found in [38] where the generalized momenta-based DO and the EKF algorithm are exploited in the joint torque estimation.

Another critical problem for MPC is the weight adaptation problem. In the traditional MPC, the weights of the cost function are usually fixed during the control process after tuning by human experts; however, fixed weights may not work well in some complex applications. To overcome this problem, Taherian et al. in [39] derived the transformation of a linear quadratic regulator weighting matrix using a discrete algebraic Riccati equation, and then an MPC controller adopting the idea of a discrete time linear quadratic tracking problem was proposed. Moreover, an exponential weight is introduced in [40] to solve the mathematical problem inherent in the MPC path tracking controller. In the work of [41], a weight adaptive strategy based on the fuzzy logic algorithm is proposed in the NMPC cost function to adaptively prioritize vehicle dynamics or energy efficiency, depending on the driving conditions. In the physical interaction process, the FUAVs will also go through different states or situations that require different weights. Thus, designing an adaptive weight strategy is also a challenging problem.

Based on the above observations and inspired by [33,34], a novel control scheme using nonlinear model predictive impedance control (NMPIC) on the basis of a weight-adaptive strategy together with DO for physical interaction with an FUAV considering constraints is proposed in this paper. The main contributions are listed as follows.

• A novel control scheme combining NMPC and IC is proposed to handle the constraints and maintain the compliant behavior for physical interactions with a FUAV.
• A weight-adaptive strategy is proposed for NMPIC to handle the inherent conflict between the reference signals and the constraints.
• Comparative simulations are conducted to verify the effectiveness and advantages of the proposed method in different scenarios.
• The proposed method also opens a new way for interaction force regulation.

This paper is organized as follows. The system model together with IC and DO designed based on the model are introduced in Section 2. Dependent on the IC and DO, the proposed NMPIC method with a weight adaptive strategy for the cost function is proposed in Section 3. Comparative simulation results and discussions are given in Section 4. Finally, conclusions and perspectives are presented in Section 5.

2. Problem Formulation

To describe the system, the model of the platform is firstly established. After that, a model-based impedance controller and disturbance observer are introduced. The controller and observer will be exploited as the reference dynamics of the NMPC methods presented in the next section.

2.1. System Model

To describe the dynamics of the platform, two coordinate frames are introduced, the body frame $\left(O_B,X_B,Y_B,Z_B\right)$ and the world frame $\left(O_W,X_W,Y_W,Z_W\right)$. Figure 1 illustrates the FUAV platform used in this work. The model is transformed from a classical hexarotor with six equally spaced arms around the $Z_B$ axis. Each propeller is tilted by a fixed angle around the corresponding arm. A manipulation tool is rigidly attached to the platform body with a length L from the tip to the center of mass. The attached tool is assumed to pass through the center of mass of the robot such that there is no offset. Additional mass will be also added for counter balance with the attached tool. Using the Newton–Euler formalism, the simplified dynamic model with respect to the center of mass of FUAV can be expressed as follows.

$$\begin{array}{c}m\ddot{\mathit{r}}=-mg{\mathit{e}}_3+\mathit{R}\mathit{F}+\mathit{R}{\mathit{F}}_e\hfill \\ \dot{\mathbf{\Psi }}=\mathit{W}\mathit{\omega }\hfill \\ \mathit{J}\dot{\mathit{\omega }}=S\left(\mathit{J}\mathit{\omega }\right)\mathit{\omega }+\mathit{\tau }+{\mathit{\tau }}_e\hfill \end{array}$$

(1)

where $\mathit{r}={\left[x,y,z\right]}^T$ denotes the position of the platform with respect to the world frame. $\mathbf{\Psi }={\left[\phi ,\theta ,\psi \right]}^T$ and $\mathit{\omega }={\left[{\omega }_x,{\omega }_y,{\omega }_z\right]}^T$ represent the attitude and the angular velocity of the platform expressed in the body frame, respectively. $S\left(·\right)$ denotes the skew-symmetric matrix operator. m and $\mathit{J}=diag\left(J_{xx},J_{yy},J_{zz}\right)$ are the total mass and moment of inertia of the platform, respectively. g is the gravity constant. ${\mathit{F}}_e,{\mathit{\tau }}_e\in {\mathbb{R}}^3$ denotes the external force and torque expressed in body frame, respectively. $\mathit{F},\mathit{\tau }\in {\mathbb{R}}^3$ are the total thrust force and torque generated by the six tilted propellers, respectively. $\mathit{R}\in \mathit{SO}\left(\mathit{3}\right)$ is the rotation matrix expressed with Euler angles $\mathbf{\Psi }$. $\mathit{W}$ is defined as

$$\mathit{W}=\left[\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi sec\theta & cos\varphi sec\theta \end{array}\right]$$

(2)

The usage of Euler angles will certainly cause singularity when $\theta =\pm \frac{\pi }{2}$; however, in our case, the angles will be limited by the constraints to avoid the singularity, which can be seen in the next section. Thus, it is sufficient to exploit Euler angles here. For compact expression, system (1) can be rewritten in the following form.

$$\mathit{M}\dot{\mathit{\upsilon }}+\mathit{C}\mathit{\upsilon }+\mathit{G}=\tilde{\mathit{u}}+\tilde{\mathit{d}}$$

(3)

with

$$\mathit{M}=\left[\begin{array}{cc}m{\mathit{I}}_3& \mathbf{0}\\ \mathbf{0}& \mathit{J}\end{array}\right],\mathit{C}=\left[\begin{array}{cc}\mathbf{0}& \mathbf{0}\\ \mathbf{0}& -S\left(\mathit{J}\mathit{\omega }\right)\end{array}\right]$$

$$\mathit{G}=\left[\begin{array}{c}mg{\mathit{e}}_3\\ \mathbf{0}\end{array}\right],\mathit{H}=\left[\begin{array}{cc}\mathit{R}& \mathbf{0}\\ \mathbf{0}& {\mathit{I}}_3\end{array}\right]$$

$$\tilde{\mathit{u}}=\mathit{H}\mathit{u},\tilde{\mathit{d}}=\mathit{H}\mathit{d}$$

where $\mathit{\upsilon }={\left[{\dot{\mathit{r}}}^T,{\mathit{\omega }}^T\right]}^T$ is the generalized velocities. It is worth noting that $\dot{\mathit{r}}$ is expressed in the world frame and $\mathit{\omega }$ is expressed in the body frame. $\mathit{u}={\left[{\mathit{F}}^T,{\mathit{\tau }}^T\right]}^T$ is the control wrench and $\mathit{d}={\left[{\mathit{F}}_e^T,{\mathit{\tau }}_e^T\right]}^T$ is the external wrench. ${\mathit{I}}_3\in {\mathbb{R}}^{3\times 3}$ is the identity matrix and ${\mathit{e}}_3={\left[0,0,1\right]}^T$ is the unit vector around the z axis.

To properly allocate the control wrench to each propeller, the allocation matrix is briefly introduced here.

$$\mathit{u}=\left[\begin{array}{c}{\displaystyle \sum _{i=1}^6}{\mathit{f}}_i\\ {\displaystyle \sum _{i=1}^6}\left({\mathit{p}}_i\times {\mathit{f}}_i+{\mathit{\tau }}_i\right)\end{array}\right]=\mathit{A}\mathit{T}$$

(4)

with

$$\begin{array}{c}{\mathit{f}}_i=c_f{T}_i{\mathit{R}}^{{}^{\prime }}{\mathit{e}}_3,\hfill \\ {\mathit{\tau }}_i={\left(-1\right)}^{i-1}{c}_{\tau }{T}_i{\mathit{R}}^{{}^{\prime }}{\mathit{e}}_3,\hfill \\ {\mathit{R}}^{{}^{\prime }}={\mathit{R}}_z\left(\left(i-1\right)\frac{\pi }{3}\right){\mathit{R}}_x\left({\left(-1\right)}^{i-1}\alpha \right),\hfill \\ {\mathit{p}}_i={\mathit{R}}_z\left(\left(i-1\right)\frac{\pi }{3}\right)l+{\mathit{R}}_x\left({\left(-1\right)}^{i-1}\alpha \right)d,\hfill \\ i=1,\dots ,6\hfill \end{array}$$

where ${\mathit{f}}_i,{\mathit{p}}_i,i=1,2,\dots ,6,$ are the force generated by the ith propeller and the position of each propeller expressed in body frame, respectively. $c_f$ and $c_{\tau }$ are the motor constant and the moment constant, respectively. $\alpha$ is the tilted angle around the the corresponding arm where the rotor is installed. l and d are the arm length and the position offset from the arm, respectively. $\mathit{T}={\left[T_1,\dots ,T_6\right]}^T$ denotes the thrust generated by each propeller and $\mathit{A}$ is the resulting allocation matrix. More details can be found in [23].

2.2. Impedance Control

To design the impedance controller, the close-loop dynamics should be designed to perform compliant behavior, formulated as

$${\mathit{M}}_{\upsilon }\dot{\mathit{\upsilon }}+{\mathit{D}}_{\upsilon }{\mathit{e}}_{\upsilon }+{\mathit{K}}_{\upsilon }{\mathit{e}}_p=\tilde{\mathit{d}}$$

(5)

with

$$\begin{array}{c}{\mathit{e}}_r=\mathit{r}-{\mathit{r}}_d,{\mathit{e}}_{\dot{r}}=\dot{\mathit{r}}-{\dot{\mathit{r}}}_d\hfill \\ {\mathit{e}}_R=\frac{1}{2}{\left({\mathit{R}}_d^T\mathit{R}-{\mathit{R}}^T{\mathit{R}}_d\right)}^{\vee },{\mathit{e}}_{\omega }=\mathit{\omega }-{\mathit{R}}^T{\mathit{R}}_d{\mathit{\omega }}_d\hfill \\ {\mathit{e}}_p={\left[{\mathit{e}}_r^T,{\mathit{e}}_R^T\right]}^T,{\mathit{e}}_{\upsilon }={\left[{\mathit{e}}_{\dot{r}}^T,{\mathit{e}}_{\omega }^T\right]}^T\hfill \end{array}$$

where ${\mathit{e}}_r$, ${\mathit{e}}_R$, ${\mathit{e}}_{\dot{r}}$, and ${\mathit{e}}_{\omega }$ are the position, rotation, velocity, and angular velocity tracking errors, respectively. ${\mathit{r}}_d$, ${\dot{\mathit{r}}}_d$, ${\mathit{R}}_d$, and ${\mathit{\omega }}_d$ are the desired position, velocity, rotation, and angular velocity, respectively. ${\left(·\right)}^{\vee }$ denotes the unskew operation. ${\mathit{M}}_{\upsilon }$, ${\mathit{D}}_{\upsilon }$ and ${\mathit{K}}_{\upsilon }\in {\mathbb{R}}^{6\times 6}$ are the positive definite diagonal matrices representing the desired apparent inertia matrix, damping matrix and stiffness matrix, respectively. By combining (3) and (5), the impedance controller can be derived as

$${\tilde{\mathit{u}}}^{\ast }=\left(\mathit{M}{\mathit{M}}_{\upsilon }^{-1}-{\mathit{I}}_6\right)\widehat{\mathit{d}}-\mathit{M}{\mathit{M}}_{\upsilon }^{-1}\left({\mathit{D}}_{\upsilon }{\mathit{e}}_{\upsilon }+{\mathit{K}}_{\upsilon }{\mathit{e}}_p\right)+\mathit{C}\mathit{\upsilon }+\mathit{G}$$

(6)

where $\widehat{\mathit{d}}$ is the external wrench estimated by the disturbance observer, which will be introduced in the following. The relationship between ${\tilde{\mathit{u}}}^{\ast }$ and ${\mathit{u}}^{\ast }$ is ${\tilde{\mathit{u}}}^{\ast }=\mathit{H}{\mathit{u}}^{\ast }$.

2.3. Disturbance Observer

To estimate the external wrench, a widely used momentum-based disturbance observer is introduced here. For a unified form, the disturbance observer can be constructed as

$$\left[\begin{array}{c}\mathit{M}\dot{\widehat{\mathit{\upsilon }}}\\ \dot{\widehat{\mathit{d}}}\end{array}\right]=\left[\begin{array}{c}-\mathit{C}\mathit{\upsilon }-\mathit{G}\\ \mathbf{0}\end{array}\right]+\left[\begin{array}{c}\tilde{\mathit{u}}\\ \mathbf{0}\end{array}\right]+\mathit{L}\left(\mathit{M}\mathit{\upsilon }-\mathit{M}\widehat{\mathit{\upsilon }}\right)$$

(7)

where $\mathit{L}={\left[{\mathit{L}}_1,{\mathit{L}}_2\right]}^T\in {\mathbb{R}}^{12\times 6}$ with ${\mathit{L}}_1=2diag\left({\omega }_{o1},\dots ,{\omega }_{o6}\right)$ and ${\mathit{L}}_2=diag\left({\omega }_{o1}^2,\dots ,{\omega }_{o6}^2\right)$. ${\omega }_i,i=1,2,\dots ,6$ denotes as the observer bandwidth. $\widehat{\mathit{\upsilon }}$ and $\widehat{\mathit{d}}$ are the estimation of the generalized velocity $\mathit{\upsilon }$ and the external wrench $\tilde{\mathit{d}}$, respectively. Similar structures for the exploited disturbance observer can be found in [26].

3. Nonlinear Model Predictive Impedance Control

3.1. NMPIC Design

To design a control strategy that can simultaneously handle the constraints and maintain the compliant behavior, the NMPC method is exploited to be combined with the impedance controller. When the FUAV is interacting with the environment within the constraints, the controller should have the same performance as the impedance controller. However, when one of the constraints is reached or some unexpected circumstances happen, the controller should be able to maintain stability within the constraints. In order to achieve this goal, the reference control input at time $t_k$ is set to be ${\tilde{\mathit{u}}}^{\ast }\left(t_k\right)$. This is also denoted as the virtual input of an outer loop in [34]. The real control input at time $t_k$ of the impedance controller is set to be

$$\tilde{\mathit{u}}\left(t_k\right)={\tilde{\mathit{u}}}^{\ast }\left(t_k\right)$$

(8)

From another point of view, the control law (8) is equal to an NMPC problem with the following quadratic cost function without constraints.

$$\underset{\tilde{\mathit{U}}}{min}\sum _{i=k}^{k+N-1}{\left(\tilde{\mathit{u}}\left(t_i\right)-{\tilde{\mathit{u}}}^{\ast }\left(t_i\right)\right)}^T\mathbf{\Gamma }\left(\tilde{\mathit{u}}\left(t_i\right)-{\tilde{\mathit{u}}}^{\ast }\left(t_i\right)\right)$$

(9)

where $\mathbf{\Gamma }=diag\left({\gamma }_1,\dots ,{\gamma }_6\right)\in {\mathbb{R}}^{6\times 6}$ is a positive diagonal weighting matrix and $\tilde{\mathit{U}}={\left[{\tilde{\mathit{u}}}^T\left(t_k\right),\dots ,{\tilde{\mathit{u}}}^T\left(t_{k+N-1}\right)\right]}^T$. The optimal problem (9) is solved over a finite horizon $\Delta t=N{T}_s$ of N steps with $T_s$ being the time step. It is obvious that the solution of the (9) without any constraints is exactly (8) when only the optimal control at current time $t_k$ is applied. However, in real physical interactions, some constraints exist objectively based on the physical laws, such as the actuator constraints and the model constraints. Other constraints for ensuring the safety in case that some unexpected situations occur during the process of physical interaction should also be considered, such as position constraints and velocity constraints. In this paper, the following constraints are considered.

Firstly, the model constraints should satisfy (1) with the state variables selected as $\mathit{X}={\left[{\mathit{r}}^T,{\dot{\mathit{r}}}^T,{\mathbf{\Psi }}^T,{\mathit{\omega }}^T\right]}^T$. In a real system, the model parameters cannot be accurately measured and some unmodeled dynamics may occur due to the simplicity in the modeling process. Therefore, the external wrench cannot be accurately predicted based on model. In this case, the external wrench is replaced by the estimated wrench provided by the DO. However, even with DO, only the current external wrench can be known, while the external wrench in the future remains unknown. To deal with this, we assume that the external wrench changes slowly during the prediction horizon. Thus, the estimated wrenches will be regarded as constants during the prediction period. To summarize, the model constraints are formulated as follows.

$$\left\{\begin{array}{c}\dot{\mathit{X}}\left(t_{m+1}\right)=\mathit{f}\left(\mathit{X}\left(t_m\right),\tilde{\mathit{u}}\left(t_m\right),\widehat{\mathit{d}}\left(t_k\right)\right)\hfill \\ \mathit{X}\left(t_k\right)=\mathit{current}\mathit{state}\hfill \end{array}\right.$$

(10)

where $m=k,\dots ,k+N-1$ and $\mathit{f}\left(·\right)$ is the discrete form of system (1).

Next, since physical actuators are bound to have their limits in the rotational speeds, the generated forces should also have boundaries. One can construct the actuator constraints as

$${\mathit{T}}_{min}\le \mathit{T}\le {\mathit{T}}_{max}$$

(11)

where ${\mathit{T}}_{min}$, ${\mathit{T}}_{max}$ are the minimum and maximum thrusts generated by each propeller. However, this is not intuitive during the physical interaction process, where the composite generated wrench should be limited. Thus, instead of using inequality (11), the following wrench constraints are used.

$${\mathit{u}}_{min}\le \mathit{u}\le {\mathit{u}}_{max}$$

(12)

where ${\mathit{u}}_{min}$ and ${\mathit{u}}_{max}$ are the minimum and maximum wrenches which have an indefinite sign.

Moreover, to show the power of the NMPC method, the constraints of the state variables $\mathit{X}$ are also added.

$${\mathit{X}}_{min}\le \mathit{X}\le {\mathit{X}}_{max}$$

(13)

These constraints indicate that the positions, velocities, angles, and angular velocities are all limited. In this case, the angles will be limited to avoid meeting the singularity.

To conclude, the proposed NMPIC method is formulated as

$$\begin{array}{c}\underset{\tilde{\mathit{U}}}{min}{\displaystyle \sum _{i=k}^{k+N}}{\left(\tilde{\mathit{u}}\left(t_i\right)-{\tilde{\mathit{u}}}^{\ast }\left(t_i\right)\right)}^T\mathbf{\Gamma }\left(\tilde{\mathit{u}}\left(t_i\right)-{\tilde{\mathit{u}}}^{\ast }\left(t_i\right)\right)\hfill \\ st.\left\{\begin{array}{c}\stackrel{.}{\mathit{X}}\left(t_{m+1}\right)=f\left(\mathit{X}\left(t_m\right),\tilde{\mathit{u}}\left(t_m\right),\widehat{\mathit{d}}\left(t_k\right)\right)\hfill \\ \mathit{X}\left(t_k\right)=\mathit{current}\mathit{state}\hfill \end{array}\right.,\hspace{1em}\hspace{1em}\hspace{1em}\begin{array}{cc}m=k,\dots ,k+N-1& \end{array}\hfill \\ {\mathit{u}}_{min}\le \mathit{u}\left(t_m\right)\le {\mathit{u}}_{max},\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}m=k,\dots ,k+N\hfill \\ {\mathit{X}}_{min}\le \mathit{X}\left(t_m\right)\le {\mathit{X}}_{max},\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} m=k,\dots ,k+N\hfill \end{array}$$

(14)

3.2. Weight Adaptive Strategy

In the traditional NMPC method, the parameters of the weighting matrix $\mathbf{\Gamma }$ are tuned based on human expert knowledge, which is time-consuming and cumbersome. Moreover, the weighting matrix is usually set to remain fixed all through the control process. However, in our case, this does not work for the reason that there is an inherent conflict between the reference signals and the constraints. The control force in the x axis will be taken as an example for explanation. Consider that a constraint limits the force in x axis in a certain range. Since the reference control input provided by (6) has no limit and the cost function is designed to make the real control input as close to the reference control input as possible. As a consequence, a large cost will be generated in the x axis to prevent the constraint from being conceded. In contrast, the costs in other five channels become relatively less, resulting in not following the reference control inputs of these channels. This may cause large tracking errors in other channels and even stability problems. To address this problem, a weight adaptive strategy is proposed in this work.

In the weight adaptive strategy, the weighting matrix $\mathbf{\Gamma }$ is the composition of two parts: the base weighting matrix ${\mathbf{\Gamma }}_b$ and constraints-related weighting matrix ${\mathbf{\Gamma }}_c$, formulated as,

$$\mathbf{\Gamma }={\mathbf{\Gamma }}_b{\mathbf{\Gamma }}_c$$

(15)

The base weighting matrix ${\mathbf{\Gamma }}_b=diag\left({\gamma }_{b1},\dots ,{\gamma }_{b6}\right),{\gamma }_{bi}>0,i=1,\dots ,6$ is the same as the fixed weighting matrix in the traditional NMPC. The constraints-related weighting matrix is designed to satisfy ${\mathbf{\Gamma }}_c=diag\left({\gamma }_{c1},\dots ,{\gamma }_{c6}\right),{\gamma }_{ci}\in \left(0,1\right],i=1,\dots ,6$. The design of ${\mathbf{\Gamma }}_c$ should meet the following requirements. When the reference controls can be reached within the constraints, there is no need to use ${\mathbf{\Gamma }}_c$ so that all the parameters in ${\mathbf{\Gamma }}_c$ should be set to 1. When the reference controls cannot be reached within the constraints, the parameter(s) associated with the corresponding channel(s) in ${\mathbf{\Gamma }}_c$ should decrease according to some strategies in order to balance the other costs. A simple strategy is given as

$$\begin{array}{c}{\gamma }_{ci}={\displaystyle \prod _{j=1}^n}{w}_{ji},i=1,\dots ,6\hfill \\ w_{ji}=\left\{\begin{array}{cc}{\epsilon }_j\hfill & q_j\le {\alpha }_j\hfill \\ \frac{1-{\epsilon }_j}{{\Delta }_j}\left(q_j-{\alpha }_j\right)+{\epsilon }_j\hfill & {\alpha }_j<q_j<{\alpha }_j+{\Delta }_j\hfill \\ 1\hfill & {\alpha }_j+{\Delta }_j\le q_j\le {\beta }_j-{\Delta }_j\hfill \\ -\frac{1-{\epsilon }_j}{{\Delta }_j}\left(q_j-{\beta }_j\right)+{\epsilon }_j\hfill & {\beta }_j-{\Delta }_j<q_j<{\beta }_j\hfill \\ {\epsilon }_j\hfill & q_j\ge {\beta }_j\hfill \end{array}\right.\hfill \end{array}$$

(16)

where ${\alpha }_j\le q_j\le {\beta }_j$ with $q_j\in {\mathit{X}}_d\cup {\mathit{u}}^{\ast }$ is one of the inequality constraints in (12) and (13). The subscript j denotes the jth constraint that relates to the corresponding control channel. n is the total number of constraints that correspond to the ith channel. The curve of $w_{ji}$ with respect to $q_j$ is illustrated in Figure 2.

For each channel, the corresponding relationship is given in

Table 1. The corresponding relationship between the constraints and the control channel.

Control Input	Parameter	Constraint Variable	Description
${\tilde{u}}_1$	${\gamma }_{c1}$	$x_d$, ${\dot{x}}_d$, $u_1^{\ast }$	Force around the x axis
${\tilde{u}}_2$	${\gamma }_{c2}$	$y_d$, ${\dot{y}}_d$, $u_2^{\ast }$	Force around the y axis
${\tilde{u}}_3$	${\gamma }_{c3}$	$z_d$, ${\dot{z}}_d$, $u_3^{\ast }$	Force around the z axis
${\tilde{u}}_4$	${\gamma }_{c4}$	${\varphi }_d$, ${\omega }_{xd}$, $u_4^{\ast }$	Torque around the x axis
${\tilde{u}}_5$	${\gamma }_{c5}$	${\theta }_d$, ${\omega }_{yd}$, $u_5^{\ast }$	Torque around the y axis
${\tilde{u}}_6$	${\gamma }_{c6}$	${\psi }_d$, ${\omega }_{zd}$, $u_6^{\ast }$	Torque around the z axis

.

To further explain the corresponding relationship, we will take the force control regarding the x axis as an example. In this case, $q_j$ is specified as $q_1=x_d$, $q_2={\dot{x}}_d$, and $q_3=u_1^{\ast }$. Then, the corresponding $w_{j1}$ and ${\gamma }_{c1}$ are computed by expressions given in (16). It can be seen that only the position, velocity, and force constraints regarding the x axis are assumed to be related to ${\tilde{u}}_1$. However, the dynamics in (1) reveal that the angle constraints and angular velocity constraints will also have impacts on the control input in the x axis. This may be solved by a coupled weighting matrix in future research. The results in our tests show that the proposed strategy is sufficient to improving the performance. The signal block diagram of the whole control scheme is presented in Figure 3.

4. Simulations

4.1. Implementation Details

To show the effectiveness and advantages of the proposed method, several simulations were implemented using the PX4/Gazebo platform [42] together with the Robot Operating System (ROS) middleware. The simulations are running on a PC equipped with an Intel^® 2.20 GHz CoreTM i7-8750H CPU (x8) and 16 GB RAM which runs the Linux Ubuntu 16.04 LTS operating system. The optimal control problem in (14) is solved by the solver qpOASES [43]. The system parameters of the FUAV platform are presented in

Table 2. System parameters of the FUAV platform.

Parameter	Value	Unit
m	1.8	Kg
$J_{xx}$	0.05	$\mathrm{Kg}{\mathrm{m}}^2$
$J_{yy}$	0.05	$\mathrm{Kg}{\mathrm{m}}^2$
$J_{zz}$	0.09	$\mathrm{Kg}{\mathrm{m}}^2$
l	0.34	m
d	0.04	m
$\alpha$	${47}^{\circ }$	deg
$c_f$	$2.0\times {10}^{-5}$	$\mathrm{N}/{\mathrm{Hz}}^2$
$c_{\tau }$	0.014	m
L	0.7	m

.

4.2. Simulation Results

Three simulations are implemented to present the improvements and capabilities of the proposed method whose scene is shown in Figure 4. The first simulation is about a comparison of the performance during the normal push and slide task between the general impedance controller and the proposed NMPIC. In the second simulation, the ability to simultaneously handle the constraints and compliance of the proposed NMPIC is presented by adding force constraints when conducting the push and slide task. The results also show a new way for force regulation only by adding proper force constraints. In addition, the advantages of the proposed weight adaptive strategy are also illustrated by comparing with a fixed weight NMPIC method. Third, by adding more constraints, the power to ensure the safety of the proposed NMPIC is shown by implementing a challenging task during which the contact will suddenly lose.

4.2.1. Normal Case

In the first case, a normal push and slide task along a smooth plane wall is applied after the steady-state hovering of the FUAV at the position ${\mathit{r}}_d={\left[0,0,1\right]}^T$. A box with length of 3 m, height of 3 m, and width of 0.2 m is employed to simulate the wall placed at position ${\mathit{r}}_w={\left[0.4,0,1.5\right]}^T$ described by the center of the box. It is assumed that all of the states and wrench variables are within the constraints in the normal case. The push and slide task is divided into three stages: the approaching–contacting stage, sliding stage, and releasing–departing stage. The stages are divided based on the desired position setpoints. In the approaching–contacting stage, the desired position setpoint gradually increases to ${\mathit{r}}_d={\left[1.5,0,1\right]}^T$ with a velocity of 0.075 m/s around the x axis in 20 s. Following this desired position setpoint, the FUAV will first approach the wall and then be contacted at some position. After that, the contact force will increase as the desired position setpoint stop increases. Then, the desired position setpoint moves to ${\mathit{r}}_d={\left[1.5,1,1\right]}^T$ with a desired velocity of 0.05 m/s along the y axis driving the FUAV to perform a sliding operation. Finally, the desired position setpoint changes to ${\mathit{r}}_d={\left[0,1,1\right]}^T$ with a velocity of 0.075 m/s along the x axis resulting in the releasing–departing stage, during which the FUAV will slowly release the contact force and leave the wall. The entire desired process is illustrated in Figure 5.

The simulations are performed between the general IC and the proposed NMPIC. For a fair comparison, the performances are conducted using the same desired position setpoint with a precise system time provided by ROS. The parameters of IC for this case are listed in

Table 3. Parameters of IC.

Parameter	Value
${\mathit{M}}_{\upsilon }$	$diag\left(1.5,1.5,1.5,0.045,0.045,0.085\right)$
${\mathit{D}}_{\upsilon }$	$diag\left(8,8,8,2,2,2\right)$
${\mathit{K}}_{\upsilon }$	$diag\left(5,5,5,3,3,3\right)$
${\mathit{L}}_1$	$diag\left(10,10,10,4,4,4\right)$
${\mathit{L}}_2$	$diag\left(25,25,25,4,4,4\right)$

. To show the improvements of the proposed NMPIC, the same parameters of the same control law are exploited to be the reference control input for the quadratic cost function in (9). Since the inequality constraints are assumed to have no effect on the control performance in the normal case, only the model constraints are added to solve the NMPIC problem. For the same reason, the weights of the cost function can be fixed in this case, given as $\mathbf{\Gamma }=diag\left(1,1,1,10,10,10\right)$. The time step is set to 0.1 s and the prediction horizon N is set to 5.

The simulation results are plotted in Figure 6. Figure 6a,b show the actual position and velocity in three dimensions of both NMPIC and IC, respectively. The three stages are denoted using different colors. In the approaching–contacting stage (blue region), it can be seen that position x increases to 0.3 m in the beginning and then maintains its position due to the contact with the wall. After contacting, the external forces begin to increase as can be found in Figure 6c,d. The measured external forces are obtained by a gazebo contact plugin attached to the wall and the estimated external forces are gained by the DOs. After the forces increase to the maximum, the FUAV begins to slide along the wall resulting in the increase in the y position in the yellow region. During the sliding stage, it can be seen that the forces remain almost constant. Then, in the releasing–departing stage (green region), the external forces begin to descend until leaving the wall surface. Finally, the position x returns to 0 m. The input forces and torques of both controllers are given in Figure 6e,f. It can be seen that they have very a similar performance in this case. However, they are not exactly the same. From Figure 6c,d, it can be observed that their applied forces are indeed different during the sliding stage, in which the applied force under NMPIC is approximately 3.1 N and the one under IC is approximately 3.6 N. This may account for the modeling errors since model constraints are considered in the optimal problem of NMPIC. Furthermore, from the represented results, we can find that both IC and NMPIC work well in the normal aerial push and slide task of FUAV.

4.2.2. Force Constraints

It is important to ensure safety when robots are interacting with environments, especially during sustained contact-based tasks. One of the possible strategies to guarantee safety is to limit the interaction forces. To show the capability of the proposed NMPIC under force constraints and the advantages of the proposed adaptive weight strategy, a scenario in which the FUAV is assigned to apply force to a fixed point is introduced. In addition, three controllers, including IC, NMPIC, and NMPIC with fixed weight (FNMPIC), are compared in this case.

Similarly with the normal case, the FUAV is first hovering at the position ${\mathit{r}}_d={\left[0,0,1\right]}^T$. Then, the desired position moves to ${\mathit{r}}_d={\left[3,0,1\right]}^T$ with a uniform velocity of 0.1 m/s along the x axis. The parameters of the impedance controller are all the same as those in the normal case. The fixed weight of the FNMPIC is set to $\mathbf{\Gamma }=diag\left(10,10,10,10,10,10\right)$. For a fair comparison, the base weighting matrix ${\mathbf{\Gamma }}_b$ in (15) is set to the same as $\mathbf{\Gamma }$ in FNMPIC. To determine the constraints-related weighting matrix, the force constraints used in this case are given as

$${\left[-5,-5,16,-2,-2,-2\right]}^T\le \mathit{u}\le {\left[2,5,24,2,2,2\right]}^T$$

(17)

Since only force constraints are considered in this case, the constraints-related weighting matrix is only related to force constraints. The total number n in (16) of the constraints for each channel is 1. To be specific, take the force related to x axis as an example. In this case, $q_1=u_1^{\ast }$ and ${\gamma }_{c1}=w_{11}$ are the specifications of (16). Based on the constraints set in (17), the parameters for calculating $w_{11}$ are set as follows: ${\Delta }_1=0.1$, ${\alpha }_1=-5$, ${\beta }_1=2$, ${ϵ}_1=0.05$. The parameters in five other channels are set by similar ways. The parameters ${\alpha }_i$ and ${\beta }_i$ are set based on the constraints (17). Other parameters $\Delta$ and $ϵ$ are identically set to 0.1 and 0.05 for all channels, respectively.

The simulation results in this case are shown in Figure 7. Figure 7a presents the actual positions of the three controllers. In this task, the FUAV firstly approaches the wall and then begins to increase the force and meanwhile maintains the position. From Figure 7d, it can be seen that the interaction force increases to approximately 8 N under IC without force constraints. Thanks to the addition of the force constraints in the proposed NMPIC, the interaction force is limited at 2 N, as seen in Figure 7b. However, purely adding the force constraints will lead to an unstable problem because of the fixed weighting matrix, as plotted in Figure 7a,c. The reason for this is the conflict between the reference force and the force constraint. The reference force related to the x axis is illustrated in Figure 7f. With the help of the proposed adaptive strategy, the weight corresponding to channel of force around the x axis will decrease as the reference force enters the constraints region (blue region), as presented in Figure 7e. As a consequence, the FUAV has the power to handle the constraint and maintain stability at the same time. From Figure 7b, it can also be seen that, even though no direct force control method is used, the controller is able to regulate the force to the force constraints. In this case, the force constraints can be viewed as the desired force. This method also has the potential to perform variable force-tracking operations by online changing the force constraints.

4.2.3. Loss of Contact

During the real inspection operations, some unexpected situations may occur such as an unpredictable loss of contact. This may account for the lack of prior information about the environment or the measurement faults of the localization system. To simulate this scenario, the FUAV is assigned to perform an inspection task along a wall with unknown length. For comparison, IC and the proposed NMPIC with all the position, velocity, and force constraints are considered in this case.

As illustrated in Figure 8, the FUAV is first assigned to contact point A and apply a force with 2 N along the x axis. This is obtained by moving the desired setpoint from ${\mathit{r}}_d={\left[0,0,1\right]}^T$ to ${\mathit{r}}_d={\left[2,0,1\right]}^T$, similarly to the last case. Then, the desired setpoint continues moving along the y axis with a desired velocity of 0.05 m/s. Since the length of the wall is assumed to be unknown in this case, the terminal desired setpoint is set to ${\mathit{r}}_d={\left[2,2,1\right]}^T$ to complete the inspection task of the whole wall. As a consequence, the FUAV will suffer from a loss of contact at point B during the inspection process. To prevent a collision with the wall, the state constraints are inserted together with the force constraints in the last case to solve the optimal problem (14), formulated as

$$\begin{array}{c}{\left[-5,-5,-1,-1,-1,-1,-\frac{\pi }{3},-\frac{\pi }{3},-\frac{\pi }{3},-2,-2,-2\right]}^T\le \mathit{X}\hfill \\ \le {\left[0.45,2,2,1,1,1,\frac{\pi }{3},\frac{\pi }{3},\frac{\pi }{3},2,2,2\right]}^T\hfill \end{array}$$

(18)

It is worth noting that the constraints for Euler angles here are designed to avoid singularity. The position x is limited to 0.45 m to avoid bumping into the wall. This value is obtained by preliminarily acquiring the position of the wall in this case. The position can be also obtained by collision detection using the DO if it cannot be measured in advance. The parameters of IC and the reference impedance controller of NMPIC are the same as those defined in the last case. The only difference between the parameters in this case is the weighting matrix constraints-related weighting matrix ${\mathbf{\Gamma }}_c$. Since more constraints are added, the calculation of ${\mathbf{\Gamma }}_c$ is more complicated. Furthermore, take the force around the x axis for explanation. The adaptive strategy of (16) becomes ${\gamma }_{c1}=w_{11}\ast w_{21}\ast w_{31}$ with $q_1=x_d$, $q_2={\dot{x}}_d$ and $q_3=u_1^{\ast }$. The parameters for calculating $w_{j1},j=1,2,3$ are set as follows: ${\Delta }_1=0.1$, ${ϵ}_1=0.05$, ${\Delta }_2=0.1$, ${ϵ}_2=0.1$, ${\Delta }_3=0.1$, and ${ϵ}_3=0.1$. ${\alpha }_i$ and ${\beta }_i,i=1,2,3$ are set according to the corresponding values defined in (18). Similar parameters are set for the five other channels. For simplicity, the ${\Delta }_i$ and ${ϵ}_i$ are set as the same as those in the channel of force around the x axis.

The simulation results in this case are visualized in Figure 9. In this challenging task, the FUAV first approaches the wall and is contacted, the same as that in the normal case. The position x increases in the beginning and then remains constant while contacting, as shown in Figure 9a. After that, the FUAV slides along the wall, which is also the direction of the y axis, as seen in Figure 9b. Since the length of the wall is unknown, the FUAV will suffer from a loss of contact at the edge of the wall, denoted as the blue dash line in Figure 9. From Figure 9a, it can be seen that the x position enters the red region after the loss of contact, which indicates that the body of the FUAV collides with the wall. It can also be seen in Figure 9c that the that the FUAV falls down after the collision. This could cause severe damage if it happens in a real flight. By applying the proposed NMPIC, the situation is significantly improved. As can be observed from Figure 9a, the FUAV will rush forward for a short distance because of inertia once the loss of contact occurs. After that, the position constraint of x will force the FUAV back to avoid colliding with the wall. Although some oscillations exist, the FUAV will eventually be stable at the position of 0.43 m within the constraint 0.45 m. This simulation result significantly shows the power of the proposed NMPIC for ensuring safety in the physical interaction task. In Figure 9d,e, the measured and estimated external forces are shown. After the loss of contact, it can be seen that the measured forces disappeared, but the estimated external forces still exist. This phenomenon accounts for the insensitivity of the DO to signals with high frequency. The oscillations may be attributed to this phenomenon. In Figure 10a, the changing weights in the entire process are presented. In contrast to that in the last case, the weights descend twice during the process. The first descent happens when the desired setpoint reaches the position constraint, as can be seen in Figure 10c. The second descent happens when the reference force reaches the force constraint, as can be seen in Figure 10b. These descents are implemented to balance the conflict between the reference signals and the constraints.

5. Conclusions

In this work, a control scheme is proposed to tackle the issue of simultaneously handling the constraints and maintaining compliant behavior in the aerial physical interaction with the environment using a FUAV. The proposed NMPIC combines the advantages of both the NMPC and general IC by treating IC as reference signals of NMPC. A DO is employed for both external wrench estimation and model compensation for the NMPIC controller. To address the conflict between the reference signals and the constraints, a weight adaptive strategy is proposed to tune the weighting matrix of the cost function online in the optimal problem of NMPIC. Three simulation scenarios are presented to validate the effectiveness and advantages of the proposed method. The results show that, in the normal case, the NMPIC can have a similar performance as a general IC. Under the force constraints, the NMPIC with the weight adaptive strategy can perform well both in restricting the interaction force and maintaining stability. By viewing the force constraints as the desired forces, the proposed method is able to regulate force without using any direct force control method. It also has potential to perform force tracking tasks by changing the force constraints online, which will be further studied in the future. Even in the case when contact is suddenly lost, the NMPIC has the capability of avoiding collision and eventually returning to stability.