李海虹,林貞國

(太原科技大學 機械工程學院,山西 太原 030024)

?

自主挖掘的平滑連續(xù)特性軌跡規(guī)劃研究

李海虹,林貞國

(太原科技大學 機械工程學院,山西 太原 030024)

為保證自主挖掘過程的平穩(wěn)連續(xù),通過對比選取了一種合理的軌跡規(guī)劃方法.以某型挖掘機為研究對象,在笛卡爾坐標空間對一次作業(yè)路徑進行規(guī)劃.在關節(jié)空間對多個路徑控制點在不同條件下分別采用n次多項式、傅里葉級數擬合法、分段多項式插值法進行軌跡規(guī)劃;通過MATLAB仿真平臺得到的各關節(jié)的關節(jié)角、角速度和角加速度的變化曲線是平滑連續(xù)的;對于函數擬合法與分段多項式插值法進行了動力學特性比較,結果表明,函數擬合法的軌跡規(guī)劃更加合理,它可以使機構工作更加平穩(wěn)連續(xù),對機構的振動沖擊強度也較低,能很好地滿足實際工作中自主挖掘的要求.

挖掘機; 軌跡規(guī)劃;n次多項式擬合; 傅里葉級數擬合; 分段多項式插值

挖掘機自主作業(yè)應用非常廣泛,如核輻射區(qū)域、深海采集地,實現挖掘機自動化作業(yè)是非常必要的,因此必須解決挖掘機的軌跡規(guī)劃問題.

國內外學者提出了多種軌跡規(guī)劃的方法,KIM 等提出一種時間最短軌跡規(guī)劃方法,這種方法是基于關節(jié)空間的,并考慮了包括動力學約束在內的各種實際約束條件,但這種方法較復雜,且只能離線完成;后來LIN 等提出了規(guī)劃機器人連續(xù)運動軌跡的3次樣條函數方法,它可以實現優(yōu)化的關節(jié)規(guī)律,但當路徑點多時,需要已知條件過多,計算量非常大;徐向榮等提出了多種不同條件下的3-5-3,4-3-4,3-3-3等多項式軌跡規(guī)方法[1],但是前兩類由于分段過少,容易使得規(guī)劃的軌跡曲線偏差較大,第3種分段方式,規(guī)劃的軌跡不具有對稱性,軌跡末端階躍較大;劉涼等采用5次多項式插值法,盡管能夠保證加速度連續(xù),但是限制條件較多,包括中間路徑點的速度和加速度都必須已知[2-3];朱世強等提出的7次樣條曲線插值方法,能夠避免振動問題,但是7次樣條曲線計算復雜,效率比較低[4];趙云等對于電動式挖掘機采用對數螺旋線理論進行挖掘軌跡的研究[5];管成等提出了5次NURBS 曲線時間最優(yōu)軌跡規(guī)劃[6],它使得關節(jié)曲線的加速度連續(xù),并且通過權因子調整挖掘軌跡.但是NURBS曲線的求導計算式比較繁瑣,編程復雜[7].

本文以某型挖掘機為研究對象,規(guī)劃了一次挖掘任務的空間挖掘軌跡,在關節(jié)空間對自主挖掘過程分別進行了5次多項式、2階傅里葉級數擬合和分段多項式插值,并對軌跡規(guī)劃結果中的動力特性進行了比較,選擇綜合性能較優(yōu)的軌跡規(guī)劃方法.

1 挖掘機工作裝置

挖掘機在工作過程中,回轉平臺、動臂、斗桿和鏟斗的3個油缸進行復合運動,使鏟斗末端沿著預定的路徑運動,完成其工作任務.挖掘機的工作裝置如圖1所示.其工作機構中共有11個鉸點,即A,B,C,D,E,F,G,H,K,N,Q.

圖1 挖掘機工作裝置圖Fig.1 Excavator working device

2 D-H坐標系下工作裝置的運動學分析

2.1D-H坐標系下的工作裝置

在挖掘機的軌跡規(guī)劃中,為求運動學逆解,依據機器人學理論,在D-H坐標系下建立挖掘機工作裝置[8],如圖2所示.其中θ1為回轉關節(jié)變量;θ2為動臂關節(jié)變量;θ3為斗桿關節(jié)變量;θ4為鏟斗關節(jié)變量.

2.2 運動學的逆解

挖掘任務中挖掘控制點是在笛卡爾坐標系中描述的,而軌跡規(guī)劃是在關節(jié)空間中進行的,所以必須將挖掘控制點的位姿坐標轉換為關節(jié)空間各關節(jié)的轉角,通過運動學的逆解來實現這一過程[8].

