大范圍平動并聯機器人運動學解耦與速度自適應規劃

趙新華，劉培昌，趙 磊*，李 彬

（1.天津理工大學天津市先進機電系統設計與智能控制重點實驗室，天津300384；2.天津理工大學機電工程國家級實驗教學示范中心，天津300384；3.天津理工大學機械工程學院，天津300384）

1 引 言

機器人技術的發展在對提高先進生產競爭力、促進國民經濟以及提高戰略地位具有重要深遠的意義。在機器人應用中，串聯機器人具有結構簡單和易操作的特點，最早被運用到工業生產領域［1］。但由于僅擁有一條控制鏈，存在誤差積累效應，工作速度相對較低。并聯機構具有負載能力強、剛度大和誤差不累積等優勢，但由于存在多條控制鏈，關節數量多，導致運動學解耦難度較大［2-4］。從力學角度分析，對于高速并聯機構而言，隨著機構原動構件轉速的提高，慣性負荷會成為激振力的主要組成部分，而且激振力的頻率也隨之提高［5］。由于連桿機構的各構件的慣性負荷與原動構件轉速的平方成正比，故隨著機械速度的增加，并聯機器人擁有的多個關節的彈性變形以及內部慣性力的耦合作用越加明顯，導致軌跡動態跟蹤時機器人伴隨著不同程度的振動［6-8］。雖然具有多支鏈、大范圍運動工業并聯機器人的控制精度相對較低，但其擁有的高速和高負載等明顯優勢使其仍在工業領域得到廣泛應用［9］。

并聯機器人具有良好的高速運動特性，但也存在精度和平穩性相對較差的難題。通常采用誤差補償來提高工作精度，選取合理的控制策略實現機器人的高速平穩控制。各種數值算法如改進蟻群算法、遺傳算法和神經網絡算法被應用到機器人路徑規劃［10-12］。現階段，工業機器人在進行實際軌跡跟蹤時，在軌跡末端存在一定的慣性速度導致不同程度的沖擊晃動，末端控制精度和平穩性較差［13-14］。目前，機器人系統中常用的速度規劃算法有梯型速度規劃算法和S型速度規劃算法。Hu等［15］通過梯型速度規劃方法控制機床，驗證加速度的不連續變化會引起振動和沖擊。史步海等［16-18］提出一種等價梯型的新型S型速度規劃算法，算法簡單，一定程度上降低機械沖擊，但實質上仍屬于梯型速度規劃。文獻［17-18］研究表明：S型速度規劃雖然控制算法比梯型控制更為復雜，但速度過渡更為平滑，有效緩解或消除梯型控制存在的因速度突變產生的誤差尖峰。然而，在實際應用中，采用S型速度規劃算法仍然無法消除或緩解在軌跡末端呈現的沖擊效應。

針對3-RRRU大范圍平動并聯機器人的運動學解耦和速度自適應規劃方法展開系統、深入地研究。應用DH法建立機器人的運動學解耦模型，實現了機器人的自動化控制；在S型速度規劃策略中引入自適應修正機制，實現了不同軌跡下最大速度的自適應優化，避免了由于速度參數設置不合理導致S型速度曲線的不對稱分布，實現了機器人軌跡跟蹤過程中速度過渡的平滑性和平穩控制。開展軌跡跟蹤實驗，驗證了所采用速度自適應規劃方法的有效性和可行性。

2 并聯機器人運動學解耦

2.1 并聯機器人結構

圖1 是本文所研究的3-RRRU并聯機器人，由靜平臺、動平臺和三條支鏈構成。圖2是其對應的結構示意圖，靜平臺固定在上方，動平臺則位于下方，且靜、動平臺均為正三角。三條支鏈結構相同，每條支鏈由三個轉動副和一個虎克鉸連接而成。靠近靜平臺的均為驅動副，其余為被動副；Aij為第i支鏈第j的運動副，三個驅動副的軸線均平行于三個相鄰被動副的軸線，同時平行于靜平臺所在平面，又分別垂直于第三個轉動副（即第二組被動副）的軸線，三個虎克鉸與動平臺連接。

