李林潤(rùn) 張建 姜洪偉 劉秋錚 王宇

（1. 中國(guó)第一汽車(chē)股份有限公司智能網(wǎng)聯(lián)開(kāi)發(fā)院，長(zhǎng)春 130013；2. 汽車(chē)振動(dòng)噪聲與安全控制綜合技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)春 130013）

主題詞：縱向坡度估計(jì) 側(cè)向加速度估計(jì) 卡爾曼濾波 轉(zhuǎn)向補(bǔ)償

EPS Electric Power Steering

ABS Antilock Brake System

ESP Electronic Stability Program

1 前言

汽車(chē)智能化與電動(dòng)化的發(fā)展對(duì)縱向運(yùn)動(dòng)控制、主動(dòng)安全控制和能量回收等功能提出了更高要求。重力在傾斜路面下產(chǎn)生的縱向分量會(huì)顯著影響車(chē)輛縱向動(dòng)力學(xué)特性，進(jìn)而影響上述功能的控制效果。以車(chē)輛右手坐標(biāo)系為基準(zhǔn)，重力縱向分量取決于車(chē)輛平面下的路面傾斜程度，即車(chē)輛縱向坡度(Grade)。因此，實(shí)時(shí)穩(wěn)定獲取車(chē)輛縱向坡度，確定重力縱向分量對(duì)提升智能電動(dòng)汽車(chē)的諸多控制系統(tǒng)性能具有實(shí)際意義。

目前常見(jiàn)的縱向坡度獲取方法可以被劃分為基于信號(hào)處理技術(shù)的傳感器信息時(shí)頻分析方法和基于汽車(chē)運(yùn)動(dòng)狀態(tài)模型的估計(jì)方法。前者主要使用各種傳感器信息，通過(guò)濾波處理、幾何關(guān)系計(jì)算等方式實(shí)現(xiàn)對(duì)路面傾角的直接測(cè)量，該類(lèi)方法一般需要加裝額外的傳感器，成本較高，實(shí)際應(yīng)用困難。后者根據(jù)所用運(yùn)動(dòng)狀態(tài)模型，分為基于動(dòng)力學(xué)模型的方法和基于運(yùn)動(dòng)學(xué)模型的方法：動(dòng)力學(xué)方法利用車(chē)載CAN 網(wǎng)絡(luò)中的力信號(hào)與車(chē)速信號(hào)構(gòu)建包含車(chē)輛縱向坡度的縱向動(dòng)力學(xué)觀測(cè)器，利用最小二乘參數(shù)辨識(shí)算法或卡爾曼濾波最優(yōu)估計(jì)方法實(shí)現(xiàn)路面坡度的實(shí)時(shí)準(zhǔn)確估算。該類(lèi)方法需額外處理制動(dòng)、換擋特殊工況，且模型參數(shù)受高頻噪聲影響較大，易導(dǎo)致估值不穩(wěn)定。運(yùn)動(dòng)學(xué)方法主要利用固定于車(chē)身上的縱軸加速度傳感器，構(gòu)建包含重力加速度縱向分量的縱向運(yùn)動(dòng)學(xué)觀測(cè)器，其狀態(tài)空間方程簡(jiǎn)單，采用卡爾曼濾波最優(yōu)估計(jì)方法可實(shí)現(xiàn)路面坡度的實(shí)時(shí)準(zhǔn)確估算。此類(lèi)方法易于調(diào)試、計(jì)算量小并且落實(shí)成本低，廣泛應(yīng)用于實(shí)際工程當(dāng)中。

基于汽車(chē)運(yùn)動(dòng)狀態(tài)模型的車(chē)輛縱向坡度估計(jì)方法的效果取決于模型精度，傳統(tǒng)的單自由度縱向運(yùn)動(dòng)學(xué)模型忽略了車(chē)輛在轉(zhuǎn)彎工況下的橫縱向耦合特性，未考慮車(chē)輛側(cè)向與橫擺運(yùn)動(dòng)對(duì)縱向運(yùn)動(dòng)狀態(tài)的影響，造成觀測(cè)器模型失真，導(dǎo)致車(chē)輛在傾斜路面轉(zhuǎn)彎或掉頭工況下，縱向坡度估計(jì)偏差較大。

