CN108444478A - 一种用于水下航行器的移动目标视觉位姿估计方法 - Google Patents

Abstract

本发明提出一种用于水下航行器的移动目标视觉位姿估计方法，根据水下航行器数学模型，结合传感器(如多普勒速度仪和方位姿态测量系统)测得水下航行器在载体系下的线速度，角速度和角度，得到状态信息；取多个已知在移动目标上的特征点，在水下航行器运动学模型的基础上，通过坐标系变换，可知全局系下的特征点在图像系下的位置，进而得到量测信息。利用无迹卡尔曼滤波算法估计出水下航行器和目标物体中心的相对位置差距和运动姿态。本发明与几何法的方法相比,突破了几何法中特征点的布置必须满足特定条件这一限制,能准确地估计水下航行器和目标物体中心的相对位置差距和运动姿态。

Description

一种用于水下航行器的移动目标视觉位姿估计方法

技术领域

本发明涉及水下航行器视觉技术领域，具体为一种用于水下航行器的移动目标视觉位姿估计方法，是一种水下航行器在跟踪特征目标时，利用视觉测量，通过基于非线性卡尔曼滤波的位姿估计方法，对在其下方的移动目标位姿参数进行估计。

背景技术

海洋对人类来说是一个巨大的财富，其蕴藏着巨大的资源，但是对海洋的探测仍然是一个巨大挑战，尤其深海、远海探测。水下传感器技术的发展也极大地带动着水下航行器技术的发展。

长期以来,人们一直致力于水声定位技术的研究,在水下航行器中长距离的目标定位及导航等方面取得了很好的研究成果,但由于水声定位系统数据更新频率低,在近距离测量中的稳定性和精度还有待进一步提高。为满足水下航行器作业的需要，人们需要实现近距离目标估计。而视觉传感器适用于近距离、高精度的目标探测和跟踪。

目前的水下航行器视觉定位主要采用几何法。几何法通过水下航行器上安装的相机获取视觉目标上的n个特征点在图像系下的位置坐标，通过PnP(Perspective-n-Piont)算法求解了水下航行器相对于目标位置和姿态。但算法在求解过程中存在着多解和鲁棒性差等问题,为了得到唯一解,特征点的布置必须满足特定条件,因此也限制了其使用的范围，同时无法获得目标的速度等信息，导致该方法在实际使用中不能够广泛的用于水下航行器的控制。

发明内容

针对现有技术存在的问题，本发明提出一种用于水下航行器的移动目标视觉位姿估计方法，该方法的特点是六自由度的水下航行器在特征目标上方对运动的特征目标跟踪，该方法能准确地估计水下航行器和目标物体中心的相对位置差距和运动姿态。

本发明的主要原理思路是：根据水下航行器数学模型，结合传感器(如多普勒速度仪和方位姿态测量系统)测得水下航行器在载体系下的线速度，角速度和角度，得到状态信息；取多个已知在移动目标上的特征点，在水下航行器运动学模型的基础上，通过坐标系变换，可知全局系下的特征点在图像系下的位置，进而得到量测信息。利用无迹卡尔曼滤波算法估计出水下航行器和目标物体中心的相对位置差距和运动姿态。

本发明的技术方案为：

所述一种用于水下航行器的移动目标视觉位姿估计方法，其特征在于：包括以下步骤：

步骤1：利用六自由度水下航行器自身安装的方位姿态测量系统以及测速仪器，获取水下航行器的初始状态信息，并估计目标的初始状态信息，按照系统状态向量设置水下航行器在k＝0时的系统状态向量初值x(0)：

x(k)＝[Δx,Δy,Δz,φ_B,θ_B,ψ_B,u_B,v_B,w_B,p_B,q_B,r_B,φ_A,θ_A,ψ_A,u_A,v_A,w_A,p_A,q_A,r_A]_k ^T

