基于動態(tài)距離窗的交叉口CAV軌跡規(guī)劃算法*

高志軍，王江鋒，陳 磊，董佳寬，羅冬宇，閆學東

（北京交通大學，綜合交通運輸大數(shù)據(jù)應用技術(shù)交通運輸行業(yè)重點實驗室，北京 100044）

前言

車路協(xié)同和自動駕駛技術(shù)在提高道路交通安全性和通行效率方面具有極其重要的應用價值，眾多學者對此進行了大量研究［1-4］。作為城市道路網(wǎng)的節(jié)點和瓶頸點，交叉口一直制約著道路的通行能力。利用車路協(xié)同和自動駕駛技術(shù)提高交叉口通行效率、減少車輛延誤，是目前的研究重點［5-7］。其中，借助車載單元（on board unit，OBU）與路側(cè)單元（road side unit，RSU）通信獲取交叉口信號燈信息，規(guī)劃網(wǎng)聯(lián)車（connected vehicle，CV）或智能網(wǎng)聯(lián)車（connect?ed and autonomous vehicle，CAV）通過交叉口的軌跡是實現(xiàn)這一目標的重要手段。

CV 通過OBU 可實現(xiàn)與信號燈信息的共享，基于共享信息對駛?cè)虢徊婵诘腃V 進行速度引導，使CV 高效并節(jié)能地通過交叉口。Yao 等［8］對CV 在駛?cè)虢徊婵谶^程中的不同位置進行限速，使CV以較小的速度波動較快通過交叉口。Xia 等［9］利用干線多個交叉口的實時配時信息，設計了CV通過干線交叉口的速度引導算法，以減少油耗和排放。Qi 等［10］考慮了駕駛員在按照引導速度行駛過程會出現(xiàn)偏差，設計的速度引導算法具有一定的容錯性，可適應駕駛員的行為。

隨著自動駕駛技術(shù)的成熟，一些學者研究了適用于具備自動駕駛與網(wǎng)聯(lián)功能的CAV 通過交叉口的軌跡規(guī)劃算法。CAV 可實現(xiàn)車輛的自動化控制，使其完全按照規(guī)劃軌跡行駛。CAV軌跡規(guī)劃算法一般均基于延誤、能耗或排放最小為目標進行設計。一些學者基于最小化油耗［11］、最小化延誤和排放［12］、最小化加權(quán)延誤和能耗［13］，分別提出了相應的CAV軌跡規(guī)劃算法，并基于混合整數(shù)線性規(guī)劃模型、雙層規(guī)劃模型、動態(tài)規(guī)劃模型對提出的算法進行求解。此外，Li等［14］通過枚舉交叉口所有可行配時方案，設計了滾動時域算法以確定與配時方案對應的CAV最優(yōu)軌跡，提高了CAV 的通行效率。上述軌跡規(guī)劃算法多只考慮通行效率，而交叉口的通行安全也是一個需要考慮的因素。Stebbins 等［15］提出了一種考慮交叉口安全約束的CAV軌跡規(guī)劃算法。

現(xiàn)有軌跡規(guī)劃算法或側(cè)重于通行效率，或側(cè)重于通行安全，較少考慮效率與安全的協(xié)同。針對此問題，本文中對駛?cè)虢徊婵谕ㄐ欧秶腃AV，兼顧其行駛的安全性與效率，引入動態(tài)距離窗（dynamic distance windows，DDW）的概念，設計了可實現(xiàn)CAV可控安全行駛下通行效率最優(yōu)的交叉口軌跡規(guī)劃算法。

1 軌跡規(guī)劃問題描述

