第5章

機器人軌跡規劃主要內容5.1軌跡規劃概述5.2軌跡規劃問題描述5.3關節空間規劃方法5.4笛卡爾空間規劃方法5.5笛卡爾空間規劃方法缺陷5.6路徑的實時生成5.7機械臂軌跡規劃優化指標5.8本章小結由于人們希望機器人在實際的生活應用當中可以解決一些更為精細的任務，為此人們在機器人上加裝了輔助用的機械臂來擴大其應用范圍。而如何為機器人機械臂進行規劃則是當前研究的一大熱點。本章主要圍繞機器人機械臂的軌跡規劃的描述，生成以及實例應用進行說明。5.1軌跡規劃概述機器人具有機械臂，機械臂的軌跡顧名思義就是指其中每個自由度隨著時間變化的位置、速度和加速度變化過程。5.1軌跡規劃概述機器人軌跡規劃實物實驗圖

5.2軌跡規劃問題描述在機械臂的運動過程中，我們也期望機械臂的運動是平穩進行的，為了達到這一目標，我們在選用機械臂運動規劃描述的函數時，為了達到這一目標，在選用機械臂運動規劃描述的函數時，使用連續函數。機械臂軌跡規劃的描述通常有兩類形式，一種是在關節空間中，一種是在坐標系空間中。5.2軌跡規劃問題描述本節主要講述在關節空間下對機器人的軌跡進行描述。由5.2節可以知道，路徑點描述是工具坐標系相對工作臺坐標系位姿。因此用逆運動學理論來求解關節空間機械臂軌跡。利用各個路徑點信息將其轉化為機械臂各關節自由度的矢量角度值，結合整個運動軌跡過程中的路徑點，可以使用一系列光滑函數來對機械臂各個關節自由度在關節空間的變化歷程進行描述。5.3關節空間規劃方法因此，關節空間規劃是利用關節角度函數對機械臂軌跡進行數學意義上的描述。關節空間規劃好處在于將笛卡爾坐標系空間中比較復雜的描述可在關節空間中實現。他的主要優點是計算簡單，不發生機械臂奇異性問題。本節接下來的部分將會介紹幾種常見的關節插值函數。5.3關節空間規劃方法

5.3.1三次多項式插值

5.3.1三次多項式插值

5.3.1三次多項式插值

5.3.1三次多項式插值

5.3.1三次多項式插值

5.3.1三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值因此運用式子（5-8），可以得到在“起始點”和“終止點”速度不為零條件下的三次多項式插值法。確定中間路徑點速度目前常見有三種方法：（1）確定中間路徑點的瞬時速度。（2）采用合適的啟發式方法。（3）控制系統自動計算得到中間路徑點的瞬時速度。5.3.2帶有中間路徑點約束的三次多項式插值第一種方法，在使用相對直角坐標系的瞬時線速度和瞬時角速度需要使用雅可比逆矩陣。對于第二種方法，控制系統選用某種適當的啟發式方法來自動選取恰當的中間路徑點速度，為了詳細說明，我們以下圖5-1進行說明舉例：5.3.2帶有中間路徑點約束的三次多項式插值5.3.2帶有中間路徑點約束的三次多項式插值0

θ

圖5-1

中間路徑點速度啟發式方法對于方法3而言，通過利用約束路徑點處的加速度連續的約束條件，從而選取中間路徑點的速度。因此可使用兩條三次曲線，將中間路徑點的轉折處連接起來，其約束條件為連接處速度和加速度均保持連續。5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值由是，我們可以根據上述方程對系數進行求解：

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.2帶有中間路徑點約束的三次多項式插值

5.3.3高階多項式插值

5.3.3高階多項式插值線性函數可以對軌跡路線插值。但末端執行器在直角坐標空間里的軌跡不一定是直線。可能會出現加速度無窮大的問題。5.3.4含有拋物線擬合的線性函數因此為了解決上述問題，需要生成一段位置和速度都連續的平滑函數來進行插值，可以在起始點和終止點附近使用拋物線進行擬合，在軌跡的中間部分用線性函數來代替。拋物線的二次導數是恒定值，這樣可以保證加速度連續，不會導致在不同路徑點之間的結點發生突變。5.3.4含有拋物線擬合的線性函數形象化的描述如圖5-2所示。5.3.4含有拋物線擬合的線性函數

圖5-2

含有拋物線擬合的線性插值函數

選用的拋物線具有以下特點，起始點和終止點的拋物線二次導數是具有相同標量的常數，而正負性則相反，此外這兩段拋物線所擬合的時間長度是一致的。在這兩個限制的條件下，可以找到許多不同的拋物線進行擬合，但是值得注意的一點是無論選用何種拋物線，拋物線的終止點（起始點）速度要與原本線性函數的在次時刻的速度一致。5.3.4含有拋物線擬合的線性函數

5.3.4含有拋物線擬合的線性函數

5.3.4含有拋物線擬合的線性函數

5.3.4含有拋物線擬合的線性函數

