一種垂直泊車軌跡規劃方法與流程

2023-05-02 15:38:11

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