Application of Model Predictive Control in Path Planning

1. Model Predictive Control

The mechanism of MPC can be expressed as: at each sampling moment, according to the current measurement information, a finite-time open-loop optimization problem is solved online, and the first element of the obtained control sequence is used for the controlled object; in the next sampling At every moment, use the new measured value as the initial test condition to predict the future dynamics of the system at this time, and refresh the optimization problem to solve. Typical model predictive control methods applied to robots:

• Problem model

• Parameter space
  
  The purpose of the above problem is to find the optimal $u$ minimizes the above formula. In engineering problems, the system input is a random variable, so it is necessary to convert the infinite dimensional system input into finite dimensional parameters. Commonly used methods are: Zero order hold (discrete method); polynomial; B-spline; numerical mapping
• Optimization
  conventional optimization method comprising: searching, and non-convex optimization convex optimization; herein, using Linear Quadratic Programming system mainly Convex Optimization (Quadratic programming) Solving; Nonlinear system is mainly used PSO (Particle swarm optimization) Solution .
• Control The
  
  traditional MPC control block diagram is: (1) Set the optimization problem (2) Use the measurement module to tell us the current initial
  state (3) Solve the optimization problem to obtain the parameters, these parameters constitute the optimal input of the system $u^{\ast }$ (4) Use$u^{\ast }$ Drive system, because the system is disturbed, it is impossible to guarantee the$u^{\ast }$ is what we want. It is only intended to be used in a short period of time, and then use the observed state to solve the problem again, and go back to step (2)

In UAV applications, the use of traditional MPC requires higher control frequency and requires faster solution speed. Considering the actual computing resources of UAVs, its use is limited. Therefore, Tube based MPC is proposed. The control block diagram is as follows:

The above framework is divided into two parts: Nominal System and Real System. The Nominal System part does not consider uncertainty, and solves the optimization problem to obtain the reference signal $x^{\ast }$ , the purpose of the Real System part of the system is to make the real state of the system and$x^{\ast \; To\; be}$ consistent, the purpose of Associate Controller is to stabilize the system so that it can track the system by dealing with model uncertainty and external interference.

There is Matlab MPC toolbox toolkit in Matlan, which can generate C/C++ code with one click.

2. Linear MPC

Take a simple model as an example. First, the continuous model is discretized into a discrete model through time:

the discrete model is transformed into a matrix form, that is, the predictive model is constructed:

the problem model is constructed according to the problem to be solved, assuming there are two goals, the goal 1 to the position, velocity, acceleration back to the origin, the target 2 is a system trajectory as smooth as possible, indicator function may be defined as follows:

the overall objective function can be constructed as follows:

the results are substituted into the prediction model, we have:

the problem can be two The sub-programming operator (such as quadprog in matlab) is solved, and the above equation is expressed as follows: the

solution result is $J=-H^{-1}F$

Hard constraints of linear MPC

Add a simple hard constraint of speed and acceleration, first rewrite it into a matrix form, and

substitute it into the aforementioned predictive model, which will be related to $v,a$ inequality is modified on$J$ 's inequality, and then modified to inequality form (required when the toolbox solves):

soft constraints

Generally, the constraint is directly added to the objective function, as follows:

However, the modified objective function above is not diversified and cannot be solved by quadratic programming. Most MPC operators are quadratic programming operators, in order to solve the above problems , Can be achieved by modifying the previous hard constraints, as an example:
$-T_{v}J<=1_{20x1}+B_v$
First add slack variable:

modify the objective function as:

LLThe weight of$L$$w_5$Generally take a larger value

3. Nonlinear MPC

The JLT
Jerk limitied trajectory (JLT) algorithm is a solution to the BVP problem. This problem mainly recurs the third-order integrator model, so as to obtain the trajectory from the initial state to the target state smoothly, including the position pp at each moment of the process.$p$ , speed$v$ , acceleration$a$和jerk $u_j$。

First consider the case of the second-order integrator (given speed):

The solution of this problem is essentially a process of classification and discussion. The value of jerk is generally three, unchanged, a certain positive value or a certain negative value. The resulting acceleration curve that meets the boundary conditions is a triangle, then a triangle The lower area is the speed. According to the boundary conditions of the speed, there are the following situations:

• One is that the acceleration curve is triangular and reaches the maximum value, as shown in the uppermost subgraph in the above figure. At this time, the time from the initial state to the maximum acceleration $△t_1$And the time from the maximum acceleration to 0 $△t_3$Can be solved directly;
• The second is that the speed exceeds the boundary conditions. At this time, the maximum acceleration (that is, the vertex of the triangle) needs to be lowered, at this time $△t_1$And $△t_3$It can also be solved directly
• The third is that the speed is not high enough, so the vertex needs to be adjusted upward, but due to the boundary conditions of acceleration, it can only be transformed into a trapezoid (that is, after reaching the maximum acceleration for a period of time $△t_2$)solve.

JLT itself is a motion planning problem (given position) corresponding to the mathematical model of the third-order integrator. The problem is expressed as follows:

This problem is similar to the problem of a given speed, and it can also be solved through classification discussion, but JLT needs to be at a given position The solution time is from 3 segments $△t_1$、$△t_2$、$△t_3$Expanded to 7 segments, as shown in the figure below:

As can be seen from the figure, the rising phase of the triangle-like corresponds to the time period $△t_1$、$△t_2$、$△t_3$; The cruise phase corresponds to $△t_4$; The descending phase corresponds to $△t_5$、$△t_6$、$△t_7$; Before determining these time parameters, it is necessary to search for the end speed of the ascent phase through parameters such as position setting value, initial position, initial speed, and initial acceleration.

The combination of JLT and nonlinear MPC
mainly solves the problem of local path planning. The steps are as follows:

• First set up the map as a cost map for the robot to observe:

In the cost map, the closer the point on the map is to the obstacle, the higher the cost;

• Then carry out global planning, which can be implemented using A* or JPS algorithm;
• Generate a dynamic trajectory and then evaluate;

The combination of JLT and nonlinear MPC is mainly used to solve the optimal trajectory. A typical MPC optimization problem is as follows:

There are 4 constraints in the model, among which the first three are satisfied in the construction of JLT, so the above problem can be transformed into :

Obstacle constraints have two expressions : hard constraints and soft constraints. For soft constraints, we can directly add them to the objective function to be optimized: the

above problem is transformed into an unconstrained optimization problem, where $C_R$And $C_F$The first item is used to control the distance of the path to the target point, $C_R$The second term is used to make the trajectory smoother, the third term of the two is used to avoid obstacles. Because of the obstacle constraint added to the above problem, the problem cannot be directly derivated, and it cannot be solved directly by gradient. One way to solve the problem is to use PSO (particle swarm optimization) to solve the problem. The algorithm steps are as follows: The

specific idea of ​​the algorithm is to initialize a group of random particles (random solution), and then find the optimal solution through iteration. In each iteration, the particles track two Extreme value (global optimal solution and historical optimal solution) to update its speed and position.

In addition, it can also be combined with neural networks to solve problems. For details, see the paper Model Predictive Local Motion Planning With Boundary State Constrained Primitives.