摘要：機械臂軌跡規劃是（shì）機器人（rén）運動規劃領域中至關（guān）重要的經典研究方（fāng）向之一。隨著機器人時代的到來，機械臂在工業製造中得到了廣泛的應用，如何更好的利用機械臂的運動性能，獲得運行時間較短或者消耗（hào）能量更少的運（yùn）動軌跡成為人們關注的（de）重點（diǎn）。
    
       關鍵詞：軌跡規劃，動為學（xué）模型，時間最優（yōu），能量最優

        引言
 
        在最（zuì）大化化器人係統的生產效（xiào）率方（fāng）麵，時（shí）間最優運動規劃（huá）一直是一個很重要的（de）課題。一壟常用的求解最短時間的軌跡規劃方法通（tōng）常隻用到機器人的運動學模型，而沒有用到機器（qì）人的動力學模型，這樣的規劃結果（guǒ）往往不能充分發揮機器人的（de）潛力（lì）。
  
        結合第（dì）二（èr）章介紹的機械臂動為學模型參數辨識方法，那麽可Ｗ辨識（shí）出機械臂的動力學模型，進（jìn）而使（shǐ）用更為有效的基（jī）於機（jī）械臂動力學模型的（de）軌跡規（guī）劃方法。本章采用了Ｖｅｒｓｃｈｅｕｒｅ［５６：ｉ等人提出的（de）基於（yú）凸優化求解時間最優軌跡（jì）規劃方法，並在此基礎上（shàng）添（tiān）加了表示能量和力矩變化（huà）率的代價函數，使得規劃（huá）軌跡更加易於機械臂的執行。
  
        本章的組織（zhī）結構如下：３．２節介（jiè）紹了基本基（jī）於力矩約束的時間最優軌跡規劃問題（tí）；３．３節（jiē）闡述了如何將基（jī）本時間最優問題轉化到凸優化控（kòng）製問題；３．４節對此凸優化控製問題進行拓展，添加額（é）外的目標函數和約束（shù）條件；３．５節介紹了此凸優（yōu）化控製（zhì）問題的數值解法；３．６節Ｗ六自由度機（jī）械臂為目標進行軌跡規劃，驗證此方法的有（yǒu）效性。
  
       ３．２基（jī）本問題描述
  
       ｉｖ自由（yóu）度機械臂的動（dòng）為學運（yùn）動模型可Ｗ表示為如式（２．１）所示的關節力矩Ｔ（s） ＂關於各關節角ｇｅ巧＂的函數。事實上機械臂的（de）軌（guǐ）跡規劃問題一般都是建立在己知路徑（jìng）上的，對於（yú）關節空間中的路徑我們可Ｗ用t（ｓ）來表示，其中偽位移Ｓ是一維路爸坐標係。這個路徑坐標係是由機械臂末（mò）端在巧卡爾空間中移動的路徑決定的，那麽軌跡的時（shí）間（jiān）約束就可表示為坐標軸Ｓ和時間t的關係式S（t)為了不失一般性，可レ乂假設軌跡起（qǐ）始於於t＝０，結束於t＝T並滿足（zú）s(0)＝０＜５(t)＜１＝s（t)，除此之（zhī）外（wài），由於現在考慮的是時間最優軌跡規劃及其相關問題（tí），故對於（yú）t包含（0，T），可化為Ｓ（t）的上的毎一點都符合（hé）s（t）＞〇，並且在Ｓ（t）上的絕大多數點上都符合s（t）＞０。同時，對於給定的（de）關節空間中的路徑ｇ（ｓ），關節的速度和加速度可Ｗ通過鏈式法則推導得到：

 
      
      
  
       那麽基於（yú）為矩約（yuē）東（dōng）的時間最優軌跡規（guī）劃可用（yòng）數學表達式來（lái）據述：

      
  
      
       
  
        基（jī）本時（shí）間最優問題到巧（qiǎo）優化控製問題的變換
 
       對式口．７）這樣形式（shì）的非線性約束下（xià）的目標函數，是無法直接看出所得到的局部最優解是否就是全局最優解。而在ＳｈｍｅｒｆＷ這篇文章中，針對時間能量最優運動規劃問（wèn）題將式貨（huò）７）進行了（le）特殊非線性（xìng）變換，令其（qí）轉化為一個受非線性狀態控製約束，並包含線性係統動為學模型、狀態微分和控製輸入的最（zuì）優控製問題，同時證明了相對於控（kòng）製輸入的漢（hàn）密（mì）爾頓函數是（shì）曲函（hán）數（shù），進而可レ乂證明擁有這種（zhǒng）結構的問題的局部最優解就是（shì）全局最優解，因此（cǐ）本文也對式（shì）（３．７）的一些變量的進行非線性變換。
  
      首先，通過（guò）將原來得積分變量＾變換為５，此時目標函數變為
  
     
  
    
  
      因此式巧．７）描述的問題可１＾＾被重構（gòu）為一個凸問題
 
     
  
      式（３．１４）所描述的問題是田問題是因為所有（yǒu）的約束都是線性的，而目標函數是凸函（hán）數。
  
      這個問題可Ｗ認為是代數（shù）微分形式的最（zuì）優控（kòng）製問題，它有著偽時（shí）間變量５，控製輸入ａ（ｓ），狀態微分６（３），代數形式的狀態ｒ（ｓ），線性係統（tǒng）的動態特性ｂ＇（ｓ）＝２ａ（ｓ）和與（yǔ）線性狀態相關的約東條件，Ｗ及初始狀態和終止狀態的約束 條件。
  
     可以看到式（３．１４）僅僅隻有一個微分狀態，而代數形式（shì）的狀態可通過Ｔ（ｓ）＝ｍ（ｓ）ａ（ｓ）＋ｃ（ｓ）６（ｓ）＋ｇ（ｓ）消除，並（bìng）且（qiě）表（biǎo）示（shì）時間（jiān）的變量ｔ不再顯式的出（chū）現在方程（chéng）中。這種問（wèn）題重構形（xíng）式最大的價值在於：首先其目標函（hán）數和約宋都轉變（biàn）為了凸函數，所求得的局部最優解即為全局最優解；其次，可（kě）Ｗ很容易的添加