其中Δx＝x_B-x_A，Δy＝y_B-y_A，Δz＝z_B-z_A，(x_B,y_B,z_B)为水下航行器中心在全局坐标系中的位置坐标,(x_A,y_A,z_A)为目标中心在全局系中的位置坐标，Δx,Δy,Δz分别为全局坐标系下水下航行器相对于跟踪目标的位置差，φ_B,θ_B,ψ_B分别为水下航行器在全局坐标系中的姿态角，u_B,v_B,w_B分别为水下航行器在水下航行器载体系中的线速度，p_B,q_B,r_B分别为水下航行器在全局坐标系中三个姿态角的角速度，φ_A,θ_A,ψ_A分别为目标在全局坐标系中的姿态角，u_A,v_A,w_A分别为目标在目标载体系中的三个方向的线速度，p_A,q_A,r_A分别为目标在全局坐标系中三个姿态角的角速度；并计算水下航行器在k＝0时刻的状态量的协方差矩阵

其中是x(0)的期望值，E(.)表示期望运算，(.)^T表示转置运算；

步骤2：根据k-1时刻得到的系统状态向量估计值和状态估计协方差矩阵P(k-1),结合k时刻测得水下航行器在水下航行器载体系中的线速度以及水下航行器在全局坐标系中的角速度和角度,估计时刻k水下航行器的系统状态向量估计值和状态估计协方差矩阵P(k)：

步骤2.1：状态向前一步预测，步骤如下：

步骤2.1.1：利用k-1时刻的估计状态和估计协方差矩阵P(k-1|k-1)，产生Sigma点

式中代表第i个Sigma点，表示n乘以估计方差P(k-1|k-1)后，进行矩阵开方运算，选择其中第i行并转置后得到的n维向量，n为系统状态向量的维数；

步骤2.1.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所计算得到的状态估计值，为系统状态方程，在一个步长T内系统状态方程为

其中是T控制周期,为水下航行器载体坐标系转换到全局坐标系下的坐标转换矩阵,为目标载体系转换到全局坐标系下的坐标转换矩阵；

步骤2.1.3：合并获得k时刻的向前一步状态估计

步骤2.1.4：考虑过程噪声，得到向前一步预测的估计协方差矩阵：：

其中Q(k-1)是系统噪声的协方差矩阵；

步骤2.2：利用量测更新向前一步预测状态，步骤如下：

步骤2.2.1：利用向前一步预测状态以及协方差P(k|k-1)，产生Sigma点

步骤2.2.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所得到的状态估计值；

步骤2.2.3：根据量测方程将状态估计量转换为量测预测

其中量测方程为：

h[x(k)]＝[μ_j(k),υ_j(k),φ_B(k),θ_B(k),ψ_B(k),u_B,ν_B,w_B,p_B,q_B,r_B]^T

(μ_j(k),υ_j(k))为目标上第j个特征点在图像坐标系下的坐标，由水下航行器的俯视摄像头采集图像数据，并根据图像数据获得目标上特征点在图像坐标系中的坐标；

步骤2.2.4：合并获得k时刻的测量预测向量

步骤2.2.5：考虑量测噪声的量测预测的协方差为：

其中R(k)量测噪声的协方差矩阵；

步骤2.2.6：估计与间的协方差

步骤2.3：计算增益矩阵K(k)＝P_XZP_Z ^-1,则更新的状态估计及方差为

P(k|k)＝P(k|k-1)-K(k)P_ZK^T(k)

其中z(k)为k时刻的量测值；

步骤2.4：重复步骤2.1到步骤2.3，直至航行结束。

有益效果

本发明与几何法的方法相比,突破了几何法中特征点的布置必须满足特定条件这一限制,能准确地估计水下航行器和目标物体中心的相对位置差距和运动姿态。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

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

图1：本发明中水下航行器的视觉系统模型示意图；

图2：实例一中水下航行器和特征点的运动轨迹示意图；

