Kinematics Forward Kinematic - pku.edu.cn
Transcript of Kinematics Forward Kinematic - pku.edu.cn
Kinematics β Forward Kinematic
Centre for Robotics Research β School of Natural and Mathematical Sciences β Kingβs College London
Compliant Robotics Peking University, Globex, July 20182
Introduction β Forward and Inverse Kinematics
Inverse
Forward
Compliant Robotics Peking University, Globex, July 20183
Linear Algebra βDot Product
The dot product of two vectors A = [A1, A2, ..., An] and B = [B1, B2, ..., Bn]
Compliant Robotics Peking University, Globex, July 20184
Linear Algebra βDot Product
[1, 0]
[1, 1]
[2, 0]
[0, 2]
[0, 1] [0, 2]
if A and B are orthogonal
if they are codirectional
Compliant Robotics Peking University, Globex, July 20185
Linear Algebra βMatrix Multiplication
=Ai β Bj
n x m
m x p
n x p
Compliant Robotics Peking University, Globex, July 20186
β’ Example
Linear Algebra βMatrix Multiplication
π ππ π
π ππ β
= ?
π ππ π
=π ππ β
?
Compliant Robotics Peking University, Globex, July 20187
Linear Algebra βMatrix Multiplication
Square matrices
AB β BA
Compliant Robotics Peking University, Globex, July 20188
Linear Algebra βMatrix Multiplication
Row vector and column vector- Dimensionality
3 β11β 3
Compliant Robotics Peking University, Globex, July 20189
Linear Algebra βMatrix Multiplication
Square matrix and column vector
Compliant Robotics Peking University, Globex, July 201810
Linear Algebra βMatrix Multiplication
If AT = C = x
Then: ABC = xTAx
Quadratic scaler function using matrix representation
Compliant Robotics Peking University, Globex, July 201811
Linear Algebra βMatrix Multiplication
Rectangular matrices
Compliant Robotics Peking University, Globex, July 201812
Linear Algebra
Compliant Robotics Peking University, Globex, July 201813
Rigid body motion
Translation + Rotation
Compliant Robotics Peking University, Globex, July 201814
Rotation Matrix in 2D
xβ
y
x
yβ
ΞΈ
Compliant Robotics Peking University, Globex, July 201815
Rotation Matrix in 2D
xβ
y
x
yβ
ΞΈ(cosΞΈ, sinΞΈ)
(-sinΞΈ, cosΞΈ)
A
B
Rab=[xa
b yab]
pa= Rab pb
Compliant Robotics Peking University, Globex, July 201816
Rotation Matrix in 2D
xβ
y
x
yβ
ΞΈ
(cosΞΈ, -sinΞΈ)
(sinΞΈ, cosΞΈ)pb= Rab-1 pa
RabT = Ra
b-1
RabT Ra
b = I
Compliant Robotics Peking University, Globex, July 201817
Rotation Matrix in 3D
Rotation of a rigid object about a point. A be the inertial
frame, B the body frame, and xab, yab, zab β R3 the
coordinates of the axes of B relative to A
A
B
Compliant Robotics Peking University, Globex, July 201818
Rotation Matrix in 3D
Consider the point q,
qb = (xb, yb, zb) be coordinates of q relative to frame B.
qa = (xa, ya, za) be coordinates of q relative to frame A.
coordinate axes of B, which, in turn, have coordinates xab, yab, zab β R3 with
respect to A
Coordinates of q relative to frame A are given by
Compliant Robotics Peking University, Globex, July 201819
Rotation matrix β Vector representation
Representation of a Vectorπ =
ππ₯ππ¦ππ§
Respect to π β π₯π¦π§
πβ² =
πβ²π₯
πβ²π¦
πβ²π§
Respect to πβπ₯β²π¦β²π§β²
β’ Since π and πβ² are representations of the same point P
π = πβ²π₯π₯β² + πβ²π¦π¦
β² + πβ²π§π§β² = π₯β² π¦β² π§β² πβ²
π = π₯β² π¦β² π§β² πβ² = π πβ²
πβ² = π ππ
β’ R represents the transformation matrix of vector
coordinates in frame πβπ₯β²π¦β²π§β²
β’ Inverse transformation is
Compliant Robotics Peking University, Globex, July 201820
Rotation matrix
π ππ = πΌ3 =1 0 00 1 00 0 1
π π = π β1
det(π ) = 1
Compliant Robotics Peking University, Globex, July 201821
Rotation matrix β Elementary rotation
The rotation matrix of frame πβπ₯β²π¦β²π§β²
with respect to frame πβ π₯π¦π§
π π§ πΌ =cos πΌ βsinπΌ 0sin πΌ cos πΌ 00 0 1
π π¦ π½ =cos π½ 0 sin π½0 1 0
βsin π½ 0 cos π½
π π₯ πΎ =1 0 00 cos πΎ βπ ππ πΎ0 π ππ πΎ cos πΎ
Compliant Robotics Peking University, Globex, July 201822
Rotation matrix β Example of a 3D vector rotation
π =001
π1 = π π₯ ππ₯ π
π π₯ ππ₯ π2 = π π¦ ππ¦ π1
π π¦ ππ¦
π π§ ππ§
π3 = π π§ ππ§ π2
π = π π₯π ππ₯ π π¦
π ππ¦ π π§π ππ§ π3
π3 = π π§ ππ§ π π¦ ππ¦ π π₯ ππ₯ π
Compliant Robotics Peking University, Globex, July 201823
Euler Angle
A representation of orientation in terms of three independent parameters constitutes
a minimal representation.
π = ππ₯ ππ¦ ππ§ T
π1 = π π₯ ππ₯ π π¦ ππ¦ π π§ ππ§ π
π =001
, ππ₯ = ππ¦ = ππ§ = 45π
π2 = π π§ ππ§ π π¦ ππ¦ π π₯ ππ₯ π?
Compliant Robotics Peking University, Globex, July 201824
Euler Angle
A representation of orientation in terms of three independent parameters
constitutes a minimal representation.
π = ππ₯ ππ¦ ππ§ T
π1 = π π₯ ππ₯ π π¦ ππ¦ π π§ ππ§ π
π =001
, ππ₯ = ππ¦ = ππ§ = 45π
π2 = π π§ ππ§ π π¦ ππ¦ π π₯ ππ₯ πβ
π1π2
π
Compliant Robotics Peking University, Globex, July 201828
Homogeneous Transformation
β’ Pose of a rigid body is completely described using position and orientation.π π
β’ It can be compactly rewritten as below.
β’ Letβs A is the homogeneous transformation matrix(4 Γ 4)
π΄ =π π0 1
π΄10 = π 1
0 π10
0 1
ππ
1= π΄1
0 π1
1
Compliant Robotics Peking University, Globex, July 201829
Homogeneous Transformation
β’ Pose of a rigid body is completely described using position and orientation.π π
β’ It can be compactly rewritten as below.
β’ Letβs A is the homogeneous transformation matrix(4 Γ 4)
π΄ =π π0 1
πππ‘π βΆ π΄β1 β π΄π
A-1
Proof execise: A A-1
Compliant Robotics Peking University, Globex, July 201830
Homogeneous Transformation
β’ Pose of a rigid body is completely described using position and orientation.π π
β’ It can be compactly rewritten as below.
β’ Letβs A is the homogeneous transformation matrix(4 Γ 4)
π1
1= π΄0
1 π0
1
π1
1= π 0
1 βπ 01π1
0
0 1π0
1
π΄10 = π 1
0 π10
0 1
ππ
1= π΄1
0 π1
1
Compliant Robotics Peking University, Globex, July 201831
Homogeneous Transformation
Aac= Aab Abc =
Proof exercise
pbc
C
Compliant Robotics Peking University, Globex, July 201832
Homogeneous Transformation
Translation without rotation
P
π§
π¦
π₯
π§β²
π¦β²
π₯β²
=
1000
P100
P010
P001
z
y
x
A
π§
π¦
π₯
π§β²
π₯β²Rotation without translation
=
1000
0
0
0
zzz
yyy
xxx
'''
'''
'''
zyx
zyx
zyx
A
Compliant Robotics Peking University, Globex, July 201833
Homogeneous Transformation
β’ Finding the Homogeneous Matrix (an open kinematic chain)
πππ π = π΄1
π π1 π΄21 π2 β¦π΄π
πβ1 ππ
The coordinate transformation describing the position and orientation of Frame n with respect to Frame 0
Compliant Robotics Peking University, Globex, July 201834
Programming exercise in class
open exercise1
homogeneousmatrix.mrotationchain_q.m
Homogeneous Transformation
Compliant Robotics Peking University, Globex, July 201835
Kinematics for manipulators
A manipulator can be schematically represented from a mechanical viewpoint as akinematic chain of rigid bodies (links) connected by means of revolute or prismatic joints.
Compliant Robotics Peking University, Globex, July 201836
Typical Manipulator Structures
Depends on combination of revolute joint and prismatic joint
Three link planar arm,Parallelogram Arm,
Spherical arm,Spherical Wrist,
Stanford Manipulator,DLR Manipulator,
β¦
Compliant Robotics Peking University, Globex, July 201837
Typical Manipulator Structures
Spherical wrist
Example of combination of revolute joints
Compliant Robotics Peking University, Globex, July 201838
Denavit-Hartenberg Convention(D-H matrix)
β’ The DβH convention allows the construction of the forward kinematics
function by composition of the individual coordinate transformations as
πππ π = π΄1
π π1 π΄21 π2 β¦π΄π
πβ1 ππ
β’ It can be applied to any open kinematic chain
D-H convention homogeneous transformation matrix and parameters
ri
ri
Compliant Robotics Peking University, Globex, July 201839
Denavit-Hartenberg Convention(D-H matrix)
ππ βΆ πππ π‘ππππ πππ‘π€πππ ππ πππππβ²
ππ: ππππ‘β ππππ ππβ1 π‘π ππβ² πππππ π§πβ1
(πππ π‘ππππ πππ‘π€πππ ππβ1 πππ π€βπππ ππππππ ππππππ ππ πππ‘ππ πππ‘ π€ππ‘β π§πβ1)πΌπ: πππππ πππ‘π€πππ ππ₯ππ π§πβ1 πππ π§πππππ’π‘ ππ₯ππ π₯πππ: πππππ πππ‘π€πππ ππ₯ππ π₯πβ1 πππ π₯π ππππ’π‘ ππ₯ππ π§πβ1
β’ π and πΌπ are always constant by geometry connection
ri-1
ri
Important: Locate the origin ππ at
the intersection of π§π with the
common normal to axes π§πβ1 and π§π
Compliant Robotics Peking University, Globex, July 201840
DH Homogeneous transformation
Frame i-1 translate by
di along zi-1 and rotate
by ΞΈi about zi-1
Translate by ri along xiβ
and rotate by Ξ±i about xiβ
ri
riri
Compliant Robotics Peking University, Globex, July 201841
Compliant Robotics Peking University, Globex, July 201842
Denavit-Hartenberg Convention(D-H matrix)
Compliant Robotics Peking University, Globex, July 201843
Denavit-Hartenberg Convention
1. Choose base frame by locating the origin on axis π§0 , and obtain a right-handed frame
2. Locate the origin ππ at the intersection of π§π with the common normal to axes π§πβ1 andπ§π
3. Establish π₯π axis. Establish or along the common normal between the π§π and π§πβ1 axeswhen they are parallel.
4. Establish π¦π axis. Assign to complete the right-handed coordinate system.
5. Find the link and joint parameters
6. Using the defined parameters, compute the homogeneous transformation matrices.
π΄ππβ1 ππ πππ π = 1,β¦ , π.
7. Compute homogeneous transformation. πππ π = π΄1
π π1 π΄21 π2 β¦π΄π
πβ1 ππ
8. πππ π is the position and orientation of the end-effector frame with respect to the
base frame
Compliant Robotics Peking University, Globex, July 201844
Denavit-Hartenberg Convention
If joint π is revolute, ππ are variablesIf joint π is prismatic, ππ are variables
ri and πΌπ are always constant by geometry connection
Remark
Compliant Robotics Peking University, Globex, July 201845
Three-linked arm example
Link ππ πΌπ ππ ππ
1 ?
2 ?
3
Compliant Robotics Peking University, Globex, July 201846
Three-linked arm example
Link π«π πΌπ ππ ππ
1 π1 0 0 π1
2 π2 0 0 π2
3 π3 0 0 π3
Compliant Robotics Peking University, Globex, July 201847
Three-linked arm example
Link π«π πΌπ ππ ππ
1 π1 0 0 π1
2 π2 0 0 π2
3 π3 0 0 π3
riri
Compare to elementary rotation
matrix:
R: rotation matrix of z axis
P: translation vector
DH arrives the same matrix!
Compliant Robotics Peking University, Globex, July 201848
Three-linked arm example
Link π«π πΌπ ππ ππ
1 π1 0 0 π1
2 π2 0 0 π2
3 π3 0 0 π3
Compliant Robotics Peking University, Globex, July 201849
Example β Anthropomorphic Arm
1. Choose base frame by locating the originon axis π§0 , and obtain a right-handedframe
2. Locate the origin of frame ππ at the
intersection of π§π with the common
normal to axes π§πβ1 and π§π
Base frame If joint π is revolute, axes π§πβ1 and π§π areparallel
parallel
Compliant Robotics Peking University, Globex, July 201850
Example β Anthropomorphic Arm
3. Establish π₯π axis. Establish or along the
common normal between the π§π and π§πβ1axes when they are parallel.
4. Establish π¦π axis. Assign to complete the
right-handed coordinate system.
Compliant Robotics Peking University, Globex, July 201851
Example β Anthropomorphic Arm
5. Find the link and joint parameter, and
form the table of parameters
Link ππ πΌπ ππ ππ
0-1 0 π/2 0 π1
1-2 π2 0 0 π2
2-3 π3 0 0 π3
revolution joint with height =0
so: d =0
Compliant Robotics Peking University, Globex, July 201852
Example β Anthropomorphic Arm
6. Using the defined parameters, compute
the homogeneous transformation matrices.
π΄ππβ1 ππ πππ π = 1,2,3.
Compliant Robotics Peking University, Globex, July 201853
Example β Anthropomorphic Arm
7. Compute πππ π . It represents the
position and orientation of the end-effector
frame with respect to the base frame
π30 π = π΄1
0π΄21π΄3
2
=
π1π23π 1π23π 230
βπ1π 23βπ 1π 23π230
π 1βπ100
π1(π2π2 + π3π23)π 1(π2π2 + π3π23)π2π 2 + π3π 23
1
Orientation Position
Compliant Robotics Peking University, Globex, July 201854
Example β Anthropomorphic Arm
π2
π3
π1
π2
π3
Given : π1 0 to 45π β π2 0 to 90π
β π3(0 to 90π) β π1 (45 to β45
π)
Compliant Robotics Peking University, Globex, July 201855
Example β Anthropomorphic Arm
Given : π1 0 to 45π β π2 0 to 90π
β π3(0 to 90π) β π1 (45 to β45
π)
Compliant Robotics Peking University, Globex, July 201856
Example βSpherical Arm
1. Choose base frame by locating the
origin on axis π§0 , and obtain a right-
handed frame
2. Locate the origin of frame ππ at the
intersection of π§π with the common
normal to axes π§πβ1 and π§π
prismatic
Compliant Robotics Peking University, Globex, July 201857
Example βSpherical Arm
prismatic
5. Find the link and joint parameter, and
form the table of parameters
Link ππ πΌπ ππ ππ
1 0 -π/2 0 π1
2 0 π/2 π2 π2
3 0 0 π3 0
Compliant Robotics Peking University, Globex, July 201858
Example β Spherical Arm
prismatic
Translation only
6. Using the defined parameters, compute
the homogeneous transformation matrices.
π΄ππβ1 ππ πππ π = 1,β¦ , π.
π΄10 π1 =
π1π 100
00β10
βπ 1π100
0001
π΄21 π2 =
π2π 200
0010
π 2βπ200
00π21
π΄32 π3 =
1000
0100
0010
00π31
Compliant Robotics Peking University, Globex, July 201859
Example β Spherical Arm
prismatic
π30 π = π΄1
0π΄21π΄3
2
=
π1π2π 1π2βπ 20
βπ 1π100
π1π 2π 1π 2π20
π1π 2π3 β π 1π2π 1π 2π3 + π1π2
π2π31
Orientation Position
Compliant Robotics Peking University, Globex, July 201860
Example β Spherical Arm
prismatic
Given :
π1 β90 to β 45π β π2 1 to 3β π3(1 to 3) β π2 (0 to 45
π) β π1 (β45 to 0π)
π2
π3
π1
π2
Compliant Robotics Peking University, Globex, July 201861
Example β Spherical Arm
prismatic
Given :
π1 β90 to β 45π β π2 1 to 3β π3(1 to 3) β π2 (0 to 45
π) β π1 (β45 to 0π)
Compliant Robotics Peking University, Globex, July 201862
Programming exercise
DHexecise
complete the spherical arm matlab simulation
Compliant Robotics Peking University, Globex, July 201863
DH Transformation for continuum mechanism
Continuum mechanism
β’ Flexible body
β’ No physical joints
β’ No direct DH Transformation
Flexible catheter
Compliant Robotics Peking University, Globex, July 201864
Continuum Robot Arm
Catheter Kinematics for Intracardiac Navigation, TBME 09
The curvature constancy and the
coupling between parameters yield:
ΞΈ5 = Ο / 2 β ΞΈ3
ΞΈ6 = Ο β ΞΈ2
The constant length of the distal end
(O4O6 ) is denoted by d7
Compliant Robotics Peking University, Globex, July 201865
Summary
β’ Type of manipulators are defined depending on different combination of revolute
and prismatic joints.
β’ Poses of a robot end-effector and joints are completely described in space by
its position and orientation
β’ Forward kinematic describe pose of a robot by joint and link variables
β’ D-H convention to derive forward kinematic using the homogeneous
transformation for manipulators
β’ Poses of a robot end-effector and each joint are compactly rewritten using the
homogeneous transformation matrix