專利名稱：一種智能輔助泊車方法及其實現系統的制作方法
技術領域：
本發(fā)明屬于車輛控制技術領域，涉及駕駛輔助技術，特別是汽車的智能輔助泊車技術。
背景技術：
由于城市汽車保有量的增加，停車位緊張的問題日益突出，泊車已經成為城市交通中的一個難點問題，由于泊車時需要駕駛員在狹小緊湊的空間中觀察和判斷周邊環(huán)境，短時間內同時控制方向盤、油門和剎車，對其駕駛技巧和反應靈敏度提出了很高要求，因此有必要開發(fā)輔助泊車系統，幫助駕駛員完成泊車操作。智能泊車或是輔助泊車系統已經成為國內外的研究重點，智能泊車系統一般包括 感知系統，控制系統和執(zhí)行系統等。而控制系統是智能泊車系統的核心，其主要是根據傳感器信息以及駕駛員操縱信息，對車輛的狀態(tài)進行調整，使車輛在泊車的過程中找到一條合適的路徑，引導車輛完成泊車操作。目前智能泊車的控制算法主要分為以下幾類路徑規(guī)劃和跟隨算法，它的基本思想是路徑規(guī)劃模塊根據起始位姿和目標位姿以及障礙物的情況規(guī)劃出可行的路徑，路徑跟隨模塊控制車輛跟隨規(guī)劃路徑，最終使車輛駛入泊車位，這種方法算法簡單，容易實現且計算成本較低；基于經驗的智能算法，它依據人類駕駛員的泊車經驗制定控制策略，控制車輛泊車入庫，一般包括模糊控制、神經網絡、遺傳算法及粒子群算法等，這些智能算法能夠較好地解決非線性問題，但是適應性差，控制規(guī)則制定復雜，并且計算成本高；姿態(tài)穩(wěn)定算法是通過穩(wěn)定性分析，使車輛的姿態(tài)收斂到零，從而逐步控制車輛達到目標位姿，這種算法具有較高的非線性且計算量大。泊車的難點可以歸納為一、車輛在泊車過程中不能與前后的障礙物車輛發(fā)生碰撞；二、車輛在泊車過程中，車輛運動不能超過車輛本身的物理約束條件。泊車路徑規(guī)劃模塊的主要任務是要解決這兩個關鍵問題；另外，在泊車過程中考慮到舒適性和安全性等因素，路徑規(guī)劃時還要考慮車輛前后運動方向的改變次數、原地轉向次數以及泊車路徑長度等因素。因此，針對輔助泊車系統，開發(fā)實用高效的路徑規(guī)劃方法具有重要意義。

