29 D‐Ov2Evo: Control Concept Reference
A PID controller per wheel is used to set the wheel speed. Each wheel controller gets its input from the wheel encoder (class bb::Encoder) and directs its output to the relevant motor (class bb::DCMotor). Controlling constants are wheel_kp, wheel_ki, wheel_kd. This controller also constrains its I output to [-255..255] because that's the maximum Arduino PWM can do, so no sense to integrate to anything higher than this. Setpoints for the wheels are coming from the overlaid controllers.
A PID controller controls the droid pitch to hold a certain setpoint. The setpoint is typically zero, but if the droid's neck is not upright but angled toward the body the setpoint is obtained by multiplying the angle with a constant, lean_head_to_body. The controller's constants are bal_kp, bal_ki and bal_kd.
The PID controller by itself does not provide driving motion, only stabilization, and it can also only give one output but we have two wheels. Therefore driving motion is added in an adapter subclass of bb::ControlOutput, DOVelControlOutput. This class takes the output from the balance controller. It then generates the setpoint for each wheel controller by adding the velocity signal from the remote (multiplied by speed_axis_gain, not shown in the graph) to both wheels and adding/subtracting the turn signal from the remote (multiplied by rot_axis_gain) from each wheel respectively.
Note 1: The droid needs to be balanced well, otherwise the balance controller will drift. The selftest routine tries to calibrate the pitch angle at rest to initialize the setpoint. Even this is not perfect, as ground unevenness may cause the droid to stand without being in balance, and the servos moving head and neck may come to rest at fractions of a degree different than at startup. Drift under velocity control is therefore inevitable, but it should be very small.
Note 2: The idea behind the lean_head_to_body setpoint is to use the body counterweight at an angle to balance out the weight of the head, with the simplifying assumption that head_weight=lean_head_to_body*body_counterweight. This works well enough but friction with ground, motor speeds etc. will induce drift and motion under velocity control.
While the droid is at rest, we do not want it to drift. The droid will recognize if there was no velocity or turn input for over half a second, and automatically switch to Position Control. This overlaid PID controller takes an average of the wheel positions (obtained from the wheel encoders) via the custom class DOPosControlInput and generates a velocity setpoint and an always-0 turn setpoint for the velocity controller via the custom class DOPosControlOutput. The setpoint for the position PID controller is generated from the current wheel position average every time the droid switches from Velocity to Position drive mode. Governing constants are pos_kp, pos_ki and pos_kd.
Note: The position controller does not influence rotation, so it is possible to manually rotate the droid in place. Allowing rotation via the remote under position control does not make a lot of sense I think; it would require holding the velocity axis VERY steady. There may be a future addition where the droid starts rotating slowly under position control.
This is currently a theoretical concept. We have an IMU to measure turn rate, and we can use it to control the droid's turn. In this mode, we do not give direct speed differences from the remote to each wheel, but the remote governs the yaw rate set point. A PID controller then turns this into differential signals for both speed-controlled wheels. This mode is something I want to try, but whether it will make any difference for the positive is doubtful. It will be sensitive to IMU heading drift, and since we have no compass correcting for it will be quite involved, like an extended Kalman filter taking encoder motion into account.
LQRs can co-optimize several model parameters, like speed over ground and pitch angle. Using LQR instead of PID may be easier to parametrize and yield much more dynamic results.
IMU balancing with a PID controller always has the disadvantage that it will overshoot with sudden manoeuvers, because the controller is always lagging about a half wave behind. Acceleration settings and feedforward will be able to compensate this to a degree, but probably not 100%. Maybe it would be a good idea to move to MPC or other advanced control concepts and leave the simple PID behind.