Front-end loader self leveling

Introduction

Last month I saw PhD position in Automation of Front-End Loaders(FEL) at the Umeå university, Sweden. First time I couldn't imagine what they would like to automatize on a FEL. After some thought and digging on the internet I figured out, that they would like to control the angle of the bucket in order to keep the angle constant relative to the ground. In the second picture above, you can see that the angle of the bucket remains constant and ensures the risk of rollback.

Figure 1.

Figure 2.

This function is called bucket self leveling. Some manufacturers, like Buhler Allied, are already selling their FEL with this operation.
For the better understanding of the project I created the kinematic model of the FEL.

Figure 3. Kinematic model of the FEL

So the first angle($\theta_{_1}$) will be controlled by the end-user using a joystick, the second angle($\theta_{_2}$) has to be adjusted in order to keep the third angle($\theta_{_3}$) constant relative to the base coordinate system.

My idea is to control the hydraulic cylinders independently. In this application the straightforward solution is not to measure the joint's angles, but to estimate the position of the piston by measuring the system's and cylinder's pressures.

Figure 4. Control block

In the control block above the $x_{_1}$, $x_{_2}$ are the positions of the piston, the $\theta_{_1}$, $\theta_{_2}$, $\theta_{_3}$ are the angles of the joint, the $u_{_1}$, $u_{_2}$ are the valve's input voltages and the $y_{_1}$, $y_{_2}$ are the measured pressures. In this case we have three references. The first one is the first piston's position($x_{_1ref}$), the second is the bucket's angle($\theta_{_3ref}$) relative to the base coordinate system, and the third is the angle of the second joint($\theta_{_2ref}$). The first piston's position and the bucket angle will be defined by the end-user with a joystick, while the angle of the second joint will be calculated in order to keep the bucket's angle constant.

System modelling

The system consists of a servo valve, a hydraulic cylinder and a coupled mass as you can see in figure 5. When we are creating a system model, we also would like to validate the model against the real system. Unfortunately I don't have such a system but I still can create a system using Matlab/Simscape, where I just have to setup the hydraulic system's parameters. With this straightforward simulation tool it takes only a few hours to build such a system. If you need the Simscape model then  just send me and email.

Figure 5. Hydraulic system

Where $p_S$ is the supply pressure, the $p_A$, $p_B$ are the cylinder chamber pressures and $u$ is the valve input voltage.

Figure 6. Hydraulic system in Matlab/Simscape

To validate my system, I am going step by step where the first step is to calculate the $Q_A$ and $Q_B$ control flows from the measured pressures($p_S$, $p_R$, $p_A$, $p_B$). The nonlinear flow equations are described in numerous papers and books, personally, I used the Hydraulic Control Systems by Herbert E. Merritt to write down these equations.

$PP_A=\frac{p_S-p_R}{2}+sign(u)\left(\frac{p_S-p_R}{2}-p_A\right) \tag{1}$
$PP_B=\frac{p_S-p_R}{2}+sign(u)\left(p_B-\frac{p_S-p_R}{2}\right) \tag{2}$

$Q_A = u \omega C_v \sqrt{\frac{2}{\rho_{oil}}abs(PP_A)} sign(PP_A) \tag{3}$
$Q_B = -u \omega C_v \sqrt{\frac{2}{\rho_{oil}}abs(PP_B)} sign(PP_B) \tag{4}$

Where $u$ is the valve input voltage, $\omega$ is the servo valve orifice area gradient, $C_v$ is the  servo valve discharge coefficient and $\rho_{oil}$ is the hydraulic oil density.

The proportional valve actuator can add some milliseconds delay to the Matlab/Simsacape system, because of this I also had to add this delay to the control signal in my model. After this correction the calculation of the flows using my formulas was identical with the measured values in the Matlab/Simsacpe model.

Figure 7. Flow calculation.

References and further readings 

[1] Herbert E. Merritt - Hydraulic Control Systems

[2] Keiwan Kashi, Dirk Söffker - Model-Based Estimation of Force and Displacement of a Hydraulic Cylinder
[3] http://www.commerce.wa.gov.au/ - Front end loaders and their attachments on tractors
[4] Matlab/Simscape help

Discrete H infinity filter design

Short introduction

In many application cases where some of the system states can't be measured, we can use a state observer. One solution would be the Kálmán filter which I already presented. Another solution is the $H_\infty$ filter which is robust regarding the unpredictable noise source. In the Kálmán filter not only the noise process needs to be zero mean, but also the $Q$, $R$ covariance matrices have to be known, without them we can't design an appropriate observer. While in the $H_\infty$ filter doesn't make any assumptions about the noise, it minimizes the worst case estimation error.

Discrete $H_\infty$ filter design

