復雜飛行任務人工磁場路徑規(guī)劃算法

李 哲 彭 鵬 黃海生

1(西安郵電大學電子工程學院 陜西 西安 710121)2(延安大學物理與電子信息學院 陜西 延安 716000)

0 引 言

隨著無人機技術(shù)的發(fā)展，路徑規(guī)劃[1]成為研究熱點，眾多學者提出了一系列路徑規(guī)劃方法。基于圖形的方法如Voronoi圖法[2]和基于勢場的方法如人工勢場法[3-4]，按照路徑搜索方法可以分為決策型搜索方法如A*算法[5]、D*算法和隨機型搜索方法如粒子群優(yōu)化算法[6]、蟻群優(yōu)化算法[7]、進化算法[8]、快速擴展隨機樹法[9]，以及多種方法的組合應用[10-11]。基于流體計算的SIMPLEC法借鑒流體流動的性質(zhì)進行路徑規(guī)劃，但只是將障礙物區(qū)域設(shè)為邊界(即第二類邊界條件)，沒有對可飛行區(qū)域風險程度進行區(qū)分。流體法的主要計算方法有數(shù)值法[12-13]和解析法[14-17]，數(shù)值法適合全局路徑規(guī)劃，運算量較大，解析法能適合局部快速路徑規(guī)劃，但是綜合環(huán)境因素能力較差。

為了解決人工勢場、SIMPLEC法等傳統(tǒng)勢場法綜合復雜環(huán)境因素能力弱的問題，提出建立磁導率線性過渡帶的方法。在將障礙物設(shè)為邊界區(qū)域的基礎(chǔ)上，對可飛行區(qū)域根據(jù)飛行任務設(shè)置不同的相對磁導率(簡稱為磁導率)以更好地綜合環(huán)境信息能力，建立障礙物、興趣區(qū)域、飛行限定高度磁導率過渡帶模型，將起點設(shè)為高磁勢點，將目標點設(shè)為低磁勢點，再利用有限元分析法生成全局磁場方向進行路徑規(guī)劃，最后對生成路徑進行評估檢測。

1 磁場特點和路徑規(guī)劃

人工磁場法利用現(xiàn)存磁場特性進行路徑規(guī)劃，用到的磁場為無源磁場[19]。以下是無源磁場和路徑規(guī)劃相關(guān)性質(zhì)的論證，包括連通性、避障性、邊界突變性。

1.1 連通性

磁場連通性是指磁感線總是從高磁勢點指向低磁勢點，且磁通密度B在磁導率不發(fā)生突變的情況下總是連續(xù)變化的，則穿過閉合曲面S的磁通量ψ為:

ψ=∮SB·dS

(1)

對式(1)應用散度定理：

(2)

式中：V為閉合曲面S所包圍的體積，穿過一個封閉面S的磁通量等于離開這個封閉曲面的磁通量，即磁通線永遠是連續(xù)的。路徑規(guī)劃的目的是從出發(fā)點到目標點規(guī)劃出一條路徑，磁感線的閉合性和連續(xù)性符合這個特點。

1.2 避障性

依據(jù)磁屏蔽效應，磁場邊界和空穴處磁場分布符合齊次諾曼邊界條件。在邊界和空穴處，磁勢下降方向垂直于交界面，n為法向單位向量，磁勢下降方向和交界面法方向關(guān)系如下：

(3)

式中：Vm為磁勢下降方向。

磁場方向垂直于磁勢下降方向，滿足路徑規(guī)劃中的避障要求。

1.3 邊界突變性

為了拓寬流體場理論在三維路徑規(guī)劃中的應用，在非障礙物區(qū)域設(shè)置為不同磁導率的材料。不同磁導率區(qū)域交界處存在磁場方向突變的情況，磁導率突變邊界的磁場方向性質(zhì)如圖1所示。

圖1 磁導率突變邊界條件

由恒定磁場的兩個基本方程∮SB·dS=0和∮CH·dl=I，其中：dl為環(huán)路線元；I為環(huán)路電流。可得到磁場的邊界條件：

