機(jī)器人學(xué)坐標(biāo)轉(zhuǎn)換

1、機(jī)器人技術(shù)數(shù)學(xué)基礎(chǔ)機(jī)器人技術(shù)數(shù)學(xué)基礎(chǔ)Mathematic Preparation for Robotics2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示2.2 2.2 坐標(biāo)變換坐標(biāo)變換2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換2.4 2.4 物體的變換及逆變換物體的變換及逆變換2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示1.位置描述 在直角坐標(biāo)系A(chǔ)中,空間任意一點(diǎn)p的位置(Position)可用3x1列向量(位置矢量)表示:2.方位描述 空間物體B的方位(Orientation)可由某個(gè)固接于此物體的坐標(biāo)系B的三個(gè)單

2、位主矢量xB,yB,zB相對(duì)于參考坐標(biāo)系A(chǔ)的方向余弦組成的3x3矩陣描述. TzyxApppPRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示 上述矩陣稱為旋轉(zhuǎn)矩陣,它是正交的.即 若坐標(biāo)系B可由坐標(biāo)系A(chǔ),通過繞A的某一坐標(biāo)軸獲得,則繞x,y,z三軸的旋轉(zhuǎn)矩陣分別為10000),(00100),(00001),(cssczcsscycsscxRRR1RR1ABTABABR333231232221131211rrrrrrrrrABRRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示 這些旋轉(zhuǎn)變換可以通過右圖推導(dǎo)這是繞Z軸的旋轉(zhuǎn).

3、其它兩軸只要把坐標(biāo)次序調(diào)換可得上頁結(jié)果.pBpApBpBpApBpBpAzzyxyyxxcossinsincospBpBpBpApApAzyxzyx1000cossin0sincosRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示旋轉(zhuǎn)矩陣的幾何意義:1) 可以表示固定于剛體上的坐標(biāo)系B對(duì)參考坐標(biāo)系的姿態(tài)矩陣.2) 可作為坐標(biāo)變換矩陣.它使得坐標(biāo)系B中的點(diǎn)的坐標(biāo) 變換成A中點(diǎn)的坐標(biāo) .3) 可作為算子,將B中的矢量或物體變換到A中.RABRABpBpARABRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.1 2.1 位置和姿態(tài)的表示位置和姿態(tài)的表示3.位姿描述 剛體位姿(

4、即位置和姿態(tài)),用剛體的方位矩陣和方位參考坐標(biāo)的原點(diǎn)位置矢量表示，即 0BAABpRB Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換平移坐標(biāo)變換平移坐標(biāo)變換 坐標(biāo)系A(chǔ)和B具有相同的方位,但原點(diǎn)不重合.則點(diǎn)P在兩個(gè)坐標(biāo)系中的位置 矢 量 滿 足 下 式 :0BABAPPPRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換2.2.旋轉(zhuǎn)變換旋轉(zhuǎn)變換 坐標(biāo)系A(chǔ)和B有相同的原點(diǎn)但方位不同,則點(diǎn)P的在兩個(gè)坐標(biāo)系中的位置矢量有如下關(guān)系:PRPBABATABABBARRR1Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換3.3.復(fù)合變換復(fù)合變換 一般情況

5、原點(diǎn)既不重和,方位也不同.這時(shí)有: (2-13)0BABABAPPRPRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.2 2.2 坐標(biāo)變換坐標(biāo)變換例例2.12.1 已知坐標(biāo)系B的初始位姿與A重合,首先B相對(duì)于A的ZA軸轉(zhuǎn)30,再沿A的XA軸移動(dòng)12單位,并沿A的YA軸移動(dòng)6單位.求位置矢量APB0和旋轉(zhuǎn)矩陣BAR.設(shè)點(diǎn)p在B坐標(biāo)系中的位置為BP=3,7,0,求它在坐標(biāo)系A(chǔ)中的位置.0612;1000866.05 .005 .0866.0)30,(00BAABzRpR0562.13098.1106120562. 7902. 00BABABAppRpRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)

