Recently, Unmanned Aerial Vehicles (UAVs) especially multi-rotors type, receive great attention due to their higher degree of mobility, speed and capability to access to regions that are inaccessible to ground vehicles. However, UAV as a standalone vehicle has a limited functionality to the search and surveillance applications.
Due to their superior mobility, much interest is given to utilize them for aerial manipulation and thus the application of UAV manipulation systems have been expanded dramatically. Applications of such systems include inspection, maintenance, structure assembly,firefighting, rescue operation, surveillance, or transportation in locations that are inaccessible, very dangerous or costly to be accessed from the ground.
Research on quadrotor-based aerial manipulation can be divided into different approaches based on the tool attached to the UAV including gripper based , cables based [2, 3], multi-DoF robotic manipulator based[4, 5], multi-DoF dual-arms manipulator based , compliant manipulator -based , Hybrid rigid/elastic-joint manipulator .
In the gripper/ tool-based approach, the attitude of the payload/tool is restricted to that of the quadrotor, and hence, the resulting aerial system has independent 4 DOFs; three translational DOFs and one rotational DOF (Yaw), i.e., the gripper/tool cannot posses pitch or roll rotation without moving horizontally. The second approach is to suspend a payload with cables but this approach has a drawback that the movement of the payload cannot be always regulated directly. To cope up with these limitations, another approach is developed in which a quadrotor is equipped with a robotic manipulator that can actively interact with the environment. Very few reports exist in the literature that investigate the combination of aerial vehicle with robotic manipulator. Kinematic and dynamic models of the quadrotor combined with arbitrary multi-DOF robot arm are derived using the Euler-Lagrangian formalism in . In , a quadrotor with light-weight manipulators, three 2-DOF arms, are tested. In , an aerial manipulation using a quadrotor with a 2-DOF robotic arm is presented but with certain topology that disable the system from making arbitrary position and orientation of the end-effector. In this system, the axes of the manipulator joints are parallel to each other and parallel to one in-plane axis of the quadrotor. Thus, the system cannot achieve orientation around the second in-plane axis of the quadrotor without moving horizontally.
From the above discussion, the current introduced systems in the literature that use a gripper suffers from the limited allowable DOFs of the end-effector. The other systems have a manipulator with either two DOFs but in certain topology that disables the end-effector to track arbitrary 6-DOF trajectory, or more than two DOFs which decreases greatly the possible payload carried by the system.
In [11, 12, 5], the authors propose a new aerial manipulation system that consists of 2-link manipulator, with two revolute joints whose axes are perpendicular to each other and the axis of the first joint is parallel to one in-plane axis of the quadrotor. Thus, the end-effector is able to reach arbitrary position and orientation without moving horizontally with minimum possible actuators.
In order to achieve position holding during manipulation, uncertainties and disturbances in the system such as wind, contact forces, measurement noise have to be compensated by using a robust control scheme. Disturbance Observer (DOb)-based controller is used to achieve a robust motion control [13, 14]. The DOb estimates the nonlinear terms and uncertainties then compensates them such that the robotic system acts like a multi-SISO linear systems. Therefore, it is possible to rely on a standard linear controller to design the controller of the outer loop such that the system performance can be adjusted to achieve desired tracking accuracy and speed. In [15, 16, 17], DOb-based motion control technique is applied to robotic-based systems and gives efficient results.
In the motion control of the aerial manipulator, achievement of the compliance control is very important because the compliance motion makes possible to perform flexible motion of the manipulator according to desired impedance . This is very critical demand in applications such as demining and maintenance. In the compliance control, end-effector position and generated force of the manipulator are controlled according to the reaction force detected by the force sensor. In this method, the desired impedance is selected arbitrary in the controller. However, the force sensor is essential to detect the reaction force as presented in [19, 20, 21]. On line identified environment impedance has also been used for transparency in teleoperation systems . These problems are more severe when environment displays sudden changes in its dynamic parameters which cannot be tracked by the identification process. In , it is found that in order to faithfully convey to the operator the sense of high frequency chattering of contact between the slave and hard objects, faster identification and structurally modified methods were required. However, these methods need the measurement of force.
Several techniques are proposed to estimate the contact force and the environment dynamics. In , the DOb and Recursive Least Squares (RLS) are used to estimate the environment dynamics. However, in this method, two DObs are used besides the RLS, and the estimation of contact force is activated only during the instance of contacting, thus there is a need to detect the instant at which the contact occurs. However, this is not practical approach especially if we target autonomous system. In [25, 26, 27, 28, 29, 30], several techniques are proposed to achieve force control without measuring the force. However, these techniques are based on ignoring some dynamics and external disturbances which will produce inaccurate force estimation. In [31, 32], an impedance control is designed for aerial manipulator without the need to measure/estimate the contact force. However, in such work, the authors neglect some dynamics as well as external disturbances, in addition to, the proposed algorithm is model-based and it does not have a robustness capability. In , a scheme is proposed which allows a quadrotor to perform tracking tasks without a precise knowledge of its dynamics and under the effect of external disturbances and unmodeled aerodynamics. In addition, this scheme can estimate the external generalized forces. However, as the authors claim, this estimator can work perfectly with constant external disturbances. In addition, the estimated forces contain many different types of forces such as wind, payload, environment impacts, and unmodeled dynamics. Thus, it can not isolate the end-effector force only from the others. The authors in  present a model-based method to estimate the external wrench of a flying robot. However, this method assumes that there are no modeling errors and no external disturbance. Moreover, it estimates the external force as one unite and it can not distinguish between external disturbance and the end-effector force which we need to calculate for teleoperation purposes. In addition, it uses a model based control which needs a full knowledge of the model.
In this article, a new scheme is proposed to cope up with these limitations of the currently developed techniques to solve the issues of this complicated multibody robotic system. Firstly, a DOb inner loop is used to estimate both the system nonlinearities and all external forces to compensate for them, as a result, the system acts like a linear decoupled MIMO system. Secondly, a fast tracking RLS algorithm is utilized with the linearization capabilities of DOb to estimate the contact force, in addition to, it enables the user to sense the contact force at the end-effector that it is not available in the current developed schemes. Thirdly, a model-free robust impedance control of the quadrotor manipulation system is implemented. The DOb is designed in the quadrotor/joint space while the impedance control is designed in the task space such that the end-effector can track the desired task space trajectories besides applying a specified environment impedance. Thus, Fourthly, a Jacobian based algorithm is proposed to transform the control signal from the task space to the quadrotor/joint space coordinates. The rigorous stability analysis of the proposed scheme is presented. Finally, the system model is simulated in MATLAB taking in to considerations all the non-idealities and based on real parameters to emulate a real system.
2 System Modeling
Fig. 1 presents a 3D CAD model of the proposed quadrotor-based aerial manipulator. The system is composed of a manipulator mounted on the bottom center of a quadrotor.
System geometrical frames, which are assumed to satisfy the Denavit-Hartenberg (DH) convention, are illustrated in Fig. 2. The manipulator has two revolute joints. The axis of the first revolute joint, , is parallel to the quadrotor -axis. The axis of the second joint, , is normal to that of the first joint and hence it is parallel to the quadrotor -axis at the extended configuration. Therefor, the pitching and rolling rotation of the end-effector is allowable independently from the horizontal motion of the quadrotor. Hence, with this proposed aerial manipulator, it is possible to manipulate objects with arbitrary location and orientation. Consequently, the end-effector can make motion in 6-DOF with minimum possible number of actuators/links that is critical factor in flight.
The quadrotor components are designed to achieve a payload capacity of g. Asctec pelican quadrotor  is utilized as a quadrotor platform. The maximum thrust force for each rotor is N. The arm is designed so that the total weight of the arm is g, it has a maximum reach in the range of , and it can carry a payload of g. It has three DC motors, (HS-422 (Max torque = N.m) for gripper, HS-5485HB (Max torque = N.m) for joint , and HS-422 (Max torque = N.m) for joint ).
The angular velocity of each rotor is and it generates thrust force
and drag momentthat are given by
where and are the thrust and drag coefficients.
Let , - , represents the quadrotor body-fixed reference frame with origin at the quadrotor center of mass, see Fig. 2. Its position with respect to the world-fixed inertial reference frame, , - , is given by the vector , while its orientation is given by the rotation matrix which is given by
where = is the triple yaw-pitch-roll angles. Note that and are short notations for and . Let us consider the frame , - , attached to the end-effector of the manipulator, see Fig. 2. Thus, the position of with respect to is given by
where the vector describes the position of with respect to expressed in . The orientation of can be defined by the rotation matrix
where describes the orientation of w.r.t . The linear velocity of in the world-fixed frame is obtained by the differentiation of (4) as
where is the angular velocity of the end-effector relative to and is expressed in .
Let be the vector of joint angles of the manipulator. The vector of the generalized velocity of the end-effector with respect to , , can be expressed in terms of the joint velocities via the manipulator Jacobian , such that
where and denote identity and null matrices, respectively. If the attitude of the vehicle is expressed in terms of yaw-pitch-roll angles, then (9) will be
with where describes the transformation matrix between the angular velocity and the time derivative of Euler angles , and it is given as
Since the vehicle is an under-actuated system, i.e., only independent control inputs are available for the 6-DOF system, the position and the yaw angle are usually the controlled variables. The pitch and roll angles are used as intermediate control inputs to control the horizontal position. Hence, it is worth rewriting the vector as follows
Thus, the differential kinematics (10) will be
where is the vector of the controlled variables, is composed by the first 4 columns of , is composed by the last 2 columns of , and .
If the end-effector orientation is expressed via a triple of Euler angles, , , the differential kinematics (12) can be rewritten in terms of the vector as follows
where is the same as but it is a function of instead of .
The equations of motion of the proposed robot have been derived in details in . The dynamical model of the quadrotor-manipulator system can be reformulated in a matrix form as
where represents the vector of the generalized coordinates, denotes the symmetric and positive definite inertia matrix of the system, represents the Coriolis and centrifugal terms, represents the gravity term, is vector of the external disturbances, is vector of the contact force effect, is the generalized input torques/forces, is vector of the actuator inputs, and is the input matrix which is used to produced the body forces and moments from the actuator inputs. The control matrix, , is given as
where , and is matrix that transforms body input forces to be expressed in and is given by
The environment dynamics, contact force, , can be modeled as following:
where and represent the environment stiffness and the environment damping, receptively.
The average wind velocity is determined by
where is the wind velocity at altitude , is the specified (measured) wind velocity at altitude . To simulate wind disturbances, it is worth calculating the wind force, , which influences the platform than the wind velocity. This force can be determined by
where is used to convert wind velocity to pressure, and is the influence effective area which depends on the quadrotor structure and its orientation.
This force can be projected on the axes of frame as
where , , , , represents the angle of wind direction, and both and depend on the quadrotor design parameters.
3 Controller Design
3.1 Control Objectives
Our goal is to design of estimation and control system to achieve the following objectives:
Robust Stability: The robotic system in Fig. 3 is robust and stable against the external disturbances, parameters uncertainties, and noises.
Force Estimation: The end-effector contact force has to be estimated with fast response and the estimation error tends to zero as the time tends to .
-DOF Impedance Control: In the presence of the applied force/desired impedance at the end-effector, the end-effector tracking error tends to zero as time tends to .
To this end, we propose a control scheme as shown in Fig. 3. In this control strategy, the system nonlinearities, external disturbances (wind), , and contact force, , are treated as disturbances, , that will be estimated, , and compensated by the DOb in the inner loop. The system can be now tackled as linear SISO plants. The output of DOb with system measurements of both joint and task spaces variables are used as the inputs to the FTRLS to obtain the end-effector contact force . The task space impedance control is used in the external loop of DOb and its output is transformed to the joint space through a transformation algorithm.
3.2 Disturbance Observer Loop
A block diagram of the DOb inner loop is shown in Fig. 4. In this figure, is the system nominal inertia matrix, and are the robot and desired inputs, respectively, with is the bandwidth of the variable of , is the matrix of the low pass filter of DOb. The DOb requires velocity measurement. Practically, the velocity have to be fed through a low pass filter, , and with cut-off frequency of . represents the system disturbances, and is the estimated disturbances.
If we apply the concept of disturbance observer to the proposed system, the independent coordinate control is possible without considering coupling effect of other coordinates. The coupling terms such as centripetal and Coriolis and gravity terms are considered as disturbance and compensated by feed forward the estimated disturbance torque.
The disturbance can be assumed as
The control input, , see Fig. 4, is given as
Applying this control law results in
Stability of this inner loop can be proved as following:
To simplify the analysis, let us ignore the effect of the velocity filter which will be considered later.
Let us use a Lyapunov function as
The time derivative of this function is
where represents a -dimensional vector, and and are positive real constants.
From property (29), one can get
Thus, the error dynamics is input/output stable with respect to the pair (,) for all with the assumption that the system states, and , are bounded.
If one considers the effect of using the velocity filter, then the characteristic equation of the inner loop is
To improve the robustness, the damping coefficient of this equation, which is , should larger than or equal . Therefore, the following inequality
should be hold. Recasting (35) with respect to gives to
If the DOb performs well, that is = , the dynamics from the DOb loop input to the output of the system is given as
Since is assumed to be a diagonal matrix, the system can be considered as a decoupled linear multi SISO systems as
or in the acceleration space as:
The next step is to design an Impedance tracking based controller in the outer loop for the system of (39).
3.3 Fast Tracking Recursive Least Squares
In this part, we develop a technique which utilizes a Fast Tracking Recursive Least Squares (FTRLS) to estimate the contact force with the aid of the DOb linearization capabilities. The FTRLS algorithm is one of the fast online least squares-based identification methods used for the identification of environments with varying dynamic parameters [43, 44]. To apply FTRLS, the dynamic equations (14- 20) have to be parametrized (i.e., to be product of measurement data regressor and dynamic parameters) as follows:
The system dynamic part, , can be rewritten as the product of data regressor, , and platform parameters, . The environment dynamics, , can be reformulated as , where, , is a function of the end effector states, (,), and is the environment parameters and . Finally, the wind effect is formulated as , where is the wind parameters. Thus, the total dynamics can be reformulated as
where and are the data regressor and parameters vector of (14), respectively.
The parameter estimation error is
while the estimation error is
By minimizing a cost function with respect to the parameter estimation error, one can find the time derivative of the estimated parameters vector, , as following
where is the parameters’ covariance matrix, and it can be calculated from
where is the forgetting factor, and it is given as
where is a constant representing the minimum forgetting factor, is the round-off operator, and is a design constant. This adaptive formulation of the forgetting factor enables the RLS to track the non-stationary parameters to be estimated.
The convergence/stability () proof of this algorithm can be implemented as following:
Let us assume the Lyapunov function as
If is chosen to be positive definite, then will be positive definite. To prove the positive definiteness of , let us use the solution of the differential equation (44) which is
where is the state transition matrix of a system described by . Thus, by choosing , then the first term in (47) will be positive definite. The second term is also positive definite. As a result, the proposed covariance matrix update formula is positive definite, and thus, the chosen Lyapunov function (46) is positive definite.
The time derivative of Lyapunov function is
Thus, the time derivative of is negative definite which ensures the asymptotic stability of the estimation error ( as )
Finally, for both teleoperation impedance control purposes, the user can calculate the estimated environment impedance, contact force, from
Therefore, unlike the current the developed schemes, with this technique, one can isolate and estimate the end-effector contact force apart from the whole estimated forces in the systems.
3.4 Impedance Control
The objective of the impedance control is to regulate the end-effector interaction force, which may vary due to the uncertainty in the location of the interaction point and/or the structural properties of the environment, besides achieving task space trajectory tracking. The linear impedance control is designed in the task space. This is based on the linearization effect of the designed DOb in the joint space. The desired acceleration in the task space, , can be calculated from
where and are the desired values of and respectively, which determine the desired impedance that the end-effector will apply to the environment. Let us define the quadrotor/joint space tracking error as
while the task space tracking error can be defined as
where , , and are the reference trajectories for the position, velocity, acceleration in the task space, respectively which are chosen to be bounded and continuous. , , and are the reference trajectories for the position, velocity, acceleration in the quadrotor/joint space, respectively. Transformation from the task space to quadrotor/joint space will be implemented via the inverse of system Jacobian. The relation between the inner loop and the outer loop errors can be obtained as follows. The DOb loop error can be expressed in the task space, , via the Jacobian by
Equation (57) can be reformulated in a state space form as
where , , , and . By inspecting the matrix and based on the boundedness of both and , one can find that the state, , is bounded and exponentially tends to zero as time tends to infinity as soon as the matrices, and , are positive definite. As a result, since the Jacobian inverse exists (no singularities), the system errors, and , are also bounded and exponentially tends to zero as time tends to infinity.
A complete and detailed block diagram of the proposed control scheme is illustrated in Fig. 5. Quadrotor position and yaw rotation are the controlled variables, while pitch and roll angles are used as intermediate control inputs to achieve the desired and . Therefore, the proposed scheme has two DOb-based controllers include one for (with , , ) and the other for (with , , ). The desired 6-DOF trajectories for the end-effector’s (), their actual values calculated by the forward kinematics, and the estimated end-effector force, are applied to the impedance control algorithm, that is given in (53). Then, a transformation from task space to joint space is done by using (59) to get . The desired acceleration in the joint space, , can be calculated by differentiating (13) with respect to time as
The desired acceleration in quadrotor/joint space, , is then applied to the DOb of the independent coordinates, , to produce . The desired values for the intermediate DOb controller, , are obtained from the output of position controller, , through the following simplified nonholonomic constraints relation
The external controller of second DOb controller, /, is used as a PD controller with velocity feedback, , as following:
After that, is applied to the second DOb to generate .
It is konwn that the response of the controller must be much faster than that of the position controller. This can be achieved by the tuning parameters of both DOb and PD of -controller.
The output of two controllers are converted to the required forces/torques applied to quadrotor/manipulator by
where is part of matrix and it is given by .
4 Simulation Results
In this section, the presented aerial manipulation robot model with the proposed control technique is implemented in MATLAB/SIMULINK.
4.1 Simulation Environment
For a more realistic simulation studies, the following setup have been made:
Linear and angular position and orientation of the quadrotor are available at rate of KHz. In , a scheme is proposed to measure and estimate the vehicle (Asctec Pelican Quadrotor) states based on IMU and Onboard camera in both indoors and outdoors.
The joints angles are measured at rate of KHz and angular velocities are estimated by a low pass filter.
KHz Control loop.
To test the robustness against model uncertainties, a step disturbance is applied at s to both the inertia matrix, , and the control matrix, , (Actuators’ losses) with error.