[發(fā)明專利]面向多導(dǎo)航站接力導(dǎo)航的運(yùn)動(dòng)體路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201611258329.3
• 申請(qǐng)日: 2016-12-30
• 公開（公告）號(hào): CN106679667B
• 公開（公告）日: 2018-02-06
• 發(fā)明（設(shè)計(jì)）人: 辛斌;漆鳴鳳;竇麗華;陳杰;宋冬雪
• 申請(qǐng)（專利權(quán)）人: 北京理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 北京理工大學(xué)專利中心11120
• 代理人: 李微微,仇蕾安

1.一種面向多導(dǎo)航站接力導(dǎo)航的運(yùn)動(dòng)體路徑規(guī)劃方法，其特征在于，包括如下步驟：

步驟0、輸入路徑規(guī)劃信息，具體包括：運(yùn)動(dòng)體起點(diǎn)和終點(diǎn)位置信息、導(dǎo)航站導(dǎo)航范圍約束信息、導(dǎo)航站導(dǎo)航交接約束信息和運(yùn)動(dòng)體曲率約束信息；所述導(dǎo)航站導(dǎo)航范圍約束信息是指導(dǎo)航站中心的坐標(biāo)及有效作用范圍，即運(yùn)動(dòng)體從起點(diǎn)到終點(diǎn)的整條路徑須全部位于導(dǎo)航圓內(nèi)；所述導(dǎo)航圓表示導(dǎo)航站有效作用范圍，半徑為r；所述導(dǎo)航站導(dǎo)航交接約束是指確保導(dǎo)航交接成功而設(shè)定的運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最短路徑長(zhǎng)度D；所述導(dǎo)航交接是指多個(gè)導(dǎo)航站之間移交運(yùn)動(dòng)體的導(dǎo)航權(quán)；所述交接區(qū)域是指兩個(gè)導(dǎo)航圓之間的重疊區(qū)域；所述運(yùn)動(dòng)體曲率約束信息是指運(yùn)動(dòng)體最小轉(zhuǎn)彎半徑r_tc，路點(diǎn)之間采用限定曲率的Dubins路徑連接；

步驟1、判斷導(dǎo)航交接約束的難易程度，并用難易系數(shù)γ_j表示難易程度；γ_j為第j個(gè)交接區(qū)域內(nèi)導(dǎo)航交接約束D_j與導(dǎo)航交接區(qū)域最長(zhǎng)Dubins路徑長(zhǎng)度l_max的比值；若γ_j＜0.7，采用基于Dubins路徑的路徑規(guī)劃方法，即采用步驟2至5的方法獲得最終路徑；若γ_j≥0.7，采用基于交接區(qū)域內(nèi)最長(zhǎng)路徑模式的Dubins路徑規(guī)劃方法，即采用步驟6獲得最終路徑；

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

其中，中間路點(diǎn)與中間路點(diǎn)之間采用Dubins的“CLC”路徑連接；

將中間路點(diǎn)設(shè)置在交接區(qū)域邊界弧上，隨機(jī)生成多條路徑組成初始路徑種群，其中，中間路點(diǎn)的位置在所屬邊界弧上隨機(jī)生成，中間路點(diǎn)的朝向范圍由其位置所決定，具體朝向在其朝向范圍內(nèi)隨機(jī)生成；

步驟3、評(píng)價(jià)路徑；評(píng)價(jià)指標(biāo)包括路徑總長(zhǎng)度和約束違反程度；所述路徑總長(zhǎng)度是指按照導(dǎo)航順序依次連接起點(diǎn)、中間路點(diǎn)、終點(diǎn)所形成的全部路段長(zhǎng)度之和；所述路段是指兩個(gè)相鄰路點(diǎn)之間的路徑；所述約束違反程度是指導(dǎo)航交接約束違反程度，當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑長(zhǎng)度大于或等于約束值時(shí)，約束違反程度為0，否則，約束違反程度為兩者的絕對(duì)差；

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

步驟5、采用差分進(jìn)化算法優(yōu)化路徑；首先通過變異、交叉操作產(chǎn)生新路徑，其中，變異和交叉操作僅作用于路點(diǎn)的位置，路點(diǎn)的朝向范圍隨其位置的改變而改變，朝向在其范圍內(nèi)隨機(jī)生成；

然后在新路徑和舊路徑中進(jìn)行選擇，留下較優(yōu)的路徑；選擇操作的原則為：當(dāng)兩條路徑的約束違反程度相等時(shí)，留下路徑總長(zhǎng)度較短的那條路徑，當(dāng)兩條路徑的約束違反程度不相等時(shí)，留下約束違反程度較小的那條路徑；將選擇操作留下的路徑與之前記錄的種群最優(yōu)路徑進(jìn)行比較，比較方法與選擇操作的原則相同，將兩者中較優(yōu)的一條作為當(dāng)前種群的最優(yōu)路徑；當(dāng)種群進(jìn)化代數(shù)達(dá)到給定的最大進(jìn)化代數(shù)時(shí)，終止循環(huán)過程，輸出當(dāng)前種群的最優(yōu)路徑，完成路徑規(guī)劃；

