徐 達，夏 祥，蘇忠亭

（裝甲兵工程學院 兵器工程系，北京 100072）

0 引言

避障路徑規(guī)劃是指在給定的障礙條件以及起始和目標的位姿，選擇一條從起始點到達目標點的路徑，使運動物體能安全、無碰撞地通過所有的障礙[1]。近年來，在機器人避障路徑規(guī)劃方法研究中，國內(nèi)外許多學者做了大量工作，根據(jù)不同規(guī)劃空間可分為兩種：機器人W-空間(Working Space)規(guī)劃[2，3]和C-空間（Configuration Space）規(guī)劃[2，3]。由于在C-空間中障礙物映射表述不夠完整，自由空間計算難度較大，運動路徑不易確定，有效性不高的缺點，本文針對彈藥裝填機器人作業(yè)環(huán)境特殊、運動控制需實時可靠的要求，提出了一種基于工作空間的彈藥裝填機器人避障路徑規(guī)劃算法。該算法首先對規(guī)劃路徑進行離散化，運用圓弧對避障路徑進行插補，同時運用樣條函數(shù)在關節(jié)空間進行第二次插補，最后進行了仿真計算，結果表明，該算法運算簡單，能有效滿足彈藥裝填機器人的避障要求，保證彈藥裝填機器人連續(xù)和平穩(wěn)作業(yè)。

1 彈藥裝填機器人運動構型與作業(yè)環(huán)境表述

1.1 彈藥裝填機器人運動構型

彈藥裝填機器人屬于關節(jié)型機器人，擁有PRRR-S型結構，其中，P為移動副，R為轉(zhuǎn)動副，S為球面關節(jié)[4]，由D-H法則建立彈藥裝填機器人連桿坐標系，其結構簡圖與連桿坐標系如圖1所示，其中l(wèi)1、l2為彈藥裝填機器人大臂和小臂的長度。機器人PRRR關節(jié)用來確定末端執(zhí)行器的空間位置，S關節(jié)用來確定末端執(zhí)行器的空間姿態(tài)。

圖1 彈藥裝填機器人結構及連桿坐標系圖

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

作業(yè)環(huán)境分析是進行避障路徑規(guī)劃的基礎。彈藥裝填機器人的作業(yè)環(huán)境主要是指裝甲車輛的車內(nèi)環(huán)境，由于坦克、自行火炮車內(nèi)空間狹窄、結構復雜，且作業(yè)過程車體顛簸振動幅度較大，與工業(yè)機器人相比，彈藥裝填機器人作業(yè)環(huán)境表現(xiàn)為狹窄、封閉、多障礙的特點，如炮塔中的炮尾，在訓練與射擊過程中，隨著炮塔俯仰而不斷變化，在W-空間中具有幾何不規(guī)則性，因此需對其進行一定簡化。本文采用球形包圍盒法對其進行簡化與建模，同時將末端執(zhí)行器、機械臂的大小、形狀因素考慮進去，得到最終簡化模型如圖2所示。其中為考慮機械臂寬度后球體包圍盒當量半徑，為包容盒球體的半徑，d為機械臂最大寬度。

圖2 障礙物包圍盒示意圖

2 避障路徑規(guī)劃

2.1 避障路徑設計

建立當量包圍盒后，障礙物幾何中心即已知，設圓心坐標為 O (x0,y0,z0)，彈藥裝填機器人末端執(zhí)行器參考點坐標為 P ( px, py, pz)，當式（1）成立時，彈藥裝填機器人即與障礙物發(fā)生碰撞，此可作為彈藥裝填機器人避碰判定式。

圖3 彈藥裝填機器人避障路徑示意圖

由于空間曲線基本可以分割為多段直線或圓弧的組合，因此在機器人W-空間路徑規(guī)劃過程中，通常采用空間直線和空間圓弧兩種基本曲線。

如圖3所示，設障礙物圓心為o，半徑為r，彈藥裝填機器人末端執(zhí)行器運動起始點為A，目標點為B。在點A與點B之間利用空間直線與空間圓弧進行規(guī)劃。避障規(guī)劃過程中，路徑有多種選擇，即空間曲線ACDEB、空間曲線和空間曲線等。根據(jù)路徑最短和機器人末端執(zhí)行器可達空間最大化原則，選取空間圓弧ADB作為避障過程路徑，其中D點由末端執(zhí)行器和障礙物的空間尺寸及避碰安全系數(shù)決定。

