# Rotor-Flying Manipulator: Modeling, Analysis, and Control

Authors: Bin Yang; Yuqing He; Jianda Han; Guangjun Liu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Equipping multijoint manipulators on a mobile robot is a typical redesign scheme to make the latter be able to actively influence the surroundings and has been extensively used for many ground robots, underwater robots, and space robotic systems. However, the rotor-flying robot (RFR) is difficult to be made such redesign. This is mainly because the motion of the manipulator will bring heavy coupling between itself and the RFR system, which makes the system model highly complicated and the controller design difficult. Thus, in this paper, the modeling, analysis, and control of the combined system, called rotor-flying multijoint manipulator (RF-MJM), are conducted. Firstly, the detailed dynamics model is constructed and analyzed. Subsequently, a full-state feedback linear quadratic regulator (LQR) controller is designed through obtaining linearized model near steady state. Finally, simulations are conducted and the results are analyzed to show the basic control performance.


---

## Body

## 1. Introduction

Rotor-flying robot (RFR) has been researched for several decades and achieved great development. To date, RFRs have shown their priority in many applications, such as search, rescue, and surveillance [1–6]. However, these applications are often passive. That means the RFR system can’t manipulate the interested objects by a direct physical interaction.Most recently, this problem has attained more and more attentions from many researches. For example, some researchers suggest to equip a gripper on the RFR system so that the RFR system can grasp as shown Figure1 [7–9]. But it still has some disadvantages including that (
1
) the manipulation can only be implemented through controlling the attitude of the RFR system. However, the precise control of the RFR system is difficult due to its complicated dynamics, and the precise manipulation is impossible. (
2
) With this structure, in order to manipulate an object, the RFR must approach it. This, however, may both bring the so-called ground effect and “blow” the object, which makes the precise control much more difficult.RFR system designed by Yale University (a) and DLR (b).
(a)
(b)In this paper, a new system structure as shown in Figure2 is proposed. The system is composed of an RFR system and a multijoint manipulator and thus called rotor-flying multijoint manipulator (RF-MJM). Compared to the structure in Figure 1, the multiple-joint manipulator can be used to regulate position and attitude of the end-gripper. This is very useful to compensate the control imprecision of the RFR system and makes precise manipulation much easier.Figure 2
Sketch of the RF-MJM system.However, it is obvious that the system shown in Figure2 is very difficult to be controlled. This is mainly because the RFR system itself is very sensitive to the external disturbance (force/moment), especially for the time varying disturbance, for example, from a moving manipulator as the new proposed RF-MJM system. Thus, it is very important and necessary for us to study the detailed model that can describe the coupling between the RFR and the manipulator. What is more, to construct a full-state high fidelity dynamics model is also a benefit for optimizing the design parameters, for example, the mass, the joint number, and the configuration of the manipulator, and testing the designed control algorithms.Thus in this paper, the dynamics model of the RF-MJM system is constructed and analyzed to show its basic performance. Moreover, the linear LQR controller is designed to test the basic closed loop performance of it. The main contributions of this paper are as the following three aspects:(
1
) a detailed full-state high fidelity nonlinear dynamics model of the RF-MJM system is constructed through using Euler-Lagrange method; (
2
) the dynamics couplings between the RFR and the manipulator are analyzed in detail, which is good for the optimal design of the system configuration; (
3
) LQR controller is designed based on the linearized system model and simulations are conducted to show the basic control performance of the new proposed system.


## 2. Dynamics Model of RF-MJM

The dynamics model of the RF-MJM is composed of three parts as shown in Figure3: the body dynamics model, the mid-dynamics model, and the actuator model. The body dynamics model describes the relation between the motion state and the external force/torque exerting on the body of the robot; the mid-dynamics model represents how the force/torque is produced, that is, the aerodynamics of the RFR, and the torque from the manipulator joint motor, while the actuator model depicts the dynamical characteristic of the actuator, for example, the motors of the manipulator and the steering engine of the RFR. For the RF-MJM system, the coupling between the RFR and the manipulator will mainly influence the body dynamics model, which, then, will be discussed in this paper.Figure 3
Model structure of the RF-MJM system.A RF-MJM system is actually a multilink system shown as in Figure4, where the cube denotes the RFR and the ellipsoids denote the link of the manipulator; Σ
0, Σ
I, Σ
E, and Σ
i are RFR body-fixed reference frame, earth-fixed inertial frame, end-gripper frame, and the frame of the ith joint of manipulator; J
i
(
i
=
1,2
,
…
,
n
) denote the joint of the manipulator; p
i denotes its position vector in the frame of Σ
I; C
0 and C
i are the position vector of the center of mass (COM) of the link RFR and the link i; d
0 and d
i are the position vectors of C
0 and C
i; a
i is the vector from J
i to C
i; b
0 is the vector from COM of the RFR to the first joint; b
i is vector from C
i to J
i
+
1; n is the number of the manipulator’s link.Figure 4
System structure of the RF-MJM system.


### 2.1. Kinematics Model

In this paper, we suppose that both the RFR system and the manipulator are rigid. Thus, the following geometric relations are satisfied:(1)
d
i
=
d
0
+
b
0
+
∑
k
=
1
i
-
1
(
a
k
+
b
k
)
+
a
i
.
Differentiate it with respect to time and we have
(2)
v
i
=
d
˙
i
=
v
0
+
ω
0
×
(
d
i
-
d
0
)
+
∑
k
=
1
i
{
k
k
×
(
d
i
-
p
k
)
θ
˙
˙
k
}
,
where v
0 and v
i are the linear velocity of the COM of the RFR and the link i, respectively; ω
0 is the angular velocity of the RFR in the frame of Σ
I; θ
k is the angular position vector of the kth joint; k
k denotes the unit vector of the axis Z
i of the ith joint frame; d
i, p
k represent the position vectors of the COM of the link i and the kth joint. The angular velocity of the ith joint can be denoted as
(3)
ω
i
=
ω
0
+
∑
k
=
1
i
k
k
θ
˙
k
.Combine (2) and (3), and the kinematic model of the RF-MJM system is
(4)
[
v
i
ω
i
]
=
J
-
b
i
[
v
0
ω
0
]
+
J
-
m
i
Θ
˙
,
where J
-
b
i is the Jacobian matrix of the RFR system and has the following form:
(5)
J
-
b
i
=
[
E
-
d
~
0
i
0
E
]
+
[
J
-
b
v
i
J
-
b
ω
i
]
.
In (5), E is the unity matrix with proper dimension:
(6)
d
0
i
=
d
i
-
d
0
=
[
d
0
i
,
x
d
0
i
,
y
d
0
i
,
z
]
T
and d
~
0
i is the skew-symmetric matrix of the vector d
0
i; that is,
(7)
d
~
0
i
=
[
0
-
d
0
i
,
z
d
0
i
,
y
d
0
i
,
z
0
-
d
0
i
,
x
-
d
0
i
,
y
d
0
i
,
x
0
]
.J
-
m
i in (4) is the Jacobian matrix of the manipulator system defined as
(8)
J
-
m
i
=
[
k
1
×
(
d
i
-
p
1
)
⋯
k
i
×
(
d
i
-
p
i
)
k
1
⋯
k
i
]
=
[
J
-
m
v
i
J
-
m
ω
i
]
.Also, we can transform the linear velocity of the RFRv
0 into the velocity in the RFR body frame; that is,
(9)
v
0
=
[
c
θ
c
φ
s
ϕ
s
θ
c
φ
-
c
ϕ
s
φ
c
ϕ
s
θ
c
φ
+
s
ϕ
s
φ
c
θ
s
φ
c
ϕ
c
φ
+
s
θ
s
ϕ
s
φ
c
ϕ
s
θ
s
φ
-
s
ϕ
c
φ
-
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
]
[
u
v
w
]
,
where c and s mean trigonometric function cos and sin, respectively; Φ
=
[
ϕ
,
θ
,
ψ
]
T represents the attitude of the RFR; [
u
,
v
,
w
] is the linear velocity of the RFR in the RFR body fixed reference frame.Similarly, the relation between the angular velocity in the frame ofΣ
I and that in the body frame is as follows:
(10)
ω
0
=
[
1
sin
ϕ
tan
θ
cos
⁡
ϕ
tan
θ
0
cos
⁡
ϕ
-
sin
ϕ
0
sin
ϕ
sec
θ
cos
⁡
ϕ
sec
θ
]
[
p
q
r
]
,
where p, q, and r are the components of the angular velocity along the axes of the RFR body fixed reference frame.


### 2.2. Dynamics Model

In this section, the dynamics model of the RF-MJM system will be deduced using Euler-Lagrange method.


