第7章工業機器人運動規劃機器人技術基礎問題導入回顧一下第六章關于控制器結構方框圖的所有圖片，你會發現每一個方框圖的輸入都是θd(s)，其中d代表desired，也就是說上一章一直默認期望軌跡是已知的。然而，如果要求搬運機器人將重物從A點搬運到B點，應該如何在兩點間確定一條合理的軌跡？如果要求焊接機器人焊接一條焊縫，應該如何確保焊槍的末端一直沿著焊縫運動？這就是本章要討論的問題。本章學習目標1.掌握軌跡規劃的概念；2.理解軌跡的生成方式及其與控制的關系；3.理解關節空間和操作空間兩種軌跡規劃方式的優劣，能夠根據實際情況選擇合適的規劃方式；4.掌握多項式和拋物線過渡的線性插補軌跡規劃方式，能夠根據實際情況選擇合適的插補函數進行兩點間的軌跡規劃；本章學習目標5.掌握空間直線插補方式，能夠進行空間兩點間的直線軌跡規劃；6.掌握空間圓弧插補方式，能夠進行過空間三點間的圓弧軌跡規劃；7.了解姿態插補方式；8.熟悉操作空間軌跡規劃的幾何問題，能夠判斷何時會出現問題并了解基本的應對方案；9.掌握利用MATLAB及其RoboticsToolbox進行軌跡規劃的方法。目錄contants7.1機器人運動規劃概述7.2關節空間的軌跡規劃方法7.3操作空間的軌跡規劃第7章工業機器人運動規劃7.1機器人運動規劃概述7.1.1

運動規劃的基本概念7.1.2

軌跡規劃的一般問題7.1.3軌跡的生成方式及控制過程7.1.4關節空間和操作空間的軌跡規劃7.1.1運動規劃的基本概念機器人的運動控制是一個十分復雜的問題，常分為三個階段進行，第一個階段是路徑規劃(pathplanning)，第二個階段是軌跡規劃(trajectoryplanning)，第三個階段是路徑跟蹤(pathtracking)或軌跡跟蹤(trajectorytracking)。我們在上一章探討機器人控制問題時默認已經獲得了期望軌跡，實際研究的就是軌跡跟蹤問題。機器人的運動規劃指的是前兩個階段，即路徑規劃和軌跡規劃，因此機器人的運動規劃也是廣義的機器人控制的一部分。7.1.1運動規劃的基本概念在機器人學中，路徑與軌跡是兩個不同的概念，經常被混淆。機器人的工作環境中往往分布著一些障礙物，為了使機器人順利地從起點運動到終點，需要規劃出一條避開障礙物的安全路徑，這就是路徑規劃。路徑規劃得到一系列離散的路徑點，需要將其擬合成一條光滑連續的路徑，然后對其進行軌跡規劃，如圖（a）所示。（a）路徑規劃（b）軌跡規劃7.1.1運動規劃的基本概念路徑是一個幾何概念，或者說是一個空間概念，它描述的是機器人位姿隨空間的變化。路徑規劃給出的是機器人運動的路線，是任務層級的機器人運動路徑，信息中不涉及運動速度、運動時間等變量，是純幾何層面的運動路徑。路徑規劃有很多方法，包括各種人工智能算法。（a）路徑規劃（b）軌跡規劃7.1.1運動規劃的基本概念軌跡則是在路徑這個空間概念的基礎上引入了時間概念，即在確定空間路徑之后，還要確定在什么時間到達什么位置，如圖（b）所示。所以軌跡是一個時空概念，描述的是機器人位姿隨時間的變化。由于軌跡規劃中機器人位姿是時間的函數，其對時間的一階和二階導數即速度和加速度。（a）路徑規劃（b）軌跡規劃7.1.1運動規劃的基本概念通過第四章的學習可知，機器人各關節的位置、速度和加速度都會影響其動態性能，因此軌跡規劃的好壞影響重大。如果規劃的軌跡不夠光滑，就會產生不平穩的運動，進而導致機器人產生震動和沖擊，使機械零部件的磨損和破壞加劇。軌跡規劃生成的是機器人各關節的運動執行指令，屬于是執行層級的運動路徑。軌跡規劃是本章討論的重點。（a）路徑規劃（b）軌跡規劃第7章工業機器人運動規劃7.1機器人運動規劃概述7.1.1

運動規劃的基本概念7.1.2

軌跡規劃的一般問題7.1.3軌跡的生成方式及控制過程7.1.4關節空間和操作空間的軌跡規劃7.1.2

軌跡規劃的一般問題圖為一個上下料機器人，需要配合數控機床工作。首先將傳送帶上送來的毛坯抓起并放到機床內工作臺的特定位置，接著機械臂退出艙門關閉，等機床完成加工任務艙門重新打開，機械臂再將加工好的零件取出，放在工件臺上。整個過程中，機械臂運行路徑不能與環境中的任何部門發生碰撞，而且其軌跡必須連續光滑確保運動平穩，此外還要與傳送帶送料、艙門開合節奏協調。7.1.2