本文針對(duì)該問(wèn)題，提出基于轉(zhuǎn)向補(bǔ)償?shù)能?chē)輛縱向坡度估計(jì)方法：分析車(chē)輛轉(zhuǎn)向時(shí)側(cè)向與橫擺運(yùn)動(dòng)對(duì)縱向運(yùn)動(dòng)的耦合影響機(jī)理；根據(jù)車(chē)輛二自由度動(dòng)力學(xué)模型設(shè)計(jì)側(cè)向速度觀測(cè)器；設(shè)計(jì)基于運(yùn)動(dòng)學(xué)的卡爾曼濾波縱向坡度估計(jì)算法，使用橫擺角速度與側(cè)向速度觀測(cè)對(duì)車(chē)身縱軸加速度信號(hào)進(jìn)行校正補(bǔ)償，最終實(shí)現(xiàn)了車(chē)輛橫縱向耦合工況下的縱向坡度可靠估計(jì)。

2 算法整體架構(gòu)

本文提出的基于轉(zhuǎn)向補(bǔ)償?shù)能?chē)輛縱向坡度估計(jì)算法整體架構(gòu)如圖1所示。

圖1 轉(zhuǎn)向補(bǔ)償縱向坡度估計(jì)算法架構(gòu)

3 車(chē)輛狀態(tài)耦合分析與建模

車(chē)輛轉(zhuǎn)彎工況下的橫縱向狀態(tài)之間存在較強(qiáng)相關(guān)性，分析其耦合機(jī)理是準(zhǔn)確建立狀態(tài)觀測(cè)模型的基礎(chǔ)。

3.1 橫縱向運(yùn)動(dòng)耦合特性分析

圖2 為車(chē)輛轉(zhuǎn)彎工況下的瞬態(tài)運(yùn)動(dòng)學(xué)過(guò)程，其中為固定于地面的坐標(biāo)系。xOy為固定于車(chē)輛的坐標(biāo)系，坐標(biāo)原點(diǎn)位于車(chē)輛的質(zhì)心O，其中代表車(chē)輛縱向，代表車(chē)輛側(cè)向，橫擺方向取逆時(shí)針為正。矢量代表車(chē)輛質(zhì)心O在大地坐標(biāo)系中的位置矢量。

圖2中，O點(diǎn)的速度矢量可以表示為：

圖2 車(chē)輛轉(zhuǎn)彎運(yùn)動(dòng)學(xué)示意

式中，和分別表示軸和軸方向的單位矢量；v和v分別表示車(chē)輛在和兩個(gè)方向上的速度分量。

上式進(jìn)一步對(duì)時(shí)間求導(dǎo)可得O點(diǎn)的加速度矢量，如式（2）所示：

3.2 考慮轉(zhuǎn)向補(bǔ)償?shù)目v向運(yùn)動(dòng)學(xué)模型

圖3 縱軸加速度信號(hào)模型

由于車(chē)輛縱向坡度的實(shí)際變化速度相較采樣時(shí)間和車(chē)輛動(dòng)力學(xué)狀態(tài)變化緩慢，因此其導(dǎo)數(shù)可近似為0。

3.3 側(cè)向速度估計(jì)模型

準(zhǔn)確獲取橫擺角速度ω和側(cè)向速度v是校正補(bǔ)償?shù)那疤?，前者可以通過(guò)車(chē)載傳感器直接獲取，后者需采用基于模型的估計(jì)方法獲取，用于估計(jì)的側(cè)向動(dòng)力學(xué)模型如圖4所示。

圖4 二自由度車(chē)輛動(dòng)力學(xué)模型

根據(jù)該模型的車(chē)輪小角度轉(zhuǎn)動(dòng)假設(shè)，建立車(chē)輛動(dòng)力學(xué)平衡方程。

