CN105890589A - 一种水下机器人单目视觉定位方法 - Google Patents

Abstract

本发明提出一种水下机器人单目视觉定位方法，结合多普勒和陀螺仪测得水下机器人在载体系下的线速度和角速度，得到状态方程；取4个已知在全局系下坐标的静止特征点，通过坐标系变化，得到特征点在图像系下的位置，得到量测方程；已知k‑1时刻的状态向量和其协方差矩阵，通过Unscented变换方法，求得Sigma点；再通过一次时间更新，估计k时刻的量测值；结合多普勒和陀螺仪测得航行器在载体系下的线速度和角速度，全局系下的特征点在图像系下的位置，可以得到k时刻量测值，经过量测更新，估计k时刻的状态向量，并得到k时刻状态向量的协方差矩阵。本发明突破了几何法中特征点的布置必须满足特定条件这一限制，弥补了EKF滤波只能估计位置信息这一缺陷，能同时估计位置信息和欧拉角。

Description

一种水下机器人单目视觉定位方法

技术领域

本发明涉及水下视觉定位技术领域，具体为一种水下机器人单目视觉定位方法，通过基于非线性卡尔曼滤波的位姿估计，实现水下机器人单目视觉定位。

背景技术

随着海洋开发事业的迅速发展,水下施工和建设项目越来越多,对于水下作用手段的性能要求也越来越高。水下机器人只有获得到高精度的位置和姿态信息才能实现精确控制，水下定位是水下机器人发展需要解决的关键技术之一。长期以来，人们一直致力于水声定位技术的研究，在水下机器人中长距离的目标定位及导航等方面取得了很好的研究成果，但由于水声定位系统数据更新频率低，在近距离测量中声学系统的稳定性和精度还有待进一步提高。

光视觉传感器近距离探测精度能达到厘米级，体积小且成本低，在水下机器人近距离目标定位及导航中有其独特的优势，因此光视觉传感器在水下机器人控制领域得到了广泛地研究与应用。

目前的水下机器人视觉定位主要采用几何法和扩展卡尔曼滤波(ExtendedKalman Filter,EKF)算法。几何法通过机器人上安装的相机获取视觉目标上的n个特征点在图像系下的位置坐标，利用PnP(Perspective-n-Point)算法求解了机器人相对于目标位置和姿态并在室内水池进行了实验，实验结果证明了该方法的可行性。但PnP算法在求解过程中存在着多解和鲁棒性差等问题，为了得到唯一解，特征点的布置必须满足特定条件，因此限制的其使用范围。扩展卡尔曼滤波(Extended Kalman Filter,EKF)算法估计出了机器人相对于目标的位置，利用获得位置信息进行水下机器人的运动控制，该方法已完成了水下物体打捞、水下动力定位和水下电缆插拔等实验并取得了成功。但目前利用EKF滤波的方法只用来估计机器人的位置，并没有考虑机器人的欧拉角，导致方法在实际使用中不能够有效用于水下机器人的控制。

发明内容

为了避免现有技术的不足之处，本发明提出非线性无迹卡尔曼滤波(UnscentedKalman 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是水下机器人的欧拉角，是水下机器人在载体坐标系下的线速度以及角速度，为n维实数域；

并根据设置的水下机器人在k＝0时刻的系统状态向量初值，计算水下机器人在k＝0时刻的状态方差矩阵x_i(0)为系统状态向量中的元素；

步骤2：根据k-1时刻得到的系统状态向量量测值和后验状态方差矩阵P(k-1)，结合k时刻多普勒和陀螺仪测得水下机器人在载体坐标系下的线速度和角速度，估计k时刻水下机器人的系统状态向量量测值和后验状态方差矩阵P(k)：

步骤2.1：计算Sigma点集χ(k-1)：

${\mathrm{\χ}}_0(k-1)=\hat{x}(k-1)$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)+\sqrt{(n+\mathrm{\λ})}\sqrt{P_i(k-1)},i=1,...,n$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)-\sqrt{(n+\mathrm{\λ})}\sqrt{P_{i-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)，进行均值加权得到系统状态向量的预测均值

$\hat{x}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_i(k/k-1)$

其中W_i是UT变换权值；进行协方差加权得到系统状态向量的预测方差

$\stackrel{\‾}{P}\left(k\right)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]}^T+\stackrel{\‾}{Q}\left(k\right)$

