丁濤 吳浩 朱大虎

摘要：

點云配準(zhǔn)是大型車身構(gòu)件位姿參數(shù)測量的關(guān)鍵方法，但現(xiàn)有算法在大量異常點云干擾下難以配準(zhǔn)至有效位姿，從而導(dǎo)致匹配失真，進(jìn)而無法保證后續(xù)機(jī)器人作業(yè)質(zhì)量。針對此問題，提出一種能夠有效抑制異常點云干擾的車身構(gòu)件魯棒性配準(zhǔn)算法——魯棒函數(shù)加權(quán)方差最小化（RFWVM）算法。建立魯棒函數(shù)加權(quán)目標(biāo)函數(shù)，通過施加隨迭代次數(shù)可變的動態(tài)權(quán)重來抑制配準(zhǔn)過程中異常點云的影響，并由高斯牛頓法迭代完成剛性轉(zhuǎn)換矩陣的求解。以高鐵白車身側(cè)墻、汽車車門框為研究對象的試驗結(jié)果表明，較經(jīng)典的最近點迭代（ICP） 算法、方差最小化（VMM） 算法、加權(quán)正負(fù)余量方差最小化（WPMAVM）算法和去偽加權(quán)方差最小化（DPWVM）算法，所提出的RFWVM算法配準(zhǔn)精度更高，能夠有效抑制各種異常點云對配準(zhǔn)結(jié)果的影響，并具有更好的穩(wěn)定性和魯棒性，能夠有效實現(xiàn)各類車身構(gòu)件點云的精確配準(zhǔn)。

關(guān)鍵詞：點云配準(zhǔn)；異常點云干擾；魯棒函數(shù)；車身構(gòu)件；機(jī)器人視覺測量

中圖分類號：TP24

DOI：10.3969/j.issn.1004132X.2024.06.013

開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

Robust Registration Method for Vehicle Body Components under Abnormal

Point Cloud Interference

DING Tao1，2? WU Hao1，2? ZHU Dahu1，2

1.Hubei Longzhong Laboratory，Wuhan University of Technology Xiangyang Demonstration Zone，

Xiangyang，Hubei，441000

2.School of Automotive Engineering，Wuhan University of Technology，Wuhan，430070

Abstract： Point cloud registration was a key method for pose parameter measurement of large vehicle body components， but the existing algorithms were difficult to register to effective pose under a large number of abnormal point cloud interference， thereby resulting in matching distortion and inability to ensure the quality of subsequent robotic operations. To address the issue， a robust registration algorithm for vehicle body components， robust function weighted variance minimization（RFWVM） algorithm was proposed that might effectively suppress the interference of abnormal point cloud. A robust function weighted objective function was established， and the influences of abnormal point cloud in the registration processes were suppressed by applying dynamic weights that varied with the number of iterations. The rigid transformation matrix was solved iteratively by the Gauss-Newton method. The experimental results on the side walls of high-speed rail body and car door frames demonstrate that the proposed RFWVM algorithm has higher registration accuracy compared to classic algorithms， such as interactive closure point（ICP）， variance minimization（VMM）， weighted plus and minimum allowance variance minimization（WPMAVM）， de-pseudo-weighted variance minimization（DPWVM）， may effectively suppress the influences of various abnormal point clouds on registration results， and also behaves better stability and robustness. The method may effectively achieve the accurate registration of various vehicle body components.

Key words： point cloud registration; abnormal point cloud interference; robust function; vehicle body component; robotic vision measurement

收稿日期：20230615

基金項目：國家重點研發(fā)計劃（2022YFB4700501）;國家自然科學(xué)基金（51975443）;湖北隆中實驗室自主創(chuàng)新項目（2022ZZ-27）

0? 引言

以機(jī)器人為制造裝備執(zhí)行體的機(jī)器人加工技術(shù)是實現(xiàn)軌道交通、先進(jìn)汽車等領(lǐng)域大型復(fù)雜構(gòu)件高效高品質(zhì)制造的主流加工模式［1-3］。在機(jī)器人執(zhí)行加工任務(wù)之前，通常需要利用視覺傳感器實現(xiàn)工件的位姿參數(shù)測量。目前，三維點云配準(zhǔn)是計算大型復(fù)雜構(gòu)件位姿參數(shù)的通用方法［4］，該方法通過計算模型點云到測量點云之間的剛體轉(zhuǎn)換矩陣得到兩片點云之間的精確位姿轉(zhuǎn)換關(guān)系，以實現(xiàn)工件的位姿測量。然而，針對高鐵白車身、汽車白車身等這類具有大曲率變化或復(fù)雜型面的車身構(gòu)件，其視覺測量定位較為困難。另外，點云配準(zhǔn)效果通常也會受到異常點云的影響，如測量噪聲、模型點云與測量點云存在結(jié)構(gòu)偏差、測點密度分布不均或加工余量過多且分布不均等。因此亟需一種能夠有效解決上述問題的點云配準(zhǔn)算法。

異常點云干擾下的車身構(gòu)件魯棒性配準(zhǔn)方法——丁? 濤? 吳? 浩? 朱大虎

中國機(jī)械工程 第35卷 第6期 2024年6月

近年來，國內(nèi)外學(xué)者對三維點云配準(zhǔn)技術(shù)進(jìn)行了深入研究。最近點迭代（iterative closet point，ICP）算法［5］是最經(jīng)典的三維點云配準(zhǔn)方法之一，通過迭代計算最小化點到點的距離平方和來完成點云精確配準(zhǔn)，但其收斂速度慢，且當(dāng)兩片點云之間初始位姿相差過大時，容易陷入局部最優(yōu)，收斂穩(wěn)定性較差。YANG等［6］將分支界定算法與改進(jìn)的ICP算法相結(jié)合，提出GO-ICP（globally optimal ICP）算法，該算法理論上能夠保證所有配準(zhǔn)收斂到全局最優(yōu)，但當(dāng)點云數(shù)量較多時，算法耗費的時間將會呈指數(shù)級增長。PAVLOV等［7］提出了AA-ICP（iterative closest point with Anderson acceleration）算法，該算法使用Anderson 加速算法對經(jīng)典ICP算法進(jìn)行改進(jìn)，大大減少了迭代所需時間與迭代次數(shù)。ICP的變體算法還有許多，如Sparse-ICP［8］、NICP［9］、FR-ICP［10］、WA-ICP［11］、稀疏Mixed-ICP［12］等。但上述ICP的變體算法在面對具有測量缺陷的點云時仍會發(fā)生匹配失真。除了優(yōu)化ICP算法本身之外，許多學(xué)者將ICP算法與一些特征提取算法進(jìn)行結(jié)合，有效擴(kuò)大了算法的應(yīng)用范圍并提高了計算精度［13-15］。

