hybrid guidance of quadrotor manipulation system for indoor...

International Journal of Mechanical & Mechatronics Engineering IJMME-IJENS Vol:20 No:04 1

201904-5757-IJMME-IJENS © August 2020 IJENS I J E N S

Abstract— This work presents a simulation of a realistic way to

guide the non-redundant 6-DOF Quadrotor Manipulation System

(QMS) toward the target object, in preparation for catching it.

Depending only on the teleoperation process to drive a QMS to do

this task is a very difficult and slow process because of a human

capability. In this paper, two stages are presented. Firstly, guiding

the teleoperation process of the QMS by controlling its position

and orientation until the target object becomes in the camera Field

Of View (FOV). Secondly, a vision system is used to autonomously

guide the QMS toward the object until it becomes accessible to the

robot gripper. This will significantly increase both the speed and

accuracy of the whole process. For these two steps, Position

Holding Scheme (PHS) and Image-Based Visual Servoing (IBVS)

are used. The proposed QMS is a non-redundant 6-DOF robot,

which is characterized by a minimum number of actuators, low

power consumption, non-complex controller compared to a

redundant QMS. It consists of a 2-serial link robotic arm installed

at the quadrotor's Center Of Mass (COM) with a unique topology,

that provides maximum mobility with minimum possible weight.

In other words, there are 6 actuators only to perform the required

6-DOF. A camera/vision sensor is attached to the end-effector to

form eye-in-hand architecture. The system kinematics and

dynamics are illustrated. A fifth-order quintic polynomial is

utilized with Image-Based Visual Servoing (IBVS) to smoothly

guide the system toward the target object and to avoid losing it.

The PID controller is used to control the 6-DOF simultaneously

during autonomous mode. Besides, it is used to control the

teleoperation process. The proposed system is designed and tested

using MATLAB/ADAMS co-simulation programs. The simulation

results indicate that this system can perform dual tasks, manual

and autonomous guiding.

Index Term— Eye-in-hand, IBVS, QMS.

I. INTRODUCTION

UNMANNED aerial vehicles (UAVs) research activities have

witnessed remarkable development over more than a decade.

Consequently, industrial communities and public opinion has

been interested in various UAVs applications in different fields.

Previously, UAVs applications are limited to passive tasks such

as inspection, surveillance, monitoring, remote sensing and so

on. Over the last decade, interest in unmanned aerial

manipulator (UAM) systems has increased to carry out active

tasks like grasping and manipulation. Aerial manipulation

vehicles such as quadrotor with a manipulator or a gripper

attached to its bottom open a new application area for robotics

because it facilitates the interaction with the environment.

However, most UAVs manipulation system researches have

typically been limited to guide the teleoperation process of the

system to get close to the desired object to pick and place it in

another place which is very difficult and slow process because

of its dependence on human capability. Besides a teleoperation

process, a vision system can be used to increase both the

accuracy and speed of this process for real outdoor/indoor tasks.

Choosing suitable QMS configuration from among the

many configurations to implement the vision system is a

challenge, so, the previous UAM researches can be divided into

three parts. In researches utilizing the UAVs for active tasks,

the first part is using a quadrotor with 1-DOF gripper/tool attached to its bottom [1]-[3] for transporting blocks and to

build structures. Accordingly, the manipulation capability of

this system becomes limited, because of the restriction of the

payload/tool attitude with the UAV's attitude and also the end

effector's accessible range is restricted because of the fixed

configuration of the gripper/tool w.r.t the UAV body and

blades. Thus, the resulted aerial system possesses total 4-DOF

that comprises of three translational DOF, and one rotational

DOF (Yaw). For example, the gripper/tool cannot make roll or

pitch rotations without horizontal movement. In [4]-[8] UAVs

are equipped with cables to suspend a payload. However, this system has some limitations such as the payload's movement

cannot be always adjusted directly as the manipulation is

accomplished by a cable which cannot always drive the

payload's motion as required.

To overcome these limitations, UAV's research has turned

to equip aerial vehicles with a multi DOF robotic manipulator

to actively do complicated tasks. This combination of the

mobility of the aerial vehicle and the versatility of a robotic

manipulator maximizes the utility of mobile manipulation. The

dynamics of the resulted manipulator are highly connected with

the aerial vehicle's dynamics, which should be carefully

considered in the controller design. Besides, the reaction forces

resulted from the external environment or from dealing with the

Hybrid Guidance of Quadrotor Manipulation

System for Indoor-Outdoor Active Tasks Amr Hamed*1, Mohamed Fanni*2, Sabah Ahmed*3 and Ahmed Sameh*2

*Mechatronics and Robotics Eng. Department, School of Innovative Design Eng., Egypt-Japan University of

Science and Technology (E-JUST), New Borg El Arab, Alexandria, Egypt.

1On leave: Electronics Eng. & Communication and Electronics Department, Faculty of Engineering Shoubra, Benha

University, Cairo, Egypt.

2On leave: Production Eng. & Mechanical Design Department, Mansoura University, Mansoura, Egypt.

3On leave: Department of Electrical Engineering, Assiut University, Assiut, Egypt.

Email: amr.hamed, mohamed.fanni, sabah.ahmed ,ahmed.ismaeil,@ ejust.edu.eg



object have to be sustained by the aerial robot. These forces

have a great effect on the stability of the aerial vehicle.

The second part includes a quadrotor with more than 2-DOF

robotic arm. In other words, we can say redundant UAM. In [9]-

[14] modeling and control of redundant aerial manipulators are

presented. Accordingly, this system suffers from overweight

and high-power consumption because of the extra links and

actuators. Additionally, a system controller becomes complex

because of the increasing number of controlled variables (more

manipulator joints).

In the third part, a quadrotor with an only 2-DOF robotic arm.

In other words, non-redundant UAM. In [15]-[17] modeling

and control of a quadrotor with a 2-DOF manipulator fixed at

its bottom are illustrated. In this case, the end-effector has no

ability to rotate or move arbitrary as the joints' axes of the

manipulator are all parallel to each other and also parallel to one

axis in the plane of the quadrotor. Consequently, the

manipulator is not able to make orientation around the

quadrotor's second in-plane axis without horizontal movement.

In [18] and [19], Khalifa et al. presented a new aerial

manipulation system which comprises of a two-link

manipulator with two revolute joints. These joints' axes are

orthogonal to each other and one of them is parallel to one in-

plane axis of the vehicle. Consequently, the end-effector can

make arbitrary motions and rotations without moving

horizontally. This unique topology enables the end-effector to

follow the desirable 6-DOF trajectory with minimum possible

actuators. In other words, the system provides maximum

mobility with minimum possible weight which makes it a

suitable system for applying vision.

Indeed, infrared multi-camera devices are used to obtain an

accurate localization for UAM to utilize them for indoor

applications. Based on visual information, visual servoing is

required to achieve autonomous outdoor/indoor tasks by UAM

such as autonomous guidance and manipulation. Visual

servoing [20] includes IBVS, Position-Based Visual Servoing

(PBVS), and a hybrid of both systems. In [21] IBVS-based

vision guidance of a redundant 7-DOF aerial manipulation is

presented. The kinematic and dynamic models of this system

are derived and also an adaptive controller is applied. The result

of the redundancy in that system is overweight because of the

extra links and actuators. Additionally, a complex controller is

required. In [22] a problem of controlling IBVS-based vision

guidance of a redundant 10-DOF aerial manipulation is

addressed. A hierarchical control law is used to prioritize

several tasks during flight. This technique adds complexity to

the controller.

From the above discussion, a non-redundant UAM with a

few DOF that is less than six suffers from the limited allowable

DOF of the end-effector. compared to non-redundant 6-DOF

UAM, a redundant UAM suffers from the overweight, complex

controller and high-power consumption. UAM uses a vision

controller for achieving outdoor/indoor active tasks like

autonomous guidance, grasping, and manipulation.

In this paper, a mixture of manual and automatic guidance of

the QMS in the direction of the target object in preparation for

