Tag: DC Motor Control

# DC Motor Position Tracking

At the end of the post Motor Position Control with Raspberry Pi, we saw that there are different ways to move a motor shaft between two angular positions. In this post, we will take a deeper look into three types of paths between an initial and a final position. While I’ll be exploring the concept using a system with one degree of freedom, with proper normalization and parametrization, the idea can be applied to any trajectory between two points in space.

The first observation to be made is that the path that our closed-loop system is going to follow is calculated before hand, i.e., the initial and final positionsand(shown in the diagram on the left), as well as other parameters that define the trajectory, are known before the position tracking is executed.

In our case, we’ll use the acceleration time(at the beginning and end of the motion) and the maximum allowed velocityas the additional parameters that are sufficient to fully determine the path.

Maximum velocity and acceleration time (or conversely the maximum acceleration) are a reasonable choice, since they are related to the physical limitations of the system.

The total motion time is therefore automatically determined once the four parameters above are specified. Even though there are different ways to go about calculating the path, I’ll use the velocity profile as the primary trace, with position and acceleration being calculated respectively as the integral and derivative of the velocity. Let’s then go over the three velocity profiles for this post: linear, quadratic, and trigonometric.

## Linear Speed Profile

Using the illustration below it’s not too difficult to arrive at the following three equations that describe the velocity as a function of time.

The timeat maximum speed is given by:

Where it can be calculated by using the fact that the area under the speed trace is equal to .

As mentioned before, and can be used interchangeably, where, in this case, . The Python function below shows one possible implementation of the path calculation, where the position and acceleration traces are calculated through numerical integration and derivative, respectively. I found this approach to be more robust to the path discretization, specially at the speed profile transitions.

```def path_linear(xstart, xstop, vmax, ta, nstep=1000):
#
# Adjusting velocity based on end points
if xstop < xstart:
vmax = -vmax
# Assigning time at max velocity
if (xstop-xstart)/vmax > ta:
# There's enough time for constant velocity section
tmax = (xstop-xstart)/vmax - ta
else:
# There isn't (triangular velocity profile)
tmax = 0
vmax = (xstop-xstart)/ta
# Assigning important time stamps
t1 = ta  # End of acceleration section
t2 = ta + tmax  # End of constant velocity section
t3 = 2*ta + tmax  # End of motion (deceleration) section
# Creating time array for path discretization
t = np.linspace(0, t3, nstep+1)
# Finding transition indices
i1 = np.nonzero(t<=t1)
i2 = np.nonzero((t>t1)&(t<=t2))
i3 = np.nonzero(t>t2)
# Calculating accelereration section array
v1 = vmax/ta*t[i1]
# Calculating constant velocity section array
if len(i2[0]) > 0:
v2 = np.array([vmax]*len(i2[0]))
else:
v2 = np.array([])
# Calculating deceleration section array
v3 = vmax - vmax/ta*(t[i3]-t2)
# Concatenating arrays
v = np.concatenate((v1, v2, v3))
# Calculating numeric integral (position)
x = xstart + t[-1]/nstep * np.cumsum(v)
# Calculating numeric derivative (acceleration)
# Returning time, position, velocity, and acceleration arrays
return t, x, v, a
```

Unlike the linear speed profile, which uses constant acceleration, the quadratic (or parabolic) speed profile uses linearly changing acceleration at the beginning and end of the motion. The equations for this type of trajectory are shown below.

An easy way to obtain the first and third velocity equations above is by writing the corresponding acceleration functions and integrating them.

Similarly to the linear case, the time at maximum speed can be obtained from the area under the velocity curve. (Tip: integrate the first velocity equation and evaluate it at, multiply the result by 2 to account for the deceleration portion, and add the area of the rectangle under)

The Python function for this case follows the same structure as the previous one. You can find it on my GitHub page.

## Trigonometric Speed Profile

The third profile type uses a cosine function for the velocity during the acceleration and deceleration phases of the trajectory. By choosing the appropriate function, it is possible to add an interesting feature to this last profile type: zero initial and final accelerations.

Also through integration of the velocity function and knowing that the area under the curve is, it is possible to solve for the time at maximum velocity:

Like the two previous cases, the Python function that implements this type of path calculation is located on the GitHub page.

## Practical Application for Position Tracking

When it comes to choosing the path calculation method, two factors have to be kept in mind: how fast do we want to (or can) go from the initial to the final position and how much acceleration change (jerk) do we want to apply to the system. One good way to go about it is to test all three and quantify how the overall closed-loop system behaves.

Just to explore a little bit, the plot on the left shows an overlay of the three types of path for the examples shown above. In this scenario, the maximum velocity is limited to 1 (m/s, for example) and the total time set to 1.4s, by proper choice of. Even though the acceleration profiles are very distinct, the difference between the three types of trajectory is visually very subtle.

In contrast, if we now limit both the maximum velocity to 1 m/s and the maximum acceleration to 4 m/s2, the total motion duration is fairly different. The linear velocity profile is the fastest one (at the expense of having the largest jerk). Conversely, the trigonometric method takes the longest time with the least amount of jerk.

Let’s use the same setup with a Raspberry Pi and a DC motor that we had in the post Motor Position Control with Raspberry Pi to test the different types of paths for the positioning of the DC motor shaft. The Python program below shows how the quadratic path is generated prior to the execution loop and then sampled during the execution loop.

The full code with the plotting features and the module `path.py`, containing the three types of path calculation functions, can be found here.

```# Importing modules and classes
import time
import numpy as np
from scipy.interpolate import interp1d
from gpiozero_extended import Motor, PID

# Setting path parameters
thetastart = 0  # Start shaft angle (rad)
thetaend = 3*np.pi  # End shaft angle (rad)
wmax = 4*np.pi  # Max angular speed (rad/s)
ta = 0.45  # Acceleration time (s)
tsample = 0.01  # Sampling period (s)

# Calculating path
t, x, _, _ = path_quad(thetastart, thetaend, wmax, ta)
f = interp1d(t, 180/np.pi*x)  # Interpolation function
tstop = t[-1]  # Execution duration (s)

# Creating PID controller object
kp = 0.036
ki = 0.260
kd = 0.0011
taupid = 0.01
pid = PID(tsample, kp, ki, kd, tau=taupid)

# Creating motor object using GPIO pins 16, 17, and 18
# (using SN754410 quadruple half-H driver chip)
# Integrated encoder on GPIO pins 24 and 25.
mymotor = Motor(
enable1=16, pwm1=17, pwm2=18,
encoder1=24, encoder2=25, encoderppr=300.8)
mymotor.reset_angle()

# Initializing variables and starting clock
thetaprev = 0
tprev = 0
tcurr = 0
tstart = time.perf_counter()

# Running execution loop
print('Running code for', tstop, 'seconds ...')
while tcurr <= tstop:
# Pausing for `tsample` to give CPU time to process encoder signal
time.sleep(tsample)
# Getting current time (s)
tcurr = time.perf_counter() - tstart
# Getting motor shaft angular position
thetacurr = mymotor.get_angle()
# Interpolating set point angle at current time step
if tcurr <= tstop:
thetaspcurr = f(tcurr)
# Calculating closed-loop output
ucurr = pid.control(thetaspcurr, thetacurr)
# Assigning motor output
mymotor.set_output(ucurr)
# Updating previous values
thetaprev = thetacurr
tprev = tcurr

print('Done.')
# Stopping motor and releasing GPIO pins
mymotor.set_output(0, brake=True)
del mymotor
```

The plots show the set point vs. actual shaft position for a 450 degrees rotation, using the quadratic velocity path.

The derivative of the signal, i.e., the shaft speed, is a useful gage to see how well the closed-loop system can follow the trajectory. The position error between set point and measured value is also a good metric. In this case, it is within +/- 5 degrees with an RMSE (Root Mean Squared Error) of about 2.5 degrees. Although not a stellar result, it’s not too bad, given the budget hardware and simple control (PID) that were used.

Finally, the graph at the bottom shows a comparison of the position tracking error for the three types of path explored in this post for the same 450 degree rotation. The average RMSE values for the linear, quadratic, and trigonometric speed profiles were respectively 3.3, 2.4, and 3.6 degrees.

