基于三軸運動模組的具有避障和等待位的運動控制方法

賀長林，殷 杰，徐其航

（1.中電鵬程智能裝備有限公司，南京 210008；2.邵東智能制造技術研究院有限公司，湖南邵陽 422213；3.揭陽職業技術學院 機電工程系，廣東揭陽 522051）

0 引言

隨著工廠人力成本的增加，生產自動化成為了一個不可逆的趨勢。在自動化裝配生產線上，取料、避障、放料等是非常常見的應用場景。在實現具有取料、放料，以及避障等動作的運動控制中，如果對成本沒有要求的，一般采用機器人來實現［1－3］。采用機器人的解決方案，方案簡單，操作方便，只需要按照固定的軌跡點進行示教，機器人內部會自動生成相應的運動軌跡，而且機器人具有自動平滑運動軌跡功能，所以機器人在避障點附近可以平滑地經過避障點，通過采用平滑功能，可以大大提升效率；但是由于機器人對安裝的空間要求比較大，所以才有機器人方案，設備的體積會變大，另一方面，由于機器人在箱取料位或者放料位運動時，沒有改變目標位置的功能，機器人只能按照既定的軌跡，固定的速度曲線來運動，所以需要在取料等待位或者放料等待位做減速運動并停止，然后才能去取料或者放料，這樣不利于提升效率。如果對成本比較敏感，一般會采用三軸的機械手來實現，采用三軸機械手，由于在取料位和放料位分別有等待位，在這兩個點有停頓，會有相應的加減速，從而影響效率［4－6］；由于存在避障點，所以三軸機械手不能做到三軸聯動，導致采用三軸機械手的效率更慢［7－9］。采用本文技術方案，可以讓三軸機械手高效地實現取料和放料的動作，能實現像機器人的運動軌跡平滑功能，和機器人的解決方案相比，該技術方案成本更低，對安裝位置要求更低，實現方法更簡單，設計的柔性更高。

1 三軸運動模組機械結構

三軸機械手如圖1所示，圖示的結構中包含了XYZ三個坐標軸，還有一個障礙物，一個取料位，既是軌跡點去取料位把標簽吸附到軌跡點上，一個放料位，既是軌跡點把從取料位吸附的標簽按照設定的軌跡運動到放料位，然后貼到放料位上。

圖1 三軸機械手結構

取料時，需要達到一定的條件，料已經在取料點時，軌跡點才能去取料；同理，放料時，需要達到一定的條件，軌跡點才能允許去放料點，所以需要有一個取料等待位和放料等待位。同時由于有障礙物存在，還需要有一個避障點。由于取料點和放料點有一定的位置精度要求，所以去取料點和放料點的位置時，運動軌跡必須是直線；同時更好地取料和放料，離開取料點和放料點時，運動軌跡必須也要是直線。

本文設計的技術方案，不限于只有兩個等待位或者一個避障點，對于多個等待位和多個避障點都適用。為了方便描述本文的技術方案，采用兩個等待位和一個避障點的情況進行說明。

2 運動控制分析

2.1 三軸模組從取料位到放料位軌跡分析

如圖2和圖3所示，為軌跡點從取料位到放料位的軌跡，由于從取料點出發時，必須要走直線，所以從取料點開始運動后，只有X軸動作，Y軸和Z軸停止，當X軸運動到某一個能避開障礙物的地方，Y軸和Z軸開始動作，這種運動方式比等X軸到位后，Y軸和Z軸再開始運動，效率更高，運動時間更短；由于Y軸和Z軸開始動作是根據X軸運動到某一個固定的點來確定的，所以從取料點到放料等待點的軌跡是固定的，就能保證軌跡點不會碰到障礙物。當X軸到達最遠距離后，Y軸和Z軸再運行一段時間后也會停止，當Y軸和Z軸到達放料等待位后，X軸再開始向放料等待位運動，X軸的運動過程分兩種情況，在X軸往放料等待位運動過程中，如果在X軸開始減速前，放料的條件能夠達到，允許放料后，利用伺服電機的目標位置更改并且立即生效的功能，在經過放料等待位時不需要減速，直接到達放料位置；第二種情況，當X軸開始減速后，放料的條件才達到，這種情況下，X軸在放料等待位減速到0后，且放料條件達到后，X軸再開始運動到放料位置。