holding it in order to comply with realistic applications has been

presented. The system presented in [18] is chosen to implement

the whole process. It is a non-redundant 6-DOF robot and also

provides maximum mobility with minimum possible weight.

Further, a camera is attached to the end-effector to form eye-in-

hand architecture. The kinematic analysis that includes

position, orientation, and velocity of the end-effector is

presented. Also, the dynamics model considering a multirotor

and 2-DOF manipulator as a unified system is presented. For

the teleoperation process, PHS is used to just make the target

object in the FOV of the camera. For guidance using vision

(autonomous guidance), IBVS utilizing a perspective camera is

chosen, because it is robust and computationally efficient. To

maintain the target image in the FOV of the camera, a fifth-

order quintic polynomial trajectory for the desired features is

used to move smoothly toward the object and avoid leaving the

object’s features the camera image plane and overcome the

instability of the system due to initial large feature error. The

dynamics of the quadrotor and manipulator are highly coupled

and controlling them separately would degrade the quality of

control. To implement the autonomous task precisely the

controller is designed to control the robot 6-DOF

simultaneously. Based on visual information from the camera,

the guidance command from IBVS is mapped into an

infinitesimal motion of the quadrotor/manipulator joints. A PID

controller is utilized to control the QMS system during

teleoperation and autonomous process.

This paper is organized as follows: a brief description of the

QMS model was introduced in section II. In section III, the

kinematic and dynamic model of the non-redundant QMS are

presented. Teleoperation using a position holding scheme is

introduced in section IV. In section V, IBVS is discussed. In

section VI, mapping between the camera and aerial manipulator

velocities are discussed in detail. Section VII introduces the

controller design based on the PID controller technique. Section

VIII presents the simulation results of the virtual prototype

QMS using MATLAB/ADAMS programs. Finally, the main

contributions are summarized in section IX.

II. SYSTEM DESCRIPTION

Fig.1a shows the three-dimensional Computer-aided design

(3D-CAD) model of the proposed QMS. The shown aerial

manipulation system is divided into two main parts. The first

part is a multirotor which is called a quadrotor because it is

lifted and propelled by four rotors. The second part is a 2-DOF

serial manipulator which is installed in the middle of the

quadrotor and the end-effector is pointed down. This

combination between the 4-DOF quadrotor and the 2-DOF

serial manipulator form non-redundant QMS. The camera is

installed at a fixed relative distance from the end-effector,

which forms eye-in-hand architecture. The 3-D sketch graph of

the proposed system with the relevant frames is shown in

Fig.1b. The quadrotor-serial link structure indicates the unique

topology which allows the end-effector to obtain the arbitrary

pose. The frames are considered to fulfill the Denavit



(a) The 3D-CAD model

(b) The 3-D sketch graph with relevant frames

Fig. 1. QMS model

Hartenberg rules. There are two revolute joints in the serial

manipulator. The first revolute joint's axis (𝑧0), which is fixed

to the quadrotor, is parallel to the quadrotor's body x-axis [see

Fig.1b]. The second joint's axis (𝑧1) is orthogonal to the first

joint's axis and parallel to the y-axis of the quadrotor's body at

home position. Consequently, the pitching and rolling rotation

of the end-effector is separately from the quadrotor's horizontal

motion. Therefore, the ability of objects' manipulation with

arbitrary position and orientation is obtained with this system.

The camera frame 𝑓𝑐 is at fixed +𝑦𝑒𝑒 distance from the end-

effector frame which means motion of the camera depends on

the end-effector motion (eye-in-hand).

III. SYSTEM MODEL

In this section, the forward kinematic and dynamic models of

the non-redundant QMS are presented.

A. The System Forward Kinematic Model

In this Part, the forward kinematic analysis (the position,

orientation, and velocity) of the QMS is presented.

Firstly, position kinematic analysis of the QMS is presented.

Let 𝑓𝐼(𝑥, 𝑦, 𝑧) and 𝑓𝑏(𝑥𝑏 , 𝑦𝑏 , 𝑧𝑏) represent inertial/world fixed

global reference frame and a multirotor body-fixed reference

frame with its origin at quadrotor COM respectively. The

𝑃𝑏𝐼 = [𝑥 𝑦 𝑧]𝑇 ∈ ℝ3𝑥1 is the position vector of origin of

𝑓𝑏 w.r.t 𝑓𝐼 and the ZYX rotation matrix between 𝑓𝑏 and 𝑓𝐼 is

given by:

𝑅𝑏𝐼 = [

𝐶𝜓𝐶𝛳 𝑆𝜙𝑆𝛳𝐶𝜓 − 𝑆𝜓𝐶𝜙 𝑆𝜓𝑆𝜙 + 𝐶𝜓𝑆𝛳𝐶𝜙

𝑆𝜓𝐶𝛳

−𝑆𝛳

𝐶𝜓𝐶𝜙 + 𝑆𝜓𝑆𝛳𝑆𝜙

𝐶𝛳𝑆𝜙

𝑆𝜓𝑆𝛳𝐶𝜙 − 𝐶𝜓𝑆𝜙

𝐶𝛳𝐶𝜙

] (1)

Where 𝑆. and 𝐶. represent the short notations for Cos(.) and

Sin(.) respectively. While Φb = [𝜙 𝛳 𝜓]𝑇 ∈ ℝ3𝑥1 are

Euler angles coordinates w.r.t 𝑓𝑏. As shown in Fig.1b,

𝑓𝑒𝑒(𝑥𝑒𝑒, 𝑦𝑒𝑒 , 𝑧𝑒𝑒), 𝑓1(𝑥1, 𝑦1, 𝑧1) and 𝑓0(𝑥0, 𝑦0, 𝑧0) denote the

frames attached to the end-effecter, revolute joints 2 and 1

respectively. These frames satisfy the Denavit Hartenberg

convention. Moreover, the manipulator has two joint angles

which represented as vector Θ = [𝛳1 𝛳2]𝑇 ∈ ℝ2𝑥1. Table 1

presents the DH parameters of the 2-DOF serial robotic arm

manipulator. The position vector of the origin of 𝑓𝑒𝑒 w.r.t 𝑓𝐼 is

given by:

𝑃𝑒𝑒𝐼 = 𝑃𝑏 +𝐼 𝑅𝑏

𝐼 𝑃𝑒𝑒𝑏 (2)

Where 𝑃𝑒𝑒𝑏 is the position vector of the origin of the 𝑓𝑒𝑒 w.r.t

𝑓𝑏 . The orientation of 𝑓𝑒𝑒 w.r.t 𝑓𝐼 is defined by the rotation

matrix:

𝑅𝑒𝑒𝐼 = 𝑅𝑏 𝑅𝑒𝑒

𝑏𝐼 (3)

TABLE I

Serial Manipulator DH parameter

Link (i) 𝑎𝑖 𝛼𝑖 𝑑𝑖 ϴ𝑖

0 0 −𝜋/2 −𝑙1 −𝜋/2

1 𝑙1 𝜋/2 0 ϴ1

2 𝑙2 0 0 ϴ2

Where 𝑅𝑒𝑒𝑏 is the rotation matrix between 𝑓𝑒𝑒 and 𝑓𝑏 . Since

the operational/task space coordinates are 𝜒𝑒𝑒 =

[ 𝑃𝑒𝑒𝐼 Φee]

𝑇 ∈ ℝ6𝑥1 where 𝑃𝑒𝑒𝐼 = [𝑥𝑒𝑒 𝑦𝑒𝑒 𝑧𝑒𝑒]𝑇 ∈

ℝ3x1 and Φee = [𝜙𝑒𝑒 𝛳𝑒𝑒 𝜓𝑒𝑒]𝑇 ∈ ℝ3x1. The

quadrotor/joint space coordinates are 𝑞 = [𝜒𝑏 Θ]𝑇 ∈ ℝ8x1,

where 𝜒𝑏 = [ 𝑃𝑏𝐼 Φb]

𝑇 ∈ ℝ6x1 . For solving the forward

