cohen

cohen

A man who want to make the world better.
首頁封存

南科大高等機器人控制課程筆記-Twist(空間速度)

#robot#math#笔记228
AI 翻譯
這篇文章透過AI由簡體中文翻譯成繁體中文。查看原文
AI 生成的摘要
大部分机器人可以视为由刚体组合而成，讨论刚体速度。物理世界中存在点和向量，刚体位置为点，速度和加速度为向量。刚体速度包括质心速度和围绕质心旋转的角速度。速度描述可以选择一个点作为参考点，任意点的速度可以通过参考点的速度和角速度来计算。速度描述与坐标系无关。

我們現實中大部分的機器人都可以視作由剛體組合而成，所以我們先討論剛體的速度。

物理量與數字之間的聯繫與區別#

在物理的抽象世界中，我們認為真實存在著兩種東西，分別是點和向量。

點就是物理世界中物理量的抽象表達，不依賴於任何坐標系而存在。

剛體的位置就可以抽象為一個點，無論是否建立坐標系以及建立什麼樣的坐標系，機器人的位置都不會隨之改變。只要剛體本身是靜止的，它在物理世界中的位置 (點)，就是固定的。

向量同樣是物理世界中物理量的抽象表達，不同於點的是，它有大小和方向。

剛體的速度、加速度就是一個向量。同樣的，無論坐標系怎麼改變，速度和加速度代表的向量是不會改變的。

這裡有一點需要注意的是，我在說的坐標系和高中物理我們學習過的參考系不是同一件事情。
![[機器人研究中的坐標系與參考系之間的區別與聯繫 #移動機器人的示例]]

當我們使用物理定律，對物理量進行表達和計算時，並且也不關心物理量具體的大小，只關心物理量之間的關係時，我們不需要坐標系，也不需要數字。

質點 $b$ 在空間運動，我們可以用一個矢量描述它的速度 vbv_b，同時我們可以使用速度的合成與分解這一物理定律，任意選取兩個矢量，使之滿足 vb1+vb2=vbv_{b_1} + v_{b_2} = v_b 。
我們可以說質點 bb 具有一個速度 vbv_b，它以 ∣vb∣|v_b| 大小的速度沿著 vbv_b 方向進行運動。
我們也可以說質點 bb 具有兩個速度 vb1v_{b_1} , vb2v_{b_2} 。

在上述這個例子中，我們沒有使用到坐標系和數字，我們使用了一個物理定律用符號表達了三個矢量之間的關係，同時也解釋了它們的物理意義。

針對點和向量，我們總是在應用的時候需要關心點到底在哪裡，向量的大小是多少，向量方向是什麼。

在實際應用過程中，我們當然希望知道機器人的位置在哪裡，機器人的速度是多少，它朝著什麼方向運動等等。

這個時候我們就需要定義坐標系，由坐標系而來，我們就可以用數字而不只是符號去表示物理量。

剛體的速度#

首先，我們要明確一點，到底什麼是剛體的速度？

在學習這門課之前，我對剛體速度的直觀理解就是剛體質心的速度以及剛體圍繞質心所在軸旋轉的角速度，它們整合起來就是剛體的速度。

這顯然是不夠準確的，所以在這裡，我們需要先定義剛體的速度。

首先，我們知道剛體是由無窮多個點組合而成。同時，我們知道剛體中的這些點之間的速度與它在剛體中的位置和其他的參數有關係（比如剛體繞軸旋轉的角速度）。

所以，我們可以用隱函數的形式表示剛體中任意一個點 pip_i 的速度是 vpi=f(pi,parameters)v_{p_i}=f(p_i, \text{parameters}) 。

基於這個形式，我們定義剛體的速度就是這裡的 parameters\text{parameters} 。

換而言之，只要我們知道了剛體的速度和一個點在剛體上的坐標，我們可以計算這個點的速度。

我們在實際中，一般都不關心剛體的速度。我們關心機械臂的末端速度、小車幾何中心的速度、四足機器人身體上中心點的速度等等。
但是如果我們一開始就計算出剛體的速度，那麼我們可以非常方便地計算出所有我們想要的點的速度。
反之，一開始就只算一個點，後續還想算其他點的話，就會變得麻煩且混亂。

當然，到這兒為止還太過於抽象了。接下來，我們一步一步的擴展和解釋，直到 spatial velocity。

image

如圖 1 所示剛體正在繞著一個固定軸旋轉，我們選擇剛體中在旋轉軸上的一個點 p0p_0 作為參考點。根據高中物理中角速度與線速度的關係，我們可以知道對於剛體中任意一個點 pip_i , 有 vpi=ω×p0pi→v_{p_i}=\omega \times \overrightarrow{p_0p_i}。

然後，為了讓我們的討論更具有普適性，向前邁進一步。假設剛體的旋轉軸不是固定，而是運動的。

image

p0p_0 仍然是剛體中在旋轉軸上的一點，我們選擇 p0p_0 作為描述質點速度的參考點，對於剛體中任意一個點 pip_i , 有 vpi=vp0+ω×p0pi→v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}。

再次提醒，我們目前的描述都是矢量之間的關係，所以不涉及任何坐標系，同時也意味著在任何一個坐標系中這些關係都是滿足的。

從物理意義上，我們來看看 vpi=vp0+ω×p0pi→v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i} 是如何得來的。

