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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

以轮桨里程数据方差作为置信度，并将四个轮桨产生的具有相同时间戳的置信度数据设为集合C

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

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

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

按照置信度大小对轮桨i进行排序，轮桨中最高的置信度定义为

使用sigmoid函数对置信度

设集合M

通过异常数据测试试验测得置信度阈值α，在连续的多个时间步内，当M

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

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

设向量

使用余弦相似度对同侧的两个轮桨i间的相似度进行表征，则滑移转向机器人的右侧扭矩向量

其中，

通过异常数据测试试验测得速度相似度阈值β

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

设odom1产生了测量值x

其中，

设odom2产生了测量值x

其中，

对于

将P

对于

将P

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

P

其中，ω为协方差交叉滤波的比例参数，P

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

其中，P

而后，通过卡尔曼滤波最终输出定位信息

X

P

其中，X

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

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是水陆两栖环境下滑移转向轮式机器人的定位导航方法的流程图。

图2是水陆两栖环境下滑移转向轮式机器人的异常数据处理流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供一种水陆两栖环境下滑移转向轮式机器人的定位导航方法，如图1所示，包括以下步骤：

以惯性导航系统作为参考系统，滑移转向轮式机器人的两个前轮的轮式里程计组成的子系统odom1与惯性导航系统(ins)组成第一个局部滤波器odom1/ins，滑移转向轮式机器人的两个后轮的轮式里程计组成的子系统odom2与ins系统组成第二个局部滤波器odom2/ins，GNSS系统与ins组成第三个局部滤波器GNSS/ins，设计联邦(卡尔曼)滤波器，三个局部滤波器输出的局部状态估计值

两栖环境极为复杂，某个或多个轮式里程计可能因此产生大量的异常数据，因此，对滑移转向轮式机器人各轮产生的传感数据进行异常数据处理，如图2所示，包括：

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

以轮桨里程数据方差作为置信度，并将四个轮桨产生的具有相同时间戳的置信度数据，设为集合C

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

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

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

按照置信度大小对轮桨i进行排序，轮桨中最高的置信度定义为

使用sigmoid函数对置信度

设集合M

通过异常数据测试试验测得置信度阈值α，在连续的多个(如5个)时间步内，当M

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

由于是四驱滑移转向机器人，因此发送指令时同侧轮桨指令一般是相同的，这使得在一个时间段内同侧的轮在速度和扭矩(电流)上应该具有相似性，设向量

使用余弦相似度对同侧的两个轮桨间的相似度进行表征，则滑移转向机器人的右侧扭矩向量

其中，

通过异常数据测试试验测得速度相似度阈值β

若在连续的多个(如5个)时间步内，以上

传统的联邦卡尔曼滤波器基于条件独立假设，即只有各局部滤波器输出的数据间不相关时才能实现一致性估计，而对于本实施例而言，各局部滤波器均使用了ins的数据，数据间的相关性是显而易见的，因此，在异常数据处理后，设计一种改进的分裂协方差交叉算法解决该问题，从而得到轮式里程计与ins的组合导航信息，具体过程为：

首先在局部滤波器中，以第一个局部滤波器odom1/ins为例，设odom1产生了测量值x

其中，

同理，设odom2产生了测量值x

其中，

显而易见的是，当想要将

对于

由于未知的可能独立项P

对于

由于未知的可能独立项P

由此，可由以下公式计算

P

其中，ω为协方差交叉滤波的比例参数，P

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

同理，我们使用相同的方法对机器人轮式里程计与ins的组合导航信息

其中，P

而后，通过卡尔曼滤波最终输出定位信息

X

P

其中，X

最终我们得到了一种水陆两栖环境下滑移转向轮式机器人的定位导航方法，基于联邦卡尔曼滤波框架，对四个轮式里程计信息与惯性导航系统进行分组融合和异常数据剔除，输出各自的组合定位导航信息，并通过提出的改进的分裂协方差交叉算法处理相关性问题从而得到轮式里程计与ins的组合导航信息，最终结合GPS绝对定位信息输出整体的定位导航信息，该方法可以为水陆两栖环境下运行的滑移转向轮式机器人提供准确的定位导航信息，实现机器人在两栖环境下的自主定位导航。

以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。