本文仅供学习使用
本文参考：
B站：CLEAR_LAB
笔者带更新-运动学
课程主讲教师：
Prof. Wei Zhang
南科大高等机器人控制课 Ch04 Exponential Coordinate of Rigid Body Configuration
- 1. Exponential Coordinate of SO(3) - rotation matrix
- 1.1 Towards Exponential Coordinate of SO(3)
- 1.2 Exponential Coordinate of SO(3)
- 1.3 Rotation Matrix as Forward Exponential Map
- 1.4 Example of Forward Exponential Map
- 1.5 Logarithm of Rotations
- 2. Euler Angles and Euler-Like Parameterizations
- 3. Exponential Coordinate of SE(3) - Homogeneous Transformation Matrix
1. Exponential Coordinate of SO(3) - rotation matrix
1.1 Towards Exponential Coordinate of SO(3)
Recall the polar coordinate system of the complex plane:
- Every complex number z = x + y j = ρ e j ϕ z=x+yj=\rho e^{j\phi} z=x+yj=ρejϕ
- Cartesian coordomaye ( x , y ) \left( x,y \right) (x,y) ↔ \leftrightarrow ↔ polar coorindate ( ρ , ϕ ) \left( \rho ,\phi \right) (ρ,ϕ)
- For some applications, polar coordinate is preferred due to its geometric meaning
Consider a set
M
=
{
(
t
,
sin
(
2
n
π
t
)
)
:
t
∈
(
0
,
1
)
,
n
=
1
,
2
,
3
,
⋯
}
M=\left\{ \left( t,\sin \left( 2n\pi t \right) \right) :t\in \left( 0,1 \right) ,n=1,2,3,\cdots \right\}
M={(t,sin(2nπt)):t∈(0,1),n=1,2,3,⋯}
1.2 Exponential Coordinate of SO(3)
Proposition [Exponential Coordinate ↔ \leftrightarrow ↔ SO(3)]
- For any unit vector
ω
^
~
∈
s
o
(
3
)
\tilde{\hat{\omega}}\in so\left( 3 \right)
ω^~∈so(3) and any
θ
∈
R
\theta \in \mathbb{R}
θ∈R (
∥
ω
^
∥
=
1
,
e
ω
⃗
~
θ
∈
R
3
×
3
\left\| \hat{\omega} \right\| =1,e^{\tilde{\vec{\omega}}\theta}\in \mathbb{R} ^{3\times 3}
∥ω^∥=1,eω~θ∈R3×3)
e ω ⃗ ~ θ ∈ S O ( 3 ) e^{\tilde{\vec{\omega}}\theta}\in SO\left( 3 \right) eω~θ∈SO(3) - For any
[
Q
]
∈
S
O
(
3
)
\left[ Q \right] \in SO\left( 3 \right)
[Q]∈SO(3), there exists
∥
ω
^
∥
∈
R
3
\left\| \hat{\omega} \right\| \in \mathbb{R} ^3
∥ω^∥∈R3 such that
[ Q ] = e ω ⃗ ~ θ \left[ Q \right] =e^{\tilde{\vec{\omega}}\theta} [Q]=eω~θ
exp : ω ⃗ ~ θ ∈ s o ( 3 ) → [ Q ] ∈ S O ( 3 ) log : [ Q ] ∈ S O ( 3 ) → ω ⃗ ~ θ ∈ s o ( 3 ) \begin{array}{l} \exp :& \tilde{\vec{\omega}}\theta \in so\left( 3 \right)& \rightarrow& \left[ Q \right] \in SO\left( 3 \right)\\ \log :& \left[ Q \right] \in SO\left( 3 \right)& \rightarrow& \tilde{\vec{\omega}}\theta \in so\left( 3 \right)\\ \end{array} exp:log:ω~θ∈so(3)[Q]∈SO(3)→→[Q]∈SO(3)ω~θ∈so(3) - The vector ω ^ θ \hat{\omega}\theta ω^θ is called the exponential coordinate for [ Q ] \left[ Q \right] [Q]
- The exponential coordinates are also called the canonical coordinates of the rotation group S O ( 3 ) SO\left( 3 \right) SO(3)
1.3 Rotation Matrix as Forward Exponential Map
- Exponential Map : By definition
e ω ^ ~ θ = E + θ ω ^ ~ + θ 2 2 ! ω ^ ~ 2 + θ 3 3 ! ω ^ ~ 3 + ⋯ e^{\tilde{\hat{\omega}}\theta}=E+\theta \tilde{\hat{\omega}}+\frac{\theta ^2}{2!}\tilde{\hat{\omega}}^2+\frac{\theta ^3}{3!}\tilde{\hat{\omega}}^3+\cdots eω^~θ=E+θω^~+2!θ2ω^~2+3!θ3ω^~3+⋯ - Rodrigues’ Formula : Given any unit vector
ω
⃗
~
∈
s
o
(
3
)
\tilde{\vec{\omega}}\in so\left( 3 \right)
ω~∈so(3) , we have
e ω ^ ~ θ = E + ω ^ ~ sin θ + ω ^ ~ 2 ( 1 − cos θ ) e^{\tilde{\hat{\omega}}\theta}=E+\tilde{\hat{\omega}}\sin \theta +\tilde{\hat{\omega}}^2\left( 1-\cos \theta \right) eω^~θ=E+ω^~sinθ+ω^~2(1−cosθ)
∥ ω ^ ∥ = 1 , ω ^ ~ = − ω ^ ~ T , ω ^ ~ 3 = − ω ^ ~ , ω ^ ~ 4 = − ω ^ ~ 2 \left\| \hat{\omega} \right\| =1,\tilde{\hat{\omega}}=-\tilde{\hat{\omega}}^{\mathrm{T}},\tilde{\hat{\omega}}^3=-\tilde{\hat{\omega}},\tilde{\hat{\omega}}^4=-\tilde{\hat{\omega}}^2 ∥ω^∥=1,ω^~=−ω^~T,ω^~3=−ω^~,ω^~4=−ω^~2
e ω ^ ~ θ = E + θ ω ^ ~ + θ 2 2 ! ω ^ ~ 2 + θ 3 3 ! ( − ω ^ ~ ) + θ 4 4 ! ( − ω ^ ~ 2 ) + ⋯ = E + ( θ − θ 3 3 ! + θ 5 5 ! + ⋯ ) ω ^ ~ + ( θ 2 2 ! − θ 4 4 ! + θ 6 6 ! + ⋯ ) ω ^ ~ 2 = E + sin θ ω ^ ~ + ( 1 − cos θ ) ω ^ ~ 2 e^{\tilde{\hat{\omega}}\theta}=E+\theta \tilde{\hat{\omega}}+\frac{\theta ^2}{2!}\tilde{\hat{\omega}}^2+\frac{\theta ^3}{3!}\left( -\tilde{\hat{\omega}} \right) +\frac{\theta ^4}{4!}\left( -\tilde{\hat{\omega}}^2 \right) +\cdots \\ =E+\left( \theta -\frac{\theta ^3}{3!}+\frac{\theta ^5}{5!}+\cdots \right) \tilde{\hat{\omega}}+\left( \frac{\theta ^2}{2!}-\frac{\theta ^4}{4!}+\frac{\theta ^6}{6!}+\cdots \right) \tilde{\hat{\omega}}^2 \\ =E+\sin \theta \tilde{\hat{\omega}}+\left( 1-\cos \theta \right) \tilde{\hat{\omega}}^2 eω^~θ=E+θω^~+2!θ2ω^~2+3!θ3(−ω^~)+4!θ4(−ω^~2)+⋯=E+(θ−3!θ3+5!θ5+⋯)ω^~+(2!θ2−4!θ4+6!θ6+⋯)ω^~2=E+sinθω^~+(1−cosθ)ω^~2
1.4 Example of Forward Exponential Map
-
Rotation matrix R x ( θ ) R_x\left( \theta \right) Rx(θ) (corresponding to x ^ θ \hat{x}\theta x^θ)
ω ^ = x ^ = [ 1 0 0 ] ⇒ ω ^ ~ = [ 0 0 0 0 0 − 1 0 1 0 ] \hat{\omega}=\hat{x}=\left[ \begin{array}{c} 1\\ 0\\ 0\\ \end{array} \right] \Rightarrow \tilde{\hat{\omega}}=\left[ \begin{matrix} 0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\\ \end{matrix} \right] ω^=x^= 100 ⇒ω^~= 0000010−10
R x ( θ ) = e ω ^ ~ θ = E + sin θ [ 0 0 0 0 0 − 1 0 1 0 ] + ( 1 − cos θ ) [ 0 0 0 0 0 − 1 0 1 0 ] 2 = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_x\left( \theta \right) =e^{\tilde{\hat{\omega}}\theta}=E+\sin \theta \left[ \begin{matrix} 0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\\ \end{matrix} \right] +\left( 1-\cos \theta \right) \left[ \begin{matrix} 0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\\ \end{matrix} \right] ^2=\left[ \begin{matrix} 1& 0& 0\\ 0& \cos \theta& -\sin \theta\\ 0& \sin \theta& \cos \theta\\ \end{matrix} \right] Rx(θ)=eω^~θ=E+sinθ 0000010−10 +(1−cosθ) 0000010−10 2= 1000cosθsinθ0−sinθcosθ -
Rotation matrix corresponding to ( 1 , 0 , 1 ) T \left( 1,0,1 \right) ^{\mathrm{T}} (1,0,1)T
ω ^ = 1 2 [ 1 0 1 ] , θ = 2 \hat{\omega}=\frac{1}{\sqrt{2}}\left[ \begin{array}{c} 1\\ 0\\ 1\\ \end{array} \right] ,\theta =\sqrt{2} ω^=21 101 ,θ=2
1.5 Logarithm of Rotations
If
[
Q
]
=
E
\left[ Q \right] =E
[Q]=E, then
θ
=
0
\theta =0
θ=0 and
ω
^
\hat{\omega}
ω^ is undefined
If
t
r
(
[
Q
]
)
=
−
1
tr\left( \left[ Q \right] \right) =-1
tr([Q])=−1, then
θ
=
π
\theta =\pi
θ=π and set
ω
^
\hat{\omega}
ω^ equal to one of the following
1
2
(
1
+
q
33
)
[
q
13
q
23
1
+
q
33
]
,
1
2
(
1
+
q
22
)
[
q
12
1
+
q
22
q
32
]
,
1
2
(
1
+
q
11
)
[
1
+
q
11
q
21
q
31
]
\frac{1}{\sqrt{2\left( 1+q_{33} \right)}}\left[ \begin{array}{c} q_{13}\\ q_{23}\\ 1+q_{33}\\ \end{array} \right] ,\frac{1}{\sqrt{2\left( 1+q_{22} \right)}}\left[ \begin{array}{c} q_{12}\\ 1+q_{22}\\ q_{32}\\ \end{array} \right] ,\frac{1}{\sqrt{2\left( 1+q_{11} \right)}}\left[ \begin{array}{c} 1+q_{11}\\ q_{21}\\ q_{31}\\ \end{array} \right]
2(1+q33)1
q13q231+q33
,2(1+q22)1
q121+q22q32
,2(1+q11)1
1+q11q21q31
Otherwise, θ = cos − 1 ( 1 2 ( t r ( [ Q ] ) − 1 ) ) ∈ ( 0 , π ) \theta =\cos ^{-1}\left( \frac{1}{2}\left( tr\left( \left[ Q \right] \right) -1 \right) \right) \in \left( 0,\pi \right) θ=cos−1(21(tr([Q])−1))∈(0,π) and ω ^ ~ = 1 2 sin θ ( [ Q ] − [ Q ] T ) \tilde{\hat{\omega}}=\frac{1}{2\sin \theta}\left( \left[ Q \right] -\left[ Q \right] ^{\mathrm{T}} \right) ω^~=2sinθ1([Q]−[Q]T)
2. Euler Angles and Euler-Like Parameterizations
2.1 Euler Angle Representation of Rotation
A common method of specifying a rotation matrix is through three independent quantities called Euler Angles.
Euler angle representation
- Initially, frame { 0 } \left\{ 0 \right\} {0} coincides with frame { 1 } \left\{ 1 \right\} {1}
- Rotate
{
1
}
\left\{ 1 \right\}
{1} about
z
^
0
\hat{z}_0
z^0 by an angle
α
\alpha
α, then rotate about
y
^
a
\hat{y}_a
y^a axis by
β
\beta
β, and then rotate about the
z
^
b
\hat{z}_b
z^b axis by
γ
\gamma
γ. This yields a net orientation
[
Q
1
0
]
(
α
,
β
,
γ
)
\left[ Q_{1}^{0} \right] \left( \alpha ,\beta ,\gamma \right)
[Q10](α,β,γ) parameterized by the ZYZ angles
(
α
,
β
,
γ
)
\left( \alpha ,\beta ,\gamma \right)
(α,β,γ)
[ Q 1 0 ] ( α , β , γ ) = [ Q Z ] ( α ) [ Q Y ] ( β ) [ Q Z ] ( γ ) \left[ Q_{1}^{0} \right] \left( \alpha ,\beta ,\gamma \right) =\left[ Q_{\mathrm{Z}} \right] \left( \alpha \right) \left[ Q_{\mathrm{Y}} \right] \left( \beta \right) \left[ Q_{\mathrm{Z}} \right] \left( \gamma \right) [Q10](α,β,γ)=[QZ](α)[QY](β)[QZ](γ)
2.2 Other Euler-Like Parameterizations
Other types of Euler angle parameterization can be devised using different ordered sets of rotation axes
Common choices include:
-
ZYX Euler angles : also called Fick angles or yaw, pitch and roll angles
-
YZX Euler angles(Helmholtz angles)
3. Exponential Coordinate of SE(3) - Homogeneous Transformation Matrix
3.1 Exponential Map of se(3) : From Twist to Rigid Motion
Theorem 1 [Exponential Map of se(3)] : For any V = ( ω ⃗ , v ⃗ ) \mathcal{V} =\left( \vec{\omega},\vec{v} \right) V=(ω,v) and θ ∈ R \theta \in \mathbb{R} θ∈R, we have e [ V ] θ ∈ S E ( 3 ) e^{\left[ \mathcal{V} \right] \theta}\in SE\left( 3 \right) e[V]θ∈SE(3)
- Case 1 ω ⃗ = 0 \vec{\omega}=0 ω=0 : e [ V ] θ = [ E v ⃗ θ 0 1 ] e^{\left[ \mathcal{V} \right] \theta}=\left[ \begin{matrix} E& \vec{v}\theta\\ 0& 1\\ \end{matrix} \right] e[V]θ=[E0vθ1]
- Csae 2
ω
⃗
≠
0
\vec{\omega}\ne 0
ω=0 : without loss of generality assume
∥
ω
⃗
∥
=
1
\left\| \vec{\omega} \right\| =1
∥ω∥=1. Then:
e [ V ] θ = [ e ω ^ ~ θ G ( θ ) v ⃗ 0 1 ] , G ( θ ) = E θ + ( 1 − cos θ ) ω ^ ~ + ( θ − sin θ ) ω ^ ~ 2 e^{\left[ \mathcal{V} \right] \theta}=\left[ \begin{matrix} e^{\tilde{\hat{\omega}}\theta}& G\left( \theta \right) \vec{v}\\ 0& 1\\ \end{matrix} \right] ,G\left( \theta \right) =E\theta +\left( 1-\cos \theta \right) \tilde{\hat{\omega}}+\left( \theta -\sin \theta \right) \tilde{\hat{\omega}}^2 e[V]θ=[eω^~θ0G(θ)v1],G(θ)=Eθ+(1−cosθ)ω^~+(θ−sinθ)ω^~2
V = [ ω ⃗ v ⃗ ] ∈ R 6 \mathcal{V} =\left[ \begin{array}{c} \vec{\omega}\\ \vec{v}\\ \end{array} \right] \in \mathbb{R} ^6 V=[ωv]∈R6 , [ V ] = [ ω ⃗ ~ v ⃗ 0 1 ] ∈ R 4 × 4 \left[ \mathcal{V} \right] =\left[ \begin{matrix} \tilde{\vec{\omega}}& \vec{v}\\ 0& 1\\ \end{matrix} \right] \in \mathbb{R} ^{4\times 4} [V]=[ω~0v1]∈R4×4
e [ V ] θ ∈ S E ( 3 ) e^{\left[ \mathcal{V} \right] \theta}\in SE\left( 3 \right) e[V]θ∈SE(3) , [ V ] ∈ s e ( 3 ) \left[ \mathcal{V} \right] \in se\left( 3 \right) [V]∈se(3)
3.2 Log of SE(3) : from Rigid-Body Motion to Twist
Theorem 2 [Log of SE(3)] : Given any
[
T
]
=
(
[
Q
]
,
R
⃗
)
∈
S
E
(
3
)
\left[ T \right] =\left( \left[ Q \right] ,\vec{R} \right) \in SE\left( 3 \right)
[T]=([Q],R)∈SE(3) , one can always find twist
S
=
(
ω
⃗
,
v
⃗
)
\mathcal{S} =\left( \vec{\omega},\vec{v} \right)
S=(ω,v) and a scalar
θ
\theta
θ such that
e
[
S
]
θ
=
[
T
]
=
[
[
Q
]
R
⃗
0
1
]
e^{\left[ \mathcal{S} \right] \theta}=\left[ T \right] =\left[ \begin{matrix} \left[ Q \right]& \vec{R}\\ 0& 1\\ \end{matrix} \right]
e[S]θ=[T]=[[Q]0R1]
Matrix Logarithm Algorithm :
- If [ Q ] = E \left[ Q \right] =E [Q]=E , then set ω ⃗ = 0 \vec{\omega}=0 ω=0 , v ⃗ = R ⃗ / ∥ R ⃗ ∥ \vec{v}=\vec{R}/\left\| \vec{R} \right\| v=R/ R , and θ = ∥ p ⃗ ∥ \theta =\left\| \vec{p} \right\| θ=∥p∥
- Otherwise, use matrix logarithm on
S
O
(
3
)
SO\left( 3 \right)
SO(3) to determine
ω
⃗
\vec{\omega}
ω and
θ
\theta
θ from
[
Q
]
\left[ Q \right]
[Q]. Then
v
⃗
\vec{v}
v is calculated as
v
⃗
=
G
−
1
(
θ
)
R
⃗
\vec{v}=G^{-1}\left( \theta \right) \vec{R}
v=G−1(θ)R, where:
G − 1 ( θ ) = 1 θ E − 1 2 ω ^ ~ + ( 1 θ − 1 2 cos θ 2 ) ω ^ ~ 2 G^{-1}\left( \theta \right) =\frac{1}{\theta}E-\frac{1}{2}\tilde{\hat{\omega}}+\left( \frac{1}{\theta}-\frac{1}{2}\cos \frac{\theta}{2} \right) \tilde{\hat{\omega}}^2 G−1(θ)=θ1E−21ω^~+(θ1−21cos2θ)ω^~2
3.3 Exponential Coordinates of Rigid Transformation
-
To sum up, screw axis S = ( ω ⃗ , v ⃗ ) = ( s ^ , R ⃗ q , h ) \mathcal{S} =\left( \vec{\omega},\vec{v} \right) =\left( \hat{s},\vec{R}_q,h \right) S=(ω,v)=(s^,Rq,h) can be expressed as a normalized twist ; its matrix representation is
[ S ] = [ ω ⃗ ~ v ⃗ 0 0 ] ∈ s e ( 3 ) \left[ \mathcal{S} \right] =\left[ \begin{matrix} \tilde{\vec{\omega}}& \vec{v}\\ 0& 0\\ \end{matrix} \right] \in se\left( 3 \right) [S]=[ω~0v0]∈se(3) -
A point started at R ⃗ p ( 0 ) \vec{R}_p\left( 0 \right) Rp(0) at time zero, travel along screw axis S \mathcal{S} S at unit speed for time t t t will end uo at [ R ⃗ ˙ p 0 ] = [ S ] [ R ⃗ p 1 ] ⇒ [ R ⃗ p ( t ) 1 ] = e [ S ] t [ R ⃗ p ( 0 ) 1 ] \left[ \begin{array}{c} \dot{\vec{R}}_p\\ 0\\ \end{array} \right] =\left[ \mathcal{S} \right] \left[ \begin{array}{c} \vec{R}_p\\ 1\\ \end{array} \right] \Rightarrow \left[ \begin{array}{c} \vec{R}_p\left( t \right)\\ 1\\ \end{array} \right] =e^{\left[ \mathcal{S} \right] t}\left[ \begin{array}{c} \vec{R}_p\left( 0 \right)\\ 1\\ \end{array} \right] [R˙p0]=[S][Rp1]⇒[Rp(t)1]=e[S]t[Rp(0)1]
-
Given S \mathcal{S} S we can use Theorem 1 to compute e [ S ] t ∈ S E ( 3 ) e^{\left[ \mathcal{S} \right] t}\in SE\left( 3 \right) e[S]t∈SE(3)
-
Given [ T ] ∈ S E ( 3 ) \left[ T \right] \in SE\left( 3 \right) [T]∈SE(3) , we can use Theorem 2 to find S = ( ω ⃗ , v ⃗ ) \mathcal{S} =\left( \vec{\omega},\vec{v} \right) S=(ω,v) and θ \theta θ such that e [ S ] θ = [ T ] e^{\left[ \mathcal{S} \right] \theta}=\left[ T \right] e[S]θ=[T]
-
We call S θ \mathcal{S} \theta Sθ the Exponential Coordinates of the homogeneous transformation [ T ] ∈ S E ( 3 ) \left[ T \right] \in SE\left( 3 \right) [T]∈SE(3)
1735
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
