[發明專利]一種水下單目亞像素相對位姿估計方法

在審

• 申請號: 202011464862.1
• 申請日: 2020-12-12
• 公開（公告）號: CN112700501A
• 公開（公告）日: 2021-04-23
• 發明（設計）人: 高劍;張元旭;張福斌;張立川;陳依民;張飛虎
• 申請（專利權）人: 西北工業大學
• 主分類號: G06T7/80
• 分類號: G06T7/80;G06K9/46;G06T7/13
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 陳星

本發明提出一種水下單目亞像素相對位姿估計方法，首先，在目標對接裝置上設置光源標靶，光源標靶上具有至少5個特征光源，事先獲取特征光源中在世界坐標系下的三維坐標。其次，利用水下單目視覺系統采集光源標靶圖像，獲取像素坐標系下，圖像中每個特征點的坐標。再次，以特征點在世界坐標系下的坐標和像素坐標系下的坐標作為輸入，利用水下單目視覺系統相機的內參數矩陣，采用共面PnP算法，得到世界坐標系相對于相機坐標系的相對位姿矩陣^cM_w的粗值。最后，以得到的^cM_w粗值為初始值，采用RLS優化算法，迭代得到^cM_w精確值。本發明可以準確的得到相對位姿信息，完成水下機器人自主對接工作。

技術領域

本發明涉及水下機器人視覺技術領域，具體為一種水下單目亞像素相對位姿估計方法。

背景技術

機器人遠距離長時間自主作業時，因為機器人本身負載和水下通訊限制，需要進行對接作業進入回收裝置，進行數據傳輸和能源補給。長期以來，國內外學者在水下水聲信號對接方面取得了很好的研究成果，但由于水聲信號只在長距離位置上性能較好，近距離穩定性差、精度低、魯棒性差，不適用于近距離對接作業中。為了滿足自主對接作業的需要，人們需要實現近距離的位姿估計數據的獲取，從而控制水下機器人進入對接裝置。視覺傳感器，以其低成本、高分辨率、高幀率、低噪聲、低延遲等優點，在自主對接的相對位姿估計中具有獨特的優勢，適用于近距離、高精度的對接作業相對位姿估計。

傳統常用的近距離視覺對接作業方法主要采用的是最簡單的幾何圖形學方法來進行導引，一般以光源標靶的中心作為目標導引點，導引水下機器人按照中心點的方向進入對接裝置，或者以目標光源點構成的圖像面積、周長作為目標，通過其變化之間的關系來導引水下機器人進入對接裝置。這類傳統的近距離視覺對接作業方法存在一定的局限性和缺陷，主要表現為：首先，簡單幾何圖形學的方法可用的特征較少，水下機器人對接過程中沒有考慮到機器人本身姿態的影響，僅包含位置信息，在工程中會導致自主對接作業因為姿態錯誤最終失敗；其次，簡單幾何圖形學的方法在水下采集圖像進行提取處理時，因為水下環境的復雜性，特征的提取會產生較大誤差或者出現丟失的情況，魯棒性極差，在工程中，會出現水下機器人丟失目標，數據誤差太大，導致自主對接作業失敗。

發明內容

為解決現有技術存在的問題，本發明提出一種水下單目亞像素相對位姿估計方法，針對水下對接作業任務，利用安裝在水下機器人上的水下單目視覺系統(單目RGB相機)采集的光源標靶圖像，設計PnP位姿估計算法，并進行迭代優化，實時輸出水下機器人相對于目標對接裝置的位姿信息，該方法可以準確的得到相對位姿信息，完成水下機器人自主對接工作。

技術方案

本發明的技術原理是：

利用水下單目視覺系統得到目標光源標靶圖像中特征點光源的中心點在像素坐標系下的二維坐標，并與其在世界坐標系下的坐標作為輸入，采用共面PnP算法和RLS優化算法，對單目視覺系統與目標光源標靶間的相對位姿進行求解。

具體步驟如下：

步驟1：在目標對接裝置上設置光源標靶，光源標靶上具有至少5個特征光源，而且其中3個特征光源中心處于同一直線上，事先獲取特征光源中在某一設定的世界坐標系下的三維坐標；

步驟2：利用水下單目視覺系統采集光源標靶圖像，獲取像素坐標系下，圖像中每個特征點的坐標；

步驟3：以特征點在世界坐標系下的坐標和像素坐標系下的坐標作為輸入，并利用水下單目視覺系統相機的內參數矩陣，采用共面PnP算法，得到世界坐標系相對于相機坐標系的相對位姿矩陣^cM_w的粗值；

步驟4：以得到的^cM_w粗值為初始值，采用RLS優化算法，迭代得到^cM_w精確值。