1、第第3章章 機器人軌跡規劃機器人軌跡規劃3.1 機器人軌跡規劃概述機器人軌跡規劃概述3.2 插補方式分類與軌跡控制插補方式分類與軌跡控制3.3 機器人軌跡插值計算機器人軌跡插值計算3.4 機器人手部路徑的軌跡規劃機器人手部路徑的軌跡規劃3.1 機器人軌跡規劃概述3.1.1 機器人軌跡的概念 機器人軌跡(trajectory)泛指工業機器人在運動過程中的運動軌跡，即運動點的位移、速度和加速度。 軌跡的生成一般是先給定軌跡上的若干個點，將其經運動學反解映射到關節空間，對關節空間中的相應點建立運動方程，然后按這些運動方程對關節進行插值，從而實現作業空間的運動要求，這一過程通常稱為軌跡規劃。 機器人運

2、動軌跡的描述一般是對其手部位姿的描述，此位姿值可與關節變量相互轉換。控制軌跡也就是按時間控制手部或工具中心走過的空間路徑(path)。3.1.2 軌跡規劃的一般性問題機器人的作業可以描述成工具坐標系T相對于工件坐標系S的一系列運動。作業可以借助工具坐標系的一系列位姿Pi (i=1，2，n)來描述。它把作業路徑描述與具體的機器人、手爪或工具分離開來，形成了模型化的作業描述方法，從而使這種描述既適用于不同的機器人，也適用于在同一機器人上裝夾不同規格的工具。3.1.2 軌跡規劃的一般性問題 在軌跡規劃中，為敘述方便，也常用點來表示機器人的狀態，或用它來表示工具坐標系的位姿，例如起始點、終止點就分別表

3、示工具坐標系的起始位姿及終止位姿。 更詳細地描述運動時不僅要規定機器人的起始點和終止點，而且要給出介于起始點和終止點之間的中間點，也稱路徑點。這時，運動軌跡除了位姿約束外，還存在著各路徑點之間的時間分配問題。例如，在規定路徑的同時，必須給出兩個路徑點之間的運動時間.3.1.2 軌跡規劃的一般性問題 機器人的運動應當平穩，不平穩的運動將加劇機械部件的磨損，并導致機器人的振動和沖擊。為此，要求所選擇的運動軌跡描述函數必須連續，而且它的一階導數(速度)，有時甚至二階導數(加速度)也應該連續。3.1.2 軌跡規劃的一般性問題 軌跡規劃既可以在關節空間中進行，也可以在直角坐標空間中進行。在關節空間中進行

4、軌跡規劃是指將所有關節變量表示為時間的函數，用這些關節函數及其一階、二階導數描述機器人預期的運動；在直角坐標空間中進行軌跡規劃是指將手爪位姿、速度和加速度表示為時間的函數，而相應的關節位置、速度和加速度由手爪信息導出。3.1.3 軌跡的生成方式(1) 示教-再現運動。這種運動由人手把手示教機器人，定時記錄各關節變量，得到沿路徑運動時各關節的位移時間函數q(t)；再現時，按內存中記錄的各點的值產生序列動作。(2) 關節空間運動。這種運動直接在關節空間里進行。由于動力學參數及其極限值直接在關節空間里描述，所以用這種方式求最短時間運動很方便。(3) 空間直線運動。這是一種直角空間里的運動，它便于描述

5、空間操作，計算量小，適宜簡單的作業。(4) 空間曲線運動。這是一種在直角空間中用明確的函數表達的運動，如圓周運動、螺旋運動等。3.1.4 軌跡規劃涉及的主要問題(1) 對工作對象及作業進行描述，用示教方法給出軌跡上的若干個結點(knot)。(2) 用一條軌跡通過或逼近結點，此軌跡可按一定的原則優化，如加速度平滑得到直角空間的位移時間函數X(t)或關節空間的位移時間函數q(t)；在結 點之間如何進行插補，即根據軌跡表達式在每一個采樣周期實時計算軌跡上點的位姿和各關節變量值。(3) 以上生成的軌跡是機器人位置控制的給定值，可以據此并根據機器人的動態參數設計一定的控制規律。(4) 規劃機器人的運動軌

