于 洋，周佳偉，馮迎賓，譚 巖

(1.沈陽理工大學(xué) 自動化與電氣工程學(xué)院，沈陽 110159；2.鳳城市邊門中學(xué)，遼寧 丹東 118119)

無人車運動規(guī)劃從任務(wù)需求上通常分為兩個層次：路徑規(guī)劃和軌跡規(guī)劃。路徑規(guī)劃是根據(jù)空間幾何信息來規(guī)劃出一條無人車在空間中的運動軌跡，該軌跡由空間中一系列離散的控制點構(gòu)成。軌跡規(guī)劃是為了解決如何在滿足車輛速度與加速度約束的狀態(tài)下，使車輛以最短的時間通過空間中離散控制點的一種規(guī)劃算法。軌跡規(guī)劃算法的輸入是離散控制點序列，輸出是一條通過這些離散控制點的空間軌跡，因此軌跡規(guī)劃問題可以看做一個插值問題[1]。

針對無人車軌跡不連續(xù)、不平滑的情況，目前多采用構(gòu)建某種幾何曲線來對控制點進(jìn)行插值，從而實現(xiàn)對車輛軌跡的平滑處理。弗吉尼亞大學(xué)的無人車Odin[2]在2007年DARPA城市挑戰(zhàn)賽中采用三次樣條插值的方法對A*算法規(guī)劃出的全局路徑進(jìn)行插值平滑的處理。Yang K等[3]利用貝塞爾曲線完成基于路徑點集合的軌跡平滑。陳杰等[4]基于螺旋樣條完成了對無人機運動軌跡的優(yōu)化。姜媛媛等[5]利用DP-B樣條對移動機器人軌跡進(jìn)行平滑優(yōu)化。Kwangjin Yang等[6]提出了通過分段光滑的方式對軌跡進(jìn)行優(yōu)化處理。但無論采用三次樣條插值，還是采用貝塞爾曲線或螺旋樣條對被控對象軌跡進(jìn)行平滑優(yōu)化處理，這些方法所采用的平滑曲線的控制點個數(shù)會受到曲線次數(shù)限制，因此控制點選取困難，并且控制點越多，計算越復(fù)雜。而通過將軌跡進(jìn)行分段平滑處理，然后將分段平滑后的曲線再次進(jìn)行拼接成一條完整曲線的方式雖然降低了計算的復(fù)雜度，但是這樣的處理方式會使得分段得到的平滑曲線在再次拼接時會在拼接點處形成曲率不連續(xù)等問題。

因此，本文提出了基于三次B樣條曲線對無人車軌跡進(jìn)行平滑優(yōu)化處理，其具有車輛局部控制點的改變不影響全局軌跡、優(yōu)化曲線的階數(shù)與車輛的控制點不相關(guān)、優(yōu)化曲線的曲率在拼接處連續(xù)等特點。同時B樣條能夠反映曲線的拐點，保證曲線函數(shù)為凸函數(shù)提供了參考依據(jù)，避免車輛因為插值曲線函數(shù)為凹函數(shù)導(dǎo)致的車輛與道路邊界發(fā)生碰撞等情況。

1 B樣條曲線

B樣條是樣條曲線的一種特殊表示形式，由Isaac Jacob Schoenberg創(chuàng)造，是形狀數(shù)學(xué)描述的主流方法之一[7]。B樣條曲線具有幾何不變性、凸包性、保凸性[8]、變差減小性、局部支撐性等諸多優(yōu)良性質(zhì)。B樣條曲線方程為

(1)

式中：di為控制點對應(yīng)的第i段的曲線方程；Ni,k為k次規(guī)范B樣條基函數(shù)；i為B樣條曲線的序號?；瘮?shù)是由一個稱為節(jié)點矢量的非遞減參數(shù)u的序列U：u0≤u1≤…≤un+k+1所決定的k次分段多項式。

B樣條的基函數(shù)通常采用Cox-deBoor遞推公式[9]：

(2)

式中k是次數(shù)，共有n+1個控制點。節(jié)點是在節(jié)點矢量U中取得，控制點則是坐標(biāo)點。Cox-deBoor遞推公式是B樣條曲線的定義的核心。