首先，本質上來說一個點的速度就是這個點的位置的變化，也就是它的導數，即vpi=pi˙v_{p_i}=\dot{p_i} 。同樣的，vp0=p0˙v_{p_0}=\dot{p_0} 。

接下來，為方便理解。我們先設坐標系原點 oo , 根據矢量合成的原則（對，你沒有想錯，這是物理規律！不是數學規則），必然有 opi→=op0→+p0pi→\overrightarrow{op_i}=\overrightarrow{op_0}+\overrightarrow{p_0p_i} 。

同時，由於剛體在運動，點 pi, p0p_i \text{, } p_0 是隨時間變化的。並且在任意 tt 時刻均有 opi→(t)=op0→(t)+p0pi→(t)\overrightarrow{op_i}(t)=\overrightarrow{op_0}(t)+\overrightarrow{p_0p_i}(t) 。

後續我們的符號表示中 pip_i 與 opi→\overrightarrow{op_i} 是同一個意思，使用 pip_i主要是為了強調它與坐標系無關，它本身以及它與其他矢量、點的關係都是無關乎坐標系的。

因此，我們有 pi(t)=p0(t)+p0pi→(t)p_i(t)=p_0(t)+\overrightarrow{p_0p_i}(t) ，等號兩邊同時求導 pi˙=p0˙+(p0pi→)′\dot{p_i}=\dot{p_0}+(\overrightarrow{p_0p_i})' 。最終，可以得到 vpi=vp0+ω×p0pi→v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}

(p0pi→)′(\overrightarrow{p_0p_i})' 中的 p0pi→\overrightarrow{p_0p_i} 長度始終沒有改變（因為這兩個點是剛體中的，剛體不會發生形變），只有方向改變了。
矢量 p0pi→\overrightarrow{p_0p_i} 的導數為ω×p0pi→\omega \times \overrightarrow{p_0p_i} 。
從物理意義上去理解ω×p0pi→\omega \times \overrightarrow{p_0p_i} 是 pip_i 這個點由角速度 ω\omega 所帶來的線速度。
{ps: 通過繪圖加上使用導數基本的定義公式很容易理解這一點}。

我們可以發現， vpi=vp0+ω×p0pi→v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i} 與 vpi=ω×p0pi→v_{p_i}= \omega \times \overrightarrow{p_0p_i} 區別就是 vp0=0v_{p_0}=0 與否。

所以無論剛體在空間中是不是在做純滾動，剛體中任意一點的速度與參考點 p0p_0 的速度之間的關係都滿足 vpi=vp0+ω×p0pi→ v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i}

結合 vpi=vp0+ω×p0pi→v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i} 與隱函數表達式 vpi=f(pi,parameters)v_{p_i}=f(p_i, \text{parameters}) ，我們根據剛體速度的定義，可以知道，此時剛體的速度是 (vp0, ω)(v_{p_0}\text{, }\omega)。

回顧前文：
我們可以用隱函數的形式表示剛體中任意一個點 pip_i 的速度是 vpi=f(pi,parameters)v_{p_i}=f(p_i, \text{parameters}) 。基於這個形式，我們定義剛體的速度就是其中的 parameters\text{parameters} 。

image

如圖 2，讓我們繼續往前走第二步，我們選擇一個剛體上任意一點 qq 作為表述速度的參考點。

這一步的意義是：讓我們在剛體速度的表達式中參考點的選擇範圍更廣一些，證明我們的參考點並不一定要選在旋轉軸上。

將參考點選在旋轉軸上自然很方便，因為可以不用考慮參考點會旋轉的問題。

但是在實際工程中，我們很多時候都沒辦法直接獲取旋轉軸的速度。
我們只能把測量速度的傳感器放在機器上合適的位置，沒辦法總是安裝在旋轉軸上。

