PID control of a robot joint

Lesson 04 produced joint angles for a target. Something now has to make each joint go to its angle and stay there while gravity, friction and payload push back. On almost every robot that something is a feedback controller in the drive, and very often it is a PID controller. This lesson builds one for a single simulated joint. It starts with a model of a DC motor driving a load, closes the loop with P, then PI, then PID, and then deals with the two problems that every real implementation meets: integral windup when the amplifier saturates, and the derivative kick on a setpoint step. Everything runs in plain numpy, so you can see each line of the controller.

A model of the joint

The joint is a DC motor driving a link through a gearbox. We refer everything to the joint output, so the gear ratio is folded into the constants. Three equations describe it:

current      i = (u - KE * omega) / R            (armature inductance neglected)
torque       tau = KT * i
acceleration J * domega/dt = tau - B * omega - tau_load

u is the voltage from the amplifier, omega the joint speed, KE the back-EMF constant, KT the torque constant, R the winding resistance, J the inertia, B viscous friction and tau_load a load torque, such as gravity acting on the link. The back-EMF term means the motor’s own speed reduces its current, which acts like extra damping. Neglecting inductance is reasonable when the electrical time constant is much shorter than the mechanical one. The numbers below are illustrative values for a small geared joint, not taken from a datasheet.

We integrate the model with a fixed step of 1 ms using semi-implicit Euler: update the speed from the acceleration, then the angle from the new speed. It is one line more careful than plain Euler and behaves much better for oscillating systems. Before adding any control, check the model in open loop. Apply 6 V and compare the simulated speed and time constant with the formulas.

import numpy as np

# Joint model: DC motor plus gearbox plus load, all referred to the joint output.
J = 0.01        # inertia, kg m^2
B = 0.01        # viscous friction, N m s/rad
KT = 0.5        # torque constant at the output, N m/A
KE = 0.5        # back-EMF constant at the output, V s/rad
R_ARM = 2.0     # armature resistance, ohm (inductance neglected)
DT = 0.001      # integration step, s

def joint_accel(omega: float, volts: float, tau_load: float) -> float:
    current = (volts - KE * omega) / R_ARM
    torque = KT * current - B * omega - tau_load
    return torque / J

# Open loop: apply 6 V with no load and integrate for 0.5 s
omega, theta = 0.0, 0.0
t63 = None
w_final = (KT / R_ARM) * 6.0 / (B + KT * KE / R_ARM)   # steady-state speed from the model
for k in range(500):
    omega += joint_accel(omega, 6.0, 0.0) * DT
    theta += omega * DT
    if t63 is None and omega >= 0.632 * w_final:
        t63 = (k + 1) * DT
tau = J / (B + KT * KE / R_ARM)
print(f"speed after 0.5 s: {omega:.2f} rad/s (model steady state {w_final:.2f} rad/s)")
print(f"time constant: simulated {t63:.3f} s, analytic {tau:.3f} s")
print(f"angle travelled: {theta:.2f} rad")

speed after 0.5 s: 11.10 rad/s (model steady state 11.11 rad/s)
time constant: simulated 0.074 s, analytic 0.074 s
angle travelled: 4.74 rad

The speed rises as a first-order lag with a 74 ms time constant to 11.1 rad/s, as the formula predicts. Speed is first order. Angle is the integral of speed, so from voltage to angle the joint is second order, with one pole at the origin. That pole is why the joint drifts with a constant voltage instead of settling at an angle, and it is why position control needs feedback.

The loop

The controller sees the error e, the setpoint minus the measured angle, and outputs a voltage:

u = kp * e  +  ki * integral(e dt)  +  kd * de/dt

Each term has a job. The proportional term pushes harder the further the joint is from the target. The integral term accumulates past error, so a small error that persists eventually produces a large correction. The derivative term reacts to how fast the error is changing, which damps the motion before it overshoots. In discrete time, at 1 kHz, the integral becomes a running sum of e * DT and the derivative becomes a difference between consecutive samples divided by DT.

The simulate function below implements this loop with the amplifier limit, a constant load torque of 0.3 N m and two switches, for anti-windup and for where the derivative is taken, that later sections use. It then runs a 1 rad step with three gain sets.

import numpy as np

J, B, KT, KE, R_ARM = 0.01, 0.01, 0.5, 0.5, 2.0   # joint model from the previous block
DT = 0.001          # controller and integrator step, s (1 kHz)
V_MAX = 12.0        # supply limit, V
TAU_LOAD = 0.3      # constant load torque, N m (for example gravity on a held link)

