mathematics

Article
Nonlinear Dynamics and Control of a Cube Robot
Teh-Lu Liao 1 , Sian-Jhe Chen 1 , Cheng-Chang Chiu 1 and Jun-Juh Yan 2, *
1 Department of Engineering Science, National Cheng Kung University, Tainan 701, Taiwan;
tlliao@mail.ncku.edu.tw (T.-L.L.); gogo0929052207@gmail.com (S.-J.C.); chang@mail.mirdc.org.tw (C.-C.C.)
2 Department of Electronic Engineering, National Chin-Yi University of Technology, Taichung 41107, Taiwan
* Correspondence: jjyan@ncut.edu.tw




Received: 31 August 2020; Accepted: 15 October 2020; Published: 19 October 2020


Abstract: The paper aims to solve problems of the mathematical modeling and realization of a cube
robot capable of self-bouncing and self-balancing. First, the dynamic model of the cube robot is derived
by using the conservation of the angular momentum and the torque equilibrium theory. Furthermore,
the controllability of the cube robot is analyzed and the angle of the cube robot is derived from the
attitude and heading reference system (AHRS). Then the parallel proportional–integral–derivative
(PID) controller is proposed for the balancing control of the self-designed cube robot. As for the
bounce control of the cube robot, a braking system triggered by the servo motor is designed for
converting the kinetic energy to the potential energy. Finally, the experimental results are included to
demonstrate that the cube robot can complete the actions of self-bouncing and self-balancing with
good robustness to external disturbances.

Keywords: nonlinear dynamics; cube robot; attitude and heading reference system; PID controller

1. Introduction
The cube robot is a cube-shaped system with an embedded reaction wheel generating the control
torque to complete self-balancing and self-bouncing actions. The mathematical model of the cube robot
is a typical nonlinear dynamic system. In addition, the system is an unstable and multi-dimensional
inverted pendulum. Therefore, we need to design an appropriate controller to generate a corresponding
torque inside the cube robot to maintain its balance. In 2012, a cube robot called Cubli was designed and
driven by the inner reaction wheel fixed on the cube’s three faces [1]. The balancing controller design
is always challenging due to the cube robot’s modeling problem and complexity. The linear-quadratic
regulator (LQR) control algorithm for the Cubli balancing on its edge and corner was conducted in [2].
In 2017, Chen et al. [3] proposed methods to construct the dynamic model of a self-balancing cube
robot and a proportional–integral–derivative (PID) controller was proposed to accomplish the action of
the cube robot balancing [4]. However, the cube robot mentioned above did not provide the dynamic
model. The methodology for realizing the cube robot’s action of bouncing and balancing is not clearly
described. In [5], a balance controller based on sliding mode control (SMC) is proposed. In the cube
robot prototype, the SMC and PID controller are compared through numerical simulations and the
conclusion is that the performance of SMC is better than that of the PID controller. Tian [6] introduced
the concept of fuzzy control to design the balancing controller. Muehlebach et al. [7] studied the cubical
robot’s balancing control based on the back-stepping method. However, the results in the works as
mentioned above were available when the system was modeled correctly. On the other hand, the PID
controller is a well-known popular control strategy and widely applied to solve the control problem in
many industrial applications due to its simple structure and robust feature [8]. Many rules have been
developed for tuning the optimal or sub-optimal PID controller gains [9,10]. Therefore, in this paper,
we utilize the PID control approach to propose a new hardware mechanism different from that of [2] to
achieve the balance and bounce control of the cube robot.

Mathematics 2020, 8, 1840; doi:10.3390/math8101840 www.mdpi.com/journal/mathematics

Mathematics 2020, 8, x FOR PEER REVIEW 2 of 15

or sub-optimal PID controller gains [9,10]. Therefore, in this paper, we utilize the PID control
approach 2020,
Mathematics to propose
8, 1840 a new hardware mechanism different from that of [2] to achieve the balance and
2 of 16
bounce control of the cube robot.
Three steps, including mathematical modeling, control, and system realization, should be
Three to
addressed steps, including
complete mathematical
the design modeling,
and realization of a cubecontrol,
robotand systemcapable
prototype realization, should be
of self-balancing
addressed to complete the design and realization of a cube robot prototype
and self-bouncing. First, to construct the model of the cube robot’s motion of bouncing up and self- capable of self-balancing
and self-bouncing.
balancing, this paperFirst, to construct
introduces the model of
the conservation of angular
the cubemomentum
robot’s motiontheoryofand
bouncing
energyup and
theory
self-balancing, this paper introduces the conservation of angular momentum
to obtain the dynamic model of the cube robot. After analyzing the dynamic system model, the state theory and energy theory
to obtain
space the dynamic
equation and state model of the cube
variables can be robot. After In
obtained. analyzing the dynamic
the proposed model,system model, the
state variables arestate
the
space equation and state variables can be obtained. In the proposed model,
angle of the cube robot, the cube robot’s angular velocity, and the velocity of the reaction wheel. state variables are the
angle
Second, of we
theusecube robot,inertial
a 6-axis the cube robot’s angular
measurement device velocity, and
to detect itsthe velocity of
acceleration andtheangular
reaction wheel.
velocity.
Second,
With these we use a 6-axis
detected inertial
data, measurement
the cube device
robot’s angle canto be
detect its acceleration
derived and angular
from the attitude velocity.
and heading
With these detected
reference system [11]. data, the cube robot’s angle can be derived from the attitude and heading reference
system [11].
Furthermore, by applying the angle and angular velocity to the parallel PID controllers, the cube
robot can balance on applying
Furthermore, by the angle
its edge. Third, weand angular
set up velocitysolution
a combined to the parallel
of the PID controllers, the cube
multi-dimensional cube
robot can balance on its edge. Third, we set up a combined solution
robot containing mechanical design, firmware architecture, and software development. of the multi-dimensional cube
Androbotto
containing
accomplishmechanical
the action ofdesign,
the cubefirmware architecture,
robot bouncing, and software
a braking system development.
is constructed to And to the
stop accomplish
reaction
the action
wheel of the such
rotation cube robot
that thebouncing,
cube robota braking system is
can bounce up.constructed
As for thetofirmware
stop the reaction wheel an
architecture, rotation
IAR-
such that the cube robot can bounce up. As for the firmware architecture, an
embedded system is introduced to set up and implement the controller. The data is collected by using IAR-embedded system is
introduced
the embedded to set up andpanel
control implement the controller.
STM32F407 with theThe data isofcollected
function the timer byinterrupt.
using the embedded control
We use Microsoft
panel STM32F407 with the function of the timer interrupt. We use Microsoft
Visual Studio Professional 2013 platform and C programming language to develop the control Visual Studio Professional
2013 platform
software for theand cubeC programming
robot in software language to develop
development. Figurethe1control
shows the software for the
cube robot cube robot
designed in
in this
software development.
paper balancing Figure 1 shows the cube robot designed in this paper balancing at its edge.
at its edge.

Figure 1.
Figure 1. The
The prototype
prototype of
of the
the cube
cube robot balancing on
robot balancing on its
its edge.
edge.

The framework
The framework of of the
the paper
paper is is given
given as
as follows.
follows. Section
Section 22 is
is the description of
the description of the cube robot
the cube robot
prototype, including
prototype, including the
the mechanical
mechanical structure
structure and
and balancing
balancing equation.
equation. Section
Section 33 is
is the
the estimation and
estimation and
control design for the proposed cube robot. The realization of the system and experimental
control design for the proposed cube robot. The realization of the system and experimental results are results
are included
included in Sections
in Sections 4 and4 and 5, respectively.
5, respectively. Section
Section 6 is conclusion.
6 is the the conclusion.

2. The Cube Robot Prototype

Figure 22 illustrates
illustratesthe
thecube
cuberobot
robotprototype
prototypethat
thatconsists
consistsofof six-sided
six-sided square
square plastic
plastic faces.
faces. One
One of
of the
the plastic
plastic facesfaces
holdsholds the reaction
the reaction wheel wheel driven
driven by theby the brushless
brushless motor motor at its and
at its center, center,
the and the
braking
brakingissystem
system fixed atis fixed at its Considering
its corner. corner. Considering the strength
the strength of the faces,
of the plastic plasticeach
faces, each
side of side of the
the cube’s
cube’s thickness
thickness is designedis designed to beFurthermore,
to be 2 mm. 2 mm. Furthermore,
to let the to let robot
cube the cube robot
swing swing
freely freely
during theduring the
balancing,
balancing,
each face ofeach face of
the cube the cube
robot must robot
be in amust be in a closed-state
closed-state at the joint,at the joint,
which which
is shown inisFigure
shown 3.in Figure
3.
 Mathematics2020,
