本發(fā)明涉及自動(dòng)駕駛車輛控制領(lǐng)域，尤其涉及一種自動(dòng)駕駛車輛分層式縱向規(guī)劃控制系統(tǒng)及方法。

背景技術(shù)：

自汽車從誕生以來其操縱者就是人，即由駕駛員觀察道路并操縱車輛，形成“人-車-路”的閉環(huán)控制系統(tǒng)。該控制系統(tǒng)中，“人”這個(gè)子系統(tǒng)駕駛車輛的水平參差不齊，而且絕大多數(shù)交通事故都是由人為因素造成。為解決這一問題，自動(dòng)駕駛車輛應(yīng)運(yùn)而生，自動(dòng)駕駛車輛將“人”的作用淡化，甚至能完全脫離“人”的操縱實(shí)現(xiàn)自主駕駛，這要求汽車本身具有一定的自動(dòng)駕駛功能。因此，為了使汽車更加安全地自動(dòng)駕駛，需要開展自動(dòng)駕駛車輛的技術(shù)研究。在自動(dòng)駕駛車輛的關(guān)鍵技術(shù)中，車輛的縱向規(guī)劃與控制是自動(dòng)駕駛車輛的基礎(chǔ)和重點(diǎn)，其在很大程度上決定了自動(dòng)駕駛車輛的性能和體驗(yàn)。

自動(dòng)駕駛車輛縱向規(guī)劃是指在可行駛縱向路徑上通過規(guī)劃算法得到該路徑上每一點(diǎn)的期望速度，而縱向控制是指在車輛行車速度方向上的控制，即控制車輛以給定的期望速度或期望車間距離進(jìn)行行駛?？v向規(guī)劃控制系統(tǒng)是一個(gè)典型的混合控制系統(tǒng)，它不僅包含連續(xù)的動(dòng)態(tài)過程，也包含離散的切換過程。在不同場景下，自動(dòng)駕駛車輛必須調(diào)整速度使其與前方目標(biāo)車輛速度相同，同時(shí)與前方車輛保持一定的安全間距。

目前國內(nèi)外使用的自動(dòng)駕駛車輛縱向規(guī)劃控制方法，包括自適應(yīng)巡航控制(ACC)、STOP&GO(起停控制)、PID控制(比例、積分、微分控制)、模糊控制、MPC(模型預(yù)測控制)等，其具有以下的缺點(diǎn)：

1)非分層式控制方法，如ACC及STOP&GO，省略了規(guī)劃層直接得到控制結(jié)果，控制過程不直觀，不具有向后預(yù)測性，在一定程度上會影響自動(dòng)駕駛車輛顯示系統(tǒng)與乘客的交互，同時(shí)難以在速度層面上進(jìn)行如最大速度、橫向穩(wěn)定性等的限制，不能與自動(dòng)駕駛車輛橫向規(guī)劃協(xié)調(diào)；

2)參數(shù)調(diào)整困難，適用范圍有限，如傳統(tǒng)單一的PID控制與模糊控制需要反復(fù)調(diào)整控制參數(shù)，同時(shí)一套參數(shù)很難保證在較大的速度變化范圍內(nèi)適用；

3)動(dòng)態(tài)環(huán)境適應(yīng)性不強(qiáng)，難以在復(fù)雜的實(shí)際城市道路工況下取得較好效果，如MPC控制方法的計(jì)算時(shí)間較長，不便于保證實(shí)時(shí)性，同時(shí)其應(yīng)用于分層式控制時(shí)，在期望速度曲線動(dòng)態(tài)變化時(shí)效果不佳；

4)過分注重控制的精確性，忽略自動(dòng)駕駛車輛乘客對舒適性的最高要求。

技術(shù)實(shí)現(xiàn)要素：

鑒于上述的分析，本發(fā)明旨在提供一種自動(dòng)駕駛車輛分層式縱向規(guī)劃控制系統(tǒng)及方法，用以解決上述技術(shù)問題。

本發(fā)明的目的主要是通過以下技術(shù)方案實(shí)現(xiàn)的：

基于本發(fā)明實(shí)施例的一個(gè)方面，提供了一種自動(dòng)駕駛車輛分層式縱向規(guī)劃控制方法，包括步驟，

S1、根據(jù)感知設(shè)備獲得的數(shù)據(jù)進(jìn)行局部路徑規(guī)劃，生成縱向局部路徑參數(shù)；

S2、根據(jù)縱向局部路徑參數(shù)進(jìn)行縱向速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果；

S3、根據(jù)自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果判斷行駛模式；

S4、進(jìn)行縱向速度控制。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S1具體包括：

S11、通過感知設(shè)備獲取環(huán)境信息；

S12、進(jìn)行環(huán)境信息解析與處理；

S13、以步長為l離散路徑，并向前探索一個(gè)步長；

S14、判斷是否遇到障礙物，否，則以車輛位置、方向等信息更新路徑長度并執(zhí)行步驟S13，是，則縱向局部路徑生成完畢。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S2具體包括：

S21、首先將縱向局部路徑參數(shù)作為初始規(guī)劃條件進(jìn)行相對速度曲線規(guī)劃，得到基本相對速度規(guī)劃曲線；

S22、對基本相對速度規(guī)劃曲線進(jìn)行約束，得到最終的相對速度規(guī)劃曲線；

S23、將最終的相對速度規(guī)劃曲線進(jìn)行絕對速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S3具體包括：

S31、分析規(guī)劃結(jié)果產(chǎn)生的平均加速度；

