劉 宇，劉春時，張義民

（東北大學 機械工程與自動化學院，遼寧 沈陽 110819）

目前，工程機械機器人化是國內(nèi)外學者的一個研究熱點問題.其主要目的是使工程機械可以在危險、惡劣作業(yè)環(huán)境下工作，如開采輻射性礦物，高溫鍋爐除渣，清除山體滑坡等，以保障駕駛員健康、生命不受到威脅和損傷.液壓挖掘機是經(jīng)濟建設中最常用的工程機械之一，國內(nèi)外學者針對挖掘機的機器人化進行了多方面的研究［1－2］.

挖掘機的軌跡規(guī)劃是根據(jù)挖掘機作業(yè)任務設計合適的機械臂運行軌跡.往往作業(yè)任務只是一些關(guān)鍵點的數(shù)據(jù)，軌跡規(guī)劃則需要依據(jù)這些數(shù)據(jù)設計出合適的運行軌跡來.本文提出一種基于非均勻有理B樣條的軌跡規(guī)劃方法來設計運行軌跡.

1 挖掘機機構(gòu)簡圖

常用的反鏟式液壓挖掘機結(jié)構(gòu)如圖1所示，由行走裝置、回轉(zhuǎn)裝置、動臂、斗桿和鏟斗組成.每個桿件的運動是由液壓閥控制相應的液壓缸驅(qū)動桿件實現(xiàn)的.挖掘機機器人化是通過使用電液比例閥替換傳統(tǒng)的手動換向閥，實現(xiàn)挖掘機的計算機對挖掘機的控制.控制系統(tǒng)根據(jù)傳感器反饋信息對環(huán)境進行識別.作為挖掘機的“大腦”，智能控制模塊實現(xiàn)工作任務分解.一般控制系統(tǒng)分為上層規(guī)劃決策層和底層控制實現(xiàn)兩級.上層是一個人工智能系統(tǒng)，而底層則是一個位置控制系統(tǒng).

2 挖掘機的軌跡規(guī)劃

圖1挖掘機機構(gòu)簡圖Fig.1 Excavtor mechanism

圖2 挖掘機挖掘工作方式分類Fig.2 Working manner of excavtor

2.1 作業(yè)任務分解

反鏟挖掘機的一個作業(yè)循環(huán)包括挖掘、回轉(zhuǎn)、卸料、返回4個過程.挖掘的工作方式可分為溝端挖掘、溝側(cè)挖掘、直線挖掘、曲線挖掘、一定角度挖掘、超深溝挖掘、溝坡挖掘等［3］.可以將挖掘機的挖掘過程分為以下4類，如圖2所示.本文所指的軌跡規(guī)劃是根據(jù)作業(yè)任務設計出鏟斗尖處的軌跡.通過實時運動學解算后可以得到各個行走裝置，以及各個關(guān)節(jié)角度的大小.

2.2 NURBS曲線定義

初始化圖形交換規(guī)范（IGES）規(guī)定 NURBS表達為［4－5］

式中：C（u）為參數(shù)曲線；n為控制點個數(shù)減1；Ni，k（u）為由節(jié)點矢量U定義的k次B樣條基函數(shù)，u為［a，b］范圍內(nèi)的控制參數(shù)；ωi為權(quán)重；Pi為為控制點.

定義在節(jié)點矢量U上的k次基函數(shù)遞推定義為

式中：ui為節(jié)點.

因為3次NURBS曲線2次導數(shù)連續(xù)，軌跡的1，2階導數(shù)分別是速度和加速度.因此采用3次NURBS曲線來規(guī)劃挖掘機軌跡可以實現(xiàn)生成的指令速度和加速度連續(xù).

2.3 基于NURBS曲線的挖掘機軌跡規(guī)劃

如果按照圖2中箭頭指示的軌跡運行，存在以下問題：①軌跡不連續(xù)，存在速度、加速度突變；②沒有可調(diào)參數(shù)，不易進行軌量調(diào)節(jié)，不利于挖掘機機器人化的實現(xiàn).因此，擬利用NURBS曲線具有n－1次（n是曲線的次數(shù)）可導，而且每個控制點都具有權(quán)重調(diào)節(jié)因子的特性對挖掘機軌跡進行規(guī)劃.

