智能換電站電池包鎖止機(jī)構(gòu)位姿視覺估計

王立輝， 蘇余足威， 韓華春， 陳良亮， 張 浩

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，江蘇 南京 210096；2. 國網(wǎng)江蘇省電力有限公司，江蘇 南京210024；3. 國電南瑞科技股份有限公司，江蘇 南京211100）

1 引 言

電動汽車具有節(jié)能環(huán)保、能量轉(zhuǎn)換效率高等優(yōu)點，在我國得到了大力推廣。更換電池包作為一種補(bǔ)電方式，可讓用戶在較短的時間內(nèi)充滿電能，彌補(bǔ)了充電方式補(bǔ)電緩慢的缺點，具有更好的用戶體驗。在電動汽車換電過程中，引導(dǎo)換電機(jī)構(gòu)與電池包精準(zhǔn)對接是高效快速換電的關(guān)鍵。該過程可通過機(jī)械浮動對接裝置或3D 視覺定位系統(tǒng)實現(xiàn)。而相對于機(jī)械式定位，3D 視覺定位更具兼容性與靈活性，因此是目前換電站技術(shù)的研發(fā)重點。

根據(jù)定位目標(biāo)的不同，3D 視覺定位方法可以大致分為標(biāo)識物定位與無標(biāo)識物定位［1］。標(biāo)識物定位提前在物體上設(shè)置定位標(biāo)識，使用相機(jī)對標(biāo)識物進(jìn)行定位，從而推算出物體的坐標(biāo)與姿態(tài)。楊寧等［2］以航天器本體和星箭對接環(huán)作為識別特征，在不依靠額外測量信息的情況下，利用視覺系統(tǒng)求解特征頂點的約束關(guān)系，從而計算出目標(biāo)物的相對姿態(tài)。李瑩等［3］提出了一種用于火星探測的人工標(biāo)識定位方法，通過邊緣檢測、橢圓擬合等手段識別定位著陸器，并對標(biāo)志中心進(jìn)行直線交點檢測獲得精確定位結(jié)果。盡管標(biāo)識物定位相對步驟較少，精度相對較高，但并不是所有情況下都能提前安裝標(biāo)識，灰塵、污漬等環(huán)境因素也會干擾標(biāo)識的定位。無標(biāo)識物定位則直接識別物體的特征，以物體特征或特征匹配的方式進(jìn)行定位，相對于換電場景具有更強(qiáng)的靈活性。Drost 等［4］提出了一種基于點對特征（Point Pair Feature， PPF）投票的6D 位姿估計方法，可基于模板的全局PPF 哈希表估計場景點云中物體的位姿，該方法可適應(yīng)多種場景下的位姿估計，但是計算量過大。 Hutchison 等［5］提出linemod 算法，采用模板匹配的方法計算3D 物體RGB-D 圖像的彩色梯度與深度圖梯度，與模板進(jìn)行比對得到估計的位姿，可在較短時間內(nèi)得到位姿估計結(jié)果，但由于存在多個匹配結(jié)果，需要對結(jié)果進(jìn)行篩選。Verikas［6］通過將構(gòu)件的CAD模型與獲取的場景點云數(shù)據(jù)進(jìn)行特征描述子匹配，再通過點云配準(zhǔn)實現(xiàn)對構(gòu)件位姿的識別和估計，但是由于缺乏全局信息，特征描述子容易誤匹配，影響位姿估計結(jié)果。

針對電動車換電對接中的鎖止機(jī)構(gòu)視覺定位問題，本文使用3D 相機(jī)拍攝電池包對接部位的鎖止裝置，獲得包含鎖止裝置的RGB 圖與點云，并針對鎖止裝置的位姿估計問題提出了基于點云分割的6D 位姿估計方法，結(jié)合深度學(xué)習(xí)點云分割網(wǎng)絡(luò)的全局感知能力與點云特征描述子的局部描述能力，實現(xiàn)更為精準(zhǔn)、穩(wěn)性的點云配準(zhǔn)，從而獲得滿足電動車換電要求的鎖止機(jī)構(gòu)位姿估計結(jié)果。

2 原 理

