Mathematical Model of Hektor

For the development of a sophisticated state controller a mathematical model of the robot is needed. Engineering mechanics provides two different approaches to derive the governing equations of motion. On the one hand there is the analytical mechanics with Lagrange's equations and on the other hand there is the synthetic mechanics where all reaction forces are calculated explicitly. In the following two sections the equations of motions are derived with both approaches.

Equations of Motion with Lagrage

The balancing robot Hektor can be modeled as a rigid body with mass $m$ , moment of inertia $J$ at the center of gravity and distance $l$ between center of gravity and wheel shaft. For simplicity the angular momentum of the wheels is neglected. The equations of motion can be derived with Lagrange introducing the two general coordinates

$\begin{eqnarray*} q_1 = r \quad q_2 = \phi \end{eqnarray*}$

where $r$ is the horizontal displacement of the wheel shaft and $\phi$ is the angle between the robot's longitudinal axis and the $x$ -axis. Lagrange's equation writes

$\begin{eqnarray}\setcounter{equation}{1} \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q_i}}\right) - \frac{\partial L}{q_i} = Q_i \end{eqnarray}$

with

$\begin{eqnarray}\setcounter{equation}{2} L = T - V \end{eqnarray}$

where $Q_i$ are the non conservative forces and $T,\ V$ are the kinetic and the potential energy of the system. In a first step the dynamic behavior of the unactuated robot is derived so the non conservative forces vanish:

$\begin{eqnarray*} Q_1 = 0 \quad Q_2 = 0 \end{eqnarray*}$

To derive the kinetic and potential energy the position and velocity of the center of gravity is written in an inertial coordinate system:

$\begin{eqnarray}\setcounter{equation}{3} x_{\rm CoG} = \left[\begin{array}{c}l\cos\phi\\0\\r-l\sin\phi\end{array}\right]_{xyz} \quad \dot{x}_{\rm CoG} = \left[\begin{array}{c}-l\dot{\phi}\sin\phi\\0\\\dot{r}-l\dot{\phi}\cos\phi\end{array}\right]_{xyz} \quad \ddot{x}_{\rm CoG} = \left[\begin{array}{c}-l\ddot{\phi}\sin\phi-l\dot{\phi}^2\cos\phi\\ 0\\ \ddot{r}-l\ddot{\phi}\cos\phi+l\dot{\phi}^2\sin\phi\end{array}\right]_{xyz} \end{eqnarray}$

For the kinetic energy the squared velocity is needed an yields to

$\begin{eqnarray*} \dot{x}_{\rm CoG}^2 &=& \dot{r}^2-2l\dot{r}\dot{\phi}\cos\phi+l^2\dot{\phi}^2\cos^2\phi+\dot{\phi}^2l^2\sin^2\phi \\ &=& \dot{r}^2-2l\dot{r}\dot{\phi}\cos\phi+l^2\dot{\phi}^2 \end{eqnarray*}$

The energy components in equation (2) are calculated now to

$\begin{eqnarray}\setcounter{equation}{4} \nonumber T &=& \frac{1}{2} m v_{\rm CoG}^2 + \frac{1}{2} J \dot{\phi}^2 \\ &=& \frac{1}{2} m \dot{r}^2 - l m \dot{r} \dot{\phi} cos \phi + \frac{1}{2}l^2m\dot{\phi}^2 + \frac{1}{2}J\dot{\phi}^2 \\ V &=& m g l cos\phi \end{eqnarray}$

hence equation (2) yields

$\begin{eqnarray}\setcounter{equation}{6} L &=& \frac{1}{2} m \dot{r}^2 - l m \dot{r} \dot{\phi} cos \phi + \frac{1}{2} ( J+l^2 m) \dot{\phi}^2 - m g l cos\phi \end{eqnarray}$

Each term in equation (1) are calculated as follows:

$\begin{eqnarray*} \frac{\partial L}{r} &=& 0 \\ \frac{\partial L}{\partial \dot{r}} &=& m\dot{r} - ml\dot{\phi}\cos\phi \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{r}}\right) &=& m\ddot{r} - ml\ddot{\phi}\cos\phi + ml\dot{\phi}^2\sin\phi \\ \frac{\partial L}{\phi} &=& ml\dot{r}\dot{\phi}\sin\phi + mgl\sin\phi \\ \frac{\partial L}{\partial \dot{\phi}} &=& -ml\dot{r}\cos\phi + ml^2\dot{\phi} + J\dot{\phi} \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right) &=& -ml\ddot{r}\cos\phi + ml\dot{r}\dot{\phi}\sin\phi + ml^2\ddot{\phi} + J\ddot{\phi} \end{eqnarray*}$

Putting all together yields:

$\fbox{\parbox{1.5\linewidth}{ \begin{eqnarray}\setcounter{equation}{7} m\ddot{r} - ml\ddot{\phi}\cos\phi + ml\dot{\phi}^2\sin\phi &=& 0 \\ -ml\ddot{r}\cos\phi + ml^2\ddot{\phi} + J\ddot{\phi} - mgl\sin\phi &=& 0 \end{eqnarray} }}$

Equations of Motion with Synthetic Mechanics

Principle of linear momentum yields

$\begin{eqnarray*} m \ddot{x}_{\rm CoG} &=& F_x - m g \\ m \ddot{z}_{\rm CoG} &=& F_z \end{eqnarray*}$

and principle of angular momentum setup at the shaft yield:

$\begin{eqnarray*} - \ddot{r} ml \cos\phi + (J+l^2m)\ddot{\phi} = mgl \sin\phi \end{eqnarray*}$

Substituting $\ddot{z}_{\rm CoG}$ with the z-component of equation (3) results in the same equations of motion (7) and (8).

Velocity Control of the Wheels

In the following section a automatic control of the wheel velocity is utilized. The velocity controller is implemented in the OpenServo boards which drives the motors so the main controller can command a setpoint velocity to the system. Relying on this inner control loop the differential equation for the shaft displacement simplifies to

$\begin{eqnarray}\setcounter{equation}{9} \dot{r} = v_{cmd} \end{eqnarray}$

Differentiate equation (9) with respect to time and substitute $\ddot{r}$ in equation (8) yields:

$\begin{eqnarray}\setcounter{equation}{10} -ml\dot{v}_{cmd}\cos\phi + ml^2\ddot{\phi} + J\ddot{\phi} - mgl\sin\phi &=& 0 \end{eqnarray}$

Equation (8) together with equation (10) solved for $\ddot{\phi}$ gives the new equations of motion for the robot:

$\begin{eqnarray}\setcounter{equation}{11} \dot{r} &=& v_{cmd} \\ \ddot{\phi} &=& \frac{mlg}{ml^2+J}\sin\phi + \frac{ml}{ml^2+J}\cos\phi\ \dot{v}_{cmd} \end{eqnarray}$

By introducing a new virtual input $u = \dot{v}_{cmd}$ the development of a controller can be further simplified. The real system is still steered by the velocity but for the development of the controller it is assumed that the acceleration can be set directly. In the implementation the output of the controller is integrated before applied to the system. The equations of motion used for the development of the controller yield:

$\fbox{\parbox{1.5\linewidth}{ \begin{eqnarray}\setcounter{equation}{13} \ddot{r} &=& u \\ \ddot{\phi} &=& \frac{mlg}{ml^2+J}\sin\phi + \frac{ml}{ml^2+J}\cos\phi\ u \end{eqnarray} }}$

The developed control scheme can be illustrated with the following diagram:

Search

Navigation

User

Mathematical Model of Hektor

Equations of Motion with Lagrage

Equations of Motion with Synthetic Mechanics

Velocity Control of the Wheels