陸瑤敏，龔建偉，王博洋,2，關海杰

(1.北京理工大學 機械與車輛學院，北京 100081；2.北京大學 信息科學技術學院，北京 100871)

0 引言

通過學習人類駕駛員的知識與經驗能夠有效提升無人或輔助駕駛系統的適用性與接受度[1-2]。軌跡規(guī)劃系統作為無人駕駛系統中的重要組成部分，為無人車規(guī)劃出了最優(yōu)的可通行路徑[3]，這其中將復雜的駕駛任務分解成簡單的基元組合，不僅能夠有效提升軌跡生成的效率[4-5]，也能提升駕駛行為學習的效率與準確度[6-7]。

目前較為實用的運動基元表征生成方法主要有基于運動學模型[5]和曲線模型[8-9]兩種方法，但上述兩種方法主要是從車輛模型約束與軌跡平滑度的角度研究基元的生成問題，而未能將駕駛行為數據作為生成參考融入到運動基元的生成過程中。此外，上述基元生成方法也不具備對縱向與橫向耦合關系的表征能力，無法實現軌跡與其相匹配速度的協同生成；而原有各獨立基元之間的拼接則主要通過增添加減速或曲率過渡基元實現平滑過渡[10-11]，但所選用的基元過渡模式較為單一，并且不具備在過渡過程中軌跡與速度的協同關聯能力。

在基于駕駛數據的類人軌跡生成方面，Xu等[12]對候選換道基元和駕駛員行車軌跡進行偏差建模，并在實時路徑生成時進行誤差補償；He等[13]將候選軌跡與駕駛員換道軌跡的相似性參量引入到運動基元選擇的成本函數中，以實現類人軌跡生成；胡文等[14]借鑒熟練駕駛員的泊車經驗提出了相應的類人自動泊車算法。Li等[15]利用深度學習算法表征類人駕駛軌跡與道路中線的偏差，實現不同道路曲率下的類人駕駛。上述方法都借助于駕駛數據實現了類人軌跡規(guī)劃，但并未對不同駕駛風格進行區(qū)分，因此未能在規(guī)劃軌跡中體現出駕駛風格參量的影響作用。

類人運動基元的泛化生成需要具備兩方面的調整能力，其一是對目標點和中間點的適應跟隨能力，其二是對不同駕駛風格的表征調整能力。Duggal等[16]基于5次多項式構建類人換道運動基元，并通過控制基元的中間點位置來調整換道過程中的橫向和縱向間距；Schnelle等[17]針對換道場景將不同駕駛員的駕駛風格以參量的形式融入到個性化軌跡的生成模型中。但這兩種方法都只針對特定的換道場景，并且模型中的參量也與場景相關，因此雖然能夠分別實現對目標點和駕駛風格的泛化調整，但不具備對多類典型場景的適用性，未能擺脫對特定場景模型的依賴。

本文針對多類典型場景下類人軌跡的生成需求，利用動態(tài)運動基元(DMP)與奇異值分解(SVD)相結合的方法對同類型多重示范軌跡進行表征，并根據運動規(guī)劃系統設定的初始條件，在單一運動基元泛化生成的基礎上，擴展實現序列中各獨立運動基元的關聯，最終通過準均勻B樣條曲線擬合生成類人運動基元序列。

1 基元表征與序列生成

在給定運動基元類別集合MP={MP1,…,MPM}(M為總的運動基元類別數)的前提下，針對給定同類型運動基元中不同駕駛風格的運動基元集合M={m1,…,mQ}(Q為同一運動基元類型下所有示范軌跡的總個數)，本文的目的為利用SVD實現對基元表征方法中主要形狀變化表征參量ωm(m為運動基元類別)與駕駛風格微調參量集sm的分離，并和原有的軌跡基元調整參量一起構成新的調整參量集γm，進一步提升單一運動基元的泛化調整能力；此外，本文還將在給定初始條件I={I1,…,IK}(K為給定的基元序列目標點總數)的基礎上，構建基元序列中各獨立運動基元的關聯關系，并選取合適的控制點集合P以準均勻B樣條曲線為依托對序列進行重擬合，最終得到符合車輛運動連續(xù)性約束的運動基元序列S={s1,…,sL}，L為最終生成的運動基元序列點總數。

本文中主要參量的定義與解釋說明如下：

1)mi(t)是t時刻運動基元點的定義，mi(t)=[xm(t),ym(t)]T∈R2×1,其中xm和ym分別是在坐標系Oxmym中的橫、縱坐標值。運動基元的初始坐標點為坐標系Oxmym的原點，軌跡基元初始點的航向與xm軸的正方向一致。

2)ωm是同類型下運動基元的主要形狀變化表征參量，該參量集在運動基元的泛化調整過程中始終保持不變，ωm=[ωx,ωy]，其中ωx和ωy分別是所選定運動基元的縱向和橫向形狀變化表征參量。