S32、基于平均加速度分析行駛模式。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S32中行駛模式由平均加速度的范圍得到，其中每個(gè)行駛模式為一個(gè)加速度閾值系數(shù)或者為兩個(gè)加速度閾值系數(shù)。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，行駛模式分別為：加速模式、勻速模式、制動(dòng)模式和緊急制動(dòng)模式。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S4具體包括：

S41、判斷在當(dāng)前行使模式下自動(dòng)駕駛車輛當(dāng)前速度所在速度區(qū)間；

S42、根據(jù)當(dāng)前行駛模式、當(dāng)前速度所在速度區(qū)間及期望速度進(jìn)行相應(yīng)控制流程。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S4還包括：在部分正常規(guī)劃結(jié)果的直接處理方案中，不依賴速度規(guī)劃結(jié)果直接進(jìn)行車輛控制。

基于本發(fā)明方法的另一個(gè)實(shí)施例中，步驟S42進(jìn)一步包括：控制流程中要對油門量或制動(dòng)量進(jìn)行飽和處理；各模式控制流程的時(shí)間累加量在模式切換時(shí)清零，開始新模式控制量的累加。

基于本發(fā)明實(shí)施例的另一個(gè)方面，提供了一種自動(dòng)駕駛車輛分層式縱向規(guī)劃控制系統(tǒng)，包括：局部路徑規(guī)劃器、縱向速度規(guī)劃器、縱向速度控制器，并通過空置量反饋與車體動(dòng)力學(xué)系統(tǒng)構(gòu)成形成閉環(huán)，局部路徑規(guī)劃器將感知設(shè)備獲得的數(shù)據(jù)進(jìn)行局部路徑規(guī)劃，生成縱向局部路徑規(guī)劃參數(shù)；縱向速度規(guī)劃器將縱向局部路徑參數(shù)進(jìn)行縱向速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度時(shí)間的規(guī)劃結(jié)果并根據(jù)規(guī)劃結(jié)果分析自動(dòng)駕駛車輛的行駛模式；縱向速度控制器將行駛模式和期望速度作為輸入，進(jìn)行縱向速度控制。

本發(fā)明有益效果如下：能與自動(dòng)駕駛車輛橫向規(guī)劃協(xié)調(diào)統(tǒng)一，有利于自動(dòng)駕駛車輛規(guī)劃控制方法的進(jìn)一步發(fā)展在不同場景下，自動(dòng)駕駛車輛可以調(diào)整速度使其與前方目標(biāo)車輛速度相同，同時(shí)與前方車輛保持一定的安全間距，也能保證緊急情況的制動(dòng)和停車?？v向規(guī)劃對車輛的所有縱向行駛情況進(jìn)行了分類處理，因此廣泛適用于各種城市道路工況，同時(shí)能兼容自動(dòng)駕駛車輛其他系統(tǒng)，如對感知系統(tǒng)的噪聲有一定抗干擾性，能適應(yīng)某些執(zhí)行機(jī)構(gòu)無法對連續(xù)量控制的特性。

本發(fā)明的其他特征和優(yōu)點(diǎn)將在隨后的說明書中闡述，并且，部分的從說明書中變得顯而易見，或者通過實(shí)施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點(diǎn)可通過在所寫的說明書、權(quán)利要求書、以及附圖中所特別指出的結(jié)構(gòu)來實(shí)現(xiàn)和獲得。

附圖說明

附圖僅用于示出具體實(shí)施例的目的，而并不認(rèn)為是對本發(fā)明的限制，在整個(gè)附圖中，相同的參考符號表示相同的部件。

圖1為分層式縱向規(guī)劃控制系統(tǒng)結(jié)構(gòu)；

圖2為縱向局部路徑規(guī)劃流程圖；

圖3為縱向局部路徑規(guī)劃示意圖；

圖4為縱向速度規(guī)劃流程圖；

圖5為基本規(guī)劃結(jié)果1曲線圖；

圖6為基本規(guī)劃結(jié)果2形式1曲線圖；

圖7為基本規(guī)劃結(jié)果2形式2曲線圖；

圖8為基本規(guī)劃結(jié)果3曲線圖；

圖9為基本規(guī)劃結(jié)果4曲線圖；

圖10為基本規(guī)劃結(jié)果5曲線圖；

圖11為基本規(guī)劃結(jié)果6形式1曲線圖；

圖12為基本規(guī)劃結(jié)果6形式2曲線圖；

圖13為約束規(guī)劃結(jié)果1曲線圖；

圖14為約束規(guī)劃結(jié)果2曲線圖；

圖15為行駛模式判斷示意圖；

圖16為縱向速度控制方法流程圖。

具體實(shí)施方式

下面結(jié)合附圖來具體描述本發(fā)明的優(yōu)選實(shí)施例，其中，附圖構(gòu)成本申請一部分，并與本發(fā)明的實(shí)施例一起用于闡釋本發(fā)明的原理。

根據(jù)本發(fā)明的一個(gè)具體實(shí)施例，公開了一種自動(dòng)駕駛車輛分層式縱向規(guī)劃控制系統(tǒng)，如圖1所示，包括局部路徑規(guī)劃器、縱向速度規(guī)劃器、縱向速度控制器，并通過車速反饋與車體動(dòng)力學(xué)系統(tǒng)形成閉環(huán)。局部路徑規(guī)劃器將感知設(shè)備獲得的數(shù)據(jù)進(jìn)行局部路徑規(guī)劃，生成縱向局部路徑規(guī)劃參數(shù)；縱向速度規(guī)劃器將縱向局部路徑參數(shù)進(jìn)行縱向速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度時(shí)間的規(guī)劃結(jié)果并根據(jù)規(guī)劃結(jié)果分析自動(dòng)駕駛車輛的行駛模式；縱向速度控制器將行駛模式和期望速度作為輸入，進(jìn)行縱向速度控制，包括油門量和制動(dòng)量控制，通過執(zhí)行機(jī)構(gòu)和車體動(dòng)力學(xué)特性控制自動(dòng)駕駛車輛的縱向速度。