B 樣條曲線按照節(jié)點矢量序列U中的節(jié)點分布情況可以分為(1)均勻B樣條曲線：節(jié)點按照等差數(shù)列均勻分布；(2)準(zhǔn)均勻B樣條曲線：兩端節(jié)點分別為0、1，所有內(nèi)部節(jié)點重復(fù)度為k+1；(3)分段Bezier曲線：兩端節(jié)點重復(fù)度為k+1，內(nèi)部節(jié)點重復(fù)度為k；(4)非均勻B樣條曲線：節(jié)點在節(jié)點矢量序列U中隨機分布，這是一般情況。

常見的B樣條曲線實例如下圖1～圖3所示。

圖1 均勻B樣條曲線

圖2 準(zhǔn)均勻B樣條曲線

圖3 分段B樣條曲線

非均勻B樣條曲線與均勻B樣條曲線線條相似，曲線均不經(jīng)過首尾控制點。為使優(yōu)化后的軌跡覆蓋車輛全局路徑的起點與終點，本文最終采用了準(zhǔn)均勻B樣條曲線。

2 車輛軌跡曲線方程的確定

2.1 車輛曲線基函數(shù)

B樣條曲線公式中的k值代表曲線的平滑度，k值越大，曲線平滑度越高，但計算復(fù)雜度也越高。為兼顧無人車軌跡的平滑度與計算的復(fù)雜度，本文選取三次B樣條曲線對車輛軌跡進(jìn)行平滑優(yōu)化，即k=3。將k=3代入式(2)整理即可獲得車輛三次B樣條曲線基函數(shù)為

(3)

2.2 車輛曲線控制點

控制點即通過測量或計算得到的曲線或曲面上少量描述曲線或曲面幾何形狀的數(shù)據(jù)點，在無人車中控制點為對車輛里程計進(jìn)行采樣的數(shù)據(jù)點。本文對車輛里程計采樣的方法分兩個階段。第一階段為車輛直行或轉(zhuǎn)彎半徑極小狀態(tài)下的采樣，采樣間隔為2.5m一個控制點；第二階段為車輛轉(zhuǎn)彎半徑大于設(shè)置閥值的情況，采樣間隔為1m一個控制點。在轉(zhuǎn)彎半徑大于設(shè)置閥值的情況下，加大采樣密度，有利于保證車輛遠(yuǎn)離道路邊緣等危險點。

將車輛采樣獲得的點集稱為全局路徑點集S。假設(shè)S中有n+1個有序空間位姿矢量v0,v1,…,vn，則按照位置矢量順序依次把相鄰的k+1個位置矢量作為一組進(jìn)行線性組合，即可以得到第i段相鄰k+1個控制點對應(yīng)的曲線公式

(4)

式中：i為B 樣條曲線的序號；di(x)為第i條B樣條曲線函數(shù)。對于三次B樣條曲線(k=3)，即

(5)

將式(3)代入式(5)并將其寫成矩陣形式可得

(6)

將車輛采樣獲得的相鄰4個坐標(biāo)點帶入公式(6)中，即可確定第i段的曲線公式di。當(dāng)確定了控制點對應(yīng)的第i段的曲線方程di、曲線的次數(shù)k以及基函數(shù)Ni,k(u)，即可確定最終車輛軌跡的B樣條曲線方程p(u)。

2.2 曲線插值

三次B樣條曲線允許改變局部控制點來改變曲線的形狀，同時保證曲率的連續(xù)性，但由于其特殊的性質(zhì)，導(dǎo)致曲線不能經(jīng)過所有的控制點[10]。因此，需要對曲線進(jìn)行插值，使得三次B樣條曲線經(jīng)過所有控制點，總體思路如下：(1)根據(jù)全局路徑點集S，采用積累弦長參數(shù)化方法構(gòu)造節(jié)點矢量，從而得到B樣條基函數(shù);(2)利用帶權(quán)控制點矩陣計算出新控制點，然后基于新控制點擬合最終軌跡。全局路徑點集S和通過全局路徑點集的B樣條曲線的新控制點之間存在的關(guān)系如式(7)所示。