(4)

式中：n為法向單位向量；t為切向單位向量；B1n和B2n分別為分界面兩側(cè)磁通密度的法向分量；H1t和H2t分別為分界面兩側(cè)磁場強度的切向分量。由式(4)知，在分界面處的磁通密度B的法向分量連續(xù)，無源磁場電流面密度JS=0，所以磁場強度H的切向分量連續(xù)，此時有：

(5)

由式(5)知，磁導率突變邊界有磁場方向突變情況，不利于產(chǎn)生平滑路徑，所以提出磁導率連續(xù)過渡帶方法。

2 人工磁場路徑規(guī)劃算法

2.1 環(huán)境建模

首先要進行環(huán)境信息提取，在物理仿真軟件COMSOL上建立等比例的飛行場景模型，飛行區(qū)域和磁場內(nèi)容對應關(guān)系如圖2所示，提出了根據(jù)環(huán)境中障礙物、興趣區(qū)域、高度限定要求建立磁導率偏移模型，然后進行網(wǎng)格劃分，計算每個網(wǎng)格單元的磁場方向，最后在MATLAB中迭代生成路徑。算法流程如圖2所示。

圖2 人工磁場路徑規(guī)劃算法流程

現(xiàn)實環(huán)境中的障礙物或者興趣區(qū)域可簡化為長方體、球體或其組合，故采用圖3所示模型闡述人工磁場算法流程。

圖3 示例模型

2.2 有限元分析求解

首先要對上述模型進行網(wǎng)格劃分，采用Delaunay四面體網(wǎng)格劃分法把飛行區(qū)域網(wǎng)格化[18-20]，網(wǎng)格化后的飛行區(qū)域如圖4所示。

圖4 示例模型網(wǎng)格劃分

采用整型數(shù)組n(i,e)將單元編碼和節(jié)點編碼聯(lián)系起來。這里用i=1，2，3，4表示每一個四面體的四個頂點，而e=1,2,3,M,其中M表示四面體單元總數(shù)。在n(i,e)中，存儲第e個單元中第i個節(jié)點的全局節(jié)點信息。每個四面體單元內(nèi)磁勢Ф近似為：

Фe(x,y,z)=ae+bex+cey+dez

(6)

將式(6)應用到單元的四個節(jié)點上可以確定四個系數(shù)ae、be、ce、de和單元體積Ve。將第j個節(jié)點上所得系數(shù)回代式(6)可得：

(7)

式(7)中插值函數(shù)為：

(8)

靜磁場是以磁勢Ф表示的無源場，可列泛函數(shù)如下：

(9)

式中：S1表示第一類邊界面；S2表示第二類邊界面；Ф0為已知磁勢點。

將上述邊值問題寫為等價的泛函數(shù)極值問題：

(10)

式中:M表示體單元總數(shù)。Fe由式(11)給出。

(11)

(12)

(13)

有了式(12)給出的單元方程后整合所有M個單元得到方程：

(14)

(15)

示例模型在z=0.15 km處磁場方向的大致分布如圖5所示。

圖5 磁場強度方向分布

2.3 飛行路徑生成

(1) 初始迭代方向選擇。磁場有限元分析法理論上會生成無數(shù)條磁通流線，出發(fā)方向的選擇對路徑的質(zhì)量影響較大，要選擇磁勢下降最快的方向，在一個以起點為球心、10 m為半徑的球面上檢測磁勢最低的點，以起點和球面上最低磁勢點的連線作為初始迭代方向。

(2) 迭代生成路徑。在確定好初始迭代方向后，從起點開始迭代，沿著飛行器當前所在單元的磁場方向He移動步長u，為了表示每一條路徑L的形式，設(shè)任意一條路徑中有n個節(jié)點(包括起始點和終點)。

L=(p1,p2,…,pi,…,pn) 1≤i≤n,n≥2

(16)

式中：p1為起點坐標；pn為終點坐標；pi為第i個節(jié)點的坐標。pi+1和pi的迭代關(guān)系如式(17)所示，從p1開始迭代直到最新點和終點距離小于設(shè)定值。

