基于非線性優化的激光雷達在線標定算法

楊 超，李天劍，胡 歡，黃 民

(北京信息科技大學 機電學院，北京 100192)

0 引 言

近些年來，基于激光雷達定位的自動引導車(automated guided vehicle，AGV)逐漸運用于各個不同領域。通過激光雷達作為主傳感器進行精確位姿估計的前提是預先知道傳感器的外參配置。對于一個機器人系統而言，標定外參通常是機器人工作前的首要步驟[1]。

在機器人領域，關于外參標定的研究一直是熱點所在，尤其是即時定位與建圖技術(simultaneous localization and mapping，SLAM)技術發展火熱以后。關于激光雷達的外參標定研究較多的包括相機與激光雷達的聯合標定等，黎云飛等提出的基于fmincon法的單線激光與單目相機外參標定法[2]，降低了標定計算量。姚文濤等提出了一種自適應攝像機與激光雷達聯合標定算法[3]，簡化了標定過程。韓棟斌等提出了基于多點匹配的三維激光雷達外參標定方法[4]，減小了標定初值對標定結果的影響。Zhe Gao等提出了利用簡單的平面測量方法在硬件要求較低情況下對2D激光雷達的標定方法[5]。祝飛等通過融合多種棋盤格約束的面陣相機與線激光約束，減少了激光噪點對標定結果的影響，提升了標定精度[6]。Miguel Oliveira等利用ROS框架提出了一種多模型的外參標定算法，同時標定多個外參并簡化了標定流程[7]。在利用里程計的激光標定方面，Andrea Censi曾對此問題展開研究，提出了一種里程計與激光雷達的聯合標定方案，該方案解決了對機器人內參與外參的同時標定，該方案在外參解決上采用了閉式求解。目前，對于7×24 h工作的移動機器人而言，其激光外參需要進行實時在線標定，以保持其精度的準確性，上述標定方案通常都需要輔助以特定標定板等工具進行標定，同時受限于環境(對標定環境有嚴格要求)，基本都是離線標定方案，不利于機器人進行在線標定，另外目前也存在標定精度不夠等問題。

針對上述問題，基于里程計的激光標定算法，本文提出了一種利用非線性優化的求解方式，該方法將標定問題建模為觀測與預測之間的非線性模型，能夠利用非線性最小二乘方案進行求解，通過誤差最小化，可以得到較為精確的標定值。

1 模型構建

使用2D激光雷達的移動機器人主要考慮其在2D平面中的運動，忽略機器人的形態大小，單純以點來描述機器人，那么在二維平面中描述機器人的位姿需要3個自由度，設位姿為p，則

p=(x,y,θ)

其中，x,y代表機器人在對應坐標系下的位置，θ描述機器人的姿態，即其繞垂于x,y平面的旋轉角，稱之為偏航角。

機器人的姿態觀測可通過傳感器獲得，本文以傳感器為2D激光雷達以及碼盤為主的差速運動機器人為研究對象，通過輪式碼盤可得到機器人的里程計數據，即機器人本體的位姿變換。而2D激光雷達亦可通過幀間匹配得到雷達在世界坐標系下的位姿變換。激光雷達與機器人本體間存在靜態的位姿變換，即所要求解的標定外參。根據二者之間的關系得到非線性數學模型，通過對非線性問題求解，即可得到激光雷達外參。

1.1 非線性優化模型構建

圖1描述了機器人在一定間隔時間t內機器人各坐標系之間的運動關系，假設機器人從t1時刻運動到t2時刻，圖中O1代表t1時刻的碼盤航跡推算得到的世界坐標系下里程計位姿，即機器人車體位姿，O2為t2時刻對應的世界坐標系下里程計位姿，To代表里程計航跡推算所得機器人在t1到t2時刻對應坐標變換。S1表示t1時刻激光雷達觀測所得激光雷達在世界坐標系下位姿，S2表示t2時刻激光雷達觀測得到激光雷達在世界坐標系下位姿，Ts代表激光雷達觀測所得雷達在t1到t2時刻之間的坐標變換。Tx表示激光雷達在機器人坐標系下的靜態坐標變換，即所求外參對應的坐標變換，該值為靜態值，不會隨機器人運動而發生變化。

圖1 機器人坐標系間運動關系

根據上述對應關系，目前已知在不同時刻下激光雷達及里程計在世界坐標系下的對應位姿，而根據歐式變換，可進一步獲得在t2時刻機器人在t1時刻里程計坐標系下的坐標變換，首先將t1時刻世界坐標系下里程計位姿轉換為歐式變換矩陣

(1)

