張書愷，陳 慧，劉美岑

（同濟(jì)大學(xué)汽車學(xué)院，上海 201804）

前言

自動(dòng)泊車技術(shù)作為汽車智能化的一項(xiàng)代表性技術(shù)，已經(jīng)受到了高校與企業(yè)的高度關(guān)注。具有高精度性能的自動(dòng)泊車指令控制算法，不僅可以使得車輛泊車無(wú)需過(guò)大的安全裕度，有助于改善庫(kù)位規(guī)劃增大土地利用效率，也可與未來(lái)需要車輛精準(zhǔn)泊車的自動(dòng)充電技術(shù)等進(jìn)行有機(jī)結(jié)合。

現(xiàn)行的自動(dòng)泊車指令控制算法大致可分為兩種：（1）端到端直接根據(jù)環(huán)境信息輸出控制指令；（2）先規(guī)劃路徑再輸出控制指令跟蹤路徑。前者多為啟發(fā)式算法如神經(jīng)網(wǎng)絡(luò)等，十分依賴傳感器與車輛相關(guān)零件如轉(zhuǎn)向器等的性能，若傳感器信息存在噪聲或車輛零件慣性影響過(guò)大，控制效果將受到影響；后者可依靠啟發(fā)式算法如模糊邏輯，或閉環(huán)控制器如PID、LQR、滑?？刂破?、MPC等，其效果的好壞取決于反饋控制狀態(tài)變量的選取，如相對(duì)路徑的橫向偏距+角度誤差、相對(duì)參考點(diǎn)的向誤差+向誤差+角度誤差等，控制理論相對(duì)成熟，效果相對(duì)可靠，因此考慮使用該方式進(jìn)行控制。

在先規(guī)劃后跟蹤的整體框架下，通常以誤差為狀態(tài)變量列出狀態(tài)方程：

式中：為輸入變量，如前輪轉(zhuǎn)角、速度等；與為與車輛軸距等參數(shù)相關(guān)的矩陣。通過(guò)設(shè)計(jì)輸入變量使得誤差向0衰減，實(shí)現(xiàn)精確跟隨。而所需狀態(tài)變量的獲取方式與路徑規(guī)劃的輸出結(jié)果直接相關(guān)：如路徑規(guī)劃結(jié)果為可以直接用數(shù)學(xué)函數(shù)表示的顯式路徑，則可用曲線相交方式求取參考點(diǎn)，而后求取相對(duì)路徑的橫向偏距與角度誤差作為狀態(tài)變量；如路徑規(guī)劃結(jié)果為無(wú)法直接用數(shù)學(xué)函數(shù)表示的、僅能用形狀與長(zhǎng)度等信息定義的隱式路徑，則利用幾何形狀推算參考點(diǎn)，而后利用相對(duì)參考點(diǎn)的向誤差+向誤差+角度誤差作為狀態(tài)變量會(huì)較為方便；如路徑規(guī)劃結(jié)果為散點(diǎn)序列，則需要借助曲線擬合、插值或按照一定邏輯直接使用散點(diǎn)信息提取狀態(tài)變量。可見(jiàn)，規(guī)劃結(jié)果的輸出形式?jīng)Q定了狀態(tài)變量的選擇，進(jìn)而導(dǎo)致?tīng)顟B(tài)方程的不同。

同時(shí)，狀態(tài)變量反映的誤差信息不同，同樣導(dǎo)致控制器的不同。文獻(xiàn)［7］中使用的橫向偏距+角度誤差能夠完全反映與路徑的橫向偏差程度，因此僅需設(shè)計(jì)橫向控制器進(jìn)行跟蹤，輸入變量?jī)H為轉(zhuǎn)向盤轉(zhuǎn)角；而文獻(xiàn)［8］中使用的相對(duì)參考點(diǎn)的向誤差+向誤差+角度誤差同時(shí)包含了相對(duì)路徑的橫向與縱向誤差，在未進(jìn)行橫縱向誤差解耦的情況下，只能根據(jù)唯一的同時(shí)包含橫縱向誤差的狀態(tài)方程設(shè)計(jì)反饋控制，輸入變量轉(zhuǎn)向盤轉(zhuǎn)角與速度兩項(xiàng)?？梢?jiàn)，控制器的設(shè)計(jì)需確保與狀態(tài)變量相對(duì)應(yīng)。而若如文獻(xiàn)［8］中，狀態(tài)變量同時(shí)包含了相對(duì)路徑的橫向與縱向誤差，理想的控制器應(yīng)能將兩部分誤差解耦，并分別交給對(duì)應(yīng)的執(zhí)行機(jī)構(gòu)進(jìn)行控制，如此在確保跟隨性能的情況下，便于橫向與縱向控制的獨(dú)立設(shè)計(jì)，降低設(shè)計(jì)難度。