發(fā)明內容
本發(fā)明的目的在于提供一種智能輔助泊車系統，當環(huán)境感知系統識別到可用于泊車的庫位后，規(guī)劃出合理的泊車路徑，并控制車輛沿規(guī)劃路徑行駛，直到車輛完成泊車動作，并提供人機交互界面，接受駕駛員意圖，顯示泊車信息。為達到以上目的，本發(fā)明所采用的解決方案是一種智能輔助泊車系統，包括環(huán)境感知單元，用于獲取庫位信息和車輛的位姿信息；路徑規(guī)劃單元，用于規(guī)劃出合適的泊車路徑；
駕駛控制單元，用于控制車輛沿給定路徑行駛；人機接口單元，用于人機交互，包含駕駛員信息輸入和系統信息輸出。該路徑規(guī)劃單元包括兩個模塊(I)庫位內調整路徑規(guī)劃模塊，規(guī)劃車輛在庫位內進行調整時的路徑；(2)入庫路徑規(guī)劃模塊，根據車輛的起始位姿規(guī)劃入庫的泊車路徑；該人機接口單元提供駕駛員意圖輸入接口，駕駛員可以設定平行泊車情況下允許的最大庫位內調整次數；垂直泊車情況下，不允許車輛進行庫位內調整。
在環(huán)境感知單元探測到庫位大小后，該路徑規(guī)劃單元可以在平行泊車情況下計算出泊入該庫位所需的庫位內調整次數，當計算得到的庫位內調整次數大于駕駛員設定次數時，放棄該庫位。當計算得到的庫位內調整次數不大于駕駛員設定次數時，該路徑規(guī)劃單元首先規(guī)劃出庫位內調整路徑，然后規(guī)劃出入庫路徑。庫位內調整路徑的起點為第一目標位姿B，終點為最終目標位姿G。庫位內調整路徑以最小轉彎半徑圓弧連接第一目標位姿B和最終目標位姿G ;入庫路徑連接車輛的起始位姿S和第一目標位姿B。庫位內調整路徑規(guī)劃模塊由限制方程生成子模塊和優(yōu)化求解子模塊構成，其中限制方程生成子模塊能夠根據庫位的位置和大小生成泊車路徑限制方程組，優(yōu)化求解模塊能夠對建立的限制方程組進行最優(yōu)化求解，得到最優(yōu)泊車目標位姿和最優(yōu)庫內調整路徑，保證車輛泊車過程中能夠得到最大的安全距離。通過庫位內調整路徑的最優(yōu)化，能夠降低車輛的庫位內調整次數，或者在相同庫位內調整次數的情況下，使車輛能夠泊入盡可能小的庫位。入庫路徑規(guī)劃模塊包含三個子模塊無碰撞路徑生成子模塊、可執(zhí)行路徑生成子模塊和路徑選擇子模塊，該無碰撞路徑規(guī)劃子模塊生成無碰撞泊車路徑采用扇形區(qū)域法，選擇扇形上距離障礙物距離最大的點，即最不可能與障礙物發(fā)生碰撞的點，連接這些點構成無碰撞泊車路徑。可執(zhí)行路徑生成子模塊規(guī)劃的路徑可能包含三種曲線類型最短距離曲線即RS曲線(Reeds&Shepp，s curve)、曲率連續(xù)曲線即 CC 曲線(Continuous-Curvature curve)和半曲率連續(xù)曲線即 hCC 曲線(half Continuous-Curvature curve)。路徑選擇子模塊可以根據不同的原則選擇合適的曲線類型(RS/hCC/CC)作為泊車路徑，這些原則對應路徑選擇子模塊的三種工作模式，其具體表述為I)泊車過程中前后運動方向改變的次數最少原則，對應模式I ;2)泊車過程中走過的距離最短原則，對應模式2 ；3)泊車過程中原地轉向的次數最少原則，對應模式3 ；人機接口單元提供駕駛員意圖輸入接口，路徑選擇子模塊接收駕駛員所輸入的路徑類型選擇模式，并據此選擇符合駕駛員意圖的曲線作為可執(zhí)行路徑。當人機接口單元接收到的駕駛員意圖為模式I時，接收駕駛員設定的允許的最大方向改變次數，當規(guī)劃的路徑超過這個設定值，放棄泊車；當人機接口單元接收到的駕駛員意圖為模式2時，系統計算泊車路徑，自行選擇符合駕駛員意圖的曲線作為可執(zhí)行路徑。當人機接口單元接收到的駕駛員意圖為模式3時，接收駕駛員設定的允許的最大原地轉向次數，當規(guī)劃的路徑超過這個設定值，放棄泊車。
本發(fā)明能夠從任意起始位姿引導車輛泊入停車位，能夠降低泊車過程中車輛原地轉向和前后運動方向改變的次數；平行泊車情況下，可根據駕駛員設置的庫位內允許最大調整次數選擇合適的停車位，降低車輛對平行泊車庫位長度的要求。