2.2 機器人運動學坐標系的建立

以第一條支鏈為例建立運動學坐標系，O是靜平臺的幾何中心，也是基坐標系O-x0y0z0的原點，OZ0垂直于靜平臺，設OX0的方向為X軸方向，如圖3所示。θij為第i支鏈第j關節的角位移，旋轉軸zi1垂直于軸zi2和zi3，zi3平行于軸zi4；動平臺的幾何中心為P，其坐標系O-x′0y′0z′0三個坐標軸的方向與靜平臺坐標系O-x0y0z0的坐標軸方向相同。設機器人支鏈中各桿件長度為Lij（第i條支鏈中第j個桿件），R和r分別為靜平臺和動平臺的外接圓半徑。

圖1 3-RRRU并聯機器人Fig.1 3-RRRU parallel robot

圖2 3-RRRU并聯機器人結構示意圖Fig.2 Structure diagram of 3-RRRU parallel robot

圖3 第一支鏈運動學坐標系Fig.3 Kinematic coordinate system of the first branch chain

2.3 運動學反解

為了實現3-RRRU并聯機器人的自動化控制，應用DH法建立其運動學模型。第一支鏈齊次傳遞矩陣T可表示為：

其中：AR為靜平臺幾何中心到第一關節的齊次變換矩陣，Aij為第i支鏈第j的運動副。

相關矩陣參數如表1所示，對式（1）作如式（2）般變換：

表1 第一支鏈DH矩陣參數Tab.1 DH matrix parameters of the first chain

代入矩陣參數可得到第一支鏈運動學方程如式（3）所示：

基于機器人結構約束，關節θ13∈[-π/2，π/2]，故由式（3）可得：

將式（4）代入式（3）矩陣方程進而可求得剩余轉角θ11和θ12。同理，將坐標系AR分別旋轉60°和120°，亦可完成其他兩條支鏈關節角的計算。結合表2中機器人各構件參數可實現運動學解耦計算，具體計算過程這里不做過多贅述，詳見參考文獻［19］。

3 速度控制策略優化

表2 各構件參數Tab.2 Parameters of each component（mm）

3.1 速度自適應修正方法

軌跡規劃是機器人運動控制的核心，旨在確定機器人末端執行器在其工作空間的軌跡［19-21］。關于梯型控制原理在文獻［16］已詳細說明，此處不做贅述，標準S型加減速控制過程共由7個階段組成，分別為加加速、勻加速、減加速、勻速、加減速、勻減速和減減速，文獻［17］給出了位移、速度和加減速過程曲線如圖4所示。圖4中每個控制階段的加速度和速度均可通過下式（5）中的加速度和速度函數計算獲取。S型控制過程相比梯型復雜很多，設最大速度vm即勻速階段速度，最大加速度am，加加速度J，軌跡長度為s，s123表示經過加加速階段s1、勻加速階段s2、減加速階段s3達到最大速度時完成的位移；s13表示僅經過加加速段s1和減加速階段s3達到最大速度時完成的位移，對s123和s13可由式（5）中的速度和加速度函數計算得到。

圖4 S型加減速控制過程Fig.4 Acceleration and deceleration of S-type control

S型速度規劃策略中，常采用參數定值控制方式，當軌跡較短時致使預設最大速度值不合理，引起速度曲線呈現不對稱分布，此時機器人產生沖擊效應即出現明顯的晃動。基于上述分析，引入最大速度自適應修正機制如下：

（1）若2s123≤s，仍采用標準S型定值速度控制策略驅動機器人進行預設軌跡的動態跟蹤；

（2）當2s13≤s＜2s123，由式（5）可知在給定路徑內可以達到最大加速度am，但無法加速到最大速度vm而進入勻速階段，故S型最大速度vm修正為vnew如下：

