FSAC賽車動態視野算法研究

宮彥喬，劉立東，李 剛，張 東

(1.遼寧工業大學 汽車與交通工程學院，遼寧 錦州 121001； 2.林肯大學，英國林肯市 LN1 1AB； 3.布魯塞爾大學，英國倫敦市 UB8 3PH)

0 引言

近些年，隨著人工智能和機器人技術的飛速發展，無人駕駛汽車作為人工智能領域的一個重要的分支已經成為國內外的研究熱點[1]。環境感知技術是無人駕駛技術的關鍵部分，主要通過布置大量車載傳感器采集并識別車輛周圍環境信息，確保無人駕駛車輛的安全性與可靠性[2]。無人駕駛方程式賽車是一種行駛在樁桶賽道里的特殊車輛，追求速度和良好的動態性能，對感知算法有更高的要求。李濤等[3]對YOLO算法進行改進，通過FSAC賽車上的相機對樁桶進行識別。張煌等[4]采用傳感器融合的方法將激光雷達和相機信息融合，對樁桶進行識別，彌補了激光雷達分辨率低和相機感知距離差的問題。趙治國等[5]為消除車輛轉向帶來的不利影響，基于車載激光雷達坐標系的變換，提出了一種目標障礙物的跟蹤方法。這些方法中純視覺或相機與激光雷達融合的感知算法需要面對相機對光照的敏感問題，所以在環境感知任務中，激光雷達具有精度高、不易受光照影響的優點[6-7]，成為了FSAC賽車的最主要的環境感知設備。基于激光雷達的感知任務主要包括提取感興趣區域、降采樣、地面分割、障礙物檢測[6]，在這些任務中感興趣區域(region of interest)的提取是感知任務中最先進行的。在激光雷達采集的點云中，以長方體、圓柱體或其他不規則形狀的立體區域框選出需要重點關注的點云信息進行算法處理，稱這個區域為激光雷達的ROI，這樣不僅可以大幅度減少需要處理的點云數量，而且將該方法應用于車輛時，可以降低非可行駛區域內障礙物的感知結果對路徑規劃算法的干擾[8]。隨著激光雷達傳感器的加速發展[9]，對激光雷達ROI方面的研究逐漸增多。尚業華等[10]采用在三維空間中框選出一個三維立方體的方法，這種方法相當于固定ROI，適應性較差，適合低速作業的農機。王濤等[1]依靠路沿的特征，將路沿作為ROI的邊界，該方法依賴于路沿的識別。王曉原等[11]提出了一種劃分多個ROI識別道路邊界的方法，該方法將車輛周圍的激光雷達點云分為4個部分，車輛左前方、右前方、左后方、右后方，然后分別處理每個部分的點云，達到簡化環境的目的。上述研究方法為論文研究提供了重要參考。能使FSAC賽車在比賽場景中獲得更多樁桶信息，有利于更好地路徑規劃。因此，論文提出了一種動態視野的算法，通過慣性導航獲得FSAC賽車自身姿態，動態改變激光雷達感興趣區域，獲得更多樁桶信息，提高感知效率，并通過實車進行了驗證。

1 傳感器工作原理及系統軟件

1.1 激光雷達原理

FSAC賽車所使用的激光雷達為機械式激光雷達。機械式激光雷達的測距方法為飛行時間測量法(time of flight)。雷達的內部設置激光發射器，隨著內部電機的旋轉，激光發射器發出多線束的超短激光脈沖，當激光遇到障礙物后發生漫反射，激光接收器接收漫反射返回的光束之后，通過激光光束在空中的飛行時間，可以精確計算得出目標物體到傳感器間的距離[12]。

(1)

式中：d為激光雷達中心軸線到物體的直線距離；c為光速；t為激光光束在空中的飛行所用時間。