kinematics, the inputs are 8 variables q and the output are 6

variables 𝜒𝑒𝑒 obtained from 6 algebraic equations. Accordingly,

the position of the end-effector w.r.t global frame could be

computed easily from equation (2), while the end-effector Euler

angles Φee are easily computed from the rotation matrix of

𝑅𝑒𝑒𝐼 .

Secondly, the velocity kinematic analysis of the QMS that

includes the linear and angular velocity of the end-effector

frame w.r.t 𝑓𝐼 is discussed in [23]. i.e. the end-effector velocity

[ ��𝑒𝑒𝐼 𝜔𝑒𝑒

𝐼 ]𝑇 ∈ ℝ6𝑥1 w.r.t 𝑓𝐼 is given by:

𝑣𝑒𝑒𝐼 = 𝐽𝑏 𝜈𝑏 +𝐼 𝐽𝑒𝑏 Θ (4)

where 𝑣𝐼 𝑏 = [ ��𝑏𝐼 𝜔𝑏

𝐼 ] 𝑇

is the linear and angular velocity

of a quadrotor expressed in 𝑓𝐼 , 𝐽𝑏 =

[𝐼3 −𝑆𝑘𝑒𝑤( 𝑅𝐼 𝑏

𝑏𝑃𝑒 )

𝑂3 𝐼3] , 𝐽𝑒𝑏 = [

𝑅𝐼 𝑏 𝑂3

𝑂3 𝑅𝐼 𝑏

] 𝐽𝑒𝑏𝑏 , where



𝐽𝑒𝑏𝑏 is the manipulator Jacobean matrix that relates end-effector

velocity w.r.t 𝑓𝑏 to joint angles velocities. Θ = [��1 ��2]𝑇 ∈

ℝ2x1 is the vector of the angular velocities of the manipulator's

revolute joints. I𝑚 and O𝑚 denote (m x m) identity and (m x m)

null matrices respectively. Because the attitude of the vehicle is

expressed in terms of roll 𝜙, pitch 𝛳 and yaw 𝜓 angles, then the

equation (4) can be rewritten as follows:

𝑣𝑒𝑒𝐼 = 𝐽𝑏 𝑄𝑏 ��𝑏 + 𝐽𝑒𝑏 �� (5)

Where ��𝑏 = [ ��𝑏𝐼 Φb]

𝑇 ∈ ℝ6𝑥1, 𝑄𝑏 = [𝐼3 𝑂3

𝑂3 𝑇𝑏] and 𝑇𝑏

describes the transformation matrix between the angular

velocity 𝜔𝑏 and the time derivative of Euler angles Φb , and it

is given as:

𝑇𝑏 = [

𝐶𝜓𝐶𝛳 −𝑆𝜓 0

𝑆𝜓𝐶𝛳 𝐶𝜓 0

−𝑆𝛳 0 1

] (6)

Thus, the system Jacobian is:

𝑣𝑒𝑒𝐼 = 𝐽�� (7)

Where 𝐽 = [𝐽𝑏𝑄𝑏 𝐽𝑒𝑏]𝑇 ∈ ℝ6𝑥8 and �� = [��𝑏 Θ]𝑇 ∈ ℝ6x1.

The quadrotor is an underactuated system because it has only 4

independent control inputs, position, and yaw angles, which are

available for the 6-DOF. Hence, it is worth to rewrite equation

(7) showing the independent coordinate 𝜁 =[𝑥 𝑦 𝑧 𝜓 ϴ1 ϴ2]𝑇 ∈ ℝ6x1 and the dependent

coordinates 𝜎𝑏 = [𝜙 𝛳]𝑇 ∈ ℝ2𝑥1 as following:

ν𝑒𝑒𝐼 = 𝐽𝜁𝜁 + 𝐽𝜎𝜎�� (8)

Where 𝐽𝜁 = [𝐽(1: 6,1: 3) 𝐽(1: 6,6: 8)] ∈ ℝ6𝑥6 and 𝐽𝜎 =

𝐽(1: 6,4: 5) ∈ ℝ6𝑥2.

B. The System Dynamic Model

The proposed QMS equations of motion have been derived

in detail [18]. To get the motion equations for both the

manipulator and the quadrotor as well as the interaction forces

and moments between them, Recursive Newton-Euler and

Newton-Euler algorithms are applied to the 2-DOF manipulator

and the quadrotor respectively.

For the system structure, the quadrotor body is assumed to be

rigid and also symmetric about 𝑥𝑏 , 𝑦𝑏 , 𝑧𝑏 axis. The equations of

motion of the manipulator are given by:

𝑀1(𝑞)𝜃1 = 𝜏𝑚1 + 𝑁1(𝑞, ��,��) (9)

𝑀2(𝑞)𝜃2 = 𝜏𝑚2 + 𝑁2(𝑞, ��,��) (10)

Where 𝑀1, 𝑀2, 𝑁1 and 𝑁2 are nonlinear terms and they are

functions of system states (𝑞, ��,��). 𝜏𝑚1 and 𝜏𝑚2 are the torques

of manipulator’s actuators.

The equations of motion of a quadrotor after adding the

interaction moments and forces of 2-DOF serial robotic arm are

given by:

𝑚�� = 𝑇 (𝐶𝜓𝑆𝜃𝐶𝜙 + 𝑆𝜓𝑆𝜙) + 𝐹𝑚,𝑞𝑥 (11)

𝑚�� = 𝑇 (𝑆𝜓𝑆𝜃𝐶𝜙 − 𝐶𝜓𝑆𝜙) + 𝐹𝑚,𝑞𝑦 (12)

𝑚�� = −𝑚𝑔𝑟 + 𝑇𝐶𝜃𝐶𝜙 + 𝐹𝑚,𝑞𝑧 (13)

𝐼𝑥�� = ��𝜓 (𝐼𝑦 − 𝐼𝑧) − 𝐼𝑟��𝛺 + 𝜏𝑥 + 𝑀𝑚,𝑞𝜙𝑏 (14)

𝐼𝑦𝜃 = ��𝜙 (𝐼𝑧 − 𝐼𝑥) − 𝐼𝑟�� + 𝜏𝑦 + 𝑀𝑚,𝑞𝜃𝑏 (15)

𝐼𝑧𝜓 = ��(𝐼𝑥 − 𝐼𝑦) + 𝜏𝑧 + 𝑀𝑚,𝑞𝜓𝑏 (16)

Where 𝐹𝑚,𝑞𝑥, 𝐹𝑚,𝑞𝑦 and 𝐹𝑚,𝑞𝑧 are the interaction forces resulted

from the manipulator and affected the quadrotor in 𝑥, 𝑦 and 𝑧

directions w.r.t 𝑓𝐼 respectively. 𝑀𝑚,𝑞𝜙𝑏 , 𝑀𝑚,𝑞𝜃

𝑏 and 𝑀𝑚,𝑞𝜓 𝑏 are

the interaction moments from the manipulator to the quadrotor

around 𝑥𝑏, 𝑦𝑏 and 𝑧𝑏 axes respectively. 𝐼𝑟 refers to the rotor

inertia. 𝑚 is the mass of the system. Because of the angular

velocity 𝛺𝑖 of rotor 𝑖, a thrust force 𝐹𝑖 and a drag moment 𝑀𝑖

are produced. They are given by:

𝐹𝑖 = 𝐾𝑓𝑖 𝛺𝑖

2, 𝑀𝑖 = 𝐾𝑚𝑖 𝛺𝑖

2 (17)

Where 𝐾𝑓𝑖 and 𝐾𝑚𝑖

are the thrust and drag coefficients

respectively. 𝑇 is the total thrust applied to the quadrotor from

all four rotors and is given by:

𝑇 = ∑𝐹𝑖

4

𝑖=1

(18)

𝜏𝑥, 𝜏𝑦, 𝜏𝑧 are the moments about 𝑥𝑏, 𝑦𝑏 and 𝑧𝑏 respectively. ��

is given by:

�� = 𝛺1 − 𝛺2 + 𝛺3 − 𝛺4 (19)

The relationship between the input total thrust force

(𝑇)/moments (𝜏𝑥, 𝜏𝑦, 𝜏𝑧) and the generated forces-moments for

all rotors are given by:

[

𝑇𝜏𝑥𝜏𝑦

𝜏𝑧

] = [

1 1 10 −𝑑 0

−𝑑0

00

𝑑0

1 0 0𝑑 0 000

0−1

01

0 00 00

−101

]

[ 𝐹1

𝐹2

𝐹3

𝐹4

𝑀1

𝑀2

𝑀3

𝑀4]

(20)



Substitute for 𝐹𝑖 and 𝑀𝑖 in equation (20). Hence, the

relationship between the input total thrust force (𝑇)/moments

(𝜏𝑥, 𝜏𝑦, 𝜏𝑧) and the quadrotor angular velocities is given by:

[

𝑇𝜏𝑥𝜏𝑦

𝜏𝑧

] =

[

𝑘𝑓1

0−𝑙𝑘𝑓1

−𝑘𝑀1

𝑘𝑓2

−𝑙𝑘𝑓2

0𝑘𝑀1

𝑘𝑓3

0𝑙𝑘𝑓3

− 𝑘𝑀3

𝑘𝑓4

𝑙𝑘𝑓4

0𝑘𝑀4]

[ 𝛺1

2

𝛺22

𝛺32

𝛺42]

(21)

Where 𝑙 is the distance between the COM of the quadrotor and

the rotor rotational axis.

IV. TELEOPERATION USING POSITION HOLDING

SCHEME

Before guiding the system autonomously using a vision

toward the object until it becomes accessible to the robot

gripper in the outdoor/indoor environment, driving manually

the aerial manipulation system to make the object in the FOV

of the camera is firstly applied. To do this accurately position

holding scheme is utilized. The position holding task can be

obtained by sending velocity commands to the controller. After

that, a zero-velocity command is sent for position holding and

assign the current position as the desired one (at the instant of

starting sending the command). The main target for the

quadrotor is to track the desired velocity without the need to

reach the target position in order to allow a smooth flight. Fig.2

shows the position holding scheme block diagram.

V. IMAGE BASED VISUAL SERVOING

The main objective behind installing the camera with an end-

effector to form an eye-in-hand configuration is guiding the

QMS autonomously towards the target object until it becomes

accessible to the robot gripper. IBVS utilizing target image

feature points is employed.

To get an accurate autonomous guiding of an aerial

manipulator with IBVS, the target image should be kept inside

of the FOV of the camera. When the camera and the target

object are close to each other, a small motion of the QMS will

make a large deviation to the position of the camera enough to

miss out the target's sight (problem of features leaving the

FOV). To overcome this issue, the trajectory planning

technique is utilized to avoid leaving the object features FOV

of the camera.

Fig. 3. The central-projection model [20].

A. IBVS using image feature points

IBVS is defined as a technique which creates the desired velocity of a camera to move it to the desired position. The

desired feature coordinates in the image plane are used as the

target image features. IBVS task is considered to be

accomplished when the image features match the target

features. This work presents a pinhole CCD camera attached to

the robot's end-effector to make an eye-in-hand configuration.

The object is considered fixed in the environment and is

recognized by 4-feature points located at its four corners.

As shown in Fig.3, the image of a 3D feature point j at world

coordinate 𝑃𝑗 = [𝑋𝑗 𝑌𝑗 𝑍𝑗]𝑇 ∈ ℝ3𝑥1is projected to the

image point 𝑝𝑗 = [𝑢𝑗 𝑣𝑗]𝑇 ∈ ℝ2𝑥1 by :

��𝑗 = C( 𝑇𝐶𝐼 )��𝑗 (22)

Where 𝐶 is the camera projection/calibration matrix and is given by:

Fig. 2. Block diagram of position holding scheme



𝐶 = [𝑓 ⁄ 𝜌𝑤

00

0𝑓 ⁄ 𝜌ℎ

0

𝑢0

𝑣0

1

000] (23)

𝑇𝐶 ∈ ℝ4𝑥4𝐼 is the transformation matrix between 𝑓𝑐 and 𝑓𝐼 . ��𝑗 is the j-th homogeneous pixel coordinate of the world point

𝑃, while ��𝑗 is the j-th homogeneous coordinate of the world

point 𝑃 in meter. 𝑓, (𝑢0 , 𝑣0) and (𝑢 , 𝑣) are a focal length,

principal point, and pixel coordinate respectively. 𝜌𝑤 and 𝜌ℎ are

the width and height of each pixel respectively. When the QMS

moves, the camera moves with velocity

𝑣𝑐 = [ ��𝑐𝑐 𝜔𝑐

𝑐 ]𝑇 ∈ ℝ6𝑥1𝑐 w.r.t 𝑓𝑐. Where ��𝑐𝑐 =

[��𝑐 ��𝑐 ��𝑐]𝑇 ∈ ℝ3𝑥1 and 𝜔𝑐

𝑐 = [𝜔𝑐𝑥 𝜔𝑐𝑦 𝜔𝑐𝑧]𝑇 ∈ ℝ3𝑥1 are the

camera translational and angular velocities w.r.t 𝑓𝑐. In a case, if

there is a world feature point 𝑃 inside of the FOV of the moving camera, its projection to image plane will move with image

velocity �� = [�� ]𝑇 ∈ ℝ2𝑥1. It is worth mentioning that image

jacobian 𝐽𝑝 ∈ ℝ2𝑥6 relates the camera velocity with the

features image velocity and it is given by:

�� = 𝐽𝑝(𝑝, 𝑧𝑑) 𝑣𝑐𝑐 (24)

Where 𝑧𝑑 is the depth of the feature point w.r.t the camera. For

N-Feature points, the equation (24) can be rewritten as:

��𝑠 = 𝐽𝑠(𝑝𝑠 , 𝑧𝑠) 𝑣𝑐𝑐 (25)

𝑝𝑠 ∈ ℝ2𝑁𝑥1 and ��𝑠 ∈ ℝ2𝑁𝑥1 are the set of the image feature

points positions and velocities respectively. 𝑧𝑠 ∈ ℝ𝑁𝑥1 refers

to the stacked image depth and 𝐽𝑠 ∈ ℝ2𝑁𝑥6 refers to the stacked image jacobian matrix.

B. Depth estimation

The accurate feature point depth 𝑧𝑑 estimation is necessary

for calculating image jacobian 𝐽𝑝. Among depth estimation

techniques, online depth estimation (ODE) is utilized. For one image feature point, ODE is calculated from:

(𝐽𝑝𝑡 ��𝑐𝑐 )

1

𝑧𝑑

= �� − 𝐽𝑝𝜔 𝜔𝑐𝑐 (26)

Where 𝐽𝑝𝑡 ∈ ℝ2𝑥3 are the first three columns of the 𝐽𝑝 and it is

affected by the value of the depth 𝑧𝑑. 𝐽𝑝𝜔 ∈ ℝ2𝑥3 are the last

three columns of the 𝐽𝑝 and does not present any information

for depth estimation. For each feature point, its depth 𝑧𝑑 is

estimated as a camera moves.

C. Trajectory Planning

IBVS with trajectory planning is utilized for three main

purposes guiding the system smoothly, overcoming instability

of the system due to initial large feature error and avoiding

leaving the object’s features the FOV of the camera during

autonomous process. Consequently, the target camera velocity

is a time-based function and it is given by:

𝑣𝑐(𝑡) = 𝐽𝑝−1(𝑝(𝑡), 𝑧𝑑(𝑡)) ��(𝑡)𝑐 (27)

Fig. 4. IBVS Block diagram with camera and QMS infinitesimal motions

relationships.

Where 𝑝(𝑡), ��(𝑡) are time-based feature position and velocity

respectively. 𝑧𝑑(𝑡) is a time-based feature depth. For