#### 2.2.1. Kinetic Energy

Firstly, the kinetic energy of the system can be denoted as(11)
E
k
=
1
2
∑
i
=
1
n
(
ω
i
T
I
i
ω
i
+
m
i
v
i
T
v
i
)
,
where m
i and I
i are the mass and the inertia tensor of the ith part[10].Substitute (2) and (3) into (11), we have
(12)
E
k
=
1
2
[
v
0
ω
0
Θ
˙
]
T
[
ϖ
E
ϖ
d
~
0
g
T
J
T
ω
ϖ
d
~
0
g
H
ω
H
ω
ϕ
J
T
ω
T
H
ω
ϕ
T
H
m
]
[
v
0
ω
0
Θ
˙
]
=
1
2
[
v
0
ω
0
Θ
˙
]
T
H
[
v
0
ω
0
Θ
˙
]
,
where H is called the inertia matrix of the RF-MJM system with
(13)
H
m
=
∑
i
=
1
n
(
J
R
i
T
I
J
R
i
+
m
i
J
T
i
T
J
T
i
)
J
T
ω
=
∑
i
=
1
n
(
m
i
J
T
i
)
H
ω
=
∑
i
=
1
n
(
I
i
+
m
i
d
~
0
i
T
d
~
0
i
)
+
I
0
H
ω
ϕ
=
∑
i
=
1
n
(
I
i
J
R
i
+
m
i
d
~
0
i
T
J
T
i
)
J
R
i
=
[
k
1
k
2
⋯
k
i
0
⋯
0
]
J
T
i
=
[
k
1
×
(
d
i
-
p
1
)
k
2
×
(
d
i
-
p
2
)
⋯
k
i
×
(
d
i
-
p
i
)
0
⋯
0
]
,
d
g
=
∑
1
=
0
n
m
i
d
i
∑
1
=
0
n
m
i
.


#### 2.2.2. Potential Energy

In this part, the potential energy due to the gravity will be deduced. Firstly, based on Figure2, (1) can be rewritten as
(14)
d
i
=
d
0
+
∑
k
=
1
i
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
C
k
,
k
I
)
,
where A
0
I and A
k
I denote the coordinate transformation matrix from the frame Σ
0 and the frame Σ
k to the frame Σ
I, respectively; C
k
,
k is the position vector from joint i to the COM of the ith partdenoted in the ith joint coordinate system.With (14), the potential energy due to gravity of the RF-MJM system can be easily obtained as follows:
(15)
E
p
=
-
∑
i
=
0
n
m
i
G
T
d
i
=
∑
i
=
0
n
m
i
G
T
(
d
0
+
∑
k
=
1
i
-
1
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
-
1
I
C
k
,
k
)
)
,
where
(16)
G
=
[
0
0
g
]
and g is the acceleration due to gravity.


#### 2.2.3. Dynamics Model

The Euler-Lagrangian equation of the RF-MJM system is(17)
L
=
E
k
-
E
p
.From (12), the kinetic energy can be rewritten as
(18)
E
k
=
1
2
[
ϖ
v
0
T
E
v
0
+
ϖ
ω
0
T
d
~
0
g
v
0
+
Θ
˙
T
J
T
ω
T
v
0
GGGG
+
ϖ
v
0
T
d
~
0
g
T
ω
0
+
ω
0
T
H
ω
ω
0
+
Θ
˙
T
H
ω
ϕ
T
ω
0
GGGG
+
v
0
T
J
T
ω
Θ
˙
+
ω
0
T
H
ω
ϕ
Θ
˙
+
Θ
˙
T
H
m
Θ
˙
]
.
Thus we have(19)
∂
E
k
∂
q
˙
=
[
∂
E
k
∂
v
0
∂
E
k
∂
ω
0
∂
E
k
∂
Θ
˙
]
=
[
ϖ
E
v
0
+
ϖ
d
~
0
g
T
ω
0
+
J
T
ω
Θ
˙
H
ω
ω
0
+
ϖ
d
~
0
g
v
0
+
H
ω
ϕ
Θ
˙
H
m
Θ
˙
+
H
ω
ϕ
T
ω
0
+
J
T
ω
T
v
0
]
,
(20)
d
d
t
(
∂
E
k
∂
q
˙
)
=
[
d
d
t
(
∂
E
k
∂
v
0
)
d
d
t
(
∂
E
k
∂
ω
0
)
d
d
t
(
∂
E
k
∂
Θ
˙
)
]
=
[
ϖ
E
v
˙
0
+
ϖ
d
~
˙
0
g
T
ω
0
+
ϖ
d
~
0
g
T
ω
˙
0
+
J
T
ω
Θ
¨
+
J
˙
T
ω
Θ
˙
H
˙
ω
ω
0
+
H
ω
ω
˙
0
+
ϖ
d
~
˙
0
g
v
0
+
ϖ
d
~
0
g
v
˙
0
+
H
˙
ω
ϕ
Θ
˙
+
H
ω
ϕ
Θ
¨
H
˙
m
Θ
˙
+
H
m
Θ
¨
+
H
˙
ω
ϕ
T
ω
0
+
H
ω
ϕ
T
ω
˙
0
+
J
˙
T
ω
T
v
0
+
J
T
ω
T
v
˙
0
]
,
(21)
∂
E
k
∂
q
=
[
∂
E
a
∂
p
+
∂
E
b
∂
p
+
∂
E
c
∂
p
∂
E
a
∂
Φ
+
∂
E
b
∂
Φ
+
∂
E
c
∂
Φ
∂
E
a
∂
Θ
+
∂
E
b
∂
Θ
+
∂
E
c
∂
Θ
]
,where
(22)
q
=
[
X
Φ
Θ
]
,
q
˙
=
[
v
0
ω
0
Θ
˙
]
are the position vector and the velocity vector of the RF-MJM system, respectively; X is the position vector of the RFR; and
(23)
E
a
=
ϖ
ω
0
T
d
~
0
g
v
0
;
E
b
=
1
2
ω
0
T
H
ω
ω
0
;
E
c
=
(
v
0
T
J
T
ω
+
ω
0
T
H
ω
ϕ
+
1
2
Θ
˙
T
H
m
)
Θ
˙
.Similarly, with respect to the potential energy term, we have(24)
∂
E
p
∂
X
=
∑
i
=
0
n
m
i
[
∂
g
T
d
0
∂
x
0
∂
g
T
d
0
∂
y
0
∂
g
T
d
0
∂
z
0
]
,
(25)
∂
E
p
∂
Θ
=
-
(
∑
i
=
0
n
m
i
∂
g
T
v
v
v
v
v
v
×
(
d
0
+
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
)
)
×
(
∂
Θ
)
-
1
,
(26)
∂
E
p
∂
Φ
=
-
[
∂
E
p
∂
ϕ
∂
E
p
∂
θ
∂
E
p
∂
φ
]
=
-
[
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
ϕ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
θ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
φ
]
.Thus, the dynamics model of the RF-MJM system can be obtained through the following Euler-Lagrange equation:
(27)
d
d
t
∂
E
k
∂
q
˙
-
∂
E
k
∂
q
+
∂
E
P
∂
q
=
τ
.Substitute (20)–(21) and (24)–(26) into (27) and after simplifying we can obtain the following dynamics model of the RF-MJM system:
(28)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
m
]
=
[
F
p
τ
m
]
,
where
(29)
X
¨
b
=
[
v
˙
0
ω
˙
0
]
.
C
b and C
m are the Coriolis and centrifugal force of the system; G
b and G
m are the force due to gravity;
(30)
F
p
=
[
F
B
M
B
]
;
F
B
=
[
F
x
F
y
F
z
]
;
M
B
=
[
L
M
N
]
are the force and moment produced by the RFR; the other terms are defined as follows:
(31)
H
b
=
[
H
b
11
H
b
12
H
b
21
H
b
22
]
H
b
11
=
ϖ
E
;
H
b
12
=
ϖ
d
~
T
o
g
;
H
b
21
=
ϖ
d
~
o
g
;
H
b
22
=
H
ω
H
b
m
=
[
H
b
m
v
H
b
m
ω
]
2
×
1
H
b
m
v
=
J
T
ω
;
H
b
m
ω
=
H
ω
ϕ
C
b
=
[
C
b
v
C
b
ω
]
C
b
v
=
ϖ
d
~
˙
0
g
T
ω
0
+
J
˙
T
ω
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
X
v
0
-
v
0
T
∂
J
T
ω
∂
X
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
X
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
X
ω
0
+
Θ
˙
T
∂
H
m
∂
X
Θ
˙
)
C
b
ω
=
H
˙
ω
ω
0
+
ϖ
d
~
˙
0
g
v
0
+
H
˙
ω
ϕ
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
Φ
v
0
-
v
0
T
∂
J
T
ω
∂
Φ
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
Φ
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
Φ
ω
0
+
Θ
˙
T
∂
H
m
∂
Φ
Θ
˙
)
.
C
m
=
1
2
J
˙
T
ω
T
v
0
+
1
2
v
0
T
J
˙
T
ω
+
1
2
H
˙
ω
ϕ
T
ω
0
+
1
2
ω
0
T
H
˙
ω
ϕ
+
H
˙
m
Θ
˙
.


