Sensor-based trajectory optimization ABB Robotics Master thesis - - PowerPoint PPT Presentation
Sensor-based trajectory optimization ABB Robotics Master thesis Martin Biel Supervisor: Mikael Norrlf Examiner: Xiaoming Hu June 9, 2016 Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 1 / 52 Outline Introduction
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
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 2 / 52
Geometric path computed on before hand.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 3 / 52
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
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
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
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
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
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
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
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
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
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
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 7 / 52
x y
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 8 / 52
x y q1 q2 Q - Configuration space
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 9 / 52
x y q1 q2 (x, y) Q - Configuration space O - Operational space
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 10 / 52
x y q1 q2 (x, y) Q - Configuration space O - Operational space W - Workspace
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 11 / 52
Forward kinematics: y y y = χy(q q q) Inverse kinematics: q q q = χ−1
y (y
y y) Velocity Jacobian: v v v = J(q q q) ˙ q q q Dynamics: M(q q q(t))¨ q q q(t) + C(q q q(t), ˙ q q q(t)) ˙ q q q(t) + g(q q q(t)) = τ τ τ(t)
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 12 / 52
Time minimizing formulation min
τ τ τ(.) T s.t.
M(q q q(t))¨ q q q(t) + C(q q q(t), ˙ q q q(t)) ˙ q q q(t) + g(q q q(t)) = τ τ τ(t) q q q(t) ∈ Q τ τ τ − ≤ τ τ τ(t) ≤ τ τ τ + y y y(t) = χy(q q q(t)) y y y(0) = y y y0, ˙ y y y(0) = ˙ y y y0 y y y(T) = y y yT, ˙ y y y(T) = ˙ y y yT
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 13 / 52
Introduce the state vector x x x(t) = q q q(t) ˙ q q q(t)
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
Introduce the state vector x x x(t) = q q q(t) ˙ q q q(t)
Discretize the trajectory into a so called Timed Elastic Band (TEB) set B := {x x x1,τ τ τ 1,x x x2,τ τ τ 2, . . . ,x x xn−1,τ τ τ n−1,x x xn, ∆T}. Note that n and ∆T are NOT fixed.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
Introduce the state vector x x x(t) = q q q(t) ˙ q q q(t)
Discretize the trajectory into a so called Timed Elastic Band (TEB) set B := {x x x1,τ τ τ 1,x x x2,τ τ τ 2, . . . ,x x xn−1,τ τ τ n−1,x x xn, ∆T}. Note that n and ∆T are NOT fixed. Determine the system dynamics for x x x(t) and approximate them using forward Euler, x x xk+1 − x x xk ∆T = Ax x xk + B(f(x x xk) + h(x x xk)τ τ τ k)
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 14 / 52
min
B
(n − 1)∆T s.t. x x xk+1 − x x xk ∆T − Ax x xk + B(f(x x xk) + h(x x xk)τ τ τ k) = 0 (k = 1, 2, . . . , n − 1) τ τ τ − ≤ τ τ τ k ≤ τ τ τ + (k = 1, 2, . . . , n − 1) x x x1 = x x xs, x x xn = x x xf, ∆T > 0
x xs = q q qs ˙ q q qs
x x xf =
y (y
y yT)
Sensor-based trajectory optimization June 9, 2016 15 / 52
min
B
(n − 1)∆T s.t. x x xk+1 − x x xk ∆T − Ax x xk + B(f(x x xk) + h(x x xk)τ τ τ k) = 0 (k = 1, 2, . . . , n − 1) τ τ τ − ≤ τ τ τ k ≤ τ τ τ + (k = 1, 2, . . . , n − 1) x x x1 = x x xs, x x xn = x x xf, ∆T > 0
x xs = q q qs ˙ q q qs
x x xf =
y (y
y yT)
predictive control techniques, in the timed elastic band framework.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 15 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner Deformation Collision avoidance Track moving targets Implementation
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 16 / 52
Deformation in time Deformation in space
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Deformation in time During each control cycle, the following TEB update is performed ¯ ITEB times TEB update i− Insert a new state if ∆Ti > ∆¯ Tref + ∆¯ Thyst ∧ ni < ¯ nmax Remove a state if ∆Ti < ∆¯ Tref − ∆¯ Thyst ∧ ni > ¯ nmin Leave the TEB unchanged otherwise Deformation in space
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Deformation in time During each control cycle, the following TEB update is performed ¯ ITEB times TEB update i− Insert a new state if ∆Ti > ∆¯ Tref + ∆¯ Thyst ∧ ni < ¯ nmax Remove a state if ∆Ti < ∆¯ Tref − ∆¯ Thyst ∧ ni > ¯ nmin Leave the TEB unchanged otherwise Deformation in space After each TEB update, the trajectory is improved by running an
ISQP iterations with respect to the underlying non-linear optimization problem.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Deformation in time During each control cycle, the following TEB update is performed ¯ ITEB times TEB update i− Insert a new state if ∆Ti > ∆¯ Tref + ∆¯ Thyst ∧ ni < ¯ nmax Remove a state if ∆Ti < ∆¯ Tref − ∆¯ Thyst ∧ ni > ¯ nmin Leave the TEB unchanged otherwise Deformation in space After each TEB update, the trajectory is improved by running an
ISQP iterations with respect to the underlying non-linear optimization problem. The underlying solver is based on Sequential Quadratic Programming and employs line-search through an l1 merit function.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Deformation in time During each control cycle, the following TEB update is performed ¯ ITEB times TEB update i− Insert a new state if ∆Ti > ∆¯ Tref + ∆¯ Thyst ∧ ni < ¯ nmax Remove a state if ∆Ti < ∆¯ Tref − ∆¯ Thyst ∧ ni > ¯ nmin Leave the TEB unchanged otherwise Deformation in space After each TEB update, the trajectory is improved by running an
ISQP iterations with respect to the underlying non-linear optimization problem. The underlying solver is based on Sequential Quadratic Programming and employs line-search through an l1 merit function. Automatic differentiation is used to compute the required gradients and Jacobians.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
Deformation in time During each control cycle, the following TEB update is performed ¯ ITEB times TEB update i− Insert a new state if ∆Ti > ∆¯ Tref + ∆¯ Thyst ∧ ni < ¯ nmax Remove a state if ∆Ti < ∆¯ Tref − ∆¯ Thyst ∧ ni > ¯ nmin Leave the TEB unchanged otherwise Deformation in space After each TEB update, the trajectory is improved by running an
ISQP iterations with respect to the underlying non-linear optimization problem. The underlying solver is based on Sequential Quadratic Programming and employs line-search through an l1 merit function. Automatic differentiation is used to compute the required gradients and Jacobians. In total, ¯ ITEB ·¯ ISQP optimization iterations are performed each cycle.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 17 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner Deformation Collision avoidance Track moving targets Implementation
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 18 / 52
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 19 / 52
min
B
(n − 1)∆T −
m
σop
||χy(Cx x xk) − Oj||2 s.t. x x xk+1 − x x xk ∆T − Ax x xk + B(f(x x xk) + g(x x xk)τ τ τ k) = 0 (k = 1, . . . , n − 1) τ τ τ − ≤ τ τ τ k ≤ τ τ τ + (k = 1, . . . , n − 1) x x x1 = x x xs, x x xn = x x xf, ∆T > 0
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 20 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner Deformation Collision avoidance Track moving targets Implementation
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 21 / 52
The objective is to track some moving target, represented here by the curve y y ytg(t) in operational space.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 22 / 52
The objective is to track some moving target, represented here by the curve y y ytg(t) in operational space. In configuration space, the end-condition becomes q q qf = χ−1
y (y
y ytg(T)) ˙ q q qf = J(q q qf)−1 ˙ y y ytg(T)
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 22 / 52
The objective is to track some moving target, represented here by the curve y y ytg(t) in operational space. In configuration space, the end-condition becomes q q qf = χ−1
y (y
y ytg(T)) ˙ q q qf = J(q q qf)−1 ˙ y y ytg(T) Alternatively, the target state is replaced with the prediction y y ytg
i + (ni−1 − 1)∆Tiv
v v
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 22 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner Deformation Collision avoidance Track moving targets Implementation
4
Simulations
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 23 / 52
Algorithm 1 Trajectory Planning Input: q q qs - current state; ˙ q q qs - current velocity; y y yf - target; ˙ y y yf - target velocity; O - obstacle information Output: (Sub-)optimal control input τ τ τ
1: procedure PLANTRAJECTORY 2:
repeat
3:
(q q qs, ˙ q q qs, y y yf, ˙ y y yf) ← READSENSORINPUT
4:
O ← INFORMABOUTOBSTACLES
5:
for each iteration 1 to ¯ ITEB do
6:
B ← DEFORMINTIME(B)
7:
P ←SETUPUNDERLYINGPROBLEM(B, O, q q qs, ˙ q q qs, y y yf, ˙ y y yf)
8:
for each iteration 1 to ¯ ISQP do
9:
B ← SQPSOLVE(B,P)
10:
end for
11:
end for
12:
τ ← APPLYCONTROL(B)
13:
until target has been reached
14: end procedure
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 24 / 52
The trajectory planner is implemented as a software package in C++.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
The trajectory planner is implemented as a software package in C++. Notable third-party libraries:
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
The trajectory planner is implemented as a software package in C++. Notable third-party libraries:
◮ Eigen - for matrix/vector storage and linear algebra operations. Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
The trajectory planner is implemented as a software package in C++. Notable third-party libraries:
◮ Eigen - for matrix/vector storage and linear algebra operations. ◮ qpOASES - for solving the arising quadratic subproblems during
the SQP procedure.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
The trajectory planner is implemented as a software package in C++. Notable third-party libraries:
◮ Eigen - for matrix/vector storage and linear algebra operations. ◮ qpOASES - for solving the arising quadratic subproblems during
the SQP procedure.
◮ CppAD - for automatic differentiation. Used to compute gradients
and Jacobians.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
The trajectory planner is implemented as a software package in C++. Notable third-party libraries:
◮ Eigen - for matrix/vector storage and linear algebra operations. ◮ qpOASES - for solving the arising quadratic subproblems during
the SQP procedure.
◮ CppAD - for automatic differentiation. Used to compute gradients
and Jacobians.
In specific applications, the trajectory planner is extended in a subclass that configures the planner and provides the appropriate system dynamics.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 25 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations Model Scenario 1: Simple target Scenario 2: Avoid obstacles Scenario 3: Track moving target Scenario 4: Pick and place
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 26 / 52
x y q1 m1 m2 q2 l1 l2
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 27 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations Model Scenario 1: Simple target Scenario 2: Avoid obstacles Scenario 3: Track moving target Scenario 4: Pick and place
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 28 / 52
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 29 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories, taken every 0.5s, together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Simple Target - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot
0.5 1 1.5 2 2.5 3 3.5 4 4.5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Simple Target - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Trajectory planning procedure for the PlanarElbow model with a simple stationary target at (−1, 1) and aiming to minimize transition time. The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 30 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Simple Target - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
1 2 3 4 5 6 7 Time t [s]
1 2 Control Input τ [Nm] Simple Target - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Trajectory planning procedure for the PlanarElbow model with a simple stationary target at (−1, 1) and aiming to minimize energy. The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 31 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations Model Scenario 1: Simple target Scenario 2: Avoid obstacles Scenario 3: Track moving target Scenario 4: Pick and place
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 32 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
0.5 1 1.5 2 2.5 3 3.5 4 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: A single obstacle with radius 0.3m is placed at (0.5, 1.8). The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 33 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
0.5 1 1.5 2 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 34 / 52
1 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
1 2 3 4 5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values, but with "obstacleCloseProximity" : 1.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 35 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values, but with "obstacleCloseProximity" : 0.1.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 36 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
1 2 3 4 5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values, but with "referenceTime" : 0.05 and "Iteb" :
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 37 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
1 2 3 4 5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values, but with "Isqp" : 4.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 38 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot.
1 2 3 4 5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Two obstacles are added to the workspace: one at (0.5, 1.8) with radius 0.3m and one at (−0.2, 1, 3) with radius 0.2m. The planner was configured with the default values, but with "Isqp" : 4 and "multipleTrajectories": true.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 39 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot and the moving
0.5 1 1.5 2 2.5 3 3.5 4 4.5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: A single obstacle with radius 0.2m is placed at (1, 1.2), and moving in the direction (0.94, 0.35) with speed 0.1m/s. The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 40 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid Obstacles - Robot movement Initial position Intermediate positions Final Position
(b) The movement pattern of the robot and the moving
0.5 1 1.5 2 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Avoid Obstacles - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Three obstacles are added to the workspace: One at (1, 1.2) with radius 0.2m, moving in the direction (0.94, 0.35) with speed 0.1m/s; One stationary obstacle at (−1, −1) with radius 0.4m; and finally one at (0.5, 0.5) with radius 0.3m, moving in the direction (0, 1) with speed 5m/s. The planner was configured with the default values, but with "multipleTrajectories" : true.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 41 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations Model Scenario 1: Simple target Scenario 2: Avoid obstacles Scenario 3: Track moving target Scenario 4: Pick and place
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 42 / 52
1 2 3 Position x [m]
0.5 1 1.5 2 Position y [m] Avoid obstacles - Trajectories Planned trajectories Target trajectory Actual trajectory
(a) Snapshots of the intermediate planned trajectories together with the actual realized trajectory.
0.5 1 1.5 2 Position x [m]
0.5 1 1.5 2 Position y [m] Moving Target - Robot movement Target trajectory Initial position Intermediate positions Final Position
(b) The movement pattern of the robot and the moving target.
0.5 1 1.5 2 2.5 3 3.5 4 4.5 Time t [s]
0.5 1 1.5 2 2.5 Control Input τ [Nm] Moving Target - Control Input Joint 1 Joint 2
(c) The control input signal that was applied during the procedure. Figure: Trajectory planning procedure for the PlanarElbow model with a moving target initially located at (−1, −1) and moving in the direction (0, 1) with speed 0.1m/s. The aim is to minimize transition time. The planner was configured with the default values.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 43 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations Model Scenario 1: Simple target Scenario 2: Avoid obstacles Scenario 3: Track moving target Scenario 4: Pick and place
5
Discussion and conclusion
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 44 / 52
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 45 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations
5
Discussion and conclusion Evaluation of results Improvements and future work Final remarks
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 46 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints. When employing the time-minimizing strategy, the resulting trajectories often appear to be quasi time-optimal.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints. When employing the time-minimizing strategy, the resulting trajectories often appear to be quasi time-optimal. The results of the pick-and-place scenario shows the planners potential as an alternative to the two-step approach when
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints. When employing the time-minimizing strategy, the resulting trajectories often appear to be quasi time-optimal. The results of the pick-and-place scenario shows the planners potential as an alternative to the two-step approach when
The results of the more complicated examples indicate that many
explored further.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints. When employing the time-minimizing strategy, the resulting trajectories often appear to be quasi time-optimal. The results of the pick-and-place scenario shows the planners potential as an alternative to the two-step approach when
The results of the more complicated examples indicate that many
explored further. Many of the examples, and the appararent parameter sensitivity, indicate that the planner needs to be tailored for use in specific applications.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
The planner is, in many different scenarios, successful in generating a feasible trajectory in relation to the different goals and the provided constraints. When employing the time-minimizing strategy, the resulting trajectories often appear to be quasi time-optimal. The results of the pick-and-place scenario shows the planners potential as an alternative to the two-step approach when
The results of the more complicated examples indicate that many
explored further. Many of the examples, and the appararent parameter sensitivity, indicate that the planner needs to be tailored for use in specific applications. More work required to make the planner real-time capable.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 47 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations
5
Discussion and conclusion Evaluation of results Improvements and future work Final remarks
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 48 / 52
Utilize the sparsity of the underlying optimization problem.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 49 / 52
Utilize the sparsity of the underlying optimization problem. Explore trust-region methods as an alternative to line-search in the SQP procedure.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 49 / 52
Utilize the sparsity of the underlying optimization problem. Explore trust-region methods as an alternative to line-search in the SQP procedure. Robust MPC techniques to counter issues that arise from inaccurate models.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 49 / 52
1
Introduction
2
Preliminaries
3
Trajectory Planner
4
Simulations
5
Discussion and conclusion Evaluation of results Improvements and future work Final remarks
6
Questions
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 50 / 52
A trajectory planning procedure has been implemented and presented in this work, and it looks promising for future use.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 51 / 52
A trajectory planning procedure has been implemented and presented in this work, and it looks promising for future use. The intended application has been robotic manipulators, but the planner can be extended to other optimal control problems.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 51 / 52
A trajectory planning procedure has been implemented and presented in this work, and it looks promising for future use. The intended application has been robotic manipulators, but the planner can be extended to other optimal control problems. The planner is successively applied in simple scenarios, but needs further work before it can be used in a real applications.
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 51 / 52
Martin Biel (KTH) Sensor-based trajectory optimization June 9, 2016 52 / 52