目標函數和不等式約（yuē）束，而不改（gǎi）變整個最優化控製問題的凸性，這將（jiāng）在下（xià）麵的章節中進行討（tǎo）論；最後，利用數（shù）值方法可Ｗ非常有效的對這種表巧形式進行求解。
 
     ３．４擴展（zhǎn）的西優化控製問（wèn）題
  
     在３．４．１節中介紹一些實（shí）用的約束條件，在３．４．２節中本文提出了額外的目標函數（shù），這些目（mù）標（biāo）函數的增加是（shì）為了使得規劃出來的軌跡為（wéi）矩變（biàn）化較小，更（gèng）加易於實際機械臂的執行，最後在３．４．３節中將這些目標函數和約宋結合起來，得到一個（gè）更加（jiā）一般但仍能保持凸特性的最優化控製問題。
 
 
      ３．４．１其他不等式約束
  
      除（chú）了各關節為矩約束，還有其他一（yī）些很有用的約束。
  
     １．速度約（yuē）束：一些特別的（de）任務中可能要求加入速度（dù）限製（zhì），此（cǐ）時可Ｗ在第ｉ個關節上施加對稱的最小速度約束－和最大速度約束亮（５），其（qí）具體表（biǎo）達式如下所示：


   
 
  
     加速度約束（shù）和速度約束類似，化可ｗ被應用在笛卡爾空間中的加速度分量上，而不管是作用在關節空間中的加速度（dù）約宋（sòng）還是作（zuò）用在笛卡爾空間中的加速度約（yuē）束都可Ｗ被寫（xiě）成一個（gè）統一的形式：
  
    
 
     ３．４．２目（mù）標函數優化
 
     由於時間最優軌跡通常有著ｂａｎｇ－ｂａｎｇ特（tè）性，並（bìng）不易於機械臂的執行。所（suǒ）Ｗ本文在這個（gè）基於凸優化的問題框架中（zhōng）添加一些其（qí）它（tā）目（mù）標函數，從而得到一條更加理想的、考慮其它準則的（de）時間最優運動軌跡。
  
     １．能量消耗：
 
     第關節力矩的平方關於時間的積分可Ｗ表示為：
 
    
  
     這個目標函數表示的是驅動第ｉ個關節運動所消耗（hào）的能量，並且此函（hán）數是凸函數。
   
     ２．為矩變（biàn）化率絕對值的積分：
  
    
  
     因為Ｎ是關於Ｅ的巧函數，所Ｗ式（３．２巧表示的目標（biāo）函數也是凸函數。雖然這樣的目標函數並沒（méi）有比較直觀的物理解釋，但是在最終的目標函數中包含這項可Ｗ減小關節為矩的變化（huà）率。
  
     ３．４．３廣義的時間最優問題（tí）
  
     結合式（shì）口．７），式（３．２２）和式（shì）口．２３）所表示的目標函數與式（３．７），式（shì）口（kǒu）．１９）和式口．２１）所表示的巧宋項，可Ｗ得到一個廣義的廣義最優控製問題
  
    
  
     由（yóu）於這個表適式（shì）中的目標函數和（hé）不等式約束都是凸的，而等式約束是線性的，所上述廣義時間最優問（wèn）題還是凸的。
  
     ３．５數值計算
 

  
     由於式（３．２９）描述（shù）的問題（tí）為凸，所Ｌ乂任何局部最優解也是全局最優的。因此可Ｗ任意選取一個非線性求解器來求解這個問（wèn）題。本文采用（yòng）的是ｍａｔｌ油（yóu）中的（de）ＹＡＬＭＩＰ王具箱進行求解，這是一（yī）種可Ｗ求（qiú）解高（gāo）級（jí）優化問題（tí）工具箱，並且有多種求解器可供選擇（zé）。一旦求（qiú）解出變量護（hù）的值，則可通過束解＊（ｓ）得到表示路徑坐標軸（zhóu）和時間的關係式Ｓ的，而＊（ｓ）的表達式如下所示：
  
     
  
     ３．６實驗與分析
  
     本節在ＡＢＢ六軸工（gōng）業機器人ＩＲＢ１２０上（shàng）（如困３．４所示）實現基於凸優化的時間最優軌跡規劃，３．６．１節介紹了（le）實驗任務和目標路徑，３．６．２節說明了實驗結果。
  
    
  
     ３．６．１實驗任務說明
   
     本實驗是用ＩＲＢ１２０六軸工（gōng）業機械臂執行（háng）複雜的書寫任務，書寫任務為在水平麵上寫出＂ＺＪＵ＂字樣（如圖３．５所示），書寫要求（qiú）是盡可能快的完成這一任務。
  
     
  
     為了實現基於動力學（xué）模型的最優時間軌跡規劃，需要知（zhī）道ＩＲＢ１２０六軸工業機械臂的動為學模型（xíng）參數，由於本實驗室的ＩＲＢ１２０機械臂缺芝（zhī）參數辨識的（de）條件（這在上一章有所介紹），故此處軌（guǐ）跡規（guī）劃（huá）使用的機械臂動力學模型參數是由ＡＢＢ麽司提供的。
 
     ３．６．２實驗結果
  
     根據３．５小節提（tí）出的數值計算方法，可得（dé）到完（wán）成書寫’’ＺＪＵ＂任務的（de）最短時間為６．４巧。如圖３．６所示（shì）的（de）是（shì）完成這一任務的機械臂六個自由度關節的驅動力矩曲（qǔ）線，可看到在任意一個時刻都有一個關節的驅動為矩是處於滿負荷運行的，由於第一關節總是承受這絕大（dà）部（bù）分的重力作用，所Ｗ其很多時間都是處於最大力矩控製狀態是可Ｗ理解的。圖３．８表示的是時間最（zuì）優軌（guǐ）跡規劃對應的位置和速度曲線（xiàn），由速度曲線可レ乂看出（chū）規劃（huá）軌跡速（sù）度在很多地方都有尖角，這也符合控製為矩的突變（biàn）。
  
     
  
    
  
     可見單純的時間最優軌跡規劃力矩（jǔ）變化十分劇烈，這種軌跡的（de）執行會造成機械（xiè）臂（bì）很大的負擔，並且在實際的機械臂上也是很難實現的，本章在凸優化框架基礎上加入的代表能量（liàng）消耗的目（mù）標函數和代表為矩變化率的目標函數就是（shì）為了解決這一問題。圖３．９表示的是（shì）考慮運（yùn）動時間和能量消耗時的軌跡規劃結果，可Ｗ看到運動的時（shí）間增大到１８．３ｓ而運動（dòng）過程（chéng）中（zhōng）各關節最大輸出（chū）為矩顯著減小，同時根據式（３．２４）可Ｗ通（tōng）過增大Ａｉ的取值（zhí）使得規劃出的軌跡的能量消（xiāo）耗變小。圖３．１０表示的是考（kǎo）慮運動（dòng）時間和為矩變化率的軌跡規劃結果，可Ｗ看到運動（dòng）的（de）時間增大（dà）到７．２ｓ而各關節力矩變化率降低了，同時根據（jù）式（３．２４）可Ｗ通過增大乂２的取值使得規劃出的軌跡（jì）的力（lì）矩變化率進一步減小。
  
    
  
     
  
     由於六自由度機械臂ＩＲＢ１２０並（bìng）沒有（yǒu）提供力矩輸入的接（jiē）口，本次實驗規劃出來的軌跡沒有在實際機械臂上運行，然而軌（guǐ）跡規劃的運動軌跡符合機械臂動為學橫型等一係（xì）列約束條件，故可認為規劃結果在實際機械（xiè）臂上化是可行的。
  
    
  
     ３、 本章小結
  
     本章介紹（shào）了用（yòng）凸優（yōu）化方法（fǎ）求解時間最（zuì）優軌跡規劃問題，拓展了優化的目標函數，考慮能量消耗和各關節為矩的變化率，使得得到的（de）軌跡更加（jiā）平滑，易於機械臂的實現。最後針對六自由度機械臂ＩＲＢ１２０進行了最優軌跡的規劃（huá），驗證了（le）算法的有效性。