黃振東 ，曹雛清 ，劉志恒 ，陳榮娜 ，張爽華

(1.安徽省六安恒源機(jī)械有限公司，安徽六安 237000；2.蕪湖哈特機(jī)器人產(chǎn)業(yè)技術(shù)研究院有限公司，安徽蕪湖 241000；3.安徽國(guó)防科技職業(yè)學(xué)院，安徽六安 237000)

0 前言

在水利設(shè)施中，為了保證攔水閘門的正常啟閉或水力發(fā)電設(shè)備的安全運(yùn)轉(zhuǎn)，需要對(duì)攔水閘門、攔水壩前及水力發(fā)電設(shè)備進(jìn)水口處的雜物進(jìn)行攔截和清理?，F(xiàn)有的清污設(shè)備多為傳統(tǒng)的回轉(zhuǎn)式和抓斗式清污機(jī)，由于清污形式較固定，不能適應(yīng)各種類型污物的清理；或者下壓抓取力不夠，清污能力較弱。因此，需設(shè)計(jì)一款清污機(jī)器人，可通過抓臂帶動(dòng)抓斗抓取污物，且其液壓驅(qū)動(dòng)系統(tǒng)應(yīng)能夠提供較大的抓取力和提升力。本文作者研究清污機(jī)器人液壓驅(qū)動(dòng)控制方法，分析清污機(jī)器人抓臂運(yùn)動(dòng)過程，控制抓臂沿?cái)r污柵向下運(yùn)動(dòng)，可同時(shí)實(shí)現(xiàn)攔污柵上污物的清理和攔污柵前污物的抓取，有效提高清污效果和清污效率。

1 清污機(jī)器人的設(shè)計(jì)

所設(shè)計(jì)的清污機(jī)器人主要由軌道移動(dòng)車(移動(dòng)行走機(jī)構(gòu))、抓臂、液壓伺服泵站及智能控制系統(tǒng)等四部分構(gòu)成。其中，抓臂作為清污動(dòng)作的主要執(zhí)行機(jī)構(gòu)，固定安裝在軌道移動(dòng)車上，由大臂、小臂、抓斗組成，相鄰兩關(guān)節(jié)間鉸接，通過液壓缸牽引作旋轉(zhuǎn)運(yùn)動(dòng)，實(shí)現(xiàn)末端抓斗在水平、垂直方向的運(yùn)動(dòng)，從而完成清污所需的上升、下降動(dòng)作?；谇度胧娇刂破鞔罱ㄖ悄芸刂葡到y(tǒng)，構(gòu)建抓臂運(yùn)動(dòng)學(xué)和驅(qū)動(dòng)控制模型，實(shí)現(xiàn)清污機(jī)器人對(duì)末端運(yùn)動(dòng)軌跡的精確控制。清污機(jī)器人結(jié)構(gòu)如圖1所示。

圖1 清污機(jī)器人結(jié)構(gòu)

1.1 清污機(jī)器人工作流程

為能夠清理攔污柵上的污物，所設(shè)計(jì)的清污機(jī)器人的梳齒式抓斗需要插入攔污柵中，并沿?cái)r污柵平面向下運(yùn)動(dòng)。為提高清污效率和質(zhì)量，要求抓斗運(yùn)動(dòng)準(zhǔn)確、平穩(wěn)、快速。清污機(jī)器人在工作時(shí)，軌道移動(dòng)車沿地軌移動(dòng)至清污位置，地軌與攔污柵水平安裝，其機(jī)械臂運(yùn)動(dòng)平面與攔污柵平面垂直，使得抓斗可插入攔污柵中。清污機(jī)器人抓臂有3個(gè)自由度，可帶動(dòng)抓斗上升、下降運(yùn)動(dòng)，具體過程如下：初始時(shí)，抓斗處于攔污柵上方位置，抓斗張開，如圖2(a)所示；抓斗沿?cái)r污柵向下運(yùn)行，抓斗保持張開狀態(tài)，清理攔污柵上的污物，如圖2(b)所示；抓斗運(yùn)行到底后，閉合抓斗抓取攔污柵前的污物，如圖2(c)所示；抓斗向上運(yùn)行，回到初始位置，抓斗保持閉合狀態(tài)，如圖2(d)所示。抓取中，抓斗需要沿著攔污柵作直線運(yùn)動(dòng)，且抓斗表面與攔污柵始終保持水平，因此需要對(duì)末端進(jìn)行精確的位置控制。

