Balance 2-wheeled robot without making it drift forward/backward

Question

I'm trying to design a controller to balance a 2-wheels robot (around 13kg) and making it robust against external forces (e.g. if someone kicks it, it should not fall and not drift indefinitely forward/backward). I'm pretty experienced with most control techniques (LQR, Sliding Mode Control, PID etc), but I've seen online that most people use LQR for balancing 2-wheels robots, hence I'm going with LQR.

My problem is that, despite I'm able to make the robot not fall down, it rapidly starts going forward/backward indefinitely, and I don't how to make it mantain a certain position on the ground. What I want to achieve is that, when the robot gets kicked by an external force, it must be able to stop moving forward/backward while mantaining the balance (it's not necessary to mantain a position on the ground, I just want the robot to stop moving). The measurements to which I have access from the sensors are: position on both wheels (x), velocity of both wheels (x_dot), angular position of robot (theta), angular velocity of robot (theta_dot). Since now I tried 2 approaches:

1. set all reference signals to 0 and try to tune the LQR gain. With this (simple) approach I'm not sure if the coefficients of gain K relative to x and theta should have same or opposite sign, because, if for example the robot gets kicked away from its reference for x, the wheels should move in the direction that makes the robot return to the 0 point, but this would make theta go in the opposite direction. When the robot gets kicked, I would like that first theta gets adjusted in order to brake the movement given by the external force, and then x_dot should go in the same direction as theta in order to stop the robot.
2. use best LQR gains that I could find empirically/with MATLAB and use some "heuristic" in order to, given the current state of the robot (x, x_dot, theta, theta_dot), choose the reference signals for the state variables. I tried the heuristic "if x_dot goes forward/backward, then make theta inclide backward/forward", which makes the robot avoid drifting forward/backward in case there're no disturbances, but if I kick the robot it starts oscillating really fast until it falls (I tried to adjust the K gain of LQR to solve this problem, but I couldn't find any that solved it).

Which approach would you suggest me to use? Should I implement some more sophisticated heuristics (any suggestion?) or should I just tune the LQR gain until I find the perfect one? Should I consider using an integrator (for controlling which states?) together with the LQR?

Answer 1

Quick summary:

How to avoid drift when self-balancing:

Jump straight down to the "In summary" section below. Here that section is, quoted, for convenience:

To keep your 2-wheeled robot balanced without letting it drift forward/backward, you should have a linear velocity controller with a command of 0 m/s, rather than just an angle controller with an angle command of 0 deg. Your linear velocity controller calculates a desired acceleration which feeds into your linear acceleration controller, which calculates a desired tilt angle, or pitch, which feeds into your pitch angle controller, which adjusts motor throttles to achieve that pitch (using, for example, a PID or LQR controller, or a physics-based controller you can come up with for this inner, self-balancing loop as well).

To achieve your objective you stated, of no longer drifting or rolling forwards, you'd stop there.

But, I'd go further: what if your vehicle has already drifted or rolled a little bit, wouldn't you want to move it back to where it started to "undo" that drift?

Here's how:...[add a position controller too, which feeds into the velocity controller]...

Details:

Physics-based "cascaded controls", and control systems: the many layers of control

AKA: a full description of all of the necessary control loops for a robust vehicle controller, including for self-balancing systems like 2-wheeled self-balancing Segway-like robots, or quadcopters/drones.

In any complicated control system, you have to have multiple layers of controllers.

From inner-most to outer-most controller, here is what you need:

1. Pitch angle controller: In your case, your inner-most controller sounds like it is pitch angle: I think you are using an LQR controller to adjust wheel motor throttle to control pitch angle. You could also use a PID controller for this, or come up with a physics-based feed-forward solution instead, summed with a PID feedback solution to remove error.

   If you make your pitch angle set-point 0 deg, then the robot will stay stationary standing straight up so long as no outside force acts upon it, and so long as it also began at rest. If you push the robot, it will start to translate linearly (ex: move forward or backwards), at the constant velocity you imparted upon it, while maintaining a fixed, upright angle. Essentially, keeping your pitch angle set-point at 0 degrees makes this the same as giving a shove to a motorless ball or cart--it will keep rolling in the direction you push it per Newton's 1st Law of Motion, which is about inertia: an object in motion stays in motion.

