54. agxModel: PID Controller

In agxModel::PidController1D there is an implementation of a PID controller with the classic gains for proportional, integral, and derivative gain described by the following mathematical expression:

\[u(t) = MV(t) = K_p e(t) + K_i \int_{0}^{t} e(\tau) d\tau + K_d \frac{de}{dt},\]

where \(u\) is the out signal, also known as the Manipulated Variable (MV), and \(K_p\), \(K_i\), and \(K_d\) are the tuning parameters or proportional gain, integral gain, and derivative gain. In the figure below, Fig. 54.1, there is a schematic representation of the PID controller including the process the PID is controlling, known as the Plant.

The above equation is discretized with the forward Euler Method and this leads to an explicit solution of \(u(t)\). The error, \(e\), is calculated as the difference between the measured Process Variable (PV) on the Plant and the desired Set Point (SP), the derivative of the error is discretized as \(\frac{de}{dt} = \frac{e - e_{n-1}}{t - t_{n-1}}\), where \(e_{n-1}\) is the error from the previous time step and \(t - t_{n-1}\) is the time step. The integral part is approximated by adding up the error, \(e\), multiplied by the time step. This explicit solution of \(u(t)\) is relatively sensitive to high frequencies in the Plant.

The PID controller has an integral windup algorithm with both “clamping” and/or an integral back-calculation algorithm. The clamping is disabled per default and the back-calculation is activated if the PID controller are given constraints on the Manipulated Variable (MV). The backward calculation is done by recomputing the integral term to its previous state before the windup started and by that disabling the integration.

../_images/pid_diagram.png

Fig. 54.1 Above is a schematic diagram over the PID controller. From left to right we have the error which is the difference between the Set Point (SP) and the Process Variable (PV). \(K\) represents the three gains and the block to the right represent the integral windup algorithm activated when the output signal, Manipulated Variable (MV), is outside the Plant input limits.

54.1. Usage

It is possible to use the PID controller in a stand alone application but the agxModel::PidController1D is preferable to be used together with the agxModel::ControllerHandler which is a StepEventListener. The agxModel::ControllerHandler reads the Process Variable (PV) from the Plant, or controlled process, and lets the PID controller calculate a new Manipulated Variable (MV) with respect to the current Set Point (SP), and then that value is sent to the Plant.

In the tutorial below we create a pendulum with a load attached via a wire to a winch, see Fig. 54.2. The winch is controlled with a PID controller where the Plant is the winch together with the pendulum. The Manipulated Variable is the winch speed, the pendulum weight z-position is the Process Variable, and the Set Point is the desired height of the load above the xy-plane at z equal to zero. The winch base has an oscillating vertical motion which moves the pendulum up and down while the PID controller is counteracting this vertical motion plus the vertical motion due to the swing of the pendulum load. Note that the tutorial code doesn’t contain any graphical nodes.

../_images/pid_controller_scene.png

Fig. 54.2 This is the pendulum scene in the tutorial. The green cube represents the winch with an oscillating vertical motion. The red sphere is the pendulum load that the PID controller is stabilizing and the gray transparent box is the vertical Set Point for the PID controller.

The first part of our tutorial is an implementation of the agxModel::ControllerHandler::Plant base class that can read process data and set signal values on the system we are controlling. In this specific case the Plant Process Variable is the z-position of the pendulum weight and the Manipulated Variable is the winch speed controlling the length of the wire. The class overrides two abstract methods in agxModel::ControllerHandler::Plant, the methods getProcessVariable and setManipulatedVariable, see code below. Both the getProcessVariable and the setManipulatedVariable are called in the post step of the agxModel::ControllerHandler.

/// A thin implementation of the Plant base class. This implementation reads the
/// pendulum position and sets the winch speed.
class PlantWinch : public agxModel::ControllerHandler::Plant
{
public:
  PlantWinch(agxWire::Winch* winch, agx::RigidBody* pendulumWeight) :
      m_winch(winch), m_pendulumWeight(pendulumWeight)
  {
  }

  /// \return The current measured Process Variable (PV).
  agx::Real getProcessVariable(const agx::TimeStamp& time) override
  {
    // Update the pendulum base oscillating motion.
    // Note: It is not considered best practice to modify the simulation in this method.
    agx::Real winchBaseVelocity = 0.5 * std::cos(1 / 10.0 * time);
    m_winch->getRigidBody()->setVelocity(agx::Vec3(0, 0, winchBaseVelocity));

    // Here the Plant Value is the z position of the load attached to the wire in the winch.
    return m_pendulumWeight->getPosition().z();
  }

  /// Set the Manipulated Variable (MV) controlling the Plant, also known as Control Variable.
  void setManipulatedVariable(agx::Real manipulatedVariable) override
  {
    // The PID manipulate variable is set as the winch speed.
    // Negative since a negative error should pull in the weight.
    m_winch->setSpeed(-manipulatedVariable);
  }

private:
  agxWire::WinchRef m_winch;
  agx::RigidBodyRef m_pendulumWeight;
};

The next part of our example is to setup the pendulum and add a Control System with a PID controller to the simulation. In our control system, agxModel::ControllerHandler, the above specified PlantWinch is added together with an agxModel::PidController1D.

agxSDK::SimulationRef sim = new agxSDK::Simulation;

RigidBodyRef pendulumWeight = new RigidBody(new agxCollide::Geometry(new agxCollide::Sphere(1)));
pendulumWeight->setPosition(-0.5, 0.5, -10);

// Create wire to hang between the winch base and the winch load
agxWire::WireRef wire = new agxWire::Wire(0.015, 1, false);
// Attach wire to load
wire->add(new agxWire::BodyFixedNode(pendulumWeight));
// Wire nodes
wire->add(new agxWire::FreeNode(agx::Vec3(0, 0, 0)));

agx::RigidBodyRef winchBase = new agx::RigidBody("winchBase");
winchBase->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(1, 1, 1))));
winchBase->setMotionControl(agx::RigidBody::KINEMATICS);

agxWire::WinchRef winch = new agxWire::Winch(winchBase, agx::Vec3(0, 0, 0), agx::Vec3(0, 0, 1));
agx::Real pulledInLength = 10;
// attach wire to winch
wire->add(winch, pulledInLength);

// Create the Plant describing the process we should control. The class, PlantWinch,
// implements the base class agxModel::ControllerHandler::Plant which reads the
// pendulum z-position and sets the winch speed.
agxModel::ControllerHandler::PlantRef plant = new PlantWinch(winch, pendulumWeight);

agx::Real setPoint = -10;
agx::Real proportionalGain = 0.1;
agx::Real integralGain = 0.05;
agx::Real derivativeGain = 5;
agxModel::PidController1DRef pidController = new agxModel::PidController1D();
pidController->setGains(proportionalGain, integralGain, derivativeGain);
pidController->setSetPoint(setPoint);

agxModel::ControllerHandlerRef controller_handler =
  new agxModel::ControllerHandler(pidController, plant);
sim->add( pendulumWeight );
sim->add( winchBase );
sim->add( wire );
sim->add( controller_handler );