用于机器人区域定位的多传感器数据融合方法及系统

技术领域

本发明涉及机器人自动控制领域和自动驾驶汽车控制系统领域，具体地说是一种用于机器人区域定位的多传感器数据融合方法及系统。

背景技术

机器人自主导航逐步替代传统单一寻迹、固定路线运动的方式，成为了机器人移动控制中的一大主要控制方式。该控制方式主要依赖于SLAM技术，即建图与定位技术。这两项技术也是现今常用的自主导航、路径规划算法的两大基础。更精确的定位，意味着路径规划结果具有更准确、更加优化的提升空间。因此，优秀的定位算法是机器人稳定、精确运行的重要基础之一。

故如何实现传感器数据滤波，同时完成多种传感器的数据融合，确保输出较单一传感器数据更为精确的位姿结果数据是目前亟待解决的问题。

发明内容

本发明的技术任务是提供一种用于机器人区域定位的多传感器数据融合方法及系统，来解决如何实现传感器数据滤波，同时完成多种传感器的数据融合，确保输出较单一传感器数据更为精确的位姿结果数据的问题。

本发明的技术任务是按以下方式实现的，一种用于机器人区域定位的多传感器数据融合方法，该方法是通过多种可选的传感器，实现多种传感器数据的接入和采集，以扩展卡尔曼滤波为基础，对数据进行融合计算，拟合得到机器人位姿数据；具体如下：

传感器数据预处理；

扩展卡尔曼滤波数据融合。

作为优选，传感器数据预处理具体如下：

坐标系转换：将传感器采集到的原始数据统一转换为以机器人核心坐标系为参考坐标系的数据；机器人核心坐标系以机器人质心为原点；

数据格式统一：根据不同传感器类型分别处理，将数据类型统一为位姿数据，即空间位置坐标加四元数表示的姿态数据结构类型，并根据位置和姿态数据转换得到的位姿矩阵。

更优地，所述传感器类型包括视觉里程计传感器(VO)、惯性测量单元(IMU)、卫星定位系统(GPS)、轮式或编码器里程计(Odometer)及激光里程计(Laser-Odom)。

更优地，扩展卡尔曼滤波数据融合具体如下：

通过统一传感器数据结构，按顺序将多种传感器数据送入同一扩展卡尔曼滤波器；

利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终输出位姿数据及其协方差矩阵。

作为优选，所述扩展卡尔曼滤波器是针对非线性系统，通过线性化方法将非线性系统近似为线性系统，从而使用卡尔曼滤波方法完成滤波的扩展卡尔曼滤波器；

卡尔曼滤波方法是利用状态方程和观测构建的自回归滤波器。

一种用于机器人区域定位的多传感器数据融合系统，该系统包括，

数据预处理单元，用于通过多种可选的传感器，实现多种传感器数据的接入和采集，对不同传感器数据进行数据预处理；

数据融合模块，用于以扩展卡尔曼滤波为基础，对数据进行融合计算，拟合得到机器人位姿数据。

作为优选，所述数据预处理单元包括，

坐标系转换模块，用于将传感器采集到的原始数据统一转换为以机器人核心坐标系为参考坐标系的数据；机器人核心坐标系以机器人质心为原点；

数据格式统一模块，用于根据不同传感器类型分别处理，将数据类型统一为位姿数据，即空间位置坐标加四元数表示的姿态数据结构类型，并根据位置和姿态数据转换得到的位姿矩阵。

更优地，所述数据融合模块包括，

数据送入模块，用于通过统一传感器数据结构，按顺序将多种传感器数据送入同一扩展卡尔曼滤波器；

输出模块，用于利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终输出位姿数据及其协方差矩阵；

更优地，该系统的工作过程具体如下：

(1)、通过多种传感器采集原始数据，通过数据预处理模块对不同传感器数据进行数据预处理，具体包括：

①、通过坐标系转换模块统一转换位置关系为机器人核心坐标与地图坐标系相对位置关系；

②、通过数据格式统一模块统一数据结构为机器人位置与姿态结构类型；

(2)、通过数据送入模块将数据按照顺序送入同一扩展卡尔曼滤波器；

(3)、利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终通过输出模块输出位姿数据及其协方差矩阵。

一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行时，实现如上述的用于机器人区域定位的多传感器数据融合方法。

本发明的用于机器人区域定位的多传感器数据融合方法及系统具有以下优点：

(一)本发明既解决了传感器数据滤波的问题，又同时完成了多种传感器数据融合，输出较单一传感器数据更为精确的位姿结果数据；

(二)本发明通过多种可选的传感器实现多种传感器数据接入和采集，以扩展卡尔曼滤波算法为基础，对数据进行融合，拟合得到机器人位姿数据，可用于具有自主导航能力的机器人，提高机器人运行过程中自身位姿数据的精确度，为导航算法提供保障。

附图说明

下面结合附图对本发明进一步说明。

附图1为用于机器人区域定位的多传感器数据融合方法的流程示意图。

具体实施方式

参照说明书附图和具体实施例对本发明的用于机器人区域定位的多传感器数据融合方法及系统作以下详细地说明。

实施例1：

