# Trajectory Evaluation of Rotor-Flying Robots Using Accurate Inverse Computation Based on Algorithm Differentiation

Authors: Yuqing He; Yingjun Zhou; Jianda Han
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Autonomous maneuvering flight control of rotor-flying robots (RFR) is a challenging problem due to the highly complicated structure of its model and significant uncertainties regarding many aspects of the field. As a consequence, it is difficult in many cases to decide whether or not a flight maneuver trajectory is feasible. It is necessary to conduct an analysis of the flight maneuvering ability of an RFR prior to test flight. Our aim in this paper is to use a numerical method called algorithm differentiation (AD) to solve this problem. The basic idea is to compute the internal state (i.e., attitude angles and angular rates) and input profiles based on predetermined maneuvering trajectory information denoted by the outputs (i.e., positions and yaw angle) and their higher-order derivatives. For this purpose, we first present a model of the RFR system and show that it is flat. We then cast the procedure for obtaining the required state/input based on the desired outputs as a static optimization problem, which is solved using AD and a derivative based optimization algorithm. Finally, we test our proposed method using a flight maneuver trajectory to verify its performance.


---

## Body

## 1. Introduction

The autonomous rotor-flying robot (RFR) is one of the frontier research topics in the field of robotics. Extensive research has been conducted on issues related to RFR, including flight control [1, 2], path/trajectory planning [3, 4], and intelligent navigation [5, 6].Flight maneuvering is a very important ability for an RFR system because it can greatly extend the system’s field of applications. However, the flight maneuver control of an RFR is a challenging and absorbing problem in the field of autonomous RFR research [2]. This can be explained from the following two points. On the one hand, the model of an RFR system is an extremely complicated structure [7, 8], designing a suitable controller for which is very difficult. On the other hand, flight maneuvering requires that both the interval states (i.e., the attitude angles and the angular rates) and the control surfaces to have a large scope variation (or even approach the largest permitted value) [9]. This may produce a great deal of uncertainty and thus presents that this flight maneuvering is dangerous and difficult to be implemented and thus requires a much more robust controller than usual [10].Most work on flight maneuver control, such as [11, 12], is based on successful flight demonstrations and grounded in learning techniques used to update the control laws. These methods have successfully pushed the envelope of what is possible with autonomous control, but they lack performance guarantees and are limited to situations where safety is not a critical concern. Thus, it is very important to assess the flight maneuvering ability of an RFR system prior to designing a controller and conducting tests. Some research has been conducted in this area. For instance, research in [2] is based on reachability analysis and explores aerobatic maneuvers and multiple vehicle collision avoidance of the RFR systems. This involves designing a sequence of modes and conducting safe transition from one to the next. However, reachability analysis is very time consuming, due to which the proposals in [2] are viable only for simplified models. Similarly, [9] involves a feasibility analysis of planned trajectories by using a greatly simplified dynamics model (without considering angular dynamics). Both the positions and attitude angles are predetermined and then used to evaluate the feasibility of the planned trajectories. Although the algorithm is simple and easily implemented, it is unfit for many planning algorithms since for which it is difficult to obtain the positions and attitude angles simultaneously.In this paper, based on a full state dynamics model, we investigate the feasibility analysis problem of a flight maneuvering trajectory. This is actually an inverse computation problem, that is, to compute both the interval states and the inputs based on predetermined position, yaw angle, and their higher-order derivatives. In our work, we model it as a static optimization problem and then use a derivative-based optimization algorithm to obtain an accurate solution of it. We use algorithm differentiation (AD) to obtain precise higher-order derivatives of the nonlinear cost function. The advantages of our proposed algorithm are as follows:(
1
) only with desired positions and their derivatives, which is the case for many planning algorithm, the feasibility of the trajectories (including inner states and inputs) can be evaluated; (
2
) inverse computation can be very accurate because AD is a numerical algorithm that can precisely obtain the derivative of a nonlinear function. Thus, an accurate pointwise numerical feasibility analysis of planned trajectories becomes possible.The remainder of this paper is organized as follows. We first present a model of the RFR system and show that it is flat by taking the three positions and the yaw angle as a group of flat outputs. This allows us to model the trajectory evaluation problem as an optimization problem by taking into account the dynamical model, which is solved by using the AD algorithm along with a derivative-based optimization algorithm. Finally, in order to test our proposed method, we use it to analyze a flight maneuvering trajectory.