2. Linear acceleration controller: You need to add an outer controller where you adjust pitch angle to control linear acceleration (forwards or backwards).

   Think about it like this: this is a physics problem: the more a 2-wheeled Segway-like robot is tilted forward, the faster gravity causes it to "fall forward". The faster it "falls forward", the faster you must drive those wheels to try to get those wheels back underneath it, to make it maintain a fixed tilt angle rather than continuing to tilt further until it hits the ground. Moving the wheels underneath it to prevent it from falling over causes it to accelerate in that direction.

   For a tilted vehicle at a fixed altitude (for air vehicles; or on a level surface, for ground vehicles) and at a fixed tilt angle, the linear acceleration, a, is: a = g*tan(theta), where g = acceleration due to gravity = 9.81 m/s^2, and theta = tilt angle. Here is a "Balance of Forces" diagram (side view: down is towards the ground and up is towards the sky) I just drew:

   

   Solve for theta (tilt angle), and you get: theta = atan(a/g).

   So, a in a translational (forwards or backwards) direction is in units of m/s^2. Over time (s), this translational acceleration results in a certain translational velocity (m/s^2 * s = m/s). So, if you hold it tilted for a moment, let it accelerate, and then stand it back up straight again, you'll now be continuing forward at a fixed velocity, assuming level ground and no friction. That's what's happening to you when someone pushes or kicks it! To counter this translational velocity, you'll need a velocity controller which applies the necessary acceleration in the opposite direction in order to stop the vehicle's motion.

3. Linear velocity controller: The next controller is a velocity controller. You need to set a desired velocity (ex: 0 m/s to stop the vehicle). In this controller, you adjust linear acceleration to control linear velocity.

   You can then set a tuning parameter, tau [sec], which is the time constant over which period you'd like to achieve your desired velocity. Tune this to be very small for a fast response, and for systems with low inertia, and tune it to be very large for a slow response, and for systems with high inertia. If you tune it too low, the system will have bad jitter and respond to noise--kind of like setting your control loop too fast or your derivative gain too high in a PID controller. If you tune tau too high, the system will be very slow and sluggish to respond. Essentially, tau is like a "gain" tuning parameter, where response_gain = 1/tau. Therefore, a large time constant, tau, results in a slow response or "low gain", and a small tau results in a fast response or "high gain".

   You can see my tau value in my quadcopter flight controller circled in yellow here:

   

   (video link to this moment in time). As shown in the image, it is currently set to 0.75 sec. My notes for that parameter in the image above say:

   time_const, tau (sec) (gain is proportional to 1/tau): 0.75

   <--(increase for "lower gain," decrease for "higher gain")

   This "linear velocity controller" is therefore another layer with a physics-based controller. You need the physics equation of motion for this: dv [m/s] = a [m/s^2] * dt [sec]. Solve for a and you get a = dv/dt. The time period, tau, over which you'd like to accomplish this is your dt (time change, in sec), resulting in a = dv/tau.

   Full example calculation up to this point:

   So, if your actual velocity is 2.5 m/s and your desired velocity is 0 m/s, then the desired velocity change, dv, that you need is dv = 2.5 m/s - 0 m/s = 2.5 m/s. You ultimately need to tune tau as a tuning parameter, through experimentation or simulation and modelling, or both. Let's assume you have chosen an initial tau value of tau = 2 sec. Then, the necessary acceleration you need to achieve this velocity change over that time period tau is a = dv/dt = dv/tau = 2.5m/s / 2 sec = 1.25 m/s^2.

   This liner acceleration required by your linear velocity controller here is your input to the linear acceleration controller above. Solve for pitch angle, theta, from the linear acceleration controller above: theta = atan(a/g) = atan(1.25 m/s^2 / 9.81 m/s^2) = atan(0.12742) = 0.1267 rad x 180 deg/pi rad = 7.26 deg. So, input 7.25 deg (with the correct sign, per your situation) as your set-point into your pitch angle controller in order to begin decelerating from 2.5 m/s to 0 m/s over a time period, tau, of 2 sec.

   Run the inner control loop as fast as is reasonable, perhaps 50 to 500 Hz.

   Run the outer control loops as fast as is reasonable, perhaps 25 to 50 Hz.

   Note that the farther "outside" that your control loop is, the slower you can run your control loop. Self-driving cars, for instance, run their outer-most control loops at around 10 Hz, partly since this is good enough, and partly since the computational complexity is so high that this is all the computers can do.

   In summary

   To keep your 2-wheeled robot balanced without letting it drift forward/backward, you should have a linear velocity controller with a command of 0 m/s, rather than just an angle controller with an angle command of 0 deg. Your linear velocity controller calculates a desired acceleration which feeds into your linear acceleration controller, which calculates a desired tilt angle, or pitch, which feeds into your pitch angle controller, which adjusts motor throttles to achieve that pitch (using, for example, a PID or LQR controller, or a physics-based controller you can come up with for this inner, self-balancing loop as well).

   To achieve your objective you stated, of no longer drifting or rolling forwards, you'd stop there.

   But, I'd go further: what if your vehicle has already drifted or rolled a little bit, wouldn't you want to move it back to where it started to "undo" that drift?

   Here's how: add yet another layer of control as a linear position controller outside your velocity controller!

