基于人工勢(shì)場(chǎng)法的筒彈自動(dòng)裝填路徑規(guī)劃

陳 飛，王旭烽，劉可平，唐曉峰，劉 寧

(1.南京理工大學(xué) 機(jī)械工程學(xué)院， 南京 210094；2.上海機(jī)電工程研究所， 上海 201109)

傳統(tǒng)的筒彈裝填是利用吊機(jī)及吊索在操作人員的輔助下完成，此方法費(fèi)時(shí)費(fèi)力，影響備戰(zhàn)效率。在改進(jìn)過(guò)程中引入自動(dòng)裝填的概念。在已知障礙物的空間信息后，對(duì)裝填車上起始位置及發(fā)射車上目標(biāo)裝填位置進(jìn)行路徑規(guī)劃，得出一條最優(yōu)的路徑，供吊機(jī)和吊具執(zhí)行筒彈裝填操作。近年來(lái)，路徑規(guī)劃逐漸成為國(guó)內(nèi)外智能機(jī)器研究中的一項(xiàng)關(guān)鍵內(nèi)容[1]。機(jī)器根據(jù)已知的局部環(huán)境信息，在不碰到障礙物的要求下，規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)并滿足特定要求的最優(yōu)或次最優(yōu)路徑，以供機(jī)器執(zhí)行下一步操作。

目前解決路徑規(guī)劃的方法很多，有人工勢(shì)場(chǎng)法[2]、遺傳算法[3]、模糊算法[4]、神經(jīng)網(wǎng)絡(luò)算法[5]、蟻群算法[6]等。其中人工勢(shì)場(chǎng)法因其原理簡(jiǎn)單、規(guī)劃路徑時(shí)間短、效率高等特點(diǎn)，被廣泛應(yīng)用于智能機(jī)器的各種路徑規(guī)劃中。但同時(shí)，傳統(tǒng)的人工勢(shì)場(chǎng)法存在陷入局部極小值點(diǎn)導(dǎo)致目標(biāo)點(diǎn)不可達(dá)、靠近障礙物時(shí)出現(xiàn)抖動(dòng)等問(wèn)題。針對(duì)這些問(wèn)題，許多研究者提出了有效的改進(jìn)方法。彭艷等[7]利用切點(diǎn)法對(duì)人工勢(shì)場(chǎng)法進(jìn)行優(yōu)化，以切點(diǎn)為施力點(diǎn)進(jìn)行路徑規(guī)劃，解決了震蕩和目標(biāo)點(diǎn)不可達(dá)問(wèn)題；張鵬彬等[8]設(shè)置中間目標(biāo)點(diǎn)，有效地使機(jī)器人脫離局部極小值點(diǎn)；韓偉等[9]借助專家經(jīng)驗(yàn)進(jìn)行模糊決策[10]，對(duì)機(jī)器合力的大小和方向做出調(diào)整，使得機(jī)器人準(zhǔn)確地到達(dá)目標(biāo)點(diǎn)；師五喜等[11]提出了雙層遞階模糊系統(tǒng)的方法，一階系統(tǒng)調(diào)整斥力增益系數(shù)，二階系統(tǒng)對(duì)合力的信息進(jìn)行改變，有效地避免抖動(dòng)并達(dá)到優(yōu)化路徑的目的；梁獻(xiàn)霞等[12]提出設(shè)置虛擬障礙物的方法，改變機(jī)器人的運(yùn)動(dòng)方向，從而順利到達(dá)目標(biāo)點(diǎn)；鄧青等[13]提出一種混合路徑規(guī)劃的方法，使得裝甲車能夠在虛擬戰(zhàn)場(chǎng)環(huán)境里根據(jù)地形信息選擇機(jī)動(dòng)路線，實(shí)現(xiàn)良好的路徑規(guī)劃。

現(xiàn)有研究中，大多都是針對(duì)移動(dòng)機(jī)器人、車輛、機(jī)械臂和無(wú)人機(jī)進(jìn)行路徑規(guī)劃，而筒彈自動(dòng)裝填條件下的路徑規(guī)劃鮮有研究。尤其是在考慮三維障礙物模型時(shí)，實(shí)際情況中車上障礙物區(qū)域多為矩形，但大多數(shù)研究都只將障礙物簡(jiǎn)化成球體，并未考慮障礙物的邊角等矩形區(qū)域。此外在優(yōu)化增益系數(shù)時(shí)，多數(shù)研究者都只單獨(dú)對(duì)斥力增益系數(shù)進(jìn)行優(yōu)化，并未同時(shí)考慮引力增益系數(shù)和斥力增益系數(shù)對(duì)三維路徑的影響。對(duì)此，本文以某筒彈自動(dòng)裝填車和發(fā)射車為研究對(duì)象，建立裝填車和發(fā)射車上的矩形障礙物模型。在改進(jìn)勢(shì)場(chǎng)函數(shù)的基礎(chǔ)上，利用最優(yōu)化方法同時(shí)對(duì)引力增益系數(shù)和斥力增益系數(shù)進(jìn)行優(yōu)化，以實(shí)現(xiàn)避開(kāi)局部極小值點(diǎn)和優(yōu)化路徑的目的。文中所提到的最優(yōu)路徑即為長(zhǎng)度最短的路徑。