We have the following discrete-time system

$$x_{k+1}=A_k x_k+B_k u_k + w_k\\y_k=C_k x_k+D_k u_k+v_k\tag{1}$$

The $1$ system has to be controllable and observable. We would like to achieve a small estimation error $e_k=z_k - \hat{z}_k$ for any $w_k,v_k$. In this case we want to solve the minimax problem as follows:

$$min_xmax_{w,v}J\tag{2}$$

where $J$ is:

$$J=\frac{\sum\limits_{k=0}^{N-1}{\left|\left|z_k - \hat{z}_k\right|\right|_{Q_k}^2}}{\left|\left|x_0 - \hat{x}_0\right|\right|_{P_0}^2+\sum\limits_{k=0}^{N-1}{\left|\left|w_k\right|\right|_{W_k}^2+\left|\left|v_k\right|\right|_{V_k}^2}}\tag{3}$$
Theorem: Let $\gamma>0$ be a prescribed level of noise attenuation. Then there exists a $H_\infty$ filter for $z_k$ if and only if there exists a stabilizing symmetric solution $P_k>0$ to the following discrete-time Ricatti equation:

 $$P_{k+1}=A_kP_k(I-\gamma Q_kP_k+C_k^TV_k^{-1}C_kP_k)^{-1}A_k^T+B_kW_kB_k^T\tag{4}$$
where
$$\hat{x}_{k+1}=A \hat{x}_k+B u_k+K_k(y_k-C_k\hat{x}_k)\tag{5}$$
The $K_k$ is the gain of the $H_\infty$ filter and it is given by:
$$K_k=A_kP_k(I-\gamma Q_kP_k+C_k^TV_k^{-1}C_kP_k)^{-1}C_k^TV_k^{-1}\tag{6}$$
Proof: Xuemin Shen and Li Deng: Game Theory Approach to Discrete $H_\infty$ Filter Design

As you can see the filter gain doesn’t depend from the states of the system so it's possible to calculate offline.

Cpp implementation

On github you can find a C++ implementation of the $H_\infty$ filter, this is only a chunk of the C++ class.

/*
  *@summary: The filter gain calculation
  *@param A: State space model A matrix
  *@param B: State space mode B matrix
  *@return: Filter Gain
  */
 Matrix HINF::calkGain(Matrix A, Matrix B) {
  if (A.rows() != P.cols())
   ERRORH::throwerror("A rows has to be equal with P columns");

  Matrix I = Matrix::Identity(A.cols(), A.cols());

  Matrix L = (I - lambda * Q * P + C.transpose() * V.inverse() * C * P).inverse();

  P = A * P * L * A.transpose() + B * W * B.transpose();

  K = A * P * L * C.transpose() * V.inverse();

  return K;
 }

 /*
  *@summary: Update the state
  *@param A: State space model A matrix
  *@param B: State space mode B matrix
  *@return: The new state
  */
 Matrix HINF::updateState(Matrix A, Matrix B, Matrix u) {
  x = A * x + B * u;
  return x;
 }

 /*
  *@sumary: Estimate the next states
  *@param M: Measured state
  */
 Matrix HINF::estimate(Matrix M) {

  x = x + K * (M - C * x);

  return x;
 }

Readings:

• Kemin Zhou: ESSENTIALS OF ROBUST CONTROL
• Xuemin Shen and Li Deng: Game Theory Approach to Discrete $H_\infty$ Filter Design

MPC implementation in C++

I was really curious about how fast can we calculate the control signal using MPC. Because of this, first I wanted to write a short C code, but finally I end up with C++. because it is easy to overload the operations and more flexible and easier to implement the Matrix operations. Also there are disadvantages just to mention that the std::vector is slower than the C array and C++ can increase the size of the compiled code significantly.

I have to mention that this is my first code in C++, but I already have experience with OOP(Matlab, Python, Java, PHP) and at the University I got the basics of the C. 

For the first test I used the Eigen library which is really straight forward, but where is the challenge if I'm using a libraries instead of writing my own code and learning new OP language. With the Eigen I could achieve around 6e-5s which is really good, but this package is optimized for big matrices, take a look on the benchmark. In my case, the biggest matrix is a 4x4 matrix, which is really small.

In the second test I wrote my own library(you can fork from this on the github), I'm using arrays instead of std::vectors, because it's faster as I mentioned before.
The result of the test:

Runtime

My next step was to test on my Raspberry Pi using ChibiOS/RT(ARM  CPU). In this platform to calculate one control signal took around 2ms which is not really good. In the mean time I also install Arch Linux on the RPI where the runtime was 0.4ms which is a bit better, so I will try to compile the Linux kernel with real time capability.

Finally this is the test code

