Modeling Dynamics and Exploring Control of
a Single-Wheeled Dynamically Stable Mobile
Robot with Arms
Eric M. Schearer
CMU-RI-TR-06-37
August 31, 2006
Robotics Institute
Carnegie Mellon University
Pittsburgh, Pennsylvania 15213
c Carnegie Mellon University
Submitted in partial fulfillment of the requirements for the degree of
Master of Science
Abstract
This paper focuses on simulations of a dynamically stable mobile robot (Ballbot)
with arms. The simulations are of Ballbot lifting its arms in various directions. A
PD arm controller works independently of an LQR-designed balancing/station keeping
controller. The PD controller drives the arms to follow desired trajectories. When
the arms are raised, Ballbot assumes a leaning equilibrium (the physical equilibrium)
as opposed to the standing equilibrium (body stands totally upright - a predefined
desired equilibrium) that the LQR drives toward. The conflict between these two
equilibria causes the robot to lose its balance when lifting heavy (10 kg) loads. A
unified arm and station keeping/balancing controller is also described. The unified
controller outperforms the independent controllers in some cases. Balancing only using
arms and driving body movement with arms are briefly explored.
I
Acknowledgments
Thanks to my advisor, Dr. Ralph Hollis for his vision in creating Ballbot and guiding
my work. Thanks to Dr. George Kantor for constant technical advice and brainstorming.
Thanks to Anish Mampetta for his companionship and ideas through all of our experimenting
with Ballbot. Anish was also the first to derive the Ballbot motion equations using the
current coordinates. Thanks to Dr. Matt Mason and Jonathan Hurst for reviewing my
work as part of my committee. Thanks to my family, friends, and classmates for every day
support. This research is supported in part by NSF grant IIS-030867. The United States
Air Force funds my tuition. Thanks to Col(ret.) Chuck Wolfe, my supervisor at the Air
Force Research Lab Munitions Directorate and to the Engineering Mechanics Department
at the U.S. Air Force Academy for making the Air Force support possible.
II
Contents
1 Introduction
1
2 Modeling and Simulation
2.1 Simulation in SimMechanics . . . . .
2.1.1 SimMechanics Block Diagram
2.1.2 Friction Model . . . . . . . .
2.1.3 Actuation Model . . . . . . .
2.2 2D Ballbot with Arms Model . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3
3
3
5
9
10
3 Independent Balancing and Arm Controllers
13
4 Unified Balancing and Arm Controller
15
5 Performance
5.1 Arm Controller Performance . . . . . . .
5.2 Balancing When Arms Move . . . . . . .
5.3 Compare Independent to Unified Control
5.4 New Ideas . . . . . . . . . . . . . . . . .
16
17
19
27
39
. . . . . .
. . . . . .
Approach
. . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6 Conclusions and Future Work
41
7 References
43
A SimMechanics Block Diagrams
A.1 Independent Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.2 Unified Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
44
44
49
III
1
Introduction
Ballbot is a mobile robot with human-like height, width, and weight. It actively balances and
moves on a single wheel using closed loop feedback, making it dynamically stable. Dynamic
stability affords it advantages in maneuverability over statically stable robots and makes it
a good candidate for operating in human environments. Balancing on a ball allows Ballbot
to be omni-directional, allowing it to move in any direction without turning.
The current version of Ballbot has no arms and is described in detail in [4] and [5].
Ballbot’s structure is three aluminum channels held together by circular decks that rest on
top of a single ball as seen in Figure 1. Figure 1a shows Ballbot standing with its three
legs (used when not dynamically balancing) deployed. On the circular decks are a battery,
a computer, a battery charger, and an inertial measurement unit (IMU) for measuring the
tilt angle of the body as seen in Figure 1b. Figure 1c. shows Ballbot balancing.
Figure 1:
The drive mechanism in Figure 2 includes four motors that drive the rollers which drive
the ball. Encoders on the motors measure the position of the rollers and thus the position
of the ball. A feedback controller allows Ballbot to move and balance.
Future versions of Ballbot will include arms with two degrees of freedom and will look
something like Figure 3. Arms will give Ballbot the capability to manipulate objects. Thibodeau et. al. [9] have shown that dynamically stable mobile robots with arms can apply
larger contact forces than similar statically stable mobile manipulators. Arms can also increase the stability and mobility of Ballbot.
The objectives of this work are:
1
servomotor
motor encoders
belt tensioner
ball transfers (3)
drive belt
drive roller
ball
Figure 2:
Figure 3:
• Create a 3D mechanical simulation of Ballbot with arms to study the dynamics of the
armed configuration. Section 2 discusses this model.
• Design a simple arm controller to operate simultaneously with and independently of a
body station keeping/balancing controller. This is seen in Section 3.
• Design a unified station keeping/balancing and arm controller that uses feedback from
the arms, ball, and body to both balance and move the arms. Section 4 describes this
controller.
• Section 5 evaluates and compares the performances of these two controllers.
• Section 6 suggests future work.
2
2
Modeling and Simulation
This section describes a 3D Ballbot simulation in SimMechanics and a planar Ballbot model
used for deriving controllers.
2.1
Simulation in SimMechanics
A 3D simulation of Ballbot with arms was created using SimMechanics. SimMechanics
r . SimMechanics uses the Newtonis a mechanical modeling package used with Simulink
Euler equations to describe the motion of rigid bodies and allows users to create models in
r block environment. Rigid bodies are modeled by a center of mass position
the Simulink
and body orientation relative to some coordinate system (a fixed coordinate system or a
coordinate system of a connected body), a mass, and an inertia tensor. The user can create
degrees of freedom by connecting bodies with joints and take away degrees of freedom with
constraints. Virtual sensors connected to bodies and joints allow the user to measure outputs
and feed them back to controllers which send signals to actuators that can apply forces,
torques, or prescribed motions to joints and bodies.
2.1.1
SimMechanics Block Diagram
Figure 4 shows a block diagram modeling the Ballbot with arms in SimMechanics. The
model consists of nine rigid bodies (the ground, the ball, the body, two arms, and four drive
rollers), the joints connecting them, the constraints limiting their motion, and two controllers
that sense movement and apply torques to the bodies. Orange blocks represent bodies, green
blocks represent joints, red blocks represent constraints, blue blocks represent controllers,
and the cyan block represents the fixed ground. The “Ball” is connected to “Ground1” by
a six degree of freedom joint. The 6-DoF joint is three perpendicular prismatic axes and
a quaternion representing rotation. The “Rolling Constraint” block constrains the relative
motion of the ball with respect to the ground. The rolling constraint equation is:
 g  
g 
vbx
rb ωby
g 
g 
 vby
