CN105022394A - 动态环境下的移动机器人可靠定位方法 - Google Patents

Abstract

本发明利用无线传感器网络对移动机器人提供辅助数据，实现动态环境下的可靠定位，包括由ZigBee无线传感器节点组成的网络系统、移动机器人平台。无线传感器网络节点包括三种类型，即网关节点、静态节点和移动节点，无线传感器网络采用ZigBee协议。移动机器人为四轮式结构，由中央控制器、近距红外避障模块、惯性导航、环境感知装置及电源模块组成。避障模块包括激光雷达、超声波模块，用于避障；惯性导航包括加速度计和陀螺仪；环境感知装置包括温度、湿度、有害气体、光强、人体等传感器；电源模块用于为各模块供电。本发明利用无线传感器网络持续发送无线信号，与移动机器人搭载的无线传感器网络节点进行数据通信，两者之间的无线信号强度作为测距的主要依据。

Description

动态环境下的移动机器人可靠定位方法

技术领域

本发明涉及无线传感器网络和机器人控制等领域，具体涉及利用无线传感器网络对移动机器人进行可靠定位。

背景技术

作为具有自主能力的智能体，移动机器人在灾难救援和军事领域有着广泛的应用。为确定机器人的位置，通常装备GPS和惯导装置。但对于室内环境（比如大型建筑内），GPS将无法使用，目前通常依靠惯导装置为机器人提供位置数据。由于惯导装置存在时漂特性，其误差随着系统运行时间的延长而迅速增加，因此很难满足移动机器人定位的需求。采用高精度的惯导装置，可以在一定程度上改善时漂特性，但其成本过于高昂。利用无线传感器网络对移动机器人进行辅助定位和控制，成为解决这一问题的有效手段。

无线传感器网络（Wireless Sensor Network, WSN），作为一种新兴的先进技术，被评选为改变未来世界的十大新兴技术之一和可能掀起新产业浪潮的未来四大高新科技之一，在军事、工业和安全监控等领域得到了越来越广泛的应用。WSN具有密集分布的节点，能够提供灾难现场全局环境信息和变化趋势。将WSN应用到机器人系统当中，可以极大地扩展机器人的环境感知能力，同时WSN还能够作为机器人通讯和计算的媒介，延长机器人的通信距离并提高其受控效率。以动态部署的传感器网络对机器人进行辅助定位，可以有效解决机器人因长距离运动产生的误差累积而导致的机器人无法准确自定位的技术难点，增加移动机器人控制的准确性和可靠性。

目前的无线传感器网络移动机器人定位技术研究，通常设定较理想的环境条件，与实际情形存在较大偏差。主要体现在：通常考虑比较理想的环境，很少考虑机器人在行进过程中的无线信号受到阻碍等情况。然而，现实的监测环境常常存在很高的复杂性及不确定性，移动机器人与WSN节点之间的无线信号传播很容易受到障碍物阻挡，导致信号传播路径发生改变而形成非视距传播，使定位算法的性能急剧下降。当前的研究通常忽略这种现实情况，导致其研究与实际情况存在较大偏差。 

发明内容

针对移动机器人定位现有技术的不足，本发明提供了一种动态环境下的移动机器人可靠定位方法，可以有效解决机器人在动态环境下无法准确定位的技术难题。

本发明的技术方案是：事先在目标区域（主要是建筑内空间）部署无线传感器网络，实时监测动态环境变化，同时以无线通信方式为移动机器人提供辅助定位信号；移动机器人携带无线传感器网络节点在动态环境中进行信息采集和侦察，与无线传感器网络建立无线数据通信；通过分析无线信号在室内环境传播的特性，建立无线信号传播特征的离线数据库；考察环境扰动因素对无线信号传播的影响，鉴别机器人与传感器网络之间无线通信的视距与非视距状态，根据信号的堵塞情况进行误差补偿并实现精确测距；在运动过程中，机器人从多个无线传感器网络节点获得距离信息，由此形成当前的位置估计，结合机器人依托惯性导航设备获得的运动估计数据，经过信息融合实现准确、可靠的机器人定位。

所部署的无线传感器网络节点包括三种类型，即网关节点（1）、静态节点（2）和移动节点（3）。无线传感器网络采用ZigBee协议，网关节点通过串行口与远程控制中心相连接并进行数据交互。静态节点预先部署在目标环境中，作为监测网络对环境信息进行有效感知，同时实现移动机器人（4）通信与控制距离的有效延伸。移动节点由移动机器人携带，通过红外信号与机器人进行数据通信，负责在移动机器人和传感器网络之间建立数据联系，并作为传感器网络节点无线信号强度的检测单元。

