CN115601432B - 一种基于fpga的机器人位置最优估计方法及系统 - Google Patents

Abstract

本发明公开了一种基于FPGA的机器人位置最优估计方法及系统，该方法包括：通过机器人设备获取所处区域对应的两路视觉信息和激光传感信息；通过FPGA设备对两路视觉信息和激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一视觉特征点信息对应的激光特征点信息；对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置。可见，本发明能够借助多种信息来实现更加精确的机器人位置估计，从而为后续的机器人工作提供更加准确的参考计算数据基础。

Description

一种基于FPGA的机器人位置最优估计方法及系统

技术领域

本发明涉及机器人技术领域，尤其涉及一种基于FPGA(Field Programmable GateArray，现场可编程门阵列)的机器人位置最优估计方法及系统。

背景技术

环境中机器人的位置估计对于移动机器人的应用有着重要的作用，其对机器人的一些关键应用如机器人环境探测和导航等方面起着重要的作用。随着机器人技术的发展，其在机器人应用具有广阔的发展前途。

目前，公知的机器人位置估计有许多的方法，主要包括全球定位方法(GPS)、基于视觉的位置估计方法和基于激光传感器的位置估计方法。但全球定位方法估计的精度较低而且容易受到周围环境的影响，如高大的建筑物等。基于视觉的位置估计方法一般是利用视觉传感器扫描周围的环境信息，再结合其它算法如扩展卡尔曼方法估计机器人的位置，但当环境的纹理信息稀少的时候，估计的精度显著下降。该方法易受环境影响，如阴影、遮断等，而且常常庞大的视觉信息导致计算效率低下。基于激光传感器的位置估计方法相对于上述方法可以获得更高的位置估计精度，但是在每一瞬间获取的环境的信息和环境范围相对于视觉传感器要少很多，使其难以应用于复杂的工作环境。可见，现有技术存在缺陷，亟待解决。

发明内容

本发明所要解决的技术问题在于，提供一种基于FPGA的机器人位置最优估计方法及系统，能够实现更加精确的机器人位置估计。

为了解决上述技术问题，本发明第一方面公开了一种基于FPGA的机器人位置最优估计方法，所述方法包括：

通过机器人设备获取所述机器人设备所处区域对应的两路视觉信息和激光传感信息；所述两路视觉信息分别通过所述机器人设备上的两个摄像设备获取；

通过FPGA设备对所述两路视觉信息和所述激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一所述视觉特征点信息对应的激光特征点信息；

对所述两路视觉信息进行立体匹配，得到每一所述视觉特征点信息的位置信息，根据所述位置信息，对所述多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；

根据所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息和所述激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算所述机器人设备的最优位置。

本发明实施例第二方面公开了一种基于FPGA的机器人位置最优估计系统，所述系统包括：

信息获取模块，用于通过机器人设备获取所述机器人设备所处区域对应的两路视觉信息和激光传感信息；所述两路视觉信息分别通过所述机器人设备上的两个摄像设备获取；

特征提取模块，用于通过FPGA设备对所述两路视觉信息和所述激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一所述视觉特征点信息对应的激光特征点信息；

物体分割模块，用于对所述两路视觉信息进行立体匹配，得到每一所述视觉特征点信息的位置信息，根据所述位置信息，对所述多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；

位置估计模块，用于根据所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息和所述激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算所述机器人设备的最优位置。

与现有技术相比，本发明实施例具有以下有益效果：

本发明实施例中，公开了一种基于FPGA的机器人位置最优估计方法及系统，该方法包括：通过机器人设备获取所处区域对应的两路视觉信息和激光传感信息；通过FPGA设备对两路视觉信息和激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一视觉特征点信息对应的激光特征点信息；对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置。可见，本发明实施例能够借助多种信息来实现更加精确的机器人位置估计，从而为后续的机器人工作提供更加准确的参考计算数据基础。

附图说明

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

图1是本发明实施例公开的一种基于FPGA的机器人位置最优估计方法的流程示意图。

图2是本发明实施例公开的一种基于FPGA的机器人位置最优估计系统的结构示意图。

具体实施方式

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

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、装置、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

本发明公开了一种基于FPGA的机器人位置最优估计方法及系统，能够借助多种信息来实现更加精确的机器人位置估计，从而为后续的机器人工作提供更加准确的参考计算数据基础。以下分别进行详细说明。