#### 2.2.4. Extended Dynamics Model

When the manipulator contacts some external objects, the dynamics model (28) becomes
(32)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
b
]
=
[
F
p
τ
m
]
+
[
J
b
T
J
m
T
]
F
e
,
where F
e is the force and torque exerting on the end of the manipulator; J
b and J
m are the Jacobian matrix defined as
(33)
J
b
=
[
E
-
p
~
0
i
0
E
]
;
p
0
i
=
p
i
-
d
0
;
J
m
=
[
k
1
×
(
p
e
-
p
1
)
k
2
×
(
p
e
-
p
2
)
⋯
k
n
×
(
p
e
-
p
n
)
k
1
k
2
⋯
k
n
]
.Furthermore, if we consider the aerodynamics of the RFR system, the force and moment produced by the RFRF
p can be denoted as the following mathematical equations [11]:
(34)
F
X
=
-
T
M
sin
a
1
s
;
F
Y
=
T
M
sin
b
1
s
-
T
T
;
F
Z
=
-
T
M
cos
⁡
a
1
s
cos
⁡
b
1
s
;
L
=
-
(
∂
L
M
∂
b
1
s
)
b
1
s
-
Q
M
sin
a
1
s
;
M
=
(
∂
M
M
∂
a
1
s
)
a
1
s
-
Q
M
sin
b
1
s
-
Q
T
;
N
=
-
Q
M
cos
⁡
a
1
s
cos
⁡
b
1
s
,
where T
M and T
T are the forces derived from the main rotor and tail rotor of the RFR, and a
1
s and b
1
s stand for the longitudinal and lateral flapping angle of main rotor, respectively; the forces T
M and T
T and the moments Q
M and Q
T can be calculated as [12, 13].Up to now, we have constructed the nonlinear dynamics model of the RF-MJM system.


## 2.1. Kinematics Model

In this paper, we suppose that both the RFR system and the manipulator are rigid. Thus, the following geometric relations are satisfied:(1)
d
i
=
d
0
+
b
0
+
∑
k
=
1
i
-
1
(
a
k
+
b
k
)
+
a
i
.
Differentiate it with respect to time and we have
(2)
v
i
=
d
˙
i
=
v
0
+
ω
0
×
(
d
i
-
d
0
)
+
∑
k
=
1
i
{
k
k
×
(
d
i
-
p
k
)
θ
˙
˙
k
}
,
where v
0 and v
i are the linear velocity of the COM of the RFR and the link i, respectively; ω
0 is the angular velocity of the RFR in the frame of Σ
I; θ
k is the angular position vector of the kth joint; k
k denotes the unit vector of the axis Z
i of the ith joint frame; d
i, p
k represent the position vectors of the COM of the link i and the kth joint. The angular velocity of the ith joint can be denoted as
(3)
ω
i
=
ω
0
+
∑
k
=
1
i
k
k
θ
˙
k
.Combine (2) and (3), and the kinematic model of the RF-MJM system is
(4)
[
v
i
ω
i
]
=
J
-
b
i
[
v
0
ω
0
]
+
J
-
m
i
Θ
˙
,
where J
-
b
i is the Jacobian matrix of the RFR system and has the following form:
(5)
J
-
b
i
=
[
E
-
d
~
0
i
0
E
]
+
[
J
-
b
v
i
J
-
b
ω
i
]
.
In (5), E is the unity matrix with proper dimension:
(6)
d
0
i
=
d
i
-
d
0
=
[
d
0
i
,
x
d
0
i
,
y
d
0
i
,
z
]
T
and d
~
0
i is the skew-symmetric matrix of the vector d
0
i; that is,
(7)
d
~
0
i
=
[
0
-
d
0
i
,
z
d
0
i
,
y
d
0
i
,
z
0
-
d
0
i
,
x
-
d
0
i
,
y
d
0
i
,
x
0
]
.J
-
m
i in (4) is the Jacobian matrix of the manipulator system defined as
(8)
J
-
m
i
=
[
k
1
×
(
d
i
-
p
1
)
⋯
k
i
×
(
d
i
-
p
i
)
k
1
⋯
k
i
]
=
[
J
-
m
v
i
J
-
m
ω
i
]
.Also, we can transform the linear velocity of the RFRv
0 into the velocity in the RFR body frame; that is,
(9)
v
0
=
[
c
θ
c
φ
s
ϕ
s
θ
c
φ
-
c
ϕ
s
φ
c
ϕ
s
θ
c
φ
+
s
ϕ
s
φ
c
θ
s
φ
c
ϕ
c
φ
+
s
θ
s
ϕ
s
φ
c
ϕ
s
θ
s
φ
-
s
ϕ
c
φ
-
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
]
[
u
v
w
]
,
where c and s mean trigonometric function cos and sin, respectively; Φ
=
[
ϕ
,
θ
,
ψ
]
T represents the attitude of the RFR; [
u
,
v
,
w
] is the linear velocity of the RFR in the RFR body fixed reference frame.Similarly, the relation between the angular velocity in the frame ofΣ
I and that in the body frame is as follows:
(10)
ω
0
=
[
1
sin
ϕ
tan
θ
cos
⁡
ϕ
tan
θ
0
cos
⁡
ϕ
-
sin
ϕ
0
sin
ϕ
sec
θ
cos
⁡
ϕ
sec
θ
]
[
p
q
r
]
,
where p, q, and r are the components of the angular velocity along the axes of the RFR body fixed reference frame.


## 2.2. Dynamics Model

In this section, the dynamics model of the RF-MJM system will be deduced using Euler-Lagrange method.


### 2.2.1. Kinetic Energy

Firstly, the kinetic energy of the system can be denoted as(11)
E
k
=
1
2
∑
i
=
1
n
(
ω
i
T
I
i
ω
i
+
m
i
v
i
T
v
i
)
,
where m
i and I
i are the mass and the inertia tensor of the ith part[10].Substitute (2) and (3) into (11), we have
(12)
E
k
=
1
2
[
v
0
ω
0
Θ
˙
]
T
[
ϖ
E
ϖ
d
~
0
g
T
J
T
ω
ϖ
d
~
0
g
H
ω
H
ω
ϕ
J
T
ω
T
H
ω
ϕ
T
H
m
]
[
v
0
ω
0
Θ
˙
]
=
1
2
[
v
0
ω
0
Θ
˙
]
T
H
[
v
0
ω
0
Θ
˙
]
,
where H is called the inertia matrix of the RF-MJM system with
(13)
H
m
=
∑
i
=
1
n
(
J
R
i
T
I
J
R
i
+
m
i
J
T
i
T
J
T
i
)
J
T
ω
=
∑
i
=
1
n
(
m
i
J
T
i
)
H
ω
=
∑
i
=
1
n
(
I
i
+
m
i
d
~
0
i
T
d
~
0
i
)
+
I
0
H
ω
ϕ
=
∑
i
=
1
n
(
I
i
J
R
i
+
m
i
d
~
0
i
T
J
T
i
)
J
R
i
=
[
k
1
k
2
⋯
k
i
0
⋯
0
]
J
T
i
=
[
k
1
×
(
d
i
-
p
1
)
k
2
×
(
d
i
-
p
2
)
⋯
k
i
×
(
d
i
-
p
i
)
0
⋯
0
]
,
d
g
=
∑
1
=
0
n
m
i
d
i
∑
1
=
0
n
m
i
.


### 2.2.2. Potential Energy

In this part, the potential energy due to the gravity will be deduced. Firstly, based on Figure2, (1) can be rewritten as
(14)
d
i
=
d
0
+
∑
k
=
1
i
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
C
k
,
k
I
)
,
where A
0
I and A
k
I denote the coordinate transformation matrix from the frame Σ
0 and the frame Σ
k to the frame Σ
I, respectively; C
k
,
k is the position vector from joint i to the COM of the ith partdenoted in the ith joint coordinate system.With (14), the potential energy due to gravity of the RF-MJM system can be easily obtained as follows:
(15)
E
p
=
-
∑
i
=
0
n
m
i
G
T
d
i
=
∑
i
=
0
n
m
i
G
T
(
d
0
+
∑
k
=
1
i
-
1
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
-
1
I
C
k
,
k
)
)
,
where
(16)
G
=
[
0
0
g
]
and g is the acceleration due to gravity.