步驟6、采用基于交接區(qū)域內(nèi)最長(zhǎng)路徑模式的Dubins路徑規(guī)劃，具體包括如下步驟：

S401、計(jì)算各個(gè)交接區(qū)域的寬度rd，以及交接區(qū)域?qū)挾萺d與導(dǎo)航圓半徑r的比值所述交接區(qū)域的寬度是指交接區(qū)域入弧中點(diǎn)與出弧中點(diǎn)之間的距離；

S402、計(jì)算運(yùn)動(dòng)體最小轉(zhuǎn)彎半徑r_tc與導(dǎo)航圓半徑r的比值

S403、根據(jù)與通過查下表確定運(yùn)動(dòng)體在各個(gè)交接區(qū)域內(nèi)的路徑模式：

所述交接區(qū)域內(nèi)的路徑模式包括7種，分別定義如下：

模式1：雙頂點(diǎn)相切S型；該模式下，運(yùn)動(dòng)體在交接區(qū)域的入點(diǎn)和出點(diǎn)分別是交接區(qū)域的兩個(gè)頂點(diǎn)，運(yùn)動(dòng)體在入點(diǎn)的朝向與本交界區(qū)域?qū)?yīng)的第一個(gè)導(dǎo)航圓相切，運(yùn)動(dòng)體在出點(diǎn)的朝向與本交界區(qū)域?qū)?yīng)的第二個(gè)導(dǎo)航圓相切；運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“S”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；所述入點(diǎn)輔助圓是指以入點(diǎn)為切點(diǎn)，以r_tc為半徑作的兩個(gè)輔助圓，稱之為左轉(zhuǎn)彎圓和右轉(zhuǎn)彎圓；所述出點(diǎn)輔助圓是指以出點(diǎn)為切點(diǎn)，以r_tc為半徑作的兩個(gè)輔助圓，稱之為左轉(zhuǎn)彎圓和右轉(zhuǎn)彎圓；運(yùn)動(dòng)體從上頂點(diǎn)進(jìn)入交接區(qū)域，從下頂點(diǎn)離開交接區(qū)域，或者從下頂點(diǎn)進(jìn)入交接區(qū)域，從上頂點(diǎn)離開交接區(qū)域；

模式2：中心相切S型；該模式下，所述交接區(qū)域的中心為交接區(qū)域兩頂點(diǎn)連線與交接區(qū)域所屬的兩個(gè)導(dǎo)航圓圓心連線的交點(diǎn)；運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“S”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；入點(diǎn)輔助圓與出點(diǎn)輔助圓相切于交接區(qū)域的中心，直線段的長(zhǎng)度為0；運(yùn)動(dòng)體從上半段入弧進(jìn)入交接區(qū)域，從下半段出弧離開交接區(qū)域，或者從下半段入弧進(jìn)入交接區(qū)域，從上半段出弧離開交接區(qū)域；

模式3：?jiǎn)雾旤c(diǎn)相切S型；該模式下，運(yùn)動(dòng)體在交接區(qū)域的入點(diǎn)和出點(diǎn)為一個(gè)位于交接區(qū)域的頂點(diǎn)，若入點(diǎn)位于交接區(qū)域的頂點(diǎn)，則其朝向與第一個(gè)導(dǎo)航圓相切，若出點(diǎn)位于交接區(qū)域的頂點(diǎn)，則其朝向與第二個(gè)導(dǎo)航圓相切；運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“S”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；運(yùn)動(dòng)體從上半段入弧進(jìn)入交接區(qū)域，從下半段出弧離開交接區(qū)域，或者從下半段入弧進(jìn)入交接區(qū)域，從上半段出弧離開交接區(qū)域；

模式4：雙頂點(diǎn)相切C型；該模式下，運(yùn)動(dòng)體在交接區(qū)域的入點(diǎn)和出點(diǎn)分別是交接區(qū)域的兩個(gè)頂點(diǎn)，運(yùn)動(dòng)體在入點(diǎn)和出點(diǎn)的朝向要么都與第一個(gè)導(dǎo)航圓相切，要么都與第二個(gè)導(dǎo)航圓相切；運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“C”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；運(yùn)動(dòng)體從上頂點(diǎn)進(jìn)入交接區(qū)域，從下頂點(diǎn)離開交接區(qū)域，或者從下頂點(diǎn)進(jìn)入交接區(qū)域，從上頂點(diǎn)離開交接區(qū)域；