移动机器人（4）为轮式结构，由中央控制器、近距红外避障模块、惯性导航、环境感知装置及电源模块组成。避障模块包括激光雷达、超声波模块，用于避障；惯性导航包括加速度计和陀螺仪；环境感知装置包括温度、湿度、有害气体、光强、人体等传感器；电源模块用于为各模块供电。

无线传感器网络持续发送无线信号，与移动机器人搭载的移动节点进行数据通信，两者之间的无线信号强度作为测距的主要依据。移动机器人获得三个以上节点的距离数据时，能够通过合适的三边定位算法估计自身的位置。

所述的一种动态环境下的移动机器人可靠定位方法，主要包括以下步骤：

步骤一：利用预先部署的无线传感器网络，在典型建筑空间内进行测试和统计，结合无线信道特性提取信号传播状态的特征值，考虑信号堵塞和多径干扰等因素所造成的信号强度不规则分布特征，形成离线信息数据库。

在建筑物内部由于环境较为狭小，人员、家具或设备的移动可能使无线信号RSSI数值受到了严重的影响。在走廊、大厅等较为开阔的环境下，人体、设备移动等因素的影响相对较弱。对这些典型的建筑空间进行测试与统计，所建立的离线信息数据库能够在较大程度上反映目标环境内不同位置的信号分布特征。

步骤二：模拟存在动态扰动时的场景，针对室内空间的物体移动、人员走动等情景，对传感器网络节点之间的RSSI(Received Signal Strength Indicator，接收信号强度指示)影响进行统计分析，形成概率密度函数。

由于动态扰动存在很大的不确定性，无法以确定的数据反映其对于信号传播的影响，采取概率密度函数是一种较为可取的方式。

步骤三：使移动机器人机载的传感器网络节点（移动节点）加入网络，建立机器人与传感器网络之间的通信联系，由移动节点从周边的传感器网络节点获得RSSI信号强度。

步骤四：当移动机器人附近的环境条件发生改变时，根据建筑物的空间构型确定相关联的节点，采用支持向量机和Bayes估计等算法对离线数据库进行动态调整，使其能够正确反映当前的环境扰动所造成的影响。

步骤五：在移动机器人的机载计算单元中，将实时获得的接收信号强度与离线数据库相结合，利用假设检验等方法确定当前信号是否受到非视距污染，结合机器人状态估计使用卡尔曼滤波等融合算法将假设检验的结果进行融合，以获得可靠的状态鉴别结果。

步骤六：当信号受到动态扰动的影响，存在非视距污染情况时，结合前面所建立的离线信息数据库和概率密度函数对非视距状态进行量化分级，按照不同的信号阻塞级别对非视距误差进行补偿，获得较为准确的非视距测距数据。

步骤七：结合移动机器人从周边节点获得的视距与非视距测距数据，形成移动机器人的位置估计数据源，根据各节点的视距或非视距级别赋予相应的权重，使用合适算法如高斯混合模型等计算并获得移动机器人的具体位置。由于根据环境因素和动态扰动的影响给予周边静态节点测距数据以不同的权值，所获得的机器人定位数据具有较高的可靠性。

步骤八：当无法获得周边WSN节点的数据时，移动机器人依靠惯导设备进行定位和状态估计；在获得周边节点的数据后，机器人控制器采用扩展卡尔曼滤波算法，将惯导数据与WSN数据相融合获得准确的机器人位置。

所述步骤一的预先部署WSN节点和环境测试与统计，包括如下步骤：

（1）在走廊、室内的顶部安置传感器网络节点，使其能够对所测试的空间形成完全覆盖；

（2）将目标空间按照一定的间隔划分为正方形网格，网格的顶点作为测试点；

（3）在每个测试点位置上，采用ZigBee频谱分析仪同时获得多个节点的信号强度数据；

（4）对无线信号强度数据进行分析，并提取其特征。

信号堵塞，指WSN节点之间存在障碍，造成信号不能直接传播至目的地，必须经过一个衰减的过程。

所述步骤二的RSSI，指无线传感器网络节点接收到邻居节点发出的信号后，经过硬件获得的信号强度（Received Signal Strength Indicator）；在不同的扰动影响下，RSSI会发生相应的变化，而且其数值并不稳定，因此需要确定概率密度函数。

所述步骤四的环境空间构型，指无线传感器网络所部署区域的物理形状和空间位置。由于WSN节点之间的信号强度根据其物理相对位置密切相关，当空间构型变化时将造成RSSI数值的相应变化。