def simulate(kp: float, ki: float, kd: float, setpoint: float = 1.0, t_end: float = 1.2,
             anti_windup: bool = False, d_on_measurement: bool = True):
    """Closed-loop step response. Returns time, angle and commanded voltage arrays."""
    n = int(round(t_end / DT))
    theta = omega = integral = 0.0
    prev_theta, prev_err = 0.0, 0.0          # before t = 0 the setpoint and the angle were both 0
    t_log, th_log, u_log = [], [], []
    for k in range(n + 1):
        err = setpoint - theta
        if d_on_measurement:
            deriv = -(theta - prev_theta) / DT   # derivative of the measured angle, sign flipped
        else:
            deriv = (err - prev_err) / DT        # derivative of the error
        u_cmd = kp * err + ki * integral + kd * deriv
        u = min(max(u_cmd, -V_MAX), V_MAX)       # the amplifier saturates
        if not anti_windup or u == u_cmd:        # clamping: freeze the integral while saturated
            integral += err * DT
        t_log.append(k * DT)
        th_log.append(theta)
        u_log.append(u_cmd)
        prev_theta, prev_err = theta, err
        current = (u - KE * omega) / R_ARM
        alpha = (KT * current - B * omega - TAU_LOAD) / J
        omega += alpha * DT                      # semi-implicit Euler: velocity first,
        theta += omega * DT                      # then position with the new velocity
    return np.array(t_log), np.array(th_log), np.array(u_log)

def step_metrics(t, th, setpoint=1.0, band=0.02):
    overshoot = max(0.0, (th.max() - setpoint) / setpoint * 100)
    outside = np.where(np.abs(th - setpoint) > band * abs(setpoint))[0]
    settled = outside[-1] + 1 < len(t)
    t_settle = t[outside[-1] + 1] if settled else None
    return overshoot, th[-1], t_settle

gains = {"P": (10.0, 0.0, 0.0), "PI": (10.0, 20.0, 0.0), "PID": (10.0, 10.0, 0.4)}
results = {}
for name, (kp, ki, kd) in gains.items():
    t, th, u = simulate(kp, ki, kd)
    results[name] = (t, th)
    over, final, ts = step_metrics(t, th)
    ts_text = f"{ts:.3f} s" if ts is not None else "never (outside 2 % band)"
    print(f"{name:3s} kp={kp:4.1f} ki={ki:4.1f} kd={kd:3.1f}  overshoot {over:5.1f} %  "
          f"final {final:.3f} rad  2 % settling {ts_text}  peak |u| {np.abs(u).max():.1f} V")

# Coarse samples for the chart: every 40 ms
for name, (t, th) in results.items():
    pts = [[round(float(t[i]), 2), round(float(th[i]), 3)] for i in range(0, len(t), 40)]
    print(name, len(pts), pts)

P   kp=10.0 ki= 0.0 kd=0.0  overshoot   7.8 %  final 0.880 rad  2 % settling never (outside 2 % band)  peak |u| 10.0 V
PI  kp=10.0 ki=20.0 kd=0.0  overshoot  25.6 %  final 0.999 rad  2 % settling 0.569 s  peak |u| 10.1 V
PID kp=10.0 ki=10.0 kd=0.4  overshoot   0.9 %  final 0.993 rad  2 % settling 0.243 s  peak |u| 10.0 V
P 31 [[0.0, 0.0], [0.04, 0.147], [0.08, 0.453], ...]
PI 31 [[0.0, 0.0], [0.04, 0.152], [0.08, 0.482], ...]
PID 31 [[0.0, 0.0], [0.04, 0.133], [0.08, 0.384], ...]

The last three lines are shortened here. The chart below uses the full 31 samples of each.

P: fast, but short of the target

With kp of 10 V/rad the joint rises quickly, overshoots to 1.078 rad and settles at 0.880 rad. It never reaches the setpoint. The reason is the load. To hold the link still against 0.3 N m, the motor needs a current of 0.3 / 0.5 = 0.6 A, which takes 1.2 V across the 2 Ω winding. A proportional controller only produces voltage when there is error, so it settles where kp * e equals 1.2 V: an error of 0.12 rad. Doubling kp halves that error, but it also makes the response more oscillatory and, in a real system, amplifies sensor noise and excites resonances in the gearbox. Proportional gain alone cannot remove a constant disturbance.

The oscillation has a simple explanation too. Ignoring saturation, the closed loop is a second-order system. Its natural frequency is sqrt(25 * kp), about 15.8 rad/s, where 25 is KT / (R * J). Its damping ratio is 13.5 divided by twice that frequency, 0.43, where 13.5 is (B + KT * KE / R) / J. That damping ratio predicts 22.7 % overshoot of the final value, and the simulation shows (1.078 - 0.880) / 0.880, which is 22.5 %.