本文基于以上考慮，在路徑為隱式路徑（直線段與曲率連續(xù)曲線，即CC 曲線），采取跟隨參考點(diǎn)進(jìn)而跟隨路徑的方案的情況下，基于微分平坦理論，通過(guò)將車輛后軸中點(diǎn)與參考點(diǎn)之間的誤差進(jìn)行橫縱向解耦并進(jìn)行橫縱向分別控制：縱向控制上，提出有效距離積分方法，并設(shè)定縱向誤差修正方法；橫向控制上，采用線性二次調(diào)節(jié)器（LQR），并將路徑曲率變化率與可以加速橫向誤差衰減的因子建立聯(lián)系。如此使得車輛全過(guò)程能夠以更高精度跟隨沿路徑移動(dòng)的參考點(diǎn)，進(jìn)而完成對(duì)整條路徑更高精度的跟隨。

1 問(wèn)題描述與分析

1.1 泊車路徑說(shuō)明

如圖1和圖2所示，平行泊車與垂直泊車的規(guī)劃路徑均可視為一系列至之間的直線段與CC 曲線的拼接。而規(guī)劃結(jié)果的表達(dá)形式為分段的隱式形式，如式（2）所示。

圖2 垂直泊車基本規(guī)劃路徑

式中：、、為沿弧長(zhǎng)的分段邊界，可理解為圖1 中、等之間的弧長(zhǎng)距離；、、為本段內(nèi)的曲率值；、、為本段路徑的車輛行駛方向（前進(jìn)為1，倒車為-1）；、、為本段路徑的轉(zhuǎn)向盤轉(zhuǎn)向方向（左轉(zhuǎn)為1，右轉(zhuǎn)為-1）；為沿路徑的長(zhǎng)度。

圖1 平行泊車基本規(guī)劃路徑

1.2 參考點(diǎn)的獲取

由于路徑為隱式路徑，且難以直接離散為散點(diǎn)序列，因此參考點(diǎn)坐標(biāo)及航向角的計(jì)算是根據(jù)幾何形狀遞推獲得。將總參考距離，也即沿路徑的長(zhǎng)度，與分段邊界值、、等相比較，從而獲得此時(shí)參考點(diǎn)的曲率、行駛方向和轉(zhuǎn)向方向。若此時(shí)參考點(diǎn)坐標(biāo)及航向角分別為、、，沿路徑的長(zhǎng)度增加?，則可遞推下一時(shí)刻參考點(diǎn)坐標(biāo)及航向角、、為

在泊車低速運(yùn)動(dòng)下，上述遞推準(zhǔn)確度能夠滿足要求。

1.3 系統(tǒng)控制框圖與控制目標(biāo)

車輛控制的整體框圖如圖3 所示。整個(gè)控制器整體分為縱向控制（上半部分）與橫向控制（下半部分）：

圖3 系統(tǒng)控制整體框圖

（1）縱向控制中，車輛速度規(guī)劃器內(nèi)部存儲(chǔ)目標(biāo)車速與沿路徑長(zhǎng)度、路徑總長(zhǎng)度相關(guān)的函數(shù)=(，)，通過(guò)參考行駛距離計(jì)算器實(shí)時(shí)獲取總參考行駛距離，結(jié)合路徑總長(zhǎng)度計(jì)算目標(biāo)車速；目標(biāo)車速與實(shí)際車速之間的差值被傳輸給車輛速度控制器，由車輛速度控制器控制驅(qū)動(dòng)電機(jī)給出驅(qū)動(dòng)力矩，最終實(shí)現(xiàn)輪速控制。

（2）橫向控制中，將規(guī)劃路徑總長(zhǎng)度與總參考行駛距離相比對(duì)，獲取沿路徑長(zhǎng)度處應(yīng)有的曲率等信息，通過(guò)遞推計(jì)算參考點(diǎn)位置信息；參考點(diǎn)位置信息與車輛后軸中點(diǎn)的差值被傳輸給車輛轉(zhuǎn)角控制器，由車輛轉(zhuǎn)角控制器控制轉(zhuǎn)向電機(jī)給出轉(zhuǎn)向力矩，最終實(shí)現(xiàn)轉(zhuǎn)向盤轉(zhuǎn)角控制。

如此，整個(gè)系統(tǒng)通過(guò)使用總參考行駛距離與規(guī)劃路徑長(zhǎng)度形成反饋信息作用于橫向與縱向控制，實(shí)現(xiàn)了控制上的橫縱向解耦，如此便于車速、轉(zhuǎn)角的單獨(dú)設(shè)計(jì)。

