[足式机器人]Part4 南科大高等机器人控制课 Ch05 Instantaneous Velocity of Moving Frames

本文仅供学习使用
本文参考：
B站：CLEAR_LAB
笔者带更新-运动学
课程主讲教师：
Prof. Wei Zhang

南科大高等机器人控制课 Ch05 Instantaneous Velocity of Moving Frames

• [1.Instantanenous Velocity of Rotating Frames](#1.Instantanenous Velocity of Rotating Frames)
• [2.Instantanenous Velocity of Moving Frames](#2.Instantanenous Velocity of Moving Frames)
• [3.Review/Summary of Rigid Body Velocity & Operation](#3.Review/Summary of Rigid Body Velocity & Operation)

---

Given Frame trajectory [ T ] ( t ) = ( [ Q ] ( t ) , R ⃗ p ( t ) ) \left[ T \right] \left( t \right) =\left( \left[ Q \right] \left( t \right) ,\vec{R}_p\left( t \right) \right) [T](t)=([Q](t),R p(t)) wrt { O } \left\{ O \right\} {O}

Question : What is the velocity (twist/spatial velocity) of frame at time t t t

T \] ( t ) ∈ R 4 × 4 \\left\[ T \\right\] \\left( t \\right) \\in \\mathbb{R} \^{4\\times 4} \[T\](t)∈R4×4 : 4 × 4 4\\times 4 4×4 matrix S E ( 3 ) SE\\left( 3 \\right) SE(3) log ⁡ ( \[ T \] ( t ) ) → S θ \\log \\left( \\left\[ T \\right\] \\left( t \\right) \\right) \\rightarrow \\mathcal{S} \\theta log(\[T\](t))→Sθ : is S \\mathcal{S} S (unit) the velocity of \[ T \] ( t ) \\left\[ T \\right\] \\left( t \\right) \[T\](t)? ------ **no** R ⃗ p ( t ) \\vec{R}_p\\left( t \\right) R p(t) : position vector ; R ⃗ ˙ p ( t ) \\dot{\\vec{R}}_p\\left( t \\right) R ˙p(t) velocity vector; S θ ↔ \[ T \] ( t ) \\mathcal{S} \\theta \\leftrightarrow \\left\[ T \\right\] \\left( t \\right) Sθ↔\[T\](t) : position coordination (what velocity?------ \[ T ˙ \] ( t ) \\left\[ \\dot{T} \\right\] \\left( t \\right) \[T˙\](t)?) \[ T ˙ \] ( t ) ∈ R 4 × 4 ∉ S E ( 3 ) \\left\[ \\dot{T} \\right\] \\left( t \\right) \\in \\mathbb{R} \^{4\\times 4}\\notin SE\\left( 3 \\right) \[T˙\](t)∈R4×4∈/SE(3) ## 1.Instantanenous Velocity of Rotating Frames { A } \\left\\{ A \\right\\} {A} frame is rotating with orientation \[ Q A \] ( t ) \\left\[ Q_A \\right\] \\left( t \\right) \[QA\](t) and velocity ω ⃗ A ( t ) \\vec{\\omega}_A\\left( t \\right) ω A(t) at time t t t (**Note: everything is wrt { O } \\left\\{ O \\right\\} {O} frame**) Let ω \^ θ = log ⁡ ( \[ Q A \] ( t ) ) \\hat{\\omega}\\theta =\\log \\left( \\left\[ Q_A \\right\] \\left( t \\right) \\right) ω\^θ=log(\[QA\](t)) can be obtained from the reference frame (say { O } \\left\\{ O \\right\\} {O} frame) by rotating about ω \^ \\hat{\\omega} ω\^ by θ \\theta θ degree * ω \^ θ \\hat{\\omega}\\theta ω\^θ only describes the current orientation of { A } \\left\\{ A \\right\\} {A} relative to { O } \\left\\{ O \\right\\} {O} , it does not contain info about how the frame is rotating at time t t t > What is the relation between ω ⃗ A ( t ) \\vec{\\omega}_A\\left( t \\right) ω A(t) and \[ Q A \] ( t ) \\left\[ Q_A \\right\] \\left( t \\right) \[QA\](t) > d d t \[ Q A \] ( t ) = ω ⃗ \~ A ( t ) \[ Q A \] ( t ) ⇒ ω ⃗ \~ A ( t ) = \[ Q ˙ A \] ( t ) \[ Q A \] − 1 ( t ) 1 \\frac{\\mathrm{d}}{\\mathrm{d}t}\\left\[ Q_A \\right\] \\left( t \\right) =\\tilde{\\vec{\\omega}}_A\\left( t \\right) \\left\[ Q_A \\right\] \\left( t \\right) \\Rightarrow \\tilde{\\vec{\\omega}}_A\\left( t \\right) =\\left\[ \\dot{Q}_A \\right\] \\left( t \\right) \\left\[ Q_A \\right\] \^{-1}\\left( t \\right) 1 dtd\[QA\](t)=ω \~A(t)\[QA\](t)⇒ω \~A(t)=\[Q˙A\](t)\[QA\]−1(t)1 > What is ω ⃗ A A \\vec{\\omega}_{\\mathrm{A}}\^{A} ω AA? > ω ⃗ A A = \[ Q O A \] ω ⃗ A O \\vec{\\omega}_{\\mathrm{A}}\^{A}=\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \\vec{\\omega}_{\\mathrm{A}}\^{O} ω AA=\[QOA\]ω AO > > velocity of A A A relative to { O } \\left\\{ O \\right\\} {O} , expressed in { A } \\left\\{ A \\right\\} {A} > ω ⃗ \~ A A = \[ Q O A \] ω ⃗ A O \~ = \[ Q O A \] ω ⃗ \~ A O \[ Q O A \] T = \[ Q O A \] \[ Q ˙ A O \] \[ Q A O \] − 1 \[ Q O A \] T = \[ Q O A \] \[ Q ˙ A O \] = \[ Q A O \] − 1 \[ Q ˙ A O \] \\tilde{\\vec{\\omega}}_{\\mathrm{A}}\^{A}=\\widetilde{\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \\vec{\\omega}_{\\mathrm{A}}\^{O}}=\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \\tilde{\\vec{\\omega}}_{\\mathrm{A}}\^{O}\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \^{\\mathrm{T}}=\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \\left\[ \\dot{Q}_{\\mathrm{A}}\^{O} \\right\] \\left\[ Q_{\\mathrm{A}}\^{O} \\right\] \^{-1}\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \^{\\mathrm{T}}=\\left\[ Q_{\\mathrm{O}}\^{A} \\right\] \\left\[ \\dot{Q}_{\\mathrm{A}}\^{O} \\right\] =\\left\[ Q_{\\mathrm{A}}\^{O} \\right\] \^{-1}\\left\[ \\dot{Q}_{\\mathrm{A}}\^{O} \\right\] ω \~AA=\[QOA\]ω AO =\[QOA\]ω \~AO\[QOA\]T=\[QOA\]\[Q˙AO\]\[QAO\]−1\[QOA\]T=\[QOA\]\[Q˙AO\]=\[QAO\]−1\[Q˙AO

2.Instantanenous Velocity of Moving Frames

{ A } \left\{ A \right\} {A} moving frame with configuration [ T A ] ( t ) \left[ T_A \right] \left( t \right) [TA](t) at t ime t t t undergoes a rigid body motion with velocity V A ( t ) = ( ω ⃗ , v ⃗ ) \mathcal{V} _A\left( t \right) =\left( \vec{\omega},\vec{v} \right) VA(t)=(ω ,v ) (Note: everything is wrt { O } \left\{ O \right\} {O} frame)

The exponential coordinate S ^ ( t ) θ ( t ) = log ⁡ ( [ T A ] ( t ) ) \hat{\mathcal{S}}\left( t \right) \theta \left( t \right) =\log \left( \left[ T_A \right] \left( t \right) \right) S^(t)θ(t)=log([TA](t)) only indicates the current configuration of { A } \left\{ A \right\} {A} , and does not tell us about how the frame is moving at time t t t

What is the relation between V A ( t ) \mathcal{V} _A\left( t \right) VA(t) and [ T A ] ( t ) \left[ T_A \right] \left( t \right) [TA](t) ?
d d t [ T A ] ( t ) = [ V A ] ( t ) [ T A ] ( t ) ⇒ [ V A ] ( t ) = [ T ˙ A ] ( t ) [ T A ] − 1 ( t ) \frac{\mathrm{d}}{\mathrm{d}t}\left[ T_A \right] \left( t \right) =\left[ \mathcal{V} _A \right] \left( t \right) \left[ T_A \right] \left( t \right) \Rightarrow \left[ \mathcal{V} _A \right] \left( t \right) =\left[ \dot{T}_A \right] \left( t \right) \left[ T_A \right] ^{-1}\left( t \right) dtd[TA](t)=[VA](t)[TA](t)⇒[VA](t)=[T˙A](t)[TA]−1(t)

3.Review/Summary of Rigid Body Velocity & Operation

• spatial velocity / twist V = [ ω ⃗ v ⃗ O ] \mathcal{V} =\left[ \begin{array}{c} \vec{\omega}\\ \vec{v}{\mathrm{O}}\\ \end{array} \right] V=[ω v O] , v ⃗ O \vec{v}{\mathrm{O}} v O reference point O O O may/may not move with the body
  ω ⃗ \vec{\omega} ω : angular velocity
  v ⃗ O \vec{v}_{\mathrm{O}} v O : velocity of the body-fixed partical currently coincides with O O O

  Given V = [ ω ⃗ v ⃗ O ] \mathcal{V} =\left[ \begin{array}{c} \vec{\omega}\\ \vec{v}{\mathrm{O}}\\ \end{array} \right] V=[ω v O] , any body fixed point P P P , its velocity is v ⃗ P = v ⃗ O + ω ⃗ × R ⃗ O P \vec{v}{\mathrm{P}}=\vec{v}{\mathrm{O}}+\vec{\omega}\times \vec{R}{\mathrm{OP}} v P=v O+ω ×R OP

• Twist in frames : Given frame { B } \left\{ B \right\} {B} , { O } \left\{ O \right\} {O} with relation
  V O = [ ω ⃗ O v ⃗ O O ] , V B = [ ω ⃗ B v ⃗ O B ] \mathcal{V} ^O=\left[ \begin{array}{c} \vec{\omega}^O\\ \vec{v}{\mathrm{O}}^{O}\\ \end{array} \right] ,\mathcal{V} ^B=\left[ \begin{array}{c} \vec{\omega}^B\\ \vec{v}{\mathrm{O}}^{B}\\ \end{array} \right] VO=[ω Ov OO],VB=[ω Bv OB] ------ V \mathcal{V} V is a twist of some rigid body
  V O = [ X B O ] V B \mathcal{V} ^O=\left[ X_{\mathrm{B}}^{O} \right] \mathcal{V} ^B VO=[XBO]VB ------ change of coordinate for twist [ X B O ] ∈ R 6 × 6 \left[ X_{\mathrm{B}}^{O} \right] \in \mathbb{R} ^{6\times 6} [XBO]∈R6×6

  X B O \] ∈ R 6 × 6 \[ X B O \] = \[ \[ Q B O \] 0 R ⃗ \~ B O \[ Q B O \] \[ Q B O \] \] \\left\[ X_{\\mathrm{B}}\^{O} \\right\] \\in \\mathbb{R} \^{6\\times 6}\\left\[ X_{\\mathrm{B}}\^{O} \\right\] =\\left\[ \\begin{matrix} \\left\[ Q_{\\mathrm{B}}\^{O} \\right\]\& 0\\\\ \\tilde{\\vec{R}}_{\\mathrm{B}}\^{O}\\left\[ Q_{\\mathrm{B}}\^{O} \\right\]\& \\left\[ Q_{\\mathrm{B}}\^{O} \\right\]\\\\ \\end{matrix} \\right\] \[XBO\]∈R6×6\[XBO\]=\[\[QBO\]R \~BO\[QBO\]0\[QBO\]

  For given [ T ] = ( [ Q ] , R ⃗ ) ⇒ [ X ] = [ A d T ] = [ [ Q ] 0 R ⃗ ~ [ Q ] [ Q ] ] \left[ T \right] =\left( \left[ Q \right] ,\vec{R} \right) \Rightarrow \left[ X \right] =\left[ Ad_T \right] =\left[ \begin{matrix} \left[ Q \right]& 0\\ \tilde{\vec{R}}\left[ Q \right]& \left[ Q \right]\\ \end{matrix} \right] [T]=([Q],R )⇒[X]=[AdT]=[[Q]R ~[Q]0[Q]]

• screw axis : S = ( s ^ , R ⃗ q , h ) ↔ [ ω ⃗ v ⃗ ] = [ s ^ h s ^ − s ^ × R ⃗ q ] \mathcal{S} =\left( \hat{s},\vec{R}{\mathrm{q}},h \right) \leftrightarrow \left[ \begin{array}{c} \vec{\omega}\\ \vec{v}\\ \end{array} \right] =\left[ \begin{array}{c} \hat{s}\\ h\hat{s}-\hat{s}\times \vec{R}{\mathrm{q}}\\ \end{array} \right] S=(s^,R q,h)↔[ω v ]=[s^hs^−s^×R q]

  Al rigid body motion can be "thought of " screw motion rotation & linear motion along the axis

  we typically write V = S θ ˙ \mathcal{V} =\mathcal{S} \dot{\theta} V=Sθ˙ ( S \mathcal{S} S - 'unit' normalized twist)

• rotation operation / Exp coordinate (wrt { O } \left\{ O \right\} {O})

  OED for rotation : R ⃗ ˙ p = ω ⃗ × R ⃗ p = ω ⃗ ~ R ⃗ p ⇒ R ⃗ p ( t ) = e ω ⃗ ~ t R ⃗ p ( 0 ) , s o ( 3 ) = { ω ⃗ ~ : ω ⃗ ∈ R 3 } \dot{\vec{R}}{\mathrm{p}}=\vec{\omega}\times \vec{R}{\mathrm{p}}=\tilde{\vec{\omega}}\vec{R}{\mathrm{p}}\Rightarrow \vec{R}{\mathrm{p}}\left( t \right) =e^{\tilde{\vec{\omega}}t}\vec{R}_{\mathrm{p}}\left( 0 \right) ,so\left( 3 \right) =\left\{ \tilde{\vec{\omega}}:\vec{\omega}\in \mathbb{R} ^3 \right\} R ˙p=ω ×R p=ω ~R p⇒R p(t)=eω ~tR p(0),so(3)={ω ~:ω ∈R3}

  if ω ⃗ = ω ^ \vec{\omega}=\hat{\omega} ω =ω^ , unit vector , t = θ t=\theta t=θ, ω ^ θ ↔ [ Q ] = e ω ⃗ ~ θ ∈ S O ( 3 ) \hat{\omega}\theta \leftrightarrow \left[ Q \right] =e^{\tilde{\vec{\omega}}\theta}\in SO\left( 3 \right) ω^θ↔[Q]=eω ~θ∈SO(3), ω ^ θ \hat{\omega}\theta ω^θ os calld exponention cooedinate of [ Q ] \left[ Q \right] [Q] denoted log ⁡ ( [ Q ] ) \log \left( \left[ Q \right] \right) log([Q])
  R ⃗ p ′ = e ω ⃗ ~ θ R ⃗ p \vec{R}{\mathrm{p}^{\prime}}=e^{\tilde{\vec{\omega}}\theta}\vec{R}{\mathrm{p}} R p′=eω ~θR p

  Given a frame { A } \left\{ A \right\} {A}, [ Q A ] = [ x ^ A , y ^ A , z ^ A ] \left[ Q_A \right] =\left[ \hat{x}_A,\hat{y}_A,\hat{z}_A \right] [QA]=[x^A,y^A,z^A] then

  Q \] \[ Q A \] = e ω ⃗ \~ θ \[ Q A \] \\left\[ Q \\right\] \\left\[ Q_A \\right\] =e\^{\\tilde{\\vec{\\omega}}\\theta}\\left\[ Q_A \\right\] \[Q\]\[QA\]=eω \~θ\[QA\] : \[ Q \] \\left\[ Q \\right\] \[Q\] action operation , means rotate \[ Q A \] \\left\[ Q_A \\right\] \[QA\] about ω \^ \\hat{\\omega} ω\^ by θ \\theta θ degree Experssion of rotation operator \[ Q \] \\left\[ Q \\right\] \[Q\] in { O } \\left\\{ O \\right\\} {O} and { B } \\left\\{ B \\right\\} {B} : \[ Q \] \\left\[ Q \\right\] \[Q\] in { O } \\left\\{ O \\right\\} {O} ; \[ Q B O \] − 1 \[ Q \] \[ Q B O \] \\left\[ Q_{\\mathrm{B}}\^{O} \\right\] \^{-1}\\left\[ Q \\right\] \\left\[ Q_{\\mathrm{B}}\^{O} \\right\] \[QBO\]−1\[Q\]\[QBO\] same rotation operator in { B } \\left\\{ B \\right\\} {B}

  ODE : R ⃗ ˙ p = v ⃗ + ω ⃗ × R ⃗ p ⇒ [ R ⃗ ˙ p 0 ] = [ ω ⃗ ~ v ⃗ 0 0 ] ∣ 4 × 4 [ R ⃗ p 1 ] ⇒ [ R ⃗ ˙ p 0 ] = e [ V ] t [ R ⃗ p 1 ] \dot{\vec{R}}{\mathrm{p}}=\vec{v}+\vec{\omega}\times \vec{R}{\mathrm{p}}\Rightarrow \left[ \begin{array}{c} \dot{\vec{R}}{\mathrm{p}}\\ 0\\ \end{array} \right] =\left. \left[ \begin{matrix} \tilde{\vec{\omega}}& \vec{v}\\ 0& 0\\ \end{matrix} \right] \right|{4\times 4}\left[ \begin{array}{c} \vec{R}{\mathrm{p}}\\ 1\\ \end{array} \right] \Rightarrow \left[ \begin{array}{c} \dot{\vec{R}}{\mathrm{p}}\\ 0\\ \end{array} \right] =e^{\left[ \mathcal{V} \right] t}\left[ \begin{array}{c} \vec{R}_{\mathrm{p}}\\ 1\\ \end{array} \right] R ˙p=v +ω ×R p⇒[R ˙p0]=[ω ~0v 0] 4×4[R p1]⇒[R ˙p0]=e[V]t[R p1] e [ V ] t e^{\left[ \mathcal{V} \right] t} e[V]t - matrix representation of V \mathcal{V} V, s e ( 3 ) = { [ V ] , V ∈ R 6 } se\left( 3 \right) =\left\{ \left[ \mathcal{V} \right] ,\mathcal{V} \in \mathbb{R} ^6 \right\} se(3)={[V],V∈R6}
  
  S ^ θ \hat{\mathcal{S}}\theta S^θ is the exp coordinate of [ T ] \left[ T \right] [T]

  R ⃗ p ′ 1 \] = \[ T \] \[ R ⃗ p 1 \] \\left\[ \\begin{array}{c} \\vec{R}_{\\mathrm{p}\^{\\prime}}\\\\ 1\\\\ \\end{array} \\right\] =\\left\[ T \\right\] \\left\[ \\begin{array}{c} \\vec{R}_{\\mathrm{p}}\\\\ 1\\\\ \\end{array} \\right\] \[R p′1\]=\[T\]\[R p1

  T \] \[ T A \] \\left\[ T \\right\] \\left\[ T_A \\right\] \[T\]\[TA\] : rotate frame { A } \\left\\{ A \\right\\} {A} about screw axis S \^ \\hat{\\mathcal{S}} S\^ by θ \\theta θ degree

• S ′ = [ A d T ] S \mathcal{S} ^{\prime}=\left[ Ad_T \right] \mathcal{S} S′=[AdT]S means rotate about axis S \mathcal{S} S along S ^ \hat{\mathcal{S}} S^ by θ \theta θ degree

• expression of [ T ] \left[ T \right] [T] in { B } \left\{ B \right\} {B}: [ T B ] − 1 [ T ] [ T B ] \left[ T_{\mathrm{B}} \right] ^{-1}\left[ T \right] \left[ T_{\mathrm{B}} \right] [TB]−1[T][TB]

• velocity of moving frame [ T ] ( t ) \left[ T \right] \left( t \right) [T](t) : [ V ] = [ T ˙ ] [ T ] − 1 \left[ \mathcal{V} \right] =\left[ \dot{T} \right] \left[ T \right] ^{-1} [V]=[T˙][T]−1