模式5：勺型；該模式下，運(yùn)動(dòng)體在交接區(qū)域的入點(diǎn)和出點(diǎn)只有一個(gè)位于交接區(qū)域的頂點(diǎn)，運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“勺子”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；運(yùn)動(dòng)體從上半段入弧進(jìn)入交接區(qū)域，從下半段出弧離開交接區(qū)域，或者從下半段入弧進(jìn)入交接區(qū)域，從上半段出弧離開交接區(qū)域；

模式6：?jiǎn)雾旤c(diǎn)相切C型；該模式下，運(yùn)動(dòng)體在交接區(qū)域的入點(diǎn)和出點(diǎn)只有一個(gè)位于交接區(qū)域的頂點(diǎn)，若入點(diǎn)位于交接區(qū)域的頂點(diǎn)，則入點(diǎn)朝向與第二個(gè)導(dǎo)航圓相切，若出點(diǎn)位于交接區(qū)域的頂點(diǎn)，則出點(diǎn)朝向與第一個(gè)導(dǎo)航圓相切；運(yùn)動(dòng)體在交接區(qū)域內(nèi)的最長(zhǎng)路徑為Dubins的CLC路徑，形如“C”，其中第一個(gè)C表示入點(diǎn)輔助圓的圓弧段，第二個(gè)C表示出點(diǎn)輔助圓的圓弧段，L表示與兩條圓弧相切的直線段；運(yùn)動(dòng)體從上半段入弧進(jìn)入交接區(qū)域，從下半段出弧離開交接區(qū)域，或者從下半段入弧進(jìn)入交接區(qū)域，從上半段出弧離開交接區(qū)域；

模式7：循環(huán)轉(zhuǎn)圈圓型；運(yùn)動(dòng)體在交接區(qū)域內(nèi)循環(huán)轉(zhuǎn)圈移動(dòng)；

S404、由運(yùn)動(dòng)體所在非交接區(qū)域的前、后兩個(gè)相鄰交接區(qū)域內(nèi)的路徑模式確定運(yùn)動(dòng)體在非交接區(qū)域內(nèi)的路徑模式；

S405、對(duì)中間路點(diǎn)位置進(jìn)行編碼，具體為：中間路點(diǎn)包括入點(diǎn)和出點(diǎn)兩種，入點(diǎn)為位于入弧上的中間路點(diǎn)，入弧為運(yùn)動(dòng)體進(jìn)入交接區(qū)域的邊界弧；出點(diǎn)為位于出弧上的中間路點(diǎn)，所述出弧為運(yùn)動(dòng)體離開交接區(qū)域的邊界弧；

入點(diǎn)位置的表示方式如下：將交接區(qū)域的后導(dǎo)航圓的圓心作為極點(diǎn)，水平方向向右為極軸，逆時(shí)針為正方向，建立局部極坐標(biāo)系，入點(diǎn)的位置用極角表示；所述交接區(qū)域的后導(dǎo)航圓是指運(yùn)動(dòng)體即將進(jìn)入的導(dǎo)航圓；

出點(diǎn)位置的表示方法如下：將交接區(qū)域的前導(dǎo)航圓的圓心作為極點(diǎn)，水平方向向右為極軸，逆時(shí)針為正方向，建立局部極坐標(biāo)系，出點(diǎn)的位置用極角θ表示；所述交接區(qū)域的前導(dǎo)航圓是指運(yùn)動(dòng)體即將離開的導(dǎo)航圓；