## 2. Dynamical Model of the RFR System

Usually, the complete dynamical model of an RFR system can be divided into three parts: the actuator dynamics, the aerodynamics, and the rigid body dynamics. In this paper, only the rigid body dynamics as shown in the following equation (1) is considered [3]:
(1)
[
p
˙
v
˙
p
Θ
˙
ω
˙
b
]
=
[
v
p
1
m
R
f
b
Ψ
ω
b
J
-
1
(
τ
b
-
ω
b
×
J
ω
b
)
]
,
where p
=
[
x
y
z
]
T
∈
R
3 and v
p
=
[
x
˙
y
˙
z
˙
]
∈
R
3 are translational position and velocity vector of the RFR in the inertia frame; R is the rotation matrix between the body frame and the inertia frame; ω
b is angular velocity vector; Θ
=
[
ϕ
θ
ψ
]
T is Euler angle vector; m and J are, respectively, the mass and inertia matrix of the RFR; Ψ is the transformation matrix from angular velocity vector in body frame to angular velocity vector in inertia frame; and f
b and τ
b are force and moment of the RFR presented in body frame. Different kinds of RFR have very different aerodynamics, that is, the relationship between the force/moment and the control surfaces. Here we focus on the quadracopter flying robot, whose driving forces and moments can be denoted as follows:
(2)
f
b
=
[
f
x
f
y
f
z
]
=
[
0
0
T
M
]
=
[
0
0
F
1
+
F
2
+
F
3
+
F
4
]
,
τ
b
=
[
M
x
M
y
M
z
]
=
[
l
(
F
4
-
F
2
)
l
(
F
3
-
F
1
)
l
(
F
1
-
F
2
+
F
3
-
F
4
)
]
,
where F
1, F
2, F
3, and F
4 are force produced by four rotors, respectively, and l is the distance between each rotor center and the RFR’s center of gravity (see Figure 1).Figure 1
Sketch of quadrocopter.Thus, define inputs as(3)
u
=
[
u
1
u
2
u
3
u
4
]
≜
[
T
¨
M
M
x
M
y
M
z
]
.
System (1) can be rewritten as
(4)
x
¨
=
cos
⁡
ψ
sin
θ
cos
⁡
φ
+
sin
ψ
sin
φ
m
T
M
,
y
¨
=
sin
ψ
sin
θ
cos
⁡
φ
-
sin
φ
cos
⁡
ψ
m
T
M
,
z
¨
=
-
g
+
cos
⁡
θ
cos
⁡
φ
m
T
M
,
φ
¨
=
I
y
-
I
z
I
x
θ
˙
ψ
˙
+
1
I
x
u
2
,
θ
¨
=
I
z
-
I
x
I
y
φ
˙
ψ
˙
+
1
I
y
u
3
,
ψ
¨
=
I
x
-
I
y
I
z
θ
˙
φ
˙
+
1
I
z
u
4
,
T
¨
M
=
u
1
.It can be easily shown that system (4) is input-to-state feedback linearizable through defining the following state vector:
(5)
[
x
y
z
x
˙
y
˙
z
˙
x
¨
y
¨
z
¨
x
⃛
y
⃛
z
⃛
ψ
ψ
˙
]
T
.
From (1) and (2), we have
(6)
p
(
4
)
=
-
1
m
R
[
ω
b
×
(
ω
b
×
[
0
0
T
M
]
)
]
+
1
m
R
[
0
-
T
M
0
T
M
0
0
0
0
0
]
×
J
-
1
(
τ
b
-
ω
b
×
J
ω
b
)
-
2
m
R
{
ω
b
×
[
0
0
T
˙
M
]
T
}
-
1
m
R
[
0
0
1
]
T
T
¨
M
,
ψ
¨
=
-
[
0
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
cos
⁡
θ
]
J
-
1
(
ω
b
×
J
ω
b
-
τ
b
)
+
(
q
ϕ
˙
cos
⁡
ϕ
-
r
ϕ
˙
sin
ϕ
)
cos
⁡
θ
×
[
p
+
2
q
sin
ϕ
tan
θ
+
2
r
cos
⁡
ϕ
tan
θ
]
and the system can be transformed into the following linear form:
(7)
p
(
4
)
=
[
x
(
4
)
y
(
4
)
z
(
4
)
]
=
[
v
1
v
2
v
3
]
,
ψ
¨
=
v
4
with the feedback linearization controller
(8)
u
=
[
g
2
g
3
]
-
1
(
[
v
1
v
2
v
3
v
4
]
-
[
f
2
f
3
]
)
,
where
(9)
f
2
=
-
2
m
T
˙
M
R
(
ω
b
×
[
0
0
1
]
)
-
1
m
T
M
R
[
ω
b
×
(
ω
b
×
[
0
0
1
]
)
]
-
1
m
T
M
R
{
[
J
-
1
(
ω
b
×
J
ω
b
)
]
×
[
0
0
1
]
}
;
g
2
=
[
-
1
m
R
[
0
0
1
]
T
-
T
M
m
R
[
0
1
0
-
1
0
0
0
0
0
]
J
-
1
]
;
f
3
=
-
[
0
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
cos
⁡
θ
]
J
-
1
(
ω
b
×
J
ω
b
)
+
(
q
cos
⁡
ϕ
-
r
sin
ϕ
)
×
sec
θ
[
p
+
2
q
sin
ϕ
tan
θ
+
2
r
cos
⁡
ϕ
tan
θ
]
;
g
3
=
[
0
[
0
sin
ϕ
cos
⁡
θ
cos
⁡
ϕ
cos
⁡
θ
]
J
-
1
]
.That means system (1) is flat and that the [
p
T
,
ψ
] are the corresponding flat outputs.


