陳新宇，雷 斌,1b，蔣 林,1b，張文俊

(1.武漢科技大學(xué)a.冶金裝備及其控制教育部重點實驗室；b.機器人與智能系統(tǒng)研究院，武漢 430081；2.立得空間信息技術(shù)股份有限公司，武漢 430073)

0 引言

隨著響應(yīng)國家“中國制造2025”戰(zhàn)略規(guī)劃，移動機器人技術(shù)在實際生活中和產(chǎn)品應(yīng)用等領(lǐng)域也獲到了高速的進展，如物流機器人[1-2]對貨物的搬運、服務(wù)機器人[3]在家庭、酒店和商場中的使用、巡檢機器人[4-5]在電力維修巡檢方面的運用和自動駕駛技術(shù)[6-7]的逐漸崛起。

其中移動機器人的定位就是要通過不同的傳感器獲取其在當前環(huán)境下的位姿信息。常用的傳感器如輪式里程計、GPS、IMU(慣性測量單元)、相機和激光測距儀等?；谳喪嚼锍逃嫷亩ㄎ籟8]和基于IMU(慣性測量單元)的定位方法在定位過程中會產(chǎn)生累計誤差，需要進行修正。基于相機的定位方法[9]主要是通過獲取環(huán)境中的視覺特征如ORB[10]特征來進行匹配定位，但是會受到光照和測距有限影響，定位魯棒性不高?；诩す饫走_的定位方法是通過當前幀和上一幀數(shù)據(jù)進行搜索匹配，常用的匹配方法有：ICP[11](迭代最近點)、NDT[12](正態(tài)分布轉(zhuǎn)換)和KD樹等點云配準方法。而激光雷達不受環(huán)境光照等影響，具有精度高，測量距離遠和魯棒性更高等優(yōu)點。

激光雷達也可以通過獲取環(huán)境中人工設(shè)定的特殊標簽進行定位，RONZONI等[12]通過布置的標簽進行機器人定位，但是這種利用線段進行特征匹配的方法效率不高，定位精度和定位魯棒性較差。吳波[13]提出了一種利用二維激光雷達和設(shè)置的反光板來進行移動機器人定位的方法，并結(jié)合里程計使用擴展卡爾曼濾波算法融合定位，但是該方法的使用場景有限。周凱月等[14]提出了一種聯(lián)合二維激光雷達和反光柱的定位系統(tǒng)，將反光柱作為位姿圖優(yōu)化的一個節(jié)點，采用聯(lián)合優(yōu)化激光點誤差和反光柱匹配誤差的方式定位機器人的位姿。丁松等[15]提出了一種基于特征分割與特征匹配的智能叉車定位算法，該方法采用單線激光雷達，基于反光柱反射點特征分割算法和以反光柱之間距離為先驗條件的特征匹配來求解智能叉車的位姿。

但是上述方法在更加空曠的環(huán)境和缺少特征信息的環(huán)境中的定位效果較差。因此，本文針對上述問題設(shè)計了一種基于三維激光雷達和反光柱的移動機器人定位算法，該算法利用激光雷達的反射強度信息對反光柱進行點云分割。采用ALOAM算法構(gòu)建點云地圖。構(gòu)建KD樹最近鄰域搜索進行匹配求解機器人的位姿。本文分別借助仿真環(huán)境和真實環(huán)境實驗平臺進行相關(guān)方法闡述及驗證。

1 反光柱識別和分割

1.1 激光雷達反射強度原理

三維激光雷達的測距原理為飛行時間測量法。主要是通過激光的飛行時間計算物體與激光傳感器的距離。其計算方法如式(1)所示。

(1)

式中，d為距離；c為光速；t為激光束的飛行時間。激光雷達的轉(zhuǎn)子上固定安裝多對激光發(fā)射和接收裝置，通過內(nèi)部電機旋轉(zhuǎn)實現(xiàn)水平方向360°掃描。物體在傳感器坐標中的位置由發(fā)射角度和距離(即到達時間)確定。強度值由從反射信號接收的能量與發(fā)射的激光功率之比確定。接收功率Pr的物理公式為：

(2)