然而，文獻(xiàn)［11］和文獻(xiàn)［12］中直接以車輛全部行駛距離作為總參考行駛距離，如此則未能進(jìn)行誤差上的解耦，導(dǎo)致信息反饋有誤。圖4 為實(shí)際車輛進(jìn)行路徑跟隨時(shí)的一種常見(jiàn)場(chǎng)景。

圖4 車輛以全部行駛距離作為總參考行駛距離sd時(shí)參考點(diǎn)Or與車輛后軸中點(diǎn)O之間的位置關(guān)系示意

（1）縱向控制中，函數(shù)=(，)應(yīng)確保車輛在全程行駛完畢后停車，因此設(shè)計(jì)時(shí)，當(dāng)總參考行駛距離達(dá)到規(guī)劃路徑的總長(zhǎng)度時(shí)，被設(shè)計(jì)為0。此時(shí)，若計(jì)算不當(dāng)，則存在如圖5 左圖所示的沿路徑的縱向誤差時(shí)，參考點(diǎn)到達(dá)終點(diǎn)導(dǎo)致車速指令值為0，使得車輛將提前停車，車輛后軸中點(diǎn)無(wú)法到達(dá)終點(diǎn)。

圖5 車輛縱向誤差（左）與橫向誤差（右）

（2）橫向控制中，車輛后軸中點(diǎn)與參考點(diǎn)之間的誤差將被反饋給橫向控制器進(jìn)行橫向控制。若車輛后軸中點(diǎn)與參考點(diǎn)之間的誤差同時(shí)包含圖5 所示的縱向誤差與橫向誤差兩部分，則橫向控制將一同接受不能被其控制的縱向誤差信息，而縱向誤差反而將對(duì)橫向控制造成影響，這一點(diǎn)將在后續(xù)第3章說(shuō)明。

基于上述對(duì)橫縱向誤差的解耦分析，本文在文獻(xiàn)［11］和文獻(xiàn)［12］的基礎(chǔ)上，對(duì)橫縱向控制器進(jìn)行設(shè)計(jì)，從而實(shí)現(xiàn)對(duì)上述橫縱向誤差的分別控制：縱向上，改進(jìn)圖3（1）處的參考行駛距離計(jì)算器，增加修正策略計(jì)算，使圖5左圖所示的車輛后軸中點(diǎn)與參考點(diǎn)之間的縱向誤差保持在較小值；橫向上，改進(jìn)圖3（2）處的車輛轉(zhuǎn)角控制器，基于微分平坦理論改進(jìn)反饋控制器進(jìn)行轉(zhuǎn)角控制，使圖5 右圖所示的車輛后軸中點(diǎn)與參考點(diǎn)之間的橫向誤差在跟隨全過(guò)程均保持在較小值。

2 縱向控制設(shè)計(jì)

如前所述，總參考行駛距離的計(jì)算將同時(shí)決定車速指令與參考點(diǎn)的位置，進(jìn)而決定控制效果的好壞，因此需要首先進(jìn)行改進(jìn)?；谙惹胺治鼋Y(jié)果，需要重新設(shè)計(jì)參考行駛距離計(jì)算器，使得車輛盡可能避免出現(xiàn)圖4 所示的表示的縱向誤差；若確實(shí)出現(xiàn)，則能夠通過(guò)對(duì)的計(jì)算進(jìn)行調(diào)整，達(dá)到消除縱向誤差的效果。

2.1 有效距離積分方法

車輛盡管始終在前進(jìn)，但車輛走過(guò)的全部距離并非都是沿路徑行駛的。仍以圖4 為例，車輛雖然由點(diǎn)出發(fā)并運(yùn)行至點(diǎn)，但在沿路徑的方向上，車輛實(shí)際上僅行駛到了點(diǎn)。若直接以車輛行駛過(guò)的全部距離，即弧長(zhǎng)的長(zhǎng)度作為總參考行駛距離，則會(huì)導(dǎo)致參考點(diǎn)位于如所在的領(lǐng)先于點(diǎn)的位置，如此則導(dǎo)致了縱向誤差。

為應(yīng)對(duì)上述問(wèn)題，引入有效距離積分方法。有效距離被定義為車輛行駛時(shí)行駛路徑長(zhǎng)度向規(guī)劃路徑投影后的長(zhǎng)度，示意圖如圖6 所示。由于車輛行駛時(shí)，車輛會(huì)在直線部分、定曲率圓弧部分、變曲率曲線部分處行駛，在3 種情況下，對(duì)應(yīng)的有效距離按如下規(guī)則定義：

圖6 車輛在3種路徑下有效距離定義

