NextCloud/dlib
1
0
Fork 0
mirror of https://github.com/davisking/dlib.git synced 2024-11-01 10:14:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

fleshed out this example with comments

Browse Source
This commit is contained in:
Davis King 2015-06-02 17:47:46 -04:00
parent c290381bf0
commit 1294d6d398
1 changed files with 86 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
examples/mpc_ex.cpp
View File

@ -1,10 +1,24 @@
// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
This is an example illustrating the use of the linear model predictive
control tool from the dlib C++ Library. To explain what it does, suppose
you have some process you want to control and the process dynamics are
described by the linear equation:
x_{i+1} = A*x_i + B*u_i + C
That is, the next state the system goes into is a linear function of its
current state (x_i) and the current control (u_i) plus some constant bias or
disturbance.
A model predictive controller can find the control (u) you should apply to
drive the state (x) to some reference value, which is what we show in this
example. In particular, we will simulate a simple vehicle moving around in
a planet's gravity. We will use MPC to get the vehicle to fly to and then
hover at a certain point in the air.
*/
#include <dlib/gui_widgets.h>
#include <dlib/control.h>
#include <dlib/image_transforms.h>
@ -17,61 +31,114 @@ using namespace dlib;
int main()
{
// state is x, y, x_vel, y_vel
matrix<double,4,4> A;
A = 1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1;
const int STATES = 4;
const int CONTROLS = 2;
matrix<double,4,2> B;
// The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
// Our state space (the x) will have 4 dimensions, the 2D vehicle position
// and also the 2D velocity. The control space (u) will be just 2 variables
// which encode the amount of force we apply to the vehicle along each axis.
// Therefore, the A matrix defines a simple constant velocity model.
matrix<double,STATES,STATES> A;
A = 1, 0, 1, 0, // next_pos = pos + velocity
0, 1, 0, 1, // next_pos = pos + velocity
0, 0, 1, 0, // next_velocity = velocity
0, 0, 0, 1; // next_velocity = velocity
// Here we say that the control variables effect only the velocity. That is,
// the control applies an acceleration to the vehicle.
matrix<double,STATES,CONTROLS> B;
B = 0, 0,
0, 0,
1, 0,
0, 1;
matrix<double,4,1> C;
// Let's also say there is a small constant acceleration in one direction.
// This is the force of gravity in our model.
matrix<double,STATES,1> C;
C = 0,
0,
0,
0.1;
matrix<double,4,1> Q;
const int HORIZON = 30;
// Now we need to setup some MPC specific parameters. To understand them,
// let's first talk about how MPC works. When the MPC tool finds the "best"
// control to apply it does it by simulating the process for HORIZON time
// steps and selecting the control that leads to the best performance over
// the next HORIZON steps.
//
// To be precise, each time you ask it for a control, it solves the
// following quadratic program:
//
// min sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i
// x_i,u_i
//
// such that: x_0 == current_state
// x_{i+1} == A*x_i + B*u_i + C
// lower <= u_i <= upper
// 0 <= i < HORIZON
//
// and reports u_0 as the control you should take given that you are currently
// in current_state. Q and R are user supplied matrices that define how we
// penalize variations away from the target state as well as how much we want
// to avoid generating large control signals. We also allow you to specify
// upper and lower bound constraints on the controls. The next few lines
// define these parameters for our simple example.
matrix<double,STATES,1> Q;
// Setup Q so that the MPC only cares about matching the target position and
// ignores the velocity.
Q = 1, 1, 0, 0;
matrix<double,2,1> R, lower, upper;
matrix<double,CONTROLS,1> R, lower, upper;
R = 1, 1;
lower = -0.5, -0.5;
upper = 0.5, 0.5;
mpc<4,2,30> controller(A,B,C,Q,R,lower,upper);
// Finally, create the MPC controller.
mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);
// Let's tell the controller to send our vehicle to a random location. It
// will try to find the controls that makes the vehicle just hover at this
// target position.
dlib::rand rnd;
matrix<double,4,1> target;
matrix<double,STATES,1> target;
target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
controller.set_target(target);
matrix<double,4,1> current_state;
current_state = 200,200,0,0;
// Now let's start simulating our vehicle. Our vehicle moves around inside
// a 400x400 unit sized world.
matrix<rgb_pixel> world(400,400);
image_window win;
matrix<double,STATES,1> current_state;
// And we start it at the center of the world with zero velocity.
current_state = 200,200,0,0;
int iter = 0;
while(!win.is_closed())
{
matrix<double,2,1> action = controller(current_state);
// Find the best control action given our current state.
matrix<double,CONTROLS,1> action = controller(current_state);
cout << "best control: " << trans(action);
// Now draw our vehicle on the world. We will draw the vehicle as a
// black circle and its target position as a green circle.
assign_all_pixels(world, rgb_pixel(255,255,255));
const dpoint pos = point(current_state(0),current_state(1));
const dpoint goal = point(target(0),target(1));
draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
draw_solid_circle(world, pos, 7, 0);
// We will also draw the control as a line showing which direction the
// vehicle's thruster is firing.
draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
current_state = A*current_state + B*action + C;
win.set_image(world);
// Take a step in the simulation
current_state = A*current_state + B*action + C;
dlib::sleep(100);
// Every 100 iterations change the target to some other random location.