軌跡規劃的一般問題第6章介紹過機器人的控制有點位控制和連續軌跡控制兩種方式，前者對中間路徑沒有限制，稱為無路徑約束，而后者則稱為有路徑約束。此外還要考慮路徑中是否有障礙物，稱為障礙約束的有無。路徑約束和障礙約束的組合將機器人的軌跡規劃與控制方式劃分為以下四類。7.1.2

軌跡規劃的一般問題圖中上下料機器人環境中顯然有很多障礙，但它并不需要沿著特定軌跡運動，其作業屬于有障礙約束無路徑約束的情況，因此關鍵在于做好起始點和終止點的的位置控制。如果環境中的障礙處于不可預期的運動狀態，則還需要增加在線障礙探測功能從而精準避障。7.1.2

軌跡規劃的一般問題如果機器人的任務是焊接一條焊縫，其作業環境中必然有傳送帶、焊接臺等設備，此時作業為既有障礙約束又有路徑約束，通常需要在離線狀態下完成無碰撞的軌跡規劃，在作業過程中再進行在線軌跡跟蹤。理想環境下機器人工作環境中沒有障礙物，與前一種情況類似同樣要做離線軌跡規劃以及在線軌跡跟蹤，只是可以不考慮避障問題。最簡單的就是既無路徑約束又無障礙約束，這時只要做好位置控制即可。本章主要研究的是無障礙物的兩種情況。第7章工業機器人運動規劃7.1機器人運動規劃概述7.1.1

運動規劃的基本概念7.1.2

軌跡規劃的一般問題7.1.3軌跡的生成方式及控制過程7.1.4關節空間和操作空間的軌跡規劃7.1.3軌跡的生成方式及控制過程7.1.3軌跡的生成方式及控制過程運動軌跡的描述或生成主要有以下幾種方式。①示教-再現運動：這種運動由人手把手示教，機器人定時記錄各關節變量，得到沿路徑運動時各關節的位移時間函數。再現時，按內存中記錄的各點的值產生序列動作。②關節空間運動：這種運動直接在關節空間里進行，由于動力學參數及其極限值直接在關節空間里描述（比如關節的轉角范圍，關節允許的最大轉速、加速度等），所以這種方式求最短時間運動很方便。7.1.3軌跡的生成方式及控制過程③空間直線運動：這是一種操作空間中的運動，它便于描述空間操作，計算量小，適合簡單的作業。④空間曲線運動：這是一種在操作空間中用明確的函數表達的運動，如圓周運動、螺旋運動等。7.1.3軌跡的生成方式及控制過程為描述一個完整的作業，往往需要將上述軌跡生成方式進行組合，通常需要下面幾個步驟。①對工作對象及作業進行描述，并給出軌跡上的若干關鍵節點。這些節點可以通過示教獲得，也可以通過考察機器人與工作對象的相對位置并通過幾何計算獲得。7.1.3軌跡的生成方式及控制過程②用一條軌跡通過或逼近節點，這條軌跡有多種選擇，可以是直線、圓弧、多項式曲線等，該軌跡可按一定的原則優化，比如時間最短原則、加速度平滑原則等。如果是在關節空間做軌跡規劃，此時就可以直接獲得各關節的位移時間函數q(t)。如果是在操作空間做軌跡規劃，此時就可以得到操作空間的位移時間函數X(t)。節點之間的中間點可用插補算法獲得，即根據軌跡表達式在每一個采樣周期實時計算軌跡上各點的位姿X(ti)或各點關節變量值q(ti)。如果求得的是X(ti)則還需要通過機器人逆向運動學算法求出對應逆解q(ti)。7.1.3軌跡的生成方式及控制過程③以上生成的軌跡q(ti)就是機器人位置控制的期望值，可以據此及機器人的動態參數設計機器人控制律。機器人的軌跡控制過程如下圖所示。第7章工業機器人運動規劃7.1機器人運動規劃概述7.1.1

運動規劃的基本概念7.1.2