实施例一

请参阅图1，图1是本发明实施例公开的一种基于FPGA的机器人位置最优估计方法的流程示意图。其中，图1所描述的基于FPGA的机器人位置最优估计方法应用于数据处理系统/数据处理设备/数据处理服务器(其中，该服务器包括本地处理服务器或云处理服务器)中。如图1所示，该基于FPGA的位置最优估计方法可以包括以下操作：

101、通过机器人设备获取机器人设备所处区域对应的两路视觉信息和激光传感信息；

具体的，两路视觉信息分别通过机器人设备上的两个摄像设备获取；

102、通过FPGA设备对两路视觉信息和激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一视觉特征点信息对应的激光特征点信息；

103、对两路视觉信息进行立体匹配，得到每一视觉特征点信息的位置信息，根据位置信息，对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；

104、根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置。

可见，上述发明实施例能够借助多种信息来实现更加精确的机器人位置估计，从而为后续的机器人工作提供更加准确的参考计算数据基础。

可选的，上述步骤中的，根据位置信息，对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象，包括：

根据位置信息，基于K均值聚类算法，对多个视觉特征点信息进行物体分割，以得到多个物体对象；

基于特征点速度计算算法，根据多个视觉特征点信息，确定静态物体对象和动态物体对象。

具体的，可以根据每一物体对象对应的至少一个视觉特征点信息的特征点速度，确定该物体对象是静态物体对象或动态物体对象，例如速度大于0或其他预设阈值时为动态物体对象，否则为静态物体对象。

具体的，k均值聚类算法(k-means clustering algorithm)是一种迭代求解的聚类分析算法，是一种比较经典的物体分割算法，物体分割后，通过匹配的特征点比如SURF特征点计算出这些特征点的速度后就可确认物体是静态还是动态物体。

可选的，视觉特征点信息或激光特征点信息包括特征点的强度信息、方向信息和运动特征信息。

具体的，在一个实施方案中，可以由机器人对左右两个摄像头获取环境的两路视频信息与激光传感器扫描信息同步并行进入FPGA设备中的DRAM(Dynamic Random AccessMemory，动态随机存取存储器)中。同时对视觉信息和对应的激光信息进行特征提取，特征信息包括强度、方向、运动特征等；视觉信息同时立体匹配得到特征点的位置信息。对这些特征点采用K-均值聚类并分割成静态物体和动态物体。其中，激光传感器扫描信息主要是激光传感器输出的点云数据，其实质是传感器扫描到的物体的每个点的三维数据，即位置信息。

可选的，上述步骤中的，根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置，包括：

基于以下公式对机器人设备的最优位置进行计算：

其中，

为机器人设备的位置/>

的最优估计，U_t为机器人设备的控制信号信息,Z_t为三维测量信息，Z_t＝{Z_C,t,Z_L,t}，其中，Z_C,t,和Z_L,t分别为视觉特征点信息的三维测量信息和激光特征点信息对应的三维测量信息，且有Z_C,t＝{Z_CS,t,Z_CD,t}，Z_L,t＝{Z_LS,t,Z_LD,t}，其中，Z_CS,t和Z_CD,t分别为静态物体对象和动态物体对象对应的视觉特征点信息的三维测量信息；Z_LS,t和Z_LD,t分别为静态物体对象和动态物体对象对应的激光特征点信息的三维测量信息，t为采样时间，t＝1,…,n，n为第n个采样时间。

具体的，机器人设备的控制信号信息是指驱动机器人的控制信号，因为实际控制可能存在偏差的原因，使得估计的精度降低，这个降低反应在运动误差上，而运动误差可以通过实验来测定。

其中，基于贝叶斯公式，可以推导：

其中η₁为归一化系数，具体的，考虑到测量点的独立性并且假定测量和运动均符合高斯分布，则

可取：

其中η₂为归一化系数，

分别为Z_C,t,和Z_L,t中的第i个特征点，N为特征点数量；

其中：

其中Z_D,t-1＝{Z_CD,t-1,Z_LD,t-1}，η₃为归一化系数。考虑到机器人的运动，假定机器人的运动符合高斯分布，则取：

其中N()为高斯分布密度函数，

