劉子超

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

【信息科學(xué)與控制工程】

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

劉子超

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

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

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

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

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

1 步履式挖掘機(jī)行走底盤建模

1.1 步履式挖掘機(jī)行走裝置簡介

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

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

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

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

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

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

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

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

建模后的向量組如圖2所示。圖2中l(wèi)表示左，r表示右，f表示前，b表示右，下標(biāo)0表示支腿末端，下標(biāo)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)平臺與地面的夾角。因?yàn)樽藨B(tài)調(diào)平需要的輸入量為車體與水平面夾角，即俯仰角、橫滾角，調(diào)平前先假定使用姿態(tài)角傳感器已經(jīng)獲知了當(dāng)前位姿。

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

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

假設(shè)當(dāng)前俯仰角θf與橫滾角θh均小于0，最高側(cè)腿則為右后腿。首先根據(jù)某型步履式挖掘機(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)橫滾角時需要先對橫滾角進(jìn)行修正。使用式(4)修正θh：

(4)

由于坐標(biāo)系固定，而回轉(zhuǎn)平臺與xoy平面不再平行，旋轉(zhuǎn)軸應(yīng)當(dā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è)腿無升降運(yùn)動， 即輪腿末端位置在調(diào)平過程中固定，根據(jù)實(shí)際尺寸可測得右后輪軸在坐標(biāo)系中的坐標(biāo)如式(7)所示：

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

(7)

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

(8)

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

|lf1-lf0|=|lf|

(12)

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

(13)

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

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

lf=lf0-lf1

(14)

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

(15)

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

(16)

同法可解得θrf、θlb。

3 仿真驗(yàn)證

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

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

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

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

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

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

4 結(jié)論

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

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

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

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

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

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

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

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

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

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

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

(責(zé)任編輯楊繼森)

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—)，男，碩士，助理工程師，主要從事計算機(jī)控制技術(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

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