水陆两栖环境下滑移转向轮式机器人的定位导航方法

本发明属于定位导航领域，涉及一种水陆两栖环境下滑移转向轮式机器人的定位导航方法。

背景技术：

1、水陆两栖区域的开发和研究具有重要的意义，如军事防护、珊瑚礁保护、近岸洋流观测，而两栖机器人在其中扮演了重要的角色。滑移转向机器人结构及控制相对简单，且具有强大的移动能力，能够辅助人类完成两栖区域的工作任务。但连续自定位作为控制、传感器融合及其他必要功能的基础是其自主化和广泛应用的最大制约之一。作为目前商业应用最广泛定位导航技术——gnss技术，由于机器人需要频繁在水和空气两种介质下穿梭，其在两栖环境下的应用并不可靠；而由于海浪冲击，浑浊海水不断影响机器人行驶，使得激光雷达、相机等外部传感器难以有效使用，这些原因使得两栖环境下的定位导航十分困难。

2、目前并没有针对两栖环境的机器人定位导航系统，将其他环境介质下的定位方法直接迁移会产生定位精度不高、计算量过大等问题。

技术实现思路

1、本发明实施例的目的在于提供一种水陆两栖环境下滑移转向轮式机器人的定位导航方法，以解决现有gnss定位导航技术在两栖环境下的应用不可靠、两栖环境下的定位导航十分困难的问题。

2、本发明实施例所采用的技术方案是：水陆两栖环境下滑移转向轮式机器人的定位导航方法，包括以下步骤：

3、以惯性导航系统作为参考系统，滑移转向轮式机器人的两个前轮的轮式里程计组成的子系统odom1与惯性导航系统组成第一个局部滤波器odom1/ins，滑移转向轮式机器人的两个后轮的轮式里程计组成的子系统odom2与惯性导航系统组成第二个局部滤波器odom2/ins，gnss系统与惯性导航系统组成第三个局部滤波器gnss/ins，设计联邦卡尔曼滤波器；

4、对滑移转向轮式机器人各轮桨产生的传感数据进行异常数据处理；

5、异常数据处理后，基于改进的分裂协方差交叉算法处理第一个局部滤波器odom1/ins和第二个局部滤波器odom2/ins输出的数据间的相关性问题，从而得到轮式里程计与ins的组合导航信息；

6、将轮式里程计与ins的组合导航信息和第三个局部滤波器gnss/ins输出的gps绝对定位信息输入主滤波器中进行全局更新和数据融合，主滤波器最后输出整体的定位导航信息。

7、进一步的，所述对滑移转向轮式机器人各轮桨产生的传感数据进行异常数据处理，包括：

8、对滑移转向轮式机器人各轮桨产生的传感数据进行置信度异常判断，具体过程如下：

9、以轮桨里程数据方差作为置信度，并将四个轮桨产生的具有相同时间戳的置信度数据设为集合cw，集合cw的表示如下：

10、

11、其中，n表示采集数据的时间步，i表示轮桨编号；

12、将出现异常数据的情况分为两类，第一类是四轮均沉陷或阻塞，第二类为至少有一个轮正常运行，通过以下步骤判断异常：

13、ins数据在以0为均值的传感器静置误差内波动，而各轮依然按照指令转动且里程数据高于最低速度阈值，此时判断该机器人整体沉陷或阻塞；

14、按照置信度大小对轮桨i进行排序，轮桨中最高的置信度定义为其他轮桨的置信度定义为j为轮桨i根据置信度从大到小排序的次序；

15、使用sigmoid函数对置信度和进行处理，使得输出控制在0～1之间，结果记为和

16、设集合mn表示其他轮置信度与最高置信度轮的差异，其表达式如下式所示；

17、

18、通过异常数据测试试验测得置信度阈值α，在连续的多个时间步内，当mn中的元素均大于置信度阈值α时记为正常运行，否则，认为该元素对应的轮桨可能异常。

19、进一步的，所述对滑移转向轮式机器人各轮桨产生的传感数据进行异常数据处理，还包括：

20、对置信度判断所得可能异轮桨产生的传感数据进一步进行相似度异常判断，具体过程如下：

21、设向量和分别表示在时间步n时刻的轮桨i在过去m个时间步内的扭矩和速度向量：

22、

23、

24、使用余弦相似度对同侧的两个轮桨i间的相似度进行表征，则滑移转向机器人的右侧扭矩向量右侧速度向量左侧扭矩向量左侧速度向量表达公式如下：

25、

26、

27、

28、

29、其中，为右前轮的扭矩向量，为右后轮的扭矩向量，为左前轮的扭矩向量，为左后轮的扭矩向量；为右前轮的速度向量，为右后轮的速度向量，为左前轮的速度向量，为左后轮的速度向量；

