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.

Considerations:

• Requires a good understanding of the system dynamics for effective modeling.
• Limited applicability to strictly linear systems.

Acknowledgments

        This project is part of the coursework for FRA333 Robot Kinematics at the Institute of Field Robotics, King Mongkut’s University of Technology Thonburi. Special thanks to the course instructors for their guidance and support.

Feel free to explore, modify, and extend this project for educational and research purposes.

Reference

• Swing Up and Balancing of a Reaction Wheel Inverted Pendulum

• Inverted Pendulum: State-Space Methods for Controller Design