（1）直線部分：有效距離為實(shí)際行駛路徑在直線上的直接投影長(zhǎng)度；

（2）定曲率圓弧部分：車輛行駛時(shí)，通過(guò)使用車輛后軸中點(diǎn)起點(diǎn)與終點(diǎn)的坐標(biāo)，可計(jì)算出車輛相對(duì)圓弧行駛過(guò)的有效角度，有效距離為這部分角度對(duì)應(yīng)的圓弧弧長(zhǎng)；

（3）變曲率曲線部分：由于這部分有效距離難以精確算出，因此在計(jì)算時(shí)，利用起點(diǎn)與終點(diǎn)對(duì)應(yīng)的曲率按（2）中計(jì)算方式分別計(jì)算前后曲率下對(duì)應(yīng)的長(zhǎng)度，而后取兩者平均值近似為有效距離。

通過(guò)這種方式，每當(dāng)車輛運(yùn)行一個(gè)時(shí)間間隔，則將車輛在這個(gè)時(shí)間間隔內(nèi)的前進(jìn)距離按照上述規(guī)則進(jìn)行修正獲取本段時(shí)間間隔內(nèi)運(yùn)行的有效距離?，并用這樣的?進(jìn)行積分，獲取總長(zhǎng)度。

2.2 縱向積分修正方法

僅僅對(duì)有效距離進(jìn)行積分，仍不足以達(dá)到最優(yōu)性能，這是由于有效距離積分的計(jì)算會(huì)受到偶然誤差的影響導(dǎo)致計(jì)算存在偏差。此外，如圖7 所示，由于車輛進(jìn)行多段泊車時(shí)，在跟隨完第1 段路徑時(shí)，處于換段交界處點(diǎn)時(shí)存在溜車的情況，這導(dǎo)致車輛會(huì)在跟隨下一段時(shí)存在箭頭標(biāo)識(shí)出的初始縱向誤差，而這部分不能被上述積分機(jī)制所彌補(bǔ)。因此，還需要設(shè)計(jì)對(duì)縱向積分的修正方法來(lái)消除這部分誤差。

圖7 車輛在分段交界處溜車示意

存在初始的縱向誤差時(shí)，一個(gè)最直觀的想法是，當(dāng)車輛落后于參考點(diǎn)時(shí)，希望參考點(diǎn)移動(dòng)較少使得車輛追上參考點(diǎn)；反之，希望參考點(diǎn)移動(dòng)較多使得車輛與參考點(diǎn)之間的誤差減少。按此思路，可設(shè)置縱向積分修正方法。在計(jì)算出有效距離?后，還需進(jìn)行以下修正才能被計(jì)入總長(zhǎng)度：

（1）車輛落后于參考點(diǎn)時(shí)：希望參考點(diǎn)不要移動(dòng)過(guò)大，本次計(jì)入的有效距離按的倍數(shù)縮?。?/p>

（2）車輛領(lǐng)先于參考點(diǎn)時(shí)：希望計(jì)入更多距離，使得參考點(diǎn)追上車輛，本次計(jì)入的有效距離按的倍數(shù)放大。

、為可調(diào)節(jié)的縱向補(bǔ)償系數(shù)。在補(bǔ)償時(shí)，車輛的超前與落后需要計(jì)算沿路徑的縱向誤差，并結(jié)合車輛行駛方向來(lái)判斷。同時(shí)，為避免系統(tǒng)過(guò)于敏感，這種修正應(yīng)在縱向誤差超過(guò)某一閾值后才能進(jìn)行，該閾值可設(shè)為一個(gè)碼盤的刻度值。

由于僅有一個(gè)參考點(diǎn)的信息，因此縱向補(bǔ)償時(shí)需要的縱向誤差實(shí)際上無(wú)法計(jì)算出精確值，僅能通過(guò)使用參考點(diǎn)的信息對(duì)縱向誤差進(jìn)行近似計(jì)算。

圖8展示了一種跟隨過(guò)程常見(jiàn)的場(chǎng)景，車輛后軸中點(diǎn)與參考點(diǎn)之間同時(shí)存在橫向與縱向誤差，顯然最理想的橫向參考點(diǎn)為點(diǎn)，而與之間的距離為縱向誤差。然而目前僅有關(guān)于的信息，無(wú)法求出兩者之間的路徑長(zhǎng)度。但是由于的坐標(biāo)、航向角、所在處曲率、轉(zhuǎn)向方向均已知，因此所在的、與其航向角方向相切的圓是唯一確定的。直線與之間對(duì)應(yīng)的圓弧長(zhǎng)，即認(rèn)為是近似的縱向誤差，其正負(fù)結(jié)合車輛的前進(jìn)方向確定。實(shí)際運(yùn)行時(shí)，在數(shù)據(jù)每一次更新時(shí)均計(jì)算一次縱向誤差，若超過(guò)閾值，則對(duì)該時(shí)刻的有效距離?進(jìn)行修正并計(jì)入總長(zhǎng)度。由于縱向誤差無(wú)法獲取精確值，其對(duì)縱向補(bǔ)償實(shí)際上起到的是“警告”作用。

