Convex Optimization approach for Time-Optimal Path Tracking
of Robots with Speed Dependent Constraints
Tohid Ardeshiri, Mikael Norrlöf, Johan Löfberg, Anders Hansson
for s ∈ [0, 1], where a(s) and b(s) represents
−0.8
b(s) = ṡ ,
a(s) = s̈.
0.75
−0.7
0.40
−0.6
0.10
0.15
−0.5
0.45
0.25
0.05
0.05
0
0.55
Convex speed dependent constraints
0.85
0.90
0.95
−0.1
In general, two limitations have to be considered in the drive system:
1.00
0
−0.1
−0.2
−0.3
−0.4
0.75
−0.5
−0.6
x−coordinate (m)
−0.7
0.8
Scenario 1
2
2.5
0.80
−0.2
0.65
0.7
path coordinate (−)
Joint 1
0.20
−0.3
0.6
3
0.60
−0.8
−0.9
The trajectory q(t) and its derivatives can now be expressed as,
1. The armature current is limited due to the heat produced in the motor,
2. The DC-voltage that can be used to drive the motors is bounded.
q̇(s(t)) = q (s(t))ṡ(t),
0
00
2
q̈(s(t)) = q (s(t))s̈(t) + q (s(t))ṡ (t),
2
1.5
1
0.5
0
200
0
−200
−400
−3
−2
−1
0
1
2
3
500
0
−500
−4
−3
−2
−1
0
1
2
3
4
velocity of joint 2 q̇2 (rad/s)
200
100
0
−100
−200
−5
0
5
velocity of joint 3 q̇3 (rad/s)
−0.5
−1
−1.5
A typical torque versus speed capability specification for a brushless DCmotor is shown in the two left diagrams below.
0
400
velocity of joint 1 q̇1 (rad/s)
0.15
0.1
• The discretized problem can be posed as a Second Order Cone Program.
0.55
0.2
0.50
0.35
−0.4
0.3
0.65
0.30
0.00
0.35
joint velocity (rad/sec)
y−coordinate (m)
2
0.70
Pseudo−velocity
0.25
Path in the plane parallel to the XY−plane
−0.9
torque of joint 1 τ1 (N.m)
The path is given as a function, q(s), in the joint space, where s is an
index function that parameterize the path as shown below.
• Scenario 2: An affine set of constraints is imposed on each one of
the first three joints. This set of constraint is approximated by another
affine set of constraint which is convex with respect to τi and q̇i2. The
figure below shows the resulting optimal joint torque versus joint speed
square.
torque of joint 2 τ2 (N.m)
Problem formulation
• Scenario 1: Rotational speed constraints are imposed on joints 1 − 3.
The calculated optimal joint torque versus its angular speed for this
scenario is shown in yellow in the figure below.
torque of joint 3 τ3 (N.m)
• The minimum time trajectory is computed in a highly efficient way
using convex optimization.
• Compared to previous formulations of the problem, speed dependent
constraints are added.
• The minimum time optimization can easily be changed to optimization
with respect to energy.
Example
pseudo−velocity (1/s)
Summary
• The optimization problem can be reformulated into a convex optimization problem when dynamic friction is neglected.
Z 1
1
p
ds
minimize
a(.),b(.),τ (.) 0
b(s)
s.t. τ (s) = m(s)a(s) + c(s)b(s) + g(s)
2
b(0) = s˙0
b(1) = s˙T 2
0
b (s) = 2a(s)
b(s) ≥ 0
b(s) ≤ b(s)
τ (s) ≤ τ (s) ≤ τ (s)
f (s) ≤ f (s)a(s) + h(s)b(s) ≤ f (s)
−2
1
2
3
4
5
time [sec]
6
7
8
9
feasible set of scenario 2 before approximation
feasible set of scenario 1 before approximation
Scenario 1: Optimal torque vs optimal velocity
Scenario 2: optimal torque vs optimal velocity
τi
τ i,affine
The time-optimal path tracking problem for the robotic manipulator can
be expressed with respect to the scalar path coordinate s as,
2
q̇ i,affine
q̇i2
minimize T
T,s(.),τ (.)
2
subject to τ (t) = m(s(t))s̈(t) + c(s)ṡ (t) + g(s(t))
s(0) = 0, s(T ) = 1
ṡ(0) = ṡ0, ṡ(T ) = ṡT
ṡ(t) ≥ 0, ṡ(t) ≤ ṡ(s)
τ (s) ≤ τ (s) ≤ τ (s)
τ i,affine
To be able to guarantee a convex constraint, the true feasible set with
respect to q̇i and τi is approximated by a set which is convex with respect
to q̇i2 and τi. In the right most figure above the approximated constraint
is illustrated for the constraint in the middle diagram.
Results
• The algorithm utilizes the available additional torque and speed due to
the speed dependent constraints for the three actuators.
• Some of the extra torque cannot be utilized due to the convex approximation which leads to cut-off of the non-convex part of the feasible
set.
• The cycle time for the given trajectory is decreased from 9.83s to 9.38s,
which means a cycle time reduction of 4.6%.
for t ∈ [0, T ].
LINK-SIC Linköping Center for Sensor Informatics and Control — A Vinnova Industry Excellence Center
http://www.linksic.isy.liu.se/