[發明專利]從多波束聲吶數據中建立地圖的魯棒優化方法和系統

有效

• 申請號: 202011112700.1
• 申請日: 2020-10-16
• 公開（公告）號: CN112581610B
• 公開（公告）日: 2023-06-13
• 發明（設計）人: 陳德山;劉衡睿
• 申請（專利權）人: 武漢理工大學
• 主分類號: G06T17/05
• 分類號: G06T17/05;G06T19/00;G01S15/89
• 代理公司: 廣州嘉權專利商標事務所有限公司 44205
• 代理人: 胡輝

1.一種從多波束聲吶數據中建立地圖的魯棒優化方法，其特征在于，包括以下步驟：

獲取一組聲吶圖像數據以及所述聲吶圖像數據對應的水下航行器的航行數據；所述航行數據包括時間戳數據、聲吶朝向角數據、航行朝向角數據、深度數據和航行速度數據；

根據水下航行器的航行數據建立水下航行器位姿優化模型；

其中，所述水下航行器位姿優化模型N為：

N＝(Y_t-X_t)^TE*A^-1(Y_t-X_t)+(ΔY_t-ΔX_t)^TB^-1(ΔY_t-ΔX_t)；

其中，ΔY_t＝Y_t-Y_t-1，ΔX_t＝X_t-X_t-1；A為高斯分布協方差矩陣；A^-1為信息矩陣，用于表示傳感器的誤差，E為設定信任矩陣，t為行情器的航行時間，Y_t表示時間為t的待優化數據，X_t表示時間為t的航行數據；

根據所述聲吶圖像數據，構建地圖點云關聯模型；其中，所述聲吶圖像數據包括目標輪廓單線圖像，根據所述目標輪廓單線圖像與航行器的航行數據，計算出目標點世界坐標系中的位置信息作為依據，從而建立相鄰兩幀目標點的聯系；

根據所述行器位姿優化模型和所述點云關聯模型構建位姿優化代價函數；

通過最小二乘法對所述位姿優化代價函數進行求解，得到優化量；

根據所述優化量和所述聲吶圖像數據得到優化后的水下環境數據。

2.根據權利要求1所述的從多波束聲吶數據中建立地圖的魯棒優化方法，其特征在于，其中，X_t和Y_t均為五維向量，X_t和Y_t中的參數均包括俯仰角數據pitch、偏航角數據roll、翻滾角數據yaw、航行朝向角數據cog和深度數據depth。

3.根據權利要求1所述的從多波束聲吶數據中建立地圖的魯棒優化方法，其特征在于，所述地圖點云關聯模型M為：

M＝∑min[α，min^j{(f(Y_t，Z_t)ⁱ-f(Y_t-1，Z_t-1)^j)^T*C^-1*(f(Y_t，Z_t)ⁱ-f(Y_t-1，Z_t-1)^j)}]；

其中，C為高斯協方差矩陣，C^-1為信息矩陣；Z_t表示在t時刻下的聲吶圖像數據在本地坐標系中的坐標值；j表示在t-1時刻下的聲吶圖像數據映射到三維空間中的第j個點云；內層min表示在t-1時刻下的空間映射函數前三維數據與t時刻下的空間映射函數前三位數據中最近的點；f(Y_t，Z_t)ⁱ為空間映射函數，其結果為五維向量。

4.根據權利要求3所述的從多波束聲吶數據中建立地圖的魯棒優化方法，其特征在于，所述f(Y_t，Z_t)ⁱ＝[R(pitch，roll，yaw，cog)*Z_t，cog，depth]；

其中，R為旋轉矩陣函數；i表示在t時刻下三維空間中的第i個點云，即函數結果的前三維向量，α為最大距離系數；其中，

5.根據權利要求3所述的從多波束聲吶數據中建立地圖的魯棒優化方法，其特征在于，所述位姿優化代價函數F為：