Realtime Application of Constrained Predictive Control for Mobile Robot Navigation

This work addresses the implementation issue of constrained Model Predictive Control (MPC) for the autonomous trajectory-tracking problem. The chosen process to control is a Wheeled Mobile Robot (WMR) described by a discrete, Multiple Input Multiple Output (MIMO), state-space and linear parameter varying kinematic model. The main motivation of the constrained MPC usage in this case relies on its ability in considering, in a straightforward way, control and states constraints that naturally arise in trajectory tracking practical problems. The efficiency of the presented control scheme is validated through experimental results on a two wheeled mobile robot using both STM32F429II and STM32F407ZG microcontrollers. The controller implementation is facilitated by the usage of the automatic C code generation and interesting optimization before real-time execution. Based on the experimental results obtained, the good performance and robustness of the proposed control scheme are established. Keywords—Embedded C; STM32; microcontrollers; constrained model predictive control; otpimization


I. INTRODUCTION
The gap between theory and practice has been discussed for many years across the spectrum of academia and industry [1].One of the reasons that explains this gap is that control systems for the first time is often found too abstract and theoretical in nature; that is, too many mathematical equations as well as block diagrams.Furthermore, the use of control algorithms simulation only will not provide sufficient illustration of the real physical application of using control theory in solving engineering problems.The above shortcomings can be avoided throughout control system experimentation which is an important way to go through practical applications of control theory so as to overcome the above-mentioned difficulty.Hence real hands-on experiments or design problems are an alternative way of augmenting the conventional way of dealing with control theory, as it can be related to real engineering applications, such as modeling, controller design, and implementation.Moreover, although some control algorithms has been found to be quite a robust type of control in most reported applications [2], [19], their implementation on low-cost system on chip solutions (such as microcontrollers) has been historically hindered by many restrictions and constraints [11]- [18].Among these constraints are mathematical complexities, which are not a problem in general for the research control community but represent a drawback for the use in practice.The time-to-market delays and possible design errors if the algorithm is manually written in embedded C language on one side and the high computing and associated memory demands of the algorithm on the other side are restricting the implementation of these control algorithms.
In fact, modern applications generally involve many computationally demanding iterations and have strong requirements on resource optimization.Therefore, the main drawback of complex control algorithms such as Model Predictive Control (MPC), is the need to solve mathematical program on line to compute the control action [3].This computation prevents the application of MPC in several contexts, mainly because the computer technology needed to solve the optimization problem within the sampling time which is too expensive or sometime infeasible [5].In fact, the resource constraints associated with embedded systems, combined with non-optimized software components used for their implementation, introduce nondeterminism in the real-time system.For control systems this is of particular concern, since timing variations induced by the implementation degrade the control performance.
To cope with the above mentioned shortcoming and problems, the goal of this work is the implementation of constrained MPC algorithms on a fast system with no manual embedded C coding effort.The trade-offs between data size and computation speed, versus numerical precision and effectiveness of the computed control action is also focused on.By using MPC it follows that the tuning parameters are directly related to a cost function which is minimized in order to obtain an optimal control sequence; constraints on state, control inputs and control inputs deviation can be considered in a straightforward way [3]- [5], [7].This objective is challenging especially in case of system with fast dynamics [6], system described with Multiple Input Multiple Output (MIMO) and parameter varying model.In this direction, the proposed control system is the nonholonomic two Wheeled Mobile Robot (WMR).The usage of the WMR as a plant is motivated by the following reasons: 1) The selected kinematic model of the WMR belongs to the class of MIMO, non-square, linear parameter varying which is challenging for control; 2) despite the apparent simplicity of the kinematic model of a WMR, the existence of nonholonomic constraints turns the design of stabilizing control laws for those systems in a considerable challenge [21]; 3) in recent years, autonomous mobile robots are finding widespread application in many areas and are at the heart of most modern control systems [22].
As a result, validating the proposed control framework in such a challenging systems will expand the number of addressable real time control applications.
On other hand, a general control system of two-wheeled robot usually has a complex structure due to plenty of sensors, which price is generally expensive.A control system with DC motor driver is proposed instead, using low cost STM32 Discovery kits.These kits allow the controller implementation on high-performance MCUs with ARM Cortex-M4 core.Experimental results on a real WMR exhibit that the proposed control scheme yields some interesting results on autonomous trajectory tracking problem.In fact, compared to existing works such as [9] and [10], the proposed automatic C code generation saves time, avoids possible design errors with no C code manual coding effort.With regards to [3], the proposed implementation framework yields much more interesting results in term of execution speed.
The outline of this paper is as follows.In Section II, the kinematic robot model as well as the proposed MPC algorithm are presented.Implementation results demonstrating the good performance and robustness of the proposed controller are presented in Section III.Section IV concludes the paper.