圖8 縱向誤差近似計(jì)算示意

同時(shí)，如圖3 所示，由于橫縱向控制均基于同一個(gè)有效距離，若在實(shí)際跟隨時(shí)存在縱向誤差，則在縱向誤差未被補(bǔ)償完畢之前，橫向控制器仍然受到縱向誤差的影響而無(wú)法良好跟隨路徑，引入額外的橫向誤差。然而在泊車的低速工況下，近似計(jì)算能夠引入的縱向誤差很小，同時(shí)，即使出現(xiàn)溜車，引入的縱向誤差也僅為10 cm 左右。因此，縱向誤差實(shí)時(shí)數(shù)據(jù)更新3～4 次，在很短時(shí)間內(nèi)即可補(bǔ)償完畢，而補(bǔ)償完畢后橫向控制器即可根據(jù)相應(yīng)設(shè)計(jì)，消除在縱向誤差未補(bǔ)償完畢時(shí)引入的橫向誤差。

綜上，設(shè)計(jì)參考行駛距離計(jì)算器如圖9所示。

圖9 參考行駛距離計(jì)算器整體框圖

3 橫向控制設(shè)計(jì)

3.1 車輛運(yùn)動(dòng)學(xué)模型

由于在泊車過(guò)程中，車輛運(yùn)動(dòng)速度很低，因此忽略車輛側(cè)偏角的影響，使用車輛運(yùn)動(dòng)學(xué)模型：

式中：()、()為車輛后軸中點(diǎn)坐標(biāo)；()為車輛航向角；()為后軸中點(diǎn)速度；()為前輪轉(zhuǎn)角，為軸距。

進(jìn)一步，車輛的轉(zhuǎn)向機(jī)構(gòu)也可簡(jiǎn)化為慣性環(huán)節(jié)：

式中：()為執(zhí)行機(jī)構(gòu)的輸入；為慣性環(huán)節(jié)的時(shí)間常數(shù)。

由式（4）和式（5）可見(jiàn)，運(yùn)動(dòng)學(xué)方程中包含了時(shí)變量速度()，后續(xù)控制指令設(shè)計(jì)時(shí)，不利于速度與轉(zhuǎn)角的獨(dú)立設(shè)計(jì)。因此，首先引入弧長(zhǎng)作為新自變量，以實(shí)現(xiàn)橫縱向解耦。同時(shí)，為便于后續(xù)反饋線性化處理，引入時(shí)間尺度函數(shù)()：

式中：即為沿路徑的總行進(jìn)距離；()實(shí)際上可理解為微小時(shí)間d內(nèi)，車輛實(shí)際行駛距離d與沿路徑行駛距離d之比，而其用于反饋線性化的方式將在后文說(shuō)明。顯然，若車輛能夠完全跟隨路徑，意味著車輛的全部行駛距離等同于沿路徑的行駛距離，此時(shí)() = 1。如此，通過(guò)鏈?zhǔn)椒▌t利用式（6）對(duì)式（4）和式（5）進(jìn)行處理，可將()、()、()、()均轉(zhuǎn)化為以弧長(zhǎng)為自變量的關(guān)系式：

同時(shí)

這里需說(shuō)明，式（7）中包含的速度=()。

3.2 反饋線性化

式（7）為車輛關(guān)于弧長(zhǎng)的運(yùn)動(dòng)學(xué)模型，仍然為非線性模型，不便于后續(xù)狀態(tài)方程的處理和反饋控制的設(shè)計(jì)，因此還需進(jìn)行反饋線性化的過(guò)程。

微分平坦理論形成于20世紀(jì)90年代，其可被用來(lái)進(jìn)行將非線性系統(tǒng)進(jìn)行反饋線性化的方法。通過(guò)將系統(tǒng)所有狀態(tài)和輸入均轉(zhuǎn)化為平坦輸出元素及其有限次導(dǎo)數(shù)的函數(shù)，反饋控制率即可對(duì)平坦輸出元素進(jìn)行設(shè)計(jì)，最終達(dá)到要求。應(yīng)用于自動(dòng)泊車的微分平坦算法由Mueller 等于2007 年提出，后來(lái)，程昆朋等在此基礎(chǔ)上增加了縱向控制并成功在實(shí)車平臺(tái)上實(shí)現(xiàn)了功能。

