PMAC應(yīng)用五-運(yùn)動(dòng)學(xué)

• 運(yùn)動(dòng)學(xué)概念
• • 運(yùn)動(dòng)學(xué)
  • 機(jī)器人運(yùn)動(dòng)學(xué)分析基礎(chǔ)
• PMAC運(yùn)動(dòng)學(xué)子程序
• • 子程序類型
  • 案例介紹
  • 編寫運(yùn)動(dòng)學(xué)子程序
  • • 正運(yùn)動(dòng)學(xué)
    • 逆運(yùn)動(dòng)學(xué)
    • 位置報(bào)告PLC
• 結(jié)后語

運(yùn)動(dòng)學(xué)概念

運(yùn)動(dòng)學(xué)

關(guān)于運(yùn)動(dòng)學(xué)克雷格的《機(jī)器人學(xué)導(dǎo)論》是這樣解釋的：運(yùn)動(dòng)學(xué)研究操作臂的運(yùn)動(dòng)特性，而不考慮使操作臂產(chǎn)生運(yùn)動(dòng)時(shí)施加的力，在操作臂運(yùn)動(dòng)學(xué)中，將要研究操作臂的位置、速度、加速度、以及位置變量的所有高階導(dǎo)數(shù)（包括對(duì)時(shí)間或其他變量的導(dǎo)數(shù)）。
簡(jiǎn)單理解是不考慮動(dòng)力的情況下，當(dāng)工具坐標(biāo)和機(jī)器人結(jié)構(gòu)兩個(gè)坐標(biāo)系不一致時(shí)，要實(shí)現(xiàn)一種轉(zhuǎn)換，使得兩個(gè)坐標(biāo)系重合。
官方文檔是這樣介紹運(yùn)動(dòng)學(xué)子程序的：當(dāng)工具坐標(biāo)與執(zhí)行機(jī)構(gòu)(關(guān)節(jié))的匹配位置之間存在非線性數(shù)學(xué)關(guān)系時(shí)，需要一個(gè)運(yùn)動(dòng)子學(xué)程序，子程序是實(shí)現(xiàn)輸入轉(zhuǎn)換電機(jī)位置到軸位置(正運(yùn)動(dòng)學(xué))和反之(逆運(yùn)動(dòng)學(xué))的方程(自己推到的方程)。

機(jī)器人運(yùn)動(dòng)學(xué)分析基礎(chǔ)

對(duì)于機(jī)器人來說，最重要的是能準(zhǔn)確的描述出機(jī)器人各個(gè)關(guān)節(jié)的位置、姿態(tài)，以及各個(gè)關(guān)節(jié)之間的關(guān)系。一個(gè)物體在空間的位姿可以通過在它上面建立一個(gè)坐標(biāo)系，再將該坐標(biāo)系通過一個(gè)4×4 矩陣R 表示。這個(gè)矩陣R 就是齊次變換矩陣。
$$R=\left(\begin{array}{cc}{}_i^{0}R& {}_i^{0}P\\ 0& 1\end{array}\right)=?\begin{array}{cccc}n{}_x& o{}_x& a{}_x& p{}_x\\ n{}_y& o{}_y& a{}_y& p{}_y\\ n{}_z& o{}_z& a{}_z& p{}_z\\ 0& 0& 0& 1\end{array}?$$
齊次矩陣中3×3 矩陣${}_i^{0}R$表示固連在物體i 上的坐標(biāo)系{$O{}_i$}在基座坐標(biāo)系{$O{}_o$} 中的姿態(tài)，3×1 矩陣${}_i^{0}P$表示固連在物體i 上的坐標(biāo)系{$O{}_I$}在基座坐標(biāo)系{$O{}_o$}中的位置。坐標(biāo)系之間的變換可以通過齊次變換矩陣表示，基本的變換矩陣有：
平移齊次變換：
$$Trans(x,y,z)=?\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}?$$
旋轉(zhuǎn)齊次變換：
$$Rot(X,\theta )=?\begin{array}{cccc}1& 0& 0& 0\\ 0& c\theta & ?s\theta & 0\\ 0& s\theta & ?c\theta & 0\\ 0& 0& 0& 1\end{array}?$$
$$Rot(Y,\theta )=?\begin{array}{cccc}c\theta & 0& s\theta & 0\\ 0& 1& 0& 0\\ ?s\theta & 0& c\theta & 0\\ 0& 0& 0& 1\end{array}?$$
$$Rot(Z,\theta )=?\begin{array}{cccc}c\theta & ?s\theta & 0& 0\\ s\theta & c\theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}?$$
式中 Trans 表示平移，Rot 表示旋轉(zhuǎn)，cθ 表示cosθ ， sθ 表示sinθ 。

