Efficient Trajectory Reshaping in a Dynamic Environment Martin - - PowerPoint PPT Presentation

▶
efficient trajectory reshaping in a dynamic environment
SMART_READER_LITE
LIVE PREVIEW

Efficient Trajectory Reshaping in a Dynamic Environment Martin - - PowerPoint PPT Presentation

Apr 05, 2023 326 likes •738 views

Efficient Trajectory Reshaping in a Dynamic Environment Martin Biel, Mikael Norrlf KTH - Royal Institute of Technology, Linkping University, ABB MARCH 10, 2018 Motivation: Standard Approach y x Geometric path planning Martin Biel,

slide-1
SLIDE 1

Efficient Trajectory Reshaping in a Dynamic Environment

Martin Biel, Mikael Norrlöf

KTH - Royal Institute of Technology, Linköping University, ABB

MARCH 10, 2018

slide-2
SLIDE 2

Motivation: Standard Approach

x y

Geometric path planning

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15

slide-3
SLIDE 3

Motivation: Standard Approach

x y

Optimal trajectory along path

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15

slide-4
SLIDE 4

Motivation: Standard Approach

x y

Not viable in a dynamic environment

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15

slide-5
SLIDE 5

Motivation: New Approach

x y

A combined approach is required!

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 2 / 15

slide-6
SLIDE 6

Contribution

  • C++ Framework for efficient trajectory reshaping

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 3 / 15

slide-7
SLIDE 7

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

slide-8
SLIDE 8

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

slide-9
SLIDE 9

Content

  • Overview of reshaping procedure

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15

slide-10
SLIDE 10

Content

  • Overview of reshaping procedure
  • Extensions

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15

slide-11
SLIDE 11

Content

  • Overview of reshaping procedure
  • Extensions
  • Demo

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15

slide-12
SLIDE 12

Content

  • Overview of reshaping procedure
  • Extensions
  • Demo
  • Final Remarks

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 4 / 15

slide-13
SLIDE 13

Timed Elastic Band (TEB)

minimize

u u u(.)

tf s.t.                          ˙ x x x(t) = f (x x x(t),u u u(t), t) y y y(t) = g(x x x(t),u u u(t)) x x x(t) ∈ Xt u u u(t) ∈ Ut y y y(t) ∈ Yt φ0(x x x(t0),u u u(t0),y y y(t0), t0) = 0 φf (x x x(tf ),u u u(tf ),y y y(tf ), tf ) = 0

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 5 / 15

slide-14
SLIDE 14

Timed Elastic Band (TEB)

  • Discretized states and inputs are collected into a TEB set:

B := {x x x1,u u u1,x x x2,u u u2, . . . ,x x xn−1,u u un−1,x x xn, ∆T}

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 6 / 15

slide-15
SLIDE 15

Timed Elastic Band (TEB)

  • Discretized states and inputs are collected into a TEB set:

B := {x x x1,u u u1,x x x2,u u u2, . . . ,x x xn−1,u u un−1,x x xn, ∆T}

  • Approximated system dynamics:

x x xk+1 − x x xk ∆T = f (x x xk,u u uk)

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 6 / 15

slide-16
SLIDE 16

Timed Elastic Band (TEB)

  • Discretized states and inputs are collected into a TEB set:

B := {x x x1,u u u1,x x x2,u u u2, . . . ,x x xn−1,u u un−1,x x xn, ∆T}

  • Approximated system dynamics:

x x xk+1 − x x xk ∆T = f (x x xk,u u uk)

  • 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

slide-17
SLIDE 17

Timed Elastic Band (TEB)

Problem reformulation in TEB space minimize

B

(n − 1)∆T s.t. x x xk+1 − x x xk ∆T − f (x x xk,u u uk) = 0 x x xk ∈ Xk u u uk ∈ Uk g(x x xk,u u uk) ∈ Yk φs(x x x1,u u u1, g(x x x1,u u u1)) = 0 φf (x x xn,u u un, g(x x xn,u u un)) = 0 ∆T > 0, k ∈ [1, n − 1]

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 7 / 15

slide-18
SLIDE 18

Switching Strategy

  • No convergence guarantees in TEB formulation

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15

slide-19
SLIDE 19

Switching Strategy

  • No convergence guarantees in TEB formulation
  • Switch to standard NMPC when target is close enough

minimize

B\{∆T } n−1

  • k=1

x x xk − x x xf 2 s.t. x x xk+1 − x x xk ∆¯ T − f (x x xk,u u uk) = 0 x x xk ∈ Xk u u uk ∈ Uk g(x x xk,u u uk) ∈ Yk φs(x x x1,u u u1, g(x x x1,u u u1)) = 0 φf (x x xn,u u un, g(x x xn,u u un)) = 0

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15

