機(jī)器人控制系統(tǒng)運(yùn)動(dòng)學(xué)方程

機(jī)器人控制系統(tǒng)運(yùn)動(dòng)學(xué)方程控制機(jī)器人系統(tǒng)的運(yùn)動(dòng)通常要求了解終端受動(dòng)物體的方位（通常指工具提示或者受控參照系或者受控坐標(biāo)系統(tǒng)）與物理控制的用于操作終端受動(dòng)物體的執(zhí)行器或者電機(jī)方位（通常指受控參照系或者受控坐標(biāo)系統(tǒng)）之間的關(guān)系。這些知識(shí)可以大體表征機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)結(jié)構(gòu)，通常用運(yùn)動(dòng)學(xué)方程表示。一些高性能運(yùn)動(dòng)控制器具有整理這些方程并得到相對運(yùn)動(dòng)軌跡的能力，確保機(jī)器人系統(tǒng)的實(shí)時(shí)位置控制。本文中，將介紹2種特定機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)方程。

為什么使用運(yùn)動(dòng)學(xué)？3自由度起重機(jī)

3自由度（DOF）起重機(jī)是一個(gè)相對簡單的機(jī)器人系統(tǒng)，用戶可以控制起重距離、起重機(jī)回轉(zhuǎn)角度和起重機(jī)的傾角（見圖1）。通常一臺(tái)起重機(jī)還有第四個(gè)自由度——起重物體的懸掛高度，這里不考慮此自由度的作用，并不會(huì)影響分析結(jié)果的普適性。

圖1一臺(tái)如圖所示的三自由度起重機(jī)，允許用戶控制起重距離、回轉(zhuǎn)角和傾斜角，但是不允許起重機(jī)的工作端作直線運(yùn)動(dòng)。通常，起重機(jī)的起重臂旋轉(zhuǎn)路徑就像一個(gè)圓弧，而且，如果起重臂傾斜角增加，終端受動(dòng)物體（起重機(jī)末端的起重物）就會(huì)沿著圓弧路徑向上。這些自然形成的軌跡對于某些環(huán)境是可以接受的，然而，一旦用戶希望起重物體的運(yùn)動(dòng)路徑是由很多線段組成的話，該怎么辦？或者是在任意三個(gè)方向上的隨意形狀，又該如何？例如起重物體是某些測量設(shè)備或者圖像采集系統(tǒng)。簡單的例子是將起重物體沿著方形軌跡移動(dòng)，這對于一些系統(tǒng)是很有用的。

那些學(xué)習(xí)過坐標(biāo)幾何學(xué)的人應(yīng)該對起重機(jī)的受控坐標(biāo)系統(tǒng)很熟悉——通常就是球坐標(biāo)系?？臻g中的一個(gè)點(diǎn)可以用球坐標(biāo)系中的三個(gè)參數(shù)來表征：距離原點(diǎn)的距離、x-y平面上距離x軸的方位角θ（0–2π區(qū)間）和與z周的夾角θ（0–π區(qū)間）。球坐標(biāo)系的圖例如圖2所示。

圖2那些學(xué)習(xí)過坐標(biāo)幾何學(xué)的人應(yīng)該對起重機(jī)的受控坐標(biāo)系統(tǒng)很熟悉，通常就是球坐標(biāo)系。

沿著線段作移動(dòng)，我們希望在標(biāo)準(zhǔn)3D笛卡爾坐標(biāo)系中工作。笛卡爾坐標(biāo)系中的一個(gè)點(diǎn)由(x,y,z)表征，從直觀上說，這個(gè)坐標(biāo)系更便于進(jìn)行線段位移控制。例如，方形的運(yùn)動(dòng)軌跡由4條線段組成，線段運(yùn)動(dòng)就是笛卡爾坐標(biāo)系中最基本的運(yùn)動(dòng)模式。問題轉(zhuǎn)化為：我們?nèi)绾卧谶@兩種坐標(biāo)系統(tǒng)進(jìn)行轉(zhuǎn)換。答案是運(yùn)動(dòng)方程。運(yùn)動(dòng)方程可以將笛卡爾坐標(biāo)系(x,y,z)與起重機(jī)球坐標(biāo)系(r,θ,Φ)聯(lián)系在一起。