=  rb ωbx
g
vbz
0
where vb and ωb are the linear and angular ball velocities, rb is the ball radius, the z axis is
vertical, and the x and y axes are parallel to the ground. These constraints prescribe that
the ball rolls on the ground without slipping and does not move in the vertical direction.
Superscripts indicate the coordinate system where g stands for ground. Subscripts refer to
bodies and vector components.
The “Body” which represents the Ballbot’s tower of motors, sensors, computers, etc. is
connected to the “Ball” by a spherical joint represented by a quaternion. Revolute joints,
defined by an axis and rotation angle, connect the four rollers (“y1 roller”, “x1 roller”, “y2
roller”, and “x2 roller”) to the “Body”. The red block, “Roller Constraints”, constrains the
relative motion of the rollers and ball. These constraint equations are:
3
Act Y1
Sense Y
Env
B
F
Act X1
Machine
Environment
Ground1
Six−DoF
CS2
B
F
Sense X
CS6
Act Y2
CS7
Spherical
CS4
Conn1
CS2
CS10
Sense Body
Act X2
4
Figure 4: SimMechanics Block Diagram
CS5
Conn2
CS9
Control
and Actuation
CG
CS1
Conn3
CS1
CS4
CS8
B
CS2
F
Rolling
Constraint
Ball
Conn1
CS1
Conn2
y1 roller
Revolute
CG
CS3
B
CS2
F
Conn3
CS1
Conn4
CS1
F
B
CS2
CS1
F
B
x1 roller
CS8
Revolute1
CG
CS2
hand1
arm1
Weld
CS6
B
F
Universal
Conn5
CS1
y2 roller
Revolute2
Conn6
CG
CS2
CS5
B
F
Conn7
CS1
x2 roller
Revolute3
CS1
F
B
CS2
CS1
F
B
CS9
Conn9
hand2
Weld1
arm2
Conn8
CS7
Universal1
Roller
Constraints
Body
Act Arm1
Sense Arm1
Act Arm2
Sense Arm2
Arm Controller
g
x1r
x2r
rx1r ωx1rz
= rb ωby
= rx2r ωx2rz
y1r
g
y2r
ry1r ωy1rz
= rb ωbx
= ry2r ωy2rz
where x1r, x2r, y1r, and y2r, refer to the rollers and b refers to the ball. The z axis of
each roller is its rotation axis. These constraints mean that the rollers roll without slipping
on the ball. The rollers can slip on the ball in the non-rolling directions. For example, in
Figure 2, if the ball is rolling on the roller labeled “drive roller”, it must be slipping on the
roller perpendicular to the “drive roller” (the other visible roller). The second roller still will
roll on the ball without slipping when it is rotating. Note that the ball angular velocity is
expressed in the ground frame when it should be expressed in the corresponding roller frame.
A major flaw of SimMechanics is that it only allows the user to express constraint equations
in either the body frame or the ground frame and not in the coordinate frame of another
body. Here we want to express the ball’s angular velocity in the roller frame. With these
limitations, if Ballbot yaws, the constraints are no longer correct (making the simulation
somewhat invalid) because the ground frame no longer lines up with the roller frames.
The arms, “arm1”, and “arm2” are connected to the “Body” by universal joints, which
allow two perpendicular degrees of rotational freedom. Based on the Ballbot’s height (about
1.5 m), Ballbot’s arms are 0.58 m long - proportional to a human. The arms are cylinders
with the density of aluminum. The hands “hand1” and “hand2” are spheres welded to the
ends of the arms.
The blue “Control and Actuation” block senses the orientation of the “Body” and positions of the rollers (and the arms in one controller) and applies torques to the rollers. The
“Arm Controller” block senses the position of the arms (and the body orientation and roller
positions in one controller) and applies torques to the arms. The appendix discusses these
two blocks in more detail. Section 3 describes the controllers in detail.
2.1.2
Friction Model
The friction model is found in the “Control and Actuation/Actuation and Friction” block of
Figure 4. No friction is modeled in the arms, and no friction is explicitly modeled between the
ball and ground and between the ball and body. The ball/ground, ball/body, and ball/roller
friction torques are lumped together and implemented by subtracting friction torques from
the torque applied to the rollers as follows:
τ = τr − τs − τv − τc
(1)
where τ , τr , τs , τv , and τc , are the total torque applied to the roller, the torque that the
controller prescribes, the torque on the roller due to static friction, the torque on the roller
due to viscous friction, and the torque on the roller due to coulomb friction, respectively.
The friction torques are further defined below.
τr if τr ≤ τsm
(2)
τs =
0 if τr > τsm
where τsm is the maximum static friction torque.
5
τv = γv ωrz
(3)
where ωrz is the angular velocity of the roller about its rotation axis.
τc = γc sign(ωrz )
(4)
To determine values for τsm , γv , and γc , a series of Ballbot tests were conducted. The tests
were conducted in the first floor hallway of Smith Hall on the black ribbed carpet. Ballbot
rolled on a hollow aluminum ball with urethane coating. No controller was used. For each
test, predetermined torque commands were given to the motors. Without a controller to
keep Ballbot standing, people held Ballbot vertical as it moved. The first test series used
a slow ramping (increment torque input by 0.0785 Nm or 5 DAC counts per second) of
the input torque. For much of each of these runs the torque was not enough to cause any
ball movement. Eventually enough torque was applied to move the ball. This first series
determined the static friction torque. The next set of tests used faster increases to the input
torque (1.57 Nm or 100 counts per second) to provide data to estimate the viscous and
coulomb friction torques. We ran tests in positive and negative x and y directions and in
the diagonal directions as well.
The maximum static friction torque for each test run is the value of τr when the ball
velocity becomes non-zero. By looking at the data (ball velocity vs. time plots), we can
determine the maximum static friction. For example, in Figure 5a the ball appears to move
shortly after 3 seconds. We know that the rollers applied 4.71 Nm to the ball at that time.
This torque is the maximum static friction for the particular test run. The ball velocity
threshold used was 0.1 rad/s. τv and τc were determined by assuming a quasi-steady state
for the region of the data where the velocity is non-zero, namely:
τr − τv − τc = 0
(5)
substituting equations (3 - 4) into equation (5) and rearranging terms:
γv ωrz + γc = τr
(6)
The process for finding γv and γc is outlined below:
• Fit a line to the ball velocity vs. time plot in the non-zero velocity region.
• Compute the value of the ball velocity at the end of the test run using the best fit line.
• Plug this final ball velocity (converted to roller velocity) into equation (6) for each test.
• Now we have a system of n (number of test runs for each direction) equations in two
unknowns, γv and γc . Solve these equations using singular value decomposition (SVD).
• An alternate solution method was using two ball velocity values from the best fit line
and solving a system of 2 equations and 2 unknowns for each test run. Then average the
γv and γc values for each test run over the set of similar (same ball velocity direction)
test runs.
6
Table 1 shows the values of each of the friction terms for a number of different directions
(e.g. +x-y is the direction 45 degrees from the positive x and 45 degrees from the negative y)
and for all directions. The fact that the values vary across the different directions suggests
that friction is not isotropic.
Direction τsm (AVG)
+x
4.91
-x
5.06
+y
4.3
-y
5.38
+x+y
4.35
+x-y
7.04
-x+y
4.35
-x-y
5.55
All
5.01
γv (AVG)
2.05
2.18
1.56
2.01
0.96
1.84
0.61
2.74
2.08
γc (AVG)
5.45
5.60
5.30
6.41
5.82
7.06
6.08
6.02
5.61
γv (SVD)
2.11
0.91
0.61
1.12
0.96
1.75
0.58
1.48
0.57
γc (SVD)
5.42
6.38
6.72
8.58
5.82
7.33
6.46
6.25
8.88
Table 1: Model Coefficients
Note that the data in Table 1 are for ball torques which were used for a previous simulator.
The current simulator uses roller torques so the appropriate conversion must be made. Also
note that the test data was taken when Ballbot had only two driven rollers (now all four are
driven). To account for more driven rollers, the current simulation uses the Table 1 values
divided by 2. This may be a poor assumption, and the tests should be run again to obtain
more accurate friction terms.
To check the goodness of the friction terms in Table 1, a 1D simulation of the ball motion
ignoring the effects of the body was created. The simulation solves the model equation
(7) using the new friction values in Table 1. The moment of inertia of the ball is Ib =
0.0463 kg m2 , and the angular acceleration of the ball is θ̈b . Plots of the simulation output
with the corresponding plots of hallway test runs are displayed in Figure 5. In the legend
of Figure 5, “drive1” and “drive2” refer to the x and y ball positions taken from the motor
encoders, and “idle1” and “idle2” refer to the x and y ball positions taken from the passive
roller encoders. Note that the motor encoders and passive roller encoders have opposite
polarity, but are both meant to measure ball position.
Ib θ¨b = τr − τs − τv − τc
(7)
Figure 6a shows ball velocity vs. time for the simulation using a model with no friction
and using one with only static friction. Figure 6b shows ball velocity vs. time for the
simulation using a model with all of the friction terms and one using static and viscous
friction. Notice the large disparity of velocity magnitudes between these two figures. This
disparity indicates that viscous friction is a major factor. Also notice in Figure 6b that
coulomb friction has a significant effect on the ball velocity.
The quasi-static assumption means that by ramping the torque slowly, the dynamic
effects are negligible. Judging by the accelerations seen in Figure 5a-b (1-2 rad/s) and Ib ,
the left side of equation (7) is small (< 0.1 Nm) compared to the right side (> 5 Nm) making
the quasi-static assumption reasonable.
7
8
15
model
model
drive1
6
drive1
drive2
drive2
10
idle1
idle1
idle2
4
idle2
ball velocity (rad/s)
ball velocity (rad/s)
5
2
0
0
−5
−2
−10
−4
−6
0
1
2
3
4
5
time (s)
6
7
8
9
−15
10
(a) Positive x Direction, Ramp = 100
0
1
2
3
4
5
time (s)
6
7
8
9
10
(b) Negative x Direction, Ramp = 100
20
5
model
drive1
15
drive2
0
idle1
idle2
ball velocity (rad/s)
ball velocity (rad/s)
10
5
0
−5
−10
model
−5
drive1
drive2
−15
idle1
−10
idle2
−15
0
2
4
6
time (s)
8
10
−20
12
(c) Positive y Direction, Ramp = 100
0
1
2
3
4
5
time (s)
6
7
8
9
10
(d) Negative y Direction, Ramp = 100
Figure 5: Friction Test and Simulation Results
3500
8
all terms
all terms
static and viscous
3000
no coulomb
7
static
no friction
6
ball velocity (rad/s)
ball velocity (rad/s)
2500
2000
1500
5
4
3
1000
2
500
0
1
0
1
2
3
4
5
time (s)
6
7
8
9
0
10
(a) Output of Limited Friction Models
0
1
2
3
4
5
time (s)
6
7
8
9
10
(b) Output of Better Friction Models
Figure 6: Comparison of Friction Models
8
2.1.3
Actuation Model
The actuation model is found in the “Control and Actuation/Actuation and Friction” block
of Figure 4. Figure 7 is a block diagram of the Ballbot actuation system. An input is sent
to the digital to analog converter (DAC) which outputs a voltage to the amplifier. The
amplifier sends a current to the motor which outputs a torque to the roller. The roller then
transmits a torque to the ball. In each block the input is multiplied by the number in the
block to produce the output.
Figure 7: Actuation System Block Diagram
Below are the DAC output voltage, VDAC , amplifier output current, Iout , and motor
torque, τr , expressed in terms of the DAC input, CDAC . The DAC input is an integer in the
range -2048 to 2047 and has units counts.
10 ∗ CDAC
10 ∗ CDAC
=
≈ 0.00489 ∗ CDAC V
(8)
Cmax
2047
where the maximum magnitude DAC input, Cmax , is 2047. We have assumed that the
amplifier operates in torque mode. According to page 23 of the amplifier spec sheet, the
amplifier outputs a current, Iout , according to:
VDAC =
10 ∗ CDAC ∗ 20
VDAC Ipeak
A=
A ≈ 0.00977 ∗ CDAC A
(9)
10
2047 ∗ 10
where Ipeak =20A is the peak current limit. If the magnitude of Iout is greater than Ipeak ,
then Iout =Ipeak sign(Iout ). In the real amplifier (Copley Controls Model 412), if the continuous
current limit is exceeded, a capacitor with a 1 second time constant at peak current begins to
charge. When it is fully charged, the output current decays exponentially to the continuous
current. The simulation models this as follows: If the magnitude of Iout is greater than
Icont =10 A, the continuous current limit, an integral begins accumulating. If the integral
is less than 14.427, then Iout =Iout , otherwise Iout =Icont sign(Iout ). The 14.427 was chosen so
that the integral acts like the real circuit. Note that the peak and continuous current limits
can be changed in the amplifier.
Iout =
9
The torque constant, Kτ , for the motor is 0.133 Nm/A. To convert DAC input to motor
torque,
Nm
≈ 0.00130 ∗ CDAC Nm
A
which assuming no losses, is the torque applied to the roller τr .
τr = Iout Kτ ≈ 0.00977 ∗ CDAC A ∗ 0.133
2.2
(10)
2D Ballbot with Arms Model
In order to design controllers we derive equations of motion for Ballbot. The dynamics of
Ballbot with arms are derived using the Lagrangian formulation. Figure 8 shows a planar
model of Ballbot. The planar equations of motion (and the corresponding planar controllers)
are much simpler both to derive, understand, and implement than the 3D equations. Two
perpendicular planar models can represent a 3D Ballbot accurately if Ballbot operates near
the upright standing position where coupling effects between the two planes is small. The
planar model is likely not as accurate in the case of arms that will operate out of the plane.
Note that:
• φ is measured with respect to the fixed world vertical, just as the IMU on the physical
robot measures pitch and roll.
• θ is measured with respect to moving φ, just as the encoders on the physical robot
measure the position of the ball.
• ψ is measured with respect to the moving φ, like the arm encoders would measure the
arm angles on the physical robot.
a
cm
y
l
a
l
B
Figure 8: Planar Ballbot Model
The position of the ball, pb , the position of the body, pB , and the position of the arm, pa are:
10


rb (θ + φ)

pb = 
0
0


rb (θ + φ) + lB sin φ

0
pB = 
lB cos φ


rb (θ + φ) + la sin φ − acm (cos φ sin ψ + sin φ cos ψ)

0
pa = 
la cos φ + acm (sin φ sin ψ + cos φ cos ψ)
The velocity of the ball, vb , the velocity of the body, vB , and the velocity of the arm, va are:

 rb θ̇ + φ̇


vb = 

0
0


rb θ̇ + φ̇ + lB φ̇ cos φ


vB = 

0
−lB φ̇ sin φ
rb θ̇ + φ̇ + la φ̇ cos φ + acm φ̇ sin φ sin ψ − ψ̇ cos φ cos ψ − φ̇ cos φ cos ψ + ψ̇ sin φ sin ψ

0
va = 

−la φ̇ sin φ + acm φ̇ cos φ sin ψ + ψ̇ sin φ cos ψ + φ̇ sin φ cos ψ + ψ̇ cos φ sin ψ

