Another Two-Wheel Self-Balancing Robot

This blog post was due a very long time ago, what took me so long? Well, when the blog post about the v1 went live, the v2 of the robot was already complete, and I had already posted videos of it working in different situations. But when it came to write the blog post, I realized I couldn’t really explain the math behind what I did and, frankly, that’s because I didn’t really grasp what I was doing. I will dare to say that actually I didn’t know that I didn’t know what I was doing. The understanding came to me recently, after following a variety of courses on non-linear control systems during my MSc (at the time of this project, I was still doing my BSc).

I also had some doubts on how to correctly set the simulations. I tried asking some professors at the time but, again, I didn’t even know what to ask and I ended up not making the right questions. I was in that strange limbo where I could follow along on reference papers, mostly understand, but kept having a thing or two that kept bugging me and I couldn’t really explain.

And yet, the project worked, and I had it not only keeping balance, but also moving to a desired velocity, rotating on the spot, climbing slopes.

I ended up revisiting the project lately, now with the knowledge of a MSc in control engineering. In the rest of this blog post I will go through what I had done at the time, and what I have done now, compare my understanding of the time with my current one, and explain what I got wrong and why. I will also highlight some problems with my choices on the v2, and link them to sensible control theory concepts.

As a recap, I wanted to replace the two puny gearmotors on the v1 with two stepper motors, so that the variable that I use to control the motors would be angular velocity and not torque, for which I had to find a weird relationship to voltage.

Gotcha #1: voltage/torque relationship #

To move the robot (and keep balance), the motors need to exhert some torque. This torque is computed by a controller (a PID in my case) which takes as input the error between the desired and actual orientation of the robot. However, the only way I have to move the motor –and thus exhert a torque– is by applying some voltage to the rotor of the two brushed DC motors.

What I didn’t really appreciate at the time is that the motor itself is another dynamical system and the relationship between voltage and torque cannot be derived a priori like I did for the v1. Well actually it can, but the relationship would only be valid at steady state. In general, though, there will always be some modeling or measuring error that causes the actual steady state torque value corresponding to some voltage to deviate from the predicted one.

To properly exhert a desired torque by applying some voltage, we need yet another controller for the motor alone (a PI controller should be enough for a DC motor) which takes as input the error between the desired and actual torque and applies the right voltage to the motor in a dynamical way. These are usually called low-level controllers in robotics. They abstract the actuator (such as the motor) so that the high-level controller (the one that keeps the robot balanced in this case) doesn’t become too complex. Low-level controllers come with their own set of conditions on bandwidth and sampling time, but we don’t need to worry about it for now: I will explore torque control for a brushed DC motor in another project.

I looked at two different papers explicitely building self-balancing robots using stepper motors, [1] and [2]. They use two approaches that, at the time, I thought were quite different, but turns out are actually two different expressions of the same control technique. This is strictly related to nonlinear systems, and I had no idea about it at the time.

Let’s follow through [1], like I did originally. Take the full model of the robot and linearize is around $\theta = 0$, i.e. the equilibrium point.

\begin{equation} \begin{cases} (I_b+m_bl^2)\ddot{\theta} + m_bl\ddot{x} - m_bgl\theta = -(\tau_r + \tau_l) \\ (m_b + \frac{2I_w}{r^2} + 2m_w) \ddot{x} + m_bl\ddot{\theta} = \frac{\tau_r + \tau_l}{r} - 2b_w\frac{\dot{x}}{r^2} \end{cases} \end{equation}

Then again, ignore yaw and set $\tau = \tau_r + \tau_l$. Like last time pass into the Laplace domain. Solve one of the two equations for $tau(s)$ and plug it into the other equation. Then solve for $\frac{\theta(s)}{sX(s)}$, as so to obtain a transfer function of the desidered output $\theta(s)$ using the input $sX(s)$, meaning the angular velocity of the motor.

Gotcha #2: kinematic model #

At the time I briefly though that, by doing this, I had obtained a kinematic model, i.e. a model in which only generalized positions and velocities (both cartesian and polar) appear, and control inputs also appear exclusively at the velocity level (never in position). This is clearly not true, since accelerations and torques appear.

Gotcha #3: dynamic extension #

The linearized model above does have a velocity as input, and is useful to derive a controller, but the full nonlinear model must be used in simulation to properly evaluate the performance of the controller.

My biggest doubt was: if my controller computes a velocity, but my model takes a torque as input, how can I simulate it? I was convinced I needed to modify the model so that it would take a velocity as an input, or take the derivative of the control action (which would be a acceleration/torque) and then apply it to the model, but everything led to strange behaviours when putting it all together in simulink. I was kind of close to the solution.

