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

有效

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

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

步驟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)：

χ0(k-1)=x^(k-1)]]>

χi(k-1)=x^(k-1)+(n+λ)Pi(k-1),i=1,...,n]]>

χi(k-1)=x^(k-1)-(n+λ)Pi-n(k-1),i=n+1,...,2n]]>

其中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)：

其中χ_i(k/k-1)為點集χ(k/k-1)的第i列，系統狀態項為：

其中T是控制周期，R為載體坐標系轉換到全局坐標系下的坐標轉換矩陣；

步驟2.3：根據步驟2.2得到的變換后的Sigma點集χ(k/k-1)，進行均值加權得到系統狀態向量的預測均值

x^(k/k-1)=Σi=02nWiχi(k/k-1)]]>

其中W_i是UT變換權值；進行協方差加權得到系統狀態向量的預測方差

P‾(k)=Σi=02nWi[χi(k/k-1)-x^(k/k-1)][χi(k/k-1)-x^(k/k-1)]T+Q‾(k)]]>

其中是系統噪聲的協方差矩陣；

步驟2.4：將步驟2.2得到的變換后的Sigma點集χ(k/k-1)代入量測項進行非線性變換：

z_i(k/k-1)＝h[χ_i(k/k-1)，k/k-1]，i＝0，…，2n

其中量測項為：

h[x(k),k]＝[[m_j(k) n_j(k)]_×4 u(k) v(k) w(k) p(k) q(k) r(k)]^T

(m_j(k),n_j(k))為第j個特征點在圖像坐標系下的坐標，j＝1,2,3,4：

mj(k)=fxR11(xgj-x(k))+R21(ygj-y(k))+R31(zgj-z(k))R13(xgj-x(k))+R23(ygj-y(k))+R33(zgj-z(k))nj(k)=fyR12(xgj-x(k))+R22(ygj-y(k))+R32(zgj-z(k))R13(xgj-x(k))+R23(ygj-y(k))+R33(zgj-z(k))]]>

為第j個特征點在全局坐標系下的坐標，f_x和f_y分別是相機的兩個焦距，相機坐標系與水下機器人的載體坐標系重合；并加權求和計算得到系統量測量的預測值：

z^(k/k-1)=Σi=02nWizi(k/k-1)]]>

步驟2.5：計算更新的狀態方差：

Pz(k)z(k)=Σi=02nWi[zi(k/k-1)-z^(k/k-1)][zi(k/k-1)-z^(k/k-1)]T+R‾(k)]]>

Px(k)z(k)=Σi=02nWi[χi(k/k-1)-x^(k/k-1)][zi(k/k-1)-z^(k/k-1)]T]]>

其中是量測噪聲的協方差矩陣；計算增益矩陣：

K(k)=Px(k)z(k)Pz(k)z(k)-1]]>

估計k時刻水下機器人的系統狀態向量量測值為：

x^(k)=x^(k/k-1)+K(k)[z(k)-z^(k/k-1)]]]>

其中為量測噪聲；k時刻水下機器人的后驗狀態方差矩陣P(k)為：

P(k)=P‾(k)-K(k)Pz(k)z(k)KT(k).]]>