[發明專利]融合NDT和ICP的柵格地圖定位方法

在審

• 申請號: 202011548133.4
• 申請日: 2020-12-24
• 公開（公告）號: CN112612029A
• 公開（公告）日: 2021-04-06
• 發明（設計）人: 伍永健;陳智君;郝奇
• 申請（專利權）人: 哈爾濱工業大學蕪湖機器人產業技術研究院
• 主分類號: G01S17/06
• 分類號: G01S17/06;G01S17/88;G01S17/89;G01S7/48;G01C21/00;G06F16/29
• 代理公司: 蕪湖安匯知識產權代理有限公司 34107
• 代理人: 鐘雪

本發明公開了一種融合NDT和ICP的柵格地圖定位方法，包括如下步驟：S1、針對上一幀激光點云和當前激光幀點云，采用NDT算法獲取激光雷達的初始位姿；S2、基于初始位姿進行當前激光幀點云與上一幀激光點云的匹配，獲取激光雷達當前的位姿。移動機器人出現“綁架”或重啟等異常情況后的重定位問題，以及僅通過激光傳感器實現精確定位。

技術領域

本發明屬于定位技術領域，更具體地，本發明涉及一種融合NDT和ICP的柵格地圖定位方法。

背景技術

隨著人工智能技術的發展，自動化工廠、智能倉儲物流和家用服務等領域隨處可見移動機器人的身影，通過自主定位導航完成各種復雜任務。因此，定位技術是至關重要的。當移動機器人重啟或突然被“綁架”到其他位置，機器人無法定位其位姿，快速準確地實現重定位成為關鍵。

在現有解決機器人定位的方案中，大多數在周圍場景中通過各種標簽的方式實現機器人定位，例如粘貼二維碼或者反光板等，這些都需要經常維護，在限制機器人使用范圍的同時也增加了成本。激光SLAM定位導航技術是目前移動機器人最為先進的定位導航技術，已成為一種趨勢，移動機器人在運動過程中根據自身攜帶的傳感器和對周圍環境的感知進行自身定位，根據環境測量結果更新位置，不需要反射器或其它人工地標，降低了安裝成本，減少了維護工作。

發明內容

本發明提供了一種融合NDT和ICP的柵格地圖定位方法，旨在改善上述問題。

本發明是這樣實現的，一種融合NDT和ICP的柵格地圖定位方法，所述方法具體包括如下步驟：

S1、針對上一幀激光點云和當前激光幀點云，采用NDT算法獲取激光雷達的初始位姿；

S2、基于初始位姿進行當前激光幀點云與上一幀激光點云的匹配，獲取激光雷達當前的位姿。

進一步的，所述步驟S1具體包括如下步驟：

S11、初始化位姿變換參數ξ，根據初始位姿變換參數ξ將當前幀的激光點云轉換到上一幀激光點云坐標系下，計算當前幀激光點云轉換到上一幀激光點云坐標系對應的概率值，如公式(2)、(3)所示：

x’_i＝ξ·x_i (2)

p表示當前幀激光點云轉換到上一幀激光點云坐標系對應的概率值；x_i′表示當前幀激光點云中第i個點轉換到上一幀激光點云坐標下的坐標；

S22、用牛頓法優化參數ξ，獲取p得分最高的位姿變換參數ξ，將該位姿變換參數對應的位姿作為初始位姿。

進一步的，所述步驟S2具體包括如下步驟：

S21、基于初始矩陣對當前幀激光點云進行變換，得到變換后的激光點云II，然后將激光點云II與上一幀激光點云進行比較，在上一幀激光點云中查找距激光點云II中各點最近的鄰近點；

S22、構建目標函數E(R,t)，求解最優的旋轉平移矩陣：

l_i表示當前幀激光點云中第i個點的坐標，用n_i表示基于初始位姿將當前幀激光點云中第i個點變換至上一幀激光點云坐標系中的坐標，m_i表示上一幀激光點云中距點n_i最近點的坐標，即點n_i在上一幀激光點云中的鄰近點坐標；

S23、回到步驟S21中的尋找最鄰近點方法，進行迭代，直到滿足一些迭代終止條件，輸出激光雷達當前的最優位姿，實現連續定位。

進一步的，在步驟S1之前還包括激光雷達的重定位，重定位方法具體如下：