基于NURBS的挖掘機(jī)自主控制鏟斗軌跡規(guī)劃方法

劉 宇，劉春時(shí)，張義民

（東北大學(xué) 機(jī)械工程與自動(dòng)化學(xué)院，遼寧 沈陽 110819）

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

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

1 挖掘機(jī)機(jī)構(gòu)簡(jiǎn)圖

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

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

圖1挖掘機(jī)機(jī)構(gòu)簡(jiǎn)圖Fig.1 Excavtor mechanism

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

2.1 作業(yè)任務(wù)分解

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

2.2 NURBS曲線定義

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

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

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

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

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

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

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

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

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

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

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

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

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

2.4 兩種離散軌跡的方法

2.4.1 等步長法

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

2.4.2 一階泰勒近似方法

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

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

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

沿著曲線的進(jìn)給速度為

可以得到

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

式中：O（t3）為時(shí)間t的三次余項(xiàng).

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

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

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

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

4 仿真與實(shí)驗(yàn)研究

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

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

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

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

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

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

5 結(jié)論

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

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

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

［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］張海濤，何清華，張新海，等.機(jī)器人液壓挖掘機(jī)運(yùn)動(dòng)系統(tǒng)的建模與控制［J］.機(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］劉宇，趙波，戴麗，等.基于傳動(dòng)系統(tǒng)動(dòng)力學(xué)的 NURBS曲線插補(bǔ)算法研究［J］.機(jī)械工程學(xué)報(bào)，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曲線插補(bǔ)算法研究［J］.東北大學(xué)學(xué)報(bào)：自然科學(xué)版，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.