所述步骤八的机器人状态估计，指移动机器人的运动方向、速度和姿态等参数，通常根据机器人所搭载的惯性导航装置推算获得。 

本发明的优点与积极效果是：

预先部署的无线传感器网络将构成移动机器人运动过程中的定位点，为机器人提供辅助定位信息。在复杂的动态环境中，通过判断机器人与传感器网络之间的信号传播状态，进行有效的误差补偿和精确测距，有助于消减机载惯性传感器的累积误差和位移数据误差，准确获取机器人及其搜索目标的实时位置数据。

附图说明

图1　本发明的系统示意图

图2　机器人结构示意图 

图3  本发明的数据交互架构示意图

图4  视距/非视距状态混合的场景示意图

图5  目标环境的平面示意图

图6  动态环境下移动机器人可靠定位方法的工作流程图。

具体实施方式

下面结合实例对本发明作进一步具体说明。

事先在目标区域（主要是建筑内空间）部署无线传感器网络节点，实时监测动态环境变化并提供无线数据通信。移动机器人携带无线传感器网络节点（移动节点）与无线传感器网络建立数据通信，在动态环境进行信息采集和侦察。通过分析无线信号在室内环境传播的特性，建立无线信号传播特征的离线数据库。考察环境扰动因素对无线信号传播的影响，鉴别机器人与无线传感器网络之间的视距与非视距状态，然后进行误差补偿并实现精确测距；在运动过程中，机器人从多个无线传感器网络节点获得距离信息，由此形成当前的位置估计，结合机器人自身的运动估计数据，经过信息融合使机器人定位更加准确，信息更为有效。

在部署的网络系统中，无线传感器网络节点包括三种类型，即网关节点、静态节点和移动节点，如附图1所示。无线传感器网络采用ZigBee协议，网关节点通过串行口与远程控制平台相连接并进行数据交互。静态节点预先部署至目标环境中，作为数据网络实现机器人通信与控制距离的有效延伸。移动WSN节点由移动机器人携带，通过红外信号与机器人进行数据通信，负责在机器人和传感器网络建立数据联系，并作为传感器网络节点无线信号强度的检测单元。

移动机器人为轮式结构，由中央控制器、近距红外避障模块、惯性导航、环境感知装置及电源模块组成。避障模块包括激光雷达、超声波模块，用于避障；惯性导航包括加速度计和陀螺仪；环境感知装置包括温度、湿度、有害气体、光强、人体等传感器；电源模块用于为各模块供电。

无线传感器网络持续发送无线信号，与移动机器人搭载的无线传感器网络节点进行数据通信。两者之间的无线信号强度作为测距的主要依据。

所述的一种动态环境下的移动机器人可靠定位方法，其具体实施主要包括以下步骤：

步骤一：利用预先部署的无线传感器网络，在典型建筑空间内进行测试和统计，结合无线信道特性提取信号传播状态的特征值，考虑信号堵塞和多径干扰等因素所造成的信号强度不规则分布特征，形成离线信息数据库。

在建筑物内部由于环境较为狭小，人员、家具或设备的移动可能使无线信号RSSI数值受到了严重的影响。在走廊、大厅等较为开阔的环境下，人体、设备移动等因素的影响相对较弱。对这些典型的建筑空间进行测试与统计，所建立的离线信息数据库能够在较大程度上反映目标环境内不同位置的无线信号分布特征。

所述的环境测试与统计，包括如下步骤：

（1）采用以CC2531为核心的ZigBee节点，无线通信采用ZStack-1.4.3-1.2.1协议栈，并搭载相应的温度、震动等传感器；

（2）在走廊、室内的顶部安置传感器网络节点，使其对所测试的空间形成完全覆盖；

（3）将目标空间按照一定的间隔划分为正方形网格，网格的顶点作为测试点；比如以0.5m为间隔，将房间内的地面划分为网格，将每个网格顶点作为测试点并赋予编号；

（4）在每个测试点位置上，采用ZigBee协议分析仪同时获得多个节点的信号强度数据；

（5）对信号强度数据进行分析，并提取其特征，主要指多个节点在该测试点的信号强度强弱以及强度数值波动的范围；

（6）分别在节点间人为加入木板、家具、桌子等障碍物，改变WSN网络与机器人之间的通信环境，通过协议分析仪观测无线通信的数据。