## 3. Automatic Differential Algorithm Based Inverse Computation

AD is a numerical method that is effective to accurately compute the derivatives of some complicated nonlinear function. In this section, we will briefly introduce how to conduct the inverse computation of a nonlinear flat system using AD algorithm [13].Consider the following nonlinear system:(10)
x
˙
=
f
(
x
)
,
y
=
h
(
x
)
.It has been shown that the inverse computation problem of system (10), that is, to obtain a state vector x
d with h
(
x
d
)
=
y
d (a predefined output), is solvable if y
=
h
(
x
) is a flat output of system (1) [14].Derivatives ofy with respect to x are necessary to effectively solve the preceding inverse problem. In the following contents of this section, the basic idea on how to obtain ∂
y
/
∂
x using AD algorithm will be introduced.For this purpose, we firstly use the following Taylor series to denote the state vectorx
(
t
):
(11)
x
(
t
)
=
x
0
+
x
1
t
+
x
2
t
2
+
⋯
+
x
d
t
d
.
Provided that f
(
*
) is d times continuously differentiable, z
=
f
(
x
) can also be expressed by Taylor series as follows:
(12)
z
(
t
)
=
z
0
+
z
1
t
+
z
2
t
2
+
⋯
+
z
d
t
d
+
O
(
t
d
+
1
)
.It has been shown that each Taylor coefficientz
i  (
i
=
0,1
,
…
,
d
) is uniquely determined by the coefficients x
i  (
i
=
0,1
,
…
,
d
) as follows [15, 16]:
(13)
z
0
=
f
(
x
0
)
z
1
=
∂
f
(
x
0
)
∂
x
0
x
1
z
2
=
∂
f
(
x
0
)
∂
x
0
x
2
+
1
2
∂
2
f
(
x
0
)
∂
x
0
∂
x
0
x
1
x
1
⋮Thus, ifx
i  (
i
=
0,1
,
…
,
d
) in (11) are preknown, all z
i  (
i
=
0,1
,
…
,
d
) can be obtained based on some derivatives, which can be computed by basic “forward mode” of AD [13]. Similarly, the following A
j
-
i can also be derived using “reverse mode” of AD [13–15]:
(14)
A
j
-
i
(
j
≥
i
)
≜
∂
z
j
-
i
∂
x
0
.Furthermore, based on (10), we have
(15)
z
(
t
)
=
z
0
+
z
1
t
+
z
2
t
2
+
⋯
+
z
d
t
d
+
O
(
t
d
+
1
)
=
f
(
x
)
=
x
˙
=
x
1
+
2
x
2
t
+
⋯
+
d
x
d
t
d
-
1
;
that is,
(16)
z
i
≡
(
i
+
1
)
x
i
+
1
,
i
=
0
,
…
,
d
-
1
.Through (16) and (13), we can easily compute all z
i based on x
0.Define a new matrix as(17)
B
k
≜
d
x
k
+
1
d
x
0
=
1
k
+
1
d
z
k
d
x
0
=
1
k
+
1
∑
j
=
0
k
∂
z
k
∂
x
j
d
x
j
d
x
0
.From [15], we have
(18)
∂
z
k
∂
x
j
=
∂
z
k
-
j
∂
x
0
.Thus,B
k can be iteratively obtained as follows:
(19)
B
k
=
1
k
+
1
∑
j
=
0
k
∂
z
k
-
j
∂
x
0
d
x
j
d
x
0
=
1
k
+
1
(
A
k
+
∑
j
=
0
k
A
k
-
j
B
j
-
1
)
.Furthermore, if we denotey
(
t
) as
(20)
y
(
t
)
=
y
0
+
y
1
t
+
y
2
t
2
+
⋯
+
y
d
t
d
+
O
(
t
d
+
1
)
,then the same process can be used to computey through the second equation of (10); that is, with a preknown x
0, compute y
i by forward mode and compute the following derivatives C
j
-
i by reverse mode:
(21)
C
j
-
i
≜
∂
y
j
∂
x
i
.Also, it is not difficult to show that the derivatives of output with respect to the states can be denoted as follows:(22)
D
k
≜
d
y
k
d
x
0
=
∑
j
=
0
k
∂
y
k
∂
x
j
d
x
j
d
x
0
=
C
k
+
∑
j
=
1
k
C
k
-
j
B
j
-
1
.Up to now, we have shown that, with a predefined statex
0, both the corresponding outputs y
(
x
0
) and its higher-order derivatives with respect to time, as well as the derivatives with respect to state x
0 (22), can be obtained through AD algorithm. These results can be further used to conduct the inverse computation of system (1), and the detailed steps are given as follows.AD Based Inverse Computation AlgorithmInitialization. d (the highest order of the outputs’ derivatives); γ (step length of searching algorithm); k
=
0 (iterative number).Step 1.
Compute the output vectorY
(
x
k
) using forward mode of AD algorithm at each time instant as a nonlinear map; that is,
(23)
Θ
:
x
k
⟶
Y
(
x
k
)
≜
(
y
0
(
x
k
)
⋮
y
d
(
x
k
)
)
.Step 2.
Based on (22), AD can be used to obtain the partial derivative of Y with respect to x
0; that is,
(24)
Θ
′
(
x
k
)
=
∂
Y
∂
x
k
=
(
D
0
⋮
D
d
)
.Step 3.
Update the statex using the following searching iteration:
(25)
x
k
+
1
≜
x
k
-
γ
(
Ξ
′
(
x
k
)
)
+
Ξ
(
x
k
)
=
x
k
-
γ
(
Ξ
′
(
x
k
)
)
+
[
Θ
(
x
k
)
-
Y
d
]
=
x
k
-
γ
(
Θ
′
(
x
0
)
)
+
[
Θ
(
x
k
)
-
Y
d
]
,
where superscript “+” means the pseudoinverse of a matrix; that is,
(26)
Q
+
=
(
Q
T
Q
)
-
1
Q
T
,
Ξ
(
x
)
=
Θ
(
x
)
-
Y
d
.Step 4.
Go to Step1 until the terminal conditions are satisfied.
If the system is nonautonomous, that is, it can be rewritten as following form,(27)
x
˙
=
f
(
x
,
u
)
,
y
=
h
(
x
)
.
We can first extend it into the following form:
(28)
x
˙
=
f
(
x
,
u
)
,
u
˙
=
0
,
y
=
h
(
x
)
and the new system can be rewritten as
(29)
x
-
˙
=
F
(
x
-
)
,
y
=
H
(
x
-
)
,
where
(30)
x
-
=
[
x
u
]
.Thus, the inverse computation can be conducted using the preceding algorithm.As for the proposed quadrocopter system introduced in Section2, this algorithm can be directly utilized through defining the following output vector:
(31)
Y
≜
[
p
T
p
˙
T
p
¨
T
p
⃛
T
(
p
(
t
)
)
T
ψ
ψ
˙
ψ
¨
]
18
×
1
T
.