6、變換齊次坐標(biāo)變換1.1.齊次變換齊次變換 (2-13)式可以寫為: (2-14)P點(diǎn)在A和B中的位置矢量分別增廣為:而齊次變換公式和變換矩陣變?yōu)? (2-15,16)11010PPRPBBAABATBBBBTAAAAzyxzyx1,1PP10,0BAABABBABAPRTPTPRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換2.2.平移齊次坐標(biāo)變換平移齊次坐標(biāo)變換 AA分別沿B的X、Y、Z坐標(biāo)軸平移a、b、c距離的平移齊次變換矩陣寫為：用非零常數(shù)乘以變換矩陣的每個(gè)元素，不改變特性。例例2-3:求矢量2i+3j+2k被矢量4i-3j+7k平移得到的新矢量.1000100

7、010001),(cbacbaTrans190612321000710030104001Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換3.3.旋轉(zhuǎn)齊次坐標(biāo)變換旋轉(zhuǎn)齊次坐標(biāo)變換將上式增廣為齊次式：10000),(00100),(00001),(cssczcsscycsscxRRR100001000000),(100000001000),(100000000001),(cssczcsscycsscxRRRRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 引入齊次變換后，連續(xù)的變換可以變成矩陣的連乘形式。計(jì)算簡化。 例2-4 ：U=7i+3j+2

8、k,繞Z軸轉(zhuǎn)90度后，再繞Y軸轉(zhuǎn)90度。例2-5：在上述基礎(chǔ)上再平移（4，-3，7）。1273; ; ;1237; ; ;1000010000010010)90,(zR1372; ; ;1273; ; ;1000010100100100)90,(yR; ; ;1000710030104001)7 , 3, 4(TransRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.3 2.3 齊次坐標(biāo)變換齊次坐標(biāo)變換 由矩陣乘法沒有交換性，可知變換次序?qū)Y(jié)果影響很大。1000701030014100)90,()90,()7 , 3, 4(zRotyRotTransRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體

9、的變換及物體的變換及 逆變換逆變換1.1.物體位置描述物體位置描述 物體物體可以由固定于其自身坐標(biāo)系上的若干特征點(diǎn)描述。物體的變換也可通過這些特征點(diǎn)的變換獲得。Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換1.1.物體位置描述物體位置描述11111144000011111144664411111100220044000011111110000010000141001000001000014100)90,()90,()0,0,4(zRotyRotTransTRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換2.2.齊次坐

10、標(biāo)的復(fù)合變換齊次坐標(biāo)的復(fù)合變換B相對(duì)于A: ABT; C相對(duì)于B: BCT;則C相對(duì)于A:1010100000BACBABBCABCBBCBAABBCABACppRRRpRpTTTTRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換3.3.齊次坐標(biāo)的逆變換齊次坐標(biāo)的逆變換B相對(duì)于A: ABT; A相對(duì)于B: BAT;兩者互為逆矩陣.求逆的辦法:1.直接求ABT-12.簡化方法0ppRpTpRRpRT000100)(1010ABBABABABABBATABTABABBABARobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換及逆變換物體的變換及逆變換3

11、.3.齊次坐標(biāo)的逆變換齊次坐標(biāo)的逆變換一般,若則1000zzzzyyyyxxxxpaonpaonpaonT10001apopnpTzyxzyxzyxaaaooonnnTzyxTzyxTzyxTzyxaaaooonnnpppaonp,Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.4 2.4 物體的變換物體的變換 及逆變換及逆變換3.3.變換方程初步B:基坐標(biāo)系T:工具坐標(biāo)系S:工作臺(tái)坐標(biāo)系G:目標(biāo)坐標(biāo)系 或工件坐標(biāo)系滿足方程TTTTGTSGBSBTRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題: :P43, P43, 題題2.32.3P44, P44, 題題2.92.9Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.

12、5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換1.通用旋轉(zhuǎn)變換公式求:繞從原點(diǎn)出發(fā)的f旋轉(zhuǎn)角時(shí)的旋轉(zhuǎn)矩陣.S:物體上固接的坐標(biāo)系T:參考坐標(biāo)系C:Z軸與f重合的輔助坐標(biāo)系xTYTZTTCSzSf, ZcO),(fRotRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換在S上取一點(diǎn)p,其坐標(biāo)為向量P,它繞T中直線f旋轉(zhuǎn)角。1）將S上p點(diǎn)坐標(biāo)變換到T中，其坐標(biāo)為2）直接計(jì)算繞f旋轉(zhuǎn)的坐標(biāo)為， 目前上式在T無法直接求。采取如下步驟：3）建立輔助坐標(biāo)系C，使其Z軸與f重合。這樣問題 變?yōu)槔@ZC旋轉(zhuǎn)。將S中的點(diǎn)p變換到C中，變換 為：4）在C中繞Z軸旋轉(zhuǎn)有：5）將C中坐標(biāo)變換回T中有，pT

