[發明專利]一種面向近岸水域的實時定位與建圖方法

有效

• 申請號: 202011395614.6
• 申請日: 2020-12-03
• 公開（公告）號: CN112562052B
• 公開（公告）日: 2021-07-27
• 發明（設計）人: 徐雍;黃增鴻;陳梓杰;魯仁全;饒紅霞
• 申請（專利權）人: 廣東工業大學
• 主分類號: G06T15/00
• 分類號: G06T15/00;G06N3/04;G01C21/16;G01D21/02;G06Q10/04;G06T7/70;G08G3/02
• 代理公司: 佛山市禾才知識產權代理有限公司 44379
• 代理人: 羅凱欣;曹振

1.一種面向近岸水域的實時定位與建圖方法，其特征在于：包括以下步驟：

步驟A包括：

步驟A1：多個魚眼攝像頭采集數據后通過解算得到特征點歸一化坐標；

步驟A2：慣性測量單元獲取無人艇的慣性數據PVQB；

所述步驟A2具體為：

所述慣性測量單元通過四階Runge-kutta法預積分出當前時刻與上一時刻多視圖幀之間的慣性測量單元的慣性數據PVQB，其中所述慣性數據PVQB包括轉移矩陣速度v_k+1、旋轉矩陣重力偏置和加速度偏置

所述轉移矩陣的四階Runge-kutta法為：

所述旋轉矩陣的四階Runge-kutta法為：

其中，表示慣性測量單元中陀螺儀的測量值；

所述速度v_k+1的四階Runge-kutta法為：

其中，q＝[q_w q_v]是從四元數q轉換到旋轉矩陣R的函數，的作用是把四元數歸一化，表示慣性測量單元中加速度計的測量值，g^w表示當前位置的地球加速度；

所述重力偏置的算法為：

所述加速度偏置的算法為：

步驟A3：聲吶采集無人艇周圍聲吶點云數據；

步驟A4：北斗衛星導航系統定位無人艇當前坐標并解算出全局平面坐標系位置；

步驟A5：磁力計測量當前磁場強度的測量值并解算出無人艇姿態；

步驟A6：氣壓計測量氣壓從而計算出無人艇高度；

步驟B：

所述慣性數據PVQB進行線性插值后結合所述特征點歸一化坐標獲得多視圖幀位姿；

所述步驟B中，檢測系統是否有初始化；

當未初始化時，檢測慣性測量單元的激勵是否足夠；當所述激勵滿足閾值時，對滑窗內所有的多視圖幀做StructurefromMotion(SfM)得到路標點；根據所述路標點使用PnP算法求解出滑窗內其余幀的位姿，并使用三角測量算法恢復出滑窗內各幀所看到的路標點的三維坐標；通過對滑窗內的多視圖幀與慣性測量單元進行對齊，從而校正慣性測量單元的bias，以及各多視圖幀的速度、重力和尺度因子；

當已初始化時，使用三角測量算法恢復出當前多視圖幀所看到的路標點的三維坐標；計算慣性測量單元誤差相對于優化變量的Jacobian矩陣以及多視圖幀的重投影誤差的Jacobian矩陣；對滑窗內所有的多視圖幀進行Bundle Adjustment算法從而得到優化后的多視圖幀位姿、陀螺儀的bias、加速度計的bias、各魚眼攝像頭與慣性測量單元之間的外參以及逆深度；計算下一次滑窗優化的先驗約束；

步驟C：所述多視圖幀位姿結合所述全局平面坐標系位置、無人艇姿態、無人艇高度和聲吶點云數據通過回環檢測和建圖生成柵格地圖和三維點云地圖；

所述步驟C具體為：先檢測當前多視圖幀位姿是否為關鍵幀；

當所述多視圖幀位姿為關鍵幀時，所述多視圖幀位姿與多視圖關鍵幀數據庫進行相似度計算，相似度計算的得分大于閾值時，所述多視圖幀位姿與閉環幀進行特征點匹配，并使用PnP算法計算所述多視圖幀位姿與閉環幀之間的相對位姿；固定多視圖關鍵幀數據庫中最老的幀、添加序列邊和閉環邊，并進行四自由度的位姿圖優化形成多視圖關鍵幀，把所述多視圖關鍵幀添加到多視圖關鍵幀數據庫；

當所述多視圖幀位姿不是關鍵幀時，所述多視圖幀位姿形成多視圖非關鍵幀；

然后，通過對所述多視圖關鍵幀或多視圖非關鍵幀結合所述全局平面坐標系位置、無人艇姿態和無人艇高度的約束進行非線性最大似然估計得到無人艇的位姿，所述位姿與所述聲吶點云數據進行自適應時間戳同步算法和位姿關聯，然后使用覆蓋柵格地圖算法構建出柵格地圖以及使用降采樣濾波器構建三維點云地圖；

所述覆蓋柵格地圖算法為：

其中m_i表示第i個柵格的狀態，z_1:t表示1到t時刻的聲吶點云數據，x_1:t表示1到t時刻的無人艇位姿；

所述步驟C中，北斗衛星導航系統的全局平面坐標系位置的約束為：

其中，^BDSZ_t表示北斗衛星導航系統t時刻位姿的觀測值，^BDSh_t(X)表示北斗衛星導航系統t時刻的觀測方程，^wp_t表示無人艇t時刻在全局坐標系下的位置；

磁力計的無人艇姿態的約束為：

其中，^mh_t(X)表示磁力計t時刻的觀測方程，^wyaw′_t為磁力計在t時刻經數據解算后的無人艇在世界坐標系下的偏航角，^wyaw_t為無人艇t時刻在世界坐標系下的偏航角的估計值；

氣壓計的無人艇高度的約束為：

其中，^BZ_t表示氣壓計t時刻在全局坐標系下的觀測值，^Bh_t(X)表示氣壓計t時刻的觀測方程,^wZ_t表示無人艇t時刻在全局坐標系z軸的數值。