TABLE OF CONTENTS Page LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2. Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1. System Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.

End Point Position Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1. End Point Sensor Operation . . . . . . . . . . . . . . . . . . . . . . . . v 1 5 5 7 8

2.2.2. End Point Sensor Integration . . . . . . . . . . . . . . . . . . . . . . . 9 2.2.3. End Point Sensor Use . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2.4. End Point Sensor Calibration and Compensation . . . . . . . . . . 14 2.2.5. End Point Sensor Performance . . . . . . . . . . . . . . . . . . . . . . 21 3. Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.1. Design Issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.2. End Point Feedback Nonlinearities . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.2.1. Effect of Nonlinearities . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.2.2. Exact Linearization Methods . . . . . . . . . . . . . . . . . . . . . . . . 32 3.2.3. Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.2.4. Feedforward Linearization . . . . . . . . . . . . . . . . . . . . . . . . . 40 3.3. Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.3.1. Control Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.3.2. Compensator Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 4. Hardware Test Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.1. Classical Compensator Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 4.2. LQG Compensator Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 4.3. Joint Based Control Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

5. Further Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 6. Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63 Appendix 1. Parameter Values . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 Appendix 2. Code Listings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

2.1. AC100-C30 Serial Line Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 2.2. Slew Commander . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82 2.3. Linearization Matrix Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 2.4. Singular State Transition LQR Solution . . . . . . . . . . . . . . . . . . . . . . 102 2.5. LQR Compensator Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

Appendix 3. Equation Derivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 3.1 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 3.2 Rigid Body Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 3.3 PSD Integrals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 3.4 Second Order Slew Commander . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 Appendix 4. Hardware Diagrams . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112

ii

LIST OF FIGURES Number Page Figure 1.1 Typical Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 Figure 1.2 Joint vs. End Point Feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Figure 2.1 Table Arm with Tip Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Figure 2.2 Real Time System Command and Data Flow . . . . . . . . . . . . . . . . . . . Figure 2.3 Sensor Triangulation (2 axis) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 5 7 8

Figure 2.4 IR Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 Figure 2.5 Sensor Data Flow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 Figure 2.6 Sensor Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 Figure 2.7 Table Top Misalignments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 Figure 2.8 Sensor Misalignments and Constraints . . . . . . . . . . . . . . . . . . . . . . . 16 Figure 2.9 Sensor Noise PSD Format . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 Figure 2.10 Sensor Noise PSD Hardware Test . . . . . . . . . . . . . . . . . . . . . . . . . . 23 Figure 3.1 Zero-Pole Map - End Point Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . 28 Figure 3.2 Zero-Pole Map - End Point and Joint Sensors . . . . . . . . . . . . . . . . . . . 29 Figure 3.3 Plant Shoulder Torque to X Position T.F. . . . . . . . . . . . . . . . . . . . . . 35 Figure 3.4 Plant Shoulder Torque to Y Position T.F. . . . . . . . . . . . . . . . . . . . . . 35 Figure 3.5 Plant Elbow Torque to X Position T.F. . . . . . . . . . . . . . . . . . . . . . . . 36 Figure 3.6 Plant Elbow Torque to Y Position T.F. . . . . . . . . . . . . . . . . . . . . . . . 36 Figure 3.7 Linearized X Axis Force to X Position T.F. . . . . . . . . . . . . . . . . . . . . 37 Figure 3.8 Linearized X Axis Force to Y Position T.F. . . . . . . . . . . . . . . . . . . . . 37 Figure 3.9 Linearized Y Axis Force to X Position T.F. . . . . . . . . . . . . . . . . . . . . 38 Figure 3.10 Linearized Y Axis Force to Y Position T.F. . . . . . . . . . . . . . . . . . . . 38 Figure 3.11 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 Figure 3.12 Compensated Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . 39 Figure 3.13 Feedforward Linearization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 Figure 3.14 Control Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 Figure 3.15 Classical Compensator Loop Transfer Function . . . . . . . . . . . . . . . . 44 Figure 3.16 Classical Compensator Nichols Plot . . . . . . . . . . . . . . . . . . . . . . . . 45 Figure 3.17 Classical Compensator I/O Magnitude Response . . . . . . . . . . . . . . . . 46

iii

Figure 3.18 Classical Compensator I/O Phase Response . . . . . . . . . . . . . . . . . . . 46 Figure 3.19 LQG Compensator Loop Transfer Function . . . . . . . . . . . . . . . . . . . 49 Figure 3.20 LQG Compensator Nichols Plot . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 Figure 3.21 LQG Compensator I/O Frequency Response . . . . . . . . . . . . . . . . . . 50 Figure 3.22 LQG Stability Regions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 Figure 4.1 Classical Compensator Response to Slew Command . . . . . . . . . . . . . . 52 Figure 4.2 Classical Compensator X Axis Disturbance Response . . . . . . . . . . . . . 53 Figure 4.3 Classical Compensator Y Axis Disturbance Response . . . . . . . . . . . . . 53 Figure 4.4 LQG Compensator Response to Slew Command . . . . . . . . . . . . . . . . 55 Figure 4.5 LQG Compensator Commanded Slew cont. . . . . . . . . . . . . . . . . . . . . 55 Figure 4.6 LQG Compensator X Axis Disturbance Response . . . . . . . . . . . . . . . . 56 Figure 4.7 LQG Compensator Y Axis Disturbance Response . . . . . . . . . . . . . . . . 56 Figure 4.8 Joint Base Comparison End Point Positions . . . . . . . . . . . . . . . . . . . . 58 Figure 4.9 Joint Based Comparison End Point Errors . . . . . . . . . . . . . . . . . . . . . 59

iv

LIST OF TABLES Number Page Table 2.1 Sensor Output Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 Table 2.2 Hardware Connection Editor Data . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 Table 2.3 Calibration Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 Table 2.4 Sensor Noise Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 Table 2.5 Sensor Noise Levels . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

ACKNOWLEDGMENTS The author would like to acknowledge the help provided by a number of individuals during the design and documentation of this work. Thanks to Juris Vagners PhD., who provided the laboratory in which this work was based and the time to critique this document. Thanks to Martin Berg PhD., who provided the project, inspiration, and guidance under which all this work was completed as well as many useful comments about the documentation. Thanks to David Bossert, who answered all those annoying little day to day questions in the laboratory.

vi

1. Introduction

Typical robotic manipulators consist of a number of mostly rigid links connected together with free moving joints. Each joint has some method of sensing the local joint position (and possibly rate) along with some type of force/torque actuator. The objective of the electromechanical device is often to simply place the end point, or tip, of the manipulator in the desired position/orientation.
Sliding Joint

Rotary Joint

0 1 1 0

Rotary Joint

Figure 1.1 Typical Manipulator Historically, end point position control of robotic manipulators is based on the joint position coordinates (which may be either rotations or linear translations). Desired tip positions and rates are transformed to the equivalent joint positions and rates, assuming rigid link kinematics (although these inverse kinematics are often not simplistic and occasionally nonunique). Feedback control is then applied, based on joint position errors and feedforward accelerations. Control strategies can often be as simple as PD control, because of its guaranteed stability. Although joint based control has advantages when examined from a stand point of feedback stability, is has serious disadvantages in regards to absolute end point positioning accuracy. Even if the assumption of perfect joint control could be made, there are still plant uncertainties and disturbances that will result in end point position errors. They can neither be eliminated nor reduced through joint feedback compensation. Commanded joint positions generated through the use of manipulator inverse kinematics are made using assumptions about the manipulator geometry (arm lengths and/or axis orientations). Errors in these quantities lead to commanded joint angles that

2 result in actual end point positions different from the commands. for these errors. Certain types of link flexible deformation can also be unobservable in the joint displacements. Dynamic vibration in the structure can either be sensed through joint rates, or estimated through dynamic models (Kalman filter). Joint based controllers can also increase damping in the flexible modes through proper phase relationships. However, joint based control cannot compensate for static flexing that results from external forces (usually applied at the end point). Because this

transformation is in a feedforward path, the joint based feedback control cannot compensate

( ref = real ) (Xref = Xreal) X ref Assumed Inverse Kinematics ref Tor Manipulator Dynamics real Real Forward Kinematics

Compensator

X real

Parameter Errors

X ref

Compensator

Tor

Manipulator Dynamics

real X real

Figure 1.2 Joint vs. End Point Feedback

Joint feedback control is unable to compensate for errors in end point position due to kinematic uncertainties and end point disturbances because feedback is not made directly around the quantity of interest. As shown in Figure 1.2, the error sources are not in the feedback loop, and therefore are not affected by the compensation. To correct for this problem, this document examines a control strategy based on direct sensing and feedback of the manipulator end point position. This methodology does not suffer from any of the intrinsic problems of unobservable errors that joint feedback does. End point position sensing also has the advantage of a greater supply of more

3 accurate sensors. Because of the geometry of the manipulator, small errors in rotary joint angles are magnified to larger errors in end point position. Rotational position sensing instruments can be capable of measurements with a resolution in the range of approximately 2 /1024 to 2 /65,536 radians. Coupled to an arm length of 1 meter, end point errors of 0.1 to 6 mm result. Laser based interferometric position sensing instruments, on the other hand, are capable of single axis measurements of the end point position with quantization on the order of nanometers.

Although direct end point feedback does not suffer from the same intrinsic errors associated with joint based control, it does present difficulties for feedback control design. One of the difficulties is the nonlinear nature of end point measurements for noncartesian manipulator geometries. This type of nonlinearity has large effects on the system dynamics across the manipulator workspace and must be compensated for in the controller structure. End point feedback also suffers from the noncolocated nature of the sensor and the actuation device. Flexibility in the manipulator structure results in plant dynamics whose linearization contains transmission zeros in the right half of the real/imaginary plane. This is well known to cause increased difficulties in the control of the plant. As shown in reference [1], this does allow for a faster response time, if noticeable deformation of the structure is allowed. Flexible modes of the structure may be also be unobservable from the end point position and therefore cannot be further stabilized.

A reduced scale hardware testbed was used to examine the key issues associated with control of industrial robotic manipulators. It provides a complete emulation of a full scale industrial manipulator in a controlled environment. The properties of the testbed are described in the following chapters. It has exaggerated link flexibility to provide an environment to study the relation between the compensator and the plant dynamic modes. The sensors are typical of real world manipulators with similar properties and behavior. An optical infrared tracking sensor was used to provide end point measurements instead of a more accurate interferometric laser tracker. Although the accuracy of the IR

4 sensor is significantly less than a laser tracker, it still provides end point measurements with a quantization size eighteen times better than the joint angle encoders. This more cost effective solution gives position measurements of the end point with data properties similar to the laser tracker. The hardware testbed, equipped with the IR position sensor, was used to examine the issues associated with direct end point position feedback. Particular attention is paid to the difficulties with control of the plant, focusing mainly on the compensation of the plant nonlinearity.

2. Hardware

This section contains a description of the hardware testbed used in the validation of the end point feedback control structure and compensator design. The end point sensing instrumentation has been implemented on the flexible manipulator testbed in the Control Systems Laboratory of the University of Washington. The flexible manipulator testbed has been used for other research related to control of electromechanical manipulator systems, including general control, surface following, and tip force control (see reference [10]).

2.1. System Hardware

Since the end point control system was implemented on the existing flexible manipulator testbed, this is only a general description of the system. Only the new end point position measurement hardware is described in detail. See reference [2] for more details.
Granite Table Shoulder Motor Optical Encoder (on shaft under motor) Flexible Links Target for Position Sensing System End Effector

Elbow Motor Optical Encoder Air Cushion Supports

Figure 2.1 Table Arm with Tip Sensor

The testbed is a two link planar robot with two rotational axes of control. (see Figure 2.1) It floats on top of a granite table with the elbow and tip supported on air cushion disks. The granite table reduces the effect of all ground motion. The air cushion support reduces the surface friction to inertia ratios to very small levels. The arms have a tall thin cross section to allow for ease of bending in the plane of the table top, but be stiff

6 to torsion and perpendicular bending. The first four flexible modes of the structure are at 1.8, 3.4, 4.5, and 8.0 hz. See reference [3] for a description of the plant dynamic model and hardware validation. The two joints are controlled with individual direct drive DC torque motors. The joint angles are measured with optical encoders. The end point position is measured using an optical infrared emitter/detector and a reflective target on the manipulator tip. The end point rotational orientation is neither measured nor controlled. The controller is implemented in an AC100-C30 DSP based real time computer. The actuators are AeroFlex TQ82W-1FA and TQ40W-11FA brushless DC torque motors. The controller D/A converters are 12 bit with a 2.603 Nm maximum shoulder torque for a 0.00127 Nm quantization. The elbow motor has a 0.6215 Nm maximum for a 0.000303 Nm quantization. The shoulder motor begins to leave the linear region at around 35. The elbow motor loses linearity at about 25. Both motors are at 75% sensitivity at 60. The joint angle sensors are Hewlett Packard HEDS-6000 optical encoders. Each has a 1024 cycle/revolution TTL quadrature output. The resulting angle quantization from the encoder converter is 12 bit for a 0.00153 rad resolution. This is equivalent to a 1.00 mm end point resolution. The encoder linearity is unknown. The position errors are specified as 0.0052 rad maximum. The controller hardware is a digital AC100-C30 real time computer supplied by Integrated Systems Incorporated (ISI) (see reference [11]). It is based on a Texas Instruments TMS320C30 DSP card housed in a 486 PC. All sensor/actuator I/O is structured through industry pack I/O daughter cards connected to the local DSP bus. The system is commanded through software on a Sun SPARC computer using a standard ethernet interface. The dedicated DSP allows the controller to operate with a consistent sample update, without being burdened with network and disk traffic. The control software is built on the Sun SPARC station using both graphical block diagram autocoding and user written C code. It is cross compiled on the PC for the TMS320C30 DSP. The real time system allows for any regular fixed multi-rate digital timing structure.

Sun SPARC Control Design and Coding Software

000000000 111111111 111111111 000000000 111111111 000000000 111111111 000000000 11111 00000 111111111 000000000 111111111 000000000 11111 00000 111111111 000000000 111111111 000000000 111111111 000000000 111111111 000000000
AC100 486 PC
|| Local Bus || commands -> Ethernet I/O <- Saved Data

Direct Connect Real Time Control ->

TMS320C30 DSP

I/O

Serial Line Real Time <- Sensing

0000000 1111111 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000 1111111 0000000
Hardware Torquers End Point Sensor

Figure 2.2 Real Time System Command and Data Flow

The end point position sensor is a DynaSight bi-optic infrared emitter/detector that tracks a small reflective target connected to the manipulator tip. It is suspended over the top of the table, looking down perpendicularly at the operating plane. It supplies absolute 3-D position measurements of the target with a nominal 0.05 mm resolution. The mounting structure contains a number of mounting holes so that the sensor can be placed in almost any location above the table plane with four inch increments. The current mounting location is the 5 th hole in the table +z axis (from the bottom), the 9th hole in the table +y axis (from the back), and the 5th and 6th hole in the table +x axis (to the side).

2.2. End Point Position Sensor

The DynaSight position sensor is an absolute 3-D position sensor built by Origin Instrument Corporation in Grand Prairie Texas. (see reference [7]) It was originally developed as an observer location sensor for virtual reality applications. It was chosen based on its ease of integration and performance for a relatively low cost. Its accuracy is less than that of a high performance laser tracking system, but is still sufficiently better the joint based encoders. It has been shown to have sensor noise characteristics of the same type as laser tracking systems.

8 2.2.1. End Point Sensor Operation

The DynaSight sensor is a self contained bi-optic system. It projects IR light into its field of view and looks for the reflection from a small circular target. The target is made from a material with good IR reflective properties so that it appears as the brightest IR location in the sensor field of view. The sensor optics focus the reflection from the image onto an internal CCD plane at a location proportional to the angles to the target. The cartesian centroid of the CCD image gives the two angles from the sensor to the target. The sensor has two optical inputs with a fixed separation, and uses triangulation to convert from the four (redundant) angle measurements into three linear position measurements. The noise and errors are therefore noticeably greater in the distance (z) axis. End Point Sensor
CCD 1

0 1 1 0 1 0 1 0 1 0 1 0 1 0 1 0

CCD 2

XD ZD

0 1 1 0 1 0 1 0 1 0 1 0 1 0 1 0

X Z

Target Figure 2.3 Sensor Triangulation (2 axis)

The DynaSight sensor uses target tracking rather than target scanning. The initial brightest reflection point is determined from a complete scan of the field of view. Afterwards, that reflection is tracked (using an initial location guess based on the previous measurements). This method decreases the measurement time, but increases the sensitivity to tracking loss of high speed targets. Typical reacquisition times after a loss of target are observed to be approximately 0.6 seconds with a 1.0 second maximum.

9 Maximum target range and position measurement noise levels are partially dependent on the size of the reflective target. Larger size targets allow for longer ranges. With the chosen target, a 19 mm diameter, the useful range is on the order of 1.0m.

00000 11111 11111 00000 11111 00000 11111 00000 1 0 10 0 1 1 0

Figure 2.4 IR Target There is a small three color LED on the face plate of the sensor that gives the internal tracking state of the sensor. A green state implies tracking. A yellow state implies danger. A red state implies loss of tracking. This tracking information is also made available to the controller through the data interface. The exact implementation of the tracking algorithms in the sensor firmware (redundant measurement triangulation, target centroiding, noise rejection) is proprietary to Origin Instruments.

2.2.2. End Point Sensor Integration

The DynaSight sensor is a self contained unit. Communication with the AC-100 controller is made through a standard 9 pin RS-232C serial communication port. Completed position measurements are passed asynchronously in packets with initiation from the DynaSight. Since the firmware does not currently support hardware handshaking, the host computer is expected to pick up all data when it arrives. Input to the AC-100 controller is made through a standard IP-serial I/O card connected to the DSP local bus. The IP-SERIAL I/O card has two independent channels that support either RS-232 or