在進(jìn)一步探討之前，讓我們快速地判斷一下，為什么這些方程是必要的。如果用戶想要在笛卡爾坐標(biāo)系下控制運(yùn)動(dòng)路徑，他/她就需要確定一條由一系列(x,y,z)坐標(biāo)位置組成的軌跡。當(dāng)使用運(yùn)動(dòng)控制器時(shí)，對于很多種類的運(yùn)動(dòng)，明確地指明運(yùn)動(dòng)軌跡是沒有必要的。運(yùn)動(dòng)控制通常產(chǎn)生一個(gè)運(yùn)動(dòng)輪廓（一系列(x,y,z)坐標(biāo)位置）用于控制運(yùn)動(dòng)，例如點(diǎn)到點(diǎn)運(yùn)動(dòng)就意味著笛卡爾坐標(biāo)系下的直線運(yùn)動(dòng)。如果我們知道受動(dòng)物體的目標(biāo)(x,y,z)位置，然后就可以反推運(yùn)動(dòng)方程，運(yùn)動(dòng)控制器就可以計(jì)算出如何控制實(shí)際的起重機(jī)（包括起重臂長度、傾斜角和回轉(zhuǎn)角——(r,θ,Φ)）

前向運(yùn)動(dòng)方程更多地用于初次校準(zhǔn)。他們可以用于測量反饋位置，并將(r,θ,Φ)結(jié)果轉(zhuǎn)換為用戶更加關(guān)心的(x,y,z)坐標(biāo)。這個(gè)過程也可以用于確定安裝位置，和用于將任意位置的起重機(jī)坐標(biāo)初始化為相對的(x,y,z)坐標(biāo)。由此可見運(yùn)動(dòng)方程的必要性，現(xiàn)在就該討論如何解運(yùn)動(dòng)方程了。先從反推運(yùn)動(dòng)學(xué)方程開始，我們希望得到起重機(jī)的(r,θ,Φ)坐標(biāo)：

實(shí)際上依靠對球坐標(biāo)系/笛卡爾坐標(biāo)系的觀察就可以解這個(gè)等式，使用一些三角公式，可以得到如下等式：

觀察上面第三個(gè)等式，Φ是由關(guān)于r的等式表述的，而r又可由第一個(gè)等式中的(x,y,z)解出。前向運(yùn)動(dòng)方程的形式類似：

通過觀察，這幾個(gè)方程同樣可以輕松解出：

更復(fù)雜的例子——6自由度Stewart六腳平臺(tái)

Stewart六腳平臺(tái)在很多場合都有應(yīng)用，包括自動(dòng)檢測、機(jī)器人手術(shù)、人造衛(wèi)星和望遠(yuǎn)鏡定位以及機(jī)械仿真等等。六腳包括6個(gè)獨(dú)立的受控執(zhí)行器（長度），在一端匯聚到一個(gè)固定的基座，另一端與一平面平臺(tái)連接，允許6個(gè)自由度，(α(roll),(pitch),γ(yaw),x,y,z)。幾何學(xué)實(shí)例如圖3所示。

圖3Stewart六軸平臺(tái)在很多場合都有應(yīng)用，包括自動(dòng)檢測、機(jī)器人手術(shù)、人造衛(wèi)星和望遠(yuǎn)鏡定位以及機(jī)械仿真等等。來源:ACSMotionControl對于此系統(tǒng)，反推運(yùn)動(dòng)學(xué)方程可以告訴我們：對于給定的(α,,γ,x,y,z)，可以知道執(zhí)行器的長度(l1,l2,l3,l4,l5,l6)是多少，還可以知道姿態(tài)（P）。前向方程用于計(jì)算姿態(tài)P，用執(zhí)行器的腳長度I表示。前向運(yùn)動(dòng)方程是封閉的方程組，傳統(tǒng)計(jì)算方法是不可解的。但是，可以通過使用牛頓迭代法來解此前向方程，下文將作討論。

