廖興華　甘樺?！×﹦P

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

關鍵詞：路徑規(guī)劃 TEB算法 智能車

1 引言

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

2 路徑規(guī)劃算法介紹

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

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

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

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

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

3 TEB算法原理

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

1.控制量是車速和前輪轉(zhuǎn)角。

2.約束條件是：

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

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

基于TEB算法的以上特點，本文采用TEB算法在仿真環(huán)境中研究算法參數(shù)對阿克曼轉(zhuǎn)向的智能小車路徑規(guī)劃的影響。

4 仿真環(huán)境搭建

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

4.1 TEB算法的參數(shù)影響分析

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

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

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

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

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

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

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

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

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

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

optimization（優(yōu)化）參數(shù)關鍵參數(shù)包括no_inner_iterations（被外循環(huán)調(diào)用后內(nèi)循環(huán)執(zhí)行優(yōu)化次數(shù)），no_outer_iterations（執(zhí)行的外循環(huán)的優(yōu)化次數(shù)），penalty_epsilon（對于硬約束近似在懲罰函數(shù)中添加安全范圍），weight_kinematics_forward_drive（優(yōu)化過程中，只讓機器人選擇前進方向），weight_kinematics_turning_radius（優(yōu)化過程中，最小轉(zhuǎn)彎半徑的權重）。如圖10所示。

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

5 總結

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

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

參考文獻：

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

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

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

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

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

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

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

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

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

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