輪胎側(cè)向力可以通過(guò)線性輪胎模型表示。

式中，α和α分別為前后輪胎側(cè)偏角；C和C分別為前后輪胎側(cè)偏剛度。

聯(lián)合式（7）、式（8）最終得到二自由度車(chē)輛動(dòng)力學(xué)方程為：

4 考慮轉(zhuǎn)彎補(bǔ)償?shù)目v坡卡爾曼濾波估計(jì)

4.1 卡爾曼濾波方法

卡爾曼濾波算法能夠根據(jù)當(dāng)前時(shí)刻離散狀態(tài)方程的預(yù)測(cè)值、離散觀測(cè)方程的觀測(cè)值與上一時(shí)刻離散的最優(yōu)估值，通過(guò)不需要存儲(chǔ)歷史數(shù)據(jù)的遞推過(guò)程，計(jì)算出新的最優(yōu)估計(jì)，利用有限的、不直接的測(cè)量信息實(shí)現(xiàn)目標(biāo)參數(shù)的實(shí)時(shí)估計(jì)。

該算法具體包含2個(gè)更新過(guò)程，共5個(gè)步驟，整體流程如圖5所示。

圖5 卡爾曼濾波道路坡度估計(jì)算法流程

時(shí)間更新過(guò)程包含如下2個(gè)步驟：

（1）狀態(tài)預(yù)測(cè)：通過(guò)上一時(shí)刻的最優(yōu)估值計(jì)算當(dāng)前時(shí)刻的預(yù)測(cè)狀態(tài)，具體如式（10）。

（2）協(xié)方差預(yù)測(cè)：通過(guò)上一時(shí)刻的最優(yōu)估值誤差協(xié)方差計(jì)算當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的誤差協(xié)方差，具體如式（11）。

測(cè)量更新過(guò)程包含如下3個(gè)步驟：

（1）卡爾曼增益計(jì)算：通過(guò)當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的誤差協(xié)方差和測(cè)量噪聲協(xié)方差來(lái)計(jì)算卡爾曼增益，具體如式（12）。

式中，為觀測(cè)矩陣；測(cè)量噪聲協(xié)方差。

（2）狀態(tài)更新：通過(guò)當(dāng)前時(shí)刻的預(yù)測(cè)狀態(tài)、測(cè)量值和卡爾曼增益計(jì)算最優(yōu)估值，具體如式（13）。

式中,為系統(tǒng)狀態(tài)觀測(cè)。

（3）協(xié)方差更新：通過(guò)當(dāng)前時(shí)刻預(yù)測(cè)狀態(tài)的誤差協(xié)方差和卡爾曼增益計(jì)算最優(yōu)估值誤差協(xié)方差，具體如式（14）。

將系統(tǒng)模型離散化帶入式（10），并設(shè)定系統(tǒng)初始參數(shù)后，反復(fù)迭代上述5個(gè)步驟，即可實(shí)現(xiàn)基于卡爾曼濾波的狀態(tài)估計(jì)。

4.2 側(cè)向速度的卡爾曼濾波估計(jì)

將式（9）離散化，得到用于卡爾曼濾波估計(jì)的狀態(tài)預(yù)測(cè)和測(cè)量式（15）。

將式（15）帶入上述卡爾曼濾波遞推式（10）～式（14），并設(shè)定系統(tǒng)初始參數(shù)，即可實(shí)現(xiàn)側(cè)向速度的實(shí)時(shí)估計(jì)，并將其用于縱向坡度估計(jì)的轉(zhuǎn)向補(bǔ)償。

4.3 車(chē)輛縱向坡度的卡爾曼濾波估計(jì)

將考慮轉(zhuǎn)向補(bǔ)償?shù)目v向運(yùn)動(dòng)學(xué)模型式（6）離散化，結(jié)合上述側(cè)向速度估計(jì)值，得到用于卡爾曼濾波估計(jì)的狀態(tài)預(yù)測(cè)和測(cè)量方程式（16）。