3)γm是同類型運動基元的泛化調整參量集，能夠依據不同的目標需求實現對運動基元的調整，γm=[bm,gm,Tm,sm]。其中bm是運動基元的起始位置，gm是運動基元的終點位置，Tm是運動基元的時間尺度，sm是運動基元駕駛風格微調參量，該參量允許在同類型運動基元主要形狀的基礎上，對軌跡及相應速度的變化做進一步的微調。

4)Ik是運動基元序列所設定的初始條件，Ik=[id,Tk,bk,gk,θk]T∈R7×1，其中id是運動基元序列中第k個運動基元的類別信息，Tk是期望時間尺度信息，bk是期望起始位置，gk是期望目標位置，θk是期望目標航向。

7)sl是重擬合之后最終生成的運動基元序列點，sl=[x(l),y(l),v(l)]T∈R3×1。其中x(l)和y(l)是生成的基元序列位置點在Oxy坐標系下的坐標位置值，v(l)是相應的速度值。Oxy坐標系與基元序列中第1個獨立基元的坐標系Oxmym相一致。

本文所提出的方法致力于解決如下的兩個關鍵問題：其一，如何從相互關聯但又不完全一致的同類型多重駕駛風格的示范軌跡中，實現表征運動基元主要形狀變化參量與駕駛風格調整參量的分離；其二，在給定基元序列初始條件的前提下，完成基元序列中各獨立運動基元軌跡與速度的協同關聯，并在保證連接點處平滑過渡的基礎上，實現對目標點位置和航向的鉗位。整體的處理流程圖如圖1所示。

圖1 運動基元表征與序列生成流程

2 單一運動基元的表征

原有的DMP表征方法雖然具備表征單一運動基元的能力，但其無法實現對多重同類型示范運動基元的表征，也不具備關聯基元序列中各獨立運動基元的能力。正是基于上述兩方面的考量，本文對原有DMP表征方法進行了改進，即改進DMP(MDMP)表征方法，改進主要涉及3方面的內容：衰減函數zm、目標函數rm以及非線性形狀表征函數f(t,zm)。同類型下多重駕駛風格單一運動基元的表征及參量學習的總體流程如圖2所示。在單一基元表征方法中，非線性函數用以表征駕駛行車時序軌跡點的縱向與橫向協同配合規(guī)律；類臨界阻尼系統則實現了基元應對目標狀態(tài)變化的泛化調整。為了實現對不同駕駛風格參量的表征，非線性函數又被進一步分解成描述縱向與橫向耦合的基本形狀變化參量集ωm與駕駛風格微調參量集sm. 圖2中，Fd為非線性函數參量集，Db為表征同類型下運動基元主要形狀的數據。

圖2 基于多重示范的單一運動基元表征流程

2.1 MDMP表征方法

首先，MDMP表征方法利用Sigmoidal衰減函數替代了原有的Exponential衰減函數，Sigmoidal衰減函數和Exponential衰減函數分別為

(1)

(2)

式中：αz為設定的衰減系數；τ為時間縮放系數；Tm為多重示范基元的時間尺度；Δt為運動基元點采樣的時間間隔，由于同類型中Tm并不完全一致，因此對每個運動基元進行數據重采樣以保證其具備同樣數目的采樣點個數，相鄰采樣點之間的間隔與采樣總時間T的關系為Δt=T/100.

雖然改進前后的衰減函數都會在單一運動基元的終止時刻衰減到0，但二者整體上的衰減過程卻有顯著的差別。原有的衰減函數在初始階段便會向零點衰減，但改進后的衰減函數只有到了最后階段才會開始迅速衰減。因為衰減函數控制的是表征軌跡形狀變化的非線性函數衰減規(guī)律，因此非線性函數的延遲衰減使得基元在整個時間尺度內具有較高的表征精度，尤其有利于提升運動基元后半段特別是末尾處的表征精度。

其次，MDMP表征方法將原有固定目標點位置數值gm替換為目標函數rm，MDMP表征方法如(3)式～(5)式所示，原DMP表征方法加速度表達式如(6)式所示：

(3)

式中：

(4)

(5)

(6)

最后，非線性函數是DMP方法中表征軌跡形狀變化的核心要素。改進后的非線性函數將原有單一的非線性函數表征方式轉換成J個基底函數的組合，如(7)式所示，原DMP方法的非線性函數如(8)式所示：

(7)

(8)

2.2 基于多重示范的形狀表征參量學習方法

整體的形狀表征參量學習過程分為兩個步驟展開：其一，利用SVD方法從多重示范軌跡中提取出同類型運動基元主要形狀的基底函數Db，并實現駕駛風格微調參量集sm的分離；其二，利用回歸算法得到表征運動基元軌跡主要形狀變化的參量集ωm，以使得表征數據與示范數據之間的偏差最小。由于組成運動基元橫、縱向參量的訓練方法完全一致，因此只在此處介紹縱向參量的訓練過程。

