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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
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)
로봇공학, Chapter 5
18
Joint space control vs. Task space control
▪ Joint space control → Joint 궤적을 제어함으로써 end-effector의 궤
적을 간접적으로 추종
▪ Task space control → task space (end-effector)에서의 제어 오차를
feedback하여 end-effector의 궤적을 직접 추종