[發(fā)明專(zhuān)利]一種抓臂式清污機(jī)器人控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011246802.2
• 申請(qǐng)日: 2020-11-10
• 公開(kāi)（公告）號(hào): CN112323755B
• 公開(kāi)（公告）日: 2022-11-08
• 發(fā)明（設(shè)計(jì)）人: 黃振東;高云峰;楊世杰;王亞;王之輝;袁家寶
• 申請(qǐng)（專(zhuān)利權(quán)）人: 安徽省六安恒源機(jī)械有限公司
• 主分類(lèi)號(hào): E02B15/10
• 分類(lèi)號(hào): E02B15/10;E02F3/413;G06F17/11
• 代理公司: 合肥正則元起專(zhuān)利代理事務(wù)所(普通合伙) 34160
• 代理人: 劉培越

1.一種抓臂式清污機(jī)器人控制方法，其特征在于，具體控制步驟如下：

步驟一：設(shè)定清污機(jī)器人的起始位置為P1，攔污柵上端點(diǎn)為P2，河面為P3；

步驟二：根據(jù)抓臂空間軌跡規(guī)劃技術(shù)將P1到P3的運(yùn)動(dòng)路徑分為5個(gè)階段，P1-A加速階段，A-B第一段直線勻速階段，B-C直線段間軌跡過(guò)渡階段，C-D第二段直線勻速階段，D-P3減速階段；

步驟三：根據(jù)運(yùn)動(dòng)學(xué)分析得出運(yùn)動(dòng)路徑規(guī)劃，參考運(yùn)動(dòng)路徑規(guī)劃得到大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的參考位置，具體分析方法為：

S1：建立坐標(biāo)，其中AE為大臂連桿(5)，BD為大臂液壓缸(3)，EQ為小臂連桿(7)，F(xiàn)G為小臂液壓缸(4)，NR為抓斗液壓缸(6)，RS為梳齒(9)，QRS為抓斗(8)，Q距RS的距離為定值h，在機(jī)器人清污過(guò)程中，保持Q位置與攔污柵距離為定值h，同時(shí)調(diào)整RS姿態(tài)與攔污柵平行；

S2：根據(jù)清污機(jī)器人的參數(shù)尺寸，求解RS在運(yùn)動(dòng)空間的位置和姿態(tài)；

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

y＝I_AEcosθ₁+I_EQcos(θ₁+θ₂)，z＝I_AEsinθ₁+I_EQsin(θ₁+θ₂)，式中，I_AE為大臂連桿長(zhǎng)度，I_EQ為小臂連桿長(zhǎng)度；

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

Φ＝θ₁+θ₂+θ₃+θ₄，式中，θ₄是梳齒相對(duì)抓斗的角度，且θ₄為定值；

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

根據(jù)余弦定理將大臂液壓缸(3)行程I_BD轉(zhuǎn)換為角度∠DAB：

式中，I_AD、I_AB為大臂液壓缸(3)兩端點(diǎn)距原點(diǎn)A的距離，與I_BD構(gòu)成三角形；

S6：大臂關(guān)節(jié)角度θ₁與角度∠DAB的關(guān)系：

θ₁＝∠DAB+∠CAB-π/2-∠DAE，式中，∠CAB、∠DAE為大臂液壓缸(3)兩端點(diǎn)的關(guān)節(jié)角；

S7：小臂關(guān)節(jié)角度θ₂與小臂液壓缸(4)行程I_FG的關(guān)系：

θ₂＝π-∠GEF-∠GEA-∠FEQ，

式中，I_EG、I_EF為小臂液壓缸(4)兩端點(diǎn)距原點(diǎn)E的距離，與I_FG構(gòu)成三角形；∠GEA、∠FEQ為小臂液壓缸(4)兩端點(diǎn)的關(guān)節(jié)角；

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

θ₃＝∠NQE+∠NQR-π

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

步驟四：將大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的參考位置變化通過(guò)差分計(jì)算，可得到參考運(yùn)動(dòng)速度；

步驟五：將參考運(yùn)動(dòng)速度與速度增益值(Kv)相乘，計(jì)算出速度前饋控制的輸入；

步驟六：速度前饋控制進(jìn)行模擬計(jì)算得出大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的前瞻運(yùn)動(dòng)趨勢(shì)，并控制伺服比例閥來(lái)控制液壓缸油壓的壓力；

步驟七：安裝于大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的編碼器進(jìn)行大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的實(shí)時(shí)位置獲取；

步驟八：將大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)實(shí)時(shí)獲取的位置與位置增益值(Kp)相乘，計(jì)算得出位置反饋控制的輸出，將位置反饋控制的輸出與大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的參考位置進(jìn)行比對(duì)，進(jìn)而對(duì)大臂液壓缸(3)、小臂液壓缸(4)及抓斗液壓缸(6)的位置進(jìn)行實(shí)時(shí)修訂。