激光雷達(dá)數(shù)學(xué)模型及運(yùn)動(dòng)畸變?nèi)コ?/h1>

2021-11-29 02:20:44郁漢琪陸宇豪

儀器儀表用戶訂閱 2021年11期 收藏

關(guān)鍵詞：里程計(jì)輪式位姿

黃 越，郁漢琪，陸宇豪

（南京工程學(xué)院 機(jī)械工程學(xué)院，南京 211100）

激光雷達(dá)作為SLAM系統(tǒng)的主要傳感器，影響定位準(zhǔn)確度與地圖構(gòu)建效果。

1 激光雷達(dá)傳感器介紹

1.1 激光雷達(dá)常用測(cè)距原理

對(duì)于市面上的主流激光雷達(dá)，主要是用于環(huán)境探測(cè)、地圖構(gòu)建，按技術(shù)路線可分為：三角測(cè)距激光雷達(dá)，TOF激光雷達(dá)。

TOF測(cè)距是通過測(cè)量光的飛行時(shí)間來計(jì)算距離。根據(jù)發(fā)射激光信號(hào)的不同形式，TOF測(cè)距法可分為脈沖測(cè)距和相位測(cè)距[1]。脈沖激光測(cè)距基本原理是激光器發(fā)射激光脈沖，計(jì)時(shí)器記錄發(fā)射時(shí)間T1；接收器接收被反射的激光，計(jì)時(shí)器記錄接收時(shí)間T2。兩個(gè)時(shí)間的間隔△T即為激光的“飛行時(shí)間”。光速c是一定的，因此距離可由速度與時(shí)間計(jì)算得到。相位測(cè)距是將發(fā)射的連續(xù)的激光進(jìn)行幅度調(diào)制（調(diào)制光的光強(qiáng)隨時(shí)間做周期性變化）[2]，被反射后，激光產(chǎn)生相位變化，根據(jù)相位差來間接測(cè)量時(shí)間和距離（曲線測(cè)距）[3]。

三角測(cè)距根據(jù)攝像頭的光斑成像位置，利用相似三角形的幾何關(guān)系計(jì)算距離。雙目視覺、結(jié)構(gòu)光測(cè)距等，都可歸納為三角測(cè)距原理。激光器發(fā)射激光，激光照射到物體表面后發(fā)生反射；基線另一端的CCD相機(jī)接收反射信號(hào)，并記錄入射光與反射光的夾角。依照光學(xué)路徑，不同距離的物體會(huì)顯示在CCD上的不同位置。因此，在已知激光源和CCD間距離的情況下，根據(jù)相似三角形的幾何關(guān)系[4]，就可以推導(dǎo)出激光源與物體表面的距離。

1.2 激光雷達(dá)數(shù)學(xué)模型介紹

1）光束模型

激光雷達(dá)的光束模型需要第t時(shí)刻機(jī)器人的位姿xt和地圖m，來獲得第t時(shí)刻激光數(shù)據(jù)，包含K個(gè)激光束測(cè)量的距離值z(mì)t。在非結(jié)構(gòu)化環(huán)境中機(jī)器人位姿微小的變化就會(huì)造成期望值巨大的改變，從而導(dǎo)致得分進(jìn)行突變，因此光束模型很少有應(yīng)用場(chǎng)景。

2）似然場(chǎng)模型

似然場(chǎng)模型是對(duì)圖像進(jìn)行高斯平滑，在任何環(huán)境中期望值對(duì)于位姿都是平滑的。似然場(chǎng)得分的技術(shù)不需要經(jīng)過光線追蹤，而是通過高斯分布計(jì)算出一個(gè)得分換成查表的形式。似然場(chǎng)模型從程序一開始就是在進(jìn)行離線計(jì)算，所以對(duì)一幀激光來說，只需要查360次表，同時(shí)適合結(jié)構(gòu)化環(huán)境和非結(jié)構(gòu)化環(huán)境中[5]。

2 激光雷達(dá)運(yùn)動(dòng)畸變

2.1 運(yùn)動(dòng)畸變產(chǎn)生的原因

