TEB算法參數對智能車路徑規劃的影響仿真分析

廖興華　甘樺福　柳皓凱

摘 要：本文針對智能車在室內環境的路徑規劃，分析研究了TEB算法不同參數對智能小車的路徑規劃影響。仿真結果表明，5大參數中有部分關鍵參數對智能小車的路徑規劃生成影響重大。需要設置最優的值才能最終獲得一條時間最短、距離最短、避開障礙物的最優路徑。才能讓智能小車進行路徑規劃并跟蹤路徑順利到達目標點。

關鍵詞：路徑規劃 TEB算法 智能車

1 引言

隨著科學技術的發展，無人駕駛技術已經不是遙不可及，隨著智能車走進我們日常生活，為我們提供了極大的便利。如掃地機器人的應用減輕了家務勞動。疫情期間無人送貨車也發揮了巨大的作用。部分發達城市開展的無人駕駛汽車乘坐體驗等。都預示著智能車技術的發展越來越成熟。而作為智能車的核心技術，路徑規劃和路徑跟蹤算法決定著智能車能否正常運行的關鍵。因此對算法的研究已經成為當前研究者的熱點問題。本文以TEB算法為例，研究TEB算法中的參數對智能車路徑規劃生成和跟蹤的影響。

2 路徑規劃算法介紹

路徑規劃是指在一定的環境模型基礎上，給定無人駕駛汽車或智能網聯汽車起始點和目標點后，按照性能指標規劃出一條無碰撞、能安全到達目標點的有效路徑[1]。

智能車常用的路徑規劃算法有A*算法，Dijkstra算法，TEB（Time Elastic Band）算法，DWA（Dynamic Window Approach）算法等。不同算法都有自己的特點。A*算法的核心部分是它對每個道路節點均設計了一個估價函數，為了保證搜索路徑的最優性，通常選擇啟發函數h（s）為曼哈頓距離、歐幾里得距離。A*算法用于規劃路徑缺點是拐點多、不平滑，不利于機器人的運行[2]。

Dijkstra算法核心思想是設置兩個節點的集合C和O，集合C中存放已找到最短路徑的節點，集合O存放當前還未找到最短路徑的節點。初始狀態時，集合C中只包含起始點，然后不斷從集合O中選擇到起始節點路徑長度最短的節點加人集合C中。集合C中每加入一個新的節點，都要修改從起始點到集合O中剩余節點的當前最短路徑長度值，集合O中各節點新的前最短路徑長度值為原來最短路徑長度值與從起始點經過新加入節點到達該節點的路徑度中的較小者。不斷重復此過程，直到集合O中所有結點全部加入集合C中為止[1]。

Dijkstra算法是一種貪心算法，每一步都選擇局部最優解，以期望產生一個最優解。缺點是該算法執行時間長，算法較復雜，速度較慢，很難滿足路徑規劃的實時性要求[3]。

DWA（Dynamic Window Approach）動態窗口法，動態窗口算法是由 Dieter Fox[4]等于1997年基于曲率速度思想提出的一種局部路徑規劃方法，考慮到運動速度、載體的運動方向和到最近障礙物的距離3個方面，通過離散搜索空間進行優化。該算法只適合全向運動機器人和差速運動機器人[5]。雖然學者們都根據需要針對特定的環境采用了不同的算法，改進了算法。但是很少有學者提出算法中設置的參數對路徑規劃的影響。基于以上問題，本文以TEB算法為例，研究TEB算法中的參數對智能車路徑規劃生成和跟蹤的影響。

3 TEB算法原理

TEB（Time Elastic Band）算法俗稱橡皮筋算法。連接起始、目標點，并讓這個路徑可以變形，變形的條件就是將所有約束當做橡皮筋的外力。中間插入N個控制橡皮筋形狀的控制點，點與點之間定義運動時間time[6]-[7]。非常適合阿克曼轉向車型。它的特點是[8]：

1.控制量是車速和前輪轉角。

2.約束條件是：

（1）路徑跟蹤和避障。路徑跟蹤將運動物體拉向全局路徑，避障又推出全局路徑。（2）速度和加速度要在最大值和最小值之間。

經過上面的目標函數的約束，然后使用開源的G2O庫（General Graph Optimization通用圖優化法）進行優化，發送速度指令。最終獲得一條時間最短、距離最短、避開障礙物且盡可能跟蹤全局路徑的局部路徑。

基于TEB算法的以上特點，本文采用TEB算法在仿真環境中研究算法參數對阿克曼轉向的智能小車路徑規劃的影響。

4 仿真環境搭建

仿真環境地圖是讓智能小車1通過3個障礙物，其中兩個圓形障礙物2和4，一個圍墻障礙物3，如圖1和圖2所示，考察不同參數對小車的影響。

4.1 TEB算法的參數影響分析

TEB算法有5大參數，分別是[9]：Trajectory（軌跡）、robot（智能小車）、GoalTolerance（目標容忍度）、Obstacles（障礙物）、Optimization（優化）。每個參數包括一系列子參數如圖3所示，列出了軌跡參數的子參數。

其中Trajectory關鍵參數包括dt_ref：含義是局部路徑規劃的解析度，即兩個相鄰點位姿時間的分辨率[10]。保持默認設置0.3。設置過小會導致小車計算量大，速度很慢，如果設置過大導致仿真時小車無法通過轉彎處并嚴重偏離規劃軌跡。如圖4所示分別設置為0.3和1的路徑規劃效果。

max_global_plan_lookahead_dist含義是考慮優化的全局計劃子集的最大長度。決定局部規劃初始軌跡的最大長度，由于局部軌跡在每個控制周期都被更新，實際執行的指令僅是軌跡上第一個點的速度值，保持默認3即可。

