A roughly 3 kg quadruped on DYNAMIXEL XL430-W250-T servos wobbles while standing and shows almost no visible benefit from IMU correction while walking. The IMU in this build updates at 50 Hz, and that update rate adds a delay that the tilt-correction loop cannot tolerate at useful gain. Work the problem in the order the data travels: IMU, host, gait calculation, servo bus, joint. Each section below sets one thing and ends with a check.
The correction starts as a physical tilt of the body and ends as a goal-position write on the servo bus. Every hop between them adds delay.
| Hop | Delay source | How to measure |
|---|---|---|
| IMU sensing and internal filter | Output data rate (50 Hz here) and any on-chip low-pass group delay | Read the IMU configuration registers; compare filter bandwidth with the output rate |
| IMU to host link | Bus transfer time and driver buffering (link type: read it from your wiring) | Timestamp before and after the read call |
| Estimator (tilt from accel/gyro) | Filter lag, sample-and-hold between updates | Step the body by hand, compare estimator output with raw gyro |
| Gait calculator | Loop period, waiting on inputs | Log loop start time per cycle; look at jitter |
| Servo bus write | Packet length, baud rate, one write per servo versus one grouped write | Timestamp before and after the write |
| Servo position controller and joint | Internal profile, load, compliance | Compare goal and present position on a step |
Check:Compute the age of the IMU sample at the moment the goal-position write leaves the host (mean and maximum). Record both numbers; every later change is judged against them.
Is the IMU reference level before any gain is tuned?
The IMU in this build is not mounted parallel to the ground, so the estimator reports a constant tilt while the body is actually level. A closed loop drives that offset to zero and tilts the body to do it. This looks like a standing wobble or lean regardless of loop speed.
- Disable servo torque and support the body on a surface verified level.
- Subtract the mean offsets in the estimator output, not in the servo goals.
Check: With the body level and torque off, the corrected roll and pitch mean sits at zero within the noise spread you logged in step 2.
How much phase lag does a 50 Hz sample rate add?
A pure delay of τ seconds lags a sinusoid at frequency f by φ = 360 · f · τ degrees. The frequency at which the delay alone reaches 90° is 1/(4τ), and 180° is 1/(2τ).
| Delay τ | Source (assumption) | Lag at 2 Hz | Lag at 5 Hz | Lag at 10 Hz | 90° at |
|---|
These figures cover the sampling delay only. Estimator filtering, bus latency, and servo response add to them. A loop with gain above unity where total lag reaches 180° sustains an oscillation. That is the delayed, oscillatory register behaviour seen here: the correction arrives after the body has already moved past the point it was correcting.
Design rule (engineering practice, adjust to your margin): keep the delay contribution below about 30° at the loop crossover frequency, which gives fc≤ 1/(12τ). A gait with a step frequency and body dynamics above that cannot be stabilised by this IMU at high gain.
Check:Log roll while standing with the loop closed. Read the wobble frequency from zero crossings or an FFT. If it sits far lower and looks like slow rocking, suspect the mounting offset or foot compliance instead.
| Symptom | Likely cause | Test |
|---|---|---|
| Standing wobble at several Hz to tens of Hz | Loop gain too high for total delay | Halve the tilt gain; the wobble amplitude should collapse |
| Constant lean or drift while standing | IMU mounting offset | Compare torque-off level reading against zero |
| Feet slide, legs barely lift | Foot friction versus servo torque | Read present load while lifting a leg in the air, then loaded |
| Tumble or roll-over without IMU | No attitude feedback at all | Compare with IMU correction enabled |
How do I cut the loop latency before touching the gains?
Apply the fixes in this order; each one removes delay rather than hiding it.
- Raise the IMU output rate. Check whether 50 Hz is a configured output data rate or the sensor's limit. If it is configurable, raise it and match the on-chip filter bandwidth to the new rate. If the part tops out at 50 Hz, replace it with a faster unit.
- Read the newest sample. Run the IMU read in its own thread or interrupt and hand the gait loop the latest value plus its timestamp. A gait loop that polls out of phase with the IMU adds up to one extra sample period of jitter.
- Use the gyro for the fast term. Blend the gyro rate into the tilt estimate (complementary or similar filter) so the damping term does not wait on the slower accelerometer path.
-
Shorten the servo bus. Send all goal positions in one Protocol 2.0 Sync Write and collect present positions with Sync Read instead of per-servo transactions. Check the servo
Return Delay Timecontrol-table item and lower it if it is above the minimum. - Lower the tilt gain until the delay lag at the crossover stays under the 30° rule above. Do this only for the interval until steps 1 to 4 are done.
Check: Repeat the timestamp log from the first section. The mean and maximum sample age must drop, and the standing wobble amplitude must fall with them. If the age drops and the wobble does not, the remaining cause is mechanical (see below).
Would four IMUs, one per leg, reduce the latency?
Four IMUs feeding one gait calculation raise the latency in this build. Each added sensor is another 50 Hz stream, and the calculation waits for the slowest or reads them in sequence over a shared link. Four body-mounted IMUs all measure the same rigid-body attitude, so they add redundancy but no new information. Leg-mounted IMUs measure link orientation instead, which requires per-leg kinematics before the data is useful for body balance.
| Option | Effect on loop delay | Use when |
|---|---|---|
| One faster IMU on the body | Lowers delay directly | First choice |
| Four IMUs read in parallel at a higher rate | No added delay; more data to fuse | Only after the single-IMU loop is stable |
Per-leg foot targets can still be shaped inside the gait calculation from one attitude estimate, without a sensor per leg.
Check: Any multi-IMU trial must show a sample age at or below the single-IMU number from the first section, measured the same way.
How do I keep a fully extended leg from flipping the joint backwards?
The front-right leg reaches full extension just before the stop is triggered, and the next step would flip the joint backwards and drop the robot. The IMU correction is being added on top of a gait trajectory that already sits near the end of the reachable range. Bound both.
- Clamp every joint goal to a range inside the kinematic limit, with a margin before the fully extended pose.
- Saturate the IMU correction term as a fraction of the stride so it cannot push a joint past that range.
- Program the servo min/max position limits in the control table as a second, hardware-side stop.
- Log the commanded joint angles and flag any cycle where the clamp is active.
Check: Walk with the clamp logging enabled. A clamp hit on the front-right joint is now a logged event instead of a fall; if hits are frequent, the gait trajectory needs a shorter stride rather than a wider limit.
Are the servos and feet limiting the gait?
A comment on this build argues the XL430-W250-T servos are marginal for a 3 kg robot with high foot friction: the legs barely lift, and friction can distort the structure. The quoted torque figure is written as 1.4 mN, which is not a plausible servo torque; read the stall torque for your supply voltage from the servo datasheet table instead of relying on that number. Rubber grip pads on the feet and a different leg geometry were also suggested. Rubber raises friction, which loads the servos harder, so change the foot and the leg together.
Estimate the joint torque demand: τjoint ≈ (m · g / nsupport) · d, where m is robot mass (about 3 kg, giving m · g ≈ 29.4 N), nsupport is the number of legs carrying load, and d is the horizontal distance from the joint axis to the foot contact. Measure d on the robot in each pose. Keep the continuous demand well below the datasheet stall figure.
Check: Read present load or current from each servo while the leg is lifted free, then while it carries weight. If the loaded reading sits near the servo's limit, the fix is mechanical: a shorter lever arm, lower foot friction, or stronger servos.
What is the end-to-end test that proves the fix?
- With torque off and the body level, confirm corrected roll and pitch read zero within the logged noise.
- Confirm the mean and maximum IMU-sample age at the goal-position write is lower than the baseline, and that maximum age stays below the period you designed for.
- Tap the body by hand and log the recovery. The response must decay without a sustained oscillation.
- Walk with clamp logging on. Compare a run with IMU correction against a run without it; the run without it should show the tumble, and the run with it should stay upright with no clamp hits on the extended leg.
FAQ
How do I measure the real latency of my IMU feedback loop?
Report the mean and the maximum; the maximum decides whether the loop can go unstable.
How do I calculate the phase lag a 50 Hz IMU adds?
Use φ = 360 · f · τ.
How do I stop a leg from extending past its limit during IMU correction?
Clamp joint goals to a range inside full extension, saturate the IMU correction as a fraction of stride, and set the servo position limits in the control table as a backup stop. Log every clamp event so you can see where the gait approaches the limit.
How do I verify that the latency fix worked?
All three must drop, and a hand tap must decay without a sustained oscillation.