車路協(xié)同環(huán)境下，交叉口布設的RSU 可將實時信號燈信息發(fā)送給駛?cè)虢徊婵谕ㄐ欧秶鶦AV 的車載OBU，軌跡規(guī)劃算法將信號燈信息和CAV 位置、速度等初始行駛狀態(tài)參數(shù)作為輸入，規(guī)劃CAV 的行駛軌跡，實現(xiàn)CAV 不停車通過交叉口，在保證安全的情況下提升綠燈時間利用率和交叉口通行效率。圖1 為典型有/無軌跡規(guī)劃引導CAV 通過交叉口的軌跡示意圖，圖中vmax表示道路最大限速，tr表示綠燈啟動損失時間。

圖1 有/無軌跡規(guī)劃的時空軌跡和速度軌跡

CAV 進入交叉口通信范圍后，剩余綠燈時間內(nèi)無法通過時，規(guī)劃軌跡為先減速，再勻速，最后加速達到最大速度，在綠燈亮起時CAV 恰好以最大速度通過停車線，可以消除傳統(tǒng)交通中的綠燈損失時間。而在傳統(tǒng)交通中，車輛需要在停車線前停車，在綠燈亮起后，經(jīng)過tr后加速通過交叉口。

2 基于DDW的CAV軌跡規(guī)劃算法

2.1 算法思路

軌跡規(guī)劃算法一般將延誤最小作為優(yōu)化目標，理論上，CAV 在綠燈相位期間以道路最大限速通過交叉口時，延誤最小。但在實際軌跡規(guī)劃中，須考慮CAV行駛安全，行駛過程應滿足必要安全約束條件。該約束條件可保證CAV在停車線前安全停車。

式中：t為當前時刻；tG為綠燈亮起時刻；aB為最大舒適減速度；v(t)和x(t)分別為t時刻CAV 的速度和位置；xI為交叉口停車線的位置。

在考慮式（1）安全約束的基礎上，Stebbins 等［15］研究發(fā)現(xiàn)車輛通過交叉口的延誤為

式中：d為車輛通過交叉口的延誤；xG和vG分別為綠燈亮起時車輛的位置和速度；vmax為道路最大限速；aA為最大舒適加速度。

基于文獻［15］中的研究，車輛通過交叉口過程中在綠燈起始時刻tG，存在一個特定的位置和對應的速度，分別定義為“目標位置”和“目標速度”，用x*和v*表示。當車輛在tG時刻以速度v*到達位置x*時，其延誤最小。目標速度v*和目標位置x*分別為

基于式（1）～式（4），本文中研究車路協(xié)同環(huán)境下CAV 通過交叉口的軌跡規(guī)劃算法，當CAV 駛?cè)虢徊婵谕ㄐ欧秶鷷r，根據(jù)當前速度和位置、信號燈信息，按照設計的CAV 規(guī)劃算法所確定的引導速度行駛，可實現(xiàn)延誤最小。

考慮到CAV 駛?cè)虢徊婵谕ㄐ欧秶鷷r刻的初始行駛狀態(tài)具有隨機性，以及道路限速、舒適加/減速度等約束條件，并非所有CAV 都可實現(xiàn)在tG時刻以目標速度v*到達目標位置x*。當CAV 駛?cè)虢徊婵谕ㄐ欧秶某跏嘉恢门c目標位置之間距離處于一定范圍內(nèi)，才可設計可行軌跡使CAV 實現(xiàn)此目標。本文中將此距離范圍區(qū)間定義為“距離窗”。將CAV 可在tG時刻以目標速度v*到達目標位置x*稱為“實現(xiàn)初始目標”，將CAV 初始位置與目標位置的距離稱為“初始距離”。本文中基于距離窗的軌跡規(guī)劃算法框架如圖2所示。

圖2 基于距離窗的軌跡規(guī)劃算法框架

當CAV 進入交叉口通信范圍時，根據(jù)初始行駛狀態(tài)參數(shù)計算初始距離是否處于距離窗內(nèi)。針對初始距離處于距離窗內(nèi)和距離窗外的兩種情況，分別設計軌跡規(guī)劃算法。

2.2 動態(tài)距離窗（DDW）

