Lecture 2 - Dynamics - Part 3 and 4
Lecture 2 - Dynamics - Part 3 and 4
Lecture 2 - Dynamics - Part 3 and 4
[email protected]
Contents
Folie 2
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Equations of motion of a multi-body system
and robot dynamics
Folie 3
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Geometry of a free-flying robot as a multibody system
Folie 4
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Differential kinematics of a robot with a moving base
Recall from Lesson 1 that the differential kinematics of a robot with revolute joints
are given by 𝑛𝑛 𝑗𝑗
𝑣𝑣 𝑒𝑒 = ∑𝑗𝑗=1 𝝎𝝎
� 𝑙𝑙 𝑗𝑗
with ω𝑖𝑖 = ∑𝑖𝑖𝑗𝑗=1 ω𝑗𝑗(𝑗𝑗−1) = ∑𝑖𝑖𝑗𝑗=1 𝜃𝜃̇ 𝑗𝑗 𝑢𝑢 𝑗𝑗
and ω𝑒𝑒 = ∑𝑛𝑛𝑗𝑗=1 𝜃𝜃̇ 𝑗𝑗 𝑢𝑢 𝑗𝑗
For a robot with a moving base, the motion of the base must also be accounted
for. The expression which relates v , ω , v , ω and θ is
e e 0 0
𝑗𝑗
𝑣𝑣 𝑒𝑒 = 𝑣𝑣 0 + ∑𝑛𝑛𝑗𝑗=1 𝝎𝝎
� 𝑙𝑙 𝑗𝑗
with ω𝑖𝑖 = ω0 + ∑𝑖𝑖𝑗𝑗=1 ω𝑗𝑗(𝑗𝑗−1) = ω0 + ∑𝑖𝑖𝑗𝑗=1 𝜃𝜃̇ 𝑗𝑗 𝑢𝑢 𝑗𝑗
and ω𝑒𝑒 = ω0 + ∑𝑛𝑛𝑗𝑗=1 𝜃𝜃̇ 𝑗𝑗 𝑢𝑢 𝑗𝑗
Folie 5
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Differential kinematics in matrix form
This can be expressed in matrix form as:
x e = J b x 0 + Jθ
with x = [v
e e
ω e ]T , x 0 = [v 0 ω 0 ]T . The expression for the base Jacobian Jb
(6x6) is
I 3 −~
r e0
Jb =
0 I3
n
where I3 is the identity matrix (3x3) an r
e0
= ∑ l i , i.e. the vector from the base
i =0
centre of mass to the end-effector.
Note: see a Jacobian as any matrix which linearly multiplies two velocity vectors
of a mechanical system. For example, given the velocity of the base
x 0, the velocity of the end-effector which results from it is J b x 0.
Folie 6
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
General equations of motion of a free-flying robot
Fe2 Fe
Fe3
We can write the equations of motion for a
free-flying robot with n joints as {Oe3,ee3}
𝐉𝐉bT e
𝐅𝐅 0
𝐌𝐌𝐲𝐲̇ II + 𝐂𝐂𝐲𝐲II = + T 𝐅𝐅
𝛕𝛕 𝐉𝐉
θi
[ ]
T
where y II = v ω θ ... θ ,
0 1 0 n
of dimensions (6+n)x1. F0
yI=[r0 φ0 θ]
Recall the derivation of the
{OI,eI}
equations for a two-body system in
Lesson 2 Part I and II, S. 35-41.
Modelling of a general multibody
open-tree system with n joints
Folie 7
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
General equations of motion of a free-flying robot
To derive the second term on the RHS, recall the result that (see slide 41, Part I):
𝐅𝐅 = 𝛗𝛗𝑇𝑇 𝐅𝐅� where x II = φ y II
In this case, 𝐅𝐅� = 𝐅𝐅 𝒆𝒆 , to say that the external forces/torques of interest are those
acting on the last link.
We then derive the expression for φ for the last link, i.e. for the end-effector body:
letting then v 0
v
e
ve v0
x II = e , y II = ω0 and given that e = J b 0 + Jθ
ω θ ω ω
it follows that
J b
φ=
J
Folie 8
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
General equations of motion of a free-flying robot
𝐅𝐅 0 𝐉𝐉bT e
𝐌𝐌𝐲𝐲̇ II + 𝐂𝐂𝐲𝐲II = + T 𝐅𝐅 Fe2 Fe1
𝛕𝛕 𝐉𝐉 Fe3
Folie 9
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
General equations of motion of a free-flying robot
𝐅𝐅 0 𝐉𝐉bT e
𝐌𝐌𝐲𝐲̇ II + 𝐂𝐂𝐲𝐲II = + T 𝐅𝐅 Fe2 Fe1
𝛕𝛕 𝐉𝐉 Fe3
Both kinematical and dynamical models are meant to represent the physical
system as closely as possible, to minimize the modelling errors. Often though,
due to complexity or computational burden, simplifying modelling assumptions
are or have to be made (e.g. rigid body assumption).
Folie 12
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Integration of the differential equations (ODEs)
Solution of an initial value problem:
Kinematics:
d𝛉𝛉 yI t y I
𝑟𝑟̇ 0 = 𝑣𝑣 0 , 𝜙𝜙̇ = 𝓒𝓒 𝜙𝜙 0 𝜔𝜔0 , = 𝛉𝛉̇ y ∫ y (y , y ) dt
=
dt II 0 II I II
Dynamics: 𝐅𝐅 0 𝐉𝐉bT e
𝐌𝐌𝐲𝐲̇ II + 𝐂𝐂𝐲𝐲II = + T 𝐅𝐅 given F0(t), τ(t), Fe(t)
𝛕𝛕 𝐉𝐉
Initial conditions:
r 0 (0) = r00 , v 0 (0) = v00 , φ 0 (0) = φ00 , ω 0 (0) = ω00 , θ(0) = θ 0 , θ (0) = θ 0
Integration methods:
Runge-Kutta order 5 (define accuracy, may have variable step size, etc.)
Euler (order 1) is generally too inaccurate
Other
Example: to guarantee the conservation of momentum for the free-floating
dynamics case (see later), the integrator accuracy must be in the range 10-8.
Folie 13
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Example: 7 DOF robot dynamics
Parameter description [kg, kg m2]:
mass m i
inertia i I i
Folie 14
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Dynamical properties
of a free-floating robot
Folie 15
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
TECSAS (2001-2005) -Technology Satellite for
Demonstration and Verification of Space Systems
Target satellite
mTARGET ≈ mCHASER
Manipulator
SARAH hand
Handle
(7 D.O.F.)
Chaser satellite
(thruster controlled)
Folie 16
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
TECSAS - Grasping Task
Folie 17
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
DEOS Deutsche Orbitale Servicing Mission
System description:
• Servicer satellite
• 7 DOF Manipulator
• Client satellite (cylinder with no appendages)
Operational conditions:
• Servicer attitude control inactive
• Client motion constrained to limited
range of free tumbling states
Folie 18
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Definition of a Free-floating and a Free-flying robot
A free-floating robot is one for which the base body is in free flight. Furthermore,
all external forces are zero. This includes environmental forces, such as orbital
disturbances, as well as control forces applied to the base body, deriving from
actuators such as thrusters or reaction wheels.
As a result, as we will see, the momentum of the robot system is conserved and
a free-floating dynamics results. We will now analyse this dynamics.
A free-flying robot is one for which the base body is generally not attached to
ground, as opposed to a fixed-base robot. It may however, other than with a free-
floating robot, be subjected to control forces applied to the base body and to
other external forces.
Folie 19
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Linear and Angular Momentum of a multibody system
We recall that the linear and angular momentum vectors of a multibody system
are defined as (see L.2-I-II, S.28-29):
n
P = ∑ m j v CM
j
j =0
CM H=∑
n
i =0
[CM i
I . ω + m (d × v
i i i i CM i
) ]
Note that both quantities are functions of the velocity variables v , ω , θ .
0 0
In fact, recalling the differential kinematics expressions for the end-effector (S. 5),
we can similarly write for the velocity of the centre of mass of the i th body:
𝑖𝑖
𝑣𝑣 𝐶𝐶𝑀𝑀 = 𝑣𝑣 0 + ∑𝑖𝑖−1 � 𝑗𝑗 𝑙𝑙 𝑗𝑗 + 𝜔𝜔
𝑗𝑗=1 𝜔𝜔 � 𝑖𝑖 𝑐𝑐 𝑖𝑖
with 𝜔𝜔𝑗𝑗 = 𝜔𝜔0 + ∑𝑖𝑖−1
𝑘𝑘=1 𝜃𝜃̇ 𝑘𝑘 𝑢𝑢 𝑘𝑘 and 𝜔𝜔𝑖𝑖 = 𝜔𝜔 + ∑𝑖𝑖
0
̇ 𝑘𝑘 𝑘𝑘
𝑘𝑘=1 𝜃𝜃 𝑢𝑢
These are linear functions in the velocities which allow a simple matrix formulation.
Folie 20
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Generalized Jacobian (1/3)
The linear and angular momentum can in fact be written in matrix form, as
P
L = = F0 x 0 + G 0 θ (1)
H
where F0 and G0 are the appropriate corresponding matrices and L is a vector
array containing vectors P and H.
F0 and G0 can be shown to be related to the inertia matrix of the multibody
system M. In fact, writing the equations of motion for the free-floating case, for
which no external forces and torques act :
Mb (6x6) – inertia matrix
Partitioning of M M b M bm x C b 0
0
of base body
M T + =
C τ
m θ
and C matrices Mm (nxn) – inertia matrix
bm M m of robot arm
𝐂𝐂b Mbm (6xn) - dynamic
where = 𝐂𝐂𝐲𝐲II , it follows that: coupling term
𝐂𝐂m
M bx0 + M bmθ + C b = 0 (2)
(6x6) (6x1) + (6xn)(nx1) + (6x1) = 0
Folie 21
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Generalized Jacobian (2/3)
dL 0 θ + G θ = 0
= F0 x + F0x0 + G 0 0
(3)
dt
where the derivative of L is zero, given that there are no external forces acting.
Equating coefficients between (3) and (2), it follows that
F0 = M b G 0 = M bm and θ = C
F 0 x 0 + G 0 b
This is also equivalent to saying that equation (1) is solution (first integral) to the
equations of motion (2), i.e. for the case of zero external forces, for which the
linear and angular momentum of the system are constant in time.
Folie 22
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Generalized Jacobian (3/3)
From (1), the dependence of x e on x 0 can be eliminated. We first write from (1)
x 0 = −M -1 M θ + M -1 L
b bm
(4)
b
= J b x + Jθ
Recalling again (S. 5) that: x
e 0
x e = J gθ + x e0
where
J g = J − J bM -1bM bm - generalized Jacobian, function of M b and M bm
x e0 = J bM -1bL
x e = J gθ
- differential kinematics for
If L = 0, then x
e0
=0 and a free-floating robot
t=0 t=tf
tf tf
𝐱𝐱 𝑒𝑒 = � 𝐉𝐉g 𝛉𝛉̇ 𝑑𝑑𝑑𝑑 or 𝛉𝛉 = � 𝐉𝐉g−1 𝐱𝐱̇ 𝑒𝑒 𝑑𝑑𝑑𝑑
0 0
(𝛉𝛉̇(t) given) (𝐱𝐱̇ 𝑒𝑒 (t) given)
Folie 24
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Video – point-to-point maneuver
x e = J gθ
Folie 25
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Dynamic singularities
The dynamic singularities correspond to configurations for which the generalized
Jacobian is rank-deficient and the end-effector again loses mobility in some
inertial direction.
They are affected by the dynamic coupling between the robot and its free-floating
base. In fact, the expression of the generalized Jacobian includes a term which is
function of the inertia and inertia coupling terms (see S.23). Note however, that
for a given system, the rank of the Jacobian is solely configuration dependent.
θ = J g⊥ v e + (I n − J g⊥ J g ) v 0
with an arbitrary vector v0 multiplying the null-space term (see Lesson 1), which
can be used to move away from singularities.
Folie 26
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Linear Momentum of a Free-floating Robot
The momentum equations of a free-floating robot are again:
L = M b x 0 + M bm θ (5)
[ ]
T
with x = v ω . We can extract the first three equations from (5), relative to
0 0 0
P=M ˆ θ
ˆ x 0 + M (6)
b bm
(3x1) = (3x6)(6x1) + (3xn)(nx1)
This acts like a motion constrain between 𝐱𝐱̇ 0 and 𝜽𝜽̇ (note that P is constant). It
can be integrated analytically to express the fact that the centre of mass of the
system is either stationary (𝐏𝐏 = 𝟎𝟎) or moves with a constant velocity (𝐏𝐏 ≠ 𝟎𝟎):
d n j CM j d
( )
n
∑m v j CM j
(t ) = ∑ m r (t ) = Mr CM (t ) = P = const.
j =0 dt j =0 dt
(see also L. 2-I-II, S. 9-10).
As a result, for any new configuration of the robot, the position of the base body
can be determined, given the center of mass position of the system.
Folie 27
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Angular Momentum of a Free-floating Robot
We can now extract the last three equations from (5), relative to the angular
motion, and write them as
~ 0 ~
H = M x + M θ
b bm
(7)
(3x1) = (3x6)(6x1) + (3xn)(nx1)
This is also a motion constraint between x and θ (note that H is constant),
0
Folie 29
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Angular Momentum of a Free-floating Robot
or
� b# 𝜔𝜔0 = −𝐌𝐌
𝐌𝐌 � bm
#
𝛉𝛉̇
� # = (𝐌𝐌
where 𝐌𝐌 � 0 − 𝐌𝐌
� b𝑣𝑣 0 𝐌𝐌
� −10 𝐌𝐌 � bm
� 0 ) and 𝐌𝐌 # � bm − 𝐌𝐌
= 𝐌𝐌 � b𝑣𝑣 0 𝐌𝐌
� −10 𝐌𝐌
� bm , to
b b𝜔𝜔 b𝑣𝑣 b𝜔𝜔 b𝑣𝑣
obtain
−𝟏𝟏
𝜔𝜔0 � b#
= − 𝐌𝐌 � bm
𝐌𝐌 #
𝛉𝛉̇
Folie 30
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Free-floating robot dynamics – point-to-point maneuver
Folie 31
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Free-floating robot dynamics – closed-loop maneuver
- ∆φ20 P
∆φ10
∆φ
If 𝐎𝐎𝐎𝐎1: Δ𝜙𝜙10
𝐎𝐎𝐎𝐎2: Δ𝜙𝜙20
and Δ𝜙𝜙10 ≠ Δ𝜙𝜙20
Initial configuration: 𝜃𝜃1 = 0, 𝜃𝜃2 = 0, 𝜃𝜃3 = 0
then
𝐎𝐎𝐎𝐎1 −𝐎𝐎𝐎𝐎2:
Final configuration: 𝜃𝜃1 = 0, 𝜃𝜃2 = 0, 𝜃𝜃3 = 0
Δ𝜙𝜙= Δ𝜙𝜙10 - Δ𝜙𝜙20 ≠ 0
Folie 32
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Simulation Example – closed-loop maneuver
At the end of each closed-loop the robot returns to the same initial configuration.
However, the base body attitude changes.
Folie 33
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Video – closed-loop maneuver
Note that the example on the left does not give rise to a base body attitude change.
The robot motion in this case is not a closed-loop motion but a “back and forth”
motion on a fixed path in joint space.
Folie 34
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
ETS-VII robot “attitude control” maneuvers (1998)
SIMPACK
simulation
model
Robot joint space
Satellite attitude
Measured
attitude
change
Folie 35
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Free-floating and free-flying robot DOFs
Free-flying robot – base body is actuated:
- degrees of freedom = 6+n
Folie 36
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics
Literature
Folie 37
On-Orbit Dynamics and Robotics | R. Lampariello | TU Munich | Summer Semester 2016 | Dynamics