新型救援工程機(jī)械末端執(zhí)行器軌跡控制研究

吳海宇， 陳欠根，， 趙喻明，， 陳冬良， 王廣松

（1.中南大學(xué) 高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙 410012；2.山河智能裝備股份有限公司 國(guó)家級(jí)企業(yè)技術(shù)中心，湖南 長(zhǎng)沙410100）

由于全球的地震災(zāi)害進(jìn)入了高發(fā)期（僅2010年前4個(gè)月，國(guó)內(nèi)外便連續(xù)發(fā)生了41次6級(jí)以上的地震），復(fù)雜的地震坍塌現(xiàn)場(chǎng)急需專業(yè)、高效的救援機(jī)械裝備［1］。因此，各種新型救援工程機(jī)械相繼誕生，如俄羅斯烏拉爾拖拉機(jī)廠生產(chǎn)的171.1k3型防核輻射推土機(jī)、美國(guó)雪龍公司生產(chǎn)的融雪機(jī)、俄羅斯生產(chǎn)的Rm140型全路面運(yùn)輸車以及美國(guó)卡特彼勒的挖掘裝載車［2］等。

地震救援是一種特殊的行為動(dòng)作，對(duì)機(jī)械動(dòng)作有極高的要求：救援機(jī)械在切割鋼筋混凝土建筑構(gòu)件、預(yù)應(yīng)力柱時(shí)要保證穩(wěn)定和可靠，防止二次垮塌。能否完成這一要求取決于人員操作的準(zhǔn)確性，故救援工程機(jī)械需引入機(jī)電一體化［3］。在工程機(jī)械領(lǐng)域，機(jī)電一體化控制包含機(jī)械電子化、自動(dòng)化和機(jī)器人化［4］。已有學(xué)者在這些方面進(jìn)行了研究，如文獻(xiàn)［5］在對(duì)挖掘機(jī)機(jī)器人化改造的基礎(chǔ)上，建立了工作裝置運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，針對(duì)系統(tǒng)動(dòng)力學(xué)高非線性、參數(shù)不確定性等特點(diǎn)，提出了在自適應(yīng)魯棒基礎(chǔ)上非連續(xù)映射的方法處理運(yùn)動(dòng)系統(tǒng)，實(shí)現(xiàn)了挖掘機(jī)智能化目的；文獻(xiàn)［6］基于行為控制的思想進(jìn)行軌跡任務(wù)控制，對(duì)挖掘機(jī)器人工作狀態(tài)進(jìn)行評(píng)估，實(shí)現(xiàn)自主挖掘等。

救援工程機(jī)械工作時(shí)環(huán)境惡劣，負(fù)載復(fù)雜多變，自身慣性大，且存在多種非線性因素，故其需要一種基于現(xiàn)場(chǎng)實(shí)時(shí)監(jiān)控反饋的半自動(dòng)化控制模式。本課題研究設(shè)計(jì)了一種多功能7自由度雙臂手搶險(xiǎn)救援工程機(jī)械，其多功能體現(xiàn)在末端執(zhí)行器為可快速更換的屬具，本文結(jié)合其結(jié)構(gòu)特點(diǎn)與作業(yè)環(huán)境提出了3種不同的操作模式，其中屬具末端模式通過(guò)Matlab中的SimMechanics對(duì)動(dòng)態(tài)系統(tǒng)進(jìn)行建模、仿真和分析，得到所需的各關(guān)節(jié)角度值，從而實(shí)現(xiàn)半自主控制［7－9］。

1 新型救援工程機(jī)械

本課題救援工程機(jī)械的機(jī)械臂手具有冗余度，有較大的操作空間，能滿足較多的約束，可以有效躲避自身物理極限和環(huán)境障礙物［8］。該機(jī)械每個(gè)臂手有7個(gè)自由度，均由單個(gè)手柄控制，如圖1所示。采用電控控制，可實(shí)現(xiàn)動(dòng)作輕便及遠(yuǎn)程遙控。

圖1 新型救援工程機(jī)械

2 3種操作模式