int main(int argc, char* argv[]) {

 /*
  * Defining the variables
  */
 //Control horizon
 int nc = 4;
 // Prediction horizon
 int np = 4;
 //Control signal 
 Matrix u(1, 1);
 // Sampling time
 double Ts = 5e-5;

 MPC mySysMat(2, 1, 1);
 //Error matrix, just for testing; 

 Matrix DeltaU(np, 1);

 Matrix R = 1e-5
   * Matrix::Identity(mySysMat.C.rows() * nc,
     mySysMat.C.rows() * nc);
 Matrix Q = 1e4 * Matrix::Identity(np, np);

 Matrix x0(2, 1);
 //Reference signal
 Matrix y_ref(100000, 1);
 y_ref.rblock(0, 0, 50000, 1) = 100 * Matrix::Ones(50000, 1);
 y_ref.rblock(50000, 0, 100000, 1) = -100 * Matrix::Ones(50000, 1);

 double start = now();

 //DC motor parameters
 double Rm = 0.35;
 double Km = 0.0296;
 double Ke = 0.0296;
 double b = 6.7 * 10e-5;
 double J = 2.9 * 10e-6;
 double Fc = 0.02;
 double L = 25 * 10e-6;

 /*
  Parsing the input parameters
  */
 for (int i = 1; i < argc; i++) {
  if (i + 1 != argc) // Check that we haven't finished parsing already
   if (!strcmp(argv[i], "-s")) {
    Ts = atof(argv[i + 1]);
   }
  if (!strcmp(argv[i], "-nc")) {
   nc = atoi(argv[i + 1]);
  }
  if (!strcmp(argv[i], "-np")) {
   np = atoi(argv[i + 1]);
  }

  if (!strcmp(argv[i], "-h")) {
#ifdef CMD
   std::cout << "Coming soon..." << std::endl;
#endif

  }
 }

 //Initializing the system matrices
 mySysMat.Fi << 1 - Ts * (Rm / L), -Ts * (Ke / L), Ts * (Km / J), 1
   - Ts * (b / J);

 mySysMat.Ga << Ts * 1 / L, 0;

 mySysMat.C << 0, 1;

 mySysMat.calcMPCFi(np);
 mySysMat.calcMPCGa(np);
 mySysMat.calcMPCGy(np, nc);
#ifdef DEBUG
 std::cout << "Calculation Fi,Ga,Gy took : " << (double) (now() - start)
   << std::endl;
#endif

 Matrix calcT(1, y_ref.rows());
 Matrix u_hist(1, y_ref.rows());
 Matrix w_hist(1, y_ref.rows());

 for (int i = 0; i < y_ref.rows() - np; i++) {

  start = now();
  //Calculating the error

  DeltaU = mySysMat.calcContSig(
    mySysMat.calcError(y_ref.block(i, 0, i + np, 1), x0, u), Q, R);

  u(0, 0) += DeltaU(0, 0);

  //We are not including this in the time measurement
  if (abs(u(0, 0)) > 5)
   u(0, 0) = sgn(u(0, 0)) * 5;

  u_hist(0, i) = u(0, 0);

  w_hist(0, i) = x0(1, 0);
  //Simulating the system
  x0 = mySysMat.Fi * x0 + mySysMat.Ga * u;

  //Storing the calculation time
  calcT(0, i) = (double) (now() - start);

 }
#ifdef DEBUG
 std::cout << "Minimum:" << calcT.min() << std::endl;
 std::cout << "Maximum:" << calcT.max() << std::endl;
#endif

#ifdef PLOT
 Gnuplot g3("lines");
 plot_x(y_ref, Ts, &g3);
 plot_x(w_hist, Ts, &g3);

 Gnuplot g2("lines");
 plot_x(u_hist, Ts, &g2);

 Gnuplot g1("lines");
 plot_x(calcT, Ts, &g1);
 std::cin.get();
#endif

Model predictive control of brushed DC motor

Motor model

The state space model of a brushed DC motor as follow:
$$A =\left[ {\begin{array}{cc} -\frac{R_{a}}{L_{a}} & -\frac{K_{e}}{L_{a}} &  0 \\ \frac{K_{m}}{J} & -\frac{b}{J} & 0 \\ 0 & 1 & 0 \end{array} } \right] B=\left[ {\begin{array}{cc} \frac{1}{L_{a}} \\ 0 \\ 0 \end{array} } \right] C=\left[ {\begin{array}{cc} 0 \\ 1 \\ 0 \end{array} } \right] D=\left[ {\begin{array}{cc} 0 \end{array} } \right]\tag{1}$$
As you can see, I assumed that the torque is zero. 
Where the states are:
$$\dot{x(t)} =\left[ {\begin{array}{cc} \frac{di}{dt} \\ \frac{d\omega}{dt} \\ \frac{d\theta}{dt} \end{array} } \right]\tag{2}$$

Controller design

I already introduced the MPC controller, please read that first before you continue to read this blog. 

The first step is to determine the Toeplitz matrix:

$$\begin{bmatrix}x_{k+1|k} \\ \vdots \\ x_{k+N_c|k} \\ x_{k+N_c+1|k} \\ \vdots \\ x_{k+N|k} \end{bmatrix}=\begin{bmatrix}\Phi \\ \vdots \\ \Phi^{N_c} \\ \Phi^{N_c+1} \\ \vdots \\ \Phi^N \end{bmatrix}x_k + \begin{bmatrix}\Gamma \\ \vdots \\ \sum_{i=0}^{N_c-1} \Phi^{i} \Gamma\\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma\\ \vdots \\ \sum_{i=0}^{N}\Phi^i \Gamma \end{bmatrix}u_{k-1}\\+ \begin{bmatrix}\Gamma & \cdots & 0 \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma & \cdots & \Phi \Gamma+\Gamma \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N-1}\Phi^i \Gamma & \cdots & \sum_{i=0}^{N-N_c}\Phi^i \Gamma \end{bmatrix} \begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k} \end{bmatrix}\tag{1}$$ 

