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

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

【技术实现步骤摘要】

本专利技术涉及水下视觉定位
，具体为一种水下机器人单目视觉定位方法，通过基于非线性卡尔曼滤波的位姿估计，实现水下机器人单目视觉定位。
技术介绍
随着海洋开发事业的迅速发展,水下施工和建设项目越来越多,对于水下作用手段的性能要求也越来越高。水下机器人只有获得到高精度的位置和姿态信息才能实现精确控制，水下定位是水下机器人发展需要解决的关键技术之一。长期以来，人们一直致力于水声定位技术的研究，在水下机器人中长距离的目标定位及导航等方面取得了很好的研究成果，但由于水声定位系统数据更新频率低，在近距离测量中声学系统的稳定性和精度还有待进一步提高。光视觉传感器近距离探测精度能达到厘米级，体积小且成本低，在水下机器人近距离目标定位及导航中有其独特的优势，因此光视觉传感器在水下机器人控制领域得到了广泛地研究与应用。目前的水下机器人视觉定位主要采用几何法和扩展卡尔曼滤波(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时刻的状态方差矩阵xi(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列，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列，系统状态项为：其中T是控制周期，R为载体坐标系转换到全局坐标系下的坐标转换矩阵；步骤2.3：根据步骤2.2得到的变换后的Sigma点集χ(k/k-1)，进行均值加权得到系统状态向量的预测均值x^(k/k-1)=Σi=02nWiχi(k/k-1)]]>其中Wi是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)代入量测项进行非线性变换：zi(k/k-1)＝h[χi(k/k-1),k/k-1],i＝0,…,2n其中量测项为：h[x(k),k]＝[[mj(k) nj(k)]×4 u(k) v(k) w(k) p(k) q(k) r(k)]T(mj(k),nj(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个特征点在全局坐标系下的坐标，fx和fy分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：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)本文档来自技高网...

【技术保护点】
一种水下机器人单目视觉定位方法，其特征在于：包括以下步骤：步骤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)：χ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列，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列，系统状态项为：其中T是控制周期，R为载体坐标系转换到全局坐标系下的坐标转换矩阵；步骤2.3：根据步骤2.2得到的变换后的Sigma点集χ(k/k‑1)，进行均值加权得到系统状态向量的预测均值x^(k/k-1)=Σi=02nWiχi(k/k-1)]]>其中Wi是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)代入量测项进行非线性变换：zi(k/k‑1)＝h[χi(k/k‑1),k/k‑1],i＝0,…,2n其中量测项为：h[x(k),k]＝[[mj(k) nj(k)]×4 u(k) v(k) w(k) p(k) q(k) r(k)]T(mj(k),nj(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个特征点在全局坐标系下的坐标，fx和fy分别是相机的两个焦距，相机坐标系与水下机器人的载体坐标系重合；并加权求和计算得到系统量测量的预测值：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).]]>...

【技术特征摘要】
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)：χ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列，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列，系统状态项为：其中T是控制周期，R为载体坐标系转换到全局坐标系下的坐标转换矩阵；步骤2.3：根据步骤2.2得到的变换后的Sigma点集χ(k/k-1)，进行均值加权得到系统状态向量的预测均值x^(k/k-1)=Σi=02nWiχi(k/k-1)]]>其中Wi是UT变换权值；进行协方差加权得到系统状态向量的预测方差P‾(k)=Σi=02nWi[χi(k/k-1)-x^(k/k-1)][χi(k/...

【专利技术属性】
技术研发人员：高剑，孙笑笑，严卫生，张福斌，夏飞，崔荣鑫，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61