基于W-空間的彈藥裝填機(jī)器人避障路徑規(guī)劃

徐 達(dá)，夏 祥，蘇忠亭

（裝甲兵工程學(xué)院 兵器工程系，北京 100072）

0 引言

避障路徑規(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è)。

1 彈藥裝填機(jī)器人運(yùn)動(dòng)構(gòu)型與作業(yè)環(huán)境表述

1.1 彈藥裝填機(jī)器人運(yùn)動(dòng)構(gòu)型

彈藥裝填機(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)系圖

1.2 作業(yè)環(huán)境概述

作業(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 障礙物包圍盒示意圖

2 避障路徑規(guī)劃

2.1 避障路徑設(shè)計(jì)

建立當(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 障礙物包圍盒示意圖

2.2 避障算法步驟與流程

根據(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ī)器人避障路徑。

2.3 空間圓弧插補(bǔ)

對(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)為：

3 避障路徑仿真與分析

根據(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í)行器各軸速度曲線

4 結(jié)束語

本文以彈藥裝填機(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.