基于向量的步履式挖掘機姿態(tài)調(diào)平規(guī)劃

劉子超

(中國兵器裝備集團自動化研究所武器裝備信息與控制技術(shù)研發(fā)中心， 四川 綿陽 621000)

【信息科學與控制工程】

基于向量的步履式挖掘機姿態(tài)調(diào)平規(guī)劃

劉子超

(中國兵器裝備集團自動化研究所武器裝備信息與控制技術(shù)研發(fā)中心， 四川 綿陽 621000)

針對步履式挖掘機自動控制的需求，設(shè)計了一種步履式挖掘機姿態(tài)調(diào)平規(guī)劃方法，使用向量描述了步履式挖掘機支腿與底座之間的角約束關(guān)系，使用向量長度以及各向量表示的支腿與底座的鉸接關(guān)系確定姿態(tài)調(diào)平的目標位置，解算結(jié)果能夠滿足地面對輪的約束；通過仿真驗證該方法能夠?qū)δ繕宋恢眠M行解算，且在各個支腿起始端均位于底座上的同時，末端位置共面。

步履式挖掘機；姿態(tài)調(diào)節(jié)；運動學逆解

步履式挖掘機是一種適用于特殊地形的工程機械，工作裝置與傳統(tǒng)履帶式挖掘機大致相同，主要區(qū)別在于底盤行走部分克服了傳統(tǒng)履帶式挖掘機不能在復雜地形工作的特點，利用獨立多自由度轉(zhuǎn)向液壓支腿進行自由角度調(diào)整，補償?shù)匦瓮拱几叨炔睿瑢崿F(xiàn)在大角度傾斜地形行駛、行走、施工[1]。

步履式挖掘機行走裝置結(jié)構(gòu)上與輪腿式機器人類似，其各個支腿對車身姿態(tài)的影響受其他支腿的當前狀態(tài)影響[2]，為了實現(xiàn)步履式挖掘機姿態(tài)的智能調(diào)平，本文通過姿態(tài)傳感器獲取當前位置，運用向量針對其底盤進行運動學逆解，根據(jù)當前位置規(guī)劃各關(guān)節(jié)的目標位置，為人工操作提供指導或?qū)崿F(xiàn)姿態(tài)調(diào)平的自動規(guī)劃。

1 步履式挖掘機行走底盤建模

1.1 步履式挖掘機行走裝置簡介

為使步履式挖掘機適應(yīng)非結(jié)構(gòu)化地形工況，底座周圍設(shè)置4條支腿，并通過8個液壓缸對支腿關(guān)節(jié)獨立驅(qū)動，使其成為既能上下俯仰又能左右擺動的多自由度空間運動機構(gòu)，從而調(diào)平車體姿態(tài)，適應(yīng)各種復雜地形。小輪支腿安裝有支爪，在工作時展開，用于承受挖掘反力，保持車身穩(wěn)定；在非工作狀態(tài)下，步履式挖掘機通過四個車輪內(nèi)安裝的液壓馬達接受發(fā)動機的動力，驅(qū)動整機行進，大輪設(shè)有轉(zhuǎn)向液壓缸用于轉(zhuǎn)向[3]。步履式行走底盤結(jié)構(gòu)如圖1所示。

1.支爪; 2.前伸縮支腿; 3.前車輪; 4.前支腿俯仰液壓缸; 5.前支腿鉸接體; 6.底座; 7.后支腿俯仰液壓缸; 8.后支腿鉸接體; 9.后支腿; 10.后車輪; 11.后車輪驅(qū)動馬達; 12.后支腿擺動液壓缸; 13.后車輪轉(zhuǎn)向液壓缸; 14.前支腿擺動液壓缸; 15.前支腿套筒

圖1 行走底盤結(jié)構(gòu)示意圖