CAV 駛?cè)虢徊婵谕ㄐ欧秶鷷r，不同的初始行駛狀態(tài)及不同的tG，對應可以實現(xiàn)初始目標的距離窗不同，因此稱為“動態(tài)距離窗”（DDW）。圖3 為vmax、aA、aB和xI取值固定，對應不同初始速度v0及不同tG可實現(xiàn)初始目標的DDW示意圖。

圖3 不同v0下tG不同時可以實現(xiàn)初始目標的DDW

圖3 中，CAV 加/減速過程均取最大舒適加/減速度，tg0、tg1和tg2分別表示3 個臨界時間。其中tg0表示由v0可達到v*所需的最小時間，如果tG

3個臨界時間tg0、tg1和tg2的計算公式為

與3 個臨界時間tg0、tg1和tg2對應，可實現(xiàn)初始目標DDW 上限和下限的具體取值，可由圖3 中DDW示意圖結(jié)合運動學公式進行計算。對于tg0tg2等情況，同理可得到實現(xiàn)初始目標的DDW。當tG>tg2時，可實現(xiàn)初始目標的DDW 上限為粗點劃線表示的速度軌跡（前面一段被粗實線和粗虛線覆蓋）與橫縱坐標圍成的面積。但是此時的距離窗下限值必須大于細點劃線表示的速度軌跡（前面一段被細實線和細虛線覆蓋）與橫縱坐標圍成的面積，否則車輛在減速過程會出現(xiàn)速度為0 的情況，即出現(xiàn)停車現(xiàn)象，這不符合一般的駕駛習慣。為了避免規(guī)劃軌跡出現(xiàn)速度太小甚至接近于0 的情況，實際的軌跡規(guī)劃算法中可設定最小速度的閾值。另外，當初始參數(shù)不同時，tg1可等于或大于tg2，如圖4所示，但DDW的計算原理不變。

圖4 tg1≥tg2時可實現(xiàn)初始目標的DDW

2.3 算法設計

2.3.1 初始距離處于DDW內(nèi)

當初始距離處于DDW 內(nèi)時，可實現(xiàn)初始目標。軌跡規(guī)劃算法根據(jù)車輛的當前行駛狀態(tài)，在滿足所有約束條件的情況下，設計3 個階段的速度軌跡，即減速-勻速-加速、減速-勻速-減速、加速-勻速-減速、加速-勻速-加速中的一種以實現(xiàn)初始目標，如圖5 所示。圖中t0表示軌跡規(guī)劃開始時刻，t1和t2分別為加速度切換時刻。在一些特殊情況下的初始行駛狀態(tài)，只需1 個階段或2 個階段即可實現(xiàn)初始目標。

圖5 3階段速度軌跡

基于CAV 初始行駛狀態(tài)及約束條件，結(jié)合圖3所示DDW 示意圖，可確定能實現(xiàn)初始目標的唯一速度軌跡，如表1所示。

表1 中，vuni表示3 階段中勻速階段的CAV 速度，，其他變量關系如下：

表1 初始距離處于DDW內(nèi)的CAV 3階段軌跡規(guī)劃

表1 中，第1 列表示規(guī)劃軌跡的速度變化模式；第2 列是不同速度軌跡階段間的過渡時刻，t1和t2分別表示第1 和第2 階段、第2 和第3 階段之間的過渡時刻；第3 列為需要滿足的約束條件；第4 列為各速度軌跡階段的加速度，a1、a2和a3分別表示第1、第2和第3階段的加速度。

根據(jù)表1 進行軌跡規(guī)劃的計算時，當某一階段的持續(xù)時間為0 時，3 階段就退化為2 階段軌跡。若計算得到t1= 0，此時vuni=v0，則是3 階段中第1 階段的持續(xù)時間為0 的特例，即t1= 0，此時的速度模式為勻速-加速或勻速-減速；若計算得到t2=tG，此時vuni=v*，則是3 階段中第3 階段的持續(xù)時間為0 時的特例，此時的速度模式為減速-勻速或加速-勻速；若計算得到t1=t2，則是3階段中第2階段的持續(xù)時間為0 時的特例，此時的速度模式為減速-加速或加速-減速。當規(guī)劃軌跡只由勻減速、勻加速或勻速中的1 個速度軌跡變化模式即可實現(xiàn)初始目標時，3階段就退化為1階段軌跡。