圖2 從取料點到放料點運動軌跡（a基于X－Z坐標）

圖3 從取料點到放料點運動軌跡（b基于X－Y坐標）

2.2 去放料位Y－Z軸啟動點分析

為了能準確確定Y軸和Z軸開始運動時，X軸的坐標值，從兩個坐標系內分別計算出這個X軸坐標的最小值，然后從這兩個最小值中選一個比較大的值，即是需要計算的值。首先需要找到避障點，即是運動軌跡只需要避開這個點，就能達到避障的目的，在X－Z坐標系內，找到障礙物中X軸的最大數值，同時也找到Z軸坐標最大的數據，這兩個數值組成的坐標點就是避障點。

如圖所示，障礙物的左上角即是要找的避障點，根據這個可以計算出在X－Z坐標系內的Y軸和Z軸開始動作X軸的坐標X1，X1≥a－v2×（T1＋2×（b1－b））／2v1，其中v2為X軸最高速度，T1為Z軸加速時間，v1為Z軸最高速度，而且假定X軸和Z軸的加速時間比運動時間要短很多；同理也可以計算出基于X－Y軸坐標系內Y軸和Z軸開始動作X軸的坐標X2，X2≥c－v2×（T1＋2×（d1－d））／2v1，其中v2為X軸最高速度，T1為Z軸加速時間，v1為Z軸最高速度。如果X1＞X2，X1就是需要的X軸坐標值；如果X1＜X2，X2就是需要的X坐標值。

2.3 從取料位到放料位速度分析

從取料點到放料點，Y軸和Z軸都只是從取料等待位到放料等待位，只有兩個位置點，而且從放料點開始運動之后的一段時間內，以及軌跡點從放料等待位到放料運動過程中，這兩個節點，Y軸和Z軸都必須是靜止的，所以不論放料條件是否及時成立，Y軸和Z軸的速度曲線都是一樣的，所以不討論Y軸和Z軸的速度曲線，只討論X軸速度曲線。由圖4可以看到，當放料條件沒有及時滿足時，X軸會多一次加減速過程，這時速度曲線和傳統的三軸模組控制方案的速度曲線是一致的，當放料條件及時滿足時，X軸會減少一次加減速過程，這樣就可以提升運動效率。

圖4 X軸從取料點到放料點速度曲線

當放料條件能及時達到，軌跡點能平滑地從取料位運動到放料位，大大提升運動效率，減少運動節拍時間，同時還能減少運動機構加減速次數，減少運動機構由于加減速帶來的磨損；且不會和障礙物干涉。

當放料條件沒有能及時達到，軌跡點會在放料等待位進行等待，不會直接去放料等待位，利用伺服電機的目標位置連續更改且立即生效的功能［10－11］，可以實現兩種情況都能達到使用要求。

2.4 三軸模組從放料位回取料位軌跡分析

如圖5和圖6所示，軌跡點從放料點返回取料點，由于從放料點出發時，必須要走直線，所以從放料點開始運動后，只有X軸動作，Y軸和Z軸停止，當X軸運動到某一個能避開障礙物的地方，Y軸和Z軸開始動作，這種運動方式，比等X軸到位后，Y軸和Z軸再開始運動，這樣的效率更高，運動時間更短；由于Y軸和Z軸開始動作是根據X軸運動到某一個固定的點來確定的，所以從放料點到取料等待點的軌跡是固定的，就能保證軌跡點不會碰到障礙物。當X軸到達最遠距離后，Y軸和Z軸再運動一段時間后也會停止，當Y軸和Z軸到達取料等待位后，X軸再開始向取料等待位運動，X軸的運動過程分兩種情況，當X軸運動到取料等待位過程中，如果在X軸開始減速前，取料的條件能夠達到，允許取料后，利用伺服電機的目標位置更改并且立即生效的功能，在經過取料等待位時不需要減速，直接到達取料位置；第二種情況，當X軸開始減速后，取料的條件才達到，這種情況下，X軸在取料等待位減速到0后，且取料條件達到后，X軸再開始運動到取料位置。

