System Modeling
FBD.png

To make my initial approach simple and manageable, I consider only a two-dimensional movement in which BoRam moves in one plane. I assume the two motors symmetrically move together when an identical input is given, although this is unlikely to be the case, it is a good approximation for motors which are manufactured as the same product. A control voltage is applied to the motors, which results in displacement of the wheels and change in angular attitude of the body. A mathematical model for the system is obtained by observing the FBDs in the figure above. The first diagram in the figure indicates the inertial frame. All the equations will be derived in this inertial frame. $h$ is the friction force between the each wheel and surface. $H$, $V$ are the horizontal and vertical force respectively acting among the body and the motor shaft. $l$ is the length from the motor shaft to the CG of the body. and finally $\tau$ is the torque produced by the DC motor.

Applying Newton's law in the third diagram to the horizontal motion of the wheel, and to the angular motion induced by two torques yield

(1)
\begin{align} m\ddot{x}=h-H \end{align}
(2)
\begin{align} \tau-hr=J\dot{\omega} \end{align}

It is assumed that no slip occurs between the wheels and the surface, so the following relationship must hold.

(3)
\begin{align} \omega=\frac{\dot{x}}{r} \end{align}
(4)
\begin{align} \dot{\omega}=\frac{\ddot{x}}{r} \end{align}

Note that $\omega$ is the angular velocity of the wheel in the inertial frame and it is different from $\omega_{enc}$that I used in the DC motor identification section. This will be explained in Coupling between IMU and Optical Encoder. It is important to know this relationship especially when it comes to implementing in the
microcontroller.

The second diagram in the figure illustrates the body dynamics. The rotational motion of the pendulum body about
its center of gravity can be described by

(5)
\begin{align} I\ddot{\theta}=-2\tau+-2Hl\cos\theta+2Vl\sin\theta \end{align}

where $I$ is the moment of inertia of the pendulum body about its center of gravity. The horizontal force, vertical force and torque are applied as pairs on both sides and each pair are assumed to be equal because I consider only two-dimensional motion.

The horizontal motion is given by

(6)
\begin{align} 2H=M\frac{d^{2}}{dt^{2}}(x+l\sin\theta) \end{align}

The vertical motion is given by

(7)
\begin{align} -2V+Mg=M\frac{d^{2}}{dt^{2}}(-l\cos\theta) \end{align}

Since I must keep the inverted pendulum robot vertical, I can assume that $\theta$is small such that $\sin\theta\approx\theta$, $\cos\theta=1$. Then, Eq.(5) through Eq.(7) can be linearized. The linearized equations are

(8)
\begin{align} I\ddot{\theta}=-2\tau-2Hl+2Vl\theta \end{align}
(9)
\begin{align} 2H=M(\ddot{x}+l\ddot{\theta}) \end{align}
(10)
\begin{equation} 2V=Mg \end{equation}

The objective here is to find suitable differential equations that fully describe the system in terms of input $v$, and the and states $x$, $\theta$ and their derivatives. The following describes how to characterize the essence of the system in two differential equations.

With Eq.(1), Eq.(4), and Eq.(9)) substituted into Eq.(2) yields $\tau$ in terms of $\ddot{x}$ and $\ddot{\theta}$.

(11)
\begin{align} \tau=\left(\frac{J}{r}+rm+\frac{rM}{2}\right)\ddot{x}+\frac{rMl}{2}\ddot{\theta} \end{align}

Substituting Eq.(7), Eq.(8), Eq.(3) and Eq.(11) into Eq.(9) yields

(12)
\begin{align} \frac{R}{K}\left(\frac{J}{r}+rm+\frac{rM}{2}\right)\ddot{x}+\frac{RrMl}{2K}\ddot{\theta}+\frac{K}{r}\dot{x}-K\dot{\theta}=v \end{align}

Finally substituting Eq.(9), Eq.(10), Eq.(11) into Eq.(8) yields

(13)
\begin{align} \left(I+rMl+Ml^{2}\right)\ddot{\theta}+2\left(\frac{J}{r}+rm+\frac{rM+lM}{2}\right)\ddot{x}-Mgl\theta=0 \end{align}

Equations Eq.(12) and Eq.(13) describes the motion of BoRam, and these are the equations that constitute the two-dimensional linearized mathematical model of the system and can be expressed in State-Space form as

(14)
\begin{align} \left[\begin{array}{c} \dot{x}\\ \dot{\theta}\\ \ddot{x}\\ \ddot{\theta}\end{array}\right]=\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & A_{32} & A_{33} & A_{34}\\ 0 & A_{42} & A_{43} & A_{44}\end{array}\right]\left[\begin{array}{c} x\\ \theta\\ \dot{x}\\ \dot{\theta}\end{array}\right]+\left[\begin{array}{c} 0\\ 0\\ B3\\ B4\end{array}\right]v \end{align}

The rest of the unknown properties, $I$ and $l$ were found by conducting a reverse engineering on the physical model using CATIA.

CatiaModel.png
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License