A Kinematically Compatible Framework for Collaboration of .... Waseem Khan for his contribution in the development of the Wheeled Mobile. Robot Virtual ...
A Kinematically Compatible Framework for Collaboration of Multiple Nonholonomic Wheeled Mobile Robots
Michel Abou-Samah
Department of Mechanical Engineering Center of Intelligent Machines McGill University, Montreal, Canada
A Thesis submitted to the Faculty of Graduate Studies and Research in partial fulfillment of the requirements of the degree of Masters of Engineering ã Michel Abou-Samah, Nov 2001
I dedicate this research document to all of humankind. I would like this paper to be recognized as an addition to the attempts of pushing for the pursuit of imagination, knowledge and truth, via the advancements contributed towards robotics and technology.
i
Abstract In this thesis, we examine the design, development and implementation of a modular system of multiple, wheeled mobile manipulators that can team up to cooperatively transport a large common object. Each individually autonomous mobile manipulator consists of a differentially driven wheeled mobile robot (WMR) with a passive, two-degree-of-freedom, planar revolute-jointed arm mounted in the plane parallel to the base of the WMR. Mobile manipulator systems typically possess more degrees of freedom than required for the task and hence effective redundancy resolution methods become important for the control. Particular attention is paid for the development of kinematic control schemes, which take into account both the non-holonomic constraints of the base and the presence of passive joints in the manipulator system. The composite multi-degree-of-freedom vehicle, formed by placing a common object on the end-effector of two (or more) such mobile manipulator systems, possesses the ability to change its relative configuration as well as accommodate relative positioning errors of the mobile bases. However, closed kinematic loops are also formed constraining the relative motions of the overall system and requiring a careful treatment. Two variants of the control schemes developed for mobile manipulators are adapted for the control of the overall collaborating system of two mobile manipulators carrying a common object along a desired trajectory. The performance of both methods – the Leader-Follower approach and the Decentralized Control approach – is evaluated using virtual prototypes, to test various design and control system alternatives in software, ii
prior to actual implementation on the experimental test-bed. Particular attention is paid to evaluate the ability of the overall collaborating system to accommodate, detect and correct for systematic and non-systematic errors in the relative positioning of the mobile platforms. Finally, we also examine the optimal selection of the configuration of the articulations to attain near-isotropic manipulability of the common object in the presence of closed kinematic loops and mixture of active and passive joints.
iii
Résumé Dans cette thèse, nous examinons la conception, le développement et la mise en place d'un système modulaire de plusieurs manipulateurs mobiles muni de roues qui peuvent transporter en équipe un grand objet. Chaque manipulateur mobile autonome est composer d'un robot mobile (WMR) muni de roues différentiel piloté avec un bras passif, qui a deux-degré-de-liberté, un joint revolute planaire monté dans le plan parallèle à la base du WMR. Typiquement, les systemes de manipulateurs mobiles possedent plus de degrés de liberté éxigé par la tâche et par conséquent les méthodes de résolution de redondance deviennent importantes pour le control. Une attention particulière est payee pour le développement des arrangements cinématiques de commande, qui tiennent compte des contraintes non-holonomic de la base et de la présence des joints passifs dans le système de manipulateur. Le véhicule composé de multiple degré-de-liberté, obtenu en plaçant un objet commun sur l’organe terminal de deux (ou de plus) systèmes mobiles de manipulateur, possède la capacité de changer sa configuration relative et de prendre en compte les erreurs de positionnement relatives des bases mobiles.
Cependant, des boucles
cinématiques fermées sont également formées contraignant les mouvements relatifs du système global et exigeant un traitement speciale.
iv
Deux
variantes des arrangements de commande développés pour les
manipulateurs mobiles sont adaptées pour le controle du système global de collaboration de deux manipulateurs mobiles portant un objet commun le long d'une trajectoire désirée. La performance des deux méthodes – l'approche de Maître-Esclave et l’äpproche de Controle Décentralisée – est évalué en utilisant les prototypes virtuels, pour tester de diverses solutions de rechange de conception et de système de commande dans le logiciel, avant la mise en place réelle sur le banc d'essai expérimental. Une attention particulière est prêtée pour évaluer la capacité du système global de collaboration a accomoder, détecter et corriger des erreurs systématiques et non-systématiques dans le positionnement relatif des plateformes mobiles. En conclusion, nous examinons également la sélection optimale de la configuration des articulations pour atteindre le manipulability proche-isotrope de l'objet commun en présence des boucles cinématique fremés et du mélange des joints actifs et passifs.
v
Acknowledgements I have had the privilege of working with some exemplary people throughout my 2 year Master’s program. Their exceptional help and support made this Masters degree a pleasurable and exciting adventure. I would like to thank: •
Professor Venkat Krovi, my supervisor, for his priceless help throughout my program. His intense knowledge and expertise in the knowledge of both analytical and experimental robotic science as well as, mechanical engineering, helped me to strengthen my knowledge in these disciplines. It was an unmatchable pleasure and honor to study and work under his supervision.
•
Waseem Khan for his contribution in the development of the Wheeled Mobile Robot Virtual Prototypes.
•
Professor Jorge Angeles for his optimization teachings in his course, which helped in the development of the optimization studies in my research.
•
Professor Martin Beuhler, for allowing me to work in the ARL lab for the period of the Masters Program. Working in the ARL environment was an unquestionable treat and honor.
•
The ARL bunch for providing, truly, a professional, and extremely pleasurable working atmosphere. I am happy to have met and worked along, I think, some of the finest students. They are a great bunch of people who love life, who constantly practice professionalism in their fields, and are a joy to be around. Their vi
dedications to the field of robotics engineering, was very inspiring, and I predict a grand future, in the discipline, for all of them. I particularly thank, Carl Steeves, Dave McMordie, Don Campbell, Edward Moore, Eivgini Kiry, Ioannis Poulakakis, James Smith, Martin de Lasa, Matt Smith, Shervin Talebinejad, Aky, and the rest of the ARL students, for all their insightful inputs and great attitudes. •
Samer Shafik and James Smith for helping me with the French translation of the abstract.
•
Ralph Choufani for his help in providing access to a printer.
•
My friends and family for their help, love and support. Thankyou. Without you all I am nothing.
vii
Contents 1 Introduction..............................................................................................1 1.1
Research Issues ................................................................................... 3
1.2
Organization of Thesis ........................................................................ 4
2 Wheeled Mobile Robots ..........................................................................5 2.1
The Disc Wheel ................................................................................... 5
2.2
Multiple Disc Wheels.......................................................................... 8 2.2.1
Our Wheeled Mobile Robot System: A R N O L D.......................... 14
3 Wheeled Mobile Robots Modeling and Control....................................16 3.1
Introduction ....................................................................................... 16
3.2
Modeling of Differential Drive WMR .............................................. 16 3.2.1
The knife edge model ........................................................................ 17
3.2.2
Roller unicycle model ....................................................................... 19
3.2.3
Differential Drive model ................................................................... 20
3.3
Control of the WMR ......................................................................... 21
3.4
Implementation of the Virtual Robot algorithm................................ 25
3.5
Testing the performance of the virtual robot control algorithm for prescribed trajectories ....................................................................... 26
3.6
3.5.1
Test 1 ................................................................................................. 26
3.5.2
Test 2 ................................................................................................. 29
3.5.3
Test 3 ................................................................................................. 30 Discussion ......................................................................................... 31
4 Mobile Manipulators..............................................................................32 viii
4.1
Kinematic Redundancy Resolution Schemes.................................... 33
4.2
Proposed method for control of a mobile manipulator with passive joints .................................................................................................. 36
4.3
Experimental Setup and Evaluation .................................................. 42
5 Collaboration of Two Mobile Manipulators: A Leader-Follower Approach...................................................................................................48 5.1
Problem Formulation......................................................................... 50
5.2
Experimental Setup and Evaluation .................................................. 54
6 Collaboration of Two Mobile Manipulators: A Decentralized Approach ...................................................................................................................60 6.1
Problem Formulation......................................................................... 62
6.2
Experimental Setup and Evaluation .................................................. 62
7 Optimal Isotropic Design of Articulations ............................................69 7.1
Background ....................................................................................... 69
7.2
Outline of our Optimal Isotropic Design Method ............................. 74
7.3
Problem Formulation......................................................................... 76
7.4
Optimization Case Study................................................................... 83
8 Virtual and Physical Prototyping of ARNOLD.....................................88 8.1
Virtual Prototyping............................................................................ 89
8.2
Mechanical Development.................................................................. 92 8.2.1
Design of the Individual Mobile Robots ........................................... 92
8.2.2
Design and Assembly of the Top frame, Articulated Arm and Entire system................................................................................................ 94
8.3
Electronics Framework ..................................................................... 99
8.4
Software Development Framework................................................. 102
9 Conclusion .......................................................................................... 105 9.1
Summary ......................................................................................... 105
9.2
Future Work .................................................................................... 108
A Mathematical Tools ........................................................................... 115 A.1
Singular Value Decomposition ....................................................... 115 ix
A.2
Dexterity Measures ......................................................................... 116 A.2.1
Measure of Manipulability .............................................................. 116
A.2.2
Measure of ill-conditioning: The Condition Number ..................... 117
A.3
A Geometric framework for closed chain kinematic analysis ........ 118
B Tuning the Virtual Robot ................................................................... 121 B.1
Effectiveness of the control algorithm for an initial error/offset in the x direction of the robot .................................................................... 121
B.2
Effectiveness of the control algorithm for an initial error/offset in the orientation of the robot .................................................................... 125
B.3
Effectiveness of the control algorithm for an initial error/offset in the orientation of the robot and the x direction ..................................... 129
C Simulink Blocks ................................................................................. 133 C.1
Simulink Blocks that interface hardware ........................................ 133 C.1.1
Block for Encoder Card................................................................... 133
C.1.2
Block for Motor Driver Card: ......................................................... 134
C.2
Simulink Models representing the different control Algorithms: ... 137 C.2.1
Single Virtual Robot Algorithm (Ch. 3): ........................................ 138
C.2.2
Single 2-Link Mobile Manipulator Models (Ch.4): ........................ 139
C.2.3
Leader-Follower Approach: Uses odometry to estimate positions of MP A (Ch. 5)................................................................................... 144
C.2.4
Leader-Follower Approach: Uses articulation to estimate position of MP A (Ch. 5)................................................................................... 145
C.2.5
Decentralized Approach: Uses odometry to estimate positions of MP A and MP B (Ch. 6): ....................................................................... 146
C.2.6
Decentralized Approach: Uses articulation to estimate positions of MP A and MP B (Ch. 6).................................................................. 147
x
List of Figures Figure 1.1: Conceptual Model of ARNOLD with intended application purpose. .............. 2
Figure 2.1: Rolling disc wheel on (a) Uneven terrain and (b) Even (horizontal) surface. .. 6 Figure 2.2: (a) Tracked mobile robot (Borenstein, et al. 1996), (b) WMR using omnidirectional Swedish MECANUM wheels. .................................................................. 8 Figure 2.3: Possible locations of the ICR for a Differential Drive WMR. ......................... 9 Figure 2.4: Possible locations of the ICR for a Tricycle drive WMR (Borenstein et al., 1996).................................................................................................................................. 10 Figure 2.5: 4-wheeled synchro drive WMR (Borenstein et al, 1996). ............................. 11 Figure 2.6: An 8 degree of freedom vehicle with 4 wheels driven and steered independently (Borenstein et al., 1996). ........................................................................... 12 Figure 2.7: A 4 wheeled WMR with Ackerman steering for the front wheels (Borenstein et al., 1996)........................................................................................................................ 12 Figure 2.8: CLAPPER design: (a) Actual vehicle with compliant linkage; (b) Schematic top view (Borenstein et al., 1996). .................................................................................... 13 Figure 2.9: OMNIMATE design: (a) Actual vehicle with compliant linkage; (b) Schematic top view (Borenstein et al., 1996).................................................................... 14 Figure 2.10: ARNOLD, MDOF vehicle with compliant linkage; (a) Actual system; (b) Solid works Cad rendering. ............................................................................................... 15
Figure 3.1: Knife edge model of a Wheeled Mobile Robot. ............................................. 17 xi
Figure 3.2: Diagram of a wheeled mobile robot model with frame M, and global frame F. ........................................................................................................................................... 20 Figure 3.3: Schematic, depicting nomenclature for the control problem.......................... 23 Figure 3.4: Plots of the actual mobile base trajectories against the reference robot’s trajectories for a straight line desired trajectory lead by the reference robot. ................... 27 Figure 3.5: Rescaled Plots of the actual mobile base trajectories against the reference robot’s trajectories for a straight line desired trajectory lead by the reference robot........ 28 Figure 3.6: Plots of the actual mobile base trajectories against the reference robot’s trajectories for a circle desired trajectory lead by the reference robot. ............................. 29 Figure 3.7: Plots of the actual mobile base trajectories against the reference robot’s trajectories obtained by inputting a sinusoidal wave input to the angular velocity of the reference robot................................................................................................................... 30
Figure 4.1: Mobile Manipulator (Yamamoto and Yun, 1993). ......................................... 33 Figure 4.2: Schematic diagram of our 2-link mobile manipulator. ................................... 36 Figure 4.3: Case A: Odometric Estimation of frame M used in the control of the Mobile base (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time........................................... 44 Figure 4.4: Case B: Articulation based Estimation of Frame M used for the control of the Mobile base (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. .............................. 46
Figure 5.1: Collaboration of Two Mobile Manipulators (a) carrying common object (b) forms effective 4 bar linkage............................................................................................. 49 Figure 5.2: Schematic diagram of new 3 link mobile manipulator under analysis. .......... 50 Figure 5.3: Schematic diagram of two mobile robot system............................................. 51 Figure 5.4: Case A: Odometric Estimation of Frame M used in the control of MP A with respect to MPB (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. ............ 56
xii
Figure 5.5: Case B: Articulation based Estimation of Frame M used for control of MPA with respect to MP B (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. ............ 58
Figure 6.1: Schematic diagram of two mobile robot system, made up of two mobile manipulators connected at their respective end effectors.................................................. 61 Figure 6.2: Schematic diagram of two, separate and independent, 2-link mobile manipulators, under analysis. ............................................................................................ 61 Figure 6.3: Case A: Odometric Estimation of Frames M of MP A and MP B, used in the control of MP A with respect to MP B (a) XY trajectory of Frame M of MP A; (b) XY trajectory of Frame M of MP B; (c) Relative Orientation, between MP A and MP B, versus Time; (d) X distance, between MP A and MP B, versus Time; and (e) Y distance, between MP A and MP B, versus Time. ........................................................................... 65 Figure 6.4: Case B: Articulation based Estimation of Frames M of MP A and MP B, used for the control of MP A with respect to MP B (a) XY trajectory of Frame M of MP A; (b) XY trajectory of Frame M of MP B; (c) Relative Orientation, between MP A and MP B, versus Time; (d) X distance, between MP A and MP B, versus Time; and (e) Y distance, between MP A and MP B, versus Time. ........................................................................... 68
Figure 7.1: Schematic of the overall system depicting the critical model parameters...... 77
( )
Figure 7.2: Plot of objective function f χ for a range of values of φ4 ......................... 85 Figure 7.3: Actual Matlab generated plot of the optimal configuration of the dual robot system................................................................................................................................ 86
Figure 8.1: Screen-shot of simulation of one mobile platform within the visualNastran simulation environment being controlled by algorithm in developed in Simulink........... 90 Figure 8.2: Screenshot of visualNastran simulation environment with models two unconnected independent 2-link mobile manipulators...................................................... 91
xiii
Figure 8.3: (a) Two mobile manipulators carrying a common object; and (b) Effective articulated linkage formed between the two mobile bases................................................ 91 Figure 8.4: First Mobile Platform (a) CAD model and (b) Actual prototype. ................. 93 Figure 8.5: Second Mobile Platform (a) Cad Model; and (b) Actual prototype. .............. 93 Figure 8.6: Cad model of mobile platform with mounted frame. ..................................... 94 Figure 8.7: (a) Cad rendering of articulated arm, (b) Actual mechanical device.............. 95 Figure 8.8: (a) Cad rendering of complete mobile robot, (b) Actual complete mobile robot. ................................................................................................................................. 96 Figure 8.9: ARNOLD, MDOF vehicle with compliant linkage (a) Cad rendering not showing motorized fourth joint, (b) Actual system showing motorized fourth joint........ 98 Figure 8.10: Full schematics of the electronics cards interfaced with the motors and encoders of our system. ................................................................................................... 100 Figure 8.11: More detailed schematic of the ESC629 motor driver interfacing two motors and their respective encoders. ......................................................................................... 101 Figure 8.12: More detailed schematic of the DM6814 encoder card interfacing the 3 encoders........................................................................................................................... 102 Figure 8.13: Real-Time Control Framework................................................................... 103
Figure A.1: Metric conversions and their respective Manifolds. .................................... 118 Figure B.1: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ .................................................................................. 123 Figure B.2: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ ................................................................................. 128 Figure B.3: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ .................................................................................. 132 Figure C.1: DM6814 Encoder Simulink Block that interfaces DM6814 Encoder Card. 133 Figure C.2: DM6814 parameters mask requires Sample Time and Base Address. ........ 134
xiv
Figure C.3: ESC629 Motor Driver Simulink Block interfaces the ESC629 Motor Driver electronic card, showing also required inputs before block and conversion factors after block. Here block is shown for left motor....................................................................... 134 Figure C.4: ESC 629 motor driver block parameter mask, requires some information.. 135 Figure C.5: Simulink Model of Single Mobile Robot Controlled with Virtual Robot Algorithm. ....................................................................................................................... 138 Figure C.6: Simulink Model of single 2 link Mobile Manipulator: uses odometry to estimate current position of the MP A. ........................................................................... 140 Figure C.7: Simulink Model of single 2 link Mobile Manipulator: uses articulation to estimate current position of the MP A. ........................................................................... 142 Figure C.8: Simulink Model of Leader-Follower Approach: uses odometry to estimate positions of the Actual MP A.......................................................................................... 144 Figure C.9: Simulink Model of Leader-Follower Approach: uses sensing of articulation for MP A’s position estimation. ...................................................................................... 145 Figure C.10: Simulink Model of Decentralized Approach: uses odometry to estimate positions of MP A and MP B. ......................................................................................... 146 Figure C.11: Simulink Model of Decentralized Approach: uses sensing of articulation for MP A’s and MP B’s position estimation......................................................................... 147
xv
Chapter 1 Introduction In this thesis, we investigate the design, analysis and implementation of a flexible, scalable system of multiple mobile robots, that are individually autonomous and can team up to cooperatively transport large objects and to perform tasks that simply cannot be performed by a single mobile platform. Such frameworks for remotely controlled or remotely supervised cooperation of multiple autonomous mobile robots with their unique non-odometric dead-reckoning accuracy and active reconfigurability, have many applications from uneven terrain exploration to material handling on the shop floor. Our system, called ARNOLD, derives its name from Autonomous mobile Robot Navigation
with
Odometric
and
Link-based
Dead-reckoning.
In
its
current
implementation, it consists of two wheeled mobile robots with a planar 2 d.o.f arm mounted atop each mobile robot. An effective articulated physical coupling between the modules is formed when the arms come in contact with a common object or when the tips of the arms of two adjacent individual modules are attached to each other, as shown in Figure 1.1.
1
Figure 1.1: Conceptual Model of ARNOLD with intended application purpose. While several researchers have investigated the creation of omnidirectional robots with special wheels, we will restrict our attention to systems with nonholonomically constrained disk wheels. The presence of these nonholonomic constraints requires the introduction of articulations between the individual wheels. The resulting composite systems share many features with the class of Multi-Degree-of-Freedom (MDOF) Vehicles (Borenstein et al., 1996) as well as the class of Wheeled Actively Articulated Vehicles (Sreenivasan and Waldron, 1996; Wilcox et al, 1992; 1998). Examples of MDOF vehicles include HERMIES-III, with two powered and individually steered disk wheels; and the CLAPPER, which employs a dual system of differentially-driven mobile robots attached to each other by a compliant link forming an MDOF platform. Such MDOF systems have been found to display exceptional maneuverability in tight quarters in comparison to conventional 2-DOF mobility systems, but have been found to be difficult to control due to their over constrained nature and the commensurately increased wheel slippage, and thus reduced odometry accuracy. 2
1.1 Research Issues In our system, each differentially-driven mobile base possesses a single rigid axle between two fixed disk wheels with the usual complement of nonholonomic constraints. Two mobile robots, with such rigid axles, cannot be rigidly coupled to each other without losing some or all of their mobility. In our work, we examine the potential for further relaxing the rigid body constraints between the various axles by introducing further articulations. The presence of the planar, 2-DOF arms lying between adjacent modules, and supporting the common object, creates an effective articulated linkage between the two modules. The resulting articulations endow the composite vehicle with: (i) ability to accommodate changes in the relative configuration; (ii) redundant sensing for localizing the modules; and (iii) redundant actuation method for moving the common object to compensate for disturbances in the motions of the base. These articulations (with at least 3 degrees-of-freedom) now permit us to relax the requirement for a common "Center of Rotation" between the multiple axles. However, while the velocity-level kinematic constraints for the system are eliminated, other holonomic constraints are introduced between the relative motions of the bases. Hence, the suitable selection of the parameters of the intermediate linkage thus formed becomes critical. The important design parameters include the various link lengths and the configuration of the linkage, at any instant of time. Further, the existence of the holonomic (loop-closure) constraints, limit the degrees of freedom and hence not all articulations need to be actuated. The selection of location of the active and passive joints will play an important role in determining the overall manipulability of the transported object. Finally, as mentioned earlier wheel-slippage and reduced odometric accuracy are important problems with such MDOF systems.
We have developed a
system with proprioceptive control in which the link-mounted joint encoders will be employed to localize the individual modules relative to each other, permitting us to augment/correct the odometry based dead-reckoning. 3
1.2 Organization of Thesis In this thesis, we will investigate the design, analysis and implementation of a flexible, scalable system of multiple mobile robots, that are individually autonomous but can team up to cooperatively transport large objects. Chapter 2 examines some of the key design issues pertaining to wheeled mobile robots with multiple disk wheels. In particular, we examine the constraints imposed by the attachment of multiple disk wheels to various locations of a common rigid body as well as survey a number of previously proposed configurations to overcome these limitations. Chapter 3 gives a brief review of the modeling and control of differentially driven wheeled mobile robots and summarizes the particular method we have selected for controlling each of the mobile bases. In Chapter 4 we study some of the key issues and challenges for the design and control of a class of systems called mobile manipulators. Relevant past work is reviewed prior to, proposing our mobile manipulator configuration, and, studying its performance. Two variants of this developed method are then adopted and implemented in a decentralized approach to control the combined system in Chapters 5 and 6. The overall manipulability of the common object depends upon the selection of the relative configuration of the links as well as their dimensions. Hence in Chapter 7 we present the preliminary examination of the optimal design of the combined system to maximize the overall manipulability of the common object. Chapter 8 summarizes several aspects of the virtual and physical prototyping methods adopted during the development of the combined system. In particular the relevant details of the mechanical system development, the electronics framework and the overall software development environment are briefly summarized. Finally, Chapter 9 concludes this thesis with a discussion of the contributions and avenues for future work.
4
Chapter 2 Wheeled Mobile Robots Wheeled Mobile Robots (WMR) have been extensively studied in the past by many researchers in the field of robotics. In general wheeled mobile robots comprise of a platform supported by rolling wheels. Such wheels, allow for relative motion between the platform and the ground. Wheeled robots are mechanically simple and easy to construct and the payload weight-to-mechanism ratio is favorable. In addition, wheeled platforms are simpler to control, pose fewer stability problems, use less energy per unit distance of motion, and can travel with significant speeds.
2.1 The Disc Wheel The mobility and controllability of the wheeled mobile robot therefore depends largely upon the nature of the attached wheels. The most popular choice of wheels for wheeled vehicles is the disk wheel, whose geometric shape allows for: (a) ease of addition to platforms; (b) ease of control; (c) robust physical construction; and (d) ease of operation in the presence of bumps, cracks, or any other terrain irregularities. However, despite its incredible versatility, the nature of operation of the disk wheel imposes constraints on the design of the mobile platform. These constraints take the form of the requirement for each wheel to: (a) not slip sideways; and (b) roll without 5
slip; for controllable operation. These constraints on the disc wheel are usually referred to as nonholonomic constraints and can be further explained using Figure 2.1.
(a)
(b)
Figure 2.1: Rolling disc wheel on (a) Uneven terrain and (b) Even (horizontal) surface. Figure 2.1 illustrates that the direction of the wheel rolling and the direction of the surface normal at the wheel/ground contact prescribes the direction of the velocity vector of the axle of the wheel. Further, if the wheel is driven and the wheel rolls without slip, the magnitude of the velocity vector is also prescribed. On horizontal (even) ground plane, the instantaneous velocity of the axle is perpendicular to the steering axis of the wheel as shown in Figure 2.1 (b) and we will restrict our attention to such a situation in this thesis. However, it is worth noting that if the terrain is uneven then the instantaneous ground plane (and hence the instantaneous velocity) is not perpendicular to the steering axis, which has significant implication for the design. Two design limitations arise due to the nonholonomic constraints imposed by disk wheels. The first and most obvious problem is that these constraints do not permit 6
instantaneous motion of the wheel (and thereby the attached body) in all directions. However, this, itself, does not pose a significant problem since it can be shown that by a suitable set of steering and rolling motions any position on the plane may be reached. The second and more restricting limitation is that when multiple disk wheels are attached to a common rigid body the constraints on the motion of the rigid body are the union of all the individual constraints of each wheel. Hence for controllable motion of the rigid body every constraint must be maintained which may not be possible for arbitrary attachment of the wheel to the rigid body. Hence the designer has the choice of: a)
Limiting the locations of points of attachment of the wheels to a very small set, if arbitrary set of motions need to be achieved/accommodated;
b)
Enlarging the set of attachment points by restricting the set of controllable motions to be achieved by the rigid body. In most practical situations, the latter approach is adopted for design of such
bases. However, this has the consequence that in the presence of motions outside the prescribed design set of motions some of the constraints (which are unilateral constraints) have to be violated. For example, wheels may have a tendency to slip both sideways and along the rolling direction bringing about uncontrollable and undetectable uncertainties that cannot be foreseen or compensated easily. This is especially critical if the mobile robot uses odometry for self-localization where a bump or slip of one wheel can cause an estimation error, which: a) cannot be detected without an external reference and b) has the tendency to cumulatively grow. For this reason many researchers have considered designs for wheeled mobile robots using special wheels, such as the Swedish MECANUM wheel which do not possess the nonholonomic constraints of the disc wheels. The Swedish MECANUM also known as the Universal wheel, has passive rollers embedded in the wheel itself (S. Ostrowskaya, 2001), which inherently require slippage and accommodation in order to be effective. However, the uncontrolled nature of the accommodation due to the passive elements in the system leaves lesser room for accurate motion control. 7
Other researchers have even tried to avoid using wheels, opting instead for the use of tracks and relying on skid steering in order to move. However, skid steering is implemented using track slippage, which allows poor dead reckoning and reduced odometric accuracy. For these reasons, such tracked vehicles are rarely automated and are usually tele- or manually-operated.
(a)
(b)
Figure 2.2: (a) Tracked mobile robot (Borenstein, et al. 1996), (b) WMR using omnidirectional Swedish MECANUM wheels. Thus, these special kinds of wheels and/or tracks that are not affected by nonholonomic constraints, often come with their own special sets of problems. Thus, for purposes of this thesis we will restrict ourselves to conventional disk wheels, examining design of a composite wheeled mobile system with multiple disk wheels that take advantage of the disk wheel while taking into account the constraints.
2.2 Multiple Disc Wheels Multiple disc wheels are desirable for many reasons. Platform solutions, that make use of multiple disc wheels that are attached to the same platform, have advantages in the sense of distributing the load and the tractive forces amongst the wheels. In general such platforms have the potential to perform tasks that cannot be performed by other mobile robots, such as legged mobile robots, or tracked mobile robots or flying robots.
8
However, problems arise, firstly, if disc wheels are attached at arbitrary locations to the same platform, and, secondly, if they are arbitrarily actuated. Rigidly attaching the disk wheels to the same mobile platform, often results in very poor performance. With behaviors like, rattle, shake, unintentional compliance, wear and tear of the tires and wheel slip. These problems arise from the incompatibility of the velocities of the wheel axles and tend to be especially aggravated if the platform travels over uneven terrain. Therefore many approaches in the literature advocate the addition of further articulations (passive/active) between the various wheel axles in order to accommodate the rigid body constraints. In the plane, the rigid body constraint takes the form of requirement of all the wheels to have a common instantaneous center of rotation (ICR). The planar assumption for the surface now effectively restricts all axle velocities to two-dimensional vectors in the plane. The rigid body constraint restricts these axle velocities to directions that are instantaneously tangent to the radial vectors emanating from the ICR to the point of attachment of the wheels. Driven wheels
Figure 2.3: Possible locations of the ICR for a Differential Drive WMR. We will now examine how this requirement for a common ICR is satisfied in a variety of common designs for wheeled mobile platforms. The differential drive mobile robot, for example consists of a mobile platform with two powered wheels, and two 9
unactuated casters. Therefore, for the differential drive WMR, the ICR can occur anywhere along the intersection of the axis of rotation of the two wheels as seen in Figure 2.3. This is because the two wheels are parallel to each other and their respective axes are always coincident with each other. For the tricycle drive, the tricycle wheel in front is driven and steered while the other two rear wheels are passive and cannot steer. This will always result in the ICR existing for all the wheels and it lies on the intersection of, the axis of the steerable wheel and the axis of the other two non steerable wheels. This vehicle is now kinematically compatible with respect to wheel axle velocities.
Figure 2.4: Possible locations of the ICR for a Tricycle drive WMR (Borenstein et al., 1996). When we need to add 3 or more drivable wheels, to a platform, the solution is to now add articulations, about a vertical axis between the wheel axles and the frame. An interesting design called the “Synchro Drive” consists of three driven and steered wheels that are mechanically coupled by timing belts to one drive motor and one steering motor. The system is still 2 degrees of freedom with 2 independent controlled motors. Since all the wheels of the robot are driven at the same velocities and steered with the same angles at every instant, they are all synchronized with each other, hence the term “synchro” (Borenstein et al., 1996). Such vehicles are free to move in any direction but there is no control of the orientation of the chassis since the wheels move with respect to the chassis, requiring an independent orientation mechanism like a turret platform. Further, such a drive would be most suitable for a round or nearly round vehicle, whereas many transport tasks require a rectangular platform. 10
Another such design, using three independently steered passive wheels is the “Tricycle Cobot” developed by Peshkin and co-workers at Northwestern University. In their design, the three independently, actively steered but unactuated disk wheels are coordinated in software to produce those vehicles that would result in a desired rigid body motion of the frame, which is pushed by an external motion frame (Wannasuphopresit et al., 1998). Some (of the many) possibilities for four wheeled mobile platform designs include: a) 4 independently steered and driven wheels. b) Two actuated but unsteered wheels combined with two unactuated but steered wheels. c) Two sets of axles with 2 differentially driven wheels with an intermediate compliant linkage. The first may be implemented as a 4 wheel version of the Synchro Drive as seen in Figure 2.5 or simply as a platform with 4 independent drive and a further 4 independent steering motors as seen in Figure 2.6.
Figure 2.5: 4-wheeled synchro drive WMR (Borenstein et al, 1996). Such vehicles fall under the multiple-degree-of-freedom (MDOF) vehicles category, since it now depends upon the actuation of more than 2 actuators. MDOF vehicles are very maneuverable however they are limited in terms of controllability due to 11
their overconstrained nature. Problems arise with such a configuration such as wheel slippage, which hinders odometry accuracy.
Figure 2.6: An 8 degree of freedom vehicle with 4 wheels driven and steered independently (Borenstein et al., 1996). The second possibility for a 4 wheeled mobile platform design is shown in Figure 2.7. Such platforms consist of two rear driven wheels that are fixed in the direction of forward motion of the platform and two unactuated front wheels that can be steered.
Figure 2.7: A 4 wheeled WMR with Ackerman steering for the front wheels (Borenstein et al., 1996). In such a configuration the ICR of the wheels lie in the intersection of the axis of all the wheels, and for that reason these axes must all intersect at every instant in order for the vehicle to turn and the wheels to be kinematically compatible. For this reason, the Ackerman steering helps to ensure that the axes of the front two wheels always intersect 12
with the axes of the other (non-steered) wheels at the same position as shown in Figure 2.7. The resulting car-like robots are used for transporting large objects. The problem with such robots is that they have to undergo complex maneuvers in order to move to certain configurations (like parallel parking in order to move sideways). The third possibility for having 4 wheels connected to a common platform is to implement a compliant linkage between the axles of two sets of differentially driven wheels. Two such exemplary vehicles, each with a compliant linkage, were built by researchers at the University of Michigan (Borenstein et al., 1996). Each vehicle is comprised of two, differential-drive, ‘LabMate’ robots, which are considered to be “trucks”, and are connected by a 3 d.o.f. compliant linkage. The compliant linkage consists of two rotary joints and one prismatic joint for a total of three internal degrees of freedom between the two trucks. This compliant linkage enables the MDOF system to accommodate momentary controller errors without transferring any mutual force reactions between the trucks; thereby eliminating the excessive wheel slippage reported for other MDOF vehicles.
(a)
(b)
Figure 2.8: CLAPPER design: (a) Actual vehicle with compliant linkage; (b) Schematic top view (Borenstein et al., 1996). In the CLAPPER design, the two revolutes (R) and one prismatic (P) joint are arranged to form a passive RPR (Revolute, Prismatic, Revolute) linkage between the two 13
mobile bases as shown in Figure 2.8. A linear encoder measures the relative distance between the two trucks, and two rotary encoders measure the rotation of the trucks relative to the compliant linkage. Each of the four drive wheels in the system has a shaft encoder to allow conventional dead reckoning. In the OMNIMATE design, the two revolute and one prismatic joint are arranged to form a passive PRR (Prismatic, Revolute, Revolute) articulation between the two mobile bases. The problem with either such configuration is that the two robots are forever assembled together because of the compliant linkage and cannot be reconfigured easily.
(a)
(b)
Figure 2.9: OMNIMATE design: (a) Actual vehicle with compliant linkage; (b) Schematic top view (Borenstein et al., 1996).
2.2.1
Our Wheeled Mobile Robot System: A R N O L D ARNOLD is comprised of two differentially driven wheeled mobile robot
‘trucks’, which are connected via a compliant linkage that uses only revolute (R) joints between the two mobile bases. For symmetry we have 2 WMRs which each use a passive RR articulated arm to attach the mobile base to the common object, resulting in an RRRR linkage between the two mobile bases. However by holding one of the (R)s fixed we reduce it to the RRR configuration, which is the minimum required d.o.f for the compliant linkage. The advantages of our approach is modularity in that we offer the possibility of changing the configuration, the possibility to optimize the location of the 4th R joint and the convenience of removing and/or attaching each module easily. We will examine the design, optimization, realization and control of such a system in the remainder of this thesis. 14
(a)
(b)
Figure 2.10: ARNOLD, MDOF vehicle with compliant linkage; (a) Actual system; (b) Solid works Cad rendering.
15
Chapter 3 Wheeled Mobile Robots Modeling and Control 3.1 Introduction A wheeled mobile robot, with 2 differentially driven wheels mounted on a common axle and operating on a planar surface, has 3 positional degrees of freedom but can instantaneously move only in 2 directions due to the presence of a velocity level constraint. This constraint takes the form of requirement that the mobile robot cannot have a velocity parallel to the direction of the axle. In the diagram below we have a mobile robot whose frame of reference is denoted by M, which is within an environment whose inertial frame of reference is denoted by F.
3.2 Modeling of Differential Drive WMR We will briefly examine the three different ways to model a differentially driven WMR. They are: a) the single knife edge, b) the single rolling unicycle wheel and c) the differential drive WMR.
16
3.2.1
The knife edge model This is the simplest and most minimalistic representation in which the
configuration of the wheeled mobile robot may be represented by the vector q = [ x, y, φ ]T . Using this representation, the behavior of a mobile robot can be modeled as being equivalent to that of a thin knife. The knife is assumed to be capable of sliding with a velocity Vx in the body fixed x-direction and rotating with an angular velocity ω but cannot have a velocity V y in the body fixed y direction.
Figure 3.1: Knife edge model of a Wheeled Mobile Robot.
17
Therefore due to the fact that we cannot slip sideways we must introduce the nonholonomic constraint of the knife edge model. The constraint can be represented in the form of an equation: x sin φ − y cos φ = 0 .
(3.1)
The constraint equation can be derived easily from the following equations, which represent the velocity of the frame on the knife edge with respect to the inertial frame of the knife edge.
x = Vx cos φ − Vy sin φ
(3.2)
y = Vx sin φ + Vy cos φ
Noting that V y =0 we may obtain simplified expressions for x and y from which Vx may be eliminated. x = Vx cos φ y = Vx sin φ
(3.3)
x y = ⇔ x sin φ − y cos φ = 0 cos φ sin φ
We now can represent this constraint in matrix form as:
[sin φ
é x ù 0] êê y úú = 0 , êëφ úû
− cos φ
where, A(q) = [sin φ
− cos φ
(3.4)
0] .
(3.5)
We next choose to find the null space, S(q) , of the matrix A(q) . The significance of this null space is that the columns of the null space span the set of feasible motion directions, which do not violate any of the constraints. However, since the null space is
18
non-unique, a careful selection becomes necessary so as to permit a physically meaningful interpretation of the coefficients. Upon selection of S(q) to be: é x ù écos φ q = êê y úú = êê sin φ êëφ úû êë 0
0ù év ù év ù 0 úú ê 1 ú = S(q) ê 1 ú , v ëv2 û 1 úû ë 2 û
(3.6)
v1 and v2 may now be interpreted as that set of feasible velocities that do not violate the constraint as well as corresponding to a selection of the two independent velocities to be the forward linear velocity v and angular velocity ω . é x ù é cos φ q = êê y úú = êê sin φ êëφ úû êë 0
3.2.2
0ù évù 0 úú ê ú ω 1 úû ë û
(3.7)
Roller unicycle model The roller unicycle model is a modification to the knife-edge model in that we
introduce the rolling angle of the wheel as an additional degree of freedom. The configuration space may be written as q = [ x
y φ θ ] . However, since the system T
can instantaneously posses only 2 d.o.f, we now have two velocity level constraints, which can be represented as: x sin φ − y cos φ = 0
(3.8)
x cos φ + y sin φ − Rθ = 0 .
(3.9)
The first equation makes sure that there exists no side ways motion of the unicycle while the second equation makes sure that the unicycle can roll without slipping. In matrix form, the constraints may be expressed as:
19
é sin φ êcos φ ë
− cos φ sin φ
é x ù 0 0 ù êê y úú = 0, 0 − R úû êφ ú ê ú ëθ û
(3.10)
and the set of generalized velocities may be written in terms of the independent velocities as: é 0 − cos φ ù é x ù ê ú ê y ú ê 0 − sin φ ú ω é ù ê ú = ê1 ú 0 ê ú. êφ ú ê úëvû ê ú ê 1 ú ëθ û ê 0 R ûú ë
3.2.3
(3.11)
Differential Drive model
Figure 3.2: Diagram of a wheeled mobile robot model with frame M, and global frame F. The differential drive model is the most elaborate representation of a WMR. The configuration of the system can be shown as: 20
q = [ x c , y c , φ , θ R ,θ L ] .
(3.12)
The three velocity level constraints reduce the system to a 2 d.o.f system at the velocity level, and the derivation of these constraints is shown in detail in (Sarkar, 1995). Thus, the three constraints may now be represented in matrix form as: é − sin φ A(q)q = êê − cos φ êë − cos φ
cos φ − sin φ − sin φ
−d
0 0ù −b r 0 úú = 0 . b 0 r úû
(3.13)
Selecting the two wheel velocities to be the independent velocities we now represent the vector of generalized velocities as: écb cos φ ê cb sin φ ê q = ê c ê ê 1 êë 0
where, c =
cb cos φ ù cb sin φ úú éθ ù −c ú ê R ú , ú θ 0 úë Lû 1 úû
(3.14)
r and the d , shown in Figure 3.2 is zero. 2b
3.3 Control of the WMR Three different control problems that can be formulated for nonholonomic wheeled mobile robots are: a)
the “Posture Tracking” problem; where a control law needs to be found to permit a frame on the wheeled mobile robot to track the time indexed trajectory of a desired reference frame that is in constant motion.
b)
the “Path Following” problem; where the control law is required to permit the frame on the WMR to follow the geometric path traced by a 21
reference frame independent of the time parametrization of the frame’s motion. c)
the “Posture Stabilization” problem; where a control law needs to be found which permits a frame on the WMR to asymptotically stabilize to a fixed reference posture.
In our work we will focus on the Posture Tracking and the Posture Stabilization problems. We will briefly review some of the literature in the field of control of WMR prior to describing the particular scheme we will adopt for the control. Bloch and McClamroch (1989) first demonstrated that a nonholonomic system cannot be stabilized to a single equilibrium point by a continuous (smooth) time-invariant pure state feedback law, derived from a violation of Brockett’s necessary condition for stabilizability (Brockett, 1983). Further, Bloch and McClamroch (1989) showed that the WMR system is only small time locally controllable. Campion et al (1991) showed that a WMR system is controllable regardless of the structure of the nonholonomic constraints. Therefore our choices for control are limited to using: (a) discontinuous time invariant feedback laws or (b) continuous but time varying non linear feedback control laws. Samson and Ait-Abderrahim (1991), in a series of papers, proposed a method of using continuous time varying nonlinear feedback for posture tracking and stabilization problems, as well as a technique for addressing the point stabilization problem as an extension of the tracking problems. These results are also described, in detail, in the book by Siciliano et al (1996). We will adopt this technique for stabilization as an extension of the tracking problem for controlling our WMR. A brief summary of the method is provided below:
22
Figure 3.3: Schematic, depicting nomenclature for the control problem. This method, termed the ‘Virtual Robot Method’, assumes that a virtual unicycle robot generates the desired reference trajectory by a suitable selection of a forward velocity (v r (t )) and an angular velocity (ω r (t )) . The overall goal is to find a control law that is a function of the states of the actual and reference WMR and the time varying linear and angular velocity of the virtual robot. Initially we will consider the tracking problem only without considering “stabilization” to a posture. This translates to the requirement that the reference robot is never at rest; i.e. (v r (t )) and (ω r (t )) never become zero simultaneously. Therefore the control problem can be explained with the following equations: ævö ç ÷ = k (z, z r , vr , ω r ) , èω ø
23
(3.15)
such that, Limit (z (t ) − z r (t )) = 0 , t →∞
where,
(3.16)
é xr ù éxù é xr ù é x ù z r = êê yr úú , z = êê y úú , z r = êê y r úú , z = êê y úú , êëθ r úû êëθ úû êëθr úû êëθ úû
(3.17)
and, x = v cos θ y = v sin θ θ = ω
(3.18)
xr = vr cos θ r y r = vr sin θ r . θ = ω
(3.19)
r
r
Equation 3.16 represents the requirement that the error between the state of the actual robot and that of the reference robot tends to zero as time progresses. Upon transforming the errors in the Cartesian frame to the moving frame; æ e1 ö é cos θ ç ÷ ê ç e2 ÷ = ê − sin θ çe ÷ ê 0 è 3ø ë
sin θ cos θ 0
0 ù é xr − x ù 0 úú êê yr − y úú , 1 úû êëθ r − θ úû
(3.20)
and introducing a change of inputs: u1 = −v + vr cos(e3 ) u2 = ω r − ω
the system may be written as:
24
,
(3.21)
æ e1 ö æ 0 ω ç ÷ ç ç e2 ÷ = ç −ω 0 ç e ÷ ç 0 0 è 3ø è
0 öæ e1 ö æ 0 ö é1 0 ù éu ù ÷ç ÷ ç ÷ 0 ÷ç e2 ÷ + ç sin e3 ÷ vr + êê0 0 úú ê 1 ú . u ÷ ç ÷ êë0 1 úû ë 2 û 0 ÷ç øè e3 ø è 0 ø
(3.22)
The resulting system can be shown to be exponentially convergent to the desired trajectory, provided (v r (t )) and (ω r (t )) never become zero simultaneously.
3.4 Implementation of the Virtual Robot algorithm In the stabilization problem, we will assume that the virtual reference robot moves along a path, which passes through a point ( x r , y r ) about which we seek to stabilize the system. As noted earlier, stabilization cannot be directly handled by the previously defined control system because v r and ω r became zero. Hence in this extension, the idea is to make v r or ω r as time varying controls and dependent on the error such that convergence of the tracking error to zero will ensure convergence of the actual robot to the desired reference robot position. One suitable choice for the time varying control law recommended by Siciliano et al. (1996) is: u1 = − k1 (vr , ω r )e1 u2 = − k4 (vr , ω r )vr
, sin(e3 ) e2 − k3 (vr , ω r )e3 e3
(3.23)
where the coefficients of the control laws may be written as:
2
2
2
2
1
k1 (ω r , vr ) = 2ξ (ω r + bvr ) 2 1
k3 (ω r , vr ) = 2ξ (ω r + bvr ) 2 . k4 = b
(3.24)
b>0
Such a control law has the advantages of both, velocity scaling for regularization of the controller as well as enlarged domain of asymptotic stability. We, then, calculate a velocity term that is used when calculating the coefficients of the control laws: 25
2
v r = v rd − k 4 ( x r − x) 2 + ( y r − y ) 2 + e sin(t ) 2
2
e = e1 + e2 + e3
2
.
(3.25)
The coefficients for the control laws equations are: 2
2
2
2
k1 = 2ξ ω r + bvr k2 = b vr
.
k3 = 2ξ ω r + bvr
(3.26)
k4 = b
3.5 Testing the performance of the virtual robot control algorithm for prescribed trajectories A series of tests were performed to enable suitable selection of values for the parameters, b and ξ , based on minimizing the error and optimizing the transient performance; and are reported in Appendix B. This permitted the selection of the values as b=0.05 and ξ =0.9. A brief summary of the three different performed tests, are given below:
3.5.1
Test 1 In the first test, the reference robot was required to travel forward at 1 inch/sec
(2.54 cm/sec) without any angular velocity. This creates a straight line desired trajectory for the actual robot, which attempts to follow this trajectory and the results of the test are shown in Figure 3.4. The same data is also reported in Figure 3.5 with a different set of scaling for the axes, which better demonstrates the fact that resultant error is in the order of 10e-02 cm.
26
Figure 3.4: Plots of the actual mobile base trajectories against the reference robot’s trajectories for a straight line desired trajectory lead by the reference robot.
27
Figure 3.5: Rescaled Plots of the actual mobile base trajectories against the reference robot’s trajectories for a straight line desired trajectory lead by the reference robot.
28
3.5.2
Test 2 In the second test, we prescribe a forward velocity, for the virtual robot to be 1
inch/sec (2.54 cm/sec) and set the angular velocity to be 10 degrees/sec. This creates a requirement for our virtual robot to move in a circular trajectory once every 36 seconds, which the real system attempts. The results of the test conducted for 48 seconds are seen below:
Figure 3.6: Plots of the actual mobile base trajectories against the reference robot’s trajectories for a circle desired trajectory lead by the reference robot. 29
3.5.3
Test 3 In the third test, we set the forward velocity of the reference robot to 1 inch/sec
(2.54 cm/sec) but the desired angular velocity of the virtual robot to be a sine wave of amplitude 5 deg/sec with a frequency of 0.1 Hz. These settings for the velocities of the virtual robot, create a zigzag trajectory for the virtual robot, which our system will attempt to follow this trajectory. The results of the test are seen below:
Figure 3.7: Plots of the actual mobile base trajectories against the reference robot’s trajectories obtained by inputting a sinusoidal wave input to the angular velocity of the reference robot. 30
3.6 Discussion The net result of this testing is that the individual mobile robots, using the ‘Virtual Robot Algorithm’, are capable of following most desired trajectories with minimal errors. Hence in the next sections we will see how we decompose the task of planning and coordination of the system into one of specifying trajectories/velocities for the individual virtual robots, which the actual systems then follow very well.
31
Chapter 4 Mobile Manipulators Mobile Manipulator systems are typically composed of a mobile base platform with one (or more) mounted manipulators. The mobile base can take the form of a gantry system, another manipulator or some wheeled or tracked platform while the mounted manipulator is often selected to be a fully or partially actuated serial chain linkage. Taking advantage of the increased mobility and workspace provided by the mobile base, such systems have found applications in many arenas ranging from forestry to construction to mining and extraterrestrial applications. In our case, the mobile manipulator system consists of a differentially driven wheeled mobile robot with an unactuated planar 2 d.o.f arm mounted on it. This mounted arm is capable of motions in the horizontal plane. Some of the key issues and challenges, which arise for this system, are outlined below: First, combining the mobility of the base platform and the manipulator creates redundancy since the combined system typically possesses more degrees of freedom than necessary. In particular, any given point in the workspace may be reached by moving either the manipulator arm or moving the mobile base or by a combination of the two as can be seen in Figure 4.1. Hence a suitable method of resolution of this redundancy becomes important. 32
Figure 4.1: Mobile Manipulator (Yamamoto and Yun, 1993). Second, if the base mobility were provided by a track or gantry or another manipulator, the kinematics of the base platform are very similar to those of the mounted manipulator, and, the base may be treated as additional revolute/prismatic joints of the mounted manipulator. However, if the base platform takes the form of a wheeled mobile robot, it is subjected to non-integrable velocity level kinematic constraints and the resulting class of “nonholonomic mobile manipulators” requires special treatment. Lastly, most past work, on resolution of redundancy, focuses on fully actuated manipulator systems. The presence of kinematic loops and/or unactuated joints within the chain often also creates a need for special treatment.
4.1 Kinematic Redundancy Resolution Schemes The determination of the actuator rates for a given end effector motion of a redundant manipulator is typically an under-constrained problem. A number of schemes have been proposed in the literature for the resolution of the redundancy. The principal underlying theme is one of optimizing some measure of performance based on kinematics of the system and in some cases extended to include the dynamics. However, in this thesis, we focus our attention purely on kinematic redundancy resolution schemes. Whitney (1969) first proposed the use of the pseudo-inverse of the manipulator Jacobian to determine the minimum norm solution for the joint rates of a serial chain 33
manipulator that can give a desired end-effector velocity. A weighted pseudo-inverse solution approach is also possible to take into account different capabilities of different joints as discussed in Nakamura (1991). A variant of this approach includes the superposition of the null space component of the Jacobian on the minimum norm solution to optimize a secondary objective function. Similarly, modeling the joint compliance (actual or through an artificial potential function) has been shown to be an effective method for resolving the redundancy. However, it is important to note that the resulting solutions may be purely local, in that, cyclic end effector trajectories may not result in cyclical joint trajectories causing the configuration of the system to drift as noted by several authors (Klein and Maciejewski, 1985; Wang and Kumar, 1993). In the context of mobile manipulators, several of these results have been extended and applied to such systems resulting in two principal approaches, which will be briefly outlined next. •
The first approach builds on the method for resolution of kinematic redundancy proposed by Whitney (1969) and is exemplified by a recent paper by Seraji (1998). In this paper, Seraji proposes an approach to motion
control
of mobile manipulators which incorporates the
nonholonomic base constraints directly into the task formulation as kinematic constraints and thus treats the base nonholonomy and kinematic redundancy in a unified manner. Seraji also proposes the addition of further used defined constraints (in differential form) to augment the full rank Jacobian which may then be inverted to uniquely obtain the joint rates. However, as Wang and Kumar (1993) note, if the combined Jacobian is not the Jacobian of a kinematic function f(q), the resulting trajectories may not be globally integrable. Since, the set of additional constraints that can guarantee such global integrability tends to be very restricted, Seraji (1998) proposes a Baumgarte-type stabilization approach to stabilize the solution to the desired configuration manifold.
34
However, this approach assumes and hinges on a fully actuated manipulator configuration, which makes it difficult to adapt this approach
to our case, since our mobile manipulator possesses a mixture of active and passive joints. •
An alternative approach to the resolution of the redundancy was proposed by Yun & Yamamoto (1993). In this approach, the motion of the mobile manipulator is decomposed into two subsystems based on the concept of preferred operating region. The mobile platform is controlled so as to bring the manipulator into a preferred operating region/configuration with respect to the mobile platform as the manipulator performs a variety of unknown manipulation tasks. The authors note that while the preferred operating region may be selected using a variety of criteria, they use the manipulability measure of the manipulator arm to define the preferred operating region. The authors also present results of an experimental implementation using a TRC ‘LabMate’ WMR with a mounted PUMA 250 for dragging and following tasks. The principal advantage of this approach is that it permits decentralized planning and control of the mobile base and the manipulator arm. However, the case when the manipulator is mounted at the center of the axle between the two driving wheels lies at a singularity in the method proposed by Yamamoto and Yun (1993). The approach we have developed is derived from this decentralized approach but additionally permits us to handle the case when the base of the two-link manipulator arm is mounted at the center of the axle.
35
4.2 Proposed method for control of a mobile manipulator with passive joints In this section we develop a method for controlling mobile manipulators, which is a variant of the method proposed by Yamamoto & Yun (1993). The principal difference lies in the fact that the proposed approach can easily accommodate the case when the manipulator is mounted at the midpoint of the axle between the 2 driven wheels.
Figure 4.2: Schematic diagram of our 2-link mobile manipulator.
36
Figure 4.2 shows a wheeled mobile robot which has a passive two-link manipulator mounted at the midpoint of the axle between the two driving wheels. A Frame OM X M YM is attached to the wheeled mobile robot with the X axis oriented in the direction of forward travel of the mobile robot. The Frame OE X EYE denotes the end effector of the 2-link manipulator, which can be located relative to the 2 d.o.f planar manipulator. Our developed control scheme may be summarized as follows: The scheme is based on trying to maintain a fixed configuration between the end-effector’s frame of reference with respect to the mobile platform. While the use of passive joints in the manipulator permits accommodation of position and orientation errors between the endeffector and the mobile base, the configuration of the manipulator is very prone to drift. Noting if the manipulator can hold the fixed configuration, then prescribing a desired trajectory for the end-effector’s frame of reference defines the desired trajectory for the frame of reference mounted on the mobile base. The mobile base is controlled to follow this desired trajectory using the control scheme developed in Chapter 3. The net result is that the end-effector of the mobile manipulator now attempts to follow a given reference trajectory while maintaining a predetermined configuration of the manipulator with respect to the platform. The principal difference from the scheme of Yamamoto and Yum (1993) arises from the fact that the mounting of the base of the manipulator atop the midpoint of the driving wheels creates a direct coupling between the orientation of the base and the first link’s rotation. Once the desired manipulator configuration is determined (as parameterized by θ1 and θ 2 ), the forward kinematics equations may be used to determine the desired position
(
M
)
X E , M YE and orientation
M
Θ E of the end effector with respect to the mobile base as:
M
é X E ù é L1 cosθ1 + L2 cos(θ1 + θ 2 ) ù ê Y ú = ê L sin θ + L sin(θ + θ ) ú . 1 2 1 2 û ë Eû ë 1 M Θ E = θ1 + θ 2
37
(4.1)
The current configuration of the mobile base is described by three variables
( X M , YM ,φ )
which define the position and orientation of the frame OM X M YM attached to
the mobile robot relative to the global frame OF X F YF or simply OXY . Hence, the forward kinematics may be rewritten to show the location of the endeffector, E, in the global frame, F, as: F
F
éXE ù é X M ù é cos φ ê Y ú = ê Y ú + ê sin φ ë Eû ë M û ë F Θ E = φ + θ1 + θ 2
− sin φ ù é L1 cos θ1 + L2 cos(θ1 + θ 2 ) ù cos φ úû êë L1 sin θ1 + L2 sin(θ1 + θ 2 ) úû .
(4.2)
Due to the location of the base, we note that the rotation of the basejoint θ 1 and the orientation of the mobile base φ are coupled by a relationship of the form:
γ = φ + θ1 .
(4.3)
Noting, with this coupling, Equation 4.2 may be rewritten as: F
F
éXE ù é X M ù écos γ ê Y ú = ê Y ú + ê sin γ ë Eû ë M û ë F ΘE = γ + θ2
− sin γ ù é L1 + L2 cos(θ 2 ) ù cos γ úû êë L2 sin(θ 2 ) úû .
Given a desired end-effector trajectory
F
X Ed , FYEd and
F
(4.4)
ΘdE , a preferred
d
manipulator configuration θ 2 we may use these equations to obtain expressions for the F
X Md , FYMd and γ d as shown below:
γ d = ΘE d − θ 2d F
é X Md ù ê d ú= ë YM û
F
é X E d ù écos γ d ê d ú−ê d êë YE úû ë sin γ
d − sin γ d ù é L1 + L2 cos(θ 2 ) ù ú. úê cos γ d û êë L2 sin(θ 2 d ) úû
38
(4.5)
(4.6)
At this stage we have not clearly resolved the calculated γ
d
into a desired
d
d
orientation of the mobile base φ and a desired orientation of the 1st link θ1 . This may be done by differentiating Equation 4.4 to yield: F
F é X E ù é X M ù écos γ − sin γ ù é − L2 sin(θ 2 ) ù = ê ú ê ú+ê ú ê L cos(θ ) ú θ 2 sin γ cos γ Y Y ë ûë 2 2 û ë Eû ë M û é − sin γ − cos γ ù é L1 + L2 cos(θ 2 ) ù +ê . úê ú γ ë cos γ − sin γ û ë L2 sin(θ 2 ) û = γ + θ Θ E 2
(4.7)
Since we would like to hold the manipulator at a predetermined configuration we d will now require that θ 2 =0. Given the desired velocity of the end effector F X Ed , FYEd and F
d , we can now use Equation 4.7 to obtain an expression for the desired velocities for Θ E
the origin of frame M with respect to the global reference frame F as: F
é X M d ù ê d ú= ëê YM ûú d γ d = Θ
F
é X E d ù é − sin γ d ê d ú−ê d ëê YE ûú ë cos γ
d − cos γ d ù é − L2 sin(θ 2 ) ù d ú γ úê − sin γ d û ëê L2 cos(θ 2 d ) ûú .
(4.8)
E
Note that frame M is rigidly attached to the mobile robot and aligned with its X axis in the direction of forward travel of the mobile robot. Since the nonholonomic constraints of the mobile robot do not permit velocities in any other direction, this now permits us to determine the desired orientation of the frame M with respect to the global frame F as:
φ
d
æ FY d ö = tan −1 ç F Md ÷ , è XM ø
(4.9)
and the magnitude of the desired forward velocity to be, vd =
( X ) + (Y ) d M
2
39
d M
2
.
Finally, noting the relationship in Equation 4.3 we see that, d
d
θ1 = γ
d
−φ ,
(4.10)
which implies that, d d θ1 = γ − φ d
= γ − ω
d d
.
Since in the ideal case we do not want the configuration to drift we will require d θ1 = 0 which yields,
ω
d
= γ
d
d. =Θ E
(4.11)
Thus, the velocity level relationship combined with the requirement for zero sideslip permits the composite angle γ to be resolved into its two individual components. d ) can Thus, any desired trajectory for the end-effector ( F X Ed , FYEd F ΘdE , F X Ed , FYEd F Θ E d
d
now be translated into a desired trajectory for the mobile robot ( X M , YM , φ d , v d , ω d ). This information can now be provided as input to the virtual robot algorithm discussed in Chapter 3; and the actual mobile base will now attempt to follow this desired trajectory as closely as possible in a decentralized manner. This above approach permits the creation of trajectories for the mobile bases in response to specified trajectories for the end-effector with the following features: •
The overall motion for the system is generated as if the manipulator is a rigid body while still permitting accommodation of the relative positioning errors between frames M and E.
•
The desired trajectories for the mobile bases obey the nonholonomic constraints imposed on the bases.
40
•
The overall method is relatively simple and well suited for implementation as an online planning algorithm.
We now examine the performance of this method of control in two situations. •
In the first case, the mobile robot uses its odometry to estimate the current position of the frame M at all times. This method would be the most appropriate method if the joints of the planar arm were unsensed.
•
In the second case, the current position of the frame M is estimated at all times relative to the end-effector frame E by sensing the joint angles of the planar 2 d.o.f arm.
In both cases, the end effector E is assumed to be undergoing a prescribed motion along a known trajectory in the global reference frame; or sensed through some independent means and made available to the system. The motivation for conducting these two tests is described below: We note that since the joints of the manipulator are unactuated, the configuration may drift from the desired configuration due to a variety of reasons including bumps, disruption and inertial effects. The above algorithm provides a means for correction of this drift provided it can be detected. When the joints of the articulated 2 d.o.f linkage are unsensed, the sole method for detection of drift is based on odometric estimation of the current position of the WMR. However when the joints of the planar 2 d.o.f arm are sensed, a very good measurement of the relative location of the frame E with respect to the frame M can be obtained at all times permitting corrective actions to be taken to restore the original desired configuration.
41
4.3 Experimental Setup and Evaluation In both cases, a straight line trajectory was prescribed for the end-effector. This motion was realized by moving the end-effector along the global X axis at a fixed rate of 1 in/s (2.54 cm/sec) for 48 seconds using another robot. This was then used to generate the corresponding desired trajectory for the mobile base (frame M), which is labeled “Desired” in all the subsequent graphs. The corresponding odometric estimate for the location of frame M was also calculated at each timestep and shown in subsequent figures labeled as “Odometry”. Similarly, the articulation based estimate of the location of frame M is shown labeled as “Actual”. In each case, a disruption was introduced in the form of a bump, which causes severe (and instantaneous) change in the configuration of the mobile base as well as the manipulator arm. The experimental data collected in the two cases is presented next in the form of 4 graphs per case. The first graph presents the trajectory of the frame M (both position and orientation) in the XY plane during the test while the remaining three graphs present the componentwise time histories of three configuration variables ( X M , YM and φM ).
(a)
42
(b)
(c) 43
(d) Figure 4.3: Case A: Odometric Estimation of frame M used in the control of the Mobile base (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. The graphs above show that the large but instantaneous disturbance given to the system was virtually undetectable by the system, which estimated the location of frame M using odometry alone. While, the articulation based estimation of the position of frame M did detect the disruption, in case A we do not use this additional information. Hence a large offset results between the desired and actual configurations of the manipulator. It is also important to note that even in the absence of explicit disturbances, small “systematic errors” are inevitable due to the nature of the odometry estimation. Specifically, incorrect estimation of critical values for mobile robot parameters such as base length or wheel radius can cause incorrect estimation of the location of Frame M, leading to similar results. Thus, the lack of internal sensing of the manipulator configuration may cause the configuration of the manipulator to drift.
44
(a)
(b)
45
(c)
(d) Figure 4.4: Case B: Articulation based Estimation of Frame M used for the control of the Mobile base (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. 46
The same test is performed here to examine the response of the system to a disruption when the sensed angles of the articulation are used to locate the frame M (relative to Frame E and thus to Frame G). This correction takes the form of creation of a new “Corrected Desired” trajectory for the frame M to which the “Actual” trajectory of frame M converges quickly. The rate of convergence is determined by the selection of the gains for the virtual robot algorithm of chapter 3 which permit the actual mobile robot to track the desired trajectory.
47
Chapter 5 Collaboration
of
Two
Mobile
Manipulators:
A
Leader-Follower
Approach We now examine a system of two mobile manipulators, which can collaborate to collectively transport the common object. When the common object is placed on the end effectors of the two mobile manipulators as shown in Figure 5.1 (a) it forms an effective link and thus the combined system may be treated as a system of 2 mobile robots linked together by a 4 bar linkage as shown in Figure 5.1 (b). The bases of the individual articulated manipulators become the fixed pivots of the 4 bar linkage while the object takes on the role of the coupler link.
48
(a)
(b)
Figure 5.1: Collaboration of Two Mobile Manipulators (a) carrying common object (b) forms effective 4 bar linkage. Hence, one method of controlling such a system is to expand on the mobile manipulator concept developed in Chapter 4. In this case, the manipulator mounted on the mobile base (called Mobile Platform A) has an added link and a rotary joint and forms a 3-link planar manipulator. However, instead of possessing a free end effector as shown as shown in Figure 5.2 we attach it rigidly to the midpoint of the two driven wheels of a second mobile robot (called Mobile Platform B). This Mobile Platform B now takes on the role of the leader and can be controlled to follow any trajectory normally feasible for a differentially driven mobile robot. This, in effect, prescribes the motion of the end effector of the mobile manipulator. By now desiring a fixed predetermined configuration for the manipulator arm we may obtain the desired motion trajectory for Mobile Robot A to follow as closely as possible. Under ideal situations and perfect tracking by the two mobile bases, the entire system behaves like one large rigid body following a prescribed trajectory. However, in reality, small disturbances and deviations from perfect trajectory tracking by the two mobile bases will be inevitable. The three unactuated degrees of freedom in the articulated manipulator arm can now accommodate these relative positioning errors of the mobile bases. Further, by instrumenting this linkage we may now use this internal measurement to detect and correct errors in the relative positioning of the mobile bases without the need for external sensing. 49
This in effect forms the leader-follower scheme for the control of the overall system of two mobile robots collaboratively carrying a common object.
5.1 Problem Formulation
Figure 5.2: Schematic diagram of new 3 link mobile manipulator under analysis. Figure 5.2 shows the schematic diagram of a 3-link mobile manipulator under consideration and depicts the key parameters. 50
Figure 5.3: Schematic diagram of two mobile robot system. In Figure 5.3 we depict how the end-effector frame OE X EYE is rigidly attached to a second mobile robot (Platform B). Note that in the most general case, there is a constant rigid body transformation between the frame OE X EYE and the frame of the mobile robot B ( OB X BYB ). Thus, in effect, mobile platform B provides a mechanism for motion of the end effector and in this particular case will be the set of motions that are compatible with the non holonomic constraints of Mobile Platform B. 51
As in Chapter 4, we will now examine how once a desired trajectory is specified for Mobile Platform B (and thus the End Effector), we can use the forward kinematics equations of the intermediate manipulator to obtain a suitable desired trajectory for the Mobile Platform A. The position and orientation of the end effector of a 3-link mobile manipulator may be written as: F
F
éXE ù é X A ù é cos φ ê Y ú = ê Y ú + ê sin φ ë Eû ë Aû ë Θ E = φ + θ1 + θ 2 + θ 3
− sin φ ù é L1 cos θ1 + L2 cos(θ1 + θ 2 ) + L3 cos(θ1 + θ 2 + θ 3 ) ù cos φ úû êë L1 sin θ1 + L2 sin(θ1 + θ 2 ) + L3 sin(θ1 + θ 2 + θ 3 ) úû . (5.1)
By, once again, noting the intimate coupling between φ and θ1 :
γ = φ + θ1 ,
(5.2)
the above equations can be rewritten as: F
F
éXE ù é X A ù é cos γ ê Y ú = ê Y ú + ê sin γ ë Eû ë Aû ë ΘE = γ + θ 2 + θ3
− sin γ ù é L1 + L2 cos(θ 2 ) + L3 cos(θ 2 + θ 3 ) ù cos γ úû êë L2 sin(θ 2 ) + L3 sin(θ 2 + θ 3 ) úû .
(5.3)
Hence, given a trajectory for Mobile Platform B (MPB) (and thus for the EE frame X Ed , YEd , ΘdE ) and the preferred manipulator configuration ( θ 2d ,θ 3d ), the above equations may be rewritten as:
γ d = Θ E d − θ d 2 − θ 3d F
é X Ad ù ê d ú= ë YA û
F
é X E d ù écos γ d ê d ú−ê d êë YE úû ë sin γ
− sin γ d ù é L1 + L2 cos(θ 2 ) + L3 cos(θ 2 + θ 3 úê cos γ d û êë L2 sin(θ 2 d ) + L3 sin(θ 2 d + θ 3d ) d
d
d
) ù , (5.4) ú úû
d d Similarly by differentiating Equation 5.4 and noting that θ2 and θ3 must be zero
in order to maintain the desired configuration, since we would like:
52
θ 2 = θ 2 d ,θ 3 = θ 3 d θ2 = 0,θ3 = 0 γ = ΘE d −θ 2d −θ3d d γ = Θ E
,
(5.5)
we may write:
d γ d = Θ E F
é X A d ù ê d ú= ëê YA ûú
F
é X E d ù é − sin γ ê d ú−ê ëê YE ûú ë cos γ
− cos γ ù é − L2 sin(θ 2 ) − L3 sin(θ 2 + θ 3 ) ù d . (5.6) ΘE − sin γ ûú êë L2 cos(θ 2 ) + L3 cos(θ 2 + θ 3 ) úû
This is the desired velocity of the frame OA X AYA that is rigidly attached to MPA. Since we know that MPA can only produce this velocity in the direction of its forward travel, this effectively determines the orientation of frame OA X AYA as well as the magnitude of the forward velocity as: d 2 2 vA = X A + YA
æ Y ö . φ A d = tan −1 ç A ÷ è XA ø
(5.7)
Noting the relationship in Equation 5.3, we may also write:
θ1d = γ 1d − φ A d . θ1d = γ1d − ω A d
(5.8)
If we now place an additional restriction that we do not desire θ1 to drift we obtain a condition on the angular velocity of MPA as: d d d wA = φA = Θ E .
d
d
(5.9)
Thus, all the necessary parameters ( X M , YM , φ d , v d , w d ) to give the trajectory of the virtual (reference) robot for MPA can thus be determined. 53
5.2 Experimental Setup and Evaluation A straight line trajectory is prescribed for mobile platform B (and thus the end effector of the mobile manipulator). Given a desired configuration of the manipulator arm, we then use the algorithm described in the previous section to obtain a desired trajectory for MPA. As in the case of Chapter 4 we introduce a large disruption by causing one of the wheels of MPA to run over a bump. As discussed in the previous chapter we present the results of two scenarios. •
In the first case, MPA obtains an estimate of its current position by odometric estimation for use in the control
•
In the second case, MPA obtains an estimate of its current position relative to MPB using the sensed articulations for use in the control.
(a)
54
(b)
(c) 55
(d) Figure 5.4: Case A: Odometric Estimation of Frame M used in the control of MP A with respect to MPB (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. As in the previous chapter, we see that the system using odometry alone is unable to detect or correct for changes in the relative position of MP B with respect to MP A. As can be seen from Figure 5.4, the odometry estimation indicates that Mobile Platform A was successful in closely following the desired trajectory. However the data obtained from using the articulations was able to detect the errors between the frames of references of MP B and MP A.
56
(a)
(b)
57
(c)
(d) Figure 5.5: Case B: Articulation based Estimation of Frame M used for control of MPA with respect to MP B (a) XY trajectory of Frame M; (b) Orientation versus Time; (c) X position of Frame M versus Time; and (d) Y position of Frame M versus Time. Hence, when the articulation based estimation of current position of MPA is used for the control, we note that the system can not only detect such a disturbance, but it now can correct for it, as well, to restore the original configuration of the articulation as
58
desired. Whether it follows such a trajectory with respect to an absolute frame of reference is beyond the scope of this research. While the system is able to maintain the relative configuration it must be noted that it would be impossible to detect errors relative to a global reference frame if both robots undergo identical slip simultaneously. Detection of such absolute errors would require an external reference and is beyond the scope of this research.
59
Chapter 6 Collaboration
of
Two
Mobile
Manipulators: A Decentralized Approach Another method of controlling a system of two collaborating mobile robots based on the mobile manipulator control approach (proposed in Chapter 4) is discussed here. In this method, we now treat a frame attached to a point of interest on the common object as the end-effector frame of both the flanking mobile manipulator systems. Thus, we may now specify a desired trajectory for this object frame, which then provides the desired reference trajectories for the two mobile platforms using the framework developed in Chapter 4. This is illustrated using Figures 6.1 and 6.2.
60
Figure 6.1: Schematic diagram of two mobile robot system, made up of two mobile manipulators connected at their respective end effectors.
Figure 6.2: Schematic diagram of two, separate and independent, 2-link mobile manipulators, under analysis.
61
Thus while the combined system has the same configuration as the system discussed in Chapter 5, a decentralized control scheme is now possible. Each two-link mobile manipulator now attempts to control its configuration with reference to this common end effector frame mounted on the object. The principal benefit of such a scheme is that it lends itself well to specification of the task in terms of a frame attached to the common manipulated object.
6.1 Problem Formulation The formulation of the problem is identical to the formulation for a 2-link mobile manipulator developed in Chapter 4 with the following precautions being taken. •
The points of attachment of the physical manipulators with respect to the reference frame fixed in the object need to be known a priori since this defines the effective configuration of the 2nd link of each mobile manipulator.
6.2 Experimental Setup and Evaluation A straight line trajectory is prescribed for the frame attached to the object and a desired configuration is specified for each mobile manipulator system relative to this frame. The object frame was required to travel forward with a velocity of 1 in/s and zero angular velocity. As in the previous cases, a large disruption is introduced by causing one of the wheels of MPA to run over a bump and the results from two scenarios discussed below are presented. •
In the first case, both mobile platforms use the odometric estimation of their current position for purposes of control. 62
•
In the second case, both mobile platforms estimate their current position relative to the frame mounted on the common object by sensing the configuration of the articulations.
(a)
(b)
63
(c)
(d) 64
(e) Figure 6.3: Case A: Odometric Estimation of Frames M of MP A and MP B, used in the control of MP A with respect to MP B (a) XY trajectory of Frame M of MP A; (b) XY trajectory of Frame M of MP B; (c) Relative Orientation, between MP A and MP B, versus Time; (d) X distance, between MP A and MP B, versus Time; and (e) Y distance, between MP A and MP B, versus Time. As in the previous chapter, we see that the system using odometry alone is unable to detect or correct for changes in the relative position of MP B with respect to MP A. As can be seen from Figure 6.2, the odometry estimation indicates that Mobile Platform A was successful in closely following the desired trajectory. However the data obtained from using the articulations was able to detect the errors between the frames of references of MP B and MP A.
65
(a)
(b)
66
(c)
(d)
67
(e) Figure 6.4: Case B: Articulation based Estimation of Frames M of MP A and MP B, used for the control of MP A with respect to MP B (a) XY trajectory of Frame M of MP A; (b) XY trajectory of Frame M of MP B; (c) Relative Orientation, between MP A and MP B, versus Time; (d) X distance, between MP A and MP B, versus Time; and (e) Y distance, between MP A and MP B, versus Time.
68
Chapter 7 Optimal Isotropic Design of Articulations In the previous two chapters we discussed the process of controlling the MDOF system that is formed when two mobile manipulators carry a common object. During this process, we had examined the control regulation of the two base platforms to ensure the maintenance of a desired configuration of the manipulator arms (or alternatively the relative position of the frames of reference mounted on the object and mobile platform). The selection of this desired configuration plays a critical role in the overall ability of the system to carry the common object while accommodating and carrying for relative positioning errors, and will be the focus of this chapter.
7.1 Background One indicator of this ability to accommodate relative positioning errors comes from the measure of manipulability of a suitably mounted end effector. Since, their original proposition (Salisbury and Craig, 1982; Yoshikawa, 1985) manipulability indices have seen extensive use in both analysis as well as design of robotic systems. Yoshikawa (1985) is credited with the first comprehensive treatment of manipulability for general open chains while Salisbury and Craig (1982) are credited with the first use of manipulability in a design context. Since then various alternative formulations and various aspects of manipulability have been investigated by a number of authors. See 69
Nakamura (1991) for a list of references. As Bicchi and Prattichizzo (2000) note the kinematic manipulability index in the sense of Yoshikawa, serves to define a ratio of a measure of performance in the task space to a measure of effort in the joint space. Thus the manipulability index carries the connotation of the efficiency of the manipulator arm. Salisbury and Craig (1982) consider the manipulability index as relating differential end effector motions to differential joint motions. In this context the same manipulability index may now be considered as a ratio between the norm of the errors in positioning the end effector to the norm of the errors in controlling the joints to their set points. For a ‘n’ d.o.f. serial chain manipulator working in an ‘m’ dimensional task space
(m ≤ n)
(
a linear relationship exists between the joint rates of the manipulator θ& ∈ ¡ n
)
and the set of end effector velocities ( x& ∈ ¡ m ) . This may be expressed in matrix form as: x&
m×1
= éë J (θ ) ùû m×n θ&n×1 ,
(7.1)
where J (θ ) is the configuration dependent m × n Jacobian matrix. A unit sphere in the joint velocity space ( ¡ n ) is called the manipulability ellipsoid and describes the flexibility of the end effector motion in response to a unit norm motion in the joint velocity space. Thus, this definition of manipulability can be used as an indicator of the efficiency of the manipulator in generating certain task space velocity vector in response to a unit norm joint space velocity vector. A weighted norm of the joint velocity vectors is also possible to take into account the different actuator capabilities by normalizing each joint velocity by the maximum possible velocity. Further, for a serial chain manipulator, the Jacobian also features in the relationship between the joint torques and the task space forces as:
τ n×1 = éë JT ùû n×m Fm×1 .
70
(7.2)
Hence a unit sphere in the joint Torque space (τ ∈ ¡ n ) is mapped into an ellipsoid in the end effector force space ( F ∈ ¡ m ) as:
τ Tτ = 1 = F T ( JJ T ) F .
(7.3)
The resulting ellipsoid is the force manipulability ellipsoid and represents the flexibility of the end effector to apply a desired force. We note that:
•
The principal axes for the force manipulability ellipsoid coincide with the motion manipulability ellipsoid
•
The lengths of the principal axes for the force manipulability ellipsoid are the reciprocals of the corresponding motion manipulability ellipsoid which implies that the directions of greatest motion manipulability are the directions in which the least forces may be exerted and vice versa.
A number of authors have shown the geometric relationship between the resulting manipulability ellipsoid and the singular values and singular vectors of the J (θ ) (or correspondingly the eigenvalues and eigenvectors of JJ T ). Given the singular value decomposition of the Jacobian matrix as:
J (θ ) = U å VT ,
(7.4)
where ( U ∈ ¡ m×m ) and ( V ∈ ¡ m×m ) are the orthogonal matrices and å is given by:
å = diag (σ 1 ,K , σ m ) ∈ ¡ m×n
σ 1 ≥ σ 2 ≥ Lσ m ≥ 0
.
(7.5)
L The σ i ' s are called the singular values and the column vectors of U ( u i ) are called the singular vectors of the Jacobian matrix. Thus the principal axes of the resulting 71
L manipulability ellipsoid are given as σ i ui . Thus, the direction of the principal axis is L given by the vector ui , (which is a unit vector), and the magnitude of the principal axis is
given by σ i . These may also be related to the eigenvalues and eigenvectors of JJ T since the singular values are the square roots of the corresponding eigenvalues and the singular vectors are the eigenvectors of JJ T . A number of quantitative measures have been proposed for manipulability which may all be interpreted in the context of the singular value decomposition of the J (θ ) . For example Yoshikawa (1985) introduced a measure of manipulability to quantify the manipulability as:
ω = det(J (θ ) JT (θ ) .
(7.6)
Noting that the U and V matrices in the singular value decomposition are orthogonal the above equation reduces to:
ω = det ( å åT ) = σ 1σ 2σ 3 Kσ m .
(7.7)
Thus geometrically this manipulability index is proportional to the volume of the manipulability ellipsoid. Salsbury and Craig proposed the use of the condition number of the Jacobian matrix as a means to evaluate the workspace quality. In terms of the singular values this condition number may be expressed as k=
σ min , σ max
(7.8)
where σ min and σ max are the smallest and largest singular values of J (θ ) , respectively. Thus the condition number has been used both as a measure of the proximity to singularity and as a measure of kinematic accuracy for the design of manipulators. Several authors (Angeles et al, 1997) prefer the use of a measure of manipulability 72
defined as the inverse of the condition number due to better numerical behavior. One criticism of the measure of manipulability as defined by Yoshikawa is that it condenses considerable information about the manipulability ellipsoid into a single number representative of the volume. Thus, the measure is unable to distinguish between a small, (but near spherical), and a considerably larger (but near singular) manipulability ellipsoids. In contrast by using the condition number (or its reciprocal) as a measure, several authors have designed manipulators to realize configurations where the condition number of the Jacobian is close to (or equal to) 1. Under these conditions, the Jacobian is isotropic in that it preserves distances and angles such that a unit sphere in the joint space is mapped into a corresponding unit sphere in the task space. It is in this spirit that we will adopt a measure of manipulability as, f =
σ max −1 , σ min
(7.9)
which will serve as our objective function for our optimization. Numerous extensions to manipulability analysis have been studied both in the context of multiple cooperating robot arms (Bicchi et al, 1995; Chiacchio et al, 1991) as well as parallel manipulators (Gosselin and Angeles, 1990; 1991). Both cases of manipulators feature one of more closed kinematic loops, which tend to complicate the manipulability analysis. The distinction arises in that multiple cooperating robot arms often also feature redundancy of actuation, which is typically not seen in parallel manipulators. Park and Kim (1996, 1998) present an elegant geometric framework for analysis of manipulability of both such classes of systems. As they note in their work, closed kinematic loops present a number of subtleties that are not often seen in open kinematic chain manipulators.
73
•
Notably, the kinematic configuration space of a closed loop manipulator is no longer a flat space but becomes a curved manifold embedded in a higher dimensional vector space.
•
Dual to the open chain case, the forward kinematics of closed loop manipulators are more difficult to solve than their inverse kinematics.
•
Since often time only a subset of the joints tends to be actuated the selection of actuator locations can and does affect the manipulability.
They note that while a number of coordinate-based formulations have been proposed for manipulability of systems with closed kinematic loops, particular case needs to be exercised due to some of the unique nonlinear characteristics of such systems. In doing so they propose a coordinate-free, differential geometric formulation arguing the case for treating manipulability in a coordinate free manner independent of the nonlinearities introduced by the curved configuration manifold or the taskspace consisting of rigid body displacements. Further, they claim that their geometric method has the ability to uniformly treat redundantly actuated arm systems as well as exactly actuated parallel manipulator systems in a unified fashion.
7.2 Outline of our Optimal Isotropic Design Method In our work we will focus on optimization of the parameters and the configuration of the articulations of the systems of 2 mobile manipulators carrying the common object to attain isotropic manipulability of the common object. The task frame will be assumed to be attached to the object and the closed kinematic loop is formed between the mobile bases due to the nature of the articulated connections. One of the mobile platforms (MP A) will be assumed to be fixed and the effective Jacobian of the system relating the motions of the other mobile platform to the motions of the object mounted end effector frame. 74
The measure of manipulability defined in terms of the singular values of this effective Jacobian will serve as the objective function in our optimization based search for an isotropic configuration. A number of equality constraints are introduced to limit the design search space resulting in a constrained optimization problem. We then examine the process of explicitly solving the constant equations to eliminate several of the optimization variables. Thus, the problem is transformed into an unconstrained optimization problem in a reduced design space and is solved numerically. Further, since the reduced design space is low dimensional (one and two dimensional) we also examine the nature of the obtained solution by performing a parameter sweep over the independent variables and visualizing the results. Adopting the method of Park and Kim:
K represents the k-dimensional manifold corresponding to the ambient space, and M denotes the m-dimensional
( m < k ) curved
manifold corresponding to the joint
configuration space of the mobile manipulator system. Finally, N denotes the ndimensional manifold corresponding to the end effector space of the mobile manipulator system. Let x = ( x1 ,K , x k ) and u = (u1 ,K , u m ) and f = ( f 1 ,K , f n ) represent coordinates on K , M and N . Since M is embedded in K, x can be written explicitly as a function of
u , i.e., x = x(u) . Similarly the forward kinematics may be written as:
{f : M → N | u → f ( u )} . {x : M → K | u → x ( u )}
(7.10)
The Riemannian metrics on each of these spaces may be expressed as positive definite matrices E,G and H , on K, M and N respectively. The existence of these mappings creates a corresponding mapping of the Riemannian metrics through the tangent map. Hence in the presence of the forward kinematics map f : M → N the Riemannian metric of N is mapped onto the manifold N as: J T HJ ,
75
(7.11)
.
where J = ∇u f .
7.3 Problem Formulation Our system consists of two differentially driven mobile platforms with an effective 4 bar linkage formed when a common object is placed on the two passive 2R manipulators mounted on each platform. A schematic of the resulting system is shown in Figure 7.1 along with the various joint angles and the relevant link parameters. The Li ’s are the various link lengths, and the φ ’s denote the relative angles between the frame mounted on the i th link and the previous frame using the DH notation. L4 is the length of virtual baseline between the centers of the two mobile platforms while γ is the angle made by this baseline with respect to the frame of reference mounted on Mobile Platform A (MP A). x and y are the position coordinates of the center of Mobile Platform B (MP B) with respect to MP A and δ represents the orientation of the frame of reference of MP B with respect to the frame of reference of MP A. For the rest of this model, we will consider MP A to be fixed and only consider the relative motions of MP B. Further, we will also consider, the angle φ4 to be held at a constant value by some means. The midpoint of the commonly supported object which forms the effective coupler link of the four-bar (link 2) is selected to be the end-effector of the overall system. Our goal is to determine that configuration of the system that will yield isotropic manipulability of the object.
76
Figure 7.1: Schematic of the overall system depicting the critical model parameters. To this end, we need to first obtain the effective Jacobian of the system, which maps the actuated degrees of freedom (the wheels of MP B) to the velocity of the end effector frame mounted on the object. The various variable parameters of the system may be expressed as a vector of generalized coordinates as: q = [φ1 φ2 φ3 φ4
L4 γ
77
x
y δ ].
(7.12)
The forward kinematics of the articulated linkage may be written in an inertial frame of reference, instantaneously coincident with the frame of MP A as:
L1 cos φ1 + L2 cos φ12 + L3 cos φ123 = x
(7.13)
L1 sin φ1 + L2 sin φ12 + L3 sin φ123 = y
δ − φ4 = φ1 + φ2 + φ3 ,
(7.14)
where, cos φi ,i +1,L,n −1,n = cos(φi + φi +1 + L + φn −1 + φn ) sin φi ,i +1,L,n −1,n = sin(φi + φi +1 + L + φn −1 + φn )
.
(7.15)
The position of MP B with respect to MP A may also be written in terms of the length L4 and angle γ as:
x = L4 cos γ y = L4 sin γ
.
(7.16)
These above equations place constraints on the various generalized coordinates reducing the number of independent generalized coordinates to define the configuration of the system. Thus, the configuration space manifold of the current system may be expressed in implicit form as: æ L1 cos φ1 + L2 cos φ12 + L3 cos φ123 − x ö ç ÷ ç L1 sin φ1 + L2 sin φ12 + L3 sin φ − y ÷ ÷=0. Φ(q) = ç δ − φ4 − φ1 − φ2 − φ3 ç ÷ L4 cos γ − x ç ÷ ç ÷ − sin γ L y 4 è ø
(7.17)
By differentiating the above equations we get:
( q ) = éê ∂Φ ( q ) ùú q = A ( q ) q = 0 . Φ ë ∂q û
78
(7.18)
é a11 a12 êa ê 21 a22 Φ(q ) = ê 1 1 ê 0 ê0 êë 0 0
a13 a23 1 0 0
0 0 0 cos γ sin γ
0 0 0 − L4 sin γ L4 cos γ
1 0 0 1 0
0 1 0 0 1
é φ1 ù ê ú φ 0ù ê 2 ú ê φ ú 0úú ê 3 ú L 1ú ê 4 ú ú ê γ ú 0ú ê ú ê x ú 0úû ê ú y ê ú êë δ úû
(7.19)
where, a11 = − L1 sin φ1 − L2 sin φ12 − L3 sin φ123 , a12 = − L2 sin φ12 − L3 sin φ123 , a13 = − L3 sin φ123
.
(7.20)
a21 = L1 cos φ1 + L2 cos φ12 + L3 cos φ123 , a22 = L2 cos φ12 + L3 cos φ123 , a23 = L3 cos φ123
By
partitioning
q 1 = éëφ1 φ2 φ3
the
vector
T L4 γ ùû and q 2 = éë x
of
generalized
velocities
into
two
parts
T y δ ùû we may partition the matrix A ( q ) in to
two submatrices, A1 , A 2 such that: A ( q ) q = éë A1 ( q ) ùû 5×5 q 1 + éë A 2 ( q ) ùû 5×3 q 2 = 0 .
(7.21)
Thus, we can now express the vector of dependent velocities q 1 in terms of the vector of independent velocities q 2 as: q 1 = − [ A1 ]
-1
[ A 2 ] q 2 .
79
(7.22)
However, the velocities q 2 = éë x
T
y δ ùû are themselves not independent since
they pertain to the velocities of the frame mounted on MP B, which are subject to the nonholonomic constraints on the platform. With respect to an inertial frame, instantaneously fixed at the location of mobile platform A, the nonholonomicity of the mobile platform B may be expressed as: x cos (δ ) − y sin (δ ) = 0 .
(7.23)
Thus, the three velocities may also be expressed in terms of two independent velocities of the mobile platform: the forward velocity of MP B in the forward direction, v , and its angular velocity, ω = δ in matrix form as:
é x ù é cos δ ê y ú = ê − sin δ ê ú ê êëδ úû êë 0
0ù évù 0úú ê ú . ω 1 úû ë û
(7.24)
Given the constant transformation that maps the forward velocity, v , and angular velocity, ω , to the velocities of left and the right wheels, θL and θR of the differentially driven mobile robot as: é r év ù ê 2 êω ú = ê r ë û ê− ëê b
rù 2ú ú rú b ûú
éθL ù ê ú, ëθ R û
(7.25)
where r is the radius of the wheels of our mobile platform, b is the length of the wheel base. We may now write an expression for the q 2 = éë x T u = éëθL θR ùû as:
80
T y δ ùû in terms of the
é x ù é cos δ ê y ú = ê − sin δ ê ú ê êëδ úû êë 0
0ù é r ê 2 0 úú ê r 1 úû êê − ë b
rù éθ ù 2 ú éθL ù ú ê ú = [ K ]3×2 ê L ú r ú ëθ R û ëθ R û , b úû
(7.26)
where, é r ê 2 cos δ ê r K = ê − sin δ ê 2 ê ê −r êë b
r ù cos δ ú 2 ú r − sin δ ú . ú 2 ú r ú úû b
(7.27)
Thus, Equation 7.22 can be written in terms of the vector of independent wheel velocities as: q 1 = − [ A1 ]
−1
[ A 2 ][ K ] u = [ Ψ ]5×2 u .
(7.28)
The Jacobian that maps the velocity vector of the end effector in terms of some of the joint rates may be written as: L é − L sin φ1 − ( 2 ) sin φ12 é xobj ù ê 1 2 ê y ú = ê ë obj û ê L cos φ + ( L2 ) cos φ 1 12 êë 1 2
L2 ù ) sin φ12 ú éφ ù 2 ú ê 1 ú , L φ ( 2 ) cos φ12 ú ë 2 û ú û 2
−(
(7.29)
where xobj and y obj are the velocity of the center point of the object in the x and y direction respectively with respect to an inertial frame instantaneously fixed at the
location of MP A’s frame of reference. The vector of dependent velocities may be expressed in terms of the independent velocities as:
éφ1 ù é Ψ11 ê ú=ê ëφ2 û ë Ψ 21
Ψ12 ù Ψ 22 úû
81
éθL ù ê ú, ëθ R û
(7.30)
where ψ ij is the entry on the i th row and j th column of the matrix defined in Equation 7.30. Thus, the combined system Jacobian maybe written as: L2 é ê − L1sφ1 − ( 2 ) sφ12 J=ê ê L cφ + ( L2 )cφ 1 1 12 ëê 2
L2 ù ) sφ12 ú 2 ú L2 ( )cφ12 ú ûú 2
−(
é Ψ (1,1) Ψ (1, 2) ù ê Ψ (2,1) Ψ (2, 2) ú . ë û
(7.31)
We now examine the singular value decomposition of this system Jacobian to determine the nature of the manipulability ellipsoid, as ell as to optimize it to attain the isotropic configuration. This approach works well in this case since the system is not redundantly actuated. However, by adopting the approach of Park and Kim (1998) we note that we will also be able to handle the case when the system is redundantly actuated. The method of Park and Kim (1998) is summarized in the Appendix A and if we try to represent our system now in terms of the geometric framework presented by Park and Kim (1998), we would have the following definitions. The ambient space K is parameterized by the vector of generalized coordinates, x , while the space of minimal (independent) coordinates, M, is parameterized by the vector, u , where: x = [φ1 φ2 φ3
L4 γ
θ L θ R ]T ,
T
u = [θ L θ R ] .
(7.32) (7.33)
Therefore the gradient of x with respect to u may be written as:
é − A1−1A 2 K ù ê ú ∇ u x = ê1 0 ú, ê0 1 ú ë û
82
(7.34)
and the matrix E as: é0 ê0 ê ê0 ê E = ê0 ê0 ê ê0 ê0 ë
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 1 0
0ù 0 úú 0ú ú 0ú . 0ú ú 0ú 1 úû
(7.35)
Thus the projected metric G can be computed to be: é1 0 ù G = (∇u x)T E(∇u x) = ê ú, ë0 1 û
(7.36)
and the H matrix may be written as: é1 0 ù H=ê ú. ë0 1 û
(7.37)
The eigenvalues of JG −1J T H correspond to the lengths of the principal axes of the manipulability ellipsoid and could alternatively be used to formulate the objective function for our optimization even in the presence of redundant actuation.
7.4 Optimization Case Study We now perform optimization to determine the isotropic configuration of the overall system. Our objective function is chosen to be: f (χ ) =
σ max −1 , σ min
(7.38)
over a vector of optimization parameters χ containing all the relevant design parameters that can be chosen as: 83
χ = [ L1 , L2 , L3 , L4 , φ1 , φ2 , φ3 , φ4 , γ , x, y, δ ] . T
(7.39)
For the purposes of this case study we opt to fix several of he parameters of the optimization vector χ . We specify the values for the link lengths L1 , L2 , L3 to be 27.94 cm (11.0 inches) each and pre-specify the relative position ( y =60.96cm (24 inches) and x = 0.00 cm) and orientation ( δ = 0) between the frames of references of the two mobile
platforms. This will inherently fix L4 and γ which have a direct dependence on x and y . This pre-specification of various parameters results in a reduced set of optimization variables χ = [φ1 φ2 φ3 φ4 ] . Further we also note that the loop closure T
equations introduces several constraints among the remaining variables as shown below: x = L1 cos φ1 + L2 cos φ12 + L3 cos φ123 y = L1 sin φ1 + L2 sin φ12 + L3 sin φ123
(7.40)
δ − φ4 = φ1 + φ2 + φ3 Thus the optimization problem can be stated as a constrained problem of the form:
( )
Min f χ χ
s.t.
.
(7.41)
é L1 cos φ1 + L2 cos φ12 + L3 cos φ123 − x ù ê L sin φ + L sin φ + L sin φ − y ú = 0 1 2 12 3 123 ê 1 ú êëφ1 + φ2 + φ3 + φ4 − δ úû
The presence of these constraints implies that the number of independent optimization variables is in fact 1, which we select to be φ4 . The three constraint equations are of a form that can be solved explicitly to eliminate the dependent variables
(φ1 , φ2 , φ3 )
in terms of the independent variables φ4 at every iteration. Thus the
optimization problem is transformed into an unconstrained optimization problem in the single independent variable, which can be solved numerically.
84
However, since the search space is one dimensional, we also explicitly evaluate the objective function for a suitably discretized set of values of φ4 in a desired range (0° to 180°) and visualize the result in the form of the graph shown in Figure 7.2. We would like to note that there exists regions within this range of values where the objective function becomes ill-defined, or equality constraints cannot be satisfied and hence there exist ranges of φ4 for which the corresponding objective function values are not plotted.
( )
Figure 7.2: Plot of objective function f χ for a range of values of φ4 . The plot shows two distinct minima occurring with the lowest value for the range of φ4 between 120° and 145°. We therefore choose a value of 125 degrees as our initial guess for our gradient based numerical optimization search which results in the optimal value of φ4 =128.5915° where the objective function value attains a minimum of 1.9922. it is also noteworthy to see that the value of the objective function does not change 85
significantly around the minimum value. This is interpreted to be indicative of the robustness of the resulting solution to fluctuations in attaining and maintaining the optimal configuration. Based on this optimal φ4 , we can solve for the other 3 angles of our articulation and obtain an optimal configuration for the articulation that would result in isotropic manipulability for the center of the object, which is modeled by the second link in our models which is shown in Figure 7.3 below.
Figure 7.3: Actual Matlab generated plot of the optimal configuration of the dual robot system. 86
Thus in this chapter we present some preliminary results of an optimization based method to determine the isotropic configuration for the entire system. Other optimization studies may now be conducted by eliminating some of the (excessive) restrictions that we placed on the set of optimization variables and remains to be developed in future work.
87
Chapter 8 Virtual and Physical Prototyping of ARNOLD In this chapter, we discuss the various key aspects of the design, analysis, refinement, and ultimately, prototyping of a system of two cooperating mobile robots. In this process, we adopt a two-stage approach towards the ultimate goal of developing the experimental test bed: •
In the first stage, we examine the use of the tools of virtual prototyping to aid the designer in rapid creation, evaluation and refinement of the model of the overall system within a virtual environment
•
In the subsequent stage, we will discuss the mechanical deign, embedded electronic and software development frameworks that enabled us to rapidly develop the experimental test bed.
88
8.1 Virtual Prototyping Virtual prototyping gives us the opportunity to tryout different designs and various algorithms for the individual components such as the mobile robots as well as the overall system prior to committing ourselves to a specific final design and without running the risk of wearing out mechanical and electronic components during design iterations. However, it is important to note that the utility of such virtual testing is limited by: •
The ability to correctly model and simulate the various phenomena within the virtual environment;
•
The fidelity of the available simulation tools; and
•
Ultimately, the ability of the designer to correctly model the desired system and suitably interpret the results.
In this section, we will briefly discuss our use of software tools such as Computer Aided Design (CAD) packages, dynamic simulation environments and control system design tools to create and refine models of the overall system. 3D Solid models of the mobile platforms and the manipulators of interest are created in a CAD package (Solid Works 99) which not only facilities easy creation of machine drawings for entire assemblies but also the export of the entire model (with their corresponding geometric and material properties) into a dynamic simulation environment (visualNastran 4D). VisualNastran 4D provides a user friendly 3D virtual environment for studying the various phenomena of interest to the mechanical designer such as kinematics, dynamics, contact, collision and friction among others. MATLAB/Simulink is our selected software for developing the control algorithms since it permits: i) High-level (block diagrammatic) development of the control schemes (ii) Bidirectional data interfaces with a number of simulation packages (like visualNastran) to test the developed algorithms by co-simulation; and (iii) Finally, ease of conversion of the developed control 89
algorithm into C-code and ultimately and executable to run in real-time on the embedded control computers on our system. Figure 8.1 shows a screenshot of one such virtual model for a mobile robot within the visualNastran dynamic simulation environment being controlled by an algorithm developed in block-diagrammatic form within Simulink. We also note that while it is important to develop a simulation, which matches the real system in all aspects there are limitations placed primarily by the availability of computing power. Hence in the development of our simulation models, we opted to eliminate considerable amount of geometric detail while retaining the principal kinematic and dynamic characteristics. By doing so we allow more room for rapid evaluation of our different control algorithms under a much broader set of conditions.
Figure 8.1: Screen-shot of simulation of one mobile platform within the visualNastran simulation environment being controlled by algorithm in developed in Simulink. 90
Figure 8.2: Screenshot of visualNastran simulation environment with models two unconnected independent 2-link mobile manipulators. Figure 8.2 depicts a system of two independent 2-link mobile manipulators within the visual NASTRAN environment. When a common object is placed on the end – effector of the two mobile robots an effective articulated system is formed between the two mobile bases as can be seen in Figure 8.3.
(a)
(b)
Figure 8.3: (a) Two mobile manipulators carrying a common object; and (b) Effective articulated linkage formed between the two mobile bases. Three aspects are important during the discussion of the physical prototyping of our system and are outlined below and discussed in more detail in the subsequent three subsections. 91
8.2 Mechanical Development The first aspect is the mechanical development, which in our case involved the mechanical design of the mobile platforms, the articulated arms and other mechanical system components and their assembly. The assembly-level design was done using Solid Works and then produced in the machine shop. Most of the parts were individually machined from aluminum components in the machine shop. However, many others such as the ball-casters, wheels etc. were purchased as off-the-shelf components.
8.2.1
Design of the Individual Mobile Robots Each individual mobile robot system consists of a small mobile platform with two
powered wheels and two unactuated casters. Conventional powered disk-type rear wheels are chosen because of robust physical construction and ease of operation in the presence of bumps, cracks, or any other terrain irregularities. Passive ball casters were preferred over wheel casters to simplify the constraints on maneuverability introduced by the casters. Each mobile base was designed to simplify manufacturing and assembly times and costs. The frame of the first robot was created using angle plates, bolted together, to serve as structural members and, to permit the mounting of motors, casters, and electronics.
92
Angle plates
Castor Supports Motor Supports (a)
(b)
Figure 8.4: First Mobile Platform (a) CAD model and (b) Actual prototype. The advantages of this design include simplicity of design assembly and physical prototyping. However having two independent motor support plates introduces a slight possibility for misalignment of the axes of rotation of the wheels. Hence, a different design was chosen for the second mobile platform to create a solid axle between the two driving wheels. A further advantage of this second design was also the reduction of the number of distinct parts required for the design. Bottom plank
Top plank
Support piece
(a)
(b)
Figure 8.5: Second Mobile Platform (a) Cad Model; and (b) Actual prototype. 93
8.2.2
Design and Assembly of the Top frame, Articulated Arm and Entire system A rectangular frame like structure, created from a series of angle plates is mounted
on each mobile platform. This frame serves to enclose the box containing the control electronics as well as to serve as a mounting point for the 2 d.o.f planar arms as shown in Figure 8.7.
Top Frame
Figure 8.6: Cad model of mobile platform with mounted frame. Each arm shown in Figure 8.7 has two revoulte joints with axes of rotation parallel to each other and perpendicular to the base of the mobile platform. The first joint is placed appropriately at the geometric center on top frame of the platform. The second joint can be placed anywhere along the slotted link. Each joint of the articulated arm is designed to rotate passively about its axis of rotation and incorporates a mechanical interface to attach as optical encoder that can measure the joint rotations.
94
Flat support
Axes of Rotation of joints
2nd Joint Slotted Link
Encoder
1st Joint
Bridge
Brackets
(a)
(b)
Figure 8.7: (a) Cad rendering of articulated arm, (b) Actual mechanical device. A set of brackets, are bolted to the opposite ends of the top support frame. Supported by those brackets is a slotted aluminum segment that acts as a bridge. It is there for mounting the first revolute joint of the arm. Henceforth, this bridge becomes the base of the 2 DOF arm. Bolted to the joint will be another slotted aluminum segment, which we call a “link”. The encoder is then screwed onto the joint-link assembly to form a new assembly. The second revolute joint is then placed in any place of preference along the slotted link. A flat cup shaped aluminum assembly is then bolted to the joint and an encoder is then attached to that particular assembly.
95
The completely assembled 2-link mobile manipulator is shown in Figure 8.8.
(a)
(b)
Figure 8.8: (a) Cad rendering of complete mobile robot, (b) Actual complete mobile robot. The second mobile manipulator utilizes the same platform and articulated arm design. However, instead of 2 completely passive joints, this design now incorporates a motor at the base joint. Figure 8.9 (b), shows the actual second mobile robot that uses the motorized joint. This motor now permits the joint to operate in 2 modes: •
When the motor is switched off the joint now reverts to a passive joint (albeit with much greater damping)
96
•
The motor may be used to control the joint motion along a predetermined trajectory (which includes braking/holding the joint at a predetermined position).
The addition of this motor is important because as noted earlier, for symmetry, we mount a 2 d.o.f planar arm on each mobile platform. The addition of a common object between such 2-link mobile manipulator creates an effective 4 bar linkage between the mobile bases. For a fixed relative distance between the two mobile bases, the articulated linkage now has 1 d.o.f, which must be actively or passively controlled to prevent “failing” motions of the articulated linkage. Effective 4 bar
(a)
97
Motor with encoder
own
Passive Encoders
(b) Figure 8.9: ARNOLD, MDOF vehicle with compliant linkage (a) Cad rendering not showing motorized fourth joint, (b) Actual system showing motorized fourth joint.
98
8.3 Electronics Framework The second aspect is the electronics frame work to support the implementation of our control schemes. The principal control computer is a PC/104 format computer that in addition to standard peripherals has several hardware I/O interface (such as motors driver and encoder reader) cards installed. For the purposes of our research one computer was adequate even to implement the decentralized control scheme. However, if desired, each mobile robot may be retrofitted with its own embedded control computer to achieve true decentralization. Each mobile platform has two independent gear motors, which are used to differentially drive the mobile base. Each pair of such motors is interfaced to a 2-channel PWM output motor driver card. All the joints in the articulated 2-link manipulators are instrumented with encoders, which are interfaced to the computer by means of encoder cards, which provide the power to each encoder and decode the resulting pulse train in quadrature mode. The process of selection of the various components (motors, encoders, motor driver cards, encoder cards) depended on their key performance characteristics. Figure 8.10 shows the general circuit schematics of how we connected the motor drivers to their motors. The set up shows in general that each motor driver is responsible for powering two motors with correct voltage and current, and their respective encoders with their power and data. The third motor driver only is responsible for one motor and its encoder. The encoder card satisfies the three other encoders by providing the necessary 5 volts of power and the data. The motor driver card’s schematics can be looked at in more detail in Figure 8.11, likewise, the encoder card can be also looked at in more detail in Figure 8.12.
99
Figure 8.10: Full schematics of the electronics cards interfaced with the motors and encoders of our system.
100
Figure 8.11: More detailed schematic of the ESC629 motor driver interfacing two motors and their respective encoders.
101
Figure 8.12: More detailed schematic of the DM6814 encoder card interfacing the 3 encoders. The numbers shown coming from the bus connecting the DM6814 to the wires connecting the encoders are pin numbers that assign the correct wire or connection, to the correct pin number on the DM6814 card.
8.4 Software Development Framework The third aspect of the physical prototyping is the development of a real-time framework for actual hardware level control of the mobile robots. A real-time operating system called xPC Target was installed on the PC/104 system, which takes on the role of a target computer. This target computer is able to communicate with a designated host computer using a variety of networking protocols including TCP/IP. All system level software development occurs on a host computer while the real-time execution of the code including access to hardware-in-the-loop components, occurs on the target computer.
102
The host computer with its MATLAB/Simulink graphical user interface provides an environment for system-level software development using a block-diagrammatic language. The compiled executable may then be downloaded over the network and executes in real-time on the target computer, including permitting access to locally installed hardware components such as the encoder cards and the motor drivers. Various critical data elements are logged during the real-time execution and transmitted back to the host computer for visualization or for subsequent offline analysis. Figure 8.13 graphically illustrates the implementation of such a system on one of our mobile robots.
Figure 8.13: Real-Time Control Framework. Electronic components such as encoder cards and motor drivers may also be included within block diagrams provided a standardized block has been created for them. While Simulink xPC Target offers standardized blocks for a number of I/O cards this set is rather limited. 103
Hence this necessitated the creation of a specialized set of blocks to interface the motor driver and encoder cards within block diagrammatic Simulink diagram. These blocks were developed using C Style S-function and successfully integrated into the block diagrams and the details of the creation of such blocks are given in Appendix C.
104
Chapter 9 Conclusion In this thesis, we examined the design, development and successful implementation of a system of two collaborating wheeled mobile manipulators for transporting a common object while maintaining a desired relative configuration. The developed system is capable of accommodating, detecting and correcting disturbances to the relative configuration that arise due to its interaction with the world and the framework developed here can be easily generalized to treat larger systems with further addition of mobile manipulator modules.
9.1 Summary •
We first examined the nature of the nonholonomic constraints imposed by the attachment of multiple disk-wheels to a common platform/object, the role of introducing passive and active articulations between such wheels, and its implications in the context of existent designs prior to proposing our design. A mobile manipulator design, consisting of a differentially-driven wheeled mobile base with a mounted planar passive 2 revolute jointed manipulator, was selected to form the modular basis for the individually autonomous wheeled mobile modules. 105
•
We examined various aspects physical implementation of such a system of cooperating wheeled mobile manipulators. A physical prototype of the cooperating system, consisting of two mobile manipulators was developed to serve as the experimental test-bed as the culmination of considerable amount of design, hardware-integration and implementation efforts.
•
A design environment was developed to aid the designer to rapidly generate, evaluate and refine designs for both the physical hardware embodied by the mobile manipulators as well as the software algorithms used for the control of these electromechanical systems. The paradigm adopted emphasizes: (a) development of the control scheme in a user-friendly, graphical, high-level block diagrammatic language that preserves design intent but permits hierarchical abstraction and encapsulation; (b) simulation, testing and refinement of both the design and control system by virtual prototyping; and (c) rapid conversion of the refined control system into a form suitable for realtime execution on an embedded controller. Our proposed framework permits the designer to combine the benefits of virtual prototyping, hardware-in-theloop simulation and full-fledged experimental implementation within the same framework with minimal change and/or user intervention.
•
We adopted the non-linear control approach termed “Virtual Mobile Robot Algorithm” (Canudas de Wit et al., 1996) as the basis for control of differentially-driven, non-holonomic wheeled mobile platforms for both the Trajectory Tracking problem as well as the Posture Stabilization problem.
Experimental testing of the Virtual Mobile Robot Algorithm was conducted with a variety of trajectories. The results of such testing then aided the suitable selection of control parameters, which would permit the physical mobile platform to follow the trajectory of the virtual robot with minimal error.
106
•
A method for controlling a mobile manipulator system was proposed as a variant of the method proposed by Yamamoto and Yun (1993). In their decoupled mobile manipulator control framework, the mobile base is controlled to bring the manipulator arm into a preferred operating region with respect to the mobile base as the manipulator performs a variety of interaction/manipulation tasks. Our method extends their approach to handle the case when the manipulator arm is mounted at the center of the axle between the two driven wheels, which is a singularity of their approach. The resulting mobile manipulator planning method produces desired trajectories for the wheeled base, which explicitly account for the non-holonomic constraints while permitting treatment of the manipulator system as a virtual rigid body.
•
Further, this developed algorithm is suitable for implementation as an online planning algorithm and was experimentally implemented on each of the mobile manipulators. Using only the internal sensing of the instrumented articulations, this method was demonstrated to be capable of accommodating, detecting and correcting errors between the end effector and the mobile base for the mobile manipulator and maintaining the manipulator arm in a preferred operating region.
•
This mobile manipulator planning and control algorithm was then adapted as a method of controlling a collaborating system of two mobile manipulators carrying a common object in a prescribed relative configuration. Two variants of this method – the Leader Follower Approach and the Decentralized Control approach were developed and experimentally evaluated on the test-bed. The experimental results verified the ability of the combined system to accommodate instantaneous disturbances that could arise due to bumps and other terrain irregularities as well as to detect and correct for deviations from the desired relative configuration.
107
•
A framework for suitable selection of a desired configuration of the multiple mobile manipulator systems to achieve isotropic manipulability of the transported object was also developed. This optimization-based approach enables the designer to select the most suitable dimensions and configuration of the two mobile manipulators with respect the common object and preliminary results are presented.
9.2 Future Work •
Absolute System Configuration: In its current form, the method is unable to detect absolute errors for the configuration of the system with reference to a global reference frame. However, in the presence of a suitable external reference for at least one component of the system (perhaps the common object), the same method can be easily extended to detect absolute as well as relative positioning errors.
•
Other Trajectories: We tested our system for a set of trajectories while holding a fixed trajectory with the intent of demonstrating the ability to recover from disruption to the relative configuration. Hence one interesting test would be to test the ability of the system to actively reconfigure from one given configuration to another during the course of travel along a given trajectory. Further, since, different relative configurations would to be more suitable for maintaining isotropic manipulability of the common object for different trajectories, this would provide a mechanism for active adaptation of the configuration of the system.
•
Modularity: In this thesis, we examined the collaboration of a system of two wheeled mobile manipulators. The framework developed here may be extended to involve three (or more) such mobile manipulators in performing collaborating maneuvers. In particular, the decentralized approach of control the two mobile robots, allows for each of the mobile manipulators to act independently of one another and still be sensitive to 108
each other’s relative motions due to that common physical connection (the object), thus allowing for modularity when more than two such modules are involved. •
Dynamics: Our systems were developed based on kinematic models. Another venue of analysis and development would involve the exploration of such systems using dynamic models. Dynamics brings in another element into the picture. With the inclusion of internal and external forces and acceleration of the WMRs as well as the articulations, we would be able to examine our systems for greater speeds, larger and heavier mobile robots and larger and heavier articulations.
•
Different Articulations: The manipulators mounted on each mobile base have passive revolute (R) joints. Other type joints can be explored such as spherical joints to allow for major disruptions in height (the Z axis) that can occur on uneven terrain.
•
Actuated Articulations: As a consequence of the passive R joints, a passive planar RRRR linkage is formed between the collaborating wheeled mobile platforms. Active actuation and control of these joints raises interesting issues such as the control of the force distribution of the common object between the wheeled mobile robots.
109
Bibliography [1]
A. Bicchi and D. Prattichizzo,
"Manipulability of Cooperating Robots with
Unactuated Joints and Closed-Chain Mechanisms," IEEE Transactions on Robotics and Automation, Vol. 16, No. 4, pp. 336-345, 2000.
[2]
A. Bicchi, C. Melchiorri, and D. Balluchi, “On the mobility and manipulability of general multiple limb robots,” IEEE Trans. Robotics and Automation, Vol. 11, No. 2, pp. 215-228, 1995.
[3]
A. M. Bloch and N. H. McClamroch, “Control of mechanical systems with classical non holonomic constraints,” Proc. 28th IEEE Conf. on Decision and Control, Tampa, FL, pp. 201-205, 1989.
[4]
J. Borenstein et al., "Mobile Robot Positioning-Sensors and Techniques," Journal of Robotics Systems, Vol. 14, No. 4, pp. 231-249, 1997.
[5]
J. Borenstein and David K. Wehe, "Internal Correction of Odometry Errors with the Omnimate," Proceedings of the 7th Topical Meeting on Robotics Systems, pp. 323-329, 1997.
[6]
J. Borenstein, H.R. Everett, and L. Feng., “Where am I ?,” University of Michigan, 1996.
[7]
J. Borenstein, "The CLAPPER: A Dual-drive Mobile Robot With Internal Correction of Dead-reckoning Errors," IEEE International Conference on Robotics and Automation, Vol. 99, pp. 3085-3090, 1994.
110
[8]
J. Borenstein, "Compliant-Linkage Kinematic Design for Multi-Degree-of-freedom mobile robots," SPIE Symposium on Advances in Intelligent Systems, pp. 344-351, 1992.
[9]
R. W. Brockett, “Asympotic stability and feedback stabilization,” in Differential Geometric Control Theory, R.W. Brockett, R. S. Millman and H. J. Sussman (Eds),
Birkhauser, Boston, MA, pp. 181-208, 1983. [10] G. Campion, B. d’Andrea-Novel and G. Bastin, “Modelling and state feedback control of nonholonomic mechanical systems,” Proceedings of the 30th IEEE Conf. on Decision and Control, Vol. 2, pp. 1184-1189, 1991.
[11] G. Campion, B. d’Andrea-Novel and G. Bastin, “Modeling and control of nonholonomic wheeled mobile robots,” Proc. of IEEE Intl. Conf. on Robotics and Automation, Vol. 2, pp. 1130-1135, 1991.
[12] C. Canudas de Wit, Bruno Siciliano, and G. Bastin, “Theory of Robot Control”, Springer-Verlag New York Inc, 1996.
[13] P. Chiacchio, S. Chiaverini, L. Sciavicco and B. Siciliano, “Global task space manipulability ellipsoids for multiple-arm systems,” IEEE Trans. Robotics Autom., Vol. 7, No. 5, pp. 678-685, 1991. [14] C. Gosselin and J. Angeles, “A global performance index for the kinematic optimization of robotic manipulators,” ASME J. of Mechanical Design, Vol. 113, No. 3, pp. 220-226, 1991. [15] C. Gosselin and J. Angeles, “Singularity Analysis of closed loop kinematic chains,” IEEE Trans. Robotics & Autom., Vol. 6, No. 3, pp. 281-290, 1990.
[16] Jin-Oh Kim and Pradeep K. Khosla, "Dexterity Measures for Design and Control of Manipulators," IEEE/RSJ International Workshop on intelligent Robots and Systems IROS, 1991.
111
[17] Manja V. Kircanski, “ Robotic Isotropy and Optimal Robot Design of Planar Manipulators,” IEEE Conference on Robotics & Automation, 1994. [18] A. Maciejewski and C. Klein, “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments,” Intl. J. Robotics Res., Vol 4, pp. 109-117, 1985. [19] Y. Nakamura, “Advanced Robotics: Redundancy and Optimization”, AddisonWesley, 1991.
[20] S. Ostrovskaya, “Dynamics of Quasiholonomic and Nonholonomic Reconfigurable Rolling Robots,” Ph.D thesis, McGill University, Montreal, 2001. [21] E. Papadapoulos and J. Poulakakis, "On Motion Planning of Nonholomic Mobile Robots," Proc.of International Symposium on Robotics ISR, 2000. [22] F.C Park and Jin Wook Kim, “Manipulability and Singularity Analysis of Multiple Robot Systems: A Geometric Approach,” IEEE Conference on Robotics & Automation, 1998.
[23] F. C. Park and J. M. Kim, “Manipulability of closed kinematic chains,” in Recent Advances in Robot Kinematics, J. Lenarcic and V. Parenti-Castelli, eds., Kluwer,
Dordrencht, 1996. [24] F. G. Pin et al, "Autonomous Mobile Robot Research Using the HERMIES-III Robot," IROS International Conference on Intelligent Robot and Systems, pp. 251256, 1989. [25] D. B. Reister, "A New Wheel Control System for the Omni-directional HERMIESIII Robot," Proceedings of the IEEE Conference on Robotics and Automation, Vol. 3, pp. 2322-2327, 1991. [26] J. K. Salisbury and J. J. Craig, “Articulated hands: force control and kinematic issues,” Intl. J. Robotics Reseach, Vol. 1, No. 1, pp.. 4-17, 1982. 112
[27] C.
Samson and K. Ait-Abderrahim, “Feedback control of a non-holonomic
wheeled cart in Cartesian space,” Proc. 1991 IEEE Int.Conf. on Robotics and Automation, Sacramento, CA, pp. 1136-1141, 1991.
[28] C. Samson and K. Ait-Abderrahim, “Feedback stabilization of a non-holonomic wheeled mobile robot,” Proc. 1991 IEEE/RSJ Int. Work. on Intelligent Robots and Systems, Osaka, J, pp. 1242-1247, 1991.
[29] Nilanjan Sarkar, Xiaoping Yun, and Vijay Kumar, “Control of Mechanical Systems with Rolling Constraints: Application to Dynamic control of Mobile robots,” General Robotics and Active Sensory Perception (GRASP) Laboratory.
[30] N. Sarkar, X. Yun, and V. Kumar, “Control of a single robot in a decentralized multi-robot system,” Proceedings of the IEEE Intl. Conf. on Robotics and Automation, Vol. 2, pp. 896-901, 1994.
[31] Homayoun Seraji, "A Unified Approach to Motion Control of Mobile Manipulators," The International Journal of Robotics Research, Vol. 17, No. 2, pp. 107-118, 1998. [32] S. V. Sreenivasan and K. J. Waldron, "Displacement Analysis of an Actively Articulated Wheeled Vehicle Configuration With Extensions to Motion Planning on Uneven Terrain," ASME Journal of Mechanical Design, Vol. 118, No. 2, pp. 312317, 1996. [33] S. A. Velinsky and J. F. Gardner, "Kinematics of Mobile manipulators and implications for design," Journal of Robotics Systems, Vol. 17, No. 6, pp. 309-320, 2000. [34] W. Wannasuphoprasit, P. Akella, M. Pehkin and J.E. Colgate, “Cobots: A Novel Material Handling Technology,” Proceedings of IMECE, 1998.
113
[35] C. -C. Wang, V. Kumar, “Velocity Control of mobile manipulators”, Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 2, pp. 713-
718, 1993. [36] A. M. West and H. Asada, "Design of Ball Wheel Mechanisms for Omnidirectional Vehicles with Full Mobility and Invariant Kinematics," ASME Journal of Mechanical Design, Vol. 117, 1995.
[37] B. H. Wilcox, J. C. Morrison, H. W. Stone, B. K. Cooper, T. T. Nguyen, A. H. Mishkin, “Experiences with operations and autonomy of the Mars Pathfinder Microrover”, IEEE Aerospace Conference, Vol. 2, pp. 337-351, 1998. [38] B. Wilcox, L. Matthies, D. Gennery, B. Cooper, T. Nguyen, T. Litwin, A. Mishkin, H. Stone, “Robotic Vehicles for planetary exploration”, Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 1, pp. 175-180, 1992.
[39] D.E Whitney, “Resolved Motion Rate Control of Manipulators and Human Prosthetics”, IEEE Trans on Man Machine Systems, Vol 10, pp. 47-53, 1969. [40] Yoshio Yamamoto and Xiaoping Yun, "Control of Mobile Manipulators Following a Moving Surface," IEEE International Conference on Robotics and Automation, Vol. 3, pp. 1-6, 1993. [41] T. Yoshikawa, “Manipulability of robotic mechanisms,” Intl. J. Robotics Research Vol. 4, No. 2, pp. 3-9, 1985. [42] K.E. Zanganeh and J. Angeles, “Kinematic isotropy and the optimum design of parallel manipulators,” Int. J. Robotics Research, Vol. 16, No. 2, pp. 185-197, 1997.
114
Appendix A Mathematical Tools The mathematical preliminaries shown are going to be a brief review of the following topics: i) Singular Value Decomposition, ii) Dexterity Measures, and iii) A geometric framework for closed chain kinematic analysis.
A.1 Singular Value Decomposition The singular value decomposition (SVD) is a very significant decomposition of matrices. Certain characteristics of matrices are examined from various information that can be deduced from the SVD. SVD is defined by the following relationship; A = UΣV T ,
(A.1)
where A ∈ R m×n and rank A = k , and, U = (u1 L u m ) ∈ R m×m V = ( v 1 L v n ) ∈ R n×n
,
Σ = diag (σ 1 , L , σ p ) ∈ R m×n , ∆
115
(A.2)
(A.3)
where p = min{m, n}, and,
σ1 ≥ σ 2 ≥ L ≥ σ k > 0 . σ k +1 = L = σ p = 0
(A.4)
In equation A.4, the σ i (i = 1,L, p) are called the singular values of A . Selectively, σ 1 and σ p are referred to as the largest and smallest singular values respectively. The singular values are uniquely determined, although U and V may not be.
A.2 Dexterity Measures The dexterity measures that we will briefly touch upon in this section are a) Measure of Manipulability and b) Measure of ill-conditioning: The Condition Number.
A.2.1
Measure of Manipulability
The degree of manipulability provides information about the quality of kinematic structure in executing tasks described by the manipulation variable. In general, the measure of manipulability (simply manipulability) can be defined as
{
}
w = det J (θ)J T (θ) .
(A.5)
Manipulability also has a relationship with singular values. The singular value decomposition of the Jacobian Matrix is :
J (θ) = UΣV T ,
(A.6)
where U ∈ R m×m and V ∈ R n×n are orthogonal matrices, and Σ is given by,
Σ = diag (σ 1,L, σ m ) ∈ R m×n
σ1 ≥ σ 2 ≥ L ≥ σ m ≥ 0
116
,
(A.7)
where the σ i s are called singular values. Considering that U and V are orthogonal matrices, substituting Eq. A.6 into Eq. A.5 results in, w = det( ΣΣ T )
(A.8)
= σ 1σ 2 Lσ m .
(A.9)
Geometrically, this manipulability is proportional to the volume Ve of the manipulability ellipsoid whose principal axes have the same magnitudes as singular values of the Jacobian matrix; m
π2 Ve = dw , where d = , éæ m ö ù Γ êç ÷ + 1ú ëè 2 ø û
(A.10)
⋅ is the gamma function. where the function Γ[]
A.2.2
Measure of ill-conditioning: The Condition Number The condition number of a matrix, has been used to evaluate property of linear
equations. Salisbury and Craig (1982) proposed that we use the condition number of the Jacobian matrix to evaluate workspace quality. The condition number of the Jacobian matrix is defined as:
κ=
σ max , σ min
(A.11)
whose lower bound is 1and where σ max and σ min are the largest and the smallest singular values of the Jacobian matrix, respectively. These singular values are equal to the square roots of the maximum and minimum eigenvalues ( λ max and λ min ) of JJ T . The condition number can be used for different purposes, some of these are 1) as a measure of proximity
117
to singularity, 2) as a measure of kinematic accuracy and 3) in general as a measure of illconditioning.
A.3 A Geometric framework for closed chain kinematic analysis In this section we introduce the Riemannian geometric formulation for closed chain manipulability as described by Park and Kim (1998). For an m degree-of-freedom mechanism, the joint configuration space forms an m dimensional surface in a higher-dimensional (possibly curved) ambient space. The
joint configuration space of a planar five-bar closed-loop linkage, for example, forms a two-dimensional surface in the five-dimensional torus T 5 . Let K denote the kdimensional manifold corresponding to the ambient space, and M denote the mdimensional manifold corresponding to the joint configuration space of the mechanism; here we use the term “manifold” in a looser sense than the strict mathematical definition, to include manifolds containing boundaries and other singular sets of measure zero. Let N denote the n-dimensional manifold corresponding to the end effector space of the mechanism. For example, if orientations are ignored, then N will be ℜ 2 for a planar mechanism, and ℜ 3 for a spatial mechanism. In the general spatial case N is usually taken to be the special Euclidean group SE(3).
Figure A.1: Metric conversions and their respective Manifolds. 118
Let x = ( x 1 ,K, x k ) and u = (u1 ,K , u m ) denote local coordinates on K and M, respectively. Since M is embedded in K, x can be written as a function of u , i.e., x = x(u) . Let f = ( f 1 ,K , f n ) be local coordinates on N, and also denote the forward
kinematics f : M → N in local coordinates by f (u) . We use the notation J = ∇u f to represent the derivative of f with respect to u . The Riemannian metrics on K, M and N can then be expressed in local coordinates by the symmetric positive definite matrices E,
G and H, respectively. Now, any symmetric function of the roots of det(Gλ − J T HJ ) = 0 will be a coordinate-invariant function defined on the Riemannian manifold M. At a regular point,
J by definition will have maximal rank, and proper values of f * h will have exactly n nonzero values (assuming m ≥ n) ; we label these, in descending order, as
λ , λ2 ,K, λn . These proper values are also eigenvalues of JG −1JT H . We now discuss the issue of the choice of Riemannian metrics. The basic strategy is to choose the metric e on the ambient space K corresponding to the virtual work performed by the mechanism; the metric g is then obtained by projecting e onto M. since passive joint do not contribute to virtual work, we need only consider the actuated joints in formulating the metric e. To illustrate, consider once again the five-bar planar closedloop linkage, in which the five revolute joints are parametrized by local coordinates x1 through x5 . The metric e is then chosen to be, 5
ds 2 = å ε i dxi , 2
(A.12)
i =1
where ε i = 1 if the i'th joint is actuated, and 0 otherwise. The corresponding matrix E in this case is diagonal, with 1’s on the diagonal entries corresponding to the actuated joints, and all other entries 0. Note that this procedure uniformly treats both nonredundantly actuated cases. Moreover, it can be generalized to mechanisms containing more complex joint types, be selecting a suitable set of local coordinates for the joint, and choosing a metric such that only actuated joints contribute to work . In general, however, the 119
actuated joints of most mechanisms are one degree-of-freedom revolute or prismatic joints, in which case the metric E on K will as before be a diagonal matrix composed of 1’s and 0’s. Observe that E may only be positive semidefinite, and hence speaking a pseudo-Riemannian metric. Once the metric e on ξ is chosen, the metric on M is obtained by projecting e into M as follows. Since dx = ∇u x ⋅ du it follows that, 1 T dx E dx 2 . 1 T T = du (∇u x) E(∇u x)du 2
ds 2 =
(A.13)
The projected metric on M, denoted G, is therefore G = (∇u x)T E(∇u x).
120
(A.14)
Appendix B Tuning the Virtual Robot B.1 Effectiveness of the control algorithm for an initial error/offset in the x direction of the robot The data obtained is used to analyze the error between the actual robot’s distance and the reference robot’s distance in the x direction. An initial error is given to the system, by having the actual robot think it is 5 inches behind the reference robot. Each set of graphs show how the three errors (error in the x direction, error in the Y direction and orientation error) behave due to the control algorithm as the actual robot tries to follow the reference robot as it starts off with this error/offset. Two control gains in the algorithm are thereby modified to test their effectiveness on the control algorithm’s performance. The two gains that we modify for this test are b, and ξ (please refer to chapter with control algorithm). The ξ term was modified 5 times and it took on these values: 0,0.5,0.9,1.2 and 1.5. For each of these values we modified the b coefficient 5 times as well, and it took on these values: 0,0.03,0.05,0.07,0.09. Therefore the graphs below are listed in order from lowest ξ value to highest ξ value, depending on the set of values stated above. In the z-axis we plot the error term in the x direction that we choose to minimize in the algorithm. This value is plotted versus time (x axis), for different values of b (y axis), as seen below. 121
Error IN X
Error IN Y
X ERROR behaviour depending b & ksi=0
Y ERROR behaviour depending b & ksi=0
7.5
0.03
7 ERROR IN Y
ERROR IN X
0.02 6.5 6 5.5
0.01 0
5 4.5 0.1
-0.01 0.1 6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=0.5 X ERROR behaviour depending b & ksi=0.5
0.025 6
0.02 ERROR IN Y
ERROR IN X
4 2
0.015 0.01 0.005
0
0 -0.005 0.1
-2 0.1
6
6 0.05
0.05
4
4 2
2 0
b gain
0
0
b gain
TIME(seconds)
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=0.9 X ERROR behaviour depending b & ksi=0.9
5
40
0 ERROR IN Y
ERROR IN X
30 20 10
-5
-10
0
-15 0.1
-10 0.1
6
6 0.05
0.05
4
4 2
2 0
b gain
0
0
b gain
TIME(seconds)
0
TIME(seconds)
X ERROR behaviour depending b & ksi=1.2 Y ERROR behaviour depending b & ksi=1.2
50 5 0
30 ERROR IN Y
ERROR IN X
40
20 10 0 -10 0.1
-5 -10
-15 0.1
6 0.05
4
6 0.05
4
2 b gain
0
0
2
TIME(seconds)
b gain
122
0
0
TIME(seconds)
X ERROR behaviour depending b & ksi=1.5 Y ERROR behaviour depending b & ksi=1.5
50 5 0
30
ERROR IN Y
ERROR IN X
40
20 10 0 0.1
-5 -10
-15 0.06
6 0.05
6
0.04
4 2 0
b gain
0
4
0.02
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Orientation Error Phee ERROR behaviour depending b & ksi=0 Phee ERROR behaviour depending b & ksi=1.2
0.2 0 ERROR IN Phee
ERROR IN Phee
50
0.1
0
-0.1
-50 -100 -150 -200
-0.2 0.1
-250 0.1
6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=1.5 Phee ERROR behaviour depending b & ksi=0.5
100
0.1
ERROR IN Phee
ERROR IN Phee
0.2
0
-0.1
0 -100 -200
-300 0.1
-0.2 0.1
6
6 0.05
0.05
4
4 2
2 b gain
0
0
b gain
TIME(seconds)
0
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=0.9
50
ERROR IN Phee
0 -50 -100 -150 -200 -250 0.1 6 0.05
4 2
b gain
0
0
TIME(seconds)
Figure B.1: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ . 123
The general behavior shows that the X error drops for lower values of b as time progresses. The optimum performance seems to be when ξ is 0.9 and b is 0.03 or 0.05. If we look at the graphs, which represent the ξ = 0.9 data, we can see that the X error converges fastest to a minimum of around zero. In addition to this we see that at b=0.03~0.05, the errors in the Y direction and the error in orientation of the robot are kept at a minimum as well. This is what we desired from the control algorithm. When the
ξ =1.2 and 1.5, there is quick convergence to a minimal error in the X, but we end up with large distortions of error in the Y direction and the orientation of the robot. This essentially means that as the actual robot tries to correct for that initial offset/error that was given to it in the X direction, it ended up finding the error too large for correction and it begun to react uncontrollably causing errors in all the other directions and its orientation. Therefore in terms of finding a good set of values for the control algorithms’ gains ξ =0.9 and b=0.05 seem to be optimum choices.
124
B.2 Effectiveness of the control algorithm for an initial error/offset in the orientation of the robot The data obtained is used to analyze the error between the actual robot’s distance and the reference robot’s distance in orientation. An initial error is given to the system, by having the actual robot think it is lagging 90 degrees w/r/t the reference robot in orientation. Each set of graphs show how the three errors (error in the x direction, error in the Y direction and orientation error) behave due to the control algorithm as the actual robot tries to follow the reference robot as it starts off with this error/offset. Two control gains in the algorithm are thereby modified to test their effectiveness on the control algorithm’s performance. The two gains that we modify for this test are b, and ξ (please refer to chapter with control algorithm). The ξ term was modified 5 times and it took on these values: 0,0.5,0.9,1.2 and 1.5. For each of these values we modified the b coefficient 5 times as well, and it took on these values: 0,0.03,0.05,0.07,0.09. Therefore the graphs below are listed in order from lowest ξ value to highest ξ value, depending on the set of values stated above. In the z-axis we plot the error term in the x direction that we choose to minimize in the algorithm. This value is plotted versus time (x axis), for different values of b (y axis), as seen below.
125
Error IN X
Error IN Y Y ERROR behaviour depending b & ksi=0
X ERROR behaviour depending b & ksi=0
0 6
-1 ERROR IN Y
ERROR IN X
4 2 0
-2 -3 -4
-2
-5 0.1
-4 0.1
6
6 0.05
0.05
4
4 2
2 b gain
0
0
0
b gain
TIME(seconds)
X ERROR behaviour depending b & ksi=0.5
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=0.5
0.03
0.04
0.02 ERROR IN Y
ERROR IN X
0.02 0.01 0 -0.01
0
-0.02
-0.02 -0.03 0.1
-0.04 0.1 6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=0.9 X ERROR behaviour depending b & ksi=0.9
0.04 0.02
0.02 ERROR IN Y
ERROR IN X
0.01 0 -0.01
0
-0.02
-0.04 0.1
-0.02 0.1
6
6 0.05
4
0.05
4 2
2 b gain
0
0
TIME(seconds)
b gain
126
0
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=1.2
0.03
0.02
0.02
0.01
0.01
ERROR IN Y
ERROR IN X
X ERROR behaviour depending b & ksi=1.2
0 -0.01
0 -0.01 -0.02
-0.02 -0.03 0.1
-0.03 0.1 6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
X ERROR behaviour depending b & ksi=1.5 Y ERROR behaviour depending b & ksi=1.5
0.03 0.03 0.02
0.01 ERROR IN Y
ERROR IN X
0.02
0 -0.01 -0.02
0.01 0 -0.01 -0.02
-0.03 0.1
-0.03 0.1
6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Orientation Error Phee ERROR behaviour depending b & ksi=0 Phee ERROR behaviour depending b & ksi=0.9
92.5
80 ERROR IN Phee
ERROR IN Phee
100
92 91.5 91
90.5 0.1
40 20 0 0.1
6 0.05
60
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=1.2 Phee ERROR behaviour depending b & ksi=0.5
100 100
ERROR IN Phee
80
ERROR IN Phee
80 60 40 20
60 40 20 0 0.1
0 0.1
6
6 0.05
4
0.05
4 2
2 b gain
0
0
TIME(seconds)
b gain
127
0
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=1.5
100
ERROR IN Phee
80 60 40 20 0 0.1 6 0.05
4 2
b gain
0
0
TIME(seconds)
Figure B.2: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ . The general behavior shows that the orientation error drops for higher values of ξ as time progresses. We also notice that this time we are independent of b. The optimum performance seems to be when ξ is 1.5 and b is any value. We do get great conversion when ξ =0.9,1.2 as well. Throughout the different trials and different ξ ’ values we do obtain minimal errors in the x direction and the y direction. This is what we desired from the control algorithm. Therefore in terms of finding a good set of values for the control algorithms’ gains ξ =0.9,1.2 or 1.5 and b=any value seem to be great choices.
128
B.3 Effectiveness of the control algorithm for an initial error/offset in the orientation of the robot and the x direction The data obtained is used to analyze the error between the actual robot’s distance and the reference robot’s distance in orientation and distance in the x direction. An initial error is given to the system, by having the actual robot think it is lagging 90 degrees w/r/t the reference robot in orientation and 5 inches be. Each set of graphs show how the three errors (error in the x direction, error in the Y direction and orientation error) behave due to the control algorithm as the actual robot tries to follow the reference robot as it starts off with this error/offset. Two control gains in the algorithm are thereby modified to test their effectiveness on the control algorithm’s performance. The two gains that we modify for this test are b, and ξ (please refer to chapter with control algorithm). The ξ term was modified 5 times and it took on these values: 0,0.5,0.9,1.2 and 1.5. For each of these values we modified the b coefficient 5 times as well, and it took on these values: 0,0.03,0.05,0.07,0.09. Therefore the graphs below are listed in order from lowest ξ value to highest ξ value, depending on the set of values stated above. In the z-axis we plot the error term in the x direction that we choose to minimize in the algorithm. This value is plotted versus time (x axis), for different values of b (y axis), as seen below.
129
Error IN X
Error IN Y Y ERROR behaviour depending b & ksi=0
X ERROR behaviour depending b & ksi=0
0 15
ERROR IN Y
ERROR IN X
-5
10
-10 -15 -20 -25 -30 0.1
5 0.1
6
6 0.05
0.05
4
4 2
2 0
b gain
0
0
b gain
TIME(seconds)
0
TIME(seconds)
X ERROR behaviour depending b & ksi=0.5 Y ERROR behaviour depending b & ksi=0.5
12
0.025 0.02
8 ERROR IN Y
ERROR IN X
10
6 4
0.015 0.01 0.005
2
0
0 0.1
-0.005 0.1
6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=0.9 X ERROR behaviour depending b & ksi=0.9
6 10
ERROR IN Y
ERROR IN X
4 5
0
2 0
-2 0.1
-5 0.1
6
6 0.05
0.05
4
4 2
2 0
b gain
0
0
b gain
TIME(seconds)
X ERROR behaviour depending b & ksi=1.2
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=1.2
10
8 6 ERROR IN Y
ERROR IN X
5 0 -5
4 2 0 -2
-10 0.1
-4 0.1 6 0.05
6
4
0.05
4
2 b gain
0
0
2 TIME(seconds)
b gain
130
0
0
TIME(seconds)
Y ERROR behaviour depending b & ksi=1.5
10
6
5
4 ERROR IN Y
ERROR IN X
X ERROR behaviour depending b & ksi=1.5
0 -5
-10 0.1
2 0
-2 0.1 6 0.05
6
4
0.05
4
2 0
b gain
0
2 0
b gain
TIME(seconds)
0
TIME(seconds)
Orientation Error Phee ERROR behaviour depending b & ksi=0
Phee ERROR behaviour depending b & ksi=0.5
130
100
ERROR IN Phee
ERROR IN Phee
80 120 110 100
60 40 20 0
90 0.1
-20 0.1 6 0.05
6
4
0.05
4
2 b gain
0
0
2
Phee ERROR behaviour depending b & ksi=1.2
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=1.5
100
100
50
50
ERROR IN Phee
ERROR IN Phee
0
b gain
TIME(seconds)
0 -50
-100 0.1
0 -50
-100 0.1 6 0.05
6
4
0.05
4
2 b gain
0
0
2 TIME(seconds)
b gain
131
0
0
TIME(seconds)
Phee ERROR behaviour depending b & ksi=0.9
ERROR IN Phee
100
50
0
-50 0.1 6 0.05
4 2
b gain
0
0
TIME(seconds)
Figure B.3: Plots of the errors in the X, Y directions, and the orientation errors against time, and the gain b for different ξ . It can be noticed that the optimum performance seems to be when ξ is 0.9 and b =0.05 or 0.03. If one can look at the graphs which represent the data obtained for ξ is 0.9, it can be seen that the error in the x direction is converged to around zero for b =0,0.03 and 0.05. The great thing about these graphs is that the error of the orientation has also converged to around zero from 90 degrees as time progresses. However it is noticed that there is faster conversion for the orientation for values of b=0.03 and 0.05, within this set of graphs of ξ is 0.9.
132
Appendix C Simulink Blocks C.1 Simulink Blocks that interface hardware C.1.1
Block for Encoder Card
Figure C.1: DM6814 Encoder Simulink Block that interfaces DM6814 Encoder Card. The code, written for this block, retrieves the signals and words from the encoder card’s registers by accessing the card through its base address, and then converts this encoder data into encoder counts. These encoder counts are then converted into degrees of rotation. This block provides for our convenience the degrees of rotation values for all 133
three encoders that we used in our system. A mask is created for the block which looks like Figure C.2 and which takes the following parameters into the block:
Figure C.2: DM6814 parameters mask requires Sample Time and Base Address.
C.1.2
Block for Motor Driver Card:
Figure C.3: ESC629 Motor Driver Simulink Block interfaces the ESC629 Motor Driver electronic card, showing also required inputs before block and conversion factors after block. Here block is shown for left motor. Each of these motor driver blocks interface the motor driver cards that are onboard the target computer on one of the robots. In Figure C.3, the block is labeled “Esc 629 Left Motor Driver”. This block drives the left motor of the mobile platform’s left wheel, through the communication with the motor driver card. These blocks are again 134
custom designed to retrieve certain inputs as well as some constant values, initial conditions etc, and provide to the wheels the desired rotational position values, in counts. The conversion is then done later converting the counts to degrees as seen in the above figure. A mask is then created for the block, which looks like Figure C.4 and which takes the following parameters into the block:
Figure C.4: ESC 629 motor driver block parameter mask, requires some information. 1. Base Address This is the base address of the actual motor driver card being used. 2. Sample time
135
In each of the motor driver blocks a code is run that was designed to implement our desired operations. In order to know at what sampling rate the block is running at, we have to specify to the code within the block itself at what sampling rate we are running at, so that the code within the block can run at that desired sampling rate. 3. Channel This specifies which channel on the card we wish to communicate to. By inputting a zero or a one into this prompt we can assign to either the first or the second channel on the card, and hence either the first or the second motor, will be communicated to. 4. Filter Control Word This is a 32-bit word that configures our motor driver to operate at the position level. It also allows us to specify how we input the necessary PID control gains to the card. The PID control loop is closed and is taken care of in hardware. All we have to do is provide the proper values fro kp, kd, ki, il. 5. Trajectory Control Word This 32 bit word configures how we set the velocity trapezoid when in the position level. We therefore set the Acceleration and the velocity of the velocity trapezoid to be both maximum. This allows us to reach the position that we wish at the fastest time possible. 6. Acceleration Max. This is the maximum acceleration value that is given to the velocity trapezoid. 7. Velocity Max. This is the maximum velocity value that is given to the velocity trapezoid.
136
C.2 Simulink Models representing the different control Algorithms: The next few figures and sub sections will show the methods by which the Simulink models were constructed. They include the many blocks that interface hardware as well as those blocks that carry out functions in software. Here we show the brief concepts by which the algorithms were modeled.
137
Single Virtual Robot Algorithm (Ch. 3):
138
Figure C.5: Simulink Model of Single Mobile Robot Controlled with Virtual Robot Algorithm.
Trajectory Generator For F/R of reference Mobile Platform.
Virtual Robot Algorithm Block
C.2.1
The virtual robot algorithm block inputs the trajectory data as well as the sampled linear and angular velocity values to prescribe the trajectory for the reference mobile platform. It also inputs the encoder readings of the wheels that dictate the actual position and orientation of the mobile platform (according to odometry), and hence performs control. The resulting controlled rotational wheel velocities are outputted to the left and right motor driver blocks. The arrows that lead to nothing are left free to connect or disconnect to and from scopes for data acquisition.
C.2.2
Single 2-Link Mobile Manipulator Models (Ch.4): The mobile manipulator Simulink models are a slight modification from the
Simulink models of the virtual mobile platform models, in that we now incorporate the 2link manipulator. Chapter 4 shows the steps taken to produce the control algorithms for the 2-link mobile manipulator. Below we show both the control algorithm of a 2-link mobile manipulator controlling using a) wheel odometry and b) the 2-link articulation for control.
139
Virtual Robot Algorithm Block
Scopes
140
Figure C.6: Simulink Model of single 2 link Mobile Manipulator: uses odometry to estimate current position of the MP A.
End effector to mobile platform reference robot trajectory converter
Encoder Card block
Single 2 link Mobile Manipulator, uses odometry to estimate current position of the MP A (Ch. 4):
One of the blocks circled and indicated above is the Simulink subsystem block that receives the trajectory of the end effector of the 2-link manipulator and provides the trajectory for the mobile platform. It then provides this data to the virtual robot algorithm. This model is the model that prescribes a trajectory for the end effector and then using odometry to estimate the current position of the frame Actual Mobile Platform at all times. This Mobile Platform, thus, tries to follow its prescribed trajectory, in order to have the end effector travel the prescribed trajectory.
141
Articulated Control Trajectory Algorithm Block
142
Figure C.7: Simulink Model of single 2 link Mobile Manipulator: uses articulation to estimate current position of the MP A.
Encoder Information Block
Single 2 link Mobile Manipulator, uses articulation to estimate current position of the MP A (Ch. 4):
The new blocks that have been introduced to help perform articulated control are the two blocks that have been shown in the above figure. The ‘Articulated Control Trajectory Algorithm Block’, is the block that receives the same prescribed trajectory for the mobile platform as done in the Odometry control seen before, it also receives the data from the encoders from the ‘Encoder Information Block’, and performs articulated control based on such data. The Algorithm employed in the ‘Articulated Control Trajectory Algorithm Block’ is simply the algorithm discussed in chapter 4.
143
MP A Motor Drivers
4th Motorized Joint
144
Figure C.8: Simulink Model of Leader-Follower Approach: uses odometry to estimate positions of the Actual MP A.
MP B, acting as the end effector
Leader-Follower Approach: Uses odometry to estimate positions of MP A (Ch. 5)
Converts trajectory from EE to MP A
C.2.3
Desired Trajectory for MP A Articulated Control Blk.
MP B, as EE
Leader-Follower Approach: Uses articulation to estimate position of MP A (Ch. 5)
145
Figure C.9: Simulink Model of Leader-Follower Approach: uses sensing of articulation for MP A’s position estimation.
C.2.4
MP A
MP B
Decentralized Approach: Uses odometry to estimate positions of MP A and MP B (Ch. 6):
Figure C.10: Simulink Model of Decentralized Approach: uses odometry to estimate positions of MP A and MP B. 146
C.2.5
MP A
MP B
Decentralized Approach: Uses articulation to estimate positions of MP A and MP B (Ch. 6)
Figure C.11: Simulink Model of Decentralized Approach: uses sensing of articulation for MP A’s and MP B’s position estimation. 147
C.2.6
