為滿足自動(dòng)駕駛行車&泊車過(guò)程的縱向控制功能需求，需開發(fā)行車功能的縱向控制VLC以及泊車功能的軌跡速度規(guī)劃TSP，其中，VLC負(fù)責(zé)將車輛縱向控制的加速度接口ax轉(zhuǎn)化為驅(qū)動(dòng)/制動(dòng)力Fx（包括前饋控制與反饋控制），TSP進(jìn)行S-V關(guān)系式擬合。

VLC控制算法簡(jiǎn)介

VLC技術(shù)方案簡(jiǎn)介

對(duì)于車輛的縱向控制，首先基于ACC模型并選擇合適的控制算法，如極點(diǎn)配置、LQR、MPC等算法（可參考公眾號(hào)相關(guān)文章），進(jìn)而通過(guò)解算得到系統(tǒng)的控制輸入量即期望加速度。忽略擾動(dòng)項(xiàng)（前車加速度），得到ACC模型如下：

上述式子中：d為兩車間的實(shí)際距離，為車頭時(shí)距，為兩車間靜止時(shí)的期望距離，為前車速度，為本車速度，τ為一階慣性環(huán)節(jié)的慣性時(shí)間常數(shù)。

控制算法在本處選用LQR，假設(shè)通過(guò)選取合適的Q及R后，解算得到反饋系數(shù)：，進(jìn)而得到期望加速度：

其中，K及x如上述算法所示。算法模型示意圖如下圖1所示：

圖1ACC模型及LQR算法示意圖

在得到期望加速度之后，根據(jù)車輛的縱向行駛方程進(jìn)一步計(jì)算得到期望的驅(qū)動(dòng)扭矩或者制動(dòng)壓力。車輛的縱向行駛方程如下所示：

即：

其中，為期望的驅(qū)動(dòng)力矩，為本車的重力，i為道路坡度（縱坡），為空氣阻力系數(shù)，A為迎風(fēng)面積，δ為旋轉(zhuǎn)質(zhì)量換算系數(shù)，m為本車質(zhì)量。

上述通過(guò)車輛縱向行駛方程所求得的期望驅(qū)動(dòng)力矩為前饋值，由于模型存在建模的誤差及不確定性，因此在此引入反饋對(duì)其進(jìn)行補(bǔ)償，本處所采用的反饋控制器為PID控制器，該控制器的輸入為期望加速度與實(shí)際加速度的偏差，控制器的控制輸出為驅(qū)動(dòng)扭矩，該值則為反饋值。通過(guò)前饋與反饋的結(jié)合，使得該算法具備了更為優(yōu)異的加速度跟蹤性能。算法的示意圖如下圖2所示：

圖2 前饋及反饋算法示意圖

算法的仿真結(jié)果圖（跟車場(chǎng)景：前車在過(guò)程中有啟停）如下圖所示：

圖3 兩車間期望距離及實(shí)際距離對(duì)比結(jié)果圖

圖4 本車速度及前車速度對(duì)比結(jié)果圖

圖5 ACC模型得到的期望加速度與本車實(shí)際加速度對(duì)比結(jié)果圖

由圖3、4及圖5可以得到上述算法在跟馳前車上表現(xiàn)較好，對(duì)加速度的跟蹤效果尚可。

上述參數(shù)中道路坡度、本車質(zhì)量及狀態(tài)變量中的本車速度需要通過(guò)參數(shù)估計(jì)及狀態(tài)估計(jì)得到，而其他一些參數(shù)則可以通過(guò)傳感器或者實(shí)驗(yàn)測(cè)量得到，在下面部分將詳細(xì)介紹對(duì)于待估計(jì)參數(shù)及狀態(tài)變量的參數(shù)估計(jì)算法及狀態(tài)估計(jì)算法。

車速估計(jì)

在縱坡等路段，車輛的加速度傳感器能夠測(cè)得車輛的縱向加速度，但由于坡度的存在，其測(cè)量值往往可能存在較大偏差，需要通過(guò)濾波算法，對(duì)加速度進(jìn)行重新估計(jì)，求得車輛速度、加速度的準(zhǔn)確估計(jì)后，可根據(jù)加速度的測(cè)量值和估計(jì)值估計(jì)得到路面的坡度。

首先，利用輪速傳感器測(cè)得的4個(gè)車輪轉(zhuǎn)速估算車速：

其中，為4個(gè)輪的輪速，可由輪速傳感器得到。為4個(gè)車輪旋轉(zhuǎn)的當(dāng)量車速。因?yàn)樵谲囕v運(yùn)動(dòng)過(guò)程中，輪胎不可避免地存在驅(qū)動(dòng)打滑、制動(dòng)打滑等狀態(tài)，所以需要對(duì)車輪打滑時(shí)地速度進(jìn)行修正，此時(shí)使用上一時(shí)刻的速度與加速度估算當(dāng)量車速。

