Autonomous navigation of unicycle robots using MPC M. Farina marcello.farina@polimi.it Dipartimento di Elettronica e Informazione Politecnico di Milano 7 June 26
Outline Model and feedback linearization Goals and constraints Some experimental results Marcello Farina MPC control of unicycle robots 2
Outline Model and feedback linearization Goals and constraints Some experimental results Marcello Farina MPC control of unicycle robots 3
Model and feedback linearization y U θ v x U = v cos θ ቐy U = v sin θ θ = ω Our first goal is to find a way to linearize the system s dynamics or, in a way, to force the system to behave as a linear system. x U We resort to feedback linearization. Two different approaches can be taken. Marcello Farina MPC control of unicycle robots 4
Model and feedback linearization first approach x U = v cos θ y U = v sin θ θ = ω v = a x U = v x y U = v y v x = cos θv v sin θ θ = cos θa v sin θ ω v y = sin θv + v cos θ θ = sin θa + v cos θ ω We define x U = v x y U = v y v x = a x v y = a y Inputs of the linearized model a x = cos θa v sin θ ω a y = sin θa + v cos θ ω a cos θ = ω sin θ = v cos θ v sin θ and the model becomes v sin θ v cos θ v sin θ a x cos θ a y a x a y Real inputs of the unicycle Marcello Farina MPC control of unicycle robots 5
Model and feedback linearization first approach a x a y v v cos θ sin θ v sin θ cos θ a x a y a ω x U = v cos θ y U = v sin θ θ = ω v = a x U y U θ, v a x a y x U = v x y U = v y v x = a x v y = a y x U y U Marcello Farina MPC control of unicycle robots 6
Model and feedback linearization first approach Feedback-linearized model: x U = v x y U = v y v x = a x v y = a y Discrete time feedback-linearized model (sampling time τ) x U (k + ) τ x U (k) τ 2 /2 v x (k + ) = v x (k) y U (k + ) τ + τ a x (k) y U (k) τ 2 /2 a y (k) v y (k + ) v y (k) τ Output: position x t + = Ax t + Bu(t) x U (k) y U (k) = Marcello Farina MPC control of unicycle robots 7 x U (k) v x (k) y U (k) v y (k) = Cx(k)
Model and feedback linearization second approach y U ε θ v P x U = v cos θ ቐy U = v sin θ θ = ω x P = x U + ε cos θ y P = y U + ε sin θ x U x P = x U ε sin θ θ = v cos θ ωε sin θ y P = y U + ε cos θ θ = v sin θ + ωε cos θ Marcello Farina MPC control of unicycle robots 8
Model and feedback linearization second approach ቊ x P = v cos θ ωε sin θ y P = v sin θ + ωε cos θ We define v x = v cos θ ωε sin θ v y = v sin θ + ωε cos θ and the model becomes x P = v y y P = v y Inputs of the linearized model v ω = ε cos θ = sin θ ε cos θ sin θ ε sin θ ε cos θ ε sin θ v x cos θ v y v x v y Real inputs of the unicycle Marcello Farina MPC control of unicycle robots 9
Model and feedback linearization second approach v x v y ε ε cos θ sin θ ε sin θ cos θ v x v y v ω x U = v cos θ ቐy U = v sin θ θ = ω x P y P θ a x a y x P = v y y P = v y x U y U Marcello Farina MPC control of unicycle robots
Model and feedback linearization second approach Feedback-linearized model: ቊ x P = v x y P = v y Discrete time feedback-linearized model (sampling time τ) x U (k + ) y U (k + ) = x U (k) y U (k) + τ τ v x (k) v y (k) x t + = Ax t + Bu(t) Output: position (i.e., the whole state) y(k) = x U (k) y U (k) = Cx(k) Marcello Farina MPC control of unicycle robots
Outline Model and feedback linearization Goals and constraints Some experimental results Marcello Farina MPC control of unicycle robots 2
Goals and constraints With any feedback linearization approach we obtain the linear model x k + = Ax k + Bu(k) y k = Cx k position We aim to solve the simplest problem, i.e., the parking problem Starting point Goal Marcello Farina MPC control of unicycle robots 3
Goals and constraints To fulfill this, we must include, in the MPC problem, the following ingredients: Constraints: Collision avoidance constraints with respect to external walls Fixed obstacle avoidance constraints If more vehicles are included, inter-robot collision avoidance constraints Constraints for fulfilling operational limitations (e.g., maximum speed, etc) Cost function: For approaching the goal position To prevent deadlock solutions with fixed obstacles Proper terminal cost function and terminal constraints for guaranteeing recursive feasibility. Marcello Farina MPC control of unicycle robots 4
Collision avoidance constraints with respect to external walls Goal If the red area is polytopic and convex, we can write the corresponding constraint as follows: A area y(k) b area A area Cx(k) b area Marcello Farina MPC control of unicycle robots 5
Collision avoidance constraints with respect to (fixed) obstacles Goal The problem is that, to avoid obstacles of this type, the «free area» is not convex, and so we cannot express the problem using constraints of the type: A coll avoidance y(k) b coll avoidance Marcello Farina MPC control of unicycle robots 6
Collision avoidance constraints with respect to (fixed) obstacles Steps:. Assume that it is possible to circumscribe each obstacle with a polytope with L sides (better if L is large). The interior of the obstacle can be represented as A obst y b obst L inequalities, all satisfied 2. Note that a collision-free point is such that at least one of the above L inequalities is violated, i.e., at least one of the elements of the vector ρ = A obst y b obst is positive Marcello Farina MPC control of unicycle robots 7
Collision avoidance constraints with respect to (fixed) obstacles Example The interior of the square is y characterized by L=4 inequalities (intersection of half planes) - x x x x - y y y Pick an external point, e.g., ( x, ҧ തy)=(.5,2). Which inequalities are violated? The vector ρ = xҧ തy =.5 2.5 3 has two positive entries. This is the «most violated». Marcello Farina MPC control of unicycle robots 8
Collision avoidance constraints with respect to (fixed) obstacles 3. In the MPC context, we can take advantage of the fact that, at time t, the optimal collision-free trajectory computed at the previous time instant t- is available, i.e., u t t, u t t,, u(t + N 2 t ) x t t,, x(t + N t ) y t t,, y(t + N t ) Feasible inputs States Positions y t t y t + N t Since the trajectory is collision-free, for all k=t,,t+n-, at least one entry of the vector ρ k t = A obst y(k t ) b obst is positive. We select the corresponding constraint to be enforced (at time t) on y(k). Marcello Farina MPC control of unicycle robots 9
Collision avoidance constraints with respect to (fixed) obstacles y k t This convex set defines the constraint for y(k) in the optimization problem solved at time t. Formally: We compute, for all k=t,,t+n- the vector ρ k t = A obst y(k t ) b obst We select iҧ such that the entry ρ iҧ k t ρ i k t, i=,,l. We impose only the constraint A obst, iҧ y(k) b obst, iҧ Marcello Farina MPC control of unicycle robots 2
Collision avoidance constraints with respect to (fixed) obstacles goal agent obstacle Marcello Farina MPC control of unicycle robots 2
Inter-robot collision avoidance constraints If two vehicles are present, we aim to generate collision-free trajectories. We exploit communication! Each agent is regarded, by the other agent, as a moving obstacle. The «most violated» constraint is selected (similarly to the previous case) based on the collision-free trajectory predicted at the previous optimization step. Each agent is allowed to move towards the constraint half the distance, to avoid collision. Marcello Farina MPC control of unicycle robots 22
Operational limitations Consider, for example, the feedback-linearized model obtained with the first approach, i.e., x U (k + ) v x (k + ) y U (k + ) v y (k + ) = τ τ x U (k) v x (k) y U (k) v y (k) + τ 2 /2 τ τ 2 /2 τ a x (k) a y (k) Velocities (in x and y directions) are states Accelerations (in x and y directions) are inputs Constraints on these variables may be used to enforce constraints on the longitudinal speed v. Constraints on these variables may be used to enforce constraints on the longitudinal acceleration a. Marcello Farina MPC control of unicycle robots 23
Operational limitations We want to enforce: v M v v M This can be done, for example, by imposing: v y θ v v x v M 2 v x(k) v M 2 v M 2 v y(k) v M 2 x(k) v M 2 Marcello Farina MPC control of unicycle robots 24
Operational limitations We want to enforce: a M a a M This can be done, for example, by imposing: a y θ a a x a M 2 a x(k) a M 2 a M 2 a y(k) a M 2 u(k) a M 2 Marcello Farina MPC control of unicycle robots 25
Cost function for approaching the goal position The goal position is തy G. The naive approach consists in minimizing, at time t, the following cost function: J = t+n k=t y k തy 2 G Q + u(k) 2 2 R + x t + N xҧ G P where xҧ G = തy G,x തy G,y (we aim to rest at the goal) Marcello Farina MPC control of unicycle robots 26
Cost function for approaching the goal position However, assume that, in N steps not all the trajectory to the goal can be covered Point that can be reached in N steps In such case is more reasonable to define temporary goals Marcello Farina MPC control of unicycle robots 27
Cost function for approaching the goal position An alternative approach, indeed, consists in the tracking one: t+n J= k=t y k y G (t) Q 2 + u(k) R 2 + x t + N x G (t) P 2 + γ y G t തy G 2 temporary goal (for the whole trajectory), additional degree of freedom Other approaches can be taken depending also on the specific problem at hand (parking, trajectory tracking, etc.) Marcello Farina MPC control of unicycle robots 28
Cost function to prevent deadlock solutions with fixed obstacles So far collision avoidance is enforced just by using hard constraints.? Deadlock situations may occurr! Without additional terms, the cost function pushes the robot towards the bottleneck An additional «force» is necessary to circumnavigate the obstacle Temporary goals can be set using the theory of vortex fields Marcello Farina MPC control of unicycle robots 29
Cost function to prevent deadlock solutions with fixed obstacles Two words on vortex fields If obstacles «generate» repulsive fields local minima (i.e., bottlenecks) may occur Instead, if repulsive forces are «rotated», i.e., they are directed in tangent direction with respect to the obstacle, circumnavigation is guaranteed. A fictitious goal x VF (t) is generated in the direction of the vector field and an additive term x k x VF (t) 2 appears in the stage cost Marcello Farina MPC control of unicycle robots 3
Terminal constraints Assume again that, in N steps, not all the trajectory to the goal can be covered Terminal point Recursive fesibility can be compromized if this point is not in a «safe region» Marcello Farina MPC control of unicycle robots 3
Terminal constraints In regulation problems, the «safe region» is a region is characterized by the fact that, by using the auxiliary control law, we go towards the goal without colliding with obstacles or other vehicles and without violating the operational constraints That s not possible in this case! Marcello Farina MPC control of unicycle robots 32
Terminal constraints To guarantee theoretical properties we may rely on the «tracking» approach, with terminal point. Basically, we require that, after N steps, we may be able to stop at the intermediate goal, defined as the further degree of freedom for our MPC optimization problem. This is cast as the linear equality terminal constraint: x t + N = x G (t) Marcello Farina MPC control of unicycle robots 33
Goals and constraints - summary Ingredients: Constraints: Collision avoidance constraints with respect to external walls Fixed obstacle avoidance constraints If more vehicles are included, inter-robot collision avoidance constraints Constraints for fulfilling operational limitations (e.g., maximum speed, etc) Cost function: For approaching the goal position To prevent deadlock solutions with fixed obstacles Proper terminal cost function and terminal constraints for guaranteeing recursive feasibility. Linear inequality (time invariant) constraint Linear inequality (time-varying) constraint Linear inequality (time-varying) constraint Linear inequality (time invariant) constraint Quadratic cost function Quadratic additive terms Linear equality constraint Marcello Farina MPC control of unicycle robots 34
Outline Model and feedback linearization Goals and constraints Some experimental results Marcello Farina MPC control of unicycle robots 35
Some experimental results Marcello Farina MPC control of unicycle robots 36
Some experimental results Marcello Farina MPC control of unicycle robots 37
Some experimental results Marcello Farina MPC control of unicycle robots 38