軌跡規劃的一般問題7.1.3軌跡的生成方式及控制過程7.1.4關節空間和操作空間的軌跡規劃7.1.4關節空間和操作空間的軌跡規劃機器人從起始位姿T1移動到終止位姿T2的過程有兩種實現方式:一種是關節空間的軌跡規劃，這種方式可以確保機械臂起始點和終止點位姿為T1，T2，但插補點的位姿則不確定；另一種是操作空間的軌跡規劃，這種方式可以確保機械臂末端沿著指定軌跡運動，即在所有插補點上的位姿都是確定的。7.1.4關節空間和操作空間的軌跡規劃關節空間的規劃過程如圖所示，首先對初始位姿與終止位姿求逆解，獲得對應的初始關節角和終止關節角，然后在初始關節角與終止關節角之間進行插補，從而獲得足夠密集的插補關節角_時間序列θ(ti)，并基于此控制機器人運動。由于運動學逆解只在初始點和終止點進行過兩次，因此這種方式計算簡單，而且軌跡規劃過程不會出現奇異點問題（在7.3.4中會對此詳細介紹）。7.1.4關節空間和操作空間的軌跡規劃但關節空間規劃的劣勢也很明顯，比如我們期望機械臂末端沿著兩點間的直線運動（右圖），但在關節空間做規劃的結果卻如左圖所示，軌跡類似一條弧線。這還只是一個2關節機械臂，如果關節數更多，其空間軌跡的不確定性會更強。期望軌跡實際軌跡7.1.4關節空間和操作空間的軌跡規劃操作空間的軌跡規劃過程如圖所示，在操作空間對機器人的初始位姿和結束位姿之間進行插補，從而獲得足夠密集的插補位姿_時間序列X(ti)，然后對得到的所有插補位姿求逆解，規劃出對應的插補關節角_時間序列θ(ti)。由于運動學逆解要對所有插補點進行，因此這種方式計算量很大，而且容易碰到奇異點問題。7.1.4關節空間和操作空間的軌跡規劃但由于軌跡規劃在操作空間進行，可以保證機器人末端按照預定軌跡運動，如圖所示，機械臂按預期沿兩點間直線運動。鑒于兩種軌跡規劃方式長短互補，在實際生產中則應根據具體情況選擇適合的軌跡規劃方式。比如，搬運機器人需要將貨物從A點運至B點，對其在空中劃過的路徑并無特殊要求，這時就可以選擇關節空間的軌跡規劃；而焊接機器人作業時，必須沿著焊縫運動，這時就必須選擇操作空間的軌跡規劃。7.1.4關節空間和操作空間的軌跡規劃無論是關節空間軌跡規劃還是操作空間軌跡規劃，最終都會轉化為多個標量的軌跡規劃，因此標量的軌跡規劃方法是機器人軌跡規劃的基礎。這里所說的標量，可以是某個關節的轉角，也可以是機器臂末端的X坐標。標量的軌跡規劃就是指某個標量按照某種插補函數從初始位置變化到最終位置，并且經過給定的中間點，在數學上稱之為插補。比如6軸機械臂在關節空間做軌跡規劃時，實質上就是分別為6個關節變量規劃從初始位置到達最終位置的軌跡，而每個關節變量的軌跡規劃都基于標量的插補方法。下面將分別介紹關節空間和操作空間常用的軌跡規劃方法。感謝大家觀看化學工業出版社第7章工業機器人運動規劃機器人技術基礎目錄contants7.1機器人運動規劃概述7.2關節空間的軌跡規劃方法7.3操作空間的軌跡規劃7.2關節空間的軌跡規劃方法關節空間的軌跡規劃就是關節變量的插補問題。雖然機械臂有多個關節，但各個關節的插補過程是互相獨立的，所以只要理清單一關節變量（即標量）的插補方法即可。由于不平穩的運動會導致機器人關節產生震動和沖擊，加劇機械零部件的磨損和破壞。因此軌跡規劃的原則是加速度為有限值且盡量連續，速度與位置則必須連續。7.2關節空間的軌跡規劃方法第7章工業機器人運動規劃7.2關節空間的軌跡規劃方法7.2.1三次多項式插補7.2.2高階多項式插補7.2.3多項式插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補7.2.5拋物線過渡的線性插補用于多點間軌跡規劃7.2.1三次多項式插補在數學課中我們熟悉的三次多項式標準形式如下：我們可以賦予自變量x和因變量y任何物理意義，但人們習慣于將(x,y)看作平面兩坐標，此時y(x)代表的是一個空間概念，即平面上的一條曲線。做軌跡規劃時，自變量為時間t，對于轉動關節而言因變量是轉角θ，所以θ(t)代表轉角隨時間的變化規律，為了強調這一點，本節將三次多項式插補函數寫為但請不要因此認為7.2節介紹的插補方法僅適用于關節空間軌跡規劃，事實上這些方法可以用于所有標量的插補。7.2.1三次多項式插補7.2.1三次多項式插補7.2.1三次多項式插補插補函數表達式起點

關節角終點

關節角終點

時間從起點到終點過程中，任一時間點對應的轉角7.2.1三次多項式插補例7.1解：①

將已知條件帶入下式所以滿足該運動的三次多項式軌跡插補函數為代入7.2.1三次多項式插補例7.1位置曲線光滑連續遞增速度曲線為拋物線，也是光滑連續的，初始點速度為0，然后逐漸增大，運行到半程（t=1.5s）時速度達到極大值，然后逐漸減小，到終止點的速度降為0加速度相對時間t的變化為一條直線，初始點加速度為最大，然后逐漸減小，運行到半程（t=1.5s）時加速度為0，然后反向加速度逐漸增大，直到終止點的加速度達到負向最大。7.2.1三次多項式插補例7.1拓展練習：讀者可以嘗試自己編寫代碼求解該題。7.2.1三次多項式插補例7.27.2.1三次多項式插補例7.2解：①

