自主挖掘的平滑連續特性軌跡規劃研究

李海虹,林貞國

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

?

自主挖掘的平滑連續特性軌跡規劃研究

李海虹,林貞國

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

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

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

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

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

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

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坐標系下的工作裝置

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

2.2 運動學的逆解

挖掘任務中挖掘控制點是在笛卡爾坐標系中描述的,而軌跡規劃是在關節空間中進行的,所以必須將挖掘控制點的位姿坐標轉換為關節空間各關節的轉角,通過運動學的逆解來實現這一過程[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 笛卡爾坐標系下的空間挖掘路徑規劃

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

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

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

4 關節空間自主挖掘過程的軌跡規劃

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

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

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

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

4.1n次多項式擬合法的軌跡規劃

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

(3)

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

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

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

(4)

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

(5)

(6)

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

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

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

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

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

4.2 傅里葉級數擬合法軌跡規劃

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

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

(7)

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

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

(8)

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

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

(9)

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

(10)

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

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

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

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

4.3 分段多項式插值法軌跡規劃

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

設挖掘機任一關節的軌跡多項式為

(11)

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

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

(12)

(13)

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

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

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

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

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

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

5 結語

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

表2 不同軌跡規劃方法的動態特性指標對比

[1] 徐向榮,馬香峰.機器人運動軌跡規劃分析與算法[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] 劉涼,陳超英.考慮關節摩擦的并聯機器人平滑軌跡規劃[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].農業機械學報,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] 朱世強,劉松國.機械手時間最優脈動連續軌跡規劃算法[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] 趙云.電動式挖掘機動力學與軌跡規劃理論的研究[D].沈陽:東北大學,2007.

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

[6] 管成,張登雨.基于NURBS的挖掘機器人時間最優軌跡規劃[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] 邵輝,胡偉石.自動挖掘機的動作規劃[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] 陳小立,嚴宏志.基于遺傳算法的四自由度混聯機器人軌跡規劃[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