K.I.W.I, aka ‘Kinetic Inverted Wheeled Invention’ is the next project on our journey forwards. You may have noticed a trend in naming: I really like bacronyms, no matter how forced. This project will be a little more involved than QUAIL, as an inverted pendulum is inherently unstable, requiring us to do some analysis before getting into the design. This is one of my favorite parts of working on robots, there is something really satisfying about being able to do a bit of math to predict how the robot will behave. This intuition will play into the choices we make through the rest of development, things like hardware, software and electrical will all be influenced by what we infer about the system in the following sets of equations.

On top of the extra required mathematical analysis, I’d like to use KIWI as a chance to do a bit of exploration. For each of the areas of development; mathematical, hardware, software etc, I want to try at least one new thing and see where it gets me. There is definitely a chance I’ll just be making more work for myself, but regardless it’s a good way to expand my horizons. Before we start putting hammer on anvil, let’s grab a pen and paper, see how much we can glean about our system, starting with a good ol’ fashioned Free Body Diagram!

For now, I’m only interested in the 2d case along the sagittal plane, as this is where most of the interesting dynamics will be found and accounted for. When doing background research, it seemed that most of the examples for deriving the equations of motion treat the chassis as a massless rod, with a spherical mass at the end. While this definitely helps with reducing the complexity, I think we can do better than that without too much extra effort and account for inertia. There are a lot of interesting things about the process of using the Lagrangian to find the equations of motion, I’d really like to do a full post about the Lagrangian sometime.

Deriving the system dynamics

We can define our state space to be the rotational positions of the wheel $\omega$ and the torso $\theta$, giving $\begin{bmatrix} \omega \\ \theta \end{bmatrix} \triangleq \begin{bmatrix} q \end{bmatrix}$

If we assume no external forces influence our system, the Lagrangian can be written as: $L(q, \dot q) = T - V$, Where $V$ is the potential energy of the system, and $T$ is the kinetic energy of the system. Looking first at the potential energy (We assume both the wheel and the chassis are of uniform density for this model): $$V=\left[m_w* r * g \right] + \left[m_t* (r + \frac{L}{2}\cos\theta) * g \right]$$ Alright, so far, so simple. Moving on to the Kinetic energy, $T$. Whenever I’m doing this, I find that I have an easier time when I separate the translation and rotational velocities, starting with translational: $$ T_{tr} = \frac{1}{2}m_{t}(\dot x ^2*t + \dot z ^2*t) + \frac{1}{2}m_{w1}(\dot x^2_{w1} + \dot z^2_{w1}) + \frac{1}{2}m_{w2}(\dot x^2_{w2} + \dot z^2_{w2})$$ Noting that $\dot x = \frac{L}{2}\dot\theta\cos\theta + r\dot\omega$, and $\dot z = -\frac{L}{2}\dot\theta\sin\theta$. We can also assume that the wheels never leave the ground $z_w, \dot z_w = 0$. We can also sum both wheels to get a total wheel mass/inertia $\frac{1}{2}m_1 + \frac{1}{2}m_2 = m_w$, this gives: $$ T_{tr} = \frac{1}{2}m_{t}\left(\dot\omega^2 r^2 + \dot\omega\dot\theta 2r\frac{L}{2}\cos\theta + \frac{L}{2}^2\dot\theta^2\right) + m_{w}\dot\omega^2 r^2$$ Looks good! Now let’s have a look at the rotational energy: $$T_{rot} = I_w\dot\omega^2 + \frac{1}{2}I_t\dot\theta^2$$ Summing the kinetic energies: $$T = T_{tr} + T_{rot} = \frac{1}{2}m_{t}\left(\dot\omega^2 r^2 + \dot\omega\dot\theta 2r\frac{L}{2}\cos\theta + \frac{L}{2}^2\dot\theta^2\right) + m_{w}\dot\omega^2 r^2 + I_w\dot\omega^2 + \frac{1}{2}I_t\dot\theta^2$$ Now we have all the needed terms, we can turn to the Lagrangian of the second kind to get our equations of motion. At this stage, we can also include any losses in the system. In this case, $b_{drive}(\dot\theta-\dot\omega)$ and $b_{Ground}(\dot\omega)$ describe the losses in the drive system, and the losses from ground friction respectively. This results in: $$\begin{aligned}\frac{d}{dt}\left(\frac{\partial L}{\partial\dot \omega}\right) - \frac{\partial L}{\partial \omega} &= b_{drive}(\dot\theta-\dot\omega) - b_{ground}(\dot\omega) + \tau \\ \frac{d}{dt}\left(\frac{\partial L}{\partial\dot \theta}\right) - \frac{\partial L}{\partial \theta} &= - b_{drive}(\dot\theta-\dot\omega) - \tau\end{aligned}$$ From here, it’s just calculus (Or a symbolic solver of your choice ;)) resulting in a pair of nonlinear differential equations $$\begin{aligned} ((m_t + m_w)r^2 + I_w)\ddot\omega + (m_tr\frac{L}{2}\cos(\theta))\ddot\theta &= - b_{ground}\dot\omega+b_{drive}(\dot\theta - \dot\omega)+m_tr\frac{L}{2}\dot\theta^2\sin(\theta) + \tau \\ (m_tr\frac{L}{2}\cos(\theta))\ddot\omega + (m_t\frac{L}{2}^2 + I_t)\ddot\theta &= -b_{drive}(\dot\theta - \dot\omega)+m_tg\frac{L}{2}\sin\theta - \tau \end{aligned}$$ We can re-arrange these equations to fit the standard form of a nonlinear state space: $$M(x)\dot x = F(x) + u$$ Given that the wheels are a continuous joint, we can ignore the positional variable, we can focus on only 3 variables: $\begin{bmatrix} \theta & \dot\omega & \dot\theta \end{bmatrix}$

