# Inverted Pendulum

Contents

## Introduction

The inverted pendulum is one of the most popular demonstrations in system control, has been studied since the 19601 and is a prime example at unis and other research institutes. However, the inverted pendulum, which is nothing more than a pendulum that has the centre of mass above the pivot point, yet has no particular use case. To change this once in for all, I build an inverted pendulum lamp, which can be used as a normal stylish desk lamp. (The main incentive, however, wasn’t so heroic. Honestly! It sounded just like a nice project idea!)

Note
This blog post will give you a step by step recreation guide and highlights the design choices I made as well as introduce you to the implementation of the LQR Control Algorithm I used to balance the Lamp. I will also mention the resources that I used to speed up the programming part at the end of the post!

## Building

### Mechanical Setup

To accomplish this symbiosis between this still highly complex balancing problem and the potential usage as a design element, I first had to design a balancing cart 🚗 with a pivoting E27 socket for the lamp 💡. This brings the extra challenge of feeding a, in this case, 230V, cable through various mountings and controlling it using an ordinary relay.

Design-choices

### ⚙️ DV-Motor (first axel)

• the L298N Motor Driver Module works up to 12V, without removing the jumper wire, therefore a 12V DV-Motor is a good choice for the setup
• should have sufficient torque and RPM. In my case around 300-440 RPM (depending on your wheel size) with a 15:1 gear ratio and 10Ncm was enough
• ❗ be careful with the maximum current! The allowed Amps for the L298N Motor Driver Module is 2A (more information here)

### 📏 Contious Angle Sensor (second axel)

• used to monitor the position of the cart 🚗 relative to its starting position
• works like a normal potentiometer but allows for more than 360° turning angle.
• not the most reliable solution as it produced high noise in the zero orientation
=> Quick-fix: use a capacitor as a low-pass filter between the ground and output voltage.

### 📟 Arduino

• the L298 has a 5V regulator and can be used as the power supply for the Arduino (see connection diagram)
• ❗ don’t mount any high current consumer like the lamp 💡 or the motor ⚙ to the 5V output of the board it is not made for such high currents.

### 📏 Potentiometer 50k (linear)

• used to detect the angle of the pendulum
• additionally used as a mount for the pendulum

### 🔌 Capsule Slip Ring AC 240V

• the lamp 💡 is powered by an isolated electric circuit with 240V AC
• the slip ring feeds the power thought the rotational pivot point
• also used as a mount for the pendulum

### 🔌 Relay

• isolates the 12V DC and the 240V AC
• is triggerd by the Arduino

## Control Algorithm

In the following section, I will give a brief introduction to the theory of the LQR Controller and provide you with my recommended resources which help you determine the coefficients of the feedback regulator!

### Mathematical representation

The simplified system consists of an inverted pendulum which is connected to a motorized card. Without further inspection, we can recognize that we are looking at an unstable system. If the cart is not moved it is nearly impossible to balance the pendulum in the upright position. The objective is it now to design a feedback controller which accelerates the cart enough so that the angle $\phi$ of the pendulum is close to 180°.

For this we first have to define the nonlinear model:

List of Variables
• M mass of the cart
• m mass of the pendulum
• b coefficient of friction for cart
• l length to pendulum centre of mass
• I mass moment of inertia of the pendulum
• F force applied to the cart
• x cart position coordinate
• $\theta$ pendulum angle from vertical (down)
• u (input) here equivalent to the Force F

### LQR Controller

The LQR or Linear quadratic regulator is one of many optimal control techniques, which takes into account the whole state vector and computes the control decision based on a linear model. For non-linear plants like our inverted pendulum, the system equations first have to be linearized about the upright (unstable) equilibrium. This consequently means that our LQR controller is only sufficient within small deviations around the upright position. After linearizing we have to calculate the gains $K$, this is done using the script linked here. With all that done, we now can compute the input voltage of the Motor using the following equation:

$$u = -Kx$$

A deeper dive into the theory behind the calculation of the linearized model can be found here!

### Determine grains

The script which already includes the linearized model to determine the gains for the LQR Controller can be found here!

Tip
When you calculate the model-specific gains $K$ it is very important that you measure the weight ⚖ of the cart 🚗 and the pendulum + lamp 💡 very precisely as the controller is very sensitive!

To furthermore approve your parameters the script [controlled-cart-pendulum.py] from a similar project3 simulates the control behaviour.

### DC-Motor system identification

The LQR Controller takes as an input the Force F of the DC-Motor. Therefore the next step is to find the underlying DC-Motor model and estimate the corresponding system parameters. Then the Arduino can change the speed of the engine using the PWM signal.

We can model a standard DC-Motor using the following equation: $$\frac{d w}{d t}\left(J+m r^{2}\right)+w\left(B+\frac{k^{2}}{R}\right)=\frac{k U}{R}$$

Simplifying this further:

To determine the parameters a,b,c we can record several velocity curves with different input voltages. Using the following plot and theses scripts [dc_motor_lqr_control.py,estimate_params.py] we can now find the exact model parameters!

Credits to zjor for creating the scripts3

## Code