其中，T為采樣周期，i為發(fā)生打滑的車輪。下一步將4個(gè)車輪旋轉(zhuǎn)的當(dāng)量車速按照從小到大進(jìn)行升序排列：

當(dāng)汽車加速度為正值時(shí)，驅(qū)動(dòng)輪可能發(fā)生打滑，從動(dòng)輪的當(dāng)量車速更加接近真實(shí)車速，選擇第三大的當(dāng)量車速作為參考車速；反之，選擇第二大當(dāng)量車速作為參考車速；否則將4個(gè)當(dāng)量車速的平均值作為參考車速：

其中，是判斷車輛驅(qū)動(dòng)或者制動(dòng)工況的加速度閾值，將由上式獲得的參考車速作為車速的測(cè)量值。 在得到車速的測(cè)量值之后，再結(jié)合卡爾曼濾波對(duì)其進(jìn)一步的處理，得到濾波后的車速值。具體過(guò)程如下：

（1）建立系統(tǒng)狀態(tài)方程：

假設(shè)車輛在一個(gè)周期內(nèi)勻加速行駛（CA模型），狀態(tài)變量，其中為當(dāng)前車輛行駛的速度，為當(dāng)前車輛行駛的加速度?？傻玫较到y(tǒng)的狀態(tài)方程：

其中，為高斯白噪聲。當(dāng)采樣周期時(shí)，狀態(tài)轉(zhuǎn)移矩陣為：

（2）建立測(cè)量方程：

車輛在當(dāng)前周期內(nèi)的行駛速度將使用上述的，加速度則通過(guò)加速度傳感器（IMU）獲得，則測(cè)量方程為：

其中，為測(cè)量噪聲。由于系統(tǒng)狀態(tài)完全可測(cè)，故觀測(cè)矩陣為H為單位矩陣。 （3）卡爾曼濾波： 給定濾波過(guò)程的初始狀態(tài)，再根據(jù)如下所示的卡爾曼濾波的5個(gè)公式即可對(duì)車速進(jìn)行濾波，最終得到車速的估計(jì)值及加速度的估計(jì)值。

其中，測(cè)量信息為：。 坡度估計(jì) 設(shè)上述通過(guò)卡爾曼濾波估計(jì)得到的加速度為，則可以根據(jù)下式估算道路坡度i：

其中，a為IMU測(cè)得的車輛縱向加速度，g為重力加速度，該方法的原理示意圖如下所示：

圖6 縱向加速度測(cè)量

質(zhì)量估計(jì)

對(duì)車輛縱向行駛方程進(jìn)行變形得：

考慮到公路的設(shè)計(jì)規(guī)范，道路坡度一般較小，因此對(duì)上式進(jìn)行簡(jiǎn)化，得：

假設(shè)質(zhì)量變化及坡度變化為一個(gè)慢時(shí)變過(guò)程，可得：

選取V、m、i為狀態(tài)變量，構(gòu)建狀態(tài)空間表達(dá)：

對(duì)其進(jìn)行離散化得：

選取車速作為觀測(cè)量，則系統(tǒng)的測(cè)量方程為：

由上述可得系統(tǒng)的狀態(tài)空間表達(dá)：

對(duì)上述系統(tǒng)使用擴(kuò)展卡爾曼濾波即可重構(gòu)得到系統(tǒng)狀態(tài)，即獲得了質(zhì)量的估計(jì)值。

TSP技術(shù)方案

TSP縱向速度規(guī)劃方案

在自動(dòng)泊車場(chǎng)景下，本車周圍通常存在許多的障礙物，如：墻壁、立柱、旁邊車位停放的車輛等。如何在不發(fā)生碰撞的情況下，將車輛自動(dòng)泊入所選定的車位，是自動(dòng)泊車中的主要技術(shù)點(diǎn)，即路徑規(guī)劃。而在完成了路徑規(guī)劃之后，也需要對(duì)其進(jìn)行相應(yīng)的速度規(guī)劃，使得車輛能夠安全、高效、舒適的完成自動(dòng)泊車任務(wù)。

從人類駕駛員的經(jīng)驗(yàn)出發(fā)，一般距離障礙物越近時(shí)，人類駕駛員往往會(huì)選擇降低車速，緩行通過(guò)，因此，本次TSP縱向速度規(guī)劃主要是基于上述經(jīng)驗(yàn)，對(duì)泊車過(guò)程中的本車速度進(jìn)行規(guī)劃。

由上所述，本次TSP縱向速度規(guī)劃目標(biāo)是得到本車速度與本車與障礙物之間的距離d的關(guān)系。在經(jīng)過(guò)分析后，本次規(guī)劃選取三次多項(xiàng)式曲線來(lái)進(jìn)行速度規(guī)劃，即速度與加速度的多項(xiàng)式映射關(guān)系為：