(9)

Fd=UΣVT≈smDb，

(10)

同類型多重示范運動基元的固有形狀參量集ωm=[ω1,…,ωJ]T∈RJ×N可以通過求解如(11)式所示的優(yōu)化問題得到：

(11)

3 運動基元序列的生成

基元序列生成方法的核心是序列中各獨立運動基元的關聯與拼接問題。雖然簡單的運動基元首尾相接算法(即上一個運動基元的終點位置坐標作為下一個運動基元的起點位置坐標)能夠實現位置參量層面的關聯能力，但不具備關聯速度層面信息的基本能力。

區(qū)別于簡單的基元拼接算法，本文所提出的運動基元序列生成方法將序列看作是一個完整的整體而非各個獨立的基本單元。通過MDMP方法完成各獨立運動基元衰減函數、目標函數和非線性函數的關聯，并利用準均勻B樣條曲線對生成的運動基元序列進行重擬合，進一步提升運動基元序列的平滑度以及對目標點位置和航向的鉗位能力。運動基元序列的生成方法流程如圖3所示，其中，μk、pk、ωk分別是運動基元序列中第k個運動基元高斯內核函數的均值、帶寬和權重，μ′和p′分別是MDMP基元序列非線性表征函數中單一基底函數的高斯內核中心參量和帶寬參量，Fx和Fy是指坐標分別向x軸和y軸轉換時，考量坐標系轉換關系生成的非線性函數。其中在各獨立基元關聯的過程中，其核心是通過高斯內核組合與坐標系轉換，構建了表征縱橫向耦合關系的基元序列非線性函數，將各獨立基元的關聯問題轉換成了基元序列的重表征問題，使得基元序列具備了與單一獨立基元完全相同的泛化調整能力。

圖3 運動基元序列生成流程圖

3.1 基于MDMP方法的基元序列表征

表征運動基元序列的MDMP方法中衰減函數z′的定義為

(12)

根據給定的運動基元序列生成的初始條件，最終生成的運動基元序列的目標位置函數為

(13)

從(13)式中能夠觀察到，運動基元序列的目標位置函數以各獨立基元所處的時間段為依據，對目標位置函數進行分段化表征，實現了各獨立基元目標位置函數的關聯。

MDMP序列非線性表征函數中單一基底函數的高斯內核中心參量μ′j和帶寬參量p′j，也根據基元序列中各獨立運動基元的參量進行組合重生成，如(14)式～(17)式所示：

(14)

(15)

(16)

(17)

由于整個非線性內核函數是基于時間定義的，因此在生成整個運動基元序列的非線性內核函數時，也需要根據整體的時間尺度T′完成各獨立運動基元中非線性內核參量的關聯。其中，高斯內核中心參量μ′j是在單一獨立基元按時間尺度縮放的基礎上再進行的簡單疊加，帶寬參量p′j則是在時間尺度上的縮放。

基元序列非線性函數中各高斯內核的權重系數僅僅是各獨立運動基元參量的簡單拼接，如(18)式所示：

(18)

此外，由于基元序列中每一個獨立的運動基元都定義在Oxmym坐標系中，但運動基元序列生成時所選用的坐標系為Oxy.兩個坐標系之間的轉換關系如(19)式～(22)式所示：

(19)

(20)

(21)

(22)

式中：x′,y′和v′x,v′y分別為基元序列在Oxy坐標系下的位置坐標值和速度值。坐標分別向x軸和y軸轉換時，考量坐標系轉換關系生成的非線性函數Fx和Fy如(23)式～(26)式所示：

(23)

(24)

(25)

(26)

3.2 基于準均勻B樣條曲線的序列重擬合

本文采用準均勻B樣條曲線對實現軌跡與速度層面協同關聯的運動基元序列進行重擬合，q次B樣條曲線C(u)的定義如(27)式～(29)式所示：

(27)

(28)

(29)

式中：[P0,…,Pnc]=P，P為給定的nc+1個控制點；[u0,…,umk]=U，U為mk+1個節(jié)點；q為曲線的次數，q∈N+，mk、nc、q三者之間需要滿足的基本關系式為mk=nc+p+1；Ni,q(u)是第i個q次B樣條基函數。

準均勻B樣條曲線就是在B樣條曲線的基礎上增加了部分控制點，使得擬合之后的曲線能夠滿足對目標點位置和航向的鉗位需求。考慮到無人車行駛的期望軌跡和速度參考量需要滿足加速度連續(xù)性的基本需求[19]，因此選取q=5，采用5次準均勻B樣條曲線進行擬合。軌跡采樣控制點PT與速度采樣控制點PV的選取依據如圖4所示。