infinitesimal camera motion, which results in infinitesimal feature motion, the equation (27) becomes:

∆𝜁𝑐𝑐 (𝑡) = 𝐽𝑝

−1(𝑝(𝑡), 𝑧𝑑(𝑡)) (𝑝∗(𝑡) − 𝑝(𝑡)) (28)

Where 𝑝∗(𝑡) is the desired time-based trajectory of a target

image feature position. The upper part (UP) in Fig.4 shows the

block diagram of IBVS, which illustrates estimation of

infinitesimal camera motion ∆𝜁𝑐𝑐 (𝑡) based on the desired

image feature position trajectory 𝑝∗(𝑡).

In this paper, 𝑝∗(𝑡) is chosen to be a fifth-order polynomial

[24] and it is given by:

𝑝∗(𝑡) = 𝑎0 + 𝑎1𝑡 + 𝑎2𝑡2 + 𝑎3𝑡

3 + 𝑎4𝑡4 + 𝑎5𝑡

5 (29)

Where 𝑎𝑖 is a constant real number and 𝑖 = 0,1, ,5. the reason

behind using this kind of trajectory planning is avoiding a discontinuity in acceleration, which leads to an impulse Jerk,

that may excite vibrational modes in the system and reduce

tracking accuracy. Besides, the constraints on the position,

acceleration, and velocity could be specified.

By taking the first and the second derivative of the equation

(29), we can get six equations/constraints (one each for initial

and final image feature positions, velocities, and accelerations).

By arranging these equations, we get:

[ 𝑝∗(𝑡0)��∗(𝑡0)��∗(𝑡0)𝑝∗(𝑡𝑓)

��∗(𝑡𝑓)

��∗(𝑡𝑓)]

=

[ 1 𝑡0 𝑡0

2 𝑡03 𝑡0

4 𝑡05

0 1 2𝑡0 3𝑡02 4𝑡0

3 5𝑡04

0 0 2 6𝑡0 12𝑡02 20𝑡0

3

1 𝑡𝑓 𝑡𝑓2 𝑡𝑓

3 𝑡𝑓4 𝑡𝑓

5

0 1 2𝑡𝑓 3𝑡𝑓2 4𝑡𝑓

3 5𝑡𝑓4

0 0 2 6𝑡𝑓 12𝑡𝑓2 20𝑡𝑓

3]

[ 𝑎0

𝑎1

𝑎2

𝑎3

𝑎4

𝑎5]

(30)

Where 𝑡0, 𝑡𝑓 are the initial and final time. ��∗(𝑡) and ��∗(𝑡) are

the time-based target image feature velocity and acceleration.



VI. SYSTEM VELOCITY DISTRIBUTION

The camera reference velocity ν𝑐∗𝑐 generated by equation

(27), which guides the camera in front of a target object is

distributed into the quadrotor/manipulator joint velocity 𝜁∗. The

camera is set up on the end-effector of the robot that refers to

the relative position between the camera and the gripper as

fixed. The non-changed transformation matrix 𝑇𝑐𝑒𝑒 ∈ ℝ4𝑥4

between 𝑓𝑐 and 𝑓𝑒𝑒 is given by:

𝑇𝑐 = [𝑅𝑐

𝑒𝑒 𝑝𝑐𝑒𝑒

𝑂1𝑥3 1]𝑒𝑒 (31)

Where 𝑅𝑐𝑒𝑒 ∈ ℝ3𝑥3 and 𝑝𝑐 ∈ ℝ3𝑥1𝑒𝑒 are the rotation matrix

and position vector of 𝑓𝑐 w.r.t 𝑓𝑒𝑒. The end-effector velocity

w.r.t 𝑓𝐼 in terms of camera velocity ν𝑐𝑐 is given by:

ν𝑒𝑒𝐼 = 𝐷 ν𝑐

𝑐 (32)

Where 𝐷 = [𝑅𝑐

𝐼 𝑂3𝑥3

𝑂3𝑥3 𝑅𝑐 𝐼

] ∈ ℝ6𝑥6 and 𝑅𝑐𝐼 = 𝑅𝑒𝑒

𝐼 𝑅𝑐 𝑒𝑒 is

the rotation matrix of 𝑓𝑐 w.r.t 𝑓𝐼 . The lower part (LP) in Fig.4

shows the relationship between the infinitesimal motion of the

task space ∆𝜒𝑒𝑒 w.r.t 𝑓𝐼 and the infinitesimal motion of the

camera ∆𝜁𝑐𝑐 w.r.t 𝑓𝑐, which is given by:

∆𝜒𝑒𝑒 = 𝐷 ∆𝜁𝑐𝑐 (33)

By substituting 𝑣𝑒𝑒𝐼 in equation (7), the camera-aerial

manipulation joint velocity relationship is given by :

𝐷 𝑣𝑐∗𝑐 = 𝐽��∗ (34)

Rewriting the equation (33) showing the independent 𝜁 and

dependent 𝜎𝑏 variables as following:

𝜁∗ = 𝐽𝜁−1(𝐷 𝑣𝑐

∗𝑐 − 𝐽𝜎𝜎��) (35)

The infinitesimal camera ∆𝜁𝑐𝑐 is mapped into for six

infinitesimal motion of the QMS joint space variables ∆𝜁

instead of eight. This is because of the under-actuation of the

quadrotor, which is meaning pitch 𝛳 and roll 𝜙 angles are used

as intermediate control inputs for horizontal position control.

The equation (35) becomes:

∆𝜁 = 𝐽𝜁−1(𝐷 ∆𝜁𝑐

𝑐 − 𝐽𝜎∆𝜎𝑏) (36)

VII. CONTROLLER DESIGN

In this section, the controller design of the combined system

based on the PID is described. The controller block diagram of

the proposed QMS shown in Fig.5 illustrates the switching between teleoperation and autonomous process.

For guiding the teleoperation process of the system, the

switches 𝑆3 and 𝑆4 should be closed to send the velocity

commands to the controller. In this case, the system is driven

manually to get close the target object until it becomes in the

Fig. 5. shows a block diagram of a control system of QMS.

camera FOV. The position holding scheme utilized during this

mode of operation is described in section (IV).

For autonomous process, the switches 𝑆1 and 𝑆2 should be

closed. In this case, IBVS described in section (V) is used to drive the system autonomously toward the target object until it

becomes accessible to the robot gripper. Based on the current

pose of the gripper, time-based trajectory of the target image

features and the current image features, the IBVS block

generates the infinitesimal motion of the camera/vision-sensor

w.r.t 𝑓𝑐. The detailed IBVS block diagram is shown in Fig 4.

This camera infinitesimal motion ∆𝜁𝑐𝑐 is converted into the

infinitesimal motion of the end-effector ∆𝜒𝑒𝑒 w.r.t 𝑓𝐼 . Next, these values are applied to the jacobian blocks, that satisfy

equation (36), to generate the infinitesimal motion of the QMS

independent variables ∆𝜁.

By observing the operation of the quadrotor, the movement

in x and y directions depends on the pitch 𝜃 and roll 𝜙 rotation

angles respectively. Therefore, the infinitesimal motions along

x and y directions are controlled through controlling of ∆𝜎𝑏. i.e.

∆𝑥 and ∆𝑦 are applied to the Simplified Nested Controller

(SNC) that generates ∆𝜎𝑏. After that, ∆𝜁 and ∆𝜎𝑏 are applied to the PID block that generates the thrust forces and moments

(𝑇, 𝜏𝑥 , 𝜏𝑦 , 𝜏𝑧) of the quadrotor. The matrix G of the rotor

velocity generation (RVG) block is used to transform these

values into assigned angular velocities of the 4 motors. This

matrix is given by:

𝐺 =

[

𝑘𝑓1

0−𝑙𝑘𝑓1

−𝑘𝑀1

𝑘𝑓2

−𝑙𝑘𝑓2

0𝑘𝑀1

𝑘𝑓3

0𝑙𝑘𝑓3

− 𝑘𝑀3

𝑘𝑓4

𝑙𝑘𝑓4

0𝑘𝑀4]