13、ST),(pfRotTST pTSCTTT),(pzRTSCTTT),(pzTRTSCTTCTTRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換步驟2）和5）中的結(jié)果應(yīng)該相同，即：由于C的Z軸與f重合,所以),(),(pfRotpzTRTSTSCTTCTTT1),(),(),(TTTCTCCTTCzTRzTRfRot10000001000010000001000000zyxzyxzyxzzzyyyxxxaaaooonnncsscaonaonaonzzyyxxfafafaRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換根據(jù)坐標(biāo)軸的正交性, ,有

14、令 ,則ona1222zyxzyxyxzyxzxzyxzyzyxaaafnoonafnoonafnoonacos1)(vers1000000),(cversffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversfffRotzzxzyyzxxyzyyzyxyxzzxyxxRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換2.等效轉(zhuǎn)角與轉(zhuǎn)軸給出任一旋轉(zhuǎn)變換,能夠由上式求得進(jìn)行等效旋轉(zhuǎn)角的轉(zhuǎn)軸.已知旋轉(zhuǎn)變換R,R,令令R=R=Rot(f,),即有將上式對(duì)角線元素相加,并簡化得10000001000000cv

15、ersffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversffaonaonaonzzxzyyzxxyzyyzyxyxzzxyxxzzzyyyxxxccversfffaonzyxzyx213)(222) 1(21xyxaoncRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換非對(duì)角元素成對(duì)相減,有平方后有設(shè) ,sfonsfnasfaozxyyzxxyz22222221)()()(xyzxyzonnaaoso18001)()()(tan222zyxxyzxyzaononnaaosonfsnafsaofx

16、yzzxyyzx2/)(2/)(2/)(Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換例2-7 一坐標(biāo)系B與參考系重合,現(xiàn)將其繞通過原點(diǎn)的軸 轉(zhuǎn)30,求轉(zhuǎn)動(dòng)后的B.以 ,代入算式,有Tf0707. 0707. 0ozyxfff0 .300 . 0707. 010000866. 0354. 0354. 00354. 0933. 0067. 00354. 0067. 0933. 0)30,(okRotRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換一般情況,若f不通過原點(diǎn),而過q點(diǎn)(qx,qy,qz),則齊次變換矩陣為:其中,1000),(Cc

17、versffsfversffsfversffBsfversffcversffsfversffAsfversffsfversffcversfffRotzzxzyyzxxyzyyzyxyxzzxyxxzyxzzxzyyzxxyzyyzyxyxzzxyxxzyxqqqcversffsfversffsfversffsfversffcversffsfversffsfversffsfversffcversffqqqCBARobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)2.5 2.5 通用旋轉(zhuǎn)變換通用旋轉(zhuǎn)變換例2-8 一坐標(biāo)系B與參考系重合,現(xiàn)將其繞通過q=1,2,3T的軸 轉(zhuǎn)30,求轉(zhuǎn)動(dòng)后的B.以 ,代入算式,有Tf0