一般在使用人力操縱或遙控操作時，姿態(tài)調(diào)平僅使用4個升降油缸，待調(diào)平結(jié)束后，伸縮油缸驅(qū)動支爪插入地面，挖掘機即可正常作業(yè)[4]。

1.2 步履式挖掘機行走底盤向量表示方法

1987年就有學者提出使用向量進行串聯(lián)機構(gòu)的運動學正解[5]。步履式挖掘機行走裝置為并聯(lián)機構(gòu)，直接使用三角函數(shù)計算較為復雜，本文采用向量關(guān)系表示關(guān)節(jié)角。在使用向量進行運動學解算時，首先構(gòu)建適當?shù)淖鴺讼?，確定表示底盤的向量組，再對向量組進行運算后。從原點出發(fā)，沿向量間的關(guān)系逐一確定向量所表示的邊在空間中的位置，完成運動學逆解。

本文使用歐拉角方位表示法[6]。首先將坐標系原點固定于回轉(zhuǎn)中心，以車輛行駛方向為x軸， 小輪側(cè)為正半軸；以回轉(zhuǎn)平臺平面法向量為z軸方向，回轉(zhuǎn)平臺上方為正半軸；以xoz平面法向量為y軸方向，右邊為y軸正半軸。當車體姿態(tài)調(diào)平時，該坐標系不隨車體運動。各坐標值以毫米為單位，后文不再標注。

將底座視為一個固定的等腰梯形，頂點為四個升降機構(gòu)鉸接點。該等腰梯形不會隨支腿的運動變化，可通過事先測量確定[7]。輪支腿的末端為大、小輪的軸，軸與地平面的間距為定值，即輪的半徑。在建模時，將輪軸中心視作整條輪腿末端。

建模后的向量組如圖2所示。圖2中l(wèi)表示左，r表示右，f表示前，b表示右，下標0表示支腿末端，下標1表示支腿始端與回轉(zhuǎn)平臺鉸接處。回轉(zhuǎn)平臺的俯仰角θf為向量k與其水平面投影之間的夾角，橫滾角θh為向量f或向量b與其水平面投影之間的夾角。

圖2 行走底盤向量組表示

2 姿態(tài)調(diào)平解算

在姿態(tài)調(diào)平前，車體一般處于行走狀態(tài)，在該狀態(tài)下回轉(zhuǎn)平臺與地面平行，無需使用正解求解回轉(zhuǎn)平臺與地面的夾角。因為姿態(tài)調(diào)平需要的輸入量為車體與水平面夾角，即俯仰角、橫滾角，調(diào)平前先假定使用姿態(tài)角傳感器已經(jīng)獲知了當前位姿。

各個支腿僅升降機構(gòu)動作，理論上支腿末端所能到達的全部位置構(gòu)成一段以其鉸接點為圓心、以支腿在xoz平面投影的長度為半徑的圓弧[8]。為了使姿態(tài)調(diào)平算法具有最大的調(diào)節(jié)量，首先確定最高側(cè)腿的位置，應(yīng)當與回轉(zhuǎn)平臺位于同一平面內(nèi)。最高側(cè)腿由俯仰角與橫滾角值的決定，如表1所示。

表1 最高側(cè)腿與各傾角關(guān)系

假設(shè)當前俯仰角θf與橫滾角θh均小于0，最高側(cè)腿則為右后腿。首先根據(jù)某型步履式挖掘機結(jié)構(gòu)數(shù)據(jù)[9]設(shè)定初始狀態(tài)下的已知向量l、r、f、b、rb:

(1)

先計算俯仰角調(diào)節(jié)至0后各向量的方向，即各向量繞y軸旋轉(zhuǎn)θf。根據(jù)羅德里格旋轉(zhuǎn)公式[10]：

vr=cosφv+(1-cosφ)(v·k)k+sinφk×v=Rv

(2)

將已知的5個向量分別旋轉(zhuǎn)，記為lf、rf、ff、bf、rbf，其中，旋轉(zhuǎn)矩陣R如式(3)所示：