图3：实例一中水下航行器与特征目标中心相对位置的估计值示意图；

图4：实例一中特征目标的姿态角估计值示意图。

具体实施方式

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

本实施例基于非线性无迹卡尔曼滤波(Unscented Kalmen Filter UKF),方法的硬件基础是六自由度水下航行器下方中部安装有工业相机并向下俯视，相机利用密封舱密封防水，密封舱下端为有机玻璃，相机的帧率至少达到20帧以上。水下航行器主密封舱中安装有工控机，工控机的处理器性能需要到达i5处理器性能及以上，硬盘内存需达到32GB以上。水下航行器中安装有方位姿态测量系统，可以得到水下航行器在全局坐标系下实时的角速度，角度信息。在水下航行器下方头部安装多普勒测速仪，可以得到水下航行器在水下航行器载体系下实时的速度信息。

方法的原理是根据水下航行器数学模型,结合多普勒速度仪和方位姿态测量系统测得水下航行器在载体系下的线速度，角速度和角度,得到状态信息；利用工控机中opencv的图像提取算法提取4个已知在移动目标上的特征点,在水下航行器运动学模型的基础上,通过坐标系变换,得到可知全局系下的特征点在图像系下的位置,得到量测信息。利用无迹卡尔曼滤波算法估计出水下航行器和目标物体中心的相对位置差距和运动姿态。

具体步骤如下：

步骤1：利用六自由度水下航行器自身安装的方位姿态测量系统以及测速仪器，获取水下航行器的初始状态信息，并估计目标的初始状态信息，按照系统状态向量设置水下航行器在k＝0时的系统状态向量初值x(0)：

x(k)＝[Δx,Δy,Δz,φ_B,θ_B,ψ_B,u_B,v_B,w_B,p_B,q_B,r_B,φ_A,θ_A,ψ_A,u_A,v_A,w_A,p_A,q_A,r_A]_k ^T

其中Δx＝x_B-x_A，Δy＝y_B-y_A，Δz＝z_B-z_A，(x_B,y_B,z_B)为水下航行器中心在全局坐标系中的位置坐标,(x_A,y_A,z_A)为目标中心在全局系中的位置坐标，Δx,Δy,Δ_z分别为全局坐标系下水下航行器相对于跟踪目标的位置差，φ_B,θ_B,ψ_B分别为水下航行器在全局坐标系中的姿态角，u_B,v_B,w_B分别为水下航行器在水下航行器载体系中的线速度，p_B,q_B,r_B分别为水下航行器在全局坐标系中三个姿态角的角速度，φ_A,θ_A,ψ_A分别为目标在全局坐标系中的姿态角，u_A,v_A,w_A分别为目标在目标载体系中的三个方向的线速度，p_A,q_A,r_A分别为目标在全局坐标系中三个姿态角的角速度；并计算水下航行器在k＝0时刻的状态量的协方差矩阵

其中是x(0)的期望值，E(.)表示期望运算，(.)^T表示转置运算。

本实施例中系统状态向量初值x(0)取

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

步骤2：根据k-1时刻得到的系统状态向量估计值和状态估计协方差矩阵P(k-1),结合k时刻测得水下航行器在水下航行器载体系中的线速度以及水下航行器在全局坐标系中的角速度和角度,估计时刻k水下航行器的系统状态向量估计值和状态估计协方差矩阵P(k)：

步骤2.1：状态向前一步预测，步骤如下：

步骤2.1.1：利用k-1时刻的估计状态和估计协方差矩阵P(k-1|k-1)，产生Sigma点

式中代表第i个Sigma点，表示n乘以估计方差P(k-1|k-1)后，进行矩阵开方运算，选择其中第i行并转置后得到的n维向量，n为系统状态向量的维数；

步骤2.1.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所计算得到的状态估计值，为系统状态方程，在一个步长T内系统状态方程为

