Svenska ElektronikForumet

God morgon matematikälskare!

Jag har hållit på med kvadratisk programmering förut. Allt handlar om att hitta lambda-uttrycken \(\lambda_i\) och det ska uppfylla dessa Karush Kuhn Tucker villkor.

\(\bigtriangledown f(\bar x) + \sum_{i=1}^m(\lambda_i)\bigtriangledown g_i(\bar x) = 0\)
\(g_i(\bar x) \leq 0 \forall i\)
\(\lambda_i g_i(\bar x) = 0 \forall i\)
\(\lambda_i \geq \forall i\)

Detta betyder att om vi har vektorn \(\bar x\) och vi tror att det är våran lokala minimum. Men om Karush Kuhn Tucker villkoren uppfylls, så har vi hittat vårat globala minimum. Uttrycket \(g_i(\bar x)\) är våra begränsningar.

Villkoren löser inte det kvadratiska problemet. För att lösa problemet så använder vi exempelvis Frank-Wolfe's metod, eller modifierad Simplex. Enklaste är om vi använder qpOASES_e. Vilket vi ska göra!

Nu börjar vi igen.

Jag har nyss laddat ned qpOASES_e och sett två intressanta mappar - include och src. Mappen src innehåller .c filer och include innehåller .h filer.

Selección_017.png

Jag har ett litet problem att "länka" till .h filerna. Vet ni hur man gör?

Selección_018.png

Det finns ju trevlig cmake-definition så att bygga för host torde vara enkelt (på mac fungerar det klockrent) se t.ex. https://asciinema.org/a/x5NTAqv5iRgYauabqYM2VqQtf
Provade sen att dundra hem cross compile toolchain för ARM (typ för STM/blue-pill m.m.) https://asciinema.org/a/L7cUHeHVIZvd6314lYZU7ocYO

Värt att notera att ett "install" target skapas som paketerar ihop headerfiler följande filer är vad som kopieras:

Bounds.h
Constraints.h
Matrices.h
QProblem.h
UnitTesting.h
Constants.h
Flipper.h
MessageHandling.h
QProblemB.h
Utils.h
ConstraintProduct.h
Indexlist.h
Options.h
Types.h
extras/OQPinterface.h

Så med det byggda libbet och headerfilerna ovan så bör du kunna immitera exempel-filerna dvs

Kod: Markera allt

#include <qpOASES_e.h>

Under förutsättningen att du lägger till mappen där du har headerfilerna ovan till vart gcc letar dvs med växeln

Kod: Markera allt

-I

du kan också lägga headerfiler och allt tillsammans med ditt program bör fungera lika bra. Och glöm inte att länka med

Kod: Markera allt

-l qpOASES_e

ps. från asciinema så är det bara att markera, copy-paste om det är något av värde som du ser

ds.

Tackar! Synd att jag inte hann säga till men jag har faktiskt fått min QP-lösare att fungera och den stämmer överens med MATLAB's Quadprog!

Mycket glädjande.

Då minimerar jag:
\(\frac{1}{2}x^THx + x^Tg\)
med begränsingarna
\(lb \leq x \leq ub\)
\(lbA \leq Ax \leq ubA\)

Matris \(H\) är en hessianmatris om ni undrar. Det betyder i praktisk mening att \(H \geq 0\) och \(H\) är symmetrisk.

Detta betyder att jag kan optimera in och utsignaler hos ett system och även begränsa hastigeterna i systemet. Har ni något förslag på en reglermodell? Tänkte om ni är intresserade? Ge mig något förslag på en linjär modell, eller olinjär om den inte alls är för svår. Föredrar linjär helst

För er som inte har en MATLAB licens från Mathworks, kan använda GNU Octave's QP-lösare. Den fungerar lika bra.
Här är ett exempel på körning i GNU Octave

Kod: Markera allt

>> H = [1 4; 4 5]
H =

   1   4
   4   5

>> g = [1.5; 1.0]
g =

   1.5000
   1.0000

>> lb = [0.5; -2.0]
lb =

   0.50000
  -2.00000

>> ub = [10.0; 2.0]
ub =

   10
    2

>> lbA = -1
lbA = -1
>> ubA = 2
ubA =  2
>> [x, obj, info, lambda] = qp([], H, g, [], [], lb, ub, lbA, A, ubA)
x =

   3
  -2

