面向行泊一體的車輛縱向控制算法

點擊上方藍字談思實驗室

獲取更多汽車網(wǎng)絡(luò)安全資訊

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

VLC控制算法簡介

VLC技術(shù)方案簡介

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，是判斷車輛驅(qū)動或者制動工況的加速度閾值，將由上式獲得的參考車速作為車速的測量值。

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

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

（2）建立測量方程：

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

其中，為測量噪聲。由于系統(tǒng)狀態(tài)完全可測，故觀測矩陣為H為單位矩陣。

其中，測量信息為：。

設(shè)上述通過卡爾曼濾波估計得到的加速度為，則可以根據(jù)下式估算道路坡度i：

圖6 縱向加速度測量

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

TSP技術(shù)方案

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

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

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

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

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

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

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

其中，為泊車過程中本車所行駛過的總距離。對上式進行解方程組可得：

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

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

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

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

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

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

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

在本次的算法測試中初始條件設(shè)置為：，最后求得的，完成整個泊車過程的時間為2.286s，帶入得到：，的關(guān)系圖如下圖所示：

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

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

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

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