機械臂運動學

1、機械臂運動學基礎1、機械臂的運動學模型機械臂運動學研究的是機械臂運動，而不考慮產生運動的力。運動學研究機械臂的位置，速度和加速度。機械臂的運動學的研究涉及到的幾何和基于時間的內容，特別是各個關節(jié)彼此之間的關系以及隨時間變化規(guī)律。典型的機械臂由一些串行連接的關節(jié)和連桿組成。每個關節(jié)具有一個自由度，平移或旋轉。對于具有n個關節(jié)的機械臂，關節(jié)的編號從1到n，有n +1個連桿，編號從0到n。連桿0是機械臂的基礎，一般是固定的，連桿n上帶有末端執(zhí)行器。關節(jié)i連接連桿i和連桿i-1。一個連桿可以被視為一個剛體，確定與它相鄰的兩個關節(jié)的坐標軸之間的相對位置。一個連桿可以用兩個參數描述，連桿長度和連桿扭轉，這

2、兩個量定義了與它相關的兩個坐標軸在空間的相對位置。而第一連桿和最后一個連桿的參數沒有意義，一般選擇為0。一個關節(jié)用兩個參數描述，一是連桿的偏移，是指從一個連桿到下一個連桿沿的關節(jié)軸線的距離。二是關節(jié)角度，指一個關節(jié)相對于下一個關節(jié)軸的旋轉角度。為了便于描述的每一個關節(jié)的位置，我們在每一個關節(jié)設置一個坐標系， 對于一個關節(jié)鏈，Denavit和Hartenberg提出了一種用矩陣表示各個關節(jié)之間關系的系統(tǒng)方法。對于轉動關節(jié)i，規(guī)定它的轉動平行于坐標軸zi-1，坐標軸xi-1對準從zi-1到zi的法線方向，如果zi-1與zi相交，則xi-1取zi1 ×zi的方向。連桿，關節(jié)參數概括如下：l

3、 連桿長度ai沿著xi軸從zi-1和zi軸之間的距離;l 連桿扭轉i 從zi-1軸到zi軸相對xi-1軸夾角;l 連桿偏移di從坐標系i-1的原點沿著zi-1軸到xi軸的距離;l 關節(jié)角度ixi-1軸和xi軸之間關于zi-1軸的夾角。對于一個轉動關節(jié)i是關節(jié)變量，di是常數。而移動關節(jié)di是可變的，i是恒定的。為了統(tǒng)一，表示為運用Denavit-Hartenberg（DH）方法，可以將相鄰的兩個坐標系之間的變換關系表示為一個4x4的齊次變換矩陣上式表示出了坐標系i相對于坐標系i-1的關系。即其中表示坐標系i相對于世界坐標系0的位置與姿態(tài)，簡稱位姿。2、正向和反向運動學對于一個n-軸剛性連接的機

4、械臂，正向運動學的解給出的是最后一個連桿坐標系的位置和姿態(tài)。重復利用上式,得到機械臂末端位姿在笛卡爾坐標系中有6個自由度，3個平移，3個旋轉。所以，一般來說具有6個自由度的機械臂可以使末端實現任意的位姿?？偟臋C械臂變換一般簡寫為Tn，對6個自由度的機械臂簡寫為T6。對于任意的機械臂，無論其它有多少個關節(jié)，具有什么結構，正向運動學解都是可以得到的。在機械臂的路徑規(guī)劃中，用到的是反向運動學的解，它給出了特定的末端位姿對應的機械臂的關節(jié)角度。一般來說，反向運動學的解不是唯一的，對具有某種結構的機械臂，封閉解可能不存在。對于6自由度的機器人而言，運動學逆解非常復雜，一般沒有封閉解。只有在某些特殊情況下

5、才可能得到封閉解。不過，大多數工業(yè)機器人都滿足封閉解的兩個充分條件之一（Pieper準則） （1）三個相鄰關節(jié)軸交于一點 （2）三個相鄰關節(jié)軸相互平行如果機械臂多于6個關節(jié)，稱關節(jié)為冗余的，這時解是欠定的。如果對于機械臂某個特別的位姿，解不存在，稱這個位姿為奇異位姿。機械臂的奇異性可能是由于機械臂中某些坐標軸的重合，或位置不能達到引起的。機械臂的奇異位姿分為兩類：(1)邊界奇異位姿，當機械臂的關節(jié)全部展開或折起時，使得末端處于操作空間的邊界或邊界附近，雅克比矩陣奇異，機械臂的運動受到物理結構的約束，這時機械臂的奇異位姿稱為邊界奇異位姿。(2)內部奇異位姿，兩個或兩個以上的關節(jié)軸線重合時，機械臂