除了以點到點的距離為目標(biāo)函數(shù)的ICP算法之外，POTTMANN等［16］以點到切平面距離為目標(biāo)函數(shù)，提出了TDM（tangent distance minimization）算法，該算法能夠有效提高收斂速度，但會降低一小部分收斂穩(wěn)定性。在此基礎(chǔ)上，LI等［17］提出了ADF（adaptive distance function）算法，該算法結(jié)合了不同比例的點到模型面的切向距離與法向距離，力求實現(xiàn)既提高收斂速度又保證收斂穩(wěn)定性的效果。XIE等［18-19］提出了一種距離方差最小化（variance minimization， VMM） 算法，該算法基于點對的距離方差建立目標(biāo)函數(shù)，通過迭代計算完成配準(zhǔn)，能夠有效抑制點云密度不均及點云缺失引起的匹配失真，然而該算法在面對余量不均時會陷入局部最優(yōu)。LYU等［20］提出了一種加權(quán)正負(fù)余量方差最小化（weighted plus and minimum allowance variance minimization ，WPMAVM） 算法，該算法將測點分為正負(fù)測點，并對其距離函數(shù)附加權(quán)重，以減少測點異常對配準(zhǔn)的影響。該算法能夠有效抑制余量不均點云、測量噪聲點云對配準(zhǔn)的影響，但當(dāng)測量點云存在結(jié)構(gòu)偏差時會陷入局部最優(yōu)。吳浩等［21］提出一種去偽加權(quán)方差最小化（de-pseudo-weighted variance minimization ，DPWVM） 算法，并建立了自適應(yīng)協(xié)調(diào)距離來提高收斂穩(wěn)定性，該算法能夠有效抑制由點云結(jié)構(gòu)偏差引起的匹配傾斜，但在余量較多且余量偏差距離不大的情況下會將余量視為正常點云參與配準(zhǔn)，最終陷入局部最優(yōu)。

為解決上述存在大量異常點云的大型復(fù)雜構(gòu)件配準(zhǔn)問題，本文提出一種能在異常點云干擾下有效配準(zhǔn)的魯棒函數(shù)加權(quán)方差最小化（robust function weighted variance minimization ，RFWVM） 算法，所建立的魯棒權(quán)重函數(shù)能夠隨迭代過程而變化，在配準(zhǔn)前期能夠增加匹配穩(wěn)定性，后期能夠抑制大量點云異常，不易陷入匹配失真，具有更強的魯棒性與穩(wěn)定性。

1? 問題描述

理論上，待加工工件與其CAD模型在尺寸上應(yīng)完全一致。但由于生產(chǎn)過程中不可避免地存在制造誤差，以及采集測量點時會引入噪聲，測量點云與理想模型點云無法保持一致，表現(xiàn)為存在測量噪聲點云或者不均勻加工余量點云等，如圖1所示。

不均勻的異常余量點云對工件的配準(zhǔn)定位精度影響極大，傳統(tǒng)的配準(zhǔn)算法往往會陷入局部最優(yōu)甚至完全匹配失真。本文針對上述問題提出了RFWVM點云配準(zhǔn)算法，其流程如圖2所示。

2? 魯棒函數(shù)加權(quán)方差最小化算法

2.1? 目標(biāo)函數(shù)建立

如圖3所示，將所有測點分為正余量測點pf與負(fù)余量測點pg，分別滿足nTj·（pf－qj）>0與nTj·（pg－qj）<0，qj為CAD模型曲面上一點，nj為曲面在qj處的法向量，pf+與pg+分別為pf與pg經(jīng)G（R，t）單步轉(zhuǎn)換后的點，R、t分別為微小旋轉(zhuǎn)矩陣和微小平移向量，pv為pf+與pg+在qj切平面上的投影點。

為了減少異常余量對配準(zhǔn)過程的影響，定義正負(fù)余量魯棒權(quán)重函數(shù)wf與wg，并對每個測點施加權(quán)重，以弱化異常測點對配準(zhǔn)的影響。

以正向測點為例，定義所有正向測點的魯棒函數(shù)加權(quán)距離偏差dfR+如下：

dfR+=wfdfTDM－d-w+（1）

d-w+=∑mf=1wfdfTDM/∑mf=1wf（2）

wf=1????? ｜dfT/d-T+|≤1

－|k－2k|［（（dfT/d-T+）2α2|k－2|+1）k2－1］+1

|dfT/d-T+|wf=0≥|dfT/d-T+|>1

0|dfT/d-T+|>|dfT/d-T+|wf=0

dfT=nTj·（pf－qj）? d-T+=∑mf=1wfdfT/∑mf=1wf

其中，dfTDM為該正余量測點經(jīng)G（R，t）單步轉(zhuǎn)換后到最近點處的切平面距離;m為正余量測點的數(shù)目，f∈［1，m］；d-w+為單步轉(zhuǎn)換后正余量測點到最近點處的魯棒函數(shù)加權(quán)切平面距離均值；wf為正余量魯棒權(quán)重函數(shù)；k為控制權(quán)重函數(shù)魯棒性的變形參數(shù)，其取值范圍為{k∈R|k≠2，k≠0}。將dfT/d-T+視為自變量，設(shè)wf函數(shù)零點為|dfT/d-T+|wf=0，則α為控制權(quán)重函數(shù)零點值的尺度因子，α∈（0，+∞）。當(dāng)dfT/d-T+大于曲線零點|dfT/d-T+|wf=0時，視該點為無效測點，賦予權(quán)值0；dfT為當(dāng)前位置的正余量測點到最近點處切平面的距離；d-T+為當(dāng)前正余量測點到最近點處魯棒函數(shù)加權(quán)切平面距離均值。

