復(fù)雜環(huán)境下多飛行器的優(yōu)化人工勢(shì)能場(chǎng)運(yùn)動(dòng)規(guī)劃算法

劉婳婳 王東輝 錢佳程 孟 瑤 宣建強(qiáng)

（上海宇航系統(tǒng)工程研究所，上海 201109）

0 前言

人工勢(shì)能場(chǎng)（Artificial Potential Field，APF）算法在運(yùn)動(dòng)規(guī)劃中的應(yīng)用范圍較廣[1]。而在實(shí)際問(wèn)題中，客觀存在許多動(dòng)態(tài)不確定性，這些因素能否得到有效處理，將直接影響規(guī)劃效果。經(jīng)過(guò)多年的發(fā)展，許多研究人員為這一課題做出了巨大貢獻(xiàn)[2-3]。然而，APF 理論和方法在一些方面仍有優(yōu)化的空間，例如新模型的構(gòu)建、詳細(xì)參數(shù)的設(shè)置、理論的優(yōu)化和更新等。此外，APF 方法在復(fù)雜條件下也存在一定的缺陷[4-5]。

研究人員主要通過(guò)與其他算法的結(jié)合或模型函數(shù)優(yōu)化對(duì)傳統(tǒng)APF 算法進(jìn)行優(yōu)化[6-8]。趙炳巍等[7]采用模擬退火算法求解局部極小值，存在收斂慢、隨機(jī)性等問(wèn)題。顧育津等[8]給優(yōu)化后的函數(shù)引入1 個(gè)額外的控制器，以避免局部最小困境，但是當(dāng)移動(dòng)代理接近密集障礙環(huán)境時(shí)，仍會(huì)發(fā)生抖動(dòng)。該文研究了一種在三維復(fù)雜靜態(tài)、復(fù)雜動(dòng)態(tài)環(huán)境中，考慮相對(duì)速度、距離以及轉(zhuǎn)向角的優(yōu)化的多飛行器APF 運(yùn)動(dòng)規(guī)劃算法，該算法可以以平滑的路徑擺脫局部最小困境。

1 APF 局部最小困境和路徑振蕩的優(yōu)化

如圖 1 所示，APF 方法的基本原理為將工作環(huán)境中移動(dòng)代理的運(yùn)動(dòng)視為在接收不同潛在場(chǎng)力的影響下的運(yùn)動(dòng)過(guò)程，勢(shì)力場(chǎng)對(duì)應(yīng)的坐標(biāo)點(diǎn)如圖2 所示。目標(biāo)點(diǎn)在整個(gè)移動(dòng)代理的工作環(huán)境中生成吸引場(chǎng)UG（如公式（1）所示），將飛行器從原始區(qū)域引導(dǎo)至目標(biāo)區(qū)域，而障礙物在一定范圍的影響半徑d0內(nèi)產(chǎn)生排斥場(chǎng)UO（如公式（2）所示），所得合勢(shì)能場(chǎng)U（如公式（3）所示）可以通過(guò)其在當(dāng)前位置接收的所有排斥勢(shì)能場(chǎng)和吸引勢(shì)能場(chǎng)的疊加來(lái)獲得，飛行器在引力和排斥力的作用下運(yùn)動(dòng)，二者的合力就是移動(dòng)飛行器的加速力，方向即為飛行器的移動(dòng)方向[8]。

圖1 勢(shì)能場(chǎng)示意圖

圖2 勢(shì)能場(chǎng)對(duì)應(yīng)的坐標(biāo)點(diǎn)

圖3 附加斥力和引力勢(shì)能場(chǎng)點(diǎn)

式中：UG為目標(biāo)點(diǎn)在整個(gè)移動(dòng)代理的工作環(huán)境中生成的吸引場(chǎng)；Xi為飛行器i的位置；kG為吸引力增益因子；d為飛行器i至目標(biāo)地的距離；XG為目標(biāo)G的位置。

式中：UO為障礙物在一定范圍的影響半徑d0內(nèi)產(chǎn)生的排斥場(chǎng)；kO為當(dāng)飛行器和障礙之間的距離；XO為障礙所處位置；d0為影響半徑；U為合勢(shì)能場(chǎng)。