關節1的插補函數在例7.1中已經求出。將關節2的已知條件代入代入所以滿足關節2運動的三次多項式軌跡插補函數為7.2.1三次多項式插補例7.2據此可畫出兩關節的位置、速度和加速度隨時間變化的函數曲線。關節2的三次多項式軌跡插補函數為已知關節角求末端位姿，需要用到機器人正向運動學。在例3.5中已經推導出該機械臂末端的坐標為7.2.1三次多項式插補例7.2機械臂末端從起始點（左圖）運動到終止點（右圖）的軌跡拓展練習：讀者可以嘗試自己編寫代碼求解該題7.2.1三次多項式插補例7.27.2.1三次多項式插補思考：在上例中要求起始點和終止點的關節速度均為0，如果要求起始點和終止點的關節速度為某給定值，是否仍舊可以通過三次多項式插補得到符合要求的運動曲線？由圖可以看出，兩關節在起始點和終止點的加速度都比較大，如果要在這兩點指定期望的加速度，三次多項式插補是否還適用？第7章工業機器人運動規劃7.2關節空間的軌跡規劃方法7.2.1三次多項式插補7.2.2高階多項式插補7.2.3多項式插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補7.2.5拋物線過渡的線性插補用于多點間軌跡規劃7.2.2高階多項式插補用三次多項式做插補函數時，由于共有4個待定系數，要獲得唯一解就只能有4個約束。如果對運動軌跡有更為嚴格的要求，約束條件必然增多，三次多項式就不能滿足需求，此時高階多項式就有了用武之地。7.2.2高階多項式插補例如，對某關節運動的起始點和終止點的位置、速度和加速度均有明確要求，則有6個約束條件，這時就可以選用5次多項式做插補函數（有6個系數），即其約束方程組為求導7.2.2高階多項式插補7.2.2高階多項式插補例7.3解：將tf=3代入可求得線性方程組系數矩陣A根據第一組已知條件可以得常數向量b=[15750000]’用MATLAB命令a=A\b即可求得5次多項式的系數數組a。對于第2組和第3組情況，分別有b=[1575503000]’和b=[157530-3000]’，同樣用a=A\b可求得對應5次多項式的系數。7.2.2高階多項式插補例7.37.2.2高階多項式插補例7.37.2.2高階多項式插補例7.3思考：①第二組的速度曲線在t=1.5~1.8區間速度接近0，這意味著什么？②前兩組插補函數在t=0~3秒內都是單調遞增的，而第三組在t=0~2.3區間內為遞增，t=2.3~3區間內為遞減。這意味著什么？這個插補軌跡是否令人滿意？第7章工業機器人運動規劃7.2關節空間的軌跡規劃方法7.2.1三次多項式插補7.2.2高階多項式插補7.2.3多項式插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補7.2.5拋物線過渡的線性插補用于多點間軌跡規劃7.2.3多項式插補用于多點間軌跡規劃根據作業要求，經常需要機械臂末端連續順滑地經過操作空間的某些路徑點，比如下圖中，不但要求機械臂在3秒內從起始點A順滑地運動到終止點D，而且必須在t=1和t=2秒時經過B和C兩個路徑點。我們應如何規劃出符合要求的各條關節軌跡？7.2.3多項式插補用于多點間軌跡規劃7.2.3多項式插補用于多點間軌跡規劃但這種插補方法的問題在于，因為約束中包含起始點和終止點速度為0，機械臂經過B，C等路徑點時必然會停下。對于很多作業，這種不斷啟停的運行方式不夠平穩，效率也不高。如果我們希望機械臂經過路徑點但不停留，就需要將前述方法加以推廣，將三次多項式插補用于多點間的軌跡規劃。7.2.3多項式插補用于多點間軌跡規劃回顧7.2.1中三次多項式插補函數的約束條件，如果希望經過中間路徑點不停留，只要將關節速度約束修改為給定的速度即可，因此四個約束方程變為：解該線性方程組可得7.2.3多項式插補用于多點間軌跡規劃從上式可以看出，

的取值會直接影響多項式系數，對插補函數的走向有重大影響。那么，應該如何合理確定機械臂經過路徑點時各關節的過渡速度？7.2.3多項式插補用于多點間軌跡規劃應該如何合理確定機械臂經過路徑點時各關節的過渡速度？目前主要有以下三種方式。（1）根據機械臂末端在操作空間的瞬時線速度和角速度來確定路徑點的關節速度利用機械臂末端在該路徑點上的速度逆雅克比矩陣，可將其在該點在瞬時線速度和角速度映射為所要求的關節速度。該方法生成的軌跡雖然能滿足用戶設置速度的需求，但是逐點設置速度工作量很大，而且如果某個路徑點是奇異點還無法任意設置速度值。因此這種方法并不常用。7.2.3多項式插補用于多點間軌跡規劃（2）系統采用適當的啟發式方法，自動選擇合理的過渡速度圖為一種簡單而合理的過渡速度選擇方式，圖中短小直線段的系列代表各路徑點的速度。首先用直線把路徑點連接起來，如果路徑點兩側的直線的斜率正負發生變化，則將該點速度設定為零，圖中

的速度均屬于這種情況；如果路徑點兩側的直線的斜率正負沒有改變，則選取兩條直線的平均斜率作為該點的速度，圖中路徑點

