×
![]()
Control strategies for an underactuated two-wheeled robot in MuJoCo — PID balancing and full-state LQR
Implementation and exploration of different control strategies for an underactuated system. The robot is a chassis on two wheels. Adapted from Shawn Hymel’s reinforcement learning for robotics course.
Accelerometer:
$$\theta_{\text{acc}} = \operatorname{atan2}\!\left(a_x,\; a_z\right)$$Gyroscope:
$$\theta_{\text{gyro},k} = \hat{\theta}_{k-1} + \dot{\theta}_k \, \Delta t$$Complementary filter, \(\alpha = 0.99\) at \(200\,\text{Hz}\):
$$\hat{\theta}_k = \alpha\left(\hat{\theta}_{k-1} + \dot{\theta}_k\,\Delta t\right) + (1-\alpha)\,\theta_{\text{acc},k}$$Complementary filter fusing accelerometer and gyroscope.
PD on pitch alone, with the lean offset held at zero:
$$u = K_p\left(\hat{\theta} - \theta_{\text{ref}}\right) + K_d\,\dot{\hat{\theta}}, \qquad u \in [-1,\,1]$$Holds vertical on flat ground. It never reads the wheel encoders, so it cannot resist drift, reject a push, or return to where it started.
Tried driving the same pitch loop from the keyboard: a differential offset on the two motor commands for yaw, a pitch setpoint bias for forward and back but failed. Changing the yaw setpoint clips a motor and the robot goes over. Since I never passed position or velocity into the controller, so nothing in the controller could control that.
Linearizing the planar Euler–Lagrange equations about vertical gives a four-state model that couples position to lean:
$$\mathbf{x} = \begin{bmatrix} p & \dot{p} & \theta & \dot{\theta}\end{bmatrix}^{\!\top}, \qquad \dot{\mathbf{x}} = A\mathbf{x} + Bu$$Full-state feedback. Position and velocity feed back alongside pitch.
From the model to the gain vector.
Discretized at \(200\,\text{Hz}\), the gain is the one minimizing
$$J = \sum_{k=0}^{\infty} \left(\mathbf{x}_k^{\top} Q\, \mathbf{x}_k + u_k^{\top} R\, u_k\right)$$That gives four gains, one per state. Each multiplies its own error and they sum into a single motor command, with the keyboard supplying the targets:
$$u = K_\theta\,\hat{\theta} + K_{\dot\theta}\,\dot{\theta} + K_v\left(v - v^{*}\right) + K_p\left(p - p^{*}\right)$$Arrow keys mapped straight to motor torque, no feedback of any kind. A baseline to confirm the pendulum cannot be caught by hand.
An outer loop on encoder odometry producing a lean target clamped to \(\pm 3.5^\circ\), with an inner balance PD tracking it and closed-loop yaw steering alongside. Four coupled gains to tune by hand, and not yet working under teleoperation.
The same two loops, with the gains taken from the LQR solution instead of tuned. Factoring \(K_\theta\) out of the position terms turns one flat gain vector into an outer loop that can be clamped:
$$\theta_{\text{target}} = -\frac{K_p}{K_\theta}\,p \;-\; \frac{K_v}{K_\theta}\,\dot{p}$$| Controller | States fed back | Gains from | Balances | Drives | Holds position |
|---|---|---|---|---|---|
| Pitch PD | \(\hat{\theta},\ \dot{\theta}\) | Hand tuning | Yes | No | No |
| Pitch PD, teleoperated | \(\hat{\theta},\ \dot{\theta}\) | Hand tuning | Until steered | No | No |
| Full-state LQR | \(p,\ \dot{p},\ \hat{\theta},\ \dot{\theta}\) | Riccati solution, given \(Q\) and \(R\) | Yes | Yes | Yes |
Both balance. Only the one with position in its state vector can be driven.