如前所述，救援工程機(jī)械所受負(fù)載為變載荷、動(dòng)載荷，故控制需要考慮現(xiàn)場(chǎng)環(huán)境，在實(shí)現(xiàn)控制能動(dòng)基礎(chǔ)上，結(jié)合自動(dòng)控制原理，實(shí)現(xiàn)半自動(dòng)化的控制。實(shí)現(xiàn)的最可靠的方式是從操作員的操作方式入手，因此本文提出了3種不同的操作模式。

操作員處在司機(jī)室內(nèi)，操作必須避免誤動(dòng)作，故需要手柄控制簡(jiǎn)單、輕便。本機(jī)采用了Gessmann公司的單軸多控手柄，如圖2所示，手柄主軸沿X、Y、Z方向運(yùn)動(dòng)，手柄面板上2個(gè)小按鍵可分別沿X、Y方向運(yùn)動(dòng)，可同時(shí)實(shí)現(xiàn)7個(gè)自由度的控制。單手柄可實(shí)現(xiàn)控制單臂。

圖2 操作手柄

2.1 常規(guī)關(guān)節(jié)模式

常規(guī)關(guān)節(jié)模式中執(zhí)行器單獨(dú)控制，單手柄對(duì)應(yīng)控制單臂手。此控制模式的優(yōu)點(diǎn)是控制靈活，能夠通過(guò)操作實(shí)現(xiàn)機(jī)械臂手的各種位姿，但對(duì)操作員技能要求較高。

手柄的控制方式如圖3所示。

圖3 常規(guī)關(guān)節(jié)模式

2.2 屬具末端模式

當(dāng)救援機(jī)械處于需要實(shí)現(xiàn)特定的救援動(dòng)作，如需要將抓起的重物往水平直線路徑放置，此控制點(diǎn)為腕部中心，對(duì)中心點(diǎn)的移動(dòng)通過(guò)聯(lián)合主臂擺動(dòng)、變幅和副臂變幅油缸實(shí)現(xiàn)直線軌跡，操作手柄也只需控制手柄X、Y、Z方向控制受控點(diǎn)（即腕部中心）?？刂七^(guò)程中的3個(gè)執(zhí)行油缸通過(guò)算法實(shí)時(shí)反饋，實(shí)現(xiàn)自動(dòng)的控制輸出。

手柄的控制如圖4所示。

圖4 屬具末端模式

對(duì)于屬具末端模式，其難點(diǎn)在于機(jī)器人技術(shù)與大型機(jī)械控制技術(shù)的融合問(wèn)題。機(jī)器人技術(shù)常用于穩(wěn)定載荷工況下的簡(jiǎn)化分析，而大型工程機(jī)械中必須考慮變負(fù)載、大尺寸等復(fù)雜工況，故需要取舍。

2.3 雙臂協(xié)調(diào)模式

為了適應(yīng)多種救援環(huán)境，本新型機(jī)械采用了雙臂手設(shè)計(jì)，故在控制中必須考慮雙臂的協(xié)調(diào)問(wèn)題。在半自主控制中，特殊救援過(guò)程需要實(shí)現(xiàn)雙臂的協(xié)調(diào)動(dòng)作、協(xié)同工作，如雙臂同時(shí)抬起一塊石板且不傾覆。把雙臂看成一個(gè)整體，操作人員通過(guò)只操作一個(gè)手柄同時(shí)控制雙臂的運(yùn)動(dòng)，而此運(yùn)動(dòng)是基于屬具末端實(shí)現(xiàn)的，同樣實(shí)現(xiàn)末端軌跡直線控制，故手柄操作仍為圖4所示的控制方式。

此操作模式的難點(diǎn)在于如何將同一個(gè)手柄的輸出信號(hào)通過(guò)系統(tǒng)識(shí)別來(lái)分別控制2個(gè)臂手的輸出信號(hào)，同時(shí)2個(gè)臂手末端的相對(duì)位置不改變。

3 控制策略簡(jiǎn)述

