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

VLC控制算法簡介

VLC技術方案簡介

對于車輛的縱向控制，首先基于ACC模型并選擇合適的控制算法，如極點配置、LQR、MPC等算法（可參考公眾號相關文章），進而通過解算得到系統的控制輸入量即期望加速度。忽略擾動項（前車加速度），得到ACC模型如下：

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

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

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

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

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

即：

其中，為期望的驅動力矩，為本車的重力，i為道路坡度（縱坡），為空氣阻力系數，A為迎風面積，δ為旋轉質量換算系數，m為本車質量。

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

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

算法的仿真結果圖（跟車場景：前車在過程中有啟停）如下圖所示：

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

圖4 本車速度及前車速度對比結果圖

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

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

上述參數中道路坡度、本車質量及狀態變量中的本車速度需要通過參數估計及狀態估計得到，而其他一些參數則可以通過傳感器或者實驗測量得到，在下面部分將詳細介紹對于待估計參數及狀態變量的參數估計算法及狀態估計算法。

車速估計

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

首先，利用輪速傳感器測得的4個車輪轉速估算車速：

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

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

當汽車加速度為正值時，驅動輪可能發生打滑，從動輪的當量車速更加接近真實車速，選擇第三大的當量車速作為參考車速；反之，選擇第二大當量車速作為參考車速；否則將4個當量車速的平均值作為參考車速：

其中，是判斷車輛驅動或者制動工況的加速度閾值，將由上式獲得的參考車速作為車速的測量值。 在得到車速的測量值之后，再結合卡爾曼濾波對其進一步的處理，得到濾波后的車速值。具體過程如下：

（1）建立系統狀態方程：

假設車輛在一個周期內勻加速行駛（CA模型），狀態變量，其中為當前車輛行駛的速度，為當前車輛行駛的加速度。可得到系統的狀態方程：

其中，為高斯白噪聲。當采樣周期時，狀態轉移矩陣為：

（2）建立測量方程：

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

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

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

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

圖6 縱向加速度測量

質量估計

對車輛縱向行駛方程進行變形得：

考慮到公路的設計規范，道路坡度一般較小，因此對上式進行簡化，得：

假設質量變化及坡度變化為一個慢時變過程，可得：

選取V、m、i為狀態變量，構建狀態空間表達：

對其進行離散化得：

選取車速作為觀測量，則系統的測量方程為：

由上述可得系統的狀態空間表達：

對上述系統使用擴展卡爾曼濾波即可重構得到系統狀態，即獲得了質量的估計值。

TSP技術方案

TSP縱向速度規劃方案

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

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

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

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

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

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

此時，再將三個邊界條件帶入新的三次多項式曲線中可得：

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

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

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

對上式兩邊同時對時間求一階及二階導數，可得速度和加速度分別為：

縱向速度優化模型

從舒適性的角度出發，加速度與沖擊度應當盡量的小；從效率的角度出發，希望車輛完成泊車任務的時間盡可能的短；因此，在考慮上述目標后，設定目標函數如下：

其中，目標函數中的第一項的物理意義為完成泊車過程所用的時間，即對應效率的目標，第二項及第三項則對應舒適性的目標即平穩泊車的目標。為優化的權重系數。最后將上面的的表達式帶入該代價函數，化簡可得關于變量的代價函數如下式：

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

泊車速度規劃求解

TSP車速規劃相關的算法如下圖示：

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

，的關系圖如下圖所示：

圖7 規劃的速度與加速度

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

其極值點為：

考慮到上述極值點條件有多個可行解，快速尋找其最優解也是一個較為復雜的問題，可利用泊車完成一半時候的規劃速度非負這個條件來加以判斷，因此設定驗證條件如下所示：

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

圖8 規劃的速度與加速度（可行解1）

圖9 規劃的速度與加速度（可行解2）

?審核編輯：湯梓紅