運(yùn)動(dòng)畸變是由于激光雷達(dá)在采集數(shù)據(jù)過程中，隨著載體運(yùn)動(dòng)，導(dǎo)致一幀激光數(shù)據(jù)中的激光點(diǎn)不是在同一時(shí)刻采集的，即不同激光點(diǎn)的坐標(biāo)不一致。在靜止場(chǎng)景中，即激光雷達(dá)載體本身靜止且環(huán)境中沒有運(yùn)動(dòng)物體，那么采集到的一幀點(diǎn)云沒有畸變。在運(yùn)動(dòng)場(chǎng)景中，即激光雷達(dá)載體運(yùn)動(dòng)或者環(huán)境中存在運(yùn)動(dòng)物體，采集到的點(diǎn)云數(shù)據(jù)有畸變現(xiàn)象。采集一幀數(shù)據(jù)起始點(diǎn)的雷達(dá)原點(diǎn)位置和采集一幀數(shù)據(jù)終止點(diǎn)時(shí)的雷達(dá)原點(diǎn)位置出現(xiàn)了偏移，三維環(huán)境信息出現(xiàn)了畸變。

由于雷達(dá)計(jì)算激光點(diǎn)坐標(biāo)時(shí)，都是以接收到激光束時(shí)刻的雷達(dá)自身坐標(biāo)系為基礎(chǔ)的，所以載體運(yùn)動(dòng)過程中，每一列激光點(diǎn)的基準(zhǔn)坐標(biāo)系都是不一樣的，但是他們?cè)谕粠c(diǎn)云里，希望能統(tǒng)一在同一個(gè)坐標(biāo)系下，所以需要知道每次采集時(shí)的坐標(biāo)系相對(duì)于初始時(shí)刻坐標(biāo)系的轉(zhuǎn)換關(guān)系。

2.2 運(yùn)動(dòng)畸變的去除

點(diǎn)云的畸變是由于一幀數(shù)據(jù)的采集過程中，雷達(dá)載體進(jìn)行運(yùn)動(dòng)導(dǎo)致，因而需要先計(jì)算采集過程中雷達(dá)的運(yùn)動(dòng)，然后在每幀中根據(jù)每個(gè)點(diǎn)的相對(duì)時(shí)間進(jìn)行補(bǔ)償這個(gè)運(yùn)動(dòng)量，包括旋轉(zhuǎn)和平移的補(bǔ)償。

去除激光雷達(dá)運(yùn)動(dòng)畸變的方法有包括ICP（Iterative Closest Point）、VICP（ Velocity Estimation Iterative Closest Point）的純估計(jì)方法和傳感器的輔助方法。ICP方法沒有考慮到激光的運(yùn)動(dòng)畸變，以及當(dāng)前激光數(shù)據(jù)錯(cuò)誤問題；VICP法忽略了低頻率激光雷達(dá)掃描對(duì)小車勻速運(yùn)動(dòng)的假設(shè)不成立，以及數(shù)據(jù)預(yù)處理和狀態(tài)估計(jì)過程耦合的問題。針對(duì)ICP和VICP方法的缺點(diǎn)，主要介紹傳感器的輔助方法以及提出輪式方法與匹配方法進(jìn)行融合從而去除運(yùn)動(dòng)畸變。

3 傳感器輔助方法

傳感器輔助方法包括慣性測(cè)量單元（IMU）輔助方法及輪式里程計(jì)輔助方法。慣性測(cè)量單元輔助方法是直接測(cè)量機(jī)器人的角速度和線加速度，具有較高的角速度測(cè)量精度[6]，但測(cè)量頻率極高，一般在1kHz～8kHz，并且線加速度精度太差，二次積分在局部的精度依然很差。輪式里程計(jì)直接測(cè)量機(jī)器人的位移和角度，具有較高的局部角度測(cè)量精度[7]，與IMU輔助方法相比，更新速度更高，一般在 100kHz～ 200kHz。

輪式里程計(jì)方法的原理是求解當(dāng)前幀激光數(shù)據(jù)中每一個(gè)激光點(diǎn)對(duì)應(yīng)的機(jī)器人位姿，根據(jù)求解的位姿把所有激光點(diǎn)轉(zhuǎn)換到同一坐標(biāo)系下，重新封裝成一幀激光數(shù)據(jù)發(fā)布出去。