其中，環(huán)境約束包括道路環(huán)境，周圍車輛、行人情況等約束條件，車輛性能約束包括車輛本身能達(dá)到的最高速度、最大轉(zhuǎn)向角等約束條件。二者共同作用影響縱向速度規(guī)劃器的規(guī)劃結(jié)果。

車速反饋包括反饋上層控制和反饋下層控制。

反饋上層控制作用于車輛縱向速度規(guī)劃器，為保證規(guī)劃結(jié)果的實(shí)時(shí)性，速度規(guī)劃是以當(dāng)前速度為基準(zhǔn)進(jìn)行車輛整體速度規(guī)劃的，因此速度反饋對于速度規(guī)劃非常重要。

反饋下層控制作用于車輛縱向速度控制器，縱向速度控制器是以上層規(guī)劃及模式判斷結(jié)果為基本參考量進(jìn)行速度控制的，但在一些特殊情況中(如停車制動(dòng))縱向速度控制器可根據(jù)當(dāng)前速度信息直接干預(yù)車輛控制，并且縱向速度控制器根據(jù)車速信息對控制量(如油門量、制動(dòng)量)進(jìn)行分級限制。

本發(fā)明公開了一種城市道路工況下適用廣泛并充分保證自動(dòng)駕駛車輛舒適性的分層式縱向規(guī)劃控制系統(tǒng)，并能結(jié)合自動(dòng)駕駛車輛平臺達(dá)到實(shí)際應(yīng)用效果。

一種應(yīng)用于上述系統(tǒng)的方法，具體包括：

S1、根據(jù)感知設(shè)備獲得的數(shù)據(jù)進(jìn)行局部路徑規(guī)劃，生成縱向局部路徑參數(shù)；

本步驟由局部路徑規(guī)劃器完成，如圖2所示具體步驟包括：

S11、感知設(shè)備獲取環(huán)境信息；

感知設(shè)備如毫米波雷達(dá)、激光測距儀及視頻攝像頭等獲得的數(shù)據(jù)作為局部路徑規(guī)劃器的輸入，示意圖如圖3所示。

S12、進(jìn)行環(huán)境信息解析與處理；該路徑規(guī)劃器以車輛前方某一寬度范圍內(nèi)的第一個(gè)目標(biāo)作為前車，以該目標(biāo)與本車的距離作為路徑長度L_init并同時(shí)得到前車的速度v_terminal以及本車速度v_init；

S13、以步長為l離散路徑，并向前探索一個(gè)步長；在該路徑上以l為步長離散出若干個(gè)路徑點(diǎn)得到縱向局部路徑，

S14、判斷是否遇到障礙物，否，則以車輛位置、方向等信息更新路徑長度并執(zhí)行步驟S13，是，則縱向局部路徑生成完畢；將上述參數(shù)作為局部路徑規(guī)劃器的輸出，輸入給縱向速度規(guī)劃器。

S2、根據(jù)縱向局部路徑參數(shù)進(jìn)行縱向速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果；

縱向速度規(guī)劃方法的設(shè)計(jì)原則如下：

a.為保證安全性，即保證足夠的制動(dòng)距離，應(yīng)在安全距離之外的路徑上進(jìn)行規(guī)劃，當(dāng)自動(dòng)駕駛車輛與前方障礙物的相對距離等于安全距離時(shí)，二者速度應(yīng)相等，即相對速度為零；

b.為保證行駛效率，應(yīng)在待規(guī)劃路徑的安全范圍內(nèi)盡可能地高速行駛，即以盡快的時(shí)間到達(dá)最終控制目標(biāo)；

c.為保證舒適性以及最大速度等限制，應(yīng)對加速度、速度進(jìn)行相應(yīng)約束。

基于上述原則，縱向速度規(guī)劃可分為三個(gè)步驟，首先進(jìn)行基本相對速度曲線規(guī)劃，然后根據(jù)限制條件對該曲線進(jìn)行約束，最后將之轉(zhuǎn)化為絕對速度規(guī)劃曲線。

S21、首先將縱向局部路徑參數(shù)作為初始規(guī)劃條件進(jìn)行相對速度曲線規(guī)劃，得到基本相對速度規(guī)劃曲線；

根據(jù)初始相對速度Δv_init(Δv_init＝v_init-v_terminal)、初始距離L_init等初始邊界條件的不同，可以衍生出若干種形式不同的基本規(guī)劃結(jié)果，其規(guī)劃流程圖如圖4所示。

其中，Δv_max＝v_max-v_terminal，v_max為該規(guī)劃路徑上的最大允許速度，由行駛環(huán)境決定；D_safe為安全距離，由安全距離模型得到,如式(1)所示：

D_safe＝τv_init+kv_terminal+d₀ (1)

其中τ為系統(tǒng)延時(shí)時(shí)間，k為安全距離系數(shù)，d₀為停車時(shí)的間距。

同時(shí)，圖4中所指的加速指加速度為a_a的行駛過程；勻速為加速度為零的行駛過程；減速為減速度為a_{d_exp}的行駛過程，其滿足a_{d_min}≤a_{d_exp}≤a_{d_max}<0。此外，由于該縱向速度規(guī)劃控制系統(tǒng)每過一個(gè)周期都會根據(jù)最新信息重新規(guī)劃，這段時(shí)間內(nèi)可視前方障礙物為勻速運(yùn)動(dòng)，因此，本發(fā)明的方法皆以前方障礙物勻速運(yùn)動(dòng)計(jì)算。

