chapter 5. trajectory planning and control...

Robotics (KAU AME)

로봇공학, Chapter 5

1

Chapter 5. Trajectory Planning and Control

경로 계획 및 제어

1) Trajectory Planning in Joint space

2) Trajectory Planning in Cartesian space

3) 3차 및 5차 다항식 궤적 (3rd order & 5th order

polynomial trajectory)

4) Robot dynamics and control simulation

로봇공학 (Robotics)

Robotics (KAU AME)


2

5.1 Introduction

Path (or trajectory) Planning :

• Path and trajectory planning means the way that a robot is

moved from one location to another in a controlled manner.

• The sequence of movements for a controlled movement between

motion segment, in straight-line motion or in sequential motions.

• It requires the use of both kinematics and dynamics of robots.

- Kinematic planning

- Dynamic planning

- Optimization

Robotics (KAU AME)


3

5.2 Path VS. Trajectory

◆ Path: A sequence of robot configurations in a particular order

without regard to the timing of these configurations.

◆ Trajectory: A sequence of robot configurations with specified times

⚫ It concerned about when each part of the path must be attained,

thus specifying timing.

⚫ 동일한 Path에 대하여 같은 위치를 지나는 시간에 따라 여러 trajectory

산출 가능

Fig. 5.1 Sequential robot movements in a path.

Robotics (KAU AME)


4

5.3 Joint Space VS. Cartesian Space

• Joint-space description: - The description of the motion to be made by the robot by its joint values.

- The motion between the two points is unpredictable.

• Cartesian space description: - The motion between the two points is known at all times and controllable.

- It is easy to visualize the trajectory, but is difficult to ensure that singularity.

Fig. 5.2 Sequential motions of a robot to follow a straight line.

Fig. 5.3 Cartesian-space trajectory (a) The trajectory specified in Cartesian coordinates may force the robot to run into itself, and (b) the trajectory may requires a sudden change in the joint angles.

Robotics (KAU AME)


5

Task space trajectory → Joint space trajectory

▪ Joint space trajectory vs. Cartesian space trajectory

• Joint space trajectory: 각 조인트의 이동경로

• Cartesian space trajectory: End-effector의 이동경로

• Cartesian space(or Task space or Operational space):

→ End-effector의 작업공간

Task space

Trajectory

Planning

End-effector

Joint

space

Trajectory

산출

1) Inverse kinematics

2) Teaching 방법

(산업용 로봇)

Joints

Joint control에적용

Robotics (KAU AME)


6

5.4 Basics of Trajectory Planning

• Let’s assume that the robot’s hand follow a known path between

point A to B with straight line.

• The simplest solution would be to draw a line between points A

and B, so called interpolation.

Fig. 5.6 Cartesian-space movements of a two-degree-of-freedom robot.

• It is assumed that the robot’s actuators are strong enough to provide large forces necessary to accelerate and decelerate the joints as needed. → 동적 모델에 기반한 dynamic Planning의 필요성

Fig. 5.7 Trajectory planning with anacceleration-deceleration regiment.

등속 경로 가감속 경로

Robotics (KAU AME)


7

5.5 Joint Space Trajectory Planning

11 0

22 0

0

11 0

22 0

0

( )( )

( )( )

( )( )

( )( )

( )( )

( )( )

f

f

n fn

f

f

n fn

q tq t

q tq t

q tq t

q tq t

q tq t

q tq t

⎯⎯→

<Initial time> <Final time>

:

:

위치

각속도

How to specify intermediate points of the trajectory?

→ 3차 또는 5차 다항식(polynomial)

A

B

C

Robotics (KAU AME)


8

3차 다항식(3rd order Polynomial) 궤적

2 3

0 1 2 3

0 0

0 0

0

( )

( )( )

( ) 0 ( )

,

f f

f f

t c c t c t c t

tt

t or t

c c

•

= + + +

= = ⎯⎯→

= =

•

3rd order polynomial

(I.C)

4 Coefficients(

초기조건 Final condition

위 개의 조건식을 가지고

0 0 1 2 32

3

3

1 2

3 2, 0, ,

, ,

f f

f f

c c c

c

ct

c

t

−= = = =

)를 결정하면

Robotics (KAU AME)


9

5차 다항식(5th order Polynomial) 궤적

0 0

0 0

0

2 3 4 5