則修正前、后速度曲線如圖5所示。

圖5 修正前后S型控制速度曲線Fig.5 S-type speed curve before and after correction

基于上述修正機制，速度自適應規劃步驟如下：

Step1：在預設路徑軌跡上共采樣n個位置點，Po(xo，yo，zo)和Pi(xi，yi，zi)分別為機器人初始位置原點和路徑軌跡上第i（i=1，2，...，n）點得空間理論坐標，基于運動學反解控制機器人進行軌跡跟蹤；

Step2：依據機器人系統設定的最大速度vm，最大加速度am和加加速度J等參數值進行快速判定，是否修正最大速度參數，修正后最大速度為vnew；

Step3：設采樣時間為Tc，n為采樣點個數，則可獲取第i個采樣點在t=iTc時刻的速度和加速度，進而可計算出該位置點坐標；

Step4：依據Step2中確定的控制參數應用激光跟蹤儀對機器人終端進行動態跟蹤，并記錄Step1中軌跡上各理論采樣位置點所對應的實際空間坐標則第i點跟蹤誤差為：

Step5：機器人實際速度ve(t)可由傳感器反饋數據獲得，則任意時刻t的速度誤差Δv(t)=

Step6：采用梯型控制策略進行上述軌跡跟蹤，并記錄對應采樣點數據。

3.2 實驗

機器人初始位置為Po(0，0，-0.4)，相關控制參數如表3所示，直線軌跡跟蹤在相關文獻已做了大量研究，這里不再重復。對xoy平面上兩個不同半徑的圓軌跡進行動態跟蹤實驗，圓心均為初始位置點Po，圓半徑分別為0.4 m和0.1 m，其對應軌跡長度s分別為2.512 m和1.256 m。

基于預設軌跡通過快速計算和判定，機器人可以加速到最大速度且能夠保持速度控制曲線的對稱分布。圖7是第一條軌跡即半徑為0.4 m圓形路徑的跟蹤情況，基于激光跟蹤儀獲取的空間采樣點位置如圖7（a），擬合給出了梯型和常規S型控制策略下機器人實際與理論采樣點分布。

圖6 3-RRRU并聯機器人軌跡跟蹤實驗Fig.6 Trajectory tracking experiment of 3-RRRU parallel robot

圖7 （b）實際跟蹤軌跡可以看出：S型控制策略對于曲線軌跡跟蹤精度明顯優于梯型控制策略。采用梯型控制機器人的跟蹤軌跡反映出由于加減速引起的速度突變導致跟蹤精度較差且軌跡整體平滑性不如S型速度算法；圖8是機器人跟蹤誤差曲線，從誤差曲線可知：S型控制下機器人的位置誤差跟蹤曲線不存在尖峰值，表明整個路徑下機器人運動的平穩性良好，梯型控制由于速度突變產生的最大位置誤差達到4.513 mm，是S型控制策略的3倍；此外，從x，y和z方向誤差曲線反映機器人在z向誤差明顯小于x和y方向，S型z向誤差分布在-1～1 mm，梯型最大誤差約是S型控制策略的2倍；由于并聯機器人采用三支鏈對稱設計，存在多支鏈閉環約束效應，故兩種控制策略下x和y向跟蹤誤差分布也基本一致；若單向誤差極大表明機器人結構組裝存在很大的誤差，需要對支鏈重新定位和安裝。

表3 機器人控制參數Tab.3 Control parameters of robot

圖7 空間位置點分布和軌跡跟蹤Fig.7 Distribution of space points and trajectory tracking

圖8 S型和梯型速度規劃下的位置跟蹤誤差Fig.8 Position tracking error under S-type and ladder type velocity planning

圖8 表明：S型速度規劃策略對軌跡跟蹤時具有良好的平穩性，加、減速轉變時速度過渡平滑，整體誤差分布在0.225～1.615 mm，且沒有明顯尖峰。