電動車換電示意圖如圖1 所示。在換電站中，換電平臺的作業(yè)空間處于車輛停靠區(qū)域的正下方，換電機(jī)器人置于該空間底部的柔性作業(yè)平臺上，通過導(dǎo)軌沿相應(yīng)的軌跡移動。電池包通過鎖止機(jī)構(gòu)固定在車輛底盤之上，換電機(jī)器人則經(jīng)由電池包定位信息的引導(dǎo)，利用搭載的加解鎖裝置對電池包進(jìn)行對接解鎖并取下電池包。

圖1 電動車換電示意圖Fig.1 Schematic diagram of electric vehicle battery swapping

在換電過程中，電池包定位信息來源于電池包鎖止機(jī)構(gòu)的定位結(jié)果。對于電池包鎖止機(jī)構(gòu)的定位，有機(jī)械式定位與視覺定位兩種方法。機(jī)械式定位方法根據(jù)已知的車輛尺寸、車輛?？课恢?、電池包尺寸和換電站結(jié)構(gòu)尺寸推算出電池包鎖止機(jī)構(gòu)的空間位置，通過加解鎖裝置的柔性機(jī)構(gòu)彌補(bǔ)定位估計的誤差。視覺定位方法則通過3D 視覺傳感器，如雙目相機(jī)、RGBD 相機(jī)、激光掃描儀等，拍攝車輛底部電池包的圖像，通過相應(yīng)算法處理得到鎖止機(jī)構(gòu)的具體空間位姿。

相較于機(jī)械式定位方法，視覺定位方法更加精確，可獲得旋轉(zhuǎn)角度信息，并能兼容更多的車輛型號與電池包型號，同時能更好地應(yīng)對環(huán)境干擾與意外情況，因此選用3D 視覺定位方式。在3D 視覺定位方案中，視覺傳感器選用高精度雙目結(jié)構(gòu)光相機(jī)，可獲得RGBD 圖像。相機(jī)置于換電平臺底部0.5 m 處的固定位置，圖1 中換電機(jī)器人下方，設(shè)置方式為眼到手（Eye to hand）型，相機(jī)使用前要經(jīng)過標(biāo)定，獲得彩色攝像頭的內(nèi)參參數(shù)K={fx，fy，cx，cy}、彩色圖片與深度圖片的對齊關(guān)系f以及相機(jī)相對于換電站工作坐標(biāo)系的外參參數(shù)，即視覺傳感器坐標(biāo)系相對換電坐標(biāo)系的相對位姿Tc。視覺傳感器拍攝上方的汽車底盤電池包，獲得包括車底盤電池包、部分換電站結(jié)構(gòu)以及鎖止機(jī)構(gòu)的RGBD 圖像，其中鎖止機(jī)構(gòu)為位姿估計的目標(biāo)。使用K將深度圖投影為點云，即：

其中:x，y，z為點云三維坐標(biāo)，d為像素的深度值，u，v為圖像像素坐標(biāo)。

對點云使用位姿估計算法繼續(xù)處理，得到鎖止機(jī)構(gòu)相對于相機(jī)的位姿Tr。最后經(jīng)過相機(jī)外參的轉(zhuǎn)換，計算鎖止機(jī)構(gòu)在換電站空間中的絕對位姿：

To用于引導(dǎo)換電機(jī)器人與電池包的對接。

但使用RGBD 相機(jī)拍攝生成的點云不可避免會有噪聲，其值約為±1～3 mm；車輛經(jīng)過較長時間的行駛后，其電池包鎖止機(jī)構(gòu)也可能會不均勻地黏附泥濘和污漬，產(chǎn)生新的表面形貌噪聲，其值約為±2～4 mm。而鎖止機(jī)構(gòu)尺寸較小，約為40 mm×40 mm×30 mm，上述噪聲會對位姿估計產(chǎn)生較大的干擾，導(dǎo)致位姿估計算法的準(zhǔn)確度下降，甚至可能得出錯誤的結(jié)果。為了解決上述問題，本文提出一種穩(wěn)定的位姿估計方法。

3 基于點云分割的6D 位姿估計算法

針對換電視覺定位穩(wěn)定性的需求，提出基于點云分割的鎖止機(jī)構(gòu)6D 位姿估計方法，通過引入點云分割神經(jīng)網(wǎng)絡(luò)解決點云配準(zhǔn)算法中源點云與目標(biāo)點云之間對應(yīng)關(guān)系難以估計的問題。該鎖止機(jī)構(gòu)6D 位姿估計方法由鎖止機(jī)構(gòu)目標(biāo)檢測，鎖止機(jī)構(gòu)點云預(yù)處理，鎖止機(jī)構(gòu)點云部分分割，鎖止機(jī)構(gòu)源點云與目標(biāo)模型點云配準(zhǔn)四部分組成。