N_C,t＝Q_C，N_L,t＝Q_L，N_D,t＝Q_D；其中，Q_C，Q_L，Q_D分别为机器人设备单独在视觉特征点信息、激光特征点信息、动态物体对象对应的特征信息的辅助下的运动误差值，此处的运动误差值是指单独在视觉特征点信息、激光特征点信息、动态物体对象对应的特征信息的辅助下估计得到的位置和实际位置之间的位置差，单位为位置坐标的单位。其中，w₁、w₂、w₃为加权系数。

可选的，加权系数w₁、w₂和w₃可以由下式求解得到：

其中，E{}为均方误差函数，0≤w₁,w₂,w₃≤1，w₁+w₂+w₃＝1。

通过上述实施例提供了一种基于FPGA机器人位置的高效多层融合最优估计方法，可以结合视觉传感器和激光传感器使机器人更高效更精确地估计其在工作环境中的位置，同时减小了系统体积，提高系统的移植性。

实施例二

请参阅图2，图2是本发明实施例公开的一种基于FPGA的机器人位置最优估计系统的结构示意图。如图2所示，该基于FPGA的机器人位置最优估计系统可以包括：

信息获取模块201，用于通过机器人设备获取机器人设备所处区域对应的两路视觉信息和激光传感信息；两路视觉信息分别通过机器人设备上的两个摄像设备获取；

特征提取模块202，用于通过FPGA设备对两路视觉信息和激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一视觉特征点信息对应的激光特征点信息；

物体分割模块203，用于对两路视觉信息进行立体匹配，得到每一视觉特征点信息的位置信息，根据位置信息，对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；

位置估计模块204，用于根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置。

可见，上述发明实施例能够借助多种信息来实现更加精确的机器人位置估计，从而为后续的机器人工作提供更加准确的参考计算数据基础。

可选的，物体分割模块203根据位置信息，对多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象的具体方式，包括：

根据位置信息，基于K均值聚类算法，对多个视觉特征点信息进行物体分割，以得到多个物体对象；

基于特征点速度计算算法，根据多个视觉特征点信息，确定静态物体对象和动态物体对象。

具体的，可以根据每一物体对象对应的至少一个视觉特征点信息的特征点速度，确定该物体对象是静态物体对象或动态物体对象，例如速度大于0或其他预设阈值时为动态物体对象，否则为静态物体对象。

具体的，k均值聚类算法(k-means clustering algorithm)是一种迭代求解的聚类分析算法，是一种比较经典的物体分割算法，物体分割后，通过匹配的特征点比如SURF特征点计算出这些特征点的速度后就可确认物体是静态还是动态物体。

可选的，视觉特征点信息或激光特征点信息包括特征点的强度信息、方向信息和运动特征信息。

具体的，在一个实施方案中，可以由机器人对左右两个摄像头获取环境的两路视频信息与激光传感器扫描信息同步并行进入FPGA设备中的DRAM(Dynamic Random AccessMemory，动态随机存取存储器)中。同时对视觉信息和对应的激光信息进行特征提取，特征信息包括强度、方向、运动特征等；视觉信息同时立体匹配得到特征点的位置信息。对这些特征点采用K-均值聚类并分割成静态物体和动态物体。其中，激光传感器扫描信息主要是激光传感器输出的点云数据，其实质是传感器扫描到的物体的每个点的三维数据，即位置信息。

可选的，位置估计模块204根据静态物体对象和动态物体对象对应的视觉特征点信息和激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算机器人设备的最优位置的具体方式，包括：

基于以下公式对机器人设备的最优位置进行计算：

其中，

为机器人设备的位置/>

的最优估计，U_t为机器人设备的控制信号信息,Z_t为三维测量信息，Z_t＝{Z_C,t,Z_L,t}，其中，Z_C,t,和Z_L,t分别为视觉特征点信息的三维测量信息和激光特征点信息对应的三维测量信息，且有Z_C,t＝{Z_CS,t,Z_CD,t}，Z_L,t＝{Z_LS,t,Z_LD,t}，其中，Z_CS,t和Z_CD,t分别为静态物体对象和动态物体对象对应的视觉特征点信息的三维测量信息；Z_LS,t和Z_LD,t分别为静态物体对象和动态物体对象对应的激光特征点信息的三维测量信息，t为采样时间，t＝1,…,n，n为第n个采样时间。

