Motor Control, PID and Trajectories

Q: My motor oscillates or buzzes. What do I change?

Lower Kp (and Kd), check for delay or a slow loop rate, and look for backlash or a resonance in the mechanics. A filter on the feedback often cures a buzz.

Q: Why does the robot overshoot after being held at its limit?

Integral windup: the integral kept growing while the output was saturated. Clamp the integral or stop integrating while the output is limited.

Q: Do I need S-curves?

For light robots, trapezoids are fine. S-curves help when the load is heavy, flexible or fragile, or when vibration limits the settling time, at the price of slightly longer moves.

Q: Should the velocity loop run on the computer?

No. Keep the fast loops (current, velocity) in firmware or the drive at 1 kHz or more; the computer sends setpoints. Delay and jitter on a computer make fast loops unstable.

Cascade loops, feedforward, PID tuning, trapezoidal and S-curve profiles, and the limits set by sampling, delay and saturation.

On this page

The motor as a plant
Cascade loops: current, velocity, position
PID for motors
Feedforward
Tuning step by step
Trajectories: trapezoids, S-curves and splines
Sampling, delay and noise
Saturation, friction and backlash
Joint control of an arm: gravity and coupling
A tuning and commissioning checklist
FAQ

The motor as a plant

A brushed DC motor (or a brushless motor under field-oriented control) behaves like a first-order system: apply a voltage and the speed rises exponentially to a final value. Two constants describe it: the torque constant K_t (N·m per ampere) and the back-EMF constant K_e (volts per rad/s, numerically equal to K_t in SI units), plus the winding resistance R and inductance L and the rotor inertia J.

Worked example: a small motor

A motor with R = 2.0 Ω, L = 1.0 mH, K_t = K_e = 0.03 and J = 2.0 × 10⁻⁵ kg·m² on 12 V.
No-load speed = V ÷ K_e = 12 ÷ 0.03 = 400 rad/s (3 820 rpm). Stall torque = K_t V ÷ R = 0.03 × 12 ÷ 2.0 = 0.18 N·m at a stall current of 6 A (more than the driver may allow: current-limit it).
Mechanical time constant τ_m = R J ÷ (K_t K_e) = 2.0 × 0.00002 ÷ (0.03 × 0.03) = 44.4 ms: the speed reaches 63 % of its final value in 44 ms and 95 % in about 133 ms. The electrical time constant L ÷ R = 0.5 ms is much faster, so the speed loop sees a first-order plant with a gain of 1 ÷ K_e = 33.3 rad/s per volt.
With a load the inertia adds: a wheel and a robot reflected through a gearbox of ratio N multiplies the load inertia by 1 ÷ N² at the motor. Include it in J, or τ_m is badly underestimated.

Cascade loops: current, velocity, position

Motion control is built from nested loops, fast inside, slow outside: each loop treats the faster loops inside it as nearly instantaneous.

Loop	Controls	Typical rate	Where it runs
Current (torque)	Motor current, hence torque	20 000 Hz (10–40 kHz)	Motor driver or firmware; protects the motor
Velocity	Shaft or wheel speed	1 000 Hz (500–2 000 Hz)	Microcontroller or drive
Position	Shaft angle or distance	200 Hz (100–1 000 Hz)	Microcontroller, drive or ros2_control
Path / trajectory	The reference that the position loop follows	50–500 Hz	Controller or planner
Navigation	Where the robot goes	1–20 Hz	Computer (Nav2, MoveIt)

Tune from the inside out: first the current loop (often the driver’s own), then velocity, then position. Each outer loop’s bandwidth should be about 4 to 10 times lower than the inner loop it relies on.
Limit between the loops: the velocity command from the position loop is limited to the maximum speed, and the current command from the velocity loop to the maximum current (see Robot Software Architecture for where each loop lives).
Many drives and servo amplifiers close the inner loops for you; you then command velocity or position over a bus (see Motion Control and Actuators).

PID for motors

u = K_p e + K_i ∫ e dt + K_d de/dt

Step responses of a first-order plant: proportional control leaves an error, adding integral action removes it with some overshoot.

The code for a discrete PID with output limits and anti-windup is on Filters, PID and Signal Code. Here we use it on the motor above.

Worked example: a velocity step on the motor

Command a speed of 100 rad/s (955 rpm) from rest, running the loop at 1 kHz with a ±12 V output limit. The simulated responses:

Controller	Gains	Final speed	90 % rise time	Overshoot	Within 2 % of the target
P only	K_p = 0.05	62.5 rad/s	not reached	0.0 %	not settled
PI	K_p = 0.05, K_i = 0.8	100.0 rad/s	90 ms	0.0 %	210 ms
P + velocity feedforward	K_p = 0.05	100.0 rad/s	38 ms	0.0 %	63 ms
PI + velocity feedforward	K_p = 0.05, K_i = 0.8	100.0 rad/s	28 ms	10.3 %	227 ms