選取我院2017年6月～2018年6月47例行人工髖關(guān)節(jié)置換術(shù)患者的臨床資料，男29例，女18例，年齡59～91歲，平均（73±3.5）歲，股骨頸骨折19例，老年性退行性骨關(guān)節(jié)炎11例；單股骨頭置換術(shù)7例。

在復(fù)雜的環(huán)境中，前方區(qū)域或目標(biāo)區(qū)域周圍可能存在多個(gè)密集障礙物。如果來(lái)自周圍障礙物的排斥勢(shì)能場(chǎng)大于來(lái)自目標(biāo)區(qū)域的吸引勢(shì)能場(chǎng)，那么飛行器在到達(dá)目標(biāo)區(qū)域之前無(wú)法繼續(xù)向前移動(dòng)，即落入局部最小區(qū)域。也就是說(shuō)，當(dāng)飛行器的方向偏離目的地的方向超過(guò)90°或者在當(dāng)前狀態(tài)和相鄰時(shí)間的狀態(tài)的變化接近0 時(shí)，可以判定運(yùn)動(dòng)規(guī)劃陷入了局部最小困境。如公式（4）所示，引入飛行器當(dāng)前位置和目標(biāo)區(qū)域之間的距離因子，以提供一個(gè)排斥力，從而逃離局部最小點(diǎn)，打破吸引和排斥相互抵消的狀態(tài)。

式中：Ua為附加場(chǎng)。

新的總勢(shì)場(chǎng)U1如公式（5）所示。

分析：There was significant difference between the audio and video groups when image and sound were strongly mapped semantically

推動(dòng)農(nóng)業(yè)知識(shí)產(chǎn)權(quán)服務(wù)普及化，為創(chuàng)新創(chuàng)業(yè)團(tuán)隊(duì)提供知識(shí)產(chǎn)權(quán)的服務(wù)是培養(yǎng)“三農(nóng)”知識(shí)產(chǎn)權(quán)創(chuàng)新團(tuán)隊(duì)的意義。首先，可提升學(xué)生對(duì)知識(shí)產(chǎn)權(quán)運(yùn)用、運(yùn)營(yíng)的基本能力，使其掌握知識(shí)產(chǎn)權(quán)產(chǎn)生和轉(zhuǎn)化的過(guò)程，激發(fā)農(nóng)業(yè)高校學(xué)生的創(chuàng)新創(chuàng)業(yè)動(dòng)力。其次，將團(tuán)隊(duì)對(duì)知識(shí)產(chǎn)權(quán)工具的運(yùn)用能力轉(zhuǎn)化為服務(wù)產(chǎn)業(yè)，將科技創(chuàng)新和創(chuàng)業(yè)有機(jī)結(jié)合起來(lái)，形成經(jīng)濟(jì)效益，提高科研轉(zhuǎn)化率。

除此之外，還需要進(jìn)一步考慮障礙物、飛行器和目標(biāo)區(qū)的共線性問(wèn)題。簡(jiǎn)單地引入吸引距離因子會(huì)引起飛行器與共線障礙物的碰撞或振蕩。如圖 3 所示：1） 情形一。當(dāng)確定飛行器處于局部最小狀態(tài)且存在障礙物時(shí)，飛行器與目標(biāo)區(qū)域不共線，在飛行器周圍設(shè)置較小范圍的完整邊界，并在飛行器與目標(biāo)方向的邊界處增設(shè)1 個(gè)吸引勢(shì)能場(chǎng)點(diǎn)，飛行器被吸引離開局部最小陷阱。2） 情況二。當(dāng)判斷飛行器處于局部最小狀態(tài)時(shí)，障礙物與飛行器、目標(biāo)處于共線狀態(tài)，如果在飛行器指向目標(biāo)區(qū)的方向放置額外的吸引勢(shì)能點(diǎn)，就會(huì)與共線障礙物進(jìn)行碰撞或引起路徑抖動(dòng)的問(wèn)題。因此，在完整區(qū)域邊界垂直于飛行器指向目標(biāo)區(qū)域方向的位置增設(shè)1 個(gè)附加斥力勢(shì)能點(diǎn)。為了避免飛行器落入復(fù)雜障礙物區(qū)域，將附加的斥力勢(shì)能場(chǎng)放置在斥力勢(shì)能場(chǎng)較多的方向，推動(dòng)飛行器向更簡(jiǎn)單的勢(shì)能場(chǎng)方向移動(dòng)。如果飛行器方向相對(duì)目的地方向小于90°，那么可以確定飛行器路徑規(guī)劃逃脫了局部最小困境。因此，增強(qiáng)型APF 算法U2如公式（6）所示。