具体的，机器人设备的控制信号信息是指驱动机器人的控制信号，因为实际控制可能存在偏差的原因，使得估计的精度降低，这个降低反应在运动误差上，而运动误差可以通过实验来测定。

其中，基于贝叶斯公式，可以推导：

其中η₁为归一化系数，具体的，考虑到测量点的独立性并且假定测量和运动均符合高斯分布，则

可取：

其中η₂为归一化系数，

分别为Z_C,t,和Z_L,t中的第i个特征点，N为特征点数量；/>

其中：

其中Z_D,t-1＝{Z_CD,t-1,Z_LD,t-1}，η₃为归一化系数。考虑到机器人的运动，假定机器人的运动符合高斯分布，则取：

其中N()为高斯分布密度函数，

N_C,t＝Q_C，N_L,t＝Q_L，N_D,t＝Q_D；其中，Q_C，Q_L，Q_D分别为机器人设备单独在视觉特征点信息、激光特征点信息、动态物体对象对应的特征信息的辅助下的运动误差值，此处的运动误差值是指单独在视觉特征点信息、激光特征点信息、动态物体对象对应的特征信息的辅助下估计得到的位置和实际位置之间的位置差，单位为位置坐标的单位。其中，w₁、w₂、w₃为加权系数。

可选的，加权系数w₁、w₂和w₃可以由下式求解得到：

其中，E{}为均方误差函数，0≤w₁,w₂,w₃≤1，w₁+w₂+w₃＝1。

通过上述实施例提供了一种基于FPGA机器人位置的高效多层融合最优估计系统，可以结合视觉传感器和激光传感器使机器人更高效更精确地估计其在工作环境中的位置，同时减小了系统体积，提高系统的移植性。

上述对本说明书特定实施例进行了描述，其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，附图中描绘的过程不一定必须按照示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本说明书时可以把各单元的功能在同一个或多个软件和/或硬件中实现。

本领域内的技术人员应明白，本说明书实施例可提供为方法、系统、或计算机程序产品。因此，本说明书实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本说明书实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本说明书是参照根据本说明书实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带式磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

本说明书可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本说明书，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

最后应说明的是：本发明实施例公开的一种基于FPGA的机器人位置最优估计方法及系统所揭露的仅为本发明较佳实施例而已，仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述各项实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应的技术方案的本质脱离本发明各项实施例技术方案的精神和范围。

Claims (4)

1.一种基于FPGA的机器人位置最优估计方法，其特征在于，所述方法包括：

通过机器人设备获取所述机器人设备所处区域对应的两路视觉信息和激光传感信息；所述两路视觉信息分别通过所述机器人设备上的两个摄像设备获取；

通过FPGA设备对所述两路视觉信息和所述激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一所述视觉特征点信息对应的激光特征点信息；

对所述两路视觉信息进行立体匹配，得到每一所述视觉特征点信息的位置信息，根据所述位置信息，基于K均值聚类算法，对所述多个视觉特征点信息进行物体分割，以得到多个物体对象；

基于特征点速度计算算法，根据所述多个视觉特征点信息，确定静态物体对象和动态物体对象；

基于以下公式对所述机器人设备的最优位置进行计算：

其中，

为所述机器人设备的位置/>

的最优估计，U_t为所述机器人设备的控制信号信息,Z_t为三维测量信息，Z_t＝{Z_C,t,Z_L,t}，其中，Z_C,t,和Z_L,t分别为所述视觉特征点信息的三维测量信息和所述激光特征点信息对应的三维测量信息，且有Z_C,t＝{Z_CS,t,Z_CD,t}，Z_L,t＝{Z_LS,t,Z_LD,t}，其中，Z_CS,t和Z_CD,t分别为所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息的三维测量信息；Z_LS,t和Z_LD,t分别为所述静态物体对象和所述动态物体对象对应的所述激光特征点信息的三维测量信息，t为采样时间，t＝1,…,n，n为第n个采样时间；

其中，基于贝叶斯公式，可以推导：

其中η₁为归一化系数，

可取：

其中η₂为归一化系数，

分别为Z_C,t,和Z_L,t中的第i个特征点，N为特征点数量；