6、各個關節(jié)的運動相互抵消，不產生操作運動，這時機械臂的奇異位姿稱為內部奇異位姿。機械臂運動學逆解的方法可以分為兩類：封閉解和數值解、在進行逆解時總是力求得到封閉解。因為封閉解的計算速度快，效率高，便于實時控制。而數值解法不具有這些特點。機械臂運動學的封閉逆解可通過兩種途徑得到：代數法和幾何法。一般而言，非零連桿參數越多，到達某一目標的方式也越多，即運動學逆解的數目也越多。在從多重解中選擇解時，應根據具體情況，在避免碰撞的前提下通常按“最短行程”準則來選擇。同時還應當兼顧“多移動小關節(jié)，少移動大關節(jié)”的原則。n個自由度的機械臂的末端位姿由n個關節(jié)變量所決定，這n個關節(jié)變量統(tǒng)稱為n維關節(jié)矢量，記為q

7、。所有的關節(jié)矢量構成的空間稱為關節(jié)空間。機械臂末端的位姿用6個變量描述，3個平移(x,y,z)和3個旋轉(wx, wy, wz)，記x=(x,y,z, wx, wy, wz)，x是機械臂末端在基坐標空間中的坐標，所有的矢量x構成的空間稱為操作空間或作業(yè)定向空間。工作空間是操作臂的末端能夠到達的空間范圍，即末端能夠到達的目標點集合。值得指出的是，工作空間應該嚴格地區(qū)分為兩類： (1) 靈活（工作）空間 指機械臂末端能夠以任意方位到達的目標點集合。因此，在靈活空間的每個點上，手爪的指向可任意規(guī)定。 (2) 可達（工作）空間 指機械臂末端至少在一個方位上能夠到達的目標點集合。機械臂各關節(jié)驅動器的位置

8、組成的矢量稱為驅動矢量s，由這些矢量構成的空間稱為驅動空間。驅動空間關節(jié)空間工作空間正向運動學運動學逆解3、Jacobian矩陣機械臂的Jacobian矩陣表示機械臂的操作空間與關節(jié)空間之間速度的線性映射關系，對于一個n軸的機械臂，機械臂末端在基坐標系中的速度是其中x是6個元素的向量。對于6個關節(jié)機械臂Jacobian矩陣是方陣，如果它是可逆的，則可以由機械臂的末端速度求出各個關節(jié)的速度。Jacobian矩陣在機械臂的奇異位姿上是不可逆的。在實際應用中，當機械臂的末端位置接近奇異位置時，Jacobian矩陣是病態(tài)的，可能導致關節(jié)速度不能正確地得到。上式解決的是正速度問題，即已知q和求末端執(zhí)行器

9、的速度。對于逆速度解問題，由上式可以得到速度逆解公式為，注意到此時需要求雅可比矩陣的逆，由線性方程組理論知上式對任意的，都有解的必要條件是雅可比矩陣的秩rank(J)=6，這意味著機械臂的自由度數n6。這也說明了具有冗余自由度的機械臂，在末端位姿固定的條件下，能使關節(jié)在一個較大的關節(jié)空間的子空間中運動，有效地避開障礙或奇異位姿，并把關節(jié)位移限制在允許范圍內，從而具有更大的運動靈活性。雅可比矩陣可以看成是從關節(jié)空間到操作空間運動速度的傳動比，同時也可用來表示兩空間之間力的傳遞關系。對于冗余自由度機械臂，其雅可比矩陣是長方矩陣，因J滿秩且方程個數少于未知數個數，所以有無窮多個解，這時，一般是求其中

10、的最小范數解，或采用加權最小范數解也就是說使最小的解，其中D是對稱正定加權矩陣。此時的解是使機械臂在能量消耗最小的情況下的解。這時，逆速度問題便轉為：求滿足且使最小。實際上等同于求性能指標L在約束條件下的極值。應用Lagrange乘子法，以上極值為題的解是，當D=I時，雅可比矩陣是，稱為雅可比矩陣的偽逆。下面通過一個兩自由度的平面機械臂說明雅可比矩陣的特性，根據右圖中的幾何關系容易求得兩邊微分后寫成矩陣形式 即 簡寫成 dx=Jd，式中J就稱為機械臂的雅可比（Jacobian）矩陣，它由函數x，y的偏微分組成，反映了關節(jié)微小位移d與機械臂末端微小運動dx之間的關系。 將兩邊