3.1 RGB 圖像目標(biāo)檢測

拍攝得到的底部電池包圖像包含車底盤、換電站背景等信息，其中多余信息會嚴(yán)重干擾算法的處理結(jié)果。為順利進(jìn)行鎖止機(jī)構(gòu)的位姿估計，首先需要從圖像中找出鎖止機(jī)構(gòu)的位置。實例分割是一種基于深度學(xué)習(xí)的圖像分割技術(shù)，可在存在如灰塵污漬等干擾的圖像中較為準(zhǔn)確地得到目標(biāo)物的分割結(jié)果。由于需要分割的鎖止機(jī)構(gòu)在圖像中相對較小，且分割任務(wù)對于速度有一定的要求，因此，本文使用小目標(biāo)檢測性能良好、運行速度較快的Yolov5 網(wǎng)絡(luò)［7］，并配合實例分割模塊［8］進(jìn)行目標(biāo)分割。

該網(wǎng)絡(luò)使用多層殘差卷積神經(jīng)網(wǎng)絡(luò)進(jìn)行圖像的特征提取，利用金字塔結(jié)構(gòu)得到多尺度的圖像特征，并經(jīng)過包圍框預(yù)測層與分割掩碼預(yù)測層，輸出目標(biāo)檢測結(jié)果與實例分割掩膜。網(wǎng)絡(luò)結(jié)構(gòu)如圖2 所示。輸出掩膜為每個圖像像素對前景與背景的隸屬概率p。當(dāng)p<0.5 時，像素隸屬于背景；當(dāng)p>0.5 時，像素隸屬于前景。選取所有p>0.5的像素，作為鎖止機(jī)構(gòu)目標(biāo)的分割模板，同時根據(jù)網(wǎng)絡(luò)的目標(biāo)檢測框確定目標(biāo)所屬類別。

圖2 YOLOv5 網(wǎng)絡(luò)結(jié)構(gòu)Fig.2 Structure of YOLOv5 network

3.2 點云預(yù)處理

由神經(jīng)網(wǎng)絡(luò)分割得到電池包鎖止機(jī)構(gòu)的RGBD 圖像，通過相機(jī)的內(nèi)參矩陣K將該部分深度圖像素投影至三維空間而轉(zhuǎn)換為點云P0。P0點云數(shù)目巨大，且由于相機(jī)精度的限制，灰塵、物體表面材料特性的影響，點云坐標(biāo)z軸會包含一定的噪聲，因此，后續(xù)步驟首先通過體素濾波［9］進(jìn)行點云降采樣，再利用移動最小二乘算法［10］盡可能去除點云的表面噪聲。設(shè)計體素濾波器的體素方格邊長為fz=2.0 mm，在鎖止機(jī)構(gòu)輸入點云P0中建立3D 體素方格，計算每個體素內(nèi)的點云的質(zhì)心并作為輸出點云，得到濾波后的點云P1。P1點云數(shù)量更少并最大限度地保留了點云精細(xì)結(jié)構(gòu)，但仍然包含一定的z 軸噪聲并且還有部分偏差較大的離群點，因此，通過移動最小二乘方法進(jìn)行點云z軸方向的平滑處理。在待擬合點的一個鄰域上，擬合函數(shù)表示為：

其中：a(x)=[a1(x)，a2(x)，a3(x)]為待求的擬合系數(shù)，p(x)=[p1(x)，p2(x)，p3(x)] 為基函數(shù)，對于所要擬合的點云z軸坐標(biāo)，基函數(shù)p(x)=[1，p1x，p1y]，p1x，p1y分別為點的x，y軸坐標(biāo)。利用移動最小二乘方法得到：

其中：w(x-xi)為權(quán)函數(shù)，用來控制擬合的光滑度，y為函數(shù)原值。經(jīng)過體素濾波與移動最小二乘平滑的點云P2如圖3 所示，可以看到，處理后點云更加精簡，且曲面表面更加光滑，有益于后續(xù)步驟的處理。

圖3 鎖止機(jī)構(gòu)點云預(yù)處理結(jié)果Fig.3 Pretreatment result of locking mechanism point cloud

3.3 點云分割