10 RS-422 communication protocols. The DynaSight is connected to the A channel of the IPSERIAL card in RS-232 mode. The DynaSight sensor is capable of a number of operating modes. Each of these modes has a different data collection timing method and speed with a number of possible output formats. See reference [7]. The sensor mode is set through either an external dip switch or, more generally, through serial line commands. Although the DynaSight is capable of running in a synchronized mode through serial line initiation commands from the AC-100 host computer, to increase data collection speed and reduce data latency (time delay between the actual image capture on the sensor CCD and when completed position information is used by the controller), the DynaSight sensor is put into a free running mode. This increases data update rates and high speed tracking ability at the cost of a certain amount of ambiguity over data latency. The DynaSight sensor for the flexible manipulator testbed is configured to run in stereoSync60 mode (or V mode). This is a free running mode with a nominal update rate of 67 hz. It trades off centroiding and environmental light rejection accuracy for reduced data latency. This is the mode with the fastest update rate and best high speed target tracking ability. The sensor outputs an eight byte packet at 19200 baud (3.3 msec total transmit time) every 15.2 msec. Since the sensor read and controller sampling are asynchronous processes, the data latency is variable. A delay of up to 0.015 seconds is produced by the sensor internal processing. As the two processes shift in time relative to each other, the delay between the receipt of the measurement packet and the beginning of the next controller sample period changes. This delay is bounded between 0 seconds and the sample period. As the sample rate is increased, the delay is shortened (in an absolute sense). However, for sampling rates above the sensor rate of 67 hz., a certain percentage of the sample periods will be initiated without any new sensor data. It is possible to identify these sample periods by looking for transitions in the packet count number, but this is currently not implemented in the controller designs. As data packets are created, they are sent over the serial line. The IP-SERIAL I/O card contains a three byte hardware buffer to reduce the interaction of the serial line

11 hardware and the software (see Figure 2.5). The AC-100 executables contain interrupt drivers that collect and save all incoming bytes to a ring buffer for later processing. The size of the software buffer is arbitrary (it is currently set to 1024 bytes which corresponds to a maximum sample period of 1.95 seconds). At the sample rate in which serial data is requested, a processing routine is called that retrieves all the bytes in the ring buffer and attempts to decode the packetized position information into floating point position data. The decoding routine uses a static state data structure so that partial packet information can be processed. This allows the receipt of sensor data to span multiple processing calls. The data packets also contain unique header patterns which allows the beginning of a data packet to be identified in a larger list of received data bytes. These two features allow the decoding process to run completely independently of the timing of the incoming data.
Hardware Software

0000000000000 1111111111111 0 1 01 1 000000000001 11111111111 0 00 11 0 0 1 000 1 0 11 0 1 111 000 1 0 111 000 1 0 111 000 1 0 000000000000000000 111111111111111111 00 11 01 1 000000 111111 0 00 11 00 11 0 1
IP-Serial Card Ring Buffer Decode Subroutine End Point Sensor Serial Line 3-Byte Buffer

To Control Software

Target

Asyncronous - Fast

Syncronous - Slower

Figure 2.5 Sensor Data Flow

The controller can sample the position information at nearly any interval between a maximum of about 2.0 sec to a minimum of about 0.0003 sec. For periods less than 15 msec, it is possible that new sensor data may not have arrived since the last sample period, and the decoding routine outputs will not have changed between sample periods. The user written serial decoding routines exist in two different versions. The first version is used to set the operating mode of the sensor. Its only function is to pass a command packet to the DynaSight sensor. The second version is used to decode sensor position packets. It assumes that the sensor is in a compatible operating mode. Its only function is to receive and convert data from the sensor. These functions were separated to avoid the one second retargeting delay that occurs when the sensor operating mode is

12 changed. The decoding routine is based on a sample routine supplied by Origin Systems. See appendix 2.2 for code listings and further information.

2.2.3. End Point Sensor Use

The controller inputs from the DynaSight sensor are given in table 2.1. Table 2.1 Sensor Output Data Name X position Y position Z position Data Quality Sync Quality Packet Number Units mm mm mm n/a n/a n/a Range 500 500 500 -1,0,1,2 0,1 1 - 1e37 Resolution 0.05 0.05 0.05 n/a n/a 1

The positions are the rectilinear distances of the target centroid from a small mark on the sensor face plane. Measurements are given in the sensor local coordinate frame. (see figure 2.6) A transformation to the manipulator table coordinates needs to be made in the controller software. The exact transformation depends on the mounting location and orientation of the sensor. In the nominal mounting location, the transformation is given by: x table = x center y table = y DynaSight
1000

x DynaSight
1000
(1)

y center

13 Y

000 111 111 000

00 11 11 00 11 00
Z

Figure 2.6 Sensor Coordinate System

The Data Quality flag marks the tracking state of the end point sensor according to the following enumeration : -1 0 1 2 enumeration : 0 1 decode software is synchronized to sensor packet headers decode software has lost packet location new data has not been received since last output request sensor is tracking, data is good sensor is in danger of loosing track, data is suspect sensor or software has lost track, data is either bad or not changing

The Sync Quality flag marks the state of the decoding software according to the following

The packet count contains the number of data packets from the sensor that have been successfully decoded. It can be used to test for the arrival of new position data. If the sensor has lost target track, the position returned from the sensor is the latest tracked position, but the packet count will still increment (examine the Data Quality output to identify this case.) All the appropriate code has already been included into the AC100 link list and object libraries. No special code needs to be included in the System Build controller, apart from a coordinate transformation / calibration block. All connections to the hardware are made inside the AC100 Hardware Connection Editor.

14 To configure the sensor to operate in the proper mode, a simple AC-100 project must run that has no other function than to send a single configuration command to the sensor. An MS-DOS batch file was created that runs the AC-100 project. At the DOS prompt on the AC-100 host computer, type : C:/> dynaset After the sensor face plate status light has turned red, the project can be canceled by typing CTRL-C. The face plate status light should turn green in approximately one second if a target is in the sensor field of view. The setup routine only needs to be run once after the sensor has been turned on. The operating mode is maintained internally within the sensor hardware. The connections from the sensor hardware to the software inputs are made in the Matrix/X AC-100 Hardware Connection Editor. All inputs are currently connected to module C. The module type is IP_SERIAL_A. The data is indexed from zero.

Table 2.2 Hardware Connection Editor Data Name X position Y position Z position Data Quality Sync Quality Packet Number Type IP_SERIAL_A IP_SERIAL_A IP_SERIAL_A IP_SERIAL_A IP_SERIAL_A IP_SERIAL_A Module 3 3 3 3 3 3 Index 0 1 2 3 4 5

2.2.4. End Point Sensor Calibration and Compensation

In addition to additive noise, the physical configuration of the sensor relative to the

15 manipulator table gives rise to constant position and rotation errors that need to be compensated. In particular, if the end point position measurements are to be used in conjunction with the joint angles, the transformation must be made so that the nominal outputs for undeformed links are consistent. The geometry gives rise to nine parameters : 1 2 3 4 5 6 7 8 9 sensor center table x axis location sensor center table y axis location arm 1 length arm 2 length shoulder (joint 1) zero rotation angle elbow (joint 2) zero rotation angle sensor x axis rotation sensor y axis rotation sensor z axis rotation. kx ky L1 L2 1 2 x y z

Parameters 1 and z are not independent, so there are actually only eight compensation parameters. There are five possible measurements: 1 2 3 4 5 DynaSight x position DynaSight y position DynaSight z position shoulder (joint 1) encoder angle elbow (joint 2) encoder angle xD yD zD 1 2

In addition, there is one constraint, the end point is constrained to move in only two dimensions (the table plane).

16 kx
XD

Ytable

2
Sensor Zero Point

YD

L2 ky 90 L1 1 Xtable Figure 2.7 Table Top Misalignments

0000 1111 1 0 0000000000000 1111111111111 1111 0000 1111 0000 1 0 1111 0000 1111 0000 1 0 1 0 1111 0000 1111 0000 1 0 1 0 1111 0000 1 0 0 1 1111 0000 1 0 1 0 1111 0000 1 0 111 000 1 0 1111 0000 1 0 111 000 1 0 1111111 0000000 1111 0000 1 0 111 000 1111111 0000000 111111111 000000000 111 000 1111 0000 1 0 111111111 000000000 1111111 0000000 111 000 1111 0000 1 0 111111111 000000000 1111 0000 1111111 0000000 1 0 111 000 111111111 000000000 1111 0000 1000 0 111
real sensor virtual sensor
y

XR

Z R = Zo

ZD

XD

table

target

Figure 2.8 Sensor Misalignments and Constraints

Under the assumptions of small sensor misalignments (x and y), the calibration procedure for these misalignments can be separated from the other six parameters. The real forward kinematic model for the end point location is given as : x e = L 1 cos( 1 ) + L 2 cos( 1 + 2 ) y e = L 1 sin( 1 ) + L 2 sin( 1 + 2 )
(2)

17 where, x e, y e real end point location real joint angles

1, 2

The end point sensor compensation equations are given as : xD x e = k x 1000 ye = yD + ky 1000
(3)

The joint angle compensation equations (for a 12 bit per revolution, relative encoder with the elbow joint at a nominal angle of 90) are given as :

1 = 2 =
where x D, y D

2 + 1 212 1 2 + 2 1 2 2 + 2 2

(4)

DynaSight outputs encoder outputs static offsets

1 , 2 1 , 2
written as,

Under the assumption of small nominal joint angle offsets, the sinusoid terms can be

sin( + ) . sin( ) + cos( ) cos( + ) . cos( ) sin( ) Parameters (1) to (6) can now be written in a linear relationship to the measurements.
xD yD = 1000 0 1 sin( 1 ) 1 0 cos( 1 ) cos( 1 + 2 ) sin( 1 ) sin( 1 + 2 )
kx
(6) (5)

sin( 1 + 2 ) cos( 1 ) cos( 1 + 2 ) k y

L1 L2

1 2

where

18

1 = L 1 1 2 = L 2 ( 1 + 2 ) 1 = 2 =
2 212 1 2 1 2 2 + 2 2
(7)

The sensor mounting misalignments can be written as the following transformation. Notice that the end point sensor twist, z , is not independent of the other table top parameters and is not included in the equations. xR yR zR z0 =
0 1 0 0 cos( x ) sin( x ) 0 sin( x ) cos( x ) cos( y ) 0 sin( y ) 1 0 0 sin( y ) 0 cos( y )

xD yD zD z0
(8)

The z equation, under the constraint zR=z0, can be written in the form, zD = sin( y ) x + cos( y ) D sin( x ) y + z0 cos( x ) cos( y ) D
(9)

which becomes the matrix equation

1
zD = xD yD 1

2
z0

(10)

where 1 = tan( y )

2 =

tan( x ) cos( y )

The exact compensation equation for the misalignments could be used as given above. However, because of the greater noise associated with the distance (z) measurement, there is a second form of the compensation equations that doesn't use z D. This second form is preferable. By application of the two dimensional table top constraint, zD can be removed from the equations.

19 xR yR cos( y ) 0 sin( y ) xD yD zD z0
(11)

sin( y )sin( x ) cos( x ) sin( x )cos( x )

with zR z0 = 0 = sin( y )cos( x )x D + sin( x )y D + cos( x )cos( y )(z D z 0 ) zD z0 = can be rearranged to give, 1 tan( x )tan( y ) cos( y ) 0 1 cos( x ) sin( y )cos( x )x D sin( x )y D cos( x )cos( y )

xR yR

xD yD
(12)

At this point, we have a series of equations, linear in the unknown parameters. By measurement of the known sensor outputs at a number of points, a series of over determined linear equations can be formed. m a1 m a2 ! m an Ma where m ai vector of the i th measurement of x D and y D (z D ) m bi matrix from the i th measurement of 1 and 2 (x D and y D ) p a vector of parameters This has a solution which minimizes the square of the errors,
1 p = (M T MT a Ma ) a Mb

m b1 = m b2 ! m bn p
(13)

= Mb p

(14)

20 or, numerically more stable alternatives using a singular value decomposition (given here in Matlab language) : p = pinv(M a ) M b = Ma \ Mb
(15)

Measurement data was collected at fifty different points. They were evenly spaced across the sensor field of view work space to improve the condition number of the measurement matrices. Because of the stochastic nature of the DynaSight outputs, 20 consecutive pieces of data were taken at each location and averaged to create a single measurement. The minimum least squares error equations were solved to give the following results: Table 2.3 Calibration Parameters Parameter kx ky L1 L2 1 2 1 2 1 2 z0 Value 0.7621 0.6533 0.6519 0.6019 0.0504 0.01855 0.07736 -0.04654 0.002906 0.01294 0.7746 Units m m m m m-rad m-rad rad rad rad rad m

Which results in the compensation equations : xr yr = 1. 0000042 0. 0000376 0. 0000000 1. 0000838 xD yD

(16)

21 x = 0. 7621 0. 001 x r y = 0. 6533 + 0. 001 y r

(17)

Due to the small size of the misalignments, the rotational compensation was not implemented in the control software. Note that due to the initialization properties of the encoders, the values for 1, 2 , 1, and 2 will change for each series of calibration runs, and all data must be collected in a single series of runs without powering down any of the hardware.

2.2.5. End Point Sensor Performance

Because the DynaSight sensor depends on precise imaging of the target, the performance is strongly effected by the location of the target relative to the optics (the face plate). The sensor has a limited field of view with a noticeable decrease in performance near the edges. With the 6mm target, the sensor has a distance limit of about 1.2 m. The sensor was placed at 0.8 m above the table plane to place the work space well within the high performance area. The limited imaging area of the optics gives a square cone field of view of approximately 32. This results in a usable working area of a square with 0.8 meter sides. There is a noticeable increase in sensor noise near the work space edges. This is compounded by the fact that, because of the constant target normal vector, the angle of incidence between the target and the optics increases near the edges. Even at the center, the maximum angle of incidence for acceptable tracking is about 30. The sensor tracks the target centroid, as opposed to scanning the entire field of view. This decreases required processing time and increases the data update rate. However, if the target moves too far during the update interval, the target can be lost. This results in a maximum linear rate of the target for low frequency motion. The stereoSync60 mode maximizes this rate. The exact maximum rate is unknown (~0.8 m/s), but seems to be less than the rate exhibited during control. Other modes had barely acceptable

22 maximum allowable linear rates. No problems have been observed with either target tracking or rejection of spurious environmental light (sun light, overhead fluorescents, metallic objects in the field of view). The sensor thermal transient effects were measured. Short term variations of 0.6 mm total position accuracy were observed over the first hour. These dropped to 0.2 mm after thermal steady state was reached. The sensor is operated in a free running mode, where it is supplying new position measurements at as fast a rate as possible. The nominal data update rate was measured to be 65.75 hz 0.47 3 (20 trials with 0.125 hz quantization). Short term variations in the sample period where not measured, but were observed to be small. Target reacquisition time after a tracking loss was also measured. Typical values were in the 0.60 to 0.66 second range. Maximum time was around 1.0 seconds. Sensor noise (variations from the mean when the target undergoes no motion), contain both quantization and (assumed) stochastic effects. For the sensor mounting geometry, the quantization is fixed at 0.05 mm. The DynaSight firmware does have a dynamically adjusted gain that doubles the quantization as the absolute target distance increases. This feature should not be seen in testbed fixed mounting configuration. The stochastic noise has an RMS magnitude that changes with a number of parameters. The noise level increases for the following situations : 1) larger perpendicular distance from the sensor center line, 2) larger distance from the face plate, 3) smaller reflective target. The stochastic noise is mostly constant for a large range of configuration values, with a dramatic increase at some limiting point (i.e. the noise is constant across the workspace until 350 mm from the center line, at which point the noise increases until targeting is lost at 400 mm). The form of the sensor noise spectral variation is shown in Figure 2.9. The theoretical covariance of the noise is given by

cov(x ) = 2 I PSD() d
0

(18)

23

= 2 IN
0

2 + b 2 d 2 + a 2

2 2 0 + b a tan 1 ( 0 ) = 2N a a An example PSD is shown in Figure 2.10 for the centerline target location and no external

disturbances. High frequency data was collected at the full 67 hz rate for 66 minutes (218 data points). Low frequency data was collected at a reduced 67/500 hz rate with a 0.02 hz anti-aliasing low pass filter for 13 hours (2 13 data points). For each data set, an adaptive window size frequency average was used with square windows.

0 1 1 0 1 0 1 0 0000000 1111111 1 0 1 0 1 0 0 1
a b

log( spectal density power )

-40 dB/dec slope

log(frequency)

Figure 2.9 Sensor Noise PSD Format

10

10

spectral power (mm 2/hz)

10

10

-2

10

-4

10

-6

10

-6

10

-4

10 frequency (hz)

-2

10

10

Figure 2.10 Sensor Noise PSD Hardware Test

24 Data was collected at a number of locations, both with and without external disturbances (physical vibrations from the motor power supplies are transmitted through the sensor mounting structure and are thought to have a possible effect on the position errors). The results are given in Table 2.4. Table 2.4 Sensor Noise Parameters Test # 1 2 3 Location 0, 0 0, 0 350, 0 External Noise off on off N 4.0e-5 5.2e-5 3.0e-5 a < 1e-6 < 1e-6 < 1e-6 b 0.008 0.024 0.020

Since the bandwidth of the compensator will almost certainly be well above the noise PSD pole frequency (b), and the zero is much less than the pole (a<b) it makes sense to think of the noise as consisting of two separate additive components. cov(x ) 2N 0 + N a
b2
(19)

The first term contains high frequency noise. Its effect on the final covariance of the end point position error is a direct function of the compensator bandwidth. The second term contains only low frequency noise which will be passed by the compensator directly to the end point position error covariance. Assuming a simple PD compensator of bandwidth , the end point position error covariance due to the sensor noise is approximated by :
b2 cov(x err ) . N + a
(20)

Without an acceptable standard, it was difficult to measure the absolute (nonstochastic) position errors. The sensor documentation gives a specification of 2 mm RMS absolute accuracy at 0.80 meters for a 7 mm target. They were observed to be bounded by 2 mm at a few random test points. There is no spatial frequency information.

25 Table 2.5 Sensor Noise Levels xmean (mm) 0 50 100 150 200 250 300 350 400 N [no disturbance] (mm2/hz) 1.04e-5 0.93e-5 0.50e-5 1.27e-5 2.75e-5 2.11e-5 15.8e-5 3.00e-5 23.9e-5 N [with disturbance] (mm2/hz) 1.72e-5 0.95e-5 0.64e-5 1.14e-5 1.96e-5 3.67e-5 4.08e-5 7.26e-5 53.6e-5