−1

(37)

The control design criteria are chosen to achieve zero position

error for the 𝑃𝑏𝐼 and 𝜓 as well as joint angles θ1, θ2 and

Consequently, for end-effector variables 𝜒𝑒𝑒 . i.e. zero error for

image features.

A. Vision Controller

IBVS control objective is to achieve zero error between the

desired and current image feature points 𝑒𝑝 = 𝑝∗ − 𝑝. The N-

image feature points obtained from the vision system is defined

as 𝑝𝑠 = [𝑝1, 𝑝2, …… , 𝑝𝑁]𝑇 and the output of the vision



controller is defined as 𝑣𝑐𝑐 . Applying this to equation (24)

yields:

𝑣𝑐𝑐 = 𝐽𝑠

+(𝑝𝑠 , 𝑧𝑠) 𝑝�� (38)

𝐽𝑠+ is the pseudo inverse of the estimated interaction/image

jacobian matrix 𝐽𝑠. In this work, four feature points are used and

in this case, the infinitesimal motion of camera is given by.

∆𝜁𝑐𝑐 (𝑡) = 𝐽𝑠

+(𝑡) 𝑒𝑠(𝑡) (39)

Where 𝑒𝑠 = [𝛥𝑝1 𝛥𝑝2 𝛥𝑝3 𝛥𝑝4]𝑇 is the error stack matrix of the

four image feature points.

B. PID Controller

For two cases, manual and autonomous guiding using the position holding Scheme (PHS) and IBVS, the classical PID

controller is utilized to control the altitude, attitude of the

quadrotor and joint angles of the manipulator. The altitude

controller is given by:

𝑇 =(𝑢𝑧 + 𝑚𝑔)

𝐶𝜓(𝑡)𝐶𝛳(𝑡)

(40)

𝑢𝑧 = 𝑘𝑝𝑧Δz(t) + 𝑘𝑑𝑧Δ��(𝑡) + 𝑘𝐼𝑧 ∫ Δz(t)𝑡

𝑡0

𝑑𝑡 (41)

Where Δz(t) = (z∗(t) − z(t)) and Δz(t) = (z∗(t) − z(t)) are the time-based altitude and altitude derivative error

respectively. z∗(t) and z∗(t) are the time-based desired value

of the altitude and its derivative respectively. 𝑘𝑝𝑧, 𝑘𝑑𝑧 and 𝑘𝐼𝑧

are proportional, derivative, and Integral gains respectively.

Since the motion in x and 𝑦 direction depends on rotation

about the 𝑦 -axis (pitch angle 𝜃) and x-axis (roll angle 𝜙)

respectively. i.e. an SNC loop takes x∗ and y∗ values and

generate 𝜃∗ and 𝜙∗ angles respectively. These controller

equations are given by:

𝜏𝑥 = 𝑘𝑝𝜙𝛥𝜙(𝑡) + 𝑘𝑑𝜙𝛥��(𝑡) + 𝑘𝐼𝜙 ∫ Δ𝜙(t)𝑡

𝑡0

𝑑𝑡 (42)

𝜏𝑦 = 𝑘𝑝𝜃𝛥𝜃(𝑡) + 𝑘𝑑𝜃𝛥��(𝑡) + 𝑘𝐼𝜃 ∫ Δ𝜃(t)𝑡

𝑡0

𝑑𝑡 (43)

𝜃∗(𝑡) = (𝛥𝑥(𝑡)𝐶𝜓(𝑡) + 𝛥𝑦(𝑡)𝑆𝜓(𝑡) + 𝛥��(𝑡)𝐾𝑑𝑥)𝐾𝑝𝑥 (44)

𝜙∗(𝑡) = (𝛥𝑦(𝑡)𝐶𝜓(𝑡) − 𝛥𝑥(𝑡)𝑆𝜓(𝑡) + 𝛥𝑦(𝑡) 𝐾𝑑𝑦)𝐾𝑝𝑦 (45)

Where 𝛥𝜙(𝑡) = (𝜙∗(t) − 𝜙(𝑡)) and 𝛥��(t) = (��∗(t) − ��(t)) are the time-based error in the roll angle and its

derivative respectively. 𝛥𝜃(𝑡) = (𝜃∗(t) − 𝜃(𝑡)) and 𝛥��(𝑡) = (��∗(t) − ��(𝑡)) are time-based error in pitch angle and its

derivative respectively. 𝛥𝑥(𝑡) = (x∗(t) − x(t)) are 𝛥𝑦(𝑡) = (y∗(t) − y(t)) are the time-based error in 𝑥 and 𝑦 positions

respectively. Also 𝜙∗(𝑡), 𝜃∗(𝑡), x∗(𝑡), y∗(𝑡) are the time-based

desired values for roll, pitch, x-position, and y-position

respectively. ��∗(t) and ��∗(t) are the time-based desired value

of the derivative of roll and pitch angles respectively. 𝑘𝑝 , 𝑘𝑑

and 𝑘𝐼 are the proportional, derivative, and Integral gains

respectively.

The PID yaw controller is given by:

𝜏𝜓 = 𝑘𝑝𝜓𝛥𝜓(𝑡) + 𝑘𝑑𝜓𝛥��(𝑡) + 𝑘𝐼𝜓 ∫ Δ𝜓(t)𝑡

𝑡0

𝑑𝑡 (46)

Where 𝛥𝜓(𝑡) = (𝜓∗(t) − 𝜓(𝑡)) and 𝛥��(𝑡) = (��∗(t) −

��(𝑡)) are the time-based yaw angle and its derivative error

respectively. 𝜓∗ and ��∗ are the time-based desired value of the

yaw angle and its derivative respectively. 𝑘𝑝𝜓 , 𝑘𝑑𝜓 and 𝑘𝐼𝜓 are

the proportional, derivative, and Integral gains respectively.

The PID controller for manipulator’s joints is given by:

𝜏𝑚1 = 𝑘𝑝1𝛥𝜃1(𝑡) + 𝑘𝑑1𝛥��1(𝑡) + 𝑘𝐼𝜃1∫ 𝛥𝜃1(𝑡)

𝑡

𝑡0

𝑑𝑡 (47)

𝜏𝑚2 = 𝑘𝑝2𝛥𝜃2(𝑡) + 𝑘𝑑2𝛥��2(𝑡) + 𝑘𝐼𝜃2∫ 𝛥𝜃2(𝑡)

𝑡

𝑡0

𝑑𝑡 (48)

Where Δθ1(t) = (θ1∗( t) − θ1(t)) and Δθ1(t) = (θ1

∗( t) − θ1(t)) are the time based error in the manipulator’s joint angle

1 and its derivative respectively. Δθ2(t) = (θ2∗( t) − θ2(t))

and Δθ2(t) = (θ2∗( t) − θ2(t)) are the time based error in the

manipulator’s joint angle 2 and its derivative respectively. Also

θ1∗( t) and θ2

∗( t) are the time-based desired values of

manipulator’s joint angles 1 and 2 respectively. θ1∗( t) and

θ2∗( t) are the desired time-based derivative values of

manipulator’s joint angles 1 and 2 respectively. 𝑘𝑝𝑖 , 𝑘𝑑𝑖 , 𝑘𝐼𝑖

are proportional, derivative, and integral gains respectively and

𝑖 = 1,2.

VIII. VIRTUAL PROTOTYPING RESULTS AND

DISCUSSION

In this section, the simulation results based on the prescribed

system model are presented. The (3D-CAD) model of the

proposed aerial manipulator robot shown in Fig.1 is imported

to ADAMS® program as shown in Fig.6. Since the ADAMS®

program performs an accurate dynamic simulation with a

realistic results. i.e. the simulation results of the proposed robot will be like a true prototype (Virtual Prototyping). The

characteristics of all system parts such as material, mass, and

inertia are established. In addition, Revolute joints, Fixed joints

are only used. The initial robot position is (0, 0, 0)𝑚 with