## 4. Evaluation of Circle Flying Maneuver

In this section, a so-called “Circle Maneuver,” that is, the RFR system flies along a circle as shown in the following equation (32) and Figure 2 [10], of the Quadrocopter is taken as an example to show the validity of the proposed algorithm. Consider
(32)
ψ
d
=
π
+
t
T
×
2
π
,
x
d
=
50
sin
(
t
T
×
2
π
)
,
y
d
=
50
cos
⁡
(
t
T
×
2
π
)
,
z
d
=
0
,
where T is the period of the circle maneuver.Figure 2
Circle maneuver trajectory.The parameters of the RFR system are as follows:(33)
m
=
9.5
kg
J
=
[
0.1364
0
0
0
0.5782
0
0
0
0.6306
]
N
·
m
.The trajectory feasibility is decided by some constrains on both inputs and states. Here the following constrains are considered.Input Constrains. Consider
(34)
T
M
∈
[
49.4,190.8
]
N
,
M
x
∈
[
-
25.5,25.5
]
N
·
m
,
M
y
∈
[
-
40.2,40.2
]
N
·
m
,
M
z
∈
[
-
34.6,34.6
]
N
·
m
.Angular Velocity Constrains. Consider
(35)
p
,
q
∈
[
-
0.4,0.4
]
rad
/
s
,
r
∈
[
-
1.45,1.45
]
rad
/
s
.Attitude Constrains. Consider
(36)
θ
,
ϕ
∈
[
-
0.5,0.5
]
rad
.In this paper, the software toolbox LIEDRIVERS [17] based on ADOL-C is used as the basic AD algorithm, and the optimization searching is implemented using the golden sector algorithm, instead of the direct derivative-based algorithm as in Section 4. During the simulations, we test the circle maneuvers with different period T, and the results are given in Figures 3, 4, and 5.Control input:T
¨
M (a); M
x (b); M
y (c); M
z (d).
(a)
(b)
(c)
(d)State:ϕ (a); θ (b).
(a)
(b)State:p (a); q (b); r (c); T
M (d).
(a)
(b)
(c)
(d)From these figures, we can obtain the following results.(1)
For input constraints (Figures3 and 5), only the maneuvering trajectories with T
=
10
s are infeasible because the required dragging force is almost 210 N, larger than the maximum permitted value of 190.8 N, while the other inputs, that is, the three moments, are all much smaller than the maximum values.
(2)
For angular velocity constrains (Figure5), again, only the maneuvering trajectories with T
=
10
s are infeasible, and the maneuvering trajectories with T
=
12.5
s aredangerous since the maximum angular velocity approaches the maximum value of the permitted angular velocity.
(3)
Based on the attitude constrains (Figure4), the maneuvering trajectories with T
=
14.3
s, 12.5 s, and 10 s are all infeasible.Finally, from the preceding results, it can be seen that the circle maneuvers are really a highly dynamical maneuver, and the quantitative analysis prior to the flight test is useful and necessary to evaluate whether or not a maneuver trajectory is feasible or even dangerous.