傳統(tǒng)的全局點云配準(zhǔn)方法如快速全局配準(zhǔn)（FGR）［11］使用快速點特征直方圖（Fast Point Feature Histogram， FPFH）局部3D 描述子［12-13］匹配的方法來尋找目標(biāo)點云與模板點云之間的對應(yīng)關(guān)系，從而由對應(yīng)關(guān)系計算得到目標(biāo)點云的位姿估計。但由于FPFH 較為依賴點鄰域的局部形貌特征，而噪聲會對FPFH 的計算造成較大的干擾，導(dǎo)致誤匹配；本文的鎖止機(jī)構(gòu)中存在著具有相同特征的不同表面，其FPFH 描述子相似，該部分描述子后續(xù)步驟中也會發(fā)生誤匹配。以上因素會較大程度地減小描述子匹配的準(zhǔn)確率，降低后續(xù)隨機(jī)抽樣一致性（Random Sample Consensus, RANSAC）步驟的效率和精度。針對這一問題，本文提出為FPFH 描述子引入全局的語義特征。鎖止機(jī)構(gòu)點云具有明顯的形狀特征和分明的幾何輪廓，其全局幾何語義特征相較局部的幾何特征更為可靠穩(wěn)定，可改善FPFH 的穩(wěn)定性。因此，利用深度學(xué)習(xí)網(wǎng)絡(luò)較為強(qiáng)大的特征感知能力，提取點云的全局語義特征，彌補(bǔ)FPFH 僅考慮局部特征的缺點。

首先，利用點云分割網(wǎng)絡(luò)對鎖止機(jī)構(gòu)點云進(jìn)行語義分割，對鎖止機(jī)構(gòu)點云的不同部位進(jìn)行分類，賦予點云全局的語義信息。由于鎖止機(jī)構(gòu)點云包含噪聲和離群點，需要網(wǎng)絡(luò)能夠關(guān)聯(lián)全局特征信息，具備較好的穩(wěn)定性，因此，采用包含Encoder-Decoder 結(jié)構(gòu)的點云分割［14］網(wǎng)絡(luò)，網(wǎng)絡(luò)結(jié)構(gòu)如圖4 所示。其中，Encoder 層分為3 個采樣-聚合層，每層使用fps 采樣算法進(jìn)行下采樣，并聚合采樣點的領(lǐng)域特征，由PointNet 進(jìn)行運算［15］；Decoder 層分為兩個插值層，每層都對Encoder 層的輸出特征進(jìn)行鄰域插值；最后，通過Shared-MLP層與LogSoftmax 函數(shù)輸出點云的部件分割標(biāo)簽。通過逐層的特征聚合和逐層的插值上采樣，該網(wǎng)絡(luò)可以更好地捕捉上下文信息，在存在干擾的情況下輸出準(zhǔn)確的結(jié)果。

圖4 點云分割網(wǎng)絡(luò)結(jié)構(gòu)Fig.4 Structure of point cloud segmentation network

選擇鎖止裝置幾何特征明顯的部分作為分割對象，如圖5 所示（彩圖見期刊電子版），設(shè)置鎖頭頂部（黃）、鎖頭中部（紅），鎖頭底部（綠）和鎖頭支撐面（藍(lán)）共4 個分割部分，對每個部分使用One-Hot 標(biāo)簽進(jìn)行標(biāo)記。

圖5 鎖止機(jī)構(gòu)分割示意圖Fig.5 Schematic diagram of locking mechanism division segmentation

將P2隨機(jī)采樣至4 096 個點，并進(jìn)行歸一化，輸入點云分割網(wǎng)絡(luò)，令網(wǎng)絡(luò)輸出One-Hot 標(biāo)簽最大值對應(yīng)位為輸出標(biāo)簽。

3.4 點云配準(zhǔn)

采用FPFH 3D 描述子尋找目標(biāo)點云與模板點云的對應(yīng)關(guān)系。FPFH 是一種姿態(tài)不變的實時3D 描述子，主要描述點云的局部幾何特征。某一點的FPFH 為特征直方圖的加權(quán)和，即：

其中：SPFH 為簡化的特征直方圖，pi為p的鄰域點，wi為pi到p的歐式距離。SPFH 的計算流程如下：

（1）建立p與各鄰域點pi的局部坐標(biāo)系正交坐標(biāo)軸(u，v，ω)，其中ns為p處的法向量：