1 階段規(guī)劃軌跡與2 階段和3 階段的判斷方式不同，在初始時刻，首先通過距離約束判斷初始行駛狀態(tài)是否滿足1 階段規(guī)劃。若=x*-x0，則滿足1階段規(guī)劃，然后根據(jù)v0和v*的大小關系確定加速度：

若初始行駛狀態(tài)不滿足1 階段規(guī)劃軌跡時，則通過比較與x*-x0的大小，在3階段規(guī)劃的兩個速度模式中選擇其一。若>x*-x0，則選擇減速-勻速-加速/減速的模式進行計算；若

2.3.2 初始距離處于DDW外

當初始距離處于距離窗之外時，則無法實現(xiàn)初始目標。由式（2）可知，tG時刻CAV 速度越大，同時越接近交叉口，則車輛延誤越小。結(jié)合初始距離處于DDW 內(nèi)的軌跡規(guī)劃算法思路，初始距離處于DDW外的軌跡規(guī)劃算法原理如圖6和圖7所示。

圖6 初始距離處于DDW外的兩類軌跡規(guī)劃

圖7 初始距離處于距離窗外的CAV軌跡規(guī)劃

圖6中，粗虛線和細虛線（前面一段被粗實線和細實線覆蓋）分別表示初始行駛狀態(tài)對應的DDW 上限和下限，粗實線和細實線分別表示初始距離大于DDW 上限和小于DDW 下限兩種情況的軌跡規(guī)劃，uG和vG分別表示tG時刻上述兩種情況的速度。tN′和tN分別表示DDW 上限的軌跡規(guī)劃和實際軌跡轉(zhuǎn)折時刻，tM′和tM分別表示DDW 下限的軌跡規(guī)劃和實際軌跡轉(zhuǎn)折時刻。

圖7 所示為初始距離處于DDW 外的CAV 軌跡規(guī)劃算法原理。圖中曲線表示CAV接近交叉口停車線過程中保證安全的位置-速度約束，即在停車線前任何位置以aB減速都可以安全停車。曲線及其下方區(qū)域為安全速度區(qū)域，圖中表示位置的軸為其橫軸。點劃線表示以aB減速行駛，最后在停車線前停下的過程中的時間-速度線，圖中表示時間的軸為其橫軸。圖中粗/細實線和粗/細虛線含義與圖6一致。

在圖7（a）中所示的初始距離小于DDW 下限的規(guī)劃軌跡中，在t

在圖7（b）中所示的初始距離大于DDW 上限的規(guī)劃軌跡中，在tN′前的一段時間內(nèi)，CAV 按照初始行駛狀態(tài)對應DDW上限的速度軌跡行駛。當t=tN′時，CAV軌跡與DDW上限的軌跡開始不同。因為初始距離大于DDW 上限，如果按照DDW 上限的速度軌跡行駛，即若在t=tN′時開始減速，則在tG時車輛所到達的位置和速度在位置-速度曲線之下，此時的延誤不是最小。因此，在tN′之后的一段時間內(nèi)，CAV 應該仍按原來的加速度行駛。直到若當t=tN時，CAV開始進入到交叉口前安全約束范圍內(nèi)，則開始以aB減速行駛，在tG之后，則以aA加速行駛。

3 仿真驗證

3.1 仿真案例描述

