面向多導航站接力導航的運動體路徑規劃方法與流程

本發明屬于運動體路徑規劃研究領域，具體涉及一種面向多導航站接力導航的運動體路徑規劃方法，用于實現海、陸、空不同情形多約束條件下運動體從起點到終點的路徑規劃。

背景技術：

運動體的多導航站接力導航是通過散布于不同空間位置且導航作用范圍能夠覆蓋較大空間的多個導航站依次為運動體接續導航的聯合導航方法。這種導航方式不僅可靠性強，而且可以有效延伸運動體的可控飛行范圍。面向多導航站接力導航的運動體路徑規劃問題是實現這種具有前沿性的先進導航方式所需解決的一個關鍵問題，需要考慮三個基本約束條件，即導航范圍約束、導航交接約束、運動體機動能力約束。其中，運動體機動能力約束用運動體曲率約束表示，導航范圍約束和導航交接約束使得本發明中的路徑規劃問題有別于一般的路徑規劃問題，也是本發明中路徑規劃問題的主要難點。

運動體有多種導航方式，包括自身導航和外部導航，外部導航包括衛星導航、導航站導航等。其中，運動體自身導航方式精度有限且易受干擾，衛星導航方式易遭破壞且造價成本高，而導航站導航方式可靠性強，且可通過多站聯合接力導航的方式為運動體執行遠程任務提供路徑導引。

技術實現要素：

為了有效延伸運動體的可控移動范圍，提高導航的可靠性，本發明提供了一種面向多導航站接力導航的運動體路徑規劃方法。該方法是在考慮導航站導航范圍約束、導航交接約束和運動體曲率約束的前提下，為運動體規劃出一條能夠各種滿足各種約束條件且快速從起點抵達終點的水平二維空間路徑。

實現本發明的技術方案如下：

一種面向多導航站接力導航的運動體路徑規劃方法，包括如下步驟：

步驟1、輸入路徑規劃信息，具體包括：運動體起點和終點位置信息、導航站導航范圍約束信息、導航站導航交接約束信息和運動體曲率約束信息；所述導航站導航范圍約束信息是指導航站中心的坐標及有效作用范圍，即運動體從起點到終點的整條路徑須全部位于導航圓內；所述導航圓表示導航站有效作用范圍；所述導航站導航交接約束信息是指確保導航交接成功而設定的運動體在交接區域內的最短路徑長度D；所述導航交接是指多個導航站之間移交運動體的導航權；所述交接區域是指兩個導航圓之間的重疊區域；所述運動體曲率約束信息是指運動體最小轉彎半徑r_tc；

步驟2、隨機生成初始路徑種群；一條路徑由起點位置、終點位置和中間路點的位置及朝向共同確定；所述中間路點是指除起點和終點之外的路點；

其中，中間路點與中間路點之間采用Dubins的“CLC”路徑連接；

將中間路點設置在交接區域邊界弧上，隨機生成多條路徑組成初始路徑種群，其中，中間路點的位置在所屬邊界弧上隨機生成，中間路點的朝向范圍由其位置所決定，具體朝向在其朝向范圍內隨機生成；

步驟3、評價路徑:評價指標包括路徑總長度和約束違反程度；所述路徑總長度是指按照導航順序依次連接起點、中間路點、終點所形成的全部路段長度之和；所述路段是指兩個相鄰路點之間的路徑；所述約束違反程度是指導航交接約束違反程度，當運動體在交接區域內的路徑長度大于或等于約束值時，約束違反程度為0，否則，約束違反程度為兩者的絕對差；

步驟4、記錄初始種群的最優路徑；若初始種群中存在約束違反程度為0的路徑，則在約束違反程度為0的路徑中選取路徑總長度最短的那條路徑作為初始種群的最優路徑，若初始種群所有路徑的約束違反程度均大于0，則選取約束違反程度最小的那條路徑作為初始種群的最優路徑；