其中，Oθ1，Ox1，Oy1分別為機器人里程計在t1時刻對應的位置參數與位姿參數。To1代表從世界坐標系到t1時刻機器人坐標系的坐標變換。

將t2時刻世界坐標系下里程計位姿轉換為歐式變換矩陣

(2)

To2代表從世界坐標系在t2時刻機器人坐標系的坐標變換。

根據歐式群變換理論，可獲得t2時刻機器人到t1時刻機器人的坐標變換為

To=To1To2

(3)

同理，可獲得t2時刻激光雷達在t1時刻對應激光雷達坐標系下坐標變換為

Ts=Ts1Ts2

(4)

根據式(3)、式(4)求解得到坐標變換To、Ts，以及激光雷達到機器人坐標系下的坐標變換Tx，可得到t2時刻激光雷達到t1時刻里程計坐標系下的坐標變換，該坐標變換可通過兩種方式得到

T1-2=TxTs

(5)

T1-2=ToTx

(6)

將這兩個等式連列，消去T1-2，則得到下面方程

TxTs=ToTx

(7)

將式(7)稍作化簡，則得

(8)

式(8)即所求數學模型，該數學模型為非線性化數學模型。

1.2 里程計位姿求解

差速運動機器人的里程計運動學模型依賴固連于連桿兩端的雙電機通過速度差分求得，如圖2所示，其對應機器人速度及角速度為

圖2 差速運動學模型

(9)

(10)

其中，vL與vR分別為左右輪輪速，d為輪間距。

假設機器人在t1時刻位姿為p(x，y，θ), 在理想情況下，通過航機推算，t2時刻機器人的位姿為

(11)

而實際運用中，機器人的運動收到噪聲的影響，真實的速度與測量得到的速度存在一定誤差。將這種誤差描述為以0為中心的有限方差的隨機變量[8]。實際的速度及角速度表達如下

(12)

其中，εα1v2+α2w2,εα3v2+α4w2為方差為α1v2+α2w2，α3v2+α4w2， 均值為0的誤差變量，α1～α4為機器人特定的誤差參數。

因此，對應的運動學航跡推算模型為

(13)

1.3 激光雷達位姿求解

激光雷達的位姿求解有多種方式，SLAM技術中大量涉及關于激光傳感器的定位問題，主流的激光雷達位姿求解主要通過幀間匹配的方式，即通過匹配相鄰兩幀激光或關鍵激光幀的重合部分，求解得到激光雷達的位姿變換。而匹配方式也包含多種方法，包括原始的迭代鄰近點匹配[9]，融合速度估計的VICP[10]匹配以及點線匹配等。目前，在匹配方式中，點線匹配方式效果較好，本文選用點線匹配來進行雷達位姿求解。

點線匹配算法又稱點線迭代臨近點匹配，由于兩幀激光在掃描過程中，激光點并不會完全進行點到點重合，該算法將原始的點到點匹配優化為點到線直接的匹配，用點去匹配最鄰近的直線段，提升了激光幀間的精度[11]，如圖3所示。其算法流程如下：

圖3 點線匹配

(1)列出兩幀激光之間的變換關系，將當前激光幀中激光點用pi表示，將參考幀中激光點用piw表示，則激光點之間的變換關系為

(14)

其中，Δx,Δy表示激光點從pi到piw的位置變換，θk表示激光點從pi到piw的位姿變換。

(2)計算激光幀從當前變換到參考幀坐標系后，激光點pi到臨近兩個激光點所組成的線段之間的最近距離；

(3)通過最小二乘，將此問題表達為非線性優化問題，通過最小化殘差，求解得到兩幀激光之間的坐標變換，其優化模型為

(15)

其中，i∈{1,2,…,n}，n為當前激光幀與參考激光幀所匹配得到的激光點數量，pj1i為參考激光幀中距離點piw最近的激光點，niT為過激光點piw且垂直于線段pj1ipj2i的單位法向量。

2 外參求解

2.1 實際考慮

根據以上所述，已經求得激光雷達位姿變換Ts，里程計位姿變換To，可將二者帶入式(8)中，由于該式為非線性數學模型，無法通過線性方式之間求解。設所解Tx如下

由于Tx為坐標變換，則x7=x8=0，x9=1，x1=x5，x2=-x4則Tx的形式變換為

