Balancing Robot Control Model

Question

I am trying to find a control model for the system of a balancing robot. The purpose of this project is control $\theta_2$ by the 2 motors in the wheels i.e. through the torque $τ$ I started with the dynamic equations and went to find the transfer function.

Then I will find the PID gains that will control the robot and keep it balanced with the most optimum response. For the time being I am only interested in finding the transfer function for the dynamic model only.

Here is an example: https://www.youtube.com/watch?v=FDSh_N2yJZk

However, I am not sure of my result.Here are the free body diagrams for the wheels and the inverted pendulum (robot body) and calculations below:

Dynamic Equations:

$$ \begin{array}{lcr} m_1 \ddot{x}_1 = F_r - F_{12} & \rightarrow & (1)& \\ m_2 \ddot{x}_2 = F_{12} & \rightarrow & (2) &\\ J_1 \ddot{\theta}_1 = F_r r - \tau & \rightarrow & (3) &\\ J_2 \ddot{\theta}_2 = \tau - mgl\theta & \rightarrow & (4) & \mbox{(linearized pendulum)}\\ \end{array} $$

Kinematics:

$$ x_1 = r\theta_1 \\ x_2 = r\theta_1 + l\theta_2 \\ $$

Equating (1) and (3): $$ m_1 \ddot{x}_1 + F_{12} = F_r \\ \frac{J_1 \ddot{\theta}_1}{r} + \frac{\tau}{r} = F_r $$

Yields:

$$ \frac{J_1 \ddot{\theta}_1}{r} - m_1 \ddot{x}_1 + \frac{\tau}{r} = F_{12} \rightarrow (5) $$

Equating (5) with (2):

$$ \frac{J_1 \ddot{\theta}_1}{r} - m_1 \ddot{x}_1 + \frac{\tau}{r} - m_2 \ddot{x}_2 = 0 \rightarrow (6) \\ $$

Using Kinematic equations on (6):

$$ (J_1 - m_1 r^2 - m_2 r^2) \ddot{\theta}_1 + m_2 l r \ddot{\theta}_2 = -\tau \rightarrow (7) \\ $$

Equating (7) with (4):

$$ \begin{array}{ccc} \underbrace{(J_1 - m_1 r^2 - m_2 r^2) }\ddot{\theta}_1 &+& \underbrace{(m_2 l r + J_2 ) }\ddot{\theta}_2 &+& \underbrace{m_2 gl}\theta &= 0 \rightarrow (8) \\ A & &B & & C & \\ \end{array} $$

Using Laplace transform and finding the transfer function:

$$ \frac{\theta_1}{\theta_2} = -\frac{Bs^2 + C}{As^2} \\ $$

Substituting transfer function into equation (7):

$$ (J_1 - m_1 r^2 - m_2 r^2) \frac{\theta_1}{\theta_2}\theta_2 s^2 + m_2 lr\theta_2 s^2 = -\tau \\ $$

Yields: $$ \frac{θ_2}{τ} = \frac{-1}{(mlr-B) s^2+C} $$

Simplifying: $$ \frac{θ_2}{τ}= \frac{1}{J_2 s^2-m_2 gl} $$

Comments:

-This only expresses the pendulum without the wheel i.e. dependent only on the pendulums properties.

-Poles are real and does verify instability.

Answer 1

At a glance, you haven't defined your x/y datums so when you have an equation it's hard to tell what you are referencing.

That said, you give an equation:

$$ m_2 \ddot{x}_2 = F_{12} $$

But later, for kinematics, you state what I believe to be correct:

$$ x_2 = r \theta_1 +l \theta_2 $$

Which could also be stated, per your definition, as

$$ x_2 = x_1 + l \theta_2 $$

But, if you take two derivatives of the above, you do not get anything close to the first two dynamic equations.

Using the kinematic equation as an example, I would suggest that the acceleration $\ddot{x_2}$ is due to both the acceleration at the base and due to the rotational acceleration of the pendulum, but again you need to pick a datum and see for yourself. I would suggest:

$$ m_2 \ddot{x}_2 = F_{12} + l \ddot{\theta}_2 $$

Where again, per your definition, this could be rewritten as:

$$ m_2 \ddot{x}_2 = F_r - m_1 \ddot{x}_1 + l \ddot{\theta}_2 $$

See now the similarity between this and your kinematic definition? This also couples rotation to the wheel motion. I didn't read any further than the kinematic definitions, so there may be other issues, but try this first, see what you get, then ask more questions if you have them.