其中是T控制周期，取为T＝0.5,为水下航行器载体坐标系转换到全局坐标系下的坐标转换矩阵,为目标载体系转换到全局坐标系下的坐标转换矩阵；

步骤2.1.3：合并获得k时刻的向前一步状态估计

步骤2.1.4：考虑过程噪声，得到向前一步预测的估计协方差矩阵：：

其中Q(k-1)是系统噪声的协方差矩阵，通过估计和历史经验得到，本实例中取为：

Q＝diag{0.1,0.1,0.1,0.1π/180,0.1π/180,0.1π/180,1,1,1,0.1π/180,0.1π/180,0.1π/180,0.1π/180,0.1π/180,0.1π/180,1,1,1,0.1π/180,0.1π/180,0.1π/180,}²；

步骤2.2：利用量测更新向前一步预测状态，步骤如下：

步骤2.2.1：利用向前一步预测状态以及协方差P(k|k-1)，产生Sigma点

步骤2.2.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所得到的状态估计值；

步骤2.2.3：根据量测方程将状态估计量转换为量测预测

其中量测方程为：

h[x(k)]＝[μ_j(k),υ_j(k),φ_B(k),θ_B(k),ψ_B(k),u_B,ν_B,w_B,p_B,q_B,r_B]^T

(μ_j(k),υ_j(k))为目标上第j个特征点在图像坐标系下的坐标，由水下航行器的俯视摄像头采集图像数据，并根据图像数据由工控机利用opencv中的图像提取算法获得目标上特征点在图像坐标系中的坐标；本实施例中采用4个特征点j＝1,2,3,4；

特征点转换到摄像机系下的坐标为

其中，是全局坐标系到水下航行器载体系的转换矩阵，(x,y,z)为水下航行器在全局坐标系下的位置坐标，为t时刻第j个特征点在全局坐标系下的位置坐标，为t时刻第j个特征点在摄像机坐标系下的坐标，j＝1,2,3,4,如图1所示。结合小孔相机模型和水下航行器运动学模型可知，全局坐标系下4个特征点在图像系下的坐标为

其中，R_ij是旋转矩阵的第i行第j列的元素，(μ_j,ν_j)是第j个点在相机系下的二维坐标，k_x，k_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)，摄像机的焦距f_x＝f_y＝40像素。

步骤2.2.4：合并获得k时刻的测量预测向量

步骤2.2.5：考虑量测噪声的量测预测的协方差为：

其中R(k)量测噪声的协方差矩阵，通过纪录的数据和仪器的特性得到，本实例中取为R(k)＝diag{4,4,4,4,4,4,4,4,π/180,π/180,π/180,0.01,0.01,0.01,π/180,π/180,π/180,}²

步骤2.2.6：估计与间的协方差

步骤2.3：计算增益矩阵K(k)＝P_XZP_Z ^-1,则更新的状态估计及方差为

P(k|k)＝P(k|k-1)-K(k)P_ZK^T(k)

其中z(k)为k时刻的量测值；

步骤2.4：重复步骤2.1到步骤2.3，直至航行结束。

完成水下航行器相对特征目标的位置与特征目标的姿态估计，水下航行器与特征目标中心相对位置的估计值Matlab仿真结果如图3所示，特征目标的角度估计值Matlab仿真结果如图4所示。根据实例，利用Matlab分别仿真了特征目标的运动状态，模拟了水下航行器与目标的距离差值以及特征目标角度的估计，结果表明，该方法可以准确得完成对运动目标参数的估计。

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

Claims (1)

1.一种用于水下航行器的移动目标视觉位姿估计方法，其特征在于：包括以下步骤：

步骤1：利用六自由度水下航行器自身安装的方位姿态测量系统以及测速仪器，获取水下航行器的初始状态信息，并估计目标的初始状态信息，按照系统状态向量设置水下航行器在k＝0时的系统状态向量初值x(0)：

