Robotics

HIL Implementation of
Harmonic Drive Motor

Part I: Discretization and Simulation

One approach quite common in industrial control nowadays, when doing analysis
and design with a real plant is expensive, is to implement the plant model on a
computer, usually an embedded system. Some advantages of this so-called
Hardware-In-the-Loop (HIL) approach, in addition to lowering development cost
and time, are flexibility in adjusting the parameters, and the ability to perform
extreme experiments which might damage the real plant.
From our previous articles on robot joint driven by DC motor with harmonic drive
mechanism (from now I will abbreviate it as HDM for short) , we obtain a linear
model and simulate with PID and custom-designed controllers. Here we continue
our discussion on how to implement this math model on a real-time embedded
system, to be used as an HIL plant for feedback control analysis and design.
To be honest, this is my first HIL project that is based on a math model of a physical
plant, except for some simple memoryless systems. Certain factors, both hardware
and software related, such as ADC and PWM accuracy, delay, fixed-point math and
other numerical issues, could affect the performance of this embedded system. With
limited resources, at the end I am not so sure it could resemble the real plant good
enough to warrant its place in a control lab. Nevertheless, it is worth a try. If things
work out properly, it could be an interesting topic for my control engineering
students for the upcoming semester. If not, I believe we learn something together in
such attempt. ;)
The proposed procedure is as follows
1. Discretize the continuous-time transfer function of the HDM
2. Simulate the discrete-time plant and compare the responses with continuous-
time model
3. Adjust overall plant gain to suit the input and output limits of embedded
system
scilab.ninja
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

4. Implement on the selected microcontroller (in this case, a PIC24EP product

from Microchip)
5. Experiment with the HIL model
Part I of this material covers step 1 – 3. The rest is left to future article(s).

Discretization

From the HDM block diagram derived from our previous article and displayed again
in Figure 1, we can manipulate to achieve a reduced block diagram in Figure 2,
consisting of only 2 sub-blocks
kmk
P1 ( s )  (1)
a c 5 s  a c 4 s  a c 3 s 3  a c 2 s 2  a c1 s
5 4

rPl ( s )
P2 ( s )  (2)
k

Figure 1 block diagram of HDM plant

Figure 2 reduced block diagram of HDM plant

with
a c 5  LJ m J l
a c 4  RJ m J l  L( J m Bl  J l B m )
a c 3  L( J m k  B m Bl  J l k )  R ( J m Bl  J l Bm )  rk m k b J l (3)
a c 2  Lk ( Bm  Bl )  R ( J m k  B m Bl  J l k )  rk m k b Bl
a c1  k ( Lk  R ( Bm  Bl )  kL  rk m k b )
2
 Robotics

and
Pl ( s )  J l s 2  Bl s  k (4)
The reduction process is straightforward but quite tedious. We leave the details to
the reader. Note that P2(s) is not proper, so this diagram is not suitable for
continuous-time simulation. However, we can use this structure conveniently in the
discrete-time domain.
There are a couple of discretization methods. In our experiment we exploit the so-
called bilinear, or Tustin method. The easiest way to implement is to rely on Scilab
or MATLAB to discretize the plant for a particular set of parameters, and then hard-
code the results in the C-program (or write some online coefficient updating
function). The drawback is you need to redo the discretization offline each time a
parameter is changed.
So here we take the pain to derive discrete-time transfer function coefficients as
functions of physical parameters. By using the bilinear relationship
2 ( z  1)
s (5)
T ( z  1)
with sampling period T, we eventually have the discrete-time representation of P1
and P2 as
k m k (b1z 5 z 5  b1 z 4 z 4  b1z 3 z 3  b1 z 2 z 2  b1z1 z  b1 z 0 )
P1 ( z )  (6)
a1 z 5 z 5  a1 z 4 z 4  a1 z 3 z 3  a1z 2 z 2  a1z1 z  a1 z 0

r (blz 2 z 2  blz1 z  blz 0 )