For a nonlinear system $\dot x=f(x, u)$, we can define the concept of relative degree. It has many definitions, but the easiest is that the relative degree is the number of times one needs to derive the output of a system for the input to appear. For this robot, the dimension of the system is $n=4$: two variables for cartesian and angular position $(x, \theta)$ and the two variables for cartesian and angular velocity $(\dot x, \dot\theta)$. By considering the output as $y=\theta$, the relative degree is $r=2$, as the first time $u$ appear is in the second derivative of the output $\ddot y = \ddot\theta$.

Nonlinear systems can also be put into a particular form, called normal form, in which the dynamics cleary splits into two parts depending on the relative degree. The first part (call it a vector $z$) is a chain of integrators with an input which is, in general, nonlinear. The second part (call it a vector $\eta$) is whatever is left. When we try to force the output of the system to zero (which in this case means trying to keep the robot balanced), this remaining part is called zero dynamics. The most important characteristic of the zero dynamics is that it is unobservable when measuring the output alone.

For the sake of simplicity, assume that the system is expressed in the following form, derived from the Lagrangian dynamics:

\[ M(q) \ddot q + \eta(q, \dot q) = u \]

where $M(q)$ is the inertia matrix, and $\eta(q, \dot q)$ collects all nonlinear terms (coriolis/centrifugal forces, dissipation and gravity terms).

\[ \begin{aligned} M(q) &:= \begin{bmatrix}M_x & M_{x \theta} \\ M_ {x \theta} & M_\theta \end{bmatrix} =\left[\begin{matrix}\frac{2.0 I_{w}}{r^{2}} + 1.0 m_{b} + 2.0 m_{w} & 1.0 l m_{b} \cos{\left(\theta{\left(t \right)} \right)}\\1.0 l m_{b} \cos{\left(\theta{\left(t \right)} \right)} & 1.0 I_{b} + 1.0 l^{2} m_{b}\end{matrix}\right] \\ \eta(q, \dot q) &:= \left[\begin{matrix}\frac{2.0 b_{w} \frac{d}{d t} x{\left(t \right)}}{r^{2}} - 1.0 l m_{b} \sin{\left(\theta{\left(t \right)} \right)} \left(\frac{d}{d t} \theta{\left(t \right)}\right)^{2}\\- g l m_{b} \sin{\left(\theta{\left(t \right)} \right)}\end{matrix}\right] \\ u&:= \left[\begin{matrix}\frac{\tau{\left(t \right)}}{r}\\- \tau{\left(t \right)}\end{matrix}\right] \end{aligned} \]

Also set, for later:

\[ \begin{aligned} q&=\begin{bmatrix}x \\ \theta\end{bmatrix} :=\begin{bmatrix}x_0 \\ t_0\end{bmatrix} \\ \dot q&=\begin{bmatrix}\dot x \\ \dot\theta\end{bmatrix} :=\begin{bmatrix}x_1 \\ t_1\end{bmatrix} \\ \ddot q&=\begin{bmatrix}\ddot x \\ \ddot\theta\end{bmatrix} :=\begin{bmatrix}x_2 \\ t_2\end{bmatrix} \end{aligned} \]

(The steps to derive this form can be found in [3], [4], and implemented in jupyter notebook in the project repository)

Input at the torque level: When the input is a torque (thus appears in the expression of $\ddot x$ and $\ddot\theta$), the relative degree is $r=2$. The first part of the dynamics is $z=\begin{bmatrix}\theta & \dot\theta\end{bmatrix}^T$ , while the zero dynamics is $\eta=\begin{bmatrix}x & \dot x\end{bmatrix}^T$

The zero dynamics can be proved to be stable using the Lyapunov Function $V(\eta)=\frac{1}{2}M_x \dot x^2$, which is positive definite at $(x, \dot x)=(0,0)$. Its derivative boils down to

$$ \dot V(\eta) = \begin{matrix}\frac{x_{1}^{2} \left(- 8.0 I_{w} - 4.0 m_{b} r^{2} - 8.0 m_{w} r^{2}\right)}{r^{2} \left(4.0 I_{w} + 2.0 l m_{b} r + 2.0 m_{b} r^{2} + 4.0 m_{w} r^{2}\right)}\end{matrix} $$

which is negative semidefinite since all physical parameters are positive. It makes sense that the origin is only a stable equilibrium for the zero dynamics, and not an asymptotically stable one, as the robot can keep balance at any point $z$ at not just the origin. The stability of the origin for the zero dynamics is a necessary condition to regulate the output to zero (in this case, keeping balance).