（2）根據(jù)局部坐標(biāo)系與法向量建立特征：

（3）對特征f進(jìn)行統(tǒng)計，計算其對應(yīng)的直方圖。具體而言，將特征向量中的每一個角度化為11 等分，共有33 維，將特征放入對應(yīng)的維度中，即為相應(yīng)的SPFH 特征。

然后計算得到FPFH 特征，結(jié)合上一小節(jié)的內(nèi)容，將點云語義分割結(jié)果融合進(jìn)FPFH 特征直方圖中。改進(jìn)后的FPFH 特征為：

其中：L∈{1，2，3，4}為點云語義分割的標(biāo)簽，μ為超參數(shù)。該特征描述子可確保點云在后續(xù)的匹配過程中能夠同時結(jié)合局部特征與全局特征，使具有相同語義標(biāo)簽的點匹配，從而減少誤匹配，提升后續(xù)點云配準(zhǔn)步驟的精度與速度。

獲得FPFH 特征后，記由3D 相機(jī)拍攝且經(jīng)過前述步驟處理的目標(biāo)點云為Ps，其FPFH 特征記為F(Ps)，由CAD 模型采樣的模板點云為Qm，其FPFH 特征為F(Qm)。點云配準(zhǔn)的目的就是求得Qm至Ps的姿態(tài)變換矩陣Tsm。

首先使用KD 樹進(jìn)行特征點的匹配，對于每一個ps∈Ps，尋找F(Qm)中離F(ps)最近的點F(qm)，將qm與ps作為一對匹配點。同理對于每一個qm∈Qm，可獲得另一組匹配點。上述得到的匹配點，再通過互換性測試與三角測試，篩選得到匹配關(guān)系。

使用RANSAC［12，16］方法獲得位姿估計Tsm，其流程如下：

（1）從上述的匹配關(guān)系中，隨機(jī)選擇3 對匹配點，進(jìn)行SVD 奇值分解，計算變換位姿T；

（2）根據(jù)T計算兩幅點云之間的誤差，l=其中h(?)為Huber Loss［17］；

（3）重復(fù)步驟（1）和（2），直到l小于閾值ξ或迭代次數(shù)n≥100；

（4）取所有迭代中l(wèi)最小時的位姿T作為輸出Tsm。

最后，使用迭代最近點方法（Iteration Closest Point， ICP）［18］進(jìn)行估計位姿Tsm的精配準(zhǔn)，得到更為精準(zhǔn)的Tsm。

4 實驗與結(jié)果分析

電池包實物圖如圖6 所示，其底面的鎖止機(jī)構(gòu)位于電池包的兩邊。

圖6 電池包實物圖Fig.6 Photo of battery pack

3D 相機(jī)選用知像光電的SURFACE HD50雙目結(jié)構(gòu)光相機(jī)，其有效工作距離為250～750 mm，RGB 相機(jī)的分辨率可達(dá)1 920×1 080，深度圖的分辨率為960×680，精度最高可達(dá)±0.15 mm。相機(jī)置于換電作業(yè)空間的底部，用于拍攝上方車輛底部的電池包鎖止機(jī)構(gòu)，相機(jī)與電池包的距離為0.55 m。

為了進(jìn)行室內(nèi)實驗，并更好地模擬各種姿態(tài)下的電池包，建立電池包的CAD 模型，對電池包中包含鎖止機(jī)構(gòu)的部分進(jìn)行了3D 打印，如圖7 所示。3D 打印模型擺放在距離相機(jī)0.4～0.7 m 處，實驗中在一定范圍內(nèi)調(diào)整它相對于相機(jī)的姿態(tài)。

圖7 電池包的3D 打印模型Fig.7 Three dimensional printing model of battery pack

實驗首先需要采集電池包照片，制作數(shù)據(jù)集以訓(xùn)練Yolov5 網(wǎng)絡(luò)，并根據(jù)鎖止機(jī)構(gòu)標(biāo)簽制作鎖止機(jī)構(gòu)分割數(shù)據(jù)集，訓(xùn)練點云分割網(wǎng)絡(luò)，然后使用訓(xùn)練后的網(wǎng)絡(luò)進(jìn)行位姿估計。實驗流程如圖8所示。

圖8 六維位姿估計實驗流程Fig.8 Flow chart of 6D pose estimation experiment

4.1 網(wǎng)絡(luò)訓(xùn)練

