TEB算法參數(shù)對(duì)智能車路徑規(guī)劃的影響仿真分析

廖興華　甘樺?！×﹦P

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

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

1 引言

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

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

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

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

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

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

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

3 TEB算法原理

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

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

2.約束條件是：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

optimization（優(yōu)化）參數(shù)關(guān)鍵參數(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（對(duì)于硬約束近似在懲罰函數(shù)中添加安全范圍），weight_kinematics_forward_drive（優(yōu)化過程中，只讓機(jī)器人選擇前進(jìn)方向），weight_kinematics_turning_radius（優(yōu)化過程中，最小轉(zhuǎn)彎半徑的權(quán)重）。如圖10所示。

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

5 總結(jié)

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

基金項(xiàng)目：柳州鐵道職業(yè)技術(shù)學(xué)院資助研究。

參考文獻(xiàn)：

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

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

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

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

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

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

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

[8]蓋軍雄，雷曉春，江澤濤.基于 ROS 的阿克曼機(jī)器人室內(nèi)導(dǎo)航實(shí)現(xiàn)[J].中國(guó)體視學(xué)與圖像分析. 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.