respect to inertial/global frame and the gravity is in the negative

𝑍-Direction. The total mass of the robot is 2Kg. The

MATLAB/ADAMS co-simulation is performed with a 0.005

communication interval to test the proposed robot and

controller.

The suggested scenario for the simulation process includes first guiding the robot manually by controlling the position and

orientation of it until the target object becomes in the camera



Fig. 6. The proposed aerial manipulator robot in ADAMS®

FOV. Then, based on visual information, robot autonomous

guiding will be started to approach the same target object until

it becomes accessible to the robot gripper.

In a teleoperation mode, which is a first step before

autonomous mode, Position holding (PH) task is achieved by

sending the desired velocity commands to the controller. For

example, the user can send them by ground-station (joystick,

computer or both). Two types of velocity commands can be sent

non-zero and zero velocity commands. For non-zero velocity

commands, the controller track the desired velocity. For zero

velocity commands, the controller assign the current position as the desired position. It is importance to mention that there are

only six controlled variables (𝑥, 𝑦, 𝑧, 𝜓, 𝜃1, 𝜃2) instead of eight.

This is because of under-actuation of the quadrotor, which

means pitch 𝜃 and roll 𝜙 angles are used as intermediate control

inputs for horizontal position control as illustrated in section

VII. Therefore, up to six velocity commands can be sent. In this

mode of operation, the classical PID controller is utilized. The

MATLAB/SIMULINK and ADAMS® co-simulation starts

with the initial values of the controlled variables, which are

(𝑥 = 0 𝑚, 𝑦 = 0 𝑚, 𝑧 = 0 𝑚, 𝜓 = 0 𝑟𝑎𝑑, 𝜃1 = 1.57 𝑟𝑎𝑑, 𝜃2 =0 𝑟𝑎𝑑). Fig.7 shows the co-simulation results of the robot using

PHC during the teleoperation process. In the time interval 0𝑠 <𝑡 ≤ 10𝑠, 0.05 𝑚/𝑠 and 9 𝑟𝑎𝑑/𝑠 desired velocity commands

are sent to the altitude 𝑧 and a head angle 𝜓 controllers

respectively, while zero velocity commands are sent to the

attitude (𝑥, 𝑦) and manipulator joint angles (𝜃1, 𝜃2) controllers.

As a result, the robot just moves in +ve direction along and

rotate counter clockwise about 𝑧 -axis. In the meanwhile, the

controllers hold the attitude at (𝑥 = 0 𝑚, 𝑦 = 0 𝑚) and the

manipulator joint angles at (𝜃1 = 1.57 𝑟𝑎𝑑, 𝜃2 = 0 𝑟𝑎𝑑).

Then,0.1 𝑚/𝑠 and 0.1 𝑚/𝑠 velocity commands are sent to the

attitude (𝑥, 𝑦) controllers, while zero velocity commands are

sent to the altitude 𝑧, head angle 𝜓 and manipulator joint angles

(𝜃1, 𝜃2) controllers in the time interval 10𝑠 < 𝑡 ≤ 20𝑠 . As a

result, the robot just moves only along +ve 𝑥 and 𝑦-direction.

In the meanwhile, the controllers hold the altitude at (𝑧 =0.5 𝑚), heading angle at (𝜓 = 1.57 𝑟𝑎𝑑/𝑠) and the


Thereafter, In the time interval 20𝑠 < 𝑡 ≤ 30𝑠 , 0.1 𝑚/𝑠 and

−9 𝑟𝑎𝑑/𝑠 desired velocity commands are sent to the altitude 𝑧

and head angle 𝜓 controllers respectively, while zero velocity

commands are sent to attitude (𝑥, 𝑦) and manipulator joint

angles (𝜃1, 𝜃2) controllers. As a result, the robot just moves in

+ve direction along and rotate clockwise about z-axis. In the

meanwhile, the controllers hold the attitude at (𝑥 = 1 𝑚, 𝑦 =1 𝑚) and the joint angles at (𝜃1 = 1.57 𝑟𝑎𝑑, 𝜃2 = 0 𝑟𝑎𝑑).

Afterthat, −0.1 𝑚/𝑠 and −0.1 𝑚/𝑠 velocity commands are

sent to the attitude (𝑥, 𝑦) controllers, while zero velocity

commands are sent to altitude 𝑧 , heading 𝜓 and manipulator

joint angles (𝜃1, 𝜃2) controllers in the time interval 30 < 𝑡 ≤40. As a result, the robot just moves only along -ve 𝑥 and 𝑦-direction. In the meanwhile, the controllers hold the altitude at

(𝑧 = 1.5 𝑚), heading angle at (𝜓 = 0 𝑟𝑎𝑑/𝑠) and the


Finally, In the time interval 40𝑠 < 𝑡 ≤ 50𝑠 , −0.05 𝑚/𝑠

desired velocity command is sent to the altitude 𝑧 controller,

while zero velocity commands are sent to attitude (𝑥, 𝑦),

heading angle 𝜓 and manipulator joint angles (𝜃1, 𝜃2) controllers. As a result, the robot just moves in -ve direction

along z-axis. In the meanwhile, the controllers hold the attitude

at (𝑥 = 0 𝑚, 𝑦 = 0 𝑚), heading angle 𝜓 = 0 and the

manipulator joint angles at (𝜃1 = 1.57 𝑟𝑎𝑑, 𝜃2 = 0 𝑟𝑎𝑑). As

noted, the results of pitch 𝜃 and roll 𝜙 angles show spikes at

𝑡 = 10, 20, 30, 40𝑠. This is because the value of the attitude

linear velocities at these moments change suddenly from non-

zero to zero and vice versa. Also, The manipulator joint angles

(𝜃1, 𝜃2) show spikes at the same moments because they are

affected by vibration around 𝑥 and 𝑦 respectively. it is worth

stating that these spikes are unobtrusive because they have a

small time and an amplitude. Now the four image feature points

of the vertices of the box (target object) become in the FOV of

the camera as shown in Fig.8.

In an autonomous mode, visual information is used to

autonomously guide the robot to reach the desired object until

it becomes accessible to the gripper. Based on that, the vision

controller (IBVS) is utilized for the autonomous robot guidance. Also, the quintic polynomial is applied as the desired

image features for a smooth robot motion, because a velocity

and a sinusoidal acceleration of it has a zero boundary

conditions, which makes it better to avoid a vibrational mode.

As a result, the instability of the system due to the initial large

feature error is eliminated and leaving the object’s features the

FOV of the camera is avoided. Then, the infinitesimal position

and orientation of the camera ∆𝜁𝑐𝑐 are distributed into the robot

infinitesimal motion ∆𝜁. The PID controller is utilized to minimize the error in joint space variables until the actual and

the desired image feature points are matched. Figs.8 to 11

summarize the co-simulation results of the robot using IBVS

during the autonomous process. The MATLAB/SIMULINK

and ADAMS® co-simulation starts with the new initial values

of the robot controlled variables due to the previous

teleoperation process, which are (𝑥 = 0 𝑚, 𝑦 = 0 𝑚, 𝑧 =1 𝑚,𝜓 = 0 𝑟𝑎𝑑, 𝜃1 = 1.57 𝑟𝑎𝑑, 𝜃2 = 0 𝑟𝑎𝑑). The target

object is chosen to be a fixed box, while the image features are

the vertices of this box. The number of feature points are set to be four. Fig.8 shows the desired image feature points.



The quintic polynomial trajectories, which are applied as the

desired four image feature points are shown in Fig.9 and their

duration are 15s of 20s. The additional 5s is to ensure zero

feature error. Fig.10 shows the infinitesimal motion of the

camera and robot joint space variables. the infinitesimal motion

here represents the error. As noted, it converges to zero. During IBVS, the four image feature points converge to their desired

values. In other words, the feature errors gradually degrade to

zero. i.e. the aerial manipulator approaches the desired target

and the target object becomes accessible to robot gripper.

Fig.11 shows the position and orientation of this end-effector