根據(jù)城市道路工況下不同的車距、車速情況，縱向速度規(guī)劃可分為如下若干種情況：

規(guī)劃結(jié)果1：當(dāng)相對距離L_init、安全距離D_safe、初始相對速度Δv_init和最大相對速度Δv_max分別滿足式(2)、式(3)、式(4)時(shí),以直接減速為規(guī)劃結(jié)果，減速度即為期望減速度a_{d_exp}，如圖5所示。

L_init>D_safe (2)

Δv_init≤Δv_max (3)

式(4)中，當(dāng)Δv_init>＝0，不等式左邊取符號；反之取負(fù)號。

圖5中，l為離散路徑點(diǎn)的步長(下同)；點(diǎn)O表示規(guī)劃起始狀態(tài)，點(diǎn)A表示規(guī)劃目的狀態(tài)，自動(dòng)駕駛車輛的縱向狀態(tài)從點(diǎn)O開始沿圖中曲線的方式變化，最終達(dá)到點(diǎn)A的目的狀態(tài)，即與前方障礙物的相對速度為零，相對距離為安全距離D_safe。

以Δv_i表示第i個(gè)離散路徑點(diǎn)上相對速度的規(guī)劃結(jié)果(下同)，則可得到如式(5)所示的規(guī)劃方程:

其中L_OA表示從初始狀態(tài)到達(dá)目的狀態(tài)時(shí)自動(dòng)駕駛車輛與前方障礙物之間相對距離的變化。

規(guī)劃結(jié)果2：若初始邊界條件滿足式(2)、式(3)，而不滿足式(4)，則可以存在加速段，即以加速、減速兩個(gè)階段為規(guī)劃結(jié)果，而這種情況下，規(guī)劃結(jié)果又可以出現(xiàn)兩種形式。

若Δv_init>＝0，則規(guī)劃結(jié)果第一種形式如圖6所示。

由此可得到該初始邊界條件下的規(guī)劃方程，如式(6)所示。

若初始條件Δv_init<0，則圖6將變形為圖7，得到該規(guī)劃結(jié)果的第二種形式。

該初始條件下的規(guī)劃方程如式(7)所示。

其中L_O→B＝L_OA+L_AB，L_O→C同理，下同。

規(guī)劃結(jié)果3：當(dāng)規(guī)劃初始條件滿足式(2)而不滿足式(3)時(shí)，需要判斷初始相對距離L_init是否足夠大，能夠使得規(guī)劃結(jié)果存在勻速段，判斷方法如式(8)所示。

若初始條件同時(shí)滿足式(8)，則規(guī)劃結(jié)果存在勻速段，即以減速、勻速、減速三個(gè)階段為規(guī)劃結(jié)果，如圖8所示：

該初始條件下的規(guī)劃方程如式(9)所示：

d.規(guī)劃結(jié)果4

若規(guī)劃初始條件滿足式(2)，不滿足式(3)和式(4)，則說明初始安全距離L_init不夠長，不能存在勻速段，因此需要直接減速，減速度為計(jì)算所得的期望減速度a_{d_exp}，計(jì)算方法同式(4)。此時(shí)規(guī)劃結(jié)果如圖9所示。

該初始條件下的規(guī)劃方程如式(10)所示：

規(guī)劃結(jié)果5：由于本發(fā)明針對于真實(shí)的城市交通環(huán)境，而這種工況中經(jīng)常會出現(xiàn)插車的現(xiàn)象，即等同于本車前方近距離處突然出現(xiàn)障礙物，并且該距離經(jīng)常會小于二者應(yīng)有的安全距離，因此同樣有必要對這種狀況進(jìn)行適當(dāng)?shù)奶幚怼?/p>

當(dāng)初始條件不滿足式(2)時(shí)，說明當(dāng)前時(shí)刻自動(dòng)駕駛車輛和前方障礙物的相對距離L_init已經(jīng)小于或等于安全距離D_safe，此時(shí)若還有Δv_init>0，即自動(dòng)駕駛車輛速度高于前方障礙物速度，則必須要減速行駛。

有必要指出的是，在減速行駛的過程中，二者的相對距離還會有一段減小的過程，直到二者速度相等，達(dá)到最小相對距離L_min。此時(shí)有必要引入一個(gè)允許最小相對距離D_min，若D_min<L_init≤D_safe，則可以根據(jù)D_min≤L_min的要求設(shè)計(jì)期望減速度a_{d_exp}；若L_init≤D_min，則認(rèn)為已經(jīng)處于危險(xiǎn)情況，此時(shí)應(yīng)直接采用最大減速度a_{d_min}。

允許最小相對距離D_min應(yīng)是一個(gè)與安全距離D_safe有關(guān)的量，可以以式(11)描述：

D_min＝βD_safe (11)

規(guī)劃結(jié)果如圖10所示。在該圖中，初始條件滿足D_min<L_init≤D_safe，則以D_min＝L_min的條件設(shè)計(jì)期望減速度，如式(12)所示：

自動(dòng)駕駛車輛的縱向狀態(tài)變化過程為S_O→S_A→S_B→S_C，其中S_O→S_B為以a_{d_exp}為減速度的減速過程，S_B→S_C為以a_a為加速度的加速過程，最終達(dá)到縱向目的狀態(tài)S_C。

該初始條件下的規(guī)劃方程如式(13)所示：