為了解此系統(tǒng)的反向運(yùn)動(dòng)方程，必須確定平臺(tái)種類以及執(zhí)行器匯聚點(diǎn)的位置，因?yàn)槟_長就是點(diǎn)與點(diǎn)之間的距離。平臺(tái)執(zhí)行器所處位置用基點(diǎn)坐標(biāo)系表示如下：

上述等式的下標(biāo)表明了向量的參考坐標(biāo)系。這里，點(diǎn)的位置實(shí)際上是齊次坐標(biāo)，以(x,y,z,w)或者(x/w,y/w,z/w)的形式表述，為了簡化討論，這里的w我們可以令其等于1。R是變換矩陣，可以將平臺(tái)點(diǎn)（Ppi）也就是平臺(tái)系數(shù)轉(zhuǎn)換成(Bpi)，也就是基座系數(shù)。R是3×4的矩陣，包括3×3的旋轉(zhuǎn)矩陣和3×1的平移矩陣。

在R等式中，“s”代表正弦函數(shù)；“c”代表余弦函數(shù)。上述等式中的旋轉(zhuǎn)矩陣是單位矩陣，用來變換roll、pitch和yaw三個(gè)向量的方向。平移矩陣就是一個(gè)簡單的向量。由于（Bbi）的值是已知的，所以一旦知道了(Bpi)的值，就可以通過計(jì)算兩點(diǎn)的距離得到腳的長度。

上面的等式實(shí)際上很簡單，但是由于引入了矩陣?yán)碚摚杂泻芏囗?xiàng)。下面是最終的反向運(yùn)動(dòng)方程（對于腳“i”）。

此系統(tǒng)的前向運(yùn)動(dòng)學(xué)方程相對復(fù)雜一些，由于處理上的要求，前向方程也不容易解。這里，比直接解方程更好的方法是使用迭代法，初始估計(jì)值代入方程、更新，然后重復(fù)，直到估計(jì)值的誤差小于某一限定值。具體的計(jì)算方法在此就不再贅述，此方法對于以下的過程都可以通用。此方法是前文提及的牛頓迭代法的推廣。關(guān)于此方法有很多相關(guān)文章，本文在此著重討論前向運(yùn)動(dòng)方程的應(yīng)用。第一步是估計(jì)初始姿態(tài)K，或者換種說法，估計(jì)(α,,γ,x,y,z)的值。對于一個(gè)運(yùn)動(dòng)控制器，初始估計(jì)值通常是(α,,γ,x,y,z)的受控位置。從此估計(jì)值，反推運(yùn)動(dòng)學(xué)方程，可以計(jì)算出執(zhí)行器的長度，稱之為(g1,g2,g3,g4,g5,g6)，或者以向量的形式寫成g.，數(shù)學(xué)表達(dá)式如下：g=I(k)。

然后，基于估計(jì)值算出的長度與來自反饋設(shè)備的實(shí)際長度I相比較，得到“估值誤差”e，可以寫成e=g–l。

如果估值誤差小于某一個(gè)限定值，那么此過程就此結(jié)束。如果估值誤差不小于這個(gè)限定值，那么，就需要一個(gè)更好的估計(jì)值。這個(gè)過程一直重復(fù)，直到估計(jì)值足夠完美（此時(shí)，這個(gè)估計(jì)值就被作為方程的解！）為了理解如何從數(shù)學(xué)上確定一個(gè)“更好的估計(jì)值”，首先考慮下面這個(gè)簡單的微積分學(xué)例子。假設(shè)我們有一個(gè)通用函數(shù)y=f(x)，f是非線性的。如果我們要計(jì)算由x的變化所導(dǎo)致的y的變化，下面的等式有效：

