[發明專利]一種激光雷達與IMU緊耦合的定位方法

有效

• 申請號: 202010197086.7
• 申請日: 2020-03-19
• 公開（公告）號: CN111366153B
• 公開（公告）日: 2022-03-15
• 發明（設計）人: 呂攀;張恒;楊國青;李紅;潘之杰;吳朝暉
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S17/02;G01S17/87
• 代理公司: 杭州天勤知識產權代理有限公司 33224
• 代理人: 王琛

1.一種基于激光雷達和IMU緊耦合的機器人定位方法，包括如下步驟：

(1)讀取IMU的輸出數據，通過運動模型對機器人的實時運動狀態進行估計；

(2)讀取激光雷達的輸出數據，利用觀測模型對由IMU估計得到的機器人運動狀態進行糾正；具體地：激光雷達的輸出數據為點云數據，通過提取點云數據中的平面特征并對其進行跟蹤，通過跟蹤在滑動窗口中不同激光雷達狀態下的多個觀測約束來對由IMU估計得到的機器人運動狀態進行糾正；

提取點云數據中平面特征的方法為：首先利用隨機霍夫變換計算點云中每個點的法向量，根據點與點之間的歐式距離差值以及法向量夾角差值進行區域生長聚類；然后，對于每個聚類通過PCA計算關于其點云集合位置的特征向量和特征值，若最小特征值與第二大特征值的比值大于一定閾值，則認為該聚類不是平面，否則計算最大的兩個特征值所對應的特征向量的叉乘得到平面的法向量，計算聚類中點到該平面的平均距離，若平均距離大于一定閾值，說明該平面不夠平整，丟棄該聚類，否則將該聚類對應的平面作為平面特征提取出來；

通過隨機霍夫變換進行平面特征提取時，首先使用KD樹查找每個點附近的K個最近鄰點，然后在近鄰點中多次隨機選點進行霍夫投票得到該點的法向量；其中近鄰點的查找過程具體為：將點云數據通過球面投影得到其前視圖，以每個點在前視圖中的坐標為矩形框中心，從固定尺寸的矩形框內提取像素點所對應的點云作為潛在近鄰點，并度量潛在近鄰點到中心點的距離，將距離小于一定閾值的作為近鄰點進行后續的隨機霍夫變換；

對平面特征進行跟蹤的方法為：首先將上一幀的點云數據經過球面投影得到其前視圖，計算上一幀平面特征對應的聚類在該前視圖中的2D包圍盒；然后，在當前幀的點云數據中依次計算包圍盒中的每個點到該包圍盒所對應的上一幀平面特征的距離，將距離小于一定閾值的點作為inlier點，當inlier點數量大于一定閾值且inlier點數量占包圍盒中所有點數量的比例大于一定閾值時，認為平面特征跟蹤成功，并根據inlier點計算平面特征在下一幀點云數據中的參數方程，否則認為平面特征跟蹤失敗；

(3)反復迭代執行步驟(1)和(2)，使用步驟(2)糾正后的機器人運動狀態作為下次迭代時步驟(1)中所需的初始狀態信息。

2.根據權利要求1所述的機器人定位方法，其特征在于：所述IMU的輸出數據包括機器人的加速度和角速度，機器人的運動狀態包括速度、位置以及朝向；步驟(1)通過維護一個固定大小的滑動窗口以保存機器人運動軌跡中最新的若干個激光雷達狀態，從而實現對機器人實時運動狀態的估計，滑動窗口中的激光雷達狀態整體表示為其中：m為滑動窗口大小，T表示轉置，為單位四元數且表示全局坐標系G到IMU坐標系I的旋轉，b_g和b_a均為3×1大小的向量且分別表示影響IMU中陀螺儀和加速度計的測量偏差，^Gp_I和^Gv_I均為3×1大小的向量且分別表示IMU相對于全局坐標系G的位置和速度，為滑動窗口中的第i個激光雷達狀態，表示全局坐標系G到第i個激光雷達狀態的旋轉，表示第i個激光雷達狀態在全局坐標系G中的位置。

3.根據權利要求2所述的機器人定位方法，其特征在于：所述步驟(1)中的運動模型表達式如下：

其中：表示當前時刻全局坐標系G到IMU坐標系I的旋轉，表示關于時間的導數，ω表示當前時刻機器人的角速度，表示當前時刻機器人的加速度，表示當前時刻機器人相對于全局坐標系G的速度，[ω_×]為ω的偏置矩陣，表示^Gp_I關于時間的導數，表示關于時間的導數，C()表示用于將四元數轉換為對應旋轉矩陣的函數，^Gg為全局坐標系G下的重力加速度，和分別為b_g和b_a關于時間的導數，0_3×1為3×1大小且元素值全為0的向量。