MPC模型预测控制（三）-FAST_MPC MATLAB代码实现

最新推荐文章于 2024-09-23 16:15:34 发布

tingfenghanlei

最新推荐文章于 2024-09-23 16:15:34 发布

阅读量1.3w

点赞数 5

分类专栏：自动驾驶文章标签： fast_mpc matlab

自动驾驶专栏收录该内容

7 篇文章 19 订阅

订阅专栏

fast_mpc: code for fast model predictive control

Version Alpha (Sep 2008)
Yang Wang and Stephen Boyd

Purpose

fast_mpc contains two C functions, with MATLAB mex interface, that implement the fast model predictive control methods described in the paper Fast Model Predictive Control Using Online Optimization. See this paper for the precise problem formulation and meanings of the algorithm parameters.

Download and install

Get and unpack the package files from either of

This will create a directory that contains all source, as well as this documentation.

See the file INSTALL for installation instructions.

What fast_mpc does

We consider the control of a time-invariant linear dynamical system

x(t+1) = Ax(t)+Bu(t)+w(t),quad t=0,1,ldots,

where x(t) , u(t) , and w(t) are the state, input, and disturbance at time , and and are the dynamics and input matrices.

In model predictive control (MPC), at each time we solve the QP

$begin{array}{ll} mbox{minimize} & x(t+T)^TQ_fx(t+T) +displaystylesum_{tau = 0}^{t+T-1} x(tau)^TQx(tau)+u(tau)^TRu(tau) mbox{subject to} & x_{mbox{scriptsize min}} leq x(tau) leq x_{mbox{scriptsize max}}, quad tau = t+1,ldots,t+T, & u_{mbox{scriptsize min}} leq u(tau) leq u_{mbox{scriptsize max}}, quad tau= t,ldots,t+T-1, & x(tau+1) = Ax(tau) + Bu(tau),quad tau = t,ldots,t+T-1, end{array}$

with variables

x(t+1),ldots,x(t+T),quad u(t),ldots,u(t+T-1),

and data

$x(t),A,B,Q,Q_f,R,x_{mbox{scriptsize min}},x_{mbox{scriptsize max}}, u_{mbox{scriptsize min}},u_{mbox{scriptsize max}},T.$

The MPC input is u^star(t) . We repeat this at the next time step.

fast_mpc is a software package for solving this optimization problem fast by exploiting its special structure, and by solving the problem approximately. The function fmpc_step solves the problem above, starting from a given initial state and input trajectory. The function fmpc_sim carries out a full MPC simulation of a dynamical system with MPC control.

Using fmpc_step

The function fmpc_step solves the above optimization problem and returns the approximately optimal and trajectories. (In this case, you must implement the MPC control loop yourself.) The calling procedure is as follows.

[X,U,telapsed] = fmpc_step(sys,params,X0,U0,x0);

Inputs

System description (sys structure):

    sys.A       :   dynamics matrix A
    sys.B       :   input matrix B
    sys.Q       :   state cost matrix Q
    sys.R       :   input cost matrix R
    sys.xmax    :   state upper limits x_{max}
    sys.xmin    :   state lower limits x_{min}
    sys.umax    :   input upper limits u_{max}
    sys.umin    :   input lower limits u_{min}
    sys.n       :   number of states
    sys.m       :   number of inputs

MPC parameters (params structure):

    params.T        :   MPC horizon T
    params.Qf       :   MPC final cost Q_f
    params.kappa    :   Barrier parameter
    params.niters   :   number of newton iterations
    params.quiet    :   no output to display if true

Other inputs
    X0   :   warm start X trajectory (n by T matrix)
    U0   :   warm start U trajectory (m by T matrix)
    x0   :   initial state

The inputs X0 and U0 need not satisfy the constraints; they are first
projected into the bounding box before the fast algorithm is applied.

Outputs

X        :  optimal X trajectory (n by T matrix)
U        :  optimal U trajectory (m by T matrix)
telapsed :  time taken to solve the problem

Using fmpc_sim