0 2 3 4 5

0

1

( )( )

( ) 0 ( )

( ) 0 (

(

) 0

)

f f

f f

f f

t c

tt

c t c t c t c t c

t or t

t or t or

t

•

==

= ⎯⎯→ =

= =

= + + + +

+

5th order polynomial

(I.C)

초기조건 최종조건

0 1 2 3 4 5

0

1

2

3

5

6

, , , , ,

c

c

c

c

c

c

c c c c c c

=

=

=

=

•

=

6 6 (

위 개 조건식을 이용하여 개의 계수들 )을

=

결정

Robotics (KAU AME)


10

5.6 Cartesian Space Trajectory Planning

0

0

0

0

0

0

3

3

( ) ( )

( ) ( )

( ) ( )

( )

( )

( )

Polynomial

Polynomial

Task space 5

1)

2)

f

f

f

x t x t

y t y t

z t z t

x t

y t

z t

•

⎯⎯⎯⎯⎯⎯⎯⎯⎯→

⎯⎯⎯⎯⎯⎯⎯ →

차 or 5차 궤적

차 or 5차 궤적

궤적을 3차 또는 차 다항식으로 Planning

위치:

속도:

0

0

0

0

0

0

3

3

( )

( )

( )

( ) ( )

( ) ( )

( ) ( )

( )

( )

( )

Polynomial

Polynomial

3)

4) RPY rate

f

f

f

f

f

f

x t

y t

z t

t t

t t

t t

t

t

t

⎯⎯

⎯⎯⎯⎯⎯⎯⎯⎯⎯→

⎯

차 or 5차 궤적

차 or 5차 궤적

자세:

각속도 or

:

( )

( )

( )

f

f

f

t

t

t

⎯⎯⎯⎯⎯⎯⎯⎯→

Robotics (KAU AME)


11

경로계획상의 주의점

▪ 주어진 로봇제어 문제에 따라 속도 경로 또는 위치경로, 자세각 또는 각속도

(또는 RPY rate) 경로를 선택가능.

▪ 매니퓰레이터의 자유도에 따라 Joint space의 경로 수와 Task space의 경로

수가 결정됨.

▪ Task space 궤적 ➔ Joint space 궤적

• 대개의 로봇제어 문제는 task space에서 end-effector(robot hand)의 작업

경로를 추종하는 것

• 따라서 먼저 task space에서의 경로를 정하고 inverse kinematics를 이용

하여 조인트 경로를 정함.

• Position level에서 역기구학 해를 구하기 힘든 경우 →

속도 레벨에서의 Jacobian relationship을 이용하여 조인트 속도 경로를

구한 후, 이를 실시간 적분하여 조인트 경로를 결정함.

• 정해진 조인트 경로를 각 조인트 서보의 위치 명령으로 인가 → Joint

space control 수행.

Robotics (KAU AME)


12

Robot dynamics Simulation: (1)Joints의 위치/자세/속도

1

2

3

( )1

( , ) ( ) ( )

( ) ( ), ( )

( , ) ( )

( , ) ( )

Hq C q q G q t

t q t q t

Hq C q q G q

q H C q q G q

−

+ + =

→

= − −

→ = − −

→Numer

Robot dynamic equation of motion:

Given joint torque Robot joint motions

(Forward dynamics)

( ), ( )

( )

q t q t

t

→

ical Integration: (Ex.) RK4 Algorithm

Position and velocity

How to determine the joint torques in simulations?

1) Arbitrary functions : step input, sinusoidal input

2)

( ) ( ) ( )P Dt K e t K e t = +

Control input :

(ex) PD control

1 1 1q q q→ →

2 2 2q q q→ →

3 3 3q q q→ →

Robotics (KAU AME)


13

0

0

1 0

( , , )

( , , )

x

y

z

T

x y z

x

y

z

P o

s c c

c c s

s

v v v

v

v

v

−

=

⎯⎯

2) End-effecto Velocity kinematics:

Velocity:

RPY rate:

r 속도

J p = q = qJ J

1 1

, o

n

P

x

y

n z

q q

q q

= =

J J

0

1 1 2 2 3 3

0 0 0 1

( , , )

( , , )

( ) ( ) ( ) ( )

x x x x

y y y y

x y z n

z z z z

n n

n o a p

n o a p

n o a pp p p T

