1. 先說清楚這個項(xiàng)目到底在解決什么問題做機(jī)器人物流、倉儲搬運(yùn)這類項(xiàng)目的人多半會被同一個問題折磨過機(jī)械臂自由度一上來路徑規(guī)劃就變得非常難算。6 自由度的機(jī)械臂要在堆滿料箱的貨架間取放件或者 7 自由度冗余機(jī)械臂要穿過一個極其狹窄的工藝窗口你會發(fā)現(xiàn)普通柵格法直接算不動RRT*雖然能出結(jié)果但出來的路徑往往貼著障礙物邊緣來回“蹭”看著就讓人心里發(fā)毛。我最近在整理這些高維路徑規(guī)劃方案時反復(fù)接觸到一個思路“hyperframes”。這個詞直譯過來是“超幀”或者“超平面框架”它不是某個開源庫的名字而是一套處理高維配置空間的幾何建模方法。核心思想很簡單不要把整個高維連續(xù)空間切成海量小方格而是把障礙物的限制關(guān)系壓縮成一組低維的“超平面切片”再用這些切片組成的框架來指導(dǎo)路徑搜索。這套思路我在實(shí)際調(diào)試對比過幾次之后確實(shí)覺得它比純粹的隨機(jī)采樣規(guī)劃器更容易落地。這篇文章不是學(xué)院派綜述我就圍繞 hyperframes 的完整實(shí)操鏈路來寫它解決什么問題、幾何上怎么建模、路徑搜索時怎么用它加速、以及我在試錯過程中總結(jié)出來的幾個常用避坑點(diǎn)。適合正在做機(jī)械臂規(guī)劃、自動駕駛局部路徑、或者研究多自由度系統(tǒng)避障的朋友參考。先說一個具體的例子大家就能快速 get 到痛點(diǎn)一個 6 軸機(jī)械臂在狹窄的機(jī)柜內(nèi)部作業(yè)末端要從一個 30 厘米見方的檢修口伸進(jìn)去完成螺絲擰緊。這時候末端可達(dá)的空間是 6 維的如果按照傳統(tǒng)的均勻網(wǎng)格切分每維切 100 份那就是 100 的 6 次方個狀態(tài)點(diǎn)也就是 10 的 12 次方量級。這個數(shù)量級在普通工控機(jī)上根本沒法實(shí)時查詢。更麻煩的是真正的障礙物在 6 維空間里并不均勻分布它們往往只占很小的體積可網(wǎng)格還是把大量資源花在了空蕩蕩的“安全區(qū)”上。hyperframes 的思路就是避開這種無腦切法。它承認(rèn)高維空間的連續(xù)性也承認(rèn)障礙物的“低維流形”特性用一組靈活的超平面去包裹、描述障礙物在實(shí)際運(yùn)動方向上的限制然后用這套壓縮后的幾何框架去指導(dǎo)規(guī)劃。說得直接點(diǎn)原本路徑規(guī)劃是在 6 維狀態(tài)空間里“探路”現(xiàn)在變成了在一組只有 1 到 2 維限制關(guān)系里“繞擋板”維度降下來之后計(jì)算量自然就下來了。當(dāng)然理解這一切的前提是先把人工智能和機(jī)器人規(guī)劃里的“狀態(tài)空間”“配置空間”這些概念對齊。我下面會完整展開也會把每一個關(guān)鍵判斷背后的理由講清楚。2. hyperframes 的核心設(shè)計(jì)把障礙物變成一張張可以拼合的“超平面”2.1 “幀”到底是什么從本地坐標(biāo)系到障礙限制切片在幾何學(xué)里超平面非常簡單n 維空間里n-1 維的線性子空間就是一個超平面。在二維平面里它是一條直線在三維空間里它是一張平面在六維空間里它就是一個五維的“切片”。hyperframes 里的“frame”指的就是這種局部切片或者說局部線性化后的約束面。為什么這套東西能壓低計(jì)算量關(guān)鍵在于一個事實(shí)機(jī)器人身上的大部分碰撞約束在高維空間里并不會到處亂竄它們通常集中在某幾個維度方向的組合上。比如一個 6 軸機(jī)械臂要避開的桌面在任務(wù)空間里是一張二維平面但在關(guān)節(jié)空間里它會展開成一個低維曲面。如果我們能找到一種方式把這個曲面用若干局部超平面去擬合那么在大多數(shù)情況下只需要保存這些超平面的參數(shù)而不是保存空間中所有被占用的小格子。打個比方這就像用切片機(jī)切火腿。一整塊火腿是三維的直接記錄它每個部位的紋理信息非常費(fèi)空間。但你把它切成許多薄片之后每片就是一個二維形狀加上一個厚度信息存儲量小很多而且要對某一片單獨(dú)做加工時也特別好定位。hyperframes 里的超平面切片就是把“一整塊高維障礙”切成多個局部低維限制片每個片對應(yīng)一個基準(zhǔn)位形附近的約束關(guān)系。我在項(xiàng)目里通常會把“幀”定義成這樣一個對象它包含一個基準(zhǔn)位形 q0一組描述局部坐標(biāo)軸單位向量的張量以及一組描述從 q0 到障礙物邊界距離的系數(shù)。也就是說一幀不僅僅是一個數(shù)學(xué)超平面它還帶著“參考點(diǎn)”和“局部坐標(biāo)系”。有了這三個要素任意一個新位形 q只要換算到該幀的本地坐標(biāo)系就能很快判斷它離障礙物有多遠(yuǎn)、往哪個方向遠(yuǎn)離障礙物最有效。這就為后面路徑搜索埋下了伏筆路徑搜索時不再逐個檢查連續(xù)空間里的點(diǎn)而是把搜索約束表達(dá)成“在這些超平面組成的分割邊界里走”相當(dāng)于從點(diǎn)對點(diǎn)的避障變成了在幾何框架內(nèi)做拓?fù)鋵?dǎo)航。2.2 離線建幀與在線搜索兩階段拆解的關(guān)鍵原因整個 hyperframes 方案我習(xí)慣拆成兩個階段離線階段負(fù)責(zé)“建幀”在線階段負(fù)責(zé)“查幀”。這兩個階段的目標(biāo)完全不同拆分之后實(shí)時性才有保障。離線階段做的是批量計(jì)算。環(huán)境里有哪些固定障礙物機(jī)器人各個關(guān)節(jié)的運(yùn)動范圍是多少把這些信息都讀進(jìn)來然后在配置空間里做一次相對粗糙的采樣再把采到的每個有效區(qū)域按照障礙物表面的法向量擬合成局部超平面。這個階段的產(chǎn)出是一份“幀庫”它可以理解成一張壓縮過的“障礙物影響分布圖”。在線階段做的事情反而很輕。機(jī)器人每移動一小步只需要找到當(dāng)前位置附近有哪幾個幀把新的目標(biāo)位形變換到這幾個幀的局部坐標(biāo)系下做距離測試。如果是靜態(tài)環(huán)境這份幀庫甚至可以做成只讀共享內(nèi)存讓多個規(guī)劃線程同時訪問。如果是動態(tài)環(huán)境比如有人頻繁往料箱里放新工件那就只需要更新受影響區(qū)域的幀而不需要把整套幾何模型推到重來。我實(shí)際測下來在 6 自由度的避障場景里這種增量更新比全量重建快一個數(shù)量級以上。核心關(guān)鍵詞是“局部化”。原本高維路徑規(guī)劃之所以貴是因?yàn)閼T性思維認(rèn)為必須維護(hù)一張全局地圖。但 hyperframes 告訴我們大多數(shù)時候機(jī)器人的當(dāng)前位置只會和附近少數(shù)幾個障礙約束產(chǎn)生交互。與其把事情做全不如把事情做精。每一幀只影響它覆蓋范圍內(nèi)的路徑搜索路徑搜索也只在幀覆蓋的交界區(qū)域格外小心這樣整體計(jì)算量就被自然壓下來了。提示不要把 hyperframes 理解成一種全新的規(guī)劃算法它更像是一種空間預(yù)處理技術(shù)。它最后還是要配合 A*、Dijkstra 或者 RRT 系列算法使用只是大家在各自算法前面加了一道“空間壓縮”工序效果往往立竿見影。3. 實(shí)操過程與核心環(huán)節(jié)實(shí)現(xiàn)3.1 從環(huán)境模型到 hyperframe 建幀的完整步驟我這里按照自己項(xiàng)目中的真實(shí)流程把從原始點(diǎn)云到幀庫的步驟寫出來每一步我都會說明為什么這么做以及參數(shù)大概怎么定。第一步先給機(jī)器人建立簡化的碰撞幾何模型。機(jī)械臂的連桿和末端工具我一般用球體和膠囊體去近似而不是用精細(xì)的三角網(wǎng)格。理由是球體和膠囊體在做點(diǎn)到幾何體的距離計(jì)算時可以直接用解析公式速度快、穩(wěn)定性高。如果拿精細(xì)網(wǎng)格做距離計(jì)算每個點(diǎn)都得遍歷大量三角面哪怕用上 BVH 加速樹在高頻調(diào)用時依然吃不住。第二步把環(huán)境的障礙物也做一次降采樣處理。激光雷達(dá)或者深度相機(jī)掃進(jìn)來的原始點(diǎn)云通常有幾十萬個點(diǎn)直接拿去做超平面擬合會非常慢。我通常先做一次體素降采樣讓點(diǎn)云密度降低到大約每 5 到 10 毫米一個點(diǎn)。在這個密度下機(jī)械臂末端為 2 厘米的避障任務(wù)還不會丟失關(guān)鍵幾何信息但計(jì)算量已經(jīng)輕了非常多。第三步完成配置空間采樣。這一步其實(shí)就是確定要在哪些位形附近創(chuàng)建幀。兩種采樣策略很常見一種是沿著障礙物邊緣做漸進(jìn)式采樣另一種是沿著目標(biāo)路徑附近做局部隨機(jī)采樣。我自己大多數(shù)情況下選擇前者因?yàn)閹闹饕獌r(jià)值是描述障礙限制而不是在自由區(qū)域里均勻鋪開。你可以在自由區(qū)域里節(jié)約采樣密度但在障礙邊界附近把采樣密度提上去。第四步對每個采樣點(diǎn)做局部超平面擬合。假設(shè)我們在一個 n 維配置空間里對采樣點(diǎn)周圍一定半徑范圍內(nèi)的碰撞數(shù)據(jù)點(diǎn)做 PCA 分解取特征值最小的主成分方向作為超平面的法向量再用法向量和中心點(diǎn)確定一張超平面。這一步如果在 2D 空間里做就是把障礙邊界擬合成直線在 3D 空間里做就是擬合成平面在 6D 空間里做得到的就是 5D 超平面。雖然“高維”聽著嚇人但 PCA 分解的計(jì)算復(fù)雜度只和維度相關(guān)和空間體積無關(guān)所以即使到 10 維以上也不會特別吃力。第五步幀入庫。每個幀記錄自己的覆蓋半徑、法向量、中心位置和關(guān)聯(lián)的超平面方程。這一步我會隨手把幀的包圍盒信息也一起存進(jìn)去。后續(xù)在線查詢的時候先用包圍盒粗篩掉不相干的幀再做精確距離測試兩層判斷的速度能差出一個量級。3.2 路徑搜索階段如何利用幀庫幀庫建好之后路徑搜索的做法就會變得非常直觀。最常見的做法是先用一個低分辨率的拓?fù)鋱D連接幀與幀之間的自由空間再用 A* 或者 Dijkstra 在圖上面找一條初始路徑。因?yàn)閹窍∈璧慕Y(jié)構(gòu)化表達(dá)這個拓?fù)鋱D的節(jié)點(diǎn)數(shù)往往只有幾十到幾百個搜索速度極快。拿到這條粗路徑之后再做二次平滑。平滑時每走一步就把候選位形投影到附近的幀上做距離檢查如果觸發(fā)了幀上的碰撞限制就往幀的法向量方向退一個步長直到重新回到安全區(qū)間。我還會在搜索階段嵌入一個“最少幀穿越”的啟發(fā)值。什么意思呢就是盡量讓路徑避免跨過太多幀邊界。每跨一次幀就意味著機(jī)器人要在不同約束片之間重新對齊坐標(biāo)系這在實(shí)際執(zhí)行時意味著更頻繁的關(guān)節(jié)方向調(diào)整路徑觀感會變得很“碎”。如果 A* 的啟發(fā)函數(shù)里加入跨越幀數(shù)的權(quán)重最后得到的路徑往往會更平滑也更接近人工示教的走法。這一步的關(guān)鍵是“粗規(guī)劃輕細(xì)規(guī)劃重”。兩者都是快速操作但不必一上來就把分辨率拉滿?？蚣芤?guī)劃管拓?fù)湔_性局部修正確保無碰撞兩者配合路徑質(zhì)量和計(jì)算速度可以兼顧。注意不要忘了給幀設(shè)定“安全余量”。我在第一版實(shí)現(xiàn)時幀的超平面都是嚴(yán)格貼著障礙物表面擬合的。結(jié)果路徑規(guī)劃器在狹窄通道里生成出來的路徑雖然無碰撞但留的安全間隙幾乎是零機(jī)械臂稍微有點(diǎn)跟蹤誤差就會刮到障礙物。后來我在所有幀的距離測試中統(tǒng)一加了一個 2 到 3 厘米的膨脹系數(shù)路徑立刻變得保守且可執(zhí)行了。3.3 關(guān)鍵參數(shù)的選擇思路與計(jì)算過程這里列幾個我踩過坑之后整理出來的參數(shù)公式和經(jīng)驗(yàn)范圍大家可以直接抄作業(yè)。幀覆蓋半徑 r 的選擇。這個參數(shù)代表一個幀能影響的空間范圍。太小時幀庫會很碎路徑搜索會頻繁切換幀太大時單幀描述局部障礙趨勢的能力下降容易把遠(yuǎn)距離的彎曲障礙擬合成一條斜線產(chǎn)生誤導(dǎo)。我一般取機(jī)器人最大連桿長度 L 的 20% 到 30% 作為幀半徑。比如一個 6 軸機(jī)械臂末端到基座的最大伸展是 1.2 米那么幀半徑取 0.25 米到 0.35 米之間比較合適。實(shí)際操作時先把半徑調(diào)大跑一次再逐步縮小觀察路徑變化幅度趨于穩(wěn)定時就是當(dāng)前環(huán)境的最佳半徑。碰撞膨脹距離 d。這取決于機(jī)器人定位精度和末端工具的物理尺寸。定位重復(fù)精度 0.5 毫米的工業(yè)臂d 通常在 1.5 厘米就夠如果是移動底盤加機(jī)械臂的組合底盤定位誤差可能有 2 到 3 厘米d 至少要按 5 厘米來取。我的公式很簡單d 工具最大外接球半徑 定位誤差 1 厘米安全墊。別小看這 1 厘米它救過我很多次。初始采樣點(diǎn)數(shù) N。在配置空間里進(jìn)行邊緣采樣時樣本數(shù) N 直接決定了幀數(shù)量和建幀的時間開銷。我的經(jīng)驗(yàn)是從 N 500 起步跑一次完整規(guī)劃看結(jié)果如果出現(xiàn)明顯的路徑曲折或者誤碰再逐步增加到 2000、4000。大多數(shù) 6 自由度場景N 在 1000 到 3000 之間就足夠了再往上就是純粹堆算力路徑質(zhì)量提升非常有限。坐標(biāo)軸對齊和 PCA 擬合法向量的維度閾值保持默認(rèn)就可以。4. 常見問題與排查技巧實(shí)錄4.1 高維下“碰撞泄漏”問題為什么總感覺不夠安全用 hyperframes 做規(guī)劃最典型的問題是“碰撞泄漏”路徑規(guī)劃器認(rèn)為某一段路是安全的但實(shí)際情況中機(jī)械臂還是會和障礙物發(fā)生擦碰而且在監(jiān)控畫面里看路徑明明離障礙物還有一大段距離。這個問題的根源是 hyperframes 的稀疏表達(dá)本質(zhì)上是一種近似。擬合出來的超平面只是障礙物真實(shí)表面的簡化如果障礙物是嚴(yán)重凹凸的復(fù)雜形狀單個超平面根本覆蓋不了它的全部限制區(qū)域。路徑在經(jīng)過這個區(qū)域時只做了到超平面的距離測試卻沒做真正的碰撞體積檢測于是“以為安全”和“實(shí)際安全”之間就出現(xiàn)了縫隙。排查思路就一句話在離線建幀階段做質(zhì)量驗(yàn)收而不是在在線規(guī)劃階段反復(fù)撞墻。我問自己三個問題第一所有障礙物的表面點(diǎn)是否都有至少一個幀覆蓋到第二局部超平面擬合的殘差是否小于安全余量的一半第三靠近關(guān)節(jié)極限位置的區(qū)域有沒有額外建幀如果這三個答案都是“否”那碰撞泄漏就是必然的了。我自己的補(bǔ)法是在粗糙幀之上疊加一層細(xì)粒度碰撞檢查。路徑搜索時先用幀做粗安全判斷留下一個“疑似安全”的區(qū)域再用真實(shí)的碰撞幾何模型對這一段路徑做逐點(diǎn)檢查。這樣幀負(fù)責(zé)把大量不相關(guān)的安全空間快速過濾掉真實(shí)碰撞檢查負(fù)責(zé)守住最后的底線。兩跳檢查配合起來既保證了速度也堵住了泄漏的漏洞。4.2 動態(tài)環(huán)境下超平面刷新過慢增量更新怎么處理倉儲機(jī)器人經(jīng)常面臨環(huán)境動態(tài)變化的問題比如 AGV 把一箱料從 A 點(diǎn)搬到了 B 點(diǎn)或者人臨時在機(jī)械臂旁邊放了一把椅子。如果你用靜態(tài)幀庫跑在線規(guī)劃新障礙物出現(xiàn)時規(guī)劃器還是按舊幀走輕則路徑繞遠(yuǎn)重則直接撞上新增障礙物。動態(tài)環(huán)境的更新策略我建議采用“局部幀失效 增量重建”的機(jī)制。具體做法是每個幀保存自己的包圍盒當(dāng)外部傳感器檢測到環(huán)境變化時先定位變化區(qū)域覆蓋了哪些包圍盒把這些幀標(biāo)記為失效然后在失效區(qū)域內(nèi)重新采集點(diǎn)云、重新建幀。沒有涉及到的區(qū)域幀原封不動繼續(xù)使用。我實(shí)測過一套場景機(jī)械臂旁邊有一條傳送帶傳送帶上的工件位置每分鐘都在變。如果全量重建幀庫一次耗時大約 80 毫秒采用局部增量方案之后平均一次刷新只要 15 毫秒左右而且路徑規(guī)劃器幾乎感受不到刷新延遲。這里有個經(jīng)驗(yàn)陷阱增量重建不能只更新新增障礙物的幀還要把新增障礙物附近原本“安全”的幀也順帶重建一遍。因?yàn)榕f幀的超平面是根據(jù)“無障礙”環(huán)境擬合的此時它的法向量方向可能指向新增障礙物所在的位置。如果不重新擬合新障礙物邊界附近會出現(xiàn)幀之間的“共識盲區(qū)”路徑可能從這個盲區(qū)里穿越過去。4.3 與 RRT*、PRM、八叉樹方案的橫向?qū)Ρ葹榱俗尨蠹覍?hyperframes 的定位更清楚我整理了一張對比表基準(zhǔn)是 6 自由度機(jī)械臂在靜態(tài)料箱環(huán)境中的路徑規(guī)劃方案空間表示主要開銷路徑質(zhì)量動態(tài)環(huán)境支持適用場景均勻柵格/八叉樹離散體積占用內(nèi)存爆炸、查詢快階梯狀需平滑更新局部容易但高維不實(shí)用2D 到 3D 的低維場景RRT*隨機(jī)采樣樹采樣量大、迭代收斂慢初始路徑不夠優(yōu)需多次重連動態(tài)環(huán)境處理麻煩高維空間的通用探索PRM 概率路線圖預(yù)采樣路標(biāo)圖建圖耗時長、查詢快依賴采樣密度環(huán)境變化需重建路標(biāo)靜態(tài)環(huán)境多次查詢hyperframes障礙約束超平面庫建幀耗時中等、在線查詢極快路徑平滑接近人工示教局部增量刷新很方便高維且局部變化頻繁的場景這張表是我個人觀點(diǎn)不是標(biāo)準(zhǔn)答案但它說明了一個趨勢hyperframes 最大的競爭力在高維空間的實(shí)時規(guī)劃和動態(tài)環(huán)境的局部更新上。它不像 RRT*那樣“從零探路”而是先把空間壓縮成一張高效約束網(wǎng)再用傳統(tǒng)圖搜索快速找出路徑。代價(jià)是需要額外投入一些離線計(jì)算來建幀而這部分時間可以通過緩存復(fù)用在實(shí)際生產(chǎn)中并不吃虧。4.4 奇異性處理與高維冗余自由度一個壓箱底的小技巧再寫一個我在高維機(jī)械臂上踩過的特殊坑機(jī)械臂在某些接近奇異的位形時PCA 擬合得到的超平面法向量會發(fā)生明顯的方向突變。上一幀擬合出的約束面朝左下一幀擬合出的約束面朝右兩個幀之間形成明顯的不連續(xù)夾縫路徑規(guī)劃器在這個區(qū)域里來回震蕩最終生成一條反復(fù)“抽搐”的路徑。出現(xiàn)這個問題的本質(zhì)是機(jī)械臂在奇異位形附近丟失了某個方向上的運(yùn)動能力而這個“丟失的方向”恰好對應(yīng)超平面法向量所在的方向。直接改法是在擬合超平面時對 PCA 的特征值設(shè)一個下限閾值。當(dāng)某個方向上的特征值低于閾值時就認(rèn)為這個方向不可靠強(qiáng)制把法向量對齊到可靠性更高的軸上。這樣犧牲了一點(diǎn)局部的精度但換來了幀之間的連貫性。如果是 7 自由度冗余機(jī)械臂還有一個更聰明的辦法把冗余的 1 個自由度單獨(dú)拿出來做“前向微調(diào)”。具體來說在框架路徑搜索階段固定住冗余自由度不動只搜索剩余 6 個自由度的路徑這樣建幀的復(fù)雜度依然是 6 維級別。找到初始路徑之后再用冗余自由度對每條路徑上的關(guān)節(jié)位置做局部微調(diào)把姿態(tài)調(diào)整到更舒適的角度。這種分而治之的做法比 7 維滿空間規(guī)劃要穩(wěn)定得多也快得多。5. 我在實(shí)際調(diào)試中留下的幾點(diǎn)心得hyperframes 這套思路最初是同事推薦給我的。當(dāng)時我正被 6D 均勻柵格的內(nèi)存崩潰問題折騰得焦頭爛額抱著試一試的心態(tài)改成了超平面約束的方案。第一版跑通之后規(guī)劃一幀的時間從原來的 2 秒多降到了 30 毫秒左右我當(dāng)時的第一反應(yīng)是不信后又反復(fù)確認(rèn)了幾遍才敢相信數(shù)據(jù)是真的。后來項(xiàng)目里陸續(xù)碰到了動態(tài)環(huán)境、奇異夾縫、幀間距盲區(qū)這些實(shí)際問題每次排查都會讓我把對這套方法的理解再加深一層。它不是一個開箱即用的魔法而是需要你花時間理解環(huán)境幾何、調(diào)試安全余量和覆蓋半徑的一套工程方法?？梢坏┌褏?shù)調(diào)順了它在高維路徑規(guī)劃里的速度優(yōu)勢非常可觀尤其是在需要快速響應(yīng)外部變更的現(xiàn)場這種優(yōu)勢會被放大得更加明顯。如果大家也要在自己的項(xiàng)目里嘗試我建議不要一上來就追求全自動。先用一個小場景手動采集點(diǎn)云、手動設(shè)置采樣密度、手動檢查生成的幀方向和位置把每個中間結(jié)果都可視化出來。等到你肉眼能看出哪些幀在“分擔(dān)壓力”、哪些幀在“誤導(dǎo)規(guī)劃”的時候這套方法的核心思想你就真正吃透了。機(jī)器換道人先換思路路徑規(guī)劃這種活最終拼的還是對空間幾何的理解力。