智能換電站電池包鎖止機構位姿視覺估計

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

（1. 東南大學 儀器科學與工程學院 微慣性儀表與先進導航技術教育部重點實驗室，江蘇 南京 210096；2. 國網江蘇省電力有限公司，江蘇 南京210024；3. 國電南瑞科技股份有限公司，江蘇 南京211100）

1 引 言

電動汽車具有節能環保、能量轉換效率高等優點，在我國得到了大力推廣。更換電池包作為一種補電方式，可讓用戶在較短的時間內充滿電能，彌補了充電方式補電緩慢的缺點，具有更好的用戶體驗。在電動汽車換電過程中，引導換電機構與電池包精準對接是高效快速換電的關鍵。該過程可通過機械浮動對接裝置或3D 視覺定位系統實現。而相對于機械式定位，3D 視覺定位更具兼容性與靈活性，因此是目前換電站技術的研發重點。

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

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

2 原 理

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

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

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

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

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

對點云使用位姿估計算法繼續處理，得到鎖止機構相對于相機的位姿Tr。最后經過相機外參的轉換，計算鎖止機構在換電站空間中的絕對位姿：

To用于引導換電機器人與電池包的對接。

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

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

針對換電視覺定位穩定性的需求，提出基于點云分割的鎖止機構6D 位姿估計方法，通過引入點云分割神經網絡解決點云配準算法中源點云與目標點云之間對應關系難以估計的問題。該鎖止機構6D 位姿估計方法由鎖止機構目標檢測，鎖止機構點云預處理，鎖止機構點云部分分割，鎖止機構源點云與目標模型點云配準四部分組成。

3.1 RGB 圖像目標檢測

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

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

圖2 YOLOv5 網絡結構Fig.2 Structure of YOLOv5 network

3.2 點云預處理

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

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

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

圖3 鎖止機構點云預處理結果Fig.3 Pretreatment result of locking mechanism point cloud

3.3 點云分割

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

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

圖4 點云分割網絡結構Fig.4 Structure of point cloud segmentation network

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

圖5 鎖止機構分割示意圖Fig.5 Schematic diagram of locking mechanism division segmentation

將P2隨機采樣至4 096 個點，并進行歸一化，輸入點云分割網絡，令網絡輸出One-Hot 標簽最大值對應位為輸出標簽。

3.4 點云配準

采用FPFH 3D 描述子尋找目標點云與模板點云的對應關系。FPFH 是一種姿態不變的實時3D 描述子，主要描述點云的局部幾何特征。某一點的FPFH 為特征直方圖的加權和，即：

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

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

（2）根據局部坐標系與法向量建立特征：

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

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

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

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

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

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

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

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

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

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

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

4 實驗與結果分析

電池包實物圖如圖6 所示，其底面的鎖止機構位于電池包的兩邊。

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

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

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

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

實驗首先需要采集電池包照片，制作數據集以訓練Yolov5 網絡，并根據鎖止機構標簽制作鎖止機構分割數據集，訓練點云分割網絡，然后使用訓練后的網絡進行位姿估計。實驗流程如圖8所示。

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

4.1 網絡訓練

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

為了盡可能提升模型訓練的效果，采用遷移訓練。首先，使用ShapeNet 數據對模型進行預訓練，再使用自制的數據集對模型進行微調。使用交叉熵作為損失函數，其學習率設置為0.01，采用Adam 優化器，共訓練50 個epoch。訓練結束后鎖止機構的點云分割結果如圖9 所示。

圖9 鎖止機構的點云分割結果Fig.9 Point cloud segmentation result of locking mechanism

模型訓練完成后，通過檢驗，其標簽預測準確率可達95%，分割交并比（Intersection Over Union， IoU）達90%，滿足使用要求。其結果如圖10 所示。

圖10 網絡訓練損失與測試結果Fig.10 Network training loss and test results

4.2 位姿估計實驗

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

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

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

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

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

表1 算法對比實驗結果Tab.1 Algorithm comparison results

為驗證算法對由泥漬等因素引起的表面形貌誤差的穩定性，在上述的鎖止機構點云中額外添加標準差為4 mm 的高斯噪聲和大小為2 mm的表面形貌噪聲，實驗結果如表2 所示。

表2 算法對比實驗結果（含噪聲）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 仍能較為精確地估計鎖止機構的位姿，表明其點云分割網絡可更好地避免誤匹配，使得算法對噪聲有更好的穩定性，因此精度更高。

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

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

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

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

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

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

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

5 結 論

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