[发明专利]一种基于混合拓扑结构的多机器人协同导航定位算法

说明书

一种基于混合拓扑结构的多机器人协同导航定位算法，其特征在于，包括如下步骤：搭建MMRS工作环境，测量工作环境中固定路标点的位置、移动机器人的初始位置和姿态信息；建立MMRS协同导航非线性系统方程，包括状态方程和观测方程；移动机器人开始工作，对MMRS协同导航系统按照EKF滤波框架进行时间更新；移动机器人分别对工作环境中的固定路标点以及其它机器人进行实时观测，获取观测信息；利用混合拓扑结构对系统进行量测更新；对MMRS中移动机器人的位姿信息进行更新，完成MMRS高精度协同导航定位过程。本发明方法利用混合拓扑结构可实现观测信息的高效率利用，能够同时解决MMRS协同导航中的非线性和不确定性问题，从而提高MMRS的协同导航定位精度。

技术领域

本发明涉及机器人领域，具体涉及一种一种基于混合拓扑结构的多机器人协同导航定位算法。

背景技术

自1960年第一台移动机器人问世之后，移动机器人的发展得到了广泛的关注。由于移动机器人具有自主性、独立性和小体积等诸多优点，被广泛应用于室内、室外、空间、水面、水下等多种领域。随着任务复杂性的提高，单机器人越来越无法满足需求，而多机器人不仅能够完成各类复杂的任务、提高效率，而且具有更高的精度和更好的冗余性，因此，多移动机器人系统逐渐成为机器人领域的一个研究热点。

多移动机器人系统的协同导航是多机器人顺利、高效完成任务的前提和保障，而协同导航的本质是滤波估计。因此，对多移动机器人系统的可观测性进行分析，在此基础上开展对高精度协同导航定位算法的研究具有重要的理论和实际意义。

目前，在进行系统的可观测性分析时一般将多移动机器人系统近似为线性系统，但在实际的系统存在非线性，尤其是在机器人运动情况下。将非线性系统近似为线性系统来处理将会降低滤波算法的精度，甚至会引起滤波发散。在非线性系统可观测性分析方面，最常用的是基于李导数的相对位置量测图(Relative Position Measurement Graph,RPMG)可观测性分析方法，该方法采用扩展卡尔曼滤波器(Extended Kalman Filter,EKF)解决了系统的非线性问题，用利用RPMG方法对系统的可观测性进行分析。但是，实际的多移动机器人系统中由于机器人的移动、机器人之间的相互遮挡以及所搭载传感器的测量局限，系统存在较大的不确定性，RPMG方法中可观测性矩阵是实时变化的，若仍然使用该方法则会引入较大的误差，降低多移动机器人系统的导航定位精度。

发明内容

本发明的目的是提供一种能够同时解决多移动机器人系统中非线性和不确定性问题,移动机器人之间的观测信息利用率更高，导航定位精度更优的一种基于混合拓扑结构的多机器人协同导航定位算法。

本发明的目的是通过以下步骤来实现的：

步骤1：搭建多移动机器人系统工作环境，测量工作环境中固定路标点的位置、移动机器人的初始位置和姿态信息；

步骤2：建立多移动机器人系统协同导航非线性系统方程，包括状态方程和观测方程；

步骤3：移动机器人开始工作，对整个多移动机器人系统协同导航系统按照EKF滤波框架进行时间更新；

步骤4：移动机器人分别对工作环境中的固定路标点以及其它机器人进行实时观测，获取观测信息；

步骤5：利用混合拓扑结构对系统进行量测更新；

步骤6：对多移动机器人系统中移动机器人的位姿信息进行更新，

步骤7：重复步骤3～步骤6，完成多移动机器人系统高精度协同导航定位。

进一步地，所述的利用混合拓扑结构对系统进行量测更新，包括如下子步骤：首先利用基于李导数的RPMG可观测性分析方法对多移动机器人系统的可观测性进行分析，获取具有完全可观测性的子系统；然后利用混合拓扑结构将可观测性进行传递，获得新的具有完全可观测性的子拓扑结构，最后对该子拓扑结构进行量测更新。