18、707. 0707. 03210 .300 . 0707. 0zyxozyxqqqfff100004. 0866. 0354. 0354. 013. 1354. 0933. 0067. 013. 1354. 0067. 0933. 0)30,(okRotRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算Matlab是美國Mathworks公司推出的數(shù)值計(jì)算軟件.在數(shù)值計(jì)算及科學(xué)研究中,是其它語言無法相比的.其主要特點(diǎn)有:1.1.語言簡潔緊湊語言簡潔緊湊, ,使用方便靈活使用方便靈活, ,庫含數(shù)極其豐富庫含數(shù)極其豐富. .2.2.具有非常多的矩陣函數(shù)具有非常多

19、的矩陣函數(shù), ,矩陣計(jì)算異常方便矩陣計(jì)算異常方便. .3.3.具有多種功能的工具包具有多種功能的工具包. .4.4.具有與具有與FORTRANFORTRAN、C C等同樣多的運(yùn)算符和結(jié)構(gòu)控制指令的同等同樣多的運(yùn)算符和結(jié)構(gòu)控制指令的同 時(shí)，語法限制卻不嚴(yán)格，使程序設(shè)計(jì)很自由時(shí)，語法限制卻不嚴(yán)格，使程序設(shè)計(jì)很自由. .5.5.圖形功能強(qiáng)大圖形功能強(qiáng)大, ,數(shù)據(jù)可視化好數(shù)據(jù)可視化好. .6.6.原程序和庫函數(shù)代碼公開原程序和庫函數(shù)代碼公開. .但但. .程序執(zhí)行效率較低程序執(zhí)行效率較低. .本節(jié)主要介紹其矩陣計(jì)算在機(jī)器人分析中的應(yīng)用本節(jié)主要介紹其矩陣計(jì)算在機(jī)器人分析中的應(yīng)用. .Robotics 數(shù)

20、學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算矩陣的輸入:1)矩陣的直接輸入.(操作) 以 作為首尾,行分隔用”;”,元素分隔用”,”或空格.2)矩陣編輯器.(操作) 先在工作區(qū)定義矩陣,用編輯器修改矩陣.3)用函數(shù)創(chuàng)建矩陣,如.(操作) zeros(m,n):zeros(m,n):零矩陣零矩陣 ones(m,n):ones(m,n):全部元素都為全部元素都為1 1的矩陣的矩陣 eye(m,n):eye(m,n):單位陣單位陣 randn(m,n):randn(m,n):正態(tài)分布的隨機(jī)矩陣正態(tài)分布的隨機(jī)矩陣 vander(A):vander(A):由矩陣由矩陣A A產(chǎn)生的產(chǎn)

21、生的VandermondeVandermonde矩陣矩陣Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算矩陣的計(jì)算矩陣的計(jì)算.(操作)1)1)加減加減2)2)轉(zhuǎn)置轉(zhuǎn)置3)3)乘法乘法4)4)除法與線性方程組除法與線性方程組5)5)逆逆6)6)冪和指數(shù)冪和指數(shù)Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)MatlabMatlab使用與矩陣計(jì)算使用與矩陣計(jì)算例: 計(jì)算:100001000000)60,(100000001000)45,(100000000001)45,(2123232102222222222222222zRotyRotxRotoo)45,()45,()60

22、,()60,()45,()45,(ooooooxRyRzRzRyRxRRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3坐標(biāo)系坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XB B轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)矩陣. .cscsccsssscc0Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3變化變化坐標(biāo)系坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XA A轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋

23、轉(zhuǎn)矩陣. .cscssscccssc0Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.3:2.3變化變化坐標(biāo)系坐標(biāo)系BB初始與初始與AA重合重合, ,讓讓BB繞繞Z ZB B旋轉(zhuǎn)旋轉(zhuǎn)角角; ;然后再然后再繞繞X XA A轉(zhuǎn)轉(zhuǎn)角角. .求把求把B BP P變?yōu)樽優(yōu)锳 AP P的旋轉(zhuǎn)矩陣的旋轉(zhuǎn)矩陣. .cscssscccssccssccssczRotxRot01000000001),(),(Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9:2.9將圖將圖(a)(a)變換到變換到(b).(b).Robotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9 :2.9 解一解一1000001001000001)

24、90,(oxRot1000010000010010)90,(ozRotRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9 :2.9 解一解一10000100100100001)0 ,10, 0(1Trans1000010090100001)0 , 9, 0(2TransRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9 :2.9 解一解一1000001001000001)90,(oxRot1000010000010010)90,(ozRotRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9 :2.9 解一解一1000010000102001)0 , 0 , 2(2Trans10000100100100001)0 ,10, 0(1TransRobotics 數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)習(xí)題習(xí)題:2.9 :2.9 解一解一1000001000010100)90,()90,()0 ,10, 0()0 ,10, 0(ooxRotzRotTransTrans1140110