標(biāo)準(zhǔn)DH參數(shù)齊次變換矩陣：
$${}^{i}A{}_i{+}_1=Rot(Z,\theta {}_i)Trans(0,0,d{}_i)Trans(a{}_i,0,0)Rot(X,\alpha {}_i)=$$
$$?\begin{array}{cccc}c\theta {}_i& ?c\alpha {}_{i}s\theta {}_i& s\alpha {}_{i}s\theta {}_i& a{}_{i}c\theta {}_i\\ s\theta {}_i& c\alpha {}_{i}c\theta {}_i& ?s\alpha {}_{i}c\theta {}_i& a{}_{i}s\theta {}_i\\ 0& s\alpha {}_i& c\alpha {}_i& d{}_i\\ 0& 0& 0& 1\end{array}?$$
求出機(jī)構(gòu)的DH參數(shù)后，把相應(yīng)的參數(shù)出入，然后把所有變化矩陣相乘，就能得到該機(jī)構(gòu)的齊次變換矩陣。關(guān)于運(yùn)動(dòng)學(xué)的求解可以查看相關(guān)資料，這里不做講述。

PMAC運(yùn)動(dòng)學(xué)子程序

子程序類型

正運(yùn)動(dòng)學(xué)
????????????? 輸入：關(guān)節(jié)/電機(jī)位置 ????????????? 輸出：刀尖/軸坐標(biāo)系
逆運(yùn)動(dòng)學(xué)
????????????? 輸入：刀尖/軸坐標(biāo)系 ????????????? 輸出：關(guān)節(jié)/電機(jī)位置
官方圖解：

案例介紹

我以哈爾濱工業(yè)大學(xué)朱銘的《面向鞋業(yè)點(diǎn)膠五自由度機(jī)器人機(jī)構(gòu)設(shè)計(jì)》里已經(jīng)求解出來的運(yùn)動(dòng)學(xué)為例，介紹如何在PMAC運(yùn)動(dòng)學(xué)子程序里實(shí)現(xiàn)。

正解

逆解

由此，我們得到了一個(gè)從關(guān)節(jié)1到關(guān)節(jié)5其次變換矩陣，這是一個(gè)正運(yùn)動(dòng)學(xué)。

編寫運(yùn)動(dòng)學(xué)子程序

正運(yùn)動(dòng)學(xué)

在Kinematic Routines文件夾下新建一個(gè)Forward Kinematic程序。

軸的設(shè)置和相關(guān)變量(以下兩者都是運(yùn)動(dòng)學(xué)所需要的)，可在坐標(biāo)系配置里設(shè)置好。

#x->I //使用電機(jī)x的運(yùn)動(dòng)學(xué)方程 Coord[x].SegMoveTime > 0 //細(xì)分模式，單位ms

輸入：

• 電機(jī)位置在局部變量Lx中表示電機(jī)x

• x受Sys.MaxMotors的限制

• 在IDE中，變量KinPosMotorx自動(dòng)用于Lx

輸出：

• 軸位置在局部變量C0 - C31中表示，這些變量被D0覆蓋，作為變量KinAxisUsed
• e.g.如果使用X, Y, Z和C軸,KinAxisUsed = $40+$80+$100+$4 = $1C4,在IDE中，變量KinPosAxis axis會(huì)自動(dòng)用于Ci。
  
  常用的幾個(gè)終端查詢命令

&xp //在線查詢軸位 &xv //查詢軸速度 &xf //查詢軸跟隨錯(cuò)誤

從DH參數(shù)中可以看到，d1、d2、d3分別為移動(dòng)軸的變量，θ4和θ5為兩個(gè)旋轉(zhuǎn)軸的變量，d1=KinPosMotor1、d2=KinPosMotor2、d3=KinPosMotor3、d4=KinPosMotor4、d5=KinPosMotor5。$p{}_x$=KinPosAxisX、$p{}_y$=KinPosAxisY、$p{}_z$=KinPosAxisZ、θ4=KinPosAxisB、θ5=KinPosAxisC。