式中，Pe是發(fā)射激光束的功率；Dr是接收器直徑；ηsys是系統(tǒng)發(fā)射因子；ηatm是大氣發(fā)射因子；α是物體表面與激光束之間的入射角；ρ是物體的材料反射率；測量距離R和入射角α為外部參數(shù)。而ηsys和ηatm是常數(shù)參數(shù)。因此反射強度I的物理公式為：

(3)

式中，ηall是一個常數(shù)。表面反射率ρ僅與入射角α和測量距離R有關(guān)：

(4)

式中，R為激光雷達的測量距離，物體表面和激光束之間的入射角α采用分析局部法線獲得。對于每個點P∈P3，尋找最近的兩點P1和P2，局部表面法線n可以表示為：

(5)

式中，×表示叉積，入射角為：

(6)

校準強度掃描I～可以從具有幾何讀數(shù)P和強度讀數(shù)I的原始掃描中推導(dǎo)出，三維激光雷達使用距離測量部分來校準入射角度。同時低強度值往往會導(dǎo)致較低的信噪比(SNR)，這也降低了在低值下進行強度測量的測距精度。因此在相同的測量距離和入射角下，采用不同的反射材料，強度值會隨著物體的材料反射率發(fā)生改變，引入高反射率的材料可改變點云反射強度信息。

1.2 反光柱點云識別和分割

反光柱是一種外表貼附高反射強度材料的圓柱體，激光照射在其表面上會有超高的反射強度。因此，本文利用反光柱的反射強度進行特征提取，進而對反光柱點云進行分割，進而實現(xiàn)機器人的全局定位。圖1所示為反光柱的實物圖，由激光雷達掃描獲得的含有反光柱的激光幀中的反射強度值較大，因此設(shè)置閾值，進行點云分割。在本文中選擇的反射強度閾值是200。對于激光雷達掃描到的每一幀點云保存XYZI信息，設(shè)置一個反射強度閾值，認為小于反射強度閾值200的都為離散點，將其濾除。

圖1 反光柱實物圖

1.3 特征角點提取

本文使用十六線激光雷達獲取激光數(shù)據(jù)進行特征角點的提取。采用從一幀激光幀中計算曲率的方式進行特征點的提取。具體的特征提取方法為，先對每一根激光掃描線進行遍歷，從第六個點開始對每一個點分別提取其前后各五個點，利用這十個點計算該點的距離偏差平方，作為其曲率。曲率公式為：

(7)

1.4 特征匹配

(8)

對于求解得到的點到直線的距離采用列文伯格-馬夸特法非線性優(yōu)化進行求解。LM方法的框架為下：

步驟1：設(shè)置先驗值x0和先驗優(yōu)化半徑μ；

步驟2：對于第k次迭代，在高斯牛頓法的基礎(chǔ)上加上信賴區(qū)域，求解：

(9)

(10)

式中，f(xk)是目標函數(shù)；J(xk)T是f(xk)的一階導(dǎo)數(shù)也稱為雅可比矩陣；μ是信賴區(qū)域的半徑；D是系數(shù)矩陣。

步驟3：計算ρ；

(11)

式中，ρ是近似程度的評價指標。

步驟6：如果ρ過大，則令xk+1=xk+Δxk；

步驟7：判斷結(jié)果是否達到極小值。如果沒有達到返回步驟2。

在該非線性優(yōu)化中引入拉格朗日乘子來對目標函數(shù)進行約束，其公式為：

(12)

(13)

(14)

式中，λ是系數(shù)因子，對上式求導(dǎo)得到：

J(Δxk)f(x)+J(Δxk)(Δxk)+λDTDΔx=0

(15)

化簡后求解增量的線性方程：

(H+λDTD)Δxk=g

(16)

對上式進行求解直到達到收斂為止。結(jié)合反光柱的定位環(huán)境因此本文選用列文伯格-馬夸爾特優(yōu)化的方法來對機器人的位姿進行優(yōu)化，得到更加精確的定位信息。

2 反光柱匹配定位系統(tǒng)

2.1 反光柱的布置

為了實現(xiàn)機器人定位的精度，在反光柱環(huán)境中對反光柱的擺放主要有以下要求：機器人在行駛軌跡上每個位置至少能觀測到兩個反光柱；反光柱的高度和機器人上的激光傳感器高度相同。

2.2 反光柱定位系統(tǒng)

