MEEN 408/612

MECHANICS OF ROBOTIC
MANIPULATION
Lecture Note 10
Robot Control - Part 2

Kiju Lee, Ph.D. [email protected]

Overview
■ Robot Control: Textbook Chapter 11 continued
– Motion control with torque inputs
– Task-space motion control
– Force control
Motion Control with Torques or Force Inputs
■ Velocity-control algorithms do not make use of a dynamic model of
the robot.
à Robot-control engineers use amplifiers in torque/force-control
mode.
à This makes use a dynamic model of the robot in the design of the
control law.
Feedback Control with Force/Torque Input
■ Controller generates force or torque input (𝜏) for the robot.
■ PD control: 𝜏 = 𝑘! 𝜃" + 𝑘# 𝜃̇"
■ PID control: 𝜏 = 𝑘! 𝜃" + 𝑘$ ∫ 𝜃" 𝑇 𝑑𝑇 +𝑘# 𝜃̇"
Control of a single joint
𝑟
𝜃
Dynamic mode of this system: 𝑚𝑔
𝜏 = 𝑀𝜃̈ + 𝑏𝜃̇ + 𝑚𝑔𝑟 cos 𝜃
- M, the scalar inertia of the link about the axis of rotation
- m, the mass of the link
- b, friction coefficient for the rotational friction
- r, distance from the axis to the center of mass of the link
- g, gravitational acceleration
Rewrite the above dynamic model in a more compact way:
𝜏 = 𝑀𝜃̈ + ℎ(𝜃, 𝜃)
̇
Control of a single joint - PD control !
"
■ PD control: 𝜏 = 𝑘! 𝜃" + 𝑘# 𝜃̇" !"
■ Consider g = 0: 𝜏 = 𝑀𝜃̈ + 𝑏𝜃̇
𝜏 = 𝑀𝜃̈ + 𝑏𝜃̇ = 𝑘! 𝜃" + 𝑘# 𝜃̇"
■ In a setpoint control (i.e, 𝜃̇# = 𝜃̈# = 0 → 𝜃̇" = −𝜃;̇ 𝜃̈# = −𝜃):
̈
– Error dynamics:
(𝑘" + 𝑏) 𝑘#
̈
𝜃! + 𝜃! + 𝜃! = 0 ↔ 𝜃̈! + 2𝜁𝑤$ 𝜃̇! + 𝑤$% 𝜃! = 0
̇
𝑀 𝑀
&'(! ("
where 𝜁 = and 𝑤$ = .
% (" ) )
– If 𝑘# > 0; 𝑘" + 𝑑 > 0, stable with 𝑒** = 0.
– For no overshoot + fast response, pick 𝑘# and 𝑘" to satisfy critical
damping.
– For fast response, 𝑘# must be as high as possible (but consider practical
issues, e.g., actuator satuation, undesired rapid torque changes,
vibrations).
Control of a single joint - PD control
■ Error dynamics:
(𝑘# + 𝑏) 𝑘!
𝜃̈" + 𝜃̇" + 𝜃" = 0 ↔ 𝜃̈" + 2𝜁𝑤% 𝜃̇" + 𝑤%& 𝜃" = 0
𝑀 𝑀
'()! )"
where 𝜁 = & )" *
and 𝑤% = *
.
– If 𝑘! > 0; 𝑏 + 𝑘# > 0, stable with 𝑒++ = 0.
– Let us use M=0.5kgm2, m=1kg, r=0.1m, b=0.1Nms/rad, and
g = 9.81m/s2.
– Q: For critical damping, what control gains should be used? If
we choose 𝑘! = 9, what value should we choose for 𝑘# ?
Control of a single joint - PD control
■ Now, consider the robot moves in a vertical plane (g ≠ 0).
M𝜃̈" + (𝑘# +𝑏)𝜃̇" + 𝑘! 𝜃" = 𝑚𝑔𝑟 cos 𝜃 .
– This means that the joint comes to rest at 𝑘! 𝜃" = 𝑚𝑔𝑟 cos 𝜃 .
– The final error 𝜃" ≠ 0 when 𝜃# ≠ ±𝜋/2.
– The reason is that the robot must provide a nonzero torque to
hold the link at rest at 𝜃 ≠ ±𝜋/2.
■ PD control law creates a nonzero torque at rest only if 𝜃" ≠ 0à
resulting in a steady state error à Increase 𝑘! to reduce this
steady state error (but there are practical limitations).
■ Use PID controller with 𝑘$ > 0.
Control of a single joint - PID control
■ Setpoint error dynamics:
𝑀𝜃̈" + 𝑏 + 𝐾# 𝜃̇" + 𝐾! 𝜃" + 𝐾$ ∫ 𝜃" 𝑡 𝑑𝑡 = 𝜏#$+,
where ‘𝜏#$+, ’ is a disturbance torque substituted for the gravity
term ′𝑚𝑔𝑟𝑐𝑜𝑠𝜃′.
■ Taking derivatives:
(.)
𝑀𝜃" + 𝑏 + 𝐾# 𝜃̈" + 𝐾! 𝜃̇" + 𝐾$ 𝜃" = 𝜏̇ #$+,
If 𝜏#$+, is constant, 𝜏̇ #$+, = 0, and the characteristics equation is
.
𝑏 + 𝐾# & 𝐾! 𝐾$
𝑠 + 𝑠 + 𝑠 + = 0.
𝑀 𝑀 𝑀
– If all roots of the above equation have a negative real part à
error dynamics is stable and error converges to zero.
Control of a single joint - PID control
.
𝑏 + 𝐾# & 𝐾! 𝐾$
𝑠 + 𝑠 + 𝑠+ =0
𝑀 𝑀 𝑀
– If all roots of the above equation have a negative real part à
the error dynamics is stable and the error converges to zero.
– This means that the following conditions on the control gains
must be satisfied for stability:
𝐾! > −𝑏
𝐾" > 0
𝑏 + 𝐾! 𝐾"
> 𝐾# > 0
𝑀
– Pick 𝐾# and 𝐾! for a good transient response, then choose 𝐾$
to be large enough to reduce/eliminate steady-state errors but
small enough not to impact stability.
Control of a single joint - PID control
𝑟
𝜃
■ PID controller: 𝑚𝑔
𝜏 = 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡 + 𝑘# 𝜃̇"

where all control gains are positive.

– 𝑘! acts as a virtual spring that tries to reduce the position
error.
– 𝑘# acts as a virtual damper that tries to reduce the velocity
error.
– 𝑘$ is used to reduce or eliminate steady-state errors.
Control of a single joint - PID control
■ Block diagram of PID control

Image Courtesy: Modern Robotics by K. Lynch and F. Park

Control of a single joint – PD vs PID control
■ Tracking errors for a PD controller with kd=2 and kp=2.205
Nm/rad for critical damping and a PID controller with the same PD
games and ki=1Nm/(rad s)
Feedforward + Feedback Control
■ Feedforward – recall, directly feeding the desired trajectory
to the control input.
■ No model or environment is perfect à Feedback is needed!
■ Combine PID with a model of the robot dynamics to achieve
the error dynamics:
𝜃̈" + 𝑘# 𝜃̇" + 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡 = 𝑐

■ Since 𝜃̈" = 𝜃̈# − 𝜃,̈ the robot’s commanded acceleration is

chosen to be
𝜃̈ = 𝜃̈# − 𝜃̈" .
Feedforward + Feedback Control
■ Combining the two:
𝜃̈ = 𝜃̈# + 𝑘# 𝜃̇" + 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡

■ Substituting the above 𝜃̈ into a model of the robot dynamics:

K 𝜃
𝜏=𝑀 𝜃̈# + 𝑘# 𝜃̇" + 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡 + ℎL 𝜃, 𝜃̇

This controller includes a feedforward component due to

the use of the planned acceleration 𝜃̈# -- called feedback
linearizing because feedback of 𝜃 and 𝜃̇ is used to
generate the linear error dynamics.
Feedforward + Feedback Control
■ Block diagram:
Performance comparisons
■ Feedforward only (ff), feedback only (fb), and computed torque
control (ff+fb).
Motion Control of a Multi-Joint Robot
■ Consider the dynamic equation in a more general vector-
valued form:
𝜏⃗ = 𝑀 𝜃⃗ + ℎ(𝜃, ⃗̇
⃗ 𝜃)
where 𝑀 𝜃⃗ is the n-by-n positive-definite mass matrix.
■ Consider two types of control for multi-joint robots:
– Decentralized: each joint controlled separately with no
sharing of information between joints
– Centralized: full state information for each of n joints to
calculate the controls for each joint
Decentralized Multi-Joint Control
■ Applicable for a system where dynamics for individual joints
are decoupled.
■ Acceleration of each joint depends only on the torque,
position, and velocity of that joint.
■ Mass matrix is diagonal (e.g., Cartesian robot with x, y, z
linear actuators – this kind of robot is equivalent to three
single-joint systems).
■ Approximate decoupling can be achieved in highly geared
robots in the absence of gravity à Mass matrix becomes
nearly diagonal; friction at the individual joints also
contributes to the decoupling of the dynamics.
Centralized Multi-Joint Control
■ Decentralization may not yield acceptable performance when gravity forces
and torques are significant and coupled; or when the mass matrix is not
well approximated by a diagonal matrix.
■ In this case, your PID controller will have the general vector form:
̈ ̈ ̇
𝜃⃗ = 𝜃⃗! + 𝐾$ 𝜃⃗% + 𝐾" 𝜃⃗% + 𝐾# 0 𝜃⃗% 𝑡 𝑑𝑡

resulting in the control torque input to your robot:

4 𝜃⃗ ̈ ̇ ⃗ 𝜃⃗̇
7 𝜃,
𝜏⃗ = 𝐌 𝜃⃗! + 𝑘$ 𝜃⃗% + 𝑘" 𝜃⃗% + 𝑘# 0 𝜃⃗% 𝑡 𝑑𝑡 + 𝐡

– Typically, the control gain matrices, 𝐾$ , 𝐾" , and 𝐾# , are chosen to be

diagonal matrices, such that 𝑘$ 𝐼, 𝑘" 𝐼, and 𝑘# 𝐼 for all nonnegative
scalars.
– 𝑘# is typically chosen to be zero or very small.
TASK SPACE MOTION CONTROL
Task-Space Motion Control – Velocity Input
,
̇ ̇
■ Recall: 𝜃 𝑡 = 𝜃# 𝑡 + 𝑘! 𝜃" 𝑡 + 𝑘$ ∫0 𝜃" t dt
■ Let 𝑋 = 𝑇+' 𝑡 ∈ 𝑆𝐸 3 be the configuration of the end-effector
and 𝒱' 𝑡 be the end-effector twist expressed in the end-effector
frame {b} à 𝑋 12 𝑋̇ = [𝒱' ].
■ Task-space version of the control law:
,
𝒱' 𝑡 = 𝐴𝑑3 #$3! 𝒱# 𝑡 + 𝑘! 𝑋⃗" 𝑡 + 𝐾$ I 𝑋⃗" t dt.
0
Recall: Numerical Inverse Kinematics
■ Now consider both the desired end-effector position AND
orientation à Desired rigid-body transformation 𝑇+# .
■ The difference between the desired and the current poses:
– 𝑇'# 𝜃⃗ $ = 𝑇+'
12 ⃗ $
𝜃 𝑇+# = 𝑇'+ 𝜃⃗ $ 𝑇+# .
■ Then, find the twist: 𝒱' = log 𝑇'# 𝜃⃗ $
Task-Space Motion Control – Velocity Input
(
𝒱% 𝑡 = 𝐴𝑑& +,&- 𝒱# 𝑡 + 𝐾! 𝑋⃗" 𝑡 + 𝐾$ F 𝑋⃗" t dt.
'
■ 𝐴𝑑& +,&- 𝒱# 𝑡 : feedforward twist of 𝒱# in the actual end-
effector frame at 𝑋(= 𝑇)% ). When 𝑋 = 𝑋# , this term reduces to
𝒱# 𝑡 .
■ Also note that 𝑋⃗" = log 𝑋 *+ 𝑋# .
■ Control gains: 𝐾! , 𝐾$ ∈ ℝ,×, , diagonal matrices where 𝑘! , 𝑘$ > 0.
■ Then, the commanded joint velocities can be calculated by
̇
𝜃⃗ = 𝐽%. (𝜃)𝒱%
Task-Space Motion Control
■ End-effector trajectory specified by 𝑋 𝑡 , 𝒱% 𝑡 , 𝑋 ∈ 𝑆𝐸 3 .
̇
■ Forward kinematics: 𝑋 = 𝑇 𝜃 ; 𝒱% = 𝐽% 𝜃⃗ 𝜃⃗
■ Inverse kinematics:
– 𝜃⃗ 𝑡 = 𝑇 *+ 𝑋 𝑡 (𝑖𝑛𝑣𝑒𝑟𝑠𝑒 𝑘𝑖𝑛𝑒𝑚𝑎𝑡𝑖𝑐𝑠)
̇
– 𝜃⃗ 𝑡 = 𝐽%. 𝜃⃗ 𝑡 𝒱% 𝑡
̈ ̇ ̇
– 𝜃 𝑡 = 𝐽%.
⃗ 𝜃⃗ 𝑡 𝒱% 𝑡 − 𝐽%̇ 𝜃⃗ 𝑡 ⃗
𝜃 𝑡

■ This process is computationally expensive.

■ Exam 2 includes until this slide.
 Task-Space Motion Control – Torque/Force Input
■ Express the robot’s dynamics in task-space coordinates.
■ Recall: Newton-Euler Recursive method for deriving the robot
dynamics).

ℱ' = Λ 𝜃 𝒱̇' + 𝜂 𝜃, 𝒱' → 𝜏 = 𝐽'4 𝜃 ℱ'

Recall: Newton-Euler Inverse Dynamics Algorithm

■ Forward Iterations
– Given 𝜃, 𝜃,̇ 𝜃,̈ for 𝑖 = 1 to n, calculate

𝑇$,$12 = 𝑒 1 𝒜% 7% 𝑀$,$12
𝒱$ = 𝐴𝑑 4%,%#$ 𝒱$12 + 𝒜$ 𝜃̇$
𝒱̇$ = 𝐴𝑑 4%,%#$ 𝒱̇$12 + 𝑎𝑑𝒱% 𝒜$ 𝜃̇ + 𝒜$ 𝜃̈$

where
𝑅 0 𝑤 0
𝐴𝑑 4 = ∈ ℝ9×9 ; 𝑎𝑑𝒱 = ∈ ℝ9×9
𝑝⃗ 𝑅 𝑅 𝑣⃗ 𝑤
Recall: Newton-Euler Inverse Dynamics Algorithm

■ Backward Iterations
– For i = n to 1, calculate

ℱ$ = 𝐴𝑑 44%'$,% ℱ$(2 + 𝒢$ 𝒱̇$ − 𝑎𝑑𝒱4% (𝒢$ 𝒱$ )

𝜏$ = ℱ$4 𝒜$
 Task-Space Motion Control – Torque/Force Input
ℱ' = Λ 𝜃 𝒱̇' + 𝜂 𝜃, 𝒱' → 𝜏 = 𝐽'4 𝜃 ℱ'
■ Now, write a control law in task space.
■ Recall the feedforward + feedback control with torque/force input:
𝜃̈ = 𝜃̈# + 𝑘# 𝜃̇" + 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡

– The above acceleration is the controlled output which is then used to

calculate the torque/force using the plant (robot) dynamic model:

K 𝜃
𝜏=𝑀 𝜃̈# + 𝑘# 𝜃̇" + 𝑘! 𝜃" + 𝑘$ I 𝜃" 𝑡 𝑑𝑡 + ℎL 𝜃, 𝜃̇
 Task-Space Motion Control – Torque/Force Input
ℱ' = Λ 𝜃 𝒱̇' + 𝜂 𝜃, 𝒱' → 𝜏 = 𝐽'4 𝜃 ℱ'
■ In a similar way:
ℱ& = Λ< 𝜃 𝒱̇ & + 𝜂? 𝜃, 𝒱&
– Control law for the acceleration term:
𝑑
̇
𝒱& = 𝐴𝑑'!" '# 𝒱! + 𝐾" 𝑋% + 𝐾# ∫ 𝑋% 𝑑𝑡 + 𝐾! 𝒱%
𝑑𝑡
– Then, we have
𝑑
<
ℱ& = Λ 𝜃 𝐴𝑑'!" '# 𝒱! + 𝐾" 𝑋% + 𝐾# ∫ 𝑋% 𝑑𝑡 + 𝐾! 𝒱% + 𝜂? 𝜃, 𝒱&
𝑑𝑡
< 𝜂? represents the controller’s dynamics model.
– Λ,
– Velocity error: 𝒱% = 𝐴𝑑'!" '# 𝒱! − 𝒱.
 Task-Space Motion Control – Torque/Force Input
ℱ' = Λ 𝜃 𝒱̇' + 𝜂 𝜃, 𝒱' → 𝜏 = 𝐽'4 𝜃 ℱ'
■ In a similar way:
ℱ& = Λ< 𝜃 𝒱̇ & + 𝜂? 𝜃, 𝒱&
– Using the fact that 𝜏 = 𝐽&( 𝜃 ℱ& ,
𝑑
( <
𝜏 = 𝐽& 𝜃 Λ 𝜃 𝐴𝑑'!" '# 𝒱! + 𝐾" 𝑋% + 𝐾# ∫ 𝑋% 𝑑𝑡 + 𝐾! 𝒱% + 𝜂? 𝜃, 𝒱&
𝑑𝑡
– < 𝜂? represents the controller’s dynamics model.
Λ,
– Velocity error: 𝒱% = 𝐴𝑑'!" '# 𝒱! − 𝒱.
Block Diagrams

Trajectory Trajectory
Controller Robot
Generator Converter

Trajectory
Controller Robot
Generator
FORCE CONTROL
Force Control
■ When the task at the end-effector is to apply forces and
torques to the environment à Use force control!
■ Pure force control is possible if the environment provides
resistance forces in every direction (e.g., end-effector
embedded in concrete or attached to a spring providing
resistance in every motion direction) à Not the case of
robots.
■ Let’s consider an ideal case.
Force Control
■ ℱ,$! : the wrench applied by the manipulator to the environment.
■ Manipulator dynamics:
𝑀 𝜃 𝜃̈ + 𝑐 𝜃, 𝜃̇ + 𝑔 𝜃 + 𝑏 𝜃̇ + 𝐽4 𝜃 ℱ,$! = 𝜏
■ Assume the robot moves slowly or does not move during a force
control task:
𝑔 𝜃 + 𝐽4 𝜃 ℱ,$! = 𝜏
■ In case no force-torque measurement, use the force control law
𝜏 = 𝑔i 𝜃 + 𝐽4 𝜃 ℱ#
– requires a good model for gravity compensation and precise
control of the torques produced at the robot joints.
What we learned so far
■ Robot components & workspace
■ Kinematics
■ Velocity kinematics
■ Inverse kinematics
■ Inverse dynamics
■ Forward dynamics
■ Introduction to manipulator control