The code is based on an existing Arduino project3

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209  #define A 9 #define B 1.0 #define C 2.1 #define Kth 61.20847945 #define Kw 10.42165907 #define Kx 16.2303454 #define Kv 13.43669139 #define Ki 0 #define PI 3.1415926535897932384626433832795 //define pins int speed = 9; int front = 6; int back = 7; int lamp = 2; //define constans float wheelCirc = 141.37; float THETA_THRESHOLD = PI / 8; unsigned long now = 0L; unsigned long lastTimeMicros = 0L; //init States float x=0; float d_x; float phi; float d_phi; float phi_sum=0; //usefull variables float pos_last1; float pos_last2; float pos_last3; float pos_last4; float last_phi; float control, u, dt , v_soll; // the setup routine runs once when you press reset: void setup() { // initialize serial communication at 9600 bits per second: Serial.begin(9600); pinMode(speed, OUTPUT); pinMode(front, OUTPUT); pinMode(back, OUTPUT); pinMode(lamp, OUTPUT); digitalWrite(lamp,HIGH); digitalWrite(front, LOW); digitalWrite(back, LOW); pos_last1= mapf(analogRead(A1),0,1023,0,wheelCirc)/1000; pos_last2=pos_last1; pos_last3=pos_last1; pos_last4=pos_last1; last_phi= mapf(analogRead(A0)-512+4 , -512, 512, -PI ,PI ); lastTimeMicros = 0L; //Debugging //Serial.print("x");Serial.print("\t"); //Serial.print("dx");Serial.println("\t"); //Serial.print("Kv*d_x");Serial.print("\t"); //Serial.print("Kth*phi");Serial.print("\t"); //Serial.print("Kw*d_phi");Serial.print("\t"); //Serial.print("control");Serial.print("\t"); //Serial.print("u");Serial.println("\t"); } float mapf(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void driveMotor(float pwm_out){ analogWrite(speed, fabs(pwm_out)); if (pwm_out > 0) { digitalWrite(front, HIGH); digitalWrite(back, LOW); }else if (pwm_out < 0) { digitalWrite(front, LOW); digitalWrite(back, HIGH); }else{ digitalWrite(front, LOW); digitalWrite(back, LOW); } } boolean isControllable(float phi) { return fabs(phi) < THETA_THRESHOLD; } float saturate(float v, float maxValue) { if (fabs(v) > maxValue) { return (v > 0) ? maxValue : -maxValue; } else { return v; } } float derivative(float x_new, float x_old, float dt){ float d_temp= x_new - x_old; float d_pos = 0; if (fabs(d_temp) < fabs(d_temp+wheelCirc/1000) and (fabs(d_temp) < fabs(d_temp-wheelCirc/1000))){ d_pos= d_temp; }else if (fabs(d_temp+wheelCirc/1000) < fabs(d_temp-wheelCirc/1000)){ d_pos= (d_temp+wheelCirc/1000); }else { d_pos= (d_temp-wheelCirc/1000); } if (fabs(d_pos/dt)>1.1){ d_pos= -d_x*dt; } return - d_pos/dt; } void updateStates(float dt){ //read sensor input (Angle of wheel) float sensor_x = analogRead(A1); float pos_new =mapf(sensor_x,0,1023,0,wheelCirc)/1000; //smoothing the signal input float pos_avg= (pos_last1+pos_last2+pos_last3+pos_last4)/4; float d_x_2 = derivative(pos_new, pos_last2,2*dt); float d_x_3 = derivative(pos_new, pos_last3,3*dt); d_x = (d_x_2+d_x_3)/2; //update state variables x = x + dt*d_x; pos_last4=pos_last3; pos_last3=pos_last2; pos_last2=pos_last1; pos_last1=pos_new; //read sensor phi float sensor_phi = analogRead(A0) - 512; phi = mapf(sensor_phi+8 , -512, 512, -PI, PI ); // map sensor input to correct mean an variance d_phi=(phi-last_phi)/dt; last_phi= phi; phi_sum=phi_sum+dt*phi; if (isControllable(phi)){ phi_sum=0; } //Debugging //Serial.print(sensor_x, 4);Serial.print("\t"); //Serial.print(sensor_x, 4);Serial.print("\t"); //Serial.print(pos_new, 4);Serial.print("\t"); //Serial.print(d_x*1000, 4);Serial.println("\t"); //Serial.print(d_x_2*1000, 4);Serial.print("\t"); //Serial.print(d_x_3*1000, 4);Serial.println("\t"); //Serial.print(d_x_4*1000, 4);Serial.println("\t"); //Serial.print(phi, 4);Serial.print("\t"); //Serial.print(d_phi, 4);Serial.print("\t"); } // the loop routine runs over and over again forever: void loop() { now = micros(); dt = (1.0*(now - lastTimeMicros))/ 1000000; updateStates(dt); if (isControllable(phi)) { digitalWrite(lamp, LOW); control = (Kx * x + Kv * d_x + Kth * phi + Kw * d_phi); u = (control + A * d_x + copysignf(C, d_x)) / B; u = 255.0 * u / 12; v_soll = saturate(u, 254); driveMotor(v_soll); } else { v_soll= 0; driveMotor(v_soll); digitalWrite(lamp, HIGH); } lastTimeMicros = now; delay(5); // delay in between reads for stability }