[發明專利]一種多模式自切換的管線儀自適應精密定位方法

有效

• 申請號: 202210178916.0
• 申請日: 2022-02-25
• 公開（公告）號: CN114608570B
• 公開（公告）日: 2023-06-30
• 發明（設計）人: 周澤波;張曉丹
• 申請（專利權）人: 電子科技大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C22/00;G06F18/2415;G06F18/27
• 代理公司: 電子科技大學專利中心 51203
• 代理人: 陳一鑫

1.一種多模式自切換的管線儀自適應精密定位方法，該方法包括數據采集和數據計算，所述數據采集包括：

步驟1：將慣性測量單元放入管線儀中時，與管道的右向、前向、天向保持對齊；

步驟2：將管線儀放置于發球站內；

步驟3：保持靜止3～5分鐘，配置初始偏航角與初始位置；

步驟4：待靜止初始化階段結束后，開始牽引運動，并在運動過程中標記多個地面標志點位置信息；

步驟5：待管線儀到達收球筒后，再靜置3～5分鐘，并測量管線儀的末端偏航角與末端位置；

步驟6：將所采集的慣性測量單元原始數據、里程計原始數據、地面標志點位置以及起末偏航信息融合處理，以獲取管線儀高精度運動軌跡；

所述數據計算包括：

步驟1：獲取慣性測量單元原始數據、里程計原始數據、地面標志點位置；

步驟2：進行原始數據預處理；

步驟2-1：設置閾值剔除角速率粗差；

步驟2-2：設置閾值剔除比力粗差；

步驟2-3：設置閾值剔除里程計粗差；

步驟2-4：使用有限脈沖響應濾波器對慣性測量單元的數據濾波；

步驟2-5：標注里程計打滑時段；

(1)對里程計的輸出速度求導，計算加速度a_ODO；

(2)計算當前慣性測量單元姿態捷聯矩陣

其中表示的微分形式，表示載體系相對于地心慣性坐標系的旋轉角速率，表示導航系相對于i系的旋轉角速率，分別表示的反對稱矩陣；

(3)計算慣性測量單元測量的載體加速度a_fsf：

其中f_x,f_y,f_z分別表示加速度計輸出的三軸比力信息，為n系至b系的旋轉矩陣，g為當地重力加速度；

(4)計算里程計與慣性測量單元導出的加速度差值δ_v：

δ_v＝|a_fsf|-|a_ODO|?(3)

(5)給定加速度閾值k_dv以標注里程輪打滑時段；

步驟3：利用機器學習方法識別管線儀在各時刻下的運動模態；

步驟3-1：生成訓練樣本與測試樣本；

步驟3-2：構造多元線性回歸分類模型；

步驟3-3：定義損失函數；

步驟3-4：訓練模型并獲得模型關鍵參數；

步驟3-5：將測試數據集導入訓練模型中獲取分類結果；

步驟3-6：按分類結果記錄對應的時間區間；

步驟4：設計多模式自切換的卡爾曼濾波器進行精密定位解算；

步驟4-1：搭建狀態模型、初始對準、消除常值零偏；

(1)建立15維系統誤差狀態模型如下：

其中

狀態向量X依次包含：平臺失準角φ、速度誤差δvⁿ、位置誤差δp、陀螺儀零偏誤差ε^b、加速度計零偏誤差其中，位置誤差依次包含緯度誤差δL、經度誤差δλ、高度誤差δh，其他元素依次包含相應的x、y、z三軸分量；和分別為陀螺角速率和加速度計比力測量白噪聲，w_rG和w_rA表示一階馬爾科夫過程激勵白噪聲；在狀態轉移矩陣F中，τ_Gi和τ_Ai分別為陀螺儀與加速度計的相關時間常數，其大小應根據傳感器特性測試分析結果而設定，其他符號取值如下：

M_av＝M₂,M_ap＝M₁+M₃

M_vp＝(vⁿ×)(2M₁+M₃)+M₄

其中：

式中表示比力在導航系下的投影；vⁿ表示載體在導航系下的速度，其北向分量與東向分量分別表示為v_N、v_E；L、h表示載體所處的緯度與高程；R_Mh＝R_M+h，表示子午圈主曲率半徑與高程之和；R_Nh＝R_N+h，表示卯酉圈主曲率半徑與高程之和；ω_ie表示地球自轉角速率；g_e表示赤道重力；β,β₁,β₂為地球重力模型中的常數；

(2)初始對準

通過初始靜止階段的加速度計輸出數據計算初始俯仰角θ₀與滾轉角γ₀：

初始偏航角ψ₀與初始位置由外界測量得到，又知初始狀態下三軸速度均為0，則初始對準階段完成；

(3)采用如下方式消除常值零偏：

D_GYRO＝D′_GYRO-m_GYRO?(7)

其中下標GYRO與ACC分別表示陀螺儀與加速度計；D與D'分別表示處理后和處理前的數據；m_GYRO,m_ACC意為這段靜止數據的平均值；

步驟4-2：進行當前運動模態的智能判決；

步驟4-3：若當前處于靜止狀態，則采用零速修正(ZUPT)模式，否則轉至步驟4-4；

步驟4-4：若當前處于轉彎或過焊接口狀態，則采用純慣性導航力學編排方程進行位置、速度以及姿態解算，否則轉至步驟4-5；

步驟4-5：若當前為直線運動狀態，且俯仰角小于閾值k_θ，則采用SINS/ODO/AGM/HC/YC/PC的完整信息求解模式，否則轉至步驟4-6；

步驟4-6：采用SINS/ODO/AGM/PC/YC模式；

若步驟4-5的判定均不滿足，則判定載體做“非水平直線運動”，采用“SINS/ODO/AGM/YC/PC”組合定位，這一步驟與步驟4-5的主要區別在于屏蔽了HC模式；

步驟4-6-1搭建SINS/ODO/AGM/HC濾波模型

(1)若當前時刻同時存在里程計速度觀測量、AGM位置觀測量，則觀測方程為：

若此時恰處在里程計打滑區間內，則應適當放大里程計觀測噪聲V⁽³⁾；

(2)若當前時刻存在里程計速度觀測量、無AGM位置觀測量，則觀測方程為：

若此時恰處在里程計打滑區間內，則應適當放大里程計觀測噪聲V⁽³⁾；

(3)若當前時刻無里程計速度觀測量、存在AGM位置觀測量，則觀測方程為：

Z⁽⁴⁾＝[0_3×6?I_3×3?0_3×6]X+V⁽⁴⁾?(11)

(4)若當前時刻里程計速度觀測量無效、AGM位置觀測量無效，則不使用量測修正；

通過上述步驟對系統狀態量進行一步估計，再用平臺失準角修正載體姿態，得到管線儀的俯仰角以及偏航角，分別記為：

步驟4-6-2：與步驟4-5-2一致；

步驟4-6-3：與步驟4-5-3一致；

步驟4-7：存儲當前解算狀態參數及其方差-協方差矩陣；

步驟4-8：提取下一時刻的采樣數據，返回步驟4-2，直至所有數據解算完成；

步驟5：引入反向濾波平滑策略，進一步優化運動軌跡；

步驟5-1：反向濾波系統的初始化；

步驟5-2：反向濾波平滑的計算步驟與前向濾波執行邏輯類似，參考步驟4-2至4-8，此處不再贅述；

步驟5-3：將正向與反向計算結果進行加權融合并輸出最終優化軌跡。