3. Design

The physical configuration of the hardware to be controlled gives rise to a situation where simple classical techniques (e.g., PID control) are incapable of stabilizing the system under end point feedback except for a small limited set of joint angles. In this section, we describe the difficulties and propose some possible solutions.

3.1. Design Issues

In addition to typical feedback control system design issues (uncertain mass properties, non-zero sensor noise, limited control authority, high frequency flexibility, digital implementation high frequency phase loss, unknown and possible nonlinear joint friction), the use of the end point sensor for direct feedback gives rise to a number of special issues.

With the position sensor referenced to the end point location as opposed to the joint angles, the output equations relating the plant state variables and sensor measurements become nonlinear. (It is possible to linearize the output equations through a change of state variables, but then the input equations become nonlinear, so nothing is gained.) This will be shown to be a strict nonlinearity, which can not be removed by the assumption of either small rates or small arm deformations. This becomes one of the driving issues for the controller structure and is a major part of this design (see section 3.2).

Sensing at the arm end point and the control actuation at the joints, along with arm flexibility, creates a noncolocated system. The finite time required for a displacement wave produced by the control torques to reach the end point sensor location gives rise to dynamics that can have a different zero structure than that of a colocated system. As an example, the general form of the linearized transfer function from the torque

27 input to the position output (either rotational or translational) for a single link is given by :
2 2 2 1 X (s ) 1 1 1 2 2 2 n n n = + + + 2 2 2 2 (s ) Js 2 s 2 + 2 1 1 s + 2 s + 2 2 2 s + 2 s + 2 n n s + n 1 1 func( 1 , 2 , , n , 1 , 2 , , n ) = 2 2 2 2 2 2 2 Js s + 2 1 1 s + 1 s + 2 2 2 s + 2 s + 2 n n s + n

(21)

where i is the modal amplitude for mode i at the input point and i is the modal amplitude for mode i at the output point (i.e. the sensor location). If the output and input points are different ( i i ), then it is possible for the output modal magnitude to either have a

different sign than the input modal magnitude, or to be zero. This can significantly change the transfer function numerator structure. In any real system, one or more modes will have an output out of phase with the input and the model magnitude will be negative ( i i < 0). This results in at least one transfer function zero with a positive real part. If the output location is a node of a particular mode, the modal magnitude will be zero (i i = 0). The result is a pair of complex zeros directly on top of two of the complex poles, removing them from the transfer function. In the multi-input, multi-output manipulator case, when the outputs are limited to the end point positions, the flexibility between the input joint torques and the output translational position produces transmission zeros with positive real parts in the linearized dynamics. This has two effects. It limits the maximum achievable controller bandwidth to the lowest frequency of all the system transmission zeros with positive real parts. (This is essentially a period that is less than the delay of the traveling displacement wave between the actuator and the sensor.) It also reduces the ability of the compensator to stabilize the plant (for a sufficiently high gain, any linear compensator will result in an unstable system). Cannon et.al. [reference 1] has shown that speed of response can be increased in the noncolocated case, but this requires sufficient knowledge of the lower flexible mode frequencies and mode shapes. In addition, greater arm deformations will result. The exact placement of the end point position sensing can also greatly effect the ability of the compensator to effect the flexible modes. If the end point sensing location is placed at a node, that mode becomes unobservable to the compensator. This difficulty is not often faced by a joint based control scheme, where the multiple measurements (one per

28 joint) are made at different locations. The probability that all joints are located at a flexible mode node is small. This is not true for an end point sensor, where multiple measurements (x,y,z axes) are made at a single location. The flexible manipulator testbed was observed to have a mode (the second lowest flexible mode) where the end point measurement location was very near a node.
60

40 x = system poles o = system transmission zeros 20

-20

-40

-60 -100

-80

-60

-40

-20

0 Real Part

20

40

60

80

100

Figure 3.1 Zero-Pole Map - End Point Sensor

The sensor suite for the flexible manipulator testbed consists of not only the end point position sensor, but also the joint angle encoders. It can be shown that the linearized plant dynamics which uses both end point and joint angle measurements does not have the positive real valued transmission zeros associated with the case of end point measurements only. Therefore, if the joint measurement quantization mapped to the end point position is comparable (not significantly more than an order of magnitude larger) to the end point measurement noise, there is a real advantage to using both types of sensors.

29
60

40 x = system poles o = system transmission zeros 20

Imaginary Part

-20

-40

-60 -100

-80

-60

-40

-20

0 Real Part

20

40

60

80

100

Figure 3.2 Zero-Pole Map - End Point and Joint Sensors

The objective of this analysis is, however, to examine the control of flexible manipulator structures to high accuracy using end point feedback. Given the extremely high accuracy possible with an optical linear position sensor, it is possible that absolute control accuracy could be brought well below that of the mapping of the joint position measurement quantization to the end point location. In that case, the joint angle measurements do not provide any additional information. Note that this is not analogous to a Kalman filter where any new measurement, no matter how noisy, can lower estimation error covariances, because for the encoder quantization case, the sensor measurement errors are not independent, additive, or Gaussian. For this reason, joint angle measurements are not used in this feedback control system design. This does not invalidate the use of a (nonlinear) compensation scheme based on derived joint rates to achieve a greater level of global stability.

The DynaSight image processing and asynchronous update method result in time delays of the sensor position measurements. Although they are small enough not to be destabilizing to the controller in general, they do produce a phase loss that was shown to be destabilizing to some of the higher frequency modes if not accounted for.

30 3.2. End Point Feedback Nonlinearities

The rigid body output equations for the end point location are given by: x = L 1 cos( 1 ) + L 2 cos( 1 + 2 ) y = L 1 sin( 1 ) + L 2 sin( 1 + 2 )
(22)

If the full form flexible equations (using the systems mode representation modification of the assumed modes method) are written out (see reference [4]), the end point location is given by : x = (L1 d 1) cos(1) 3 1iq i sin(1) + d1cos 1 + 3 1i N q i
n n

i = 1

i =1

(23)

+ (L2 d 2) cos 3 2iq i sin 1 + 3 1i N q i + 2 1 + 3 1i N q i + 2

n n n

i = 1 n n + d2cos 1 + 3 1i N q i + 2 + 3 2i N q i i =1 i =1
i =1

i =1

y = (L1 d 1) sin(1) + 3 1iq i cos(1) + d1sin 1 + 3 1i N q i

n n

i = 1

i =1

+ (L2 d 2) sin 3 2iq i cos 1 + 3 1i N q i + 2 + 1 + 3 1i N q i + 2

n n n

i = 1 + d2sin 1 + 3 1i N q i + 2 + 3 2i N q i i =1 i =1
i =1

i =1

where

ki

is the amplitude of mode shape i at the end of link k

ki N is the slope of mode shape i at the end of link k

q i (t) is the time varying magnitude of mode i Lk dk is the length between joint axes is the distance from the end of the flexible link k to either the elbow joint (k = 1) or the end point (k = 2)

When the flexible mode deformations are assumed to be small, the equations result in:

31
x = L1cos(1) sin(1) 3 ( 1i + d 1 1i N )q i + L2cos(1 + 2) sin(1 + 2) 3 ( 2i + d 2 2i N + L2 1i N )q i = L1cos(1) + L2cos(1 + 2)
(24)

n i = 1

n i = 1

3 i qi sin(1 + 2) 3 i qi sin(1) n i = 1

n i = 1

n i = 1

y = L1sin(1) + cos(1) 3 ( 1i + d 1 1i N )q i + L2sin(1 + 2) + cos(1 + 2) 3 ( 2i + d 2 2i N + L2 1i N )q i = L1sin(1) + L2sin(1 + 2)

n i = 1

3 i qi + cos(1 + 2) 3 i qi + cos(1) However, the joint angles (1 and 2) cannot be assumed to be small, and the state to output equations will remain nonlinear. Note that the variation of the end point position due to the flexible modes is 90 in phase ahead of the rigid body modes.

n i = 1

n i = 1

3.2.1. Effect of Nonlinearities

Because the output equations contain nonlinearities that have a major role in the dynamics of the system, standard robust compensator design methodologies, which assume a plant with linear dynamics, will not work well in this situation. Joint angle variations can produce changes in the linearized version of the output equations with infinite gain variation. As an example, the linearized output matrices for four nominal joint angles are given below : X ep Y ep C(0,
2

= C( 1 , 2 ) 1 2 q 1 q 2 q 3 q 4 )
2

= ) =

C(0,

-0.6012 -0.6012 -0.3312 -0.4130 0.0156 0.6325 0 0.1950 -0.0952 -0.0023 0.6012 0.6012 0.3312 0.4130 -0.0156 0.6325 0 0.1950 -0.0952 -0.0023

0.0047 -0.0007 -0.0047 -0.0007

32 C(
2

,
2

)
2

= -0.6012 -0.6012 -0.3312 -0.4130 0.0156 0.0047 ) =

0.6325 0 0.1950 -0.0952 -0.0023 -0.0007 0.6012 0.6012 0.3312 0.4130 -0.0156 -0.0047

-0.6325

-0.1950 0.0952 0.0023 0.0007