圖5 從放料點到取料點運動軌跡（a基于X－Z坐標）

圖6 從放料點到取料點運動軌跡（b基于X－Y坐標）

回取料位Y－Z軸啟動點分析如下。

由于回取料位的軌跡和去放料位軌跡是一個類似的運動控制，根據2.2節分析原理，可以得到在X－Z坐標系內的Y軸和Z軸開始動作X軸的坐標X1，X1≥a－v2×（T1＋2×（b－b1））／2v1，其中v2為X軸最高速度，T1為Z軸加速時間，v1為Z軸最高速度，而且假定X軸和Z軸的加速時間比運動時間要短很多；同理也可以計算出基于X－Y軸坐標系內Y軸和Z軸開始動作X軸的坐標X2，X2≥c－v2×（T1＋2×（d－d1））／2v1，其中v2為X軸最高速度，T1為Z軸加速時間，v1為Z軸最高速度。X1和X2中比較大的那個值即為Z和Y軸開始啟動X軸的坐標。

2.5 三軸模組從放料位回取料位速度分析

從放料點回到取料點的速度分析，與從取料點到放料點的情況類似，類似的分析并結合圖7可以得到，當取料條件及時滿足時，X軸會減少一次加減速過程，這樣就可以提升運動效率。

圖7 X軸從取料點到放料點速度曲線

當取料條件能及時達到，軌跡點能平滑地從放料位運動到取料位，大大提升運動效率，減少運動節拍時間，同時還能減少運動機構加減速次數，減少運動機構由于加減速帶來的磨損；且不會和障礙物干涉。

當取料條件沒有能及時達到，軌跡點會在取料等待位進行等待，不會直接去取料等待位，利用伺服電機的目標位置連續更改且立即生效的功能，可以實現兩種情況都能達到使用要求。

從取料位到放料位的軌跡和從放料位到取料位的軌跡不一樣，取料條件有沒有及時成立，但是從取料位到放料位的軌跡都是一樣的，都是固定的，只不過，當取料條件沒有及時成立，軌跡點會在接近取料等待位進行減速，并減速到0，等取料條件成立后，再去取料。當取料條件及時成立，軌跡點不會在取料等待位進行減速，而是直接去取料。同理，當放料條件有沒有及時成立，但是從放料位到取料位的軌跡都是一樣的，當放料條件沒有及時成立，軌跡點會在接近放料等待位進行減速，并減速到0，等放料條件成立后，再去放料。當放料條件及時成立，軌跡點不會再放料等待位進行減速，而是直接去放料。

3 應用效果分析

采用本文涉及的技術方案用到電表自動裝銘牌的設備上，相比采用普通的三軸運動控制方法，可以使設備節拍從8 s減少到4.5 s，比傳統的三軸運動模組控制方法效率提升了43%。

4 結束語

本文為具有取料、放料、避障等功能的運動控制場景，提供了一種全新的解決方案。采用三軸運動控制模組的機械結構，結合本文的運動控制方法，比傳統的技術方案成本低，效率高。相比于多軸機械手，本文涉及的技術方案成本更低，更容易安裝。相比于普通的三軸機械臂運動控制方案，本文設計的技術方案能大大提升效率，能大大減少節拍時間；同時還能實現三軸聯動，且運動軌跡是固定不變的，保證軌跡點不會和障礙物干涉；面對取料和放料條件能否及時達到的多種情況時，都能按照固定的軌跡來達到設定的工藝要求；當取料條件或者時放料條件能及時達到，利用伺服電機中的設定位置實時更改，且立即生效的功能，來實現在取料等待點或者放料等待位不進行減速停止，而是高速通過等待位，直接去放料或者取料位，這樣能大大提高效率，而且還能減少伺服電機的加減速次數來減少機械沖擊。