對激光雷達數據包進行解析時，點云數據UDP包中主要儲存的測距數據為水平旋轉角度和距離參量。研究過程中需要對點云進行可視化，為了達到將數據轉換為點云圖進行顯示的目的，需要將極坐標下的點云數據三維坐標轉化為笛卡爾坐標系下的三維坐標，可以通過下列關系式轉化：

(2)

式中：r為激光雷達到障礙物激光點的距離；ω為激光雷達到該激光點的垂直角度；α為激光雷達到該激光點的水平旋轉角度；x、y、z分別為極坐標投影到笛卡爾x、y、z軸上的坐標[13]。

1.2 慣性導航原理

FSAC賽車需要獲取自身姿態信息，慣性導航設備具有全天候、完全自主且不受外部干擾、短時定位精度高等特點，被廣泛應用于智能車輛上[14-15]。慣性導航設備由陀螺儀和加速度計等慣性器件組成，以牛頓力學定律為基礎，測量車輛的運動加速度等信息，可以積分運算得到載體的速度、位置等導航參數，是一種航位推算式導航系統。

車輛航向角為地面坐標系下車輛質心速度與橫軸的夾角，計算方法主要依靠慣性導航中的磁力計實時感受到的地球磁場，進而通過計算載體坐標系與導航坐標系的關系得到車輛當前的朝向。計算航向角的一般步驟為：① 首先利用慣性導航x、y、z三軸加速度計計算姿態角；② 再利用姿態角將磁場強度補償到水平東北方向；③ 最后利用公式tan(y/x)計算得到航向角。

1.3 FSAC賽車感知系統軟件

FSAC賽車軟件部分選用基于 Linux內核的Ubuntu 18.04系統。在系統中部署ROS(robot operating system)[16]，利用其分布式框架結構的優勢，可以通過系統內多個節點(node)之間通訊的方式讓機器人分別運行不同的進程完成不同的任務。另一個優勢是ROS中有很多優秀的、開源的、支持調用的API(application programming interface)如可以對點云進行處理的PCL點云庫，對機器人進行仿真的Gazebo，支持可視化的Rviz，還有支持當前主流編程語言的編譯器。ROS系統的另一個特點是開源性質，對從事相關工作的開發者十分友好，避免開發者們重復“造輪子”，在各開發者的共同努力下使其具有強大的生態鏈[17]。

2 FSAC賽車動態視野算法設計

FSAC賽車激光雷達感知系統目標是盡可能多地識別樁桶，使路徑規劃模塊獲得更遠的規劃路線，或者對先驗地圖有更好的匹配效果。

2.1 算法流程

算法的整體思路是利用FSAC賽車橫向運動幅度大的特點，基于固定ROI方法進行改進，融入車輛的運動特性，從點云處理初期對賽車前方視野加以限制。

算法流程如圖1所示：由慣性導航獲取當前幀航向角Yaw_t，并與保存的上一幀航向角Yaw_t-1求差得到Diff_yaw，對Diff_yaw進行加權滑動平均濾波，由路徑規劃程序節點獲取當前車速V、轉向方向C，將各個參數進行關聯，計算得出ROI參數前方視野減少量X和兩側視野變化量Y，通過同步后的ROS消息傳入點云處理模塊，得到當前時刻的感興趣區域。

圖1 算法流程框圖

2.2 加權滑動平均濾波

在傳感器工作過程中，采集的數據難以避免地受到多種形式的影響和干擾，引起某些時刻采集的傳感器信號與基線偏差大的問題，這些問題會影響最終結果的可靠性。論文采用加權滑動平均濾波的方法對數據進行平滑處理，使傳感器采集的信號曲線更加光滑，從而達到降低干擾信號的影響，提高數據可信度的目的。滑動平均濾波(又稱遞推平均濾波)，該方法的原理為：連續取N個采樣值構造一個長度為N的隊列，隊列中的內容隨數據的進入實時向前流動，將采樣得到最新的數據插入隊尾，同時隊首的第一個數據從隊列中流出保持隊列的長度，對隊列中的N個數據求平均值，得到的結果便是經過濾波的值[18]。濾波公式為:

(3)

式中：N為固定隊列的長度，根據傳感器使用場景的不同，選取不同的值，隊列長度越短對曲線的平滑效果越差；反之，隊列長度越長，得到的曲線平滑度越高，但是響應速度也隨之變慢。

為了提高濾波的響應速度，采用加權的方法對越早時刻數據分配更小權重，即加權滑動平均濾波[19]，公式為：

(4)

式中：h(n)為加權因子；Y(n)稱為X(n)的加權平均，可以把Y(n)當作X(n)和h(n)的卷積。按照距離當前時刻越近的采樣點權值越大的原則，并考慮FSAC賽車采集的數據和傳感器特性確定加權平均因子。

IMU中的陀螺儀和加速度計等測量器件存在一定的系統誤差和隨機誤差。論文使用的航向角之差數據由上下幀作差得到，減小了受誤差影響的效果，并利用加權滑動平均濾波使數據更加接近真實值。圖2為FSAC賽車在八字環繞賽道中，以15 km/h勻速行駛右側單個圓形賽道所采集的的航向角差隨時間變化曲線。

圖2 航向角差隨時間變化曲線

圖2中位于最上方曲線為采集到航向角差Diff_yaw的原始圖像，位于中間的曲線為經過窗口為10的滑動平均濾波后的曲線，位于最下方的曲線為經過窗口為10的加權滑動平均濾波后的曲線。由圖2可知，加權滑動平均濾波有更好的效果，經過加權滑動平均濾波，以當前車速航向角差穩定分布在0°～0.8°。分析不同車速下的上下幀航向角變化量隨時間變化曲線圖像關系，為接下來確定不同車速對應的參數P作準備。

2.3 建立聯系

根據賽道情況確定初始ROI參數X、Y，X表示前方視野距，Y表示左右方視野距離變化。通過傳感器和其他程序傳遞來的各項參數與之關聯。

2.3.1確定初始ROI參數

確定FSAC賽車初始感興趣區域，經過實地實驗激光雷達在0～30 m范圍內對樁桶有很好的檢測效果，綜合考慮路徑規劃所需要的規劃距離，選取初始感興趣區域前方視野距離設置為X始，左右方視野距離設置為各±Y始。

2.3.2確定X、Y參數

設參數P為濾波得到的下一時刻x方向減小值，對不同車速下的航向角差Diff_yaw數據進行采集并分析，按照保證路徑規劃距離合適的前提條件設計參數P。舉例如下：假設在速度為 60 km/h時，采集到的上下幀航向角差Diff_yaw角度的區間為0～9°，那么將其分成1～9份(1°，2°，3°，4°，5°，6°，7°，8°，9°)；假設按照路徑規劃需求，下一時刻x方向減小值P設計為最大9 m，那么也將其分成1～9份(1 m，2 m，3 m，4 m，5 m，6 m，7 m，8 m，9 m)；將上下幀航向角差Diff_yaw角度與下一時刻x方向減小值P以正比例關系相對應。

如表1所示，不同車速下航向角差Diff_yaw與參數P的對應關系，實際使用時通過數值查表的方式快速獲得P值，進行后續計算。

表1 車速V與P值、Diff_yaw關系參數

X=X始-P

(5)

(6)

(7)

式中：車輪轉角C向左為正，向右為負；控制Y參數視野變化方向，視野向左側轉移量為Y左，視野向右變化為Y右。

因為FSAC賽車的航向角差Diff_yaw反應了賽車此時橫向運動的幅度大小，幅度越大，說明賽車正處于急轉彎的工況，感興趣區域應該向彎道的內側移動。當FSAC賽車在八字環繞賽道中行駛到當前位置時，原算法激光雷達感知區域為藍色方框與紅色虛線框聯合的區域，只有約1/4的區域可以覆蓋賽道，感知效率低,如圖3所示。而經過動態視野算法處理后激光雷達感知區域為藍色方框與綠色方框聯合的區域，有大約1/2的區域可以覆蓋到賽道，可以多感知到6個樁桶。

