The default Segbot balances based on a manually tuned balance point "accelzBalancePoint" as it called in the code. The balance controller tries to keep the robot's tilt such that the current z-axis gyro measurement is equal to the value of accelzBalancePoint.
This code setup has two main consequences:
- Where the robot is calibrated has a large impact on it's balance quality. Any inconsistency in calibration throws off the hard-coded balance point and messes up the Segbot. In ME461 this means that there is a 4 second calibration period any time the robot is started up.
- Any changes to the robot's configuration or center of gravity change where the "true" accelzBalancePoint is.
Our solution to this is to add a PID controller to the balance point, enabling the robot to adjust to changes in its center of gravity, as well as enabling instant calibration.
The concept is to assume that the Segbot balancing controller can get the robot to stay upright at any given accelzBalancePoint. If the balance point is too far forward, to keep the robot balanced, the wheels will constantly be moving forward, and the reverse if the balance point is too far back. In this way the wheel velocity can be used as an error on the balance point, and a PID controller can be added to adjust the balance point.
ImplementationThe following is excerpts of code to implement this controller
/* ----- Main.c ----- */
/* ----- File Header ----- */
// Set accelzBalancePoint to zero
// this makes the robot balance at it's starting angle as a first guess
float accelzBalancePoint = -0.0;
// define PID control variables
float ebal = 0.0;
float ebal_1 = 0.0;
float Iebal = 0.0;
float ebal_dot = 0.0;
float ebal_eff = 0.0;
float Kp_bal = 0.25;
float Ki_bal = 0.15;
float Kd_bal = -0.05;
/* ----- SWI_isr ----- */
// update control variables with new vals
ebal = Segbot_refSpeed - (vel_left + vel_right) / 2.0;
// using the average wheel speed minus ref speed
// enables turning and driving to still work as intended
ebal_dot = ebal * 100 - ebal_1 * 100 + 0.6 * ebal_dot;
Iebal += (ebal + ebal_1)*0.002;
// calculate the balance point "control effort"
ebal_eff = Kp_bal * ebal * fabs(ebal) + Ki_bal*Iebal + Kd_bal*ebal_dot;
// saturate control effort
if (ebal_eff > 10.0) { ebal_eff = 10.0; }
if (ebal_eff < -10.0) { ebal_eff = -10.0; }
// rescale balance control effort
accelzBalancePoint = ebal_eff/20.0;A few key points
- The closed loop error "ebal" is the difference between the desired robot speed and average wheel speed. This allows driving and turning identically to a regular Segbot.
- The PID gains are fairly high here. This results in underdamped Segbot motion, but allows it to quickly adjust to changing center of gravity.
- One critical change to get the robot to quickly adjust to changes is squaring of the proportional error ("ebal * fabs(ebal)"). When the robot's CG changes, it is acceleration of the wheels that allows it to stay balanced. In our tuning, linear controllers could be very stable, but wouldn't adjust fast enough to respond to removing or adding large amounts of weight. Squaring the error gets the fast early response we need, while the integral gain helps the robot converge to an extremely stable balance point.
Here is a quick video of the robot adjusting to changing center of gravity.
CalibrationChanges were made to the calibration in the included code. It is still a two stage calibration, but the motors are disabled and it is much shorter.
Instead of resting the robot on the kickstands, hold the Segbot at roughly the angle it will balance at, as shown in the video. With these changes it is also possible to reset the robot and recalibrate without turning off the motors.
Comments