將式（16）帶入上述卡爾曼濾波遞推公式，并設(shè)定系統(tǒng)初始參數(shù)，即可實(shí)現(xiàn)車(chē)輛縱向坡度的實(shí)時(shí)估計(jì)。

5 測(cè)試驗(yàn)證

5.1 仿真測(cè)試驗(yàn)證

利用MATLAB∕Simulink 編寫(xiě)考慮轉(zhuǎn)彎補(bǔ)償?shù)穆访嫫露裙烙?jì)算法，并利用CarSim提供的車(chē)輛動(dòng)力學(xué)模型和仿真場(chǎng)景，進(jìn)行聯(lián)合仿真驗(yàn)證。

與本算法相關(guān)的仿真車(chē)輛參數(shù)如表1 所示，均與后續(xù)實(shí)車(chē)參數(shù)對(duì)應(yīng)。

表1 仿真車(chē)輛參數(shù)

考慮基于式（6）的坡度估計(jì)原理簡(jiǎn)單，其狀態(tài)預(yù)測(cè)方程相對(duì)精確，因此過(guò)程噪聲協(xié)方差矩陣較小。由于式（9）中部分參數(shù)不易準(zhǔn)確獲取，且二自由度模型本身存在一定簡(jiǎn)化，因此側(cè)向速度估計(jì)的過(guò)程噪聲協(xié)方差矩陣相對(duì)較大。測(cè)量更新過(guò)程中，傳感器精度較高，測(cè)量誤差小，因此測(cè)量噪聲協(xié)方差均較小，最終得到的算法預(yù)設(shè)參數(shù)如表2所示。

表2 算法預(yù)設(shè)參數(shù)

設(shè)置仿真車(chē)輛期望行駛路徑如圖6 所示，以此為基礎(chǔ)設(shè)置2種仿真道路。

圖6 仿真工況示意

（1）道路傾斜方向（縱向坡度角）沿期望路徑保持不變，如圖7(a)所示。此工況用于分析車(chē)輛側(cè)向運(yùn)動(dòng)對(duì)縱向坡度估計(jì)結(jié)果的直接影響，也可以反應(yīng)山區(qū)道路的坡度設(shè)置情況。

（2）道路傾斜方向固定，如圖7(b)所示。此工況下，AB段車(chē)輛縱向坡度角與道路傾斜角度一致，BC段車(chē)輛縱向坡度角逐漸減小，CD段為0。該工況可以體現(xiàn)車(chē)輛在坡道上掉頭時(shí)，車(chē)輛縱向坡度角沿期望路徑變化的情況。

圖7 仿真道路示意

設(shè)置道路傾斜程度=0.1，車(chē)速保持36 km∕h，單位采樣時(shí)間=0.001 s，得到如圖（8）所示的仿真結(jié)果。

由于縱軸加速度傳感器固定于車(chē)身上，基于運(yùn)動(dòng)學(xué)的縱向坡度估計(jì)算法實(shí)質(zhì)是對(duì)車(chē)身俯仰角的估計(jì)。當(dāng)車(chē)速穩(wěn)定時(shí)，俯仰角波動(dòng)較小，能夠較好反映縱向坡度，因此仿真階段以車(chē)身俯仰角為真值，保證了與實(shí)車(chē)測(cè)試真值對(duì)應(yīng)。圖（8）中實(shí)際值的波動(dòng)是由于CarSim 軟件內(nèi)置的車(chē)速控制模塊引起的實(shí)際車(chē)速波動(dòng)導(dǎo)致，但是由此導(dǎo)致的車(chē)身俯仰角波動(dòng)幅度在0.007 rad 以?xún)?nèi)，遠(yuǎn)小于車(chē)輛所處的坡道縱向坡度。

