CN100461058C - 在复杂环境下智能机器人自主定位的方法 - Google Patents

Abstract

本发明结合最新的概率定位方法，将人类思维和认知过程移植到机器人对外部环境的感知中，很好的解决了实际定位过程中地标数量不足和碰撞影响定位精度等问题，使智能机器人能够在复杂环境中实现较为精确的定位。本发明针对智能移动机器人受各种复杂干扰影响精确定位的问题，提供一种自主定位的方法，该方法可适用于各种基于视觉传感器的移动机器人在复杂环境下完成自定位，为自主蔽障，导航等复杂任务提供良好的依据。本发明将人类思维和认知过程移植到机器人对外部环境的感知和记忆中，很好的改进了传统机器人定位模式，并对国际最新研究成果进行了改善。使用本发明的方法定位的智能自主机器人将能更好的适应实际生产生活。

Description

在复杂环境下智能机器人自主定位的方法

技术领域

本发明属于智能控制和计算机应用技术领域，涉及一种用于机器人自主定位的方法，更具体涉及一种机器人在复杂环境下基于预存储信息或群体协作实现自主定位的方法。可以用于机器人智能控制，机器人在复杂环境下自主作业时自定位，为自主机器人正确决策提供有利的前提条件。

背景技术

近百年来，机器人的发展大致经历了三个成长阶段，也即三个时代。第一代为简单个体机器人；第二代为群体劳动机器人，具备了感觉能力；第三代为具有高度自主智慧的智能机器人，它的未来发展方向是有知觉、有思维、能与人对话。现在，机器人正处于第二个发展时代，向第三个发展时代迈进。

目前，机器人研究和发展方向正在向智能化发展，因此机器人的自主性就显得尤为重要。移动机器人在诸多领域有着重要的作用，被广泛应用于生产生活中。在实际应用中，当机器人要自主的完成一项任务时，最重要的是要进行自定位，也就是机器人需要知道自己现在所处的位置，包括自身得局部坐标和所处大环境得全局坐标，然后才能决策下一步应该怎么做。

在此定位过程中，可以把机器人所能利用的信息分成两大类：内部传感器信息和外部传感器信息。前者通常是利用里程计得到的信息，既从内部运动机构得到得运动位移信息。此方法可以直接的确定移动机器人的位移，但只在短距离内才有足够的精度。后者则实际上是对外部环境(如路标)的探测信息。然而，所有的对定位有作用的环境信息都不是随时随地可以得到的，且都存在不确定性。这种不确定性往往导致位置数据不可靠，从而影响机器人的正确决策和后续行为。因此，有必要融合来自各传感器的互补或者冗余数据，得到机器人位置的可靠估计，再应用于机器人的其它子系统。

此外在处理传感器数据时，由于传感器测量的不确定性、融合算法的局限性、对周围环境缺乏精确的描述、以及路标特征的丰富性等原因，很多传感器信息都被丢弃了。如：在现有的移动机器人定位方法中，大多要求传感器数据是同时获得的，但机器人在实际工作中，常常不能同时获得能够满足方法要求的多个传感器数据，若某时刻的数据不能满足要求，则通常会丢弃该时刻已经获得的数据。因此提高传感器数据的利用效率也是需要关注的另外一个问题。

移动机器人的视觉导航主要包括定位、路径规划和避障等几方面，其中定位是移动机器人导航中最基本的环节，机器人的工作环境、配备传感器的种类和数量的不同使得移动机器人有多种定位方法，而基于路标的定位方法和基于全局视觉信息的定位方法是目前最常用的两种定位方法，路标可分为人工路标，如：直线、箭头或其他图形，和自然路标如：门、窗户、拐角、灯、柱子，所谓路标定位是从图像中分割、识别出路标，根据这些路标在环境中的已知坐标、形状等特征来确定机器人的相对位置，因此，这些路标应具有明显的特征并且应很容易从场景图像中被分割出来，但是在非结构化的环境中，当不能设置人工路标或者自然路标很难分割时，使用全局视觉信息来进行定位是可行的方法之一。基于全局视觉信息的定位方法常通过一个全局摄像头得到全局信息，包括机器人当前位置和场景里其他物体的位置，机器人本身不需要利用自主视觉对图像进行分割和局部特征提取，而是利用全局摄像头得到的位置信息确定自己的位置。这种定位方法对机器人本身的视觉能力和运算水平要求不高，甚至没有要求。但是此方法最大的缺点在于，如何获得全局信息。在绝大多数情况下，无法架设全局摄像头，机器人无法得到全局信息，进而无法进行自主定位。特别是在野外开阔环境下，机器人由于信息缺失，单纯依靠全局信息是不准确甚至无法实现的。因此，适用于各种复杂环境的自主定位方法就显得尤其重要，也是机器人向更高级化发展的核心技术之一。