其中L_O→B、L_O→C意義同規(guī)劃結(jié)果2。

若初始條件有L_init≤D_min，則此時(shí)處于危險(xiǎn)情況，直接采用最大減速度a_{d_min}，即a_{d_exp}＝a_{d_min}。這種情況的規(guī)劃結(jié)果示意圖和規(guī)劃方程與圖10和式(13)一致，即二者只是計(jì)算期望減速度的方法不同而已。

規(guī)劃結(jié)果6：當(dāng)初始條件不滿足式(2)且有Δv_init≤0時(shí)，雖然自動(dòng)駕駛車輛與前方障礙物當(dāng)前的相對距離小于安全距離，但本車速度并不高于前方障礙物速度，故此時(shí)并不一定需要減速，這由L_init與D_safe的相對大小決定，如式(14)所示：

當(dāng)初始條件滿足式(14)時(shí)，說明自動(dòng)駕駛車輛與前方障礙物的相對距離已經(jīng)接近安全距離，此時(shí)的規(guī)劃結(jié)果如圖11所示。

該初始條件下的規(guī)劃方程如式(15)所示：

而當(dāng)初始條件不滿足式(14)時(shí)，則得到該規(guī)劃結(jié)果的第二種形式，如圖12所示：

這種情況下，雖然本車速度小于前方障礙物的速度，但二者相對距離已經(jīng)較小，為達(dá)到縱向目的狀態(tài)，需要先進(jìn)行減速后加速。

該初始條件下的規(guī)劃方程如式(16)所示：

以上即為幾種基本相對速度曲線的規(guī)劃結(jié)果，同時(shí)這幾種規(guī)劃結(jié)果已經(jīng)涵蓋了所有初始邊界條件可能出現(xiàn)的情況。

S22、根據(jù)限制條件對步驟S21的結(jié)果進(jìn)行約束，得到最終的相對速度規(guī)劃曲線；

限制條件包括縱向約束特性及橫向約束特性?？v向約束特性包括：最大速度約束、最大加速度約束、最大減速度約束；橫向約束特性包括：橫向穩(wěn)定性約束(即側(cè)向加速度約束)。

在步驟S21得到基本相對速度規(guī)劃曲線之后，還應(yīng)該對該曲線上的相對速度大小進(jìn)行限制，該限制主要包括最大允許速度Δv_max(對于基本規(guī)劃中已考慮該限制的不用再進(jìn)行相應(yīng)約束)、最小允許速度Δv_min以及橫向動(dòng)力學(xué)限制。

最大允許速度Δv_max主要由行駛環(huán)境決定，最小允許速度Δv_min除了決定于行駛環(huán)境外，還由絕對零速約束，如式(17)所示：

Δv_min＝-v_terminal (17)

最小允許速度指相對速度規(guī)劃中最小可以達(dá)到的速度，由公式Δv_init＝v_init-v_terminal可知，因速度規(guī)劃時(shí)車輛絕對速度不小于零，代入公式可計(jì)算Δv_min最小值。

與此同時(shí)，對于自動(dòng)駕駛車輛而言，其縱、橫向規(guī)劃系統(tǒng)應(yīng)當(dāng)協(xié)調(diào)控制才能實(shí)現(xiàn)安全行駛。橫向規(guī)劃以車輛避障為主，規(guī)劃結(jié)果為一條可行駛路徑，該路徑上的各路徑點(diǎn)都包含有轉(zhuǎn)向曲率的信息，因此為提高路徑跟蹤精度，同時(shí)滿足橫向安全性和舒適性要求，縱向速度規(guī)劃應(yīng)當(dāng)受橫向規(guī)劃的約束，該約束主要是由轉(zhuǎn)向曲率決定的車輛橫向動(dòng)力學(xué)約束。

車輛的橫向動(dòng)力學(xué)約束主要體現(xiàn)在橫向加速度上，只有對橫向加速度進(jìn)行限定才能保證不發(fā)生側(cè)滑、側(cè)翻等事故。根據(jù)車輛縱向速度與路徑曲率、橫向加速度之間的動(dòng)力學(xué)關(guān)系，可以對縱向速度進(jìn)行進(jìn)一步約束。

設(shè)期望路徑上車輛的最大橫向加速度為a_L，路徑上第i個(gè)路徑點(diǎn)的曲率為k_i，則該路徑點(diǎn)上的縱向速度約束如式(18)所示：

上述限制條件的約束可由式(19)和式(20)描述：

經(jīng)過約束，圖6可能變化為如圖13所示的形式，圖10可能變化為如圖14所示的形式。

S23、將最終的相對速度規(guī)劃曲線進(jìn)行絕對速度規(guī)劃，得到自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果；

經(jīng)過上述兩步，已經(jīng)得到了最終的相對速度規(guī)劃曲線，即Δv＝Δv(L)，由于上述規(guī)劃任意兩個(gè)相鄰點(diǎn)之間都是勻變速運(yùn)動(dòng)，故可以計(jì)算出相對速度規(guī)劃與時(shí)間t的關(guān)系，如式(21)所示：

其中t_i表示到達(dá)第i個(gè)路徑點(diǎn)的時(shí)間。

同時(shí)根據(jù)式(22)，可以得到自動(dòng)駕駛車輛的絕對速度v_init與時(shí)間t的關(guān)系v_init(t)：

v_init(t)＝Δv(t)+v_terminal(t)＝Δv(t)+v_terminal (22)

其中v_terminal意義同上，為規(guī)劃初始時(shí)刻前方障礙物的速度。