Combining our equations into state space form gives: $$ M(x) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & (m_t + m_w)r^2 + I_w & m_tr\frac{L}{2}\cos(\theta) \\ 0 & m_tr\frac{L}{2}\cos(\theta) & m_t\frac{L}{2}^2 + I_t \end{bmatrix}$$ $$ F(x) = \begin{bmatrix} \dot\theta \\ -b_{ground} \dot\omega + b_{drive}(\dot\theta - \dot\omega)+m_tr\frac{L}{2} \dot\theta^2\sin(\theta) \\ -b_{drive}(\dot\theta - \dot\omega) + m_tg\frac{L}{2}\sin(\theta) \end{bmatrix}$$ $$u = \begin{bmatrix}0 & \tau & -\tau\end{bmatrix}^T$$ From here, we can integrate our system, to see if our equations of motion work. I wrote a simple python script to integrate our system. I took a lot of ‘inspiration’ for my integration from a blog post on N-Body Orbit Simulation, found here: The Cyber Omelette, I think this is one of the best explanations of rk4 integration I’ve ever stumbled across. Using this, I created the following function:

def rk4_int_nl_static(dt, xn):
    """
    Fourth order runge-kutta state space integration
    Params are numpy matrices. Note: No input
    """
    # acceleration at the start of the timestep
    k1 = dt * np.matmul(m_inv_mat_func(xn), f_mat_func(xn))
    # acceleration 0.5 timesteps (dt *= 0.5)
    k2 = dt * np.matmul(m_inv_mat_func(xn + k1 / 2), f_mat_func(xn + k1 / 2))
    # acceleration at dt *= 0.5, under the acceleration of k2
    k3 = dt * np.matmul(m_inv_mat_func(xn + k2 / 2), f_mat_func(xn + k2 / 2))
    # acceleration at dt *= 1 under acceleration k3
    k4 = dt * np.matmul(m_inv_mat_func(xn + k3), f_mat_func(xn + k3))

    return xn + (k1 + (2 * k2) + (2 * k3) + k4) / 6

m_inv_mat_func() returns the inverse of the M matrix given xn, and f_mat_func() similarly returns the F matrix given xn. this allows me to visualise the behavior of the system under just gravity. During my undergrad, I learned a very good trick for verifying that your equations of motion are correct. It’s all in the potential and kinetic energies, if your losses are zero, the total energy should remain constant. A simple plot is a great way of confirming this.

With these plots, we can observe the exchange of potential and kinetic energy in the periodic motion denoted by $\theta$. The effects of our damping value, $b$ can also be seen, with the gradual reduction of the total energy in the system. It’s safe to say that our system is behaving as expected, so our equations are probably correct. Throughout the build, we will be re-visiting these equations to ensure the other elements of the system obey the constraints we’re contending with. I think the value of good visualisation would be difficult to overstate, there is so much value to be gained from getting a good understanding of how your system is behaving.

At the start of this post, I mentioned that I had set an additional goal of investigating at least one new thing in each field. For this post, at least two thirds of the total time I spent on getting this post done was spent attempting to create a new type of 3d plot of the system. There a a million and one phase portraits across the Internet of the inverted pendulum, but I had an idea for something I hadn’t seen before. I thought I may be able to somehow combine plots of the potential energy, overlayed with the phase portrait and acceleration across the state space to create a 3d plot that would encapsulate the behavior of the system in a novel way. I think I got kind of close, but I ran into so many issues with getting matplotlib’s 3d quiver to look decent. I tried all kinds of things! Eventually, I decided I would just accept what I have, finish this post, and perhaps I’ll come back to it at some point in the future to try and improve it. I seriously considered just pretending I didn’t even try to create it, but I think it would have been disingenuous to do so. Failure is part of the !!FUN!!, I may as well just get comfortable with it.

And with that, we’re done (for now) with the analysis! The next stage of the build is designing the chassis, which I am very excited to begin working through. I think you’re really going to enjoy some of the things I have lined up! Until next time.