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