相對速度規(guī)劃不作為車輛真實(shí)期望速度進(jìn)行其他模塊速度判斷及使用，只有將相對速度轉(zhuǎn)換為大地坐標(biāo)系中車輛真實(shí)期望速度才可進(jìn)行車輛期望速度下發(fā)及使用。

至此，已經(jīng)得到了自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃。規(guī)劃曲線總是指向目的狀態(tài)，能有效達(dá)到最終控制目的，即保持安全距離的同時(shí)保證車速相等，也能保證緊急情況的制動(dòng)和停車；由于在可規(guī)劃路徑上采用了盡可能大的速度，能盡快達(dá)到目的狀態(tài)。

S3、根據(jù)自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果判斷行駛模式；

針對自動(dòng)駕駛車輛的行駛狀態(tài)，模擬駕駛員行駛意圖，本發(fā)明實(shí)施例中引入四種模式，分別為：加速模式、勻速模式、制動(dòng)模式、緊急制動(dòng)模式，各模式的判斷依據(jù)自動(dòng)駕駛車輛的縱向速度隨時(shí)間的規(guī)劃結(jié)果。

以某一規(guī)劃周期內(nèi)的規(guī)劃結(jié)果為例，如圖15所示：

S31、分析規(guī)劃結(jié)果產(chǎn)生的平均加速度

圖中縱坐標(biāo)為車輛絕對速度，橫坐標(biāo)為時(shí)間，規(guī)劃結(jié)果由加速、勻速、減速三段組成，t_terminal為達(dá)到縱向目的狀態(tài)的時(shí)間，ΔT為待分析時(shí)間段。

基于速度規(guī)劃結(jié)果，向前預(yù)測ΔT時(shí)刻，對當(dāng)前時(shí)刻起ΔT時(shí)間段內(nèi)的規(guī)劃情況進(jìn)行分析。為全面考慮ΔT時(shí)刻內(nèi)的整體情況，計(jì)算這段時(shí)間內(nèi)的平均速度作為該規(guī)劃周期的期望速度v_d，如式(23)所示：

與此同時(shí)，為分析該時(shí)間段內(nèi)的曲線形狀，考慮其平均加速度如式(24)所示：

S32、基于平均加速度分析行駛模式；

行駛模式由平均加速度的范圍得到，如表1所示：

表1行駛模式判斷表

上表中，a_a、a_{d_max}意義同上，即為標(biāo)準(zhǔn)加速和標(biāo)準(zhǔn)減速度；k_a、k_d、k_{d_h}分別為加速、制動(dòng)、緊急制動(dòng)模式的閾值系數(shù)，即當(dāng)時(shí)，認(rèn)為ΔT時(shí)間段內(nèi)規(guī)劃結(jié)果處于加速狀態(tài)，故該時(shí)刻應(yīng)該以加速為期望模式。

S321、改進(jìn)的行駛模式判斷分析

進(jìn)一步分析，如果某一個(gè)模式只存在一個(gè)閾值系數(shù)，即其進(jìn)出條件一樣，則當(dāng)在閾值左右波動(dòng)時(shí)，容易造成模式的來回切換，因此對每個(gè)模式設(shè)置雙閾值系數(shù)，并考慮上一周期行駛模式的歷史信息，保留一個(gè)緩沖區(qū)域，對行駛模式進(jìn)行濾波處理，避免其來回跳變。改進(jìn)后的行駛模式判斷表如表2所示：

表2改進(jìn)行駛模式判斷表

表格第一行為上一周期規(guī)劃模式，即車輛是基于之前規(guī)劃結(jié)果的再規(guī)劃，而非純加速度閾值判斷；第一列為加速度的范圍判斷，即根據(jù)規(guī)劃加速度判斷車輛處于的加速度區(qū)間段；其余為車輛根據(jù)模式判斷產(chǎn)生的當(dāng)前規(guī)劃模式結(jié)果，以此為基準(zhǔn)進(jìn)行模式控制。

上表中，k_a1、k_a2、k_d1、k_d2、k_{d_h1}、k_{d_h2}分別為相應(yīng)的閾值系數(shù)，都為正數(shù)。k_a1反映了規(guī)劃出加速模式的頻繁程度，此值越小，進(jìn)入加速模式的次數(shù)越頻繁，即車輛行駛越激進(jìn)，為了使控制效果盡量與規(guī)劃曲線一致，該值一般取0.7～1；k_a2為k_a1的對應(yīng)閾值系數(shù)，為保證足夠的緩沖區(qū)域，該值應(yīng)與k_a1有一定的偏差，同時(shí)為了充分利用勻速模式，該值不能太??；k_d2反映了規(guī)劃出制動(dòng)模式的頻繁程度，此值越小，進(jìn)入制動(dòng)模式的次數(shù)越頻繁，即車輛行駛越保守，為了使控制效果盡量與規(guī)劃曲線一致，該值一般取0.7～1；k_d1為k_d2的對應(yīng)閾值系數(shù)，該值同樣應(yīng)與k_d2有一定的偏差，且為了充分利用勻速模式該值不能太??；k_{d_h2}反映了規(guī)劃出緊急制動(dòng)模式的頻繁程度，同時(shí)也定義了緊急制動(dòng)時(shí)減速度的大小，該值的選取應(yīng)與駕駛習(xí)慣有關(guān)，如一般情況下，認(rèn)為緊急制動(dòng)時(shí)減速度超過-2m/s²，即當(dāng)期望減速度超過-2m/s²時(shí)應(yīng)該進(jìn)入緊急制動(dòng)模式，由此可推算出k_{d_h2}的大?。籯_{d_h1}為k_{d_h2}的對應(yīng)閾值系數(shù)，同樣應(yīng)與k_{d_h2}存在偏差，同時(shí)為使控制效果盡量與規(guī)劃曲線一致，該值一般取1左右。

