[发明专利]一种面向家庭服务机器人的联合定位系统及方法

说明书

技术领域

本发明涉及一种面向家庭服务机器人的联合定位系统及方法。

背景技术

伴随着人们对精确无线定位的迫切需求，其技术发展越来越迅猛，应用领域也变的更加广阔，例如：火场中被救人员的精确定位，物流仓库中货物的定位，家庭环境中服务机器人的定位等。目前实际工程中应用广泛的无线定位方法主要有：蓝牙无线定位方法、Wi-Fi无线定位方法、ZigBee无线定位方法等等。随着无线通信技术的发展，可用的频谱资源日渐匮乏，因此，迫切需要提出一种与其他通信频谱互不干扰的通信技术，超宽带(Ultra Wide Band,UWB)定位技术应运而生。

机器人自定位的精准性作为服务机器人重要的性能指标之一，是机器人完成运动控制和其它任务功能模块的基础。而传统的无线定位方法存在着控制精度不高、控制算法过程复杂和计算量大等各种不足，如Fang算法利用已知节点与未知节点之间的TDOA值建立双曲线方程，然后对方程进行线性化近似求得未知节点的位置估计，其不能通过增加参考点数目提高定位精度，只能使用固定的TDOA测量值进行定位等。

因此研究基于超宽带的定位技术具有重大的实际意义。

发明内容

本发明为了解决上述问题，提出了一种面向家庭服务机器人的联合定位系统及方法，本发明能够实现机器人在室内环境下的绝对定位。

为了实现上述目的，本发明采用如下技术方案：

一种面向家庭服务机器人的联合定位系统，包括定位节点系统和机器人系统，其中：

所述定位节点系统包括一个标签节点和多个锚节点，所述标签节点设置于机器人本体上，跟随机器人本体运动，所述锚节点记录锚节点到标签节点的TOA值，并将该数据回传给标签节点；

所述机器人系统，包括设于机器人本体的上层处理系统和下层控制系统，所述上层处理系统包括处理器，被配置为采用Fang算法和泰勒级数展开法进行联合定位，确定标签节点与各个锚节点的位置，以实现对于机器人的定位，并控制机器人本体进行相应的运动；

所述下层控制系统，包括无线通信模块和控制器，控制器通过无线通信模块与上层处理系统通信，接收上层处理系统下达的控制指令，控制机器人本体执行相应的运动操作。

进一步的，所述定位节点系统包括一个标签节点和至少三个锚节点，锚节点在分别固定在坐标确定的某个位置。

进一步的，所述标签节点与锚节点之间用的协议是IEEE超宽带协议。

进一步的，所述下层控制系统包括主控制器，所述主控制器连接有电源模块、蓝牙模块、存储模块和超宽带芯片。

进一步的，所述主控制器与超宽带芯片之间采用SPI协议。

进一步的，所述主控制器与显示屏连接，显示调试信息，且与显示屏采用的是I²E协议。

进一步的，所述机器人本体外表面设置避障探测器。

进一步的，所述机器人本体下端设置有若干个跌落探测器。

基于上述系统的工作方法，在机器人本体上设置标签节点，跟随机器人本体运动，在机器人本体的活动区域内设置多个位于不同位置的锚节点，记录锚节点到标签节点的TOA值，并将该数据回传给标签节点；接收标签节点采集的各个TOA值，采用Fang算法和泰勒级数展开法进行联合定位，确定标签节点与各个锚节点的位置，以实现对于机器人的定位，并控制机器人本体进行相应的运动。

进一步的，联合定位的具体步骤包括：对得到的TOA值进行求差，得到一组TDOA，对获得的每一组TDOA数据信号直波鉴定，如果在数据中不存在NLOS情况，对该组TDOA数据进行Fang算法估计，否则对该组TDOA数据进行Fang算法估计，确定初始值，运用泰勒级数展开法对初始值进行迭代，确定目标节点位置估计。

进一步的，上层处理系统被配置有蓝牙串口数据包的接收线程、TDOA定位算法线程和节点配置线程，当定位系统开始运行时，上层处理系统接收上传的数据，当数据包接收完成并且校验正确后对数据包进行解析，之后调用TDOA定位算法函数进行定位计算。

进一步对，上电后系统首先进行初始化，检查系统存储历史数据标志位是否置位，如果置位，则直接将存储的数据通过无线的方式发送给上位机，发送完毕以后，开启测距算法；如果存储历史数据标志位没有被置位，直接开启测距算法，在测距成功以后，底层会对数据进行整理打包，然后约定的协议通过无线的方式发给上位机，如果发送成功，则直接进行倒计时准备下一次测距算法的开启；如果不成功，则将数据保存在外部存储器，等待网络恢复后再上传数据。