Professional Documents
Culture Documents
This appendix proves through analysis that low gearing substantially reduces the dynamics of a
motor's external load. It also shows that low gearing uncouples link dynamics in the PR
manipulator. Lagrange's equation applied to the PR manipulator and an example presented by
Ogata (1980), pp 130-131 are used to support this claim. Ogata's example is applied to the
manipulator's translation link and is presented in Figure A.1
N6
Tmotor N1
TLoad
θ1, V1, J1 N3 θ4, V4, J4
N5
N4 θ3, V3, J3
where
The torque equation for the first shaft (the motor shaft) is:
124
where T1 is the load torque on gear 1 due to the rest of the gear-train. For the second shaft, the
torque equation is:
where T2 is the torque transmitted to gear 2 and T3 is the load torque on gear 3 due to the rest of
the gear-train. Since the work done by gear 1 is equal to that of gear 2 then:
T1θ 1 = T2θ 2
and
N 1θ 1 = N 2θ 2
θ 2 N1
=
θ 1 N2
T2 θ 1 N 2
= =
T1 θ 2 N1
N2
T2 = T1 (A.3)
N1
and
N4
T4 = T3 (A.5)
N3
and
N6
T6 = T5 (A.7)
N5
If equations (A.1-A.7) are back substituted into one another then:
(
N
) NN
( ) NN N
( )
Tmotor = J 1θ1 + D1θ1 + 1 J 2θ2 + D2θ2 + 1 3 J 3θ3 + D3θv3 + 1 3 5 J 4θ4 + D4θ4 + TL
N2 N2N4 N2 N4 N6
125
Equation (A.8)
N5 N N
θ4 = θ3 , θ 3 = 3 , θ 2 = 1 θ1
N6 N4 N2
N N N
θ 4 = 1 3 5 θ1
N2 N4 N6
then equation (A.8) becomes:
N
2
NN
2
N1 N 3 N 5
2
Tmotor = J 1 + 1
J 2 + 1 3 J 3 + J 4 θ1
N2 N2N4 N2N4 N6
N1
2
N1 N 3
2
N1 N 3 N 5
2
+ D1 + D2 + D3 + D4 θ1 (A.9)
N2 N2N4 N2N4 N6
NN N
+ 1 3 5 TL
N2N4 N6
N N N
NT = 1 3 5 (A.10)
N2 N4 N6
If each gear pair in NT has a smaller numerator than denominator, then NT will be a number
much less than unity. For this study, low gearing refers to a gear ratio that has a denominator
much greater than its numerator. By using Equation (A.10), then equation (A.9) becomes:
2 2 2
N N N N N N
J eq = J1 + 1 J 2 + 1 3 J 3 + 1 3 5 J 4 (A.12)
N2 N2 N4 N2 N4 N6
2 2 2
N N N N N N
Deq = D1 + 1 D2 + 1 3 D3 + 1 3 5 D4 (A.13)
N2 N2 N4 N2 N4 N6
If the gear-train damping and inertia terms are taken to be zero, then equation (A.11) becomes:
126
Equation (A.14) illustrates the dramatic effect gearing has on external inertial and torque loads.
If NT results in a small ratio, then the inertial and damping effects associated with the load can be
assumed negligible. The magnitude of the torque load TLoad , which is comprised of constant
friction terms that are generated in the mechanism as well as the torque developed from the
opposing load, is also reduced by NT. Thus, if sufficiently low gearing is applied to a motor, the
external inertial, viscous, and torque terms become insignificant. The only inertial and viscous
effects that the motor must overcome are internal to itself (J1 and D1 ).
Now consider solving the dynamical equations for the PR manipulator. The generalized
equation given by Craig (1989) is:
where τ is the applied torque or force, θ is the position vector, M(θ) is the symmetric positive
definite mass/inertia matrix, V (θ , θ) represents the centrifugal and Coriolis forces, and
F (θ) represents terms due to friction effects. The derivation of the dynamic equations begins by
diagramming the system. The friction matrix in (A.15) is neglected for this diagram.
Body 3
l1
Body 1
Y
l2 m3
m1
X
A
θ m2,l1
τ2 τ1
Body 2
xOA
θ [rotation angle]
xOA [translation displacement]
m1 [mass of length 1]
m2 [mass of length 2]
m3 [point mass of the non-rotating frame holder]
l1 [length of the rotating link]
l2 [distance to rotating link's center of gravity ]
IZZ2 [moment of inertia (l2)]
τ1 [torque applied to rotation link]
τ2 [force applied to translation link]
127
Stoten's (1992) algorithm was followed to determined the manipulator's dynamics. It is
presented as follows:
2) Determine the total kinetic energy T, and potential energy, U in the machine. Form the
Lagrangian:
L= T-U (A.16)
d ∂L ∂L
− = τi
dt ∂x i ∂xi
(A.17)
where xi is the generalized co-ordinate of each degree of freedom and τi is the generalized
external force applied at each co-ordinate xi .
The system operates in the X-Y plane, so gravity is not a factor, thus energy as related to the
potential U is eliminated from the Lagrangian. Kinetic energy equations are developed for each
body as labeled in Figure A.2.
1 1 1
k1 = m1 x12 + I ZZ 1θ12 + m1 y12
2 2 2
where
y1 = 0
θ = 0
1
x1 = xOA
then
1
k1 = m1 xOA
2
(A.18)
2
1 1 1
k2 = m2 x 22 + I ZZ 2θ22 + m2 y 22
2 2 2
128
where
θ = θ2
x 2 = xOA + l 2 cos θ
y 2 = l 2 sin θ
x 2 = xOA − l2 sin θθ
y = l cos θθ
1 2
then
k2 =
1
2
( 1
) 1
(
m2 xOA − l2 sin θθ + m2 l 2 cos θθ + I ZZ 2θ 2
2
2
2
2
) (A.19)
k3 =
1
2
( 1
)
m3 xOA − l1 sin θθ + m3 l1 cos θθ
2
2
( )2
(A.20)
T = k1 + k 2 + k 3 =
1 2 1
( 2 1 ) (
2 1 2
2 m1 xOA + 2 m2 xOA − l 2 sin θθ + 2 m2 l 2 cos θθ + 2 I ZZ 2θ +) (A.21)
1
( )
2 1 (
2 m3 xOA − l1 sin θθ + 2 m3 l1 cos θθ
2 )
and the Lagrangian is:
1
L = T − U = T = m1 xOA 2
1
(2 1
)
2 1
(
+ m2 xOA − l2 sin θθ + m2 l 2 cos θθ + I ZZ 2θ 2 + )
2 2 2 2
(A.22)
1
( )
2 1 ( 2
2 m3 x OA − l1 sin θθ + 2 m3 l1 cos θθ )
Applying Lagrange's equation to each link produces:
d ∂L ∂L
− = τ1 (rotation torque) (A.23)
dt ∂θ ∂θ
d ∂L ∂L
− =τ2
dt ∂xOA ∂xOA
(translation force) (A.24)
129
where L is defined by equation (A.22).
Next consider gearing the displacement variables and writing them in terms of:
θ = N T1θ1 xOA = N T2 rθ 2
θ = N T1θ1 xOA = N T2 rθ2
θ = N T1θ1 xOA = N T2 rθ2
where θ1 is the angular position of the rotation motor, θ2 is the angular position of the
translation motor, N T(1, 2 ) are the respective gear ratios, and r is the translation link's equivalent
radius. Also consider gearing the torque and force terms in (A.23) and (A.24) and writing them
as:
τ g1
τ1 =
N T1
τ g2
τ2 =
N T2 r
where the subscript g stands for the torque generated by the motor.
Taking the partial and time derivatives of equation (A.22) with respect to θ = N T1θ1 yields:
∂L
∂N T1θ1
( )
= − m2 l 2 N T2 rθ2 sin (N T1θ1 ) + m2 l 22 (sin (N T1θ1 )) N T1θ1 + m2 l 22 (cos(N T1θ1 )) N T1θ1 + I ZZ 2 N T1θ1
2 2
( ) ( ) ( (
− m3l1 N T2 rθ2 sin N T1θ1 + m3l12 sin N T1θ1 )) N
2
θ + m3l12 (cos(N T1θ1 )) N T1θ1
T1 1
2
Equation (A.25)
and
130
d ∂L
dt ∂N T1θ
( ) ( )
= −m2 l 2 N T2 rθ2 sin(N T1θ 1 )− m2 l 2 N T2 rθ2 (cos(N T1θ 1 ))N T1θ1 + m2 l 2 (sin (N T1θ 1 )) N T1θ1 +
2 2
( ) ( )
m2 l 22 2(sin(N T1θ 1 )cos(N T1θ 1 )) N T1θ1 − 2m2 l22 (sin(N T1θ 1 )cos(N T1θ 1 )) N T1θ1 + m2 l 22 (cos(N T1θ 1 )) N T1θ1 +
2
Equation (A.26)
and
∂L
∂N T1θ1
( ) ( )
= − m2 N T2 rθ2 l2 (cos(N T1θ1 )) N T1θ1 + m2 l 22 (sin (N T1θ1 )cos(N T1θ1 )) N T1θ1 ( )2
( ( ) ( ))( ) (
− m2 l22 sin N T1θ1 cos N T1θ1 N T1θ1 − m3 N T2 rθ2 l1 cos N T1θ1 N T1θ1
2
) ( ( ))( ) (A.27)
τ g1
N T1
( ) ( ( )
= m2 l22 N T1 + m3l12 N T1 + I ZZ 2 N T1 θ1 + − m2 l 2 N T2 r sin N T1θ1 - m3l1 N T2 r sin N T1θ1 θ2 ( ))
Equation (A.28)
Taking the partial and time derivatives of equation (A.22) with respect to xOA = N T2 rθ2 yields:
∂L
∂N T2 rθ 2
( ( )) (
= m1 N T2 rθ2 + m2 N T2 rθ2 − l 2 sin (N T1θ1 ) N T1θ1 + m3 N T2 rθ2 − l1 sin (N T1θ1 ) N T1θ1 ( ))
or
∂L
= (m1 + m2 + m3 )N T2 rθ2 − (sin (N T1θ1 ))(l1m3 + l2 m2 )N T1θ1 (A.28)
∂N T2 rθ2
131
d ∂L
[ ( ) (
= − m2 l2 sin N T1θ1 − m3l1 sin N T1θ1 N T1θ1 + (m1 + m2 + m3 ) N T2 rθ2
dt ∂N T2 rθ2
)]( ) ( )
(A.29)
( ( )
− m2 l 2 cos N T1θ1 + m3l1 cos N T1θ1 N T1θ1 ( ))( )
2
∂L
=0 (A.30)
∂xOA
τ g1
N T2 r
[ ( ) ( )]( ) (
= − m2 l 2 N T1 sin N T1θ1 − m3l1 N T1 sin N T1θ1 θ1 + m1 N T2 r + m2 N T2 r + m3 N T2 r θ2 )( )
( ( )
− m2 l 2 N T21 cos N T1θ1 + m3l1 N T21 cos N T1θ1 θ1 ( ))( ) 2
Equation (A.31)
which represents the force applied to the translation link. The system presented in matrix form is:
τ g1
=
(
m2 l22 + m3l12 + I ZZ 2 N T21 )( ) ( ( ) ( ))( )
− m2 l 2 sin N T1θ 1 + m3l1 sin N T1θ 1 N T1 N T2 r θ1
τ
( ( )
g 2 − m2 l 2 sin N T1θ 1 + m3l1 sin N T1θ 1 N T1 N T2 r ( ))( ) (
(m1 + m2 + m3 ) N T2 r 2 )
θ 2
0
+
( ( )
− m2 l2 cos N T1θ 1 + m3l1 cos N T1θ 1 θ 1
( ))( ) (N
2 2
T1 )
N T2 r
Equation (A.32)
Equation (A.32) represents (after gearing) the torque each motor sees due to the PR manipulator's
link dynamics.
If the generalized equation of links dynamics for a robotic manipulator is expressed as:
then a friction vector similar to then one above could be added to equation (A.32). Gearing
would apply to the friction matrix similar to the method used in equation (A.14) where the TL
term is considered to include a constant Coulomb friction term. However, for this study it is
assumed that the links are frictionless, and friction terms are not included in the dynamically
equations. In Chapter 2 of this thesis, de Wit (1991) et al. claims that this is a reasonable
assumption, and presents a generalized motor friction model that accounts for external joint
friction.
132
In summary, it is obvious that equation (A.32) is dynamically coupled and highly nonlinear.
However, low gearing and proper material selection uncouples the system and substantially
reduces its nonlinearities. Thus, from equations (A.32) and (A.14) it can be seen that low
gearing eliminates the mechanism's dynamics, and leaves only the dynamics of the actuators for
consideration.
133
APPENDIX B
The derivation of the electro-mechanical equation that describes the dynamics of a simple, non-
geared, PM brush commutated d.c. motor is developed in this appendix. Only viscous and
Coulombic friction are considered in the model. In Chapter 2, static friction, loading and gearing
are added to the results generated here.
The electrical equation of a PM d.c. motor is derived from the simple motor circuit illustrated in
Figure B.1 (Electro-Craft, 1980).
R L
Vapplied Iarmature Vb
Tg,ω
where
dI
V = L + RI + Vb (B.1)
dt
where Vb , the internally generated voltage, is proportional to the motor velocity ω and is given
by:
Vb = Kbω (B.2)
The motor back emf constant, Kb, is a measure of the voltage per unit speed generated when the
rotor is turning. The magnitude and polarity of Kb are functions of the shaft angular velocity, ω,
and direction of rotation respectively. Combining the above equations produces:
134
dI
V = L + RI + Kbω (B.3)
dt
T g = Kt I (B.4)
where Tg is the total internally generated torque a motor must deliver in order to overcome
opposing torques. Kt is the motor torque constant which is a measure of the torque per unit
current produced by the motor. In a PM d.c. motor, torque is a linear function of the motor
current, I.
The following terms describe the motor's mechanical properties used in Tg:
The opposing torque seen by the motor due to Coulomb and viscous friction is given by:
dω
Tarm = J1 (B.6)
dt
dω
Tg = J1 + Tc sgn(ω ) + D1ω = Kt I (B.7)
dt
Equation (B.7) describes the mechanical dynamics of a d.c. motor. It neglects the static friction
or "stiction" term, and does not model disturbances or resonant modes of motor couplings.
After describing a motor in terms of its electrical and mechanical characteristics, the next step is
to combine them to form a transfer function. To simplify the derivation, nonlinear Coulomb
friction is not considered.
The derivation begins by considering the motor equations in terms of their Laplace transforms.
135
V(s)=(sL + R) I(s) + Kb ω(s) (B.8)
1
I ( s) =
Kt
[sJ1 + D1 ]ω ( s) (B.11)
1
V ( s) =
Kt
[( sL + R)( sJ1 + D1 )]ω ( s) + Kbω ( s) (B.12)
ω ( s) Kt
Gm = = (B.13)
V ( s) [( sL + R)( sJ1 + D1 ) + Kb Kt ]
The transfer function for iron core permanent magnet DC motors has two real, negative poles.
These poles are the roots of the characteristic equation:
The motor transfer function can be written in terms of time constants τ1 and τ2 or
1 / Kb
Gm = (B.15)
[( sτ 1 + 1)( sτ 2 + 1)]
where the time constants are related to the poles of (B.15) by:
1 1
τ1 = − and τ2 = − (B.16)
p1 p2
136
In most PM d.c. motors, the inductance, L, and the viscous damping, D1, values are small
relative to the other terms in equation (B.17). If their product, LD1 ≈ 0, is considered to be true,
then the term under the radical in (B.17) is always greater than zero. Thus, the poles are negative
real, and are reduced to:
(1 − 2 LKb Kt )
− ( RJ1 ) + RJ1
R 2 J1
p1 =
2 LJ1
or
Kb Kt
p1 = − (B.18)
RJ1
and
(1 − 2 LKb Kt )
− ( RJ1 ) − RJ1
R 2 J1 −2 RJ1
p2 = ≈
2 LJ1 2 LJ1
or
R
p2 = − (B.19)
L
By using the equations in (B.16), the motor's mechanical and electrical time constants can be
stated in terms of motor parameters:
RJ
τ1 = τ m = (Mechanical time constant) (B.20)
Kb Kt
L
τ2 =τe = (Electrical time constant) (B.21)
R
If τ2 is one tenth the magnitude of τ1 , then the motor can be modeled with good success as a
first order system. The transfer function written in terms of velocity for such a system is:
1 / Kb
Gm ( s) = (B.22)
( sτ m + 1)
137
APPENDIX C
The Physical Dimensions of the model system are listed in Table C.1. Refer to Figure
A.2 in Appendix A for manipulator orientation.
The model's motor parameters are listed in Table C.2 (a) and (b). Most of the parameters are
nominal values taking from the manufacturer's data sheets. The values are experimentally
verified in Chapter 5 and restated here in tables C.2 (c) and (d).
(Note: that an additional gear box with a ratio of 15:1 was coupled to this motor.)
138
Table C.2 (b): Model's (translation) motor nominal parameters
139
Table C.2 (d): Model's (translation) motor experimentally verified parameters
The Physical Dimensions of the prototype system are listed in Table C.3. Refer to Figure A.1 in
Appendix A for orientation.
The prototype's motor parameters are listed in Table C.4 (a) and (b). Most of the parameters are
nominal values taking from the manufacturer's data sheets. The values are experimentally
verified in Chapter 5 and restated in tables C.4 (c) and (d).
140
Table C.4 (a): Prototype's (Rotation) motor nominal parameters
Frame size: Length= 32.5 mm; diameter=27.5 mm. Shaft length 51.0 mm
141
Tc Coulomb friction .1407 oz-in
Ts static friction .1850 oz-in
Vref rated voltage 24 volts
IP peak current .915 amps
Is break-out current .078 amps
τm mechanical time constant 34.0 ms
p1 1/τm 29.3982 Hz
NT gear ratio 420.3 none
Kcr rotation angle sensor gain .2301 rad/v
142
The following motor data was used in section 4.3 of Chapter 4. It is the data for the "A"
prototype motor. These terms were not experimentally verified.
Table C.5 : Nominal motor parameters for Prototype A (data used in table 4.2)
143
APPENDIX D
The simulation model presented here is for the complete "Model" system. The Matlab code
used to generate the setpoint references from the trajectory planner algorithm is followed by the
Simulink model which is numerically integrated using the Runge-Kutta method. The Simulink
model includes a closed loop PID control option along with the feedforward inverse model. It is
presented here to provide the reader with a visual reference of the system under study.
The Matlab Code used to generate the trajectory references and to call the Simulink Model is:
% Notes: This version of the traj planner uses a delt=dt of the RK integrator,
% or delta=dt=.0006 sec. Also note that this version takes numerical derivatives of the traj. planner
clear all
xc=17.6;
xoa=10.7;
l=6.9;
144
% Time, I.C., and dt of simulation
for i=1:L
%kinematic conversion
thetasp(i)=asin((ysp(i)-y)/l);
xoasp(i)=xsp(i)-l*cos(thetasp(i))-x;
end
delt=fixstep;
for k=2:L
vthetasp(k)=(thetasp(k)-thetasp(k-1))/delt;
athetasp(k)=(vthetasp(k)-vthetasp(k-1))/delt;
vxoasp(k)=(xoasp(k)-xoasp(k-1))/delt;
axoasp(k)=(vxoasp(k)-vxoasp(k-1))/delt;
end
U=[thetasp',athetasp',vthetasp',xoasp',axoasp',vxoasp'];
145
disp('Start clock')
disp('Stop clock')
The Complete Simulink Model called by the Matlab code is presented next.
+ .5 2
+ theta4
FF + D/A staturation Limit kamp1
PID Limiter Gain 1 theta4''
1 Rot
Vamp1
Thetad Xoa''_
To Workspace Rotational Motor
Traj Rotation Link Dynamics
tau1 (Rot Torque)
2 PID Theta
Thetad'' theta4_
Traj
2 theta4'
3 theta4''_
Limiter Kamp2
Thetad' Xoa''
Traj
Translational Motor Translation Link Dynamics
4 PID Tran Ff (trans link)
Xoad
Traj
Vamp2
5 To Workspace1
+
Xoad'' .5
+
Traj
FF + PID D/A staturation
6 Trans Limiter Gain2
Computed Torque
w/ Friction
Xoad' Compensation
Traj
The following figures are exploded views of the main blocks in Figure D.1. They are presented
in the following order:
1. link dynamics
2. servo motors
3. PID model
4. inverse model
The block diagrams for the rotational and translation links are
146
1 theta4
sin(u[1]) sin(theta4)
* sumx(Xoa'') * sumx(theta4'')
- + Sum2
1 theta4
+ + + Sum3
- - + Sum
T2
Translation Force 1 T2 (Force for Translation link)
147
The block diagram representation of the translational motor is:
x''
2 alpha2 du/dt 1
Trans alpha2 to wksp1 Derivative xoa''
Torq -
+ a w wg x'
f(u) + f(u) 1/s f(u) .334645 y1b
-
-
Sum Kt/R 1/Jo Integrator1 N xoa', geared
Kt/(sL+R) R equiv
Sum2
1
Vamp 1/s
x
.0217 omega2 Integrator
f(u) omega to wksp
Kb +
D1
T2a y2b
+ .2370
Trans Torque Load xoa, geared
+ Tc Sign
T2gear Sum1
Trans Torque Load1 *
f(u)
Note that the motor model includes an inductance term, making it a second order system. Also
note that the stiction term is modeled with the Stribeck effect. All torque terms are summed at a
mutual junction point. For all practical purposes, this is the most comprehensive model available
for a PM brushed commutated d.c. motor.
The following figure represents the common PID controller which functions by comparing the
desired position reference signal generated from the trajectory planner to a position feedback
signal. The error signal is differentiated and integrated and multiplied by the appropriate gains.
e2_cmd
error command
.03953
1 Kp (Kb)
+
in_1 -
1 f(u) du/dt + 1
+
K Kd Derivative + out_1
2 Sum1 Sum
in_2 1
2
s
Integrator Ki
148
1 f(u)
a (1/N)(1/req)
Mux f(u) +
1
(JR/Kt)a+(Kb)w +
Out to PID
Mux1 Sum1 Sum Jcn
2 f(u) f(u) +
w (1/N)(1/req)1 .845(Fv)(R/Kt) +
+
f(u)
Sum2 xoaff
w2g Sign1 .9728(Fc)(R/Kt)
To Workspace1
omega 2 gear
f(u) *
Product fcomp2
Pulse deltaFs, (R/Kt)
Stiction Trq Friction Compensation
at start of run output variable for plotting
Note that the inverse model in Figure D.6 only represents the translation servo motor dynamics
and uses the cut-off stiction model rather than the Stribeck model. This model also neglects the
inductance term. Because of the low gearing and fast electrical response of the PM motors, this
inverse model is shown to be appropriate in the simulations.
149
APPENDIX E
The following control code was used for simulating the model and prototype systems. This
controller does not use position feedback. The dimensions and motor parameters used in the
Matlab code and Simulink model are specified for the "model" system. The Matlab code pre-
computes the entire command reference signal using a sample period of 10 ms, and places it in
an array which is passed to the Simulink model. This model is numerically integrated using a
third order Runge-Kutta method at a sample period of .6 ms. The Simulink model only contains
the "real" world analog components of this system. The Matlab code presented here simulates
the digital controller.
dt=.01 % Simulated sample period for computed torque control. Matches experimental sample period.
tref=3.6 % Nominal Setpoint time
%% Develop The Traj Planner and Model Ref Commands for Simulink
%% Simulation System-"Model" %%
% motor parameters
% Theta Motor
J_th=.00039;
R_th= 8.5;
Kt_th=5.6;
Kb_th=.03953;
Fv_th=1.07927e-4;
Fc_th= .56;
Fs_th=1.58111;
N1=127.78*15;
% Xoa Motor
J_x=.00039;
150
R_x= 8.4;
Kt_x=5.6;
Kb_x=.03953;
Fv_x=1.07927e-4;
Fc_x=.5376;
Fs_x=1.4667;
N2=127.78*4.2333418;
% Load Some of the Experimental Ref Data to be used as initial conditions for Simulations
load c:\thesis\testdata\data9413\posth.txt;
load c:\thesis\testdata\data9413\posx.txt;
thetal=posth(1); %This should be the avg of the 1st sample a/d period
xoal=posx(1);
clear posth;
clear posx;
%%%%%%%%%%%%%%%%%%%%%
%%%% Set Point Programmer %%%%
%%%%%%%%%%%%%%%%%%%%%
for i=1:L
151
% set point programmer generator
%kinematic conversion
thetasp(i)=asin((ysp(i)-y)/l);
xoasp(i)=xsp(i)-l*cos(thetasp(i))-x;
end
delttheta=max(thetasp)-min(thetasp)
deltxoa=max(xoasp)-min(xoasp)
for k=2:L
vthetasp(k)=(thetasp(k)-thetasp(k-1))/dt;
athetasp(k)=(vthetasp(k)-vthetasp(k-1))/dt;
vxoasp(k)=(xoasp(k)-xoasp(k-1))/dt;
axoasp(k)=(vxoasp(k)-vxoasp(k-1))/dt;
end
%%%%%%%%%%%%%%%%%%%%%%%
%%% Computed Torque Reference %%%%%
%%%%%%%%%%%%%%%%%%%%%%
vthetasp=N1*vthetasp;
athetasp=N1*athetasp;
vxoasp=N2*vxoasp;
axoasp=N2*axoasp;
for i=1:L
if vthetasp(i)==0
sgn_th=0;
else
sgn_th=abs(vthetasp(i))/vthetasp(i) ;
end
if vxoasp(i)==0
sgn_x=0;
else
152
sgn_x=abs(vxoasp(i))/vxoasp(i) ;
end
%Theta Motor
u1a=((J_th*R_th)/Kt_th)*athetasp(i)+ Kb_th*vthetasp(i);
u2a=((Fv_th*R_th)/Kt_th)*vthetasp(i);
u3a=sgn_th*(Fc_th*R_th)/Kt_th;
if i<5
u4a=((Fs_th-Fc_x)*(R_th/Kt_th))*sgn_th;
else
u4a=0;
end
uths(i)=u1a+u2a+u3a+u4a;
%Xoa Motor
u1b=((J_x*R_x)/Kt_x)*axoasp(i)+ Kb_x*vxoasp(i);
u2b=((Fv_x*R_x)/Kt_x)*vxoasp(i);
u3b=sgn_x*(Fc_x*R_x)/Kt_x;
if i<4
u4b=((Fs_x-Fc_x)*(R_x/Kt_x))*sgn_x;
else
u4b=0;
end
uxs(i)=u1b+u2b+u3b+u4b;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Start of Simulink Simulation %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
uths=.5*uths;
uxs=.5*uxs;
U=[uths',uxs'];
153
theta0=thetasp(1);
xoa0=xoasp(1);
X0=[theta0,0,0,0,xoa0,0]';
disp('Stop clock')
% Note on variables returned from integration: y1a(theta4dot), y2a(theta4) are from the rot motor;
% y1b(xoadot), and y2b(xoa) are from the translation motor
th0=y2a(1)
thdot0=omega1(1)
thdotdot=alpha1(1)
x0=y2b(1)
xdot0=omega2(1)
xdotdot=alpha2(1)
dxoasim=y2b;
theta4=y2a;
m=length(theta4);
for k=1:m,
Yg(k,1)=l*sin(theta4(k,1));
Xg(k,1)= dxoasim(k,1) + l*cos(theta4(k,1));
end
minYg=min(Yg)
minXg=min(Xg)
Xnew=xc-min(Xg(:,1))
Ynew=0-minYg
errX_pu=(x-Xnew)/x
errY_pu=(y-Ynew)/y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% End of Simulation code %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
154
load c:\thesis\testdata\data9413\ch2.txt; %Xoa control signal
load c:\thesis\testdata\data9413\ch3.txt; %Xoa psotion feedback signal
load c:\thesis\testdata\data9413\uth.txt; % Theta mod ref command in volts
load c:\thesis\testdata\data9413\ux.txt; % Xoa mod ref command in volts
load c:\thesis\testdata\data9413\posth.txt; % Theta Pos mod ref command in rads
load c:\thesis\testdata\data9413\velth.txt; % Theta Vel mod ref command in rads/s
load c:\thesis\testdata\data9413\posx.txt; % Xoa Pos mod ref command in inches
load c:\thesis\testdata\data9413\velx.txt; % Xoa Vel mod ref command in inches/s
load c:\thesis\testdata\data9413\ax.txt; % Xoa accel mod ref command in inches/s
load c:\thesis\testdata\data9413\ath.txt; % theta accel mod ref command in inches/s
% Time and Delta T used for Experimental Traj Planner Calc to creat time array
dt_exp=.01;
ls=length(uth);
Ts=dt*ls;
ts=[0:dt_exp:Ts-dt_exp];
% Conversion Gains
Kc1=.2667; %rad/v gain on rot pot, New gear train
Kc2=.7591; %in/v gain on tran pot, new measure 10-13-97
%%%%%%%%%%%
%% Plot Results %%
%%%%%%%%%%%
disp('')
disp(' Compare Trajectory Calculations between Matlab and C code')
uths=2*uths;
uxs=2*uxs;
155
figure(8),plot(tsp,uxs,'r.',ts,ux,'y.'),grid,title('Uxoa Matlab vs C code '),zoom on
disp('')
disp(' Compare Simulink System Response to Exper: Mod Ref and Actual System Resp')
uth=.5*uth;
ux=.5*ux;
figure(11),plot(t1,ch0,t1,ch1),grid,zoom on,
title('Exp:Theta (Ch0) Cmd vs Pos (Ch1) Feedback'),xlabel('t,sec'),ylabel('Volts')
figure(12),plot(t1,ch2,t1,ch3),grid,zoom on,
title('Exp: Xoa (Ch2) Cmd U vs Pos (Ch3) Feedback'),xlabel('t,sec'),ylabel('Volts')
figure(13),plot(ts,posth,t1,ch1k,'r'),grid,
title('Exp: Pos theta Traj Plan vs Exp'),xlabel('t,sec'),ylabel('rads'),zoom on
figure(14),plot(t1,ch1k,t,y2a,'r'),grid,
title('Exp: Theta Pos Fdbk vs Simulink Model Fdbk '),xlabel('t,sec'),ylabel('rads'),zoom on
figure(15),plot(ts,posx,t1,ch3k,'r'),grid,
title('Exp: Pos Xoa Traj Plan vs Exp'),xlabel('t,sec'),ylabel('inches'),zoom on
figure(16),plot(t,y2b,t1,ch3k,'r'),grid,
title('Exp: Xoa Pos Fdbk vs Simulink Model'),xlabel('t,sec'),ylabel('inches'),zoom on
figure(17),plot(t,y2b,tsp,xoasp,'r'),grid,
title('Exp: Xoa Simulink Model Pos ref vs Pos Fbk'),xlabel('t,sec'),ylabel('inches'),zoom on
156
Vamp1
To Workspace
theta4
1 2
Uth Cmd Limiter_ kamp1 theta4''
Xoa''_
Rotational Motor Rotation Link Dynamics
tau1 (Rot Torque)
theta4_
2 2 theta4'
Ux Cmd Limiter theta4''_
Kamp2
Xoa''
Translational Motor Translation Link Dynamics
Ff (trans link)
Vamp2
To Workspace1
Note that this model is numerically integrated using a third order Runge-Kutta method at a
sample period of .0006 seconds. The digital trajectory planner and the inverse model used a
sample period of .01 seconds.
157
APPENDIX F
The following control code was used in a PC based controller. It was written and compiled in
Borland 3.1 C for DOS. Calls are made to function routines written by ComputerBoards Inc. that
perform the A/D, D/A signal conversion. Although this code shows parameters for the prototype
system, the same program was used to run the model system. Only the physical dimensions,
setpoints and motor parameters were changed for the model version. Both systems were tuned
by accurately identifying their motor parameters, with the static and Coulomb friction terms
being the most important. This controller does not use feedback error correction.
/**************************************************************************
Program Description:
This is an Open Loop program that produces a polynomial trajectory path for a
two link two motor planar robot. User provides position reference
in the local coordinate system. Program uses the traj path to compute the so called
"Computed Torque Control Law" (Craig, 1989)." 2 control channels
and 2 position feedback channels are read in while
an 8254 counter timer paces the A/D input and D/A output. Sample Rate is 100 hz.
**************************************************************************/
/* Include files */
#include <stdio.h>
#include <conio.h>
#include <dos.h>
#include <time.h>
#include <cb.h>
#include <math.h>
#include <stdlib.h>
void main ()
{
/* Variable Declarations for Control and A/D Boards*/
158
float dt=.01; /* sec; Stated free run DAC Sample Period */
int N; /*Number of Loops through Control Array*/
int CounterNum;
int RegName;
int Config;
int LoadValue;
unsigned Countn;
/* Motor Parameters */
/* Theta Motor */
float J_th=8.3711e-5;
float R_th=23.25;
float Kb_th=.02;
float Kt_th=141.623*Kb_th;
float Fv_th=1.0e-5;
float Fc_th=.147; /*(.19 to sustain bkout) Torque Coulombic Friction */
float Fs_th=.22437; /* Torque break out */
float N1_th=420.3; /* Gearing gain */
float N4_th=1; /* (No) Extra Gearing for Rotation */
/* Xoa Motor */
float J_x=8.3711e-5;
float R_x=23.25;
float Kb_x=.0217;
float Kt_x=141.623*Kb_x;
float Fv_x=1.0e-5;
float Fc_x=.237;
float Fs_x=.3319;
float N1_x=420.3;
float req=2.988235; /* inverse of 1/.3346456=1/[(17/2mm)*(1/25.4)] equiv radius, inches */
int i=0;
159
int j=0;
int BoardNum1 = 1;
int ULStat;
int UDStat;
int Gain = BIP5VOLTS;
unsigned *cmd1;
unsigned *thfbk;
unsigned *cmd2;
unsigned *xfbk;
unsigned DataVal0; /* DataVal0 is temp var which is passed data from *cmd1 */
unsigned DataVal1;
unsigned DataVal2;
unsigned DataVal3;
int BoardNum0 = 0;
float EngUnits=0;
unsigned DataVal;
long Rate_DA, Count_DA;
int Options_DA;
float motact=0; /* 4094 = 1 pu= 5V, 0=-5V, 2047=0V, control output to analog amp */
float motactx=0; /* 4094 = 1 pu= 5V, 0=-5V, 2047=0V, control output to analog amp */
unsigned DataValue; /* Used for conversion for motact to counts */
unsigned DataValuex;
160
float sgn_th=1; /* Variable to use for sign function on theta */
float sgn_x=1; /* Variable to use for sign function on Xoa */
float xsp=0;
float ysp=0;
float thetal=0; /* The last theta, or starting angle */
float theta=0; /* New theta */
float xoal=0; /* The last xoa, or starting translation position */
161
FILE *stream7; /* output file for ADData */
FILE *stream8; /* output file for ADData */
FILE *stream9; /* output file for ADData */
FILE *stream10; /* output file for ADData */
FILE *stream11; /* output file for ADData */
vel1=malloc((N+1)*sizeof(double));
if (!vel1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for vel1 Variable\n");
exit(1);
}
a1=malloc((N+1)*sizeof(double));
if (!a1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for a1 Variable\n");
162
exit(1);
}
uth=malloc((N+1)*sizeof(double));
if (!uth) /* Make sure it is a valid pointer */
{
printf("\nout of memory for uth Variable\n");
exit(1);
}
pos2=malloc((N+1)*sizeof(double));
if (!pos2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for pos2 Variable\n");
exit(1);
}
vel2=malloc((N+1)*sizeof(double));
if (!vel2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for vel2 Variable\n");
exit(1);
}
a2=malloc((N+1)*sizeof(double));
if (!a2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for a2 Variable\n");
exit(1);
}
ux=malloc((N+1)*sizeof(double));
if (!ux) /* Make sure it is a valid pointer */
{
printf("\nout of memory for ux Variable\n");
exit(1);
}
U0 = malloc((N+1)*sizeof(unsigned));
if (!U0) /* Make sure it is a valid pointer */
{
printf("\nout of memory for U0 variable\n");
exit(1);
}
U1 = malloc((N+1)*sizeof(unsigned));
if (!U1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for U1 variable\n");
exit(1);
}
cmd1 = malloc((N+1)*sizeof(unsigned));
163
if (!cmd1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for cmd1 Variable\n");
exit(1);
}
thfbk = malloc((N+1)*sizeof(unsigned));
if (!thfbk) /* Make sure it is a valid pointer */
{
printf("\nout of memory for thfbk variable\n");
exit(1);
}
cmd2 = malloc((N+1)*sizeof(unsigned));
if (!cmd2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for cmd2 Variable\n");
exit(1);
}
xfbk = malloc((N+1)*sizeof(unsigned));
if (!xfbk) /* Make sure it is a valid pointer */
{
printf("\nout of memory for xfbk variable\n");
exit(1);
}
CH0 = malloc((N+1)*sizeof(float));
if (!CH0) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH0 Variable\n");
exit(1);
}
CH1 = malloc((N+1)*sizeof(float));
if (!CH1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH1 variable\n");
exit(1);
}
CH2 = malloc((N+1)*sizeof(float));
if (!CH2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH2 Variable\n");
exit(1);
}
CH3 = malloc((N+1)*sizeof(float));
if (!CH3) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH3 variable\n");
exit(1);
}
164
/* Initialize the pointed arrays */
i=0;
/* Theta Terms */
*(pos1 + i)=0.0;
*(vel1 + i)=0.0;
*(a1 + i)=0.0;
*(uth+ i)=0.0;
*(uth+ i+1)=0.0;
/* Xoa Terms */
*(pos2 + i)=0.0;
*(vel2 + i)=0.0;
*(a2 + i)=0.0;
*(ux+ i)=0.0;
*(ux+ i+1)=0.0;
/*************************************************************/
/**** END Variable Declarations ********************/
/*************************************************************/
/* EngUnits=-1.3;
ULStat = cbFromEngUnits(0, Gain, EngUnits, &DataValue);
ULStat = cbAOut (0, 0, Gain, DataValue);
delay(0);
EngUnits=0;
ULStat = cbFromEngUnits(0, Gain, EngUnits, &DataValue);
ULStat = cbAOut (0, 0, Gain, DataValue); */
/* Read in present Analog coordinates theta and xoa as the starting point */
/* Loop through and average before taking points for I.C. */
for (i=1;i<2500;i++)
{
UDStat = cbAIn(1, 1, BIP5VOLTS, &ch1fbk); /* Get position feedback on theta*/
UDStat = cbToEngUnits (1,BIP5VOLTS, ch1fbk, &EngUnits1);/* Convert Pos fbk to Eng units */
posfbksum=posfbksum+EngUnits1;
165
posfbksumx=posfbksumx+EngUnits3;
}
xpo=xoal + l*cos(thetal) + x;
ypo=l*sin(thetal) + y;
if (y==0)
{
ypo=0;
}
for (i=0;i<N;i++)
{
166
for (i=0;i<N;i++)
{
*(vel1+ i+1)=(*(pos1+ i+1)-*(pos1+ i))/dt; /* theta dot */
}
/* End Trajectory Planner */
for (i=0;i<N;i++)
{
if (*(vel2+ i)==0)
sgn_x=0;
else
sgn_x=fabs(*(vel2+ i))/(*(vel2+ i)); /* sign for xoa */
/* Theta Motor */
u1a=((J_th*R_th)/Kt_th)*(*(a1+ i)) + Kb_th*(*(vel1+ i)); /* inverse dynamics */
u2a=((Fv_th*R_th)/Kt_th)*(*(vel1+ i)); /* viscous damping */
u3a=sgn_th*((Fc_th*R_th)/Kt_th); /* Coulomb Friction */
if (i<4)
u4a=((Fs_th-Fc_th)*(R_th/Kt_th))*(sgn_th); /* Step for stiction for 2 dt's */
else
u4a=0;
*(uth+ i)=u1a + u2a + u3a + u4a; /* command out put, Xoa full scale */
/* Xoa Motor */
u1b=((J_x*R_x)/Kt_x)*(*(a2+ i)) + Kb_x*(*(vel2+ i)); /* inverse dynamics */
u2b=((Fv_x*R_x)/Kt_x)*(*(vel2+ i)); /* viscous damping */
167
u3b=sgn_x*((Fc_x*R_x)/Kt_x); /* Coulomb Friction */
if (i<4)
u4b=((Fs_x-Fc_x)*(R_x/Kt_x))*sgn_x; /* Step for stiction for 2 dt's */
else
u4b=0;
*(ux+ i)=u1b + u2b + u3b + u4b; /* command out put, Theta full scale */
printf("Saving Trajectory Planner and Model Ref Commands for Th and X\n");
for (i=0; i<N; i++)
{
fprintf(stream4, "%f\n", *(uth + i));
fprintf(stream5, "%f\n", *(ux + i));
fprintf(stream6, "%f\n", *(pos1 + i));
fprintf(stream7, "%f\n", *(vel1 + i));
fprintf(stream8, "%f\n", *(pos2 + i));
fprintf(stream9, "%f\n", *(vel2 + i));
fprintf(stream10, "%f\n", *(a1 + i));
fprintf(stream11, "%f\n", *(a2 + i));
/* Convert Voltage command arrays to binary Counts, *.5 for amp gain */
for(j=0;j<N;j++)
{
motact=(*(uth+j))*.5; /* reduce by .5 & Store theta output to temp float var */
motactx=(*(ux+j))*.5; /* reduce by .5 & Store X output to temp float var */
168
ULStat = cbFromEngUnits(BoardNum0, BIP5VOLTS, motact, &DataValue);
ULStat = cbFromEngUnits(BoardNum0, BIP5VOLTS, motactx, &DataValuex);
U0[j]=DataValue;
U1[j]=DataValuex;
free(pos1);
free(vel1);
free(a1);
free(pos2);
free(vel2);
free(a2);
free(uth);
free(ux);
/****************************************************************/
/* D to A Configuration */
/****************************************************************/
Rate_DA = NOTUSED;
Count_DA = 2;
Options_DA = 0;
/*************************************************************
***** 8254 Counter Configurations *************************
**************************************************************/
/* Config Counter
Parameters:
BoardNum# :the number used by CB.CFG to describe this board
CounterNum :the counter to be setup
Config :the operation mode of counter to be configured */
169
RegName = LOADREG3;
/***************************************************************
******* Begin Control DAC out sequence *************************
****************************************************************/
for(j=0;j<N;j++)
*(cmd1+j)=ch0fbk;
*(thfbk+j)=ch1fbk;
*(cmd2+j)=ch2fbk;
*(xfbk+j)=ch3fbk;
dac_out[0]=U0[j];
dac_out[1]=U1[j];
ULStat=cbAOutScan(BoardNum0,0,1,Count_DA,&Rate_DA,Gain,dac_out,Options_DA);
while (Countn>350)
{
ULStat = cbCIn (BoardNum1, CounterNum, &Countn);
end = clock();
printf("\nThe time for Run according to DOS System clock was: %f\n", (end - start) / CLK_TCK);
170
/* Convert Points in Array to Engineering Units (Volts) and Save */
for (i = 0; i < N; i++) /* loop through the ADData array and convert */
{
DataVal0=*(cmd1 +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal0,&EngUnits0);
*(CH0 +i)=EngUnits0;
DataVal1=*(thfbk +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal1,&EngUnits1);
*(CH1 +i)=EngUnits1;
DataVal2=*(cmd2 +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal2,&EngUnits2);
*(CH2 +i)=EngUnits2;
DataVal3=*(xfbk +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal3,&EngUnits3);
*(CH3 +i)=EngUnits3;
fprintf(stream0,"%f\n",*(CH0+i));
fprintf(stream1,"%f\n",*(CH1+i));
fprintf(stream2,"%f\n",*(CH2+i));
fprintf(stream3,"%f\n",*(CH3+i));
/* Free Arrays */
free(cmd1);
free(thfbk);
free(cmd2);
free(xfbk);
free(CH0);
171
free(CH1);
free(CH2);
free(CH3);
}
/*******************************************************
************* END OF PROGRAM ************************
********************************************************/
172
VITA
173