此外，該表還能避免模式的躍變，即某一模式只能向其相鄰的模式變化，如緊急制動(dòng)模式不會直接變化為加速模式。至此，基于行駛模式分析的縱向速度規(guī)劃完成，得到最終的輸出量，即當(dāng)前時(shí)刻應(yīng)該進(jìn)行的行駛模式以及作為輔助控制的期望速度v_d。提出行駛模式的概念，具有向前預(yù)測性，能綜合考慮預(yù)測時(shí)間內(nèi)的整個(gè)規(guī)劃內(nèi)容，將加速度的概念模糊化，有效避免規(guī)劃方法中加速度突變的問題；在控制上以行駛模式為控制對象，可獨(dú)立設(shè)計(jì)各模式的控制方法，并能充分利用勻速模式小幅度精確控制的特性，實(shí)現(xiàn)加速、勻速、減速之間的平滑過渡和合理的油門、制動(dòng)切換，同時(shí)使用雙閾值濾波避免了加速度在切換邊界波動(dòng)時(shí)模式的突變和躍變，在達(dá)到規(guī)劃預(yù)期效果的同時(shí)有效提升乘坐舒適性，具有很強(qiáng)的實(shí)際應(yīng)用性；采用實(shí)驗(yàn)建表逆查詢的方法，回避了復(fù)雜的模型建立問題，減小計(jì)算量，同時(shí)建表的過程中就充分考慮了主觀舒適性，實(shí)際應(yīng)用時(shí)能得到良好的體現(xiàn)；

S4、進(jìn)行縱向速度控制；

S41、判斷在當(dāng)前行使模式下自動(dòng)駕駛車輛當(dāng)前速度所在速度區(qū)間；

速度區(qū)間判斷依據(jù)為事先定義完成的速度區(qū)間表，例如，以車輛最大速度限制為26.4m/s為例，可將速度區(qū)間劃分為0、1.5、3.5、7.4、11.3、14.9、18.8、26.4七個(gè)速度區(qū)間段，完成速度區(qū)間表。

該表由現(xiàn)實(shí)自動(dòng)駕駛車輛平臺實(shí)驗(yàn)所得，記錄了該平臺各換擋點(diǎn)的速度，以該速度點(diǎn)將整個(gè)速度范圍劃分為若干區(qū)間(以換擋點(diǎn)劃分速度段是為避免相同油門下?lián)Q擋造成加速度突變)。

S42、根據(jù)當(dāng)前行駛模式、當(dāng)前速度所在速度區(qū)間及期望速度進(jìn)行相應(yīng)控制流程；

當(dāng)處于加速模式時(shí)候，首先根據(jù)當(dāng)前速度所在區(qū)間逆查詢加速油門初值表，得到該區(qū)間對應(yīng)的加速油門初值C_gas1。

在速度跟隨控制中，一般需要建立精確的強(qiáng)非線性驅(qū)動(dòng)、制動(dòng)系統(tǒng)動(dòng)力學(xué)逆模型作為車速與節(jié)氣門開度、制動(dòng)控制量之間的橋梁，同時(shí)測量包括發(fā)動(dòng)機(jī)MAP圖在內(nèi)的諸多車輛動(dòng)力學(xué)參數(shù)，而這些參數(shù)往往難以準(zhǔn)確測量，而自動(dòng)駕駛車輛平臺本身的改裝性又進(jìn)一步增加了建立動(dòng)力學(xué)模型和測量參數(shù)的難度?；诖?，并借鑒模糊控制理論，本發(fā)明建立了若干個(gè)基于道路試驗(yàn)的油門量、制動(dòng)量查詢表，以此來回避上述必要的參數(shù)辨識問題，模擬人類駕駛員的經(jīng)驗(yàn)學(xué)習(xí)，降低了設(shè)計(jì)復(fù)雜性，同時(shí)提高了控制系統(tǒng)的魯棒性，具有很強(qiáng)的實(shí)用價(jià)值。

基于上述原因，加速模式中查詢的加速油門初值表同樣由現(xiàn)實(shí)自動(dòng)駕駛車輛平臺實(shí)驗(yàn)所得，該表記錄了各個(gè)速度區(qū)間達(dá)到a_a加速度的油門值。由于a_a的選取已經(jīng)考慮了自動(dòng)駕駛車輛具體的動(dòng)力學(xué)性能以及乘客的舒適度感受，因此該加速油門值能充分保證各個(gè)速度區(qū)間的舒適性要求。

除了油門初值外，最終的加速油門值還包括一個(gè)加速模式下的時(shí)間累加量C_gas2，即：

C_gas＝C_gas1+C_gas2 (25)

C_gas2為時(shí)間累加量，定義為油門增益，即認(rèn)為當(dāng)自動(dòng)駕駛車輛一直處于加速狀態(tài)時(shí)，車輛一直沒有達(dá)到目的狀態(tài)，即說明當(dāng)前油門不足以使之達(dá)到目的狀態(tài)，此時(shí)應(yīng)當(dāng)增大油門控制量，盡快結(jié)束加速模式。其算計(jì)方法如式(26)所示：

C_gas2＝k_gas2Δt (26)

其中，k_gas2為增益系數(shù)，Δt為加速模式下的連續(xù)累加時(shí)間，等同一個(gè)P調(diào)節(jié)。