且存在約束x12+x22=1， 代入式(8)后可發現該方程存在解析解，可通過拉格朗日乘子將該問題轉換為閉式解形式。但這種解析解僅適用于理想情況，在實際情況中，里程計中速度與角速度的獲取本身帶有高斯誤差，而在實際場景中，機器人要面臨不同的路面情況，其左右輪會存在不同程度打滑，這都是不可忽略的誤差。其次，激光雷達的波束模型同樣帶有噪聲，實際中的一面直墻在激光雷達的觀測中不盡然是一條直線，如圖4所示，同時在幀間匹配過程中，存在誤匹配的情況，這些都會造成激光雷達位姿求解的誤差。這些誤差最終都會造成解析解的標定精度，Andrea Censi曾將此閉式解問題通過反復迭代標定來提高標定精度。

圖4 激光噪點

本文提供另一種求解方式，直接將此問題作為優化問題進行求解。

2.2 非線性求解

考慮將問題轉化為優化問題，將式(8)轉化為如下數學模型

eT=Ts-T′o

(16)

e=z′-o′x

(17)

其中，e為位姿殘差，z為激光雷達在世界坐標系中位姿，o′x為里程計位姿通過外參求得激光雷達在世界坐標系中位姿，該問題可通過非線性最小二乘進行求解。

目標為最小化預測和觀測的差，將里程計得到坐標變換轉換得到的激光坐標變換作為預測值，將激光幀間匹配得到的坐標變換作為觀測值，則

ei(x)=zi-fi(x)

(18)

zi為ti時刻激光觀測位姿，fi(x) 為ti時刻里程計預測位姿，ei(x) 為ti時刻位姿殘差。假設誤差服從高斯分布，設其對應的信息矩陣為Σi， 因此該觀測值誤差的平方定義為

Ei(x)=ei(x)TΣiei(x)

(19)

因此，非線性最小二乘的目標函數為

(20)

對誤差函數進行線性化

ei(x+Δx)=ei(x)+Ji(x)Δx

其中，雅克比矩陣Ji為映射函數對狀態向量x的導數，其值為

因此，目標函數轉化為

(21)

其中， F(x+Δx) 為關于變量Δx的二次函數，其極值可通過令其關于Δx的導數為0求解得到

(22)

令x=x+Δx, 然后不斷迭代，直到收斂。

2.3 其它考慮

(1)由于激光雷達幀間匹配對初值要求較高，在初值較為不準確的前提下有可能在解算時陷入局部最優值，因此將里程計得到位姿變換作為初值進行幀間匹配。

(2)里程計采樣過程中，輪子可能存在打滑，造成里程計數據的不準確，因此在優化之前，首先進行濾波，去除一定百分比的較大誤差。

(3)為進一步提高精度，采用二次優化的方式，即一次優化完畢后，將第一次優化所得外參作為初值帶入進行二次優化，得到較為精確的外參。

(4)算法實現以ROS[12]系統為框架，ROS(robot operating system)為開源機器人操作系統，其利用節點/話題通信方式以及功能包開發方式加快了機器人軟件的開發進程，其中，ROS中便捷的TF轉換與監聽功能能夠幫助進行里程計及激光雷達數據同步。

整體算法流程如下：

(1)訂閱激光雷達數據/scan；

(2)通過訂閱/tf話題得到每幀激光雷達對應時間里程計位姿；

(3)通過兩幀里程計位姿得到相鄰里程計位姿差 d_point_odom；

(4)判斷d_point_odom運動距離間隔與角度間隔是否足夠大，若不滿足返回第(1)步；

(5)通過給定外參初值，將d_point_odom轉換為激光坐標系下位姿tmpPose，并作為初值傳遞給幀間匹配PL-ICP；

(6)通過幀間匹配，得到相鄰激光幀位姿差d_point_scan；

(7)通過角度偏差進行濾波；

(8)構建非線性優化模型目標函數，添加殘差；

(9)達到預定優化數量后，進行迭代優化求解，得到一次標定參數；

(10)將所得標定參數作為新一次優化初值進行二次優化，返回第(1)步；

(11)達到優化數量后，通過距離偏差進行濾波，迭代求解后，得到最終外參。

3 實 驗

本文使用搭載雙激光雷達的差速運動機器人為實驗對象，其雷達型號為SICK TIM561。

(1)機器人選擇：本文使用機器人為中型AGV，該機器人其左后方與右后方分別放置一枚SICK TIM561激光雷達(圖5)。同時安裝有雙電機及輪式編碼器，為差速運動機器人。

圖5 機器人及激光雷達

(2)環境選擇：考慮到幀間匹配的準確性，為保證標定準確，標定環境傾向于選擇結構化環境，本文使用木板搭建方形結構環境(圖6)，這種環境能夠保證幀間匹配(點線匹配)的準確性，環境大小為(3m×3m)。

圖6 標定環境