6、跡時，尚需明確其路徑上是否存在障礙約束的組合。3.2 插補方式分類與軌跡控制3.2.1 插補方式分類3.2.2 機器人軌跡控制過程 對于有規律的軌跡，僅示教幾個特征點，計算機就能利用插補算法獲得中間點的坐標，如直線需要示教兩點，圓弧需要示教三點，通過機器人逆向運動學算法由這些點的坐標求出機器人各關節的位置和角度(q1, , qn)，然后由后面的角位置閉環控制系統實現要求的軌跡上的一點。繼續插補并重復上述過程，從而實現要求的軌跡。3.3 機器人軌跡插值計算 給出各個路徑結點后，軌跡規劃的任務包含解變換方程，進行運動學反解和插值計算。在關節空間進行規劃時，需進行的大量工作是對關節變量的插值計算。3

7、.3.1 直線插補 空間直線插補是在已知該直線始末兩點的位置和姿態的條件下，求各軌跡中間點(插補點)的位置和姿態。由于在大多數情況下，機器人沿直線運動時其姿態不變，所以無姿態插補，即保持第一個示教點時的姿態。 如圖所示，已知直線始末兩點的坐標值P0(X0，Y0，Z0)、Pe(Xe，Ye，Ze)及姿態，其中P0、Pe是相對于基坐標系的位置。這些已知的位置和姿態通常是通過示教方式得到的。設v為要求的沿直線運動的速度；ts為插補時間間隔。3.3.1 直線插補3.3.2 圓弧插補一、平面圓弧插補 平面圓弧是指圓弧平面與基坐標系的三大平面之一重合，以XOY平面圓弧為例。已知不在一條直線上的三點P1、P2

8、、P3及這三點對應的機器人手端的姿態，設v為沿圓弧運動速度；ts為插補時時間隔。3.3.2 圓弧插補一、空間圓弧插補 空間圓弧是指三維空間任一平面內的圓弧。 空間圓弧插補可分三步來處理：(1) 把三維問題轉化成二維，找出圓弧所在平面。(2) 利用二維平面插補算法求出插補點坐標(Xi+1, Yi+1)。(3) 把該點的坐標值轉變為基礎坐標系下的值。3.3.3 定時插補與定距插補 機器人實現一個空間軌跡的過程即是實現軌跡離散的過程，如果這些離散點間隔很大，則機器人運動軌跡與要求軌跡可能有較大誤差。只有這些插補得到的離散點彼此距離很近，才有可能使機器人軌跡以足夠的精確度逼近要求的軌跡。模擬CP控制實

9、際上是多次執行插補點的PTP控制，插補點越密集，越能逼近要求的軌跡曲線。 插補點要多么密集才能保證軌跡不失真和運動連續平滑呢？可采用定時插補和定距插補方法來解決。一、定時插補 由軌跡控制過程知道，每插補出一軌跡點的坐標值，就要轉換成相應的關節角度值并加到位置伺服系統以實現這個位置，這個過程每隔一個時間間隔ts完成一次。為保證運動的平穩，顯然ts不能太長。 當然ts越小越好，但它的下限值受到計算量限制，即對于機器人的控制，計算機要在ts時間里完成一次插補運算和一次逆向運動學計算。對于目前的大多數機器人控制器，完成這樣一次計算約需幾毫秒。這樣產生了ts的下限值。當然，應當選擇ts接近或等于它的下限

