機械臂即時軌跡產生-以Delta Robot為例

摘要：本文開發一種即時軌跡產生演算法，能讓機械手臂動態狀態下，隨時接收新的目標命令，即時產生具有連續加速度的三階多項式運動軌跡。演算法可用於配備3D，視覺感測器的智慧機械人系統，上層控制器偵測到外部環境變化，而可立刻改變路徑，即時產生新軌跡。軌跡產生適用於任意初始速度與加速度狀態，並且可指定運動的最大速度，加速度與Jerk值，但目標狀態是靜止。運用本產生器於多自由度機械臂的運動控制時，可使各軸軌跡同時抵達目標點。新創的軌跡演算法，軌跡更新可在一個控制周期完成，達到即時反應。本演算法在自製的Delta機械臂，以虛擬模型模擬及實驗驗證其性能。透過model-based design，以電腦模型完成:開發演算法，模擬驗證，產生控制軟體和實驗測試的工作階段。

Abstract：This article presents a real time motion-profile generation algorithm, which can instantly provide a profile based on the current velocity and acceleration with new destination target position. The motion profile has continuous acceleration with constant jerk. Dynamic motion profile generation is important to intelligent robots equipped with multiple vision or depth sensors to react instantaneously to external events under dynamic condition. The proposed algorithm adopts a third-order polynomial function in displacement to provide continuous acceleration under constraints of maximum velocity and maximum acceleration to generate a new profile at arbitrary instant during motion. The algorithm includes a motion-synchronization function to schedule profiles for all joints to arrive the target angles at the same time. Conceptual design and the aftermentioned methods are implemented in a self-made Delta robot arm. We take a model-base approach in the development stages. The algorithm is developed and simulated with 3D robot CAD model. Later, simulation-proven algorithm was combined with EtherCAT communication interface to control this EtherCAT-controlled robot arm.
From the simulation and experiment results, the algorithm can react to suddenly changed target position command, and the total update time for the trajectory is less than one cycle time at the worst case.

關鍵詞：路徑產生、機械臂、即時
Keywords：Trajectory generation, Robot arm, Real time

前言
多數機械臂的運動軌跡規劃假設機械臂自靜止狀態，以指定的速度與加速度開始加速，通過指定的中途點後，再行減速到達目標位置後靜止。運動中途，若因避障或追蹤移動目標，而須臨時更改運動軌跡時，常需暫停現有運動，待機械臂靜止後，另行規畫新軌跡再行移動。 近年配備力感測器，平面或3D視覺的智慧型機械臂日漸普及，機械臂具有足夠的感測與計算能力， 如同行人閃躲突然從路口衝出的機車般，能夠自主決定運動路徑，以達到適應工件變異或避免危及人類的目標。為達到此目標，我們發展一套即時路徑規劃演算法，它能在速度，加速度的限制下，根據機械臂的現在狀態，在偵測外部事件的瞬間，立刻重新規劃運動軌跡，並在下一個控制周期開始執行新軌跡。同時也能顧及各軸反應速度的差異，同步各軸的運動，降低手臂末端(End Effector)的空間循跡誤差，達到平順與高速的運動。隨著智慧型機械臂動態軌跡規劃的需要，Liu [1]提出具運動限制的三次多項式單軸即時軌跡產生器。Macfarlane等[2]提出加速度以正弦函數為基礎的五次多項式位置曲線，具備限制Jerk極限值的多軸連續路徑點，但無法處理臨時變更的命令。 Ahn等[3]提出由任意初始狀態變化至任意目標狀態的單軸五次多項式連續軌跡，但無法限制運動狀態的極限。
Kröger等[4]發展出可在任意初始狀態，產生位置連續的二次多項式軌跡。此方法可以指定目標位置及速度。並具有多軸同動功能，是完整的即時軌跡規劃方案。 Xavi等[5]針對服務型機械人，開發Soft Motion Trajectory Planner。 此方法根據軌跡的初始狀態，產生三次多項式的連續軌跡，並使用四元數限制軌跡的極限值。 Haschk等[6]提高Kröger[4]的二次多項式軌跡為三次多項式軌跡。 Kröger以[4]為基礎，擴展為三次軌跡的即時演算法，並加入即時變更運動限制的功能[7]。 Katzschmann等[8]再擴充Kröger的演算法[7]，軌跡產生器加入機器臂動態模型以及驅動器的輸出限制。
上述文獻以Kröger[7]的功能最完整，並以Reflexxes Motion Libraries名稱授權商業應用。此程式庫依產生軌跡的加度度連續性分為九種[9]:Type I~ XI，三大類。第三類(VI， IX)類採用四次位置多項式曲線; 第二類(III， IV， V)採用三次;I~II則是二次。只有第一類(Type I， II)開放一般使用， Type IV 開放學術研究。本論文提出的演算方法，其架構類似文獻[7]採連續加速度的三次多項式位置函數，但演算流程及方法皆新創設計。
機械臂路產生演算法
智慧型機械臂上層控制系統需要整合不同感測器的信號，決定機械臂的空間運動路徑，並將此決定下達到運動控制系統。運動控制系統根據各軸的狀態，計算執行其運動軌跡Motion Profile。為接收臨時更動的路徑決定，我們發展即時Real Time運動規劃法，可在一個控制周期內，產生新的三階函數的位置軌跡。因為三階時間多項式是滿足加速度曲線連續性，所謂Jerk-Free的最低階函數。
當加速度的微分函數: ，所謂jerk，是常數:時，對應的三階多項式位置，速度與加速度函數分別為如圖 1。

圖 1 三階多項式軌跡