其中是系统噪声的协方差矩阵；

步骤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：

$\left\{\begin{array}{c}{m}_j\left(k\right)=f_x\frac{R_{11}(x_{g_j}-x(k\left)\right)+R_{21}(y_{g_j}-y(k\left)\right)+R_{31}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\\ n_j\left(k\right)=f_y\frac{R_{12}(x_{g_j}-x(k\left)\right)+R_{22}(y_{g_j}-y(k\left)\right)+R_{32}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\end{array}\right.$

为第j个特征点在全局坐标系下的坐标，f_x和f_y分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：

$\hat{z}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{z}_i(k/k-1)$

步骤2.5：计算更新的状态方差：

$P_{z\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[z_i(k/k-1)-\hat{z}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T+\stackrel{\‾}{R}\left(k\right)$

$P_{x\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T$

其中是量测噪声的协方差矩阵；计算增益矩阵：

$K\left(k\right)=P_{x\left(k\right)z\left(k\right)}{P}_{z\left(k\right)z\left(k\right)}^{-1}$

估计k时刻水下机器人的系统状态向量量测值为：

$\hat{x}\left(k\right)=\hat{x}(k/k-1)+K\left(k\right)\[z\left(k\right)-\hat{z}(k/k-1)\]$

其中为量测噪声；k时刻水下机器人的后验状态方差矩阵P(k)为：

$P\left(k\right)=\stackrel{\‾}{P}\left(k\right)-K\left(k\right)P_{z\left(k\right)z\left(k\right)}{K}^T\left(k\right).$

有益效果

本发明与几何法和EKF滤波的方法相比，突破了几何法中特征点的布置必须满足特定条件这一限制，弥补了EKF滤波只能估计位置信息这一缺陷，能同时估计位置信息和欧拉角，大大拓展了使用范围。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1：视觉导航系统示意图

图2：机器人在全局系下的运动轨迹

图3：像素点在图像系下的运动轨迹

图4：实际位姿(黑)和估计位姿(红)

图5：实际位姿与UKF估计位姿的误差

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例基于非线性无迹卡尔曼滤波(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是水下机器人的欧拉角，是水下机器人在载体坐标系下的线速度以及角速度，为n维实数域；

并根据设置的水下机器人在k＝0时刻的系统状态向量初值，计算水下机器人在k＝0时刻的状态方差矩阵x_i(0)为系统状态向量中的元素；

步骤2：根据k-1时刻得到的系统状态向量量测值和后验状态方差矩阵P(k-1)，结合k时刻多普勒和陀螺仪测得水下机器人在载体坐标系下的线速度和角速度，估计k时刻水下机器人的系统状态向量量测值和后验状态方差矩阵P(k)：

步骤2.1：计算Sigma点集χ(k-1)：

${\mathrm{\χ}}_0(k-1)=\hat{x}(k-1)$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)+\sqrt{(n+\mathrm{\λ})}\sqrt{P_i(k-1)},i=1,...,n$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)-\sqrt{(n+\mathrm{\λ})}\sqrt{P_{i-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)，进行均值加权得到系统状态向量的预测均值

$\hat{x}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_i(k/k-1)$

其中W_i是UT变换权值；进行协方差加权得到系统状态向量的预测方差

$\stackrel{\‾}{P}\left(k\right)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]}^T+\stackrel{\‾}{Q}\left(k\right)$

其中是系统噪声的协方差矩阵；

步骤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：

$\left\{\begin{array}{c}{m}_j\left(k\right)=f_x\frac{R_{11}(x_{g_j}-x(k\left)\right)+R_{21}(y_{g_j}-y(k\left)\right)+R_{31}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\\ n_j\left(k\right)=f_y\frac{R_{12}(x_{g_j}-x(k\left)\right)+R_{22}(y_{g_j}-y(k\left)\right)+R_{32}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\end{array}\right.$

为第j个特征点在全局坐标系下的坐标，f_x和f_y分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：

$\hat{z}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{z}_i(k/k-1)$

步骤2.5：计算更新的状态方差：

$P_{z\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[z_i(k/k-1)-\hat{z}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T+\stackrel{\‾}{R}\left(k\right)$

$P_{x\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T$

其中是量测噪声的协方差矩阵；计算增益矩阵：

$K\left(k\right)=P_{x\left(k\right)z\left(k\right)}{P}_{z\left(k\right)z\left(k\right)}^{-1}$

估计k时刻水下机器人的系统状态向量量测值为：

$\hat{x}\left(k\right)=\hat{x}(k/k-1)+K\left(k\right)\[z\left(k\right)-\hat{z}(k/k-1)\]$

其中为量测噪声；k时刻水下机器人的后验状态方差矩阵P(k)为：

$P\left(k\right)=\stackrel{\‾}{P}\left(k\right)-K\left(k\right)P_{z\left(k\right)z\left(k\right)}{K}^T\left(k\right).$

具体到本实施例

当k＝0时，设置初值

$\hat{x}\left(0\right)=(-0.5,2.5,-3.5,2\mathrm{\π}/180,-2\mathrm{\π}/180,2\mathrm{\π}/180,0.1,0.1,-0.1,-\mathrm{\π}/180,\mathrm{\π}/180,-\mathrm{\π}/180)$

P(0)＝diag(10,10,10,10π/180,10π/180,10π/180,1,1,1,π/180,π/180,π/180)

以根据k-1＝0时刻得到的系统状态向量量测值和后验状态方差矩阵P(k-1)，结合k时刻多普勒和陀螺仪测得水下机器人在载体坐标系下的线速度和角速度，估计k＝1时刻水下机器人的系统状态向量量测值和后验状态方差矩阵P(k)：

步骤2.1：计算Sigma点集χ(k-1)：

${\mathrm{\χ}}_0(k-1)=\hat{x}(k-1)$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)+\sqrt{(n+\mathrm{\λ})}\sqrt{P_i(k-1)},i=1,...,n$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)-\sqrt{(n+\mathrm{\λ})}\sqrt{P_{i-n}(k-1)},i=n+1,...,2n$

其中n＝12，λ是UT变换参数，λ＝-12+1.2000e-05；得到Sigma点集

步骤2.2：将步骤2.1得到的Sigma点集χ(k-1)代入系统状态项得到变换后的Sigma点集χ(k/k-1)：

其中χ_i(k/k-1)为点集χ(k/k-1)的第i列，系统状态项为：

其中T是控制周期，R为载体坐标系转换到全局坐标系下的坐标转换矩阵。

结合多普勒和惯性测量单元测得水下机器人在载体坐标系下的线速度和角速度，取系统噪声可得状态方程

本实施例T＝0.01，得到

本实施例得到变换后的Sigma点集为：

步骤2.3：根据步骤2.2得到的变换后的Sigma点集χ(k/k-1)，进行均值加权得到系统状态向量的预测均值

$\hat{x}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_i(k/k-1)$

其中W_i是UT变换权值；本实施例中

$\hat{x}(1/0)=\left[\begin{array}{c}-0.4992\\ 2.5009\\ -3.5008\\ 0.0347\\ -0.0347\\ 0.0347\\ 0.1000\\ 0.1000\\ -0.1000\\ -0.0175\\ 0.0175\\ -0.0175\end{array}\right]$

进行协方差加权得到系统状态向量的预测方差

$\stackrel{\‾}{P}\left(k\right)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]}^T+\stackrel{\‾}{Q}\left(k\right)$

其中是系统噪声的协方差矩阵，取

$\stackrel{\‾}{Q}\left(k\right)=diag(0.1,0.1,0.1,0.1\mathrm{\π}/180,0.1\mathrm{\π}/180,0.1\mathrm{\π}/180,1,1,1,\mathrm{\π}/180,\mathrm{\π}/180,\mathrm{\π}/180)$

得到

步骤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：取4个已知在工作空间的静特征点的全局坐标系下坐标，在水下机器人动力学模型的基础上，通过坐标系变化，得到可知全局坐标系下的特征点在图像坐标系下的位置

$\left\{\begin{array}{c}{m}_j\left(k\right)=f_x\frac{R_{11}(x_{g_j}-x(k\left)\right)+R_{21}(y_{g_j}-y(k\left)\right)+R_{31}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\\ n_j\left(k\right)=f_y\frac{R_{12}(x_{g_j}-x(k\left)\right)+R_{22}(y_{g_j}-y(k\left)\right)+R_{32}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\end{array}\right.$

为第j个特征点在全局坐标系下的坐标，f_x和f_y分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；4个已知在全局坐标系下静止特征点的坐标O₁(0.25 0.25 0)、O₂(0.25 -0.25 0)、O₃(-0.25 -0.25 0)和O₄(-0.25 0.25 0)，单位为m，相机的内参数f_x＝1000Pixel，f_y＝1000Pixel，图像坐标系的原点就是图像的中心。特征点在图像坐标系下的位置为

$\left\{\begin{array}{c}{m}_1=223.2040\\ n_1=-605.9487\\ m_2=217.1903\\ n_2=-742.7737\\ m_3=76.9334\\ n_3=-734.2584\\ m_4=82.2094\\ n_4=-598.0352\end{array}\right..$

得到非线性变换后的：

并加权求和计算得到系统量测量的预测值：

$\hat{z}(k/k-1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{z}_i(k/k-1)$

得到

$\hat{z}(1/0)=\left[\begin{array}{c}0.4597\\ -1.2356\\ 0.4538\\ -1.5322\\ 0.1632\\ -1.5033\\ 0.1709\\ -1.2103\\ 0.0001\\ 0.0001\\ -0.0001\\ -0.0000\\ 0.0000\\ -0.0000\end{array}\right].$

步骤2.5：计算更新的状态方差：

$P_{z\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[z_i(k/k-1)-\hat{z}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T+\stackrel{\‾}{R}\left(k\right)$

$P_{x\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T$

其中是量测噪声的协方差矩阵；得到

计算增益矩阵：

$K\left(k\right)=P_{x\left(k\right)z\left(k\right)}{P}_{z\left(k\right)z\left(k\right)}^{-1}$

得到

估计k时刻水下机器人的系统状态向量量测值为：

$\hat{x}\left(k\right)=\hat{x}(k/k-1)+K\left(k\right)\[z\left(k\right)-\hat{z}(k/k-1)\]$

其中为量测噪声；

$h\[x\left(1\right),1\]=\left[\begin{array}{c}223.2040\\ -605.9487\\ 217.1903\\ -742.7737\\ 76.9334\\ -734.2584\\ 82.2094\\ -598.0352\\ 0.1000\\ 0.1000\\ -0.1000\\ -0.0175\\ 0.0175\\ -0.0175\end{array}\right]$

得到

$\hat{x}\left(1\right)=\left[\begin{array}{c}-0.0539\\ 1.7708\\ -5.4851\\ 0.0440\\ -0.0697\\ 0.0189\\ 0.1010\\ 0.1015\\ -0.0966\\ -0.0181\\ 0.0101\\ -0.0203\end{array}\right]$

k时刻水下机器人的后验状态方差矩阵P(k)为：

$P\left(k\right)=\stackrel{\‾}{P}\left(k\right)-K\left(k\right)P_{z\left(k\right)z\left(k\right)}{K}^T\left(k\right)$

得到

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

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是水下机器人的欧拉角，是水下机器人在载体坐标系下的线速度以及角速度，为n维实数域；

并根据设置的水下机器人在k＝0时刻的系统状态向量初值，计算水下机器人在k＝0时刻的状态方差矩阵x_i(0)为系统状态向量中的元素；

步骤2：根据k-1时刻得到的系统状态向量量测值和后验状态方差矩阵P(k-1)，结合k时刻多普勒和陀螺仪测得水下机器人在载体坐标系下的线速度和角速度，估计k时刻水下机器人的系统状态向量量测值和后验状态方差矩阵P(k)：

步骤2.1：计算Sigma点集χ(k-1)：

${\mathrm{\χ}}_0(k-1)=\hat{x}(k-1)$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)+\sqrt{(n+\mathrm{\λ})}\sqrt{P_i(k-1)},i=1,\mathrm{...},n$

${\mathrm{\χ}}_i(k-1)=\hat{x}(k-1)-\sqrt{(n+\mathrm{\λ})}\sqrt{P_{i-n}(k-1)},i=n+1,\mathrm{...},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)，进行均值加权得到系统状态向量的预测均值

$\hat{x}(k/k-1)=\underset{i=0}{\overset{2n}{\Σ}}{W}_i{\mathrm{\χ}}_i(k/k-1)$

其中W_i是UT变换权值；进行协方差加权得到系统状态向量的预测方差

$\stackrel{\‾}{P}\left(k\right)=\underset{i=0}{\overset{2n}{\Σ}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]}^T+\stackrel{\‾}{Q}\left(k\right)$

其中是系统噪声的协方差矩阵；

步骤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：

$\left\{\begin{array}{c}{m}_j\left(k\right)=f_x\frac{R_{11}(x_{g_j}-x(k\left)\right)+R_{21}(y_{g_j}-y(k\left)\right)+R_{31}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\\ n_j\left(k\right)=f_y\frac{R_{12}(x_{g_j}-x(k\left)\right)+R_{22}(y_{g_j}-y(k\left)\right)+R_{32}(z_{g_j}-z(k\left)\right)}{R_{13}(x_{g_j}-x(k\left)\right)+R_{23}(y_{g_j}-y(k\left)\right)+R_{33}(z_{g_j}-z(k\left)\right)}\end{array}\right.$

为第j个特征点在全局坐标系下的坐标，f_x和f_y分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：

$\hat{z}(k/k-1)=\underset{i=0}{\overset{2n}{\Σ}}{W}_i{z}_i(k/k-1)$

步骤2.5：计算更新的状态方差：

$P_{z\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\Σ}}{W}_i\[z_i(k/k-1)\hat{z}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T+\stackrel{\‾}{R}\left(k\right)$

$P_{x\left(k\right)z\left(k\right)}=\underset{i=0}{\overset{2n}{\Σ}}{W}_i\[{\mathrm{\χ}}_i(k/k-1)-\hat{x}(k/k-1)\]{\[z_i(k/k-1)-\hat{z}(k/k-1)\]}^T$

其中是量测噪声的协方差矩阵；计算增益矩阵：

$K\left(k\right)=P_{x\left(k\right)z\left(k\right)}{P}_{z\left(k\right)z\left(k\right)}^{-1}$

估计k时刻水下机器人的系统状态向量量测值为：

$\hat{x}\left(k\right)=\hat{x}(k/k-1)+K\left(k\right)\[z\left(k\right)-\hat{z}(k/k-1)\]$

其中 为量测噪声；k时刻水下机器人的后验状态方差矩阵P(k)为：

$P\left(k\right)=\stackrel{\‾}{P}\left(k\right)-K\left(k\right)P_{z\left(k\right)z\left(k\right)}{K}^T\left(k\right).$