10、值，這樣可保證較高的軌跡精度和平滑的運動過程。 設機器人需要的運動軌跡為直線，運動速度為v(mm/s)，時間間隔為ts (ms)，則每個ts間隔內機器人應走過的距離為 Pi Pi+1= v ts 可見兩個插補點之間的距離正比于要求的運動速度，兩點之間的軌跡不受控制，只有插補點之間的距離足夠小，才能滿足一定的軌跡精度要求。 采用定時中斷方式每隔ts中斷一次進行一次插補，計算一次逆向運動學，輸出一次給定值。由于ts僅為幾毫秒，機器人沿著要求軌跡的速度一般不會很高，且機器人總的運動精度不如數控機床、加工中心高，故大多數工業機器人采用定時插補方式。 當要求以更高的精度實現運動軌跡時，可采用定距插補。二

11、、定距插補v是要求的運動速度，它是可以變化的，如果要兩插補點的距離PiPi+1恒為一個足夠小的值，以保證軌跡精度，ts就要變化。也就是在此方式下，插補點距離不變，但ts要隨著不同工作速度v的變化而變化。 這兩種插補方式的基本算法相同，只是定時插補固定ts，易于實現，定距插補保證軌跡插補精度，但ts要隨之變化，實現起來比前者困難。3.3.4 關節空間插補在機器人運動過程中，若末端執行器的起始和終止位姿已知，由逆向運動學即可求出對應于兩位姿的各個關節角度。與此相應的各個關節位移、速度、加速度在整個時間間隔內的連續性要求以及其極值必須在各個關節變量的容許范圍之內等。一、三次多項式插值二、過路徑點的三

12、次多項式插值三、高階多項式插值四、用拋物線過渡的線性插值一、三次多項式插值 對應于兩位姿的各個關節角度，末端執行器實現兩位姿的運動軌跡描述可在關節空間中用通過起始點和終止點關節角的一個平滑軌跡函數q(t)來表示。 四個約束條件：即兩端點位置約束和兩端點速度約束。 00fiffiitttt 332210tctctcct二、過路徑點的三次多項式插值 對于這種情況，假如末端執行器在路徑點停留，即各路徑點上速度為0，則軌跡規劃可連續直接使用前面介紹的三次多項式插值方法；但若末端執行器只是經過，并不停留，就需要將前述方法推廣。 對于機器人作業路徑上的所有路徑點可以用求解逆運動學的方法先得到多組對應的關節

13、空間路徑點，進行軌跡規劃時，把每個關節上相鄰的兩個路徑點分別看做起始點和終止點，再確定相應的三次多項式插值函數，把路徑點平滑連接起來。一般情況下，這些起始點和終止點的關節運動速度不再為零。 速度不為0 對于只有一個中間路徑點的機器人作業，其路徑點處的關節加速度連續。可以用兩個三次多項式分兩段插補，約束條件如下： ffiiffiitttt三、高階多項式插值 對某段路徑的起始點和終止點都規定了關節的位置、速度和加速度，則要用一個五次多項式進行插值，即 約束條件相應地有六個。四、用拋物線過渡的線性插值在關節空間軌跡規劃中，對于給定起始點和終止點的情況選擇線性函數插值較為簡單，如上圖所示。然而，單純線

14、性插值會導致起始點和終止點的關節運動速度不連續，且加速度無窮大，顯然，在兩端點會造成剛性沖擊。為此應對線性函數插值方案進行修正，在線性插值兩端點的鄰域內設置一段拋物線形緩沖區段。由于拋物線函數對于時間的二階導數為常數，即相應區段內的加速度恒定，這樣保證起始點和終止點的速度平滑過渡，從而使整個軌跡上的位置和速度連續。如下圖所示。3.4 機器人手部路徑的軌跡規劃 3.4.1 操作對象的描述 任一剛體相對參考系的位姿是用與它固接的坐標系來描述的。剛體上相對于固接坐標系的任一點用相應的位置矢量P P表示；任一方向用方向余弦表示。給出剛體的幾何圖形及固接坐標系后，只要規定固接坐標系的位姿，便可重構該剛體在空間的位姿。 3.4.2 作業的描述 機器人的作業過程可用手部位姿結點序列來規定，每個結點可用工具坐標系相對于作業坐標系的齊次變換來描述。相應的關節變量可用運動學反解程