[發明專利]基于多傳感器融合的定位方法、定位系統及機器人

有效

• 申請號: 202110722413.0
• 申請日: 2021-06-29
• 公開（公告）號: CN113252033B
• 公開（公告）日: 2021-10-15
• 發明（設計）人: 王錦山;于泠汰;王宇雨;冼志懷;方銳涌
• 申請（專利權）人: 長沙海格北斗信息技術有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S17/86;G01S19/45;G01S19/47
• 代理公司: 長沙永星專利商標事務所(普通合伙) 43001
• 代理人: 周詠;米中業

1.一種基于多傳感器融合的定位方法，其特征在于包括如下步驟：

S1. 采用若干種傳感器獲取當前機器人所處環境的環境信息和機器人當前的運動信息；具體為采用多線激光雷達采集三維點云信息，采用慣性傳感器采集三軸加速度信息，采用北斗系統采集經緯度信息，采用輪式里程計采集機器人的輪子轉速信息；

S2. 根據步驟S1獲取的環境信息和運動信息，計算得到機器人的位姿變換信息；具體為將多線激光雷達采集的三維點云信息通過NDT算法計算機器人的位姿變換信息；將慣性傳感器采集的三軸加速度信息通過慣導算法計算機器人的位姿變換信息；

S3. 對若干種傳感器獲取的信息進行實時不間斷融合，從而得到機器人的當前位置信息；具體為將慣性傳感器對應的慣導算法得到的位姿變換信息、輪式里程計對應的航位推算算法得到的位置信息和多線激光雷達對應的NDT算法得到的位姿變換信息，不間斷地采用無跡卡爾曼濾波算法進行融合，從而得到odom→robot之間的坐標變換；同時，通過NDT算法粗估和ICP精細匹配，提高點云匹配的精度；其中odom為慣性傳感器、輪式里程計和多線激光雷達融合后的坐標系，robot為機器人的坐標系；

所述的不間斷地采用無跡卡爾曼濾波算法進行融合，從而得到odom→robot之間的坐標變換，具體為采如下步驟進行融合并得到坐標變換：

根據輪式里程計系統計算的位姿變換，IMU里程計系統計算的位姿變換，對無跡卡爾曼濾波下一個狀態進行預測，預測函數為：

其中g()為機器人非線性運動模型的狀態變換函數，為機器人在k時刻的狀態，為機器人在k時刻預測k+1時刻的狀態，為控制變量；

采用如下步驟進行計算：

（1）讀取k時刻最優估計狀態以及最優估計狀態協方差矩陣；

（2）計算上一時刻k與這一時刻k+1的時間間隔；

（3）根據、、、、、以及采樣參數選取sigma point點集，其中為直線加速度，為偏航角加速度噪聲，為直線加速度過程噪聲的方差、為偏航角加速度過程噪聲的方差；

（4）將選取的sigma point點集代入機器人非線性運動模型g()中，計算得到預測的sigma point點集，利用預測得到的sigma point點集帶權重地得到k+1時刻的預測狀態和協方差矩陣；

（5）讀取k+1時刻傳感器數值，判斷傳感器類型，選擇對應的h()轉換函數，對預測sigmapoint點集從預測空間下轉換到測量空間下；

（6）使用測量空間下的sigma point點集和測量協方差矩陣R計算測量空間下的預測狀態均值和預測狀態協方差矩陣；

（7）計算sigma point在測量空間轉換到狀態空間下的轉換函數；

（8）計算卡爾曼增益，更新k+1時刻的最佳估計狀態以及最佳估計狀態矩陣；

上述步驟（1）~（8）為UKF算法迭代一次的步驟，循環讀取各傳感器數據作融合處理；融合出odom-robot之間的坐標變換；

所述的通過NDT算法粗估和ICP精細匹配，提高點云匹配的精度，具體為采用如下步驟提高點云匹配的精度：

基于UKF的慣性導航系統，其結果作為NDT算法的每一輪迭代的迭代初值，提高NDT算法的迭代穩定性與準確度；配合ICP精細匹配模塊進一步增加點云匹配的精度；

假設t時刻與t+1時刻周圍環境點云數據分別為和；

通過慣性里程計計算點云數據對應的第二相對位置與點云數據對應的第一相對位置；經過NDT算法對第一相對位置與第二相對位置計算后輸出的點云分別為和，并計算和的變換參數；ICP算法通過計算與對應點距離，計算出旋轉平移矩陣，通過對變換，并計算變換之后的均方差；若均方差小于預設的誤差閾值，則算法結束；否則繼續重復迭代直至均方差小于預設的誤差閾值或者迭代次數滿足預設的迭代數閾值，則終止計算；

S4. 直接獲取機器人當前的經緯度信息，從而得到機器人當前的絕對位置；

S5. 將步驟S3得到的機器人的當前位置信息和步驟S4得到的機器人當前的絕對位置進行融合，從而得到機器人的精確位姿；

S6. 對定位過程的連續性進行判斷，并在滿足設定條件時，進行重定位；所述的重定位，具體包括局部重定位和全局重定位；局部重定位是依靠北斗導航數據的回溯以及大尺度的搜索，適用于NDT算法輸出位置不符合連續性判斷的情況；全局重定位用于在系統出現傳感器失效的情況時。