I Introduction
Physical interaction with a surrounding environment has been a research topic of growing interest in aerial robotics. [1] Most studies have been carried out with a hardware platform called unmanned aerial manipulator (UAM), an unmanned aerial vehicle attached with one or more manipulators, with which various reallife applications are investigated such as maintenance tasks [2] and autonomous sensor installation and retrieval operation [3]. One compelling employment could be an interaction with a moving structure in the surroundings. Although most of the latest works on UAM physical interaction [4, 2, 5] have focused on an inspection on a static structure, further expansion of the capability of an aerial robot cannot be overlooked. If we can facilitate a UAM to move any movable surrounding structure, a more exhaustive exploration can be performed by accessing once unreachable places, pushing or pulling a movable structure; also, a more active response to disaster or rescue operation can be achieved with its augmented versatility.
There are at least two additional issues in addressing a movable structure compared to an interaction with a static counterpart: 1) structure dynamics, and 2) collision avoidance. Contrary to a static structure, a movable structure contains its own dynamics which often entails significant force/torque reaction. Without proper modeling or estimation on the movement of the structure, it cannot be guided to the desired position; furthermore, collision avoidance with this dynamic structure cannot be guaranteed which is necessary for safe operation.
As part of such topics, this paper handles the problem of a UAM opening a hinged door. It deals with a particular application of a multirotorbased UAM operating a movable structure like doors, windows, and heavy cargo. This problem requires additional consideration on the movement of the structure which would involve intrinsic constraints, which is, in our case, a hinge constraint. Moreover, crash prevention with a dynamic structure, the door, and a static structure, a doorframe, is considered along with a selfcollision avoidance in generating a collisionfree trajectory. After modeling the combined dynamics of the UAM and the hinged door, we applied model predictive control (MPC) to ensure dynamic feasibility and collision avoidance of a generated trajectory. State constraints which are formulated from kinematic relationships are imposed on the optimal control problem, and the generated trajectory from MPC is tracked by a disturbance observerbased robust controller designed in [6].
Ia Related Works
In [2], tool handling aerial manipulator is suggested while in contact with a vertical surface. For a contactbased inspection task, a motion planning algorithm presented in [7] is validated with experiments using a multidirectional thrust aerial vehicle [8]. An omnidirectional aerial vehicle for contactbased inspection on a curved surface is presented in [5] with state estimation and disturbance rejection. However, none of these considers a moving structure, and since the planner suggested in [7] is based on a samplingbased method, it cannot be applied for an online scenario.
There exist some studies about an aerial manipulator coping with a movable structure. In [9], a dualarmed aerial manipulator is employed to perform three different tasks, and the one about a movable structure is a valveturning task. However, in this scenario, only a rotational motion in the zaxis is required for the task, and certainly, more issues have to be considered for less constrained tasks. In [10], the author presents an experiment of an aerial manipulator operating an unknown drawer. Coupled dynamics between the UAM and the drawer are derived, and the desired force is computed through the velocity of the drawer. However, a structure with only a translational motion is considered, and collision avoidance is not explicitly addressed. In [11], an aerial vehicle opens a hinged door with a proposed mechanism. Though suitable for this particular purpose, versatility that a general UAM contains seems to lack in this approach.
MPC has been utilized not only as an optimal controller for aerial vehicles but also as an optimal planner. In [12], an explicit MPC is used to generate offline control policies for an aerial manipulator interacting with a static structure. Later, in [13], MPC with SLQ is presented for an online optimal control input computation. Similar techniques are applied in many other papers to produce an online optimal trajectory for a multirotor with a suspended load [14], and for a multirotor with a network delay [15]. In this paper, MPC is utilized as a suboptimal planner generating a trajectory complying with dynamics and constraints.
IB Contributions
To the best of our knowledge, this paper presents the first attempt for a UAM to open a hinged door while avoiding collisions. Simplified dynamics and state constraints are formulated to construct an MPC problem, and by adopting a constrained version of differential dynamic programming, dynamically feasible and onlineapplicable trajectories satisfying constraints are generated. Finally, our proposed approach is validated through real experiments.
IC Outline
This paper is structured as follows. In Section II, problem description with general assumptions throughout the paper are explained. Kinematics, dynamics and simplified dynamics are all derived in Section III. In Section IV, problem formulation for MPC with various state constraints guaranteeing collisionfree trajectories are presented. Control framework, experimental setup and results are explained in Section V, while conclusions are drawn in Section VI.
Ii Problem Description
This paper considers the problem of a UAM opening a hinged door. Unlike interaction with a static structure, the problem of interacting with a movable structure requires deliberation on the movement of the structure as well. Furthermore, to assure safe operation, constraints on collision avoidance should be reflected. Such collision avoiding constraints in this scenario would involve avoiding collision with a door, a doorframe, and itself.
To satisfy the need for both being aware of the structure’s movement and generating a constraintabiding trajectory, MPC is applied. An integrated model of the coupled dynamics between the UAM and the hinged door is developed, and three independent constraints are applied to the optimal control problem.
Following assumptions are made throughout this paper:

The weight of the robotic arm attached to the bottom of the multirotor is negligible compared to that of the whole system.

Servomotors in the robotic arm have negligible dynamics and are assumed to follow the desired velocity command with little time delay.

The endeffector’s tip and the door’s surface are rigidly connected.

Physical properties of the door (i.e., mass moment of inertia, width, height, and position) are known.
With the followed assumptions, equations of motion and constraints for the system are developed.
Iii Equations of Motion
In this section, equations of motion (EoM) of the whole system including the multirotorbased UAM and the hinged door are derived. A general dynamical model of the system is first introduced by Lagrange equation, and a simplified dynamics is then suggested for real application.
Iiia Notations
In order to clarify further derivations, basic notations for variables and parameters that describe the system are predefined. Other additional variables and parameters adopted for notational simplicity are defined across the paper.
The position of the UAM , the lower part of the door hinge , and the endeffector are all expressed in the inertial world frame centered at . The attitude of the aerial vehicle and the angle of the door are each denoted by ZYX Euler angle and in . For the rotational velocity of the aerial vehicle in the vehicle’s body frame centered at , is employed. is positioned at the center of mass (CoM) of the UAM. , where , describes each servomotor angle composing the multirotorattached robotic arm in , and they are concatenated as . Conventionally, denotes an elementwise time derivative of an arbitrary matrix . Among these variables, state variables that describe the coupled EoM of the UAM and the hinged door are defined as , while inputs to the system are defined as where , , and each denotes a thrust, torque of the multirotor, and a desired servomotor speed all in .
Parameters for the UAM are , which are mass, mass moment of inertia at , and robotic arm’s linkage length, respectively. For the parameters of the door, mass moment of inertia at , the shortest distance from to the door hinge
, the projected distance of the relative position vector between
and to the door hinge , and the door width and height are used. Additionally, is used to describe the radius of the multirotor including blades in the XY plane of the frame .IiiB Kinematics
To derive a unified dynamical model of the UAM and the door while assuring that constraint forces do not appear, Lagrange dynamics are employed. Required kinematic relations for the derivation are listed in this subsection.
With a rotation matrix from the frame to and a position vector of the endeffector from described in , the kinematic constraint between the UAM and the door can be written as the following:
(1) 
Defining a new configuration vector , Jacobian matrices mapping to , , and can be written as , , and . Note that is chosen to describe the dynamic behavior of the multirotor and the door independent from the motion of the robotic arm, based on the assumption of negligible servomotor dynamics.
IiiC Dynamics
To apply Lagrange equation about and , kinetic and potential energies are calculated as follows:
(2)  
where , and . By calculating the Lagrange equation with external forces , the EoM of the multirotor and the door can be drawn analytically from the below equation:
(3) 
The entire system’s EoM from the equation (3) combined with the motions of the servomotor is as follows:
(4) 
where , , and are mass, Coriolis, and gravitational matrices induced from the equation (3), and with denoting unmodeled forces and torques from the manipulator and the door. Detailed derivation can be found in [16] where a sufficiently slow servomotor speed during the entire horizon is assumed. Since servomotors are speedcontrolled, with a proper control strategy assuring input limits, the assumption can be satisfied.
IiiD Simplified Dynamics
A simplified dynamical model for the whole system is derived in this subsection. Necessity for the simplified dynamics can be summarized as follows. Since our approach to handle the door opening scenario with a UAM includes an optimal planner in a receding horizon manner, a more concise model though including all sufficient information of the system dynamics would be more advantageous in terms of computation time. With some additional assumptions, the number of system states can be reduced, and numerical matrix inversion is no longer required which is mandatory in the equation (4).
The simplified version is derived with the following two assumptions:

Multirotor’s rotational dynamics is negligible.

UAM is in a quasistatic state in translational motion during its operation.
The first assumption can be justified from the fact that a multirotor’s low rotational inertia and its ability to generate high torque ensure that the onboard controller is able to control the angular velocity of the vehicle sufficiently fast. Several other papers [17, 18] include this assumption in their dynamical model for multirotor planning. Secondly, considering the door’s relatively higher inertia compared to that of the UAM, the second assumption can be assumed without losing generality. Similar research carried out for operating a drawer with a UAM in [10] also assumed this quasistatic motion to generate the desired path. Thanks to the first assumption, we can redefine the states and inputs for the system as , and , where denotes a desired angular velocity of the UAM in . Furthermore, according to the second assumption, introducing an actionreaction force between the UAM and the hinged door, can be written as where is a gravitational acceleration. Then, the nominal dynamics of the hinged door can be represented as where stands for a unit normal vector to the door surface pointing to the opposite direction to the UAM. Therefore, the door’s angular acceleration can be rearranged as , and the whole system dynamics with the redefined states and inputs are as follows:
(5) 
where is a mapping matrix such that .
Iv Trajectory Generation Using
Model Predictive Control
This section contains ingredients and recipes for a model predictive control problem that produces a desired trajectory to the robust controller. Starting with a problem formulation, hard constraints for avoiding a collision, and an optimal control solver yielding a suboptimal solution in less than a second, thus enabling a realtime application are presented sequentially. By solving an optimal control problem at every time interval, our MPC algorithm could generate a dynamically feasible and safe trajectory applicable to a realworld experiment.
Iva Problem Formulation
Considering a discretization over time with a time interval , for the given time horizon , an initial state , and an initial input sequence at the time element, the MPC problem can be formulated as follows:
(6)  
s.t.  
where , , and each denotes a discretized dynamics, a state constraint set, and an input constraint set with and . Dynamics , states , and inputs are discretized based on the simplified model in equation (5). Since the door’s state is included in the system states , through the quadratic cost function with respect to state and input error, state and input trajectories for opening the door while spending minimum energy can be generated. The terminal cost and the stage cost are defined with each variable’s desired value denoted with a superscript of as follows:
(7)  
IvB Hard Constraints
To ensure a safe operation for the scenario, several hard constraints on states should be considered. In this section, three types of constraints for collision avoidance are handled: selfcollision avoidance, door collision avoidance, and doorframe collision avoidance. These constraints are first derived using kinematic relationship and organized into a state constraint set as in equation (6). No hard constraints on inputs are considered in this paper; therefore, the input constraint set is set to be .
First of all, unlike a general multirotor, our UAM’s additional freedom in the robotic arm should be carefully managed to avoid a crash between the multirotor airframe and the robotic arm. To ensure this selfcollision avoidance, following constraints are devised:
(8)  
where , , and are position vectors of the servomotor, the servomotor, and the endeffector of the robotic arm described in while having their origins at as in the Fig. 3, and the subscript in denotes the third component of a vector . Since is assumed to be centered at the CoM of the multirotor, and these vectors are all described in , the above constraints imply that the robotic arm must always stay below the airframe of the multirotor. Note that all three position vectors , , and are a function of which can be derived with forward kinematics; therefore, these constraints can be formulated only with system states.
The second constraint, which is avoiding collision with the door, is constructed as follows:
(9) 
In equation (9), the lefthand side indicates the shortest distance between the CoM of the UAM and the door surface, and the righthand side quantifies the distance between the CoM of the UAM and the multirotor’s airframe closest to the door surface. If we introduce , a door surface unit normal vector in , the equation (9) can be arranged as follows:
(10) 
Since all variables in this constraint are functions of states only, this second constraint can also be formulated only with the system states.
The last doorframe avoiding constraint is formulated as the following:
(11) 
where subscript of denotes the second component of a vector . This equation can be further organized and can only be expressed with states as
(12) 
where is the endeffector position vector from the CoM of the UAM described in the frame .
IvC Optimal Control Solver
Based on the problem and constraints formulated in the subsection IVA and IVB, the existing algorithm of the constrained version of differential dynamic programming in [19] is employed to handle nonlinear dynamics with state constraints. This solver could generate an optimized nominal trajectory satisfying constraints in about 30 Hz with a time horizon of 1 second.
V Experiments
Va Control Framework
Overall control framework is illustrated in the Fig. 4. , , , and with as their superscript denotes final target states . Since only observable states from the UAM are , , , , , and , states for the door have to be converted in state converter in Fig. 4 as:
(13)  
where , , and . is a hat operator describing a cross product between two vectors as . This state conversion is based on the kinematic equation (1) and its time derivatives.
After the MPC module receives all state information, it generates state and input trajectories for certain time steps . These trajectories are translated back into the aerial manipulation’s desired position and velocity by the state converter, using a similar process as in the equation (13). Among the series of desired position and velocity for both the multirotor and the robotic arm’s servomotors, the first arrived desired setpoints are subscribed to the controller as new setpoints. The robotic arm in the flow chart contains an inherent velocity controller, and therefore desired velocity is depicted to be directly published to the robotic arm.
Stability is one of the inevitable and challenging problems in controlling an aerial manipulator. Although the input trajectory subscribed from MPC could be directly applied to the flight controller (FCU) for attitude control, which is Pixhawk 2 in our case, it is hard to guarantee stability during flight. Consequently, we adopt a disturbance observerbased robust controller implemented in [6] for position control where its stability is fully analyzed along with experimental validations. This controller generates the desired attitude and angular velocities again to the flight controller by which we can ensure the aerial manipulation’s stability.
VB Experimental Setup
We use a DJI F550 multirotor frame with 2312E motors controlled by 420Lite electronic speed controllers. The robotic arm is composed of ROBOTIS dynamixel XH430 series, and frames of OPEN MANIPULATORX. As the onboard computer, Intel NUC running Robot Operating System (ROS) in Ubuntu 16.04 executes MPCbased trajectory planner, DOBbased robust position controller, and navigation algorithm with VICON. As in Fig. 4, the vehicle’s attitude is controlled by the onboard lowlevel controller in Pixhawk 2. In addition, to address a realistic scenario, a door with its width and height of , and is employed. The weight of the door is about , and with these values, the mass moment of inertia of the door can be calculated as .
Weight matrices, desired final states and inputs for the optimal problem are designed as
(14)  
where denotes a final desired state which is used as . We set the initial door state as and .
VC Results
In the experiment, an aerial manipulator pushing a customized hinged door is conducted. Thanks to the capability of considering the state constraints in planning trajectory, the aerial manipulator successfully opens the door without collision as illustrated in Fig. 1. The history of the states during the experiment is described in Fig. 5. As expected, the door angle tends to converge to the desired final value, implying that the door is sufficiently opened. Furthermore, followed by the changes in the door angle , the vehicle’s yaw motion rotates accordingly. However, discrepancies between the desired and measured states, especially in and , occur due to the fact that the assumption 3 happens to be violated intermittently during the experiment. Although it is assumed that the endeffector is firmly attached to the door surface, uncertainty in door parameters and unmodeled dynamics between the UAM and the door seem to cause a faster door movement which results in a detachment between the door surface and the endeffector. Force control strategy like impedance control seems to be capable of handling this problem, and we leave it as a future work.
Vi Conclusion
In this paper, a systematic methodology for an aerial manipulator operating a hinged door is presented. Coupled equations of motion encompassing the aerial manipulator and the hinged door are first derived and later simplified to be applicable to an onlinesolvable optimal control problem. State constraints guaranteeing a safe trajectory are proposed, and the formulated MPC problem is solved with a constrained DDP algorithm. Generated trajectory is then tracked by a DOBbased robust controller, which provides stability during execution. For future studies, along with the one mentioned in the subsection VC
, a robotic arm with higher degrees of freedom in the endeffector is anticipated to provide better maneuverability while in interaction.
References
 [1] F. Ruggiero, V. Lippiello, and A. Ollero, “Aerial manipulation: A literature review,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 1957–1964, 2018.
 [2] H. W. Wopereis, W. L. Van De Ridder, T. J. Lankhorst, L. Klooster, E. M. Bukai, D. Wuthier, G. Nikolakopoulos, S. Stramigioli, J. B. Engelen, and M. Fumagalli, “Multimodal aerial locomotion: An approach to active tool handling,” IEEE Robotics & Automation Magazine, vol. 25, no. 4, pp. 57–65, 2018.
 [3] S. Hamaza, I. Georgilas, M. J. Fernandez, P. J. SanchezCuevas, T. Richardson, G. Heredia, and A. Ollero, “Sensor installation and retrieval operations using an unmanned aerial manipulator,” IEEE Robotics and Automation Letters, 2019.
 [4] K. Alexis, G. Darivianakis, M. Burri, and R. Siegwart, “Aerial robotic contactbased inspection: planning and control,” Autonomous Robots, vol. 40, no. 4, pp. 631–655, 2016.
 [5] K. Bodie, M. Brunner, M. Pantic, S. Walser, P. Pfändler, U. Angst, R. Siegwart, and J. Nieto, “An omnidirectional aerial manipulation platform for contactbased inspection,” arXiv preprint arXiv:1905.03502, 2019.
 [6] S. Kim, S. Choi, H. Kim, J. Shin, H. Shim, and H. J. Kim, “Robust control of an equipmentadded multirotor using disturbance observer,” IEEE Transactions on Control Systems Technology, vol. 26, no. 4, pp. 1524–1531, 2017.
 [7] M. Tognon, E. Cataldi, H. A. T. Chavez, G. Antonelli, J. Cortés, and A. Franchi, “Controlaware motion planning for taskconstrained aerial manipulation,” IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2478–2484, 2018.
 [8] M. Tognon, H. A. T. Chávez, E. Gasparin, Q. Sablé, D. Bicego, A. Mallet, M. Lany, G. Santi, B. Revaz, J. Cortés, et al., “A trulyredundant aerial manipulator system with application to pushandslide inspection in industrial plants,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1846–1851, 2019.
 [9] M. Orsag, C. Korpela, S. Bogdan, and P. Oh, “Dexterous aerial robots—mobile manipulation using unmanned aerial systems,” IEEE Transactions on Robotics, vol. 33, no. 6, pp. 1453–1466, 2017.
 [10] S. Kim, H. Seo, and H. J. Kim, “Operating an unknown drawer using an aerial manipulator,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 5503–5508.
 [11] H. Tsukagoshi, M. Watanabe, T. Hamada, D. Ashlih, and R. Iizuka, “Aerial manipulator with perching and dooropening capability,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 4663–4668.
 [12] G. Darivianakis, K. Alexis, M. Burri, and R. Siegwart, “Hybrid predictive control for aerial robotic physical interaction towards inspection operations,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 53–58.
 [13] M. Neunert, C. De Crousaz, F. Furrer, M. Kamel, F. Farshidian, R. Siegwart, and J. Buchli, “Fast nonlinear model predictive control for unified trajectory optimization and tracking,” in 2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016, pp. 1398–1404.
 [14] C. Y. Son, H. Seo, T. Kim, and H. J. Kim, “Model predictive control of a multirotor with a suspended load for avoiding obstacles,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 1–6.
 [15] D. Jang, J. Yoo, C. Y. Son, H. J. Kim, and K. H. Johansson, “Networked operation of a uav using gaussian processbased delay compensation and model predictive control,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 9216–9222.
 [16] D. Lee, D. Jang, H. Seo, and H. J. Kim, “Model predictive control for an aerial manipulator opening a hinged door,” in 2019 19th International Conference on Control, Automation and Systems (ICCAS). IEEE, 2019, p. to appear.
 [17] D. Brescianini and R. D’Andrea, “Computationally efficient trajectory generation for fully actuated multirotor vehicles,” IEEE Transactions on Robotics, vol. 34, no. 3, pp. 555–571, 2018.
 [18] H. Seo, D. Lee, C. Y. Son, C. J. Tomlin, and H. J. Kim, “Robust trajectory planning for a multirotor against disturbance based on hamiltonjacobi reachability analysis,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, p. to appear.
 [19] B. Plancher, Z. Manchester, and S. Kuindersma, “Constrained unscented dynamic programming,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 5674–5680.
Comments
There are no comments yet.