的速度均屬于這種情況。7.2.3多項式插補用于多點間軌跡規劃（2）系統采用適當的啟發式方法，自動選擇合理的過渡速度如圖所示，啟發法規劃出的軌跡是連續光滑的，不難看出路徑點的速度正好是軌跡曲線在該點的切線斜率。從速度圖可以發現在路徑點處速度曲線出現尖點，導致加速度在該點出現突變，而且起始點和終止點的加速度也都比較大，這顯然不是我們希望看到的。因此還需要進一步優化算法。7.2.3多項式插補用于多點間軌跡規劃（3）以路徑點處加速度連續為原則選取各點的速度θ0θ1θ2θ3θ4θ5θf7.2.3多項式插補用于多點間軌跡規劃（3）以路徑點處加速度連續為原則選取各點的速度另外起始點和終止點還有位置和速度約束，即有n個路徑點則可以確定4n個約束。4個約束這樣我們就確定了4(n+1)個線性方程，求解該線性方程組就可以獲得4(n+1)個待定系數的唯一解。以上正是構造三次樣條函數的基本思想。由于三次樣條函數使用非常廣泛，各種數學軟件一般都會提供相關函數方便大家調用，比如MATLAB中的spline函數。7.2.3多項式插補用于多點間軌跡規劃例7.47.2.3多項式插補用于多點間軌跡規劃例7.4解：MATLAB函數spline可用于求過多點的三次樣條函數。主要命令如下。表示三次樣條曲線的需要經過的路徑點坐標為(t,q)，且起始點和終止點的速度為0，計算出的各段三次多項式的系數等信息存儲在結構pp中。7.2.3多項式插補用于多點間軌跡規劃例7.4啟發法加速度連續法（樣條函數）對比啟發法的軌跡規劃圖可以發現，以加速度連續為原則的插補方法，其速度曲線更加光滑，加速度曲線連續，也就是說通過各個路徑點時不會出現加速度突變。7.2.3多項式插補用于多點間軌跡規劃例7.4程序NCUT7_4a：加速度連續法

NCUT7_4b：啟發法7.2.3多項式插補用于多點間軌跡規劃不論三次還是五次多項式插補都有一個缺點，即其速度一直在變化中。為了提高效率，我們希望機器人各關節的允許速度盡量穩定在允許范圍之內的高位上，這就是下一種的插補方法閃亮登場的原因。第7章工業機器人運動規劃7.2關節空間的軌跡規劃方法7.2.1三次多項式插補7.2.2高階多項式插補7.2.3多項式插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補7.2.5拋物線過渡的線性插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補采用多項式函數插補的優點是，在區間內速度和加速度永遠是連續的，五次多項式甚至可以指定區間端點處加速度的值，但是區間內的速度是變化的。顯然，這對于機器人的工作效率是一個很大的限制，機器人必須持續地高速運動，才能獲得更高的工作效率。所以可以嘗試線性插補。比如讓關節在3秒內以恒定的速度從運轉到，則其軌跡是一條直線，如圖所示。7.2.4拋物線過渡的線性插補但這里有個明顯的問題。由于起始點要求速度從0瞬間上升到20，理論上加速度無窮大，勢必造成很大的沖擊，在終止點也有類似的問題，這就需要對線性插補函數做一些改進。7.2.4拋物線過渡的線性插補為了生成一條位置和速度都連續光滑的運動軌跡，在使用線性函數進行插補時，需要在啟動和停止時各增加一段拋物線（二次多項式）作為過渡，從而平滑地改變速度。直線函數和兩個拋物線函數組合成一條光滑軌跡在起始點和終止點，速度是漸變的，加速度也穩定在可接受范圍之內7.2.4拋物線過渡的線性插補構造拋物線過渡的直線插補函數的思路與多項式的類似，首先寫出拋物線和直線的標準表達式，然后再根據約束條件確定其中的系數。為構造這樣的插補函數需要增加一個約束條件，即兩段拋物線關于中點（th,θh）中心對稱，這意味兩段拋物線具有相同的持續時間且加速度大小相等符號相反。7.2.4拋物線過渡的線性插補首先寫出首段拋物線方程，即二次多項式的標準表達式求導可得其速度方程由于拋物線在起始點的位置為θ0，速度為0，所以有其速度方程為根據兩段拋物線的對稱性可知，第二段拋物線方程為其速度方程為7.2.4拋物線過渡的線性插補中間部分軌跡為直線，其速度方程及位置方程分別為7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補例7.5解：①根據式有7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補例7.5第二段拋物線方程第一段拋物線方程直線方程代入右式可以得到三組插補函數，每組插補函數分為三段7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補思考：當過渡拋物線加速度變大時，軌跡中的直線段如何變化？過渡時間如何變化？平臺速度如何變化？如果加速度無窮大，軌跡會有什么變化？7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補運行NCUT7_5b看加速度變大時，線性插補軌跡的變化情況動畫7.2.4拋物線過渡的線性插補7.2.4拋物線過渡的線性插補例7.5拓展練習：讀者可以嘗試自己編寫代碼求解該題。第7章工業機器人運動規劃7.2關節空間的軌跡規劃方法7.2.1三次多項式插補7.2.2高階多項式插補7.2.3多項式插補用于多點間軌跡規劃7.2.4拋物線過渡的線性插補7.2.5拋物線過渡的線性插補用于多點間軌跡規劃7.2.5拋物線過渡的線性插補用于多點間軌跡規劃在7.2.3中，我們將多項式插補用于多點間軌跡規劃，本節研究如何用拋物線過渡的線性插補解決多路徑點的軌跡規劃問題。求解多路徑點的拋物線過渡的線性插補曲線的核心思想是，首先用直線連接各路徑點，相鄰兩條直線之間用拋物線過渡。7.2.5拋物線過渡的線性插補用于多點間軌跡規劃這種軌跡規劃的關鍵在于如何確定過渡拋物線函數的各個系數。拋物線函數的標準形式為其中有三個待定系數。如果軌跡需要經過n個路徑點，則需要n個路徑點過渡區，加上起始點和終止點，軌跡中共需要n+2段拋物線，待定系數則有3(n+2)個。7.2.5拋物線過渡的線性插補用于多點間軌跡規劃下面再看看有多少約束。首先，為保證速度連續，每個路徑點都有2個約束，即起點和終點各有兩個約束2n個4個以上共有約束2(n+2)個，少于待定系數3(n+2)個，因此軌跡有無窮多解。與上一節的思路類似，我們必須指定各段拋物線的加速度或者過渡時間，軌跡才會被唯一確定下來。7.2.5拋物線過渡的線性插補用于多點間軌跡規劃例7.6①取過渡時間為0.8秒，請用拋物線過渡的線性函數規劃出符合要求的軌跡，并繪制出軌跡的位置、速度、加速度曲線，請借助軟件完成。②上一問繪制出的軌跡是否通過各路徑點？如果希望軌跡盡量接近路徑點，應該加大還是減小過渡時間？此時對應的加速度是增大還是減小了？%調用mstraj生成過多路徑點的拋物線過渡的直線插補軌跡，插補點存在traj中

