[發(fā)明專(zhuān)利]基于Dubins曲線的多車(chē)隊(duì)形切換方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011371251.2
• 申請(qǐng)日: 2020-11-30
• 公開(kāi)（公告）號(hào): CN112596516B
• 公開(kāi)（公告）日: 2022-11-18
• 發(fā)明（設(shè)計(jì)）人: 皮大偉;萬(wàn)晨曦;王洪亮;謝伯元;王爾烈;王顯會(huì);王霞;孫曉旺
• 申請(qǐng)（專(zhuān)利權(quán)）人: 南京理工大學(xué)
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02
• 代理公司: 南京理工大學(xué)專(zhuān)利中心 32203
• 代理人: 張玲

1.一種基于Dubins曲線的多車(chē)隊(duì)形切換方法，其特征在于，包括如下步驟：

步驟(1)：在接到變換指令后，車(chē)隊(duì)系統(tǒng)根據(jù)幾何約束計(jì)算目標(biāo)點(diǎn)，系統(tǒng)應(yīng)用車(chē)輛運(yùn)動(dòng)學(xué)模型和Dubins曲線在一定速度范圍內(nèi)進(jìn)行規(guī)劃，得到時(shí)間序列T_s，并得到最短的行駛時(shí)間T_c以及其軌跡隊(duì)形的路徑長(zhǎng)度S_c和速度V_c；

步驟(2)：計(jì)算領(lǐng)航車(chē)輛行駛T_c內(nèi)的平均速度V_L，以及行駛的路程長(zhǎng)度S_L，最終位置；隨后依據(jù)隊(duì)形要求計(jì)算出跟隨車(chē)輛的新目標(biāo)點(diǎn)P_pre；

步驟(3)：根據(jù)步驟(2)得到的新目標(biāo)點(diǎn)P_pre進(jìn)行重規(guī)劃并迭代選取最接近耗費(fèi)時(shí)間的軌跡從而得到最優(yōu)軌跡；

步驟(4)：根據(jù)步驟(3)得到的最優(yōu)軌跡，進(jìn)行軌跡跟蹤，并且在運(yùn)動(dòng)過(guò)程中進(jìn)行終止條件判斷；

步驟(5)：達(dá)到判定的終止條件后，車(chē)隊(duì)系統(tǒng)進(jìn)入隊(duì)形保持階段。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟(3)具體步驟如下：

步驟(3-1)：已知行駛時(shí)間T_c反算Dubins曲線，獲得車(chē)輛從當(dāng)前位置到新目標(biāo)點(diǎn)P_pre的Dubins曲線長(zhǎng)度S_pre和此時(shí)Dubins曲線的速度V_pre，最大轉(zhuǎn)彎半徑R；

步驟(3-2)：假如V_pre≤V_max則車(chē)輛執(zhí)行V_pre，否則系統(tǒng)剔除當(dāng)前路徑曲線并且從步驟(1)開(kāi)始循環(huán)，選取次小的時(shí)間記為T(mén)_c，直至滿(mǎn)足條件結(jié)束循環(huán)，其中V_max為車(chē)輛速度最大值。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述步驟(3)“行重規(guī)劃并迭代選取最接近耗費(fèi)時(shí)間的軌跡從而得到最優(yōu)軌跡”采用如下算法整合公式：

滿(mǎn)足以下條件：

T(Dubins(pstart,pend,R),V)＝Dubins/V

其中path表示跟隨車(chē)輛的最終軌跡，Dubins表示軌跡，T(Dubins)表示該軌跡所需時(shí)間，R為車(chē)輛最小轉(zhuǎn)彎半徑，P表示跟所車(chē)輛位置，下標(biāo)start和end分別表示起始和最終，aL為車(chē)輛最大橫向加速度，一般取0.4g，K為道路曲率，V為車(chē)輛速度向量，下標(biāo)min，max分別表示最小值和最大值，ΔV為速度在時(shí)間間隔T的變化量。

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述步驟(4)中“進(jìn)行軌跡跟蹤”的軌跡跟蹤控制方法為MPC；

變換的終止條件如下：

||P-Po||≤0.5L，cos(φ-α)≥0.8；

P為車(chē)輛當(dāng)前時(shí)刻位置，Po為目標(biāo)點(diǎn)位置，后者cos(φ-α)為領(lǐng)航車(chē)和跟隨車(chē)輛的航向角之差，L為隊(duì)形的距離要求，φ為跟隨車(chē)輛航向角，α為領(lǐng)航車(chē)航向角。

5.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述步驟(5)中“隊(duì)形保持階段”的隊(duì)形保持具體步驟為：

步驟(5-1)：將車(chē)輛運(yùn)動(dòng)學(xué)模型及領(lǐng)航跟隨者模型線性離散化；

步驟(5-2)：構(gòu)建MPC框架，融合自身軌跡及隊(duì)形要求設(shè)計(jì)代價(jià)函數(shù)，結(jié)合ORCA理論設(shè)計(jì)碰撞約束。

6.根據(jù)權(quán)利要求5所述的方法，其特征在于，所述步驟(5-2)中的代價(jià)函數(shù)J(k)如下：

Q，R和ρ分別是加權(quán)因子，η為隊(duì)形及軌跡列向量，η_ref為參考向量，ε為大于零的正實(shí)數(shù)，T為當(dāng)前時(shí)刻，k+i為預(yù)測(cè)時(shí)刻，H_p是預(yù)測(cè)時(shí)刻總數(shù)，H_c為控制時(shí)刻總數(shù)，函數(shù)在第一項(xiàng)中考慮車(chē)輛自身的軌跡誤差和編隊(duì)誤差；第二項(xiàng)限制車(chē)速和轉(zhuǎn)向加速度，以使車(chē)輛平穩(wěn)行駛，應(yīng)用第三項(xiàng)以確保優(yōu)化問(wèn)題具有可行的解決方案；車(chē)輛的輸入受到極限值的限制，并且在T時(shí)間段內(nèi)輸入的變化也需要被限制以確保車(chē)輛的穩(wěn)定性；

碰撞約束如下：

u_min≤u_k,t≤u_max △u_min≤△u_k,t≤△u_max

u為輸入列向量，包含車(chē)輛的速度和角速度，u_k,t與Δu_k,t分別表示u(k|t)和Δu(k|t)；

避免碰撞約束條件如下：

||P_A-P_B+κT(V_A-V_B)||r_A+r_B

其中P為車(chē)輛位置，r為車(chē)輛的圓形模型的輪廓半徑，下標(biāo)A和B表示A車(chē)和B車(chē)，κ是合作決策系數(shù)，它取決于兩輛車(chē)的決策措施，隨著B(niǎo)采取減速等主動(dòng)防撞措施，κ的值會(huì)增加，在大多數(shù)情況下κ為1.5，A和B的性能相同。