NMPC-Based Trajectory Optimization and Hierarchical Control of a Ducted Fan Flying Robot with a Robotic Arm

Abstract

Ducted fan flying robots with robotic arms can perform physical interaction tasks in complex environments such as indoors. However, the coupling effects between the aerial platform, the robotic arm, and physical environment pose significant challenges for the robot to accurately approach and stably contact the target. To address this problem, we propose a unified control framework for a ducted fan flying robot that encompasses both flight planning and physical interaction. This contribution mainly includes the following: (1) A nonlinear model predictive control (NMPC)-based trajectory optimization controller is proposed, which achieves accurate and smooth tracking of the robot’s end effector by considering the coupling of redundant states and various motion and performance constraints, while avoiding potential singularities and dangers. (2) On this basis, an easy-to-practice hierarchical control framework is proposed, achieving stable and compliant contact of the end effector without controller switching between the flight and interaction processes. The results of experimental tests show that the proposed method exhibits accurate position tracking of the end effector without overshoot, while the maximum fluctuation is reduced by up to 75.5% without wind and 71.0% with wind compared to the closed-loop inverse kinematics (CLIK) method, and it can also ensure continuous stable contact of the end effector with the vertical wall target.

1. Introduction

1.1. Background and Motivation

Unmanned aerial vehicles (UAVs) are widely applied in non-contact observation tasks such as damaged building detection [1] and fire monitoring [2]. In recent years, unmanned aerial vehicles with robotic arms (i.e., flying robots or aerial manipulators) have garnered increasing attention [3], which can be applied in contact-based interaction tasks, such as remote valve operation in nuclear leakage plants [4], contact inspection on industrial oil and gas pipelines [5], vegetation sampling in dense forests [6], etc., to liberate people from dangerous work and effectively improve work efficiency. The aerial platform of a flying robot is most often a helicopter [7] or a multirotor [8]. However, such an open rotor structure necessitates that the robot maintains a sufficient distance from the surrounding environment to ensure safety and stability. Additionally, to avoid the range of the rotor disc, the operating space of the robotic arm fixed on the platform is also limited, especially for side targets (such as switches on vertical walls). In view of this, a ducted fan-based flying robot is proposed in this paper, as shown in Figure 1. Benefiting from the aerodynamic advantages of ducted fans (see [9] for details), the proposed robot has both a compact structure and high payload capacity, exhibiting better confined space adaptability and environmental interaction safety. Moreover, compared to open rotor-based flying robots, the robotic arm of the proposed robot can more easily perform side operations with small-range joint motions. However, there is a strong kinematic and dynamics coupling between the aerial platform and the robotic arm. On the one hand, the aerial platform is subjected to time-varying disturbance forces and moments generated during the motion of the robotic arm, including quasi-static effects caused by changes in the system’s center of gravity and inertia, as well as rapid dynamic effects caused by the inertial forces and moments of the robotic arm joints. Moreover, the physical contact reaction forces further exacerbate this influence. On the other hand, the floating aerial platform induces real-time changes in the position, attitude, velocity, and acceleration of the robotic arm base, thereby altering the kinematic and dynamics characteristics of the robotic arm, significantly increasing its complexity.

The general process of aerial interaction tasks includes four stages, namely, take off and free flight, approach to the target, contact with the target, and departure. In this process, there are primarily two challenges. First, the ducted fan flying robot with a robotic arm is a redundant system with multiple degrees of freedom (DOF), where there are significant differences in dynamic characteristics between the DOF of the platform and arm. In addition, the system faces various time-varying environmental constraints and self-constraints, including operation space constraints, actuator performance constraints, joint self-collision constraints, ducted fan aerodynamics constraints, etc. Thus, one challenge is how to perform redundant DOF motion planning to ensure the robot’s end effector reaches the target position accurately during the approach stage. Second, the robot’s aerial platform is different from the fixed base of conventional industrial robot arms, as there is an inevitable continuous relative motion between it and the target. Due to the limited allowable reaction force of the platform, the transient forces generated by the end effector’s physical interaction may cause the system to deviate from state equilibrium and become unstable. Thus, another challenge is how to implement compliant control to ensure the robot’s end effector interacts with the target stably during the contact stage.

1.2. Related Works

In terms of trajectory planning, the most direct approach is to consider the motions of the aerial platform and robotic arm independently [10]. Ubellacker et al. [11] proposed a minimum-snap trajectory planning method for a quadrotor platform combined with a visual three-dimensional object pose estimator for aerial rapid grasping tasks. The tracking error during the grasping process is tolerated by a soft gripper. Based on the assumption that the aerial platform can ideally hover near the target, the analytical inverse kinematics method [12] and the inverse Jacobian method [13] are applied to the trajectory planning of the robotic arm, in which the aerial platform is regarded as a fixed base. Liu et al. [14] proposed a reinforcement learning-based trajectory planning method for the robotic arm of a quadrotor flying robot, which minimizes the interference of the arm movement on the quadrotor platform while ensuring the tracking performance of the end effector. Cao et al. [15] designed a partially decoupled motion planning framework for a quadrotor flying robot with a delta arm to effectively perform a pick-and-place task. In this framework, the three-dimensional position of the aerial platform is first planned, and then the motions of the arm’s joints are planned based on this input. The independent planning method described above may lead to the arm’s joints being at their limits or even singularity when the relative position between the aerial platform and the target is close to the arm’s operation space boundary. Moreover, the hovering error of the aerial platform can directly deteriorate the end effector’s tracking accuracy. To perform trajectory planning for the platform and the arm simultaneously, a closed-loop inverse kinematics (CLIK) method based on the robot’s integrated kinematic model is proposed in [16]. Furthermore, Lee et al. [17] introduced a null-space-based CLIK method, which utilizes the joint redundancy of the flying robot to avoid pose singularity in trajectory planning by adding singularity point damping. However, CLIK is essentially an open-loop planning (but closed-loop tracking) method, and the real-time response characteristics of the system are not considered in the optimization process. The response performance of the robot’s end effector may be affected by dynamic characteristic differences between the aerial platform and the arm, and it is difficult to fully consider the environmental constraints and self-constraints of the system. Dimmig et al. [18] proposed a model-based deep reinforcement learning method for task planning in unknown environments, enabling a flying robot to autonomously plan object pushing tasks under varying friction conditions without prior dynamical knowledge. Liu et al. [19] introduced a proximal policy optimization-based intelligent planning method to bypass complex dynamic modeling and reduce disturbances during manipulation, supporting efficient trajectory planning and replanning for enhanced system stability. He et al. [20] developed an end-effector-centric planning framework that incorporates a high-level policy learned via imitation learning, allowing a fully actuated flying robot to perform versatile tasks such as writing and pick-and-place through intuitive teleoperation and policy transfer. Although these methods demonstrate superior performance, the design process is complex and exhibits low generalization, making it inconvenient for engineering implementation.

In terms of physical contact control, Zhang et al. [13] regarded the external force acting on the flying robot’s end effector as a part of the disturbances sustained by the aerial platform, and focused on improving the disturbance rejection performance of the aerial platform controller. Meng et al. [21] performed a vertical wall pressing operation using a quadrotor with a fixed rod, and the contact force was generated by adjusting the quadrotor’s desired pitch angle. Cuniato et al. [22] designed a position/force hybrid strategy to decompose the controller of the flying robot into pose space and wrench space. This method demonstrates superior control performance, but it is typically applicable to known targets and specific environments. To improve the adaptability of flying robots when interacting with different environments, joint servos with a spring–lever transmission mechanism are employed to achieve compliance [23]. Zhang et al. [24] designed controllers for a flying robot for the free flight stage and contact operation stage, focusing on the stability during the controller switching process. Different from direct force control methods, the impedance control method ensures contact stability by adjusting the robot’s motion variables to make the system show desired impedance characteristics. This method has a simple controller structure and is easy to implement. Thus, it is widely used in the compliant contact process of flying robots [25,26,27]. However, the impedance controller will degenerate into an inverse dynamics position controller in non-contact mode (target approach mode), and better compliance may deteriorate trajectory tracking performance.

1.3. Contribution

In view of the challenges discussed above, this paper proposes a unified control framework for a ducted fan flying robot with a 4-DOF robotic arm. The main contributions include the following:

• A nonlinear model predictive control-based trajectory optimization controller for the flying robot is proposed. The designed controller achieves smooth and accurate tracking of the end effector without overshoot by balancing the aerodynamic wall effects of the ducted fan and the motion coupling of the robotic arm. Meanwhile, by considering various motion and performance constraints, potential singularities and dangers in conventional methods are avoided.
• A unified hierarchical control framework for both approach and contact stages is proposed, which includes the physical interaction control layer, trajectory optimization layer, and motion control layer, and is easy-to-practice. The proposed framework avoids performance coupling between the flight and interaction processes, achieving stable and compliant contact for the end effector without controller switching.
• Both simulation analyses and actual experimental tests are performed to verify the effectiveness of the controller proposed.

The remaining parts are organized as follows. The dynamics and kinematic models of the ducted fan flying robot are introduced in Section 2. Then, the NMPC-based trajectory optimization is detailed in Section 3. Section 4 details the hierarchical control framework, including the motion controller as well as the admittance contact controller of the robot. The simulation analyses and experimental tests of the proposed controller are given in Section 5 and Section 6, respectively. Conclusions are detailed in Section 7.

2. System Description of the Robot

2.1. Introduction of the Robot

Figure 1 shows the prototype of the flying robot. The aerial platform of the robot consists of four coaxial ducted fans. A 4-DOF serial robotic arm with an end effector is arranged on the front side of the robot, and correspondingly, the rear side of the robot is provided with a counterweight to balance the gravitational moment from the arm in the initial posture. The flying robot’s key structural parameters are shown in

Table 1. Key structural parameters of the flying robot.