自主定位作为智能机器人最为核心的几项关键技术之一，在机器人应用中起到了至关重要的作用。智能机器人体现的各种智能行为，多机器人配合体现的智能策略等都要基于准确的自定位信息。国际上对于机器人定位的研究热点主要集中在自主机器人的主动定位上。过去很多常见的基于全局摄像头的方法都变得不太适用。

实际生产生活中，以视觉传感器作为主要传感工具的机器人广泛存在。自主移动机器人，特别是有足机器人、类人机器人，在复杂环境中活动时，由于身体运动关节的3-D运动，导致视觉信息受到极大的干扰。传统的基于地标的定位方法，在实际应用中，特别是当环境未知时，变得十分不可靠。国际最新研究使用的基于概率的方法，集中考虑了通过优化运动模型和传感模型来解决定位的思想。但是这些方法一旦遇到机器人发生相互碰撞或与障碍物碰撞，就会给定位结果带来很大的误差。

发明内容

人类对自身位置的确定往往是依靠过去的经验，而不是时刻都在具体识别一些标志物。当人来到一个过去曾经来到的地方时，即使很久没有具体标志物出现，一些细微的视觉信息也可以帮助人完成自身位置的确定。另外，当一个人得到了自身所在位置，就可以判断出周围的某些物体的全局位置。如果此时另一个人也看到了这些物体，则可以根据此全局位置推算出自己的所在位置。这种协作的方法在人类社会的团体协作中尤为常见。正是由以上两种人类思维基本过程的优点所启发，我们提出了一种全新的智能机器人自定位方法。

本发明结合最新的概率定位方法，将人类思维和认知过程移植到机器人对外部环境的感知中，很好的解决了实际定位过程中地标数量不足和碰撞影响定位精度等问题，使智能机器人能够在复杂环境中实现较为精确的定位。

本发明针对智能移动机器人受各种复杂干扰影响精确定位的问题，提供一种自主定位的方法。该方法适用于各种基于视觉传感器的移动机器人在复杂环境下完成自定位，为自主蔽障，导航等复杂任务提供良好的依据。本发明将人类思维和认知过程移植到机器人对外部环境的感知和记忆中，很好的改进了传统机器人定位模式，并对国际最新研究成果进行了改善。使用本发明的方法进行定位的智能自主机器人将能更好的适应实际生产生活。

本发明的一个方面提供一种在复杂环境下智能机器人自主定位的方法，该方法适用于单个机器人独立工作的情况，或者多个机器人同时工作但是机器人之间无法精确识别彼此的情况，定位方法具体包括以下步骤：

1)在机器人工作区域内，随机生成若干个采样点，表征机器人当前的位置及该位置的概率；

2)如果机器人发生位置移动(即电机运动模块被驱动)，则根据运动模型更新所有采样点；

3)如果有传感器得到地标信息，则根据此信息更新各个采样点的概率，并增加候选点；

4)如果在一段时间内，都没有得到地标信息，则调用经验数据库，进行图像匹配；

如果在经验数据库中找到匹配程度符合要求的信息时，根据此经验信息更新采样点的概率，并增加候选点；

5)删除采样点中概率值最低的若干个点，并从候选点中选取相同个数的点加入到采样点中，删除所有候选点；

6)找出概率和最大的区域，计算此区域内的采样点的位置的加权平均值，所得到的位置作为机器人当前最佳位置；

7)反复执行2)至6)步，完成机器人的实时定位。

本发明的另一个方面提供一种在复杂环境下智能机器人自主定位的方法，该方法适用于多个机器人协同工作，即多个机器人之间可以相互进行通信，定位方法具体包括以下步骤：

1)在机器人工作区域内，随机生成若干个采样点，表征机器人当前的位置及该位置的概率；

2)如果机器人发生位置移动(即电机运动模块被驱动)，则根据运动模型更新所有采样点；

3)如果有传感器得到地标信息，则根据此信息更新各个采样点的概率，并增加候选点；

