[發(fā)明專利]一種基于ANFIS-ESKF的地面移動機器人多傳感器融合定位方法

在審

• 申請?zhí)枺?/td>: 202210160522.2
• 申請日: 2022-02-22
• 公開（公告）號: CN115060260A
• 公開（公告）日: 2022-09-16
• 發(fā)明（設計）人: 解楊敏;馮洋;朱杰;耿昊
• 申請（專利權）人: 上海大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G06K9/62
• 代理公司: 上海上大專利事務所(普通合伙) 31205
• 代理人: 何文欣

1.一種基于ANFIS-ESKF的地面移動機器人多傳感器融合定位方法，包括模塊1、模塊2、模塊3和模塊4共四個計算模塊，其特征在于：

所述模塊1，建立輪式-IMU里程計(WIO)的測量模型，推導系統(tǒng)的狀態(tài)方程；

所述模塊2，建立視覺里程計(VO)的測量模型，推導系統(tǒng)的觀測方程；

所述模塊3，用誤差狀態(tài)卡爾曼濾波(ESKF)對WIO和VO的運動估計信息進行融合；

所述模塊4，建立自適應模糊神經(jīng)推理系統(tǒng)(ANFIS)，根據(jù)WIO和VO的直接信息，推斷里程計的運動狀態(tài)，實時調(diào)整ESKF中預測模型和測量模型的協(xié)方差矩陣；該模塊在使用前需要根據(jù)GPS數(shù)據(jù)對ANFIS進行訓練；

所述模塊1的輸入是輪式里程計數(shù)據(jù)和IMU數(shù)據(jù)，輸出是相鄰時刻IMU坐標系的位姿估計；模塊2的輸入是雙目相機拍攝的照片，輸出是相鄰時刻相機坐標系的位姿估計；模塊3的輸入是模塊1和模塊2的輸出，輸出是相鄰時刻IMU坐標系的位姿估計；模塊4的輸入是模塊1中WIO和模塊2中VO的中間數(shù)據(jù)，輸出是模塊3中需要使用的WIO和VO的協(xié)方差矩陣；

整個定位程序的輸入是IMU數(shù)據(jù)、輪式里程計數(shù)據(jù)、相機數(shù)據(jù)，輸出是IMU坐標系的位姿變化。

2.根據(jù)權利要求1所述的基于ANFIS-ESKF的地面移動機器人多傳感器融合定位方法，所述模塊1中建立WIO測量模型，具體如下：

IMU包括三軸陀螺儀和三軸加速度計，考慮到測試系統(tǒng)加速度計中的噪聲極大地影響了位移估計，選擇僅使用陀螺儀來估計方向變化；作為補償，引入車輪編碼器進行平移運動計算，并與陀螺儀集成，融合為輪式-IMU里程計WIO，提供完整的六自由度運動估計；WIO為ESKF系統(tǒng)提供移動機器人運動狀態(tài)的預測；

下面的式子(1)展示了i+1時刻WIO測得的IMU坐標系{I}的位置變化：

其中是i+1時刻測得的IMU坐標系的姿態(tài)變化，是輪式里程計坐標系{W}相對于IMU坐標系的位置標定關系，是輪式里程計坐標系相對于IMU坐標系的姿態(tài)標定關系，是i+1時刻測得的輪式里程計坐標系的位置變化，I是單位矩陣。

3.根據(jù)權利要求1所述的基于ANFIS-ESKF的地面移動機器人多傳感器融合定位方法，其特征在于：所述模塊1中建立系統(tǒng)狀態(tài)方程，具體如下：

定義系統(tǒng)的狀態(tài)為：

其中pⁱ為i時刻IMU坐標系相對于初始時刻的位置，qⁱ為i時刻IMU坐標系相對于初始時刻的姿態(tài)，為i時刻IMU角速度偏移；

那么根據(jù)WIO測量模型得到系統(tǒng)的名義狀態(tài)方程為：

其中是i時刻輪式里程計的測量值，是i時刻IMU坐標系相對于0時刻的姿態(tài)，是i時刻IMU測得的角速度，是i時刻IMU的角速度偏移，Δt是IMU的采樣周期，[γⁱ]_×指的是γⁱ對應的反對稱矩陣；pⁱ⁺¹表示i+1時刻IMU坐標系相對于初始時刻IMU坐標系的位置，表示i+1時刻IMU坐標系相對于初始時刻IMU坐標系的姿態(tài)對時間的導數(shù)，表示i+1時刻IMU角速度偏移對時間的導數(shù)；

誤差狀態(tài)方程為：

其中θⁱ是qⁱ對應的李代數(shù)，ω_nⁱ是IMU的測量噪聲，是IMU角速度偏移的導數(shù)；表示i時刻輪式里程計的測量噪聲；ω_nⁱ表示i時刻IMU的角速度測量噪聲；Δt表示IMU采樣周期。