P alone settles short: with K_p = 0.05 V per rad/s the loop gain is K_p ÷ K_e = 1.67, so the final speed is 100 × 1.67 ÷ (1 + 1.67) = 62.5 rad/s: a steady-state error of 37.5 %.
Integral action removes the error, at the cost of a little overshoot.
Feedforward gets most of the way immediately (below), so the feedback only has to correct the rest: P with feedforward already reaches the target, in a third of the time of PI alone.
Do not simply add everything: with the same integral gain, PI plus feedforward overshoots (about 10 % here), because the integral collects error during the rise although the feedforward is already doing the work. With feedforward, use a lower K_i, start the integral late, or limit it.

Feedforward

Feedback reacts to an error; feedforward uses a model to supply the output that should be needed, before any error appears. A velocity feedforward adds the voltage the motor needs to hold the reference speed (u_ff = reference ÷ K = reference × K_e); an acceleration feedforward adds the torque to accelerate the inertia (J × reference acceleration); gravity and friction terms add the rest. Feedforward does the bulk of the work and feedback trims the errors of the model, so the gains can be lower and the loop calmer.

Worked example: following a moving target

Position loop gain. If the velocity loop has a bandwidth of 60 rad/s, choose the position gain about a quarter of it: K_p,pos = 15 per second.
Following error without feedforward. A position loop with proportional gain lags a ramp by e = v ÷ K_p,pos: at 0.5 m/s, 0.5 ÷ 15 = 33.3 mm.
With velocity feedforward (add the reference velocity to the velocity command) the lag drops to almost zero: the position loop only corrects the residual. This is why servo drives offer “velocity feedforward” as a parameter and why path-following accuracy improves at once when it is set.
Friction feedforward: add the voltage that overcomes Coulomb friction in the direction of motion, so that small commands move the wheel.

Tuning step by step

1. Make it safe: wheels off the ground or the arm free to move, current and speed limits set, a hand on the emergency stop.
2. Check the signs and units: a positive command must produce a positive encoder count; calibrate the scale (see Mobile Robot Kinematics and Odometry).
3. Measure the plant: apply a voltage step and record the speed: the final value gives the gain and the time to 63 % gives τ_m. Set the feedforward from the gain.
4. Tune P: raise K_p until the response is quick with a little overshoot; stop before it rings or buzzes.
5. Add I (a time constant of a few times τ_m) to remove the steady-state error; limit it to prevent windup.
6. D only if needed, filtered, for lightly damped mechanical loads.
7. Test the extremes: small and large steps, low and high speed, with the payload, in both directions.
8. Record the step response (rise time, overshoot, settling time) and keep the parameters in version control.

The Ziegler-Nichols rules (find the gain at which the loop sustains oscillation, then take 0.6 K_u for PID) are a quick start, but they give a lively response with about 25 % overshoot; for motors a gentler tune with a damping ratio near 0.7 is better. The method and a worked table are in Filters, PID and Signal Code.

Trajectories: trapezoids, S-curves and splines

A controller should be given a smooth reference, not a step: a jump in position demands infinite speed and shakes the machine. A trajectory generator produces position, speed and acceleration as functions of time that respect the limits.

A trapezoidal speed profile: constant acceleration, a cruise at the speed limit, and constant deceleration.

Worked example: a 1 m move

Trapezoid. Distance 1.0 m, speed limit 0.5 m/s, acceleration limit 1.0 m/s². Accelerating takes t_a = v ÷ a = 0.50 s and covers ½ a t_a² = 0.125 m, and so does braking. The cruise covers 1.0 − 2 × 0.125 = 0.750 m in 1.50 s. Total time 2.50 s.
A short move never reaches the limit: the profile is a triangle when d < v² ÷ a = 0.25 m. For d = 0.2 m the peak speed is √(d a) = 0.447 m/s and the move takes 2 √(d ÷ a) = 0.89 s.
The S-curve limits jerk (the rate of change of acceleration) as well, so the acceleration ramps instead of jumping, which reduces vibration and wear. With a jerk limit of 10 m/s³ the acceleration ramps up over 0.10 s; each acceleration phase takes v ÷ a + a ÷ j = 0.60 s and covers 0.150 m, so the cruise is 1.40 s and the total 2.60 s: only 100 ms longer than the trapezoid, with much smoother force.
A quintic (fifth-order) polynomial from rest to rest in T = 2.0 s has zero speed and acceleration at both ends: s(t) = 10 τ³ − 15 τ⁴ + 6 τ⁵ with τ = t ÷ T. Its peak speed is 1.875 d ÷ T = 0.938 m/s and its peak acceleration 5.77 d ÷ T² = 1.44 m/s²: the limits fix the shortest T.

Multi-axis moves are synchronized: all axes start and finish together, so the slowest axis sets the time and the others are scaled.
Splines and via points (cubic, B-spline) pass through a list of positions with continuous speed; MoveIt’s time parameterization (TOTG, Ruckig) turns a path into a time-optimal trajectory within limits.
Straight-line (Cartesian) moves need inverse kinematics at every step and can fail near singularities (see Frames, Transforms and Arm Kinematics).
Replan online when the target changes: libraries such as Ruckig generate a new jerk-limited trajectory in microseconds from the current state.

