This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
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 -3 -2 -1 0 1 2 3 4-4
-3
-2
-1
0
1
2
3
4
shoulder angle (rad)
elbo
w a
ngle
(ra
d)
o = stable point x = unstable point
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 10 20 30 400.3
0.4
0.5
0.6
0.7
0.8
seconds
X P
ositi
on (
m)
0 10 20 30 400.3
0.4
0.5
0.6
0.7
0.8
seconds
Y P
ositi
on (
m)
0 10 20 30 40-50
0
50
100
150
seconds
Join
t Ang
les
(deg
)
0 10 20 30 40
-0.2
-0.1
0
0.1
0.2
seconds
Join
t Tor
ques
(N
m)
Figure 4.1 Classical Compensator Response to Slew Command
53
0 5 10 15 20 25 30 35 400.58
0.6
0.62
0.64
0.66
0.68
0.7
0.72
0.74
seconds
End
Poi
nt P
ositi
ons
(m)
X position
Y position
Figure 4.2 Classical Compensator X Axis Disturbance Response
0 2 4 6 8 10 120.58
0.59
0.6
0.61
0.62
0.63
0.64
0.65
0.66
0.67
seconds
End
Poi
nt P
ositi
ons
(m)
X position
Y position
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 5th 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 5 10 15 20 25 300.35
0.4
0.45
0.5
0.55
0.6
0.65
0.7
0.75
time (sec)
posi
tion
(m)
X axis position
Y axis position
Figure 4.4 LQG Compensator Response to Slew Command
0 10 20 30-0.4
-0.2
0
0.2
time (sec)
posi
tion
(rad
)
shoulder angle
0 10 20 30 401.2
1.4
1.6
1.8
time (sec)
posi
tion
(rad
)
elbow angle
0 10 20 30-0.1
-0.05
0
0.05
0.1
time (sec)
torq
ue (
Nm
)
elbow joint
0 10 20 30-0.1
-0.05
0
0.05
0.1
time (sec)
torq
ue (
Nm
)
shoulder joint
Figure 4.5 LQG Compensator Commanded Slew cont.
56
0 2 4 6 8 10 12 14 16 18 200.74
0.76
0.78
0.8
0.82
time (sec)
posi
tion
(m)
X axis
Y axis
0 2 4 6 8 10 12 14 16 18 200.58
0.6
0.62
0.64
0.66
time (sec)
posi
tion
(m)
Figure 4.6 LQG Compensator X Axis Disturbance Response
0 5 10 15 20 250.73
0.74
0.75
0.76
0.77
time (sec)
posi
tion
(m)
X axis
0 5 10 15 20 250.63
0.64
0.65
0.66
0.67
time (sec)
posi
tion
(m)
Y axis
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, L2, ε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 1 2 3 4 5 6 7 80
0.2
0.4
0.6
0.8
1
time (sec)
posi
tion
(m)
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 1 2 3 4 5 6 7 8
-0.02
-0.01
0
0.01
0.02
0.03
time (sec)
X a
xis
erro
r (m
)
0 1 2 3 4 5 6 7 8
-0.02
-0.01
0
0.01
0.02
0.03
time (sec)
Y a
xis
erro
r (m
)
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 ElectromechanicalSystems 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 Transactionson 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 InstrumentsCorporation, 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 TimeDelay”, M.S.Wang and B.Yang, Journal of Dynamic Systems, Measurement, andControl. vol. 116, June 1994, pg 216-222.
10. “Experimental Evaluation of Robotic Reduced-Order Hybrid Position/Force Control ona Two-Link Flexible Manipulator”, D.Bossert, U.Lee, J.Vagners, 1996 IEEEInternational Conference on Robotics and Automation.
11. “System Build Users Guide”, ver 3.0, Integrated Systems Inc. 3260 Jay St, SantaClara, CA 95054, 1992.
12. “SANDY™. A Design Tool for Linear Multivariable Optimal Control, User’s 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 Description Value
m1, m2 Mass of the links in the rigid body
formulation
3.6745 kg, 1.0184 kg
L1, L2 Length of the links including both
flexible and rigid sections
0.6519 m, 0.6019 m
a1, a2 Distance from the joint to the center of
mass of the links in the rigid body
formulation
0.3365 m, 0.2606 m
I1, I2 Inertia of the links about the center of
mass in the rigid body formulation
0.370 kgm2, 0.081 kgm2
∆θ1, ∆θ2Encoder angular quantization 0.0015 r, 0.0015 r
x, y End point position sensor quantization 0.05 mm, 0.05 mm
ωiFlexible mode frequencies 1.8, 3.4, 4.5, 8.0 hz
fDYNA maximum sensor update rate 65.8 hz
kx, ky Sensor origin in table top coordinates 0.762 m, 0.653 m
z0 Sensor distance from table top 0.775 m
ηx, ηySensor rotational misalignments 0.0029 r, 0.0129 r
d1, d2 Inboard distances to flexible link 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
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 floating- point vector which is used as input to the model for the current sampling interval.
67
INPUTS : This version supports no inputs. To configure the sensor, see the IP_SETUP routine.
OUTPUTS :
[1] target X position in millimeters. (sensor frame)
[2] target Y position in millimeters. (sensor frame)
[3] target Z position in millimeters. (sensor frame)
See the DynaSight Developers manual for a deffinition of the sensor frame coordinate system.
[4] data quality flag. -1=old, 0=good, 1=supect, 2=bad.
/* 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;};
/* 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. Function called as scheduled output event.
Inputs : sysptr user-transparent pointer to systemattributes
model_float[] SystemBuild output vector ser_channel serial channel
/* put the output data into the System build output vector */ 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;
/* The data quality depends on the sensor tracking status */ if ( local_ptr->dyna_struct.status == STS_TRACK ) {
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, Origin Instruments Corporation06 MAR 95 DB.Rathbun modified for single data structure
/* structure for raw data from NATIVE0 packet */struct dyNative0 {
unsigned status : 2; /* sensor operating mode */unsigned btnr : 1; /* reserved field */unsigned sync : 1; /* debounced sync indicator, 0=>open */unsigned dummy0 : 4; /* dummy field for 16-bit alignment */unsigned exp : 2; /* base 2 exponent for x, y, z */unsigned fmt : 2; /* data format indicator, 0=>NATIVE0 */unsigned dummy1 : 4; /* dummy field for 16-bit alignment */long x; /* x measurement */long y; /* y measurement */long z; /* z measurement */int pktCount; /* count of total 'ok' packets */
73
int pktState; /* buffer pointer for state machine */char pktbuf[PKT_MAX_LENGTH]; /* used to sync to data format */int config_flag; /* flag to mark initialization */int buffer_flag; /* flag to mark initialization */
};
/* Defines for decode of ".status" field in struct dyNative0: */#define STS_SEARCH 0#define STS_COAST 1#define STS_CAUTION 2#define STS_TRACK 3
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 Original, Origin Instruments Corporation06 MAR 95 DB.Rathbun convert to a single data structure
passed control of static structure to caller updated bit operations for a 32 bit arch.
/******************************************************************** 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 dyNative0that* will receive the most recently decoded packet.
74
** Return Value:* TRUE if a packet synchronization error was detected.* FALSE if no error was detected.********************************************************************/
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 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 floating- point 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 ISI Original 06 MAR 95 DB.Rathbun Adapted to DynaSight format
/* 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. Function called as scheduled output event.
Inputs : sysptr user-transparent pointer to systemattributes
model_float[] SystemBuild output vector ser_channel serial channel
System Build format subroutine source code to implement the slew commander for the two link flexible manipulator.
Inputs : [ 0] Xpo X axis initial position [ 1] Ypo Y axis initial position [ 2] Xpf X axis final position [ 3] Ypf Y axis final position [ 4] Xvo X axis initial velocity [ 5] Yvo Y axis initial velocity [ 6] tinit slew start time [ 7] r_init re-initialization flag 1=init, 0=no Outputs : [ 0] XepComm x axis tip position command (m) [ 1] YepComm y axis tip position command (m) [ 2] XepRate x axis tip rate command (m/s) [ 3] YepRate y axis tip rate command (m/s) [ 4] XepAccel x axis tip accel command (m/s^2) [ 5] YepAccel y axis tip accel command (m/s^2)
States : none Parameters : [ 0] vmax maximum rate constraint [ 1] amax maximum acceleration constraint
Notes : none History : 18 Nov 95 D.Rathbun created ======================================================================*/
/* 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;
Data types/defines for slew command generation. NOTES: none HISTORY: 12 Oct 95 D.Rathbun created 13 Nov 95 D.Rathbun updated for real 2nd order slews 18 Nov 95 D.RAthbun altered for MatrixX real time data types
/* MatrixX-SystemBuild data types */#include "sa_types.h"
/* Slew Data Types */struct SLEW_PARAM { struct { RT_FLOAT xpo; /* initial x axis position */ RT_FLOAT ypo; /* initial y axis position */ RT_FLOAT xvo; /* initial x axis velocity */ RT_FLOAT yvo; /* initial y axis velocity */ RT_FLOAT xpf; /* final x axis position */ RT_FLOAT ypf; /* final y axis position */ RT_FLOAT tinit; /* initial slew time */
85
} inputs; struct { RT_FLOAT vmax; /* maximum allowable single axis rate */ RT_FLOAT amax; /* maximum allowable acceleration */ } limits; struct { RT_FLOAT t1,t2,t3; /* slew phase durations */ RT_FLOAT T0,T1,T2,T3; /* slew phase end times */ RT_FLOAT xa1, xa2; /* x axis accelerations */ RT_FLOAT ya1, ya2; /* y axis accelerations */ RT_FLOAT xp1, xv1; /* x axis point 1 states */ RT_FLOAT yp1, yv1; /* y axis point 1 states */ RT_FLOAT xp2, xv2; /* x axis point 2 states */ RT_FLOAT yp2, yv2; /* y axis point 2 states */ } data;
RT_INTEGER setup;};
/* Function Prototypes */void slew_set( );void slew_comm( );
Generates an optimmum two dimensional second order slew command between two points under rate and acceleration constraints. INPUTS : SLEW_PARAM INPUTS
xpo initial x axis position ypo initial y axis position xvo initial x axis rate yvo initial x axis rate xpf final x axis position ypf final y axis position tinit initial slew time LIMITS vmax maximum allowable single axis rate amax maximum allowable single axis acceleration
tnow current time OUTPUTS : TIP_POSITION x_pos current x axis position command
y_pos current y axis position command x_rate current x axis rate command y_rate current y axis rate command x_accel current x 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 95 D.Rathbun created 13 Nov 95 D.Rathbun updated for real 2nd order slews 18 Nov 95 D.Rathbun updated for MatriX real time data types 10 Dec 95 D.Rathbun updated for a initialization check updated for non-ANSI compliant compilers
/* 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;
} /* 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 back to x/y system */ if ( slew_type == 'X' ) {
System Build format subroutine source code to implement the rigid body linearization for the two link flexible manipulator.
Inputs : [ 0] Xep cartesian X axis end point command (m) [ 1] Yep cartesian Y axis end point command (m) [ 2] TorX desired X axis force (N) [ 3] TorY desired Y axis force (N) Outputs : [ 0] TorTh1 theta 1 joint torque (Nm) [ 1] TorTh2 theta 2 joint torque (Nm)
States : none Parameters : none
Notes : none History : 18 Nov 95 D.Rathbun created ======================================================================*/
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>
/* ----- Local defines and includes ----- */#include "properties.h"#include "theta_pos.h"#include "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;
/* 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 commaned end point based positions */ep->x_pos = u[0];ep->y_pos = u[1];ep->x_rate = 0.0e0;ep->y_rate = 0.0e0;ep->x_accel = 0.0e0;ep->y_accel = 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 ============================================*/
Data types/defines for rigid body linearization matrix generation NOTES: none
93
HISTORY: 12 Oct 95 D.Rathbun created 13 Nov 95 D.Rathbun broke into individual routines 18 Nov 95 D.Rathbun altered for MatrixX real time data types 10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers ---------------------------------------------------------------------*/
/* MatrixX-SystemBuild data types */#include "sa_types.h"
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 95 D.Rathbun created 12 Oct 95 D.Rathbun converted to subroutine format 13 Nov 95 D.Rathbun broke into individual routines 18 Nov 95 D.Rathbun altered for MatrixX real time data types
94
10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers
Data types/defines for coordinate transformations NOTES: none HISTORY: 12 Oct 95 D.Rathbun created 13 Nov 95 D.Rathbun broke into individual routines 18 Nov 95 D.Rathbun altered for MatrixX real time data types 10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers
Transforms coodrinate systems for a planer two link manipulator from cartessian end point position to joint angle. INPUTS : TIP_POSITION x_pos x axis position
y_pos y axis position x_rate x axis rate y_rate y axis rate x_accel x axis acceleration y_accel x axis acceleration
OUTPUTS : THETA_POSITION th1_pos theta 1 axis position
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 95 D.Rathbun created 13 Nov 95 D.Rathbun broke into individual routines 18 Nov 95 D.Rathbun altered for MatrixX real time data types 10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers
Data types/defines for end point coordinate systems. NOTES: none HISTORY: 12 Oct 95 D.Rathbun created 18 Nov 95 D.Rathbun altered for MatrixX real time data types ---------------------------------------------------------------------*/
/* MatrixX-SystemBuild data types */#include "sa_types.h"
/* Data Types */struct TIP_POSITION { RT_FLOAT x_pos; /* x axis position */ RT_FLOAT y_pos; /* y axis position */ RT_FLOAT x_rate; /* x axis rate */ RT_FLOAT y_rate; /* y axis rate */ RT_FLOAT x_accel; /* x axis acceleration */ RT_FLOAT y_accel; /* x axis acceleration */};
Data types/defines for joint based coordinate systems.
100
NOTES: none HISTORY: 12 Oct 95 D.Rathbun created 18 Nov 95 D.Rathbun altered for MatrixX real time data types ---------------------------------------------------------------------*/
/* MatrixX-SystemBuild data types */#include "sa_types.h"
Data types/defines for rigid body mass properties. NOTES: none HISTORY: 12 Oct 95 D.Rathbun created 13 Nov 95 D.Rathbun added dynamic and kinematic matricies 18 Nov 95 D.Rathbun updated for MatrixX real time data types 10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers ---------------------------------------------------------------------*/
101
/* MatrixX-SystemBuild data types */#include "sa_types.h"
/* Data Types */struct MASS_PROPERTIES { RT_FLOAT L1; /* Arm 1 length = 0.6325 */ RT_FLOAT L2; /* Arm 2 length = 0.6012 */ RT_FLOAT M1; /* Arm 1 mass = 3.6745 */ RT_FLOAT M2; /* Arm 2 mass = 1.0184 */ RT_FLOAT A1; /* Arm 1 center of mass = 0.3365 */ RT_FLOAT A2; /* Arm 2 center of mass = 0.2606 */ RT_FLOAT I1; /* Arm 1 moment of inertia = 0.3703 */ RT_FLOAT I2; /* Arm 2 moment of inertia = 0.0811 */ RT_FLOAT M[2][2]; /* Mass Maxtrix */ RT_FLOAT M_inv[2][2]; /* Mass Matrix Inverse */ RT_FLOAT J[2][2]; /* Jacobian Matrix */ RT_FLOAT J_inv[2][2]; /* Jacobain Inverse */ RT_FLOAT J_dot[2][2]; /* Jacobain Derivative */};
/* Function Prototypes */void set_mass_properites( );
Sets the two link flexible manipulator mass properties. INPUTS : MASS_PPOPERTIES L1 Arm 1 length
L2 Arm 2 length M1 Arm 1 mass M2 Arm 2 mass A1 Arm 1 center of mass A2 Arm 2 center of mass I1 Arm 1 moment of inertia (about c.m.) I2 Arm 2 moment of inertia (about c.m.)
NOTES : * in kg-m-s units HISTORY : 12 Oct 95 D.Rathbun created 18 Nov 95 D.Rathbun altered for MatrixX real time data types 10 Dec 95 D.Rathbun updated for non-ANSI compliant compilers
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
% 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
% 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,:);