### 2.2.3. Dynamics Model

The Euler-Lagrangian equation of the RF-MJM system is(17)
L
=
E
k
-
E
p
.From (12), the kinetic energy can be rewritten as
(18)
E
k
=
1
2
[
ϖ
v
0
T
E
v
0
+
ϖ
ω
0
T
d
~
0
g
v
0
+
Θ
˙
T
J
T
ω
T
v
0
GGGG
+
ϖ
v
0
T
d
~
0
g
T
ω
0
+
ω
0
T
H
ω
ω
0
+
Θ
˙
T
H
ω
ϕ
T
ω
0
GGGG
+
v
0
T
J
T
ω
Θ
˙
+
ω
0
T
H
ω
ϕ
Θ
˙
+
Θ
˙
T
H
m
Θ
˙
]
.
Thus we have(19)
∂
E
k
∂
q
˙
=
[
∂
E
k
∂
v
0
∂
E
k
∂
ω
0
∂
E
k
∂
Θ
˙
]
=
[
ϖ
E
v
0
+
ϖ
d
~
0
g
T
ω
0
+
J
T
ω
Θ
˙
H
ω
ω
0
+
ϖ
d
~
0
g
v
0
+
H
ω
ϕ
Θ
˙
H
m
Θ
˙
+
H
ω
ϕ
T
ω
0
+
J
T
ω
T
v
0
]
,
(20)
d
d
t
(
∂
E
k
∂
q
˙
)
=
[
d
d
t
(
∂
E
k
∂
v
0
)
d
d
t
(
∂
E
k
∂
ω
0
)
d
d
t
(
∂
E
k
∂
Θ
˙
)
]
=
[
ϖ
E
v
˙
0
+
ϖ
d
~
˙
0
g
T
ω
0
+
ϖ
d
~
0
g
T
ω
˙
0
+
J
T
ω
Θ
¨
+
J
˙
T
ω
Θ
˙
H
˙
ω
ω
0
+
H
ω
ω
˙
0
+
ϖ
d
~
˙
0
g
v
0
+
ϖ
d
~
0
g
v
˙
0
+
H
˙
ω
ϕ
Θ
˙
+
H
ω
ϕ
Θ
¨
H
˙
m
Θ
˙
+
H
m
Θ
¨
+
H
˙
ω
ϕ
T
ω
0
+
H
ω
ϕ
T
ω
˙
0
+
J
˙
T
ω
T
v
0
+
J
T
ω
T
v
˙
0
]
,
(21)
∂
E
k
∂
q
=
[
∂
E
a
∂
p
+
∂
E
b
∂
p
+
∂
E
c
∂
p
∂
E
a
∂
Φ
+
∂
E
b
∂
Φ
+
∂
E
c
∂
Φ
∂
E
a
∂
Θ
+
∂
E
b
∂
Θ
+
∂
E
c
∂
Θ
]
,where
(22)
q
=
[
X
Φ
Θ
]
,
q
˙
=
[
v
0
ω
0
Θ
˙
]
are the position vector and the velocity vector of the RF-MJM system, respectively; X is the position vector of the RFR; and
(23)
E
a
=
ϖ
ω
0
T
d
~
0
g
v
0
;
E
b
=
1
2
ω
0
T
H
ω
ω
0
;
E
c
=
(
v
0
T
J
T
ω
+
ω
0
T
H
ω
ϕ
+
1
2
Θ
˙
T
H
m
)
Θ
˙
.Similarly, with respect to the potential energy term, we have(24)
∂
E
p
∂
X
=
∑
i
=
0
n
m
i
[
∂
g
T
d
0
∂
x
0
∂
g
T
d
0
∂
y
0
∂
g
T
d
0
∂
z
0
]
,
(25)
∂
E
p
∂
Θ
=
-
(
∑
i
=
0
n
m
i
∂
g
T
v
v
v
v
v
v
×
(
d
0
+
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
)
)
×
(
∂
Θ
)
-
1
,
(26)
∂
E
p
∂
Φ
=
-
[
∂
E
p
∂
ϕ
∂
E
p
∂
θ
∂
E
p
∂
φ
]
=
-
[
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
ϕ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
θ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
φ
]
.Thus, the dynamics model of the RF-MJM system can be obtained through the following Euler-Lagrange equation:
(27)
d
d
t
∂
E
k
∂
q
˙
-
∂
E
k
∂
q
+
∂
E
P
∂
q
=
τ
.Substitute (20)–(21) and (24)–(26) into (27) and after simplifying we can obtain the following dynamics model of the RF-MJM system:
(28)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
m
]
=
[
F
p
τ
m
]
,
where
(29)
X
¨
b
=
[
v
˙
0
ω
˙
0
]
.
C
b and C
m are the Coriolis and centrifugal force of the system; G
b and G
m are the force due to gravity;
(30)
F
p
=
[
F
B
M
B
]
;
F
B
=
[
F
x
F
y
F
z
]
;
M
B
=
[
L
M
N
]
are the force and moment produced by the RFR; the other terms are defined as follows:
(31)
H
b
=
[
H
b
11
H
b
12
H
b
21
H
b
22
]
H
b
11
=
ϖ
E
;
H
b
12
=
ϖ
d
~
T
o
g
;
H
b
21
=
ϖ
d
~
o
g
;
H
b
22
=
H
ω
H
b
m
=
[
H
b
m
v
H
b
m
ω
]
2
×
1
H
b
m
v
=
J
T
ω
;
H
b
m
ω
=
H
ω
ϕ
C
b
=
[
C
b
v
C
b
ω
]
C
b
v
=
ϖ
d
~
˙
0
g
T
ω
0
+
J
˙
T
ω
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
X
v
0
-
v
0
T
∂
J
T
ω
∂
X
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
X
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
X
ω
0
+
Θ
˙
T
∂
H
m
∂
X
Θ
˙
)
C
b
ω
=
H
˙
ω
ω
0
+
ϖ
d
~
˙
0
g
v
0
+
H
˙
ω
ϕ
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
Φ
v
0
-
v
0
T
∂
J
T
ω
∂
Φ
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
Φ
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
Φ
ω
0
+
Θ
˙
T
∂
H
m
∂
Φ
Θ
˙
)
.
C
m
=
1
2
J
˙
T
ω
T
v
0
+
1
2
v
0
T
J
˙
T
ω
+
1
2
H
˙
ω
ϕ
T
ω
0
+
1
2
ω
0
T
H
˙
ω
ϕ
+
H
˙
m
Θ
˙
.


### 2.2.4. Extended Dynamics Model

When the manipulator contacts some external objects, the dynamics model (28) becomes
(32)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
b
]
=
[
F
p
τ
m
]
+
[
J
b
T
J
m
T
]
F
e
,
where F
e is the force and torque exerting on the end of the manipulator; J
b and J
m are the Jacobian matrix defined as
(33)
J
b
=
[
E
-
p
~
0
i
0
E
]
;
p
0
i
=
p
i
-
d
0
;
J
m
=
[
k
1
×
(
p
e
-
p
1
)
k
2
×
(
p
e
-
p
2
)
⋯
k
n
×
(
p
e
-
p
n
)
k
1
k
2
⋯
k
n
]
.Furthermore, if we consider the aerodynamics of the RFR system, the force and moment produced by the RFRF
p can be denoted as the following mathematical equations [11]:
(34)
F
X
=
-
T
M
sin
a
1
s
;
F
Y
=
T
M
sin
b
1
s
-
T
T
;
F
Z
=
-
T
M
cos
⁡
a
1
s
cos
⁡
b
1
s
;
L
=
-
(
∂
L
M
∂
b
1
s
)
b
1
s
-
Q
M
sin
a
1
s
;
M
=
(
∂
M
M
∂
a
1
s
)
a
1
s
-
Q
M
sin
b
1
s
-
Q
T
;
N
=
-
Q
M
cos
⁡
a
1
s
cos
⁡
b
1
s
,
where T
M and T
T are the forces derived from the main rotor and tail rotor of the RFR, and a
1
s and b
1
s stand for the longitudinal and lateral flapping angle of main rotor, respectively; the forces T
M and T
T and the moments Q
M and Q
T can be calculated as [12, 13].Up to now, we have constructed the nonlinear dynamics model of the RF-MJM system.


## 2.2.1. Kinetic Energy