根據(jù)作業(yè)要求進行軌跡規(guī)劃步驟如下：

（1）選擇控制點.一般選擇起點、終點作為首末控制點，然后選擇曲線關(guān)鍵點即可.

為了采用3次NURBS曲線規(guī)劃軌跡，對于圖2b中以深度h挖掘長度l可以直接選擇矩形的4個頂點作為控制點.圖2c和圖2d中以角度θ向下或向上挖掘長度l需要選擇增加一個重復控制點，該重復控制點與挖掘最低點重合.

（2）確定各點權(quán)重因子矢量.權(quán)重因子是相對而言的，一般設定起點和終點的權(quán)重因子為1，然后設定各控制點的權(quán)重因子.設定的依據(jù)是，要求軌跡盡量接近該控制點，則增大權(quán)重因子，反之，減小權(quán)重因子.

（3）確定節(jié)點矢量.節(jié)點矢量的確定可以按照式（3）進行［6］：

一般情況下，令a＝0，b＝1.這樣設定節(jié)點矢量的目的是保證生成的NURBS曲線通過首末控制點.

2.4 兩種離散軌跡的方法

2.4.1 等步長法

等步長參數(shù)是指當獲得了控制點、權(quán)重矢量、節(jié)點矢量之后，按照平均將參數(shù)u等分，以此方法生成參數(shù)序列，然后由式（1）求得軌跡點的方法.該方法的特點是運算簡單，適合由單片微處理器組成的控制系統(tǒng)進行處理，但是該方法生成的軌跡點在起始和終止段間距較大，生成的速度曲線和加速度曲線有波動.

2.4.2 一階泰勒近似方法

在進行NURBS曲線插補模塊之前，需要進行預運算，進行軌跡規(guī)劃.軌跡規(guī)劃包括求NURBS曲線的長度，根據(jù)指令速度進行速度規(guī)劃，常用的方法有梯形速度規(guī)劃、S型速度規(guī)劃等.NURBS曲線插補的算法流程圖如圖1所示.

不失一般性，參數(shù)曲線可以表示為

式中：u為樣條控制參數(shù)；i，j，k為三維方向矢量.

沿著曲線的進給速度為

可以得到

參數(shù)曲線C（u）是u的函數(shù)，參數(shù)u是時間t的函數(shù).由當前參數(shù)ui以及u對時間的一、二階導數(shù)和插補周期T，即可利用泰勒展開公式計算出下一個采樣點的參數(shù)ui＋1，代入式（1）得到下一插補點C（ui）.利用泰勒公式求參數(shù)為

式中：O（t3）為時間t的三次余項.

由式（6）和式（7）可得到一階、二階NURBS插補公式為［4］

3 完整控制結(jié)構(gòu)

挖掘機軌跡控制結(jié)構(gòu)分為上層規(guī)劃系統(tǒng)和底層控制系統(tǒng)，如圖3所示.上層控制系統(tǒng)是具有人工智能的任務規(guī)劃期，任務規(guī)劃完成后生成鏟斗目標軌跡曲線［7－10］.底層控制系統(tǒng)建立工作裝置的動力學模型，將挖掘機各組件和關(guān)節(jié)的運動與液壓系統(tǒng)的輸出力相結(jié)合，將軌跡指令轉(zhuǎn)化為驅(qū)動電液閥的電壓指令.

圖3 機器人化挖掘機控制結(jié)構(gòu)Fig.3 Control system structure of robotic excavtor

4 仿真與實驗研究

挖掘機機器人化控制系統(tǒng)采用工業(yè)控制機，通過USB口擴展CAN總線與外部設備相連.控制軟件采用VB6.0編寫.

以深度為400mm，挖掘長度為800mm的挖掘如圖4所示.挖掘軌跡是由關(guān)鍵點（400mm，0），（400mm，－300mm），（－400mm，－300mm），（－400mm，0）組成的控制多邊形內(nèi)部的光滑曲線組成.權(quán)重因子分別為1，10，10，1.

