[發明專利]基于UWB無線定位與IMU融合的導航定位系統及方法

在審

• 申請號: 202110597733.8
• 申請日: 2021-05-31
• 公開（公告）號: CN113900061A
• 公開（公告）日: 2022-01-07
• 發明（設計）人: 劉健;黃達健
• 申請（專利權）人: 深圳市易艾得爾智慧科技有限公司
• 主分類號: G01S5/02
• 分類號: G01S5/02;G01S5/06;G01C21/16;H04W4/024
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于UWB無線定位與IMU融合的導航定位系統,其特征在于，包括：

安裝于指定工作區域內的至少四個UWB基站；

由兩個UWB定位標簽和兩個IMU組成的標簽裝置；其中一個所述UWB定位標簽與一個IMU以剛體連接方式安裝在一起組成第一復合標簽，另外一個所述UWB定位標簽和IMU以剛體連接安裝在一起組成第二復合標簽，所述第一復合標簽與第二復合標簽以設定距離間隔安裝在固定組件上組成所述標簽裝置；

主控裝置，用于接收所述標簽裝置數據，采用卡爾曼濾波器緊耦合的方式對UWB標簽數據和IMU數據進行實時融合，得到當前位置數據和航向角數據；

由所述標簽裝置及主控裝置組成的定位裝置。

2.根據權利要求1所述的基于UWB無線定位與IMU融合的導航定位系統,其特征在于，所述第一復合標簽與第二復合標簽之間的距離為0.5m~1.0m。

3.一種應用權利要求1或2所述導航定位系統進行導航定位的方法，其特征在于，包括如下步驟：

步驟1，在多個UWB基站中選取其中一個作為參考坐標原點，精確測量其他基站的相對位置坐標，從而建立參考坐標系；

步驟2，獲取UWB定位標簽到各基站的UWB測距信息；獲取IMU的加速度計和陀螺儀信息進行姿態解算，得到標簽裝置的IMU線性加速度信息；

步驟3，采用緊耦合的方式構建卡爾曼濾波器模型，對UWB測距信息和IMU線性加速度信息進行融合；所述卡爾曼濾波器模型包括位置卡爾曼濾波器模型及角度卡爾曼濾波器模型；

步驟4，啟動布置于工作區域內的定位裝置，進行位置卡爾曼濾波器模型初始化及角度卡爾曼濾波器模型初始化，得到初始位置坐標和航向角，并進入位置跟蹤狀態及角度跟蹤狀態；

步驟5，初始化完成后，實時獲取UWB定位標簽和IMU的測量數據，并對位置卡爾曼濾波器模型和角度卡爾曼濾波器模型進行實時更新，得到當前位置數據和航向角數據。

4.根據權利要求3所述的方法，其特征在于，所述卡爾曼濾波器模型用于：

系統狀態預測，計算公式為：

U(k)

上式中，A為系統的狀態轉移矩陣，表示本次系統的最優估計值,在本系統構建的公式中指由位置、速度、加速度計誤差一同構成的矩陣，表示系統的控制矩陣，表示系統的控制量，本系統中指加速度矩陣；

系統協方差預預測，計算公式為:

上式中，A為系統的狀態轉移矩陣，為本次系統先驗協方差最優化估計值，為上一次系統的先驗協方差最優化估計值，Ｑ為系統的過程噪聲協方差矩陣；

系統的卡爾曼增益計算，計算公式為:

上式中，表示k時刻系統的卡爾曼增益，H為系統的測量矩陣，R為測量噪聲協方差矩陣，；

濾波器更新，最優狀態估計公式為:

上式中，表示k時刻的系統最優狀態估計值，表示k時刻的系統測量值，為k-1時刻的系統最優狀態估計值，為k-2時刻系統的最優狀態估計值，為k時刻系統的卡爾曼增益，H為系統的測量矩陣;

濾波器更新，最優狀態協方差公式：

上式中，為k時刻系統后驗估計協方差，為k時刻系統的卡爾曼增益，H為系統的測量矩陣，為k-1時刻系統的后驗協方差最優化估計結果。