令鏟斗末端V點的坐標為(x,y,z),Q點的坐標為(xq,yq,zq),α,β,γ分別表示CF,CQ和CV與水平面的夾角,則有

圖2 D-H坐標系下挖掘機工作裝置簡圖Fig.2 D-H coordinate system of excavator workingdevice

(1)

式中:a1為回轉中心與動力臂鉸點之間的距離;a2為動臂連桿長度;a3為斗桿連桿長度；d1為底座回轉平臺中心的距離；ζ=θ2+θ3+θ4.

(2)

式中:a4為鏟斗長度.

3 笛卡爾坐標系下的空間挖掘路徑規(guī)劃

挖掘機通過鏟斗鏟入土壤、拖拽和鏟斗回旋出土3個步驟自主完成梯形深坑挖掘任務[9].在這3個步驟中,由3個關節(jié)角的復合運動來完成挖掘動作:由動臂關節(jié)運動來完成提升過程;接著回轉裝置旋轉到指定卸載位置,通過鏟斗關節(jié)變化來完成卸載;然后由3個關節(jié)的復合運動到達下一個指定挖掘點,再繼續(xù)作業(yè).

挖掘機完成一次挖掘任務的空間路徑控制點坐標與挖掘機的工作參數有關[8],本文中規(guī)劃的挖掘控制點坐標如表1所示.通過空間樣條曲線插值完成一次挖掘作業(yè)的空間軌跡,如圖3所示.

圖3 挖掘機空間挖掘軌跡曲線Fig.3 Excavator space trajectory curve

4 關節(jié)空間自主挖掘過程的軌跡規(guī)劃

挖掘機工作過程中,鏟斗的運動(提升、回轉、以及運行至下一起始點)屬于空間點到點的運動,依據初始位置、速度、加速度等已知條件及連續(xù)性條件,采用5次多項式插值法規(guī)劃軌跡,這樣比較簡單.挖掘軌跡規(guī)劃屬于連續(xù)路徑規(guī)劃,由起始點、終止點以及挖掘中間控制點組成挖掘路徑,將該路徑軌跡分為5段:鏟入、回旋、拖拽、回旋和鏟出[11].設y=0,則xz平面內自主挖掘過程的軌跡曲線如圖4所示.

圖4 xz平面內自主挖掘過程的軌跡曲線Fig.4 Process of self excavation trajectory curve within xz plan

在關節(jié)空間,多路徑點處的關節(jié)角隨著工作時間的變化形成一個關節(jié)序列,此關節(jié)序列通過多項式函數擬合成一條光滑的函數,得到挖掘機的軌跡;或者在每兩個路徑點之間采用多項式插值法,通過求1階導數及2階導數獲得速度、加速度特性,得到軌跡規(guī)劃的結果.在本文中多路徑點的軌跡需根據路徑中控制點的點數來選取合理的多項式形式,進行擬合與插值.

表1 控制點從位姿空間到關節(jié)空間的轉換

4.1n次多項式擬合法的軌跡規(guī)劃

由于高階多項式及其1階、2階導數平滑連續(xù)的特性,對于過多路徑點的軌跡規(guī)劃,采用多項式來進行擬合,設多項式通式為

(3)

式中:p(t)為隨時時間t變化的關節(jié)變量；n為路徑點數p(t)為隨時時間t變化的關節(jié)變量.

本文中n=6,則挖掘機各關節(jié)點的多項式為

pi(t)=p5it5+p4it4+p3it3+p2it2+p1it1+p0i

(4)

其中i=1,2,3.通過對光滑多項式分別求1階、2階導數,得到各關節(jié)的速度、加速度公式為

(5)

(6)

式中:v(t),a(t)分別為關節(jié)的速度、加速度函數.

通過各路徑點的時間與表1中各關節(jié)點處的關節(jié)角,求得多項式的各項系數,從而得到關節(jié)角隨時間變化的擬合光滑函數.通過MATLAB仿真得到各關節(jié)的軌跡規(guī)劃結果,如圖5—7 所示.

圖5 動臂關節(jié)角、角速度、角加速度曲線圖Fig.5 Boom joint angle,angular velocity,angular acceleration curve

圖6 斗桿關節(jié)角、角速度、角加速度曲線圖Fig.6 Arm joint angle,angular velocity,angular acceleration curve

圖7 鏟斗關節(jié)角、角速度、角加速度曲線圖Fig.7 Bucket joint angle,angular velocity,angular acceleration curve

4.2 傅里葉級數擬合法軌跡規(guī)劃