(3)

經(jīng)過上述旋轉(zhuǎn)變換后回轉(zhuǎn)平臺與水平面的幾何關(guān)系改變，在調(diào)橫滾角時需要先對橫滾角進行修正。使用式(4)修正θh：

(4)

由于坐標系固定，而回轉(zhuǎn)平臺與xoy平面不再平行，旋轉(zhuǎn)軸應(yīng)當選取回轉(zhuǎn)平臺上與x軸方向共面的中軸線，定義單位向量k為旋轉(zhuǎn)軸：

(5)

令k=(xk,yk,zk)T,則旋轉(zhuǎn)矩陣R如式(6)所示：

(6)

將旋轉(zhuǎn)后的各個向量記為lfh、rfh、ffh、bfh、rbfh，姿態(tài)調(diào)平時最高側(cè)腿無升降運動， 即輪腿末端位置在調(diào)平過程中固定，根據(jù)實際尺寸可測得右后輪軸在坐標系中的坐標如式(7)所示：

rb0=(4, -1 710, -2 160)

(7)

回轉(zhuǎn)平臺4個頂點坐標可通過式(8)求得：

(8)

各輪均位于地面，則各支腿末端距地面高度為輪的半徑。由于xoy平面與地面平行，在姿態(tài)調(diào)平后各點z坐標保持不變。以lf0點為例，其z坐標為-121，理想圓弧所處的平面記為α，其法向量為ffh,平面α與對應(yīng)的支腿鉸接點間距為初始狀態(tài)下各支腿向量的y坐標，在擺動機構(gòu)擺至與行駛方向平行時，該值為619。在平面α上可確定一點tlf，該點坐標如式(9)所示：

(9)

將平面α表示為解析形式，如式(10)所示：

Ax+By+Cz+D=0

(10)

根據(jù)法向量的性質(zhì)，可令(A,B,C)T=ffh。將tlf代入式(10)，可解得

D=-tlf·ffh

(11)

令lf0=(xlf0,ylfo,zlf0),lf1=(xlf1,ylf1,zlf1)，則兩點間距滿足式(12)：

|lf1-lf0|=|lf|

(12)

經(jīng)實測得|lf|=1 255,代入zlf0=-121,可得

(13)

在方程組(13)中僅有兩個未知數(shù)xlf0、ylf0，若該方程組有解，即可確定lf0點在坐標系中的位置。同法可解得rf0、lb0。

姿態(tài)調(diào)平規(guī)劃的結(jié)果為各升降油缸的伸縮量，所以還需要通過支腿目標位置求解上述調(diào)平結(jié)束后支腿的關(guān)節(jié)角。仍以左前腿為例，調(diào)平后的向量如式(14)所示：

lf=lf0-lf1

(14)

將向量lf分解為升降機構(gòu)分向量lfsj與擺動機構(gòu)分向量lfbd。求解升降機構(gòu)的關(guān)節(jié)角時，首先需要消除擺動機構(gòu)產(chǎn)生的分向量，該分向量為向量lf在向量f或向量b的投影：

(15)

左前腿升降關(guān)節(jié)角如式(16)所示：

(16)

同法可解得θrf、θlb。

3 仿真驗證

本文中僅涉及目標位置解算，仿真驗證的內(nèi)容為行走底盤處于姿態(tài)調(diào)平規(guī)劃的目標位置時，回轉(zhuǎn)平臺的位姿是否到位，以及4條支腿是否共面。

首先使用UG工具測量圖紙，得到各個向量與坐標點的初值，使用Matlab工具實現(xiàn)上述算法，分別輸入多組俯仰角與橫滾角得出規(guī)劃結(jié)果，再使用UG繪制，如圖3所示，每張圖上半部分為主視圖，表示橫滾角，下半部分為左視圖，表示俯仰角：