C(

There are no design methods based on linear time invariant systems that are equipped to handle such extreme gain variations (which actually are sign variations). To overcome these nonlinearities, a nonlinear controller is needed. Possible methods include gain scheduling, exact linearization, and transformation of end points to hub angles. This analysis will consider the exact continuous linearization methods.

3.2.2. Exact Linearization Methods

The dynamic equations of motion in general form are given by: + N( , ) + G( ) + K + R( ) = T M( ) x = H( )

(25)

where M() is the system mass matrix, N( ) is a vector of coriolis and centripetal terms that depend on the square of the velocities, G( ) is a vector of gravity terms (which are zero in this case), K is a matrix of stiffness coefficients for the flexible modes, R( ) is a vector of nonlinear joint torques (friction), T is an input force transformation matrix, and H( ) is an output transformation function. For the rigid body case, the equations of motion reduce to : + N( , ) + R( ) = M( ) x = H( ) If a new input to the system, f, is defined as,
) J 1 ( ) + N(, ) + R() + K T = M()J1 ( ) f M()J1 ( ) J ( ,
.

(26)

(27)

where J( ) is the system Jacobian matrix and f is the new control variable, then the dynamic model between the new input and the output end point position is just a pair of double

33 integrators. + N + R + K = MJ 1 f MJ 1 J J 1 + N + R + K M = MJ 1 f MJ 1 J J 1 M + J J1 = f J + Jx = f J x = f However, construction of the real plant input, , from the new compensator output, f, is far from straight forward. The computations require knowledge of the current state vector for the flexible modes and the joint friction torques. Neither is directly measurable without additional sensors. (Estimation may be possible using complex, nonlinear estimation schemes and is currently under investigation.) With actuators only at the joints, the input transformation matrix, T, is not square, and therefore not invertible. If the flexible modes and joint friction are ignored, the simpler rigid body equations of motion give a result that is computationally more tractable. In the paper On Manipulator Control by Exact Linearization [reference 5] it is shown that all methods of exact linearization (computed torque technique, resolved acceleration control, nonlinear decoupling theory, operational space control, and geometric control theory) give identical results. This simplified transformation inverts the nonlinearity associated with the output transformation matrix, but only for frequencies below the first flexible mode.
1 1 1 + N( , ) = MJ f MJ J J

(28)

(29)

If the rates are also assumed small, so that all terms with an angular rate squared can be dropped, then a further simplification results,

= MJ f
1 2

MJ

fx fy

(30)

For the two link flexible manipulator, this matrix is,

34
L2 cos( 1 + 2 ) L2 sin ( 1 + 2 ) p1 + p2 + 2p3 p2 + p3 p2 + p3 p2

MJ

L 1 L 2 sin(2 )
L 1 cos( 1 ) + L2 cos( 1 + 2 )

L 1 L 2 sin(2 )
L 1 sin ( 1 ) + L2 sin ( 1 + 2 )

L 1 L 2 sin(2 )

L 1 L 2 sin(2 )
(31)

p1 = m1 a + m2 L + I1
2 1 2 1

p 2 = m 2 a2 2 + I2 p 3 = m 2 L 1 aa cos( 2 ) where mk Lk ak Ik is the mass of link k is the length of link k is the distance from the inboard joint to the c.g. of link k is the inertia about the c.g. of link k

Because the mass matrix is always positive definite, the transformation matrix is finite and nonsingular for all locations except at the singular points of the Jacobian matrix. This singular point corresponds to 2 = 0 for the flexible manipulator testbed. This state is one where the dynamic system is uncontrollable, so the difficulty is a property of the dynamics, not of the linearization method. From here on, this situation will be ignored (although near singular conditions can be important). The same result can be obtained in a more intuitive manner, if we look at the Jacobian equation. x = J( ) and make the following (incorrect) assumptions, x = J( ) ) . 0 N ( , then we get the same result.
(32)

(33)

= MJ 1 f

(34)

Figures 3.3 to 3.6 give four possible transfer functions ( 1, 2 inputs to xep,yep outputs) for the original flexible plant as 2 varies between - and .

35
50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.3 Plant Shoulder Torque to X Position T.F.

50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.4 Plant Shoulder Torque to Y Position T.F.

36
50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.5 Plant Elbow Torque to X Position T.F.

50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.6 Plant Elbow Torque to Y Position T.F.

37
50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.7 Linearized X Axis Force to X Position T.F.

Figures 3.7 to 3.10 give the equivalent four transfer functions when the transformation matrix is applied to the plant inputs (fx,fy inputs to x ep, y ep outputs). The transformation matrix has removed the variations in the low frequency regions.
50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.8 Linearized X Axis Force to Y Position T.F.

38
50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.9 Linearized Y Axis Force to X Position T.F.

50

Magnitude (dB)

-50

-100 10
-1

10

10

10

200

Phase (deg)

-200

10

-1

10

10 Frequency (rad/sec)

10

Figure 3.10 Linearized Y Axis Force to Y Position T.F.

39 3.2.3. Feedback Linearization

The rigid body linearization matrix is a function of the joint angles, which need to be known before the matrix can be constructed. An obvious choice is to get the joint angles from the joint position encoders. X

X ref

C(s)

MJ

-1

Plant

Figure 3.11 Feedback Linearization

Unfortunately, this situation does not result in a satisfactory response. Although the linearization is algebraically exact, because the system parameters are not exactly known and the encoder measurements are not exact, the linearization is a feedback loop and is subject to all the same stability concerns. Still it might be possible to include a dynamic compensator, as in a standard control loop. B(s)
f -1

X ref

C(s)

MJ

Plant

Figure 3.12 Compensated Feedback Linearization

As will be shown by performing a perturbation analysis around the joint angles, this loop has a multiplication nonlinearity with the commanded control actuation forces, so that it is also nonlinear and position dependent. If we assume that the joint angles consist of a fixed nominal value and a small perturbation around the nominal, we can make the assumptions,

40 sin( ) Y sin( o ) + cos( o ) cos( ) Y cos( o ) sin( o ) the equation for the applied torque,
2 P = g 1 ( 1 o , 2 o ) + g 2 ( 1 o , 2 o , 2 1 , 2 ) + g 3 ( 1 o , 2 o , 1 , 2 )

(35)

When these are applied to the rigid body linearization matrix, three terms appear in

(36)

The g1 term is just the desired nominal torque from the commanded forces if angle perturbations are ignored. The g2 term contains 2 terms and is ignored. The g 3 term is linear in the angle perturbations and shows the effect of the linearization matrix multiplication on the joint angle feedback loop. g3 = R ( 1 , 2 )
fx
1 2
p3 s2 ( L1 c 1 L2 c 12 ) L2 s12 ( p1 + p3 c 2 ) L2 p3 ( s1 c 1 2 + c 2 s12 ) p3 s2 ( L1 s1 L2 s12 ) + L2 c 12 ( p1 + p3 c 2 ) L2 p3 ( c 2 c 1 2 s2 s12 )

= L L s 1 2 2 + L L s 1 2 2
fy

L1 s1 ( p2 + p3 c 2 ) L2 s12 ( p1 + p3 c 2 ) L1 p2 s1 L2 p3 c 2 s1 2 L2 c 12 ( p1 + p3 c 2 ) L1 c 1 ( p2 + p3 c 2 ) L2 p3 c 2 c 1 2 L1 p2 c 1

1 (37) 2 1 2

where s i = sin( i ) c i = cos( i ) s ij = sin( i + j ) c ij = cos( i + j ) Because this matrix, R , depends on the commanded force, a joint angle feedback dynamic compensator can not be designed independently from the end point feedback compensation loop. In addition, it has the same type of nonlinearities that the matrix transformation was meant to solve.
(38)

3.2.4. Feedforward Linearization

Although the linearization matrix can be constructed using measured joint angles, it can also be constructed from the input reference signal. By assuming no flexible modal

41 deformations, the commanded end point can be transformed to the equivalent joint angles. This alleviates the problems associated with the feedback loop.
Inverse Kinematics f

est

X ref

C(s)

MJ

-1

Plant

Figure 3.13 Feedforward Linearization

The transformation from the commanded end point position to the equivalent joint angles is through the arm inverse kinematics.

1 2

2 2 2 L2 1 + (x + y ) L 2 = tan 1 y , x cos 1 2 L1 x 2 + y 2

L 1 + L 2 (x + y ) = cos 2 L1 L 2
1
2 2 2 2

(39)

The main drawback associated with this method is the limited capture region (where control errors are small enough so that the system is stable) of the controller. The torque errors produced from the difference between the actual and commanded joint angles are proportional to and should have negligible effects on the system stability only if the control errors are small. Because of this limitation, the controller structure requires some type of slew commander, so that transitions are smooth and errors are guaranteed to be small. Large step changes in the reference input can result in an unstable system. See appendix 2.2 and 3.4 for the slew commander equations. The size of the capture region can be estimated from the same perturbation analysis as was done in the joint angle feedback method. By assuming that there is a difference between the command and the plant state and then expanding the input equations about the plant state (the plant state is o and the command is o+), the linearized plant input perturbation is given by,

42 B p B N + B N = B M J 1 + B R F ( 1 , 2 ) where the linearized plant dynamics are z = Az + B = Az + BN f x = Cz and

R F ( 1 , 2 ) = L L s 1 2 2
1
L1 s1 ( p2 + p3 c 2 ) L2 s12 ( p1 + p3 c 2 ) L1 p2 s1 L2 p3 c 2 s1 2 L2 p3 ( s1 c 1 2 + c 2 s12 ) L2c 12 ( p1 + p3 c 2 ) L1 c 1 ( p2 + p3 c 2 ) L2 p3 c 2 c 1 2 L1 p2 c 1 p3 s2 ( L1 s1 L2 s12 ) + L2 c 12 ( p1 + p3 c 2 ) L2 p3 ( c 2 c 1 2 s2 s12 )

(40)

(41)

+ L L s 1 2 2

p3 s2 ( L1 c 1 L2 c 12 ) L2 s12 ( p1 + p3 c 2 )

This implies that the size of the capture region can be maximized by a design methodology that maximizes the controller robustness to an additive structured uncertainty.

3.3. Controller Design

3.3.1. Control Structure

The controller for the end point feedback system has the following structure:

Inverse Kinematics X target X ref Slew Command

est

z = Az z + Br Xref + Be Xest f = C z + Dr Xref + De Xest X est

MJ

-1

tor

Torque Comp

Plant

Sensor Comp

Figure 3.14 Control Structure

The slew commander generates algebraically consistent 2-D positions, rates, and

43 accelerations under maximum single axis rate and acceleration constraints. The sensor compensation transforms the end point position data from local sensor to table coordinates. The inverse kinematics computes the equivalent joint angles from the end point location based upon the rigid body kinematics. The compensator is a general state space digital dynamic filter. The linearization block transforms the control forces to the joint torques based on the rigid body linearization matrix. The actuator compensation converts the commanded control actuation from torques in Newton-meters to the appropriate voltages.

3.3.2. Compensator Design

The plant seen by the compensator is, for the most part, linear and time invariant. Any number of control methodologies are then applicable. The most relevant feature is now the arm flexibility, both the noncolocation zeros and the variations in the modal output over the different nominal joint angles. Possible control methodologies include : Classical Single Loop - has the advantage of being able to directly deal with any type of uncertainty due to the flexible modes. It has the disadvantage of a difficulty in dealing with the cross axis uncertainty (which must still exist, since the decoupling is not real, but rather artificially enforced. See reference [8]). LQG - has the advantage of its ability to compensate for cross axis coupling and it offers guaranteed nominal stability. It has a disadvantage in the difficulty in correcting for uncertainty in the higher frequency flexible modes. SANDY - (reference [12]) is perhaps the most general format. It's main advantage here is its ability to handle the uncertainty in the flexible modes based on the different operating points. For demonstration purposes, both a classical and an LQG design were constructed. The classical design attempts to create a compensator that is stable without any knowledge of the flexible mode shapes or frequencies, except for a lower bound on the frequencies. The LQG design is constructed assuming a reasonable knowledge of the two lowest frequency modes and a limited knowledge of the frequencies of the next two modes.

44 Classical Compensator Design:

The compensator is based on a Bode loop shaping design (the filter which gives a system loop gain function that surround the instability points with appropriate distances in the gain and phase dimensions) in the continuous frequency domain which was later transformed to the digital domain. The compensator filter provides a zero frequency integrator pole to provide zero steady state errors to constant disturbances, a series of lower frequency zeros to provide phase stabilization, and a number of higher frequency poles to reduce feedback gain at higher frequencies at the uncertain flexible mode locations. The continuous pole zero structure is given by C(s) = 8.4 x10 4
(s + 2 )(s2 + 0 .6 S + 0 .1 )(s2 + 2 s + 120 ) s(s + 60 )(s + 40 )(s + 8 )(s + 6 )(s2 + 15s + 120 )
(42)

100

Magnitude (dB)

50 0 -50 -100 10
-2

10

-1

10

10

10

Phase (deg)

-200

-400 10
-2

10

-1

10 Frequency (rad/sec)

10

10

Figure 3.15 Classical Compensator Loop Transfer Function

45 The resulting digital transfer function (through the use of a Tustin transformation approximation) is given as, C(z ) =
4.31 z7 + 11.9 z6 + 2.78 z5 + 20.1 z4 - 18.5 z3 - 4.40 z2 + 11.4 z - 3.76 z7 - 4.48 z6 + 8.27 z5 - 8.04 z4 + 4.35 z3 - 1.25 z2 + 0.160 z - 0.00553
(43)

The resulting single loop stability margin frequency plots are shown in figures 3.15 and 3.16.
150

100

Magnitude (dB)

50

-50

-100 -500

-400

-300

-200 Phase (deg)

-100

100

Figure 3.16 Classical Compensator Nichols Plot

The resulting input-output frequency response between the commanded and actual end point for the x axis is given in figure 3.17 (the y axis is similar).

46
20

10

Magnitude (dB)

-10

-20

-30

-40

-50

-60 -2 10

10

-1

10 Frequency (rad/sec)

10

Figure 3.17 Classical Compensator I/O Magnitude Response

100 0 -100 -200

Phase (deg)

-300 -400 -500 -600 -700 -800 -2 10

10

-1

10 Frequency (rad/sec)

10

10

Figure 3.18 Classical Compensator I/O Phase Response

47 In figure 3.17 the lower frequency flexible modes of the structure are seen to cross the unity gain threshold in the closed loop input-output frequency response. This is a direct result of the design strategy of the compensator. The system bandwidth was raised to nearly as high a frequency as possible, without any direct compensation of the flexible frequencies. This is presented as a demonstration of a design for a system where no information on the flexible modes of the system dynamics is available apart from a lower limit on the modal frequencies. This can be contrasted with the LQG design, to follow, which directly compensates for the flexible modes of the system dynamics, based on an accurate dynamics model, and produces a much smoother closed loop input-output frequency response. Monte Carlo analysis of the classic compensator has shown that it is stable for all nominal joint angles.

Linear Quadratic Gaussian Compensator Design:

The LQG compensator design methodology is model based. It is used here as an example of an attempt to actively control the manipulator flexible modes. Unfortunately, as shown in section 3.2, the spatial variation of the flexible modes is 90 out of phase with the rigid body modes. This makes control difficult. The methodology also suffers from limitations that required a slight modification in the problem formulation. The design model for the LQR and LQE sections of the LQG design was the linearized version of the plant dynamics with a nominal 1=0, 2=72. It was converted to a discrete time model using a standard matrix exponential method under the assumption of constant inputs over the sample period. To account for the computation time of the end point sensor hardware and the control hardware, a two sample delay was added to the model. (see the Matlab c2dt function). The first set of control designs lacked the time delay and, consequentially, stabilized the plant, but, in general, did a poor job of stabilizing the second flexible mode. The destabilizing effect manifested itself in the second mode because this mode was the mode closest to the Nyquist frequency, that did not have the

48 gain attenuation that was applied to the higher frequency modes. The LQG methodology assumes that the complete set of all information known about the plant dynamics is contained in the design model. In general, this is not true. Dynamic models are often more uncertain in the higher frequency modes and completely unreliable above the last modeled mode. Experimental tests have shown this to be true for the flexible manipulator plant model. To account for this uncertainty in the LQR design, two steps were taken. First, additional outputs were constructed based on the plant eigenvectors. For the flexible modes, the two eigenvectors were added to cancel the imaginary parts. Differing amounts of control were then applies to each individual mode, which would not be possible if the end point positions were weighted directly. The different weighting in the LQR design was based on assumed relative uncertainties for the modes. Secondly, a pair of first order low pass filters were added to the design model input (and resulting compensator output) in order to force a high frequency roll-off to reduce feedback at higher frequencies. This forces a low feedback gain at high frequencies where the plant model contains no information about the dynamics. The break point was set immediately above the second flexible mode frequency. A matrix transformation from the commanded end point positions to a system modal vector was added to map from the command to the regulator inputs. In the full nonlinear world, this matrix consists of the model Jacobian matrix (with additional zeros for the flexible modes). However, since the estimator state output is based on the linearized plant at a single design point, only the static input matrix was used.

In the LQR design, the zero frequency eigenvalues were weighted to achieve a 2 hz bandwidth. The flexible mode eigenvalues were marginally weighted to increase damping, but still allow for a certain amount of uncertainty. Increased weighting on the flexible modes, particularly the first mode, decreased the amplitude of the arm deformation under a step input, but also decreased the stable region of the compensator. In the LQE design, the sensor noise weights were set at the nominal DynaSight RMS positional errors. The plant input noise weights were set to correspond to 20% of the nominal commanded torque for a 0.1m step input command. Counter to intuition, an

49 increase of the plant model input noise weights, which should signal a plant model that can not be trusted, decreased the variation on the nominal joint angles before compensator instability. With the pure time delay in the design model, the Matlab digital LQR solution, which uses eigen decomposition of the system Hamiltonian matrix, could not proceed due to the singular digital plant model transition matrix. An LQR solution function was written that allows for a singular plant state transition matrix. It is based on the time variable backwards iteration solution of the discrete finite time horizon Kalman solution. The steady state solution is approximated as the iterative solution when the 2-norm of the LQR gain matrix changes by less than 1e-7 percent over ten iterations. See appendix 2.4.

50

magnitude (dB)

-50

-100 -2 10

10

-1

10

10

10

phase (degrees)

-200 -400 -600 -800 -2 10

10

-1

10 frequency (rad/sec)

10

10

Figure 3.19 LQG Compensator Loop Transfer Function The final compensator design x axis open loop gain and closed loop I/O frequency response plots are given in figures 3.19 through 3.21. The y axis plots are similar.

50
100 80 60 40

magnitude (dB)

20 0 -20 -40 -60 -80 -800

-700

-600

-500

-400

-300 -200 phase (degrees)

-100

100

200

Figure 3.20 LQG Compensator Nichols Plot

10

-10

magnitude (dB)

-20

-30

-40

-50

-60 -2 10

10

-1

10 frequency (rad/sec)

10

10

Figure 3.21 LQG Compensator I/O Frequency Response

Monte Carlo analysis of the LQG compensator has shown that it is stable for all nominal joint angles where cos(1+2) = constant. See figure 3.22. This implies that the

51 variation in the flexible modes over the work space is causing the instability. This conclusion is supported by the fact that for all nominal 2 design points, instability for off nominal elbow angles is always in one of the flexible modes (not always the same one).

4 o = stable point 3 x = unstable point

elbow angle (rad)

-1

-2

-3

-4 -4

-3

-2

-1

0 1 shoulder angle (rad)

Figure 3.22 LQG Stability Regions

4. Hardware Test Results

The controllers were tested using both a commanded slew and a set of disturbance rejection tests. The commanded slew sweeps across the usable work space with a maximum rate of 0.2 m/s and a maximum acceleration of 0.03 m/s/s. The response is meant to show both performance of the compensator and the stability over the nominal joint angles. The disturbance rejection tests were performed by applying a small impulsive force to the tip in either the x or y axes. Absolute pointing and pointing stability errors were not tested since a truth position was not available to compare against.

4.1. Classical Compensator Results

Results for the classical compensator are given below.

0.8
X Position (m)

0.8
Y Position (m)

0.7 0.6 0.5 0.4 0.3 0 10 20 seconds 30 40

0.7 0.6 0.5 0.4 0.3 0 10 20 seconds 30 40

150 100 50 0 -50

Joint Torques (Nm) Joint Angles (deg)

0.2 0.1 0 -0.1 -0.2 0 10 20 seconds 30 40

10

20 seconds

30

40

Figure 4.1 Classical Compensator Response to Slew Command

53
0.74

0.72

0.7

End Point Positions (m)

0.68 X position 0.66

0.64

0.62 Y position 0.6

0.58 0

10

15

20 seconds

25

30

35

40

Figure 4.2 Classical Compensator X Axis Disturbance Response

0.67 0.66 0.65 0.64 0.63 0.62 0.61 Y position 0.6 0.59 0.58 0 X position

End Point Positions (m)

6 seconds

10

12

Figure 4.3 Classical Compensator Y Axis Disturbance Response

54 From these results, it can be seen that : * The peak in the closed loop frequency response manifests itself as a small amount of oscillation in the slew response. This is a direct result of pushing the bandwidth of the compensator as high as possible without direct compensation of the first flexible mode. A smoother frequency response is only possible with either a reduction in the compensator bandwidth or stability margins. * The phase lag during the slew maneuver is due to the compensator bandwidth. This could be solved with an acceleration feedforward term. Such a term was left out of the compensator because of the large excitation of the flexible modes which resulted. Either a higher order slew function (such as a standard 5 th order trajectory generator) or a filter on the acceleration could be used to reduce this effect. * There are a number of rather slow poles in the closed loop system. They are a result of the integrator poles added to reject constant magnitude disturbances. The slow poles can be speeded up by either a reduction in the overall compensator bandwidth or by removal of the compensator integral terms. The integrator terms in the compensator are considered important because of the residual magnetism in the actuators which results in steady state errors for state feedback based filters (LQG, PD). * The Y impulse disturbance rejection test shows that there is still a reasonable amount of coupling between the two axes. This coupling is much less apparent in the X direction impulse disturbance rejection plots. * The classical compensator was seen to be (during other tests) stable for 2 angles out to 35. Since this angle is also just outside of the linear region of the actuator, it is unclear whether it is the compensator or the actuator that is causing the instability. The system is stable for all testable 1 angles.

4.2. LQG Compensator Results

The results for the LQG compensator design are given in figures 4.4 through 4.7.

55
0.75 X axis position 0.7

0.65

0.6

position (m)

0.55

0.5

0.45 Y axis position 0.4

0.35 0

10

15 time (sec)

20

25

30

Figure 4.4 LQG Compensator Response to Slew Command

shoulder angle 0.2 1.8

elbow angle

position (rad)

position (rad)
10 time (sec) shoulder joint 20 30

1.6

-0.2

1.4

-0.4

1.2 0

10

20 time (sec) elbow joint

30

40

0.1 0.05

0.1 0.05

torque (Nm)

0 -0.05 -0.1

torque (Nm)
10 time (sec) 20 30

0 -0.05 -0.1

10 time (sec)

20

30

Figure 4.5 LQG Compensator Commanded Slew cont.

56
0.82 0.8

position (m)

X axis 0.78 0.76 0.74 0

10 time (sec)

12

14

16

18

20

0.66 0.64

position (m)

0.62 0.6 0.58 0 Y axis

10 time (sec)

12

14

16

18

20

Figure 4.6 LQG Compensator X Axis Disturbance Response

0.77 0.76

position (m)

0.75 X axis 0.74 0.73 0

10 time (sec)

15

20

25

0.67 0.66

position (m)

Y axis 0.65 0.64 0.63 0

10 time (sec)

15

20

25

Figure 4.7 LQG Compensator Y Axis Disturbance Response

57 The following observations can be made from these results. * There is a steady state position error for constant commands. The offsets are a result of the residual magnetism in the actuators coupled with the lack of position error integration terms in the LQG compensator. Position error compensation can be added to LQG designs through augmentation of the plant model (reference 6, pg. 289 - 294). * The controller response during a command with non-zero rates is far from optimal. This behavior is most likely the result of the design procedure based on regulation. The commanded end point position is converted to an equivalent stable plant state command. The definition of stable in this context includes zero rates. An adaption of the input transformation to include non-zero rate commands should alleviate the problem. * The LQG compensator exhibits less separation of axes than the classical design. Examination of Figure 4.4 shows a Y axis slew command resulting in large X axis motion. It is unknown if the addition of non-zero rate command inputs would improve the behavior. * The slew response with the LQG compensator to the slew command exhibits less overshoot than the classical compensator. These results are consistent with the smoother closed loop frequency response. * The response to an impulse input disturbance is no faster than the classical design. Once again, there appears to be more cross axis coupling.

4.3. Joint Based Control Comparison

The initiative for direct end point feedback control of flexible manipulators was the desire to reduce the errors in the final end point position that a joint based control structure cannot compensate for. To illustrate these errors, a joint based controller was run on the identical hardware (except for the substitution of stiffer links). The controller was designed as part of a research effort in hybrid force-position control of the manipulator end point (see

58 reference [10]). The end point position of the manipulator was measured using the end point sensor, but the measurements were not used for position control. A 20 slew in both axes was performed on the two link manipulator. The end point position and joint angle measurement data was recorded. A reduced sensor parameter calibration was performed to calculate the new L1, L 2 , 1 , and 2 parameters. The end point position was calculated based on both the end point sensor and the joint angles with the forward rigid body kinematics. Figures 4.8 and 4.9 show the resulting end point positions and the errors between the two position calculations.

0.8

position (m)

0.6

0.4

0.2

0 0

4 time (sec)

Figure 4.8 Joint Base Comparison End Point Positions

These tests show an end point error of approximately 5% of the slew magnitude that results from the assumption of rigid body kinematics. These errors could be reduced through the use of a dynamic estimator, however, they could not be completely eliminated. The errors would have been much larger for the more flexible links.

59
0.03 0.02

X axis error (m)

0.01 0 -0.01 -0.02 0 1 2 3 4 time (sec) 5 6 7 8

0.03 0.02

Y axis error (m)

0.01 0 -0.01 -0.02 0 1 2 3 4 time (sec) 5 6 7 8

Figure 4.9 Joint Based Comparison End Point Errors

5. Further Work

The following are possible areas where further analysis could be done : * A compensator based on classical techniques that assumes little knowledge of the flexible modes is shown. One possible compensator based on LQR techniques that directly controls the flexible modes through an assumed knowledge of their frequencies and eigenvectors is shown. Many other possible techniques that attempt to control the flexible modes, each based on different assumptions of the level of knowledge of the system properties and uncertainties, can be examined. These include H , direct parameter optimization with multiple plants, linear quadratic worst case optimization (LQW, a quadratic cost function optimization method similar to LQR that defines the optimal state feedback control gain and the coupled worst case disturbance, reference [13]), and various fuzzy and neural techniques. * The control structure as presented is stable for only a limited capture region around the commanded end point. The linearized perturbation of the plant dynamics for end points away from the commanded position is given. The actual size of the stable region for various compensators has not yet been calculated. Compensator design techniques that seek to maximize this region could be employed. * The control structure and compensator designs presented have assumed a single position sensor that tracks the manipulator end point. The advantages of a second, possibly redundant, position sensor have not been examined. The effects of placing the second target at various locations (the elbow joint being an obvious choice) have not been examined. * The plant dynamics model linearization assumed both the existence of no flexible modes (therefore K = 0) and that the nonlinear terms where zero. All of the nonlinear terms in this model formulation took the form of squared joint angular rates. Direct compensation of these rate squared terms is possible to guarantee stability for any magnitude joint rates (not only small rates as assumed for the given controller formulation). A simple direct cancellation of the nonlinear terms through joint angle rate feedback (as typically found in literature on exact linearization on manipulators) could be applied. Alternatives include both a nonlinear feedback that is in effect only

61 for large joint rates and a Lyapunov based approach that defines a simple, low order joint rate feedback that guarantees global stability for all magnitudes of joint angle rate. * How the uncertainties in the parameter values for the computed feedforward rigid body linearization matrix effect the stability of the compensator has not been fully examined. * How the properties of the end point position sensor effect the final performance of the end point position placement has only been given a simplistic treatment. * The application of a joint angle rate feedback scheme which, because of low quality joint angle position sensors is not meant to improve end point placement performance, but rather is meant to guarantee stability of modes which may be unobservable to the end point position sensor has not been examined.

6. Conclusions

The difficulties with the use of an end point position sensor on mechanical manipulators with non-cartesian geometries is examined with focus on the sensor-actuator noncolocation and the nonlinear output function. The various forms of the dynamics linearization through input transformation functions, both exact and approximate, are given. A simple (and therefore computationally inexpensive) form that linearizes only the rigid body modes is presented and the full form of the matrix for a two link planar manipulator is given. It has been shown that simple application of the linearization matrix to the plant inputs with joint angle feedback is unstable. A feedforward scheme that is stable for a limited region around the input command is presented. An end point position sensor has been integrated into the two link planar flexible manipulator test bed in the Control Systems Laboratory at the University of Washington. The geometric compensation equations for the end point sensor are given and the parameter calibration techniques are presented to give the transformation from sensor to table top coordinates. The data quality and performance of the chosen sensor are reported. Use of the end point sensor in the existing control hardware is described. A simple set of linear controllers is presented. They were implemented on the two link flexible manipulator hardware and shown to have reasonable performance for a large range of nominal joint angles. Instability is exhibited to exist near the manipulator geometric configuration Jacobian singular points, as was expected from the linearization mathematics.

REFERENCES

1. Initial Experiments on End-Point Control of a Flexible One-Link Robot, Robert H. Cannon Jr., Eric Schmitz, The International Journal of Robotics Research. vol. 3, no. 3, Fall 1984, pg 62-75. 2. The UWCSL Two-Link Manipulator Facility: System Description, Steve Evers. Internal Report, University of Washington, Seattle, WA. 1994 3. Development of a Mathematical Model for the Control of Flexible Electromechanical Systems test Bed, V. Lertpiriyasuwat, M.S. Thesis, University of Washington, Seattle, WA. 1994. 4. Experiments in Modeling and End-Point Control of Two-Link Flexible Manipulators, Celia Oakley PhD Dissertation Stanford University, 1991. 5. On Manipulator Control by Exact Linearization, Kenneth Kreutz, IEEE Transactions on Automatic Control, vol. 34, no. 7, July 1989, pg 763-767 6. Digital Control of Dynamic Systems 2nd Ed., Franklin, Powell, and Workman. 1980,1992. pg 422-439. 7. The DynaSight Sensor : Developer Manual ver 1.1, Origin Instruments Corporation, 854 Greenview Drive, Grand Prairie, TX. 1993 8. Linear Robust Control, Michael Green and David Limebeer. 1995. pg. 23-26. 9. Discrete Time Noncolocated Control of Flexible Mechanical Systems Using Time Delay, M.S.Wang and B.Yang, Journal of Dynamic Systems, Measurement, and Control. vol. 116, June 1994, pg 216-222. 10. Experimental Evaluation of Robotic Reduced-Order Hybrid Position/Force Control on a Two-Link Flexible Manipulator, D.Bossert, U.Lee, J.Vagners, 1996 IEEE International Conference on Robotics and Automation. 11. System Build Users Guide, ver 3.0, Integrated Systems Inc. 3260 Jay St, Santa Clara, CA 95054, 1992. 12. SANDY. A Design Tool for Linear Multivariable Optimal Control, Users Guide, A.J. Controls Inc. 25825 104th Ave. SE, Suite 442, Kent, WA. 98031. 1993 13. Personal conversations with A.E. Byson, PhD.

Appendix 1. Parameter Values

Parameter m 1 , m2

Description Mass of the links in the rigid body formulation

Value 3.6745 kg, 1.0184 kg

L 1 , L2

Length of the links including both flexible and rigid sections

0.6519 m, 0.6019 m

a 1 , a2

Distance from the joint to the center of mass of the links in the rigid body formulation

0.3365 m, 0.2606 m

I 1 , I2

Inertia of the links about the center of mass in the rigid body formulation

0.370 kgm2, 0.081 kgm2

1 , 2 x, y i fDYNA k x , ky z0 x, y d 1 , d2

Encoder angular quantization End point position sensor quantization Flexible mode frequencies maximum sensor update rate Sensor origin in table top coordinates Sensor distance from table top Sensor rotational misalignments Inboard distances to flexible link

0.0015 r, 0.0015 r 0.05 mm, 0.05 mm 1.8, 3.4, 4.5, 8.0 hz 65.8 hz 0.762 m, 0.653 m 0.775 m 0.0029 r, 0.0129 r 0.104 m, 0.0195 m

Appendix 2. Code Listings

2.1. AC100-C30 Serial Line Interface

The AC100 decoding software consists of two pieces; the serial driver and the serial decoding/encoding routines. Each piece exists as an object module in the /AC100C30 directory on the AC100 PC host. References to these modules are made in the AC100C30.LNK file. They are automatically linked during compile time, so no additional code needs to be included. The serial driver is a set of several software routines that define the timing and structure of the serial I/O. It is loaded as an object library (IPSERIAL.OBJ) supplied by ISI. The decoding/encoding routines are routines written locally, customized to the DynaSight interface. They transform the stream of received bytes into formatted floating point outputs, based on the known DynaSight packet definitions. They also transform the model floating point outputs into a byte stream to send to the sensor. Versions of these routines were developed for the DynaSight sensor and are located in \AC100C30\IP_DYNA.OBJ (default object code) and \AC100C30\DYNA.DIR (source code directory). The decoding/encoding software consists of three functions which are called by the serial interface driver module. The usr_SERIAL_out and usr_sample_SERIAL_in routines are called for input and output transfers of serial data. Each function is implemented to two different formats, depending on the intended use of the project (see below) The get_SERIAL_paramters routine defines the serial line communication protocol and also initializes the user structure. BAUD = 19200 DATA BITS = 8 PARITY = NONE STOP BITS = 1 CLOCK MULT = 16 or 32 BUFFER SIZE = 200

66 2.1.1. De-packetizing Code

This is the normal use of the serial line decoding/encoding routines. It is called during run time to decode the DynaSight position packets into floating point engineering format. The routines are : usr_SERIAL_parameters: standard protocol definitions usr_SERIAL_out : is not called or used, since there are no outputs to the sensor user_sample_SERIAL_in : decodes the DynaSight data packets into a vector of floating point controller inputs. Any number of input bytes can be decoded during processing. When the end of an eight byte packet is reached, the data is transformed into an integer position. The latest complete integer position is converted to a floating point measurement in millimeters and returned to the controller.

/*===================================================================== IP_DRIVER.C FUNCTION : Required top level serial I/O drivers for the AC100 DSP based real time controller. Data format translation between serial device asyncronous byte stream format and the System Build syncronous float/int vector format. (AC100 object-code performs the buffered serial I/O interupt functions.) This version is for the DynaSight IR position sensor. get_SERIAL_parameters Function sets the asynchronous communication parameters for the IP-SERIAL module. user_poll_SERIAL_out Creates the transmit byte array and calling function write_serial which transmits the byte array across the serial channel. user_sample_SERIAL_in Function is called every sampling interval. Fills the floatingpoint vector which is used as input to the model for the current sampling interval.

67
INPUTS : This version supports no inputs. IP_SETUP routine. OUTPUTS : [1] [2] [3] target X position in millimeters. (sensor frame) target Y position in millimeters. (sensor frame) target Z position in millimeters. (sensor frame) See the DynaSight Developers manual for a deffinition of the sensor frame coordinate system. [4] [5] [6] data quality flag. -1=old, 0=good, 1=supect, 2=bad. packet syncronization flag. 0=sync'ed, 1=error. packet count (decodable packets only). To configure the sensor, see the

HISTORY : 18 Nov 93 06 MAR 95

ISI Original DB.Rathbun

Adapted to DynaSight format

====================================================================*/ /* DynaSight/AC100 includes */ #include "dynadriver.h" #define NULL 0 /* semaphores and serial parameters for each physical channel */ private struct { boolean allSent; boolean broken; unsigned baud; } line_state[2]; /* definition of user memory structure for each channel */ struct user_type { int update_interval; int update_count; int last_pktCount; struct dyNative0 dyna_struct; }; /*-------------------------------------------------------------------*/ public void get_SERIAL_parameters( unsigned int hardware_channel, volatile struct user_param *device_param, volatile struct ring_buffer_param *rec_buffer, IOdevice *device ) { struct user_type *user_ptr; serial_param_type *serptr;

68
int inx; serptr = device->parameters; /* Initialize the user data structure memory/values */ if (SERIAL_USER_PTR == NULL) { SERIAL_USER_PTR = (struct user_type *)malloc(sizeof(struct user_type)); user_ptr = SERIAL_USER_PTR; user_ptr->update_interval = 50; user_ptr->update_count = 0; user_ptr->last_pktCount = 0; user_ptr->dyna_struct.pktCount = 0; user_ptr->dyna_struct.pktState = 0; for ( inx=0; inx<PKT_MAX_LENGTH; inx++ ) user_ptr->dyna_struct.pktbuf[inx] = 0; user_ptr->dyna_struct.x = 0; user_ptr->dyna_struct.y = 0; user_ptr->dyna_struct.z = 0; user_ptr->dyna_struct.config_flag = 1; user_ptr->dyna_struct.buffer_flag = 1; } /* Set the serial I/O communication parameters */ if ((hardware_channel == chanA) | (hardware_channel == chanB)) { /* set port parity = NONE, EVEN, ODD */ device_param->parity = NONE; /* set port baud = 300, 9600, 19200, ... */ device_param->baud_rate = 19200; /* set port stop bits = ONE, TWO, ONE_AND_HALF */ device_param->stop_bits = ONE; /* set port transmit bit size = 5, 6, 7, 8 */ device_param->transmit_data_size = 8; /* set port recieve bit size = 5, 6, 7, 8 */ device_param->receive_data_size = 8; /* set clock multiplier = 1, 16, 32, 64 */ device_param->clock_multiplier = 16; /* set size for receive ring buffer. Should be set to a number greater than the maximum expected recives bytes in one sample period. */ rec_buffer->buffer_size = 2000; } else { printx("INVALID CHANNEL\n"); }

69
}

