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

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

1 引 言

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

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

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

2 原 理

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

圖1 電動(dòng)車換電示意圖Fig.1 Schematic diagram of electric vehicle battery swapping

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

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

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

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

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

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

3 基于點(diǎn)云分割的6D 位姿估計(jì)算法

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

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

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

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

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

3.2 點(diǎn)云預(yù)處理

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

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

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

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

3.3 點(diǎn)云分割

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

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

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

選擇鎖止裝置幾何特征明顯的部分作為分割對(duì)象，如圖5 所示（彩圖見期刊電子版），設(shè)置鎖頭頂部（黃）、鎖頭中部（紅），鎖頭底部（綠）和鎖頭支撐面（藍(lán)）共4 個(gè)分割部分，對(duì)每個(gè)部分使用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 個(gè)點(diǎn)，并進(jìn)行歸一化，輸入點(diǎn)云分割網(wǎng)絡(luò)，令網(wǎng)絡(luò)輸出One-Hot 標(biāo)簽最大值對(duì)應(yīng)位為輸出標(biāo)簽。

3.4 點(diǎn)云配準(zhǔn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

圖8 六維位姿估計(jì)實(shí)驗(yàn)流程Fig.8 Flow chart of 6D pose estimation experiment

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

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

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

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

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

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

4.2 位姿估計(jì)實(shí)驗(yàn)

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

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

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

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

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

表1 算法對(duì)比實(shí)驗(yàn)結(jié)果Tab.1 Algorithm comparison results

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

表2 算法對(duì)比實(shí)驗(yàn)結(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。在兩次實(shí)驗(yàn)中，D-SAC-IA 均精度最高，證明了該方法的有效性。由表2 可知，即使添加較大噪聲，DSAC-IA 仍能較為精確地估計(jì)鎖止機(jī)構(gòu)的位姿，表明其點(diǎn)云分割網(wǎng)絡(luò)可更好地避免誤匹配，使得算法對(duì)噪聲有更好的穩(wěn)定性，因此精度更高。

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

表3 位姿估計(jì)實(shí)驗(yàn)結(jié)果Tab.3 Results of pose estimation experiment

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

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

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

表4 位姿估計(jì)實(shí)驗(yàn)結(jié)果Tab.4 Results of pose estimation experiment

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

5 結(jié) 論

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