[发明专利]一种基于多源信息融合的小型水下多机器人协同定位方法及系统

权利要求书

1.一种基于多源信息融合的小型水下多机器人协同定位方法，其特征在于，包括下述步骤：

步骤一、获取传感器数据；其中，所述传感器数据包括定位机器人水下压力传感器值、被定位机器人水下压力传感器值、包含被定位机器人的图像序列；

步骤二、根据定位机器人水下压力传感器值和被定位机器人水下压力传感器值计算获得定位机器人和被定位机器人之间的垂直距离；具体过程包括：

步骤二一、建立定位机器人和被定位机器人压力差与垂直距离的线性方程；

定位机器人和被定位机器人之间的压力差与垂直距离的关系式为：

Z_p＝k_pP₁₂

其中，Z_p表示垂直距离；P₁₂表示压力差，P₁₂＝P₁-P₂，P₁是定位机器人压力传感器值；P₂是被定位机器人压力传感器值；k_p为压力差与垂直距离的比例参数；

步骤二二、根据上述线性方程确定第一系统状态方程和观测方程；

第一系统的状态方程和观测方程为：

第一系统的状态向量和观测向量为：

其中，k表示时刻；v_p表示被定位机器人垂直方向速度；A表示状态转移矩阵；C表示观测矩阵，为高斯过程噪声；为高斯观测噪声；

步骤二三、根据确定的第一系统状态方程和观测方程，采用卡尔曼算法对垂直距离进行滤波，获得滤波后定位机器人和被定位机器人之间的垂直距离；

步骤三、根据包含被定位机器人的图像序列计算获得被定位机器人的三维空间位置坐标；

步骤四、将定位机器人和被定位机器人之间的垂直距离与被定位机器人的三维空间位置坐标进行信息融合，获得被定位机器人最终的三维空间位置。

2.根据权利要求1所述的一种基于多源信息融合的小型水下多机器人协同定位方法，其特征在于，步骤三的具体过程包括：

步骤三一、根据包含被定位机器人的图像序列，通过视觉目标识别算法获得被定位机器人的像素坐标；

步骤三二、建立被定位机器人视觉定位模型方程；

被定位机器人视觉定位模型方程为：

其中，i表示双目相机序号；l和r分别表示双目相机中左右相机坐标系；u和v表示像素坐标；表示被定位机器人在机器人本体坐标系下坐标；

步骤三三、根据上述视觉定位模型方程确定第二系统状态方程和观测方程；

第二系统的状态方程和观测方程为：

其中，为k时刻状态向量；为k时刻观测向量；为高斯过程噪声；为高斯观测噪声；

第二系统的状态向量和观测向量为：

步骤三四、根据确定的第二系统状态方程和观测方程，采用无迹卡尔曼滤波算法对被定位机器人在机器人本体坐标系下坐标进行滤波，获得滤波后被定位机器人的三维空间位置坐标

3.根据权利要求2所述的一种基于多源信息融合的小型水下多机器人协同定位方法，其特征在于，步骤四的具体过程包括：

步骤四一、获取被定位机器人的姿态角；

步骤四二、根据所述姿态角将被定位机器人在机器人坐标系下的三维空间位置坐标转换为在世界坐标系下的三维空间位置坐标

步骤四三、将Z轴方向的坐标和步骤二获得的垂直距离Z_p进行融合获得全局状态估计和协方差矩阵为：

其中，为垂直距离协方差，为Z轴方向坐标的协方差；

步骤四四、将获得的结合被定位机器人在世界坐标系下的位置坐标获得被定位机器人最终的三维空间位置。