本發(fā)明屬于智能車輛與輔助駕駛領(lǐng)域，特別涉及一種基于路徑-速度解耦的垂直泊車軌跡規(guī)劃方法。
背景技術(shù)：
：自動泊車系統(tǒng)能夠代替或者輔助駕駛員進(jìn)行泊車，對于提高泊車安全性，降低泊車的操縱難度有著十分重要的意義。自動泊車軌跡規(guī)劃問題是自動泊車系統(tǒng)的一個重要技術(shù)，其主要任務(wù)是在泊車場景中，為智能駕駛車輛提供從起始位姿到目標(biāo)位姿的一系列速度和方向盤轉(zhuǎn)角輸入。作為自動泊車系統(tǒng)重要組成部分,軌跡規(guī)劃結(jié)果的優(yōu)劣直接決定著自動泊車系統(tǒng)的實用性及總體性能，隨著應(yīng)用背景的不斷復(fù)雜化以及應(yīng)用領(lǐng)域的不斷擴(kuò)大，自動泊車系統(tǒng)軌跡規(guī)劃子系統(tǒng)一直向著更高層次的智能化目標(biāo)發(fā)展。自動垂直泊車軌跡規(guī)劃技術(shù)主要在考慮車輛的最小轉(zhuǎn)彎半徑等運動學(xué)特性的基礎(chǔ)上進(jìn)行規(guī)劃。目前，通常通過采用幾何軌線將泊車軌跡用帶參數(shù)的數(shù)學(xué)模型表示，同時將障礙物以及汽車運動學(xué)性能進(jìn)行約束化，從而利用非線性規(guī)劃離線生成最優(yōu)軌跡。在已知Reeds-Shepp路徑組成的前提情況下，也可以利用MinkowskiSum在生成的無碰撞區(qū)域內(nèi)選取最優(yōu)路徑，但這種方法沒有考慮線速度等動力學(xué)約束。神經(jīng)網(wǎng)絡(luò)和模糊控制器雖然不直接進(jìn)行軌跡規(guī)劃，但其仿熟練駕駛員的規(guī)則本身包含了許多用于參考的軌跡先驗信息，對泊車場景具有一定的要求，缺少靈活性。上述方法或者需要軌跡數(shù)學(xué)模型，或者采用開環(huán)離線規(guī)劃方式，缺乏靈活性、不能動態(tài)調(diào)整路徑。因此，采用無軌跡模型的在線軌跡規(guī)劃是智能車輛局部狹小環(huán)境中導(dǎo)航的趨勢。技術(shù)實現(xiàn)要素：本發(fā)明的目的是克服現(xiàn)有技術(shù)的缺點，提供一種基于路徑-速度解耦的垂直泊車軌跡規(guī)劃基本方法。該方法利用路徑-速度解耦將軌跡規(guī)劃問題分解為路徑規(guī)劃問題和速度規(guī)劃問題，在無需知道泊車軌跡模型的情況下，利用繞墻走策略和動態(tài)窗口法在線規(guī)劃，實時輸出自動泊車系統(tǒng)的速度和方向盤轉(zhuǎn)角控制量，提高了泊車軌跡規(guī)劃的智能化程度。為解決上述技術(shù)問題，本發(fā)明的采用的技術(shù)方案為：一種垂直泊車軌跡規(guī)劃方法，其特征在于，包括以下步驟：a.基于車輛運動學(xué)模型的轉(zhuǎn)向特性獲?。浩嚥捎冒⒖寺D(zhuǎn)向模型，則車輛轉(zhuǎn)向時，全車的轉(zhuǎn)向中心位于后輪軸線的延長線上，車輛的狀態(tài)由橫縱坐標(biāo)及航向角表示q＝(x,y,θ)，車輛的輸入為速度和方向盤轉(zhuǎn)角(v,α)，在泊車的低速行駛模式下忽略輪胎和地面的側(cè)滑，這樣，系統(tǒng)存在不能側(cè)向位移、不能原地轉(zhuǎn)向的非完整性約束，轉(zhuǎn)向時存在轉(zhuǎn)彎半徑，通過解析，可得到車輛轉(zhuǎn)向的兩個特性：前后輪運動軌跡是不重合的，轉(zhuǎn)彎半徑最小的點在內(nèi)側(cè)后車輪；其中，橫縱坐標(biāo)中橫坐標(biāo)x與車輛泊車完成后車身側(cè)邊平行，縱坐標(biāo)y與橫坐標(biāo)x在同一水平面且相互垂直；b.車輛軌跡規(guī)劃：根據(jù)步驟a中獲取的車輛轉(zhuǎn)向特性，將垂直泊車軌跡規(guī)劃解耦為路徑規(guī)劃和速度規(guī)劃；路徑規(guī)劃的具體方法為：b11.將垂直泊車地圖環(huán)境進(jìn)行凸包分割，形成凸多邊形障礙物邊緣；b12.計算離起始位姿q0＝(x0,y0,θ0)、終止位姿qe＝(xe,ye,θe)距離最近的障礙物點O0和Oe；b13.從O0和Oe沿初始姿態(tài)和終止姿態(tài)方向作車輛和障礙物邊緣作距離為w/2的等距線段，w為車寬，記錄線段曲率不連續(xù)的點為Oi，并計算在該點姿態(tài)角的變化量；b14.對于每個點Oi，設(shè)置一個與之對應(yīng)的中間等效狀態(tài)qi＝(xi,yi,θi)；該點位于線段qeOi和線段q0Oi延長線所包圍的區(qū)域內(nèi)；b15.求最短圓弧路徑q0qi和qeqi，且設(shè)置對應(yīng)的半徑為R1和R2；b16.設(shè)置一個最小碰撞距離dc，此時對應(yīng)的參考點的值為d＝w/2+dc；b17.計算最小碰撞距離時對應(yīng)的車輛路徑，獲得車輛路徑規(guī)劃；速度規(guī)劃的具體方法為：假設(shè)車輛本身存在速度和方向盤轉(zhuǎn)角限制如下：v∈[vmin,vmax],α∈[αmin,αmax]采用動態(tài)窗口法進(jìn)行速度規(guī)劃，假設(shè)動態(tài)窗口法單步模擬的時間間隔為Δt，那么加速度、轉(zhuǎn)角速度約束如下：其中，分別對應(yīng)最大加速度和方向盤轉(zhuǎn)角速度；采用考慮離參考路徑偏差的軌跡選取策略，選擇目標(biāo)函數(shù)代價最低的軌跡，目標(biāo)函數(shù)cost為：其中，pd,hdiff,gd分別為距離參考路徑的距離偏差、航向偏差以及離目標(biāo)點距離偏差的系數(shù)因子。本發(fā)明的有益效果為：(1)能有效安全地完成車輛垂直泊車，本發(fā)明將車輛垂直泊車路徑規(guī)劃問題分解為路徑規(guī)劃和速度規(guī)劃，并分別采用繞墻走策略和動態(tài)窗口法完成規(guī)劃，有機綜合了兩種方法的優(yōu)點，可使車輛實現(xiàn)有效、安全地垂直泊車；(2)無需先驗軌跡數(shù)學(xué)模型，本發(fā)明通過對軌跡規(guī)劃問題進(jìn)行解耦后，通過車輛轉(zhuǎn)向運動的分析，采用繞墻走策略完成垂直泊車軌跡規(guī)劃中的路徑規(guī)劃，該路徑不需要先驗的軌跡數(shù)學(xué)模型，提高了軌跡規(guī)劃的靈活性；(3)軌跡局部可調(diào)，本發(fā)明軌跡規(guī)劃解耦后的速度規(guī)劃中提出運用動態(tài)窗口法，實現(xiàn)了優(yōu)化與反饋的結(jié)合，使軌跡規(guī)劃由開環(huán)變?yōu)殚]環(huán)，大大提高了軌跡規(guī)劃的魯棒性；(4)算法可擴(kuò)展性強，本發(fā)明也可根據(jù)實際情況運用于其他狹小情況下的智能車輛導(dǎo)航，使智能車輛能在運動中快速找到合理的路徑，滿足未來智能駕駛汽車的發(fā)展要求。附圖說明圖1是本發(fā)明中軌跡規(guī)劃方法的流程圖；圖2是本發(fā)明中車輛阿克曼轉(zhuǎn)向示意圖；圖3是本發(fā)明中垂直泊車場景示意圖；圖4是本發(fā)明中前進(jìn)式垂直泊車和后退式垂直泊車對應(yīng)的繞墻走路徑；圖5是本發(fā)明中動態(tài)窗口單步模擬的輸出控制量所對應(yīng)的軌跡圖。具體實施方式下面結(jié)合附圖和實施例，詳細(xì)描述本發(fā)明的技術(shù)方案：如圖1所示，本發(fā)明主要包括以下步驟：1、建立基于運動學(xué)模型的轉(zhuǎn)向運動特性汽車普遍采用阿克曼轉(zhuǎn)向模型，如圖2所示，車輛轉(zhuǎn)向時，全車的轉(zhuǎn)向中心位于后輪軸線的延長線上。車輛的狀態(tài)由橫縱坐標(biāo)及航向角表示q＝(x,y,θ)，車輛的輸入為速度和方向盤轉(zhuǎn)角(v,α)。在泊車的低速行駛模式下忽略輪胎和地面的側(cè)滑，這樣，系統(tǒng)存在不能側(cè)向位移、不能原地轉(zhuǎn)向的非完整性約束，轉(zhuǎn)向時存在轉(zhuǎn)彎半徑。通過解析，車輛轉(zhuǎn)向有以下特性：(1).前后輪運動軌跡是不重合的；(2).轉(zhuǎn)彎半徑最小的點在內(nèi)側(cè)后車輪。2、垂直泊車軌跡規(guī)劃問題解耦垂直泊車場景如圖3所示，軌跡規(guī)劃問題是規(guī)劃出一系列的控制輸入(v,α)，使車輛安全快速地完成泊車。利用Path-VelocityDecomposition對路徑和速度進(jìn)行解耦，將軌跡規(guī)劃問題分解成路徑規(guī)劃和速度規(guī)劃。3、基于繞墻走策略解決路徑規(guī)劃垂直泊車路徑規(guī)劃問題就是找到一系列從泊車起始位姿到目標(biāo)位姿的位姿序列qi＝(xi,yi,θi)。車輛在泊車過程中，通過車身周圍的超聲波傳感器能夠感知泊車環(huán)境，構(gòu)建泊車地圖，基于繞墻走策略的路徑規(guī)劃步驟分為：(1).將垂直泊車地圖環(huán)境進(jìn)行凸包分割，形成凸多邊形障礙物邊緣；(2).計算離起始位姿q0＝(x0,y0,θ0)、終止位姿qe＝(xe,ye,θe)距離最近的障礙物點O0和Oe；(3).分別從O0和Oe沿初始姿態(tài)和終止姿態(tài)方向作車輛和障礙物邊緣作距離為w/2的等距線段(w為車寬)，記錄線段曲率不連續(xù)的點為Oi，并計算在該點姿態(tài)角的變化量；由于原地轉(zhuǎn)向中間狀態(tài)處的曲率是不連續(xù)的，為了滿足車輛的非完整性運動學(xué)約束，基于車輛的轉(zhuǎn)向特性對路徑作如下處理：(1).對于每個點Oi，設(shè)置一個與之對應(yīng)的中間等效狀態(tài)qi＝(xi,yi,θi)；那么，該點位于線段qeOi和線段q0Oi延長線所包圍的區(qū)域內(nèi)；(2).求最短圓弧路徑q0qi和qeqi，且對應(yīng)的半徑為R1和R2；(3).設(shè)置一個最小碰撞距離dc，該距離可由傳感器精度確定，此時對應(yīng)的參考點的值為d＝w/2+dc；(4).計算最小碰撞距離時對應(yīng)的繞墻走路徑。滿足車輛轉(zhuǎn)向特性的垂直泊車?yán)@墻走路徑如圖4所示。4、運用動態(tài)窗口法進(jìn)行速度規(guī)劃車輛本身存在速度和方向盤轉(zhuǎn)角限制：v∈[vmin,vmax],α∈[αmin,αmax](1)假設(shè)動態(tài)窗口法單步模擬的時間間隔為Δt，那么加速度、轉(zhuǎn)角速度約束為：其中，分別對應(yīng)最大加速度和方向盤轉(zhuǎn)角速度。如圖5所示，時間窗口內(nèi)的控制量可以映射為軌跡。采用考慮離參考路徑偏差的軌跡選取策略，選擇目標(biāo)函數(shù)代價最低的軌跡，目標(biāo)函數(shù)cost為：其中，pd,hiff,gd分別為距離參考路徑的距離偏差、航向偏差以及離目標(biāo)點距離偏差的系數(shù)因子。實施例本例為采用ROSIndigo與Gazebo作為實現(xiàn)工具的仿真，具體包括：步驟1.基于車輛運動學(xué)模型建立轉(zhuǎn)向特性車輛的轉(zhuǎn)向模型如圖2所示，后輪軸線中心Pr作為全車軌跡的參考點，車輛的狀態(tài)由橫縱坐標(biāo)及航向角表示為q＝(x,y,θ)，車輛的輸入為速度和方向盤轉(zhuǎn)角(v,α)。Lf、Lr以及w為車輛的前懸、后懸長度和寬度。在泊車的低速行駛模式下忽略輪胎和地面的側(cè)滑，這樣，系統(tǒng)存在如下非完整性約束：該轉(zhuǎn)角狀態(tài)下對應(yīng)路徑的曲率半徑和曲率：以速度和方向盤轉(zhuǎn)角(v,α)為控制量，以q＝(x,y,θ)為狀態(tài)量的車輛運動學(xué)模型：通過分析，車輛轉(zhuǎn)向有以下特性：(1).前后輪運動軌跡是不重合的；(2).轉(zhuǎn)彎半徑最小的點在內(nèi)側(cè)后車輪。同時，兩個車輪的轉(zhuǎn)彎半徑為：其中，R1,R2代表轉(zhuǎn)向內(nèi)側(cè)后輪和外側(cè)前輪的轉(zhuǎn)彎半徑，Rr通常稱為內(nèi)輪差。步驟2.軌跡規(guī)劃問題解耦車輛的速度只影響行駛該軌跡所用的時間，行駛的軌跡形狀以及在各個軌跡點上的姿態(tài)角僅僅與移動距離以及在不同位置的曲率(轉(zhuǎn)向角)有關(guān)。車輛的線速度以及路徑的曲率可以表示為：結(jié)合公式(2)和(6)，可以得到：公式(7)表明，車輛的速度只影響行駛該軌跡所用的時間，行駛的軌跡形狀以及在各個軌跡點上的姿態(tài)角僅僅與移動距離以及在不同位置的曲率(轉(zhuǎn)向角)有關(guān)。那么，垂直泊車軌跡規(guī)劃問題就解耦為路徑規(guī)劃問題和速度規(guī)劃問題。步驟3.基于繞墻走策略解決路徑規(guī)劃通過自動泊車系統(tǒng)環(huán)境感知子系統(tǒng)可以獲得如圖4所示的泊車環(huán)境地圖，帶箭頭線段表示了完整性約束的繞墻走路徑，通過車輛轉(zhuǎn)向特性，對該繞墻走路徑進(jìn)行滿足非完整約束的等效，等效后的路徑可以由圖4中曲線表示。在仿真環(huán)境中，最小碰撞距離dc設(shè)置為0.2m，車輛參數(shù)如表1所示，后退式垂直泊車場景參數(shù)設(shè)置如表2所示：表1車輛參數(shù)表2垂直泊車場景參數(shù)參數(shù)符號單位參數(shù)值車位長bm3.0車位寬am2.0車道寬dsm4.0起始位姿q0(m,m,rad)(0,0,1.57)終止位姿qe(m,m,rad)(5.5,3.2,3.14)步驟4.運用動態(tài)窗口法進(jìn)行速度規(guī)劃動態(tài)窗口法通過在當(dāng)前位姿狀態(tài)下，前向模擬，通過參考繞墻走路徑偏差因子、目標(biāo)點偏差因子、參考航向偏差因子動態(tài)的選取車輛在該狀態(tài)下的最優(yōu)速度和方向盤轉(zhuǎn)角輸出，動態(tài)窗口法所采用的各個參數(shù)如表3所示。表3動態(tài)窗口法參數(shù)動態(tài)窗口法可以根據(jù)實時的車輛狀態(tài)信息對軌跡進(jìn)行調(diào)整。綜上，本發(fā)明提出的基于路徑-速度解耦的軌跡規(guī)劃方法可以實現(xiàn)無先驗軌跡知識的、局部動態(tài)可調(diào)的垂直泊車，提高了垂直泊車規(guī)劃決策的智能化程度?；诶@墻走策略的路徑規(guī)劃可以實現(xiàn)無軌跡模型的路徑規(guī)劃，同時在不進(jìn)行精確的軌跡跟蹤控制的情況下，也能通過軌跡規(guī)劃中動態(tài)窗口法本身所具備的閉環(huán)特點對軌跡進(jìn)行局部調(diào)整。當(dāng)前第1頁1 2 3