[發明專利]一種水下機器人單目視覺定位方法

有效

• 申請號: 201610207731.2
• 申請日: 2016-04-05
• 公開（公告）號: CN105890589B
• 公開（公告）日: 2017-12-22
• 發明（設計）人: 高劍;孫笑笑;嚴衛生;張福斌;夏飛;崔榮鑫
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/20;G01C11/00;G01S13/88;G01S13/86
• 代理公司: 西北工業大學專利中心61204
• 代理人: 陳星

技術領域

本發明涉及水下視覺定位技術領域，具體為一種水下機器人單目視覺定位方法，通過基于非線性卡爾曼濾波的位姿估計，實現水下機器人單目視覺定位。

背景技術

隨著海洋開發事業的迅速發展,水下施工和建設項目越來越多,對于水下作用手段的性能要求也越來越高。水下機器人只有獲得到高精度的位置和姿態信息才能實現精確控制，水下定位是水下機器人發展需要解決的關鍵技術之一。長期以來，人們一直致力于水聲定位技術的研究，在水下機器人中長距離的目標定位及導航等方面取得了很好的研究成果，但由于水聲定位系統數據更新頻率低，在近距離測量中聲學系統的穩定性和精度還有待進一步提高。

光視覺傳感器近距離探測精度能達到厘米級，體積小且成本低，在水下機器人近距離目標定位及導航中有其獨特的優勢，因此光視覺傳感器在水下機器人控制領域得到了廣泛地研究與應用。

目前的水下機器人視覺定位主要采用幾何法和擴展卡爾曼濾波(Extended Kalman Filter,EKF)算法。幾何法通過機器人上安裝的相機獲取視覺目標上的n個特征點在圖像系下的位置坐標，利用PnP(Perspective-n-Point)算法求解了機器人相對于目標位置和姿態并在室內水池進行了實驗，實驗結果證明了該方法的可行性。但PnP算法在求解過程中存在著多解和魯棒性差等問題，為了得到唯一解，特征點的布置必須滿足特定條件，因此限制的其使用范圍。擴展卡爾曼濾波(Extended Kalman Filter,EKF)算法估計出了機器人相對于目標的位置，利用獲得位置信息進行水下機器人的運動控制，該方法已完成了水下物體打撈、水下動力定位和水下電纜插拔等實驗并取得了成功。但目前利用EKF濾波的方法只用來估計機器人的位置，并沒有考慮機器人的歐拉角，導致方法在實際使用中不能夠有效用于水下機器人的控制。

發明內容

為了避免現有技術的不足之處，本發明提出非線性無跡卡爾曼濾波(Unscented Kalman Filter,UKF)的方法聯合估計水下機器人的狀態和參數。

根據水下機器人數學模型，結合多普勒和陀螺儀測得水下機器人在載體系下的線速度和角速度，得到狀態方程；取4個已知在全局系下坐標的靜止特征點，在水下機器人動力學模型的基礎上，通過坐標系變化，得到可知全局系下的特征點在圖像系下的位置，得到量測方程；已知k-1時刻的狀態向量和其協方差矩陣，通過Unscented變換方法，求得Sigma點；再通過一次時間更新，估計k時刻的量測值；結合多普勒和陀螺儀測得航行器在載體系下的線速度和角速度，全局系下的特征點在圖像系下的位置，可以得到k時刻量測值，經過量測更新，估計k時刻的狀態向量，并得到k時刻狀態向量的協方差矩陣。

本發明的技術方案為：

所述一種水下機器人單目視覺定位方法，其特征在于：包括以下步驟：

步驟1：設置計算時刻k＝0，并設置水下機器人在k＝0時刻的系統狀態向量初值：x(k)＝[x(k) y(k) z(k) φ(k) θ(k) ψ(k) u(k) v(k) w(k) p(k) q(k) r(k)]^T其中[x(k) y(k) z(k)]^T是水下機器人在全局坐標系下的位置，[φ(k) θ(k) ψ(k)]^T是水下機器人的歐拉角，[u(k) v(k) w(k) p(k) q(k) r(k)]^T∈R⁶是水下機器人在載體坐標系下的線速度以及角速度，Rⁿ為n維實數域；

并根據設置的水下機器人在k＝0時刻的系統狀態向量初值，計算水下機器人在k＝0時刻的狀態方差矩陣x_i(0)為系統狀態向量中的元素；

步驟2：根據k-1時刻得到的系統狀態向量量測值和后驗狀態方差矩陣P(k-1)，結合k時刻多普勒和陀螺儀測得水下機器人在載體坐標系下的線速度和角速度，估計k時刻水下機器人的系統狀態向量量測值和后驗狀態方差矩陣P(k)：

步驟2.1：計算Sigma點集χ(k-1)：

其中n為系統狀態向量的維數，χ_i(k-1)為點集χ(k-1)的第i列，P_i(k-1)為P(k-1)的第i列，λ是UT變換參數；

步驟2.2：將步驟2.1得到的Sigma點集χ(k-1)代入系統狀態項得到變換后的Sigma點集χ(k/k-1)：