Real-Time Control of Trussarm Mark II

Regina Sun Kyung Lee

A thesis submitted in conformity with the requirements for the Degree of Master of Applied Science

Institut e for Aerospace S t die s University of Toronto

@Copyright by Regina Sun Kyung Lee 1995

National Library 191 ofCanada Bibliothèque nationale du Canada

Acquisitions and Acquisitions et Bibliographie Services seivices bibliographiques 395 Wellington Street 395, nie Wellington OttawaON KIAON4 OttawaON K1AOiU4 Canada canada

The author has granted a non- exclusive licence aiiowing the National Library of Canada to reproduce, loan, distribute or sel1 copies of this thesis in microforni, paper or electronic formats.

The author retains ownership of the copyright in this thesis. Neither the thesis nor substantial extracts &om it may be printed or otherwise reproduced without the author's permission.

Yaur iib Vatm r é M f u t œ

Our tïb Narre retérenu

L'auteur a accordé une ticence non exclusive permettant a la Bibliothèque nationale du Canada de reproduire, prêter, distribuer ou vendre des copies de cette thèse sous la forme de microfiche/^, de reproduction sur papier ou sur format électronique.

L'auteur conserve la propriété du droit d'auteur qui protège cette thèse. Ni la thèse ni des extraits substantiels de celle-ci ne doivent être imprimes ou autrement reproduits sans son autorisation.

Abstract

Trussa.rm Mark II is a four-bay tnissarm with 12 degrees of freedom. in this thesis, a control system for Trussarm Mark II was designed. Oniy two bays were act uated for 6-degree-of-&dom cont rol. The two- bay kinemat ics was implemented and the writing application and visual control were designed. System performance was evaluated and the controllers were calibrated to minimize the effects of the structural defects. A four-bay control for Tmssarm Mark II is still under development. This and other improvements wil1 be made in the future*

Acknowledgement s

1 would like to thank my supervisor, Professor Peter C. Hughes for his guidance and encouragement t hroughout the development of t his t hesis.

Special thanks go to Stephen Oikawa and Thierry Cherpillod, and the mem- bers of the Space Robotics Group, for their support and advice. Without the generous help of these people, the completion of this thesis would not have been possible.

My gratitude also extends to my family and friends for their ongoing supports and love.

Contents

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Abstract i . . . . . . . . . . . . . . . . . . . . . . . . . . . Acknowledgements i i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Contents 11

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . List of Figures v

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . List of Tables vi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . List of Symbols vii

1 Introduction 1 . . . . . . . . . . . . . . . . . . . 1.1 Industrial Trussarm Project 1

. . . . . . . . . . . . . . . . . 1.2 Thesis Definition and Objectives 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Thesis Outline 2

1.4 Trussarm Mark II Control Unit: C500 . . . . . . . . . . . . . 5

2 Kinematics 7 2.1 Trussarrn Kinematics Review . . . . . . . . . . . . . . . . 7

. . . . . . . . . . . . . . 2.1.1 One-Bay Tnissann Kinematics 7 2.1.2 Two-Bay T~ssamn Fomrard Kinematics . . . . . . . . 7 2.1.3 Two-Bay Trussarm hverse Kinematics . . . . . . . . . 8

2.2 Kinematics Software Development . . . . . . . . . . . . . . . . 10 . . . . . . . . . 2.2.1 C500 Kinernatics Development Option 10

. . . . . . . . . . . . . . . 2.2.2 Coordinate Transformation 11 . . . . . . . . . . . . . . . . . . 2.2.3 Software Development 13

2.2.4 System Parameters . . . . . . . . . . . . . . . . . . . . 13 2.2.5 Implementation . . . . . . . . . . . . . . . . . . . . . . 14

2.3 Kinematics Initialization and Tests . . . . . . . . . . . . . 14

3 Sewomechanism and Tool Positionhg Control 17 3.1 Servomechanism~ . . . . . . . . . . . . . . . . . . . . . . . . 17

. . . . . . . . . . . . . . . . . . . . . . . . 3.2 Task Space Control 18

. . . . . . . . . . . . . . . . . . . . . . . . 3.3 Writing Application 20 . . . . . . . . . . . . . . . . . . . . 3.3.1 System Description 20 . . . . . . . . . . . . . . . . . . . . 3.3.2 Control Algorithm 22

. . . . . . . . . . . . 3.3.3 Results and Future Consideration 22

4 Visual Control 26 . . . . . . . . . . . . . . . . . . . . . . 4.1 Visual Control Concept 26

. . . . . . . . . . . . . . . . . . . . . . . . 4.2 System Description 27 . . . . . . . . . . . . . . . . . . . . . 4.3 Visud Control Software 31

. . . . . . . . . . . . . . . . . . . 4.3.1 Vision Data Transfer 31 . . . . . . . . . . . . . . . . . . . . 4.3.2 Control Algorithm 32

. . . . . . . . . . . . . . . . . . . . . 4.3.3 Motion Command 32 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Results 34

. . . . . . . . * * . . . . . . . . . . . 4.5 Future Considerations I 34 4.5.1 Improvements to the Visud Control System . . . . . . 34

. . . . . . . . . . . . . . . . . 4.5.2 Four-Bay Visual Control 35

5 Systern Evaluation and Calibration 36 . . . . . . . . . . . . . . . . . . . . . . . . 5.1 System Evaluation 36 . . . . . . . . . . . . . . . . . . . . . . . . 5.2 System Calibration 40

. . . . . . . . . . . . . . . 5.3 Kinematic Algonthm Modification 43

6 Future Plans for Wuasarm Mark II 45 . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 New Location 45

. . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Improved System 46 . . . . . . . . . . . . . . . . . . . 6.3 Four-Bay Trussm Control 47

7 Conciusion 49

References 51

List of Figures

Trussarm Mark 1 . . . . . . . . . . . . . . . . . . . . . . . . . 2 . . . . . . . . . . . . . . . . . . . . . . . . . Trussarm Mark II 3

. . . . . . . . . . . . . . . . . . . n-Bay Tnissarm Kinematics 9 . . . . . . . . . Geometric Equivalent of a Two-Bay Tnissarm 10

. . . . . . . Trussarm Mark II Servomechanism: PID Control 19 Conceptual Scheme of Hierarchal approach to Cartesian Control 21

. . . . . . . . . . Trussarm Mark II Writing Application Setup 23 . . . Examples of End-Effector Traces for Writing Application 25

. . . . . . The UTIAS Acronym Drawn by Trussarm Mark II 25

The Visud Control System . . . . . . . . . . . . . . . . . . . . 28 . . . . . . . . . . . . . Trussarm Mark II Visual Control Setup 29

PAMI Virtek Cross-Pattern Target . . . . . . . . . . . . . . . 30 . . . . . . . . . . Trussarm Mark II Visud Control Algonthm 33

. . . . . . . . . RAPL commands for System Evaluation Test 37 . . . . . . . . . . . . . . . . . . . . . Cdibration Test Resdts 39

List of Tables

. . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Thesis Objectives 4

. . . . . . . . . . . . . 2.1 Trussm Mark II Physical Parameters 16 2.2 RAPL Vilriables used in the Trussarm Mark II Kinematics . . 16

. . . . . . . . . . . . . . 3.1 Trussarrn Mark II PID Control Gains 18

. . . . . . . . . . . . . . . . . . . . . . 5.1 Kinematics Test Result 38 5.2 Kinematics Error Caused by Structural Defects . . . . . . . . 42

List of Symbols