/*-------------------------------------------------------------------Function : user_SERIAL_out Abstract : user-defined serial output function. scheduled output event. Inputs : sysptr attributes model_float[] ser_channel Returns : error status Function called as

user-transparent pointer to system SystemBuild output vector serial channel

--------------------------------------------------------------------*/ public RetCode user_SERIAL_out(IOdevice *device, float model_float[], u_int ser_channel ) { Byte cbuffer[3]; u_int i; struct user_type *local_ptr; /* NOT USED. See the file=[ip_setup.c] / project=[ac_setup] for configuration of the sensor. */ return OK; }

/*--------------------------------------------------------------------Function : user_sample_SERIAL_in Abstract : Specifies user-defined serial input function. called as a scheduled input event. Inputs Outputs Returns : sysptr ser_channel : *model_float : error status Function

user-transparent pointer to system attributes serial channel pass back floating pt vector see file header for indexed description

---------------------------------------------------------------------*/ public RetCode user_sample_SERIAL_in( IOdevice *device, float model_float[], u_int ser_channel ) { u_int i, num;

70
unsigned char buf[80]; struct user_type *local_ptr; int sync_error; /* set user pointer to buffer allocated by get_parameters */ local_ptr = SERIAL_USER_PTR; /* If this is the first time through, reset the buffers */ if ( local_ptr->dyna_struct.buffer_flag ) { if ( ser_channel == chanA ) { reset_buffer( device->parameters ); local_ptr->dyna_struct.buffer_flag = 0; } } /* Only do a real process function for channel A */ if ( ser_channel == chanA ) { /* Get the number of new data bytes in the buffer */ num = numbytes_in_buffer( device->parameters ); /* Only process data if there are at least [?] new bytes */ if ( num != 0 ) { /* set the no_data count to zero */ local_ptr->update_count = 0; /* read in all the data from the buffer */ read_serial( device->parameters, num, buf ); /* sync to byte field and convert latest good data to float */ sync_error = decodeNative0( buf, num, &(local_ptr->dyna_struct) ); /* put the output data into the model_float[0] = 0.05 * (float) model_float[1] = 0.05 * (float) model_float[2] = 0.05 * (float) System build output vector */ local_ptr->dyna_struct.x; local_ptr->dyna_struct.y; local_ptr->dyna_struct.z;

/* The data quality depends on the sensor tracking status */ if ( local_ptr->dyna_struct.status == STS_TRACK ) { model_float[3] = 0.0e0; } else if ( local_ptr->dyna_struct.status == STS_CAUTION ) { model_float[3] = 1.0e0; } else { model_float[3] = 2.0e0; } if ( sync_error ) { model_float[4] = 1.0e0; /* if sync error, assume bad data */ model_float[3] = 2.0e0; } else {

71
model_float[4] = 0.0e0; } if ( local_ptr->dyna_struct.pktCount == local_ptr->last_pktCount ) { model_float[3] = -1.0e0; } local_ptr->last_pktCount = local_ptr->dyna_struct.pktCount; model_float[5] = (float) (local_ptr->dyna_struct.pktCount); } else { /* put the latest data into the output vector and flag old */ model_float[0] = 0.05 * (float) local_ptr->dyna_struct.x; model_float[1] = 0.05 * (float) local_ptr->dyna_struct.y; model_float[2] = 0.05 * (float) local_ptr->dyna_struct.z; model_float[3] = -1.0e0; model_float[4] = 0.0e0; model_float[5] = (float) (local_ptr->dyna_struct.pktCount); /* count number of scheduler ticks that go by without recieving all 4 bytes. If too long, signal a line break and reset the buffer. */ local_ptr->update_count++; if ( local_ptr->update_interval < local_ptr->update_count ) { IOerr( SERIAL_receive_interrupt_error, ser_channel ); reset_buffer( device->parameters ); local_ptr->update_count = 0; } } } else { /* This is channel B so reset the buffer so it won't overflow */ reset_buffer( device->parameters ); printx("Reseting Buffer B \n"); } return OK; } /*************************************************************** END OF FILE ****************************************************************/

/*==================================================================== decode.h

72
FUNCTION : Include file for decode.c. Definitions and function prototypes for a decoder for the DynaSight NATIVE0 data format for 3-D position measurements. HISTORY : xx xxx 92 Original, 06 MAR 95 DB.Rathbun Origin Instruments Corporation modified for single data structure

********************************************************************/ /* AC100 #include #include #include #include #include #include #include #include #include includes */ <stddef.h> <stdlib.h> "sa_types.h" "types.h" "ioerrs.h" "errcodes.h" "iodriver.h" "ipserial.h" "printx.h"

#ifndef DECODE_H #define DECODE_H #ifndef #define #endif #ifndef #define #endif typedef typedef typedef typedef #define #define #define #define FALSE FALSE 0 TRUE TRUE (!FALSE) int BOOL; unsigned char UCHAR; unsigned short USHORT; unsigned long ULONG; PKT_MAX_LENGTH PKT_LENGTH PKT_MARKER MARKER_MASK 8 8 0x80 0xF0 NATIVE0 packet */ sensor operating mode */ reserved field */ debounced sync indicator, 0=>open */ dummy field for 16-bit alignment */ base 2 exponent for x, y, z */ data format indicator, 0=>NATIVE0 */ dummy field for 16-bit alignment */ x measurement */ y measurement */ z measurement */ count of total 'ok' packets */

/* structure for raw data from struct dyNative0 { unsigned status : 2; /* unsigned btnr : 1; /* unsigned sync : 1; /* unsigned dummy0 : 4; /* unsigned exp : 2; /* unsigned fmt : 2; /* unsigned dummy1 : 4; /* long x; /* long y; /* long z; /* int pktCount; /*

73
int pktState; /* buffer char pktbuf[PKT_MAX_LENGTH]; /* int config_flag; /* flag int buffer_flag; /* flag }; /* Defines for decode #define STS_SEARCH #define STS_COAST #define STS_CAUTION #define STS_TRACK of ".status" field in struct dyNative0: */ 0 1 2 3 pointer used to to mark to mark for state machine */ sync to data format */ initialization */ initialization */

/*** Function Prototypes */ int decodeNative0( unsigned char *cp, u_int count, struct dyNative0 *tgt ); #endif

/*===================================================================== decode.c FUNCTION : Packet decoder for DynaSight NATIVE0 data format. Source for a simple state machine to decode the DynaSight NATIVE0 data format for 3-D position measurements. HISTORY : xx xxx 92 06 MAR 95 Original, Origin Instruments Corporation DB.Rathbun convert to a single data structure passed control of static structure to caller updated bit operations for a 32 bit arch.

=====================================================================*/ #include "dynadriver.h" /******************************************************************* * decodeNative0() * * Decode a segmented stream of input characters and return the * most recent NATIVE0 packet that has been completely and * successfully decoded. This function decodes but ignores * packets for which the "fmt" field is non-zero. * * Input Parameters: * cp - pointer to a string of characters to be decoded * count - the number of characters in the string * tgt - pointer to a user-allocated struct dyNative0 that * will receive the most recently decoded packet.

