Self stabilizing two wheeler robot locks on 0 pitch angle and rate - and then slides to divergence

[ryanflux93]

It’s now two weeks that I’ve been trying to tune my self stabilizing two wheeler to stay put, but to no avail. I’ve literally tried everything I could, including endless fine tuning and seeking internet material. As you’ll see below, I’m sending telemetry and have dived in depth to this, but still can’t figure out what’s wrong.

1x angle PID Actually, this is a PD controller as I'm only using Kp and Kd.
1x speed PID Actually, this is a P controller as I'm only using Kp, in some cases I added Ki too, though I first wanted to see that I manage to prevent it from falling before closing any steady state error.

I guess anyone would tell me that the first step is to tune the angle controller first, before moving on to speed control. This is what I did, and below is the system’s behavior (Please don’t try to follow the legend as it might be confusing, I’ll explain what’s showing):

Cyan is pitch angle. Purple is the speed controller - it’s zeroed out. Brown is the dominant term of the angle controller. The device oscillates around pitch=0. As time passes, it’s less frequent to visit both positive and negative pitch angles, and starts spending more time in positive pitches, briefly closing, until the low point right before diverging. At that low point the pitch is 0 and rate is 0 (not shown). This is where the command is the smallest, and the device now starts diverging forward. In my assumption this is due to the forward momentum of the car right at that point (you can see the pitch command is wide and strong), together with the small command due to the small angle. This causes the device to “lock” on very very small pitch and pitch rate - and then diverge and fall forwards without being able to correct back.

According to this instructional video on balancing two wheelers (I don’t speak Chinese but I had what he said translated), this is a natural phenomenon - a two wheeler CAN’T be kept put in place with just an angle controller, but MUST have a speed controller too.

So, assuming that these results are enough, I went to the next step, which is tuning the speed controller.

Question 1: Assumption correct?

Continuing. The cascaded version of the speed PID looks like this:

u_speed_hold = PID_Update(&speed_pid, target_speed, chassisData.xdot, 0.0f, dt_chassis); // 0.0f is acceleration - not available
…
float combined_angle_setpoint = (target_pitch_angle - u_speed_hold);
//float combined_angle_setpoint = (target_pitch_angle + u_speed_hold);
…
u_pitch_angle = PID_Update(&pitch_angle_pid, combined_angle_setpoint, pitch_kalman1x1.state_estimate, pitch_rate, dt);
…
float u_forward = u_pitch_angle; // To wheels

Assuming for a minute that there should be a - sign in the combined angle setpoint. After tuning the best I could, below are results that I found the most interesting (Please don’t try to follow the legend as it might be confusing, I’ll explain):

Cyan - pitch angle. Brown - dominant term of the overall pitch command. Red - Brown’s portion due to pitch angle. Purple - Brown’s portion due to speed.
Kind of like the one above with no speed controller right? First pitch oscillates, then as time passes moves to visit the positive angles more frequently, then slowly diverges. This is the point where I thought the “You MUST have a speed controller” should come to action. But it turns out the speed controller (purple), which is pretty weak in the case above, is eating away from the angle controller’s command to close theta, and can’t be tuned to both affect the speed and having the angle controller close the angle.
Keeping the “-” sign in the combined angle setpoint never lead me to any see any indication that there’s an effective speed controller, even after lots and lots of tuning efforts.
So in contrast to my own logic, I switched to adding the controls of the cascaded controller:
combined_angle_setpoint = (target_pitch_angle + u_speed_hold);
This did lead me to some cases where I thought I had seen the speed controller in action, as shown below, but still the robot ultimately gets “locked” on zero angle and zero rate, and slides to divergence:

This is forward → backward → forward movement to divergence. Yellow - chassis speed: kinda hard to see but it moves slowly forwards (~t=15-25), then backwards (~t=25-40), then forward and diverges.
Purple - the contribution of the cascaded speed controller to the overall pitch controller (through its proportional term).
Green - output of the speed controller. Blue: Kp term in the speed controller. Orange: Ki term in the speed controller.

Divergence happens right when the integral zeros out, and as shown below, on very very small values of pitch and pitch rate:

This is literally 0.01[degrees] measured by the IMU, and small rate. As mentioned above, this is where I thought the speed controller that “must exist” will take place and put the robot in place. But I never seem to be able to be able to get out of this situation where the angle commands are very small, and the robot “locks”. I’ve tried all the tuning I could, including telemetry analysis.
Question 2: Sign in combined_angle_setpoint when cascading the speed and angle PIDs?

Continuing. One might suggest, why cascading the controllers when you can just do superposition and sum their outputs:

u_speed_hold =...
u_pitch_angle = PID_Update(&pitch_angle_pid, target_pitch_angle, pitch_kalman1x1.state_estimate, pitch_rate, dt); // and not combined target
…
u_forward = u_pitch_angle + u_speed_hold; // To PWM

