基于三次B樣條曲線的無人車軌跡優(yōu)化方法研究

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

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

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

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

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

1 B樣條曲線

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

(1)

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

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

(2)

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

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

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

圖1 均勻B樣條曲線

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

圖3 分段B樣條曲線

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

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

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

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

(3)

2.2 車輛曲線控制點(diǎn)

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

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

(4)

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

(5)

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

(6)

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

2.2 曲線插值

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

(9)

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

3 仿真實(shí)驗(yàn)

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

表1 車輛參數(shù)表

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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