反光柱定位系統(tǒng)的框架如圖2所示。按照反光柱的擺放規(guī)則布置反光柱環(huán)境，基于三維激光雷達禾賽PandarXT16激光雷達進行激光數(shù)據(jù)的采集，對采集的激光雷達數(shù)據(jù)進行點云預(yù)處理，提取點云中所有點的反射強度信息，設(shè)置一個反射強度閾值，對反光柱點云進行分割。采用ALOAM算法利用反光柱點云構(gòu)建反光柱點云地圖。對當前幀進行預(yù)處理，計算點云中每個點的曲率，利用曲率進行特征提取根據(jù)前一幀的位姿將當前幀的角點坐標轉(zhuǎn)換到世界坐標系下。利用反光柱點云地圖構(gòu)建KD樹，在世界坐標系下在KD樹中通過最近鄰域搜索最近的5個角點，由5個角點的坐標均值，構(gòu)建協(xié)方差矩陣。對協(xié)方差矩陣進行特征值分解。利用特征值和特征向量計算點到直線的距離，利用列文伯格-馬夸爾特對特征向量進行非線性優(yōu)化，求解輸出機器人的當前位姿。

圖2 反光柱定位系統(tǒng)框架圖

3 實驗結(jié)果和分析

3.1 仿真環(huán)境實驗

本文所采用的移動機器人仿真實驗平臺是基于Ubuntu16.04操作系統(tǒng)和ROS機器人開發(fā)平臺的GAZEBO仿真平臺自主開發(fā)和搭建。所搭建的仿真機器人MRobot的底盤為差分運動底盤，所使用的激光傳感器為仿真的VLP-16三維激光雷達，其技術(shù)參數(shù)如表1所示。最大測量距離為100 m，最大掃描范圍為0°～360°，最高分辨率可達0.1°，最高掃描頻率可至20 Hz。所使用的反光柱的直徑為0.1 m，高度為1 m。在GAZEBO中搭建一個長度為10 m，寬度為2.5 m的仿真走廊環(huán)境，反光柱在走廊兩側(cè)以2 m為間距，對稱擺放。為驗證本文反光柱定位算法在仿真環(huán)境中機器人靜態(tài)定位的精度，分別進行了靜態(tài)定位實驗和動態(tài)定位實驗。將機器人在仿真走廊反光柱系統(tǒng)中進行靜態(tài)定位測試，將其與GAZEBO仿真環(huán)境中的真實機器人坐標進行比較。動態(tài)定位實驗是將記錄的機器人定位軌跡和預(yù)定的真實機器人運動軌跡進行比較。定位的誤差計算方式為：

Δx=|xt-X|

(17)

Δy=|yt-Y|

(18)

Δθ=|θt-θ|

(19)

Δs2=(xt-X)2+(yt-Y)2

(20)

式中，Δx為橫坐標定位誤差；Δy為縱坐標定位誤差；Δθ為航向角定位誤差；Δs2為距離偏差平方；xt為實際測量的橫坐標；yt為實際測量的縱坐標；θt為實際測量的航向角；X為真實的橫坐標；Y為真實的縱坐標；θ為真實的航向角。

表1 VLP-16激光雷達性能參數(shù)

圖3 仿真反光柱環(huán)境 圖4 仿真機器人實驗平臺

3.1.1 仿真環(huán)境靜態(tài)實驗

實驗步驟如下：訂閱激光雷達激光話題，利用ALOAM算法構(gòu)建反光柱地圖。輸入反光柱點云地圖和當前幀激光點云。啟動仿真機器人，進行靜態(tài)定位實驗。靜態(tài)的定位誤差如圖5所示，分別為X誤差、Y誤差、yaw誤差和距離平方偏差。表2為靜態(tài)定位數(shù)據(jù)的誤差分析，由表2可知，由本文算法圖5仿真環(huán)境靜態(tài)定位誤差得到的橫坐標、縱坐標、航向角和距離偏差的平均值分別為0.017 38 m、0.034 17 m、0.002 61 m和0.005 04 m。標準差分別為0.025 19 m、0.023 05 m、0.005 42 m和0.002 37 m。從實驗數(shù)據(jù)可知，機器人定位位姿平均值在橫坐標和縱坐標上的差值在0.05 m之內(nèi)，航向角上差值在0.01之內(nèi)。它們的標準差在橫坐標和縱坐標方向上都是0.05 m量級或以下，航向角在0.01以下。因此可以證明本文算法在仿真環(huán)境下機器人定位精度較高，穩(wěn)定性較好，基本沒有出現(xiàn)定位失敗的情況。

