PMAC應用五-運動學

• 運動學概念
• • 運動學
  • 機器人運動學分析基礎
• PMAC運動學子程序
• • 子程序類型
  • 案例介紹
  • 編寫運動學子程序
  • • 正運動學
    • 逆運動學
    • 位置報告PLC
• 結后語

運動學概念

運動學

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

機器人運動學分析基礎

對于機器人來說，最重要的是能準確的描述出機器人各個關節的位置、姿態，以及各個關節之間的關系。一個物體在空間的位姿可以通過在它上面建立一個坐標系，再將該坐標系通過一個4×4 矩陣R 表示。這個矩陣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 上的坐標系{$O{}_i$}在基座坐標系{$O{}_o$} 中的姿態，3×1 矩陣${}_i^{0}P$表示固連在物體i 上的坐標系{$O{}_I$}在基座坐標系{$O{}_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}?$$
旋轉齊次變換：
$$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 表示旋轉，cθ 表示cosθ ， sθ 表示sinθ 。

標準DH參數齊次變換矩陣：
$${}^{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}?$$
求出機構的DH參數后，把相應的參數出入，然后把所有變化矩陣相乘，就能得到該機構的齊次變換矩陣。關于運動學的求解可以查看相關資料，這里不做講述。

PMAC運動學子程序

子程序類型

正運動學
????????????? 輸入：關節/電機位置 ????????????? 輸出：刀尖/軸坐標系
逆運動學
????????????? 輸入：刀尖/軸坐標系 ????????????? 輸出：關節/電機位置
官方圖解：

案例介紹

我以哈爾濱工業大學朱銘的《面向鞋業點膠五自由度機器人機構設計》里已經求解出來的運動學為例，介紹如何在PMAC運動學子程序里實現。

正解

逆解

由此，我們得到了一個從關節1到關節5其次變換矩陣，這是一個正運動學。

編寫運動學子程序

正運動學

在Kinematic Routines文件夾下新建一個Forward Kinematic程序。

軸的設置和相關變量(以下兩者都是運動學所需要的)，可在坐標系配置里設置好。

#x->I //使用電機x的運動學方程 Coord[x].SegMoveTime > 0 //細分模式，單位ms

輸入：

• 電機位置在局部變量Lx中表示電機x

• x受Sys.MaxMotors的限制

• 在IDE中，變量KinPosMotorx自動用于Lx

輸出：

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

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

從DH參數中可以看到，d1、d2、d3分別為移動軸的變量，θ4和θ5為兩個旋轉軸的變量，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 // 為指定坐標系打開正向運動學緩沖區 if (KinVelEna > 0) callsub 100; // 檢查是否需要雙通道，否則轉到第100行。D0作為輸入 KinAxisUsed =KinEnaAxisX+KinEnaAxisY+KinEnaAxisZ+KinEnaAxisB+KinEnaAxisC; // D0作為輸出，用于指定反向運動學中使用的軸 n100: // 標記為第100行 if (Coord[1].HomeComplete) // 判斷是否回原點？ {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) // 從運動程序啟動?{Coord[1].ErrorStatus = 255; // 用戶設置中止錯誤}else // 從軸查詢中調用{KinPosAxisX = sqrt(-1); //X軸返回非法值KinPosAxisY = sqrt(-1); //Y軸返回非法值KinPosAxisZ = sqrt(-1); //Y軸返回非法值KinPosAxisB = sqrt(-1); //Y軸返回非法值KinPosAxisC = sqrt(-1); //Y軸返回非法值} } return; // 返回調用程序 close // 關閉正向運動學緩沖區

到此正運動學編寫完畢！

逆運動學

在Kinematic Routines文件夾下新建一個inverse Kinematic程序。

輸入：

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

輸出：

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

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

逆運動學編寫完畢！

位置報告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

結后語

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

John J.Craig《機器人學導論》
朱銘《面向鞋業點膠五自由度機器人機構設計》
OMRON《Power PMAC 5-Day Training (Plus Opt ECAT Training)》

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。