Open forward // 為指定坐標(biāo)系打開正向運(yùn)動(dòng)學(xué)緩沖區(qū) if (KinVelEna > 0) callsub 100; // 檢查是否需要雙通道，否則轉(zhuǎn)到第100行。D0作為輸入 KinAxisUsed =KinEnaAxisX+KinEnaAxisY+KinEnaAxisZ+KinEnaAxisB+KinEnaAxisC; // D0作為輸出，用于指定反向運(yùn)動(dòng)學(xué)中使用的軸 n100: // 標(biāo)記為第100行 if (Coord[1].HomeComplete) // 判斷是否回原點(diǎn)？ {KinPosAxisB=KinPosMotor4;KinPosAxisC=KinPosMotor5;KinPosAxisX=a5+sind(KinPosMotor5)-d4-KinPosMotor3;KinPosAxisY=a5*cosd(KinPosMotor4)*cosd(KinPosMotor5);KinPosAxisZ=-a5*sind(KinPosMotor4)*cosd(KinPosMotor5)+KinPosMotor2; } else //無效;停止操作 {if (Ldata.Status & $40) // 從運(yùn)動(dòng)程序啟動(dòng)?{Coord[1].ErrorStatus = 255; // 用戶設(shè)置中止錯(cuò)誤}else // 從軸查詢中調(diào)用{KinPosAxisX = sqrt(-1); //X軸返回非法值KinPosAxisY = sqrt(-1); //Y軸返回非法值KinPosAxisZ = sqrt(-1); //Y軸返回非法值KinPosAxisB = sqrt(-1); //Y軸返回非法值KinPosAxisC = sqrt(-1); //Y軸返回非法值} } return; // 返回調(diào)用程序 close // 關(guān)閉正向運(yùn)動(dòng)學(xué)緩沖區(qū)

到此正運(yùn)動(dòng)學(xué)編寫完畢！

逆運(yùn)動(dòng)學(xué)

在Kinematic Routines文件夾下新建一個(gè)inverse Kinematic程序。

輸入：

• 軸位置在局部變量C0 - C31中
• 變量KinPosAxisi自動(dòng)用于Ci(用戶單位)

輸出：

• 電機(jī)位置在局部變量Lx中，用#x->I語句表示電機(jī)x。
• 變量KinPosMotorx自動(dòng)用于Lx(電機(jī)計(jì)數(shù))
  

open inverse (1) // Put Coordinate System number inside "(cs)" // --------------------User Code Goes Here------------------------ KinPosMotor4=KinPosAxisB; KinPosMotor5=KinPosAxisC; KinPosMotor3=a5*sind(KinPosAxisC)-d4-KinPosAxisX; KinPosMotor2=KinPosAxisY+a5*sind(KinPosAxisB)*cosd(KinPosAxisC); KinPosMotor1=a5*cosd(KinPosAxisB)*cosd(KinPosAxisC)+KinPosAxisZ; close

逆運(yùn)動(dòng)學(xué)編寫完畢！

位置報(bào)告PLC

global ReportActPosX,ReportActPosY,ReportActPosZ,ReportActPosB,ReportActPosC global ReportDesPosX,ReportDesPosY,ReportDesPosZ,ReportDesPosB,ReportDesPosC global ReportActVelX,ReportActVelY,ReportActVelZ,ReportActVelB,ReportActVelCopen plc PositionReportingPLC // --------------------User Code Goes Here------------------------ Ldata.coord = 1 // Select coordinate system 1 PREAD ReportActPosX = D6 // Actual X ReportActPosY = D7 // Actual Y ReportActPosZ = D8 // Actual Z ReportActPosB = D1 // Actual B ReportActPosC = D3 // Actual CDREAD ReportDesPosX = D6 // Desired X ReportDesPosY = D7 // Desired Y ReportDesPosZ = D8 // Desired Z ReportDesPosB = D1 // Desired B ReportActPosC = D2 // Desired Cvread ReportActVelX = D6 // actual velocities X ReportActVelY = D7 // actual velocities Y ReportActVelZ = D8 // actual velocities Z ReportActVelB = D1 // actual velocities B ReportActVelC = D2 // actual velocities Cclose

結(jié)后語

只要算出了正反解，PMAC的正反解子程序編寫并不難，新手一定要多看文檔，文檔中都給出了相信程序的模板，下一節(jié)簡(jiǎn)單講講前瞻。

John J.Craig《機(jī)器人學(xué)導(dǎo)論》
朱銘《面向鞋業(yè)點(diǎn)膠五自由度機(jī)器人機(jī)構(gòu)設(shè)計(jì)》
OMRON《Power PMAC 5-Day Training (Plus Opt ECAT Training)》

總結(jié)

以上是生活随笔為你收集整理的PMAC应用五-运动学的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。