pythonsimulationpid-controller

How to tune PID controller on non-linear model (here: diving of a body)


My task is to develop a controller (I decided to do a PID) in Python.

This is the model I've been given: We consider a body in water with mainly two forces acting on it: gravity and buoyancy (friction is neglected here). The body-depth is controlled by ballast tanks within its hull, e.g. it changes mass to accelerate up or down. More precisely: The speed of flooding the ballast tanks is what I can control.

This is what I did so far:

import math
import matplotlib.pyplot as plt


WATER_DENSITY = 997  # [kg/m³]
GRAVITATIONAL_ACCELERATION = 9.81  # [m/s²]
BODY_VOLUME = 0.00238  # [m³]
BODY_VOLUME_GAGE = 0.00005  # [m³]

BODY_MASS = BODY_VOLUME * WATER_DENSITY  # [kg]
BODY_MASS_MAX = BODY_MASS + BODY_VOLUME_GAGE * WATER_DENSITY  # [kg]
BODY_MASS_MIN = BODY_MASS - BODY_VOLUME_GAGE * WATER_DENSITY  # [kg]

SIMULATION_TIME = 90  # [s]
SIMULATION_STEP = 0.001  # [s]
R0 = 0.0  # [m]
CONTROLLER_TARGET = -1  # [m]

KP = 2
KI = 0
KD = 0


def system_response(state, control, time_step):
    current_mass = state['m'] - control * time_step  # [kg]
    current_mass = max(BODY_MASS_MIN, current_mass)  # limit the current mass to min / max values
    current_mass = min(BODY_MASS_MAX, current_mass)
    f_gravity = current_mass * GRAVITATIONAL_ACCELERATION  # [N]
    f_buoyancy = WATER_DENSITY * BODY_VOLUME * GRAVITATIONAL_ACCELERATION  # [N]
    f_total = f_buoyancy - f_gravity  # [N]
    acceleration = f_total / current_mass  # [m/s²]
    velocity = state['v'] + time_step * acceleration  # [m/s]
    position = min(0, state['r'] + time_step * velocity)  # [m]
    return {
        'm': current_mass,
        'a': acceleration,
        'v': velocity,
        'r': position
    }


print("Running simulation...")
t = [0.0]  # [s]
m = [BODY_MASS]  # [kg]
a = [0.0]  # [m/s²]
v = [0.0]  # [m/s]
r = [R0]  # [m]
e = [CONTROLLER_TARGET - R0]
ie = 0.0
u = 0.0  # [kg/s]
i = 0
while True:
    new_state = system_response({
        'm': m[-1],
        'a': a[-1],
        'v': v[-1],
        'r': r[-1]
    }, u, SIMULATION_STEP)
    t.append(t[-1] + SIMULATION_STEP)
    m.append(new_state['m'])
    a.append(new_state['a'])
    v.append(new_state['v'])
    r.append(new_state['r'])
    e.append(CONTROLLER_TARGET - new_state['r'])
    de = e[-1] - e[-2]
    dt = SIMULATION_STEP
    ie += e[-1] * dt
    u = KP * e[-1] + KI * ie + KD * de / dt
    i += 1
    if t[-1] > SIMULATION_TIME:
        break

print("Simulation stopped after {0} steps".format(i))
plt.title('Simulation Results')
plt.plot(t, r, label='Position [m]')
plt.plot(t, [CONTROLLER_TARGET for j in range(len(t))], label='Target [m]')
plt.xlabel('Time [s]')
plt.legend()
plt.show()
plt.close()

showing me an oscillating output, no matter what I choose KP to be. I tried to do the Ziegler-Nichols method, but this seems undoable since I cant get a stable response. When I choose KD big, it shows a more stable response.

How do I proceed from here as a beginner in control loops?


Solution

  • I would suggest adding a second order derivative term to your PID controller to stabilize this. The goal of this is to provide a derivative term that stabilizes the system in response to both the submarine moving too fast (first derivative,) and water tank being too full / empty (second derivative.)

    while True:
        new_state = system_response({
            'm': m[-1],
            'a': a[-1],
            'v': v[-1],
            'r': r[-1]
        }, u, SIMULATION_STEP)
        t.append(t[-1] + SIMULATION_STEP)
        m.append(new_state['m'])
        a.append(new_state['a'])
        v.append(new_state['v'])
        r.append(new_state['r'])
        e.append(CONTROLLER_TARGET - new_state['r'])
        de = e[-1] - e[-2]
        if len(e) >= 3:
            dde = (e[-1] + e[-3] - 2 * e[-2])
        else:
            dde = 0
        dt = SIMULATION_STEP
        ie += e[-1] * dt
        u = KP * e[-1] + KI * ie + KD * de / dt + KDD * dde / dt ** 2
        i += 1
        if t[-1] > SIMULATION_TIME:
            break
    

    I then get reasonable results with the following settings:

    KP = 1.5
    KI = 0
    KD = 5
    KDD = 5
    

    fixed graph of submarine depth versus time