傅里葉變換能將滿足一定條件的某個函數表示成三角函數(正弦函數或余弦函數)或者它們的積分的線性組合.在不同的研究領域,傅里葉變換具有多種不同的變體形式,如連續(xù)傅里葉變換和離散傅里葉變換.傅里葉變換的基本性質決定了它是一種很好的函數擬合工具.

當通過路徑點數為偶數點時,我們選取傅里葉函數在關節(jié)空間進行擬合光滑的軌跡曲線,設其通式為

(7)

式中:ai,bi為函數f(t)傅里葉系數;w為角頻率；n=2m,m≥2;n為路徑點數.

本文中n=6,所以其傅里葉擬合多項式為

(8)

分別對多項式求1階、2階導數得到各關節(jié)的速度、加速度公式:

2a2wsin(2wt)+ 2b2wcos(2wt)

(9)

4a2w2sin(2wt)-2b2w2sin(2wt)

(10)

通過各路徑點的時間與表1中各關節(jié)點處的關節(jié)角,求得級數的各個未知系數,從而擬合求得關節(jié)角隨時間變化的光滑函數.通過MATLAB仿真得到各關節(jié)的軌跡規(guī)劃結果,如圖8—10 所示.

圖8 動臂關節(jié)角、角速度、角加速度曲線圖Fig.8 Boom joint angle,angular velocity, angularaccelerationcurve

圖9 斗桿關節(jié)角、角速度、角加速度曲線圖Fig.9 Arm joint angle,angular velocity,angular acceleration curve

圖10 鏟斗關節(jié)角、角速度、角加速度曲線圖Fig.10 Bucket joint angle,angular velocity,angular acceleration curve

4.3 分段多項式插值法軌跡規(guī)劃

對于過多路徑點的工作過程,采用合理分段點的軌跡規(guī)劃方式,使得實際工作路徑與規(guī)劃路徑偏差較小,軌跡、速度、加速度曲線平滑連續(xù).本文采用了5段3-3-3-3-3高階多項式進行軌跡規(guī)劃.

設挖掘機任一關節(jié)的軌跡多項式為

(11)

其中ai0,ai1，ai2，ai3(i=1,2,3,4,5)表示挖掘任一關節(jié)在第i段路徑的軌跡多項式是系數.

對軌跡多項式中的實際時間變量τ求1階和2階導數,得到各關節(jié)速度和加速度為

(12)

(13)

通過各關節(jié)在路徑點處的關節(jié)角度值、初始速度、加速度等已知條件,以及位置、速度、加速度連續(xù)性條件,可以求得多項式的每個系數.通過MATLAB仿真軟件得到動臂、斗桿、鏟斗各關節(jié)的軌跡規(guī)劃結果,如圖11—13所示.

圖11 動臂關節(jié)角、角速度、角加速度曲線圖Fig.11 Boom joint angle,angular velocity,angular acceleration curve

圖12 斗桿關節(jié)角、角速度、角加速度曲線圖Fig.12 Arm joint angle,angular velocity,angular acceleration curve

圖13 鏟斗關節(jié)角、角速度、角加速度曲線圖Fig.13 Bucket joint angle,angular velocity,angular acceleration curve

將以上3種軌跡規(guī)劃結果中(圖5,8,11;圖6,9,12;圖7,10,13)的各關節(jié)軌跡規(guī)劃結果進行對比,得到的結果如表2所示.

從表2中數據對比可知,用傅里葉擬合與用5次多項式擬合的各關節(jié)角變化曲線平緩,階躍比較小,而用3-3-3-3-3分段插值法規(guī)劃的軌跡,各關節(jié)角的變化曲線變化范圍比較大,階躍比較大,因此5次多項式擬合法與傅里葉擬合法的軌跡規(guī)劃的效果明顯優(yōu)于分段多項式插值法,因此用此法規(guī)劃挖掘機的運動軌跡,可使挖掘機關節(jié)變化平穩(wěn)連續(xù),各工作裝置所受的振動沖擊較小,可以很好地實現自主挖掘功能.

5 結語

本文結合挖掘機實際工作參數,通過3次樣條函數插值法在笛卡爾坐標系得到挖掘機空間挖掘的軌跡曲線,在關節(jié)空間對自主挖掘工作過程分別采用n次多項式、傅里葉級數擬合法和分段多項式插值法進行軌跡規(guī)劃,通過對規(guī)劃結果的動力學特性指標進行比較,得出n次多項式、傅里葉級數擬合法軌跡規(guī)劃比分段多項式插值法效果更好,因此用此法規(guī)劃挖掘機的運動軌跡,可使挖掘機關節(jié)變化平緩連續(xù),各工作裝置所受的振動沖擊較小.另外,n次多項式適用于任何控制點數,而傅里葉級數則只適用于控制點數為偶數時的情況,所以必須根據實際情況來選用這兩種方法.