4)如果在一段时间内，都没有得到地标信息，则调用经验数据库，进行图像匹配，如果在经验数据库中找到匹配程度符合要求的信息时，根据此经验信息更新采样点的概率，并增加候选点；

5)如果机器人发现自己一段时间内既没有得到地标信息，又没有在经验数据库里找到符合要求的匹配，则跟其它机器人进行通讯，根据其他机器人传送的信息来更新采样点的概率，并增加候选点；

6)删除采样点中概率值最低的若干个点，并从候选点中选取相同个数的点加入到采样点中，删除所有候选点；

7)找出概率和最大的区域，计算此区域内的采样点的位置的加权平均值，所得到的位置作为机器人当前最佳位置；

8)反复执行步骤2)至7)，即可实现多个机器人协作实时定位。

附图说明

下面结合附图对本发明进一步详细地说明：

图1为四腿机器人足球比赛的场地示意图；

图2为单机器人独立工作实时定位步骤流程图；

图3为视觉信息匹配情况图；其中图3b和图3d是经验数据库中预存储的图像，图3a和图3c是机器人实时采集到的，需要匹配的图像；

图4为本发明概率更新过程示意图；其中图4a是随机采样示意图，图4b是概率更新后的位置信息示意图，图4c是最后得到的最佳位置示意图；

图5为多机器人协作实时定位流程图；

图6是本发明的方法与国际最新方法效果比较图。

具体实施方式

下面参照本发明的附图，更详细地描述本发明的最佳实施例。

本优选实施方式以四腿足球机器人在足球比赛中实时定位为例，说明在复杂环境下智能机器人自主定位的方法。本发明的智能机器人自主定位方法包括两种，第一种是单个机器人自主定位，这也是智能机器人用于定位的主要方法；第二是多个机器人协作定位，协作定位的基础是每个机器人自主独立的定位信息，通过机器人之间的相互通信，告知其他机器人自己的定位信息，然后把这些信息根据可信度进行综合，最后得到更为准确的定位。

四腿机器人足球是机器人足球比赛中备受瞩目的项目之一，它是连接传统的轮式机器人和最终的类人机器人(即两足机器人)的重要纽带。机器人足球是一个综合性的项目，它涉及众多的传统理论和前沿技术。机器人制造本身就集合了结构工程、电子电路、精密机械、仿生材料等多种技术，而计算机、自动控制、传感、无线通讯等技术则是机器人能够运动和踢球所不可缺少的，多个机器人之间的配合更是涉及比较复杂的关于多主体的协调、合作与策略等问题。

如图1所示为四腿机器人足球比赛的场地。在场地的两边放有四个标识柱，每个标识柱顶部的颜色各部相同，由粉色分别与黄色和蓝色组合而成。场地两边球门的颜色也不相同，机器人正是通过这些不同的颜色可以实现定位。但是紧紧依靠这种定位还不够精确。

颜色的不同通过机器人身上自带的图像传感器(即摄像头)来采集，机器人身上不仅仅包含图像传感器，还包括其他如压力传感器、以及触觉传感器等。因此，综合分析、利用这些传感器采集到的所有信息，可以实现更精确的定位。

本实施例中详细说明根据本发明的定位方法，实现机器人在场上精确定位的过程。为了详细说明本发明的机器人定位方法，先定义以下坐标系：

①绝对坐标系：它是在机器人工作区域内选定的固定坐标系。

②机器人坐标系：它是固连在机器人身上的动坐标系，其原点是机器人的几何中心，x轴正向是机器人身体的正前方向。

③视觉坐标系：它是固连在机器人摄像头上的动坐标系，其原点是机器人摄像头的光心，x轴正向是主光轴的出射光方向。

在比赛中单个机器人独立工作时候，或者多个机器人同时工作但是机器人之间无法精确识别的时候，这也是机器人主要的定位方法。

如图2所示为单机器人独立工作实时定位步骤流程示意图，具体定位方法按照以下步骤实现：

1)将机器人所在位置建模为一系列点的密度。而这些点都是对当前位置的预估计。在机器人工作区域内，随机生成若干个估计点，表征机器人当前的位置估计。每一个可能位置对应其位置置信度。

如图4为本发明概率更新过程示意图，图4a中的小箭头即表示随机采样的位置概率示意。由于开始时候没有任何特征信息和经验值信息，所以随即生成的估计点可能分布于场上的任意位置。

