Building An Autonomous Balacning Robot!
This is a project about building some self-balancing robots!
In this post, we will learn how to build an autonomous balancing robot from scratch. The context would cover the design of hardware, measurement of the physcial factors of robots, and combination of the theory and simulation. Finally, we would implement a PID controller to make the robot adjust its pose instantly while tilting.
Robots
There are two version of robot car, right one is is 1st ver. robot car in big size, operating in 12V, but since it's so bulky to balance itself, so there is the 2nd ver. in small size, which is small, compact, modularized and easy to train and verified the control theorem.
Small size
Big size
Physical Model
The robot which is equivalent to a inveted pendulum consists of two parts, the rod and the wheel. The goal is to drive the wheel properly to prevent tilting, so we have to calculate the relative inertial force casued by the acceleration of wheel.
Following pictures are the Free Body Diagram of the two part, rod and wheel.
Free body diagram of the rod and the wheel.
There are two method to deal with the model, one is the classical way in Newton Physics, another one is using Lagrangian method, i.e. energy method.
Newton Method
Angular acceleration of wheel \[ I\omega \ddot{\phi}=\tau -F\cdot R \]
Linear acceleration of wheel C.M. \[ \hat{i}:m_w \cdot\ddot{x}=-P_x-F \] \[ \hat{j}:0=N-P_y-m_wg \]
Angular acceleration of rod \[ I_r\ddot{\theta}=-\tau + P_yL\sin \theta+P_xL\cos \theta \]
Linear acceleration of rod C.M. \begin{align} \hat{i}&:m_r(\ddot{x}-\ddot{\theta}L\cos\theta+\dot{\theta}^2L\sin\theta)=P_x \\\ \cos\theta \hat{i}+\sin\theta \hat{j}&:m_r(\ddot{x}\cos\theta-\ddot{\theta}L)=-m_rg\sin\theta+P_y\sin\theta+P_x\cos\theta \end{align}
Position of C.M. of rod \[ r=x\hat{i}-L\sin\theta\hat{i}+L\cos\theta\hat{j} \]
Velocity of C.M. of rod \[ \dot{r}=\dot{x}\hat{i}-\dot{\theta}L\cos\theta\hat{i}-\dot{\theta}L\sin\theta\hat{j} \]
Acceleration of C.M. of rod \begin{align} \ddot{r}&=(\ddot{x}-\ddot{\theta}L\cos\theta+ \dot{\theta}^2L\sin\theta)\hat{i}-(\ddot{\theta}L\sin\theta+\dot{\theta}^2L\cos\theta)\hat{j}\\\ &=(\ddot{x}\cos\theta-\ddot{\theta}L)(\cos\hat{i}+\sin\hat{j})-(\ddot{x}\sin\theta+\dot{\theta}^2L)(\cos\hat{j}-\sin\hat{i}) \end{align} \begin{align} &\Rightarrow P_y\sin\theta+P_x\cos\theta=\frac{I_r\ddot{\theta}+\tau}{L}\\\ &\Rightarrow m_r(\ddot{x}\cos\theta-\ddot{\theta}L)=-m_rg\sin\theta\frac{I_r\ddot{\theta}+\tau}{L}\\\ &\Rightarrow -(m_rL\cos\theta)\ddot{x}+(I_r+m_rL^2)\ddot{\theta}=m_rgL\sin\theta-\tau \end{align}
and \(P_x=-m_w\ddot{x}-F\), \(F=\frac{-(I_w\ddot{\phi}-\tau)}{R}\)
\begin{align} &\Rightarrow m_r(\ddot{x}-\ddot{\theta}L\cos\theta-\dot{\theta}^2L\sin\theta)=-m_w\ddot{x}+\frac{(I_w\ddot{\phi}-\tau)}{R}\\\ &\Rightarrow I_w\ddot{\phi}-(m_rR+m_wR)\ddot{x}+(m_rRL\cos\theta)\ddot{\theta}=m_rR\dot{\theta}L\sin\theta+\tau \end{align}
No slip condition: \(\ddot{x}=-R\ddot{\phi}\) \begin{align} \Rightarrow \begin{cases} (m_rRL\cos\theta)\ddot{\phi}+(I_r+m_rL^2)\ddot{\theta}=m_rgL\sin\theta-\tau\\\ (I_w+(m_r+m_w)R^2)\ddot{\phi}+(m_rRL\cos\theta)\ddot{\theta}=m_rRL\dot{\theta}^2\sin\theta+\tau \end{cases} \end{align}
Lagrangian Method
Start from the energy equations \begin{cases} \text{K.E. of wheel:}& T_w=\frac{1}{2}m_w\dot{x}^2+\frac{1}{2}I_w\dot{\phi}^2\\\ \text{K.E. of rod:}& T_r=\frac{1}{2}m_r(\dot{x}-L\dot{\theta}\cos\theta)^2+ \frac{1}{2}m_r(L\dot{\theta}\sin\theta)^2+\frac{1}{2}I_r\dot{\theta}^2\\\ \text{P.E. :}& V=m_rg\cos\theta \end{cases}
and then apply the Lagrangian \[ \frac{d}{dt}\big(\frac{\partial \mathcal{L}}{\partial \dot{q}}\big)-\frac{\partial\mathcal{L}}{\partial q} \\\ \] \[ \mathcal{L}=T_w+T_r-V, q= \begin{pmatrix} x \\\ \phi \\\ \theta \end{pmatrix} \] \begin{align} \frac{d}{dt}\big(\frac{\partial \mathcal{L}}{\partial \dot{x}}\big)-\frac{\partial\mathcal{L}}{\partial x} &=\frac{d}{dt}\big(m_w\dot{x}+m_r(\dot{x}-L\dot{\theta}\cos\theta)\big) \\\ &=m_w\ddot{x}+m_r\ddot{x}-m_rL\ddot{\theta}\cos\theta+m_rL\dot{\theta}^2\sin\theta=F \end{align} \begin{align} \frac{d}{dt}\big(\frac{\partial \mathcal{L}}{\partial \dot{\phi}}\big)- \frac{\partial\mathcal{L}}{\partial \phi}&=\frac{d}{dt}\big(I_w\dot{\phi}\big) \\\ &=I_w\ddot{\phi}=\tau-F\cdot R \\\ \frac{d}{dt}\big(\frac{\partial \mathcal{L}}{\partial \dot{\theta}}\big)- \frac{\partial\mathcal{L}}{\partial \theta} &=(-m_rL\cos\theta)\ddot{x}+(I_r+m_rL^2)\ddot{\theta}-m_rgL\sin\theta=-\tau \end{align} \begin{align} \Rightarrow \begin{pmatrix} (m_w+m_r)R^2+I_w &m_rRL\cos\theta \\\ m_rRL\cos\theta & I_r+m_rL^2 \end{pmatrix} \begin{pmatrix} \ddot{\phi} \\\ \ddot{\theta} \end{pmatrix} + \begin{pmatrix} -m_rRL\dot{\theta}^2\sin\theta \\\ -m_rgL\sin\theta \end{pmatrix} = \begin{pmatrix} \tau \\\ -\tau \end{pmatrix} \end{align}
Measurement
To obtain the physical parameter of the car, we have to measure the some important element derived in above equaitons. The weight of the robot can be obtained easily by weighing on a platform scale, and the length and width are given in designed CAD. But there is no direct method to measure the momentum inertia, so we have to set up an experiment like the following picture, hanging the robot by two wires and measure the spin period, and derive the theoretical momentum inertia.
To measure the theoretical momentum inertia.
And we use the following simple python code to compute the inertia.
import numpy as np
# unit: MKS
# W is width, L is length, M is mass
W = #width
L = #length
M = #mass
# I is theoretical inertia, J is pratical inertia
I = M/12*(W**2+L**2)
print("theoretical inertia is {}".format(I))
# calculate inertia from period T
# T is period, A is rotation radius, G is gravity, H is height
T = #period
A = #radius
G = #gravity
H = #height
J = (T/2/np.pi)**2*A**2*M*G/H
print("pratical inertia is {}".format(J))
error = (J-I)/I*100
print("Error is {}%".format(error))
Finally, we got the table of desired parameter.
| Robot | Rod Inertia | Wheel Inertia | Rod Mass | Wheel Mass | Rod Length |
|---|---|---|---|---|---|
| Big | 0.0012 | 7.8537e-05 | 0.709 | 0.087 | 0.0412 |
| Small | 5.3493e-04 | 3.4951e-05 | 0.367 | 0.042 | 0.0175 |
(units: MKS)
Simulation
At the simulation part, we discuss about the control model applied on the robot, there are two aspects on the model, one is state space in modern control, anthor one is transfer fucntion in classical control.
State Space
After linearization with \(\cos\theta \approx 1, \sin\theta \approx \theta, \dot{\theta}^2 \approx 0\) \begin{align} &\big((I_r+m_rL^2)[(m_w+m_r)R^2+I_w]-(m_rRL)^2\big)\ddot{\phi} \\\ &=[(I_r+m_rL^2)+m_rRL]\tau-(m_rRL)m_rgL\theta \\\ \\\ &\ddot{\phi}=\frac{m_r^2L^2Rg}{(m_rRL)^2-(I_r+m_rL^2)[I_w+(m_w+m_r)R^2]}\theta \\\ &+\frac{-(I_r+m_rL^2+m_rRL)}{(m_rRL)^2-(I_r+m_rL^2)[I_w+(m_w+m_r)R^2]}\tau \\\ \\\ &\big[(m_rRL)^2-\big((m_w+m_r)R^2+I_w\big)(I_r+m_r L^2)\big]\ddot{\theta} \\\ &=\big(m_rRL+(m_w+m_r)R^2+I_w\big)\tau-\big((m_w+m_r)R^2+I_w\big)m_rgL\theta \\\ \\\ &\ddot{\theta}=\frac{\big(I_w+(m_w+m_r)R^2\big)m_rgL}{\big(I_w+(m_w+m_r)R^2\big)(I_r+m_r L^2)-(m_rRL)^2}\theta \\\ &+\frac{-\big(m_rRL+(I_w+(m_w+m_r)R^2)\big)}{\big(I_w+(m_w+m_r)R^2\big)(I_r+m_r L^2)-(m_rRL)^2}\tau \end{align}
Let \(\tau\) as input \(u\), with states \(\phi, \dot{\phi}, \theta, \dot{\theta}\), and the system is \begin{align} \dot{x}&=Ax+Bu \\\ y&=Cx+Du \end{align} where \begin{align} A&= \begin{bmatrix} 0 & 1 & 0 & 0 \\\ 0 & 0 & \frac{m_r^2L^2Rg}{(m_rRL)^2-(I_r+m_rL^2)[I_w+(m_w+m_r)R^2]} & 0 \\\ 0 & 0 & 0 & 1 \\\ 0 & 0 & \frac{\big(I_w+(m_w+m_r)R^2\big)m_rgL}{\big(I_w+(m_w+m_r)R^2\big)(I_r+m_r L^2)-(m_rRL)^2} & 0 \end{bmatrix} \\\ B&= \begin{bmatrix} 0 \\\ \frac{-(I_r+m_rL^2+m_rRL)}{(m_rRL)^2-(I_r+m_rL^2)[I_w+(m_w+m_r)R^2]} \\\ 0 \\\ \frac{-\big(m_rRL+(I_w+(m_w+m_r)R^2)\big)}{\big(I_w+(m_w+m_r)R^2\big)(I_r+m_r L^2)-(m_rRL)^2} \end{bmatrix} \\\ C&= \begin{bmatrix} 1 & 0 & 0 & 0 \\\ 0 & 0 & 1 & 0 \end{bmatrix}\\\ D&= \begin{bmatrix} 0 \\\ 0 \end{bmatrix} \end{align}
Transfer Function
Orignal PID control block diagram
After rearrange
Thus get the transfer function of the controlled system
Plug in the ideal motor model to approximate the true model
Then we have the complete transfer function
and the PID response curves.
Implementation
Hardware
The robot car is designed in Onshape CAD and made of laser-cutted acrylic board, and the main controller is LinkIt 7697 which is much powerful then Arduino board. To keep balance, it's needed to add an IMU(inertia measurement unit) to monitor the linear accelertaion and angular velocity.
Schematics
Diagram
The following diagram shows the structure of the whole implementation of the robot car, some difficult works are the conversion from continuous model to discrete computation on the micro controller, and the correction of the derived angle by adding some filter like compimentary filter/Kalman filter.
Demo
Big robot
Samll robot
Analysis
The controller 7697 has the ability to transfer the recording data to computer for analysis, the following pictures are recorded data while small car balancing with PID control.
Angle
Angle Rate
Angle Sum
Reference
- MATLAB: State-Space Methods for Controller Design
- MIT OCW Linear Quadratic Regulator
- MIT OCW Feedback Control
- Measurement of moment of inertia: Katsuhiko Ogata(2004). System Dynamics (4th ed). pp.82-83 New Jersey:Pearson