從圖3可以看出：各支腿末端位置共面，回轉(zhuǎn)平臺在目標位置調(diào)節(jié)方向與調(diào)節(jié)量正確，通過對模型的測量，角度誤差較小。通過人工操作在水平地面采集了一組實際數(shù)據(jù)，使用姿態(tài)傳感器測出俯仰角與橫滾角后取負值代入Matlab程序，得到仿真數(shù)據(jù)與實際數(shù)據(jù)的對比，結(jié)果如表2所示。

表2 實際數(shù)據(jù)與仿真數(shù)據(jù)

從表2可以看出誤差較小，結(jié)果基本正確，滿足實際工作的需求。

圖3 目標位置仿真結(jié)果

4 結(jié)論

步履式挖掘機行走底盤是一種并聯(lián)機構(gòu)，其姿態(tài)調(diào)平規(guī)劃包含大量的角度計算。筆者在進行逆解時使用向量對行走底盤的角度進行描述，在計算目標位置時先滿足角約束條件，再按照鉸接關(guān)系確定整個車體模型，最后使用向量計算關(guān)節(jié)轉(zhuǎn)角。該方法仿真結(jié)果正確，易于理解，可通過修改向量與各坐標點的初值應(yīng)用在不同型號、尺寸的步履式挖掘機行走裝置，具有較好的應(yīng)用前景。如何運用文中所得的結(jié)果進行軌跡規(guī)劃與跟蹤控制，是下一步重點研究的課題。

[1] 張鑫.智能型步履式挖掘機動態(tài)穩(wěn)定性分析[D].大慶:東北石油大學,2014.

[2] 王進,王樹彥,步履式液壓挖掘機的發(fā)展概況[J].建筑機械化,2002(3):17-19.

[3] 張云.一種步履式挖掘機的設(shè)計與研制[D].西安:長安大學,2016.

[4] 王福德,寇智慧,張云.基于ADAMS的災害救援步履式挖掘機姿態(tài)規(guī)劃與越障分析[J].建筑機械,2014(11):71-74.

[5] 王曉星.用球面向量法解機械手逆運動學問題[J].機器人,1988,2(1):1-8.

[6] 徐巖,段星光.輪腿式機器人的姿態(tài)耦合優(yōu)化控制[J].中國機械工程,2016(2):427-432.

[7] 魏宇明,陳秀峰,張云.ET80型步履式挖掘機[J].建筑機械,2014(11):4-7.

[8] 鄔俊奇,張云,施曉明,等.某型步履式挖掘機步行式底盤自由度分析[J].工程機械,2015(4):39-45.

[9] 施曉明,梁濱.ET111型步履式液壓挖掘機[J].建筑機械,2015(11):46-48.

[10] JOHN.J.CRAIG,贠超.機器人學導論[M].北京:機械工業(yè)出版社,2006.

(責任編輯楊繼森)

AttitudeAdjustmentofWalkingExcavatorBasedonVector

LIU Zichao

(Research & Development Center of Weapon Equipment information & Control, Automation Research Institute of China South Industries Group Corporation, Mianyang 621000, China)

To apply automatic control technology on walking excavator, this paper describes the angle between base and four legs by vector, and then calculates the desired location by the articulation of four legs and base. The simulation results show that the method can correctly give the desired location, and every wheel is on the ground.

walking excavator; attitude adjustment; inverse kinematics

2017-03-22；

：2017-04-25

劉子超(1994—)，男，碩士，助理工程師，主要從事計算機控制技術(shù)研究。

10.11809/scbgxb2017.09.024

format:LIU Zichao.Attitude Adjustment of Walking Excavator Based on Vector[J].Journal of Ordnance Equipment Engineering,2017(9):117-120.

TJ812.2

：A

2096-2304(2017)09-0117-04

本文引用格式：劉子超.基于向量的步履式挖掘機姿態(tài)調(diào)平規(guī)劃[J].兵器裝備工程學報，2017(9):117-120.