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

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN105890589A(43)申请公布日2016.08.24(21)申请号201610207731.2G01S13/86(2006.01)(22)申请日2016.04.05(71)申请人西北工业大学地址710072陕西省西安市友谊西路127号(72)发明人高剑孙笑笑严卫生张福斌夏飞崔荣鑫(74)专利代理机构西北工业大学专利中心61204代理人陈星(51)Int.Cl.G01C21/00(2006.01)G01C21/16(2006.01)G01C21/20(2006.01)G01C11/00(2006.01)G01S13/88(2006.01)权利要求书2页说明书19页附图5页(54)发明名称一种水下机器人单目视觉定位方法(57)摘要本发明提出一种水下机器人单目视觉定位方法，结合多普勒和陀螺仪测得水下机器人在载体系下的线速度和角速度，得到状态方程；取4个已知在全局系下坐标的静止特征点，通过坐标系变化，得到特征点在图像系下的位置，得到量测方程；已知k-1时刻的状态向量和其协方差矩阵，通过Unscented变换方法，求得Sigma点；再通过一次时间更新，估计k时刻的量测值；结合多普勒和陀螺仪测得航行器在载体系下的线速度和角速度，全局系下的特征点在图像系下的位置，可以得到k时刻量测值，经过量测更新，估计k时刻的状态向量，并得到k时刻状态向量的协方差矩阵。本发明突破了几何法中特征点的布置必须满足特定条件这一限制，弥补了EKF滤波只能估计位置信息这一缺陷，能同时估计位置信息和欧拉角。CN105890589ACN105890589A权利要求书1/2页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时刻的状态方差矩阵xi(0)为系统状态向量中的元素；步骤2：根据k-1时刻得到的系统状态向量量测值和后验状态方差矩阵P(k-1)，结合k时刻多普勒和陀螺仪测得水下机器人在载体坐标系下的线速度和角速度，估计k时刻水下机器人的系统状态向量量测值和后验状态方差矩阵P(k)：步骤2.1：计算Sigma点集χ(k-1)：其中n为系统状态向量的维数，χi(k-1)为点集χ(k-1)的第i列，Pi(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列，系统状态项为：2CN105890589A权利要求书2/2页其中T是控制周期，R为载体坐标系转换到全局坐标系下的坐标转换矩阵；步骤2.3：根据步骤2.2得到的变换后的Sigma点集χ(k/k-1)，进行均值加权得到系统状态向量的预测均值其中Wi是UT变换权值；进行协方差加权得到系统状态向量的预测方差其中是系统噪声的协方差矩阵；步骤2.4：将步骤2.2得到的变换后的Sigma点集χ(k/k-1)代入量测项进行非线性变换：zi(k/k-1)＝h[χi(k/k-1),k/k-1],i＝0,…,2n其中量测项为：Th[x(k),k]＝[[mj(k)nj(k)]×4u(k)v(k)w(k)p(k)q(k)r(k)](mj(k),nj(k))为第j个特征点在图像坐标系下的坐标，j＝1,2,3,4：为第j个特征点在全局坐标系下的坐标，fx和fy分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：步骤2.5：计算更新的状态方差：其中是量测噪声的协方差矩阵；计算增益矩阵：估计k时刻水下机器人的系统状态向量量测值为：其中为量测噪声；k时刻水下机器人的后验状态方差矩阵P(k)为：3CN105890589A说明书1/19页一种水下机器人单目视觉定位方法技术领域[0001]本发明涉及水下视觉定位技术领域，具体为一种水下机器人单目视觉定位方法，通过基于非线性卡尔曼滤波的位姿估计，实现水下机器人单目视觉定位。背景技术[0002]随着海洋开发事业的迅速发展,水下施工和建设项目越来越多,对于水下作用手段的性能要求也越来越高。水下机器人只有获得到高精度的位置和姿态信息才能实现精确控制，水下定位是水下机器人发展需要解决的