點云分割網(wǎng)絡(luò)輸出標(biāo)簽包含鎖頭頂部、鎖頭中部，鎖頭底部和鎖頭支撐面4 類。由于難以直接標(biāo)注3D 相機(jī)拍攝得到的點云，因此采用由CAD 模型仿真的方式來生成用于網(wǎng)絡(luò)訓(xùn)練的點云數(shù)據(jù)集。首先，由鎖止機(jī)構(gòu)CAD 模型均勻采樣得到模型點云，然后采用CloudCompare 軟件進(jìn)行點云的標(biāo)簽標(biāo)注，按照3.3 節(jié)所述將鎖止機(jī)構(gòu)點云分為鎖頭頂部、鎖頭中部、鎖頭底部和鎖頭支撐面4 個部分進(jìn)行標(biāo)注，其標(biāo)簽分別為{1，2，3，4}。之后，為點云添加σ=1.5 mm 的隨機(jī)高斯噪聲，同時進(jìn)行隨機(jī)位姿變換，共生產(chǎn)600個具有不同姿態(tài)的點云。為了盡可能模擬從某一視角拍攝而得的點云，對每個數(shù)據(jù)集中的點云，根據(jù)相機(jī)內(nèi)參K將點云投影至深度圖并消去重疊部分中的遠(yuǎn)點，保留近點，再根據(jù)K將深度圖重新投影至三維空間，獲得具有視角遮擋的點云。生成數(shù)據(jù)集后，將數(shù)據(jù)集按照5∶1 的比例分為訓(xùn)練集與測試驗證集。

為了盡可能提升模型訓(xùn)練的效果，采用遷移訓(xùn)練。首先，使用ShapeNet 數(shù)據(jù)對模型進(jìn)行預(yù)訓(xùn)練，再使用自制的數(shù)據(jù)集對模型進(jìn)行微調(diào)。使用交叉熵作為損失函數(shù)，其學(xué)習(xí)率設(shè)置為0.01，采用Adam 優(yōu)化器，共訓(xùn)練50 個epoch。訓(xùn)練結(jié)束后鎖止機(jī)構(gòu)的點云分割結(jié)果如圖9 所示。

圖9 鎖止機(jī)構(gòu)的點云分割結(jié)果Fig.9 Point cloud segmentation result of locking mechanism

模型訓(xùn)練完成后，通過檢驗，其標(biāo)簽預(yù)測準(zhǔn)確率可達(dá)95%，分割交并比（Intersection Over Union， IoU）達(dá)90%，滿足使用要求。其結(jié)果如圖10 所示。

圖10 網(wǎng)絡(luò)訓(xùn)練損失與測試結(jié)果Fig.10 Network training loss and test results

4.2 位姿估計實驗

實驗中，使用相機(jī)拍攝多組電池包RGBD 照片，用于驗證基于點云分割的6D 位姿估計方法。在位姿估計時，首先使用Yolov5 網(wǎng)絡(luò)對RGB 圖進(jìn)行目標(biāo)檢測與分割，并通過對齊關(guān)系獲得相應(yīng)的深度圖分割結(jié)果，將它投影為點云，對點云進(jìn)行預(yù)處理。然后，利用點云分割結(jié)果計算改進(jìn)的FPFH 描述子，進(jìn)行點云初配準(zhǔn)，最后使用ICP 方法進(jìn)行點云精配準(zhǔn)，得到估計位姿。

為了獲取拍攝得到的點云位姿的真值，首先使用Yolov5 獲取分割后的鎖止機(jī)構(gòu)點云，再使用CloudCompare 軟件的手動配準(zhǔn)功能，利用人工放置匹配點的方式實現(xiàn)拍攝的目標(biāo)點云與模型點云之間的精準(zhǔn)配準(zhǔn)，獲得準(zhǔn)確的位姿變換矩陣Tgt。

配準(zhǔn)精度用角度誤差Δθ、距離誤差Δt和均方根誤差（Root Mean Square Error， RMSE）衡量，即：

其中：Rgt，tgt為真實的旋轉(zhuǎn)矩陣與位移向量，R，t為旋轉(zhuǎn)矩陣與位移向量的估計值，qi為模板點云中的一點，pi為目標(biāo)點云中距離qi最近的點。