Firstly, the kinetic energy of the system can be denoted as(11)
E
k
=
1
2
∑
i
=
1
n
(
ω
i
T
I
i
ω
i
+
m
i
v
i
T
v
i
)
,
where m
i and I
i are the mass and the inertia tensor of the ith part[10].Substitute (2) and (3) into (11), we have
(12)
E
k
=
1
2
[
v
0
ω
0
Θ
˙
]
T
[
ϖ
E
ϖ
d
~
0
g
T
J
T
ω
ϖ
d
~
0
g
H
ω
H
ω
ϕ
J
T
ω
T
H
ω
ϕ
T
H
m
]
[
v
0
ω
0
Θ
˙
]
=
1
2
[
v
0
ω
0
Θ
˙
]
T
H
[
v
0
ω
0
Θ
˙
]
,
where H is called the inertia matrix of the RF-MJM system with
(13)
H
m
=
∑
i
=
1
n
(
J
R
i
T
I
J
R
i
+
m
i
J
T
i
T
J
T
i
)
J
T
ω
=
∑
i
=
1
n
(
m
i
J
T
i
)
H
ω
=
∑
i
=
1
n
(
I
i
+
m
i
d
~
0
i
T
d
~
0
i
)
+
I
0
H
ω
ϕ
=
∑
i
=
1
n
(
I
i
J
R
i
+
m
i
d
~
0
i
T
J
T
i
)
J
R
i
=
[
k
1
k
2
⋯
k
i
0
⋯
0
]
J
T
i
=
[
k
1
×
(
d
i
-
p
1
)
k
2
×
(
d
i
-
p
2
)
⋯
k
i
×
(
d
i
-
p
i
)
0
⋯
0
]
,
d
g
=
∑
1
=
0
n
m
i
d
i
∑
1
=
0
n
m
i
.


## 2.2.2. Potential Energy

In this part, the potential energy due to the gravity will be deduced. Firstly, based on Figure2, (1) can be rewritten as
(14)
d
i
=
d
0
+
∑
k
=
1
i
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
C
k
,
k
I
)
,
where A
0
I and A
k
I denote the coordinate transformation matrix from the frame Σ
0 and the frame Σ
k to the frame Σ
I, respectively; C
k
,
k is the position vector from joint i to the COM of the ith partdenoted in the ith joint coordinate system.With (14), the potential energy due to gravity of the RF-MJM system can be easily obtained as follows:
(15)
E
p
=
-
∑
i
=
0
n
m
i
G
T
d
i
=
∑
i
=
0
n
m
i
G
T
(
d
0
+
∑
k
=
1
i
-
1
(
A
k
-
1
I
C
k
-
1
,
k
-
A
k
-
1
I
C
k
,
k
)
)
,
where
(16)
G
=
[
0
0
g
]
and g is the acceleration due to gravity.


## 2.2.3. Dynamics Model

The Euler-Lagrangian equation of the RF-MJM system is(17)
L
=
E
k
-
E
p
.From (12), the kinetic energy can be rewritten as
(18)
E
k
=
1
2
[
ϖ
v
0
T
E
v
0
+
ϖ
ω
0
T
d
~
0
g
v
0
+
Θ
˙
T
J
T
ω
T
v
0
GGGG
+
ϖ
v
0
T
d
~
0
g
T
ω
0
+
ω
0
T
H
ω
ω
0
+
Θ
˙
T
H
ω
ϕ
T
ω
0
GGGG
+
v
0
T
J
T
ω
Θ
˙
+
ω
0
T
H
ω
ϕ
Θ
˙
+
Θ
˙
T
H
m
Θ
˙
]
.
Thus we have(19)
∂
E
k
∂
q
˙
=
[
∂
E
k
∂
v
0
∂
E
k
∂
ω
0
∂
E
k
∂
Θ
˙
]
=
[
ϖ
E
v
0
+
ϖ
d
~
0
g
T
ω
0
+
J
T
ω
Θ
˙
H
ω
ω
0
+
ϖ
d
~
0
g
v
0
+
H
ω
ϕ
Θ
˙
H
m
Θ
˙
+
H
ω
ϕ
T
ω
0
+
J
T
ω
T
v
0
]
,
(20)
d
d
t
(
∂
E
k
∂
q
˙
)
=
[
d
d
t
(
∂
E
k
∂
v
0
)
d
d
t
(
∂
E
k
∂
ω
0
)
d
d
t
(
∂
E
k
∂
Θ
˙
)
]
=
[
ϖ
E
v
˙
0
+
ϖ
d
~
˙
0
g
T
ω
0
+
ϖ
d
~
0
g
T
ω
˙
0
+
J
T
ω
Θ
¨
+
J
˙
T
ω
Θ
˙
H
˙
ω
ω
0
+
H
ω
ω
˙
0
+
ϖ
d
~
˙
0
g
v
0
+
ϖ
d
~
0
g
v
˙
0
+
H
˙
ω
ϕ
Θ
˙
+
H
ω
ϕ
Θ
¨
H
˙
m
Θ
˙
+
H
m
Θ
¨
+
H
˙
ω
ϕ
T
ω
0
+
H
ω
ϕ
T
ω
˙
0
+
J
˙
T
ω
T
v
0
+
J
T
ω
T
v
˙
0
]
,
(21)
∂
E
k
∂
q
=
[
∂
E
a
∂
p
+
∂
E
b
∂
p
+
∂
E
c
∂
p
∂
E
a
∂
Φ
+
∂
E
b
∂
Φ
+
∂
E
c
∂
Φ
∂
E
a
∂
Θ
+
∂
E
b
∂
Θ
+
∂
E
c
∂
Θ
]
,where
(22)
q
=
[
X
Φ
Θ
]
,
q
˙
=
[
v
0
ω
0
Θ
˙
]
are the position vector and the velocity vector of the RF-MJM system, respectively; X is the position vector of the RFR; and
(23)
E
a
=
ϖ
ω
0
T
d
~
0
g
v
0
;
E
b
=
1
2
ω
0
T
H
ω
ω
0
;
E
c
=
(
v
0
T
J
T
ω
+
ω
0
T
H
ω
ϕ
+
1
2
Θ
˙
T
H
m
)
Θ
˙
.Similarly, with respect to the potential energy term, we have(24)
∂
E
p
∂
X
=
∑
i
=
0
n
m
i
[
∂
g
T
d
0
∂
x
0
∂
g
T
d
0
∂
y
0
∂
g
T
d
0
∂
z
0
]
,
(25)
∂
E
p
∂
Θ
=
-
(
∑
i
=
0
n
m
i
∂
g
T
v
v
v
v
v
v
×
(
d
0
+
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
)
)
×
(
∂
Θ
)
-
1
,
(26)
∂
E
p
∂
Φ
=
-
[
∂
E
p
∂
ϕ
∂
E
p
∂
θ
∂
E
p
∂
φ
]
=
-
[
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
ϕ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
θ
-
∑
i
=
1
n
m
i
∂
g
T
∑
j
=
1
i
(
A
j
-
1
I
*
C
j
-
1
,
j
-
A
j
I
*
C
j
,
j
)
∂
φ
]
.Thus, the dynamics model of the RF-MJM system can be obtained through the following Euler-Lagrange equation:
(27)
d
d
t
∂
E
k
∂
q
˙
-
∂
E
k
∂
q
+
∂
E
P
∂
q
=
τ
.Substitute (20)–(21) and (24)–(26) into (27) and after simplifying we can obtain the following dynamics model of the RF-MJM system:
(28)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
m
]
=
[
F
p
τ
m
]
,
where
(29)
X
¨
b
=
[
v
˙
0
ω
˙
0
]
.
C
b and C
m are the Coriolis and centrifugal force of the system; G
b and G
m are the force due to gravity;
(30)
F
p
=
[
F
B
M
B
]
;
F
B
=
[
F
x
F
y
F
z
]
;
M
B
=
[
L
M
N
]
are the force and moment produced by the RFR; the other terms are defined as follows:
(31)
H
b
=
[
H
b
11
H
b
12
H
b
21
H
b
22
]
H
b
11
=
ϖ
E
;
H
b
12
=
ϖ
d
~
T
o
g
;
H
b
21
=
ϖ
d
~
o
g
;
H
b
22
=
H
ω
H
b
m
=
[
H
b
m
v
H
b
m
ω
]
2
×
1
H
b
m
v
=
J
T
ω
;
H
b
m
ω
=
H
ω
ϕ
C
b
=
[
C
b
v
C
b
ω
]
C
b
v
=
ϖ
d
~
˙
0
g
T
ω
0
+
J
˙
T
ω
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
X
v
0
-
v
0
T
∂
J
T
ω
∂
X
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
X
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
X
ω
0
+
Θ
˙
T
∂
H
m
∂
X
Θ
˙
)
C
b
ω
=
H
˙
ω
ω
0
+
ϖ
d
~
˙
0
g
v
0
+
H
˙
ω
ϕ
Θ
˙
-
ϖ
ω
0
T
∂
d
~
0
g
∂
Φ
v
0
-
v
0
T
∂
J
T
ω
∂
Φ
Θ
˙
-
ω
0
T
∂
H
ω
ϕ
∂
Φ
Θ
˙
-
1
2
(
ω
0
T
∂
H
ω
∂
Φ
ω
0
+
Θ
˙
T
∂
H
m
∂
Φ
Θ
˙
)
.
C
m
=
1
2
J
˙
T
ω
T
v
0
+
1
2
v
0
T
J
˙
T
ω
+
1
2
H
˙
ω
ϕ
T
ω
0
+
1
2
ω
0
T
H
˙
ω
ϕ
+
H
˙
m
Θ
˙
.


