基于多體動力學(xué)建模的激光雷達(dá)誤差研究

蒼 鵬, 于 深, 王 震,2*

(1.長春工業(yè)大學(xué) 機(jī)電工程學(xué)院, 吉林 長春 130012;2.長春工業(yè)大學(xué) 工程訓(xùn)練中心, 吉林 長春 130012)

1 多體動力學(xué)數(shù)學(xué)模型

對建筑物的掃描通常采用車載激光雷達(dá)進(jìn)行掃描,該系統(tǒng)主要包含激光雷達(dá)和車載平臺[1-3]。其中車載平臺由基座、控制設(shè)備掃描方位的方位軸框系、維持設(shè)備橫傾角度的橫傾軸框系、即時調(diào)整設(shè)備工作俯仰角度的俯仰軸框系組成[4-7],如圖1所示。

圖1 車載掃描系統(tǒng)結(jié)構(gòu)簡圖

將車載平臺的基座、三個軸系及激光雷達(dá)視為一個多體結(jié)構(gòu),每個組成部分都視為剛體,并建立剛體坐標(biāo)系。

在多體動力學(xué)理論中需要將系統(tǒng)中所有剛體的參數(shù)轉(zhuǎn)換到同一個坐標(biāo)系再進(jìn)行測量。在多體動力學(xué)模型中,主要有剛體坐標(biāo)系和整體坐標(biāo)系[8-9]。整體坐標(biāo)系不會隨時間產(chǎn)生變化。而在剛體坐標(biāo)系中,每個剛體的平動和轉(zhuǎn)動都會隨時間變化而變化。

1.1 約束方程

將車載激光雷達(dá)掃描系統(tǒng)簡化為n個剛體,建立對應(yīng)數(shù)量的廣義坐標(biāo)來描述此系統(tǒng),用q表示為

q=[q1,q2,…,qn]T。

(1)

一個完整系統(tǒng)應(yīng)當(dāng)具有完整的約束條件,其約束方程的定義域是有界的,可以完整地表達(dá)位移、速度等參數(shù)。如果約束方程定義域是無界或包含速度參數(shù),則稱為不完整約束方程,對應(yīng)的系統(tǒng)是不完整的。

假設(shè)在一個系統(tǒng)中有m1個獨(dú)立的完整約束,定義一個具有廣義坐標(biāo)的非線性函數(shù)

φ(q,t)=0,

其中

φ=(φ1,φ2,…,φm1)T。

然后對式(1)兩端求一階導(dǎo)數(shù),得到該系統(tǒng)速度的約束方程為

(2)

將式(2)兩端繼續(xù)求導(dǎo),可得系統(tǒng)加速度方程為

(3)

(4)

式中:φq----非線性函數(shù)φ對廣義坐標(biāo)q的一階偏導(dǎo)數(shù),這是一個m1×n階的矩陣,公式為

(5)

式中:φt----φi對時間t的導(dǎo)數(shù);

φq----m1階方陣;

φqt----對廣義坐標(biāo)q和時間t求二階偏導(dǎo)得到的一個矩陣。

假設(shè)該系統(tǒng)具有m2個不完整約束為

(6)

不完整約束以及廣義速度二者的線性關(guān)系為

(7)

(8)

將式(2)、式(6)～式(8)聯(lián)立,得到車載激光雷達(dá)掃描系統(tǒng)總速度和加速度約束方程為

(9)

(10)

(11)

式(10)和式(11)是該系統(tǒng)約束的雅可比矩陣,是一個(m1+m2)×n階的矩陣,公式為

(12)

(13)

1.2 獨(dú)立剛體動力學(xué)方程

在OXYZ慣性坐標(biāo)系中,oixiyizi是空間中一個剛體對應(yīng)的局部坐標(biāo)系,每個坐標(biāo)系的圓心Oi與空間中各個剛體的質(zhì)心Ci重合。用ri表示局部坐標(biāo)系oixiyizi相對空間中慣性坐標(biāo)系OXYZ的平動四元數(shù)矩陣,θi表示的轉(zhuǎn)動四元數(shù)矩陣為