整機(jī)控制，需要保證目標(biāo)的可靠性及穩(wěn)定性?；谇笆鼍仍畽C(jī)械的特殊要求以及工程機(jī)械特點(diǎn)，救援工程機(jī)械的電控系統(tǒng)設(shè)計(jì)如圖5所示。通過(guò)電液比例閥實(shí)現(xiàn)控制的精細(xì)化及高可靠性，同時(shí)設(shè)置各種傳感器，即對(duì)臂手的位置、位姿進(jìn)行實(shí)時(shí)跟蹤分析及反饋，從而實(shí)現(xiàn)閉環(huán)控制，達(dá)到自動(dòng)化控制的目的。雙臂手分別控制，能很好地滿足后續(xù)的避碰以及協(xié)調(diào)作業(yè)的要求?？刂撇呗院?jiǎn)圖如圖5所示。

圖5 控制策略簡(jiǎn)圖

4 屬具末端操作實(shí)現(xiàn)

實(shí)現(xiàn)臂手末端模式控制的第1步是對(duì)救援機(jī)械進(jìn)行數(shù)學(xué)建模，其系統(tǒng)為與運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)密切相關(guān)的、強(qiáng)耦合、非線性的多變量系統(tǒng)，故在建模時(shí)須充分考慮這些因素。

建模時(shí)首先建立臂手的運(yùn)動(dòng)學(xué)模型，利用DH法分析，確定各個(gè)關(guān)節(jié)的坐標(biāo)及D－H參數(shù)，得到運(yùn)動(dòng)學(xué)正解和逆解；再根據(jù)逆解進(jìn)行末端執(zhí)行器運(yùn)動(dòng)規(guī)劃，實(shí)現(xiàn)末端控制［7－11］。臂手的結(jié)構(gòu)簡(jiǎn)圖如圖6所示。

在模型搭建過(guò)程中，不能忽視單臂自身的尺寸對(duì)其工作范圍的影響，故需要限制其可運(yùn)動(dòng)角度范圍。

圖6 臂手的結(jié)構(gòu)簡(jiǎn)圖

4.1 運(yùn)動(dòng)學(xué)分析

表1所列為臂手的各關(guān)節(jié)參數(shù)，求解運(yùn)動(dòng)學(xué)方程式，用齊次變換矩陣Ai來(lái)描述第i個(gè)相對(duì)于第（i－1）個(gè)坐標(biāo)系的位置方向和方位［6］。

表1 D－H參數(shù)表

臂手的末端相對(duì)于基坐標(biāo)系的位置和姿態(tài)，用A60表示為：

運(yùn)動(dòng)學(xué)逆解的求解方法是將運(yùn)動(dòng)方程（7）式的兩端依次左乘各A矩陣的逆矩陣，并使兩端矩陣的對(duì)應(yīng)元素相等，即可求各個(gè)關(guān)節(jié)變量［8－9］。

4.2 軌跡規(guī)劃和仿真數(shù)據(jù)

軌跡規(guī)劃是指根據(jù)作業(yè)任務(wù)要求（作業(yè)規(guī)劃），對(duì)末端執(zhí)行器在工作流程中位姿變化的位置、姿態(tài)以及它們的速度、加速度的人為設(shè)定，并通過(guò)運(yùn)動(dòng)學(xué)逆解得到各關(guān)節(jié)的運(yùn)動(dòng)指令［12－13］。根據(jù)本機(jī)械所要精確跟蹤的軌跡，確定給定路徑的解析函數(shù)式，并利用Matlab中SimMechanics進(jìn)行運(yùn)動(dòng)學(xué)仿真，通過(guò)編寫輸入軌跡控制Function文件，對(duì)機(jī)構(gòu)進(jìn)行實(shí)時(shí)再現(xiàn)仿真，以得到關(guān)節(jié)空間的各個(gè)關(guān)節(jié)所需的角度值［13－15］。

根據(jù)臂手的機(jī)構(gòu)組成，基于SimMechanics的仿真模型如圖7所示。

（1）物理模型。例如圖7中左下角的Revolute、Body模塊以及theta1輸入模塊共同組成了關(guān)節(jié)1的物理模型。6個(gè)關(guān)節(jié)，6個(gè)連桿和6個(gè)關(guān)節(jié)驅(qū)動(dòng)器尺寸、初始位姿等參數(shù)均由各模塊對(duì)話框設(shè)置，進(jìn)行動(dòng)力學(xué)測(cè)量還需要設(shè)置連桿的質(zhì)心位置、質(zhì)量和慣性矩等參數(shù)。