## 2.2.4. Extended Dynamics Model

When the manipulator contacts some external objects, the dynamics model (28) becomes
(32)
[
H
b
H
b
m
H
b
m
T
H
m
]
[
X
¨
b
Θ
¨
]
+
[
C
b
C
m
]
+
[
G
b
G
b
]
=
[
F
p
τ
m
]
+
[
J
b
T
J
m
T
]
F
e
,
where F
e is the force and torque exerting on the end of the manipulator; J
b and J
m are the Jacobian matrix defined as
(33)
J
b
=
[
E
-
p
~
0
i
0
E
]
;
p
0
i
=
p
i
-
d
0
;
J
m
=
[
k
1
×
(
p
e
-
p
1
)
k
2
×
(
p
e
-
p
2
)
⋯
k
n
×
(
p
e
-
p
n
)
k
1
k
2
⋯
k
n
]
.Furthermore, if we consider the aerodynamics of the RFR system, the force and moment produced by the RFRF
p can be denoted as the following mathematical equations [11]:
(34)
F
X
=
-
T
M
sin
a
1
s
;
F
Y
=
T
M
sin
b
1
s
-
T
T
;
F
Z
=
-
T
M
cos
⁡
a
1
s
cos
⁡
b
1
s
;
L
=
-
(
∂
L
M
∂
b
1
s
)
b
1
s
-
Q
M
sin
a
1
s
;
M
=
(
∂
M
M
∂
a
1
s
)
a
1
s
-
Q
M
sin
b
1
s
-
Q
T
;
N
=
-
Q
M
cos
⁡
a
1
s
cos
⁡
b
1
s
,
where T
M and T
T are the forces derived from the main rotor and tail rotor of the RFR, and a
1
s and b
1
s stand for the longitudinal and lateral flapping angle of main rotor, respectively; the forces T
M and T
T and the moments Q
M and Q
T can be calculated as [12, 13].Up to now, we have constructed the nonlinear dynamics model of the RF-MJM system.


## 3. Analysis of Dynamics Model and Linearization

### 3.1. Analysis of the Dynamics Model

In order to understand the coupling between the RFR and manipulator clearly, we rewrite the system model (28) as follows:
(35)
[
H
b
(
Φ
,
Θ
)
H
b
m
(
Φ
,
Θ
)
H
b
m
T
(
Φ
,
Θ
)
H
m
(
Φ
,
Θ
)
]
[
X
¨
b
Θ
¨
]
+
[
C
b
(
v
0
,
ω
0
,
Θ
,
Φ
,
Φ
˙
)
C
m
(
v
0
,
ω
0
,
Θ
,
Φ
,
Φ
˙
)
]
+
[
G
b
(
X
,
Θ
,
Φ
)
G
b
(
Θ
,
Φ
)
]
=
[
F
p
τ
m
]
.
From (35), it can be easily seen that the coupling between the RFR and the manipulator appears in all the terms except for the exerting force/moment. That means the RF-MJM system model is more complicated than the RFR system model as shown in [11]. These can be summarized as follows. (1)
Compared to the RFR model, there are some new terms in the system (28), such as C
b
v. These new terms make the system model more complicated, and the result is that some control algorithm that has been shown to be fit for RFR system cannot be used directly in the RF-MJM system. For example, in reference [11], a RFR system is proved to be approximate feedback linearizable. This, unfortunately, cannot be implemented in the RF-MJM system.
(2)
The system structure of the RF-MJM is of great complication compared to the RFR system. This can be easily seen through the preceding system equations. Again, the reason is because of the coupling between the RFR and the manipulator. The higher complication results in heavier nonlinearity which makes the controller design of the RF-MJM systemextraordinarily challenging.


### 3.2. Linearization and LQR Controller Design

In the above section, we have obtained the detailed mathematical model of the RF-MJM system, which can be easily computed and simulated through using symbolic computation toolbox of MATLAB. However, this kind of controller is difficult to be used due to the high complexity and nonlinearities. Thus, in this section, we will try to find the linearized model of the RF-MJM system and analyze the influence of the parameters on the system parameters.The linearization can be implemented through the following steps: firstly, search the trim point of the RF-MJM system; secondly, compute the derivatives of the system model with respect to the state and input to obtain the system matrix of the desired linear model. With the linearized model, some linear controller design strategies, for example, the LQR controller, can be used to stabilize the original nonlinear system. In the following content of this section, taking one-joint RF-MJM system as an example, the linearization and the LQR control design will be conducted and system performance will be analyzed.A trim point, also known as an equilibrium point, of a nonlinear system is a point in the state space of a dynamic system, and at this point, the derivatives of the states with respect to time are precisely zeros.The state vector of the RF-MJM can be denoted as(36)
[
x
y
z
ϕ
θ
ψ
u
v
w
p
q
r
Θ
Θ
˙
]
and the input vector is
(37)
[
a
1
s
b
1
s
θ
M
θ
T
τ
Θ
]
,
where a
1
s and b
1
s are the cyclic pitch angle of the main rotor, respectively; θ
M and θ
T are the collective pitch angle of the main rotor and the tilt rotor, respectively.Based on the definition of trim point, all the velocity state should be set to zeros; that is,(38)
u
=
v
=
w
=
p
=
q
=
r
=
Θ
˙
=
0
.Also, all the derivatives of the states with respect to time should be zeros. Under condition (38), this is equivalent to
(39)
[
X
¨
b
Θ
¨
]
=
H
-
1
(
[
F
p
τ
m
]
+
[
J
b
T
J
m
T
]
F
e
-
[
C
b
C
m
]
-
[
G
b
G
b
]
)
=
0
7
×
1
.The right-hand side of (31) is only related toϕ
,
θ
,
ψ
,
Θ, and input vector, so we have 9 free variables and 7 equalities. Furthermore, if we define τ
Θ
=
0 and ψ
=
0, we will have only 7 free variables. Thus, the trim point can be obtained directly by solving the nonlinear equalities (39), which can be easily conducted using some searching function in MATLAB.In order to evaluate the influence of the mass of the manipulator on the whole system, we list out the trim point of the linearization system with different manipulator masses (the parameters of the system are listed out in Table1, and the trim point is in Table 2 in the next section).Table 1
Parameters of RF-MJM system.
Parameter
Describe
Unit
m
0 = 9.5
The mass of rotor-flying robot (RFR)
kg
m
1 = 2.5
The mass of manipulator
kg
I
0
x
x = 0.1634
Moment of inertia of RFR
kgm2
I
0
y
y = 0.5782
Moment of inertia of RFR
kgm2
I
0
z
z = 0.6306
Moment of inertia of RFR
kgm2
I
1
x
x = 0.1399
Moment of inertia of manipulator
kgm2
I
1
y
y = 0.1399
Moment of inertia of manipulator
kgm2
I
1
z
z = 0.00112
Moment of inertia of manipulator
kgm2
l
0 = 0.3
The length from the centroid of RFR
m
l
1 = 0.4
The length of half of the first link
m
ϕ = 0.0769
Roll angle
rad
θ = 0.0211
Pitch angle
rad
ψ = any value
Yaw angle
rad
θ
1 = 0.0211
Joint movement angle
rad
F
0
=
[
0
0
0
]
T
External force
N
T
0
=
[
0
0
0
]
T
External torque
NmTable 2
Trim points corresponding to different masses of manipulator.
Trim point
m
1 = 2.5 kg
m
1 = 2 kg
m
1 = 1.5 kg
m
1 = 1 kg
m
1 = 0.5 kg
ϕ
0.0769
0.0732
0.0697
0.0662
0.0626
θ
0.0211
0.0204
0.0197
0.0190
0.0183
θ
1
0.0211
0.0205
0.0198
0.0191
0.0183
θ
M
0.0436
0.0409
0.0381
0.0354
0.0326
θ
T
−0.1282
−0.1245
−0.1207
−0.1169
−0.1130
a
1
s
0.0211
0.0204
0.0197
0.0190
0.0183
b
1
s
0.0195
0.0169
0.0145
0.0123
0.0103The linearization system model is(40)
Δ
X
˙
=
A
Δ
X
+
B
Δ
u
,
where
(41)
Δ
X
=
X
-
X
trim
A
=
[
0
3
×
3
E
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
1
A
1
A
2
A
3
A
4
A
5
A
6
0
3
×
3
0
3
×
3
0
3
×
3
E
0
3
×
1
0
3
×
1
A
7
A
8
A
9
A
10
A
11
A
12
0
1
×
3
0
1
×
3
0
1
×
3
0
1
×
3
0
1
A
13
A
14
A
15
A
16
A
17
A
18
]
B
=
[
0
5
×
3
B
1
T
0
5
×
3
B
2
T
0
5
×
1
B
3
T
]
T
.Furthermore, in order to analyze the performance of system (40), the eigenvalues of A matrix are given in Figure 5. From it, we can get the following results.(1)
The whole system is static-instable since it has positive eigenvalues.
(2)
With increase of the manipulator mass, the distribution of eigenvalues will be more diverging.Figure 5
Eigenvalue distribution ofA matrix with different manipulator masses.