(17)

(3) 路徑檢測。對所得路徑進行無人機運動學檢測，若不符合則調(diào)整模型參數(shù)，重新生成路徑直到生成路徑滿足要求為止。

2.4 路徑評估函數(shù)的建立

用相鄰兩路徑段pi-1pi和pipi+1夾角的余弦cosθi評估路徑平滑度，函數(shù)值越大路徑越平滑。路徑平滑度評價函數(shù)f1[21]為：

(18)

式中：n表示路徑節(jié)點數(shù)。

路徑風險評估函數(shù)f2的建立。計算每一個路徑點pi到各個障礙物最近點的距離d和障礙物表面風險參數(shù)V以評估路徑的風險，計算如式(19)所示，函數(shù)值越小路徑風險越小。

(19)

式中：m表示障礙物個數(shù)；n表示路徑節(jié)點數(shù)。

3 磁導率偏移模型

為了將不同障礙物危險程度和飛行任務要求體現(xiàn)在磁場模型中，首先依據(jù)SIMPLEC流體場算法，將障礙物實體對應為磁場中的空穴；其次對可飛行區(qū)域處理，對可飛行空間中各個位置設(shè)統(tǒng)一的較高磁導率，稱為基準磁導率μs；然后分別對障礙物、興趣區(qū)域、飛行高度限定建立不同的磁導率偏移模型函數(shù)Δμ；最后將所有飛行區(qū)域組成部分的磁導率偏移模型函數(shù)Δμ疊加到基準磁導率μs上，生成磁場方向以進行路徑規(guī)劃。

3.1 障礙物磁導率偏移模型

在無人機飛行任務中，不同障礙物具有不同的危險程度，比如某些山體有泥石流滑坡威脅。為了將這種危險系數(shù)隨距離障礙物變化的情況加入到路徑規(guī)劃影響因素中，提出磁導率過渡帶障礙物模型，以過渡帶厚度(R1-R0)和風險程度V正比關(guān)系作為建模依據(jù)，障礙物風險越大，過渡帶厚度越大。建模時，障礙物內(nèi)部依舊對應為磁場內(nèi)部空穴，在障礙物外部建立磁導率偏移過渡帶函數(shù)，以球型為例建立障礙物磁導率偏移模型函數(shù)Δμa：

(20)

式中：μ0表示空穴；r為自變量，代表當前點距離球心的距離；μ1表示障礙物實體邊緣偏移磁導率；R0表示球形障礙物半徑；R1表示球形障礙物過渡帶半徑；Ω0為障礙物實體內(nèi)部；Ω1為障礙物外部過渡帶。過渡帶模型示意圖如圖所示6所示，圖中S和E點分別為出發(fā)點和目標點。

圖6 障礙物磁導率偏移模型

3.2 興趣區(qū)域磁導率偏移模型

在無人機飛行任務中，往往有些區(qū)域是飛行中的感興趣區(qū)域，路徑規(guī)劃需要經(jīng)過這些區(qū)域。以球形興趣區(qū)域為例，磁導率偏移函數(shù)如下：

(21)

式中：μ3表示較高磁導率；R0表示興趣區(qū)域半徑；R1表示興趣區(qū)域磁導率過渡帶半徑；Ω2為興趣區(qū)域內(nèi)部；Ω3為興趣區(qū)域外部過渡帶。興趣區(qū)域模型磁場方向分布如圖7所示，可以看出，磁導率漸變過渡帶使磁場分布連續(xù)，不會出現(xiàn)突變情況，且對磁場分布的擾動能力增強。

圖7 興趣區(qū)域模型流線

3.3 飛行高度限定磁導率偏移模型

根據(jù)飛行高度要求，將在限定高度之下建立磁導率線性過渡帶，將目標高度之上設(shè)為低磁導率，以此建立了飛行高度限定磁導率偏移函數(shù)如下：

(22)

式中：z表示空間中某點的高度；h表示限定高度；μ4表示z>h位置的偏移磁導率；k表示磁導率從地面向高處的變化率。

