基于動態距離窗的交叉口CAV軌跡規劃算法*

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

（北京交通大學，綜合交通運輸大數據應用技術交通運輸行業重點實驗室，北京 100044）

前言

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

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

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

現有軌跡規劃算法或側重于通行效率，或側重于通行安全，較少考慮效率與安全的協同。針對此問題，本文中對駛入交叉口通信范圍的CAV，兼顧其行駛的安全性與效率，引入動態距離窗（dynamic distance windows，DDW）的概念，設計了可實現CAV可控安全行駛下通行效率最優的交叉口軌跡規劃算法。

1 軌跡規劃問題描述

車路協同環境下，交叉口布設的RSU 可將實時信號燈信息發送給駛入交叉口通信范圍CAV 的車載OBU，軌跡規劃算法將信號燈信息和CAV 位置、速度等初始行駛狀態參數作為輸入，規劃CAV 的行駛軌跡，實現CAV 不停車通過交叉口，在保證安全的情況下提升綠燈時間利用率和交叉口通行效率。圖1 為典型有/無軌跡規劃引導CAV 通過交叉口的軌跡示意圖，圖中vmax表示道路最大限速，tr表示綠燈啟動損失時間。

圖1 有/無軌跡規劃的時空軌跡和速度軌跡

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

2 基于DDW的CAV軌跡規劃算法

2.1 算法思路

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

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

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

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

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

基于式（1）～式（4），本文中研究車路協同環境下CAV 通過交叉口的軌跡規劃算法，當CAV 駛入交叉口通信范圍時，根據當前速度和位置、信號燈信息，按照設計的CAV 規劃算法所確定的引導速度行駛，可實現延誤最小。

考慮到CAV 駛入交叉口通信范圍時刻的初始行駛狀態具有隨機性，以及道路限速、舒適加/減速度等約束條件，并非所有CAV 都可實現在tG時刻以目標速度v*到達目標位置x*。當CAV 駛入交叉口通信范圍的初始位置與目標位置之間距離處于一定范圍內，才可設計可行軌跡使CAV 實現此目標。本文中將此距離范圍區間定義為“距離窗”。將CAV 可在tG時刻以目標速度v*到達目標位置x*稱為“實現初始目標”，將CAV 初始位置與目標位置的距離稱為“初始距離”。本文中基于距離窗的軌跡規劃算法框架如圖2所示。

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

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

2.2 動態距離窗（DDW）

CAV 駛入交叉口通信范圍時，不同的初始行駛狀態及不同的tG，對應可以實現初始目標的距離窗不同，因此稱為“動態距離窗”（DDW）。圖3 為vmax、aA、aB和xI取值固定，對應不同初始速度v0及不同tG可實現初始目標的DDW示意圖。

圖3 不同v0下tG不同時可以實現初始目標的DDW

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

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

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

圖4 tg1≥tg2時可實現初始目標的DDW

2.3 算法設計

2.3.1 初始距離處于DDW內

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

圖5 3階段速度軌跡

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

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

表1 初始距離處于DDW內的CAV 3階段軌跡規劃

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

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

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

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

2.3.2 初始距離處于DDW外

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

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

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

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

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

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

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

3 仿真驗證

3.1 仿真案例描述

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

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

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

3.2 結果分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

（4）各仿真場景中的定量統計指標

各仿真場景中仿真結果的定量指標統計如表2所示。

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

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

4 結論

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