Model Predictive Collision-Free Path Following Control for Nonholonomic Mobile Robots

In this research, a model predictive collision-free path following controller is developed and applied for an omnidirectional mobile robot (OMR). The mobile robot is controlled to track a reference path while avoiding collision with obstacles. The path-following problem is reformulated into the regulation problem of an extended plant by introducing a virtual degree of freedom, the path parameter of a geometric reference curve. Then a Model Predictive Controller (MPC) is then applied to steer the mobile robot. The optimization cost function is established from the difference between the state of the robot and the parameter path. The solution of MPC can be obtained by repeatedly solving an optimal control problem (OCP) to reduce the optimization cost function to a minimum value, making the robot state as close to the state of the path as possible. Obstacle avoidance is considered by adding terms as a function of the gap between the mobile robot and the objects in front of the robot. Constraints on the states and inputs of the system are also easily considered in the optimal control problem of MPC. This makes the control inputs not exceed the allowable limits of the robot. Simulations are carried out to reveal the controller's efficiency and show how to choose the right parameters to synchronize path tracking and obstacle avoidance tasks.


INTRODUCTION
Over the last two decades, nonholonomic mobile robots have gained more and more research interest from engineers and scientists.The advantages of an omnidirectional mobile robot (OMR) are that it can synchronize the rotation motion (steering) and transla-tion motion (linear) in any direction on the ground, greatly improving the robot's flexibility to attain rapid target tracking and obstacle avoidance.So, it meets the growing requirement for high flexibility, high perfor-mance, and safety of various applications in practice, such as healthcare assistance [1], workshop assistance [2], home assistance [3], and domestic [4].Omni-directional mobile robots are built using un-steered Omni wheels or mecanum wheels.In [5,6], two omni-directional mobile structures with mecanum wheels were developed.
In many automation applications, mobile robots must be able to move autonomously in a plant, laboratory, home,... (without any human assistance) [7].So, the motion control problem is essential in many mobile robotic systems.Control problems are divided into three basic problems: set-point stabilization, trajectory tracking, and path-following.The path-following problem is the more general problem; the other two problems can be considered as specific cases of it.In the pathfollowing problem, the robots are driven to follow a predefined geometric curve while satisfying dynamic constraints along the path.Many studies have proposed solutions for the path-following problems to apply in many applications (applied for a two-link robot manipulator in [8], for a KUKA LWR IV robot in [9], for an n trailer vehicle in [10], for aircraft and marine vehicles in [11]).Many studies have also investigated the motion planning problems for omnidirectional mobile robots [12][13][14].And in recent years, Model Predictive Control (MPC) has been applied for pathfollowing tasks with many successful results [15][16].
MPC is also known as receding horizon control, in which a finite-horizon optimal control problem (OCP) is solved online at every control cycle.The optimal solution's first control action is used as the control input for the real system [20][21][22][23].The main advantage of the MPC algorithm is that constraints on inputs and states are considered in the optimal problem.This helps to avoid exceeding the limits of the system (e.g., workspace) and control signals (e.g.velocity, acceleration, etc.), and collision avoidance constraints [24][25][26].Normally, obstacles must be considered in pathfollowing applications, and the controller must synchronize the path-following and obstacle avoidance.
The problems of following a parameterized curve have been introduced in many studies that use the nonlinear MPC strategy to solve them.In [17], Yu et al. presented a nonlinear MPC scheme for the pathfollowing problem by converting the problem into a regulation problem.A Polytopic Linear Differential Inclusion Problem (PLDI) is used to choose a suitable terminal constraint and cost.An acar-like mobile robot is used in the simulation to confirm the control performance.Faulwasser and Findeisen [18] provided a general framework for solving constrained output pathfollowing problems by designing a continuous-time predictive control.The transverse normal forms are analyzed and used to compute stabilizing terminal regions and end penalties.Two different cases of the path-following problem are investigated.In the first case, the velocity assignment for the reference evolution is not specified.The other case is extended with a velocity assignment.In [19], a path-following controller based on the kinematics model of underactuated vehicles is developed by combining the MPC controller with a nonlinear auxiliary control law.By assuming that the terminal set can be neglected in the case of unconstrained inputs, MPC controllers provide a global solution to the addressed constrained motion control problems.In [20], a real-time nonlinear model predictive path-following controller is developed for a labo-ratory tower crane to move a load along a predefined geometric path.The MPC is adopted for an extended system where the time evolution along the path is an extra degree of freedom to be determined by the controller.In previous studies, the selection of the controller coefficients in the case of with and without obstacles has yet to be presented.When it is required to follow the path and avoid obstacles simultaneously, the coefficients in the cost function must be adjusted so that the robot does not stop when approaching the obstacle.In this work, a Model Predictive Collision-Free Path Following Control (MPCPFC) is developed to control an OMR robot.The mobile robot is driven to move along the path while avoiding collisions with obstacles.The parametrized path introduces a new virtual degree of freedom into the controller.The path-following problem is formulated into the regulation problem of the augmented plant.The input and state constraints are directly considered in the control design.Moreover, obstacle avoidance is achieved by adding constraints and penalty terms in the optimal function that depend on the relative position of the obstacles and the mobile robot.Simulation results are performed, thereby giving the correlation between the coefficients in the controller when considering collision avoidance.
The remainders of this paper are structured as follows: Section 2 presents the kinematic of the omnidirectional mobile robot; In section 3, The path following problem is reformulated as the regulation problem, and the model predictive control strategy is applied to solve the regulation problem by optimizing the cost function established from the error of the robot state and the state of the parameter path, obstacle avoidance is also simultaneously addressed by adding components related to the position of the obstacle in the cost function; Section 4 shows the simulation results to demonstrate the efficiency of the proposed method; Finally, Section 5 is the conclusion.