道路1 的傾斜方向隨期望路徑航向?qū)崟r(shí)變化，車(chē)輛縱向坡度始終為0.10 左右，如圖8(a)中實(shí)線所示。在里程 50～110 m 時(shí)，車(chē)輛處于圖 6 中 BC 段，轉(zhuǎn)向運(yùn)動(dòng)導(dǎo)致基于直線運(yùn)動(dòng)學(xué)的縱向坡度估計(jì)方法失真，未進(jìn)行轉(zhuǎn)向校正的縱向坡度估計(jì)誤差較大，與實(shí)際值的最大絕對(duì)誤差為0.022 1，與實(shí)際值均值的最大相對(duì)誤差為21.5%；而校正后的估計(jì)誤差明顯減小，最大絕對(duì)誤差為0.001 9，最大相對(duì)誤差僅為1.86%。

圖8 0.10坡道縱向坡度估計(jì)結(jié)果對(duì)比

道路2的傾斜方向不隨路徑變化，車(chē)輛處于AB段時(shí)車(chē)輛縱向坡度始終等于道路傾斜程度，當(dāng)車(chē)輛處于BC段時(shí)，車(chē)輛縱向坡度隨行駛航向逐漸變小，直到車(chē)輛處于CD段，道路傾斜方向與車(chē)輛軸重合，完全處于側(cè)坡工況，車(chē)輛縱向坡度為0，具體實(shí)際值變化如圖8(b)中實(shí)線所示。在里程50～110 m 時(shí)，車(chē)輛處于BC 段，轉(zhuǎn)向運(yùn)動(dòng)導(dǎo)致?tīng)顟B(tài)空間方程（1）失真，未校正的估計(jì)誤差較大，與實(shí)際值的最大絕對(duì)誤差為0.024 2,與實(shí)際值均值的最大相對(duì)誤差為37.4%；而校正后的估計(jì)誤差明顯減小，最大絕對(duì)誤差為0.002 4，最大相對(duì)誤差為3.75%。

為進(jìn)一步說(shuō)明該校正方法的有效性，多次更改坡度進(jìn)行仿真，統(tǒng)計(jì)得到道路1和道路2下的誤差如表3和表4所示。可見(jiàn)本文提出的側(cè)向運(yùn)動(dòng)補(bǔ)償方案能夠有效提高車(chē)輛轉(zhuǎn)彎工況下的坡度估計(jì)精度。

表3 道路1估計(jì)誤差對(duì)比統(tǒng)計(jì)表

表4 道路2估計(jì)誤差對(duì)比統(tǒng)計(jì)表

5.2 實(shí)車(chē)測(cè)試驗(yàn)證

實(shí)車(chē)驗(yàn)證階段，將所開(kāi)發(fā)算法嵌入到以dSPACE MicroAutobox 為原型控制器的實(shí)車(chē)測(cè)試平臺(tái)上?？刂破魍ㄟ^(guò)CAN 總線實(shí)時(shí)采集原車(chē)電動(dòng)助力轉(zhuǎn)向系統(tǒng)EPS 的方向盤(pán)轉(zhuǎn)角信號(hào)，制動(dòng)防抱死系統(tǒng)ABS 的縱向車(chē)速信號(hào)，車(chē)身穩(wěn)定控制系統(tǒng)ESP 的縱軸加速度和橫擺角速度信號(hào)，實(shí)現(xiàn)車(chē)輛縱向坡度估計(jì)，通過(guò)RT3000 組合慣導(dǎo)裝置獲取車(chē)身俯仰角，作為縱向坡度真值。具體的實(shí)車(chē)測(cè)試平臺(tái)系統(tǒng)架構(gòu)與實(shí)車(chē)布置圖如圖9、圖10 所示。測(cè)試在一處郊區(qū)道路上進(jìn)行，道路坡度最大在0.05，實(shí)際行駛軌跡如圖11 所示。

圖9 實(shí)車(chē)測(cè)試平臺(tái)系統(tǒng)架構(gòu)

圖10 實(shí)車(chē)布置

圖11 車(chē)輛實(shí)際行駛軌跡

