王立輝, 蘇余足威, 韓華春, 陳良亮, 張 浩
(1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 210096;2. 國網(wǎng)江蘇省電力有限公司,江蘇 南京210024;3. 國電南瑞科技股份有限公司,江蘇 南京211100)
電動(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é)果。
電動(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ì)方法。
針對(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)四部分組成。
拍攝得到的底部電池包圖像包含車底盤、換電站背景等信息,其中多余信息會(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
由神經(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
傳統(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)簽。
采用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。
電池包實(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
點(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
實(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)車換電過程中的定位要求。
針對(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)定換電。