圖4 準均勻B樣條曲線控制點選取

4 基元表征與序列生成結果與分析

4.1 數據采集平臺及所建立的駕駛員數據庫

通過圖5所示的無人車輛平臺，在人工駕駛條件下，采集了駕駛員在典型交通場景下的駕駛行為數據。其中，典型場景包括換道、直角彎、U形彎、縱向跟馳等。駕駛行為數據中的車輛行駛速度信息由底層控制器局域網絡(CAN)提供，航向信息由慣性組合導航系統(英國OxTs公司生產的 Inertial + GNSS/INS系統)提供。各獨立運動基元中的相對位置參量mi(t)由速度和航向偏差根據采樣時間積分產生。本文以前期既有的運動基元類別辨識方法作為同類型的判別依據[18]，并構建同類別多重示范的運動基元數據集合，但需要指出的是本文方法并不僅限于上述聚類方法。

圖5 駕駛行為數據采集平臺

4.2 單一運動基元表征與再生成結果分析

表1 多重示范軌跡平均表征偏差

從圖6和表1中可以觀察得到，在所選定兩個類型下的運動基元中，所有示范軌跡的位置表征偏差或速度表征偏差都隨著J值的增大而減小。但是，表征精度的提升對于同類型下的多重示范軌跡并不完全一致，也就是說，J值的提升引入了越來越多同類型下的主要軌跡形狀特征，有效提升了同類型單一運動基元的表征能力。雖然通過SVD算法只提取出了同類別下的主要形狀變化參量，在一定程度上犧牲了表征的精度，但精度上的犧牲有效限制了分離出的駕駛風格微調參量集維度數。圖7展示了選定兩種交通場景類別下的運動基元，基于駕駛風格微調參量調整變化而產生的在軌跡層面和相應速度層面上的聯動調定結果，在調整過程中起始位置、目標位置和時間尺度泛化調整參量維持不變。

圖7 不同駕駛風格微調參量sm泛化調整結果

4.3 運動基元序列的生成結果分析

選取直角彎場景實現對運動基元序列重擬合效果的評估，直角彎場景中包含兩個直駛運動基元與一個直角彎基元。以關聯過程中產生的最大加速度amax(m/s2)與位置偏差Δdi(m)作為速度平滑與對目標點跟蹤精度的基本評價指標，amax為在各獨立運動基元關聯過程中所產生的最大加速度，Δdi為生成軌跡與初始設定編號為i目標點之間的最小距離。

在兩個場景下，獨立基元首尾相接的簡單拼接(SJ)方法、利用MDMP方法關聯得到的運動基元序列生成結果，以及利用重擬合(B-MDMP)方法得到的運動基元序列如圖8所示。相應的每一個連接點處的評價指標如表2所示，其中amax1為第1個過渡點最大加速度，amax2為第2個過渡點最大加速度，過渡點為不同基元序列之間的連接點。在直角彎場景中，3個目標點的航向信息為0°、90°、90°.

圖8 直角彎場景運動基元序列生成結果

當準均勻B樣條曲線的節(jié)點個數mk+1取值為21時，基元序列生成過程的平均耗時為10 ms，滿足規(guī)劃實時性需求。

從圖8和表2中可得：SJ方法僅僅實現了軌跡層面的關聯而忽略了軌跡和速度的協同關聯關系，因此雖然位置偏差較小，但卻有較大的速度跳變；MDMP方法實現了序列中各獨立運動基元軌跡和速度層面的協同關聯，避免了連接點處的速度跳變，并且最終生成的運動基元序列作為一個整體依然具備

表2 運動基元序列生成評價指標

對目標點位置gi和時間尺度Ti的泛化調整能力(見圖9)；B-MDMP方法在進一步提升軌跡平滑度的同時，實現了對目標點位置的零偏差擬合，并且增加了對目標點航向的鉗位能力。

圖9 直角彎場景運動基元序列整體泛化調整結果

5 結論

本文在對運動基元進行分類提取的基礎上，提出一種MDMP表征方法，以滿足類人參考軌跡生成對于獨立基元生成和關聯的基本需求。得出以下主要結論：

1)本文利用MDMP方法對單一運動基元進行表征以及泛化，并使用SVD實現對表征軌跡形狀非線性函數的分解，提取出同類型多重示范軌跡集中的主要形狀變化參量ωm與駕駛風格微調參量集sm，并且對比了sm的不同維度數J對多重示范表征精度的影響。

2)運動基元序列生成方法在關聯各獨立運動基元衰減函數、目標函數與非線性函數的基礎上，將基元的拼接問題轉換成基元序列的重表征問題，實現了軌跡和速度層面的協同關聯表征與整體泛化生成。

3)利用準均勻B樣條曲線在進一步平抑拼接時速度跳變的基礎上，實現了對目標點位置和航向的零偏差鉗位。