5.3.4含有拋物線擬合的線性函數從等式中可以看出當式子(5-19)可以的等號成立時，此時就是兩段拋物線進行擬合，直線段部分減少至0，且兩擬合段的交接處的速度相等。當加速度逐漸增大時，拋物線部分的時間將會減小，當加速度趨近于無窮大時，此時又回歸于純粹的線性模型進行描述。5.3.4含有拋物線擬合的線性函數

5.3.5含有中間路徑點約束的拋物線擬合線性函數5.3.5含有中間路徑點約束的拋物線擬合線性函數

圖5-3

含有中間路徑點約束得拋物線擬合線性函數示意圖

5.3.5含有中間路徑點約束的拋物線擬合線性函數其中sign(.)函數為符號函數，它的函數定義是當自變量大于0時，函數值為1，自變量小于0時，函數值為-1，自變量為0時，函數值為0。值得一提的是，對于第一段路徑和最后一段路徑的，上面的表達式要相應作出一定的改變，因為路段的起始和結尾部分的擬合區段都要計入到該路徑段的整個時間間隔里。5.3.5含有中間路徑點約束的拋物線擬合線性函數

5.3.5含有中間路徑點約束的拋物線擬合線性函數

5.3.5含有中間路徑點約束的拋物線擬合線性函數

5.3.5含有中間路徑點約束的拋物線擬合線性函數因此根據式子(5-20)至式子(5-24)，我們可以將完整的帶有中間路徑點約束的使用拋物線擬合線性函數的插值描述方法進行給出。但有一點要說明的是，使用這種拋物線擬合的方法進行描述時，那些中間路徑點并沒有實際被經過，當拋物線的加速度取值越大時，實際的路徑也就會更接近于中間路徑點。5.3.5含有中間路徑點約束的拋物線擬合線性函數但若是用戶希望機械臂的末端執行器可以精確經過某個中間點而不作停留的話，則需要作以下處理:系統將會在用戶期望經過的中間路徑點的兩端選取位于兩側的輔助點，然后利用這兩個輔助點作為路徑點，仿照之前5.3.5節中的計算方法進行求解。5.3.5含有中間路徑點約束的拋物線擬合線性函數由圖5-4可以看出，用戶期望精確經過的中間路徑點轉化為兩段拋物線擬合區段的交接點，滿足用戶的需求。為了和之前的中間路徑點進行區別，將這種必須要機械臂末端執行器經過的路徑點叫做經過點。5.3.5含有中間路徑點約束的拋物線擬合線性函數

圖5-4

機械臂末端執行器精確經過中間點的示意圖

輔助用中間點精確經過中間點

5.3.5含有中間路徑點約束的拋物線擬合線性函數在本節中，我們將研究在給定了機械臂末端執行器的笛卡爾空間數學描述方式后，通過逆運動學求解的方法給出各個關節角的變化形式。比較常見的末端執行器的路徑為直線、圓、正弦等形狀。使用笛卡爾空間規劃方式時具有一定的普適性，即使工作臺坐標系發生改變，但是仍然能夠完成軌跡規劃任務，無需重新設定參數。5.4笛卡爾空間規劃方法使用笛卡爾空間規劃方式時具有一定的普適性，即使工作臺坐標系發生改變，但是仍然能夠完成軌跡規劃任務，無需重新設定參數。但是使用笛卡爾空間規劃方法的計算量十分得大，在運行時需要實時地根據末端執行器的路徑規劃函數進行逆運動學求解給出機械臂各個關節角的角度，從而進行更新完成軌跡規劃任務。而在這個過程中通常會涉及到較為復雜的計算，因此計算量會顯得比較大。機器人的笛卡爾空間軌跡規劃控制過程可以如下圖5-5所示：5.4笛卡爾空間規劃方法5.4笛卡爾空間規劃方法機器人軌跡規劃器（插補算法）機器人運動學逆解求取機器人關節角控制操作臂沿著規劃的軌跡運動軌跡上幾個示教點的位姿軌跡中間插補點的位姿各關節角圖5-5

機械臂軌跡空間規劃方法控制過程插補可以分為定時插補和定距插補。定時插補是指每隔固定時間周期插補一個插補點，簡單易用，但是缺點是如果目標點過遠，即定時插補出來的兩個插補點距離較遠，精度降低，但可以通過插入中間點來解決。定距插補是指每隔一段固定的距離就插補一個點進去，如果插補距離足夠近，那么插補得到的曲線精度就越高，但該方法缺陷是恒定距離插補，對于不同速度會導致不同時間，難以實現，并對算法要求較高，因此接下來的研究均采用定時插補進行講解。5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃

5.4.1笛卡爾空間直線軌跡規劃在三維空間中任意不共線三點便可確定一個空間圓，空間圓弧軌跡規劃也使用拋物線過渡線性函數，歸一化因子的求取與上一節一樣。5.4.2空間圓弧的軌跡規劃