Parm	Description	Value
$m_a$	Mass of aerial platform	5.7 kg
${\mathit{I}}_a$	Inertia matrix of aerial platform	diag (0.314, 0.307, 0.326) kg∙m2
$m_{l1}$	Mass of arm link 1	0.0984 kg
${\mathit{I}}_{l1}$	Inertia matrix of arm link 1	diag (3.454 × 10−5, 3.269 × 10−5, 1.885 × 10−5) kg∙m2
$m_{l2}$	Mass of arm link 2	0.1385 kg
${\mathit{I}}_{l2}$	Inertia matrix of arm link 2	diag (6.035 × 10−5, 3.306 × 10−4, 3.429 × 10−4) kg∙m2
$m_{l3}$	Mass of arm link 3	0.1327 kg
${\mathit{I}}_{l3}$	Inertia matrix of arm link 3	diag (3.065 × 10−5, 2.423 × 10−4, 2.516 × 10−4) kg∙m2
$m_{l4}$	Mass of arm link 4	0.2259 kg
${\mathit{I}}_{l4}$	Inertia matrix of arm link 4	diag (8.087 × 10−5, 9.313 × 10−5, 7.598 × 10−5) kg∙m2

.

Figure 2 illustrates the robot’s coordinate system. Specify ${\sum }_{\mathrm{I}}-{\mathrm{X}}_{\mathrm{I}}{\mathrm{Y}}_{\mathrm{I}}{\mathrm{Z}}_{\mathrm{I}}$ as the inertial coordinate system (ICS), ${\sum }_{\mathrm{a}}-{\mathrm{X}}_{\mathrm{a}}{\mathrm{Y}}_{\mathrm{a}}{\mathrm{Z}}_{\mathrm{a}}$ as the aerial platform body-fixed coordinate system (ACS), and ${\sum }_i-{\mathrm{X}}_i{\mathrm{Y}}_i{\mathrm{Z}}_i$ as the robotic arm link coordinate system, where $i=1,2,3,4$ refers to each link. In particular, ${\sum }_0-{\mathrm{X}}_0{\mathrm{Y}}_0{\mathrm{Z}}_0$ denotes the robotic arm base-fixed coordinate system, and ${\sum }_{\mathrm{e}}-{\mathrm{X}}_{\mathrm{e}}{\mathrm{Y}}_{\mathrm{e}}{\mathrm{Z}}_{\mathrm{e}}$ denotes the end effector coordinate system. Let $p_a={\left[\begin{array}{ccc}{x}_a& y_a& z_a\end{array}\right]}^{\mathrm{T}}$ be the position vector of the platform with respect to ICS, and ${\Phi }_a={\left[\begin{array}{ccc}{\phi }_a& {\theta }_a& {\psi }_a\end{array}\right]}^{\mathrm{T}}$ be the attitude vector of the aerial platform described by Euler angles. The velocity and angular rate vector of the aerial platform with respect to ACS are defined as $v_a^a={\left[\begin{array}{ccc}{u}_a& v_a& w_a\end{array}\right]}^{\mathrm{T}}$ and ${\omega }_a^a={\left[\begin{array}{ccc}{p}_a& q_a& r_a\end{array}\right]}^{\mathrm{T}}$, and then we have $v_a^a=R_a^{\mathrm{T}}{\dot{p}}_a$ and ${\omega }_a^a=R_a^{\mathrm{T}}{T}_a{\dot{\Phi }}_a$, where $R_a$ is the rotation matrix describing ACS relative to ICS, and $T_a$ denotes the transformation matrix between the aerial vehicle’s Euler angles’ differential and angular rates. ${\mathsf{\omega }}_{ij}\left(i,j=1,2,3,4\right)$ denotes the rotational speed of each rotor of the ducted fan system. The description of the robotic arm coordinate system follows modified Denavit–Hartenberg (D-H) rules, and is illustrated in Figure 3. Let $q={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right]}^{\mathrm{T}}$ be the arm joint angle vector, and the corresponding D-H parameters are given in

Table 2. D-H parameters of the flying robot.

Link $\mathit{i}$	${\mathit{a}}_{\mathit{i}-\mathbf{1}}$ (m)	${\mathit{\alpha }}_{\mathit{i}-\mathbf{1}}$ (Rad)	${\mathit{d}}_{\mathit{i}}$ (m)	${\mathit{\theta }}_{\mathit{i}}$ (Rad)
1	0	0	0	${\mathit{q}}_{\mathbf{1}}$
2	0	π/2	0	${\mathit{q}}_2$
3	0.130	0	0	${\mathit{q}}_3$
4	0.124	0	0	${\mathit{q}}_4$
EE	0.170	0	0	0

, where link EE denotes the end effector, $a_{i-1}$, ${\alpha }_{i-1}$, $d_i$, and ${\theta }_i$ denote the link’s length, twist, offset, and joint angle, respectively. Notice that $e_1$ in Figure 3 refers to the length of link 1, which is not reflected in Table 2 according to the modified D-H rules.

2.2. Kinematic Model

According to the definition of the coordinate system in Section 2.1, the velocity vector $v_i^i$ of the origin of ${\sum }_i$ with respect to ${\sum }_i$ and the angular rate vector ${\omega }_i^i$ of link $i$ with respect to ${\sum }_i$ can be derived recursively by