（2）傳感器。在各關(guān)節(jié)放置關(guān)節(jié)傳感器，以測(cè)量角度（如圖中Joint Sensor模塊），在手抓末端放置剛性傳感器，以測(cè)量末端運(yùn)動(dòng)狀態(tài)，數(shù)據(jù)輸出至workspace供繪制三維軌跡。

（3）驅(qū)動(dòng)模塊。此為本模型最關(guān)鍵部分，需根據(jù)上述運(yùn)動(dòng)學(xué)逆解，建立各關(guān)節(jié)在實(shí)際軌跡發(fā)生時(shí)各關(guān)節(jié)角度驅(qū)動(dòng)所需的角度值，如圖7中的theta1、theta2等輸入值可通過(guò)4.1中運(yùn)動(dòng)學(xué)逆解計(jì)算得到。由于關(guān)節(jié)以角度、角加速度驅(qū)動(dòng)，以弧度為單位，故需要轉(zhuǎn)換為角度值。

仿真系統(tǒng)設(shè)定后運(yùn)行，可觀察救援機(jī)械臂手的模擬運(yùn)動(dòng)，結(jié)束后觀察各曲線，并輸出數(shù)據(jù)給后續(xù)計(jì)算以實(shí)現(xiàn)末端控制。連桿的設(shè)計(jì)長(zhǎng)度參數(shù)為l1＝150mm，l2＝4 663mm，l3＝2 586mm，l4＝400mm，l5＝1 907mm，經(jīng)過(guò)轉(zhuǎn)化得到目標(biāo)物在基坐標(biāo)中的坐標(biāo)。臂手的三維簡(jiǎn)圖如圖8所示。

屬具末端模式下的控制是通過(guò)手柄X、Y、Z方向來(lái)分別控制末端在基座坐標(biāo)系上X、Y、Z方向的運(yùn)動(dòng)，故本文采用了3段行程來(lái)模擬末端的控制（分別為X方向上7 000～9 000mm、Y方向上0～2 000mm、Z方向上0～1 500mm，起始坐標(biāo)都是（7 000，0，0））。

經(jīng)過(guò)仿真實(shí)驗(yàn)可以得到3段運(yùn)動(dòng)各關(guān)節(jié)的角度曲線，如圖9所示。

圖7 基于SimMechanics的運(yùn)動(dòng)學(xué)仿真模型

圖8 臂手的三維簡(jiǎn)圖

圖9 X、Y、Z方向運(yùn)動(dòng)各關(guān)節(jié)的角度值

對(duì)于臂手的動(dòng)作，執(zhí)行器是液壓油缸，故通過(guò)已求得的各關(guān)節(jié)角度值，經(jīng)過(guò)換算得到油缸所需流量，便可通過(guò)電控系統(tǒng)實(shí)現(xiàn)所需角度值控制，即實(shí)現(xiàn)末端控制。

5 試驗(yàn)驗(yàn)證

把上述控制程序?qū)懭肟刂破髦校瑯?biāo)定好傾角傳感器，如圖10、圖11所示。通過(guò)手柄控制實(shí)時(shí)再現(xiàn)上述3段軌跡，記錄各傳感器數(shù)據(jù)，通過(guò)計(jì)算得到末端執(zhí)行器中心點(diǎn)相對(duì)轉(zhuǎn)臺(tái)坐標(biāo)系的軌跡，見(jiàn)表2所列，如圖12所示。

圖10 司機(jī)室控制器

圖11 傾角傳感器

表2 末端執(zhí)行器的軌跡數(shù)據(jù) mm

圖12 通過(guò)試驗(yàn)實(shí)測(cè)的末端軌跡

由試驗(yàn)結(jié)果可以得到，當(dāng)分別進(jìn)行3段行程時(shí)，軌跡變動(dòng)都在5%誤差范圍內(nèi)，精度滿足實(shí)際需求。分析可知，在實(shí)際操作中此算法完全可以實(shí)現(xiàn)精確末端軌跡控制。

6 結(jié)束語(yǔ)