圖5 仿真環(huán)境靜態(tài)定位誤差

表2 仿真環(huán)境靜態(tài)定位誤差 (m)

3.1.2 仿真環(huán)境動態(tài)實驗

為進一步驗證本文算法的定位效果，進行機器人進行動態(tài)定位測試。實驗步驟如下：機器人以0點為起點沿著Y軸方向分別進行了勻速為0.1 m/s、0.2 m/s和0.3 m/s的動態(tài)定位軌跡的測試，行走的距離為10 m。機器人的真實運動軌跡為Y軸。機器人輸出定位軌跡和機器人真實運動軌跡的軌跡以及動態(tài)定位誤差如表3所示。

表3 仿真環(huán)境動態(tài)定位實驗分析

由表3的實驗結(jié)果可知機器人的輸出定位軌跡和機器人真實運動軌跡重合度較高，方向相同，軌跡曲線基本保持一致。機器人仿真動態(tài)定位誤差數(shù)值分析如表4所示，其中X、Y和S分別為X軸誤差、Y軸誤差和距離平方差。機器人運動速度分別為0.1 m/s、0.2 m/s和0.3 m/s時定位距離偏差的平均值分別為0.001 837 m、0.002 3 m和0.002 38 m。標準差分別為0.001 743 m、0.001 65 m和0.001 692 m。從實驗數(shù)據(jù)可知，機器人定位位姿平均值在橫坐標和縱坐標上的差值在0.06 m之內(nèi)。它們的標準差在橫坐標和縱坐標方向上都是0.05 m量級或以下。經(jīng)過實驗數(shù)據(jù)表明本文定位算法可以滿足在仿真環(huán)境動中實現(xiàn)較高的定位精度。

表4 仿真環(huán)境動態(tài)定位誤差

3.2 真實環(huán)境定位實驗

本文所使用的實驗平臺硬件部分主要包括工控機處理器為Core i7-860S、移動機器人底盤是ROBOTM500、激光傳感器為禾賽pandarXT16線激光雷達。禾賽pandarXT16線激光雷達數(shù)據(jù)參數(shù)如表5所示。搭建的實驗平臺如圖6所示。

表5 PandarXT16激光雷達性能參數(shù)

本文所使用的軟件平臺為ubuntu16.04操作系統(tǒng)。在ubuntu系統(tǒng)中采用ROS(機器人開發(fā)平臺)進行通信。在本文實驗中，禾賽pandarXT16多線激光雷達數(shù)據(jù)通過多倍通電臺傳入工控機。反光柱環(huán)境如圖7所示，在走廊兩側(cè)依次排列。反光柱的直徑為0.09 m，高度為0.5 m，在走廊兩側(cè)以2 m為間距，對稱擺放。

圖6 機器人實驗平臺 圖7 實際反光柱環(huán)境

3.2.1 反光柱點云分割實驗

基于上述真實反光柱環(huán)境，進行反光柱點云分割實驗。實驗步驟如下：構(gòu)建反光柱環(huán)境的原始點云地圖。設(shè)置反光柱反射強度閾值，對反光柱原始點云地圖進行分割，濾除多余的點云，保留反光柱點云。圖8為機器人通過環(huán)境信息構(gòu)建的原始反光柱點云地圖。圖9為分割后的反光柱點云地圖，圖中的柱狀點云即為分割得到的反光柱。分割前后的反光柱點云地圖點云數(shù)量以及點云所占比例如表6所示。反光柱點云地圖分割出的反光柱點云不包含其他冗余點云，只保留反光柱點云，進行匹配定位。

圖8 原始反光柱點云地圖 圖9 分割后的反光柱點云地圖

表6 反光柱點云地圖點云分割點云數(shù)量統(tǒng)計

3.2.2 靜態(tài)定位實驗

為驗證本文反光柱定位算法在真實環(huán)境中機器人靜態(tài)定位的精度，分別測試機器人在不用位置下的位姿，與機器人真實位姿進行對比，計算定位誤差。本文算法在真實環(huán)境下的靜態(tài)定位誤差如圖10所示。真實環(huán)境下的靜態(tài)定位誤差分析如表7所示。