traj=mstraj(wp,qdmax,tseg,q0,dt,tacc);7.2.5拋物線過渡的線性插補用于多點間軌跡規劃例7.6解：①RoboticsToolbox提供的mstraj這個函數可以求過多個路徑點的拋物線過渡的線性軌跡，完成本題目的主要程序如下，核心語句只有三句。dq=diff(traj)/dt;%通過對位置差分求速度ddq=diff(dq)/dt;%通過對速度差分求加速度起始點位置經過各點的時間間隔路徑點和終點位置最大速度過渡時間兩插補點之間的時長7.2.5拋物線過渡的線性插補用于多點間軌跡規劃例7.6②由圖可以看出，由于過渡區的存在軌跡并沒有經過路徑點。如果希望軌跡盡量接近路徑點，應該減小過渡時間，此時對應的加速度必然會增大。修改上面程序中的過渡時間為tacc=0.2;再次繪制圖形如圖7-20所示?？梢钥闯龃藭r軌跡非常接近路徑點，但軌跡基本也類似簡單直線了，起始點和終止點的加速度非常大。當然我們也可以逐一設定適合各點的過渡時間，從而達到最佳軌跡規劃效果。過度時間

tacc=0.87.2.5拋物線過渡的線性插補用于多點間軌跡規劃例7.6修改上面程序中的過渡時間為tacc=0.2;再次繪制圖形如圖所示?？梢钥闯龃藭r軌跡非常接近路徑點，但軌跡基本也類似簡單直線了，起始點和終止點的加速度非常大。當然我們也可以逐一設定適合各點的過渡時間，從而達到最佳軌跡規劃效果。過度時間

tacc=0.8過度時間