### 3.3. LQR Controller Design

Next, we will design the full state-feedback linear quadratic regulation (LQR) controller for the RFM system. In the state-feedback version of the LQR problem [14], we assume that the whole state x can be measured and therefore it is available to control. Solution to the optimal state-feedback LQR problem is to find u
(
t
)
=
-
K
x
(
t
) that minimizes
(42)
J
LQR
=
∫
0
∞
(
x
T
Q
x
+
u
T
R
u
)
d
t
,
where K is given by K
=
R
-
1
B
T
P and P is found by solving some continuous time algebraic Riccati equations. So we can easily get the eigenvalues of the open loop system and the closed loop system by the matrices A and A
-
B
K, respectively.


## 3.1. Analysis of the Dynamics Model

In order to understand the coupling between the RFR and manipulator clearly, we rewrite the system model (28) as follows:
(35)
[
H
b
(
Φ
,
Θ
)
H
b
m
(
Φ
,
Θ
)
H
b
m
T
(
Φ
,
Θ
)
H
m
(
Φ
,
Θ
)
]
[
X
¨
b
Θ
¨
]
+
[
C
b
(
v
0
,
ω
0
,
Θ
,
Φ
,
Φ
˙
)
C
m
(
v
0
,
ω
0
,
Θ
,
Φ
,
Φ
˙
)
]
+
[
G
b
(
X
,
Θ
,
Φ
)
G
b
(
Θ
,
Φ
)
]
=
[
F
p
τ
m
]
.
From (35), it can be easily seen that the coupling between the RFR and the manipulator appears in all the terms except for the exerting force/moment. That means the RF-MJM system model is more complicated than the RFR system model as shown in [11]. These can be summarized as follows. (1)
Compared to the RFR model, there are some new terms in the system (28), such as C
b
v. These new terms make the system model more complicated, and the result is that some control algorithm that has been shown to be fit for RFR system cannot be used directly in the RF-MJM system. For example, in reference [11], a RFR system is proved to be approximate feedback linearizable. This, unfortunately, cannot be implemented in the RF-MJM system.
(2)
The system structure of the RF-MJM is of great complication compared to the RFR system. This can be easily seen through the preceding system equations. Again, the reason is because of the coupling between the RFR and the manipulator. The higher complication results in heavier nonlinearity which makes the controller design of the RF-MJM systemextraordinarily challenging.


## 3.2. Linearization and LQR Controller Design

In the above section, we have obtained the detailed mathematical model of the RF-MJM system, which can be easily computed and simulated through using symbolic computation toolbox of MATLAB. However, this kind of controller is difficult to be used due to the high complexity and nonlinearities. Thus, in this section, we will try to find the linearized model of the RF-MJM system and analyze the influence of the parameters on the system parameters.The linearization can be implemented through the following steps: firstly, search the trim point of the RF-MJM system; secondly, compute the derivatives of the system model with respect to the state and input to obtain the system matrix of the desired linear model. With the linearized model, some linear controller design strategies, for example, the LQR controller, can be used to stabilize the original nonlinear system. In the following content of this section, taking one-joint RF-MJM system as an example, the linearization and the LQR control design will be conducted and system performance will be analyzed.A trim point, also known as an equilibrium point, of a nonlinear system is a point in the state space of a dynamic system, and at this point, the derivatives of the states with respect to time are precisely zeros.The state vector of the RF-MJM can be denoted as(36)
[
x
y
z
ϕ
θ
ψ
u
v
w
p
q
r
Θ
Θ
˙
]
and the input vector is
(37)
[
a
1
s
b
1
s
θ
M
θ
T
τ
Θ
]
,
where a
1
s and b
1
s are the cyclic pitch angle of the main rotor, respectively; θ
M and θ
T are the collective pitch angle of the main rotor and the tilt rotor, respectively.Based on the definition of trim point, all the velocity state should be set to zeros; that is,(38)
u
=
v
=
w
=
p
=
q
=
r
=
Θ
˙
=
0
.Also, all the derivatives of the states with respect to time should be zeros. Under condition (38), this is equivalent to
(39)
[
X
¨
b
Θ
¨
]
=
H
-
1
(
[
F
p
τ
m
]
+
[
J
b
T
J
m
T
]
F
e
-
[
C
b
C
m
]
-
[
G
b
G
b
]
)
=
0
7
×
1
.The right-hand side of (31) is only related toϕ
,
θ
,
ψ
,
Θ, and input vector, so we have 9 free variables and 7 equalities. Furthermore, if we define τ
Θ
=
0 and ψ
=
0, we will have only 7 free variables. Thus, the trim point can be obtained directly by solving the nonlinear equalities (39), which can be easily conducted using some searching function in MATLAB.In order to evaluate the influence of the mass of the manipulator on the whole system, we list out the trim point of the linearization system with different manipulator masses (the parameters of the system are listed out in Table1, and the trim point is in Table 2 in the next section).Table 1
Parameters of RF-MJM system.
Parameter
Describe
Unit
m
0 = 9.5
The mass of rotor-flying robot (RFR)
kg
m
1 = 2.5
The mass of manipulator
kg
I
0
x
x = 0.1634
Moment of inertia of RFR
kgm2
I
0
y
y = 0.5782
Moment of inertia of RFR
kgm2
I
0
z
z = 0.6306
Moment of inertia of RFR
kgm2
I
1
x
x = 0.1399
Moment of inertia of manipulator
kgm2
I
1
y
y = 0.1399
Moment of inertia of manipulator
kgm2
I
1
z
z = 0.00112
Moment of inertia of manipulator
kgm2
l
0 = 0.3
The length from the centroid of RFR
m
l
1 = 0.4
The length of half of the first link
m
ϕ = 0.0769
Roll angle
rad
θ = 0.0211
Pitch angle
rad
ψ = any value
Yaw angle
rad
θ
1 = 0.0211
Joint movement angle
rad
F
0
=
[
0
0
0
]
T
External force
N
T
0
=
[
0
0
0
]
T
External torque
NmTable 2
Trim points corresponding to different masses of manipulator.
Trim point
m
1 = 2.5 kg
m
1 = 2 kg
m
1 = 1.5 kg
m
1 = 1 kg
m
1 = 0.5 kg
ϕ
0.0769
0.0732
0.0697
0.0662
0.0626
θ
0.0211
0.0204
0.0197
0.0190
0.0183
θ
1
0.0211
0.0205
0.0198
0.0191
0.0183
θ
M
0.0436
0.0409
0.0381
0.0354
0.0326
θ
T
−0.1282
−0.1245
−0.1207
−0.1169
−0.1130
a
1
s
0.0211
0.0204
0.0197
0.0190
0.0183
b
1
s
0.0195
0.0169
0.0145
0.0123
0.0103The linearization system model is(40)
Δ
X
˙
=
A
Δ
X
+
B
Δ
u
,
where
(41)
Δ
X
=
X
-
X
trim
A
=
[
0
3
×
3
E
3
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
1
A
1
A
2
A
3
A
4
A
5
A
6
0
3
×
3
0
3
×
3
0
3
×
3
E
0
3
×
1
0
3
×
1
A
7
A
8
A
9
A
10
A
11
A
12
0
1
×
3
0
1
×
3
0
1
×
3
0
1
×
3
0
1
A
13
A
14
A
15
A
16
A
17
A
18
]
B
=
[
0
5
×
3
B
1
T
0
5
×
3
B
2
T
0
5
×
1
B
3
T
]
T
.Furthermore, in order to analyze the performance of system (40), the eigenvalues of A matrix are given in Figure 5. From it, we can get the following results.(1)
The whole system is static-instable since it has positive eigenvalues.
(2)
With increase of the manipulator mass, the distribution of eigenvalues will be more diverging.Figure 5
Eigenvalue distribution ofA matrix with different manipulator masses.


