一種基于環(huán)境不確定性的無人車動態(tài)路徑規(guī)劃方法
【技術領域】
[0001]本發(fā)明涉及無人駕駛汽車路徑規(guī)劃領域，尤其是涉及一種基于環(huán)境不確定性的無 人車動態(tài)路徑規(guī)劃方法。
【背景技術】
[0002] 隨著計算機技術和人工智能的發(fā)展，無人駕駛汽車（以下簡稱無人車)在軍事、交 通、工業(yè)生產、物流倉儲、日常生活等方面展現(xiàn)出巨大的應用前景。在國防軍事方面，無人車 多用于執(zhí)行危險場景下的軍事任務，如軍事救援和物資輸送等。在交通安全方面，無人駕駛 技術是促進智能交通系統(tǒng)發(fā)展的有效手段，基于人工智能的無人駕駛技術可以提高車輛行 駛的主動安全性，能夠有效減少駕駛員由于誤操作導致的交通事故，從而提高交通行駛效 率和安全性。在工業(yè)生產、物流倉儲方面，無人車可以配合自動化生產線實現(xiàn)全自主無人生 產，進一步推進工業(yè)生產的自動化和智能化，進而提高生產效率。另外，無人車的出現(xiàn)也將 極大地方便人們的工作、旅游等日常生活。
[0003] 無人駕駛技術主要包括環(huán)境信息的感知，駕駛行為的智能決策，無碰撞路徑的規(guī) 劃，以及車輛的運動控制等四個部分。路徑規(guī)劃是無人駕駛技術中十分關鍵的組成部分，為 環(huán)境感知和運動控制起著承上啟下的作用。基于感知系統(tǒng)的環(huán)境數(shù)據(jù)，車輛需要在復雜的 道路環(huán)境中，基于一定的性能指標(安全性最高、車輛可行駛、能量最優(yōu)等)規(guī)劃出一條安全 可靠的、車輛可行駛的、從起始位置到目標位置的最短無碰撞路徑。
[0004] 移動機器人的路徑規(guī)劃問題由來已久，且很多學者已經提出了許多成熟的方法。 但近10年來，隨著無人駕駛技術的迅速發(fā)展，無人車的路徑規(guī)劃問題再次成為國內外學者 最為關注的問題之一。
[0005] 基于啟發(fā)式的搜索算法被用于無人車的路徑規(guī)劃。最常見的有A*、D*算法，分別用 于解決靜態(tài)和動態(tài)柵格環(huán)境下的路徑規(guī)劃問題。通過引入起始點到目標點之間的啟發(fā)函 數(shù)，提出了基于柵格化方法的A*算法。A*對起始柵格與目標柵格之間的距離以及柵格的被 占用情況進行估價，向鄰居節(jié)點中估價最小的柵格擴散而最終到達目標柵格。A*算法在全 局環(huán)境信息已知、障礙物為靜態(tài)的情況下，能夠很好的規(guī)劃出一條無碰撞的路徑。針對復雜 多變的動態(tài)環(huán)境，提出了動態(tài)A*算法（即D*算法），首先進行全局靜態(tài)規(guī)劃，在環(huán)境沒有更新 時，不斷保持上一次的搜索結果，當有新的障礙物出現(xiàn)時，對搜索結果進行修正，從而實現(xiàn) 動態(tài)避障。在柵格化環(huán)境中，只要最短路徑存在，A*和D*就能夠求得最短路徑，但是在兩種 算法中只是將機器人看成質點，并沒有考慮機器人的結構特性和模型約束，生成的路徑不 夠平滑，可能導致路徑不可行，另外隨著柵格數(shù)目的增多，算法的運算量會急劇增加。
[0006] 人工勢場法等、遺傳算法和神經網絡等人工智能方法也被應用于無人車的路徑規(guī) 劃。人工勢場法通過引進勢場的概念，分別建立與目標點和障礙物之間的引力場和斥力場， 機器人在引力場和斥力場的共同作用下沿著勢場減小的方向行駛。但是這種方法的缺陷是 存在極小值點，從而導致機器人陷入死區(qū)而不能到達目標點。部分學者將路徑規(guī)劃問題轉 化為多目標優(yōu)化問題，將路徑的快速性、平順性等作為優(yōu)化指標，通過遺傳迭代求得最優(yōu) 解，實現(xiàn)避障。神經網絡方法也應用于無人車的路徑規(guī)劃，外部的輸入激活相應的神經元， 從而執(zhí)行相應的動作到達相應的位置，但算法的運算量會隨神經網絡規(guī)模的增大急劇增 加，算法的運算效率較低。
[0007] 無人車雖然屬于移動機器人的一種，但是由于其具有特定的結構和運動學特性， 在進行無人車的路徑規(guī)劃時，勢必要考慮模型的約束，也就是說，不滿足模型約束的路徑無 人車是無法被跟蹤的。另一方面，在實際環(huán)境中，無人車的行駛環(huán)境復雜多變，無人車對于 環(huán)境信息、特別是動態(tài)障礙物的運動很難具有先驗的知識，這也就要求無人車在路徑規(guī)劃 時要對動態(tài)多變的環(huán)境進行預測和威脅評估，考慮動態(tài)環(huán)境的不確定性，不僅要評估已有 的威脅還要評估潛在的威脅，基于此進行路徑規(guī)劃，才能保障規(guī)劃路徑的安全性。
[0008] 中國專利CN104933228A公開了一種基于速度障礙的無人車實時軌跡規(guī)劃方法，將 動態(tài)環(huán)境中的運動障礙物投影到車輛的速度空間，并將三次多項式函數(shù)光滑可控基元中的 速度變量設置為最優(yōu)速度，從而構造搜索三維位形空間的啟發(fā)式函數(shù)，進而在車輛的位形 空間及速度空間進行搜索實現(xiàn)軌跡規(guī)劃。中國專利CN102591332A公開了一種用于無人駕駛 汽車局部路徑規(guī)劃的裝置及方法，包括傳感器裝置以及基于人工勢場法的局部路徑規(guī)劃方 法，通過視覺傳感器探測道路邊界并計算出道路中心線，通過雷達探測障礙物信息，并設計 了當前位置距離車道中心線的引力函數(shù)和距離障礙物的斥力函數(shù)，進而計算合力的方向， 通過高斯組合隸屬度函數(shù)系數(shù)的方法，解決了可能會陷入局部極小和路徑震蕩的問題。以 上兩個專利并沒有考慮到車輛所具有的結構特性和運動學特性以及對于移動障礙物的處 理，并不能反映障礙物的運動特性，且沒有考慮移動障礙物的大小和運動的不確定性，也沒 有對移動障礙物運動軌跡進行預測，難以反映移動障礙物的潛在威脅。