Mathematics 2020,8,8,x1840
FOR PEER REVIEW 33ofof15
16
Mathematics
Mathematics2020,
2020,8,
8,xxFOR
FORPEER
PEERREVIEW
REVIEW 33of
of1515

Figure
Figure 2. The
Thecube
cuberobot
robotprototype.
Figure 2.
2. The
The cube
cube robot
robot prototype.
prototype.

Figure
Figure3.
Figure 3.The
3. Thejoint
The jointof
joint ofthe
of thecube
the cuberobot.
cube robot.
robot.
Figure 3. The joint of the cube robot.
2.1. SystemDynamics
2.1. Dynamics
2.1.System
2.1. System
System Dynamics
Dynamics
The overall system
The consists ofofa cube robot, reaction wheel, servoservo motor, brushless motor, a braking
Theoverall
The overallsystem
overall systemconsists
system consists
consists of ofaaacube
cuberobot,
cube robot,reaction
robot, reactionwheel,
reaction wheel,
wheel, servo servomotor,motor,brushless
motor, brushlessmotor,
brushless motor,aaa
motor,
system
braking [12],
system and a
[12], control
and a processor.
control The
processor. cubical
The robot’s
cubical edge
robot’s is fixed
edge with
is the
fixed axis,
with and
the the brushless
axis,
braking
braking system
system [12], [12], and and aa control
control processor.
processor. The The cubical
cubical robot’s
robot’s edge edge is is fixed
fixed with
with the axis,and
the axis, andthe
and the
the
motor is set
brushless motor on the is center
set on of
the the cubical
center of robot’s
the cubicalsingle face. The
robot’s single concepts
face. of torque
The concepts andofrotation
torque angle
brushless
brushless motor motor is is set
set onon thethe center
center of of the
the cubical
cubical robot’srobot’s single
single face.face. The
The concepts
concepts of torqueand
of torque and
and
are shown in are
rotation Figure 4. TheFigure torque4.analysis of the internal reaction wheel driving the cube robot is
rotationangle
rotation angle
angle are areshownshownin
shown in
in Figure
Figure 4. 4.The
Thetorque
The torqueanalysis
torque analysisof
analysis ofthe
of theinternal
the internalreaction
internal reactionwheel
reaction wheeldriving
wheel drivingthe
driving the
the
shown
cube in Figure 5. In Figure 4, l denotes the distance from the cube robot’s
P Figure 4, 𝑙 denotes the distance from the cube robot’s center of center of gravity to the
cuberobot
cube robotis
robot isshown
is shownin
shown inFigure
in Figure5.
Figure 5.In
5. In
In Figure
Figure 4, 4, 𝑙𝑃𝑙𝑃𝑃 denotes
denotes the the distance
distance from from the the cube
cube robot’s
robot’s centercenter of of
axis. FPtoisthe
gravity the axis.
cube 𝐹 robot’s
is the gravity
cube and lw denotes
robot’s gravity the distance
and 𝑙 denotes from the the reactionfrom
distance wheel’sthe center of
reaction
gravity
gravity to to the
the axis. 𝑃
axis. 𝐹𝐹𝑃𝑃 is is the
the cube
cube robot’s
robot’s gravity
gravity and 𝑤
and 𝑙𝑙 denotes
denotes the the distance
distance from from the the reaction
reaction
the gravity to the FW denotes
axis.gravity the reaction wheel’s𝑤𝑤gravity, CW and CP represent the androtating
wheel’s
wheel’s center
wheel’s center of
center of the
of the gravity to
the gravity to the
to the axis. 𝐹𝐹𝐹𝑊
the axis.
axis. 𝑊 𝑊
denotes
denotes the
denotes the reaction
the reaction wheel’s
reaction wheel’s gravity, 𝐶𝐶𝐶𝑊
wheel’s gravity,
gravity, 𝑊
𝑊 and 𝐶𝐶𝐶𝑃𝑃𝑃
and
axis friction
represent the coefficients
rotating axis of the
frictioncube robot
coefficients and ofthethereaction
cube wheel,
robot and respectively,
the reaction and T
wheel, ( t ) denotes
R respectively, the
represent
represent the the rotating
rotating axis axis friction
friction coefficients
coefficients of of the
the cube
cube robot
robot and and thethe reaction
reaction wheel,wheel, respectively,
respectively,
torque. The tilt angle oftorque. the cube robot is defined as θcube
P (t) and the rotation angle𝜃of(𝑡) the reaction wheel is
and 𝑇𝑇𝑇𝑅𝑅𝑅(t)
and
and (t) denotes
(t) denotesthe
denotes the torque.The
the torque. Thetilt
The tiltangle
tilt angleof
angle ofofthe
the cuberobot
the cube robotis
robot isdefined
is definedas
defined as (𝑡) and
as 𝜃𝜃𝑃𝑃𝑃(𝑡) andthe
and therotation
the rotation
rotation
defined as θ ( t ) . The torque, applying to the cube robot and reaction wheel, is the sum of the torques
as 𝜃𝜃𝜃𝑊 (𝑡).
angle W
angleof
angle ofthe
of thereaction
the reactionwheel
reaction wheelis
wheel isdefined
is definedas
defined as 𝑊 (𝑡).The
𝑊(𝑡). Thetorque,
The torque,
torque, . applying
applyingto
applying tothe
to thecube
the cuberobot
cube robotand
robot andreaction
and reaction
reaction
generated from the brushless motor and friction torque CW θWmotor , which is opposite to the reaction𝜃𝜃̇𝑊 wheel
wheel,
wheel,is
wheel, isthe
is thesum
the sumof
sum ofthe
of thetorques
the torquesgenerated
torques generatedfrom
generated fromthe
from thebrushless
the brushless
brushlessmotor motorand andfriction
and friction torque 𝐶𝐶𝐶𝑊
frictiontorque
torque 𝑊𝑊𝜃
̇𝑊,,,which
̇𝑊 which
which
isrotation, as shown in Figure 5.
isopposite
is oppositeto
opposite tothe
to thereaction
the reactionwheel
reaction wheelrotation,
wheel rotation,as
rotation, asasshown
shownin
shown inFigure
in Figure5.
Figure 5.
5.

Figure
Figure4.
Figure 4.The
4. Thetorque
The torqueand
torque androtation
and rotationangle
rotation angleof
angle ofthe
of thecube
the cuberobot.
cube robot.
robot.
Figure 4. The torque and rotation angle of the cube robot.
Mathematics 2020, 8, 1840 4 of 16
Mathematics 2020, 8, x FOR PEER REVIEW 4 of 15

Figure 5. The
Figure 5. The torque
torque of
of the
the reaction
reaction wheel.
wheel.

By applying the conservation of the angular momentum and torque equilibrium theory,
By applying the conservation of the angular momentum and torque equilibrium theory, the
the nonlinear dynamic equation of the cube robot shown in Figure 4. is given as:
nonlinear dynamic equation of the cube robot shown in Figure 4. is given as:
..
𝑂 ̈
. .
̇𝑃t)(𝑡) 𝜃̇𝑊

O (𝑂t(𝑡)
 M𝑀 𝐶 ) == I O𝐼θ
