Reaction Wheel Inverted Pendulum Simulation

This project simulates the dynamics of a Reaction Wheel Inverted Pendulum and includes controllers such as LQR and a Bang-bang controller.

This project is a part of FRA333 Robot Kinematics @ Institute of Field Robotics, King Mongkut’s University of Technology Thonburi

RWIP.mp4

Overview

The Reaction Wheel Inverted Pendulum is a complex system that combines a pendulum with a reaction wheel, this simulation implements Bang-bang controller to breing the pendulum up, then use LQR to keep it stable.

Features

LQR Controller: Stabilize the system using a Linear Quadratic Regulator (LQR) controller.
PID Controller: Stabilize the system by PID controller (Choose 1 Stabilize controller in param.py).
Bang-Bang controller: Implement a Bang-bang controller as a swing-up controller to bring the pendulum upright.
Realistic Dynamics: The simulation incorporates realistic dynamics and parameters of a DC motor and an inverted pendulum.

Getting Started

Prerequisites

Ensure you have the following dependencies installed:

numpy
pygame
matplotlib
PyQt5
pygetwindow
pyaudio
threading
control (install via pip install control)

Installation

Clone the repository and install the dependencies:

git clone https://github.com/B-Paweekorn/Reaction-wheel-inverted-pendulum.git
cd Reaction-wheel-inverted-pendulum
pip install -r requirements.txt

Usage

Inject - Type the amount of torque into the box and click INJECT or ENTER key to inject a disturbance to the system

Reset - Click RESET to reset the system

Plotting - Click on the pendulum plot or the plot window to plot the data

Edit the parameters in param.py

Methodology

System Modeling

The simulation involves modeling the dynamics of a Reaction Wheel Inverted Pendulum (RWIP) system. The key components include:

Reaction Wheel Inverted Pendulum Dynamics

RWIP Parameter

L1 - Pendulum length from orgin to center of mass
L2 - Pendulum length
m1 - Mass of pendulum
m2 - Mass of fly wheel
θp - Angle of pendulum
θr - Angle of fly wheel
I1 - Innertia moment of pendulum
I2 - Innertia moment fly wheel and Innertia moment of motor
g - Gravitational acceleration
Tm - Torque apply by DC motor
Td - Disturbance

Kinetic Energy

$K = \frac{1}{2}(m_{1}L_{1}^{2}+m_{2}L_{2}^{2}+I_{1})\dot{θ_p}^{2} + \frac{1}{2}I_{2}(\dot{θ_p}+\dot{θ_r})^{2}$

$K = \frac{1}{2}(m_{1}L_{1}^{2}+m_{2}L_{2}^{2}+I_{1}+I_{2})\dot{θ_p}^{2} + I_{2}\dot{θ_p}\dot{θ_r} + \frac{1}{2}I_{2}\dot{θ_r}^{2}$

Potential Energy

$V = (m_1L + m_2L)g\cosθ_p$

Lagrange Method

$\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{θ_r}} \right) - \frac{\partial L}{\partial θ_r} = 0$ and

$\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{θ_p}} \right) - \frac{\partial L}{\partial θ_p} = T_m$

Where Lagrangian (L) is

$L = K - V$

Mathematical equations of RWIP is described as

$\ddot{\theta_p} = \frac{m_{1}gL_{1}\sin\left(\theta_{p}\right)\ +\ m_{2}gL_{2}\sin\left(\theta_{p}\right)\ -\ T_{m} +\ T_{d}}{m_{1}L_{1}^{2}+m_{2}L_{2}+I_{1}}$

$\ddot{\theta_r} = \frac{T_{r}}{I_{2}}-\frac{m_{1}gL_{1}\ +\ m_{2}gL_{2}\ -\ T_{m} +\ T_{d}}{m_{1}L_{1}^{2}+m_{2}L_{2}+I_{1}}$

DC Motor Dynamics

Brushed DC Motor Parameter

Vin - Input Voltage
R - Motor resistance
L - Motor inductance
i - Motor current
B - Motor damped
J - Motor rotor Innertia
ke - Back EMF constant
kt - Torque constant

Electrical part

$Vin = R i + L \frac{di}{dt} + k_e θ_r$

Mechanical Part

$T_{m} = Bθ_r + J\frac{\dot{θ_r}}{dt}$

$T_{m} = k_t i$

Controllers

LQR Controller: A linear quadratic regulator designed to stabilize the pendulum in the upright position.

Linearization dynamics model
- Part RWIP
  
  when sinθp -> 0 sinθp = θp
