郭曉宇 王芃 陳寧 徐曉美 姚嘉凌

（南京林業(yè)大學，南京 210018）

主題詞：緊急轉(zhuǎn)向避障 Mittag-Leffler函數(shù) Sigmoid函數(shù) 改進Stanley算法

1 前言

車輛遇到緊急工況時，可采用緊急制動或緊急轉(zhuǎn)向的方式避開障礙物[1]。當前自動緊急制動（Autonomous Emergency Braking，AEB）技術日趨成熟，而自動緊急轉(zhuǎn)向（Autonomous Emergency Steering，AES）作為下一代主動安全技術正在迅速發(fā)展中。AES 技術根據(jù)無人車感知模塊提供的道路信息生成避障路路徑，避障路徑規(guī)劃方法分為數(shù)學公式描述法、人工勢場法和隨機樹搜索法等。徐媛[2]采用Sigmoid 函數(shù)初步規(guī)劃避障路徑，使用模型預測控制對路徑進行再規(guī)劃。趙志國等[3]提出了基于Sigmoid 函數(shù)曲線以及物理約束的避障路徑規(guī)劃方法，建立考慮最優(yōu)曲率預瞄與閉環(huán)反饋的駕駛員模型對規(guī)劃出的路徑進行跟蹤。白成盼等[4]使用Sigmoid 函數(shù)作為智能車的換道軌跡，基于微分平坦理論構(gòu)造非線性性能函數(shù)進行優(yōu)化求解，完成速度規(guī)劃，但避障的情況比換道更為復雜，所以Sigmoid 函數(shù)作為避障曲線具有局限性。閆堯等[5]采用五次多項式作為換道軌跡，約束車輛橫向速度、橫向加速度和橫擺角速度，采用序列二次規(guī)劃算法求解參數(shù)。邊明遠等[6]建立了安全車距模型，采用正弦加速度模型規(guī)劃避障曲線，能夠保證車輛行駛的橫向穩(wěn)定性，縮短了緊急避障的極限距離。人工勢場法[7]廣泛應用于路徑規(guī)劃，其假設物體在運動中受到力場作用，原理簡單、實時性強，但存在著局部最小值問題，且參數(shù)需要長時間標定。杜明博等[8]提出了連續(xù)曲率（Continuous-Curvature RRT，CC-RRT）算法，可在復雜工況下規(guī)劃出避障路徑，但限于低速工況下。在路徑跟蹤算法方面，斯坦利（Stanley）跟蹤算法[9]不同于純跟蹤算法精度依賴于預瞄距離，該算法主要考慮車輛動力學和瞬時車速的影響。靳欣宇[10]等在Stanley 預瞄轉(zhuǎn)向幾何模型的基礎上，設計出優(yōu)化函數(shù)選擇最優(yōu)預瞄時間，但優(yōu)化函數(shù)的系數(shù)選擇過程復雜。Amer 等[11-12]提出一種取消預瞄距離的改進Stanley 跟蹤器，增加橫擺角速度誤差作為反饋量，但進行高速路徑跟蹤時存在抖動。

本文考慮車輛避障時的約束條件和安全性，提出基于廣義Sigmoid 曲線作為避障路徑，并考慮到Stanley 算法對前方曲率與車速的變化不敏感，在高速跟蹤時存在抖動的問題，提出一種增加橫擺角速度阻尼項的Stanley跟蹤器。最后，通過仿真驗證所提出策略的有效性。

2 基于廣義Sigmoid函數(shù)的緊急避障路徑設計

2.1 米塔-列夫勒函數(shù)

米塔-列夫勒（Mittag-Leffler）函數(shù)[13]是指數(shù)函數(shù)的直接推廣。本文引入2個參數(shù)的Mittag-Leffler函數(shù)：

其中，Γ函數(shù)由以下無窮積分定義：

雙參數(shù)Mittag-Leffler函數(shù)的特例為：

式中，z為變量；α、β為可變參數(shù)。

2.2 廣義Sigmoid避障函數(shù)的提出

Sigmoid函數(shù)用數(shù)學公式可描述為：

式中，ε、c、k為形狀參數(shù)。