The kinetic energy of the ball, Tb , the kinetic energy of the body, TB , and the kinetic energy
of the arm, Ta , are:
2
1 1
mb vb T vb + Ib θ̇ + φ̇
2
2
1
1
mB vB T vB + IB φ̇2
=
2
2
2
1
1
=
ma va T va + Ia ψ̇ + φ̇
2
2
Tb =
TB
Ta
The potential energy of the ball, Vb , the potential energy of the body, VB , and the potential
energy of the arm, Va , are:
Vb = 0
VB = −mB glB cos φ
Va = −mB g (la cos φ + acm (sin φ sin ψ − cos φ cos ψ))
The Lagrangian, L, is the total kinetic energy minus the total potential energy.
11




L=T −V
The Lagrangian formulation dictates:
d ∂L ∂L
−
= τ1
dt ∂ θ̇
∂θ
d ∂L ∂L
= 0
−
dt ∂ φ̇
∂φ
d ∂L ∂L
= τ3
−
dt ∂ ψ̇ ∂ψ
(11)
(12)
(13)
where τ1 is the torque applied to the ball by the rollers (including friction), and τ3 is the arm
r symbolic package and rearranging
torque. Evaluating equations (11-13) using the Matlab
yields the following equation of motion.


 2 
θ̈
φ̇



M φ̈ + C ψ̇ 2  + G = T
(14)
ψ̈
φ̇ψ̇
where M is the 3x3 mass matrix with columns, M(:, 1), M(:, 2), and M(:, 3),


mrb2 + Ib
M(:, 1) =  mrb2 + rb (cos φ (mB lB + ma la ) − ma acm cos (φ + ψ)) + Ib 
−ma rb cma cos (φ + ψ)


mrb2 + rb (mB lB cos φ + ma la cos φ − ma acm cos (φ + ψ)) + Ib
2
+ ma (la2 + a2cm ) + I 
M(:, 2) =  mrb2 + 2rb (cos φ (mB lB + ma la ) − ma acm cos (φ + ψ)) + mB lB
−ma rb cma cos (φ + ψ) − ma la acm cos ψ + ma a2cm + Ia


ma rb acm cos (φ + ψ)
M(:, 3) =  −ma rb acm cos (φ + ψ) − ma la acm cos ψ + ma a2cm + Ia 
ma c2ma + Ia
m is the sum of the three masses,
m = mb + mb + ma
I is the sum of the three inertias,
I = Ib + IB + Ia
C is the 3x3 matrix of coriolis and centrifugal terms with columns C(:, 1), C(:, 2), and C(:, 3),


−rb (sin φ (mB lB + ma la ) − ma acm sin (φ + ψ))
C(:, 1) =  −rb (sin φ (mB lB + ma la ) − ma acm sin (φ + ψ)) 
−ma la cma sin ψ
12


ma rb acm sin (φ + ψ)
C(:, 2) =  ma rb acm sin (φ + ψ) + ma la acm sin ψ 
0


2ma rb acm sin (φ + ψ)
C(:, 3) =  2ma rb acm sin (φ + ψ) + 2ma la acm sin ψ 
0
G is the 3x1 gravity matrix,


0
G =  mB lB g sin φ + ma la g sin φ − ma acm g sin (φ + ψ) 
−ma acm g sin (φ + ψ)
and U is the 3x1 matrix of applied torques.


τr − τs − τc − γv θ̇

0
U =
τa − γva ψ̇
where γva is the coefficient of viscous friction for the arms.
To apply linear control strategies we must linearize this system using the form of equation
(15).
dẋ
dẋ
x+ u
(15)
dx
du
h
iT
where the state vector is x = θ φ ψ θ̇ φ̇ ψ̇ , and the input vector is u = [τr τa ]T . We
ẋ =
linearize the system of equation (14) about the equilibrium point θ = φ = ψ = θ̇ = φ̇ = ψ̇ = 0
to fit the system of equation (15). This corresponds to Ballbot standing upright up with its
arms at its sides.
3
Independent Balancing and Arm Controllers
The logical first step for implementing arms with Ballbot is to design an arm controller
that works independently of an existing balancing/station keeping controller that uses only
the rollers. To control the rollers we design a planar linear quadratic regulator (LQR)[1]
h
iT
excluding the arm states. In this case the state vector is x = θ φ θ̇ φ̇ , and the input
vector is u = τr . LQR finds a state feedback law, u = −Kx that minimizes the following
cost function near the linearization point.
Z
J=
xT Qx + uT Ru dt
where Q is a 4x4 matrix of weights for the states, and R is the weight for the applied torque.
The diagonal terms in Q correspond to the ball position, θ, the body angle, φ, the ball
13
velocity, θ̇, and the body angular velocity, φ̇.
follow are:

3000
 0
Q=
 0
0
The Q and R used to obtain the results that

0 0 0
30 0 0 

0 30 0 
0 0 30
and R = 1. These values were chosen only to achieve reasonable balancing (qualitatively),
and not to achieve any specific quantitative performance goal. They are intended to give
high importance to the ball position (first diagonal term) and low importance to the torque
r command dlqr finds K for the given linear system of equation (15). In
used. The Matlab
order to make Ballbot move we define the augmented control law,
u = −K (x − xp )
(16)
where xp is a vector of desired states that can change in time. In other words, if Ballbot is
doing something other than station keeping (non-zero desired state), we must specify xp as
a function of time. We choose the body tilt angle path, φp , and angular velocity path, φ̇p
to be zero at all times. We choose the ball position path, θp , to be a quintic polynomial in
time as in equation (17) and ball velocity path, θ̇p , as the derivative of the polynomial as in
equation (18).
θp = At5 + Bt4 + Ct3 + Dt2 + Et + F
(17)
θ̇p = 5At4 + 4Bt3 + 3Ct2 + 2Dt + E
(18)
and
If one defines initial and final (or even a knot point in between) positions, velocities, and
accelerations, one can solve for the coefficients, A, B, C, D, E, and F . The choice of a
quintic polynomial is to ensure smooth accelerations and thus smooth torques. Sciavicco
and Siciliano [8] suggest using a polynomial joint trajectory when no specific trajectory is
desired between points. Eventually we will determine trajectories which respect Ballbot’s
dynamic constraints (e.g. the body must lean forward and the ball must move backward
before Ballbot moves forward while the quintic polynomial moves forward immediately).
Equation (16) defines a planar controller. The real Ballbot and the SimMechanics simulation are both in 3D, so in practice we use two independent planar controllers. The first is
for x ball movement, θx and θ̇x , and x body movement, namely pitch and pitch rate, φy and
φ̇y . The second is for y ball movement, θy and θ̇y , and y body movement, φx and φ̇x . Two
independent planar controllers will be most effective when the body is near its equilibrium
point and coupling between x and y dynamics is small.
Each arm degree of freedom (2 DoF for each arm) is controlled independently with a PD
controller defined in equation (19)
τa = Pψ (ψ − ψp ) + Dψ ψ̇ − ψ̇p
(19)
14
where Pψ and Dψ are the gains, and ψp and ψ̇p are desired arm paths defined by quintic
polynomials similar to equations (17) and (18). This controller requires solving the inverse
kinematics problem for ψp and ψ̇p given a goal point in space. The inverse kinematics problem
is not solved here because this work deals only with simple movements (e.g. lifting the arms
in the x direction).
4
Unified Balancing and Arm Controller
There might be some cases where coordinating roller and arm movement is desirable. One
example is when lifting a heavy object causes the Ballbot to begin to lose balance. With the
independent controllers of the previous sections, the arms might continue to lift and cause
Ballbot to fall. With a unified controller, the arms might lower the heavy object to regain
balance.
h
iT
Again we turn to LQR using the full state vector x = θ φ ψ θ̇ φ̇ ψ̇ , and the input
vector u = [τr τa ]T and linearizing equation (14) about the zero state. This time Q is a 6x6
matrix of weights for the states and R is a 2x2 matrix of the weights for the applied torques.
The Q and R used to obtain the results that follow are:


3000 0
0
0 0 0
 0
30
0
0 0 0 


 0
0 3000 0 0 0 


Q=

0
0
0
30
0
0


 0