30、通过异常数据测试试验测得速度相似度阈值β1和扭矩相似度阈值β2，若在连续的多个时间步内，和/或均小于β1，和/或均小于β2，则判断和/或对应的轮桨异常，剔除和/或对应的轮桨的里程计数据，并在联邦滤波器中弃用该轮桨的里程计组成的局部滤波器的输出信息。

31、进一步的，所述基于改进的分裂协方差交叉算法处理第一个局部滤波器odom1/ins和第二个局部滤波器odom2/ins输出的数据间的相关性问题，从而得到轮式里程计与ins的组合导航信息的具体过程如下：

32、设odom1产生了测量值xodom1、xodom1的协方差矩阵podom1，ins产生了测量值xins、xins的协方差矩阵pins，在卡尔曼滤波的融合下，第一个局部滤波器odom1/ins产生了以下信息：

33、

34、

35、其中，表示第一个局部滤波器odom1/ins输出的两个前轮的轮式里程计的融合信息，p1,kf表示的协方差矩阵；

36、设odom2产生了测量值xodom2、xodom2的协方差矩阵podom2，在卡尔曼滤波的融合下，第二个局部滤波器odom2/ins局部滤波器中产生了以下信息：

37、

38、

39、其中，表示第二个局部滤波器odom2/ins输出的两个后轮的轮式里程计的融合信息，p2,kf表示的协方差矩阵；

40、对于将其分为由轮式里程计产生的信息和由ins产生的信息两个部分，将x1,odom1作为独立项产生的数据，x1,ins作为相关项产生的数据；而协方差矩阵p1,kf分成已知的独立项p1,ind、未知的可能独立项p1,ind'以及相关项p1,d三个部分，通过这样的划分得出：

41、

42、将p1,d和p1,ind'共同作为相关项分割，计算公式为：

43、

44、对于将其分为由轮式里程计产生的信息和由ins产生的信息两个部分，将x2,odom2作为独立项产生的数据，x2,ins作为相关项产生的数据；而协方差矩阵p2,kf分成已知的独立项p2,ind、未知的可能独立项p2,ind'以及相关项p2,d三个部分，通过这样的划分得出：

45、

46、将p2,ind'与p2,d共同作为相关项分割，计算公式为：

47、

48、由此，由以下公式计算机器人轮式里程计与ins的组合导航信息和podom：

49、

50、

51、

52、

53、

54、podom,d＝podom-podom,ind；

55、其中，ω为协方差交叉滤波的比例参数，p1为第一个局部滤波器odom1/ins的修正协方差矩阵，p2为第二个局部滤波器odom2/ins的修正协方差矩阵，为机器人轮式里程计融合信息，podom为轮式里程计融合信息的协方差矩阵，podom,ind为协方差矩阵podom中的独立项，podom,d为协方差矩阵podom中的相关项；p1-1、对应为podom、p1、p2的逆。

56、进一步的，所述将轮式里程计与ins的组合导航信息和第三个局部滤波器gnss/ins输出的gps绝对定位信息输入主滤波器中进行全局更新和数据融合，主滤波器最后输出整体的定位导航信息的具体步骤是：采用机器人轮式里程计与ins的组合导航信息和第三个局部滤波器gnss/ins输出的gps绝对定位信息对应替换和使用和的融合方法对和进行进一步融合，其中：

57、

58、

59、

60、

61、

62、

63、其中，p3,ind表示gps绝对定位信息的协方差矩阵p3,kf的已知独立项，pgps表示gnss系统产生的测量值的协方差矩阵，p3,d表示gps绝对定位信息的协方差矩阵p3,kf的相关项，p3,g表示第三个局部滤波器gnss/ins的修正协方差矩阵，podom,g表示轮式里程计融合信息的修正协方差矩阵，pg,m表示和的融合信息的协方差矩阵，对应为p3,g、podom,g、pg,m的逆；

64、而后，通过卡尔曼滤波最终输出定位信息和协方差矩阵pg：

65、xg-＝fxg++u；

66、pg-＝fpg+ft+q；

67、

68、其中，xg-和pg-表示卡尔曼滤波的预测值和预测协方差，xg+和pg+表示上一时刻的和pg，f表示状态转移矩阵，u表示控制量，q表示系统噪声，kalman()表示卡尔曼滤波过程。

69、本发明实施例的有益效果是：以联邦卡尔曼滤波器为基本定位导航框架，首先基于置信度和余弦相似度的阈值设定，提出置信度异常判断和相似度异常判断，并结合剔除规则得到了可以避免异常数据污染的异常数据剔除算法；其次基于滑移转向轮式机器人运动特点，提出了改进的分裂协方差交叉算法，用于方便的解决联邦卡尔曼滤波器的数据相关性问题，通过高效的异常剔除手段，结合兼具适用性、鲁棒性和准确性的多传感器位置信息融合方法，可以为水陆两栖环境下运行的滑移转向轮式机器人提供准确的定位导航信息，实现机器人在两栖环境下的自主导航，解决了现有gnss定位导航技术在两栖环境下的应用不可靠、两栖环境下的定位导航十分困难的问题。