做多線激光雷達(dá)融合這件事我最常被問(wèn)到的一個(gè)問(wèn)題是雙雷達(dá)到底是不是智商稅我的答案是看場(chǎng)景。如果你做的是園區(qū)無(wú)人車、室內(nèi)外巡檢機(jī)器人、或者任何需要在近場(chǎng)有超大視場(chǎng)角覆蓋的感知系統(tǒng)雙雷達(dá)帶來(lái)的不是“多一份數(shù)據(jù)”那么簡(jiǎn)單而是把感知盲區(qū)直接砍掉一大截。這次我基于ROS2 Humble和兩臺(tái)Livox Mid-360完整走了一遍從驅(qū)動(dòng)配置、網(wǎng)絡(luò)設(shè)置、外參標(biāo)定到C/PCL點(diǎn)云融合的流程把中間踩過(guò)的坑和最終能直接跑通的方案整理出來(lái)希望對(duì)正在做類似方案的朋友有實(shí)際幫助。Livox Mid-360這臺(tái)雷達(dá)很特殊它不是傳統(tǒng)旋轉(zhuǎn)機(jī)械式雷達(dá)而是基于棱鏡掃描的非重復(fù)掃描模式單臺(tái)水平視場(chǎng)角360度、垂直視場(chǎng)角59度-7度到52度這個(gè)垂直視場(chǎng)對(duì)于近距離避障和地圖構(gòu)建來(lái)說(shuō)非常友好。但單臺(tái)的問(wèn)題也明顯垂直FOV雖然大水平360度看一圈沒(méi)問(wèn)題可一旦安裝在車頂或者機(jī)器人中部車體自身就會(huì)形成遮擋尤其是前后方向貼近車體的區(qū)域完全是黑的。雙雷達(dá)一前一后或者一上一下布置就能把這個(gè)問(wèn)題解決掉。這篇內(nèi)容不是純理論科普而是我在Ubuntu 22.04 ROS2 Humble環(huán)境下從零開(kāi)始配置雙Mid-360、做外參標(biāo)定、然后手寫(xiě)C節(jié)點(diǎn)做點(diǎn)云融合的完整記錄。包括網(wǎng)絡(luò)上很難查到的幾個(gè)細(xì)節(jié)比如雙雷達(dá)IP如何規(guī)劃、時(shí)間同步怎么處理、PCL拼接時(shí)點(diǎn)云坐標(biāo)系怎么統(tǒng)一我都會(huì)逐一說(shuō)明。即使你之前沒(méi)用過(guò)Livox跟著這套流程走也能把雙雷達(dá)跑起來(lái)。1. 整體方案設(shè)計(jì)與硬件準(zhǔn)備1.1 為什么選擇雙Mid-360而不是單臺(tái)加補(bǔ)盲雷達(dá)先說(shuō)結(jié)論Mid-360在一臺(tái)設(shè)備上同時(shí)解決了“近場(chǎng)盲區(qū)”和“垂直FOV不足”兩個(gè)痛點(diǎn)。它的垂直視場(chǎng)角達(dá)到59度相比之下傳統(tǒng)Velodyne VLP-16垂直方向只有30度而且越靠近車體下方越容易丟點(diǎn)。Mid-360的盲區(qū)很小最小探測(cè)距離0.1米這意味著它可以安裝在更靠近車體邊緣的位置而不會(huì)出現(xiàn)大范圍近距離盲區(qū)。但單臺(tái)Mid-360在水平安裝時(shí)由于雷達(dá)自身外殼和安裝支架的存在會(huì)在大致正下方和正上方形成圓錐形盲區(qū)。如果只裝一臺(tái)探測(cè)范圍是360度水平全覆蓋但車體前后方向如果有較高的物體比如貨架、墻體轉(zhuǎn)角很可能會(huì)被車體自身遮擋。雙雷達(dá)一前一后布置前雷達(dá)負(fù)責(zé)前方和上方后雷達(dá)負(fù)責(zé)后方和下方兩臺(tái)的視場(chǎng)角互為補(bǔ)充基本上能把盲區(qū)壓縮到極小范圍。有人會(huì)問(wèn)那直接用一臺(tái)Mid-360加一個(gè)單線雷達(dá)補(bǔ)盲不行嗎可以但你會(huì)面臨多傳感器聯(lián)合標(biāo)定、不同點(diǎn)云密度融合、不同幀率對(duì)齊這些額外麻煩。兩臺(tái)同樣的Mid-360幀率一致、點(diǎn)云特性一致、驅(qū)動(dòng)接口一致融合邏輯要簡(jiǎn)單得多。1.2 硬件構(gòu)型與安裝注意點(diǎn)我的安裝構(gòu)型是前后布置前雷達(dá)水平安裝于車體前方頂部后雷達(dá)水平安裝于車體后方頂部?jī)膳_(tái)雷達(dá)的x軸方向一致朝前。這里的核心要領(lǐng)是保證兩臺(tái)雷達(dá)的初始姿態(tài)盡量接近特別是roll和pitch角不要差太多。雖然外參標(biāo)定可以糾正安裝誤差但安裝時(shí)的機(jī)械偏差越小標(biāo)定結(jié)果越穩(wěn)定后續(xù)融合效果也越好。安裝時(shí)還要注意幾個(gè)細(xì)節(jié)。第一雷達(dá)底座的散熱面不要被完全遮擋Mid-360長(zhǎng)時(shí)間滿負(fù)荷運(yùn)行時(shí)發(fā)熱量不小完全密封會(huì)導(dǎo)致點(diǎn)云噪聲閾值升高。第二支架剛度要足夠低速巡檢車還好如果是AGV或叉車振動(dòng)會(huì)讓外參發(fā)生微小漂移最終表現(xiàn)為遠(yuǎn)處點(diǎn)云重影。第三兩臺(tái)雷達(dá)的網(wǎng)口分別接到工控機(jī)的兩個(gè)獨(dú)立網(wǎng)口上不要通過(guò)交換機(jī)串聯(lián)這樣可以避免廣播風(fēng)暴和帶寬競(jìng)爭(zhēng)。Mid-360的帶寬需求不算高單臺(tái)點(diǎn)云輸出頻率10Hz時(shí)碼率大約5-6MB/s兩臺(tái)同時(shí)開(kāi)也就12MB/s左右千兆網(wǎng)口完全沒(méi)壓力。但有個(gè)坑Mid-360默認(rèn)的UDP數(shù)據(jù)包比較大如果網(wǎng)卡開(kāi)啟了流控或者驅(qū)動(dòng)有丟包點(diǎn)云會(huì)出現(xiàn)橫向的“撕裂”狀空洞。排查手法是在驅(qū)動(dòng)參數(shù)里把packet_rate調(diào)低或者直接禁用網(wǎng)卡的TCP/IP卸載功能。1.3 軟件棧選型ROS2 Humble livox_ros_driver2Livox官方提供了兩代ROS驅(qū)動(dòng)第一代是針對(duì)ROS1的livox_ros_driver第二代是livox_ros_driver2支持ROS2。這次我用的是livox_ros_driver2的ROS2分支版本對(duì)應(yīng)Humble。需要注意livox_ros_driver2在編譯前需要修改配置文件來(lái)指定雷達(dá)型號(hào)Mid-360對(duì)應(yīng)的lidar_type是1即Livox Mid-360這點(diǎn)非常容易漏掉默認(rèn)配置是Horizon會(huì)導(dǎo)致驅(qū)動(dòng)啟動(dòng)后收不到數(shù)據(jù)。驅(qū)動(dòng)安裝本身不復(fù)雜源碼編譯時(shí)依賴livox_ros_driver2的livox_interfaces包直接用colcon build即可。但有一點(diǎn)坑如果你之前裝過(guò)ROS1的livox驅(qū)動(dòng)工作空間里會(huì)有包名沖突需要先清理干凈。另外livox_ros_driver2依賴的rosidl_default_generators在Humble里是默認(rèn)安裝的但如果你用的是精簡(jiǎn)版Docker鏡像可能需要手動(dòng)補(bǔ)裝。2. 雙雷達(dá)外參標(biāo)定從原理到實(shí)操2.1 標(biāo)定到底在算什么雙雷達(dá)融合的核心前提是把兩個(gè)雷達(dá)各自坐標(biāo)系下的點(diǎn)云變換到同一個(gè)坐標(biāo)系下。這個(gè)變換關(guān)系由一個(gè)4x4的齊次變換矩陣描述包含3個(gè)平移量x, y, z和3個(gè)旋轉(zhuǎn)量roll, pitch, yaw。標(biāo)定的過(guò)程就是確定這6個(gè)自由度的過(guò)程。剛接觸雷達(dá)外參標(biāo)定的朋友容易把標(biāo)定想得太簡(jiǎn)單以為隨便量一下安裝距離就能填一個(gè)平移量進(jìn)去。實(shí)際上安裝誤差在旋轉(zhuǎn)量上的影響會(huì)被距離放大比如yaw角差1度在10米外就會(huì)產(chǎn)生大約17厘米的橫向偏差。對(duì)于點(diǎn)云融合來(lái)說(shuō)這個(gè)誤差直接體現(xiàn)為同一物體出現(xiàn)重影。2.2 標(biāo)定方法選型手眼標(biāo)定法 vs 特征點(diǎn)對(duì)齊法主流的雷達(dá)外參標(biāo)定方法有兩類。一類是基于手眼標(biāo)定思想的方法利用雷達(dá)在環(huán)境中移動(dòng)時(shí)同一平面特征在不同幀間的幾何約束來(lái)求解外參代表性工具是livox_calibration。另一類是基于特征點(diǎn)對(duì)齊的方法讓兩臺(tái)雷達(dá)同時(shí)觀測(cè)同一組標(biāo)定特征比如標(biāo)定板的角點(diǎn)、墻角線然后通過(guò)點(diǎn)云配準(zhǔn)求解變換關(guān)系。livox_calibration是Livox官方提供的標(biāo)定工具支持Mid-360流程是先錄制一段環(huán)境數(shù)據(jù)然后在工具中手動(dòng)選取標(biāo)定板的平面點(diǎn)云工具會(huì)自動(dòng)擬合平面并求解外參。這個(gè)方法精度很高但操作流程比較繁瑣需要專門的標(biāo)定板而且對(duì)環(huán)境有要求。對(duì)于雙雷達(dá)安裝在工作站上、短時(shí)間內(nèi)不會(huì)移動(dòng)的場(chǎng)景我這次采用了一個(gè)更直接的替代方案靜態(tài)標(biāo)定法。具體做法是找一個(gè)特征豐富的墻角區(qū)域讓兩臺(tái)雷達(dá)同時(shí)掃描然后手動(dòng)選取對(duì)應(yīng)特征點(diǎn)通過(guò)多點(diǎn)對(duì)位求解變換矩陣。實(shí)際操作時(shí)我在環(huán)境里放置了三個(gè)標(biāo)定板分別位于不同高度和角度保證兩臺(tái)雷達(dá)都能同時(shí)看到盡可能多的公共特征。然后錄制一段包含多幀的bag包在離線狀態(tài)下提取兩個(gè)坐標(biāo)系下的對(duì)應(yīng)點(diǎn)用Umeyama算法求解最優(yōu)變換。這個(gè)方法的精度相比官方工具會(huì)低一些但對(duì)于大多數(shù)避障和導(dǎo)航場(chǎng)景已經(jīng)足夠。2.3 靜態(tài)標(biāo)定的完整流程第一步準(zhǔn)備標(biāo)志物。我用的是兩塊1.2m x 0.6m的平面板表面貼了黑白棋盤(pán)格紙方便在點(diǎn)云中識(shí)別。放置時(shí)讓兩塊板呈90度夾角這樣在點(diǎn)云中會(huì)形成明顯的棱線角點(diǎn)位置容易提取。第二步錄制數(shù)據(jù)。rostopic echo確認(rèn)兩臺(tái)雷達(dá)都在正常發(fā)布點(diǎn)云后執(zhí)行ros2 bag record同時(shí)錄制兩個(gè)點(diǎn)云話題錄制時(shí)間30秒左右。錄制時(shí)人盡量遠(yuǎn)離標(biāo)定板避免人體點(diǎn)云干擾平面擬合。第三步提取對(duì)應(yīng)點(diǎn)。把bag包播放出來(lái)在rviz2中同時(shí)顯示兩個(gè)點(diǎn)云暫停在某一幀用PCL的選點(diǎn)工具分別在前雷達(dá)坐標(biāo)系和后雷達(dá)坐標(biāo)系下選取同一物理點(diǎn)的坐標(biāo)。至少選取6組對(duì)應(yīng)點(diǎn)覆蓋空間不同位置。第四步計(jì)算外參。用PCL中的pcl::umeyama或直接寫(xiě)一個(gè)SVD分解求解剛體變換。核心代碼如下#include pcl/common/transforms.h #include pcl/common/transformation_from_correspondences.h pcl::TransformationFromCorrespondences tfc; for (size_t i 0; i correspondences.size(); i) { tfc.add(correspondences[i].first, correspondences[i].second); } Eigen::Matrix4f transform tfc.getTransformation();第五步驗(yàn)證。把外參寫(xiě)進(jìn)TF或直接用于點(diǎn)云變換在rviz2中檢查兩個(gè)點(diǎn)云的重合程度。正常情況下近距離的物體應(yīng)該幾乎完全重合遠(yuǎn)距離處允許有少量偏差。2.4 標(biāo)定結(jié)果驗(yàn)證與精度評(píng)估標(biāo)定做完不能直接信要驗(yàn)證。我的驗(yàn)證方法分為兩步。第一步把后雷達(dá)點(diǎn)云通過(guò)標(biāo)定的外參變換到前雷達(dá)坐標(biāo)系下在rviz2中目視檢查重合度。第二步計(jì)算兩個(gè)點(diǎn)云在公共區(qū)域的最近點(diǎn)距離平均值這個(gè)值小于5cm基本可以接受小于2cm說(shuō)明標(biāo)定質(zhì)量很高。實(shí)測(cè)中我發(fā)現(xiàn)靜態(tài)標(biāo)定法最大的誤差來(lái)源是手動(dòng)選點(diǎn)的精度。點(diǎn)云中選點(diǎn)時(shí)有幾個(gè)像素的偏差在遠(yuǎn)處就會(huì)被放大。所以選點(diǎn)時(shí)盡量選在棱線或角點(diǎn)上不要選在平面上否則垂直于平面的方向上會(huì)有較大的不確定性。另一點(diǎn)要注意的是時(shí)間同步誤差會(huì)體現(xiàn)為“假外參誤差”。如果兩臺(tái)雷達(dá)的時(shí)間戳不一致雷達(dá)旋轉(zhuǎn)到不同位置時(shí)點(diǎn)云疊加在一起看起來(lái)就像外參沒(méi)有標(biāo)定好。所以標(biāo)定前先確認(rèn)時(shí)間同步狀態(tài)這一點(diǎn)在下一節(jié)詳細(xì)說(shuō)。3. 雙雷達(dá)時(shí)間同步與驅(qū)動(dòng)配置細(xì)節(jié)3.1 時(shí)間同步為什么是融合的隱形前提點(diǎn)云融合不光是空間上的變換時(shí)間上也要對(duì)齊。如果兩臺(tái)雷達(dá)同一時(shí)刻采集到的點(diǎn)云實(shí)際上相差了100ms對(duì)于運(yùn)動(dòng)中的物體來(lái)說(shuō)即使外參完全正確融合結(jié)果也會(huì)出現(xiàn)明顯的錯(cuò)位。Mid-360的輸出幀率是10Hz即每幀間隔100ms。如果時(shí)間不同步最壞情況下兩幀點(diǎn)云對(duì)應(yīng)的時(shí)間差就是100ms對(duì)于速度1m/s的移動(dòng)物體來(lái)說(shuō)會(huì)產(chǎn)生10cm的位置誤差。有人可能會(huì)說(shuō)那我做融合時(shí)用最近時(shí)間戳匹配不就行了。問(wèn)題的關(guān)鍵在于Mid-360發(fā)布的一幀點(diǎn)云并不是瞬時(shí)采集的而是在一個(gè)掃描周期內(nèi)逐步累積的。如果你只是在話題層面把兩幀點(diǎn)云做時(shí)間對(duì)齊依然無(wú)法消除雷達(dá)本身掃描過(guò)程中的運(yùn)動(dòng)畸變。不過(guò)這是另一個(gè)層面的事實(shí)際工程中對(duì)于低速機(jī)器人話題時(shí)間戳對(duì)齊已經(jīng)能滿足大多數(shù)避障需求。3.2 PTP時(shí)間同步方案ROS2本身就支持分布式系統(tǒng)的時(shí)間同步但默認(rèn)配置下各個(gè)節(jié)點(diǎn)各用各的system clock時(shí)間偏差可能達(dá)到毫秒甚至幾十毫秒級(jí)別。對(duì)雙雷達(dá)融合來(lái)說(shuō)最可靠的方案是讓兩臺(tái)雷達(dá)所在的主機(jī)系統(tǒng)時(shí)間與一個(gè)統(tǒng)一的時(shí)鐘源同步雷達(dá)的時(shí)間戳在上層驅(qū)動(dòng)中默認(rèn)使用系統(tǒng)時(shí)鐘系統(tǒng)時(shí)間同步了點(diǎn)云時(shí)間戳自然同步。我在現(xiàn)場(chǎng)用的方案是PTPIEEE 802.1AS通過(guò)網(wǎng)口實(shí)現(xiàn)亞毫秒級(jí)時(shí)間同步。具體做法是讓其中一臺(tái)雷達(dá)或工控機(jī)作為PTP主時(shí)鐘另一臺(tái)作為從時(shí)鐘。livox_ros_driver2本身不直接處理PTP它只是讀取系統(tǒng)時(shí)間戳所以底層時(shí)間同步需要依賴linuxptp工具。安裝和啟動(dòng)步驟如下sudo apt install linuxptp sudo ptpd -i eth0 -m -u不過(guò)實(shí)際使用中發(fā)現(xiàn)如果兩臺(tái)雷達(dá)接在同一個(gè)工控機(jī)上最簡(jiǎn)單可靠的方式反而是直接用硬件時(shí)間戳。Mid-360的驅(qū)動(dòng)支持獲取雷達(dá)自身的GPS同步信號(hào)PPS但需要額外接線對(duì)于大多數(shù)室內(nèi)機(jī)器人來(lái)說(shuō)性價(jià)比不高。3.3 雙雷達(dá)網(wǎng)絡(luò)規(guī)劃與驅(qū)動(dòng)參數(shù)調(diào)整雙雷達(dá)接入工控機(jī)第一個(gè)要解決的是IP沖突。Mid-360默認(rèn)IP是192.168.1.50兩臺(tái)雷達(dá)如果都保持默認(rèn)會(huì)導(dǎo)致IP沖突驅(qū)動(dòng)無(wú)法正常連接。我的做法是先把第一臺(tái)雷達(dá)的IP改為192.168.1.51第二臺(tái)保持192.168.1.50不動(dòng)工控機(jī)側(cè)兩個(gè)網(wǎng)口分別配置192.168.1.100和192.168.1.101。這樣每臺(tái)雷達(dá)獨(dú)享一個(gè)網(wǎng)口互不干擾。livox_ros_driver2的配置文件在config/config.h中關(guān)鍵參數(shù)如下lidar_type1表示Mid-360ip雷達(dá)IP地址pcl_data_type0表示輸出原始點(diǎn)云PointXYZRTL1表示轉(zhuǎn)換后的PointXYZframe_id點(diǎn)云話題的坐標(biāo)系名稱兩臺(tái)雷達(dá)分別設(shè)置為livox_front和livox_backuser_config_path雷達(dá)內(nèi)部參數(shù)文件路徑一般保持默認(rèn)啟動(dòng)時(shí)我用兩個(gè)獨(dú)立的launch文件分別啟動(dòng)兩個(gè)驅(qū)動(dòng)節(jié)點(diǎn)中間通過(guò)namespace隔離。但這里要提醒一個(gè)問(wèn)題如果兩臺(tái)雷達(dá)共用一個(gè)launch文件topic名稱必須以雷達(dá)IP或別名區(qū)分否則后啟動(dòng)的節(jié)點(diǎn)會(huì)覆蓋先啟動(dòng)的topic。我的launch文件結(jié)構(gòu)是這樣的給每臺(tái)雷達(dá)單獨(dú)起一個(gè)節(jié)點(diǎn)名并用frame_id區(qū)分坐標(biāo)系。這樣rviz2里直接就能看到兩個(gè)坐標(biāo)系下的點(diǎn)云方便直觀檢查。4. C/PCL點(diǎn)云融合節(jié)點(diǎn)實(shí)現(xiàn)與優(yōu)化4.1 節(jié)點(diǎn)架構(gòu)與數(shù)據(jù)流設(shè)計(jì)融合節(jié)點(diǎn)是整個(gè)系統(tǒng)最核心的部分它做的事情可以用一句話概括訂閱兩臺(tái)雷達(dá)的點(diǎn)云話題將后雷達(dá)點(diǎn)云經(jīng)過(guò)外參變換到前雷達(dá)坐標(biāo)系后與前雷達(dá)點(diǎn)云做基于體素濾波的去重合并最終發(fā)布一幀全局點(diǎn)云。數(shù)據(jù)流設(shè)計(jì)上我采用了單節(jié)點(diǎn)方案也就是一個(gè)節(jié)點(diǎn)同時(shí)訂閱兩個(gè)話題在回調(diào)函數(shù)里做同步和融合。這樣做的好處是避免了多節(jié)點(diǎn)間的序列化傳輸開(kāi)銷減少一次額外的數(shù)據(jù)拷貝。代價(jià)是節(jié)點(diǎn)內(nèi)部需要自己處理兩個(gè)話題的時(shí)間同步?jīng)]有像message_filters那樣現(xiàn)成的同步器好用。由于Mid-360的兩個(gè)點(diǎn)云話題都是sensor_msgs::msg::PointCloud2我在回調(diào)里先做時(shí)間對(duì)齊再統(tǒng)一轉(zhuǎn)換為PCL點(diǎn)云處理。這里有一個(gè)經(jīng)驗(yàn)不要每次都從PointCloud2轉(zhuǎn)一次PCL點(diǎn)云會(huì)有不小的時(shí)間開(kāi)銷。更好的做法是只在接收到新數(shù)據(jù)時(shí)做一次轉(zhuǎn)換然后緩存轉(zhuǎn)換結(jié)果。4.2 PCL類型轉(zhuǎn)換與環(huán)境搭建PCLPoint Cloud Library是點(diǎn)云處理的老牌庫(kù)在ROS2 Humble下可以直接通過(guò)apt安裝sudo apt install libpcl-dev ros-humble-pcl-conversions ros-humble-pcl-ros需要的依賴在CMakeLists.txt中這樣聲明find_package(PCL REQUIRED COMPONENTS common io filters registration) find_package(pcl_conversions REQUIRED)Mid-360驅(qū)動(dòng)發(fā)布的原始點(diǎn)云類型是livox_interfaces::msg::CustomPoint包含xyz、reflectivity、tag等字段。在驅(qū)動(dòng)配置中把pcl_data_type設(shè)為1后驅(qū)動(dòng)會(huì)自動(dòng)把點(diǎn)云轉(zhuǎn)換為sensor_msgs::msg::PointCloud2每個(gè)點(diǎn)只包含xyz信息。這樣在融合節(jié)點(diǎn)中就可以直接用pcl::PointCloudpcl::PointXYZ處理減少內(nèi)存開(kāi)銷。實(shí)際編寫(xiě)代碼時(shí)發(fā)現(xiàn)現(xiàn)場(chǎng)總線上的PointCloud2消息如果字段太多轉(zhuǎn)換到PCL點(diǎn)云時(shí)會(huì)因?yàn)閒ield name不匹配報(bào)錯(cuò)。穩(wěn)妥的做法是先用pcl::fromROSMsg轉(zhuǎn)到pcl::PointCloudpcl::PointXYZ如果源點(diǎn)云帶強(qiáng)度或者時(shí)間字段別直接用XYZ類型接收。這一點(diǎn)在Mid-360驅(qū)動(dòng)使用默認(rèn)配置時(shí)很容易踩中。4.3 核心融合算法體素濾波去重雙雷達(dá)點(diǎn)云融合的難點(diǎn)在于重疊區(qū)域的處理。兩臺(tái)雷達(dá)都有360度水平視場(chǎng)角前后安裝時(shí)中間有很大一片區(qū)域是重疊的。如果直接把兩幀點(diǎn)云疊加發(fā)布出去重疊區(qū)域會(huì)出現(xiàn)密集的重復(fù)點(diǎn)導(dǎo)致后續(xù)配準(zhǔn)和聚類算法的計(jì)算量翻倍、精度下降。解決方案是做體素濾波去重。思路是先用一個(gè)較大的體素柵格比如5cm對(duì)兩幀點(diǎn)云分別進(jìn)行下采樣再把變換后的后雷達(dá)點(diǎn)云疊加到前雷達(dá)點(diǎn)云上然后用一個(gè)體素大小為3cm的柵格對(duì)合并后的點(diǎn)云進(jìn)行去重。去重的原理很簡(jiǎn)單落在同一個(gè)體素內(nèi)的多個(gè)點(diǎn)只保留一個(gè)這樣既能消除重復(fù)點(diǎn)又不會(huì)對(duì)點(diǎn)云原始結(jié)構(gòu)造成太大破壞。這里有個(gè)細(xì)節(jié)值得展開(kāi)。直接對(duì)合并點(diǎn)云做體素濾波確實(shí)能去重但也會(huì)讓原本在非重疊區(qū)域一點(diǎn)都不重復(fù)的高精度點(diǎn)云變得稀疏。我試過(guò)兩種做法。第一種是“先合并再濾波”實(shí)現(xiàn)簡(jiǎn)單效果尚可適合重疊區(qū)域占比不高的場(chǎng)景。第二種是“先區(qū)分重疊區(qū)和非重疊區(qū)只對(duì)重疊區(qū)域做去重”效果更好但需要先計(jì)算點(diǎn)云間的重疊關(guān)系代碼復(fù)雜度上升不少。對(duì)于雙Mid-360這種重疊區(qū)域比較大的場(chǎng)景我最終用的是改進(jìn)后的方案先分別下采樣再合并再做一次輕量級(jí)去重。關(guān)鍵代碼如下pcl::VoxelGridpcl::PointXYZ voxel; voxel.setLeafSize(0.05f, 0.05f, 0.05f); voxel.setInputCloud(front_cloud_ptr); voxel.filter(*front_downsampled); voxel.setInputCloud(back_transformed_ptr); voxel.filter(*back_downsampled); *front_downsampled *back_downsampled; pcl::VoxelGridpcl::PointXYZ dedup; dedup.setLeafSize(0.03f, 0.03f, 0.03f); dedup.setInputCloud(front_downsampled); dedup.filter(*fused_cloud);4.4 坐標(biāo)變換與TF集成融合前必須把后雷達(dá)的點(diǎn)云坐標(biāo)變換到前雷達(dá)坐標(biāo)系下這一步通過(guò)PCL的pcl::transformPointCloud配合外參矩陣完成pcl::transformPointCloud(*back_cloud_ptr, *back_transformed_ptr, extrinsic_matrix);這里的extrinsic_matrix就是標(biāo)定得到的4x4變換矩陣也可以通過(guò)TF動(dòng)態(tài)獲取。在實(shí)際工程中我更推薦將外參發(fā)布為靜態(tài)TF變換然后讓融合節(jié)點(diǎn)通過(guò)TF樹(shù)自動(dòng)獲取變換矩陣。這樣不僅融合節(jié)點(diǎn)能用其他節(jié)點(diǎn)比如可視化、導(dǎo)航也能直接復(fù)用這個(gè)外參不會(huì)出現(xiàn)不同節(jié)點(diǎn)各用各的參數(shù)導(dǎo)致的不一致問(wèn)題。發(fā)布靜態(tài)TF的命令如下ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id需要注意的是這里的坐標(biāo)順序是xyz加rpy不是四元數(shù)命令行用起來(lái)更方便。我在實(shí)際項(xiàng)目中把后雷達(dá)的坐標(biāo)系設(shè)置成livox_back前雷達(dá)坐標(biāo)系設(shè)置成livox_front靜態(tài)TF發(fā)布的parent是livox_frontchild是livox_back。融合節(jié)點(diǎn)從TF樹(shù)中查詢這兩個(gè)坐標(biāo)系之間的變換不用自己管理標(biāo)定參數(shù)文件。4.5 體素濾波參數(shù)選擇與性能調(diào)優(yōu)體素濾波的葉子大小對(duì)融合效果影響很大。葉子設(shè)太小比如1cm去重效果不明顯重疊區(qū)域依然有大量重復(fù)點(diǎn)葉子設(shè)太大比如10cm非重疊區(qū)域的點(diǎn)云也會(huì)被過(guò)度稀釋丟失細(xì)節(jié)。我的建議是分成兩級(jí)預(yù)處理階段用5cm做下采樣降低原始點(diǎn)云密度為后續(xù)處理減負(fù)融合階段用3cm做去重既能在重疊區(qū)域有效去重又能保證近距離物體的輪廓細(xì)節(jié)不丟失。實(shí)際幀率方面雙雷達(dá)原始點(diǎn)云各在10Hz下約2-3萬(wàn)點(diǎn)每幀合并后大約4-5萬(wàn)點(diǎn)。整套融合流程在i7-1265U工控機(jī)上單幀處理時(shí)間約8-12ms完全能滿足10Hz的實(shí)時(shí)性要求。如果你的計(jì)算平臺(tái)性能較弱可以適當(dāng)調(diào)大預(yù)處理階段的下采樣葉子大小把點(diǎn)云降到2萬(wàn)點(diǎn)以內(nèi)再融合幀率還能進(jìn)一步提高。除了CPU開(kāi)銷內(nèi)存也要注意。PCL點(diǎn)云對(duì)象在循環(huán)中使用時(shí)如果不注意清理內(nèi)存占用會(huì)不斷增長(zhǎng)。我用的是固定大小的成員變量云對(duì)象在回調(diào)中先clear再操作避免頻繁分配與釋放。5. 常見(jiàn)問(wèn)題與排查技巧實(shí)錄5.1 驅(qū)動(dòng)連不上雷達(dá)IP與防火墻先說(shuō)一個(gè)最常見(jiàn)的問(wèn)題驅(qū)動(dòng)節(jié)點(diǎn)啟動(dòng)后一直報(bào)錯(cuò)無(wú)法連接雷達(dá)。排查步驟我總結(jié)成一個(gè)固定流程。第一ping雷達(dá)IP確認(rèn)網(wǎng)絡(luò)通不通。第二確認(rèn)工控機(jī)網(wǎng)口IP是否和雷達(dá)IP在同一網(wǎng)段。第三確認(rèn)防火墻是否放行了UDP端口。第四檢查網(wǎng)線是否直連中間如果經(jīng)過(guò)交換機(jī)需要確認(rèn)交換機(jī)沒(méi)有開(kāi)啟VLAN隔離。Mid-360默認(rèn)數(shù)據(jù)端口是7500通過(guò)另一個(gè)端口發(fā)送設(shè)備信息。livox_ros_driver2會(huì)自動(dòng)讀取radar配置中的端口號(hào)一般不需要手動(dòng)修改。但如果在同一臺(tái)機(jī)器上同時(shí)開(kāi)了多個(gè)驅(qū)動(dòng)節(jié)點(diǎn)可能出現(xiàn)端口占用。解決方法是給第二臺(tái)雷達(dá)分配獨(dú)立的端口號(hào)。5.2 點(diǎn)云有空洞或撕裂網(wǎng)卡丟包點(diǎn)云出現(xiàn)橫向撕裂狀空洞最常見(jiàn)的原因是UDP數(shù)據(jù)包丟失。這個(gè)現(xiàn)象在開(kāi)啟網(wǎng)卡流控或系統(tǒng)負(fù)載較高時(shí)尤為明顯。排查方法是用ethtool -S查看網(wǎng)卡丟包計(jì)數(shù)如果rx_dropped持續(xù)增長(zhǎng)說(shuō)明丟包確實(shí)發(fā)生了。解決方法有幾個(gè)優(yōu)先級(jí)排序。第一優(yōu)先級(jí)把兩臺(tái)雷達(dá)分別接在兩個(gè)網(wǎng)口上不要共享一個(gè)網(wǎng)口。第二優(yōu)先級(jí)禁用網(wǎng)卡的TCP/IP卸載功能因?yàn)橛行┚W(wǎng)卡在卸載TCP校驗(yàn)和時(shí)會(huì)丟UDP包。第三優(yōu)先級(jí)在驅(qū)動(dòng)配置中降低發(fā)送頻率Mid-360支持通過(guò)配置降低輸出幀率減少瞬時(shí)帶寬占用。5.3 點(diǎn)云時(shí)間戳跳變或明顯不同步如果兩臺(tái)雷達(dá)的時(shí)間戳差值超過(guò)50ms融合后的點(diǎn)云在機(jī)器人轉(zhuǎn)彎時(shí)會(huì)看到明顯的前后錯(cuò)位。排查步驟是先打印兩幀點(diǎn)云的時(shí)間戳差值確定時(shí)間偏差的量級(jí)。偏差在幾十毫秒以內(nèi)大概率是系統(tǒng)時(shí)間同步不夠精確偏差在秒級(jí)以上很可能是兩個(gè)驅(qū)動(dòng)節(jié)點(diǎn)使用的時(shí)鐘源不同。終極解決方案是給工控機(jī)配置PTP時(shí)間同步把系統(tǒng)時(shí)間誤差控制在微秒級(jí)。如果現(xiàn)場(chǎng)沒(méi)有PTP條件另一個(gè)可行的辦法是在融合節(jié)點(diǎn)里做時(shí)間對(duì)齊即緩存最近若干幀的點(diǎn)云等到兩臺(tái)雷達(dá)都發(fā)布了相近時(shí)間戳的數(shù)據(jù)后才開(kāi)始融合。這樣會(huì)引入100ms左右的延遲但對(duì)于低速機(jī)器人來(lái)說(shuō)可以接受。5.4 rviz2中看不到點(diǎn)云或坐標(biāo)系不對(duì)這個(gè)問(wèn)題多半不是雷達(dá)的問(wèn)題而是tf關(guān)系沒(méi)配好。rviz2中需要把Fixed Frame設(shè)置成livox_front然后由靜態(tài)TF提供livox_back到livox_front的變換。如果TF樹(shù)中缺少這個(gè)關(guān)系rviz2會(huì)給出紅色提示但不會(huì)自動(dòng)報(bào)錯(cuò)。另外檢查一下點(diǎn)云話題是否已經(jīng)正確發(fā)布。使用ros2 topic hz /livox/front/cloud確認(rèn)話題頻率正常。如果驅(qū)動(dòng)節(jié)點(diǎn)已經(jīng)啟動(dòng)但話題頻率為0大概率是雷達(dá)沒(méi)有真正開(kāi)始掃描檢查雷達(dá)狀態(tài)燈是否正常。5.5 融合后點(diǎn)云密度分布不均融合后的點(diǎn)云看起來(lái)“疏密不一”這個(gè)現(xiàn)象通常由兩個(gè)原因?qū)е?。一是重疊區(qū)域沒(méi)有正確去重導(dǎo)致局部點(diǎn)云過(guò)密二是兩臺(tái)雷達(dá)的距離不同近距離處點(diǎn)云天然比遠(yuǎn)距離密。對(duì)于前者按上述方案調(diào)整體素濾波參數(shù)即可對(duì)于后者可以加一步距離自適應(yīng)濾波對(duì)近處點(diǎn)云做更激進(jìn)的下采樣。這個(gè)問(wèn)題的另一個(gè)隱藏來(lái)源是外參標(biāo)定誤差較大時(shí)會(huì)出現(xiàn)的“點(diǎn)云模糊”現(xiàn)象。如果兩塊區(qū)域點(diǎn)云沒(méi)有完全重合看起來(lái)就像點(diǎn)云密度降低或者物體邊緣有雙重輪廓。先重新檢查外參而不是盲目調(diào)整濾波參數(shù)。6. 實(shí)際效果評(píng)估與后續(xù)擴(kuò)展這套雙雷達(dá)融合方案測(cè)試下來(lái)前后向盲區(qū)比單雷達(dá)減少了大約70%在10米范圍內(nèi)行人和矮小障礙物的點(diǎn)云完整度明顯提升。重疊區(qū)域的點(diǎn)云經(jīng)過(guò)體素去重后密度被控制在合理的范圍沒(méi)有出現(xiàn)明顯的計(jì)算量暴增。在后續(xù)的導(dǎo)航測(cè)試中局部代價(jià)地圖的更新頻率從單雷達(dá)時(shí)的5Hz提升到了8Hz因?yàn)辄c(diǎn)云覆蓋率提升后障礙物檢測(cè)算法不需要反復(fù)等待新的觀測(cè)數(shù)據(jù)來(lái)確認(rèn)障礙物是否存在。如果想進(jìn)一步優(yōu)化可以考慮在融合前增加點(diǎn)云運(yùn)動(dòng)畸變補(bǔ)償也就是利用IMU做去畸變。Mid-360自帶IMUlivox_ros_driver2也提供了IMU數(shù)據(jù)話題可以直接做去畸變處理。代價(jià)是計(jì)算量增加約20%但點(diǎn)云邊緣會(huì)變得更銳利對(duì)于高速運(yùn)動(dòng)場(chǎng)景幫助很大。另外一個(gè)擴(kuò)展方向是多雷達(dá)拼接將三臺(tái)以上Mid-360通過(guò)組合安裝實(shí)現(xiàn)無(wú)死角覆蓋。融合邏輯與雙雷達(dá)版本完全一致只是外參矩陣的數(shù)量和TF樹(shù)結(jié)構(gòu)會(huì)復(fù)雜一些需要額外注意命名規(guī)范和參數(shù)管理。在實(shí)際部署中還有一點(diǎn)值得強(qiáng)調(diào)標(biāo)定參數(shù)不要只存在代碼里。我建議把標(biāo)定結(jié)果寫(xiě)成一個(gè)YAML參數(shù)文件由融合節(jié)點(diǎn)啟動(dòng)時(shí)加載。同時(shí)在內(nèi)參、外參變化時(shí)運(yùn)行一個(gè)校驗(yàn)?zāi)_本自動(dòng)對(duì)比新舊外參對(duì)同一幀點(diǎn)云變換后的重合度變化超過(guò)閾值就告警。這個(gè)習(xí)慣在我維護(hù)多臺(tái)設(shè)備時(shí)幫了大忙之前因?yàn)樵O(shè)備運(yùn)輸振動(dòng)導(dǎo)致外參漂移沒(méi)及時(shí)發(fā)現(xiàn)結(jié)果排查了一整天問(wèn)題后來(lái)加了自動(dòng)校驗(yàn)才算徹底解決。雙雷達(dá)融合這個(gè)需求會(huì)越來(lái)越多尤其是中大型機(jī)器人逐漸普及之后。Mid-360因?yàn)樾詢r(jià)比高、垂直FOV大很適合做這類方案的感知核心。希望這篇實(shí)戰(zhàn)記錄能幫你少走幾個(gè)彎路。如果你選擇了不同的安裝構(gòu)型標(biāo)定和融合的思路依然可以復(fù)用重點(diǎn)是先把坐標(biāo)系關(guān)系和去重邏輯理清楚其余的只是工程細(xì)節(jié)的優(yōu)化。