與此同時(shí)，為防止油門量的無限增長，需要對之進(jìn)行飽和處理，即保證當(dāng)前速度區(qū)間的最終油門量不大于下一速度區(qū)間的油門初值，這樣同時(shí)也保證了車輛在不同速度區(qū)間變化時(shí)油門量的連續(xù)。

當(dāng)處于勻速模式時(shí)，勻速模式的控制方法與加速模式類似，首先根據(jù)當(dāng)前速度所在區(qū)間逆查詢勻速油門初值表得到勻速油門初值C_{gas_e1}。該表也由實(shí)車道路實(shí)驗(yàn)所得，記錄了各個(gè)速度區(qū)間保持勻速運(yùn)動(dòng)的油門值。

同樣地，最終的勻速油門量也由兩部分構(gòu)成，即：

C_{gas_e}＝C_{gas_e1}+C_{gas_e2} (27)

其中C_{gas_e2}為經(jīng)過PID調(diào)節(jié)的時(shí)間累加量。

由于勻速模式的定義不僅包括勻速行駛，還包括一定程度上的加速和減速，同時(shí)，勻速油門表是在特定試驗(yàn)工況下建立的，而實(shí)際行駛時(shí)，車輛自身狀態(tài)與外部環(huán)境均有可能發(fā)生較大的變化，因此，為了準(zhǔn)確體現(xiàn)勻速模式的功能，以期望速度為輔助控制參數(shù)通過離散PID控制方法對最終油門進(jìn)行微調(diào)，補(bǔ)償由于實(shí)際汽車總質(zhì)量、坡阻、風(fēng)阻等與實(shí)驗(yàn)條件不同帶來的誤差，實(shí)現(xiàn)期望速度的準(zhǔn)確跟蹤。

PID控制律如式(28)和式(29)所示：

ΔC_{gas_e2}(k)＝k_p[e(k)-e(k-1)]+k_ie(k)

+k_d[e(k)-2e(k-1)+e(k-2)] (28)

C_{gas_e2}(k)＝C_{gas_e2}(k-1)+ΔC_{gas_e2}(k) (29)

其中，k_p、k_i、k_d分別為比例、積分、微分系數(shù)；e(k)為第k次PID控制時(shí)期望速度v_d與當(dāng)前速度v_init的差值，即e(k)＝v_d-v_init。

與加速模式一樣，需要對勻速油門進(jìn)行飽和處理，或積分環(huán)節(jié)的反終結(jié)處理，即保證當(dāng)前速度區(qū)間的最終勻速油門量不大于該速度區(qū)間的加速油門初值，這樣同時(shí)也保證了車輛在勻速、加速模式變化時(shí)油門量的連續(xù)。

當(dāng)處于制動(dòng)、緊急制動(dòng)模式時(shí)，制動(dòng)、緊急制動(dòng)模式的最終制動(dòng)量同樣由兩部分組成，其計(jì)算方法與加速模式一致：首先查詢制動(dòng)或緊急制動(dòng)初值，再計(jì)算相應(yīng)模式下的時(shí)間累加量，最后對制動(dòng)或緊急制動(dòng)量進(jìn)行飽和處理，得到最終控制量。

對油門量、制動(dòng)量的飽和處理一方面保證了舒適性，一方面也避免了執(zhí)行機(jī)構(gòu)的大幅度調(diào)節(jié)；

進(jìn)一步地，各模式控制流程的時(shí)間累加量在模式切換時(shí)清零，開始新模式控制量的累加。

S43、本發(fā)明還設(shè)計(jì)了一個(gè)特殊處理流程，對一些特殊行駛情況或者自動(dòng)駕駛車輛特殊性能的處理，之后再將所得控制量發(fā)送到車體執(zhí)行機(jī)構(gòu)，體現(xiàn)到車輛動(dòng)力學(xué)特性上，形成控制閉環(huán)。

特殊處理流程包括部分正常規(guī)劃結(jié)果的直接處理方案，主要包括：非行進(jìn)擋無油門處理、油門制動(dòng)互鎖處理、依速度分級油門上限處理、緊急情況制動(dòng)處理、停車制動(dòng)處理、速度反饋直接控制處理等。其可以不依賴速度規(guī)劃器規(guī)劃結(jié)果直接進(jìn)行車輛控制，保證車輛行駛安全性與合理性。

本發(fā)明有益效果包括：

能與自動(dòng)駕駛車輛橫向規(guī)劃協(xié)調(diào)統(tǒng)一，有利于自動(dòng)駕駛車輛規(guī)劃控制方法的進(jìn)一步發(fā)展在不同場景下，自動(dòng)駕駛車輛可以調(diào)整速度使其與前方目標(biāo)車輛速度相同，同時(shí)與前方車輛保持一定的安全間距，也能保證緊急情況的制動(dòng)和停車?？v向規(guī)劃對車輛的所有縱向行駛情況進(jìn)行了分類處理，因此廣泛適用于各種城市道路工況，同時(shí)能兼容自動(dòng)駕駛車輛其他系統(tǒng)，如對感知系統(tǒng)的噪聲有一定抗干擾性，能適應(yīng)某些執(zhí)行機(jī)構(gòu)無法對連續(xù)量控制的特性。

本領(lǐng)域技術(shù)人員可以理解，實(shí)現(xiàn)上述實(shí)施例方法的全部或部分流程，可以通過計(jì)算機(jī)程序來指令相關(guān)的硬件來完成，所述的程序可存儲于計(jì)算機(jī)可讀存儲介質(zhì)中。其中，所述計(jì)算機(jī)可讀存儲介質(zhì)為磁盤、光盤、只讀存儲記憶體或隨機(jī)存儲記憶體等。

以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。