II. MODEL PREDICTIVE CONTROL ALGORITHM
The basic idea of predictive control consists in calculating, at each sampling instant, a control sequence on a prediction horizon aimed at minimizing a quadratic cost function.The control algorithm is based on the following: (i) The use of a model to predict, on a future horizon, the output of the process, (ii) Computing the control sequence which minimizes a performance criterion which involves a sequence of the predicted output.
The following section states the robot model presentation followed by the proposed MPC algorithm investigation.

A. Robot model presentation
We consider a WMR made up by a rigid body and nondeforming wheels (Fig. 1).It is assumed that the vehicle moves without slipping on a plane, i.e., there is a pure rolling contact between the wheels and the ground [21], [22].Referring to [21], we can write the kinematic model of the WMR as in the following system: or else, in a more simplified form: where x describes the configuration (position and orientation) of the wheels axis's center, C, with respect to a global inertial frame {O, X, Y}. u is the control input vector, where v and w are the linear and the angular velocities, respectively.Now, considering a sampling period T s , by applying the Euler's approximation to (1), one can obtain the following discrete-time model for the robot motion: or, in a compact representation, On the other hand, the problem of trajectory tracking can be stated as to find a control law such that: Where, x r = [x r y r θ r ] T is a known, pre-specified reference trajectory.It is usual in this case to associate to this reference trajectory a virtual reference robot, which has the same model than the robot to be controlled.A linearized discrete time model of the system, described by (6), is obtained by computing an error model with respect to a reference car.This is ensured by expanding the right side of (3) in Taylor series around the point (x r , u r,) and using forward differences. with is the (3×3) identity matrix.x = x − x r represents the error with respect to the reference car, ũ = u−u r is its associated error control input with u r = [v r w r ] T and ỹ = y−y r denotes the system output.It is easy to see that when the robot is not moving (i.e., v r = 0), the linearization around a stationary operating point is not controllable.However, this linearization becomes controllable as long as the control input u is not zero [21].

B. MPC Direct Output Method
In this paper we consider the Model Predictive Control algorithm, where the objective function is rewritten as standard quadratic form.At that point, we target to solve the Quadratic Problem (QP) in using interior-point method, widely used in applications, to solve problems iteratively such that all iterates satisfy the inequality constraints strictly.By extending the output direct method equations provided in [20] to MIMO model process and if we consider (6) for the discrete state space model, the following output at time (k + j) is obtained: The MPC based on a state space model aims to minimize the quadratic criterion given by: Here, H p is the prediction horizon, H c is the control horizon, λ 1 = [λ 1s ] s=1:2 = [ λ 11 00 λ 12 ] is the input weighting matrix with (m×m) dimension.
The output error weighting matrix with (n×n) dimension is given by: It is assumed that there is no control action after time (k+H c -1), i.e. u(k+i) = 0 for i > (H c −1).
Since the MPC is a receding horizon approach, only the first computed control input u is implemented.
In matrix presentation, the objective function can be expressed as: As in [20], the output sequence on H p can be written as follows: where: The L matrix with the (nH p ×mH c ) dimension and M which is an (nH p ×n) dimensional matrix are given by: One can rewrite the objective function in a standard quadratic form: where: In this way, the constrained MPC problem is formulated as a compact Quadratic Problem (QP) described by (12): G is (mH c × mH c ) matrix, g is (mH c × 1) vector, F is (q × q) matrix where q is the number of inequality constraints, q is equal to (6mH c ) in our case, and b is a (6mH c × 1) vector.
The QP problem is subject to linear inequality constraints on the system inputs, system inputs deviation (∆u = u(k)-u(k-1)) and system outputs as follows: www.ijacsa.thesai.org in which: Replacing Y in ( 14) by its value in equation (10), and by considering ∆U value at each iteration (k), we obtain: As a consequence, the following inequalities will be easily obtained: The operator ( + ) denotes the Moore-Penrose pseudoinverse operator.Hence, we obtain: with I c ∈ R mHc×mHc is the identity matrix.