同理，對于所有負(fù)向測點，其魯棒函數(shù)加權(quán)距離偏差dgR-如下：

dgR-=wgdgTDM－d-w-（3）

d-w-=∑ng=1wgdgTDM/∑ng=1wg（4）

wg=1????? |dgT/d-T-|≤1

－k－2k［（（dgT/d-T-）2α2|k－2|+1）k2－1］+1

|dgT/d-T-|wg=0≥|dgT/d-T-|>1

0|dgT/d-T-|>|dgT/d-T-|wg=0

dgT=nTj（pg－qj）? d-T-=∑ng=1wgdgT/∑ng=1wg

式中，dgTDM為該負(fù)余量測點經(jīng)G（R，t）單步轉(zhuǎn)換后到最近點處的切平面距離;n為負(fù)余量測點總數(shù)，g∈［1，n］；d-w-為單步轉(zhuǎn)換后負(fù)余量測點到最近點處的魯棒函數(shù)加權(quán)切平面距離均值；wg為負(fù)余量魯棒權(quán)重函數(shù)；dgT為當(dāng)前位置的負(fù)余量測點到最近點處的切平面距離；α、k的取值與正余量測點相同；d-T-為當(dāng)前負(fù)余量測點到最近點處魯棒函數(shù)加權(quán)切平面距離均值。

k值控制權(quán)重函數(shù)的魯棒性，為了更直觀地看出k值對魯棒性的影響，設(shè)定α=2.5，取不同k值并繪出其魯棒權(quán)重函數(shù)曲線，如圖4所示，圖中dT為測點的切平面距離，|d-T|則為其對應(yīng)的正負(fù)余量測點的切平面距離均值，w為權(quán)值大小。由式（2）、式（4）可知，k=0與k=2時權(quán)重函數(shù)無定義，這里取極限進(jìn)行近似；圖4中，權(quán)重函數(shù)的值相對于k是單調(diào)遞減的，隨k值的增大，權(quán)重函數(shù)曲線變陡。曲線的形狀反映了該算法在此參數(shù)下對異常點的容忍程度，對于距平均距離較遠(yuǎn)的測點，加權(quán)會弱化它對配準(zhǔn)方向的影響，當(dāng)賦予該測點權(quán)重為0時，該點會被稀疏掉；然而，并非弱化效果越強越好，對于弱特征或無特征點云，則需要保留部分遠(yuǎn)點的影響力，故針對不同點云應(yīng)選取不同k值以達(dá)到最優(yōu)配準(zhǔn)效果。經(jīng)實驗驗證，本文取k=－2用于本文點云模型。

α值對權(quán)重函數(shù)零點的位置有較大影響，隨配準(zhǔn)過程的進(jìn)行去逐漸縮小α可以提高配準(zhǔn)穩(wěn)定性，其過程如圖5所示。實際上是加權(quán)后的測點控制配準(zhǔn)方向，本文算法在配準(zhǔn)前期將余量不大的測點視為可控異常點云，參與配準(zhǔn)提高配準(zhǔn)穩(wěn)定性，配準(zhǔn)中期通過加權(quán)減小異常點云的影響，配準(zhǔn)后期將更多點視為異常余量點云并賦權(quán)值0，將其稀疏掉。因為本文算法是以方差最小化［18］的方法進(jìn)行迭代計算的，故即使稀疏掉一些點云造成點云分布不均也不會導(dǎo)致配準(zhǔn)陷入局部最優(yōu)。若在前期直接將α設(shè)置為一個較小的值，則配準(zhǔn)會因為稀疏掉許多有效測點而進(jìn)入錯誤的配準(zhǔn)方向?qū)е缕ヅ涫д妗?/p>

本文中取α初值為10，隨配準(zhǔn)過程每迭代4次，算法令α←α/2，當(dāng)α值小于0.5時，令α=0.5以防止函數(shù)稀疏掉重要特征點。此時，可得wf與wg變化曲線如圖6所示。

結(jié)合式（1）與式（3）可建立魯棒函數(shù)加權(quán)方差最小化算法的目標(biāo)函數(shù)如下：

min G（R，t）=∑mf=1d2fR++∑ng=1d2fR－=

∑mf=1（wfdfT－d-w+）2+∑ng=1（wgdgT－d-w-）2（5）

2.2? 轉(zhuǎn)換矢量求解

以正向余量測點為例，設(shè)該點每步迭代的微小平移向量t=（Δx，Δy，Δz）T，微小旋轉(zhuǎn)量r=（δx，δy，δz）T，設(shè)ξ=（tT，rT）T為一組轉(zhuǎn)換向量，表示單步轉(zhuǎn)換的旋轉(zhuǎn)平移參數(shù)。因此單步轉(zhuǎn)換后的正向測點pf+可表示為

pf+=pf+r×pf+t（6）

因此，正余量測點切平面距離可由下式表示：

dfT=（pf+－qj）Tnj=（pf+r×pf+t－qj）Tnj=

（pf－qj）TnfT+nfTpf×nfTTξ=dfT0+Afξ（7）

式中，Af為1×6矩陣；nfT為pf最近點qj點的法向量，nfT=nj；dfT0為當(dāng)前測點的切平面距離。

同理，建立負(fù)余量測點切平面距離公式如下：

dgT=（pg+－qj）Tnj=（pg+r×pg+t－qj）Tnj=

（pg－qj）TngT+ngTpg×ngTTξ=dgT0+Afξ（8）

式中，Af為1×6矩陣；ngT為pg最近點qj點的法向量，ngT=nj；dgT0為當(dāng)前測點的切平面距離。

由2.1節(jié)可知，求解該目標(biāo)函數(shù)是一個非線性最小二乘問題。根據(jù)非線性優(yōu)化理論，使用高斯牛頓法（Gauss-Newton）求解轉(zhuǎn)換向量ξ。