tacc=0.2感謝大家觀看化學工業出版社第7章工業機器人運動規劃機器人技術基礎目錄contants7.1機器人運動規劃概述7.2關節空間的軌跡規劃方法7.3操作空間的軌跡規劃7.3操作空間的軌跡規劃操作空間的軌跡規劃有很多方法，最常用的是空間直線插補和空間圓弧插補，其他非直線和圓弧軌跡都可以用空間直線和圓弧去逼近，并通過合適的算法將誤差控制在允許范圍之內即可。如果僅從位置插補的角度講，機器人操作空間的插補方式與數控加工的插補方式類似，但機器人除了末端位置，還需要對其末端姿態進行插補。操作空間的軌跡規劃可以保證機器人末端在操作空間的運動軌跡符合期望，但有一些問題需要小心避免。第7章工業機器人運動規劃7.3操作空間的軌跡規劃7.3.1空間直線插補7.3.2空間圓弧插補7.3.3機器人的末端姿態插補7.3.4操作空間軌跡規劃的幾何問題7.3.1空間直線插補在操作空間中，機械臂末端軌跡上的每個節點包含了三個位置變量和三個姿態變量，關于姿態的插補在7.3.3中會詳細介紹，這里先假設機械臂沿直線運動過程中姿態不變，所以只關心三個位置變量的直線插補即可。假設機械臂沿直線做勻速運動，操作空間的直線軌跡插補方法如下。7.3.1空間直線插補操作空間的直線軌跡插補方法如下。①首先確定機械臂末端在操作空間的運動速度v和插補時間ts。插補時間是機器人的伺服控制周期決定的，通常為毫秒級，比如10ms，20ms等。②計算始末點的直線長度③計算插補步數7.3.1空間直線插補④計算各軸的增量⑤計算各插補點坐標其中i=0，1，2，...，N。⑥最后根據機械臂D-H參數求逆解，獲得插補點對應的關節角時間序列θ(t)。7.3.1空間直線插補例7.77.3.1空間直線插補例7.7解：①首先計算直線長度、步長以及插補步數各軸的增量為最后即可求出各插補點的坐標，具體計算比較繁瑣，交給程序完成。其中i=0，1，2，...，142。7.3.1空間直線插補例7.7②RoboticsToolbox提供了球形6軸機械臂求逆解的函數ikine6s，利用下面幾行程序就可以完成題目的要求，繪制的圖形如下。7.3.1空間直線插補例7.7程序NCUT7_7a.m及其繪制的動畫視頻勻速直線插補7.3.1空間直線插補例7.7思考：例7.7假設整個過程機械臂末端一直是勻速運動的，如果要求起點和終點速度均為0，應如何做軌跡規劃？7.3.1空間直線插補例7.7程序NCUT7_7b.m及其繪制的動畫視頻勻加減速直線插補第7章工業機器人運動規劃7.3操作空間的軌跡規劃7.3.1空間直線插補7.3.2空間圓弧插補7.3.3機器人的末端姿態插補7.3.4操作空間軌跡規劃的幾何問題7.3.2空間圓弧插補空間圓弧是指空間任意平面內的圓弧，即空間一般平面內的圓弧?？臻g圓弧的插補可分如下三步進行。（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題（2）利用二維平面圓弧插補算法求出插補點坐標（xi，yi，zi）（3）將二維平面坐標系中的插補點坐標（xi，yi，zi），通過齊次坐標變換矩陣T再轉化為參考坐標系下的插補點坐標（Xi，Yi，Zi）。7.3.2空間圓弧插補在圓弧所在平面上建立坐標系{R}，其Z軸垂直于圓弧平面，原點位于圓心。只要求出{R}對應的齊次變換矩陣T，則通過坐標變換，可將參考坐標系內已知的三點坐標轉換為{R}坐標系的坐標，這時空間一般平面上的圓弧就變為XRORYR平面上的圓弧，從而將三維問題轉化為二維問題。因此關鍵在于如何建立圓弧平面坐標系{R}，即如何確定平面的法向量作為坐標系的ZR軸，以及如何確定圓心坐標，從而將坐標系{R}原點置于圓心。（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系7.3.2空間圓弧插補已知到P1,P2,P3三點決定了圓弧所在平面，根據叉積的定義可知，向量P1P2和P2P3的叉積即為該平面的法向量方向（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系由圓的性質可知，線段P1P2和P2P3的中垂線交點即為圓心。所以首先需要求出兩中垂線方程，再求其交點。7.3.2空間圓弧插補由于P1P2中垂線L1同時垂直于平面法向量n和向量P1P2，所以其方向向量可以通過叉積獲得（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系中點A的坐標為所以，過中點A且法向量為的中垂線所在的直線方程為7.3.2空間圓弧插補同理，P2P3的中垂線所在的直線方程為（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系其中現在已經求出兩條中垂線的方程，其交點即為圓心。7.3.2空間圓弧插補（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系7.3.2空間圓弧插補（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題

