RC Submarine 4.0 – PID control (9/10)

Now I need to code a PID controller. The input will be depth from the pressure sensor, or distance from the laser. The output will be syringe ballast position. Syringe will change the weight of the submarine, that will introduce gravity force that changes the position. Position is measured by the sensors and looped back to the controller.

Main loop

The first thing to do is create a while loop. It will have many tasks:

• calculate syringe position based on tacho count
• read pressure sensor
• read laser sensor
• calculate PID control output
• drive syringe motor
• log data to file for later analysis

The code will be run on Raspberry Pi Zero 2 that has a powerful 1 GHz Quad-core ARM Cortex-A53 processor. One loop takes about 5 ms based on my measurements. From that about 1 ms is spent on a blocking call to read the pressure sensor.

I could run the loop as fast as possible as both sensors can handle 1000 Hz data rate. But that would consume batteries and heat up the Raspberry. Also, the submarine will move slowly, so timing won’t be an issue. I settled on using a 20 ms sleep in each loop.

With 20 ms sleep, loop interval is 25 ms and single core CPU load is 15% for the program.

PID control for depth

Here is the PID controller code in Python. Depth measurement comes in depthFiltered variable. Set point comes in targetDepth variable. Loop interval is stored in timeDelta. KP, KI and KD are the gain settings that will be tuned later.

#PID controller
error = targetDepth - depthFiltered
integral += error * timeDelta
if math.isnan(prevError) or buttonDive.value == 1 or buttonSurface.value == 1:
    prevError = error
derivative = (error - prevError) / timeDelta
derivative = 0.05 * derivative + 0.95 * prevDerivative
PIDoutput = KP * error + KI * integral + KD * derivative
prevError = error
prevDerivative = derivative

There are two abnormalities in the code. The first is conditions to set prevError = error. It’s purpose is to omit spikes in derivative that come from changes in the target depth. When pressing radio buttons, the target depth will have a sharp step in value, so the derivative would contain a sudden large impulse. The spikes would have a lasting effect since I’m using filter. The math.isnan(prevError) is just for testing purposes, so that I can start the program with an initial targetDepth value without derivative spikes.

Secondly there is an exponential moving average filter on the derivative. I added it after noticing restless jerky movements in the syringe motor. It needs a high 95% filter factor to settle down.

The result of the controller, PIDoutput, will be used to adjust the syringe position. Syringe neutral position is added to the result, as we want PIDoutput = 0 to be the state where the submarine is neutrally buoyant and doesn’t move.

#move syringe to the target position
syringeTargetPos = syringeNeutralPos + PIDoutput  #[ml]

The syringe target position is then used to drive the syringe motor in a very simple manner. Just drive the motor at full power until the actual position is equal to the target position.

syringeMotorCtrl = syringeTargetPos - syringePos

#drive syringe motor
motorDive.value = (syringeMotorCtrl > 0)
motorSurface.value = (syringeMotorCtrl < 0)

I added one safety feature to make sure the submarine can always surface and does it as fast as possible. When targetDepth is 0, bypass the PID control and run the syringe to the minimum position.

if targetDepth <= 0:
    #go to surface
    if syringePos > 0:
        syringeMotorCtrl = -100
    else:
        syringeMotorCtrl = 0

PID control for laser distance

Laser PID control is the same, but input data comes from laserDistanceFiltered variable and set point comes in targetDistance variable. As the laser is pointing downwards to the bottom of the pool/lake, the error has opposite sign compared to the depth PID controller.

error = laserDistanceFiltered - targetDistance

To protect the submarine from going too deep, a max depth limit is added.

if depthFiltered >= MAX_DEPTH:
    #limit to max depth
    syringeMotorCtrl = -100

Tuning parameters

I began tests in a 35 cm deep water container. At first I noticed that the water surface tension would hold the submarine and mess up the tests. Very low KP values couldn’t even make the submarine leave the surface. Many times I had to hold the submarine 1 cm below surface at the beginning of tests, to get a clean step response.

I started with changing KP, while keeping KI and KD zero. The tests were step responses from 0 cm to 15 cm targetDepth.

Step response from 0 to 15 cm of depth with different KP factors.

All values made the system unstable. Only the rate and amplitude of oscillation changed.

OK, this is going to be harder than I thought. When starting this project, I really thought PID tuning will be a walk in the park because the submarine is so slow.

I think the problem is with high inertia. The submarine is heavy and responds to commands slowly, as the syringe range is only 42 grams whereas the total weight is 2.4 kg. Once it starts moving, it pushes on like a heavy truck. Need to add derivative to dampen movements and increase stability. Here are test results for KD while KP is 20.

