See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/340296957

The Collinear Mecanum Drive

Thesis · August 2019

CITATIONS READS

0 388

1 author:

Matthew Watson
Opteran Technologies
5 PUBLICATIONS 34 CITATIONS

SEE PROFILE

All content following this page was uploaded by Matthew Watson on 01 June 2021.

The user has requested enhancement of the downloaded file.

 The Collinear Mecanum Drive

Matthew Thomas Watson

August 2019

A thesis submitted in partial fulfilment of the requirements for the degree of

Doctor of Philosophy

Department of Electronic and Electrical Engineering

Faculty of Engineering
The University of Sheffield
UK
To Lucy
Abstract

This thesis focuses on the modelling, analysis, and control of a novel robot lo-
comotion system known as the Collinear Mecanum Drive (CMD). The CMD
utilises three or more collinear Mecanum wheels to generate omnidirectional mo-
tion, whilst simultaneously dynamically balancing. By dynamically balancing,
the ground footprint of a robot utilising a CMD can be designed to be arbitrarily
thin, only lower bounded by choice of wheel diameter. The omnidirectional ma-
noeuvrability of the CMD in combination with this narrow footprint allows for
the navigation of much smaller gaps between obstacles than existing omnidirec-
tional locomotion methods, achieved by translating directly along the common
wheel rotation axis. This provides improved manoeuvrability in confined or clut-
tered environments. Being a dynamically balanced system, the height of the
center of mass of a robot driven by a CMD can be increased without requiring a
proportional increase in the width of the ground footprint so as to avoid toppling
during acceleration or disturbance, as would be the case for existing statically
stable omnidirectional locomotion methods. The CMD therefore promises to en-
able the creation of tall, space-efficient robots of more human-like form factors,
that are better able to navigate the confined and cluttered environments com-
monly encountered in the home, office, and retail personal robotics sectors, and
in the manufacturing and warehousing mobile industrial robotics sectors. This
thesis derives and analyses the CMD’s kinematics and dynamics models, explores
a variety of control approaches, and develops the trajectory planning methods
necessary for the autonomous navigation of an environment. It is hoped that this
locomotion technology will see application across a range of existing and future
mobile robotics sectors.

i
Acknowledgements

Firstly, I would like to thank my supervisor Dr Daniel Gladwin for his unwaver-
ing support and belief in me throughout the past six years of my education, for
the freedom with which he has provided me during this PhD, and for his inspi-
rational work ethic. Dr Gladwin has gone far beyond his expected supervisory
duties to facilitate my development as both a researcher and a professional. I
would also like to express my gratitude to Consequential Robotics and Sebastian
Conran for supporting this project, and for exposing me to further interesting
work during this PhD. I hope we can continue to work together to apply this
research. Thank you to the University of Sheffield’s Department of Electronic
and Electrical Engineering, and in particular the Electrical Machines and Drives
group, for their support throughout the past eight years of my education. Thank
you to Tony Prescott, and to the staff and researchers of Sheffield Robotics for
their support and companionship, and in particular thank you to Michael Port,
whose machining and manufacturing experience greatly improved the quality of
the prototype developed during this PhD. Finally, I would like to thank my won-
derful partner Lucy for her boundless love and encouragement over the past six
years, and I hope many more to come!

iii
Contents

1 Introduction 1
1.1 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Novel Contributions . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2 Wheeled Robot Locomotion 7

2.1 Related Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.1 Manoeuvrability . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.2 Stability and System Footprint . . . . . . . . . . . . . . . 10
2.1.3 The Ideal Indoor Mobile Robot . . . . . . . . . . . . . . . 13
2.1.4 Dynamics of Mechanical Systems . . . . . . . . . . . . . . 13
2.1.5 Underactuated Mechanical Systems . . . . . . . . . . . . 15
2.1.6 Control of Dynamically Stable Mobile Robots . . . . . . . 16
2.1.7 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . 17
2.2 Existing Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2.1 Nonholonomic Statically Stable Systems . . . . . . . . . . 20
2.2.2 Omnidirectional Statically Stable Systems . . . . . . . . . 20
2.2.3 Dynamically Stable Systems with Nonholonomic Kinematic
Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.4 Omnidirectional Dynamically Stable Systems . . . . . . . 23

3 The Collinear Mecanum Drive 29

3.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2 Dynamics Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.2.1 Underactuation of Dynamics . . . . . . . . . . . . . . . . 41
3.2.2 Controllability . . . . . . . . . . . . . . . . . . . . . . . . 43
3.2.3 The Largest Feedback Linearisable Subsystem . . . . . . . 47
3.2.4 Dynamics Analysis Conclusion . . . . . . . . . . . . . . . 48

v
Contents

3.3 Collinear Mecanum Drive Design Considerations . . . . . . . . . 49

3.3.1 Wheel Quantity . . . . . . . . . . . . . . . . . . . . . . . . 49
3.3.2 Choice and Ordering of Roller Angles . . . . . . . . . . . 50
3.3.2.1 Torque Distribution . . . . . . . . . . . . . . . . 50
3.3.2.2 Accessible Acceleration Spaces . . . . . . . . . . 54
3.3.2.3 Translation Efficiency . . . . . . . . . . . . . . . 57
3.3.3 Wheel Spacing . . . . . . . . . . . . . . . . . . . . . . . . 59
3.3.4 Center of Mass Height . . . . . . . . . . . . . . . . . . . . 61
3.3.5 Overall Optimal Parameter Choice . . . . . . . . . . . . . 62
3.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4 A CMD Experimental Prototype 65

4.1 Prototype 1 - Proof of Concept . . . . . . . . . . . . . . . . . . . 65
4.2 Prototype 2 - An Ideal Research Platform . . . . . . . . . . . . . 66
4.2.1 Wheel Selection . . . . . . . . . . . . . . . . . . . . . . . . 67
4.2.2 Wheel Configuration and Suspension . . . . . . . . . . . . 69
4.2.3 Motor Selection . . . . . . . . . . . . . . . . . . . . . . . . 70
4.2.4 Physical Construction . . . . . . . . . . . . . . . . . . . . 71
4.2.5 Motor Control . . . . . . . . . . . . . . . . . . . . . . . . 72
4.2.6 Inertial Sensing . . . . . . . . . . . . . . . . . . . . . . . . 73
4.2.7 Compute . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.3 Motor Cogging Torque and Kinetic Friction Compensation . . . . 74
4.4 Parameter Estimation . . . . . . . . . . . . . . . . . . . . . . . . 78
4.4.1 Wheel Bearing Viscous Friction Coefficient . . . . . . . . 78
4.4.2 Wheel Rolling Friction and Roller Viscous Friction Coeffi-
cients . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.4.3 Pendulum Center of Mass Height . . . . . . . . . . . . . . 80
4.4.4 Pendulum Inertia about the Balance Axis . . . . . . . . . 81
4.4.5 Pendulum Inertia about Vertical and Longitudinal Axes . 82
4.4.6 Wheel Mass and Inertia . . . . . . . . . . . . . . . . . . . 83
4.4.7 Comparison of Experimentally Measured Parameters with
CAD Derived Estimates . . . . . . . . . . . . . . . . . . . 83
4.5 Sensing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
4.5.1 Wheel Encoders . . . . . . . . . . . . . . . . . . . . . . . 84
4.5.2 Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . 85

vi
 Contents

4.5.3 Accelerometers . . . . . . . . . . . . . . . . . . . . . . . . 88
4.5.4 Omitted Sensors . . . . . . . . . . . . . . . . . . . . . . . 89
4.6 State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
4.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95

5 Nonlinear Control 99
5.1 Partial Feedback Linearisation . . . . . . . . . . . . . . . . . . . 100
5.2 Nonlinear Control of the Partially Feedback Linearised CMD . . 104
5.3 Backstepping Control of Local Body Frame Velocities . . . . . . 106
5.4 Backstepping Inertial Frame Velocity Control . . . . . . . . . . . 121
5.5 Backstepping Global Position Control . . . . . . . . . . . . . . . 128
5.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130

6 Model Predictive Control 135

6.1 Controller Derivation . . . . . . . . . . . . . . . . . . . . . . . . . 138
6.1.1 Reference Tracking . . . . . . . . . . . . . . . . . . . . . . 139
6.1.2 Autonomous Model Formulation and Reference Previewing 140
6.1.3 Cost Function Derivation . . . . . . . . . . . . . . . . . . 142
6.1.4 Infeasible Reference Tracking . . . . . . . . . . . . . . . . 144
6.1.5 Quadratic Cost Weights and Discretisation Period . . . . 146
6.1.6 Constraint Derivation . . . . . . . . . . . . . . . . . . . . 146
6.1.7 Velocity Constraint Approximation . . . . . . . . . . . . . 148
6.1.8 Slack Variables and their Distribution . . . . . . . . . . . 149
6.1.9 The Effect of Input Constraints on Feasibility . . . . . . . 151
6.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
6.2.1 Step Reference Tracking - Forward Translation . . . . . . 154
6.2.2 Step Reference Tracking - Lateral Translation . . . . . . . 157
6.2.3 Constraint-Violating Disturbance Handling . . . . . . . . 157
6.2.4 Time-Varying Trajectory Tracking . . . . . . . . . . . . . 158
6.2.5 Complex Trajectory Tracking . . . . . . . . . . . . . . . . 163
6.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165

7 Fast Online Trajectory Optimisation 169

7.1 Differentially Flat Model Derivation . . . . . . . . . . . . . . . . 172
7.2 Closed Loop Trajectory Tracking . . . . . . . . . . . . . . . . . . 175

vii
Contents

7.3 Trajectory Generation using Optimally Smooth Polynomials . . . 178

7.3.1 Polynomial Cost and Piecewise Smoothness Requirements 179
7.3.2 Polynomial Degree Requirements . . . . . . . . . . . . . . 181
7.3.3 QP Formulation . . . . . . . . . . . . . . . . . . . . . . . 181
7.3.4 Experimental Unconstrained Trajectory Tracking . . . . . 183
7.3.5 Selection of Segment Durations . . . . . . . . . . . . . . . 184
7.4 Velocity Constrained Trajectory Generation . . . . . . . . . . . . 188
7.4.1 Splitting Polynomial Segments . . . . . . . . . . . . . . . 189
7.4.2 Velocity Constraint Approximation . . . . . . . . . . . . . 191
7.4.3 Velocity Constraint Enforcement using Quadratic Program-
ming . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
7.4.4 Velocity Constraint Enforcement using Sum-of-Squares Pro-
gramming . . . . . . . . . . . . . . . . . . . . . . . . . . . 193
7.4.5 Velocity Constrained Subsegment Duration Selection . . . 197
7.4.6 Experimental Velocity Constrained Trajectory Planning and
Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200
7.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203

8 Conclusion 209
8.1 A Comparison of Controllers . . . . . . . . . . . . . . . . . . . . 210
8.1.1 Aggressiveness of Control . . . . . . . . . . . . . . . . . . 210
8.1.2 Computational Requirements . . . . . . . . . . . . . . . . 211
8.1.3 Actuation Power Requirements . . . . . . . . . . . . . . . 212
8.1.4 Safety . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212
8.1.5 Smoothness and Grace of Motion . . . . . . . . . . . . . . 213
8.1.6 Map Navigation . . . . . . . . . . . . . . . . . . . . . . . 213
8.1.7 A Ranking of Controllers & Application Suitability . . . . 214

viii
 Contents

Appendix A CMD Model Matrix Definitions 239

A.1 Matrices for CMD Dynamics Model in (q, q̇) . . . . . . . . . . . . 239
A.2 Matrices for CMD Dynamics Model in (p, ṗ) . . . . . . . . . . . . 242
A.3 Matrices for CMD Dynamics Model in (p, v) . . . . . . . . . . . . 248

Appendix B MATLAB Model Derivation and Analysis Code 253

ix
List of Figures

2.1 Typical omnidirectional wheels . . . . . . . . . . . . . . . . . . . 9

2.2 A demonstration of how statically stable systems become less sta-
ble when carrying off-axis loads or operating on sloped surfaces. . 12
2.3 Typical omnidirectional statically stable mobile robots, also re-
ferred to as holonomic robots. . . . . . . . . . . . . . . . . . . . . 22
2.4 Notable two-wheeled mobile inverted pendulums . . . . . . . . . 23
2.5 Notable ball-balancing robots . . . . . . . . . . . . . . . . . . . . 25
2.6 Omnidirectional balancing systems utilising omniwheels with ac-
tuated rollers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.1 Collinear Mecanum Drive coordinates and parameters for the ex-
perimental prototype shown in Figure 4.3 . . . . . . . . . . . . . 31
3.2 Contribution of each wheel torque to acceleration v̇x over varying
roller orderings and angles . . . . . . . . . . . . . . . . . . . . . . 51
3.3 Contribution of each wheel torque to acceleration v̇y for the nw = 3
case over varying roller orderings and angles . . . . . . . . . . . . 52
3.4 Contribution of each wheel torque to acceleration v̇x for the nw = 4
case over varying roller orderings and angles . . . . . . . . . . . . 53
3.5 Sets of feasible body accelerations with wheel torque constraints
τ = 0.1 N m for varying orderings of α, calculated at the stationary
upright equilibrium with nw = 3 . . . . . . . . . . . . . . . . . . 55
3.6 Sets of feasible body accelerations with wheel torque constraints
τ = 0.1 N m for varying orderings of α, calculated at the stationary
upright equilibrium with nw = 4. . . . . . . . . . . . . . . . . . . 56
3.7 Translational power requirements over body relative translation
direction and roller angle α, with nw = 4 and α = [a, −a, −a, a]. 58
3.8 Translational power requirements over body relative translation
direction and roller angle α, with nw = 3. . . . . . . . . . . . . . 60

xi
List of Figures

3.9 CMD rate of energy dissipation due to viscous friction for nw = 3

and l = [d, 0, −d] . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
3.10 Variation in forward acceleration due to nonzero lean angle over
pendulum center of mass height . . . . . . . . . . . . . . . . . . . 62

4.1 A proof of concept prototype . . . . . . . . . . . . . . . . . . . . 66

4.2 The proof-of-concept prototype in Figure 4.1 attempting to track
a square trajectory whilst maintaining a constant heading . . . . 67
4.3 The Collinear Mecanum Drive experimental prototype used in this
thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4.4 A close-up image of the drive assembly of the prototype in Figure
4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.5 Weight distributing suspension design used in the prototype in
Figure 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4.6 Motor terminal current step responses to small and large square
wave reference inputs . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.7 A diagram showing the organisation of the prototype’s subsystems 75
4.8 Control diagram for the Collinear Mecanum Drive prototype . . 75
4.9 A measurement of cogging torque for motor 1 of the prototype in
Figure 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.10 Variation in wheel angular velocity over time whilst tracking a con-
stant velocity under a closed-loop PI controller, in which cogging
torque compensation is enabled for the first half of the experiment. 76
4.11 Steady state wheel angular velocities over a slowly ramping torque
input for both rotation directions, in which the gradient represents
the wheel viscous friction coefficient kvw . . . . . . . . . . . . . . . 79
4.12 Velocity and torque data for a translation y = [0, 3, 0], in which a
constant velocity is attained for the majority of each movement.
This data is used to estimate the wheel rolling friction coefficient. 80
4.13 Velocity and torque data for a translation x = [0, 3, 0], in which a
constant velocity is attained for the majority of each movement.
This data is used to estimate the roller bearing viscous friction
coefficient. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.14 Experiment used to locate the position of the prototype’s center
of mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

xii
 List of Figures

4.15 Experimental data from a physical pendulum experiment, used to

determine the inertia of the prototype’s pendulum mass. . . . . . 83
4.16 Wheel angular acceleration with a square wave torque input, with
viscous and kinetic friction compensation active. . . . . . . . . . 84
4.17 CMD CAD model from which parameter verification values are
derived . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
4.18 Integrated gyroscope scale and misalignment calibration data both
pre and post compensation . . . . . . . . . . . . . . . . . . . . . 87
4.19 Accelerometer scale and bias calibration data . . . . . . . . . . . 88
4.20 State estimation data gathered by moving the system through a
range of random trajectories, with the same real-world position
and pose maintained at t = 20 s and t ≥ 68 s to allow a measure-
ment of dead reckoning drift. . . . . . . . . . . . . . . . . . . . . 96

5.1 Simulated state trajectories for the partially feedback linearised

CMD, demonstrating the expected linear behaviour. . . . . . . . 105
5.2 Forward acceleration v̇y over θp = [−2.14, 2.14] for 20 random
samples of x . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.3 A cross section of the attainable set of steady state body velocities
vx , vy , and φ̇, taken across vy = 0 and in which colour is used to
encode θp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
5.4 A Lyapunov barrier function used to enforce input and state con-
straints in this thesis’s backstepping derived controllers. . . . . . 114
5.5 A plot of f˙v̇ (θ̇pr ) over θpr ∈ P [− π , π ] for 1000 uniformly random
y,ss 2 2
samples {vx , vy , φ̇} ∈ Ass . . . . . . . . . . . . . . . . . . . . . . . 117
5.6 A phase portrait of a backstepping derived body frame velocity
controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
5.7 Simulated state trajectories for a backstepping derived body frame
velocity controller with a constant nonzero reference. . . . . . . . 119
5.8 Experimental state trajectories for a backstepping derived body
frame velocity controller with a constant nonzero reference. . . . 120
5.9 A strobe-effect image of the trajectory shown in Figure 5.8. . . . 120
5.10 Simulated state trajectories for a backstepping derived inertial
frame velocity controller with a constant nonzero reference. . . . 126

xiii
List of Figures

5.11 Experimental state trajectories for a backstepping derived inertial

frame velocity controller with a constant nonzero reference. . . . 127
5.12 A long exposure image of the trajectory in Figure 5.11, in which
two blue LEDs are used to capture the experimentally tracked path.128
5.13 Simulated state trajectories for a backstepping derived inertial
frame position controller for a step translation. . . . . . . . . . . 131
5.14 Experimental state trajectories for a backstepping derived inertial
frame position controller for a step translation. . . . . . . . . . . 132
5.15 A long exposure trajectory capturing a step translation under a
backstepping derived inertial frame position controller. . . . . . . 133

6.1 Polytope approximations of the quadratic velocity constraint ẋ2 +

ẏ 2 ≤ v 2 with v = v x = v y = 1. . . . . . . . . . . . . . . . . . . . . 148
6.2 Comparison of controller response in the regulation case to an
initial constraint violation of vy = 2v y for ni = [1, 2, 3] . . . . . . 150
6.3 A cross section of SMCAS through θp|k and vy|k , taken through
both the origin and through θ̇p|k = 4 rad s−1 with a varying control
horizon. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
6.4 A cross section of SMCAS through θp|k and vy|k for a varying control
horizon nc and modified quadratic state cost. . . . . . . . . . . . 153
6.5 Simulated MPC response to a step reference of y = [0, 1]m . . . . 155
6.6 Experimental response to a step reference of ry = [0, 1]m . . . . . 156
6.7 Simulated MPC response for a step reference of rx = [0, 1]m . . . 158
6.8 Experimental response to a step reference of rx = [0, 1]m . . . . . 159
6.9 Simulated controller response in regulation scenario to an initial
disturbance of vy = 2v y = 2 m s−1 . . . . . . . . . . . . . . . . . . 160
6.10 Experimental response in the regulation case to an initial velocity
disturbance of approximately vy = 2v y = 2 m s−1 . . . . . . . . . 161
6.11 Simulated MPC ry tracking error (top) for a linearly ramping po-
sition setpoint over varying nr . . . . . . . . . . . . . . . . . . . . 162
6.12 Simulated MPC state trajectories for control horizons nc = 10 and
nc = 28 for a figure-of-eight reference of 10 s duration and constant
heading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
6.13 State trajectories for the figure-of-eight trajectory in Figure 6.12
for a control horizon of nc = 10. . . . . . . . . . . . . . . . . . . . 164

xiv
 List of Figures

6.14 Experimental response to the same 10 s figure-of-eight trajectory

as in Figures 6.13 and 6.12 with a control horizon of nc = 12 . . 166
6.15 Experimental response to the same 10 s figure-of-eight trajectory
as in Figures 6.13 and 6.12 with a control horizon of nc = 12 . . 167

7.1 A trajectory in S2 for a translation from S2 = 0 to S2 = 1 in 2 s,

with corresponding state and input trajectories derived from the
differentially flat model. . . . . . . . . . . . . . . . . . . . . . . . 175
7.2 A comparison of minimising different choices of polynomial deriva-
tive for a trajectory through six random waypoints . . . . . . . . 180
7.3 Simulated tracking of the trajectory in Figure 7.1 . . . . . . . . . 184
7.4 Experimental tracking of the trajectory in Figure 7.1 . . . . . . . 185
7.5 Experimental tracking of a pirouetting trajectory from (x, y, φ) =
0 to (x, y, φ) = (0, 2, 2π) generated using a differential flatness
based approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
7.6 Comparison of choice of T2 for a trajectory through the Cartesian
waypoints (0, 0), (1, 1), and (0, 2), with T1 = 3 s. . . . . . . . . . 187
7.7 Flat output and state trajectories through five Cartesian way-
points with both constant segment durations of 1 s and optimised
durations, in which the time penalty weighting Kt is chosen to
yield equal total duration. . . . . . . . . . . . . . . . . . . . . . . 188
7.8 A high-degree velocity constrained polynomial exhibiting Runge’s
phenomena, in which approximating a constant velocity requires
large variations in the polynomial’s higher order derivatives. . . . 189
7.9 A point-to-point trajectory split into three subsegments, with a
linear polynomial describing the middle subsegment, demonstrat-
ing how this description of point-to-point trajectories in the pres-
ence of velocity constraints can be better parametrised by three
concatenated polynomial subsegments. . . . . . . . . . . . . . . . 190
7.10 Polytope approximations of the quadratic velocity constraint ẋ2 +
ẏ 2 ≤ v 2 with v = 1. . . . . . . . . . . . . . . . . . . . . . . . . . . 192
7.11 A comparison of the original unconstrained polynomial QP solu-
tion with the proposed velocity constrained planner . . . . . . . . 193

xv
List of Figures

7.12 A series of histograms depicting the distribution of solve time over

waypoint quantity nw for the polynomial based optimisation of
velocity constrained trajectories, solved using both the recursively
constrained QP and SOS constrained SDP methods. . . . . . . . 197
7.13 Distribution of trapezoidal timing optimisation execution time for
increasing numbers of waypoints in three synchronised flat out-
puts, implemented on an Intel i7-4720HQ processor. . . . . . . . 200
7.14 Minimum time trapezoidal and full degree optimally smooth tra-
jectories through nine random waypoints . . . . . . . . . . . . . . 201
7.15 Optimally smooth flat output and state trajectories for both trape-
zoidal optimised polynomial durations and gradient descent opti-
mised polynomial durations, with equal total duration between the
two schemes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202
7.16 A strobe-effect image capturing the navigation of a gap narrower
than the robot’s width using the differential flatness based plan-
ning approach, demonstrating one of the major advantages of the
Collinear Mecanum Drive over alternative locomotion solutions. . 202
7.17 A long exposure image capturing a differential flatness derived
trajectory, in which two pirouettes are performed along with a
translation of 2 m. . . . . . . . . . . . . . . . . . . . . . . . . . . 203
7.18 State trajectories for the gap navigation experiment shown in Fig-
ure 7.16 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204
7.19 State trajectories for the pirouetting trajectory experiment shown
in Figure 7.17 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 205
7.20 A long exposure image capturing a circular trajectory passing
through the four corners of a 1.5 m square whilst performing a
rotation of 6π rad. This is intended to demonstrate the complete
freedom of motion achievable using the CMD. . . . . . . . . . . . 206
7.21 State trajectories for the same experiment as shown in Figure 7.20,
but whilst only performing a rotation of 4π rad in φ in order to
improve clarity. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 207

8.1 An example of a Holonomic Collinear Mecanum Drive built using

a reaction wheel . . . . . . . . . . . . . . . . . . . . . . . . . . . 218

xvi
List of Tables

4.1 Motor parameters for that used in the prototype in Figure 4.3 . . 71
4.2 Table of experimentally and CAD derived parameters for the pro-
totype depicted in Figure 4.3. . . . . . . . . . . . . . . . . . . . . 85

8.1 A qualitative ranking of the three presented controllers against a

number of metrics, where a value of 1 denotes the most suitable
controller for a given metric. . . . . . . . . . . . . . . . . . . . . . 215

xvii
Nomenclature

αi Roller angle of rotation of wheel i relative to the wheel rotation axis and
about a vertical axis running through the center of the wheel

B Local body frame coordinate system

E Global inertial frame coordinate system

b̂x A unit vector along the x axis of the coordinate system B, or similar.

SMAS Maximal admissible set

L Lagrangian

Xe The set of a dynamic system’s equilibrium states

Wi Local wheel frame coordinate system of wheel i

SMCAS Maximal constrained admissible set

Ωi Angular position of the ground-contacting roller of wheel i about r̂x

x The maximum constrained value of x in the constraint |x| ≤ x

φ CMD rotation about the vertical

R The set of real numbers

P Local pendulum frame coordinate system

τ A vector of wheel torques

θi Angular position of wheel i relative to vertical about ŵx

θp System lean angle relative to vertical

xix
Nomenclature

θei Angular position of wheel i relative to the pendulum body, measured by

an encoder.

~ A vector of angular rates about the three pendulum axes Ω

~ = Ωp Ωq Ωr T
 
Ω

~m
Ω A vector of angular rates about the three pendulum axes as measured by
the onboard accelerometers Ω~ m = Ωm,p Ωm,q Ωm,r T
 

A vector of accelerations along the three pendulum axes ~a = ax ay az

 T
~a

~am A vector of angular rates about the three pendulum axes as measured by
the onboard accelerometers ~am = am,p am,q am,r
 T

~vEC,B The velocity of C relative to E expressed in B, or similar.

B+ The Moore–Penrose inverse of B

Cn The set of functions for which derivatives exist and are continuous up to
the nth order

cj A control perturbation at time step j

g Acceleration due to gravity (m s−2 )

H A Hessian matrix

hp Height of pendulum center of mass along p̂z and relative to B

hcm Height of overall system center of mass along p̂z and relative to B

Ipbx Pendulum inertia about b̂x

Ipby Pendulum inertia about b̂y

Ipbz Pendulum inertia about b̂z

Ipx Pendulum inertia about p̂x

Ipy Pendulum inertia about p̂y

Ipz Pendulum inertia about p̂z

J A cost function

xx
 Nomenclature

Kt Motor torque constant

krw Coefficient of rolling friction between wheel and ground

kvr Coefficient of roller bearing viscous friction

kvw Coefficient of wheel bearing viscous friction

li Location of wheel i along b̂x , measured from the platform’s center.

mp Pendulum body mass

nc Control horizon

ni Independent constraint softening horizon

nr Reference previewing horizon

ns Slack variable quantity

nw Number of waypoints

nw Number of wheels

ncon Constraint horizon

P Power

p Reduced vector of generalised coordinates

p, p(t) A polynomial in t

Q Quadratic state cost

Q Q in a QR decomposition

q Vector of generalised coordinates

R Quadratic input cost

R R in a QR decomposition

r A vector of target output references

rr Roller radius, measured at widest point.

xxi
Nomenclature

Rw Motor terminal resistance

rw Wheel radius

Rbp The rotation matrix that rotates a vector from coordinate frame P into
B, or similar.

Sn Flat output n

t Time in seconds

Ti Duration for which polynomial pi (t) defines a trajectory segment

Ts Sampling period

v Vector of system local body frame velocities

vx System local body frame velocity along b̂x

vy System local body frame velocity along b̂y

x Component of Cartesian position in inertial frame

x State space model state vector

y Component of Cartesian position in inertial frame

y State space model output

Zk Autonomous model state vector at timestep k

xxii
Chapter 1

Introduction

This chapter introduces the broad robotics problem that is to be addressed by the
work in this thesis, and gives an outline of the structure of the thesis and the
content of the chapters to follow. It concludes with a statement of the novel
contributions to the literature made by the work presented in this thesis, and a
list of publications.

It is a widely accepted belief that robots will continue to see ever increasing
application across a wide variety of industries, ranging from industrial ware-
housing and manufacturing applications to personal robotics in the home and
office. Whilst in previous decades this growth has been largely centered around
fixed-base manipulator robots, over the past number of years there has been sig-
nificant growth in the field of mobile robotics, robots which are able to freely
navigate their environments whilst performing inspection, manipulation, and
human-interaction tasks.
The environments in which mobile robots have to operate can be divided into
two categories: structured, man-made environments, such as that found within
indoor spaces, and the unstructured environments encountered in the natural
world, with each possessing significantly different properties and challenges from
a mobile robotics perspective. Indoor environments are typically designed around
their human users; ground surfaces are usually smooth and solid, with the ex-
ception of stairs, and obstacles such as walls, doors, and furniture are sized and
located as to be comfortably navigated and worked around by humans. This sets
a lower bound on the minimum width of free space left between obstacles that
a mobile robot must navigate. Compared to the height of a human this may be

1
Chapter 1 Introduction

a relatively small gap, as the smallest dimension of a standing human’s projec-

tion onto the ground plane will typically be around a sixth of their height [1],
a relatively large ratio. These indoor environments are in contrast to the un-
constrained environments encountered in the natural world, which feature rough
and discontinuous terrain, compliant ground materials, and three-dimensional
obstacles, with no bounds on the required minimum navigable gap.
The structured nature of indoor environments also extends to the task space
in which a mobile robot’s manipulators, sensors, and interfaces must operate.
Again, these spaces often desired around humans; sensor-obscuring furniture ex-
tends up to heights of over a meter, equipment interfaces are located as to be
operated by a sitting or standing user, and items to be reached by a robot may
be stored anywhere from ground level to high shelving. Similarly, in applications
where a robot is to interact with a user, such as in personal robotics or retail
customer service, this interaction may need to be performed in a face-to-face
manner with a standing human.
Unsurprisingly, considering the optimal indoor robot form factor and locomo-
tion characteristics as that which best fit the above criteria points to an optimal
form factor of around human-height, with a ground footprint similar to that of
a human, and with the ability to instantaneously move in any direction. Whilst
locomotion methods have been demonstrated which can meet these criteria, all
suffer from one or more drawbacks that have prevented their mainstream adop-
tion.
The work in this thesis aims to address this gap: presenting, analysing, and
controlling a novel form of omnidirectional locomotion that is free of the faults
of these existing solutions.

1.1 Thesis Outline

This thesis is split into two introductory chapters, five novel contribution chap-
ters, and one concluding chapter.
Chapter 2 aims to familiarise the reader with some of the key concepts ex-
plored in this thesis, and gives an overview of the generalised methods used to
approach these problems. Chapter 3 introduces the invention that is the focus of
this thesis, the Collinear Mecanum Drive (CMD). Generalised CMD kinematic

2
 1.2 Novel Contributions

and dynamic models are derived, and are analysed with respect to the CMD’s
underactuation and nonholonomy properties, nonlinear controllability, and the
size of the maximum feedback linearisable subsystem. Chapter 4 describes the
reasoning and methodology behind the design and development of a CMD ex-
perimental prototype, which is later used for control validation throughout this
thesis. Experimental methods for the measurement of model parameter are pre-
sented, and an online full-state estimator is derived and demonstrated. Chapter
5 partially feedback linearises the CMD, and then demonstrates three nonlinear
controllers for the regulation of local frame body translational velocities, inertial
frame translational velocities, and global inertial frame positions, all with Lya-
punov stability guarantees. Chapter 6 addresses the problem of wheel traction
and the generation of toward minimum-time trajectories by the development and
implementation of a constrained dual-mode model predictive controller, capable
of enforcing velocity, lean angle, and wheel torque constraints. Finally, Chapter
7 shows how the dynamics model of a CMD can be made to be differentially flat,
allowing the generation of approximately dynamically feasible state and input
trajectories from sufficiently smooth geometric Cartesian paths. These methods
are demonstrated using existing polynomial trajectory planning techniques, al-
lowing for the fast online generation of trajectories through multiple successive
Cartesian waypoints. Finally, a novel approach to this polynomial optimisation
problem is demonstrated, in which sum-of-squares programming is used to en-
force trajectory constraints in continuous time, yielding a significant reduction
in computation time over the state-of-the-art.

1.2 Novel Contributions

The main novel contributions of this thesis are summarised below:

1. Dynamics Modelling and Analysis of the Collinear Mecanum Drive

- Chapter 3
This work is the literature’s first derivation of the kinematics and dynamics
models of the CMD. These are then analysed in terms of nonholonomy and
underactuation properties, the size of the largest feedback linearisable sub-
system is determined, and controllability and accessibility of the CMD are
proven. An analysis of the effect of parameter choices on the CMD char-

3
Chapter 1 Introduction

acteristics is performed, providing generalised guidelines for the optimal

selection of these parameters.

2. Partial Feedback Linearisation of the CMD and Nonlinear Con-

trol - Chapter 5

Chapter 5 shows how the CMD can be partially feedback linearised by

a change of input and coordinate transformation, which is then used in
the derivation of three nonlinear controllers using backstepping techniques,
providing Lyapunov stability and convergence guarantees. These three
controllers regulate local frame body velocities, required when operating
the CMD as a vehicle or teleoperated platform, and both inertial frame
velocities and positions, required when the CMD is required to operate
autonomously. These are all experimentally demonstrated on the CMD
prototype.

3. Constrained Model Predictive Control of the CMD - Chapter 6

Chapter 6 details the derivation and implementation of an online CMD

model predictive controller. This incorporates optimal satisfaction of ve-
locity, lean angle, and wheel torque constraints. A dual-mode topology is
used, providing a guarantee of stability and convergence for the linearised
model. This is successfully implemented in real-time, and represents both
the first implementation of a model predictive controller for the position
control of a CMD, as well as the literature’s first demonstration of the model
predictive position control of a dynamically balancing mobile system of any
wheel configuration.

4. Dynamically Feasible Trajectory Generation for the CMD using

Differential Flatness - Chapter 7

A method is demonstrated by which the CMD dynamics model can be made

to be differentially flat, achieved through a combination of suitable ficti-
tious output selection and model simplification. This allows the derivation
of dynamically feasible state trajectories from sufficiently smooth polyno-
mials in the flat outputs, allowing for fast and dynamically feasible online
trajectory planning.

4
 1.3 Publications

5. Constrained Trajectory Planning using Sum-of-Squares Program-

ming - Chapter 7
This contribution is a novel constrained trajectory planning method, in
which sum-of-squares programming is used to enforce velocity inequality
constraints in continuous time, rather than in a discrete manner at a re-
cursively grown list of points as in the existing state-of-the-art. This yields
around a 40% reduction in execution time over the existing state-of-the-art.

6.

1.3 Publications
• M. T. Watson, D. T. Gladwin, and T. J. Prescott, ”The Collinear Mecanum
Drive: Modelling, Analysis, Partial Feedback Linearization, and Nonlinear
Control,” IEEE Transactions on Robotics, (preprint), 2020. doi.org/10.
1109/TRO.2020.2977878.

• M. T. Watson, D. T. Gladwin, T. J. Prescott, and S. O. Conran, “Dual-

Mode Model Predictive Control of an Omnidirectional Wheeled Inverted
Pendulum,” IEEE/ASME Transactions on Mechatronics, vol. 24, no. 6, pp.
2964-2975, Dec. 2019. doi.org/10.1109/TMECH.2019.2943708.

• M. T. Watson, D. T. Gladwin, T. J. Prescott, and S. O. Conran. ”Veloc-

ity Constrained Trajectory Generation for a Collinear Mecanum Wheeled

5
Chapter 1 Introduction

Robot” in International Conference on Robotics and Automation (ICRA),

Montreal, Canada, 2019, pp. 4444–4450. doi.org/10.1109/ICRA.2019.
8794019.

• M. T. Watson, D. T. Gladwin, T. J. Prescott, and S. O. Conran. ”Design

and Control of a Novel Omnidirectional Dynamically Balancing Platform
for Remote Inspection of Confined and Cluttered Environments” in IEEE
International Conference on Industrial Electronics for Sustainable Energy
Systems (IESES), Hamilton, New Zealand, 2018, pp. 473–478. doi.org/
10.1109/IESES.2018.8349923.

• M. T. Watson, UK Patent Application No. 1901297.0, filed 30/01/2019 by

The University of Sheffield and Consequential Robotics Ltd.

6
Chapter 2

Wheeled Robot Locomotion

This chapter aims to introduce and frame some of the key concepts used in
this thesis. It then introduces some alternative existing solutions to the problem
outlined in Chapter 1, along with a brief review of some common approaches to
their control and trajectory planning.

2.1 Related Concepts

A number of key concepts are now introduced to provide background and context
for the rest of the thesis.

2.1.1 Manoeuvrability

The manoeuvrability of a wheeled mobile robot is defined by its kinematic con-

straints. These are constraints on the velocities of a system imposed by the con-
figuration of its wheels and their prevention of any sliding motion of the wheel
surface against the ground, with the assumption of constant traction. They are
linear in the system velocities, and can be represented in the Pfaffian matrix form

AT (q)q̇ = 0 (2.1.1)

in which each row represents a single Pfaffian constraint aTi (q)q̇ = 0, where each
aTi is smooth and independent.
A Pfaffian constraint is referred to as a nonholonomic constraint if it is nonin-
tegrable, i.e. if it cannot be integrated to an equivalent set of configuration con-
straints h(q) = c where ∂h(q)
∂q = λ(q)aT (q), with some integrating factor λ(q) =
6 0

7
Chapter 2 Wheeled Robot Locomotion

and integration constant c. Systems subject to one or more nonholonomic kine-

matic constraints are referred to as nonholonomic systems, whereas those with
only holonomic or no kinematic constraints are referred to as either omnidirec-
tional or holonomic systems. While a nonholonomic system’s velocities are re-
stricted to evolving on the null space of AT (q), unlike in systems with holonomic
constraints there is no loss of accessibility in the system’s configuration space. A
car is a common example of a nonholonomic system due to its use of standard
wheels, in which these constraints prevent direct translation in a direction orthog-
onal to the car’s heading, with such a translation instead only achievable using a
parallel parking manoeuvre. Despite these constraints, a car is still able to reach
any given (x, y, φ) configuration in the absence of obstacles by combinations of
forward motion and a varying steering angle, demonstrating the nonintegrabil-
ity of its nonholonomic constraints and the retention of full configuration space
accessibility. Despite this full accessibility, i.e. despite being fully controllable,
Brockett’s theorem shows that systems with nonholonomic kinematic constraints
cannot be asymptotically stabilised to an equilibrium by smooth time invariant
feedback of the form u = u(q) [2]. This complicates control design, and prevents
high bandwidth Cartesian position control as achieved by humans and other om-
nidirectional systems, yielding systems with poor navigational ability in confined
environments and inefficient movement when performing precise position tracking
tasks.
While in the context of kinematics it appears that all omnidirectional sys-
tems are holonomic systems, referring to a system as being holonomic implies
the absence of nonholonomic constraints of any order, whereas the concepts of
manoeuvrability and omnidirectionality only consider first-order kinematic con-
straints. These two terms are therefore not equivalent: holonomic systems form
a subset of omnidirectional systems.
Systems with omnidirectional manoeuvrability typically utilize either omni-
wheels or Mecanum wheels1 [3], both exampled in Figure 2.1. While omnidi-
rectional motion can be also achieved by the use of two or more independently

1
Mecanum wheels, invented by Swedish engineer Bengt Erland Ilon in 1972, are named after
the Swedish company Mecanum AB who originally patented the concept. This patent was
bought by the US Navy for logistics applications on aircraft carriers, and was later sold
to KUKA for use on warehouse vehicles and robots. Mecanum wheels are also commonly
referred to as Ilon or Swedish wheels.

8
 2.1 Related Concepts

(a) A Mecanum wheel, (b) A Mecanum wheel, (c) An omniwheel, α =

α = ±π/4. side view. π/2.

Figure 2.1: Typical omnidirectional wheels

actuated and steered regular wheels, this requires a delay before motion can be
commenced whilst the wheels are aligned with the desired direction of motion.
Omnidirectional wheels provide intentional directional slip orthogonal to their
direction of traction by the use of a ring of unactuated rollers affixed about the
circumference of the wheel. For an omniwheel these rollers have axes of rotation
offset from that of the parent wheel by a rotation of 90° about an axis orthogonal
to that of the parent wheel’s axis of rotation, whereas for a Mecanum wheel an
angle of 45° is used2 . The rollers on both wheel types are shaped such that the
projection of the wheel onto a plane normal to its rotation axis forms a perfect
circle, which for omniwheels necessitates the use two separate rows of rollers,
visible in Figure 2.1c. Gfrerrer derives the parametric equations required to de-
fine the desired shape of an omnidirectional wheel’s rollers [4]. Ferriere compares
Mecanum and omniwheels of varying construction with regards to horizontal
and vertical vibration, load capacity, and step climbing ability [5], arguing that
Mecanum wheels advantageously offer a continuous roller contact transition and
a reduction in vertical vibration over omniwheels, but introduce a parasitic yaw
torque due to the discontinuous movement of the ground contact point along
each roller during wheel rotation. Dickerson shows that frictionless Mecanum
wheels achieve half the traction of regular wheels [6], as half of the net ground
force generated by each wheel acts along the direction of intentional slip and can
therefore do no work. Systems using Mecanum wheels therefore require greater

2
While roller angles other than 45° or 90° can also be used, no examples of wheels of any
alternative angles could be found, and no name appears to be ascribed such wheels.

9
Chapter 2 Wheeled Robot Locomotion

drive torques to achieve the same body accelerations as a similar system with
regular wheels. This is exacerbated by the discontinuous ground contact point
during wheel rotation, as each transition from one roller to the next provides an
opportunity for the wheel to slip.
Any deviation of a Mecanum wheel’s rotation axis from parallelism with the
ground results in a non-circular projection, meaning they are best operated on
hard flat surfaces3 , and suspended such that their rotation axis always remains
parallel to the ground. Whilst the degree of imperfection in ground flatness
that can be tolerated is wheel, application, and specification specific, in general
any variation in wheel geometry or ground surface would manifest as a vertical
vibration and a variation in wheel diameter, preventing smooth rolling. Omni-
directional wheels must be used in sets of three or more, appropriately oriented
to define a unique inverse kinematic mapping. Mecanum wheels are often used
in sets in which roller angles of both π/4 and −π/4 are used, allowing the defi-
nition of a unique inverse kinematic mapping whilst keeping all wheel rotation
axes parallel. Provided a unique kinematic mapping and thus full controllability,
any omnidirectional acceleration can be instantaneously achieved by appropriate
actuation of the wheels, without requiring any prior steering to align with the
desired direction of motion. This allows for omnidirectional motion using only
three actuators, compared to a steered system with regular wheels that requires
at least four actuators and a delay whilst its wheels are steered to their new
required heading. For solutions using any kind of omnidirectional wheel, care
must be taken to ensure that all of the roller rotation axes do not intersect at a
single point, as this allows for uncontrolled rotation about the intersection point.

2.1.2 Stability and System Footprint

In the context of this section consider the stability of a robot to be a measure

of its ability to handle disturbances that act to perturb its attitude relative
to the vertical, without this causing a reduction in controllability, i.e. without
non-redundant wheels losing contact with the ground. In typical vehicles this
stability is attained statically by the possession of three or more ground contact
points, forming the vertices of a polygon beneath the vehicle. Assuming an
initially stationary system the vehicle will always return to its single statically
3
as opposed to rough terrain such as that encountered outdoors

10
 2.1 Related Concepts

stable equilibrium from any attitude under the force of gravity alone4 , provided
a vertical line drawn through its center of mass (CoM) intersects this ground
contact polygon. This means tall systems with small footprints are more easily
toppled than short, wide systems, and so there exists a minimum ratio of CoM
height to minimum footprint dimension for a system to possess a given degree of
disturbance rejection in all directions. If such a system is to operate on sloped
surfaces or carry objects at a horizontal offset from the system’s center of mass
as in Figure 2.2 this ratio must be increased further, as both of these tasks act
to move the system’s CoM towards the edge of its footprint, reducing the margin
that is left to ensure stability under disturbance. This upper bounds the degree
of slope angle and mass of payload that can be tolerated without falling, even in
the absence of disturbance. Similarly, this approach to attaining stability limits
the maximum accelerations that can be generated by the system changing its
direction of motion, requiring tall systems to always move at a sufficiently slow
speed if they are required to quickly react to dynamic obstacles without falling.
Static stability can be increased by using more than three wheels to increase the
minimum footprint width, though this comes at a cost of requiring the inclusion
of suspension if all wheels are to maintain simultaneous ground contact. This,
along with inevitable flex in the robot’s structure present even in three-wheeled
systems, introduces further coupled dynamics into the system. This is evident
when a tall statically stable robot drives quickly over an imperfect surface, as the
top of the robot tends to shift and oscillate due to excitation by perturbations
from the ground. While static stability can actually be achieved using only two
wheels in a differential drive configuration with a CoM below the wheel axle, this
requires impractically large wheel diameters for robots of any significant height,
and so are not a viable locomotion solution for human form factored indoor
robotics.
Dynamically stable systems attain stability by a degeneration of this ground
contact polygon into a line or point, achieved either by a collinear arrangement
of contact points, or by reducing the number of contact points to two or one.
Dynamically stable systems instead use active actuation to apply forces to the
ground that allow them to dynamically balance about what is now an unstable
4
Though depending on the exact shape of the footprint the CoM may still overshoot the
opposite side of the footprint provided insufficient damping, so this is a necessary but not
sufficient condition for static stability.

11
Chapter 2 Wheeled Robot Locomotion

Tipping moment Tipping moment

(a) (b)

Figure 2.2: A demonstration of how statically stable systems become less stable
when carrying off-axis loads or operating on sloped surfaces.

upright equilibrium. Such systems are often referred to as mobile inverted pen-
dulums, with a common example of this being two-wheeled inverted pendulums
(TWIPs) such as the Segway5 . This elimination of a footprint dimension allows
the robot to be designed to be arbitrarily thin in this dimension, allowing the cre-
ation of robots with the same height and resilience to disturbance as a statically
stable robot, whilst possessing a much thinner form factor and subsequently more
lightweight construction. As a dynamically balancing system is able to control
the position of its CoM relative to its contact point(s) it can adapt to changes
in its CoM position due to the carrying of additional loads, without the same
associated reduction in stability that is experienced in statically stable systems.
Similarly, as a dynamically stable system has a one or two dimensional ground
contact polygon it can climb ramped surfaces without an associated change in its
attitude, provided in the 2D case that the ground contact line is kept sufficiently
perpendicular to the direction of slope. These two operations are exampled in
Figure 2.2.
As a dynamically balanced robot must be sensitive to changes in its attitude
in order to balance, intentional attitude perturbations can be used as a user
input from which to control the robot. For example, a balancing robot that is
controlled to regulate its velocities to zero can be guided to a new location by
a gentle continuous push from a user. This is potentially a more natural way of
5
Though technically a Segway requires a rider to become dynamically balanced, as the system
alone has a CoM below its wheel axle and is therefore statically stable.

12
 2.1 Related Concepts

positioning or teaching a robot, as opposed to a statically stable robot that would

require a joystick or strategically placed force sensors to achieve a similar effect.
This concept is well demonstrated by Nagarajan [7], in which a ball-balancing
robot is taught desired movement trajectories by guidance from a human using
a single finger.

2.1.3 The Ideal Indoor Mobile Robot

From Chapter 1 it is known that the optimal mobile indoor robot form factor and
locomotion characteristics are similar to that attained by a human. This requires
a larger ratio of height to footprint size than attainable using statically stable
locomotion methods, necessitating the use of a dynamically balanced solution.
This is to be achieved whilst also possessing omnidirectional manoeuvrability,
allowing this reduced footprint to be used to achieve improved navigation of
confined environments, better position regulation during manipulation and en-
vironment interaction tasks, and increased gracefulness of motion. To maintain
low complexity and cost this is to be achieved using a wheeled solution, rather
than a bipedal legged approach.

2.1.4 Dynamics of Mechanical Systems

The dynamics of an open-chain of rigid bodies can be expressed in the form

M (q)q̈ + C(q, q̇)q̇ + G(q) = Qf (2.1.2)

where q ∈ Rn denotes a vector of generalised coordinates, Qf ∈ Rn a vector of

forces or torques acting on the generalised coordinates, M (q) ∈ Rn×n a symmetric
positive-definite mass matrix, C(q, q̇) a matrix containing Coriolis and centripetal
force terms, and G(q) a vector capturing forces acting on the generalised coordi-
nates due to the system’s pose relative to gravity. These equations are linear in
q̈, quadratic in q̇, and trigonometric in q, and are not unique for a given system,
allowing some derivation methods to yield ‘simpler’ or more convenient models
than others.
Three commonly used methods exist for the derivation of these equations for
a given system: the classical Newton-Euler and Lagrangian formulations, and
the more recent Kane’s method. While these three methods all yield equivalent

13
Chapter 2 Wheeled Robot Locomotion

models, some models may contain fewer terms or fewer uses of transcendental
functions. Comparisons of these methods typically show Kane’s method yields
the most numerically simple models, shortly followed by the Lagrangian method,
and with the Newton-Euler method yielding a significant increase in complex-
ity [8, 9]. The results of these studies, however, ignore the potential benefits of
compiler-level optimisation, and so may not be representative of real-time imple-
mentations.

The incorporation of nonholonomic constraints into this derivation is challeng-

ing for the Newton-Euler formulation, requiring the explicit calculation of the as-
sociated constraint forces, whereas for the Lagrangian and Kane’s methods more
straightforward approaches exist [10, 11]. In this thesis the Lagrangian method
is used, as this features more commonly in the literature than Kane’s method,
whilst allowing for the systematic incorporation of nonholonomic constraints.

The Lagrangian is defined as the difference of the system’s total kinetic energy
K(q, q̇) and potential energy U(q) as

L(q, q̇) = K(q, q̇) − U(q) (2.1.3)

from which the equations of motions can be expressed as

d ∂L ∂L
M (q)q̈ + C(q, q̇)q̇ + G(q) = − = Qf (2.1.4)
dt ∂ q̇ ∂q

Nonholonomic constraints can be incorporated by the use of the Lagrange

equations in the form

d ∂L ∂L
− = Qf + A(q)T λ (2.1.5)
dt ∂ q̇ ∂q

where A(q)T denotes the matrix defining the k Pfaffian constraints in (2.1.1),
and λ ∈ Rk a vector of Lagrange multipliers.

These multipliers can be eliminated by multiplication with the transpose of

a matrix Φ(q) representing the null space of A(q), such that ΦT (q)AT (q) = 0,
allowing the derivation of the new dynamic model

M (p)p̈ + C(p, ṗ)ṗ + G(p) = B(p)τ (2.1.6)

14
 2.1 Related Concepts

defined in a new minimal coordinate vector p, where now p ∈ Rn−k , and q̇ = Φṗ
[10].

2.1.5 Underactuated Mechanical Systems

Underactuated mechanical systems are rigid body systems of the form

M (q)q̈ + C(q, q̇)q̇ = B(q)u (2.1.7)

where m = rank(B(q)), n = dim(q), and m < n, i.e. systems which have fewer
independent control inputs than degrees of freedom [12]. This means the in-
put u cannot be used to independently generate forces on all q, and the system
cannot be exactly linearised by feedback. Assuming by suitable choice of in-
put and coordinates a system can be described in the form (2.1.7) in which
B(q) = Im , 0n−m , the first m equations can be referred to as the fully ac-
 T

tuated subsystem, and the latter n − m equations can be referred to as the

unactuated subsystem. Spong showed that the fully actuated subsystem can al-
ways be linearised by a suitable change of coordinates and control [13], termed
partial feedback linearisation. However, the unactuated system remains nonlin-
ear and coupled to the new control, still necessitating the use of nonlinear control
techniques, albeit now to control a system of dimension n − m rather than n.
The stabilisation of such systems has been well studied [14, 15]. Olfati-Saber
introduces methods for finding explicit changes of coordinates and control that
transform a large range of underactuated mechanical systems into one of three
cascade normal forms [16], along with a classification system for determining
the required normal form based on some model properties. Generalised control
techniques are then presented for these normal forms.
Balancing systems such as the inverted pendulum are classic examples of un-
deractuated mechanical systems, in which the base of the pendulum must be
actuated to translate the pendulum to some new position, whilst also regulating
the pendulum’s lean angle as to come to rest at the upright unstable equilibrium
and remaining bounded to some neighbourhood of the upright position. Un-
deractuated balancing systems have unstable zero dynamics, so are also referred
to as non-minimum phase systems. In practice, such systems have states that
when tracking a step response must first move in a direction away from that of

15
Chapter 2 Wheeled Robot Locomotion

the step, with the position state of the base of an inverted pendulum forming
a classic example. These systems cannot track arbitrary trajectories in config-
uration space, as their dynamics must evolve according to one or more second
order nonholonomic constraints. This is in contrast to holonomic systems, which
can track arbitrary trajectories in configuration space, greatly simplifying their
control and trajectory planning. A number of techniques for the approximation
of a desired trajectory by a trajectory that satisfies dynamic constraints and can
thus be exactly tracked using an inversion of the system dynamics have been
shown [17–19]. This non-minimum phase property limits the bandwidth of any
controller, and prevents perfect tracking of dynamically feasible trajectories in the
presence of disturbance. This greatly limits the control performance achievable
for such systems.

2.1.6 Control of Dynamically Stable Mobile Robots

Feedback control is required to asymptotically drive a robot’s states towards some

time-varying reference trajectories by suitable actuation of its inputs. Depending
on the application, these reference trajectories may be C ∞ smooth, or they may
be discontinuous. This is to be achieved in the presence of measurement noise,
external disturbance, unmodelled dynamics, trajectory dynamic infeasibility, and
model parameter uncertainty. The degree of trajectory dynamic infeasibility
that must be tolerated by the controller varies depending on the properties of
the overarching trajectory planner; some planners may yield fully dynamically
feasible trajectories, whereas a human user may demand step changes in reference.
This control may have to be performed whilst enforcing constraints on wheel
torques so as to avoid wheel slip, and whilst also enforcing constraints on system
states to maintain safety and/or stability, such as limiting the lean angle of the
pendulum body and translational velocities.
This control problem also varies depending on the type of state references
that are to be tracked. In this thesis two distinct use cases are considered: the
control of local frame body velocities, with the system’s Cartesian positions states
allowed to grow unboundedly, and the control of global inertial frame positions.
A local frame body velocity controller for a dynamically balancing mobile robot
aims to iteratively generate input trajectories such that the system’s local frame
body velocities converge to a time varying reference trajectory, whilst bounding

16
 2.1 Related Concepts

deviations of the system’s lean angle from the unstable equilibrium and safely
handling actuator saturation. This is useful in applications where a human user
is expected to generate the reference velocity trajectory, such as when used as a
personal vehicle or teleoperated platform. The integration of local frame body
velocity trajectories to inertial frame position trajectories is easily performed by
the human user rather than the controller, providing a more intuitive input to
the user and simplifying control design.
An inertial frame position controller instead aims to iteratively generate input
trajectories to yield asymptotic tracking of a time-varying Cartesian position
reference, allowing the robot to be commanded to move to a particular location
in an obstacle-free environment. This must be performed while enforcing lean
angle and actuator constraints as above, but must also enforce constraints on
system velocities so as to generate a safe evolution toward the target position.
This form of controller is required for autonomous navigation tasks, such as
moving through a series of waypoints.

2.1.7 Trajectory Planning

Trajectory planning is the process of calculating system state and input trajec-
tories that yield a desired transition from an initial to terminal state, potentially
passing through a number of intermediate states. This allows entire complex
state trajectories to be planned at once, as opposed to achieving such a tran-
sition using just closed-loop control, in which the resulting time evolution of
the system states are instead defined by the dynamics of the closed-loop system.
Planning entire trajectories at once allows the minimisation of some cost function
along the entire trajectory, whilst potentially also enforcing constraints on system
states and inputs. The planning of trajectories is typically a more computation-
ally intensive process than the calculation of a controller feedback, meaning the
resulting trajectories can only respond to large disturbances, so small deviations
from a planned trajectory must be corrected by a low-level tracking controller
instead.
For some systems the planning of trajectories can be broken down into two
parts: planning a geometric path q(s), s ∈ [0, 1] from an initial configuration
q(0) to a new configuration q(1), and defining a smooth monotonic timing law
s(t), ṡ(t) ≥ 0 that describes the rate at which the system is to progress along the

17
Chapter 2 Wheeled Robot Locomotion

path by the mapping of s to t ∈ [0, T ], s : [0, T ] → [0, 1], see [10]. This splitting
of trajectory planning allows velocity constraint satisfaction by uniformly slowing
the trajectory through a linear scaling of the timing law. It is also possible to
plan paths directly in the time domain to define trajectories q(t), though this
prevents the same linear time scaling of trajectories. For holonomic systems it is
sufficient for q(s) to possess bounded first order derivatives to represent dynam-
ically feasible configuration trajectories, whereas for systems with nonholonomic
kinematic constraints q(s) must also have bounded second order derivatives to
avoid instantaneous changes in direction. For systems with second-order non-
holonomic constraints the resulting time domain trajectories q(s(t)) or q(t) must
satisfy these dynamic constraints in order to represent feasible configuration tra-
jectories. Depending on the system this could require bounded derivatives up to
much higher orders, tightly coupling the planning of the path and timing laws,
meaning these are often derived simultaneously. These differing requirements ne-
cessitate the use of significantly different planning techniques depending on the
kinematic and dynamic constraints acting on the system.

For some systems the mapping of Cartesian and heading trajectories to the
dynamically feasible state space trajectories required for the exact tracking of a
sufficiently smooth Cartesian path can be performed using the concept of dif-
ferential flatness [20, 21]. This is a property exhibited by a number of common
mobile systems, including holonomic vehicles [10] and quadrotors [22,23]. A non-
linear input-affine system ẋ = f (x) + G(x)u is differentially flat if there exists a
set of outputs y = h(x) such that all system states x and inputs u can be defined
algebraically in terms of the flat outputs and their time derivatives up to a finite
order r. For the systems listed above the Cartesian position and heading states
are found to be flat outputs, allowing dynamically feasible state trajectories to be
calculated by an algebraic function of trajectories in the Cartesian position and
heading states, provided they are continuously differentiable up to the necessary
order r. The required degree of smoothness r is entirely dependent on dynamic
model structure, with r = 2 for holonomic systems and r = 4 for quadrotors.

For some systems this differential flatness property can also exist partially [24].
This technique partitions the system state vector as x = xTs xTr , in which the
 T

xs states and input can be algebraically defined in terms of some flat outputs,
allowing trajectories in these states to be algebraically derived from a sufficiently

18
 2.1 Related Concepts

smooth output trajectory. The remaining xr state trajectories must then be de-
rived by numerical integration of the system dynamics from initial conditions,
though this integration now only need be performed on a lower dimensional sys-
tem. Nonlinear trajectory optimisation techniques can then be used to optimise
trajectories in the flat outputs to yield some optimal evolution of both the xs
and xr states, potentially a less complex problem than optimising trajectories in
the system’s inputs. As the correct choice of output to yield partial or full differ-
ential flatness is not always obvious for some systems, methods for numerically
searching for these outputs have been demonstrated [25].

The complexity of the path planning problem varies greatly depending on the
inclusion or exclusion of obstacles, and the presence of any other state or input
constraints. A number of methods exist for planning dynamically unconstrained
paths in the presence of obstacles, such as RRT-Connect [26], A* [27], PRM [28],
and artificial potential fields [29]. Methods also exist for the planning of trajecto-
ries in the presence of obstacles that are subject to kinematic and dynamic con-
straints, such as kinodynamic RRT* [30] and non-convex nonlinear optimisation
techniques such as various shooting and transcription methods [31], all forming
searches of higher-dimensional spaces. However, through this high-dimensionality
and the potential presence of non-convex constraints due to obstacles, these prob-
lems can be hard to solve in real-time for real-world navigation tasks, and are
strongly susceptible to local minima. The planning of dynamically feasible tra-
jectories through 2D or 3D occupancy grids is therefore usually approached by
splitting the problem into two parts: first finding the shortest continuous path to
a goal through the occupancy grid using a 2D or 3D search, and then convexifying
the obstacle constraint set along this path or sampling the path into an appropri-
ate set of waypoints and performing a second optimisation to find dynamically
feasible trajectories through this constraint tube or set of waypoints.

As the 2D path planning part of this process is relatively system agnostic,

this thesis instead focuses on the latter part of this problem, the task of finding
dynamically feasible state trajectories that pass through two or more waypoints
in configuration space, whilst also satisfying state and input constraints.

19
Chapter 2 Wheeled Robot Locomotion

2.2 Existing Solutions

Existing wheeled locomotion solutions can be grouped into four categories based
on their manoeuvrability and stability: nonholonomic statically stable systems,
holonomic systems, nonholonomic dynamically stable systems, and omnidirec-
tional dynamically stable systems. These are described in this section, and a
review is performed of methods commonly used in their control and trajectory
planning.

2.2.1 Nonholonomic Statically Stable Systems

This class includes a variety of configurations of fixed and steered standard wheels
that allow for motion subject to nonholonomic kinematic constraints, with the car
being a classic example of this class. Research into the control of these systems
is typically concerned with finding solutions to the parallel parking problem in
mobile robotics, as again from Brockett’s theorem a system with nonholonomic
kinematic constraints cannot be stabilised to a given equilibrium by smooth time
invariant feedback [2,32]. Furthermore, as nonholonomic statically stable mobile
robots are not subject to any constraints on their dynamics, the paths of their
trajectories can be planned independently of the path timing law, allowing the
separation and simplification of these tasks. The bulk of this research therefore
has limited relevance to the system explored in this thesis, and that which is rele-
vant is also applicable to omnidirectional statically stable systems; these relevant
topics are therefore reviewed in the next section.

2.2.2 Omnidirectional Statically Stable Systems

Three or more omnidirectional wheels located on the vertices of a polygon can

be used to achieve static stability and omnidirectional motion. A number of
omnidirectional statically stable robots are commercially available, exampled in
Figure 2.3, which typically either use a triangular arrangement of omniwheels
with intersecting rotation axes, or a rectangular arrangement of Mecanum wheels
with parallel rotation axes and alternating handedness. These systems have
been well studied in the literature, with the main topics of interest being that
of planning trajectories to navigate environments in the presence of obstacles,
and the creation of controllers for the asymptotic tracking of these trajectories.

20
 2.2 Existing Solutions

As through their inherent static stability these systems are not subject to any
second-order nonholonomic constraints, and as through the use of omnidirectional
wheels also do not possess kinematic constraints, these systems are said to be
holonomic, reflecting the lack of nonholonomic constraints of any order acting on
the system. In practice, holonomic systems are able to instantaneously generate
sustained acceleration in any direction, yielding a maximally controllable system.
Through this lack of kinematic constraints these systems can be asymptotically
stabilised in configuration space by a linear feedback of the form u = −Kx, with
suitable gain K, allowing for simple and high-bandwidth control.
Kinematic and dynamics modelling of holonomic systems is straightforward
[10, 33–35], and various approaches to modelling and identifying friction effects
in the omnidirectional wheels used in these systems have been demonstrated [36].
Han introduces a calibration method to account for dead reckoning error intro-
duced by roller slippage, bearing play, and friction [37], showing a 50% improve-
ment in dead reckoning accuracy during lateral translation. Purwin models and
analyses the effects of weight transfer on traction during acceleration [38].
Trajectory planning for holonomic systems is an easier task than for systems
with kinematic or dynamic constraints, as arbitrary continuous paths can be fol-
lowed, requiring no bounded derivatives. As with kinematically nonholonomic
systems, arbitrary smooth and monotonic timing laws may be chosen, again al-
lowing the separation of these two planning tasks. This path planning is typically
achieved by defining paths using low order splines [39, 40], upon which heuristic
based timing laws can be defined to satisfy acceleration and velocity constraints.
This has been extended to shape-aware planning around obstacles for robots of
complex 2D geometries [41].

2.2.3 Dynamically Stable Systems with Nonholonomic

Kinematic Constraints

The most common configuration of robot that dynamically balances whilst be-
ing subject to nonholonomic kinematic constraints is the two-wheeled inverted
pendulum (TWIP). This configuration utilises two differential drive wheels at-
tached to the robot body, with a center of mass located above the wheel axle,
dynamically balancing about the wheel rotation axis. Just as in a statically stable
differential drive robot, the full configuration space is accessed by combinations

21
Chapter 2 Wheeled Robot Locomotion

(a) A KUKA YouBot, an omnidirec- (b) Pepper [42], a small omnidirectional

tional robot utilising four Mecanum humanoid robot utilising three omni-
wheels. wheels.

Figure 2.3: Typical omnidirectional statically stable mobile robots, also referred
to as holonomic robots.

of forward motion, controlled by manipulation of the system’s lean angle, and

rotation about a vertical axis running through the center of a line drawn be-
tween the two wheels. The TWIP is very well studied in the literature, and is
closely related to the standard inverted pendulum, a classic testbed system for
control development. A number of review articles [43, 44] and books [45] com-
pare different modelling and control methods for both the regular and mobile
inverted pendulum. Modelling is typically performed from first principles by the
Newton-Euler [46–49], Euler-Lagrange [45, 50, 51], or Kane’s methods [50, 52, 53],
all of which yield equivalent models. A number of authors only consider one-
dimensional models that ignore yaw dynamics, or include yaw dynamics but
ignore cross-coupling effects by using two independent models. These modelling
approaches are not suitable for the coupled system considered in this thesis, as
these would likely fail to capture significant cross-coupled dynamics. Black-box
model estimation approaches have also been applied [54], though without demon-
strating any improvement in model accuracy over modelling from first principles.
Controllability of TWIPs has been proven and their maximum relative degree has
been derived [45, 51]. Partial feedback linearisation of the TWIP has been per-
formed, with the TWIP’s forward velocity and position states forming unstable

22
 2.2 Existing Solutions

(a) EPFL’s JOE, an early mobile in- (b) Boston Dynamic’s Handle, a two-
verted pendulum [46]. wheeled inverted pendulum robot for
package handling [67].

Figure 2.4: Notable two-wheeled mobile inverted pendulums

zero dynamics [45, 51, 55, 56].

Being a classic control test bed, a huge variety of control approaches have
been applied to the TWIP, such as linear state space control [46, 57], sliding
mode control [58], model predictive control [59–61], and a variety of model-free
approaches [62–65]. Traction modelling for TWIPs has been studied, along with
the development of a traction-aware model predictive controller [66].
As with statically stable nonholonomic systems, part of the related literature
is concerned with the stabilisation of the TWIP to some Cartesian position and
pose in the presence of nonholonomic kinematic constraints, and the planning of
kinematically feasible paths. Again this is a substantially different task for these
systems compared to that studied in this thesis, so this portion of the TWIP
literature is of limited relevance and is not reviewed.

2.2.4 Omnidirectional Dynamically Stable Systems

Omnidirectional dynamically balancing systems currently rely on just two dis-

tinct wheel configurations: ball-drive actuators, and omniwheels with actuated
rollers. Both of these systems balance in two dimensions simultaneously, and
represent the most similar class of mobile robot to that explored in this thesis.

23
Chapter 2 Wheeled Robot Locomotion

Ball-balancing robots are single-wheeled robots that dynamically balance on

top of a ball [68]. Two notable ball-balancing research platforms exist in the
control literature: ETH Zurich’s Rezero [69], and Carnegie Mellon University’s
Ballbot [70], both shown in Figure 2.5. Multiple methods exist for actuation
of the ball. The most elegant solution contacts the ball with three individually
actuated omnidirectional wheels [69], allowing the robot to apply a net torque
to the ball about any arbitrary axis. A similar drive mechanism has been cre-
ated using rollers with linear slip [71], though no advantage over omniwheels is
shown. Alternatively a mouse-ball configuration can be used, in which two hori-
zontal rollers press on the sides of the ball to control its pitch and roll, with yaw
control achieved by a revolute joint in the body [70]. Promising results have been
shown by a spherical induction motor, achieving actuation of the ball using no
actuated moving parts other than the ball [72]. By manipulation of the ball such
a robot is able to control its attitude, which in turn allows it to control its trans-
lational acceleration. Such a robot is therefore able to navigate an environment
whilst remaining balanced about the upright unstable equilibrium. However, in
possessing only a single ground contact point the maximum yaw torque that can
be applied to the ball is restricted to that which can be achieved without the ball
twisting against the ground as static friction is overcome. This limits the ability
of ball-balancing robots to interact with their environment or resist disturbance
in this dimension. Despite a number of efforts no ball-balancing robot has ever
been successfully commercialised, due to vibration issues caused by the use of
omniwheels on the non-flat ball surface, and effects similar to static friction due
to deformation of the ball under the roller and restraint contact points.

Decoupled planar models of a ball-balancing robot have been developed using

the Newtonian [73] and Euler-Lagrange [69, 70, 74] methods, and fully coupled
dynamics modelling of ball-balancing robots has been performed using the New-
tonian [75, 76] and Euler-Lagrange [69, 77] methods. Full state estimation based
on inertial and odometry data has been demonstrated using an extended Kalman
Filter [78], achieving more than sufficient performance and dead reckoning accu-
racy for the evaluation of different control approaches.

Closed-loop trajectory tracking control of ball-balancing robots has been per-

formed using gain scheduling to provide a nonlinear controller with a wide op-
erating region [69]. Differential flatness [20, 21] and polynomial optimisation

24
 2.2 Existing Solutions

(a) CMU’s Ballbot [70] (b) ETH Zurich’s Rezero [69]

Figure 2.5: Notable ball-balancing robots

(a) OmnUR robot from UCLA’s (b) Honda’s UNI-CUB personal mobil-
RoMeLa [79] ity device [80]

Figure 2.6: Omnidirectional balancing systems utilising omniwheels with actu-

ated rollers

25
Chapter 2 Wheeled Robot Locomotion

techniques have been used to plan dynamically feasible trajectories through mul-
tiple fixed state waypoints [73, 81] using a decoupled 2D dynamics model. This
optimisation can be performed very quickly, planning long trajectories in < 1 ms,
but does require some model simplification in order to achieve differential flat-
ness, making planned trajectories less dynamically feasible with deviation from
the vertical. This model simplification can be avoided by utilizing partial dif-
ferential flatness, in which a sufficiently smooth parametrised trajectory in the
fully differentially flat outputs is optimised to yield a desired trajectory in the
non-differentially flat states [24]. This method is able to plan very aggressive
trajectories that remain dynamically feasible for arbitrary lean angles, however,
the trajectories of non-differentially flat states must still be determined by in-
tegration of the system dynamics from initial conditions, so this remains a slow
and computationally expensive method. Nagarajan [74] plans trajectories in con-
figuration space by describing trajectories in the lean angle states of the robot in
each planar axis using a series of parametrised hyperbolic secant functions, with
parameters optimised subject to the system’s planar dynamic constraints. These
trajectories are then tracked using a zero moment point (ZMP) technique and
PID controller. Nagarajan [82] also generates dynamically feasible trajectories
through state waypoints by performing a nonlinear optimisation subject to the
nonholonomic dynamic constraint imposed by the system’s underactuation to
optimise a linear map between some sufficiently smooth desired trajectory and a
subset of the system dynamics, performed using a full 3D coupled model. This
optimisation takes in the region of 1 s for typical trajectories, and can therefore
feasibly be used for some real-time planning applications. Pardo implements
direct transcription to generate optimal trajectories in the presence of simple ob-
stacles using nonlinear programming [83]. While producing optimal trajectories,
this technique is too computationally expensive to be implemented in real-time
for complex trajectories or multiple obstacles. Neunert [84] demonstrates ef-
fective unconstrained optimal model predictive control for planning trajectories
through multiple temporally fixed waypoints. This is performed by minimising
a quadratic input and state error cost function over a receding horizon by opti-
misation of a linear time-varying feedback and feedforward controller, using the
full nonlinear coupled model for prediction. This optimisation can be performed
very quickly, taking 20 ms to plan trajectories of 4 s duration, and with solve time

26
 2.2 Existing Solutions

scaling linearly with trajectory length.

A handful of balancing robots utilising a single omniwheel with actively driven
rollers have been demonstrated, exampled in Figure 2.6. These omniwheels re-
quire either complex mechanical design to transfer torque from a single central
actuator to the rollers, or incorporate an individual actuator within every roller.
By possessing a single contact point these systems suffer from the same lack of
significant yaw control as ball-balancing robots. This is addressed in Figure 2.6b
by the inclusion of a second omniwheel with passive rollers. Limited research
has been undertaken into the control of and trajectory planning for this wheel
configuration.

27
Chapter 3

The Collinear Mecanum Drive

This chapter introduces the Collinear Mecanum Drive (CMD), the locomotion
system that is to be studied in this thesis. It begins with the derivation of the
kinematic and dynamic models of the CMD, and then analyses the underactu-
ation properties of the dynamics model, its controllability, and the size of the
maximum feedback linearisable subsystem. An analysis of parameter selection is
then performed to provide insight into the optimal CMD design.

The Collinear Mecanum Drive (CMD) utilizes three or more collinear Mecanum
wheels to enable omnidirectional locomotion, whilst simultaneously dynamically
balancing about the wheel rotation axis. By dynamically balancing, the ground
footprint of a robot utilising a CMD can be designed to be arbitrarily thin, only
lower bounded by choice of wheel diameter. The omnidirectional manoeuvrability
of the CMD in combination with this narrow footprint allows for the navigation
of much smaller gaps between obstacles than existing omnidirectional locomo-
tion methods, achieved by translating directly along the common wheel rotation
axis. This is in contrast to two-wheeled inverted pendulum systems, which would
have to instead perform a parallel-parking style manoeuvre to achieve the same
translation. Being a dynamically balanced system, the height of the center of
mass of a robot driven by a CMD can be increased without requiring a pro-
portional increase in the width of the ground footprint so as to avoid toppling
during acceleration or disturbance, as would be the case for existing statically
stable omnidirectional locomotion methods.
Omnidirectional dynamically balanced motion has previously only been demon-
strated using ball-balancing robots, which possess a number of disadvantages

29
Chapter 3 The Collinear Mecanum Drive

compared to the CMD. By only balancing about a single axis, as opposed to the
two dimensional balancing required in a ball-balancing robot, the CMD retains
greater control authority over the statically stable subsystem, and less energy is
required to maintain balance. The CMD also retains significant control author-
ity over its rotational dynamics about the vertical by virtue of multiple ground
contact points, allowing for greater control performance and environment inter-
action.
This new locomotion system allows for the creation of omnidirectional systems
of the same height as existing statically stable omnidirectional platforms, whilst
requiring a fraction of the ground footprint and overall system size, and with
a much smaller minimum navigable gap. This enables the creation of tall and
slender robots that are better able to navigate cluttered environments such as
those encountered in personal robotics in the home, retail, and office sectors.
This is achieved with a fraction of the complexity of existing ball-balancing or
legged solutions, requiring only three actuators and three moving parts1 , allowing
for increased reliability and reduced unit cost.
Only a single example of a CMD existed in the literature prior to this research,
in which a three wheeled CMD demonstrated the possibility of omnidirectional
movement whilst simultaneously dynamically balancing [85]. However, this CMD
demonstrates poor performance, taking 60 s to perform a 10 cm translation along
its wheel axis, and no attempt is made to model or analyse the system dynamics.

3.1 Kinematic Model

In order to derive the inverse kinematics and dynamics models of the proposed
platform, the nonholonomic constraints imposed by the Mecanum wheels must
first be derived.
Consider the proposed CMD platform depicted in Figure 3.1 on a flat plane,
where {E, êx , êy , êz } denotes the fixed inertial reference frame. The body attached
frame {B, b̂x , b̂y , b̂z } is obtained by a rotation of E about eˆz by φ, followed by a
translation of xêx + yêy , with B located on the wheel rotation axis in the center
of the platform. The pendulum attached frame {P, p̂x , p̂y , p̂z } is obtained by a
translation of B by hp along b̂z , followed by a rotation of θp about b̂x , where hp
1
This is excluding the unactuated Mecanum wheel rollers, as compared to a typical moving
part within a robot these are very simple and low cost.

30
Chapter 3 The Collinear Mecanum Drive

be massless and inertialess for sake of model simplicity.

Considering a single Mecanum wheel, let µ̂p represent the unit vector running
parallel to r̂i through the ground contact point, expressed in the local body
attached frame, let W represent the wheel’s centre, and let the roller contact
the ground directly under W at C as C = W − rw b̂z , where rw denotes the
wheel radius measured to the roller contact point and perpendicular to the wheel
rotation axis.

For no slip to occur, the component of the roller’s velocity at the contact point
along µ̂p must always be zero, so

~vEC,B · µ̂p = 0 (3.1.1)

in which ~vEC,B represents the velocity of C relative to E expressed in the local

body frame B, and where · denotes the dot product.

~vEC,B can be expressed as the body frame velocity of the wheel at W relative
to E summed with the tangental velocity due to wheel angular velocity θ̇i as

~vEC,B = ~vEW,B − rw b̂y θ̇i (3.1.2)

Similarly, ~vEW,B can be defined in terms of the body frame velocity of B relative
to E as
~vEW,B = ~vEB,B + φ̇li b̂y (3.1.3)

Finally, ~vEB,B can be expressed in the inertial frame as

T
~vEB,B = REB ~vEB,E (3.1.4)

Combining (3.1.1)-(3.1.4) and splitting ~vEB,E into its components along êx and
êy , denoted x and y, yields the nonholonomic no-slip constraint

ẋ cos(αi − φ) − ẏ sin(αi − φ) − φ̇li sin(αi ) + θ̇i rw sin(αi ) = 0 (3.1.5)

Similarly, the angular velocity of the roller Ω̇i is proportional to its velocity along
the vector µ̂t , where µ̂t is perpendicular to µ̂p and parallel to the ground, so

~vEC · µ̂t = rr Ω̇i (3.1.6)

32
 3.2 Dynamics Model

which by substitution with (3.1.2)-(3.1.3) yields the nonholonomic rolling con-

straint

ẋ sin(αi − φ) + ẏ cos(αi − φ) + φ̇li cos(αi ) − θ̇i rw cos(αi ) = Ω̇i rr (3.1.7)

Equation (3.1.5) can be applied to wheels 1 through nw and rewritten in matrix

form to define the platform’s inverse kinematic mapping f −1 : (ẋ, ẏ, φ̇) → θ̇i
 
  ẋ
− cos(αi − φ) sin(αi − φ) li  
θ̇i = rw  ẏ  for i = [1 . . nw ] (3.1.8)
rw sin(αi ) rw sin(αi )
φ̇

Remark 1. Minimum wheel quantity

As the row vector on the left of (3.1.8) is clearly of rank 1 and dimension 3,
a minimum of three wheels, with (αi , li ) chosen so that the rows of the matrix
composed by stacking the row vectors in (3.1.8) are independent, are required
to create a unique forward kinematic mapping f : θ̇ → (ẋ, ẏ, φ̇) where θ =
θ1 . . θnw , nw ≥ 3.
 T

3.2 Dynamics Model

Here the generalised CMD dynamics model is derived using the Lagrangian
method, chosen for its systematic incorporation of nonholonomic constraints and
straightforward derivation. This is derived both in terms of the generalised co-
ordinates and their derivatives, and in terms of generalised positions and local
body frame velocities, a more useful formulation for control development later in
this thesis.
There exist two methods of deriving a dynamics model subject to these non-
holonomic constraints using the Lagrangian method; Lagrange multipliers can be
used to directly incorporate the nonholonomic constraints, or the constraints can
be approximately ’holonomised’ using the psuedo-inverse of the inverse kinematic
transformation matrix. Zimmerman showed both methods to be equivalent in the
context of Mecanum wheeled vehicles [86]. Here the former approach is taken.
The system’s dynamics equations are derived by use of the Euler-Lagrange
equation in terms of the Lagrangian L(q, q̇), generalised coordinates q, generalised

33
Chapter 3 The Collinear Mecanum Drive

forces Q, Lagrange multipliers λ, and Pfaffian constraint matrix AT (q), defined

as  
d ∂L ∂L
− = Q + AT (q)λ (3.2.1)
dt ∂ q̇ ∂q
where A(q) follows the Pfaffian constraint form AT (q)q̇ = 0.

The generalised coordinates q are selected as

h iT
q = x y φ θp θ1 . . . θnw Ω1 . . . Ωnw (3.2.2)

and from (3.1.5) and (3.1.7) AT (q) is defined as


cos(α1 − φ) − sin(α1 − φ) −l1 sin(α1 ) 0
.
.. .
.. .
.. ..
.




cos(αnw − φ) − sin(αnw − φ) −lnw sin(αnw ) 0
AT (q) = 
 sin(α − φ)
1 cos(α1 − φ) l1 sin(α1 ) 0
. . . ..

.. .. ..


 .
sin(αnw − φ) cos(αnw − φ) lnw sin(αnw ) 0

rw sin(α1 ) ... 0 0 ... 0
.. .. .. .. .. .. 
. . . . . . 

0 ... rw sin(αnw ) 0 ... 0  (3.2.3)
−rw cos(α1 ) . . . 0 rr . . . 0
.. .. .. .. . . .. 

. . . . . .
0 . . . −rw cos(αnw ) 0 ... rr

The Lagrangian L(q, q̇) is found as the difference of kinetic and potential energy
in the system L(q, q̇) = K(q, q̇) − U(q), where K(q, q̇) represents the sum of
translational and rotational kinetic energy, and U(q) the total potential energy.
The rotational kinetic energy of the system is defined as the sum of rotational
energy of the pendulum mass and four wheel masses. As wheel torques act about
the b̂x axis, pendulum inertia Ip must be redefined about P−hp p̂z as Ip,b , achieved
h iT
using the parallel axis theorem with translation vector rp = 0 0 −hp as

Ip,b = Ip + mp [(rp · rp ) I3×3 − rp ⊗ rp ] (3.2.4)

34
 3.2 Dynamics Model

where ⊗ denotes the outer product. The wheel rotation axes ŵx,i are already
aligned with τi , so Iw remains unchanged.

This allows rotational kinetic energy Kr to be defined as

wn
1 T 1X
Kr (q̇) = ω~ p Ip,b ω
~p + ω
~wT
I ω
~
i w wi
(3.2.5)
2 2
i=1

where

ω
~ b = φ̇b̂z (3.2.6)
ω T
~ p = Rbp ω
~ b + θ̇p p̂x (3.2.7)
ω T
~ w,i = Rbw ω
~ + θ̇i ŵx
i b
(3.2.8)

Similarly, translational kinetic energy is defined as the sum of that of the

pendulum and four wheel masses as

w n
1 1
(3.2.9)
X
Kt (q̇) = ~vpT mp~vp + mw T
~vw,i ~vw,i
2 2
i=1

where
h iT
T T
~vp = Rbp Reb ẋ ẏ 0 + ω ~ p × hp̂z (3.2.10)
h iT
T
~vw,i = Rbw i
T
Reb ẋ ẏ 0 + ω~ wi × li ŵx (3.2.11)

Finally, potential energy is purely that due to the action of gravity on the pen-
dulum body, defined as
U = mp ghp cos(θp ) (3.2.12)

The generalised forces Q capture all non-conservative forces acting on the system,
which here are motor torques Qτ and both rolling and viscous friction forces
Qf for Q = Qτ + Qf . The nw motor drive torques τ = τ1 . . . τnw act
 T

individually on each wheel, with each also producing an opposing counter-torque

on the pendulum body. No motor torques act directly on the x, y, φ, or Ωi
generalised coordinates; the interactions between these and the motor torques
are instead captured by the nonholonomic constraints (3.1.5) and (3.1.7). This

35
Chapter 3 The Collinear Mecanum Drive

defines Qτ as  
03×1
Pnw 
 (−τi )
Qτ = i=1 (3.2.13)
 

τ
 
 
0nw ×1

Viscous friction is modelled at two interfaces for this system: at the body-
to-wheel revolute joints, with coefficient kvw , and at the wheel-to-roller revolute
joints, with coefficient kvr . It is assumed that kvw is also able to approximate the
various motor phenomena that sum to yield a non-zero no-load current. Linear
rolling friction is modelled at the roller-to-ground interface as a torque about ŵx
proportional to wheel angular velocity θ̇i , with coefficient krw . While there will
also exist a rolling friction force acting along b̂x , there does not exist a simple
experimental approach to allow the independent measurement of this coefficient
and kvr , so it is assumed that this can be sufficiently captured by the existing
kvr coefficient. Tractive friction forces between the roller and ground are already
assumed to be infinite in the definition of the nonholonomic constraints in (3.1.5)
and (3.1.7). It is assumed that kinetic friction in the wheel bearings can be fully
compensated by application of a discontinuous torque offset to the wheel actu-
ators, allowing its exclusion from the dynamics model, and it is assumed that
static friction is negligible for model simplicity. Kinetic friction in the roller bear-
ings cannot be compensated in such a manner, and cannot easily be modelled
without introducing a discontinuity, so is therefore treated as a external distur-
bance. Again, static friction in this interface is also assumed to be negligible for
model simplicity.
Viscous friction in the wheel-to body-revolute joint acts proportionally to the
difference between each wheel angular velocity θ̇i and the pendulum’s angular
velocity θ̇p , applying a torque of kvw (θ̇p − θ̇i ) to each θi generalised coordinate
nw
and a torque of (kvw (θ̇i − θ̇p )) to the pendulum body θp . Viscous friction in
P
i=1
the wheel-to-roller revolute joint acts proportionally to −Ω̇i , applying a torque
of −kvr Ω̇i to each Ωi generalised coordinate. The counter-torque from this fric-
tion force acts about two axes on the wheel. That about the ŵx axis acts to
rotate the wheel, applying a torque of kvr b̂x · Rbr Ω̇i 0 0 to each of θi . That
 T

orthogonal to ŵx and parallel to the ground imparts an axial load on the wheel,

36
 3.2 Dynamics Model

which is transmitted through the wheel mounting to directly apply a force on

the pendulum body along the b̂x axis. This is equivalent to a force acting on the
[x y]T generalised coordinates of

1
   
h i −rw +rr nw kvr Ω̇i
(3.2.14)
X
I2×2 02×1 Reb  0  b̂y · Rbr,i  0 
   

0 i=1 0

Rolling friction acting about bx is proportional to wheel angular velocity θ̇i .

This defines Qv as
  1   
−rw +rr k vr Ω̇ i
h i Pnw 
 I2×2 02×1 Reb  0  b̂y · i=1 Rbr,i  0 
 
  
0 0
 
 
 

 0 

 Pnw 

 (kvw (θ̇i − θ̇p )) 

 i=1 h iT 
(3.2.15)
 
Qf =  −θ̇ 1 k rw + k vw (θ p − θ̇ 1 ) + k vr b̂x · Rwr Ω̇ 1 0 0 
..
 
.
 
 
 
 h iT 
 −θ̇ k + k (θ − θ̇ ) + k b̂ · R
wr Ω̇nw 0 0

 nw rw vw p nw vr x 
 
 −kvr Ω̇1 
.
 
..
 
 
 
−kvr Ω̇nw

Introducing 2nw Lagrange multipliers λ = λ1 λ2 . . . λ2nw allows the so-

 T

lution of (3.2), giving a system of 4 + 2nw ODEs. These can be arranged into
the matrix form

M (q)q̈ + C(q, q̇)q̇ + G(q) = AT (q)λ + F q̇ + Bτ (3.2.16)

with symmetric positive semidefinite2 inertia matrix M (q), Coriolis and cen-
tripetal matrix C(q, q̇), derived using the Christoffel symbols of M (q) such that

2
Usually for Lagrangian systems M (q) is positive definite, however, in choosing to model the
wheel roller as being massless and inertialess eigenvalues of zero are introduced into M (q).

37
Chapter 3 The Collinear Mecanum Drive

Ṁ (q) − 2C(q, q̇) is skew symmetric as

4+2n 
1 Xw ∂Mij (q) ∂Mik (q) ∂Mjk (q)

ci,j = + + q̇k (3.2.17)
2 ∂ q̇k ∂ q̇j ∂ q̇i
k=1

and with gravity matrix G(q), viscous and rolling friction matrix F , and input
matrix B. These matrices are defined in full in Appendix A.1.
Provided the conditions set out in Remark 1 are met, examining rank(A) = 2nw
indicates that 2nw of the model’s 4+2nw degrees of freedom are fully constrained
by A, meaning 2nw generalised coordinates can be made redundant by elimination
of the Lagrange multipliers. Defining the nullspace of A as Φ, such that AΦ = 0
and therefore ΦT AT = 0, it is evident that λ can be eliminated from (3.2.16) by
premultiplication with ΦT .
As the choice of Φ must satisfy AT q̇ = 0, there exists a minimal vector of
generalised velocities ṗ that map back to q̇ as q̇ = Φṗ. As there are infinite
solutions for Φ and therefore choices of ṗ, it is possible to choose Φ such that
the rows mapping ṗ to ẋ ẏ φ̇ θ̇p in q̇ form the rows of an identity matrix,
 T

allowing Φ to provide a mapping from the full generalised coordinates vector q̇

to a convenient minimal generalised coordinate vector ṗ = ẋ ẏ φ̇ θ̇p , giving
 T

Φ as  
1 0 0 0
 

 0 1 0 0 
0 0 1 0
 

 

 0 0 0 1 
 − cos(α1 +φ) − sin(α1 +φ)
− rlw1 0

 rw sin(α1 ) rw sin(α1 )
.. .. .. .. 
 
. . . .

Φ= (3.2.18)


 
 − cos(αnw +φ) − sin(αnw +φ)
− lrnww 0

 rw sin(αn ) rw sin(αnw )
 w 
 2 cos(φ) cos(α1 ) 2 sin(φ) cos(α1 ) 

 rr rr 0 0 
.. .. .. .. 
 
. . . .


 
 2 cos(φ) cos(αnw ) 2 sin(φ) cos(αnw ) 
rr rr 0 0

Multiplying by ΦT , substituting q̇ = Φṗ and q̈ = Φp̈ + Φ̇ṗ, and recognising

that all matrices that depend on q in (3.2.16) depend only upon elements that

38
 3.2 Dynamics Model

also exist within p, allows (3.2.16) to be rewritten in the reduced generalised

coordinates p as

ΦT M (q)Φp̈ + ΦT M (q)Φ̇ṗ + ΦT C(q, q̇)Φṗ + ΦT G(q) = ΦT F Φṗ + ΦT Bτi (3.2.19)

which can be written as the new set of model matrices

Mp (p) = ΦT M (q)Φ (3.2.20)

Cp (p, ṗ) = ΦT M (q)Φ̇ + ΦT C(q, q̇)Φ (3.2.21)
Gp (p) = ΦT G(q) (3.2.22)
T
Fp (p) = Φ F Φ (3.2.23)
T
Bp (p) = Φ B (3.2.24)

in which Mp (p) is now both symmetric and positive definite, and Ṁp (p)−2Cp (p, ṗ)
remains skew symmetric. These matrices are defined in full in Appendix A.2.
As det(Mp (p)) =
6 0 ∀ p ∈ R4 for sensical parameter choices Mp (p) is invertible,
allowing (3.2.19) to be solved for p̈ as

p̈ = Mp (p)−1 (Fp (p)ṗ + Bp (p)τ − Cp (p, ṗ)ṗ − Gp (p)) (3.2.25)

thus allowing integration of the system dynamics over time from an initial state
(p0 , ṗ0 ) and with input trajectory u(t).
As rank(Bp ) < dim(τ ) when nw > 3, as in the experimental prototype in this
thesis, the input τ does not represent a linearly independent set of inputs. This
means there exists a linear map Λ : τ → u that maps τ onto a new minimal set
of independent inputs u, written in matrix form as u = Λτ , in which there exist
infinite choices for u. Separating Bp (p) into its nonlinear and linear components

Bp (p) = Bp,nl (p)Bp,l (3.2.26)

. . . − cotrαwnw
 cot α
− cotrwα2

− 1
" #  rw
1
 − rw − r1w − r1w

Reb (p) 02×2  ...
(3.2.27)

= 
02×2 I2×2  l
 − rw1 − rlw2 ... − lrnww


−1 −1 ... −1

and defining B̂p,l as a basis for the column space of Bp,l , one suitable map can

39
Chapter 3 The Collinear Mecanum Drive

be found as Λ = B̂p,l
+
Bp,l , where
 
1 0 0  
  cot α1 cot α2 . . . cot αnw
0 1 0
B̂p,l =  Λ=− 1 
rw  1 1 ... 1  (3.2.28)

0 0 1

l1 l2 ... lnw

0 rw 0

giving the overall input matrix Bp (p) = Bp,nl (p)B̂p,l Λτ . Replacing Bp (p) in
3.2.19 with B̂p (p) = Bp,nl (p)B̂p,l and using u = Λτ as the new input yields the
new system
Mp (p)p̈ + Cp (p, ṗ)ṗ + Gp (p) = Fp (p)ṗ + B̂p (p)u (3.2.29)

in which dim(u) = rank(B̂p ). Intuitively, the elements of this new input represent
force on the body parallel to êx , force on the body parallel to êy , and torque on
the body about êz .
A known input u can be mapped back to a choice of τ in which is
Pnw 2
i=1 τi
minimised as τ = Λ+ u. If wheel torques are to be constrained this can be enforced
by the solution of the constrained least-squares minimisation

min kτ k22 s.t. Λτ = u, −τ ≤ τi ≤ τ ∀ i ∈ [1 . . nw ] (3.2.30)

τ

solvable as a quadratic program for feasible combinations of u and τ .

In some cases it is more useful to express (3.2.19) in terms of inertial frame
positions, but local body frame velocities and accelerations. This can be achieved
by defining (3.2.18) such that the first two rows of Φ equal
h i
Φ1:2 = Reb 02×2 (3.2.31)

to give the new set of velocity states v = Φ−1

v q̇, where v = vx vy φ̇ θ̇p . This
 T

therefore also defines the kinematic model

" #
Reb 02×2
ṗ = Ψv, Ψ= (3.2.32)
02×2 I2×2

which can be differentiated to allow the substitution

p̈ = Ψv̇ + Ψ̇v (3.2.33)

40
 3.2 Dynamics Model

This allows the definition of the two models

Mv (p)v̇ + Cv (p, v)v + Gv (p) = Fv v + Bv τ (3.2.34)

and
Mv (p)v̇ + Cv (p, v)v + Gv (p) = Fv v + B̂v u (3.2.35)

in which again Mv (p) is symmetric positive definite, Ṁv (p) − 2Cv (p, v) is skew
symmetric, and matrices Fv , Bv , and B̂v no longer depend on p. Again, det(Mp )
6 0 ∀ p ∈ R4 for sensical parameter choices. These matrices are defined in full
=
in Appendix A.3.
As in (3.2.25), the model (3.2.34) can be solved for v̇ as

v̇ = Mv (p)−1 (Fv v + Bv τ − Cv (p, v)v − Gv (p)) (3.2.36)

It is this model which is to be used for simulation of CMD throughout the rest
of this thesis, with numerically integration performed using MATLAB’s ode45,
an explicit Runge-Kutta (4,5) solver [87], with absolute and relative tolerances
of 1 × 10−9 .

3.2.1 Underactuation of Dynamics

As m = rank(B̂v ) = 3, n = dim(p) = 4, m < n, the system is underactuated,
meaning there are more configuration variables than independent control inputs
and therefore no general inverse of B̂p or B̂v exists. From [16] underactuated
systems can be analysed by splitting their configurations into external variables
and shape variables, where shape variables ps are defined as those appearing
in the inertia matrix M (p), and external variables px are defined as those not
appearing in M (p). This means that external variables do not appear in the
kinetic energy expression K(p, v) = 12 v T M (p)v, so ∂K(p,v)
∂px = 0. If this is the case
the Lagrangian is said to possess kinetic symmetry w.r.t. the external variables.
From a seminal work by Saber [16] underactuated systems can be classified based
the properties of the shape variables. The system (3.2.29) possesses the following
properties:

1. The shape and external variables are ps = (φ, θp ) and px = (x, y) respec-
tively.

41
Chapter 3 The Collinear Mecanum Drive

2. Both the shape and external variables are actuated, in that all rows of B
are nonzero, meaning the shape and external variables are coupled.

3. Partitioning Mp (p) into shape and external variables as

" #
mxx (p) mxs (p)
Mp (p) = (3.2.37)
msx (p) mss (p)

the normalised momentums conjugate to px take the form

πx = ṗx + m−1xx (ps )mxs (ps )ṗs (3.2.38)

  
ẋ − a cos(φ)φ̇ + b cos(φ) sin(θp )φ̇ + cos(θp ) sin(φ)θ̇p
=   (3.2.39)
ẏ − a sin(φ)φ̇ + b sin(φ) sin(θp )φ̇ − cos(θp ) cos(φ)θ̇p

with known positive constants a and b, which clearly by being globally

smooth are integrable functions.

However, the normalised momentums conjugate to ps

πs = ṗx + m−1
sx (ps )mss (ps )ṗs (3.2.40)

which for brevities sake are not shown, are not integrable due to the exis-
tence of singularities, that is @ h(px , ps ) : ḣ = πs .

4. There are equal numbers of shape and external variables, so dim(ps ) =

dim(px ).

5. As rank(mxs (p)) = n − m ∀ {p ∈ R | θp 6= π/2} the system is said to be

strongly locally inertially coupled [12].

Surprisingly, these properties make this system unclassified by the system set out
by Saber [16], as this author states never to have found a system with this com-
bination of having actuated shape variables, coupled inputs, and nonintegrable
momentums, therefore this combination is not considered nor analysed. This
means the literature at present provides no expected normal form for which it
should be possible for the system to be rearranged into.

42
 3.2 Dynamics Model

3.2.2 Controllability

The controllability of a system describes its ability to move from any initial point
in its state space x0 ∈ Rn to any other point xT ∈ Rn within finite time T < ∞
by manipulation of its inputs u ∈ Rm . The global controllability of linear systems
in the form ẋ = Ax + Bu, x ∈ Rn , u ∈ Rm is easily proven by determining if the
Kalman controllability matrix Co is of full rank, i.e. rank(Co ) = n, where
h i
Co = B AB . . . An−1 B (3.2.41)

Linear systems satisfying this condition can always be globally stabilised to the
origin by a feedback of the form u = −Kx.
Such a proof does not exist for nonlinear systems. A weaker form of this proof is
to instead show that a nonlinear system is small-time locally controllable (STLC),
and a further weaker form is to show that a nonlinear system is small-time locally
accessible (STLA).
Letting W represent an infinitely small region in state space centered around
x0 , i.e. the local neighbourhood of x0 , define RW as the set of configurations xT
that can be achieved by manipulation of u in an infinitely small time T without
leaving W . A STLC system will be able to use sequences of control input to
affect change in x0 in all directions in W , meaning x0 will be an interior point
within RW , p0 ∈ int(RW ), and therefore RW = W .
A STLA system, whilst still able to locally access a space with the same di-
mension as W , is restricted to accessing a subset RW ⊂ W , in which p0 is on the
boundary of RW and so p0 ∈
/ int(RW ) [88].

Theorem 1. The CMD is STLC from its equilibrium states for sensical model
parameters.

The set of equilibrium states Xe is defined as the set of states x with constant
input u = 0m×1 at which ẋ = 0n×1 , given for the state space x = p v as
 T

Xe = {x | ẋ = f (x, u) = 0, u = 0}
nh iT
= x1 x2 . . . x8 | (x1 , x2 , x3 ) ∈ R3 ,
o (3.2.42)
x4 = πk, k ∈ Z, xj = 0, j = [5 . . 8]

43
Chapter 3 The Collinear Mecanum Drive

As 0n×1 ∈ Xe , (3.2.35) can be linearised about the stationary upright equilib-

rium at the origin, yielding a system ẋ = Ax + Bu, where A and B take the
form    
04×4 I4×4 04×3
0 0 0 0 a 0 0 0  i 0 0
   

(3.2.43)
   
A = 0 0 0 b 0 c 0 d  B = 
 
0 j 0
0 0 0 0 0 0 e 0  0 0 k
   

0 0 0 f 0 g 0 h 0 l 0
in which A possesses some positive eigenvalues, meaning the upright equilibrium
is unstable. Likewise, linearising about any x ∈ {Xe : x4 = π} yields a negative
semidefinite A, meaning the lowest pendulum position is a stable equilibrium, as
would be expected.
Examining the Kalman controllability rank condition yields rank(Co ) = 8 = n,
indicating controllability of the linearised model at the equilibrium states.
Given the complexity of A and B for the fully parametrised model it is not
possible to directly examine their structure to ascertain the effect of parameter
choice on controllability, so the effect of exceptional parameter values is examined
instead. Physically impossible parameter choices, such as rw = 0, l1 = l2 etc. are
not considered. The conditions on αi and li set out in Remark 1 must be met for
the model to remain defined. Setting hp = 0 yields rank(Co ) = n − 1 and thus a
reduction in controllability, meaning the pendulum CoM must not lie inline with
the wheel axis.
A nonlinear system that is controllable when linearised at its equilibrium states
is STLC from the equilibrium states for the full nonlinear system [89, p. 74-75],
meaning the CMD is STLC for x ∈ Xe given sensical parameter choices.
For comparison a two-wheeled inverted pendulum moving on a 2D plane yields
rank(Co ) = 6, as the nonholonomic constraints imposed by the use of regular
wheels prevent translation parallel to the wheel axis. A TWIP on a 2D plane
therefore does not satisfy the KCRC, and is therefore not STLC, though a number
of authors claim the TWIP to be STLC by analysis of the TWIP’s model in
joint space [90], which ignores a dimension of the configuration space required to
uniquely locate the TWIP on a 2D plane.

Theorem 2. The CMD is STLA ∀ x ∈ R8 .

44
 3.2 Dynamics Model

Arranging (3.2.35) in the nonlinear input-affine form

3
(3.2.44)
X
ẋ = f (x) + gj (x)uj
j=1

where x = p v , the drift vector field f (x) and input vector fields gj (x) take
 T

the form  
x5 cos(x3 ) − x6 sin(x3 )
 
x5 sin(x3 ) + x6 cos(x3 )
 

 x7 

 
x8
(3.2.45)
 
f (x) = 
 
 f5 (x4 , x5 , x6 , x7 , x8 ) 

 
 f6 (x4 , x5 , x6 , x7 , x8 ) 
 
 f (x , x , x , x , x ) 
 7 4 5 6 7 8 
f8 (x4 , x5 , x6 , x7 , x8 )
 
04×3
 T 
g1 (x) g51 (x4 ) 0 g53 (x4 )


(3.2.46)
 
g2 (x) =  0 g (x ) 0
 
62 4

 
g3 (x) g71 (x4 ) 0 g73 (x4 )
 

0 g82 (x4 ) 0

in which g53 (x4 ) ≡ g71 (x4 ).

The distribution spanned by the vector fields f and gj , j = [1 . . 3] is defined

as ∆ = span{f, g1 , g2 , g3 }, or in bracket notation ∆ = hf, g1 , g2 , g3 i, in which
∆ is nonsingular, as assuming sensical parameters dim(∆) = 4 ∀ x ∈ R8 . The
accessibility algebra A is defined as the involutive closure of ∆, written as ∆ =
∆A . A distribution is involutive if [f, g] ∈ ∆ ∀ (f, g) ∈ ∆, where [ , ] denotes
the Lie bracket operator, defined as

∂f2 ∂f1
[f1 , f2 ](x) = f1 (x) − f2 (x)
∂x ∂x

The involutive closure of a distribution ∆ can be calculated as the distribution

spanned by all possible combinations of Lie brackets calculable from its vector

45
Chapter 3 The Collinear Mecanum Drive

fields, which can be derived iteratively as

∆1 = ∆, ∆i = h{∆i−1 , {[X, Y ] | X ∈ ∆1 , Y ∈ ∆i−1 }}i , i≥2 (3.2.47)

This procedure terminates when ∆i+1 = ∆i = ∆A , with the terminal value of

i required to define this distribution referred to as the nonholonomy degree of
the system, with an upper bound of i ≤ n − m [10].
For the system (3.2.44), clearly dim(∆1 ) = 4. ∆2 is calculable as

∆2 = h{∆1 , [∆1 , ∆1 ]}i (3.2.48)

which in knowing [f, f ] = 0, [G, G] = 0 ∀ gj ∈ G, where G = {g1 , g2 , g3 }, can be

simplified to

∆2 = h{∆1 , [f, G]}i = h{f, g1 , g2 , g3 , [f, g1 ], [f, g2 ], [f, g3 ]}i (3.2.49)

yielding dim(∆2 ) = 7. ∆3 is calculable as

∆3 = h{∆2 , [∆1 , ∆2 ]}i (3.2.50)

in which a single additional Lie bracket is required to yield the distribution

D = h{f, g1 , g2 , g3 , [f, g1 ], [f, g2 ], [f, g3 ], [f, [f, g1 ]]}i (3.2.51)

that is of full rank dim(D) = n, meaning D = D, and therefore ∆3 ≡ D ≡ ∆A .

This indicates a nonholonomy degree of 3, the same as a TWIP [51, 91]. Unlike
a TWIP, it is found that dim(∆3 ) = n even for the frictionless hp = 0 case,
indicating STLA even when the pendulum mass generates no force on the body
due to gravity. This is to be expected, as the full Cartesian state space can be
accessed by combinations of rotation about b̂z and translation along b̂x , whilst
using the rotational dynamics about b̂x to purely control the θp subsystem. As
dim(∆A ) = n this proves that the CMD is STLA ∀ x ∈ Rn .

Theorem 3. The CMD is kinematically holonomic

From (3.2.32) the kinematic model of a CMD can be expressed as a sum of

46
 3.2 Dynamics Model

vector fields as
4
(3.2.52)
X
ṗ = Ψv = gj (p)v
j=1

As the accessibility distribution formed by these vector fields ∆A = hΨi is found

to have full rank dim(∆A ) = dim(p), the individually nonholonomic constraints
(3.1.5) and (3.1.7) are together completely integrable, meaning as in conventional
statically stable Mecanum wheeled vehicles the kinematic model (3.2.32) is holo-
nomic [10, p. 477]. The CMD is therefore a kinematically holonomic system.

3.2.3 The Largest Feedback Linearisable Subsystem

Using the adjoint representation of the Lie bracket [f, g] = adf g, successive Lie
brackets of the vector fields f and g up to j iterations can be defined as

adjf g = adf (adj−1

f g), e.g. ad2f g = [f, [f, g]] (3.2.53)

Following the notation of [92]

G = {g1 , g2 , g3 }
Gf = f + G = {f + g : g ∈ G}
n o (3.2.54)
adjf ∆ = adjf X : X ∈ ∆

[∆1 , ∆2 ] = {[X, Y ] : X ∈ ∆1 , Y ∈ ∆2 }

define the distributions

Qi−1 , adif Q0 (3.2.55)


Q0 = hg1 , g2 , g3 i, Qi = i≥1

where Qi denotes the involutive closure of Qi .

Again, it is clear from the structure of (3.2.46) that {[g1 , g2 ], [g1 , g3 ], [g2 , g3 ]} =
0, so Q0 is involutive and therefore Q0 = Q0 . Q1 is calculated as

Q0 , adf Q0 (3.2.56)

Q1 = = h{g1 , g2 , g3 , [f, g1 ], [f, g2 ], [f, g3 ]}i

with involutive closure

Q1 = h{g1 , g2 , g3 , [f, g1 ], [f, g2 ], [f, g3 ], [g1 , [f, g1 ]], [[f, g1 ], [f, g3 ]]}i (3.2.57)

47
Chapter 3 The Collinear Mecanum Drive

which is found to be of full rank dim(Q1 ) = n, meaning Q2 must be equivalent

as Q2 ≡ Q1 .
From these distributions the following sequence of non-increasing integers are
computed [92, 93]
r0 = dim(Q0 ) (3.2.58a)

ri = dim(Qi ) − dim(Qi−1 ), i≥1 (3.2.58b)

ki∗ = card{rj ≥ i | j ≥ 0} (3.2.58c)

in which r0 = 3, r1 = 6 − 3 = 3, r2 = 8 − 8 = 0, giving controllability indices

k1∗ = 2, k2∗ = 2, k3∗ = 2, k4∗ = 0. This indicates that the largest feedback
linearisable subsystem has dimension nλ = k1∗ + k2∗ + k3∗ = 6 [92], meaning
this subsystem can be rewritten as three linear double integrators. Intuitively,
this subsystem will encompass the (φ, φ̇), (θp , θ̇p ), and ( vx , vx ) dynamics, with
R

the ( vy , vy ) dynamics therefore not linearisable by static feedback and state

R

transformation. The size of this maximum feedback linearisable subsystem is

greater than that of a TWIP, which has a maximum relative degree of 4 [51].

3.2.4 Dynamics Analysis Conclusion

To summarise this section, the dynamics of the CMD have been derived, and both
the complex nonholonomic constraints imposed by the use of Mecanum wheels
and the friction forces acting upon their components have been successfully in-
corporated into a minimal dynamics model with state vector x ∈ R8 . The CMD
has been proven to be a kinematically holonomic and therefore omnidirectional
system, which is underactuated and unstable about the upright equilibrium. It
has been proven to be fully controllable from its equilibrium states, and can fully
access its state space, providing the strongest possible proof of controllability for
a nonlinear system. Finally, in proving that the CMD has a relative degree of 6,
there exists the possibility of achieving significant model simplification through
feedback linearisation, potentially only requiring the use of nonlinear control
techniques to stabilise two states, with the remaining six states controllable us-
ing linear controllers. It has also been shown that a CMD of nw > 3 can always
be simplified to a system of input dimension m = 3 by simple linear algebra,
simplifying the control of many-wheeled CMDs. The MATLAB script used to
perform these derivations and analyses is included in Appendix B.

48
 3.3 Collinear Mecanum Drive Design Considerations

3.3 Collinear Mecanum Drive Design Considerations

This section aims to explore how the selection of a number of CMD parameters
affects the overall CMD dynamics and properties, with the aim of drawing some
conclusions as to help guide good CMD design practices.

3.3.1 Wheel Quantity

From Remark 1 it is known that a minimum of three wheels are required to

ensure controllability of the CMD. However, this may not be the optimal number
of wheels for all applications. One drawback of using only three wheels is a
resulting lack of redundancy; loss of traction of a single wheel results in the loss
of system controllability. Loss of traction is more likely to occur in a CMD than
a conventional wheeled vehicle, as the generation of a net force along b̂x or b̂y
requires a component of the body force generated by each wheel to be cancelled
by an equal and opposite force from the other wheels. This is clear in analysis of
Λ in (3.2.28), as in order to generate a net force along b̂y equally signed torques
must be applied to each wheel whilst ensuring ni=1 cot(αi )τi = 0, meaning the
P w

force components along b̂x do no work. Similarly, generating a net force along
b̂x requires wheels of opposing sgn(α) to be rotated against one another whilst
ensuring ni=1 τi = 0, meaning force components along b̂y due to wheel torques
P w

do no work. While doing no work, these forces still require the existence of
an equal and opposite tractive force between the roller and ground, meaning
the CMD is more likely to experience wheel slip than a conventional wheeled
vehicle when generating the same net force on the body [5]. This is compounded
by the requirement of the Mecanum wheels to present a circular projection, as
this necessitates manufacture of the rollers using hard rubber compounds, and
prevents the inclusion of a tread pattern as in a typical vehicle tire, reducing wheel
traction. An ideal Mecanum wheel has only a single contact point, compared to
a cylindrical wheel that has a contact line.
The introduction of a fourth wheel therefore helps to prevent loss of control
when a wheel slips due to a transient change in wheel traction, for example
when slipping on debris or when moving over small gaps or bumps between floor
surfaces. However, this does not reduce incidence of loss of traction due to overly
aggressive control, meaning this must be addressed in the control design.

49
Chapter 3 The Collinear Mecanum Drive

3.3.2 Choice and Ordering of Roller Angles

While only Mecanum wheels (α = ±π/4) are currently available off-the-shelf3 ,

these roller angles may not necessarily be the optimal choice for use in a CMD.
Multiple arguments could be made as to the definition of this optimality; this may
be the sets of roller angles that allow each wheel to generate the same contribution
to body accelerations v̇x and v̇y for a given input torque, or the optimal wheel
configuration may be that which yields the most evenly distributed accessible
acceleration space, or even a distribution most suited to a particular application.
This optimality could also be considered in terms of translational efficiency, with
the optimal configuration being that which minimises average dissipative losses
for translations in every direction.
In this section these three definitions of optimality are examined for both
the three and four-wheeled cases. Only two wheel configurations need to be
considered for each; for the three-wheeled case these are α = [a, −a, a] and α =
[a, a, −a], as the configurations α = [−a, a, a] and α = [−a, a, −a] are identical
to the former following a rotation, and for the four wheeled case these are α =
[a, −a, −a, a] and α = [a, −a, a, −a], with a similar argument for the other two
combinations. Combinations with three wheels of one sign of a and one of the
other are viewed as highly unlikely to exhibit desirable properties, and so are not
considered further.

3.3.2.1 Torque Distribution

One definition of the optimal roller ordering could be that which yields equal
contributions to body accelerations v̇x and v̇y from each wheel torque. This can
be determined by analysing the first and second rows of the matrix

Mv (p)−1 Bv (3.3.1)

that multiplies onto τ to yield contributions to v̇x and v̇y due to a wheel input
torque in (3.2.34). As from Remark 1 nw ≥ 3 is required for controllability, the
nw = 3 case is considered first. The wheels are assumed to be evenly spaced,
3
While omniwheels (α = π/2) are also available off-the-shelf, they cannot be used in a CMD
arrangement as cot(π/2) = 0, and thus no quantity of wheels are able to generate a unique
inverse kinematics mapping.

50
 3.3 Collinear Mecanum Drive Design Considerations

v̇x,1 v̇x,2 v̇x,3 v̇x,1 v̇x,2 v̇x,3

20 20
δ v̇x (m s−2 )

δ v̇x (m s−2 )
0 0

−20 −20
0 π/4 π/2 0 π/4 π/2

a (rad) a (rad)
(a) α = [a, −a, a] (b) α = [a, a, −a]

Figure 3.2: Contribution of each wheel torque τi to v̇x for the nw = 3 case in
(3.3.1) over a ∈ [0, π/2] with nw = 3, where α takes the form α = [a, −a, a] (a)
and α = [a, a, −a] (b). All others parameters are taken from Table 4.2.

placing the middle wheel directly under the CoM, i.e. l2 = 0 and l3 = −l1 , and
roller angles are set to α = [a, −a, a], for which the first row of (3.3.1) is found
to take the form

T
− sin(a) γ + Iwx rw cos(a)(2mp h2p − mp rw hp + 2Ipbx )
1
 sin(a) γ + Iwx rw cos(a)(4mp h2p − mp rw hp + 4Ipbx )  (3.3.2)


Γ
− sin(a) γ + Iwx rw cos(a)(2mp h2p − mp rw hp + 2Ipbx )

where γ and Γ are known scalar functions. This shows that the central wheel
is able to generate greater acceleration in v̇x than the outer two wheels. The
α = [a, a, −a] case is too complex to show, but yields a very similar relationship,
with an additional imbalance between the two wheels of identical handedness.
These two cases are shown in Figure 3.2 for a ∈ [0, π/2], with all other param-
eters taken from Table 4.2. It is found that the α = [a, −a, a] case yields 3.7%
more total acceleration in v̇x than the α = [a, a, −a] case for appropriately signed
wheel torques of equal magnitude. For the α = [a, −a, a] case it is found that
the central wheel is able to generate 6.3% more v̇x acceleration than the outer
two for the same input torque. In both cases maximum acceleration is achieved
with roller angles of ±14°, a significantly shallower angle than used in Mecanum
wheels.

51
Chapter 3 The Collinear Mecanum Drive

v̇y,1 v̇y,2 v̇y,3 v̇y,1 v̇y,2 v̇y,3

−12 −12
δ v̇y (m s−2 )

δ v̇y (m s−2 )
−22 −22
0 π/4 π/2 0 π/4 π/2

a (rad) a (rad)
(a) α = [a, −a, a] (b) α = [a, a, −a]

Figure 3.3: Contribution of each wheel torque τi to v̇y in (3.3.1) over a ∈ [0, π/2]
with nw = 3, where α takes the form α = [a, −a, a] (a) and α = [a, a, −a] (b).
All others parameters are taken from Table 4.2.

Performing the same analysis for contributions of each wheel torque to v̇y
yields a similar variation between roller orderings, with the α = [a, −a, a] case
yielding two wheels with identical contributions, and with the α = [a, a, −a]
case introducing a small variation. This also shows that in both cases the single
uniquely angled wheel is again able to generate greater v̇y acceleration than the
two wheels of identical handedness.

For the nw = 4 case where the platform is assumed to be symmetrical about

the (b̂y , b̂z ) plane, i.e. l4 = −l1 , l3 = −l2 , α = [a, −a, a, −a], and assuming l1 = 2l2
as an example since wheels cannot be collocated, the top row of (3.3.1) is found
to take the form

3 cos(a)(5m l2 + 2I 2

T
− sin(a) rw w 1 pbz + 8Iwyz ) + 3Iwx l1 rw cos(a)
3 2 2


1   sin(a) rw cos(a)(5mw l1 + 2Ipbz + 8Iwyz ) + 6Iwx l1 rw cos(a)
 (3.3.3)

Γ(a)  3 2 2
− sin(a) rw cos(a)(5mw l1 + 2Ipbz + 8Iwyz ) + 6Iwx l1 rw cos(a) 

3 cos(a)(5m l2 + 2I 2


sin(a) rw w 1 pbz + 8Iwyz ) + 3Iwx l1 rw cos(a)

where Γ(a) is a large scalar function. This shows a torque bias toward the inner
two wheels when accelerating along b̂x .

Considering the same scenario but now with rotational symmetry about b̂z ,

52
 3.3 Collinear Mecanum Drive Design Considerations

v̇x,1 v̇x,2 v̇x,1 v̇x,2

v̇x,3 v̇x,4 v̇x,3 v̇x,4
20 20
δ v̇x (m s−2 )

δ v̇y (m s−2 )
0 0

−20 −20
0 π/4 π/2 0 π/4 π/2

a (rad) a (rad)
(a) α = [a, −a, a, −a] (b) α = [a, −a, −a, a]

Figure 3.4: Contribution of each wheel torque τi to v̇x in (3.3.1) over α ∈ [0, π/2]
with nw = 4, where α takes the form α = [a, −a, a, −a] (a) and α = [a, −a, −a, a]
(b). This shows uneven torque distribution in (a), and equal distribution in (b).
All others parameters are taken from Table 4.2.

such that α = [a, −a, −a, a], all elements of this row become equivalent as

rw sin(2a)
(3.3.4)
2(4Iwx − 4Iwx sin(a)2 2 sin(a)2 + 4m r 2 sin(a)2 )
+ mp rw w w

This shows that only when α takes the form α = [a, −a, −a, a] do each of the
input torques contribute equally to v̇x . This goes against the intuition that the
optimal platform configuration would be symmetrical about the (b̂y , b̂z ) plane;
the optimal platform configuration for distribution of torques when generating
acceleration v̇x instead possesses 2-fold rotational symmetry about b̂z . Obviously
this analysis would be more complex with asymmetrical wheel spacings, for which
this conclusion would likely not hold. The variation of these coefficients in each
case is shown in Figure 3.4 for the parameters in Table 4.2 over a ∈ [0, π/2].
Maximum acceleration is found to occur at roller angles of ±15.5°, a slightly
greater angle than the three-wheeled case.
Analysis of the second row of (3.3.1) that multiplies onto τ to yield the con-
tribution of wheel torques to v̇y shows this to be independent of a for the two
considered wheel orderings, so contribution of each wheel torque to v̇y is invariant
in the selection and ordering of roller angles, provided all roller angles possess

53
Chapter 3 The Collinear Mecanum Drive

the same magnitude.

3.3.2.2 Accessible Acceleration Spaces

A second approach to defining the optimal wheel arrangement is to consider

the acceleration spaces that can be achieved by each configuration in the pres-
ence of input torque constraints. The acceleration space considered here is that
achievable at the stationary upright position with |τ | ≤ τ , where τ = 0.1 N m.
This is shown in Figure 3.5 for the nw = 3 case for α = [π/4, −π/4, π/4] and
α = [π/4, π/4, −π/4]. Examining the intersection of the red (v̇x , v̇y ) plane with this
accessible acceleration set shows that a larger set of v̇x and v̇y accelerations are
achievable for the α = [π/4, −π/4, π/4] case, assuming φ̈ = 0. For both cases, this
set demonstrates a strong directional bias, and is a poor approximation of an
ideal spherical or ellipsoidal acceleration set.

In the four wheeled case, the forward dynamics mapping is underdetermined,

meaning there exists multiple combinations of input torques that yield the same
net body accelerations. Two acceleration spaces are therefore considered: that
achievable using directly constrained wheel torques, i.e. τ ∈ R4 : |τ | ≤ τ , and


a subset of this space defined by the set of accelerations for which the inverse
dynamics least-squares solution yields constraint satisfying wheel torques, defined
as
v̇ ∈ R3 : B + M (p)v̇ ≤ τ (3.3.5)


These sets are visualised in Figure 3.6, in which the inner dark space represents
the inverse dynamics least-squares solution set, and the outer lighter space the
less-efficient maximal achievable acceleration set. While the least-squares solu-
tion sets retain a similar form to the three wheeled case, the maximal accessible
acceleration spaces exhibit shapes much closer to an ideal spherical or ellipsoidal
distribution. Examining the intersection of the red (v̇x , v̇y ) plane with both ac-
celeration sets shows the α = [π/4, −π/4, −π/4, π/4] case yields greater accessible
sets of v̇x and v̇y accelerations when φ̈ = 0.

54
 3.3 Collinear Mecanum Drive Design Considerations

3.3.2.3 Translation Efficiency

The CMD’s rate of energy dissipation due to viscous and rolling friction can be
calculated from (3.2.34) as
P = −v T Fv v (3.3.6)

Similarly, the rate of energy dissipation due to motor resistive losses can be
calculated as
nw  nw  2
τi 2 (Bv+ Fv v)i

(3.3.7)
X X
P = Rw = Rw
Kt Kt
i=1 i=1

where (B + F v)i denotes the ith element of the vector Bv+ Fv v, Kt the motor torque
constant, and Rw the motor terminal resistance.
As these are the only dissipative elements in the model, the total power required
to maintain a given velocity v is therefore the sum of (3.3.6) and (3.3.7), an
expression from which it is possible to analyse the efficiency of motion for any
given wheel configuration. Figure (3.7) shows this function evaluated for a unit
body frame velocity in all directions over a range of roller angles for nw = 4
and α = [a, −a, −a, a], performed for both the motors used in the prototype in
this thesis (Kt = 0.037 N m A−1 , Rw = 0.61 Ω) and for a smaller motor (Kt =
0.01 N m A−1 , Rw = 1.2 Ω) as a comparison. A similar analysis is not performed
for the α = [a, −a, a, −a] case, as (3.3.6) and (3.3.7) are found to be relevantly
invariant in roller ordering, and so this figure would be nearly identical to the
α = [a, −a, −a, a] case. This analysis yields optimal roller angles of a = ±74°
for the motor used in the prototype in this thesis, and a = ±65° for the smaller
motor. For the larger motors used in this thesis, a roller angle of a = ±56°
allows for equally efficient motion in all directions. Interestingly, both of these
figures again show that the standard Mecanum wheel roller angle of α = ±45° is
suboptimal with respect to this metric.
For the three-wheeled case translational efficiency is found to depend heavily on
choice of roller ordering. Figure 3.8 shows a comparison between the two possible
orderings α = [a, −a, a] and α = [a, a, −a], using the same motor parameters as
that used on the prototype in this thesis. Interestingly, this shows around a
10° variation in the optimal roller angle between the two orderings, and the
most efficient choice of a for the α = [a, −a, a] configuration in Figure 3.8a
requires 45% less power to translate along b̂x than the most efficient choice of a

57
Chapter 3 The Collinear Mecanum Drive

90 3 a = ±36°
120 60 a = ±40°
2 a = ±44°
150 30 a = ±48°
1 a = ±52°
a = ±56°
a = ±60°
180 0 a = ±64°
a = ±68°
a = ±72°
a = ±76°
210 330 a = ±80°

240 300
270
(a) Kt = 0.037 N m A−1 , Rw = 0.61 Ω

90 15 a = ±36°
120 60 a = ±40°
10 a = ±44°
150 30 a = ±48°
5 a = ±52°
a = ±56°
a = ±60°
180 0 a = ±64°
a = ±68°
a = ±72°
a = ±76°
210 330 a = ±80°

240 300
270
(b) Kt = 0.01 N m A−1 , Rw = 1.2 Ω

Figure 3.7: CMD translation power requirements over body relative translation
direction and roller angle α, with nw = 4 and α = [a, −a, −a, a], using motor
parameters from that used on the prototype in this thesis (a) and a smaller
motor with Kt = 0.01 N m A−1 and Rw = 1.2 Ω (b). All other CMD parame-
ters are taken from Table 4.2. This demonstrates a strong relationship between
locomotion efficiency and both choice of roller angle and motor selection.

58
 3.3 Collinear Mecanum Drive Design Considerations

for the α = [a, a, −a] configuration in Figure 3.8b. Both configurations show a
substantial decrease in efficiency for translation along b̂x compared to b̂y , even for
the most efficient choices of a. This is in significant contrast to the four-wheeled
case with identical motors, for which there exists choices of a that yield more
efficient translation along b̂x than along b̂y .
Analysis of losses purely due to the viscous and rolling friction term (3.3.6)
finds this function to be invariant in roller ordering, and of significantly lower
magnitude than the overall losses in Figure 3.8. This function is shown in Figure
3.9. This means that the substantial change in power requirements with direction
visible in Figure 3.8 originates in the motor losses term (3.3.7), which is larger
due to the increased torque requirement imposed on the single unique wheel. The
efficiency of a three-wheeled configuration can therefore be greatly increased by
driving the single unique angled wheel using a larger motor than the other two
as to minimise resistive losses.

3.3.3 Wheel Spacing

As one would expect, maximum yaw authority is achieved by spacing the wheels
as widely as possible, with symmetry across the (b̂y , b̂z ) plane. Less obviously,
it should also be considered that the opposing forces generated by each wheel
in directions parallel to b̂y when moving along b̂x generate a net torque on the
ground between wheels of opposing handedness, with this torque proportional
to wheel spacing. On a hard surface this torque is inconsequential, but on soft,
movable, surfaces this could result in an undesirable rotation of the surface under
the platform. This torque can be minimised by reducing the distance between
wheels of opposite handedness, meaning a good configuration of four wheels is
an arrangement of two pairs of wheels with opposite handedness within each
pair, with minimal distance between the wheels within each pair. These can be
arranged such that the inner two wheels 2 and 3 share the same roller handedness,
preventing the generation of a net torque between these two wheels. As the two
pairs are to be widely spaced as to provide maximum yaw control authority, it
is these two inner wheels that can potentially generate the largest net ground
torque when vx =
6 0, which is eliminated by this choice of α = [a, −a, −a, a].
While the pairs (1,2) and (3,4) will also each generate a net ground torque, as
the wheels in each pair are tightly spaced this torque is minimised.

59
Chapter 3 The Collinear Mecanum Drive

90 8 a = ±36°
120 60 a = ±40°
6
a = ±44°
150 4 30 a = ±48°
a = ±52°
2 a = ±56°
a = ±60°
180 0 a = ±64°
a = ±68°
a = ±72°
a = ±76°
210 330

240 300
270
(a) α = [a, −a, a]

90 10 a = ±36°
120 60
8 a = ±40°
6 a = ±44°
150 30 a = ±48°
4 a = ±52°
2 a = ±56°
a = ±60°
180 0 a = ±64°
a = ±68°

210 330

240 300
270
(b) α = [a, a, −a]

Figure 3.8: CMD translation power requirements over body relative translation
direction and roller angle α, using motor parameters for the prototype, with
nw = 3, l = [d, 0, −d] for both figures, and with roller angles α = [a, −a, a] (a)
and α = [a, a, −a] (b). All other CMD parameters are taken from Table 4.2.

60
 3.3 Collinear Mecanum Drive Design Considerations

90 6 a = ±34°
120 60 a = ±38°
4 a = ±42°
150 30 a = ±46°
2 a = ±50°
a = ±54°
a = ±58°
180 0 a = ±62°
a = ±66°
a = ±70°
a = ±74°
210 330 a = ±78°
a = ±82°
240 300 a = ±86°
270 a = ±90°

Figure 3.9: CMD rate of energy dissipation due to viscous friction for nw = 3
and l = [d, 0, −d]. This choice of wheel spacings makes F invariant in d, so all
choices of d produce identical friction profiles. All other CMD parameters are
taken from Table 4.2.

3.3.4 Center of Mass Height

The effect of a varying center of mass height is studied by examining the v̇y
acceleration achieved when maintaining a constant θp , i.e. θ̇p = θ̈p = 0. As a
varying center of mass would usually also be associated with a varying Ipbx , the
pendulum body is approximated as a solid thin rod with inertia Ipbx = 31 (2h2p )mp .
This relationship for hp ∈ [0, 2]m is shown in Figure 3.10, in which all other states
and inputs remain at zero. This shows the relationship one would intuitively
expect, with higher CoMs yielding greater accelerations for the same non-zero
θp , reaching an asymptote as hp → ∞. This function shows strong linearity over
small accelerations up to around 5 m s−2 . Two considerations must be balanced
in choosing hp ; a higher CoM allows for greater acceleration for a given θp ,
allowing the system to follow Cartesian trajectories with reduced deviation from
the upright pose, though with diminishing returns for hp ' 0.3 m. However,
a higher CoM in turn requires larger translations of the base to move under
the CoM for a given nonzero θp , and a higher CoM increases the inertia of the
pendulum, reducing the ability of the system to control θ̈p directly through the

61
Chapter 3 The Collinear Mecanum Drive

20
hp =0
hp = 0.008
10 hp = 0.02
hp = 0.04

v̇y (m s−2 )
hp = 0.07
0 hp = 0.12
hp = 0.19
hp = 0.31
−10 hp = 0.5
hp = 0.8
hp = 1.3
−20
−π/2 −π/4 0 π/4 π/2

θp (rad)

Figure 3.10: v̇y over θp for varying positive hp , with wheel torques controlled
such that θ̈p = 0.

body reaction torque applied by the wheel motors. These effects result in the
pendulum base having to make greater corrective movements to maintain balance
during disturbance.

3.3.5 Overall Optimal Parameter Choice

For the nw = 3 case the analyses of torque distribution, accessible accelera-
tion spaces, and translation efficiency all point to an optimal wheel handed-
ness ordering of α = [a, −a, a]. For optimal translation efficiency a roller angle
of a ≈ ±60° should be chosen, whereas for maximum control authority over
v̇x angles of a ≈ ±14° should be chosen, a significant contradiction. For the
nw = 4 case these analyses all point to an optimal wheel handedness ordering of
α = [a, −a, −a, a], and again a contradiction is found between the optimal roller
angle in terms of translational efficiency at a ≈ 75°, and in terms of v̇x control
authority at a = ±15.5°.
While for the cases where wheels of equal roller angles are to be used it is
clear that the orderings α = [a, −a, a] and α = [a, −a, −a, a] should be observed,
the otherwise contradicting requirements listed above for different perspectives of
wheel configuration optimality mean that no clear conclusion can be drawn as to
a single or set of overall optimal wheel configurations. This analysis has, however,

62
 3.4 Conclusion

shown that motor selection can have a significant impact on overall translational
efficiency, and that there exists complex coupled interactions between the selec-
tion of each parameter and these differing views of optimality. The conclusion
to be drawn is therefore that the robot designer should perform a simultaneous
end-to-end optimisation of all CMD and motor parameters for each specific CMD
application.
As all of these conclusions are subject to modelling accuracy, a future effort
should be made to undertake a full system identification of a Mecanum wheel in
order to ensure that the linear friction models used in this thesis are of sufficient
accuracy for the analyses in this section to remain accurate. It may be that
friction in other interfaces yield significant unmodelled dynamics that would alter
these results, such as friction between the roller and its mounting during axial
roller loading.

3.4 Conclusion
This chapter has introduced the Collinear Mecanum Drive, a novel dynamically
balancing omnidirectional locomotion system. It has derived the CMD’s inverse
kinematic and dynamics models, analysed the CMD in terms of controllability
and accessibility, has analysed the CMD’s underactuation characteristics, and has
determined the size of the largest feedback linearisable subsystem. It has then
explored the effect of parameter selection on control authority, achievable accel-
erations in the presence of wheel torque constraints, and translation efficiency,
granting insight toward good CMD design practice.

63
Chapter 4

A CMD Experimental Prototype

This chapter serves to detail the reasoning and methodology behind the design and
construction of a Collinear Mecanum Drive prototype, which is to be used for the
experimental validation of the control and trajectory planning techniques developed
throughout the rest of this thesis. Where necessary, experimental methods are
described for the measurement of unknown model parameters, and an online
nonlinear full-state estimator is derived and demonstrated.

4.1 Prototype 1 - Proof of Concept

The CMD concept was imagined within the first months of this research, at which
point only a single previous CMD example existed in the literature [85], which
did not conclusively demonstrate that this invention was feasibly realisable. To
ascertain this, a basic proof-of-concept prototype was created, shown in Figure
4.1. This prototype had no suspension, and so constantly suffered from wheel
slip, used geared brushed motors which introduced a significant amount of back-
lash, could only control motor open-loop speeds rather than directly controlling
torques, and used external encoders, requiring custom mechanical coupling that
introduced measurement error.
Despite these numerous shortcomings, this prototype demonstrated signifi-
cantly better performance than the existing state-of-the-art [85], with the rudi-
mentary open-loop tracking of a square trajectory shown in Figure 4.2. Where
the system demonstrated in [85] showed very slow and indirect omnidirectional
movement, taking 60 s to move approximately 10 cm along its wheel axis, the first
prototype of this thesis was able to perform translations of 1 m in 6 s. This pro-
totype therefore more conclusively proved the real-world feasibility of the CMD

65
Chapter 4 A CMD Experimental Prototype

Figure 4.1: The proof-of-concept prototype created at the beginning of this PhD.
This did not incorporate any form of suspension, and so suffered from constant
wheel slip, used geared brushed motors with a large amount of backlash, and
used external encoders with a custom coupling, introducing measurement error.

invention, but suffered from many undesirable nonlinearities, preventing this pro-
totyping from achieving good performance or being accurately described by the
derived dynamics model.

4.2 Prototype 2 - An Ideal Research Platform

To better provide experimental validation of the modelling and control developed
in this thesis a second improved experimental prototype was created, shown in
Figure 4.3. In order to fulfil this purpose it was important that the prototype fit
the expected dynamics model as closely as possible. This presents the following
idealised specification:

• No backlash between the wheel and motor, and purely viscous motor bear-
ing friction.

• Instantaneous motor torque setpoint tracking.

• Mecanum wheels with perfectly circular profiles, and with minimal roller
bearing friction that is purely viscous.

66
 4.2 Prototype 2 - An Ideal Research Platform

Figure 4.2: The proof-of-concept prototype in Figure 4.1 attempting to track a

square trajectory whilst maintaining a constant heading. Each side of the square
took approximately 6 s to traverse.

• A fully rigid chassis so as to ensure validity of the rigid body assumption.

• Even and instantaneous weight distribution across the platform’s wheels.

• No transport delay in sensing, state estimation, or control.

• Low mass and inertia, so as to better enable highly dynamic manoeuvring.

• A sufficiently low overall mass as to minimise risk to surroundings during

experiments.

This specification led to the following component choices:

4.2.1 Wheel Selection

There is relatively limited choice of Mecanum wheels available off-the-shelf, and
their complex roller geometry and tight tolerances frustrate low-cost bespoke
manufacturing. There are also no off-the-shelf omnidirectional wheels available

67
Chapter 4 A CMD Experimental Prototype

Figure 4.3: The Collinear Mecanum Drive experimental prototype used in this
thesis. This features passive weight-distributing suspension and direct-drive
brushless motors with integrated high precision encoders, overcoming the main
actuation and sensing shortcomings of the proof-of-concept prototype. Compute
is provided by both a National Instruments myRIO for low-level control, and an
Intel NUC with an i7-8650U processor for high-level planning.

with roller angles other than α = ±45°. An existing 60 mm Mecanum wheel

is therefore chosen. This small wheel diameter was chosen to reduce torque
requirements and overall prototype size.

Minimal vibration is apparent during level rotation, and so it is assumed these

wheels are sufficiently round to fit the modelling assumption of constant wheel
radius. It would be expected that violation of this assumption would be obvious
during stationary balancing, as the vehicle would be seen to preferentially balance
at certain wheel positions, and would struggle to maintain a constant low velocity.
These effects were not noticed in practice. Up to around 1 mm of play exists
axially between each of the rollers and the wheel structure, which is assumed to
be sufficiently negligible for the purposes of this thesis. There is also a small
amount of static friction present in both the roller and wheel bearings, though
this is again assumed to be minor enough to be considered negligible.

68
 4.2 Prototype 2 - An Ideal Research Platform

Figure 4.4: A close-up image of the drive assembly of the prototype in Figure 4.3.
Visible in each of the four wheel assemblies is a brushless DC motor, attached to
a suspended mounting plate and bearing assembly, with its shaft directly coupled
to a 60 mm Mecanum wheel.

4.2.2 Wheel Configuration and Suspension

Despite requiring a minimum of only three wheels, in order to preserve symmetry

and motor torque distribution a four-wheeled configuration is chosen, arranged
in two pairs of opposite handedness. Contrary to the recommended ordering of
α discussed in Chapter 3, here the form α = [a, −a, a, −a] is chosen, purely to
maintain symmetry for aesthetic purposes.
Suspension of the wheels is required to evenly distribute the prototype’s weight.
For this thesis the system is assumed to be operating on a perfectly flat surface,
so this suspension is not required to filter out any wheel vibration due to ground
surface irregularities. This means no spring or dampener components as used
in traditional suspension are required, avoiding the introduction of suspension
dynamics into the model. Traditional vehicle suspension topologies are there-
fore disregarded in favour of a simple parallelogram weight distribution method,
shown in Figure 4.5. This allows the opposing vertical movement of each wheel in
a given pair until both make contact with the ground, whilst ensuring the wheel

69
 4.2 Prototype 2 - An Ideal Research Platform

Table 4.1: Motor parameters for that used in the prototype in Figure 4.3
Parameter Value Unit
Nominal voltage 24 V
No-load speed 6110 rev/min
Nominal speed 4860 rev/min
Nominal torque 128 mN m
Nominal current 3.21 A
Stall torque 1460 mN m
Stall current 39.5 A
Terminal resistance 0.608 Ω
Torque constant 36.9 mN m A−1
Speed constant 259 rev/min/V
Number of pole pairs 8
Encoder precision 2048 steps/rev

error. As this application requires operation around zero speed Hall effect sensors
are also required. Finally, given the collinear arrangement of wheels a low motor
length is desirable to reduce the overall size of the prototype.
This specification makes external rotor brushless motors attractive, as this
topology yields a higher torque-to-weight ratio than internal rotor motors, typical
exhibits low torque ripple due to a larger number of pole-pairs, are more efficient
at the low speeds used in this application, and often have larger diameter to
length ratios. An external rotor brushless sensored motor from Maxon Motor is
chosen, with model number 397172 and specification given in Table 4.1.

4.2.4 Physical Construction

In order to enable highly dynamic manoeuvring, to better highlight modelling
error, and to better enable near-stationary balancing, the system is designed
to minimise both the height of the center of mass hp and the pendulum body
inertia term Ipx . The use of a direct drive configuration places a relatively large
lower bound on the dimensions of the prototype in the bx dimension, so this
dimension is determined by a relatively tight packing of the wheels and motors.
External wheel bearings are incorporated due to the low radial load capacity

71
Chapter 4 A CMD Experimental Prototype

of the chosen motors, a characteristic of external rotor motors, at an expense

of a small increase in friction. An aluminium chassis construction is chosen to
maximise rigidity whilst minimising weight, ensuring validity of the rigid body
assumption used in the modelling of the system dynamics. The prototype has
bounding dimensions of a width of 337 mm, a depth of 82 mm, and a height of
233 mm.

4.2.5 Motor Control

Motor controllers are required to drive the phase currents of each of the motors to
that specified by given setpoints, along with performing electronic commutation.
This control is to be as aggressive and with as little transport delay as possible
so as to allow for maximum control bandwidth. Whilst this will come at an
expense of increased power consumption, gains can be reduced for a particular
application and desired control performance if necessary. As similar systems such
as quadcopters perform control at update rates of around 400 Hz [94], a motor
current loop update rate of at least an order of magnitude greater frequency is
required. The controller must have a rated current of at least the nominal current
of the chosen motor.
Maxon ESCON 50/5 controllers are therefore chosen, rated at 5 A. These take
an analogue reference signal with 12 bit resolution, and perform PI current control
with a loop rate of 54 kHz. Motor current response to small and large reference
signals is shown in Figure 4.6, using proportional gain Kp = 250 and integral
time constant τi = 350 µs. Whilst ideally these results would demonstrate a
worst case rise time of less than 10% of the system’s maximum control update
rate, in practice this cannot be achieved without introducing excessive noise
into the motor phase currents. As the absolute worst case rise time is close
to this target at 20% of the control period, and as the more typical smaller
setpoint change achieves a rise time of 5% of the maximum control update rate,
it is decided that this controller demonstrates sufficiently high bandwidth torque
setpoint tracking to validate the assumption of instantaneous torque setpoint,
with the knowledge that the maximum achievable system control aggressiveness
will be slightly reduced.

72
 4.2 Prototype 2 - An Ideal Research Platform

Im Iref Im Iref
0.4
2 0.2
Current (A)

Current (A)
0 0

−2 −0.2

−0.4
0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1
t (s) ·10−2 t (s) ·10−2
(a) τr = ±0.1 N m (b) τr = ±0.01 N m

Figure 4.6: Motor current setpoints Iref and tracked trajectories Im for ±0.1 N m
(a) and ±0.01 N m (b) 100 Hz square wave torque setpoints, showing a constant
130 µs transport delay within the motor controller and a 486 µs worst case rise
time.

4.2.6 Inertial Sensing

Angular rate and linear acceleration sensing is performed using an MPU9250

inertial measurement unit (IMU) from Invensense [95], with full scale ranges of
±500 ° s−1 and ±2gm s−2 . Communication is performed over SPI at 20 MHz for
minimum communication delay, with interrupt driven sampling of the gyroscope
and accelerometer at 32 kHz and 1 kHz respectively. Gyroscope measurements are
low-pass filtered using a 2nd order Butterworth IIR filter with corner frequency
fc = 80 Hz, giving a worst case group delay of 1.2 ms and −27 dB attenuation
at 1 kHz. This filtering is performed on an FPGA to downsample the gyroscope
measurements to a more computationally tractable sampling time of fs = 1 kHz,
whilst avoiding aliasing.

4.2.7 Compute

Data acquisition, state estimation, low-level control, and telemetry are performed
using a National Instruments myRIO [96]. This features an ARM A9 processor
and a field programmable gate array (FPGA), interfaced by a high speed inter-
connect. Data acquisition and filtering is performed entirely on the FPGA for
minimum jitter, with all remaining tasks performed within Labview RT, running

73
Chapter 4 A CMD Experimental Prototype

within a real-time Linux-based operating system on the ARM processor. This

is able to run a 1 kHz control loop whilst performing complex background tasks
with approximately 10% jitter. A Robot Operating System (ROS) node runs on
the ARM processor to provide a standardised protocol for communication with
other robotics oriented software. Communication with the controlling desktop
PC is performed over WiFi. A second single board computer (SBC) featuring
a 64-bit Intel i7-8650U processor is used to perform more computationally de-
manding planning and control tasks. In floating point benchmarks this processor
achieves a performanceo f 18GFLOPS. This is interfaced to the myRIO via Gi-
gabit Ethernet.
Figure 4.7 shows a graphical overview of the organisation of the prototype’s
subsystems and their interconnections, and Figure 4.8 shows a high-level control
diagram.

4.3 Motor Cogging Torque and Kinetic Friction

Compensation

Cogging torque is a no-current torque generated by interaction between the rotor

permanent magnets and stator slots. This torque causes an undriven motor under
a small load to ‘jump’ between fixed angular positions, and is undetectable from
measurement of stator current, meaning it cannot be compensated for by closed-
loop current feedback. The magnitude of this torque is independent of stator
current, meaning it results in a proportionally larger output torque error for
small torque setpoints than large. When a balancing system is exactly at the
upright equilibrium no motor torque is required to maintain this in the absence
of disturbance, and only small correction torques are required for small deviations
from this equilibrium. Output torque error due to cogging torque can therefore
have a noticeable effect on control performance during stationary balancing.
Fortunately, as cogging torque is entirely dependent on physical motor con-
struction and rotor angular position, it may be described by a time invariant
function of rotor position, which once identified allows an open-loop compensa-
tion. In practice, this function is obtained by implementing an angular position
controller with integral action to control motor absolute position, using the in-
cremental encoder with a known initial position as an absolute position feedback.

74
 4.3 Motor Cogging Torque and Kinetic Friction Compensation

Intel
NUC

Ethernet
WiFi
Desktop & ROS myRIO ARM
PC Cortex-A9

AXI Interconnect

SPI MPU9250
myRIO FPGA
IMU

A/D IO

Incremental ESCON 50/5

Encoders Controllers

Drive, Hall Effect Sensors

Wheel
Motors

Figure 4.7: A diagram showing the organisation of the prototype’s subsystems

θe

Torque τcomp ~ m , ~am , θe

Ω Sensor
Plant
Compensation Calibration

~ ~a, θe
Ω,

State
τ Control
p̃, ṽ Estimation

pr , vr

Figure 4.8: Control diagram for the Collinear Mecanum Drive prototype

75
Chapter 4 A CMD Experimental Prototype

·10−3
τc,1 (θ1 ) fτc,1 (θ1 ) τc,1
5 10 τc,2

Torque (|N m|)

τc,3
Torque (N m)

τc,4

5
0

0
0 π/2 0 20 40 60 80
θ1 (rad) θ−1 (rev−1 )
(a) (b)

Figure 4.9: Raw cogging torque τc,1 data for θ1 ∈ [0, π/2] (a, blue), the Fourier
transform of this data for all four wheels (b), and the resulting cogging torque
prediction function derived by the selection of dominant frequency components
from the FFT for τc,1 (a, red).

3.5
Velocity (rad s−1 )

2.5
θ̇
2 θ̇r
0 2 4 6 8 10 12 14 16
t (s)

Figure 4.10: Wheel angular velocity whilst running a closed-loop PI speed con-
troller, with proportional gain Kp = 1, integral time constant τi = 6 s, and con-
stant setpoint θ̇r = 3 rad s−1 , in which cogging torque compensation is enabled
until t = 8 s.

76
 4.3 Motor Cogging Torque and Kinetic Friction Compensation

The angular position setpoint is slowly ramped through θ ∈ [0, 2π], such that mo-
tor dynamics have minimal effect on the output response, and so that a steady
state is achieved for every encoder position. The resulting steady state torque
achieved is therefore that required to hold the rotor in position against the cog-
ging torque, providing a measurement at each encoder position. This is averaged
over equal numbers of forward and backward rotations to eliminate static and
kinetic friction effects, and to reduce the impact of tracking error on fitting ac-
curacy, with the resulting function for wheel 1 shown in Figure 4.9a, and in the
frequency domain for all wheels in Figure 4.9b. The FFT shows a frequency
component at 48 cycles/rev with equal magnitude for all wheels, representing six
cogging steps per electrical revolution multiplied by the eight pole pairs of the
motors. A number of other components exist at factors of this frequency with
varying amplitudes, capturing unique variations in motor construction, neces-
sitating individualised cogging torque models. The cogging torque function is
determined by selecting only the dominant frequencies from the FFT. No effort
is made to compensate for friction at this point, as this is dependent on direc-
tion of rotation and wheel loading. Figure 4.10 shows motor angular velocity
under a closed-loop PI controller, with a slow constant setpoint of θ̇r = 3 rad s−1 .
Cogging torque compensation is enabled until t = 8 s, from which a decrease in
tracking performance. This serves to demonstrate the effectiveness and value of
this compensation.
Once cogging torque can be assumed to be eliminated, it is possible to com-
pensate for kinetic friction in the motor bearings, modelled as the discontinuous
torque
τk = − sgn(θ̇)FN µk (4.3.1)

where FN denotes the normal force acting through the wheel, and µk the coeffi-
cient of kinetic friction.
Measurement of this torque can be performed by applying an increasing drive
torque to a free-spinning wheel, with τk identified as the torque at which an
initially slowly spinning wheel exhibits only a very slow exponential deceleration.
In order to correctly approximate FN whilst allowing the wheel to spin free, a
mass of mp/4 is attached to the motor in the place of the wheel. This finds a
kinetic friction torque of τk = 3.5 mN m, a 3.5% error over the full torque range,
proportionally a much larger error over the small range of torques required to

77
Chapter 4 A CMD Experimental Prototype

achieve stationary balancing. This compensation yields the overall compensated

torque function



sgn(θ̇)τk for |θ̇| > 0

τcomp = τ − τc + sgn(τ )τk for θ̇ = 0, 6 0
τ= (4.3.2)

for τ θ̇ = 0


0

4.4 Parameter Estimation

A number of model parameters are not directly measurable without the use of
expensive test equipment. While they can be derived from a perfect CAD model,
a number system components are modelled in CAD as homogeneous volumes,
meaning they possess incorrect center of mass and inertial properties. Further-
more, the CAD model does not include minor components such as cabling and
some fasteners and fixings. This section outlines the experiments used to instead
provide a real-world measurement of these parameters.

4.4.1 Wheel Bearing Viscous Friction Coefficient

Applying a slowly ramping torque to each freely rotating wheel connected to a
mass of mp/4 allows the measurement of a steady state torque over angular velocity
curve. Assuming prior compensation of cogging torque and kinetic friction, these
steady state torques are entirely due to viscous friction within the wheel bearings,
and motor effects that can be similarly modelled. This experiment is shown for
both rotation directions in Figure 4.11, showing a relatively constant gradient
between wheels. A linear fitting on this data gives a viscous friction coefficient
measurement of kvw = 2.3 × 10−5 N m rad−1 s.

4.4.2 Wheel Rolling Friction and Roller Viscous Friction

Coefficients
Combining (3.1.5) and (3.1.7) for the case where vx = φ̇ = 0, vy 6= 0 shows that
no roller rotation relative to the wheel occurs when translating purely along b̂y . It
is therefore possible to measure the effects of wheel rolling friction independently
of any friction due to roller rotation. The wheel rolling friction coefficient krw
can be measured by maintaining a constant velocity vy 6= 0 whilst maintaining all

78
 4.4 Parameter Estimation

100 θ̇1
θ̇2
|θ̇i | (rad s−1 ) 80
θ̇3
60 θ̇4

40

20
0.5 1 1.5 2 2.5 3
|τss | (N m) ·10−3

Figure 4.11: Steady state wheel angular velocities over a slowly ramping torque
input for both rotation directions. The gradient of this data represents the wheel
viscous friction coefficient kvw .

others velocities at vx = φ̇ = θ̇p = 0, examining the torque required to maintain

this vy velocity in steady state. Substituting this and the knowledge that v̇ = 0 in
steady state into (3.2.34), substituting roller angles, and assuming a symmetrical
wheel spacing yields Mv (p)v̇ = 0 and C(p, v)v = 0, allowing the dynamics model
to be simplified to
     
0 0 −τ1 + τ2 − τ3 + τ4
   −4vy (kvw +krw )   
0  −τ1 − τ2 − τ3 − τ4
2
= 1
   rw
 
 +  
0  rw −l τ − l τ − l τ − l τ 
0
  
     1 1 2 2 3 3 4 4
−4vy kvw
−ghp mp sin(θp ) rw
−rw (τ1 + τ2 + τ3 + τ4 )
| {z } | {z } | {z }
G(p) Fv Bτ
(4.4.1)
from which krw can be calculated as

4
rw X
krw = −kvw + τi (4.4.2)
4vy
i=1

which using the steady state data from the experiment shown in Figure 4.12 and
the previously measured value for kvw yields krw = 1.97 × 10−4 N s.

With krw known, this procedure can be completed for a constant velocity

79
Chapter 4 A CMD Experimental Prototype

τ1 τ2
vy θp τ3 τ4
·10−2

vy (m s−1 ), θp (rad)
1 5

τ (N m)
0 0

−1 −5
0 5 10 0 5 10
t (s) t (s)
(a) (b)

Figure 4.12: Velocity (a) and torque data (b) for a translation y = [0, 3, 0], in
which a constant velocity is attained for the majority of each movement.

vx 6= 0 using the similarly derived expression

4kvr vx 4vx (kvw + krw ) 8kvr vx 1

− 2
− = (−τ1 + τ2 − τ3 + τ4 ) (4.4.3)
rw (rr − rw ) rw rr rw rw

to calculate kvr as

rr (rr − rw )(τ1 − τ2 + τ3 − τ4 ) rr (krw + kvw )(rr − rw )

kvr = − (4.4.4)
4vx (rr − 2rw ) rw (rr − 2rw )

which for the data in Figure 4.13 yields

kvr = 1.01 × 10−4 N m rad−1 s (4.4.5)

4.4.3 Pendulum Center of Mass Height

It is common knowledge that if an object is suspended by a single point its center

of mass will lie directly under the suspension point in steady state. Provided
there exists some method of measuring the body-relative position of this vector,
multiple measurements can be used to triangulate the center of mass of any
object. Fortunately, for this prototype it is reasonable to assume that symmetry
exists in the (b̂y , b̂z ) and (b̂x , b̂z ) planes, meaning the center of mass must lie

80
 4.4 Parameter Estimation

τ1 τ2
vx τ3 τ4
·10−2
1
0.05
vx (m s−1 )

τ (N m)
0 0

-0.05
−1
0 2 4 6 8 10 0 2 4 6 8 10
t (s) t (s)
(a) (b)

Figure 4.13: Velocity (a) and torque data (b) for a translation x = [0, 3, 0], in
which a constant velocity is attained for the majority of each movement.

somewhere along b̂z . This assumption has already been included in the derivation
of the dynamics model, with the position of the center of mass on b̂z parametrised
as hp .
Suspending the prototype from a single point will therefore yield a vector that
intersects b̂z , allowing measurement of the overall system’s center of mass hcm .
This experiment is performed twice for two different suspension points, shown in
Figure 4.14, with both experiments yielding hcm = 0.072 m.
Knowing the mass and location of the wheels, it is then possible to calculate
the location of the pendulum’s center of mass hp as

4mw hcm
hp = + hcm (4.4.6)
mp

4.4.4 Pendulum Inertia about the Balance Axis

Rigidly fixing the prototype’s wheels and inverting it to allow free rotation of
the body about and beneath the wheel axis allows the dynamics model to be
simplified to that of a physical pendulum as

−mp ghp sin(θp )

θ̈p = − kv θ̇p (4.4.7)
Ipbx

81
Chapter 4 A CMD Experimental Prototype

(a) (b)

Figure 4.14: Experiment used to measure hp , performed by measuring the inter-

section of the vertical and a ruler attached along b̂z . A red line is superimposed
along the string to allow the measurement of hp . Both measurements yield ex-
actly hp = 0.072 m.

in which a viscous friction term kv θ̇p is introduced to model aerodynamic and

friction forces damping the oscillation of the pendulum. As the wheels are fixed
θp can be directly measured by use of the wheel encoders to eliminate state
estimation error. Parameters Ipbx and kv can then be estimated by nonlinear
least-squares fitting, provided suitable initial conditions as to avoid local minima.
An example of this data and the estimated model is shown in Figure 4.15, in which
a 99.1% fit is achieved.

4.4.5 Pendulum Inertia about Vertical and Longitudinal Axes

As accurate identification of Ipy and Ipz is a less critical requirement for good
control performance, these are simply estimated by modelling the prototype as
a solid cuboid as

1 1
Ipy = mp (w2 + h2 ), Ipz = mp (w2 + d2 ) (4.4.8)
12 12

where w, d, h denote the bounding width, depth, and height of the prototype
respectively, given in Table 4.2.

82
 4.4 Parameter Estimation

θp
θp (rad) 0.5 θ̃p

−0.5

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

t (s)

Figure 4.15: Experimental data θp (t) from physical pendulum experiment (blue),
with the response predicted by the fitted physical pendulum model (4.4.7) θ̃p
(red), achieving a 99.1% fit. θp is normalised about the pendulum stable equilib-
rium.

4.4.6 Wheel Mass and Inertia

Wheel mass mw is assumed to be equal to the weight of a single Mecanum wheel

plus half the weight of an individual drive motor, as a rough approximation of
the weight of just the rotor. To estimate the inertia Iwx of the wheel and rotor
assembly about b̂x the prototype is secured in a position that allows free rotation
of the wheels, and a square wave torque profile with zero mean is applied to the
motor. Kinetic and viscous friction can be eliminated by terms similar to (4.3.2)
and that in Section 4.4.1, allowing the simple model

τ + τv + τk
θ̈w = (4.4.9)
Iwx

to be used for estimation. Figure 4.16 shows a dataset generated by this method,
in which measurements taken around θ̇ = 0 are removed due to inaccurate speed
measurement at this point. The gradient of a linear fitting of this data represents
−1 , yielding I −5 kg m2 .
Iwx wx = 5.12 × 10

4.4.7 Comparison of Experimentally Measured Parameters with

CAD Derived Estimates

The resulting experimentally estimated parameters and their CAD derived values
are given in Table 4.2. The CAD model, shown in Figure 4.17, includes all

83
Chapter 4 A CMD Experimental Prototype

1,300

θ̈ (rad s−2 ) 1,200

1,100

5.6 5.7 5.8 5.9 6 6.1 6.2 6.3

|τ − τv − τk | (N m) ·10−2

Figure 4.16: Wheel angular acceleration over viscous and kinetic friction com-
pensated torque for τ = ±0.06 N m, along with the linear fit representing an
estimate of Iwx , in which intersection with the origin is enforced.

of the main system components, but omits minor complex parts such as cable
assemblies. This results in an overall mass difference of 0.1 kg, with similar
differences in center of mass height and pendulum inertia. A comparison of these
values shows a close fit, which is assumed to sufficiently verify the validity of the
experimental methods used.

4.5 Sensing

None of the system states (p, v) are directly measurable, meaning they must be
estimated using available sensor data. This data is provided by the following
sources:

4.5.1 Wheel Encoders

Each Mecanum wheel motor is equipped with a 2048 count/rev incremental quad-
rature encoder. This provides two square waves in quadrature that can be used to
measure a relative change in angular position. These sensors exhibit no drift, and
only a small amount of quantization error. This angular position measurement
θei is relative to the pendulum body as

θei = θi − θp + ωi (4.5.1)

84
 4.5 Sensing

Table 4.2: Table of experimentally and CAD derived parameters for the prototype
depicted in Figure 4.3.
Experimentally Measured CAD Derived
Parameter Unit
Value Value
α1 , α 3 rad N/A π/4

α2 , α 4 rad N/A −π/4

Ipbx kg m2 0.0315 0.0289
Ipby kg m2 N/A 0.0534
Ipbz kg m2 N/A 0.0271
Iwx kg m2 5.12 × 10−5 4.74 × 10−5
Iwyz kg m2 N/A 1.1 × 10−4
mp kg 2.64 2.53
mw kg 0.145 0.145
hcm m 0.072 0.070
hp m 0.0874 0.0854
−l1 , l4 m N/A 0.105
−l2 , l3 m N/A 0.063
rw m 0.030 0.030
rr m N/A 0.0055
kvw N m rad−1 s 2.3 × 10−5 N/A
kvr N m rad−1 s 1.01 × 10−4 N/A
krw Ns 1.97 × 10−4 N/A
h m N/A 0.233
w m N/A 0.337
d m N/A 0.082

where ωi represents quantisation noise.

4.5.2 Gyroscopes

Three orthogonal gyroscopes are used to provide angular rate measurements

about the three pendulum axes as Ω~ = Ωp Ωq Ωr T . These measurements
 

possess a high signal-to-noise ratio, but are distorted by a significant temper-

ature and time varying bias about zero, sufficiently modelled as an integrated

85
Chapter 4 A CMD Experimental Prototype

Figure 4.17: CMD CAD model from which parameter verification values are
derived. Only components which are straightforward to model are included, i.e.
parts such as cabling are omitted.

additive Gaussian white noise of variance σg2 . These sensors also suffer from axis
misalignment, meaning the gyroscope axes are only approximately orthogonal to
one another, along with the IMU package axes not being perfectly aligned with
that of the body, presenting as a ‘cross-talk’ when performing rotations exactly
about each of the body axes. For the MPU9250 this cross-axis sensitivity is
reported as typically ±2 % [95], which combined with package mounting error
can yield a significant overall misalignment. A scale error is also present, for the
MPU9250 reported as ±3 % at 25 ◦C. Combining these error sources gives the
linear sensor model mapping measured angular rates Ω
~ m to true rates Ω
~ as
     
Ωp 1 Mpq Mpr 1/Sp 0 0 Ωm,p − bp
~ = Ωq  = Mqp
Ω 1 Mqr   0 1/Sq 0  Ωm,q − bq  (4.5.2)
      

Ωr Mrp Mrq 1 0 0 1/Sr Ωm,r − br

The gyroscope biases ~bΩ = bp bq br , while correlated with sensor tempera-

 T

86
 4.5 Sensing

3π θb
ψb
Position (rad) 2π φb
θa
π ψa
φa
0

−π

−2π
0 50 100 150 200
t (s)

Figure 4.18: Integrated gyroscope scale and misalignmentRcalibration data pre-

(dashed) and post-calibration (solid), where (θ, ψ, φ) = (p, q, r) dt. Data is
acquired by rotating the system exactly about each of the individual body axes
by known multiples of π/2, allowing a comparison of the measured integrals with
the known angle of rotation. A gyroscope free from scale and misalignment error
would have no error between the measured and integrated angles, and would
measure zero rotation on axes that are not in motion. This misalignment error is
exampled at t = 80 s where a movement in Ωr couples into Ωq . This is completely
removed by calibration.

ture, largely follow a random walk about zero, and therefore cannot be calibrated
offline. Scale and cross-axis misalignment errors, however, are relatively time in-
variant, and can be measured by rotating the sensor at known angular rates
exactly about each of the body frame axes. This requires the use of a rate table,
an expensive piece of test equipment, so this could not be performed. Instead,
the system is rotated exactly about its body axes whilst recording angular rate
data, which is then integrated to provide a measurement of each rotation an-
gle. Comparison of this with knowledge of the exact angle through which the
system was rotated allows an estimation of these scale and misalignment error
matrices. This experiment is shown in Figure 4.18, with measured angles pre-
and post-calibration. A significant misalignment error is visible in the ψ angle,
corresponding to the Ωq gyroscope, and small scale errors are visible in all three
sensors. The calibrated data shows these errors have been eliminated, with no
visible cross-coupling remaining.

87
Chapter 4 A CMD Experimental Prototype

1.08
kam k
1.06 kak

Acceleration (gm/s2 )
1.04

1.02

0.98

0.96
0 10 20 30 40 50 60 70 80
t (s)

Figure 4.19: The norms of a set of acceleration data pre- (blue) and post-
calibration (red). This data was gathered by slowly rotating the platform through
a wide range of angles to provide a measurement of g from all orientations, The
data is normalised to g and smoothed for readability.

4.5.3 Accelerometers

Three orthogonal accelerometers provide measurements of acceleration along

three axes parallel to {b̂x , b̂y , b̂z }, with the sensor mounted approximately at
the overall system center of mass to minimise measurement of acceleration due
to angular acceleration of the system. These measure both dynamic acceleration
due to movement of the system, as well as the constant acceleration of the grav-
ity vector, defined in B as ~g = 0 0 − 9.81 . These measurements are very
 T

noisy due to mechanical vibration of the system when in motion, with this noise
source modelled as an additive white noise of variance σa2 . Accelerometers are
also affected by cross-axis misalignment, scale, and bias errors. These errors are
relatively time and temperature invariant, allowing a fixed compensation. The
measured accelerometer reading ~am is therefore mapped to the true reading ~a
using a similar linear sensor model
     
ax 1 Mxy Mxz 1/Sx 0 0 am,x − bx
~a = ay  = Myx 1 Myz   0 1/Sy 0  am,y − by  (4.5.3)
     

az Mzx Mzy 1 0 0 1/Sz am,z − bz

88
 4.6 State Estimation

Figure 4.19 shows the magnitude of a set of acceleration data gathered by

slowly rotating the platform through a wide range of angles, normalised to g. As
this data is for a nearly static system this should be mostly just a measurement
of gravity, and should therefore be of relatively constant magnitude, which is
clearly not the case for the uncalibrated data. The magnitude of the calibrated
data remains much closer to 1, with some as variation expected due to minor
acceleration of the platform whilst it is rotated.

4.5.4 Omitted Sensors

A number of sensors commonly used in robotics could have been included on this
prototype, but were omitted for the following reasons:

• Magnetometers - These allow the measurement of the earth’s magnetic field,

allowing an absolute measurement of heading. However, when operating
inside modern buildings containing large amounts of structural steel the
Earth’s weak ambient field is often heavily distorted, making these sensors
ineffective indoors. It is therefore assumed that a small amount of drift will
be present in any derived yaw estimate.

• Computer Vision - Various optical and LIDAR sensing technologies could

allow the platform to perform simultaneous location and mapping (SLAM).
As the control techniques developed in this thesis do not aim to incorpo-
rate obstacle avoidance, the online generation of maps is not vital to this
research. Furthermore, it was found that the dead reckoning used here in-
stead exhibited very slow drift, and so is more than adequate for control
development and evaluation.

• GPS - This would provide an absolute position measurement that could

be used to correct dead reckoning error, but has low accuracy and cannot
operate indoors.

4.6 State Estimation

Using the above sensors an estimate of the full system state (p, v) must be pro-
duced. By use of the inverse kinematic model (3.1.8) estimates of vx , vy , and φ̇
can be produced. This requires the measurement of wheel velocities θ̇i , which

89
Chapter 4 A CMD Experimental Prototype

must be derived from wheel encoder angular position measurements by numer-

ical differentiation, for which there are two obvious options. The first common
approach is to measure the time between changes of encoder state, with velocity
inversely proportional to ∆t as θ̇k = , where θs denotes the size of a
sgn(θk −θk−1 )
θs ∆tk
single encoder step. This produces a measurement with good accuracy on every
change in encoder state for the majority of speeds. However, as speed approaches
zero the period between measurement updates increases, to the point where no
measurement of velocity is returned for a stationary wheel. This differentiation
scheme also tends to amplify high frequency noise, such as that generated by
vibration about a quantisation step. Alternatively, instead of performing mea-
surements over a fixed ∆θe,i , it is possible to fix ∆t to some suitable period, and
instead compares the encoder position at the start and end of the measurement
window in order to calculate a speed measurement. This provides measurements
at a fixed known update rate, and is able to measure a speed of zero. Unfortu-
nately, in order to avoid acting as a low-pass filter, a relatively short value of ∆t
is required, which through the quantisation of position measurements leads to a
coarsely quantised velocity measurement.

For these reasons, these approaches are typically superseded by observer and
estimation methods [33]. These methods model wheel velocity as an internal
state, which is continuously integrated to give a position estimate that can be
compared with the true measurement returned by the encoder. An error in this
comparison indicates an incorrect estimate of θ̇i , which can be used to recursively
update θ̇i until this error is eliminated. These methods are able to provide
estimates at a fast fixed update rate, and are able to provide high resolution
estimates even at very low or zero speed.

Rather than directly estimating each of θ̇i , here body velocities vx , vy , and φ̇
are modelled as internal states instead, which can then be integrated with each
iteration to give estimates of x, y, and φ. By the inverse kinematics model (3.1.8)
these body velocities can be uniquely mapped to equivalent wheel velocities, that
can in turn be integrated to give an expected change in wheel position over
each estimator timestep ∆θei . This can then be compared with the measured
difference in wheel position between this and the previous iteration, providing an
error signal that can be used to correct the estimate of (vx , vy , φ̇).

90
 4.6 State Estimation

This gives the prediction model

   
vbx vbx,
(4.6.1)
 
vby  = 
vby, 

+w
~k
 
φ
ḃ φ̇ k−1
k

and measurement model

     
θ̃e1 θe1 −b
vx,k cot(α1 ) − vby,k − φbk l1 − Rw (Ωp − bp )
     
−b
 vx,k cot(α2 ) − vby,k − φk l2 − Rw (Ωp − bp ) + ~vk
θ̃e2 
  = θe2  1
  b 
+
θ̃ 
 e3 
θ 
 e3  rw −b
 vx,k cot(α3 ) − vby,k − φk l3 − Rw (Ωp − bp )
b 
θ̃e4 k θe4 k−1 −b
vx,k cot(α4 ) − vby,k − φbk l4 − Rw (Ωp − bp )
(4.6.2)
where w
~ k and ~vk are process and observation noise vectors with covariances Qk
and Rk , both assumed to be multivariate zero mean Gaussian noise sources.
Position estimates can then be derived by numerical integration with timestep
δt as
   
xb xbk−1 + δtk (bvx,k−1 cos(φk−1 ) − vby,k−1 sin(φk−1 ))
 yb =  ybk−1 + δtk (b
vx,k−1 sin(φk−1 ) + vby,k−1 cos(φk−1 ))  (4.6.3)
   

φb k
φbk−1 + δtk φ̇k−1

However, this method is found to yield excessive position estimate drift unless a
very short velocity estimator time constant is used, which in turn yields excessive
velocity estimate noise. A position estimate with less drift can instead be derived
by discrete integration of the inertial frame inverse kinematics model evaluated

91
Chapter 4 A CMD Experimental Prototype

at a time-varying φ as

 cos(α1 +φk−1 ) sin(α1 +φk−1 ) −1

− rlw1
   
x x rw sin(α1 ) − rw sin(α1 ) −1
 cos(α 2 +φk−1 ) sin(α2 +φk−1 )
− rw sin(α − rlw2 −1
    
y y  r sin(α
  =   w 2) 2)
+  cos(α3 +φk−1 ) sin(α3 +φk−1 )

φ
 
φ
   rw sin(α3 ) − rw sin(α3 ) − rlw3 −1
cos(α4 +φk−1 ) sin(α4 +φk−1 ) l4
θp k θp k−1 r sin(α ) − rw sin(α − rw −1
w 4 4)
    
θe,1 θe,1
    
θe,2  θe,2  
θ  − θ   (4.6.4)
·    
 e,3   e,3  
θe,4 k θe,4 k−1

Assuming known measurements of φ and θp , the accuracy of this inversion can

be improved, yielding

 cos(α1 +φk−1 ) sin(α1 +φk−1 ) −1

rw sin(α1 ) − rw sin(α1 )
" # " #  cos(α 2 +φk−1 ) sin(α2 +φk−1 ) 
x x  r sin(α
2)
− rw sin(α2 ) 
= +  w 
y y  cos(α3 +φk−1 ) sin(α3 +φk−1 ) 
− rw sin(α
k k−1  rw sin(α3 ) 3)

cos(α4 +φk−1 ) sin(α4 +φk−1 )
rw sin(α4 ) − rw sin(α4 )
      
θe,1 θe,1 − l1 −1 "
     rlw2  #
θe,2  − r −1 −
  − θe,2  φ k φ k−1  (4.6.5)
  
· −  w
− l3

θ 
 e,3 
θ 
 e,3   rw −1
 θp − θp,k−1 

θe,4 k θe,4 k−1 − rlw4 −1

No absolute position measurement is available on this system, preventing the

correction of dead reckoning integration drift. This integration is therefore per-
formed outside of the state estimator to reduce the size of the estimator’s state
vector.

The wheel encoders and inverse kinematics model alone are not able to provide
an estimate of θp . Biased measurements of φ̇ and θ̇p can be obtained by rotation
of Ω by Rbp , and it is known that no rotation occurs about b̂y when assuming

92
 4.6 State Estimation

operation on a level surface, allowing the definition of the mapping

    
θ̇p bp
 0  = Rbp Ω − bq  (4.6.6)
    

φ̇ br
 
Ωp − bp
= cos(θp )(Ωq − bq ) − sin(θp )(Ωr − br ) (4.6.7)
 

sin(θp )(Ωq − bq ) + cos(θp )(Ωr − br )

These measurements of φ̇ and θ̇p can be integrated to yield estimates of φ and

θp , giving the expanded prediction model
  
  φbk−1 + δtk sin(θp,k−1 )(Ωq,k − bbq,k−1 )
φb  
+ cos(θp,k−1 )(Ωr,k − bbr,k−1 ) 
 
θp 
b  
 
θbp,k−1 + δtk (Ωp − bp )
   
vbx   
   
vby  = v x,k−1 +w
~k (4.6.8)
   b 
vby,k−1
   
bbp   
   
bbp,k−1
bq 
b   
 
bbr

 bbq,k−1 

k
bbr,k−1

While φ̇ and θ̇p could be modelled as internal states to allow for filtering of
gyroscope noise, this is instead assumed to be sufficiently performed by the FPGA
low-pass filter, leaving a noise-free but biased signal.
Determination of gyroscope biases and elimination of integral drift in the esti-
mates of φ and θp each require the incorporation of an additional measurement.
A measurement of θp can be calculated by comparison of the measured body
T . From (4.6.6),
acceleration vector with that expected by a rotation of ~g by Rbp
when θp = 0 a measurement of bq is available, distorted by noise due to non-level
terrain. No measurement of φ is available to eliminate integral drift when θp = 0,
and the inverse kinematics derived measurement of φ̇ is found to suffer from a
greater time varying bias than Ωr , so this cannot be used to improve the estimate
of br . This bias is therefore unobservable, and must be set to a constant value
measured while the system is stationary, and so the estimate of φ will always be

93
Chapter 4 A CMD Experimental Prototype

subject to a slow drift.

This yields the final prediction and measurement models
   
θbp θbp,k−1 + δtk (Ωp − bp )
vbx  vbx,k−1
   
 
(4.6.9)
   
vby  =  +w
   vby,k−1  ~k
bp  bbp,k−1
b   
 
bbq bbq,k−1
k

θ̃e1,k−1 + r1w (−b

   
θ̃e1 vx,k cot(α1 ) − vby,k − φbk l1 ) − (Ωp,k − bp,k )
θ̃e2  θ̃e2,k−1 + r1w (−bvx,k cot(α2 ) − vby,k − φbk l2 ) − (Ωp,k − bp,k )
   
   
θ̃e3  θ̃e3,k−1 + 1 (−b v x,k cot(α 3 ) − v y,k − bk l3 ) − (Ωp,k − bp,k )
φ
   rw b 
1
θ̃e4  = θ̃e4,k−1 + rw (−b vx,k cot(α4 ) − vby,k − φk l4 ) − (Ωp,k − bp,k ) + ~vk
   b 
   
 ãy   g sin( θ
bp,k ) 
   
 ãz  g cos(θbp,k )
   
 
0 k cos(θp,k )(Ωq,k − bq,k ) − sin(θp,k )(Ωr,k − br )
(4.6.10)
The nonlinearity of (4.6.10) necessitates the use of a nonlinear state estimator.
In the field of attitude estimation the extended Kalman filter (EKF) is the de
facto standard [97], making this an obvious choice. The standard EKF equations,
omitted here for brevity, are therefore evaluated with every new set of sensor data
to iteratively estimate the modelled state. While this estimator has no guarantee
of optimality or convergence, in practise this occurs very quickly, even for an
initially non-stationary system.
Behaviour of the filter is controlled by selection of the process and measurement
covariance matrices Q and R, assumed to be diagonal. The element of Q mapping
to θbp represents uncertainty due to integration error and any remaining gyroscope
noise, so is set to a very small value. The elements of Q that map to the estimates
of vx and vy control the rate at which these velocities are expected to change.
They are therefore selected to provide a suitably quick velocity decay to zero in
the absence of a change in wheel position, whilst avoiding the production of a
sawtooth shaped estimate when operating at a small constant velocity. Finally,
rows mapping Q to bbp and bbq reflect the rate at which these biases are expected
to drift, and therefore are made very small so that once a correct estimate is
achieved it only changes very slowly.

94
 4.7 Conclusion

Rows of R that map to wheel encoder angular positions are chosen to reflect
encoder quantization noise and uncertainty in the inverse kinematics model due
to imperfect wheel geometry and unmodelled effects such as backlash in the
roller mountings and wheel slip. This is therefore chosen by trial and error until
desirable body velocity estimates are achieved. Rows of R that map to ãy and
ãz partially represent intrinsic sensor noise in the accelerometers, but are mostly
dominated by noise due to vibration and dynamic acceleration of the system.
These are therefore tuned in tandem with Q to ensure suitable time constants
for the estimation of the gyroscope biases and sufficiently fast correction of drift
of the estimate of θp , whilst avoiding the coupling of any noticeable amount of
accelerometer noise into these estimates. These elements are scaled by the factor
1 + eK kak2 −g , where K  1, such that the accelerometer is trusted less when
acceleration other than that due to gravity is measured. Finally, the element of
R mapping to bbq is chosen to ensure a desirable rate of convergence of bbq → bq .
Figure 4.20 shows the output of this estimator for a set of experimental data.
This experiment starts with the system lying down, with a movement at t = 3.5 s
to a known upright position. This is maintained until t = 20 s, before random
movements in all states are performed until t = 68 s, ending at the same location
as at t = 20 s. This shows a total position drift due to dead reckoning error of
0.073 m. This increases to 0.094 m if φ is derived from the inverse kinematics
model rather than the gyroscopes. The plot of φ contains a second trajectory
φb , representing the φ estimate generated when assuming bq = 0, demonstrating
the value in estimating this bias. θp is seen to quickly converge to the correct
estimate from the origin, with all of the integral drift visible in the raw integral
of Ωp eliminated. Gyroscope bias estimates bbp and bbq show mostly monotonically
decreasing variances, converging to nearly constant values as intended.

4.7 Conclusion

This chapter has described the reasoning and methodology behind the design
and construction of a Collinear Mecanum Drive prototype. Methods for the
compensation of undesirable actuation nonlinearities are presented, allowing the
assumption minimal input model mismatch. A methodology for the experimental
measurement of all model parameters for a given CMD has been demonstrated,

95
Chapter 4 A CMD Experimental Prototype

x
0.4 y

Position
(m)
0.2
0
−0.2

φ
Heading

0 φb
(rad)

−2
−4

0.4 vx
vy
Velocity
(m s−1 )

0.2
0
−0.2
−0.4
2
θp
Pitch (rad)

R
1 Ωp

0
Bias (rad s−1 )

bp
Gyroscope

0.2 bq

101 Var(θbp )
rad2 s−2 )
Variance
(rad2 ,

Var(bbp )
10−2
Var(bbq )
10−5
0 10 20 30 40 50 60 70 80
t (s)

Figure 4.20: State estimation data gathered by moving the system through a
range of random trajectories, with the same real-world position and pose main-
tained at t = 20 s and t ≥ 68 s to allow a measurement of dead reckoning drift.
φb shows an estimate of φ generated whilst forcing bq ≡ 0, demonstrating the
value of estimating bq . Similar, plot 4 shows both θbp and an estimate achieved
by integration of Ωp , demonstrating rapid divergence due to gyroscope bias and
integral drift. All states are initialised at the origin, and the EKF covariance
matrix is initialised to P0 = I5×5 × 102 to represent unknown initial conditions.

96
 4.7 Conclusion

with the results of this verified by comparison with that expected from a CAD
model of the prototype. Finally, the sensor calibration and state estimation
methods necessary for the accurate online estimation of the full system state
from distorted sensor data has been derived and demonstrated experimentally.
It is therefore assumed that the prototype developed in this chapter accurately
fits the model derived in Chapter 3. This will allow for the meaningful experi-
mental evaluation of control and trajectory planning techniques developed in the
remainder of this thesis.

97
Chapter 5

Nonlinear Control

This chapter introduces the first of three control methods explored in this thesis.
The CMD is first locally partially feedback linearised by a local state space diffeo-
morphism and nonlinear feedback, expressing the system dynamics as five linear
and three nonlinear ODEs with three new inputs. Three nonlinear controllers are
then derived using a backstepping approach, controlling system local body frame
velocities, inertial frame body velocities, and global Cartesian position. Lya-
punov functions are derived for all three controllers to guarantee stability, and
asymptotic convergence to the desired references from a bounded set of initial
states is proven. These controllers are evaluated in both simulation and on the
experimental prototype.

Feedback linearisation is a procedure by which a nonlinear system can be

transformed into an equivalent fully or partially linear system, achieved using
a change of control input, along with either a change of state space coordinates,
or a transformation of the output [98]. The extent to which a system can be
linearised by these methods can be determined by examining the system’s relative
degree; only systems with a maximum relative degree equal to the size of their
state space can be fully linearised by feedback. These methods result in a system
that is either partially or fully linear, allowing the application of classical linear
control and analysis techniques to a previously nonlinear plant. In the partially
linearised case, the remaining nonlinear subsystems can then be controlled using
nonlinear control techniques, which is typically an easier task than applying these
techniques to the original higher dimensional nonlinear system.
Feedback linearisation of systems with a relative degree of less than n will

99
Chapter 5 Nonlinear Control

yield systems that contain zero dynamics, new states and dynamics that are
unobservable from the new outputs, which may be unstable. In practise it can
be dangerous for these unobservable states to be allowed to grow unboundedly,
so their behaviour must be considered during control design.
These techniques have been applied to various forms of inverted pendulum,
such as the single and double cart-pole inverted pendulums [99,100], the reaction
wheel inverted pendulum [101], the acrobot [102, 103], and most relevantly, the
two-wheeled inverted pendulum [51, 56, 104]. Partial feedback linearisation has
also been utilised in the control of quadrotors [105, 106]. These methods have
never been applied to a ball-balancing system. As all of these systems are under-
actuated only partial feedback linearisation is achieved, with nonlinear controllers
subsequently designed to control the remaining nonlinear dynamics.

5.1 Partial Feedback Linearisation

In order to facilitate the derivation of a feedback linearising control, the input

vector fields of (3.2.44) are first simplified using a change of input v = P (x)u to
define a new decoupled input v. It is found that
 
g15 g25 g35
P (x) = g17 g27 g37  (5.1.1)
 

g18 g28 g38

is nonsingular for |θp | . 2.39 rad using the parameters in Table 4.2, and is there-
fore invertible under this condition, allowing the definition of the new input vector
fields  
03×4
 T 
g˜1 1 0 0


(5.1.2)
 
g˜2  = 
g̃16 g̃26 g̃36 
 

g˜3 0 1 0
 

0 0 1
where
 T  T
g̃16 g16
g̃26  = g26  P
−1
(5.1.3)
   

g̃36 g36

100
 5.1 Partial Feedback Linearisation

in which (g̃16 , g̃26 ) = 0 for the parameters in Table 4.2, and g̃36 is scalar valued
function that is again smooth for |θp | . 2.39 rad. The ẋ5 , ẋ7 , and ẋ8 subsystems
can then be linearised by the feedback

v1 = w1 − f5 (x)
v2 = w2 − f7 (x) (5.1.4)
v3 = w3 − f8 (x)

in which w = w1 w2 w3 is used as the new input, yielding the new drift and
 T

unchanged input vector fields

 
cos(x3 )x5 − sin(x3 )x6
 

 sin(x 3 )x5 + cos(x 3 )x6 


 x7 

 
x8
˜ (5.1.5)
 
f (x) = 
 
 0 

 
f6 − g̃16 (x4 )f5 (x) − g̃26 (x4 )f7 (x) − g̃36 (x4 )f8 (x)
 

 0 

0
 
01×4 01×4 01×4
 T 
g˜1 (x4 )  1 0 0 


(5.1.6)
 
g˜2 (x4 ) = 
g̃16 (x4 ) g̃26 (x4 ) g̃36 (x4 )
  
g˜3 (x4 )  0 1 0 
 

0 0 1

In order for the coordinates x to fully span R8 they must be linearly indepen-
dent, meaning their gradients ẋ must be linearly independent of one another [89].
Clearly in (5.1.5) this property has been lost, as ẋ6 is now a linear function of ẋ5 ,
ẋ7 , and ẋ8 . A state transformation T : x → z is therefore required to transform
x into some new set of linearly independent coordinates z as z = T(x). As ẋi for
i = [1 . . 5, 7, 8] are already linearly independent, these can be mapped as zi = xi .

101
Chapter 5 Nonlinear Control

As w ≡ ẋ5 ẋ7 ẋ8 , it is required that

 T

 T
g˜1 (x4 )
∂z6 
g˜2 (x4 ) = 0 (5.1.7)

∂x
g˜3 (x4 )

Writing ∇z6 = α1 . . . α8 , by (5.1.7) it is implied that

 

α5 + α6 g̃16 = 0
α7 + α6 g̃26 = 0 (5.1.8)
α8 + α6 g̃36 = 0

which is satisfied for

α5 = −λg̃16
α6 = λ
(5.1.9)
α7 = −λg̃26
α8 = −λg̃36
Choosing λ = 1, z6 can be defined as

z6 = x6 − x5 g̃16 − x7 g̃26 − x8 g̃36 (5.1.10)

defining the transformation T(x) as

 
x1
 

 x2 


 x 3


 
x
(5.1.11)
 4 
z = T(x) =  

 x5 

 
x6 − x5 ĝ16 − x7 ĝ26 − x8 ĝ36 
 

 x7 

x8

For the parameters in Table 4.2

 
∂T(x)
det 6= 0 ∀ {x ∈ R8 | x4 mod 2π 6= ±2.39 rad} (5.1.12)
∂x

102
 5.1 Partial Feedback Linearisation

therefore the Jacobian of T is locally invertible, meaning T is a local diffeomor-

phism under this condition [89], with an inverse mapping x = T−1 (z). Taking
the differential of T(x) w.r.t. time allows ż to be expressed in terms of x as

żi = ẋi , i = [1, . . . , 5, 7, 8]

∂g̃16 ∂g̃26 ∂g̃36 (5.1.13)
ż6 = −ẋ5 g̃16 − x5 − ẋ7 g̃26 − x7 − ẋ8 g̃36 − x8
∂x4 ∂x4 ∂x4

which when substituted with differentials of x from (5.1.5) yields the new set of
dynamic equations

ż1 = cos(z3 )z5 − sin(z3 )(z6 + z5 g̃16 (z) + z7 g̃26 (z) + z8 g̃36 (z)) (5.1.14)
ż2 = sin(z3 )z5 + cos(z3 )(z6 + z5 g̃16 (z) + z7 g̃26 (z) + z8 g̃36 (z)) (5.1.15)
ż3 = z7 (5.1.16)
ż4 = z8 (5.1.17)
ż5 = w1 (5.1.18)
ż6 = f6 (z) − g̃16 (z4 )f5 (z) − g̃26 (z4 )f7 (z) − g̃36 (z4 )f8 (z)
∂g̃16 (z) ∂g̃26 (z) ∂g̃36 (z)
− z5 − z7 − z8 (5.1.19)
∂z4 ∂z4 ∂z4
ż7 = w2 (5.1.20)
ż8 = w3 (5.1.21)

where all f (x) and g̃(x) have been rewritten in terms of z using x = T−1 (z).
Under this state transformation and feedback it is evident that w has been elim-
inated from the expression for ż6 , meaning ż6 , ż1 , and ż2 now represent internal
dynamics, and in which ż5 , ż7 , and ż8 are now independent of the drift vector,
and are linear and decoupled in the new input w.

The internal dynamics (5.1.19) are found to contain zeroth to second time
derivatives of θp , and cannot be integrated to eliminate either of these velocity or
acceleration terms. This expression therefore forms a second order nonholonomic
constraint, also referred to as a dynamic constraint.

Examining the zero dynamics found by setting w = z5 = z7 = z8 = 0 in

(ż1 , ż2 , ż6 ), and whilst assuming defined roller angles, wheel spacing symmetry,

103
Chapter 5 Nonlinear Control

and zero friction for model simplification, yields

ż1 = −z6 sin(z3 ) (5.1.22)

ż2 = z6 cos(z3 ) (5.1.23)
ghp mp rw sin(z4 )
ż6 = − 2 2 + h m r cos(z )
(5.1.24)
4Iwx + mp rw+ 4mw rw p p w 4

in which it is clear that the zero dynamics do not have a stable equilibrium for
6 0, and so the system is non-minimum phase [107].
z4 =
To summarise, through input transformation, coordinate transformation, and
nonlinear feedback, the nonlinear system (3.2.35) has been transformed into an
equivalent system of five linear and three nonlinear ODEs, with new input w ≡
v̇x φ̈ θ̈p . Actual motor torques are retrieved by the mapping w → v → u → τ .
 T

The simulated response of this system to a 0.25 Hz square wave input of unit
amplitude to each of w is shown in Figure 5.1, demonstrating the correct linear
response of the feedback linearised subsystems vx , φ̇, and θ̇p , and unbounded
growth of vy as expected.

5.2 Nonlinear Control of the Partially Feedback

Linearised CMD
Numerous approaches exist for the control of the remaining nonlinear dynam-
ics. The simplest methods approximate the nonlinear dynamics by linearisation
about a desired operating point [104], in this case the unstable upright equilib-
rium. This typically yields only a small region about the operating point in which
desirable behaviour is achieved, and does not lend itself to straightforward sta-
bility analysis of the closed-loop nonlinear system. This region of operation can
be extended using gain scheduling, in which the system is linearised about multi-
ple operating points distributed about an operating region of interest within the
state space, from which linear controllers relevant to each subset of the operating
region can be derived. A switching mechanism then selects the relevant controller
based on the current state. While improving performance, the required quantity
of controllers increases exponentially with system order, making application to
complex systems such as that in this thesis challenging [108]. Again, this method
does not facilitate the derivation of a stability proof for the full nonlinear system.

104
 5.2 Nonlinear Control of the Partially Feedback Linearised CMD

1 vx
0.5
0 φ̇
−0.5 θ̇p
−1
0 1 2 3 4 5 6 7 8 9 10
0 vy
−10
−20

0 1 2 3 4 5 6 7 8 9 10
1 τ1
τ2
0 τ3
τ4
−1
0 1 2 3 4 5 6 7 8 9 10
t (s)

Figure 5.1: Simulated state trajectories of the partially feedback linearised CMD,
initialised at the origin and with each of w driven by a 0.25 Hz square wave of
unit amplitude. This demonstrates the expected triangular velocity profile in the
vx , φ̇, and θ̇p states, while vy grows unboundedly. τ remains defined throughout
the simulation, as the singularity in (5.1.2) is avoided.

Pathak controls the remaining nonlinear dynamics using a backstepping ap-

proach [51]. In this method a cascade nonlinear system is controlled by recursively
stabilising each subsystem whilst ‘stepping back’ through the cascaded subsys-
tems. This stabilisation is usually performed by deriving nonlinear controllers
that allows each closed-loop subsystem to be formulated as a Lyapunov function,
yielding an overall controlled system with stability and convergence guarantees
for the full nonlinear dynamics.
A second common approach to the control of these remaining nonlinear subsys-
tems is sliding mode control, which has been applied to the feedback linearised
two-wheeled inverted pendulum [58], quadrotors [109], and an array of other
forms of inverted pendulum [110]. The main advantage of sliding mode control
is its robustness to modelling error and disturbance, and as a bang-bang con-
troller sliding mode control is equivalent to time-optimal control for a number of
systems. This can, however, introduce switching chatter into the control signal,

105
Chapter 5 Nonlinear Control

increasing energy usage and generating noise.

It is often necessary, as in this application, to enforce input and state con-
straints within the controller. In backstepping control these can be incorporated
using Lyapunov barrier functions [111], scalar functions in which a unique min-
imum is attained at the desired steady state, and which tend to infinity as the
constraint is approached. This allows an embedding of constraints directly into
the control law, whilst retaining a stability proof for the closed-loop system.
In this chapter the former approach is chosen, opting to control the remaining
nonlinear subsystems by use the use of a backstepping approach. Three separate
controllers are to be derived, starting with the control of local frame body veloc-
ities. This controller is relevant to applications where the CMD is to be ‘driven’
by a user, such as when operating as a personal mobility vehicle or teleoperated
platform. The second approach controls velocities in the fixed inertial frame, and
the latter controls the position of the CMD in the fixed inertial frame. These
are more useful in situations where the CMD is to operate autonomously, such
as when moving between waypoints as to navigate an environment. In providing
Lyapunov stability guarantees for the full nonlinear closed-loop system, these
controllers can be viewed as the safety-critical control option within this thesis,
albeit without a guarantee of robustness to model uncertainty.

5.3 Backstepping Control of Local Body Frame

Velocities

This controller is required to drive the system local body frame velocities (vx , vy , φ̇)
to setpoints (vxr , vyr , φ̇r ). This must be performed whilst bounding deviation of
θp from zero as to avoid attempting to translate using slip-inducing lean angles,
and accelerations v̇x and φ̈ must be bounded to again avoid inducing wheel slip.
By using a backstepping design approach each subsystem can be proven to be
stable by ensuring it can be formulated as a Lyapunov function following loop
closure. With appropriate embedding of constraints and bounding of reference
inputs this allows the design of a controller with an asymptotic stability guarantee
for the full bounded set of references and states.
Control is to be split into two layers. Aggressive control of the θp state is de-
sired to provide resistance to disturbances, especially those generated by varying

106
 5.3 Backstepping Control of Local Body Frame Velocities

friction forces when translating in the b̂x direction, so the linear controller

w3 = −Kθ̇p θ̇p − Kθp (θp − θpr ) (5.3.1)

with suitable gains Kθ̇p and Kθp is used to provide global exponential conver-
gence of θp → θpr , where θpr represents a new internal reference signal. As this
subsystem has relatively fast dynamics, and as low-noise measurements of θ̇p and
θp are available, high gains can be used to allow rapid convergence in the region
of 100 ms for large step changes. While linear controllers could also be used to
control the feedback linearised vx and φ̇ subsystems, these are instead to be con-
trolled by the outer loop as to allow the embedding of acceleration constraint
enforcement.
The goal of the outer controller is to generate w1 , w2 , and θpr trajectories that
result in convergence of (vx , vy , φ̇) → (vxr , vyr , φ̇r ) within finite time. Unlike a
TWIP, cross coupling between the (θp , vy ) subsystem and the vx and φ̇ subsys-
tems, for example acceleration forces acting on θp when vx φ̇ 6= 0, means that
θpr 6→ 0 may be required for v̇y → 0 in steady state.
From (5.1.5), acceleration v̇y can be expressed as

v̇y = fv̇y (x, w) = f6 (x) − ĝ16 (x)f5 (x) − ĝ26 (x)f7 (x) − ĝ36 (x)f8 (x)
+ ĝ16 (x)w1 + ĝ26 (x)w2 + ĝ36 (x)w3 (5.3.2)

This is a complex expression for which it is difficult to analyse the effect of

parameter choice, so this is instead substituted with the parameters in Table
4.2, with the assumption that the properties of this function are unlikely to
significantly change over realistic ranges of parameter variation. This yields an
expression of the form

fv̇y (x, w) = aw3 − vx φ̇ − b sin(θp )φ̇2

 
cw3 − dvy + sin(θp ) eφ̇2 − f θ̇p2 − g + hvx φ̇
+ (5.3.3)
cos(θp ) + i

where a to i denote time invariant constants, and where 0 < e  {a, b, f, h} 

{c, d, i}  g, in which the operator  denotes a difference of approximately an
order of magnitude. For the prototype’s parameters given in Table 4.2 these

107
Chapter 5 Nonlinear Control

coefficients evaluate to a = 0.03, b = 0.072, c = 0.20, d = 0.13, e = 0.0091,

f = 0.030, g = 9.8, h = 0.038, and i = 0.54. For comparison the same analysis
of coefficients is performed for a taller and heavier system with hp = 1, mp = 20,
Ipx = 20, yielding coefficients of approximate magnitude 0 < h  {d, e} 
{a, f, i}  {b, c}  g. Importantly, while some coefficients change in magnitude
relative to one another, the constant g remains significantly larger than all other
coefficients.

Equation (5.3.3) has no analytical solution for θp . However, arranging it into

the form 0 = f (x, w, v̇y ) and examining f (x, w, v̇y ) for θp = ±π/2 yields
 
a d h
f (x, w, v̇y ) = w3 − vy − v̇y − 1 − φ̇vx
i i i
 e 2 f 2 g
∓ b− φ̇ ∓ θ̇p ∓ for θp = ±π/2 (5.3.4)
i i i

Equation (5.3.3) is a continuous smooth function over the interval

θp ∈ (− cos−1 (−i), cos−1 (−i)) (5.3.5)

where cos−1 (−i) = 2.14 rad for the prototype parameters in Table 4.2. By the
intermediate value theorem as long as for a given {x, w3 , v̇y } ∈ R7 × R × R these
two expressions are of opposite sign, there must exist some intermediate value of
θp for which f (x, w, v̇y ) = 0, i.e. a solution to (5.3.3) must exist. This condition
is necessary for there to exist an inverse function θp = fv̇−1
y
(x, w3 , v̇y ) that can be
used to determine the lean angle required to achieve a given v̇y for some state x
and input w, though the existence of this inverse also requires a unique mapping.
The condition under which at least one solution exists can be written as

aw3 − dvy − iv̇y − (i − h) φ̇vx ≤ (bi − e) φ̇2 + f θ̇p2 + g |θp | ≤ π/2 (5.3.6)

It is apparent that the large constant term g on the rhs means this inequality
is satisfied for a large set of accelerations v̇y and w3 , of which the origin is con-
tained strictly within the interior, provided the φ̇vx and vy terms are not driven
excessively large. Satisfaction of this condition can therefore be guaranteed by
suitably bounding the user reference inputs v̇xr and φ̇r , whilst ensuring controller
selection as to apply a suitable bound to w3 and v̇y . While technically a larger

108
 5.3 Backstepping Control of Local Body Frame Velocities

feasible set could be achieved by allowing |θp | < cos−1 (i), as this requires inter-
section with the pendulum CoM and the ground this bound on θp is sensible, and
simplifies analysis.
It is found that
 
∂ v̇y cos(θp ) −eφ̇2 + f θ̇p2 + g
=− − bφ̇2 cos(θp )
∂θp i + cos(θp )
 
sin(θp )2 −eφ̇2 + f θ̇p2 + g − cw3 + dvy − hφ̇vx
− (5.3.7)
(i + cos(θp ))2

from which it is apparent that < 0 ∀ θp ∈ [− π2 , π2 ] under the condition

∂ v̇y
∂θp

   
cos(θp ) f θ̇p2 + g sin(θp )2 f θ̇p2 + g + dvy
+ bφ̇2 cos(θp ) +
i + cos(θp ) (i + cos(θp ))2
cos(θp )eφ̇2 sin(θp )2 eφ̇2 + cw3 + hφ̇vx
> + (5.3.8)
i + cos(θp ) (i + cos(θp ))2

Again, by virtue of g being very large compared to e, c, and h, this inequality

is satisfied for a large region of states centered about the origin. By ensuring
< 0 (5.1.5) is monotonic in θp , and therefore the solution to fv̇−1 (x, w, v̇y ) is
∂ v̇y
∂θp y

guaranteed to be unique. The inverse function fv̇−1y

(x, w, v̇y ) is therefore guaran-
teed to exist for θp ∈ − π/2, π/2 under conditions (5.3.4) and (5.3.8). fv̇−1
 
y,ss
(x, v̇y )
can be efficiently solved using the Newton-Raphson method with an analytically
derived Jacobian, yielding solutions in 5 µs when executing within MATLAB.
As a demonstration of the typical shape of this function, Figure 5.2 shows v̇y
over θp ∈ [−2.14, 2.14] for values of x and w taken independently from a zero
mean normal distribution of standard deviation 3, using model parameters from
Table 4.2. This shows how a solution of θp for v̇y = 0 does not exist within the
interval (−π/2, π/2) when condition (5.3.4) is violated.
Steady state acceleration v̇y,ss for a given steady state value of θp can be found
by substituting (5.3.2) with w3 = θ̇p = 0, yielding
 
−dvy + sin(θp ) eφ̇2 − g + hvx φ̇
fv̇y,ss (x, w) = −vx φ̇ − b sin(θp )φ̇2 + (5.3.9)
cos(θp ) + i

109
Chapter 5 Nonlinear Control

40

20

v̇y (m s−1 )
0

−20

−40
−π/2 −π/4 0
θp (rad)

Figure 5.2: Forward acceleration v̇y over θp = [−2.14, 2.14] for 20 samples of
x and w3 taken from a zero mean normal distribution of standard deviation 3.
Note how some curves do not intersect v̇y = 0 within θp ∈ [−π/2, π/2], meaning a
steady state value of vy cannot be achieved for these particular choices of x and
w. The monotonicity of (5.3.2) demonstrated in (5.3.7) is clearly visible in all
curves.

with solutions to fv̇−1

y,ss
(x, v̇y ) for v̇y = 0 existing within the set bounded by

−dvy − (i − h) φ̇vx ≤ (bi − e) φ̇2 + g |θp | < π/2 (5.3.10)

From here on fv̇−1

y,ss
(x, v̇y ) shall be written as fv̇−1
y,ss
(v̇y ) for brevity.

Remark 2. Absence of oddness property of fv̇y,ss (x, w) in θp

In Pathak’s [51] backstepping control of a TWIP it is shown that the TWIP’s
expression for steady state acceleration fv̇y,ss (x, w) is odd in θp , such that θp
fv̇y,ss (x, w) ≥ 0 ∀ θp ∈ [−π, π]. This property requires the assumption that φ̇ = 0.
In order for any stability proof that relies on this oddness property to remain valid,
such as that demonstrated by this author, it is therefore necessary for the system
to perform control of φ and vy separately such that φ̇θ̇p = 0. The system in this
thesis is required to perform these movements simultaneously, invalidating this
assumption. Also, the function fv̇y,ss (x, w) for this system contains two significant
even terms vx φ̇. This oddness property therefore does not extend to this system
and so cannot be exploited for Lyapunov function derivation, necessitating a
different approach to that used by Pathak [51].

Remark 3. Velocity equilibria in the local body frame

110
 5.3 Backstepping Control of Local Body Frame Velocities

For this controller it is desired for the system’s local frame body velocities
to converge to some user controlled reference velocities vxr , vyr , and φ̇r , with
no interest in position states other than θp . Defining the reduced state vector
and examining x̃˙ = 0 in (5.1.5) shows these equilibria
 T
x̃ = θp vx vy φ̇ θ̇p
exist at any w = 0, θ̇p = 0, {vx , vy , φ̇} ∈ Ass , where Ass is defined as the set of
{vx , vy , φ̇} for which solutions to fv̇y,ss (x) = 0 exist. A cross-section of this set
is shown in Figure 5.3, taken through vx and φ̇ for vy = 0, with parameters from
Table 4.2. Note while a vy term does feature in fv̇y,ss , it vanishes when friction
is negated and has a very small coefficient, and so does not represent significant
dynamics. This cross-section is therefore relatively invariant in vy , and in reality
a sufficiently large vy φ̇ term would result in rotation of the system about b̂y and
a subsequent loss of traction long before the shape of this figure is significantly
altered.

To summarise, a steady state equilibrium can be obtained for any {vx , vy , φ̇} ∈
Ass , where Ass is a large set centered about the origin. It is therefore feasible for
this controller to achieve {vx , vy , φ̇} → {vxr , vyr , φ̇r } as t → ∞, i.e. asymptotic
tracking of local body frame velocity references is feasible.

With the θp subsystem globally asymptotically stabilised by linear feedback,

the outer loop is required to generate suitable θpr , w1 , and w2 trajectories that
yield asymptotic convergence of (vx , vy , φ̇) → (vxr , vyr , φ̇r ). These subsystems
have substantially slower dynamics than the θp subsystem, allowing the assump-
tion that the linear inner loop has converged, i.e. θp = θpr and θ̇p = w3 = 0. While
the vx and φ̇ subsystems have been rendered linear by feedback linearisation, a
nonlinear controller is still used in order to allow the embedded enforcement of the
constraints |w1 | ≤ w1 and |w2 | ≤ w2 . These constraints act to make the result-
ing control laws favour smooth steady accelerations over aggressive acceleration
impulses during a step reference change, and can therefore be used to alleviate
the risk of wheel slip by acting as an analogue for a wheel torque constraint.

111
 5.3 Backstepping Control of Local Body Frame Velocities

with time derivative

     
2 2
θpr − fv̇−1
y,ss
(0) f˙v̇−1
y,ss
(0) θ p − θ 2
pr + θ̇ pr f −1
v̇y,ss (0)θ pr − θ p
V̇Σ =  2
2 2
θp − θpr

− Kv (w1 (vxr − vx ) + v̇y (vyr − vy )) − Kφ̇ w2 (φ̇r − φ̇)

w1 ẇ1 w2 ẇ2
+
2 + 2 2 (5.3.12)
w21 − w12 φ̈ − w22

The first term of (5.3.11) has a unique minimum at fv̇−1

y,ss
(0) = θpr , i.e. it is
minimised when θpr has converged to the steady-state lean angle θp,ss required
to maintain v̇y = 0, found by solution of θp,ss = fv̇−1
y,ss
(0), whilst tending to infinity
as θpr → ±θpr , meaning θpr remains bounded. This barrier function is exampled
in Figure 5.4. The second term of (5.3.12) has a unique minimum at vy = vyr ,
vx = vxr , with a quadratic cost on deviation from this minimum. Similarly, the
third term imposes a quadratic cost on deviation of φ̇ from φ̇r . The final two terms
act as barrier functions to enforce |w1 | ≤ w1 and |w2 | ≤ w2 , with minimums at
w1 = 0 and w2 = 0. VΣ is therefore globally positive semidefinite when constraints
are satisfied and Kv , Kφ̇ > 0, i.e. VΣ ≥ 0, has a single unique minimum, and

by inspection is radially unbounded for states within the constrained set, but
is bounded for θpr → ∞ as the first term of (5.3.11) converges to 1. As these
conditions are not met for states outside of the constraints, care must be taken
to initialise the system with constraints satisfied, i.e. the controller is not able
to recover from a constraint violation. However, as the constrained signals exist
purely internally this will never occur. The inverse function f˙−1 (0) in (5.3.12) v̇y,ss
can be calculated as
dfv̇y,ss
f˙v̇−1 (0) = (5.3.13)
y,ss
dt θp =fv̇−1 (0)
y,ss

This represents the rate of change in the target steady state lean angle θp,ss due
to time variation of vx , vy , and φ̇.

113
Chapter 5 Nonlinear Control

10

V (θpr )
5

−0.8 −0.4 0 0.4 0.8

θpr (rad)
 2
θpr −fv̇−1 (0)
Figure 5.4: Barrier function V (θpr ) =  2 y,ss
2
 , with θp = 0.8 rad
2 θp −θpr
(dashed, vertical) and fv̇−1y,ss
(0) = 0.4 rad. Note the single unique minimum at
V (fv̇y,ss (0)) = 0, and how V (θpr ) → ∞ as |θpr | → θpr .
−1

Consider the control laws

 
2
−f˙v̇−1
y,ss
(0) θ p − θ 2
pr  
2

θ̇pr =   − Kr θpr − fv̇−1 (0) f −1
v̇ (0)θpr − θ p
2 y,ss y,ss
fv̇−1
y,ss
(0)θpr − θp
 2
2 2
θp − θpr Kv fv̇y,ss (θpr )(vyr − vy )
+   (5.3.14)
−1 −1 2
θpr − fv̇y,ss (0) fv̇y,ss (0)θpr − θp

(5.3.15)

2
ẇ1 = −Kw1 w1 + Kv (vxr − vx ) w21 − w12
(5.3.16)

2
ẇ2 = −Kw2 w2 + Kφ̇ (φ̇r − φ̇) w22 − w22

Substituting (5.3.14)-(5.3.16) into (5.3.12) yields

2 
2 2
 
−Kr θpr − fv̇−1
y,ss
(0) f −1
v̇y,ss (0)θpr − θ p
V̇Σ =  2
2 2
θp − θpr
−Kw1 w12 −Kw2 w22
+ 2 +
2 (5.3.17)
w21 − w12 w22 − w22

114
 5.3 Backstepping Control of Local Body Frame Velocities

from which it is clear that V̇Σ ≤ 0 for (Kr , Kw1 , Kw2 ) > 0, thus proving closed-
loop stability. This stability proof does, however, require {vxr , vyr , φ̇r } ∈ Ass ,
x0 ∈ Ass , and x ∈ Ass : |θp | ≤ θp ∀ t. While the first two conditions can


be trivially ensured, the latter cannot be guaranteed, as any overshoot when

approaching references that require θp,ss to lie close to θp could violate this con-
dition. This can be addressed by bounding the solution to fv̇−1
y,ss
(0) to lie within
constraint bounds. Using LaSalle’s invariance principle it is apparent that

θ = fv̇−1 (0) =⇒ vy = vyr
 pr

 y,ss

lim w1 = 0 =⇒ vx = vxr (5.3.18)

t→∞ 


w = 0 =⇒ φ̇ = φ̇
2 r

thus guaranteeing asymptotic convergence to the desired references.

The dynamics of the controller can be tuned by modification of the ‘damping’

terms Kr , Kw1 , and Kw2 , and ‘proportional’ terms Kv and Kφ̇ . Convergence of
the expression
fv̇y,ss (θpr )
lim   (5.3.19)
θpr →fv̇−1
y,ss
(0) fv̇−1
y,ss
(0) − θpr

in the latter term of (5.3.14) cannot be directly determined, as performing the

substitution θpr = fv̇−1
y,ss
(0) yields the indeterminate expression

fv̇y,ss (θpr ) fv̇y,ss (fv̇−1 (0)) 0

lim  = y,ss
= (5.3.20)
θpr →fv̇−1 (0) fv̇−1 (0) − θpr fv̇−1 (0) − fv̇−1 (0) 0
y,ss y,ss y,ss y,ss

However, as these functions are known to be continuously differentiable within

the region of interest, convergence can instead be determined by L’Hôpital’s rule
as
 df 
v̇y,ss (θpr )
fv̇y,ss (θpr ) dθpr
lim  = lim  ! (5.3.21)
θpr →fv̇−1 fv̇−1 θpr →fv̇−1 −1
y,ss
(0)
y,ss
(0) − θpr y,ss
(0) d fv̇y,ss (0)−θpr
dθpr

The inverse function fv̇−1

y,ss
(0) is independent of θpr as fv̇−1
y,ss
(0) = θp,ss and θpr 6≡

115
Chapter 5 Nonlinear Control

dfv̇−1 (0)
θp,ss , so = 0, and clearly − dθpr = −1, therefore
y,ss dθ
dθpr pr

 df 
v̇y,ss (θpr )
dθpr dfv̇y,ss (θpr )
lim ! = lim −
dθpr

θpr →fv̇−1
y,ss
(0) −1
d fv̇y,ss (0)−θpr θpr →fv̇−1
y,ss
(0)
dθpr (5.3.22)
dfv̇y,ss (θpr )
=−
dθpr θpr =fv̇−1 (0)
y,ss

taking the derivative of (5.3.9) w.r.t. θp yields

   
dfv̇y,ss (θpr ) g − φ̇2 e + cos (θpr ) gi − φ̇2 ei + sin(θpr ) dvy − φ̇hvx
=−
dθpr (i + cos (θpr ))2
− bφ̇2 cos (θpr ) (5.3.23)

which is clearly a smooth function over the usual interval θpr ∈ − cos−1 (−i),
cos−1 (−i) , therefore (5.3.19) is convergent as θpr → fv̇−1 (0), and thus the


y,ss

control law (5.3.14) remains defined.

Finally, all that remains to be proven is that (5.3.19) does not converge to zero
within the operating region of interest, as if this were the case the third term of
(5.3.14) would vanish at θpr = fv̇−1
y,ss
(0), even if vy 6= vyr . As the first term of this
control law vanishes when w1 = w2 = 0, and the second term also vanishes when
θpr = fv̇−1
y,ss
(0), this would force θ̇pr = 0 ∀ t → ∞, and thus prevent any further
control action even when vy =
6 vyr .

Ideally this would be performed by calculating the Hessian matrix of (5.3.23)

w.r.t. θpr vx vy φ̇ , then by examining the eigenvalues of this matrix prov-
 T

ing that (5.3.23) is either positive or negative definite. This would ensure that
(5.3.23) never evaluates to zero, avoiding the above problem.

Unfortunately, these eigenvalues when represented symbolically are too com-

plex for this analysis to be performed, so a less rigorous approach must be used.
For the vx = vy = φ̇ = 0 case (5.3.23) simplifies to

dfv̇y,ss (θpr ) g(1 + i cos (θpr )

=− (5.3.24)
dθpr (i + cos(θpr ))2

116
 5.3 Backstepping Control of Local Body Frame Velocities

−5
dfv̇y,ss (θpr )
dθpr

−10

−15

−20
−π/2 −π/4 0 π/4 π/2

θpr (rad)

Figure 5.5: Plot of (5.3.23) over θpr ∈ [− π2 , π2 ] for 1000 uniformly random samples
{vx , vy , φ̇} ∈ Ass . This shows that in practice this function appears to be negative
definite for |θp | / 1.2 rad, {vx , vy , φ̇} ∈ Ass

which is clearly negative definite for θpr ∈ [− π2 , π2 ], indicating that the origin is
contained within the unknown set of {vx , vy , φ̇} for which this condition holds.
The effect of nonzero vx , vy , and φ̇ is examined by evaluating (5.3.23) over θpr ∈
[− π2 , π2 ] for 1000 uniformly random samples {vx , vy , φ̇} ∈ Ass , shown in Figure
5.5. This shows that in practice this function appears to be negative definite for
|θp | / 1.2 rad. This controller therefore remains well defined for all {vx , vy , φ̇} ∈
Ass , |θp | / 1.2 rad. This is a tighter bound on θp than found previously, but still
far larger than is expected to be attained in practice.
As this stability proof relies on the assumption of prior convergence of the
inner θp → θpr control loop, update of the control law (5.3.14) should be avoided
when |θp − θpr |  0. This can be achieved by multiplication of (5.3.14) by the
expression
e−K|θp −θpr | (5.3.25)

where K  1, and where e is redefined as the natural logarithm. This prevents

substantial change of θpr when the inner loop is still converging.
Figure 5.6 shows the phase portrait of (5.3.14), showing convergence from a
number of initial states to the desired vyr reference and non-zero solution to
fv̇−1
y,ss
(0), whilst satisfying the |θp | < θp constraint.

Remark 4. Boundedness of internal dynamics

From (5.1.11) z6 is a linear combination of states x5 to x8 . As these states are

117
 5.3 Backstepping Control of Local Body Frame Velocities

1 w1
10 w2
0.5
5
vx
0 vy 0
0 1 2 3 4 5 0 1 2 3 4 5

4 0 θp
θpr
−0.2 fv̇−1 (0)
2 yss

−0.4
0 φ̇
−0.6
0 1 2 3 4 5 0 1 2 3 4 5

Figure 5.7: Simulated system state trajectories for a reference (vxr , vyr , φ̇r ) =
(1, 1, 4), initialised at the origin with θp = 0.6, w1 = 2, and w2 = 4. This
shows asymptotic convergence to the reference whilst satisfying θp , w1 , and w2
constraints, with θp correctly converging to the required steady state θp = θp,ss =
fv̇−1
yss
(0). The error θp − θpr remains small, indicating that (5.3.25) functions as
intended and thus the assumption of convergence of this inner loop holds, with
full convergence achieved in steady state.

will always be imperfect and therefore not fully converge. This manifests as a
steady state tracking error θe , i.e. θp → θpr + θe , which due to the propor-
tional feedback term in this controller results in a non-zero steady state w3 , i.e.
6 0. This invalidates the assumption in (5.3.9), yielding the steady
w3 → Kθp θe =
state bias in the solution to fv̇−1
y,ss
, visible in this figure. A significant steady-state
tracking error is visible in the vx → vxr controller, and w1 6→ 0. This again
indicates an error in the feedback linearisation, as while w1 6= 0 the velocity vx
reaches a steady state. This could be addressed by improved friction modelling
in the underlying model, as to predict this force resisting w1 in steady state, or
by some form of integral action.

119
Chapter 5 Nonlinear Control

0.8
vx 10 w1
0.6 vy w2
0.4
5
0.2
0
0
−0.2
0 2 4 6 8 10 0 2 4 6 8 10

0.1
2 θp
0 θpr
1 fv̇−1
y,ss
(0)
−0.1
0 φ̇
−0.2
0 2 4 6 8 10 0 2 4 6 8 10

Figure 5.8: Experimental system state trajectories for a reference (vxr , vyr , φ̇r ) =
(1, 0, 2), initialised at the origin with θp = 0.4, w1 = 3, and w2 = 15. This shows
good tracking of θp → θpr , however, now θpr 6→ fv̇−1 y,ss
(0). This is found to be due
to imperfect tracking within the inner loop yielding a steady state bias in w3 ,
which is in turn due to imperfect feedback lineariisation. A combination of this
and further model error yields a steady state tracking error of the vxr and vyr
references, though in reality this is visually imperceptible.

Figure 5.9: A strobe-effect image of the trajectory shown in Figure 5.8.

120
 5.4 Backstepping Inertial Frame Velocity Control

5.4 Backstepping Inertial Frame Velocity Control

Control of inertial frame velocities ẋ and ẏ is more useful in applications that

involve the autonomous navigation of an environment. The desired steady state
body accelerations are now defined as v̇x = φ̇vy and v̇y = −φ̇vx , representing
unforced body acceleration due to the mapping of inertial frame velocities into
the rotating local frame.

Remark 5. Inertial frame velocity equilibria

For an inertial frame velocity controller it is desired that the inertial frame body
velocities {ẋ, ẏ, φ̇} asymptotically converge to the references {ẋr , ẏr , φ̇r } within
finite time. In steady state the local frame body accelerations must therefore be
purely that due to rotation of inertial frame velocities into the local body frame,
i.e. v̇x = φ̇vy , v̇y = −φ̇vx , and local frame body velocities must be simply a rotation
of the time invariant inertial frame velocity reference into the local frame, so
" # " # " #
vx ẋ cos(φ)ẋ + sin(φ)ẏ
T
= REB = (5.4.1)
vy ẏ − sin(φ)ẋ + cos(φ)ẏ

In steady state it is therefore required that ẋ = ẋr and ẏ = ẏr , which for time
invariant references implies (ẍ, ÿ) → 0. It is also required that φ̇ = φ̇r , so
φ = φ̇r t + φ0 in steady state.

Expressing inertial frame body accelerations in terms of inertial frame velocities

and local body frame acceleration v̇y , and performing the above substitutions, yields
" # " # " #
ẍ 0 − sin(φ̇r t)(v̇y + φ̇r ẋr cos(φ̇r t) + φ̇r ẏr sin(φ̇r t))
= = (5.4.2)
ÿ 0 cos(φ̇r t)(v̇y + φ̇r ẋr cos(φ̇r t) + φ̇r ẏr sin(φ̇r t))

Substituting v̇y in (5.4.2) with (5.3.3), solving again for ẍ ÿ , and equating
 T

to zero as required in steady state yields

 
− sin(φ̇r t)Γ(t)
" # " #
ẍ 0
= =  i+cos(θp ) 
cos(φ̇r t)Γ(t)
(5.4.3)
ÿ 0
i+cos(θp )

121
Chapter 5 Nonlinear Control

where
 
Γ(t) = w3 (c + ai + a cos(θp )) − sin(θp ) g − f θ̇p2 + φ̇2r (e − bi − b cos(θp ))

+ φ̇r ẏr h sin(φ̇r t) − ẏr d cos(φ̇r t) + φ̇r ẋr h cos(φ̇r t) + ẋr d sin(φ̇r t) (5.4.4)

For φ̇r =
6 0 these equalities clearly require Γ(t) = 0, which allows a unique
solution for the required w3 dynamics as

1  
w3 = sin(θp ) g + f θ̇p2 + φ̇2r (bi − e + b cos(θp ))
c + a cos(θp ) + ai
!
   
+ cos(φ̇r t) −φ̇r ẋr h + ẏr d + sin(φ̇r t) −ẋr d − φ̇r ẏr h (5.4.5)

For the parameter choices in this thesis (bi − e)  0, so

 
sin(θp ) g + φ̇2r b cos(θp ) + f θ̇p2 + φ̇2r (bi − e)
(5.4.6)
c + a cos(θp ) + ai

is an odd function within a neighbourhood of the origin for up to much larger

values of φ̇r than are expected to be encountered. Any deviation of θp from 0 will
result in a similarly signed w3 , making θp= 0 an unstable
 equilibriumfor this part
of (5.4.5). The cos(φ̇r t) −φ̇r ẋr h + ẏr d + sin(φ̇r t) −ẋr d − φ̇r ẏr h expression
is time varying when φ̇r 6= 0, which acts to perturb (5.4.6).
These dynamics can therefore only be stabilised if this is achieved by the
   
cos(φ̇r t) −φ̇r ẋr h + ẏr d + sin(φ̇r t) −ẋr d − φ̇r ẏr h

term, which given that this expression is invariant in θp cannot be the case. An
unstable equilibrium can be achieved for the φ̇r = 0 case, as this makes the latter
expression time invariant, allowing a time invariant solution for w3 . The overall
dynamics are therefore unstable for θp ∈ [− π2 , π2 ], meaning that for ẍ = ÿ = 0
to be maintained when φ̇r =
6 0 the system is is forced to violate the constraint
|θp | < π2 , making the asymptotic tracking of constant ẋr and ẏr trajectories with
a time varying heading impossible.

As from Remark 5 no states satisfying kvkφ̇ =

6 0 represent equilibria, asymp-

122
 5.4 Backstepping Inertial Frame Velocity Control

totic tracking of constant inertial frame velocity references is not possible, and
thus a degree of tracking error is to be expected. As in the body frame velocity
controller it is desired that θp → fv̇−1
y,ss
(−vx φ̇), so a similar energy function to that
in (5.3.11) can be used, though now this will never be perfectly tracked in steady
state. Quadratic energy functions are defined with unique minimums at ẋ = ẋr
and ẏ = ẏr . However, as these minimums no longer represent equilibria of the
system it is expected that the controlled system will follow a periodic trajectory
about these references in steady state, with the characteristics of this limit cycle
tunable by manipulation of control gains. Identical energy functions to that in
(5.3.11) are used to describe a quadratic cost on |φ̇ − φ̇r | and a barrier function
on w2 . A similar barrier function is used to constrain w1 . As the purpose of
this barrier is to constrain wheel torque demands, it makes more sense in this
application to only apply the barrier to forced body acceleration, rather than
also constraining acceleration due to rotation of inertial frame velocities into the
local body frame. The barrier function is therefore chosen to instead enforce
w1 − vy φ̇ < v xf .

These new constraints and quadratic reference tracking costs can be captured
by the Lyapunov function candidate

 2
θpr − fv̇−1 (−vx φ̇) Kv (ẋr − ẋ)2 + (ẏr − ẏ)2 Kφ̇ (φ̇r − φ̇)2


y,ss
VΣ = 
2
 + +
2 θp − θpr 2 2 2

(w1 − φ̇vy )2 1
+  2 +   (5.4.7)
2 v̇ xf − (w1 − φ̇vy )2 2 w22 − w22

with time derivative

   
2 2
f˙v̇−1 2 + θ̇ −1 −1


y,ss
(−vx φ̇) θ p − θ pr pr f v̇y,ss (−v x φ̇)θ pr − θ p θ pr − f v̇y,ss (−vx φ̇)
V̇Σ =  2
2 2
θp − θpr


− Kv ẍ(ẋr − ẋ) + ÿ(ẏr − ẏ) − Kφ̇ w2 (φ̇r − φ̇)
 
2 dφ̇v
v̇ xf (φ̇vy − w1 ) dt y − ẇ1 w2 ẇ2
+ 2  2 + 2
2

2 (5.4.8)
v̇ xf + φ̇vy − w1 v̇ xf − φ̇vy + w1 φ̈ − w2

123
Chapter 5 Nonlinear Control

Substituting inertial frame accelerations (ẍ, ÿ) and velocities (ẋ, ẏ) for their body
frame equivalents using
" # " # " #
ẋr vxr cos(φ)vxr − sin(φ)vyr
= REB = (5.4.9)
ẏr vyr sin(φ)vxr + cos(φ)vyr
" # " # " #
ẋ vx cos(φ)vx − sin(φ)vy
= REB = (5.4.10)
ẏ vy sin(φ)vx + cos(φ)vy
" # " #! " #
ẍ d vx (v̇x − vy φ̇) cos(φ) − (v̇y + vx φ̇) sin(φ)
= REB = (5.4.11)
ÿ dt vy (v̇x − vy φ̇) sin(φ) − (v̇y + vx φ̇) cos(φ)

allows (5.4.8) to be rewritten as

   
2 2
f˙v̇−1 2 + θ̇ −1 −1


y,ss
(−vx φ̇) θ p − θ pr pr fv̇y,ss (−v x φ̇)θ pr − θ p θ pr − fv̇y,ss (−v x φ̇)
V̇Σ =  2
2 2
θp − θpr
w2 ẇ2

+
− Kv (w1 − vy φ̇)(vxr − vx ) + (fv̇y,ss (θpr ) + vx φ̇)(vyr − vy )
2 2
w22 − w2
 
2 dφ̇v
v̇ xf (φ̇vy − w1 ) dt y − ẇ1
− Kφ̇ w2 (φ̇r − φ̇) +  2  2 (5.4.12)
v̇ x,f + φ̇vy − w1 v̇ x,f − φ̇vy + w1

Substituting the control laws

 
2
−f˙v̇−1
y,ss
(−vx φ̇) θ p − θ 2
pr
θ̇pr =  2

fv̇−1
y,ss
(−v x φ̇)θ pr − θ p
  
2
− Kr θpr − fv̇−1 y,ss
(−v x φ̇) f −1
v̇y,ss (−v x φ̇)θ pr − θ p
 2
2 2
θp − θpr Kv (fv̇yss (θpr ) + vx φ̇)(vyr − vy )
+ 
2
 (5.4.13)
θpr − fv̇−1y,ss
(−v x φ̇) f −1
v̇y,ss (−v x φ̇)θ pr − θ p

124
 5.4 Backstepping Inertial Frame Velocity Control

ẇ1 = φ̇fv̇yss (θpr ) + vy w2 − Kw1 (w1 − φ̇vy )

 2  2
Kv (vxr − vx ) v̇ xf − φ̇vy + w1 v̇ xf + φ̇vy − w1
+ 2 (5.4.14)
v̇ xf
 2
ẇ2 = −Kw2 w2 + Kφ̇ (φ̇r − φ̇) w22 − w22 (5.4.15)

into (5.4.12) yields

2 
2 2
 
−Kr θpr − fv̇−1
y,ss
(−vx φ̇) f −1
v̇y,ss (−vx φ̇)θ pr − θ p −Kw2 w22
V̇Σ = 2 +
2
w22 − w22

2 2
θp − θpr
2
−Kw1 v̇ xf (w1 − φ̇vy )2
+ 2  2 (5.4.16)
v̇ xf + φ̇vy − w1 v̇ xf − φ̇vy + w1

from which it is clear that V̇Σ ≤ 0 ∀ {Kr , Kw1 , Kw2 } > 0, thus proving stability
under the assumption that non-zero inertial frame velocities are attainable in
6 0. As from Remark 5 this is not possible, this stability
steady state while φ̇ =
proof is invalidated, though as the necessary resulting limit cycle is expected
to be small it is assumed that this stability proof is still relevant to some de-
gree. Control gains are tuned as to achieve a desirable trade-off between control
performance and minimisation of this periodic error trajectory.
As in the body velocity controller this controller also relies on the assumption
θp = θpr , so update of the control is slowed by multiplying the second and third
terms of (5.4.13) by
e−K|θpr −θp | K1 (5.4.17)

such that the control law is slowed when the inner loop has not converged, but
without affecting the first term of (5.4.13) that is required to feedforward a nec-
essary variation in θp due to rotation of inertial frame velocities into the local
body frame.

Remark 6. Boundedness of internal dynamics

As in Remark 4, by controlling states x5 to x8 , z6 , which is a linear combination
of these, remains bounded. Again, z1 and z2 are not controlled and are expected
to grow unboundedly.

125
Chapter 5 Nonlinear Control

1.5 4
w1 − vx φ̇
w2
1
2
0.5
ẋ
0
0 ẏ
0 2 4 6 0 2 4 6

6 0
4
−0.1 θp
2 θpr
φ̇ fv̇−1
yss
0 −0.2
0 2 4 6 0 2 4 6

Figure 5.10: Simulated system state trajectories for a reference (ẋr , ẏr , φ̇r ) =
(1, 1, 6), with the system initialised at the origin and with θp = 0.4, v̇ xf = 2, and
w2 = 4. w1 − vx φ̇ is shown rather than w1 , as this represents acceleration in the
vx subsystem not due to rotation, and is the value that is constrained. θp is seen
to converge towards fv̇−1yss
(0), though a small tracking error is now observed. This
is due to the infeasibility of asymptotically tracking inertial velocity references,
as well as the now invalid assumption of θp converging to a constant value, i.e.
now θ̇p =6 0, w3 6= 0 in steady state. ẋ and ẏ are seen to converge to a small limit
cycle about the target reference.

Figure 5.10 shows the simulated response of the controlled system to a reference
(ẋr , ẏr , φ̇r ) = (1, 1, 6) with constraints θp = 0.4, v̇ xf = 2, and w2 = 4, with the
system initialised at the origin. This demonstrates convergence to an acceptable
velocity trajectory limit cycle with an RMS error of 4.1%, and satisfaction of
the constraints |θpr | < θpr , |w1 − vx φ̇| < v̇ xf , and |w2 | < w2 . In steady state
θp is seen to closely track fv̇−1
y,ss
(−vx φ̇). Controller parameters are selected to
best demonstrate the controller; more aggressive gains can obtain faster tracking
without significantly altering the limit cycle.
Figure 5.11 shows the experimental response of the prototype to a reference
(ẋr , ẏr , φ̇r ) = (0, 1, 3). Wheel traction limitations necessitate a less aggressive

126
 5.4 Backstepping Inertial Frame Velocity Control

15
1 w1 − vx φ̇
10 w2
0.5
0 5

−0.5 ẋ 0
ẏ
−1 −5
0 2 4 6 8 0 2 4 6 8
t(s) t(s)
4
θp
θpr
0.2
2 fv̇−1
yss
(−vx φ̇)

0 0
φ̇
0 2 4 6 8 0 2 4 6 8
t(s) t(s)

Figure 5.11: Experimental system state trajectories for a reference (ẋr , ẏr , φ̇r ) =
(0, 1, 3), with the system initialised at the origin and with θp = 0.4, v̇ xf = 2, and
w2 = 15. w1 − vx φ̇ is shown rather than w1 , as this represents acceleration in the
vx subsystem not due to rotation, and is the value that is constrained. RMS ẋ
and ẏ tracking errors of 4.2% and 7.1% are visible, due to a combination of the
infeasibility of perfect tracking, imperfect feedback linearisation, and modelling
error in the outer control laws.

reference than that in Figure 5.10. This experiment highlights a weakness in this
controller; just as selection of controller gains affects the system’s resulting limit
cycle, this is also influenced by imperfect feedback linearisation and modelling
error in the control laws, yielding larger periodic velocity tracking errors, with
RMS errors of 10.8% and 7.8% respectively. From observation it is believed that
the main influencing unmodelled dynamic is related to friction in the Mecanum
wheel rollers, which in practise will not be perfectly modelled by the linear friction
models used in this thesis. Figure 5.12 uses a long exposure image to demonstrate
this experiment.

127
Chapter 5 Nonlinear Control

Figure 5.12: A long exposure image of the trajectory in Figure 5.11, in which
two blue LEDs are used to capture the experimentally tracked path.

5.5 Backstepping Global Position Control

With system inertial frame velocities successfully controlled it is relatively straight-

forward to design a controller capable of generating (ẋr , ẏr , φ̇r ) reference trajec-
tories that drive the system to some arbitrary position in the inertial frame
(pxr , pyr , pφr ). This must be performed whilst enforcing a velocity constraint
in order to bound the system’s kinetic energy as to generate safe velocity tra-
jectories. Such a controller is significant, as this allows the system to perform
point-to-point translations in its environment, and is therefore a prerequisite for
autonomous navigation between waypoints.

Consider the candidate Lyapunov function

Kp (pxr − x)2 + (pyr − y)2 Kφ (pφr − φ)2

VΣ = +
2 2
1 1
+ + (5.5.1)
2(v 2 − ẋ2r − ẏr2 ) 2
2(φ̇ − φ̇2r )

128
 5.5 Backstepping Global Position Control

with time derivative

ẋr ẍr + ẏ ÿr

V̇Σ = −Kp (ẋr (pxr − x) + ẏr (pyr − y)) +
2
v2 − ẋ2r − ẏr2
φ̇r φ̈r
− Kφ φ̇r (pφr − φ) + 2 (5.5.2)
(φ̇2r − φ̇r )2

in which convergence of the lower velocity controller is assumed such that ẋ = ẋr ,
ẏ = ẏr , φ̇ = φ̇r . The first two terms of (5.5.1) define a cost quadratic in position
error, the third term forms a barrier function enforcing the constraint ẋ2r +ẏr2 < v 2 ,
with a unique minimum at ẋr = ẏr = 0, and the last term enforces |φ̇| < φ̇r with
a unique minimum at φ̇r = 0.

Substituting the control laws

(5.5.3)

2
ẍr = −Kvr ẋr + v 2 − ẋ2r − ẏr2 Kp (pxr − x)
(5.5.4)

2
ÿr = −Kvr ẏr + v 2 − ẋ2r − ẏr2 Kp (pyr − y)
2
φ̈r = −Kφ̇ φ̇r + (φ̇2r − φ̇r )2 Kφ (pφr − φ) (5.5.5)

into (5.5.2) yields

−Kvr ẋ2r + ẏr2 −Kφ̇r φ̇2r


V̇Σ =
2 + 2
2 (5.5.6)
v 2 − ẋ2r − ẏr2 φ̇2r − φ̇r

from which is it clear that V̇Σ ≤ 0 ∀ {Kvr , Kφ̇r } > 0, and that V̇Σ = 0 has
a unique solution at the desired steady state, proving stability. Similar to the
velocity controller, update of each control law is slowed by multiplication with
terms of the form
e−K|ẋr −ẋ| K1 (5.5.7)

so that the assumption of lower loop convergence holds.

Figure 5.13 shows the simulated response of the above controller to the ref-
erence (pxr , pyr , pφr ) = (2, 2, 2π), with the system initialised at the origin and
with θp = 0.6, v = 1, v̇ xf = 2, and w2 = 4. w1 − vx φ̇ is shown rather than
w1 , as this represents acceleration in the vx subsystem exclusive of that due to
rotation; it is this value that is constrained by the lower velocity controller. This
demonstrates asymptotic position reference tracking with minimal overshoot, and

129
Chapter 5 Nonlinear Control

sensible smooth velocity trajectories that satisfy constraints.

Figure 5.14 shows the experimental response of the prototype to exactly the
same reference trajectories with identical control gains. This results in the sys-
tem tracking a nearly identical position trajectory, though now there is more
disturbance in the ẋ and ẏ states due to kvkφ̇ =
6 0. Similarly, a more aggres-
sive θp trajectory is required than in simulation to counter this deviation from
the velocity reference. Again, all constraints are satisfied, and all state and in-
put trajectories evolve as expected. A long exposure image demonstrating this
controller is shown in Figure 5.15, in which two LEDs are used to capture the
resulting tracked path.

5.6 Conclusion
This chapter has demonstrated a novel partial feedback linearisation of the CMD,
transforming its dynamics from a system of six nonlinear and two linear ODEs
to a system of three nonlinear and five linear ODEs. Backstepping control design
is then used to create novel constrained local frame body velocity, inertial frame
body velocity, and inertial position controllers, all with Lyapunov derived stabil-
ity guarantees. While these stability proofs are not robust to model uncertainty,
in practise this controller is found to exhibit significantly robust behaviour, and
so it is expected that with more work a robust controller and stability proof
could also be derived. These controllers have been demonstrated both in simu-
lation and experimentally on a CMD prototype, with their performance in each
evaluated.

130
 5.6 Conclusion

15
x
2 y
10
1
5
0 0 φ
0 2 4 6 8 10 0 2 4 6 8 10
·10−2
5 1
ẏ
ẏr
0
0.5

−5 ẋ
ẋr 0

0 2 4 6 8 10 0 2 4 6 8 10

6 0.8
φ̇ kvr k
4 0.6
φ̇r
2 0.4

0 0.2
−2 0
0 2 4 6 8 10 0 2 4 6 8 10
·10−2
10
w1 − vx φ̇
5 w2
5
0
0
−5 θp
θpr
−5
0 2 4 6 8 10 0 2 4 6 8 10

Figure 5.13: Simulated system state trajectories for a reference (xr , yr , φr ) =

(2, 2, 2π), initialised at the origin with θp = 0.6, v = 1, v̇ xf = 3, and w2 =
15. w1 − vx φ̇ is shown rather than w1 , as this represents acceleration in the vx
subsystem exclusive of that due to rotation of the body frame, and is the value
that is constrained. Convergence is slowed to better demonstrate asymptotic
stability.

131
Chapter 5 Nonlinear Control

x
2 y 10

1
5
0 φ
0
0 2 4 6 0 2 4 6

1
ẋ 1 ẏ
0.5 ẋr ẏr
0 0.5

−0.5
0
−1
0 2 4 6 0 2 4 6

6 0.8
φ̇ kvr k
0.6
4 φ̇r
0.4
2
0.2
0 0
0 2 4 6 0 2 4 6

0.2
10 w1 − vx φ̇
0.1 w2
5
0
0
−0.1 θp
θpr −5
−0.2
0 2 4 6 0 2 4 6

Figure 5.14: Experimental system state trajectories for reference (xr , yr , φr ) =

(0, 2, 4π), initialised at the origin with θp = 0.4, v = 1, v̇ xf = 3, w2 = 15, v = 1,
and φ̇ = 6. w1 − vx φ̇ is shown rather than w1 , as this represents acceleration in
the vx subsystem not due to rotation, and is the value that is constrained. Small
steady state errors in the tracking of ẋr and ẏr are due to the presence of static
friction in the real-world system and a lack of integral action in the controller.

132
 5.6 Conclusion

Figure 5.15: A long exposure image capturing a trajectory from the origin to
(xr , yr , φr ) = (1, 2, 4π), in which two blue LEDs are used to capture the experi-
mentally tracked path, progressing from the bottom to top of the figure.

133
Chapter 6

Model Predictive Control

This chapter describes the development of the second CMD controller presented
in this thesis, an input and output constrained dual-mode model predictive po-
sition controller. Like the backstepping controllers developed in the previous
chapter, this controller is able to enforce lean angle and velocity constraints,
whilst guaranteeing stability and convergence. However, this controller enforces
these constraints in an optimal rather than asymptotic fashion, allowing for the
generation of closer to time-optimal trajectories. This controller is also able to
optimally enforce wheel torque constraints, providing a guarantee of wheel slip
prevention for a given degree of traction whilst maintaining stability during actu-
ator saturation. In a further advantage over the previous chapter, this controller
is able to make optimal use of advance reference trajectory knowledge, allowing
for improved tracking of complex Cartesian paths. The controller is demonstrated
in both simulation and on a CMD experimental prototype, and its performance
and shortcomings are evaluated. This chapter marks the first implementation of
such a controller on a mobile balancing robot of any wheel configuration, and
is therefore novel in a more general sense than just to the CMD on which it is
applied.

As shown in Chapter 3, a Collinear Mecanum Drive requires larger wheel

torques to generate motion than a similar TWIP, especially when accelerating
along the b̂x direction, due to both a required cancelling of body forces parallel to
b̂y and the addition of substantial friction in the wheel roller bearings. In prac-
tice, this means that typical desired accelerations along b̂x often require wheel
torques that are close to overcoming the tractive friction force between the roller

135
Chapter 6 Model Predictive Control

surface and the ground, i.e. torques that could cause wheel slip, especially when
simultaneously accelerating in other dimensions. As slip is likely to result in in-
stability or loss of control, a hard constraint must be observed on the magnitude
of tractive force required at the roller-ground interface. Assuming a constant
coefficient of static friction, this can be achieved by enforcing a hard constraint
on the torque generated by each wheel motor. Here a ‘hard’ constraint refers
to a constraint for which no violation is acceptable, as opposed to a ‘soft’ con-
straint, which may be violated during disturbance, and after which the controller
is required to re-establish constraint satisfaction and resume tracking. While the
backstepping controllers developed in Chapter 5 incorporated hard constraints
on body accelerations v̇x , φ̈, and θ̈p , these only act to approximately constrain
wheel torques, and therefore in order to avoid wheel slip these constraints must
be sized such that slip does not occur for the worst case combination of constraint
satisfying accelerations. These constraints are therefore conservative for the vast
majority of the constrained acceleration set. Furthermore, the enforcement of
acceleration and velocity constraints in Chapter 5 is performed in an asymptotic
rather than exact manner, meaning the resulting state trajectories are always
suboptimal w.r.t. the constraints. In practice it would instead be desirable if
the system’s inputs could be saturated for long durations in order to sustain the
greatest accelerations possible, and if the system could make maximum use of
its constrained accessible velocity set in order to achieve the quickest possible
translations. This must all be achieved without detrimentally affecting system
stability, as would often occur in a rudimentary controller when its actuators
saturate.

Rather than attempting to define a fixed control law such that the system’s
input and state trajectories iteratively evolve in a desirable manner, as demon-
strated in Chapter 5, these trajectories can instead be calculated in an optimal
fashion by minimisation of a suitable cost function subject to some constraints,
an approach referred to as model predictive control (MPC).

Model predictive control typically uses a model of the plant to predict the
system’s response to a piecewise constant sequence of future control moves over
a receding horizon, which can be optimally chosen to reduce a cost function that
captures the controller’s desired performance over a receding or infinite prediction
horizon. This online optimisation allows for the systematic and exact handling

136
of constraints, making MPC well suited to this application. However, these ap-
proaches are computationally demanding due to their need to numerically solve
a constrained minimisation problem for every control iteration, making their
real-time implementation on systems with fast unstable dynamics such as the
TWIP or CMD challenging. The complexity of this minimisation problem de-
pends heavily on the prediction model and constraint structure; those with linear
prediction models and constraints can usually be formulated as convex quadratic
programs (QPs), which can be efficiently solved to global optimality. Those with
nonlinear prediction models and constraints require the solution of a nonlinear
program (NLP), typically a more computationally expensive undertaking. Non-
linear MPCs often possess nonconvex cost functions and constraints, meaning a
guarantee of convergence to global optimality may not exist. Despite this, NMPC
has been successfully applied to the cart-pole inverted pendulum [112]; however,
this system possesses simpler and substantially slower dynamics than that in this
thesis, and a fixed control delay is used to allow for a lengthy computation time,
at a cost of a significant reduction in high frequency disturbance rejection. Com-
plexity also varies greatly with choice of discretisation period, prediction and
control horizons, and constraint quantity, meaning there is often value in tailor-
ing each MPC implementation to its particular plant. Linear MPC formulations
exist which allow for the straightforward derivation of proofs of convergence and
stability, though similar techniques can sometimes also be applied to NMPC.
This is typically achieved by minimising the cost function over an infinite rather
than receding horizon, in which the control actions are used to drive the state
into an invariant terminal constraint set, after which a fixed closed-loop control
is implemented over the infinite horizon. These methods are often referred to
dual-mode MPC, and are the de-facto standard for MPC implementations in
which convergence and stability guarantees are required [113, 114].

Model predictive controllers with linear prediction models and constraints have
been well studied in the context of cart-and-pole inverted pendulums [112, 115–
117], but their application to TWIPs has been limited. Dini [60] implements a
finite horizon MPC, but only for the control of the yaw and pitch states. Hi-
rose [61] and Yue [59] also control a TWIP’s forward velocity, incorporating
both pitch and velocity constraints. However, both rely on an externally gener-
ated reference velocity trajectory in order to avoid infeasibility when performing

137
Chapter 6 Model Predictive Control

point-to-point manoeuvres, meaning the controller has no intrinsic guarantee of

reference feasibility. Ohhira [118] comes close to implementing a full-state MPC
position controller, in which a stabilising inner LQR is used to provide a closed-
loop system that is then augmented by an outer optimal predictive controller,
in a similar manner to dual-mode MPC. However, the inner loop is calculated
with a 0.01 s sample time, whilst the outer optimisation is computed with a 0.08 s
sample time, meaning constraint satisfaction can only be guaranteed when the
inner and outer loop sampling instants coincide. This intermittent enforcement
of the hard input constraints could allow constraint violation during the 70 ms
gap between calculations of the augmenting input, inducing wheel slip and loss of
control. Currently no successful implementations of a constrained MPC position
controller for a TWIP exist in the literature.
As this controller is to be used to directly calculate desired wheel torque set-
points, minimal computation time is critical. While sensitivity to control delay
could be minimised by optimising a delayed input so as to allow a fixed time
period for computation [112], as this controller is to be responsible for high fre-
quency disturbance rejection this is not a suitable solution.
The goal of this chapter is therefore to develop a fast MPC capable of track-
ing both continuously varying and discontinuous position reference trajectories,
whilst optimally enforcing hard wheel torque constraints |τi | ≤ τ , i ∈ [1 . . 4],
and softened lean angle and velocity constraints |θp | ≤ θp and vx2 + vy2 ≤ v 2 . For
sake of safety, guarantees of stability and convergence should be provided.

6.1 Controller Derivation

The Jacobian linearisation of (3.2.19) about the stationary upright φ = 0 position
yields a linear state space prediction model suitable for the development of a
linear model predictive controller in the form

xk+1 = Axk + Buk , yk = Cxk (6.1.1)

where the output is chosen as C = I3×3 03×5 . This linearisation ignores the
 

nonlinearity that is inherent in the integration of local body frame velocities to

global inertial frame positions with a time varying φ; correction of this error is
assumed to be provided in the external generation of reference trajectories. These

138
 6.1 Controller Derivation

can be mapped into body frame position trajectories at the start of every control
iteration, and replanned if excessive deviation occurs.

A dual-mode controller is chosen for its systematic design approach, its a priori
stability guarantee, and its improved numerical conditioning in the prediction of
open-loop unstable plants [114]. This controller uses the control law

uk+i = −Kxk+i + ck+i i ≤ nc

(6.1.2)
uk+i = −Kxk+i i > nc

for prediction timestep i ≥ 0, i.e. i = 0 denotes the state at the current timestep
k, K represents an unconstrained optimal feedback, and ck represents the first
element of an optimised sequence of nc future perturbations from the uncon-
strained optimal, denoted →k
c . The notation →k
c represents the column vector
formed by stacking future values of cj for j = [k . . k + nc − 1], i.e. →k
c =
ck ck+1 . . . ck+nc −1 .
 T

6.1.1 Reference Tracking

In order to track a varying reference rk the cost function evaluated at an upright

unstable equilibrium with yk = rk must propose no change to the input. To
achieve this the model must be redefined in terms of deviations x̂k and ûk from
the desired steady states xss|k and uss|k at step k as xk = x̂k + xss|k and uk =
ûk + uss|k . The steady state values are calculated by the simultaneous equations
yss|k = Cxss|k , xss|k = Axss|k + Buss|k , arranged to solve for xss|k , uss|k in matrix
form with yss|k = rk as
" #+ " # " # " #" #
C 0 r xss|k Mx r
= = (6.1.3)
A−I B 0 uss|k Mu 0

allowing xss|k and uss|k to be defined as

xss|k = Mx rk uss|k = Mu rk (6.1.4)

139
Chapter 6 Model Predictive Control

where it is found that

h iT
Mx = C + = I3×3 03×5 (6.1.5)

Mu = B −1 (A − I)C + = 04×3 (6.1.6)

Substituting (6.1.2) into (6.1.1) and applying the above change of variables gives

xk+1 − xss|k+1 = Φ(xk − xss|k ) + Bck

(6.1.7)
uk − uss|k = −K(xk − xss|k ) + ck

where Φ = A − BK, which can then be substituted with (6.1.4) to give new
closed-loop prediction model and control law

xk+1 = Φxk + (I − Φ)Mx rk+1 + Bck (6.1.8)

uk = −Kxk + (KMx + Mu )rk+1 + ck (6.1.9)

6.1.2 Autonomous Model Formulation and Reference Previewing

Instead of continuing with two separate control laws as in (6.1.2), analysis can be
simplified by the formation of an all-encompassing autonomous prediction model
of the form Zk+1 = Ψ Zk . This allows constraints of the form Gx ≤ f to be
applied to predicted step Zk+n as GΨ n Zk ≤ f .

Additionally, advanced knowledge of nr future reference inputs can be incorpo-

rated by the inclusion of →k+1 = rk+1 rk+2 . . . rk+nr +1 into the autonomous
 T
r
model state. Typically nr ≤ nc , as designing a controller to optimise its trajec-
tory against a longer reference previewing horizon than its control horizon can
introduce a transient tracking error, yield undesirable performance, and result
in infeasibility of the QP [114]. In this thesis it is assumed that nr = nc , pro-
viding the controller with the maximum possible horizon of advanced reference
knowledge. The change in advanced knowledge available to the controller over

140
 6.1 Controller Derivation

the prediction horizon k + i can be defined using a shift matrix Dr as

   
rk+2 0 I 0 ... 0  
    rk+1
 rk+3  0 0 I . . . 0 
..   .. .. .. . . ..   rk+2 
    
.  = . . . . .  .. (6.1.10)
  
.
 
    
r  0 0 0 0 I 
 k+nr +1  
rk+n +1

rk+nr +1 0 0 0 0 I | {zr }
| {z } | {z } → r k+1
r
→k+2 D r

in which at k > nr the controller’s final steady state reference is assumed to be

equal to rk+nr +1 .

Similarly, the matrix Dc captures the change in the future perturbation vector
c over one prediction step
→k
 
  0 I 0 ... 0  
ck+1 ck
 .  0
 
0 I . . . 0
 ..   .

.. .. . . ..  
  ck+1 
 =  .. . . . .  (6.1.11)

 .
  .. 

ck+nc  
  
0 
0 0 0 I
0 ck+n
 
| {z } 0 0 0 0 0 | {z c }
c
→k+1
| {z } → ck
Dc

in which no perturbation occurs for k + i > nc .

This allows the definition of the autonomous prediction model Zk+1 = Ψ Zk as

   
xk+1 Φ BDck (Φ − I)Mx Drk+1 xk
c  = 0 Dc 0 c  (6.1.12)
    
→k+1   →k
r
→k+2
0 0 Dr r
→k+1
| {z } | {z } | {z }
Zk+1 Ψ Zk

where Dck and Drk+1 select the first elements of →k

c and →k+1
r respectively.

141
Chapter 6 Model Predictive Control

6.1.3 Cost Function Derivation

A quadratic cost function that drives the state and input towards their desired
steady state values can be defined as

∞
X
J= (xk+i − xss|k+i )T Q(xk+i − xss|k+i )
i=1

+ (uk+i−1 − uss|k+i−1 )T R(uk+i−1 − uss|k+i−1 ) (6.1.13)

where Q and R are diagonal matrices scaling the quadratic cost of each state and
input error where Q  0, R  0.
The terms xk+i − xss|k+i and uk+i−1 − uss|k+i−1 can be redefined in terms of
the autonomous model state Zk as
h i
xk+i − xss|k+i = I 0 −Mx Drk+1 Zk+i−1 (6.1.14)
| {z }
Kxss

 T
−K
uk+i−1 − uss|k+i−1 =  Dck  Zk+i−1 (6.1.15)
 

−Mu Drk+1
| {z }
Kuss

allowing (6.1.13) to be rewritten in terms of Kxss , Kuss , and Zk as

∞
(6.1.16)
X
J= (Kxss Zk+i )T Q(Kxss Zk+i ) + (Kuss Zk+i−1 )T R(Kuss Zk+i−1 )
i=1

which can be rewritten as

∞
(6.1.17)
X
J= (Kxss Zk+i+1 )T Q(Kxss Zk+i+1 ) + (Kuss Zk+i )T R(Kuss Zk+i )
i=0

It is then possible to substitute for Zk+i+1 using Zk+i+1 = ΨZk+i

∞
(6.1.18)
X
T
(Kxss Ψ)T Q(Kxss Ψ) + (Kuss )T R(Kuss ) Zk+i
 
J= Zk+i
i=0

From the autonomous model it is then seen that Zk+i = Ψi Zk , allowing substi-

142
 6.1 Controller Derivation

tution of Zk+i and factorisation of the now constant Zk term

(∞ )
 
(6.1.19)
X
J= ZkT i T
(Ψ ) T
Kxss ΨT QKxss Ψ + T
Kuss RKuss i
Ψ Zk
i=0

The discrete Lyapunov equation can then be used to convert this convergent
infinite series into the form
 T   
xk Sx Sxc Sxr xk
J = ZkT SZk =  →k (6.1.20)
  T
c  Sxc c 
Sc Scr   →k
  

r T
Sxr T
Scr Sr r
→k+1 →k+1

This can be minimised by the solution of

∂J
=0 (6.1.21)
∂ →k
c

to define the unconstrained optimal perturbation required to incorporate ad-

vanced reference knowledge
 
c
→k
= −Sc
−1
S x
xc k + S r
cr →k+1 (6.1.22)

As expected, it is found that →k

c = 0 0 . . . 0 for →k+1 = rk+1 rk+1 . . . rk+1 ,
   T
r
and Sxc ≡ 0 and can therefore be omitted [114].

It is therefore clear that →k

c =6 0 if any advanced reference knowledge is
included, even for the unconstrained case. This goes against the dual-mode
paradigm, in that for the unconstrained case the optimal trajectory should be
fully captured by the underlying control law, meaning that in the absence of
constraints it is desired that →k . To reinstate this
 T
c = 0 0 ... 0 ∀ →k+1
r
intrinsic unconstrained optimality, the perturbation term can be redefined as
c
→k
ĉ + −Sc−1 Scr →k+1
= →k r , now instead optimising for →k
ĉ . The elements not
dependant on the DoF →k
ĉ can then be removed, allowing the redefinition of the
QP and control law (6.1.8) as

143
Chapter 6 Model Predictive Control

h iT h i
ĉ − Sc−1 Scr →k+1
min J = →k r ĉ − Sc−1 Scr →k+1
Sc →k r
ĉ
→k
h iT
ĉ − Sc−1 Scr →k+1
+ 2 →k r Scr →k+1
r (6.1.23)

uk = −Kxk + (KMx + Mu )Drk+1 − Dck Sc−1 Scr →k+1 (6.1.24)

 
r + ĉk

Since the unconstrained optimal perturbation is known to be →k

ĉ = 0, the per-
formance index must be purely quadratic [119], allowing (6.1.23) to be simplified
to
ĉ Tk Sc →k
min J = → ĉ (6.1.25)
ĉ
→k

The autonomous model (6.1.12) can be redefined to incorporate this reference

previewing feedforward as
   
xk+1 Φ BDck Γ xk
c  = 0 Dc 0   →kc  (6.1.26)
    
→k+1
r
→k+2
0 0 Dr r
→k+1
| {z } | {z } | {z }
Zk+1 Ψ Zk

where Γ = −BDck Sc−1 Scr + (I − Φ)Mx Drk+1 .

6.1.4 Infeasible Reference Tracking

For this controller to be practically useful it must be able to drive the platform
to any reference position and heading {xr , yr , φr } ∈ R3 from any initial position.
However, the nature of the dual mode controller means that there must exist a
feasible perturbation sequence →k
c that takes the platform into the set of states
from which the closed-loop system at k > nc will not violate any constraints over
the infinite horizon, referred to as the maximal admissible set (MAS, SMAS ) [114].
This is an invariant set, meaning once the underlying closed-loop system enters
this set it will never leave, and will therefore never violate constraints. This lies
within a set of states from which there exists a sequence of feasible control moves
c that drive the initial state into the MAS without constraint violation, referred
→k
to as the maximal controlled admissible set (MCAS, SMCAS ), SMAS ∈ SMCAS . For

144
 6.1 Controller Derivation

any initial state outside SMCAS there does not exist a sequence of control moves
that can guide the state into SMAS within k ≤ nc without violating constraints,
rendering the QP infeasible. For this application this limits step translations
to approximately 0.1m for nc = 10, dependant on initial state and choice of
quadratic cost function matrices Q and R, rendering the controller impractical
for real-world implementation.
Multiple approaches exist to address this problem. Simon [120] replaces r with
a pseudo-reference r̃ as an additional degree of freedom, penalising deviation of
this from the true reference, making the QP feasible for all possible references.
Dughman [121] introduces an extra perturbation term c∞ to the end of the
control sequence →k
c , which acts as a constant perturbation to the input for
k > nc . This constant perturbation has the same effect as a pseudo-reference,
with the equivalent pseudo-reference calculable as r̃ = C(I −Φ)−1 Bc∞ +r. These
methods ensure that SMCAS spans all {x, y, φ} ∈ R3 .
Here the latter approach is taken, introducing a c∞ term for k > nc . The
opportunity is also taken to introduce a vector of slack variables →k
s that will
be later used to soften the controller’s output constraints, giving an updated
autonomous model Ψ and state Z
    
xk+1 Φ BDck 0 Γ 0 xk
c  0 Dc Ec 0 0   →k c 
    
→k+1
(6.1.27)
    
 c∞  =  0 0 I 0 0  c∞ 
    
r  0 0 0 Dr r
0  →k+1
    
→k+2 
s
→k+1
0 0 0 0 Ds s
| {z } | {z } | →k{z }
Zk+1 Ψ Zk

where Ec is used to replace the last value of →k+1 with c∞ as Ec = 0 0 . . . 0 I ,

 T
c
and where Ds represents a shift matrix with a similar structure as Dc .
The QP (6.1.25) can now be redefined to additionally minimise c2∞ and →
s 2k as

T 
  
ĉ
Sc
→k
0 0 ĉ
→k
 min J = c∞   0 W Sc∞ 0  c∞  (6.1.28)
    
ĉ ,c∞ ,→k
s
→k s
→k
0 0 Ss s
→k

Sc∞ is scaled by a very small factor W so that choice of c∞ has a minimal

145
Chapter 6 Model Predictive Control

effect on →k
c , whilst ensuring c∞ → 0 as k → ∞ and feasibility ∀→k+1
r . Ss is
a diagonal matrix of large slack weights used to heavily penalise deviation of
s from 0, such that its elements are only optimised to be substantially larger
→k
than zero if feasibility of the QP would be otherwise lost. This means that the
output constraints are only significantly violated if this is necessary to maintain
feasibility of the QP.

6.1.5 Quadratic Cost Weights and Discretisation Period

The discretisation period Ts must be chosen as a trade-off between computational

simplicity and prediction model accuracy, with the latter requirement related to
the expected magnitude of control inputs. This is chosen through trial and error
as Ts = 35 ms. This does not dictate the control update rate - this can be
performed over a much shorter period, only limited by the time required to solve
each QP.
As for this application the controller is required to track reference body po-
sitions and heading, Q is initially set to Q = diag 1 1 1 01×5 . R must be
 

chosen as a trade-off between control performance and avoiding the excitation of

unmodelled dynamics, so a value of R = 0.1I4×4 is chosen.

6.1.6 Constraint Derivation

Hard input wheel torque constraints |τ | ≤ τ are required in order to avoid wheel
slip. Output constraints are required on the θp state in order to prevent the
controller from attempting to translate using an unrealistic lean angle, as well as
to keep the system near the model operating point. The vx , vy , and φ̇ states must
be constrained in order to maintain a safe margin from the edge of SMCAS for
the controller to be able to handle disturbances, as well as to bound the system’s
kinetic energy as to ensure the controller generates safe and sensible velocity
profiles.
Hard constraints on the input |uk | ≤ u and softened output constraints |xk | ≤

146
 6.1 Controller Derivation

x + sk at timestep k can be represented in the form GZk ≤ f as

 
  xk  
−K Dck 0 P 0 u
c   
 
   →k
 K −Dck 0 −P 0   ≤ u (6.1.29)
    
 c ∞
 C 0 0 0 −Dsk    x 
r
    
→k+1 
−C 0 0 0 −Dsk x
| {z } →k s |{z}
G | {z } f
Zk

where P = −Dck Sc−1 Scr + (KMx + Mu ) Drk .

These constraints can be projected over a k + ncon constraint horizon by use
of the autonomous model as1
 
 
G f
   
 GΨ  f 
   
(6.1.30)
 GΨ 2 
 Zk ≤ f 
 
 .  .

 ..   .. 
   
GΨ ncon f
| {z } |{z}
F t

where ncon > nc , and ncon is sufficiently large as to fully define SMAS , as from
Section 6.1.4 membership of SMAS at k + nc + 1 is necessary to guarantee con-
straint satisfaction over the infinite horizon. However, this approach to defining
SMAS can result in redundant constraints, and provides no systematic method
for selection of ncon .
Fortunately, algorithms for deriving the minimal set of constraints required to
define SMAS are well studied in the literature [122,123]. Generally these function
as follows. First, the initial set S0 = {Zk : GZk − t ≤ 0} is defined. Linear
programming is then used to maximise each row of GZk − t ≤ 0 subject to the
other remaining constraints. If any row can be made to be larger than zero, then
there exists a Zk such that Zk ∈ S0 but ΨZk+1 ∈
/ S0 , meaning S0 is not invariant.
The next set " #
GZk − t
S1 = {Zk : ≤ 0} (6.1.31)
GΨZk − t

1
Note the rows of the G entry in F that constrain xk serve no purpose and can be removed.

147
Chapter 6 Model Predictive Control

1 ẋ2 + ẏ 2 ≤ v 2
M = 16
M =8
0.5 M =4

0
ẏ

−0.5

−1
−1 −0.5 0 0.5 1
ẋ

Figure 6.1: Polytope approximations of the quadratic velocity constraint ẋ2 +

ẏ 2 ≤ v 2 with v = v x = v y = 1.

is then defined, and the above procedure is repeated until there exists no choice
of Zk that is able to force a constraint violation, so Zk ∈ Sn =⇒ Zk ∈ Sn+1
and thus Sn is invariant. This procedure always converges if the state asymptote
lies within SMAS , i.e. limi→∞ Zk+i ∈ GΨi Zk ≤ t [114]. Typically a number of
redundant constraints will be introduced, which can be eliminated by various
pruning methods.

6.1.7 Velocity Constraint Approximation

Whilst exact constraint satisfaction is achieved by enforcing ẋ2 + ẏ 2 ≤ v 2 ,

this forms a quadratic constraint, and therefore cannot be implemented using
quadratic programming2 . This can be avoided by approximating this constraint

2
Quadratic constraints must be implemented using second-order cone programming if appli-
cable, otherwise using semidefinite programming. These are both significantly more compu-
tationally expensive than quadratic programming.

148
 6.1 Controller Derivation

using a convex polytope [124], defined by the linear inequalities

       
2πm 2π(m + 1) 2πm
ẏ − v sin cos − cos
M M M
       
2πm 2π(m + 1) 2πm
− ẋ − v cos sin − sin ≥ 0 (6.1.32)
M M M

where M ∈ [3 . . ∞], m ∈ [1 . . M ]. Choices of M = [4, 8, 16] are shown in Figure

6.1. Here M = 8 is chosen, yielding only a minor suboptimality w.r.t. the exact
velocity constraint, whilst avoiding excessive growth of G.

6.1.8 Slack Variables and their Distribution

For the prototype used in this thesis with nc = 10, SMAS can be defined using
65 individual output constraints. Softening each of these constraints individually
requires an equal number of slack variables, increasing the QP DoF from 44 to
109, a significant increase in complexity. To lessen this, the number of slack vari-
ables can be reduced by the sharing of slack variables across multiple timesteps
through a redefinition of Fs and removal of diagonal entries from Ss , provided this
does not reintroduce the possibility of output constraint infeasibility. Multiple
slack variable sharing approaches were considered:

1. One slack variable per constraint per timestep.

2. One slack variable per constraint per timestep for k ≤ nc , with no constraint
softening for k > nc .

3. Sharing a single slack variable per constraint over the entire horizon.

4. Sharing a single slack variable per constraint for k ≤ nc , and sharing a

single slack variable per constraint for k > nc

5. Allocating one slack variable per constraint per timestep for k ≤ nc , with
a single slack variable per constraint shared for k > nc .

6. Allocating one slack per constraint per nblock timesteps, with a single slack
per constraint shared for k > nc .

149
Chapter 6 Model Predictive Control

0.4
ni = 1
0.2 ni = 2

θp (rad)
0 ni = 3
±θp
−0.2
−0.4
0 0.5 1 1.5 2 2.5 3 3.5 4

2 ni = 1
vy (m s−1 )

ni = 2
1 ni = 3
0 ±v y
−1
0 0.5 1 1.5 2 2.5 3 3.5 4
t (s)

Figure 6.2: Comparison of controller response in the regulation case to an initial

constraint violation of vy = 2v y for ni = [1, 2, 3]

7. Allocating one slack variable per constraint per timestep for k ≤ ni , and
a single shared slack variable per constraint for k > ni , with ni chosen by
trial and error.

8. Allocating one slack per constraint per timestep for k ≤ ni , one slack per
constraint per nblock timesteps for ni < k ≤ nc , and a single slack variable
per constraint shared for k > nc .

Methods 2, 3, 4, and 6 showed eventual infeasibility during Monte Carlo anal-

ysis, whereas methods 1, 5, 7, and 8 all maintained feasibility. Of the feasible
methods method 7 with ni = 1 for ẋ and φ̇ constraints and ni = 3 for θp and ẏ
constraints allowed for the minimum number of slack variables, reducing ns from
the initial ns ≈ 65 down to ns = 12. While this method with ni = 1 for θp and
ẏ constraints was also able to maintain feasibility, the controller had insufficient
degrees of freedom to quickly address constraint violations. A comparison of con-
troller response for ni = [1, 2, 3] in the regulation case with an initial condition
of ẏ = 2v y is shown in Figure 6.2, in which a large variation in the time taken
to re-establish output constraint satisfaction following a constraint violating dis-
turbance is visible between different values of ni .

150
 6.1 Controller Derivation

Slack weights in (6.1.28) for non-shared slack variables are set to near zero
to allow the controller to worsen constraint violation over a short horizon to
improve the rate of convergence to SO over the infinite horizon. For example, for
an initially upright system with |vy | > 2v y , the non-minimum phase nature of this
system means that the desired control response is for the controller to first briefly
increase vy , driving θp < 0 in order to start generating sustained deceleration
v̇y < 0. A large slack weight for the slacks at i ≤ 3 penalises this type of quick
correction, preventing the system from correcting the violation as aggressively as
is desired. The infinite horizon slack weights are made very large to ensure that
all resulting trajectories bring the state towards SO as quickly as possible. The
cost associated with non-zero s∞ must also be much greater than that practically
encountered due to tracking error, otherwise a distant reference trajectory will
cause the controller to purposefully generate an undesirable constraint violation.

6.1.9 The Effect of Input Constraints on Feasibility

While the output constraints enforced on vx , vy , φ̇, and θp are softened to main-
tain feasibility during disturbance, the hard input constraints applied to →k
u ef-
fectively apply a second set of output constraints, albeit with a larger constrained
range in this particular application. This is due to the requirement for the con-
troller to direct the state into SMAS at k + nc + 1, in which the underlying control
law must not violate the hard input constraints as i → ∞. Given the range of
output constraints acting on this system, this could take a reasonably long time
from some choices of xk ∈ SO ; for example, a system initialised with vy = v y and
θp = θp with xk ∈
/ SMAS will require a large control horizon to give the controller
enough time to manipulate the plant toward the origin and into SMAS using its
constrained input. This can lead to parts of the output constraint set SO be-
ing infeasible, despite using output constraint softening. Care must therefore be
taken to ensure nc and u are sufficiently large when specifying SO in order to
ensure SO ⊆ SMCAS . Additionally, a large margin must be allowed due to the
use of softened output constraints, otherwise an anticipated constraint violation
could result in infeasibility. For this application the maximum possible vy and
θp values for which the controller must remain feasible are expected to be in the
region of ±3 m s−1 and ±0.5 rad respectively, with minimal disturbance expected
in the open-loop stable vx and φ̇ states.

151
Chapter 6 Model Predictive Control

6.2 Results

This section now aims to demonstrate the efficacy and efficiency of the proposed
controller in simulation and on an experimental prototype. The quadratic pro-
gram is solved using qpOASES [126], an online active set solver, implemented on
an Intel i7-4720HQ processor for simulated results and an Intel i7-8650U for ex-
perimental results. In single threaded benchmarks the latter achieves 17% greater
performance than the former. Simulation is performed by numerical integration
of the continuous time nonlinear model using MATLAB’s ode45. In simulation
the controller is updated at a rate of 1/Ts , whereas on the experimental proto-
type it is updated as continuously as execution time allows in order to improve
disturbance rejection, with a typical control update rate of around 500 Hz.

6.2.1 Step Reference Tracking - Forward Translation

First, a step reference of ry = [0, 1] is used to demonstrate the step response of

the (y, θp ) subsystem, with the simulated response shown in Figure 6.5. This
shows a response with asymptotic convergence, minimal overshoot, and sensi-
ble preemption of the reference change. The infinite horizon perturbation c∞
correctly increases at the moment of the reference step to maintain feasibility,
tending to zero once x ∈ SMCAS . The perturbation terms c0,i show a similar
response, tending to zero once x ∈ SMAS . The controller shows satisfaction of all
constraints, saturating the input for a number of samples and producing toward
minimum-time vy and θp trajectories. A small amount of |vy | ≤ v y constraint
violation is observed; this is to be expected, as this is related to the low cost
associated with the softening of this constraint for k < ni . Execution time peaks
at the instant of the reference step to 3.5 ms, an acceptable control delay, quickly
dropping below 2 ms for the remainder of the trajectory, and computing nearly
instantly once x ∈ SMAS . The cost J is observed to be monotonic from the first
knowledge of the impending reference change onwards, indicating good problem
formulation.
Figure 6.6 shows the response of the experimental prototype to this step ref-
erence. This shows a strong similarity to that predicted in Figure 6.5, with only
minor increases in constraint violation due to external disturbance. Execution
time is found to be slightly faster, demonstrating the real-world feasibility of

154
 6.2 Results

τ1 τ2 τ3
τ4 ±τ
0.2
0.1
0
0 ck,1
−0.2
ck,2
ck,3
−0.1 −0.4 ck,4
0 2 4 6 0 2 4 6

0.1
J
2
0

−0.1 c∞,1
c∞,2 1
c∞,3
−0.2 c∞,4
0
0 2 4 6 0 2 4 6

0.4
1 θp
0.2 ±θp
0.5 0
y −0.2
0 ry
−0.4
0 2 4 6 0 2 4 6
·10−3
4
vy texec
1 ±v y 3

0 2

1
−1
0
0 2 4 6 0 2 4 6

Figure 6.5: Simulated MPC response to a step reference of y = [0, 1]m, showing
asymptotic convergence, minimal overshoot, and sensible preemption of the ref-
erence change. The i = 0 perturbation c0,i and infinite horizon perturbation c∞
terms correctly increase at the moment of the reference step to maintain feasi-
bility, and tend to zero as x ∈ SMAS and x ∈ SMCAS respectively. The controller
shows satisfaction of all constraints, saturating the input for a number of samples
and producing toward minimum-time vy and θp trajectories.
155
Chapter 6 Model Predictive Control

τ1 τ2 τ3
τ4 ±τ
0.2
0.1
0
−0.2
0 ck,1
−0.4 ck,2
−0.6 ck,3
−0.1 ck,4
−0.8
0 2 4 6 0 2 4 6

J
0 2

c∞,1
−0.2 c∞,2 1
c∞,3
c∞,4
0
0 2 4 6 0 2 4 6

0.4
1 θp
0.2 ±θp
0.5 0
y −0.2
0 ry
−0.4
0 2 4 6 0 2 4 6
·10−3
3
vy texec
1 ±v y
2
0
1
−1
0
0 2 4 6 0 2 4 6

Figure 6.6: Experimental response to a step reference of ry = [0, 1]m. This shows
a strong similarity to that predicted in Figure 6.5, with only minor increases
in constraint violation due to external disturbance. Execution time is found
to be slightly faster, demonstrating the real-world feasibility of applying online
constrained optimal control to this highly dynamic system.

156
 6.2 Results

applying online constrained optimal control to this highly dynamic system.

6.2.2 Step Reference Tracking - Lateral Translation

Figure 6.7 shows the system’s response to an rx = [0, 1] step reference in-
put, showing close to bang-bang control without any constraint violation. This
demonstrates both the advantageous ability of this optimal control approach to
produce close to minimum-time trajectories in the presence of constraints, and
how by directly controlling wheel torques this controller is able to optimally
saturate the input for a significant duration without any negative impact on
closed-loop stability.
Figure 6.8 shows the response of the experimental system to this reference,
again showing a very similar response to that predicted in simulation. This
response does exhibit a small steady-state error, believed to be due to static
friction in the Mecanum wheel roller bearings. This could be addressed in future
work by the incorporation of integral action into the controller, likely through
an output disturbance observer [114], or by an improved discontinuous form of
friction compensation. Also shown in this figure are the tracked θp , vy , and y
trajectories, as in practice lateral motion generates large disturbances in these
states. This shows peak deviations of θp = 0.027 rad and y = 1.5 cm, indicating
that these disturbances are mostly successfully rejected, especially given that
through the system’s non-minimum phase property it is impossible to drive this
error to zero in the presence of disturbance. This control performance is achieved
whilst maintaining texec < 2 ms.
As expected, a step reference applied to rφ generates very similar trajectories
to that in Figures 6.7 and 6.8, and is therefore omitted.

6.2.3 Constraint-Violating Disturbance Handling

Figure 6.9 demonstrates the controller’s handling of large constraint-violating
disturbances. For this scenario the reference is kept at the origin and the sys-
tem is given an initial forward velocity vy = 2v y = 2 m s−1 , whilst maintaining
all other initial states the origin. As expected, the controller is seen to briefly
worsen constraint violation by increasing vy to lean the platform towards the
origin, maintaining θp ≈ θp to decelerate as quickly as constraints allow, before
maintaining vy ≈ −v y until y = 0, with minimal overshoot.

157
Chapter 6 Model Predictive Control

τ1 τ2 τ3
τ4 ±τ
1
0.1
0.5

0 0 ck,1
ck,2
−0.5 ck,3
−0.1 ck,4
−1
0 2 4 6 0 2 4 6

vx
1 1 ±v x

0.5 0

x
0 rx −1

0 2 4 6 0 2 4 6

Figure 6.7: Simulated MPC response for a step reference of rx = [0, 1]m. This
shows close to bang-bang control without, any constraint violation. This demon-
strates both the advantageous ability of this optimal control approach to produce
close to minimum-time trajectories in the presence of constraints, and how by
directly controlling wheel torques this controller is able to optimally saturate
all four inputs for a large number of timesteps without any negative impact on
closed-loop stability.

Figure 6.10 shows a similar scenario, though real-world initial conditions are
somewhat harder to control, and so contain some visible error. Despite this, a
very similar response is observed, again showing fast re-establishment and main-
tenance of constraint satisfaction, before asymptotically re-converging to the ori-
gin.

6.2.4 Time-Varying Trajectory Tracking

This controller provides no mechanism for directly incorporating feedforward ve-

locity reference information as would be performed for a simple full-state feedback
controller. The improved steady state tracking of a time-varying reference that

158
 6.2 Results

τ1 τ2 τ3
τ4 ±τ
1
0.1
0.5
ck,1
0 0 ck,2
−0.5 ck,3
−0.1 ck,4
−1
0 1 2 3 4 5 0 1 2 3 4 5
1
J
0.5 1
c∞,1
0 c∞,2
c∞,3 0.5
−0.5
c∞,4
−1 0
0 1 2 3 4 5 0 1 2 3 4 5
·10−2
2
1 y
ry
1
0.5
x 0
0 rx
0 1 2 3 4 5 0 1 2 3 4 5

1 1 vy
±v y
0 0

−1 vx ±v x −1
0 1 2 3 4 5 0 1 2 3 4 5
·10−3
0.4 2
θp ±θp texec
0.2 1.5
0 1
−0.2 0.5
−0.4 0
0 1 2 3 4 5 0 1 2 3 4 5

Figure 6.8: Experimental response to a step reference of rx = [0, 1]m, showing a

very similar response to that predicted in simulation in Figure 6.7. This response
does now exhibit a small steady-state error, believed to be due to static friction
in the Mecanum wheel roller bearings. The θp and y states show peak deviations
of θp = 0.027 rad and y = 1.5 cm, indicating disturbance in these states gener-
ated during lateral motion is mostly rejected, especially given that through the
system’s non-minimum phase property it is impossible to drive this error to zero,
even with perfect control. 159
Chapter 6 Model Predictive Control

τ1 τ2 y
0.1 τ3 τ4 1 ry
±τ
0 0.5

−0.1 0
0 2 4 6 0 2 4 6

0.4
θp 2 vy
0.2 ±v y
±θp 1
0 0
−0.2 −1
−0.4 −2
0 2 4 6 0 2 4 6

Figure 6.9: Simulated controller response in regulation scenario to an initial

disturbance of vy = 2v y = 2 m s−1 , demonstrating the controller’s handling of
large constraint-violating disturbances. For this scenario the reference is kept at
the origin and the system is given an initial forward velocity vy = 2v y = 2 m s−1 ,
whilst maintaining all other initial states at the origin. As expected, the controller
is seen to briefly worsen constraint violation by increasing vy to lean the platform
towards the origin, maintaining θp ≈ θp to decelerate as quickly as constraints
allow, before maintaining vy ≈ −v y until y = 0, with minimal overshoot.

this would bring is instead incorporated via the reference previewing mechanism,
meaning tracking performance is related to choice of nr . Figure 6.11 shows sim-
ulated position tracking error for a ramping y reference signal over increasing nr .
This shows a steady state tracking error for short reference previewing horizons,
with this error vanishing around nr ≈ 18. This effect highlights a drawback of
the dual-mode MPC approach; the use of a closed-loop prediction model means
that when given a previewed section of a continuously changing reference the
controller is optimising its future trajectory to come to rest at rk+nc +1 , meaning
it must plan for its state at k = nc + 1 to lie within SMAS . In practise this
means that the system can only increase vy to a value from which it can enter
SMAS in nc timesteps, which for the value of nc used in the previous figures is

160
 6.2 Results

τ1 τ2 y
0.1 τ3 τ4 1 ry
±τ
0 0.5

−0.1 0
0 2 4 6 0 2 4 6

0.4
θp 2 vy
0.2 ±v y
±θp 1
0 0
−0.2 −1
−0.4 −2
0 2 4 6 0 2 4 6

Figure 6.10: Experimental response in the regulation case to an initial velocity

disturbance of approximately vy = 2v y = 2 m s−1 . This shows a very similar
response to that predicted by simulation in Figure 6.10, again showing fast re-
establishment and maintenance of constraint satisfaction, before asymptotically
re-converging to the origin.

insufficient to correctly track the given ramping reference trajectory. This results
in the system lagging behind the desired trajectory, which in turn increases the
effective stopping distance from the target steady state that the controller is able
to use to enter SMAS at k = nc + 1. This position lag increases until the system
has accumulated sufficient distance from rk+nc +1 to be able to safely reach the
velocity required to keep up with the moving reference.
This tracking error can be addressed by ensuring a reference previewing period
that is of sufficient length to fully capture the transition of the system from any
state within SO into SMAS without constraint violation. This value of nc can
be approximated by examining the system response in the regulation case to
an initial forward velocity of vy = v y , which indicates a stopping distance of
0.53 m, taking 0.56 s for the system to enter SMAS . Two approaches exist to
ensure this; the reference previewing and control horizons can be increased to
match the stopping time at nr = nc = 28, at a cost of greater computational

161
Chapter 6 Model Predictive Control

0.6
nr =1
nr =2
0.4 nr =3

ry − y
nr =5
0.2 nr =8
nr = 12
nr = 18
0 nr = 25
0 1 2 3 4 5
nr = 35

4
3
y

2
1
0
0 1 2 3 4 5

Figure 6.11: Simulated MPC ry tracking error (top) for a linearly ramping posi-
tion setpoint over varying nr , with corresponding ry reference (dashed) and state
trajectories (bottom). This demonstrates how an insufficient reference previewing
horizon can prevent asymptotic tracking of a time-varying ry , with convergence
only achieved for nc ' 18.

complexity, or the underlying gain K can be increased and the output constraint
set enlarged in order to allow the system to reach the required steady state in less
time. However, increasing K in turn decreases the size of SMCAS , as discussed
in Section 6.1.9, as well as increasing sensitivity to noise and the excitation of
unmodelled dynamics. Enlarging the output constraint set is also ineffective,
as the same problem occurs, only at a larger velocity. For demonstration nc
and nr are increased to nc = nr = 28, with the response to the same figure-of-
eight trajectory also shown in Figure 6.12. This also allows a reduction of R in
contrast to Section 6.1.9, as this larger control horizon already enlarges SMCAS to
encompass SO with sufficient margin. However, this increase in control horizon
increases worst case execution time to texec = 6.4 ms, which in practise is close
to being too slow for the real-time control of this particular system. The control
horizon is therefore left unchanged, and a small amount of steady state tracking

162
 6.2 Results

0.5

−0.5

−1 nc = 10
nc = 28
r

−1.5 −1 −0.5 0 0.5 1 1.5

Figure 6.12: Simulated MPC state trajectories with nc = 10 (blue) and nc =

28 (red) for a figure-of-eight reference of 10 s duration and constant φ (yellow,
dashed), starting at the origin with an initial direction of down and left. The
system is initialised in a stationary pose at (−1, −1), a tracking error that would
result in constraint violation and wheel slip for a rudimentary controller.

error at large constant velocities is accepted.

6.2.5 Complex Trajectory Tracking

Finally, a figure-of-eight trajectory of 10 s duration with a large initial tracking

error is used to demonstrate the ability of this controller to track complex trajec-
tories whilst maintaining stability and feasibility for large reference deviations.
In Figures 6.12 and 6.13 this is shown in simulation for both nc = 10 and nc = 28,
though state trajectories are only plotted for the nc = 10 case. In the nc = 10
case a ry tracking error as in Figure 6.11 is observed, resulting in the system
following a distorted figure-of-eight. As expected, increasing nc to nc = 28 elimi-
nates this error. A small rx tracking error is also observed at the extremes of this
reference. This is due to the controller optimising a trade-off between perfect
tracking and the increased control effort required to achieve this, and is therefore
an expected behaviour, and can be lessened by a reduction of R or increase of
Q1,1 .

163
Chapter 6 Model Predictive Control

τ1 τ2 τ3
τ4 ±τ
0.1 0.5
0
0 −0.5
ck,1 ck,2
−1 ck,3 ck,4
−0.1
0 2 4 6 8 10 12 0 2 4 6 8 10 12
0.4 4
0.2 J
3
0
2
−0.2
c∞,1 c∞,2 1
−0.4 c∞,3 c∞,4
−0.6 0
0 2 4 6 8 10 12 0 2 4 6 8 10 12
2 0.5
1
0
0
−0.5
−1 x y
rx −1 ry
−2
0 2 4 6 8 10 12 0 2 4 6 8 10 12

1 vx vy ±v y
±v x 1
0
0
−1
−1
0 2 4 6 8 10 12 0 2 4 6 8 10 12
·10−3
0.4 texec
θp ±θp 2
0.2
0 1
−0.2
0
0 2 4 6 8 10 12 0 2 4 6 8 10 12

Figure 6.13: State trajectories for the figure-of-eight trajectory in Figure 6.12
with nc = 10. An ry tracking error is observed as in Figure 6.11, resulting in the
system following a distorted figure-of-eight path. As expected, increasing nc to
nc = 28 eliminates this error. A small rx tracking error is also observed at the
extremes of this reference due to the controller optimising a trade-off between
perfect tracking and the increased control effort required to achieve this.

164
 6.3 Conclusion

Figures 6.12 and 6.15 shows the experimental response to the same figure-of-
eight trajectory with nc = 12. This shows the same expected phase lag in the
y state as in simulation, along with the same x error as in Figure 6.8, now also
presenting as a lag behind the desired trajectory. A large spike in computation
time occurs at the start of the experiment to texec = 20 ms. This is due to the
significant change in optimal solution that occurs at this point, requiring a large
number of solver iterations to re-converge. Once this is solved subsequent control
iterations are ‘warm started’, meaning the solver is initialised with the solution
to the previous control iteration. Increasing the control horizon to nc = 28 as
in simulation in practice makes this initial computation time too large, causing
instability, and hence this is not shown.
This scenario demonstrates the combined advantages of this MPC approach,
in that both constraint satisfaction and stability are maintained throughout the
correction of the large initial tracking error, where a rudimentary controller would
yield significant constraint violation and loss of control, before continuing to yield
good tracking of time varying references.

6.3 Conclusion

This chapter has demonstrated the first successful implementation of a real-

time constrained model predictive controller for position and heading control
of a Collinear Mecanum Drive. Through a redefinition of the nonholonomic
constraints (3.1.5)-(3.1.7) and a reduction of the state vector this controller could
be simplified to be applicable to a two-wheeled inverted pendulum, for which such
a controller has not been successfully demonstrated either. This work is therefore
novel in a more general sense than in the control of specifically just a Collinear
Mecanum Drive.
Given the good experimental performance demonstrated by this controller on
this small and highly dynamic prototype, future work would explore application
of this controller to a larger system with slower dynamics. This would allow for
longer optimisation execution times, therefore allowing a larger control horizon
to be used to eliminate the tracking error demonstrated in Figures 6.11 and 6.15.
Future work will also explore introducing move blocking as discussed in Section
6.1.9 to allow for a larger nr and higher gain feedback for the same number of

165
Chapter 6 Model Predictive Control

0.5

−0.5

−1
nc = 10
r

−1.5 −1 −0.5 0 0.5 1 1.5

Figure 6.14: Experimental response to the same 10 s figure-of-eight trajectory as

in Figures 6.13 and 6.12 with nc = 12. This shows convergence to and tracking
of the reference trajectory from a large initial tracking error. A small rx remains
at the extremes of the path, believed to be due to static friction effects in the
Mecanum wheel rollers.

decision variables, along with exploring the size and distribution of these blocks.
The development of a high level planner capable of generating reference trajec-
tories for this controller to follow would enable the navigation of a known map.
This reference could be a series of discontinuous position waypoints, or it could
be a 2D path returned by a sampling based planner such as RRT*, with some
simple heuristic-derived timing law. In contrast with polynomial-based trajectory
generation methods [73] this removes the requirement for the outer planner to
specify a dynamically feasible and constraint satisfying trajectory between way-
points; here the dynamically feasible and constraint satisfying state and input
trajectories are instead derived iteratively by the MPC. The MPC demonstrated
here is better suited to this task than existing TWIP MPC implementations, as
by maintaining feasibility across the full reference set waypoints can be placed at
arbitrarily sparse intervals, rather than needing to consider the controller’s feasi-
ble reference set. The embedding of input and output constraint satisfaction into
the low level controller also improves safety and robustness in the event of delay

166
 6.3 Conclusion

τ1 τ2 τ3
τ4 ±τ
0.1
0
ck,1
0 ck,2
−0.5 ck,3
−0.1 ck,4
−1
0 2 4 6 8 10 12 0 2 4 6 8 10 12

0.2 100 J
0
−0.2 50
c∞,1 c∞,2
−0.4 c∞,3 c∞,4
−0.6 0
0 2 4 6 8 10 12 0 2 4 6 8 10 12
2 0.5
1
0
0
−0.5
−1 x y
rx −1 ry
−2
0 2 4 6 8 10 12 0 2 4 6 8 10 12

1 vx 1
±v x
0 0

−1 −1 vy ±v y
0 2 4 6 8 10 12 0 2 4 6 8 10 12
·10−2
0.2 2 texec
0
1
−0.2
θp ±θp
−0.4 0
0 2 4 6 8 10 12 0 2 4 6 8 10 12

Figure 6.15: Experimental response to the same 10 s figure-of-eight trajectory

as in Figures 6.13 and 6.12 with nc = 12. This shows convergence toward and
tracking of the reference trajectory from an initially large tracking error. Both
rx and ry tracking errors are visible, believed to be due to static friction effects
in the Mecanum wheel rollers and an insufficient control horizon.

167
Chapter 6 Model Predictive Control

or error in the high level planner, with the system guaranteed to safely come to
rest at the end of the last specified reference. This also improves recovery in the
event of significant disturbance, with tracking safely resumed whilst satisfying
constraints, as opposed to relying on the high level planner to quickly generate a
suitable ‘recovery’ trajectory. This is all achieved with a proof of convergence and
stability for the linearised model, which given the small degree of nonlinearity
over the constrained operating region can reasonably be assumed to extend to
the full nonlinear system.

168
Chapter 7

Fast Online Trajectory Optimisation

This chapter demonstrates the final control approach of this thesis, in which a
concept known as differential flatness is used to generate dynamically feasible
state trajectories from optimally smooth geometric paths between waypoints. A
differentially flat model of the CMD is derived, and the suitability of this method
for the generation of state trajectories between waypoints is demonstrated. A
novel approach to the generation of toward minimum-time velocity constrained
trajectories is then developed, allowing the generation of dynamically feasible and
velocity constrained state trajectories through multiple waypoints. This is again
demonstrated in both simulation and on an experimental CMD prototype. Finally,
a novel approach to polynomial trajectory optimisation is demonstrated, in which
a concept known as sum-of-squares programming is used to yield an average 40%
reduction in constrained polynomial optimisation time over the state-of-the-art.

The ability for dynamically balancing omnidirectional robots to plan dynam-

ically feasible state trajectories through multiple temporally free position way-
points is a key precursor to enabling the autonomous navigation of an environ-
ment. This is a more challenging task than the simple kinematic planning that is
commonly used to generate trajectories for statically-stable holonomic wheeled
vehicles, as the underactuation property of dynamically balancing robots means
they cannot be commanded to follow arbitrary trajectories through configura-
tion space. The generated trajectories must therefore meet the desired naviga-
tion goals and constraints, whilst also evolving exactly on the surface of feasible
trajectories satisfying one or more second-order nonholonomic constraints.
Trajectory optimisation methods for general constrained nonlinear systems can

169
Chapter 7 Fast Online Trajectory Optimisation

typically be grouped into two categories: shooting methods, and transcription

methods [31, 83, 127]. The single shooting method is the simplest form of tra-
jectory optimisation, in which at each optimisation iteration a parametrised in-
put function u(t) and trajectory duration te are defined to allow the continuous
Runge-Kutta integration of the full nonlinear dynamics over t ∈ [0, te ]. Some
suitable cost function can then be evaluated along the resulting state and input
trajectories, and satisfaction of constraints can be evaluated. By numerically
calculating gradients along each optimisation variable a search direction can be
defined, allowing the iterative improvement of u(t) and te toward a minimum.
While these methods are suitable for systems with simple dynamics and with a
good initial guess, when applied to highly nonlinear and unstable systems such
as the CMD a small variation in the optimisation variables can yield vastly dif-
ferent state trajectories, making such an optimisation ill-conditioned. This can
be greatly improved using multiple shooting methods, in which the trajectory
is split into multiple sections, with equality constraints used to enforce continu-
ity at segment boundaries. While reducing solution time, this method is still
computationally expensive even for simple systems.

For complex nonlinear systems, transcription methods typically yield faster

solutions [127]. These methods split the trajectory into multiple sections as in
multiple shooting, but instead of simulating the full nonlinear dynamics between
nodes the state and input dynamics are instead approximated using polynomial
splines. These techniques have been applied to complex dynamic systems such as
that in this thesis, with solution times in the region of hundreds of milliseconds
for simple trajectories [83]. However, complexity and solve time rapidly increases
for trajectories through multiple waypoints, still limiting the suitability of these
methods for real-time planning in this application.

Trajectory optimisation can also be approached by extension of existing sam-

pling based kinematic planners commonly used in ground vehicle and manipula-
tor trajectory planning to incorporate dynamic constraint satisfaction, referred
to as kinodynamic planning, of which kinodynamic RRT* is the most commonly
encountered example [128, 129]. This method builds a random tree of trajecto-
ries in the system’s configuration space, rooted at the system’s initial state. By
integrating a linearisation of the system dynamics between nodes all resulting
trajectories through the tree are approximately dynamically feasible. However,

170
this too is a computationally expensive method, taking seconds to minutes to
plan simple trajectories between waypoints for similar nonlinear systems [129].
An approach used in trajectory generation for ball-balancing robots optimises
lean angle trajectories comprised of piecewise summed parametrised hyperbolic
secant and cubic spline functions [74], forming a single shooting trajectory opti-
misation problem. However, this choice of basis function requires the solution of a
relatively complex constrained nonlinear program, again at high computational
cost, and through this basis can only optimise over a limited set of solutions.
Nagarajan presents a further method for the planning of ball-balancing robot
trajectories in which desired Cartesian trajectories in the external variables are
defined using high-order polynomials p(t). In recognising that in a neighbour-
hood of the origin there exists a linear mapping of steady state external variable
accelerations p̈(t) to the shape variables q(t), i.e. the body lean angles, a linear
... ....
mapping p̈(t) = K1 q(t), p (t) = K2 q̇(t), p (t) = K3 q̈(t) can be optimised to min-
imise the error J = kp̈(t) − p̈∗ (t)k22 , where p∗ (t) represents the external variable
acceleration trajectory derived from evaluation of the second order nonholonomic
constraint imposed by the system’s underactuation along q(t) [82]. This optimi-
sation forms a nonlinear program, with computation times of around 1 s for short
trajectories, so can feasibly be implemented in real-time on slow moving systems.
This method yields exactly dynamically feasible trajectories, at the expense of
a small deviation from the infeasible desired Cartesian trajectory. As this linear
map only exists in a neighbourhood of the origin it will decrease in validity for
larger lean angles. This method is also applied to a system with a pair of 2-DOF
arms, with minimal increase in problem complexity [130].
Despite this plethora of trajectory optimisation methods, none are able to
provide sufficiently fast execution times so as to allow for the real-time planning
required in this application, preventing fast reaction to dynamic obstacles and
large disturbances.
A less computationally demanding alternative applies the concept of differen-
tial flatness to the planning problem, a technique that allows for state trajectories
to be calculated algebraically from sufficiently continuously differentiable geomet-
ric trajectories in some possibly fictitious system outputs [20]. This is similar to
Nagarajan’s shape-space planner, but instead of optimising a linear mapping of
external to shape variables to minimise tracking error, this map is instead model-

171
Chapter 7 Fast Online Trajectory Optimisation

derived, constant, and potentially nonlinear. This allows the planning problem
to be addressed in a top down manner by optimising trajectories in the system’s
outputs rather than its inputs, yielding less computationally demanding prob-
lem formulations than shooting or transcription methods. This subset of the
trajectory planning literature mainly focuses on quadrotors, which are naturally
differentially flat systems [22, 23], and therefore well suited to this type of plan-
ning, though some research has been undertaken into applying these techniques
to ball-balancing robots [73, 81], in which smooth trajectories between position
waypoints are generated for a single planar direction of a ballbot.
Differential flatness based trajectory planning techniques are therefore more
suitable for this application. As no differential model of a CMD exists in the
literature this is first derived.

7.1 Differentially Flat Model Derivation

From Chapter 3 the dynamic model of a Collinear Mecanum Drive can be de-
scribed in terms of inertial frame positions p and local frame body velocities v in
the form
M (p)v̇ + C(p, v)v + G(p) + F v = Bτ (7.1.1)

By treating these as four simultaneous equations, equation 4 of these can be

divided by rw and summed with equation 2 to isolate the system’s internal dy-
namics by elimination of τ , yielding an expression of the form

f (θp , θ̇p , θ̈p , vx , vy , v̇y , φ̇) = 0 (7.1.2)

This contains multiple orders of derivative of θp , and therefore does not allow an
expression of θp as an algebraic function of the system’s Cartesian positions and
their derivatives, meaning the CMD’s Cartesian positions and heading are not
differentially flat outputs.
Shomin [73] showed that a single planar axis of a ball-balancing robot can be
approximately differentially flattened by a combination of model simplification
and a flat output of the form S = y + λθp . This technique can be applied to a
CMD and extended to the fully coupled nonlinear model by choosing the system’s

172
 7.1 Differentially Flat Model Derivation

flat outputs as

S1 = x − sin(S3 )λθp (7.1.3a)

S2 = y + cos(S3 )λθp (7.1.3b)
S3 = φ (7.1.3c)

with some time invariant λ. The system states x, y, vx , vy , v̇y , and all derivatives
of φ can be trivially derived by differentiation and rotation of the flat outputs as

x = S1 + sin(S3 )λθp (7.1.4a)

y = S2 − cos(S3 )λθp (7.1.4b)
φ = S3 (7.1.4c)
vx = cos(S3 )Ṡ1 + sin(S3 )Ṡ2 + λθp Ṡ3 (7.1.4d)
vy = cos(S3 )Ṡ2 − sin(S3 )Ṡ1 − λθ̇p (7.1.4e)
v̇y = − cos(S3 )Ṡ1 Ṡ3 − sin(S3 )Ṡ2 Ṡ3 − sin(S3 )S̈1 + cos(S3 )S̈2 − λθ̈p (7.1.4f)
φ̇ = Ṡ3 (7.1.4g)
φ̈ = S̈3 (7.1.4h)

Substituting (7.1.4a)-(7.1.4h) into (7.1.2) and performing a small angles ap-

proximation of sin(θp ) and cos(θp ) about θp = 0 yields the expression
 
0 = θp a + eṠ32 + f λṠ32 + cθp θ̇p2 − bθ̇p λ − θ̈p (g + dλ)
   
− sin(S3 ) bṠ1 + dS̈1 + hṠ2 Ṡ3 + cos(S3 ) bṠ2 + dS̈2 − hṠ1 Ṡ3 (7.1.5)

where a to h are time invariant positive constants1 . θ̈p can be eliminated from this
expression by selection of λ = −g/d. It is a safe assumption that the centripetal
force acting on the θp generalised coordinate due to θ̇p will always be small
relative to other forces, so the term cθp θ̇p2 can be omitted. Finally, the term
−bθ̇p λ is found to vanish when rolling friction is not considered, i.e. vrw = 0,
so from the knowledge that rolling friction has only a small effect on system
dynamics it is assumed that this term can be omitted without significant impact
1
redefined from the definitions in previous chapters

173
Chapter 7 Fast Online Trajectory Optimisation

on the accuracy of the differentially flat model. This allows for solution of θp as
   
sin(S3 ) bṠ1 + dS̈1 + hṠ2 Ṡ3 − cos(S3 ) bṠ2 + dS̈2 − hṠ1 Ṡ3
θp = (7.1.6)
a + eṠ32 − f (g/d) Ṡ32

in which all differentials of θp have been eliminated, converting this from an ODE
to a simple algebraic equation.

θ̇p and θ̈p can then be determined by differentiation of θp , thus algebraically

defining all system states in terms of the flat outputs (S1 , S2 ) ∈ C 4 and S3 ∈ C 3 .
Finally, as state trajectories θp and θ̇p are now known to evolve with approximate
satisfaction of the system’s nonholonomic dynamic constraint, the associated
input trajectories τ can be derived by inversion of the system dynamics in (7.1.1)
using the pseudo-inverse B + and substitution with states in terms of S. This
inversion only exists for feasible accelerations v̇, but as these accelerations are
to be derived from the differentially flattened model it is assumed that they will
be sufficiently close to the feasible set for this assumption to be valid, and thus
the inverse exists. The resulting expression is too large to be printed here and is
therefore omitted.

Note that this method of differential flattening introduces a singularity at

r
−a
Ṡ3 = ± (7.1.7)
e − f (g/d)

and is divergent as Ṡ3 → ± e−f (g/d) , which for the parameters in Table 4.2 occur
q
−a

at Ṡ3 = ±23.8 rad s−1 . Fortunately this rate of rotation is unlikely to occur in
any real-world scenario.

This model is demonstrated in Figure 7.1, in which corresponding state and

input trajectories are derived using the differentially flat model from a trajectory
in S2 representing a translation of S2 = 0 to S2 = 1 in 2 s, with the first to fourth
derivatives of S2 constrained to zero at t = 0 and t = 2. Note how despite the
flat output being monotonic, the y state trajectory correctly first translates away
from the target in order to start leaning towards the target, and likewise at the
end of the trajectory, thus capturing the non-minimum phase behaviour present
in this system.

174
 7.2 Closed Loop Trajectory Tracking

1 0.4
0.2
0.5 0
y −0.2
0 S2 −0.4 θp
0 0.5 1 1.5 2 0 0.5 1 1.5 2

1.5 2
vy
θ̇p
1 Ṡ2 1
0.5 0
0
−1
0 0.5 1 1.5 2 0 0.5 1 1.5 2
0.1
τ

−0.1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

Figure 7.1: A trajectory in S2 for a translation from S2 = 0 to S2 = 1 in 2 s,

with corresponding state and input trajectories derived from the differentially
flat model. The first to fourth derivatives of S2 are constrained to zero at t = 0
and t = 2 so that this represents a rest-to-rest trajectory. The other two flat
outputs remain at rest. All resulting τi trajectories are identical, so only one is
shown.

7.2 Closed Loop Trajectory Tracking

While perfectly dynamically feasible state and input trajectories can in theory be
tracked in open-loop, in reality the system will quickly deviate from the target
trajectory. This is due to model parameter uncertainty, unmodelled dynamics,
external disturbances, and in this case the simplifications required in the deriva-
tion of the differentially flat model, all compounded by the unstable dynamics of
the open-loop system about the upright equilibrium. A fast tracking controller
is therefore required to regulate the error between the current state and time

175
Chapter 7 Fast Online Trajectory Optimisation

varying optimal state trajectories to zero. As asymptotic convergence can only

be achieved with a perfect error model, integral action is incorporated to ensure
convergence.

Consider the error dynamics obtained by rotating Cartesian position error and
its integral into the local body frame
  R 
e1 (xr − x) dt
R
 e2   (yr − y) dt 
  
  R 
 e3   (φr − φ) dt
   
 e4   xr − x 
    
T
 e5  REB 0 0 0
   
   0  yr − y 
1 0 0   
e =  e6  =   φr − φ  (7.2.1)
   
T
 e7   0 0 REB 0 
     
θpr − θp 
  0 0 0 I6×6  
 e8   vxr − vx 
  
   
 e9   vyr − vy 
   
e10   φ̇r − φ̇ 
   

e11 θ̇pr − θ̇p

with derivative    
ė1 e2 φ̇ + e4
 ė2   −e1 φ̇ + e5 
   
   
 ė3   e6 
   
 ė4   e5 φ̇ + vxr − vx 
   
   
 ė5  −e4 φ̇ + vyr − vy 
   
ė =  ė6  =  φ̇r − φ̇ (7.2.2)
   

   
 ė7  
   θ̇pr − θ̇p 

 ė8   v̇xr − v̇x
   

   
 ė9  
   v̇yr − v̇ y


ė10   φ̈r − φ̈
   

ė11 θ̈pr − θ̈p

Substituting system dynamics of the form ẋi = fi (x, u), i ∈ [1 . . 8] yields the

176
 7.2 Closed Loop Trajectory Tracking

error dynamics

ė1 = e2 f3 (x, u) + e4 (7.2.3a)

ė2 = −e1 f3 (x, u) + e5 (7.2.3b)
ė3 = e6 (7.2.3c)
ė4 = e5 f3 (x, u) + cos(φ)f1 (xr , ur ) + sin(φ)f2 (xr , ur )
− cos(φ)(f1 (x, u) − sin(φ)f2 (x, u)
(7.2.3d)
ė5 = −e4 f3 (x, u) − sin(φ)f1 (xr , ur ) + cos(φ)f2 (xr , ur )
+ sin(φ)(f1 (x, u) − cos(φ)f2 (x, u)
(7.2.3e)
ėj = fi (xr , ur ) − fi (x, u), i = [3 . . 8], j =i+3 (7.2.3f)

in which substituting x = xr − e and u = ur − eu yields an expression of the form

ė = F (xr , ur , e, eu , ei ) (7.2.4)

where  
Z t
xr − x
ei =  yr − y dt (7.2.5)
 
0
φr − φ

Linearising the error dynamics about e = 0, eu = 0, and ei = 0 yields the time

varying linear error model

ė = A(xr , ur )e + B(xr , ur )eu (7.2.6)

in which the error dynamics are linearised about the desired trajectory.
The goal of the controller is to regulate e → 0, ei → 0, eu → 0. As xr (t) and
ur (t) are to be derived from the differentially flat model they can be assumed
to be dynamically feasible, meaning e = 0 can be maintained in steady state
and therefore represents an equilibrium. This allows the design of a time-varying
infinite-horizon LQR, and thus the optimal algebraic definition of K in the control
law u = Ke + ur . While this linearisation decreases in accuracy with deviation
from the desired trajectory, it is assumed that any substantial deviation would

177
Chapter 7 Fast Online Trajectory Optimisation

be accounted for by online trajectory replanning, as significant deviations may

result in violation of constraints that would have otherwise been satisfied had the
system correctly followed the desired trajectory.

7.3 Trajectory Generation using Optimally Smooth

Polynomials

With a differentially flat model, it is now possible to calculate feasible state tra-
jectories from any geometric paths in the flat outputs satisfying (S1 , S2 ) ∈ C 4 ,
S3 ∈ C 3 . This is typically achieved by representing the transition of a flat out-
put from one state to another using parametrised univariate polynomial basis
functions, such as representing a path between two waypoints or a change in
velocity. While from the unisolvence theorem [131] it is known that there al-
ways exists a unique polynomial of at most degree nw − 1 that can be used to
describe a path between nw waypoints in the absence of constraints, the large
monomial powers required for paths through more than a handful of waypoints
quickly lead to poor numerical conditioning. It is therefore better to describe
trajectories through multiple waypoints in a piecewise continuous fashion using
multiple chained polynomials of lower degree, with equality constraints enforced
on a finite number of derivatives at the boundaries between these polynomial
segments.

Trajectories in each flat output with j ∈ [1 . . nw ] waypoints wj are therefore

defined as piecewise functions using i ∈ [1 . . nw − 1] univariate polynomials
pi (t) each defined over t ∈ [0, Ti ], with constraints on the initial and terminal
values of the zeroth to nd derivatives enforced at t = 0 and t = Ti . Segment
transitions are constrained to occur at wi , and continuity of segment derivatives
up to order nd are enforced at segment transitions. The first to nd th derivatives
at w1 are constrained in order for the trajectory to evolve from the system’s
initial state, and the zeroth to nd th derivatives at wnw are constrained to zero

178
 7.3 Trajectory Generation using Optimally Smooth Polynomials

for the trajectory to end with the system at rest.

for m = [1, nd ] (7.3.1a)

(m)
p1 (0)(m) = w1
pnw −1 (Tnw −1 )(m) = wn(m) for m = [1, nd ] (7.3.1b)
pi (Ti )(m) = pi+1 (0)(m) for i = [1, nw − 2], m = [2, nd ] (7.3.1c)
pi (Ti ) = wi for i = [1, nw − 1] (7.3.1d)

Work applying differentially flat trajectory generation to quadrotors [22, 23]

argues that desirable smooth trajectories can be generated by minimising the
2-norm of the kth derivative of a system of order k − 1, as this acts to penalise
changes in control action. This is important for quadrotors, as their thrust re-
sponse to setpoint changes is not instantaneous due to propeller inertia. This
yields the cost function for a single flat output

w −1 Z Ti
nX 2
dk pi (t)

J= dt (7.3.2)
0 dtk
i=1

Segment continuity at boundaries is enforced for the zeroth to nd th derivatives,

where nd should be sufficiently large so as to enforce piecewise continuity of the
resulting input trajectories, and is therefore typically chosen to equal the order of
the highest order flat output differential in the differentially flat input function.

7.3.1 Polynomial Cost and Piecewise Smoothness Requirements

In examining the differentially flattened inverse dynamics function τ = f (S) it

is apparent that S1 and S2 appear up to their fourth derivatives, the snap of
the outputs, and S3 appears up to its third derivative, the jerk of the output.
Following the argument used in the quadrotor literature k = 5 is selected for S1
and S2 , minimising the crackle of the output, and constraints are enforced up
to nd = 4. Similarly, for S3 k = 4 and nd = 3 are selected. However, in this
application it could be argued that due to wheel traction limitations and a desire
to minimise energy usage it may be more sensical to minimise the kth derivative
of a system of order k, as while making the input trajectory less smooth, this
will act to minimise kτ k22 to a greater degree than the previous scheme. Also, as
the flat expression for θ̈p contains coefficients of S1 and S2 , and as it is known
(4) (4)

179
Chapter 7 Fast Online Trajectory Optimisation

1
2

θp
y 0
1

0 −1
0 1 2 3 4 0 1 2 3 4

4
4
2 2

θ̇p
vy

0
0
−2
−4 −2
0 1 2 3 4 0 1 2 3 4

0.2 k=3
k=4
k=5
τ

−0.2
0 0.5 1 1.5 2 2.5 3 3.5 4

Figure 7.2: A comparison of choices of k in the cost function (7.3.2) for a tra-
jectory through six random waypoints with k = 3 (blue), k = 4 (red), and k = 5
(yellow). The differentially flat model is used to derive state and input trajecto-
ries from these polynomials, demonstrating the effect of polynomial smoothness
on the actual system trajectories. Clearly k = 3 yields non-smooth τ trajectories,
and k = 4 yields lower peak lean angles than k = 5.

that θ̈p ∝
∼ sin(θp ), minimising the snap of S1 and S2 also acts to reduce the peak
lean angle of the trajectory. Both of these schemes are compared in Figure 7.2,
along with k = 3. As predicted, a choice of k = 4 shows reduced peak θp and τ
trajectories over k = 5, whilst maintaining relatively smooth state trajectories.
However, a large reduction in smoothness is apparent in reducing k further to
k = 3, so by this metric k = 4 appears to be the most suitable choice.

180
 7.3 Trajectory Generation using Optimally Smooth Polynomials

7.3.2 Polynomial Degree Requirements

Each independent equality constraint increases the required polynomial degrees

of freedom by one. For trajectories with fully constrained initial and terminal
conditions through nw − 2 intermediary waypoints the minimum average degree
of the polynomial segments can be found as

2(nd + 1) + (nw − 2)(nd + 2)

deg(p) ≥ −1 (7.3.3)
nw − 1

with limits 
2n + 1
d for nw = 2
deg(p) ≥ (7.3.4)
n + 1
d for nw → ∞

Similarly, if only the zeroth derivative is constrained at the terminal waypoint,

such as in a receding horizon planning scenario, this requirement is reduced to

(nd + 1) + 1 + (nw − 2)(nd + 2)

deg(p) ≥ −1 (7.3.5)
nw − 1
≥ nd + 1 (7.3.6)

While deg(p) could be adapted with change in nw , for simplicity is it decided

that deg(p) = 2nd + 1 ∀ nw ≥ 2 in order to maintain feasibility when nw = 2
with fully constrained initial and terminal states. For nw > 2 the problem is
therefore underdetermined, so the extra DoF can be used to better minimise
the cost function. Future work could explore reducing polynomial degree as
nw increases, or even using segments of varying degree, which would decrease
problem complexity at some increase in trajectory cost.

7.3.3 QP Formulation

The cost function (7.3.2) is found to be quadratic in the polynomial coefficients,

so by concatenating these into a vector (7.3.2) can be rewritten in the quadratic
form
J = xT Hx (7.3.7)

181
Chapter 7 Fast Online Trajectory Optimisation

where x contains stacked coefficients of polynomials 1 to nw − 1 as

h iT
x = p1,1 . . . p1,deg(p) . . . pnw −1,1 . . . pnw −1,deg(p) (7.3.8)

and with H  0. The constraints (7.3.1) can be rewritten in the linear form
Ax = b where A ∈ Rn×m , m < n, rank(A) = m, defining the problem in the
standard quadratic program form

min xT Hx s.t. Ax = b (7.3.9)

x

This allows a unique solution for the globally optimally smooth piecewise contin-
uous trajectory using iterative quadratic programming techniques such as MAT-
LAB’s quadprog.

Alternatively, as this is a purely equality constrained quadratic program, it is

possible to use QR decomposition to find a faster non-iterative solution. By QR
decomposition A can be rewritten as
" #
h i R̂
AT = Q̂ QN (7.3.10)
0

which allows Ax = b to be rewritten as

Q̂T R̂T x = b (7.3.11)

x can then be split into its fixed and free DoF u and v as

x = Q̂u + QN v (7.3.12)

which by substitution into (7.3.11) allows the unique solution of u as

u = R̂−T b (7.3.13)

and therefore a partial solution of x as

x = QN v + Q̂R̂−T b (7.3.14)

Substituting this into the original quadratic cost function then allows solution of

182
 7.3 Trajectory Generation using Optimally Smooth Polynomials

v as

−1  
v = − QTN HQN Q̂R̂−T bHQN (7.3.15)

which by substitution into (7.3.14) yields a numerical solution for x with a sub-
millisecond execution time, even for very large numbers of waypoints. This
method is also numerically more robust than iterative solvers, typically achieving
a higher degree of optimality.

7.3.4 Experimental Unconstrained Trajectory Tracking

To demonstrate the accuracy of the differentially flat model, and the suitability
of describing flat output trajectories using polynomials, the trajectory shown in
Figure 7.1 is tracked in simulation, with the resulting state and input trajecto-
ries shown in Figure 7.3. These trajectories are generated from a single nonic
polynomial in S2 , with constraints enforced up to the fourth derivative, uniquely
defining the polynomial. In simulation there is clearly minimal deviation from the
planned state trajectories, indicating that they are nearly exactly feasible, and
thus the differentially flat model is of sufficient accuracy for this application. The
same trajectories are then tracked experimentally, with state and input trajecto-
ries given in Figure 7.4. This shows more deviation from the planned trajectories
than in simulation, though this is to be expected given the complex unmodelled
dynamics acting on the system. Despite these errors, minimal position overshoot
is observed, and the experimental trajectories strongly resemble that which was
planned.
A more complex trajectory is shown in Figure 7.5, in which a pirouetting path
is followed from (x, y, φ) = 0 at t = 0 to (x, y, φ) = (0, 2, 2π) at t = 4. This
shows good tracking of the x, y, vx , vy , φ, and φ̇ states, though while their
shapes are similar the θp and θ̇p state trajectories are poorly tracked. This is
both acceptable and expected; the system’s non-minimum phase property means
that any deviation of vy or its integral from the planned dynamically feasible
trajectory necessitates a significant deviation of θp from its reference if tracking
is to be resumed. It is therefore through this θp tracking error that good position
tracking of the remaining states is achieved. This experiment demonstrates how
this planning approach is able to generate complex coupled state trajectories
from a simple polynomial basis in the decoupled flat outputs.

183
Chapter 7 Fast Online Trajectory Optimisation

1
0.2

0.5 0
y θp
yr −0.2 θpr
0
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5

1
1 vy
vyr θ̇p
0.5
θ̇pr
0.5 0
−0.5
0
−1
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
·10−2
5 τ
τff
0

−5

0 0.5 1 1.5 2 2.5

Figure 7.3: Simulated tracking of the trajectory in Figure 7.1. The near-absence
of tracking error indicates that the differentially flat model generates close to
exactly dynamically feasible state and input trajectories for the isolated θp and
vy subsystems, at least for the range of θp angles present in this trajectory. All
τ and τff trajectories are equal, so only one of each is shown.

7.3.5 Selection of Segment Durations

The choice of segment durations Ti has a large effect on the resulting shape of the
optimised trajectories. This is demonstrated in Figure 7.6, in which the second
segment duration of a trajectory through three waypoints is varied from half of
to equal to the duration of the first segment, showing how elaborate paths can be
taken as segment durations are varied. This is undesirable, as while smoothness is
important, it is still desired that the robot chooses somewhat direct and energy
efficient paths. Also, arbitrary choices of Ti may lead to uneven distribution

184
 7.3 Trajectory Generation using Optimally Smooth Polynomials

1 θp
0.2
θpr
0.5 0
y
yr −0.2
0
0 1 2 3 4 5 0 1 2 3 4 5

1
1 vy
vyr 0.5
0.5 0
−0.5 θ̇p
0 −1 θ̇pr
0 1 2 3 4 5 0 1 2 3 4 5
·10−2
5 τ1
τ2
τ3
0 τ4
τr
−5

0 1 2 3 4 5

Figure 7.4: Experimental tracking of the trajectory in Figure 7.1. This shows
more deviation from the planned trajectories than in simulation, though this is
to be expected given the complex unmodelled dynamics acting on the system.
Despite these errors, minimal position overshoot is observed, and the tracked
trajectories strongly resemble that which was planned.

of cost along the trajectory, making some segments smoother than others and
producing step changes in the perceived ‘aggressiveness’ of the overall trajectory.
A cost function [22] used in the quadrotor trajectory planning literature to
optimise for Ti across n flat outputs is defined as
 
w −1 Z
n nX Tj 2
dk p

i,j (t)
X 
min dt + Kt Tj (7.3.16)
Tj  0 dpi,j (t)k 
i=1 j=1

where Kt is a scaling factor used to incentivize an appropriate reduction in seg-

ment durations. In practise this can be viewed as an analog to the ’aggressiveness’

185
Chapter 7 Fast Online Trajectory Optimisation

·10−2
5 1
vx
0.5 vxr
0
0
−5 x −0.5
xr −1
0 1 2 3 4 0 1 2 3 4

2
0
1
y −1 vy
0 yr vyr
0 1 2 3 4 0 1 2 3 4

4
6 φ̇
3
4 φ̇r
2
2 φ 1
φr
0 0
0 1 2 3 4 0 1 2 3 4

0.2
1 θ̇p
0.1
θ̇pr
0
0
−0.1
θp θpr
−0.2 −1
0 1 2 3 4 0 1 2 3 4
0.1 τ1
τ2
τ3
0 τ4
τr1
τr2
τr3
−0.1 τr4
0 0.5 1 1.5 2 2.5 3 3.5 4

Figure 7.5: Experimental tracking of a pirouetting trajectory from (x, y, φ) = 0

to (x, y, φ) = (0, 2, 2π). Wheel torque data is smoothed to improve readability.

186
 7.3 Trajectory Generation using Optimally Smooth Polynomials

2
T2 = 1.5 s
T2 = 1.75 s
1.5 T2 = 2s
T2 = 2.25 s
T2 = 2.5 s
y (m)

1 T2 = 2.75 s
T2 = 3s

0.5

0
−0.5 0 0.5 1 1.5 2
x (m)

Figure 7.6: Comparison of choice of T2 for a trajectory through the Cartesian

waypoints (0, 0), (1, 1), and (0, 2), with T1 = 3 s.

of a trajectory, with a given choice of Kt yielding similarly ’aggressive’ trajectories

across any set of waypoints.

This cost function can be globally minimised by gradient descent, which when
performed using the QR decomposition solution method in Section 7.3.3 can
be performed with sub-second execution times for tens of waypoints using an
Intel i7-4720HQ processor. If the QR decomposition method cannot be used,
for example if any inequality constraints are to be included, iterative quadratic
programming solvers make the process of numerically calculating gradients ex-
cessively expensive. This is compounded by the requirement to solve each QP
to a high degree of optimality, as too lax of an optimality tolerance can intro-
duce noise into the numerical gradient estimates. Figure 7.7 shows trajectories
through a set of waypoints with both identical segment durations and with op-
timised durations, in which Kt is chosen to yield an equivalent total duration.
This demonstrates how such an optimisation can yield more sensical and direct
paths, whilst also providing a ten-fold reduction in total trajectory cost.

187
Chapter 7 Fast Online Trajectory Optimisation

1 1

0.5 0.5

0 0

0 0.5 1 1.5 2 0 0.5 1 1.5 2

(a) T = {1, 1, 1, 1} s (b) T = {1.58, 0.42, 0.42, 1.58} s

Figure 7.7: Flat output (red) and state trajectories (blue) through five Carte-
sian waypoints (black markers) with constant segment durations of 1 s (a) and
optimised durations (b), in which Kt is chosen to yield equal total duration. The
initial QP has cost J = 6507, whilst the QP with optimised durations has cost
J = 629, a ten-fold reduction.

7.4 Velocity Constrained Trajectory Generation

In optimising a simple rest-to-rest trajectory for minimum crackle, the resulting

velocity trajectory forms a bell-shaped profile, in which the average velocity is
much smaller than the peak, visible in Figure 7.1. In real-world scenarios robotic
systems that operate in confined environments or around people must adhere
to velocity constraints to bound the system’s kinetic energy, meaning the peak
of this bell curve must lie within constraint bounds. Using the cost function
(7.3.16), Kt must be decreased to lengthen the duration of the trajectory until all
peak velocities lie within constraint bounds. This potentially decreases the peak
velocity of some segments much below the constraint in order to ensure constraint
satisfaction of the segment with greatest violation. Alternatively, these velocity
constraints can be represented by smooth barrier penalty functions, allowing the
optimal selection of segment durations using the same cost function (7.3.16), so
that all segments that were constraint violating have their peak velocities reduced
to equal the constraint, at a cost of increased problem complexity. However, both
of these methods yield far from time-optimal trajectories, as the resulting velocity
profiles between waypoints will only equal the constraint at a single point at most,
with the majority of the trajectory being slower than constraints allow.

188
 7.4 Velocity Constrained Trajectory Generation

1.5
1 p(t) ṗ(t)
1
0.5
0.5

0 0
0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1
t (s) t (s)
10 ...
p̈(t) p (t)
5 100

0
0
−5
−10 −100
0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1
t (s) t (s)

Figure 7.8: A high-degree velocity constrained polynomial exhibiting Runge’s

phenomena, in which approximating a constant velocity requires large variations
in the polynomial’s higher order derivatives.

An ideal velocity constrained rest-to-rest trajectory would reach v as quickly as

the smoothness-to-time cost ratio allows, maintain v for an appropriate amount
of time, before decelerating as quickly as the smoothness-to-time cost ratio al-
lows. This can be approximated by using polynomials of increased degree, with
the constant velocity part of the trajectory flattening with increasing deg(p(t)).
This method, however, will always exhibit some deviation from constant velocity
during this middle section, and yield undesirable higher trajectory derivatives due
to Runge’s phenomenon, demonstrated in Figure 7.8. Furthermore, any increase
in deg(p(t)) requires larger monomial powers, quickly leading to poor numerical
conditioning of the resulting optimisation when t 6≈ 1.

7.4.1 Splitting Polynomial Segments

This desired trajectory shape can instead be achieved by splitting all polynomial
segments into three separate polynomial subsegments, whilst enforcing the same
continuity of derivatives between subsegments, and without any intermediary po-

189
Chapter 7 Fast Online Trajectory Optimisation

p(t)
1 ṗ(t)
p̈(t)
0

−1

0 0.5 1 1.5 2 2.5 3

t (s)

Figure 7.9: A point-to-point trajectory split into three subsegments, with a linear
polynomial describing the middle subsegment. Dotted vertical lines represent
subsegment boundaries. This demonstrates how this description of point-to-point
trajectories in the presence of velocity constraints can be better parametrised by
three concatenated polynomial subsegments.

sition constraints. This is exampled in Figure 7.9. Whilst for distant waypoints
the middle of these three subsegments should be optimised to represent a con-
stant velocity, and could therefore be represented by a linear polynomial, such
a parametrisation will yield less smooth solutions for closely spaced waypoints
where the velocity constraint is not reached. The degrees of the three new sub-
segments are therefore chosen to be equal. This parametrisation is demonstrated
in Figure 7.9 for a 1 m point-to-point trajectory with equal segment durations of
1 s.

For state-to-rest trajectories through nw − 2 intermediary position waypoints

each subsegment requires an average subsegment polynomial degree of

2(nd + 1) + 2(nd + 1) + (3(nd + 1) + 1)(nw − 2)

deg(p) ≥ −1 (7.4.1)
3(nw − 1)
nd 1
≥ nd + + (7.4.2)
3(nw − 1) 3

with limits 
 4(nd +1) − 1 for nw = 2
deg(p) ≥ 3
(7.4.3)
n + 1
d for nw → ∞

190
 7.4 Velocity Constrained Trajectory Generation

or in a receding horizon problem requires

(nd + 1) + 1 + 2(nd + 1) + (3(nd + 1) + 1)(nw − 2)

deg(p) ≥ −1 (7.4.4)
3(nw − 1)
1
≥ nd + ∀ nw ≥ 2 (7.4.5)
3

7.4.2 Velocity Constraint Approximation

As discussed in Chapter 6, whilst exact constraint satisfaction is achieved by

enforcing ẋ2 + ẏ 2 ≤ v 2 this forms a quadratic constraint, greatly increasing the
complexity of any optimisation. This can be avoided by approximation with a
convex polytope [124], defined by the linear inequalities
       
2πm 2π(m + 1) 2πm
ẏ − v sin cos − cos
M M M
       
2πm 2π(m + 1) 2πm
− ẋ − v cos sin − sin ≥ 0 (7.4.6)
M M M

where M ∈ [3 . . ∞], m ∈ [1 . . M ]. Choices of M = [4, 8, 16] are shown in

Figure 7.10. Whilst this linear formulation yields some simplification, any choice
of M other than M = 4 prevents the decoupling of the S1 and S2 trajectories
during polynomial optimisation, resulting in around a ten-fold increase in solver
execution time over the simpler box constraint. M = 4 is therefore chosen to
√
maintain feasible execution times, and v is assumed to be scaled to 2v as to
access the full constrained velocity set. Whilst this allows a constraint violation
along the diagonals of the global coordinates, this violation remains bounded to a
worst case of 41%. More computationally efficient methods for minimising these
violations are introduced later.

7.4.3 Velocity Constraint Enforcement using Quadratic

Programming

Velocity constraints can be incorporated into the QP in (7.3.9) as linear inequal-

ities in the form Ax ≤ b. This requires a finite discrete set of c = [1 . . n] values
of t to be chosen at which the constraint is to be enforced as ±ṗ(tc ) ≤ v. One
obvious choice is to uniformly sample tc ∈ [0, Ti ] for every subsegment, a method
known as gridding. A single QP solution is then numerically found, albeit with

191
Chapter 7 Fast Online Trajectory Optimisation

1 ẋ2 + ẏ 2 ≤ v 2
M = 16
M =8
0.5 M =4

0
ẏ

−0.5

−1
−1 −0.5 0 0.5 1
ẋ

Figure 7.10: Polytope approximations of the quadratic velocity constraint ẋ2 +

ẏ 2 ≤ v 2 with v = 1.

a large number of inequalities, and as long as a feasible solution is found it is

assumed that the continuous constraints are satisfied, though violation could still
occur between the grid points. In practice this method can generate oscillatory
trajectories, in which the resulting velocity profile repeatedly satisfies constraints
where they are sampled but then violates them between.
Alternatively, velocity constraints can be enforced in a recursive manner, pro-
viding a guarantee of constraint satisfaction. This method is described in Algo-
rithm 1. On the first iteration the QP’s decision variable invariant H, Aeq , and
beq matrices and vectors are calculated, and the A and b matrix and vector in the
inequalities Ax ≤ b are initialised as empty. The QP is then solved, providing
the optimally smooth connecting path between the waypoints in the absence of
velocity constraints. Numerical root finding is then used to find the roots tr of
the second derivative of every subsegment within tr ∈ [0, Ti ], i.e. the values of
tr within tr ∈ [0, Ti ] where p̈(tr ) = 0. These represent local velocity maxima
and minima along the trajectory, and so if ṗ(tr ) at these points is velocity con-
straint violating tr represents the instant of peak violation. Appropriately signed
inequalities can then be inserted at these points by addition of rows to A and
b, and the QP is resolved. This process repeats until all local velocity maxima
and minima lie within constraint bounds, or fails if an iteration renders the QP

192
 7.4 Velocity Constrained Trajectory Generation

2
Unconstrained
1 Constrained
p(t) (m)

Waypoint
0 Subsegment
−1 Boundary

−2
0 0.5 1 1.5 2 2.5 3 3.5 4
t (s)
2 Unconstrained
Constrained
ṗ(t) (m s−1 )

1
±v
0 Subsegment
Boundary
−1

0 0.5 1 1.5 2 2.5 3 3.5 4

t (s)

Figure 7.11: A comparison of the original unconstrained polynomial QP solu-

tion with the proposed velocity constrained planner. Subsegment boundaries are
denoted by vertical dotted lines, the timings of which are manually specified.

infeasible.
An example of this trajectory planner is shown in Figure 7.11, along with the
original QP solution without enforcement of velocity constraints. Subsegment
durations are selected by a method introduced later in this chapter.

7.4.4 Velocity Constraint Enforcement using Sum-of-Squares

Programming

While in the recursively constrained QP method each individual QP can be solved

very quickly, often up to around ten iterations can be required to fully constrain
the trajectory. When combined with the overhead of performing numerical root
finding on every subsegment at each iteration, this can result in a significantly
longer overall execution time than the original unconstrained QP. Here a novel
non-recursive method is presented, utilizing sum-of-squares (SOS) programming
to directly enforce the velocity constraints in continuous time, rather than a dis-
crete enforcement at a recursively grown list of sampling points. This is achieved

193
Chapter 7 Fast Online Trajectory Optimisation

Algorithm 1: Recursively velocity constrained QP algorithm

Result: Concatenated polynomial coefficients x
Define H, Aeq , beq ;
A0 = [], b0 = [];
for k=1:maxIter do
Solve min xT Hx s.t. Aeq x = beq , Ax ≤ b;
x
if QP is infeasible then
break;
end
for j=1:nw -1 do
Use numerical root finding to find all ti , i ∈ [1 . . nr ] where
p̈j (ti ) = 0;
for i=1:nr do
if ti ∈ [0, Tj ] then
if |ṗj (ti )| > v then
Add row to Anew and bnew to enforce |ṗj (ti )| ≤ v;
end
end
end
end
Ak = ATk−1 ATnew , bk = bTk−1 bTnew ;
 T  T

if Ak = Ak−1 then
Ak is invariant, therefore constraints are satisfied;
return x
end
end
return Failed to iteratively constrain trajectory, problem is infeasible.

whilst maintaining both the same guarantee of optimality and the same solution
space as the recursively constrained approach.
A polynomial p(t) of degree 2d that is a sum-of-squares polynomial can be
written in the Gram matrix form

p(t) = z(t)T Hz(t), H0 (7.4.7)

where z(t) is a column vector containing the monomials of p(t) up to degree d,

and H is positive semidefinite. If a univariate polynomial p(t) can be represented
in this form, then p(t) ≥ 0 ∀ t ∈ R. All univariate non-negative polynomials are

194
 7.4 Velocity Constrained Trajectory Generation

sum-of-squares polynomials, meaning this basis retains the same set of solutions
as that achieved by recursively enforcing non-negativity as in the previous section
[132].
Furthermore, the constraint p(t) ≥ 0 can be enforced on just the interval
t ∈ [a, b] if p(t) can be decomposed in the form

s(t) + (t − a)(b − t)q(t), if deg (p(t)) is even
p(t) = (7.4.8)
(t − a)s(t) + (b − t)q(t), if deg (p(t)) is odd

where s(t) and q(t) are SOS. For even deg (p), deg (p) = 2d, deg (s) ≤ 2d, and
deg (q) ≤ 2d − 2. For odd deg (p), deg (p) = 2d + 1, deg (s) ≤ 2d, deg (q) ≤ 2d
[133]. Similarly, all univariate polynomials that are non-negative on an interval
can be decomposed in this way, so again this basis does not yield a reduced
polynomial set.
It is therefore possible to enforce the constraint −p ≤ p(t) ≤ p ∀ t ∈ [a, b] by
considering the necessary equivalence

p + p(t) ≡ s1 (t) + (t − a)(b − t)q1 (t) (7.4.9)

p − p(t) ≡ s2 (t) + (t − a)(b − t)q2 (t) (7.4.10)

which can be achieved by enforcement of the four SOS constraints

SOS (p + p(t) − (t − a)(b − t)q1 (t)) (7.4.11)

SOS (p − p(t) − (t − a)(b − t)q2 (t)) (7.4.12)
SOS (q1 (t)) (7.4.13)
SOS (q2 (t)) (7.4.14)

for the even deg(p(t)) case, and

 
p + p(t) − (b − t)q1 (t)
SOS (7.4.15)
(t − a)
 
p − p(t) − (b − t)q2 (t)
SOS (7.4.16)
(t − a)
SOS (q1 (t)) (7.4.17)
SOS (q2 (t)) (7.4.18)

195
Chapter 7 Fast Online Trajectory Optimisation

for the odd deg(p(t)) case.

Applying these constraints to the first derivative of every subsegment allows
the enforcement of the velocity constraint |ṗi (t)| ≤ v ∀ t ∈ [0, Ti ].
Writing the convex QP (7.3.9) in the epigraph form

min t s.t. xT Hx ≤ t, Aeq x = beq , Ax ≤ b (7.4.19)

x,t

and using the Schur complement test for positive semidefiniteness, with positive
definite H decomposed as H = P T P by eigendecomposition, allows (7.4.19) to
be written in the linear matrix inequality form
 
Iρ Px 0ρ×m
(7.4.20)
 T T
x P t 01×m 0


0m×ρ 0m×1 diag(b − Ax)

where m = rank(A), ρ = rank(P ). This allows the definition of an equivalent

semidefinite program (SDP)
 
Iρ Px 0ρ×m
min t s.t. xT P T t 01×m 0 (7.4.21)
 
x,t
0m×ρ 0m×1 diag(b − Ax)

The SOS constraints (7.4.11)-(7.4.18) can then be enforced by additionally

enforcing the positive semidefinite constraint H  0 in (7.4.7) for each SOS
polynomial. The implementation of this is handled by YALMIP’s SOS module
[134, 135], and the resulting SDP is solved by MOSEK V8.1 [136] using default
optimality tolerances.
The distributions of execution time over increasing waypoint number nw for
both the recursively constrained QP and SOS constrained SDP methods are given
in Figure 7.12 over nw ∈ [2 . . 10]. 1000 random sets of waypoints are evaluated
for each value of nw , generated by selecting nw − 1 waypoints from a standard
normal distribution, with the system initialised at the origin. Subsegment timings
are chosen as the minimum time solution to a constrained trapezoidal profile,
introduced in the next section. Both the QP and SDP solvers are provided by
the MOSEK software package for fairness of comparison, and all solver tolerances
are left at their defaults. The SOS constrained method shows an average 40%

196
 7.4 Velocity Constrained Trajectory Generation

0.25
Recursively Constrained QP
0.2 SOS Constrained SDP

0.15
texec (s)

0.1

0.05

nw = 10
nw = 2

nw = 3

nw = 4

nw = 5

nw = 6

nw = 7

nw = 8

nw = 9
Figure 7.12: Monte Carlo analysis of the distribution of solve time over waypoint
quantity for velocity constrained trajectories, solved using both the recursively
constrained QP and SOS constrained SDP methods. Each value of nw is eval-
uated using a 1000 point Monte Carlo simulation, where each set of waypoints
are drawn from a standard normal distribution. Subsegment durations are se-
lected as the minimum time solution to a constrained trapezoidal profile. This
shows that the new SOS constrained SDP trajectory optimisation method yields
a significant decrease in computation time over the recursively constrained QP
method.

reduction in computation time over the recursively constrained QP method, with

up to a 60% reduction observed around nw = 5. Both methods prove sufficiently
fast for use in online replanning, allowing for quick reaction to dynamic obstacles
and disturbance.

7.4.5 Velocity Constrained Subsegment Duration Selection

As in Section 7.3.5 it is necessary to select all segment durations a priori, which

now also involves the selection of subsegment durations, whilst maintaining total
segment duration coherence between flat outputs. This is more challenging than
in the unconstrained case, as now selecting too short of a segment duration
could require an average segment velocity greater than v, rendering the problem
infeasible.

197
Chapter 7 Fast Online Trajectory Optimisation

The globally optimal solution with a similar cost function as (7.3.16) can be
found by gradient descent as in Section 7.3.5, with velocity constraint violation
included using inequalities with a quadratic cost on violation. Using the recursive
QP method in Section 7.4.3 this optimisation takes tens of seconds for trajectories
through five random waypoints, and is therefore impossible to perform in real-
time.
While it is impossible to optimise segment and subsegment durations for the
full problem in real-time, it is possible to optimise a simplified problem suffi-
ciently quickly for real-time implementation. Noting the similarity between the
velocity constrained trajectories in Figure 7.11 and a piecewise constant accelera-
tion trapezoidal profile, every middle subsegment of each segment is simplified to
a linear polynomial, and all other subsegments are simplified to quadratic poly-
nomials. This reduced basis is then optimised for minimum time whilst enforcing
velocity and acceleration constraints. This simplified formulation also allows the
exact enforcement of the quadratic velocity constraint ẋ2 + ẏ 2 ≤ v 2 without a
significant increase in problem complexity, which by lengthening the durations of
segments during combined motion along x and y effectively acts to approximately
enforce the full quadratic velocity constraint on any subsequent full-degree op-
timisation. This alleviates some of the potential constraint violation introduced
in Section 7.4.2.
Defining subsegment polynomials as pi,j (t), waypoints as wi,j , and subsegment
durations as Ti,j , where i ∈ [1 . . 3] specifies the flat output index, and j ∈
[1 . . 3(nw − 1)] specifies the subsegment index, this nonlinear program can be
defined as

3 3(nw −1)

(7.4.22)
X X
min Ti,j
x,T
i=1 j=1
k+2
X k+2
X
s.t. Ti,j = Ti+1,j ∀ i ∈ [1 . . 2], k ∈ [1, 4, . . 1 + 3(nw − 1)]
j=k j=k
(7.4.23)
(n) (n)
pi,j (Ti,j ) = pi,j+1 (0) ∀ i ∈ [1 . . 3], j ∈ [1 . . 3(nw − 1)],
n ∈ [0 . . 1]
(7.4.24)

198
 7.4 Velocity Constrained Trajectory Generation

(7.4.25)
(n) (n)
pi,1 (0) = wi,1 ∀ i ∈ [1 . . 3], n ∈ [0 . . 1]
pi,3j (Ti,3j ) = wi,j+1 ∀ i ∈ [1 . . 3], j ∈ [1 . . nw − 1] (7.4.26)
2 2 2
(ṗ1,j (0)) + (ṗ2,j (0)) ≤ v ∀ i ∈ [1 . . 3], j ∈ [2, 5, . . 3(nw − 1) − 1] (7.4.27)
|ṗ3,j (0)| ≤ φ̇ ∀ j ∈ [2, 5, . . 3(nw − 1) − 1] (7.4.28)
|p̈i,j (0)| ≤ v̇ ∀ i ∈ [1 . . 2], j ∈ [1, 3, 4, 6, 7 . . 3(nw − 1)] (7.4.29)
|p̈3,j (0)| ≤ φ̈ ∀ j ∈ [1, 3, 4, 6, 7 . . 3(nw − 1)] (7.4.30)

This is made up of the cost function (7.4.22), constraints required to ensure

segment duration coherence across flat outputs (7.4.23), inter-subsegment conti-
nuity constraints (7.4.24), initial state and waypoint position constraints (7.4.25)-
(7.4.26), and velocity and acceleration constraints (7.4.27)-(7.4.30). By enforc-
ing exact velocity constraints the violations along the diagonals of the coordinate
system discussed in Section 7.4.2 can be significantly reduced.
This minimisation problem forms a nonlinear program (NLP), solved in this
thesis using MATLAB’s fmincon. NLP solve times for increasing numbers of
random waypoints are shown in Figure 7.13. This shows real-time execution fea-
sibility, again allowing for fast online replanning for up to nw ≈ 7. Furthermore,
assuming relatively sparse waypoints it is reasonable to assume that only the
first handful of waypoints will have a significant effect on the timings of the first
segment. This optimisation could therefore be approached in a receding horizon
fashion using a horizon of only three or four waypoints, which would allow for a
linear scaling of execution time as nw → ∞.
A minimum time trapezoidal profile trajectory through nine random waypoints
is shown in Figure 7.14, along with the corresponding constrained and optimally
smooth full-degree flat output trajectories. The trapezoidal trajectory in S2 is
visibly a bang-bang trajectory for the majority of the path, as would be expected
for a minimum-time solution. The only part of S2 that deviates from bang-
bang behaviour is that from the first to second waypoint, where constraints on
the trajectory in S1 prevent any further reduction in segment duration. This
demonstrates the coupling between flat outputs that prevents the minimum-time
problem from being addressed as three independent subproblems, frustrating the
development of a simpler heuristic-based subsegment duration allocation method.
A comparison of trapezoidal and globally optimised subsegment timings for

199
Chapter 7 Fast Online Trajectory Optimisation

Trapezoidal timing optimisation

0.6

solve time (s)

0.4

0.2

0
2 3 4 5 6 7 8 9 10
nw

Figure 7.13: Distribution of trapezoidal timing optimisation execution time for

increasing numbers of waypoints in three synchronised flat outputs, implemented
on an Intel i7-4720HQ processor.

five waypoints is shown in Figure 7.15, showing nearly identical solutions. This
two-stage duration selection scheme is found to yield trajectories that are close
to the globally optimal solution for smoothness over time, whilst achieving a 100
to 1000 fold reduction in execution time.

7.4.6 Experimental Velocity Constrained Trajectory Planning

and Tracking

As the recursively constrained QP and SOS constrained SDP optimization ap-

proaches yield identical outputs, and the trapezoidal timing allocation method
yields very similar segment timings to the globally optimal approach, there is little
to be gained by an experimental comparison of these methods. This final exper-
imental section is therefore intended to serve as a tour de force, demonstrating
both the performance and manoeuvrability achievable by the CMD using these
control and trajectory planning methods.
Figure 7.16 shows a rest-to-rest trajectory passing through a gap narrower than
the prototype’s width. This serves as a minimal example of the manoeuvrability
achievable by the CMD in confined spaces, and how smooth transitions can be
achieved between forward and lateral motion. State trajectories for this experi-

200
 7.4 Velocity Constrained Trajectory Generation

S1,trap S1,SDP S2,trap S2,SDP

2 2

0 0

−2 −2
0 5 10 15 20 0 5 10 15 20

Ṡ1,trap Ṡ1,SDP Ṡ2,trap Ṡ2,SDP

1 1

0 0

−1 −1
0 5 10 15 20 0 5 10 15 20

S̈1,trap S̈1,SDP S̈2,trap S̈2,SDP

1 1

0 0

−1 −1

0 5 10 15 20 0 5 10 15 20

Figure 7.14: Minimum time trapezoidal (blue) and full degree optimally smooth
(red) trajectories through nine waypoints (black markers) with constraints v = 1,
a = 1. The trajectory in S3 is left at rest and not shown. Vertical dashed lines
denote subsegment boundaries.

ment are shown in Figure 7.18, demonstrating good tracking, further validating
the accuracy of the differentially flat model.
Figure 7.17 shows a rest-to-rest pirouetting trajectory from the origin to (x, y,
φ) = (0, 2, 4π), in which two blue LEDs are used to capture the tracked trajec-
tory. This again demonstrates smooth transitions between different body relative
directions of motion, and shows zero overshoot. Corresponding state trajectories
are shown in Figure 7.19. While a small tracking error is observed, this error

201
Chapter 7 Fast Online Trajectory Optimisation

2 (S1 , S2 )trap (x, y)trap

(S1 , S2 )glob (x, y)glob
1.5

0.5

0
0 0.5 1 1.5 2 2.5 3 3.5 4

Figure 7.15: Optimally smooth flat output (blue) and state (red) trajectories
for trapezoidal optimised polynomial durations (solid) and gradient descent op-
timised polynomial durations (dashed) through five waypoints (black markers),
with equal total duration between the two schemes.

Figure 7.16: Navigation of a gap narrower than the robot’s width, demonstrating
one of the advantages of the Collinear Mecanum Drive. This trajectory passes
through four waypoints, spaced over 2 m and progressing right across the figure
in a total of 3 s, in which a 90° φ angle is enforced at the middle two waypoints.

202
 7.5 Conclusion

Figure 7.17: A long exposure image of a trajectory performing a 2 m translation

and a 720° rotation in φ, with two blue LEDs used to capture the path of the
robot, moving left to right across the figure. This is intended to demonstrate the
smoothness of the resulting trajectories achievable with this approach, and the
accuracy with which they can be tracked.

remains bounded over the duration of the trajectory, and is indistinguishable in

the long exposure image in Figure 7.17.
Finally, as a demonstration of the complete freedom of movement attainable
by the CMD, Figure 7.20 shows a long exposure image capturing the prototype
tracking a circular trajectory through the four corners of a 1.5 m square, whilst
also performing a rotation in φ of 6π rad. This also serves as a good demonstra-
tion of the navigation accuracy achievable using only dead reckoning, showing a
drift of only around a 4 cm during this experiment. Reference and tracked state
trajectories for a trajectory similar to this are shown in Figure 7.21, in which a
reduced rotation of 4π rad is performed to improve the clarity of the figure.

7.5 Conclusion
This chapter has shown that through the selection of suitable fictitious flat out-
puts and model simplification the dynamic model of a CMD can be differentially
flattened, allowing the derivation of dynamically feasible state trajectories from
any sufficiently smooth trajectories in the flat outputs. The accuracy of this

203
Chapter 7 Fast Online Trajectory Optimisation

·10−2
vx
5 1 vxr
0
0.5
−5
x xr
0
0 1 2 3 0 1 2 3

1
2 vy vyr

0.5
1
y
yr 0
0
0 1 2 3 0 1 2 3

1.5 φ 2 φ̇ φ̇r
φr
1
0
0.5
0 −2
0 1 2 3 0 1 2 3

2
0.2 θ̇p θ̇pr
1
0
0
−0.2 θp θpr
−1
0 1 2 3 0 1 2 3

0.1 τ1
τ2
τ3
0 τ4
τff ,1
τff ,2
τff ,3
−0.1 τff ,4
0 0.5 1 1.5 2 2.5 3 3.5

Figure 7.18: State trajectories for the gap navigation experiment shown in Figure
7.16, in which a 2 m translation is performed through a gap narrower than the
CMD’s width, achieved by rotating the CMD to translate along its wheel axle.

204
 7.5 Conclusion

·10−2
5 1
vx
0.5 vxr
0
0
−5 x −0.5
xr
−1
0 1 2 3 4 0 1 2 3 4

2 1 vy
vyr
0.5
1 0
y −0.5
0 yr
−1
0 1 2 3 4 0 1 2 3 4

6 φ̇ φ̇r
10
4
5
φ 2
0 φr
0
0 1 2 3 4 0 1 2 3 4

0.4 2
θp θ̇p θ̇pr
0.2 θpr 1

0
0
−1
−0.2
0 1 2 3 4 0 1 2 3 4

0.1 τ1
τ2
τ3
0 τ4
τff ,1
τff ,2
τff ,3
−0.1 τff ,4
0 0.5 1 1.5 2 2.5 3 3.5 4

Figure 7.19: State trajectories for the pirouetting trajectory experiment shown in
Figure 7.17, in which the CMD translates from the origin to (x, y, φ) = (0, 2, 4π)
in 4 s.

205
Chapter 7 Fast Online Trajectory Optimisation

Figure 7.20: A long exposure image of a circular trajectory passing through the
four corners of a 1.5 m square whilst performing a rotation in φ of 6π rad. This
is intended to demonstrate the complete freedom of motion achievable using the
CMD.

model has then been demonstrated both in simulation and on the experimental
prototype, showing it generates nearly exactly dynamically feasible trajectories
that are closely tracked in real-world testing.
Two novel velocity constrained trajectory planners have then been demon-
strated, allowing for the online derivation of optimally smooth and constrained
trajectories through large numbers of waypoints. Finally, a method for the se-
lection of the timing properties of these waypoints as been demonstrated, which
is again sufficiently computationally efficient for real-time implementation. To-
gether, these methods allow for a CMD to smoothly navigate complex environ-
ments in a close to time-optimal fashion.
Future work would explore the autonomous selection of minimal sets of way-
points required to describe paths between locations in an occupancy map, allow-
ing the navigation of environments with obstacles. This problem has been well
addressed in the quadrotor trajectory planning literature [22, 137, 138]. In this

206
 7.5 Conclusion

2 1
x vx
xr 0.5 vxr
1 0
−0.5
0
−1
0 2 4 6 8 10 0 2 4 6 8 10

2
y 1 vy
yr vyr
1 0

0 −1
0 2 4 6 8 10 0 2 4 6 8 10

4
φ̇ φ̇r
10 3
2
5
φ 1
0 φr
0
0 2 4 6 8 10 0 2 4 6 8 10

0.4 θp θpr 2 θ̇p θ̇pr

0.2 1
0 0
−0.2 −1
0 2 4 6 8 10 0 2 4 6 8 10

0.1 τ1
τ2
τ3
0 τ4
τff ,1
τff ,2
τff ,3
−0.1 τff ,4
0 2 4 6 8 10

Figure 7.21: State trajectories for the same experiment as shown in Figure 7.20,
but whilst only performing a rotation of 4π rad in φ in order to improve clarity.

207
Chapter 7 Fast Online Trajectory Optimisation

application these methods could be extended to incorporate knowledge of the

non-spherical shape of this robot. This would allow for the appropriate choice
of heading trajectories in order to navigate smaller gaps than could be achieved
when using the standard spherical robot model, similar to that which has been
achieved for statically stable omnidirectional robots [40].

208
Chapter 8

Conclusion

This concluding chapter serves to summarise and present the work of this thesis
as a whole, performs a qualitative comparison of the CMD control and planning
techniques developed within, and sets out proposals for future work building on
that detailed in this thesis.

This thesis has presented a systematic and comprehensive analysis of the

Collinear Mecanum Drive, which should come to represent the de facto stan-
dard reference for the modelling and analysis of this type of system.
This analysis includes the kinematic and dynamics modelling of the CMD with
first order friction models, a proof of the CMD’s controllability, determination
of size of the maximum feedback linearisable subsystem, and an analysis of the
system’s underactuation and nonholonomy properties. Following this, three dis-
tinct approaches to the control of this system have been developed, implemented
in both simulation and on an experimental prototype, and analysed.
First, partial feedback linearisation and backstepping techniques are used to
develop local body frame velocity, inertial frame velocity, and inertial frame posi-
tion controllers, all with stability guarantees for the full nonlinear model. These
enforce asymptotic body acceleration constraints, yielding smooth motion and an
approximate wheel torque constraint. The two velocity controllers also enforce
an asymptotic lean angle constraint, and the position controller also enforces
asymptotic velocity constraints.
Second, an input and output constrained linear model predictive controller is
developed, allowing for the systematic satisfaction of constraints whilst main-
taining a stability guarantee for the linearised model. Despite the difficulties sur-

209
Chapter 8 Conclusion

rounding the implementation of online optimisation based controllers on plants

with fast unstable dynamics such as the CMD, the controller developed here
showed good experimental performance. This controller is most useful in scenar-
ios where aggressive control is required while suffering from poor wheel traction,
necessitating the rationing of available wheel grip, and tolerating prolonged wheel
actuator saturation without detrimentally affecting system stability.
Finally, a method for the tracking of smooth polynomials has been developed,
using the concept of differential flatness. A novel velocity constrained polynomial
trajectory planner is then demonstrated, allowing the planning and tracking of
near time-optimal trajectories through multiple position waypoints. Provided a
method of selecting suitable waypoints in a map, this method allows the optimal
navigation of an environment with obstacles.

8.1 A Comparison of Controllers

In this section a qualitative comparison of the properties of the backstepping,

MPC, and differential flatness based controllers developed in this thesis is per-
formed. While a quantitative comparison would usually be preferred, the three
controllers in this thesis are all designed for different end uses, and so a quan-
titative would not allow for a meaningful comparison. Instead, a qualitative
approach is used to compare the overall properties of the controllers, and their
suitability for different applications is considered.

8.1.1 Aggressiveness of Control

Defining the ‘aggressiveness’ of a controller as how near it is able to come to gen-

erating minimum-time trajectories for given input and state constraints, and to
what degree the controller is able to reject disturbances, an approximate compar-
ison of the maximum aggressiveness achievable by each controller can be made.
The backstepping controllers developed in Chapter 5 are inherently conserva-
tive due to the need to enforce the assumption of convergence of the inner linear
θp → θpr controller. This acts to limit θ̇p , preventing fast trajectory tracking
and disturbance rejection in the vy state and its integral. However, high band-
width disturbance rejection is maintained for the feedback linearised subsystems,

210
 8.1 A Comparison of Controllers

though gains must be chosen as to avoid excessive actuator saturation and sub-
sequent instability.
Chapter 6’s model predictive controller is the most aggressive developed in this
thesis, capable of optimally saturating the wheel torque setpoints for prolonged
durations without any negative effect on system stability, along with exactly
enforcing velocity and lean angle constraints. Decreasing R in the underlying cost
function effectively tends this controller towards the generation of minimum-time
bang-bang trajectories for the linearised plant.
The differential flatness based polynomial planner of Chapter 7 does not in-
corporate input constraint enforcement, and so segment durations must be con-
servative as to avoid actuator saturation. The tracking TVLQR controller is also
unable to enforce wheel torque constraints, so again gains must be conservatively
chosen as to maintain stability for a given degree of trajectory deviation and
input constraint.

8.1.2 Computational Requirements

The computational complexity of the developed controllers varies considerably

between methods, with a large impact on onboard processing requirements and
subsequent power consumption.
Chapter 5’s backstepping controllers possess the least demanding computa-
tional requirements of this thesis, allowing for implementation on low-cost mi-
crocontrollers. Conversely, Chapter 6’s model predictive controller brings the
greatest computational requirements, as every control iteration requires the nu-
merical solution of a constrained quadratic program whilst introducing as little
execution delay as possible, necessitating the use of a high-performance processor.
However, the CMD studied in this thesis represents a difficult CMD to which to
apply this control approach, as this particular system’s low inertia and center of
mass yield very fast dynamics and subsequent sensitivity to control computation
delay. A taller, human-sized system would possess significantly slower dynamics,
and would therefore be much less sensitive to this delay, potentially allowing for
implementation on low-power hardware.
The differential flatness based trajectory planning approach of Chapter 7 im-
poses different timing requirements for the inner trajectory tracking TVLQR
controller and for the outer trajectory planner. As the TVLQR controller is re-

211
Chapter 8 Conclusion

sponsible for closed-loop stability and high bandwidth disturbance rejection, real
time operation is required, which for this simple control formulation can be triv-
ially performed by a low-power microcontroller. The outer trajectory planner has
much greater computational requirements, but only needs to plan new trajecto-
ries during large disturbances or a change in objective, potentially allowing this
planning to be performed in parallel with other high-level tasks such as SLAM
and communication.

8.1.3 Actuation Power Requirements

To allow a comparison of the energy requirements of each method total actuation
power consumption can be approximated as purely that due to resistive losses
in the motor windings as E = I(t)2 dt, which is in turn proportional to motor
R

torque, so E ∝ kτ k22 dt. The most power efficient controller would therefore
R

be that which uses the least energy to perform some point-to-point manoeuvre,
provided equivalent total duration and aggressiveness of disturbance rejection.
This latter property is difficult to quantify, and so therefore an experimental
comparison is difficult to fairly perform.
The backstepping derived controllers all asymptotically constrain v̇x , φ̈, and θ̈p
accelerations, which will in turn act to indirectly minimise kτ k22 . This controller
does not incorporate any mechanism for the planning of the most efficient tra-
jectories through multiple waypoints. The MPC approach, on the other hand,
is able to both directly optimally minimise kτ k22 , whilst using advance reference
knowledge to optimise for minimal control action over a receding horizon. The
differential flatness based trajectory planning approach is able to perform this
minimisation in a more global sense, finding the smoothest possible path achiev-
able using the given polynomial basis between any number of waypoints. This
optimal smoothness is, however, only an analogue to minimising kτ k22 .

8.1.4 Safety
The safety of a controller is considered here in terms of the ability to maintain
closed-loop stability in the presence of input and output constraints. The nonlin-
ear backstepping control approach provides stability and convergence guarantees
for the full nonlinear plant, though conservative acceleration constraint selection
is required in order to avoid wheel slip. The MPC approach maintains a stabil-

212
 8.1 A Comparison of Controllers

ity proof whilst directly observing wheel torque constraints, though this proof
only holds for a linearisation of the CMD dynamics. The TVLQR controller
in Chapter 7 provides no such constrained stability guarantee, and can there-
fore only operate for small deviations from the planned trajectory, meaning large
deviations much be addressed by sufficiently fast trajectory replanning.

8.1.5 Smoothness and Grace of Motion

In user-facing applications smoothness and decisiveness of motion may be an im-

portant criteria in control performance. Here, the ‘gracefulness’ of a trajectory is
considered as the ability for a controller to achieve a desired motion with minimal
perceived control ‘action’, such as perceived changes in lean angle direction.
While in simulation the backstepping derived controllers yield smooth and
decisive trajectories, in practice model mismatch causes the inertial velocity and
position controllers to exhibit a small consistent variation in lean angle whilst
maintaining kvkφ̇ =
6 0. A lack of future reference knowledge utilisation prevents
these controllers from taking smoother paths through multiple waypoints.
The enforcement of torque and velocity constraints tends Chapter 6’s model
predictive controller toward aggressive transitions between constant and varying
torque and velocity trajectories, and no cost is associated with changes in input,
resulting in input trajectories with unbounded first derivatives. It’s receding
reference previewing horizon does, however, allow some trajectory optimisation
based on knowledge of future waypoints.
The smoothest and most graceful motion achieved in this thesis is that using
Chapter 7’s differential flatness based approach, as this planner directly optimises
for trajectory smoothness, and does not suffer from the same performance degra-
dation due to model mismatch as the backstepping derived approaches. This
planner finds the globally optimally smooth trajectory through any number of
waypoints, making maximal use of advanced reference knowledge to yield sensical
and direct paths.

8.1.6 Map Navigation

It is expected that in future the CMD will be required to navigate a 2D occupancy

map via some provided waypoints, whilst avoiding collisions with obstacles. This

213
Chapter 8 Conclusion

will therefore require the additional enforcement of positional constraints on the

system.
The backstepping derived nonlinear controllers of Chapter 5 are poorly suited
to this task, as no straightforward method is provided for the incorporation of
position constraints. The MPC approach is much more amenable to this task,
with the possibility of enforcing positional constraints in the same way as veloc-
ity constraints are already enforced, provided the chosen set of path constraints
remains convex. The differential flatness and polynomial based planning tech-
niques of Chapter 7 are even better suited, allowing the planning of smooth tra-
jectories around obstacles, whilst enforcing positional constraints using the same
techniques as used in velocity constraint enforcement. Again, this constraint set
must remain convex for each polynomial, so to navigate through a complex map
with multiple route options a sampling-based planner must be used to select the
desired convex obstacle corridor.

8.1.7 A Ranking of Controllers & Application Suitability

Table 8.1 aims to roughly assign a ranking to each of the presented controllers
against the above qualitative metrics. Conclusion as to controller suitability for
likely applications are then drawn. For use as a personal vehicle or teleoperated
platform, safety and smoothness of motion are of greatest importance, making
the backstepping controller of Chapter 5 the most suitable choice. For a fast
reactive or local planning application, such as an autonomous robot that is to
track a moving reference, the MPC controller of Chapter 6 is most suitable,
achieving fast setpoint tracking through optimal constraint satisfaction and to-
ward bang-bang control. Finally, for fully autonomous applications requiring
the navigation of an obstacle filled map, the differential flatness and polynomial
based planning techniques of Chapter 7 are most suitable, as these allow for
straightforward incorporation of obstacle avoidance whilst producing maximally
smooth trajectories.

8.2 Future Work

214
 8.2 Future Work

Table 8.1: A qualitative ranking of the three presented controllers against a num-
ber of metrics, where a value of 1 denotes the most suitable controller for a given
metric.
Metric Feedback MPC Differential
Linearised Flatness Based
Nonlinear Control Planning
Aggressiveness 3 1 2
Computational 1 2 3
Complexity
Power requirements 3 1 2
Safety 1 2 3
Smoothness of Motion 2 3 1
Map Navigation 3 2 1

topics listed as follows:

8.2.1

215
Chapter 8 Conclusion

8.2.2

8.2.3

216
Bibliography

[1] D. A. Winter, Biomechanics and motor control of human movement.

Wiley, 2009. [Online]. Available: https://onlinelibrary.wiley.com/doi/
book/10.1002/9780470549148

[2] R.W.Brockett, “Asymptotic stability and feedback stabilization,” in

Differential Geometric Control Theory, 1983, pp. 181–191. [Online].
Available: http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.324.
9912

[3] B. E. Ilon, “Wheels for a course stable selfpropelling vehicle movable in any
desired direction on the ground or some other base,” Patent US3 876 255,
1975. [Online]. Available: https://patents.google.com/patent/US3876255

[4] A. Gfrerrer, “Geometry and kinematics of the mecanum wheel,”

Computer Aided Geometric Design, vol. 25, no. 9, pp. 784–791,
Dec. 2008. [Online]. Available: http://www.geometrie.tugraz.at/gfrerrer/
publications/MecanumWheel.pdf

[5] L. Ferriere, B. Raucent, and G. Campion, “Design of omnimobile robot

wheels,” in Proceedings of IEEE International Conference on Robotics
and Automation, vol. 4. IEEE, 2002, pp. 3664–3670. [Online]. Available:
http://ieeexplore.ieee.org/document/509271/

[6] S. Dickerson and B. Lapin, “Control of an omni-directional robotic

vehicle with mecanum wheels,” in NTC ’91 - National Telesystems
Conference Proceedings. IEEE, 1991, pp. 323–328. [Online]. Available:
http://ieeexplore.ieee.org/document/148039/

[7] U. Nagarajan, G. Kantor, and R. L. Hollis, “Human-robot physical

interaction with dynamically stable mobile robots,” in Proceedings of the
4th ACM/IEEE international conference on Human robot interaction -

219
Bibliography

HRI ’09. New York, New York, USA: ACM Press, 2009, p. 205. [Online].
Available: http://portal.acm.org/citation.cfm?doid=1514095.1514176

[8] M. S. Ju and J. M. Mansour, “Comparison of methods for developing

the dynamics of rigid-body systems,” The International Journal of
Robotics Research, vol. 8, no. 6, pp. 19–27, 1989. [Online]. Available:
https://journals.sagepub.com/doi/pdf/10.1177/027836498900800602

[9] L. A. Sandino, M. Bejar, and A. Ollero, “A survey on methods for

elaborated modeling of the mechanics of a small-size helicopter. analysis
and comparison,” vol. 72, no. 2, pp. 219–238, 2013. [Online]. Available:
https://link.springer.com/article/10.1007/s10846-013-9821-y

[10] B. Siciliano, L. Sciavicco, L. Villani, and O. Giuseppe, Robotics :

Modelling, Planning and Control. Springer, 2009. [Online]. Available:
https://www.springer.com/gp/book/9781846286414

[11] K. Thanjavur and R. Rajagopalan, “Ease of dynamic modelling

of wheeled mobile robots (WMRS) using Kane’s approach,” in
Proceedings of International Conference on Robotics and Automation,
vol. 4. IEEE, 2002, pp. 2926–2931. [Online]. Available: http:
//ieeexplore.ieee.org/document/606731/

[12] M. W. Spong, “Underactuated mechanical systems,” in Control Problems

in Robotics and Automation, vol. 230, 1998, pp. 135–150. [Online].
Available: http://citeseerx.ist.psu.edu/viewdoc/download;jsessionid=
90248BB5065A6863DAD1B87872AB8166?doi=10.1.1.51.3969{&}rep=
rep1{&}type=pdf

[13] ——, “Energy based control of a class of underactuated mechanical

systems,” IFAC Proceedings Volumes, vol. 29, no. 1, pp. 2828–2832, Jun.
1996. [Online]. Available: http://citeseerx.ist.psu.edu/viewdoc/download?
doi=10.1.1.467.5126&rep=rep1&type=pdf

[14] A. Isidori, Nonlinear control systems : an introduction. Springer-

Verlag, 1989. [Online]. Available: https://www.springer.com/gp/book/
9783540199168

220
 Bibliography

[15] I. Fantoni and R. Lozano, Non-linear Control for Underactuated

Mechanical Systems. Springer-Verlag London, 2002. [Online]. Available:
https://www.springer.com/gp/book/9781447110866

[16] R. Olfati-Saber, “Nonlinear control of underactuated mechanical

systems with application to robotics and aerospace vehicles,” Ph.D.
dissertation, Massachusetts Institute of Technology, 2001. [Online].
Available: https://dspace.mit.edu/handle/1721.1/8979

[17] S. Devasia and B. Paden, “Exact output tracking for nonlinear time-
varying systems,” in Proceedings of 1994 33rd IEEE Conference on
Decision and Control, vol. 3. IEEE, 1994, pp. 2346–2355. [Online].
Available: http://ieeexplore.ieee.org/document/411465/

[18] L. Benvenuti, M. D. di Benedetto, and J. W. Grizzle, “Approximate

output tracking for nonlinear non‐minimum phase systems with an
application to flight control,” International Journal of Robust and
Nonlinear Control, vol. 4, no. 3, pp. 397–414, Jan. 1994. [Online].
Available: http://doi.wiley.com/10.1002/rnc.4590040307

[19] D. Chen and B. Paden, “Stable inversion of nonlinear non-minimum phase

systems,” International Journal of Control, vol. 64, no. 1, pp. 81–97, May
1996. [Online]. Available: https://www.tandfonline.com/doi/full/10.1080/
00207179608921618

[20] M. Fliess, J. Levine, P. Martin, and P. Rouchon, “Flatness and

defect of non-linear systems: Introductory theory and examples,”
International Journal of Control, vol. 61, no. 6, pp. 1327–1361, Jun.
1995. [Online]. Available: https://www.tandfonline.com/doi/full/10.1080/
00207179508921959

[21] M. van Nieuwstadt and R. M. Murray, “Real time trajectory

generation for differentially flat systems,” IFAC Proceedings Volumes,
vol. 29, no. 1, pp. 2301–2306, Jun. 1996. [Online]. Available: https:
//onlinelibrary.wiley.com/doi/abs/10.1002/%28SICI%291099-1239%
28199809%298%3A11%3C995%3A%3AAID-RNC373%3E3.0.CO%3B2-W

221
Bibliography

[22] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for ag-
gressive quadrotor flight in dense indoor environments,” in Springer Tracts
in Advanced Robotics, vol. 114, 2016, pp. 649–666. [Online]. Available:
https://link.springer.com/chapter/10.1007{%}2F978-3-319-28872-7{_}37

[23] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and

control for quadrotors,” in Proceedings - IEEE International Conference
on Robotics and Automation. IEEE, May 2011, pp. 2520–2525. [Online].
Available: http://ieeexplore.ieee.org/document/5980409/

[24] S. Ramasamy, G. Wu, and K. Sreenath, “Dynamically feasible motion

planning through partial differential flatness,” Robotics: Science and
Systems, 2014. [Online]. Available: http://www.roboticsproceedings.org/
rss10/p53.pdf

[25] C. Sferrazza, D. Pardo, and J. Buchli, “Numerical search for local (partial)
differential flatness,” in IEEE International Conference on Intelligent
Robots and Systems, vol. 2016-Novem, 2016, pp. 3640–3646. [Online].
Available: https://ieeexplore.ieee.org/document/7759536

[26] J. Kuffner and S. LaValle, “RRT-connect: An efficient approach to

single-query path planning,” in 2000 IEEE Int’l Conf. on Robotics
and Automation (ICRA 2000), 2002, pp. 995–1001. [Online]. Available:
https://ieeexplore.ieee.org/abstract/document/844730

[27] P. Hart, N. Nilsson, and B. Raphael, “A formal basis for the heuristic
determination of minimum cost paths,” IEEE Transactions on Systems
Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968. [Online].
Available: http://ieeexplore.ieee.org/document/4082128/

[28] L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars, “Probabilistic

roadmaps for path planning in high-dimensional configuration spaces,”
IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp.
566–580, Aug 1996. [Online]. Available: https://ieeexplore.ieee.org/
document/508439

[29] C. Warren, “Global path planning using artificial potential fields,” in 1989

222
 Bibliography

International Conference on Robotics and Automation, 1989. [Online].

Available: https://ieeexplore.ieee.org/abstract/document/100007/

[30] D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Asymptotically

optimal motion planning for robots with linear dynamics,” in 2013 IEEE
International Conference on Robotics and Automation. IEEE, May 2013,
pp. 5054–5061. [Online]. Available: http://ieeexplore.ieee.org/document/
6631299/

[31] M. Kelly, “An introduction to trajectory optimization: How to

do your own direct collocation,” Society for Industrial and Applied
Mathematics, vol. 59, no. 4, pp. 849–904, 2017. [Online]. Available:
http://www.siam.org/journals/ojsa.php

[32] B. Triggs, “Motion planning for nonholonomic vehicles: An introduction,”

Oxford University Robotics Group, Tech. Rep., 1993. [Online]. Available:
https://hal.inria.fr/inria-00548415/document

[33] R. Siegwart and I. R. Nourbakhsh, Introduction to Autonomous Mobile

Robots. MIT Press, 2004, vol. 23. [Online]. Available: https://mitpress.
mit.edu/books/introduction-autonomous-mobile-robots-second-edition

[34] Z. Hendzel and Rykała, “Modelling of dynamics of a wheeled

mobile robot with Mecanum wheels with the use of Lagrange
equations of the second kind,” International Journal of Applied
Mechanics and Engineering, vol. 22, no. 1, pp. 81–99, 2017. [Online].
Available: https://www.degruyter.com/dg/viewarticle/j$002fijame.2017.
22.issue-1$002fijame-2017-0005$002fijame-2017-0005.xml

[35] N. Tlale and M. D. Villiers, “Kinematics and dynamics modelling of a

Mecanum wheeled mobile platform,” in 15th International Conference
on Mechatronics and Machine Vision in Practice, M2VIP’08. IEEE,
Dec. 2008, pp. 657–662. [Online]. Available: http://ieeexplore.ieee.org/
document/4749608/

[36] M. D. Correia, A. Gustavo, and S. Conceição, “Modeling of a

three wheeled omnidirectional robot including friction models,” IFAC

223
Bibliography

Proceedings Volumes, vol. 45, no. 22, pp. 7–12, 2012. [Online]. Available:
https://doi.org/10.3182/20120905-3-HR-2030.00002

[37] K. L. Han, H. Kim, and J. S. Lee, “The sources of position

errors of omni-directional mobile robot with Mecanum wheel,” in
Conference Proceedings - IEEE International Conference on Systems, Man
and Cybernetics. IEEE, Oct. 2010, pp. 581–586. [Online]. Available:
http://ieeexplore.ieee.org/document/5642009/

[38] O. Purwin and R. D’Andrea, “Trajectory generation and control

for four wheeled omnidirectional vehicles,” Robotics and Autonomous
Systems, vol. 54, no. 1, pp. 13–22, 2006. [Online]. Available:
https://ieeexplore.ieee.org/document/1470795

[39] B. Lau, C. Sprunk, and W. Burgard, “Kinodynamic motion planning

for mobile robots using splines,” in 2009 IEEE/RSJ International
Conference on Intelligent Robots and Systems, IROS 2009, 2009,
pp. 2427–2433. [Online]. Available: https://ieeexplore.ieee.org/abstract/
document/5354805

[40] C. Sprunk, “Highly accurate mobile robot navigation,” Ph.D. dissertation,

Albert-Ludwigs-University of Freiburg, 2015. [Online]. Available: http:
//ais.informatik.uni-freiburg.de/publications/papers/sprunk15phd.pdf

[41] M. Dakulovic, C. Sprunk, L. Spinello, I. Petrovic, and W. Burgard,

“Efficient navigation for anyshape holonomic mobile robots in dynamic
environments,” in IEEE International Conference on Intelligent Robots
and Systems, 2013, pp. 2644–2649. [Online]. Available: https://ieeexplore.
ieee.org/abstract/document/6696729

[42] SoftBank Robotics. Pepper the humanoid robot. Online. Accessed:

2019-08-19. [Online]. Available: https://www.softbankrobotics.com/emea/
en/pepper

[43] R. P. M. Chan, K. A. Stol, and C. R. Halkyard, “Review of

modelling and control of two-wheeled robots,” Annual Reviews in
Control, vol. 37, no. 1, pp. 89–103, Apr. 2013. [Online]. Available:
https://doi.org/10.1016/j.arcontrol.2013.03.004

224
 Bibliography

[44] O. Boubaker, “The inverted pendulum benchmark in nonlinear

control theory: A survey,” International Journal of Advanced Robotic
Systems, vol. 10, no. 5, p. 233, May 2013. [Online]. Available:
http://journals.sagepub.com/doi/10.5772/55058

[45] Z. Li, C. Yang, and L. Fan, Advanced control of wheeled inverted pendulum
systems. Springer, 2013. [Online]. Available: https://link.springer.com/
book/10.1007/978-1-4471-2963-9

[46] F. Grasser, A. D’Arrigo, S. Colombi, and A. C. Rufer, “JOE:

A mobile, inverted pendulum,” IEEE Transactions on Industrial
Electronics, vol. 49, no. 1, pp. 107–114, 2002. [Online]. Available:
https://ieeexplore.ieee.org/abstract/document/982254

[47] L. Jingtao, G. Xueshan, H. Qiang, D. Qinjun, and D. Xingguang,

“Mechanical design and dynamic modeling of a two-wheeled inverted
pendulum mobile robot,” in Proceedings of the IEEE International
Conference on Automation and Logistics, ICAL 2007. IEEE, aug 2007,
pp. 1614–1619. [Online]. Available: http://ieeexplore.ieee.org/document/
4338830/

[48] S. Y. Seo, S. H. Kim, S. H. Lee, S. H. Han, and H. S. Kim,

“Simulation of attitude control of a wheeled inverted pendulum,”
in ICCAS 2007 - International Conference on Control, Automation
and Systems. IEEE, 2007, pp. 2264–2269. [Online]. Available: http:
//ieeexplore.ieee.org/document/4406742/

[49] K. M. Goher, M. O. Tokhi, and N. H. Siddique, “Dynamic modeling and

control of a two wheeled robotic vehicle with a virtual payload,” ARPN
Journal of Engineering and Applied Sciences, vol. 6, no. 3, pp. 7–41, 2011.

[50] S. Kim and S. J. Kwon, “Dynamic modeling of a two-wheeled inverted

pendulum balancing mobile robot,” International Journal of Control,
Automation and Systems, vol. 13, no. 4, pp. 926–933, aug 2015. [Online].
Available: http://link.springer.com/10.1007/s12555-014-0564-8

[51] K. Pathak, J. Franch, and S. K. S. Agrawal, “Velocity and position control

of a wheeled inverted pendulum by partial feedback linearization,” IEEE

225
Bibliography

Transactions on Robotics, vol. 21, no. 3, pp. 505–513, Jun. 2005. [Online].
Available: http://ieeexplore.ieee.org/document/1435497/

[52] M. Muhammad, S. Buyamin, M. N. Ahmad, and S. W. Nawawi,

“Dynamic modeling and analysis of a two-wheeled inverted pendulum
robot,” in Proceedings - CIMSim 2011: 3rd International Conference on
Computational Intelligence, Modelling and Simulation. IEEE, Sep. 2011,
pp. 159–164. [Online]. Available: http://ieeexplore.ieee.org/document/
6076349/

[53] Y. Kim, S. H. Kim, and Y. K. Kwak, “Dynamic analysis of a nonholonomic

two-wheeled inverted pendulum robot,” Journal of Intelligent and Robotic
Systems: Theory and Applications, vol. 44, no. 1, pp. 25–46, Sep. 2005.
[Online]. Available: http://link.springer.com/10.1007/s10846-005-9022-4

[54] J. Jahaya, S. W. Nawawi, and Z. Ibrahim, “Multi input single output

closed loop identification of two wheel inverted pendulum mobile robot,”
in Proceedings - 2011 IEEE Student Conference on Research and
Development, SCOReD 2011. IEEE, Dec. 2011, pp. 138–143. [Online].
Available: http://ieeexplore.ieee.org/document/6148723/

[55] A. Shimada and N. Hatakeyama, “High-speed motion control of wheeled

inverted pendulum robots,” in Mechatronics, 2007 IEEE International
Conference on. IEEE, May 2007, pp. 1–6. [Online]. Available:
http://ieeexplore.ieee.org/document/4280028/

[56] R. M. Brisilla and V. Sankaranarayanan, “Nonlinear control of mobile

inverted pendulum,” Robotics and Autonomous Systems, vol. 70, pp.
145–155, 2015. [Online]. Available: http://dx.doi.org/10.1016/j.robot.
2015.02.012

[57] Y.-S. Ha and S. Yuta, “Trajectory tracking control for navigation of

the inverse pendulum type self-contained mobile robot,” Robotics and
Autonomous Systems, vol. 17, no. 1-2, pp. 65–80, Apr. 1996. [Online].
Available: https://doi.org/10.1016/0921-8890(95)00062-3

[58] J. Huang, Z. H. Guan, T. Matsuno, T. Fukuda, and K. Sekiyama,

“Sliding-mode velocity control of mobile-wheeled inverted-pendulum

226
 Bibliography

systems,” IEEE Transactions on Robotics, vol. 26, no. 4, pp. 750–758, aug
2010. [Online]. Available: http://ieeexplore.ieee.org/document/5512655/

[59] M. Yue, C. An, and J.-Z. Sun, “An efficient model predictive control for
trajectory tracking of wheeled inverted pendulum vehicles with various
physical constraints,” International Journal of Control, Automation and
Systems, vol. 18, no. 1, pp. 265–274, 2018. [Online]. Available: http:
//dx.doi.org/10.1007/s12555-016-0393-zhttp://www.springer.com/12555

[60] N. Dini and V. J. Majd, “Model predictive control of a wheeled inverted

pendulum robot,” in 2015 3rd RSI International Conference on Robotics
and Mechatronics (ICROM). IEEE, Oct. 2015, pp. 152–157. [Online].
Available: http://ieeexplore.ieee.org/document/7367776/

[61] N. Hirose, R. Tajima, N. Koyama, K. Sukigara, and M. Tanaka,

“Following control approach based on model predictive control for wheeled
inverted pendulum robot,” ADVANCED ROBOTICS, vol. 30, no. 6, pp.
374–385, 2016. [Online]. Available: http://dx.doi.org/10.1080/01691864.
2016.1141115

[62] Z. Li and C. Xu, “Adaptive fuzzy logic control of dynamic balance

and motion for wheeled inverted pendulums,” Fuzzy Sets and Systems,
vol. 160, no. 12, pp. 1787–1803, Jun. 2009. [Online]. Available:
https://doi.org/10.1016/j.fss.2008.09.013

[63] C.-H. Huang, W.-J. Wang, and C.-H. Chiu, “Design and implementation
of fuzzy control on a two-wheel inverted pendulum,” IEEE Transactions
on Industrial Electronics, vol. 58, no. 7, pp. 2988–3001, Jul. 2011. [Online].
Available: http://ieeexplore.ieee.org/document/5556004/

[64] S. Jung and S. S. Kim, “Control experiment of a wheel-driven mobile

inverted pendulum using neural network,” IEEE Transactions on Control
Systems Technology, vol. 16, no. 2, pp. 297–303, Mar. 2008. [Online].
Available: http://ieeexplore.ieee.org/document/4431881/

[65] C. Yang, Z. Li, R. Cui, and B. Xu, “Neural network-based

motion control of an underactuated wheeled inverted pendulum
model,” IEEE Transactions on Neural Networks and Learning Systems,

227
Bibliography

vol. 25, no. 11, pp. 2004–2016, Nov. 2014. [Online]. Available:
http://ieeexplore.ieee.org/document/6762995/

[66] R. P. M. Chan, “Traction control of two-wheeled robots with

model predictive control,” Ph.D. dissertation, 2015. [Online]. Available:
http://hdl.handle.net/2292/27667

[67] Boston Dynamics. Handle. Online. Accessed: 2019-08-19. [Online].

Available: https://www.bostondynamics.com/handle

[68] Ralph L. Hollis, “Dynamic balancing mobile robot,” Patent US7 847 504B2,
Oct., 2007. [Online]. Available: https://patents.google.com/patent/
US7847504B2/en

[69] P. Fankhauser and C. Gwerder, “Modeling and control of a ballbot,”

B.S. thesis, Eidgenössische Technische Hochschule Zürich, 2010. [Online].
Available: https://doi.org/10.3929/ethz-a-010056685

[70] U. Nagarajan, G. Kantor, and R. Hollis, “The ballbot: An

omnidirectional balancing mobile robot,” The International Journal of
Robotics Research, vol. 33, no. 6, pp. 917–930, May 2014. [Online].
Available: http://journals.sagepub.com/doi/10.1177/0278364913509126

[71] M. Kumagai and T. Ochiai, “Development of a robot balancing on

a ball,” in 2008 International Conference on Control, Automation and
Systems, ICCAS 2008. IEEE, Oct. 2008, pp. 433–438. [Online]. Available:
http://ieeexplore.ieee.org/document/4694680/

[72] G. Seyfarth, A. Bhatia, O. Sassnick, M. Shomin, M. Kumagai, and

R. Hollis, “Initial results for a ballbot driven with a spherical induction
motor,” in Proceedings - IEEE International Conference on Robotics and
Automation, vol. 2016-June. IEEE, May 2016, pp. 3771–3776. [Online].
Available: http://ieeexplore.ieee.org/document/7487565/

[73] M. Shomin and R. Hollis, “Fast, dynamic trajectory planning for a

dynamically stable mobile robot,” in IEEE International Conference on
Intelligent Robots and Systems, 2014, pp. 3636–3641. [Online]. Available:
https://ieeexplore.ieee.org/document/6943072

228
 Bibliography

[74] U. Nagarajan, G. Kantor, and R. L. Hollis, “Trajectory planning

and control of an underactuated dynamically stable single spherical
wheeled mobile robot,” in Proceedings - IEEE International Conference
on Robotics and Automation, 2009, pp. 3743–3748. [Online]. Available:
https://ieeexplore.ieee.org/abstract/document/5152624

[75] A. N. Inal, O. Morgul, and U. Saranli, “A 3D dynamic model of a spherical

wheeled self-balancing robot,” in IEEE International Conference on
Intelligent Robots and Systems, 2012, pp. 5381–5386. [Online]. Available:
https://ieeexplore.ieee.org/abstract/document/6385689

[76] A. Bonci, “New dynamic model for a ballbot system,” in MESA 2016 -
12th IEEE/ASME International Conference on Mechatronic and Embedded
Systems and Applications - Conference Proceedings. IEEE, aug 2016, pp.
1–6. [Online]. Available: http://ieeexplore.ieee.org/document/7587176/

[77] U. Nagarajan, “Fast and graceful balancing mobile robots,” Ph.D.

dissertation, Carnegie Mellon University, 2012. [Online]. Available: https://
www.ri.cmu.edu/publications/fast-and-graceful-balancing-mobile-robots/

[78] L. Hertig, D. Schindler, M. Bloesch, C. D. Remy, and R. Siegwart, “Unified

state estimation for a ballbot,” in Proceedings - IEEE International
Conference on Robotics and Automation. IEEE, May 2013, pp. 2471–2476.
[Online]. Available: http://ieeexplore.ieee.org/document/6630913/

[79] L. A. University of California. RoMeLa. Online. Accessed: 2019-08-19.

[Online]. Available: http://www.romela.org/robots/

[80] Honda. Honda Global - UNI-CUB. Online. Accessed: 2019-08-19. [Online].

Available: https://global.honda/innovation/robotics/UNI-CUB.html

[81] M. Shomin and R. Hollis, “Differentially flat trajectory generation for a

dynamically stable mobile robot,” in Proceedings - IEEE International
Conference on Robotics and Automation. IEEE, May 2013, pp. 4467–4472.
[Online]. Available: http://ieeexplore.ieee.org/document/6631211/

[82] U. Nagarajan, “Dynamic constraint-based optimal shape trajectory planner

for shape-accelerated underactuated balancing systems,” Robotics: Science

229
Bibliography

and Systems, 2010. [Online]. Available: http://www.roboticsproceedings.

org/rss06/p31.pdf

[83] D. Pardo, L. Moller, M. Neunert, A. W. Winkler, and J. Buchli,

“Evaluating direct transcription and nonlinear optimization methods
for robot motion planning,” IEEE Robotics and Automation Letters,
vol. 1, no. 2, pp. 946–953, Jul. 2016. [Online]. Available: http:
//ieeexplore.ieee.org/document/7400956/

[84] M. Neunert, C. De Crousaz, F. Furrer, M. Kamel, F. Farshidian,

R. Siegwart, and J. Buchli, “Fast nonlinear model predictive control
for unified trajectory optimization and tracking,” in Proceedings -
IEEE International Conference on Robotics and Automation, vol.
2016-June. IEEE, May 2016, pp. 1398–1404. [Online]. Available:
http://ieeexplore.ieee.org/document/7487274/

[85] S. Reynolds-Haertle and M. Stilman, “Design and development of a

dynamically-balancing holonomic robot,” Georgia Institute of Technology.
Center for Robotics and Intelligent Machines, Tech. Rep., 2011. [Online].
Available: https://smartech.gatech.edu/handle/1853/41706

[86] K. Zimmermann, I. Zeidis, and M. Abdelrahman, “Dynamics of

mechanical systems with Mecanum wheels,” in Springer Proceedings in
Mathematics and Statistics, vol. 93, 2014, pp. 269–279. [Online]. Available:
https://doi.org/10.1007/978-3-319-08266-0_19

[87] J. Dormand and P. Prince, “A family of embedded runge-kutta formulae,”

Journal of Computational and Applied Mathematics, vol. 6, no. 1, pp.
19 – 26, 1980. [Online]. Available: http://www.sciencedirect.com/science/
article/pii/0771050X80900133

[88] K. Lynch and F. Park, Modern Robotics: Mechanics, Planning,

and Control. Cambridge University Press, 2017. [Online]. Available:
https://books.google.co.uk/books?id=8uS3AQAACAAJ

[89] H. H. Nijmeijer and A. J. van der. Schaft, Nonlinear dynamical

control systems. Springer-Verlag, 1990. [Online]. Available: https:
//www.springer.com/gp/book/9780387972343

230
 Bibliography

[90] A. Salerno and J. Angeles, “A new family of two-wheeled mobile

robots: Modeling and controllability,” IEEE Transactions on Robotics,
vol. 23, no. 1, pp. 169–173, Feb. 2007. [Online]. Available: http:
//ieeexplore.ieee.org/document/4084575/

[91] A. Salerno, “Design, dynamics and control of a fast two-wheeled

quasiholonomic robot,” Ph.D. dissertation, McGill University, 2006.
[Online]. Available: https://core.ac.uk/download/pdf/41887250.pdf

[92] R. Marino, “On the largest feedback linearizable subsystem,” Systems and
Control Letters, vol. 6, no. 5, pp. 345–351, Jan. 1986. [Online]. Available:
https://doi.org/10.1016/0167-6911(86)90130-1

[93] A. J. Fossard and D. Normand-Cyrot, Nonlinear Systems : Control

3. Springer US, 1997. [Online]. Available: https://doi.org/10.1007/
978-1-4615-6395-2

[94] ArduCopter. (2019) ArduCopter complete parameter documentation.

[Online]. Available: http://ardupilot.org/copter/docs/parameters.html

[95] MPU9250 Product Specification Revision 1.1, Online, InvenSense, 2016.

[Online]. Available: http://www.invensense.com/wp-content/uploads/
2015/02/PS-MPU-9250A-01-v1.1.pdf

[96] NI myRIO-1900 User Guide and Specifications, Online, National Instru-

ments. [Online]. Available: http://www.ni.com/pdf/manuals/376047d.pdf

[97] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear

estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422,
2004. [Online]. Available: https://ieeexplore.ieee.org/abstract/document/
1271397

[98] A. Isidori, Nonlinear Control Systems, ser. Communications and Control

Engineering. London: Springer London, 1995. [Online]. Available:
http://link.springer.com/10.1007/978-1-84628-615-5

[99] B. Srinivasan, P. Huguenin, and D. Bonvin, “Global stabilization of

an inverted pendulum–control strategy and experimental verification,”

231
Bibliography

Automatica, vol. 45, no. 1, pp. 265–269, Jan. 2009. [Online]. Available:
https://doi.org/10.1016/j.automatica.2008.07.004

[100] Wei Zhong and H. Rock, “Energy and passivity based control of
the double inverted pendulum on a cart,” in Proceedings of the
2001 IEEE International Conference on Control Applications (CCA’01)
(Cat. No.01CH37204). IEEE, 2002, pp. 896–901. [Online]. Available:
http://ieeexplore.ieee.org/document/973983/

[101] M. W. Spong, P. Corke, and R. Lozano, “Nonlinear control of the reaction

wheel pendulum,” Automatica, vol. 37, no. 11, pp. 1845–1851, Nov. 2001.
[Online]. Available: https://doi.org/10.1016/S0005-1098(01)00145-5

[102] M. W. Spong, “The swing up control problem for the acrobot,” IEEE
Control Systems, vol. 15, no. 1, pp. 49–55, Feb. 1995. [Online]. Available:
https://ieeexplore.ieee.org/document/341864/

[103] J. Hauser and R. M. Murray, “Nonlinear controllers for non-

integrable systems: the acrobot example,” in 1990 American Control
Conference. IEEE, May 1990, pp. 669–671. [Online]. Available:
https://ieeexplore.ieee.org/document/4790817/

[104] M. A. Karkoub and M. Parent, “Modelling and non-linear feedback

stabilization of a two-wheel vehicle,” Proceedings of the Institution
of Mechanical Engineers, Part I: Journal of Systems and Control
Engineering, vol. 218, no. 8, pp. 675–686, Dec. 2004. [Online]. Available:
http://journals.sagepub.com/doi/10.1177/095965180421800807

[105] A. Mokhtari and A. Benallegue, “Dynamic feedback controller of Euler

angles and wind parameters estimation for a quadrotor unmanned aerial
vehicle,” in IEEE International Conference on Robotics and Automation,
2004. Proceedings. ICRA ’04. 2004. IEEE, 2004, pp. 2359–2366 Vol.3.
[Online]. Available: http://ieeexplore.ieee.org/document/1307414/

[106] A. Benallegue, A. Mokhtari, and L. Fridman, “Feedback linearization

and high order sliding mode observer for a quadrotor UAV,” in
Proceedings of the 2006 International Workshop on Variable Structure

232
 Bibliography

Systems, VSS’06, vol. 2006. IEEE, 2006, pp. 365–372. [Online]. Available:
http://ieeexplore.ieee.org/document/1644545/

[107] H. K. Khalil, Nonlinear Systems. Prentice Hall, 1996, vol. 3.

[108] D. J. Leith and W. E. Leithead, “Survey of gain-scheduling analysis and

design,” International Journal of Control, vol. 73, no. 11, pp. 1001–1025,
Jan. 2000. [Online]. Available: https://www.tandfonline.com/doi/full/10.
1080/002071700411304

[109] R. Xu and U. Ozguner, “Sliding mode control of a quadrotor

helicopter,” in Proceedings of the 45th IEEE Conference on Decision
and Control. IEEE, 2006, pp. 4957–4962. [Online]. Available: http:
//ieeexplore.ieee.org/document/4177181/

[110] M. A. Khanesar, M. Teshnehlab, and M. A. Shoorehdeli, “Sliding mode

control of rotary inverted pendulm,” in 2007 Mediterranean Conference
on Control and Automation, MED. IEEE, Jun. 2007, pp. 1–6. [Online].
Available: http://ieeexplore.ieee.org/document/4433653/

[111] K. P. Tee, S. S. Ge, and E. H. Tay, “Barrier Lyapunov functions

for the control of output-constrained nonlinear systems,” Automatica,
vol. 45, no. 4, pp. 918–927, 2009. [Online]. Available: https:
//doi.org/10.1016/j.automatica.2008.11.017

[112] A. Mills, A. Wills, and B. Ninness, “Nonlinear model predictive

control of an inverted pendulum,” in Proceedings of the American
Control Conference. IEEE, 2009, pp. 2335–2340. [Online]. Available:
http://ieeexplore.ieee.org/document/5160391/

[113] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert,

“Constrained model predictive control: Stability and optimality,”
Automatica, vol. 36, no. 6, pp. 789–814, Jun. 2000. [Online]. Available:
https://doi.org/10.1016/S0005-1098(99)00214-9

[114] J. A. Rossiter, Model-Based Predictive Control: A Practical Approach.

Sheffield, UK: CRC Press, 2003. [Online]. Available: https://doi.org/10.
1201/9781315272610

233
Bibliography

[115] J. Maciejowski, Predictive Control with Constraints. Prentice Hall, 2002.

[Online]. Available: http://www-control.eng.cam.ac.uk/jmm/mpcbook/
mpcbook.html

[116] L. Magni, R. Scattolini, and K. Åström, “Global stabilization of the

inverted pendulum using model predictive control,” IFAC Proceedings
Volumes, vol. 35, no. 1, pp. 141–146, 2002. [Online]. Available:
https://doi.org/10.3182/20020721-6-ES-1901.00592

[117] P. J. Gawthrop and L. Wang, “Intermittent predictive control of an

inverted pendulum,” Control Engineering Practice, vol. 14, no. 11,
pp. 1347–1356, 2006. [Online]. Available: https://doi.org/10.1016/j.
conengprac.2005.09.002

[118] T. Ohhira and A. Shimada, “Model predictive control for an inverted-

pendulum robot with time-varying constraints,” IFAC-PapersOnLine,
vol. 50, no. 1, pp. 776–781, Jul. 2017. [Online]. Available: https:
//doi.org/10.1016/j.ifacol.2017.08.252

[119] S. S. Dughman and J. A. Rossiter, “A survey of guaranteeing

feasibility and stability in MPC during target changes,” in IFAC-
PapersOnLine, vol. 28, no. 8, 2015, pp. 813–818. [Online]. Available:
https://doi.org/10.1016/j.ifacol.2015.09.069

[120] D. Simon, J. Lofberg, and T. Glad, “Reference tracking MPC using

terminal set scaling,” in 2012 IEEE 51st IEEE Conference on Decision
and Control (CDC). IEEE, Dec. 2012, pp. 4543–4548. [Online]. Available:
http://ieeexplore.ieee.org/document/6426550/

[121] S. S. Dughman and J. A. Rossiter, “Systematic and effective

embedding of feedforward of target information into MPC,” International
Journal of Control, pp. 1–15, Jan. 2017. [Online]. Available: https:
//www.tandfonline.com/doi/full/10.1080/00207179.2017.1281439

[122] B. Pluymers, J. Rossiter, J. Suykens, and B. De Moor, “The

efficient computation of polyhedral invariant sets for linear systems
with polytopic uncertainty,” in Proceedings of the 2005, American

234
 Bibliography

Control Conference, 2005. IEEE, 2005, pp. 804–809. [Online]. Available:

http://ieeexplore.ieee.org/document/1470058/

[123] E. G. Gilbert and K. T. Tan, “Linear systems with state and

control constraints: The theory and application of maximal output
admissible sets,” IEEE Transactions on Automatic Control, vol. 36,
no. 9, pp. 1008–1020, 1991. [Online]. Available: http://ieeexplore.ieee.org/
document/83532/

[124] Yongxing Hao, A. Davari, and A. Manesh, “Differential flatness-based

trajectory planning for multiple unmanned aerial vehicles using mixed-
integer linear programming,” in Proceedings of the 2005, American
Control Conference, 2005. IEEE, 2005, pp. 104–109. [Online]. Available:
http://ieeexplore.ieee.org/document/1469916/

[125] G. Valencia-Palomo, M. Pelegrinis, J. A. Rossiter, and R. Gondhalekar,

“A move-blocking strategy to improve tracking in predictive control,”
pp. 6293–6298, Jun. 2010. [Online]. Available: http://ieeexplore.ieee.org/
document/5531512/

[126] H. J. Ferreau, C. Kirches, A. Potschka, H. G. Bock, and M. Diehl,

“qpOASES: a parametric active-set algorithm for quadratic programming,”
Mathematical Programming Computation, vol. 6, no. 4, pp. 327–363, 2014.
[Online]. Available: https://doi.org/10.1007/s12532-014-0071-1

[127] J. T. Betts, “Survey of numerical methods for trajectory optimization,”

Journal of Guidance, Control, and Dynamics, vol. 21, no. 2, pp. 193–207,
Mar. 1998. [Online]. Available: http://arc.aiaa.org/doi/10.2514/2.4231

[128] S. Karaman and E. Frazzoli, “Optimal kinodynamic motion planning

using incremental sampling-based methods,” in Proceedings of the IEEE
Conference on Decision and Control. IEEE, Dec. 2010, pp. 7681–7687.
[Online]. Available: http://ieeexplore.ieee.org/document/5717430/

[129] D. J. Webb and J. Van Den Berg, “Kinodynamic RRT*: Asymptotically

optimal motion planning for robots with linear dynamics,” in Proceedings
- IEEE International Conference on Robotics and Automation. IEEE,

235
Bibliography

May 2013, pp. 5054–5061. [Online]. Available: http://ieeexplore.ieee.org/

document/6631299/

[130] U. Nagarajan, B. Kim, and R. Hollis, “Planning in high-dimensional

shape space for a single-wheeled balancing mobile robot with arms,” in
Proceedings - IEEE International Conference on Robotics and Automation,
2012, pp. 130–135. [Online]. Available: https://ieeexplore.ieee.org/
document/6225065

[131] P. J. Davis, Interpolation and approximation. Blaisdell Pub. Co, 1963.

[132] S. Prajna, A. Papachristodoulou, P. Seiler, and P. Parrilo, “New

developments in sum of squares optimization and SOSTOOLS,” in
Proceedings of the 2004 American Control Conference. IEEE, 2004,
pp. 5606–5611 vol.6. [Online]. Available: https://ieeexplore.ieee.org/
document/1384747/

[133] G. Polya and G. Szego, Problems and theorems in analysis.

Springer, 1998. [Online]. Available: https://link.springer.com/book/
10.1007/978-1-4757-1640-5

[134] J. Lofberg, “YALMIP : a toolbox for modeling and optimization in

MATLAB,” in 2004 IEEE International Conference on Robotics and
Automation (IEEE Cat. No.04CH37508). IEEE, 2004, pp. 284–289.
[Online]. Available: http://ieeexplore.ieee.org/document/1393890/

[135] J. Löfberg, “Pre- and post-processing sum-of-squares programs in practice,”

IEEE Transactions on Automatic Control, vol. 54, no. 5, pp. 1007–1011,
2009. [Online]. Available: https://ieeexplore.ieee.org/document/4908937

[136] M. ApS, The MOSEK optimization toolbox for MATLAB manual. Version
8.1., 2017. [Online]. Available: http://docs.mosek.com/8.1/toolbox/index.
html

[137] J. Chen, T. Liu, and S. Shen, “Online generation of collision-free

trajectories for quadrotor flight in unknown cluttered environments,” in
Proceedings - IEEE International Conference on Robotics and Automation,
vol. 2016-June. IEEE, May 2016, pp. 1476–1483. [Online]. Available:
http://ieeexplore.ieee.org/document/7487283/

236
 Bibliography

[138] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor,

and V. Kumar, “Planning dynamically feasible trajectories for quadrotors
using safe flight corridors in 3-D complex environments,” IEEE Robotics
and Automation Letters, vol. 2, no. 3, pp. 1688–1695, Jul. 2017. [Online].
Available: http://ieeexplore.ieee.org/document/7839930/

237
Appendix A

CMD Model Matrix Definitions

A.1 Matrices for CMD Dynamics Model in (q, q̇)

 
m1,1 0 m1,3 m1,4
 0 m2,2 m2,3 m2,4
 

04×8 
(A.1.1)

M (q) = m3,1 m3,2 m3,3
 0 

m4,1 m4,2 0 m4,4
 

08×4 08×8
where

m1,1 = mp + 4mw
m1,3 = hp mp cos(φ) sin(θp ) − l2 mw sin(φ) − l3 mw sin(φ) − l4 mw sin(φ)
− l1 mw sin(φ)
m1,4 = hp mp cos(θp ) sin(φ)
m2,2 = mp + 4mw
m2,3 = l1 mw cos(φ) + l2 mw cos(φ) + l3 mw cos(φ) + l4 mw cos(φ)
+ hp mp sin(φ) sin(θp )
m2,4 = −hp mp cos(φ) cos(θp )
m3,1 = hp mp cos(φ) sin(θp ) − l2 mw sin(φ) − l3 mw sin(φ) − l4 mw sin(φ)
− l1 mw sin(φ)
m3,2 = l1 mw cos(φ) + l2 mw cos(φ) + l3 mw cos(φ) + l4 mw cos(φ)
+ hp mp sin(φ) sin(θp )
m3,3 = 4Iwyz + Ipby sin(θp )2 + l12 mw + l22 mw + l32 mw + l42 mw − Ipbz (sin(θp )2 − 1)
+ h2p mp sin(θp )2

239
Appendix A CMD Model Matrix Definitions

m4,1 = hp mp cos(θp ) sin(φ)

m4,2 = −hp mp cos(φ) cos(θp )
m4,4 = mp h2p + Ipbx

 
0 0 c1,3 c1,4
0 0 c2,3 c2,4
 

04×8 
(A.1.2)

C(q, q̇) = 
0 0 c3,3 c3, 4 

0 0 c4,3 0
 

08×4 08×8
where

c1,3 = hp mp cos(φ) cos(θp )θ̇p − l2 mw cos(φ)φ̇ − l3 mw cos(φ)φ̇ − l4 mw cos(φ)φ̇

− l1 mw cos(φ)φ̇ − hp mp sin(φ) sin(θp )φ̇
c1,4 = hp mp cos(φ) cos(θp )φ̇ − hp mp sin(φ) sin(θp )θ̇p
c2,3 = hp mp cos(φ) sin(θp )φ̇ − l2 mw sin(φ)φ̇ − l3 mw sin(φ)φ̇ − l4 mw sin(φ)φ̇
− l1 mw sin(φ)φ̇ + hp mp cos(θp ) sin(φ)θ̇p
c2,4 = hp mp cos(θp ) sin(φ)φ̇ + hp mp cos(φ) sin(θp )θ̇p
c3,3 = 1/2(sin(2θp )θ̇p (mp h2p + Ipby − Ipbz ))
c3,4 = 1/2(sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))
c4,3 = −1/2(sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))

 
03×1
G(q) = −ghp mp sin(θp ) (A.1.3)
 

08×1

240
 A.1 Matrices for CMD Dynamics Model in (q, q̇)

 
0 0 0 0 0 f1,9 f1,10 f1,11 f1,12
0 0 0 0 0
f2,9 f2,10 f2,11 f2,12 
 

 

 0 0 0 0 0 0 0 0 0  
f4,4 kv w k v w k v w k v w 0 0 0 0 
 

08×3
(A.1.4)

F =
 kvw f5,5 0 0 0 f5,9 0 0 0  
kvw 0 f6,6 0 0 0 f6,10 0 0 
 

 

 kvw 0 0 f7,7 0 0 0 f7,11 0  
kvw 0 0 0 f8,8 0 0 0 f8,12 
 

04×3 04×9

where

kvr cos(φ) sin(α1 ) kvr cos(φ) sin(α2 )

f1,9 = f1,10 =
rr − rw rr − rw
kvr cos(φ) sin(α3 ) kvr cos(φ) sin(α4 )
f1,11 = f1,12 =
rr − rw rr − rw
kvr sin(φ) sin(α1 ) kvr sin(φ) sin(α2 )
f2,9 = f2,10 =
rr − rw rr − rw
kvr sin(φ) sin(α3 ) kvr sin(φ) sin(α4 )
f2,11 = f2,12 =
rr − rw rr − rw
f4,4 = −4kvw f5,5 = −krw − kvw
f5,9 = kvr cos(α1 ) f6,6 = −krw − kvw
f6,9 = kvr cos(α2 ) f7,7 = −krw − kvw
f7,9 = kvr cos(α3 ) f8,8 = −krw − kvw
f8,9 = kvr cos(α4 )

 
03×4
 
−11×4 
B=
 I

 (A.1.5)
 4×4 
04×4

241
Appendix A CMD Model Matrix Definitions

A.2 Matrices for CMD Dynamics Model in (p, ṗ)

 
m1,1 m1,2 m1,3 m1,4
 
m2,1 m2,2 m2,3 m2,4 
Mp (p) = 
m
 (A.2.1)
 3,1 m3,2 m3,3 0  
m4,1 m4,2 0 m4,4
where

m1,1 = mp + 4mw − (Iwx cos(α1 + φ)2 )/(rw

2
(cos(α1 )2 − 1))
− (Iwx cos(α2 + φ)2 )/(rw
2
(cos(α2 )2 − 1))
− (Iwx cos(α3 + φ)2 )/(rw
2
(cos(α3 )2 − 1))
− (Iwx cos(α4 + φ)2 )/(rw
2
(cos(α4 )2 − 1))
2
m1,2 = (Iwx sin(2α4 + 2φ))/(2rw sin(α4 )2 ) + (Iwx (sin(α1 )2 sin(α2 )2 sin(2α3 + 2φ)
+ sin(α1 )2 sin(α3 )2 sin(2α2 + 2φ)
+ sin(α2 )2 sin(α3 )2 sin(2α1 + 2φ)))/(2rw
2
sin(α1 )2 sin(α2 )2 sin(α3 )2 )
m1,3 = hp mp cos(φ) sin(θp ) − l2 mw sin(φ) − l3 mw sin(φ) − l4 mw sin(φ)
2
− l1 mw sin(φ) + (Iwx l1 cos(α1 + φ))/(rw sin(α1 ))
2 2
+ (Iwx l2 cos(α2 + φ))/(rw sin(α2 )) + (Iwx l3 cos(α3 + φ))/(rw sin(α3 ))
2
+ (Iwx l4 cos(α4 + φ))/(rw sin(α4 ))
m1,4 = hp mp cos(θp ) sin(φ)
2
m2,1 = (Iwx sin(2α4 + 2φ))/(2rw sin(α4 )2 )
+ (Iwx (sin(α1 )2 sin(α2 )2 sin(2α3 + 2φ) + sin(α1 )2 sin(α3 )2 sin(2α2 + 2φ)
+ sin(α2 )2 sin(α3 )2 sin(2α1 + 2φ)))/(2rw
2
sin(α1 )2 sin(α2 )2 sin(α3 )2 )
m2,2 = mp + 4mw + (Iwx sin(α1 + φ)2 )/(rw
2
sin(α1 )2 )
+ (Iwx sin(α2 + φ)2 )/(rw
2
sin(α2 )2 ) + (Iwx sin(α3 + φ)2 )/(rw
2
sin(α3 )2 )
+ (Iwx sin(α4 + φ)2 )/(rw
2
sin(α4 )2 )
m2,3 = l1 mw cos(φ) + l2 mw cos(φ) + l3 mw cos(φ) + l4 mw cos(φ)
2
+ hp mp sin(φ) sin(θp ) + (Iwx l1 sin(α1 + φ))/(rw sin(α1 ))
2 2
+ (Iwx l2 sin(α2 + φ))/(rw sin(α2 )) + (Iwx l3 sin(α3 + φ))/(rw sin(α3 ))
2
+ (Iwx l4 sin(α4 + φ))/(rw sin(α4 ))
m2,4 = − hp mp cos(φ) cos(θp )

242
 A.2 Matrices for CMD Dynamics Model in (p, ṗ)

m3,1 = hp mp cos(φ) sin(θp ) − l2 mw sin(φ) − l3 mw sin(φ) − l4 mw sin(φ)

2
− l1 mw sin(φ) + (Iwx l1 cos(α1 + φ))/(rw sin(α1 ))
2 2
+ (Iwx l2 cos(α2 + φ))/(rw sin(α2 )) + (Iwx l3 cos(α3 + φ))/(rw sin(α3 ))
2
+ (Iwx l4 cos(α4 + φ))/(rw sin(α4 ))
m3,2 = l1 mw cos(φ) + l2 mw cos(φ) + l3 mw cos(φ) + l4 mw cos(φ)
2
+ hp mp sin(φ) sin(θp ) + (Iwx l1 sin(α1 + φ))/(rw sin(α1 ))
2 2
+ (Iwx l2 sin(α2 + φ))/(rw sin(α2 )) + (Iwx l3 sin(α3 + φ))/(rw sin(α3 ))
2
+ (Iwx l4 sin(α4 + φ))/(rw sin(α4 ))
m3,3 = 4Iwyz + (Iwx l12 + Iwx l22 + Iwx l32 + Iwx l42 )/rw
2
+ Ipby sin(θp )2 + l12 mw + l22 mw
+ l32 mw + l42 mw − Ipbz (sin(θp )2 − 1) + h2p mp sin(θp )2
m4,1 = hp mp cos(θp ) sin(φ)
m4,2 = − hp mp cos(φ) cos(θp )
m4,4 = mp h2p + Ipbx

 
c1,1 c1,2 c1,3 c1,4
 
c2,1 c2,2 c2,3 c2,4 
Cp (p, ṗ) = 
c
 (A.2.2)
 3,1 c3,2 c3,3 c3,4 

0 0 c4,3 0

2
c1,1 = − (Iwx sin(2α4 + 2φ)φ̇)/(2rw sin(α4 )2 )
− (Iwx φ̇(sin(α1 )2 sin(α2 )2 sin(2α3 + 2φ) + sin(α1 )2 sin(α3 )2 sin(2α2 + 2φ)
+ sin(α2 )2 sin(α3 )2 sin(2α1 + 2φ)))/(2rw
2
sin(α1 )2 sin(α2 )2 sin(α3 )2 )
c1,2 = − (Iwx cos(α1 + φ)2 φ̇)/(rw
2
(cos(α1 )2 − 1))
− (Iwx cos(α2 + φ)2 φ̇)/(rw
2
(cos(α2 )2 − 1))
− (Iwx cos(α3 + φ)2 φ̇)/(rw
2
(cos(α3 )2 − 1))
− (Iwx cos(α4 + φ)2 φ̇)/(rw
2
(cos(α4 )2 − 1))
c1,3 = hp mp cos(φ) cos(θp )θ̇p − l2 mw cos(φ)φ̇ − l3 mw cos(φ)φ̇ − l4 mw cos(φ)φ̇
− l1 mw cos(φ)φ̇ − hp mp sin(φ) sin(θp )φ̇

243
Appendix A CMD Model Matrix Definitions

c1,4 = hp mp cos(phi) cos(θp )φ̇ − hp mp sin(φ) sin(θp )θ̇p

c2,1 = − (Iwx sin(α4 + φ)2 φ̇)/(rw
2
sin(α4 )2 ) − (Iwx φ̇(sin(α1 + φ)2 sin(α2 )2 sin(α3 )2
+ sin(α2 + φ)2 sin(α1 )2 sin(α3 )2 + sin(α3 + φ)2 sin(α1 )2 sin(α2 )2 ))
2
/(rw sin(α1 )2 sin(α2 )2 sin(α3 )2 )
2
c2,2 = (Iwx sin(2α4 + 2φ)φ̇)/(2rw sin(α4 )2 ) + (Iwx φ̇(sin(α1 )2 sin(α2 )2 sin(2α3 + 2φ)
+ sin(α1 )2 sin(α3 )2 sin(2α2 + 2φ) + sin(α2 )2 sin(α3 )2 sin(2α1 + 2φ)))
2
/(2rw sin(α1 )2 sin(α2 )2 sin(α3 )2 )
c2,3 = hp mp cos(φ) sin(θp )φ̇ − l2 mw sin(φ)φ̇ − l3 mw sin(φ)φ̇ − l4 mw sin(φ)φ̇
− l1 mw sin(φ)φ̇ + hp mp cos(θp ) sin(φ)θ̇p
c2,4 = hp mp cos(θp ) sin(φ)φ̇ + hp mp cos(φ) sin(θp )θ̇p
c3,1 = − (Iwx φ̇(l1 cos(φ)
+ l2 cos(φ) + l3 cos(φ) + l4 cos(φ) + l1 sin(φ) cot(α1 ) + l2 sin(φ) cot(α2 )
2 2
+ l3 sin(φ) cot(α3 )))/rw − (Iwx l4 sin(φ) cot(α4 )φ̇)/rw
2
c3,2 = (Iwx l4 cos(φ) cot(α4 )φ̇)/rw − (Iwx φ̇(l1 sin(φ) + l2 sin(φ) + l3 sin(φ) + l4 sin(φ)
2
− l1 cos(φ) cot(α1 ) − l2 cos(φ) cot(α2 ) − l3 cos(φ) cot(α3 )))/rw
c3,3 = (sin(2θp )θ̇p (mp h2p + Ipby − Ipbz ))/2
c3,4 = (sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))/2
c4,3 = − (sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))/2

" #
03×1
Gp (p) = (A.2.3)
−ghp mp sin(θp )

 
f1,1 f1,2 f1,3 f1,4
 
f2,1 f2,2 f2,3 f2,4 
Fp (p) = 
f
 (A.2.4)
 3,1 f3,2 f3,3 f3,4 

f4,1 f4,2 f4,3 f4,4

244
 A.2 Matrices for CMD Dynamics Model in (p, ṗ)

where

f1,1 = (kvr cos(φ) sin(φ) cot(α1 ))/(rr rw )

2
− ((krw + kvw )(cos(2α1 + 2φ) + 1))/(2rw sin(α1 )2 )
2
− ((krw + kvw )(cos(2α2 + 2φ) + 1))/(2rw sin(α2 )2 )
2
− ((krw + kvw )(cos(2α3 + 2φ) + 1))/(2rw sin(α3 )2 )
2
− ((krw + kvw )(cos(2α4 + 2φ) + 1))/(2rw sin(α4 )2 )
− (kvr cos(φ)2 cot(α1 )2 )/(rr rw ) − (kvr cos(φ)2 cot(α2 )2 )/(rr rw )
− (kvr cos(φ)2 cot(α3 )2 )/(rr rw ) − (kvr cos(φ)2 cot(α4 )2 )/(rr rw )
− (4kvr cos(φ)2 )/(−rr2 + rw rr ) + (kvr cos(φ) sin(φ) cot(α2 ))/(rr rw )
+ (kvr cos(φ) sin(φ) cot(α3 ))/(rr rw ) + (kvr cos(φ) sin(φ) cot(α4 ))/(rr rw )
f1,2 = (kvr cot(α1 ))/(2rr rw ) − (2kvr sin(2φ))/(−rr2 + rw rr ) + (kvr cot(α2 ))/(2rr rw )
+ (kvr cot(α3 ))/(2rr rw ) + (kvr cot(α4 ))/(2rr rw )
2
− (sin(2α1 + 2φ)(krw + kvw ))/(2rw sin(α1 )2 )
2
− (sin(2α2 + 2φ)(krw + kvw ))/(2rw sin(α2 )2 )
2
− (sin(2α3 + 2φ)(krw + kvw ))/(2rw sin(α3 )2 )
2
− (sin(2α4 + 2φ)(krw + kvw ))/(2rw sin(α4 )2 )
− (kvr sin(2φ) cot(α1 )2 )/(2rr rw ) − (kvr sin(2φ) cot(α2 )2 )/(2rr rw )
− (kvr sin(2φ) cot(α3 )2 )/(2rr rw ) − (kvr sin(2φ) cot(α4 )2 )/(2rr rw )
− (kvr cos(2φ) cot(α1 ))/(2rr rw ) − (kvr cos(2φ) cot(α2 ))/(2rr rw )
− (kvr cos(2φ) cot(α3 ))/(2rr rw ) − (kvr cos(2φ) cot(α4 ))/(2rr rw )
2
f1,3 = − (l4 cos(α4 + φ)(krw + kvw ))/(rw sin(α4 ))
− ((krw + kvw )(l1 cos(α1 + φ) sin(α2 ) sin(α3 )
+ l2 cos(α2 + φ) sin(α1 ) sin(α3 )
2
+ l3 cos(α3 + φ) sin(α1 ) sin(α2 )))/(rw sin(α1 ) sin(α2 ) sin(α3 ))
f1,4 = (4kvw sin(φ))/rw − (kvw cos(φ) cot(α1 ))/rw − (kvw cos(φ) cot(α2 ))/rw
− (kvw cos(φ) cot(α3 ))/rw − (kvw cos(φ) cot(α4 ))/rw

245
Appendix A CMD Model Matrix Definitions

2
f2,1 = (sin(2α1 + 2φ)(krw + kvw ))/(rw (cos(2α1 ) − 1))
2
+ (sin(2α2 + 2φ)(krw + kvw ))/(rw (cos(2α2 ) − 1))
2
+ (sin(2α3 + 2φ)(krw + kvw ))/(rw (cos(2α3 ) − 1))
2
+ (sin(2α4 + 2φ)(krw + kvw ))/(rw (cos(2α4 ) − 1))
+ (cos(φ)((kvr sin(φ) sin(α1 ))/(rr − rw )
− (kvr sin(α1 + φ) cos(α1 ))/(rw sin(α1 ))))/(rr sin(α1 ))
+ (cos(φ)((kvr sin(φ) sin(α2 ))/(rr − rw )
− (kvr sin(α2 + φ) cos(α2 ))/(rw sin(α2 ))))/(rr sin(α2 ))
+ (cos(φ)((kvr sin(φ) sin(α3 ))/(rr − rw )
− (kvr sin(α3 + φ) cos(α3 ))/(rw sin(α3 ))))/(rr sin(α3 ))
+ (cos(φ)((kvr sin(φ) sin(α4 ))/(rr − rw )
− (kvr sin(α4 + φ) cos(α4 ))/(rw sin(α4 ))))/(rr sin(α4 ))
f2,2 = (sin(φ)((kvr sin(φ) sin(α1 ))/(rr − rw )
+ (kvr sin(α1 + φ)(2 sin(α1 /2)2 − 1))/(rw sin(α1 ))))/(rr sin(α1 ))
+ (sin(φ)((kvr sin(φ) sin(α2 ))/(rr − rw )
+ (kvr sin(α2 + φ)(2 sin(α2 /2)2 − 1))/(rw sin(α2 ))))/(rr sin(α2 ))
+ (sin(φ)((kvr sin(φ) sin(α3 ))/(rr − rw )
+ (kvr sin(α3 + φ)(2 sin(α3 /2)2 − 1))/(rw sin(α3 ))))/(rr sin(α3 ))
+ (sin(φ)((kvr sin(φ) sin(α4 ))/(rr − rw )
+ (kvr sin(α4 + φ)(2 sin(α4 /2)2 − 1))/(rw sin(α4 ))))/(rr sin(α4 ))
− (sin(α1 + φ)2 (krw + kvw ))/(rw
2
sin(α1 )2 )
− (sin(α2 + φ)2 (krw + kvw ))/(rw
2
sin(α2 )2 )
− (sin(α3 + φ)2 (krw + kvw ))/(rw
2
sin(α3 )2 )
− (sin(α4 + φ)2 (krw + kvw ))/(rw
2
sin(α4 )2 )
2
f2,3 = − (l4 sin(α4 + φ)(krw + kvw ))/(rw sin(α4 ))
− ((krw + kvw )(l1 sin(α1 + φ) sin(α2 ) sin(α3 )
+ l2 sin(α2 + φ) sin(α1 ) sin(α3 )
2
+ l3 sin(α3 + φ) sin(α1 ) sin(α2 )))/(rw sin(α1 ) sin(α2 ) sin(α3 ))

246
 A.2 Matrices for CMD Dynamics Model in (p, ṗ)

f2,4 = − (4kvw cos(φ))/rw − (kvw sin(φ) cot(α1 ))/rw − (kvw sin(φ) cot(α2 ))/rw
− (kvw sin(φ) cot(α3 ))/rw − (kvw sin(φ) cot(α4 ))/rw
2
f3,1 = − (l1 cos(α1 + φ)(krw + kvw ))/(rw sin(α1 ))
2
− (l2 cos(α2 + φ)(krw + kvw ))/(rw sin(α2 ))
2
− (l3 cos(α3 + φ)(krw + kvw ))/(rw sin(α3 ))
2
− (l4 cos(α4 + φ)(krw + kvw ))/(rw sin(α4 ))
− (kvr l1 cos(φ) cot(α1 ))/(rr rw ) − (kvr l2 cos(φ) cot(α2 ))/(rr rw )
− (kvr l3 cos(φ) cot(α3 ))/(rr rw ) − (kvr l4 cos(φ) cot(α4 ))/(rr rw )
2
f3,2 = − (l1 sin(α1 + φ)(krw + kvw ))/(rw sin(α1 ))
2
− (l2 sin(α2 + φ)(krw + kvw ))/(rw sin(α2 ))
2
− (l3 sin(α3 + φ)(krw + kvw ))/(rw sin(α3 ))
2
− (l4 sin(α4 + φ)(krw + kvw ))/(rw sin(α4 ))
− (kvr l1 sin(φ) cot(α1 ))/(rr rw ) − (kvr l2 sin(φ) cot(α2 ))/(rr rw )
− (kvr l3 sin(φ) cot(α3 ))/(rr rw ) − (kvr l4 sin(φ) cot(α4 ))/(rr rw )
f3,3 = − ((krw + kvw )(l12 + l22 + l32 + l42 ))/rw
2

f3,4 = − (kvw (l1 + l2 + l3 + l4 ))/rw

f4,1 = (4kvw sin(φ))/rw − (kvw cos(φ) cot(α1 ))/rw − (kvw cos(φ) cot(α2 ))/rw
− (kvw cos(φ) cot(α3 ))/rw − (kvw cos(φ) cot(α4 ))/rw
f4,2 = − (4kvw cos(φ))/rw − (kvw sin(φ) cot(α1 ))/rw − (kvw sin(φ) cot(α2 ))/rw
− (kvw sin(φ) cot(α3 ))/rw − (kvw sin(φ) cot(α4 ))/rw
f4,3 = − (kvw (l1 + l2 + l3 + l4 ))/rw
f4,4 = − 4kvw

 − cos(α − cos(α2 +φ − cos(α3 +φ − cos(α4 +φ

1 +φ

 −rwsin(α
sin(α1 )
1 +φ
rw sin(α2 )
− sin(α2 +φ
rw sin(α3 )
− sin(α3 +φ
rw sin(α4 )
− sin(α4 +φ 
(A.2.5)
 
Bp (p) =  rw sin(α1 ) rw sin(α2 ) rw sin(α3 ) rw sin(α4 ) 
 −l1 −l2 −l3 −l4 
 rw rw rw rw 
−1 −1 −1 −1

247
Appendix A CMD Model Matrix Definitions

A.3 Matrices for CMD Dynamics Model in (p, v)

 
m1,1 m1,2 m1,3 0
 
m2,1 m2,2 m2,3 m2,4 
Mv (p) = 
m
 (A.3.1)
 3,1 m3,2 m3,3 0  
0 m4,2 0 m4,4

2 2
m1,1 = mp + 4mw − (4Iwx )/rw + Iwx /(rw sin(α1 )2 ) + Iwx /(rw
2
sin(α2 )2 )
2
+ Iwx /(rw sin(α3 )2 ) + Iwx /(rw
2
sin(α4 )2 )
2
m1,2 = (Iwx (cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/rw
2
m1,3 = (Iwx (l1 cot(α1 ) + l2 cot(α2 ) + l3 cot(α3 ) + l4 cot(α4 )))/rw + hp mp sin(θp )
2
m2,1 = (Iwx (cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/rw
2
m2,2 = mp + 4mw + (4Iwx )/rw
2 2
m2,3 = ((mw rw + Iwx )(l1 + l2 + l3 + l4 ))/rw
m2,4 = − hp mp cos(θp )
2
m3,1 = (Iwx (l1 cot(α1 ) + l2 cot(α2 ) + l3 cot(α3 ) + l4 cot(α4 )))/rw + hp mp sin(θp )
2 2
m3,2 = ((mw rw + Iwx )(l1 + l2 + l3 + l4 ))/rw
m3,3 = 4Iwyz + (Iwx l12 + Iwx l22 + Iwx l32 + Iwx l42 )/rw
2
+ Ipby sin(θp )2 + l12 mw + l22 mw
+ l32 mw + l42 mw − Ipbz (sin(θp )2 − 1) + h2p mp sin(θp )2
m4,2 = − hp mp cos(θp )
m4,4 = mp h2p + Ipbx

 
0 c1,2 c1,3 c1,4
 
c2,1 0 c2,3 c2,4 
Cv (p, v) = 
  (A.3.2)
c c c c 
 3,1 3,2 3,3 3,4 
c4,1 0 c4,3 0

c1,2 = − φ̇(mp + 4mw )

c1,3 = hp mp cos(θp )θ̇p − l2 mw φ̇ − l3 mw φ̇ − l4 mw φ̇ − l1 mw φ̇

248
 A.3 Matrices for CMD Dynamics Model in (p, v)

c1,4 = hp mp cos(θp )φ̇

c2,1 = φ̇(mp + 4mw )
c2,3 = hp mp sin(θp )φ̇
c2,4 = hp mp sin(θp )θ̇p
c3,1 = mw φ̇(l1 + l2 + l3 + l4 )
c3,2 = − hp mp sin(θp )φ̇
c3,3 = (sin(2θp )θ̇p (mp h2p + Ipby − Ipbz ))/2
c3,4 = (sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))/2
c4,1 = − hp mp cos(θp )φ̇
c4,3 = − (sin(2θp )φ̇(mp h2p + Ipby − Ipbz ))/2

" #
03×1
Gv (p) = (A.3.3)
−ghp mp sin(θp )

 
f1,1 f1,2 f1,3 f1,4
 
f2,1 f2,2 f2,3 f2,4 
Fv = 
f
 (A.3.4)
 3,1 f3,2 f3,3 f3,4 

f4,1 f4,2 f4,3 f4,4

249
Appendix A CMD Model Matrix Definitions

2
f1,1 = (kvr rw sin(α1 )2 sin(α2 )2 − kvw rr2 sin(α1 )2 sin(α3 )2 − kvw rr2 sin(α2 )2 sin(α3 )2
− kvw rr2 sin(α1 )2 sin(α2 )2 + kvr rw
2
sin(α1 )2 sin(α3 )2
2
+ kvr rw sin(α2 )2 sin(α3 )2 − krw rr2 sin(α1 )2 sin(α2 )2
− krw rr2 sin(α1 )2 sin(α3 )2 − krw rr2 sin(α2 )2 sin(α3 )2
− kvr rr rw sin(α1 )2 sin(α2 )2 − kvr rr rw sin(α1 )2 sin(α3 )2
− kvr rr rw sin(α2 )2 sin(α3 )2 + kvw rr rw sin(α1 )2 sin(α2 )2
+ kvw rr rw sin(α1 )2 sin(α3 )2 + kvw rr rw sin(α2 )2 sin(α3 )2
+ 4krw rr2 sin(α1 )2 sin(α2 )2 sin(α3 )2 + 4kvw rr2 sin(α1 )2 sin(α2 )2 sin(α3 )2
+ krw rr rw sin(α1 )2 sin(α2 )2 + krw rr rw sin(α1 )2 sin(α3 )2
+ krw rr rw sin(α2 )2 sin(α3 )2 − 4krw rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2
+ 4kvr rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2
− 4kvw rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2 )
2
/(rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2 (rr − rw ))
− (krw rr2 sin(α1 )2 sin(α2 )2 sin(α3 )2 + kvw rr2 sin(α1 )2 sin(α2 )2 sin(α3 )2
2
− kvr rw sin(α1 )2 sin(α2 )2 sin(α3 )2 − krw rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2
+ kvr rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2
− kvw rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2 )
2
/(rr rw sin(α1 )2 sin(α2 )2 sin(α3 )2 sin(α4 )2 (rr − rw ))
2
f1,2 = − ((krw + kvw )(cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/rw
2
f1,3 = − ((krw + kvw )(l1 cot(α1 ) + l2 cot(α2 ) + l3 cot(α3 ) + l4 cot(α4 )))/rw
f1,4 = − (kvw (cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/rw
2
f2,1 = − ((krw rr + kvw rr + kvr rw )(cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/(rr rw )
2
f2,2 = − (4(krw + kvw ))/rw
2
f2,3 = − ((krw + kvw )(l1 + l2 + l3 + l4 ))/rw
f2,4 = − (4kvw )/rw
f3,1 = − ((krw rr + kvw rr + kvr rw )(l1 cot(α1 ) + l2 cot(α2 ) + l3 cot(α3 )
2
+ l4 cot(α4 )))/(rr rw )
2
f3,2 = − ((krw + kvw )(l1 + l2 + l3 + l4 ))/rw

250
 A.3 Matrices for CMD Dynamics Model in (p, v)

f3,3 = − ((krw + kvw )(l12 + l22 + l32 + l42 ))/rw

2

f3,4 = − (kvw (l1 + l2 + l3 + l4 ))/rw

f4,1 = − (kvw (cot(α1 ) + cot(α2 ) + cot(α3 ) + cot(α4 )))/rw
f4,2 = − (4kvw )/rw
f4,3 = − (kvw (l1 + l2 + l3 + l4 ))/rw
f4,4 = − 4kvw

 
− cot(α
rw
1)
− cot(α
rw
2)
− cot(α
rw
3)
− cot(α
rw
4)

 − r1 − r1w − r1w − r1w 

 
Bv = 
 − l1
w  (A.3.5)
 rw − rlw2 − rlw3 − rlw4 

−1 −1 −1 −1

251
 Appendix B

MATLAB Model Derivation and

Analysis Code

1 function [ out ] = model_derivation ( generateBlocks )

2

3 syms t real;
4 syms m_p m_w I_pbx I_pby I_pbz I_wx I_wyz h_p l_1 l_2 ...
l_3 l_4 r_w r_r g alpha_1 alpha_2 alpha_3 alpha_4 ...
k_vr k_vw K_rw real;
5 syms x(t) y(t) phi(t) theta_p (t) theta_1 (t) theta_2 (t) ...
theta_3 (t) theta_4 (t) omega_1 (t) omega_2 (t) omega_3 (...
t) omega_4 (t);
6 syms dx(t) dy(t) dtheta_p (t) dphi(t) dtheta_1 (t) ...
dtheta_2 (t) dtheta_3 (t) dtheta_4 (t) domega_1 (t) ...
domega_2 (t) domega_3 (t) domega_4 (t);
7 syms vx(t) vy(t);
8 syms ddx ddy ddphi ddtheta_p ddtheta_1 ddtheta_2 ...
ddtheta_3 ddtheta_4 ddomega_1 ddomega_2 ddomega_3 ...
ddomega_4 real;
9 syms dvx dvy real;
10 syms tau_1 tau_2 tau_3 tau_4 real;
11

12 assumeAlso (x(t), 'real ');

13 assumeAlso (y(t), 'real ');
14 assumeAlso (phi(t), 'real ');

253
 Appendix B MATLAB Model Derivation and Analysis Code

15 assumeAlso ( theta_p (t), 'real ');

16 assumeAlso ( theta_1 (t), 'real ');
17 assumeAlso ( theta_2 (t), 'real ');
18 assumeAlso ( theta_3 (t), 'real ');
19 assumeAlso ( theta_4 (t), 'real ');
20 assumeAlso ( omega_1 (t), 'real ');
21 assumeAlso ( omega_2 (t), 'real ');
22 assumeAlso ( omega_3 (t), 'real ');
23 assumeAlso ( omega_4 (t), 'real ');
24 assumeAlso (dx(t), 'real ');
25 assumeAlso (dy(t), 'real ');
26 assumeAlso (dphi(t), 'real ');
27 assumeAlso ( dtheta_p (t), 'real ');
28 assumeAlso ( dtheta_1 (t), 'real ');
29 assumeAlso ( dtheta_2 (t), 'real ');
30 assumeAlso ( dtheta_3 (t), 'real ');
31 assumeAlso ( dtheta_4 (t), 'real ');
32 assumeAlso ( domega_1 (t), 'real ');
33 assumeAlso ( domega_2 (t), 'real ');
34 assumeAlso ( domega_3 (t), 'real ');
35 assumeAlso ( domega_4 (t), 'real ');
36 assumeAlso (vx(t), 'real ');
37 assumeAlso (vy(t), 'real ');
38 assumeAlso (m_p > 0);
39 assumeAlso (m_w > 0);
40 assumeAlso (I_pbx > 0);
41 assumeAlso (I_pby > 0);
42 assumeAlso (I_pbz > 0);
43 assumeAlso (I_wx > 0);
44 assumeAlso (I_wyz > 0);
45 assumeAlso (r_w > 0);
46 assumeAlso (( alpha_1 > -pi/2 & alpha_1 < 0) | ( alpha_1 <...
pi/2 & alpha_1 > 0));
47 assumeAlso (( alpha_2 > -pi/2 & alpha_2 < 0) | ( alpha_2 <...

254
 pi/2 & alpha_2 > 0));
48 assumeAlso (( alpha_3 > -pi/2 & alpha_3 < 0) | ( alpha_3 <...
pi/2 & alpha_3 > 0));
49 assumeAlso (( alpha_4 > -pi/2 & alpha_4 < 0) | ( alpha_4 <...
pi/2 & alpha_4 > 0));
50

51 % Define number of wheels

52 nw = 4;
53

54 if nw == 3
55 theta_i = { theta_1 theta_2 theta_3 };
56 dtheta_i = { dtheta_1 dtheta_2 dtheta_3 };
57 ddtheta_i = { ddtheta_1 ddtheta_2 ddtheta_3 };
58 omega_i = { omega_1 omega_2 omega_3 };
59 domega_i = { domega_1 domega_2 domega_3 };
60 ddomega_i = { ddomega_1 ddomega_2 ddomega_3 };
61 l_i = [l_1 l_2 l_3 ];
62 alpha_i = [ alpha_1 alpha_2 alpha_3 ];
63 tau_i = [tau_1 tau_2 tau_3 ]';
64 elseif nw == 4
65 theta_i = { theta_1 theta_2 theta_3 theta_4 };
66 dtheta_i = { dtheta_1 dtheta_2 dtheta_3 dtheta_4 };
67 ddtheta_i = { ddtheta_1 ddtheta_2 ddtheta_3 ddtheta_4 };
68 omega_i = { omega_1 omega_2 omega_3 omega_4 };
69 domega_i = { domega_1 domega_2 domega_3 domega_4 };
70 domega_i = { ddomega_1 ddomega_2 ddomega_3 ddomega_4 };
71 l_i = [l_1 l_2 l_3 l_4 ];
72 alpha_i = [ alpha_1 alpha_2 alpha_3 alpha_4 ];
73 tau_i = [tau_1 tau_2 tau_3 tau_4 ]';
74 end
75 I_w = diag ([ I_wx I_wyz I_wyz ]);
76 I_p = diag ([ I_pbx I_pby I_pbz ]);
77

78

255
 Appendix B MATLAB Model Derivation and Analysis Code

79 % Full generalised coordinates

80 if nw == 3
81 q = {x; y; phi; theta_p ; theta_1 ; theta_2 ; theta_3 ; ...
omega_1 ; omega_2 ; omega_3 ;};
82 dq = {dx; dy; dphi; dtheta_p ; dtheta_1 ; dtheta_2 ; ...
dtheta_3 ; domega_1 ; domega_2 ; domega_3 };
83 ddq = {ddx; ddy; ddphi; ddtheta_p ; ddtheta_1 ; ddtheta_2...
; ddtheta_3 ; ddomega_1 ; ddomega_2 ; ddomega_3 };
84 elseif nw == 4
85 q = {x; y; phi; theta_p ; theta_1 ; theta_2 ; theta_3 ; ...
theta_4 ; omega_1 ; omega_2 ; omega_3 ; omega_4 };
86 dq = {dx; dy; dphi; dtheta_p ; dtheta_1 ; dtheta_2 ; ...
dtheta_3 ; dtheta_4 ; domega_1 ; domega_2 ; domega_3 ; ...
domega_4 };
87 ddq = {ddx; ddy; ddphi; ddtheta_p ; ddtheta_1 ; ddtheta_2...
; ddtheta_3 ; ddtheta_4 ; ddomega_1 ; ddomega_2 ; ...
ddomega_3 ; ddomega_4 };
88 end
89

90 % Reduced generalised coordinates

91 p = {x; y; phi; theta_p };
92 dp = {dx; dy; dphi; dtheta_p };
93 ddp = [ddx; ddy; ddphi; ddtheta_p ];
94

95 % pv generalised coordinates
96 v = {vx; vy; dphi; dtheta_p };
97 dv = [dvx; dvy; ddphi; ddtheta_p ];
98

99 % input
100 u = [tau_i ];
101

102

103 % It seems either these rotation matrices or their ...

transpose can be

256
104 % correct , provided the correct sign is applied to l_i ...
in the constraint
105 % matrix. Kim and others use this convention , ...
autonomous mobile robots uses
106 % another.
107

108 % Now using convention of x is right , y is forward , z ...

is up , all rotations
109 % are by right hand rule.
110

111 %% Define rotation matrices

112 % Rotation from intertial frame E to body attached ...
frame B
113 R_EB = [cos(phi) -sin(phi) 0;
114 sin(phi) cos(phi) 0;
115 0 0 1];
116 sel = [eye (2); 0 0];
117 R_EB_2d = sel '* R_EB*sel;
118

119 % Rotation from body attached frame B to pendulum rigid...

body frame P
120 R_BP = [1 0 0
121 0 cos( theta_p ) -sin( theta_p )
122 0 sin( theta_p ) cos( theta_p )];
123

124 % Rotation from body to wheel frame

125 for i=1: nw
126 R_BW{i} = [1 0 0
127 0 cos( theta_i {i}) -sin( theta_i {i})
128 0 sin( theta_i {i}) cos( theta_i {i})];
129 end
130

131 % Rotation from body to non - rotating roller frame (...

acceptable simplification if roller is massless )

257
 Appendix B MATLAB Model Derivation and Analysis Code

132 for i=1: nw

133 R_BR{i} = [cos( alpha_i (i)) -sin( alpha_i (i)) 0;
134 sin( alpha_i (i)) cos( alpha_i (i)) 0;
135 0 0 1];
136 end
137

138 %% Velocities derivation

139 % Define body angular velocity
140 omega_b = [0; 0; dphi ];
141

142 % Define pendulum angular velocity

143 omega_p = R_BP. '* omega_b + [ dtheta_p ; 0; 0];
144

145 % Define wheel angular velocity

146 for i=1: nw
147 % Both of these expressions give identical results
148 % omega_w {i} = R_BW{i}'* omega_b + [ dtheta_i {i}; 0; 0];
149 omega_w {i} = omega_b + [ dtheta_i {i}; 0; 0];
150 end
151

152 % Define body frame velocities in terms of q

153 v_b = R_EB. '*[ dx; dy; 0];
154

155 % Define pendulum translational velocities

156 v_p = R_BP. '* v_b + cross(omega_p , [0; 0; h_p ]);
157

158 % Define wheel translational velocities

159 for i=1: nw
160 % Both of these expressions give identical results
161 % v_w{i} = R_BW{i}'* v_b + cross ( omega_w {i}, [l_i(i); 0;...
0]);
162 v_w{i} = v_b + cross( omega_w {i}, [l_i(i); 0; 0]);
163 end
164

258
165 %% Derivation of constraints , inverse kinematics , and M...
(q)
166 for i=1: nw
167 % Body frame velocity at center of wheel (W)
168 v_Wb{i} = v_b + [0; dphi*l_i(i); 0];
169

170 % Body frame velocity of roller contact point (C)

171 v_Cb{i} = v_Wb{i} + [0; -r_w*- dtheta_i {i}; 0];
172

173 % Body frame roller rotation axis u_p and transverse ...
axis u_t
174 u_p{i} = R_BR{i }*[1;0;0];
175 u_t{i} = R_BR{i }*[0;1;0];
176

177 % Contraints
178 constraintNoSlip (i ,1) = dot( formula (v_Cb{i}),u_p{i});
179 constraintRolling (i ,1) = dot( formula (v_Cb{i}),u_t{i}) -...
-r_r* domega_i {i};
180 end
181

182 % Solve no slip constraint for theta_i

183 for i=1: nw
184 theta_iSolved (i ,1) = solve( stripTimeDependence (...
constraintNoSlip (i)), stripTimeDependence ( formula (...
dtheta_i {i})));
185 end
186

187 % Pfaffian constraint matrix

188 A = subs( equationsToMatrix ( stripTimeDependence ([...
constraintNoSlip ; constraintRolling ]), ...
stripTimeDependence ( formula ([dx; dy; dphi; dtheta_p ;...
dtheta_i '; domega_i ']))),stripTimeDependence (...
formula (phi)),formula (phi));
189

259
 Appendix B MATLAB Model Derivation and Analysis Code

190

191 %% Energies
192 % Translational kinetic energy
193 K_trans = 0.5*m_p *( v_p. '* v_p);
194 for i=1: nw
195 K_trans = K_trans + 0.5*m_w *( v_w{i}.'* v_w{i});
196 end
197

198 % Rotational kinetic energy

199 K_rot = 0.5*omega_p. '* I_p* omega_p ;
200 for i=1: nw
201 K_rot = K_rot + 0.5* omega_w {i}.'* I_w* omega_w {i};
202 end
203

204 K_trans = simplify ( K_trans );

205 K_rot = simplify (K_rot , 5); % Doesnt seem to simplify ...
cos(x)^2+ sin(x)^2=1 with less than 5 simplification ...
steps
206

207 % Potential Energy

208 U = m_p*g*h_p*cos( theta_p );
209

210 % Total Energy

211 L = K_trans + K_rot - U;
212

213

214 %% Euler - Lagrange Derivation

215 % Generalised forces
216 Q_tau = [
217 zeros (3 ,1);
218 -sum(tau_i);
219 tau_i;
220 zeros(nw ,1);
221 ];

260
222

223 if nw == 3
224 Q_fric = [
225 [eye (2) zeros (2 ,1) ]* R_EB*k_vr *[1/( - r_w+r_r) ;0;0]* dot...
([0;1;0] , formula (R_BR {1}*[ domega_1 ;0;0] + R_BR {2}*[...
domega_2 ;0;0] + R_BR {3}*[ domega_3 ;0;0]) );
226 0;
227 sum(k_vw *( dtheta_i - dtheta_p ));
228 -K_rw* dtheta_1 + k_vw *( dtheta_p - dtheta_1 ) + k_vr*dot...
([1;0;0] , formula (R_BR {1}*[ domega_1 ; 0; 0]));
229 -K_rw* dtheta_2 + k_vw *( dtheta_p - dtheta_2 ) + k_vr*dot...
([1;0;0] , formula (R_BR {2}*[ domega_2 ; 0; 0]));
230 -K_rw* dtheta_3 + k_vw *( dtheta_p - dtheta_3 ) + k_vr*dot...
([1;0;0] , formula (R_BR {3}*[ domega_3 ; 0; 0]));
231 -k_vr*domega_i ';
232 ];
233 elseif nw == 4
234 Q_fric = [
235 [eye (2) zeros (2 ,1) ]* R_EB*k_vr *[1/( - r_w+r_r) ;0;0]* dot...
([0;1;0] , formula (R_BR {1}*[ domega_1 ;0;0] + R_BR {2}*[...
domega_2 ;0;0] + R_BR {3}*[ domega_3 ;0;0] + R_BR {4}*[...
domega_4 ;0;0]) );
236 0;
237 sum(k_vw *( dtheta_i - dtheta_p ));
238 -K_rw* dtheta_1 + k_vw *( dtheta_p - dtheta_1 ) + k_vr*dot...
([1;0;0] , formula (R_BR {1}*[ domega_1 ; 0; 0]));
239 -K_rw* dtheta_2 + k_vw *( dtheta_p - dtheta_2 ) + k_vr*dot...
([1;0;0] , formula (R_BR {2}*[ domega_2 ; 0; 0]));
240 -K_rw* dtheta_3 + k_vw *( dtheta_p - dtheta_3 ) + k_vr*dot...
([1;0;0] , formula (R_BR {3}*[ domega_3 ; 0; 0]));
241 -K_rw* dtheta_4 + k_vw *( dtheta_p - dtheta_4 ) + k_vr*dot...
([1;0;0] , formula (R_BR {4}*[ domega_4 ; 0; 0]));
242 -k_vr*domega_i ';
243 ];

261
 Appendix B MATLAB Model Derivation and Analysis Code

244 end
245

246 % Compute Lagrangian

247 lambda = sym('lambda ', [size(A ,1) 1], 'real ');
248 model = diff( functionalDerivative (L,dq),t) - ...
functionalDerivative (L,q) == A. '* lambda + Q_tau + ...
Q_fric ;
249

250 % Substitute diff(q,t) for dq and diff(dq ,t) for ddq

251 model = subs(model , diff(q,t), dq);
252 model = subs(model , diff(dq ,t), ddq);
253

254 %% Fitting model to standard form M(q)ddq + C(q,dq)dq +...

G(q) == A' lambda + Bu + Fdq
255 % Rearrange LHS into terms of inertia and coriolis ...
matrix
256 out.qdq.Mq = equationsToMatrix (lhs(model), ddq);
257

258 % Test for symmetry

259 if ¬isequal ( simplify (out.qdq.Mq - out.qdq.Mq. '), sym(...
zeros(numel(q))))
260 warning ('Mq is asymmetric ')
261 end
262

263 % Test for positive SEMIdefiniteness of Mq - expected ...

as rollers are massless & inertialess
264 if all( isAlways (eig( simplify ( substituteParameters (...
out.qdq.Mq )))>-eps)) == false
265 warning ('Mq is not positive semidefinite ');
266 end
267

268 % Derive C(q,dq) matrix using Christoffel symbols of ...

Mq. This operation is
269 % very slow , but seems to just be due to the number of ...

262
 calls to
270 % functionalDerivative , i.e. no way no improve. Running...
this as a single
271 % thread profiled at 554s, running with the parfor at ...
212 s.
272 for i = 1: size( out.qdq.Mq ,1)
273 parfor j = 1: size( out.qdq.Mq ,1)
274 Cqdq(i,j) = sym (0); % Required to set type of cell
275 for k = 1: size( out.qdq.Mq ,1)
276 Cqdq(i,j) = Cqdq(i,j) + 0.5*( ...
277 functionalDerivative ( out.qdq.Mq (i,j),q{k}) ...
278 + functionalDerivative ( out.qdq.Mq (i,k),q{j}) ...
279 - functionalDerivative ( out.qdq.Mq (j,k),q{i}) ...
280 ) * dq{k};
281 end
282 end
283 end
284 out.qdq.Cqdq = Cqdq;
285 clear Cqdq;
286

287 Test for skew symmetry of dMq - 2Cqdq

288 dMq_minus_2Cqdq = subs(diff(out.qdq.Mq ,t) - 2*...
out.qdq.Cqdq , diff(q,t), dq);
289 if ¬isequal ( simplify ( dMq_minus_2Cqdq + dMq_minus_2Cqdq. ...
'), sym(zeros (numel (q))))
290 warning ('dMq -2 Cqdq is not skew symmetric ');
291 end
292

293 % Derive input matrix B

294 out.qdq.B = equationsToMatrix (rhs(model ), u);
295

296 if any( Q_fric 6

= 0)
297 % Derive friction matrix Fq
298 temp = sym('temp ' ,[ numel(dq) 1]);

263
 Appendix B MATLAB Model Derivation and Analysis Code

299 out.qdq.F = equationsToMatrix (subs(rhs(model) - A. '*...

lambda , dq , temp), temp);
300 else
301 out.qdq.F = sym(zeros(numel(q)));
302 end
303

304 % Derive gravity matrix Gq

305 out.qdq.Gq = formula ( simplify (lhs(model) - out.qdq.Mq *...
ddq - out.qdq.Cqdq *dq));
306

307 % Gq should contain a single term

308 if nnz( formula ( out.qdq.Gq )) > 1
309 warning ('Mq and Cqdq do not fully describe passive ...
unconstrained Lagrangian ');
310 end
311

312 % Package matrices and model

313 out.qdq.model = out.qdq.Mq *ddq + out.qdq.Cqdq *dq + ...
out.qdq.Gq == out.qdq.F *dq + out.qdq.B *tau_i;
314

315

316 %% Convert model from generalised coordinates q to p

317 % Calculate null matrix such that AH =0
318 % Calculating the null matrix for a reordering of q ...
into q =
319 % [ theta_i x y phi theta_p ] then swapping back to the ...
original
320 % ordering yields an identity mapping for x y phi ...
theta_p
321 out.pdp.H = null ([A(:,( numel(p)+1) :end) A(: ,1: numel(p))...
]);
322 out.pdp.H = [ out.pdp.H ((end -( numel (p) -1)) :end ,:); ...
out.pdp.H (1: numel(q)-numel(p) ,:)];
323

264
324 if ¬isequal ( simplify (A* out.pdp.H ), sym(zeros(numel(q)-...
numel(p), numel(p))))
325 error('H is not a basis for the nullspace of A');
326 end
327

328 % Redefine Mp
329 out.pdp.Mp = simplify (out.pdp.H. '* out.qdq.Mq * out.pdp.H )...
;
330

331 Retest for symmetry

332 if ¬isequal ( simplify (out.pdp.Mp - out.pdp.Mp. '), sym(...
zeros(numel(p))))
333 warning ('Mp is asymmetric ')
334 end
335

336 % Retest for positive definiteness

337 if all( isAlways (eig( simplify ( substituteParameters (...
out.pdp.Mp )))>eps)) == false
338 warning ('Mp is not positive definite ');
339 end
340

341 % Redefine Cqdq

342 out.pdp.Cpdp = simplify (out.pdp.H. '* out.qdq.Cqdq *...
out.pdp.H + out.pdp.H. '* out.qdq.Mq *subs(diff(...
out.pdp.H ,t), diff(p,t), dp));
343

344 % Retest for skew symmetry of dMq - 2Cqdq

345 dMp_minus_2Cpdp = subs(diff(out.pdp.Mp ,t) - 2*...
out.pdp.Cpdp , diff(p,t), dp);
346 if ¬isequal ( simplify ( dMp_minus_2Cpdp + dMp_minus_2Cpdp. ...
'), sym(zeros (numel (p))))
347 warning ('dMp -2 Cpdp is not skew symmetric ');
348 end
349

265
 Appendix B MATLAB Model Derivation and Analysis Code

350 % Redefine gravity matrix

351 out.pdp.Gp = out.pdp.H. ' * out.qdq.Gq ;
352

353 % Redefine input matrix

354 out.pdp.Bp = simplify (out.pdp.H. ' * out.qdq.B );
355

356 % Redefine friction matrix

357 out.pdp.Fp = simplify (out.pdp.H. ' * out.qdq.F * ...
out.pdp.H );
358

359 % % Build model

360 % out.pdp.model = out.pdp.Mp *ddp + out.pdp.Cpdp *dp + ...
out.pdp.Gp == out.pdp.Fp *dp + out.pdp.Bp *tau_i ;
361 % % Currently runs out of memory when inverting Mp ...
without substituting parameters
362 % out.pdp.dynamics = inv( substituteParameters (...
out.pdp.Mp ))*( out.pdp.Fp *dp + out.pdp.Bp *tau_i - ...
out.pdp.Cpdp *dp - out.pdp.Gp );
363 % out.pdp.dpddp = [dp; out.pdp.dynamics ];
364

365 % Solve no slip constraint for theta_i , and rolling ...

constraint for omega_i
366 temp = sym('temp ', [4 1]);
367 for i=1: nw
368 theta_iSolved (i ,1) = solve(subs( constraintNoSlip (i), ...
dtheta_i (i), temp(i)), temp(i));
369 omega_iSolved (i ,1) = solve(subs( constraintRolling (i), ...
domega_i (i), temp(i)), temp(i));
370 end
371 out.pdp.inverseKinematicsM = equationsToMatrix (subs(...
theta_iSolved , dp , temp), temp);
372 out.pdp.inverseKinematicsIncRollersM = ...
equationsToMatrix (subs ([ theta_iSolved ; omega_iSolved...
], dp , temp), temp);

266
373

374

375 % Calculate generalised momentums conjugate to external...

and shape variables
376 out.pdp.p_x = out.pdp.Mp (1:2 ,1:2)*dp (1:2) + out.pdp.Mp...
(1:2 ,3:4)*dp (3:4) ;
377 out.pdp.p_s = out.pdp.Mp (3:4 ,1:2)*dp (1:2) + out.pdp.Mp...
(3:4 ,3:4)*dp (3:4) ;
378

379 % Calculate normalised momentums

380 out.pdp.pi_x = inv( out.pdp.Mp (1:2 ,1:2)) * out.pdp.p_x ;
381 out.pdp.pi_s = inv( out.pdp.Mp (3:4 ,1:2)) * out.pdp.p_s ;
382

383 % Derivation method 2 for comparison

384 out.pdp.pi_x_2 = dp (1:2) + inv( out.pdp.Mp (1:2 ,1:2))*...
out.pdp.Mp (1:2 ,3:4)*dp (3:4);
385 out.pdp.pi_s_2 = dp (1:2) + inv( out.pdp.Mp (3:4 ,1:2))*...
out.pdp.Mp (3:4 ,3:4)*dp (3:4);
386

387 %% Convert model from generalised coordinates (q,dq ,ddq...

) to (p,v,dv)
388 % Calculate null matrix such that AH =0
389 % Calculating the null matrix for a reordering of q ...
into q =
390 % [ theta_i x y phi theta_p ] then swapping back to the ...
original
391 % ordering yields an identity mapping for x y phi ...
theta_p
392 out.pv.H = null ([A(:,5 :end) A(: ,1:4) ]);
393 out.pv.H = [ out.pv.H (end -3 :end ,:); out.pv.H (1: numel(q)-...
numel(p) ,:)];
394

395 % Rotate dx dy to vx vy
396 out.pv.Rv = blkdiag ( formula ( R_EB_2d ),sym(eye (2)));

267
 Appendix B MATLAB Model Derivation and Analysis Code

397 out.pv.H = out.pv.H * out.pv.Rv ;

398

399 if ¬isequal ( simplify (A* out.pv.H ), sym(zeros(numel(q)-...

numel(p), numel(p))))
400 error('H is not a basis for the nullspace of A');
401 end
402

403 % Redefine Mp
404 out.pv.Mp = simplify (out.pv.H. '* out.qdq.Mq * out.pv.H );
405

406 % Retest for symmetry

407 if ¬isequal ( simplify (out.pv.Mp -out.pv.Mp. '), sym(zeros(...
numel(p))))
408 warning ('Mp is asymmetric ')
409 end
410

411 % Retest for positive definiteness

412 if all( isAlways (eig( simplify ( substituteParameters (...
out.pv.Mp )))>eps)) == false
413 warning ('Mp is not positive definite ');
414 end
415

416 % Redefine Cqv

417 out.pv.Cpv = simplify (out.pv.H. '* out.qdq.Cqdq * out.pv.H ...
+ out.pv.H. '* out.qdq.Mq *subs(diff(out.pv.H ,t), diff(...
p,t), out.pv.Rv *v));
418

419 % Retest for skew symmetry of dMp - 2Cpv

420 dMp_minus_2Cpv = subs(diff(out.pv.Mp ,t) - 2* out.pv.Cpv ,...
diff(p,t), out.pv.Rv *v);
421 if ¬isequal ( simplify ( dMp_minus_2Cpv + dMp_minus_2Cpv. ')...
, sym(zeros (numel(p))))
422 warning ('dMp -2 Cpv is not skew symmetric ');
423 end

268
424

425 % Redefine gravity matrix

426 out.pv.Gp = out.pv.H. '* out.qdq.Gq ;
427

428 % Redefine input matrix

429 out.pv.B = simplify (out.pv.H. '* out.qdq.B );
430

431 % Calculate simplified input u

432 out.pv.Bu = colspace ( out.pv.B );
433 out.pv.Htau = pinv( out.pv.Bu )* out.pv.B ;
434 out.pv.Htausubs = double ( substituteParameters (...
out.pv.Htau ));
435

436 % Redefine friction matrix

437 out.pv.F = simplify (out.pv.H. '* out.qdq.F * out.pv.H );
438

439 % Redefine model

440 out.pv.model = out.pv.Mp *dv + out.pv.Cpv *v + out.pv.Gp ...
== out.pv.F *v + out.pv.B *tau_i;
441 out.pv.dynamics = inv( out.pv.Mp )*( out.pv.F *v + out.pv.B...
*tau_i - out.pv.Cpv *v - out.pv.Gp );
442 out.pv.inverseDynamics = pinv( out.pv.B )*( out.pv.Mp *dv +...
out.pv.Cpv *v + out.pv.Gp - out.pv.F *v);
443 out.pv.dpdv = [ out.pv.Rv *v; out.pv.dynamics ];
444

445 % Redefine inverse kinematics in terms of v

446 out.pv.inverseKinematicsM = simplify (...
out.pdp.inverseKinematicsM * out.pv.Rv );
447 out.pv.inverseKinematicsIncRollersM = simplify (...
out.pdp.inverseKinematicsIncRollersM * out.pv.Rv );
448 out.pv.inverseKinematics = out.pv.inverseKinematicsM *v;
449 out.pv.inverseKinematicsIncRollers = ...
out.pv.inverseKinematicsIncRollersM *v;
450

269
 Appendix B MATLAB Model Derivation and Analysis Code

451 % Calculate generalised momentums conjugate to external...

and shape variables
452 out.pv.p_x = out.pv.Mp (1:3 ,1:3)*v(1:3) + out.pv.Mp...
(1:3 ,4)*v(4);
453 out.pv.p_s = out.pv.Mp (4 ,1:3)*v(1:3) + out.pv.Mp (4 ,4)*v...
(4);
454

455 % Calculate normalised momentums

456 out.pv.pi_x = inv( out.pv.Mp (1:3 ,1:3)) * out.pv.p_x ;
457 out.pv.pi_s = pinv( out.pv.Mp (4 ,1:3)) * out.pv.p_s ;
458

459 % Derivation method 2 for comparison

460 out.pv.pi_x_2 = v (1:3) + inv( out.pv.Mp (1:3 ,1:3))*...
out.pv.Mp (1:3 ,4)*v(4);
461 out.pv.pi_s_2 = v (1:3) + pinv( out.pv.Mp (4 ,1:3))*...
out.pv.Mp (4 ,4)*v(4);
462

463

464 % Define expression and function for calculation of dvy

465 out.pv.dvy_tau = substituteParameters ( out.pv.dynamics...
(2));
466 state = sym('state ' ,[8 1]);
467 out.pv.dvy_tau_fun = matlabFunction (subs( out.pv.dvy_tau ...
, [p;v], state), 'Vars ', {state , tau_i }, 'File ', '...
Feedback_Linearisation / generatedFunctions / dvy_tau ');
468

469

470 % Arrange model in form d(p,v) = f(p,v) + g u

471 out.pv.f = [ out.pv.Rv *v; inv( out.pv.Mp )*( out.pv.F *v - ...
out.pv.Cpv *v - out.pv.Gp )];
472 out.pv.gtau = [ zeros (4,nw); inv( out.pv.Mp )* out.pv.B ];
473 out.pv.g = [zeros (4 ,3); inv( out.pv.Mp )* out.pv.Bu ];
474

475 % Define error model with rotated Cartesian position ...

270
 error
476 e = sym('e_ ' ,[8 ,1]); e_u = sym('e_u_ ' ,[nw 1]); xr = ...
sym('xr_ ' ,[8 ,1]); tau_r = sym('tau_r_ ' ,[nw ,1]);
477

478 % Define Cartesian error in body frame

479 phi_e_xr = xr (3) - e(3);
480 e1e2 = subs( formula ( R_EB_2d ).', phi , phi_e_xr )*[xr (1) -p...
(1);xr (2) -p(2) ];
481 e1e2 = subs(e1e2 , [p;v], xr -e);
482 e1 = e1e2 (1); e2 = e1e2 (2);
483

484 % Define dynamics

485 f_xr = subs( out.pv.dpdv , [p;v], xr);
486 f_xr = subs(f_xr , tau_i , tau_r);
487 f_xr_mns_e = subs( out.pv.dpdv , [p;v], xr -e);
488 f_xr_mns_e = subs(f_xr_mns_e , tau_i , tau_r -e_u);
489

490 % Define integral errors

491 syms e_x_int e_y_int e_phi_int real
492 e_z = blkdiag (subs( formula ( R_EB_2d ).', phi , phi_e_xr ), ...
1) * [ e_x_int ; e_y_int ; e_phi_int ];
493

494 % Define error dynamics

495 out.pv.de = [
496 -e_z (2)* f_xr_mns_e (3) + e1;
497 e_z (1)* f_xr_mns_e (3) + e2;
498 e(3);
499 e2* f_xr_mns_e (3) + cos( phi_e_xr )*f_xr (1) + sin( phi_e_xr...
)*f_xr (2) - cos( phi_e_xr )* f_xr_mns_e (1) - sin(...
phi_e_xr )* f_xr_mns_e (2);
500 -e1* f_xr_mns_e (3) - sin( phi_e_xr )*f_xr (1) + cos(...
phi_e_xr )*f_xr (2) + sin( phi_e_xr )* f_xr_mns_e (1) - ...
cos( phi_e_xr )* f_xr_mns_e (2);
501 f_xr (3:8) - f_xr_mns_e (3:8)

271
 Appendix B MATLAB Model Derivation and Analysis Code

502 ];
503

504 % Linearise error dynamics about e=0

505 out.pv.Ae = subs( jacobian (out.pv.de , [ e_x_int ; e_y_int ;...
e_phi_int ;e]) , [ e_x_int ; e_y_int ; e_phi_int ; e;e_u],...
[zeros (11 ,1); zeros(nw ,1) ]);
506 out.pv.Be = subs( jacobian (out.pv.de , e_u), [ e_x_int ; ...
e_y_int ; e_phi_int ; e;e_u], [ zeros (11 ,1); zeros(nw...
,1) ]);
507

508 out.pv.Ae_fun = matlabFunction ( simplify (...

substituteParameters ( out.pv.Ae ) ,100) , 'Vars ', {xr , ...
tau_r}, 'File ', 'Differential_Flatness /...
generatedFunctions / TVLQR_A_fun ');
509 out.pv.Be_fun = matlabFunction ( simplify (...
substituteParameters ( out.pv.Be ) ,100) , 'Vars ', {xr , ...
tau_r}, 'File ', 'Differential_Flatness /...
generatedFunctions / TVLQR_B_fun ');
510 out.pv.e_fun = matlabFunction (subs ([ subs(e_z , e, xr -[p;...
v]); formula (R_EB_2d. ')*[xr (1) -p(1);xr (2) -p(2) ]; xr...
(3:8) -[p (3:4);v]], [p;v], state), 'Vars ', {state ,xr...
,[ e_x_int ; e_y_int ; e_phi_int ]}, 'File ', '...
Differential_Flatness / generatedFunctions / TVLQR_e_fun...
');
511

512 %% Feedback linearisation

513 % Uses the fully parametrised model , using Pathak 's ...
method but for the more general case
514 % This method should work with assymetric wheel ...
configurations , odd
515 % roller angles etc.
516

517 % Calculate new input matrix P to simplify the input ...

vector fields

272
518 out.pv.P = out.pv.g ([5 7 8] ,:);
519

520 % New input vector fields g_hat - too complex to invert...

P without some substitutions
521 out.pv.gHat = [
522 zeros (4 ,3);
523 1 0 0;
524 out.pv.g (6 ,:)* simplify (inv( substituteParameters (...
out.pv.P , true)));
525 0 1 0;
526 0 0 1];
527

528 % Calculate feedback linearising input in x

529 w = sym('w', [3 1]);
530 out.pv.uFB = [
531 w(1) - out.pv.f (5);
532 (w(2) - out.pv.f (7));
533 w(3) - out.pv.f (8);
534 ];
535

536 out.pv.fNew = [
537 out.pv.f (1:4);
538 0;
539 out.pv.f (6) - out.pv.gHat (6 ,1)* out.pv.f (5) - ...
out.pv.gHat (6 ,2)* out.pv.f (7) - out.pv.gHat (6 ,3)*...
out.pv.f (8);
540 0;
541 0];
542

543 % Calculate change of coordinates z=T(x)

544 x = [p;v];
545 out.pz.T = [
546 x(1);
547 x(2);

273
 Appendix B MATLAB Model Derivation and Analysis Code

548 x(3);
549 x(4);
550 x(5);
551 x(6) - x(5)* out.pv.gHat (6 ,1) - x(7)* out.pv.gHat (6 ,2) - ...
x(8)* out.pv.gHat (6 ,3);
552 x(7);
553 x(8);
554 ];
555

556 % and inverse change of coordinates x=Tinv(z)

557 z = sym('z', [8 1], 'real ');
558 out.pz.Tinv = [
559 z(1);
560 z(2);
561 z(3);
562 z(4);
563 z(5);
564 z(6) + subs(z(5)* out.pv.gHat (6 ,1) + z(7)* out.pv.gHat...
(6 ,2) + z(8)* out.pv.gHat (6 ,3) , x(4) , z(4));
565 z(7);
566 z(8);
567 ];
568

569

570 % Calculate new drift and input vector fields in z

571 out.pz.f = subs ([
572 cos(z(3))*z(5) - sin(z(3))*(z(6) + z(5)* out.pv.gHat...
(6 ,1) + z(7)* out.pv.gHat (6 ,2) + z(8)* out.pv.gHat...
(6 ,3));
573 sin(z(3))*z(5) + cos(z(3))*(z(6) + z(5)* out.pv.gHat...
(6 ,1) + z(7)* out.pv.gHat (6 ,2) + z(8)* out.pv.gHat...
(6 ,3));
574 z(7)
575 z(8);

274
576 0;
577 out.pv.f (6) - out.pv.f (5)* out.pv.gHat (6 ,1) - out.pv.f...
(7)* out.pv.gHat (6 ,2) - out.pv.f (8)* out.pv.gHat (6 ,3) ...
- z(5)*diff( out.pv.gHat (6 ,1) ,t) - z(7)*diff(...
out.pv.gHat (6 ,2) ,t) - z(8)*diff( out.pv.gHat (6 ,3) ,t);
578 0;
579 0;
580 ], x, out.pz.Tinv );
581

582 out.pz.g = [
583 0 0 0
584 0 0 0
585 0 0 0
586 0 0 0
587 1 0 0
588 0 0 0
589 0 1 0
590 0 0 1
591 ];
592

593 % Zero dynamics should be calculated by setting dz =0 ...

for controlled states ,
594 % ie dthetap =dphi=w=0
595 % zeroDynamics = subs( substituteParameters (subs(...
out.pz.f , [k_vr , k_vw , K_rw], [0 0 0])), z([3 4 5 7 ...
8]) , zeros (5 ,1));
596 zeroDynamics = subs(out.pz.f , [z([5 7 8]); w], zeros...
(6 ,1));
597

598

599 % Calculate feedback linearising input in z

600 out.pz.uFB = subs(out.pv.uFB , [p;v], out.pz.Tinv );
601

602 % Derive expressions for dvy with and without ...

275
 Appendix B MATLAB Model Derivation and Analysis Code

acceleration due to rotation in terms of x

603 syms dvy real;
604 out.pv.dvy = out.pv.f (6) - out.pv.gHat (6 ,1)* out.pv.f (5)...
- out.pv.gHat (6 ,2)* out.pv.f (7) - out.pv.gHat (6 ,3)*...
out.pv.f (8) + out.pv.gHat (6 ,1)*w(1) + out.pv.gHat...
(6 ,2)*w(2) + out.pv.gHat (6 ,3)*w(3);
605

606 % Create functions to return evaluation of fdvy - dvy ...

for given
607 % state and input - used to numerically solve for ...
theta_p_ss , i.e.
608 % f_dvy ^-1
609 state = sym('state ' ,[8 1]);
610 fun = subs(-dvy + substituteParameters ( out.pv.dvy ), x, ...
state);
611 dFun = jacobian (fun ,state (4));
612 out.pv.f_solve_fdvy_for_theta_p_x = matlabFunction (fun ,...
dFun , 'Vars ', {state ,w,dvy}, 'Outputs ', {'fun ','...
dFun '}, 'File ', 'Feedback_Linearisation /...
generatedFunctions / f_solve_fdvy_theta_p_x ');
613

614 % Define steady state ( dtheta_p =w3 =0) dvy 's

615 syms dvy_ss real;
616 out.pv.dvy_ss = subs( substituteParameters ( out.pv.dvy ), ...
{w(3) dtheta_p }, {0 0});
617

618 % Convert to function

619 state = sym('state ' ,[8 1]);
620 out.pv.dvy_ss_fun = matlabFunction ( formula (subs(...
substituteParameters ( out.pv.dvy_ss ), x, state)), '...
Vars ', {state ,w}, 'Outputs ', {'dvy_ss '}, 'File ', '...
Feedback_Linearisation / generatedFunctions / dvy_ss_fun...
');
621

276
622 % Define steady state ( constant dvy) derivative of dvy
623 % This should capture variation of dvy for a given ...
theta_p due to
624 % dvx and ddphi
625 syms dtheta_p_ss real;
626 out.pv.ddvy_ss = subs(diff( out.pv.dvy_ss ,t), diff(...
theta_p ), dtheta_p_ss );
627 out.pv.ddvy_ss = subs( out.pv.ddvy_ss , [diff(vx) diff(...
dphi)], [w(1) w(2) ]);
628

629 syms ddvy real;

630 out.pv.dtheta_p_ss = solve (ddvy == out.pv.ddvy_ss , ...
dtheta_p_ss );
631

632 if nw ==4
633

634 %% Velocity controller (body frame ), constrained ...

theta_p
635 syms Kv Kdphi Kr Kw1 Kw2 theta_p_max w1_max w2_max ...
theta_pr fss_minus1_dvy0 vxr vyr dphir ...
K_slow_dtheta_p K_slow_theta_p real;
636 body_vel_control_dtheta_pr = -subs( out.pv.dtheta_p_ss , ...
{theta_p , ddvy}, {theta_pr , 0}) *( theta_p_max ^2 - ...
theta_pr ^2) / ( fss_minus1_dvy0 * theta_pr - ...
theta_p_max ^2) ...
637 - Kr*( theta_pr - fss_minus1_dvy0 )*( fss_minus1_dvy0 *...
theta_pr - theta_p_max ^2) ...
638 - (( theta_p_max ^2- theta_pr ^2) ^2 * subs( out.pv.dvy_ss ,...
theta_p , theta_pr )*Kv *(vyr -vy)) / (( fss_minus1_dvy0 *...
theta_pr - theta_p_max ^2) *( theta_pr - ...
fss_minus1_dvy0 ));
639 body_vel_control_dw1 = -Kw1*w(1) + Kv*(vxr -vx)*( w1_max...
^2 - w(1) ^2) ^2;
640 body_vel_control_dw2 = -Kw2*w(2) + Kdphi *( dphir -dphi)*(...

277
 Appendix B MATLAB Model Derivation and Analysis Code

w2_max ^2 - w(2) ^2) ^2;

641

642 % Slow response

643 body_vel_control_dtheta_pr = body_vel_control_dtheta_pr...
* exp(- K_slow_dtheta_p *abs( dtheta_p ) - ...
K_slow_theta_p *abs(theta_p - theta_pr ));
644

645 out.pv.body_vel_control_dw1_dw2_dtheta_pr_fun = ...

matlabFunction ( ...
646 formula (subs( formula ( body_vel_control_dtheta_pr ), x, ...
state)), ...
647 formula (subs( formula ( body_vel_control_dw1 ), x, state)),...
...
648 formula (subs( formula ( body_vel_control_dw2 ), x, state)),...
...
649 'Vars ', {state w Kv Kdphi Kr Kw1 Kw2 theta_p_max w1_max...
w2_max theta_pr fss_minus1_dvy0 vxr vyr dphir ...
K_slow_dtheta_p K_slow_theta_p }, 'Outputs ', {'...
dtheta_pr ' 'dw1 ' 'dw2 '}, 'File ', '...
Feedback_Linearisation / generatedFunctions /...
body_vel_control_dw1_dw2_dtheta_pr_fun ', 'Optimize ',...
false);
650

651

652 %% Velocity controller ( inertial frame), constrained ...

theta_p
653 syms Kv Kdphi Kr Kw1 Kw2 theta_p_max w1_max w2_max ...
theta_pr fss_minus1_dvy_mvxdphi dxr dyr dphir real;
654 inertial_vel_control_dtheta_pr = -subs(...
out.pv.dtheta_p_ss , {theta_p , ddvy}, {...
fss_minus1_dvy_mvxdphi , -w(1)*x(7) - w(2)*x(5) })*(...
theta_p_max ^2 - theta_pr ^2) / (...
fss_minus1_dvy_mvxdphi * theta_pr - theta_p_max ^2) ...
655 + exp(- K_slow_dtheta_p *abs( dtheta_p ) - K_slow_theta_p *...

278
 abs(theta_p - theta_pr ))*( ...
656 - Kr*( theta_pr - fss_minus1_dvy_mvxdphi )*(...
fss_minus1_dvy_mvxdphi * theta_pr - theta_p_max ^2) ...
657 - (( theta_p_max ^2- theta_pr ^2) ^2 * (subs( out.pv.dvy_ss , ...
theta_p , theta_pr ) + vx*dphi)*Kv*(vyr -vy)) / ((...
fss_minus1_dvy_mvxdphi * theta_pr - theta_p_max ^2) *(...
theta_pr - fss_minus1_dvy_mvxdphi )) ...
658 );
659

660 % inertial_vel_control_dw1 = (dphi* out.pv.dvy_ss + vy*w...

(2)) -Kw1 *(w(1) - dphi*vy) + Kv*(vxr -vx)*( w1_max - w...
(1) + vy*dphi)^2*( w1_max + w(1) - vy*dphi)^2 / ...
w1_max ;
661 inertial_vel_control_dw1 = (dphi* out.pv.dvy_ss + vy*w...
(2)) -Kw1 *(w(1) - dphi*vy) + Kv*(vxr -vx)*( w1_max - w...
(1) + vy*dphi)^2*( w1_max + w(1) - vy*dphi)^2 / ...
w1_max ;
662 inertial_vel_control_dw2 = -Kw2*w(2) + Kdphi *( dphir -...
dphi)*( w2_max ^2 - w(2) ^2) ^2;
663

664 % Substitute vxr for dxr etc

665 inertial_vel_control_dtheta_pr = subs(...
inertial_vel_control_dtheta_pr , [vxr; vyr], R_EB_2d. ...
'*[ dxr;dyr ]);
666 inertial_vel_control_dw1 = subs(...
inertial_vel_control_dw1 , [vxr; vyr], R_EB_2d. '*[ dxr...
;dyr ]);
667 inertial_vel_control_dw2 = subs(...
inertial_vel_control_dw2 , [vxr; vyr], R_EB_2d. '*[ dxr...
;dyr ]);
668

669

670 out.pv.inertial_vel_control_dw1_dw2_dtheta_pr_fun = ...

matlabFunction ( ...

279
 Appendix B MATLAB Model Derivation and Analysis Code

671 formula (subs( formula ( inertial_vel_control_dtheta_pr ), x...

, state)), ...
672 formula (subs( formula ( inertial_vel_control_dw1 ), x, ...
state)), ...
673 formula (subs( formula ( inertial_vel_control_dw2 ), x, ...
state)), ...
674 'Vars ', {state w Kv Kdphi Kr Kw1 Kw2 theta_p_max w1_max...
w2_max theta_pr dxr dyr dphir ...
fss_minus1_dvy_mvxdphi K_slow_dtheta_p ...
K_slow_theta_p }, 'Outputs ', {'dtheta_pr ' 'dw1 ' 'dw2 '...
}, 'File ', 'Feedback_Linearisation /...
generatedFunctions /...
inertial_vel_control_dw1_dw2_dtheta_pr_fun ', '...
Optimize ', true);
675

676

677 % Inertial velocity controller with differential ...

flatness derived perturbations
678 syms Kvx Kvy Kdphi Kr Kw1 Kw2 theta_p_max w1_max w2_max...
theta_pr fss_minus1_dvy_f_dvyp dxr dyr dphir real;
679

680 % Circular dependency here

681 [dxp , dyp , vxp , vyp , dvxp , dvyp , ddvxp , ddvyp] = ...
calculateInertialVelocityPerturbations (dxr , dyr , phi...
, dphir);
682

683 inertial_vel_control_dtheta_pr = ...

684 -subs( out.pv.dtheta_p_ss , {theta_p , ddvy}, {...
fss_minus1_dvy_f_dvyp , ddvyp }) *( theta_p_max ^2 - ...
theta_pr ^2) / ( fss_minus1_dvy_f_dvyp * theta_pr - ...
theta_p_max ^2) ...
685 + exp(- K_slow_dtheta_p *abs( dtheta_p ) - K_slow_theta_p *...
abs(theta_p - theta_pr ))*( ...
686 - Kr*( theta_pr - fss_minus1_dvy_f_dvyp )*(...

280
 fss_minus1_dvy_f_dvyp * theta_pr - theta_p_max ^2) ...
687 - (( theta_p_max ^2- theta_pr ^2) ^2 * (subs( out.pv.dvy_ss , ...
theta_p , theta_pr ) - dvyp)*Kvy *( vyr+vyp -vy)) / ((...
fss_minus1_dvy_f_dvyp * theta_pr - theta_p_max ^2) *(...
theta_pr - fss_minus1_dvy_f_dvyp )) ...
688 );
689

690 inertial_vel_control_dw1 = ddvxp - Kw1 *(w(1) - dvxp) + ...

Kvx *( vxr+vxp -vx)*( w1_max +dvxp - w(1))^2*( w1_max - ...
dvxp + w(1))^2 / w1_max ^2;
691 inertial_vel_control_dw2 = -Kw2*w(2) + Kdphi *( dphir -...
dphi)*( w2_max ^2 - w(2) ^2) ^2;
692

693 % Substitute vxr for dxr etc

694 inertial_vel_control_dtheta_pr = subs(...
inertial_vel_control_dtheta_pr , [vxr; vyr], R_EB_2d. ...
'*[ dxr;dyr ]);
695 inertial_vel_control_dw1 = subs(...
inertial_vel_control_dw1 , [vxr; vyr], R_EB_2d. '*[ dxr...
;dyr ]);
696 inertial_vel_control_dw2 = subs(...
inertial_vel_control_dw2 , [vxr; vyr], R_EB_2d. '*[ dxr...
;dyr ]);
697

698

699 out.pv.inertial_vel_control_dw1_dw2_dtheta_pr_fun = ...

matlabFunction ( ...
700 simplify (subs( formula ( inertial_vel_control_dtheta_pr ), ...
formula ( vertcat (x{:})), state)), ...
701 simplify (subs( formula ( inertial_vel_control_dw1 ), ...
formula ( vertcat (x{:})), state)), ...
702 simplify (subs( formula ( inertial_vel_control_dw2 ), ...
formula ( vertcat (x{:})), state)), ...
703 'Vars ', {state w Kvx Kvy Kdphi Kr Kw1 Kw2 theta_p_max ...

281
 Appendix B MATLAB Model Derivation and Analysis Code

w1_max w2_max theta_pr dxr dyr dphir ...

fss_minus1_dvy_f_dvyp K_slow_dtheta_p K_slow_theta_p...
}, ...
704 'Outputs ', {'dtheta_pr ' 'dw1 ' 'dw2 '}, ...
705 'File ', 'Feedback_Linearisation / generatedFunctions /...
inertial_vel_control_2_dw1_dw2_dtheta_pr_fun ', ...
706 'Optimize ', true);
707

708

709 %% Position controller

710 syms K_vr K_p K_dphir K_phi v_max dphi_max xr yr phir ...
K_slow real;
711 dxdy = formula ( R_EB_2d *[x(5); x(6) ;]);
712 ddxr = -K_vr*dxr + (v_max ^2 - dxr ^2 - dyr ^2) ^2 * K_p *(...
xr - x(1));
713 ddyr = -K_vr*dyr + (v_max ^2 - dxr ^2 - dyr ^2) ^2 * K_p *(...
yr - x(2));
714 ddphir = -K_dphir * dphir + ( dphi_max ^2 - dphir ^2) ^2 * ...
K_phi *( phir - x(3));
715

716 ddxr = ddxr * exp(- K_slow * abs(dxdy (1) - dxr));

717 ddyr = ddyr * exp(- K_slow * abs(dxdy (2) - dyr));
718 ddphir = ddphir * exp(- K_slow * abs(x(7) - dphir));
719

720 out.pv.inertial_pos_control_ddxr_ddyr_ddphir_fun = ...

matlabFunction ( ...
721 subs(ddxr , x, state), ...
722 subs(ddyr , x, state), ...
723 subs(ddphir , x, state), ...
724 'Vars ', {state K_vr K_p K_dphir K_phi v_max dphi_max ...
dxr dyr dphir xr yr phir K_slow }, 'Outputs ', {'ddxr '...
'ddyr ' 'ddphir '}, 'File ', 'Feedback_Linearisation /...
generatedFunctions /...
inertial_pos_control_ddxr_ddyr_ddphir_fun ', '...

282
 Optimize ', true);
725 end
726

727 %% Nonlinear controllability

728 runNonlinearControllability = false;
729 if runNonlinearControllability
730 % Put system into nonlinear input - affine form dx = f(x)...
+ sum_{j=1}^4 g_j(x)u_j
731 x = sym('x' ,[8 1],'real ');
732 f = substituteParameters (subs ([ out.pv.Rv *v; inv(...
out.pv.Mp )*( out.pv.F *v - out.pv.Cpv *v - out.pv.Gp )],...
[p;v], x));
733 g = substituteParameters (subs ([ zeros (4 ,3); (inv(...
out.pv.Mp )* out.pv.Bu )], [p;v], x));
734 % f = substituteParameters (subs(subs ([ out.pv.Rv *v; ...
inv( out.pv.Mp )*( out.pv.F *v - out.pv.Cpv *v - ...
out.pv.Gp )], [p;v], x) ,[h_p k_vw k_vr ] ,[0 0 0]));
735 % g = substituteParameters (subs(subs ([ zeros (4 ,3); (...
inv( out.pv.Mp )* out.pv.Bu )], [p;v], x) ,[h_p k_vw k_vr...
],[0 0 0]));
736 f = subs(f, [p;v], x);
737 g = subs(g, [p;v], x);
738

739 % Define Lie bracket operation

740 lb = @(g1 ,g2 ,x) jacobian (g2 ,x)*g1 - jacobian (g1 ,x)*g2;
741

742

743 % Confirm G_0 is the involutive closure of G - not the ...

case unless dvx
744 % and ddphi are decoupled
745 lieBracket (g,g,x)
746

747 Q_0 = g;
748 Q_0_closure = Q_0;

283
 Appendix B MATLAB Model Derivation and Analysis Code

749 Q_1 = [ Q_0_closure lb(f,g(: ,1) ,x) lb(f,g(: ,2) ,x) lb(f,g...
(: ,3) ,x)];
750 Q_1_closure = [Q_1 lieBracket (Q_1 ,Q_1 ,x)];
751

752 % Controllability indices

753 r_0 = rank(Q_0);
754 r_1 = rank(Q_1) - rank(Q_0);
755

756

757 % Strong accessibility / STLA

758 Delta {1} = [f g];
759 disp ([ 'rank( Delta_1 ) = ' num2str (rank(Delta {1}))]);
760 for i =2:100
761 Delta{i} = [ Delta {i -1} lieBracket (Delta {1}, Delta{i-1},x...
)];
762 disp ([ 'rank( Delta_ ' num2str (i) ') = ' num2str (rank(...
Delta{i}))]);
763 if rank(Delta{i}) == rank(Delta{i -1})
764 break ;
765 end
766 end
767 end
768

769

770 %% Internal dynamics derivation

771 % Eliminate tau by subtracting second and fourth ...
expressions
772 out.internalDynamics = collect ( out.pv.model (2)*r_w - ...
out.pv.model (4) , tau_i);
773

774 if any( ismember (tau_i , symvar ( out.internalDynamics )))

775 error('Internal dynamics still contain tau ');
776 end
777

284
778

779 %% Generate C code

780 codegen f_solve_fdvy_theta_p_x -d ./...
Feedback_Linearisation / codegen /...
f_solve_fdvy_theta_p_x -c -args {zeros (8 ,1) , zeros...
(3 ,1) , 0}
781

782 temp = sym('temp ', [8 1]);

783 matlabFunction (subs( substituteParameters ( out.pz.T ), x, ...
temp), 'Vars ', {temp}, 'Outputs ', {'z'}, 'File ', '...
Feedback_Linearisation / generatedFunctions / T_x_to_z ')...
;
784 matlabFunction ( substituteParameters ( out.pz.uFB ), 'Vars '...
, {z,w}, 'Outputs ', {'v'}, 'File ', '...
Feedback_Linearisation / generatedFunctions /uFB ');
785

786

787 vin = sym('vin ', [3 1]);

788 matlabFunction (subs(inv( substituteParameters ( out.pv.P ))...
*vin , x, substituteParameters ( out.pz.Tinv )), 'Vars ',...
{z,vin}, 'Outputs ', {'u'}, 'File ', '...
Feedback_Linearisation / generatedFunctions / Pz_v_to_u '...
);
789

790

791 codegen T_x_to_z -d ./ Feedback_Linearisation / codegen /...

T_x_to_z -c -args { zeros (8 ,1)}
792 codegen uFB -d ./ Feedback_Linearisation / codegen /uFB -c ...
-args { zeros (8 ,1) , zeros (3 ,1)}
793 codegen Pz_v_to_u -d ./ Feedback_Linearisation / codegen /...
Pz_v_to_u -c -args { zeros (8 ,1) , zeros (3 ,1)}
794 codegen dvy_ss_fun -d ./ Feedback_Linearisation / codegen /...
dvy_ss -c -args { zeros (8 ,1) , zeros (3 ,1)}
795 codegen body_vel_control_dw1_dw2_dtheta_pr_fun -d ./...

285
 Appendix B MATLAB Model Derivation and Analysis Code

Feedback_Linearisation / codegen /...

body_vel_control_dw1_dw2_dtheta_pr_fun -c -args {...
zeros (8 ,1) zeros (3 ,1) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0}
796 codegen inertial_vel_control_dw1_dw2_dtheta_pr_fun -d ....
/ Feedback_Linearisation / codegen /...
inertial_vel_control_dw1_dw2_dtheta_pr_fun -c -args ...
{zeros (8 ,1) zeros (3 ,1) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ...
0}
797 codegen inertial_pos_control_ddxr_ddyr_ddphir_fun -d ./...
Feedback_Linearisation / codegen /...
inertial_pos_control_ddxr_ddyr_ddphir_fun -c -args {...
zeros (8 ,1) 0 0 0 0 0 0 0 0 0 0 0 0 0}
798 codegen dvy_tau -d ./ Feedback_Linearisation / codegen /...
dvy_tau_fun -c -args { zeros (8 ,1) zeros(nw ,1)}
799

800

801

802 %% Generate optimised simulink blocks and matlab ...

functions
803 stateInput = sym('stateInput ', [8 1]);
804 if generateBlocks
805 % Create matlab function for pv dynamics model
806 out.dpdv = matlabFunction (subs( substituteParameters (...
out.pv.dpdv ), [p;v], stateInput ) ...
807 , 'vars ', {stateInput , tau_i} ...
808 , 'outputs ', {'dpdv '});
809

810 % Create /open simulink library

811 if ¬exist('model_library ', 'file ')
812 new_system ('model_library ', 'Library ');
813 else
814 load_system ('model_library ');
815 end
816 set_param ('model_library ','Lock ','off ');

286
817

818 % % Create pdp model block

819 % matlabFunctionBlock (' model_library / dynamics_dpddp ...
', subs( substituteParameters ( out.pdp.dpddp ), [p;dp],...
stateInput ) ...
820 % , 'vars ', { stateInput , tau_i } ...
821 % , 'outputs ', {'dpddp '});
822

823 % Create pv model block

824 matlabFunctionBlock ('model_library / dynamics_dqdv ', subs...
( substituteParameters ( out.pv.dpdv ), [p;v], ...
stateInput ) ...
825 , 'vars ', {stateInput , tau_i} ...
826 , 'outputs ', {'dpdv '});
827

828 % Create pv inverse kinematics block

829 matlabFunctionBlock ('model_library /...
qv_inverse_kinematics ',subs( substituteParameters (...
out.pv.inverseKinematics ), [p;v], stateInput ) ...
830 , 'vars ', { stateInput } ...
831 , 'outputs ', {'dtheta_i '});
832

833 % % Create zeta ->u block

834 % matlabFunctionBlock (' model_library / pz_zeta_to_u ',...
subs( substituteParameters ( out.pz.uFB ), [p;z], ...
stateIn ) ...
835 % , 'vars ', {stateIn , zeta} ...
836 % , 'outputs ', {'u '});
837

838 % Create feedback linearising input block

839 matlabFunctionBlock ('model_library /uFB_z ',...
substituteParameters ( out.pz.uFB ) ...
840 , 'vars ', {z,w} ...
841 , 'outputs ', {'v'});

287
 Appendix B MATLAB Model Derivation and Analysis Code

842

843 % Create x->z transform z=T(x)

844 matlabFunctionBlock ('model_library / T_x_to_z ',subs(...
substituteParameters ( out.pz.T ), x, stateInput ) ...
845 , 'vars ', { stateInput } ...
846 , 'outputs ', {'z'});
847

848 % % Create z->x transform x=Tinv(z)

849 % matlabFunctionBlock (' model_library /T_z_to_x ',...
substituteParameters ( out.pz.Tinv ) ...
850 % , 'vars ', {z} ...
851 % , 'outputs ', {'x '});
852

853 % % Create u->v mapping function v=Pu in terms of x

854 % u = sym('u', [3 1], 'real ');
855 % matlabFunctionBlock (' model_library /Px_u_to_v ',...
subs( substituteParameters ( out.pv.P *u), x, stateInput...
) ...
856 % , 'vars ', { stateInput , u} ...
857 % , 'outputs ', {'v '});
858

859 % % Create u->v mapping function v=Pu in terms of z

860 % matlabFunctionBlock (' model_library /Pz_u_to_v ',...
subs( substituteParameters ( out.pv.P *u), x, ...
substituteParameters ( out.pz.Tinv )) ...
861 % , 'vars ', {z, u} ...
862 % , 'outputs ', {'v '});
863

864 % % Create v->u mapping function v=Pu in terms of x

865 % vin = sym('vin ', [3 1], 'real ');
866 % matlabFunctionBlock (' model_library /Px_v_to_u ',...
subs(inv( substituteParameters ( out.pv.P )*vin), x, ...
stateInput ) ...
867 % , 'vars ', { stateInput , vin} ...

288
868 % , 'outputs ', {'u '});
869

870 % Create v->u mapping function v=Pu in terms of z

871 vin = sym('vin ', [3 1], 'real ');
872 matlabFunctionBlock ('model_library / Pz_v_to_u ',subs(inv(...
substituteParameters ( out.pv.P ))*vin , x, ...
substituteParameters ( out.pz.Tinv )) ...
873 , 'vars ', {z, vin} ...
874 , 'outputs ', {'u'});
875

876 save_system ('model_library ' ,[],'...

OverwriteIfChangedOnDisk ',true);
877 close_system ('model_library ');
878 end
879

880 %% Generate linear systems

881 dqdvSubbed = subs(subs( substituteParameters ( out.pv.dpdv...
), [p;v], stateInput ), {cos(phi), sin(phi)}, {1 0});
882 A = double (subs( jacobian (dqdvSubbed , stateInput ), [...
stateInput ; tau_i], zeros (8+nw ,1)));
883 B = double (subs( jacobian (dqdvSubbed , tau_i), [...
stateInput ; tau_i], zeros (8+nw ,1)));
884 C = eye (8);
885

886 out.linsys = ss(A,B,C ,0);

887

888 %% Store function handles for later use

889 out.substituteAlphas = @(x) ( substituteAlphas (x));
890 out.substituteParameters = @(x) ( substituteParameters (x...
));
891 out.removeWheelMassInertiaTerms = @(x) (...
removeWheelMassInertiaTerms (x));
892 out.stripTimeDependence = @(x) ( stripTimeDependence (x))...
;

289
 Appendix B MATLAB Model Derivation and Analysis Code

893

894 end
895

896 % Function to calculate all possible Lie brackets of ...

two distributions
897 function out = lieBracket (F,G,x,depth)
898

899 if ¬exist('depth ','var ')

900 depth = 1;
901 end
902

903 % Define Lie bracket operator

904 lb = @(g1 ,g2 ,x) jacobian (g2 ,x)*g1 - jacobian (g1 ,x)*g2;
905

906 out = sym ([]);

907 for i=1: size(F ,2)
908 for j=i+1: size(G ,2)
909 out = [out lb(F(:,i),G(:,j),x)];
910 if depth >1
911 for k=1: depth -1
912 out = [out lb(out (: ,end),G(:,j),x)];
913 end
914 end
915 end
916 end
917 end
918

919

920 function [out] = substituteParameters (in , ...

simplificationForInversion )
921

922 if ¬exist('simplificationForInversion ', 'var ')

923 simplificationForInversion = false;
924 end

290
925

926 syms m_p m_w I_pbx I_pby I_pbz I_wx I_wyz h_p l_1 l_2 ...
l_3 l_4 r_w r_r g alpha_1 alpha_2 alpha_3 alpha_4 ...
k_vw k_vr K_rw real;
927 names ={}; values = {};
928 names{end +1} = alpha_1 ; values {end +1} = pi /4;
929 names{end +1} = alpha_2 ; values {end +1} = -pi /4;
930 names{end +1} = alpha_3 ; values {end +1} = pi /4;
931 names{end +1} = alpha_4 ; values {end +1} = -pi /4;
932 if ¬simplificationForInversion
933 names{end +1} = I_pbx; values {end +1} = 0.0315 ; % ...
Measured in pendulum experiment
934 names{end +1} = I_pby; values {end +1} = 0.0534 ;
935 names{end +1} = I_pbz; values {end +1} = 0.0271 ;
936 names{end +1} = I_wx; values {end +1} = 5.12e -5; %5.5e...
-05; % Thoroughly estimated at 500 Hz using ...
fit_wheel_inertia_model_accel
937 names{end +1} = I_wyz; values {end +1} = 0 .00011 ;
938 names{end +1} = h_p; values {end +1} = 4*0 .145 *0 .07 /(3...
.22 - 4*0 .145) + 0.07;
939 names{end +1} = m_p; values {end +1} = 3.22 - 4*0 .145;
940 names{end +1} = m_w; values {end +1} = 0.145;
941 names{end +1} = l_1; values {end +1} = -0.105;
942 names{end +1} = l_2; values {end +1} = -0.063;
943 names{end +1} = l_3; values {end +1} = 0.063;
944 names{end +1} = l_4; values {end +1} = 0.105;
945 names{end +1} = r_w; values {end +1} = 0.0595 /2;
946 names{end +1} = r_r; values {end +1} = 0.0055 ;
947 names{end +1} = g; values {end +1} = 9.81;
948 names{end +1} = k_vw; values {end +1} = 2.33e -5;
949 names{end +1} = k_vr; values {end +1} = 1.01e -4;% 9.7e...
-06;
950 names{end +1} = K_rw; values {end +1} = 1.97e -4;%(3e...
-04 - 1.5e -5) *0 .65;

291
 Appendix B MATLAB Model Derivation and Analysis Code

951 else
952 names{end +1} = l_3; values {end +1} = -l_2;
953 names{end +1} = l_4; values {end +1} = -l_1;
954 end
955 out = subs(in , names , values );
956 end
957

958 function [out] = substituteAlphas (in)

959 syms alpha_1 alpha_2 alpha_3 alpha_4 real;
960 names ={}; values = {};
961 names{end +1} = alpha_1 ; values {end +1} = pi /4;
962 names{end +1} = alpha_2 ; values {end +1} = -pi /4;
963 names{end +1} = alpha_3 ; values {end +1} = pi /4;
964 names{end +1} = alpha_4 ; values {end +1} = -pi /4;
965 % names {end +1} = alpha_1 ; values {end +1} = pi /4;
966 % names {end +1} = alpha_2 ; values {end +1} = -pi /4;
967 % names {end +1} = alpha_3 ; values {end +1} = -pi /4;
968 % names {end +1} = alpha_4 ; values {end +1} = pi /4;
969 out = simplify (subs(in , names , values ));
970 end
971

972 function [ out ] = stripTimeDependence (in)

973 syms( symvar (in));
974 if contains (char(in), 'diff ')
975 error('Cannot remove time dependence when expression ...
contains differentials ')
976 end
977 removeTimeDep = @(expr) str2sym ( strrep (char(expr), '(t)...
', ''));
978 out = arrayfun ( removeTimeDep , in);
979 end

292
View publication stats