P2 ( z )  (7)
k ( a lz 2 z 2  a lz1 z  a lz 0 )
with
a1z 5  32a c 5  16Ta c 4  8T 2 a c 3  4T 3 a c 2  2T 4 a c1
a1z 4  160a c 5  48Ta c 4  8T 2 a c 3  4T 3 a c 2  6T 4 a c1
a1z 3  320 a c 5  32Ta c 4  16T 2 a c 3  8T 3 a c 2  4T 4 a c1
a1z 2  320a c 5  32Ta c 4  16T 2 a c 3  8T 3 a c 2  4T 4 a c1
a1z1  160 a c 5  48Ta c 4  8T 2 a c 3  4T 3 a c 2  6T 4 a c1
a1z 0  32a c 5  16Ta c 4  8T 2 a c 3  4T 3 a c 2  2T 4 a c1

b1 z 5  T 5 , b1z 4  5T 5 , b1z 3  10T 5 , b1z 2  10T 5 , b1z1  5T 5 , b1z 0  T 5

a lz 2  T 2 , a lz1  2T 2 , a lz 0  T 2 ,
(8)
blz 2  4 J l  2 Bl T  kT 2 , blz1  2 kT 2  8 J l blz 0  4 J l  2 Bl T  kT 2

3
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

Once again, the routine work is left as an exercise.

Verification with Scilab

Before doing anything else, it makes sense to verify that our derivation of the
discrete-time transfer functions (6), (7) and their coefficients (8) are correct. Here
we show how to verify (6) using discretization command in Scilab. You can use
similar process for (7).
Note: all Scilab commands listed below are included in the script file hdminit.sce
Given a set of parameters, we construct the continuous-time plant as before.
km = 100; // torque constant
kb = 1; // back EMF constant
k = 1000; // torsional stiffness of harmonic drive
r = 10; // gear ratio
L = 0.1 // armature inductance
R = 1; // armature resistance
Jm = 1; // motor inertia
Bm = 0.01; // motor shaft friction
Jl = 3; // load inertia
Bl = 0.05; // load friction
// plant denominator coefficient
ac5 = L*Jm*Jl;
ac4 = R*Jm*Jl+L*(Jm*Bl + Jl*Bm);
ac3 = L*(Jm*k + Bm*Bl + Jl*k) + R*(Jm*Bl + Jl*Bm) + r*km*kb*Jl;
ac2 = L*k*(Bm+Bl)+R*(Jm*k + Bm*Bl + Jl*k) + r*km*kb*Bl;
ac1 = k*(L*k + R*(Bm+Bl) - k*L + r*km*kb);

s=poly(0,'s');
pl = Jl*s^2+Bl*s+k;
pm = Jm*s^2+Bm*s+k;
// transfer function from V to load angle
pnum1 = km*k;
pden1 = ac5*s^5 + ac4*s^4 + ac3*s^3 + ac2*s^2 + ac1*s;;

P1 = syslin('c',pnum1,pden1);

Executing the above script yields the continuous-time transfer function

33333.33
P( s)  (9)
s ( s  0.0483s  11022.313)( s 2  0.9783s  302.435)
2

Specify the sampling period of discrete-time system, say, 10 milliseconds.

T = 0.01;

To use the Scilab command cls2dls to discretize the plant, first it must be described
in state-space form
P1ss = tf2ss(P1);
4
 Robotics

Then put this plant and sampling time as arguments to cls2dls, which outputs a
discrete-time state space system using Bilinear transformation.
Pd1ss = cls2dls(P1ss, T);
For comparison with our hand computation, we prefer a discrete-time transfer
function. There is some other way to do this, but here we simply use the control
textbook relationship P ( z )  C ( zI  A) 1 B  D .This is done in Scilab as follows
[Ad,Bd,Cd,Dd]=abcd(Pd1ss);
z = poly(0,'z');
Pd1 = Cd*inv(z*eye(size(Ad,1),size(Ad,2)) - Ad)*Bd + Dd;

Running the script in Scilab yields

-->Pd1
Pd1 =

2 3 4 5
0.0000008 + 0.0000039z + 0.0000078z + 0.0000078z + 0.0000039z + 0.0000008z
--------------------------------------------------------------------------
2 3 4 5
- 0.9224925 + 3.8350285z - 6.9849487z + 7.1298534z - 4.0574407z + z
Now we construct another discrete-time plant from our hand calculation.
// numerator coefficients of p1(z)
b1z5 = T^5;
b1z4 = 5*T^5;
b1z3 = 10*T^5;
b1z2 = 10*T^5;
b1z1 = 5*T^5;
b1z0 = T^5;

