[發明專利]面向三維空間的組合定位方法及其系統

有效

• 申請號: 201810339917.2
• 申請日: 2018-04-16
• 公開（公告）號: CN110388916B
• 公開（公告）日: 2022-10-28
• 發明（設計）人: 張道寧;歐陽高
• 申請（專利權）人: 北京凌宇智控科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00;H04W64/00
• 代理公司: 北京汲智翼成知識產權代理事務所(普通合伙) 11381
• 代理人: 陳曦;董燁飛

1.一種面向三維空間的組合定位方法，其特征在于在慣性測量單元進行位置積分的過程中，

當定位基站獲取定位信息后，執行基于卡爾曼濾波算法的預測環節和更新環節，具體步驟為：

使用誤差量作為濾波器的狀態量，令誤差量為位置誤差、速度誤差、姿態誤差、加速度計零值漂移及陀螺儀零值漂移，表示為：

則狀態轉移方程為

對誤差方程求導得：

所述預測環節為：

在量測方程中，為了實現卡爾曼濾波算法的非線性處理，需要對量測方程進行一階梯度近似，則量測誤差量關于狀態誤差量的偏導數為

所述更新環節為：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P

其中，K為卡爾曼增益，V為量測噪聲，P為量測噪聲的協方差，H為測量系統的參數，Q為系統噪聲的協方差，F為狀態轉移矩陣，不帶前綴的量代表實際量，其下綴里面的“k”或“t”代表該量是第k或第t時刻的實際量；帶前綴“^”的量代表預測量；帶前綴“～”的量代表與預測量對應的偏差值；

當收到所述定位基站的數據時，將所述慣性測量單元輸出的位置信息和所述定位基站提供的位置信息進行融合，利用所述更新環節提供的概率最優模型對所述慣性測量單元的當前位置進行矯正。

2.如權利要求1所述的組合定位方法，其特征在于在使用卡爾曼濾波算法的過程中，對狀態量進行一階近似。

3.如權利要求1所述的組合定位方法，其特征在于：

對所述慣性測量單元相對虛擬坐標系的姿態進行參數標定，實現外參估計。

4.如權利要求3所述的組合定位方法，其特征在于通過如下步驟實現外參估計：

在定位基站被放置并保持靜止的情況下，在虛擬坐標系上三個軸上分別安裝加速度計，用于感知重力在三個軸方向的分解力大小；

根據所述重力的分解情況，獲得靜止狀態下虛擬坐標系相對大地坐標系之間的相對姿態。

5.如權利要求1所述的組合定位方法，其特征在于：

所述定位基站從多個視角對平面標定板進行投影成像，獲取所述定位基站相對于所述平面標定板的姿態矩陣；

解算包括姿態參數的方程組，得到內參模型和外參模型的所有參數值。

6.一種面向三維空間的組合定位系統，包括慣性測量單元和三維空間定位設備；所述三維空間定位設備中包括定位基站，所述定位基站中的兩個激光發射源發射激光束分別照射到一字鏡上，形成兩個垂直的激光平面；電機轉動帶動所述一字鏡進行勻速旋轉運動，在三維空間內形成不斷旋轉的激光平面；其特征在于：

在慣性測量單元進行位置積分的過程中，當收到定位基站的數據時，將所述慣性測量單元輸出的位置信息和所述定位基站提供的位置信息進行融合，利用權利要求1～5中任意一項所提供的概率最優模型對所述慣性測量單元的當前位置進行矯正。

7.如權利要求6所述的組合定位系統，其特征在于：

所述概率最優模型是基于卡爾曼濾波算法的預測環節和更新環節得到的。