Sampling, delay and noise

Every delay in the loop (the sample period, the filter, the communication, the driver) lags the feedback and reduces the stability margin. The phase lag of a pure delay T at the crossover frequency ω_c is ω_c T radians.

Worked example: delay against bandwidth

A loop that crosses over at 50 rad/s with a 1 ms total delay loses 50 × 0.001 = 0.050 rad = 2.9° of phase margin: harmless. With a 20 ms delay (a slow serial round trip) it loses 50 × 0.02 = 1.0 rad = 57°: the loop is nearly unstable. This is why the fast loops stay in the firmware.
The rule: sample at least 10 times faster than the loop bandwidth, and keep the total delay well below one tenth of the crossover period.
Velocity from an encoder is noisy. With a count of 0.218 mm, differencing counts every 1 ms gives a resolution of 0.22 m/s; every 20 ms only 0.011 m/s (but a slower loop). Use the time between counts at low speed, a velocity observer, or a high-resolution encoder, and filter lightly.
The derivative term amplifies this noise, which is why the D gain is small and the derivative is filtered.

Saturation, friction and backlash

Effect	What it does	Remedy
Voltage and current saturation	The command cannot be reached; the integral winds up	Anti-windup; limit the reference rate; a back-calculation scheme
Static friction (stiction)	The motor does not move for small commands, then jumps; the integral builds up and causes a hunting limit cycle	Friction feedforward; dither; a small dead-band; a stronger P gain at low error
Coulomb and viscous friction	A constant offset and a speed-proportional drag	Feedforward from a measured friction curve
Backlash (gear play)	A dead zone when the direction reverses: oscillation if the loop closes around it	Put the encoder before the gearbox for the speed loop; anti-backlash gearing; a low-gain approach in one direction
Compliance and resonance	A belt, a coupling or a long arm acts as a spring: a resonance limits the bandwidth	A notch filter at the resonance; stiffer parts; keep the bandwidth below the resonance
Quantization and PWM ripple	Small steady noise on the output	A faster PWM and finer encoder; filter the feedback

Joint control of an arm: gravity and coupling

An arm joint is a motor driving a load whose torque depends on the pose: gravity pulls with m g l cos θ, and the other joints’ motion reacts through inertia coupling and Coriolis terms. A PID alone must fight these disturbances; gravity compensation feeds the computed gravity torque forward so that PID only handles what is left.

Worked example: the gravity torque on one joint

A link of 1.0 kg with its center of mass 0.2 m from the joint needs a holding torque of m g l cos θ, where θ is its angle above horizontal:

Angle θ	Holding torque
0°	1.96 N·m
30°	1.70 N·m
60°	0.98 N·m
90°	0.00 N·m

The maximum is 1.96 N·m (horizontal), so a motor with a continuous torque rating below that cannot hold the link out straight without help (a gearbox, a counterbalance spring, or a brake). With a gearbox of 50:1 at 80 % efficiency the motor sees 49 mN·m. The same calculation with the payload gives the torque to size the joint (see Frames, Transforms and Arm Kinematics for J^T F).

Computed-torque control uses the full dynamic model of the arm (inertia matrix, Coriolis, gravity) to cancel its nonlinearities; the libraries (Pinocchio, KDL, ros2_control’s gravity compensation) compute it from the URDF.
Brakes and holding torque: a joint under gravity needs a brake or a non-back-drivable gearbox so that a power loss does not drop the load.
Collaborative robots add torque sensing in the joints for force and collision limits (see Industrial Robotics).

A tuning and commissioning checklist

Check	Why
Direction, units and encoder scale verified	A sign error turns negative feedback positive
Current, speed and position limits set before the first move	The first test is the dangerous one
Plant measured (gain and time constant); feedforward set	Tuning starts from the model, not from zero
Loop rates fixed; delays measured	Delay limits the bandwidth
Anti-windup and output limits active	Saturation is normal, not an error
Step and sine responses recorded at several amplitudes	Finds friction, backlash and resonance
Tested loaded and at both ends of the speed range	The empty robot is not the real one
Watchdog and command timeout in the firmware	A lost command must not leave a motor running
Parameters and results in version control	The tune is part of the software

Robotics Tools: the overview of this guide.
Mobile Robot Kinematics and Odometry: the wheel speeds these loops produce.
Frames, Transforms and Arm Kinematics: joint trajectories from Cartesian paths.
Motion Control and Actuators: motors, drives, servo tuning and sizing.
Filters, PID and Signal Code: PID code, anti-windup and the Ziegler-Nichols method.

FAQ

My motor oscillates or buzzes. What do I change?

Lower K_p (and K_d), check for delay or a slow loop rate, and look for backlash or a resonance in the mechanics. A filter on the feedback often cures a buzz.

Why does the robot overshoot after being held at its limit?