$\ddot{\theta_p} = \frac{m_{1}gL_{1}\theta_{p}\ +\ m_{2}gL_{2}\theta_{p}\ -\ T_{m} +\ T_{d}}{m_{1}L_{1}^{2}+m_{2}L_{2}+I_{1}}$

$\ddot{\theta_r} = \frac{T_{m}}{I_{2}}-\frac{m_{1}gL_{1}\ +\ m_{2}gL_{2}\ -\ T_{m} +\ T_{d}}{m_{1}L_{1}^{2}+m_{2}L_{2}+I_{1}}$
- Part Motor
We can estimate that L << R

$Vin = R i + k_e θ_r$

$T_{m} = k_t i$

State space

The proceeding equations are valid around the operating point where θp = 0

PID Controller: The stabilize controller to compare with LQR

Transfer function

$\Large\frac{\theta_{p}(s)}{\tau_{m}(s)}=\frac{\frac{s}{-J-m_{2}L_{2}^{2}}}{s^{3}+\left(\frac{B}{I_{1}}+\frac{B+d_{p}}{J+m_{2}L_{2}^{2}}\right)s^{2}-\left(\frac{\left(m_{1}L_{1}+m_{2}L_{2}\right)g}{\left(J+m_{2}L_{2}^{2}\right)I_{1}}-\frac{B\cdot d_{p}}{\left(J+m_{2}L_{2}^{2}\right)I_{1}}\right)s-\frac{\left(m_{1}L_{1}+m_{2}L_{2}\right)Bg}{\left(J+m_{2}L_{2}^{2}\right)I_{1}}}$

Root Locus Design

To predict the system's characteristics as the gain (Kp) is adjusted and poles move, design the root locus.

Root Locus of the system by default parameter set. It has one zero (s = 0), and three poles (s = -74.93; s =-3.88e-3; s = 73.53).

Closed Loop Root Locus

Where G(s) is Kp the gain can be adjusted ti make the closed loop poles to be in stable location The resultant Root Locus can be seen below (note to plot this graph in param.py you need to set Stabilize_Controller to "PID" mode and set plot_rootlocus to "True") in this graph you can click pole position you want to know Gain Kp to adjust your system characteristics.
Bang-bang Controller: The swing up control routine and the stabilizing control routine are switched between -25 to 25 degree

Brake Controller: Used as reduced energy of RWIP when RWIP have too much energy for stabilze

Sound Generation

The simulation incorporates sound generation related to the speed of the reaction wheel. This feature adds an auditory element to the simulation, enhancing the user experience.

Compare PID controller and LQR controller

In this project, we explore and compare the performance of two different control strategies: PID (Proportional-Integral-Derivative) controller and LQR (Linear Quadratic Regulator) controller.

Linear quadratic regulator

Error (deg)	settling time (s)	Power (Watt)
5	0.66	0.6
6	0.73	1.01
7	0.85	1.75
8	1.07	3.5
9	can't stabilize	can't stabilize

Max Disturbance : 9.32 Nm

PID : Kp = 500 (choose form root locus)

Error (deg)	settling time (s)	Power (Watt)
5	20.43	10.06
6	21.02	12.17
7	21.37	13.95
8	21.59	15.96
9	can't stabilize	can't stabilize

Max Disturbance : 8.05 Nm

PID : Kp = 215800 (choose form root locus) Notice that when choose unstable pole the system still stable because now it have hardware limit so the character of controller same like Fuzzy logic control to see unstable you need to unlock hardware limitation by set MotorLimit to False in param.py

Conclusion

	Stabilize boundary
LQR	Can stabilize in every position
PID	Can stabilize only in small boundary

PID Controller The PID controller is a widely used feedback control system that relies on three components: Proportional, Integral, and Derivative. Here's a brief overview of each component:

Proportional (P): Reacts to the current error.
Integral (I): Reacts to the accumulation of past errors.
Derivative (D): Predicts future errors based on the rate of change.

Advantages of PID:

Simplicity and ease of implementation.
Effectiveness in a wide range of systems.
Intuitive tuning parameters for performance optimization.

Considerations:

Tuning may be required for optimal performance in different systems.
Limited capability to handle complex or nonlinear systems.

LQR Controller The LQR controller is designed based on the principles of optimal control theory. It minimizes a cost function that combines both state and control input, making it suitable for linear, time-invariant systems.

Advantages of LQR:

Optimal control solution for linear systems.
Ability to handle systems with multiple inputs and outputs.
Incorporates a mathematical model for optimal performance.