This also highlights an incorrect statement in part 1: the robot does not have two just equilibrium points at $\theta=0, \theta=\pi$, but has a continuum. The robot has equilibrium points at any $(x, \theta, \dot x, \dot \theta)=(x, k\pi, 0, 0)$, for $x\in\mathbb R, k\in\mathbb N$.

Input at the velocity level: Let us say, for the sake of the argument, that in this case the dynamics equation would be modified so that there is no torque input ($\tau=0$) and the input were a velocity added directly to $(\dot x, \dot\theta)$. Something like:

\[ \begin{aligned} \dot t_0 &= t_1 + \frac{v}{r} \\ \dot x_0 &= x_1 + v \\ \end{aligned} \]

This approach does not really hold from a physics point of view, as the velocity is only a command to the motor, and one should consider the dynamics of the motor to compute the corresponding torque (remember gotcha #1)

In this case the relative degree drops to $r=1$ and a new part is added to the zero dynamics, which becomes $\eta=\begin{bmatrix}\dot \theta & x & \dot x\end{bmatrix}^T$. Furthermore, when $t_0=\theta=0$ $\implies \dot t_0=0$ $\implies v=-r\cdot t_1$. Concluding on (in)stability is a bit more convoluted in this case, as this change of coordinates brings the linear approximation into a form that needs Jordan decomposition. Chetaev and Lyapunov Functions become quickly very convoluted as well. If I had to go by the limited tests I performed at the time, I’d say the equilibrium is unstable.

Dynamic extension: fortunately we have a trick up our sleeve: we can “move” the input up one level of derivation by considering its derivative as a new input instead. Instead of considering the angular velocity of the motors $\omega$ as input, we now consider its derivative $\dot \omega$, which is an angular acceleration and can be made into a torque by multiplying by multiplying by the inertia of the body. The new input $u=\dot\omega$ now appears for the first time in the expressions of $\ddot\theta$ and $\ddot x$, effectively increasing the relative degree from $r=1$ to $r=2$. This is the essence of dynamic extension.

The controller computes a torque $\tau$. To got back from $\tau$ to $\omega = v/r$, we first divide $\tau$ by the inertia of the body to obtain $\dot \omega$, then integrate to obtain $\omega$. Dynamic extension can also be seen as adding as many integrators as needed to the input, until we reach the desired relative degree. We can then command v to the motors, knowing it has been obtained by first going through the integration step, and as far as our math is concerned, everything is fine.

In practice, these integrators are part of the controller. This also clears how to simulate the robot: we can just ignore the velocity level and develop (in simulation) a controller that computes a torque, remembering to add the integrator when we put the controller on the real robot. Actually, if we wanted to be serious about it, we would have to also simulate the stepper motor, so that the full simulation model takes as input a commanded torque, integrates it into the the velocity, simulates the stepper motor, and computes the torque it would actually output. For the sake of simplicity, we just remove the simulation of the actuator.

This is clearly something I couldn’t know at the time, as I hadn’t taken a single course on nonlinear systems, but I have since seen applied in countless examples. If we now go back to the two papers, you’ll see that this is exactly what [2] does, although they don’t call it dynamic extension explicitely. As for [1], choosing $sX(s)$ (the velocity) as input, is clearly the same as using $s^2X(s) \cdot \frac{1}{s}$ (the series connection of the acceleration and an integrator, i.e. a velocity).

A note on implementation #

The first step of implementing dynamic extension entails dividing the torque $\tau$ by the inertia of the body $I_b$ to obtain the angular acceleration of the wheels $\dot\omega$. However correctly estimating inertia is difficult. Hence I modified the model to take a force applied to the wheels as input instead of a torque, with the input becoming

\[ u:= \left[\begin{matrix}F(t)\\- r\cdot F(t)\end{matrix}\right]. \]

(and the controller will need to be re-tuned accordingly). To obtain a linear acceleration $a$, $F$ is divided by the mass of the entire robot $m = 2m_w + m_b$, which, unlike the inertia, is easily measurable with a kitchen scale. The integration step of dynamic extension is computed much the same, obtained the linear velocity $v$, from which the angular velocity $\omega = \frac{v}{r}$. $\omega$ is then converted to the frequency of impulses for the stepper motors.

Below is the code snippet in Arduino:

#ifdef CONTROLLER_PID
  // compute control with PID
  pitchCtrl.Compute();
  bu[0] = angle_output;
#endif

  // Dynamic extension

  // The output of the controller is the force to accelerate the body
  // Divide by mass (easily measurable) to obtain linear acceleration
  acc = bu[0] / MASS;
  // Integrate (backward euler) to obtain linear velocity
  velocity = velocity + CONTROL_PITCH_SAMPLING_TIME*acc;

  // saturate the maximum angular velocity
  velocity = constrain(velocity, -MAX_VELOCITY, MAX_VELOCITY);

  // Divide by radius to obtain angular velocity
  double ang_velocity = velocity / WHEEL_RADIUS;

  // Set direction of the motors
  if(ang_velocity >= 0){
    digitalWriteFast(MOT_DX_DIR, HIGH);
    digitalWriteFast(MOT_SX_DIR, HIGH);
  }else{
    digitalWriteFast(MOT_DX_DIR, LOW);
    digitalWriteFast(MOT_SX_DIR, LOW);
  }

  // frequency of impulses for needed angular velocity
  double frequency = abs(ang_velocity / RAD_PER_STEP);
  // half period of impulses in microseconds
  half_period0 = max(1e6 / (2*frequency), MAX_HALF_PERIOD);

A timer is then setup (using the Pico SDK timer callbacks) to send pulses every period. The sampling time for the PID controller is set to 2ms. The complete code can be found in the project repository.

Gotcha #4: Instability and the wheels #

Technically, stepper motors are actually controlled in position. Every time an impulse is commanded, the motor takes one step. Velocity can be controlled by modulating the frequency of the impulses.

The angular displacement of each step is fixed. The Nema 17 motors I’ve used move by 1.8° per step, a total of 200 steps per revolution. Depending on the driver, there is also the possibility of microstepping by 1/4, 1/8, 1/16 or even smaller fractions of a step, practically increasing the number of steps per revolution. While the steps are discrete, making them as small as possible with microstepping makes them appear as if they were continuous.

I also used this model to print wheels from a parametric OpenSCAD model. The outer diameter of the wheels is however generated by approximating a cylinder with multiple segments. The first iteration of the wheels I printed were too rough and the robot tended to get stuck in one of the segments, especially for small movements near the equilibrium, eventually becoming unstable because it couldn’t move. You can catch a glimpse of this into these videos.

I later switched to new wheels, with a 3D printed hub and a laser-cut disk, with rubber bands on the outside for grip. This is the same technique we used for iFab. With the outer circumference now being perfectly circular, even 200 steps/revolution with 1/16th microstepping is enough for smooth motion.

Results #

Here is a short video of the latest PID+dynamic extension results.

Balancing using PID and dynamic extension

There’s a whole playlist of older results here.

Some experiments and future work #

I tried experimenting with a different controller, the Linear Quadratic Regulator, always computed on the linear approximation of the full model. Unlike PID, which is an output feedback control technique, LQR is a state feedback technique. I tried implementing a Linear Quadratic Estimator (i.e. the steady state Kalman Filter) to estimate the whole state from measurements of the angle $\theta$, however the estimation quickly diverges from reality as soon as the robot exits from the region of validity of the linear approximation. In the end, tuning both LQR and LQE proved to be very difficult. I also suspect this is due to an innacurate model, which would make sense. Having a working PID controlled, one could experiment with collecting input-output data from the robot and performing some process of parameter identification to increase the accuracy of the model.

I also tried adding a friction term on $\dot \omega$, to represent the internal friction of the motors. This brings the uncontrolled simulations, which would otherwise oscillate for a long time minimal damping, more in line with the behaviour of the real robot. Another reason to perform parameter identification.

But I’m happy with the results for now, and I happily declare this project finished.

References #

  1. (Dynamic extension in Laplace domain) Kung, Fabian. 2019. A Tutorial on Modelling and Control of Two- Wheeled Self-Balancing Robot with Stepper Motor.
  2. (Dynamic extension with integration) Iacob, Casian-George. 2020. “Linear and Angular Position Control of a Custom Built Stepper Motor Driven Self-Balancing Robot.” 2020 24th International Conference on System Theory, Control and Computing (ICSTCC), October 8, 648–53. https://doi.org/10.1109/ICSTCC50638.2020.9259706.
  3. Frankovský, P., L. Dominik, A. Gmiterko, I. Virgala, P. Kurylo, and O. Perminova. 2017. “Modeling of Two-Wheeled Self-Balancing Robot Driven by DC Gearmotors.” International Journal of Applied Mechanics and Engineering 22 (3): 739–47. https://doi.org/10.1515/ijame-2017-0046.
  4. Grasser, F., A. D’Arrigo, S. Colombi, and A. C. Rufer. 2002. “JOE: A Mobile, Inverted Pendulum.” IEEE Transactions on Industrial Electronics 49 (1): 107–14. https://doi.org/10.1109/41.982254.
  5. Project repository. Be sure to check out the interrupt branch, or the other branches for older work.

You can read the old, unfinished, version of this post here.