74
* * Return Value: * TRUE if a packet synchronization error was detected. * FALSE if no error was detected. * *******************************************************************/ int decodeNative0( unsigned char *cp, u_int count, struct dyNative0 *tgt ) { register temp; u_int tmp_count; unsigned char newChar; BOOL isMarker; int pktSyncError; /* If there are more than 16 bytes in the buffer, ignore all but the last set */ if ( count > 16 ) { tmp_count = count - ( ( count/8 - 2) * 8); for ( ; count>tmp_count; count--) cp++; } pktSyncError = FALSE; /* Process all the new input characters */ for(; count>0; count--) { newChar = *cp++; /* Is the current character a DynaSight native0 maker byte? */ isMarker = ( (newChar & MARKER_MASK) == PKT_MARKER ); /* State machine of native0 format with pointer tgt.pktState */ switch( tgt->pktState ) { case 0: if( isMarker ) { /* expected marker byte */ tgt->pktbuf[tgt->pktState++] = newChar; } else { /* must be a loss of synchonization */ pktSyncError = TRUE; /* leave pktState at 0 */; } break; case 1: if( isMarker ) { /* expected marker byte */ tgt->pktbuf[tgt->pktState++] = newChar; } else {

75
/* must be loss of synchronization, so reset */ tgt->pktState = 0; pktSyncError = TRUE; } break; case 2: if( isMarker ) { /* unexpected marker, so shift back */ tgt->pktbuf[0] = tgt->pktbuf[1]; tgt->pktbuf[1] = newChar; pktSyncError = TRUE; /* leave pktState at 2 */; } else { /* save the character */ tgt->pktbuf[tgt->pktState++] = newChar; } break; case 3: /* save the character */ tgt->pktbuf[tgt->pktState++] = newChar; break; case 4: if( isMarker ) { /* unexpected marker, so reset state machine */ tgt->pktbuf[0] = newChar; tgt->pktState = 1; pktSyncError = TRUE; } else { /* save the character */ tgt->pktbuf[tgt->pktState++] = newChar; } break; case 5: /* save the character */ tgt->pktbuf[tgt->pktState++] = newChar; break; case 6: if( isMarker ) { /* unexpected marker, so reset state machine */ tgt->pktbuf[0] = newChar; tgt->pktState = 1; pktSyncError = TRUE; } else { /* save the character */ tgt->pktbuf[tgt->pktState++] = newChar; } break;

76
case 7: /* save the character */ tgt->pktbuf[tgt->pktState] = newChar; /* ----- 8-byte packet is complete, so de-code it ----- */ /* increment packet count (allow for rollover) */ (tgt->pktCount)++; /* check the message type field to make sure it is native0 type */ if( ( (temp=tgt->pktbuf[0]) & 0x0C ) == 0) { /* update data structure with new flag data */ tgt->exp = temp; tgt->fmt = temp>>2; /* =0 */ tgt->status = temp = tgt->pktbuf[1]; tgt->btnr = (temp>>2); tgt->sync = (temp>>3); /* update position data - put the two input bytes into the 32 bit word - do a 16 to 32 bit sign extension - apply the base 2 exponent */ tgt->x = ( tgt->pktbuf[2]<<8 ) | tgt->pktbuf[3]; if( tgt->x & 0x00008000 ) tgt->x = tgt->x | 0xffff0000; tgt->x = tgt->x << tgt->exp; tgt->y = ( tgt->pktbuf[4]<<8 ) | tgt->pktbuf[5]; if( tgt->y & 0x00008000 ) tgt->y = tgt->y | 0xffff0000; tgt->y = tgt->y << tgt->exp; tgt->z = ( tgt->pktbuf[6]<<8 ) | tgt->pktbuf[7]; if( tgt->z & 0x00008000 ) tgt->z = tgt->z | 0xffff0000; tgt->z = tgt->z << tgt->exp; } else { pktSyncError = TRUE; } tgt->pktState = 0; break; } } return pktSyncError; }

77 2.1.2. Configuration Code

The DynaSight needs to be configured to its internal 'V' mode (steroSync60) to get acceptable dynamic behavior. An empty MatrixX/SystemBuild model was created in the AC_SETUP folder in the \AC100C30\PROJECTS directory. It was compiled with the configuration code only (i.e. the other version was temporarily removed from the default link list). A BAT file was setup (DYNA_SETUP) that runs this project directly from the PC command line. A CTRL-C must be hit to halt the executable. get_SERIAL_parameters : defines the communication parameters usr_SERIAL_out : constructs an output command packet and sends it to the sensor. A flag is set so that initialization only happens once. The command packet is : 0x03 ascii crtl-c, attention signal 0x56 ascii V, stereoSync60 mode command 0x00 end of command byte user_sample_SERIAL_in : is not called or used, since no sensor inputs are required.

/*==================================================================== IP_DRIVER.C FUNCTION : Required top level serial I/O drivers for the AC100 DSP based real time controller. Data format translation between serial device asyncronous byte stream format and the System Build syncronous float/int vector format. (AC100 object-code performs the buffered serial I/O interupt functions.) This version is for the DynaSight IR position sensor. get_SERIAL_parameters Function sets the asynchronous communication parameters for the IP-SERIAL module. user_poll_SERIAL_out Creates the transmit byte array and calling function write_serial which transmits the byte array across the serial channel.

78
user_sample_SERIAL_in Function is called every sampling interval. Fills the floatingpoint vector which is used as input to the model for the current sampling interval. INPUTS : This project requires a single input to activate the call to the user_SERIAL_out routine. Its value is unimportant. OUTPUTS : This version is only to configure the sensor, there are no outputs. See the normal ip_driver.c code for sensor reading. HISTORY : 18 Nov 93 06 MAR 95 ISI Original DB.Rathbun

Adapted to DynaSight format

====================================================================*/ /* AC100 #include #include #include #include #include #include #include #include #include includes */ <stddef.h> <stdlib.h> "sa_types.h" "types.h" "ioerrs.h" "errcodes.h" "iodriver.h" "ipserial.h" "printx.h"

/* DynaSight includes */ #include "decode.h" #define NULL 0 /* semaphores and serial parameters for each physical channel */ private struct { boolean allSent; boolean broken; unsigned baud; } line_state[2]; /* definition of user memory structure for each channel */ struct user_type { int update_interval; int update_count; struct dyNative0 dyna_struct; }; /*------------------------------------------------------------------*/ public void get_SERIAL_parameters( unsigned int hardware_channel, volatile struct user_param *device_param, volatile struct ring_buffer_param *rec_buffer, IOdevice *device ) { struct user_type *user_ptr;

79
serial_param_type *serptr; int inx; serptr = device->parameters; /* Initialize all the user structure memory/parameters */ if (SERIAL_USER_PTR == NULL) { SERIAL_USER_PTR = (struct user_type *)malloc(sizeof(struct user_type)); user_ptr = SERIAL_USER_PTR; user_ptr->update_interval = 50; user_ptr->update_count = 0; user_ptr->dyna_struct.pktCount = 0; user_ptr->dyna_struct.pktState = 0; user_ptr->dyna_struct.x = 0; user_ptr->dyna_struct.y = 0; user_ptr->dyna_struct.z = 0; user_ptr->dyna_struct.config_flag = 1; user_ptr->dyna_struct.buffer_flag = 1; } /* Set the serial I/O communication parameters */ if ((hardware_channel == chanA) | (hardware_channel == chanB)) { /* set port parity = NONE, EVEN, ODD */ device_param->parity = NONE; /* set port baud = 300, 9600, 19200, ... */ device_param->baud_rate = 19200; /* set port stop bits = ONE, TWO, ONE_AND_HALF */ device_param->stop_bits = ONE; /* set port transmit bit size = 5, 6, 7, 8 */ device_param->transmit_data_size = 8; /* set port recieve bit size = 5, 6, 7, 8 */ device_param->receive_data_size = 8; /* set clock multiplier = 1, 16, 32, 64 */ device_param->clock_multiplier = 16; /* set size for receive ring buffer. Should be set to a number greater than the maximum expected recives bytes in one sample period. */ rec_buffer->buffer_size = 200; } else { printx("INVALID CHANNEL\n"); } }

80
/*-------------------------------------------------------------------Function : user_SERIAL_out Abstract : user-defined serial output function. scheduled output event. Inputs : sysptr attributes model_float[] ser_channel Returns : error status Function called as

user-transparent pointer to system SystemBuild output vector serial channel

--------------------------------------------------------------------*/ public RetCode user_SERIAL_out(IOdevice *device, float model_float[], u_int ser_channel ) { Byte cbuffer[3]; u_int i; struct user_type *local_ptr; /* set user pointer to buffer allocated by get_parameters */ local_ptr = SERIAL_USER_PTR; /* Define the DynaSight start-stop codes */ cbuffer[0] = (Byte) 0x03; cbuffer[2] = (Byte) 0x00; /* Only process channel A outputs */ if ( ser_channel == chanA ) { /* Only output if current ouput buffer is empty */ if (numbytes_in_buffer(device->parameters) == 0) { /* Only do this once */ if ( local_ptr->dyna_struct.config_flag ) { /* --- Set the DynaSight operating mode command --SteroSync60 = 0x56 -------------------------------------------------*/ cbuffer[1] =(Byte) 0x56; /* Send the data */ write_serial( device->parameters, cbuffer, 3 ); /* set the already done flag */ local_ptr->dyna_struct.config_flag = 0; } } } return OK;

81
}

/*-------------------------------------------------------------------Function : user_sample_SERIAL_in Abstract : Specifies user-defined serial input function. called as a scheduled input event. Inputs Outputs Returns : sysptr ser_channel Function

user-transparent pointer to system attributes serial channel pass back floating pt vector

: *model_float : error status

--------------------------------------------------------------------*/ public RetCode user_sample_SERIAL_in( IOdevice *device, float model_float[], u_int ser_channel ) { u_int i, num; unsigned char buf[80]; struct user_type *local_ptr; int sync_error; /* set user pointer to buffer allocated by get_parameters */ local_ptr = SERIAL_USER_PTR; /* If this is the first time through, reset the buffers */ if ( local_ptr->dyna_struct.buffer_flag ) { if ( ser_channel == chanA ) { reset_buffer( device->parameters ); local_ptr->dyna_struct.buffer_flag = 0; } } return OK; } /*************************************************************** END OF FILE ****************************************************************/

82 2.2. Slew Commander

/*====================================================================== ep_slew_comm.c System Build format subroutine source code to implement the slew commander for the two link flexible manipulator. Inputs : [ 0] [ 1] [ 2] [ 3] [ 4] [ 5] [ 6] [ 7] Outputs : [ 0] [ 1] [ 2] [ 3] [ 4] [ 5] States : none Parameters : [ 0] vmax [ 1] amax Notes : none History : 18 Nov 95 D.Rathbun created maximum rate constraint maximum acceleration constraint Xpo Ypo Xpf Ypf Xvo Yvo tinit r_init XepComm YepComm XepRate YepRate XepAccel YepAccel X axis initial position Y axis initial position X axis final position Y axis final position X axis initial velocity Y axis initial velocity slew start time re-initialization flag 1=init, 0=no x y x y x y axis axis axis axis axis axis tip tip tip tip tip tip position command (m) position command (m) rate command (m/s) rate command (m/s) accel command (m/s^2) accel command (m/s^2)

======================================================================*/ /* SystemBuild Includes */ #include "sa_sys.h" /*Defines the platform*/ #include "sa_types.h" /*Defines the structure STATUS_RECORD */ #include "sa_user.h" /*Include extern declarations of your UCBs*/ /* ----- System defines ----------------- */ #include <math.h> /* ----- Local defines and includes ----- */ #include "slew_comm.h" #include "tip_pos.h"

83
/* ----- Local Static Variables ----- */ /* NOTE: because of the use of statics, this code is not re-entrant. ** therefore, you can only use it once in any simulation. A fix ** is only posible in the MatrixX/SystemBuild calling structure. */ static struct SLEW_PARAM param_s; static struct TIP_POSITION output_s;

void ep_slew_comm( info, RT_STATUS_RECORD RT_DURATION RT_FLOAT RT_INTEGER {

t, u,nu, x,xdot,nx, y,ny, rpar,ipar ) *info; t; u[],y[],x[],xdot[],rpar[]; nu,nx,ny,ipar[];

struct SLEW_PARAM *param; struct TIP_POSITION *output; param = &param_s; output = &output_s; if ( info->INIT ) { info->ERROR = 0; } /* initialization */

if ( info->STATES ) { info->ERROR = 0; }

/* compute state derivatives */

if ( info->OUTPUTS ) {

/* compute output update */

/* Do a slew point update */ if ( fabs(u[7] - 1.0e0) < 1e-5 ) { /* Reinitialize the param->inputs.xpo param->inputs.ypo param->inputs.xpf param->inputs.ypf param->inputs.xvo param->inputs.yvo param->inputs.tinit param->limits.vmax param->limits.amax slew parameters */ = u[0]; = u[1]; = u[2]; = u[3]; = u[4]; = u[5]; = u[6]; = rpar[0]; = rpar[1];

/* Set up the slew */ slew_set( param ); }

84
/* Get the new slew output */ slew_comm( param, (RT_FLOAT) t, output ); /* Set y[0] = y[1] = y[2] = y[3] = y[4] = y[5] = the outputs */ output->x_pos; output->y_pos; output->x_rate; output->y_rate; output->x_accel; output->y_accel;

info->ERROR = 0; } else { info->ERROR = 1; } return; } /* ========== End of File ============================================*/

/*--------------------------------------------------------------------slew_comm.h Data types/defines for slew command generation. NOTES: none HISTORY: 12 Oct 95 13 Nov 95 18 Nov 95 D.Rathbun D.Rathbun D.RAthbun created updated for real 2nd order slews altered for MatrixX real time data types

---------------------------------------------------------------------*/ /* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Slew Data Types */ struct SLEW_PARAM { struct { RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT xpo; ypo; xvo; yvo; xpf; ypf; tinit; /* /* /* /* /* /* /* initial initial initial initial final x final y initial x axis position */ y axis position */ x axis velocity */ y axis velocity */ axis position */ axis position */ slew time */

85
} inputs; struct { RT_FLOAT RT_FLOAT } limits; struct { RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT } data; RT_INTEGER }; /* Function Prototypes */ void slew_set( ); void slew_comm( ); /*--------------------------------------------------------------------slew_comm.c Generates an optimmum two dimensional second order slew command between two points under rate and acceleration constraints. INPUTS : SLEW_PARAM INPUTS xpo ypo xvo yvo xpf ypf tinit LIMITS vmax amax tnow vmax; amax; /* maximum allowable single axis rate */ /* maximum allowable acceleration */

t1,t2,t3; T0,T1,T2,T3; xa1, xa2; ya1, ya2; xp1, xv1; yp1, yv1; xp2, xv2; yp2, yv2; setup;

/* /* /* /* /* /* /* /*

slew phase durations */ slew phase end times */ x axis accelerations */ y axis accelerations */ x axis point 1 states */ y axis point 1 states */ x axis point 2 states */ y axis point 2 states */

initial initial initial initial final x final y initial

x axis position y axis position x axis rate x axis rate axis position axis position slew time

maximum allowable single axis rate maximum allowable single axis acceleration current time

OUTPUTS : TIP_POSITION x_pos current y_pos current y x_rate current x y_rate current y x_accel current x

x axis position command axis position command axis rate command axis rate command axis acceleration command

86
x_accel current x axis acceleration command

NOTES : * the slew_set function should be called once before any calls to the slew generator (or else you will get 0 output). * the slew generator will back correct (assuming constant velocity) for times (tnow) less than the slew start time (tinit). * the slew generator does not pick the most constrained axis correctly. The accelerations for the other axis are not garanteed to be within the acceleration limit. HISTORY : 12 Oct 13 Nov 18 Nov 10 Dec 95 95 95 95 D.Rathbun D.Rathbun D.Rathbun D.Rathbun created updated updated updated updated

for for for for

real 2nd order slews MatriX real time data types a initialization check non-ANSI compliant compilers

----------------------------------------------------------------------*/ /* standard includes */ #include <math.h> /* local includes */ #include "tip_pos.h" #include "slew_comm.h" void slew_comm( param, tnow, output ) struct SLEW_PARAM *param; RT_FLOAT tnow; struct TIP_POSITION *output; { RT_FLOAT del_t; if ( param->setup == 0 ) { output->x_pos = 0.0e0; output->y_pos = 0.0e0; output->x_rate = 0.0e0; output->y_rate = 0.0e0; output->x_accel = 0.0e0; output->y_accel = 0.0e0; } else if ( tnow < param->data.T0 ) { del_t = tnow - param->data.T0; output->x_pos = param->inputs.xpo + param->inputs.xvo*del_t; output->y_pos = param->inputs.ypo + param->inputs.yvo*del_t; output->x_rate = param->inputs.xvo; output->y_rate = param->inputs.yvo; output->x_accel = 0.0e0; output->y_accel = 0.0e0; } else if ( tnow < param->data.T1 ) { del_t = tnow - param->data.T0; output->x_pos = param->inputs.xpo + param->inputs.xvo*del_t + 0.5e0 * param->data.xa1 * del_t * del_t;