圖10 真實環(huán)境靜態(tài)定位誤差

表7 真實環(huán)境靜態(tài)定位誤差 (m)

由表7可知，X、Y、S分別為橫坐標誤差、縱坐標誤差和距離偏差。橫坐標、縱坐標和距離偏差的標準差分別為0.027 33 m、0.033 04 m和0.006 37 m，它們的平均值分別為0.010 38 m、0.094 17 m和0.012 61 m。從實驗數(shù)據(jù)可知，機器人定位位姿平均值在橫坐標和縱坐標上的差值在 0.1 m之內(nèi)。其標準差在橫坐標和縱坐標方向上都是0.04 m量級或以下。由此可以證明本文算法在真實環(huán)境下機器人的靜態(tài)定位精度較高，穩(wěn)定性較好，基本沒有出現(xiàn)定位失敗的情況，可以實現(xiàn)在真實環(huán)境中的靜態(tài)定位。

3.2.3 動態(tài)定位實驗

為驗證本文反光柱定位算法在真實環(huán)境中機器人動態(tài)定位的精度，分別測試機器人在不同線速度下的運動軌跡。在真實環(huán)境中，本文以機器人的起點為原點，以機器人運動方向為Y軸，構(gòu)建世界坐標系。分別以0.3 m/s、0.5 m/s和0.7 m/s的線速度勻速沿Y軸進行動態(tài)定位軌跡測試。機器人的真實運動軌跡為沿Y軸直線行駛9 m的直線。機器人的定位軌跡如圖11所示，機器人在不同速度下的定位誤差如圖12所示。機器人在真實環(huán)境下的動態(tài)定位誤差如表8所示。

圖11 機器人運動軌跡

圖12 不同速度下動態(tài)定位誤差

表8 真實環(huán)境動態(tài)定位誤差

由表8的實驗結(jié)果可知機器人的輸出定位軌跡和機器人真實運動軌跡重合度較高，方向相同，軌跡曲線基本保持一致。其中X、Y和S分別為X軸誤差、Y軸誤差和距離平方差。機器人運動速度分別為0.3 m/s、0.5 m/s和0.7 m/s時定位距離偏差的平均值分別為0.002 64 m、0.005 43 m和0.009 99 m。標準差分別為0.001 96 m、0.001 59 m和0.006 41 m。從實驗數(shù)據(jù)可知，機器人定位位姿平均值在橫坐標和縱坐標上的差值在0.06 m之內(nèi)。它們的標準差在橫坐標和縱坐標方向上都是0.03 m量級或以下。經(jīng)過實驗數(shù)據(jù)表明本文定位算法精度較高，穩(wěn)定性好，可以滿足在真實環(huán)境中使用。

3.2.4 對比實驗

為了驗證本文算法增加反光柱后對機器人定位精度的提升效果。本文進行了真實環(huán)境中有反光柱和無反光柱定位對比實驗，分別記錄了機器人在有反光柱的環(huán)境和無反光柱的空曠環(huán)境下機器人的定位軌跡，如圖13所示?？梢院芮宄目闯隹諘绛h(huán)境下的機器人定位軌跡在X軸負方向產(chǎn)生了很大的漂移，而增加反光住后，機器人定位軌跡和真實運動軌跡重回度較高，基本保持一致。因此可以證明在空曠環(huán)境等特殊環(huán)境下，相比于空曠環(huán)境增加反光柱后機器人定位精度更高，定位穩(wěn)定性更好，整個定位系統(tǒng)的魯棒性更高。

圖13 定位軌跡對比實驗

4 結(jié)束語

本文針對缺少特征信息的空曠環(huán)境下機器人定位穩(wěn)定性和精度不高的問題，提出了一種三維激光雷達和反光柱耦合定位算法。該算法利用激光雷達的反射強度信息對反光柱進行點云分割。對當前幀激光進行特征提取，構(gòu)建KD樹最近鄰域搜索和反光柱點云地圖實現(xiàn)幀到地圖的匹配，求解機器人的位姿。實驗結(jié)果表明，本文所提出的方法具有更高的精度和較好的魯棒性?？梢杂行M足機器人在定位中的需求。