模塊化機械臂軌跡規(guī)劃算法仿真研究*

□ 嵇建成 □ 郭 帥 □ 薛 卉 □ 王志勇

1.上海大學 上海市智能制造及機器人重點實驗室 上海 200072

2.上海電氣集團股份有限公司 中央研究院 上海 200070

模塊化機械臂軌跡規(guī)劃算法仿真研究*

□ 嵇建成1□ 郭 帥1□ 薛 卉2□ 王志勇1

1.上海大學 上海市智能制造及機器人重點實驗室 上海 200072

2.上海電氣集團股份有限公司 中央研究院 上海 200070

基于OpenGL模塊化機械臂三維仿真模型，闡述了豎直六自由度、懸掛六自由度以及豎直七自由度等多種模塊化機械臂的運動學求解，實現(xiàn)了三次多項式、五次多項式、BDJP等多種插補算法，完成了在笛卡爾空間層中以直角坐標路徑控制和以BDJP法實現(xiàn)圓弧軌跡插值控制。另外，在模塊化機械臂實驗平臺上，針對各種運動學求解算法編寫了驗算程序，完成了豎直六自由度同步在線控制、懸掛六自由度指令控制走“中”字和圓等實驗，證明了算法的正確性和精確性。

軌跡規(guī)劃 OpenGL 模塊化機械臂

服務機械臂不僅在空間探測、軍事戰(zhàn)場、家庭服務領域有著重要的應用價值和廣闊的發(fā)展前景，而且在工業(yè)生產(chǎn)、醫(yī)療保健領域也存在很大的市場潛力［1］。許多科研機構(gòu)競相開展了對服務機械臂的研究，但由于服務機械臂涉及軌跡規(guī)劃、遙控操作等復雜技術(shù)，使服務機械臂的仿真控制技術(shù)發(fā)展緩慢。

筆者以德國Schunk模塊化服務機械臂為研究對象，基于OpenGL完成了服務機械臂仿真控制平臺的研發(fā)，實現(xiàn)了三維仿真模型和實物模型的軌跡規(guī)劃和同步動作，為進一步實現(xiàn)無線遙控操作技術(shù)打好基礎。

1 簡介

平臺的硬件部分是根據(jù)德國Schunk模塊化服務機械臂電氣和硬件要求完成服務機械臂組裝，通過CAN總線和模塊API函數(shù)，實現(xiàn)硬件軟件控制［2～4］。軟件層面，上位計算機根據(jù)仿真模型和硬件模型結(jié)構(gòu)，完成運動學求解，如圖1所示。

軌跡規(guī)劃是實現(xiàn)PTP運動、過路徑點運動、直線運動、圓弧運動以及解析平臺指令的基礎，決定著服務機械臂運動的精度［5～7］。模塊化機械臂仿真軟件的軌跡規(guī)劃器主要在關節(jié)層采用拋物線過渡、三次多項式、過路徑點三次多項式、過路徑點五次多項式等插

值算法，實現(xiàn)關節(jié)運動軌跡規(guī)劃；在笛卡爾空間層采用直角坐標路徑控制法、BDJP法等插補算法，實現(xiàn)走直線、圓弧等軌跡運動［8～12］；其它兩層實現(xiàn)硬件模型和仿真模型的運動控制功能。指令系統(tǒng)負責服務機械臂指令文件的編譯、調(diào)試及執(zhí)行，軟件交互界面負責顯示三維仿真模型的運動過程及用戶交互。通過模塊的光電編碼器采集模塊的位置和速度，驅(qū)動仿真模型動作，監(jiān)視模型末端位置情況［13～16］。本文重點闡述基于BDJP插值算法的軌跡規(guī)劃工作。

▲圖1 模塊化機械臂仿真平臺軟件結(jié)構(gòu)

2 基于多項式軌跡規(guī)劃算法

筆者設計的軌跡規(guī)劃器根據(jù)用戶指令，采用拋物線過渡、三次多項式、五次多項式、過路徑點三次多項式、過路徑點五次多項式插值算法，在關節(jié)空間實現(xiàn)了服務機械臂的PTP和過路徑點運動；采用直角坐標路徑法和BDJP插值法，在笛卡爾空間實現(xiàn)服務機械臂的直線和圓弧功能。

2.1 拋物線過渡插值算法