結(jié)合式（5）、式（7）與式（8），可將目標(biāo)函數(shù)表示為

G=12∑mf=1［wf（dfT0+Afξ）］2+12∑ng=1［wg（dgT0+

Agξ）］2－12（2∑mf=1wf－m）［∑mf=1wf（dfT0+

Afξ）］2/（∑mf=1wf）2－

12（2∑ng=1wg－n）［∑ng=1wg（dgT0+

Agξ）］2/（∑ng=1wf）2=12∑mf=1［（wfdfT0－d-T0+）+

（wfAf－w+）ξ］2+12∑ng=1［（wgdgT0－d-T0－）+

（wgAg－w-）ξ］2（9）

由于每次迭代都是基于上一次迭代完成時的點云位置，故可取第一次迭代時的轉(zhuǎn)換向量ξ0=06×1，基于非線性優(yōu)化理論，使用高斯牛頓法求解轉(zhuǎn)換矢量如下：

ξ=－（2G（ξ0））－1G（ξ0）=－［∑mf=1（wfAf－

w+）T（wfAf－w+）+∑ng=1（wgAg－w-）T（wgAg－

w-）］－1·［∑mf=1（wfAf－

w+）T（wfdfT0－d-T0+）+

∑ng=1（wgAg－w-）T（wgdgT0－

d-T0－）］=

－｛［∑mf=1（w2fATfAf）－

（2∑mf=1wf－m）Tw+w+］+

［∑ng=1（w2gATgAg）－

（2∑ng=1wg－n）Tw-w-］}－1·

{［∑mf=1w2fdfT0Af－

（2∑mf=1wf－m）d-T0+w+］+

［∑ng=1w2gdgT0Ag－（2∑ng=1wg－n）d-T0－w-］｝=

－E－1F（10）

d-T0+=∑mf=1wfdfT0/∑mf=1wf

d-T0－=∑ng=1wgdgT0/∑ng=1wg

其中，2G（ξ0）為G的海森矩陣，G（ξ0）為G的梯度；E為6×6矩陣；F為6×1矩陣； w+為魯棒函數(shù)加權(quán)正向平均向量，滿足：

w+=∑mf=1wfAf/∑mf=1wf（11）

w－為魯棒函數(shù)加權(quán)負(fù)向平均向量，滿足

w－=∑ng=1wgAg/∑ng=1wg（12）

本文算法以高斯牛頓法求解，故RFWVM算法與高斯牛頓法一樣具有二階收斂速度。

求解得到轉(zhuǎn)換向量ξ后，使用下式［22］即可得到旋轉(zhuǎn)矩陣R與平移向量T：

R=er^

T=t（13）

r^=0－δzδyδz0－δx－δyδx0（14）

式中，r^為r的反對稱矩陣。

求解完成后，可對源點云進(jìn)行剛性位姿轉(zhuǎn)換，并計算單步轉(zhuǎn)換后兩片點云之間的誤差，設(shè)定三種誤差評價指標(biāo)如下：

=∑m+ni=1（pi－qj）2m+n

r=∑m1+n1i=1（pi－qj）2m1+n1

σRMSE=∑m+ni=1（pi－qj）2m+n（15）

式中，pi為測量點云中的測點；qi為模擬點云中的對應(yīng)點；為描述兩片點云之間點到最近鄰點的距離均值誤差；r為描述去除異常點云后的點到最近鄰點距離均值誤差；σRMSE為均方根誤差；m1、n1分別為去除異常點云后正負(fù)余量點的數(shù)量。

設(shè)定收斂條件為在兩次連續(xù)迭代中的差值不超過10-6? mm，若未達(dá)到該條件則繼續(xù)迭代到所設(shè)定的收斂次數(shù)為止。RFWVM算法的偽代碼如下：

算法1：魯棒函數(shù)加權(quán)方差最小化算法

輸入：掃描點云p與模型點云q；尺度因子初始值α與下限αr

輸出：q到p的剛性旋轉(zhuǎn)矩陣R與平移向量T

1：? 初始化環(huán)境：設(shè)置終止迭代次數(shù)nmax或迭代終止誤差閾值ε0；迭代次數(shù)n=1;

2：While n≤nmax or ε≤ε0 do

3：? 計算qj中每個點的法向量nj并進(jìn)行法向量定向

4：? 將模型點云qj輸入KD-tree

5：? for pi in range p do

6：??? 在KD-tree中搜索pi的最近點qt

7：??? 計算pi到qt的切平面上的最近點qtdm

8：??? 分別保存pi與qtdm點對到集合pwld與qcad中

9：? end

10：? 計算pwld中與qcad對應(yīng)點對正負(fù)測點切平面距離

11：? 計算魯棒權(quán)重wf與wg并對其賦予各測點距離

12：? 計算單步旋轉(zhuǎn)矩陣Rn與單步平移矩陣Tn

13：? 更新模型點云q位置，計算誤差ε

14：? n←n+1

15：? if n%4=0 do α=α/2 end

16：? if α<αr do α=αr end

17：end

18：計算總旋轉(zhuǎn)矩陣R與平移向量T

2.3? RFWVM算法收斂特性分析

2.3.1? 收斂正定性

當(dāng)矩陣E為非正定矩陣時，由非線性優(yōu)化理論可知，求得的轉(zhuǎn)換矢量ξ為錯誤值，算法易錯誤收斂甚至不收斂，故有必要證明它的正定性。RFWVM算法的二次型如下：

eξ=ξTEξ=ξT［∑mf=1（w2fATfAf）－（2∑mf=1wf－

m）Tw+w+］ξ+ξT［∑ng=1（w2gATgAg）－（2∑ng=1wg－

n）Tw-w-］ξ=［∑mf=1（wfAfξ）2－

2∑mf=1（wfAfξ）（w+ξ）+∑mf=1（w+ξ）2］+

［∑ng=1（wgAgξ）2－2∑ng=1（wgAgξ）（w-ξ）+

∑ng=1（w-ξ）2］=∑mf=1（wfAfξ－w+ξ）2+

∑ng=1（wgAgξ－w+ξ）2（16）

由式（16）可知eξ≥0恒成立，只有當(dāng)所有正余量測點為同一測點且所有負(fù)余量測點為同一測點時eξ=0，而實際上不存在這種情況，故eξ>0恒成立，故矩陣E為正定矩陣。

2.3.2? 收斂魯棒性分析

將測點細(xì)分為異常測點與正常測點，則目標(biāo)函數(shù)公式可進(jìn)一步表示為

GdRFWVM=∑m1fa=1（wfadfaT－d-w+）2+

∑n1ga=1（wgadga－d-w-）2+∑m1+n1+m2+n2h=m1+n1+1D2h（17）

其中，wfa、wga為正負(fù)向在正常范圍內(nèi)測點的權(quán)值，其值為1；dfa、dga為對應(yīng)正負(fù)測點的切平面距離；∑m1+n1+m2+n2h=m1+n1+1D2h為所有異常測點的加權(quán)距離偏差平方和，包括正向異常測點與負(fù)向異常測點兩部分，m2、n2表示正負(fù)異常點云的數(shù)量。以正向收斂為例，具體分析實際的d-w+，將d-w+如下式表示：

d-w+=∑mf=1wfdfT/∑mf=1wf=

∑m1fa=1wfadfaT+∑m1+m21h1+=m1+1wh1+dh1+T+∑m1+m21+m20h1+=m1+m21+1wh0+dh0+T∑m1fa=1wfa+∑m1+m21h1+=m1+1wh1++∑m1+m21+m20h0+=m1+m21+1wh0+（18）

式中，m21、m20分別為可控異常測點和不可控異常測點數(shù)量；wh1+、wh0+分別為對應(yīng)下標(biāo)測點的魯棒函數(shù)權(quán)重；dh1+T、dh0+T分別為對應(yīng)下標(biāo)測點的切平面距離。

由2.1節(jié)中魯棒函數(shù)權(quán)重可得到wh0+=0，將其與wfa=1代入式（18）中進(jìn)行化簡可得

d-w+=∑m1fa=1dfaT+∑m1+m21h1+=m1+1wh1+dh1+Tm1+∑m1+m21h1+=m1+1wh1+（19）

式（19）為實際的d-w+表達(dá)式，設(shè)理想的正向加權(quán)距離均值d-s+=∑m1fa=1dfaT/m1，計算d-w+與d-s+的差值如下：

d-w+－d-s+=∑m1fa=1dfaT+∑m1+m21h1+=m1+1wh1+dh1+Tm1+∑m1+m21h1+=m1+1wh1+－

∑m1fa=1dfaTm1=∑m1+m21h1+=m1+1wh1+dh1+T－d-s+∑m1+m21h1+=m1+1wh1+m1+∑m1+m21h1+=m1+1wh1+=

∑m1+m21h1+=m1+1wh1+（dh1+T－d-s+）m1+∑m1+m21h1+=m1+1wh1+（20）

由式（20）可知，假設(shè)全部的dh1+T與d-s+值非常相近時，d-w+－d-s+的值接近于0，即實際的加權(quán)平均距離與理想值非常接近；若wh1+為（0，1）之間的一個居中值，且存在過多的可控異常測點時，則算法將余量不大的大量偏差點云區(qū)域視為可控余量參與計算，m21m20甚至m21與m1是一個數(shù)量級的情況下，d-w+－d-s+的值會非常大，此時理想正向收斂位置與實際收斂位置存在較大偏差，從而導(dǎo)致匹配失真。而2.1節(jié)中設(shè)定的權(quán)重函數(shù)wf會隨著迭代次數(shù)的增加而減小界定可控余量點云的距離范圍來使m21減小，同時還能縮小df1+T與d-s+之間的差值，此時正向?qū)嶋H收斂位置d-w+會逐漸向正向理想收斂位置d-s+靠近，因此RFWVM算法可以抑制大量異常點云引起的匹配失真。將Dh中可控異常測點剔除得到Dh0。假設(shè)RFWVM算法使d-w+≈d-s+，同理，可證 d-w-≈d-s-。將其與wf0+=0、wf0-=0一起代入Df0中，如下式所示：

