機器人控制系統(tǒng)運動學方程

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

為什么使用運動學？3自由度起重機

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

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

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

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

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

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

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

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

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

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

更復雜的例子——6自由度Stewart六腳平臺

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

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

為了解此系統(tǒng)的反向運動方程，必須確定平臺種類以及執(zhí)行器匯聚點的位置，因為腳長就是點與點之間的距離。平臺執(zhí)行器所處位置用基點坐標系表示如下：

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

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

上面的等式實際上很簡單，但是由于引入了矩陣理論，所以有很多項。下面是最終的反向運動方程（對于腳“i”）。

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

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

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

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

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

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

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

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

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

如果反向運動方程已知，如下就是通用前向運動方程的推導步驟：

1.估計平臺的初始姿態(tài)（k）。對于運動控制器，這通常就是初始受控姿態(tài)。

2.基于估計值，計算腳長度，使用反向運動方程(g=I(k))。

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

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

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

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

總結(jié)

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