如附图1所示，本发明的用于机器人区域定位的多传感器数据融合方法,该方法是通过多种可选的传感器，实现多种传感器数据的接入和采集，以扩展卡尔曼滤波为基础，对数据进行融合计算，拟合得到机器人位姿数据；具体如下：

S1、传感器数据预处理；

本实施例中的传感器类型具体如下：

本发明通过数据采集的模块化，支持多种传感器类型，目前包括：视觉里程计传感器(VO)、惯性测量单元(IMU)、卫星定位系统(GPS)、轮式或编码器里程计(Odometer)及激光里程计(Laser-Odom)。

S2、扩展卡尔曼滤波数据融合。

本实施例中步骤S1的传感器数据预处理具体如下：

S101、坐标系转换：由于机器人系统中，各个传感器安装位置不同，采集的原始数据参考坐标系就会存在差异。为精确计算机器人位姿，将传感器采集到的原始数据统一转换为以机器人核心坐标系为参考坐标系的数据；机器人核心坐标系以机器人质心为原点；但也可由用户自定义，例如运动学或动力学中心位置。

S102、数据格式统一：根据不同传感器类型分别处理，将数据类型统一为位姿数据，即空间位置坐标加四元数表示的姿态数据结构类型，并根据位置和姿态数据转换得到的位姿矩阵。代码示例如下：

其中，矩阵M是根据位置和姿态数据转换得到的位姿矩阵；个别类型传感器并不具有全部的有效数据，例如不同型号的IMU可能具备姿态数据输出，也可能不具有该项功能。无有效数据的情况下，该项数据置零。

本实施例中步骤S2的扩展卡尔曼滤波数据融合具体如下：

S201、通过统一传感器数据结构，按顺序将多种传感器数据送入同一扩展卡尔曼滤波器；

S202、利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终输出位姿数据及其协方差矩阵。

其中，扩展卡尔曼滤波器是针对非线性系统，通过线性化方法将非线性系统近似为线性系统，从而使用卡尔曼滤波方法完成滤波的扩展卡尔曼滤波器；

卡尔曼滤波方法是利用状态方程和观测构建的自回归滤波器。

实施例2：

本发明的用于机器人区域定位的多传感器数据融合系统，该系统包括，

数据预处理单元，用于通过多种可选的传感器，实现多种传感器数据的接入和采集，对不同传感器数据进行数据预处理；

数据融合模块，用于以扩展卡尔曼滤波为基础，对数据进行融合计算，拟合得到机器人位姿数据。

本实施例中的数据预处理单元包括，

坐标系转换模块，用于将传感器采集到的原始数据统一转换为以机器人核心坐标系为参考坐标系的数据；机器人核心坐标系以机器人质心为原点；

数据格式统一模块，用于根据不同传感器类型分别处理，将数据类型统一为位姿数据，即空间位置坐标加四元数表示的姿态数据结构类型，并根据位置和姿态数据转换得到的位姿矩阵。

本实施例中的数据融合模块包括，

数据送入模块，用于通过统一传感器数据结构，按顺序将多种传感器数据送入同一扩展卡尔曼滤波器；

输出模块，用于利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终输出位姿数据及其协方差矩阵；

该系统的工作过程具体如下：

(1)、通过多种传感器采集原始数据，通过数据预处理模块对不同传感器数据进行数据预处理，具体包括：

①、通过坐标系转换模块统一转换位置关系为机器人核心坐标与地图坐标系相对位置关系；

②、通过数据格式统一模块统一数据结构为机器人位置与姿态结构类型；

(2)、通过数据送入模块将数据按照顺序送入同一扩展卡尔曼滤波器；

(3)、利用不同传感器数据更新同一观测矩阵、控制矩阵和协方差矩阵，并最终通过输出模块输出位姿数据及其协方差矩阵。

实施例3：

本发明实施例还提供了一种计算机可读存储介质，其中存储有多条指令，指令由处理器加载，使处理器执行本发明任一实施例中的用于机器人区域定位的多传感器数据融合方法。具体地，可以提供配有存储介质的系统或者装置，在该存储介质上存储着实现上述实施例中任一实施例的功能的软件程序代码，且使该系统或者装置的计算机(或CPU或MPU)读出并执行存储在存储介质中的程序代码。

在这种情况下，从存储介质读取的程序代码本身可实现上述实施例中任何一项实施例的功能，因此程序代码和存储程序代码的存储介质构成了本发明的一部分。

用于提供程序代码的存储介质实施例包括软盘、硬盘、磁光盘、光盘(如CD-ROM、CD-R、CD-RW、DVD-ROM、DVD-R用于机器人区域定位的多传感器数据融合方法及系统M、DVD-RW、DVD+RW)、磁带、非易失性存储卡和ROM。可选择地，可以由通信网络从服务器计算机上下载程序代码。

此外，应该清楚的是，不仅可以通过执行计算机所读出的程序代码，而且可以通过基于程序代码的指令使计算机上操作的操作系统等来完成部分或者全部的实际操作，从而实现上述实施例中任意一项实施例的功能。

此外，可以理解的是，将由存储介质读出的程序代码写到插入计算机内的扩展板中所设置的存储器中或者写到与计算机相连接的扩展单元中设置的存储器中，随后基于程序代码的指令使安装在扩展板或者扩展单元上的CPU等来执行部分和全部实际操作，从而实现上述实施例中任一实施例的功能。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。