87
= param->inputs.ypo + param->inputs.yvo*del_t + 0.5e0 * param->data.ya1 * del_t * del_t; output->x_rate = param->inputs.xvo + param->data.xa1 * del_t; output->y_rate = param->inputs.yvo + param->data.ya1 * del_t; output->x_accel = param->data.xa1; output->y_accel = param->data.ya1; } else if ( tnow < param->data.T2 ) { del_t = tnow - param->data.T1; output->x_pos = param->data.xp1 + param->data.xv1*del_t; output->y_pos = param->data.yp1 + param->data.yv1*del_t; output->x_rate = param->data.xv1; output->y_rate = param->data.yv1; output->x_accel = 0.0e0; output->y_accel = 0.0e0; } else if ( tnow < param->data.T3 ) { del_t = tnow - param->data.T2; output->x_pos = param->data.xp2 + param->data.xv2*del_t + 0.5e0 * param->data.xa2 * del_t * del_t; output->y_pos = param->data.yp2 + param->data.yv2*del_t + 0.5e0 * param->data.ya2 * del_t * del_t; output->x_rate = param->data.xv2 + param->data.xa2 * del_t; output->y_rate = param->data.yv2 + param->data.ya2 * del_t; output->x_accel = param->data.xa2; output->y_accel = param->data.ya2; } else { output->x_pos = param->inputs.xpf; output->y_pos = param->inputs.ypf; output->x_rate = 0.0e0; output->y_rate = 0.0e0; output->x_accel = 0.0e0; output->y_accel = 0.0e0; } return; } void slew_set( param ) struct SLEW_PARAM *param; { RT_FLOAT t1,t2,t3; RT_FLOAT a1,a2,a3,a4; RT_FLOAT delx, dely; RT_FLOAT tmp; RT_FLOAT vx, ax, vy, ay; RT_FLOAT x_check, y_check, check; RT_INTEGER test; RT_FLOAT del, vo, vm, am; RT_FLOAT del_off, voff; char slew_type; /* flag data as being initialized */ ++(param->setup); output->y_pos

88
/* Compute some often used quantities */ delx = param->inputs.xpf - param->inputs.xpo; dely = param->inputs.ypf - param->inputs.ypo; /* Check for correct slew signs */ tmp = 2.0e0 * param->limits.amax * delx - param->inputs.xvo * fabs( param->inputs.xvo); if ( tmp >= 0.0e0 ) { vx = param->limits.vmax; ax = param->limits.amax; } else { vx = - param->limits.vmax; ax = - param->limits.amax; } tmp = 2.0e0 * param->limits.amax * dely - param->inputs.yvo * fabs( param->inputs.yvo); if ( tmp >= 0.0e0 ) { vy = param->limits.vmax; ay = param->limits.amax; } else { vy = - param->limits.vmax; ay = - param->limits.amax; } /* Check for an X constrained or Y constrained slew Convert from x/y to on/off */ x_check = sqrt( param->inputs.xvo * param->inputs.xvo / 2.0e0 + ax * delx ); y_check = sqrt( param->inputs.yvo * param->inputs.yvo / 2.0e0 + ay * dely ); /* Note : this test is not the correct one. It works, but can result in off axis accelerations that exceed the given limits. */ test = x_check > y_check; if ( test ) { slew_type = 'X'; check = x_check; del = delx; vo = param->inputs.xvo; am = ax; vm = vx; del_off = dely; voff = param->inputs.yvo; } else { slew_type = 'Y'; check = y_check; del = dely; vo = param->inputs.yvo; am = ay; vm = vy; del_off = delx; voff = param->inputs.xvo;

89
} /* Get times based on the need for a required coast phase */ if ( check < fabs(vm) ) { /* Dont't need a coast pahse */ t1 = sqrt(vo*vo/2/am/am + del/am) - vo/am; t2 = 0; t3 = sqrt(vo*vo/2/am/am + del/am); a1 = am; a2 = -am; } else { /* Need a coast phase */ t1 = (vm - vo) / am; t2 = del/vm - (2*vm*vm - vo*vo)/(2*vm*am); t3 = vm/am; a1 = am; a2 = -am; /* fix for abs(vo) large */ if ( t1 < 0 ) { t1 = -t1; a1 = -a1; t2 = del/vm - (vo*vo)/(2*vm*am); } } /* Get the off axis slew parameters */ a3 = (2*del_off - voff*(2*t1+2*t2+t3))/t1/(t1+2*t2+t3); a4 = -(2*del_off - voff*(t1))/t3/(t1+2*t2+t3); /* Decode on axis/off axis if ( slew_type == 'X' ) { param->data.xa1 = a1; param->data.xa2 = a2; param->data.ya1 = a3; param->data.ya2 = a4; } else { param->data.xa1 = a3; param->data.xa2 = a4; param->data.ya1 = a1; param->data.ya2 = a2; } /* Convert times param->data.T0 = param->data.T1 = param->data.T2 = param->data.T3 = back to x/y system */

to absolute */ param->inputs.tinit; param->inputs.tinit + t1; param->inputs.tinit + t1 + t2; param->inputs.tinit + t1 + t2 + t3;

param->data.t1 = t1; param->data.t2 = t2; param->data.t3 = t3;

90
/* Fill in the way points */ param->data.xp1 = param->inputs.xpo + param->inputs.xvo*t1 + 0.5e0*param->data.xa1*t1*t1; param->data.xv1 = param->inputs.xvo + param->data.xa1*t1; param->data.xp2 = param->data.xp1 + param->data.xv1*t2; param->data.xv2 = param->data.xv1; param->data.yp1 = + param->data.yv1 = param->data.yp2 = param->data.yv2 = return; } param->inputs.ypo + param->inputs.yvo*t1 0.5e0*param->data.ya1*t1*t1; param->inputs.yvo + param->data.ya1*t1; param->data.yp1 + param->data.yv1*t2; param->data.yv1;

2.3. Linearization Matrix Generation

/*====================================================================== ep_lin_mat.c System Build format subroutine source code to implement the rigid body linearization for the two link flexible manipulator. Inputs : [ 0] [ 1] [ 2] [ 3] Outputs : [ 0] [ 1] States : none Parameters : none Notes : none History : 18 Nov 95 D.Rathbun created Xep Yep TorX TorY TorTh1 TorTh2 cartesian cartesian desired X desired Y X axis end Y axis end axis force axis force point command (m) point command (m) (N) (N)

theta 1 joint torque (Nm) theta 2 joint torque (Nm)

======================================================================*/

91
/* SystemBuild Includes */ #include "sa_sys.h" /*Defines the platform*/ #include "sa_types.h" /*Defines the structure STATUS_RECORD */ #include "sa_user.h" /*Include extern declarations of your UCBs*/ /* ----- System defines ----------------- */ #include <math.h> /* ----#include #include #include Local defines and includes ----- */ "properties.h" "theta_pos.h" "tip_pos.h"

/* ----- Local Static Variables ----- */ /* NOTE: because of the use of statics, this code is not re-entrant. ** therefore, you can only use it once in any simulation. A fix ** is only posible in the MatrixX/SystemBuild calling structure. */ static struct TIP_POSITION ep_s; static struct THETA_POSITION joint_s; static struct MASS_PROPERTIES mass_s;

void ep_lin_mat( info, t, u,nu, x,xdot,nx, y,ny, rpar,ipar ) RT_STATUS_RECORD *info; RT_DURATION t; RT_FLOAT u[],y[],x[],xdot[],rpar[]; RT_INTEGER nu,nx,ny,ipar[]; { struct TIP_POSITION *ep; struct THETA_POSITION *joint; struct MASS_PROPERTIES *mass; RT_FLOAT L1_sq, L2_sq, L12; RT_FLOAT th_right, L_opp2, L_opp; RT_FLOAT cTh1_prime, cTh2_prime; RT_FLOAT s1, c1, s2, c2, s12, c12, det; RT_FLOAT pM1, pM2, pM3; RT_FLOAT lin_matrix[4]; /* Set the structure pointers */ ep = &ep_s; joint = &joint_s; mass = &mass_s; /* initialize to an error condition to be reset if no error */ info->ERROR = 0;

92
/* initialization */ if ( info->INIT ) { /* Fill the parameter structure */ set_mass_properties( mass ); info->ERROR = 0; /* no errors */ } /* compute state derivatives */ if ( info->STATES ) { info->ERROR = 0; } /* compute output updates */ if ( info->OUTPUTS ) { /* Fill the ep->x_pos ep->y_pos ep->x_rate ep->y_rate ep->x_accel ep->y_accel commaned end point based positions */ = u[0]; = u[1]; = 0.0e0; = 0.0e0; = 0.0e0; = 0.0e0;

/* Compute joint angles using the law of Cosines */ x_to_th_pos( ep, mass, joint ); /* Get the linearization matrix */ set_lin_MASS( joint, mass ); set_lin_JACOB( joint, mass ); lin_mat_gen( mass, lin_matrix ); /* Transform the input force = output Torque */ y[0] = lin_matrix[0+0*2]*u[2] + lin_matrix[0+1*2]*u[3]; y[1] = lin_matrix[1+0*2]*u[2] + lin_matrix[1+1*2]*u[3]; info->ERROR = 0; } return; } /* ========== End of File ============================================*/ /*--------------------------------------------------------------------lin_mat_gen.h Data types/defines for rigid body linearization matrix generation NOTES: none

93
HISTORY: 12 Oct 13 Nov 18 Nov 10 Dec

95 95 95 95

D.Rathbun D.Rathbun D.Rathbun D.Rathbun

created broke into individual routines altered for MatrixX real time data types updated for non-ANSI compliant compilers

---------------------------------------------------------------------*/ /* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Function Prototypes */ void lin_mat_gen( ); void set_lin_MASS( ); void set_lin_JACOB( ); void set_lin_J_DOT( ); /*--------------------------------------------------------------------lin_mat_gen.c Computes the rigid body linearization matrix for the two link flexible manipulator. Can be used to transform endpoint forces to the resulting joint torques. INPUTS : MASS_PROPERTIES L1/L2 arm lengths M1/M2 arm mass A1/A2 arm c.m. locations L1/L2 arm inertias (about c.m.) THETA_POSITION th1_pos theta 1 axis position th2_pos theta 2 axis position th1_rate theta 1 axis rate th2_rate theta 2 axis rate OUTPUTS : MASS_PROPERTIES M rigid body dynamic mass matrix M_inv rigid body dynamic mass matrix inverse J arm kinematic Jacobian matrix J_inv arm kinematic Jacobian matrix inverse J_dot arm kinematic Jacobian matrix time derivative linMatrix 2x2 linearization matrix

NOTES : * be carefull with calling order w.r.t end point to joint angle conversion routines ( x_to_th_pos, x_to_th_rate, x_to_th_accel ) HISTORY : 25 Aug 12 Oct 13 Nov 18 Nov 95 95 95 95 D.Rathbun D.Rathbun D.Rathbun D.Rathbun created converted to subroutine format broke into individual routines altered for MatrixX real time data types

94
10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers

---------------------------------------------------------------------*/ /* Standard Includes */ #include <math.h> /* Local #include #include #include includes */ "properties.h" "theta_pos.h" "lin_mat_gen.h"

/* Define the Mass Matrix and Mass Matrix Inverse */ void set_lin_MASS( joint, mass ) struct THETA_POSITION *joint; struct MASS_PROPERTIES *mass; { RT_FLOAT pM1, pM2, pM3; RT_FLOAT c2, det; /* Often used angle sinusoids */ c2 = cos( joint->th2_pos ); /* Mass matrix pM1 = mass->M1 + mass->M2 pM2 = mass->M2 pM3 = mass->M2 parameters * mass->A1 * mass->L1 * mass->A2 * mass->A2 */ * mass->A1 * mass->L1 + mass->I1; * mass->A2 + mass->I2; * mass->L1;

/* Mass matrix */ mass->M[0][0] = pM1 + pM2 + 2.0 * pM3 * c2; mass->M[0][1] = pM2 + pM3 * c2; mass->M[1][0] = pM2 + pM3 * c2; mass->M[1][1] = pM2; /* Mass Matrix Inverse (always exists) */ det = mass->M[0][0]*mass->M[1][1] - mass->M[1][0]*mass->M[0][1]; mass->M_inv[0][0] = mass->M[1][1] / det; mass->M_inv[0][1] = - mass->M[0][1] / det; mass->M_inv[1][0] = - mass->M[1][0] / det; mass->M_inv[1][1] = mass->M[0][0] / det; return; } /* Jacobian Matrix and Jacobian Matrix Inverse */ void set_lin_JACOB( joint, mass ) struct THETA_POSITION *joint; struct MASS_PROPERTIES *mass; { RT_FLOAT s1, c1, s2, c2, s12, c12; RT_FLOAT det;

95

/* Often used angle sinusoids */ s1 = sin( joint->th1_pos ); c1 = cos( joint->th1_pos ); s2 = sin( joint->th2_pos ); c2 = cos( joint->th2_pos ); s12 = sin( joint->th1_pos + joint->th2_pos ); c12 = cos( joint->th1_pos + joint->th2_pos ); /* Jacobain */ mass->J[0][0] = -mass->L1 * s1 - mass->L2 * s12; mass->J[0][1] = -mass->L2 * s12; mass->J[1][0] = mass->L1 * c1 + mass->L2 * c12; mass->J[1][1] = mass->L2 * c12; /* Jacobian determinate (watch for singularities) */ if ( fabs( joint->th2_pos ) >= 0.05 ) { det = mass->L1 * mass->L2 * s2; } else if ( joint->th2_pos == 0 ) { det = mass->L1 * mass->L2 * sin(0.05); } else { det = mass->L1 * mass->L2 * sin(0.05*joint->th2_pos/fabs(joint->th2_pos)); } /* Jacobain inverse */ mass->J_inv[0][0] = mass->J[1][1] mass->J_inv[0][1] = -mass->J[0][1] mass->J_inv[1][0] = -mass->J[1][0] mass->J_inv[1][1] = mass->J[0][0] return; } /* Jacobian Derivative Matrix */ void set_lin_J_DOT( joint, mass ) struct THETA_POSITION *joint; struct MASS_PROPERTIES *mass; { RT_FLOAT s1, c1, s12, c12; RT_FLOAT dth1, dth12; /* Often used angle sinusoids */ s1 = sin( joint->th1_pos ); c1 = cos( joint->th1_pos ); s12 = sin( joint->th1_pos + joint->th2_pos ); c12 = cos( joint->th1_pos + joint->th2_pos ); dth1 = joint->th1_rate; dth12 = joint->th1_rate + joint->th2_rate; /* Jacobian Derivative */ / / / / det; det; det; det;

96
mass->J_dot[0][0] mass->J_dot[0][1] mass->J_dot[1][0] mass->J_dot[1][1] return; } = = = = -mass->L1 -mass->L2 -mass->L1 -mass->L2 * * * * c1 c12 s1 s12 * * * * dth1 - mass->L2 * c12 * dth12; dth12; dth1 - mass->L2 * s12 * dth12; dth12;

void lin_mat_gen( mass, lin_mat ) struct MASS_PROPERTIES *mass; RT_FLOAT *lin_mat; { /* linearizing Matrix = M*inv(J) */ lin_mat[0+0*2] = mass->M[0][0]*mass->J_inv[0][0] + mass->M[0][1]*mass->J_inv[1][0]; lin_mat[0+1*2] = mass->M[0][0]*mass->J_inv[0][1] + mass->M[0][1]*mass->J_inv[1][1]; lin_mat[1+0*2] = mass->M[1][0]*mass->J_inv[0][0] + mass->M[1][1]*mass->J_inv[1][0]; lin_mat[1+1*2] = mass->M[1][0]*mass->J_inv[0][1] + mass->M[1][1]*mass->J_inv[1][1]; return; } /*--------------------------------------------------------------------x_to_theta.h Data types/defines for coordinate transformations NOTES: none HISTORY: 12 Oct 13 Nov 18 Nov 10 Dec 95 95 95 95 D.Rathbun D.Rathbun D.Rathbun D.Rathbun created broke into individual routines altered for MatrixX real time data types updated for non-ANSI compliant compilers

---------------------------------------------------------------------*/ /* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Data Types */ /* Function Prototypes */ void x_to_th_pos( ); void x_to_th_rate( ); void x_to_th_accel( );

97
/*--------------------------------------------------------------------x_to_theta.c Transforms coodrinate systems for a planer two link manipulator from cartessian end point position to joint angle. INPUTS : TIP_POSITION x_pos y_pos x_rate y_rate x_accel y_accel

y x y x x

x axis position axis position axis rate axis rate axis acceleration axis acceleration

OUTPUTS : THETA_POSITION th1_pos theta th1_pos theta 2 th1_rate theta 1 th2_rate theta 2 th1_accel theta 1 th2_accel theta 2

1 axis position axis position axis rate axis rate axis acceleration axis acceleration

NOTES : * be careful with the calling order w.r.t the system matricies generation routines (set_lin_MASS, set_lin_JACOB, set_lin_J_dot) HISTORY : 12 Oct 13 Nov 18 Nov 10 Dec 95 95 95 95 D.Rathbun D.Rathbun D.Rathbun D.Rathbun created broke into individual routines altered for MatrixX real time data types updated for non-ANSI compliant compilers

---------------------------------------------------------------------*/ /* standard includes */ #include <math.h> /* local #include #include #include #include #include includes */ "properties.h" "lin_mat_gen.h" "tip_pos.h" "theta_pos.h" "x_to_theta.h"

#define pi (3.14159265e0) /* Convert cartesian positions to joint positions */ void x_to_th_pos( ep, mass, joint ) struct TIP_POSITION *ep; struct MASS_PROPERTIES *mass; struct THETA_POSITION *joint; { RT_FLOAT L1_sq, L2_sq, L12;

98
RT_FLOAT th_right, L_opp2, L_opp; RT_FLOAT cTh1_prime, cTh2_prime; /* Compute joint angles using the law of Cosines */ /* get arm length products */ L1_sq = mass->L1 * mass->L1; L2_sq = mass->L2 * mass->L2; L12 = mass->L1 * mass->L2; /* get 'arm triangle' properties */ L_opp2 = ep->x_pos * ep->x_pos + ep->y_pos * ep->y_pos ; L_opp = sqrt( L_opp2 ); /* th2 is the 180 deg compliment to the inside angle */ cTh2_prime = ( L1_sq + L2_sq - L_opp2 ) / ( 2.0e0 * L12 ); joint->th2_pos = pi - acos(cTh2_prime); /* th1 + inside angle = right triangle angle */ th_right = atan2( ep->y_pos, ep->x_pos ); cTh1_prime = ( L1_sq + L_opp2 - L2_sq ) / ( 2.0e0*mass->L1*L_opp ); joint->th1_pos = th_right - acos(cTh1_prime); return; }

/* Convert cartesian rates to joint rates */ void x_to_th_rate( ep, mass, joint ) struct TIP_POSITION *ep; struct MASS_PROPERTIES *mass; struct THETA_POSITION *joint; { /* Compute the joint rates THd = Jinv*Xd */ joint->th1_rate = mass->J_inv[0][0]*ep->x_rate * mass->J_inv[0][1]*ep->y_rate; joint->th2_rate = mass->J_inv[1][0]*ep->x_rate * mass->J_inv[1][1]*ep->y_rate; return; }