由此，一條路徑所有中間路點(diǎn)的位置由入點(diǎn)極角和出點(diǎn)極角聯(lián)合表示為θ＝[θ₁,θ₂,...,θ_n]，θ_i表示第i個(gè)中間路點(diǎn)的位置，i為奇數(shù)時(shí)，表示為入點(diǎn)的極角，i為偶數(shù)時(shí)，表示為出點(diǎn)的極角；限定θ_i的取值范圍為[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max為交接區(qū)域頂點(diǎn)所對(duì)應(yīng)的極角；所述交接區(qū)域頂點(diǎn)為入弧與出弧的交點(diǎn)；采取相對(duì)編碼的方式，先將中間路點(diǎn)位置的解范圍映射到[0,1]區(qū)間，即將θ＝[θ₁,θ₂,...,θ_n]，θ_i∈[θ_i,min,θ_i,max]轉(zhuǎn)化為x＝[x₁,x₂,...,x_n]，x_i∈[0,1]；

S406、隨機(jī)生成初始路徑種群；

當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑模式為模式1時(shí)，中間路點(diǎn)位置種群初始化方法如下：對(duì)于第j個(gè)導(dǎo)航交接區(qū)域，隨機(jī)生成NP×γ_j個(gè)中間路點(diǎn)位置向量(x_2j-1,x_2j)，入點(diǎn)位置x_2j-1和出點(diǎn)位置x_2j滿足：

x_2j-1∈[0,1-γ_j]&x_2j∈[0,1-γ_j]或x_2j-1∈[γ_j,1]&x_2j∈[γ_j,1]，

以及隨機(jī)生成NP-NP×γ_j個(gè)不滿足以上條件的中間路點(diǎn)位置向量；所述NP表示種群規(guī)模；

當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑模式為模式2時(shí)，設(shè)模式2中最長(zhǎng)Dubins路徑的入點(diǎn)位置為σ₁或σ₂，同樣出點(diǎn)位置為σ₂或σ₁；中間路點(diǎn)位置種群初始化方法如下：對(duì)于第j個(gè)導(dǎo)航交接區(qū)域，隨機(jī)生成NP×γ_j個(gè)中間路點(diǎn)位置向量，入點(diǎn)位置x_2j-1和出點(diǎn)位置x_2j滿足：

x_2j-1∈[σ₁-σ₁(1-γ_j),σ₁+(0.5-σ₁)(1-γ_j)]且x_2j∈[σ₁-σ₁(1-γ_j),σ₁+(0.5-σ₁)(1-γ_j)]

或者x_2j-1∈[σ₂-(σ₂-0.5)(1-γ_j),σ₂+(1-σ₂)(1-γ_j)]

且x_2j∈[σ₂-(σ₂-0.5)(1-γ_j),σ₂+(1-σ₂)(1-γ_j)]，

以及隨機(jī)生成NP-NP×γ_j個(gè)不滿足以上條件的中間路點(diǎn)位置向量；

當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑模式為模式3～模式6時(shí)，中間路點(diǎn)位置種群初始化方法如下：對(duì)于第j個(gè)導(dǎo)航交接區(qū)域，隨機(jī)生成NP×γ_j個(gè)路點(diǎn)位置向量(x_2j-1,x_2j)，入點(diǎn)位置x_2j-1和出點(diǎn)位置x_2j滿足：

x_2j-1∈[0,1-γ_j]&x_2j∈[0,0.5]或x_2j-1∈[γ_j,1]&x_2j∈[0.5,1]，

以及隨機(jī)生成NP-NP×γ_j個(gè)不滿足以上條件的路點(diǎn)位置向量；

當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑模式為模式7時(shí)，中間路點(diǎn)位置種群初始化方法如下：以交接區(qū)域?qū)?yīng)的兩個(gè)導(dǎo)航圓的圓心作半徑為r-r_tc的同心圓，這兩個(gè)同心圓的公共區(qū)域即為循環(huán)轉(zhuǎn)圈圓的圓心位置范圍；對(duì)于第j個(gè)導(dǎo)航交接區(qū)域，在循環(huán)轉(zhuǎn)圈圓圓心位置范圍內(nèi)隨機(jī)生成NP個(gè)位置向量(x_2j-1,x_2j)，其中(x_2j-1,x_2j)表示循環(huán)轉(zhuǎn)圈圓相對(duì)編碼圓心坐標(biāo)；

S407、評(píng)價(jià)路徑；評(píng)價(jià)指標(biāo)包括路徑總長(zhǎng)度和約束違反程度；所述路徑總長(zhǎng)度是指按照導(dǎo)航順序依次連接起點(diǎn)、中間路點(diǎn)、終點(diǎn)所形成的全部路段長(zhǎng)度之和；所述路段是指兩個(gè)相鄰路點(diǎn)之間的路徑；所述約束違反程度是指導(dǎo)航交接約束違反程度，當(dāng)運(yùn)動(dòng)體在交接區(qū)域內(nèi)的路徑長(zhǎng)度大于或等于約束值時(shí)，約束違反程度為0，否則，約束違反程度為兩者的絕對(duì)差；其中，當(dāng)交接區(qū)域內(nèi)的路徑模式為模式7，運(yùn)動(dòng)體的路徑存在多條時(shí)，計(jì)算每條路徑的路徑總長(zhǎng)度和約束違反程度，并選擇其中最優(yōu)的路徑作為運(yùn)動(dòng)體的路徑；

S408、記錄初始種群的最優(yōu)路徑；若初始種群中存在約束違反程度為0的路徑，則在約束違反程度為0的路徑中選取路徑總長(zhǎng)度最短的那條路徑作為初始種群的最優(yōu)路徑，若初始種群所有路徑的約束違反程度均大于0，則選取約束違反程度最小的那條路徑作為初始種群的最優(yōu)路徑；

S409、采用步驟5的方法對(duì)S406的初始路徑種群進(jìn)行路徑優(yōu)化，最后輸出當(dāng)前種群的最優(yōu)路徑，完成路徑規(guī)劃。