智能小車robot參數設置包括max_vel_x，最大x前向速度，max_vel_y，最大y前向速度，和max_vel_theta，最大轉向角速度等。如圖5所示。

其中，max_vel_x，x前向最大速度根據實際小車性能進行設置，默認是0.4m/s。如何設置太大會導致偏離路徑規劃的路線。如圖6所示綠框中綠線是原定規劃路徑，紅線是重新規劃路線，設置5m/s后導致轉彎時偏離軌跡。

由于仿真的小車是阿克曼轉向車型，所以參數max_vel_y設置為0。參數min_turning_radius（最小轉彎半徑）針對阿克曼車型根據實際設置，如果是全向移動小車設置為0。

GoalTolerance（目標容忍度）參數包括xy_goal_tolerance（目標x和y方向的偏移容忍度值），yaw_goal_tolerance（目標角度偏移容忍度），free_goal_vel（允許機器人以最大速度駛向目的地）三個參數。如圖7所示。

xy_goal_tolerance，值設置多少就表示小車距離目標點在這個值范圍內就代表達到目的地。默認是0.2m。如果設置太小如0.001，那么小車將在目標的附近往復很多次，直到達到設定的值，浪費很多時間，如圖8所示綠框處。同理yaw_goal_tolerance默認也是0.2弧度。free_goal_vel默認是false。

Obstacles（障礙物）參數用于設置環境地圖中對障礙物的處理，關鍵參數包括min_obstacle_dist（和障礙物的最小距離），include_costmap_obstacles（是否將動態障礙物預測為速度模型），costmap_obstacles_behind_robot_dist限制機器人后方規劃時考慮的局部成本地圖障礙物。如圖9所示。

其中，min_obstacle_dist和障礙物的最小安全距離根據需要設置，設置太大會導致小車無法通過狹小的地方。一般設置0.2m即可。include_costmap_obstacles默認是true，只有真時，小車才在行駛過程中規避實時探測到的建圖時不存在的動態障礙物。? costmap_obstacles_behind_robot_dist小車在行駛途中會考慮后方障礙物與自身的安全距離值。默認是1米。

optimization（優化）參數關鍵參數包括no_inner_iterations（被外循環調用后內循環執行優化次數），no_outer_iterations（執行的外循環的優化次數），penalty_epsilon（對于硬約束近似在懲罰函數中添加安全范圍），weight_kinematics_forward_drive（優化過程中，只讓機器人選擇前進方向），weight_kinematics_turning_radius（優化過程中，最小轉彎半徑的權重）。如圖10所示。

其中，no_inner_iterations和no_outer_iterations保持默認設置為5和4。penalty_epsilon參數意義為小車快到達目標點時提前減速的緩沖值，為了安全起見，該值越小越好。一般設置為0.1m。如設置過大導致小車不能按照原規劃路徑行駛，出現卡死現象，如圖 11所示設置為0.5m，小車卡住不動，不停重新規劃路徑。weight_kinematics_forward_drive參數是用于抑制小車倒車的，數值越大，則小車倒車機率越小，避免小車頻繁倒車。但是不能完全抑制倒車情況發生。weight_kinematics_turning_radius參數設置越大越容易達到小車的最小轉彎半徑，但是過大也會導致小車不停地重新規劃路徑，默認是1m。

5 總結

本文通過對TEB算法的5大參數的調試仿真，分析研究了不同參數對智能小車的路徑規劃影響。仿真發現Trajectory（軌跡）的關鍵影響參數是dt_ref和max_global_plan_lookahead_dist；robot（智能小車）關鍵影響參數是max_vel_x，min_turning_radius；GoalTolerance（目標容忍度）關鍵參數是xy_goal_tolerance； Obstacles（障礙物）關鍵參數是min_obstacle_dist、include_costmap_obstacles、costmap_obstacles_behind_robot_dist；Optimization（優化）的關鍵參數是penalty_epsilon、weight_kinematics_forward_drive、weight_kinematics_turning_radius。他們之間互相作用共同影響著智能車的路徑規劃實現。

基金項目：柳州鐵道職業技術學院資助研究。

參考文獻：

[1]陳慧妍，熊光明，龔建偉等.無人駕駛汽車概論[M] 北京理工大學出版社，2018.5.

[2]陳勁宇.DWA算法和VO混合路徑規劃算法對比研究[J].吉林大學學報（信息科學版）2022.11第40卷第6期.

[3]陳慧妍，熊光明，龔建偉.無人駕駛車輛理論與設計[M] 北京理工大學出版社，2018.3.

[4]海振洋，王健，牟思凱等.無人駕駛路徑規劃算法綜述[J]. 農業裝備與車輛工程.2022.11第60卷第11期.

[5]代婉玉. 改進TEB算法的局部路徑規劃算法研究[J].計算機工程與應用.2022，58（8）.

[6]高勝寒.智能車輛路徑規劃算法研究與仿真[D].哈爾濱：東北林業大學，2022.

[7]cabinx.Timed-Elastic-Band局部路徑規劃算法[EB/OL].https：//blog.csdn.net/xiekaikaibing/article/details/83417223.

[8]蓋軍雄，雷曉春，江澤濤.基于 ROS 的阿克曼機器人室內導航實現[J].中國體視學與圖像分析. 2021 年 第 26 卷 第 3 期.

[9]zhenz1996.TEB軌跡優化算法-代碼解析與參數建議[EB/OL].https：//blog.csdn.net/zz123456zzss/article/details/104692548.

[10] ROS.[EB/OL].http：//wiki.ros.org/teb_local_planner#Parameters.