基于多體動力學建模的激光雷達誤差研究

蒼 鵬, 于 深, 王 震,2*

(1.長春工業大學 機電工程學院, 吉林 長春 130012;2.長春工業大學 工程訓練中心, 吉林 長春 130012)

1 多體動力學數學模型

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

圖1 車載掃描系統結構簡圖

將車載平臺的基座、三個軸系及激光雷達視為一個多體結構,每個組成部分都視為剛體,并建立剛體坐標系。

在多體動力學理論中需要將系統中所有剛體的參數轉換到同一個坐標系再進行測量。在多體動力學模型中,主要有剛體坐標系和整體坐標系[8-9]。整體坐標系不會隨時間產生變化。而在剛體坐標系中,每個剛體的平動和轉動都會隨時間變化而變化。

1.1 約束方程

將車載激光雷達掃描系統簡化為n個剛體,建立對應數量的廣義坐標來描述此系統,用q表示為

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

(1)

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

假設在一個系統中有m1個獨立的完整約束,定義一個具有廣義坐標的非線性函數

φ(q,t)=0,

其中

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

然后對式(1)兩端求一階導數,得到該系統速度的約束方程為

(2)

將式(2)兩端繼續求導,可得系統加速度方程為

(3)

(4)

式中:φq----非線性函數φ對廣義坐標q的一階偏導數,這是一個m1×n階的矩陣,公式為

(5)

式中:φt----φi對時間t的導數;

φq----m1階方陣;

φqt----對廣義坐標q和時間t求二階偏導得到的一個矩陣。

假設該系統具有m2個不完整約束為

(6)

不完整約束以及廣義速度二者的線性關系為

(7)

(8)

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

(9)

(10)

(11)

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

(12)

(13)

1.2 獨立剛體動力學方程

在OXYZ慣性坐標系中,oixiyizi是空間中一個剛體對應的局部坐標系,每個坐標系的圓心Oi與空間中各個剛體的質心Ci重合。用ri表示局部坐標系oixiyizi相對空間中慣性坐標系OXYZ的平動四元數矩陣,θi表示的轉動四元數矩陣為

ri=[xiyizi]T,

(14)

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

(15)

則剛體的廣義坐標為

(16)

根據柯尼希定理,推導出剛體動能公式為

(17)

式中:m----系統中運動的獨立剛體的質量;

r----剛體在慣性坐標系OXYZ中的位置;

ω----剛體運動時的角速度;

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

根據歐拉四元數的性質、角速度矢量與歐拉四元數及其導數的關系為

(18)

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

(19)

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

(20)

對式(20)進行求導,得

(21)

(22)

(23)

(24)

(25)

(26)

(27)

剛體約束方程為

(28)

系統約束方程對ri和θi兩個參數的雅可比矩陣可以表示為

(29)

(30)

根據第一類拉格朗日方程可以推導出

(31)

(32)

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

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

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

綜上,可以推導出物體i在四元數空間的動力學方程為

(33)

寫成矩陣形式為

(34)

其中

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

mi----第i個剛體的質量;

1.3 建立多體系統動力學方程

由多個剛體組成的多體系統的通用公式為

(35)

其中

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

(36)

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

(37)

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

(38)

(39)

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

(40)

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

根據多體運動學原理,建立掃描系統數學模型,其中,規定大地作為這個系統的0體,并設定其為一個低序體。在車載雷達掃描系統中,所有剛體運動都可以歸結到0體上,得到系統中任意剛體在大地坐標系中的位置坐標和運動關系。以3和4兩個剛體為例,二者的運動關系如圖2所示。

圖2 剛體3和剛體4的運動關系示意圖

圖中符號:

O3----剛體3實際位置;

O4----剛體4實際位置;

O4 m----剛體4運動實際位置;

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

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

s4----剛體4運動矢量;

s4e----剛體4運動誤差矢量。