構(gòu)造一個(gè)具有多個(gè)障礙物的二維復(fù)雜環(huán)境，使其出現(xiàn)如圖 4 所示的局部最小困境，以驗(yàn)證該優(yōu)化算法的有效性，仿真結(jié)果如圖5 所示。由圖5 可知，考慮距離因素的U1（Xi）APF 運(yùn)動(dòng)規(guī)劃算法可以逃脫多障礙物的局部最小值陷阱，但是會(huì)陷入共線局部最小值條件，抖動(dòng)嚴(yán)重。而加入附加勢(shì)能場(chǎng)的U2（Xi）APF 運(yùn)動(dòng)規(guī)劃算法成功地?cái)[脫了共線條件下的局部最小值困境，但是仍存在路徑不平滑抖動(dòng)的問(wèn)題。最終優(yōu)化的U3（Xi）APF 運(yùn)動(dòng)規(guī)劃有效地避免了勢(shì)能場(chǎng)的過(guò)度變化，整個(gè)路徑相對(duì)平穩(wěn)，不會(huì)陷入各種局部最小值問(wèn)題。

綜上所述，為了提高國(guó)土資源所檔案管理工作的水平，使其能更好地適應(yīng)時(shí)代發(fā)展，更好地服務(wù)于經(jīng)濟(jì)建設(shè)，建議國(guó)土資源所檔案管理從以下幾個(gè)方面加以改進(jìn)。

為了高起點(diǎn)、高標(biāo)準(zhǔn)地規(guī)劃建設(shè)淮安生態(tài)新城，淮安市政府提出要將生態(tài)新城建設(shè)成生態(tài)示范區(qū)、綠色建筑示范區(qū)的目標(biāo)。水系規(guī)劃在滿足區(qū)域防洪、排澇、灌溉要求的前提下，如何充分體現(xiàn)“綠色、生態(tài)”的建設(shè)理念亦成為規(guī)劃難點(diǎn)之一。

式中：UO1為改進(jìn)排斥場(chǎng)一；ρα為距離因子；α為距離因子系數(shù)，α∈（0，1）；XG為目標(biāo)所處位置。

此外，勢(shì)能場(chǎng)的復(fù)雜性導(dǎo)致勢(shì)能場(chǎng)的變化過(guò)大，而當(dāng)前狀態(tài)受勢(shì)能場(chǎng)變化的影響，導(dǎo)致運(yùn)動(dòng)角度變化過(guò)快，運(yùn)動(dòng)規(guī)劃就會(huì)出現(xiàn)震蕩。如公式（6）所示，可以采用削弱勢(shì)能場(chǎng)、減緩相鄰2 個(gè)運(yùn)動(dòng)狀態(tài)變化率的方法減輕勢(shì)能場(chǎng)變化過(guò)大或變化太頻繁所帶來(lái)的影響。當(dāng)2 個(gè)相鄰飛行器運(yùn)動(dòng)軌跡的夾角連續(xù)超過(guò)90°時(shí)，可以判定為劇烈勢(shì)能場(chǎng)變化。此外，為了進(jìn)一步減少計(jì)算量、降低勢(shì)能場(chǎng)的復(fù)雜性，確定基于飛行器飛行方向的±90°為有效邊界，并消除距離外的障礙物和相應(yīng)的斥力場(chǎng)沖擊，使勢(shì)能場(chǎng)變化強(qiáng)度變小，從而獲得新的排斥場(chǎng)UO2，如公式（7）所示。

新的APF 長(zhǎng)為U3，如公式（9）所示。

稅收活動(dòng)不僅是中國(guó)單方面的責(zé)任，也是共同合作的各個(gè)國(guó)家之間的共有責(zé)任。因此中國(guó)在稅收征管方面為了更好地服務(wù)于國(guó)內(nèi)企業(yè)和其他國(guó)家，制定了相應(yīng)的國(guó)際稅收協(xié)定并與之談簽，在實(shí)現(xiàn)稅收征管服務(wù)的公平性的同時(shí)也加強(qiáng)了貿(mào)易合作。

式中：λ為步長(zhǎng)調(diào)整系數(shù)。

式中：Δθ為相鄰運(yùn)動(dòng)時(shí)刻變軌的轉(zhuǎn)角。

式中：UO2為改進(jìn)斥力場(chǎng)二。

當(dāng)Xo=（xo，yo，zo）T大于影響半徑d0的排斥增益因子時(shí)，定義排斥場(chǎng)UO=（xi）=0。

在3 mm厚的XLPE薄片上截取6片邊長(zhǎng)50 mm、厚度3 mm的正方形XLPE薄片，將6片樣本分為A、B、C三組。其中樣本1、2屬于A組，樣本3、4屬于B組，樣本5、6屬于C組。之后選取3組樣本正中間的圓形區(qū)域(直徑25 mm)作為水樹老化區(qū)，采用注射器針頭在此區(qū)域制作3行平行的針孔缺陷(針孔深度1.5 mm)。注射器針頭的參數(shù)如下：

這臺(tái)縫紉機(jī)，從我記事起就陪伴著母親，就如母親的孩子。可能是經(jīng)常使用的緣故，機(jī)頭的輪子上，手常常拽摸的地方，鍍鉻膜已經(jīng)脫落了，露出花花點(diǎn)點(diǎn)的底色，顯得十分老舊。傳動(dòng)輪上小拇指粗細(xì)的環(huán)形皮繩，已經(jīng)被歲月打磨得粗細(xì)不均，似乎隨時(shí)都會(huì)斷開。縫紉機(jī)臺(tái)板上的木紋依然清晰光潔，但鐵制的壓角和底盤已經(jīng)凹凸不平，那深深淺淺的劃痕，都在無(wú)聲地訴說(shuō)著母親那逝去的歲月。

Δθ越大，對(duì)相應(yīng)的排斥勢(shì)能場(chǎng)的削減程度就越高。同時(shí)，動(dòng)態(tài)調(diào)整電流步長(zhǎng)n，以減緩勢(shì)能場(chǎng)的劇烈變化。此時(shí)，如公式（8）所示，設(shè)置步長(zhǎng)n＇隨著旋轉(zhuǎn)角度的增大而變慢，路徑變化更平穩(wěn)。

圖5 優(yōu)化U1（Xi）、U2（Xi）和U3（Xi）APF 算法的比較

2 三維條件下增強(qiáng)的多飛行器APF 算法

2.1 優(yōu)化的三維APF 動(dòng)態(tài)避碰算法

針對(duì)飛行器在三維復(fù)雜環(huán)境下的運(yùn)動(dòng)規(guī)劃問(wèn)題，該文進(jìn)一步推導(dǎo)了優(yōu)化算法（APF 算法一般采用等步長(zhǎng)算法）。事實(shí)上，飛行器的運(yùn)動(dòng)速率是可變的，鑒于飛行器的狀態(tài)隨著勢(shì)能場(chǎng)U3（X）的變化而變化，因此，將此運(yùn)動(dòng)規(guī)劃優(yōu)化為機(jī)動(dòng)速度Vi的迭代更新，如公式（10）所示。

式中：λ＇為機(jī)動(dòng)速度調(diào)整系數(shù)；n為設(shè)定步長(zhǎng)；V為當(dāng)前機(jī)動(dòng)速度。

構(gòu)建三維復(fù)雜多障礙物周邊環(huán)境，對(duì)可能出現(xiàn)的局部最小困境和路徑震蕩問(wèn)題進(jìn)行驗(yàn)證，仿真結(jié)果如圖6~圖8 所示。由圖6~圖8可知，U1（Xi）的算法陷入了局部最小值困境，出現(xiàn)了嚴(yán)重的抖動(dòng)。附加勢(shì)能場(chǎng)的U2（Xi）算法可以成功擺脫局部最小的困境，但是仍然存在路徑抖動(dòng)不均勻的問(wèn)題。最終優(yōu)化的U3（Xi）算法可以有效地避免勢(shì)能場(chǎng)的過(guò)度變化。在去除不必要的障礙勢(shì)能場(chǎng)后，飛行器沿整個(gè)勢(shì)能場(chǎng)的相對(duì)簡(jiǎn)單方向運(yùn)動(dòng)，路徑相對(duì)平滑，不會(huì)陷入局部最小問(wèn)題。

圖6 U1（Xi）APF

圖7 U2（Xi）APF

圖8 U3（Xi）APF

圖9 三維復(fù)雜條件下多飛行器的優(yōu)化型APF 軌跡

此外，考慮動(dòng)態(tài)障礙物，需要同時(shí)引入相對(duì)距離和機(jī)動(dòng)速度信息，并加入相對(duì)機(jī)動(dòng)速度因子，構(gòu)建增強(qiáng)的排斥場(chǎng)函數(shù)如公式（11）所示。

式中：UO3為構(gòu)建的新勢(shì)能場(chǎng)；V為移動(dòng)障礙物與飛行器的相對(duì)機(jī)動(dòng)速度矢量；XV為動(dòng)態(tài)障礙物的位置；β為增益系數(shù)。

因此，構(gòu)建的新勢(shì)能場(chǎng)UO3將包括飛行器與障礙物的相對(duì)距離和機(jī)動(dòng)速度信息。

2.2 三維復(fù)雜環(huán)境下的優(yōu)化多飛行器APF 算法

當(dāng)多架飛行器在同一復(fù)雜環(huán)境下同時(shí)執(zhí)行任務(wù)時(shí)，應(yīng)考慮飛行器之間的避撞。在飛行器周圍設(shè)置避碰區(qū)域，如果2架飛行器進(jìn)入對(duì)方的避碰區(qū)域，它們將對(duì)方視為動(dòng)態(tài)障礙。飛行器之間的排斥場(chǎng)函數(shù)Vij如公式（12）所示。

式中：A為排斥增益因子；Xi、Xj為飛行器i和飛行器j的位置矢量；ρa(bǔ)為飛行器間的避碰區(qū)域范圍；V（Xi，Xj）為飛行器i和飛行器j之間的相對(duì)機(jī)動(dòng)速度矢量；ρ為歐幾里得距離；γ為飛行器的相對(duì)速度因子。

復(fù)雜環(huán)境中多飛行器的優(yōu)化APF 函數(shù)U4如公式（13）所示。

如圖 9 所示，在多障礙物和動(dòng)態(tài)障礙物的復(fù)雜環(huán)境中，相驗(yàn)證了優(yōu)化多飛行器APF 運(yùn)動(dòng)規(guī)劃的可行性。仿真結(jié)果表明，多飛行器可以在具有多個(gè)靜態(tài)和動(dòng)態(tài)障礙物的復(fù)雜環(huán)境中避障和防撞，并以平滑的路徑成功到達(dá)目標(biāo)區(qū)域。

3 結(jié)語(yǔ)

該文提出了一種基于三維復(fù)雜靜態(tài)和動(dòng)態(tài)障礙物條件的優(yōu)化多飛行器APF 運(yùn)動(dòng)規(guī)劃算法，該算法考慮了相對(duì)速度、相對(duì)距離和轉(zhuǎn)向角，能夠以平滑的路徑避開局部最小值，動(dòng)態(tài)調(diào)整步長(zhǎng)和排斥勢(shì)能場(chǎng)。同時(shí)，設(shè)置障礙物的有效邊界約束，以降低勢(shì)能場(chǎng)的復(fù)雜性。仿真結(jié)果表明，優(yōu)化后的算法能夠以平滑的路徑避開局部最小陷阱。