基于旋量理論的機器人建模方法介紹

1、基于旋量理論的機器人建模方法介紹,機器人運動學(xué)、動力學(xué)及其控制，本質(zhì)上就是基于對單剛體或者多剛體系統(tǒng)的運動問題研究!,旋量理論,2. 為什么要學(xué)習(xí)旋量理論？ 機器人學(xué)研究的有兩種主要工具：D-H參數(shù)法和旋量理論。相對于D-H參數(shù)法，基于旋量理論的方法有兩大優(yōu)點： 1. 整體描述剛體運動 無需在每個關(guān)節(jié)處都建立坐標(biāo)系，只需建立全局坐標(biāo)系與工具坐標(biāo)系；旋量坐標(biāo)模型蘊含各個剛體的空間絕對幾何信息，從而可直接得到系統(tǒng)整體的模型。 2. 幾何描述直觀 旋量可直觀描述剛體運動的幾何特點，從而簡化分析過程。,1. 什么是旋量理論 (screw theory)？ 旋量運動：剛體系統(tǒng)從一個位姿到另一個位姿的運動

2、都可以用繞某直線的轉(zhuǎn)動和沿該直線的移動復(fù)合表示，通常稱這種復(fù)合運動為旋量運動（screw motion）。 運動旋量（twist）： 旋量運動的無窮小量即為運動旋量。 力旋量（wrench）： 作用在剛體上的任何力系都可以合成一個沿某直線的合力和繞該直線的合力矩。 有關(guān)運動旋量的規(guī)律同樣適用于力旋量！ *互易旋量（reciprocal screw）： 若力旋量F和運動旋量V具有互易關(guān)系，則 F V=0。,基本概念,旋轉(zhuǎn)運動的定義： 如圖，A 表示固定的全局坐標(biāo)系，B 表示與剛體固定的物體坐標(biāo)系，則剛體的姿態(tài)可描述成一個如下形式的旋轉(zhuǎn)矩陣： R的求解方式一： 其中 為物體坐標(biāo)系主軸方向向量。,1

3、. 旋轉(zhuǎn)運動,旋轉(zhuǎn)矩陣 R 具有如下性質(zhì)： SO(3)是包含旋轉(zhuǎn)矩陣 R 的一種特殊正交群，我們稱之為三維旋轉(zhuǎn)群。,基本概念,旋轉(zhuǎn)運動的矩陣指數(shù)表示法,任意的三維空間旋轉(zhuǎn)運動都可以表示為繞某一單位軸 的轉(zhuǎn)動，設(shè)轉(zhuǎn)動角度為 ，則旋轉(zhuǎn)矩陣可描述為矩陣指數(shù)的形式： R的求解方式二： 其中， 是 對應(yīng)的反對稱變換矩陣。 將所有的3X3反對稱矩陣的矢量空間定義為so(3)。 即：,2) so(3)和SO(3)的關(guān)系？,指數(shù)映射關(guān)系！,1）歐拉定理（歐拉角表示法同樣可以描述 R）：,基本概念,剛體運動定義： 如圖，A 表示固定的全局坐標(biāo)系，B 表示與剛體固定的物體坐標(biāo)系，則剛體的位姿矩陣 g 可由剛體的位

4、置矢量 p 和姿態(tài)矩陣 R 共同表示，即： 注意：g 既能表示剛體的位姿狀態(tài)，又能表示剛體位姿由一個坐標(biāo)系到另一個坐標(biāo)系的坐標(biāo)變換關(guān)系。,2. 剛體運動,位姿矩陣 g 具有如下性質(zhì)： SE(3)被稱之為剛體變換群。,基本概念,剛體運動的矩陣指數(shù)表示法,任意剛體運動都可用繞某一軸的轉(zhuǎn)動加上平行于該軸的移動來實現(xiàn)。假設(shè)運動旋量坐標(biāo)為 ，運動量為 ，則剛體運動變換矩陣可表示為： 其中， 是 的運算關(guān)系為 (wedge) ，即： 所有 構(gòu)成的空間定義為se(3)。即：,2) se(3)和SE(3)的關(guān)系？,指數(shù)映射關(guān)系！,1）Chasles 定理: 剛體運動的指數(shù)矩陣表示法,運動學(xué)分析準(zhǔn)備工作運動旋量

