Upload
ghulam-mustafa-ph-d
View
48
Download
0
Tags:
Embed Size (px)
Citation preview
Kinematics & Dynamics of Robots (The Articulated & the Wheeled )
Ghulam Mustafa @ Santa Clara University
04/22/2015
(Part 1: Articulated Rigid Bodies)
Some Definitions Spatial Motion of Rigid Bodies Kinematic Representation Denavit – Hartenberg Representation Two Worlds (Forward/Inverse Kinematics) Joint Velocities & Accelerations The Jacobian Computations and Algorithms
Content of this Talk : Part 1 – Articulated Rigid Bodies
Instantaneously Coincident Coordinates Sheth - Uicker Representation Kinematics of Some Wheeled Robots Time Derivatives in ICC The Wheel Jacobian Computations and Algorithms Velocity – Torque Duality Robot Dynamics : Equations of Motion Newton—Euler Recursive Algorithm Robot Control
Content of this Talk : Part 2 – Wheeled Mobile Robots
In theory, there is no difference between theory and practice. In practice there is.
It's tough to make predictions, especially about the future.
Kinematics, Dynamics and Rigid Bodies & Equations of Motion Kinematics … is the branch of mechanics which describes the motion of points, bodies and systems of bodies without consideration of the causes of motion It is a study of trajectories of points, lines and other geometric objects and their differentials, velocity and acceleration
Dynamics … is a branch of mechanics concerned with the study of forces and torques and their effect on motion.
Rigid Body … is an abstraction of a solid in which deformation does not occur. The distance between points of a rigid body remains constant regardless of external forces exerted on it.
Equations of Motion … are relationships that relate the dynamics to the kinematics under that rigid body abstraction
Newton-Euler-Lagrange [and sometimes Hamilton]
Isaac Newton 1642-1726
Joseph-Louis Lagrange
1736-1813
Leonhard Euler 1707-1783
William Hamilton 1805-1865
Spatial Motion of a Rigid Body
Any spatial motion, no matter how complex can be decomposed and reconstructed as
General Motion
Pure Translation
Pure Rotation
Newtonian Formulism … point masses on curvilinear trajectories
Eulerian Formulism … rigid bodies rotating about a point
Parallel Universes : The Newtonian – point masses move on curves – rotation makes no sense.
The Eulerian – rigid bodies rotate about a point. Both are mathematical abstractions, there are no
point masses nor rigid bodies. Real bodies have dimensions and deform.
Translation
Rotation
Rigid Body … Location = Position and Orientation
A
Position
Orientation
B
A p
Location Position Orientation B
AR
z
y
x
pB
A B
A
B
A
B
A
B
A ZYXR Position Vector Rotation Matrix
10
B
A
B
A
A
B
PRT ITTTTpTppTp A
AC
A
B
A
C
CB
C
BBA
B
A B
Base
Articulated Robot … The Forward Kinematics (FK)
Base
0
1
2
3
T0
0
TTTT 2
3
1
2
0
1
0
3 ..
T0
1
T0
2
T0
3
Forward Kinematics
Base End Effector
Robots Two Worlds | The Inner versus the Outer
Joint Space
q1
q2
q3
.
.
.
.qn
Work Space
x
y
z
a
b
g
Camera
Kuka
The camera sends the outer world information of position and orientation of objects in work space. The robot uses this information to assume a pose.
Work Space Joint Space
Inverse Kinematics
Forward Kinematics
Forward Kinematics | FK Problem Statement Given: Link orientations (the configuration) in the joint space (qi). Determine: End Effector (aka tool) position and orientation (the pose).
TTTTqqqT m
mmm
B
W
11
2
0
1
0
21 ....),...,,(
Rotation Matrix (Orientation) Position Vector
Joint Space (m dimensional)
Work Space (n dimensional)
Mathematically nmnm xqf ,:
q x
(.)f
)(qfx
FK via DH Representation
A methodology for computing the position and orientation of the End Effector with respect to the base frame of the manipulator.
This will be done by:
Defining the geometry of each link and the joints. Attaching frames to the joints. Establishing relationships between the joint axes. Writing transform equations to represent vectors in their own frame and mapping them to frames of other links.
z0 z1
z2
x0
x1
x2
Joint 0
Joint 3 Wire Frame
zi
Joint i-1
Joint i
Link i-1
zi-1
Link & Joints
Link Parameters
Link Twist
Link Length – [ai-1] defined along the common
perpendicular. Unique except for parallel axes.
Link Length & Link Twist are Fixed (link design parameters)
Link Twist – [ai-1] – right-hand rotation about [ai-1].
Joint Parameters
Link Twist
Link Offset – [di] difference in height between ai and ai-1 along the i axis.
di - Link Offset is a Variable for Primatics Joints (Joint Angle is Fixed) qi - Joint Angle is a Variable for Revolute Joints (Link Offset is Fixed)
ai
ai-1
Axis i+1
Joint Angle – [qi] – angle spanned by ai and ai-1 right-hand rotation
about [ai-1].
Forward Kinematics
ai-1
Xi-1
Oi-1
Zi-1
{i-1}
{i-1} {R} {Q} {P} {i}
)().().().(... 11
11
izizixix
P
i
Q
P
R
Q
i
R
i
i dDRaDRTTTTT a
Xi
{i}
Oi
Zi
{Q}
{R}
Forward Kinematics
)().().().(),,,( 1111
1
izizixixiiii
i
i dDRaDRdaT aa
1000
100
0010
0001
)(
i
iz
d
dD
1000
0100
00)cos()sin(
00)sin()cos(
)(
ii
ii
izR
1000
0100
0010
001
)(
1
1
i
ix
a
aD
1000
0)cos()sin(0
0)sin()cos(0
0001
)(
11
11
ii
ii
izR
aa
aa
1000
)()()()()()(
)()()()()()(
0)()(
1111
1111
1
1
iiiiiii
iiiiiii
iii
i
idccscss
dsscccs
asc
Taaaa
aaaa
restart; with(LinearAlgebra):
> m:=3:
> alias(seq(c[i]=cos(Theta[i]), i=1..m),
> seq(s[i]=sin(Theta[i]), i=1..m)):
> A := (a,alpha,d,theta) -> Matrix(4, 4, [
> [cos(theta),-sin(theta),0,a],
> [sin(theta)*cos(alpha), cos(theta)*cos(alpha),
> -sin(alpha), -a*sin(theta)*d],
> [sin(theta)*sin(alpha), cos(theta)*sin(alpha),
cos(alpha), cos(alpha)*d],
> [0, 0, 0, 1]]):
> T(a, alpha, d, theta):
> a := Vector(m,{(2)=l1,(3)=l2}):
> alpha := Vector(m):
> theta:=Vector(m,i->Theta[i]):theta[3]:=0:
> d:=Vector(m):
> for i to m do
T[i] := A(a[i], alpha[i], d[i], theta[i])
> end do:
Zi-1 Zi
Zi+1
Oi-1
Oi
Oi+1
Xi-1
Xi Xi+1
Yi-1 Yi
Yi+1
Forward Kinematics Stanford
:= T
( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3
( )s1 c2 c3 s1 s2 s3 a3 ( ) s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3
( ) s2 c3 c2 s3 a3 ( )s2 s3 c2 c3 d4 s2 a2
:= DH
0 0 0 1
20 0 2
0 a2 d3 3
2a3 d4 4
20 0 5
20 0 6
DH Parameters
Tip Coordinates
Tip :=
( )( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 c5 ( ) c1 c2 s3 c1 s2 c3 s5 c6 ( )( )c1 c2 c3 c1 s2 s3 s4 s1 c4 s6[ ,
( )( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 c5 ( ) c1 c2 s3 c1 s2 c3 s5 s6 ( )( )c1 c2 c3 c1 s2 s3 s4 s1 c4 c6 ,
( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 s5 ( ) c1 c2 s3 c1 s2 c3 c5 ,
( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3 ]
( )( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 c5 ( ) s1 c2 s3 s1 s2 c3 s5 c6 ( )( )s1 c2 c3 s1 s2 s3 s4 c1 c4 s6[ ,
( )( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 c5 ( ) s1 c2 s3 s1 s2 c3 s5 s6 ( )( )s1 c2 c3 s1 s2 s3 s4 c1 c4 c6 ,
( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 s5 ( ) s1 c2 s3 s1 s2 c3 c5 ,
( )s1 c2 c3 s1 s2 s3 a3 ( ) s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3 ]
( )( ) s2 c3 c2 s3 c4 c5 ( )s2 s3 c2 c3 s5 c6 ( ) s2 c3 c2 s3 s4 s6[ ,
( )( ) s2 c3 c2 s3 c4 c5 ( )s2 s3 c2 c3 s5 s6 ( ) s2 c3 c2 s3 s4 c6 ( ) s2 c3 c2 s3 c4 s5 ( )s2 s3 c2 c3 c5, ,
( ) s2 c3 c2 s3 a3 ( )s2 s3 c2 c3 d4 s2 a2 ]
[ , , , ]0 0 0 1
FK from Base to Tip
The Jacobian
Base
0
1
2
3
v
11qJ
22qJ
33qJ
qJqJqJqJv 332211
Linear combination of Joint velocities
Linear Velocity Angular Velocity
Tzyxzyx vvvv
v
J
JJ
vLinear velocity contribution
Angular velocity contribution
The Jacobian – via Direct Computation
10
00
0 nn
n
PRT
x
y
x
xy
xz
yz
S
0
0
0
> restart; with(LinearAlgebra):
> m:=3:alias(seq(c[i]=cos(Theta[i]), i=1..m),
> seq(s[i]=sin(Theta[i]), i=1..m)):
> A := (a,alpha,d,theta) -> Matrix(4, 4, [
> [cos(theta),-sin(theta),0,a],
> [sin(theta)*cos(alpha), cos(theta)*cos(alpha),
> -sin(alpha), -a*sin(theta)*d],
> [sin(theta)*sin(alpha), cos(theta)*sin(alpha),
cos(alpha), cos(alpha)*d],
> [0, 0, 0, 1]]):
> A(a, alpha, d, theta):
> a := Vector(m,{(2)=l1,(3)=l2}):
> alpha := Vector(m):
> theta:=Vector(m,i->Theta[i]):theta[3]:=0:
> d:=Vector(m):
> for i to m do
T[i] := A(a[i], alpha[i], d[i], theta[i])
> end do:
> Tip := `.`(seq(T[i], i=1..m)):
> collect(Tip[1,1], [c[1], c[2], s[1]]):
> T0||m := Column(Tip, 4):
> P0||m := SubVector(%, [1..3]):
> R := SubMatrix(Tip, [1..3], [1..3]):
> thetas := [seq(Theta[i], i=1..m)]:
> with(VectorCalculus, Jacobian):
> Jv:=Jacobian(P0||m, thetas):
> Rtranspose := Transpose(R):
> for i from 1 to m-1 do
R||i:=map(diff,R,theta[i]):
R||i.Rtranspose:
omega := simplify(%):
Jomega[i] := Vector[row]
([omega[3,2], -omega[3,1],
omega[2,1]]):
print(evaln(Jomega[i])=Jomega[i])
> end do:
> Jomega := Transpose(<seq(Jomega[i], i=1..m-1)>);
> Jv:=SubMatrix(Jv, [1..3], [1..2]);
> J := <Jv , Jomega>;
v
Position Orientation
qJqq
Pq
q
Pq
q
Pv v
nnn
3
3
0
2
2
0
1
1
0
vJ
J
J
SRRRR
RRRRIRR
T
nn
T
nn
T
nn
T
nn
T
nn
0000
000000
..
0...
# Compute r0j
> for j to m do
> r0 || j := T || j[[1 .. 3], 4]
> end do;
# Compute rjm
> for j to m do
> r || j || m := r0 || m-r0 || j
> end do;
# Compute zj
> z := Vector([0, 0, 1]);
> for j to m do
> z || j := T || j[[1 .. 3], [1 .. 3]].z
> end do;
# Compute Jvj and Jwj for each joint
> for j to m-1 do
> if joints(j) = R then
> Jv || j := `&x`(z || j, r || j || m);
> Jw || j := z || j
> elif joints(j) = P then
> Jv || j := z || j;
> Jw || j := Vector([0, 0, 0])
> end if;
> J || j := collect(Vector([Jv || j, Jw || j]), {c, s})
> end do;
# Assemble J from Jv and Jw
> Jdummy := Matrix([J || 1]);
> for j from 2 to m-1 do
> J := Matrix([Jdummy, J || j]);
> Jdummy := J
> end do;
The Jacobian – via Propagation
JJv ,
J
0
j
mjr0
mr0
jmr
jmjm rrr 00
0
0
0
0
j
j
j
j
J
zJ
zJ
rzJ
jv
j
jmjv
Revolute Joint
Prismatic Joint
j
j
jj zRz 00
The Jacobian – Stanford Arm
J :=
( ) s1 c2 c3 s1 s2 s3 a3 ( )s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3[ ,
( ) c1 c2 s3 c1 s2 c3 a3 ( )c1 s2 s3 c1 c2 c3 d4 c1 s2 a2 ( ) c1 c2 s3 c1 s2 c3 a3 ( )c1 s2 s3 c1 c2 c3 d4 0 0 0, , , , ]
( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3[ ,
( ) s1 c2 s3 s1 s2 c3 a3 ( ) s1 c2 c3 s1 s2 s3 d4 s1 s2 a2 ( ) s1 c2 s3 s1 s2 c3 a3 ( ) s1 c2 c3 s1 s2 s3 d4 0 0 0, , , , ]
[ , , , , , ]0 ( )s2 s3 c2 c3 a3 ( )c2 s3 s2 c3 d4 c2 a2 ( )s2 s3 c2 c3 a3 ( )c2 s3 s2 c3 d4 0 0 0
0 s1 s1 c1 ( )c2 s3 s2 c3 s4 c1 c2 c3 s4 c1 s2 s3 s1 c4[ , , , , ,
s5 s1 s4 c5 c1 s2 c3 c5 c1 c2 s3 c1 c4 s5 c2 c3 c1 c4 s5 s2 s3]
0 c1 c1 s1 ( )c2 s3 s2 c3 s4 s1 c2 c3 s4 s1 s2 s3 c1 c4[ , , , , ,
s1 c4 s5 c2 c3 s1 c4 s5 s2 s3 s5 c1 s4 s1 c5 s2 c3 s1 c5 c2 s3]
[ , , , , , ]1 0 0 s2 s3 c2 c3 s4 ( )c2 s3 s2 c3 c4 s5 c2 s3 c4 s5 s2 c3 c5 c2 c3 c5 s2 s3
The Acceleration
qJqq
JqJ
dt
d
dt
dva
2
Coriolis Centrifugal Inertia Direct Computation
Coriolis
Centrifugal
Inertia
Propagation
# Compute Jdot=DJ.qdot.
# Jacobian of J (DJ) w.r.t q (the d.o.f)
> with(VectorCalculus);
> L := [seq(q || i, i = 1 .. m-1)];
> PDEtools[declare](L(t), prime = t);
> for j to m-1 do
> Jdot || j := Jacobian(J || j, L).
(diff(convert(L(t), Vector), t))
> end do;
> DJdummy := Matrix([Jdot || 1]);
> for j from 2 to m-1 do
> Jdot := Matrix([DJdummy, Jdot || j]);
> DJdummy := Jdot end do;
# Compute Jdot.qdot
> DJqdot := factor(simplify(Jdot.
(diff(convert(L(t), Vector), t))));
# Compute J.q2dot
> Jq2dot := factor(simplify(J.
(diff(convert(L(t), Vector), `$`(t, 2)))));
# Compute the acceleration
> Acc := DJqdot+Jq2dot;
# Extract the linear and angular
acceleration
> vdot := Acc[1 .. 3];
> wdot := Acc[4 .. 6];
Spatial Motion of Rigid Bodies Denavit – Hartenberg Representation Forward Kinematics Joint Velocities & Accelerations The Jacobian Computations and Algorithms
Part 1 : Summary
(Part 2: Wheeled Mobile Robots)
Instantaneously Coincident Coordinates Sheth - Uicker Representation Kinematics of Some Wheeled Robots Time Derivatives in ICC The Wheel Jacobian Computations and Algorithms Velocity – Torque Duality Robot Dynamics : Equations of Motion Newton—Euler Recursive Algorithm Robot Control
Content of this Talk : Part 2
Instantaneously Coincident Coordinate
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
The wheel rotates with velocity v and acceleration a, with the frame R attached to its center r whose path is indicated. At some instant t, point r becomes coincident with the point r and frame R coincides with R. At this instant, R is instantaneously coincident with R. r has a velocity v and an acceleration a with respect to r but is stationary with respect to the reference frame F. R is ICC to R at time t.
R
r
F
RR,
The Sheth - Uicker Representation
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
Floor-Wheel Contact Point
z
x
y
z
x
y
0xv
CC,
F
Frame F is fixed to the floor. Frame C is attached to the contact point, is a moving frame. The frame C is instantaneously coincident coordinate frame (ICC) to C. C’ is stationary w.r. to F – thus at every instant, C’ has a new position and orientation, yet remains stationary w.r. to F.
Floor-Robot
x
y
RR,
x
y
F
Frame F is fixed to the floor. Frame R is attached to the robot, is a moving frame. The frame R’ is instantaneously coincident coordinate frame (ICC) to R. R’ is stationary w.r. to F – thus at every instant, R’ has a new position and orientation, yet remains stationary w.r. to F.
R
F
The Sheth - Uicker Representation
Floor-Robot
x y
F
Rotation about z axis of the floor frame F and x axis of the robot instantaneously coincident frame R’. The transformation is constant since F and R’ are fixed relative to each other
R
F
z RR,
R
F d
1000
100
0)()(
0)()(
xR
F
xR
F
iR
F
iR
F
xR
F
iR
F
R
F
R
F
d
dcs
dsc
T
The Homogenous Transform
WMR Coordinate System
The Sheth - Uicker Representation
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
', RRF
',CCH
S
F
'R
R
H
S
C
'CTF
R '
'R
R
H
S
'C
C
TR
H
TS
C
TF
C '
WMR Coordinate Transformations
F
C
C
C
F
C
S
C
H
S
R
H
R
R
F
R TTTT '
'
'
' .....
ijT = Constant Transformations
ijF = Variable Transformations
The Sheth - Uicker Representation
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
ijT = Constant Transformations
ijF = Variable Transformations
WMR Coordinate System
F
RR ,
SH , CC,
1 2
3 4
WMR Coordinate Transformations
F
R
R
H
S
C
C
1 2 3 4
The Sheth - Uicker Representation
z
y
x
z
y
x CC,
F
R
R
CRR,
C
11 ...
...
TTT
TTT
C
R
C
C
C
F
R
F
R
R
C
C
C
F
C
R
R
R
R
F
TF
R
R
R
TR
C
TF
C
C
C
Monocycle Coordinate System
rr CC ,ll CC ,
z
x
z
x
z
y
x
RR,
F
R
R
rC
rC
TF
Cl
TF
R
TR
ClTR
Cr
lC
lC
TF
Cr
l
l
C
C r
r
C
CR
R
11 ...
...
TTT
TTT
C
R
C
C
C
F
R
F
R
R
C
C
C
F
C
R
R
R
R
F
2-Wheel Coordinate System
The Sheth - Uicker Representation
1
11111111
.....
..........
.....
C
C
S
C
H
S
R
H
R
R
F
R
F
C
R
H
H
S
S
C
C
C
F
C
F
R
R
H
H
S
S
C
C
C
F
C
F
R
R
R
C
C
F
C
S
C
H
S
R
H
R
R
F
R
TTTT
TTTTTTTT
TTTT
F
R
R
C
TF
R
TF
C
H
S
C
C
R
R
RR,S
HCC,
H
S
C
TR
H
TS
C
F
Steering Coordinate System
1R
2R
3R
F
F
1R
2R
3R
Truck - Trailer Coordinate System
The Sheth - Uicker Representation
3-WMR Coordinate System
R
Floor
Robot Hip
Steering
Wheel
Kinematics of the Wheeled
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
R
RC
R
C
C
C
F
R
F
R
R
C
C
C
F
C
R
R
R
R
F
TTT
TTT
11 ...
...
ijT = Constant Transformations (fixed in time)
ijF = Variable Transformations (moving with time)
z
F
R
R
C
y
x
z
y
x CC,
RR,
C
TF
R
R
R
TR
C
TF
C
C
C
11 .. TTT R
C
C
C
F
C
F
R
R
R ?
1.... C
C
R
C
R
R
F
R
F
CTTT
11 .... TT R
C
C
C
C
C
R
C
R
R
R
R
Kinematics of the Wheeled – Time Derivatives
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
Coriolis Inertia
First Derivatives Second Derivatives
The Wheel Jacobian
Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12
ii
RR
R
RR
RR
RRR
RR
R qJrrrrr ....
0
R
Floor
Robot Hip
Steering
Wheel
R
R r
iq
TR
R
yR
R
xR
R
R
R vvr
.
3
2
1
3
2
1
00
00
00
q
q
q
J
J
J
r
I
I
I
R
R
WMR Jacobian
1
......
......
......
qrR
R
1J Wheel Jacobian
3x4
The Wheel Jacobian - Computation # Compute the first time derivatives of the variable
transforms.
# Compute the first time derivatives for RcC
dP||Rc||R:=
subs({thetaRR(t)=theta_R,diff(thetaRR(t),t)=omega_R,diff(r
RRz(t),t)=0},map(diff,P_||Rc||R,t));
dP_||Rc||R:=eval(%,theta_R=0);
# Compute the first time derivaatives for CcCforall wheels
for i to m do
dP||Cc||C||i:=
subs({theta||i||CC(t)=theta_C||i,diff(theta||i||CC(t),t)=omega_
C||i,diff(r||i||CCz(t),t)=0},map(diff,P_||Cc||C||i,t)):
dP_||Cc||C||i:=eval(%,theta_C||i=0);
end do;
# Compute the second time derivaatives of the variable
transforms
d2P||Rc||R:=
subs({thetaRR(t)=theta_R,diff(thetaRR(t),t)=omega_R,diff(th
etaRR(t),t$2)=alpha_R,diff(rRRz(t),t)=0},map(diff,P_||Rc||R,t
$2)) :
d2P_||Rc||R:=eval(%,theta_R=0);
for i to m do
d2P||Cc||C||i:=
subs({theta||i||CC(t)=theta_C||i,diff(theta||i||CC(t),t)=omega_
C||i,diff(theta||i||CC(t),t$2)=alpha_C||i,diff(r||i||CCz(t),t)=0},m
ap(diff,P_||Cc||C||i,t$2)):
d2P_||Cc||C||i:=eval(%,theta_C||i=0);
end do;
# Compute the Jacobian
v:=([vx,vy,omega]);
J0:=expand(P_||Rc||R.T_||R||C||1.MatrixInverse(P_||Cc||C||1
).dP_||Cc||C||1).MatrixInverse(T_||R||C||1):
J1:=subs({omega_C1=omega,diff(r1CCx(t),t)=vx,diff(r1CCy(
t),t)=vy},J0):
J2:=map(combine,simplify(expand(J1),trig));
restart:with(LinearAlgebra):m:=1:
# Enter parameters for F and Rc frames
FRc:=[theta||FRC,r||FRcx,r||FRcy,r||FRcz]:
# Enter parameters for Rc and R frames
RcR:=[theta||RR(t),r||RRx(t),r||RRy(t),r||RRz(t)]:
PDEtools[declare]({theta||RR(t),r||RRx(t),r||RRy(t),r||RRz(t)
}, prime = t):
# Enter parameters for frames RC for all the wheels
w||1||RC:=[theta||1||RC,r||1||RCx,r||1||RCy,r||1||RCz]:
# Enter parameters for frames CcC for all the wheels
w||1||CcC:=[theta||1||CC(t),r||1||CCx(t),r||1||CCy(t),r||1||CCz
(t)]:
PDEtools[declare]({theta||1||CC(t),r||1||CCx(t),r||1||CCy(t),r|
|1||CCz(t)}, prime = t):
# Enter parameters for frames FCc for all the wheels
w||1||FCc:=[theta||1||FCc,r||1||FCcx,r||1||FCcy,r||1||FCcz]:
# Compute Transformation Matrices
P:=(theta,x,y,z) ->Matrix(4,4,[
[cos(theta),-sin(theta),0,x],
[sin(theta),cos(theta),0,y],
[0,0,1,z],
[0,0,0,1]]):
# Transformation Matrices for F||Rc and Rc||R. These are
computed once
T_||F||Rc:=P(seq(FRc[i],i=1..4)):
P_||Rc||R:=P(seq(RcR[i],i=1..4)):
# Now compute the rest of the transforms
for i to m do
T_||R||C||(i):=P(seq(w||i||RC[j],j=1..4)):
P_||Cc||C||(i):=P(seq(w||i||CcC[j],j=1..4)):
T_||F||Cc||(i):=P(seq(w||i||FCc[j],j=1..4)):
end do;
Whe
el F
ram
e P
aram
eter
s T
rans
form
atio
ns
1st D
eriv
ativ
es
2nd D
eriv
ativ
es
Whe
el
Jac
obia
n
Base
v
qJv
Forward Propagation Given joint velocities – compute the End-Effector velocity.
F
FJ T
Back Propagation Given End-Effector forces– compute joint torques.
The Velocity – Torque Duality F
3
2
1
Tzyxzyx FFFF
Applied Force Applied Torque
i
ii xfw .Virtual
work
Applied force
Virtual Displacement
qJFxFq TTT .
FJ T
Dynamics - Newton versus Euler
).( vmdt
dF ).( I
dt
d
.. II
maF
General Motion
Pure Translation
Pure Rotation
General form of EOM
)(),()( qGqqVqqM Coriolis
Centrifugal Inertia Gravity
Torque (Control)
Robot Dynamics - Recursive Newton-Euler Fa
Robot Control – via the Jacobian xJqqJx 1
(J non-singular)
xxx d
Control 1
Forward Kinematics
Joint1
Control 2 Joint2
Control 3 Joint3 3
2
1
q
q
q
1J--
3
2
1
q
q
q
x
x
dx
q
q q
)(qfx
Robot Control = via Over-Writing the Dynamics
)(),()( qGqqVqMu
Control Law
qModified Dynamics
1 q
Unit mass
)(),()( qGqqVqqM
qkqkq dp
qkqkq pd
PD Controller
Instantaneously Coincident Coordinates Sheth - Uicker Representation Kinematics of Some Wheeled Robots Time Derivatives in ICC The Wheel Jacobian Computations and Algorithms Velocity – Torque Duality Robot Dynamics : Equations of Motion Newton—Euler Recursive Algorithm Robot Control
Part 2 : Summary
Blatantly Avoided
Inverse Kinematics – does the solution exist? … multiple solutions … which one to choose
Kinematic Singularities – J non-invertible … how to locate, avoid and negotiate.
Presence of friction – how to include … how to model and later compensate for.
Force control … hybrid control (position + force)
Legged Creatures … swing, step and propel.
Computation of inertia.
Without W
afe
r W
ith W
afe
r
0 1 20
0.2
0.4
0.6
0.8
1
1.2
1.4
Time (s)
x(t
)
Step Response
Rise Time = 0.043217
Settling Time =0.48267
Over Shoot (%) =33.9612
Data
Model
0 20 40 600
0.05
0.1
0.15
0.2
0.25
(Hz)|X
()|
Step Response FFT
-100
-50
0
Mag
nitud
e (
dB
)
100
102
104
-270
-180
-90
0
90
180
270
360
Pha
se
(d
eg
)
Bode Diagram
Frequency (Hz)
0 0.5 1 1.5 20
0.2
0.4
0.6
0.8
1
1.2
1.4
Time (s)
x(t
)
Step Response
Rise Time = 0.031458
Settling Time = 0.64119
Over Shoot (%) = 29.0969
0 10 20 30 40 500
0.02
0.04
0.06
0.08
0.1
0.12
(Hz)
|X(
)|
Step Response FFT
Data
Model
-150
-100
-50
0
50
Mag
nitud
e (
dB
)10
010
210
4-270
-225
-180
-135
-90
-45
0
Pha
se
(d
eg
)
Bode Diagram
Frequency (Hz)
Robot Dynamics – Step Response
Mechanism Friction Measurement & Model
m F
f
𝑓(𝑡) = g(1)(tanh(g 2 . v t − tanh g 3 . v t + g 4 tanh g 5 . v t + g 6 . v(t)
Striebeck Friction Coulomb Friction Viscous Friction
Mechanism Friction Measurement & Model
Upper EE
Lower EE Striebeck Friction
Coulomb Friction
Viscous Friction
Mechanism Friction Measurement & Model
0 1000 2000 3000 4000 5000-2
0
2
Time (s)
N
Friction force
0 500 1000 1500 2000 2500 3000 3500 4000 4500-50
0
50Slip speed
Time (s)
mm
/s
0 1000 2000 3000 4000
-1
-0.5
0
0.5
1
1.5Friction force. (sim)
Time (s)
Fri
cti
on
fo
rce
(N
)
zv; measured
nlgr; fit: 64.27%
-100 -50 0 50 100-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Velocity (mm/s)
Fri
cti
on
(N
)
Friction v Velocity
0 500 1000 1500 2000 2500 3000 3500 4000 4500-6
-4
-2
0
2
4
Time (s)
N
Friction force
0 500 1000 1500 2000 2500 3000 3500 4000 4500-50
0
50
Slip speed
Time (s)
mm
/s
0 500 1000 1500 2000 2500 3000 3500 4000
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Friction force. (sim)
Time (s)
Fri
cti
on
fo
rce
(N
)
zv; measured
nlgr; fit: 79.56%
-100 -50 0 50 100-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
Velocity (mm/s)
Fri
cti
on
(N
)
Friction v Velocity
Before PM @ 5M Cycles
After PM
Hysteresis
Rocking of a Rigid Cylinder on Moving Foundation
What is a “Reliable” Robot?
It is the probability (R) that the robot will successfully complete the assigned task (T) under the specified conditions (C).
Specified Conditions (C): Martian Terrain / Contact with Human Body / Assembly Line Assigned Task (T): Move from A to B / Perform Surgery / Spot Weld. Probability (R). On Mars, move from A to B 50 times without failure. On a human perform surgery with failure. On an assembly line do 1 million spot welds before failure. The basic problem is to quantify R during design.
Pain
ting T
itle
: M
any
a M
oon A
go, G
. M
usta
fa ©
2011
Fin