Theorem 1.3 applies only to autonomous systems, i.e., when no explicit de- ...... In the following developments in this section, matrices ...... [81] R. W. Prouty.
SWITCHED POTENTIAL FIELDS FOR NAVIGATION AND CONTROL OF NONHOLONOMIC AND UNDERACTUATED AUTONOMOUS MOBILE ROBOTS
by Kaustubh Pathak
A dissertation submitted to the Faculty of the University of Delaware in partial fulfillment of the requirements for the degree of Doctor of Philosophy in Mechanical Engineering
Fall 2005
c 2005 Kaustubh Pathak
All Rights Reserved
SWITCHED POTENTIAL FIELDS FOR NAVIGATION AND CONTROL OF NONHOLONOMIC AND UNDERACTUATED AUTONOMOUS MOBILE ROBOTS
by Kaustubh Pathak
Approved: Thomas S. Buchanan, Ph.D. Chair of the Department of Mechanical Engineering
Approved: Eric W. Kaler, Ph.D. Dean of the College of Engineering
Approved: Conrado M. Gempesaw II, Ph.D. Vice Provost for Academic and International Programs
I certify that I have read this dissertation and that in my opinion it meets the academic and professional standard required by the University as a dissertation for the degree of Doctor of Philosophy. Signed: Sunil K. Agrawal, Ph.D. Professor in charge of dissertation I certify that I have read this dissertation and that in my opinion it meets the academic and professional standard required by the University as a dissertation for the degree of Doctor of Philosophy. Signed: Xin-Yan Deng, Ph.D. Member of dissertation committee I certify that I have read this dissertation and that in my opinion it meets the academic and professional standard required by the University as a dissertation for the degree of Doctor of Philosophy. Signed: Michael D. Greenberg, Ph.D. Member of dissertation committee I certify that I have read this dissertation and that in my opinion it meets the academic and professional standard required by the University as a dissertation for the degree of Doctor of Philosophy. Signed: Michael Keefe, Ph.D. Member of dissertation committee I certify that I have read this dissertation and that in my opinion it meets the academic and professional standard required by the University as a dissertation for the degree of Doctor of Philosophy. Signed: Eric R. Benson, Ph.D. Member of dissertation committee
ACKNOWLEDGMENTS
I would like to thank my advisor Dr. Sunil K. Agrawal for providing his support and guidance during my work on this dissertation. I appreciate his motivating various problems, allowing independent thinking, and being patient during this process. I extend my appreciation to my committee members, Dr. Eric Benson, Dr. Michael Greenberg, Dr. Michael Keefe, and Dr. Xin-Yan Deng for reviewing my work and providing guidance. I thankfully acknowledge the help of the Mechanical Engineering staff, especially Donna Fritz. I have benefited from my interaction with my labmates, and would like to thank Yongxing Hao, Randy Sleight, Kalyan Mankala, and So-ryeok Oh for their camaraderie. I affectionately thank my parents Pratibha and Satish Pathak, and my brother Gautam for their unwavering support of all my endeavors. I thank Don, Gary and Andreas for their friendship and help during this experience. Last, but not the least, my appreciation to Elias, Kris, Seth, P’taah, and Bashar for all the fun and learning.
iv
TABLE OF CONTENTS
LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiv Chapter 1 INTRODUCTION AND LITERATURE REVIEW . . . . . . . . . 1.1
1.2
1.3 1.4
Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
1.1.1
The Basic Navigation Scenario . . . . . . . . . . . . . . . . . .
1
Path Planning and Trajectory-tracking Control . . . . . . . . . . . .
3
1.2.1 1.2.2 1.2.3
3 8 9
Geometric Path-planning . . . . . . . . . . . . . . . . . . . . . Handling Constraints During Time-trajectory Planning . . . . Time-trajectory Tracking Control . . . . . . . . . . . . . . . .
Potential-Fields for Planning and Control of Mobile Robots . . . . . . 10 Stabilization of Nonholonomic Systems . . . . . . . . . . . . . . . . . 13 1.4.1 1.4.2 1.4.3 1.4.4
1.5 1.6 1.7
1
Defining Nonholonomy . . . . . . . . . . Brockett’s Theorem and Consequences . Time-varying Smooth Control . . . . . . Discontinuous, Piecewise Continuous and
. . . . . . . . . . . . . . . . . . . . . . . . . . . Hybrid Control
. . . .
. . . .
. . . .
13 14 15 16
Control of Underactuated Systems . . . . . . . . . . . . . . . . . . . . 17 Primary Research Questions and Scope of the Dissertation . . . . . . 20 Overview of Subsequent Chapters . . . . . . . . . . . . . . . . . . . . 22
v
2 FORMULATION OF THE SWITCHED POTENTIALS FRAMEWORK . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.1 2.2
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 Classical Formulation of Nonholonomic Dynamics . . . . . . . . . . . 24 2.2.1
2.3
Switched Potentials for Nonholonomic Vehicles . . . . . . . . . . . . . 28 2.3.1 2.3.2 2.3.3
2.4
Local Bounded Potential based Control Law Derivation . . . . 30 Handling Input and Speed Constraints . . . . . . . . . . . . . 32 Bubble Switching Condition . . . . . . . . . . . . . . . . . . . 35
Switched Potentials for a Class of Underactuated Vehicles . . . . . . . 36 2.4.1
2.5
Tricycle Model . . . . . . . . . . . . . . . . . . . . . . . . . . 26
Canonical Form of System Dynamics . . . . . . . . . . . . . . 36
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
3 SWITCHED POTENTIALS FOR NONHOLONOMY IN PLANAR MOTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.1 3.2 3.3
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 Unicycle Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . 41 Control Design and Handling Input/Speed Bounds . . . . . . . . . . 43 3.3.1 3.3.2 3.3.3
3.4
Kinematic Model based Control . . . . . . . . . . . . . . . . . . . . . 49 3.4.1
3.5
Controller A: Convergence to the Center . . . . . . . . . . . . 44 Controller B: Correcting the Orientation . . . . . . . . . . . . 45 Comparison with other approaches . . . . . . . . . . . . . . . 47
Handling Input Constraints . . . . . . . . . . . . . . . . . . . 51
Global Path Planning and Controller Integration . . . . . . . . . . . . 51 3.5.1
Switching Logic for Enclosing Bubble . . . . . . . . . . . . . . 53
vi
3.5.2 3.6
Experimental Framework . . . . . . . . . . . . . . . . . . . . . . . . . 56 3.6.1
3.7
Verification in Simulation . . . . . . . . . . . . . . . . . . . . 56
Reactive Avoidance for Multiple Robots . . . . . . . . . . . . 60
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
4 TWO TIME SCALE CONTROL FOR UNDERACTUATION WITH NONHOLONOMY . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.1
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.1.1
4.2 4.3 4.4 4.5
Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . Strong Accessibility Condition and Maximum Relative Degree Partial Feedback Linearization . . . . . . . . . . . . . . . . . . Velocity Controller Design Using Partial Linearization . . . . . 4.5.1 4.5.2 4.5.3
4.6
. . . .
. . . .
. . . .
. . . .
67 71 74 77
Lower-level Controller . . . . . . . . . . . . . . . . . . . . . . 80 Higher-level Controller . . . . . . . . . . . . . . . . . . . . . . 80 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 82
Position Stabilization Control . . . . . . . . . . . . . . . . . . . . . . 85 4.6.1 4.6.2 4.6.3 4.6.4
4.7
Inverted Wheeled Pendulum . . . . . . . . . . . . . . . . . . . 65
Effect of Non-zero Turning-speed on the Steady-state Heading Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . Higher-level Controller . . . . . . . . . . . . . . . . . . . . . . Lower-level Controller . . . . . . . . . . . . . . . . . . . . . . Simulation Results . . . . . . . . . . . . . . . . . . . . . . . .
85 86 90 90
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
5 SWITCHED POTENTIALS AND TWO TIME SCALES FOR UNDERACTUATION IN SPATIAL MOTION . . . . . . . . . . . 93 5.1
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
vii
5.2
Helicopters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94 5.2.1
5.3
Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 5.3.1 5.3.2 5.3.3
5.4
Lower Level Controller . . . . . . . . . . . . . . . . . . . . . . 100 Higher Level Controller . . . . . . . . . . . . . . . . . . . . . . 103 Handling Constraints . . . . . . . . . . . . . . . . . . . . . . . 108
Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 5.4.1 5.4.2 5.4.3
5.5
Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . 95
Path-Following Planning and Control . . . . . . . . . . . . . . 110 Bubble Switching and Heading Direction . . . . . . . . . . . . 113 Continuous vs. Discrete Control Simulations . . . . . . . . . . 114
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
6 A TIME-BASED ANALOG FOR SWITCHED POTENTIALS USING MODEL PREDICTIVE CONTROL . . . . . . . . . . . . . 119 6.1
Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 6.1.1
6.2
Planned Trajectory Computation . . . . . . . . . . . . . . . . . . . . 122 6.2.1
6.3
Band-limited Sinc Interpolant . . . . . . . . . . . . . . . . . . 124
Inverted Wheeled Pendulum . . . . . . . . . . . . . . . . . . . . . . . 126 6.3.1 6.3.2
6.4 6.5
System Formulation . . . . . . . . . . . . . . . . . . . . . . . 121
Trajectory Planning Simulation Results . . . . . . . . . . . . . 126 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 129
NMPC Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
7 CONCLUSIONS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138 7.1 7.2
Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139 viii
7.3 7.4 7.5
Assumptions and Limitations . . . . . . . . . . . . . . . . . . . . . . 140 Suggestions for Further Work . . . . . . . . . . . . . . . . . . . . . . 141 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . 142
APPENDICES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143 A INVERTED WHEELED PENDULUM DYNAMICS . . . . . . . . 143 B PROOFS RELATING TO HELICOPTER DYNAMICS . . . . . . 145 B.1 Rotational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 B.1.1 Miscellaneous Results . . . . . . . . . . . . . . . . . . . . . . . 145 B.1.2 Exponential Stability . . . . . . . . . . . . . . . . . . . . . . . 147 B.2 Translational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 150 BIBLIOGRAPHY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
ix
LIST OF FIGURES
1.1
The basic path-planning and control problem. . . . . . . . . . . . .
2
1.2
Example results from the path-planning algorithm. . . . . . . . . .
5
1.3
Path-planning results for a complex uneven terrain. . . . . . . . . .
5
2.1
Categorization of the examples studied in this work.
2.2
Tricycle schematic. . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.3
Controller Design Based on Switched Potentials. . . . . . . . . . . . 29
2.4
Control flow for the two time-scale scheme. . . . . . . . . . . . . . . 38
3.1
Unicycle inside a bubble with the coordinate systems illustrated. . . 41
3.2
Numerical determination of input limits for controllers A and B. . . 48
3.3
Finding vs and ωs for different potentials. . . . . . . . . . . . . . . 52
3.4
Creation of a new bubble centered at C2 . . . . . . . . . . . . . . . . 57
3.5
Links between the various bubbles in the global-plan S, and the current bubble of the robot Bi . . . . . . . . . . . . . . . . . . . . . 57
3.6
Simulation Results. . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
3.7
Image-processing for robot localization . . . . . . . . . . . . . . . . 59
3.8
Experiment 1. A single robot traverses its global-plan using bubble-switching. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
x
. . . . . . . . 24
3.9
Experiment 2. A mobile robot reactively avoids two static robots in its path which were not known to its path-planner. . . . . . . . . . 62
3.10
Experiment 3. All three robots run simultaneously to go to their goals. They reactively avoid each other using their IR sensors. . . . 63
4.1
Geometric parameters and coordinate systems for the inverted wheeled pendulum. . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
4.2
Forward acceleration as a function of the pitch angle for the inverted wheeled pendulum. . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
4.3
Comparison of responses to a stop command. . . . . . . . . . . . . 84
4.4
Comparison of responses to a step command. . . . . . . . . . . . . 84
4.5
Forward acceleration as a function of vehicle pitch-angle and turning speed. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
4.6
The position control simulation plots for 35 seconds. . . . . . . . . 92
5.1
Control blocks and their connectivity. . . . . . . . . . . . . . . . . . 96
5.2
Coordinate systems for the helicopter. . . . . . . . . . . . . . . . . 96
5.3
The HLC equation illustrated vectorially to show the definition of γi .110
5.4
Continuous-time simulation for a single bubble. . . . . . . . . . . . 112
5.5
The Cartesian path taken by the center of mass of the helicopter in the augmented plan. . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.6
Continuous-time simulation for a full path, with bubble-switching. . 116
5.7
Controllers applied discretely to a continuous model. . . . . . . . . 117
6.1
Schematic of the MPC Planner/Control Strategy. . . . . . . . . . . 121
6.2
Comparison between Sinc and polynomial approximation. . . . . . 125
xi
6.3
The planned x − y path, and the time-trajectories of α (rad), θ (rad), and v (m/s). . . . . . . . . . . . . . . . . . . . . . . . . . . . 130
6.4
This figure shows the result obtained after 10 iterations of SQP in 7 sec. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
6.5
The normalized planned motor-torques. . . . . . . . . . . . . . . . . 132
6.6
The tracking-error performance of the controller. . . . . . . . . . . 133
6.7
The first few steps of MPC shown for a particular component x of the state-vector. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
6.8
The first 4 steps shown for the NMPC applied to the inverted wheeled-pendulum example. . . . . . . . . . . . . . . . . . . . . . . 136
xii
LIST OF TABLES
3.1
Parameter Values for the Unicycle Dynamic Model . . . . . . . . . 47
3.2
Parameter Values for a Unicycle Kinematic Model . . . . . . . . . . 51
4.1
Simulation Parameters for the Inverted Wheeled Pendulum . . . . . 83
4.2
Modified/Additional Simulation Parameters for Position Control of Inverted Wheeled Pendulum. . . . . . . . . . . . . . . . . . . . . . 91
5.1
Simulation Parameters of a Scaled Helicopter Model. . . . . . . . . 111
6.1
NMPC Simulation Parameters . . . . . . . . . . . . . . . . . . . . . 131
xiii
ABSTRACT
Traditional use of potential-field methods in mobile robot navigation has been limited to omnidirectional (free-flying) robots, and the emphasis has been on the derivation of local minima free potential-functions in the configuration-space of the system. The presence of underactuation and nonholonomic constraints in many real-life robots usually render these methods ineffective. Nonholonomic constraints manifest in wheeled mobile robots due to no-slip constraints on the wheels, and in space robots, due to the conservation of total angular-momentum. Even if such a robot is subjected to a potential-field with a unique global minimum in configuration-space, it might still fail to reach this minimum on account of its kinematic constraints. A similar situation arises in underactuated robots, e.g., autonomous helicopters, where inputs based solely on potential-fields are not enough for system stability. This work introduces the concept of switched potential-fields as a viable alternative for path-planning and path-following control of autonomous mobile robots with kinematic constraints and/or underactuation. In the case of underactuation, potential-fields are combined with a two-time-scale state-space decomposition for designing control-laws. The proposed method is developed and illustrated within three classes of systems. Pure nonholonomic constraints are illustrated by a planar differentially driven two-wheeled robot. Nonholonomic constraints with underactuation is depicted by an inverted wheeled-pendulum which has an unactuated non-planar degree of freedom. Application to underactuated spatial motion is exemplified by an autonomous helicopter. xiv
The proposed method obviates the computation-time intensive step of finding a dynamically feasible time-trajectory, and its subsequent time-scaling for inputbound satisfaction. The vehicle is driven by a stabilizing control-law within an obstacle-free region, called a bubble, which is then switched to produce gross motion. The derivation of a suitable switching-law automatically guarantees speeds and inputs being restricted to specified bounds, while generating a smooth segue from bubble to bubble. The desired path can be modified in real-time based on sensor inputs. Extensions and combinations of the proposed methodology with other techniques point to its applicability for autonomous locomotion of a variety of vehicles in heterogenous media.
xv
Chapter 1 INTRODUCTION AND LITERATURE REVIEW
1.1
Motivation Autonomous locomotion of mobile air, ground and marine robots is an active
current research area. The interest in this field is fueled by the following main factors: (i) There exists a need for safe and accurate autonomous operation of payloadcarrying mobile vehicles for goods transportation, reconnaissance, and explorations in unknown environments like outer space. (ii) A wide variety of new sensors and actuators are available in the market. This is complemented by the advent of global positioning system (GPS), advances in image-processing, along with an availability of cheap and exponentially increasing onboard computation power [88]. (iii) Many of the currently employed mobile vehicles are nonholonomic and/or underactuated. Autonomous operation of such vehicles requires motion planning and controller design based on complex dynamic modeling, and the use of emerging nonlinear and hybrid control techniques. 1.1.1
The Basic Navigation Scenario Given a mobile vehicle, the objective is to have it navigate autonomously
to a desired point in its configuration-space (C-space) while avoiding obstacles in 1
Obstacle Obstacle
Goal
Obstacle NH/UA Robot 2D/3D Input/Speed Constrs. Obstacle Figure 1.1: The basic path-planning and control problem. its spatial path. This scenario is depicted in Fig. 1.1. Additional constraints on vehicle inputs and speeds may also be specified. Traditional approaches for solving the above problem usually involve the following three steps: (i) A geometrically feasible path to the goal-point is found using a search algorithm. Vehicle dynamics are ignored in this step. The result is a set of way-points leading to the goal. (ii) This path is time-parameterized. This step may be driven by constraint satisfaction or a task timing specification. (iii) A time-trajectory tracking controller is used to actually move the robot. These steps are individually considered in detail in the following Sec. 1.2. Artificial potential fields allow the aforementioned steps to be combined into one task. They will be reviewed in detail in Sec. 1.3. . Traditionally, they have been applied to omnidirectional fully actuated robots. This dissertation is motivated by the need to design potential field based controllers for a class of commonly employed nonholonomic and underactuated autonomous mobile robots. 2
A single control scheme cannot be expected to work in all scenarios. To incorporate flexibility, a mobile robot needs to be endowed with a library of controllers, each suited for a specific contingency. The robot can then switch between these controllers on the fly. These controllers should respond to local sensor information, while at the same time trying to follow higher-level global objectives. Local artificial potential fields offer an intuitive way of designing this library of motion controllers to generate different behaviors. A further motivation of this work is to design stable switching-strategies for such potential based controllers. The rest of this chapter is organized as follows: Mobile robot planning and control methods are reviewed in Sec. 1.2. Artificial potential-field theory is reviewed in Sec. 1.3. Results regarding nonholonomic stabilization are presented in Sec. 1.4. Lagrangian formulation of dynamics of nonholonomic vehicles will be covered in Sec. 2.2. Control of underactuated systems is dealt with in Sec. 1.5. Pros and cons of these approaches are highlighted and their connection to the approach taken in this dissertation is made. Finally, primary research questions and the scope of this work are outlined in Sec. 1.6. 1.2 1.2.1
Path Planning and Trajectory-tracking Control Geometric Path-planning A detailed overview of path-planning methods is provided in [55]. Briefly,
traditional methods may be categorized as (i) roadmap methods, (ii) exact and approximate cell decomposition methods, (iii) potential-field methods, and (iv) mathematical programming. The first two categories fall within the purview of Artificial Intelligence. The second category, in particular, is concerned with discretizing the motion C-space into cells, forming a connectivity-graph based on obstacles and optionally terrain, and a subsequent efficient graph-search to find a feasible and optimal path to the goal point. Many currently used search methods have been reviewed in
3
[8, 32, 55]. Although at this level nonholonomic constraints are usually not considered, trajectory planning which explicitly considers nonholonomy is explored in [93] and [65]. In this dissertation, path-planning is performed by a particular decomposition based method described in [15]. This method is based on an adaptive wavefront expansion algorithm. Fig. 1.2 depicts the output produced by this implementation for a planar and a spatial case. These figures have been generated using a C++ implementation developed by the author, as part of this work. The output is in the form of a string of obstacle-free spherical regions, henceforth called bubbles, leading from the start point to the goal point. The representation of obstacles is implementation-defined: they could be composed of a union of overlapping circles/spheres or lines/planes, as shown in Fig. 1.2. Fig. 1.3 shows a modification of this algorithm for uneven terrains. In this modification, the terrain is treated as an obstacle composed of a planar triangular plates. The advantages of this approach within the context of control-law design for autonomous robot locomotion, are as follows : 1. The discretization size varies according to the local complexity of geometry. This avoids the inefficient use of memory inherent in the prevalent uniform discretization schemes. 2. The output of the algorithm gives a feasible path to the goal and additionally, clearances to nearest obstacles. A vehicle traversing this path can thus utilize this clearance to account for its dynamics and other constraints. This sort of flexibility is not available in a traditional time-based approach wherein a rigid a priori dynamically feasible time-trajectory is tracked. A spherical bubble is a convenient virtual envelope to use for mobile robot control. (a) Such envelopes exist for conventional airplanes [106]. Each airplane flies 4
(a) A planar scenario. The opaque disks are obstacles. The planner generates a feasible path shown as a string of translucent disks.
(b) A spatial scenario. The obstacles are composed of triangular planes. The obstacle-free path is given by the translucent bubbles. The small opaque spheres represent the start and the goal points.
Figure 1.2: Example results from the path-planning algorithm.
(a) Triangular-mesh covered uneven terrain.
(b) A feasible path generated by the planner.
Figure 1.3: Path-planning results for a complex uneven terrain.
5
within a protected zone of about 5 nautical miles, which is further enclosed within an alert zone. (b) For ground vehicles equipped with a ring of proximity sensors like SONAR and Infra-red, the maximum range of these sensors also implicitly defines a known envelope around a vehicle as it moves. Such an envelope or bubble can be utilized to define a moving potential-field, which can be used to compute control-inputs for the vehicle. This procedure forms the crux of this work and is elucidated in subsequent chapters. The procedure of [15] is restated after minor modifications in Algorithm 1.2.1. Within the body of the algorithm, the terms Tree, List and Priority Queue are used as in Data-Structures and Graph Theory. All the obstacles are stored in a list Lo . The start position of the robot is denoted as rs = (xs , ys ) and the goal position as rg = (xg , yg ). The bubbles are stored in the queue along with their positions, radii and their parent bubbles, i.e. in the triplet format B(r, Rb , BP arent ). The root bubble at the goal position has no parent.
6
Algorithm 1.2.1: FindPath(Lo , rs , rg , Robot X) Initialize priority queue Q, Initialize tree T Compute Rb =
min kq q∈Lo
− rg k
BRoot = (rg , Rb , NULL) p = krg − rs k − Rb Insert BRoot in Q with priority p while Q is not empty Pop Q into B = (r, R, Bparent ) Insert B in T with parent Bparent if priority(B) < 0 comment: A path found min Compute Rs = q∈Lo kq − rs k then BEnd = (rs , Rs , B) Insert BEnd in T with parent B return (BEnd ) do Find n points pi on the boundary of B by random/uniform sampling. Delete pj ⊂ B ∈ (T ∪ Q) min kq − pi k Compute Ri = q∈L o comment: δOffset decides the min. bubble-size if Ri > (1 + δOffset )Rrobot p = kp − r k − R i s i then Insert B = (p , R , B)in Q with priority p i i i return (No Path Found)
This algorithm can be summarized as follows:- The search is started from the goal point and a tree is recursively created. The priority of each bubble-node represents 7
its distance from the start-point. The recursion continues till a bubble (Bl ) with a negative priority is found, which signifies that the start point is contained in that bubble. A final bubble BEnd with its center exactly at the start point rs is created with Bl as the parent and is inserted in the tree T . The solution S is then found to be the graph-path connecting BEnd to BRoot . 1.2.2
Handling Constraints During Time-trajectory Planning Once a geometrically-feasible path to the goal-point is found, one needs to
find a time-parameterization of this path, or a slight perturbation thereof, which is feasible for the vehicle. Graettinger et al [26] have categorized the constraints for ground vehicles as follows: (i) Path-constraints are limits on path-geometry like maximum curvature which are a function of wheel-base geometry and steering mechanism. (ii) Kinematic constraints involve only functions of velocities and accelerations. (iii) Dynamic constraints refer to force/torque inputs and frictional force limits. One way to handle the constraints is to use mathematical programming methods such as dynamic optimization to come up with a feasible trajectory. This may also be viewed as one computation step in Model Predictive Control (MPC) [24]. This may be viewed as a purely numerical procedure and has been in use in the planning and control of slow chemical processes [60], though it has begun to be applied to motion planning in recent times [102]. MPC will be revisited in Chapter 6 for comparison with the method of switched potentials presented in this work. An altogether different semi-analytical technique called time-scaling was first proposed by Hollerbach [34] for robotic manipulators. Given a geometric path in C-space, the basic idea is to assign a velocity profile to this path, such that the input bounds are not violated. A monotonically increasing time-warp function r(t) 8
is introduced with r(0) = 0. Given a configuration-space vector q, one denotes timedifferentiation with respect to time t and warped time r(t) as q˙ and q0 respectively. Then ¨ = q00 r˙ 2 + q0 r¨. q˙ = q0 r, ˙ q
(1.1)
A constructive procedure for determining a linear r(t) was presented in [34] for the case of a fully actuated manipulator. This problem was studied in the context of time-optimum control by [49]. A nonlinear warp function was utilized in [23] for a two-link manipulator. The case of an underactuated manipulator, with a second order nonholonomic constraint was solved using time-scaling in [4]. This approach was taken from the realm of robotic serial manipulators to that of conventionally steered ground vehicles in [26]. A quite similar technique, combined with fuzzy control, was employed by [3] to design a path-following controller using a kinematic controller, while keeping the speeds and accelerations bounded. 1.2.3
Time-trajectory Tracking Control Once time-parameterization and scaling of path is accomplished, a trajectory-
tracking controller is activated. There are several such controllers available in literature, and have been reviewed in [46, 32, 77, 31]. Sliding-mode controllers have been devised by [29] for holonomic robots and extended for nonholonomic robots by [109], among others. Feedback linearization techniques have been used in [91] and [20]. Asymptotic stability in trajectory-tracking using time-varying backstepping was demonstrated by [48], though the convergence was only to a neighborhood of the desired trajectory. Biologically inspired and neural-network based control strategies are presented in [110]. Such techniques suffer from chattering [109], unnatural oscillations [48], or singularities [32]. Many tracking control laws do not allow a regulation type control due to singularities.
9
1.3
Potential-Fields for Planning and Control of Mobile Robots Potential-field methods have the advantage that they achieve path-planning
and controller design simultaneously. In this approach, robot inputs in the form of actuator torques τ are generated by computing the gradient of a potential function U with respect to the configuration vector (q). τ (q) = −∇U (q).
(1.2)
The potential function is designed such that it keeps the robot away from obstacles while pointing the way to the goal point in the robot’s configuration space. This method was introduced by Khatib [39], who composed potentials as a superposition of attractive Uattr and repulsive Urep parts. Denoting the distance to goal as ρgoal and the minimum distance of the robot to an obstacle as ρ(q), potentials were devised in [39] as Uattr (q) = Urep (q) =
1 2 kρ , 2 goal ( 1 1 η ρ(q) − 2
1 ρ0
2
, if ρ(q) ≤ ρ0
(1.3)
0, otherwise. Earlier work suffered from the robot getting trapped in local minima of the potential-function where its gradient vanishes. [18] and [2] addressed this problem by using harmonic functions, i.e., functions satisfying the Laplace equation ∇2 U = 0. The panel method from fluid-mechanics was employed by [42]. A related work [108] used superquadratic potentials to shape the potential-contours near and away from arbitrary-shaped obstacles. An approach different from the use of harmonic potentials was pioneered by Rimon and Koditschek in [43, 84] who coined the term navigation function. Let F denote the path-connected free C-space for a mobile robot. Quoting [84], a navigation function, ϕ : F → [0, 1], encodes the task of moving the robot (with its dynamics accounted for) to a desired destination, qd , without hitting obstacles. To be called a navigation function, a potential has to have the following properties: 10
1. Smoothness on domain F. 2. A unique minimum at qd . 3. Admissibility on F, i.e., it should be uniformly maximal on the boundary of F. 4. Morse function property, i.e., all its critical points (where the gradient vanishes) are non-degenerate. This means that the Hessian is non-vanishing at the critical points. A constructive procedure for design of a navigation function for a omnidirectional, i.e. a holonomic or free-flying mobile robot, traversing a doughnut and star (convex deformed disk) world was given in [85, 84]. An example of a holonomic disk-shaped robot moving amongst disk-shaped obstacles was considered in [84]. The derivation of a navigation function in this case consists of the following steps 1. Let q denote the robot’s C-space vector, and let qd be the goal configuration. Define an attractive potential γκ (q) = kq − qd k2κ , κ > 0.
(1.4)
2. Let each obstacle be denoted by an obstacle function which is a real-valued map βi of the form obstaclei = {q : βi (q) ≤ 0},
(1.5)
where, βi vanishes exactly at the boundary of obstacle i. Let M be the total number of obstacles. One defines β(q) ≡
M Y i=1
11
βi (q).
(1.6)
3. The main result proven for this problem was that the function ϕκ (q) ≡
γκ 1/κ (q) (γκ (q) + β(q))1/κ
,
(1.7)
is a navigation function if the parameter κ exceeds a certain function of geometric data. This result is motivated by the observation that the undesired local minima of ϕκ (q) disappear as κ increases. Certain other shapes which are diffeomorphic to a disk were also considered in [84]. Some numerical navigation functions have been discussed in [55].
Note
that Algorithm 1.2.1 introduced in Sec. 1.2.1 also implicitly defines a numerical navigation-function, if one numbers the sequence of bubbles such that the goalbubble has an index 0, and the start-bubble has the maximum index. The bubbleindex is then a potential. Further details on this procedure may be found in [15]. Most of the current literature however focuses on holonomic robots. Potential theory applications for nonholonomic robots have been studied by Kyriakopoulos et al [50] where an obstacle free path was first found by classical methods and this path was time-parameterized. The resulting time-trajectory was then followed by a trajectory-tracking controller. In Tanner et al [103], a kinematic model of the unicycle along with a sliding-mode like controller was used to stabilize the system to the origin, which was the global minimum of a potential function. This approach suffers from chattering and also cannot be readily generalized to a dynamic model. Recently, Tanner et al [104] derived a strategy based on non-smooth Lyapunov functions and diffeomorphic transforms first introduced by [85]. Another approach, adopted by [93] in planning stage, was to locally deform the holonomic paths by adding a cost which favors directions easier for the nonholonomic system. Note that potential fields give smooth-state-feedback. It will be shown in the next section that nonholonomic systems have an inherent difficulty in smooth stabilization. This makes the above potential-fields based controllers inapplicable without some additional deliberations. 12
1.4
Stabilization of Nonholonomic Systems In this section, nonholonomy and controllability are defined, an important
stabilizability theorem is stated, and a literature review of various methods to circumvent the negative result of this theorem is done. Note that, as noted in [90] stabilization problem becomes less relevant when the mobile robot is not required to stop anywhere but continuously tracks a non-constant time-trajectory. 1.4.1
Defining Nonholonomy Let the robot configuration-space C be given in terms of a vector q ∈ Rn . A
time-invariant holonomic or geometric constraint can be written as: f (q) = 0. A time-invariant nonholonomic constraint can be written as g(q, q) ˙ = 0 : this latter constraint is non-integrable and cannot be written in the form of the former. Most practical nonholonomic constraints are linear in the rates q˙ [55] and can be written as: Ak×n (q)q˙ = 0, q ∈ C, k < n,
(1.8)
where A(q) is full-rank. This is called a differential 1-form. The rates q˙ then span an n − k dimensional linear subspace ∆(q) of the tangent space of C. ∆(q) can also be specified as S(q) which is the null-space matrix of A(q). There exist n − k C ∞ vectors-fields X1 , · · · Xn−k spanning the distribution ∆(q) [36]. Theorem 1.1 (Frobenius Theorem Restated [55]). Let ∆ be the n − k distribution on the n-dimensional manifold C associated with the 1-forms Ak×n (q). In the neighborhood of any q ∈ C, the following two conditions are equivalent: 1. The distribution ∆ is closed under the Lie-Bracket operation (i.e. is involutive). This means that if X, Y ∈ ∆ then [X, Y] ≡
∂Y X ∂q
−
∂X Y ∂q
∈ ∆.
2. The constraints Ak×n (q)q˙ = 0 are integrable and therefore holonomic.
13
If distribution ∆ is not closed under the Lie-Bracket operation, then some ¯ ∆, ¯ constraints are non-holonomic. The closure of distribution ∆ is denoted by ∆. sometimes also called Control Lie Algebra CLA(∆), is the smallest distribution which contains ∆ and is involutive. ¯ then n − k ≤ p ≤ n, by definition. The following cases arise: Let p ≡ dim(∆), h ≡ n−p k, h = 0, > 0, < k
All constraints holonomic/integrable All constraints non-holonomic
(1.9)
h constraints integrable. h coordinates eliminable.
Finally, the number of non-holonomic constraints d out of the total k are: d=k−n+p
(1.10)
The degree of nonholonomy of the robot is the minimal number of Lie bracket sufficient to express any vector in CLA(∆) [55]. Theorem 1.2. (Controllability) Any two configurations qi and qf of an open connected subset of C can be connected by a feasible path lying on this subset and fol¯ = n. lowing the flow of a finite sequence of vectors of ∆ iff dim(∆) This theorem combined with Eq. (1.9) shows that a purely nonholonomic system is controllable. Since a nonholonomic constraint puts constraints on the velocity and not the configuration, this is also intuitive. 1.4.2
Brockett’s Theorem and Consequences The key hurdle in implementing a smooth feedback controller for non-holonomic
systems using potential theory comes from the famous Brockett’s Theorem [16]. Theorem 1.3 (Brockett’s Theorem [16]). Let x˙ = f (x, u) be given with f (x0 , 0) = 0 and f (., .) continuously differentiable in a neighborhood of (x0 , 0). x ∈ D ⊂ Rn , u ∈ Rm . A necessary condition for the existence of a continuously differentiable control law which makes (x0 , 0) asymptotically stable is that: 14
1. The linearized system should have no uncontrollable modes associated with eigenvalues whose real-part is positive. 2. There exists a neighborhood N of (x0 , 0) such that for each ξ ∈ N there exists a control uξ (.) defined on [0, ∞) such that this control steers the solution of x˙ = f (x, uξ ) from x(0) = 0 to x(∞) = x0 . 3. The mapping γ : D × Rm → Rn defined by γ : (x, u) → f (x, u) should be onto an open-set containing 0. P Corollary 1.1 ([16]). For an input-affine system of the form x˙ = f (x)+ m i=1 gi (x)ui , the third condition of theorem 1.3 implies that the stabilization problem cannot have a solution if there exists a smooth distribution D containing f and gi with dim D < n. Corollary 1.2 ([16]). A driftless input-affine system of form x˙ =
Pm
i=1
gi (x)ui ,
with gi linearly independent at x0 , is stabilizable iff m = n, i.e., the system is fully actuated. The case where the set {gi (x)} drops dimension exactly at x0 is excluded from this restriction. For many nonholonomic systems, Corollary 1.2 applies. Therefore, this precludes stabilization to the origin of the configuration space of nonholonomic systems by smooth state feedback. In the following, two predominant approaches to circumventing this limitation are described. 1.4.3
Time-varying Smooth Control Theorem 1.3 applies only to autonomous systems, i.e., when no explicit de-
pendency on time is present. This fact has been used by Samson [90, 89] to design time-varying control laws explicitly dependent on time which stabilize nonholonomic systems of the form x˙ 1 = u1 (1 + a(x1 , x3 )x2 ), 15
x˙ 2 = u2 , x˙ 3 = c(x1 , x3 )u1 x2 ,
(1.11)
where dim(x1 ) = dim(x2 ) = dim(u1 ) = dim(u2 ) = 1, dim(x3 ) = m. A three wheeled vehicle model with a steering can be converted to the above structure by a coordinate-transform. A control-law is then designed with explicit time-dependence to asymptotically stabilize the vehicle to the origin of its C-space. However, under this control the vehicle has a complicated back and forth motion which seems unnatural. A time-varying controller for a more general system structure was presented in [78]. A different time-varying control-law has recently been used by [105] to achieve exponential stability. Their design is however mathematically quite involved, while providing little physical insight. 1.4.4
Discontinuous, Piecewise Continuous and Hybrid Control This has been the most popular approach for stabilization of nonholonomic
systems. It involves either a discontinuous coordinate-transform, or a controller switch, i.e., a piecewise continuous control. The controller switching may be based on a stabilizing logic, in which case it would be categorized as hybrid control. Hybrid control stability analysis tools have been discussed by Branicky [13, 14]. A popular kinematic model for a differentially driven wheeled mobile robot (WMR) is cos(θ) 0 x˙ y˙ = sin(θ) 0 ˙θ 0 1
v
! ,
(1.12)
ω
where, (x, y) are the center of mass coordinates, and θ is the orientation of the vehicle. The forward heading speed v and turning-speed ω are the two inputs. This model is also known as unicycle model and knife-edge model. This model has been used for exponential stabilization by [21, 98] using a piecewise smooth controller. This was done by making the vehicle converge to a 16
circular arc passing through the origin such that the circle’s tangent is aligned with the specified final vehicle orientation θ = 0. The strategy of sequentially stabilizing to a set of manifolds by discontinuous control for systems in power form has been studied in [40]. Discontinuous polar coordinate transformations for controller design were proposed by Astolfi [5, 7, 6]. This was combined with an adaptive scheme for regulation in the presence of parametric model uncertainty by Aguiar et al [1]. Another discontinuous controller based on a nonsmooth dipolar inverse Lyapunov function for articulated nonholonomic vehicles was presented in [104]. The control strategy proposed in this dissertation may also be categorized as hybrid and piecewise continuous. 1.5
Control of Underactuated Systems A system is called underactuated when the dimension of the configuration-
space, i.e. the number of degrees-of-freedom, is more than the number of inputs available to control the system. This, however, does not necessarily affect controllability. An overview of control methodologies for underactuated systems is presented in [100], where systems with Lagrangian dynamics are considered and a similarity to nonholonomic constraints is pointed out. An underactuated system’s state can be suitably partitioned as qT = (qT1 , qT2 ), such that the dynamic-equations can be written as [100, 83]: ¨ 1 + D12 q ¨ 2 + h1 (q1 , q2 , q˙ 1 , q˙ 2 ) + φ1 (q1 , q2 ) = 0, D11 q
(1.13)
¨ 1 + D22 q ¨ 2 + h2 (q1 , q2 , q˙ 1 , q˙ 2 ) + φ2 (q1 , q2 ) = B(q1 , q2 )τ, D21 q
(1.14)
where, q1 ∈ Rn−m , q2 ∈ Rm , and τ ∈ Rm , and B(q1 , q2 ) ∈ Rm×m is invertible. After a simple partial feedback linearization [83], the system of Eqs. (1.13) and (1.14) may be rewritten as ¨ 1 = J(q)¨ ˙ q q2 + R(q, q),
(1.15) 17
¨ 2 = v, where, q
(1.16)
−1 −1 ˙ + φ1 (q)) , ˙ = −D11 (h1 (q, q) D12 , R(q, q) J(q) ≡ −D11 −1 −1 −1 v ≡ D22 − D21 D11 D12 Bτ − h2 − φ2 + D21 D11 (h1 + φ1 ) .
The dynamics given by Eq. (1.15) can be considered as a constraint on generalized accelerations. For many important underactuated vehicles, this constraint is completely non-integrable and hence fully second-order nonholonomic. For such second-order fully nonholonomic systems, or in other words, for most underactuated systems, a result analogous to Theorem 1.2 can be given. First of all, one needs to apply a reformulated version of the Frobenius Theorem (Theorem 1.1) to prove non-integrability of Eq. (1.15). This procedure has been outlined in [83]. Thereafter, the following result may be stated: Theorem 1.4 (Strong Accessibility [83]). Let n−m ≥ 1. Then the system defined by Eqs. (1.15) and (1.16) is strongly accessible [66], if Eq. (1.15) is fully non-integrable. This shows that nonholonomic acceleration constraints do not reduce the dimension of the state-space. Brockett’s theorem (Theorem 1.3) may also be applied to this system, which may preclude stabilization by smooth time-invariant statefeedback. However, for many practical underactuated systems, the assumptions of this theorem are not satisfied, and smooth state-feedback stabilization is indeed possible [83]. Some of these state-feedback stabilizable systems are in the lowertriangular form ξ˙0 = f (ξ0 ) + ψ(ξ0 , ξ1 )ξ1 ξ˙1 = a1 (ξ1 , ξ2 ) ξ˙2 = a2 (ξ1 , ξ2 , ξ3 ) .. . ξ˙n = an (ξ1 , · · · , ξn , u), 18
(1.17)
where, u is the input, and ξ = [ξ0T , · · · , ξnT ] is a state-space-partition. Such systems are amenable to control design based on backstepping and passivity based techniques [94, 101]. For upper-triangular systems, the method of forwarding has also been presented in [94]. Discontinuous feedback control for a special class of a single unactuated degree of freedom has been presented in [82]. Further work on passivity based control combined with energy-shaping is presented in [69]. Time separation between the response of various states is another technique for designing controllers for underactuated systems. This time-separation is usually inspired by the physics of the problem and enforced by the controller. Note that backstepping also implicitly defines a time-separation. Many systems wherein backstepping is not applicable, may still be stabilized using arguments of Singular Perturbation Theory [44, 45, 38]. The pertinent stability result has been stated in the following theorem. Theorem 1.5 (Exponential Stability [38]). Consider the singularly perturbed system x˙ = f (t, x, z, ),
(1.18)
z˙ = g(t, x, z, ).
(1.19)
Assume that the following assumptions are satisfied ∀(t, x, ) ∈ [0, ∞) × Br × [0, 0 ], where, Br ≡ {x : kxk ≤ r} • f (t, 0, 0, ) = 0, and g(t, 0, 0, ) = 0. • The equation 0 = g(t, x, z, 0) has an isolated-root z = h(t, x) such that h(t, 0) = 0. • The functions f , g, h and their partial-derivatives upto second-order are bounded for z − h(t, x) ∈ Bρ . • The origin of the reduced system x˙ = f (t, x, h(t, x), 0), 19
(1.20)
is exponentially stable. • Let y ≡ z − h(t, x). The origin of the boundary-layer system dy = g(t, x, y + h(t, x), 0), τ = t/, dτ
(1.21)
is exponentially stable uniformly in (t, x). Then, there exists ∗ > 0 such that ∀ < ∗ , the origin of Eqs. (1.18) and (1.19) is exponentially stable. This result has been combined with switched-potentials for planning and control of certain underactuated vehicles in this work. In particular, the author has designed control-schemes for an underactuated inverted wheeled pendulum [75, 76] and for a hover-mode helicopter [73]. A discussion of the literature corresponding to these specific examples is presented in Chapters 4 and 5 respectively. 1.6
Primary Research Questions and Scope of the Dissertation In view of this summary of various prevalent planning and control method-
ologies for nonholonomic and underactuated vehicles, the author has identified the following primary objectives and research questions for this dissertation. 1. What are the main issues and difficulties in the application of potential-fields to nonholonomic vehicles? If such a vehicle is driven by a potential-field, how are its invariant manifolds analytically characterized? 2. For underactuated vehicles, can the idea of potential-fields be amalgamated with certain other techniques for control design? In particular, can a potentialfields based control be applied to a substate by a suitable time-scale based partition of the state-space? 3. How to design bounded potential-fields which 20
(a) restrict the vehicle motion to a prespecified obstacle-free region (a bubble), given arbitrary initial conditions? (b) can be continuously or discretely moved to cause the vehicle to follow an obstacle-free geometric path? In the case of discrete movement, how can one design a switching-scheme which i. guarantees stability while producing smooth motion? ii. automatically satisfies specified bounds on input forces/torques and/or speeds? iii. could be modified on the fly to accommodate new obstacles detected by sensors during motion? iv. obviates any explicit time parameterization and time scaling of the path, automatically speeding-up when the path is geometrically simple, and slowing down during sharp curves? 4. How can one encapsulate the complicated vehicle dynamics for use by a higherlevel planner which works with a simpler omnidirectional abstraction of the vehicle? This planner, for example, may be interested in designing group behavior for heterogenous vehicles. 5. Is the approach generalizable from planar to spatial, or to a higher dimensional C-space? The focus of this dissertation is restricted to nonlinear vehicle models coming from an Euler-Lagrange formulation. For planar vehicles, terrain effects are not considered. Further slipping is assumed not to occur, because slipping violates the nonholonomic constraints of the model. A representative set of ground and aerial vehicles is taken to illustrate the principles, but due to vast variety of nonlinear
21
models possible, a generalization cannot be made. However, classes of dynamic systems possessing certain structural properties, to which this approach is applicable, are pointed out. 1.7
Overview of Subsequent Chapters The rest of this dissertation is organized as follows: Chapter 2 gives a gen-
eral formulation of the method of switched potentials for certain classes of nonholonomic and underactuated systems. Control law derivation, input/speed bounds satisfaction, and stable switching strategies are discussed. The principal features of the approach are illustrated by a planar differentially driven unicycle model in Chapter 3. Suitable local potentials are designed, their effect on vehicle motion characterized and discrete switching schemes are designed. Chapter 4 adds underactuation to planar nonholonomy and presents the example of an inverted wheeled pendulum. Both basic physical modeling, as well as controller design are covered. This chapter also elucidates the role of time-scale separation as a feasible companion to the application of potential-fields to underactuated vehicles. Chapter 5 presents the application of the framework to an underactuated aerial vehicle, namely, a helicopter. The attitude of the helicopter is controlled at a faster time-scale whereas, the slower spatial motion is controlled by a switched local potential. The similarity of the switching-scheme to that derived in Chapter 3 is highlighted. A time-based analog of the space-based approach otherwise followed in this work is presented in Chapter 6. This approach is a modification of Model Predictive Control (MPC) for underactuated vehicles. It brings out certain limitations of time-based planning and control in real-time scenarios, which the space-based approach does not have. Chapter 7 summarizes the conclusions and outlines the major contributions of the work.
22
Chapter 2 FORMULATION OF THE SWITCHED POTENTIALS FRAMEWORK
2.1
Introduction The aim of this chapter is to summarize the salient ideas involved in the
switched potentials framework developed in this work. Sec. 2.2 derives the general dynamical equations for nonholonomic systems using the classical Lagrangian approach. For such systems, the basic steps involved in designing control laws based on switched potentials are given in Sec. 2.3. Detailed worked-out examples of the framework, as applied to nonholonomic systems, are given in Chapters 3 and 4. In Sec. 2.4, a class of underactuated (UA) dynamical systems is identified, to which the idea of switched potentials may be readily extended. Detailed worked-out examples of such underactuated systems are given in Chapters 4 and 5. As noted in Chapter 1, many nonholonomic systems are underactuated and underactuation may be considered as second order nonholonomy also. This is reflected within the choice of examples studied in this work. The categorization of examples is graphically depicted in Fig. 2.1. Note that the categories of underactuation and nonholonomy may technically be considered to subsume each other. However, within this work, we consider a nonholonomic system to be underactuated only if the number of inputs is less than the number of independent configuration-space time-rates.
23
Nonholonomic Systems
Unicycle, Tricycle
Inverted Wheeled Pendulum Helicopter
Underactuated Systems ( II Order NH ) Figure 2.1: Categorization of the examples studied in this work. 2.2
Classical Formulation of Nonholonomic Dynamics This section briefly reviews the way nonholonomic constraints are handled
in the classical Lagrangian formulation. Let us start with a set of nonholonomic constraints given by Eq. (1.8). Let all of them be nonholonomic, as checked by Theorem 1.1. Then the Lagrangian dynamic equations are derived as follows [28, 54]. One can multiply each of the k equations in Eq. (1.8) by the corresponding components of a Lagrange multiplier vector λ ≡ [λi ], i = 1 . . . k. Let Λ = diag(λ). Then the variation δq of the configuration vector q satisfies Λk×k Ak×n (q) δq = 0, k < n.
(2.1)
Since the generalized constraint force vector G does not produce virtual work, one gets GT δq = 0.
(2.2)
Subtracting the sum of of Eqs. (2.1) from Eq. (2.2), one gets G − AT λ δq = 0.
24
(2.3)
Now one chooses λ such that AT λ = G. This is possible as A is full-rank. By the usual Lagrange-multiplier argument, this now allows us to consider δq as free variations. The constraint forces G may now be considered as part of the generalized forces, which gives the Lagrange’s dynamic equations as d ∂L ∂L − = AT λ + E(q)τ, dt ∂ q˙ ∂q
(2.4)
˙ is the system’s Lagrangian, En×m is the input transformation matrix, where, L(q, q) and τ ∈ Rm is the system input vector. On expansion, this gives ˙ q˙ = AT λ + E(q)τ, M (q)¨ q + C(q, q)
(2.5)
˙ q˙ is a vector of position and velocity where Mn×n (q) is the inertia-matrix, and C(q, q) dependent forces. As shown in [97], conservation of energy in a Lagrangian system implies ˙ is skew-symmetric, ∴ zT (M˙ − 2C)z = 0, ∀z ∈ Rn . M˙ (q) − 2C(q, q)
(2.6)
Let Sn×(n−k) (q) be the null-space matrix of Ak×n (q). Due to Eq. (1.8), the rates vector q˙ lies in the null-space of A(q). Hence, ∃ ν(t) ∈ Rn−k such that q˙ = S(q)ν(t).
(2.7)
This represents the kinematic equations for the system. An example of this is the unicycle kinematics Eq. (1.12). One can pre-multiply Eq. (2.5) by S T (q) to eliminate λ by noting that A(q)S(q) = 0. In the resulting equation, one substitutes ˙ ¨ = S(q)ν q + S(q)ν, ˙ to get an equation of the following general form [91] ˙ + S T C(q, ν)Sν = S T Eτ. S T M S ν˙ + S T M Sν
(2.8)
To write Eqs. (2.7) and (2.8) in state-space form, one defines a state x = [qT , ν T ]T , and obtains !
Sν x˙ =
˙ + S T C(q, ν)Sν) −(S T M S)−1 (S T M Sν 25
0 +
(S T M S)−1 S T E
! τ.
(2.9)
Within this work we first focus on systems wherein m ≥ n − k,
(2.10)
i.e., the lower-part of Eq. (2.9) is fully or over-actuated. When this condition is not satisfied, the system is fundamentally underactuated and certain other techniques need to be used, as described in Sec. 2.4. An example of the above Lagrangian formulation is the differentially driven two-wheeled vehicle. This vehicle, which is often referred to as a unicycle in the literature, will be discussed in detail in Sec. 3.2. To illustrate the above procedure, the dynamics of a back-driven tricycle is derived in the following section. 2.2.1
Tricycle Model A back-driven tricycle model, shown in Fig. 2.2, is usually considered a simpli-
fication of a four-wheeled car-like vehicle with a rear-wheel drive [111]. A differential arrangement divides the input torque τm equally to the two rear wheels. The second input is the steering torque τs . Define system input vector τ = [τs , τm ]T . The configuration-space of the vehicle is defined by q = [x, y, θ, β]T , as shown in Fig. 2.2. The kinematics corresponding cos(θ) 0 sin(θ) 0 q˙ = S(q)ν ≡ tan(β) 0 ` 0
(2.11) to Eq. (2.7) may be written as
v β˙
! .
(2.12)
1
Let mp denote the mass of the vehicle frame without the wheels, and Ip its moment of inertia about C with respect to an axis zˆ perpendicular to the plane. Let Iw be the moment of inertia of each wheel about its axis, and Id be the moment of inertia of each wheel about a diameter. Let mw denote the mass of each wheel. The 26
β
v
OIC S C θ b O(x, y)
r
OS = ` OC = d
Figure 2.2: Tricycle schematic. The center of mass of the frame, excluding the wheels, is at C. OIC is the instantaneous center of rotation of the vehicle. All wheels are of radius r. The distance from O(x, y) to the center of each back-wheel is b. The forward heading speed of point O is v. rotation angles of the two back left and right wheels, and the front wheel, are denoted as ϕl , ϕr , and ϕf respectively. The angles are directed such that any rotation to move the vehicle forward at zero steering (β = 0) is positive. The following relations are easily derived: v = x˙ cos(θ) + y˙ sin(θ), v 2 = x˙ 2 + y˙ 2 ,
(2.13)
˙ rϕ˙ l = v − θb, ˙ rϕ˙ r = v + θb, (rϕ˙ f )2 = v 2 + θ˙2 `2 .
(2.14)
The Kinetic-energy KEp of the vehicle frame without the wheels is 2KEp = mp (v 2 + θ˙2 d2 ) + Ip θ˙2 .
27
(2.15)
The Kinetic-energy KEl of the left rear wheel is 2KEl = (mw +
Iw ˙ 2 + Id θ˙2 . )(v − θb) 2 r
(2.16)
The Kinetic-energy KEr of the right rear wheel is 2KEr = (mw +
Iw ˙ 2 + Id θ˙2 . )(v + θb) r2
(2.17)
Finally, the Kinetic-energy KEf of the front steering wheel is 2KEf = (mw +
Iw 2 ˙2 2 ˙ 2. )(v + θ ` ) + Id (θ˙ + β) r2
(2.18)
In planar motion, the Lagrangian of the system can be given by ˙ = KEp + KEl + KEr + KEf , L(q, q)
(2.19)
where, Eqs. (2.13) are used to replace v everywhere. L may then be substituted in Eq. (2.4) to derive the dynamics. To derive the input transformation matrix E(q), one needs to compute virtual work δW done by the input τ . δW = τs δβ +
τm (δϕl + δϕr ). 2
(2.20)
On using relations given by Eqs. (2.14), one obtains δW = δqT E(q)τ, 0 cos(θ) r 0 sin(θ) r E(q) ≡ . 0 0 1
(2.21)
0
This completes the derivation of the dynamics of a back-driven tricycle. 2.3
Switched Potentials for Nonholonomic Vehicles Within this section we assume that we are given a robotic vehicle whose
dynamics are described by Eq. (2.9), such that m ≥ n − k. The objectives of this section are the following 28
1
1
1
1 1
UΣ (q)
1
1
1 Robot
1
1
1
1 1
UΣ (q)
1 1 Switch? 1
Robot Bubble
1
1
E
E
1
1
1
1
1
1
1
1 1
1 1
1
1
1 1 1
1
1
1
1
1 1
New Prospective Bubble
1 1
(a) Defining a bounded potential within an obstacle-free region (a bubble). E is the invariant-manifold where the robot stops.
(b) Switching the Bubble.
Figure 2.3: Controller Design Based on Switched Potentials. 1. One would like to mathematically describe the behavior of this system when control inputs are derived from a local bounded potential. Fig. 2.3(a) depicts a bounded potential UΣ (q) defined within an obstacle-free region, called a bubble. It will be shown in Sec. 2.3.1 that on using a potential gradient based control, the system converges to, i.e., stops at, a manifold E inside this bubble. 2. Control parameters need to be constructively selected such that certain speed and input bounds are satisfied during motion. This is addressed in Sec. 2.3.2. 3. To produce gross motion, we adopt the strategy of successively switching the mobile robot’s current bubble. This is depicted in Fig. 2.3(b). However, one needs to settle the following issue: Under what conditions can the current bubble be stably switched, while maintaining a smooth motion. This issue is addressed in Sec. 2.3.3.
29
2.3.1
Local Bounded Potential based Control Law Derivation
Proposition 2.1. Define a local potential field UΣ (q) inside an obstacle-free region (a bubble) surrounding the robot, as shown in Fig. 2.3(a). UΣ (q) is assumed to be C 1 , and maps q → [0, 1) within the bubble. UΣ tends to its maximum value of unity at the bubble’s boundary and achieves its minimum value of 0 within the bubble. At time t = 0, let the robot be located inside the bubble at coordinates q0 , so that UΣ (q0 ) < 1. Let the initial speeds be given by the vector ν0 . A control law of the form ∂UΣ T T S(q) E(q) τ = −Kp S (q) − Kd ν , ∂q
(2.22)
with scalars Kp , Kd > 0, will cause the robot to converge to a manifold defined by ∂UΣ T E = q : S (q) = 0 , ν = 0, (2.23) ∂q without touching the boundary of the bubble, if Kp is selected initially such that 1 Kp (1 − UΣ (q0 )) > ν0 T S T M Sν0 . 2
(2.24)
Note that it is always possible to select such a value of Kp , if high enough torques are available. This torque-limits issue is dealt with after the proof. Proof. First we note that Eq. (2.22) can always be solved for τ because we have assumed that m ≥ n − k. In most practical examples the equality holds and hence S(q)T E(q is a full-rank square matrix. The kinetic energy of the robot is 1 1 UKE (ν) ≡ q˙ T M (q)q˙ ≡ ν T S T (q)M (q)S(q)ν 2 2
(2.25)
Consider the candidate Lyapunov function: V (q, ν) = Kp UΣ (q) + UKE (ν).
(2.26)
On differentiating V with respect to time we get 1 T T ˙ 1 T ˙T T T ∂UΣ T T T ˙ ˙ V = Kp ν S + ν S M S ν˙ + ν S M S + S M S ν + ν S M S ν. ∂q 2 2 30
One can now substitute the system dynamics from Eq. (2.9). This results in the following equation ∂UΣ T Kp S + S Eτ = ν ∂q 1 T ˙T 1 T T ˙ T ˙ + ν S M S − S M S ν + ν S M − 2C Sν. (2.27) 2 2 The last two terms of the above equation vanish. This is because S˙ T M S − S T M S˙ ˙ and M − 2C are skew-symmetric: the former result is due to the fact that T T ˙ S MS = S T M T S˙ = S T M S˙ and the latter result is from Eq. (2.6). On substiV˙
T
T
tution of the proposed control law Eq. (2.22) in the above, one gets V˙ = −ν T Kd ν ≤ 0.
(2.28)
Applying LaSalle’s Invariant Set Theorem, the system would converge to the invariant set ν = ν˙ = τ = 0, which on substitution in the control law Eq. (2.22) gives the required invariant set of Eq. (2.23). Note that the given control law makes the Lyapunov function non-increasing. Also sup(UΣ (p)) = 1, which happens only at the bubble-boundary. Therefore, the robot having started inside, will stay inside ∀t > 0 if 1 Kp > Kp UΣ (p0 ) + ν0 T S T M Sν0 . 2
(2.29)
This is the same as Eq. (2.24). Intuitively, if the above equation had an equality, it would mean that the robot could touch the bubble-boundary but only with 0 velocity. Remark 2.1 (Constraining Potential). To make sure that the potential UΣ (q) is constrained to [0, 1), we make use of the analytical switch defined in [85]. Given a potential U∞ (q) with a range [0, ∞), the action of the switch is given by ∆
hλ (x) =
x , λ > 0. λ+x 31
(2.30)
Therefore, UΣ (q) = hλ ◦ U∞ (q),
λ ∂U∞ ∂UΣ = . 2 ∂q (λ + U∞ ) ∂q
(2.31)
Hence, the zeros and the critical points of U∞ (p) are preserved under this transformation. Combining Eqs. (2.23) and (2.31), one concludes that Eq. (2.23) can also be rewritten with UΣ replaced by U∞ . On making this substitution one gets ∂U∞ T E = p : S (q) =0 ∂q 2.3.2
(2.32)
Handling Input and Speed Constraints As noted earlier, feasibility of Kp is subject to availability of high enough
torques. In the sequel, explicit feasibility bounds are derived. Let the speed-limits max for the robot be given in terms of the maximum allowable kinetic-energy UKE .
Furthermore, the Lyapunov function in Eq. (2.26), can be written as V = Kp UΣ (q)+ UKE (ν). Since the Lyapunov function is non-increasing, a sufficient condition for max is UKE to not exceed UKE max Kp UΣ (q0 ) + UKE (ν0 ) < UKE .
(2.33)
max This presumes naturally that UKE (ν0 ) < UKE . Eq. (2.24) gave a lower-bound for
Kp , whereas Eq. (2.33) gives an upper-bound. These two equations have to be consistent. They can be combined to give an easily verifiable sufficient feasibility condition for Kp as follows UKE (ν0 ) max < UKE . 1 − UΣ (q0 )
(2.34)
If this condition is met, a Kp can be found which will keep the robot kinetic energy within required bounds for the given initial conditions. Note that if the robot starts from rest, the above condition is always satisfied as UKE (ν0 ) = 0.
32
A similar strategy can be used to check for torque constraints. Let the −1 T max ˆ maximum torque vector 2−norm be given as kτ k2 . Defining B = S(q) E , Eq. (2.22) can be rewritten in terms of U∞ as Kp λ ∂U∞ T τ = −B + Kd ν , S (q) (λ + U∞ )2 ∂p
(2.35)
Let kBk2 be the spectral-norm of matrix B [35]. On using the definition of spectralnorm and the triangle-inequality, one gets
T
Kp λ ∂U ∞
S (q)
+ Kd kνk2 . kτ k2 ≤ kBk2 (λ + U∞ )2 ∂q 2
(2.36)
Therefore, for the maximum-torque constraint to be satisfied, a sufficient condition is kτ kmax 2
Kp λkBk2 ≥ (λ + U∞ )2
T
S (q) ∂U∞ + Kd kνk2
∂q 2
(2.37)
Due to the non-increasing nature of the Lyapunov function, one has Kp (UΣ (q0 ) − UΣ (q)) + UKE (ν0 ) ≥ UKE (ν) > µkνk22 ,
(2.38)
where 2 µ is the minimum eigenvalue of the positive-definite matrix S T M S . This can be incorporated in Eq. (2.37) to give a more conservative sufficient condition as
∂U∞ Kp λkBk2 T max
S (q) kτ k2 ≥ (λ + U∞ )2 ∂q 2 q Kd +√ Kp (UΣ (q0 ) − UΣ (q)) + UKE (ν0 ), (2.39) µ Let kBkm 2 = maxq kBk2 . This maximization can be done numerically for the vector q lying within the bubble. In many cases, B is a constant matrix, and hence this step is simplified. A sufficient condition for Eq. (2.39) to be always satisfied within a bubble is kτ kmax Kd 2 ≥ max Kp χ1 (q) + √ χ2 (q) , q kBkm µ 2
T
λ ∂U 4 ∞
S (q)
, χ1 (q) = (λ + U∞ )2 ∂q 2 q 4 χ2 (q) = Kp (UΣ (q0 ) − UΣ (q)) + UKE (ν0 ). 33
(2.40) (2.41) (2.42)
On using Eq. (2.42) and Eq. (2.24), one gets an upper-bound on χ2 (q) as 4
χ¯2 (q) =
p p (1 − UΣ (q)), χ2 (q) ≤ Kp χ¯2 (q).
(2.43)
This can be now used to modify Eq. (2.40), to get a more conservative sufficient condition, which is independent of initial configuration, and hence easier to compute. " # p max K K kτ k2 d p ≥ max Kp χ1 (q) + √ χ¯2 (q) . (2.44) m q kBk2 µ To factor out Kp , the damping gain Kd is chosen as Kd = α ˆ
p
Kp , where 0 < α ˆ≤α ˆm.
With this substitution we finally get an upper-bound on Kp in terms of the specified maximum motor-torque: α ˆm kτ kmax 4 2 ≥ Kp χm , χm = max χ1 (p) + √ χ¯2 (q) . q kBkm µ 2
(2.45)
Note that the domain of maximization is compact and given by the bubble interior and the boundary. Therefore a unique maximum can be found numerically. Since χm is independent of initial conditions, it can be computed initially once and for all. The inequality given in Eq. (2.45) can be combined with the lower-bound given by Eq. (2.24), to get the following sufficient feasibility condition for Kp UKE (ν0 ) kτ kmax 2 > χm . m kBk2 1 − UΣ (q0 )
(2.46)
This equation shows clearly that if the system starts from rest, a feasible Kp can always be found. Remark 2.2 (Initializing the Framework). In the sequel, it is assumed that before the robot enters the first bubble of the planning/control framework, its speed is slow enough to satisfy Eq. (2.46), and additionally Eq. (2.34) if required. If these conditions are not met, brakes are applied till the speeds are reduced to the point where the feasibility conditions are met. Once the robot enters its first bubble, subsequent feasibility is maintained by the switching algorithm. 34
Remark 2.3 (Designing U∞ ). Note that the invariant manifold given in Eq. (2.23) depends on the matrix S(q) which is system-specific. Chapters 3 and 4 will illustrate the design of the potential U∞ for two nonholonomic systems. 2.3.3
Bubble Switching Condition The manifold E, where the mobile robot stops in the absence of any switching,
was derived in Eq. (2.23). To cause gross motion along a path, however, we adopt the strategy of switching the robot’s bubble before it stops at E within its current bubble. Intuitively, this causes the robot to follow a given geometric path, while staying within an obstacle-free ‘tunnel’ at all times. It also automatically time-parameterizes and scales the path such that input/speed constraints are satisfied. Such an obstaclefree tunnel, in terms of a string of bubbles, is obtained from Algorithm 1.2.1, already described in Sec. 1.2.1. Many different switching-conditions may be tried. The most obvious is just to let the robot converge to E of its current bubble, and then switch it to a new bubble which contains the robot. This would lead to an undesirable non-smooth stop and go motion. This problem can be alleviated if we use an energy-based switching. This may be described as follows. At any instant of time, assume that the robot is considering a prospective bubble Bp to switch to. Were the robot to switch, it would have local initial configuration-space coordinates q0 , and velocity ν0 . Then Bp is feasible to switch to, only if: (i) UΣ (q0 ) < 1, i.e., Bp is geometrically feasible and encloses the robot. (ii) kν0 k is low enough such that the feasibility conditions given in Eqs. (2.34) and (2.46) are satisfied. This means that a feasible value of gain Kp can be found which keeps the kinetic-energy and torques within bounds. Note that the aforementioned conditions are always satisfied at kν0 k = 0. Since, in the absence of switching, the robot’s velocity is tending to zero, there would always 35
be a point of time when kν0 k is low enough for a new geometrically feasible bubble to become feasible. The geometric construction of new bubbles Bp is dependent on the global plan returned by Algorithm 1.2.1. This will be explained by example in Chapter 3. We have thus come up with a stable way of maneuvering a nonholonomic mobile robot around obstacles, while satisfying input/speed constraints at all times. 2.4
Switched Potentials for a Class of Underactuated Vehicles Recall that the controller given in Eq. (2.22) works only when m ≥ n−k, i.e.,
the number of inputs is at least equal to the number of degrees of freedom in the velocity-space. When this condition is not satisfied, or when the system structure of an underactuated system is not given by Eq. (2.9), the switched potentials framework needs to be modified. The main idea in this section is that for underactuated systems, potentialtheory needs to be teamed with a state time-scale separation to achieve system stability. The idea of state time-scale separation comes from Singular Perturbation Theory as described in Sec. 1.5. In this section, a general system structure is introduced which is able to describe certain important dynamically balanced mobile robots. It is shown how potential theory based control design may still be done for such systems. 2.4.1
Canonical Form of System Dynamics Consider a dynamical system whose configuration space y ∈ RNs +Nf can be
partitioned in the following way y ≡
yf ys
! , yf ∈ RNf , ys ∈ RNs ,
36
(2.47)
y1 . . ≡ . , i = 1 . . . Nf , yi ∈ R. y Nf
yf
(2.48)
where, the subscripts ‘s’ and ‘f’ stand for slow and fast respectively. Whether a configuration variable is slow or fast is either decided by the nature of the problem, or enforced by a controller. The system dynamics is assumed to be of the following triangular form (ri )
yi
= gi (yf , u), i = 1 . . . Nf , u ∈ Rm ,
y˙ s = gs (ys , yf , u).
(2.49) (2.50)
Usually, ri ≤ 2. The system may inherently be in this form, or a coordinatetransform along with a feedback linearization may be required to get to this form. The system is assumed to be controllable and is such that (Nf + Ns ) > m ≥ Nf ≥ Ns
(2.51)
Under these assumptions, the subsystem yf , given by Eq. (2.49), is fully or over actuated. This subsystem is assumed to be exponentially stabilizable to a set-point ˆ f by input u. When yf stabilizes to y ˆ f , let us denote the steady-state input u as y ˆ . Usually, u ˆ ≡ 0. u The control objective is to asymptotically take the subsystem ys to 0, while keeping the yf system stable. The main idea consists of using a local potential-field ˆ f as a pseudo-input for stabilization of subsystem ys given in Eq. (2.50), by using y to make up for its input deficiency. This idea is illustrated in Fig. 2.4. The slow ˆ f = γs (ys ) which subsystem uses a potential-field UΣ (ys ) to design a feedback law y ˆ f for the fast subsystem. This set-point is then exponentially generates a set-point y ˆ f ). This whole tracked by the fast subsystem using feedback law u = γf (yf − y procedure is justified by using Theorem 1.5, wherein the parameter may be identified as the reciprocal of the high proportional gain required for the fast subsystem controller. 37
Controller
Slow Controller ˆ f = γs(ys ) y Potential UΣ (ys)
ˆf y
Fast Controller ˆf ) u = γf (yf − y
u
System yf , ys
yf
ys
Feedback
Figure 2.4: Control flow for the two time-scale scheme. Two examples of dynamically balanced underactuated systems with structures representative of Eqs. 2.49 and 2.50 are the Inverted Wheeled Pendulum [75] and the Helicopter [73]. These are studied in Chapters 4 and 5 respectively. In such vehicles, the attitude usually constitutes the fast substate yf and the translation of the center of mass constitutes the slow substate ys . 2.5
Conclusions This chapter provides the details of the over-arching framework developed
in this work. The main idea is to design a potential-based stabilizing controller which guarantees constricted vehicle movement within an obstacle-free region called a bubble. Subsequently, this enclosing bubble is switched according to a global plan such that the autonomous mobile robot continuously moves along an obstacle-free ‘tunnel’. The switching logic simultaneously guarantees input and speed bounds satisfaction. Nonlinear mechanical mobile systems exhibit quite a rich variety of dynamics, and a control methodology which is always applicable is not possible. However, within this work, two important classes of nonholonomic and underactuated systems have been identified, to which the framework is applicable. Chapters 3, 4, and 5 provide detailed applications of this framework to certain vehicles of practical importance. 38
Chapter 3 SWITCHED POTENTIALS FOR NONHOLONOMY IN PLANAR MOTION
3.1
Introduction In this chapter, navigation and control of an autonomous planar mobile unicy-
cle robot in an obstacle-ridden environment is considered. This chapter specializes the general framework studied in Chapter 2 to the case of a unicycle robot. The unicycle dynamic model used has two differentially driven wheels, with the motortorques as the system input. The robot is balanced by a third castor wheel. Two novel potential-field based controllers are derived which stabilize the robot within a surrounding circular area of arbitrary size. As introduced in Sec. 1.2.1, this region is called a robot’s bubble. The first controller takes the unicycle to the center of its bubble while the second corrects its orientation and brings it to a point on a bubble diameter. The designed potentials also work with a kinematic model. Explicit bounds for permissible initial speeds are derived, such that maximum torque-limits and/or maximum speed-limits are not violated once the controller is activated. These controllers are then embedded in a navigation framework. A global plan is first created using an existing algorithm described in Sec. 1.2.1. The robot then keeps itself within a fixed-sized bubble, which it then moves in discrete steps according to the direction provided by the global plan, while repulsively avoiding unexpected obstacles. Hence the gross movement is created by switching local potential-field based controllers. This scheme is first verified in computer simulation 39
of a single robot moving in a maze. It is then implemented on an experimental setup of robots equipped with proximity sensors. Results are presented to illustrate the effectiveness of the system. A polar coordinate-transformation is made use of for controller design. A local potential-field is created, which has the desired property of keeping the robot contained within a bubble, and at the same time making the robot converge to specifically designed manifolds. A unicycle trapped inside such a bubble has two modes of motion (i) A local scurrying motion to keep itself within its controlling bubble while satisfying its no-slip constraint, and (ii) An overall gross omnidirectional motion which is effected by switching the robot’s controlling bubble while keeping it enclosed at all times. This approach allows the robot to make use of all obstacle-free space, instead of constraining it to follow a specific planned time-trajectory. Torque constraints are handled by scaling the control gains for each local bubble controller before switching the system to that bubble. Explicit lower and upper bounds for a critical gain parameter are derived. This keeps the robot inputs bounded within specified limits and keeps the robot from colliding with obstacles. The approach also keeps the robot kinetic-energy within user-specified limits, thereby maintaining the robot heading and turning speeds bounded within limits. The content of this chapter has been published in references [70, 72, 71]. The rest of this chapter is organized as follows: In Sec. 3.2, the unicycle dynamic model is introduced and then transformed to polar coordinates. Two new control laws are derived and their properties are proved in Sec. 3.3, wherein torque and speed constraints are also covered. An existing technique for doing global path-planning, as described in Sec. 1.2.1, is integrated with the derived controller scheme in Sec. 3.5. Switching logic is discussed in Sec. 3.5.1, and a computer simulation is used to verify the approach in Sec. 3.5.2. An experimental implementation to show the efficacy of the framework is presented in Sec. 3.6. Finally, main results are reviewed in Sec. 3.7.
40
Yo YG
Xo
Ro
V
θo/G
θ
(xc,yc)
XG
ρ
Ri
φ
(xo/G,yo/G)
Xo
Figure 3.1: Unicycle inside a bubble with the coordinate systems illustrated. XG , YG represent a global coordinate system with respect to which the robot’s bubble coordinate system Xo , Yo is at a certain offset and orientation given by (xo/G , yo/G , θo/G ). Only half of the external bubble of radius Ro is shown. Robot radius is Ri . R ≡ Ro − Ri . 3.2
Unicycle Dynamic Model The dynamic model for the unicycle used in this chapter, is a simplified ver-
sion of the model presented in [91]. It has been assumed that the inertial quantities for the two wheels are negligible compared to those of the robot platform. Referring to Fig. 3.1, let O(Xo , Yo ) be an inertially fixed referential attached to a static virtual bubble B enclosing the robot. The configuration of B with respect to another global inertial referential G(XG , YG ) is given by (xo/G , yo/G , θo/G ). The dynamic equations of the unicycle are derived with respect to the inertial referential O in which the configuration of the robot could be given by q = [xc , yc , θ]T in Cartesian coordinates or p = [ρ, φ, θ]T with respect to a polar coordinate system. Here xc , yc are the coordinates of the center of mass (assumed to coincide with the geometric center) of the robot and θ denotes its orientation in O(Xo , Yo ). The inputs to the system are the motor-torques τr and τl applied on the right and left wheels respectively. The positive direction of these torques is taken as the one which takes the vehicle forward. In the following developments in this section, matrices ˆ refer to a Cartesian coordinate system and those without a with a carat (e.g. A)
41
carat refer to polar coordinates. In each case, an argument q or p further clarifies the referred system. The nonholonomic no-slip constraint can be given by ˆ q˙ = 0, A(q) ˆ A(q) = [sin(θ), − cos(θ), 0].
(3.1)
or equivalently in polar coordinates as: A(p)p˙ = 0, A(p) = [sin(θ − φ), −ρ cos(θ − φ), 0]. ˆ The null-spaces of A(q) and A(p), respectively, are given by: cos(θ − φ) cos(θ) 0 ˆ S(q) = sin(θ) 0 , S(p) = sin(θ − φ)/ρ 0 0 1
0
(3.2)
0 .
(3.3)
1
˙ Let m Let the heading-speed of the robot be v and the turning-speed be θ. be the mass and Iz be the moment of inertia of the platform. Let 2bw denote the distance between the wheels and rw denote the radius of each wheel. For planar motion, the Lagrangian of the system is just the kinetic energy which is given by 1 ˙ = q˙ T M q, ˙ M = diag([m, m, Iz ]). L(q) 2
(3.4)
Referring to the general procedure outlined in Sec. 2.2, the system dynamic equations can then be written as ˙ T, x ν = [v, θ] ˆ = [qT ν T ]T , ! ! ˆ S(q)ν 0 x ˆ˙ = + τ, ˆ −1 SˆT E ˆ f (SˆT M S) ˆ −1 (SˆT M S)ν. ˆ˙ ˆ f = −(SˆT M S)
(3.5) (3.6) (3.7)
In the case of unicycle, simple calculations show that ˆ f ≡ 0. Since the wheel inertia is considered negligible, the input transformation matrix E(q), relating motor-torques 42
to ν, ˙ needs some intermediate steps to be computed. Let ϕl and ϕr denote the angular displacements of the left and right wheels respectively. They are oriented such that they increase when the vehicle moves forward– which is the same convention as that used for motor-torques τl and τr . Using δ to denote a variation, the virtual work δW can be computed as δW = τr δϕr + τl δϕl , τl τr (δx cos(θ) + δy sin(θ) + bw δθ) + (δx cos(θ) + δy sin(θ) − bw δθ) , = rw rw ! τr ≡ ( δx δy δθ ) E(q) . (3.8) τl This gives the following expression for the input transformation matrix cos(θ)/rw cos(θ)/rw ! τr . E(q) = sin(θ)/rw sin(θ)/rw , τ = τl bw /rw −bw /rw
(3.9)
In polar coordinates, we can derive the equations in an equivalent way ˙ T , x = [pT ν T ]T , ν = [v, θ] ! ! 0 S(p)ν τ. + x˙ = ˆ −1 SˆT E (SˆT M S) 0
(3.10) (3.11)
ˆ Note that Eq. (3.11) contains both S(p) and S(q). In the above equations, the ν-system is still as it was in Cartesian coordinates. Only the upper part of x has changed from q to p. The polar coordinates are now used to design controllers for the robot. 3.3
Control Design and Handling Input/Speed Bounds Note that Eq. (3.11) is in the general form of Eq. (2.9) with m = 2, n =
3, k = 1. Therefore, the controller scheme derived in Sec. 2.3 is directly applicable.
43
Similarly, the handling of input/speed bounds in Sec. 2.3.2 remains applicable also. The input torques are computed from Eq. (2.22). It can be rewritten as Kp λ ∂U∞ T τ = −B + Kd ν , S (p) (λ + U∞ )2 ∂p ! bw 1 rw rw rw √ ), B = , kBk2 = kBkm 2 = max( √ , 2bw bw −1 2 bw 2
(3.12)
−1 ˆ T E(q) where, B = S(q) is a constant matrix for a unicycle. This control law makes the unicycle converge to the general invariant manifold E give in Eq. (2.23), which, for the unicycle case, can be rewritten in expanded form as cos(θ − φ)
∂U∞ sin(θ − φ) ∂U∞ + = 0, ∂ρ ρ ∂φ ∂U∞ = 0, ∂θ
(3.13) (3.14)
where, S(p) has been substituted in Eq. (2.23) from Eq. (3.3). Note that (1/ρ) ∂UΣ /∂φ should be non-singular at ρ = 0. To handle torque constraints, Eq. (2.45) is used, as in the general formulation. For the unicycle, µ = min(m/2, Iz /2). Kinetic-energy bounds are handled as in the general formulation of Sec. 2.3.2. The next two subsections detail the design of specific potential functions U∞ which make the robot converge to certain desired manifolds. Also we reiterate that U∞ is first squashed to singularity-less UΣ by using the analytical switch, before computation of torques. 3.3.1
Controller A: convergence to ρ = 0 ∆
Proposition 3.1. Referring to Fig. 3.1, we define R = Ro − Ri , and = ρ/R. For the robot to stay in the bubble ∈ [0, 1). In Eqs. (3.12), taking: U∞ =
2 2 2 θ−φ + γ sin ( ), γ > 0 1 − 2 2 44
(3.15)
ensures that the robot asymptotically reaches the center of B, i.e. ρ = 0, = 0, though not necessarily with the correct orientation, i.e. θ might not be 0. The initial conditions should be such that (t = 0) < 1. Proof. Substituting U∞ in Eq. (3.14) gives: = 0, γ2 sin(θ − φ) = 0 ⇒ 2 θ − φ = nπ, n ∈ Z
or
Substituting U∞ in Eq. (3.13) gives: 2 cos(θ − φ) 1 γ 2 θ−φ + γ sin ( ) − sin2 (θ − φ) = 0 2 2 R (1 − ) 2 2R
(3.16)
(3.17)
Taking the case = 0 from Eq. (3.16) and substituting in Eq. (3.17), it is seen to vanish identically. Clearly, this does not ensure correct orientation (θ = 0). Taking the case θ − φ = nπ, and substituting in Eq. (3.17), one gets: 2 cos(nπ) 1 2 nπ + γ sin ( ) ⇒ = 0 R (1 − 2 )2 2
(3.18)
Therefore = 0 ⇒ ρ = 0 is reached in each case. To use this potential in conjunction with a torque bound, one would need to √ numerically find χm from Eq. (2.45). To this end, a plot of χ1 (p) + α ˆ m χ¯2 (p)/ µ for this controller is given in Fig. 3.2(a), and the numerical value of χm found. Recall p that the damping Kd = α ˆ Kp . Asymptotic performance can usually be enhanced by having a variable but positive damping as given by the parameter α ˆ in Table 3.1. Intuitively, this means that the damping is the highest at = 0, which acts as a brake and attenuates undesired oscillations. 3.3.2
Controller B: convergence to θ = 0, xc = 0, φ = ±π/2
Proposition 3.2. In Eqs. (3.12), taking: U∞ =
2 θ + γ sin2 ( ), γ > 0 2 1− 4 45
(3.19)
ensures that the robot asymptotically reaches the correct orientation such that xc = 0, yc < R. The initial conditions should be such that ρ(0) ∈ [0, R). = ρ/R as before. Proof. Substituting U∞ in Eq. (3.14) gives: γ θ sin( ) = 0 ⇒ θ = 2nπ, n ∈ Z 4 2
(3.20)
Substituting U∞ in Eq. (3.13) gives: ( = 0, or 2 cos(θ − φ) 1 = 0 ⇒ R (1 − 2 )2 θ − φ = (2n + 1)π/2
(3.21)
as θ = 0 is always reached from Eq. (3.20), the second of conditions in Eq. (3.21), implies that: φ = −(2n + 1)π/2, which is a position on the Yo axis with xc = 0. On the other hand, if = 0, xc = 0 is trivially satisfied. For this controller, χ1 as given in Eq. (2.41), is given by
!
2 cos(θ − φ)/ (R(1 − 2 )2 ) λ
χ1 (p) =
2 γ θ
(λ + U∞ ) sin( ) 4 2 2
(3.22)
Since the objective is to maximize χ1 , we reduce the search-space by eliminating φ dependence from the above. This can be done by noticing that χ1 (p) ≤ χ¯1 (, θ), where, λ χ¯1 (, θ) = (λ + U∞ )2
!
2/ (R(1 − 2 )2 )
γ θ
sin( ) 4
2
(3.23)
2
To use this potential in conjunction with a torque bound, one would need to numer√ ically find χm from Eq. (2.45). To this end, a plot of χ¯1 (p) + α ˆ m χ¯2 (p)/ µ for this controller is given in Fig. 3.2(b), and the numerical value of χm found. Asymptotic performance can be enhanced by having a variable but positive damping as given by the parameter α ˆ in Table 3.1.
46
Table 3.1: Parameter Values for the Unicycle Dynamic Model m Iz Ri Ro R rw bw γ λ kτ kmax max UKE Contr. A α ˆ Contr. A α ˆm Contr. B α ˆ Contr. B α ˆm σs σo δ
18.2 0.39375 0.203 2Ri Ro − Ri 0.0825 0.1624 10.0 1 0.7 0.13644 2 0.1 + 9.9e−50 10 2 2 0.5 + 9.6e−100(cos φ+sin θ) 10.1 0.7 2 0.125
Kg Kg m2 m m m m m
Nm J
Remark 3.1. The potential defined in Eq. (3.15) is not the only one which makes the robot asymptotically reach the center. Other expressions have been derived by the author, however, the given potential does appear to be one of the simplest and shows good performance in simulations. Similar remarks apply to the potential defined in Eq. (3.19).
3.3.3
Comparison with other approaches The controllers derived above can be contrasted to the several kinematic
controllers studied in [41], and the kinematic model dynamic feedback-linearization based controller studied in [68]. The main advantages offered by the approach in this chapter are:
47
40
χ(p)
30 20 10 0 −1 −0.5 0 (θ − φ)/2 = 0 0.5 1
0
−0.5
−1
0.5
1
√ (a) Polar plot of χ1 (p) + α ˆmχ ¯2 (p)/ µ for controller A. The radial dimension is ∈ [0, 1), and the angular dimension is (θ − φ)/2. The maximum value χm is found to be 30.74, which is then used for estimating the upperbound on Kp .
25 χ1(p)
20 15 10 5 −1 −0.5 0 θ/4 = 0
0.5 1 −1
−0.5
0
0.5
1
√ (b) Polar plot of χ ¯1 (p) + α ˆmχ ¯2 (p)/ µ for controller B. The radial dimension is ∈ [0, 1), and the angular dimension is θ/4. The maximum value χm is found to be 24.80, which is then used for estimating the upperbound on Kp . In the last bubble, when the external bubble shrinks to (1 + δ)Ri , and controller B is to be activated, χm = 82.04
Figure 3.2: Numerical determination of input limits for controllers A and B. Plotted for the parameter values given in Table 3.1.
48
1. The bounded potential-field controllers are based on a dynamic model. As opposed to kinematic models, this model is not driftless. As an added bonus, the same potentials can also be used to derive kinematic controllers, as shown in Sec. 3.4. 2. No unnatural coordinate transforms, or time-varying controllers, as described in Sec. 1.4.3, are employed. The control laws retain the intuitive nature of the potential-field theory. 3. Velocity and torque bounds can be explicitly accounted for. When these controllers are integrated with the path-planning framework in Sec. 3.5, it results in a scheme where a geometric obstacle-free path is followed while keeping inputs and speeds within given bounds. No trajectory time-parameterization or time-scaling, described in Secs. 1.2.2 and 1.2.3, is necessary. 4. Motion can be restricted to specified obstacle free regions. 5. The control law has no singularities. Although the convergence performance in terms of required maneuvers is good, it is not exponential. 3.4
Kinematic Model based Control Many popular commercial models of differentially-driven robots do not allow
motor-torques to be directly specified. Instead, the user specifies inputs in terms of ˙ Subsequently a rotational speed the forward-heading speed v and turning-speed θ. controllers on the two motors try to follow these set-points by computing their own set-points by using Eqs. (3.24). Let the right-wheel rotational speed be θ˙r and that of the left wheel be θ˙l , then the following relations exists v=
θ˙r + θ˙l (θ˙r − θ˙l )rw rw , θ˙ = , 2 2bw 49
(3.24)
where rw and bw are as in Sec. 3.2. In polar coordinates given by Eq. (3.10), the kinematics are given by p˙ = S(p)ν,
(3.25)
which is a subpart of the dynamic equation Eq. (3.11). It turns out that the potentials which were derived for use with a dynamic model continue to be applicable. ˙ Furthermore, now one can satisfy input-speed constraints individually for v and θ. This is shown in the following. Proposition 3.3. Define a local potential field UΣ (p) inside the robot’s bubble as in Proposition 2.1. At time t = 0, let the robot be located inside the bubble at coordinates p0 so that UΣ (p0 ) < 1. A control law of the form ν=−
kv
0
0
kω
! S T (p)
∂UΣ , ∂p
(3.26)
with scalars kv , kω > 0, will cause the robot to converge to the manifold E given by Eqs. (3.13) and (3.14). The values of kv , kω can be selected so as to guarantee that the speed-inputs will always remain within specified limites. Proof. Consider the candidate Lyapunov function V = UΣ (p). Taking the time derivative of V , one gets V˙
∂UΣ T ∂UΣ T = p˙ = S(p)ν, ∂p ∂p
(3.27)
where in the last step, the kinematic-equation (3.25) has been substituted. On substitution of the proposed control law Eq. (3.26) in the above, one gets ! k 0 v V˙ = −ν T ν ≤ 0. 0 kω
(3.28)
Applying LaSalle’s Invariant Set Theorem, the system would converge to the invariant set ν = ν˙ = τ = 0, which on substitution in the control law Eq. (3.26) gives the required invariant set of Eqs. (3.13) and (3.14). 50
Table 3.2: Parameter Values for a Unicycle Kinematic Model γ λ Ri Ro Contr. Contr. Contr. Contr.
3.4.1
A vs A ωs B vs B ωs
10.0 1 0.203 2Ri 10.65178 0.306647 9.75369 0.504752
m m m/s rad/s m/s rad/s
Handling Input Constraints Note that the expressions of v and θ˙ given by Eq. (3.26) are bounded inside
and on the boundary of a bubble. Their absolute values therefore have unique supremums in the bubble-region. These can be found out simply by plotting the functions. This has been shown in Fig. 3.3 for potentials of controller A and B which were introduced in Secs. 3.3.1 and 3.3.2 respectively. Let these supremum values be vs and ωs repectively, for kv = kω = 1. Then given a user specified maximum value of heading speed v as vm , and that of angular-speed ωm , one can select: kv =
vm ωm , kω = . vs ωs
(3.29)
This will guarantee that the robot’s control speeds never exceed the user specified limits while the robot is inside its bubble. 3.5
Global Path Planning and Controller Integration For the following deliberations, we go back to the vehicle dynamic model
and controller design as discussed in Sec. 3.2. Having derived the control laws in Sec. 3.3.1 and 3.3.2, we now proceed to integrate it with the global path-planning Algorithm 1.2.1 described in Sec. 1.2.1. The output of this algorithm is a string S of overlapping bubbles joining the goal point to the start point. The size of each 51
(a) Polar plot of v and ω for the potential of controller A for kv = kω = 1. The radial dimension is ∈ [0, 1), and the angular dimension is (θ − φ)/2.
(b) Polar plot of v and ω for the potential of controller B for kv = kω = 1. The radial dimension is ∈ [0, 1), and the angular dimension is θ/4.
Figure 3.3: Finding vs and ωs for different potentials. Parameter values are shown in Table 3.2.
52
bubble represents the radial clearance available to the nearest obstacle. S is in the form of a linked-list with the bubble at the goal as the root. Once the global path-planning is over, the robot is trapped in a fixed-sized bubble after satisfying the feasibility conditions as described in Secs. 2.3.2, 2.3.3, and 3.3. This fixed-sized bubble is then switched in discrete steps to move according to the global plan S, all the while keeping the robot inside its bubble. Bubble switching occurs only when the new bubble becomes feasible, i.e., the speeds have reduced sufficiently. This is described next. 3.5.1
Switching Logic for Enclosing Bubble This section specializes the general switching logic discussed in Sec. 2.3.3 to
the case of a unicycle. In the following, Bi denotes robot’s current bubble, B.rG stands for the center coordinates of the bubble B in the referential G, and B.Ro denotes its radius. rs/G , rg/G , and ri/G are the Cartesian coordinates of the start, goal, and robot current location respectively, in referential G. Bi has an associated target which is the parent of the bubble in the global plan which encloses the center of Bi (Fig. 3.5). Bi .Target.rG denotes the position coordinates of the center of this target bubble. First a global plan S is generated as already detailed. Each robot is now assigned a fixed bubble Bi of radius Ro = σo Ri , σo > 1, where Ri is the radius of robot. The target of Bi is set to the first bubble in the global plan BStart . Note that the robot can only be assigned an initial bubble if the input and speed feasibility conditions given by Eqs. (2.34) and (2.46) are met. If not, brakes are applied till the robot has slowed down sufficiently to satisfy these conditions. Once the robot has entered its first bubble, subsequent feasibility is guaranteed by the switching Algorithm 3.5.1. Many different bubble-switching conditions have been tried. In practice, the following Lyapunov function value based switching has been found to work well: At each switching-decision instant, the robot creates the best geometrically feasible 53
bubble, as shown in Fig. 3.5. However, it is switched to, only if it is also feasible with respect to torque and speed limits, as given in Eqs. (2.34) and (2.46). If these are not satisfied, the robot stays in its current bubble till its speeds are sufficiently reduced due to control damping, and the conditions are met. Furthermore, controller B is kept active till the last bubble. In the last bubble, which has the same center as the goal point, controller A is activated. This causes the robot to converge to the center. The robot’s orientation is then corrected by reactivating controller B within a bubble of radius (1 + δ)Ri , 0 < δ 1. The above stated strategy is formalized below. We assume that the following pre-defined procedures are available: • FindFreeBubble(ri/G , Bi ) returns BNext This procedure returns the next best enclosing bubble BNext for the robot. 4
Referring to Fig. 3.4, define ξm = σs (σo − 1)Ri . Let eβ be a unit vector oriented at angle β with respect to u. BNext is a bubble at ri/G + ξeβ of radius σo Ri . Then the following problem is solved:
arg min ri/G + ξeβ − B.Target.rG , subject to: ξβ ( ξ ∈ [0, ξm ], BNext
does not intersect any obstacles.
• CheckDynamicFeasibility (BNext , ν0 , Controller Z) For given controller Z (A or B), given initial polar coordinates p0 w.r.t to a prospective bubble BNext , and velocity ν0 , and R = BNext .Ro − Ri , checks to see if Ineqs. (2.34) and (2.46) are met. • SwitchTo (BNext , Controller Z) Recomputes robot state p w.r.t the new bubble BNext , and makes it the current bubble. Activates controller Z. – Computes a feasible Kp for controller Z. 54
– Computes current global coordinates. xG ← Bi .xo + ρ cos(φ + Bi .θo ), yG ← Bi .yo + ρ sin(φ +Bi .θo ), θG ← θ +Bi .θo . – Bi ← BNext . – Computes new relative coordinates. ρ ← k[xG − Bi .xo , yG − Bi .yo ]k, θ ← θG −Bi .θo , φ ← atan2(yG −Bi .yo , xG −Bi .xo ) − Bi .θo . • HandleLastBubble() The robot has reached its last bubble with the center the same as the goal. Switches to controller A, and keeps it active till the robot center has converged to ρ < δRi , for a given 0 < δ < 1. Waits till a bubble of radius (1 + δ)Ri becomes dynamically feasible for controller B and then activates it. The net result is that the robot center converges to within δRi to the goal with the correct orientation. Finally, the robot motion logic can be given as Algorithm 3.5.1, where ∆Ts is the switching-decision time-instant.
Algorithm 3.5.1: RobotMotion(Goal rg/G ) ∀∆Ts repeat BNext ← FindFreeBubble(ri/G , Bi ) if BNext .rG = rg/G then Contr ← A else Contr ← B if CheckDynamicFeasibility(BNext , ν0 , Contr) then SwitchTo(BNext , Contr) else Apply torques based on current bubble B i
until Contr. A activated. HandleLastBubble()
55
A switching scheme for the kinematic controller described in Sec. 3.4 can also be devised. It is, however, far simpler than the dynamic case, as it needs to consider only the geometric feasibility of a new bubble. Intuitively this is due to the driftless first-order nature of the kinematic model. 3.5.2
Verification in Simulation A computer simulation of this framework for parameter values listed in Ta-
ble 3.1 is given in Fig. 3.6(a). The corresponding input torques and kinetic-energy as a fraction of the corresponding given maximum limits, are plotted in Fig. 3.6(b). Both speed and torque limits are therefore satisfied during the motion. The two vertical lines in in Fig. 3.6(b) delineate three regions of operation. Region 1 is when Controller B is active and the robot is running. Region 2 is when Controller A is active for the last bubble, and region 3 is when Controller B is activated for a bubble shrunk to radius (1 + δ)Ri . Kinetic energy is seen to remain almost steady during run-stage. To keep the robot within the last small bubble, torque increases while the kinetic-energy decreases. For the maximum specified kinetic-energy in Table 3.1, maximum v = 0.122 m/s, and maximum θ˙ = 48o /s, though these limits cannot simultaneously occur. Refer to [53] for an animation of the simulation. 3.6
Experimental Framework The setup consists of three IRobot’s Magellan Pro robots, each equipped with
an onboard computer, odometry encoders and a ring of 16 IR and SONAR sensors around the robot’s periphery. The onboard odometry is able to provide robot’s position and orientation with respect to the starting configuration at 10 Hz. However, it suffers from a drift in readings due to wheel misalignment, slight slippages and numerical errors. As a part of this work, to provide an accurate global coordinatesystem for the robots, two overhead cameras were setup. These were calibrated using [51]. A black and white fiducial mark was put atop each robot as shown in 56
or ns e S IR Data
Ro C2
v
O C1
u
Figure 3.4: Creation of a new bubble centered at C2 . The robot’s current bubble is shown centered at C1 . The distance C1 C2 = ρc = σs (σo Ri − Ri ). The surrounding polygon depicts the maximum clearances available to the robot around it, as detected by its sensors. The vector u points towards the target bubble, i.e. the ideal direction of movement. The vector v is the new direction to the target bubble.
Plan S nt re a P Par
ent
Target
Bi
Figure 3.5: Links between the various bubbles in the global-plan S, and the current bubble of the robot Bi . The robot is now able to take much sharper turns around obstacles. This is due to the fact that it is always following a target bubble in the global plan which is one ahead of which it is currently in.
57
(a) An obstacle-free global path created using planner. The filled bubbles are obstacles, and the rings are the planned bubbles. The robot is always enclosed in a fixed size bubble shown as a dark ring. The orientation of the robot is given by the drawn thick radius.
||τ|| / ||τ||max
0.15 0.1 0.05 0 0
20
40
60
80 100 time (s)
120
140
160
20
40
60
80 100 time (s)
120
140
160
KE / KEmax
0.6 0.4 0.2 0 0
max (b) kτ k/kτ kmax and UKE /UKE as a function of time for the simulation.
Figure 3.6: Simulation Results.
58
(a) Robots with b/w markers
(b) Markers change color after detection
Figure 3.7: Image-processing for robot localization Fig. 3.7(a). An image-processing layer was developed by the author which detects the robots in the camera image and computes its position and orientation using calibration data. A successful detection of a robot is signified by a change of color of its black marks as shown in Fig. 3.7(b). This setup was able to correct odometry drift at approximately 4 Hz. A distributed software framework has been developed, which allows for each robot to do its own planning and control. The goal points for each robot along with a set of known static obstacles (real or virtual) are specified by a centralized GUI, Fig. 3.8(a). The obstacles are specified as a set of intersecting rings of various sizes. In addition, the robot’s environment can be strewn with unknown obstacles. Each robot runs independently of the other and takes into consideration other robots or unknown obstacles only if they are detected by its IR sensors. To make this framework distributed, scalable, and to be able to run hierarchies of planners and controllers, it has been programmed based on NIST’s Real-time Constrol System (RCS) framework [67]. The Magellan Pro programming interface allows only set points for forward heading speed v and turning speed θ˙ to be commanded. Since the dynamic controller is based on torques, we take the approach of numerically integrating the dynamic
59
model of the robot/controller system. This integration is done only for one timesample period (' 0.075 sec.), and the robot determines its speed set-points for the next sample interval by performing this integration. This also allows one to put in safety measures to detect error-conditions. We use a fourth-order fixed step Runge-Kutta method for predictable integration time. A parallel process running at a slower sampling rate (' 0.15 sec.), monitors robot’s geometric progress, and creates new feasible bubbles for it, as required. Figs. 3.8(a) and 3.8(b) show an experimental run. The robot runs the algorithm outlined in Sec. 3.5.1. Note that the switching-scheme adopted causes a relatively smooth motion, with the speeds kept close to the maximum permissible by the kinetic-energy constraint. The videos of this and other experiments are also available on the web [53]. The robot motion is smooth and the kinetic-energy-limits are followed. 3.6.1
Reactive Avoidance for Multiple Robots In case multiple robots are simultaneously running, the logic shown in Fig. 3.4
can be used by each robot to create new bubbles, while reactively avoiding other robots or other dynamic obstacles not known to the global planner. This reactive collision avoidance does not guarantee that each robot will reach its goal, but has been shown to be effecive in experiments. Two such experiments have been shown in Figs. 3.9 and 3.10, which point to the robustness of the framework. 3.7
Conclusions This chapter presented a unified approach to planning and control of a non-
holonomic unicycle in an obstacle-ridden environment. The approach is based on designing local potential-fields, and then switching them guided by a globally feasible plan, and local sensor information. Explicit bounds on motor torques, and
60
IR Sensor Data
Robot
Camera-View Goal
Obstacles
(a) The experiment GUI camera and logical view. 0.5
y (m)
0 −0.5 −1 −1.5 0.2
0.3
0.4
0.5
0.6 x (m)
0.7
0.8
0.9
1
KE / KEmax
1
0.5
0 0
20
40
60 80 t (sec)
100
120
max (b) The (x, y) and (UKE /UKE , t) plots for the experiment. For max this experiment UKE = 0.00375 J., and δ = 0.5. The plot is shown during the motion when controller B stays active.
Figure 3.8: Experiment 1. A single robot traverses its global-plan using bubbleswitching.
61
(a) The camera and the logical view at the start of the experiment.
t (min)
10 5 0 1
0.5
0
−0.5
−1
y (m)
−1.5
−2
−1
−0.5
0.5
0
1
1.5
x (m)
(b) The (x, y, t) plot of the experimental run.
Figure 3.9: Experiment 2. A mobile robot reactively avoids two static robots in its path which were not known to its path-planner.
62
8
8
6
6 t (min)
t (min)
(a) The initial configuration of the experiment with the robots’ goals shown
4
2
4
2 1.5
0 1
1.5 0 1
1 0.5
0 −1 y (m)
1 0.5
0
0 −2 −0.5
−1 x (m)
y (m)
0 −2 −0.5
x (m)
(b) The (x, y, t) plot of the experimental run. The plot on the right shows the path of the center of each robot
Figure 3.10: Experiment 3. All three robots run simultaneously to go to their goals. They reactively avoid each other using their IR sensors.
63
kinetic energy can be satisfied for the entire duration of the motion without explicit time-parameterization of the trajectory or time-scaling. The framework was illustrated by simulations and several experiments.
64
Chapter 4 TWO TIME SCALE CONTROL FOR UNDERACTUATION WITH NONHOLONOMY
4.1
Introduction This chapter and Chapter 5 are devoted to the study of the application of
potential fields theory to the control of dynamically balanced underactuated systems. This study takes the form of studying two realistic nonlinear examples of the general structure that was identified in Sec. 2.4. The rest of this chapter is devoted to working out the modeling and controller-design aspects of an Inverted Wheeled Pendulum whose dynamic model has this particular structure. As is clear from Fig. 4.1, this example may be considered to be an extension of the planar nonholonomic unicycle model, which was dealt with in the last chapter. Two important differences from the unicycle model are the absence of a balancing castor wheel and the addition of a non-planar unactuated degree-of-freedom. Hence, there is a need for dynamic balancing of the vehicle. A literature review of past research done on the inverted wheeled pendulum is provided in the following Sec. 4.1.1. 4.1.1
Inverted Wheeled Pendulum Dynamically balanced inverted wheeled pendulums have evoked significant
interest recently [30, 86, 27, 9, 87, 12] and at least one commercial product Segway [92] is available. Such vehicles are of interest because they have a small foot-print and can turn on a dime. The kinematic model of the system is insufficient to 65
describe the system behavior and has been proved to be uncontrollable [86]. In fact, balancing of the wheeled pendulum system is only achieved by considering dynamic effects. Similar other systems like the cart and pendulum, and the Pendubot [22] have been studied in the literature. Unlike these systems however, the pendulum’s motion in the present system is not planar and the motors driving the wheels are directly mounted on the pendulum body. In an earlier work [30], a trajectory-tracking algorithm was found using a linear state-space model. A recent effort [9] concentrates on dynamic modeling and model-identification. However, no control laws were derived. In Grasser et al [27], a dynamic model was derived using a Newtonian approach and the equations were linearized around an operating point to design a controller. In Salerno et al [86], the dynamic equations were studied, with the pendulum pitch and the rotation angles of the two wheels as the variables of interest. Various controllability properties of the system in terms of the state variables were analyzed using a differential-geometric approach. In a recent paper [87], Salerno et al also designed a linear controller for stabilization and studied its robustness. A planar model that did not include vehicle yaw, and a linear stabilizing controller were derived in [12]. In contrast, in this work, the dynamic modeling is done directly in terms of variables of interest with respect to the planning and control of vehicle’s position and orientation. A Lagrangian approach is used to derive the equations and the nonoholonomic constraint forces are eliminated. The equations are then simplified, reduced in order and then checked for the strong-accessibility condition. It has been proved in this chapter, using a result from [59], that the maximum dimension of a feedback linearizable subsystem for this system is 4. A set of outputs is then chosen with the correct total relative degree and the system is put in the normal form [97] with the internal dynamics singled-out. In Sec. 4.5, a two level controller is designed which makes use of the partial feedback linearization results. The controller
66
Figure 4.1: Geometric parameters and coordinate systems for the inverted wheeled pendulum. keeps the vehicle pitch-angle, i.e. the pendulum angle from the vertical, within specified limits and tracks the given orientation and heading-speed set-points which are assumed to be coming from a higher-level controller for motion planning. In Sec. 4.6 another two-level controller is designed which is able to stabilize the vehicle to a point from any initial configuration and speeds while keeping the vehicle pitch bounded within specified limits. 4.2
Dynamic Model Referring to Fig. 4.1, b is distance OOw , where O is the point midway
between the two wheel centers. R is the radius of both wheels. The pendulum body parameters have the subscript b, the wheel parameters have subscript w. E(X, Y, Z) is an inertial frame. B(xb , yb = y, zb ) is a frame attached to the pendulum body. V(xv , yv = y, zv = Z) are the vehicle-fixed coordinates. The pitch angle 67
α is ∠(Z, zb ), and the vehicle orientation angle θ equals ∠(X, xv ). The center of mass of the pendulum body Gb is at coordinates OGb = (cx , 0, cz ) in B. cx would later be taken as 0 to simplify equations. Mb is the mass of the pendulum body. Ixx −Ixy −Ixz Ib/B = Iyy −Iyz −Ixy , Ixy = 0, Iyz = 0, −Ixz −Iyz Izz is the inertia matrix of the pendulum body about its center of mass Gb , in the basis (xb , yb , zb ). Ixz = 0 if we take cx = 0 and also assume that the body is symmetric about the xb axis. [Iwa , Iwd ] are the moment of inertia of a wheel about its axis and about a diameter respectively and Mw is its mass. φr and φl are the angles of rotation of the right and the left wheel respectively. The position-vector of point O in E is (xo , yo , R). The rotational kinetic energy of the pendulum body TbR , and its translational kinetic energy are computed as 2TbR = ΩTB/E Ib/B ΩB/E , 2TBT = Mb vGb /E · vGb /E .
(4.1)
The gravitational potential energy of the system is UB = Mb g(R + cz cos(α)).
(4.2)
The rotational and translational kinetic energy due to the two wheels can be given by 2TwR = Iwa φ˙ 2r + Iwa φ˙ 2l + 2Iwd θ˙2 , 2TwT = Mw R2 (φ˙ 2r + φ˙ 2l ).
(4.3)
The configuration variables of the system are initially taken as q6×1 = [xo , yo , θ, α, φr , φl ]T .
(4.4)
The motor-rotor inertial quantities are considered negligible compared to those of the wheels. The Lagrangian is therefore L(q) = TBT + TBR + TwR + TwT − UB . 68
Using the Euler-Lagrange equations, the equations of motion can then be derived as ˙ = E(q)τ + AT (q)λ, M (q)¨ q + V (q, q)
(4.5)
where A(q) is the nonholonomic constraints matrix derived in the next section. τ is the input motor-torque vector given by
τ=
0
0
0 0 ! 0 τr 0 , E(q) = . −1 −1 τl 1 0 0 1
(4.6)
Note the structure of the fourth row of E(q) which arises because the motors are mounted on the pendulum-body. λ is the constraint-force vector. The three nonholonomic constraints due to no-slip can be − sin(θ) cos(θ) A(q)3×6 = cos(θ) sin(θ) cos(θ) sin(θ) The null-space of A(q) is given by the 0 0 0 S(q)6×3 = 1 0 0
written as A(q)3×6 q˙ = 0, where 0 0 0 0 b 0 −R 0 (4.7) . −b 0 0 −R
matrix S(q) as cos(θ) 0 sin(θ) 0 0 1 . 0 0 1/R b/R 1/R
(4.8)
−b/R
The vector q˙ has to lie in this null-space, therefore q˙ = S(q)ν3×1 ,
(4.9)
˙ T. ν = [α, ˙ v, θ]
(4.10)
69
where v is the forward heading speed of the vehicle in the direction xv . Next, we follow the standard procedure for the elimination of Lagrange multipliers λ by premultiplication with S T to obtain ˙ + V (q, q)) ˙ = S T E(q)τ. (S T M S)ν˙ + S T (M Sν
(4.11)
We note that φr , φl are decoupled entirely from the rest of the state-variables and φ˙ r , φ˙ l can be found uniquely given v, θ˙ from Eq. (4.9). From a control point of view, we are not interested in these variables, and can thus reduce the order of the system by 2. We now define the configuration vector (qr ) and the state vector (x) as follows ! qr T qr = [xo , yo , θ, α] , x = . (4.12) ν 7×1 This results in the last two rows of the S(q) being removed and we get a truncated matrix Sr (qr )4×2 . Another simplification can be obtained by assuming that the body is symmetric about the xb axis. This implies that Ixz = 0 and cx = 0. The input-affine form is then given by x˙ = f (x) + g(x)u ! 04×2 ,u = g(x) = (S T M S)−1 S T E 7×2 f (x) =
τr
! ,
τl
Sr ν ˙ + V (qr , q˙ r ) −(S T M S)−1 S T M Sν
! .
(4.13)
Remark 4.1 (System Structure). In the terminology of Chapter 2, these dynamics satisfy the general structure of Eq. (2.9), but do not satisfy the condition given in Eq. (2.10). Hence, the general controller given in Eq. (2.22) cannot be used. Instead, one needs to use the formulation presented in Sec. 2.4 for dynamically balanced mobile robots.
70
We partition g(x) and f (x) in the following way for later derivations: ! f1 (x) (4.14) g(x)7×2 = [g1 (x), g2 (x)] , f (x) = , f2 (x) f1 (x) = ( cos (θ) v sin (θ) v θ˙ α˙ )T . (4.15) The detailed expressions for g(x) and f2 (x) have been provided in Appendix A. In the following two sections, differential geometric properties of the system are studied and the model is feedback partially linearized. Sections 4.4 and 4.3 were contributed by Dr. Jaume Franch [76]. Sections 4.5 onwards on velocity and position controller design were contributed by the author. 4.3
Strong Accessibility Condition and Maximum Relative Degree This section is devoted to study of the strong accessibility condition and the
feedback linearization of the system, either full or partial. The strong accessibility condition [66] is defined as follows: Defining: C = h{[Xk , [Xk−1 , [. . . , [X1 , gj ] . . .]]], Xi ∈ {f, g1 , . . . , gm }, j = 1, . . . , m, k ≥ 1}i.
(4.16)
where the notation h· · ·i denotes a distribution. If the dimension of C is n, then the system x˙ = f (x) +
m X
gj (x)uj
x ∈ Rn
(4.17)
j=1
is strongly accessible. Regarding the largest feedback linearizable subsystem, we refer to the construction in [59]. Consider the following sets and distributions: G0 = hg1 , . . . , gm i, Gf = {f + g, g ∈ G0 },
(4.18)
Gi = hGi−1 , {[Gf , Gi−1 ]}i, Qi = h{adif G0 , Gi−1 }i,
(4.19)
71
where [Gf , Gi−1 ] = {[X, Y ], ∀X ∈ Gf , Y ∈ Gi−1 }, Gi−1 means the involutive 1 closure of Gi−1 , adif g = [f, adi−1 f g], while adf g = [f, g]. Based on these definitions,
compute the following integers, r0 = dim G0
(4.20)
ri = dim Qi − dim Gi−1
∀i ≥ 1
Kj = #{ri ≥ j, ∀i ≥ 0} j ≥ 1.
(4.21) (4.22)
Then, it follows that r1 ≥ r2 ≥ . . . and the maximum relative degree that one can achieve for the system (4.17) is r1 + r2 + . . . ≤ n. Before computing these distributions and integers for f, g1 , g2 given in the last section and Appendix A, we will apply a feedback law in order to simplify as much as possible the input vector fields. Since the fifth and sixth coordinate of g1 and g2 are equal, while the seventh is the same but with different sign, we suggest the feedback law v1 =
(u1 + u2 ) , Dα
v2 =
(u1 − u2 )Rb . Gα
(4.23)
Here Gα and Dα are as defined in Appendix A. Therefore, the new input vector fields are (we use the same notation for the new input vector fields) g1 = ( 0 0 0 0 g1 [5] g1 [6] 0 )T ,
(4.24)
g2 = ( 0 0 0 0 0 0 1 )T ,
(4.25)
where, g1 [5] = Mb R2 + 2Mw R2 + 2Iwa + Mb cos (x4 ) cz R, g1 [6] = −R Mb cos (x4 ) cz R + Iyy + Mb c2z .
(4.26) (4.27)
Clearly, G0 = hg1 , g2 i has dimension 2. It is a straightforward computation to see [g1 , g2 ] = 0, from where we infer that G0 is involutive or, equivalently, G0 = G0 . This involutivity property is useful to see the equality G1 = hG0 , {[Gf , G0 ]}i = hG0 , [f, G0 ]i. 72
After Lie bracket computations, G1 = hg1 , g2 , [f, g1 ], [f, g2 ]i, where, [f, g1 ] = ( α1
α2
[f, g2 ] = ( 0 0 β3
0 α4
α5
0 β5
β6
α6
(4.28)
α7 )T ,
β7 )T ,
where αi and βj are certain nonzero functions that depend on the state variables. This distribution is not involutive since [g1 , [f, g1 ]] = ( 0 0 0 0 γ5
γ6
0 )T ,
where, γ5 , γ6 are functions depending on the state variables. Let us remark that dimension of hg1 , [g1 , [f, g1 ]]i = 2. This is the reason why [g1 , [f, g1 ]] ∈ / G1 . Hence, e1 = hg1 , g2 , [f, g1 ], [f, g2 ], [g1 , [f, g1 ]]i = h[f, g1 ], η3 , η5 , η6 , η7 i. G1 ⊃ G where, η3 =
0 0 1 0 0 0 0
0
0
0
0 0 0 0 0 0 , η5 = 0 , η6 = 0 , η7 = 0 . 0 0 1 0 1 0 1 0 0
(4.29)
e1 is still not involutive since [η3 , [f, g1 ]] ∈ e1 . Therefore, G /G G1 ⊃ hη3 , η5 , η6 , η7 , [f, g1 ], [η3 , [f, g1 ]]i. It can be seen that the distribution on the right hand side is not involutive because [η3 , [η3 , [f, g1 ]]] is not in that distribution. Therefore, G1 ⊃ hη3 , η5 , η6 , η7 , [f, g1 ], [η3 , [f, g1 ]], [η3 , [η3 , [f, g1 ]]]i = R7 . 73
This implies that G1 = R7 . Since G1 ⊂ C ⊂ R7 , it follows that C = R7 and, hence, the system is strongly accessible. On the other hand, Q1 = h[f, G0 ], G0 i = G1 . has dimension 4. Therefore, the integers ri , i ≥ 0 in Eqs. (4.20) and (4.21) are r0 = dimG0 = 2, r1 = dimQ1 − dimG0 = 4 − 2 = 2, r2 = dimQ2 − dimG1 = 0.
(4.30)
There is no need to compute more of these integers since they are positive and non-increasing. Finally, the controllability indices Kj , j ≥ 1 in Eq. (4.22) are K1 = #{ri ≥ 1, ∀i ≥ 0} = 2,
K2 = #{ri ≥ 2, ∀i ≥ 0} = 2,
K3 = #{ri ≥
3, ∀i ≥ 0} = 0. As before, there is no need to compute more of these indices because successive ones vanish. Summarizing, the largest feedback linearizable subsystem for the system has dimension 4. 4.4
Partial Feedback Linearization In this section, we partially feedback linearize the system. Using a feedback
law and a change of variables, we write the system as two chains of integrators, one for each input, plus three nonlinear equations which represents the internal dynamics of the system. By definition [97], the inputs do not appear explicitly in the internal dynamics equations. We shall find two variables whose relative degrees together match the maximum relative degree found in the previous section. Later on, three new variables φ1 , φ2 , φ3 will be obtained such that they are independent of the variables, whose relative degree is two, and their derivatives. It is not difficult to see that both x3 = θ and x4 = α have relative degree 2. Their equations are: x˙ 3 = x7 , x˙ 7 = f2 [3] + v2 , 74
x˙ 4 = x5 , x˙ 5 = f2 [1] + g1 [5]v1 .
(4.31)
In order to convert (4.31) into two chains of integrators, a new feedback law is applied: w1 = f2 [3] + v2 ,
(4.32)
w2 = f2 [1] + g1 [5]v1 .
(4.33)
After this feedback law, the input-vector fields become: Gm = ( g ¯1
g ¯2 ) ,
g ¯1 (x) = ( 0 0 0 0 0 0 1 )T , g ¯2 (x) = ( 0 0 0 0 1 g1 [6]/g1 [5] 0 )T , and the drift vector field becomes
x6 cos(x3 )
x6 sin(x3 ) x7 ¯ f (x) = x5 0 f2 [2] − (g1 [6]/g1 [5])f2 [1] 0
(4.34)
,
(4.35)
where the terms f2 [1], f2 [2], f2 [3] are as defined in Appendix A. For the operational range of α ∈ (−π/2, π/2), g1 [5] > 0 unconditionally. In order to obtain the three remaining coordinates of the change of variables, the condition ∇φi Gm = 0,
75
i = 1, 2, 3
(4.36)
must be fulfilled. Moreover, the gradients of φ1 , φ2 , φ3 must be linearly independent of the gradients of the coordinates already chosen (x3 , x7 , x4 , x5 ). Let us write ∇φi = (a1 , a2 , a3 , a4 , a5 , a6 , a7 ). The orthogonality condition (4.36) implies: (4.37)
a7 = 0, a5 g1 [5] + a6 g1 [6] = 0. Eq. (4.37) is satisfied if one takes: a5 = −λg1 [6], a6 = λg1 [5],
(4.38)
for some nonzero λ which could be in general a function of x. Note that the terms g1 [5], g1 [6] are functions of x4 only. For integrability, the following conditions must be also satisfied: ∂a4 ∂a5 ∂g1 [6] ∂a4 ∂a6 ∂g1 [5] = = −λ , = =λ . ∂x5 ∂x4 ∂x4 ∂x6 ∂x4 ∂x4
(4.39)
while a1 , a2 , a3 can be chosen as desired. Therefore, one feasible choice for the remaining coordinates is φ1 = x1 , φ2 = x2 , φ3 = −x5 g1 [6] + x6 g1 [5].
(4.40)
All of the these can be verified to satisfy Eqs. (4.37) and (4.39). Summarizing, the change of coordinates is given by: z = T (x) =
x3 x7 x4 x5 x1 x2 −x5 g1 [6] + x6 g1 [5]
76
.
(4.41)
Note that g1 [5], g1 [6] are now functions of α = z3 only. Using the change of variables (4.41) and the feedback law (4.32)-(4.33), the equations of the system become z˙1 = z2 , z˙2 = w1
(4.42)
z˙3 = z4 , z˙4 = w2 z7 + g1 [6]z4 cos(z1 ) z˙5 = g1 [5] z7 + g1 [6]z4 z˙6 = sin(z1 ) g1 [5] ∂g1 [6] ∂g1 [5] z7 + g1 [6]z4 z˙7 = z4 −z4 + ∂z3 ∂z3 g [5] 1 g1 [6] + g1 [5] f2 [2] − f2 [1] . g1 [5]
(4.43)
(4.44)
where f2 [1], f2 [2], f2 [3], g1 [5], g1 [6] are understood to be written in terms of z. Note that inputs are explicitly absent from the last three equations and they therefore represent the internal dynamics. 4.5
Velocity Controller Design Using Partial Linearization The results from the sections above can be used to design a nonlinear con-
troller which can track specified reference heading speed v and vehicle orientation θ, these are denoted as vd and θd respectively. The controller makes sure that α ∈ As = {|α| < αm < π/2} for a given αm > 0 . Note that this is a physically meaningful problem because using these reference commands one can safely follow a motion plan. The essential idea is to use the pitch angle α as a ‘gas pedal’ for the vehicle and use it to accelerate and decelerate till the specified speed is attained. The following should be noted 1. α(t0 ) ∈ As should be assured before starting the controller at time t0 . 2. Referring to Eqs.
(4.26) and (4.27), we see that g1 [5] > 0, g1 [6] < 0 if
α ∈ (−π/2, π/2) which is its operating range. The limiting case is when 77
the pendulum body pitches forward or backward such that it is horizontal. Thus, making sure that α(t) ∈ As , t > t0 , ensures safe operation. First, the dependence of the acceleration v˙ on α is derived. z7 + g1 [6]z4 , z3 = α, z4 = α, ˙ z1 = θ. v = x6 = g1 [5]
(4.45)
Plugging these in the last of Eqs. (4.44), we get z˙7 + α˙ 2
∂g1 [6] ∂g1 [5] ≡ Γ = (v α) ˙ + g1 [5]f2 [2] − g1 [6]f2 [1] ∂α ∂α
(4.46)
Differentiating Eq. (4.45) and substituting expressions from Eq. (4.46), one gets 1 (v α) ˙ ∂g1 [5] (Γ + g1 [6]w2 ) − g1 [5] g1 [5] ∂α 1 = (g1 [5]f2 [2] − f2 [1]g1 [6] + g1 [6]w2 ) (4.47) g1 [5] Now the basic idea is to have θ = θd , θ˙ = 0 and α = αr , α˙ = 0, where αr is v˙ =
yet unspecified. This control is actually simple to achieve due to the linear structure in the θ-system given by Eq. (4.42), and in the α-system of Eq. (4.43). Having achieved this, in the steady state α˙ = 0, θ˙ = 0 and the acceleration expression from Eq. (4.47) can be written as:
1 α α v˙ ss = fss (αr ) = (g1 [5]f22 − f21 g1 [6]) g1 [5] α= αr
(4.48)
Note that in steady-state, w2 = 0 because it is used to control only the αα α system and α = αr . Expressions for f21 and f22 are given in Appendix A and are
marked by underbraces. The function fss (α) is plotted in Fig. 4.2. One observes that fss (α) is monotonic and odd in the range α ∈ (−π/2, π/2). Also for a considerable range of α’s near zero, the behavior is linear. We can now construct a two-layer controller: A higher-level controller with slower dynamics for setting αr and a lower level controller with much faster dynamics for tracking αr and θd using controls w2 and w1 respectively. The lower level controller is easy to design due to the linear structure of Eqs. (4.42) and (4.43). 78
80 60 40
fss(αr)
20
cz
0 −20 −40
−80 −1.5
αm
−αm
−60
−1
−0.5
0
αr (rad)
0.5
1
1.5
Figure 4.2: Forward acceleration as a function of the pitch angle for the inverted wheeled pendulum. fss (αr ) plotted for αr ∈ (−π/2, π/2), for different values of cz . Negative cz curves are shown in dotted lines. The arrow denotes the direction of increase of cz . cz values vary from −0.75R to 10R, where R is the radius of the wheels. For cz = 0, fss (αr ) ≡ 0 and αr has no effect on the steady-state forward acceleration.
79
4.5.1
Lower-level Controller For this controller Cl , one can choose w1 = −kqv θ˙ − kq (θ − θd ), w2 = −kav α˙ − ka (α − αr )
(4.49)
The gain constants (kqv , kq , kav , ka ) should be high enough to ensure sub-second convergence. To get the actual motor torques, one has to apply the reverse feed˙ α˙ to 0 in back laws w → v → τ /u. This controller tracks θd , αr while taking θ, steady-state. Since this is a linear system, this controller regulates to the set-point exponentially. 4.5.2
Higher-level Controller This controller Ch has slower dynamics and assumes that the lower-level
controller Cl is able to track its reference αr ‘fast enough’. Ch also has to make sure that αr ∈ As for a given αm ∈ (0, π/2). It has to then vary αr to track the specified desired heading speed vd . However, for cz = 0, this approach cannot be used as αr has no effect on the steady-state forward acceleration. Consider the Lyapunov function 1 kv (vss − vd )2 + 2(αm 2 − αr 2 ) 2 αr α˙ r = + kv (vss − vd )v˙ ss (αm 2 − αr 2 )2
VΣ =
(4.50)
V˙ Σ
(4.51)
Substituting Eq. (4.48) in Eq. (4.51), one gets V˙ Σ =
αr α˙ r + kv (vss − vd )fss (αm 2 − αr 2 )2
(4.52)
This suggests the control law α˙ r = −kr fss − kv (αm 2 − αr 2 )2 (vss − vd )
80
fss (αr ) αr
(4.53)
Note the following properties of fss , the first of which is due to the fact that the function is odd. αr fss (αr ) > 0, αr 6= 0 Mb Rcz g fss (αr ) = lim 2 αr →0 αr Mb R + 2Mw R2 + cz RMb + 2Iwa
(4.54) (4.55)
Substituting expression (4.53) in Eq. (4.52) one gets V˙ Σ = −
kr αr fss ≤0 (αm 2 − αr 2 )2
(4.56)
On using LaSalle’s Invariant Set Theorem and properties (4.54) and (4.55), one deduces that the system converges to lim αr = 0, α˙ r = 0 ⇒ vss = vd
t→∞
(4.57)
This can be seen by setting the control law defined by Eq. (4.53) to zero, and putting αr to zero. Also, as αr (t0 ) ∈ As , and the Lyapunov function is nonincreasing under the control law, the reference pitch angle αr cannot go out of As due to the barrier offered by the first term in VΣ . The dynamics of this controller can be slowed down by adjusting the gains kr , kv . The stability-proofs of these controllers apply in their respective time-scales only. Hence, one would like to make sure that the higher level controller is activated only when the lower level controller has converged. One approach to accomplish this would be to apply it in a discrete fashion every δt milli-seconds, where δt is greater than the stabilization time for the lower level linear controller. Another effective approach of making the dynamics of αr slower than that of the ˆ ˙ ˆ actual pitch angle α is to multiply the right hand side of Eq. (4.53) by e−k|α| , k 1.
The rationale of this is that as long as the real pitch rate is high, α˙ r remains small, i.e. αr remains almost constant till the lower level controller catches up. Both approaches work, though we present simulation results of the second approach only 81
because it automatically adjusts to any changes in the time-constant of the lower level controller. Remark on Internal Dynamics: We can note from the internal dynamics Eqs. (4.40) that φ1 and φ2 correspond to the xo , yo Cartesian coordinates respectively. The third internal dynamics variable φ3 is clearly a linear combination of the forward heading speed v and α, ˙ according to the third of Eqs. (4.40), with the scaling factors g1 [5] and g1 [6] being bounded functions of α. For the velocity controller, φ1 = xo , φ2 = yo will not be bounded. This is to be expected because one is controlling velocity, not position. However as v is bounded, because it is a controlled state, x˙ o , y˙ o will also be bounded. Similarly as both v and α˙ are controlled, φ3 , which is a linear combination, will remain bounded. 4.5.3
Simulation Results For the simulations, the parameter values taken are listed in Table 4.1. Fig.
4.3 shows the response of a stop command vd = 0 for non-zero high initial speed and pitch. In addition, the vehicle is commanded to stop at orientation θd = π/6 rads. The responses for two values of cz are compared. A higher cz shows higher oscillation amplitudes in v before settling down. The controller was able to stop the vehicle in about 15 seconds. The simulation is for illustration only and in a real system, the controller parameters should be adjusted to halt the system sooner for safety reasons. Fig. 4.4 shows the result of specifying a step-command for a high speed vd = 7 m/s and θd = π/6 rads from zero initial speed, pitch and orientation for two different pitch-limits αm . Note that the controller follows the limits rather conservatively. A higher αm leads to quicker acceleration but also more oscillation in v and hence the settling time is increased.
82
Table 4.1: Simulation Parameters for the Inverted Wheeled Pendulum Mb Mw R cz b Ixx Iyy Izz Iwa Iwd αm kq kqv ka kav kr kv kˆ
35.00 5 0.25 R, 3R 0.20 2.1073 1.8229 0.6490 0.1563 0.0781 π/9, π/4 100 5 1000 50 20 4 10/αm 100
83
Kg Kg m m m Kg m2 Kg m2 Kg m2 Kg m2 Kg m2 rad N/rad N/(rad/s) N/rad N/(rad/s) N/rad N/(rad/s) s/rad
α (deg.)
10 0 −10 −20
0
5
10
15
20
25
15
20
25
15
20
25
t(s) v (m/s)
4 2 0 −2
0
5
10
t(s) θ (rad)
0.8 0.6
cz=3R
0.4
cz=R
0.2 0
0
5
10
t(s)
Figure 4.3: Comparison of responses to a stop command. Set-points were vd = 0, θd = π/6 = 0.52 for cz = R and cz = 3R with αm = π/9 ≡ 20o . The initial speed v(t0 ) = 1 m/s and initial angles were θ(t0 ) = 0, α(t0 ) = π/18 ≡ 10o .
α (deg.)
40 20 0 −20
0
5
10
15
20
25
30
20
25
30
25
30
t(s) v (m/s)
20 10 0 −10
0
5
10
15
t(s) θ (rad)
1
αm= 20o
0.5
0
αm= 45o 0
5
10
15
20
t(s)
Figure 4.4: Comparison of responses to a step command. Set-points were vd = 7m/s, θd = π/6 = 0.52 for αm = π/9 ≡ 20o and αm = π/4 ≡ 45o . The initial speed v(t0 ) = 0 m/s and initial angles were θ(t0 ) = 0, α(t0 ) = 0. 84
4.6
Position Stabilization Control In this section, a controller is designed to stabilize the coordinates of the
point O (Fig. 4.1) (xo , yo ) to the origin of E using smooth feedback control. The correct final orientation of the robot is not achieved using this control: however, this can be easily achieved by an in-place rotation using the linear sub-system Eq. (4.42) and the previously derived nonlinear feedback Eqs. (4.32) and (4.23), once the desired position has been stabilized at. The issues involved in stabilization of nonholonomic mobile robots have been discussed in Sec. 1.4. To stabilize to the origin of E, polar coordinates are used, which for a vehicle at zero-pitch would look the same as those illustrated for a unicycle in Fig. 3.1. The configuration of the system, and its time-derivative can now be expressed as p ≡ [ρ, φ, θ, α]T , 0 cos(θ − φ) 0 α˙ 0 sin(θ − φ)/ρ 0 ˆ p˙ = v = S(p)ν. 0 0 1 ˙ θ 1 0 0
(4.58)
(4.59)
ν is as defined in Eq. (4.10). 4.6.1
Effect of Non-zero Turning-speed on the Steady-state Heading Acceleration In Eq. (4.48) and Fig. 4.2, the expression for the steady-state heading
acceleration was found assuming that the angular speeds θ˙ and α˙ have already gone ˙ The pitch angular speed α˙ to zero. Now, we also consider the effect of non-zero θ. is still considered negligibly small, due to the selection of the control law. Now the expression in Eq. (4.48) is modified as g [6] ˙ ˙ 1 α θ α θ ˙ = (f + f ) − (f + f ) v˙ ss = fss (αr , θ) . 22 22 21 21 g1 [5] α=αr 85
(4.60)
25 20
αmax
−αmax
cz = 2R
15
dvss/dt (m/s)
10
dθ/dt =0
5 0
fmax
−fmax
−5 −10 −15
dθ/dt =5 −20 −25 −1.5
−1
−0.5
0
αr (rad)
0.5
1
1.5
Figure 4.5: Forward acceleration as a function of vehicle pitch-angle and turning ˙ as a function of αr for five different values of speed. Plot of fss (αr , θ) ˙ The specified maximum allowable pitch angle αmax is marked by θ. vertical lines. The intersection of these lines with the curve for θ˙ = 0 is marked by the points ±fmax . ˙
˙
α θ α θ , f22 are given in Appendix A and are marked with , f21 , f22 The expressions for f21
underbraces. ˙ is Eq. (4.60) is plotted as a function of its arguments in Fig. 4.5. fss (αr , θ) seen to increase in range as the absolute value of θ˙ increases. The function retains its odd property. These properties will be made use of in the controller design. The position controller is again a two level controller. A higher level controller controls the θ-system of Eq. (4.42), and also determines a reference pitch angle αr (t). This is then followed by a faster lower-level controller which operates on the α-system, Eq. (4.43). 4.6.2
Higher-level Controller This controller is designed using two propositions. Direct correspondence to
the general scheme outlined in Sec. 2.4 is clearly evident.
86
Proposition 4.1. Let VΣ (p) be a potential-field such that it is lower-bounded and does not explicitly depend on α. On using a control law of the form w1 = −
∂VΣ ˙ − kw θ, ∂θ
and setting the reference pitch angle αr as the solution of ∂V sin(θ − φ) ∂V Σ Σ ˙ =− cos(θ − φ) + − kv v. fss (αr , θ) ∂ρ ∂φ ρ
(4.61)
(4.62)
(kw > 0, kv > 0), the wheeled inverted pendulum mobile robot will converge to a manifold defined by M=
∂VΣ T ˆ p : S (p) = 0, ν = 0 ∂p
(4.63)
Proof. Consider a scalar function V (p, ν) = VΣ (p) +
v 2 θ˙2 + . 2 2
(4.64)
where VΣ (p) is lower-bounded as mentioned before. For this higher-level controller one assumes that α˙ is negligible, therefore v˙ = v˙ ss = fss and therefore Eq. (4.60) can be used. On differentiating V with respect to time and substituting Eqs. (4.42) and (4.59), one gets
∂VΣ ∂p
T
p˙ + ( 0 v˙ θ¨ ) ν, 0 T ˙ . ˆ (p) ∂VΣ + fss (α, θ) = νT S ∂p w1
V˙ (p, ν) =
(4.65)
(4.66)
As mentioned before, VΣ is chosen not to have any explicit dependence on α. Eq. (4.66) then can be expanded as ∂VΣ ∂VΣ sin(θ − φ) ˙ ˙ V = v cos(θ − φ) + + fss (α, θ) ∂ρ ∂φ ρ ∂V Σ + θ˙ w1 + . ∂θ 87
(4.67)
On substitution of the proposed control law Eq. (4.62) in the above, one gets V˙
= −kw θ˙2 − kv v 2 ≤ 0.
(4.68)
V is lower-bounded as VΣ is assumed to be lower bounded. V˙ has been shown to be negative-semidefinite. Consider the system-state to lie in the set Ω = {|ν| ∈ [0, ∞); α ∈ (−π/2, π/2); θ, φ ∈ (−π, π); ρ ∈ [0, ∞) }
(4.69)
Using the Invariant Set Theorem [97], the system will converge to ∂VΣ = 0, ∂θ ∂VΣ ∂VΣ sin(θ − φ) cos(θ − φ) + = 0, ∂ρ ∂φ ρ ˙ = 0 ⇒ αr = 0, α˙ r = 0. θ˙ = 0, v = 0, fss (αr , θ)
(4.70) (4.71) (4.72)
This can be seen by setting the control laws given by Eqs. (4.62) and (4.61) to zero ˙ Eq. (4.71) gives an equilibrium invariant set and substituting zeros for v and θ. which is the same as the set M in the proposition. Note also that these equations are the same as Eqs. 3.13 and 3.14. Now remains the task of finding a suitable VΣ . Since fss (αr , θ˙ = 0) is bounded by fmax as shown in Fig. 4.5, VΣ has to be such that the control law Eq. (4.62) is realizable. Also, one needs to make sure that the invariant set M consists of ρ = 0 so that the desired position stabilization to the origin is achieved. Proposition 4.2. Consider the candidate function VΣ (ρ, φ, θ) = kΣ
ρ2 , σ>1 σ + cos(θ − φ)
(4.73)
then the requirements of proposition 4.1 are satisfied and additionally the invariant set M consists of ρ = 0, αr = 0, ν = 0. 88
Proof. As ρ2 ≥ 0 and | cos(θ − φ)| ≤ 1, VΣ is lower bounded by 0. Substituting Eq. (4.73) in Eq. (4.70) one gets kΣ
ρ2 sin(θ − φ) =0 (σ + cos(θ − φ))2
(4.74)
Substituting Eq. (4.73) in Eq. (4.71) one gets 3 cos2 (θ − φ) + 2σ cos(θ − φ) − 1 kΣ ρ =0 (σ + cos(θ − φ))2
(4.75)
Eq. (4.74) is satisfied if either ρ = 0 or if θ − φ = nπ. 1. If ρ = 0, Eq. (4.75) is also satisfied. 2. If θ − φ = nπ, Eq. (4.75) is only satisfied if ρ = 0. This is because, 3 cos2 (θ − φ) + 2σ cos(θ − φ) − 1 6= 0 for (θ − φ) = nπ, if σ > 1. In any case, our objective of converging to ρ = 0 is satisfied. Furthermore, Eq. (4.62) now amounts to finding a value of αr such that 3ξ 2 + 2σξ − 1 ˙ − kv v, ξ ≡ cos(θ − φ) ∈ [−1, 1]. fss (αr , θ) = −kΣ ρ (σ + ξ)2 Once, a maximum allowed pitch αmax is decided, Fig. 4.5 shows that, |fss (αr , θ˙ = 0)| ≤ fmax . Due to the asymptotic convergence of ρ to 0, one expects it to be a decaying function of time, with some initial oscillations due to non-zero initial velocities. One can therefore decide on a value ρm > ρ(t0 ) such that ρm > ρ(t), t > t0 . Eq. (4.76) always has a feasible |αr | ≤ αmax as solution for v = 0, if fmax ≥ Ξm , 2 max 3ξ + 2σξ − 1 k Σ ρm Ξm ≡ ξ ∈ [−1, 1] (σ + ξ)2
(4.76) (4.77)
It can be shown that this maximum value Ξm occurs at the boundary (ξ = ±1) or at ξm = −(σ 2 + 1)/2σ if |ξm | ≤ 1. For a given σ, this maximum value can be precomputed, e.g. for σ = 2, Ξm = 2ρm . We can now choose the gain 89
KΣ ≥ fmax /Ξm to always ensure a feasible solution |αr | ≤ αmax for v = 0. fmax was ˙ Fig. 4.5 shows that the available range computed assuming, θ˙ = 0. For non-zero θ, of |fss | is actually higher and therefore, a feasible αr can still be found. The leeway between Ξm and the actual value of the function on the right hand side of Eq. (4.77) can be used to compute a feasible value of kv for non-zero heading speeds. This means that the value of kv ≥ 0 can potentially change from its nominal positive value if the nominal value is not feasible at a given time-instant. Note that kv = 0 is guaranteed to be feasible so an iterative reduction of kv in case of infeasibility is bound to find a solution for any value of v. 4.6.3
Lower-level Controller This controller is designed to give a sub-second convergence to the desired
αr set by the higher-level controller. Using the linear sub-system Eq. (4.43), this can be achieved by w2 = −kav α˙ − ka (α − αr )
(4.78)
The gain constants (kav , ka ) should be high enough to ensure sub-second convergence. To get the actual motor torques one has to apply the reverse nonlinear feedback laws w → v → τ /u. Remark on Internal Dynamics: Clearly, as the controller guarantees p asymptotic convergence of the polar coordinate ρ = x2o + yo2 to 0, both φ1 = xo and φ2 = yo converge to 0. Similarly as all the speeds approach 0 due to asymptotic stability, φ3 also approaches 0 asymptotically. 4.6.4
Simulation Results The higher-level controller is conditionally executed only when |α| ˙ ≤ , where
0, and k( G
ˆ2 G
(5.12)
ˆ 3 )k = 1. G
Proof. Formulate a Lyapunov function Wf as follows 3
1X 1 Wf = kr Vr + Vk , where kr > 0, Vr ≡ k(R − Rr )ei k2 , Vk ≡ ΩT Im Ω. (5.13) 2 i=1 2 Clearly, Vr is a bounded potential, whereas Vk is the rotational kinetic-energy. ˙ f = kr W = kr
3 X i=1 3 X
˙ ˙ i + ΩT Im Ω, ei T (R − Rr )T Re ei T (R − Rr )T RS(Ω)ei + ΩT (−S(Ω)Im Ω + τf ) .
i=1
101
(5.14)
Defining sδ as in Eq. (5.7), substituting the LLC control law Eq. (5.12), and using the fact that RT R = I3 we get, ˙ f = −ΩT Kw Ω ≤ 0. W
(5.15)
Now we use LaSalle’s Invariant Set Theorem to characterize the set to which ˙ f = 0 ⇒ Ω = 0. Also, in the invariant the system converges. At equilibrium, W ˙ = 0. Substituting these on the right of Eq. (5.4), we get τf = 0, which manifold Ω implies ˆ − |Qt |e2 + |Tm |lm × G ˆ + lt × |Tt |e2 = 0, − |Qm |G
(5.16)
Now, substituting the above in the LLC control Eq. (5.12), one arrives at sδ = 0. Using Eq. (5.11), one concludes that either R = Rr , or that R and Rr differ by a rotation of ±π about an arbitrary axis. In the following Remark 5.3, it is proven that the latter case can be eliminated by choosing the gain kr appropriately. Lemma 5.2. Vr , as defined in Eq. (5.13) can be equivalently written as Vr = 3 − trace(Rp ) = 3 − trace(Rq ) = 2[1 − cos(ϕ(Rp ))] = 2[1 − cos(ϕ(Rq ))].
(5.17)
Proof is by simple algebraic manipulations and using property given by Eq. (5.5). Note that max(Vr ) = 4 at ϕ(Rp ) = ±π. Remark 5.3 (Choosing kr ). Note that R = Rr Rp , and Rp denotes the amount of rotation required for R and Rr to coincide. Therefore, the assumption ϕ(Rp ) 6= ±π means that the commanded reference matrix is not a complete flip about any axis, w.r.t. the initial attitude R. Since the Lyapunov function Wf has been proven to be non-increasing under the control law, kr Vr (t0 ) + Vk (t0 ) ≥ kr Vr (t) + Vk (t). 102
(5.18)
If we choose kr such that kr (max(Vr ) − Vr (t0 )) > Vk (t0 ),
(5.19)
then ϕ(Rp )(t), t > t0 will always lie in the open interval (−π, π). This is always possible as ϕ(Rp )(t0 ) 6= ±π. This shows that Rp = I3 is the only possible equilibrium point of the system. Also, as LLC takes the system to Vk = 0 at its steady-state, kr once set, need not be changed. Remark 5.4. We have not relied on an explicit parameterization of the rotation matrix, e.g. in terms of Euler angles, and therefore have circumvented the singularities associated with the mapping between rotation rates and angular velocity components. Remark 5.5. The LLC control-law is in the form of three nonlinear algebraic equaˆ 1, G ˆ 2, G ˆ 3 , an algorithm such as Gausstions. To numerically solve for inputs Tt , G Newton or Levenberg-Marquardt method needs to be used. Initial guesses for Tm , Tt , ˆ are provided by the HLC to the LLC. G ˆ 1, G ˆ 2, G ˆ 3 have to lie on a unit sphere, and G so they need to be parameterized accordingly. Although we have 3 equations and 3 ˆ i ’s are components of a unit vector, and LLC might have to increase the d.o.f., G nominal value of Tm = T¯m given by the HLC, to satisfy the equations exactly during high speed motion transients. Remark 5.6 (Exponential Stability). Although the above Lyapunov function shows only asymptotic stability, the control-law given by Eq. (5.12) is actually exponentially stable. The more involved proof of this is presented in Lemma B.3 of the Appendix B.1. Asymptotic stability is separately proven because the corresponding simpler Lyapunov Function Wf is useful for proving exponential stability and for deriving other conditions, e.g., as in Eq. (5.18). 5.3.2
Higher Level Controller At this level, the reference matrix Rr is computed to achieve translational
convergence to ξ = 0. In keeping with the time-separation paradigm, HLC design 103
ignores the LLC dynamics, and assumes that LLC is able to track its rotationalreference Rr instantaneously, and the angular-velocity Ω = 0. Refer to Remark 5.9 for the theoretical justification. We pose a further requirement that the range of the translation of the center of mass is limited to a spherical-ball (a bubble) centered at ξbi , and of radius Rbi , assuming that the helicopter starts somewhere inside this bubble. The super-script i defines an index for the bubbles. Let us move the origin of the inertial referential F to the center of helicopter’s current bubble ξbi . The vehicle’s translational coordinates ξ are now with respect to ξbi . We first design a local potential field Un (ρ = ξ/Rbi ), as explained in the following Lemma 5.3. This field is designed to have a global unique minimum at the origin ρ = 0, is limited to the interval [0, 1] within the bubble, and reaches its maximum of unity at the bubble’s boundary ρ = 1. Lemma 5.3. Let ρ = ξ/Rb , and let Un (ρ) be a potential-function mapping the unitball kρk ≤ 1 to [0, 1] such that in its domain ∃ kρ ≥ 1 such that Un (ρ) ≤ kρ kρk2 . The following potential-function satisfies these criteria Un (ρ) =
kρk2 , 1 ≥ λu > 0 kρk2 + λu (1 − kρk2 )
(5.20)
This can be proven by noting that the denominator lies in the interval [λu , 1]. For this potential the following properties hold, which may be proven by direct substitution, ∀ kρk ≤ 1, kρk2 ≤ Un (ρ) ≤ (1/λu )kρk2 , ∂Un (ρ) 2λu Un (ρ) ρT = ≥ 2λu Un (ρ). 2 ∂ρ kρk + λu (1 − kρk2 ) An upper-bound on the gradient of Un can be found as follows, ∀ kρk ≤ 1,
∂Un 2 2λu kρk 2λu kρk
= ≤ ≤ . 2
∂ρ (kρk2 + λ (1 − kρk2 )) λ2u λu u 104
(5.21) (5.22)
(5.23)
Proposition 5.2 (Higher level controller). Given a Un (ρ) with the properties stated in Lemma 5.3, if we compute the reference rotation-matrix Rr to satisfy the following equation
ˆ1 |Tm |G
∂U k t n ˆ Rr (τs )ss = Rr |Tm |G2 + |Tt | = − mge3 + Rb ∂ρ + kv v , ˆ3 |Tm |G ss
(5.24)
then the system converges to ξ = 0, ξ˙ = 0. The subscript ‘ss’ signifies that the ˆ 1, G ˆ 2, G ˆ 3 satisfy the steady-state conditions for the LLC, given in inputs Tm , Tt , G Eq. (5.16). A systematic way to solve this equation is given at the end of this subsection. Additionally, the system stays within its bubble during this motion, if the gain kt is such that kt (1 − Un (ρ0 )) >
1 T v mv0 , 2 0
(5.25)
where ρ0 and v0 are the initial values of ρ and v, when the vehicle first enters the bubble. Proof. We first prove asymptotic stability as it is straight-forward. This provides us with a simple Lyapunov function which can be used for computing certain bounds later. Exponential stability of this control-law is proved in Lemma B.5 of Appendix B.2. Define a Lyapunov function Ws as 1 Ws = vT mv + kt Un (ρ). 2
(5.26)
On taking the first time derivative of Ws and substituting dynamic Eqs. (5.3), one obtains
ˆ1 |Tm |G
˙ s = vT Rr |Tm |G ˆ 2 + |Tt | + mge3 + kt ∂Un , W Rb ∂ρ ˆ |Tm |G3 105
(5.27)
On substitution of the control law Eq. (5.24) in the above, one gets ˙ s = −vT kv v ≤ 0. W
(5.28)
Again, using LaSalle’s Invariant Set Theorem, one obtains the converged equilibrium ˙ s = 0 ⇒ v = 0. Also, at equilibrium v˙ = 0. On substitution of these in set as W Eq. (5.3), we get ˆ1 |Tm |G
ˆ 2 + |Tt | + mge3 = 0. lim Rr |T | G m t→∞ ˆ |Tm |G3
(5.29)
The helicopter is in translational equilibrium. Finally, on substitution of the above in the HLC control law Eq. (5.24), one gets ∂Un /∂ρ = 0. But due to our design of the potential function Un (ρ), this can only happen at ρ = 0. Remark 5.7. Now a solution strategy for Eq. (5.24) is discussed. Since Rr is a rotation matrix, it does not change the magnitude of a vector. Hence a necessary condition for solution to exist is kmge3 +
kt ∂Un ˆ 2. + kv vk2 = Tm2 + Tt2 + 2|Tt ||Tm |G Rb ∂ρ
(5.30)
ˆ in terms Since Eq. (5.16) needs to be satisfied as well, we can directly solve it for G of Tm and Tt . This gives
ˆ1 G
ˆ G 2 fm (Tm ) = ˆ3 G 1 3 3 1 2 1 3 2 1 −(lm lt Tt − lm lt Tt + lm Qt ) lm Tm 2 − (−lm Qt − lm lt Tt ) Qm Tm − lt3 Tt Qm 2 2 3 3 1 1 3 1 1 3 2 2 −Qt Qm 2 − (lm l + l l ) T T Q − (−l l T + l l T + Q l ) l T t m m t t t m t m t m t m t m m (5.31) 2 1 2 3 1 3 3 1 2 3 Tt lt1 Qm + (−lm Qt + lm lt Tt ) Tm Qm + (−lm lt Tt + lm lt Tt − lm Qt ) lm Tm 2 fm (Tm ) ≡ Qm Q2m + Tm2 klm k2 , ˆ1 k( G
ˆ2 G
ˆ 3 )k = 1. G 106
(5.32) (5.33)
Eqs. (5.30) and (5.33) have now to be solved for unknowns Tm and Tt using a nonlinear algebraic solver. Now, it only remains to find an Rr , which is dealt with in the following remark. Remark 5.8. Eq. (5.24) is of the form Rr u = v, where we’ve made sure that kuk = kvk. A solution for Rr is hence guaranteed to exist. ˆ = w
u×v , u 6= v ku × vk
(5.34)
is the axis of rotation to get from u to v. After computation of the angle between u and v, Rr can be computed by the well-known Rodrigues formula [64]. If u = v, Rr = I3 trivially. There is, however, one degree-of-freedom in the choice of Rr . This can be ˆ above, one can make seen by noticing that after rotation about the rotation-axis w a further rotation about an axis parallel to v without changing the solution. This additional degree-of-freedom can be used to our advantage to provide yaw. ˆ as computed Due to the fact that u and v have significant e3 components, w, in Eq. (5.34) is almost perpendicular to e3 , and so provides negligible yaw. Intuitively, yaw does not affect the rotational equilibrium. To keep the helicopter headed in the desired yaw direction φh , one can apply an additional yaw-compensator loop, as depicted in Fig. 5.1. φh will be specified by an additional heading-logic to keep the helicopter oriented in the direction it is heading. One can extract the yaw-angle from the current rotation matrix R as follows [99] cos(φ) =
q q T T T e1 Re1 + e2 Re1 , sin(φ) = (e2 Re1 )/ eT1 Re1 + eT2 Re1 .(5.35)
(eT1 Re1 )/
To gradually change the yaw to φh , one defines an intermediate state φr , φr (t0 ) = φ(t0 ). This state is changed by a regulating control-law φr − φh ˙ φr = −kφ sin , 2 107
(5.36)
where, kφ is kept low enough to achieve gradual change. Finally, Rr , which heretofore had a very near-zero yaw-component, is modified as cos(φr ) − sin(φr ) 0 Rφ = sin(φr ) cos(φr ) 0 , 0 0 1 Rr ← Rφ Rr .
(5.37) (5.38)
After its determination, HLC can command this Rr to the LLC along with suggested ˆ Independent manipulation of yaw in hover-mode is an nominal values of Tm , G. advantage. Remark 5.9 (Stability of the LLC and HLC combination). Under proper choice of control-gains, as detailed in Appendices B.1 and B.2, both HLC and LLC are individually exponentially-stable. On using Theorem 1.5, the resulting combined singularly-perturbed system is also exponentially-stable for high-enough time-constant separation. 5.3.3
Handling Constraints For safe operation, we would like to restrict the roll and pitch from the
direction of gravity, while computing Rr from Eq. (5.24). This is also needed to satisfy the condition stated in Remark 5.3. From Eq. (5.24) one can surmise that inclination from the gravity direction could be restricted by having a bound like
kt ∂Un
(5.39)
Rb ∂ρ + kv v ≤ βh mg, βh 1. From Eq. (5.25) and the non-increasing nature of the Lyapunov function Ws , we get an upper-bound on kvk as r kvk ≤
2kt . m
108
(5.40)
From Eq. (5.23), we get an upper-bound on the gradient of the potential. Eq. (B.38) provides an upper-bound on kv . Using triangle inequality on the LHS of Eq. (5.39), and using the aforementioned bounds, one obtains
∂Un
kt ∂Un k 2kt kt 2 t
Rb ∂ρ + kv v ≤ Rb ∂ρ + kv kvk ≤ Rb λu + Rb (1 + λu ).
(5.41)
Using the rightmost expression from the above equation, one can get a sufficient condition for the satisfaction of Eq. (5.39) as an upper-bound on kt kt ≤
βh mgRb λu . 2(1 + λu + λ2u )
(5.42)
We would like to quantify the effect of the parameter βh on the inclinationangle γi from the vertical, which the rotation-matrix Rr imparts to τs in Eq. (5.24). We take the worst case from Eq. (5.39) wherein kt ∂Un + kv v = βh mgˆ e, Rb ∂ρ
(5.43)
ˆ. This is illustrated in Fig. 5.3. We define γi to be for some arbitrary unit-vector e the angle between mge3 + βh mgˆ e and mge3 . As illustrated in Fig. 5.3, this angle is maximized for a given βh when the following orthogonality condition is satisfied ˆT (mge3 + βh mgˆ ˆT e3 = −βh . e e) = 0, ⇒ e
(5.44)
On using this condition, one gets ˆ + e3 ) eT (βh e cos(γi ) = 3 = ˆ + e3 k kβh e
q
1 − βh2 .
(5.45)
As an example, βh = 0.1 ⇒ γi = 5.73o . τs in Eq. (5.24) is very nearly vertical and by using Eq. (5.42), one can restrict the inclination imparted to it by Rr to a prespecified γi . This in turn bounds the roll and pitch of the helicopter. 5.4
Simulation Results The parameter values taken for the simulations are listed in Table 5.1. The
inertial, geometric and aerodynamic parameters have been taken from [47]. Fig. 109
Rr (τs)ss mgˆ e3 γi
βh mgˆ e
Figure 5.3: The HLC equation illustrated vectorially to show the definition of γi . ˆ along the surface of the dashed sphere, one By rotating the vector e ˆ is orthogonal to Rr (τs )ss . concludes that γi is maximized when e 5.1 shows the main blocks of the system and their connections. Fig. 5.4 shows the simulation results for a single bubble. To visualize the rotation matrix in Fig. 5.4(a), Euler angles, as defined in [57], have been used. They involve the rotation sequence: (i) yaw φ about z, (ii) pitch θ about y0 , and (iii) roll ψ about x00 . The LLC and HLC nonlinear solvers take about 6 iterations each for convergence. 5.4.1
Path-Following Planning and Control The controller given above can stabilize a helicopter to the origin of a spherical
bubble, while keeping it within the bubble at all times. Therefore, it is quite suitable for implementation on top of the spatial planning Algorithm 1.2.1. The output of this algorithm is a string of bubbles from the start to the goal point as shown in Fig. 1.2(b). The size of the start and end opaque bubbles is indicative of the relative size of the helicopter. The bubbles are of a prespecified minimum radius corresponding to a multiple of the helicopter size.
110
Table 5.1: Simulation Parameters of a Scaled Helicopter Model. m g [Ixx , Iyy , Izz ] 3 2 1 ] , lm , lm [lm 1 2 3 [lt , lt , lt ] Q Cm Q Ct Q Dm DtQ max(Tm ) min(Tm ) max(Tt ) min(Tm ) kr kˆw δ2 λf f Rb kv kt βh λu λs s ∆Tl ∆Th
4.9 9.81 [0.142413, 0.271256, 0.271492] [0.015, 0, −0.2943] [−0.8715, 0, −0.1154] 0.004452 0.005066 0.6304 0.008488 2mg 0.5mg 0.5mg 0 25 28.087 1/2 0.704 0.1913 2.0 0.891 0.131 0.1 0.8 0.0808 0.1757 0.03 0.3
111
Kg m s−2 Kg m2 m m √ m/√N m/ N Nm Nm N N N N N/(rad/s)
m N/(m/s)
s s
3.2 x y z
0.4 0.2
Pitch θ (deg.)
ξ / Rb = [x,y,z]T (m)
0.6
0 −0.2 0
10 20 time (s)
2.9 10 20 time (s)
30
10 20 time (s)
30
6 Yaw φ (deg.)
Roll ψ (deg.)
3
2.8 0
30
−2.05 −2.1 −2.15 −2.2 0
3.1
10 20 time (s)
4 2 0 −2 0
30
(a) The state time-trajectory. The initial velocity disturbance is v = [0.05, −0.05, 0.04]T , and there is an initial Cartesian offset. 2.44 2.42
48.1
Tt (N)
Tm (N)
48.15
48.05 48 0
2.4 2.38
10 20 time (s)
2.36 0
30
10 20 time (s)
30
10 20 time (s)
30
−0.6
−2.6
β (deg.)
α (deg.)
−2.4
−2.8
−0.7
−3 −3.2 0
10 20 time (s)
(b) Control inputs. range.
30
−0.8 0
All inputs are well within
Figure 5.4: Continuous-time simulation for a single bubble. The yaw angle φ is specified for the motion to be 5o .
112
Before this plan can be used for our controller, two preprocessing steps need to be performed to augment it 1. Each bubble’s radius has to be reduced by half the length of the helicopter. This gives us bubbles which represent the obstacle-free volume available for the motion of the center of mass of the helicopter. 2. Auxiliary bubbles need to be added to the plan so that the center of one bubble is completely enclosed by the next bubble and is sufficiently away from its boundary. The helicopter switches from bubble to bubble as it moves along the path. The result of this step is shown in Fig. 5.5, which should be compared to Fig. 1.2(b) which shows a preliminary plan. 5.4.2
Bubble Switching and Heading Direction Let us assume that the bubbles are indexed such that the start-bubble has
index N and the goal bubble has index 0. To come up with a stable bubble-switching, one uses the following procedure. Let us suppose that the helicopter is in bubble i with a gain kt which satisfies the upper-bound given by Eq. (5.42). Then, before it can be allowed to switch to bubble i − 1, it needs to satisfy the switching condition given by Eq. (5.25), where kt is kept constant across bubbles. This switching scheme leads to a smooth-motion as seen in Fig. 5.5. Other switching conditions based solely on geometry or velocity-norm are found to lead to much less smooth behavior. Note that Eq. (5.25) is guaranteed to be satisfied eventually within bubble i as the speed is decreasing towards zero, and the center of bubble i is within bubble i − 1. A composite Lyapunov function Wpath = (bubble-index) kt + Ws
(5.46)
is seen to be decreasing to zero across switches. This is because Ws < kt due to Eq. (5.25), and gain kt is kept constant for the whole path. This shows overall path-following stability. 113
To keep the helicopter facing its heading direction, we need to produce the reference signal φh for yaw control-law given in Eq. (5.36). This is simply achieved by taking the orientation of the vector ξbi−1 − ξbi in the Cartesian x, y plane, where ξbi is the center of bubble i. 5.4.3
Continuous vs. Discrete Control Simulations The continuous system has been shown to be stable. Fig. 5.6 shows a simu-
lation for this case. An animation of the resulting motion can be seen at [52]. To account for computation and actuator delay and flapping-angle dynamics, one would need to implement HLC and LLC such that they compute a new control input every ∆Th and ∆Tl seconds respectively. The inputs are held between samples, although the states evolve continuously. Fig. 5.7 shows a simulation for the discrete input case. ∆Th and ∆Tl have been chosen based on the exponential-decay constants λs and λf . These are introduced in Appendices B.1 and B.2 and depend on the inertial parameters as well as the gain constants. 5.5
Conclusions A new integrated path-planning and control scheme was proposed in this
chapter using a nonlinear hover-mode dynamic model for a helicopter. A two-time scale stabilizing controller is designed which is switched using a stable scheme for producing a path-following motion. This scheme is intuitive because the resulting motion is such that the helicopter always leans (pitches or rolls) in the direction of the motion. An independent yaw-control is provided to keep the helicopter facing in its heading direction. Control parameter and time-constant bounds are computed in terms of inertial quantities. Therefore, gain-selection can explicitly consider uncertainty in inertial parameters. Simulation results are provided for a scaled model
114
(a) LLC and HLC applied continuously
(b) LLC and HLC applied at sample intervals ∆Tl and ∆Th respectively.
Figure 5.5: The Cartesian path taken by the center of mass of the helicopter in the augmented plan. The start-point is on the right hand side. Notice the smooth transition of the path across bubbles. This shows the effectiveness of the switching-strategy.
115
0 −5 −10 0
100 time (s)
x y z 200
Pitch θ (deg.)
ξ / Rb = [x,y,z]T (m)
3.2
5
3.1 3 2.9 2.8 0
100 time (s)
200
100 time (s)
200
Yaw φ (deg.)
Roll ψ (deg.)
20 −2.05 −2.1 −2.15 −2.2 −2.25 0
100 time (s)
0 −20 −40 −60 −80 0
200
(a) The time-trajectory of the Cartesian coordinates and Euler angles corresponding to Fig. 5.5(a). 2.8 2.6
48.05
Tt (N)
Tm (N)
48.1
48 47.95 0
2.4 2.2
100 time (s)
2 0
200
100 time (s)
200
100 time (s)
200
−0.5
−2.6
β (deg.)
α (deg.)
−2.4
−2.8 −3 −3.2 0
−0.6 −0.7 −0.8
100 time (s)
200
−0.9 0
(b) The time-trajectory of the inputs corresponding to Fig. 5.5(a).
Figure 5.6: Continuous-time simulation for a full path, with bubble-switching. Both HLC and LLC are running continuously. Notice how the helicopter tries to turn (yaw) back initially as that is the direction of its initial motion. The last bubble is prescribed an extra yaw of 10o .
116
0 −5 −10 0
100 time (s)
x y 200 z
Pitch θ (deg.)
ξ / Rb = [x,y,z]T (m)
3.2
5
2.9 100 time (s)
200
100 time (s)
200
50 Yaw φ (deg.)
Roll ψ (deg.)
3
2.8 0
−1.8 −2 −2.2 −2.4 −2.6 0
3.1
100 time (s)
0 −50 −100 −150 0
200
(a) The time-trajectory of the Cartesian coordinates and Euler angles corresponding to Fig. 5.5(b). 3.5 3
48.05
Tt (N)
Tm (N)
48.1
48 47.95 0
2 100 time (s)
1.5 0
200
100 time (s)
200
100 time (s)
200
0
−1
β (deg.)
α (deg.)
0
−2 −3 −4 0
2.5
100 time (s)
200
−0.5 −1 −1.5 0
(b) The time-trajectory of the inputs corresponding to Fig. 5.5(b).
Figure 5.7: Controllers applied discretely to a continuous model. The controlinputs are held constant during the sample interval. Both HLC and LLC are running discretely with sample-times ∆Th and ∆Tl respectively. Note the rapid but bounded changes in inputs which were absent in the continuous-time simulation. The last bubble is not prescribed any additional yaw, and hence it goes to zero.
117
for continuous and discrete input application. More work needs to be done to investigate the robustness of this control approach to wind disturbances, which were assumed to be negligible for hover-mode operation.
118
Chapter 6 A TIME-BASED ANALOG FOR SWITCHED POTENTIALS USING MODEL PREDICTIVE CONTROL
6.1
Introduction The bubble-switching strategy to move a robot forward along an obstacle-
free path, while satisfying certain constraints, can be thought of as a space-based approach since no explicit time-parameterization of the path is done. Note that a bubble represents a stabilizing region for the system. During motion, however, the bubble is switched before the system has time to stabilize. This switching is based on an energy-like function to keep the overall behavior stable, and was illustrated in Chapters 3 and 5. In this chapter, a time-based analog of this idea is presented, wherein the physical bubble is replaced by a planning time window or horizon ∆Tp . This idea is known as Model Predictive Control (MPC), which in a nonlinear setting may be abbreviated as NMPC. The basic idea is to formulate a plan for horizon ∆Tp and apply the planned inputs to the system for a control horizon ∆Tu ≤ ∆Tp , after which the planning is done again, and the process repeats itself. The planning step involves solving a numerical optimization problem using mathematical programming, taking the system dynamical model into consideration. NMPC has the advantage of taking system state and input constraints directly into consideration. It has traditionally been applied to slow chemical systems [11, 10, 25, 24], where computation time is not a major issue. Applications of NMPC to mechanical systems have suffered 119
from the bottleneck of computation-time [102]. An implementation of MPC-like trajectory-tracking strategy for a group of mobile robots was done in [77]. However, in this case, the robots were of unicycle-type and did not need to be dynamically stabilized. In this chapter, we focus on extending NMPC to dynamically stabilized mobile robots which were presented in Chapters 4 and 5. For such systems in particular, computation time is of paramount importance. As shown in the formulation of Sec. 2.4, for many such systems, the system-state can be naturally divided into slow and fast varying components. Usually, the slow component is translation and the fast component is rotation. Using the presented strategy, one could use NMPC for a slow reduced-state subsystem, while keeping the faster substate under the control of an exponentially fast controller. The basic idea, as was introduced in Fig. 2.4, is to use fast-state as a pseudo-control-input to make the slow-state track its desired trajectory. To incorporate MPC in this scheme, it is modified, as depicted in Fig. 6.1. The rest of the chapter is organized as follows: Sec. 6.1.1 modifies the canonical structure introduced in Sec. 2.4, to accommodate MPC. Sec. 6.2 presents a formulation for computation of an optimal trajectory using band-limited interpolants. Sec. 6.3 presents a detailed worked-out example of an inverted wheeled pendulum, wherein the trajectory planning and tracking technique is applied in simulation. In Sec. 6.4, an NMPC formulation is presented which explicitly takes computational delay into account. Simulation results show the viability of this procedure. Certain limitations of this procedure, having to do with computational time delay, are pointed out in the concluding Sec. 6.5. These limitations also serve as further justifications for the space-based approach followed in Chapters 3 and 5.
120
Controller
MPC Planner
Planned Input for ∆Tp ˆf y
Band-limited
ˆf ) u = γf (yf − y
Trajectory Planner
ys
Fast Controller Cf
yf MPC Feedback
u
System yi , i = 1 . . . Nf ys
Feedback
Figure 6.1: Schematic of the MPC Planner/Control Strategy. The Model Predictive Control (MPC) loop is shown in dashed-line. The planner ˆ f (t) as pseudo-input. computes a band-limited trajectory for ys with y ˆ f (t) is then tracked by the exponential controller Cf . y 6.1.1
System Formulation The reader is referred to Sec. 2.4 for a description of the terminology in this
ˆ f ) → u has section. We assume that an exponential tracking-controller Cf : (yf − y been found for the subsystem given by Eq. (2.49), which regulates the system-error ˆ f (t) to 0 with a small time-constant of ∆Tf seconds, for a given reference yf (t) − y ˆ f (t). trajectory y ˆ f conIn the terminology of Singular Perturbation theory [38], zf = yf − y stitutes the boundary-layer state. The reduced-model can now be given as ˆ f , Cf ) . y˙ s = gs (ys , y
(6.1)
The overall modified control-scheme is shown in Fig. 6.1. The potential field based slow-state controller of Fig. 2.4 has been replaced by an MPC-based trajectoryplanner in Fig. 6.1. It plans a dynamically feasible trajectory for the system, conˆ f are then sidering the fast states yf as pseudo-inputs. The planned pseudo-inputs y followed by an exponentially fast controller Cf with a time-constant ∆Tf seconds. ˆ f (t). Let fsm be the maximum frequency contained in the planned pseudo-input y
121
For appropriate tracking, the Sampling Theorem [80] provides the guideline that, ∆Tf ≤ 1/(2fsm ). The frequency band-limitedness requirement can be met by posing the optimization problem in terms of band-limited interpolants, as described in Sec. 6.2.1. By Tikhonov’s theorem [38], the exponential convergence of the boundary-layer model then guarantees that the slow-state remains close to its desired planned trajectory, for finite durations of time. To accommodate for drift of the slow-system, the planning step is repeated in a Model Predictive Control (MPC) manner [74]. 6.2
Planned Trajectory Computation The basic idea is to formulate the problem of trajectory-planning as solution
of a numerical Nonlinear Programming (NLP) problem for which efficient readymade routines exist. This section outlines the steps involved in this formulation. For the purpose of planning, we deal with only the reduced-model given by Eq. (6.1). The objective is to find a dynamically consistent trajectory of ys , along with a ˆ f , such that certain objective-function is minimized. This can be pseudo-input y expressed as min JN (ˆ yf (t), ys ) ≡ ˆ f (t) y
Z
tN
L(ˆ yf (t), ys )dt + F (ys (tN ))
(6.2)
ti
subject to initial conditions ys (ti ) = ys0 , dynamics given by Eq. (6.1) and any other constraints on inputs and states. The duration ∆Tp = tN − ti is called the planning horizon. The integrated cost L depends on the path-taken while the cost F depends on the final state only. If this optimization is repeated every ∆Tc < ∆Tp , while updating the initial-condition ys0 from online sensor measurements, this formulation is termed Nonlinear Model Predictive Control (NMPC). The reader is referred to [24, 60, 11] for further details. A common technique for solving the above optimization problem is by orthogonal collocation on finite elements. The planning horizon is divided into N intervals, 122
and variables of interest are discretized on these sample points. In our case, the variˆ f and ys and their derivatives. Let z be a variable of interest. ables of interest are y Defining ∆Ts = ∆Tp /N , let this variable be discretized as zk|i ≡ z(ti + k∆Ts ), k = 0 · · · N.
(6.3)
For any time-instant t ∈ [ti , tN ], the value of z(t) is given by an interpolation z(t) =
N X
(6.4)
zk|i Ik (t),
k=0
where Ik (t) is an interpolating function. One observes that 1. Given tj ∈ [ti , tN ], the above expression can now be readily integrated as Z
tj
z(t)dt = z(ti ) +
wj|i = t=ti
N X k=0
Z
tj
Ik (t)dt.
zk|i
(6.5)
t=ti
This incorporates the initial condition z(ti ). Note that integral of the interpolating function only depends on the grid-spacing and not on the sample-values zk|i . 2. A given nonlinear function f (z) defined on [ti , tN ], can also be computed at the collocation points tk by fk|i = f (z(ti + k∆Ts )) , k = 0 · · · N.
(6.6)
An estimation of the value f (t) can then be obtained by using Eq. (6.4) with z replaced by f . ˆf . We choose to discretize the highest derivatives of variables involved viz. y˙ s and y From these, lower-order variables may be obtained by the integration process given in Eq. (6.5). We define the optimization parameter-vector p = [pk|i ], k = 0 · · · N ,
123
where pi|k = [(y˙ s )k|i , (ˆ yf )k|i ]. The optimization problem given in Eq. (6.2) can now be stated as an NLP as Z tN N X min JN (p) = Lk|i (p) Ik (t)dt + FN |i , p ti k=0 subject to: (y˙ s )k|i = (gs )k|i (pk|i ), k = 0 · · · N,
(6.7)
and any other constraints as required. 6.2.1
Band-limited Sinc Interpolant The most common choice for the interpolant Ik (t) in Eq. (6.4) is to use
polynomials like local splines or the well known Lagrange-polynomial ([11, 10]) on equal or unequal intervals. The Lagrange polynomial is given by Ik (t) =
LN k (t)
=
N Y
t − tj . t k − tj j=0, j6=k
(6.8)
The problem with polynomial interpolation is that as the number of collocation intervals increases, the interpolation becomes highly oscillatory between sample points. There is also no frequency band-limit associated with polynomials. This is shown in Fig. 6.2. Due to our requirement of a band-limited planned-trajectory for xs (t), as explained in Sec. 6.1.1, we resort to a different interpolant: the Sinc, or Cardinal function [107], which is defined as ST (t) =
sin(πt/T ) . πt/T
(6.9)
Note that ST (0) = 1. ST (t) is a band-limited interpolant, in the sense that its Fourier Transform SˆT (ξ) is such that SˆT (ξ) = 0, ∀ ξ 6∈ [0, 2π/T ]. In terms of Eq. (6.9), the interpolant given in Eq. (6.4) can be defined as Ik (t) = S∆Ts (t − tk ). Other salient properties of this function are ([56], [61]): 124
(6.10)
6 4
f(x)
2 0 −2 −4 −6 −1
−0.5
0
0.5
1
0.5
1
x
a
Error= f(x)− f (x)
15 Samples Sinc Polynomial
10 5 0 −5 −10 −15 −1
−0.5
0
x
Figure 6.2: Comparison between Sinc and polynomial approximation. The upper figure shows the function f (x) being interpolated with the samples shown by circles. The lower figure shows the error in approximation f (x) − fa (x). The dotted line shows the polynomial-approximation which has big oscillations near the boundary points. The Sinc approximation error is close to zero.
125
N 1. limN →∞ LN i (t) = ST (t − ti ), where Li is the Lagrange polynomial introduced
in Eq.(6.8) and ti is the i-th sample instant [61]. Similar results can be obtained for Spline interpolations. In this sense, the Sinc function is an ideal interpolator [80]. 2. In Eq. (6.5), we need to compute the integral of Ik (t). If Ik (t) is the Sinc function given in Eq. (6.10), there exist efficient algorithms for evaluating this integral [79]. Henceforth, we use the Sinc function as the interpolant of choice. In the following, we illustrate the above procedure with the example of an inverted wheeled pendulum. 6.3
Inverted Wheeled Pendulum The procedure outlined in the last section is now illustrated by applying
it to an inverted wheeled pendulum. The dynamical model of an inverted wheeled pendulum was introduced in Sec. 4.2. A partial feedback linearization of this system was done by a series of coordinate transforms in Sec. 4.4. As a result of this, the subsystem composed of the vehicle pitch α, and yaw θ becomes linear and can be directly controlled. The remaining subsystem consists of nonlinear zero-dynamics. The linear subsystem can be controlled by a linear controller Cf with prescribed time-constants, as shown in Sec. 4.5.1. In terms of our canonical structure given by Eqs. (2.49) and (2.50), we identify, yf = [α, θ]T , and ys = [z7 , xo , yo ]T . Eq. (4.42) for θ and Eq. (4.43) for α represent a linear subsystem, through which these two states can be directly controlled by the aforementioned exponential controller Cf . 6.3.1
Trajectory Planning Simulation Results Assume first that we need a dynamically consistent optimum trajectory from
t = 0 to t = tf = N T seconds. This constitutes one step of the MPC paradigm. 126
The optimization problem is formulated as in Sec. 6.1.1. ¨ and w2 = α 1. Discretize virtual inputs w1 = θ, ¨ (Eqs. (4.32), (4.33), (4.42), and (4.43)) at the N +1 collocation points. Although according to the methodology described in Sec. 6.1.1, this is enough, we additionally also discretize v which is a function of z7 to avoid solving nonlinear equations at every time-step. The parameter-set p is given by
θ¨0|0 , · · · θ¨N +1|0
p3×(n+1) = α ¨ , · · · , α ¨ 0|0 N +1|0 v0|0 , · · · , vN |0
(6.11)
At any time t during the planning interval, the value of w1 , w2 or v can be found by using the interpolation formula Eq. (6.4), with bounded limits on the summation. Furthermore, the signals w1 , w2 can be integrated twice using ˙ α, α. Eq. (6.5), given the initial values for θ, θ, ˙ We denote the initial Cartesian position of the vehicle center O as the origin. Now the goal is to find a trajectory which ends close to a given point (xg , yg ). The system starts from rest and goes to rest. The final pitch angle is taken to be 0, else the system cannot come at rest. 2. Using the above computed values of θ(ti ) and the collocated v(ti ), one can compute x˙ o (ti ) = v(ti ) cos(θ(ti ))
(6.12)
y˙ o (ti ) = v(ti ) sin(θ(ti ))
(6.13)
These samples can again be interpolated by Eq. (6.4) and then integrated using Eq. (6.5) to get the value of the final value of the xo (tf ), yo (tf ), achieved for a given parameter vector p. The distance of this point from the goal point
127
serves as an error to be minimized during the optimization. This serves as the terminal cost F in Eq. (6.2).
(xo (tf ) − xg )
F (xs (tf )) =
(yo (tf ) − yg )
(6.14)
3. All the computed samples of states for a given parameter vector p are then substituted in Eq. (4.44), at all the collocation points. The degree to which this equation is satisfied at these points represents the degree to which the planned trajectory is dynamically consistent. Therefore, this is applied as a nonlinear equality vector constraint during optimization. Let these equality constraints be collectively be denoted by E = 0. 4. The limits on pitch angle during motion are applied as nonlinear inequality constraints during optimization. To get the actual motor torques one has to apply the reverse input feedback laws w → v → τ /u given by Eqs. (4.23), (4.32), (4.33). Any specified torque limits can also be applied as nonlinear inequality constraints. Let these inequality constraints be collectively be denoted by I < 0. 5. The integral cost term L in Eq. (6.7) is chosen to be Lk|0 = |vk|0 |. This penalizes distance along the path taken by the vehicle. The optimization problem can now be defined as Z tf N X min J(p) = |vk|0 | ST (t − tk )dt + F (xs (tf )). p t=0 k=0
(6.15)
E(p) = 0,
(6.16)
I(p) < 0.
(6.17)
subject to:
This problem can be solved by a standard SQP numerical optimization package. The resulting vector p can then be used to compute time trajectories for 128
all the state variables. These trajectories are dynamically consistent within discretization error. Remark 6.1 (Linear Controller Cf ). After a trajectory has been planned, it can be followed by simply constructing a linear controller for the states α and θ. Since the planned trajectory does not have any frequencies higher than 2/T Hz., the gainconstants of the linear controller kp and kv are selected in such a way that its timeconstant is a fraction of T /4. The controller equations are the same as those given in Sec. 4.5.1. This controller tracks the planned reference trajectories θr and αr . 6.3.2
Simulation Results For the simulations, the values taken are listed in Table 6.1. A settling time
analysis of the linear controller with the values of the gain constants as listed, shows that the tracking error amplitude decays to 0.0001 within ∆Tf = 0.3 sec. Referring to Fig. 6.3, there is a time-period when the pitch angle is positive. This corresponds to a forward acceleration as seen from the graph of v(t). Similarly a negative pitch angle corresponds to a deceleration phase. Fig. 6.6 shows the tracking performance of the controller with the control gains as tabulated in Table 6.1. This is obtained by applying the controller to the dynamic equations of the model, with the planned reference trajectory as the input to the controller and the system state being fed back. As can be seen, the planned and actual trajectory are the same within the accuracy of the plots. 6.4
NMPC Framework Most traditional NMPC frameworks currently ignore the computational de-
lay. While this is practical in slow chemical systems, it cannot be justified in faster mechanical systems. Fig. 6.7 shows a reformulation of the traditional NMPC. We assume that an upper-limit of the computation-time required to compute a plan of planning-horizon ∆Tp has been found to be ∆Tc seconds. Additionally, we assume 129
0.02
1
0.01 α (rad)
y (m)
1.5
0.5 0 −0.5
0 −0.01
0
0.2
0.4 x (m)
0.6
0.8
−0.02
10
15
10
15
0.3 0.2
1
v (m/s)
θ (rad)
5 t (s)
1.5
0.5
0
0
0.1 0
0
5
10
15
t (s)
−0.1
0
5 t (s)
Figure 6.3: The planned x − y path, and the time-trajectories of α (rad), θ (rad), and v (m/s). The circled points show the collocation locations which are joined smoothly by the Sinc interpolant. The figure shows the optimal-path (straight line) found after 40 iterations of Sequential Quadratic Programming (SQP), in about 40 seconds on a 2 GHz. computer using MATLAB. The planning-horizon was 15 seconds.
130
Table 6.1: NMPC Simulation Parameters Mb Mw R cz b Ixx Iyy Izz Iwa Iwd αm kp kv T N xg yg
35.00 5 0.25 R 0.20 2.1073 1.8229 0.6490 0.1563 0.0781 π/9, π/4 828.42 46.052 0.5 30 0.5 1.3
Kg Kg m m m Kg m2 Kg m2 Kg m2 Kg m2 Kg m2 rad N/rad N/(rad/s) s m m
1.5
0.04
0.02
α (rad)
y (m)
1 0.5
0
0 −0.5 −0.2
0
0.2 x (m)
0.4
0.6
−0.02
10
15
10
15
0.3 0.2
1
v (m/s)
θ (rad)
5 t (s)
1.5
0.5
0
0
0.1 0
0
5
10
15
t (s)
−0.1
0
5 t (s)
Figure 6.4: This figure shows the result obtained after 10 iterations of SQP in 7 sec. This shows that practical results maybe obtained with nominal effort, if suboptimal solutions are acceptable. 131
1.4
1.2
Normalized Torque
1
0.8
0.6
0.4
0.2
0
0
5
10
15
t (s)
Figure 6.5: The normalized planned motor-torques kτ k/τm , τm = 0.7 Nm. This graph corresponds to Fig. 6.3. The normalized torque is seen to remain less than unity except for a short wiggle. availability of enough computation-power to assure ∆Tp > ∆Tc . The notation xs (ti ) means the actual value of state xs sensed at t = ti . x ˆs (ti|j ) denotes the value of xs (ti ) planned at the j-th step. The following steps are involved in the procedure 1. The system is kept stationary for initial time 0 − ∆Tc , while the first plan is generated. This can be easily done if the system starts from rest and the controller Cf is given a zero-reference to follow. 2. A plan is generated at discrete time-steps which are integer multiples of ∆Tc . At planning-instant ti = i∆Tc , a plan iP is generated to run the system from ti+1 = (i + 1)∆Tc to tf = ti+1 + ∆Tp . While this plan is being computed the system is using the last generated plan to run. The rear-end of each plan is discarded due to replanning.
132
−4
4
0.1
2
x 10
α error
x
Error , Error
y
0.2
0 −0.1 −0.2
0 −2
0
5
10
−4
15
0
t(s) −3
1
x 10
1
v error
θ error
0.5 0 −0.5 −1
0
5 10 time (sec)
15
5 10 time (sec)
15
−3
5 10 time (sec)
15
x 10
0
−1
−2
0
Figure 6.6: The tracking-error performance of the controller given in Eq. (4.49) corresponding to Fig. 6.3. Note that the error in α, θ, v remains quite small. The x, y error increases in amplitude towards the end, though its final value is 0. In a typical MPC implementation, the last part of the plan is usually discarded and replanning is done periodically. Therefore the Cartesian error towards the end can be tolerated.
133
∆Τp+3∆ ∆Tc
x
∆Τp+2∆ ∆Tc ∆Τp+∆ ∆Tc
t) x( P0
0
∆Tc
P2
xest P1
2∆ ∆Tc
3∆T ∆Tc 3∆ c 4∆
5∆ ∆Tc
Figure 6.7: The first few steps of MPC shown for a particular component x of the state-vector. The computation time required is denoted by ∆Tc . A new plan P i is computed between i∆Tc to (i + 1)∆Tc , to run the system from (i+1)∆Tc to (i+1)∆Tc +∆Tp , where ∆Tp is the planning horizon. In the figure, the system has run till 2∆Tc . The system was kept stationary from t = 0 to ∆Tc by setting the reference of the tracking-controller Cf to 0. Every plan computed at and after 2∆Tc requires an estimation of the state at the start of the next planninginstant. This estimation is shown by dotted lines xest .
134
3. At ti , one needs to estimate the state of the system at ti+1 , given the sensed current slow-state value xs (ti ). This estimation step is missing in traditional NMPC. It can be done in various ways. We assume that since the fast states xf are being followed by a fast controller, they will reach the values predicted by the previous plan (i − 1)P . To predict the slow state xs (ti+1 ), we take the current error offset ∆e between the sensed xs (ti ) and that predicted by the last plan (i − 1)P . Therefore, ∆e = xs (ti ) − x ˆs (ti|(i−1) ). Then assuming that this offset would not be significantly altered within ∆Tc , xs (ti+1 ) ≈ x ˆs (t(i+1)|(i−1) )+ ∆e. This then is the initial slow-state for the next plan iP . 4. The above step computed the initial state for plan iP . To get the final state we assume that the user has enumerated way-points for xs which are tabulated ∆Tp apart. For the inverted wheeled pendulum, this could be simply the tabulated x, y coordinates of robot. At planning-instant ti , the final state can be given by the interpolation of the way-points to the time ti + ∆Tc + ∆Tp . 5. To ensure stability, we make a plan such that the system always goes to rest at the end of the planning-horizon. This means that, if no new plan is made before the expiry of the current planning-horizon, the system comes to a halt and stays there. If a new-plan is made before the expiry of the current planninghorizon, the system simply switches to that plan without coming to a stop. Thus, jerky motion is avoided while ensuring safety. In the space-based approach, this is analogous to having the system asymptotically stabilize and stop within its current bubble, if no bubble-switching occurs. A realistic simulation of the strategy outlined above is shown in Fig. 6.8. It shows the first four plans, and part of the system’s actual trajectory. As the ratio ∆Tc /∆Tp reduces, the feedback delay in xs also reduces, which results in better tracking of the planned-trajectory. 135
3
x (m)
2 1 0 −1
0
5
10
15
20 25 time (sec)
30
35
40
45
0
5
10
15
20 25 time (sec)
30
35
40
45
0
5
10
15
20 25 time (sec)
30
35
40
45
3
y (m)
2 1 0 −1
0.04
α (rad)
0.02 0 −0.02 −0.04
Figure 6.8: The first 4 steps shown for the NMPC applied to the inverted wheeledpendulum example. ∆Tc = 7.5 seconds and ∆Tp = 15 seconds. The solid curve is the actual system trajectory till t = 32.5 seconds. The four plans’ sample-points are depicted by variously shaped markers to distinguish them. The start-time for the plans are depicted by vertical lines. The discarded portion of each plan is also shown. Note that the end-point for a plan slides along a user-specified way-point trajectory. α is shown to follow the plan closely. The simulation-parameters are as shown in Table 6.1, except that the values of the model Iyy , Izz and cz have been perturbed by 1.5%.
136
6.5
Conclusions The main contribution of this chapter is to formulate a general technique for
optimum trajectory planning and tracking for a certain class of dynamically stabilized systems. This method uses the time-separation of states, naturally found in such systems, to its advantage. A band-limited optimum path is planned for slow-states using the fast-states as pseudo-inputs. These pseudo-inputs are then followed by an exponentially fast controller for fast-states. Using currently available computation power, the planning procedure outlined is able to plan a trajectory in less than half the time of the planning horizon. Simulation results of a modified NMPC scheme are presented, which takes finite computational-delay into consideration. This method, therefore, offers a method for application of Nonlinear Model Predictive Control (NMPC) to dynamically stabilized autonomous robots. A parallel may be drawn to the space-based planning and control strategies presented in earlier chapters. In NMPC, a bubble in space is replaced by a planning time window. Note that due to finite computation time required for solving the optimization inherent in MPC, an estimation step, as illustrated in Fig. 6.7, is needed. This step is a major source for inaccuracy within this approach. Furthermore, the optimization is not guaranteed to converge and the computation time may show considerable variance across runs. These observations may be taken as further justifications for the space-based approach presented in Chapters 3, 4, and 5.
137
Chapter 7 CONCLUSIONS This dissertation has investigated the application of switched potential fields for motion planning and control of a certain class of nonholonomic and underactuated ground and aerial autonomous mobile robots. 7.1
Summary Chapter 1 provided an overview of the literature pertinent to the follow-
ing topics: path-planning and trajectory-tracking, potential fields for planning and control of mobile robots, stabilization of nonholonomic systems, and control of underactuated systems. The primary research questions were enumerated in Sec. 1.6. The motion of a nonholonomic planar robot under a local potential was characterized in Chapter 3. Several bounded potentials were constructed for a differentially driven vehicle, and a stable energy-based controller switching strategy was derived. Chapter 4 showed how potential fields may be combined with a state timescale decomposition to control a planar differentially driven robot with a non-planar unactuated degree-of-freedom. Chapter 5 demonstrated the same idea in a spatial setting for an autonomous helicopter. All these examples involved path-following control under specified speed and/or input constraints, with no explicit time parameterization of the path. Chapter 6 introduced a time-based approach based on Model Predictive Control. This approach used band-limited trajectory-generation and tracking for underactuated mobile vehicles. Certain shortcomings of this method, arising from computational delay, were highlighted. Thus, this time-based analog 138
formulation served to further highlight the advantages of the potential field based path-following approach of previous chapters. 7.2
Contributions The primary research questions were defined in Sec. 1.6. In this section,
these questions are revisited and their connections to the results obtained in this work are highlighted. The numbering of the following list directly corresponds to the numbering of the primary questions given in Sec. 1.6. The contributions of this dissertation are discussed in the following: 1. The behavior of a general Euler-Lagrange nonholonomic system controlled by a local potential field was characterized in Proposition 2.1. This was done by designing a potential based controller and finding an expression for the system’s invariant manifold. 2. For underactuated vehicles, Sec. 2.4 identified a canonical form for a class of systems to which the the idea of time-scale separation of states could be amalgamated with potential-fields for vehicle navigation. 3. A strategy for causing stable motion along a globally planned path using bounded switched potentials was derived in Secs. 2.3.2 and 2.3.3. It was identified that this strategy was applicable to a general class of systems of type given by Eq. (2.9) with an additional condition given in Eq. (2.10). (a) A stable energy-based switching condition was found which causes smooth motion and also guarantees satisfaction of input/speed constraints. (b) The main advantage of this approach over traditional approaches is that no a priori path time-parameterization and time-scaling was required to satisfy dynamical, speed and input constraints. As shown in Chapter 6,
139
this is a time-consuming step which is a bottle-neck for a real-time implementation, especially on dynamically balanced vehicles. (c) The approach decouples geometric feasibility satisfaction from dynamical constraint feasibility satisfaction. Traditional approaches do not handle sudden stops well. Within the presented approach, the system is always kept stable due to the locally stabilizing nature of the control law. If no new geometrically feasible bubbles are found, the system just stops within its current bubble and stays stable. 4. The approach is integrated with an existing path-planning approach which works with obstacle-free regions (bubbles). As the vehicle motion is restricted to a bubble at a time, a higher-level planner is able to plan in terms of these geometrically feasible omnidirectional bubbles, and ignore the complicated vehicle dynamics. 5. The approach was shown to work in both planar and spatial cases. Several realistic examples were solved. Experimental results were provided for a differentially driven two wheeled vehicle. Simulation studies were done for a planar Inverted Wheeled Pendulum and a spatial hover-mode Helicopter. With the advent of cheap computation power, logic-based switching of controllers is here to stay. This dissertation provides one such way of controlling the motion of a class of vehicles which retains the intuitive nature of potential-fields. 7.3
Assumptions and Limitations Some underlying assumptions and simplifications employed in this disserta-
tion are the following:
140
1. For ground vehicles, nonholonomic constraints arise due to the no-slip condition at the wheel contact point. Therefore, this work did not consider the presence of slippery or uneven terrain. 2. For the hover-mode helicopter model employed in this work, disturbances arising from strong winds were not considered. 3. Elongated robot geometries were not considered. While the method of this dissertation may still be applied in this case, it may become inefficient. 4. Multi-robot reactive motion examples were presented only to show that this approach may be embedded in more complex scenarios. No mathematical proofs were provided for multi-robot cooperative motion. 7.4
Suggestions for Further Work This work shows the promise of being extendible in the following directions:
1. Incorporation of more vehicle-types within the scheme. Submarines and certain space-robots seem to be promising candidates for this approach. 2. Embedding this scheme within other multi-robot frameworks which may assume a simpler model of robot dynamics. Even with an omnidirectional, chain of integrators type dynamic model, the problem of reactively moving n robots to their respective goals without collision, which provably works, still remains challenging. Certain heuristics have emerged but the challenge of designing a simple provable constructive approach remains. 3. Extending the potentials to more complex geometries. Complex domain transforms may be explored for this. 4. For vehicles with nonholonomic inequality constraints, like vehicles with steering angle limits, the path-planning approach used in this work may not provide 141
feasible solutions. Suitable modifications in the scheme for this problem need to be explored. 5. Rough terrain effects need to be explored for ground vehicles. While more complex vehicle models have been derived [96], real-time implementation of trajectory planning and tracking remains a challenge due to complexity and computation-time delay. 7.5
Concluding Remarks This dissertation has successfully demonstrated that the visual and intu-
itive appeal of potential field theory is not completely lost in the presence of nonintegrable rate constraints and underactuation. This work provided constructive methods for design of local potentials, and stable switching strategies for the motion control of a class of autonomous robots, under additionally specified speed and input constraints.
142
Appendix A INVERTED WHEELED PENDULUM DYNAMICS Detailed expressions from Sec. 4.2. Dα ≡ Mb 2 cos2 (α)cz 2 R2 +
Gα
−Mb 2 − 2Mw Mb cz 2 − 2Iyy Mw − Iyy Mb R2
−2Mb cz 2 Iwa − 2Iyy Iwa ≡ −Mb cz 2 + Izz − Ixx R2 cos2 (α) + Mb cz 2 + Ixx + 2Iwd + 2b2 Mw R2 + 2b2 Iwa g(x) = [g1 (x), g2 (x)]
(A.1)
(A.2) (A.3)
04×1 Mb R2 +2 Mw R2 +2 Iwa +Mb cos(α)cz R Dα g1 (x) = R(Mb cos(α)cz R+Iyy +Mb cz 2 ) − Dα
(A.4)
Rb Gα
04×1
Mb R2 +2 Mw R2 +2 Iwa +Mb cos(α)cz R Dα g2 (x) = R(Mb cos(α)cz R+Iyy +Mb cz 2 ) − Dα
(A.5)
− GRbα
¯ ≡ 1/2Mb R2 Izz + Iwa Izz − Mw R2 Ixx H −Iwa Ixx − Mb cz 2 Mw R2 − Mb cz 2 Iwa −1/2Mb R2 Ixx + Mw R2 Izz
(A.6)
Kα (4Dα ) ≡ (−4Iyy Mb R2 cz − 3R2 Mb 2 cz 3 + Mb R2 cz (Ixx − Izz )) sin(α) +(Mb R2 cz (Ixx − Izz ) + R2 Mb 2 cz 3 ) sin(3α) 143
(A.7)
¯ sin(2α)θ˙2 H Mb 2 cz 2 R2 sin(2α)(α) ˙ 2 f2 [1](x) = +1/2 Dα Dα | {z } ˙
θ f21
−2Mb 2 R2 cz − 4Iwa Mb cz − 4Mw R2 Mb cz g sin(α) + 1/2 (A.8) Dα | {z } α f21
f2 [2](x) = Kα θ˙2 + 1/2 | {z } | ˙ fθ 22
Mb 2 cz 2 R2 g sin (2 α) D {z α } α f22
−4 Iyy Mb R2 cz − 4 R2 Mb 2 cz 3 sin (α) (α) ˙ 2 +1/4 Dα 2 2 2 (−(Ixx − Izz )R − Mb cz R ) sin (2 α) α˙ θ˙ f2 [3](x) = Gα 2 sin(α)R Mb cz v θ˙ − Gα
144
(A.9)
(A.10)
Appendix B PROOFS RELATING TO HELICOPTER DYNAMICS
B.1 B.1.1
Rotational Dynamics Miscellaneous Results
Proof of Lemma 5.1. One starts with the implicit definition of sδ given Eq. (5.7) and obtains the other expressions by the following steps T
Ω sδ =
3 X
ei T I3 − RrT R S(Ω)ei
i=1
= − ⇒ sTδ = −
3 X i=1 3 X
ei T I3 − RrT R S(ei )Ω ei T I3 − RrT R S(ei )
(B.1)
i=1
where S(ei ) is the skew-symmetric cross-product matrix for the i-th unit-vector ei . Note that S T (ei ) = −S(ei ) and S(ei )ei = 0. These can be used to modify Eq. (B.1) to get Eq. (5.8). This expression of sδ given in Eq. (5.8) can be directly algebraically expanded to yield Eq. (5.9). On combining Eq. (5.9) with the property given by Eq. (5.6), one obtains Eqs. (5.10) and (5.11). Corollary B.1. For later steps we derive the derive an expression for s˙ δ . Starting from Eq. (5.8), one gets 3
s˙ δ =
3
X d d X T R Rr ei × ei = − ei × R T Rr ei dt i=1 dt i=1 145
= =
3 X i=1 3 X
3 X ei × S(Ω)RT Rr ei = ei × Ω × R T Rr ei i=1 3 X T T ei · R Rr ei Ω − (ei · Ω)R Rr ei = ei · R T Rr ei Ω − R T Rr Ω
i=1
i=1
= γΩ − Rq Ω,
(B.2)
γ ≡ trace(Rq ) ≤ 3.
(B.3)
Lemma B.1. Define 1 Vs ≡ ksδ k2 , 2
(B.4)
then given the control-law for LLC, ∃ δ ∈ (0, 1] s.t. 2Vr ≥ Vs ≥ 2δ 2 Vr .
(B.5)
Proof. Note that under the LLC control given by Eq. (5.12), the Lyapunov function W = kr Vr + Vk is non-increasing, where Vk is the rotational kinetic-energy. Also kr was selected initially such that 4kr > kr Vr (t0 ) + Vk (t0 ). Then ∃ θδ s.t. 4kr > 2kr (1 − cos(θδ )) = kr Vr (t0 ) + Vk (t0 ). This shows that ∀t > t0 , 2(1 − cos(θδ )) ≥ Vr (t). Using Lemma 5.2, one gets 1 − cos(θδ ) ≥ 1 − cos(ϕ(Rp )), ⇒ 1 + cos(θδ ) ≤ 1 + cos(ϕ(Rp )).
(B.6)
Furthermore, combining Eqs. (5.11) and (5.17), one gets Vs = Vr [1 + cos(ϕ(Rp ))] ≤ 2Vr .
(B.7)
Combining Eq. (B.6) in above, one gets Vs = Vr [1 + cos(ϕ(Rp ))] ≥ [1 + cos(θδ )]Vr ≡ 2δ 2 Vr .
(B.8)
This proves the result. We can also write 2δ
p
p Vr ≤ ksδ k ≤ 2 Vr
146
(B.9)
Remark B.1. Combining the above results with Eqs. (5.13) and (5.18), one concludes that 4kr ≥ 4kr (1 − δ 2 ) > Wf (t ≥ t0 ), cos (ϕ(Rp )) = cos (ϕ(Rq )) ≥ 2δ 2 − 1, t ≥ t0 . B.1.2
(B.10)
Exponential Stability The method utilized in this section and in Appendix B.2 is based on the
introduction of a cross-term in the Lyapunov Function [17]. Lemma B.2 (Positive-definiteness). The Lyapunov function: 1 Wf× = kr Vr + ΩT Im Ω + sTδ Ω, > 0, 2 = Wf + sTδ Ω,
(B.11)
is positive-definite for small enough values of . Wf is the Lyapunov function introduced in Eq. (5.13) which was used to show the asymptotic stability of the LLC control-law. Proof. Let us denote by µmin and µmax the minimum and the maximum singularvalues of the symmetric helicopter inertia-matrix Im respectively. Due to the symmetry and positive-definiteness of Im they are also the minimum and maximum eigenvalues respectively. µmin kΩk2 − ksδ kkΩk, (B.12) 2 √ From Eq. (B.9), we get −ksδ k ≥ −2 Vr . Substituting this in the previous step Wf× ≥ kr Vr +
gives Wf× ≥ kr Vr +
p µmin kΩk2 − 2 Vr kΩk, 2
The RHS is clearly positive-definite for r ||
0, and one of them will be more restrictive depending on the actual values of kr , δ, µmin and µmax . 149
B.2
Translational Dynamics
Lemma B.4 (Positive-definiteness). Let Un (ρ = ξ/R) be a potential-function as defined in Lemma 5.3. Then the following Lyapunov function Ws×
mvT v = kt Un (ρ) + + s mρT v, 2
(B.28)
is positive-definite for small enough values of s . Proof. Ws× ≥ kt Un (ρ) +
mvT v − s mkρk kvk 2
A sufficient condition for the RHS to be positive definite is r s 2kt Un (ρ) |s | < . m kρk2 On using Eq. (5.21), one finally gets a sufficient condition to be r 2kt . |s | < m
(B.29)
(B.30)
(B.31)
Lemma B.5 (Exponential Stability of HLC). The Lyapunov function given in Lemma B.4 can be used to prove exponential stability of the translational system under the HLC control-law. Proof. ˙× = W s
kt T ∂Un (ρ) s v + vT mv˙ + mvT v + ξ T mv˙ . Rb ∂ρ Rb
(B.32)
On substitution of the translational dynamic equations and the HLC control-law, one obtains ˙× W s
s m = − kv − Rb
v T v − s k v ρT v − 150
s kt T ∂Un (ρ) ρ . Rb ∂ρ
(B.33)
On using Eq. (5.22) one gets 2λu s kt m s × ˙ v T v − s k v ρT v − Un (ρ). W ≤ − kv − s Rb Rb
(B.34)
We now choose s > 0 such that Eq. (B.31) is satisfied. Next kv is chosen as 1 (B.35) kv = s m + αs , αs > 0. Rb This gives m 2λ ˙× −W 1 u s T ≥ 2αs v v + kt Un (ρ) + + αs s mρT v . s 2 Rb Rb
(B.36)
Next, the as yet undetermined positive scalar αs is chosen as αs =
λu . Rb
(B.37)
Combining Eqs. (B.37), (B.35) and (B.31), one gets the following upperbound on kv r kv