slide-20
SLIDE 20

Switching Strategy

  • No convergence guarantees in TEB formulation
  • Switch to standard NMPC when target is close enough

minimize

B\{∆T } n−1

  • k=1

x x xk − x x xf 2 s.t. x x xk+1 − x x xk ∆¯ T − f (x x xk,u u uk) = 0 x x xk ∈ Xk u u uk ∈ Uk g(x x xk,u u uk) ∈ Yk φs(x x x1,u u u1, g(x x x1,u u u1)) = 0 φf (x x xn,u u un, g(x x xn,u u un)) = 0

  • Gives convergence under certain assumptions

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15

slide-21
SLIDE 21

Switching Strategy

x y Target Quasi time-optimal region Tracking region

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 8 / 15

slide-22
SLIDE 22

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

slide-23
SLIDE 23

Trajectory Reshaping Procedure

input: B - Current trajectory as TEB set O - Environment information

  • utput: B∗ - Reshaped trajectory

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 9 / 15

slide-24
SLIDE 24

Trajectory Reshaping Procedure

input: B - Current trajectory as TEB set O - Environment information

  • utput: B∗ - Reshaped trajectory

1: procedure ReshapeTrajectory 2:

˜ B ← DeformInTime(B)

3:

P ← FormulateOptimizationProblem( ˜ B,O)

4:

B∗ ← DeformInSpace(P, ˜ B)

5:

return B∗

6: end procedure

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 9 / 15

slide-25
SLIDE 25

Extensions

  • Track moving targets
  • Obstacle avoidance
  • Multiple trajectories

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 10 / 15

slide-26
SLIDE 26

Extension: Obstacle avoidance

Kj,¯

σop :=

  • k :
  • g(x

x xk,u u uk) − Oj

  • 2 ≤ (¯

σop + rj)2 , j = 1, . . . , m

x y

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15

slide-27
SLIDE 27

Extension: Obstacle avoidance

minimize

B

(n − 1)∆T −

m

  • j=1
  • k∈Kj,¯

σop

  • g(x

x xk,u u uk) − Oj

  • 2

s.t. x x xk+1 − x x xk ∆T − f (x x xk,u u uk) = 0 x x xk ∈ Xk u u uk ∈ Uk g(x x xk,u u uk) ∈ Yk φs(x x x1,u u u1, g(x x x1,u u u1)) = 0 φf (x x xn,u u un, g(x x xn,u u un)) = 0 ∆T > 0, k ∈ [1, n − 1]

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15

slide-28
SLIDE 28

Extension: Obstacle avoidance

x y

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 11 / 15

slide-29
SLIDE 29

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

slide-30
SLIDE 30

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

slide-31
SLIDE 31

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

slide-32
SLIDE 32

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

slide-33
SLIDE 33

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

slide-34
SLIDE 34

Implementation

  • Framework for trajectory reshaping implemented in C++

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 13 / 15

slide-35
SLIDE 35

Implementation

  • Framework for trajectory reshaping implemented in C++
  • Derivative information is computed using automatic differentiation

⇒ Supports arbitrary system dynamics

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 13 / 15

slide-36
SLIDE 36

Implementation

  • Framework for trajectory reshaping implemented in C++
  • Derivative information is computed using automatic differentiation

⇒ Supports arbitrary system dynamics

  • In-house SQP solver
  • qpOASES for QP subproblems
  • Exploits the sparsity in the emerging Hessians & Jacobians

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 13 / 15

slide-37
SLIDE 37

Implementation

  • Framework for trajectory reshaping implemented in C++
  • Derivative information is computed using automatic differentiation

⇒ Supports arbitrary system dynamics

  • In-house SQP solver
  • qpOASES for QP subproblems
  • Exploits the sparsity in the emerging Hessians & Jacobians
  • Simulator implemented in the Julia programming language

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 13 / 15

slide-38
SLIDE 38

Simulation

Demo: SCARA Model

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 14 / 15

slide-39
SLIDE 39

Final Remarks

Summary

  • C++ Framework for efficient trajectory reshaping
  • Heuristic strategy for solving optimal control problems in real-time
  • Simulation environment

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 15 / 15

slide-40
SLIDE 40

Final Remarks

Summary

  • C++ Framework for efficient trajectory reshaping
  • Heuristic strategy for solving optimal control problems in real-time
  • Simulation environment

Ongoing work

  • Proof of concept. Initial results are promising.
  • Suitable for a pick-and-place scenario on a conveyor
  • Next step is to employ the procedure in an embedded setting

Martin Biel, Mikael Norrlöf (KTH,LiU,ABB) Efficient Trajectory Reshaping 15 / 15