1. 從“走哪條路”到“怎么走好”路徑規(guī)劃的工程化視角最近在整理一個(gè)機(jī)器人項(xiàng)目又翻出了以前關(guān)于路徑規(guī)劃的筆記。我發(fā)現(xiàn)一個(gè)挺有意思的現(xiàn)象很多剛接觸這個(gè)領(lǐng)域的朋友包括幾年前的我自己一提到“路徑規(guī)劃”腦子里蹦出來的第一個(gè)畫面可能就是地圖上兩點(diǎn)之間畫一條線或者是一堆算法名字——A*、Dijkstra、RRT。這當(dāng)然沒錯(cuò)但如果你真的動(dòng)手去實(shí)現(xiàn)一個(gè)能用的系統(tǒng)比如讓一臺(tái)小車從A點(diǎn)自動(dòng)開到B點(diǎn)或者讓機(jī)械臂完成一個(gè)抓取動(dòng)作你就會(huì)發(fā)現(xiàn)那條“最優(yōu)”的路徑往往只是萬里長征的第一步。真正的挑戰(zhàn)藏在算法選型之后的海量細(xì)節(jié)里動(dòng)態(tài)環(huán)境怎么處理執(zhí)行機(jī)構(gòu)的物理極限如何滿足計(jì)算資源夠不夠?qū)崟r(shí)跑這些才是決定一個(gè)路徑規(guī)劃模塊是“玩具”還是“工業(yè)級(jí)”的關(guān)鍵分水嶺。所以這篇筆記我不想再重復(fù)教科書上那些算法的推導(dǎo)過程網(wǎng)上優(yōu)秀的資料太多了而是想結(jié)合我最近在動(dòng)態(tài)避障小車和無人機(jī)仿真項(xiàng)目中踩過的坑聊聊當(dāng)我們從“學(xué)習(xí)算法”轉(zhuǎn)向“實(shí)現(xiàn)系統(tǒng)”時(shí)那些必須面對(duì)的工程化問題。我們會(huì)圍繞幾個(gè)核心矛盾展開全局最優(yōu)與局部反應(yīng)、理論最優(yōu)與物理可行、離線計(jì)算與在線實(shí)時(shí)。你會(huì)發(fā)現(xiàn)路徑規(guī)劃遠(yuǎn)不止是“找條路”它更像是在多重約束下尋找動(dòng)態(tài)平衡的藝術(shù)。2. 全局與局部分層規(guī)劃的必然性與協(xié)同挑戰(zhàn)幾乎所有實(shí)用的移動(dòng)機(jī)器人系統(tǒng)都會(huì)采用“全局路徑規(guī)劃”加“局部路徑規(guī)劃”的分層架構(gòu)。這幾乎是工程上的必然選擇原因在于計(jì)算復(fù)雜度和環(huán)境信息的完備性。全局路徑規(guī)劃比如基于A*、Dijkstra或者更高效的Jump Point Search它的任務(wù)是在已知的、靜態(tài)的或相對(duì)靜態(tài)的環(huán)境地圖中找到一條從起點(diǎn)到終點(diǎn)的無碰撞、代價(jià)較低的路徑。這里的地圖通常是事先構(gòu)建好的柵格地圖、拓?fù)涞貓D或特征地圖。全局規(guī)劃器像個(gè)戰(zhàn)略家它俯瞰全局但它的視野是“凝固”的它不知道下一秒會(huì)不會(huì)突然冒出個(gè)人或一輛車。局部路徑規(guī)劃例如動(dòng)態(tài)窗口法DWA、時(shí)間彈性帶TEB或者是基于優(yōu)化的局部規(guī)劃器常涉及二次規(guī)劃QP它的任務(wù)是處理全局路徑規(guī)劃“看不見”的東西未知的、突然出現(xiàn)的動(dòng)態(tài)障礙物。它只關(guān)心機(jī)器人周圍一小片區(qū)域比如未來幾秒內(nèi)能到達(dá)的范圍根據(jù)實(shí)時(shí)傳感器激光雷達(dá)、攝像頭的數(shù)據(jù)對(duì)全局路徑進(jìn)行微調(diào)甚至緊急重規(guī)劃以實(shí)現(xiàn)避障。它是個(gè)戰(zhàn)術(shù)家反應(yīng)迅速但視野狹窄。2.1 分層架構(gòu)下的典型工作流一個(gè)經(jīng)典的工作流是這樣的初始化加載預(yù)先構(gòu)建好的靜態(tài)地圖如SLAM建圖所得。全局規(guī)劃給定機(jī)器人的起始位姿和目標(biāo)位姿全局規(guī)劃器在地圖上計(jì)算出一條粗略的路徑。這條路徑通常由一系列路徑點(diǎn)Waypoints組成可能只考慮了靜態(tài)障礙物和大致的地形代價(jià)。路徑跟蹤與局部重規(guī)劃機(jī)器人開始沿著全局路徑移動(dòng)。局部規(guī)劃器開始工作感知實(shí)時(shí)獲取激光雷達(dá)等傳感器的點(diǎn)云數(shù)據(jù)并將其與靜態(tài)地圖融合識(shí)別出動(dòng)態(tài)障礙物。優(yōu)化局部規(guī)劃器以全局路徑為參考在滿足機(jī)器人運(yùn)動(dòng)學(xué)如最小轉(zhuǎn)彎半徑、動(dòng)力學(xué)如加速度限制約束的前提下生成一條短期內(nèi)如下一秒既避開動(dòng)態(tài)障礙物又盡量貼近全局路徑的局部軌跡?？刂茖⒕植寇壽E轉(zhuǎn)換為底層的速度線速度、角速度或位置指令發(fā)送給電機(jī)控制器。全局重規(guī)劃觸發(fā)當(dāng)局部規(guī)劃器發(fā)現(xiàn)無法在滿足約束的情況下繞過障礙物比如路被完全堵死或者機(jī)器人偏離全局路徑太遠(yuǎn)時(shí)會(huì)觸發(fā)一次全新的全局路徑規(guī)劃尋找新的可行路線。2.2 全局與局部規(guī)劃的“接口”與沖突這里就引出了第一個(gè)工程難題全局路徑和局部路徑如何“對(duì)話”如果接口設(shè)計(jì)不好機(jī)器人就會(huì)表現(xiàn)得“精神分裂”。問題一全局路徑的“不可達(dá)點(diǎn)”。全局規(guī)劃器在地圖上畫了一條完美的路徑但可能穿過了一個(gè)實(shí)際中因?yàn)榈孛娌黄?、有門檻而無法通過的區(qū)域或者路徑點(diǎn)之間的距離、角度對(duì)機(jī)器人的運(yùn)動(dòng)控制來說過于苛刻。局部規(guī)劃器試圖跟蹤但發(fā)現(xiàn)根本做不到就會(huì)在原地“掙扎”或報(bào)錯(cuò)。經(jīng)驗(yàn)之談在生成全局路徑時(shí)不僅要考慮“是否碰撞”還要考慮“是否可執(zhí)行”。這就需要在地圖的代價(jià)函數(shù)中融入機(jī)器人的運(yùn)動(dòng)學(xué)模型。例如對(duì)于阿克曼轉(zhuǎn)向的車輛路徑的曲率必須連續(xù)且小于車輛的最小轉(zhuǎn)彎半徑。我常用的一個(gè)技巧是在全局規(guī)劃后加入一個(gè)“路徑平滑和后處理”步驟使用曲線擬合如貝塞爾曲線、樣條曲線或?qū)ｉT的平滑算法使路徑點(diǎn)更稀疏、曲率更連續(xù)為局部跟蹤減輕負(fù)擔(dān)。問題二局部優(yōu)化的“短視”與“振蕩”。局部規(guī)劃器如DWA在每一個(gè)控制周期都采樣一堆可能的軌跡并評(píng)分。評(píng)分函數(shù)通常包括距離目標(biāo)的方向、距離障礙物的遠(yuǎn)近、速度大小、與全局路徑的貼合度等。如果“貼合全局路徑”的權(quán)重太低機(jī)器人可能為了避開一個(gè)臨時(shí)障礙物而嚴(yán)重偏離主路甚至走到死胡同。如果“距離障礙物”的權(quán)重太高機(jī)器人在穿過狹窄通道時(shí)可能會(huì)在兩個(gè)同樣“危險(xiǎn)”的選項(xiàng)間來回?fù)u擺產(chǎn)生振蕩。踩坑實(shí)錄在做一個(gè)室內(nèi)服務(wù)機(jī)器人時(shí)機(jī)器人在辦公室門口門較窄經(jīng)?？ㄗ　疤琛?。排查后發(fā)現(xiàn)是DWA的代價(jià)函數(shù)中障礙物距離的代價(jià)項(xiàng)在狹窄空間產(chǎn)生了過于尖銳的“斥力場”。調(diào)整方案是引入一個(gè)“平滑的斥力場”函數(shù)當(dāng)距離大于安全閾值時(shí)代價(jià)增長平緩只有當(dāng)距離非常近時(shí)代價(jià)才急劇上升。同時(shí)適當(dāng)提高了“路徑方向”項(xiàng)的權(quán)重讓機(jī)器人更有“決心”通過通道。問題三重規(guī)劃的頻率與抖動(dòng)。頻繁的全局重規(guī)劃比如每秒一次會(huì)消耗大量CPU資源而且可能導(dǎo)致機(jī)器人的目標(biāo)方向頻繁突變運(yùn)動(dòng)不流暢。但不重規(guī)劃又可能讓機(jī)器人在死局里浪費(fèi)時(shí)間。實(shí)操技巧設(shè)置合理的重規(guī)劃觸發(fā)條件。我通常采用組合條件1) 局部規(guī)劃器連續(xù)N個(gè)周期如20個(gè)周期對(duì)應(yīng)1-2秒報(bào)告“無可行軌跡”2) 機(jī)器人與全局路徑的橫向偏差超過閾值如0.5米3) 檢測到地圖有重大更新如原本通暢的路被標(biāo)記為永久障礙。并且在觸發(fā)重規(guī)劃后會(huì)有一個(gè)短暫的“冷卻期”期間即使條件再次滿足也不立即重規(guī)劃避免抖動(dòng)。3. 代價(jià)函數(shù)路徑規(guī)劃算法的“價(jià)值觀”如果說搜索算法如A*決定了路徑規(guī)劃的“能力邊界”那么代價(jià)函數(shù)就決定了它的“行為偏好”。你可以把它理解為算法的“價(jià)值觀”它認(rèn)為什么是“好”的路徑。一個(gè)典型的路徑代價(jià)函數(shù)是多項(xiàng)代價(jià)的加權(quán)和總代價(jià) w1 * 路徑長度 w2 * 轉(zhuǎn)向代價(jià) w3 * 靠近障礙物代價(jià) w4 * 平滑度代價(jià) ...3.1 代價(jià)函數(shù)設(shè)計(jì)中的核心條件與權(quán)衡從熱詞“路徑規(guī)劃的代價(jià)函數(shù)的條件”可以看出大家很關(guān)心怎么設(shè)這些權(quán)重w1, w2, ...。這里沒有銀彈完全取決于你的應(yīng)用場景。清潔機(jī)器人如阿克曼型機(jī)器人的全覆蓋路徑規(guī)劃核心目標(biāo)覆蓋率高、重復(fù)率低、效率高。代價(jià)側(cè)重“路徑長度”權(quán)重可能不是最高的因?yàn)樽咦疃搪窂娇赡軐?dǎo)致漏掃。更重要的是“覆蓋新區(qū)域的獎(jiǎng)勵(lì)”和“重復(fù)經(jīng)過同一區(qū)域的懲罰”。它的代價(jià)函數(shù)更復(fù)雜需要維護(hù)一個(gè)已覆蓋區(qū)域的地圖狀態(tài)。同時(shí)“平滑度”和“轉(zhuǎn)向代價(jià)”權(quán)重會(huì)比較高因?yàn)轭l繁的原地轉(zhuǎn)彎既耗電又降低覆蓋效率。常見算法不是簡單的點(diǎn)對(duì)點(diǎn)規(guī)劃而是基于柵格的Boustrophedon回字形覆蓋算法或者更智能的基于神經(jīng)網(wǎng)絡(luò)的PPO近端策略優(yōu)化算法后者可以通過學(xué)習(xí)來優(yōu)化覆蓋策略。自動(dòng)駕駛/動(dòng)態(tài)避障小車核心目標(biāo)安全、舒適、符合交規(guī)、抵達(dá)目標(biāo)。代價(jià)側(cè)重“靠近障礙物代價(jià)”是重中之重且是動(dòng)態(tài)計(jì)算的對(duì)于不同運(yùn)動(dòng)狀態(tài)的障礙物靜止、同向、相向會(huì)有不同的危險(xiǎn)系數(shù)?！笆孢m度”加速度/加加速度Jerk的權(quán)重也很高急剎和猛打方向體驗(yàn)很差。此外還需要加入“車道中心線偏移代價(jià)”、“違反交通規(guī)則代價(jià)”如壓線、逆行。與局部規(guī)劃QP的關(guān)系很多先進(jìn)的局部規(guī)劃器如Apollo的EM Planner將路徑生成建模為一個(gè)二次規(guī)劃QP問題。代價(jià)函數(shù)在這里直接體現(xiàn)為QP的目標(biāo)函數(shù)。例如希望路徑平滑二階導(dǎo)/曲率小同時(shí)貼近參考線全局路徑并且遠(yuǎn)離障礙物。QP的約束條件則包含了車輛運(yùn)動(dòng)學(xué)阿克曼轉(zhuǎn)向角限制、動(dòng)力學(xué)速度/加速度限制以及障礙物構(gòu)成的凸空間Corridor。這里的“條件”就是指QP問題中的等式約束和不等式約束它們共同保證了生成路徑的物理可行性。無人機(jī)自主路徑規(guī)劃仿真核心目標(biāo)考慮動(dòng)力學(xué)、能耗、時(shí)間、空域限制。代價(jià)側(cè)重除了長度還有“爬升/下降代價(jià)”耗能不同、“速度變化代價(jià)”、“禁飛區(qū)/威脅區(qū)代價(jià)”。在仿真中我們尤其關(guān)注如何用算法如RRT*, Informed RRT*在復(fù)雜三維空間中快速找到可行路徑并驗(yàn)證其滿足無人機(jī)的動(dòng)力學(xué)模型不是質(zhì)點(diǎn)模型。機(jī)械臂/磨齒機(jī)路徑規(guī)劃核心目標(biāo)無碰撞、關(guān)節(jié)運(yùn)動(dòng)平滑、避免奇異點(diǎn)、時(shí)間或能量最優(yōu)。代價(jià)側(cè)重這里的“路徑”是在機(jī)械臂的關(guān)節(jié)空間或笛卡爾空間中的軌跡。代價(jià)函數(shù)可能包含“關(guān)節(jié)位移總和”、“關(guān)節(jié)速度/加速度的平方和”能量最優(yōu)、“奇異度度量”遠(yuǎn)離奇異構(gòu)型、“工具末端姿態(tài)誤差”等。對(duì)于磨齒機(jī)這類精密加工還需要考慮“切削力平穩(wěn)”、“避免震顫”等工藝代價(jià)。3.2 一個(gè)動(dòng)態(tài)避障小車的代價(jià)函數(shù)設(shè)計(jì)實(shí)例假設(shè)我們用一個(gè)簡化的模型局部規(guī)劃器采樣一系列圓弧軌跡并為每條軌跡評(píng)分。# 偽代碼示例一條軌跡的代價(jià)計(jì)算 def calculate_trajectory_cost(traj, global_plan, obstacles, robot_pose): cost 0.0 # 1. 目標(biāo)導(dǎo)向代價(jià)軌跡終點(diǎn)與局部目標(biāo)點(diǎn)的距離 # 局部目標(biāo)點(diǎn)取自全局路徑上lookahead距離的點(diǎn) local_goal get_local_goal(global_plan, robot_pose) cost w_goal * distance(traj.end_point, local_goal) # 2. 障礙物代價(jià)遍歷軌跡上的點(diǎn)計(jì)算到最近障礙物的距離 min_obstacle_dist INFINITY for point in traj.points: dist_to_obs min_distance_to_obstacles(point, obstacles) if dist_to_obs min_obstacle_dist: min_obstacle_dist dist_to_obs # 使用非線性函數(shù)距離越近代價(jià)指數(shù)上升 if min_obstacle_dist danger_threshold: cost w_obs * exp(-min_obstacle_dist) # 危險(xiǎn)區(qū)域代價(jià)極高 else: cost w_obs / (min_obstacle_dist ** 2) # 安全區(qū)域代價(jià)隨距離衰減 # 3. 速度代價(jià)鼓勵(lì)以合適的速度前進(jìn) cost w_vel * (preferred_speed - traj.velocity) ** 2 # 4. 平滑代價(jià)與上一條采納軌跡的差異減少抖動(dòng) cost w_smooth * difference(traj, previous_best_traj) return cost調(diào)試心得調(diào)整這些權(quán)重 (w_goal,w_obs,w_vel,w_smooth) 是個(gè)細(xì)活。我的經(jīng)驗(yàn)是先在仿真環(huán)境中如ROS Gazebo用一組典型的場景直道、彎道、靜態(tài)障礙、動(dòng)態(tài)障礙進(jìn)行測試。先確保安全把w_obs調(diào)得足夠高讓機(jī)器人絕不撞上障礙物。再優(yōu)化體驗(yàn)在安全的前提下調(diào)整w_goal和w_smooth讓機(jī)器人行走路徑更直接、更順滑減少不必要的迂回和搖擺。w_vel則用來匹配機(jī)器人的動(dòng)力性能和環(huán)境要求室內(nèi)慢速走廊可稍快。4. 算法選型沒有最好只有最合適面對(duì)琳瑯滿目的路徑規(guī)劃算法新手很容易眼花繚亂。其實(shí)算法選型很大程度上取決于你的狀態(tài)空間維度、環(huán)境類型和實(shí)時(shí)性要求。算法類型典型算法適用場景優(yōu)點(diǎn)缺點(diǎn)/挑戰(zhàn)工程實(shí)現(xiàn)注意圖搜索算法Dijkstra, A*, D* Lite二維柵格地圖全局靜態(tài)路徑規(guī)劃完備能找到最優(yōu)解原理簡單高維空間如3維計(jì)算爆炸對(duì)動(dòng)態(tài)環(huán)境不友好A的性能極度依賴啟發(fā)函數(shù)h(n)的設(shè)計(jì)。DLite適用于增量式環(huán)境變化如部分地圖更新。采樣規(guī)劃算法RRT, RRT*, PRM高維空間如機(jī)械臂、無人機(jī)復(fù)雜幾何約束能快速在高維空間找到可行解不顯式建?？臻g概率完備非最優(yōu)RRTRRT*漸近最優(yōu)但慢路徑可能不光滑RRT生成路徑需要后處理平滑。在狹窄通道中RRT生長效率低可能需要橋測試等技巧。數(shù)值優(yōu)化算法二次規(guī)劃(QP)序列二次規(guī)劃(SQP)局部軌跡優(yōu)化滿足動(dòng)力學(xué)約束能直接處理復(fù)雜約束運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、障礙物生成平滑軌跡對(duì)初值敏感可能陷入局部最優(yōu)實(shí)時(shí)求解需要高效QP求解器常與采樣或搜索算法結(jié)合用后者提供初始解。需要將障礙物約束轉(zhuǎn)化為QP可處理的形式如凸多邊形走廊。智能優(yōu)化算法遺傳算法(GA)粒子群(PSO)PPO策略優(yōu)化離線全局優(yōu)化復(fù)雜代價(jià)函數(shù)如覆蓋規(guī)劃能處理非凸、非線性、多目標(biāo)的復(fù)雜優(yōu)化問題計(jì)算量大通常不能保證實(shí)時(shí)性結(jié)果具有隨機(jī)性PPO等強(qiáng)化學(xué)習(xí)算法需要大量仿真訓(xùn)練但學(xué)會(huì)后前向推理很快。適合規(guī)則固定但最優(yōu)解難以顯式建模的場景。曲線插值/幾何法貝塞爾曲線樣條曲線回字形結(jié)構(gòu)化環(huán)境如泊車、覆蓋路徑平滑計(jì)算快路徑天生光滑參數(shù)物理意義明確在復(fù)雜雜亂障礙物環(huán)境中難以直接應(yīng)用常用于全局路徑的后處理或?qū)⒏邔又噶钊绮窜嚾霂燹D(zhuǎn)化為具體路徑。4.1 結(jié)合場景的算法選擇思路機(jī)器人導(dǎo)航室內(nèi)/室外A全局 DWA/TEB/QP局部* 是經(jīng)久不衰的經(jīng)典組合。A*在2D柵格地圖上快速找到粗略路徑局部規(guī)劃器負(fù)責(zé)實(shí)時(shí)避障和平滑。對(duì)于更復(fù)雜的非完整約束機(jī)器人如阿克曼車輛TEB或直接使用QP的規(guī)劃器更能精確滿足運(yùn)動(dòng)學(xué)模型。機(jī)械臂抓取/磨齒機(jī)加工空間維度高6-7個(gè)關(guān)節(jié)環(huán)境相對(duì)結(jié)構(gòu)化。采樣算法RRT* 常用于在關(guān)節(jié)空間或任務(wù)空間尋找一個(gè)無碰撞的初始路徑然后通過軌跡優(yōu)化如SQP對(duì)路徑進(jìn)行平滑和優(yōu)化以滿足速度、加速度約束和工藝要求。無人機(jī)集群仿真/復(fù)雜空域規(guī)劃狀態(tài)空間是3D的可能還要考慮時(shí)間維度4D。快速探索隨機(jī)樹RRT的變種如Informed RRT* 因其在高維空間的效率而被廣泛使用。如果環(huán)境高度動(dòng)態(tài)可能需要結(jié)合速度障礙法VO或分布式模型預(yù)測控制DMPC進(jìn)行實(shí)時(shí)避碰。泊車路徑規(guī)劃環(huán)境高度結(jié)構(gòu)化車位、車道線約束明確車輛非完整、最小轉(zhuǎn)彎半徑。通常采用幾何分解法或最優(yōu)控制方法。將泊車過程分解為幾個(gè)典型的幾何階段如向前調(diào)整、倒車入庫每個(gè)階段用回旋曲線Clothoid或圓弧直線組合來生成路徑保證曲率連續(xù)。Reeds-Shepp曲線也是泊車規(guī)劃中常用的基礎(chǔ)幾何路徑集。全覆蓋路徑規(guī)劃清潔機(jī)器人這更像是一個(gè)“覆蓋”問題而非“點(diǎn)對(duì)點(diǎn)”問題。在規(guī)則矩形區(qū)域簡單的回字形Boustrophedon算法就非常有效。在不規(guī)則區(qū)域需要先將區(qū)域分解為多個(gè)子單元單元分解法再在每個(gè)子單元內(nèi)進(jìn)行回形覆蓋。近年來也越來越多地看到使用強(qiáng)化學(xué)習(xí)如PPO來學(xué)習(xí)覆蓋策略的研究讓機(jī)器人在復(fù)雜家庭環(huán)境中自主決定清掃順序和路徑。注意MATLAB是一個(gè)強(qiáng)大的算法仿真和驗(yàn)證平臺(tái)你可以用它快速實(shí)現(xiàn)和對(duì)比A*、RRT、PPO等各種算法驗(yàn)證其正確性和性能。但在實(shí)際機(jī)器人上部署時(shí)通常需要用C、Python等語言在ROS或其它機(jī)器人中間件中重新實(shí)現(xiàn)并充分考慮計(jì)算效率和與傳感器、控制器的集成。5. 從仿真到實(shí)車那些必須填平的“鴻溝”在電腦上看到一條完美的規(guī)劃路徑和讓真實(shí)的機(jī)器人穩(wěn)穩(wěn)地走出來中間隔了無數(shù)個(gè)坑。仿真如Gazebo, MATLAB/Simulink是必不可少的環(huán)節(jié)但它只能驗(yàn)證算法的邏輯正確性。5.1 仿真中無法完全模擬的實(shí)車因素傳感器噪聲與延遲仿真中的激光雷達(dá)數(shù)據(jù)是完美的沒有噪點(diǎn)延時(shí)幾乎為零。實(shí)車上激光雷達(dá)有測量噪聲點(diǎn)云可能稀疏不均攝像頭圖像有畸變視覺處理算法有延遲。這些會(huì)導(dǎo)致局部規(guī)劃器感知到的環(huán)境與真實(shí)環(huán)境有差異可能把噪點(diǎn)當(dāng)成障礙物虛警或者沒及時(shí)檢測到快速靠近的物體漏警。應(yīng)對(duì)策略在規(guī)劃器的感知輸入端必須進(jìn)行有效的濾波如統(tǒng)計(jì)濾波器去除孤立噪點(diǎn)和時(shí)序?qū)R時(shí)間同步。對(duì)于動(dòng)態(tài)障礙物要使用跟蹤算法如卡爾曼濾波來預(yù)測其未來狀態(tài)而不是只依賴當(dāng)前時(shí)刻的觀測。控制誤差與執(zhí)行延遲仿真中你下發(fā)了v, w速度指令機(jī)器人下一秒就會(huì)精確到達(dá)預(yù)定位置。實(shí)車上電機(jī)響應(yīng)有延遲輪子可能存在打滑底層PID控制器會(huì)有穩(wěn)態(tài)誤差。這會(huì)導(dǎo)致機(jī)器人實(shí)際走出的軌跡與規(guī)劃軌跡存在偏差。應(yīng)對(duì)策略局部規(guī)劃器需要有反饋。這就是為什么單純的“前饋”路徑規(guī)劃不行必須結(jié)合“路徑跟蹤”控制器如Pure Pursuit, Stanley, MPC。跟蹤控制器根據(jù)機(jī)器人的實(shí)際位姿來自里程計(jì)/融合定位與期望路徑的偏差實(shí)時(shí)計(jì)算修正后的控制指令形成一個(gè)“規(guī)劃-跟蹤-反饋”的閉環(huán)。地圖的不確定性與動(dòng)態(tài)性仿真地圖是精確且靜態(tài)的。實(shí)車環(huán)境中地圖可能有誤差SLAM建圖不準(zhǔn)環(huán)境會(huì)變化椅子被移動(dòng)了門被關(guān)上了。應(yīng)對(duì)策略需要一套健壯的地圖更新機(jī)制。不能完全依賴先驗(yàn)靜態(tài)地圖。通常采用“占據(jù)柵格地圖”并設(shè)置每個(gè)柵格的更新規(guī)則。對(duì)于激光掃描到的空閑區(qū)域降低占據(jù)概率對(duì)于掃描到的障礙提高占據(jù)概率。同時(shí)要區(qū)分“靜態(tài)障礙”和“臨時(shí)障礙”如人對(duì)于臨時(shí)障礙可以設(shè)置其占據(jù)概率隨時(shí)間衰減避免它永久地阻塞路徑。計(jì)算資源限制在仿真里你可以用最復(fù)雜的算法跑最密集的采樣。在實(shí)車的嵌入式計(jì)算平臺(tái)如Jetson Nano, Raspberry Pi上CPU和內(nèi)存是寶貴的。應(yīng)對(duì)策略算法效率優(yōu)化是關(guān)鍵。例如對(duì)A*算法使用更高效的優(yōu)先隊(duì)列設(shè)計(jì)更精準(zhǔn)的啟發(fā)函數(shù)對(duì)DWA限制采樣空間速度、角速度的分辨率和范圍對(duì)QP求解使用針對(duì)嵌入式平臺(tái)優(yōu)化的求解庫如OSQP。必要時(shí)需要做算法簡化比如用預(yù)計(jì)算的查找表來替代實(shí)時(shí)復(fù)雜計(jì)算。5.2 實(shí)車調(diào)試的“笨”辦法與核心檢查點(diǎn)當(dāng)算法從仿真移植到實(shí)車出現(xiàn)問題時(shí)一個(gè)系統(tǒng)化的排查流程至關(guān)重要隔離測試不要一上來就讓整個(gè)導(dǎo)航棧跑。先單獨(dú)測試每個(gè)模塊。傳感器發(fā)布一個(gè)固定的速度指令看機(jī)器人是否直線行走同時(shí)記錄激光雷達(dá)和里程計(jì)數(shù)據(jù)。檢查里程計(jì)積分是否準(zhǔn)確走10米實(shí)際走了多少檢查激光數(shù)據(jù)在靜止時(shí)是否穩(wěn)定。地圖與定位用手柄遙控機(jī)器人走一圈看看建出的地圖質(zhì)量如何。然后讓機(jī)器人靜止看其在地圖中的定位是否漂移AMCL粒子濾波的穩(wěn)定性。規(guī)劃器在已知好的地圖和定位下給定一個(gè)簡單目標(biāo)點(diǎn)開啟規(guī)劃器但先不執(zhí)行控制指令在RViz等可視化工具中觀察它規(guī)劃的全局路徑和局部軌跡是否合理。把障礙物代價(jià)、速度代價(jià)等可視化出來看代價(jià)函數(shù)的分布是否符合預(yù)期。逐步集成確認(rèn)每個(gè)模塊單獨(dú)工作后再逐步連接起來。先開定位和地圖讓規(guī)劃器出路徑但控制指令不發(fā)到底層只觀察。然后發(fā)送控制指令但用手扶著機(jī)器人或者在一個(gè)空曠無障的環(huán)境進(jìn)行觀察機(jī)器人的實(shí)際運(yùn)動(dòng)與規(guī)劃軌跡的偏差。最后加入真實(shí)的障礙物進(jìn)行避障測試。數(shù)據(jù)記錄與回放充分利用ROS的rosbag工具。在測試時(shí)錄制所有相關(guān)話題/scan,/odom,/map,/global_plan,/local_plan,/cmd_vel。當(dāng)出現(xiàn)異常行為時(shí)回放bag文件像看錄像一樣一步步分析問題出在哪個(gè)環(huán)節(jié)。是感知錯(cuò)了定位飄了還是規(guī)劃器算出了一條奇怪的路徑路徑規(guī)劃從理論到落地是一個(gè)不斷與不確定性斗爭、在多重約束中尋找可行解的過程。它沒有一勞永逸的“最佳算法”只有針對(duì)特定場景、特定機(jī)器人平臺(tái)不斷調(diào)優(yōu)的“合適方案”。這份筆記里提到的分層架構(gòu)、代價(jià)函數(shù)設(shè)計(jì)、算法選型依據(jù)以及仿真到實(shí)車的鴻溝都是我過去項(xiàng)目里反復(fù)驗(yàn)證過的經(jīng)驗(yàn)。真正的精通來自于在一次次調(diào)試、失敗和迭代中對(duì)機(jī)器人、環(huán)境和算法之間相互作用的深刻理解。下次當(dāng)你再看到“路徑規(guī)劃”這四個(gè)字時(shí)希望你能想到的不再只是一個(gè)孤立的算法而是一套環(huán)環(huán)相扣、需要精心設(shè)計(jì)和調(diào)試的復(fù)雜系統(tǒng)。