5.4.2空間圓弧的軌跡規劃

5.4.2空間圓弧的軌跡規劃圖5-7

構建的—UVW坐標系示意圖

5.4.2空間圓弧的軌跡規劃5.4.2空間圓弧的軌跡規劃

（a）（b）

5.4.2空間圓弧的軌跡規劃

5.4.2空間圓弧的軌跡規劃因此綜上所述，可得到圓弧上各插補點的位置。并且可以根據位移曲線為拋物線過渡的線性函數求解各插補點的三個姿態角，之后把各插補點的姿態和位置通過運動學逆解，從而得到各插補點對應關節角。5.4.2空間圓弧的軌跡規劃

5.4.3空間連續直線的軌跡規劃5.4.3空間連續直線的軌跡規劃圖5-9圓弧過渡的四點連續直線軌跡

5.4.3空間連續直線的軌跡規劃5.4.3空間連續直線的軌跡規劃圖5-10X軸坐標值分量隨插補點次序的軌跡

5.4.3空間連續直線的軌跡規劃接下來將給出求過渡圓弧段插補點的步驟：（1）直線與圓弧的連接點。（2）坐標系變換求插補點。（3）將各插補點從UVW新坐標系值變換回原來的XYZ坐標系值。5.4.3空間連續直線的軌跡規劃

5.4.4空間直線-圓弧的軌跡規劃5.4.4空間直線-圓弧的軌跡規劃圖5-11圓弧過渡的空間直線-圓弧軌跡

5.4.4空間直線-圓弧的軌跡規劃空間直線-圓弧的軌跡規劃算法是前面介紹的三種軌跡規劃算法融合。直線和過渡圓弧段可以參照空間連續直線的軌跡規劃求其插補點，而圓弧段可以參照空間圓弧軌跡規劃來求其插補點，這里就不再進行重復。5.4.4空間直線-圓弧的軌跡規劃由于笛卡爾空間描述的關節位置與路徑形狀之間有連續對應關系，因此本節將介紹笛卡爾空間的路徑規劃常出現的一些缺陷。5.5笛卡爾空間規劃方法缺陷通常情況下，機械臂的起始點和終止點都會選擇在工作空間內，但是在起始點和終止點的軌跡規劃過程中，會出現中間點不在工作空間的情況，為了方便舉例，以下圖進行說明。圖5-12顯示的是一個平面兩桿機器人以及其對應的工作空間。5.5.1中間點超出工作空間5.5.1中間點超出工作空間圖5-12中間點超出工作空間的示例AB該機械臂的特點是兩個連桿中連桿2比連桿1短，因此該機械臂的工作空間為介于兩連桿長度和與兩連桿長度差之間的圓環部分。因此圓的中心會有一部分機械臂執行器無法觸及。5.5.1中間點超出工作空間由之前的學習已知，在機械臂的工作空間中會存在某些位置，在這些位置時關節速度可能會出現不存在或者無窮大的形式。一般稱這些位置叫做奇異點，一般而言奇異點主要分為兩類，第一類是處于工作空間的邊界，這種情況下的關節速度通常是不存在；第二類情況是工作空間的內部，這種情況下的關節速度通常是趨于無窮大。5.5.2奇異點附近的過快關節速度如圖5-13給出的是一個平面兩桿(兩連桿長度相同)的機械臂，該機械臂的軌跡規劃任務是從A點沿著直線路徑以恒定的線速度勻速運動到B點。5.5.2奇異點附近的過快關節速度圖5-13奇異點附近的關節速度示意圖AB從圖5-13中可以看出該路徑上的所有點均可以到達，但是在機械臂的末端執行器越靠近中間部分時，可以看到關節轉的角度也就越大，相應的關節速度也就越快，從而可能帶來一定的安全隱患。一般的解決辦法可以減小在這個路徑上所有的運動速度，從而使得其在中間點附近的關節速度也能滿足容許的范圍內，但是這樣可能無法保證路徑上的某些瞬時特性。5.5.2奇異點附近的過快關節速度由此可見，在使用笛卡爾空間軌跡規劃方法中還會存在一些問題，大多機械臂優先使用關節空間軌跡規劃，只有在特定場合需要，才會選擇使用笛卡爾空間軌跡規劃方法。5.5.2奇異點附近的過快關節速度本節主要是介紹前文所述的關節空間和笛卡爾空間軌跡規劃過程中的實時生成。根據之前5.3節中所介紹的幾種不同擬合軌跡曲線的方法，這些方法的本質上都是根據各個路徑段的一組數據，路徑規劃器使用這些數據來實時計算關節角度、關節速度、關節角加速度信息。5.6路徑的實時生成根據之前5.3節中所介紹的幾種不同擬合軌跡曲線的方法，這些方法的本質上都是根據各個路徑段的一組數據，路徑規劃器使用這些數據來實時計算關節角度、關節速度、關節角加速度信息。因此使用三次多項式進行插值計算時，路徑規劃器只需要在隨著時間的變化不斷重