over the period of 20 sec.

IX. CONCLUSION

The combination of the manual and automatic guiding of the

aerial manipulator to meet the real requirements is proposed and

investigated. Accordingly, the non-redundant aerial

manipulator robot, that has maximum mobility with a minimum

number of actuators is presented. It overcomes some problems

such as for overweight, complex controller, and high-power

consumption. Also, the description of this system kinematic and

dynamic is presented. Besides, the teleoperation and

autonomous mode based on PH scheme and IBVS respectively

are described and simulated. During these modes of operation,

a simple PID controller has been utilized. Furthermore, the dynamic Simulations prove the feasibility of the proposed

system with satisfactory results. In future work, the proposed

system will be implemented in the real prototype instead of a

virtual prototype. Also, an adaptive/robust control for this

system after adding payload to the end effector will be utilized.

(a) Actual

(a) Desired Fig. 8. The actual (a) and desired (b) of the four image feature points

respectively

Fig. 7. The aerial manipulator joint space controlled variables during the teleoperation process using position holding scheme.



Fig. 9. The desired quintic polynomial trajectories for the four feature points.

Fig. 10. The infinitesimal/error motion of camera and an aerial manipulator joint space variables.

Fig. 11. The end-effector position and orientation during the autonomous mode.



ACKNOWLEDGMENT

The first author is supported by a scholarship from the Ministry of Higher Education of the Government of Egypt which is

gratefully acknowledged.

REFERENCES

[1] D. Mellinger, Q. Lindsey, M. Shomin, and V. Kumar, “Design, modeling,

estimation and control for aerial grasping and manipulation,” in 2011

IEEE/RSJ International Conference on Intelligent Robots and Systems,

2011, pp. 2668–2673.

[2] J. Willmann, F. Augugliaro, T. Cadalbert, R. D’Andrea, F. Gramazio, and

M. Kohler, “Aerial robotic construction towards a new field of architectural

research,” International Journal of Architectural Computing, vol. 10, no. 3,

pp. 439–459, 2012. [Online]. Available: https://doi.org/10.1260/1478-

0771.10.3.439.

[3] M. D. K. Lindsey, Quentin and Vijay, “Construction with quadrotor

teams,” Autonomous Robots, vol. 33, no. 3, pp. 323–336, 2012.

[4] M. Bisgaard, A. [la Cour-Harbo], and J. D. Bendtsen], “Adaptive control

system for autonomous helicopter slung load operations,” Control

Engineering Practice, vol. 18, no. 7, pp. 800 – 811, 2010, special Issue on

Aerial Robotics. [Online]. Available:

http://www.sciencedirect.com/science/article/pii/S0967066110000341.

[5] F. A. Goodarzi, D. Lee, and T. Lee, “Geometric stabilization of a quadrotor

uav with a payload connected by flexible cable,” in 2014 American Control

Conference, 2014, pp. 4925–4930.

[6] I. Palunko, R. Fierro, and P. Cruz, “Trajectory generation for swing free

maneuvers of a quadrotor with suspended payload: A dynamic

programming approach,” in 2012 IEEE International Conference on

Robotics and Automation, 2012, pp. 2691–2697.

[7] F.-J. K. Michael, Nathan and Vijay, “Cooperative manipulation and

transportation with aerial robots,” Autonomous Robots, vol. 30, no. 1, pp.

73–86, 2011.

[8] M. Bernard, K. Kondak, I. Maza, and A. Ollero, “Autonomous

transportation and deployment with aerial robots for search and rescue

missions,” Journal of Field Robotics, vol. 28, no. 6, pp. 914–931, 2011.

[Online]. Available: https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.

20401.

[9] F. Ruggiero, M. A. Trujillo, R. Cano, H. Ascorbe, A. Viguria, C. Perz, V.

Lippiello, A. Ollero, and B. Siciliano, “A multilayer control for multirotor

uavs equipped with a servo robot arm,” in 2015 IEEE International

Conference on Robotics and Automation (ICRA), 2015, pp. 4014–4020.

[10] F. Pierri, G. Muscio, and F. Caccavale, “An adaptive hierarchical control

for aerial manipulators,” Robotica, vol. 36, no. 10, p. 15271550, 2018.

[11] C. Dube and J. O. Pedro, “Modelling and closed-loop system identification

of a quadrotor-based aerial manipulator,” Journal of Physics: Conference

Series, vol. 1016, p. 012007, may 2018. [Online]. Available:

https://doi.org/10.1088%2F1742-6596%2F1016%2F1%2F012007.

[12] D. Wuthier, D. Kominiak, C. Kanellakis, G. Andrikopoulos, M. Fumagalli,

G. Schipper, and G. Nikolakopoulos, “On the design, modeling and control

of a novel compact aerial manipulator,” in 2016 24th Mediterranean

Conference on Control and Automation (MED), 2016, pp. 665–670.

[13] X. Liu, J. Tan, and H. Zhong, “Design and implementation of task oriented

rotor-flying manipulator*,” in 2018 13th World Congress on Intelligent

Control and Automation (WCICA), 2018, pp. 1120–1125.

[14] A. Guayasamn, P. Leica, M. Herrera, and O. Camacho, “Trajectory

tracking control for aerial manipulator based on lyapunov and sliding mode

control,” in 2018 International Conference on Information Systems and

Computer Science (INCISCOS), 2018, pp. 36–41.

[15] S. Kim, S. Choi, and H. J. Kim, “Aerial manipulation using a quadrotor

with a two dof robotic arm,” in 2013 IEEE/RSJ International Conference

on Intelligent Robots and Systems, 2013, pp. 4990–4995.

[16] H. Beikzadeh and G. Liu, “Trajectory tracking of quadrotor flying

manipulators using l1 adaptive control,” Journal of the Franklin Institute,

vol. 355, no. 14, pp. 6239 – 6261, 2018. [Online]. Available:

http://www.sciencedirect.com/science/article/pii/S0016003218304149.

[17] G. D. Sunil and R. Mohan, “Modeling and control of a uav manipulator,”

in 2019 Fifth Indian Control Conference (ICC), 2019, pp. 454–459.

[18] A. Khalifa, M. Fanni, A. Ramadan, and A. Abo-Ismail, “Modeling and

control of a new quadrotor manipulation system,” in 2012 First

International Conference on Innovative Engineering Systems, 2012, pp.

109–114.

[19] ——, “Adaptive intelligent controller design for a new quadrotor

manipulation system,” in 2013 IEEE International Conference on Systems,

Man, and Cybernetics, 2013, pp. 1666–1671.

[20] C. Peter, Robotics, vision, and control: fundamental algorithms in

MATLAB. Springer, 2017.

[21] S. Kim, H. Seo, S. Choi, and H. J. Kim, “Vision-guided aerial manipulation

using a multirotor with a robotic arm,” IEEE/ASME Transactions on

Mechatronics, vol. 21, no. 4, pp. 1912–1923, 2016.

[22] A. Santamaria-Navarro, P. Grosch, V. Lippiello, J. Sol, and J. Andrade

Cetto, “Uncalibrated visual servo for unmanned aerial manipulation,”

IEEE/ASME Transactions on Mechatronics, vol. 22, no. 4, pp. 1610–1621,

2017.

[23] M. Fanni and A. Khalifa, “A new 6-dof quadrotor manipulation system:

Design, kinematics, dynamics, and control,” IEEE/ASME Transactions on

Mechatronics, vol. 22, no. 3, pp. 1315–1326, 2017.

[24] M. V. Mark W. Spong, Seth Hutchinson, Robot Modeling and Control.

Wiley, 2006.

https://doi.org/10.1260/1478-0771.10.3.439

https://doi.org/10.1260/1478-0771.10.3.439

http://www.sciencedirect.com/science/article/pii/S0967066110000341

https://onlinelibrary.wiley.com/doi/abs/10.1002/rob

http://www.sciencedirect.com/science/article/pii/S0016003218304149

hybrid guidance of quadrotor manipulation system for indoor...

Documents