PI: zero steady-state error, more overshoot

Adding an integral term with ki of 20 V/(rad s) fixes the offset. While any error remains, the integral grows, and it stops growing only when the error is zero. At that point the integral alone supplies the 1.2 V the load needs. The final angle is 0.999 rad.

The price is overshoot: 25.6 %. During the rise the error is large and positive for a long time, so the integral builds up a store of voltage. By the time the joint reaches the setpoint, that store keeps pushing, and the joint overshoots until negative error drains it. Integral action adds phase lag, and phase lag erodes stability margins.

PID: adding damping

The derivative term opposes motion. With kd of 0.4 V s/rad, each 1 rad/s of joint speed removes 0.4 V from the command, which acts like extra viscous friction that only exists in the controller. With that damping in place we can also lower ki to 10, which reduces the stored voltage that caused the PI overshoot. The result reaches the 2 % band in 0.243 s with 0.9 % overshoot.

Look closely at the PID trace after 0.5 s. It dips to 0.986 rad and then creeps back up, reaching 0.993 rad at 1.2 s. The lower ki means the integral takes longer to build the last few tenths of a volt, so the final 1 % is slow. Every tuning is a trade of this kind: here, low overshoot against a slow tail. The PID tuning cheat sheet lists systematic methods for choosing the three gains.

Integral windup

Now ask for a bigger move, 4 rad, with the same PID gains. The proportional term alone asks for 40 V at the start, and the amplifier delivers 12. For most of the move the output is pinned at the limit, so the controller has no authority, but the integral keeps adding up error as if it did. When the joint finally nears the target, the integral holds a large voltage that is no longer wanted, and it drives the joint past the setpoint. This is integral windup.

The simplest fix is clamping, also called conditional integration: stop integrating while the output is saturated. In simulate, that is the line if not anti_windup or u == u_cmd. The block below reuses the model and controller from the previous block and runs the 4 rad move both ways. It then runs the 1 rad step with the derivative taken on the error instead of the measurement.

import numpy as np

J, B, KT, KE, R_ARM = 0.01, 0.01, 0.5, 0.5, 2.0   # joint model from the first block
DT = 0.001          # controller and integrator step, s (1 kHz)
V_MAX = 12.0        # supply limit, V
TAU_LOAD = 0.3      # constant load torque, N m (for example gravity on a held link)

def simulate(kp: float, ki: float, kd: float, setpoint: float = 1.0, t_end: float = 1.2,
             anti_windup: bool = False, d_on_measurement: bool = True):
    """Closed-loop step response. Returns time, angle and commanded voltage arrays."""
    n = int(round(t_end / DT))
    theta = omega = integral = 0.0
    prev_theta, prev_err = 0.0, 0.0          # before t = 0 the setpoint and the angle were both 0
    t_log, th_log, u_log = [], [], []
    for k in range(n + 1):
        err = setpoint - theta
        if d_on_measurement:
            deriv = -(theta - prev_theta) / DT   # derivative of the measured angle, sign flipped
        else:
            deriv = (err - prev_err) / DT        # derivative of the error
        u_cmd = kp * err + ki * integral + kd * deriv
        u = min(max(u_cmd, -V_MAX), V_MAX)       # the amplifier saturates
        if not anti_windup or u == u_cmd:        # clamping: freeze the integral while saturated
            integral += err * DT
        t_log.append(k * DT)
        th_log.append(theta)
        u_log.append(u_cmd)
        prev_theta, prev_err = theta, err
        current = (u - KE * omega) / R_ARM
        alpha = (KT * current - B * omega - TAU_LOAD) / J
        omega += alpha * DT                      # semi-implicit Euler: velocity first,
        theta += omega * DT                      # then position with the new velocity
    return np.array(t_log), np.array(th_log), np.array(u_log)

def step_metrics(t, th, setpoint=1.0, band=0.02):
    overshoot = max(0.0, (th.max() - setpoint) / setpoint * 100)
    outside = np.where(np.abs(th - setpoint) > band * abs(setpoint))[0]
    settled = outside[-1] + 1 < len(t)
    t_settle = t[outside[-1] + 1] if settled else None
    return overshoot, th[-1], t_settle

# Integral windup: a 4 rad move saturates the amplifier for a long time
for aw in (False, True):
    t, th, u = simulate(10.0, 10.0, 0.4, setpoint=4.0, t_end=3.0, anti_windup=aw)
    over, final, ts = step_metrics(t, th, setpoint=4.0)
    ts_text = f"{ts:.2f} s" if ts is not None else "not settled"
    print(f"anti-windup {str(aw):5s}  overshoot {over:5.1f} %  2 % settling {ts_text}  final {final:.3f} rad")

