[發明專利]一種慣導與激光點陣融合導航系統

在審

• 申請號: 202210740948.5
• 申請日: 2022-06-28
• 公開（公告）號: CN115127548A
• 公開（公告）日: 2022-09-30
• 發明（設計）人: 金乃慶;張亞洲;江冠華;朱宗衛;江石根
• 申請（專利權）人: 蘇州精源創智能科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S17/42;G01S17/86
• 代理公司: 江蘇昆成律師事務所 32281
• 代理人: 劉尚軻

1.一種慣導與激光點陣融合導航系統，其特征在于，包括：

三維點云模塊，所述三維點云模塊包括：激光雷達模塊、目標檢測模塊和點云算法模塊，目標檢測模塊是基于深度學習網絡訓練出的模型，能夠自動檢測多類別的目標，根據目標檢測模塊對激光雷達模塊采集的三維點云數據中的目標進行自動檢測；所述點云算法模塊通過計算得到運載體與被檢測物體之間的航向角、俯仰角和橫滾角；

慣性姿態模塊，所述慣性姿態模塊包括畢卡算法模塊和自適應卡爾曼濾波模塊，畢卡算法模塊將角速率信息通過四元數解算為姿態角；再利用自適應卡爾曼濾波模塊對俯仰角和橫滾角進行姿態修正和修正位置偏差；

雙矢量匹配模塊，將三維點云模塊和慣性姿態模塊輸出的航向角、俯仰角和橫滾角進行融合計算，采用雙矢量匹配模塊求解矢量差、計算出形變向量計算后的航向角、俯仰角和橫滾角。

2.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，所述激光雷達模塊包括圖像級激光雷達，目標檢測模塊根據任務類別種類設置類別參數，并對類別參數進行自動檢測和標注，僅標注目標類別。

3.如權利要求2所述的慣導與激光點陣融合導航系統，其特征在于，深度學習網絡先對三維點云數據進行對齊并進行深度學習迭代訓練，再對提取的特征信息進行聚合，同時進行深度學習迭代訓練得到目標檢測模塊；三維點云數據通過變換矩陣進行對齊，在深度學習迭代訓練中，通過loss束縛保證模型對空間轉換的不變性。

4.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，所述激光雷達根據光的飛行和反射，獲得圖形級的三維點云，包含了圖像內的多種物體類別與被檢測物體之間的距離，目標檢測模塊自動檢測和標識后，點云算法模塊通過多幀圖像計算出航向角、俯仰角和橫滾角。

5.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，加速計和陀螺儀分別檢測獲得的三軸加速度和三軸角速率原始數據，進行四階巴特沃斯低通濾波，濾除高斯噪聲。

6.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，所述畢卡算法模塊進行四元數解算，根據姿態矩陣得到載體當前時刻的姿態航向角，姿態航向角的俯仰角θ、橫滾角γ和航向角分別為公式S1：

其中，q₀、q₁、q₂、q₃分別為四元數的四個量，atan2表示arctan。

7.如權利要求6所述的慣導與激光點陣融合導航系統，其特征在于，四元數解算后的數據，采用畢卡算法進行更新，畢卡算法更新的計算公式S2為：

其中,Q_k為當前四元數，Q_k-1是前一時刻四元數，Δθ為角增量模值，ΔΘ為角增量Δθ計算中間變量，I為4維單位陣。其中，ΔΘ的計算公式S3為：

其中，△θx為三軸陀螺X軸角增量，△θy為三軸陀螺Y軸角增量，△θz為三軸陀螺Z軸角增量。

8.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，所述自適應卡爾曼濾波模塊將俯仰角、橫滾角的誤差作為狀態量并將三軸加速度計的值和激光點陣數據作為量測量進行自適應卡爾曼濾波，獲得修正的俯仰角、橫滾角和位置。

9.如權利要求8所述的慣導與激光點陣融合導航系統，其特征在于，對航向角進行校準，陀螺旋轉值A與理想值B成線性關系，陀螺值校準的計算公式為S4：

B＝180*A/K+b (S4)

其中，B為理想值，A為旋轉值，b為零偏誤差，K為校準值。

10.如權利要求1所述的慣導與激光點陣融合導航系統，其特征在于，所述雙矢量匹配模塊利用三維點云模塊與慣性姿態模塊在同一時刻輸出的三個姿態航向角的矢量差，采用公式S5計算形變向量后的航向角、俯仰角和橫滾角，所述S5為：

其中，θ(k)為k時刻最終輸出的俯仰角，θs為k時刻三維點云模塊輸入的俯仰角，θm為k時刻慣性姿態模塊輸入的俯仰角，α為靜態形變角矢量，β為動態形變角矢量，ζs為零偏，ζm為隨機游走，γ(k)為k時刻最終輸出的橫滾角，γs為k時刻三維點云模塊輸入的橫滾角，γm為k時刻慣性姿態模塊輸入的橫滾角，為k時刻最終輸出的航向角，為k時刻三維點云模塊輸入的航向角，為k時刻慣性姿態模塊輸入的航向角。