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

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

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

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

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

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

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

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

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

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