## 5. Conclusions

In this paper, the flight maneuvering trajectories evaluation problem is researched, and the algorithm differentiation (AD) is used to realize the inverse computation of the rotor-flying robot (RFR) system. This paper starts from constructing nonlinear dynamical model of an RFR system. Then, we show that the RFR system model is flat taking translational positions and yaw angle as flat outputs. After that, the scheme of AD based inverse computation is introduced, that is, computing the internal states and inputs based on the desired flat outputs. Finally, with the proposed scheme, a typical flight maneuver, that is, the circle maneuver, is analyzed.The advantages of our proposed algorithm are as follows:(
1
) only desired positions and their derivatives, which is the case for many planning algorithm, and the feasibility of the trajectories (including inner states and inputs) can be evaluated; (
2
) inverse computation can be very accurate because AD is a numerical algorithm that can precisely obtain the derivative of a nonlinear function. Thus, an accurate pointwise numerical feasibility analysis of planned trajectories becomes possible.The method proposed in this paper can be used to not only evaluate the feasibility and validity of the maneuver flying trajectories, but also realize the tracking control algorithm, where the computed internal states and inputs are useful for attenuating the online computational burden of the tracking controller and thus improving the closed loop performance. This will be one of our future’s works.



---

*Source: 464056-2014-08-13.xml*