ri=[xiyizi]T,

(14)

θi=[θi0θi1θi2θi3]T,

(15)

則剛體的廣義坐標(biāo)為

(16)

根據(jù)柯尼希定理,推導(dǎo)出剛體動能公式為

(17)

式中:m----系統(tǒng)中運(yùn)動的獨(dú)立剛體的質(zhì)量;

r----剛體在慣性坐標(biāo)系OXYZ中的位置;

ω----剛體運(yùn)動時的角速度;

I----剛體的慣性張量。

根據(jù)歐拉四元數(shù)的性質(zhì)、角速度矢量與歐拉四元數(shù)及其導(dǎo)數(shù)的關(guān)系為

(18)

式中:L----右變換矩陣,公式為

(19)

將式(19)代入式(18),得

(20)

對式(20)進(jìn)行求導(dǎo),得

(21)

(22)

(23)

(24)

(25)

(26)

(27)

剛體約束方程為

(28)

系統(tǒng)約束方程對ri和θi兩個參數(shù)的雅可比矩陣可以表示為

(29)

(30)

根據(jù)第一類拉格朗日方程可以推導(dǎo)出

(31)

(32)

式中:φr,φθ----分別代表約束方程對坐標(biāo)r和坐標(biāo)θ的雅可比矩陣;

Qr,Qθ----分別為對位置r和θ的剛體產(chǎn)生的廣義作用力;

λ----拉格朗日乘子。

綜上,可以推導(dǎo)出物體i在四元數(shù)空間的動力學(xué)方程為

(33)

寫成矩陣形式為

(34)

其中

式中:E3----對角矩陣,對角線上的元素均為常數(shù);

mi----第i個剛體的質(zhì)量;

1.3 建立多體系統(tǒng)動力學(xué)方程

由多個剛體組成的多體系統(tǒng)的通用公式為

(35)

其中

M=diag(M1,M2,…,MN),

(36)

q=[(r1T,θ1T),…,(rNT,θNT)]T,

(37)

φq=[φq1,φq2,…,φqn],

(38)

(39)

將式(33)與該系統(tǒng)約束方程聯(lián)立,得到多體系統(tǒng)微分方程組為

(40)

在激光雷達(dá)掃描多體系統(tǒng)中,主要由車載平臺中三個軸系產(chǎn)生回轉(zhuǎn)運(yùn)動,而各剛體之間沒有平移運(yùn)動。所以車載掃描系統(tǒng)的裝配誤差表現(xiàn)在車載雷達(dá)的安裝位置誤差、搭載雷達(dá)的平臺各軸系運(yùn)動產(chǎn)生的回轉(zhuǎn)誤差、不同軸間的垂直度誤差和相交度誤差。

根據(jù)多體運(yùn)動學(xué)原理,建立掃描系統(tǒng)數(shù)學(xué)模型,其中,規(guī)定大地作為這個系統(tǒng)的0體,并設(shè)定其為一個低序體。在車載雷達(dá)掃描系統(tǒng)中,所有剛體運(yùn)動都可以歸結(jié)到0體上,得到系統(tǒng)中任意剛體在大地坐標(biāo)系中的位置坐標(biāo)和運(yùn)動關(guān)系。以3和4兩個剛體為例,二者的運(yùn)動關(guān)系如圖2所示。

圖2 剛體3和剛體4的運(yùn)動關(guān)系示意圖

圖中符號:

O3----剛體3實(shí)際位置;

O4----剛體4實(shí)際位置;

O4 m----剛體4運(yùn)動實(shí)際位置;

p4----剛體4理想位置矢量;

p4e----剛體4位置誤差矢量;

s4----剛體4運(yùn)動矢量;

s4e----剛體4運(yùn)動誤差矢量。

通常情況,實(shí)現(xiàn)兩相鄰剛體變換的矩陣為

T=[T]p[T]pe[T]s[T]se,

(41)

式中:[T]p----剛體運(yùn)動理想位置矢量;

[T]pe----剛體運(yùn)動位置誤差矢量;

[T]s----剛體運(yùn)動矢量;

[T]se----運(yùn)動誤差矢量。