The function fmpc_sim handles the entire MPC simulation. For $t=1,ldots, n_mathrm{steps}$ , fmpc_sim solves the above optimization problem, then applies the MPC input and updates the state according to the dynamics equations. The state and control trajectories are initialized with those from the previous step, shifted in time, and appending hat u and hat x , where

hat u = Kx(T) , qquad hat x = Ax(T)+BKx(T).

(As with fmpc_step, these trajectories are then projected into the constraint box.) Here is the terminal control gain,

$K = -(R+B^TQ_fB)^{-1}B^TQ_fA.$

The calling procedure is

[Xhist,Uhist,telapsed] = fmpc_sim(sys,params,X0,U0,x0,w);

Inputs

System description (sys structure):

    sys.A       :   dynamics matrix A
    sys.B       :   input matrix B
    sys.Q       :   state cost matrix Q
    sys.R       :   input cost matrix R
    sys.xmax    :   state upper limits x_{max}
    sys.xmin    :   state lower limits x_{min}
    sys.umax    :   input upper limits u_{max}
    sys.umin    :   input lower limits u_{min}
    sys.n       :   number of states
    sys.m       :   number of inputs

MPC parameters (params structure):

    params.T        :   MPC horizon T
    params.Qf       :   MPC final cost Q_f
    params.kappa    :   Barrier parameter
    params.niters   :   number of newton iterations
    params.quiet    :   no output to display if true
    params.nsteps   :   number of steps to run the MPC simulation

Other inputs
    X0   :   warm start X trajectory (n by T matrix)
    U0   :   warm start U trajectory (m by T matrix)
    x0   :   initial state
    w    :   disturbance trajectory (n by nsteps matrix)

The inputs X0 and U0 need not satisfy the constraints; they are first
projected into the bounding box before the fast algorithm is applied.

Outputs

Xhist        :  state history (n by nsteps matrix)
Uhist        :  input history (m by nsteps matrix)
telapsed     :  time taken to solve the problem

Examples

We have provided two examples that illustrate usage:

masses_example.m uses fmpc_step to control a system of oscillating masses.
randsys_example.m uses fmpc_sim to simulate MPC on a randomly generated system.

Feedback

Please report any bugs to Yang Wang <yw224@stanford.edu>.

License

fast_mpc is under Apache license, version 2.0, January 2004.

安装fast_mpc

Installation instructions for fast_mpc
-----------------------------------------------------------------------

fast_mpc is written in C with a mex interface to MATLAB.  Before 
installation, make sure that BLAS and LAPACK libraries are installed
in your machine. 


1. Unpack the fast_mpc files, or get them individually.

2. Start MATLAB and cd to the directory containing the source files.

3. At the MATLAB command prompt type

        >> mex -setup

   and enter the number corresponding to the template option gccopts.sh.

4. Compile fmpc_sim.c using

        >> mex fmpc_sim.c

   On some machines you may need to specify the libraries

        >> mex fmpc_sim.c -lblas -llapack
    
   If you want to link your own libraries, you can. For example,
   to link the libraries libacml.a, libacml_mv.a, and libgfortran.a 
   in the directory /opt/acml/lib, use the option '-L' to specify the 
   library search path and the option '-l' to specify the individual 
   libraries, as in

       >> mex fmpc_sim.c -L/opt/acml/lib -lacml -lacml_mv -lgfortran

5. If fmpc_sim.c compiles successfully, do the same with fmpc_step.c.

6. Test the installation by running the masses_example script

       >> masses_example

   which will run MPC, for 100 iterations, on the masses example in the paper,
   using mpc_step.  You should see reports from each iteration printed to your 
   screen. The last one, for example, should look like

 iteration       step            rd                      rp
    0            0.3874          1.09294e+01             1.27108e+00
    1            1.0000          1.54052e+00             1.39748e-14
    2            1.0000          2.98332e-01             1.40216e-15
    3            1.0000          1.16741e-02             1.02482e-15

The average cost is: 5.683200

   Test mpc_sim by running the script randsys_example.

原链接：https://web.stanford.edu/~boyd/fast_mpc/