2)对于四腿足球机器人，我们选取其身体几何中心为位置参考点，并用一个四维向量φ表示。其四个分量分别为以工作区域某固定点为原点的x/y/z三轴坐标，以及机器人身体的朝向角度。如果机器人发生位置移动，即电机运动模块被驱动，则根据运动模型移动所有采样点：

φ_t(x，y，z，θ)＝φ_t-1(x，y，z，θ)+Δ_t

其中φ_t(x，y，z，θ)代表当前时刻t，机器人可能处在的位置；φ_t-1(x，y，z，θ)表示前一时刻机器人所处的位置；Δ_t表示从t—1时刻到t时刻机器人运动模块移动距离。移动内容包括在x/y/z各轴上位置移动的分量和朝向角度的变化。

根据运动模型更新位置概率可以通过机器人的里程表来计算，比如机器人在场上移动了6秒钟，根据机器人当前的移动速度均值，得到机器人行走的距离，我们的四腿机器人均速度45厘米/秒，从而得出行走270厘米，然后计算出更新后的位置。

3)如果有传感器得到地标信息，则根据此信息更新各个采样点的概率，并增加候选点：

机器人扫描当前拍摄到的图像，若在图像中识别出标志柱，则记下此标志柱在图像中的位置和大小。然后，利用已知的标志柱实际大小、机器人的摄像头属性(光心位置、像平面位置、焦距等)，根据凸透镜成像原理，可以计算出标志柱在摄像头坐标系下的坐标P_bc(x_bc，y_bc)(具体计算方法在任何一本有关凸透镜成像原理的书中都可以找到，由于不属于本发明的内容，故不作赘述)。此外，机器人通过自身的传感器，可以得到摄像头光心在机器人坐标系下的坐标P_cr(x_cr，y_cr)及主光轴在机器人坐标系下的方向θ_cr。根据P_bc，P_cr和θ_cr就可以计算出标志柱在机器人坐标系下的坐标P_br(x_br，y_br)：

x_br＝x_cr+x_bccosθ_cr-y_bcsinθ_cr

y_br＝y_cr+x_bcsinθ_cr+y_bccosθ_cr

根据标志柱在绝对坐标系下的坐标P_ba(x_ba，y_ba)及每一个采样点S_i在绝对坐标系下的坐标S_rai(x_rai，y_rai，θ_rai)，可以计算出标志柱在每一采样点所代表的机器人坐标系下的坐标P_bri(x_bri，y_bri)：

x_bri＝(x_ba-x_rai)cosθ_rai+(y_ba-y_rai)sinθ_rai

y_bri＝-(x_ba-x_rai)sinθ_rai+(y_ba-y_rai)cosθ_rai

对于每一个采样点S_i，比较(x_br，y_br)与(x_bri，y_bri)，若相差很小，则提高该采样点的概率；若相差很大，则降低该采样点的概率。判断差别大小的阈值及概率的增减幅度既可事先固定，也可实时变动。