Dh0=

wh0+dh0+T－d-w+

wh0-dh0-T－d-w-≈

wh0+dh0+T－d-s+

wh0-dh0-T－d-s-=

wh0+（f（ξRFWVM）ph0+1－qj0+1）Tnj0+T1－d-s+

wh0-（f（ξRFWVM）ph0-1－qj0-1）Tnj0-T1－d-s-≈

－d-s+? 正向測點

－d-s-負(fù)向測點（21）

式（21）可以證明，在權(quán)重作用下，不可控異常測點不會參與ξRFWVM的求解，且此時Dh0等價于理想正負(fù)余量均值距離，則最小化Dh0的目的與配準(zhǔn)的目的是一致的。

綜上所述，RFWVM算法能夠抑制大量余量不均點云造成的匹配傾斜，且具有二次收斂速度，動態(tài)權(quán)重函數(shù)使其具有較強的穩(wěn)定性。

3? 仿真與實驗驗證

3.1? 同源點云配準(zhǔn)測試

以高鐵白車身側(cè)墻為實驗對象，使用惟景三維公司的PowerScan-Pro2.3M結(jié)構(gòu)光三維掃描儀（測量精度為±0.02 mm，幅值為300 mm×400 mm）以眼在手上的方式［23］掃描并拼接獲取測量點云，如圖7所示。最后所得測量點云如圖8所示，點云數(shù)量約為310萬。

首先針對不同k值對該高鐵點云配準(zhǔn)效果影響進(jìn)行橫向?qū)嶒烌炞C，以選取最佳的配準(zhǔn)參數(shù)。將高鐵白車身掃描點云進(jìn)行預(yù)處理，使用均勻濾波將其離散至352 595個點，手動將16%的點云向外偏置10 mm，如圖9所示。在配準(zhǔn)前將未偏置源點云進(jìn)行一定角度的旋轉(zhuǎn)平移操作，這樣一方面可驗證點云穩(wěn)定性，另一方面可以排除因初始位置太好而出現(xiàn)的較好的假性配準(zhǔn)效果，以此模擬CAD點云到測量點云的配準(zhǔn)。

