基于卡尔曼滤波和红外热成像的水下机器人导航定位方法与流程

本发明涉及水下机器人领域，尤其涉及基于卡尔曼滤波和红外热成像的水下机器人导航定位方法，用于水下管道中的机器人导航定位。

背景技术：

1、水下机器人已经广泛的应用于诸多领域，能够代替人类完成探测检修任务，是海洋探索、河流水利工程检修、险情处置的重要工具，这不仅保障了相关人员的生命安全，也拓展了任务可实施的深度和广度，高精度的定位导航系统是水下机器人完成相关任务的重要前提。

2、目前水下导航定位系统主要包含声学导航系统和惯性导航系统两种。声学导航系统是由水下声发射接收器及其应答器相互作用构成的水下定位系统。根据定位系统基线长度以及工作模式的差别，一般将其划分为长基线系统、短基线系统、超短基线系统三种。长基线定位系统由预先布设的参考声信标阵列和测距仪组成，通过距离交汇解算目标位置。长基线需要事先测阵，作业成本高，主要应用于局部区域高精度定位。超短基线定位系统则是由多元声基阵与声信标组成，通过测量距离和方位定位。其优点为尺寸小、使用方便；缺点是定位误差与距离相关，仅适用于大范围作业区域跟踪。短基线定位系统由装载在载体上的多个接收换能器和声信标组成，通过距离交汇获得目标位置。短基线作业简便，但其精度易受到载体形变等因素影响。

3、惯性导航系统是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。水下常用的惯性导航系统为捷联惯性导航，具有精度高、稳定性强、受环境影响小等特点。惯性导航系统的优点是自主性强、动态性能好、导航信息全面且输出频率高，但其缺点是误差随时间不断累积，长期精度差。

4、水下导航定位时，由于传感器误差，以及惯性导航系统方程随时间累计的误差，造成水下机器人远距离自主导航定位时无法精准定位自身位置，对于水下工作的完成产生了重要影响。

技术实现思路

1、针对上述问题，本发明提供一种基于卡尔曼滤波和红外热成像的水下机器人导航定位方法，使用多普勒测速仪与水下机器人位置更新方程构成组合导航系统，采用卡尔曼滤波算法对水下机器人运动状态进行估计，以多普勒测速仪输出的速度数据作为系统观测量对系统的状态估计进行观测校正，经过观测更新后的输出结果来提高水下机器人自主导航时的定位精度。

2、本发明提供如下技术方案：基于卡尔曼滤波和红外热成像的水下机器人导航定位方法，包括以下步骤：

3、步骤1、建立导航坐标系下多普勒速度模型，导航坐标系以水下机器人前进方向为x轴，水下机器人红外热成像传感器环绕所构成的圆的圆心为坐标原点，定义水下机器人的多普勒测速仪输出值为 ；

4、步骤2、由步骤1获取多普勒测速仪量测值可推算水下机器人实际速度为

5、

6、是多普勒测速仪刻度因子误差；

7、步骤3、由步骤2完成速度更新可得到水下机器人速度在导航坐标系上x轴前进的实际速度，对导航坐标系下的x轴向速度时间累积得到位置更新，位置更新方程表示为

8、

9、其中，是水下机器人在k时刻的位置；是水下机器人在k时刻的实际速度；是从k-1时刻到k时刻的时间间隔；

10、步骤4、采用卡尔曼滤波算法对水下机器人运动状态进行估计，以多普勒测速仪输出的速度数据作为系统观测量 对系统的状态估计进行观测校正，经过观测更新后的输出结果就是对水下机器人位置的最优估计；

11、步骤5、迭代步骤3-4，不断对水下机器人状态估计得到精准的水下机器人位置信息。

12、步骤1中，在水下机器人运动的管道内顶部每隔米添加一个热源，所有热源使用同一开关进行开启或关闭加热；水下机器人本体正中间环绕一圈红外热成像传感器，红外热成像传感器探测方向垂直于管道表面，使水下机器人检测热源时，红外热成像传感器正对热源进行检测。

13、水下机器人本体中端下方安装有多普勒测速仪，水下机器人后端四周环形安装有超声波测距传感器。

14、在步骤3中，在远距离导航定位时，水下机器人的红外热成像传感器实时检测热源，检测到热源时，红外热像图上显示出来的就是光亮颜色区域的面积增大，实现红外热成像校准位置更新方程包含以下步骤：

15、步骤31、红外热传感器所拍摄的红外热像图序列尺寸为m*l像素，像素坐标系以红外热像图右下角作为原点，以机器人前进方向作为u轴，表示像素横坐标，向上方向作为v轴，表示像素纵坐标；