軌跡前半段進(jìn)行了直線行駛工況對(duì)比測(cè)試，如圖12所示。由測(cè)試結(jié)果可以看出，本文提出的縱向坡度估計(jì)算法計(jì)算結(jié)果穩(wěn)定，且與實(shí)際值基本相符，平均估計(jì)誤差能夠保持在0.01以?xún)?nèi)，證明了該算法在直線行駛工況中的有效性。

圖12 直線行駛工況測(cè)試

為進(jìn)一步說(shuō)明本文提出的側(cè)向運(yùn)動(dòng)補(bǔ)償算法的有效性，軌跡后半段進(jìn)行了坡道轉(zhuǎn)彎行駛工況對(duì)比測(cè)試，結(jié)果如圖13所示。

圖13 轉(zhuǎn)彎行駛工況對(duì)比測(cè)試

由圖13(a)可知，車(chē)輛在當(dāng)前測(cè)試場(chǎng)景完成了橫縱向耦合程度較高的掉頭工況，整個(gè)過(guò)程坡度變化量較大，能夠較好地證明本文算法的效果。由圖13(b)的對(duì)比結(jié)果可知，當(dāng)車(chē)輛橫擺角度較大時(shí)，如圖中7～13 s所示，未加入校正算法的估計(jì)結(jié)果將出現(xiàn)較大的超調(diào)，最大誤差約為0.03，失去準(zhǔn)確性。校正算法的加入能夠保證車(chē)輛轉(zhuǎn)彎工況下的坡度估計(jì)精度，減小超調(diào)現(xiàn)象，最大誤差約為0.01，減小了66.7%，證明了本文提出算法的有效性。

6 結(jié)束語(yǔ)

為提高利用車(chē)身縱軸加速度信號(hào)所設(shè)計(jì)的縱向坡度估計(jì)算法的魯棒性，考慮車(chē)輛轉(zhuǎn)向行駛工況，對(duì)車(chē)輛側(cè)向運(yùn)動(dòng)學(xué)進(jìn)行數(shù)學(xué)分析，揭示了車(chē)輛實(shí)際縱向加速度與側(cè)向運(yùn)動(dòng)特征的耦合關(guān)系。設(shè)計(jì)了基于車(chē)輛側(cè)向動(dòng)力學(xué)的側(cè)向速度卡爾曼濾波算法，為車(chē)身縱軸加速度信號(hào)提供實(shí)時(shí)的數(shù)值補(bǔ)償，并最終設(shè)計(jì)了基于車(chē)輛縱向運(yùn)動(dòng)學(xué)的縱向坡度卡爾曼濾波估計(jì)算法。

進(jìn)行了2 種典型道路下的多次仿真對(duì)比試驗(yàn)，并對(duì)估計(jì)誤差進(jìn)行了統(tǒng)計(jì)分析，相對(duì)誤差和絕對(duì)誤差均有明顯降低，證明了所提出方法在車(chē)輛轉(zhuǎn)彎時(shí)的有效性。在大坡度道路上分別進(jìn)行了直線與掉頭工況的實(shí)車(chē)測(cè)試：在直線行駛工況下，平均估計(jì)誤差保持在0.01 以?xún)?nèi)；在轉(zhuǎn)彎工況下，估計(jì)結(jié)果的超調(diào)量相較于未進(jìn)行轉(zhuǎn)向補(bǔ)償?shù)膫鹘y(tǒng)估計(jì)算法，超調(diào)量降低了66.7%，最大誤差約為0.01。測(cè)試結(jié)果證明了基于轉(zhuǎn)向運(yùn)動(dòng)補(bǔ)償?shù)目v向坡度估計(jì)算法能夠有效克服傳感器噪聲，并在車(chē)輛轉(zhuǎn)彎行駛的過(guò)程中獲得更好的估計(jì)結(jié)果。

下一步研究將進(jìn)一步考慮車(chē)輛急加速、急減速和換擋時(shí)縱向加速度劇烈變化工況下的車(chē)輛縱向坡度估計(jì)問(wèn)題，進(jìn)一步提高算法的特殊工況適應(yīng)性。