分別取k為1、0、-1、-2、-3，設(shè)定最大配準(zhǔn)次數(shù)為20，得到配準(zhǔn)結(jié)果如表1所示。由表1可知，對于高鐵白車身點云，當(dāng)k≥－2時配準(zhǔn)誤差與k成正相關(guān)，而當(dāng)k=-3時其配準(zhǔn)誤差增大，故本文相關(guān)實驗取k=-2。

為進(jìn)一步說明RFWVM算法對存在大量加工余量點云時配準(zhǔn)的魯棒性， 與各算法進(jìn)行對比，分別使用ICP、VMM、WPMAVM、DPWVM以及RFWVM算法對兩點云進(jìn)行精確配準(zhǔn)，設(shè)定最大迭代次數(shù)為30，配準(zhǔn)結(jié)果如表2所示。其中，RFWVM算法最近鄰點距離誤差均值為0.5371 mm，相比ICP、VMM、WPMAVM、DPWVM算法分別降低了85.8%、76.7%、82.98%與7.6%。

由表2可知，傳統(tǒng)ICP配準(zhǔn)的各項精度都比其他四種算法配準(zhǔn)精度差，DPWVM算法作為WPMAVM優(yōu)化后的算法，它對異常點抑制能力較強，故其配準(zhǔn)精度僅次于RFWVM算法的配準(zhǔn)精度。將各算法的最終結(jié)果映射為誤差色譜，如圖10所示。

由圖10可知，在該組實驗中，ICP算法發(fā)生嚴(yán)重的匹配失真，VMM算法與WPMAVM算法均陷入局部最優(yōu)，發(fā)生匹配偏移，而DPWVM算法與RFWVM算法能夠有效抑制異常點云的影響而配準(zhǔn)至全局最優(yōu)位置。每次迭代的距離誤差變化如圖11所示，可知，ICP、VMM、WPMAVM算法均因陷入局部最優(yōu)而無法配準(zhǔn)到正確位姿。RFWVM算法則由于變化的α值更新了配準(zhǔn)方向而跳出局部最優(yōu)，使配準(zhǔn)趨向全局最優(yōu)位姿。

為了進(jìn)一步評估RFWVM算法對噪聲的抑制能力，向高鐵白車身點云中加入?yún)?shù)為N（0，32）的高斯噪聲，以模擬實際采樣過程中產(chǎn)生大量噪聲的情況。將含有噪聲點云的白車身點云作為目標(biāo)點云，無噪聲點云的白車身點云作為源點云，改變其初始位姿后，分別使用ICP、WMPAVM、VMM、DPWVM以及RFWVM五種算法進(jìn)行點云配準(zhǔn)，最終結(jié)果如表3所示。由表3中結(jié)果可知，RFWVM算法誤差僅大于VMM算法誤差，小于其他三種算法誤差，具有一定的噪聲抑制能力。VMM算法因其方差最小化非常適合于含有高斯噪聲的配準(zhǔn)，故此處VMM算法優(yōu)于其他算法，而面對實際采集時無規(guī)律的隨機(jī)噪聲，RFWVM算法相對于其他算法會取得更好的效果。

RFWVM算法最終配準(zhǔn)效果如圖12所示，

圖中紅色的點為未加噪聲點云，藍(lán)色為噪聲點云。由圖12可以看出，該算法能夠有效抑制噪聲點云并將模型點云轉(zhuǎn)換到正確的位姿。

為對RFWVM算法魯棒性能進(jìn)行測試，設(shè)置對照實驗，將高鐵白車身點云偏置百分比φ分別為12%、16%、20%、24%的點進(jìn)行10 mm的偏置，并變換其初始位置，同樣使用ICP、VMM、WPMAVM、DPWVM以及RFWVM算法對相應(yīng)點云進(jìn)行30次迭代配準(zhǔn)，將配準(zhǔn)結(jié)果以柱狀圖形式展示，如圖13所示。

由于此次采用的配準(zhǔn)對象結(jié)構(gòu)較為簡單，且VMM、WPMAVM、DPWVM、RFWVM算法相比ICP算法增加了法向量計算時間，故圖13d所示的算法運行時間僅具有相對參考意義。由圖13a～圖13c可以看出，DPWVM算法在12%點云偏置時，得益于其對部分異常余量的抑制能力，配準(zhǔn)誤差略低于RFWVM算法配準(zhǔn)誤差，但當(dāng)異常余量增多時，DPWVM算法也難以配準(zhǔn)至理想位置，RFWVM算法各項誤差均值均為最低，且在點云偏置為20%、24%時，其精度遠(yuǎn)高于其他四種算法精度，說明RFWVM算法可以有效抑制偏置點云的影響。進(jìn)一步將每組實驗VMM、DPWVM與RFWVM算法結(jié)果以誤差色譜圖列出，如圖14所示。

圖14中VMM算法明顯受偏置點云的影響發(fā)生匹配傾斜，并沒有收斂到理想位置;DPWVM算法在少量點云偏置時能夠配準(zhǔn)至較好位姿，但當(dāng)大量點云偏置時該算法也不可避免地會匹配失真；而RFWVM算法在各種偏置條件下可以不受偏置點云的影響而收斂至理想位置，這主要得益于所設(shè)定的魯棒函數(shù)權(quán)重隨著配準(zhǔn)過程動態(tài)變化。在前期，給予較大的wf與wg可使算法往正確的方向收斂，不發(fā)生匹配失真，后期魯棒函數(shù)權(quán)重的曲線呈一種截斷式形狀，能夠排除異常點的影響，使配準(zhǔn)趨向于全局最優(yōu)位姿，具有極強的魯棒性與穩(wěn)定性。

3.2? 異源點云配準(zhǔn)測試

