[发明专利]一种数据融合导航定位方法

权利要求书

1.一种数据融合导航定位方法，其特征在于，包括：

步骤1：测量惯性坐标系下水下机器人相对于海底的速度，利用多普勒测速仪对速度进行积分测得水下机器人的相对位移；

步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后转换到目标坐标系下，再转换到惯性坐标系下，分别得到相邻时刻水下机器人在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；

步骤3：利用步骤1得到的水下机器人相对位移和步骤2得到的水下机器人在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并利用延时补偿对步骤2得到的位移进行补偿；

步骤4：对步骤3补偿后的位移与步骤1得到的相对位移进行融合，得出水下机器人抓取过程中的位置。

2.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤1具体为：

步骤1.1：利用磁罗经测得水下机器人的姿态，利用多普勒测速仪测得在载体坐标系下水下机器人相对与海底的速度

步骤1.2：使用扩展卡尔曼滤波对进行滤波；

步骤1.3：将转换到惯性坐标系下，得到在惯性坐标系下水下机器人相对于海底的速度：

其中，为在惯性坐标系下水下机器人相对于海底的速度，为从水下机器人坐标系转换到惯性坐标系下的转换矩阵；

步骤1.4：利用DVL对进行积分测得水下机器人的相对位移。

3.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤3具体为：

步骤3.1：将水下机器人相对位移看作是参考点云，将参考点云所占空间划分成网格，并计算每个网格的正态分布参数均值和协方差矩阵：

其中，n表示网格中点云数量，X_i表示参考点云的数值；

步骤3.2：对变换参数p＝(t_x t_y φ)进行初始化设定，φ设定为零，t_x,t_y设定为延时时间TD，且t_x＝t_y；

步骤3.3：利用延时时间TD对步骤2得到的位移进行补偿：

x_b＝x_a+v×TD

其中，v为DVL测得水下机器人速度，x_a为步骤2得到的位移值，x_b为延时补偿后得到水下机器人的位移值；

步骤3.4：计算每个网格的概率密度：

计算NDT配准得分：

步骤3.5：score为最大值时结束计算，此时TD为双目匹配的延迟时间，x_b为补偿后的位移。

4.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤4具体为：

步骤4.1：依据上一时刻的位置预测水下机器人在当前时刻的状态，假定水下目标是静止的，操纵水下机器人趋近目标；

预测阶段的系统状态方程为：

其中为由DVL、磁罗经测得的水下机器人在载体坐标系下x,y以及转角φ的变化，为由水下机器人在惯性坐标系下上一时刻的位置与艏向角，水下目标在惯性坐标系下的坐标为

状态量的协方差为：

其中，雅可比矩阵J和Q为：

雅可比矩阵J_v和Q_v为：

步骤4.2：将由延时补偿得出水下目标在双目坐标系下的位置，转换到载体坐标系下(x_b,y_b)，然后转换到水下机器人极坐标系下：

获得观测值为：

其中，R为观测协方差，设定为

将水下目标在惯性坐标系下的位置转换到水下机器人极坐标系下为：

扩展卡尔曼滤波的增益K_n为：

上式中的矩阵H_n为：

式中Δx,Δy的值为：

更新后的状态方程即机器人的位置，为：

其中：