以角度為30°，長度為400mm的挖掘如圖5所示.關(guān)鍵點為（400mm，0），（－230mm，0），（－230mm，0），（0，0），權(quán)重因子ω設置不同值.

在挖掘機作業(yè)過程中，對于鏟斗的軌跡要求能夠進行調(diào)整.調(diào)整軌跡可以通過修改控制點的權(quán)重因子來實現(xiàn).圖5是關(guān)鍵點為（－230mm，0），權(quán)重因子分別為10，3，1時的軌跡曲線，可以看出，權(quán)重因子越大，軌跡距離控制點越近.

為了進行計算機控制，將圖4，5中的挖掘軌跡采用一階泰勒展開方法進行離散化，采樣周期為50ms，插補誤差控制為1mm，離散后的位移曲線如圖6所示，速度曲線如圖7所示.軌跡規(guī)劃結(jié)果與理想軌跡相比較，誤差如圖8所示，軌跡誤差很小，說明當采樣周期相對較小時，該軌跡規(guī)劃方法誤差可以控制得非常小.

圖4 挖掘?qū)嵗?Fig.4 Diging case 1

5 結(jié)論

（1）利用NURBS曲線將挖掘機作業(yè)任務進行軌跡規(guī)劃，具有軌跡連續(xù)，各關(guān)節(jié)無速度、加速度突變而且可以進行軌跡調(diào)節(jié)，適合挖掘機機器人化控制.

（2）通過調(diào)節(jié)關(guān)鍵點的權(quán)重因子可以改變挖掘軌跡.

（3）該軌跡規(guī)劃方法產(chǎn)生的誤差只有微米數(shù)量級.

［1］LEE S U，CHANG P H.Control of a heavy-duty robotic excavator using time delay control with integral sliding surface［J］.Control Engineering Practice，2002，10（7）：697－711.

［2］張海濤，何清華，張新海，等.機器人液壓挖掘機運動系統(tǒng)的建模與控制［J］.機器人，2005，27（2）：113－117.

ZHANG Haitao，HE Qinghua，ZHANG Xinhai，et al.Modeling and control of the motion system of a hydraulic robotic excavator［J］.Robot，2005，27（2）：113－117.

［3］SHI Xiaobo，WANG Feiyue，LEVER P J A.Experimental results of robotic excavation using fuzzy behavior control［J］.Control Engineering Practice，1996，4（2）：145－152.

［4］劉宇，趙波，戴麗，等.基于傳動系統(tǒng)動力學的 NURBS曲線插補算法研究［J］.機械工程學報，2009，22（1）：181－192.

LIU Yu，ZHAO Bo，DAI Li，et al.Study on a feedrate system dynamics based interpolator for nurbs curve［J］.Chinese Journal of Mechanical Engineering，2009，22（1）：181－192.

［5］劉宇，戴麗，劉杰，等.泰勒展開 NURBS曲線插補算法研究［J］.東北大學學報：自然科學版，2009，29（1）：117－120.

LIU Yu，DAI Li，LIU jie，et al.Study on nurbs interpolator using taylor expansion method［J］.Jonrnal of Northeastern University ：Natural Science，2009，29（1）：117－120.

［6］PIEGL L，TILLER W.The NURBS book［M］.Berlin：Springer，1997.

［7］TOMI M，KELERVO N，RAUNO H.A 3Dmodel based control of an excavator［J］.Automation in Space Research，2006，15（5）：571－577.

［8］HA Q P，NAGUYEN Q H，RYE D C，et al.Impedance control of a hydraulically actuated robotic excavator［J］.Automation in Construction，2000，9（5－6）：421－435.

［9］FOX B，JENNINGS L S，ZOMAYA A Y.On the modeling of actuator dynamics and the computation of prescribed trajectories［J］.Computers ＆Structure，2002，80（7－8）：605－614.

［10］BRADLEY D A，SEWARD D W，MANN J E，et al.Artificial intelligence in the control and operation of construction plant—the autonomous robot excavator［J］.Automation in Construction，1993，2（3）：217－228.