CallBack：
只要我們知道了剛體的速度和一個點在剛體上的坐標，我們可以計算這個點的速度。
讓我們能更廣泛的選擇參考點，就是為了在眾多不完美的現實情況中，也能很好的計算剛體的速度。

則有 vpi=vq+ω×qpi→ v_{p_i}=v_{q} + \omega \times \overrightarrow{qp_i}
我們使用物理定律繼續來推導一下，這個式子是如何得來的。

對於點 qq ，有 vq=vp0+ω×p0q→(3) v_{q}=v_{p_0} + \omega \times \overrightarrow{p_0q} \quad\text{(3)}
對於點 pip_i，有 vpi=vp0+ω×p0pi→(4) v_{p_i}=v_{p_0} + \omega \times \overrightarrow{p_0p_i} \quad\text{(4)}

公式 (4)−(3)(4) - (3) 可得

vpi=vq+ω×p0pi→−ω×p0q→vpi=vq+ω×(p0pi→−p0q→)vpi=vq+ω×qpi→\begin{gathered} v_{p_i}=v_{q} + \omega \times \overrightarrow{p_0p_i} - \omega \times \overrightarrow{p_0q} \\ v_{p_i}=v_{q} + \omega \times (\overrightarrow{p_0p_i} - \overrightarrow{p_0q}) \\ v_{p_i}=v_{q} + \omega \times \overrightarrow{qp_i} \end{gathered}

同上，根據剛體速度的定義，此時剛體的速度是 (vq, ω)(v_{q}\text{, }\omega)。

CallBack:
我們可以用隱函數的形式表示剛體中任意一個點 pip_i 的速度是 vpi=f(pi,parameters)v_{p_i}=f(p_i, \text{parameters}) 。基於這個形式，我們定義剛體的速度就是其中的 parameters\text{parameters} 。

好了，進行到這裡，我們已經完成所有的基礎部分。

image

接下來需要往前邁出重要的第三步，如圖 3，在剛體之外，隨機選定一個 body-fixed 的點 q′q' 。

這一步的意義是：讓我們在剛體速度的表達式中參考點的選擇範圍再廣一些，證明我們的參考點並不一定要選在剛體上，只要滿足 body-fixed 就行。

在上述的第二步中，我們選的是剛體上任意的一點，現在我們選的是剛體之外但是 body-fixed 的任意一點。這二者意義完全一樣，其中的關鍵就是 body-fixed。

這裡可以用物理中” 場 “的概念來幫助理解 body-fixed。
比如磁鐵的磁場，如果我們在一個磁鐵的磁場中選定一點 A, 那麼雖然這個點不在磁鐵上，但它依然會隨著磁鐵運動而運動，也就是所謂的 body-fixed。

