Efficient Trajectory Reshaping in a Dynamic Environment Martin Biel, Mikael Norrlöf KTH - Royal Institute of Technology, Linköping University, ABB MARCH 10, 2018
Motivation: Standard Approach y x Geometric path planning Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15
Motivation: Standard Approach y x Optimal trajectory along path Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15
Motivation: Standard Approach y x Not viable in a dynamic environment Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15
Motivation: New Approach y x A combined approach is required! Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15
Contribution • C++ Framework for efficient trajectory reshaping Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 3 / 15
Contribution • C++ Framework for efficient trajectory reshaping • Heuristic strategy for solving optimal control problems in real-time Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 3 / 15
Contribution • C++ Framework for efficient trajectory reshaping • Heuristic strategy for solving optimal control problems in real-time • Simulation environment. Evaulation on SCARA type robot Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 3 / 15
Content • Overview of reshaping procedure Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15
Content • Overview of reshaping procedure • Extensions Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15
Content • Overview of reshaping procedure • Extensions • Demo Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15
Content • Overview of reshaping procedure • Extensions • Demo • Final Remarks Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15
Timed Elastic Band (TEB) x x ( t ) = f ( x ˙ x x ( t ) ,u u u ( t ) , t ) x y y y ( t ) = g ( x x ( t ) ,u x u ( t )) u x x x ( t ) ∈ X t minimize t f s.t. u u ( t ) ∈ U t u u u ( . ) u y y y ( t ) ∈ Y t φ 0 ( x x x ( t 0 ) ,u u u ( t 0 ) ,y y ( t 0 ) , t 0 ) = 0 y φ f ( x x ( t f ) ,u u ( t f ) ,y y ( t f ) , t f ) = 0 x u y Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 5 / 15
Timed Elastic Band (TEB) • Discretized states and inputs are collected into a TEB set: B := { x x x 1 ,u u u 1 ,x x x 2 ,u u 2 , . . . ,x u x x n − 1 ,u u u n − 1 ,x x x n , ∆ T } Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 6 / 15
Timed Elastic Band (TEB) • Discretized states and inputs are collected into a TEB set: B := { x x x 1 ,u u u 1 ,x x x 2 ,u u u 2 , . . . ,x x x n − 1 ,u u u n − 1 ,x x n , ∆ T } x • Approximated system dynamics: x x k +1 − x x x x k = f ( x x x k ,u u u k ) ∆ T Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 6 / 15
Timed Elastic Band (TEB) • Discretized states and inputs are collected into a TEB set: B := { x x x 1 ,u u u 1 ,x x 2 ,u x u u 2 , . . . ,x x x n − 1 ,u u u n − 1 ,x x x n , ∆ T } • Approximated system dynamics: x x k +1 − x x x x k = f ( x x x k ,u u u k ) ∆ T • The inclusion of the time increment allows the trajectory to be reshaped in both space and time Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 6 / 15
Timed Elastic Band (TEB) Problem reformulation in TEB space minimize ( n − 1)∆ T B x x x k +1 − x x x k s.t. − f ( x x u u k ) = 0 x k ,u ∆ T x x k ∈ X k x u u u k ∈ U k g ( x u k ) ∈ Y k x x k ,u u φ s ( x x u u 1 , g ( x x u u 1 )) = 0 x 1 ,u x 1 ,u φ f ( x x x n ,u u u n , g ( x x x n ,u u u n )) = 0 ∆ T > 0 , k ∈ [1 , n − 1] Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 7 / 15
Switching Strategy • No convergence guarantees in TEB formulation Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15
Switching Strategy • No convergence guarantees in TEB formulation • Switch to standard NMPC when target is close enough n − 1 � x f � 2 minimize � x x x k − x x B\{ ∆ T } k =1 x x k +1 − x x x x k s.t. − f ( x x u u k ) = 0 x k ,u ∆¯ T x k ∈ X k x x u u k ∈ U k u g ( x x k ,u x u u k ) ∈ Y k φ s ( x u 1 , g ( x u 1 )) = 0 x 1 ,u x u x x 1 ,u u φ f ( x x x n ,u u u n , g ( x x x n ,u u u n )) = 0 Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15
Switching Strategy • No convergence guarantees in TEB formulation • Switch to standard NMPC when target is close enough n − 1 � x f � 2 minimize � x x x k − x x B\{ ∆ T } k =1 x x k +1 − x x x x k s.t. − f ( x x u u k ) = 0 x k ,u ∆¯ T x k ∈ X k x x u u k ∈ U k u g ( x x k ,u x u u k ) ∈ Y k φ s ( x u 1 , g ( x u 1 )) = 0 x 1 ,u x u x x 1 ,u u φ f ( x x x n ,u u u n , g ( x x x n ,u u u n )) = 0 • Gives convergence under certain assumptions Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15
Switching Strategy y Target Quasi time-optimal region Tracking region x Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15
Trajectory Reshaping Procedure input: B - Current trajectory as TEB set O - Environment information Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 9 / 15
Trajectory Reshaping Procedure input: B - Current trajectory as TEB set O - Environment information output: B ∗ - Reshaped trajectory Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 9 / 15
Trajectory Reshaping Procedure input: B - Current trajectory as TEB set O - Environment information output: B ∗ - Reshaped trajectory 1: procedure ReshapeTrajectory ˜ B ← DeformInTime( B ) 2: P ← FormulateOptimizationProblem( ˜ B , O ) 3: B ∗ ← DeformInSpace( P , ˜ B ) 4: return B ∗ 5: 6: end procedure Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 9 / 15
Extensions • Track moving targets • Obstacle avoidance • Multiple trajectories Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 10 / 15
Extension: Obstacle avoidance � 2 ≤ (¯ � σ op + r j ) 2 � � � σ op := k : � g ( x x u k ) − O j u j = 1 , . . . , m K j, ¯ x k ,u , y x Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15
Extension: Obstacle avoidance m � 2 � � � � minimize ( n − 1)∆ T − � g ( x u k ) − O j x x k ,u u B j =1 k ∈ K j, ¯ σop x k +1 − x x x x x k s.t. − f ( x x x k ,u u u k ) = 0 ∆ T x x x k ∈ X k u k ∈ U k u u g ( x x x k ,u u u k ) ∈ Y k φ s ( x x x 1 ,u u u 1 , g ( x x 1 ,u x u u 1 )) = 0 φ f ( x x u u n , g ( x x u u n )) = 0 x n ,u x n ,u ∆ T > 0 , k ∈ [1 , n − 1] Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15
Extension: Obstacle avoidance y x Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15
Extension: Multiple Trajectories • Non-convex problem in general ⇒ The reshaper might get stuck in local optima Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 12 / 15
Extension: Multiple Trajectories • Non-convex problem in general ⇒ The reshaper might get stuck in local optima Solution: Reshape multiple trajectory candidates Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 12 / 15
Extension: Multiple Trajectories • Non-convex problem in general ⇒ The reshaper might get stuck in local optima Solution: Reshape multiple trajectory candidates • The trajectories are independent and can be planned in parallel Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 12 / 15
Extension: Multiple Trajectories • Non-convex problem in general ⇒ The reshaper might get stuck in local optima Solution: Reshape multiple trajectory candidates • The trajectories are independent and can be planned in parallel • Choose trajectory according to some performance critera (e.g. transition time + collisions) Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 12 / 15
Extension: Multiple Trajectories • Non-convex problem in general ⇒ The reshaper might get stuck in local optima Solution: Reshape multiple trajectory candidates • The trajectories are independent and can be planned in parallel • Choose trajectory according to some performance critera (e.g. transition time + collisions) • Eventually commit to traversed trajectory and drop others Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 12 / 15
Implementation • Framework for trajectory reshaping implemented in C++ Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 13 / 15
Recommend
More recommend