Schunk運動模塊接口函數(shù)提供了一種梯形運動方式，其速度曲線如圖2所示。該接口函數(shù)能實現(xiàn)帶拋物線過渡域的線性軌跡（LSPB），即拋物線過渡插值算法，手臂運行可分為3個過程：加速、勻速和減速，假設初始位置為θi，初始加速度為，手臂最大速度為，終止時加速度為，終止位置為θf。在［0，ta］時間段手臂處于加速過程，加速度為，加速時間為ta；在［ta，tf-td］時間段手臂各關節(jié)處于勻速過程，速度為，勻速運行時間為tu；在［tf-td，tf］時間段內(nèi)手臂各關節(jié)減速運行直至停止，減速時間為td。

▲圖2 梯形插值V-T圖

根據(jù)拋物線過渡運行特點可得：

由式（1）得手臂插值函數(shù)為：

為保證各關節(jié)同時啟停，對各關節(jié)的運行基于V-T圖進行時間歸一化處理，其處理過程如圖3所示。

▲圖3 時間歸一化處理

以懸掛六自由度服務機械臂為例，設：初始位置各關節(jié)為0°，目標位置關節(jié)角度為 （20，30，40，-26，-15，-23），速度為10°/s，加速度為20°/s2，進行實驗。用該算法插值時，服務機械臂各關節(jié)速度時間曲線如圖4所示。

由圖4可知，采用該算法能實現(xiàn)PTP運動，但在關節(jié)速度為零或加速度發(fā)生突變時，電機的速度時間曲線有一定幅度的振蕩，振蕩主要由加速度突變引起。

▲圖4 拋物線插值V-T圖

2.2 三次多項式插值算法

為克服加速度突變導致的電機運動振蕩，采用了三次多項式插值算法來實現(xiàn)關節(jié)空間的軌跡規(guī)劃。設

關節(jié)空間運動的三次多項式為：

由于關節(jié)運動的目標位置和運動時間tf由指令系統(tǒng)指定，并且起始點和終止點速度為零，因此可得4個約束方程。設起始點位置為θ0、終止點位置為θf，則約束方程組為：

求解上述線性方程組可得：

各組關節(jié)根據(jù)式（5）求出三次多項式因子，然后按照式（3）進行關節(jié)插值運動。需要注意的是在完成插值運動后要用軟件限幅濾波法濾波，以保證關節(jié)運動在允許的范圍內(nèi)。

以懸掛六自由度服務機械臂為例進行實驗，讓其從初始位置（0，0，0，0，0，0）運動到（30，25，60，42，-34，-28），運動時間為5 s。用該算法實驗時，服務機械臂各關節(jié)速度時間曲線如圖5所示?？梢?，在采用三次多項式插值算法的情況下，服務機械臂能實現(xiàn)PTP運動，而且運動更加平穩(wěn)，振蕩明顯減弱。

▲圖5 三次多項式插值V-T圖

2.3 過路徑點連續(xù)三次多項式插值算法

為實現(xiàn)關節(jié)空間的過路徑點連續(xù)運動，筆者設計了過路徑點連續(xù)三次多項式插值算法。算法的思想是在多組關節(jié)目標位置和各目標位置運動之間的時間值由指令系統(tǒng)指定后，采用啟發(fā)模式設定關節(jié)各路徑點的速度，再按照三次多項式插值算法完成插值運算，最后采用限幅濾波法保證其在關節(jié)工作范圍內(nèi)。

采用啟發(fā)模式設定路徑點速度，如圖6所示。圖中，θ0為起始點，θD為終止點，θA、θB和θC為路徑點。假設用直線段把這些路徑點依次連接起來，如果相鄰線段的斜率在路徑點處改變符號，則把速度設定為零；如果相鄰線段不改變符號，則選取路徑點兩側(cè)線段斜率的平均值作為該點的速度。因此，根據(jù)指令，系統(tǒng)就能夠按此規(guī)則自動生成相應的路徑點速度。

▲圖6 路徑點速度啟發(fā)原理圖

通過速度啟發(fā)模式獲得路徑點的速度后，過路徑點連續(xù)三次多項式插值問題轉(zhuǎn)換為已知起始點位置和速度、終止點位置和速度、運動時間的三次多項式插值問題。設起點位置為θ0，起點速度為，終止點位置為θf，終止點速度為，起點運動到終點的運動時間為tf。

則根據(jù)約束條件可得：

解方程組（6）得：

由式（7）求得三次插值多項式，并結(jié)合三次多項式式（6）循環(huán)完成過路徑點的連續(xù)插值，最后利用限幅濾波法進行濾波。

以懸掛六自由度服務機械臂為例進行實驗，初始位置為（0，0，0，0，0，0），第一個路徑點為（20，30，40，23，-34，-28），時間是5 s，第二個路徑點為（35，23，46，31，0，13），時間是5 s，第三個路徑點為（41，21，52，20，15，26），時間是4s，終 止 點 為（60，31，62，31，36，30），時間是3 s。用該算法進行軌跡規(guī)劃時，服務機械臂各關節(jié)速度時間曲線如圖7所示。說明：由于模塊返回速度信息不具有方向性，因此圖7中部分關節(jié)速度曲線發(fā)生了翻折。