C. Quadratic Problem: Interior Point Method
To set up the equations enabling the interior-point method design, we use the general theory on constrained optimization by defining a Lagrangian function and setting up Karush Kuhn-Tucker (KKT) conditions for the QP's that we wish to solve.The KKT-conditions (21) are conditions that must be satisfied for a vector U to be a solution of a given QP.
with λ li is Langrange multiplier of the i th inequality constraints, s i is the i th element of the slack vector s satisfying: The full algorithm is stated in Fig. 2.
The algorithm requires a starting point (x 0 , λ l0 , s 0 ) which does not need to be in the feasible region.In fact, this is accomplished by requiring that (λ l0 , s 0 ) > 0 and having the right hand side of (23) containing the residual vectors r d , r p and r sλ instead of zeros to prevent infeasibility [23].
Fundamental concepts related to this method such as central path, the stopping criteria, the predictor complementary measure for the computed Newton step µ af f , the search direction (∆x, ∆λ l , ∆s), the complementary measure µ and the centering parameter σ are detailed in [23].In the algorithm, the matrix S = diag(S 1 ,S 2 ,..S m ) in which S i are the slack vectors, Λ l = diag(λ l1 , λ l2 , ..., λ lm ) is a diagonal matrix with the elements of the Lagrange multiplier on the diagonal, µ denotes the complementary measure, (∆x af f , ∆λ af f , ∆s af f ) is the affine scaling direction, λ af f is the step length and e = (1, 1, ...1) T is a (m × 1) vector containing ones.
For implementation, v r is measured 0.2 m/s.Since we are dealing with nonhomolonic WMR, the saturation of the input control u should be considered.The maximum allowed values are v max = 0.4 m/s and w max = 5 rad/s while the minimum ones are v min = 0 and w min = −5 rad/s.

Off line treatment
Input (x 0 , λ l0 , s 0 ) and G, F, g, b Compute residuals and complementarity measure Online treatment While loop : terminate if stopping criteria (*) are satisfied Predictor step: Solve ( 23) to obtain an affine scaling direction (∆x af f , ∆λ af f , ∆s af f ) for setting up the right hand side for the corrector step and to obtain a good value of the centering parameter σ: Compute α af f Corrector and centering step: Solve ( 24) to obtain search direction Compute α λ l + α∆λ l ≥ 0 s + α∆s ≥ 0 Update (x, λ l , s) Update residuals and complementarity measure End while loop

III. IMPLEMENTATION RESULTS
In order to demonstrate the effectiveness of the proposed controller scheme, the following section presents the MPC controller implementation steps on two wheeled mobile robot.

A. Implementation Software and Hardware Environment
The considered hardware environment is composed by: * Two wheeled mobile robot with 80 RP M as no-load rotating speed at 6 V , wheel diameter is equal to 65 mm and with width and weight respectively equal to 10.2 mm and 305 g.  * STM32F4DISCOVERY and STM32F429DISCO are low cost kits based on STM32F407 and STM32F429 MCUs respectively and designed to help design engineers developing their applications easily.STM32F429DISCO integrates the gyroscope L3GD20 which is a low-power three-axis angular rate sensor includes a sensing element capable of providing the measured angular velocity w =[w x ,w y ,w z ] to the external world through a digital interface Serial Peripheral Interface (SPI) in our cases.For efficient control, the angular velocity is generally coupled with the linear velocity for the WMR position calculation.That's why, the STM32F4DISCOVERY, embedding an ST MEMS accelerometer, is also considered.* Embedded MATLAB Coder works with Real-Time Workshop to convert code from a dynamically typed language (MATLAB) to a statically typed language (C).

B. Implementation Framework
Based on the measured angular velocity, the robot angular position output θ is calculated based on the following formula: with θ 0 is WMR angular position at k =0.
In order to measure x abscissa and y ordinate, the gyroscope sensor is combined with an accelerometer MEMS sensor in the STM32F407 Discovery kit aiming to provide the robot acceleration a(k).The integration of the acceleration provides the linear velocity v(k).
in which v 0 is WMR linear velocity at k =0.Robot positions (x and y) are then calculated based on the following equations with x 0 and y 0 are respectively WMR abscissa and ordinate at k =0.
These equations allow the robot positions estimation from the real measurements obtained from sensors.However, it's known that accelerometers have an unwanted phenomenon called drift caused by a small DC bias in the acceleration signal.The presence of drift can lead to large integration errors.If the acceleration signal from a real accelerometer was integrated without any filtering performed, the output could become unbounded over time.That's why to solve the problem of drift, a pass-band filter is used to remove the DC component of the acceleration signal.The frequency response of the filter must have a very low and very high cutoff frequencies compared to the bandwidth of the signal.By filtering before integrating, drift errors are eliminated.The block diagram of the closed loop system is illustrated by Fig. 4.  Steps executed during Systick interrupt handler should not exceed the sampling time T s =0.01052 s.
Hint: knowing that w max and v max parameters are subject of change according to battery depletion over time, terrain type,... we propose in this paper a real-time measurement of w max parameter before trajectory tracking.This is done by turning the robot around (OZ) axis with maximum allowed speed and measuring the angular velocity which corresponds to w max .v max is then deduced given that a linear velocity is the product of the WMR radius r and angular velocity.