// denominator coefficients of p1(z)

a1z5 = 32*ac5 + 16*T*ac4 + 8*T^2*ac3 + 4*T^3*ac2 + 2*T^4*ac1;
a1z4 = -160*ac5 - 48*T*ac4 - 8*T^2*ac3 + 4*T^3*ac2 + 6*T^4*ac1;
a1z3 = 320*ac5 + 32*T*ac4 - 16*T^2*ac3 - 8*T^3*ac2 + 4*T^4*ac1;
a1z2 = -320*ac5 + 32*T*ac4 + 16*T^2*ac3 - 8*T^3*ac2 - 4*T^4*ac1;
a1z1 = 160*ac5 - 48*T*ac4 + 8*T^2*ac3 + 4*T^3*ac2 - 6*T^4*ac1;
a1z0 = -32*ac5 + 16*T*ac4 - 8*T^2*ac3 + 4*T^3*ac2 - 2*T^4*ac1;

// ----- p1(z) -------------------

p1numz = km*k*(b1z5*z^5 + b1z4*z^4 + b1z3*z^3 + b1z2*z^2 + b1z1*z + b1z0);
p1denz = a1z5*z^5 + a1z4*z^4 + a1z3*z^3 + a1z2*z^2 +a1z1*z + a1z0;

To make a comparison, notice that the coefficient of highest z in the denominator of

Pd1 is normalized to 1, but in p1denz it equals alz5. So we have to normalize both
p1numz and p1denz with this quantity. Using syslin command for this discrete-time
plant yields the same transfer function as the one computed by Scilab cls2dls
command.
-->Pd1h = syslin('d',p1numz/a1z5,p1denz/a1z5)
Pd1h =

2 3 4 5
0.0000008 + 0.0000039z + 0.0000078z + 0.0000078z + 0.0000039z + 0.0000008z
--------------------------------------------------------------------------
2 3 4 5
- 0.9224925 + 3.8350285z - 6.9849487z + 7.1298534z - 4.0574407z + z
and this should make us happy for the moment.
5
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

Step Response Simulation

To see how well the resulting discrete-time transfer functions (6) and (7) represent
their continuous-time counterparts, we must simulate the two and compare their
responses. Before running any simulation model below, first you need to execute the
script hdminit.sce to initialize all the variables used in the diagram.
Figure 3 shows an Xcos diagram pid_lf_cdcompare.zcos, a PID control comparison
with load angle as feedback. The continuous time and discrete time plants are placed
in the upper and lower feedback loops, respectively. So the upper loop is purely
continuous system, while the lower loop is a hybrid system (continuous-time PID
controller, discrete-time plant).
Note also that an LPF transfer function, with pole well above closed-loop
bandwidth, is inserted in the feedback path of the hybrid system. This effectively
prevents algebraic loop error during simulation.
This LPF does have its usage in our real implementation later on. The output
variables may be sent out as PWM signals, which require simple RC circuits as LPF
before feeding to an A/D of the controller.

Figure 3 [pid_lf_cdcompare.zcos] PID control with load angle feedback

Running the simulation with PID gains: kp = 4, ki = 1, kd = 0.5, yields the step
responses in Figure 4. The responses from two feedback systems closely matches,
though that of the discrete-time plant (red) tends to oscillate more. This oscillation is
more pronounced when the controller gains are increased.

6
 Robotics

Figure 4 step responses from model in Figure 3

We also construct a comparison model for the case of using motor angle feedback,
as shown in Figure 5. The responses from PID gains: kp = 5, ki = 2, kd = 1 are shown
in Figure 6.

Figure 5 [pid_mf_cdcompare.zcos] PID control with motor angle feedback

7
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

Figure 6 step responses from model in Figure 5

Adjusting Plant Gain to Embedded Hardware