Even though the trigonometric profile has the smoothest acceleration, it was outperformed by the other two for this particular system.

The trigonometric one is usually beneficial for dynamic systems that are more susceptible to vibration when subjected to a more “jerky” path profile.

# PID Tuning

Tuning a PID controller consists of choosing the best values for the proportional, integral, and derivative gains in order to satisfy some performance criteria of the closed-loop system. If we’re able to characterize or model the plant that we want to control, then it’s possible do find the best gains either through direct calculation or through simulation.

If we go back to the day when computers weren’t exactly around and there was no such thing as a digital PID, folks had to come up with methods that could be applied directly to the plant or process. Developed respectively in the 1940s and 50s, the well known Ziegler-Nichols and Cohen-Coon methods were the way to go about PID tuning back then. One problem with those two methods is that they result in a fairly aggressive set of gains that can potentially lead to an unstable (or a reduced stability margin) closed-loop system. That undesired characteristic becomes even more critical when a digital PID is employed.

Because we will be dealing mostly with DC motors, I’ll go over a method where the motor speed response is approximated by a first order system. The identified open-loop system is then used to determine the PID gains for the corresponding closed-loop system. In a future post we will go over characterizing the motor individual plant parameters, thus obtaining a better model.

So, let’s try to keep the math under control and start with characterization of the first-order system.

## First-Order System

If we look back at the digital filter post, we can extend the low-pass filter equation to a more general form of the first-order system, given by:

where the steady-state gainis the ratio between system output and input. As seen in that same post,is the response time of the system. By determining those two parameters, the first-order system can be fully characterized.

## Closed-Loop System

The block diagram below represents the closed-loop system with a PID controller and our first-order system, the latter now expressed as a differential equation. While technically correct, no one uses a time-based representation of the system as shown in the diagram. Mostly because it is really difficult to work with differential equations whose inputs and outputs are all interconnected in some way.

Thankfully, a pretty smart guy named Pierre-Simon Laplace (1749 – 1827) came up with the Laplace transform. It allows for mere mortals to treat complicated convolution problems involving differential equations as simple algebraic ones. We can then redraw the diagram using the corresponding Laplace transforms, as shown in the more familiar form below.

Because we will be doing some algebraic manipulation let’s use the following transfer function notation:

Where

and

Doing the usual “with a few steps, it can be shown that” the closed-loop system transfer function in our case is:

It has the characteristic equation of a second-order system, with damping ratio(zeta) and natural frequency

or

Where

and

After all this mumbo jumbo, we’re finally getting to a point where, for a given first-order plant (with knownand), we can determine the PID gains,andthat would give us the desired closed-loop response, characterized byand. In other words, by selecting a damping ratio and a natural frequency, we can determine a set of PID gains that make our closed-loop system behave as desired.

## PID Gains Selection

If we take a look at the last two equations, there are three unknowns (the PID gains) for two known parameters (and ). A good starting point is to make . That way, we can solve the two following equations, for and.

and

Where:

• in rad/s is the natural frequency of the closed-loop system. The larger its value the faster the system’s response time. In practical terms, you cannot make this number arbitrarily high since you may run into saturation of the controller output. While, for most applications, short saturation periods are fine, it’s better to keep them at a minimum.
• is the damping ratio of the closed-loop system. Values close to 1 (critical damping ratio) will keep the oscillations and overshoot to a step input under control. In some closed-loop positioning applications, overshoot isn’t an option. Hence, a damping ratio greater than 1 is required.

Finally, note that the PID gains should always be positive. Therefore,.

## DC Motor PID Tuning

It’s time to use the hardware setup and code from the Motor Speed Control post to do some exploration of this PID tuning method. First, a step input is applied to the open-loop motor in order to determine the first order response parametersand.

For our little DC motor, with the Raspberry Pi setup, the steady state gain is 20.9/0.5 = 41.8 rad/s/u. Remember that, in our case, u is the normalized PWM output between -1 and 1. The response time of the open-loop system is 0.184 s.

To improve the parameter extraction, a zero-phase filter was used. The code that automatically determines the parameters from the system response can be found on my GitHub page.