為了測試S型控制策略加入自適應修正機制后控制策略的有效性和可行性，設計第二次路徑規劃即半徑為0.1 m的圓形軌跡，通過修正計算公式，獲得優化后的最大速度為1.944 m/s，如表4所示，與預設2 m/s最大速度很接近，故下面測試了最大速度修正前、后機器人的動態跟蹤誤差。

表4 不同軌跡下速度修正Tab.4 Velocity correction under different trajectories

從圖9（a）中實際跟蹤位置點分布情況發現，最大速度修正后采樣點整體跟蹤精度優于修正前，而通過擬合后的跟蹤軌跡不難看出，修正前的跟蹤軌跡起始點和終點位置誤差明顯較大；雖然修正后在最大速度位置點與修正前跟蹤誤差并不明顯，但是在軌跡終點機器人速度未減速到零而具有一定的慣性速度，產生沖擊效應導致起始點和終點位置偏差很大。

圖9 速度修正前后空間位置點分布和軌跡跟蹤Fig.9 Distribution of position points and trajectory tracking before and after velocity correction

圖10 S型控制速度修正前后位置跟蹤誤差Fig.10 Tracking error before and after speed correction by S-type control

由圖10和表5數據可知：速度修正后機器人整體路徑的跟蹤精度優于修正前，二者誤差曲線中誤差尖峰很少，表明速度過渡平滑性良好；但修正前機器人在軌跡終點處產生最大誤差，表明機器人停止時因具有較大慣性速度而呈現明顯的沖擊晃動即沖擊效應，勢必產生誤差尖峰，對于航空航天和船舶制造等高精密領域而言是非常危險的；第二次實驗預設軌跡是周長未0.628 m的圓，修正前機器人加速到最大速度時其運動距離達到0.330 m，由于S型速度控制曲線是呈對稱分布，故在機器人達到終點時，速度并未由減減速階段平滑過渡為零，產生明顯的位置尖峰誤差，高達2.676 mm；修正后其位置最大誤差僅為1.125 mm，起點和終點位置跟蹤誤差分別為0.722 mm和0.382 mm，位置相對誤差為0.34 mm，表明機器人有效消除了上述尖峰誤差。

表5 修正前后位置誤差Tab.5 Error before and after correction

圖11 修正前、后速度曲線Fig.11 Velocity curve before and after correction

圖11 是機器人最大速度修正后速度控制曲線，機器人控制仍采用預設最大速度值，雖然速度增加到了2 m/s，但是按原有加加速度和加速度進行減速時，在勻減速階段已到達預設軌跡終點，此時具有一定的慣性速度，停止時勢必出現誤差尖峰，這與圖10所得結論一致；

不難發現，修正后的最大速度為1.944 m/s。雖然略小于預設值2 m/s，但終點位置誤差降低了一個數量級，末端控制平穩性大幅提高，有效解決定值控制方式下因速度參數設定不合理而導致機器人末端常出現沖擊效應的問題。

4 結 論

S型速度控制策略相比梯型控制具有良好的跟蹤精度和平穩性。梯型速度控制下位置跟蹤曲線存在多個誤差尖峰，其最大誤差達到4.513 mm；S型速度規劃策略下的位置和速度曲線更為平滑，最大位置誤差僅為梯型控制的1/3；

在較短軌跡下，修正前機器人在軌跡末端存在明顯的沖擊效應，其最大位置誤差高達2.676 mm；修正后最大誤差為1.125 mm，且機器人終點位置誤差僅為0.382 mm，相比修正前降低了一個數量級；

機器人路徑變化時，較短軌跡下仍采用常規定值控制，使得預設最大速度值過大進而導致機器人產生末端沖擊效應；

引入速度自適應修正機制，有效解決了機器人的沖擊效應問題，大幅提高了機器人的動態跟蹤精度和平穩性。