通常情況,實現兩相鄰剛體變換的矩陣為

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

(41)

式中:[T]p----剛體運動理想位置矢量;

[T]pe----剛體運動位置誤差矢量;

[T]s----剛體運動矢量;

[T]se----運動誤差矢量。

車載掃描設備在大地坐標系內實際位置為

(42)

2 激光雷達掃描系統誤差分析

2.1 車載激光雷達掃描系統幾何誤差分析

2.1.1 軸系回轉誤差

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

2.1.2 垂直度誤差

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

2.1.3 相交度誤差

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

2.1.4 設備安裝誤差

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

文中討論車載掃描系統中的軸系回轉誤差是系統運動誤差的一種,其余三種誤差均屬于系統幾何誤差。

2.2 車載激光雷達掃描系統裝配誤差分析

根據多體動力學原理,在車載掃描系統上建立相應的地面坐標系,將方位軸簡化后建立方位軸坐標系,將俯仰軸簡化后建立俯仰軸坐標系,橫傾軸簡化后得到橫傾軸坐標系,把激光雷達自身視為一個剛體建立一個坐標系,如圖3所示。

圖3 各個剛體坐標系及其相互關系示意圖

以大地坐標系原點O0為基點,三個方位軸的轉角分別為α、β和γ。

2.2.1 大地坐標系

大地坐標系設為坐標系0,其原點是O0,該點在基座所在平面中心。將方位軸坐標系設定為坐標系1,俯仰軸坐標系設為坐標系2,橫傾軸坐標系設為坐標系3,激光雷達設定為坐標系4。

2.2.2 方位軸坐標系

2.2.3 俯仰軸坐標系

2.2.4 橫傾軸坐標系

2.2.5 激光雷達自身坐標系

在安裝雷達設備時會出現裝配誤差,這類誤差存在于設備的各個自由度中,且各不相同,難以通過技術手段進行消除。

3 建立誤差模型

通過建立各個坐標系之間相互轉換的模型,可以導出掃描系統中雷達自身所處的坐標系與大地坐標系二者相互變換的轉換矩陣為

(43)

(44)

其中

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

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

(45)

由式(45)可以獲得系統所需精度,將其代入已經給定的齊次坐標系內計算,可得到激光雷達的整體誤差數據。

當qr=[0,0,1,0]T時,如果不計誤差角度對系統的影響,則可以不將各轉軸產生的回轉誤差以及設備安裝時產生的安裝誤差考慮在內,指向誤差矢量為

(46)

當激光雷達中心點qr=[0,0,0]T時,如果不計算回轉軸系的誤差,則誤差矢量為

(47)

4 誤差仿真分析

將激光雷達掃描系統中所有誤差源利用多體動力學原理建立數學模型,評估各個誤差源對點云數據精度的影響。將系統的各個剛體坐標統一轉換到大地坐標系中,利用控制變量法對每個參數進行分析,使用Matlab軟件進行仿真運算,從而獲得各系統單方面誤差影響。

4.1 垂直度誤差的影響

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

系統指向誤差系統指向誤差

4.2 相交度誤差的影響

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

系統指向誤差系統指向誤差

4.3 回轉誤差的影響

系統中傾角誤差對定位誤差的影響如圖6所示。

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

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

通過仿真實驗結果可以得出,橫傾軸與俯仰軸共同決定系統的水平度,是該系統的主要誤差來源。而方位軸決定該系統做旋轉運動時的回轉角度。

5 結 語

根據多體系統動力學原理,將車載掃描系統中基座、橫傾軸系、俯仰軸系、方位軸系和激光雷達自身坐標系簡化成5個剛體。通過尋找剛體間的關系,將掃描系統簡化為一個多體結構,建立系統模型。綜合分析多體系統中各種誤差的來源,并使用Matlab軟件進行仿真運算,得出垂直度、相交度和回轉誤差對車載掃描系統的誤差影響。