11、同除以dt dt 得到：dx/dt=Jd/dt, 因此機械臂的雅可比矩陣也可以看做是操作空間中的速度與關節(jié)空間中速度的線性變換。dx/dt稱為末端在操作空間中的廣義速度，簡稱操作速度，d/dt為關節(jié)速度?？梢钥闯觯趴杀染仃嚨拿恳涣斜硎酒渌P節(jié)不動而某一關節(jié)以單位速度運動產生的末端速度。由可以看出，J陣的值隨末端位置的不同而不同，即1和2的改變會導致J的變化。對于關節(jié)空間的某些位姿，機械臂的雅可比矩陣的秩減少，這些位姿稱為機械臂的奇異位姿。上例機械臂雅可比矩陣的行列式為：，當2=0°或2=180°時，機械臂的雅可比行列式為0，矩陣的秩為1，這時機械臂處于奇異位姿。機械臂在操

12、作空間的自由度將減少。如果機械臂的雅可比J是滿秩的方陣，相應的關節(jié)速度即可求出，即，上例平面2R機械臂的逆雅可比矩陣，顯然，當2趨于0°（或180°）時，機械臂接近奇異位姿，相應的關節(jié)速度將趨于無窮大。為了補償機器人末端執(zhí)行器位姿與目標物體之間的誤差，以及解決兩個不同坐標系之間的微位移關系問題，需要討論機器人連桿在作微小運動時的位姿變化。假設一變換的元素是某個變量的函數，對該變換的微分就是該變換矩陣各元素對該變量的偏導數所組成的變換矩陣乘以該變量的微分。例如給定變換T為：若它的元素是變量x的函數，則變換T的微分為:下面討論機械臂的微分運動，設機械臂某一連桿相對于基坐標系的位

13、姿為T，經過微運動后該連桿相對基坐標系的位姿變?yōu)門+dT，若這個微運動是相對于基坐標系（靜系）進行的(左乘)，總可以用微小的平移和旋轉來表示，即所以有根據齊次變換的對稱性，若微運動是相對某個連桿坐標系i（動系）進行的(右乘)，則T+dT可以表示為所以有令為微分算子，則相對基系有dT=0T，相對i系有dT=Ti 。這里的下標不同是由于微運動相對不同坐標系進行的。在機械臂運動學中微分變換分為微分平移和微分旋轉兩類。微分平移變換與一般平移變換一樣，其變換矩陣為:由于微分旋轉時0 ，所以sind，cos1將它們代入旋轉變換通式中得微分旋轉表達式:于是得到微分算子，即微分旋轉與有限旋轉相比，有一些特殊的

14、性質，下面分別說明。（1）微分旋轉的無序性，當0 時，有sind，cos1若令x=dx，y=dy，z=dz，則繞三個坐標軸的微分旋轉矩陣分別為 略去2次項，得到兩者結果相同，可見這里左乘與右乘等效。結論：微分旋轉其結果與轉動次序無關，這是與有限轉動（一般旋轉）的一個重要區(qū)別。（2）微分旋轉的可加性，考慮兩個微分旋轉復合后的效果若Rot（x，y，z） 和Rot（x，y，z） 表示兩個不同的微分旋轉，則兩次連續(xù)轉動的結果為：上式表明：任意兩個微分旋轉的結果為繞每個軸轉動的元素的代數和，即微分旋轉是可加的。由等效轉軸和等效轉角與等效，有所以有kxd=x，   kyd=y

15、 ， kzd=z，將它們代入得可見，微分變換由兩個部分組成微分轉動矢量，d微分平移矢量, 合稱為微分運動矢量，可表示為例：已知一個坐標系 ，相對固定系的微分平移矢量d=1 0 0.5，微分旋轉矢量=0 0.1 0 ，求微分變換dA。下面討論兩坐標系之間的微分關系，設第一個坐標系為i系，第二個坐標系為j系不失一般性，假定j系就是固定的0系。因為 ，所以，整理得到對于任何三維矢量 p=px, py, pz,其反對稱矩陣s(p) 定義為：記上式簡寫成 類似地，任意兩坐標系A和B之間廣義速度的坐標變換為：，例：已知一個坐標系 ，相對固定系的微分平移矢量d=1 0 0.5，微分旋轉矢