圖3 視野示意圖

3 實驗驗證與分析

論文將滿足大賽規則的FSAC賽車作為實驗車，賽車裝有禾賽40線激光雷達和導遠電子INS570D慣性導航系統。讓FSAC賽車在隨機擺放的賽道中進行測試，測試車速為10～35 km/h。

3.1 實驗平臺介紹

論文使用的禾賽40線激光雷達性能參數如表2所示，導遠電子INS570D組合導航系統參數如表3所示，禾賽40線激光雷達通過網口通信的方式與工控機通信，組合導航以串口通信段方式與工控機通信。

表2 激光雷達基本參數

表3 慣性導航基本參數

FSAC賽車軟件部分開發并部署于ROS，主要完成環境感知以及路徑規劃等功能。利用ROS中所提供的開發工具降低調試難度，通過Rosbag工具進行數據記錄與重放，按照需求記錄FSAC賽車在賽道中行駛時的某個節點發布的話題，將數據包移動至開發人員的電腦中，便于還原實驗場景進行算法分析和數據處理。Rviz是ROS中一款三維可視化平臺，利用Rviz實現對話題消息數據接收并進行可視化展示，實車測試中可以通過Rviz給對象發布控制信息，從而實現實時觀察硬件的響應和控制效果。

3.2 實車驗證

為了測試算法在實際使用中的效果，在FSAC賽車平臺上部署本文算法，隨機擺放樁桶進行高速循跡測試。實驗賽車在賽道中行駛如圖4所示，激光雷達布置在車頭之下前翼之上，坐標系x軸向前;慣性導航布置在賽車后軸與主環之間，坐標系x軸向前。

圖4 在試驗場中的實驗賽車行駛場景

測試過程包括：

1) 為了描述高速循跡賽道情況，對實驗場地進行建圖如圖5所示，抽取代表性彎道AB部分，A處為賽車即將進入彎道，B處為賽車已經進入彎道。

圖5 試驗場建圖

2) 為了避免干擾因素對實驗的干擾，同時部署原ROI算法與動態視野算法，并且同時運行2種算法。

3) 啟動賽車以30 km/h的速度經由AB處進入彎道。記錄激光雷達感知到的樁桶個數，路徑規劃軌跡，路徑規劃距離。

在賽道AB段激光雷達實時掃描的原始點云如圖6所示，地面比較平坦且視野清晰，激光雷達視野內干擾物體較少，滿足實驗要求。

圖6 彎道AB處原始點云

實驗結果如圖7—10所示，當賽車行駛到A處時，原算法ROI區域可以獲得11個樁桶位置信息，動態視野算法可以獲得15個樁桶位置信息，感知效率提升36%。當賽車行駛到B處時，原算法ROI區域可以獲得9個樁桶位置信息，動態視野算法可以獲得12個樁桶位置信息，感知效率提升33%。觀察2種算法賽車路徑軌跡，動態視野算法由于激光雷達感知到的樁桶個數更多，獲得的路徑規劃距離更遠，軌跡更為平滑。

4 結論

1) 考慮FSAC賽車的運動特性和傳感器的布置特點，設計了一種基于激光雷達信息和慣性導航信息的動態視野感知算法，根據激光雷達提供的賽車周圍樁桶信息，慣性導航提供的賽車自身姿態信息，動態視野算法通過動態調整賽車視野，實現在彎道行駛時獲得更多的樁桶信息，提高了賽車彎道感知效率30%以上。

2) 實車實驗驗證了算法具有準確性高、實時性好的特點。研究方法對ROI的動態調節和考慮載體的運動姿態，既可以應用于FSAC賽車上，也可以推廣至工作環境更為復雜的道路車輛中。