已知當(dāng)前幀激光起始時(shí)間ts，結(jié)束時(shí)間te，兩個(gè)激光束之間的時(shí)間間隔為Δt。輪式里程輔助算法步驟如下：

1）求解 ：ts，te時(shí)刻的位姿ps，pe

里程計(jì)隊(duì)列正好和激光數(shù)據(jù)同步，假設(shè)第i和第j個(gè)數(shù)據(jù)的時(shí)刻分布為ts，te：

在ts時(shí)刻沒有對(duì)應(yīng)的里程計(jì)位姿，則進(jìn)行線性插值。設(shè)在l，k時(shí)刻有位姿，且l＜s＜k，則 ：

2）二次插值

在一幀激光數(shù)據(jù)之間，認(rèn)為機(jī)器人做勻加速運(yùn)動(dòng)。機(jī)器人的位姿是關(guān)于時(shí)間t的二次函數(shù)，設(shè)：tm=(ts+te)/2，且l＜m＜k，則 ：

已知：ps，pm，pe，則可以插值一條二次曲線：

3）二次曲線的近似

用分段線性函數(shù)對(duì)二次函數(shù)進(jìn)行近似，分段數(shù)大于3時(shí)近似誤差可忽略不計(jì)。在ts和te時(shí)間段內(nèi)一共取m個(gè)位姿{(lán)ps,ps+1,…,ps+m-2,pe}，位姿通過線性插值獲取，在這m個(gè)位姿之間，進(jìn)行線性插值：

設(shè)ps和ps+1之間有n個(gè)位姿{(lán)ps,ps1,…,ps(n-2),ps+1}，假設(shè)si在l和k之間，則：

4）坐標(biāo)系統(tǒng)轉(zhuǎn)換與激光數(shù)據(jù)發(fā)布

一幀激光數(shù)據(jù)K個(gè)激光點(diǎn)，每個(gè)激光點(diǎn)對(duì)應(yīng)的位姿{(lán)p1,p2,…,pk}，通過上述的方法插值得到。xi為轉(zhuǎn)換之前的坐標(biāo)，為轉(zhuǎn)換之后的坐標(biāo)，則：

把轉(zhuǎn)換后的坐標(biāo)轉(zhuǎn)換為激光數(shù)據(jù)發(fā)布出去：

4 輪式方法與匹配方法的融合

4.1 融合方法思想

本文提出了將輪式里程計(jì)方法與ICP方法進(jìn)行融合，其主要思想如下：

1）用里程計(jì)方法進(jìn)行矯正，去除絕大部分的運(yùn)動(dòng)畸變。

2）認(rèn)為里程計(jì)存在誤差，但是誤差值是線性分布的。

3）用ICP方法進(jìn)行匹配，匹配的結(jié)果作為正確值，得到里程計(jì)的誤差值。

4）把誤差值均攤到每一個(gè)點(diǎn)上，重新進(jìn)行激光點(diǎn)位置修正。

5）再一次進(jìn)行ICP迭代，直到收斂為止。

4.2 實(shí)驗(yàn)結(jié)果分析

4.2.1 激光雷達(dá)性能參數(shù)

試驗(yàn)使用的激光雷達(dá)選用思嵐RPLIDAR A2型號(hào)，其性能參數(shù)見表1。

表1 思嵐RPLIDAR A2性能參數(shù)Table 1 RPLIDAR A2 performance parameters

4.2.2 激光雷達(dá)去畸實(shí)驗(yàn)

為了驗(yàn)證激光雷達(dá)運(yùn)動(dòng)畸變?nèi)コ惴▽?duì)同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）系統(tǒng)在實(shí)際環(huán)境中的影響[8]，在機(jī)器人系統(tǒng)ROS平臺(tái)中采用兩種算法構(gòu)建環(huán)境地圖，在可視化界面RVIZ中顯示環(huán)境地圖。設(shè)置智能移動(dòng)底盤移動(dòng)的線速度為0.5 m/s。

兩種算法所建地圖如圖1、圖2所示。