16、量=0 0.1 0 ，求A系中等價的微分平移矢量dA和微分旋轉矢量A。解：將d=1 0 0.5 和=0 0.1 0 代入得到。4、機械臂軌跡規(guī)劃機械臂的軌跡規(guī)劃可以在關節(jié)空間也可以在笛卡爾空間中進行，或者說機械臂軌跡規(guī)劃是指在關節(jié)空間或者笛卡爾空間中研究機械臂軌跡生成方法。簡言之，機械臂軌跡規(guī)劃是運動學逆解的實際應用，它描述了機械臂在多維空間中的運動路線 。 在知道末端位姿的前提下，通過運動學逆解得到各個關節(jié)在相應時刻的轉動量或者平移量，合理的規(guī)劃指的是規(guī)劃出的 角位移曲線、角速度曲線以及角加速度曲線，可以有效地減少了機械臂在運動過程中的沖擊和振動，使機械臂的工作壽命得以延長。械臂可以分為點到

17、點作業(yè) (Point-to-Point Motion ) 和連續(xù)路徑作業(yè) (Continuous-Path Motion ) 。點到點的運動指的是機械臂在運動過程中，只要求在某些點上有準確的位置和姿態(tài)，相鄰的點不做要求。連續(xù)運動要求機械臂嚴格的沿特定的曲線運動。機械臂的關節(jié)角位移變化率比較小，能夠有效地防止了機械臂工作時的振動和沖擊。機械臂關節(jié)角速度和角加速度變化均平順連續(xù)， 從而有效避免了機械部件的磨損，能夠保證整個機械臂系統(tǒng)的長期、穩(wěn)定的運行，滿足機械臂的工作要求。5、robotics工具箱中的相關函數link 建立一個連桿對象,例如對于本次競賽的機械臂，根據連桿參數得到L1=link(p

18、i/2 0 0 120 0 0);L2=link(pi/2 0 0 0 0 0);L3=link(-pi/2 0 0 140.8 0 pi);L4=link(-pi/2 71.8 0 0 0 pi/2 );L5=link(+pi/2 71.8 0 0 0 pi);L6=link(-pi/2 0 0 0 0 pi/2);L7=link(0 0 0 129.6 0 0);robot 建立一個機械臂對象R= robot(L)noname (7 axis, RRRRRRR)grav = 0.00 0.00 9.81standard D&H parameters alphaAthetaDR/P1

19、.5708 0 120 R(std)1.5708. 00R(std)-1.57080140.8R(std)-1.570871.80R(std)1.570871.80R(std)-1.570800R(std)0 0129.6R(std)drivebot 用滑塊控制的機械臂圖形drivebot(R,ones(1,7)*pi)plot 機械臂的圖形顯示plot(R,pi/2 pi/2 0 0 0 0 0)fkine 串聯(lián)機械臂正向運動學計算tr =fkine (ROBOT, Q)ROBOT表示機械臂對象，Q機械臂關節(jié)坐標值。tr =fkine (R, 0 0 0 pi/2 0 0 0)tr = 0.

20、0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000ikine串聯(lián)機械臂逆向運動學計算q = ikine(ROBOT, T)q = ikine(ROBOT, T, Q)q = ikine(ROBOT, T, Q, M)輸入變量ROBOT表示機械臂對象，T機械臂末端變換矩陣。輸出變量q機械臂關節(jié)的角度(單位是弧度)，一般來說逆運動學的解不是唯一的，取決于初始值Q，缺省時是0向量。如果機械臂的自由度(DOF)小于6，由于解空間的維數大于機械臂的自

21、由度，這時需要第4個輸入量M來確定笛卡爾坐標(手腕對應的坐標系)中的哪些量在求解中被忽略。M中有6個元素，分別表示沿著x,y,z方向的平移和相對于x軸，y軸，z軸的旋轉，值是0(忽略)或1。非零元素的個數應該等于機械臂的自由度。例如，對典型的有5個自由度的機械臂，一般是忽略相對手腕坐標的轉動，這時M = 1 1 1 1 1 0。另外一種用法是qt = ikine(ROBOT, TG)qt = ikine (ROBOT, TG, Q)qt = ikine (ROBOT, TG, Q, M)輸入變量ROBOT表示機械臂對象，TG是4x4xN機械臂末端變換矩陣。輸出變量qt是一組(N個)TG對應的關

22、節(jié)坐標。一行對應一個輸入變換，每一步的初始值取上一步的值。求解使用機械臂Jacobian矩陣的偽逆，這是數值求解方法，對于特定機械臂逆運動學解(如果可能)應該盡量使用解析解。但是這種方法可以得到奇異點上的解，零空間中的關節(jié)角度可以任取。q=ikine(R,tr)q = 0.0000 0.0000 0.0000 0.7854 -0.0000 -0.7854 0.0000注意：對于機械臂末端的一個位置與姿態(tài)，逆運動學計算不是唯一的，驗證tr=fkine(R,q)tr = 0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.

