trajectory path tracker

8/8/2019 Trajectory Path Tracker

1/27

1

Path and trajectory planning

Lecture 4

Mikael Norrlf

PhDCourse Robot modeling and control

Lecture 4

Up till now

Lecture 1 Rigid body motion

Representation of rotation

Homogenous transformation

Lecture 2 Kinematics

Position

Velocity via Jacobian DH parameterization

Lecture 3

Lagranges equation

(Newton Euler)

Parameter identification

Experiment design

Model structure


2/27

2


Lecture 4

Next lecture, @ABB

Feb 5

A bus (Wrnelius Buss) will leave fromLinkping University (taxi stand outsideKrallen) at 07:15.

The lectures start at ABB Robotics(ABB Robotics, Vsters, Finnsltten,bnr 331) at 10.15.

Lunch will be at Magneten ~ at 12

At 15 the bus will leave Vsters to

come back to Linkping The bus will arrive in Linkping approx

17.45

Acknowledge that youare going to attend bysending an email [email protected]

before Feb 2!!


Lecture 4

Outline

Path vs trajectory

Standard path planning techniques in industrialrobots

Trajectory generation an introduction to theproblem

More general path planning algorithms

Potential field approach

An introduction to Probabilistic Road Maps (PRMs)

Rapid and robot programming (in Part II)


3/27

3


Lecture 4

Path and trajectory planning

What is the difference between path and trajectory?

Compare: kinematics versus dynamics.

Path: Only geometricconsiderations. The way togo from config a to b.

Trajectory: Include time,i.e., consider the dynamics.


Lecture 4

Path planning

In industrial robot applications two path planningmodes can be identified

Change configuration

Perform an operation


4/27

4


Lecture 4

Path planning

In industrial robot applications two path planningmodes can be identified

Change configuration

In RAPID (ABBs programming language)

MoveJ p10, v1000, z10, tool;

Perform an operation

MoveL p20, v50, z1, tool;

MoveC p30, p40, v25, fine, tool;


Lecture 4

Rapid, robot motion instructions

Linear in joint space: MoveJ

Cartesian space: MoveL

Circle segment MoveC


5/27

5


Lecture 4

Via points

Point-to-point (fine points in Rapid). Robot has tostop.

Via points (zones in Rapid). Robot does not reach theprogrammed position.

p10

p20

p30

p40

p10

p20

p30

p40


Lecture 4

Geometric description

0.7

0.8

0.9

1

0

0.1

0.2

0.3

0.4

0.5

1.15

1.2

1.25

1.3

1.35

1.4

1.45

y

x

z

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

lc

q(lc)

4 2 4

Inversekinematics

One possibleparameterization iscubic splines.

See MSc theses, Path planning forindustrial robots by Maria Nystrm andPath generation in 6-DOF for industrialrobots by Daniel Forssman.

Chap 5 in Modeling and Control of RobotManipulators, Sciavicco and Siciliano


6/27

6


Lecture 4

Spline

Cubic spline?