在微分平坦空間中仍然以弧長(zhǎng)為自變量，并將微分平坦輸出設(shè)置為車輛后軸中點(diǎn)橫縱坐標(biāo)()、()。文獻(xiàn)［10］和文獻(xiàn)［11］中均對(duì)系統(tǒng)的微分平坦性進(jìn)行了證明，因此這里僅給出最終結(jié)果。

通過(guò)整理，可將系統(tǒng)涉及到的變量整理為微分平坦輸出元素本身及其有限階導(dǎo)數(shù)形式：

狀態(tài)方程的整理同樣需要理想點(diǎn)處的相應(yīng)值。由于前文所述的參考點(diǎn)遞推關(guān)系，參考點(diǎn)坐標(biāo)、、參考點(diǎn)處理想航向角、參考點(diǎn)所在處曲率可直接獲取。則

式中及其各階導(dǎo)數(shù)等計(jì)算與等相同。

根據(jù)式（7）有

兩次求導(dǎo)即

將式（15）展開(kāi)，令=，=，有

基于以上說(shuō)明，整理車輛前輪轉(zhuǎn)角控制器設(shè)計(jì)框圖如圖10所示。

圖10 車輛轉(zhuǎn)角控制器設(shè)計(jì)框圖

這里對(duì)于先前提及的縱向誤差對(duì)橫向的影響進(jìn)行簡(jiǎn)要說(shuō)明。如圖11所示，車輛在定曲率、半徑為的圓弧上行駛，根據(jù)阿克曼轉(zhuǎn)向模型，若車輛前輪轉(zhuǎn)角與軸距、路徑半徑的關(guān)系保持為

圖11 車輛存在縱向誤差時(shí)在定曲率路徑上行駛

則車輛可完全跟隨半徑為的圓弧路徑。

3.3 反饋控制說(shuō)明

上一部分提到，通過(guò)使用設(shè)計(jì)好的、替換、進(jìn)行計(jì)算。實(shí)際上，、是真實(shí)存在的變量，其與設(shè)計(jì)好的、存在誤差，可表示為

利用LQR 計(jì)算時(shí)，使用矩陣+代替矩陣來(lái)進(jìn)行計(jì)算，這就使得系統(tǒng)的主導(dǎo)極點(diǎn)更加遠(yuǎn)離虛軸，從而使得誤差以更快的速度衰減。這里為單位矩陣。

3.4 誤差一致性證明

圖12 車輛橫向誤差示意

對(duì)于橫向偏距顯然有

對(duì)于角度偏差-，結(jié)合ˉ≈1 與小角度假設(shè)-<5°，有

因此

3.5 反饋控制設(shè)計(jì)

由于車輛的初始誤差為0，而在跟隨過(guò)程中出現(xiàn)了誤差，這說(shuō)明是由于路徑的形狀變化引入了誤差。而又由于路徑的形狀是連續(xù)變化的，因此誤差的引入也是一個(gè)平滑的過(guò)程。因此，若能對(duì)誤差的引入規(guī)律進(jìn)行研究，則可以結(jié)合其引入規(guī)律將誤差在泊車全程均控制在較小值。

若將誤差視為一個(gè)函數(shù)()，則根據(jù)矩形積分公式，按?步長(zhǎng)離散化，配合階躍函數(shù)，可改寫為

式中(0)、(?)為階躍函數(shù)。上式說(shuō)明，誤差可視為一系列階躍函數(shù)值的依次疊加，其在數(shù)值上的正確性可通過(guò)代入= ?、= 2?等進(jìn)行驗(yàn)證。因此，若能獲得(0)、(?)等的規(guī)律，則可在誤差剛剛開(kāi)始疊加時(shí)即設(shè)計(jì)控制率進(jìn)行消除，使得誤差值不會(huì)累積至較大值。

由于車輛在保持前輪轉(zhuǎn)角固定時(shí)，車輛的運(yùn)行軌跡即為固定的圓弧。若規(guī)劃路徑為定曲率圓弧，則車輛在前輪轉(zhuǎn)角穩(wěn)定后，不會(huì)繼續(xù)出現(xiàn)橫向誤差。因此，誤差的累積是在曲率發(fā)生變化時(shí)產(chǎn)生的。

圖13 為車輛在變曲率處的運(yùn)行示意圖。當(dāng)車輛處于點(diǎn)時(shí)，車輛與參考點(diǎn)重合，由于沒(méi)有誤差反饋，車輛保持圓弧路徑行駛至點(diǎn)，然而此時(shí)由于曲率發(fā)生了變化，車輛前進(jìn)?后參考點(diǎn)實(shí)際應(yīng)為。如此，系統(tǒng)產(chǎn)生了橫向誤差，而該誤差的累積最終將表現(xiàn)為車輛偏離了路徑。