A q A q A q A q

=

=

⎯⎯

Position kinematics

Pos

1

ition:

Orientaio

) End-effect

n

or

:

위치/자세

Robot dynamics Simulation: (2)End-effector의 위치/자세/속도

1 1 1

2 2 2

,

,

,n n n

q q q

q q q

q q q

⎯

⎯

⎯

→RK4 Joints

(position & velocity:

)

적분

의 운동궤적

( )1 ( , ) ( )q H C q q G q−= − −

Robot dynamic equation

Robotics (KAU AME)


14

Robot Trajectory (Task Space)

1 1,q q

2 2,q q

3 3,q q

A(t0)

B(tf)

Robot trajectory (A → B)

: Position profile & velocity profile

,

( ), ( ), ( )

1) Position trajectory (

Task space (end-effector) trajectory about the reference frame

At every sampling time

[ ] end-effector

[ ]

)

e e e

t

Position x t y t z t

Orientation

•

위치궤적

좌표계 원점의 위치:

기준좌표계에 대 ( ), ( ), ( )

( ), ( ), ( )

( ), ( ), ( )

:

[ ] end-effector :

[ ] (Orientation rate):

2) Veloc

ity trajectory ( )

e e e

t t t

Linear velocity x t y t z t

Angular velocity t t t

한 자세각

좌표계

속도궤적

원점의 속도

자세각 변화율

( ), ( ), ( ) or (x,y,z)-axis x y zt t t 에 대한 각속도:

xy

z

Robotics (KAU AME)


15

Task space trajectory → Joint space trajectory

1 1,q q

2 2,q q

3 3,q q

A

B

Robot trajectory (A → B)

( ) ( ), , , ,

0

0

1 0

x y z

x

y

z

s c c

c c s

s

•

−

=

RPY rate

각속도 와 의 관계

1−

→ =J Jp = q q p

• 주어진 Task Space 궤적에서 Joint 궤적을 구하는 방법

1) Inverse kinematics:

주어진 end-effector의 위치/자세 궤적에 대하여

매 샘플링 시간마다 각 joint angle을 계산

2) Inverse differential kinematics:

주어진 end-effector의 선속도/Orientation rate 궤적에

대하여 각 joint 의 각속도를 계산

xy

z

Robotics (KAU AME)


16

Joint Position Control

▪ 각 Joint 대한 위치제어 loop 구성

• Task space 궤적→ Joint space 궤적

→Joint Position control loop의 reference command

• Robot dynamics 출력 값(joint angle)을 피드백

• Joint angle error (e) 계산

• Joint control algorithm (ex, PID control)에 의해 제어입력 계산

• 제어입력(u) + 외부 disturbance(d) → Joint 입력 torque 산출

( ) ( ) ( )

( ) ( ) ( )

1( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

d

d

p D I p D I

e t q t q t

e t q t q t

K s k k s k e s u t k e t k e t k e t dts

t u t d t

= −

= −

= + + → = + +

= +

- Joint angle error:

- Joint velocity error:

- PID contro

Input torque

l:

- :

Robotdynamics

K(s)

Joint controller(ex, PID)

( )Command

dq t ( )q t( )t( )e t ( )u t

( )d t

+−

++

외란

( )n t

센서 노이즈

Robotics (KAU AME)


17

Independent Joint Control (IJC)

▪ n-dof robot manipulator의 경우(multi-loop)

Joint controllers(ex. PID)

Joint spacetrajectory

Joint spacevariables

Robot

Dynamics

K1(s)1

Command

dq1q1e+

−

Kn(s)ndq ne+−

nq

2dqK2(s) 2q

Joint sensors

Task space(end-effector)

trajectory

Inverse kinematics

Pathplanning 1 1 1u d + =

n n nu d + =

2 2 2u d + =

( )d t

forward kinematics

0

1 1 2 2 3 3( ) ( ) ( ) ( )n n

T

P o

nT A q A q A q A q=

J J Jp = q = qTask space

variables

Robotics (KAU AME)


18

Joint space control vs. Task space control

▪ Joint space control → Joint 궤적을 제어함으로써 end-effector의 궤

적을 간접적으로 추종

▪ Task space control → task space (end-effector)에서의 제어 오차를

feedback하여 end-effector의 궤적을 직접 추종

chapter 5. trajectory planning and control...

Documents