4. Linear position controller: You will adjust linear velocity over time to control linear position. With your wheel encoders, you can figure out how far you've gone, and control position to get the robot to return back to where it started. Or, you can simply command any arbitrary position to have it drive certain distances and navigate around the room. This is another feed-forward controller based on simple physics/math, where the equation of motion is simply v*t = d, where v [m/s] is velocity, t [sec] is time, and d [m] is distance.

   There are a variety of ways to do this.

   If your goal is to go to a position and stop:

   One way is to command a certain velocity for a given time in order to achieve a desired distance. Ex: command 0.5 m/s for 3 seconds to go 0.5 m/s * 3 sec = 1.5 m. Then, command 0 m/s to stop at that point. You might have to use some empirical data and heuristics where you command the 0 m/s velocity a little early to give the vehicle time to respond and stop right where you want it rather than overshooting.

   This might be called a "tick controller" (I'm inventing this term right now), where you write a function to drive N seconds at X velocity to achieve Y encoder "ticks" of distance movement in that direction, with empirical adjustments as necessary. You could tweak this controller to even be able to handle ticks as small as 1 encoder tick using a rapid velocity pulse for a short amount of time, so as to get to the exact position you want to be at as you get close to your commanded position. Each control loop iteration, you pass a new value of the desired number of distance encoder "ticks" to move, based on where you are now and where you want to be. The interesting thing about this physics-based "feed-forward" controller, therefore, is that it's implicitly also a type of "feedback" controller, which is weird. Some pedantic academics out there have probably come up with some special ways of talking about this, perhaps even with some special terms for it, but I don't know what they are.

   An alternative approach would be to command a fixed velocity, ex: 0.5 m/s, until you are within some minimum distance error bound, say: 0.5m, then switch to a PID feedback controller which commands a velocity based on position error. This way, as your position error approaches zero, your commanded velocity will also approach zero, which makes sense. If your PID gains are strong enough, or conversely if your position error is large enough, this is the same thing as just using a PID feedback controller on position where you saturate the velocity command by clipping it to a fixed maximum value. Of course, even for weak gains, at some large enough distance error, the commanded velocity would still hit the maximum allowed (saturated) value and get clipped.

   If your goal is to keep a fixed velocity while following a 2D path at this velocity:

   Then you could set your velocity controller to a fixed value while changing your commanded heading to always point down the path. I do this using a "lead point" technique, or "pure pursuit" algorithm, as demonstrated in my 3 videos here: 1, 2, 3. Since my vehicle is a hovering quadcopter drone, however, I have the luxury of changing my commanded thrust vector rather than my heading, so I can just command a fixed heading if I want (ie: keep the drone always pointing North) while changing the commanded thrust vector to move in different 2D (x-y) directions.

   Getting exact measurements with wheel encoders vs numerical integration:

   While integrating velocity over time will obtain distance, numerical integration or estimating is best used in this case to calculate commanded velocity outputs for the feed-forward parts of your controller which will output a desired velocity command for a certain duration of time in order to achieve a desired change in position.

   With your wheel encoders, you can measure the actual distance traveled instead of estimating it using velocity over time.

   If you do need to estimate position traveled by integrating velocity over time, however, you should use trapezoidal numerical integration, as I explain here: Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls, since it is more accurate than rectangular integration and is trivial to implement. Again, for numerical integration, the simple idea is that velocity [m/s] * time [s] = distance [m].

   Again, remember that you shouldn't necessarily integrate to estimate the actual distance traveled. Rather, measure the actual distance traveled by counting encoder ticks since that's a more-precise measurement.

How to measure encoder "ticks" or distance moved:

Remember that for all these controllers:

1. You read the wheel encoders to determine wheel motion.
2. If you don't have wheel encoders, read the motor hall effect sensors instead. They have worse resolution, but can be used in place of encoders.
3. If you don't have hall effect sensors, but you are using 3-wire (3 phase) brushless motors, then you can read the back-EMF commutation waveforms or cycles instead, to read the commutation frequency and thereby estimate motor RPM (Rotations Per Minute). You can count commutation cycles to get an estimate of position, like from an encoder or hall effect sensors, but with less resolution still.
   1. This is complicated, but can be done in software via a microcontroller such as Arduino (I've done it). What you need to do is wire up ground on an Arduino Nano to your vehicle's battery ground. Then, run another wire to any of the 3 brushless motor's phase wires, through a diode clipping circuit to reduce the commutation voltage from V_bat peak to ~4.5V peak, and then into Arduino pin D8, which is the Input Capture pin for the ATMega328 microcontroller. Use Input Capture in software, via interrupts, to read all pulses on the phase.
   2. Here is what those pulses look like (image source): .
   3. The little pulses are the 8 KHz~16 KHz motor PWM throttle pulses. You need to digitally filter these out in software. The big trapezoidal commutation frequency waves are the commutation waveforms, and their frequency is directly proportional to your motor RPM. Hence, you can measure commutation frequency to calculate motor rotational frequency, or RPM. The relationship is scaled by the number of permanent magnetic poles in the brushless motor. I'd have to dig up my notes, but I believe the equation is something like this: RPM = freq_commutation/(num_magnetic_poles*120). The point is, the commutation frequency can be read in software by a microcontroller's input capture pin, and then converted to motor rotational velocity via a simple equation based on the number of permanent magnetic poles in the motor. Note that higher-end motor drivers (ESCs--Electronic Speed Controllers) use sinusoidal commutation waveforms, which are more efficient and have better torque, instead of trapezoidal, but the commutation frequency principal is the same.

Summary of the cascaded physics-based controllers used

The types of controllers you will be using in your case are these, again, from inner-most to outer-most controller:

1. Pitch angle controller: LQR (from what you said). You could also use PID.
2. Linear acceleration controller: A physics-based feed-forward (the bulk of the control input).
   1. Optionally, add a PID feedback on the error of actual vs commanded linear acceleration to tune it so that the actual linear acceleration approaches the commanded linear acceleration.
   2. Sum the outputs from the feed-forward physics-based controller and from the PID feedback controller.
3. Linear velocity controller: A physics-based feed-forward (the bulk of the control input for low speeds).

   1. Optionally, add another layer of physics control to compensate for air resistance as velocity increases. Drag [N] = C_D*q*A, where C_D [unitless] is your experimental drag coefficient for your particular vehicle's shape properties and how that shape interacts with the fluid of interest (air in our case), q [N/m^2] = dynamic pressure = 1/2 * rho * v^2, where rho [kg/m^3] is air density, and v [m/s] is velocity, and A [m^2] is frontal area (which is geometry-based, since it decreases the more the robot is tilted).

      Let's verify the drag equation with a quick units check: Drag [N = kg*m/s^2] = C_D [no units] * q [kg/m^3 * m^2/s^2 = kg*m*m/(m^3*s^2) = kg*m/s^2 * m/m^3 = N/m^2] * A [m^2] = [N/m^2 * m^2 = N] Yep! It comes out correctly: the right side of the drag equation (C_D*q*A) does indeed have units of Newtons ([N]). I stated the equation correctly.

      THEN, on top of that extra physics-based layer which accounts for air resistance as the robot speeds up:

   2. Optionally, add a PID feedback on error of actual vs commanded linear velocity to tune it so that the actual linear velocity approaches the commanded linear velocity.

   3. Sum the outputs of all controllers.

4. Linear position controller: A simple "physics-based" (integration of velocity with respect to time) feed-forward controller to achieve a desired position change in a given amount of time.
   1. To go to a desired position and stop:
      1. Optionally, add a PID feedback controller (commanding a velocity based on the position error) on actual vs desired position. Keep a total +/- wheel encoder displacement count, and seek to zero it--the larger the position or distance "error", the stronger the commanded linear velocity towards the target point should be.
      2. You may run a physics-based feed-forward controller followed by a PID feedback controller when your position error is within a certain bound, OR just use a well-tuned "tick controller" as I described above, OR, only use the PID feedback controller if you like, saturating the maximum commanded velocity output of course to some reasonable level.
   2. To keep a fixed velocity while following a 2D path:
      1. You should command a fixed velocity while continually moving an aim-point down a target path, continually adjusting your commanded heading to get you to go in the right direction. Consider adjusting velocity dynamically based on things such as commanded turn radius to 1) keep from tipping over, and 2) allow you to follow difficult parts of the path easier. Tuning the lead point forward-projected distance down the desired path is a type of low-pass filter on the sharpness of the commanded path. This may sound a bit abstract, but see my 3 quadcopter videos above and I think you'll see what I mean.
         1. Ex: in this image from this video at this point in time, the blue path is an overhead view of the desired path for my quadcopter, and the red path is the commanded path, which is essentially low-pass-filtered by the lead-point-distance as a tuning parameter. The shorter the lead point distance, the more closely the red commanded path will overlay the blue desired path, and the longer the lead point distance, the more "smoothed" and circular the red commanded path will be. For really large lead point distances, the red commanded path is nearly circular.
            
   3. The above linear position controllers are all "dead-reckoning"-based, using wheel encoder ticks to measure distance by looking at relative distance changes from a known starting location. Adding any absolute position measurements would require an absolute position "truth source", such as off-board camera-based positioning systems such as a Vicon motion capture system or OptiTrack system (used by @Stuff Made Here), acoustic-based positioning systems, GPS, etc., to obtain "truth data" of absolute position. This "truth data" could be used to tweak your robot's internal dead-reckoning position estimates slowly over time.

