[足式机器人]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 dtdQA(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ω ~AOQOAT=QOAQ˙AOQAO−1QOAT=QOAQ˙AO=QAO−1Q˙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) dtdTA(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=XBOVB ------ 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=\[QR ~Q0Q]

• 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 QQA=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−1QQBO same rotation operator in { B } \left\{ B \right\} {B}

• Rigid body transformation and exp coordinate(wrt { O } \left\{ O \right\} {O})

  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×4R p1⇒R ˙p0=eVtR p1 e V t e^{\left \\mathcal{V} \\right t} eVt - 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 TTA : rotate frame { A } \left\{ A \right\} {A} about screw axis S ^ \hat{\mathcal{S}} S^ by θ \theta θ degree

• rigid operation of screw axis

• S ′ = A d T S \mathcal{S} ^{\prime}=\left Ad_T \\right \mathcal{S} S′=AdTS 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−1TTB

• 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