圖I是本發(fā)明智能輔助泊車系統的系統框圖。圖2是本發(fā)明實施例庫位內調整路徑規(guī)劃模塊框圖。圖3是本發(fā)明實施例入庫路徑規(guī)劃模塊框圖。圖4是車輛模型示意圖。圖5是兩次庫位內調整時的實施例示意圖。 圖6是扇形區(qū)域法生成無碰撞路徑實施例的示意圖。圖7a是左轉的CC曲線曲率-弧長圖。圖7b是右轉的CC曲線曲率-弧長圖。圖8a是左轉的hCC曲線曲率-弧長圖。圖8b是右轉的hCC曲線曲率-弧長圖。圖9是RS曲線轉換為CC曲線示意圖。圖10是轉換后CC路徑曲率-弧長示意圖。圖11是RS曲線轉換為hCC曲線示意圖。圖12是轉換后hCC路徑的曲率-弧長圖。圖13是米樣點的選取流程不意圖。圖14RS/hCC/CC曲線選取模式I。圖15—種泊車過程仿真結果示意圖。
具體實施例方式本發(fā)明包括感知單元，用于獲取庫位信息和車輛的位姿信息；路徑規(guī)劃單元，用于規(guī)劃出合適的泊車路徑；駕駛控制單元，用于控制車輛沿規(guī)劃路徑行駛；人機接口單元，用于人機交互，包含駕駛員信息輸入和系統信息輸出，例如視覺顯示、語音提示和機械振動
坐寸ο路徑規(guī)劃單元是整個系統的中心，它接收環(huán)境感知單元發(fā)來的庫位信息和車輛位姿信息，向駕駛控制單元輸出車輛需要跟蹤行駛的路徑參數，同時把規(guī)劃的路徑信息輸出到人機接口單元，給出駕駛員相應提示。人機接口單元是系統和駕駛員進行信息交互的窗口，它把環(huán)境感知單元、路徑規(guī)劃單元、駕駛控制單元等發(fā)送來的信息輸出，并能夠接受駕駛員的輸入，允許駕駛員設定允許的最大庫位內調整次數、入庫路徑模式等參數。為了使車輛能夠泊入更為狹小的庫位，本發(fā)明允許車輛進行平行泊車時，在進入庫位后做若干次姿態(tài)調整，駕駛員可以通過人機界面設定允許姿態(tài)調整的次數，當車輛泊入庫位所需的庫位內調整次數大于駕駛員設定次數時，系統放棄該庫位。當庫位內調整次數不大于駕駛員設定次數時，系統首先規(guī)劃出車輛在庫位內的姿態(tài)調整路徑，然后規(guī)劃出車輛駛入庫位的路徑。由于在規(guī)劃庫位內調整路徑時進行了最優(yōu)化設計，庫位空間得到了充分利用。和其他輔助泊車系統相比，本發(fā)明能夠在不增加庫位內調整次數的情況下，使車輛泊入更小的庫位。為了使車輛能夠從任意起始位姿駛入庫位，在入庫路徑規(guī)劃模塊中本發(fā)明采用了兩步法路徑規(guī)劃方法，首先生成無碰撞路徑，然后根據無碰撞路徑生成車輛可跟蹤行駛的路徑。最終生成的入庫路徑有三種模式，每種模式對路徑的舒適性、長短等有不同的側重。駕駛員可以根據個人喜好通過人機界面對入庫路徑的模式進行選擇。以下結合附圖所示實施例對本發(fā)明作進一步的說明。(I)車輛控制模型
在泊車過程中，車速很低，輪胎的側偏特性可以忽略不計，所以可以采用以下車輛控制模型(見圖4)

其中，s是車輛所駛過的弧長，x、y是車輛在全局坐標系中的橫、縱坐標，Θ是車輛的航向角，K是車輛后軸中點軌跡的曲率，O是曲率的變化率，Vmax是泊車中允許的最大速度，t是時間變量，sign( ·)是符號函數車輛的兩個約束條件是I K I ^ K max,式(1-2)I σ I ^ σ max,式(1-3)其中，Kmax和是曲率和曲率變化率的最大值，它們是由車輛本身的物理特性決定的約束值。(2)庫位內位姿調整路徑規(guī)劃模塊I)庫內調整次數的設定和判斷在本發(fā)明中，當泊車庫位的尺寸確定之后，系統就能夠計算出車輛泊入庫位所需要的庫位內姿態(tài)調整次數。系統把計算得到的庫位內調整次數和駕駛員設定的最大允許調整次數進行比較，當計算得到的調整次數大于駕駛員設定的允許調整次數時，系統放棄該庫位。調整次數的設定可以采用多種方式，如采用鍵盤輸入、觸摸屏輸入或者預設在系統控制器內。根據庫位尺寸計算車輛庫位內調整次數的方法也可以具有多種方式，如查表法，數值計算法等。在實施例I中，系統根據公式計算調整次數，具體方法如下I計算泊車深度泊車深度指完成泊車動作后車輛中心線和庫位外沿的距離，其確定按照以下公式
\WJ2(Wp>Wv+eh)A = ^P式(2-1)
\W -WJ2-eh (Wp<Wv+eh)
其中h表示泊車深度，Wv表示車輛寬度，Wp表示庫位寬度，eh表示泊車深度的安全距離。II計算車輛進行η (η為非負的整數)次姿態(tài)調整時所能泊入的最小庫位長度根據以下公式計算車輛無需庫位內姿態(tài)調整時所需的最小庫位長度