現(xiàn)在，如果我們考察一小段f(x)的變化區(qū)間，我們可以將這個(gè)區(qū)間近似成線性關(guān)系，有如下等式：

上面的等式在基礎(chǔ)微積分學(xué)通常被稱為斜率局部逼近，可以寫成下面的形式：

如果前面的表達(dá)式再重寫一次，我們可以得到以x變化量為參數(shù)的等式：

現(xiàn)在，讓我們將同樣的思想，應(yīng)用到這個(gè)等式上?？紤]到我們有一個(gè)非線性函數(shù)g=I(k)，用于確定基于估計(jì)方位（k）和估計(jì)腳長度（g）。從設(shè)備反饋，我們也知道腳的實(shí)際長度（I）。我們的估值誤差就是e=g-l,，目標(biāo)是如果估值誤差不符合要求就為方位（k*）從新確定一個(gè)估計(jì)值。然后重復(fù)使用上面的方法：

上面的等式看起來很熟悉，它基本上就是我們在之前的簡單微積分例子中展示的方程。我們可以簡單地再一次重寫這個(gè)方程，以得到方位(k*)的新估值表達(dá)式：

矩陣dI(k)/dk就是我們所說的Jacobian矩陣。在將Jacobian矩陣應(yīng)用于我們的上述等式之前必須先將其翻轉(zhuǎn)，對于六階矩陣就需要6×6求逆矩陣，這是一個(gè)復(fù)雜的數(shù)學(xué)推導(dǎo)。一旦姿態(tài)的新估計(jì)值計(jì)算出來，這個(gè)過程就可以再次重復(fù)，直到誤差(e=g–l)小于某個(gè)可以接受的級別。

如果反向運(yùn)動(dòng)方程已知，如下就是通用前向運(yùn)動(dòng)方程的推導(dǎo)步驟：

1.估計(jì)平臺(tái)的初始姿態(tài)（k）。對于運(yùn)動(dòng)控制器，這通常就是初始受控姿態(tài)。

2.基于估計(jì)值，計(jì)算腳長度，使用反向運(yùn)動(dòng)方程(g=I(k))。

3.通過將此腳長度與從編碼器獲得的當(dāng)前實(shí)際腳長度進(jìn)行比較得到（e=g–l），如果誤差大小小于某個(gè)限制，此算法就將當(dāng)前值收斂于k，并且跳到第6步，如果誤差超過此限制，繼續(xù)第4步。

4.使用反向Jacobian矩陣產(chǎn)生新的估計(jì)值(k*=k-inv(dI(k)/dk)*e)。

5.用第4步的結(jié)果更新估計(jì)值。

6.從第2步重新開始迭代。這個(gè)算法并不僅僅適用于六自由度系統(tǒng)，只要反向運(yùn)動(dòng)方程已知，它就可以用于推導(dǎo)任何系統(tǒng)的前向方程（有收斂解的系統(tǒng)）。

總結(jié)

一個(gè)強(qiáng)壯的控制機(jī)器人系統(tǒng)的方法的關(guān)鍵就是運(yùn)動(dòng)方程，這些方程不僅僅描述了系統(tǒng)的幾何結(jié)構(gòu)，他們也使得具有足夠處理能力和速度的現(xiàn)代運(yùn)動(dòng)控制器，能夠完成必要的計(jì)算，以提供對系統(tǒng)的平滑運(yùn)動(dòng)控制。運(yùn)動(dòng)方程通常在實(shí)時(shí)領(lǐng)域可以直接求解，然而，對于一些復(fù)雜系統(tǒng)，直接求解無法實(shí)現(xiàn)，可以采用一些算法，進(jìn)行求解。高性能的現(xiàn)代控制器提供了特定的結(jié)構(gòu)，可以在控制系統(tǒng)內(nèi)固化這些等式，為很多領(lǐng)域提供了開發(fā)機(jī)器人系統(tǒng)的可能性。成都嵌入式培訓(xùn)哪里好？選擇