Bel_t(φ_t(x，y，z，θ)←αP(s_t|(φ_t(x，y，z，θ))Bel_t(φ_t(x，y，z，θ))

Bel_t(φ_t(x，y，z，θ)代表当前估计位置的置信度，通过概率模型P(s_t|(φ_t(x，y，z，θ))来更新。

此外，根据标志柱在绝对坐标系下的坐标P_ba(x_ba，y_ba)及标志柱在机器人坐标系下的坐标P_br(x_br，y_br)，可以得出机器人在绝对坐标系下的坐标P_ra(x_ra，y_ra，θ_ra)应满足：

x_ba＝x_ra+x_br cosθ_ra-y_brsinθ_ra

y_ba＝y_ra+x_br sinθ_ra-y_brcosθ_ra

随机生成若干组满足上述条件的P_ra(x_ra，y_ra，θ_ra)，作为候选点。

4)如果在一段时间内，都没有得到地标信息，则调用经验数据库，进行图像匹配。匹配过程中首先计算当前采集图像的特征值，本实施例中，采用图像中每一像素点的几何位置、它本身的灰度值与周围像素点的灰度值之间的关系来进行图像匹配。机器人根据图像特征值算法，计算当前图像的特征值，然后在数据库中查找与之相差最小的图像I_best，记下两者的特征值之差。若此差足够小，则认为两幅图像非常匹配，可以用I_best图像所对应的位置P_best更新采样点的概率：对于每一个采样点，将其位置与P_best相比较，若相差很小，则提高该采样点的概率；若相差很大，则降低该采样点的概率。判断差别大小的阈值及概率的增减幅度既可事先固定，也可实时变动。

Bel_t(φ_t(x，y，z，θ)←βP(ε_t|(φ_t(x，y，z，θ))Bel_t(φ_t(x，y，z，θ))

其中P(ε_t|(φ_t(x，y，z，θ))代表基于经验位置的概率更新模型。此外，在P_best附近随机生成若干个位置点作为候选点。

例如：假设数据库中顺序存放有5幅图像，特征值为10、20、30、40、50，特征值之差的阈值为4。若计算出当前图像的特征值是28.5，则I_best＝3，特征值之差为1.5，远小于阈值4。于是认为机器狗当前的位置就是第3幅数据库图像对应的位置，用此位置更新所有采样点的概率，并在此位置附近随机生成若干个位置作为候选点。

图3为视觉信息匹配情况图；其中图3b和图3d是经验数据库中预存储的图像，图3a和图3c是机器人实时采集到的，需要匹配的图像；如图3a所示，图中(2388，-1186，300)表示手工采样此图像时，机器人所处的真实位置坐标。通过使用本发明的方法，机器人很快的在经验数据库中找到了特征匹配的图像信息，并计算出此时所在的位置坐标为(2388，-1203，300)。结果与手工度量的当前位置基本一致，误差很小。体现了方法的实用性和高效性。

5)删除采样点中概率值最低的若干个点，并从候选点中选取相同个数的点加入到采样点中，删除所有候选点：

计算所有采样点的概率的平均值，以此平均值为标准确定一个阈值(比如平均值的1/3)，将概率低于此阈值的采样点删除。每删除一个采样点，就从候选点中随机选取一个添加到采样点中。最后，将多余的候选点全部删除(确定阈值的过程已保证删除的采样点个数不超过候选点的总个数)。

6)找出概率和最大的区域，计算此区域内的采样点的位置的加权平均值，所得到的位置作为机器人当前最佳位置：

将机器人的工作区域划分为若干个规则的、不相交的子区域，则每个采样点对应的位置都落入唯一的一个子区域中。统计每个子区域中的采样点的概率和，概率和最大的子区域则为最佳子区域。计算最佳子区域中的所有采样点的位置的加权平均值(权值是采样点的概率)，所得到的平均值即为机器人当前的最佳位置；最佳子区域中的所有采样点的概率的平均值就是此最佳位置的可信度。

如图4b所示为根据不同模型更新位置估计后的图示，图4c所示为最后得到的最佳位置估计。

7)反复执行步骤2)至6)，完成机器人的实时定位。

本发明的另一个方面提供一种在复杂环境下智能机器人自主定位的方法，该方法适用于多个机器人协同工作，即多个机器人之间可以相互进行通信。多机器人协作定位可作为辅助的定位方法，由于需要机器人之间的实时通信，使用协作定位的方法，定位速度不如自主定位速度快，但是定位的信息可以更为准确，当单个机器人不能很好的定位的时候，或者需要非常精确的定位信息时，采用协作定位的方式更好。协作定位方法的前提和基础是每个机器人独立自主定位的信息。根据前面的叙述，协作定位方法的步骤1)至8)跟单个机器人自主定位方法的步骤1)至7)相比，仅仅是多了步骤5)，即多个机器人之间相互通信协作定位，下面就介绍这一步的详细实施过程。

多个机器人协作定位时，机器人之间可以进行相互通信。通信的协议不限，既可以采用UDP，又可以采用TCP/IP。本实例中的四腿机器人，配有符合IEEE802.11b标准的无线网卡，采用UDP广播的方式进行通讯。

机器人A在运动过程中，如果通过图像识别出另一个机器人B，则通过图像计算机器人B在机器人A坐标系下的坐标P_BA(计算过程类似于计算标志柱在机器人坐标系下的坐标的过程)，再根据自己当前的最佳位置P_A及其可信度，计算出B的绝对坐标P_B及其可信度r_B(由图像识别的可信度和P_A的可信度共同决定)。计算完后，A就将此P_B和r_B广播出去。

如果某机器人发现自己一段时间内既没有得到地标信息，又没有在经验数据库里找到符合要求的匹配，则接收当前所有由其它机器人发送的、有关自己位置的广播信息，并从中选取最高的可信度，若此可信度大于某一阈值，则用此可信度及其对应的位置更新所有采样点的概率，并在此位置附近随机生成若干个位置作为候选点。