圖2 清污機(jī)器人工作過程

1.2 清污機(jī)器人驅(qū)動(dòng)系統(tǒng)

清污機(jī)器人的主要運(yùn)動(dòng)結(jié)構(gòu)分為軌道移動(dòng)車和抓臂。軌道移動(dòng)車由伺服電機(jī)驅(qū)動(dòng)，用于將抓臂移動(dòng)至清污位置，這里不再詳述。抓臂由液壓系統(tǒng)驅(qū)動(dòng)，分為大臂、小臂、抓斗3個(gè)旋轉(zhuǎn)關(guān)節(jié)。大臂液壓缸安裝在軌道移動(dòng)車上，驅(qū)動(dòng)大臂相對(duì)于軌道移動(dòng)車作旋轉(zhuǎn)運(yùn)動(dòng)；小臂液壓缸安裝在大臂上，驅(qū)動(dòng)小臂相對(duì)于大臂作旋轉(zhuǎn)運(yùn)動(dòng)；抓斗液壓缸安裝在小臂上，驅(qū)動(dòng)抓斗相對(duì)于小臂作旋轉(zhuǎn)運(yùn)動(dòng)，如圖3所示。采用液壓伺服比例閥分別對(duì)3個(gè)運(yùn)動(dòng)關(guān)節(jié)的液壓缸進(jìn)行控制，通過調(diào)節(jié)液壓伺服比例閥的開口實(shí)現(xiàn)對(duì)液壓缸流量和流向的控制，即控制液壓缸的伸縮長(zhǎng)度和方向。同時(shí)，由各個(gè)液壓缸的位置傳感器，得到液壓缸實(shí)際行程，作為伺服比例閥調(diào)節(jié)的反饋信號(hào)。

圖3 清污機(jī)器人液壓缸位置

2 清污機(jī)器人運(yùn)動(dòng)學(xué)分析

在清污過程中，清污機(jī)器人要控制末端抓斗梳齒始終與攔污柵重合，因此需要得到抓斗梳齒與清污機(jī)器人設(shè)計(jì)尺寸的函數(shù)關(guān)系。該機(jī)器人是一種平面三自由度串聯(lián)機(jī)器人，其大臂、小臂關(guān)節(jié)主要用于位置的移動(dòng)，抓斗關(guān)節(jié)用于姿態(tài)的調(diào)整?；趲缀畏ㄇ蠼鈾C(jī)器人運(yùn)動(dòng)學(xué)，建立如圖4所示的抓臂結(jié)構(gòu)簡(jiǎn)圖。為大臂連桿，為大臂液壓缸，為小臂連桿，為小臂液壓缸，為抓斗連桿，為抓斗液壓缸，為抓斗梳齒，清污時(shí)插入攔污柵中。

圖4 清污機(jī)器人抓臂機(jī)構(gòu)簡(jiǎn)圖

根據(jù)清污機(jī)器人的參數(shù)，求解在運(yùn)動(dòng)空間的位置和姿態(tài)。抓斗為末端執(zhí)行器，和是同一剛體，與的距離固定不變，可以簡(jiǎn)化位置的求解過程，由位置代替。在機(jī)器人清污過程中，保持位置與攔污柵距離固定不變，同時(shí)調(diào)整姿態(tài)與攔污柵平行，即可控制抓斗清污梳齒插入攔污柵中。

根據(jù)平面幾何關(guān)系求解位置，大臂關(guān)節(jié)角度、小臂關(guān)節(jié)角度與位置(，)的函數(shù)關(guān)系如下：

(1)

式中:為大臂桿件長(zhǎng)度;為小臂桿件長(zhǎng)度。

