1 // The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
2 /*
3
4 This is an example illustrating the use of the linear model predictive
5 control tool from the dlib C++ Library. To explain what it does, suppose
6 you have some process you want to control and the process dynamics are
7 described by the linear equation:
8 x_{i+1} = A*x_i + B*u_i + C
9 That is, the next state the system goes into is a linear function of its
10 current state (x_i) and the current control (u_i) plus some constant bias or
11 disturbance.
12
13 A model predictive controller can find the control (u) you should apply to
14 drive the state (x) to some reference value, which is what we show in this
15 example. In particular, we will simulate a simple vehicle moving around in
16 a planet's gravity. We will use MPC to get the vehicle to fly to and then
17 hover at a certain point in the air.
18
19 */
20
21
22 #include <dlib/gui_widgets.h>
23 #include <dlib/control.h>
24 #include <dlib/image_transforms.h>
25
26
27 using namespace std;
28 using namespace dlib;
29
30 // ----------------------------------------------------------------------------
31
main()32 int main()
33 {
34 const int STATES = 4;
35 const int CONTROLS = 2;
36
37 // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
38 // Our state space (the x) will have 4 dimensions, the 2D vehicle position
39 // and also the 2D velocity. The control space (u) will be just 2 variables
40 // which encode the amount of force we apply to the vehicle along each axis.
41 // Therefore, the A matrix defines a simple constant velocity model.
42 matrix<double,STATES,STATES> A;
43 A = 1, 0, 1, 0, // next_pos = pos + velocity
44 0, 1, 0, 1, // next_pos = pos + velocity
45 0, 0, 1, 0, // next_velocity = velocity
46 0, 0, 0, 1; // next_velocity = velocity
47
48 // Here we say that the control variables effect only the velocity. That is,
49 // the control applies an acceleration to the vehicle.
50 matrix<double,STATES,CONTROLS> B;
51 B = 0, 0,
52 0, 0,
53 1, 0,
54 0, 1;
55
56 // Let's also say there is a small constant acceleration in one direction.
57 // This is the force of gravity in our model.
58 matrix<double,STATES,1> C;
59 C = 0,
60 0,
61 0,
62 0.1;
63
64
65 const int HORIZON = 30;
66 // Now we need to setup some MPC specific parameters. To understand them,
67 // let's first talk about how MPC works. When the MPC tool finds the "best"
68 // control to apply it does it by simulating the process for HORIZON time
69 // steps and selecting the control that leads to the best performance over
70 // the next HORIZON steps.
71 //
72 // To be precise, each time you ask it for a control, it solves the
73 // following quadratic program:
74 //
75 // min sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i
76 // x_i,u_i
77 //
78 // such that: x_0 == current_state
79 // x_{i+1} == A*x_i + B*u_i + C
80 // lower <= u_i <= upper
81 // 0 <= i < HORIZON
82 //
83 // and reports u_0 as the control you should take given that you are currently
84 // in current_state. Q and R are user supplied matrices that define how we
85 // penalize variations away from the target state as well as how much we want
86 // to avoid generating large control signals. We also allow you to specify
87 // upper and lower bound constraints on the controls. The next few lines
88 // define these parameters for our simple example.
89
90 matrix<double,STATES,1> Q;
91 // Setup Q so that the MPC only cares about matching the target position and
92 // ignores the velocity.
93 Q = 1, 1, 0, 0;
94
95 matrix<double,CONTROLS,1> R, lower, upper;
96 R = 1, 1;
97 lower = -0.5, -0.5;
98 upper = 0.5, 0.5;
99
100 // Finally, create the MPC controller.
101 mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);
102
103
104 // Let's tell the controller to send our vehicle to a random location. It
105 // will try to find the controls that makes the vehicle just hover at this
106 // target position.
107 dlib::rand rnd;
108 matrix<double,STATES,1> target;
109 target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
110 controller.set_target(target);
111
112
113 // Now let's start simulating our vehicle. Our vehicle moves around inside
114 // a 400x400 unit sized world.
115 matrix<rgb_pixel> world(400,400);
116 image_window win;
117 matrix<double,STATES,1> current_state;
118 // And we start it at the center of the world with zero velocity.
119 current_state = 200,200,0,0;
120
121 int iter = 0;
122 while(!win.is_closed())
123 {
124 // Find the best control action given our current state.
125 matrix<double,CONTROLS,1> action = controller(current_state);
126 cout << "best control: " << trans(action);
127
128 // Now draw our vehicle on the world. We will draw the vehicle as a
129 // black circle and its target position as a green circle.
130 assign_all_pixels(world, rgb_pixel(255,255,255));
131 const dpoint pos = point(current_state(0),current_state(1));
132 const dpoint goal = point(target(0),target(1));
133 draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
134 draw_solid_circle(world, pos, 7, 0);
135 // We will also draw the control as a line showing which direction the
136 // vehicle's thruster is firing.
137 draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
138 win.set_image(world);
139
140 // Take a step in the simulation
141 current_state = A*current_state + B*action + C;
142 dlib::sleep(100);
143
144 // Every 100 iterations change the target to some other random location.
145 ++iter;
146 if (iter > 100)
147 {
148 iter = 0;
149 target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
150 controller.set_target(target);
151 }
152 }
153 }
154
155 // ----------------------------------------------------------------------------
156
157