[發明專利]一種高冗余的室內煤場導航方法及系統有效
| 申請號： | 202011026735.3 | 申請日： | 2020-09-25 |
| 公開（公告）號： | CN112229392B | 公開（公告）日： | 2022-11-18 |
| 發明（設計）人： | 周立群;沈潤杰;劉晨旭;修正平;趙紫燕;何宗源 | 申請（專利權）人： | 福建華電可門發電有限公司;上海樞創科技有限公司 |
| 主分類號： | G01C21/00 | 分類號： | G01C21/00;G01C21/16;G01C21/20;G01S19/48;G01S19/49;H04W4/021;H04W4/024;H04W4/33 |
| 代理公司： | 杭州橙知果專利代理事務所(特殊普通合伙) 33261 | 代理人： | 李品 |
| 地址： | 350500 福建省福州市*** | 國省代碼： | 福建;35 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 冗余 室內 煤場 導航 方法 系統 | ||
1.一種高冗余的室內煤場導航方法，其特征在于，包括步驟：
S1.分別獲取設置于煤場的UWB子系統、INS子系統、SLAM子系統的導航數據；
S2.判斷獲取的SLAM子系統的導航數據是否合格，若是，則選取SLAM子系統的導航數據與UWB子系統的導航數據作為待融合數據；若否，則選取INS子系統的導航數據與UWB子系統的導航數據作為待融合數據；
S3.根據所述待融合數據建立卡爾曼濾波模型，根據所述建立的卡爾曼濾波模型輸出SLAM子系統或INS子系統誤差量的估計值，得到煤場的定位數據。
2.根據權利要求1所述的一種高冗余的室內煤場導航方法，其特征在于，所述步驟S1中獲取的UWB子系統的導航數據為無人機當前時刻的位置xU；獲取的INS子系統的導航數據為無人機當前的加速度aI；獲取的SLAM子系統的導航數據為無人機當前時刻的位置加速度aS、速度vS、位置xS。
3.根據權利要求2所述的一種高冗余的室內煤場導航方法，其特征在于，所述步驟S2具體為：
S21.分別計算SLAM子系統的位置xS與UWB子系統的位置xU之間的差值Δx以及協方差E；
S22.判斷計算得到差值Δx是否超過第一預設閾值和/或判斷計算得到的協方差E是否超過第二預設閾值，若是，則選取INS子系統的導航數據與UWB子系統的導航數據作為待融合數據；若否，則取SLAM子系統的導航數據與UWB子系統的導航數據作為待融合數據。
4.根據權利要求3所述的一種高冗余的室內煤場導航方法，其特征在于，所述步驟S2中選取SLAM子系統的導航數據與UWB子系統的導航數據作為待融合數據具體為：
選取SLAM子系統中的加速度誤差、速度誤差、位置誤差，并將選取的加速度誤差、速度誤差、位置誤差作為組合導航系統的狀態變量，表示為：
X(t)＝[ax，ay，az，δVx，δVy，δVz，δLx，δLy，δLz]T
其中，ax，ay，az分別表示x、y、z軸的加速度誤差；δVx，δVy，δVz分別表示x、y、z軸的速度誤差；δLx，δLy，δLz分別表示x、y、z軸的位置誤差；
根據運動學公式，建立狀態變量更新的狀態方程：
其中，Fk表示狀態轉移矩陣；Gk表示噪聲驅動矩陣；Wk表示均值為0，方差為Q的噪聲。表示k-1時刻狀態量的后驗，表示k時刻狀態量的先驗。
Wk＝[ωx(k)，ωy(k)，ωz(k)]T
選取SLAM子系統與UWB子系統中的位置差值Δx，并將選取的位置差值Δx作為組合導航系統的觀測量，表示為：
Z(t)＝xS-xU＝Δx＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪聲。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于福建華電可門發電有限公司;上海樞創科技有限公司，未經福建華電可門發電有限公司;上海樞創科技有限公司許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202011026735.3/1.html，轉載請聲明來源鉆瓜專利網。