圖8 為一條安裝有信號燈的單向單車道道路，交叉口RSU 通信范圍為200 m，即CAV 駛?cè)虢徊婵谏嫌?00 m 范圍時，即可根據(jù)收到的信號燈信息進行軌跡規(guī)劃。道路限速設為vmax=50 km/h，信號燈綠燈時長為30 s，紅燈時長為30 s。最大舒適加/減速度設置為aA=1.5 m/s2和aB=2 m/s2。根據(jù)設定的參數(shù)可以確定，交叉口上游的安全約束范圍為48.2 m（圖8 中右側(cè)陰影區(qū)域），軌跡規(guī)劃中初始目標的目標速度v*=7.9 m/s，目標位置為交叉口停車線上游15.6 m（圖8 中的豎線）。將交叉口上游200 m 作為坐標軸原點，則x*=184.4 m。圖8 中左側(cè)陰影區(qū)域表示當CAV 駛?cè)胪ㄐ欧秶鷷r，當前初始行駛狀態(tài)對應的DDW。

圖8 單向單車道交叉口示意圖

仿真中車輛跟馳模型采用IDM 模型［16］，其中最小安全間距設置為2 m，車頭時距設置為2 s。仿真中時間步長設定為0.1 s，車輛啟動延誤時間tr=2 s，車輛長度設置為4 m。仿真中0-30 s為紅燈，31-60 s為綠燈（為簡化場景，沒有添加黃燈時間），仿真時長為1 個信號周期。仿真場景為一隊CAV 駛?cè)虢徊婵冢^車執(zhí)行軌跡規(guī)劃算法，頭車之后的CAV 跟隨前車行駛。仿真中頭車的初始位置為0 m，相鄰CAV的初始間距相同。

3.2 結(jié)果分析

（1）仿真場景1：所有CAV 初始速度v0=40 km/h，初始車間距為35 m，規(guī)劃軌跡為減速-勻速-加速。

根據(jù)初始參數(shù)可確定本場景中DDW 上限為405.3 m，下限為51.9 m。由于初始距離為184.4 m，處于距離窗內(nèi)，因此本場景中第1 輛CAV 可實現(xiàn)初始目標。

為進行對比分析，選擇無軌跡規(guī)劃作為對比對象。有、無軌跡規(guī)劃的第1輛CAV的行駛軌跡如圖9所示。對于圖9 中有軌跡規(guī)劃而言，第1 輛CAV 在紅燈期間以較小的速度行駛，然后在綠燈亮起時到達目標位置，在通過交叉口的過程中沒有停車，且在仿真結(jié)束時，車輛行駛的距離更遠。對于無軌跡規(guī)劃而言，第1輛CAV 在紅燈期間到達交叉口，需停車等待較長時間，當綠燈啟亮時，經(jīng)過一定的啟動延誤時間后通過交叉口。

圖9 仿真場景1頭車的時空軌跡

仿真期間，對于有軌跡規(guī)劃和無軌跡規(guī)劃引導的第1 輛CAV 的行駛速度如圖10 所示。對于有軌跡規(guī)劃而言，第1 輛CAV 只經(jīng)歷了減速-勻速-加速-勻速4 個階段。其中，前30 s 的速度變化為軌跡規(guī)劃算法中的減速-勻速-加速的3 階段速度模式。而對于無軌跡規(guī)劃而言，第1輛CAV經(jīng)歷了加速-勻速-減速-停車-加速-勻速共6個階段。

仿真中所有通過交叉口車輛的時空軌跡如圖11 所示。由圖可知，有軌跡規(guī)劃時，共有11 輛車可以不停車通過交叉口，且速度波動較小。對于無軌跡規(guī)劃而言，只有9 輛通過交叉口，并且會出現(xiàn)停車現(xiàn)象，速度波動較大。

圖10 仿真場景1的頭車速度

圖11 仿真場景1通過交叉口車輛的時刻軌跡

圖12 為有、無軌跡規(guī)劃兩種情況的仿真中所有車輛平均速度變化。對于無軌跡規(guī)劃而言，車輛平均速度波動較大；而對于有軌跡規(guī)劃而言，車輛平均速度波動較小，且最小平均速度明顯大于無軌跡規(guī)劃時的最小平均速度。