求解的姿態(tài)，平面內(nèi)的角度是可以直接相加的，因此大臂關(guān)節(jié)角度、小臂關(guān)節(jié)角度、抓斗關(guān)節(jié)角度、梳齒角度之和即為的方位角：

=+++

(2)

式中:是梳齒相對(duì)抓斗連桿的角度，為定值。

至此，可以得到大臂、小臂、抓斗關(guān)節(jié)角度與抓斗梳齒的函數(shù)關(guān)系，但是各關(guān)節(jié)傳感器測(cè)量的是對(duì)應(yīng)液壓缸的行程，由于各旋轉(zhuǎn)關(guān)節(jié)角度和對(duì)應(yīng)驅(qū)動(dòng)液壓缸行程之間是非線性關(guān)系，因此需要求解兩者的關(guān)系式。

根據(jù)圖4，結(jié)合余弦定理將大臂液壓缸行程轉(zhuǎn)換為角度∠：

(3)

式中:、為大臂液壓缸兩端點(diǎn)距原點(diǎn)的距離，與構(gòu)成三角形。根據(jù)幾何角度關(guān)系，大臂關(guān)節(jié)角度與角度∠的關(guān)系為

=∠+∠-π/2-∠

(4)

式中:∠、∠為大臂液壓缸兩端點(diǎn)的關(guān)節(jié)角。

同理可得，小臂關(guān)節(jié)角度與小臂液壓缸行程的關(guān)系為

(5)

=π-∠-∠-∠

(6)

式中：、為小臂液壓缸兩端點(diǎn)距原點(diǎn)的距離，與構(gòu)成三角形。∠、∠為小臂液壓缸兩端點(diǎn)的關(guān)節(jié)角。

小臂關(guān)節(jié)角度與小臂液壓缸行程的關(guān)系為

(7)

=∠+∠-π

(8)

式中:、為抓斗液壓缸兩端點(diǎn)距原點(diǎn)的距離，與構(gòu)成三角形。∠為抓斗液壓缸一端點(diǎn)的關(guān)節(jié)角。

3 速度前饋控制

在清污過程中，清污機(jī)器人末端抓斗梳齒沿?cái)r污柵運(yùn)動(dòng)，其期望運(yùn)動(dòng)路徑即是攔污柵的表面，是固定不變的，具有重復(fù)性，要求所設(shè)計(jì)的伺服驅(qū)動(dòng)控制器能夠快速響應(yīng)規(guī)劃的軌跡，可采用速度前饋控制方法。但是,末端抓斗梳齒在實(shí)際工作中的實(shí)際位置與期望位置可能會(huì)出現(xiàn)偏差，要求所設(shè)計(jì)的伺服驅(qū)動(dòng)控制器能夠消除位置誤差，保證精確的末端控制位置，可采用位置反饋控制方法。根據(jù)上述分析，為控制各個(gè)液壓缸快速響應(yīng)和位置精確，對(duì)清污機(jī)器人采取位置反饋結(jié)合速度前饋的控制方法，使得末端抓斗梳齒在清污過程中能夠不與攔污柵碰撞且較快速地清理污物。

3.1 控制原理

根據(jù)期望運(yùn)動(dòng)路徑規(guī)劃期望運(yùn)動(dòng)軌跡，得到各個(gè)液壓缸的期望位置，對(duì)位置的變化進(jìn)行差分計(jì)算，可得到期望的運(yùn)動(dòng)速度。將此速度值作為速度前饋的輸入，通過與速度增益值相乘，計(jì)算出速度前饋控制的輸出。速度前饋控制作為主控模型，為液壓缸前瞻運(yùn)動(dòng)趨勢(shì)，可使得系統(tǒng)快速響應(yīng)。同時(shí)，安裝于各個(gè)液壓缸的位置傳感器將所測(cè)的實(shí)際位置與期望位置進(jìn)行對(duì)比，可得出誤差值。將此誤差值作為位置反饋的輸入，通過與位置增益值相乘，計(jì)算出位置反饋控制的輸出。位置反饋控制可看作修正控制，能消除系統(tǒng)所產(chǎn)生的誤差。圖5所示為位置反饋結(jié)合速度前饋的控制框圖，其中：為位置反饋控制的比例增益值;為速度前饋控制的增益值。

