### [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.