車載掃描設(shè)備在大地坐標(biāo)系內(nèi)實(shí)際位置為

(42)

2 激光雷達(dá)掃描系統(tǒng)誤差分析

2.1 車載激光雷達(dá)掃描系統(tǒng)幾何誤差分析

2.1.1 軸系回轉(zhuǎn)誤差

車載激光雷達(dá)掃描系統(tǒng)有三種回轉(zhuǎn)軸線,即理想、實(shí)際、瞬時回轉(zhuǎn)軸線。軸系的回轉(zhuǎn)誤差也是由三個方面構(gòu)成,分別用Δs、Δc0以及Δγ來表示。回轉(zhuǎn)誤差還可以被繼續(xù)分解成純徑向的誤差Δx(t)與Δy(t)、軸向發(fā)生竄動的誤差Δz(t)、傾角回轉(zhuǎn)誤差Δα(t)和Δβ(t)。由于工況各異,所以對誤差的控制要求也存在差別。

2.1.2 垂直度誤差

在空間中有兩條相交軸線,它們之間夾角與直角的差異稱為垂直度。垂直度主要包含瞬時垂直度和平均垂直度,而后者在工程中更為常用。經(jīng)過試驗(yàn)可以得到結(jié)論,平均垂直度和兩個軸的位置及晃動量都對兩個軸的軸線之間的瞬時垂直度有較大影響。

2.1.3 相交度誤差

兩個相互垂直的軸系之間的相交度同樣能夠影響到掃描精度。用兩個軸的軸線在空間中的公垂線的長度表示軸系之間的相交度。這一類誤差常常在各個軸的平均回轉(zhuǎn)軸線中間產(chǎn)生。而且運(yùn)動中的軸的轉(zhuǎn)角位置和一些輕微晃動不會對相交度誤差產(chǎn)生影響。相交度誤差應(yīng)當(dāng)保持小于0.5 mm才能達(dá)到建筑物重建工作所需精度。

2.1.4 設(shè)備安裝誤差

為保證激光雷達(dá)能夠達(dá)到掃描精度,每個激光雷達(dá)的定位信息初始化標(biāo)定通常在出廠時便已完成。安裝設(shè)備時,對雷達(dá)的軸線與車載平臺中的方位軸的同軸度通常會有一定要求。實(shí)際上,即使采用高精度的測量手段,也很難精確地將這種裝配誤差測量出來。

文中討論車載掃描系統(tǒng)中的軸系回轉(zhuǎn)誤差是系統(tǒng)運(yùn)動誤差的一種,其余三種誤差均屬于系統(tǒng)幾何誤差。

2.2 車載激光雷達(dá)掃描系統(tǒng)裝配誤差分析

根據(jù)多體動力學(xué)原理,在車載掃描系統(tǒng)上建立相應(yīng)的地面坐標(biāo)系,將方位軸簡化后建立方位軸坐標(biāo)系,將俯仰軸簡化后建立俯仰軸坐標(biāo)系,橫傾軸簡化后得到橫傾軸坐標(biāo)系,把激光雷達(dá)自身視為一個剛體建立一個坐標(biāo)系,如圖3所示。

圖3 各個剛體坐標(biāo)系及其相互關(guān)系示意圖

以大地坐標(biāo)系原點(diǎn)O0為基點(diǎn),三個方位軸的轉(zhuǎn)角分別為α、β和γ。

2.2.1 大地坐標(biāo)系

大地坐標(biāo)系設(shè)為坐標(biāo)系0,其原點(diǎn)是O0,該點(diǎn)在基座所在平面中心。將方位軸坐標(biāo)系設(shè)定為坐標(biāo)系1,俯仰軸坐標(biāo)系設(shè)為坐標(biāo)系2,橫傾軸坐標(biāo)系設(shè)為坐標(biāo)系3,激光雷達(dá)設(shè)定為坐標(biāo)系4。

2.2.2 方位軸坐標(biāo)系

2.2.3 俯仰軸坐標(biāo)系

2.2.4 橫傾軸坐標(biāo)系

2.2.5 激光雷達(dá)自身坐標(biāo)系