# Derivative kick: the same PID with the derivative on the error instead of the measurement
for dom in (True, False):
    t, th, u = simulate(10.0, 10.0, 0.4, d_on_measurement=dom)
    over, final, ts = step_metrics(t, th)
    label = "measurement" if dom else "error      "
    print(f"D on {label}  first commanded voltage {u[0]:7.1f} V  overshoot {over:4.1f} %")

anti-windup False  overshoot  14.7 %  2 % settling 2.09 s  final 4.029 rad
anti-windup True   overshoot   1.7 %  2 % settling 0.33 s  final 4.000 rad
D on measurement  first commanded voltage    10.0 V  overshoot  0.9 %
D on error        first commanded voltage   410.0 V  overshoot  0.9 %

Without anti-windup the 4 rad move overshoots by 14.7 %, nearly 0.6 rad, and takes 2.09 s to settle. It is still 0.029 rad off at 3 s while the excess integral drains. With clamping the same gains give 1.7 % overshoot and settle in 0.33 s. The gains did not change. Only the integrator’s behaviour during saturation did.

Clamping is one of several schemes. Back-calculation feeds the difference between the commanded and the delivered output back into the integrator, so it unwinds at a controlled rate. Whatever the scheme, any controller whose output can saturate needs one, and joint controllers saturate on every large move. The limit need not be the supply voltage. A current limit, a torque limit set for safety or a velocity limit in the drive all cause the same effect.

Derivative on the measurement

The derivative of the error is the derivative of the setpoint minus the derivative of the measurement. When the setpoint jumps, its derivative over one 1 ms sample is enormous. Here the setpoint goes from 0 to 1 rad in one step, so kd * de/dt is 0.4 × 1000 = 400 V on top of the 10 V proportional term. That is the derivative kick. In the simulation the amplifier clips it to 12 V for one sample and the response hardly changes. On hardware it is a current spike at every setpoint change, which stresses the gearbox and the drive.

Taking the derivative of the measured angle instead, with the sign flipped, gives the same damping during motion, because the setpoint is constant between steps, but no kick. That is why the code above defaults to d_on_measurement=True. The first commanded voltage drops from 410 V to 10 V. In practice the derivative is also low-pass filtered, because differentiating a quantised encoder signal amplifies noise. Lesson 06 looks at noise in detail.

Servo drives commonly run PID as one layer of a cascade: a current (torque) loop inside a velocity loop inside a position loop, each faster than the one around it. Model-based terms such as gravity compensation are added as feedforward so the integral has less work to do. The control hub and the actuators and drives hub cover these structures. Lesson 09 closes a PD loop on a simulated arm in MuJoCo, with and without gravity compensation.

Exercise

1. Set TAU_LOAD to zero and rerun the P controller. What is the steady-state error now, and why? Then double the load to 0.6 N m and predict the P error before running it.
2. Raise kp to 30 with the PI gains. How does the overshoot change, and what is the peak commanded voltage? What has the saturation done to the linear analysis?
3. Implement back-calculation anti-windup: after computing u, add (u - u_cmd) * k_aw * DT to the integral, with k_aw around 5. Compare its 4 rad response with clamping.
4. Add Gaussian noise with a standard deviation of 0.001 rad to the measured angle (seed np.random.default_rng(0)). Print the standard deviation of the commanded voltage at steady state for kd of 0.4, then add a first-order low-pass filter on the derivative and repeat.

Summary

• A DC-motor joint is first order from voltage to speed and second order from voltage to angle. A 1 ms semi-implicit Euler step reproduces the analytic time constant.
• P control leaves a steady-state error under a constant load equal to the holding voltage divided by kp, 0.12 rad here. The integral term removes it at the cost of overshoot.
• The derivative term adds damping. With PID the simulated joint reached 1 rad with 0.9 % overshoot and settled to 2 % in 0.24 s.
• When the output saturates, the integral winds up. Clamping cut the overshoot of a 4 rad move from 14.7 % to 1.7 %.
• Take the derivative on the measurement to avoid a kick at every setpoint step, and filter it against sensor noise.

Further reading

• Åström and Murray, Feedback Systems: An Introduction for Scientists and Engineers, 2nd edition, Princeton University Press, 2021: chapter 11, “PID Control”, covers tuning, integrator windup and implementation, and the authors keep the full text free to read on the book’s site.
• Lynch and Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017: chapter 11, “Robot Control”, covers error dynamics and motion control of robot joints with velocity and torque inputs.
• Siciliano, Sciavicco, Villani and Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009: chapter 8, “Motion Control”, for the step from one joint to a whole manipulator.