So far we have developed a discrete-time model quite ready to be implemented on a
microcontroller. There is one issue left to be considered, and is fundamentally
related to the properties of hardware used.
We ask a simple question. How can you be sure that the plant gain is comparable
with the range of input and output of your hardware? The extreme cases are too high
gain that easily causes output saturation, or too small gain that requires too excessive
input to drive the system than the controller could deliver. So we need to calibrate
the plant gain to take advantage of the full input and output ranges.
For the plant transfer functions described by (6),(7), the plant gain is a complicated
function of many parameters. In fact, due to the presence of integrator the DC gain is
infinity. So if we simulate an open-loop step response, the motor does not settle at a
value but continues to rotate. This makes gain adjustment somewhat tricky.
Nevertheless, it is not too difficult when we realize that, in a real motor, a property
that is already available as standard specs is the velocity, commonly specified in
Revolution Per Minute (RPM).
In a simulation diagram, motor velocity can be computed by adding a derivative
block as shown in Figure 7. Note in the diagram that we “measure” both l and m,
and their derivatives. It is however only m, the motor shaft velocity, that we want to
specify.
For calibration purpose, we add an amplifier gain block kv to the HDM subblock as
in Figure 8. This in effect makes the overall plant gain independently adjustable.

8
 Robotics

Figure 7 [speedtuning.zcos] Xcos diagram for open-loop gain adjustment

Figure 8 adding an amplifier block kv for overall gain adjustment

To show how to compute kv, let us add some concrete parameter values. Suppose the
resolutions of input ADC and output PWM are 10 and 12 bits, respectively. The
maximum velocity rating for this motor is represented by Smax (RPM).
From Figure 2, with new gain block kv added at the input, the transfer function from
V to m equals

9
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

 m (s) 1 k v k m r ( J l s 2  Bl s  k ) 
   (10)
V ( s ) s  a c 5 s  a c 4 s  a c 3 s  a c 2 s  a c1 
4 3 2

Note that the term in parenthesis represents the transfer function from input voltage
to motor shaft velocity. Let m represent output rate of change per second, then at
DC we have
k v k m rk
m  V (11)
a c1
At maximum input, we want the motor to rotate at maximum speed, and translate
this to maximum output. We also have to take into consideration the motor sense,
clockwise or counter-clockwise. So the range for each direction is half of the input
range, say, for ADC input value of 511, the motor rotates at Smax RPM, and the
PWM maximum output is 4095.
Substituting all these quantities and do some math, we have
a c1 S max
k v  0.1333 (12)
k m rk
For example, a motor with maximum speed 3,000 RPM yields kv = 400. With this
gain and step input of 511, the output changes at rate 204,350 units/sec. The
equation (12) is coded in hdminit.sce and used in Xcos model speedtuning.zcos.
Figure 9 shows the simulation result of the output rate, when the step input is
applied at t = 1 second. We see the fluctuation around the expected value.

Figure 9 simulation result showing the motor output rate of change

10
 Robotics

Even more interesting open-loop load angle rate of change is shown in Figure 10.
We see a large sinusoid fluctuation that dies out quite slowly.

Figure 10 simulation result showing the load output rate of change

Let us perform further calculation from this setup. Suppose we assign the maximum
PWM value of 4095 to represent the load angle of 360 degrees (full turn). With
motor turned at maximum velocity 3,000 RPM, the load turns at 300 RPM, one
tenth of the motor velocity. The PWM value for load angle will reach its maximum
in about 200 milliseconds.
We will see in Part II whether this setup is suitable for our embedded system. Stay
tuned.
Scilab Script and Model Files

The files used this document are

 hdminit.sce : Scilab script file for parameter initialization and transfer
function computation
 pid_mf_cdcompare.zcos : Xcos model comparison for PID with motor angle
feedback
 pid_lf_cdcompare.zcos : Xcos model comparison for PID with load angle
feedback
 speedtuning.zcos : Xcos model for plant gain adjustment
Note: execute the script file first before running simulation

11
HIL Implementation of Harmonic Drive Motor (Part I: Discretization and
Simulation)

Exercises
1. Show derivation details of continuous-time transfer functions and their
coefficients (1) - (4)
2. Show derivation details of discrete-time transfer functions and their
coefficients (6) - (8)
3. Derive (12).
4. Experiment with Xcos model comparisons. Try changing sampling time, and
PID gains. Can you explain their effects on the responses?

12