1 人工勢(shì)場(chǎng)法

1.1 傳統(tǒng)人工勢(shì)場(chǎng)法

1986年，Khatib[14]首次提出人工勢(shì)場(chǎng)法，其基本原理是把運(yùn)動(dòng)體在某環(huán)境中的運(yùn)動(dòng)看作是在虛擬勢(shì)場(chǎng)中受到勢(shì)場(chǎng)力引導(dǎo)下的運(yùn)動(dòng)。虛擬勢(shì)場(chǎng)包含目標(biāo)點(diǎn)處產(chǎn)生的引力場(chǎng)和障礙物處產(chǎn)生的斥力場(chǎng)，兩個(gè)力場(chǎng)的合成方向則決定了運(yùn)動(dòng)體的運(yùn)動(dòng)方向。因此，人工勢(shì)場(chǎng)可以定義為：

U=Ua+Ur

(1)

式中:Ua為目標(biāo)點(diǎn)處的引力場(chǎng)；Ur為障礙物處的斥力場(chǎng)，定義兩個(gè)力場(chǎng)的負(fù)梯度分別為引力和斥力。

結(jié)合空間動(dòng)力學(xué)和拉格朗日方程，合成的人工勢(shì)場(chǎng)對(duì)運(yùn)動(dòng)體的作用力F為：

F=Fa+Fr

(2)

式中：Fa為目標(biāo)點(diǎn)對(duì)運(yùn)動(dòng)體的引力；Fr為障礙物對(duì)運(yùn)動(dòng)體的排斥力。

在二維平面中，運(yùn)動(dòng)點(diǎn)在未達(dá)到目標(biāo)點(diǎn)前，目標(biāo)點(diǎn)對(duì)其會(huì)持續(xù)存在吸引趨勢(shì)，且該引力與距離成正比。故將引力勢(shì)場(chǎng)函數(shù)定義為：

(3)

式中：g為引力增益系數(shù)；X為運(yùn)動(dòng)點(diǎn)的當(dāng)前位置；Xg為目標(biāo)點(diǎn)的位置；(X-Xg)為當(dāng)前運(yùn)動(dòng)點(diǎn)與目標(biāo)點(diǎn)之間的距離。對(duì)引力勢(shì)場(chǎng)函數(shù)求負(fù)梯度得出引力Fa為：

Fa=-▽(Ua)=g(X-Xg)

(4)

障礙物附近斥力場(chǎng)的處理和引力場(chǎng)相似。假設(shè)障礙物自身存在一定的影響范圍，當(dāng)運(yùn)動(dòng)點(diǎn)與障礙物之間的最短距離大于影響范圍時(shí)，運(yùn)動(dòng)點(diǎn)不受斥力的作用；當(dāng)最短距離小于或等于影響范圍時(shí)，運(yùn)動(dòng)點(diǎn)受到斥力的作用，且該斥力與最短距離成反比。故將斥力場(chǎng)函數(shù)定義為：

(5)

式中：m為斥力增益系數(shù)；ρ為運(yùn)動(dòng)點(diǎn)到障礙物之間的最短距離；w為障礙物自身的影響范圍。對(duì)斥力場(chǎng)函數(shù)求負(fù)梯度得出斥力Fr為：

(6)

運(yùn)動(dòng)點(diǎn)在引力和斥力的合力作用下運(yùn)動(dòng)時(shí)，其受力情況如圖1所示。

圖1 運(yùn)動(dòng)點(diǎn)的受力情況

傳統(tǒng)的人工勢(shì)場(chǎng)法主要存在如下兩點(diǎn)缺陷：

1) 陷入極小值點(diǎn)。當(dāng)運(yùn)動(dòng)點(diǎn)在運(yùn)動(dòng)過(guò)程中受到的引力和斥力的大小相等且方向相反時(shí)，運(yùn)動(dòng)點(diǎn)將誤以為已到達(dá)目標(biāo)點(diǎn)，無(wú)法確定下一步的運(yùn)動(dòng)方向，即陷入極小值點(diǎn)，從而停止運(yùn)動(dòng)。

