[發明專利]一種基于多傳感器融合的模型位姿測量系統與方法

有效

• 申請號: 202110320356.3
• 申請日: 2021-03-25
• 公開（公告）號: CN113267178B
• 公開（公告）日: 2023-07-07
• 發明（設計）人: 周一覽;趙帥;舒曉武
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 鄭海峰

1.一種基于多傳感器融合的模型位姿測量系統的測量方法，所述的模型位姿測量系統包括模型安裝臺(1)、慣性導航元件(2)、激光掃描裝置(3)和數據處理模塊(4)；

所述的慣性導航元件(2)固定在模型安裝臺(1)的內部，激光掃描裝置(3)的激光出射口正對于模型安裝臺(1)，所述的慣性導航元件(2)、激光掃描裝置(3)均與數據處理模塊(4)實現通訊連接；

所述的慣性導航元件(2)包括三軸加速度計、三軸光纖陀螺和無線通訊模塊，所述的無線通訊模塊與數據處理模塊無線連接；

所述的激光掃描裝置(3)包括光學陣列(9)、準直透鏡(8)、偏轉棱鏡(7)和掃描振鏡(6)，所述的光學陣列上均勻布置有若干激光發射/接收單元，由所述的激光發射/接收單元發出的激光束依次經過準直透鏡(8)、偏轉棱鏡(7)、掃描振鏡(6)后，激光束垂直于激光出射口發射至位于模型安裝臺(1)上的待測模型上，其反射光束原路返回后被數據處理模塊(4)接收；

其特征在于，所述的測量方法包括以下步驟：

1)將待測模型固定在模型安裝臺(1)上，調試三軸加速度計、三軸光纖陀螺和無線通訊模塊；

2)將激光掃描裝置(3)的激光出射口對準模型安裝臺(1)上的待測模型，開啟激光掃描裝置，由激光發射模塊(10)發射激光束，經分光棱鏡(12)反射后垂直于光學陣列(9)出射；出射的激光束先經過準直透鏡(8)準直，然后再經偏轉棱鏡(7)后入射至掃描振鏡(6)，調試掃描振鏡使得激光束垂直于激光出射口發射至位于模型安裝臺(1)上的待測模型上；

3)發射出的激光由待測模型反射后原路返回至激光掃描裝置(3)中的光學陣列(9)上，再透過分光棱鏡(12)后被激光接收模塊(11)接收；

4)將采集到的慣性導航元件的測量數據，以及激光接收模塊(11)接收到的點云數據傳輸至數據處理模塊(4)中，結合迭代擴展卡爾曼濾波解算出最終的位姿結果；

步驟4)包括：

4.1)結合慣性導航元件的測量誤差，建立系統狀態方程：

X(k)＝F(k-1)X(k-1)+G(k-1)W(k-1)

其中，X(k)為k時刻系統的六維狀態向量，F(k-1)為k-1時刻系統狀態轉移矩陣，W(k-1)為k-1時刻系統過程噪聲序列，G(k-1)為k-1時刻噪聲轉移矩陣；

δL_X,δL_Y,δL_Z分別表示系統三軸位置誤差，δα_X,δβ_Y,分別系統三軸姿態誤差，ε_AX,ε_AY,ε_AZ分別表示慣性導航元件中三軸加速度計的常值漂移誤差，ε_GX,ε_GY,ε_GZ分別表示慣性導航元件中三軸光纖陀螺的常值漂移誤差；

4.2)將慣性導航元件和激光掃描裝置解算出的信息差值作為觀測向量，將觀測方程表示為：

其中，Z(k)為k時刻系統的六維觀測向量，V(k)為k時刻系統觀測噪聲序列；H(k)為k時刻系統觀測轉移矩陣；L_INS、L_Lidar分別表示慣性導航元件和激光掃描裝置解算出的三軸位置信息；α_INS、α_Lidar分別表示慣性導航元件和激光掃描裝置解算出的三軸姿態信息；

4.3)獲取慣性導航元件測量數據，通過等效旋轉矢量更新姿態，獲得模型位姿信息，包括三軸位置信息L_INS和三軸姿態信息α_INS；

4.4)獲取激光掃描裝置采集到的點云數據，將相鄰兩幀的點云數據分別表示為P＝{p₁,p₂…p_n}和P'＝{p′₁,p′₂…p′_n}，通過點云配準，獲得模型位姿信息，包括三軸位置信息L_Lidar和三軸姿態信息α_Lidar，計算公式為：

其中，R為剛體變換旋轉矩陣，p_i為前一幀點云數據中的第i個點云位置，p_i′為后一幀點云數據中的第i個點云位置，n為每一幀點云數據中的點云數量；

4.5)利用迭代擴展卡爾曼濾波，通過迭代計算，解算出最終位姿結果。

2.根據權利要求1所述的多傳感器融合的模型位姿測量方法，其特征在于，步驟4.5)所述的迭代擴展卡爾曼濾波的迭代計算過程為：

初始化，令i＝0；

令k時刻系統的六維狀態向量X(k)作為狀態向量，k時刻系統的六維觀測向量Z(k)作為觀測向量，進行迭代計算：

其中，為h_k(·)函數在點處的雅可比矩陣，h_k(·)為局部線性化函數，為第i次迭代時k時刻的卡爾曼增益值，P_k∣k-1為k-1時刻到k時刻的一步預測誤差協方差矩陣，上角標T代表轉置，表示白噪聲序列的協方差矩陣，v表示服從高斯分布的量測噪聲，表示為k-1時刻到k時刻的狀態變量一步估計值，Z_k表示為k時刻的量測變量值，表示第i次迭代時k時刻的狀態變量估計值；

通過Gauss-Newton方法，進行非線性優化，當小于預設閾值時，停止迭代，以最后一次迭代結果作為輸出值。