圖13 中的之間的距離即為前進(jìn)?后引入的橫向偏距，與之間的航向角偏差即為引入的角度誤差。記圓與半徑為與，曲率為與，則

圖13 車輛在變曲率處運(yùn)行示意

利用余弦定理有長(zhǎng)度為

則

這里為曲率變化率。式（33）表明，經(jīng)過(guò)固定的?長(zhǎng)度，由曲率變化率引入系統(tǒng)的橫向偏距與本身近似成正比例函數(shù)關(guān)系。

同理，由引入的角度誤差為

可見(jiàn)，引入的角度誤差也與曲率變化率近似成正比例函數(shù)關(guān)系。

如文獻(xiàn)［12］中所述，若系統(tǒng)的主導(dǎo)極點(diǎn)實(shí)部為-，則誤差以e的速度衰減。若假設(shè)系統(tǒng)有一初始誤差(0)，在經(jīng)過(guò)?后，誤差變?yōu)?/p>

以橫向偏距為例，若曲率變化率為，則結(jié)合介入的橫向偏距誤差?后，誤差變?yōu)?/p>

若此時(shí)加入與曲率變化率相關(guān)的衰減因子=，則

觀察式（37）第2 部分可見(jiàn)，如果參數(shù)能夠適度調(diào)整，則在誤差積累到某一值之后，由衰減因子增加的衰減速度可以將后續(xù)引入的誤差完全抵消掉，也就是說(shuō)，誤差將維持在某一上限值(0)不會(huì)繼續(xù)增加。同時(shí)，為保持車輛在定曲率路徑行駛時(shí)有足夠的誤差衰減速度，可設(shè)定=+，其中、為可調(diào)參數(shù)，由車輛實(shí)際跟蹤效果確定。

這里說(shuō)明，實(shí)際運(yùn)行時(shí)，誤差上限值(0)不能根據(jù)式（37）由精確確定，這是由于因子的加入不嚴(yán)格使得主導(dǎo)極點(diǎn)更遠(yuǎn)離虛軸的距離；同時(shí)，縱向誤差雖然可保持在較小值，但無(wú)法完全消除至0，殘余的縱向誤差仍會(huì)對(duì)橫向控制造成影響；在推導(dǎo)中的近似也會(huì)造成些微影響。

4 仿真實(shí)驗(yàn)驗(yàn)證

仿真實(shí)驗(yàn)的目的是證明橫縱向的改進(jìn)策略有效。

為實(shí)現(xiàn)上述目的，本文使用3 種控制方法進(jìn)行對(duì)比：

（1）縱向改進(jìn)橫向不改進(jìn)：縱向控制采取有效距離積分與縱向誤差修正；橫向控制僅按文獻(xiàn)［12］加入固定不變的因子，即= 0、≠0；

（2）橫縱向均改進(jìn)：縱向控制采取有效距離積分與縱向誤差修正；橫向控制在文獻(xiàn)［12］的基礎(chǔ)上，增加與曲率變化率的相關(guān)量，即≠0、≠0，且與（1）中相同；

（3）橫向改進(jìn)縱向不改進(jìn)：縱向控制僅使用車輛全部已行走距離作為總參考距離；橫向控制在文獻(xiàn)［11］的基礎(chǔ)上，增加與曲率變化率的相關(guān)量，即≠0、≠0，取值與（2）相同。

圖14 3種控制方法下仿真跟隨結(jié)果

圖15 3種控制方法下x方向誤差

由圖14～圖16可見(jiàn)，橫縱向均改進(jìn)時(shí)，由于增加了項(xiàng)緩解了曲率變化時(shí)誤差的累積，而縱向補(bǔ)償又可對(duì)溜車誤差進(jìn)行補(bǔ)償，因此車輛能將在溜車時(shí)（圖15 和圖16 時(shí)間12 和26 s 處）引發(fā)的異常誤差值快速消除，之后將誤差控制在較小值。這里需同時(shí)說(shuō)明，由于參考點(diǎn)是根據(jù)路徑信息遞推出來(lái)的，因此參考點(diǎn)與路徑本身也存在部分誤差，但在泊車的低速運(yùn)動(dòng)下，這部分誤差是可以接受的。

圖16 3種控制方法下y方向誤差

5 實(shí)車實(shí)驗(yàn)驗(yàn)證