由圖1 可知：曲線的最大斜率為kε∕4，k、ε越大，轉(zhuǎn)向越劇烈；從初始位置轉(zhuǎn)向到最大斜率的距離c越小，單位時間內(nèi)轉(zhuǎn)向幅度越大；避障路徑的橫向目標位移k根據(jù)具體避障情況決定。

圖1 Sigmoid曲線參數(shù)含義

引入Mittag-Leffler廣義Sigmoid避障函數(shù)：

由于車輛存在運動學約束，要求參考路徑連續(xù)、橫擺角連續(xù)、曲率連續(xù)有界，生成的避障函數(shù)必須連續(xù)且一階和二階導數(shù)連續(xù)[14]。Mittag-Leffler 函數(shù)和指數(shù)函數(shù)一樣具有無限可導的性質(zhì)，廣義Sigmoid 避障函數(shù)滿足以上作為路徑的要求。由式（5）可知，廣義Sigmoid表達式較Sigmoid函數(shù)增加2個可調(diào)參數(shù)。

2.3 新增參數(shù)對曲線特性的影響

其余參數(shù)不變，α、β變化時曲線的圖像變化情況如圖2所示。相較于Sigmoid函數(shù)，廣義Sigmoid函數(shù)的α、β在給定范圍內(nèi)變化時，初始、終止位置變化小，后半段形狀改變程度較前半段大。廣義Sigmoid的一階導數(shù)和二階導數(shù)分別如圖3、圖4所示。β增大會使避障曲線的最大航向角和最大曲率增大，同時曲線的末位置航向角變?yōu)樨撝?。α增大，避障曲線的最大航向角和最大曲率均減小，且末位置航向角變?yōu)檎怠＆?、β改變對車輛進行緊急避障有實際意義，相比Sigmoid 曲線多用于換道工況，廣義Sigmoid曲線根據(jù)末位姿限制生成避障曲線。

圖3 廣義Sigmoid函數(shù)航向角變化情況

圖4 廣義Sigmoid函數(shù)曲率變化情況

3 實時最優(yōu)避障路徑生成和路徑跟蹤

3.1 三自由度車輛模型

如圖5 所示，引入側(cè)傾建立包括側(cè)向、側(cè)傾和橫擺角的三自由度車輛模型，同時考慮彈簧影響[15]，建立三自由度車輛動力學方程：

圖5 三自由度車輛模型

式中，mv為車輛總質(zhì)量；v為車輛速度；θ為車輛質(zhì)心側(cè)偏角；r為橫擺角速度；ms為簧上質(zhì)量；hs為質(zhì)心高度；p為簧上質(zhì)量側(cè)傾角速度；Jzz為汽車橫擺慣性矩；Jxx為簧上質(zhì)量繞側(cè)傾軸轉(zhuǎn)動的側(cè)傾慣性矩；Jxz為簧上質(zhì)量對側(cè)傾和橫擺2個方向組成的平面的慣性矩；Y、M、L分別為車體重心處的側(cè)偏力、橫擺力矩和側(cè)傾力矩。

設Yf、Yr分別為前、后車輪的側(cè)偏力，考慮到輪胎側(cè)偏特性和幾何特性，有：式中，Cf、Cr分別為前、后輪輪胎側(cè)偏剛度；df、dr分別為汽車重心至前、后軸的距離；δf、δr分別為前、后輪轉(zhuǎn)角；Rf、Rr分別為車體側(cè)傾運動影響系數(shù)；?為簧上質(zhì)量側(cè)傾角。

可得車輛的側(cè)偏力矩和橫擺力矩為：

對于側(cè)傾運動，可認為側(cè)傾角較小，因此，sin?≈?，cos?≈1，于是有：

式中，C?、K?分別為總側(cè)傾阻尼和總側(cè)傾剛度；g為重力加速度。

整車的運動方程為：

式中，φ為車輛橫擺角。

3.2 最優(yōu)路徑代價函數(shù)

3.2.1 車輛轉(zhuǎn)向安全性

車輛的側(cè)向加速度ay和軌跡的曲率κ近似滿足，其中vx為車輛橫向速度，在緊急轉(zhuǎn)向避障工況下車輛的縱向加速度為0，車輛的加速度約束為：