I wrote the following Matlab function to build up the  $\Phi^*$, $\Gamma^*$, $G_y$:

function [FiFi,FiGam,Gy] = getFiFi_FiGa(Fi,Ga,C,N,Nc,n)
    
    FiFi=[]; %ennek a merete N*n x n kell legyen
    
    for i=1:N,    
        FiFi=[FiFi; C*Fi^i]; 
    end
    % FiGam szamitasa
    FiGam=[];
    for i=1:N,
        S=0;
        for j=0:i-1,       
            S=S+C*Fi^j*Ga;    
        end
        FiGam=[FiGam; S];
    end
    
    ng=size(FiGam,1);
    mg=size(FiGam,2);
    % Gy szamitasa
    Gy=zeros(ng,mg*Nc);
    % Gy szamitasa
    for j=1:mg:Nc*mg   % oszlopok         
        Gy(j:end, j:j+mg-1)=FiGam(1:end-j+1,:);
    end

end

To find the optimal control signal we have to solve the following optimization problem $J_k(u,x_k)=\frac{1}{2}\Delta u_k^TH\Delta u_k+f^T\Delta u_k+const$. I already presented the solution of this problem (see Model predictive control). I am going to  implement the calculation of the $\Delta u_k$ optimal control signal in the following example:

for v=1:n-N
    %Error vector
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
     deltau=inv(Gy'*Q*Gy+R)*Gy'*Q*E;
     u=u+deltau(1:2);
     for k=1:length(u)
         if(abs(u(k))>lim)
             u(k) = sign(u(k))*lim;
         end
     end
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;
    
    
    um=[um u];
    ym=[ym;yt];
end;

In the example above I also tried to constrain the control signal by choosing the $lim$ variable to 38. After choosing the values of the noise matrices as follows

• $Q=eye(N*Nro)*1e4$ where $N$ is the number of the horizon and $Nro$ is the number of the outputs.
• $R=eye(Nc*Nrin)*1e-5$ where $Nc$ is the number of the control horizon and $Nrin$ is the number of the inputs.

I got these results:

Figure 1. Rotor speed

Figure 2. Control signal

Figure 3. Error between the reference and measured rotor speed

We can reduce the error by increasing or eliminating the constrain of control signal as you can see in $Figure 4$

Figure 4. Error between the reference and measured rotor speed without limiting the control signal

The other solution  of the optimal control signal calculation in Matlab is the built in $quadprog$ function. You can find a simple example below, also you can read the description of this function on the Matlab help page:

H=2*(Gy'*Q*Gy+R);
for v=1:n-N
    %Hiba vektor    
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
    f=-2*E'*Q*Gy;        
    
    b = [abs(lim-u(1));
        abs(lim+u(1))];
    A = [1 0 0 0 0 0 0 0;
        -1 0 0 0 0 0 0 0;
        ];
     
    Aeq=[];beq=[];
    
    [deltau,FVAL,EXITFLAG]=quadprog(H,f,A,b,Aeq,beq);
    u=u+deltau(1:2);
    
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;    
    
    um=[um u0];
    ym=[ym;yt];
    Em = [Em;E];
end;

Appendix

Motor parameters:

    Rm = 0.35;
    Km=0.0296;
    Ke = 0.0296;
    b = 6.7*10e-5;
    J = 2.9*10e-6;
    Fc = 0.02;
    L = 25 *10e-6;
    Ts = 1e-4;