机器人定位方法 |
|||||||
| 申请号 | CN201610195047.7 | 申请日 | 2016-03-31 | 公开(公告)号 | CN105911518A | 公开(公告)日 | 2016-08-31 |
| 申请人 | 山东大学; | 发明人 | 路飞; 田国会; 刘国良; 王宇恒; | ||||
| 摘要 | 本公开了一种 机器人 定位 方法，待定位的机器人基于WiFi接入无线局域网，包括以下步骤：基于WiFi指纹定位确定机器人当前所在的估计 位置 ；调用所述估计位置的地图并在该地图约束的估计位置撒点，采用粒子滤波 算法 得到机器人的精确位置。依据本 发明 的机器人定位方法定位 精度 较高，且所需要的 迭代 时间较少。 | ||||||
| 权利要求 | 1.一种机器人定位方法，待定位的机器人基于WiFi接入无线局域网，其特征在于，包括以下步骤： |
||||||
| 说明书全文 | 机器人定位方法技术领域[0001] 本发明涉及一种机器人定位方法。 背景技术[0002] 随着科学技术的发展以及人们生活水平的提高，例如服务机器人已经逐渐进入人们的生活，为人类提供各种人性化服务。相对于其他类型的机器人，服务机器人的工作环境比较复杂，因此在复杂环境中的自主与智能定位是服务机器人为人类提供高质量服务的前提。 [0003] 目前服务机器人定位问题主要分为两种：位置跟踪与全局定位。其中，全局定位是指机器人在初始位姿没有任何先验知识的情况下，通过传感器感知信息确定位姿。全局定位主要用来解决机器人位置初始化、机器人绑架以及机器人移动过程中的迷途与位置校准等问题。 [0004] 常用的服务机器人全局定位方法包括马尔可夫定位、卡尔曼滤波和基于蒙特卡洛算法的粒子滤波方法等。基于蒙特卡洛算法的粒子滤波算法能够很好地处理非线性非高斯问题，因此该算法被广泛应用于服务机器人定位和地图构建问题中。 [0005] 文献“面向拥挤环境的移动机器人改进粒子滤波定位”(《机器人》,2012,34(5):596-603，王勇，陈卫东，王景川，等)提出了一种基于可定位性的改进粒子滤波定位算法来解决拥挤环境下的移动机器人定位问题。该方法通过判断激光器数据和里程计数据的可信度，来修正它们在粒子定位中的比重，有效地解决了移动机器人在拥挤环境中的定位问题。 [0006] 文献“平方根容积Rao-Blackwillised粒子滤波SLAM算法”(《自动化学报》，2014，40(2)：357-367.宋宇，李庆玲，康轶非，等)提出了一种平方根容积FastSLAM(Simultaneous Localization and Mapping)算法，有效地提高了粒子滤波SLAM算法的精度和性能。 [0007] 文献“利于kinect传感器和测距进行移动机器人定位”(Ganganath N,Leung H.Mobile robot localization using odometry and kinect sensor[C]//IEEE International Conference on Emerging Signal Processing Applications.Piscataway,USA:IEEE,2012:91-94.)提出一种基于粒子滤波器和扩展卡尔曼滤波器的低成本定位系统。该系统在预测阶段使用里程计信息估计位姿，而在更新阶段利用Kinect和人工地标校准机器人位姿。 [0008] 由于家庭和办公环境具有较高的复杂性和动态性，在此类环境下全局粒子滤波定位算法存在以下问题：1)由于地图较大，所需粒子数多，全局粒子撒点计算时间较长；2)当采样粒子接近机器人真实位置时才能保证粒子滤波定位算法的成功率，但在地图过大且粒子数目有限的情况下，采样粒子无法覆盖机器人真实位置，造成定位精度的下降；3)在结构相似性强的环境中，全局定位成功率不高。 [0009] 针对此问题，文献“基于单目视觉的移动机器人全局定位”(机器人，2007，29(2)：140-144,178.厉茂海，洪炳镕，罗荣华，等)利用单目视觉创建环境地图并通过特征匹配实现全局定位；但图像处理较为复杂，单目视觉系统增加了机器人的成本和计算负担。文献“基于粒子滤波器的移动机器人定位和地图创建研究进展”(机器人，2007，29(3)：281-289, 297.余洪山，王耀南)介绍了一些使用粒子滤波器进行机器人地图创建与定位的方法，但在复杂环境下由于场景结构的相似性，单一的粒子滤波定位在机器人位置无任何先验知识的条件下容易造成定位失败。 发明内容[0010] 本发明的目的在于提供一种在获得较高定位精度的条件下，大大缩减计算迭代时间的机器人定位方法。 [0011] 本发明采用以下技术方案： [0012] 一种机器人定位方法，待定位的机器人基于WiFi接入无线局域网，包括以下步骤： [0013] 基于WiFi指纹定位确定机器人当前所在的估计位置； [0014] 调用所述估计位置的地图并在该地图约束的估计位置撒点，采用粒子滤波算法得到机器人的精确位置。 [0015] 上述机器人定位方法，可选地，所述估计位置为机器人估计点位和估计点位误差所约束。 [0016] 可选地，所述粒子滤波算法为基于蒙特卡洛算法的粒子滤波算法。 [0017] 可选地，1)所述粒子滤波算法以前一时刻基于粒子分布所采样的用于表征机器人状态的离散数据及机器人在相应机器人状态时的概率作为先验概率密度分布的采样集； [0018] 2)更新：以机器人所搭载激光测距传感器的信息修正所述采样集，用以更新采样集中样本的权重值，得到估计位置的后验数据，进而增加权重较大的粒子的数目，减少权重较小的粒子的数目，产生在估计位置的粒子的重新分布，而产生新的采样集； [0019] 迭代预定次数的步骤2)，输出权重最大的粒子所适配的机器人位置估计作为机器人位置的精确值； [0020] 其中，所述地图为匹配所述激光测距传感器的激光地图。 [0021] 可选地，基于机器人地图和里程计信息所匹配的运动学模型产生采样集中的样本，该样本即所述离散数据和相应的概率。 [0022] 可选地，更新步骤中的增加权重较大的粒子的数目，减少权重较小的粒子的数目的方法是： [0023] 基于样本的当前权重随机出预定数目的样本。 [0024] 可选地于，在更新步骤中，在更新采样集中样本的权重值前，对样本的权重进行归一化处理。 [0025] 可选地，基于激光测距传感器更新采样集中样本的权重值是以在采样集条件下基于激光测距传感器采样得到的机器人状态的发生概率。 [0026] 可选地，WiFi指纹定位为基于BP神经网络的WiFi指纹定位。 [0027] 可选地，支持BP神经网络的AP点在室内的分布包括对室内机器人移动区域热点的中心分布和在移动区域外围的边缘分布； [0028] 其中，中心分布是在一个矩形空间内的一个对角线上的若干AP阵列，边缘分布则是在与AP阵列相交叉的方向上相异的两端的各一个AP。 [0029] 依据本发明，在没有任何初始位姿先验知识的情况下，通过WiFi进行机器人的粗定位，在粗定位确定的机器人所处的空间内，再利用粒子滤波算法进行机器人的精确定位。由于相对于全局定位，粗定位所约束的空间相对较小，需要的粒子数相对较少，减少了计算量，且基于粒子滤波的算法迭代的次数也会减少，缩短了迭代时间，或者在迭代次数相同的情况下，定位精度相对较高。 附图说明 [0030] 图1为服务机器人全局定位系统原理框图。 [0031] 图2为服务机器人服务描述结构图。 [0032] 图3为实验室中AP点的分布图。 [0033] 图4为神经网络训练误差曲线图。 [0034] 图5(a)为已知的GLOBAL-PF全局定位采样集分布(实际采样图的灰度图)。 [0035] 图5(b)为依据本发明的WIFI-PF全局定位采样集分布(实际采样图的灰度图)。 [0036] 图5(c)为已知的GLOBAL-PF全局定位结果(实际采样图的灰度图)。 [0037] 图5(d)为依据本法名的WIFI-PF全局定位结果(实际采样图的灰度图)。 [0038] 图6为误差随迭代次数变化曲线：图6(a)为已知的GLOBAL-PF误差曲线；图6(b)为依据本发明的WIFI-PF误差曲线。 [0039] 图7为已知的GLOBAL-PF全局定位、WiFi定位与依据本发明的WiFi-PF全局定位的平均误差对比图。 [0040] 图8为已知的GLOBAL-PF全局定位与依据本发明的WiFi-PF全局定位迭代时间对比图。 [0041] 图中：1.实验室，2.AP，3.实际位置，4.估计位置。 具体实施方式[0042] 以智能空间下基于WiFi(Wireless Fidelity，是基于IEEE 802.11b标准的无线局域网)指纹定位与粒子滤波的服务机器人复合全局定位方法为例详述本发明的原理，使其目的、原理、手段更加清晰。 [0043] 如图1所示，全局定位系统由WiFi信息采集模块、智能空间调度系统和服务机器人本体3部分组成。系统工作流程描述为：首先通过UPnP(Universal Plug and Play，通用即插即用)基于WiFi接入无线局域网的服务机器人向智能空间调度系统发送请求消息，智能空间调度系统接到请求后，响应该请求，向服务机器人上的WiFi采集模块发送控制信息，从而，服务机器人上的WiFi采集模块将扫描到的AP(Wireless Access Point，无线访问接入点)的RSSI(Received Signal Strength Indication，接收的信号强度指示)信号强度通过TCP协议发送给神经网络模块，从而得到机器人估计位置，智能空间调度系统调用服务机器人的roscontrol服务中的SetPose方法，向amcl节点发送initialpose消息设定初始估计点，之后进行粒子滤波定位得到机器人精确位置。 [0044] 在一些实现中，单独的WiFi指纹定位即可获得较高的定位精度，然而，其所依赖的是成本比较高的路由器，而在家庭环境中，普遍使用成本较低，精度较差，且容易受干扰、信号强度不稳定的普通路由器，因此，服务机器人在室内的定位，尤其是家用服务机器人的定位精度在纯粹的依赖家用普通路由器的条件下是比较低的。 [0046] 在一些实施例中，WiFi采集系统由3个模块构成：CC3000模块，STM32控制器和RM04模块。CC3000模块扫描智能空间中的AP点并得到其信号强度，将信号强度数据通过SPI总线传输给STM32控制器。STM32控制器负责CC3000模块与RM04模块的初始化，从CC3000模块中接收强度信息，并将强度信息通过串口发送到RM04模块。RM04为串口转WiFi模块，由该WiFi模块负责接入智能空间WiFi网络中，将从串口接收到的AP强度数据发送给神经网络模块，从而实现WiFi指纹定位。 [0047] 关于智能空间调度系统： [0048] 智能空间调度系统主要进行WiFi指纹定位以及ROS(Route Operation System，软件路由)服务机器人的消息获取和服务执行。其中神经网络模块通过智能空间WiFi数据库中的数据，采用BP神经网络进行训练，然后对WiFi采集模块采集到的数据进行位置估计，并将估计结果发送给机器人调度模块。机器人调度模块主要功能为发现智能空间网络中的ROS机器人并取到相应设备描述，接收ROS机器人定位请求消息，调用神经网络模块进行机器人位置定位，之后调用roscontrol服务中的SetPose动作向amcl节点发送initialpose消息。 [0049] 公知地，WiFi指纹定位的原理基于以下四个条件： [0050] 1.在于每一个无线AP都有一个全球唯一的MAC(Media Access Control，媒体介入控制层，属于OSI中的数据链路层)地址，并且一般来说无线AP在一段时间内是不会移动的。 [0051] 2.设备在开启WiFi的情况下，即可扫描并收集周围的AP信号，无论是否加密，是否已连接，甚至信号强度不足以显示在无线信号列表中，都可以获取到AP广播出来的MAC地址。 [0052] 3.设备将这些能够标示AP的数据发送到位置服务器，服务器检索出每一个AP的地理位置，并结合每个信号的强弱程度，计算出设备的地理位置并返回到用户设备。 [0053] 4.位置服务商要不断更新、补充自己的数据库,以保证数据的准确性，毕竟无线AP不像基站塔那样基本100％不会移动。 [0054] 由此，当智能空间中存在多个AP时，多路采样得到的WiFi信号强度具备匹配相应位置的特征，从而具有定位的技术基础。 [0055] 关于服务机器人本体： [0056] 依据本发明的实施例中，使用基于ROS平台的家庭轮式服务机器人，ROS是一个分布式模块化的机器人软件平台，它拥有大量软件库以及相应工具帮助人们开发机器人应用。ROS机器人平台具有较好的可扩展性和分布性，近几年得到广泛关注。如图1所示，该服务机器人定位系统主要包含odometery、map_server、rplidar、ROSUPnP和amcl节点。 [0057] 其中，ROSUPnP节点为机器人设备抽象节点，它将服务机器人提供的服务抽象成统一的用户接口，并利用libupnp SDK开发库将智能服务机器人改造成UPnP设备。智能空间通过该节点可调用ROS机器人的相应服务，实现智能空间与ROS机器人之间的松耦合以及零配置，提高系统的扩展性与兼容性。当ROS机器人接入智能空间网络时，该节点将ROS机器人设备描述文档和服务描述文档发布到网络中，从而使智能空间调度模块发现相应的机器人设备，智能空间调度模块只需调用服务描述文件中的统一接口而不需要理解服务的具体实现。 [0058] 本实施例中，ROS机器人提供的服务为全局定位服务(roscontrol)，该服务相应的服务结构描述文档如图2所示，其中，在roscontrol服务中，接口以动作的形式提供，该服务提供SetPose的动作。SetPose动作包含wx、wy和mea三个输入参数，其中wx和wy为机器人位置的WIFI定位估计坐标，mea为相应误差范围。这些输入参数与机器人内部的x、y和mea状态变量相关联，x和y表示ROS机器人中initialpose消息的初始点状态变量，mea表示机器人中initialpose消息的粒子撒点范围的状态变量。 [0059] odometery、map_server、rplidar和amcl为ROS平台中导航包中的节点。其中，odometery为里程计节点，它将ROS机器人的里程计信息发送给amcl定位节点。rplidar为激光测距传感器节点，负责发布激光数据。map_server为机器人环境地图服务，主要为amcl定位节点提供相应地图。amcl为定位节点，它通过里程计数据、激光器数据以及地图信息进行粒子滤波定位。 [0060] 关于服务机器人基于粒子撒点及相应的地图信息所进行的定位，可参见“基于粒子滤波器的移动机器人定位和地图创建研究进展”(《机器人》，2007,29(3):281-289.)。“基于ROS的机器人即时定位与地图构建技术的研究”(《东北大学》，2013，夏汉均)。 [0061] 基于WIFI指纹与粒子滤波的服务机器人全局定位系统算法主要由两部分组成。优选地，首先，利用BP神经网络对机器人位置进行粗定位；其次在粗定位的基础上进行粒子撒点，根据激光器扫描数据与局部地图数据得到机器人的精确位置。 [0062] 关于基于BP神经网络的WIFI指纹定位： [0063] 无线网络定位方法主要有TOA(time of arrival)方法、RSSI方法、位置指纹方法等。其中，TOA主要根据参考节点与未知节点之间的信号到达时间计算出未知节点的位置。 [0064] RSSI方法是在未知节点中获取到已知位置的节点的信号强度，然后根据无线信号随距离衰减模型，求出待测节点与已知节点的距离，最后根据距离求出未知节点的位置。 [0065] 位置指纹方法分为两个阶段：离线阶段和定位阶段。在离线阶段，首先从环境中选取一些位置作为采样点，然后在采样点中采集环境中多个AP点的强度信息作为该位置的指纹，并利用这些指纹建立指纹库。在定位阶段，未知节点首先测量环境中的AP强度指纹，并将该点AP强度指纹与指纹库匹配，从而得到未知节点的位置。 [0066] TOA方法尽管具有较好的定位精度，但需要参考节点与未知节点之间的严格时间同步，同时要求无线信号具有较高的穿透力，所以需要专业的定位设备，造价较高。由于室内环境复杂同时存在大量的障碍物，无线信号随距离衰减模型往往很难获得，所以RSSI方法不太适合于复杂的室内环境。尽管位置指纹方法定位精度不如TOA方法高，但是技术实现和成本要求较低，不需要设计专门的定位设备，可以通过纯软件实现，所以本文选用指纹定位方法进行服务机器人的粗定位。 [0067] 指纹定位的离线学习和训练可以通过神经网络有效实现。BP神经网络是神经网络中比较常用的一种，主要由输入层、隐含层和输出层3层组成。首先，输入信号经过输入层与隐含层的处理，在输出层得到相应的估计值，比较估计值与实际值得到输出误差，将该误差反向传播至隐含层所有单元，从而得到各个单元的误差信号并根据该误差调节神经网络中的各个权值，然后重复该过程直到输出误差小于期望值。 [0068] 与传统加权K邻近和最近邻指纹定位算法相比，基于BP神经网络的指纹定位算法具有较好的非线性映射能力，可以有效地逼近智能空间中的WIFI强度模型。 [0069] 另外，BP神经网络具有较好的自学习能力，可有效地更新智能空间中的WIFI定位模型。同时，BP神经网络还具有较好的容错性，当智能空间中一些AP点出现故障导致WIFI强度信息不全时，BP神经网络仍可以进行定位。 [0070] 基于BP神经网络的指纹定位方法的具体训练步骤如下： [0071] Step1：对AP信号强度以及位置坐标进行归一化，得到神经网络输入向量x＝(x1,x2,...,xn)，n为AP点的个数，期望输出向量do＝(d1,d2,...,dq)，q为输出层的个数。 [0072] Step2：选取第k组AP点的强度数据x(k)＝(x1(k),x2(k),...,xn(k))，以及相应坐标do(k)＝(d1(k),d2(k),...,dq(k))，计算神经网络各层的输入和输出。首先，利用式(1)计算出隐含层输入向量hih(k)，其中wih为输入层与隐含层的连接权重矩阵，bh为隐含层各神经元的阈值向量，p为隐含层个数。然后利用式(2)得到隐含层输出向量hho(k)，其中f()为激活io o函数。同理可得，输出层输入向量y (k)和输出层输出向量y(k)。 [0073] hih(k)＝wihxT(k)-bh (1) [0074] hho(k)＝f(hih(k)) (2) [0075] Step3：利用BP神经网络的估计位置和机器人实际位置计算出误差函数(式(3))，并利用式(4)计算输出层各神经元的偏导数 j＝1,2,…,q，按照式(5)求出误差函数对隐含层各神经元偏导数 j＝1,2,…,p。 [0076] [0077] [0078] [0079] Step4：利用式(6)和(7)修正神经网络隐含层和输出层之间的连接权重who以及输入层和隐含层之间的连接权重wih。 [0080] [0081] [0082] Step5：计算机器人位置的全局误差，并判断全局误差是否满足要求，如果满足则训练结束，否则返回Step2。 [0083] 为了验证上述方法的定位效果，在山东大学服务机器人实验室中，搭建5个AP点，如图4所示。每隔1.8m选取20个采样点，每个采样点采样20组数据，利用400组AP点强度信息构建实验室WiFi指纹数据库。使用BP神经网络对这些AP强度数据进行训练。该BP网络具有5个输入层节点、7个隐含层节点、2个输出层节点。隐含层激活函数为sigmoid函数(如式(8))，输出层激活函数为线性函数。训练过程中神经网络误差下降曲线如图5所示。最后利用训练好的BP神经网络进行机器人位置的粗定位。 [0084] [0085] 关于基于蒙特卡洛算法的粒子滤波定位： [0086] 上述基于神经网络的WiFi指纹定位算法精度约1.5m～3m左右，所以单一的WiFi指纹定位不能满足服务机器人的定位需求，需要在WiFi指纹定位的基础上，利用机器人地图信息、里程计信息和激光器信息进行机器人精确定位。设ot为t时刻的机器人传感器测量数据，at为t时刻的里程计信息数据，gt为t时刻的激光测距传感器数据，st＝(xt,yt,θt)表示当前机器人状态，p(st|ot)表示状态估计的后验概率密度分布。Ct＝{(st(i),wt(i))|i＝1,2,...,n}表示t时刻离散采样样本集，其中st(i)为机器人在t时刻的可能状态，wt(i)为机器人(i) 状态为st 的概率。该定位过程主要分为两个阶段： [0087] (1)预测阶段 [0088] 设在t-1时刻得到WIFI指纹粗定位坐标以及误差范围，利用该坐标与误差范围初始化t-1时刻的样本集Ct-1。根据运动学模型 对样本集Ct-1进行采样得到样本所有采样得到的样本集为 用 样本集近似表示先验概率密度分布p(st|ot-1)的采样集(如式(9))。 [0089] [0090] (2)更新阶段 [0091] 设t时刻服务机器人的获得的激光器信息gt，利用式(10)更新样本集 中的权重值 并利用式(11)对权重进行归一化。用更新后的 样本集近似表示后验概率密度分布p(st|ot-1)的采样集。根据 对样本集 进行重采样得到样本Ct。重采样过程主要是为了尽量增加权重较大粒子的数目。首先通过产生一个随机数r，r∈[0,1]，如果整数m满足式(12)，则将st(m)作为Ct的一个样本。重复此过程直到得到n个样本。 [0092] [0093] [0094] [0095] 在WiFi粗定位的范围中，不断迭代调用上述步骤，使服务机器人的样本集Ct不断更新，并将最终得到的样本集中权重最大的机器人位置估计作为服务机器人位置的估计值。 [0096] 仿真实验及结果分析(Simulation experiments and results analysis)： [0097] 本实施例所配置的实验环境为山东大学服务机器人实验室如图5所示，机器人平台为基于ROS平台的家庭轮式服务机器人，激光传感器为RoboPeak团队开发的低成本360°2维激光雷达RPLIDAR A1M1，机器人环境地图分辨率大小为354×674，精度为0.025。在该实验环境中选取多个随机点进行机器人全局定位。 [0098] 图5为其中一组定位结果对比。图5(a)为未进行WiFi定位的全局定位初始样本集分布情况，左中部黑色圆点为初始随机位置。图5(b)为进行WiFi定位后的样本集分布情况，左中部黑色圆点为WiFi指纹定位估计点。图5(c)为未进行WiFi定位的全局定位(GLOBAL-PF全局定位)迭代50次之后的最终样本分布，标号3表示实际位置，标号4表示估计位置；图5(d)为进行WiFi初始定位的全局定位(WiFi-PF全局定位)迭代50次之后的最终样本分布。它们的误差随迭代次数变化曲线如图6所示。 [0099] 图6(a)为已知的GLOBAL-PF全局定位的误差随迭代次数变化曲线，可以看出当误差达到2m左右后不再下降。图6(b)为依据本发明的WiFi-PF全局定位误差随迭代次数的变化曲线，可以看出误差最终小于0.5m。图7为GLOBAL-PF全局定位、WiFi-PF全局定位和WiFi指纹定位的平均误差对比。由图7和图8可以看出，WiFi-PF全局定位的定位误差最小，在0.5m左右，可以满足服务机器人定位精度要求。图8为GLOBAL-PF全局定位和WiFi-PF全局定位迭代50次所用的时间对比，说明WiFi指纹定位的引入有效缩短了服务机器人的迭代时间。本实验通过WiFi指纹定位、GLOBAL-PF全局定位和WIFI-PF全局定位之间对比说明，依据本发明实施例的系统可有效提高服务机器人定位精度和缩短迭代时间。 [0100] 综上所述： [0101] 依据本发明较佳的实施例，针对机器人在没有任何初始位姿先验知识的情况下通过传感器感知信息确定位姿的全局定位问题，结合智能空间WiFi指纹定位和蒙特卡洛算法的粒子滤波方法，设计了一种智能空间与ROS服务机器人相结合的复合全局定位系统。该全局定位系统首先在智能空间中进行基于BP神经网络的WIFI指纹粗定位并将估计位置与估计误差发送给ROS服务机器人，机器人结合激光器信息、里程计和地图信息利用基于蒙特卡洛算法的粒子滤波方法最终获得服务机器人的精确位置。 [0102] 该全局定位系统实现了ROS机器人与智能空间之间的零配置与松耦合，ROS服务机器人可动态加入或离开智能空间，不需要人工干预并实现ROS服务机器人在智能空间下的精确全局定位。在实际环境中的实验说明了基于WiFi指纹与粒子滤波的服务机器人全局定位系统可有效提高服务机器人定位精度和缩短迭代时间。为进一步改善WiFi指纹定位误差过大时的全局定位结果，可以利用指纹信息中的特征判断神经网络的结果是否正确，并利用粒子滤波的定位结果自适应校准神经网络，这将是申请人在本发明的基础下一步需要深入研究的工作。 |
||||||
QQ群二维码
意见反馈
意见反馈