𝐶P 𝜃(𝑃t(𝑡)
) = =FP𝐹𝑃lP𝑙𝑃sin 𝑠𝑖𝑛θP𝜃(𝑃t(𝑡)
) + +F 𝐹𝑊l 𝑙𝑤sin
𝑠𝑖𝑛θ 𝜃(𝑃t)(𝑡)
− −R𝑇(𝑅t)(𝑡)
T − −P𝐶
C θ 𝑃P𝜃
( + +
C 𝐶θ
𝑊W (t(𝑡)
)

 W w P W
{C C .. .. . (1)
(1)
(𝑡) θ ̈ (𝑡) θ ̈ (𝑡)) (𝑡) θ ̇ (𝑡),

 M𝑀

W𝑊 ( t ) ==I 𝐼 (
W𝑊 P 𝑃(𝜃 ( t ) + + 𝜃
W𝑊 ( t )) = = T 𝑇(
R𝑅 t ) −−C 𝐶
W𝑊 W𝑊 𝜃 ( t ) ,

where .𝜃̇ is the angular velocity of the cube robot and ..𝜃̈ is the angular acceleration of the cube
where θP𝑃 is the angular velocity of the cube robot and θP𝑃 is the angular acceleration of the cube
robot. 𝜃. ̇𝑊 is the angular velocity of the reaction wheel, and ..𝜃̈𝑊 is the angular acceleration of the
robot. θW is the angular velocity of the reaction wheel, and θW is the angular acceleration of the
reaction wheel. 𝑀𝐶𝑂 is the sum of torques of the system at the axis and 𝑀𝑊 is the sum of torques of
reaction wheel. MO C
is the sum of torques of the system at the axis and MW is the sum of torques of the
the reaction wheel.
reaction wheel.
The system’s moment of the cube robot is given below:
The system’s moment of the cube robot is given below:
𝐼𝐶𝑂 = 𝐼𝑃𝑂 + 𝐼𝑊 𝑂
= 𝐼𝑃𝑂 + 𝐼𝑊 + 𝑚𝑊 𝑙 2 (2)
O O O O 2
I = I + I = I + IW + m W l (2)
where 𝐼𝐶𝑂 is the system’s momentCof the P inertia
W P
about the axis. 𝐼𝑃𝑂 is the moment of inertia of the
𝑂
cube robot’s
where body
ICO is the aboutmoment
system’s the axis,of
while 𝐼𝑊 is the
the inertia about moment of Ithe
the axis. O isinertia of the reaction
the moment of inertiawheel
of theabout
cube
2 P
the axis and can be substituted with
O 𝐼𝑊 + 𝑚 𝑊 𝑙 based on the parallel axis
robot’s body about the axis, while IW is the moment of the inertia of the reaction wheel about the axis theorem. 𝐼𝑊 is the
moment of the inertia of the wheel. 2Moreover, we have
and can be substituted with IW + mW l based on the parallel 𝑃axis theorem. 𝐹 = 𝑚 𝑃 𝑔 and 𝐹 = 𝑚 𝑔, where
IW𝑊 is the𝑤moment of the𝑔 is
gravitational acceleration. Therefore, from Equations (1) and (2), the nonlinear
inertia of the wheel. Moreover, we have FP = mP g and FW = mw g, where g is gravitational acceleration. balancing equation
can be expressed
Therefore, by:
from Equations (1) and (2), the nonlinear balancing equation can be expressed by:
 .. (𝑚𝑃 ⋅ 𝑙𝑃 + 𝑚𝑤 ⋅ 𝑙𝑤 )𝑔 sin 𝜃𝑃 (𝑡) − 𝑇𝑅 (𝑡) . − 𝐶𝑃 𝜃𝑃
̇ . (𝑡) + 𝐶𝑊 𝜃̇𝑊 (𝑡)
𝜃̈𝑃 (𝑡) = ( mP ·lP +mw ·lw ) g sin θ ( t)−TR (t)−CP θP2(t)+CW θW (t)
 θP (t) =
 𝑂P
 𝐼𝑃O + 𝐼𝑊 + 𝑚 𝑙
IP +IW +mW l2 𝑊

(3)

.. 𝑇𝑅 (𝑡) −T𝐶(𝑊 ̇ . (3)
t)𝜃 𝑊 (𝑡)
θ (t) ̈ ..

−C
𝜃̈𝑊 (𝑡)

 θ
=W (t) = R I W W− 𝜃− θP (t).
𝑃 (𝑡).


{ 𝐼𝑊 W

Furthermore,
Furthermore, the brushless motor generating the inner torque is driven driven with
with pulse
pulse width
width
modulation (PWM) and the mathematical equation of the armature circuit can
modulation (PWM) and the mathematical equation of the armature circuit can be expressed as be expressed as follows:
follows:
dia (t) .
va (t) = Rm ·ia (t) + La ⅆ𝑖𝑎 (𝑡) + Ke n∅(t) (4)
𝑣𝑎 (𝑡) = 𝑅𝑚 ⋅ 𝑖𝑎 (𝑡) + 𝐿𝑎dt + 𝐾𝑒 𝑛∅̇(𝑡) (4)
ⅆ𝑡
where Ke is the back electromotive force constant, Rm is the armature resistance, La is the armature
where 𝐾𝑒 is the back electromotive force constant, 𝑅𝑚 is the armature resistance, 𝐿𝑎 is the armature
inductance, va (t) is the armature voltage, ia (t) is the armature current, and ∅(t) is the magnetic flux.
inductance,
. 𝑣𝑎 (𝑡) is the armature voltage, 𝑖𝑎 (𝑡) is the armature current, and ∅(𝑡) is the magnetic
n∅(t) is ̇ the angular velocity of the rotor and n is the pulley ratio. Since the armature inductance
flux. 𝑛∅(𝑡) is the angular divelocity of the rotor and 𝑛 is the pulley ratio. Since the armature
a (t)
La is very small, the term La dt in Equation (4) can be neglected, and then the input torque can be
ⅆ𝑖 (𝑡)
inductance 𝐿𝑎 is very small, the term 𝐿𝑎 𝑎 in Equation (4) can be neglected, and then the input
expressed by: ⅆ𝑡
torque can be expressed by: Kt Kt Ke .
TR (t) = Kt ia (t) = νa ( t ) − n ∅(t) (5)
Rm𝐾𝑡 R𝐾m𝑡 𝐾𝑒
𝑇𝑅 (𝑡) = 𝐾𝑡 𝑖𝑎 (𝑡) = 𝜈 (𝑡) − 𝑛 ̇
∅(𝑡) (5)
𝑅𝑚 𝑎 𝑅𝑚
where 𝐾𝑡 is the moment constant of the motor.
Mathematics 2020, 8, 1840 5 of 16

where Kt is the moment constant of the motor.

From Equations (3)–(5), the balancing equation can be derived as:
 .. K . . .
(mP ·lP +mw ·lw ) g sin θP (t)− Rmt (va (t)−nKe ∅(t))−CP θP (t)+CW θW (t)
θ


 P ( t ) =
IO +IW +mW l2W

(6)

 Kt . .P
 .. ( v ( t ) −nK ∅ ( t )) −C θ
W W (t)
..
 θW (t) = Rm a e
− θP (t).


IW

Table 1 shows the parameters of the cube robot given in Figure 1. Kt , Ke , Rm , La , n are obtained
from the motor manufacturer (MABUCHI company, Chiba, Japan). g = 9.81 m/s2 is the gravitational
acceleration. The other parameters can be obtained from the well-known Newton’s law of motion and
the friction coefficient experiment.

Table 1. System parameters.

Coefficient Value
g (m/s2 ) 9.81
mP (kg) 0.723
mw (kg) 0.162
IPo (kg ·m2 ) 1.37 × 10−2
Iw (kg ·m2 ) 0.3267 × 10−3
lw (m) 0.11
lp (m) 0.095
CP (kg ·m2 ·s2 ) 1.02 × 10−3
Cw (kg ·m2 ·s2 ) 0.6 ×10−3
Kt (N ·m·A−1 ) 38.6 × 10−3
Rm ( Ω ) 0.8158
La (H) 3.6 × 10−3
Ke (v ·web−1 ·s) 1.78 × 10−2
n 30

2.2. The Braking System

Figure 6 shows the braking system’s gadget where a servo motor is used to trigger the spring and
let the brake pad push to the reaction wheel. The black arrow shows the placed position of the brake
pad. Figure 7 shows the server motor of the braking system. The left arrow shows where the servo
motor is placed. The
Mathematics 2020,right arrow
8, x FOR shows where the brake pad is placed, which will move6 ofbackward
PEER REVIEW 16 to
collide with the reaction wheel.

Figure 6. The
Figure gadget
6. The gadget of thebraking
of the braking system.
system.
Mathematics 2020, 8, 1840 6 of 16
Mathematics 2020,
Mathematics 2020, 8,
8,xxx FOR
2020,8, FOR PEER
FOR PEER REVIEW
PEER REVIEW
REVIEW 6 of 15
Mathematics 66 of
of15
15

Figure 7.
Figure7.
Figure The
7. The server
The server motor
server motor of
motor of the
ofthe braking
thebraking system.
brakingsystem.
system.

The
The
The whole
Thewhole
whole braking
wholebraking
braking system
brakingsystem
system
system is
is is shown
shown
is shown
shown in Figure
in Figure
in Figure
in Figure 8,
8, with with
arrow
8, with
8, with arrow A representing
A representing
arrow
arrow A the
the direction
A representing
representing direction
of where
the direction
the direction of
of
of
where
the
wherethe
where the
servo servo
motor
theservo motor
pushes
servomotor pushes
and
motorpushes and
arrow
pushesand
andarrowarrow
B B expressing
expressing the
arrowBBexpressing the
brake
expressingthe brake
pad’s
thebrake pad’s direction
direction
brakepad’s moving
pad’sdirection moving
directionmovingfrom
movingfrom from
anotheranother
side.
fromanother
another
side.
After
side. After
the
After the
servo
the servo
motor
servo motor
motor triggers
triggers the
triggers the
braking
the braking
system,
braking system,
the
system, the
brake
the brake
pad pad
attached
brake pad attached
to the
attached
side. After the servo motor triggers the braking system, the brake pad attached to the gadget to the
gadget
to the gadget
will will
collide
gadget will
will
collide
with the with
collide with
collide the
reaction
with the reaction
wheel.
the reaction wheel.
Thus,
reaction wheel. theThus,
wheel. Thus, the
design
Thus, the design
can stop
the design can
design canthestop the
reaction
can stop
stop the reaction
wheel
the reaction wheel efficiently.
efficiently.
reaction wheel
wheel efficiently.
efficiently.

Figure
Figure
Figure 8.
Figure8. The
8. The
8. braking
The braking system.
braking system.
system.

2.3.
2.3. Signal
2.3. Signal
2.3. Processing
Processing Units
Processing
Signal Processing Units
Units
Units
Figure
Figure9999shows
Figure
Figure showsthe
shows
shows thesignal
the
the signalprocessing
signal
signal processingunits
processing
processing unitsof
units
units ofthe
of
of thecube
the
the cuberobot.
cube
cube robot.The
robot.
robot. TheSTM32F407
The
The STM32F407 evaluation
STM32F407
STM32F407 evaluation board
evaluation
evaluation board
board
board
(which
(which mounted
mounted
(which mounted
(which a
a Cortex-M3
Cortex-M3
mounted aa Cortex-M3 clocked
clocked
Cortex-M3 clocked at
at
clocked at 72MHZ)
72MHZ)
at 72MHZ)
72MHZ) is is
is the
the
is the primary
primary
the primary system
system
primary system processing
processing
system processing unit.
unit.
processing unit. The
The IMU
The
unit. The IMU
IMU
IMU
sensor
sensor
sensor consists
consists
consistsof
of
ofa 3-axis
a
a 3-axisaccelerometer,
3-axis accelerometer,
accelerometer, ADXL345
ADXL345
ADXL345 made made
madefrom Analog
from
from Analog
AnalogDevices,
sensor consists of a 3-axis accelerometer, ADXL345 made from Analog Devices, and a 3-axis gyro and
Devices,
Devices, a 3-axis
and
and a
a gyro
3-axis
3-axis made
gyro
gyro
from
made InvenSense.
from
made from
made The
InvenSense.
from InvenSense. IMU
InvenSense. The The sensor
IMU
The IMU that
sensor
IMU sensor mounts
that
sensor that on
mounts
that mounts the
mounts on cube
on the
on the robot’s
cube
the cube backside
robot’s
cube robot’s is connected
backside
robot’s backside is
backside is to
connected
is connected the
connected to STM
to the
to the
the
2 22
32F407
STM board.
32F407
STM 32F407
STM And
board.
32F407 board. the
And
board. And serial
the
And the communication
serial
the serial between
communication
serial communication them
between
communication between
between them is Inter-Integrated
them
them isis Circuit
Inter-Integrated
is Inter-Integrated (I C).
Circuit
Inter-Integrated Circuit (I C).
C).
Circuit (I(I2 C).

Figure
Figure 9.
9. The
The signal
signal processing
processing units
units of
of the
the cube
cube robot.
robot.
Figure 9.
Figure 9. The
The signal
signal processing
processing units
units of
of the
the cube
cube robot.
robot.

A
A 20
20 W
W brushless
brushless motor
motor ID-549-XW
ID-549-XW made
made by
by MABUCHI
MABUCHI company
company is
is chosen
chosen toto drive
drive the
the cube
cube
A 20
A 20 W
W brushless
brushless motor
motor ID-549-XW
ID-549-XW made made byby MABUCHI
MABUCHI company
company is is chosen
chosen to to drive
drive thethe cube
cube
robot’s
robot’s reaction
reaction wheel
wheel due
due to
to its
its simple
simple control
control method
method and
and high
high efficiency
efficiency compared
compared to
to the
the brush
brush
robot’s reaction
robot’s reaction wheel
wheel due
due to
to its
its simple
simple control
control method
method and
and high
high efficiency
efficiency compared
compared to to the
the brush
brush
motor.
motor. The
The optical
optical encoder
encoder of
of the
the brushless
brushless motor
motor is
is used
used to
to sense
sense the
the velocity
velocity 𝜃
𝜃̇ ̇𝑊
̇ of
of the
the reaction
reaction
motor. The optical encoder of the brushless motor is used to sense the velocity 𝜃𝑊 𝑊 of the reaction
wheel. The
wheel. The
wheel. brushless
The brushless motor
brushless motor is
motor is driven
is driven
driven byby the
by the PWM
the PWM signal,
PWM signal, which
signal, which is
which is generated
is generated from
generated from
from the the STM32F407
the STM32F407
STM32F407
Mathematics 2020, 8, 1840 7 of 16

.
motor. The optical encoder of the brushless motor is used to sense the velocity θW of the reaction
wheel. The brushless motor is driven by the PWM signal, which is generated from the STM32F407
board. The RC servo motor, ES08MAII, which triggers the braking system, is also driven by the pulse
width modulation (PWM) signals.
The STM32 port of the timer interrupt is used as the software’s framework due to its high accuracy
sampling time, which allows us to design the appropriate filter for the IMU sensor. Furthermore,
to verify the robustness and stability of the cube robot, we use the peripheral in STM32F407 control
panel, DMA, to collect the state parameter data such as the cube robot’s angle.

3. Estimation and Control

3.1. Attitude and Heading Reference System

The angle, angular velocity, and reaction wheel’s velocity are essential state variables of the cube
robot. Thus, to acquire the rotation angle of the cube robot correctly, the pitch angle θP , one of the
.
variables of the Euler angle, and its angular velocity θP are obtained by the gyro on the IMU sensors.
The Euler angle provides a way to represent the 3D orientation of an object by using a combination
of three rotations about different axes. For instance, the rotation of the y-axis is described as Pitch,
and the rotation of the z-axis is described as Yaw. Here, the quaternion is applied to estimate the
Euler angles and to avoid the Gimbal lock effect [13]. The quaternion-derived rotation matrix can be
expressed in terms of qi as follows [14,15]:
 0  
 q1   1 − 2(q2 2 + q3 2 )
 
2(q1 q2 − q0 q3 ) 2(q0 q2 + q1 q3 )  q1 
 0  
1 − 2(q1 2 + q3 2 )
 q  = 2(q1 q2 + q0 q3 )  
 2   2(q2 q3 − q0 q1 )  q2  (7)
 0  
1 − 2 ( q1 2 + q2 2 )
 
q3 2(q1 q3 − q0 q2 ) 2(q2 q3 + q0 q1 ) q3

where qi , i = 1, 2, 3 are the three directions of q, and q is a unit quaternion, q0i i = 1, 2, 3 are the three
directions of q0 , and q0 is the estimation of q.
After calculating the quaternion-derived rotation matrix, the Euler angle can be obtained from the
quaternions via the above equation:

2(q0 q1 + q2 q3 )
θP = tan−1 ( )
1 − 2(q1 2 + q2 2 )
θR = sin−1 (2(q0 q2 − q1 q3 )) (8)
2(q0 q3 + q1 q2 )
θY = tan−1 ( )
1 − 2(q2 2 + q3 2 )

where θP denotes Pitch, θR denotes Roll, and θY denotes Yaw. By using the relationship between the
Euler angle and quaternion, the Pitch angle of the cube robot can be obtained.
The quaternion inside the rotation matrix will have drift error after long-term usage. To avoid that,
the system needs to constantly update the quaternion using the one-order Runge–Kutta method [16].
The equation of the one-order Runge–Kutta is shown below:

Td dQ
Qt+Td = Qt + · (9)
2 dt
dQ
where Qt = [q0 q1 q2 q3 ]Tt denotes the initial status of the quaternion, dt denotes the derivative of the
quaternion Qt at the time t, and the Qt+Td denotes the estimation of the quaternion Qt at the time t + Td .
Td > 0 is the sampling time.
Mathematics 2020, 8, 1840 8 of 16

After applying the Runge–Kutta method, the equation of the quaternion updated can be derived as:

−ωx q1 − ω y q2 − ωz q3
 0       
 q0   q0   q0   
ωx q0 + ωz q2 − ω y q3
 0  
 Td
    
 q   q1   q1  
 1  =  =  q  + (10)
    
 q0   q 
 2   2   2   ω y q0 − ωz q1 − ωx q3  2

ωz q0 + ω y q1 − ωx q2
 0  
q3 q3 t + T q3 t
    
d
Mathematics 2020, 8, x FOR PEER REVIEW 8 of 15
where ωx , ω y , and ωz denote the angular velocity of the three axes x, y, and z, respectively.
Figure 10
Figure 10 illustrates
illustrates how
how thethe attitude
attitude and
and heading
heading reference
reference system
systemoperates
operatesin inthe
thecube
cuberobot.
robot.
First, the acceleration and angular velocity values sensed by MPU6050 are filtered
First, the acceleration and angular velocity values sensed by MPU6050 are filtered by a low-pass filter.by a low-pass
filter. Second,
Second, the vector
the vector in the in the navigation
navigation frameframe is converted
is then then converted
into theintobody
the body
frameframe by using
by using the
the quaternion matrix. Using the outer product, the error of the gravity vector
quaternion matrix. Using the outer product, the error of the gravity vector between the body frame between the body
frame
and and navigation
navigation frame
frame can can be calculated.
be calculated. Third, PI
Third, a proper a proper PI controller
controller is used to
is used to support thesupport the
prediction
prediction
while while responding
responding to the high-frequency
to the high-frequency changeschanges in the[16].
in the error errorAfter
[16]. calculating
After calculating the error,
the error, we
we compensate for the gyro error and use the quaternion differential equation to update
compensate for the gyro error and use the quaternion differential equation to update the quaternion. the quaternion.
At last,
At last, the
the cube
cube robot angel, θ𝜃P𝑃,, can
robot angel, can be
be calculated
calculated via
via the
the relationship
relationship between
between thethe quaternion
quaternion andand
Euler angle.
Euler angle.

Figure 10.
Figure The schematic
10. The schematic diagram
diagram of
of the
the attitude
attitude and
and heading
heading reference
reference system (AHRS).

3.2. Balancing
3.2. Balancing Control
Control
◦
After linearization
After linearization Equation
Equation (3)
(3) at
at the
the equilibrium point θ𝜃P =
equilibrium point = 0° ,, the
0 the dynamic
dynamic equation
equation can
can be
be
𝑃
represented by the state space formula:
represented by the state space formula:
.
x𝑥̇(t(𝑡)
) ==Ax
𝐴𝑥(𝑡)
(t) ++Bu
𝐵𝑢(𝑡)
(t) (11)
𝑇
where 𝑥(𝑡) = h[𝜃𝑃 (t), 𝜃̇𝑃. (𝑡), 𝜃̇𝑊. (𝑡)] iTis a vector of state variables, and A, B are the system matrices
where x(t) = θP (t), θP (t), θW (t) is a vector of state variables, and A, B are the system matrices
defined below:
defined below:
0 1 0
(𝑚𝑃 𝑙𝑃 + 𝑚𝑊 𝑙𝑊 )𝑔 −𝐶𝑃 𝐶𝑊
𝐴= 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2
(𝑚𝑃 𝑙𝑃 + 𝑚𝑊 𝑙𝑊 )𝑔 𝐶𝑃 −𝐶𝑊 (𝐼𝑃𝑜 + 2𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 )
− 𝑜 2
[ 𝐼𝑃 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 ]
0 (12)
−𝐾𝑡
𝐵= 𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2
−𝐾𝑡 (𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 2 )
2
[ 𝐼𝑊 (𝐼𝑃𝑜 + 𝐼𝑊 + 𝑚𝑊 𝑙𝑊 ) ]

After calculating the state-space model, we can determine whether the cube robot’s model is
Mathematics 2020, 8, 1840 9 of 16

 
 0 1 0 
 (mP lP +mW lW ) g −CP CW 
 
A =  IPo +IW +mW lW 2 IPo +IW +mW lW 2 IPo +IW +mW lW 2 

 (m l +m l ) g
 − P P W W CP −CW (IPo +2IW +mW lW 2 ) 

2
o
IP +IW +mW lW IPo +IW +mW lW 2 Io +IW +mW lW 2
 P (12)
 0 

 −Kt 

B =  IPo +IW +mW lW 2 


 −Kt (IPo +IW +mW lW 2 ) 

IW (IPo +IW +mW lW 2 )

After calculating the state-space model, we can determine whether the cube robot’s model
is controllable.

3.3. System Controllability

To verify the system’s controllability, the continuous-time controllability matrix [17] can be
obtained by using the parameters in Table 1 and the Equation (12). The matrix is shown as:
 
 0.001 0 −0.003 
 
Wc =  0 0.2367 −3.8605 
 (13)

−0.003 −3.8605 65.3239


and its eigenvalues are: h i

eig(Wc ) = 0.0001 0.0085 65.5521 (14)

and the corresponding eigenvectors v1 , v2 , v3 are:

 
 −0.9997 −0.0245 0 
 
[v1 , v2 , v3 ] =  0.0245 0.998 −0.0591  (15)
 
0.0015 0.0591 0.9983

The rank of a matrix Wc is 3, which implies that the system is fully controllable. According to
reference [18], the smaller the eigenvalue 0.0001, the larger the input energy will be needed to drive the
system from the arbitrary state to the state of its corresponding eigenvector v1 . As the eigenvalues
.
are shown above, the velocity of the reaction wheel θW has the biggest eigenvalue 65.5521 in the
continuous-time controllability gramian matrix. This means that the corresponding eigenvector to the
biggest eigenvalue is the most controllable direction in state space [19]. In other words, the system will
.
have a greater change in the direction of the state parameters, θW , on the same input energy compared
.
to the other two state variables. This is the exact reason why the state variable θW needs a parallel
controller to balance the cube robot.

3.4. System Controller

The solution of the state space Equation (11) can be derived as:
Z t
A(t−t0 )
x(t) = e x(t0 ) + eA(t−τ) Bu(τ)dτ. (16)
t0

Let the initial time t0 = kTd , t = kTd + Td , the equation can be derived as:
Z kTd +Td
x(kTd + Td ) = eA(Td ) x(kTd ) + eA(kTd +Td −τ) Bu(τ)dτ. (17)
kTd
Mathematics 2020, 8, 1840 10 of 16

By normalizing the sampling time Td = 1, the continuous-time system given by Equation (12) can
be discretized by sampling and zero-order hold. Then the discrete-time model is given below:

x[k + 1] = Ad x[k] + Bd u[k], k ∈ N0

ATd (ATd )2
Ad = eATd = I + + ···
2! 3!
Z Td
Bd = eAρ dρ·B
0
Therefore, we have:  
 1.0321 0.0202 0 
 
A =  3.23
 1.0283 0.0002 


−3.2249 −0.0282 0.9967

  (18)
 −0.0015 
 
Bd =  −0.1467 .
 
2.5552
Ad and Bd represent the system matrix of discrete-time, corresponding to the system matrix A and
B of continuous-time. For the discrete-time system of Equation (18), a discrete-time PID controller is
designed as:
Xk
u(k) = KP e(k) + Ki e(i) + Kd [e(k) − e(k − 1)] (19)
Mathematics 2020, 8, x FOR PEER REVIEW i=0 10 of 15

where u(k) is the input at the sampling time k, e(k) is the error at the sampling time status k, e(k − 1) is
where 𝑢(𝑘) is the input at the samplingPtime k, 𝑒(𝑘) is the error at the sampling time status 𝑘,
the error at the sampling time (k − 1), and ki=0 e(i) is the summation of the error from sampling time
𝑒(𝑘 − 1) is the error at the sampling time (𝑘 − 1), and ∑𝑘𝑖=0 𝑒(𝑖) is the summation of the error from
0 to k. KP , Ki , and Kd are the parameters of the discrete-time PID controller. Figure 11 illustrates the
sampling time 0 to 𝑘. 𝐾𝑃 , 𝐾𝑖 , and 𝐾ⅆ are the parameters of the discrete-time PID controller. Figure
parallel PID controller structure designed to balance the cube robot.
11 illustrates the parallel PID controller structure designed to balance the cube robot.

Figure 11.The
Figure11. Thestructure
structureof
ofthe
thecube
cuberobot
robotbalancing
balancingcontrol.
control.

To
Tobalance
balancethe
therobot,
robot,two
twokinds
kindsofofPID
PIDcontrollers,
controllers,including
includingangle
anglebalance
balancecontroller
controllerand
andwheel
wheel
velocity
velocity controller, are processing the control signal at the same time in parallel to achieve the balance.
controller, are processing the control signal at the same time in parallel to achieve the balance.
The
Theangle
angleerror
errorPID
PIDcontroller
controlleraims
aimsatatthe
thecontrol
controlofofthe
therobot angleθP𝜃.𝑃 .The
robotangle Thewheel
wheelvelocity
velocityPID
PID
controller is used to control the angular velocity of the reaction wheel (or the brushless motor).
controller is used to control the angular velocity of the reaction wheel (or the brushless motor). BasedBased
on
onthe
thecube
cuberobot’s
robot’ssystem
systemparameters
parametersgiven
givenininTable
Table1,1,the
theparameters
parametersof ofthe
theproposed
proposedparallel
parallelPID
PID
are given in Equation (20). Moreover, the system disturbance and sensor fusion will influence the
acceleration and angular velocity measurements’ accuracy. Thus, to reduce the low-frequency noise,
we apply a one order Low-pass filter on the acceleration detection. The cut-off frequency of the low
pass filter is 25 Hz, and the sampling frequency is 200 Hz. Furthermore, to increase the smoothness
and reduce the low-frequency noise, we apply a second-order butterworth filter on the detection of
 Mathematics 2020, 8, 1840 11 of 16

are given in Equation (20). Moreover, the system disturbance and sensor fusion will influence the
acceleration and angular velocity measurements’ accuracy. Thus, to reduce the low-frequency noise,
we apply a one order Low-pass filter on the acceleration detection. The cut-off frequency of the low
pass filter is 25 Hz, and the sampling frequency is 200 Hz. Furthermore, to increase the smoothness
and reduce the low-frequency noise, we apply a second-order butterworth filter on the detection of the
angular velocity. The cut-off frequency of the butter worth filter is 40 Hz, and the sampling frequency is
200 Hz. The parameters of the PID controller and coefficient of the filters and PI controller in Figure 11
can be designed as:
Parallel PID controller:

Angle error PID : KP = 890, KI = 0.01, KD = 53.2

(20)
Wheel velocity PID : KP = 82, KI = 0.45, KD = 0.001.

AHRS PI controller and filters coefficient:

Mathematics 2020, 8, x FOR PEER REVIEW AHRS PI : KP = 2.0, KI = 0.001 11 of 15

1
Low − pass filter : HLPF (z) = (21)
0.4424z + 0.5576
3.5. Bouncing Control 0.207z2 + 0.413z + 0.207
Butterworth filter : HBWF (z) =
z2 − 0.369z + 0.196
Figure 12 shows the cube robot’s action bouncing from the initial state at the horizontal surface.
3.5. Bouncing
By applying Control
the conservation of the energy and conservation of the angular momentum, the bouncing
equation of the cube
Figure robot the
12 shows is given below:
cube robot’s action bouncing from the initial state at the horizontal surface.
By applying the conservation of the energy and conservation of the angular momentum, the bouncing
𝑜
equation of the cube robot is given below:𝐼𝑤 𝜔𝑤 = 𝐼𝑐 𝜔𝑃
1 𝑜 2 𝜋 (22)
𝐼 𝜔 1 o 𝑙2 + 𝑚 𝑙 )𝑔 (1 − 𝑠𝑖𝑛 )π
o = (𝑚
Iw2ωw = Ic ωP Ic ωw = (mP lP + mw lw ) g(1 − sin
𝑐 𝑤 𝑃 𝑃 𝑤 𝑤
4 4) (22)
2
o
andIc 𝐼and
where 𝐼𝑐𝑜where Iw are represented as above, ωP represents the angular velocity of the cube robot, and ωw
𝑤 are represented as above, 𝜔𝑃 represents the angular velocity of the cube robot,
is the angular velocity of the reaction wheel. From Equation (21), we have:
and 𝜔𝑤 is the angular velocity of the reaction wheel. From Equation (21), we have:
v
t
√ (IW mW𝑊l2W
++𝑚 o
(𝐼𝑊 𝑙2 +
+𝐼I𝑜P))
ω𝑤
𝜔 = √(2
w = (2 −− √
2)2) 2
𝑊 𝑃
𝑔g(m
𝑚B𝐵lB𝑙𝐵++
mW 𝑊)𝑙.𝑊 ).
𝑚lW (23) (23)
𝐼2𝑊
IW

Figure
Figure 12. The
12. The processofofthe
process the cube
cube robot
robotbouncing control.
bouncing control.
The ωw derived in Equation (22) is the reaction wheel’s minimum velocity to bounce up the cube
𝜔𝑤 derived
Therobot. in Equation
After triggering (22) system,
the braking is the reaction wheel’s
once the cube robot minimum
has reached velocity
the nearbytobalancing
bounce up the
cube robot. Afterthe
position, triggering thecontroller
parallel PID brakingmentioned
system, once
abovethe
willcube robot
balance has reached the nearby balancing
the system.
position, the parallel PID controller mentioned above will balance the system.

4. Realization of the System

4.1. Bouncing Procedure

Figure 13 illustrates the bouncing procedure of the cube robot. First, the reaction wheel starts to
rotate to generate enough kinetic energy. After saving enough energy, the brushless motor sets for
zero input. Simultaneously, the servo motor triggers the braking system to stop the reaction wheel.
cube robot. After triggering the braking system, once the cube robot has reached the nearby balancing
position, the parallel PID controller mentioned above will balance the system.

4. Realization of the
Mathematics System
2020, 8, 1840 12 of 16

4.1. Bouncing
4. Procedure
Realization of the System

Figure 4.1.
13 illustrates the bouncing procedure of the cube robot. First, the reaction wheel starts to
Bouncing Procedure
rotate to generate enough
Figure kinetic
13 illustrates energy.procedure
the bouncing After saving enough
of the cube energy,
robot. First, the brushless
the reaction wheel startsmotor
to sets for
zero input. rotate to generate enough
Simultaneously, thekinetic
servoenergy.
motor After saving enough
triggers energy, the
the braking brushlesstomotor
system stopsets
theforreaction
zero wheel.
input. Simultaneously, the servo motor triggers the braking system to stop the reaction wheel. During
During the bouncing, we set the brushless motor to rotate in a counterclockwise direction. Due to
the bouncing, we set the brushless motor to rotate in a counterclockwise direction. Due to Newton’s
Newton’s Third Law,
Third Law, thethe
same same
amountamount
of torqueofmade
torque made
by the bywheel
reaction the reaction
also applieswheel alsorobot
to the cube applies
in the to the cube
robot in theopposite
opposite direction.
direction. Thus, itThus,
allowsitthe
allows thetocube
cube robot bouncerobot to bounce
up more easily. up more easily.

Figure Figure
13. The13. flow chart
The flow chartofofthe cuberobot
the cube robot bouncing
bouncing control.control.

4.2. Bouncing Up and Balancing Procedure

As shown in Figure 14, the cube robot’s procedure bouncing up and balancing can be divided
into two parts: The left part is the main loop and the right part is the timer interrupt loop. The main
loop is designed to be a while loop constantly checking whether the pitch angle of the cube robot is
smaller than −39◦ (or bigger than 39◦ ). The timer interrupt is designed to set the system to update
the accelerations, the angular velocities, and the quaternion at every 5 ms. Once the pitch angle is
smaller than −39◦ , which means the cube robot is lying on the horizontal floor, the reaction wheel will
start rotating to generate the kinetic energy. The servo motor will trigger the braking system to collide
with the reaction wheel after the reaction wheels rotates for 5 s. After the collision, once the cube robot
has arrived near the equilibrium position (which is −6◦ ~−39◦ ), the parallel PID controller mentioned
above will start its function for balancing the cube robot.
the accelerations, the angular velocities, and the quaternion at every 5 ms. Once the pitch angle is
smaller than −39° , which means the cube robot is lying on the horizontal floor, the reaction wheel
will start rotating to generate the kinetic energy. The servo motor will trigger the braking system to
collide with the reaction wheel after the reaction wheels rotates for 5 s. After the collision, once the
cube robot2020,
Mathematics has8,arrived
1840 near the equilibrium position (which is −6° ~−39° ), the parallel PID controller
13 of 16
mentioned above will start its function for balancing the cube robot.

Figure
Figure 14.
14. The
The flow
flow chart
chart of
of bounce
bounce and
and balance
balance control.
control.

5. Experimental Results
To implement
implement the thediscrete-time
discrete-timeparallel
parallelPID PIDcontroller,
controller, the
the sampling
sampling timetime𝑇ⅆTof
d of
thethe system
system is
is set
setbe
to to5be 5 ms.
ms. In the Incube
the cuberobotrobot balancing,
balancing, the input
the input voltage
voltage is 20
is 20 V, andV,the
andPIDtheparameters
PID parameters are given are
given
in in Equation
Equation (20). The (20). The system’s
system’s robustnessrobustness can beunder
can be tested testedtwounder two circumstances.
circumstances. The first The first
additive
additive disturbance
disturbance is added is to added
the cubeto the
robot cube robot to determine
to determine the extentthe of extent of disturbance
disturbance without changingwithout
changing balancing
balancing status. The status.
second The second
one onethe
is to lift is to lift the platform
platform up to determine
up to determine whetherwhether
the cubethe cancube
remaincan
remain balanced
balanced at differentat different inclination
inclination angles.angles. Therefore,
Therefore, we weusedused striking
striking totogenerate
generateexternal
external force
disturbances, as
disturbances, asshown
shownininthe provided
the provided video (see(see
video Supplementary
Supplementary Materials), to make
Materials), the square
to make the squarerobot
deviate
robot with 5.13
deviate withdegrees, and then
5.13 degrees, andlift uplift
then andupput
and down the platform
put down to disturb
the platform the cube
to disturb the robot.
cube After
robot.
. .
After those disturbances,
those disturbances, from Figures
from Figures 15–17, we 15–17, we can that
can observe observe that variables
the state θP , θP , θw(𝜃
the state (variables ̇
) converge ̇
𝑃 , 𝜃𝑃 , 𝜃𝑤 )
°
converge ◦ to /s),
to (0.27 , 0( ◦ (0.27 , 0(°⁄𝑠 ), −330(rpm)).
−330(rpm)). The experimental The experimental
results reveal results reveal
that after that disturbed
being after beingbydisturbed
external
by external disturbances,
disturbances, the cube robot the can
cubequickly
robot can quickly
return return
to the to the equilibrium
equilibrium position. Inposition.
terms ofInthe terms
speed of
.
θw of
the speed ̇
𝜃𝑤 ofwheel,
the inertia the inertia wheel,
the motor willthe motor will
maintain maintain a counterclockwise
a counterclockwise rotation duringrotation
balance,during about
balance,
−330(rpm ). The−330(rpm).
about reason is that Thethe
reason
control is that the control
algorithm used algorithm
in this paperused in this
is the PIDpaper
control is method,
the PID
control method,
and it cannot and it cannot
effectively effectively
save energy loss.save energy loss.
Mathematics 2020, 8, x FOR PEER REVIEW 13 of 15

Figure 15. Time response of the tilt angle 𝜃𝑃 of the cube robot balancing on its edge.
Figure 15. Time response of the tilt angle θP of the cube robot balancing on its edge.

Figure 16. Time response of the tilt angle angular velocity 𝜃̇𝑃 of the cube robot balancing on its edge.
Mathematics 2020, 8, 1840 14 of 16

Figure 15. Time response of the tilt angle 𝜃𝑃 of the cube robot balancing on its edge.
Figure 15. Time response of the tilt angle 𝜃𝑃 of the cube robot balancing on its edge.
Figure 15. Time response of the tilt angle 𝜃𝑃 of the cube robot balancing on its edge.

.̇
16. Time response of the tilt angle angular velocity 𝜃
Figure 16. ofthe
the cuberobot
robot balancingon
on itsedge.
edge.
Figure 16. Time response of the tilt angle angular velocity θ𝜃P𝑃̇𝑃 of
Figure of thecube
cube robotbalancing
balancing onits
its edge.
Figure 16. Time response of the tilt angle angular velocity 𝜃̇𝑃 of the cube robot balancing on its edge.

Figure 17. Time response of the motor speed 𝜃 ̇ of the cube robot balancing on its edge.
Figure 17. Time response of the motor speed .𝜃𝑤 ̇𝑤 of the cube robot balancing on its edge.
Figure 17. of the motor speed θ ̇
Figure 17. Time response of the motor speed 𝜃𝑤 of
Time response w ofthe
thecube
cuberobot
robotbalancing
balancingon
onits
itsedge.
edge.
Next,
Next, wewe experiment
experiment with with bouncing
with bouncing
bouncing and and balancing.
and balancing.
balancing. As As shown
shown in
As shown Figure
in Figure
Figure 18, 18,
18, the the full
the full bounce-up
full bounce-up
bounce-up
Next,
Next, we we experiment
experiment with bouncing and balancing. As ° shown in in Figure 18, the full bounce-up
and
and balancing
balancing experiment
experiment starts
starts with
with the
the rest
rest at
at 𝜃
𝜃 = −30
𝑃 = −30◦ . As shown in Figure 19, the brushless
° . As shown in Figure 19, the brushless
at θ𝜃P𝑃and
and balancing experiment starts with the rest at 𝑃 = −30 °. As shown in Figure 19, the brushless
and
motor balancing
accelerates experiment
the starts
reaction with to
wheel the3000
rest rpm, = −30
then . Asthe shown
servo in Figure
motor triggers 19, thethebrushless
motoraccelerates
motor acceleratesthe the reaction wheel to 3000 rpm, and then the servo motor triggers the braking
braking
motor
system accelerates
to collide thereaction
with reaction
the
wheel
wheel
reaction
to 3000
to 3000
wheel,
rpm,rpm,
allowing
and and then
the
the servo
then
cube robot
motormotor
the servo
to
triggers
bounce up.
the braking
triggers
Furthermore,
system
the braking
from
system
to collide to collide
with the with the
reaction reaction
wheel, wheel,
allowing allowing
the cube the
robotcube
to robot
bounce to
up.bounce up.
Furthermore, Furthermore,
from Figurefrom
20,
system
Figure to collide
20, we with the
observe reaction
that the wheel,velocity
angular allowing the cuberapidly
becomes robot to bounce
high during up. the Furthermore,
bounce. In from
this
Figure
we 20,
observe we
that observe
the that
angular the angular
velocity velocity
becomes becomes
rapidly high rapidly
during high
the during
bounce. the
In bounce.
this In
experiment,this
Figure 20, wewe
experiment, observegive that the angular velocity becomes rapidly high during the bounce. In this
experiment,
we also give we also
external give external
externalbydisturbances
also disturbances disturbances
striking the
by
by striking
robot striking
and
the
the robot
lifting robot
the
and
and lifting
platform. lifting the
the platform.
According platform.
to the
experiment,
According towe the also give external disturbances by striking the robot (𝜃 and𝑃 ,, 𝜃
̇𝑃̇ , 𝜃̇𝑤
lifting ̇𝑤)) the platform.
Accordingstate the measured
to histories, measured state
state histories,
histories,
( θ θ
.
θ
the
. state
state variables
the) converge variables ◦ ,(𝜃 𝑃 ◦𝜃/s), 𝑃 , 𝜃870 converge
converge to
to
measured
According
(−1.03 °
° , 0 (° to
⁄ 𝑠 ), the
870 the
measured
(rpm)) state
and variables
state
we can P ,
histories,
conclude P , the
w
that state
the cube to (−1.03
variables
robot can (𝜃0 (,
bounce
𝑃 𝑃 𝑤 𝜃̇ , 𝜃 ̇ )
and (rpm)) and
converge
balance we
to
very
(−1.03° , 0 (°⁄𝑠 ), 870 (rpm)) and we can conclude that the cube robot can bounce and balance very
can
(−1.03conclude
robustly. that
, 0 (°⁄𝑠can), 870the(rpm))
cubeequilibrium
robot
and can
we bounce
can and
conclude balance
that the very
−1.03cube ° robustly.
robot canWe can
bounce findand thebalance
equilibrium
very
robustly.isWe
position We
about
find
find the
can−1.03 degrees. The
position
overall
is about
center of gravity is
degrees. The
◦the equilibrium position is about −1.03° degrees. The overall center of gravity
overall
° slightly deviated from the center point
center of gravity
robustly.
is We can find the equilibrium positionto is about −1.03 degrees. The overall center of gravity
is slightly
due slightly deviated
deviated
to the deviated
brake
from
from the
device. the center
center point
point due
due to the the brake
brake device.
device.
is slightly from the center point due to the brake device.

Figure 18. Time response of the tilt angle θP of the cube robot bouncing and balancing on its edge.
Mathematics 2020, 8, x FOR PEER REVIEW 14 of 15
Mathematics 2020, 8, x FOR PEER REVIEW 14 of 15
Mathematics 2020, 8, 1840 15 of 16
Figure 18. Time response of the tilt angle 𝜃𝑃 of the cube robot bouncing and balancing on its edge.
Figure 18. Time response of the tilt angle 𝜃𝑃 of the cube robot bouncing and balancing on its edge.

Figure 19. Time response of the motor speed 𝜃̇ of the cube robot bouncing and balancing on its
. 𝜃̇𝑤 of the cube robot bouncing and balancing on its
Figure 19. Time response of the motor speed 𝑤
edge. 19. Time response of the motor speed θw of the cube robot bouncing and balancing on its edge.
Figure
edge.

Figure 20. Time response of the tilt angle angular velocity . 𝜃̇̇𝑃 of the cube robot bouncing and
Figure 20.Time
Figure 20. Timeresponse of the
response tilt angle
of the angular
tilt angle velocity
angular θP of the
velocity 𝜃𝑃 cube robot
of the bouncing
cube and balancing
robot bouncing and
balancing on its edge.
on its edge.on its edge.
balancing

6. Conclusions
6.
6. Conclusions
This paper
This paper presented
presented the
the mathematical
mathematical modeling and realization of a cube robot with with aa PID
PID
This paper presented the mathematical modeling and realization of a cube robot with a PID
balancingcontroller.
balancing controller.
TheThe nonlinear
nonlinear dynamical
dynamical modelmodel of the
of the cube robotcube robot was
was derived derived
and its and its
controllability
balancing controller. The nonlinear dynamical model of the cube robot was derived and its
controllability
analyzed. analyzed.braking
A mechanical A mechanical
system braking system
was designed towas designed
convert to convert
the kinetic energythe
tokinetic energy
potential energyto
controllability analyzed. A mechanical braking system was designed to convert the kinetic energy to
potential
and bounce energy
up theand bounce
robot. up the robot.
Furthermore, Furthermore,
the attitude the attitude
and heading referenceand heading
system reference
(AHRS) system
and parallel
potential energy and bounce up the robot. Furthermore, the attitude and heading reference system
(AHRS)
PID and parallel
controller PID controller
were proposed to balancewere proposed
the robot. to balance
The PID the effectiveness
controller’s robot. The PIDwas controller’s
verified by
(AHRS) and parallel PID controller were proposed to balance the robot. The PID controller’s
effectiveness
the was verified
balance control experimentby and
the robustness
balance control experiment
experiment, and robustness
as demonstrated in the experiment,
attached video as
effectiveness was verified by the balance control experiment and robustness experiment, as
demonstrated
(see in theMaterials).
Supplementary attached video (see controller
The PID Supplementary could Materials).
keep the cubeTherobot
PID controller
balancing could keep
with good
demonstrated in the attached video (see Supplementary Materials). The PID controller could keep
the cube robot
robustness balancingInwith
performance. futuregood robustness
research, we willperformance. In future
discuss improving research,
the PID we will
controller’s discuss
design so
the cube robot balancing with good robustness performance. In future research, we will discuss
improving
that the PIDand
its robustness controller’s design
energy loss can so
be that its robustness and energy loss can be optimized.
optimized.
improving the PID controller’s design so that its robustness and energy loss can be optimized.
Supplementary Materials: The
Supplementary Materials: The Supplementary
Supplementary Materials
Materialsisisavailable
availableonline
onlineatathttp://www.mdpi.com/2227-7390/
www.mdpi.com/xxx/s1.
Supplementary Materials: The Supplementary Materials is available online at www.mdpi.com/xxx/s1.
8/10/1840/s1.
Author Contributions: All authors contributed
Author Contributions: contributed to the paper.
paper. T.-L.L.
T.-L.L. proposed
proposed thethe research
research idea
idea of
of the
the cube
cube robot
robot
Author Contributions:
system. Allthe
authors contributed to the paper. T.-L.L. proposed the research idea of thefrom
cubeT.-L.L.
robot
system. S.-J.C.
S.-J.C. conducted
conducted the theoretical
theoretical derivation
derivation and
and wrote
wrote the manuscript
the manuscript with
with the supervision
the supervision from T.-L.L.
system.isS.-J.C.
C.-C.C. conducted
responsible themechanical
for the theoretical derivation
and hardwareand design
wrote the
andmanuscript with the supervision
J.-J.Y. is responsible fromdesign.
for the firmware T.-L.L.
C.-C.C. is responsible for the mechanical and hardware design and J.-J.Y. is responsible for the firmware design.
All authors
C.-C.C. have read and
is responsible agreed
for the to the published
mechanical version
and hardware of the
design manuscript.
and J.-J.Y. is responsible for the firmware design.
All authors have read and agreed to the published version of the manuscript.
All authors have read and agreed to the published version of the manuscript.
Funding: This work was financially supported by the Ministry of Science and Technology, Taiwan, under grant
Funding: This work was financially
MOST-108-2221-E-006-214-MY2 and supported by the Ministry of Science and Technology, Taiwan, under grant
MOST-109-2221-E-167-017.
Funding: This work was financially supported by the Ministry of Science and Technology, Taiwan, under grant
MOST-108-2221-E-006-214-MY2 and MOST-109-2221-E-167-017.
Conflicts of Interest: The authorsand
MOST-108-2221-E-006-214-MY2 declare no conflict of interest.
MOST-109-2221-E-167-017.
Conflicts of Interest: The authors declare no conflict of interest.
Conflicts of Interest: The authors declare no conflict of interest.

References
References
Mathematics 2020, 8, 1840 16 of 16

References
1. Gajamohan, M.; Merz, M.; Thommen, I.; Andrea, R. The cubli: A cube that can jump up and balance.
In Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura,
Algarve, Portugal, 7–12 October 2012; pp. 3722–3727.
2. Brian, D.O.; Moore, J.B. Optimal Control: Linear Quadratic Methods; Courier Corporation: North Chelmsford,
MA, USA, 2007.
3. Chen, Z.; Ruan, X.; Li, Y.; Bai, Y.; Zhu, X. Dynamic modeling of a self-balancing cubical robot balancing on its
edge. In Proceedings of the 2017 2nd International Conference on Robotics and Automation Engineering
(ICRAE), Shanghai, China, 29–31 December 2017; pp. 11–15.
4. Chen, Z.; Ruan, X.; Li, Y.; Bai, Y.; Zhu, X. Dynamic modeling of a cubical robot balancing on its corner.
In Proceedings of the MATEC Web of Conferences, Cheng Du, China, 16–17 December 2017; EDP Sciences: Paris,
France, 2017; p. 0067.
5. Chen, Z.; Ruan, X.; Li, Y.; Zhu, X. A sliding mode control method for the cubical robot. In Proceedings of the
2018 13th World Congress on Intelligent Control and Automation (WCICA), Changsha, China, 4–8 July 2018;
pp. 544–548.
6. Tian, L. Research on Fuzzy Control Algorithm of Cube System; Nanjing University of Science and Technology:
Nanjing, China, 2006.
7. Muehlebach, M.; Dandera, R. Nonlinear analysis and control of a reaction-wheel-based 3-D inverted
pendulum. IEEE Trans. Control Syst. Technol. 2017, 25, 235–246. [CrossRef]
8. Ang, K.; Chong, G.; Li, Y. PID control system analysis, design, and technology. IEEE Trans. Control Syst.
Technol. 2005, 13, 559–576.
9. Nazaruddin, Y.Y.; Andrini, A.D.; Anditio, B. PSO Based PID Controller for Quadrotor with Virtual Sensor.
IFAC Papers OnLine 2018, 51, 358–363. [CrossRef]
10. Fang, J.S.; Tsai, J.H.; Yan, J.J.; Tzou, C.H.; Guo, S.M. Design of Robust Trackers and Unknown Nonlinear
Perturbation Estimators for a Class of Nonlinear Systems: HTRDNA Algorithm for Tracker Optimization.
Mathematics 2019, 7, 1141. [CrossRef]
11. Li, Y.; Dempster, A.; Li, B.; Wang, J.; Rizos, C. A low-cost attitude heading reference system by combination
of GPS and magnetometers and MEMS inertial sensors for mobile applications. J. Glob. Position. 2006, 5,
88–95. [CrossRef]
12. Che-Yu, L. Realization of One-Dimensional Cube Robot Capable of Bounce and Self-Balancing Based on
Linear Quadratic Regulator Control. Master’s Thesis, Cheng Kung University, Tainan City, Taiwan, 2008.
13. Bar-itzhack, I.Y.; Oshman, Y. Attitude determination from vector observations: Quaternion estimation.
IEEE Trans. Control Syst. Technol. 1985, 1, 128–136. [CrossRef]
14. Sheppred, S.W. Quaternion from rotation matrix. J. Guid. Control 1978, 1, 223–224. [CrossRef]
15. Krasjet. Quaternion and the Rotation in Three Dimensional; Department of Mathematics, Faculty of Science:
Ankara, Turkey, 2018; pp. 24–35.
16. Zupan, E.; Saje, M.; Zupan, D. Quaternion-based dynamics of geometrically nonlinear spatial beams using
the Runge–Kutta method. Finite Elem. Anal. Des. 2012, 54, 48–60. [CrossRef]
17. Farhangian, F.; Landry, R. Accuracy Improvement of Attitude Determination Systems Using EKF-Based
Error Prediction Filter and PI Controller. Sensors 2020, 20, 4055. [CrossRef] [PubMed]
18. Zhi-Wei, W. Reduction of Vehicle Vibration and Energy Consumption on Rugged Roads by H∞ Control
Laws. Master’s Thesis, National Chiao Tung University, Hsinchu City, Taiwan, 2003.
19. Bruton, S.L.; Kutz, J.N. Data-Driven Science and Engineering: Machine Learning, Dynamical Systems, and Control;
Cambridge University Press: Cambridge, UK, 2019.

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional
affiliations.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access
article distributed under the terms and conditions of the Creative Commons Attribution
(CC BY) license (http://creativecommons.org/licenses/by/4.0/).