## 3.3. LQR Controller Design

Next, we will design the full state-feedback linear quadratic regulation (LQR) controller for the RFM system. In the state-feedback version of the LQR problem [14], we assume that the whole state x can be measured and therefore it is available to control. Solution to the optimal state-feedback LQR problem is to find u
(
t
)
=
-
K
x
(
t
) that minimizes
(42)
J
LQR
=
∫
0
∞
(
x
T
Q
x
+
u
T
R
u
)
d
t
,
where K is given by K
=
R
-
1
B
T
P and P is found by solving some continuous time algebraic Riccati equations. So we can easily get the eigenvalues of the open loop system and the closed loop system by the matrices A and A
-
B
K, respectively.


## 4. Simulations

In this section, simulations will be conducted using the preceding nonlinear system model and the LQR controller. In the simulation, the manipulator’s mass is 2.5 kg, and the other parameters are given in Table1.And the trim point of the whole system is listed out in Table2.With these parameters, the system matricesA and B are as follows:
(43)
A
=
[
0
3
×
3
E
0
3
×
3
0
3
×
3
0
3
×
1
0
3
×
1
0
3
×
3
0
3
×
3
A
-
3
0
3
×
3
A
-
5
0
3
×
1
0
3
×
3
0
3
×
3
0
3
×
3
E
0
3
×
1
0
3
×
1
0
3
×
3
0
3
×
3
A
-
9
0
3
×
3
A
-
11
0
3
×
1
0
1
×
3
0
1
×
3
0
1
×
3
0
1
×
3
0
1
0
1
×
3
0
1
×
3
A
-
15
0
1
×
3
A
-
17
0
]
B
=
[
0
14
×
3
B
-
1
T
0
14
×
3
B
-
2
T
0
14
×
1
B
-
3
T
]
T
,
where
(44)
A
-
3
=
[
0.0015
9.7636
0.9945
-
9.8000
-
0.0005
0.2102
0.0001
0.0005
0.0014
]
;
A
-
9
=
[
-
0.0087
-
0.0083
0.1834
-
0.0130
0.0255
-
0.8587
-
0.2013
-
0.2456
-
0.0635
]
;
A
-
5
=
[
3.5861
-
0.0077
0.0310
]
;
A
-
11
=
[
-
0.0547
-
24.4857
-
0.8988
]
;
A
-
15
=
[
0.0110
-
0.0924
2.6951
]
;
A
-
17
=
40.1487
;
B
-
1
T
=
[
-
0.5451
-
7.9183
-
147.7390
-
1.2434
-
9.3478
0.6158
-
21.7522
0.4534
-
0.2015
-
0.5829
7.2377
-
0.7650
0.2990
-
0.0006
0.0009
]
;
B
-
2
T
=
[
16.7653
-
2.5282
168.3611
-
32.8659
14.0449
125.4196
20.9082
178.3726
4.0958
-
121.2649
11.0285
-
0.2925
0.0011
0.6932
-
0.0534
]
;
B
-
3
T
=
[
-
3.9371
-
9.0113
-
99.1817
-
4.2009
3.4137
]
.From these equations, it can be seen that the coupling between the manipulator and RFR, denoted byA
-
5, A
-
11, A
-
15, and A
-
17 is heavy. Using the following QR parameters in the LQR controller,
(45)
Q
=
diag
⁡
(
1,1
,
1,1
,
1,1
,
10,10,10,10,10,10,10,10
)
;
R
=
diag
⁡
(
1000,1000,1000,1000,1000
)
,
the LQR feedback control law is designed as
(46)
u
=
K
Δ
x
K
=
[
K
1
K
2
K
3
K
4
K
5
]
,
where
(47)
K
1
=
[
0.0097
-
0.0037
-
0.0226
0.0119
0.0058
0.0210
-
0.0232
-
0.0014
0.0013
-
0.0018
0.0308
-
0.0066
0.0149
-
0.0006
-
0.0008
]
K
2
=
[
0.0200
-
0.0071
-
0.0291
0.0238
0.0116
0.0287
-
0.0544
-
0.0019
0.0031
-
0.0042
0.0575
-
0.0079
0.0308
-
0.0013
-
0.0016
]
K
3
=
[
0.0502
0.1489
0.1317
-
0.0816
0.1670
0.1279
0.0053
-
0.4953
-
0.3462
-
0.3754
-
0.0384
-
0.0293
0.0104
0.2401
0.0991
]
K
4
=
[
0.0169
0.0637
0.0751
-
0.0226
0.0663
0.0827
-
0.0015
-
0.2717
-
0.0288
-
0.1208
-
0.0214
-
0.0058
0.0040
0.1085
0.0073
]
K
5
=
[
0.6994
0.1310
0.5973
0.1125
-
4.2835
-
0.8102
-
0.3735
-
0.0691
1.1293
0.2157
]
.With the controller (46), the system can be stabilized with acceptable performance near the trim point, and the simulation results with initial state (0.1, 0, −0.1, 0, 0, 0, 0.07, 0.04, 0, 0, 0, 0.03, 0) are shown in Figure 6.States and inputs profile under LQR controller (46).
(a)
(b)
(c)
(d)
(e)
(f)From Figure6, it can be seen that a linear LQR controller can stabilize the whole system near the trim point. However, the stabilizing region of LQR is very limited; we have tested that only when the attitude of the whole system satisfies the following conditions (all the initial velocities are set to zeros), the LQR control is effective (46):
(48)
-
0.1389
≤
θ
≤
0.3911
-
0.6731
≤
ϕ
≤
0.5469
-
0.0619
≤
θ
1
≤
0.0611
.Now in the next simulation, two periodic sinusoidal signals, as disturbances with different frequencies, are added to the input of the manipulator to test motion influence of the manipulator on the whole system:(49)
d
1
=
0.01
sin
(
0.5
π
t
)
(50)
d
2
=
0.01
sin
(
1.0
π
t
)
.The results are as in Figure7.States response with disturbance (49) (blue and solid) and (50) (red and dashed)*; the subscript -1 means that the results are with the disturbance (49) and the subscript -2 means that the results are with the disturbance (50).
(a)
(b)
(c)
(d)
(e)
(f)Simultaneously, in order to test the coupling between the RFR and the manipulator, the linear accelerations and the angular accelerations of the new RF-MJM system are compared to the helicopter system with the same parameters as in Table1; that is,
(51)
[
error
1
error
2
error
3
]
=
[
a
RF
-
MJM
,
x
a
RF
-
MJM
,
y
a
RF
-
MJM
,
z
]
-
[
a
RFR
,
x
a
RFR
,
y
a
RFR
,
z
]
;
[
error
4
error
5
error
6
]
=
[
ω
˙
RF
-
MJM
,
x
ω
˙
RF
-
MJM
,
y
ω
˙
RF
-
MJM
,
z
]
-
[
ω
˙
RFR
,
x
ω
˙
RFR
,
y
ω
˙
RFR
,
z
]
.The results are given in Figure8, which presents the extra force and moment exerted on the RFR due to the manipulator and its motion.Decoupling between RFR and manipulator.
(a)
(b)


## 5. Conclusions

In this paper, the detailed nonlinear dynamics model of a rotor-flying multijoint (RF-MJM) system is constructed through using Euler-Lagrange method. Compared to the rotor-flying vehicle system, the model nonlinearities and complexity of the new RF-MJM are analyzed in detail. Moreover, linear analysis is conducted with respect to the constructed nonlinear model near its trim point, and the influence of the manipulator mass on the system’s local performance is researched. Furthermore, LQR controller is designed based on the linearized system model. Finally, simulation results show that(
1
) a linear LQR controller is able to stabilize the system near steady state and presents acceptable performance; however, (
2
) the stabilization region of LQR controller is very limited, and the performance of LQR controller is sensitive to the external disturbance. Thus, in the future work, nonlinear and robust control scheme will be researched to overcome the disadvantages of the linear controller.



---

*Source: 492965-2014-05-04.xml*