實(shí)車實(shí)驗(yàn)的目的是驗(yàn)證算法在實(shí)際車輛上的可執(zhí)行性，以及是否能在不同工況下均可保持高性能以便實(shí)際應(yīng)用。實(shí)車平臺(tái)為如圖17 所示的基于榮威E50改制的智能駕駛測(cè)試平臺(tái)。車輛搭載了超聲波雷達(dá)、單線激光雷達(dá)、多線激光雷達(dá)、單目攝像頭、環(huán)視攝像頭、GPS/INS 組合導(dǎo)航儀RT3000 和dSPACE 的MicroAutoBox 作為控制器，以便實(shí)現(xiàn)自動(dòng)駕駛決策、規(guī)劃、控制算法。這里需說(shuō)明，本文使用的路徑規(guī)劃算法僅以充分利用空間為目標(biāo)，并未考慮安全距離，因此車輛輪廓會(huì)與庫(kù)位輪廓有輕微碰撞。不過(guò)，由于本文的目的僅是證明路徑跟隨算法的有效性，因此本文的規(guī)劃算法能夠滿足要求，而實(shí)際使用時(shí)若需確保安全性，只需在規(guī)劃算法中設(shè)定安全距離即可。

圖17 實(shí)車實(shí)驗(yàn)平臺(tái)

實(shí)車驗(yàn)證包括垂直泊車與平行泊車。其中，垂直泊車分為垂直1 段泊車、垂直3 段泊車、垂直4 段泊車，平行泊車分為1 段平行泊車、2 段平行泊車、3段平行泊車，共6 種工況的驗(yàn)證。圖18～圖29 為車輛在不同工況下的跟隨示意圖與、方向誤差。

圖18 垂直1段路徑泊車跟隨示意

圖29 平行3段路徑泊車x、y方向誤差

結(jié)果表明，本文提出的算法能夠保證車輛在泊車全過(guò)程的高精度跟隨。6 種工況下，車輛的、方向的誤差均可最終保持在0.05 cm以內(nèi)。

圖19 垂直1段路徑泊車x、y方向誤差

而實(shí)車實(shí)驗(yàn)結(jié)果與仿真存在偏差，其原因在于：

（1）車輛的執(zhí)行機(jī)構(gòu)（驅(qū)動(dòng)電機(jī)與轉(zhuǎn)向電機(jī)）存在慣性，而這部分在仿真中無(wú)法精確建模，導(dǎo)致在同種情況下，實(shí)車與仿真的輸出指令值相比是存在偏差的；

（2）實(shí)車試驗(yàn)時(shí)不能確保車輛所處路面是絕對(duì)平整的，路面的不平度將對(duì)車輛施加一定的干擾力，進(jìn)而影響實(shí)車結(jié)果，而路面的不平度未能在仿真中體現(xiàn)。

圖20 垂直3段路徑泊車跟隨示意

圖21 垂直3段路徑泊車x、y方向誤差

圖22 垂直4段路徑泊車跟隨示意

6 結(jié)論

圖23 垂直4段路徑泊車x、y方向誤差

圖24 平行1段路徑泊車跟隨示意

圖25 平行1段路徑泊車x、y方向誤差

本文中基于微分平坦算法的泊車路徑跟隨方法，將車輛與路徑參考點(diǎn)之間的誤差進(jìn)行解耦，并利用與橫縱向誤差相對(duì)應(yīng)的控制器進(jìn)行控制，使得車輛高精度跟隨沿路徑移動(dòng)的參考點(diǎn)，從而完成高精度路徑跟隨。具體體現(xiàn)為：

（1）縱向控制上，采用了有效距離積分方法與縱向誤差修正規(guī)則，消除了車輛后軸中點(diǎn)與路徑參考點(diǎn)之間的、沿規(guī)劃路徑的縱向誤差，有利于橫向控制部分對(duì)剩余誤差的控制；

圖26 平行2段路徑泊車跟隨示意

圖27 平行2段路徑泊車x、y方向誤差

圖28 平行3段路徑泊車跟隨示意

（2）橫向控制上，使用微分平坦算法將系統(tǒng)反饋線性化，在微分平坦空間中采用線性二次調(diào)節(jié)器（LQR），并將曲率變化率與可加速橫向誤差衰減的因子建立聯(lián)系，設(shè)計(jì)反饋控制，使得橫向誤差在定曲率與變曲率處均維持在較小值。

本文通過(guò)仿真對(duì)比驗(yàn)證了橫縱向控制的有效性，并在實(shí)車實(shí)驗(yàn)平臺(tái)上驗(yàn)證了算法的可靠性。通過(guò)對(duì)實(shí)車實(shí)驗(yàn)的分析，執(zhí)行器的慣性與路面不平度將對(duì)實(shí)際效果產(chǎn)生影響，這些問(wèn)題有待進(jìn)一步改進(jìn)。