THE KINEMATICS OF OMNIDIRECTIONAL MOBILE ROBOT
In Figure 1, a mecanum wheel is constructed by a wheel connected to a motor and some passive rollers.A coordinate frame x´O k y´ is attached to the wheel centroid.The X-axis has the same direction as the wheel's axis of rotation, the Y-axis is parallel to the ground.The wheel's radius is denoted by r w , the motor's angular velocity is denoted by ωk, the velocity of the passive roller is denoted by υpk, and the υ pk is perpendicular to the rotation axis of the rollers.The velocity of the mecanum wheels is a combination of the velocities of the motor and the rollers.Let [υ´k x , υ´k y ,ω´k] T denote the velocity of the mecanum wheel's centroid relative to x´O k y´.It can be obtained from the relationship [25]: where φ k is the deflection angle of each roller, which is the angle between the velocity υ pk and the vector O k y´, k, represents the order of the wheels.Usually, to create omnidirectional motion on the ground, omnidirectional mobile robots are usually equipped with three or four mecanum wheels.A threewheel mobile robot is utilized in this work in which three mecanum wheels are evenly distributed in a 360° circumference (Figure 2).The axes intersect at the frame's centroid, and the angles between two neigh-boring wheels are the same (equal to 120 0 ).In Figure 3, the coordinate frame XOY is attached to the centroid of the robot platform in which the centreline OO´k is collinear with the axis O´x i .The angle between two axes OO´k and OX is equal to the angle between O k x and OX, denoted by β k .
Let [v kx , v ky ] ωkT denote the centroid velocity of the mecanum wheels relative to the frame XOY.The relationships between velocities in two frames x´O k y´ and XOY are [28]: Substituteequation (1) into equation ( 2):

Figure 3. The Coordinate Frames Relationship
Denote [υ x , υ y, ω] T is the velocity vector of the centroid platform relative to the ground plane.According to the geometric relationship [28]: where (d kx ; d ky )is the coordinate of the kth wheel's mass point in XOY coordinate system and can be expressed as: Substitute Equations ( 3) into ( 4), the inverse equation to determine the velocity of each mecanum wheel can be determined by the equation: Define: The angular velocities of three mecanum wheels are calculated: In practice, the state of the mobile robot is determined in a global coordinate system x g O g y g .Let (x g ,y g ,θ g ) denote the robot's state in the global frame.The velocity relationships are defined as: From Figure 2, the values of angles are 7), ( 8) and ( 9) to get equation ( 10): with Define x = [x g ,y g ,θ g ] T as the state vector of the robot in frame x g O g y g , u = [ω 1 ,ω 2 ,ω 3 ] T as the control input vector, the state space equation of the robot is: where:

Problem of following a parameter path
Consider a geometric reference curve given by [26]: where the s is a scalar value and called the path parameter, and p(s) is a parametrization function: where ρ x (s), ρys are differentiable at least to the second order [20].The path-following problem is defined as designing a controller so that the robot state x(t) approaches the path and moves along [30]: In practice, exact path tracking is difficult due to system constraints.For the path-following problems, the mobile robot is not required to track the curve exactly.Instead, the control input and the timing s(t) are to be chosen to steer the mobile robot as close as possible to the path while still satisfying the state and control constraints.The timing law of s(t) is not defined in advance; the controller determines it.The time evolution t → s(t) and the control input must be optimized to allow the robot to follow the curve and ensure the state and control constraints (x ∈ X and u ∈ U).Furthermore, to keep the robot moving on the path, it is required that the state of the path must satisfy the state constraints, ⊂ X for all s ∈ [ ] ˆ,0 s .
Because the time evolution of s is was not specified prior, a virtual control input ω ∈ obtained in the controller is applied to control the path parameter s throughout a 'timing law', and the path parameter s is considered as a virtual state of the system.So, the controller is extended with an extra degree of freedom.Although complex timing laws can be considered, a simple integrator is used in this work: where s 0 ∈ [ ] ˆ,0 s is the initial state of the path, it must be predefined (assumed to be known in advance).The virtual control input ω is assumed to be piecewise continuous and bounded ω(t) ∈ O ⊂ >0 .The positive value of ω is to ensure that the robot does not move backward along the path.Define the extended state χ = [x T ,s] T ∈ 4 that contains the robot state and the virtual state, the extended input η = [u T , ω] T ∈ 4 consists of the control input and the virtual control variable.The constraint set of the augmented state The path-following problem is analyzed via the following augmented system: The output of the system is: It can be seen that the output path-following requires that the error e pf converges to zero while the path parameter s converges to the origin.Where epft the path following error, which is the difference between the state of the robot and the path state.So, the pathfollowing problem is reduced to consider as the pointstabilization problem and can be solved by using a Nonlinear Model Predictive Control.

Model Predictive Path-Following Control (MPFC)
Assuming that sampled-data state information is available to the controller, the output path-following problem is solved using a continuous-time sampled-data nonlinear model predictive control (NMPC) scheme, called model predictive path following control (MPFC).The solution of MPFC can be obtained by repeatedly solving an optimal control problem (OCP).Let x and u denote the predicted states and outputs to distinguish the actual value from the predicted.At each sampling instance t k = kτ, with k = 0,1,2…; τ is the sampling time, the cost function to be minimized in the OCP is [27]: with T > 0 is the prediction horizon.The OCP solved repetitively in the controller is [30]: subject to the constraints: The terms of the cost function are defined as [25]: with q ≥ 0, r ≥ 0, r s ≥ 0, and Q, Q d , R Re are the positive semidefinite diagonal matrices.The OCP is solved by applying the direct simultaneous approach with the support of CasADI software.The OCP is reformulated as Nonlinear programming (NLP) using the 4th order Runge-Kutta (RK4).Denote The state of the system is solved by RK4 equation [31]: with ( ) The cost function ( ) , J χ η is discretized as [8]: , ,..., , , / the prediction horizon length.The resulting NLP is as follows [8]: ( ) subject to: ( )

Obstacle avoidance
To avoid collision with obstacles when following the path, the cost function is extended in order to push the mobile robot away from obstacles.At the same time, the mobile robot is controlled to follow the path as closely as possible.The distances between the centroid of the robot and the obstacles are calculated: where x0i ∈ 3 is the coordinate of the ith obstacle point.Suppose that the positions of obstacles are known in advance and are available to the controller at each iteration.The cost function is augmented by a term that penalizes the squared distances between the mobile robot and the obstacle centroid, as used by Angelika in [25]: with m i ≥ 0, c 0 ≥ 0 are constant coefficients, d min is the minimum distance between the robot center and obstacles, and N 0 is the number of obstacles.It can be , ratio ( ) So, adding F 0 to the objective function helps the mobile robot move away from the obstacles.However, there is no guarantee that the mobile robot does not collide with the obstacles.For this reason, the following constraints must be considered: These constraints ensure that the distances between the centroid of the obstacles centroid and the mobile robot are always to be greater safety distance.