x(k)＝[Δx,Δy,Δz,φ_B,θ_B,ψ_B,u_B,v_B,w_B,p_B,q_B,r_B,φ_A,θ_A,ψ_A,u_A,v_A,w_A,p_A,q_A,r_A]_k ^T

其中Δx＝x_B-x_A，Δy＝y_B-y_A，Δz＝z_B-z_A，(x_B,y_B,z_B)为水下航行器中心在全局坐标系中的位置坐标,(x_A,y_A,z_A)为目标中心在全局系中的位置坐标，Δx,Δy,Δz分别为全局坐标系下水下航行器相对于跟踪目标的位置差，φ_B,θ_B,ψ_B分别为水下航行器在全局坐标系中的姿态角，u_B,v_B,w_B分别为水下航行器在水下航行器载体系中的线速度，p_B,q_B,r_B分别为水下航行器在全局坐标系中三个姿态角的角速度，φ_A,θ_A,ψ_A分别为目标在全局坐标系中的姿态角，u_A,v_A,w_A分别为目标在目标载体系中的三个方向的线速度，p_A,q_A,r_A分别为目标在全局坐标系中三个姿态角的角速度；并计算水下航行器在k＝0时刻的状态量的协方差矩阵

其中是x(0)的期望值，E(.)表示期望运算，(.)^T表示转置运算；

步骤2：根据k-1时刻得到的系统状态向量估计值和状态估计协方差矩阵P(k-1),结合k时刻测得水下航行器在水下航行器载体系中的线速度以及水下航行器在全局坐标系中的角速度和角度,估计时刻k水下航行器的系统状态向量估计值和状态估计协方差矩阵P(k)：

步骤2.1：状态向前一步预测，步骤如下：

步骤2.1.1：利用k-1时刻的估计状态和估计协方差矩阵P(k-1|k-1)，产生Sigma点

式中代表第i个Sigma点，表示n乘以估计方差P(k-1|k-1)后，进行矩阵开方运算，选择其中第i行并转置后得到的n维向量，n为系统状态向量的维数；

步骤2.1.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所计算得到的状态估计值，为系统状态方程，在一个步长T内系统状态方程为

其中是T控制周期,为水下航行器载体坐标系转换到全局坐标系下的坐标转换矩阵,为目标载体系转换到全局坐标系下的坐标转换矩阵；

步骤2.1.3：合并获得k时刻的向前一步状态估计

步骤2.1.4：考虑过程噪声，得到向前一步预测的估计协方差矩阵：：

其中Q(k-1)是系统噪声的协方差矩阵；

步骤2.2：利用量测更新向前一步预测状态，步骤如下：

步骤2.2.1：利用向前一步预测状态以及协方差P(k|k-1)，产生Sigma点

步骤2.2.2：将Sigma点带入系统的状态方程，得

其中为第i个Sigma点所得到的状态估计值；

步骤2.2.3：根据量测方程将状态估计量转换为量测预测

其中量测方程为：

h[x(k)]＝[μ_j(k),υ_j(k),φ_B(k),θ_B(k),ψ_B(k),u_B,ν_B,w_B,p_B,q_B,r_B]^T

(μ_j(k),υ_j(k))为目标上第j个特征点在图像坐标系下的坐标，由水下航行器的俯视摄像头采集图像数据，并根据图像数据获得目标上特征点在图像坐标系中的坐标；

步骤2.2.4：合并获得k时刻的测量预测向量

步骤2.2.5：考虑量测噪声的量测预测的协方差为：

其中R(k)量测噪声的协方差矩阵；

步骤2.2.6：估计与间的协方差

步骤2.3：计算增益矩阵K(k)＝P_XZP_Z ^-1,则更新的状态估计及方差为

P(k|k)＝P(k|k-1)-K(k)P_ZK^T(k)

其中z(k)为k时刻的量测值；

步骤2.4：重复步骤2.1到步骤2.3，直至航行结束。