obj = -7
info =

  scalar structure containing the fields:

    solveiter =  7
    info = 0

lambda =

   0.00000
   0.00000
   1.25000
   0.00000
   0.00000
   7.00000

>>

Här är ett exempel på en körning i C.

Kod: Markera allt

#include <stdio.h>
#include <stdlib.h>

#include "qpOASES/Header/qpOASES_e.h"

int main(void) {

	USING_NAMESPACE_QPOASES

		/* Setup data of first QP. */
		real_t H[2*2] = { 1.0, 4.0,
						  4.0, 5.0};
		real_t A[1*2] = { 0.5, -0.25};
		real_t g[2] = { 1.5,
				        1.0 };
		real_t lb[2] = { 0.5,
				        -2.0 };
		real_t ub[2] = { 10.0,
				         2.0 };
		real_t lbA[1] = { -1.0 };
		real_t ubA[1] = { 2.0 };

		/* Setting up QProblem object. */
		static Options options;
		static QProblem example;

		int nWSR;
		real_t xOpt[2];
		real_t yOpt[2+1];

		QProblemCON( &example,2,1,HST_UNKNOWN );
		Options_setToDefault( &options );
		QProblem_setOptions( &example,options );

		/* Solve first QP. */
		nWSR = 6;
		QProblem_init( &example,H,g,A,lb,ub,lbA,ubA, &nWSR,0 );

		/* Get and print solution of first QP. */
		QProblem_getPrimalSolution( &example,xOpt );
		QProblem_getDualSolution(   &example,yOpt );
		printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n",
				xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2], QProblem_getObjVal( &example ) );


		return 0;
}

Och resultatet blir xOpt likadant som GNU Octave visar sin \(x\) vektor.

Kod: Markera allt

xOpt = [ 3.000000e+00, -2.000000e+00 ];  yOpt = [ 0.000000e+00, 1.250000e+00, -7.000000e+00 ];  objVal = -7.000000e+00



####################   qpOASES  --  QP NO.   1   #####################

    Iter   |    StepLength    |       Info       |   nFX   |   nAC    
 ----------+------------------+------------------+---------+--------- 
       0   |   1.250000e-01   |   REM BND    1   |     1   |     0   
       1   |   8.571429e-02   |   REM BND    0   |     1   |     0   
       2   |   1.898734e-02   |   ADD BND    1   |     2   |     0   
       3   |   1.035824e-01   |   REM BND    0   |     1   |     0   
       4   |   1.422414e-01   |   ADD CON    0   |     1   |     1   
       5   |   1.000000e+00   |    QP SOLVED     |     1   |     1

Jag bifogar koden och projektet. Kan ni testa om det går att komplimera på en liten värre mikroprocessor? Jag talar inte om 8-bits processorer. Jag talar om 32-bits processorer som har så kallat lite mera kräm att ge?

Antar att vi har våran tillståndsmodell som vi har antingen modulerat fram via mätdata eller skrivit den för hand. Det rekommenderas att bygga från mätdata - alla gånger.

\(x(k+1) = Ax(k) + Bu(k)\)
\(y(k) = Cx(k)\)

Där efter så bygger vi \(\Gamma\) och \(\Phi\) från

\(x(2) = Ax(1) + Bu(1)\\
x(3) = Ax(2) + Bu(2) = A(Ax(1) + Bu(1)) + Bu(2)\\
x(4) = Ax(3) + Bu(3) = A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)\\
x(5) = Ax(4) + Bu(4) = A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)\\
x(6) = Ax(5) + Bu(5) = A(A(A(A(Ax(1) + Bu(1)) + Bu(2)) + Bu(3)) + Bu(3)) + Bu(3)\\ \vdots \\ x(n+1) = A^{n} + A^{n}B + A^{n-1}B + A^{n-2}B + \dots + AB + B\)

Därför att vi använder våran tillståndsmodell igenom att återanvända \(x(k+1)\) nu nuvarande \(x(k)\). Vi kan sammanfatta hela med.

