Week 8 — Control Systems Implementation
Day 7 of 7 | Summary & Preview
What We Covered This Week
| Day | Topic | Core Skill |
|---|---|---|
| 1 | Control system architecture | Understand the hierarchy: task → trajectory → motion → motor |
| 2 | PID controller design | Tune gains, implement anti-windup, handle saturation |
| 3 | State-space control & LQR | Multi-joint optimal control via Riccati equation |
| 4 | ROS2 architecture | Build modular pub/sub control stacks |
| 5 | Sensor fusion with EKF | Fuse encoders, IMU, and vision into a single state estimate |
| 6 | Python practice | Full ROS2 + Gazebo control loop with PID |
Key Formulas Cheat Sheet
PID Control (Discrete)
$$u_k = K_p e_k + K_i \sum e_j T_s + K_d \frac{e_k - e_{k-1}}{T_s}$$
LQR Optimal Gain
$$K = R^{-1} B^T P \quad \text{where} \quad A^T P + PA - PBR^{-1}B^T P + Q = 0$$
Kalman Filter Predict
$$\hat{x}{k|k-1} = F \hat{x}{k-1|k-1} + B u_k, \quad P_{k|k-1} = F P_{k-1|k-1} F^T + Q$$
Kalman Filter Update
$$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$$ $$\hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1})$$
Common Pitfalls (Learned the Hard Way)
| Pitfall | Why It Happens | Fix |
|---|---|---|
| Integral windup | Integrator accumulates error while motor is saturated | Implement clamping or back-calculation |
| Derivative noise amplification | Derivative of noisy signal is unusable | Filter the derivative term; use derivative-on-measurement |
| LQR linearization drift | Linearized model only valid near operating point | Re-linearize online, or use nonlinear MPC |
| EKF divergence | Bad initialization, wrong Q/R, or outlier measurements | Mahalanobis gating, adaptive Q/R, reset covariance |
| ROS2 message drops | Default QoS is best-effort, not reliable | Set reliability=RELIABLE for control topics |
| Control loop jitter | Non-real-time OS, competing processes | Use RT-PREEMPT kernel, isolate CPU cores |
The Big Picture
This week connected the pieces from Weeks 5–7:
- Week 5 (Dynamics): We learned $M\ddot{q} + C\dot{q} + G = \tau$ — the plant.
- Week 6 (Sensors): We learned how to measure $q, \dot{q}$ — the feedback.
- Week 7 (Actuators): We learned how to generate $\tau$ — the output.
- Week 8 (Control): We learned how to compute $\tau$ so $q \to q_{desired}$ — the brain.
A robot without control is a sculpture. Control is what makes it useful.
Preview: Week 9 — Advanced Motion Planning
Week 9 moves from control (how to follow a path) to planning (how to find a path):
| Day | Topic |
|---|---|
| 1 | Configuration space and collision detection |
| 2 | Graph search: A*, Dijkstra, D* |
| 3 | Sampling-based planning: RRT, RRT*, PRM |
| 4 | Trajectory optimization: CHOMP, STOMP, TrajOpt |
| 5 | Dynamic constraints and kinodynamic planning |
| 6 | Python practice: Implement RRT* for a 2-DOF arm |
| 7 | Week 9 summary |
We’ll build on the control foundation: once a planner generates a trajectory, the controller (from this week) executes it.
Exercise Before Week 9
Challenge: Take the Week 8 Day 6 ROS2 stack and replace the sine-wave planner with a point-to-point planner:
- Accept a target position via ROS2 service call.
- Plan a smooth trajectory (e.g., cubic polynomial) from current to target.
- Feed the trajectory into the PID controller.
- Verify the arm reaches the target with < 1% overshoot.
Hint: A cubic polynomial trajectory has the form: $$q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$$
Solve for $a_0, a_1, a_2, a_3$ using boundary conditions:
- $q(0) = q_{start}$, $q(T) = q_{goal}$
- $\dot{q}(0) = 0$, $\dot{q}(T) = 0$
End of Week 8. See you in Week 9 — Motion Planning!