[發(fā)明專利]一種單目視覺水下航行器導(dǎo)航定位校正方法

有效

• 申請?zhí)枺?/td>: 201811028732.6
• 申請日: 2018-09-01
• 公開（公告）號: CN109211240B
• 公開（公告）日: 2021-06-18
• 發(fā)明（設(shè)計）人: 孫玉山;唐同澤;張國成;王占緣;張強;盛明偉;王元慶;張宸鳴
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種單目視覺水下航行器導(dǎo)航定位校正方法，其特征在于，包括：

步驟一：通過將水下航行器航行到預(yù)定的定位校正區(qū)域，根據(jù)單目視覺辨識與量測子系統(tǒng)識別功能，追蹤到水下標(biāo)志物；

步驟二：通過調(diào)整水下航行器姿態(tài)，控制水下航行器向步驟一所述的水下標(biāo)志物航行一段距離，航行過程中，根據(jù)單目視覺辨識與量測子系統(tǒng)采集得到圖像信息，根據(jù)航位推算子系統(tǒng)記錄得到這段時間內(nèi)水下航行器位置、水下航行器位移及水下航行器姿態(tài)信息；

步驟三：通過將水下航行器圍繞步驟一所述的水下標(biāo)志物運動至不同位置，重復(fù)步驟二的全部操作，在所述的不同位置重復(fù)4次以上；

步驟四：通過匯總單目視覺辨識與量測子系統(tǒng)的輸出信息以及航位推算子系統(tǒng)的輸出信息，進(jìn)行回歸優(yōu)化分析計算，得到此時水下航行器相對步驟一所述的水下標(biāo)志物的位置信息；

步驟五：通過計算水下航行器的導(dǎo)航定位校正值，完成導(dǎo)航定位校正。

2.根據(jù)權(quán)利要求1所述的一種單目視覺水下航行器導(dǎo)航定位校正方法，其特征在于，所述步驟一，包括：

通過將水下航行器航行到預(yù)定的定位校正區(qū)域，根據(jù)單目視覺辨識與量測子系統(tǒng)識別功能，追蹤到已知位置信息的水下標(biāo)志物；

其中，所述的水下標(biāo)志物的位置信息已知，記為X＝[x，y，z]，預(yù)定的水下標(biāo)志物為人為布置或海底自然標(biāo)志物，其具體大小未知，但其大小不超過視覺量測工作距離的

3.根據(jù)權(quán)利要求1所述的一種單目視覺水下航行器導(dǎo)航定位校正方法，其特征在于：所述步驟二，包括：

通過調(diào)整水下航行器姿態(tài)，控制水下航行器向步驟一所述的水下標(biāo)志物航行一段距離，航行過程中，根據(jù)單目視覺辨識與量測子系統(tǒng)采集得到圖像信息，根據(jù)航位推算子系統(tǒng)記錄得到這段時間內(nèi)水下航行器位置、水下航行器位移及水下航行器姿態(tài)信息；

其中，所述的得到這段時間內(nèi)水下航行器位置、水下航行器位移及水下航行器姿態(tài)信息的具體方法為：

第一步：調(diào)整水下航行器姿態(tài)，使攝像頭朝向步驟一所述的水下標(biāo)志物，采集并記錄此時水下航行器的位置向量姿態(tài)角向量圖像中水下標(biāo)志物的置信度P、大小(w,h)；

上式中，上標(biāo)G表示相對于大地坐標(biāo)系；

第二步：控制水下航行器朝向步驟一所述的水下標(biāo)志物航行Δt秒，記錄此時水下航行器的位置信息、水下航行器的姿態(tài)角信息以及圖像中步驟一所述的水下標(biāo)志物信息；

第三步：保持水下航行器姿態(tài)，重復(fù)第二步4次以上，采集記錄此時水下航行器的位置向量姿態(tài)角向量A_i以及圖像中水下標(biāo)志物的大小(w_i,h_i)；

上式中，角標(biāo)i表示第i次采集到信息；

第四步：根據(jù)視覺量測算法計算獲得水下標(biāo)志物距離攝像頭坐標(biāo)系下的位置向量攝像頭朝向水下標(biāo)志物時，標(biāo)志物在視覺系統(tǒng)采集到圖像的正中央，因此

視覺量測算法為：水下航行器在預(yù)定的定位校正區(qū)域(O₁)處時，大小為|AB|的標(biāo)志物在攝像機中的成像大小為|A₁B₁|；水下航行器在朝向所述的水下標(biāo)志物航行Δt秒后的位置(O₂)處時，大小為|AB|的標(biāo)志物在攝像機中的成像大小為|A₂B₂|，由幾何關(guān)系得：

上式中，s為水下航行器兩次采樣前后的位移，由航位推算獲得，求解上式獲得位置向量同理求解采樣時的其他位置向量

成像大小|A_iB_i|由圖像中標(biāo)志物的大小(w_i,h_i)按下式計算：

上式中，dx、dy為攝像機中感光單元中單個像素的實際物理尺寸，其值通過攝像機標(biāo)定求出；

考慮到在安裝時，攝像頭坐標(biāo)系與水下航行器隨體坐標(biāo)系之間存在安裝偏差，則隨體坐標(biāo)系下標(biāo)志物的位置向量由下式得：

上式中，上標(biāo)C表示相對于攝像機坐標(biāo)系，表示安裝偏角引起的旋轉(zhuǎn)矩陣，表示安裝位置偏移引起的偏移向量，其值在安裝攝像頭時確定。