/* Convert cartesian accelerations to joint accelerations */ void x_to_th_accel( ep, mass, joint ) struct TIP_POSITION *ep; struct MASS_PROPERTIES *mass; struct THETA_POSITION *joint; { RT_FLOAT Jid[2][2]; /* Compute J_inv*J_dot */ Jid[0][0] = mass->J_inv[0][0]*mass->J_dot[0][0]

99
+ Jid[0][1] = + Jid[1][0] = + Jid[1][1] = + mass->J_inv[0][1]*mass->J_dot[1][0]; mass->J_inv[0][0]*mass->J_dot[0][1] mass->J_inv[0][1]*mass->J_dot[1][1]; mass->J_inv[1][0]*mass->J_dot[0][0] mass->J_inv[1][1]*mass->J_dot[1][0]; mass->J_inv[1][0]*mass->J_dot[0][1] mass->J_inv[1][1]*mass->J_dot[1][1];

/* Compute the joint accelerations * THdd = Jinv*Xdd - Jinv*Jd*Jinv*Xd * = Jinv*Xdd - Jinv*Jd*THd */ joint->th1_accel = mass->J_inv[0][0] * ep->x_accel + mass->J_inv[0][1] * ep->y_accel + Jid[0][0] * joint->th1_rate + Jid[0][1] * joint->th2_rate; joint->th2_accel = mass->J_inv[1][0] * ep->x_accel + mass->J_inv[1][1] * ep->y_accel + Jid[1][0] * joint->th1_rate + Jid[1][1] * joint->th2_rate; return; } /*---------------------------------------------------------------------tip_pos.h Data types/defines for end point coordinate systems. NOTES: none HISTORY: 12 Oct 95 18 Nov 95 D.Rathbun D.Rathbun created altered for MatrixX real time data types

---------------------------------------------------------------------*/ /* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Data Types */ struct TIP_POSITION { RT_FLOAT x_pos; RT_FLOAT y_pos; RT_FLOAT x_rate; RT_FLOAT y_rate; RT_FLOAT x_accel; RT_FLOAT y_accel; };

/* /* /* /* /* /*

x y x y x x

axis axis axis axis axis axis

position */ position */ rate */ rate */ acceleration */ acceleration */

/*---------------------------------------------------------------------theta_pos.h Data types/defines for joint based coordinate systems.

100
NOTES: none HISTORY: 12 Oct 95 18 Nov 95 D.Rathbun D.Rathbun created altered for MatrixX real time data types

---------------------------------------------------------------------*/ /* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Data Types */ struct THETA_POSITION { RT_FLOAT th1_pos; RT_FLOAT th2_pos; RT_FLOAT th1_rate; RT_FLOAT th2_rate; RT_FLOAT th1_accel; RT_FLOAT th2_accel; RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT }; /*---------------------------------------------------------------------properties.h Data types/defines for rigid body mass properties. NOTES: none HISTORY: 12 Oct 13 Nov 18 Nov 10 Dec 95 95 95 95 D.Rathbun D.Rathbun D.Rathbun D.Rathbun created added dynamic and kinematic matricies updated for MatrixX real time data types updated for non-ANSI compliant compilers L1; L2; M1; M2; A1; A2; I1; I2;

/* /* /* /* /* /*

theta theta theta theta theta theta 1 2 1 2 1 2 1 2

1 2 1 2 1 2

axis axis axis axis axis axis

position */ position */ rate */ rate */ acceleration */ acceleration */

/* /* /* /* /* /* /* /*

Arm Arm Arm Arm Arm Arm Arm Arm /* /* /* /* /*

length length mass = mass = center center moment moment

= 0.6325 */ = 0.6012 */ 3.6745 */ 1.0184 */ of mass = 0.3365 */ of mass = 0.2606 */ of inertia = 0.3703 */ of inertia = 0.0811 */

M[2][2]; M_inv[2][2]; J[2][2]; J_inv[2][2]; J_dot[2][2];

Mass Maxtrix */ Mass Matrix Inverse */ Jacobian Matrix */ Jacobain Inverse */ Jacobain Derivative */

---------------------------------------------------------------------*/

101
/* MatrixX-SystemBuild data types */ #include "sa_types.h" /* Data Types */ struct MASS_PROPERTIES RT_FLOAT L1; RT_FLOAT L2; RT_FLOAT M1; RT_FLOAT M2; RT_FLOAT A1; RT_FLOAT A2; RT_FLOAT I1; RT_FLOAT I2; RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT RT_FLOAT }; { /* /* /* /* /* /* /* /* Arm Arm Arm Arm Arm Arm Arm Arm /* /* /* /* /* 1 2 1 2 1 2 1 2 length length mass = mass = center center moment moment = 0.6325 */ = 0.6012 */ 3.6745 */ 1.0184 */ of mass = 0.3365 */ of mass = 0.2606 */ of inertia = 0.3703 */ of inertia = 0.0811 */

M[2][2]; M_inv[2][2]; J[2][2]; J_inv[2][2]; J_dot[2][2];

Mass Maxtrix */ Mass Matrix Inverse */ Jacobian Matrix */ Jacobain Inverse */ Jacobain Derivative */

/* Function Prototypes */ void set_mass_properites( ); /*--------------------------------------------------------------------properties.c Sets the two link flexible manipulator mass properties. INPUTS : MASS_PPOPERTIES L1 Arm L2 Arm 2 M1 Arm 1 M2 Arm 2 A1 Arm 1 A2 Arm 2 I1 Arm 1 I2 Arm 2

1 length length mass mass center of center of moment of moment of

mass mass inertia (about c.m.) inertia (about c.m.)

NOTES : * in kg-m-s units HISTORY : 12 Oct 95 18 Nov 95 10 Dec 95 D.Rathbun D.Rathbun D.Rathbun created altered for MatrixX real time data types updated for non-ANSI compliant compilers

---------------------------------------------------------------------*/ /* standard includes */ #include <math.h>

102
/* local includes */ #include "properties.h" void set_mass_properties( mass ) struct MASS_PROPERTIES *mass; { mass->L1 = 0.6325; mass->L2 = 0.6012; mass->M1 = 3.6745; mass->M2 = 1.0184; mass->A1 = 0.3365; mass->A2 = 0.2606; mass->I1 = 0.3703; mass->I2 = 0.0811; return; }

2.4. Singular State Transition LQR Solution

function [K,S] = dlqrdelay(A,B,Q,R); % DLQRDELAY Linear quadratic regulator design for discrete-time % systems. % % >> K = dlqrdelay( A, B, Q, R ); % % K is the optimal feedback gain matrix such that the feedback % law u[n] = -Kx[n] minimizes the cost function % % J = Sum {x'Qx + u'Ru} % % subject to the constraint equation: % % x[n+1] = A x[n] + B u[n] % % DLQRDELAY differs from DLQR in that the system transition % matrix is allowed to be singular. This allows for solutions % when the system contains a pure delay. Sytem eigenvalues at % Z = -1 are not allowed. % % Based on a brute force itteration method. This method is % _much_ slower than the normal eigen decomposition method % David Rathbun, rathbun@aa.washington.edu % AA Deptment, U of Washington % Check the inputs

103
[m,n] = size(Q); if (m==n), state_size = m; else, error('Q not square'); end; [m,n] = size(R); if (m==n), input_size = m; else, error('R not square'); end; % Set the initial conditions S = Q; Mp = B'*S*B + R; M = S - S*B*inv(Mp)*B'*S; K = zeros(input_size,state_size); sol_norm = 0; keep_looping = 1; % Main Loop while (keep_looping==1), % Do 10 itterations on the solution for i = 1:10, S = A'*M*A + Q; Mp = B'*S*B + R; M = S - S*B*inv(Mp)*B'*S; end; % Check the degree of convergence of the solution K = inv(B'*S*B + R)*B'*S*A; sol_norm_last = sol_norm; sol_norm = norm(K); per_error = abs(sol_norm - sol_norm_last) / ... max([sol_norm, sol_norm_last]); if (per_error < 1e-7 ), keep_looping = 0; end; end; % ----- end ------

104 2.5. LQR Compensator Solution

% cont_LQG % % generates the tip position controller for the two link % flexible manipulator using LQR/LQE techniques DT = 0.030; theta1 = 0; theta2 = pi*0.40;

% nominal joint angles

% ===== Plant Model construction ==================================== % Get the continuous plant model [Ap,Bp,Cp,Dp,xep,yep] = make_lin_model( theta1, theta2 ); % Convert to discrete with a time delay [Az,Bz,Cz,Dz] = c2dt(Ap,Bp,Cp,DT,2*DT); % Add additional outputs of system modes using eigenvector weighting [Vec,Lam] = eig(Az); ino = find( diag(Lam) ~= 0 ); Lam = Lam(ino,ino); t_ang = imag(diag(Lam))./real(diag(Lam)); [t_ang2,inx] = sort(abs(t_ang)); s_inx2 = max(size( find(t_ang2 == 0 ) )); VecI = pinv( Vec ); Cvec = real( VecI(ino(inx(1:s_inx2)),:) ); Cvec(s_inx2+1,:) = ... real(VecI(ino(inx(s_inx2+1)),:)+VecI(ino(inx(s_inx2+2)),:)); Cvec(s_inx2+2,:) = ... real(VecI(ino(inx(s_inx2+3)),:)+VecI(ino(inx(s_inx2+4)),:)); Cvec(s_inx2+3,:) = ... real(VecI(ino(inx(s_inx2+5)),:)+VecI(ino(inx(s_inx2+6)),:)); Cvec(s_inx2+4,:) = ... real(VecI(ino(inx(s_inx2+7)),:)+VecI(ino(inx(s_inx2+8)),:)); [outS1,stateS1] = size(Cz); Cz = [ Cz ; Cvec ]; [outS2,stateS2] = size(Cz); Dz = [ Dz ; zeros(outS2-outS1,2) ]; % Create a set of parallel single pole low-pass filters wp = 3.5; [a1,b1,c1,d1] = tf2ss(wp*[1],[1 wp]); [At,Bt,Ct,Dt] = c2dm(a1,b1,c1,d1,DT,'tustin'); [Alp,Blp,Clp,Dlp] = append(At,Bt,Ct,Dt,At,Bt,Ct,Dt); % Add the filters to the plant model inputs [a1,b1,c1,d1] = append(Alp,Blp,Clp,Dlp,Az,Bz,Cz,Dz); fb = [ 3 1

105
4 2 ]; ins = [ 1 2 ]; outs = [ 3:outS2+2 ]; [Azf,Bzf,Czf,Dzf] = connect(a1,b1,c1,d1, fb, ins, outs );

% ===== Control =================================================== % Define the output weighting matricies Q = diag([ 0 0 ... %tip-pos 0 0 ... %joints 0 0 0 0 ... %assumed-modes 800 200 0 0 ... %rb-modes 1 0.1 0.001 0 ... %real-modes ]); R = diag([ 10 20 ]); % Convert to the State Weighting matricies Qp = Czf'*Q*Czf; Np = Czf'*Q*Dzf; Rp = R + Dzf'*Q*Dzf; % Generate the steady state LGR control gain if ( max(max(abs(Np))) == 0 ), [ K ] = dlqrdelay(Azf,Bzf, Qp,Rp ); else, error('Can''t handle cross terms in the regulator solution'); end; % Compute the reference input gain matricies stateS3 = max(size(Azf)); AREF = [ Azf-eye(stateS3) Bzf Czf(1:2,:) zeros(2,2) ]; BREF = [ zeros(stateS3,2) eye(2) ]; NN = AREF\BREF; Nx = NN(1:stateS3,:); Nu = NN(stateS3+1:stateS3+2,:); % ===== Estimator =================================================== % Get the measurement/process noise Q = 1e-5*diag([ 2 8 ]); q1 = 0.05e-3; cov1 = q1^2/12*2*4; R = diag([cov1 cov1]); Cza = Czf([1 2],:);

106
[kt,st] = dlqrdelay( Azf', Cza', Bzf*Q*Bzf', R ); L = (st'*Cza')/(Cza*st'*Cza'+R); % ===== Assembly ==================================================== % Assemble the controller Ac = (Azf - Bzf*K)*(eye(size(Azf)) - L*Cza); Bc = [ Bzf*(K*Nx+Nu) (Azf-Bzf*K)*L ]; Cc = -K*( eye(size(Azf)) - L*Cza ); Dc = [ K*Nx+Nu -K*L ]; % Add the low pass [a1,b1,c1,d1] = append(Ac,Bc,Cc,Dc,Alp,Blp,Clp,Dlp); [Ac,Bc,Cc,Dc] = connect(a1,b1,c1,d1,[5 1;6 2],[1:4],[3 4]); % Clean clear clear clear clear clear up AREF BREF At Bt Ct Dt Azf Bzf Czf Dzf Cza NN Np Nu Nx Q Qp R Rp a1 b1 c1 d1 Vec VecI Lam ino ins inx t_ang t_ang2 outS1 outS2 stateS1 stateS2 stateS3 cov1 q1 fb outs kt st s_inx2

% Saving save controller Ac Bc Cc Dc DT

Appendix 3. Equation Derivations

3.1 Jacobian vb = va + r + Hr = =
L1s1 1 L1c1 1

0 0

+ ) L2s12( 1 2 + ) L2c12( 1 2 1 2
(44)

L 1 s 1 L 2 s 12 L 2 s 12 L 1 c 1 + L 2 c 12 L 2 c 12

3.2 Rigid Body Equations of Motion

Lagrangian Formulation d dt ML M i ML M = i i i = 1, 2
(45)

Lagrangian L = +
1 2 1 2

2 m 1 a2 11 + m2

1 2

2 + I1 1

1 2

2 + 2 1 2+ 2) I 2 ( 1 2
(46)

7 (L 2 1

2 2 + 2L 1 a2 c 2 + a2 2 ) 1 + 2a2 (a2 + L 1 c 2 ) 1 2 + (a2 )

Resulting Equations 1 m 11 m 12 m 12 m 22 2 + n1 n2 =

1 2
(47)

2 2 m 1 1 = m 1 a2 1 + m 2 (L 1 + 2L 1 a2 c 2 + a2 ) + I 1 + I 2

m 1 2 = m 2 a2 (a2 + L 1 c 2 ) + I 2 m 2 2 = m 2 a2 2 + I2 n1 n2 1 2 + 2) = m 2 L 1 a2 s 2 (2 2 2 = m 2 L 1 a2 s 2 1

3.3 PSD Integrals Integral formulas from CRC Handbook

108 x 2 dx = 2 bx + a dx I 2 2 2 = b x + a

x a dx I 2 b b bx + a 1 bx tan 1 ( ) a ab

(48)

gives (b 2 a2 ) x 2 + b2 2 I 2 2 dx = 2 x + tan 1 ( a x + a 0 (b a ) = 2 + tan 1 ( a

2 2

x a

)
0

(49)

and
4 a2 1 x 2N I 2 2 dx = 2N a tan ( ) a 0 0 x +a = 2N a = N a 2 4

(50)

3.4 Second Order Slew Commander

Assuming a maximum limit on the single axis acceleration and rate, the optimal slew profile will look as follows :
Position Rate Acceleration

000 111

Time t1 Time Time t2 t3

If there is no coast phase, the base equations are : x 1 = x o + v xo t 1 + v x1 = v xo + m t 1

1 2

m t2 1

(51)

109 x f = x 1 + v x1 t 3
1 2

m t2 3

v xf = v x1 m t 3 = 0 which solved for the time periods gives : v xo = + m v2 x xo + 2 m m

(52)

t1

t3 =

v2 x xo + 2 m m

where x = x f x o A coast phase is needed if the maximum attained velocity is greater than the allowable velocity. This gives the test : vm < v1 = v xo + m t 1 =
v2 xo + m x 2
(53)

times are now those that increase the velocity to the maximum and back to zero : t1 = t2 = v m v xo m vm m
(54)

the coast time is that required to match the final position at the maximum rate : t2 = = = x2 x1 vm (x f x o ) (x 1 x o ) (x f x 2 ) vm
2 2 2v m v xo x vm 2v m m

(55)

The suboptimal axis uses the same base equations :

110 y 1 = y o + v yo t 1 + v y1 = v yo + 1 t 1 y 2 = y 1 + v y1 t 2 v y2 = v y1 y f = y 2 + v y2 t 3
1 2 1 2

1 t2 1

(56)

3 t2 3

v yf = v y2 3 t 3 = 0 They are solved for the accelerations, using the times as given :

1 = 3 =

2 y v yo (2t 1 + 2t 2 + t 3 ) t 1 (t 1 + 2t 2 + t 3 ) 2 y v yo t 1 t 1 (t 1 + 2t 2 + t 3 )
(57)

which are applicable to both coast and no coast (t2 =0) type slews. These equations are valid for any magnitude input parameters, but they do assume that all parameters are positive. If negative inputs are to be allowed (which they must) then certain adjustments need to be made (as an example, if v xo=0 and x<0, then the equations are valid for -m). The simple test for the sign convention is as follows : given v xo = 0, if ( x < 0 ) then

m = m
vm = vm given x = 0, if ( v xo > 0 ) then

m = m
vm = vm which can be generalized to the multiple initial condition case by examining the case where both t1 and t2 need to be zero.

111

x = v xo t
therefore

1 2

t2
(58)

0 = v xo t
2 2 x v xo = 0

gives the final sign convention test : if ( 2 x v xo v xo $ 0 ), use + m , + v m else, use m , v m and the coast phase test must become : v m > m v2 x xo + 2 m m
(59)

(60)

Appendix 4. Hardware Diagrams

Serial line pin out diagram :

13 25 12 24 11 23 10 22 9 21 8 20 7 19 6 18 5 17 4 16 3 15 2 14 1

Line Symbol

25 Pin Serial Cable

25 Pin Serial Cable

AC100 Breakout 3 5 7 9 11 15 14 13

TxD RxD RTS CTS DSR DCD DTR GND

Transmitted Data Received Data Request to Send Clear to Send Data Set Ready Received Line Signal Detector Data Terminal Ready Ground

2 3 4 5 6 8 20 7