After tuning the best I could, below are results that I found the most interesting (Please don’t try to follow the legend as it might be confusing, I’ll try to explain):

Cyan: pitch angle, Yellow: chassis speed, Magenta: angle controller rate term, Green: speed controller output x100, grey: pitch controller output.

Note when the divergence happens: 0.2[deg] pitch angle, almost zero pitch rate.

Question 3: If the robot is moving forward with a positive pitch angle, the angle and pitch controllers should have conflicting directions. The angle controller would want to push forward to close the angle (small as it may be), the speed controller would want to push opposite to travel direction. This is the same case when cascaded, though in that case the quantities will be different, rather than the trends. Then how should they both work together? Speed control should be the biggest which is possible as long as it’s not interrupting the angle control from closing the angle? If so, why can’t I get there, based on all the information above? It makes sense that when the pitch angle and
rate are very small then its the speed controller’s “time to shine”. But I’m never able to avoid either the small angle and rate “lock” to divergence or, on the other side, give too strong of a control command that just results in massive unstable oscillations. I highly doubt this is only a “needs finer tuning” situation, because I scanned very finely.

Final note: Please don’t direct me to some internet material/post/code base/similar project :) I’ve read many. Still can’t explain the telemetry, sliding phenomenon, and how to overcome it.

[taha_zarif90]

I’m not an expert in this field, but based on what I’ve seen with self-balancing robots, it looks like your controller is locking into a false equilibrium: pitch ≈ 0 and pitch-rate ≈ 0, but the robot is actually drifting.

Two things often cause this:

1. The speed loop fights the angle loop
If the sign between the two loops isn’t correct, the speed PI tries to fix drift by commanding a pitch that the angle loop cancels.
Then the robot sits at a tiny offset and eventually drifts out.

Something like this usually works better:

angle_setpoint = desired_pitch - speed_output;

2. No real velocity feedback
If velocity is estimated poorly, the controller “thinks” it’s stable even while rolling slowly.
Then the angle error grows until it falls.

Typical fixes:

• angle PD (fast) + speed PI (slow)
• correct cascade direction
• PI anti-windup
• small forward bias to avoid dead-zone

If you want, share your control equations and I can take a quick look. I might be wrong, but this is what your symptoms remind me of.

EDIT:

Also, from the plots it looks like your system is balancing the pitch, but not correcting the wheel drift.
If the controller doesn’t actively hold the wheel position (or at least velocity), the robot will always “slide” even if the angle loop is perfect.

So in practice you need the angle loop to stabilize fast, and the speed/position loop to very slowly pull the robot back to center. Without that outer loop, what you’re seeing is exactly what happens: stable angle → unstable position → eventual fall.

[ryanflux93]

Thank you very much for your comment.

angle_setpoint = desired_pitch - speed_output;

This hasn't worked for me even once, as shown above. Tried to fine tune and tweak as much as possible cause I thought that sign makes more sense, but it just doesn't work.

If the controller doesn’t actively hold the wheel position (or at least velocity), the robot will always “slide” even if the angle loop is perfect.

The speed controller's portion of the total PWM command is the same order of magnitude as the angle controller. What happens when "drifting" is that the total command is so small, that PWM sent to the wheels is the minimum. Although it's the minimum, it's still active, making them continue to roll. The thing is, that no tuning combination ever succeeded in forcing them back once they started this drift (which starts at ~0 angle ~0 rate).

I'm running the speed PI in a slow loop. The Angle PD is running in a fast loop. The sensing of chassis speed is done by wheel encoders. It's not the best, but I measured their noise and when static they measure significantly less than what's measured during the "drifting" or "locking" phenomenon. Nothing value gets saturated or winded up. I've measured the responses of both the wheels backward and forward and compensated for the differences.

[taha_zarif90]

This actually helps narrow things down a lot.
If the robot locks at ~0 pitch, ~0 rate, and then slowly drifts while the wheel PI output stays very small, then it really sounds like a dead-zone problem, not a tuning problem.

Brushless/gear DC motors often need a minimum PWM to even start correcting motion.
If your “minimum active PWM” is already enough to make the robot roll, the controller will never be able to generate a small reverse command to pull it back it stays stuck in that false equilibrium you described.

A few things I’d try:

1. Add a small dead-zone compensation
   Something like:

if (pwm > 0) pwm += pwm_deadzone;
if (pwm < 0) pwm -= pwm_deadzone;

1. so the controller can actually command tiny corrections.
2. Add a small integrator bias in the speed PI
   Not full windup just enough that the PI keeps pushing until the wheels actually move back, instead of settling at the threshold.
3. Check if your wheel encoders have resolution too low near zero
   Some encoders barely register sub-rpm motion → controller thinks speed is zero while the robot is actually rolling slowly.
4. (Optional) Try making the outer loop extremely slow
   If the speed PI reacts too quickly, it will fight with the angle PD. Try dropping Ki and Kp by 5–10× just as a test.