5、坐標(biāo),如何確定運動旋量坐標(biāo)是完成基于POE公式的機器人建模的關(guān)鍵一步！,1) 轉(zhuǎn)動關(guān)節(jié)： 其中q為轉(zhuǎn)軸上任意一點的坐標(biāo)，則 轉(zhuǎn)動關(guān)節(jié)對應(yīng)的運動旋量坐標(biāo)： 2) 移動關(guān)節(jié)： 移動關(guān)節(jié)的運動旋量中 w 對應(yīng)分量為0，即移動關(guān)節(jié)旋量坐標(biāo)為：,大多數(shù)機器人都是由一組通過運動副（關(guān)節(jié)）聯(lián)接而成的剛性連桿構(gòu)成。由關(guān)節(jié)空間運動量到末端任務(wù)空間的轉(zhuǎn)化就是機器人的正向運動學(xué)建模過程，即在給定組成運動副的相鄰連桿的相對位置的情況下，確定機器人的末端位姿。,正向運動學(xué),正向運動學(xué)定義,運動鏈描述：指數(shù)積公式(POE),描述機器人的運動就是要描述由關(guān)節(jié)的運動而帶來的剛體(也就是連桿)之間的位置變化。 在如圖所示的二自

6、由度機器人，有四個連桿L0, L1, L2及L3，兩個關(guān)節(jié)1和2，其中0號坐標(biāo)系稱為基坐標(biāo)系，3號坐標(biāo)系稱為工具坐標(biāo)系。兩關(guān)節(jié)轉(zhuǎn)角分別為 和 ，關(guān)節(jié)運動旋量坐標(biāo)為 和 ，則工具坐標(biāo)系相對于基坐標(biāo)系的正向運動學(xué)關(guān)系可表示為：,其中的剛體變換矩陣 可看做關(guān)節(jié)運動對末端位姿的影響尺度。,正向運動學(xué),串聯(lián)開鏈機器人的正向運動學(xué)公式,對于n個轉(zhuǎn)動/移動關(guān)節(jié)的串聯(lián)機器人來說，設(shè) base0 為基坐標(biāo)系，Tool n+1 為工具坐標(biāo)系，則應(yīng)用POE公式計算機器人的正向運動學(xué)模型僅需三步完成： 1. 計算機器人末端初始位姿 ， 2. 計算關(guān)節(jié)對應(yīng)的運動旋量坐標(biāo) ， 3. 代入POE公式：,正向運動學(xué),SCAR

7、A機器人算例：,SCARA機器人共4個DOF，由三個轉(zhuǎn)動關(guān)節(jié)和一個移動關(guān)節(jié)組成，如圖為初始位姿下的機器人狀態(tài)，建立工具坐標(biāo)系 T 和基坐標(biāo)系 S ： 1. 計算SCARA機器人初始位姿 ： 2. 計算關(guān)節(jié)對應(yīng)的運動旋量坐標(biāo) ：,正向運動學(xué),SCARA機器人算例：,2. 計算關(guān)節(jié)對應(yīng)的運動旋量坐標(biāo) ，,3. 代入POE公式得其正向運動學(xué)模型方程：,與正向運動學(xué)模型的輸入、輸出正好相反，逆向運動學(xué)由給定的機器人末端位姿解算對應(yīng)的關(guān)節(jié)運動量。 注意：正向運動學(xué)解唯一，而逆向運動學(xué)可能有多解、唯一解或者無解。,逆向運動學(xué),逆向運動學(xué)定義,逆運動學(xué)解法介紹,給定機器人運動學(xué)正解映射 ，一個期望位姿 ，逆

