Multi-Objective Trajectory Planning of Mobile Robots Using Augmented Lagrangian

Size: px

Start display at page:

Download "Multi-Objective Trajectory Planning of Mobile Robots Using Augmented Lagrangian"

Andra Day
6 years ago
Views:

1 CSC May Marraech Morocco /0 Multi-Objective rajectory Planning of Mobile Robots Using Augmented Lagrangian Amar Khouhi Luc Baron and Mare Balazinsi Mechanical Engineering Dept. École Polytechnique of Montréal C. P Succ. CV Montréal Canada H3C 3A7 Phone: +(54) Ext. 47 Fax (54) (amar.houhi luc.baron

2 CSC May Marraech Morocco /0 Outline Introduction Robot and Constraints Modelling Non linear Programming Formulation Augmented Lagrangian with Projection Implementation on a Wheeled Mobile Robot Conclusions and Discussions

Introduction Motion Planning Problem Path planning: rajectory planning: Geometric path shortest path obstacle avoidance passing through imposed posse Includes velocities accelerations dynamic forces

3 Introduction Motion Planning Problem Path planning: rajectory planning: Geometric path shortest path obstacle avoidance passing through imposed posse Includes velocities accelerations dynamic forces energies tas and worspace requirements Y ( x( ) y( ) θ( ) B Path Constraints as Specification Planning Objectives rajectory Planer Robot state q [ x y ϕ θl θr] q [ x y ϕ θl θr] q [ x y ϕ θ θ ] L R g P P3 P Dynamic Constraints Offline rajectory Planning Framewor A P0 ( x(0) y(0) θ (0)) X CSC May Marraech Morocco 3/0

4 Robot and Constraints Modelling Kinematics CSC May Marraech Morocco 4/0 he study of object motion regardless to forces causing this motion Non-Holonomic Constraints : A( q) A q.. q 0 constraint matrix [ x y ϕ θ θ ] q S ( q)u u [ θ R θ L ] [ u L Is a inematic constraints that cannot be integrated to obtain a geometric constraint R u ] sinϕ A( q) cosϕ cosϕ cosϕ sinϕ sinϕ 0 b b generalized coordinates S(q) such that A( q) S( q) u 0 pseudo-velocity 0 r r

5 CSC May Marraech Morocco 5/0 Robot and Constraints Modelling Dynamics Study of relationship between displacements displacement rates and accelerations characterizing the robot motion and torques and forces causing this motion. Dynamic Model D ( q) q+ V( q q) B( q) τ + A ( q) D(q) Inertia matrix V( q. q) Position and velocity dependent forces B(q) τ Input matrix [ τ R τ L ] [ τ τ ] Input vector Associated Lagrange multiplier to the inematic constraint

6 Robot and Constraints Modelling Apperoximated Discrete Dynamics CSC May Marraech Morocco 6/0 he non-holonomic inematic equation. q S ( q)u Dynamic equation Let x [ q u] State-space representation Developed using Euler discretization x. u D ( q) V( q u) + B( q) τ x ( t) F ( t) x( t) + G( t) N( x( t)) + B( t) τ( t) Approximated Discrete-ime Dynamic Model + F d x + G dn( x ) + Bd τ

7 Robot and Constraints Modelling Associated Constraints CSC May Marraech Morocco 7/0 Robot Constraints Discrete dynamic equation : x f ( x τ h ) + d Limits on : Right wheel angle Left wheel angle Steering angle orque limits { } N C τ R τ < τ < τ 0...N θ < θ < θ RMin R RMax θ < θ < θ LMin L LMax ϕ < ϕ < ϕ Min Max Min Max

8 Robot and Constraints Modelling Associated Constraints as Constraints + Sampling periods H { h R h < h h } Intermediate pose limits Min < p < Min < p pmax Imposed passage points p p 0 - l Passhlp Max l... L with L is the number of imposed points Passhlp passage tolerance Environment Constraints Θ ( p n p ) p Obstacles avoidance function n p his is expressed by an inequality constraint n... N Θ( p p ) obstacles to avoid n η ER with η ER Avoidance tolerance CSC May Marraech Morocco 8/0

9 Non linear Programming Formulation Performance Index K φ Performance Index: Electric Power and ime Consumption Discrete Multi-objective Optimal Control Problem Find control inputs τ ( τ τ ) and h 0...N Solution to with ι ι K DC ( τ τ Min R N R ) h + Re K K φ e xn motor gear ratio E Re d N [ ι K ( τ + τ ) ι ] + 0 CSC May Marraech Morocco DC motor resistance K e τi torque applied to the i th motor. h torque constant : Electric Power and ime Level-Headedness Positive scalars 9/0

10 0/0 CSC May Marraech Morocco Augmented Lagrangian b b a b a i S i S ) ( ) ( μ μ + Ψ + Φ ) Max(0 ) ( a b a ab j g j g j g μ μ μ Φ )) ( ( J 0 j j j N - h h g τ ρ μ x g Inequality Constraints ) ( σ ρ λ τ μ h L x Performance Index ) ( ( N d h τ λ x f x + Ψ 0 )) ( ( N l i i i L l S h x s σ μ System s Dynamics Equality Constraints Augmented Lagrangian Non linear Programming Formulation ) ( N DC h K ι ι τ τ Penalty functions

11 CSC May Marraech Morocco /0 Non linear Programming Formulation Augmented Lagrangian with Projection Karush-Kuhn-ucer (KK) first order optimality conditions : A solution x L μ such that 0 and x τ Lagrange multipliers h ( λ ρ σ L μ L μ L 0 μ L μ τ h λ σ ρ g( x τ h) 0 σ s( x) 0 g( x τh) 0 to the problem implies there exists ) and penalty coefficients μ ( μ g μ S L μ 0 ρ ) he final state constraint x N x is verified in an initial feasible solution. When the control vector is changed the final state is shifted from the desired one. he adjustment for the final state constraint is done with an orthogonal projection on the tangent space of the constraint: Pr I Q( QQ) Q d d Pr τlμ

12 Non linear Programming Formulation Augmented Lagrangian with Projection Data Reading Initial and final states Lagrange multipliers State torques and sampling periods Limits olerances number of discretizations N and iterations Primal Optimisation * Initial Solution L μ h Compute gradients he co-states bacwardly τ L μ Projection matrix & operator Q P direction descent d λ No Max Iter Reached Yes * No No Optimal Solution End Progarm Update search direction: υ arg Min L μ ( x v+ υd ν ρ h ) Update control input: Update system state: Reduce ol. w t η t v + x v Update Lag. Multipliers ( ρ σ) Update Penalty ( μg μs) υ υ v d v h h + h υ Dual Optimisation st h.shold nd h.shold L CSC May Marraech Morocco D No No est on Steepest descent d < ε Yes Feasability est Cost minimized Constraints not violated? Yes Convergence est Yes Cost minimized? Constraints non violated? With Optimum olerances Display Optimal rajectory x h v /0

13 CSC May Marraech Morocco 3/0 Implementation on a Wheeled Mobile Robot Robot and Simulation Parameters Robot parameters Robot mass L.5(m) m 0(Kg) b 0.70(m) Wheel mass m W0(Kg) Passage tolerance Wheel radius 0 Passhlp - r 0.5 (m) Limits on worspace actuator torques steering angle and sampling periods Parameter x(m) y(m) τ (Nm) ϕ(rad) Max π / 4 0. Min π / τ (Nm) h(sec) Augmented Lagrangian parameters υ0. υ 0. 3 ηs 0.5 μ αw βw βη 0. 4 αη0.4 γ 0.5 γ.4 η * η 0 * -3 ωs 0.5 w * 0 4

14 Implementation on a Wheeled Mobile Robot A Scenario rajectory CSC May Marraech Morocco 4/0 From the pose coordinates (0.(m) (m) 30 0 ) to end at the position ((m) 5(m) 45 0 ) Initial and final linear and angular velocities are taen to zero (m/sec) and zero (rad/sec) his trajectory is sampled into a total number of N 0 points. Minimum power criterion * * ime-power criterion

15 CSC May Marraech Morocco 5/0 Implementation on a Wheeled Mobile Robot hree case studies A Scenario rajectory Minimum Power Minimum ime Minimum ime-power ι ι 0 ι 0 ι ι 0.5 ι 0. 5 * * Minimum power criterion Minimum ime criterion ime-power criterion Minimum ime-power rajectory is 3 % faster than only Minimum Power rajectory

16 Implementation on a Wheeled Mobile Robot A Scenario rajectory CSC May Marraech Morocco 6/0 he same trajectory with imposed passages over three points (0..7) ( ) and (0.8.5) (m).

17 Implementation on a Wheeled Mobile Robot Simulation Results CSC May Marraech Morocco 7/0 Convergence history for the simulated trajectory with fixed sampling time and varying sampling time Problem Fixed sampling time Varying sampling time Number of discretizations 0 0 Number of Inner Iterations 8 8 Number of Outer Iterations 7 7 Number of Adjustments 4 3 ravelling ime (sec).3 PREC (m) CPU (sec) Power (W) 67 4

18 CSC May Marraech Morocco 8/0 Conclusions and Discussions he Problem Multi-Objective rajectory Planning for Wheeled Mobile Robots A Non-Linear and Non-Convex Constrained Optimal Control Problem Augmented Lagrangain with Projection (ALP) allows Giving smooth with monotonous increasing energy trajectories Kinematic solution feasible but torques Outside the admissible domain Minimum ime-power 3% faster than only minimum power criterion Implement a Gradient Projection to reach the final state at each iteration

19 Conclusions and Discussions Limitations & Ways for Improvement Achieve robustness test by changing dynamic parameters such as inertia Implement a Mixed Integer-Non-Linear Program for ime Minimization Achieve experimentation and measurements with a physical robot Perspectives and Future rends Use the outcomes of this offline trajectory planning system along with physical measurements: as refernce trajectory to build a feedbac control system for adaptive multi-objective online planning as dataset trajectories to build an adaptive neuro-fuzzy control system for online multi-objective planning CSC May Marraech Morocco 9/0

20 CSC May Marraech Morocco 0/0 han you for your attention!!

CONTROL OF THE NONHOLONOMIC INTEGRATOR

June 6, 25 CONTROL OF THE NONHOLONOMIC INTEGRATOR R. N. Banavar (Work done with V. Sankaranarayanan) Systems & Control Engg. Indian Institute of Technology, Bombay Mumbai -INDIA. banavar@iitb.ac.in Outline