參考坐標系{0}與空間圓弧平面坐標系{R}的關系7.3.2空間圓弧插補（1）建立圓弧所在平面坐標系，將三維插補問題轉換為二維插補問題7.3.2空間圓弧插補（2）利用二維平面圓弧插補算法求出插補點坐標7.3.2空間圓弧插補（2）利用二維平面圓弧插補算法求出插補點坐標7.3.2空間圓弧插補（3）將{R}坐標系中的插補點坐標（xi，yi，zi），通過齊次坐標變換矩陣T再轉化為參考坐標系下的插補點坐標（Xi，Yi，Zi）。通過以上三步，就完成了過空間三點的圓弧的插補。7.3.2空間圓弧插補例7.87.3.2空間圓弧插補例7.87.3.2空間圓弧插補例7.87.3.2空間圓弧插補例7.87.3.2空間圓弧插補例7.8（2）利用二維平面圓弧插補算法求出插補點坐標7.3.2空間圓弧插補例7.8通過以上三步，就完成了過空間三點的圓弧的插補，所繪制的插補點如右圖所示。請參考程序NCUT7_8.m及其講解視頻第7章工業機器人運動規劃7.3操作空間的軌跡規劃7.3.1空間直線插補7.3.2空間圓弧插補7.3.3機器人的末端姿態插補7.3.4操作空間軌跡規劃的幾何問題7.3.3機器人的末端姿態插補7.3.3機器人的末端姿態插補（2）歐拉角法還可以采用一組帶有順序的旋轉角對姿態進行描述，即歐拉角方式。歐拉角按照繞不同軸的旋轉順序可以分為多種，比如依次繞X,Y,Z或Y,X,Z或Z,Y,X軸旋轉等等。7.3.3機器人的末端姿態插補（2）歐拉角法7.3.3機器人的末端姿態插補（2）歐拉角法歐拉角能夠較好地解決末端姿態的規劃問題，但由于歐拉角存在萬向死鎖問題，而且歐拉角的旋轉順序對于最后的姿態影響很大，無法同時對三個歐拉角進行規劃，因此并沒有得到廣泛應用。陀螺儀正常狀態萬向死鎖狀態（少了一個自由度）7.3.3機器人的末端姿態插補（3）等效軸角坐標系表示法采用歐拉角法時默認從一個姿態到另一個姿態的變換，需要繞三個軸旋轉三個角度。但數學上可以證明，任何兩個姿態的變換，都可以通過繞空間某個軸旋轉某個角度來獲得。這個轉換過程在數學上采用了四元數法，對四元數有興趣可以查閱相關資料做進一步了解。等效軸角插補法就是建立在這個理論基礎之上的。7.3.3機器人的末端姿態插補（3）等效軸角坐標系表示法7.3.3機器人的末端姿態插補（3）等效軸角坐標系表示法7.3.3機器人的末端姿態插補例7.9在例7.8圓弧位置軌跡規劃的基礎上，添加機械臂的姿態插補。要求機械臂經過三點時，其末端Z軸指向圓弧中心，其Y軸沿著該點的切線方向，如圖所示；請借助RoboticsToolbox求插補點逆解，求出插補點對應的6個關節角_時間序列，并繪制出Puma560沿著該圓弧運動的動畫。例7.97.3.3機器人的末端姿態插補解：主要分以下三步來求解該問題。①確定關鍵點的位姿矩陣例7.97.3.3機器人的末端姿態插補②等效軸角法進行姿態插補首先將上一步求得的關鍵點位姿矩陣轉換為四元數表達法，接著在四元數空間進行插補，最后再將插補好的插補點四元數轉換回齊次矩陣。需要利用RoboticsToolbox工具箱的函數完成上述步驟，主要是語句如下：例7.9q1=UnitQuaternion(T1);%表達位姿的齊次矩陣T1轉換為四元數q1q2=UnitQuaternion(T2);%表達位姿的齊次矩陣T2轉換為四元數q2q3=UnitQuaternion(T3);%表達位姿的齊次矩陣T3轉換為四元數q3qq1=erp(q2,N1);%利用四元數法進行插補，從q1繞等效軸旋轉到q2，共N1個姿態qq2=erp(q3,N2);%利用四元數法進行插補，從q2到繞等效軸旋轉到q3共N2個姿態qq=[qq1qq2];%將兩段插補點連在一起Tqq=qq.SE3;%將四元數qq轉化為代表旋轉變換的齊次矩陣Tqq%將末端位置坐標及姿態矩陣合成為完整的末端位姿齊次矩陣Tc=transl([x(k),y(k),z(k)])*double(Tqq(k));7.3.3機器人的末端姿態插補③求逆解對插補好的末端位姿求逆解，得到插補點對應的6個關節角_時間序列，然后就可以將動畫繪制出來。主要是語句如下：例7.9%求puma560機器人末端位姿Tc對應的逆解qcqc(k,:)=p560.ikine6s(Tc);%繪制機器人沿圓弧運動的軌跡p560.plot(qc)參考程序NCUT7_9.m及程序講解視頻第7章工業機器人運動規劃7.3操作空間的軌跡規劃7.3.1空間直線插補7.3.2空間圓弧插補7.3.3機器人的末端姿態插補7.3.4操作空間軌跡規劃的幾何問題7.3.4操作空間軌跡規劃的幾何問題（1）不可到達的中間點圖中直線AB代表一條焊縫，我們希望Puma560末端沿著該焊縫運動從而完成該焊接工作，但事實上它是無法做到的。這是因為其工作空間的中心有一個空心圓柱，如圖（b）所示。也就是說該圓柱內的點機械臂末端均無法到達。7.3.4操作空間軌跡規劃的幾何問題圖中直線AB代表一條焊縫，我們希望Puma560末端沿著該焊縫運動從而完成該焊接工作，但事實上它是無法做到的。這是因為其工作空間的中心有一個空心圓柱，如圖（b）所示。也就是說該圓柱內的點機械臂末端均無法到達。（1）不可到達的中間點7.3.4操作空間軌跡規劃的幾何問題在操作空間做直線軌跡規劃時，看似兩個端點A、B均在工作空間之內，人們往往會誤以為該直線上的所有點機械臂末端均可到達。在操作空間做軌跡規劃時，需要特別關注軌跡上是否有中間點落在該空心圓柱之內，如果確有點落入其中，就必須重新規劃路徑，比如調整機械臂和焊接臺之間的距離，使焊縫遠離空心圓柱（比如圖中虛線的位置），從而確保機械臂末端可以到達直線上所有點。此外由于各關節都有運動范圍限制，機器人的工作空間并非完美的球形，做軌跡規劃時一定要充分了解特定機器人工作空間的形狀和大小，確保軌跡上所有點都是機械臂可以到達的。7.3.4操作空間軌跡規劃的幾何問題思考：在例7.8和例7.9中，如果我們將三點坐標修改為如圖所示，那么Puma560末端還能到達圓弧上所有點嗎？為什么？7.3.4操作空間軌跡規劃的幾何問題在機械臂的工作空間中，如果期望機械臂末端在奇異點附近以較大速率運行，可能會導致很大的問題。這是因為機械臂沿著直線路徑接近