8、運動學(xué)即是解算如下方程： 運動學(xué)逆解可以分為兩種思路： 1. 解析解一般是位置級解法 1）幾何法直接解算： 對于簡單的平面運動模型，一般直接根據(jù)幾何關(guān)系求解。 2）P-K子問題： 將運動學(xué)逆問題轉(zhuǎn)化成三類P-K子問題，而后通過求解子問題得到逆解。 2. 數(shù)值解一般是速度級解法 即是合理選擇數(shù)值算法對 進(jìn)行求解，例如牛頓迭代法,逆向運動學(xué),1. 解析解P-K子問題,P-K子問題法的基本技巧是：將POE運動學(xué)模型應(yīng)用于某些特殊點，比如兩個或多個軸的交點，這樣可以將消去這些軸關(guān)節(jié)的耦合，從而消掉其對應(yīng)的變化矩陣 ：,Subproblem 1: 繞一個軸的旋轉(zhuǎn) ，求,Subproblem 2: 繞兩個

9、有序軸的旋轉(zhuǎn)，求 和,Subproblem 3: 旋轉(zhuǎn)至給定距離，求,1). 基于POE公式的機器人正向運動學(xué)模型： 2). 定義機器人末端的空間速度： 3). 由上式展開，得到末端空間速度與關(guān)節(jié)速度的線性關(guān)系： 其中，可定義雅克比矩陣：,逆向運動學(xué),2. 數(shù)值解法速度級解法,速度級算法是逆運動學(xué)常用的方法，首先求解運動學(xué)微分模型，即機器人各關(guān)節(jié)和末端執(zhí)行器的速度關(guān)系（雅克比矩陣）。在速度級上求取機器人逆運動學(xué)模型比較快，適合于實時控制，但由于它只得到速度解，所以位置精度難以保證。,速度級模型雅克比矩陣,逆向運動學(xué),速度級模型雅克比矩陣,4). 將式(3)展開，得雅克比矩陣： 其中， Ad 為

10、伴隨變換運算符， 表示經(jīng)剛體運動后運動旋量。 5). 機器人速度級逆解可表示成： 若為冗余機器人，則運動學(xué)解不唯一，可優(yōu)化： 其中，雅克比矩陣的M-P逆矩陣 ，雅克比矩陣的零空間矩陣 。 若為欠約束機器人，則運動學(xué)解不一定存在。 運動學(xué)奇異性是否存在可根據(jù)雅克比矩陣的秩來判斷，當(dāng)雅克比矩陣降秩則機器人產(chǎn)生了運動學(xué)奇異。,逆向運動學(xué),逆運動學(xué)牛頓迭代法,1). 由雅克比矩陣得機器人速度級解： 2). 選擇速度模型的最小二乘逆解，兩邊等式同乘 dt ： 3). 定義機器人末端位姿的微分為： 4). 由(2)得角度微分量，應(yīng)用關(guān)節(jié)角度逆解更新方式： 5). 關(guān)節(jié)角度微分滿足迭代收斂條件時，得到機器人關(guān)節(jié)角度值。,算例,算例 Stanford 機械臂,Stanford 機器人是6-DOF機器人（如圖），由基座的兩個轉(zhuǎn)動關(guān)節(jié)，一個移動關(guān)節(jié)和末端的一個球腕關(guān)節(jié)構(gòu)成。分析機器人的正逆向運動學(xué)解法。 正向運動學(xué) 1. 機器人初始位姿： 2.計算關(guān)節(jié)對應(yīng)的運動旋量坐標(biāo)： 3. 將運動旋量帶入POE公式：,算例,算例 Stanford 機械臂,逆向運動學(xué) 逆向運動學(xué)可通過求解雅克比矩陣得到，而雅克比矩陣的列即是機器人關(guān)節(jié)實時的運動旋量坐標(biāo)值，在此，給出基于雅克比矩陣的運動學(xué)建模方法： 1. 旋量坐標(biāo)變換求解： 1) 第一二、三個關(guān)節(jié)的運動旋量：