式中，ax為橫向加速度；μ為路面附著系數(shù)。

3.2.2 路徑安全性加權項

將障礙物簡化為圓形，考慮路徑在滿足車輛安全距離約束的情況下，與障礙物的碰撞區(qū)域沒有重合，需滿足：

式中，(xob,yob)為障礙物位置；R為障礙物半徑；d為安全距離。

碰撞檢測不能完全反映路徑的安全性，路徑跟蹤的精度不高或檢測障礙物的位置誤差過大時，路徑與障礙物接近將導致碰撞發(fā)生。

如圖6 所示，只考慮避開障礙物問題，候選曲線①的安全性高于候選曲線②。評估路徑的特點在于靠近障礙物的一段路徑對整個路徑的安全性影響最大。

圖6 碰撞檢查

考慮車輛避障時的安全性與實時性，使用高斯卷積和碰撞風險的方法評估路徑的安全性：

其中：

式中，JGauss(i)為曲線簇中的碰撞安全代價加權項；Li為第i條候選路徑的碰撞檢測結(jié)果；G(j)為高斯卷積函數(shù)；N為候選路徑i離散點數(shù)量；Δρ為比例因子；σ為標準差。

3.2.3 目標位姿約束加權項

車輛的位姿為(xt,yt,θt)，應盡量趨近于設定路徑的末位置(x(tend),y(tend))和末位置航向角，且滿足末位置約束Jp和末位置航向角偏離約束Jt：

3.2.4 歸一化處理

考慮到各加權項具有不同的數(shù)量級，導致選取比例因子困難，所以將各加權項的權值進行均值歸一化處理：

式中，J為加權項；Jmax、Jmin分別為加權項的最大、最小值。

構(gòu)建評價曲線的代價函數(shù)：

式中，w1、w2、w3為比例因子；分別為歸一化后JGauss、Jp、Jt的值。

3.3 改進Stanley路徑跟蹤器

Stanley算法中控制參數(shù)間的幾何關系如圖7所示。

圖7 Stanley算法原理

設δyaw(t)為車輛航向角與參考軌跡航向角的誤差，e(t)為橫向誤差。預瞄距離Ld與車輛縱向速度vx成比例：

式中，kp為比例系數(shù)。

車輛前輪轉(zhuǎn)角關系可以表示為：

為了提高算法在高速轉(zhuǎn)向時的跟蹤精度和車輛的穩(wěn)定性，對Stanley算法進行改進。取消預瞄距離，使用橫擺角速度誤差作為反饋量提供未來軌跡預測。設Ksofe為平緩系數(shù)，當車速較低時，減小橫向偏差對前輪轉(zhuǎn)角變化的影響，當車速較高時幾乎不影響控制。此時整個軌跡跟蹤器成為比例反饋控制系統(tǒng)，同時，為了減小系統(tǒng)的超調(diào)量和延遲時間，增加橫擺角速度阻尼作為微分反饋調(diào)節(jié)項kd(rdiscrete(i)-rdiscrete(i+1))。其中rdiscrete(i)為離散時間內(nèi)橫擺角速度，i為前一控制周期的索引值。整個Stanley算法改寫為：

式中，kyaw、k1、kr、kd為比例系數(shù)；rref為參考橫擺角速度。

3.4 仿真評價與分析

3.4.1 搭建仿真環(huán)境

為驗證緊急避障算法的可行性，開展仿真驗證，采用改進Stanley跟蹤器對車輛進行跟蹤，車輛以20 m∕s的速度沿規(guī)劃好的路徑行駛，探測到障礙物時，車輛執(zhí)行緊急避障。三自由度車輛參數(shù)如表1所示。

表1 三自由度車輛部分參數(shù)

3.4.2 仿真結(jié)果分析

3.4.2.1 路徑安全性加權項

如圖8 所示，道路邊界和障礙物均進行碰撞檢測，如圖9所示，在生成的避障曲線簇中曲線6的碰撞安全值最小。路徑安全性加權項對靠近避障曲線的障礙物和道路邊界更加敏感。