16、红外热像图中每个像素点对应一组红绿蓝颜色通道像素值rgb值并且像素点的分量值均在0-255之间，对红外热像图采用平均值法进行灰度化处理，即将红外热像图中每个像素的rgb值求平均得到一个灰度图，计算公式如下：

17、

18、其中，是指计算后灰度图中的像素坐标为的像素点的灰度值；是指红外热像图中像素坐标为的像素点的r分量值；是指红外热像图中像素坐标为的像素点的g分量值；是指红外热像图中像素坐标为的像素点的b分量值；

19、步骤32、在步骤31中完成红外热像图灰度化处理，得到序列尺寸为m*l像素的灰度图，设定阈值为热源在红外热像图中的灰度值t，将得到的灰度图进一步二值化，计算公式如下：

20、

21、其中，为红外热像图经过灰度化后进一步得到的二值化图像的(u,v)处的像素值，经上式计算后得到二值化图像；像素值为255代表白色空白区域的二值化图像，像素值为0代表热源区域的二值化图像；

22、步骤33、在步骤32中将红外热成像图二值化后得到，进而统计二值化图像中像素值为0的像素点的个数，得出热源区域的面积a，将各像素值为0的像素点坐标分别进行横纵坐标求平均值得到热源区域的二值化图像的质心坐标，计算公式如下：

23、

24、其中，为热源区域的质心的横坐标；为热源区域的质心的纵坐标；为各像素值为0的像素点的横坐标；为各像素值为0的像素点的纵坐标；

25、步骤34、水下机器人在自主导航时一直处于移动状态，在经过第n个热源时，水下机器人走过实际距离为，是每个热源之间的间隔距离，当热源区域的二值化图像的质心横坐标时，对水下机器人位置进行外部校准，即，将其代入水下机器人位置更新方程中。

26、序列尺寸为m*l像素的灰度图，其中的m*l为480*640，设定阈值为热源在红外热像图中的灰度值t中的t为200。

27、在管道内部安装可随意启停的热源，使用水下机器人，在其中间环绕一圈红外热成像传感器，对生成的红外热成像进行灰度处理，进一步得到二值化图像，计算热源图像位置，修正水下机器人位置更新算法，提高远距离自主导航定位精度。

28、所述步骤4中，卡尔曼滤波位置估计的步骤如下：

29、步骤41、卡尔曼滤波位置估计系统的参数初始化

30、由于观测数据为水下机器人在管道中的运动状态, 所以第时刻的系统状态向量选择为水下机器人运动状态，是步骤3中水下机器人位置更新方程计算得出水下机器人在时刻的位置，是水下机器人在k时刻的速度，状态向量维度为2*1，

31、建立系统的状态方程，确定系统的状态转移矩阵

32、

33、其中，为系统k-1时刻的过程噪声，p(w)~n(0,q)，

34、此系统具有一个观测量，为多普勒测速仪输出的水下机器人实时速度；

35、

36、其中h为观测矩阵，；是以多普勒测速仪输出的速度数据为系统观测的k时刻测量噪声，p(y )~n(0,r)。

37、初始化系统状态，初始化系统不确定性协方差矩阵、系统状态噪声协方差矩阵q以及系统观测量的噪声协方差矩阵r；

38、步骤42、根据k-1时刻的水下机器人运动状态估计k时刻的水下机器人运动状态，

39、

40、

41、步骤43、确定系统不确定性协方差矩阵的先验估计，

42、

43、步骤44、根据观测矩阵h的数据计算卡尔曼增益，

44、

45、步骤45、更新计算系统的后验不确定性协方差矩阵 ，

46、

47、步骤46、多普勒测速仪输出的速度数据作为系统观测量对系统的状态估计进行观测校正，表示观测值第k时刻的值，

48、

49、得到更新的水下机器人状态估计，其中的元素是水下机器人位置估计信息。

50、通过上述描述可以看出本方案，近距离导航定位时，使用多普勒测速仪与水下机器人位置更新方程构成组合导航系统，采用卡尔曼滤波算法对水下机器人运动状态进行估计实现近距离高精度定位；远距离导航时，使用红外热成像传感器检测热源，对生成的红外热成像进行灰度处理，进一步得到二值化图像，计算热源图像位置，修正水下机器人位置更新算法，实现远距离高精度定位。本发明包括三个部分：一、选择合适的硬件运行平台，包括对于管道中热源的放置和水下机器人设计；二、构建多普勒测速仪输出速度方程与水下机器人位置更新方程，包括卡尔曼滤波算法对于水下机器人运动状态预测与修正；三、对红外热像图进行灰度处理，进一步得到二值化图像，计算热源图像位置，对水下机器人位置更新方程做校准处理。