圖4 機(jī)械手軌跡規(guī)劃示意圖

　　位于中間層的控制是機(jī)械手抓取運(yùn)動(dòng)控制中最關(guān)鍵的部分。它實(shí)際上是一個(gè)補(bǔ)間器。機(jī)械手的運(yùn)動(dòng)控制可分為姿態(tài)控制與手位置控制。腰、大臂、小臂、手爪的控制，是由主控計(jì)算機(jī)通過機(jī)械手運(yùn)動(dòng)學(xué)位移問題進(jìn)行反向運(yùn)動(dòng)求解，得出腰、大臂、小臂、手爪的位置，然后給出各關(guān)節(jié)位置指令傳至上述關(guān)節(jié)的嵌入式系統(tǒng)控制計(jì)算機(jī)，完成帶有速度反饋環(huán)的位置控制。四個(gè)關(guān)節(jié)的運(yùn)動(dòng)速度通過機(jī)械手的軌跡規(guī)劃來決定，從而避免機(jī)械手的蛇形運(yùn)動(dòng)。由于有觸覺傳感器，手爪可以自動(dòng)控制，也可以是在圖像信號(hào)的指引下作精細(xì)“柔軟控制”。抓取可疑爆炸物時(shí)，控制手爪夾持力約大于可疑爆炸物脫落重力，不至用力過大而夾壞可疑爆炸物，從而完成專家指定的動(dòng)作。

　　位于最下層是控制級(jí)位置控制器與速度調(diào)節(jié)器均采用成熟的PID（包括P、PI、PD）控制。當(dāng)PID控制其選擇合適的參數(shù)時(shí)，具有一定魯棒性，簡(jiǎn)單、實(shí)用。當(dāng)PID無法實(shí)現(xiàn)有效控制時(shí)，可改進(jìn)之或結(jié)合其它方法，如前饋控制。運(yùn)動(dòng)控制包括機(jī)械手各個(gè)關(guān)節(jié)運(yùn)動(dòng)控制和行走運(yùn)動(dòng)，初步統(tǒng)計(jì)控制回路有13路，如圖5所示。

圖5 自動(dòng)控制原理圖

　　嵌入式控制計(jì)算機(jī)根據(jù)上位計(jì)算機(jī)傳來的位置與速度信號(hào)，對(duì)各個(gè)伺服機(jī)構(gòu)進(jìn)行位置與速度設(shè)定，按控制規(guī)律（如PID）對(duì)伺服電機(jī)進(jìn)行控制，使實(shí)際位置按預(yù)計(jì)的速度跟蹤或運(yùn)動(dòng)到目標(biāo)位置。

　　機(jī)器人的手眼協(xié)調(diào)系統(tǒng)：機(jī)器人的手眼協(xié)調(diào)系統(tǒng)是本系統(tǒng)最大亮點(diǎn)之一。由于于補(bǔ)間器算法需要知道目標(biāo)物與機(jī)器人基坐標(biāo)（一般是機(jī)械人的腰關(guān)節(jié)底座），相對(duì)空間座標(biāo)才能求解。而本系統(tǒng)目標(biāo)物是在未知環(huán)境里。這相對(duì)坐標(biāo)必須由于手眼協(xié)調(diào)系統(tǒng)來求解。排爆機(jī)器人機(jī)械手上裝有雙目系統(tǒng)，利用雙目系統(tǒng)和目標(biāo)物可構(gòu)成“測(cè)矩三角型”，原理示意如圖6。測(cè)矩三角型求得距離誤差較大，需要利用激光測(cè)距的“光斑”輔助測(cè)矩，從而獲得坐標(biāo)物的空間座標(biāo)。隨著機(jī)器人的雙目接近目標(biāo)物，目標(biāo)物與基坐標(biāo)的相對(duì)坐標(biāo)精度提高，經(jīng)過數(shù)次雙目逼近目標(biāo)物之后，最終實(shí)現(xiàn)機(jī)械手準(zhǔn)確地抓到目標(biāo)物。

圖6 測(cè)矩三角型原理圖