3.4 模型重疊區(qū)域μ的處理

不同模型存在重疊情況，為了保證全局磁導率連續(xù)，就要保證磁導率分布的連續(xù)，在模型重疊區(qū)域做磁導率數(shù)值疊加處理，其中障礙物實體內(nèi)部不疊加保持空穴，疊加公式如下：

(23)

式中：l、m分別表示為障礙物、興趣區(qū)域數(shù)量。

4 綜合仿真和分析

4.1 飛行高度約束與避險綜合仿真

為了人工磁場測試障礙物模型和飛行高度限定模型的綜合路徑規(guī)劃性能，進行了若干組不同限定高度的避障對比實驗，仿真環(huán)境設(shè)置I、II和II三個障礙物，風險系數(shù)V=0.1，其中：仿真一的限定高度為0.1 km；仿真二的限定高度為0.2 km；起點為(0,0.5,0) km；終點為(2,0.5,0) km；k=2 000。飛行環(huán)境信息見表1。

表1 障礙物和飛行限高綜合仿真飛行環(huán)境參數(shù)

仿真結(jié)果如圖8和表2所示，生成路徑在保證平滑度和距離障礙物保持安全距離的條件下分別實現(xiàn)了限高0.1 km和0.2 km的避障飛行任務，滿足飛行高度路徑長度占比分別為79%和73%。

(a) 限高h=0.1 km 3D視圖 (b) 限高h=0.1 km 2D主視圖

表2 不同飛行高度限定仿真結(jié)果對比

4.2 算法對比

為測試障礙物模型和興趣區(qū)域中的綜合路徑規(guī)劃性能，分別使用SIMPLEC法、人工勢場法和人工磁場算法進行路徑規(guī)劃。對于興趣區(qū)域，在實驗環(huán)境設(shè)置I和II兩個圓柱形障礙物，預設(shè)障礙物I風險系數(shù)V1=0.12大于障礙物II風險系數(shù)V2=0.04，故設(shè)障礙物I的過渡帶大于障礙物II，過渡帶內(nèi)外邊緣偏移磁導率分別為Δμi和Δμo，III為圓柱形興趣區(qū)域，設(shè)起點為(0,0.5,0.15) km，終點為(2,0.5,0.15) km，飛行環(huán)境信息見表3。

表3 障礙物和興趣區(qū)域綜合仿真飛行環(huán)境組成

三組仿真結(jié)果見圖9和表4。可以看出三種方法均能規(guī)劃出從起點到終點且完成避障和興趣區(qū)域巡視的路徑規(guī)劃，人工磁場法的路徑平滑性能、興趣區(qū)域搜索性能、避險性能比傳統(tǒng)SIMPLEC分別提高了4.6%、3.1%、21.01%，比人工勢場法分別提高了4.3%、47%、12.14%。

(a) SIMPLEC法3-D視圖 (b) SIMPLEC法平面俯視圖

表4 不同算法仿真結(jié)果對比

5 結(jié) 語

本文提出一種人工磁場的三維路徑規(guī)劃方法，該方法以人工磁場方向生成路徑，在建模過程中，在可飛區(qū)域根據(jù)風險程度設(shè)立磁導率偏移模型。仿真證明了人工磁場算法能完成擴大障礙物擾動磁場、興趣區(qū)域搜索、定高飛行等復雜飛行任務，同時滿足路徑平滑和無人機動力學約束。仿真數(shù)據(jù)表明：

(1) 人工磁場法可實現(xiàn)傳統(tǒng)勢場法不具備的飛行高度限定路徑規(guī)劃，路徑規(guī)劃均可保持70%以上路徑長度滿足設(shè)定飛行高度。

(2) 人工磁場法在路徑平滑性能、興趣區(qū)域搜索性能、避險性能上比傳統(tǒng)SIMPLEC法和人工勢場法更具優(yōu)勢，在路徑長度方面與傳統(tǒng)SIMPLEC和人工勢場法相當。其擴展了勢場法在避險路徑規(guī)劃與決策上的應用，具有應用價值。