base frame first bay's end plane centroid Erame second bay's end plane centroid b e camera frame pen trame end-effector frame (in general) target Erame (in visual control) Homogeneous Transformation matrix from & to Fe Homogeneous Transformation matrix from Fb to Fi Hornogeneous Transformation matrix £rom Fb to T2 Homogeneous Transformation matrix fiom r2 to FC Homogeneous Transformation matrix from 3' to 7; ith actuator length, 1 5 i 5 6 yaw (rotation about 2-axis) pitch (rotation about y-axis) roll (rotation about x-axis) displacement vector of the end-effector (in general) in 36 displacement vedor of the centroid of the first bay end plane in Fb, also denoted di displacement vector of the centroid of the second bay end plane in Fb displacement vector of the centroid of the ith bay end plane in 36 displacement vector of the second bay end plane with respect to the fist bay end plane expressed in 3 6

displacement vector of the ith bay end plane with respect to the i-lthbay end plane expressed in Fb displacement vector of the vision target expressed in displacement vector of the camera expressed in Fb displacement vector from camera to the target in Fb t'b expressed in 7, baae unit normal normal unit vector of the first bay end plane

n3 normal unit vector of the second bay end plane n normal unit vector of the end-effector (in generd) n, normal unit vector of the camera n, normal unit vector of the pen

viii

Chapter 1 Introduction

1.1 Indust rial Trussarm Pro ject

In 1994, DynaconL and UTIAS2 made plans to develop a new manipula- tor: Trussarm Mark II. The primary goal of this project, called Indus td Trussarm Project, is to develop a four-bay tnissarm3 using a similar design techniques as developed with the existing one-bay model, Trussarm Mark 1 (see Figure 1.1).

Tmssarm Mark II is a quater-scale version of Trussm Mark 1, and has 12 degrees of freedom. The control unit of Trussarm Mark II is a C500 Robotics Controller (hereafter referred to simply as C500), which cm provide control of up to 8 degrees of &dom. Due to this limitation, ody the two bays of Trussarm M x k II were controlled in this project. When the control computer is upgraded, additional control appropriate for all four bays will be designed and applied at that time.

' Dynacon Enterprises Ltd., Domsview, Ontario "niversity of Toronto hstitute for Aeroepace Studies 31n this report, "trussarm" refers to a Yaciable Çeomet y xnuis Manipulator (VGTM)

design, developed at UTIAS.

2 1 2 . THESIS DEFINITION AND OBJECTIVES

Figure 1.1: Trussarm Mark 1

The Industrial Trussm Project has since been completed with a four-bay t m s s m having been successfdy designed and constructed. The working prototype presently resides at UTIAS (see Figure 1.2). The kinematics and control for two bayfi II*

1.2 Thesis

have been implemented and tested on Trussarm Mark

Definit ion and Objectives The objective of this thesis is to design and implement a control system for Trussm Mark II. In order to adiieve this goal, the tasks Iisted in Table 1.1 have been perfomed.

1.3 Thesis Outline This thesis consists of five main sections. First, the methods used in the development and implementation of the two-bay tnissarm kinematics are dis- cussed in Chapter 2. One-bay and multi-bay trussarm kinematics are briefly

Base Pla

Jo in t 3

Joint 2

J o i n t 4

Joint 6

J o i n t 5

End

J o i n t

7-

Figure 1.2: Tmssarm Mark II

1. Trussarm Mark II Kinematics

a Review existing kinematics

a Develop C-code kinematics software

r Modify the kinematics software for C500 implement at ion

r hplement and evaluate the kinematics software

2. Trussarm Mark II Servomechanism and Control

a Review and design the servomechanisrn

a Design an open-loop control routine

3. Tmssami Mark II Visual Control

Develop a PCP-baaed visud control routine

a Integrate the vision system with the C500

a Evaluate the visual control system

4. S yatem Evaluation and Calibration

5. Outline Future Plans for Tmssarm Mark II

Table 1.1: Thesis Objectives

1.4. TRUSSARlM MARK II CONTROL UNIT= CM0 5

reviewed ôt the beginning of the chapter. The two-bay trussarm kinematics software and its implementation on Trussarm Mark II are described.

In Chapter 3, the servomechanisrn of Tn i s sm Mark II and its control software are described. The writing application developed for Trussarm Mark II is presented in detail.

The visual control mechanism of T rus sm Mark II is described in Chapter 4. The control goal, system setup, software development, test results and future considerations are discussed.

Finally, evduation and cdbration methods axe iilustrated in C hapter 5. Once the error sources for the control tests are determined, calibration routines are designed. The improved test results are presented at the end of the section.

Future pians for Trussarm Mark II are detailed in Chapter 6. These include proposed changes to the controller and system setup. Plans for the four-bay trussarm control are also previewed.

1.4 Trussarm Mark II Control Unit: C500

Through Chapter 2 to 4 of this thesis, development of control algonthm to run with the C500 are frequently discussed. In ordet to help readers to undentand the nature of the dgorit hms, supporting software packages used with the C500 are discussed here.

Trussarm Mark II's C500 is an AlphalBeta Controller with one serial communication device. The C500 consists of three processors. One is a 286- compatible CPU board on which the control programs (both the RAPL and PCP programs) are executed; another is a processor dedicated to kinematics; and the third is dedicated to the servomechanism. More information regard- ing C500 AlphalBeta controller can be found in the C500 manuals listed in the References at the end of this thesis.

Robcomm is a software package that provides a programming environ- ment for the C500 on a PC. The Robcomm Version 4.0 has been instailed on the Trussarm Mark II Development PC. Robcomm includes a terminal emulator for direct ASCII communication to the C500. Through this termi- nal emulator, an operator can command variety of RAPL commands in real tirne. Robcomm also permits upload and download of the RAPL and PCP programs, locations and variables. More details regarding the Robcomm's

6 1 .P. TRUSSARM MARK II CONTROL UNIT= C500

functions and usage can be found in the Robcomm-II Manual [3]. Robotic btomation Erogramming b g u a g e (RAPL) is used for most -

CRS Robotics Corporation's family of industrial robots. RAPL Version 2.40 has been instded on the Trussarm Mark II's C500. RAPL commands include motion command, system commands, I/O commands, path commands, and more. Refer to the RAPL manual [5] for more information.

The C500 is also equipped with a software tool cdled Kinematics De- velopment Option, which allows the development and implementation of user-defined kinematics algorit hms. User-defined kinematics dgori t hms are downloaded on the C500 using Firmware Load Utility. The Kinematics Development Option manual [4] may be referred to for details.

Finally, Erocess Control Programfi (PCP) is the library of C-functions, developed by the CRS Robotics Corporation. It dows C500 users to design and implement algorithms that are more flexible and sophisticated t han RAPL programs. It is used like any other C libraries in compilation and iinking. PCP is used for visual control in this thesis. Refer to the PCP manual [6] for more information.

Chapter 2

2.1 Trussarm Kinemat ics Review

2.1.1 One-Bay Trussarm Kinemat ics