▲圖7 過路徑點三次多項式插值V-T圖

由圖7可知，過路徑點三次多項式插值算法，能實現(xiàn)服務機械臂關節(jié)空間的過路徑點連續(xù)軌跡運動，但由于路徑點速度、加速度不連續(xù)，導致路徑點連接處的振蕩幅度偏大。

2.4 過路徑點五次多項式實現(xiàn)過路徑點連續(xù)運動

為克服路徑點連接處振蕩較大問題，采用過路徑點五次多項式實現(xiàn)過路徑點連續(xù)運動。算法采用速度和加速度啟發(fā)模式，設定各個路徑點關節(jié)的速度和加速度，啟發(fā)規(guī)則與方式，如圖6所示。完成速度和加速啟發(fā)后，問題轉(zhuǎn)化為：已知起始點和終止點的位置、速度、加速度及運動時間的三次多項式插值問題。設起始點位置為θ0，起始點速度為，起始點的加速度為，終止點位置θf，終止點速度為，終止點的加速度為，運動時間為tf。設關節(jié)五次插值多項式為：

根據(jù)式（10），求解出各路徑點五次多項式參數(shù)進行插值，再利用限幅濾波法進行濾波。

以懸掛六自由度服務機械臂為例進行試驗，初始位置為（0，0，0，0，0，0），第一個路徑點為（20，30，40，23，-34，-28）、時間是5 s，第二個路徑點為（35，23，46，31，0，13）、時間是5 s，第三個路徑點為（41，21，52，20，15，26）、時 間是 4 s，終止點為（60，31，62，31，36，30）、時間是3 s。用該算法時，服務機械臂各關節(jié)速度時間曲線如圖8所示。

▲圖8 過路徑點五次多項式插值V-T圖

由圖8可知，過路徑點五次多項式插值算法能實現(xiàn)過路徑點連續(xù)運動，而且路徑點連接處速度、加速度連續(xù)，整個運動過程振蕩極小。

2.5 直角坐標路徑控制法實現(xiàn)直線運動

為實現(xiàn)服務機械臂末端的直線運動，軌跡規(guī)劃器實現(xiàn)了直角坐標路徑控制插值算法，采用拋物線過渡離散規(guī)則，根據(jù)指令系統(tǒng)傳入目標位姿、位置運動速度、姿態(tài)運動速度、運動時間，利用四元數(shù)進行位姿離散。設當前位姿為P0，目標位姿為P1，位置直線運動速度為V1，運動時間為T，則：

（1）根據(jù)當前位置P0=（x0，y0，z0）、目標位置P1=（x1，y1，z1）、直線運動速度V1和運動時間T，按照拋物線過渡運動，計算出從初始位姿P0直線插值運動到P1的位置直線運動加速度A1：

（2）由姿態(tài)矩陣R0和目標姿態(tài)矩陣R1，計算出兩姿態(tài)矩陣對應的四元數(shù)，然后利用四元數(shù)的共軛性質(zhì)求解出，利用四元數(shù)乘法性質(zhì)計算出工具姿態(tài)由R0轉(zhuǎn)為R1的姿態(tài)旋轉(zhuǎn)矩陣R為：

最后將姿態(tài)旋轉(zhuǎn)矩陣R，利用旋轉(zhuǎn)變換式（13），計算出等效旋轉(zhuǎn)軸n的等效旋轉(zhuǎn)角度θ。

（3）由上步計算得等效旋轉(zhuǎn)角度θ，根據(jù)拋物線過

渡運動，計算出姿態(tài)運動速度V2和加速度A2，即：

（4）根據(jù)計算出來的拋物線過渡參數(shù)，采用拋物線過渡法計算離散位姿。

（5）用運動學求解器求解出各離散位姿對應的關節(jié)位置。

（6）采用過路徑點五次多項式插值算法，控制服務機械臂和仿真模型按離散關節(jié)位置實現(xiàn)過路徑點的連續(xù)運動，實現(xiàn)直線運動軌跡。

以懸掛六自由度服務機械臂為例進行實驗，設：初始位姿為 （-190，25，-500，0，180，0），末端位姿為（-70，25，-500，0，120，0），位置運動速度為20 mm/s，運動時間為8 s，用該算法規(guī)劃時，服務機械臂末端運動軌跡如圖9所示，與理想位置比較所得偏差如圖10所示。