其中Ltl表示無需庫位內姿態(tài)調整時所能泊入的最小庫位長度，h表示泊車深度，LV表示車輛長度，Wv表示車輛寬度，b表示車輛軸距，Lr表示車輛后軸中點距離車輛尾端的距離，R表示車輛外側前輪的最小轉彎半徑，如圖4所示。根據以下公式計算車輛進行η次庫位內姿態(tài)調整時所能泊入的最小庫位長度Ln = -kn arctan (n) +L0 式(2-3)其中Ln表示η次姿態(tài)調整所能泊入的最小庫位長度，kn表示對應于調整次數η的調節(jié)參數，由標定得來，L0表示無需庫位內姿態(tài)調整時所能泊入的最小庫位長度。III計算得到泊入現有庫位所需的庫位內調整次數。根據步驟II所得的庫位內調整次數和庫位長度的關系，計算得到泊入現有庫位所需的庫位內調整次數，當調整次數大于預設值時，放棄該庫位，并通過人機界面對駕駛員做出提示。2)庫位內調整路徑規(guī)劃庫位內調整路徑規(guī)劃模塊利用車輛最小轉彎半徑圓弧規(guī)劃車輛在庫位內進行姿態(tài)調整的運動路徑，其包含兩個子模塊限制方程生成子模塊和優(yōu)化求解子模塊，其中限制方程生成子模塊能夠根據庫位的位置和大小生成泊車路徑限制方程組，車輛在庫位內運動時其軌跡必須滿足限制方程組；優(yōu)化求解模塊能夠對建立的限制方程組進行最優(yōu)化求解，得到最優(yōu)泊車目標位姿和最優(yōu)庫位內調整路徑，保證車輛泊車過程中得到最大的安全距離。通過庫位內調整路徑的最優(yōu)化，能夠減少車輛在庫位內的調整次數，或者在相同庫位內調整次數的情況下，使車輛能夠泊入盡可能小的庫位。一條典型的庫位內調整路徑如圖5所示。限制方程組的建立方法可以具有多種方式，即可以在線完成，也可以預設在控制器的算法里。對方程組進行優(yōu)化求解的方法也可以具有多種方式，如線性規(guī)劃法或者迭代求解法等。在實施例2中，路徑規(guī)劃算法通過計算發(fā)現車輛泊入庫位需要進行2次庫位內姿態(tài)調整，小于駕駛員的預設值，其庫位內調整路徑規(guī)劃方法如下所示I建立限制方程組如圖5所示，庫位是由障礙物I和障礙物2構成的，環(huán)境感知模塊探測到庫位長度SLmin,車輛需要在庫位內做兩次姿態(tài)調整，庫位內的調整路徑由兩段圓弧組成，第一段是由第一目標位姿B到中間目標位姿C的向前運動，第二段是由中間目標位姿C到最終目標位姿G的向后運動。車輛沿規(guī)劃路徑依次到達B點、C點和G點，設B點坐標為(xB，yB, θ Β)，C點坐標為(Xd y。Θ J, G點坐標為(xe，y(；，Θ G),車輛的左上角點、右上角點、右后角點、左后角點依次表示為點a、b、c、d,路沿L6的縱坐標為y6,坐標軸的原點設定在前方障礙物的左后角點。車輛長度表示SLv，車輛寬度表示為Wv，車輛軸距表示為b，車輛后軸中點距離車輛尾端的距離表示為Lp當車輛到達G點后，要求車輛剛好和庫位保持平行，并且達到所需的泊車深度，于是有yG = ~h 式(2—4)
Θ G = O 式(2-5)其中h表示泊車深度?？梢栽O想，為了使車輛在庫位內調整過程中獲得最大的安全距離，車輛在B點、C點、G點時與最近障礙物的距離應該相同。車輛在G點時，其左后角點d距離后方障礙物最近，其橫坐標為xGd = (-Lr+xG)式(2-6 )其中xed表示車輛在G點時其左后角點d的橫坐標。車輛在B點時距離后方障礙物最近，其左后角點d點的橫坐標為xBd = xB-Lr cos Θ B-0. 5ffvsin θ Β 式(2—7)其中xBd表示車輛在B點時其左后角點d點的橫坐標。車輛在C點時距離前方障礙物最近，其右前角點b的橫坐標為xcb = xc+(Lv-Lr) cos Θ c+0. 5Wvsin Θ c 式(2—8)其中表示車輛在C點時其右前角點b點的橫坐標。要求車輛在B點、C點、G點處與最近障礙物的距離相同，并且要求距離大于零，據此可得xBd+Lmin =-Xcb > O 式(2-9)xGd+Lmin = xBd+Lmin > O 式(2-10)根據B點和C點的位置關系可得RmunrCOS Θ「R—COS θ β = yc_yB 式(2-11)-RminrSin Θ C+Rminrsin θ B = xc-xB 式(2-12)根據C點和G點的位置關系可得RminlSinQc = Xc-XG 式(2-13)Rminl-RminlCOS Θ c = H 式(2-14)其中Rmim表示車輛后軸中點向右的最小轉彎半徑，Rfflinl表示車輛后軸中點向左的最小轉彎半徑。假設車輛從庫外以最小轉彎半徑圓弧倒車到達B點，要求倒車過程中車輛右前角點b不與庫位發(fā)生碰撞，右后角點c不與路沿L6發(fā)生碰撞，可得限制不等式
權利要求
1.一種智能輔助泊車方法，其特征在于探測感知車輛周圍環(huán)境，識別到可用于泊車的庫位后，規(guī)劃出合理的泊車路徑，并控制車輛沿規(guī)劃路徑行駛，直到車輛完成泊車動作；并提供人機交互界面，接受駕駛員意圖，顯示泊車信息；其中，對泊車路徑分兩部分進行規(guī)劃，首先規(guī)劃出庫位內調整路徑，然后規(guī)劃出入庫路徑。
2.根據權利要求I所述的智能輔助泊車方法，其特征在于 規(guī)劃庫位內調整路徑時，采用最小轉彎半徑圓弧構成路徑，首先建立庫位邊界對庫位內姿態(tài)調整路徑的限制方程，然后對方程進行優(yōu)化求解，據此求出最優(yōu)泊車目標位姿和最優(yōu)庫位內調整路徑； 規(guī)劃入庫路徑時采用兩步法路徑規(guī)劃方法首先采用扇形區(qū)域法生成無碰撞路徑，然后根據無碰撞路徑生成車輛可跟蹤行駛的路徑，最后從多條可執(zhí)行的路徑中選擇合適類型的路徑。
3.根據權利要求2所述的智能輔助泊車方法，其特征在于提供人機界面，允許駕駛員對泊車路徑進行個性化設定； (O依據庫位尺寸計算出車輛需要的庫位內姿態(tài)調整的次數，由駕駛員根據個人喜好通過人機界面對庫位內調整次數上限值進行設定，當所需調整次數不大于駕駛員設定的上限值時，開始路徑規(guī)劃； (2)依據路徑的舒適性、長短區(qū)分出不同的入庫路徑模式，由駕駛員根據個人喜好通過人機界面對入庫路徑的模式進行選擇。
4.實現權利要求I至3中任一所述方法的智能輔助泊車系統，其特征在于該系統包括相互之間具有通信聯系的環(huán)境感知單元、路徑規(guī)劃單元、駕駛控制單元和人機接口單元。
5.根據權利要求4所述的智能輔助泊車系統，其特征在于該路徑規(guī)劃單元包括兩個模塊 (1)庫位內調整路徑規(guī)劃模塊，規(guī)劃車輛在庫位內進行調整時的路徑； (2)入庫路徑規(guī)劃模塊，根據車輛的起始位姿規(guī)劃入庫的泊車路徑。
6.根據權利要求4所述的智能輔助泊車系統，其特征在于該人機接口單元提供駕駛員意圖輸入接口，由駕駛員設定平行泊車情況下允許的庫位內調整次數上限值；垂直泊車情況下，不允許車輛進行庫位內調整。
7.根據權利要求4所述的智能輔助泊車系統，其特征在于在環(huán)境感知單元探測到庫位大小后，該路徑規(guī)劃單元計算出泊入該庫位所需的庫位內調整次數，當計算得到的庫位內調整次數大于駕駛員設定次數時，放棄該庫位；當計算得到的庫位內調整次數不大于駕駛員設定次數時，該路徑規(guī)劃單元首先規(guī)劃出庫位內調整路徑，然后規(guī)劃出入庫路徑；庫位內調整路徑以最小轉彎半徑圓弧連接第一目標位姿B和最終目標位姿G ;入庫路徑連接車輛的起始位姿S和第一目標位姿B。
8.根據權利要求5所述的智能輔助泊車系統，其特征在于該庫位內調整路徑規(guī)劃模塊由限制方程生成子模塊和優(yōu)化求解子模塊構成，其中限制方程生成子模塊能夠根據庫位的位置和大小生成泊車路徑限制方程組，優(yōu)化求解模塊能夠對建立的限制方程組進行最優(yōu)化求解，得到最優(yōu)泊車目標位姿G和最優(yōu)庫位內調整路徑； 或者，該入庫路徑規(guī)劃模塊包含三個子模塊無碰撞路徑生成子模塊、可執(zhí)行路徑生成子模塊和路徑選擇子模塊，該無碰撞路徑規(guī)劃子模塊生成無碰撞泊車路徑時采用扇形區(qū)域法，選擇扇形上距離障礙物車輛距離最大的點，即最不可能與障礙物發(fā)生碰撞的點；連接這些點構成無碰撞泊車路徑； 或者，該可執(zhí)行路徑生成子模塊規(guī)劃的路徑包含三種曲線類型RS曲線(Reeds& Shepp，s curve)、CC 曲線(continuous-Curvature curve)和 hCC 曲線(halfContinuous-Curvature curve)； 或者，該路徑選擇子模塊可以根據不同的原則選擇合適的曲線類型(RS/hCC/CC)作為泊車路徑，這些原則對應路徑選擇子模塊的三種工作模式，包括 1)泊車過程中前后運動方向改變的次數最少原則，對應模式I; 2)泊車過程中走過的距離最短原則，對應模式2； 3)泊車過程中原地轉向的次數最少原則，對應模式3。
9.根據權利要求8所述的智能輔助泊車系統，其特征在于人機接口單元提供駕駛員意圖輸入接口，路徑選擇子模塊接收駕駛員所輸入的路徑類型選擇模式，并據此選擇符合駕駛員意圖的曲線作為可執(zhí)行路徑。
10.根據權利要求9所述的智能輔助泊車系統，其特征在于當人機接口單元接收到的駕駛員意圖為模式I時，接收駕駛員設定的允許的最大前后運動方向改變次數，當規(guī)劃的路徑超過這個設定值，放棄泊車；當人機接口單元接收到的駕駛員意圖為模式2時，系統計算泊車路徑，自行選擇符合駕駛員意圖的曲線作為可執(zhí)行路徑；當人機接口單元接收到的駕駛員意圖為模式3時，接收駕駛員設定的允許的最大原地轉向次數，當規(guī)劃的路徑超過這個設定值，放棄泊車。
全文摘要
一種智能輔助泊車方法，對泊車路徑分兩部分進行規(guī)劃，首先規(guī)劃出庫位內調整路徑，然后規(guī)劃出入庫路徑；并提供人機交互界面，接受駕駛員意圖，顯示泊車信息；一種實現所述方法的智能輔助泊車系統，該系統包括相互之間具有通信聯系的環(huán)境感知單元、路徑規(guī)劃單元、駕駛控制單元和人機接口單元。本發(fā)明能夠從任意起始位姿引導車輛泊入停車位，能夠降低泊車過程中車輛原地轉向和前后運動方向改變的次數；平行泊車情況下，可根據駕駛員設置的庫位內允許最大調整次數選擇合適的停車位，降低車輛對平行泊車庫位長度的要求。
文檔編號B60W30/06GK102963355SQ20121042972
公開日2013年3月13日 申請日期2012年11月1日 優(yōu)先權日2012年11月1日
發(fā)明者陳慧, 張野, 程昆朋 申請人:同濟大學