Insight behind overshoot/oscillation in forced pendulum simulation

50 Views Asked by At

I have written a discrete-time simulation of a pendulum. It behaves fine when unforced, however I am trying to apply PI(D) control to make it move and reach a particular angle. When I do this and tune my gains, it is able to reach the angle but it won't stay there, and whatever I try seems to make it go wonky.

Beyond playing with the gains for ages, is there a more correct way of doing this PID control to achieve angle control?

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

m    = 1.0  # mass (kg)
k    = 0.0  # spring constant k_{s}
r    = 1.0  # link length (m)
g    = 9.81 # gravity term (m/s^{2})
damp = 1.0  # damping term (for unforced pendulum)

start_time = 0.0  # start time
end_time   = 50.0 # end time
dt         = 0.01  # time increment
num_steps  = int((end_time - start_time) / dt)

t         = np.linspace(start_time, end_time, num_steps)
theta     = np.zeros_like(t)
thetadot  = np.zeros_like(t)
thetaddot = np.zeros_like(t)
tau_u     = np.zeros_like(t)
error     = np.zeros_like(t)

# PID control gains
kp        = 1.2
ki        = 0.0
kd        = 0.0

# ICs
theta[0]     = -np.pi/2
thetadot[0]  = 0.0
thetaddot[0] = 0.0
tau_u[0]     = 0.0
target_angle = 0.0
error[0]     = theta[0] - target_angle 


for i in range(1, num_steps):

    # mod term to keep angle between 0 and 2pi radians
    if theta[i] > 2*np.pi:
        theta[i] = theta[i] % 2*np.pi
    
    error[i] =  theta[i - 1] - target_angle #target_angle - theta[i - 1]
    tau_u[i] = (kp * error[i - 1]) - (ki * np.sum(error) * dt) - (kd * thetadot[i - 1])

    thetadot[i] = thetadot[i - 1] - ( ((3/(m*r**2))*tau_u[i]) + 
                                      (((3*k)/(m*r**2))*theta[i - 1]) + 
                                      ((3*g*np.cos(theta[i - 1]))/(2*r)) )* dt

    theta[i] = (theta[i - 1] + (thetadot[i] * dt) )


# plotting omitted for space

PID control pendulum not settling

0

There are 0 best solutions below