圖12 仿真場景1的車輛平均速度

（2）仿真場景2：所有CAV 初始速度v0=22 km/h，初始車間距為15 m，規(guī)劃軌跡為勻速-加速。

根據(jù)初始參數(shù)可確定本場景中的DDW 上限為387.6 m，下限為30.3 m。由于初始距離為184.4 m，處于DDW 內(nèi)，因此本場景中的第1輛CAV 可以實現(xiàn)初始目標。仿真期間，對于有、無軌跡規(guī)劃引導的第1輛CAV的行駛速度如圖13所示。

（3）仿真場景3：所有CAV的初始速度v0=10 km/h，初始車間距為8 m，規(guī)劃軌跡為加速-勻速-加速。

根據(jù)初始參數(shù)可以確定本場景中的距離窗上限為366.7 m，下限為22.9 m。由于初始距離為184.4 m，處于DDW 內(nèi)，因此本場景中的第1輛CAV 可以實現(xiàn)初始目標。仿真期間，對于有、無軌跡規(guī)劃引導的第1輛CAV的行駛速度如圖14所示。

圖13 仿真場景2的頭車速度

圖14 仿真場景3的頭車速度

（4）各仿真場景中的定量統(tǒng)計指標

各仿真場景中仿真結(jié)果的定量指標統(tǒng)計如表2所示。

表2 3個場景中有/無軌跡規(guī)劃的定量指標比較

由表2 可以看出，與無軌跡規(guī)劃相比，3 個仿真場景中有軌跡規(guī)劃的車輛平均延誤減少了12.4%～16.9%，平均速度提高了11.9%～13.6%。對于無軌跡規(guī)劃而言，綠燈期間可以通過9 輛車，其中4～5 輛需要停車，總停車時間為26.3～38.6 s。而對有軌跡規(guī)劃而言，可以通過11 輛，增加了22.2%，停車現(xiàn)象完全消失。仿真結(jié)束時，有軌跡規(guī)劃的頭車比無軌跡規(guī)劃頭車多行駛64.5～65.7 m，與無軌跡規(guī)劃尾車相同序號的有軌跡規(guī)劃車輛多行駛41.7～42.8 m。上述結(jié)果說明軌跡規(guī)劃算法可以有效提高交叉口的通行效率，增加交叉口的通行能力。另外，相比于無軌跡規(guī)劃，有軌跡規(guī)劃的速度方差降低了42.8%～70.9%，說明有軌跡規(guī)劃的車輛速度波動更小，有助于平滑交通流，保證乘員舒適性和行車安全性。

4 結(jié)論

引入動態(tài)距離窗概念設計了一種CAV 通過交叉口的軌跡規(guī)劃算法。根據(jù)CAV 駛?cè)虢徊婵谕ㄐ欧秶鷷r的初始行駛狀態(tài)參數(shù)和信號燈信息、最大舒適加/減速度和道路限速等約束條件確定DDW 的范圍，針對CAV 初始位置與停車線上游特定位置間的距離處于DDW 范圍之內(nèi)和之外兩種情況，分別設計了相應的軌跡規(guī)劃算法，使得不同初始行駛狀態(tài)下CAV 都可實現(xiàn)延誤最小，在保證安全的同時使CAV高效通過交叉口。仿真結(jié)果表明，算法可有效提高CAV 行駛速度，減少延誤，提高交叉口通行能力，同時有助于減小CAV 通過交叉口過程中的速度波動。所提出的軌跡規(guī)劃算法具有形式簡易、可獲得解析解和求解速度快等特點，對于未來車聯(lián)網(wǎng)和自動駕駛中的車輛軌跡規(guī)劃具有一定的參考價值。本研究主要針對單車進行軌跡規(guī)劃，在下一步的工作中，將重點研究多車軌跡的協(xié)同規(guī)劃。