步驟5、采用差分進化算法優化路徑；首先通過變異、交叉操作產生新路徑，其中，變異和交叉操作僅作用于路點的位置，路點的朝向范圍隨其位置的改變而改變，朝向在其范圍內隨機生成；

然后在新路徑和舊路徑中進行選擇，留下較優的路徑；選擇操作的原則為：當兩條路徑的約束違反程度相等時，留下路徑總長度較短的那條路徑，當兩條路徑的約束違反程度不相等時，留下約束違反程度較小的那條路徑；將選擇操作留下的路徑與之前記錄的種群最優路徑進行比較，比較方法與選擇操作的原則相同，將兩者中較優的一條作為當前種群的最優路徑；基于當前路徑種群，繼續進行變異、交叉和選擇操作并得到新的路徑種群和最優路徑，直到種群進化代數達到給定的最大進化代數時，終止循環過程，輸出當前種群的最優路徑，完成路徑規劃。

較佳的，所述步驟2中中間路點位置的具體表示方式為：

定義入點為位于入弧上的中間路點，所述入弧為運動體進入交接區域的邊界弧；定義出點為位于出弧上的中間路點，所述出弧為運動體離開交接區域的邊界弧；入點位置的表示方式如下：將交接區域的后導航圓的圓心作為極點，水平方向向右為極軸，逆時針為正方向，建立局部極坐標系，入點的位置用極角表示；所述交接區域的后導航圓是指運動體即將進入的導航圓；

出點位置的表示方法如下：將交接區域的前導航圓的圓心作為極點，水平方向向右為極軸，逆時針為正方向，建立局部極坐標系，出點的位置用極角θ表示；所述交接區域的前導航圓是指運動體即將離開的導航圓；