Final (and philosophical) thoughts

Anyway, the way I see it, that's the idea. That's the type of approach I took on my quadcopter controller: Quadrotor 2 - Physics-based Flight controller demo w/lead point navigation & Arduino interface to RC Tx, and that controller would also work perfectly for a 2-wheeled Segway-like self-balancing robot, too, since the quadcopter is governed by the same principles as the self-balancing robot. Controls has many options and layers. Physics should be part of many if not most of them.

I'll also mention that I believe the entire thing above could be done on almost any computational platform, from a single Arduino Nano (ATmega328 microcontroller), to a powerful desktop Linux laptop. It just requires the right amount of software skill, I think. Engineering is hard. Programming is complicated. But, if you know enough about both, you can do really complicated things on really weak processors like the ATmega328 (Arduino Uno, Nano, etc). I have done some really complicated things on those processors, and I still have a ton more I'd like to do and learn.

References:

1. My answer: Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls
2. My physics notes from college: eRCaGuy_Engineering/Equation_Sheets/Physics 110 (General Physics I) - Constants and Equations - Staples#2.pdf
3. My quadcopter simulation: 1/3 - Autonomous quadcopter guidance and control (physics-based pure pursuit simulation)
   1. See the notes under this video. I jotted down there that a = F/m = g*tan(tilt_angle). That's how I was able to remember how to draw my "Balance of Forces" diagram above.
4. My quadcopter live demo of my physics-based controller I described above: 3/3 - Autonomous quadcopter guidance & control (physics-based pure pursuit demonstration)
5. All 3 of my flight controller videos: https://github.com/ElectricRCAircraftGuy#flight-controller-videos
6. https://en.wikipedia.org/wiki/Equations_of_motion

See also:

1. My answer: Quadcopter PID Controller for distance
2. Potential vehicle I could buy to experiment with my full algorithm as I described it: ELEGOO Tumbller Self-Balancing Robot Car Kit Compatible with Arduino, STEM Kits STEM Toys for Kids. This would be a really fun project.
3. Google search for "how to write an lqr controller" (LQR = Linear Quadratic Regulator)
4. Google search for "cascade control". This refers to the idea of "nested control loops", which is what my controller described above is/does.
   1. https://en.wikipedia.org/wiki/PID_controller#Cascade_control