Next step consists of choosing values for the closed-loop system’s natural frequencyand damping ratio. The graphs on the left show the system response to a step input for a natural frequency of 2 Hz () and. Using the last two formulas for the integral and proportional gains

,

with all the values we gathered so far, would result inand. We can observe that the motor now takes 0.11 s to reach the target speed (instead of 0.184 to reach 63% of it). That comes of course at expense of using the full capacity of the PWM with some brief saturation.

The reasoning behind my choice ofandis that I wanted a faster response with limited saturation. 2 Hz was the right amount for that. I also wanted a damping ratio slightly greater than one to avoid any oscillations. At this point you might be asking yourself: why is the closed-loop response showing an oscillatory behavior then! The answer is a combination of:

• The motor is in fact a second order system, not a first order one as our approximation suggests. So, higher order modes may start showing up in the closed-loop response
• The PID is discrete, not continuous (as that was the basis for all the deductions above). And we’ve seen that the digitalization of the PID tends to promote a more oscillatory behavior
• The actual closed-loop system includes the transfer function of a low-pass filter for the motor speed. So, the overall system is not composed only by a PID and a plant, as we initially assumed.

Acknowledging that the choice of gains was fairly straightforward, the resulting closed-loop response is quite good! And we haven’t incorporated the derivative gain yet. While it’s usage is not always necessary, it tends to be quite helpful in the case of closed-loop DC motor control. A good starting point foris a couple of orders of magnitude smaller than the proportional gain. So, after starting with 0.001, I ended up with a value of 0.005. The addition of the derivative term really decreased the overshoot and virtually removed the oscillations.

Finally, the next set of graphs shows the results when choosing different natural frequencies and damping ratios. Notice the effect on the response time, the overshoot and the saturation of the controller output.

# Motor Speed Control with Raspberry Pi

Let’s use the Motor class with encoder and the Digital PID class to create a closed-loop speed controller for a DC motor. Those two classes have all the required attributes and methods that we need to program an execution loop for a PID controller with very few lines of code.

The starting point is the Raspberry Pi setup that was used to implement encoder capability into the Motor class. I took the artistic liberty to create the diagram below, where I combine visual elements of the PID loop, the hardware connections, and the class methods used for the I/O between the Pi computer and the physical system.

Going back to the concept of execution loops, the I/O tasks are taken care by the Motor class methods `get_angle()` and `set_output()`. The computation tasks happening in between are essentially the shaft speed calculation, low-pass filtering, and PID control action. The Python code below implements the execution loop, where the different elements in the Pi box above can be identified. The complete program, which includes the plotting features, along with the Python module with all necessary classes, can be found on my GitHub page.

```# Importing modules and classes
import time
import numpy as np
from gpiozero_extended import Motor, PID

# Setting general parameters
tstop = 2  # Execution duration (s)
tsample = 0.01  # Sampling period (s)
wsp = 20  # Motor speed set point (rad/s)
tau = 0.1  # Speed low-pass filter response time (s)

# Creating PID controller object
kp = 0.15
ki = 0.35
kd = 0.01
taupid=0.01
pid = PID(tsample, kp, ki, kd, umin=0, tau=taupid)

# Creating motor object using GPIO pins 16, 17, and 18
# (using SN754410 quadruple half-H driver chip)
# Integrated encoder on GPIO pins 24 and 25.
mymotor = Motor(
enable1=16, pwm1=17, pwm2=18,
encoder1=24, encoder2=25, encoderppr=300.8)
mymotor.reset_angle()

# Initializing previous and current values
ucurr = 0  # x[n] (step input)
wfprev = 0  # y[n-1]
wfcurr = 0  # y[n]

# Initializing variables and starting clock
thetaprev = 0
tprev = 0
tcurr = 0
tstart = time.perf_counter()

# Running execution loop
print('Running code for', tstop, 'seconds ...')
while tcurr <= tstop:
# Pausing for `tsample` to give CPU time to process encoder signal
time.sleep(tsample)
# Getting current time (s)
tcurr = time.perf_counter() - tstart
# Getting motor shaft angular position: I/O (data in)
thetacurr = mymotor.get_angle()
wcurr = np.pi/180 * (thetacurr-thetaprev)/(tcurr-tprev)
# Filtering motor speed signal
wfcurr = tau/(tau+tsample)*wfprev + tsample/(tau+tsample)*wcurr
wfprev = wfcurr
# Calculating closed-loop output
ucurr = pid.control(wsp, wfcurr)
# Assigning motor output: I/O (data out)
mymotor.set_output(ucurr)
# Updating previous values
thetaprev = thetacurr
tprev = tcurr

print('Done.')
# Stopping motor and releasing GPIO pins
mymotor.set_output(0, brake=True)
del mymotor
```