首先確定邊界條件。在規(guī)劃的初始點(diǎn)，本車與障礙物之間的初始距離為，而本車當(dāng)前速度，當(dāng)前加速度；在規(guī)劃的結(jié)束點(diǎn)，車輛成功泊入車位，本車與障礙物的最終距離為，本車規(guī)劃的速度為。將上述兩個(gè)邊界條件帶入三次多項(xiàng)式中，可得：

考慮到上述三個(gè)邊界條件帶入三次多項(xiàng)式曲線后得到的方程組求解的復(fù)雜度，在此處通過(guò)引入中間變量的方式對(duì)其進(jìn)行變形，以簡(jiǎn)化求解，提高算法的實(shí)時(shí)性。通過(guò)分析，選取本車從泊車開始累計(jì)的行駛距離S作為中間變量，該中間變量與d之間的關(guān)系如下式：

將上式帶入三次多項(xiàng)式曲線中，可得到以S作為自變量的新的三次多項(xiàng)式曲線，表達(dá)式如下：

此時(shí)，再將三個(gè)邊界條件帶入新的三次多項(xiàng)式曲線中可得：

其中，為泊車過(guò)程中本車所行駛過(guò)的總距離。對(duì)上式進(jìn)行解方程組可得：

將上式方程組的解代回到速度曲線的三次多項(xiàng)式，可得：

因此，由三個(gè)邊界條件所得到的三個(gè)約束條件，使得三次多項(xiàng)式曲線的自由變量（系數(shù)）只剩下一個(gè)，最后將結(jié)合優(yōu)化的方法來(lái)求得該系數(shù)的最優(yōu)值，在此處該自由變量為。

對(duì)上式兩邊同時(shí)對(duì)時(shí)間求一階及二階導(dǎo)數(shù)，可得速度和加速度分別為：

縱向速度優(yōu)化模型

從舒適性的角度出發(fā)，加速度與沖擊度應(yīng)當(dāng)盡量的??；從效率的角度出發(fā)，希望車輛完成泊車任務(wù)的時(shí)間盡可能的短；因此，在考慮上述目標(biāo)后，設(shè)定目標(biāo)函數(shù)如下：

其中，目標(biāo)函數(shù)中的第一項(xiàng)的物理意義為完成泊車過(guò)程所用的時(shí)間，即對(duì)應(yīng)效率的目標(biāo)，第二項(xiàng)及第三項(xiàng)則對(duì)應(yīng)舒適性的目標(biāo)即平穩(wěn)泊車的目標(biāo)。為優(yōu)化的權(quán)重系數(shù)。最后將上面的的表達(dá)式帶入該代價(jià)函數(shù)，化簡(jiǎn)可得關(guān)于變量的代價(jià)函數(shù)如下式：

上述代價(jià)函數(shù)為關(guān)于的一元多次函數(shù)，目標(biāo)是找到最優(yōu)的讓代價(jià)函數(shù)取得最小值。對(duì)于一元函數(shù)求極值的問(wèn)題，使用KKT原理是比較有效的方法，因此，對(duì)上式關(guān)于進(jìn)行求導(dǎo)，并令該式等于0得到若干個(gè)解，最后比較這幾個(gè)點(diǎn)的函數(shù)值即可確定最優(yōu)的解，而根據(jù)的關(guān)系也得到，至此所規(guī)劃的速度曲線已經(jīng)被唯一確定，完成了TSP縱向速度規(guī)劃。

泊車速度規(guī)劃求解

TSP車速規(guī)劃相關(guān)的算法如下圖示：

在本次的算法測(cè)試中初始條件設(shè)置為：，最后求得的，完成整個(gè)泊車過(guò)程的時(shí)間為2.286s，帶入得到：

，的關(guān)系圖如下圖所示：

圖7 規(guī)劃的速度與加速度

上述算法還是較復(fù)雜，考慮到其實(shí)時(shí)求解問(wèn)題，可將其簡(jiǎn)化，假設(shè)，則簡(jiǎn)化后關(guān)于變量的代價(jià)函數(shù)如下式：

其極值點(diǎn)為：

考慮到上述極值點(diǎn)條件有多個(gè)可行解，快速尋找其最優(yōu)解也是一個(gè)較為復(fù)雜的問(wèn)題，可利用泊車完成一半時(shí)候的規(guī)劃速度非負(fù)這個(gè)條件來(lái)加以判斷，因此設(shè)定驗(yàn)證條件如下所示：

在同樣邊界條件下可求得有兩個(gè)可行解，分別如下圖所示。

圖8 規(guī)劃的速度與加速度（可行解1）

圖9 規(guī)劃的速度與加速度（可行解2）

?審核編輯：湯梓紅