本发明涉及一种基于滤波的多传感器融合冰下里程计方法，属于水下机器人导航与定位领域。
背景技术：
1、针对水下机器人在冰下环境中的导航和定位问题，传统的方法存在一些限制，例如：航位推算导航方法经济成本高，且系统体积重量大，不适用于小型低成本水下机器人；声学导航方法需要预设声信标或母船附近，难以应用于大面积冰盖场景；地球物理导航方法依赖高分辨率环境先验信息。相比之下，多传感器融合里程计方法则利用水下机器人携带的多种传感器信息进行位置与姿态估计，具有低成本、高灵活性、高可靠性等优势，其在冰下环境中，采用双目相机、前视声呐、惯性测量单元(imu)和深度计的紧耦合融合方法，可以发挥各传感器优势，实现高精度和高鲁棒性的水下机器人冰下位置与姿态估计。
技术实现思路
1、本发明的目的是为了解决小型且低成本的水下机器人在冰下作业时的导航与定位困难问题，进而提供的一种基于滤波的多传感器融合冰下里程计方法。
2、一种基于滤波的多传感器融合冰下里程计装置，应用于一种搭载有多种传感器的可移动物体上，包括：
3、紧耦合双目相机、成像声呐、imu和深度计，获取冰下的测量信息；
4、可移动物体在冰下作业时，利用搭载的双目相机垂直对上，采集冰盖底部的光学图像；利用搭载的前视声呐对前斜向上倾角30度，采集冰盖底部的视觉图像，并记录惯性测量单元和深度计的实时加速度、角速度和深度数据；
5、根据获得到的多传感器数据，估算可移动物体的实时位置与姿态。
6、本发明第二方面提供了一种基于滤波的多传感器融合冰下里程计方法，具体步骤如下：
7、s1：对imu状态进行初始化，计算初始姿态和初始位置；
8、s2：初始化imu的误差状态向量和误差状态协方差矩阵f，设置陀螺仪和加速度计的测量噪声与偏置噪声ni，以及噪声的协方差矩阵q；
9、s3：利用观测到的双目图像对imu误差状态向量和误差状态协方差矩阵进行扩增；
10、s4：利用滑动窗口维护一定数量的相机状态，当扩增的相机状态数量达到滑动窗口的最大长度，需对相机状态进行剪枝，每次移除两个相机帧；
11、s5：当某个特征点丢失或对相机状态剪枝后，进行双目相机的观测更新；
12、s6：可移动物体在冰盖下实时获取深度测量，用imu的帧间积分进行imu帧与深度计帧的对齐；
13、s7：利用对齐后的相邻深度计数据计算相对深度，并进行观测更新；
14、s8：观测更新后，计算观测值与更新后的值相对深度残差μk；
15、s9：利用实时观测到的声呐图像进行线速度估计；
16、s10：利用imu的帧间积分进行imu帧与声呐帧的对齐，对齐方法与步骤六一致；
17、s11：利用对齐后的声呐线速度进行观测更新；
18、s12：观测更新后，利用线速度残差，自适应估计声呐图像线速度的噪声协方差矩阵，计算方法与步骤八一致。
19、优选的，所述步骤s1中计算初始姿态和初始位置置的方法为：
20、s111：控制水下机器人在冰盖下稳定漂浮，并记录1～2秒的imu数据；
21、s112：利用加速度平均值aavg的方向与重力方向计算初始姿态r0；
22、s113：利用角速度的平均值ωavg作为陀螺仪的偏置bg0；
23、s114：利用施密特正交化计算初始姿态r0；
24、s115：最后利用如下公式计算加速度计的偏置ba0：
25、ba0＝aavg-r0g。
26、优选的，所述步骤s2使用四阶龙格库塔积分计算imu的误差状态，并根据imu的线性化运动学公式进行误差状态协方差矩阵的转移：
27、
28、式中，
29、
30、qk＝φkgqgφk(tk+1-tk)。
31、优选的，所述步骤s3中图像的扩增方法如下：
32、s311：对双目图像进行预处理，使用暗通道先验算法和pinax模型进行增强与矫正；
33、s312：对于第一帧双目图像，提取fast角点，并使用klt光流算法进行跟踪；
34、s313：用左目图像对应的镜头位置与姿态扩增imu的误差状态向量和误差状态协方差矩阵，得到包含imu与相机误差状态系统误差状态向量
35、s314：协方差矩阵的扩增方法包括基于的imu方法和基于前一帧图像的方法，对于第一帧图像，采用基于imu的方法扩增；
36、s315：对于之后到来的图像，根据特征跟踪情况和可移动物体的运动状态自适应选取扩增方法，当特征跟踪状态良好时，采用基于前一帧图像的方法扩增。
37、优选的，步骤s4中移除相机帧的判断方法为：根据特征跟踪情况和滑动窗口内最新的相机帧fk与fk-3帧和fk-2帧之前的位移与旋转判断需要移除的相机状态，若当前的跟踪状态大于设定的阈值且fk帧与fk-3帧的位移与旋转均小于设定的阈值，则移除fk-3帧，否则移除滑动窗口内最老的相机帧，对于fk-2帧，同理。
38、优选的，步骤s5双目相机的观测更新方法为：若某一特征点被三个及以上的相机观测到，则利用高斯牛法计算三维位置，构建双目相机的测量模型，通过双目相机对特征点的观测，形成相机对某一特征点的几何约束，并构建重投影残差：
39、
40、对重投影残差线性化得到：
41、
42、式中，为残差对相机状态的雅可比矩阵，为残差对特征点位置的雅可比矩阵，叠加多个相机对一个特征点的残差或单个相机对多个特征点的残差，得到：
43、
44、通过将残差投影到hf的零空间可以得到残差的标准形式：
45、
46、然后使用卡方检测判断特征点是否是异常值，若判断为异常值则剔除，若不为异常值则将特征点对应的线性化残差合并，然后进行观测更新。
47、优选的，步骤s6中利用imu的帧间积分进行imu帧与深度计帧的对齐：
48、
49、式中，δz为imu沿着全局坐标系z轴的位置积分，tj表示深度数据对应的时间戳，ti和ti+1表示深度计数据两侧的imu数据对应的时间戳。
50、优选的，所述步骤s7中与相对深度相关的残差可以表示为：
51、
52、式中，ipd为深度计在imu坐标系下的位置；
53、将残差线性化得到：
54、
55、利用所有观测到的深度数据进行更新。
56、优选的，所述步骤s8中自适应估计相对深度的噪声协方差矩阵，并引入遗忘因子α：
57、
58、
59、式中，用时间均值表示
60、优选的，所述步骤s9中对声呐图像进行线速度估计方法为：首先使用阈值分割对极坐标系下的声呐图像进行去噪，然后使用相位相关法计算相邻两帧声呐图像之间的像素平移s：
61、i1(x,y)＝i1(u,v)，i2(x,y)＝i2(u,v)
62、
63、c(u,v)＝f-1(c(x,y))
64、
65、式中，i1和i2为声呐图像i1和i2的傅里叶变换，为i2的共轭。然后在s附近取一个邻域，并用离散傅里叶变换进行上采样以计算亚像素平移；
66、然后利用如下公式计算得到线速度：
67、
68、式中，r和c为声呐图像的行数和列数，rmax为声呐的最大探测范围，θmax为与声呐平行的最大视野。
69、优选的，所述步骤s11利用对齐后的声呐线速度进行观测更新的方法：
70、与线速度相关的残差表示为：
71、
72、式中，λ为线速度补偿因子，由相邻两帧声呐图像观测的近似几何关系导出；
73、将残差线性化得到：
74、
75、本发明第三方面提供了一种基于滤波的多传感器融合冰下里程计方法存储介质，所述存储介质烧录有计算机程序，所述计算机程序在服务器的内存中运行时实现上述权利要求中的任一项方法。
76、本发明第四方面提供了一种水下机器人，包括计算机程序/指令，该计算机程序/指令被处理器执行时实现上述权利要求中的任一项方法。
77、本发明有益效果：
78、与现有技术相比，本发明的有益效果是：小型且低成本的水下机器人在冰下作业时的，受经济成本与时间成本的限制，难以应用传统的航位推算导航方法和声学导航方法或需要精确的先验信息的地球物理导航方法进行冰下定位。
79、本发明提出一种低成本且灵活性高的多传感器融合冰下里程计方法，在ekf框架内紧耦合imu的高频加速度和角速度测量、双目相机的图像测量、前视声呐的线速度估计和深度计的相对深度，使用多约束的双目相机观测模型进行测测更新，使用一种高效的亚像素相位相关法估计声呐图像的线速度，使用基于残差的自适应方法估计深度数据声呐线速度的噪声协方差，实现高精度和高鲁棒性的冰下定位与姿态估计。
1.一种基于滤波的多传感器融合冰下里程计装置，其特征在于：应用于一种搭载有多种传感器的可移动物体上，包括：
2.一种基于滤波的多传感器融合冰下里程计方法，其特征在于：具体步骤如下：
3.根据权利要求2所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：所述步骤s3中图像的扩增方法如下：
4.根据权利要求2所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：步骤s4中移除相机帧的判断方法为：根据特征跟踪情况和滑动窗口内最新的相机帧fk与fk-3帧和fk-2帧之前的位移与旋转判断需要移除的相机状态，若当前的跟踪状态大于设定的阈值且fk帧与fk-3帧的位移与旋转均小于设定的阈值，则移除fk-3帧，否则移除滑动窗口内最老的相机帧，对于fk-2帧，同理。
5.根据权利要求2所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：步骤s5双目相机的观测更新方法为：若某一特征点被三个及以上的相机观测到，则利用高斯牛法计算三维位置，构建双目相机的测量模型，通过双目相机对特征点的观测，形成相机对某一特征点的几何约束，并构建重投影残差：
6.根据权利要求2、3、4或5所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：所述步骤s7中与相对深度相关的残差可以表示为：
7.根据权利要求2、3、4或5所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：所述步骤s8中自适应估计相对深度的噪声协方差矩阵，并引入遗忘因子α：
8.根据权利要求2、3、4或5所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：所述步骤s9中对声呐图像进行线速度估计方法为：首先使用阈值分割对极坐标系下的声呐图像进行去噪，然后使用相位相关法计算相邻两帧声呐图像之间的像素平移s：
9.根据权利要求2、3、4或5所述的一种基于滤波的多传感器融合冰下里程计方法，其特征在于：所述步骤s11利用对齐后的声呐线速度进行观测更新的方法：
10.基于滤波的多传感器融合冰下里程计方法存储介质，其特征在于：所述存储介质烧录有计算机程序，所述计算机程序在服务器的内存中运行时实现上述权利要求2～9任一项所述的方法。