其中：

其中Z_D,t-1＝{Z_CD,t-1,Z_LD,t-1}，η₃为归一化系数；其中：

/>

其中N()为高斯分布密度函数，

N_C,t＝Q_C，N_L,t＝Q_L，N_D,t＝Q_D；其中，Q_C，Q_L，Q_D分别为所述机器人设备单独在所述视觉特征点信息、所述激光特征点信息、所述动态物体对象对应的特征信息的辅助下的运动误差值，w₁、w₂、w₃为加权系数；所述加权系数w₁、w₂和w₃可以由下式求解得到：

其中，E{}为均方误差函数，0≤w₁,w₂,w₃≤1，w₁+w₂+w₃＝1。

2.根据权利要求1所述的基于FPGA的机器人位置最优估计方法，其特征在于，所述视觉特征点信息或所述激光特征点信息包括特征点的强度信息、方向信息和运动特征信息。

3.一种基于FPGA的机器人位置最优估计系统，其特征在于，所述系统包括：

信息获取模块，用于通过机器人设备获取所述机器人设备所处区域对应的两路视觉信息和激光传感信息；所述两路视觉信息分别通过所述机器人设备上的两个摄像设备获取；

特征提取模块，用于通过FPGA设备对所述两路视觉信息和所述激光传感信息进行特征信息提取，得到多个视觉特征点信息和每一所述视觉特征点信息对应的激光特征点信息；

物体分割模块，用于对所述两路视觉信息进行立体匹配，得到每一所述视觉特征点信息的位置信息，根据所述位置信息，对所述多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象；所述物体分割模块根据所述位置信息，对所述多个视觉特征点信息进行聚类分割，以确定静态物体对象和动态物体对象的具体方式，包括：

根据所述位置信息，基于K均值聚类算法，对所述多个视觉特征点信息进行物体分割，以得到多个物体对象；

基于特征点速度计算算法，根据所述多个视觉特征点信息，确定静态物体对象和动态物体对象；

位置估计模块，用于根据所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息和所述激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算所述机器人设备的最优位置；所述位置估计模块根据所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息和所述激光特征点信息，基于最优估计算法模型和贝叶斯算法模型，计算所述机器人设备的最优位置的具体方式，包括：

基于以下公式对所述机器人设备的最优位置进行计算：

其中，

为所述机器人设备的位置/>

的最优估计，U_t为所述机器人设备的控制信号信息,Z_t为三维测量信息，Z_t＝{Z_C,t,Z_L,t}，其中，Z_C,t,和Z_L,t分别为所述视觉特征点信息的三维测量信息和所述激光特征点信息对应的三维测量信息，且有Z_C,t＝{Z_CS,t,Z_CD,t}，Z_L,t＝{Z_LS,t,Z_LD,t}，其中，Z_CS,t和Z_CD,t分别为所述静态物体对象和所述动态物体对象对应的所述视觉特征点信息的三维测量信息；Z_LS,t和Z_LDt分别为所述静态物体对象和所述动态物体对象对应的所述激光特征点信息的三维测量信息，t为采样时间，t＝1,…,n，n为第n个采样时间；

其中，基于贝叶斯公式，可以推导：

其中η₁为归一化系数，

可取：

其中η₂为归一化系数，

分别为Z_C,t,和Z_L,t中的第i个特征点，N为特征点数量；

其中：

其中Z_D,t-1＝{Z_CD,t-1,Z_LD,t-1}，η₃为归一化系数；其中：

其中N()为高斯分布密度函数，

N_C,t＝Q_C，N_L,t＝Q_L，N_D,t＝Q_D；其中，Q_C，Q_L，Q_D分别为所述机器人设备单独在所述视觉特征点信息、所述激光特征点信息、所述动态物体对象对应的特征信息的辅助下的运动误差值，w₁、w₂、w₃为加权系数；所述加权系数w₁、w₂和w₃可以由下式求解得到：

其中，E{}为均方误差函数，0≤w₁,w₂,w₃≤1，w₁+w₂+w₃＝1。

4.根据权利要求3所述的基于FPGA的机器人位置最优估计系统，其特征在于，所述视觉特征点信息或所述激光特征点信息包括特征点的强度信息、方向信息和运动特征信息。

Images