\(\begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix} = \begin{bmatrix}
CA\\
CA^2\\
CA^3\\
CA^4\\
\vdots\\
CA^{n-1}
\end{bmatrix}x_0+\begin{bmatrix}
CB & 0 & 0 & 0 & \dots & 0\\
CAB & CB & 0 & 0 & \dots & 0\\
CA^2B & CAB & CB & 0 & \dots& 0\\
CA^3B & CA^2B & AB & CB & \dots& 0\\
\vdots & \vdots & \vdots &\ddots &\ddots & 0\\
CA^{n-2}B & CA^{n-3}B & CA^{n-4}B & CA^{n-5}B & \dots & CB
\end{bmatrix} \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)

Där \(\Gamma\) är den stora lågtriangulära toeplitz matrisen och \(\Phi\) är den avlånga och smala matrisen. Vårat mål är att hitta \(U = \begin{bmatrix}
u_1\\
u_2\\
u_3\\
u_4\\
\vdots\\
u_n
\end{bmatrix}\)

När vi sätter \(R = \begin{bmatrix}
x_1\\
x_2\\
x_3\\
x_4\\
\vdots\\
x_{n}
\end{bmatrix}\)

Vi sätter in exakt allt på denna formel:

\(J = \frac{1}{2}U^TQU + c^TU\)

Där \(Q = \Gamma^T \alpha I \Gamma\) och \(c = \Gamma^T Q \Phi x_0 - \Gamma^T Q R\)

\(\alpha I\) ska vara mellan 0 och 1 för att undvika hård styrning av insignalerna.

Notera att vectorn \(c\) skall alltid uppdateras med nya tillståndsvektorn \(x_0\) för varje iteration. Du ska estimera \(x_0\) från ett kalman filter.

Nu fokuserar vi på begränsingarna:

\(ulb \leq U \leq uub\)
\(ylb \leq \Gamma U \leq yub\)

Där \(yub = MAX_Y - \Phi x_0\) och \(ylb = MIN_Y - \Phi x_0\)
För \(ulb, uub\) så är det bara sätta en enkel vektor t.ex. rada upp 1,1,1,1,1,1 om \(U\) ej får vara över 1 för alla element.

För enkelhetens skull så har jag varit snäll och gjort ett MATLAB-skript till er som är kopatibelt med GNU Octave.

Kod: Markera allt

function [H, Ay, g0, g1, ylb, yub, ulb, uub] = QP(A, B, C, x0, r, u_lb, u_ub, y_lb, y_ub, horizon, alpha)

  % Compute the PHI matrix now!
  PHI = phiMat(A, C, horizon);
  % Compute the GAMMA matrix now
  GAMMA = gammaMat(A, B, C, horizon);
  % Compute the tuning matrix - Set it to identity matrix
  Q = alpha*eye(size(GAMMA, 1), size(GAMMA, 1));
  % Compute H matrix
  H = GAMMA'*Q*GAMMA;
  % Create g vector. g = g0*x - g1*r where x is the state vector and r is the reference vector
  g0 = GAMMA'*Q*PHI;
  g1 = GAMMA'*Q;
  % Create the matrix for saturation on inputs
  Ay = GAMMA;

  % Comput best inputs - Try a computation
  g = g0*x0 - g1*repmat(r, horizon, 1)
  ylb = repmat(y_lb, horizon, 1) - PHI*x0;
  yub = repmat(y_ub, horizon, 1) - PHI*x0;
  ulb = repmat(u_lb, horizon, 1);
  uub = repmat(u_ub, horizon, 1);
  u = qp([], H, g, [], [], ulb, uub, ylb, Ay, yub)  % Replace qp with quadprog if you are using MATLAB
    
end

function PHI = phiMat(A, C, N)
  
  % Create the special Observabillity matrix
  PHI = [];
  for i = 1:N
    PHI = vertcat(PHI, C*A^i);
  end
  
end

function GAMMA = gammaMat(A, B, C, N)
  
  % Create the lower triangular toeplitz matrix
  GAMMA = [];
  for i = 1:N
    GAMMA = horzcat(GAMMA, vertcat(zeros((i-1)*size(C*A*B, 1), size(C*A*B, 2)),cabMat(A, B, C, N-i+1)));
  end
  
end

function CAB = cabMat(A, B, C, N)
  
  % Create the column for the GAMMA matrix
  CAB = [];
  for i = 0:N-1
    CAB = vertcat(CAB, C*A^i*B);
  end
  
end

Låt oss säga att jag använder dessa matriser som ska symbolisera ett dynamiskt beteende.

Kod: Markera allt

A =

       0.69922   0.34629
      -1.03887   0.35293

    B =

       0.20052
       0.69258

    C =

       1   0