表2 不同軌跡規(guī)劃方法的動態(tài)特性指標對比

[1] 徐向榮,馬香峰.機器人運動軌跡規(guī)劃分析與算法[J].機器人,1988,2(6):18-24.

XU Xiangrong,MA Xiangfeng.Analysis and alorithm for trajectory planning of robot manipulators [J]Robotics,1988,2(6):18-24.

[2] 劉涼,陳超英.考慮關節(jié)摩擦的并聯(lián)機器人平滑軌跡規(guī)劃[J].機械工程學報,2014,50(19):9-15.

LIU Liang,CHEN Chaoying.Smooth trajectory planning for parallel manipulator with joint friction torque[J].Journal of Mechanical Engineering,2014,50(19):9-15.

[3] 黎波,嚴駿,郭剛,等.挖掘機工作裝置復雜軌跡跟蹤實驗[J].農業(yè)機械學報,2012,43(12):39-43.

LI Bo,YAN Jun,GUO Gang,et al.Experiment of complex path tracking for excavator working unit [J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(12):39-43.

[4] 朱世強,劉松國.機械手時間最優(yōu)脈動連續(xù)軌跡規(guī)劃算法[J].機械工程學報,2010,46(3):47-52.

ZHU Shiqiang,LIU Songguo.Time-optimal and Jerk-continuous trajectory planning algorithm for Manipulators[J].Journal of Mechanical Engineering,2010,46(3):47-52.

[5] 趙云.電動式挖掘機動力學與軌跡規(guī)劃理論的研究[D].沈陽:東北大學,2007.

ZHAO Yun.Invefigation of dynamics and trck design for eletronic excator[D].Shenyang:Northeastern Universty,2007.

[6] 管成,張登雨.基于NURBS的挖掘機器人時間最優(yōu)軌跡規(guī)劃[J].吉林大學學報:工學版,2015,45(2):540-546.

GUAN Cheng,ZHANG Dengyu.NURBS-basedtime-optimal trajectory planning on robotic excavators[J].Journal of Jilin University：Engineering and Technology,2015,45(2):540-546.

[7] SARAVANAN R,RAMABALAN S,BALAMURUGAN C.Multiobjective trajectory planner for industrial robots with payload constraints[J].Robotica,2008,26(6):753-765.

[8] 宋軍,賀繼林.液壓挖掘機自主挖掘技術研究[D].長沙:中南大學,2013.

SONG Jun,HE Jilin.Self-mining technology of hydraulic excavator[D].Changsha:Central South University,2013.

[9] 邵輝,胡偉石.自動挖掘機的動作規(guī)劃[J].控制工程.2012,19(2):594-597

SHAO Hui,HU Weishi.Action planning of automatic excavator[J].Control Engineering of China,2012,19(2):594-597.

[10] 陳小立,嚴宏志.基于遺傳算法的四自由度混聯(lián)機器人軌跡規(guī)劃[J]計算機仿真,2014,31(5)346-350.

CHEN Xiaoli,YAN Hongzhi.Trajectory planning of four DOF hybrid robot based on genetic algorithm [J].Computer Simulation,2014,31(5):346-350.

Smooth continuous track planning on autonomous mining

LI Hai-hong,LIN Zhen-guo

(School of Mechanical Engineering ,Taiyuan University of Science and Technology,Taiyuan 030024 ,China)

In order to ensure the smoothness and continuity of autonomous mining,a rational track planning method is comparatively selected for a specific excavator.By planning a job path in Cartesian coordinate space,the track planning is conducted on joint space via n-degree polynomial,Fourier series fitting and piecewise polynomial interpolation under different path control points and conditions.To this end,the curves of joint angle,angular velocity and acceleration are smooth and continuous.In comparison with dynamic properties of functional fitting and piecewise polynomial interpolation,it is found that the mechanism via functional fitting works more smoothly and continuously with lower impact strength so as to meet autonomous mining demands.

excavator; track planning; n-degree polynomial fitting; fourier fitting; piecewise polynomial

國家自然科學基金資助項目(51541501);山西省青年科技研究基金(2010021021-2)

李海虹，副教授，博士. E-mail:lihaihongty@sina.com

TP 29

A

1672-5581(2016)02-0093-06