SIMULATION RESULTS
In this part, simulations are conducted to confirm the correctness of the kinematic equation of the OMR and the effectiveness of the model's predictive collision-free path following the control algorithm.Two reference paths chosen for simulation are: (1) a circle of radius 0.8m, the centroid at (0.5 m; 1 m), no obstacle, the initial coordinate of the mobile robot is (1.3m; 1m; π/4 rad) (robot do not lie on the reference path at the beginning).
In simulation 1, all coefficients for the MPFC are as follows.
Q = diagonal(4000, 4000, 800) To simulate the real state of the mobile robot, noise with a mean of zero and a variance of 5mm is included in the output state.The results are shown in Figure 4. Plot a of Figure 4 is the comparison of the reference and real path of the robot.The reference path is represented by the green dashed line, and the red solid line marks the movement of the mobile robot.Plot b of Figure 4 shows the error on the x and y-axes and the direction angle error.Plot c of Figure 4 shows the control input applied to the mobile robot.From Figure 4.a, it can be seen that robot is controlled from the initial position to approach the reference path and follow exactly the reference.The maximum error at the initial stage is (15mm, 200 mm, 0.084 rad).When the robot has approached the reference, the maximum tracking error is about 4mm.The control inputs are bound in the constraints with a maximum 1 rad/s.It can be seen that the robot tries to move to the first point of the path before moving along the path.To ensure small tracking errors, the coefficients of the matrix Q have very large values.In contrast, the value of r S is very small.In this case, the robot needs to track the path accurately rather than move to the target or overcome an obstacle.
The parameters for the MPFC in Simulation Case 2 are chosen: In this case, the robot is controlled to follow the path and avoid obstacles simultaneously.Obstacle avoidance task has a higher priority to follow the path.So, the weight matrix must be changed to achieve the targets.If the parameters are kept the same as in the previous task with high coefficients ofpath-following error, the robot tries to follow the path exactly.So, it does not leave the path to pass through the obstacle and halts in front of the obstacle, as shown in Figure 5.
For this reason, the coefficients of matrix Q and R e in the cost function are reduced, and the coefficients q and r in the cost of the path parameter are increased.The results are shown in Figure 6.The MPFC can synchronize the path following the task and avoiding a collision.However, in the early stages, the robot takes longer to approach the reference path.Then, the robot follows the path exactly until it encounters an obstacle.The MPFC steers the robot as close to the path as possible while avoiding collisions with obstacles.In Figure 6.b, it can be seen that the maximum position error is equal to the distance d min .Due to using the cost F 0 (x), the trajectory of the robot is smooth.If only using the constraints about the minimum distance to the obstacle, the robot's path would be the same as the outline of the obstacles, and it may not be smooth.
The parameters in test case 2 are used to simulate the path defined by the equation: p x = 1.8sin(s), p y = 1.2sin(2s) and avoid colliding with three obstacles located at positions (0,0), (-1.9, 0.5), and (1.4,-1.2), respectively.The results in Figure 7 show that the robot can track the path and avoid colliding with obstacles.The selected set of parameters can be used for other path types.From the simulation results, it can be seen that the choice of parameters in the cost function is very important.Depending on which tasks need to be prioritized, we choose the appropriate parameters.If accuracy is the preferred metric, then the coefficients related to tracking error (matrix Q and Q P ) must be large.The coefficients of the matrices R and R S or the values of q and r will be related to the speed of travel.If the matrices R and R S have elements with large values, then the control signal u will have a small value, so the robot will move more slowly.If collision avoidance is also considered, then the elements of the matrix Q and Q P must have small values and the coefficients q and r must have large values.When encountering an obstacle, the robot can overcome it without considering the tracking error.

CONCLUSION
This work has presented a model predictive path for the following controller for an omnidirectional mobile robot.The kinematic equation of the OMR is obtained and used to estimate the next state of the robot in the controller.The extended system with the output is the path-following error, the virtual state is established, and the path-following problem is reformulated into the regulation problem.Obstacle avoidance is obtained by adding a penalty into the cost function that depends on the gap between the mobile robot and the obstacles.
The simulations are conducted, and the results reveal that the proposed controller can steer the robot to follow the path and avoid colliding with obstacles simultaneously.If only a reference path is required to follow exactly, the parameters related to the path following the error in the cost function are chosen with high values.If obstacles are considered, these values should be reduced so that the controller can synchronize the path-following task and collision avoidance.In the test case of the pathfollowing task, the maximum tracking error is only 4mm, and the robot quickly approaches the path (in 0.1s).If obstacle avoidance is considered, tracking error is not a priority metric, so the robot slowly approaches the path.It takes about 1s for the robot to approach the path.The tracking error is large at the beginning of the path.
In future work, we will develop a real robot prototype to conduct the experiment test.The real robot will be equipped with sensors to perceive the surrounding environment and measure the distance to obstacles.Robots can be used for purposes such as transporting goods in warehouses; cleaning and disinfection in hospitals or healthcare centers; delivering food, drinks, and goods to guests in restaurants or in communal areas…

NOMENCLATURE x
The state vector of the robot p(s) The parametrization function s The path parameter X The state constraint U The control constraint ω The virtual control input χ The extended state η The extended input Ω The constraint set of the augmented state V The augmented control constraint e pf The path follows error J The cost function F The penalty term of the error and input E The terminal penalty term K i

Coefficients of RK4 function N T
The prediction horizon length x 0i The centroid of the obstacles d i The centroid distance F 0 The term that penalizes the centroid distance The terminal term that penalizes the centroid distance

Figure 4 .
Figure 4.The simulation result of the circular path: (a) robot path and reference in x-y plane, (b) the pose error, (c) the angular velocity of wheels

Figure 5 .Figure 6 .Figure 7 .
Figure 5.The result with high coefficients of the path following error.