1 Introduction
Recently, Unmanned Aerial Vehicles (UAVs) especially multirotors 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 quadrotorbased aerial manipulation can be divided into different approaches based on the tool attached to the UAV including gripper based [1], cables based [2, 3], multiDoF robotic manipulator based[4, 5], multiDoF dualarms manipulator based [6], compliant manipulator based [7], Hybrid rigid/elasticjoint manipulator [8].
In the gripper/ toolbased 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 multiDOF robot arm are derived using the EulerLagrangian formalism in [9]. In [10], a quadrotor with lightweight manipulators, three 2DOF arms, are tested. In [4], an aerial manipulation using a quadrotor with a 2DOF robotic arm is presented but with certain topology that disable the system from making arbitrary position and orientation of the endeffector. In this system, the axes of the manipulator joints are parallel to each other and parallel to one inplane axis of the quadrotor. Thus, the system cannot achieve orientation around the second inplane 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 endeffector. The other systems have a manipulator with either two DOFs but in certain topology that disables the endeffector to track arbitrary 6DOF 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 2link manipulator, with two revolute joints whose axes are perpendicular to each other and the axis of the first joint is parallel to one inplane axis of the quadrotor. Thus, the endeffector 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 multiSISO 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], DObbased motion control technique is applied to roboticbased 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 [18]. This is very critical demand in applications such as demining and maintenance. In the compliance control, endeffector 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 [22]. These problems are more severe when environment displays sudden changes in its dynamic parameters which cannot be tracked by the identification process. In [23], 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 [24], 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 modelbased and it does not have a robustness capability. In [33], 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 endeffector force only from the others. The authors in [34] present a modelbased 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 endeffector 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 endeffector that it is not available in the current developed schemes. Thirdly, a modelfree 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 endeffector 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 nonidealities and based on real parameters to emulate a real system.
2 System Modeling
Fig. 1 presents a 3D CAD model of the proposed quadrotorbased 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 DenavitHartenberg (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 endeffector 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 endeffector can make motion in 6DOF 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 [35] 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, (HS422 (Max torque = N.m) for gripper, HS5485HB (Max torque = N.m) for joint , and HS422 (Max torque = N.m) for joint ).
The angular velocity of each rotor is and it generates thrust force
and drag moment
that are given by(1) 
(2) 
where and are the thrust and drag coefficients.
2.1 Kinematics
Let ,  , represents the quadrotor bodyfixed reference frame with origin at the quadrotor center of mass, see Fig. 2. Its position with respect to the worldfixed inertial reference frame, ,  , is given by the vector , while its orientation is given by the rotation matrix which is given by
(3) 
where = is the triple yawpitchroll angles. Note that and are short notations for and . Let us consider the frame ,  , attached to the endeffector of the manipulator, see Fig. 2. Thus, the position of with respect to is given by
(4) 
where the vector describes the position of with respect to expressed in . The orientation of can be defined by the rotation matrix
(5) 
where describes the orientation of w.r.t . The linear velocity of in the worldfixed frame is obtained by the differentiation of (4) as
(6) 
where is the skewsymmetric matrix operator [36], while is the angular velocity of the quadrotor expressed in . The angular velocity of is expressed as
(7) 
where is the angular velocity of the endeffector relative to and is expressed in .
Let be the vector of joint angles of the manipulator. The vector of the generalized velocity of the endeffector with respect to , , can be expressed in terms of the joint velocities via the manipulator Jacobian [37], such that
(8) 
(9) 
where ,
,
where and denote identity and null matrices, respectively.
If the attitude of the vehicle is expressed in terms of yawpitchroll angles, then (9) will be
(10) 
with where describes the transformation matrix between the angular velocity and the time derivative of Euler angles , and it is given as
(11) 
Since the vehicle is an underactuated system, i.e., only independent control inputs are available for the 6DOF 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
(12)  
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 endeffector orientation is expressed via a triple of Euler angles, , , the differential kinematics (12) can be rewritten in terms of the vector as follows
(13)  
where is the same as but it is a function of instead of .
2.2 Dynamics
The equations of motion of the proposed robot have been derived in details in [11]. The dynamical model of the quadrotormanipulator system can be reformulated in a matrix form as
(14) 
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
(15) 
where , and is matrix that transforms body input forces to be expressed in and is given by
(16) 
The environment dynamics, contact force, , can be modeled as following:
(17) 
where and represent the environment stiffness and the environment damping, receptively.
The average wind velocity is determined by
(18) 
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
(19) 
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
(20) 
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 endeffector 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 endeffector, the endeffector 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 endeffector 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 cutoff 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
(21) 
Substituting from (21), then (14) can be rewritten as
(22) 
The control input, , see Fig. 4, is given as
(23) 
Applying this control law results in
(24) 
where
(25) 
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
(26) 
The time derivative of this function is
(27) 
Substituting from (24), then (27) becomes
(28) 
To complete this proof, the properties of the dynamic equation of motion (14) will be utilized. Theses properties are [41, 36]:
Property 3.1
(29) 
Property 3.2
(30) 
where represents a dimensional vector, and and are positive real constants.
Substituting from (30), then (28) will be
(31) 
From property (29), one can get
(32) 
From the analysis presented in [42], (32) can be reformulated as
(33) 
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
(34) 
where .
To improve the robustness, the damping coefficient of this equation, which is , should larger than or equal . Therefore, the following inequality
(35) 
should be hold. Recasting (35) with respect to gives to
(36) 
Summarizing, (32) shows that the stability and robustness of the control system is enhanced by increasing , i.e., by increasing and , but without violating the robustness constraint given in (36).
If the DOb performs well, that is = , the dynamics from the DOb loop input to the output of the system is given as
(37) 
Since is assumed to be a diagonal matrix, the system can be considered as a decoupled linear multi SISO systems as
(38) 
or in the acceleration space as:
(39) 
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 squaresbased 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
(40) 
where and are the data regressor and parameters vector of (14), respectively.
The parameter estimation error is
(41) 
while the estimation error is
(42) 
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
(43) 
where is the parameters’ covariance matrix, and it can be calculated from
(44) 
where is the forgetting factor, and it is given as
(45) 
where is a constant representing the minimum forgetting factor, is the roundoff operator, and is a design constant. This adaptive formulation of the forgetting factor enables the RLS to track the nonstationary parameters to be estimated.
The convergence/stability () proof of this algorithm can be implemented as following:
Let us assume the Lyapunov function as
(46) 
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
(47) 
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
(48) 
However, by differentiating both sides of (41) with respect to time, one can find that , by substituting from the proposed formula of (43) and (42), then
(49) 
Substituting from (49) in (48), then will be
(50) 
Substituting from the proposed formula (44) for into (50), then
(51) 
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
(52) 
Therefore, unlike the current the developed schemes, with this technique, one can isolate and estimate the endeffector 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 endeffector 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
(53) 
where and are the desired values of and respectively, which determine the desired impedance that the endeffector will apply to the environment. Let us define the quadrotor/joint space tracking error as
(54) 
while the task space tracking error can be defined as
(55) 
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
(56) 
where . From the previous analysis, it is proved that is bounded as in (33). If we define , then by substituting from (53), one can get
(57) 
Equation (57) can be reformulated in a state space form as
(58) 
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 DObbased controllers include one for (with , , ) and the other for (with , , ). The desired 6DOF trajectories for the endeffector’s (), their actual values calculated by the forward kinematics, and the estimated endeffector 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
(59) 
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
(60) 
The external controller of second DOb controller, /, is used as a PD controller with velocity feedback, , as following:
(61) 
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
(62) 
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 [45], 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.

The measured signal are affected by a normally distributed measurement noise with mean of
and standard deviation of
. 
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.
Par.  Value  Unit  Par.  Value  Unit 

Comments
There are no comments yet.