2) 目標(biāo)點(diǎn)不可達(dá)。當(dāng)目標(biāo)點(diǎn)在障礙物的影響范圍內(nèi)時(shí)，運(yùn)動(dòng)點(diǎn)受到的斥力大于引力，從而導(dǎo)致運(yùn)動(dòng)點(diǎn)一直在目標(biāo)點(diǎn)附近徘徊，無(wú)法到達(dá)目標(biāo)點(diǎn)。

1.2 改進(jìn)人工勢(shì)場(chǎng)法

針對(duì)上述人工勢(shì)場(chǎng)法的缺陷，引入相對(duì)距離的概念。即在計(jì)算斥力場(chǎng)的同時(shí)考慮運(yùn)動(dòng)點(diǎn)與目標(biāo)點(diǎn)間的相對(duì)距離，通過(guò)改進(jìn)斥力場(chǎng)函數(shù)[15]來(lái)解決目標(biāo)點(diǎn)不可達(dá)問(wèn)題。

當(dāng)目標(biāo)點(diǎn)在障礙物的作用范圍內(nèi)時(shí)，整個(gè)勢(shì)場(chǎng)的最小點(diǎn)并不是目標(biāo)點(diǎn)，原因在于運(yùn)動(dòng)點(diǎn)在靠近目標(biāo)點(diǎn)的同時(shí)，障礙物的勢(shì)場(chǎng)也在迅速增大，從而運(yùn)動(dòng)點(diǎn)到達(dá)不了目標(biāo)點(diǎn)。若運(yùn)動(dòng)點(diǎn)在靠近目標(biāo)點(diǎn)時(shí)，障礙物的勢(shì)場(chǎng)趨向于零，則目標(biāo)點(diǎn)將是該勢(shì)場(chǎng)環(huán)境下的最小點(diǎn)。因此在定義斥力場(chǎng)函數(shù)時(shí)，需要將運(yùn)動(dòng)點(diǎn)與目標(biāo)點(diǎn)間的相對(duì)距離考慮在內(nèi)。修改后的斥力場(chǎng)函數(shù)如下：

(7)

其中增加了調(diào)節(jié)項(xiàng)(X-Xg)n，n為大于零的實(shí)數(shù)。此調(diào)節(jié)項(xiàng)可確保斥力場(chǎng)在目標(biāo)點(diǎn)處最小。相應(yīng)的斥力函數(shù)修改為：

(8)

式中:Fr1和Fr2為Fr的兩個(gè)分力；Fr1的方向?yàn)閺恼系K物指向運(yùn)動(dòng)點(diǎn)；Fr2的方向?yàn)閺倪\(yùn)動(dòng)點(diǎn)指向目標(biāo)點(diǎn)，分別表達(dá)如下：

(9)

(10)

2 模型建立

在野外作戰(zhàn)環(huán)境下實(shí)施筒彈自動(dòng)裝填前，發(fā)射車和裝填車按要求停車到位，如圖2所示。經(jīng)調(diào)平后，兩車相對(duì)空間位置固定，圖中所示圓為吊機(jī)攜帶吊具所能到達(dá)的吊裝范圍。在筒彈裝填前，需將裝填空間內(nèi)的障礙物區(qū)域信息提前錄入主控計(jì)算機(jī)內(nèi)進(jìn)行路徑解算。發(fā)射車和裝填車上的障礙物主要有光電裝置、雷達(dá)、轉(zhuǎn)塔、油機(jī)電站等，這些設(shè)備作為障礙物所構(gòu)成的區(qū)域大多為矩形區(qū)域。但很多研究者僅僅將三維障礙物區(qū)域簡(jiǎn)化擴(kuò)展成球體進(jìn)行建模計(jì)算。本文綜合考慮實(shí)際障礙物區(qū)域和斥力計(jì)算方法，采用矩形障礙物模型對(duì)實(shí)際筒彈裝填進(jìn)行數(shù)值仿真。

圖2 裝填車與發(fā)射車的相對(duì)位置示意圖