pi-1(1)=pi(0)=Si(i=0,1,…,n-1)

(7)

將式(7)的關(guān)系帶入式(6)可得

Vi+4Vi+Vi+2=Si(i=0,1,…,n-1)

(8)

在公式(8)中，有n+2個未知數(shù)，但只有n個方程，缺少兩個方程，不能求出方程的解，所以添加全局路徑點集首尾端點約束條件如下。

(9)

將式(8)與式(9)帶入式(6)，即可求出經(jīng)過所有控制點的曲線方程。

3 仿真實驗

本文仿真環(huán)境硬件配置為4核2.6Ghz CPU、NVIDIA GEFORCE 720M顯卡、8GB內(nèi)存，采用的仿真軟件為ROS機器人操作系統(tǒng)下的Gazebo。車輛的主要參數(shù)如表1所示。

表1 車輛參數(shù)表

圖4為Gazebo仿真的世界環(huán)境，在該環(huán)境下已經(jīng)提前為車輛設(shè)置好了厘米級定位信息，即默

圖4 Gazebo世界環(huán)境

認(rèn)車輛里程計信息可以準(zhǔn)確獲得且誤差為厘米級。車輛的軌跡跟隨方案為MPC跟蹤，同時默認(rèn)車輛的最大加速度與最大車速能夠準(zhǔn)確響應(yīng)，即無人車最終軌跡在仿真條件下無偏差，只有速度與加速度的偏差。

將全局路徑點集S經(jīng)過三次B樣條曲線插值優(yōu)化后的結(jié)果如圖5所示。

圖5 全局路徑點集S優(yōu)化結(jié)果

圖5中圓圈為車輛里程計采樣獲得的控制點，實曲線為采用三次準(zhǔn)均勻B樣條曲線未進(jìn)行插值的軌跡擬合，虛曲線為采用三次準(zhǔn)均勻B樣條曲線對全局路徑點集S進(jìn)行插值擬合的結(jié)果。從圖中可以看出，采用三次準(zhǔn)均勻B樣條曲線插值可以使得最終擬合的軌跡經(jīng)過所有的控制點。

圖6為車輛跟蹤三次準(zhǔn)均勻B樣條曲線插值軌跡的速度與加速度變化曲線圖。圖7為車輛跟蹤未經(jīng)過曲線插值的原始軌跡的速度與加速度的變化曲線圖。圖6、圖7中實線為速度曲線，單位為m/s，虛線為加速度曲線，單位為m/s2。

圖6 跟蹤插值優(yōu)化后軌跡的速度與加速度變化曲線圖

圖7 跟蹤原始軌跡的速度與加速度變化曲線圖

從圖6中可以看出，車輛在跟蹤三次準(zhǔn)均勻B樣條曲線插值優(yōu)化后的軌跡時，車輛的速度在中段變化幅度平穩(wěn)，并且加速度振幅為2m/s2。而圖7中在跟蹤未經(jīng)優(yōu)化的原始軌跡時，車輛的速度會經(jīng)常經(jīng)過零點，并且加速度的振幅為4m/s2。這是因為原始軌跡近似一條折線，曲率不連續(xù)，導(dǎo)致車輛在線段的連接處必須將車速調(diào)整為零，然后調(diào)整方向后再次前進(jìn)，這給車輛的控制帶來很大麻煩；同時如果將該方案應(yīng)用在乘用型無人車上，將會大大影響乘坐者的舒適感。而采用三次B樣條曲線插值方法對軌跡進(jìn)行優(yōu)化處理后，車輛在跟蹤軌跡時車速平穩(wěn)，加速度振幅也比較低，可以顯著提高乘車的舒適度。

4 結(jié)論

利用三次B樣條曲線對無人車軌跡進(jìn)行平滑優(yōu)化處理，并利用Gazebo對平滑后的軌跡進(jìn)行仿真跟蹤實驗。實驗結(jié)果表明，采用三次B樣條曲線插值方法對無人車軌跡進(jìn)行優(yōu)化，能夠在車輛控制層及動力學(xué)約束不變的情況下，有效抑制速度與加速度的波動振幅，增強無人車控制的連續(xù)性與穩(wěn)定性。