【發(fā)明內容】

[0009] 本發(fā)明的目的就是為了克服上述現(xiàn)有技術存在的缺陷而提供一種基于環(huán)境不確 定性的無人車動態(tài)路徑規(guī)劃方法，不僅可以滿足車輛行駛的安全性要求，還能夠在滿足車 輛模型約束的情況下保證行駛效率，通過不同的權重分配實現(xiàn)性能指標的協(xié)調優(yōu)化，同時 在多個動態(tài)障礙物存在條件下實現(xiàn)實時規(guī)劃，有效提高無人車行駛的安全性。
[0010] 本發(fā)明的目的可以通過以下技術方案來實現(xiàn)：
[0011] -種基于環(huán)境不確定性的無人車動態(tài)路徑規(guī)劃方法包括以下步驟：
[0012] S1:建立車輛運動學模型，無人車和障礙物均滿足車輛運動學模型，所述車輛運動 學模型滿足以下公式：
[0015]其中，X為車輛運動狀態(tài)，（x，y)為以車輛后軸中心點為原點建立的坐標系下的橫 向坐標和縱向坐標，Θ為車輛相對于坐標系X軸的航向角，v和δ分別為車輛的速度和前輪轉 角，1為車輛前軸與后軸之間的距離，i、v沒、分別對應為x、y、9、v的一階求導；
[0016] S2:建立表征無人車運動環(huán)境的動態(tài)環(huán)境模型，并根據(jù)動態(tài)環(huán)境模型建立重新規(guī) 劃路徑的滿足條件；
[0017] S:獲取無人車的車輛iiz辦It態(tài)起始值^哞輛態(tài)初始目標值兔，并根據(jù)X獲取多個車輛 @ > ·?? =K.>^o*^'??^%Λ]，***-[* ? 0 ο,-ν^,Γ? 4=[4..v*,??.4'?.?]^> 4 = \.，《=丨，.-4 下標〇表示起始值，下標g表示初始目標值，下標h表示候選目標 值，上標i表示第i個候選目標值，d為設定的橫向間隔；
[0018] S4:基于車輛運動學模型，生成從X0到4的候選路徑；
[0019] S5:評估候選路徑的安全性指標和快速性指標，基于安全性指標和快速性指標從 各候選路徑中選取得到最優(yōu)路徑；
[0020] S6:跟蹤無人車以最優(yōu)路徑運動的過程，當無人車運動環(huán)境滿足重新規(guī)劃路徑的 滿足條件時，跳轉步驟S3，重新規(guī)劃無人車的最優(yōu)路徑。
[0021 ]所述動態(tài)環(huán)境模型包括：
[0022] 1)建立坐標系，用圓心位于(Y，y')、半徑為V的圓表示無人車，無人車速度/ = [ν' x，/ y ]τ，其中，/ x、/ j別表示/在X軸和Y軸上的速度分量；
[0023] 2)用圓心位于(^^^^八半徑為^的圓表示障礙物^表示障礙物的編號^章礙物 速度￥。,。=[￥。,。,^。4,\] 1'，其中，￥。^￥。4,\分別表示￥。,。在乂軸和7軸上的速度分量；
[0024] 3)以（X7 )為中心、ri為半徑的區(qū)域定義為危險區(qū)域，r<ri<rd，rd為設定值，以 以Y )為中心、?為半徑的區(qū)域定義為警示區(qū)域，rd<r2<ra，ra為設定值；
[0025] 4)定義障礙物正在靠近的滿足條件為：Δ Vy · Δ y<〇, Δ Vy表示無人車與障礙物的 橫向相對速度，△ Ay表示無人車與障礙物的橫向相對距離， y7 )〇
[0026] 所述重新規(guī)劃路徑的滿足條件為：
[0027] 1)障礙物進入危險區(qū)域；
[0028] 2)障礙物進入警示區(qū)域且障礙物正在靠近；
[0029] 3)當肖U最優(yōu)路徑被跟蹤完畢；
[0030] 4)最優(yōu)路徑與障礙物的運動軌跡出現(xiàn)相交；
[0031] 當無人車運動環(huán)境滿足重新規(guī)劃路徑的滿足條件中的至少一條時，重新規(guī)劃無人 車的最優(yōu)路徑。
[0032] 所述步驟S3中車輛運動狀態(tài)初始目標值xg中的速度目標值^與道路的曲率和道路 的限速有關。
[0033] 所述步驟S4具體為：
[0034] 41:設車輛的軌跡(Χ(3，^)為六階多項式，滿足以下公式：
[0036]其中，t為時間，ak、bk為待定系數(shù)；
[0037] 42:結合車輛運動學模型和六階多項式，根據(jù)x〇和4將車輛的軌跡(xe，y e)表示為：
[0044]其中，ie(·)表示對Xe( ·)的一階求導，毛(·)表示對Xe( ·)的二階求導，九(·)表示 對ye( ·)的一階求導，九0表示對ye( ·)的二階求導；
[0045] 43:定義性能指標J(xe，ye)為偏離連接幼和知的曲線的偏差之和，滿足以下公式：
[0048] 其中，xn=xn(t)，yn = yn(t)，（xn，yn)為連接X。和xg的曲線；
[0049] 44:根據(jù)xq和& ,聯(lián)立式⑶和式⑷求解得到待定系數(shù)ak、bk，待定系數(shù)ak、b k代入 公式(2)得到車輛的軌跡(Χ(3，％)，即從xo到&的候選路徑。
[0050] 所述步驟S5中根據(jù)代價函數(shù)從各候選路徑中選取得到最優(yōu)路徑，所述代價函數(shù)滿 足以下公式：
[0051 ] Ji - W1 J〇, i，cp+W2Jo, i，dev ( 5 )
[0052] 其中，心為從xo到的候選路徑的代價，W1為對應安全性指標的權重系數(shù)，w2為對 應快速性指標的權重系數(shù)，Jm#為從X0到4的候選路徑的安全性指標，Jndev為從X0到xi 的候選路徑的快速性指標，最優(yōu)路徑為最小的候選路徑。
[0053] 所述從xo到xl的候選路徑的安全性指標Ju#滿足以下公式：
[0055]其中，E-為在預測周期內無人車的位置分布，EV(3h為在預測周期內障礙物的位置 分布，EobsflEveh矣0表示在每個預測時刻障礙物的位