為驗證基于點云分割的改進(jìn)FPFH 點云配準(zhǔn)算法（記為D-SAC-IA）的有效性，分別使用ICP、正態(tài)分布變換配準(zhǔn)（NDT）、FGR、采樣一致性初始配準(zhǔn)算法（SAC-IA）等算法與提出的DSAC-IA 算法對同一姿態(tài)下相機(jī)拍攝的鎖止機(jī)構(gòu)進(jìn)行對比實驗，共拍攝10 組并計算相應(yīng)的Δθ，Δt和RMSE 平均值，結(jié)果如表1 所示。

表1 算法對比實驗結(jié)果Tab.1 Algorithm comparison results

為驗證算法對由泥漬等因素引起的表面形貌誤差的穩(wěn)定性，在上述的鎖止機(jī)構(gòu)點云中額外添加標(biāo)準(zhǔn)差為4 mm 的高斯噪聲和大小為2 mm的表面形貌噪聲，實驗結(jié)果如表2 所示。

表2 算法對比實驗結(jié)果（含噪聲）Tab.2 Algorithm comparison results（with noise）

表1 中，D-SAC-IA 的Δθ=2.86°，Δt=1.4 mm，RSME=1.6 mm；表2 中，D-SAC-IA 的Δθ=2.51°，Δt=1.9 mm，RSME=1.8 mm。在兩次實驗中，D-SAC-IA 均精度最高，證明了該方法的有效性。由表2 可知，即使添加較大噪聲，DSAC-IA 仍能較為精確地估計鎖止機(jī)構(gòu)的位姿，表明其點云分割網(wǎng)絡(luò)可更好地避免誤匹配，使得算法對噪聲有更好的穩(wěn)定性，因此精度更高。

最后進(jìn)行包含ICP 精配準(zhǔn)步驟的完整位姿估計實驗，以驗證整個方法的有效性。將3D 打印模型置于10 個不同的姿態(tài)下，每個姿態(tài)下進(jìn)行5 次位姿估計實驗并計算平均值，結(jié)果如表3 和圖11 所示。

表3 位姿估計實驗結(jié)果Tab.3 Results of pose estimation experiment

圖11 位姿估計匹配結(jié)果Fig.11 Pose estimation matching result

由表3 可知，位姿估計的平均角度誤差達(dá)到1.30°，平均位移誤差達(dá)到1.2 mm，RMSE 平均值達(dá)到1.3 mm。

為了更進(jìn)一步驗證所提方法的有效性，采用電池包實物進(jìn)行包含ICP 精配準(zhǔn)的完整位姿估計實驗。從5 個不同的視角拍攝電池包的鎖止機(jī)構(gòu)，在每個視角下進(jìn)行5 次位姿估計實驗并計算實驗結(jié)果的平均值，結(jié)果如表4 所示。

表4 位姿估計實驗結(jié)果Tab.4 Results of pose estimation experiment

由表4 可知，位姿估計的平均角度誤差達(dá)到1.90°，平均位移誤差達(dá)到1.4 mm，RMSE 平均值達(dá)到1.5 mm。換電站換電作業(yè)對于對鎖止機(jī)構(gòu)定位精度的要求為：重合誤差≤3 mm。因此，DSAC-IA 算法對于電池包鎖止機(jī)構(gòu)的位姿估計具有較高的精度和穩(wěn)定性，可以滿足電動車換電過程中的定位要求。

5 結(jié) 論

針對換電電池包的對接定位問題，本文提出了一種基于點云分割的電池包鎖止機(jī)構(gòu)6D 位姿估計方法。該方法使用YOLOv5 從場景中分割除鎖止機(jī)構(gòu)的點云，并使用體素濾波與移動最小二乘擬合進(jìn)行點云的濾波與平滑；通過引入點云分割網(wǎng)絡(luò)為FPFH 特征加入全局語義特征，彌補(bǔ)FPFH 只有點云局部特征的缺陷，并根據(jù)該特征進(jìn)行RANSAC 點云配準(zhǔn)，估計鎖止機(jī)構(gòu)點云的位姿；最后使用ICP 算法校正位姿估計結(jié)果。實驗結(jié)果表明，基于點云分割的鎖止機(jī)構(gòu)6D 位姿估計算法精度較高，其位姿估計的平均角度誤差為1.90°，平均位移誤差為1.4 mm，RMSE 平均值為1.5 mm，能夠?qū)崿F(xiàn)換電站電動車的快速穩(wěn)定換電。