由此，一條路徑所有中間路點的位置由入點極角和出點極角聯合表示為θ＝[θ₁,θ₂,...,θ_n]，θ_i表示第i個中間路點的位置，i為奇數時，表示為入點的極角，i為偶數時，表示為出點的極角；限定θ_i的取值范圍為[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max為交接區域頂點所對應的極角；所述交接區域頂點為入弧與出弧的交點；采取相對編碼的方式，先將中間路點位置的解范圍映射到[0，1]區間，即將

θ＝[θ₁，θ₂，...，θ_n]，θ_i∈[θ_i，min，θ_i，max]轉化為x＝[x₁，x₂，...，x_n]，x_i∈[0，1]。

較佳的，所述步驟2中中間路點朝向范圍的具體計算方法為：

過入點作交接區域的后導航圓的切線，切線將以入點為中心的360°角度范圍分割成兩半，取靠近終點的180°角度范圍作為入點的朝向范圍；過出點作交接區域的前導航圓的切線，切線將以出點為中心的360°角度范圍分割成兩半，取靠近終點的180°角度范圍作為出點的朝向范圍。

本發明的有益效果是：

第一、本發明提供了一種面向多導航站接力導航的運動體路徑規劃方法，采用多導航站接力導航這種具有前沿性的先進導航方式，打破了單個導航站導航范圍小的局限，有效延伸了運動體的可控移動范圍；

第二、本發明采用角度編碼的方式，通過角度(一個變量)代替坐標(兩個變量)來表示路點的位置，既壓縮了解空間，又便于路徑表示和約束處理；

第三、本發明采用部分編碼方案，僅對路點的位置進行編碼優化，朝向在其范圍內隨機生成；這種方案具有兩個主要優點：一是便于差分進化算法的操作，降低了操作的維數，二是如果朝向也進行優化，因為路點的位置決定路點的朝向范圍，當路點的位置確定后，還需要大量的迭代次數去優化路點的朝向，從而導致整體路徑規劃的迭代次數為位置迭代次數與朝向迭代次數的乘積，計算量過大，時間成本過高，而采用這種部分編碼方案顯著減小了計算代價，有利于提高路徑規劃的效率；

第四、本發明采用外部引導與運動體自身導航相結合的方式，精度高，可靠性強，即使在導航站范圍縮小的情況下，仍有可能為運動體規劃出一條可行的路徑；

第五、本發明中的路徑規劃方法適用范圍廣，導航站可以是天基、地基、機載、艦載等，運動體可以是無人機、無人車、無人水下航行器等，可用于實現海、陸、空不同情形多約束條件下運動體從起點到終點的路徑規劃。

附圖說明

圖1為面向多導航站接力導航的運動體路徑規劃流程圖；

圖2為導航交接約束示意圖；

圖3為運動體曲率約束示意圖；

圖4為入點位置表示方法示意圖；

圖5為出點位置表示方法示意圖；

圖6為路點朝向范圍示意圖；

圖7為仿真實驗一結果圖；

圖8為仿真實驗二結果圖。

具體實施方式

下面結合附圖和實施例對本發明作詳細說明。

本發明提供了一種面向多導航站接力導航的運動體路徑規劃方法。該方法是在考慮導航站導航范圍約束、導航交接約束和運動體曲率約束的前提下，為運動體規劃出一條能夠各種滿足各種約束條件且快速從起點抵達終點的水平二維空間路徑。本發明中路徑規劃為連續域約束優化問題，一些具有較強全局搜索能力的智能優化算法適用于解決該問題，本發明以差分進化算法作為面向多導航站接力導航的運動體路徑規劃的優化求解算法；

如圖1所示，所述面向多導航站接力導航的運動體路徑規劃方法具體步驟如下：

步驟1、輸入路徑規劃信息，具體包括：運動體起點和終點位置信息、導航站導航范圍約束信息、導航站導航交接約束信息和運動體曲率約束信息；所述運動體起點和終點位置信息是指起點和終點的二維坐標；所述導航站導航范圍約束信息是指導航站中心的坐標及有效作用范圍，運動體從起點到終點的整條路徑須全部位于導航圓內；所述導航圓表示導航站有效作用范圍，半徑為r；所述導航站導航交接約束信息是指確保導航交接成功而設定的運動體在交接區域內的最短路徑長度D；所述導航交接是指多個導航站之間移交運動體的導航權；所述交接區域是指兩個導航圓之間的重疊區域；所述運動體曲率約束信息是指運動體最小轉彎半徑r_tc，路點之間采用限定曲率的Dubins路徑連接；

如圖2所示，運動體的起點位于第一個導航站的導航圓內，終點位于第三個導航站的導航圓內，因此，運動體需由三個導航站進行接力導航；運動體的路徑由從起點到終點的多個路點并依此連接各路點形成的路段1～路段5構成，其中路段2和路段4為運動體在交接區域內的路徑；l_2j(j＝1,2)表示第j個交接區域內運動體路徑長度，若l_2j≥D(j＝1,2)且交接區域內運動體的路徑不超出交接區域范圍，則該路徑滿足導航交接約束；

如圖3所示，A和B表示路點，箭頭表示運動體朝向，所述運動體曲率約束是指運動體采用限定曲率的Dubins路徑實現其水平二維空間內朝向和位置的變化，即路段由直線段和半徑為運動體最小轉彎半徑r_tc的圓上的弧組成；

步驟2、隨機生成初始路徑種群；

定義入點為位于入弧上的中間路點，所述入弧為運動體進入交接區域的邊界弧；定義出點為位于出弧上的中間路點，所述出弧為運動體離開交接區域的邊界弧；入點位置表示方法如圖4所示，將交接區域的后導航圓的圓心作為極點，水平方向向右為極軸，逆時針為正方向，建立局部極坐標系，入點的位置用極角θ和極徑ρ表示，由于交接區域的后導航圓的半徑已知，且入點位于交接區域后導航圓的圓周上，因此，入點的位置可用θ一個變量來表示；出點位置表示方法如圖5所示，將交接區域的前導航圓的圓心作為極點，水平方向向右為極軸，逆時針為正方向，建立局部極坐標系，出點的位置用極角θ和極徑ρ表示，由于交接區域的前導航圓的半徑已知，且出點位于交接區域的前導航圓的圓周上，因此，出點的位置用θ一個變量來表示；由此，一條路徑所有中間路點的位置表示為θ＝[θ₁,θ₂,...,θ_n]，限定θ_i的取值范圍為[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max為交接區域頂點所對應的極角；所述交接區域頂點為入弧與出弧的交點；本發明采取相對編碼的方式，將中間路點位置的解范圍映射到[0,1]區間，即將

θ＝[θ₁,θ₂,...,θ_n](θ_i∈[θ_i,min,θ_i,max])轉化為x＝[x₁,x₂,...,x_n](x_i∈[0,1])；

一條路徑由起點位置、終點位置和中間路點的位置及朝向共同確定；

中間路點與中間路點之間采用Dubins路徑連接；所述Dubins路徑是指對于在同一平面上具有朝向的任意兩點，在限定曲率的情況下，采用幾何方法計算出的由一點到達另一點且具有朝向的最短路徑；本發明主要采用Dubins路徑的“CLC”路徑，第一個C表示Dubins路徑起點輔助圓上的圓弧，第二個C表示Dubins路徑終點輔助圓上的圓弧，L表示兩條圓弧的切線；所述Dubins路徑起點輔助圓是指以Dubins路徑起點為切點、以r_tc為半徑作的兩個輔助圓，稱之為Dubins路徑起點左轉彎圓和右轉彎圓；所述Dubins路徑終點輔助圓是指以Dubins路徑終點為切點、以r_tc為半徑作的兩個輔助圓，稱之為Dubins路徑終點左轉彎圓和右轉彎圓；所述Dubins路徑的“CLC”路徑共有4種情形，分別為“Dubins路徑起點左轉彎圓圓弧-切線段-Dubins路徑終點左轉彎圓圓弧”、“Dubins路徑起點左轉彎圓圓弧-切線段-Dubins路徑終點右轉彎圓圓弧”、“Dubins路徑起點右轉彎圓圓弧-切線段-Dubins路徑終點左轉彎圓圓弧”和“Dubins路徑起點右轉彎圓圓弧-切線段-Dubins路徑終點右轉彎圓圓弧”，Dubins路徑算法在這4種情形中選擇長度最短的那條路徑作為最終的Dubins路徑；

起點與中間路點之間采用首端朝向松弛的Dubins路徑連接；所述首端朝向松弛是指路徑首端朝向可以是任意方向；所述首端朝向松弛的Dubins路徑共有2種情形，分別為“切線-Dubins路徑終點左轉彎圓圓弧”和“切線-Dubins路徑終點右轉彎圓圓弧”；首端朝向松弛的Dubins路徑算法在這2種情形中選擇長度最短的那條路徑作為最終的Dubins路徑；

中間路點與終點之間采用終端朝向松弛的Dubins路徑連接；所述終端朝向松弛是指路徑終端朝向可以是任意方向；所述終端朝向松弛的Dubins路徑共有2種情形，分別為“Dubins路徑起點左轉彎圓圓弧-切線”和“Dubins路徑起點右轉彎圓圓弧-切線”；終端朝向松弛的Dubins路徑算法在這2種情形中選擇長度最短的那條路徑作為最終的Dubins路徑；

將中間路點設置在交接區域邊界弧上，隨機生成多條路徑組成初始路徑種群，其中，中間路點的位置在所屬邊界弧上隨機生成，中間路點的朝向范圍由其位置所決定，具體朝向在其朝向范圍內隨機生成；

中間路點的朝向范圍如圖6所示，過入點作交接區域的后導航圓的切線，切線將以入點為中心的360°角度范圍分割成兩半，取靠近終點的180°角度范圍作為入點的朝向范圍；過出點作交接區域的前導航圓的切線，切線將以出點為中心的360°角度范圍分割成兩半，取靠近終點的180°角度范圍作為出點的朝向范圍；

步驟3、評價路徑；評價指標包括路徑總長度和約束違反程度；所述路徑總長度是指按照導航順序依次連接起點、中間路點、終點所形成的全部路段長度之和；所述路段是指兩個相鄰路點之間的路徑；所述約束違反程度是指導航交接約束違反程度，當運動體在交接區域內的路徑長度大于或等于約束值時，約束違反程度為0，否則，約束違反程度為兩者的絕對差；

步驟4、記錄初始種群的最優路徑；若初始種群中存在約束違反程度為0的路徑，則在約束違反程度為0的路徑中選取路徑總長度最短的那條路徑作為初始種群的最優路徑，若初始種群所有路徑的約束違反程度均大于0，則選取約束違反程度最小的那條路徑作為初始種群的最優路徑；

步驟5、采用差分進化算法優化路徑；首先隨機生成初始路徑種群并評價，記錄當前種群的最優路徑；然后進入到算法的主循環，反復執行進化操作，直至達到算法終止條件，最后輸出當前種群的最優路徑作為路徑規劃問題的較優解；一次完整的循環包括：變異、交叉、選擇和更新當前種群的最優路徑；首先通過變異、交叉操作產生新路徑，其中，變異和交叉操作僅作用于路點的位置，路點的朝向范圍隨其位置的改變而改變，朝向在其范圍內隨機生成；然后在新路徑和舊路徑中進行選擇，留下較優的路徑；選擇操作的原則為：當兩條路徑的約束違反程度相等時，留下路徑總長度較短的那條路徑，當兩條路徑的約束違反程度不相等時，留下約束違反程度較小的那條路徑；將選擇操作留下的路徑與之前記錄的種群最優路徑進行比較，比較方法與選擇操作的原則相同，將兩者中較優的一條作為當前種群的最優路徑；當種群進化代數達到給定的最大進化代數時，終止循環過程，輸出當前種群的最優路徑，完成路徑規劃。

下面結合仿真實驗結果對多導航站接力導航下運動體的路徑規劃方法進行說明；

如圖7所示，導航站中心坐標為(0,0)、(1,0)、(2,1)，導航圓的半徑為r＝1，運動體起點坐標為(-1,0)，終點坐標為(3,1)，運動體最小轉彎半徑為r_tc＝0.2，導航交接約束為D＝[0.2,0.1]，差分進化算法種群規模為NP＝50，最大進化代數為Gen＝600；由于導航交接約束較小，理論最優路徑為從起點到終點的一條直線段，路徑總長度為4.1231；由仿真實驗結果可知，算法得到的路徑幾乎為一條直線段，路徑總長度為4.1251，相對誤差為0.0485％，誤差較小，十分逼近理論最優路徑；

如圖8所示，對于導航交接約束值較大的情況，導航站中心坐標設為(0,0)、(1.5,0)、(3,0)，導航圓的半徑為r＝1，運動體起點坐標為(-1,0)，終點坐標為(3,0)，運動體最小轉彎半徑為r_tc＝0.3，導航交接約束為D＝[0.8689,0.8689]，差分進化算法種群規模為NP＝50，最大進化代數為Gen＝1000，得到的路徑規劃結果滿足所有約束條件，且運動體在交接區域內的路徑，即路段2(0.8959)與路段4(0.8803)的長度十分接近導航交接約束值，相對誤差小于3.1％，總路徑長度較小，得到了較優的路徑規劃結果；

以上公開的實施例在以本發明技術方案為前提下進行實施，給出了詳細的實施方式和具體的操作過程，但本發明的保護范圍不限于所述的實施例。通過以上所述可知，本發明中的許多內容可作修改和替換，本實施例固定了某些取值只是為了更好地說明本發明的原理和應用，從而更易理解和運用。凡在本發明的技術方案的基礎上所做的局部改動、等同替換、改進等均應包含在本發明的保護范圍之內。