$$\left\{\begin{array}{c}{v}_i^i=R_{i-1}^i\left(v_{i-1}^{i-1}+{\omega }_{i-1}^{i-1}\times p_i^{i-1}\right)\hfill \\ {\omega }_i^i=R_{i-1}^i{\omega }_{i-1}^{i-1}+{\dot{q}}_i{e}_{\mathrm{z}i}\hfill \end{array}\right.$$

(1)

where $i=1,2,3,4$ refers to each link, $R_{i-1}^i$ refers to the rotation matrix describing ${\sum }_{i-1}$ relative to ${\sum }_i$, $p_i^{i-1}$ refers to the origin’s position vector of ${\sum }_i$ with respect to ${\sum }_{i-1}$, and $e_{\mathrm{z}i}$ refers to the Z axis unit vector of ${\sum }_i$. Notice that the base of the arm is fixed to the aerial platform, while the end effector is fixed to arm link 4, thus following Equation (1). The velocity vector $v_0^0$ of the arm base with respect to ${\sum }_0$, the angular rate vector ${\omega }_0^0$ of the arm base with respect to ${\sum }_0$, the velocity vector $v_e^e$ of the end effector with respect to ${\sum }_e$, and the arm’s angular rate vector ${\omega }_e^e$ end effector with respect to ${\sum }_e$ can be derived respectively by

$$\left\{\begin{array}{c}{v}_0^0=v_a^a+{\omega }_a^a\times p_0^a\hfill \\ {\omega }_0^0={\omega }_a^a\hfill \end{array}\right.$$

(2)

$$\left\{\begin{array}{c}{v}_e^e=v_4^4+{\omega }_4^4\times p_e^4\hfill \\ {\omega }_e^e={\omega }_4^4\hfill \end{array}\right.$$

(3)

where $p_0^a$ refers to the position vector of the arm base relative to the center of mass (CoM) of the aerial vehicle, and $p_e^4$ refers to the end effector’s position vector with respect to ${\sum }_4$.

Let $\zeta ={\left[\begin{array}{cc}{p}_a^{\mathrm{T}}\hfill & {\Phi }_a^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$ be the motion variables of the aerial platform and $\xi ={\left[\begin{array}{cc}{\zeta }^{\mathrm{T}}\hfill & q^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$ be the full state variables of flying robot, and then combine Equations (1)–(3). The end effector’s velocity with respect to ${\sum }_e$ is described as

$$v_e^e=J_{t,e}^e\left({\Phi }_a,q\right)\dot{\xi }$$

(4)

where $J_{t,e}^e\left({\Phi }_a,q\right)$ is called translation Jacobian matrix of the robot’s end effector in ${\sum }_e$. Specify $p_e={\left[\begin{array}{ccc}{x}_e& y_e& z_e\end{array}\right]}^{\mathrm{T}}$ as the position vector of the end effector with respect to ICS and $R_e$ as the rotation matrix describing ${\sum }_e$ relative to ICS, and transform Equation (4) to ICS, and then the robot’s kinematic model is described in a compact form as follows:

$${\dot{p}}_e=R_e{J}_{t,e}^e\left({\Phi }_a,q\right)\dot{\xi }=J_{t,e}\left({\Phi }_a,q\right)\dot{\xi }$$

(5)

2.3. Dynamics Model

The robot’s dynamics model is established using recursive Newton–Euler (RNE) method. The Newton–Euler equations of the robotic arm in ${\sum }_i$ and the aerial platform in ACS can be established, respectively, as

$$\left\{\begin{array}{c}{F}_i^i=m_{li}{\dot{v}}_{C_i}^i\hfill \\ M_i^i=I_{li}{\dot{\omega }}_i^i+{\omega }_i^i\times I_{li}{\omega }_i^i\hfill \end{array}\right.$$

(6)

$$\left\{\begin{array}{c}{F}_a^a=m_a\left({\dot{v}}_a^a+{\omega }_a^a\times v_a^a\right)\hfill \\ M_a^a=I_a{\dot{\omega }}_a^a+{\omega }_a^a\times I_a{\omega }_a^a\hfill \end{array}\right.$$

(7)

In Equation (6), $m_{li}$ is the mass of the robotic arm’s link $i$, and $I_{li}$ is the inertia matrix of link $i$, illustrated in Table 1. $F_i^i$ is the resultant force, and $M_i^i$ is the resultant moment on link $i$, which can be derived by force and moment equilibrium equations as

$$\left\{\begin{array}{c}{F}_i^i=f_i^i-{\left(R_i^{i+1}\right)}^{\mathrm{T}}{f}_{i+1}^{i+1}+R_i^{\mathrm{T}}{m}_{li}g\hfill \\ M_i^i=n_i^i-{\left(R_i^{i+1}\right)}^{\mathrm{T}}{n}_{i+1}^{i+1}-p_{i+1}^i\times {\left(R_i^{i+1}\right)}^{\mathrm{T}}{f}_{i+1}^{i+1}-p_{C_i}^i\times (F_i^i-R_i^{\mathrm{T}}{m}_{li}g)\hfill \end{array}\right.$$

(8)

where $f_i^i$ and $n_i^i$ denote the force and moment vector of link $i-1$, respectively, and act on link $i$ with respect to ${\sum }_i$, $p_{C_i}^i$ denotes the position vector of the CoM of link $i$ with respect to ${\sum }_i$, and $R_i$ denotes the rotation matrix describing ${\sum }_i$ relative to ICS. Notice that the driving torque on joint $i$ can be expressed by the projection of $n_i^i$ along the $e_{\mathrm{z}i}$ axis as ${\tau }_i={(n_i^i)}^{\mathrm{T}}{e}_{\mathrm{z}i}$. ${\dot{v}}_{C_i}^i$ denotes the acceleration vector of the CoM of link $i$ with respect to ${\sum }_i$, and ${\dot{\omega }}_i^i$ denotes the angular acceleration vector of link $i$ with respect to ${\sum }_i$, which is derived by differentiating Equation (1) as

$$\left\{\begin{array}{c}{\dot{v}}_{C_i}^i={\dot{\omega }}_i^i\times p_{C_i}^i+{\omega }_i^i\times ({\omega }_i^i\times p_{C_i}^i)+{\dot{v}}_i^i\hfill \\ {\dot{v}}_i^i=R_{i-1}^i({\dot{\omega }}_{i-1}^{i-1}\times p_i^{i-1}+{\omega }_{i-1}^{i-1}\times ({\omega }_{i-1}^{i-1}\times p_i^{i-1})+{\dot{v}}_{i-1}^{i-1})\hfill \\ {\dot{\omega }}_i^i=R_{i-1}^i{\dot{\omega }}_{i-1}^{i-1}+R_{i-1}^i{\omega }_{i-1}^{i-1}\times {\dot{q}}_i{e}_{\mathrm{z}i}+{\ddot{q}}_i{e}_{\mathrm{z}i}\hfill \end{array}\right.$$

(9)

In Equation (7), $m_a$ is aerial platform mass, and $I_a$ is the platform inertia matrix, illustrated in Table 1. $F_a^a$ is the resultant force and $M_a^a$ is the resultant moment on the ducted fan aerial platform, which can be derived as

$$\left\{\begin{array}{c}{F}_a^a={\displaystyle \sum _{i=1}^4{F}_{aero,i}^a}+F_{fus}^a+F_{arm}^a+F_{bal}^a+R_a^{\mathrm{T}}{m}_{a}g\hfill \\ M_a^a={\displaystyle \sum _{i=1}^4{M}_{aero,i}^a}+M_{gyro}^a+M_{arm}^a+M_{bal}^a\hfill \end{array}\right.$$

(10)

$F_{aero,i}^a$ and $M_{aero,i}^a$ are the aerodynamic force and aerodynamic moment of the coaxial ducted fans, which are modeled based on the blade element momentum theory (BEMT) in our previous work [28], and can be calculated by

$$\left\{\begin{array}{c}{F}_{aero}^a={\left[0 0 -\left(T_{r1}+T_{r2}+T_{duct}\right)\right]}^{\mathrm{T}}\hfill \\ M_{aero}^a={\left[\begin{array}{ccc}-e_d\left(T_{r1}+T_{r2}+T_{duct1}\right)& e_d\left(T_{r1}+T_{r2}+T_{duct1}\right)& Q_{r1}-Q_{r2}\end{array}\right]}^{\mathrm{T}}\hfill \end{array}\right.$$

(11)

where $T_{duct}$ is the duct augmentation thrust, $T_{r1}$ and $T_{r2}$ are the thrusts of the upper and lower rotors, $Q_{r1}$ and $Q_{r2}$ are the counter-torques of the upper and lower rotors, and $e_d$ represents the distance between the rotor’s center and the CoM of the aerial platform along ${\mathrm{X}}_{\mathrm{a}}$ and ${\mathrm{Y}}_{\mathrm{a}}$ directions.

$F_{fus}^a$ is the fuselage resistance, which can be calculated by

$$F_{fus}^a={\left[\begin{array}{ccc}-1/2\rho S_{fx}\left|u_a\right|u_a\hfill & -1/2\rho S_{fy}\left|v_a\right|v_a\hfill & -1/2\rho S_{fz}\left|w_a\right|w_a\hfill \end{array}\right]}^{\mathrm{T}}$$

(12)

where $S_{fx}$, $S_{fy}$, and $S_{fz}$ are the effective resistance areas of the platform along the three axes.

$M_{gyro}^a$ is the rotor gyro moment, which can be calculated by

$$M_{gyro}^a=n_r{J}_r\left[\begin{array}{c}-q_a\\ p_a\\ 0\end{array}\right]{\displaystyle \sum _{i=1}^4{(-1)}^{i+1}({\omega }_{i1}-{\omega }_{i2})}$$

(13)

where $n_r$ is the number of blades per rotor disc, and $J_r$ is the moment of inertia of a blade.

$F_{arm}^a$ and $M_{arm}^a$ are the force and moment of the robotic arm acting on the platform, which can be calculated from Equation (8) as

$$\left\{\begin{array}{c}{F}_{arm}^a=-R_1^0{f}_1^1\hfill \\ M_{arm}^a=-\left(R_1^0{n}_1^1+p_0^a\times R_1^0{f}_1^1\right)\hfill \end{array}\right.$$

(14)

$F_{bal}^a$ and $M_{bal}^a$ are the force and moment generated by the counterweight, which can be calculated by

$$\left\{\begin{array}{c}{F}_{bal}^a=R_a^{\mathrm{T}}{m}_{bal}g\hfill \\ M_{bal}^a={[\begin{array}{ccc}-e_{bal}& 0\hfill & 0\hfill \end{array}]}^{\mathrm{T}}\times R_a^{\mathrm{T}}{m}_{bal}g\hfill \end{array}\right.$$

(15)

where $m_{bal}$ is the mass of the counterweight, and $e_{bal}$ is the distance between the counterweight and the CoM of the aerial platform.

Combining Equations (6)–(15), the robot’s dynamics model can be solved recursively and organized into the following compact form:

$$M\left(\xi \right)\ddot{\xi }+C\left(\xi ,\dot{\xi }\right)\dot{\xi }+g\left(\xi \right)={\mu }_u+{\mu }_{ext}$$

(16)

where $M\left(\xi \right)$ denotes the inertia terms, $C\left(\xi ,\dot{\xi }\right)$ denotes the Coriolis and centrifugal terms, and $g\left(\xi \right)$ denotes the gravity terms. ${\mu }_u$ refers to the generalized input force vector, which denotes the active force inputs corresponding to the generalized state variables. ${\mu }_{ext}$ is the generalized external force vector, which denotes the physical interaction force between the end effector and the environment expressed in the generalized state space. Let $f_{ext}$ be the force vector acting on the end effector from physical interaction, and $\tau ={\left[\begin{array}{cccc}{\tau }_1\hfill & {\tau }_2\hfill & {\tau }_3\hfill & {\tau }_4\hfill \end{array}\right]}^{\mathrm{T}}$ be the joint torque vector. ${\mu }_u$ and ${\mu }_{ext}$ can be calculated by

$$\left\{\begin{array}{c}{\mu }_u={\left[{\mu }_{u,f}\hspace{1em}{\mu }_{u,m}\hspace{1em}{\mu }_{u,\tau }\right]}^{\mathrm{T}}={\left[R_a{\displaystyle \sum _{i=1}^4{F}_{aero,i}^a}\hspace{1em}{T}_a^{\mathrm{T}}{R}_a{\displaystyle \sum _{i=1}^4{M}_{aero,i}^a}\hspace{1em}\tau \right]}^{\mathrm{T}}\\ {\mu }_{ext}=J_{t,e}^{\mathrm{T}}\left({\Phi }_a,q\right)f_{ext}\hfill \end{array}\right.$$

(17)

2.4. Environment Model

In this paper, a physical contact scenario for the end effector with a vertical wall is considered. The wall is perpendicular to the ground and parallel to ${\mathrm{Y}}_{\mathrm{I}}$ robot axis. Therefore, the end effector is only subjected to external force from ${\mathrm{X}}_{\mathrm{I}}$ direction during the contact process. Following the method of Forte et al. [29], a simplified model of physical contact process can be established as follows:

$$f_{ext,x}:=\left\{\begin{array}{cc}-k_{wall}\left(x_e-x_{wall}\right)\hfill & \mathrm{if}{x}_e-x_{wall}>0\hfill \\ 0\hfill & \mathrm{if}{x}_e-x_{wall}\le 0\hfill \end{array}\right.$$

(18)

where $x_{wall}$ denotes the position of the wall in ICS, and $k_{wall}$ indicates equivalent stiffness coefficient of the wall.

3. NMPC-Based Trajectory Optimization

3.1. Problem Formulation

The flying robot has eight independent DOF (excluding the clamping DOF of the end effector), where the aerial platform has four DOF in longitude, latitude, altitude, and yaw, and the robotic arm has four DOF in four joints. Notice that the aerial platform is an under-actuated system, where its roll and pitch states, as inner-loop dependent states, are coupled with the lateral and longitudinal DOF, respectively. Separate the roll and pitch angles of the aerial platform from full state variables $\xi$, and let ${\eta }_a={\left[\begin{array}{cc}{p}_a^{\mathrm{T}}\hfill & {\psi }_a\hfill \end{array}\right]}^{\mathrm{T}}$, ${\sigma }_a={\left[\begin{array}{cc}{\phi }_a\hfill & {\theta }_a\hfill \end{array}\right]}^{\mathrm{T}}$, and then the independent state variables of the robot can be denoted as $x={\left[\begin{array}{cc}{\eta }_a^{\mathrm{T}}\hfill & q^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$, which are redundant with respect to the end effector’s three-dimensional position.

The aim of trajectory optimization is to obtain the ideal trajectory of the robot’s independent state variables based on the end effector’s desired target position, in order to ensure both tracking accuracy and efficiency. During the trajectory tracking process of the robot’s end effector, the aerial platform’s motion should be dominant to reduce the disturbance of the arm acting on the aerial platform and improve tracking efficiency. However, environments will have a significant impact on the ducted fan aerodynamics when the platform is close to the target or constrained obstacles, resulting in performance degradation or even collision risks. In addition, the flying robot is also subject to constraints such as actuator performance limitations, joint angle limitations, joint self-collision limitations, etc., which must be carefully considered in trajectory optimization.

From the above, the trajectory optimization problem is formulated as follows: For a desired three-dimensional position $p_{e,d}$ of the end effector, find the independent state variable trajectory references $x_r$ of the robot to minimize tracking error by solving a constrained NMPC problem. As a part of the hierarchical control framework, the state variable references $x_r$ will then be used as the reference inputs of the platform position controller and the robotic arm joint controller, which will be introduced in Section 4.2.

The trajectory optimization includes three parts: prediction model, receding optimization, and feedback correction. Let $u={\left[\begin{array}{cc}{\dot{\eta }}_a^{\mathrm{T}}\hfill & {\dot{q}}^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$ be the control variables, and then the discrete state model of the robot with independent state variables $x$ is formulated as

$$x\left(k+1\right)=x\left(k\right)+T_{s}u\left(k\right)$$

(19)

where $T_s$ is the sampling period, and $k$ indicates the current time step. Define Equation (19) as the prediction model to predict the future states of the system, and then the optimization problem is formulated as

$$\begin{array}{cc}\hfill \underset{u_k}{\min }& J\left(x\left(k\right),u_k\right)\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& x\left(k+i+1\left|k\right.\right)=x\left(k+i\left|k\right.\right)+T_{s}u\left(k+i\right)\hfill \\ & x\left(k\left|k\right.\right)=x\left(k\right)\hfill \\ & g_{\min }\le g\left(x\left(k+i\left|k\right.\right),u\left(k+i\right)\right)\le g_{\max }\hfill \\ & x_{\min }\le x\left(k+i\left|k\right.\right)\le x_{\max }\hfill \\ & u_{\min }\le u\left(k+i\right)\le u_{\max }\hfill \\ & \Delta u_{\min }\le \Delta u\left(k+i\right)\le \Delta u_{\max }\hfill \end{array}$$

(20)

where $x\left(k+i\left|k\right.\right)$ and $u\left(k+i\right)$ are the predictive states and the control inputs, respectively. $i=0,1,\cdots ,N_p$, where $N_p$ is prediction horizon. $\Delta u\left(k+i\right)=u\left(k+i\right)-u\left(k+i-1\right)$ is the control input increments. $u_k={\left[\begin{array}{cccc}{u}^{\mathrm{T}}\left(k\right)\hfill & u^{\mathrm{T}}\left(k+1\right)\hfill & \cdots \hfill & u^{\mathrm{T}}\left(k+N_m-1\right)\hfill \end{array}\right]}^{\mathrm{T}}$ is the control input vector to be optimized, where $N_m$ is control horizon. $J\left(x\left(k\right),u_k\right)$ is the cost function with multiple optimization objectives $h_j\left(x\left(k\right),u_k\right)$, which can be denoted as

$$J\left(x\left(k\right),u_k\right)={\displaystyle \sum _{j=1}^m{h}_j\left(x\left(k\right),u_k\right)}={\displaystyle \sum _{j=1}^m{h}_j\left(x\left(k+i\left|k\right.\right),u\left(k+i\right)\right)}$$

(21)

Notice that the prediction model in Equation (20) is an integrator, and the nonlinearity of the optimization problem is mainly manifested in the cost function. In addition, $x_{\min }$ and $x_{\max }$, $u_{\min }$ and $u_{\max }$, $\Delta u_{\min }$ and $\Delta u_{\max }$, and $g_{\min }$ and $g_{\max }$ refer to the lower bound and upper bound of the state constraints, control constraints, control increment constraints, and general constraints, respectively. In this optimization problem, the cost function is the soft constraints of the system, which makes the overall performance optimal by balancing various optimization objectives. Correspondingly, the upper and lower bounds refer to the hard constraints of the system, which must be strictly satisfied.

The optimization problem in Equation (20) at each time step is solved based on the current states $x\left(k\right)$ to obtain the control input sequence $u_k^{\ast }$ that optimizes the system performance criterion $J\left(x\left(k\right),u_k\right)$, and then the first element $u^{\ast }\left(k\right)$ of $u_k^{\ast }$ is taken and combined with Equation (19) to obtain the optimal independent state variable references $x_r^{\ast }$ of the next time step. Finally, $x_r^{\ast }$ act on the low-level motion controller of the robot, and the prediction model is updated according to the actual states to resolve the above optimization problem at next time step to overcome the model errors and disturbances. The problem in Equation (20) is a constrained nonlinear programming (NLP) problem, which can be solved by sequential quadratic programming (SQP) algorithm. Notice that the position and attitude control layer of the robot is closed-loop-stable, and the trajectory optimization layer is not involved in the low-level motion feedback control and runs at a lower frequency, which can meet the requirements of the online solution algorithm.

3.2. Optimization Tasks

(1)

End effector tracking error

The accurate tracking of the desired target by the end effector of robot is the primary optimization task. Rearrange Equation (5) with independent state variables $x$ as

$${\dot{p}}_e\left(t\right)=J_{tx,e}\left({\Phi }_a\left(t\right),q\left(t\right)\right)\dot{x}\left(t\right)+J_{t\sigma ,e}\left({\Phi }_a\left(t\right),q\left(t\right)\right){\dot{\sigma }}_a\left(t\right)$$

(22)

where $J_{tx,e}$ and $J_{t\sigma ,e}$ are the terms related to $x$ and ${\sigma }_a$ in the Jacobian matrix $J_{t,e}$. Since the flying robot does not perform aerobatics, its attitude angles conform to the small-angle assumption and have small rates of change [30]. Thus, the first term in Equation (22) plays a dominant role in ${\dot{p}}_e$, and the second term can be approximately ignored. The discrete prediction model of the three-dimensional position of the end effector can be obtained according to Equation (22) as

$$p_e\left(k+i+1\left|k\right.\right)=p_e\left(k+i\left|k\right.\right)+T_s{J}_{tx,e}\left(x\left(k+i\left|k\right.\right)\right)u\left(k+i\right)$$

(23)

Then the end effector tracking error cost function is defined as

$$h_1={\displaystyle \sum _{i=1}^{N_p-1}{‖p_e\left(k+i\left|k\right.\right)-p_{e,d}\left(k+i\right)‖}_{W_1}^2}+{‖p_e\left(k+N_p\left|k\right.\right)-p_{e,d}\left(k+N_p\right)‖}_{W_{s1}}^2$$

(24)

where $W_1$ refers to the weight matrix for the tracking error, and in particular, $W_{s1}$ refers to the terminal weight matrix. The terminal cost term is used to ensure the stability of the finite horizon NMPC problem, and the detailed theoretical proof is referred to in [31].

(2)

CoM of the robotic arm

The motion of the arm brings static disturbances, i.e., gravity and gravity moment, and dynamic disturbances, i.e., inertial force and inertia moment, to the aerial platform, of which the static disturbances play a dominant role [32]. Although the position and attitude controller of the robot has disturbance stability to some extent, it is still necessary to reduce the effect of the change of the robotic arm’s CoM on the system performance.

From the kinematic model of the robot, the position vector relative to the aerial platform is

$$p_m^a\left(t\right)=\left({\displaystyle \sum _{i=1}^4{m}_{li}{p}_{C_i}^a\left(q\left(t\right)\right)}\right)\cdot {\left({\displaystyle \sum _{i=1}^4{m}_{li}}\right)}^{-1}$$

(25)

where $p_{C_i}^a$ denotes the position vector of the CoM of link $i$ with respect to ACS, a function of joint angle vector $q$. According to Equation (25), calculate the projection of the change of the arm’s CoM on aerial platform’s ${\mathrm{X}}_{\mathrm{a}}{\mathrm{Y}}_{\mathrm{a}}$ plane, and rewrite it as a function of the state variables $x$, then the arm’s CoM change discrete prediction model can be obtained as

$$e_{m,xy}\left(k+i\left|k\right.\right)=e_{xy}{p}_m^a\left(x\left(k+i\left|k\right.\right)\right)-e_{xy}{p}_{m0}^a\left(q_0\right)$$

(26)

where $e_{xy}$ is the unit projection matrix of the aerial platform’s ${\mathrm{X}}_{\mathrm{a}}{\mathrm{Y}}_{\mathrm{a}}$ plane, and $p_{m0}^a$ denotes the arm’s CoM position vector relative to the aerial platform at the initial joint posture $q_0$.

Then, the cost function of the change of the arm’s CoM is defined as

$$h_2={\displaystyle \sum _{i=1}^{N_p-1}{‖e_{m,xy}\left(k+i\left|k\right.\right)‖}_{W_2}^2}+{‖e_{m,xy}\left(k+N_p\left|k\right.\right)‖}_{W_{s2}}^2$$

(27)

where $W_2$ and $W_{s2}$ refer to the weight matrix and the terminal weight matrix, respectively.

(3)

Wall effect of the ducted fan

Compared to the open environment, the constrained environment, i.e., the vertical wall target, can cause the inflow of the ducted fan to be asymmetric on the near-wall and far-wall sides, leading to changes in the ducted fan aerodynamic characteristics, which is called wall effect [33]. Our previous work studied the flow field mechanism of wall effect [34,35], indicating that it can induce an unbalanced moment for the ducted fan, leading the aerial platform to tilt toward the near-wall side and rapidly deteriorating its response performance, even posing a collision risk. Therefore, despite the cost function $h_2$ aiming to avoid excessive extension of the robotic arm, the distance limit between aerial platform and environment (target) must be considered.

The computational fluid dynamics (CFD) simulation results of the velocity and pressure fields of the ducted fan under near-wall condition shown in Figure 4 and Figure 5 illustrate the induced tilting moment $M_{WE}$ accordingly, where $V$ is the airflow velocity, $C_p$ is the normalized pressure coefficient, $\mathsf{\omega }$ and $R_r$ are the rotor’s hovering speed and radius, and $d_{WE}$ denotes the distance between the ducted fan center and the wall plane (D-W distance). When the ducted fan approaches the wall plane, the airflow on the duct lip at the near-wall side (region B) decelerates and the static pressure rises, causing a decrease in the lift $F_B$, while the airflow at the far-wall side (region A) is minimally affected by the wall, and $M_{WE}$ is induced by the difference between $F_A$ and $F_B$. Notice that $M_{WE}$ is for single ducted fan, and for the robot in this paper, there are two ducted fans affected by wall effect simultaneously, and the total tilting moment is $2M_{WE}$. Defining the normalized tilting moment coefficient as $C_{WE}=M_{WE}/\left(\rho \pi {\mathsf{\omega }}^2{R}_r^5\right)$, the relationship between $C_{WE}$ and normalized distance $d_{WE}/R_r$ can be obtained from CFD results, as shown in Figure 5, where the dotted line indicates the outer edge of the duct. It can be seen that when the D-W distance decreases, the induced tilting moment increases rapidly, and the relationship between $C_{WE}$ and $d_{WE}/R_r$ can be approximated as a power function by fitting as

$$C_{WE}=f_{WE}\left(d_{WE}\right)\approx 0.002052{\left(d_{WE}/R_r\right)}^{-1.323}+3.153\times {10}^{-5}$$

(28)

The discrete prediction model of $d_{WE}$ can be calculated as

$$d_{WE}\left(k+i\left|k\right.\right)=\left|e_x\left(C_{p}x\left(k+i\left|k\right.\right)-p_{e,d}\left(k+i\right)\right)\right|-e_d$$

(29)

where $e_x$ denotes the unit vector of the X axis, $C_p$ is the state selection matrix for selecting $p_a$ from $x$, and $e_d$ is the distance along the axis of ACS between the ducted fan center and the origin of ACS. Substitute Equation (29) into Equation (28) to have

$$C_{WE}\left(k+i\left|k\right.\right)=f_{WE}\left(d_{WE}\left(k+i\left|k\right.\right)\right)$$

(30)

Then the cost function of the wall effect can be defined by the induced tilting moment coefficient as

$$h_3={\displaystyle \sum _{i=1}^{N_p-1}{\left|C_{WE}\left(k+i\left|k\right.\right)\right|}_{W_3}^2}+{\left|C_{WE}\left(k+N_p\left|k\right.\right)\right|}_{W_{s3}}^2$$

(31)

where $W_3$ and $W_{s3}$ refer to the weight and the terminal weight, respectively.

(4)

Control action

The control action is penalized to avoid excessive control increment to the motion control layer of the robot, which can be given as

$$h_4={\displaystyle \sum _{i=0}^{N_m-1}{‖\Delta u\left(k+i\right)‖}_{W_4}^2}$$

(32)

where $W_4$ is the weight matrix.

3.3. System Constraints

(1)

Safety D-W distance of the ducted fan

In Section 3.2, the soft constraint of wall effect is introduced into the cost function to balance the motion allocation between the aerial platform and the arm. Furthermore, direct collision between duct and wall should be strictly avoided for safety. Therefore, the minimum D-W distance should be considered as a hard constraint, which can be defined from Equation (29) as

$$\left|d_{WE}\left(k+i\left|k\right.\right)\right|\ge d_{WE,\min }$$

(33)

where $d_{WE,\min }$ is the minimum safety D-W distance. Notice that the definition in Equation (33) conforms to the form of general constraint in Equation (20).

(2)

Self-collision avoidance of the robotic arm

Interference and self-collision between the trajectories of the arm and the aerial platform (including both the base and the ducted fan) must be avoided. For the 4-DOF serial arm in this paper, this constraint can be realized by limiting the distance between the end effector and the geometric center of the aerial platform as

$${‖p_e^a\left(x\left(k+i\left|k\right.\right)\right)‖}_2\ge d_{e,\min }$$

(34)

where $d_{e,\min }$ is the minimum safety distance between the end effector and the platform center. Notice that the definition in Equation (34) also conforms to the form of general constraint in Equation (20).

(3)

Joint angle limits

Consider the joint angle limits, which can be expressed as state constraints, as follows:

$$q_{j,\min }\le q_j\left(k+i\left|k\right.\right)\le q_{j,\max }$$

(35)

where $q_{j,\min }$ is the minimum and $q_{j,\max }$ is the maximum angles of joint $j\left(j=1,2,3,4\right)$.

(4)

Velocity limits of the motion variables

The velocity limits of the motion variables are expressed as control constraints, including the aerial platform’s velocity and yaw rate limits, and the angular velocity limits of arm joints, which are given as

$$\left\{\begin{array}{c}{\dot{x}}_{a,\min }\le {\dot{x}}_a\left(k+i\right)\le {\dot{x}}_{a,\max }\hfill \\ {\dot{y}}_{a,\min }\le {\dot{y}}_a\left(k+i\right)\le {\dot{y}}_{a,\max }\hfill \\ {\dot{z}}_{a,\min }\le {\dot{z}}_a\left(k+i\right)\le {\dot{z}}_{a,\max }\hfill \\ {\dot{\psi }}_{a,\min }\le {\dot{\psi }}_a\left(k+i\right)\le {\dot{\psi }}_{a,\max }\hfill \\ {\dot{q}}_{j,\min }\le {\dot{q}}_j\left(k+i\right)\le {\dot{q}}_{j,\max }\hfill \end{array}\right.$$

(36)

where ${\Delta }_{\min }$ and ${\Delta }_{\max }$ refer to the minimum and maximum limits of the corresponding motion variables, respectively.

(5)

Acceleration limits of the motion variables

Similarly to the velocity limits, the acceleration limits of the motion variables can be expressed as control increment constraints, as follows:

$$\left\{\begin{array}{c}{\ddot{x}}_{a,\min }\le {\ddot{x}}_a\left(k+i\right)\le {\ddot{x}}_{a,\max }\hfill \\ {\ddot{y}}_{a,\min }\le {\ddot{y}}_a\left(k+i\right)\le {\ddot{y}}_{a,\max }\hfill \\ {\ddot{z}}_{a,\min }\le {\ddot{z}}_a\left(k+i\right)\le {\ddot{z}}_{a,\max }\hfill \\ {\ddot{\psi }}_{a,\min }\le {\ddot{\psi }}_a\left(k+i\right)\le {\ddot{\psi }}_{a,\max }\hfill \end{array}\right.$$

(37)

where ${\Delta }_{\min }$ is the minimum and ${\Delta }_{\max }$ is maximum limits of the corresponding motion variables, in which the longitudinal and lateral acceleration limits denote the corresponding pitch and roll angle limits, respectively.

4. Hierarchical Control Framework

4.1. Control Framework Description

The unified hierarchical control framework is illustrated in Figure 6, which includes four layers: (a) physical interaction control layer and (b) trajectory optimization layer, constituting the top-level layer (in green) for tracking and interaction tasks, (c) aerial platform position and attitude control layer, and (d) robotic arm joint control layer, constituting the low-level layer (in yellow) for inner loop attitude stabilization.

(1)

The physical interaction controller is for achieving contact compliance through joint action and realizing the control performance decoupling between the flight and interaction processes under unified framework. Specifically, when physical contact occurs, the equivalent references $q_{r,d}$ of the joint variables are updated according to the environmental reaction force $f_{ext}$ on the end effector.

(2)

The trajectory optimization controller is for desired target position tracking of the end effector, which has been introduced in Section 3. It obtains the independent state variable references of the platform (position and yaw angle ${\eta }_{a,r}$) and robotic arm (joint angles $q_r$) as the inputs of low-level layer.

(3)

The aerial platform controller and arm joint controller are for stabilization of the robot to ensure their motion performance under dynamics coupling, the outputs of which are set as the control inputs of the actuators.

In Figure 6, $u_a={\left[\begin{array}{cccc}{u}_{alt}\hfill & u_{rol}\hfill & u_{pit}\hfill & u_{yaw}\hfill \end{array}\right]}^{\mathrm{T}}$ denotes the altitude, latitude, longitude, and yaw control channels of the aerial platform, and $u_m={\left[\begin{array}{cccc}{u}_{m1}\hfill & u_{m2}\hfill & u_{m3}\hfill & u_{m4}\hfill \end{array}\right]}^{\mathrm{T}}$ denotes the four joint control channels of the arm. The actuators of the aerial platform consist of eight rotor motors, and the platform is controlled by its differential speed. Define the normalized motor speed vector as $u_r$, and then it can be calculated from the control inputs $u_a$ through mixing matrix $K_{mix}$ as $u_r=K_{mix}{u}_a$. Furthermore, the actuators of the arm are four joint servos, which directly correspond to $u_m$. Notice that since the dynamic characteristics of the platform and the arm joints are different, to improve the control performance and facilitate engineering practice, the controllers of the platform and the arm are designed separately in this framework.

A pseudocode for the control process under the hierarchical control framework is illustrated in Algorithm 1.

Algorithm 1. Algorithm of the hierarchical control framework.

Input: End effector desired target trajectory $p_{e,d}$, environmental interaction force $f_{ext}$, and current system state $\xi$ Output: Aerial platform control input $u_a$ and robotic arm joint control input $u_m$

1.  while true do 2.   if contact detected then 3.     $\Delta q_r$←Admittance_Controller ($f_{ext},\xi$) // Equation (40) 4.   else 5.     $\Delta q_r$←0 6.   end if 7.   $u_k^{\ast }$←NMPC_Optimization ($p_{e,d},\xi$) // Equation (15) 8.   (${\eta }_{a,r},q_r$)←Take Optimal Independent State Variable from $u_k^{\ast }$ 9.   $q_{r,d}$←$q_r+\Delta q_r$ 10.   $u_a$←AerialPlatform_Motion_Controller (${\eta }_{a,r}$) // Section 4.2.1 11.   $u_m$←RoboticArm_Motion_Controller ($q_{r,d}$) // Section 4.2.2 12.   Apply $u_a,u_m$ to the actuators of the robot 13.  end while

4.2. Motion Control

4.2.1. Aerial Platform Control

Due to the ducted fan’s complex aerodynamic characteristics, it is difficult to directly design the aerial platform controller based on the platform’s nonlinear dynamics model to guarantee the control performance. Considering that the robot does not perform aerobatics and the attitude angles always conform to the small-angle assumption, linearize the dynamics model of the aerial platform in Equation (16) at the hovering equilibrium point, and the control-oriented multi-input multi-output (MIMO) state space equation can be formulated as

$$\left\{\begin{array}{c}{\dot{x}}_a\left(t\right)=A_a{x}_a\left(t\right)+B_a{u}_a\left(t\right)+f_m\left(t\right)\hfill \\ y_a\left(t\right)=C_a{x}_a\left(t\right)\hfill \end{array}\right.$$

(38)

where $x_a$ is the full state vector of the aerial platform composed of $v_a^a$, ${\omega }_a^a$, and ${\Phi }_a$, which have clear physical meanings and are all measurable. $y_a$ is the output vector composed of $v_a^a$ and ${\psi }_a$, $f_m$ is the disturbance vector from robotic arm, $A_a$, $B_a$, and $C_a$ refer to the state matrix, control matrix, and output matrix, respectively. In our previous work [36], a robust-adaptive composite controller is proposed for the system described in Equation (38). The aerial platform controller in this section is designed in the same way, and it consists of three parts: (1) A static feedback controller based on robust $H_{\infty }$ synthesis is designed as the basic controller to ensure the closed-loop system’s MIMO decoupling and robust stabilization. (2) Based on the basic controller, an auxiliary adaptive loop is designed to estimate and compensate the disturbances $f_m$ acting on platform from arm. Specifically, the adaptive loop is organized as follows: First, design a state predictor for state prediction; then design an adaptive law for disturbance estimation on the base of prediction error; finally, to compensate the disturbances $f_m$ whilst tracking the desired references $y_{a,r}$, the control law is reconstructed. (3) In the outer loop of the robust-adaptive controller, design a series of PD controllers for platform references ${\eta }_{a,r}$ trajectory tracking. The design process and stability analysis for the composite controller will not be detailed here, and can be referred to our previous work [36].

4.2.2. Robotic Arm Control

Different from the fixed base of conventional industrial robot arms, the aerial platform of the flying robot is a floating base, and its real-time dynamic changes must be considered in the arm controller. On the other hand, due to the large angular motion range and strong coupling of each arm joint, and the arm controller cannot be designed by an approximate linearization model near a specific equilibrium point as the aerial platform controller. A robotic arm feedforward–feedback composite controller considering platform coupling compensation is proposed, which consists of a nonlinear feedforward loop and a linear feedback loop. The purpose of the feedforward loop is to compensate the nonlinear coupling and eliminate the steady-state errors whilst improving the system’s response speed, and that of the feedback loop is to ensure the joint angles closed-loop tracking. The proposed controller decouples the nonlinear inverse dynamics computation from the joint feedback control to reduce the onboard computational burden.

The dynamics model of the arm can be derived from Equation (16) as

$$M_m\left(\xi \left(t\right)\right)\ddot{q}\left(t\right)+C_m\left(\xi \left(t\right),\dot{\xi }\left(t\right)\right)\dot{q}\left(t\right)+g_m\left(\xi \left(t\right)\right)+M_a\left(\xi \left(t\right)\right)\ddot{\zeta }\left(t\right)=\tau \left(t\right)$$

(39)

where $M_m\left(\xi \right)$, $C_m\left(\xi ,\dot{\xi }\right)$, and $g_m\left(\xi \right)$ are the submatrices of $M\left(\xi \right)$, $C\left(\xi ,\dot{\xi }\right)$, and $g\left(\xi \right)$ respectively, and $M_a\left(\xi \right)\ddot{\zeta }$ denotes the dynamics coupling term of the aerial platform.

Based on Equation (39), the feedforward loop is designed as

$$u_{ff}\left(t\right)=M_m\left(\xi \left(t\right)\right){\ddot{q}}_r\left(t\right)+C_m\left(\xi \left(t\right),\dot{\xi }\left(t\right)\right){\dot{q}}_r\left(t\right)+g_m\left(\xi \left(t\right)\right)+M_a\left(\xi \left(t\right)\right)\ddot{\zeta }\left(t\right)$$

(40)

where ${\dot{q}}_r$ is the angular velocity desired reference input, and ${\ddot{q}}_r$ is desired reference input of the acceleration of the joints. $\xi$ and $\dot{\xi }$ are the robot’s real-time feedback states, including both the arm joint states and the aerial platform states, and $\ddot{\zeta }$ is the acceleration states of the aerial platform, all of which can be directly obtained from the onboard sensor module.

On this basis, the feedback loop can be simply implemented by PD controller as

$$u_{fb}\left(t\right)=K_P\left(q_r\left(t\right)-q\left(t\right)\right)+K_D\left({\dot{q}}_r\left(t\right)-\dot{q}\left(t\right)\right)$$

(41)

where $K_P$ and $K_D$ are the corresponding control parameters.

Combining Equations (40) and (41), the robotic arm controller finally can be synthesized as

$$u_m\left(t\right)=u_{ff}\left(t\right)+u_{fb}\left(t\right)$$

(42)

Since the designed controller does not require complex inverse dynamics computation at each control step, it can perform feedback control with high frequency. Notice that the controller input contains the desired joint angle $q_r$, the joint angular velocity ${\dot{q}}_r$, and the joint angular acceleration ${\ddot{q}}_r$ to ensure the motion trajectory’s smoothness. For the flying robot in this paper, $q_r$ and ${\dot{q}}_r$ come from the trajectory optimization controller, while ${\ddot{q}}_r$ is calculated from the difference of ${\dot{q}}_r$.

4.2.3. Interaction Control

The above motion controller has good tracking and disturbance rejection performance, which makes the closed-loop system exhibit large stiffness toward the external environment. Consequently, it cannot show compliance when subjected to interaction forces. In order to decouple the contact performance from motion performance, an admittance controller is constructed at the top-level layer based on the proposed hierarchical framework, which indirectly performs contact control through Cartesian position control, as illustrated in Figure 6. The proposed control framework achieves compliant contact without losing tracking efficiency and accuracy of the end effector, while avoiding contact state detection and control mode switching in position/force hybrid controller.

Since the relationship between force and position of a system can be described by its admittance characteristics, the ideal contact dynamics between end effector and environment are expressed as a virtual mass–spring–damping system. The ideal admittance contact model can be denoted as

$$M_d\left({\ddot{p}}_{e,c}\left(t\right)-{\ddot{p}}_{e,d}\left(t\right)\right)+B_d\left({\dot{p}}_{e,c}\left(t\right)-{\dot{p}}_{e,d}\left(t\right)\right)+K_d\left(p_{e,c}\left(t\right)-p_{e,d}\left(t\right)\right)=f_{ext}\left(t\right)$$

(43)

where $p_{e,d}$ is the end effector’s desired trajectory, $p_{e,c}$ is the end effector’s corresponding compliance trajectory, and $\Delta p_e=p_{e,c}-p_{e,d}$ is the position deviation. $M_d$, $B_d$, and $K_d$ are the ideal contact model’s diagonal positive-definite inertia matrix, damping matrix, and stiffness matrix, respectively, which can be adjusted to change the response characteristics between contact force and motion.

Based on the ideal admittance contact model, the following controller can be designed: First, the force sensor on end effector measures the real-time contact force $f_{ext}$, and then it is converted into the desired position deviation $\Delta p_e$ via the contact model by adjusting the equilibrium point of the virtual mass–spring–damping system. Next, considering the high motion efficiency and accuracy of the arm joint compared with the aerial platform, $\Delta p_e$ is converted into the desired joint angle deviation $\Delta q_r$ through the Jacobian pseudo inverse matrix $J_{tm,e}^{†}\left({\Phi }_a,q\right)$, which is superposed with the original joint reference $q_r$ to obtain the new reference input $q_{r,d}=q_r+\Delta q_r$ to the arm joint controller. Finally, by driving the joint to track $q_{r,d}$, the compliant response of the system to external force is indirectly implemented through motion control.

This paper focuses on the vertical wall target. Thus, the end effector is only subjected to the force in ${\mathrm{X}}_{\mathrm{I}}$ direction, which can be denoted as $f_{ext,x}$. Let $\Delta x_e$ be the component of $\Delta p_e$ along ${\mathrm{X}}_{\mathrm{I}}$ axis, and $m_d$, $b_d$, and $k_d$ be the components corresponding to $\Delta x_e$ of matrices $M_d$, $B_d$, and $K_d$, and then Equation (43) can be simplified as

$$m_d\Delta {\ddot{x}}_e\left(t\right)+b_d\Delta {\dot{x}}_e\left(t\right)+k_d\Delta x_e\left(t\right)=f_{ext,x}\left(t\right)$$

(44)

Applying Laplace transform to Equation (44), the admittance contact controller is designed as

$$\left\{\begin{array}{c}\Delta x_e\left(s\right)={\displaystyle \frac{1}{m_d{s}^2+b_{d}s+k_d}}{f}_{ext,x}\left(s\right)\hfill \\ \Delta q_r\left(t\right)=J_{tm,ex}^{†}\left({\Phi }_a\left(t\right),q\left(t\right)\right)\Delta x_e\left(t\right)\hfill \end{array}\right.$$

(45)

where $J_{tm,ex}^{†}\left({\Phi }_a,q\right)$ is the component corresponding to $\Delta x_e$ of $J_{tm,e}^{†}\left({\Phi }_a,q\right)$. The parameters of the controller can be selected according to the desired overdamping behavior. Notice that to avoid the force sensor noise influence on the controller, the measured contact force is filtered by a low-pass filter $D\left(s\right)$ before being fed to the controller.

Since the system’s low-level layer is closed-loop-stable, the contact controller’s stability can be guaranteed as long as the equivalent bandwidth of the closed-loop motion system is greater than that of the contact controller. When the end effector is not in contact with the environment, the proposed controller has no effect ($\Delta q_r=0$) and the end effector tracks $p_{e,d}$ in free space. When contact occurs, the controller quickly adjusts the closed-loop motion system reference to make the end effector compliant to absorb impact and achieve stable contact.

5. Simulation Analyses

Simulation analyses are performed for position tracking and physical contact scenarios, respectively, to verify the effectiveness of the proposed trajectory optimization controller and the admittance contact controller based on the hierarchical framework.

5.1. Position Tracking Scenario

In this scenario, set the desired target position 0.5 m ahead of the flying robot. The end effector responses are shown in Figure 7. The corresponding trajectory references of the motion state variables (the aerial platform’s position and yaw angle and robotic arm’s joint angles) under the proposed trajectory optimization controller and the corresponding tracking responses under the low-level motion controller are illustrated in Figure 8. For comparison, Figure 7 also gives the responses under the CLIK controller and the two-stage controller, which are commonly adopted in studies, as discussed in Section 1. Similarly, Figure 9 and Figure 10 show the trajectory references and responses of the motion state variables with the CLIK controller and the two-stage controller, respectively.

The results indicate that the proposed trajectory optimization controller can accurately track the position of the end effector. The trajectory is smooth without overshoot and the tracking time is about 6 s. Although the attitude change of the aerial platform in forward flight will cause $z_e$ of the end effector to deviate from expectation, it can be adjusted immediately through the unified trajectory prediction and optimization of the aerial platform and arm joints. It can be seen from Figure 8 that the platform and arm joints actuate simultaneously in the optimized motion trajectory, where the aerial platform dominates to improve the tracking efficiency, while the robotic arm also undertakes part of motion (joint 2 and joint 3 change by about 17° and 15°, respectively) to prevent the ducted fans being too close to the target, which is consistent with the analysis in Section 3. Furthermore, since the arm has revolute joints, its motion in $x_e$ direction will cause coupling motion in $z_e$ direction. The proposed NMPC-based strategy can effectively offset the altitude error of the end effector caused by the forward extension of the arm joint by moving the aerial platform along $z_a$ direction. Overall, the above analysis shows that the proposed trajectory optimization controller can carry out coordinated planning for the motion of the platform and the arm, balancing various constraints such as the ducted fan wall effect and the arm’s CoM change to perform smooth and accurate tracking of the end effector.

As a comparison, although the CLIK controller’s response time is faster than the time of the NMPC-based controller, it suffers from severe overshoot and oscillation, with a maximum deviation error of 40%. This is because the tracking of motion state variables in the low-level layer of the system requires response time, and the CLIK controller cannot predict the future motion states in the manner of the NMPC-based controller, resulting in an excessive response. Such a response may cause accidental collision of the end effector with the target, which is unacceptable. In addition, due the end effector’s motion coupling in $x_e$ and $z_e$, the altitude response under the CLIK controller also oscillates to some extent. Figure 9 shows that the motion of the end effector is almost entirely provided by the aerial platform, and the steady-state variation of the four joint angles of the arm is small, and it only participates in the adjustment during the early movement of the end effector. The reason why the DOFs of the platform exhibit higher motion priority is that the CLIK strategy does not consider the differences between the motion DOFs of the platform and the arm in the trajectory allocation process. The DOFs of the aerial platform can directly generate linear motion, while the DOFs of the arm joints can only generate angular motion, whose equivalent mapping to the end effector’s position is smaller. Moreover, the CLIK strategy does not take into account constraints such as ducted fan wall effect and safety distance limitations. For targets like vertical walls, the transient deviation shown in Figure 7 may have already led to direct collision and damage to the ducted fans in practice.

The two-stage controller performs trajectory planning for the platform and the arm independently: in first stage, the platform flies forward by 0.45 m and then keeps hovering; in second stage, the arm extends by 0.05 m to reach the target. The results indicate that the trajectory of the end effector is not smooth with this controller. When the robotic arm moves, the hovering aerial platform drifts under its disturbance, resulting in a slight overshoot of the end effector. Although the platform can quickly return to its equilibrium position with the composite controller, this situation should still be avoided. In addition, the two-stage controller performs open-loop independent planning for the aerial platform and the robotic arm, and the arm joints cannot compensate for the response deviation of the end effector in real time caused by the platform’s attitude change, which results in a large error in $z_e$. Moreover, in practice, it is difficult for the above offline-based strategy to effectively deal with the time-varying target. Furthermore, Figure 8, Figure 9 and Figure 10 show that although there are significant differences in the solved motion state variable references under different trajectory planning controllers, all responses can achieve good tracking performance, which illustrates the effectiveness of the platform’s low-level motion controllers and the robotic arm in Section 4.

5.2. Physical Contact Scenario

In this scenario, set a vertical wall 0.5 m ahead of the flying robot and let the robot move forward to contact the wall; the contact force and position responses are shown in Figure 11 and Figure 12. To ensure continuous contact of the end effector with the wall, the desired target position is set 2 cm behind the wall. For comparison, the responses without the admittance controller are also illustrated. Accordingly, Figure 13 and Figure 14 show the responses of the motion state variables of the robot with and without the admittance controller, respectively. The white parts in these figures indicate that the end effector has not made contact with the wall, and the blue parts indicate that it has made physical contact.

The results show that the proposed admittance controller based on the hierarchical framework can perform stable and compliant contact of the end effector, while maintaining the desired contact position during contact process. The instantaneous contact force between vertical wall and end effector is about 1.5 N, which then decays rapidly and reaches a stable equilibrium around 0.25 N. As a comparison, the contact process without an admittance controller lacks compliance, and the contact force oscillates continuously with a peak of 2.2 N. Meanwhile, as Figure 14 shows, the responses of the motion state variables of both the aerial platform and the robotic arm deviate from the references and oscillate due to the environmental reaction force. In particular, the joint angles are completely unable to track the references, with a maximum error exceeding 13°, resulting in the end effector deviating from the contact position desired. The maximum response error of $z_e$ is close to 0.14 m. The results show the necessity and effectiveness of the admittance controller, as well as the corresponding hierarchical control framework.

6. Experimental Verifications

6.1. Experimental Setup

The actual experimental test scenarios of position tracking and physical contact of the flying robot are illustrated in Figure 15. The OptiTrack motion capture system is set up for three-dimensional positioning of the robot. A flat acrylic plate fixed on an industrial wire mesh fence is adopted as a vertical wall, and its transparency feature can avoid blocking of the cameras of the motion capture system. Figure 16 demonstrates the hardware system of the robot. A ground station is applied to transmit the real-time three-dimensional positioning information of the robot to the onboard main controller, and to remotely communicate and control the robot. The onboard system consists of onboard main controller, aerial platform control system, robotic arm control system, and force sensor system. The onboard main controller is Raspberry Pi 3, which has both low cost and good onboard computing performance. The flight controller of the aerial platform is CUAV V5, integrating high-precision IMU, as well as magnetometer and barometer modules. A ROBOTIS Open CR embedded board is used as the robotic arm controller to drive the joint servos. The servos are Dynamixel XM430-W350, which integrates ARM-based microprocessor to provide various feedback signals, including angle position, velocity, and current (torque), and supports torque control mode based on a high-performance current sensing algorithm. An ATI Nano17 lightweight force sensor is attached to the end effector to measure and feed back the physical contact forces, and the measured signals are amplified, solved, and calibrated by an ATI NETCANOEM board before being sent to the onboard main controller.

The parameters of the proposed trajectory optimization controller are given in Table 3, and the corresponding system constraints are given in Table 4. Notice that the weight of the cost function $h_3$ is larger than those of the others, which is because this $h_3$ is denoted by the dimensionless normalized tilting moment coefficient and is not in the same order of magnitude as other cost functions. The admittance contact controller parameters are given in Table 5.

6.2. Experimental Results

6.2.1. Position Tracking Scenario

To verify the effectiveness of the trajectory optimization controller, a four-point position tracking experiment for the flying robot is performed, and the end effector’s responses are shown in Figure 17. Figure 18 provides the corresponding three-dimensional trajectory to illustrate the responses more intuitively. The results under the CLIK controller are also given for comparison. The parameters of the CLIK controller are tuned following the method in [37], which is a widely used criterion. As analyzed in Section 5, the two-stage controller finds it difficult to deal with the time-varying position tracking and is therefore not implemented in actual experiments. Since the DOFs of the arm joints are primarily within the ${\mathrm{X}}_{\mathrm{I}}{\mathrm{Z}}_{\mathrm{I}}$ plane, trajectory optimization is primarily reflected in the longitudinal and vertical directions, which should be the mainly focus, while the response accuracy of the robot in the lateral direction is primarily determined by the low-level motion controller. The maximum fluctuations of the end effector under both the CLIK controller and the proposed NMPC controller during the position tracking process are illustrated in Table 6. The results show that the proposed NMPC controller has good tracking performance with smoothness and no overshoot, and the maximum fluctuations of the trajectory in the longitudinal and vertical directions are 0.0391 m and 0.0293 m, respectively. The tracking performance of the CLIK controller is relatively poor, especially in the longitudinal direction, where there is a severe overshoot, with a deviation of about 0.1 m and a maximum fluctuation error of 0.1596 m. An overshoot also occurs in the vertical direction under the CLIK controller, with a maximum fluctuation reaching 0.0685 m. Compared to the CLIK controller, the maximum fluctuations with the proposed controller are reduced by 75.5% and 57.2% in the longitudinal and vertical directions, respectively. Figure 17 also shows that the proposed controller and the CLIK controller exhibits similar values performance in the lateral direction, which is consistent with the above analysis.

Table 3. Trajectory optimization controller parameters.

Parm	Value	Parm	Value
$N_p$	10	$N_m$	2
$W_1$	diag (4, 4, 4)	$W_{s1}$	diag (20, 20, 20)
$W_2$	diag (2, 2)	$W_{s2}$	diag (2, 2)
$W_3$	10,000	$W_{s3}$	10,000
$W_4$	diag (1, 1, 1, 1, 1, 1, 1, 1)	$T_s$	100 ms

Table 3. Trajectory optimization controller parameters.

Parm	Value	Parm	Value
$N_p$	10	$N_m$	2
$W_1$	diag (4, 4, 4)	$W_{s1}$	diag (20, 20, 20)
$W_2$	diag (2, 2)	$W_{s2}$	diag (2, 2)
$W_3$	10,000	$W_{s3}$	10,000
$W_4$	diag (1, 1, 1, 1, 1, 1, 1, 1)	$T_s$	100 ms

Table 4. System constraints of trajectory optimization.

Constraint	Value	Constraint	Value
$d_{WE,\min }$	0.215 m	$d_{e,\min }$	0.445 m
$\left(q_{1,\min },q_{1,\max }\right)$	(−pi/3, pi/3) rad	$\left(q_{2,\min },q_{2,\max }\right)$	(0, pi) rad
$\left(q_{3,\min },q_{3,\max }\right)$	(−5 pi/6, −pi/6) rad	$\left(q_{4,\min },q_{4,\max }\right)$	(−pi/2, pi/2) rad
$\left({\dot{x}}_{a,\min },{\dot{x}}_{a,\max }\right)$	(−1.5, 1.5) m/s	$\left({\dot{y}}_{a,\min },{\dot{y}}_{a,\max }\right)$	(−1.5, 1.5) m/s
$\left({\dot{z}}_{a,\min },{\dot{z}}_{a,\max }\right)$	(−1.5, 1.5) m/s	$\left({\dot{\psi }}_{a,\min },{\dot{\psi }}_{a,\max }\right)$	(−1.57, 1.57) rad/s
$\left({\dot{q}}_{j,\min },{\dot{q}}_{j,\max }\right)$	(−4.8, 4.8) rad/s	$\left({\ddot{x}}_{a,\min },{\ddot{x}}_{a,\max }\right)$	(−5.6, 5.6) m/s2
$\left({\ddot{y}}_{a,\min },{\ddot{y}}_{a,\max }\right)$	(−5.6, 5.6) m/s2	$\left({\ddot{z}}_{a,\min },{\ddot{z}}_{a,\max }\right)$	(−6.0, 4.0) m/s2
$\left({\ddot{\psi }}_{a,\min },{\ddot{\psi }}_{a,\max }\right)$	(−2.4, 2.4) rad/s2

Table 4. System constraints of trajectory optimization.

Constraint	Value	Constraint	Value
$d_{WE,\min }$	0.215 m	$d_{e,\min }$	0.445 m
$\left(q_{1,\min },q_{1,\max }\right)$	(−pi/3, pi/3) rad	$\left(q_{2,\min },q_{2,\max }\right)$	(0, pi) rad
$\left(q_{3,\min },q_{3,\max }\right)$	(−5 pi/6, −pi/6) rad	$\left(q_{4,\min },q_{4,\max }\right)$	(−pi/2, pi/2) rad
$\left({\dot{x}}_{a,\min },{\dot{x}}_{a,\max }\right)$	(−1.5, 1.5) m/s	$\left({\dot{y}}_{a,\min },{\dot{y}}_{a,\max }\right)$	(−1.5, 1.5) m/s
$\left({\dot{z}}_{a,\min },{\dot{z}}_{a,\max }\right)$	(−1.5, 1.5) m/s	$\left({\dot{\psi }}_{a,\min },{\dot{\psi }}_{a,\max }\right)$	(−1.57, 1.57) rad/s
$\left({\dot{q}}_{j,\min },{\dot{q}}_{j,\max }\right)$	(−4.8, 4.8) rad/s	$\left({\ddot{x}}_{a,\min },{\ddot{x}}_{a,\max }\right)$	(−5.6, 5.6) m/s2
$\left({\ddot{y}}_{a,\min },{\ddot{y}}_{a,\max }\right)$	(−5.6, 5.6) m/s2	$\left({\ddot{z}}_{a,\min },{\ddot{z}}_{a,\max }\right)$	(−6.0, 4.0) m/s2
$\left({\ddot{\psi }}_{a,\min },{\ddot{\psi }}_{a,\max }\right)$	(−2.4, 2.4) rad/s2

Table 5. Admittance contact controller parameters.

Parm	Value	Parm	Value	Parm	Value	Parm	Value
$m_d$	1	$b_d$	30	$k_d$	1	$D\left(s\right)$	$1/\left(0.05s+1\right)$

Table 5. Admittance contact controller parameters.

Parm	Value	Parm	Value	Parm	Value	Parm	Value
$m_d$	1	$b_d$	30	$k_d$	1	$D\left(s\right)$	$1/\left(0.05s+1\right)$

Table 6. Maximum fluctuations in position tracking scenario.

Scenario	Controller	Longitudinal Direction	Vertical Direction
Without wind	CLIK	0.1596 m	0.0685 m
	NMPC	0.0391 m	0.0293 m
With wind	CLIK	0.2692 m	0.0958 m
	NMPC	0.0781 m	0.0351 m

Table 6. Maximum fluctuations in position tracking scenario.

Scenario	Controller	Longitudinal Direction	Vertical Direction
Without wind	CLIK	0.1596 m	0.0685 m
	NMPC	0.0391 m	0.0293 m
With wind	CLIK	0.2692 m	0.0958 m
	NMPC	0.0781 m	0.0351 m

To further evaluate the effectiveness of the proposed controller in realistic environments, a position tracking experimental test under wind gusts was carried out. The responses of the end effector are shown in Figure 19, and the corresponding three-dimensional trajectory is illustrated in Figure 20. During the test, a fan with a center maximum wind speed of 6 m/s is set in front of the flying robot along the ${\mathrm{X}}_{\mathrm{I}}$ direction. The results with the CLIK controller are also provided, and the maximum fluctuations of the end effector are presented in Table 6. The results show that, compared to the situation without wind gusts, the trajectories of the end effector with both the NMPC controller and the CLIK controller exhibit deterioration. The maximum fluctuations with the NMPC controller in the longitudinal and vertical directions are 0.0781 m and 0.0351 m, respectively. However, the controller can still maintain the ability to track and hold the desired position. Correspondingly, the trajectory with the CLIK controller experiences severe oscillations, especially in the longitudinal direction, with a maximum fluctuation of 0.2692 m. This is due to the wind field being mainly concentrated in the ${\mathrm{X}}_{\mathrm{I}}$ direction. In this situation, the CLIK controller cannot meet the task requirements. Compared to the CLIK controller, the maximum fluctuations with the proposed controller are reduced by 71.0% and 63.4% in the longitudinal and vertical directions, respectively. Moreover, Figure 21 illustrates the computation time of the proposed algorithm at each sampling instant during the test. It can be seen that the average computation time of the algorithm is 0.0426 s, while the maximum and minimum computation times are 0.0911 s and 0.0218 s, respectively, which can meet the real-time requirements under hardware performance constraints.

6.2.2. Physical Contact Scenario

A flying robot physical contact experiment is performed to verify effectiveness of the admittance controller with the hierarchical control framework, and the contact force and corresponding trajectory response of the end effector are shown in Figure 22, where the white part indicates that the end effector has not made contact with the wall and the blue part indicates that it has made physical contact with the wall. To ensure safety, only the condition with the admittance controller is carried out in the experimental tests. The results show that the admittance controller designed in this paper can perform stable and compliant contact. The instantaneous contact force between the wall and the end effector is about 2 N, which then decays rapidly and oscillates stably in a small range around 1 N. It should be mentioned that, compared with the simulation results, the reason why the contact force does not converge to a constant value is mainly due to the inevitable floating error of the aerial platform in the actual experiments.

7. Conclusions

This paper focuses on the tracking and interaction control of a ducted fan flying robot. First, a ducted fan flying robot with a 4-DOF robotic arm is designed, and the nonlinear kinematic and dynamics models of the robot are established. Second, a NMPC-based trajectory optimization method is proposed, which solves the optimal trajectory allocation of the robot’s redundant motion state variables to achieve the desired three-dimensional position tracking of the end-effector, while satisfying time-varying motion and performance constraints. Third, a unified hierarchical control framework for the robot is proposed to avoid controller switching and performance coupling between the flight and interaction processes, in which the top-level admittance contact controller and the low-level motion controller are designed in detail. Both simulation analyses and actual experimental tests were performed to verify the effectiveness of the controller. The results indicate that the end effector of the robot can track the position smoothly and accurately, with the maximum fluctuation with the proposed method reduced by up to 75.5% without wind and 71.0% with wind, compared to the CLIK method. Moreover, the end effector can also achieve continuous stable contact with the vertical wall target, which lays the foundation for the flying robot practical application. Future work will concentrate on contact force control strategies for the end effector to meet actual industrial task requirements. Furthermore the aerodynamic coupling effects of the flying robot under dynamic motion conditions and complex environments will also be explored to enhance the robots’ applicability to various scenarios. On this basis, more challenging scenarios such as interacting with curved or angled surfaces will be investigated.