## Motor Speed Filtering

Now that the Python code is laid out, let’s go over some of its components. The motor shaft speed is calculated as the derivative of the angular position of the shaft using a backward difference formula `wcurr = np.pi/180 * (thetacurr-thetaprev)/(tcurr-tprev)`.

The encoder that’s being used has a resolution of `encoderppr=300.8`, which corresponds to approximately 360/300.8 = 1.2 degrees. The difference between two consecutive time steps `tcurr` and `tprev `is about `tsample = 0.01 `seconds. That gives us a not so great speed resolution of `np.pi/180 * (1.2)/(0.01) = 2` rad/s (or 19 rpm!). As a side note, the speed resolution could be improved by increasing either the encoder PPR or the sampling rate. However the first option might reach the limit of the GPIO zero RotaryEncoder class, while the latter will eventually make the closed-loop system unstable. Therefore a compromise has to be reached given the constraints at hand.

One way reduce the impact of this limitation is to apply a low-pass digital filter to the motor speed signal: `wfcurr = tau/(tau+tsample)*wfprev + tsample/(tau+tsample)*wcurr`. The graphs below show the raw and the filtered signal as well as the controller output.

On the left, `tau = 0` and therefore no filtering is being done. Observe how the controller output bounces between saturation limits. On the right, `tau = 0.1`, causing the control output to be less erratic, which in turn makes the raw speed more stable. One can also see the “discretization effect” of the 2 rad/s speed resolution on the top row plots.

As discussed in the post Encoder with Raspberry Pi, we cannot use a more accurate sampling period during the execution loop due to the intensive computing resources of the RotaryEncoder class.

However, as shown on the left, having to rely on the command `time.sleep(tsample)` inside the execution loop, still produces repeatability of the sampling period within 1 ms.

## PID Derivative Term Filtering

If you notice in the Python code above, I am also filtering the PID derivative when I created the PID class instance: `pid = PID(tsample, kp, ki, kd, umin=0, tau=taupid)`, where `taupid = 0.01` seconds.

If we turn it off by making `taupid = 0`, we obtain the results on the left, where the controller output shows a bigger oscillation amplitude. However, the effect of turning the derivative term filter off is not as pronounced as the effect of not filtering the motor speed signal.

By comparing all the system responses so far, even though the PID gains were never changed from case to case, the closed-loop system response is changing. Ultimately, by adding low-pass filters, the closed-loop dynamic system is different from its original form without the filters.

## PID Saturation and Anti-Windup

As it can be seen in the previous graphs, the PID output is saturating right after the step input. We can observe the effect of PID saturation and the integral term windup (which has been avoided so far) by creating the PID class instance: `pid = PID(tsample, kp, ki, kd, umin=-10, umax=10, tau=taupid)`.

Because the controller output (PWM in the Motor class) is in fact limited between -1 and 1, applying -10 and 10 as the new PID controller limits will effectively turn off the anti-windup feature and cause the integral term to wind up.

The plots on the left show the resulting speed overshoot due to that modification as the actual PWM output is saturated at 1.

## Final Remarks

We have been building up our knowledge with the previous posts (namely: Encoder with Raspberry Pi, Digital Filtering, and Digital PID Controller) so we could finally run a DC motor in closed-loop speed control. While this could have been achieved from the very beginning, I believe it was more educational to do it in smaller parts through the use of Python classes.

When building more complex automation systems, having a modularized approach with classes that can be tested and used independently, as well as combined in different ways, will make things much easier to tackle in the long run.