比如，机器人A发现自己一段时间内既没有得到地标信息，又没有在经验数据库里找到符合要求的匹配，而此时有另外三个机器人广播了A的位置及可信度，这三个信息用(x，y，θ，r)表示分别为(100，200，30；0.7)，(120，190，35，0.9)和(90，210，25，0.6)。则B选取可信度最高的第二个信息，若此可信度0.9高于阈值，则用第二个信息更新所有采样点：提高与位置(120，190，35)相差较近的采样点的概率，降低与位置(120，190，35)相差较远的采样点的概率；此外，在(120，190，35)附近随机生成若干个候选点。

此外，如果机器人之间无法精确识别，即机器人并不知道周围的机器人具体是哪一个，则使用广播位置信息的方法存在局限。为解决之一问题，使协作定位的方法应用到各种系统中，不论机器人之间能否精确识别彼此，本发明引入了动态参照物的概念。所谓动态参照物，就是在某一时刻，有多于一个机器人可以看见的能够被视觉模块准确识别的物体。当机器人之间无法精确识别彼此时，他们可以选取周围的动态参照物，并根据自己的位置置信度给所识别动态参照物位置设置参照物位置置信度，并伴随自己识别的此物体的位置信息一同广播给其他机器人。如果在同一时刻，另一个机器人需要通过协作来完成定位，则可根据自己判断的与参照物的距离和夹角，并通过广播得到的此刻参照为的最高置信度的位置，计算出当前自身所在位置。如此就解决了机器人之间不能精确识别的问题。本发明即可用于更为广泛的系统，对机器人之间的精确识别也不再有限制。

图6为使用本发明后机器人实际定位情况示意。其中图6a)表示当前机器人在场地上的实际位置(800，700)和朝向角度180°。图6b)表示使用概率方法时，采样点的初始随机分布，其中箭头表示采样点的位置和朝向角度。图6c)表示使用国际最新概率定位方法(Landmark-based Monte Carlo Localization)在10秒后，机器人感知自身位置的采样点分布。图6d)为使用本发明后，在相同的10秒后，机器人感知自身位置的采样点分布。从图中可以明显地比较出本发明用于自主机器人定位的效果和执行效率。为了强调实际环境的未知影响，在图6c)和6d)执行过程中，随机手动改变机器人的位置然后回到原始位置，从而模拟现实条件下出现冲撞和其他未知因素对自主定位的影响。

尽管为说明目的公开了本发明的具体实施例和附图，其目的在于帮助理解本发明的内容并据以实施，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换、变化和修改都是可能的。因此，本发明不应局限于最佳实施例和附图所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (3)

1.一种在复杂环境下多个机器人协作定位的方法，具体包括以下步骤：

1)在机器人工作区域内，随机生成若干个采样点，表征机器人当前的位置及该位置的概率；

2)如果机器人发生位置移动，则根据运动模型更新所有采样点；

3)如果有传感器得到地标信息，则根据此信息更新各个采样点的概率，并增加候选点；

4)如果在一段时间内，都没有得到地标信息，则调用经验数据库，进行图像匹配，如果在经验数据库中找到匹配程度符合要求的信息时，根据此经验信息更新采样点的概率，并增加候选点；

5)如果机器人发现自己一段时间内既没有得到地标信息，又没有在经验数据库里找到符合要求的匹配，则跟其它机器人进行通讯，把本机器人和至少一个其他机器人同时看到的物体作为动态参照物，读取并记录其他机器人发送的该动态参照物的位置最高置信度，本机器人根据自己当前与动态参照物的夹角和距离，进行自定位；

6)删除采样点中概率值最低的若干个点，并从候选点中选取相同个数的点加入到采样点中，删除所有候选点；

7)找出概率和最大的区域，计算此区域内的采样点的位置的加权平均值，所得到的位置作为机器人当前最佳位置；

8)反复执行步骤2)至7)，即可实现多个机器人协作实时定位。

2.如权利要求1所述的在复杂环境下多个机器人协作定位的方法，其特征在于：所述动态参照物的位置最高置信度，是由能够进行自定位的机器人，根据自己的当前位置，结合与动态参照物的夹角和距离确定的。

3.如权利要求1或2所述的在复杂环境下多个机器人协作定位的方法，其特征在于：机器人相互通信采用UDP无线广播的方式，或者TCP/IP协议方式。

Images