0
0
0 30 0 
0
0
0
0 0 30
and
R=
1 0
0 1
This means that the relative importance of the ball position, θ, and the arm angle, ψ, is high
and again the importance of the torque is low.
When merely balancing, an LQR controller optimized to operate around the standing
position with arms down is effective. In situations where Ballbot must move its arms (reach
for something on a shelf, or carry a drink, for instance) this controller is not sufficient. First,
LQR is a linear controller, and the sines and cosines in the gravity terms when the arms
move away from the sides of Ballbot are nonlinear. Second, the gains for moving the arms
are optimized for movement around the zero position where gravity has little effect. When
lifting the arms, gravity works against the motion, and the LQR derived controller does not
apply enough torque to move the arms very far. To make up for this, we introduce gravity
compensation [8] for the arms. The arm torque, τa is a combination of the torque, τLQR ,
prescribed by the LQR gains (a feedback torque) and gravity (a feed forward torque).
τa = τLQR + ma acm g sin ψ
15
(20)
Gravity compensation fixes the two problems mentioned above. It essentially takes away the
nonlinear gravity terms and allows control about non-zero arm positions.
This unified controller is also a planar controller, and in practice we use two planar
controllers. In the previous section we assumed that the x and y dynamics are uncoupled
when Ballbot is standing. Here the arms certainly have coupled dynamics in any out of plane
movement. This controller is most effective when operating in the plane. Planar arm control
is not a final solution. The unified controller of this section is intended to demonstrate
the possible benefits of the unified approach vs. the independent roller and arm controller
approach.
5
Performance
The purpose of these simulations was to:
• Tune an arm controller independent of the balancing controller. For a number of simple
movements (lifting the arms in various directions) can the controller follow the desired
path? How much torque is required?
• Observe the behavior of the balancing controller when the various arm motions are
executed. Determine what problems exist with the balancing controller as a result of
arm movement.
• Compare the behaviors of the independent controllers and the unified controller.
• Explore a few new control ideas.
One SimMechanics output is a visualization window, shown in Figure 9. The red parts
of the figure are what the visualization window actually shows. The black outline of the
Ballbot’s body and the black dots representing the connection points of the arms are added
for clarity. Red ellipsoids represent the bodies. The ellipsoids come from the inertia tensors
of the bodies. The actual top of the Ballbot body is about 1.5 m whereas its ellipsoid does
not extend that far. The arm ellipsoids extend further than the physical arms.
Figure 9: Example of SimMechanics Visualization Window
16
5.1
Arm Controller Performance
The basic design of the arms is an aluminum cylinder with a 1 kg hand. One test of the arm
controller is to lift both arms up along Ballbot’s sides (the y direction) as seen in Figure 10a.
This test was conducted using various hand masses to simulate what happens when Ballbot
lifts objects. Note that lifting both arms in opposite directions will not affect Ballbot’s
balance, allowing us to test the arm controller while Ballbot has perfect balance. The results
in Figure 10 are for 5 kg hands. Figure 10b shows that the peak torque required for this
action is 30.76 Nm. The peak torque required for 1 kg hands is 7.37 Nm. Figure 10c-d shows
that this controller follows the desired path very well.
Obviously the arms cannot supply an infinite amount of torque. Figure 11 shows results
from a simulation where a 15 Nm torque limit was put on the arm motors. Figure 11a-b
shows that when the motor saturates, the arms oscillate undamped (no friction is modeled)
about a position less than the desired position.
35
X
Y
Torque Arm 1 (Nm)
30
25
20
15
10
5
0
0
0.5
1
1.5
2
Time (s)
(a) Final Pose
(b) Torque vs. Time
2
1.5
1.5
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
0.5
Velocity (radians/s)
Position (radians)
1
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
1
0
−0.5
0.5
0
−0.5
−1
−1
−1.5
−2
0
0.5
1
1.5
2
Time (s)
−1.5
0
0.5
1
1.5
Time (s)
(c) Arm y Angle vs. Time
(d) Arm y Angular Velocity vs. Time
Figure 10: Both 5 kg Arms Raised in y Direction
17
2
2
1.5
1.5
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
Velocity (radians/s)
0.5
0
−0.5
0.5
0
−0.5
−1
−1
−1.5
−2
0
1
2
3
4
5
−1.5
0
1
2
Time (s)
3
4
Time (s)
(a) Arm y Angle vs. Time
(b) Arm y Angular Velocity vs. Time
16
14
Torque Arm 1 (Nm)
Position (radians)
1
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
1
12
X
Y
10
8
6
4
2
0
0
1
2
3
4
5
Time (s)
(c) Torque vs. Time
Figure 11: Both 5 kg Arms Raised in y Direction with Saturated Torque
18
5
5.2
Balancing When Arms Move
Figure 12a shows the final pose of Ballbot raising one arm in the y direction. Note in Figure
12b-c that the ball moves to the right by about 6 cm and from Figure 12d-e that the body
leans to the left by about 1.3 degrees. When Ballbot raises an arm, it settles into a leaning
equilibrium that is different from the desired standing upright equilibrium where the body
angle is zero. The arm movement causes an overall change in the center of mass which
changes the equilibrium position. Figure 12f-g shows that the arm controller follows the
planned path well.
Figure 13a-f shows results of the same arm lifting simulation except with heavier 5 kg
arms. This models what might happen when Ballbot picks up a load. In this case the station
keeping/balancing controller fails. Figure 13a-b shows increasing ball oscillation and Figure
13c-d shows increasing body oscillation. As Ballbot loses its balance, the arm controller is
not as effective as seen near the end of Figure 13e-f. The increased arm mass causes a larger
change is the leaning equilibrium. Eventually the ball and body are far enough away from
the desired standing upright equilibrium that the station keeping/balancing controller fights
back. This causes the ball and body to oscillate between the leaning and standing upright
equilibria and finally fall.
(a) Final Pose
Figure 12: One 1 kg Arm Raised in y Direction
19
0.07
0.06
0.2
Actual Path
Planned Path
Actual Path
Planned Path
0.15
Y Velocity (m/s)
Y Position (m)
0.05
0.04
0.03
0.02
0.1
0.05
0.01
0
0
−0.01
0
0.5
1
1.5
2
−0.05
0
2.5
0.5
1
Time (s)
(b) y Position vs. Time
Roll Rate (radians/s)
Roll (radians)
Actual Path
Planned Path
0.1
0.02
0.015
Actual Path
Planned Path
0.01
0.05
0
−0.05
0.005
0.5
1
1.5
2
−0.1
0
2.5
0.5
1
Time (s)
1.5
2
2.5
Time (s)
(d) Roll vs. Time
(e) Roll Rate vs. Time
1.6
1.6
1.4
1.4
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
1
0.8
0.6
0.4
1
0.8
0.6
0.4
0.2
0.2
0
0
0.5
1
1.5
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
1.2
Velocity (radians/s)
1.2
Position (radians)
2.5
0.15
0.025
−0.2
0
2
(c) y Velocity vs. Time
0.03
0
0
1.5
Time (s)
2
2.5
Time (s)
−0.2
0
0.5
1
1.5
2
2.5
Time (s)
(f) Arm y Angle vs. Time
(g) Arm y Angular Velocity vs. Time
Figure 12: One 1 kg Arm Raised in y Direction
20
3
0.25
0.2
Actual Path
Planned Path
2
Actual Path
Planned Path
Y Velocity (m/s)
Y Position (m)
0.15
0.1
0.05
0
−0.05
1
0
−1
−0.1
−2
−0.15
−0.2
0
0.5
1
1.5
2
−3
0
2.5
0.5
1
1.5
2
2.5
Time (s)
Time (s)
(a) y Position vs. Time
(b) y Velocity vs. Time
3
0.25
0.2
Actual Path
Planned Path
2
Actual Path
Planned Path
Roll Rate (radians/s)
Roll (radians)
0.15
0.1
0.05
0
−0.05
1
0
−1
−0.1
−2
−0.15
−0.2
0
0.5
1
1.5
2
−3
0
2.5
1.5
2
(c) Roll vs. Time
(d) Roll Rate vs. Time
2.5
6
5
1.6
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
1.2
1
0.8
0.6
0.4
3
2
1
0
−1
0.2
−2
0
−3
0.5
1
1.5
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
4
Velocity (radians/s)
1.4
Position (radians)
1
Time (s)
1.8
−0.2
0
0.5
Time (s)
2
2.5
−4
0
0.5
1
1.5
2
2.5
Time (s)
Time (s)
(e) Arm y Angle vs. Time
(f) Arm y Angular Velocity vs. Time
Figure 13: One 5 kg Arm Raised in y Direction
21
Raising the arms in opposite x directions causes Ballbot to rotate about its vertical axis
(a yaw). Figure 14a shows this pose in the xz plane. The xy plane view (from above)
of Figure 14 makes the yawing apparent, as Ballbot began the simulation facing in the x
direction. Keep in mind that no friction is modeled between the ground and ball or between
the body and ball. In the real case where there is friction, there might not be any yawing,
or at least there will be less yawing.
(a) Final Pose: xz View
(b) Final Pose: xy View
Figure 14: 5 kg Arms Raised in Opposite x-Directions
Figure 15a-b is the final pose of Ballbot raising one arm in the x direction and out in
the y direction. This causes both a yaw (Figure 15b) and the leaning equilibrium (Figure
15b-f and i-l) seen earlier. Note that the leaning is in both the x and y directions. The arm
controller performs well (Figure 15g-h and m-n).
(a) Final Pose: 3D View
(b) Final Pose: xy View
Figure 15: 1 kg Arm Raised in x and y Directions
22
0.02
0.1
Actual Path
Planned Path
0.01
0.05
X Velocity (m/s)
X Position (m)
0
−0.01
−0.02
−0.03
Actual Path
Planned Path
0
−0.05
−0.04
−0.1
−0.05
−0.06
0
0.5
1
1.5
−0.15
0
2
0.5
Time (s)
(c) x Position vs. Time
2
0.15
Actual Path
Planned Path
Actual Path
Planned Path
0.1
Pitch Rate (radians/s)
0.02
Pitch (radians)
1.5
(d) x Velocity vs. Time
0.03
0.025
1
Time (s)
0.015
0.01
0.005
0.05
0
0
−0.05
−0.005
−0.01
0
0.5
1
1.5
−0.1
0
2
0.5
(e) Pitch vs. Time
2
1.6
1.4
1.4
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
1
0.8
0.6
0.4
1
0.8
0.6
0.4
0.2
0.2
0
0
0.5
1
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
1.2
Velocity (radians/s)
1.2
Position (radians)
1.5
(f) Pitch Rate vs. Time
1.6
−0.2
0
1
Time (s)
Time (s)
1.5
2
Time (s)
−0.2
0
0.5
1
1.5
Time (s)
(g) x Arm Angle vs. Time
(h) x Arm Angular Velocity vs. Time
Figure 15: 1 kg Arm Raised in x and y Directions
23
2
0.4
0.06
0.05
Actual Path
Planned Path
0.2
0
Y Velocity (m/s)
Y Position (m)
0.04
0.03
0.02
−0.2
−0.4
0.01
−0.6
0
−0.8
−0.01
0
0.5
1
1.5
−1
0
2
Actual Path
Planned Path
0.5
1
1.5
2
Time (s)
Time (s)
(i) y Position vs. Time
(j) y Velocity vs. Time
0.6
0.03
0.025
Actual Path
Planned Path
0.4
Roll Rate (radians/s)
0.02
Roll (radians)
0.015
0.01
0.005
0
−0.005
−0.01
−0.015
−0.02
0
Actual Path
Planned Path
0.5
0.2
0
−0.2
−0.4
−0.6
1
1.5
−0.8
0
2
0.5
(k) Roll vs. Time
2
1.6
1.4
1.4
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
1
0.8
0.6
0.4
1
0.8
0.6
0.4
0.2
0.2
0
0
0.5
1
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
1.2
Velocity (radians/s)
1.2
Position (radians)
1.5
(l) Roll Rate vs. Time
1.6
−0.2
0
1
Time (s)
Time (s)
1.5
2
Time (s)
−0.2
0
0.5
1
1.5
2
Time (s)
(m) y Arm Angle vs. Time
(n) y Arm Angular Velocity vs. Time
Figure 15: 1 kg Arm Raised in x and y Directions
24
Figure 16a-j shows results of Ballbot raising one arm in the y direction (as in Figure
12a) but starting off balance by 5 degrees of roll and pitch (rotations about x and y). Note
the initial roll (Figure 16g) and pitch (Figure 16c). Ballbot recovers its balance nicely and
establishes its leaning equilibrium. It finishes with non-zero x and y position and pitch and
roll and zero x and y velocity and pitch rate and roll rate. The arm controller is initially a
bit noisy (Figure 16j), but eventually follows the planned path closely.
1.4
0.16
Actual Path
Planned Path
0.12
1
0.1
0.8
0.08
0.06
0.04
0.6
0.4
0.2
0.02
0
0
−0.2
−0.02
0
0.5
1
1.5
Actual Path
Planned Path
1.2
X Velocity (m/s)
X Position (m)
0.14
−0.4
0
2
0.5
(a) x Position vs. Time
2
0.4
0.2
Actual Path
Planned Path
0.08
0
Pitch Rate (radians/s)
0.06
Pitch (radians)
1.5
(b) x Velocity vs. Time
0.1
0.04
0.02
0
−0.02
−0.2
−0.4
−0.6
−1
−1.2
−0.06
−1.4
0.5
1
1.5
2
Actual Path
Planned Path
−0.8
−0.04
−0.08
0
1
Time (s)
Time (s)
−1.6
0
0.5
1
1.5
Time (s)
Time (s)
(c) Pitch vs. Time
(d) Pitch Rate vs. Time
Figure 16: 1 kg Arms Raised in y Direction From Off Balance
25
2
0.5
0.02
0
0
Y Velocity (m/s)
Y Position (m)
−0.02
−0.04
−0.06
−0.08
Actual Path
Planned Path
−0.1
−0.5
Actual Path
Planned Path
−1
−0.12
−0.14
−0.16
0
0.5
1
1.5
−1.5
0
2
0.5
(e) y Position vs. Time
1.5
2
(f) y Velocity vs. Time
0.5
0.1
Actual Path
Planned Path
0.08
0
Roll Rate (radians/s)
0.06
Roll (radians)
1
Time (s)
Time (s)
0.04
0.02
0
−0.02
−0.04
Actual Path
Planned Path
−0.5
−1
−1.5
−0.06
−0.08
0
0.5
1
1.5
−2
0
2
0.5
(g) Roll vs. Time
2
1.5
1.5
Arm 1 Y Vel Actual
Arm 1 Y Vel Planned
Arm 2 Y Vel Actual
Arm 2 Y Vel Planned
1
Velocity (radians/s)
1
Position (radians)
1.5
(h) Roll Rate vs. Time
2
0.5
0
−0.5
Arm 1 Y Pos Actual
Arm 1 Y Pos Planned
Arm 2 Y Pos Actual
Arm 2 Y Pos Planned
−1
−1.5
−2
0
1
Time (s)
Time (s)
0.5
1
0.5
0
−0.5
−1
1.5
2
Time (s)
−1.5
0
0.5
1
1.5
Time (s)
(i) y Arm Angle vs. Time
(j) y Arm Angular Velocity vs. Time
Figure 16: 1 kg Arms Raised in y Direction From Off Balance
26
2
5.3
Compare Independent to Unified Control Approach
Figure 17a-b shows the final pose of Ballbot lifting its arms in the x direction using the
independent controllers and the unified controller. The controllers show very similar performance (Figure 17c-j) in that Ballbot assumes the leaning equilibrium in both cases. With
both control strategies the ball moves about 10 cm to the right and leans about 2.3 degrees
to the left. We can see from Figure 17k-n that the independent arm controller follows the
planned path slightly better than the unified controller does, while the unified controller
requires slightly less torque (Figure 17o-p).
(a) Final Pose: Independent Controllers
(b) Final Pose: Unified Controller
0.1
0.08
0.12
Actual Path
Planned Path
Actual Path
Planned Path
0.1
X Position (m)
X Position (m)
0.08
0.06
0.04
0.02
0.06
0.04
0.02
0
−0.02
0
0
0.5
1
1.5
2
Time (s)
−0.02
0
0.5
1
1.5
2
Time (s)
(c) x Position vs. Time: Independent
(d) x Position vs. Time: Unified
Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control
27
0.5
0.2
0.4
0.15
0.3
X Velocity (m/s)
X Velocity (m/s)
0.25
0.1
0.05
0.2
0.1
0
0
Actual Path
Planned Path
−0.05
−0.1
0
Actual Path
Planned Path
0.5
1
1.5
−0.1
−0.2
0
2
0.5
0.01
0.01
0
0
−0.01
−0.01
−0.02
Actual Path
Planned Path
−0.03
−0.04
−0.05
−0.05
0.5
1
2
−0.02
−0.04
−0.06
0
1.5
(f) x Velocity vs. Time: Unified
Pitch (radians)
Pitch (radians)
(e) x Velocity vs. Time: Independent
−0.03
1
Time (s)
Time (s)
1.5
−0.06
0
2
Actual Path
Planned Path
0.5
Time (s)
1
1.5
2
Time (s)
(g) Pitch vs. Time: Independent
(h) Pitch vs. Time: Unified
0.2
0.05
0.1
Pitch Rate (radians/s)
Pitch Rate (radians/s)
0
−0.05
−0.1
Actual Path
Planned Path
0
−0.1
−0.2
−0.3
−0.15
−0.4
−0.2
0
0.5
1
1.5
2
−0.5
0
Actual Path
Planned Path
0.5
1
1.5
2
Time (s)
Time (s)
(i) Pitch Rate vs. Time: Independent
(j) Pitch Rate vs. Time: Unified
Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control
28
0
0.2
−0.2
−0.6
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
−0.2
Position (radians)
−0.4
Position (radians)
0
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
−0.8
−1
−0.4
−0.6
−0.8
−1
−1.2
−1.2
−1.4
−1.4
−1.6
0
−1.6
0.5
1
1.5
−1.8
0
2
0.5
Time (s)
0.2
0.4
0
0.2
−0.2
0
−0.4
−0.6
−0.8
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
−1.2
−1.4
−1.6
0
0.5
1
−0.4
−0.6
−0.8
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
−1
−1.2
−1.4
1.5
−1.6
0
2
0.5
1
0
0
−1
−1
−2
Torque Arm 1 (Nm)
Torque Arm 1 (Nm)
1.5
2
(n) x Arm Ang. Vel. vs. Time: Unified
1
X
Y
−3
−4
−5
−2
−4
−5
−6
−7
−7
1.5
2
Time (s)
X
Y
−3
−6
1
1
Time (s)
(m) x Arm Ang. Vel. vs. Time: Independent
0.5
2
−0.2
Time (s)
−8
0
1.5
(l) x Arm Angle vs. Time: Unified
Velocity (radians/s)
Velocity (radians/s)
(k) x Arm Angle vs Time: Independent
−1
1
Time (s)
−8
0
0.5
1
1.5
2
Time (s)
(o) Arm Torque vs. Time: Independent
(p) Arm Torque vs. Time: Unified
Figure 17: Both 1kg Arms Raised in x-Direction: Compare Independent and Unified Control
29
When executing the same x lifting motion with heavier 5 kg arms, the results are different.
This time the independent controller (Figure 18a) fails almost immediately, and the unified
controller (Figure 18b) balances for longer. The independent arm controller follows the
planned path closely and uses more arm torque (Figure 18k, m, and o) while the unified
controller deviates from the planned arm path and uses less arm torque (Figure 18l, n, and
p) in order to stay balancing for longer. The unified controller has “decided” to abandon the
goal of moving the arms to a desired position in favor of moving the arms to help balance.
The next comparison is recovering from a 16 degree initial body pitch (Figure 19). It is
obvious from Figure 19c-l that the unified controller is able to recover and the independent
controllers are not. The independent arm controller attempts to keep the arms at Ballbot’s
sides and uses small amounts of torque (Figure 19m, o, and q), while the unified controller
swings the arms to help balance using more torque (Figure 19n, p, and r).
(a) Final Pose: Independent Controllers
(b) Final Pose: Unified Controller
0.5
0.7
Actual Path
Planned Path
0.4
0.5
X Position (m)
X Position (m)
0.3
0.2
0.1
0
0.4
0.3
0.2
0.1
−0.1
−0.2
0
Actual Path
Planned Path
0.6
0
0.5
1
1.5
2
2.5
3
Time (s)
−0.1
0
0.5
1
1.5
2
2.5
3
3.5
Time (s)
(c) x Position vs. Time: Independent
(d) x Position vs. Time: Unified
Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control
30
2
1.5
1
1
X Velocity (m/s)
X Velocity (m/s)
0.5
0
−0.5
−1
Actual Path
Planned Path
0
−1
Actual Path
Planned Path
−2
−1.5
−3
−2
−2.5
0
0.5
1
1.5
2
2.5
−4
0
3
0.5
1
1.5
(e) x Velocity vs. Time: Independent
3
3.5
0.15
Actual Path
Planned Path
Actual Path
Planned Path
0.1
0.05
0.8
0
Pitch (radians)
Pitch (radians)
2.5
(f) x Velocity vs. Time: Unified
1.2
1
2
Time (s)
Time (s)
0.6
0.4
−0.05
−0.1
−0.15
−0.2
0.2
−0.25
0
−0.3
−0.2
0
0.5
1
1.5
2
2.5
−0.35
0
3
0.5
1
Time (s)
2.5
3
3.5
(h) Pitch vs. Time: Unified
3
3
Actual Path
Planned Path
Actual Path
Planned Path
2.5
2
2
Pitch Rate (radians/s)
Pitch Rate (radians/s)
2
Time (s)
(g) Pitch vs. Time: Independent
2.5
1.5
1.5
1
0.5
1.5
1
0.5
0
−0.5
−1
0
−1.5
−0.5
0
0.5
1
1.5
2
2.5
3
Time (s)
−2
0
0.5
1
1.5
2
2.5
3
3.5
Time (s)
(i) Pitch Rate vs. Time: Independent
(j) Pitch Rate vs. Time: Unified
Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control
31
0
0.5
−0.2
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
−0.6
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
0
Position (radians)
Position (radians)
−0.4
−0.8
−1
−1.2
−0.5
−1
−1.5
−1.4
−2
−1.6
−1.8
0
0.5
1
1.5
2
2.5
−2.5
0
3
0.5
1
Time (s)
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
3.5
1
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
2
Velocity (radians/s)
Velocity (radians/s)
3
3
2
0
−1
−2
−3
1
0
−1
−2
−3
0.5
1
1.5
2
2.5
−4
0
3
0.5
1
Time (s)
1.5
2
2.5
3
3.5
Time (s)
(m) x Arm Ang. Vel. vs. Time: Independent
(n) x Arm Ang. Vel. vs. Time: Unified
50
5
X
Y
40
30
0
−5
20
Torque Arm 1 (Nm)
Torque Arm 1 (Nm)
2.5
(l) x Arm Angle vs. Time: Unified
3
10
0
−10
−20
−10
−20
−25
−30
−40
−35
0.5
1
1.5
2
2.5
3
Time (s)
X
Y
−15
−30
−50
0
2
Time (s)
(k) x Arm Angle vs. Time: Independent
−4
0
1.5
−40
0
0.5
1
1.5
2
2.5
3
3.5
Time (s)
(o) Arm Torque vs. Time: Independent
(p) Arm Torque vs. Time: Unified
Figure 18: Both 5kg Arms Raised in x Direction: Compare Independent and Unified Control
32
(a) Initial Pose: Independent Controllers
(b) Initial Pose: Unified Controller
(d) Final Pose: Unified Controller
1
0.8
0.8
0.7
0.6
0.6
0.4
0.5
X Position (m)
X Position (m)
(c) Final Pose: Independent Controllers
0.2
0
−0.2
−0.4
0.4
0.3
0.2
0.1
−0.6
Actual Path
Planned Path
−0.8
−1
0
Actual Path
Planned Path
0.2
0.4
0.6
0
0.8
1
1.2
1.4
1.6
−0.1
0
1
2
3
4
Time (s)
Time (s)
(e) x Position vs. Time: Independent
(f) x Position vs. Time: Unified
Figure 19: Extreme Balancing: Compare Independent and Unified Control
33
5
3
3
Actual Path
Planned Path
2
Actual Path
Planned Path
2
X Velocity (m/s)
X Velocity (m/s)
1
1
0
−1
0
−1
−2
−2
−3
−3
0
0.2
0.4
0.6
0.8
1
1.2
1.4
−4
0
1.6
1
2
(g) x Velocity vs. Time: Independent
0.3
0.2
0.2
0
5
Actual Path
Planned Path
0.1
Pitch (radians)
Pitch (radians)
4
(h) x Velocity vs. Time: Unified
0.4
−0.2
−0.4
−0.6
0
−0.1
−0.2
−0.8
Actual Path
Planned Path
−1
−1.2
0
3
Time (s)
Time (s)
0.2
0.4
0.6
−0.3
0.8
1
1.2
1.4
−0.4
0
1.6
1
2
3
4
5
Time (s)
Time (s)
(i) Pitch vs. Time: Independent
(j) Pitch vs. Time: Unified
4
0.5
0
Pitch Rate (radians/s)
−0.5
Pitch Rate (radians/s)
Actual Path
Planned Path
3
−1
−1.5
−2
2
1
0
−2.5
−3.5
0
−1
Actual Path
Planned Path
−3
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
−2
0
1
2
3
4
Time (s)
Time (s)
(k) Pitch Rate vs. Time: Independent
(l) Pitch Rate vs. Time: Unified
Figure 19: Extreme Balancing: Compare Independent and Unified Control
34
5
−3
x 10
1.2
Position (radians)
1
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
1.5
1
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
0.8
Position (radians)
2
0.5
0
0.6
0.4
0.2
0
−0.5
−0.2
−1
0
0.2
0.4
0.6
0.8
1
1.2
1.4
−0.4
0
1.6
1
2
3
4
5
Time (s)
Time (s)
(m) x Arm Angle vs. Time: Independent
(n) x Arm Angle vs. Time: Unified
3
0.06
2
Velocity (radians/s)
0.02
Velocity (radians/s)
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
0.04
0
−0.02
−0.04
1
0
−1
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
−2
−3
−0.06
0
0.2
0.4
0.6
0.8
1
1.2
1.4
−4
0
1.6
1
2
3
4
5
Time (s)
Time (s)
(o) x Arm Ang. Vel. vs. Time: Independent
(p) x Arm Ang. Vel. vs. Time: Unified
6
1
X
Y
5
0.5
Torque Arm 1 (Nm)
Torque Arm 1 (Nm)
4
0
−0.5
−1
3
2
1
0
−1.5
−2
0
X
Y
0.2
0.4
0.6
0.8
1
1.2
1.4
−1
1.6
−2
0
1
2
3
4
Time (s)
Time (s)
(q) Arm Torque vs. Time: Independent
(r) Arm Torque vs. Time: Unified
Figure 19: Extreme Balancing: Compare Independent and Unified Control
35
5
1.2
1.2
1
1
0.8
0.8
X Position (m)
X Position (m)
The next simulation is Ballbot waiting one second and then attempting to move 1 m in the
x direction in the next 3 seconds (Figure 20a-n). The unified controller, which uses its arms
to aid movement, gets to the goal slightly faster as seen in (Figure 20a-b). The independent
arm controller, which is trying to keep its arms at Ballbot’s sides, has higher frequency (albeit
very small) arm oscillations (Figure 20i, k, and m) while the unified controller operates more
smoothly (Figure 20j, l, and n).
As stated in section 4, the unified controller is two planar controllers, and its weakness
is out of plane arm motion. Figure 21a-b shows that with an initial out of plane body angle
(5 degrees of roll and pitch), the unified controller can still balance Ballbot. This balancing
scenario does not require much arm motion. Excessive out of plane arm motion might cause
Ballbot to fall.
0.6
0.4
0.2
0.4
0.2
Actual Path
Planned Path
0
−0.2
0
0.6
1
2
3
4
Actual Path
Planned Path
0
−0.2
0
5
1
2
Time (s)
(a) x Position vs. Time: Independent
4
5
(b) x Position vs. Time: Unified
1
1
0.8
0.8
0.6
0.6
X Velocity (m/s)
X Velocity (m/s)
3
Time (s)
0.4
0.2
0
0.4
0.2
0
−0.2
Actual Path
Planned Path
−0.4
0
1
Actual Path
Planned Path
−0.2
2
3
4
5
Time (s)
−0.4
0
1
2
3
4
Time (s)
(c) x Velocity vs. Time: Independent
(d) x Velocity vs. Time: Unified
Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control
36
5
0.1
0.1
Actual Path
Planned Path
0.08
0.06
Actual Path
Planned Path
0.08
0.06
Pitch (radians)
Pitch (radians)
0.04
0.02
0
−0.02
0.04
0.02
0
−0.04
−0.02
−0.06
−0.04
−0.08
−0.1
0
1
2
3
4
−0.06
0
5
1
2
Time (s)
(e) Pitch vs. Time: Independent
4
5
(f) Pitch vs. Time: Unified
0.5
0.3
Actual Path
Planned Path
0.4
0.2
0.3
Pitch Rate (radians/s)
Pitch Rate (radians/s)
3
Time (s)
0.2
0.1
0
−0.1
0.1
0
−0.1
−0.2
Actual Path
Planned Path
−0.3
−0.2
0
1
2
3
4
−0.4
0
5
1
2
Time (s)
3
4
5
Time (s)
(g) Pitch Rate vs. Time: Independent
(h) Pitch Rate vs. Time: Unified
−4
x 10
0.15
1.5
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
Position (radians)
1
0.5
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
0.1
Position (radians)
2
0
−0.5
0.05
0
−1
−0.05
−1.5
−2
0
1
2
3
4
5
Time (s)
−0.1
0
1
2
3
4
Time (s)
(i) x Arm Angle vs. Time: Independent
(j) x Arm Angle vs. Time: Unified
Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control
37
5
−3
x 10
0.3
3
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
Velocity (radians/s)
2
1
0.2
Velocity (radians/s)
4
0
−1
−2
0.1
0
−0.1
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
−0.2
−0.3
−3
−4
0
1
2
3
4
−0.4
0
5
1
2
Time (s)
3
4
5
Time (s)
(k) x Arm Ang. Vel. vs. Time: Independent
(l) x Arm Ang. Vel. vs. Time: Unified
5
0.2
4
0.15
3
Torque Arm 1 (Nm)
Torque Arm 1 (Nm)
0.1
0.05
0
−0.05
2
1
0
−1
−0.1
X
Y
−0.15
−0.2
0
X
Y
1
2
3
4
−2
5
−3
0
1
2
3
4
5
Time (s)
Time (s)
(m) Arm Torque vs. Time: Independent
(n) Arm Torque vs. Time: Unified
Figure 20: Move 1m in 3 seconds: Compare Independent and Unified Control
(a) Initial Pose
(b) Final Pose
Figure 21: Unified Controller Balancing Out of Plane
38
5.4
New Ideas
Two other means of controlling Ballbot were briefly explored. The first is using only the
arms to balance starting from a 1 degree body pitch. Figure 22a-g shows that Ballbot eventually falls down with this specific arm-only controller. Note in Figure 22c-d that Ballbot
does reverse its pitch before going unstable. This requires a large initial torque (Figure 22g)
of over 100 Nm. This large torque makes balancing using only arms unpractical.
The second new idea is inspired by the Segway personal mobility device (www.segway.com)
and the Robotic Mobility Platform [7]. These devices balance in 2D and move by leaning
forward. The lean causes the controller to accelerate to regain the robot’s balance. It is
possible that Ballbot can move by inducing the same body lean by moving its arms. Here
we control Ballbot’s forward motion by defining a ball path and applying an arm torque
proportional to the planned ball velocity while damping the arm motion so that the arms do
not swing out of control. Figure 23a-d show that this is a feasible approach, but at least in
this initial trial, this strategy does not follow the planned path as closely as the controllers
described earlier (see Figure 20).
8
0.6
Actual Path
Planned Path
0.5
Actual Path
Planned Path
6
4
X Velocity (m/s)
X Position (m)
0.4
0.3
0.2
0.1
0
−2
−4
0
−0.1
0
2
−6
0.2
0.4
0.6
0.8
1
1.2
−8
0
1.4
0.2
0.4
0.6
0.8
1
Time (s)
Time (s)
(a) x Position vs. Time
(b) x Velocity vs. Time
Figure 22: Balancing With Only Arms
39
1.2
1.4
0.1
15
0
10
Pitch Rate (radians/s)
Pitch (radians)
−0.1
Actual Path
Planned Path
−0.2
−0.3
−0.4
5
0
−5
−10
−0.5
−0.6
0
Actual Path
Planned Path
0.2
0.4
0.6
0.8
1
1.2
−15
0
1.4
0.2
0.4
0.6
(c) Pitch vs. Time
1.2
1.4
1.2
1.4
20
3
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
1
0
−20
Velocity (radians/s)
2
0
−1
−40
−60
−80
−2
−100
−3
−120
0.2
0.4
0.6
0.8
1
1.2
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
−140
0
1.4
0.2
0.4
0.6
Time (s)
0.8
1
Time (s)
(e) Arm x Angle vs. Time
(f) Arm x Ang. Vel. vs. Time
800
X
Y
600
400
Torque Arm 1 (Nm)
Position (radians)
1
(d) Pitch Rate vs. Time
4
−4
0
0.8
Time (s)
Time (s)
200
0
−200
−400
−600
−800
−1000
0
0.2
0.4
0.6
0.8
1
1.2
1.4
Time (s)
(g) Arm Torque vs. Time
Figure 22: Balancing With Only Arms
40
1.2
1.2
1
1
0.8
Actual Path
Planned Path
0.6
X Velocity (m/s)
X Position (m)
0.8
0.4
0.6
0.4
0.2
0.2
0
0
−0.2
0
Actual Path
Planned Path
2
4
6
8
−0.2
0
10
2
4
Time (s)
(a) x Position vs. Time
10
3
0.6
2.5
Arm 1 X Pos Actual
Arm 1 X Pos Planned
Arm 2 X Pos Actual
Arm 2 X Pos Planned
0.2
Arm 1 X Vel Actual
Arm 1 X Vel Planned
Arm 2 X Vel Actual
Arm 2 X Vel Planned
2
Velocity (radians/s)
0.4
Position (radians)
8
(b) x Velocity vs. Time
0.8
0
−0.2
−0.4
1.5
1
0.5
0
−0.5
−0.6
−0.8
0
6
Time (s)
−1
2
4
6
8
−1.5
0
10
2
4
6
8
10
Time (s)
Time (s)
(c) Arm x Angle vs. Time
(d) Arm x Angular Velocity. vs. Time
Figure 23: Driving Body Movement With Arms
6
Conclusions and Future Work
This work examined the dynamics of a dynamically stable mobile robot with arms and
explored two control strategies. The first control strategy uses independent controllers for
balancing/station keeping (ball and body feedback to drive the rollers) and for the arms
(arm feedback to drive the arms). The second controller is a unified controller with full
state feedback (ball, body, and arms) to drive the rollers and arms. From the results of
simulations, we make these conclusions:
• When working independently of the balancing/station keeping controller, the PD arm
controller followed simple planned arm trajectories very well. The exception is in
extreme cases when Ballbot is falling.
• For both the independent controllers and the unified controller, Ballbot assumes a
leaning equilibrium when the arms move. This moves Ballbot’s center of mass.
41
• Both control strategies try to drive Ballbot to a standing equilibrium. When Ballbot
carries a heavy load, the leaning equilibrium is significantly different than the standing
equilibrium. In this case, the independent controllers cannot stabilize Ballbot because
the controller tries to keep the arms up. The unified controller also fails in this circumstance, but unified balancing/arm control in general might prove to be successful
in handling heavy loads.
• In cases where there is no planned arm motion (simply balancing or simply moving)
the unified controller outperforms the independent controllers. This is because the arm
controller uses it arms to help balance or move Ballbot’s body.
• The unified arm controller uses gravity compensation. This strategy only works when
the gravity forces are known. If Ballbot carries an object of unknown mass, Ballbot
does not know how to compensate for its weight. Another problem of unknown objects
is possible saturation of the arm motors which causes arm oscillations.
• The SimMechanics simulation does not handle yaw well because of a software quirk in
specifying constraints.
To overcome some of the problems mentioned the following work is suggested:
• Because of the conflict between a leaning equilibrium and the standing equilibrium new
control strategies should be explored. Calculating the natural equilibrium point online
and setting that as the control goal is an obvious idea to try. Research in humanoid
balancing provides a wealth of resources including momentum methods [10] and zero
moment point methods [3].
• Instead of using LQR to balance Ballbot while arms act independently as disturbances,
treat the arms as disturbances and try a disturbance rejection strategy such as H∞ [2].
• Explore other unified control strategies or a hybrid strategy that uses independent
controllers in some cases and a unified controller in other cases.
• Explore adaptive controllers or online load estimation to compensate for heavy or
unknown loads.
These tasks need to be done to advance Ballbot in general:
• First and foremost, develop an interface/operating environment like RHexLib to make
Ballbot testing easy.
• Solve the yaw problem in SimMechanics or use other software that has a more flexible
way of defining constraints.
• The controllers of this paper are planar. This is because of the complexity of Ballbot’s
3D dynamics. 3D controllers must be developed for the arms especially. The ball and
body control problem also becomes a 3D problem when the body is away from the
standing equilibrium as it is when carrying heavy loads.
42
• Develop better trajectory plans for both the ball/body and the arms. The quintic
polynomial is smooth, but it does not always plan things that are physically possible.
For example, Ballbot cannot begin moving forward immediately. It must develop a
forward lean first which causes it to move backward.
• Develop better contact models. The friction model here lumps all friction into friction
between the ball and rollers. There is also contact between the ground and ball and
between the body and ball. A good starting reference is Montana [6].
• Determine the parameters of the system experimentally. Some of the parameters were
measured directly (body and ball mass and center of mass) and others are taken from
data sheets (the entire actuation system like the motor torque constant). Run tests
to tune or even optimize the physical parameters so that the simulation fits the real
Ballbot test data.
7
References
[1] Ȧström, K.J. and Wittenmark, B. Computer-Controlled Systems: Theory and Design.
3rd Ed., Prentice Hall, Upper Saddle River, NJ, 1997.
[2] Chen, B. H∞ Control and Its Applications, Springer, London, 1998.
[3] Erbatur, K., Obazaki, A., Obiya, K., Takahashi, T., and Kawamura, A. ”A Study on the
Zero Moment Point Measurement for Biped Walking Robots”, Proc. of the 7th International
Workshop on Advanced Motion Control, 3-5 July, 2002, Pages 431-436.
[4] Lauwers, T.B., Kantor, G.A., and Hollis, R.L. ”One is Enough!” Proc. of 2005 International Symposium of Robotics Research, October 12-15, 2005.
[5] Lauwers, T.B., Kantor, G.A., and Hollis, R.L. ”A Dynamically Stable Single-Wheeled
Mobile Robot With Inverse Mouse-Ball Drive”. Proc. of 2006 IEEE International Conference on Robotics and Automation, May 15-19, 2006, Pages:2884 - 2889.
[6] Montana, D.J., ”The Kinematics of Contact and Grasp” International Journal of Robotics
Research, Vol. 7, No. 3, June, 1988, pages 17-32.
[7] Nguyen, J., Morrell, J., Mullens, A., Burnmeister, S., Farrington, K., Thomas, K., and
Gage, D. ”Segway Robotic Mobility Platform”. Proc. of SPIE - Volume 5609: Mobile Robots
XVII, October, 2004.
[8] Sciavicco, L. and Siciliano, B. Modelling and Control of Robot Manipulators. 2nd Ed.,
Springer, London, 2000.
[9] Thibodeau, B.J., Deegan, P., and Grupen, R. ”Static Analysis of Contact Forces With
a Mobile Manipulator”. Proc. of 2006 IEEE International Conference on Robotics and
Automation, May 15-19, 2006, Pages:4007 - 4012.
[10] Yoshida, E., Guan, Y., Sian, N.E., Hugel, V., Blazevic, P., Kheddar, A., and Yokoi,
K. ”Motion Planning for Whole Body Tasks by Humanoid Robots”, Proc. of 2005 IEEE
International Conference on Mechatronics and Automation, July, 2005, Pages 1784-1789.
43
A
SimMechanics Block Diagrams
A.1
Independent Controllers
Figure 24 is a lower level SimMechanics block diagram of the “Control and Actuation” block
in Figure 4. Starting from the left side of the figure, one sees the “X Sensor” block which
senses the angular position and velocity of the revolute joint connecting the “Body” and the
“x1 roller” in Figure 4. The range of the angular position measurement is 0 to 2π, so it
is converted to a continuous angle in the next block. Below the “X Sensor” is the “Body
Sensor” block which outputs the orientation of the body in the form of a quaternion and
quaternion derivative. The next block converts the quaternion to pitch, pitch rate, roll, and
roll rate. Below the “Body Sensor” is the “Path” block. This block takes a distance, the
desired number of seconds in which to move that distance, an amount of time to wait before
moving, and a direction and creates a path according to equations (17) and (18). Below the
“Path” block is the “Y Sensor” which measures the angular position and velocity of the joint
connecting the “Body” and the “y1 roller” in Figure 4. Moving to the right, the augmented
(continuous angles and quaternion changed to pitch and roll) outputs of these three sensor
blocks and the “Path” block output are input to the “Controller”. The “Controller” outputs
a signal to the “Actuation and Friction” block, which in turn sends torques (after being
divided by two by the two gain blocks to split the overall torque between the two motors)
to the four roller actuation blocks, “x1 Act”, “x2 Act”, “y1 Act”, and “y2 Act”.
ap
3
Sense X
Angle
Continuous Angle
Rate
av
X Continuous Angle1
X Sensor
pitch
pitchrate
2
Sense Body
u
X Ball/Body
Y DAC
7
Act Y2
fcn
Y2 Act
roll
Body Sensor
Y Dac
Y Vel
rollrate
Torque Y
.5
6
Act Y1
X Vel
Gain
Y1 Act
DAC Y
Torque X
xpath
1
dist
distance
1
3
move
move time
dead
.5
5
Act X1
DAC X
Gain1
xvelpath
X Dac
pitchpath
pitchratepath
Y Ball/Body
X1 Act
Actuation
and Friction
X DAC
4
Act X2
fcn
wait time
0
0
x direction
ypath
xdir
X2 Act
yvelpath
ydir
rollpath
y direction
time
rollratepath
Clock
Path
Controller
ap
1
Sense Y
av
Y Sensor
Angle
Continuous Angle
Rate
Y Continuous Angle
Figure 24: Ball Control and Actuation Block for Independent Controllers Model
Figure 25 is a lower level SimMechanics block diagram of the “Controller Block” in
Figure 24. It takes all of the sensor data described above and inputs them into two identical
controllers - one in the x direction and one in the y direction Moving from left to right, each
controller takes the differences between roller position, roller velocity, body angle (either roll
44
or pitch), and body velocity and their respective paths (all measured in radians). These
differences are multiplied by gains and then added together and multiplied by negative one.
This results in a torque (in Nm) to be applied to the ball. The ball torque is first converted
to a roller torque and then to a DAC input.
xvel
−K−
To Workspace6
X Ball Torque
X Ball/Body
to Roller Torque1
Cont
xpos
xcont
To Workspace4
To Workspace8
−3.1
P_PosX
X Pos
−2.5
X Dac
D_PosX
−K−
X Vel
1
X Ball/Body
−K−
2
X DAC
X Ball Torque
X Roller Torque
to Roller Torque
to DAC
−1212.6
P_AngleX
xdac
Pitch
To Workspace3
−374.1
D_AngleX
Pitch
Rate
Subtract
pitch
To Workspace10
pitchrate
To Workspace11
rollrate
roll
To Workspace12
To Workspace9
374.1
D_AngleY
Roll
Rate
1212.6
Y Dac
P_AngleY
−K−
Roll
2
Y Ball/Body
Y Ball Torque
to Roller Torque
−2.5
−K−
1
Y DAC
Y Roller Torque
to DAC
D_PosY
ydac
Y Vel
To Workspace2
−3.1
P_PosY
Y Pos
Subtract1
ycont
ypos
To Workspace5
−K−
To Workspace1
yvel
To Workspace7
Y Ball Torque Y Ball/Body
to Roller Torque1
Cont
Figure 25: Ball Controller Block for Independent Controllers Model
Figure 26 is a lower level SimMechanics block diagram of the “Actuation and Friction”
block in Figure 24. This block contains the actuation model summarized by Figure 7. The
blocks “Handle X Friction” and “Handle Y Friction” contain Matlab code that implements
45
the friction model described in Section 2.1.2.
1
Y Vel
current
3
DAC Y
10/2047
Y DAC
Saturation
fcn i_out
1
s
2
DAC to VoltVolt to Current
Peak Current
Limit
10/2047
Integrator
10
X DAC
Saturation
DAC to Volt1 Volt to Current1 Peak Current
Limit1
10
Handle Y Cont
Current Limit
0
1
Torque Y
fcn ty_out
ty_in
Motor Torque
Constant (Nm/Amp)
Integrator1
fcn i_out
ytorque
Handle Y
Friction
To Workspace1
2
X Vel
current
1
s
2
yvel_in
integral
Cont Current
Limit
4
DAC X
0.133
0.133
xvel_in
fcn tx_out
0
2
Torque X
tx_in
integral
Handle X Cont
Current Limit
Motor Torque
Constant1 (Nm/Amp)
Handle X
Friction
Cont Current
Limit1
xtorque
To Workspace4
Figure 26: Actuation and Friction Block for Both Models
Figure 27 is a lower level SimMechanics block diagram of the “Arm Controller” block in
Figure 4. At the bottom are four blocks with light blue outlines. These are the arm sensors.
They sense the angular position and velocity of the x and y rotations of each arm. Also at the
bottom is the “Path” block which is similar to the “Path” block in Figure 24. The outputs of
the arm sensors and the “Path” block are sent to the “Controller” block. The “Controller”
outputs torques (in Nm) which are compared to torque limits in the “Saturation” blocks.
The outputs of the “Saturation” blocks are sent to the four arm actuation blocks, “Arm1x
Torque”, “Arm1y Torque”, “Arm2x Torque”, and “Arm2y Torque”. These four blocks apply
torques to the arms.
46
47
Figure 27: Arm Control and Actuation Block for Independent Controllers Model
arm2xvel
To Workspace5
arm2xpos
arm1xvel
To Workspace7
To Workspace4
arm1xpos
arm1xtorque
To Workspace6
ap
To Workspace8
av
3
Sense Arm1
Arm1
XPos
Arm1x
Act
Arm1x
Y1 Torque
Arm1
XVel
ap
Arm2
XPos
av
Saturation
X Arms
Arm1x
Torque
Arm2x
Arm2
XVel
arm1_xpath
arm1_xdist
arm1_xvelpath
0
arm
dist −C−
X1 Torque
Saturation1
arm1_ydist
arm2_xpath
arm
dist1
0
Mechanical
Branching
Bar2
1
Act
Arm1
Arm1y
torque
arm2_xvelpath
arm2_xdist
fcn
arm
dist2 −C−
arm1_ypath
arm2_ydist
2
arm1_yvelpath
arm_move
arm2_ypath
arm move
time
arm_time
arm2_yvelpath
arm
dist3
Clock1
Act
Arm1y
X2 Torque
arm1ytorque
To Workspace9
Path1
Y Arms
time
arm2xtorque
ap
To Workspace12
av
To Workspace10
Arm1
YPos
Y2 Torque2
Arm1y
Act
Arm2x
Arm1
YVel
4
Sense Arm2
ap
av
Controller
Arm2
YPOS
Saturation2
Arm2y
Arm2
YVel
Arm2x
Torque
Mechanical
Branching
Bar3
arm2yvel
arm2ypos
To Workspace
Saturation3
Arm2y
Torque
To Workspace1
arm1yvel
arm1ypos
To Workspace2
Act
Arm2y
arm2ytorque
To Workspace3
To Workspace11
2
Act
Arm2
Figure 28 is a lower level SimMechanics block diagram of the “Controller Block” in Figure
27. It shows four identical PD controllers (2 DoFs for each arm). The differences between
the arm angular position and velocity and their respective paths are multiplied by gains and
then multiplied by negative one. The controllers output torques in Nm.
Figure 28: Arm Controller Block for Independent Controllers Model
48
A.2
Unified Controller
Figure 29 is very similar to Figure 24 except that it is the “Control and Actuation” block for
the model that uses the unified balancing/station keeping and arm controller. The difference
is the addition of arm sensing blocks and an arm path block in the lower left corner. The
“Actuation and Friction” block is identical to the block for the independent controllers shown
in Figure 26
ap
4
Sense X
Angle
Continuous Angle
Rate
av
X Continuous Angle1
X Sensor
8
Act Y2
Y2 Act
X Ball/Body
pitch
Y Vel
Torque Y
.5
Gain
5
Sense Body
u
2
Act Y1
X Vel
Y DAC
pitchrate
fcn
Y1 Act
DAC Y
roll
Body Sensor
Torque X
Y Ball/Body
.5
3
Act X1
DAC X
Gain1
rollrate
Y Dac
X1 Act
Actuation
and Friction
9
Act X2
xpath
1
dist
X Dac
X Arms
X2 Act
xvelpath
distance
1
3
move
move time
dead
pitchpath
X DAC
pitchratepath
fcn
wait time
0
0
ypath
xdir
x direction
yvelpath
ydir
Y Arms
rollpath
y direction
time
rollratepath
Clock
Path
ap
1
Sense Y
Controller
Angle
Continuous Angle
Rate
av
Y Continuous Angle
Y Sensor
ap
6
Sense Arm1
av
Arm1x
ap
−C−
arm
dist
2
arm move
time
arm_dist
arm_xpath
av
Arm2x
arm_xvelpath
arm_movefcn
arm_ypath
arm_timearm_yvelpath
ap
av
Clock1
Path1
7
Sense Arm2
Arm1y
ap
av
Arm2y
Figure 29: Ball Control and Actuation Block for Unified Controller Model
Figure 30 is a lower level SimMechanics block diagram of the “Controller Block” in Figure
29. It takes all of the sensor data from the rollers, body, and arms and inputs them into two
49
identical controllers - one in the x direction and one in the y direction. Moving from left to
right, each controller takes the differences between roller position, roller velocity, body angle
(either roll or pitch), body velocity, arm 1 angular position and velocity, and arm 2 angular
position and velocity and their respective paths (all measured in radians). These differences
are multiplied by gains and then added together and multiplied by negative one. There are
eight inputs to each controller. The controllers output torques (in Nm) to be applied to the
ball. The ball torque is first converted to a roller torque and then to a DAC input.
−3.1
P_PosX
xrollcont
−2.5
D_PosX
1
X Ball/Body
To Workspace4
−K−
X Ball Torque
X Ball/Body
to Roller Torque1
Cont
xdac
−1215.6
To Workspace5
P_AngleX
−373.7
X Dac
D_AngleX
−K−
−K−
X Ball Torque
to Roller Torque
2
X DAC
X Roller Torque
to DAC
24.3
P_XA1
7.2
D_XA1
3
X Arms
Subtract
24.3
−K−
P_XA2
X Ball Torque
to Roller Torque2
X Arm
Cont
7.2
xrollarmcont
D_XA2
To Workspace1
373.7
yrollcont
D_AngleY
To Workspace2
−K−
1215.6
Y Ball Torque
to Roller Torque2
Y Ball/Body
Cont
P_AngleY
2
Y Ball/Body
ydac
−2.5
To Workspace6
D_PosY
Y Dac
−3.1
P_PosY
−K−
−K−
Y Ball Torque
to Roller Torque
24.3
1
Y DAC
Y Roller Torque
to DAC
P_YA1
7.2
D_YA1
4
Y Arms
Subtract1
24.3
−K−
P_YA2
Y Ball Torque
to Roller Torque1
Y Arm
Cont
7.2
yrollarmcont
D_YA2
To Workspace3
Figure 30: Ball Controller Block for Unified Controller Model
Figure 31 is very similar to Figure 27 except that it is the “Arm Controller” block for the
model that uses the unified balancing/station keeping and arm controller. One difference
is the addition of arm sensing blocks and an arm path block in the lower left corner. The
other difference is the addition of four “gravity compensation” blocks (Matlab code inside).
50
These blocks use equation (20).
ap
4
Sense X
Angle
Continuous Angle
Rate
av
X Continuous Angle1
X Sensor
xpos
arm1xtorque
To Workspace5
To Workspace18
XPos
X Ball/BodyY1 Torque
XVel
pitch
xvel
u
Act
Arm1x
torque
fcn
Gain1
gtorque
psi
To Workspace1
Pitch
pitchrate
5
Sense Body
0.5
Y1 gravity
compensation
Saturation
fcn
Pitch
Rate
roll
Body Sensor
Y Ball/BodyX1 Torque
0.5
Gain
Mechanical
Branching
Bar2
torque
fcn
pitch
rollrate
Arm1x
Torque
gtorque
Saturation1
psi
To Workspace4
Arm1y
torque
x gravity
compensation1
arm1ytorque
pitchrate
To Workspace17
To Workspace7
xpath
1
rollrate
dist
X Arms
X2 Torque
0.5
torque
Gain2
psi
1
3
move
move time
dead
To Workspace8
pitchpath
roll
0
0
Y Arms
ydir
Y2 Torque
Roll
Rate
rollpath
time
psi
fcn
gtorque
Saturation2
Arm2x
Torque
Mechanical
Branching
Bar3
Controller
YVel
Saturation3
ypos
Angle
Continuous Angle
Rate
av
torque
Gain3
x gravity
compensation3
Roll
Path
ap
0.5
rollratepath
Clock
3
Sense Y
arm2xtorque
To Workspace19
yvelpath
x direction
Act
Arm2x
To Workspace6
ypath
xdir
y direction
gtorque
x gravity
compensation2
pitchratepath
fcn
wait time
Act
Arm1y
fcn
xvelpath
distance
1
Act
Arm1
2
Act
Arm2
Arm2y
Torque
To Workspace2
YPos
Y Continuous Angle
Y Sensor
Act
Arm2y
yvel
To Workspace3
time
arm1xvel
arm2ytorque
To Workspace21
arm1xpos
To Workspace11
To Workspace20
To Workspace9
ap
av
6
Sense Arm1
Arm1
XPos
Arm1x
Arm1
XVel
ap
Arm2
XPos
av
Arm2x
arm
dist
Arm2
XVel
arm1_xpath
arm1_xdist
arm1_xvelpath
−C−
0
arm2xpos
arm1_ydist
arm2_xpath
arm2xvel
arm
dist1 −C−
To Workspace10
arm2_xvelpath
arm2_xdist
fcn
arm
dist2
arm1_ypath
arm2_ydist
0
2
arm1_yvelpath
arm_move
arm2_ypath
arm move
time
arm_time
arm2_yvelpath
arm
dist3
Clock1
To Workspace12
arm1ypos
To Workspace13
Path1
time1
ap
To Workspace22
av
arm1yvel
Arm1
YPos
To Workspace15
Arm1y
Arm1
YVel
7
Sense Arm2
arm2ypos
ap
av
To Workspace14
Arm2
YPOS
Arm2y
Arm2
YVel
arm2yvel
To Workspace16
Figure 31: Arm Control and Actuation Block for Unified Controller Model
Figure 32 is a lower level SimMechanics block diagram of the “Controller Block” in Figure
31. There are four identical controllers - one for each DoF of each arm. Moving from left to
right, each controller takes the differences between roller position, roller velocity, body angle
(either roll or pitch), body velocity, the arm angular position of the DoF being controlled
and the arm angular velocity of the DoF being controlled, and their respective paths (all
measured in radians). These differences are multiplied by gains and then added together and
multiplied by negative one. There are six inputs to each controller. The controllers output
torques (in Nm) to be applied to the arms.
51
−0.2
xarmcont
P_PosX
To Workspace19
−0.1
X Ball/Body
Cont
D_PosX
1
X Ball/Body
X1 Dac
−37.6
2
X1 Torque
P_AngleX
−11.5
D_AngleX
Subtract
X Arm1
Cont
20.8
P_XA1
xarmarm1cont
X2 Dac
5.2
Subtract3
To Workspace2
D_XA1
3
X Arms
3
X2 Torque
X Arm2
Cont
20.8
xarmarm2cont
To Workspace4
P_XA2
5.2
D_XA2
yarmcont
Subtract2
To Workspace1
11.5
Y Ball/Body
Cont
D_AngleY
37.6
P_AngleY
Y1 Dac
2
Y Ball/Body
1
Y1 Torque
−0.1
D_PosY
−0.2
Subtract1
P_PosY
Y Arm 1
Cont
20.8
P_YA1
yarmarm1cont
To Workspace3
5.2
Y2 Dac1
D_YA1
Subtract4
4
Y Arms
4
Y2 Torque
20.8
Y Arm 2
Cont
P_YA2
5.2
D_YA2
yarmarm2cont
Subtract5
To Workspace5
Figure 32: Arm Controller Block for Unified Controller Model
52