在3. 1節(jié)中，主要是利用手動偏置的同源點云進(jìn)行仿真測試，目的是驗證RFWVM算法的配準(zhǔn)性能。在本節(jié)中，以復(fù)雜車門框沖壓件為對象，使用實際掃描獲取的車門框沖壓件的測量點云（點云數(shù)量約為47.4萬）與其CAD模型點云進(jìn)行配準(zhǔn)測試（點云數(shù)量約為47.6萬），目的是測試本文算法在實際運用過程中對異源點云的配準(zhǔn)效果，并進(jìn)一步驗證算法的普適性。車門框的初始狀態(tài)及其主要存在的結(jié)構(gòu)偏差區(qū)域如圖15所示。

使用ICP、VMM、WPMAVM、DPWVM以及RFWVM算法分別對車門框異源點云進(jìn)行30次迭代配準(zhǔn)，得到結(jié)果如表4所示。其中，RFWVM算法的平均距離誤差為1.4238 mm，相比ICP、VMM、WPMAVM、DPWVM算法的結(jié)果分別降低了12.4%、7.1%、10.6%與8.6%。

由表4可知，本文算法各項誤差值均小于其他四種算法誤差值。將配準(zhǔn)結(jié)果距離誤差映射為誤差云圖，各算法誤差如圖16所示。由圖16可知，本文算法能更加有效地區(qū)分出測量點云中的結(jié)構(gòu)偏差點云并加以抑制。將配準(zhǔn)過程中各算法誤差變化曲線繪制成點線圖（圖17）。由圖17可知，本文算法在對車門框沖壓件進(jìn)行配準(zhǔn)時，前期相對于DPWVM算法能更快找到初始配準(zhǔn)方向，后期能有效抑制異常點云，不易陷入局部最優(yōu)，其誤差曲線段落式的下降恰好證明了動態(tài)變化的魯棒權(quán)重函數(shù)的有效性。

4? 結(jié)論

本文基于機(jī)器人視覺測量技術(shù)提出了一種魯棒函數(shù)加權(quán)方差最小化（RFWVM）算法，能夠有效解決車身構(gòu)件點云配準(zhǔn)時由于受到異常點云影響而無法有效配準(zhǔn)的問題。主要結(jié)論如下：

（1）RFWVM算法以最小化正負(fù)測點到其對應(yīng)切平面距離的加權(quán)平方和為優(yōu)化目標(biāo)，通過高斯牛頓法迭代求解剛體轉(zhuǎn)換矩陣，保證了二次收斂速度，提高了配準(zhǔn)效率；在大型復(fù)雜構(gòu)件的點云配準(zhǔn)中，通過對目標(biāo)函數(shù)施加動態(tài)可變的魯棒函數(shù)權(quán)重，抑制了異常點云的影響，并基于變化的權(quán)重曲線，實現(xiàn)了構(gòu)件位姿的魯棒性配準(zhǔn)。

（2）同源點云配準(zhǔn)測試中，RFWVM算法最近鄰點距離誤差均值為0.5371 mm，相比ICP、VMM、WPMAVM、DPWVM算法分別降低了85.8%、76.7%、82.98%與7.6%；不同偏置點云數(shù)量的對照實驗表明，RFWVM算法可以有效抑制匹配失真，不易陷入局部最優(yōu)，存在大量異常點云情況下配準(zhǔn)效果明顯優(yōu)于ICP、VMM、WPMAVM、DPWVM算法；最后設(shè)置帶有高斯噪聲的高鐵白車身側(cè)墻點云進(jìn)行配準(zhǔn)實驗，結(jié)果表明本文算法能夠有效抑制高斯噪聲。

（3）異源點云配準(zhǔn)測試中，RFWVM算法的平均距離誤差為1.4238 mm，相比ICP、VMM、WPMAVM、DPWVM算法的結(jié)果分別降低了12.4%、7.1%、10.6%與8.6%；對比其汽車車門框誤差色譜圖可知，本文算法能更好地分辨異常點云并加以抑制。

參考文獻(xiàn)：

［1］? 陶波， 趙興煒， 李汝鵬， 等. 機(jī)器人測量操作加工一體化技術(shù)研究及其應(yīng)用［J］. 中國機(jī)械工程， 2020， 31（1）：49-56.

TAO Bo， ZHAO Xingwei， LI Rupeng， et al. Research on Robotic Measurement-Operation-Machining Technology and Its Application［J］. China Mechanical Engineering， 2020， 31（1）：49-57.

［2］? WANG W B， GUO Q， YANG Z B， et al. A State-of-the-art Review on Robotic Milling of Complex Parts with High Efficiency and Precision［J］. Robotics and Computer-integrated Manufacturing， 2023，79：102436.

［3］? 朱大虎， 徐小虎， 蔣誠， 等. 復(fù)雜葉片機(jī)器人磨拋加工工藝技術(shù)研究進(jìn)展［J］. 航空學(xué)報， 2021， 42（10）：524265.

ZHU Dahu， XU Xiaohu， JIANG Cheng， et al. Research Progress in Robotic Grinding Technology for Complex Blades［J］. Acta Aeronautica et Astronautica Sinica， 2021， 42（10）：524265.

［4］? 李建微， 占家旺. 三維點云配準(zhǔn)方法研究進(jìn)展［J］. 中國圖象圖形學(xué)報， 2022， 27（2）：349-367.

LI Jianwei， ZHAN Jiawang. Review on 3D Point Cloud Registration Method［J］. Journal of Image and Graph， 2022， 27（2）：349-367.

［5］? BESJ P J， MCKAY H D. A Method for Registration of 3-D Shapes［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence， 1992， 14（2）：239-256.

［6］? YANG J L， LI H D， CAMPBELL D， et al. Go-ICP：a Globally Optimal Solution to 3D ICP Point-set Registration［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2016， 38（11）：2241-2254.

［7］? PAVLOV A L， OVCHINNIKOV G V， DERBYSHEV D Yu， et al. AA-ICP：Iterative Closest Point with Anderson Acceleration［C］∥ 2018 IEEE International Conference on Robotics and Automation（ICRA）. Brisbane， 2018：1-6.

