?--關(guān)注、星標、回復(fù)“自主泊車”--
↓↓領(lǐng)取：自主代客泊車系統(tǒng)總體技術(shù)要求↓↓原文鏈接：https://reurl.cc/WDW6Ze
大到類似百度apollo無人車，無人配送車；小到掃地機器人，其實都是無人駕駛車輛，都是一個復(fù)雜的軟硬件結(jié)合系統(tǒng)。其軟件算法模塊主要包含：感知、定位、軌跡預(yù)測、行為決策、運動規(guī)劃及反饋控制等，在這里介紹和補充的是反饋控制部分部分軟件實現(xiàn)可參考：個人開源項目path-following-control兩者協(xié)同工作以使無人車按照預(yù)定的參考軌跡行駛，其中，橫向控制根據(jù)上層運動規(guī)劃輸出的路徑、曲率等信息進行跟蹤控制，以減少跟蹤誤差，同時保證車輛行駛的穩(wěn)定性和舒適性根據(jù)橫向控制使用車輛模型的不同，可以將其分為兩種類型，包括：無模型的橫向控制即傳統(tǒng)的PID控制算法，將車輛當(dāng)前的路徑跟蹤偏差作為輸入量，對跟蹤偏差進行比例(Proportion)、積分(Integration)和微分(Differentiation)控制得到轉(zhuǎn)向控制量，但該算法由于沒有考慮車輛本身的特性，因此算法對外界干擾的魯棒性較差，無法滿足車輛在高速行駛過程中的有效控制。- 基于車輛運動學(xué)模型的純跟蹤控制(PurePursuit)算法
- 后輪反饋控制(Rearwheelfeedback)
- 前輪反饋控制(Frontwheelfeedback)算法
- 基于車輛動力學(xué)模型的線性二次型調(diào)節(jié)器(LinearQuadraticRegulator，LQR)控制算法
基于車輛運動學(xué)模型的控制方法主要考慮了車輛的運動學(xué)約束對車輛控制的影響
平時見到的汽車屬于阿克曼轉(zhuǎn)向，通常可以簡化為兩輪的自行車模型
其假設(shè)車輛的前后輪均為剛體，只能沿著車輪滾動的方向前進，無側(cè)向滑動由上述假設(shè)可得自行車模型的運動微分方程如下式：
角速度和角速度的關(guān)系：v=ωr，即ω=v/r，因此這里ψ˙=v*tan(δ)/L，其他的比較簡單，就不加以說明。純跟蹤控制算法（PurePursuit）是一種典型的橫向控制方法，最早由R.Wallace在1985年提出，該方法對外界的魯棒性較好。基于當(dāng)前車輛后輪中心位置，在參考路徑上向llld（自定義）的距離匹配一個預(yù)瞄點，假設(shè)車輛后輪中心點可以按照一定的轉(zhuǎn)彎半徑??行駛抵達該預(yù)瞄點，然后根據(jù)預(yù)瞄距離Id，轉(zhuǎn)彎半徑??，車輛坐標系下預(yù)瞄點的朝向角??之間的幾何關(guān)系來確定前輪轉(zhuǎn)角。
弧的弧度為2??是因為這個三角形是等腰三角形，邊長為??，而其中一個角為π/2-??，所以這個弧度為π-（π/2-??）*2=2??。如圖所示，弧線為經(jīng)過車輛后輪中心以及預(yù)瞄點的圓弧，圓弧的半徑??與朝向角??和預(yù)瞄距離llld有關(guān)，其實也就是與(xg，yg)有關(guān)
根據(jù)圖中的幾何關(guān)系，??,??,與ld的關(guān)系為：
根據(jù)圖中的幾何關(guān)系，??,??,與ld的關(guān)系為：
根據(jù)運動學(xué)方程(2-1)中ψ˙=v?tan(δ)/L，則tan(δ)=ψ˙?L/v，而在這里ψ˙/v=??，即：
再把式(2-4)帶入(2-5)可得前輪轉(zhuǎn)角為：
后輪反饋式（Rearwheelfeedback）是利用后輪中心的路徑跟蹤偏差量來進行轉(zhuǎn)向控制量計算的方法。
根據(jù)運動學(xué)方程(2-1)及車輛后輪與參考路徑的幾何關(guān)系，可推導(dǎo)出參考路徑坐標系(??,??)。Ferent坐標系下(??,??,ψ\psiψ??)的變化率為：
Frenet坐標系使用道路的中心線作為參考線，使用參考線的切線向量和法線向量建立坐標系，那么基于參考線的位置，就可以使用縱向距離和橫向距離來描述任意位置，同時縱向和橫向的速度、加速度、加加速度等信息也更便于計算。對于二次連續(xù)可導(dǎo)的參考線，需要設(shè)計車身橫擺角速度??保證在李亞普洛夫方程下局部漸進收斂：
李亞普洛夫穩(wěn)定（一個分析系統(tǒng)的穩(wěn)定性方法）用數(shù)學(xué)的語言描述為：在給定的參考軌Xref(t)下，存在?ε>0，t10，滿足下列條件：
對于李亞普洛夫穩(wěn)定也分為漸進穩(wěn)定和指數(shù)穩(wěn)定，李亞普洛夫穩(wěn)定性理論是從能量的觀點出發(fā)的，系統(tǒng)的能量隨著時間的推移不斷變化（增加or減少），在有限時間內(nèi)，系統(tǒng)趨于穩(wěn)定，能量不再變化，此時，系統(tǒng)的k+1時刻的能量減去k時刻的能量是<0的，即V(k+1)-V(k)<0，V代表能量函數(shù)。漸進穩(wěn)定指對于時變系統(tǒng)，??在條件(1)下獨立于時間t1，指數(shù)穩(wěn)定指收斂率是以指數(shù)下降，對于后輪反饋式算法，為了保證車輛的李亞普洛夫穩(wěn)定性，車身橫擺角速度??可表示為式(2-10)。
其中，??ψ為橫擺角偏差反饋控制增益，??e為橫向位置偏差反饋控制增益，因此，根據(jù)運動學(xué)方程(2-1)，可得到前輪轉(zhuǎn)角??為:
前輪反饋控制（Frontwheelfeedback）又稱Stanley控制，其核心思想是利用前輪中心的路徑跟蹤偏差量對方向盤轉(zhuǎn)向控制量進行計算。
式(2-12)中可以看出前輪橫向位置偏差的變化率e˙可以直接通過前輪轉(zhuǎn)角??進行控制，當(dāng)定義e=?k??后，可以保證前輪橫向位置偏差??滿足李亞普洛夫的指數(shù)化收斂，即：
李亞普洛夫指數(shù)：在相空間中相互靠近的兩條軌線隨著時間的推移，按指數(shù)分離或聚合的平均變化速率。關(guān)于arcsinarcsinarcsin函數(shù)：
那么對于式(2-13)來說，當(dāng)∣?ke/vf∣>1時，函數(shù)無解。對于有限域內(nèi)的指數(shù)收斂可以放寬到局部指數(shù)收斂，從而將前輪轉(zhuǎn)角??定義為：
Stanley前輪反饋控制算法在變曲率路徑且路徑連續(xù)可導(dǎo)的條件下，可以滿足局部指數(shù)收斂特性，但也可以看出該算法不適用于倒車的場景，導(dǎo)致對于自動泊車等工況不適用。
基于車輛運動學(xué)模型的方法忽略了車輛的動力學(xué)特性，因此在車速過快或者曲率變化率過大的情況下該算法無法滿足車輛的穩(wěn)定性控制要求，這里介紹一種典型的基于車輛動力學(xué)模型的橫向控制方法，包括- 車輛線性二自由度動力學(xué)模型的構(gòu)建
- 車輛路徑跟蹤偏差狀態(tài)方程的構(gòu)建
對于基于車輛動力學(xué)模型的控制方法，首要的工作是對車輛動力學(xué)進行建模，由于車輛動力學(xué)模型具有強非線性時變的特性，因此精確建模整個車輛動力學(xué)十分困難，在橫向控制中，主要關(guān)心的是車輛橫向運動特性。通常將車輛動力學(xué)模型簡化為的二自由度橫向動力學(xué)模型，假設(shè)車身的縱向速度Vx保持不變,其橫向動力學(xué)模型的兩個自由度為：由于精確的二自由度動力學(xué)模型是非線性的，為了便于進行實時的跟蹤控制計算，通常還需要在精確的二自由度動力學(xué)模型基礎(chǔ)上進行一些簡化近似，得到線性二自由度動力學(xué)模型。這里介紹基于轉(zhuǎn)向小角度及輪胎動力學(xué)線性化假設(shè)下的車輛線性二自由度動力學(xué)模型，下圖為車輛為車輛橫向動力學(xué)坐標系示意圖。
假設(shè)道路平整(不考慮道路橫向的傾斜)，且轉(zhuǎn)向角度較小時，則車身??軸方向應(yīng)用牛頓第二定律可得：
其中，??是整車質(zhì)量，??y是車輛質(zhì)心處的橫向加速度，F(xiàn)yf，F(xiàn)yr為地面給前輪胎和后輪施加的橫向力，而橫向加速度??y由兩部分組成：- 車輛沿車身??軸橫向運動產(chǎn)生的加速度y˙
然后，車輛繞??軸的轉(zhuǎn)矩平衡方程為：
其中，lf,lr分別是前輪軸和后輪軸到質(zhì)心的距離。接下來對橫向輪胎力Fyf，F(xiàn)yr進行計算，實驗結(jié)果表明，輪胎側(cè)偏角較小時，地面施加給輪胎的橫向力(側(cè)偏力)與輪胎側(cè)偏角成線性關(guān)系(側(cè)偏角是車輪速度方向與車輪縱軸的夾角)。
θvf是前輪胎速度方向與車身縱軸的夾角，??為前輪轉(zhuǎn)向角。
θvr是后輪胎速度方向與車身縱軸的夾角，因此，前輪(2個)橫向輪胎力為:
C??f是前輪側(cè)偏剛度(corneringstiffness)
將(3-5)，(3-6)，(3-7)，(3-8)，(3-11)，(3-12)代入(3-3)和(3-4)可得線性二自由度模型的狀態(tài)方程:
3.2.車輛路徑跟蹤偏差狀態(tài)方程的構(gòu)建有了式(3-13)的系統(tǒng)狀態(tài)方程之后，就可以分析出在給定的前輪轉(zhuǎn)角輸入下，車輛的橫向位移、橫向速度、橫擺角以及橫擺角速度的響應(yīng)。但是橫向跟蹤控制的目的是為了減小跟蹤偏差，需要的狀態(tài)方程是能夠分析在給定的前輪轉(zhuǎn)角下車輛跟蹤偏差的響應(yīng)，因此些偏差通常是：
將式(3-16)，(3-17)，(3-18)代入(3-3)，(3-4)化簡后可以得到新的狀態(tài)方程:
上式(3-20)就是橫向控制所需的路徑跟蹤偏差狀態(tài)方程基于前面推導(dǎo)的路徑跟蹤偏差系統(tǒng)狀態(tài)方程(3-20)，就可以分析在前輪轉(zhuǎn)角控制輸入??作用下，車輛路徑跟蹤偏差??的響應(yīng)特性，期望的響應(yīng)特性就是跟蹤偏差能夠快速、穩(wěn)定地趨近于零，并保持平衡。同時前輪轉(zhuǎn)角控制輸入又盡可能小，這就是一個典型的多目標優(yōu)化最優(yōu)控制問題，且優(yōu)化的目標函數(shù)可以表示為跟蹤過程累計的跟蹤偏差與累計的控制輸入的加權(quán)和，如式(3-21)所示：
其中，??為半正定的狀態(tài)加權(quán)矩陣,??為正定的控制加權(quán)矩陣，且??,??通常取為對角陣。若只是x和δ做為代價，那么存在x的正誤差被δ的負誤差消除。相反，自身相乘，這樣負值也會產(chǎn)生正平方，稱這些為二次項。??矩陣元素變大意味著希望跟蹤偏差能夠快速趨近于零，??矩陣元素變大意味著希望控制輸入能夠盡可能小設(shè)M是n階方陣，如果對任何非零向量z，都有zTMz>0，其中zT表示z的轉(zhuǎn)置，就稱M為正定矩陣。則式(3-21)目標函數(shù)中，第一項優(yōu)化目標∫0∞(xTQx)dt表示跟蹤過程路徑偏差的累積大小，第二項優(yōu)化目標∫0∞(δT?Rδ)dt表示跟蹤過程控制能量的損耗，這樣就將橫向控制問題轉(zhuǎn)化為一個最優(yōu)控制問題：求解最優(yōu)的前輪轉(zhuǎn)角控制輸入δ*，使得式(3-21)目標函數(shù)??可以取極小值而式(3-21)是一個關(guān)于狀態(tài)變量??和控制輸入??的二次型目標函數(shù)，對它的優(yōu)化求解是一個典型的LQR最優(yōu)控制問題，根據(jù)LQR最優(yōu)控制理論，對式(3-21)目標函數(shù)的優(yōu)化求解，解出的最優(yōu)控制規(guī)律δ\deltaδ*是關(guān)于狀態(tài)變量??的線性函數(shù):
其中，??d和Bd代表狀態(tài)方程(3-20)離散化后的??，B1矩陣,??是式(3-23)黎卡提方程的解:
因此，根據(jù)式(3-22)可以設(shè)計一個狀態(tài)反饋調(diào)節(jié)器，通過狀態(tài)反饋實現(xiàn)閉環(huán)最優(yōu)控制，如圖所示
根據(jù)求解的狀態(tài)反饋調(diào)節(jié)器增益??，則可以得到最優(yōu)的狀態(tài)反饋前輪轉(zhuǎn)角δfb：
由于在LQR狀態(tài)反饋控制求解過程中沒有考慮路徑本身的動態(tài)變化特性，因此只有式(3-25)的反饋控制輸入時LQR控制在曲線行駛時會存在穩(wěn)態(tài)誤差。穩(wěn)態(tài)誤差是系統(tǒng)從一個穩(wěn)態(tài)過渡到新的穩(wěn)態(tài)，或系統(tǒng)受擾動作用又重新平衡后，系統(tǒng)出現(xiàn)的偏差。為了消除該穩(wěn)態(tài)誤差，還需要加上一個前饋控制輸入量δff，前饋控制考慮了路徑的曲率及車輛的轉(zhuǎn)向不足特性
其中，KV為不足轉(zhuǎn)向梯度，表征了車輛的轉(zhuǎn)向特性，e2ss為穩(wěn)態(tài)時的角度跟蹤偏差，k3為??矩陣中的角度偏差反饋控制增益部分
本文主要介紹了基于車輛運動學(xué)和動力學(xué)模型的橫向控制算法，從上文對兩類方法的分析可以看出，兩類方法在應(yīng)用場景上都存在一定的局限性，并具有較強的互補性，因此在實際使用中，綜合使用這些方法會更有效。
無模型的PID橫向控制算法參數(shù)少，簡單易用，但是由于沒有考慮車輛系統(tǒng)動力學(xué)特性及路徑本身的動態(tài)變化特性，對外界干擾的魯棒性較差，在高速或曲率較大的彎道場景時，會出現(xiàn)較大的跟蹤誤差和“畫龍”現(xiàn)象。因此，該方法比較適合應(yīng)用于低速曲率較小的路徑跟蹤控制場景中。基于運動學(xué)模型的橫向控制算法中，Pure Pursuit和Stanley前輪反饋算法在中低速場景下，他們的路徑跟蹤的性能較好，Pure Pursuit在大的跟蹤誤差和非連續(xù)的路徑場景下魯棒性較好，其控制的關(guān)鍵在于對最佳前向預(yù)瞄距離的確定。其中，增大前向預(yù)瞄距離將提高車輛控制的穩(wěn)定性，但隨之會帶來路徑跟蹤性能降低及穩(wěn)態(tài)誤差增大的后果，表現(xiàn)出轉(zhuǎn)彎內(nèi)切現(xiàn)象。相比于Pure Pursuit算法，Stanley前輪反饋算法還額外考慮了橫擺角偏差，因此在大多數(shù)場景下，跟蹤性能更佳。然而，由于沒有設(shè)置前向預(yù)瞄，Stanley算法會出現(xiàn)轉(zhuǎn)向過度的情況，與Pure Pursuit和Stanley算法相比，后輪反饋控制算法計算更加復(fù)雜，對路徑的平滑性要求更高。在中等速度下的跟蹤性能及魯棒性與Stanley方法近似，然而在速度較大時，穩(wěn)態(tài)誤差也會變大，從而導(dǎo)致控制效果不佳，LQR算法使用二自由度動力學(xué)模型來設(shè)計橫向控制器，與前述基于運動學(xué)模型的幾種算法相比，LQR參數(shù)調(diào)節(jié)更加復(fù)雜。其不僅需要獲取車輛自身的模型參數(shù)，還需要調(diào)節(jié)LQR目標函數(shù)的Q，R矩陣以獲得較優(yōu)的跟蹤性能。LQR算法的優(yōu)點在于，通過與轉(zhuǎn)向前饋進行有效結(jié)合，LQR能夠很好的解決曲線行駛時的穩(wěn)態(tài)跟蹤誤差，在中等速度曲線行駛時其穩(wěn)態(tài)誤差趨近于零，從而極大提升跟蹤性能，LQR非常適用于路徑平滑的高速公路及城市駕駛場景，具有較好的車輛高速控制性能。但是，由于模型的固有缺陷，LQR與前饋控制的結(jié)合也無法解決所有跟蹤控制問題，由于該方法采用基于簡化后的二自由度動力學(xué)模型，因此當(dāng)車輛運動不滿足二自由度動力學(xué)模型轉(zhuǎn)向小角度，或輪胎動力學(xué)線性化的假設(shè)條件時，LQR算法的跟蹤性能會大幅降低，從而導(dǎo)致控制失效。轉(zhuǎn)載自氫鍵H-H，文中觀點僅供分享交流，不代表本公眾號立場，如涉及版權(quán)等問題，請您告知，我們將及時處理。
-- END --