本文針對(duì)大型救援工程機(jī)械在實(shí)際救援過(guò)程中所需的特定動(dòng)作進(jìn)行了控制研究。結(jié)合實(shí)際工況，提出了3種不同的操作控制模式，能很好地應(yīng)對(duì)救援實(shí)際所需。屬具末端控制模式結(jié)合機(jī)器人末端執(zhí)行器軌跡控制原理，利用基于SimMechanics的仿真模型，建立運(yùn)動(dòng)學(xué)仿真模型，通過(guò)參數(shù)設(shè)置及仿真類型選擇進(jìn)行仿真。仿真分析可知，通過(guò)控制可以實(shí)現(xiàn)救援工程機(jī)械的臂手末端平穩(wěn)連續(xù)運(yùn)動(dòng)，且無(wú)柔性沖擊。根據(jù)所得數(shù)據(jù)，換算得到液壓油缸所需控制流量，結(jié)合液壓系統(tǒng)，對(duì)臂手進(jìn)行控制試驗(yàn)。試驗(yàn)分析表明，通過(guò)手柄能夠?qū)崿F(xiàn)對(duì)末端執(zhí)行器中心點(diǎn)的準(zhǔn)確控制。

［1］劉紅亮.雙臂手救援工程機(jī)械轉(zhuǎn)臺(tái)設(shè)計(jì)及優(yōu)化［D］.大連：大連理工大學(xué)，2012.

［2］劉國(guó)輝，趙曉宇.國(guó)內(nèi)外新型應(yīng)急救援工程機(jī)械設(shè)備分析［J］.中國(guó)機(jī)械，2014（2）：40－41.

［3］胡道鵬.挖掘機(jī)作業(yè)運(yùn)動(dòng)軌跡控制研究［D］.廣州：華南理工大學(xué)，2013.

［4］何 冰，溫銀萍.論工程機(jī)械機(jī)電一體化及機(jī)器人化［J］.科技風(fēng)，2011（15）：152.

［5］何清華，張大慶，郝 鵬，等.液壓挖掘機(jī)工作裝置模型及控制的試驗(yàn)研究［J］.中南大學(xué)學(xué)報(bào)：自然科學(xué)版，2006，37（3）：542－546.

［6］戴興建.挖掘機(jī)機(jī)器人化技術(shù)研究［D］.沈陽(yáng)：東北大學(xué)，2010.

［7］熊有倫.機(jī)器人技術(shù)基礎(chǔ)［M］.武漢：華中科技大學(xué)出版社，2002：30－40.

［8］劉亞軍，黃 田.6R操作臂逆運(yùn)動(dòng)學(xué)分析與軌跡規(guī)劃［J］.機(jī)械工程學(xué)報(bào)，2012，48（3）：9－15.

［9］Qiao S，Liao Q，Wei S，et al.Inverse kinematic analysis of the general 6Rseriel manipulators based on double quaternions［J］.Mechanism and Machine Theroy，2010，45（2）：193－199.

［10］Guo Chuangqiang，Ni Fenglei，Sun Jingting，et al.Joint driver and control design for large torge，long arm space remote manipulator［C］／／International Conference on Information and Automation.IEEE，2010：1426－1431.

［11］王魯敏，李艷文.基于Matlab的教學(xué)型機(jī)器人空間運(yùn)動(dòng)軌跡仿真［J］.機(jī)械與電子，2005（9）：55－57.

［12］王健強(qiáng)，程 汀.SCARA機(jī)器人結(jié)構(gòu)設(shè)計(jì)及軌跡規(guī)劃算法［J］.合肥工業(yè)大學(xué)學(xué)報(bào)：自然科學(xué)版，2008，31（7）：1026－1028，1041.

［13］李萬(wàn)莉，陳熙巍，茹 蘭，等.基于SimMechanics的4自由度機(jī)器人的軌跡規(guī)劃和仿真系統(tǒng)設(shè)計(jì)［J］.中國(guó)工程機(jī)械學(xué)報(bào)，2008，6（2）：144－148.

［14］MathWorks Corp.SimMechanics user’s guide 1.1［R］.Natick：MathWorks Corporation，2002.

［15］羅家佳，胡國(guó)清.基于MATLAB的機(jī)器人運(yùn)動(dòng)仿真研究

［J］.廈門大學(xué)學(xué)報(bào)：自然科學(xué)版，2005，44（5）：640－644.