### [Part 11] UPC Project : Programming Overview

The principle idea behind all the self-balancing robots is that it is all about managing unstable equilibrium. Now the word equilibrium indicates that there exist a certain state at which the system will stay there for an indefinite amount of time if its initial condition is zero. The term unstable is to further specify its special property that if a slightest perturbation is applied to the system the equilibrium abruptly breaks and the system starts to wander off from the equilibrium state and never gets back to its initial state. If it does go back to the initial state or at least approach to it at some infinite time scale, it is called a stable equilibrium.

If the system is at a stable equilibrium, it is already self-balancing in a sense that it can always restore back to its initial state under any types of perturbation. The beauty of self-balancing robot is to take any unstable equilibrium system and force it to become stable by means of active feedback.

For example, when balancing a long rod on a palm, at some point in time, it will start to skew in some direction, then you have to move your palm to the same direction to compensate for the falling action in order to keep it upright. In this example, the rod is initially at unstable equilibrium position. If the center of mass of the rod is located precisely on where it stands, it will never fall. But in real world there is always imperfection such as your palm shaking and this will force the system out of equilibrium and the rod will start to fall in a certain direction. Then you get to observe its position, speed, and acceleration at which the rod is falling and move your hand to get the system back to where it equilibriates. If the act of observation is analogous to the measurement of angular position and speed measurement by balancing bots, moving your hand accordingly is analogous to the actuation of motor based on these measurements (mechanical feedback).

That’s it. There isn’t anything more fancy really. All it does is to measure and actuate accordingly. Although I have to say that in a real world, these types of measurement of physical states always suffer from noise and it requires some knowledge in the theory of digital noise control/filtering. Depending on how nice you want your robot to balance, this can be a hell lot of more study or just downloading a piece of already working decent code that other people have written out for you.

Another important aspect is to set out feedback gain constants for your specific system. Even if a system is built under identical blueprints of previous systems, it can never be the same product due to manufacturing tolerances and especially if you are DIYing, this is always true. A slight mismatch in physical dimensions of the system changes natural frequencies at its unstable equilibrium and this has to be compensated by setting up feedback constants right (usually by trial and error).

For my system, I’m measuring angular position and the derivative of angular position (angular speed) using accelerometer and gyroscope. By applying a Kalman filter, I combine these two to give out a most accurate estimate of current angular position as well as the angular speed. The Kalman filter works here beautifully because it automatically compensates for a gyroscope drift.

In principle, one can get either of two by measuring either position or speed only because the Newton’s law says that if one knows position at each time, speed can be derived from it by dividing out position differential by a time length of measurement. Or if the speed is known, position is simply the previous position plus a measured speed times the time scale. In reality, we are forced to use two sensors to measure each separately because accelerometers do not usually have enough response time to measure change in millisecond scale whereas gyroscopes can measure this okay but their outputs drift at a longer time scale so they themselves cannot give out a reliable measurement as an absolute reading. The marriage of these two types of sensors is realized by the introduction of Kalman filter which produces the most accurate estimates of their readings by combining their readings based on some statistical assumptions. The Kalman filter is an advanced topic and I won’t go through the details about it.

Once I had the filtered readings, it’s time to do some feedback design. This is separated into two parts. The very first thing is about maintaining its balance at a fixed location and the second one is about navigating around while maintaining the balance. In my feedback loop design, for the first part, I’m playing with two sets of feedback constants. One is to compensate for the angular speed variation and the other for the square of angular position. The first constant therefore is to counteract how fast the system is changing about its equilibrium and the second constant is to counteract the square of how much the system has already changed under a given time length of measurement. But why use a square? To answer this question, think about this. If the feedback response is to be mathematically exact (or, in other words, a perfect representation of counter-oscillation response function at unstable equilibrium), its feedback gain must be a Maclaurian (or Taylor) expansion of changes in the system. If you can notice from this, I’m doing nothing but keeping the second order term here instead of dropping all the higher order terms. So my square term comes from here. I wanted to do more accurately, I would have to include higher order terms.

Then you may ask, why did you skip the first order term, namely, the feedback compensation constant for the angular position but kept the square of it? This is indeed a very well-rounded question. I happened to notice by trial-and-error that the effect of gain constant that I put in front of angular position tends to manifest itself dramatically when I’m cruising at a constant speed. So I’m just separating it out from the first case and made it into the second case to give it a special importance. This makes partly sense because when I’m cruising at a constant speed, the angular position of UPC stays at a constant value whose magnitude depends on the cruising speed. Mathematically speaking this is not entirely true, but for most of engineering purposes, we ignore the details if the result of application justifies our ignorance of its underlying theoretical background.

For my specific system, I’m using feedback constants of 0.7 for angular speed, 37 for angular position, and 48 for the square of angular position. Here the units of angular measurements are done in degrees (degrees/seconds). Their absolute values bear no significance at all because they are largely affected by the motor and gearbox characteristics as well as other programming specifics. However, I’m thinking their ratios can carry at least some meaning to other balancing bots that has similar dimensions and mass distribution as mine.