From your description, your loops are logically correct it’s the motor behavior near zero that is breaking the balance, not the math.

If you want, upload one short log where you show:

• pitch
• pitch rate
• wheel speed
• angle PD output
• speed PI output
• final PWM

I can take a look and guess where the dead-zone is biting.

[ryanflux93]

Thank you very much for your comments sir.
I'm applying deadzone according to these values:

#define Min_PWM_Left_Forward 370
#define Min_PWM_Right_Forward 370
#define Min_PWM_Left_Backward -230
#define Min_PWM_Right_Backward -230

The reason for it is that I activated both the wheels in both forward and backward directions when the robot was in a fixed place right near the wall and measured the distance in [cm] from the wall where the wheel landed in order to check for differences. here's what it looks like:

I'm not crossing the 600 PWM area, so in my working zone, for a fixed pwm command the motors will drive the wheels further backwards. So i'm compensating for it with a higher minimal command forward. I took the best loop that i currently have (lasts around a minute) and checked where the robot will fall to for different values of the minimum commands, and the -230, +370 looks to give the best results.

unfortunately i'm unable to command tiny corrections: there isn't a clear zone where the motors immediately start to work. this is because of the mechanical configuration that i'm using. between not moving at all, and moving smoothly, there's a PWM area where the wheels will move slightly then stop due friction. so the -230 is the lowest minimal command that will run smoothly without missing cycles.

what i'm basically getting right now is SOME kind of speed control - i can see that the term that comes from cascading the speed loop is on the same order of magnitude as the term that comes straight from raw pitch angle and pitch rate. But as seen in the third figure from the original post, I'm always arriving at a divergence after a minute of so, and after moving back and forth

[taha_zarif90]

If your usable range is roughly [-230 … 0 … +370] and there’s a sticky zone in between where the wheels “twitch then stop”, then I don’t think this is a PID-tuning problem anymore. In that region the controller basically has no continuous control authority: either nothing happens, or you get a fairly big step of motion.

That would also explain why you always end up with a slow drift and then divergence after ~1 minute: once the robot enters that small-angle / small-PWM region, the math wants tiny corrections but the mechanics simply can’t produce them.

So my feeling is:

• with this mechanical setup you’ll never get a true “stand still”; at best you can get a small back-and-forth limit cycle
• any controller (cascade or superposition) will eventually fall into that dead band and then slide away

The only real fixes I can see are on the actuator side (different gearing / motors / wheel friction) or a deliberately non-linear strategy (e.g. accept a small dither motion and control position over that range, instead of trying to hold exactly still).

I might be wrong, but from what you’ve shown it really looks like you’ve hit a hardware limit, not a missing tuning trick.

[ryanflux93]

That would also explain why you always end up with a slow drift and then divergence after ~1 minute: once the robot enters that small-angle / small-PWM region, the math wants tiny corrections but the mechanics simply can’t produce them.

But at a certain point after sliding a bit that math would want bigger corrections and not those tiny ones, so the controller should be able to provide them - but it isn't..

also: every mechanical configuration should have a small "sticky zone". some might be very tiny such zones and much less noticeable, but i would expect even friction doing its job on every assembly.

unfortunately I currently have no option to make changes in the mechanical setup.

I wouldn't mind controlling it to a small position range rather than stand still, though only if i have absolutely no other option

[taha_zarif90]

I might be wrong, but what you describe really feels like the drift is happening inside a zone where the controller simply doesn’t “see” enough error to escape. The pitch stays tiny, the encoder barely moves, and the controller keeps asking for micro-corrections that your motors can’t physically produce. So even though the math would demand a bigger correction once the drift grows, the system never actually gets there it keeps sliding while still inside the dead-zone.

Since you can’t change the mechanics, the only realistic workaround I can think of is to not let the robot sit perfectly still. If it’s allowed to make a very slow, controlled motion instead of trying to freeze at exactly zero, the wheels never drop into that dead zone, and the controller has something to “push against.” Some people add a tiny bias or a tiny oscillation for exactly this reason It’s not ideal, but given your motor behavior, it might be the only practical way to stop that lock-and-slide divergence

[ryanflux93]

hmm what would be the term to push this bias to? is this completely handled in code?

[taha_zarif90]

I might be wrong, but the bias usually goes into the speed loop output, not the angle loop.
The angle PD must stay clean, otherwise the robot will lean permanently.

In practice you add the bias right before the speed loop feeds into the angle setpoint, something like:

float u_speed = speed_PI_output + small_bias;  // e.g. ±20…40 PWM
float angle_setpoint = target_pitch - u_speed;

The bias is just big enough to keep the wheels “alive” so the controller never falls into the dead-zone where small corrections don’t move the motor.

Yes it’s all handled in code, nothing mechanical

[ryanflux93]

I'll be checking this direction as well. Thank you very much sir