步骤二：模拟存在动态扰动时的场景，针对室内空间的物体移动、人员走动等情景，对传感器网络节点之间的RSSI(Received Signal Strength Indicator，接收信号强度指示)影响进行统计分析，形成概率密度函数。

由于动态扰动存在很大的不确定性，无法以确定的数据反映其对于信号传播的影响，采取概率密度函数是一种较为可取的方式。

RSSI指无线传感器网络节点接收到邻居节点发出的信号后，经过硬件获得的信号强度（Received Signal Strength Indicator）。当物体移动或人员走动时，无线传感器网络节点之间的信号传播将受到影响。由于这种移动和走动通常是不规则的，并不存在固定的规律，因而对于无线信号强度分布是一种扰动。在不同的扰动影响下，RSSI会发生相应的变化，而且其数值并不稳定，因此需要确定概率密度函数。

步骤三：使移动机器人机载的传感器网络节点（移动节点）加入网络，建立机器人与传感器网络之间的通信联系，由移动节点从周边的传感器网络节点获得RSSI信号强度。

本实验系统是基于北京博创机器人公司的旅行家2号机器人和成都无线龙公司的CC2531传感器节点构建的。旅行家2号机器人采用模块化的设计，具有高负载能力和高运动精度的直流伺服控制系统，其控制计算部分由便携式计算机来承担。便携式计算机搭载Ubuntu 12.04系统，移植了ROS系统（Robot operating system，机器人操作系统），可以很方便地实现机器人与控制中心的交互。 

无线传感器网络节点加入网络的流程如下：

（1）通过编写SAPI_Init()函数。初始化一个WSN节点为协调器设备，并且为自启动模式建立网络，其它的子节点配置成为路由器或终端节点；

（2）WSN节点设备自动启动以后，向任务层发送一个设备状态改变的消息，由任务层SAPI_StartConfirm()函数响应；当设备进行确认状态发生变化的时候，协调器开始允许设备进行绑定，而其他节点发送绑定请求；

（3）进行绑定入网设置，由zb_StartConfirm()函数读取设备的类型。如果为协调器，则允许设备进行绑定。如果为其他设备类型，则发送设备绑定请求。

移动机器人通过机载WSN节点与周边的无线传感器网络建立数据联系。机载WSN节点的具体工作流程为：

（1）初始化单片机，设置晶振频率和无线通信频率，配置各个IO口的输入和输出，配置ZigBee协议；

（2）接收周边WSN节点的信号，将CC2531无线芯片通信寄存器中的信息保存作为无线通信的接收信息；

（3）根据移动机器人机载WSN节点的状态，执行不同的内容：若节点处于无线接收状态，则不断循环读取发射/接收控制信号和无线通信接收的信息；若节点处于无线发射状态，则将要发送的信息写入无线通信寄存器，该寄存器在工作于发射模式时会将信息通过无线发射出去，发射完成后节点转为无线接收状态；

（4）通过上述步骤，建立起无线接收与发送的链路，并将周边节点的信息通过串口送给移动机器人控制器。

步骤四：当移动机器人附近的环境条件发生改变时，根据建筑物的空间构型确定相关联的节点，采用支持向量机和Bayes网络等算法对离线数据库进行动态调整，使其能够正确反映当前的环境扰动所造成的影响。

环境空间构型指无线传感器网络所部署区域的物理形状和空间位置。由于WSN节点之间的信号强度根据其物理相对位置密切相关，当空间构型变化时将造成RSSI数值的相应变化。一般地，按照就近原则选取移动机器人周边的关联节点。由于WSN网络节点在部署时已经获得具体的物理位置，可以根据移动机器人的当前位置和运动趋势，选择周边合适数量的节点参与定位。

步骤五：在移动机器人的机载计算单元中，将实时获得的接收信号强度与离线数据库相结合，利用假设检验等方法确定当前信号是否受到非视距污染，结合机器人状态估计使用卡尔曼滤波等融合算法将假设检验的结果进行融合，以获得可靠的状态鉴别结果。

步骤六：当信号受到动态扰动的影响，存在非视距污染情况时，结合前面所建立的离线信息数据库和概率密度函数对非视距状态进行量化分级，按照不同的信号阻塞级别对非视距误差进行补偿，获得较为准确的非视距测距数据。

信号堵塞，指WSN节点之间存在障碍或动态扰动，造成信号不能直接传播至目的地，必须经过一个衰减的过程。根据对目标环境所进行的测试和统计，可以建立不同扰动因素对于无线信号传播的影响，将非视距状态划分为不同级别（例如完全阻塞、中度阻塞、轻度阻塞等），将非视距误差的标准差与影响程度权值作为因子，按照无线信号的非视距分级进行精确的误差补偿。