為了仿真不同障礙物區(qū)域的效果，建立矩形障礙物模型如圖3所示，球體障礙物模型如圖4所示。圖3中，黑色部分代表發(fā)射車和裝填車的車體平面，白色部分代表筒彈聯(lián)裝架。并將每個(gè)長(zhǎng)方體離散成若干個(gè)點(diǎn)，便于后續(xù)計(jì)算距離ρ。裝填車上筒彈聯(lián)裝架起吊的起始點(diǎn)簡(jiǎn)化為起點(diǎn)(50，30，65)，發(fā)射車上裝填位上方點(diǎn)簡(jiǎn)化為目標(biāo)點(diǎn)(242.5，90，73)，障礙物的起點(diǎn)分別為(25，40，50)、(25，50，60)、(200，40，50)、(200，50，60)，長(zhǎng)寬高分別為(50，100，10)、(15，80，5)、(50，100，10)、(15，80，5)。圖4中起點(diǎn)與目標(biāo)點(diǎn)的位置與圖3中相同，球體障礙物的中心點(diǎn)為(50，65，50)和(235，65，50)，半徑為25(高度方向上的包絡(luò)值)。

圖3 矩形障礙物模型

圖4 球體障礙物模型

3 仿真計(jì)算

運(yùn)動(dòng)點(diǎn)的運(yùn)動(dòng)計(jì)算流程如圖5所示。運(yùn)動(dòng)點(diǎn)從起點(diǎn)出發(fā)，按照運(yùn)動(dòng)點(diǎn)指向目標(biāo)點(diǎn)的方向運(yùn)動(dòng)；每走一步，計(jì)算當(dāng)前運(yùn)動(dòng)點(diǎn)與矩形障礙物中每個(gè)點(diǎn)的距離ρ，并比較該距離與障礙物的影響范圍w。若ρ>w，則按運(yùn)動(dòng)點(diǎn)原來(lái)的運(yùn)動(dòng)方向繼續(xù)運(yùn)動(dòng)；若ρ≤w，則計(jì)算虛擬力在每個(gè)方向上分力的大小，從而更新上一步運(yùn)動(dòng)點(diǎn)的坐標(biāo)，改變方向繼續(xù)運(yùn)動(dòng)。

圖5 運(yùn)動(dòng)點(diǎn)的計(jì)算流程框圖

3.1 球體與矩形障礙物對(duì)比

首先利用傳統(tǒng)的人工勢(shì)場(chǎng)法分別對(duì)球體和矩形障礙物進(jìn)行仿真分析，式(2)為人工勢(shì)場(chǎng)對(duì)運(yùn)動(dòng)體的作用力。取引力增益系數(shù)g=5，斥力增益系數(shù)m=1，障礙物影響范圍w=1。仿真結(jié)果如圖6所示。

圖6(a)中，黑線表示運(yùn)動(dòng)點(diǎn)在球體障礙物區(qū)域下規(guī)劃的路徑，仿真得出該路徑的長(zhǎng)度L為201.8；圖6(b)中，黑線表示運(yùn)動(dòng)點(diǎn)在矩形障礙物區(qū)域下規(guī)劃的路徑，運(yùn)動(dòng)點(diǎn)運(yùn)動(dòng)到(194，74，71)該位置時(shí)，陷入局部極小值點(diǎn)并停止運(yùn)動(dòng)，無(wú)法到達(dá)目標(biāo)點(diǎn)。

圖6 傳統(tǒng)人工勢(shì)場(chǎng)法規(guī)劃的路徑

3.2 傳統(tǒng)與改進(jìn)人工勢(shì)場(chǎng)法對(duì)比

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法下出現(xiàn)的如圖6(b)所示極小值點(diǎn)情況，采用公式(7)對(duì)斥力場(chǎng)函數(shù)進(jìn)行改進(jìn)。取n=2，在其余參數(shù)不變的情況下進(jìn)行仿真，結(jié)果如圖7所示。改進(jìn)后運(yùn)動(dòng)點(diǎn)能夠在該特殊位置下，改變極小值點(diǎn)處的運(yùn)動(dòng)方向，順利繞過(guò)障礙物到達(dá)目標(biāo)點(diǎn)。仿真得出該路徑的長(zhǎng)度L為197.6。

圖7 改進(jìn)人工勢(shì)場(chǎng)法規(guī)劃的路徑

對(duì)比圖6和圖7可知：如圖6(a)，采用傳統(tǒng)的人工勢(shì)場(chǎng)法，障礙物為球體時(shí)，規(guī)劃路徑的長(zhǎng)度L=201.8；如圖6(b)，障礙物為矩形時(shí)，傳統(tǒng)人工勢(shì)場(chǎng)法存在陷入局部極小值點(diǎn)、目標(biāo)不可達(dá)問(wèn)題，無(wú)法進(jìn)行完整的路徑規(guī)劃；而在圖7中，改進(jìn)后的人工勢(shì)場(chǎng)法使得矩形障礙物解決了原有問(wèn)題，并成功規(guī)劃出一條優(yōu)于球體障礙物的更短路徑L=197.6。因此，矩形障礙物模型是合理的。矩形障礙物模型比球體障礙物模型規(guī)劃的路徑更優(yōu)，且矩形障礙物模型更接近真實(shí)場(chǎng)景。