圖8 碰撞檢測示意

圖9 候選路徑碰撞風險

若不存在道路的約束，則曲線1 為最安全的避障曲線。由圖2 可知，此時α最大，增大α能夠提高避障曲線的安全性。曲線11 為β最大的曲線，增大β會降低安全性。

3.4.2.2 代價函數(shù)分析

圖10所示為根據(jù)代價函數(shù)生成的車輛坐標系下目標點為(60 m,3.5 m)，目標航向角分別為1°、0°、-1°的最優(yōu)化避障曲線。生成的目標路徑對應目標航向角為1.2°、0.09°、-0.55°，末位置點與目標點的誤差分別為0.110 0 m、0.000 3m、0.010 0 m，表明設計的代價函數(shù)能夠滿足避障需求，相比由于一般Sigmoid曲線，可設置避障末位姿，用于更多避障工況。

圖10 代價函數(shù)生成的路徑示意

3.4.2.3 改進Stanley跟蹤器

圖11 所示為不加阻尼項與增加阻尼項的2 種Stanley 跟蹤器對傳統(tǒng)Sigmoid 函數(shù)路徑的跟蹤效果，2種Stanley 跟蹤器均能對一般Sigmoid 函數(shù)路徑進行精確跟蹤且跟蹤效果類似，從圖12可知，增加阻尼項的跟蹤器輸出的前輪轉(zhuǎn)角比不加阻尼項的更加平穩(wěn)，在車輛高速轉(zhuǎn)向時能提高轉(zhuǎn)向平穩(wěn)性。由此可知，改進的Stanley跟蹤器更適合高速轉(zhuǎn)向時的路徑跟蹤。

圖11 2種Stanley跟蹤器跟蹤傳統(tǒng)Sigmoid路徑

圖12 2種Stanley跟蹤器的前輪轉(zhuǎn)角對比

3.4.2.4 廣義Sigmoid函數(shù)路徑跟蹤

對改進Stanley跟蹤器的各參數(shù)進行調(diào)整，圖13、圖14所示分別為參考路徑與跟蹤路徑的對比結(jié)果和跟蹤誤差，參數(shù)調(diào)整后改進Stanley跟蹤器能夠精準地跟蹤參考路徑，最大側(cè)向誤差小于0.035 m，隨著廣義Sigmoid函數(shù)的α發(fā)生改變，路徑后半段的轉(zhuǎn)彎角度持續(xù)增大，跟蹤的最大側(cè)向誤差持續(xù)減小，說明改進Stanley跟蹤器能夠感知路徑前方的曲率變化，能夠更好地對轉(zhuǎn)彎路徑進行跟蹤。

圖13 路徑跟蹤效果

圖14 路徑跟蹤誤差

圖15 所示為路徑跟蹤過程中前輪轉(zhuǎn)角的變化情況，車輛跟蹤時，前輪轉(zhuǎn)角變化的絕對值不超過1.8°，且變化平穩(wěn)，說明在整個跟蹤過程中車輛能夠保持穩(wěn)定。

圖15 前輪轉(zhuǎn)角變化

4 結(jié)束語

本文提出了一種基于廣義Sigmoid函數(shù)的緊急避障路徑，可用于緊急避障且可調(diào)整車輛末位姿，相比于一般Sigmoid函數(shù)路徑，應用的避障工況范圍更廣，擴展了Sigmoid 函數(shù)在緊急避障中的應用場合。同時，設計的考慮車輛末位姿的代價函數(shù)能夠篩選出最優(yōu)避障曲線且耗時很短，符合緊急避障時快速生成路徑的要求。仿真結(jié)果表明：路徑安全加權項對障礙物和道路邊界敏感，能夠作為判斷路徑安全性的指標；改進Stanley跟蹤器能夠?qū)崿F(xiàn)在高速緊急避障時精確跟蹤路徑，且減小了高速跟蹤時前輪轉(zhuǎn)角的抖動，使轉(zhuǎn)向更加平穩(wěn)；在前方道路曲率發(fā)生變化時，依然能夠保持跟蹤的精度且車輛能夠平穩(wěn)轉(zhuǎn)向。