一种基于分体超声结合射频的室内厘米级定位系统的制作方法
【技术领域】
[0001 ]本发明涉及所述机器人定位方法，特别涉及一种基于分体超声波和射频的室内定 位系统的技术领域。
【背景技术】
[0002] 室内定位是一个长久的研究课题，机器人在室内若想移动，需要解决三个问题。通 俗地讲，机器人需要解决1.我在哪里；2.我要去哪里；3.我要怎么去。解决这三个问题的核 心就在于环境地图的构建以及机器人的自定位。
[0003] 目前已有多种室内定位的方法：如基于多个wifi信号强度的目标定位方法，基于 多个蓝牙信号强度的目标定位方法，基于多个环境摄像头的地图构建及目标定位方法，基 于磁导线的目标定位方法。其中多个Wifi信号强度的目标定位方法和基于多个蓝牙信号强 度的目标定位方法，均法存在误差较大的缺陷，其定位精度的误差往往大于l〇m，完全无法 用于机器人的精确定位。基于多个环境摄像头的地图构建及目标定位方法优点在于能提供 定位误差精度小于1厘米，速度大于15帧的定位服务，但缺点在于需要架设多个摄像头，成 本较高，动辄超过十万元，并且只能对小于5立方米的环境进行定位。基于磁导线的目标定 位方法优点在于能够进行大范围铺设且成本可控，定位误差精度小于1厘米;缺点在于预先 铺设完成后，线路无法修改，当要改变机器人运动轨迹时需要重新铺设，相对耗时耗力。
【发明内容】
[0004] 本发明所要解决的技术问题是提供一种误差精度小的基于分体超声波和射频的 室内定位系统，为此，本发明采用以下技术方案：
[0005] -种基于分体超声结合射频的室内厘米级定位系统，所述定位系统分为两部分： 第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络;所述 第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声 波发射端、射频交互端;所述的第二部分的多传感器节点网络设有底层和顶层，底层为分体 超声接收端，顶层为射频交互端及定位服务仲裁端；
[0006] 所述系统的定位方法包括以下步骤：
[0007]步骤一:装设传感器节点:将底层分体超声接收端装设在天花板上，所述底层分体 超声接收端的数量大于等于四个；
[0008] 步骤二:组建多传感器节点网络:将所有底层分体超声接收端与顶层射频交互端 及定位服务仲裁端连接，使得顶层射频交互端及定位服务仲裁端与所有底层分体超声接收 端交互信息；
[0009] 步骤三:请求定位服务的移动机器人发送定位请求:机器人通过分体超声结合射 频模块的射频交互端发送给多传感器节点网络中的顶层射频交互端及定位服务仲裁端；
[0010] 步骤四：顶层射频交互端及定位服务仲裁端响应请求，开始定位:顶层射频交互端 通过射频发送给请求定位服务的移动机器人信号，进行时间同步；同时定位服务仲裁端发 送给所有底层为分体超声接收端开始计时，等待接收超声波信号；
[0011]步骤五:请求定位服务的移动机器人开始定位:机器人的射频交互端接收到顶层 射频交互端的时间同步信号，启动超声波发射端，发送超声波；
[0012] 步骤六:多传感器节点网络接收超声波信号并汇总至顶层为射频交互端及定位服 务仲裁端:多传感器节点网络的各个分体超声接收端分别接收到移动机器人的超声波发射 端发出的超声波，得到各个分体超声接收端相对与移动机器人的距离，并将数据发送至顶 层为射频交互端及定位服务仲裁端；；
[0013] 步骤七:顶层射频交互端及定位服务仲裁端计算目标位置:顶层接收到底层分体 超声接收端发送来的位置信息后，查找是否存在有效数据，不存在，则本次定位失败，若存 在，通过计算可得目标点位置的坐标；
[0014] 步骤八:请求定位服务的移动机器人获得位置信息：顶层射频交互端及定位服务 仲裁端计算出目标点位置的坐标后，通过射频模块发送给移动机器人，机器人获得自身的 定位信息。
[0015] 在采用上述技术方案的基础上，本发明还可采用以下进一步的技术方案：
[0016] 所述超声波发射端由超声波发射探头、信号放大器及微控制单元组成，所述射频 交互端由NRF24L01芯片及微控制单元组成。
[0017] 所述底层的分体超声接收端由超声波接收探头，信号放大器，信号滤波器，信号整 形器及微控制单元组成;所述顶层射频交互端及定位服务仲裁端由NRF24L01芯片及微控制 单元组成;所述顶层和底层通过CAN总线连接。
[0018] 所述定位方法步骤七中的目标点计算方法如下：
[0019] 设点为AUlYlZ!)，B(X2，Y2，Z2)，C(X3，Y 3，Z3)，D(X4，Y4，Z4)，各个点到移动机器人 发射端的距离为1,(12,(13，d 4,再假设被定位点的位置为M(x，y，z)，由勾股定理可得公式 (2):(x-Xi)2+(y-Yi)2+(z-Zi)2 = di2i = l，2,3,4整理式⑵可得式(3):
[0020]
[0021 ]将式(3)局部线性化得式(4):
[0022] fi(x〇,y〇,zo) + (x-xo)gii+(y-yo)gi2+(z-Zi)gi3 = Oi = 1,2,3,4；
[0023]
[0024]
[0025]
[0026] 通过线性超定方程式可得式(5)
[0027]
[0028] 通过超定方程组最小二乘解式可得式(6):
[0029
[0030] 通过初值修正可得式(7):
[0031]
[0032] 通过牛顿-高斯迭代可得式(8)
[0033]
·;:式（8)为目标点位置的坐标。经过5次 迭代目标点基本收敛，即使得整个过程坐标的误差精度保持在厘米级。
[0034]所述系统的多传感器节点网络接收端在天花板朝下安装或者安装在地面上朝上 安装。
[0035] 请求定位服务的移动机器人为飞行器，所述飞行器底部装有方向向下的分体超声 结合射频模块;本系统即可变为能对上方存在传感器节点信号的范围进行定位。
[0036] 由于采用本发明的技术方案，本发明的有益效果为:本发明的系统及方法能再在 保持厘米级精度的前提下，通过简单的设备实现，大幅度降低了系统成本，拓展了使用的空 间。本发明在地面上可以很方便的布置多个接收端来增加定位精度。
【具体实施方式】
[0037] -种基于分体超声结合射频的室内厘米级定位系统，所述定位系统分为两部分： 第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络;所述 第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声 波发射端、射频交互端;所述超声波发射端由超声波发射探头、信号放大器及微控制单元组 成，所述射频交互端由NRF24L01芯片及微控制单元组成。所述的第二部分的多传感器节点 网络设有底层和顶层，底层为分体超声接收端，顶层为射频交互端及定位服务仲裁端;所述 底层的分体超声接收端由超声波接收探头，信号放大器，信号滤波器，信号整形器及微控制 单元组成;所述顶层射频交互端及定位服务仲裁端由NRF24L01芯片及微控制单元组成;所 述顶层和底层通过CAN总线连接。
[0038] 所述系统的定位方法包括以下步骤：
[0039] 步骤一、装设传感器节点:将底层分体超声接收端间隔lm装设在天花板上，令超声 波接收探头垂直向下。所述系统的多传感器节点网络接收端在天花板朝下安装或者安装在 地面上朝上安装。
[0040] 请求定位服务的移动机器人为飞行器，所述飞行器底部装有方向向下的分体超声 结合射频模块;本系统即可变为能对上方存在传感器节点信号的范围进行定位。
[0041] 步骤二、组建多传感器节点网络:将所有底层分体超声接收端通过CAN总线连接到 顶层射频交互端及定位服务仲裁端。使用CAN总线能保证顶层射频交互端及定位服务仲裁 端能在固定的时间内与所有底层分体超声接收端交互信息。本系统CAN总线采用1000k波特 率发送数据帧，实测信息传输延时为固定的128us。
[0042]步骤三、请求定位服务的移动机器人发送定位请求:机器人通过分体超声结合射 频模块的射频交互端发送给多传感器节点网络中的顶层射