The kinematics of a mult i- bay trussarm is based on the kinematics of a one- bay trussarm. The one-bay forward kinematics was solved using Petal Angle Method by Naccarato (refer to Nacmato's thesis [12]), and has since b e n re-evaluated. A closed-form solution for the forward kinematics problem is yet to be found. The one-bay inverse kinematics problem, however, does have a closed-form solution. Sallmen solved one-bay kinematics in chapter 4.1 and 4.2 of his thesis [16].

2.1.2 Two-Bay 'Ikussarm Forward Kinemat ics Forward kinematics of an n-bay Tmssarm, for 2 n E N , is a simple vector addition of n solutions of the one-bay forwaxd kinematics.

where di and &-i,i are displacement vector and rotation matrix of ith bay th end plane with respect to the i - 1 bay end plane. They can be obtained

by solving one-bay forward kinematics wit h t hree actuator lengt hs of the i bay. Refer to Figure 2.1 for the definition of vectors and reference frames.

The two-bay kinematics is a specific case of the above problem for n = 2.

2.1.3 Two-Bay Tkussarm Inverse Kinematics The inverse kinematics of a two-bay tnissarm was developed by Naccarato and Hughes (131.

The two-bay inverse kinernatics involves solving for centroid displacement and orientation of the two bays, di, R2, d2, and fi3, given the five constrôints r (end-effector position) and ii (directional unit normal). The sixth inde- pendent variable is the twist angle, q , about fi3. There are two fundamental problems associated wit h 7: (quoted from [13])

1. The range of motion of q is severely limited.

2. Twist is impossible if il, ii2 and ii3 are all in the same plane.

Therefore, the inverse kinematics is solved for 5 degrees of freedom with one redundancy resolution, 7 (refer to Figure 2.2).

Using the symmetry of the structure, the first centroid displacement and orientation are &en by

Assuming that there is no tool vector, the second centroid displacement is

Newton's the two bays b y:

d2 = r -dl (2-5) ïi3 = n (2.6)

Method is used to solve for a to optimize the configuration of with respect to their workspace. The constant b is related to a

(a + b)l = Ilr - di, - b&l12 (2.7)

2.1. TRUSSARM KLNEMATICS REVEW 9

Figure 2.1 : n-Bay Trussami Kinemat ics

10 2.2. KNEMATrCS SOFTWARE DEVELOPMENT

Figure 2.2:

T herefor e

Geornetric Equivalent of a Two-Bay

Once the centroid displacement and orientation of the two bays are known, the actuator lengths are found using the one-bay inverse kinematics.

2.2 Kinemat ics Software Development

2.2.1 C500 Kinematics Development Option

To implement the two-bay kinematics on the C500, the software was designed in C, and modified to run with the C500. Both forward and inverse kine- matics use the one-bay kinematics software, which was implemented on the C500 by Hertz and Oikawa in May of 1994.

Using the Kinematics Development Option, several kinematics functions were progrilfnmed in the kinematics software, including joiatzo-.orid, woric~-to-joint and HanualJoiPtCheck.

World coordinat es are defined in the Kinematics Development Option as an array of 6 elements: x , y, and z components of the displacement vector, and yaw, pitch, and roll rotation angles, {s, y, t, y , 9, $}. The joint coordinates are the 6 actuator lengths, {Il, . . . , 16} .

The joint coordinates are computed based on the encoder counts. The Kinematics Development Option defines encoder counts as a motor coordi- nate, and cornputes a corresponding joint coordinate based on them. 192 encoder pulses correspond to 1 motor pulse, which corresponds to one revo- lution of the motor. Each revolution of the motor causes 4.4 revolutions of the lead screw', which has 1/20 inches travel distance per revolution. The C500 solves for the joint lengths as follows:

lim = mi/(Transmission Ratio x Rotary Resolution)

2.2.2 Coordinate 'Ikansformat ion

The trussarrn kinematics were origindy developed based on a different defi- nition of world coordinates than those for Kinematics Development Option. The trussarm kinematics defines world coordinates as x, y, z and the three components of the unit normal vector, { x , y,z,ii,,R,,fi,}. The Kinematics Development Option defuies a world coordinate of a point in 3-D space as a combination of its displacement and orientation ables, or in components, { x , y, z, 9 , 8 , $1. Hereinafter, the former definition will be referred to as the task coordinates; and the latter definition as world coordinates. In order to implement two-bay kinematics in the (2500, the transformation between the task coordinates and the world coordinates needs to be defined.

For transformation between the world and the task coordinates, mot her representation of a 6-degree-of-freedom (DOF) coordinate system is intro- duced. A Bomogeneous ~ransfomation Matrix (HTM) is, by the definition given in the Kinematics Development Option, a 4 x 4 matrix:

'10:44 gear ratio waa selected in the actuator design. See [14] for detaib.

12 2.2. KINEMATICS S O F W . ' DEVELOPMENT

where p is the end-effector displacement r, expressed in Fb; and Rb, = [a b n] is the rotation matrix fiom Fb to 7,. The rotation matrix, Rb, c m be computed based

cos cp cos 8

- sin 8

on the world coordinates:

cos cp sin 8 sin $ - sin cp cos iI, cos cp sin 0 cos 11, + sin cp sin 1C, sin 9 sin 0 sin S, + cos cp cos 11 sin cp sin 9 cos 11, - cos cp sin (I>

cos 0 sin $I cos e cos (2.11)

1 and

From the HTM, ii, which coincides with [O O lIT:

the task coordinates can be computed. The unit normal, with the r-axis of Fr, is computed by multiplying Rb,

cos Q sin O cos $ + sin cp sin 1C, sincpsinBcosII> -cosy sinII>

cos e cos * 1 When solving an inverse kinematics problem, the world coordinates are trans- fomed to HTM; then, the HTM is transformed to the task coordinates using (2.13). The inverse kinematic algorithm, discussed in Section 2.1 3, is applied to the resulting task coordinates.

For a forward kinematics problem, task coordinates are determined from joint coordinates by the forward kinematics algorithm in Section 2.1.2. Next, HTM is computed from the task coordinates. The world coordinates axe calculated from the BTM. The yaw(cp), pitch(9) and roll($) can be extracted from the HTM as follows:

2.2. KINEMATICS SOFTWARE DEVELOPMENT 13

2.2.3 Software Development

The forwôrd kinematics algorithm, joiatzo-iorid, solves (2.2) for n = 2. The first bay and the second bay forward kinematics are solved separately to give Hbl and Hl*. The ha1 results are then obtained by mat& multiplication:

where Ha is the end-effector displacement fiom the second bay end plane centroid. The world coordinate representation of Hh. is referred to as the tooi vector in RAPL.

The inverse kinematics algori thm, worlbt O-j o i n t , performs the following:

1. Hbr is computed from the world coordinates using (2.1 1) and (2.12).

2. The second bay end plane centroid displacement is found:

3. eg and ii3 are extracted from Hb2 using (2.12) and (2.13).

4. Using Newton's Method of optimization discussed in Section 2.1.3, dl, dl and fi2 are calculated based on e* and fi3.

5. The one-bay trussarm inverse kinematics are solved for (di,&) and (d2 $3 )

The manual joint-checking algorit hm, Hanuaï JointChecl, compares each joint length to the maximum and minimum actuator lengths. If a joint is out of limit, the kinematics algorithm returns a failure flag and the RAPL commands are not executed.

2.2.4 System Parameters

The physical parameters of Tmssarm Mark II, implemented in the kinematics algorithm, are listed in Table 2.1.

14 2.3. KINEMATICS INITIALIZATION AND TESTS

2.2.5 Implementation

The kinematics developed in C are compiled in DOS, to generate a bin file. A bin file is downloaded on to the C500 using Firmware Load Utility.

Prior to the kinematics load procedure, the AC1 download mode has to be initiated on the C500 by entering the W D I A G command followed by the L-4

option. More details of the procedure can be found in the Cs00 Kinernatics Development Option Manual [4].

The Tnissarm Mark II kinematics was loaded on the Flash memory of the C500.

2.3 Kinemat ics Init ializat ion and Tests The system parameters are initialized prior to any move commands. A RAPL program IHIT-~AX contains series of commonds to assign vdues to parameters such as OXLIHITS, ~~LIIKS, GXRATIO, and ~XP~LSHS. The definitions of these RAPL variables are shown in Table 2.2 and aiso can be found in the RAPL manual [5] .

The second step in the kinematics initiaiization routine is the HOHE se- quence. The six actuators are separately brought to the minimum length to trigger limit switchs. The actuators are moved at 5% of their maximum speed. After al1 six limit switches are registered by the controller, each axis is lengt hened to t heir Baselengt h2.

At the end of the aom routine, the command is issued to initialize the encordera. As previously discussed, the joint coordinates are computed based on the encoder counts. In the kinematics, a joint coordinate represents the length of the actuator compared to its Baselength. For example, li = 0.001 implies the ith joint is 0.388 + 0.001 m long. When the kinernatics are solved, it is the actual lengths of the joints that determine the geometry of the trussazm, not the relative length of the joint. Consequently, it is important that the encoder counts are zeroed when the joints are exactly at t heir BaseIengt h.

A simple calibration technique is to measure the length of each axis with a

2A geometry of the Ihissarm Mark Il is referred to as Basekngth configrration when aii 6 actuators am at their Baselength, 38.8 cm, and joint coordinates are (0,0, - --, 0). More details can be found in Oikawa's master's thesis. [14]

2.3. KINEMATICS INITIALIZAirlON AND TESTS 15

carefully marked piece of aluminum and ensure that they are precisely at their Baselength, 38.8 m, at the end of the EOHE routine. More details regarding system evaluation and the calibration method are discuased in Chapter 5.

16 2.3. KINEMATICS INITIALIZATION AND TESTS

Parameters (~ariables in the Kinematics Development Option) Dimensions

Degrees of Freedom (numbrrofaxe8) L1 (equilateral A side length at bay end) L2 (isoscoles A length forming the petal) Ai (112 of distance b/w nodes of adjoining bays) Al (112 of distance b/w petal apex and act. node) hoff (actuator offset from node) Transmission Ratio (hanrmiasion8atio) Rotatory Resolution ( ~ o t ary~es ) Baselengt h Maximum Act. Lengt h Maximum Pos. Travel (POU ~ o i n t ~ i i i t a) Minimum Act. Length Maximum Neg. Travel ( ~ e ~ ~ o i n t ~ i i i t s)

Total Travel Extension Ratio Canera Location (~ool_hanafoa) Pen Location (~oo l -~ruis f orm)

OXLIHITS OXLIIKS

OXRATIO OXPULSES OXll lXVEL OACCEL OCLIISPD OCLIIACC

OCROTSPD dCROTACC

OLOFB

Table 2.1: Tmssaxm Mark II Physical Parameters

Joint Lirnits Link Lengths Transmission Ratio Roratory Resolus tion Maximum Pulse Output Rate Maximul Accelerat ion Rate Cartesian Linear Speed Limit Cartesian Linear Accerieration Limi t Cartesian Orientational Speed Limit Cartesian Orientational Accerleration Limi t Loss Of FeedBack

Table 2.2: RAPL Variables used in the Tmssarm Mark II Kinematics

Chapter 3 Servomechanism and

Tool u

Control

3.1 Servomechanism

The C500 is equipped with a servomechanism, that implements an indepen- dent joint PID controller with user-specified gains. In order to implement more advanced control mechanism, such as a global control or an optimai control, custom-built servo software has to be designed and implemented on the servo transputer. Unfortunately, the Servo Development Option was not installed on the Tnissarm Mark II's C500. Therefore, for the purpose of this project, the C500's built-in servomechanism was used.

Independent joint PID control is the most frequently uaed control mech- anism for commercial robots. It is known for its simplicity and high per- formance. Each joint receives feedback oniy from the sensor (in this case,

18 3.2. TASK SPACE CONTROL

- Axis

- 1 2 3 4 5 6 -

Gain Constants KP KD KI 8 O 20 8 O 20 8 O 20 60 O 10 60 O 10 60 O 10

Table 3.1: Trussann Mark II PiD Control Gains

encoder) on that joint. Based on the feedback, PID control law is applied:

where u is the control output and q is the position of a joint. In the C500 servomechanism, the following approximation is made:

Err = ( K p x PosErr) + (KD x (PosErr - PosErriMt)) + (ErriaSt + Kr x PosEn) (3.2)

where PosErr is given in encoder pulses. The control output sent to the amplifiers (u in volts) is computed based on ER.

10/2048 is the digital- to-analog conversion factor. The gains, K p , Kg and Kr, were determined by trial-and-error to achieve

the desired performance. The gain constants vary £rom one joint to another mainly because of the amplifier characteristics.

3.2 Task Space Control So far in this chapter, only the contml of rnotors (i.e., servomechanism) has been discussed. In pradice, many applications also requite a control in Cartesian space.

The hierarchai solution to the Cartesian control problem is often used. First, the desired Cartesian trajectory (a time sequence of position andlot

3.2. TASK SPACE CONTROL 19

Figure 3.1: Tnissarm Mark II Servomechanism: PID Control

20 3.3. WMTING APPLICATION

orientation) of the end-effector is computed independent of servomechanism. Second, the Cartesian trajectory is converted to the joint space trajectory (i.e., a time sequence of joint variables) by solving the inverse kinernatics. Third, a joint control is applied.

The computation of the Cartesian trajectory is sometimes referred to as Cartesian trajectory planing, or outer-loop control, as opposed to the inner-loop control for the servomechanism. For some applications, such as the writing application, outer-loop control is an open loop control. For other applications, such as visual control presented in Chapter 4, the desired Cartesian trajectory is computed for a given task, in a closed loop control.

The alternatives to the hierarchal solution are more complicated Carte- sian control schemes such as Cartesian computed torque control and Carte- sian space control using Cartesian dynamics. These control schemes start from the error in the task space, to calculate the moments (andior forces) which should be produced on the end-effector. Compared to the hierarchal approach, they are more complicated and require heavy numerical opera- tions. Due to these disadvantages, most present day commercial robots are controlled solely in the joint coordinates.

3.3 Writ ing Application

The focus of the the rest of this chapter is on the writing application of Trussarm Mark II. The design goal of this application is to control Trussaxm Mark II to write letters on the whiteboard. This application demonstrates high dexterity. A similar design was attempted with Trussarm Mark 1 by S h e n . Refer to [16] for the Trussarm Mark I Typewriter Demo.

3.3.1 System Description

Tmssaxm Mark II is mounted on the baseplate, which is attached to the gantry. For the writing application, a whiteboard used for the Trussarm Mark I Typewriter Demo, was set up below Trussarm Mark II. in order to write letters precisely, the board must be balanced. The program assumes that the board is pardel to the zy-plane of Fb.

A marker pen, with a spring-loaded mounting kame, was attached to a corner of the fourth bay end plane (see Figure 3.3). The pen is perpendicuiar

3.3. WMTLNG APPLICATION 21

Cartesian Trajectory

X dm Inverse

Kineamtxs

Generatron

Figure 3.2: Conceptual Scheme of Hierarchal approach to Cartesian Control

Servo Mechanzsm

T N S D O ~ J o i n t s

2 Forward Klnematics

X m

22 3.3. WRITING APPLICATION

to the whiteboard throughout the writing procedure. The program stmts 4 above the board. The Baselength coufiguration

does not allow enough clearance between the pen and the board and, there- fore, it is important to move Trussarm Mark II to the proper location before the application is init iated.

3.3.2 Control Algorit hm

RAPL commands, x, y, z and J O ~ were used to maneuver Trussarm Mark II. These commands also keep the other variables constant and, therefore, the orientation remains constant. (p, 8, and $J = 0.)

Each letter is 4 x 6 at scale = 1 with 1 between letters. If required, letters can be scaled up or down by changing a RAPL variable, scaie.

Three steps are taken to write a letter. First, the pen is moved to the proper starting point, P,(x,, y,). Second, the pen is lowered 4 to touch the board; then, the letter is traced on the board. Third, the pen is iifted 4

and moved to the next position, Pl(xo + 5, yo). Examples u e shown in Figure 3.4.

3.3.3 Results and Future Considerat ion

In Figure 3.5, the world UUTIAS" drawn by Trussarm Mark II is shown. The performance is compatible to that of Trussarm Mark 1 Typewriter Demo.

In order to improve performance, velocity control is recommended in place of position control. Even though odine mode is used for writing, Tmssarm Mark II cornes to a near stop before proceeding to the next motion command. If velocity control can be irnplemented on the servomechanism, the errors shown at the corners of each letter would be reduced.

There are two ways to move the end-effector fiom one point to another using a RAPL motion command: using straight-line interpolation and joint interpolation. The former keeps the center of the end-effector moving in a straight line while the latter coordinates a motion such that the motion of al1 joints start and stop at the same time. For the writing application, the straight-line is recommended since each letter has been segmented to straight h e s . Unfortunately, Trussarm Mark II's C500 can not exenite straight- line interpolated motion commands. If an improved controIler (instead of

3.3. WRITlNG APPLICATION 23

Base PI

-4 End Plane

Figure 3.3: Trussarm Mark II Writing Application Setup

24 3.3, WRITING APPLICATION

the C500) c m be integated with the Tmssarm Mark II in the future, the progam may be revised to give better performance.

Mounting a pen closer to the end plane of the actuated bays (in this case, the second bay end plane) would dso improve the system. As writing with a 10 a pen is far easier than writing with a meter-long pen, the Trussarm Mark II would write much better if it had a srnder tool vector.

Lastly, the slop of the mechanicd structure contt-ibutes to writing per- formance. Physical wiggles (non-rigidity) of the baseplate and of joints are partly responsible for slop at the tip of the structure.

The application may be modified for conventional pick-and-place maneu- vers and approach-departure applications.

3.3. W T I N G APPLICATION 25

Figure 3.4: Examples of End-Effector Traces for Writing Appiication

UTIAS

Figure 3.5: The UTIAS Acronym Drawn by Trussm Mark II

Chapter 4

Visual Control

4.1 Visual Control Concept

One of the areas of interest in the lndustnal Trussamn Project waa to apply a vision system to provide a real-time Cartesian control.

The first attempt at visual control for the one-bay t r u s s m was per- formed by Fenske in 1993. In his study [7], he chose machine vision as the sensing method for position tracking with the one-bay t r u s sm. The PAMI-Virtek Vision System (or, Virtek) waa used to provide a real-time machine vision. To utilize the Virtek system, visual control software was also developed. Fenske's control goal was to position the camera to point at the target. A similar visual control system was developed and implemented on Tmssann Mark II.

The goal of the Tmssarm Mark II visual control system is to control the orientation of the camera to track the vision target. The desired position and orientation of the camera were computed to satisfy the following two objectives:

The camera points at the vision target at all times;

4.2. SYSTEM DESCRIPTION 27

O The distance between the camera and the target is reasonably adjusted such that Trussarm Mark II is within its workspace at d times.

4.2 System Description Trussarm Mark II visual control system can be viewed as three separate systems interacting closely with one another, namely the PAMI-Virtek vision system, the (3500, and Trussarm Mark II (see Figure 4.1).

The visual control software and other control algorithms required for the real- time cont rot, including kinemat ics and servomechanism, are inst alled on the C500. PAMI-Virtek vision system transfers vision data to the (2500 through a serial line at 9600 baud rate. The visuai control software on the C500 has been designed to recognize the vision data when the first block of data arrives on the serial line.

PAMI-Virtek vision system consists of five main components: a frame grabber board, a CCD camera, a host PC, vision target and vision software. A data translation 2851 real-time digitizing frame-gabber board was pur- chased in 1993 for one-bay visual control. The same board was utilized in this project to digitize the analog video signal. A Hitachi KP-Ml CCD camera, once mounted on the one-bay trussarm, is now mounted on Tnissarm Mark II. The characteristics of the camera and its 16 mm lem can be found in Appendix A of Fenske's thesis [7]. The camera is mounted at one corner of the fourth bay end plane, across from the pen. From the second bay end plane centroid, the camera is located at (-8.4, -19.5, 78.0) m. The x-auis and y-axes of the camera were Lined with the base axes. The tool vector corresponding to the canera offset is: (see Figure 4.2 for the camera setup.)

The PC on which the Virtek vision software and the hune-grabber are installed, is a multipurpose 486 PC-clone. The cornputer receives the video signal from an extra monitor and processes Virtek software. This same PC also hosts the Robcomm software. In order to avoid the complication, the Robcomm is disabled while the vision system is in operation. This implies that the operator can not interact with the C500, once the visual control routine has started,

28 4.2. SYSTEM DElSCRIPTION

Industrial Trussarm

- Vision System

Figure 4.1: The Visual Control System

4.2. SYSTEM DESCRIPTION 29

rC--- Virtek Vision Target *

Plane

End Plane)

End Plane

Figure 4.2: Truasarm Mark II Visual Control Setup

30 4.2. SYSTEM DESCRIPTION

Figure 4.3: PAMI Virtek Cross-Pattern Target

A Virtek cross-pattern target was used as a vision target. Because the field-of-view of the existing lens is ody 1 Io, the area t hat the camera can view at one instant (frame) is severely limited. One solution is to reduce the size of the target. Instead of the 12 x 12 a2 full sale Virtek cross pattern, a half-scale cross was used (see Figure 4.3). Such modification dowed larger workspace and higher speed.

A modified riii fde was also generated for the half-scale target. A riii file is a part of the vision software. It contains information regarding vision system operation options such as screen output, port output, scale of the target and more. Refer to the Virtek manual [l] for more about mi Mes.

Generdy, the performance of the vision system is Limited by the following three factors: speed of the host computer, camera resolution, and lens field- of-view. In the m e n t setup, the update rate is about 1 - 5 HZ at 9600 baud and fd screen output. The data accuracy is about f 1 - 2%. The scope of the vision system is about 60 x 40 aii2 when the target is located 120 cm away from the camera.

4.3. VlSUAL CONTROL SOFTVClTARE 31

4.3 Visual Control Software

The visual control software performs the following three tasks:

O Read vision data from the Virtek vision system

O Compte the desired position and orientation of the cornera based on the vision data

O Move Trussarm Mark II to a new, desired location

To implement the algorithm in more efficient manner, the software was developed in C with PCP libraxy.

4.3.1 Vision Data Tkansfer

When the visual control algorithm is initiated, it ôutomatically resets the configuration of the C500 for vision data transfer. The serial line is configured for 9600 baud rate, parity none, 8 data bits, 1 stop bit, no handshake and no echo. The configuration has been chosen to match those of the vision system.

Each block of vision data is of the following format:

POS : 0.9932 -0.09126 -0.0154 -7.0312 0.01591 0.9734 -0.0648 -3.7034 -0.0099 0.0256 -0.9786 120.71253

The first line is the indication of the beginning of the new block. The visual control algorithm waits for the character ':' to statt reading the block. The fust three columns of the block represent the rotation matrix; the last column is the displacement vector in m. Even though the rotation matrix is not used in the computation, it is read and interpreted in the visual control software. The displacement vector is read only up to the second decimal places, and is converted to meters (IO-' m accuracy). For instance, the vector tC = (-0.0703, -0.0370, 1.2071) is extracted from the block above and passed to the computational segment of the visual control software. t: is the vector from camera to the vision target, expressed in F,.

32 4.3. VISUAL CONTROL SOFTWARE

4.3.2 Control Algorithm The objective of the control algorithm is to calculate the new position and orientation of the camera, given the position of the vision target with respect to the camera. First, the current world coordinate of the camera is read using the RAPL cornmand AC~U.~. Next, the position and orientation of the camera, q and Ra,, are computed baaed on the world coordinates. The position of the target is calculated in the base fiame (refer to the List of S ymbols for the vector definit ions):

Third, to describe the control procedure, a new frame of reference is defined. Fb# is at a corner of the first bay bottom plane, directty below F,, as shown in Figure 4.2. The desired location of Fe is on the line Eiom Fi,! to Ft , with ii, also on the same Lne. Furthemore, the distance between FC and &/ is kept constant. Figure 4.4 shows the algorithm at difference target location. The distance is denoted as robot length in Figure 4.4.

Finally, the new task coordinate (~~,,,îi,+,.~) is transfotmed to the world coordinates.

4.3.3 Motion Command The visual control software commands Trussarm Mark II to move to the new location. The motion is executed by the RAPL command, Hove. The desired location is specified in world coordinates. The online mode is optional and cm be selected at the main menu of the program. However, it has been shown that the performance is better when the online mode is off.

The main menu dso allows a user to specify the tolerance of the vision data, the robot length and the number of control loops. Tolerance is the minimum distance the tatget needs to move before triggering the control sequence. The default value is 1 a. A reasonable robot length in the current configuration is 1.75 m.

4.3. VISUAL CONTROL SOFTWARE 33

Figure 4.4: Trussazm Mark II Visual Control Algorithm

Trussarm Mark II is moved at 25% of the maximum encoder speed (6500 rpm) and each move command is joint interpolated.

4.4 Results

Speed It is very difficult to determine the speed of a system where two inde-

pendent systems are in operation simult aneously. The vision update rate is about 1 HZ and the tip speed is approximately 1.9 =/S. With these two independent systems at full speed, the visual control systern update rate is approximately 1 HZ, if the vision tasget is within the camera's sight at al1 timea-that is, if the target moves at less than 1.9 at all times.

Workspace The workspace of the vision target is about 2.5 x 2.5 m2, if the taxget

remains 1 to 1.2 m away from the camera. As the target moves away from the camera, a larger workspace can be achieved.

Accuracy The accuracy of the system cannot be evaluated qualitatively. It has

been experimentaliy s h o w that the performance of the system is equivalent to that of the visual control system of Trussaxm Mark 1.

The fluttering problem described by Fenske in his thesis [7] still exists and therefore control in z-direction is less accurate than in other directions.

The performance of the system deneases as the distance between the target and the camera increases.

4.5 Future Considerations

4.5.1 Improvements to the Visual Control System

To improve the tracking performance, the most significant improvement to the system would be made by replacing the Virtek with a vision system that can provide a higher sampling frequency. Furthemore, communication speed also dec t s the system speed. While the C500 can communicate at a higher

4.5. FUTURE CONSDERATIONS 35

baud rate (up to 38400 baud), the host PC of the vision software is only capable of 9600 baud.

Control precision could be improved by solving non-redundant inverse kinematics. Even though the problem is negligible in visud control compared to the d t i n g application, the twist angle at the tip of the rnanipulator is visible near the workspace boundaries.

4.5.2 Four-Bay Visual Control Once al1 four bays are actuated and proper kinematics are implemented, the four-bay visual control system can be implemented. A 12-DOF visual control may be developed for a variety of applications. Many examples of visual robotic control already exist and include:

visual recognition and interpretation of the task environment

visual inspection of manufacturing parts

vision directed task planning for the robot manipulator and robot workcell

vision-guided on-line trajectory generation wi th obstacle and singular- i ty avoidance capability.

on-board coordination, navigation and control of mobile robots

space telerobotics and autonomous robotics.

Chapter 5 Svstem Evaluation and

Calibration

5.1 System Evaluation

System evaluation consists two kinematic tests. First, when inverse and fornard kinematics are performed on a set of coordinates repeatedly, the solution should be close to the original coordinates. Second, the tip of the end-effector should travel the correct distance and direction when a move command is issued. Trussarm Mark II may move d c when commanded to move d, and thinks it h a moved d m. The system should be calibrated such that E is negligible.

For the first test, Trussarm Mork II was moved to various locations and the motor and world coordinates at each location were recorded using RAPL commands, ACTUAL +A and ACTUAL A, respectively. Using the SET LOCATIOII command twice, the motor coordinates (#D) were solved for the actual world coordinates (A), then back to the world coordinates (D). The SET LOCATIOII command solves kinematics depending on the type of the location variable.

5.1. SYSTEM EVAL UATION 37

ACTUAL #A ACTUAL A SET LOCATIOI #D = A SET LOCATIOI D = #D U O C A,D

Figure 5.1: RAPL commands for System Evaluation Test

In RAPL, location variables starting with # are motor or joint variables, while the others are world coordinates. The actual world coordinates (A) were compared to the solved ones (D). The sequence of RAPL comrnands are shown in Figure 5.1. The results of this test is shown in Table 5.1. As ahown in the results, the cdculated coordinates agree with the actual coordinates at most points. The accuracy decreases as Trussarm Mark II reaches its workspace limi ts.

For the second test, a scheme to measure the location of the end-effector in 3-dimensional space had to be introduced. A pen mounted at the tip of the fourth boy was considered a tool to mark the location of the end-effector. Trussarm Mark II was moved in a pick-and-place routine around a 10 x 10 cd-square.

The fist attempt at a pick-and-place routine was unsatisfactory as shown in Figure 5.2. After numerous experiments, the following three main struc- tural errors, that contribute to Trussarm Mark II's performance, were found: flexibility in the mounting of the baseplate to the gantry; the slop in the joint assembly connecting the actuator plane with the passive structure; and, the baseplate being mounted on the gantry at a small angle with respect to the inertial horizontal plane.

The angle at which the baseplate is mounted exists regardless of trussarm geometry. The flexibiiity of the baseplate and joints have different effects depending on the structure geometry.

38 5.1. SYSTEM EVALUATION

r Point 1 : Center of the workspace actual: 0.0, 0.0, 0.9663, 0.0, 0.0, 0.0 computed: 0.0, 0.0, 0.9663, 0.0, 0.0, 0.0

O Point 2 : Top-center actual: 0.0, 0.0, 0.8152, 0.0, 0.0, 0.0 computed: 0.0, 0.0, 0.8152, 0.0, 0.0, 0.0

a Point 3 : Bottom-center actual: 0.0, 0.0, 1.0625, 0.0, 0.0, 0.0 computed: 0.0, 0.0, 1.0625, 0.0, 0.0, 0.0

0 Point 4 : Maximum Negative travel in x-direction act ual: -0.549 1, 0.0, 0.6698 ,O .OO76, -75.5582,-0.0099 computed: -0.5491, 0.0, 0.6699,0.0128, -75.5430,-0.0165

O Point 4 : Positive travel in x-direction actual: -0.2315, 0.4017, 0.8566, 12.4660, -24.7143, -52.9950 computed: -0.2323, 0.4024, 0.8559, 12.5419, -24.7906, -53.1297

O Point 5 : Negative travel in y-direction actual: -0.2323, -0.4024, 0.8559, -12.5450, -24.7947, 53.1331 computed: -0.2323, -0.4023, 0.8559, -12.5416, -24.7924,53.1251

Point 6 : Positive travel in y-direction actual: 0.1517, 0.2721, 0.8743, -6.0004, 18.5619, -35.5646 computed: 0.1517, 0.2721, 0.8743, -6.0003, 18.5619, -35.5645

Table 5.1: Kinematics Test Result

(Lines have been added to the original test results.)

4 2 ) A f t r The Base Frame J u s t i f i c a t i o n

3 ) The F i n a l Result

Figure 5.2: Calibration Test Results

5.2 System Calibration The system was calibrated in three steps. In order to compensate the problem associated with the non-horizontal base hame, the orientation of Fb was defined with respect to an inertial fiame of reference, Fi. Unlike all ot her reference hames dehed so far, Fi is h e d in space, and independent of trussarrn geometsy. The center of f i is located at the center of Fb, and its xy-plane h s a horizontal plane. The z-axis of Fi points downward. Once the inertial £rame is defined with the OFFSET command in RAPL, the world coordinates of the end-effector are computed with respect to 7;-. The forward kinematics equation (2.17) is then replaced by the following:

and the corresponding inverse kinematics equat ion (2.18) becomes

The orientation of the Fi with respect to Fb waa determined in two steps. Fint, with Trussm Mark II at its Baselength configuration, each joint was carefully adjusted so that the top planes of al1 four bays were horizontal. (For future reference, we recomrnend manually adjusting the fust bay actuaton to level the first end plane, then the second bay actuators for the second end plane, etc.) In this balanced configuration, the pitch and roll angles (Oib and *ib) were read with the ACTUAL command. These measurements provide the orientation of the xy-plane of Fi d t h respect to the xy-plane of Fb. The directions of the x and y axes of Fi on this plane were not determined. In other words, yaw (yib) between Fb and Fi c m be chosen freely.

Second, a temporary inertid frarne was set up at via, Bib and +ib with an arbitrary yaw angle. Trussarm Mark II was moved in a pick-and-place routine along a square. With varying <pi69 the procedure was repeated until the results were satisfactory. The result is show in F i p e 5.2 dong with the square drawn with the originai kinematics. In the curent setup, the rotation angles were cpib = 10.lO, Bib = -0.670° and $is = -0.9W0. They provided for a transformation matrix Hi6, Hnth the displacement vector being zero. It is reminded that the world coordinates in the (2500 is given in z-, not in Fb, once the OFFSET is set. The default OFFÇST is (0,0,0). Furthmore, the X, Y, z commands move Trussarm Mark II dong x , y and %-axes of 5, instead of Fb

5.2. SYSTEM CALEIRATION 41

It has been noted that the 1-beam of the gantry on which Trussazm Mark II is mounted is in the direction close to the x-axis of &. Because it is an 1-beam, the baseplate is allowed to move far more freely about the x-axis than y-axis of 3 b . That is, the roll of the end-effector about Fb is afTected by the flexibility of the baseplate more than the pitch.

Consider the case where the end-effector is on rb = (O, y, 2 ) . The gav- itational force will add the positive roll' of the structure about Fb, and in response, the baseplate will be under positive torque about z. Then, the actual location of the end-effector (r') would be close to the r-axis than the forwaxd kinematics would calculate. (rj, = (O, y', 2') where y' < y and a' < 2.)

The effects of the slop in joints have no specific pattern. Note that each joint is deformed under tension, but not under compression. For instance, with the structure in a configuration with negative roll as in the previous example (end-efFector located along y-axis of Fb), Joint2 1-2 and 4-5 ore under tension; Joint 1-3 and 4-6 are under compression; and Joint 2-3 and 5-6 are under no influence. Therefore, only joints 1-2 and 4-5 will deform due to the gravity and result in adding positive roll on the system. Refer to Figure 1.1 for joint locations.

However, the joint assembly deformation is not always predictable. For instance, to achieve the y movement of the end-effector with zero pitch, the first bay may pitch by +O and the second bay by -B. Such geome- try is achieved becauae of the method of optimization used in the two-bay trussarm inverse kinematics. It follows t hat the scenario presented earlier no longer holds; Joint 2-3 is also under tension while Joint 5-6 is under compression. Consequently, the elongation of Joint 2-3 adds negative pitch to the system, adding to the positive roll from the other joints. Furthemore, the arnount of deformation in each joint is unknown.

In summazy, positive displacement of the end-effector dong x in Fb causes negative pitch and negative roll ; negative displacement dong z cause positive pitch and positive roll. Similady, positive displacement of the end-effector along y in Fb causes negative pitch and positive roll; negative displacement along y cause positive pitch and negative roll. Refer to Table 5.2 for summary.

To mode1 the system with the errors, let us assume that the actuai

'Be reminded that the rotation about z-axis is refmed to as roll, about yaxis as pitch and about z as yaw.

lloint i-j conesponds to the joint 888embly between actuator i and j.

42 5.2. SYSTEM CALIBRATION

1 end-effector displacement 1) pitch 1 roll]

+a: -2 I I ; IJ

Table 5.2: Kinematics Error Caused by Structural Defects

location of the end-effector, r; is related to the predicted value, rb, aa follows:

Hbrl = EHb, (5.3)

and the error factor matrix E is of the following format:

cos 0, sin 0, sin sin 0, cos $, .=[ 0 cos +, - s i U ] - sin 0, cos 0, sin $ J ~ cos 0, cos $, O

where the error angles ( B e and are functions of z and y displacements of the second bay centroid in .& which are directly related to the end-effector dis placement fiom i ts location at the Baselengt h configuration.

The constants (kps, kW, k;, and kW) were approximated using expenmental met hods.

Wit h the error approximation, the forwatd kinematic equation (5.1) be- cornes,

Hi# = Hib (E(&lHl2))H2~- (5*8)

and, E is computed based on the x and y of Hb2. The correspondhg inverse kinematics equation (5.2) is, then,

5.3. KINEMATIC ALGORITHM MODIFICATION 43

Note that in the inverse kinematics, E is a function of HbZ, which is the solution being sought. For s m d 8, and &, (HzlHirH~,?) is very close to Haz and therefore an approximation of E can be made. However, the error is cumulative and precision of measurements decrease over tirne. Simulations show that when world coordinates are transformed to joint coordinates via inverse kinematics, t hen forward kinematics are solved based on t hem again, the original coordinates are different hom the computed ones by 0.3 to 0.5 percent.

Another calibration method is to simply magnify x and y of Hb2 by constant factors. Since 6, and $, axe very small at most times (approximately 0.01-0.03" over 1 cm of end-effector travel) a proportionality may be assumed for a small area :

with kl, k2 3 1 and k3 5 1. Again, the constants were detennined experi- mentally. In the inverse kinematics, s, y and z of &, are replaced by x'/kl, y'/ k2 and z'/ k3, respectively.

The result of the pick-and-place experirnent after the sys tem calibration is presented in Figure 5.2.

5.3 Kinemat ic Algorit hm Modification For the cdibration, the kinematic algorithm was redesigned to d o w users to choose calibration constants. Sorne of the global variables defined by Kinematics Development Option can be changed using RAPL commands. One of them is the linltJength, which are dedicated for 8 real numbers to describe the link lengths of the robot. This variable plays a significant role only if the manipulator's joints are rotary and the length of each joints are variable. This definition of link lengths do not apply to Trussaxm Mark II, and therefore are not used in the original kinematics algorithm.

For the calibration, the vanable l i n l l eng th was redefined as the following:

linklength [O] = flag

44 5.3. KINEMATIC ALGORITHM MODIFICATION

linklength [1] Linklength [2] linklength [3] linklengt h (41 link-lengt h [5] linklength [6] linklength [7]

and the 0ag is used to choose the method of error correction.

0ag = O : No error correction flag = 1 : Linear scale flag = 2 : Error angle approximation

The calibration parameters now can be assigned with ~XLIMKS command in RAPL.

Chapter 6 Future Plans for

Trussarm Mark II

6.1 New Location

The Space Mechanics Laboratory, where Trussarm Mark II is located at the time this thesis is published, has been expanded recently and Tmssarm Mark II wiU be relocated in the new area.

Prior to the relocation, the workspace of the 12-DOF robot was con- sidered. With ail four bays operational, it has 135% expansion ratio in r direction. At its f d y retracted geornetry, the total height is 1.58 m. The M y extended reach is 2.14 m. It can reach from -1.64 to 1.45 m in x- direction and is also capable of U-curve and S-cuve maneuvers. In order to demonstrate its maximum capability, a new laboratory needs to d o w for a workspace of at le& 3.1 x 3.1 x 2.2 m3.

One of the structural defects discussed earlier is the flexibility of the base plate. When Trussarm Mark II is moved to the new location, an improved mounting procedure needs to be introduced to reduce the slop

46 6.2. IMPROVED SYSTEM

in the baseplate.

6.2 Improved System

New Controller, Amplifiers and Development PC

Throughout this project, several problems associated with the CS 00 have been confronted. They include limi ted access to the kinemat ics algori t hms, fixed servomechanism, fâilure in straight-line interpolation, and small rnem- ory capacity. The most significant problem is that the C500 c m support no more than 8 control DOF. For the Trussarm Mark II, a controller with at least 12 control DOF is required for four-bay operation. It ha been decided that the C500 will be replaced with a Galil based control system.

The power amplifien will also be replaced with a Galil amplifier system and the host PC will be upgaded accordingly. In the near future, the kinematics, servo mechanism and control software for two bays will have to be modified and redesigned for the Galil system.

Joint Assemblv Desim

As Oikawa noted in his thesis [14], the two most significant problems with the current Trussarm Mark II design are the overly stressed bottom bay actuators and slop in the joints. Suggestions for improvements have been made in Chapter 4 of [14]. Redesigning the joint assemblies and parts of the actuaton to increase the rigidity of the structure are likely the most significant improvements to the system.

T wo-Bay Trussarm Inverse Kinematics

In Chapter 2, it was stated that the two-bay inverse kinematics solves for position and orientation of the second bay end plane centroid.

Then, the joint coordinates are aolved based on Hb2. This method assumes that the tool vector is aligned with the z-axis of Fb, or if it is not, that the

6.3. FO UR-BAY TR USSARM CONTROL 47

tool vector is small enough so that the redundancy resoiution, q , does not have an effect on the result.

In the calibration tests, it Bas been shown that the performance is not as good near the workspace limits as it is in the center, where q is the most lirnited. The problem becomes more obvious when the longer tool-vector is used.

In order to eliminate the problem, non-redundant (6-DOF) inverse kine- matics should be solved. Another possibility is to modify the inverse kine matics such that the twist angle about the tool normal, îi, instead of the second bay direction vector, fi3, is considered a redundant variable and to solve the kinematics for r and ii, not e? and ii3.

6.3 Four-Bay Trussarm Control

Redundancv Cont rol

In the long run, the goal of the Industrial Trussarm Project is to control a 12-DOF, four-bay trussarm. The kinematics of a four-bay trussarm has been studied and presented by Naccarato in 1991 using Reference Shape C w e s [13]. This method will be implemented on Tnissaxm Mark II.

Other approaches to a hyper-redundant control problems will also be studied including: a continuous approach proposed by Hayashi and Kuipers [9]; a backbone curve approach studied by Chirikjian and Burdick [3,4,5,19] and distributed control methods such as a distributed autonomous control strategy [IO].

Visual Control

The four bay trussarm visual control will be designed and implemented on Trussazm Mark II. Suggestions for the applications and vision system improvement have been addressed in Section 4.5.

Four-Bay Trussaxm Application

Both the research and industrial applications for a 12-DOF tmssarm are being considered. One of the suggested applications for Tmssarm Mark II is

48 6.3. E'OUR-BAY TROSSARM CONTROL

to utilize its high dexterity and autonomous control to inspect nuclear dump sites. To demonstrate its capability, a maneuver to snake into an S-shape duct with the vision system or proximity sensors is being considered.

Several types of actuators, such a s numeric or hydrolytic actuators, to increase the stifiess and reduce the size of the system are also suggested as profitable lines of study.

The main objective of the Industrial Trussm Project was to design, con- struct and control a four-bay trussarm, based on the t n i s s m technology âvailable at UTIAS. The h d design of Trussarm Mark II is a four-bay t russm, with only two bays (6 degrees of freedom) controiled. The de- sign can be extended for 12-degree-of-freedom control when the controller is upgraded.

As part of this project, this thesis involved designing and implementing a controlier. To achieve this goal several tasks were defined: (1) review and implement two-bay tmssarm kinematics; (2) develop servomechanism and control routines; (3) design a visual control; (4) system evaluation and calibration. The goals of the thesis have been accomplished and future plans for Trussarm Mark II have been outlined.

The trussarm kinematics were developed prior to this project and were reviewed here. One-bay kinematics was studied by S h e n [l6]. The two- bay forward kinematics is solved by fùst , solving two one-bay forward kinematics, then adding the solutions as vectors. Two-bay inverse kinematics is solved by optimizing the configuration of the trussarm with respect to the center of its workspace. The solution is found to satisfy the given displacement and

50 CHAPTER 7. CONCLUSION

directional normal of the second bay's end plane centroid, ignoring the effect of the tool vector.

Independent joint PID control was applied to the servomechanism. The performance of the servomechanisrn was demonstrated using writing ap- plications designed to perform a series of end-effector movements. The results of the experiments showed that 6-degree-of-&dom control for the Trussarm Mark II is possible. This application also demonstrated dexterity of the Trussarm Mark II. The performance is compatible with the one-bay tmssa,rrn's Typewriter Demo.

Visual control was successfully designed. Utilizing the Virtek vision system, closed-loop Cartesian control was applied. Again, the performance is compatible with the one-bay visud control developed by Fenske [7], and it con be improved when the vision system is upgaded.

The system was evaluated for accuracy. A calibration routine was de- signed to overcome a number of structural errors. The performance was judged upon the accuracy of the writing application and the system was calibrated accordingly.

Trussarm Mark II is in the process of relocation asd system upgrade. Several tasks have been outlined and scheduled for the future. The C500 will be replaced by a Galil system, which will provide 12 control DOF. The baseplate, joint assemblies, and actuators will be considered for design improvements. A four-bay redundant control and visual control will then be s tudied.

In conclusion, Trussarm Mark II now has a complete 6-DOF control system capable of real- time, Cartesian control. Alt hough this particular project was successful, the complete Trussarm Mark II design has not yet been optimized. In the near future, we look forward to an improued Trussm Mark II!

References

[l] Pat tem analysis and machine interlligence group. Technical report, Department of Systems Design Engineering University of Waterloo, Waterloo, Ontario, May 1992.

[2] H. Asada and J. Slotine. Robot Analysis and Control. Wiley and Sons Inc., 1986.

[3] CRS Plus hc., Burlington, Ontario. Small Industn'al Robot System ROBCOMM-II Users Manual, 1990.

[4] CRS Plus hc., Burlington, Ontario. C500 Industrial ControUeî Kinematzcs Development Option, 1993.

[5] CRS Plus Inc., Burlington, Ontario. RAPL-II Progrmming Manual, 1993.

[6] CRS Plus Inc., Burlington, Ontario. Small Industrial Robot Systems Controller Process Control Pmgrurnming Kit, 1993.

[7] Dave Allan Fenske. Real-the control of the t r u s s m variable-geometry- truas maniputor utilizing machine vision. Master's thesis, University of Toronto, 1993.

[8] A. A. Goldenberg. Robot kinematics and dynamics. Department of Mechanical Engineering, Univensty of Toronto, 1993. Lecture notes for MEC1602.

[9] Akira Hayashi and Benjamin Kuipers. A continuous aproach to robot motin planning with many degrees of heedorn. In IROSY92 Intelligent Robots and systems: sensor-based robotics and opportunities f o ~ its industrial applications, volume 3, pages 1935-1942, J d y 1992.

[IO] H. Kobayashi, E.Shimemura, and K. Suzuki. A distributed control for hyper redundant manipualtor. Li IROS.12 Intelligent Robots and systerns: sensor-bused robotics and opportunities for its industrial applications, volume 3, pages 1958-1963, July 1992.

[l l] Dynacon Enterprises Limited. Industrial trussarm project - ists project op port unity fund technical and management proposal. January 1994.

[12] Frank Naccarato. Kinematics of Variable Geometry Tmssarm. PhD t hesis, University of Toronto Institute for Aerospace S tudies, 1994.

[13] Frank Naccarato and Peter Hughes. Redundancy resolution in variable- geometry truss manipulators using reference shape cuves. Journal of Ro botic Systems, 8(2):539-555, 1991.

(14) Stephen Oliver Oikawa. Design and construction of a Cbay variable- geometry-truss manipulator m. Master's thesis, University of Toronto, 1995.

[15] Kurt Rueb. Vision System Usage. PAMI lab, Systems Design Engineering University of Waterloo, Waterloo, Ontario, October 1992.

[16] Joseph Sallmen. A real-tirne computer control and tracjectory generator environment for t n i s s m . Master's thesis, University of Toronto, 1993.

[17] Miomir Vukobratovic and Dragan Stokic, editors. Applied Control of Manipulator Robots - Analysis, Synthesis and Exercises. Sptinger-Velag, 1989.