[發明專利]一種巡檢機器人雷達里程計的速度與精度優化方法

有效

• 申請號: 202211125540.3
• 申請日: 2022-09-16
• 公開（公告）號: CN115328163B
• 公開（公告）日: 2023-03-28
• 發明（設計）人: 田發存;張目華;馬磊;沈楷;孫永奎
• 申請（專利權）人: 西南交通大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 成都信博專利代理有限責任公司 51200
• 代理人: 秦立飛

本發明公開了一種巡檢機器人雷達里程計的速度與精度優化方法，具體為：使用三維激光雷達采集點云數據，計算曲率，按照曲率的大小篩選特征點；進行兩幀點云位姿轉換，再進行線特征關聯和面特征關聯；使用Ceres優化器進行位姿平移部分的凸優化，輸出三維雷達數據，更新三維雷達里程計數據，再進行插值。本發明提升了運動控制平順程度與傳感器數值匹配的精度，同時提高了定位的效率與精度。

技術領域

本發明屬于空間環境感知技術領域，尤其涉及一種巡檢機器人雷達里程計的速度與精度優化方法。

背景技術

車底巡檢機器人用于代替人工，在停車場或車輛段的檢修地溝內對車輛的車底零部件進行數據采集，通過人工智能算法進行分析，自動給出車輛維保意見，以保證車輛的運行安全，降低巡檢工人的工作強度。為了保證對車底零部件數據采集的精度、保障機器人與車底零部件不發生碰撞，機器人在檢修溝內定位數據源的速度與精度非常重要。

基于激光雷達的同步定位與建圖技術是機器人領域最常用的定位方法。Ji Zhang在2014年提出的LOAM(Lidar Odometry and Mapping)是一種經典的三維雷達里程計，該算法將同步定位與建圖分為不同輸出頻率，以提高效率與精度。然而，由于現有的基于純三維雷達點云數據的LOAM算法，在六自由度運動估計中誤差達到了2.5％以上，且三維雷達性能受限于點云采集速度與點云輸入速度，其運動估計的輸出頻率通常處于10Hz左右，這非常不利于巡檢機器人基于該定位數據源的運動控制與傳感器數據參考。

因此，有必要基于現有LOAM算法，為車底巡檢機器人在檢修地溝這類特殊場景下的定位需求，提供一種輪速計耦合高頻高精度雷達里程計解決方案。

發明內容

為實現上述目的，本發明提供一種巡檢機器人雷達里程計的速度與精度優化方法。

本發明的一種巡檢機器人雷達里程計的速度與精度優化方法，首先在機器人上安裝的三維激光雷達、電機輪速計和慣性測量單元IMU，其優化步驟具體為：

步驟1：用機器人上裝載的三維激光雷達進行掃描，獲取巡檢地溝內的激光點云數據，三維激光雷達在t_k時刻掃描組合起來的點云數據記為p。

步驟2：計算曲率：計算當前點前后各5個點與當前的點的曲率c，其計算公式為：

其中，X表示三維激光雷達單個掃描線上的點，X_i表示第i個點，|X|表示向量的模，也就是這個點到三維激光雷達坐標系原點的距離。

步驟3：篩選特征點：按照曲率的大小篩選特征點，共分為兩類：曲率大的點為線特征點、曲率小的點為面特征點，并對特征點的數量進行限制，這樣就在整個三維空間內，將線特征點和面特征點提取出來，用以代替整個點云數據。

步驟4：兩幀點云位姿轉換：第k+1幀與第k幀的相對位姿為：其中，R是旋轉矩陣，t是平移向量；第k+1幀中的點p_i轉換到第k幀坐標系：//

使用慣性測量單元IMU提供的在兩幀點云采樣時間內的旋轉運動估計R_IMU作為點云平移變換的結果R。

使用輪速計的位姿變換中的平移變換t_odom作為點云平移變換的迭代初值，即用于優化的相對位姿中的平移向量t的迭代初值由電機輪速計在這個時間內的變換給出。

步驟5：特征關聯：

步驟5.1：線特征關聯：當p_i為線特征點時，在上一幀中搜索離最近的線特征點記為點a，并在相鄰線上再找一個線特征點記為點b，兩點組成直線，其殘差函數為點到直線的距離：