徐 達(dá),夏 祥,蘇忠亭
(裝甲兵工程學(xué)院 兵器工程系,北京 100072)
避障路徑規(guī)劃是指在給定的障礙條件以及起始和目標(biāo)的位姿,選擇一條從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑,使運(yùn)動(dòng)物體能安全、無碰撞地通過所有的障礙[1]。近年來,在機(jī)器人避障路徑規(guī)劃方法研究中,國內(nèi)外許多學(xué)者做了大量工作,根據(jù)不同規(guī)劃空間可分為兩種:機(jī)器人W-空間(Working Space)規(guī)劃[2,3]和C-空間(Configuration Space)規(guī)劃[2,3]。由于在C-空間中障礙物映射表述不夠完整,自由空間計(jì)算難度較大,運(yùn)動(dòng)路徑不易確定,有效性不高的缺點(diǎn),本文針對(duì)彈藥裝填機(jī)器人作業(yè)環(huán)境特殊、運(yùn)動(dòng)控制需實(shí)時(shí)可靠的要求,提出了一種基于工作空間的彈藥裝填機(jī)器人避障路徑規(guī)劃算法。該算法首先對(duì)規(guī)劃路徑進(jìn)行離散化,運(yùn)用圓弧對(duì)避障路徑進(jìn)行插補(bǔ),同時(shí)運(yùn)用樣條函數(shù)在關(guān)節(jié)空間進(jìn)行第二次插補(bǔ),最后進(jìn)行了仿真計(jì)算,結(jié)果表明,該算法運(yùn)算簡單,能有效滿足彈藥裝填機(jī)器人的避障要求,保證彈藥裝填機(jī)器人連續(xù)和平穩(wěn)作業(yè)。
彈藥裝填機(jī)器人屬于關(guān)節(jié)型機(jī)器人,擁有PRRR-S型結(jié)構(gòu),其中,P為移動(dòng)副,R為轉(zhuǎn)動(dòng)副,S為球面關(guān)節(jié)[4],由D-H法則建立彈藥裝填機(jī)器人連桿坐標(biāo)系,其結(jié)構(gòu)簡圖與連桿坐標(biāo)系如圖1所示,其中l(wèi)1、l2為彈藥裝填機(jī)器人大臂和小臂的長度。機(jī)器人PRRR關(guān)節(jié)用來確定末端執(zhí)行器的空間位置,S關(guān)節(jié)用來確定末端執(zhí)行器的空間姿態(tài)。
圖1 彈藥裝填機(jī)器人結(jié)構(gòu)及連桿坐標(biāo)系圖
作業(yè)環(huán)境分析是進(jìn)行避障路徑規(guī)劃的基礎(chǔ)。彈藥裝填機(jī)器人的作業(yè)環(huán)境主要是指裝甲車輛的車內(nèi)環(huán)境,由于坦克、自行火炮車內(nèi)空間狹窄、結(jié)構(gòu)復(fù)雜,且作業(yè)過程車體顛簸振動(dòng)幅度較大,與工業(yè)機(jī)器人相比,彈藥裝填機(jī)器人作業(yè)環(huán)境表現(xiàn)為狹窄、封閉、多障礙的特點(diǎn),如炮塔中的炮尾,在訓(xùn)練與射擊過程中,隨著炮塔俯仰而不斷變化,在W-空間中具有幾何不規(guī)則性,因此需對(duì)其進(jìn)行一定簡化。本文采用球形包圍盒法對(duì)其進(jìn)行簡化與建模,同時(shí)將末端執(zhí)行器、機(jī)械臂的大小、形狀因素考慮進(jìn)去,得到最終簡化模型如圖2所示。其中為考慮機(jī)械臂寬度后球體包圍盒當(dāng)量半徑,為包容盒球體的半徑,d為機(jī)械臂最大寬度。
圖2 障礙物包圍盒示意圖
建立當(dāng)量包圍盒后,障礙物幾何中心即已知,設(shè)圓心坐標(biāo)為 O (x0,y0,z0),彈藥裝填機(jī)器人末端執(zhí)行器參考點(diǎn)坐標(biāo)為 P ( px, py, pz),當(dāng)式(1)成立時(shí),彈藥裝填機(jī)器人即與障礙物發(fā)生碰撞,此可作為彈藥裝填機(jī)器人避碰判定式。
圖3 彈藥裝填機(jī)器人避障路徑示意圖
由于空間曲線基本可以分割為多段直線或圓弧的組合,因此在機(jī)器人W-空間路徑規(guī)劃過程中,通常采用空間直線和空間圓弧兩種基本曲線。
如圖3所示,設(shè)障礙物圓心為o,半徑為r,彈藥裝填機(jī)器人末端執(zhí)行器運(yùn)動(dòng)起始點(diǎn)為A,目標(biāo)點(diǎn)為B。在點(diǎn)A與點(diǎn)B之間利用空間直線與空間圓弧進(jìn)行規(guī)劃。避障規(guī)劃過程中,路徑有多種選擇,即空間曲線ACDEB、空間曲線和空間曲線等。根據(jù)路徑最短和機(jī)器人末端執(zhí)行器可達(dá)空間最大化原則,選取空間圓弧ADB作為避障過程路徑,其中D點(diǎn)由末端執(zhí)行器和障礙物的空間尺寸及避碰安全系數(shù)決定。
圖4 障礙物包圍盒示意圖
根據(jù)式(1)及選取的路徑ADB,可設(shè)計(jì)彈藥裝填機(jī)器人避障步驟,流程如圖4所示。具體如下:
1)確定彈藥裝填機(jī)器人工作空間,求解障礙物中心與末端執(zhí)行器空間坐標(biāo);
2)將空間圓弧ADB離散化若干控制點(diǎn),確定避障路徑起點(diǎn)與終點(diǎn);
3)運(yùn)用運(yùn)動(dòng)學(xué)逆解求得起點(diǎn)、終點(diǎn)與離散控制點(diǎn)對(duì)應(yīng)各關(guān)節(jié)變量;
4)對(duì)步驟3中所得關(guān)節(jié)變量進(jìn)行二次插值,得到各關(guān)節(jié)運(yùn)動(dòng)函數(shù);
5)取各關(guān)節(jié)運(yùn)動(dòng)函
數(shù)極值對(duì)應(yīng)點(diǎn),進(jìn)行運(yùn)動(dòng)學(xué)正解,利用式(1)判斷是否與障礙物相碰,檢驗(yàn)二次插值精度和離散點(diǎn)數(shù)目是否符合要求;
6)利用符合要求的關(guān)節(jié)運(yùn)動(dòng)函數(shù)驅(qū)動(dòng)控制電機(jī),實(shí)現(xiàn)彈藥裝填機(jī)器人避障路徑。
對(duì)于彈藥裝填機(jī)器人工作空間路徑規(guī)劃,由于作業(yè)環(huán)境的特殊性,只考慮位置變化。設(shè)給定的三個(gè)點(diǎn)位置分別為,如圖5所示,由此確定平面M,方程為:
其中:1a,1b,1c,1d分別為式(2)中關(guān)于x,y,z,1的余子式。
圖5 彈藥裝填機(jī)器人空間圓弧路徑示意圖
過AD的中點(diǎn)E并垂直于AD,可以唯一確定一個(gè)平面T,則該平面上的任何直線均垂直于AD,從而得平面T的方程為:
同理,過DB中點(diǎn)并垂直于DB的平面S的方程可寫為:
設(shè)彈藥裝填機(jī)器人末端執(zhí)行器沿圓弧勻速運(yùn)動(dòng),運(yùn)動(dòng)時(shí)間為ft,得圓弧上任意時(shí)刻t點(diǎn)的位置坐標(biāo)為:
根據(jù)第三節(jié)設(shè)計(jì)的路徑以及算法步驟,采用空間圓弧進(jìn)行插補(bǔ),得到彈藥裝填機(jī)器人末端執(zhí)行器軌跡如圖6所示,其運(yùn)動(dòng)速度與圓弧插補(bǔ)速度相關(guān),當(dāng)圓弧插補(bǔ)為勻速時(shí),其末端執(zhí)行器在x、y、z方向速度矢量如圖7所示。
從圖6可知,末端執(zhí)行器空間軌跡與所規(guī)劃曲線吻合,符合彈藥裝填機(jī)器人供彈過程避障要求,從圖7可知,其各個(gè)軸向速度曲線具有單調(diào)性,速度變量在約束條件內(nèi),各軸最大速度滿足彈藥裝填機(jī)器人綜合特性,末端執(zhí)行器各軸速度曲線變化平滑和連續(xù),這表明彈藥裝填機(jī)器人末端執(zhí)行器在避障過程中不會(huì)產(chǎn)生振動(dòng),能夠保證作業(yè)中平穩(wěn)控制與運(yùn)動(dòng)。
圖6 彈藥裝填機(jī)器人末端執(zhí)行器空間軌跡
圖7 末端執(zhí)行器各軸速度曲線
本文以彈藥裝填機(jī)器人為對(duì)象,從最短路徑與易控制性能角度出發(fā),研究了彈藥裝填機(jī)器人作業(yè)過程中的避障問題。在分析了彈藥裝填機(jī)器人工作環(huán)境與障礙物特性基礎(chǔ)上,提出了一種基于W-空間圓弧插補(bǔ)的避障軌跡方法,并規(guī)劃了該算法的步驟與流程,具體給出了該算法的實(shí)現(xiàn)過程,并對(duì)所研究的算法進(jìn)行了仿真與分析,結(jié)果表明:該算法運(yùn)算簡單,能有效滿足彈藥裝填機(jī)器人的避障要求,保證彈藥裝填機(jī)器人末端執(zhí)行器按照預(yù)設(shè)路徑進(jìn)行運(yùn)動(dòng),且滿足各個(gè)軸向速度、加速度連續(xù)平穩(wěn)的要求。
[1] 施琴,馬履中,劉翊,王君雄.六自由度機(jī)器人避障問題的MATLAB仿真[J].機(jī)床與液壓,2010,38(9):89-91.
[2] 孫樹棟.工業(yè)機(jī)器人技術(shù)基礎(chǔ)[M].西安:西北工業(yè)大學(xué)出版社,2006(5):92-119.
[3] 戈志勇.番茄采摘機(jī)械手運(yùn)動(dòng)學(xué)仿真與避障算法研究[D].江蘇大學(xué),2007.
[4] 徐達(dá),王中盛,劉廣洋,等.基于串并聯(lián)結(jié)構(gòu)的彈藥裝填機(jī)器人設(shè)計(jì)[J].裝甲兵工程學(xué)院學(xué)報(bào),2008,22(5):45-50.
[5] 徐達(dá),帥元,郝琢,侯潤峰.彈藥裝填機(jī)器人自適應(yīng)PD控制算法[J].裝甲兵工程學(xué)院學(xué)報(bào),2011,25(1):49-53.