Step response from 0 to 15 cm of depth with different KD factors while KP is 20.

That helped a lot. KD = 200 seems to be the best, as it reduces oscillations but doesn’t slow down the response too much.

Finally I added integrator to fix residual errors. Those may be uncompensated hull compression, error in neutral buoyancy initial setting, sensor read errors, leaks, even changes in atmospheric pressure. Here are test results for KI.

Step response from 0 to 15 cm of depth with different KI factors while KP is 30 and KD is 200.

I want to pick the largest integrator without losing stability. I chose KI = 1. It adds a little bit of overshoot but not too much.

The final tuned parameter set is KP=30, KI=1, KD=200.

Hull compression

While testing the submarine, I noticed a systematic error based on depth. Here is a graph that shows syringe position at 5 cm depth and 20 cm depth.

As you see, the submarine dives too deep when aiming at 20 cm, actually hitting the bottom of the container. It takes over 60 sec for the integrator to fix the error, and when it does, it settles down on using a 1.5 ml lower syringe position. So about 1.5 ml of buoyancy is lost due to hull compression. The same happens when submerging.

To fix it, I added a compensator to the code. The code adjusts the neutral buoyancy position based on depth measurements. The result is used together with PIDoutput to run the syringe motor. The compression effect slows down as depth increases, therefore I used square root of depth in the formula.

#calculate syringe neutral position
#  hull compression compensation (15 ml at 100 cm depth)
syringeNeutralPos = SYRINGE_NEUTRAL_POS
if depthFiltered > 1.0:
    syringeNeutralPos -= 15
elif depthFiltered > 0:
    syringeNeutralPos -= math.sqrt(depthFiltered) * 15 / 1.0

In the formula I used 15 ml compensation at 100 cm depth. It is actually overcompensating, but step responses looked more accurate that way, having less overshoot. In reality the compression effect was 5-10 ml at 100 cm in different test runs.

Here is the previous test again with compensator.

And here another test in a swimming pool with compensation active.

Hysteresis

I have a hysteresis compensation with the syringe position data. I tested it with different settings. The setting is how many rotations the syringe motor needs to do before change in actual syringe position.

Step response from 0 to 15 cm of depth with different hysteresis compensation settings.

The hysteresis compensation seems to improve stability a little. About 1 rotation seems to be the best setting. That is in line with my visual observations, eyeballing the syringe during operation.

Deadband

While testing I noticed how the motor would go crazy with the PID control output. It would change direction constantly and be active all the time. I need some amount of deadband to protect the motor and syringe from wear and to save batteries.

I first tried this very simple limiter. SYRINGE_DEADBAND is the deadband setting in milliliters.

#syringe deadband limitation
if abs(syringeMotorCtrl) < SYRINGE_DEADBAND:
    syringeMotorCtrl = 0

It would nudge the syringe forward at the edge of the deadband range. I found it to limit motor runtime well, but motor activation count was high. Also the syringe would not reach the target position, and therefore the submarine was oscillating more.

Here is the second version I tried.

#syringe deadband limitation
if abs(syringeMotorCtrl) > SYRINGE_DEADBAND:
    deadbandDir = sign(syringeMotorCtrl)
if sign(syringeMotorCtrl) != sign(deadbandDir):
    deadbandDir = 0
if deadbandDir == 0:
    syringeMotorCtrl = 0

It starts the motor when outside of deadband, and stops only after it reaches the target. It does better job in following the target and therefore causes less oscillation. But it sometimes runs the motor back and forth unnecessarily and therefore motor runtime is higher.

I chose the second version, as it leads to better stability.

Here is the syringe position data with different settings.

Here are depth measurements from the same test.

As you see, larger deadband makes the system more unstable. I selected 0.5 ml for the final implementation.

Filters

I have two filters. The first is for measurements, either depth or laser distance. The second is for PID control derivative.

Here is a comparisons of filter settings.

Step response from 0 to 15 cm of depth with different filter settings while KP=30, KI=1 and KD=200. The first percentage is exponential moving average factor for measured depth, the second is for PID controller derivative.

As you see, both too little and too much filtering makes the system unstable. I chose 90% for depth and 95% for derivative (the red line on graph).

Final tests

Here are some tests of the final implementation.

PID over pressure sensor depth measurements. Done in a water container.

PID over laser distance sensors. Done in a water container.

Testing in a swimming pool.

I wouldn’t say the PID control is perfect in any means. There was +/- 10 cm error in depth when testing in a swimming pool. But it was good enough to enjoy driving the submarine around.