由圖10可以看出，各采樣點3個方向的目標位置與理想位置的偏差都在2.5 mm范圍內(nèi)，則直線位置最大偏差為4.33 mm。由圖9可知，服務機械臂走出一條較精準直線，但該算法無法顯示設定直線精度。

▲圖9 直角坐標控制法末端軌跡

▲圖10 末端軌跡偏差

3 基于BDJP法軌跡規(guī)劃

3.1 BDJP法實現(xiàn)直線運動

為克服直線精度無法設定問題，筆者采用了BDJP（Bounded Deviation Joint Path）法實現(xiàn)直線插值。設：起始位姿為P0，末端位姿為P1，運動時間為T，位置最大偏差為δp，旋轉(zhuǎn)最大偏差為δR，其具體步驟如下。

（1）計算關節(jié)變量解，計算對應于P0和P1的關節(jié)矢量q0和q1。

（2）求出關節(jié)空間的中間點，計算關節(jié)變量空間的中間點：

式（16）中，Δq1=q1-q0，再由qm計算相應的工具坐標系Pm。

（3）求出直角坐標空間的中點，計算相應的直角坐標路徑的中點PC：

（4）求出偏離誤差，計算Pm和PC之間的偏差：

（6）根據(jù)總時間T和插值最大位移，將總時間進行細分。

（7）然后按照過路徑點五次多項式插值算法，進行關節(jié)插值控制。

以懸掛六自由度服務機械臂為例進行實驗，設：初始位姿為 （-190，25，-500，0，180，0），末端位姿為（-70，25，-500，0，120，0），位置最大偏差為2 mm，最大旋轉(zhuǎn)偏差為3°，運動時間為10 s，用該算法規(guī)劃時，服務機械臂末端運動軌跡如圖11所示，與理想位置比較所得偏差如圖12所示。

由圖12可知，采用BDJP法實現(xiàn)軌跡規(guī)劃時，各采樣點3個方向的目標位置與理想位置的偏差都在1.1 mm范圍內(nèi)，則直線位置最大偏差為1.91 mm，滿足2 mm的設定要求。由圖11可知，該算法能夠?qū)崿F(xiàn)可控制精度的直線插值。

3.2 BDJP法實現(xiàn)圓弧運動

在BDJP直線插補算法的基礎上，實現(xiàn)末端軌跡圓弧插值，基本思想是首先根據(jù)指令系統(tǒng)指定的圓弧中間點位姿、圓弧終點位姿計算出圓弧中心和圓弧半徑；然后再根據(jù)最大位置精度計算出細分弧，如圖13所示，獲得細分點位姿；然后計算出圓弧在精度內(nèi)細分

的各個點，再按照BDJP法直線插值算法代替圓弧進行插值運動控制。

以懸掛六自由度服務機械臂為例，設：圓弧初始位姿為（-20，0，-750，0，180，0），中間點位姿為（-120，-100，-750，0，180，0），圓弧終點位姿為（-220，0，-750，0，180，0），位置最大偏差為2 mm，旋轉(zhuǎn)最大偏差為3°，T為13 s，用該算法規(guī)劃時，服務機械臂末端運動軌跡如圖14所示，與理想位置比較所得偏差如圖15所示。

由圖14可知，服務機械臂按照該算法能夠?qū)崿F(xiàn)圓弧軌跡插值運動，而且由圖15可知，各方向最大偏差值都小于1.5 mm，圓弧位置偏差最大小于2 mm，滿足算法設定精度。

▲圖11 BDJP法末端軌跡

▲圖12 末端軌跡偏差

▲圖13 圓弧細分作直線處理方法

▲圖14 圓弧插值實驗末端軌跡

▲圖15 末端軌跡偏差

4 機械臂軌跡規(guī)劃實驗

該平臺已完成多種配置模塊化服務機械臂的同步在線控制測試，以豎直六自由度服務機械臂進行在線模型控制實驗為例，其測試如圖16所示。

為測定BDJP直線插補算法，用直線指令LINE，設：位置插值誤差為5 mm，旋轉(zhuǎn)插值誤差為1°，編寫指令文件，完成服務機械臂走“中”字的實驗，如圖17所示。

為驗證BDJP圓弧插補算法，用圓弧指令CIR3P指令，設：位置偏差為5 mm，旋轉(zhuǎn)偏差為1°，編寫指令文件，完成懸掛六自由度服務機械臂走圓實驗，如圖18所示。

經(jīng)過多次實驗，算法的穩(wěn)定性、可靠性得到了檢驗，平臺的功能和性能也得到了完善和提高，驗證了仿真控制平臺在模塊化服務機器人的模型構(gòu)建、仿真控制、在線同步控制以及軌跡規(guī)劃、指令控制等方面的功能。