Selección_003.png

Jag sätter \(yub = 0.7\) och \(ylb = 0\) för hela \(U\)

Jag kör en beräkning. Jag väljer att jag vill titta 10 iterationer i framtiden, 0.5 som mitt "börvärde" och jag ska stå på tillståndet\(x_0 = [0;0]\) från början och alpha är 0.9

Kod: Markera allt

[H, Ay, g0, g1, ylb, yub, ulb, uub] = QP(sysd.A, sysd.B, sysd.C, [0;0], 0.5, 0, 100, 0, 0.7, 10, 0.9);
>> u = 1.57215
           0.00000
           0.31200
           1.41416
           0.19002
           1.22215
           0.35191
           1.08564
           0.46700
          0.98861

Sedan kör jag ett exempel i C.

Kod: Markera allt

#include <stdio.h>
#include <stdlib.h>

#include "qpOASES/Header/qpOASES_e.h"

int main() {

	/*
	 * In this example we have selected the following configuration inside Confiugration.h
	 * NOTICE THAT YOU NEED A CPU that support "double" data type here if you want correct calculations
	 * SELECTED_CONTROL_STRATEGY MANUAL
	 * ADIM 2
	 * YDIM 1
	 * RDIM 1
	 * HORIZON 10
	 */

	/**
	 * State space model that are being used with sample time 0.5
	 *
	A =

       0.69922   0.34629
      -1.03887   0.35293

    B =

       0.20052
       0.69258

    C =

       1   0

	 */

	// Create the objective function
	real_t H[HORIZON*RDIM*HORIZON*RDIM] = {
			   0.2805315,   0.2018439,   0.0426243,  -0.0759651,  -0.1036495,  -0.0619421,  -0.0040178,   0.0286614,   0.0272949,   0.0098612,
			   0.2018439,   0.2778442,   0.1994990,   0.0429871,  -0.0714696,  -0.0964493,  -0.0568636,  -0.0070791,   0.0149778,   0.0086050,
			   0.0426243,   0.1994990,   0.2757980,   0.1998156,   0.0469099,  -0.0651867,  -0.0920177,  -0.0595349,  -0.0190196,  -0.0013312,
			  -0.0759651,   0.0429871,   0.1998156,   0.2757491,   0.1992087,   0.0459379,  -0.0658723,  -0.0916045,  -0.0576876,  -0.0164965,
			  -0.1036495,  -0.0714696,   0.0469099,   0.1992087,   0.2682287,   0.1871637,   0.0374421,  -0.0607511,  -0.0687136,  -0.0264218,
			  -0.0619421,  -0.0964493,  -0.0651867,   0.0459379,   0.1871637,   0.2489367,   0.1735563,   0.0456445,  -0.0240875,  -0.0186362,
			  -0.0040178,  -0.0568636,  -0.0920177,  -0.0658723,   0.0374421,   0.1735563,   0.2393389,   0.1793417,   0.0715045,   0.0112337,
			   0.0286614,  -0.0070791,  -0.0595349,  -0.0916045,  -0.0607511,   0.0456445,   0.1793417,   0.2358515,   0.1637535,   0.0502133,
			   0.0272949,   0.0149778,  -0.0190196,  -0.0576876,  -0.0687136,  -0.0240875,   0.0715045,   0.1637535,   0.1661745,   0.0685843,
			   0.0098612,   0.0086050,  -0.0013312,  -0.0164965,  -0.0264218,  -0.0186362,   0.0112337,   0.0502133,   0.0685843,   0.0361865};

	real_t Ay[HORIZON*YDIM*HORIZON*RDIM] = {
			   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,
			   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,
			   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,
			   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,
			  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,
			  -0.14641,  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,   0.00000,
			  -0.09141,  -0.14641,  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,   0.00000,
			  -0.00738,  -0.09141,  -0.14641,  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,   0.00000,
			   0.04768,  -0.00738,  -0.09141,  -0.14641,  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052,   0.00000,
			   0.05464,   0.04768,  -0.00738,  -0.09141,  -0.14641,  -0.10327,   0.06225,   0.27824,   0.38004,   0.20052};

	real_t g0[HORIZON*RDIM*ADIM] = {
			   0.0895555,   0.2675379,
			  -0.2126023,   0.1248676,
			  -0.2760081,  -0.0300475,
			  -0.1621426,  -0.1061071,
			  -0.0076856,  -0.0926476,
			   0.0835982,  -0.0338390,
			   0.0884757,   0.0180790,
			   0.0461764,   0.0363724,
			   0.0083306,   0.0259372,
			  -0.0022316,   0.0080912};

	real_t g1[HORIZON*RDIM*HORIZON*YDIM] = {
			   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,  -0.13177,  -0.08227,  -0.00664,   0.04291,   0.04918,
			   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,  -0.13177,  -0.08227,  -0.00664,   0.04291,
			   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,  -0.13177,  -0.08227,  -0.00664,
			   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,  -0.13177,  -0.08227,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,  -0.13177,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,  -0.09294,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,   0.05602,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,   0.25042,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.18047,   0.34204,
			   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.00000,   0.18047};

	// Minimum input signal
	real_t ulb[HORIZON*RDIM] = {
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0};

	// Maximum input signal
	real_t uub[HORIZON*RDIM] = {
			   100,
			   100,
			   100,
			   100,
			   100,
			   100,
			   100,
			   100,
			   100,
			   100};

	// Minimum output signal
	real_t ylb[HORIZON*YDIM] = {
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0,
			   0};

	// Maximum output signal
	real_t yub[HORIZON*YDIM] = {
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7,
			   0.7};

	// Reference
	double r = 0.5;

	//Initial state vector - Start with zeros. That works. Let kalman filter estimate the states later
	double x0[ADIM];
	memset(x0, 0, ADIM*sizeof(float));


	// Setting up QProblem object
	static QProblem objective;
	static Options options;
	QProblemCON(&objective, HORIZON*RDIM,HORIZON*YDIM, HST_UNKNOWN);
	Options_setToDefault(&options);
	QProblem_setOptions(&objective, options);

	// Declare best input vector
	int nWSR;
	real_t best_inputs[HORIZON*RDIM];

	// g1r = g1*r
	real_t g1R[HORIZON*RDIM];
	real_t R[HORIZON*RDIM];
	for(int i = 0; i < HORIZON*RDIM; i++)
		R[i] = r;
	dmul(g1, R, g1R, HORIZON*RDIM, HORIZON*YDIM, 1);

	// g0x0 = g0*x0
	real_t g0x0[HORIZON*RDIM];
	dmul(g0, x0, g0x0, HORIZON*RDIM, ADIM, 1);

	// Compute g = g0*x0 - g1*r
	real_t g[HORIZON*RDIM];
	for(int i = 0; i < HORIZON*RDIM; i++){
		g[i] = g0x0[i] - g1R[i];
	}

	// Solve first QP
	nWSR = 20;
	QProblem_init(&objective, H, g, Ay, ulb, uub, ylb, yub, &nWSR, 0); // This can not be repeated. Need to be called only ONCE!

	// Get the best_inputs
	QProblem_getPrimalSolution(&objective, best_inputs);

	// Print best_inputs
	printf("Best inputs that we can implement into the system = \n");
	for(int i = 0; i < HORIZON*RDIM; i++)
		printf("%f\n", best_inputs[i]);
	printf("\n\n");

	// Let say that you are going to do another QP
	// Update vector g = g0*x0 - g1*r where x0 is the state from kalman filter and r is our reference vector
	// Now solve again
	nWSR = 20;
	QProblem_hotstart(&objective, g, ulb, uub, ylb, yub, &nWSR, 0); // This can be repeated

	printf("Best inputs that we can implement into the system = \n");
		for(int i = 0; i < HORIZON*RDIM; i++)
			printf("%f\n", best_inputs[i]);

	return EXIT_SUCCESS;
}

Resultatet \(U\) är exakt samam i MATLAB och GNU Octave.

Kod: Markera allt

Nu använder vi \(U\) för att simulera. Det går fintrimma mer också om man vill.

Selección_004.png

Svenska ElektronikForumet

Kvadratisk programmering på en mikrokontroller - Optimering

Re: Kvadratisk programmering på en mikrokontroller - Optimer

Re: Kvadratisk programmering på en mikrokontroller - Optimer

Re: Kvadratisk programmering på en mikrokontroller - Optimer

Re: Kvadratisk programmering på en mikrokontroller - Optimer

Re: Kvadratisk programmering på en mikrokontroller - Optimer