從圖中可以明顯看出，當(dāng)移動(dòng)底盤發(fā)生運(yùn)動(dòng)時(shí)，激光雷達(dá)所掃描的點(diǎn)云數(shù)據(jù)發(fā)生了畸變現(xiàn)象。比較圖1與圖2可以看出，采用輪式里程計(jì)與ICP融合的方法去除激光雷達(dá)產(chǎn)生運(yùn)動(dòng)畸變的效果更好，所建地圖精度更高。通過比較兩種算法的建圖效果，提出的輪式里程計(jì)與ICP融合方法提高了系統(tǒng)構(gòu)建環(huán)境地圖的精度。

圖1 輔助方法的建圖效果Fig.1 Mapping effect of the auxiliary method

圖2 輪式里程計(jì)和ICP融合方法的建圖效果Fig.2 Mapping effect of wheel odometer and ICP fusion method

5 結(jié)論

本文介紹了激光雷達(dá)的測(cè)距原理以及數(shù)學(xué)模型，并根據(jù)載體運(yùn)動(dòng)時(shí)激光雷達(dá)采集數(shù)據(jù)過程中產(chǎn)生的運(yùn)動(dòng)畸變，詳述了傳感器輔助方法去除激光雷達(dá)運(yùn)動(dòng)畸變的原理。在此基礎(chǔ)上，針對(duì)輪式里程計(jì)輔助方法的不足，提出了輪式里程計(jì)和ICP 融合的方法。當(dāng)智能底盤在室內(nèi)移動(dòng)時(shí)，分別使用兩種算法進(jìn)行構(gòu)建環(huán)境地圖。實(shí)驗(yàn)結(jié)果表明，輪式里程計(jì)和 ICP 融合的方法所構(gòu)建的地圖精度更高。

猜你喜歡

里程計(jì)輪式位姿

室內(nèi)退化場(chǎng)景下UWB雙基站輔助LiDAR里程計(jì)的定位方法

導(dǎo)航定位學(xué)報(bào)(2022年5期)2022-10-13 09:11:28

輪式裝備搶救搶修車

軍民兩用技術(shù)與產(chǎn)品(2021年10期)2021-03-16 06:05:36

一種單目相機(jī)/三軸陀螺儀/里程計(jì)緊組合導(dǎo)航算法

中國(guó)慣性技術(shù)學(xué)報(bào)(2019年1期)2019-05-21 00:58:30

對(duì)輪式正面吊輪胎使用

經(jīng)濟(jì)技術(shù)協(xié)作信息(2018年7期)2019-01-14 03:05:40

基于模板特征點(diǎn)提取的立體視覺里程計(jì)實(shí)現(xiàn)方法

傳感器與微系統(tǒng)(2018年7期)2018-08-29 00:44:28

高通過性輪式無人機(jī)平臺(tái)誕生記

中國(guó)軍轉(zhuǎn)民(2018年1期)2018-06-08 05:52:57

基于共面直線迭代加權(quán)最小二乘的相機(jī)位姿估計(jì)

光學(xué)精密工程(2016年5期)2016-11-07 09:05:55

基于CAD模型的單目六自由度位姿測(cè)量

光學(xué)精密工程(2016年4期)2016-11-07 09:05:11

大角度斜置激光慣組與里程計(jì)組合導(dǎo)航方法

導(dǎo)航定位與授時(shí)(2016年6期)2016-03-16 09:31:31

小型四旋翼飛行器位姿建模及其仿真

湖北工業(yè)大學(xué)學(xué)報(bào)(2016年5期)2016-02-27 13:14:48

儀器儀表用戶2021年11期

儀器儀表用戶的其它文章

做好微壓感知領(lǐng)域的“專精特新”企業(yè)
——訪合肥杜威智能科技股份有限公司董事長(zhǎng)

數(shù)字化轉(zhuǎn)型為核電智能運(yùn)維插上騰飛翅膀
——訪北京廣利核系統(tǒng)工程有限公司副總經(jīng)理 董孝勝

承德菲時(shí)博特自動(dòng)化設(shè)備有限公司

應(yīng)用OCS實(shí)現(xiàn)J1939協(xié)議的轉(zhuǎn)速采集

淺析海上平臺(tái)火氣系統(tǒng)

雙碳下綜合能源服務(wù)體系的技術(shù)發(fā)展趨勢(shì)與展望