3.3 增益系數(shù)的影響

采用改進(jìn)后的人工勢(shì)場(chǎng)法，在矩形障礙物模型的基礎(chǔ)上，考慮增益系數(shù)的影響。整個(gè)勢(shì)場(chǎng)函數(shù)中存在兩個(gè)增益系數(shù)，為引力增益系數(shù)g和斥力增益系數(shù)m，它們分別對(duì)引力和斥力進(jìn)行調(diào)節(jié)。研究中發(fā)現(xiàn)，增益系數(shù)的變化影響特定環(huán)境下的最優(yōu)路徑。若要單獨(dú)對(duì)某一增益系數(shù)進(jìn)行優(yōu)化時(shí)，需在給定區(qū)間內(nèi)利用最優(yōu)化方法進(jìn)行求解，從而得到最優(yōu)路徑；若要同時(shí)對(duì)兩個(gè)增益系數(shù)進(jìn)行優(yōu)化，則需給定兩個(gè)增益系數(shù)分別對(duì)應(yīng)的區(qū)間，并對(duì)應(yīng)相應(yīng)的路徑長(zhǎng)度；再結(jié)合最優(yōu)化方法，對(duì)區(qū)間內(nèi)的增益系數(shù)進(jìn)行調(diào)整，并尋找其中的最優(yōu)解，以使得路徑最優(yōu)，從而實(shí)現(xiàn)路徑優(yōu)化。

最優(yōu)化方法[16]是求一個(gè)多元函數(shù)在某個(gè)給定集合上的極值，可用如下數(shù)學(xué)模型描述：

minL=f(m,g)

s.t.m∈K1,g∈K2

(11)

其中：K1、K2為給定的集合;L=f(m,g)為定義在集合K1、K2上的路徑實(shí)值函數(shù)；m、g為兩增益系數(shù)。

分析不同的引力增益系數(shù)和斥力增益系數(shù)對(duì)路徑的影響，采用改進(jìn)后的勢(shì)場(chǎng)函數(shù)進(jìn)行仿真，路徑的長(zhǎng)度為L(zhǎng)，結(jié)果如圖8、圖9所示。

由仿真結(jié)果可以看出，在斥力增益系數(shù)及其他參數(shù)相同的情況下，改變引力增益系數(shù)，路徑的長(zhǎng)度會(huì)隨之改變；同樣，在引力增益系數(shù)相同的情況下，改變斥力增益系數(shù)，路徑也會(huì)隨之變化。因此以引力增益系數(shù)和斥力增益系數(shù)為自變量，以路徑長(zhǎng)度為目標(biāo)，構(gòu)建多元函數(shù)，并利用最優(yōu)化函數(shù)即式(11)對(duì)其進(jìn)行優(yōu)化，所得結(jié)果如圖10所示。

由圖10可知，兩增益系數(shù)共同影響路徑長(zhǎng)度L。當(dāng)引力增益系數(shù)g在(1，5)、斥力增益系數(shù)m在(5，15)組成的平面內(nèi)時(shí)，路徑的長(zhǎng)度L變化幅度非常小。因此在取增益系數(shù)時(shí)，只要兩增益系數(shù)落在上述區(qū)間內(nèi)，都能得到最優(yōu)路徑，其取值L=195。

圖8 相同斥力增益系數(shù)，不同引力增益系數(shù)下的路徑

圖9 相同引力增益系數(shù)，不同斥力增益系數(shù)下的路徑

圖10 優(yōu)化后的路徑長(zhǎng)度隨增益系數(shù)的變化

4 結(jié)論

1) 較之球體障礙物模型，矩形障礙物模型能更真實(shí)地模擬車上障礙物的實(shí)際區(qū)域，并且矩形障礙物下的路徑規(guī)劃更優(yōu)。

2) 對(duì)斥力場(chǎng)函數(shù)進(jìn)行改進(jìn)后的人工勢(shì)場(chǎng)法，能夠使運(yùn)動(dòng)點(diǎn)成功脫離局部極小值點(diǎn)到達(dá)目標(biāo)點(diǎn)。

3) 同時(shí)對(duì)引力增益系數(shù)和斥力增益系數(shù)進(jìn)行優(yōu)化時(shí)，給定系數(shù)一定范圍內(nèi)，能夠?qū)ふ业教囟ㄔ鲆嫦禂?shù)，使得該增益系數(shù)下的路徑為最優(yōu)。