Gyroscope calibration
Reference trajectory plot on LCD W max and V max deduction Systick interrupt enable every T s Real Time plot of robot position on (OXY) plan

C. Automatic Code Generation for the Implementation
For code generation, Embedded Simulink Coder embeds many configuration options and advanced optimization for fine-grain control of the generated code' functions based on the processor architecture.These options allow control function boundaries, preserve expressions and apply optimization on multiple blocks to further reduce code size.The MDK-ARM tool integrates an ARM C compiler including a number of compiler optimization allowing code generation based on chosen microcontroller device and application area.The maximum level of performance optimization is chosen.Steps for automatic C code generation are detailed in [24].

D. Implementation Footprint and Execution Speed Optimization
Control algorithms implementation on system on chip solutions has been historically hindered by the high computing and associated memory demands constraints.To overcome this problem, we propose some optimization hints detailed on [24] which results in a faster and more efficient systemdevelopment work-flow.MDK-ARM toolchain is used as development and debugging environment, Among these optimization hints, the usage of the simple precision floating type is particularly relevant.In fact, considering the hardware Floating Point Unit (FPU) of the STM32F429 device rather than the software double precision types (generated by default by MATLAB Embedded Coder) is an optimization hint for performance increase.In fact a simple addition of two variables, declared as double, requires 82 CPU cycles due to double software library call whereas in simple precision case, it requires only 2 CPU cycles to be executed thanks to the hardware FPU usage.To avoid the unnecessary type conversion or confusion, the letter "f" is assigned following the numeric value.
Results provided in Table 1 focus on execution speed results found after applying code optimization techniques on inputs constraints only and in the case of constraints on inputs and outputs.From Table 1, it is deduced that generated code is subject to very interesting execution time optimization.In fact, applying inputs, inputs deviation and outputs constrains does not exceed the half of the sampling period (10 ms) which reflects the effectiveness of the proposed control scheme.

E. Implementation Results: Inputs Constraints
The robot trajectory is displayed using both LCD screen of the STM32 Discovery Kit and the STMStudio tool.This software tool is non-intrusive to the application code and used to monitor STM32 applications while they are running by reading and displaying their variables in real-time.
The maximum allowed linear velocity v max is identified as 0.4 m/s and its minimal value v min = 0 m/s.The angular velocities w min and w max are ±5 rad/s.The linear input deviation ∆v max and ∆v min are measured and fixed to ±0.25 m/s.Similarly, ∆w max and ∆w min are branded to ±1 rad/s.Fig. 7 shows that the robot tracks successfully the reference path.This is confirmed by the robot abscissa, ordinate and orientation errors with regards to the reference path presented in Fig. 8, 9 and 10.

IV. CONCLUSION
In this manuscript, Model Predictive Control implementation was focused on taking into account control inputs, inputs deviation and outputs constraints.The proposed control scheme exhibited suitable global performance when applied to wheeled mobile robots (WMR) for trajectory tracking problem.
Implementation results highlight the efficiency of the proposed design method.A framework for embedding MPC controller on a high performed STM32F429 microcontroller has been used.Optimization techniques have been applied to the generated code.Hence, an efficient implementation of the proposed control method yields a low computational burden with a high execution speed.Indeed, based on the experimental results, we noticed that the proposed method control successfully the process with a good set-point tracking.
In forthcoming article, we will focus on performance analysis comparison between the constrained MPC method proposed in this paper and a MIMO adaptive Proportional Integral Derivative (PID) regulator [8] for the trajectory tracking problem.
is the set-point and ω s (k + j) denotes the set-point at time(k + j), u = [u s ]T s=1:m and y = [y s ] T s=1:n denote the inputs and outputs, respectively.

Fig. 3
Fig.3provides an overview of the used robot.

Fig. 5
Fig. 5 and 6 summarize needed steps for the proposed controller implementation.

F
. Implementation Results: Inputs and Output Constraints In addition to inputs constraints already defined in the previous paragraph, output constraints are also considered in this section.The output constraints are defined as y min = ω -[0.01 0.01 0.25] and y max = ω + [0.01 0.01 0.25].

Fig. 11
Fig. 11 and 12 present the plot of the robot trajectory as well as the reference path.It is concluded from Fig. 11 and 12 that the output constraints are satisfied with errors very close to zero along the path.From Fig.11, 12, 13 and 14, it is established that inputs control and inputs control deviations constraints are satisfied and the proposed control method controls successfully the process with a very good set-point tracking.

TABLE I .
OPTIMIZATION RESULTS