［8］? FIEN B， ANDREA T， MARK P. Sparse Iterative Closest Point［J］. Computer Graphics Forum， 2013， 32（5）：113-123.

［9］? SERAFIN J， GRISETTI G. NICP：Dense Normal Based Point Cloud Registration［C］∥ 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）.Hamburg， 2015：742-749.

［10］? ZHANG J Y， YAO Y X， DENG B L. Fast and Robust Iterative Closest Point［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2021， 44（7）：3450-3466.

［11］? WANG R， GENG Z. WA-ICP Algorithm for Tackling Ambiguous Correspondence［C］∥ 2015 3rd IAPR Asian Conference on Pattern Recognition. Kuala Lumpur ，2015：76-80.

［12］? 王子瑋， 張小儉， 嚴(yán)思杰， 等. 一種稀疏混合ICP匹配方案［J］. 中國科學(xué)：技術(shù)科學(xué)， 2021， 51（7）：837-849.

WANG Ziwei， ZHANG Xiaojian， YAN Sijie， et al. Sparse Mixed ICP Registration Method［J］. scientia Sinica Technologica， 2021， 51（7）：837-849.

［13］? 林洪彬， 劉彬， 張玉存. 逆向工程中散亂點云變尺度配準(zhǔn)算法研究［J］. 機(jī)械工程學(xué)報， 2011， 47（14）：1-6.

LIN Hongbin， LIU Bin， ZHANG Yucun. Research on a Variable Scale Registration Algorithm for Scattered Point Clouds in Reverse Engineering［J］. Journal of Mechanical Engineering， 2011， 47（14）：1-6.

［14］? 張瑾賢， 吳曉峰， 葉才銘， 等. 一種基于點云場景分割與改進(jìn)配準(zhǔn)算法的物體位姿估計方法［J/OL］. 機(jī)械工程學(xué)報， 2023：1-10 .https：∥kns.cnki.net/kcms/detail/11.2187.TH.20230320.1645.026.html.

ZHANG Jinxian， WU Xiaofeng， YE Caiming， et al. An Object Pose Estimation Method Based on Point Cloud Scene Segmentation and Improved Registration［J/OL］. Journal of Mechanical Engineering， 2023：1-10. https：∥kns.cnki.net/kcms/detail/11.2187.TH.20230320.1645.026.html.

［15］? 孫殿柱， 郭洪帥， 李延瑞， 等. 基于局部泊松曲面重建的點云剛性配準(zhǔn)方法［J］. 機(jī)械工程學(xué)報， 2018， 54（15）：141-149.

SUN Dianzhu， GUO Hongshuai， LI Yanrui， et al. Method of Rigid Registration Based on Poisson Reconstruction of Local Sample Points［J］. Journal of Mechanical Engineering， 2018， 54（15）：141-149.

［16］? POTTMANN H， HUANG Q， YANG Y， et al. Geometry and Convergence Analysis of Algorithms for Registration of 3D Shapes［J］. International Journal of Computer Vision， 2006， 67（3）：277-296.

［17］? LI W L， YIN Z P， HUANG Y A， et al. Three-dimensional Point-based Shape Registration Algorithm Based on Adaptive Distance Function［J］. IET Computer Vision， 2011， 5（1）：68-76.

［18］? XIE H， LI W L， YIN Z P， et al. Variance-minimization Iterative Matching Method for Free-form Surfaces—Part Ⅰ：Theory and Method［J］. IEEE Transactions on Automation Science and Engineering， 2019， 16（3）：1181-1191.

［19］? XIE H， LI W L， YIN Z P， et al. Variance-minimization Iterative Matching Method for Free-form Surfaces—Part Ⅱ：Experiment and Analysis［J］. IEEE Transactions on Automation Science and Engineering， 2019， 16（3）：1192-1204.

［20］? LYU R， LIU H D， WANG Z J， et al. WPMAVM：Weighted Plus-and-minus Allowance Variance Minimization Algorithm for Solving Matching Distortion［J］. Robotics and Computer-integrated Manufacturing， 2022， 76：102320.

［21］? 吳浩， 馮曉志， 華林. 基于去偽加權(quán)方差最小化算法的大型復(fù)雜零部件局部配準(zhǔn)全局方法研究［J/OL］. 中國科學(xué)：技術(shù)科學(xué)， 2023：https：∥link.cnki.net/urlid/11.5844.TH.20230821.0859.004.

WU Hao， FENG Xiaozhi， HUA Lin. Local-to-global Registration Method of Large Complex Components Based on De-pseudo-weighted Variance Minimization Algorithm［J/OL］. Scientia Sinica Technologica， 2023：https：∥link.cnki.net/urlid/11.5844.TH.20230821.0859.004.

［22］? 熊有倫， 李文龍， 陳文斌， 等. 機(jī)器人學(xué)：建模，控制與視覺［M］. 武漢：華中科技大學(xué)出版社， 2020.

XIONG Youlun， LI Wenlong， CHEN Wenbin， et al. Robotics：Modelling， Control and Vision［M］. Wuhan：Huazhong University of Science and Technology Press， 2020.

［23］? 呂睿， 彭真， 呂遠(yuǎn)健， 等. 基于重定位的葉片機(jī)器人磨拋系統(tǒng)手眼標(biāo)定算法［J］. 中國機(jī)械工程， 2022， 33（3）：339-347.

LYU Rui， PENG Zhen， LYU Yuanjian， et al. Relocalization-based Hand-eye Calibration Algorithm for Blade Robotic Grinding Systems［J］. China Mechanical Engineering， 2022， 33（3）：339-347.

（編輯? 王艷麗）

作者簡介：

丁? 濤，男，2000年生，碩士研究生。研究方向為機(jī)器人測量-加工一體化。E-mail：dtao@whut.edu.cn。

朱大虎（通信作者），男，1983年生，教授、博士研究生導(dǎo)師。研究方向為視覺測量與機(jī)器人加工技術(shù)。E-mail：dhzhu@whut.edu.cn。