(3)激光雷達：本文選用激光雷達型號為SICK TIM561，激光雷達掃描頻率為15 Hz角度分辨率為0.33度，掃描范圍為0.05 m～10 m，掃描角度范圍為270°，該激光雷達可在一幀激光內得到721束激光束，影響該激光的掃描精度因素包括掃描距離，由于激光本身在觀察過程中存在噪聲，當激光距離障礙物過近或過遠時，其噪聲相對較大，在實驗中，需首先對激光進行預處理濾波，排除較近與較遠距離數據。激光在機器人運行過程中會存在畸變問題，尤其是激光幀率較小(5 hz ～10 hz)，其畸變隨機器人運行速度提高而增大，在標定過程中，應慢速運行。另外，在掃描反射比低于10%(深色物體)時，因光本身的性質，激光的掃描范圍會相應減小，最遠距離為8 m。

(4)數據提取：在進行優化前，需提取優化數據，包括激光幀間匹配數據以及里程計的變換位姿。由于輪子打滑等因素的影響，過短時間內，里程計的誤差將會對里程計位姿造成顯著影響。因此，在數據提取時，應盡量選擇稍長時間段內的數據。

(5)軌跡選擇：激光外參標定包括兩個部分：位置參數與姿態參數，單純的平移變換無法標定位置參數，單純的旋轉變換無法標定姿態參數。機器人在運動過程中需包含平移與旋轉軌跡。

3.1 外參標定波動值

在激光雷達外參真值無法實際獲得的情況下，通過多組實驗的外參波動大小驗證算法有效性。通過在所搭建結構化環境中，進行多次實驗，實驗結果外參見表1。

表1 機器人左右激光外參標定值

從標定結果可以看出，左激光外參在x方向波動值在1 mm 以內，y方向波動值在2 mm以內，theta值波動范圍在0.01弧度以內，右激光x方向波動值在2 mm以內，y方向波動值在2 mm以內，theta值波動范圍在0.01弧度以內。

3.2 雙激光雷達驗證

SICK TIM561激光雷達的掃描角度范圍為270°，在rviz中觀察標定運動狀態下左右激光的點云匹配程度如圖7、圖8所示。

圖7 左右激光點云匹配(1)

圖8 左右激光匹配(2)

圖中黑色激光點云為左激光捕獲點云，白色為右激光捕獲點云。左圖為標定前，即初值分別為(-0.335，0.36，2.36)(-0.335，-0.36，2.36)，左右激光幀捕獲點云無法嚴格重合，標定后，取標定參數為(-0.3375，0.350，2.34)(-0.330，-0.360，-2.37)，其左右激光點云重合程度優于標定前。

3.3 對比實驗

與閉式求解方式精度對比：錄制相同數據包，分別通過兩種算法對同一數據包進行處理，處理結果見表2。

表2 算法對比

在相同數據包下，閉式求解x方向波動為6 mm，y方向波動為6 mm，theta值波動為0.02弧度，與本文算法相比，本文算法精度優于閉式求解方式。

3.4 實際項目場景應用

本文算法適用于不同場景，圖為在實際工廠中進行在線標定后標定效果，非結構化環境下標定效果如圖9、圖10 所示。

圖9 實際工廠環境標定效果(1)

圖10 實際工廠環境標定效果(2)

在非結構化環境中，激光噪點較多，激光標定精度不如結構化環境，但標定前后左右激光仍可達到較好的匹配程度，算法可有效運用于非結構化環境。

3.5 激光外參標定在SLAM中應用

SLAM技術在定位過程中，對傳感器外參精度有較高要求，不精確的激光外參會對機器人定位效果產生較大影響，進而影響建圖效果，圖11為使用本文算法標定前后，利用圖優化SLAM算法進行建圖效果。

圖11 標定前后SLAM效果

從圖11可看到，標定前，SLAM建圖存在重影問題，在A、B、C這3處位置存在不同程度重影，標定后，重影問題明顯減小。

4 結束語

針對以輪式里程計以及激光雷達為主要傳感器的差速機器人的激光標定問題，提出了一種基于優化方式的標定算法，算法將標定問題轉換為預測與觀測之間的非線性優化模型，標定過程中，對標定軌跡有一定要求，需包含平移與旋轉，就標定環境而言，不僅適用于結構化環境，同樣適用于非結構化環境，結構化環境中的標定結果優于非結構化環境。通過實際實驗驗證，通過雙激光雷達點云匹配，標定后點云重合程度明顯優于標定前，在結構化環境中，該算法的標定波動值在x、y方向誤差保持在2 mm以內，角度誤差保持在0.01弧度以內，優于閉式求解方式。在實際工廠環境標定實驗中，該算法標定結果能夠有效提高點云匹配精度，在SLAM應用方面，標定后激光建圖精度明顯優于標定前。