▲圖16 豎直六自由度機械臂同步在線測試

5 結(jié)論

本文以Schunk模塊化服務機器人為研究對象，基于BDJP插值算法完成了服務機器人的軌跡規(guī)劃工作，完成模塊化服務機器人仿真控制平臺的搭建。在該仿真平臺上，完成了在線同步測試、懸掛六自由度服務機械臂走 “中”字和走圓實驗，驗證了平臺的功能和性能。

作為模塊化機器人仿真控制平臺，本文的研究工作在實時性、場景精細等方面還存在較多不足，整個仿真控制軟件主要從服務機械臂的建模和運動控制出發(fā)，未考慮服務機械臂的工作環(huán)境、手臂受

力情況等因素，下一步將考慮把增加服務機械臂作業(yè)環(huán)境的建模作為后續(xù)研究內(nèi)容，并考慮把實現(xiàn)服務機械臂運動過程中各模塊受力情況的仿真分析作為研究重點。另外，設計的服務機械臂運動學求解器還具有一定的局限性，不能夠適應建立的所有模型。下一步將考慮從服務機械臂機構(gòu)學的角度分析其運動學求解過程，建立具有普遍適應性的統(tǒng)一運動學求解算法。最后，設計的軌跡規(guī)劃器實現(xiàn)的軌跡規(guī)劃算法還非常有限，下一步將考慮研究具有避障、指定任務軌跡自動生成等更加高級的軌跡規(guī)劃算法。

▲圖17 走"中"字實驗

▲圖18 走圓實驗

［1］姚玉峰，蘇衍宇，黃博.面向服務機械臂有效操作特征提取的示教方法［J］.機械工程學報，2013，49（3）：10-17.

［2］高美真.OpenGL在基于圖形建模技術(shù)中的應用［J］.現(xiàn)代計算機，2005（12）：92-94.

［3］伍文偉，朱志杰，伍良偉.一種基于Lab Windows/CVI和OpenGL的六自由度機械手三維建模的方法［J］.電子測量技術(shù)，2007，30（8）：75-77.

［4］李憲華，郭永存，郭帥，等.基于CAN總線結(jié)構(gòu)的服務機器人模塊化雙臂系統(tǒng)構(gòu)建 ［J］.機床與液壓，2013，41（17）：22-23.

［5］趙建文，杜志江，孫立寧. 7自由度冗余手臂的自運動流形［J］.機械工程學報，2007，43（9）：132-137.

［6］封文春，林貴平.四元數(shù)在彈射座椅性能仿真中的應用［J］.北京航空航天大學學報，2006，32（8）：881-884.

［7］李憲華，郭永存，張軍，等.模塊化六自由度機械臂逆運動學解算與驗證［J］.農(nóng)業(yè)機械學報，2013，44（4）：246-251.

［8］孫亮，馬江，阮曉鋼.六自由度機械臂軌跡規(guī)劃與仿真研究［J］.控制工程，2010，17（3）：388-392.

［9］熊有倫.機器人技術(shù)基礎［M］.武漢：華中科技大學出版社，2008.

［10］張小江，高秀華.三次樣條插值在機器人軌跡規(guī)劃應用中的改進研究［J］.機械設計與制造，2012（9）：170-171.

［11］劉鵬，宋濤，贠超，等.焊接機器人運動學分析及軌跡規(guī)劃研究［J］.機電工程，2013，30（4）：390-394.

［12］Kubota N，Arakawa T.Trajectory Generation for Redundant Manipulator Using Virus Evolutionary Genetic Algorithm.［C］Robotics and Automation，1997 Proceedings，IEEE International Conference,Albuquerque USA，1997.

［13］劉鵬飛，楊孟興，宋科，等.‘S’型加減速曲線在機器人軌跡插補算法中的應用研究［J］.制造業(yè)自動化，2012，（20）：4-11.

［14］R H Taylor.The Planning and Execution of Straight Line Manipulator Trajectories［J］.IBM Journal of Research and Develapment，1979，23（4）：424-426.

［15］欒海英，劉學君.基于CANopen協(xié)議絕對值光電編碼器的伺服位置控制的研究［J］.制造業(yè)自動化，2013,35（9）：151-153.

［16］卓揚娃，白曉燦，陳永明.機器人的三種規(guī)則曲線插補算法［J］.裝備制造技術(shù)，2009（11）：27-29.

（編輯 丁 罡）

TH122；TP242.6

A

1000-4998（2015）10-0011-07

*上海市科學技術(shù)委員會科技支撐項目（編號：14111104502）

2015年3月