优选地，本发明将非视距状态划分为5个级别，包括完全阻塞、中度阻塞、轻度阻塞、信号减弱、轻度减弱等，分别对应于金属门或厚墙、普通墙壁、薄墙、木门、玻璃等建筑物内常见的非视距情况。当判断移动机器人与周边节点的信号联系处于视距时，直接将RSSI信号强度转换为距离数据；当存在非视距污染时，则结合离线数据库进行判断和非视距程度量化，按照相应的级别进行非视距误差补偿，以获得准确的测距数据。

步骤七：结合移动机器人从周边节点获得的视距与非视距测距数据，形成移动机器人的位置估计数据源，根据各节点的视距或非视距级别赋予相应的权重，使用合适算法如高斯混合模型等计算并获得移动机器人的具体位置。由于根据环境因素和动态扰动的影响给予周边静态节点测距数据以不同的权值，所获得的机器人位置估计数据具有较高的可靠性。

结合3个以上节点对机器人的距离估计数据，计算移动机器人的具体位置。具体地，通过距离数据计算移动机器人位置的过程，可以采用常见的极大似然估计或最小二乘等方法。

步骤八：当无法获得周边WSN节点的数据时，移动机器人依靠惯导设备进行定位和状态估计；在获得周边节点的数据后，机器人控制器采用扩展卡尔曼滤波算法，将惯导数据与WSN数据相融合获得准确的机器人位置。 

Claims (1)

1.动态环境下的移动机器人可靠定位方法，依托无线传感器网络实现移动机器人在动态环境下的可靠定位，所述ZigBee无线传感器网络包括三种类型的节点，即网关节点、静态节点和移动节点；无线传感器网络采用ZigBee协议，网关节点通过串行口与远程控制平台相连接并进行数据交互；静态节点可以由旋翼无人机、机器人或救援人员携带并部署至未知的目标环境中，作为数据网络实现机器人通信与控制距离的有效延伸；移动节点由移动机器人携带，通过红外信号与机器人进行数据通信，负责在机器人和传感器网络建立数据联系，并作为传感器网络节点无线信号强度的检测单元；所述的动态环境下的移动机器人可靠定位方法，其特征在于：包括如下步骤：

步骤一：利用预先部署的无线传感器网络，在典型建筑空间内进行测试和统计，结合无线信道特性提取信号传播状态的特征值，考虑信号堵塞和多径干扰等因素所造成的信号强度不规则分布特征，形成离线信息数据库；

步骤二：模拟存在动态扰动时的场景，针对室内空间的物体移动、人员走动等情景，对传感器网络节点之间的RSSI(Received Signal Strength Indicator，接收信号强度指示)影响进行统计分析，形成概率密度函数；

步骤三：使移动机器人机载的传感器网络节点（移动节点）加入网络，建立机器人与传感器网络之间的通信联系，由移动节点从周边的传感器网络节点获得RSSI信号强度；

步骤四：当移动机器人附近的环境条件发生改变时，根据建筑物的空间构型确定相关联的节点，采用支持向量机和Bayes估计等算法对离线数据库进行动态调整，使其能够正确反映当前的环境扰动所造成的影响；

步骤五：在移动机器人的机载计算单元中，将实时获得的接收信号强度与离线数据库相结合，利用假设检验等方法确定当前信号是否受到非视距污染，结合机器人状态估计使用卡尔曼滤波等融合算法将假设检验的结果进行融合，以获得可靠的状态鉴别结果；

步骤六：当信号受到动态扰动的影响，存在非视距污染情况时，结合前面所建立的离线信息数据库和概率密度函数对非视距状态进行量化分级，按照不同的信号阻塞级别对非视距误差进行补偿，获得较为准确的非视距测距数据；

步骤七：结合移动机器人从周边节点获得的视距与非视距测距数据，形成移动机器人的位置估计数据源，根据各节点的视距或非视距级别赋予相应的权重，使用合适算法如高斯混合模型等计算并获得移动机器人的具体位置；

由于根据环境因素和动态扰动的影响给予周边静态节点测距数据以不同的权值，所获得的机器人定位数据具有较高的可靠性；

步骤八：当无法获得周边WSN节点的数据时，移动机器人依靠惯导设备进行定位和状态估计；在获得周边节点的数据后，机器人控制器采用扩展卡尔曼滤波算法，将惯导数据与WSN数据相融合获得准确的机器人位置。