圖5 位置反饋結(jié)合速度前饋的控制框圖

實(shí)驗(yàn)測(cè)試時(shí)，根據(jù)實(shí)際清污工作要求，在抓臂行程范圍內(nèi)，運(yùn)動(dòng)最長(zhǎng)清污距離(包含下降和上升過程)的時(shí)間不超過40 s。根據(jù)示教點(diǎn)，規(guī)劃末端抓斗期望清污軌跡，經(jīng)運(yùn)動(dòng)學(xué)解算，給出各個(gè)液壓缸的期望軌跡，對(duì)它進(jìn)行測(cè)試。按照工作流程，清污機(jī)器人末端抓斗分為上升和下降2種運(yùn)動(dòng)狀態(tài)，分別對(duì)應(yīng)各個(gè)液壓缸的伸長(zhǎng)和縮回動(dòng)作，所以分別對(duì)3個(gè)液壓缸進(jìn)行測(cè)試，驗(yàn)證其速度前饋控制的效果。所設(shè)置的位置反饋和速度前饋增益值如表1所示。其中，表示末端抓斗上升運(yùn)動(dòng)時(shí)各液壓缸的位置反饋增益;表示末端抓斗下降運(yùn)動(dòng)時(shí)各液壓缸的位置反饋增益;表示末端抓斗上升運(yùn)動(dòng)時(shí)各液壓缸的速度前饋增益，表示末端抓斗下降運(yùn)動(dòng)時(shí)各液壓缸的速度前饋增益。

表1 3個(gè)液壓缸增益值

3.2 實(shí)驗(yàn)結(jié)果

位置輸入為清污過程規(guī)劃的期望軌跡時(shí)，對(duì)清污機(jī)器人3個(gè)液壓缸的伺服驅(qū)動(dòng)控制器進(jìn)行測(cè)試，結(jié)果如圖6—圖8所示。

圖6 大臂液壓缸期望軌跡和實(shí)際軌跡比對(duì) 圖7 小臂液壓缸期望軌跡和實(shí)際軌跡比對(duì)

圖8 抓斗液壓缸期望軌跡和實(shí)際軌跡比對(duì)

從圖6—圖8中可以看出：3個(gè)液壓缸實(shí)際軌跡曲線和期望軌跡曲線基本重合，液壓缸實(shí)際運(yùn)動(dòng)軌跡能跟蹤上規(guī)劃的運(yùn)動(dòng)軌跡，且起始狀態(tài)無延遲，運(yùn)動(dòng)過程位置無較大偏差，表明位置反饋結(jié)合速度前饋的控制效果較好。清污機(jī)器人清污過程中，末端抓斗梳齒能夠插入攔污柵中平穩(wěn)、快速運(yùn)動(dòng)，且無碰撞，清污效果較好。

4 結(jié)論

(1)分析三自由度串聯(lián)清污機(jī)器人結(jié)構(gòu)，采用幾何法求解清污機(jī)器人運(yùn)動(dòng)學(xué)，計(jì)算末端位姿與3個(gè)關(guān)節(jié)角度之間的關(guān)系，進(jìn)而得到與3個(gè)關(guān)節(jié)液壓缸之間的函數(shù)關(guān)系；

(2)根據(jù)清污機(jī)器人清污過程路徑重復(fù)的特點(diǎn)，設(shè)計(jì)位置反饋結(jié)合速度前饋的伺服控制器，控制機(jī)械臂運(yùn)動(dòng)，其中速度前饋用于預(yù)測(cè)系統(tǒng)所需速度，位置反饋用于減小系統(tǒng)誤差；

(3)按照實(shí)際清污軌跡進(jìn)行驗(yàn)證，速度前饋控制可較好地跟蹤規(guī)劃的軌跡，位置反饋控制能有效減小系統(tǒng)誤差，清污機(jī)器人清污過程運(yùn)行平穩(wěn)、快速，清污效果良好。