所以，基於第二步的結果我們還是能得到 vpi=vq′+ω×q′pi→(5) v_{p_i}=v_{q'} + \omega \times \overrightarrow{q'p_i} \quad\text{(5)}
再重複一次，這時我們圖中剛體的速度就是 (vq′,ω)(v_{q'}, \omega)。

好了，讓我們走向最終的第四步：讓我們在剛體速度的表達式中參考點的選擇範圍達到極限，證明我們的參考點並不一定要滿足 body-fixed。可以是任意的點，無論它動或者不動、是否 body-fixed 等等。

image

我們選擇一個坐標系 {A}\{A\} ，我們可以說每時每刻都有一個 body-fixed 的點與坐標系 {A}\{A\} 的原點 OAO_A 重合。

我們規定：數學符號 vOA(t)v_{O_A}(t) 代表 t 時刻與點 OAO_A 重合的 body-fixed 的點 q′q' 的速度。

這句話很重要，符號vOA(t)v_{O_A}(t) 代表 t 時刻與點 OAO_A 重合的 body-fixed 的點 q′q' 的速度。

而不是點 OAO_A 的速度，點 OAO_A 的速度我不關心，無論它是多少都無所謂。

所以，根據公式 (5) 我們將其中的 vq′v_{q'} 替換為 vOAv_{O_A} vpi=vOA+ω×OApi→(6) v_{p_i}=v_{O_A} + \omega \times \overrightarrow{O_Ap_i} \quad\text{(6)}
又因為點 OAO_A 是原點，所以如果我在坐標系 {A}\{A\} 下表述這個式子就可以寫作 Avpi=AvOA+Aω×AOApi→(6)^{A}v_{p_i}=^{A}v_{O_A} + ^{A}\omega \times ^{A}\overrightarrow{O_Ap_i} \quad\text{(6)}
Avpi=AvOA+Aω×Api(7)^{A}v_{p_i}=^{A}v_{O_A} + ^{A}\omega \times ^{A}p_i \quad\text{(7)}

最後一次，根據剛體速度的定義，我們可以知道這個時候剛體的速度是 (vOA,ω)(v_{O_A}, \omega)。

你應該感覺到了，這個 (vOA,ω)(v_{O_A}, \omega) 很不符合直覺，它肯定不是剛體質心在 XYZ 方向的速度，和繞 XYZ 的角速度，組合而成。它感覺 “很不像” 剛體的速度......
直覺的犧牲是有價值的，有了這個東西。我們後面算多剛體結構的機器人正逆運動學時會非常方便。

在教科書中給 (vOA,ω)(v_{O_A}, \omega) 的命名就是 spatial velocity 或者 旋量 (twist)。

看到這兒，肯定還是有點迷糊的，我們再多舉兩個例子。

image

令 ω=50 rad/s\omega = 50 \> rad/s，∣OBOA→∣=0.04|\overrightarrow{O_BO_A}| = 0.04 . 其中坐標系 {A}\{A\} 是固定不動的世界坐標系，坐標系 {B}\{B\} 是會跟著圓錐一起旋轉的。

注意 ω=50rad/s\omega = 50 rad/s 意思就是圓錐相對著世界坐標系的角速度。

我們先來看在坐標系 {A}\{A\} 的表述下，圓錐的旋量是：Aν=(AωAvOA)^{A}\nu=\left( \begin{array} {ccc} ^{A}\omega\\ ^{A}v_{O_A} \end{array} \right)
其中很自然的，Aω=(0050)^{A}\omega = \left( \begin{array} {ccc} 0\\ 0 \\ 50 \end{array} \right) 。再看 AvOA^{A}v_{O_A} 是多少？

CallBack：
符號vOA(t)v_{O_A}(t) 代表 t 時刻與點 OAO_A 重合的 body-fixed 的點 q′q' 的速度

所以，我們就直接找出這個 body-fixed 的點 q′q' 即可。

而 body-fixed 的點和剛體上的點，意義完全一樣，它們都隨著剛體運動而運動。

那麼，剛體上的點 q′q' 的速度怎麼求？ 直接套用公式 (5) vq′=vr+ω×rq′→ v_{q'}=v_{r} + \omega \times \overrightarrow{rq'}
其中 vrv_r 就是參考點速度， rr 是參考點。

注意速度 vv 和旋量 ν\nu 是不一樣的哈。

我們就可以選一個好用的參考點 OBO_B 來方便的求 vq′v_{q'}，因為我們知道 vOBv_{O_B} 為零。

圓錐只是在旋轉，沒有平移，所以 vOBv_{O_B} 為零。 ps：這個 vOBv_{O_B} 就是指剛體上 body-fixed 的點 OBO_B 的速度。
因為我們現在是在 {A}\{A\} 下表述旋量，所以只有 vOAv_{O_A} 是特殊。
再次 CallBack：
符號vOA(t)v_{O_A}(t) 代表 t 時刻與點 OAO_A 重合的 body-fixed 的點 q′q' 的速度，不是點 OAO_A 的速度。

於是，我們可以得到 Avq′=AvOB+Aω×AOBq′→ ^{A}v_{q'}=^{A}v_{O_B} + ^{A}\omega \times \overrightarrow{^{A}O_Bq'}
不要忘了！ 我們這裡的 $^{A} v_{q'}$ 其實就是我們規定的 AvOA^{A}v_{O_A} 。

AvOA=AvOB+Aω×AOBOA→AvOA=Aω×AOBOA→AvOA=(0050)×(0−0.040)AvOA=(200)\begin{align} ^{A}v_{O_A}&=^{A}v_{O_B} + ^{A}\omega \times ^{A}\overrightarrow{O_BO_A} \\ ^{A}v_{O_A}&= ^{A}\omega \times ^{A}\overrightarrow{O_BO_A} \\ ^{A}v_{O_A}&= \left( \begin{array} {ccc} 0 \\ 0 \\ 50 \end{array} \right) \times \left( \begin{array} {ccc} 0 \\ -0.04 \\ 0 \end{array} \right) \\ ^{A}v_{O_A}&= \left( \begin{array} {ccc} 2 \\ 0 \\ 0 \end{array} \right) \end{align}

至此，我們得到了旋量表示的剛體圓錐的速度，在坐標系 {A}\{A\} 下的表示為Aν=(0050200)^{A}\nu=\left( \begin{array} {ccc} 0 \\ 0 \\ 50 \\ 2 \\ 0 \\ 0 \end{array} \right)
接下來，再來計算一下剛體圓錐的速度，在坐標系 {B}\{B\} 下的表示是多少？

CallBack：
注意，這裡我們描述的圓錐的速度始終是相對於世界坐標系的。

我們只是在換不同的坐標系對這個已經存在的物理量進行數學表述。

Bν=(BωBvOA)^{B}\nu=\left( \begin{array} {ccc} ^{B}\omega\\ ^{B}v_{O_A} \end{array} \right)
首先是 Bω^{B}\omega ，從圖中可以看到 $^{A}\omega$ 是繞 $\widehat {z}$ 旋轉的，而 Bω^{B}\omega 是繞 $\widehat {x}$ 軸旋轉的。所以，Bω=(5000)^{B}\omega = \left( \begin{array} {ccc} 50\\ 0 \\ 0 \end{array} \right).

從圖中我們可以看出，在圓錐旋轉的整個過程裡，所有與這個圓錐 body-fixed 的點始終沒有變過，一直是圓錐的頂點，而且它既沒有旋轉又沒有平移。所以，BvOA=(000)^{B}v_{O_A} = \left( \begin{array} {ccc} 0\\ 0 \\ 0 \end{array} \right).
綜上所述 Bν=(5000000)^{B}\nu=\left( \begin{array} {ccc} 50\\ 0 \\0 \\0\\0\\0 \end{array} \right)
到這裡，我們可以引入一個重要的知識點：坐標變換。

既然，我們只是在不同的坐標系表述同一個物理矢量。那么這些不同坐標系的表示，它們之間應該如何轉換。這有利於我把我們關心的物理量，轉換了便於計算或者寫代碼的坐標系中。

[[Todo：旋量的坐標變換]]

再來一個移動機器人的例子。

image

圖中描述的是一個移動機器人繞著旋轉中心 rr 點旋轉， rr 點在坐標系 {s}\{s\} 中的坐標 sr=(2,−1,0)^{s}r = (2,-1,0)，在坐標系 {b}\{b\} 中的坐標是 br=(2,−1.4,0)^{b}r=(2,-1.4,0) 。旋轉的角速度是 ω=2 rad/s\omega = 2 \> rad/s 。

圖中共有兩個坐標系，世界坐標系 {s}\{s\} 與車身坐標系 {b}\{b\} . 都採用右手原則，其中 {s}\{s\} 的 zz 軸是指向紙面外，坐標系 {b}\{b\} 的 zz 軸與之相反。

類似圓錐的例子，我們先看坐標系 {s}\{s\} 中的旋量 sν=(sωsv)^{s}\nu=\left( \begin{array} {ccc} ^{s}\omega\\ ^{s}v \end{array} \right)

這裡的符號 sv^{s}v 與 svOs^{s}v_{O_s} 同義，後續如果不是明確需要都用 sv^{s}v 表示 svOs^{s}v_{O_s}。

其中，sω=(002)^{s}\omega = \left( \begin{array} {ccc} 0\\ 0 \\ 2 \end{array} \right),

sv=vr+sω×srOs→sv=0+ω×(−sr)sv=(002)×(−210)sv=(−2−40)\begin{align}^{s}v &= v_r + ^{s}\omega \times \overrightarrow{^{s}rO_{s}} \\^{s}v&=0 + \omega \times (-^{s}r) \\^{s}v&= \left( \begin{array} {ccc} 0\\ 0 \\ 2 \end{array} \right) \times \left( \begin{array} {ccc} -2\\ 1 \\ 0 \end{array} \right)\\ ^{s}v&= \left( \begin{array} {ccc} -2\\ -4 \\ 0 \end{array} \right)\end{align}

再算一次，坐標系 {b}\{b\} 中的旋量，答案是（過程嘗試手算一下以作練習） bν=(00−22.840)^{b}\nu=\left( \begin{array} {ccc} 0\\ 0\\-2\\2.8\\4\\0 \end{array} \right)

參考資料：
南科大張巍老師的高等機器人控制課程課程官方頁面（作業 + PPT）B 站課程視頻地址
Modern Robotics: Mechanics, Planning, and Control," Kevin M. Lynch and Frank C. Park, Cambridge University Press, 2017 書籍官方網站（示例程序源碼 + PDF 下載）

載入中......
此文章數據所有權由區塊鏈加密技術和智能合約保障僅歸創作者所有。