23、0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000transl 計算平移變換tr= transl (X, Y, Z)返回機械臂末端坐標X, Y, Z對應的齊次表換矩陣tr=transl(129.6,0,20.8)tr = 1.0000 0 0 129.6000 0 1.0000 0 0 0 0 1.0000 20.8000 0 0 0 1.0000X Y Z' = transl(T)返回齊次表換表示中的平移值，作為一個3元素的列向量xyz=transl(tr)'xyz = 129.6000 0 20.8000ctraj 計算工作空間中兩點T0,T

24、1之間的軌跡 tc= ctraj(T0, T1, N) tc = ctraj(T0, T1, R)返回從T0到T1笛卡爾坐標系的軌跡 TC N表示軌跡中的點數。在第1中情況下，軌跡中的點在T0到T1中等距離分配。在第2中情況下，向量R給出軌跡中每個點的距離，R中的元素取值為0 1。一個軌跡是4x4xN 矩陣，最后一個下標表示點索引。旋轉插值使用四元球形線性插值。tr0=fkine(R,0 0 0 0 0 0 0)tr0 = 1.0000 -0.0000 -0.0000 -0.0000 0.0000 1.0000 0.0000 0.0000 -0.0000 -0.0000 1.0000 108.

25、8000 0 0 0 1.0000tr1=fkine(R,pi/4 pi/6 0 pi/3 0 0 0)tr1 = 0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000tc(:,:,1) = 1.0000 0 0 -0.0000 0 1.0000 0 0.0000 0 0 1.0000 108.8000 0 0 0 1.0000tc(:,:,2) = 0.8976 -0.3822 0.2198 47.8004 0.3571 0.9226 0.

26、1458 47.8004 -0.2585 -0.0523 0.9646 109.5503 0 0 0 1.0000tc(:,:,3) = 0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000transl(tc)ans = -0.0000 0.0000 108.8000 47.8004 47.8004 109.5503 95.6008 95.6008 110.3005jtraj 計算關節(jié)中兩點Q0,Q1之間的軌跡 Q QD QDD = jtra

27、j(Q0, Q1, N) Q QD QDD = jtraj (Q0, Q1, N, QD0, QD1) Q QD QDD = jtraj (Q0, Q1, T) Q QD QDD = jtraj (Q0, Q1, T, QD0, QD1)軌跡中的點數是N，或者是一個時間向量T。插值使用7次多項式，邊界速度由QD0, QD1指定，缺省時邊界速度和加速度為0。q0=pi pi pi pi pi pi pi;q1=pi pi/2 0 0 0 pi/2 0;tr0=fkine(R,pi pi pi pi pi pi pi);tr1=fkine(R,pi pi/2 0 0 0 pi/2 0);QT,QD

28、,QDD=jtraj(q0,q1,30);figuresubplot(2,2,1),plot(R,QT)subplot(2,2,2),plot(QT),grid on,legend('q1','q2','q3','q4','q5','q6','q7','Location', 'NorthWest')subplot(2,2,3),plot(QD),grid onsubplot(2,2,4),plot(QDD),grid on%注意：其中有一些曲線重合ja

29、cob0 計算機械臂在基坐標系中 Jacobian 矩陣J = jacob0(ROBOT, Q)tr2jac 計算機械臂在基坐標系中 Jacobian 矩陣J = TR2JAC(T)diff2tr 微分表示轉換為齊次變換tr = diff2tr(D)返回表示微分平移與旋轉的齊次變換矩陣,矩陣中包含一個反對稱的旋轉子矩陣。tr2diff 轉換為齊次變換轉換為微分表示D =tr2diff(T)D = tr2diff(T1, T2)第一種形式將齊次表換矩陣表示轉換為6-元素向量微分表示。第二種形式返回6-元素向量，表示從T1 到T2的在基坐標系中需要的微分移動。J = jacob0(R, q1)%

30、Jacobian and differential motion demonstration% A differential motion can be represented by a 6-element vector with elements% dx dy dz drx dry drz% where the first 3 elements are a differential translation, and the last 3 % are a differential rotation. When dealing with infinitisimal rotations, % th

31、e order becomes unimportant. The differential motion could be written % in terms of compounded transforms% transl(dx,dy,dz) * trotx(drx) * troty(dry) * trotz(drz)% but a more direct approach is to use the function diff2tr()D = .1 .2 0 -.2 .1 .1'diff2tr(D)T=fkine(R,q1)% then the differential motion in the second frame would be g