q(l) = a l3 + b l2 + c l + d, l [l1, l2[

Given q(l1), q(l2), q(l1), q(l2) theparameters a, b, c and dcan be deriveduniquely.

Complete spline


Lecture 4

Orientation interpolation

The orientation along the path is interpolated to get asmooth change of orientation.

Given initial orientation (as a quaternion), q0 and finalorientation q1 compute

q01 = q1q0-1

and interpolate the angle from 0 to 01 in

qip() = where (01,s01) is the angle-axis representation of q01

The interpolation scheme is often referred to as slerpinterpolation


7/27

7


Lecture 4

70

Description of the trajectory generation problem


Lecture 4

70

Description of the trajectory generation problem


8/27

8


Lecture 4

The trajectory generation problem

Optimal control!

q0

qf


Lecture 4


Optimal control!

The path is parameterized insome index l qr(l) whichintroduces additionalconstraints.

q0

qf


9/27

9


Lecture 4

Path parameterization

Path: P(l), q(l)

Dynamic properties are given by l(t)


Lecture 4


Resulting optimization problem

Complexity:

Robots 6

A robot can have manydynamic constraints

Total of approx 700inequalities.

Complexity:

Robots 6

A robot can have manydynamic constraints

Total of approx 700inequalities.


10/27

10


Lecture 4

Dynamic scaling of trajectories

The dynamic model can be rewritten in the form

Let r = ct(time scaling). The original speed/acc deptorque is

If time scaling is applied

and with linear time scaling

=++

)q(gq)q,q(Cq)q(D

]qq)[q(

]qq)[q(q)q(Ds +=

)r(q))r(q(Drr ss += 2

ss c 2=


Lecture 4

0

0.1

0.2

0.3

0.4

0.2

0.4

0.6

0.8

10.9

0.95

1

1.05

1.1

1.15

xy

zxx

Trajectory generation problem

Dec.length

vd = 500 mm/s

vd = 1000 mm/s


11/27

11


Lecture 4

0

0.1

0.2

0.3

0.4

0.2

0.4

0.6

0.81

0.9

0.95

1

1.05

1.1

1.15

xy

zxx

Trajectory generation problem

vd = 1000 mm/s

vd = 500 mm/s


Lecture 4

0 0.5 1 1.5 2 2.5-1

-0.5

0

0.5

1

1.5

Time [sec]

q(t)

O

P

T

IM

E

R

I

N

G

O

P

T

I

M

E

R

I

N

G

Dynamisk optimering

0

0.1

0.2

0.3

0.4

0.2

0.40.6

0.8

10.9

0.95

1

1.05

1.1

1.15

xy

z

0

0.1

0.2

0.3

0.4

0.20.4

0.6

0.81

0.9

0.95

1

1.05

1.1

1.15

xy

zxx

vd = 1000 mm/s

vd = 500 mm/s

q(t)


12/27

12


Lecture 4

Application to a robot system

Use more than one robot toincrease the flexibility in anapplication.

Here with application arcweld.


Lecture 4

A more general path planning scheme


13/27

13


Lecture 4

Configuration space

A complete specification of the location of every pointon the robot is a configuration (Q).

The set of all configurations is called the configurationspace.

q + kinematics give configuration.


Lecture 4

Obstacles

The workspace of the robot is W. The subset of Woccupied by the robot isA(q).

The configuration space obstacleis defined as

with

The collision free configurations are

{ }= O)q(AQqQO

iOO =

QO\QQ =free


14/27

14


Lecture 4

Collision detection

A number of packages exists on the internet:

One example is from the group team gamma at Berkleyhttp://www.cs.unc.edu/~geom/collide/packages.shtml

Another at University of Oxfordhttp://web.comlab.ox.ac.uk/people/Stephen.Cameron/distances/

See also wikipediahttp://en.wikipedia.org/wiki/Collision_detection

An application where collision detection is used canbe found in MSc thesishttp://www.control.isy.liu.se/student/exjobb/xfiles/2050.pdf


Lecture 4

General formulation of the path planning problem

Find a collision free path from an initial configuration qsto a final configuration qf.

More formally

with (0) = qs and (1) = qf.

Examples of methods:

Path planning using potential fields

Probabilistic road maps (PRMs)

free10 Q],[:


15/27

15


Lecture 4

Slide from: http://ai.stanford.edu/~latombe/


Lecture 4

Artificial potential field

Idea: Treat the robot as a point particle in theconfiguration space, influenced by an artificialpotential field. Construct the field U such that therobot is attracted to the final configuration qfwhilebeing repelled from the boundaries of QO.

In general it is difficult/impossible to construct a fieldwithout having local minima.


16/27

16


Lecture 4

Construction of the field U

Ucan be constructed as an addition of an attractivefield and a second component that repels the robotfrom the boundary of QO

U(q) = Uatt(q) + Urep(q)

Path planning can be treated as an optimizationproblem, finding the global minimum of U(q). Onesimple approach is to use a gradient descentalgorithm.Let )q(U)q(U)q(U)q( repatt ==


Lecture 4

Construction of the field U

Comments

In general difficult to construct the potential field inconfiguration space (the field is often based on thenorm of the min length to the obstacles)

Easier to define the field in the robot workspace

For an n-link manipulator a potential field isconstructed for each DH-frame

The link between workspace and configuration spaceis the Jacobian


17/27

17


Lecture 4

The attractive field

Conic well potential (Uatt,i(q) = ||oi(q) - oi(qf)||)

Parabolic well potential (Uatt,i(q) = i ||oi(q) - oi(qf)||2)

Parabolic well potential with upper bound

and

>

=

d)q(o)q(o,d)q(o)q(od

d)q(o)q(o,)q(o)q(o)q(U

fiiifiii

fiifiii

i,att

22

2

2

1

2

1

( )

>

=d)q(o)q(o,

)q(o)q(o

)q(o)q(od

d)q(o)q(o,)q(o)q(o)q(F

fii

fii

fii

i

fiifiii

i,att


Lecture 4

The repulsive field

Criteria for the repulsive field to satisfy,

Repel the robot from obstacles (never allow the robotto collide)

Obstacles should not affect the robot when being faraway


18/27

18


Lecture 4

Path planning obstacle free path

Notice:Including only the origin of the DH-frames does not guaranteecollision free path. (Additional points can however be added.)


Lecture 4

The repulsive field

One possible choice

with

If the obstacle region is convex and b is the closest point to oi

>

=

0

0

2

0

2

1

0

11

))q(o(,

))q(o(,))q(o()q(U

i

i

i

ii,rep

))q(o(

))q(o())q(o(

)q(F iii

ii,rep

=

2

0

111

,b)q(o))q(o( ii =

b)q(o

b)q(o)x(

i

i

)q(ox i

=

=


19/27

19


Lecture 4

Comment on the convex assumption

The force vector has adiscontinuity

Distance function notdifferentiable everywhere

Can be avoided if repulsivefields of distinct obstaclesdo not overlap.


Lecture 4

Mapping the workspace forces into joint torques

If a force is exerted at the end-effector

The Jacobian can be derived in all the points oi.

Notice that the full Jacobian can be used when mappingforces and torques from workspace to torques inconfiguration space.

=FJTv


20/27

20


Lecture 4

Path construction in configuration space

Build the path using the resulting configuration space torquesand an optimization algorithm

Gradient descent algorithm

Design parameters, i, i, i,0.

Typical problem: Can get stuck in local minima.


Lecture 4

Escape local minima using randomization

When stuck in a local minimum execute a random walk

New problems:

Detect when a local minimum is reached

Define how the random walk should behave (how many steps,define the random terms, variance, distribution, )


21/27

21


Lecture 4

The cost of computing an exact representation of theconfiguration space of a multi-joint articulated object is oftenprohibitive.

But very fast algorithms exist that can check if an articulatedobject at a given configuration collides with obstacles.

Basic idea of Probabilistic Roadmaps (PRMs):Compute a very simplified representation of the freespace by sampling configurations at random.

Slidefrom:http://a

i.stanford.e

du/~latombe/

A more systematic way to build collision free paths


Lecture 4

free spaceSpace

forbidden space

Probabilistic Roadmap (PRM)

Slidefrom:http://ai.stanford.e

du/~latom

be/


22/27

22


Lecture 4

Configurations are sampled by picking coordinates at random


Slidefrom:http://a

i.stanford.e

du/~latombe/


Lecture 4

Configurations are sampled by picking coordinates at random



du/~latom

be/


23/27

23


Lecture 4

Sampled configurations are tested for collision (in workspace!)


Slidefrom:http://a

i.stanford.e

du/~latombe/


Lecture 4

The collision-free configurations are retained as milestones



du/~latom

be/


24/27

24


Lecture 4

Each milestone is linked by straight paths to its k-nearest neighbors


Slidefrom:http://a

i.stanford.e

du/~latombe/


Lecture 4

Each milestone is linked by straight paths to its k-nearest neighbors



du/~latom

be/


25/27

25


Lecture 4

The collision-free links are retained to form the PRM


Slidefrom:http://a

i.stanford.e

du/~latombe/


Lecture 4

The start and goal configurations are included as milestones



du/~latom

be/


26/27

26


Lecture 4

The PRM is searched for a path from s to g


Slidefrom:http://a

i.stanford.e

du/~latombe/


Lecture 4

Comments

In industrial robotic applications the path planningproblem is very much left to the user

New ideas from mobile robotics (potential fieldalgorithms, PRMs, etc. could be applied)

Automatic planning algorithms highly complex

The trajectory generation problem can be solved byapplying optimal control techniques

Conceptually easy to solve the offline problem

Difficult to implement online


27/27


Lecture 4

Comments

Other planning requirements in many applications

trajectory path tracker

Documents