圖4 障礙物包圍盒示意圖

2.2 避障算法步驟與流程

根據(jù)式（1）及選取的路徑ADB，可設計彈藥裝填機器人避障步驟，流程如圖4所示。具體如下：

1）確定彈藥裝填機器人工作空間，求解障礙物中心與末端執(zhí)行器空間坐標；

2）將空間圓弧ADB離散化若干控制點，確定避障路徑起點與終點；

3）運用運動學逆解求得起點、終點與離散控制點對應各關節(jié)變量；

4）對步驟3中所得關節(jié)變量進行二次插值，得到各關節(jié)運動函數(shù)；

5）取各關節(jié)運動函

數(shù)極值對應點，進行運動學正解，利用式（1）判斷是否與障礙物相碰，檢驗二次插值精度和離散點數(shù)目是否符合要求；

6）利用符合要求的關節(jié)運動函數(shù)驅(qū)動控制電機，實現(xiàn)彈藥裝填機器人避障路徑。

2.3 空間圓弧插補

對于彈藥裝填機器人工作空間路徑規(guī)劃，由于作業(yè)環(huán)境的特殊性，只考慮位置變化。設給定的三個點位置分別為，如圖5所示，由此確定平面M，方程為：

其中：1a，1b，1c，1d分別為式（2）中關于x，y，z，1的余子式。

圖5 彈藥裝填機器人空間圓弧路徑示意圖

過AD的中點E并垂直于AD，可以唯一確定一個平面T，則該平面上的任何直線均垂直于AD，從而得平面T的方程為：

同理，過DB中點并垂直于DB的平面S的方程可寫為：

設彈藥裝填機器人末端執(zhí)行器沿圓弧勻速運動，運動時間為ft，得圓弧上任意時刻t點的位置坐標為：

3 避障路徑仿真與分析

根據(jù)第三節(jié)設計的路徑以及算法步驟，采用空間圓弧進行插補，得到彈藥裝填機器人末端執(zhí)行器軌跡如圖6所示，其運動速度與圓弧插補速度相關，當圓弧插補為勻速時，其末端執(zhí)行器在x、y、z方向速度矢量如圖7所示。

從圖6可知，末端執(zhí)行器空間軌跡與所規(guī)劃曲線吻合，符合彈藥裝填機器人供彈過程避障要求，從圖7可知，其各個軸向速度曲線具有單調(diào)性，速度變量在約束條件內(nèi)，各軸最大速度滿足彈藥裝填機器人綜合特性，末端執(zhí)行器各軸速度曲線變化平滑和連續(xù)，這表明彈藥裝填機器人末端執(zhí)行器在避障過程中不會產(chǎn)生振動，能夠保證作業(yè)中平穩(wěn)控制與運動。

圖6 彈藥裝填機器人末端執(zhí)行器空間軌跡

圖7 末端執(zhí)行器各軸速度曲線

4 結束語

本文以彈藥裝填機器人為對象，從最短路徑與易控制性能角度出發(fā)，研究了彈藥裝填機器人作業(yè)過程中的避障問題。在分析了彈藥裝填機器人工作環(huán)境與障礙物特性基礎上，提出了一種基于W-空間圓弧插補的避障軌跡方法，并規(guī)劃了該算法的步驟與流程，具體給出了該算法的實現(xiàn)過程，并對所研究的算法進行了仿真與分析，結果表明：該算法運算簡單，能有效滿足彈藥裝填機器人的避障要求，保證彈藥裝填機器人末端執(zhí)行器按照預設路徑進行運動，且滿足各個軸向速度、加速度連續(xù)平穩(wěn)的要求。

[1] 施琴,馬履中,劉翊,王君雄.六自由度機器人避障問題的MATLAB仿真[J].機床與液壓,2010,38(9):89-91.

[2] 孫樹棟.工業(yè)機器人技術基礎[M].西安:西北工業(yè)大學出版社,2006(5):92－119.

[3] 戈志勇.番茄采摘機械手運動學仿真與避障算法研究[D].江蘇大學,2007.

[4] 徐達,王中盛,劉廣洋,等.基于串并聯(lián)結構的彈藥裝填機器人設計[J].裝甲兵工程學院學報,2008,22(5):45－50.

[5] 徐達,帥元,郝琢,侯潤峰.彈藥裝填機器人自適應PD控制算法[J].裝甲兵工程學院學報,2011,25(1):49-53.