在安裝雷達(dá)設(shè)備時會出現(xiàn)裝配誤差,這類誤差存在于設(shè)備的各個自由度中,且各不相同,難以通過技術(shù)手段進(jìn)行消除。

3 建立誤差模型

通過建立各個坐標(biāo)系之間相互轉(zhuǎn)換的模型,可以導(dǎo)出掃描系統(tǒng)中雷達(dá)自身所處的坐標(biāo)系與大地坐標(biāo)系二者相互變換的轉(zhuǎn)換矩陣為

(43)

(44)

其中

k=1,2,3;l=1,3;m=2;n=1,2,3,4。

如果只考慮設(shè)備自身,假設(shè)一個矢量r的齊次坐標(biāo)qr,并將其代入下式,利用插值算法來求出空間中任意點(diǎn)的定位誤差。

(45)

由式(45)可以獲得系統(tǒng)所需精度,將其代入已經(jīng)給定的齊次坐標(biāo)系內(nèi)計(jì)算,可得到激光雷達(dá)的整體誤差數(shù)據(jù)。

當(dāng)qr=[0,0,1,0]T時,如果不計(jì)誤差角度對系統(tǒng)的影響,則可以不將各轉(zhuǎn)軸產(chǎn)生的回轉(zhuǎn)誤差以及設(shè)備安裝時產(chǎn)生的安裝誤差考慮在內(nèi),指向誤差矢量為

(46)

當(dāng)激光雷達(dá)中心點(diǎn)qr=[0,0,0]T時,如果不計(jì)算回轉(zhuǎn)軸系的誤差,則誤差矢量為

(47)

4 誤差仿真分析

將激光雷達(dá)掃描系統(tǒng)中所有誤差源利用多體動力學(xué)原理建立數(shù)學(xué)模型,評估各個誤差源對點(diǎn)云數(shù)據(jù)精度的影響。將系統(tǒng)的各個剛體坐標(biāo)統(tǒng)一轉(zhuǎn)換到大地坐標(biāo)系中,利用控制變量法對每個參數(shù)進(jìn)行分析,使用Matlab軟件進(jìn)行仿真運(yùn)算,從而獲得各系統(tǒng)單方面誤差影響。

4.1 垂直度誤差的影響

不同垂直度誤差對系統(tǒng)定位誤差影響如圖4所示。

系統(tǒng)指向誤差系統(tǒng)指向誤差

4.2 相交度誤差的影響

相交度對定位誤差影響如圖5所示。

系統(tǒng)指向誤差系統(tǒng)指向誤差

4.3 回轉(zhuǎn)誤差的影響

系統(tǒng)中傾角誤差對定位誤差的影響如圖6所示。

(a) Δβ2=10′系統(tǒng)指向誤差

回轉(zhuǎn)誤差對系統(tǒng)的影響與垂直度誤差相似,當(dāng)Δβ2=10′時,定位誤差影響見圖6(a);圖6(b)表示Δγ2=10′時,定位誤差受到的影響。通過對比可知,其他傾角造成的誤差對系統(tǒng)的影響與Δβ2相比較小。

通過仿真實(shí)驗(yàn)結(jié)果可以得出,橫傾軸與俯仰軸共同決定系統(tǒng)的水平度,是該系統(tǒng)的主要誤差來源。而方位軸決定該系統(tǒng)做旋轉(zhuǎn)運(yùn)動時的回轉(zhuǎn)角度。

5 結(jié) 語

根據(jù)多體系統(tǒng)動力學(xué)原理,將車載掃描系統(tǒng)中基座、橫傾軸系、俯仰軸系、方位軸系和激光雷達(dá)自身坐標(biāo)系簡化成5個剛體。通過尋找剛體間的關(guān)系,將掃描系統(tǒng)簡化為一個多體結(jié)構(gòu),建立系統(tǒng)模型。綜合分析多體系統(tǒng)中各種誤差的來源,并使用Matlab軟件進(jìn)行仿真運(yùn)算,得出垂直度、相交度和回轉(zhuǎn)誤差對車載掃描系統(tǒng)的誤差影響。