Sensor-based trajectory optimization ABB Robotics Master thesis Martin Biel Supervisor: Mikael Norrlöf Examiner: Xiaoming Hu June 9, 2016 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 1 / 52
Outline Introduction 1 Preliminaries 2 Trajectory Planner 3 Simulations 4 Discussion and conclusion 5 Questions 6 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 2 / 52
Traditional approach Geometric path computed on before hand. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 3 / 52
Traditional approach Geometric path computed on before hand. Optimal path following along the computed path. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 3 / 52
Traditional approach Geometric path computed on before hand. Optimal path following along the computed path. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 4 / 52
Traditional approach Geometric path computed on before hand. Optimal path following along the computed path. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 5 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. The trajectory should be consistent with some given system dynamics. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. The trajectory should be consistent with some given system dynamics. Target application: Conveyor tracking Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. The trajectory should be consistent with some given system dynamics. Target application: Conveyor tracking Pick and place. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. The trajectory should be consistent with some given system dynamics. Target application: Conveyor tracking Pick and place. Collision avoidance. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Problem formulation General problem: Investigate the possibility of constructing a real-time capable trajectory planner, where: The underlying path should be allowed to change dynamically. The planner should be able to react to sensor events, and deform the trajectory accordingly. The trajectory should be consistent with some given system dynamics. Target application: Conveyor tracking Pick and place. Collision avoidance. Track moving targets. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 6 / 52
Outline Introduction 1 Preliminaries 2 Trajectory Planner 3 Simulations 4 Discussion and conclusion 5 Questions 6 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 7 / 52
Preliminaries - Robot modelling y x Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 8 / 52
Preliminaries - Robot modelling y q 2 q 1 x Q - Configuration space Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 9 / 52
Preliminaries - Robot modelling y ( x , y ) q 2 q 1 x Q - Configuration space O - Operational space Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 10 / 52
Preliminaries - Robot modelling y ( x , y ) q 2 q 1 x Q - Configuration space O - Operational space W - Workspace Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 11 / 52
Preliminaries - Robot modelling Forward kinematics : y y y = χ y ( q q q ) q = χ − 1 q y ( y y ) y Inverse kinematics : q q ) ˙ Velocity Jacobian : v v v = J ( q q q q q q ( t ))¨ q ( t ) , ˙ q ( t )) ˙ q q q q q q τ Dynamics : M ( q q q ( t ) + C ( q q q ( t ) + g ( q q q ( t )) = τ τ ( t ) Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 12 / 52
Preliminaries - Optimal control problem Time minimizing formulation q ( t ))¨ q ( t ) , ˙ q ( t )) ˙ τ M ( q q q q q ( t ) + C ( q q q q q q q ( t ) + g ( q q q ( t )) = τ τ ( t ) q q ( t ) ∈ Q q τ − ≤ τ τ τ τ ( t ) ≤ τ τ τ + τ min τ ( . ) T s.t. y y ( t ) = χ y ( q q ( t )) q y τ τ y 0 , ˙ y ( 0 ) = ˙ y ( 0 ) = y y y y y y y y 0 y y T , ˙ y ( T ) = ˙ y y ( T ) = y y y y y y y y T Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 13 / 52
Preliminaries - Timed elastic band Introduce the state vector � q q � q ( t ) x x ( t ) = x ˙ q q ( t ) q as a solution trajectory to the optimal control problem. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
Preliminaries - Timed elastic band Introduce the state vector � q q � q ( t ) x x ( t ) = x ˙ q q q ( t ) as a solution trajectory to the optimal control problem. Discretize the trajectory into a so called Timed Elastic Band (TEB) set B := { x x 1 ,τ x τ τ 1 , x x x 2 ,τ τ τ 2 , . . . , x x x n − 1 ,τ τ τ n − 1 , x x x n , ∆ T } . Note that n and ∆ T are NOT fixed. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
Preliminaries - Timed elastic band Introduce the state vector � q q � q ( t ) x x ( t ) = x ˙ q q q ( t ) as a solution trajectory to the optimal control problem. Discretize the trajectory into a so called Timed Elastic Band (TEB) set B := { x x x 1 ,τ τ τ 1 , x x x 2 ,τ τ τ 2 , . . . , x x x n − 1 ,τ τ n − 1 , x τ x n , ∆ T } . Note that n and x ∆ T are NOT fixed. Determine the system dynamics for x x x ( t ) and approximate them using forward Euler, x x x k + 1 − x x x k = Ax x x k + B ( f ( x x x k ) + h ( x x x k ) τ τ k ) τ ∆ T Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
Preliminaries - Timed elastic band min ( n − 1 )∆ T B x x x k + 1 − x x x k s.t. − Ax x x k + B ( f ( x x x k ) + h ( x x k ) τ x τ τ k ) = 0 ( k = 1 , 2 , . . . , n − 1 ) ∆ T τ τ τ − ≤ τ τ τ k ≤ τ τ τ + ( k = 1 , 2 , . . . , n − 1 ) x x x 1 = x x x s , x x x n = x x x f , ∆ T > 0 χ − 1 � � q q � � �� q s y ( y y T ) y x x s = , x x f = x x ˙ q q s q 0 0 0 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 15 / 52
Preliminaries - Timed elastic band min ( n − 1 )∆ T B x x x k + 1 − x x x k s.t. − Ax x x k + B ( f ( x x x k ) + h ( x x k ) τ x τ τ k ) = 0 ( k = 1 , 2 , . . . , n − 1 ) ∆ T τ τ τ − ≤ τ τ k ≤ τ τ τ τ + ( k = 1 , 2 , . . . , n − 1 ) x x x 1 = x x x s , x x x n = x x f , ∆ T > 0 x χ − 1 � � q q � � �� q s y ( y y T ) y x s = x , x x f = x x ˙ q q s q 0 0 0 The optimization problem is solved on-line using non-linear model predictive control techniques, in the timed elastic band framework. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 15 / 52
Outline Introduction 1 Preliminaries 2 Trajectory Planner 3 Deformation Collision avoidance Track moving targets Implementation Simulations 4 Discussion and conclusion 5 Questions 6 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 16 / 52
Trajectory Planner - Deformation Deformation in time Deformation in space Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Recommend
More recommend
