CN105510917A

CN105510917A - 一种基于分体超声结合射频的室内厘米级定位系统

Info

Publication number: CN105510917A
Application number: CN201510975777.4A
Authority: CN
Inventors: 刘勇; 徐晋鸿
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2015-12-22
Filing date: 2015-12-22
Publication date: 2016-04-20
Anticipated expiration: 2035-12-22
Also published as: CN105510917B

Abstract

本发明提供了一种误差精度小的基于分体超声波和射频的室内定位系统，所述定位系统分为两部分：第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络；所述第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声波发射端、射频交互端；所述的第二部分的多传感器节点网络设有底层和顶层，底层为分体超声接收端，顶层为射频交互端及定位服务仲裁端；本发明的系统及方法能再在保持厘米级精度的前提下，通过简单的设备实现，大幅度降低了系统成本，拓展了使用的空间。本发明在地面上可以很方便的布置多个接收端来增加定位精度。

Description

一种基于分体超声结合射频的室内厘米级定位系统

技术领域

本发明涉及所述机器人定位方法，特别涉及一种基于分体超声波和射频的室内定位系统的技术领域。

背景技术

室内定位是一个长久的研究课题，机器人在室内若想移动，需要解决三个问题。通俗地讲，机器人需要解决1.我在哪里；2.我要去哪里；3.我要怎么去。解决这三个问题的核心就在于环境地图的构建以及机器人的自定位。

目前已有多种室内定位的方法：如基于多个wifi信号强度的目标定位方法，基于多个蓝牙信号强度的目标定位方法，基于多个环境摄像头的地图构建及目标定位方法，基于磁导线的目标定位方法。其中多个wifi信号强度的目标定位方法和基于多个蓝牙信号强度的目标定位方法，均法存在误差较大的缺陷，其定位精度的误差往往大于10m，完全无法用于机器人的精确定位。基于多个环境摄像头的地图构建及目标定位方法优点在于能提供定位误差精度小于1厘米，速度大于15帧的定位服务，但缺点在于需要架设多个摄像头，成本较高，动辄超过十万元，并且只能对小于5立方米的环境进行定位。基于磁导线的目标定位方法优点在于能够进行大范围铺设且成本可控，定位误差精度小于1厘米；缺点在于预先铺设完成后，线路无法修改，当要改变机器人运动轨迹时需要重新铺设，相对耗时耗力。

发明内容

本发明所要解决的技术问题是提供一种误差精度小的基于分体超声波和射频的室内定位系统，为此，本发明采用以下技术方案：

一种基于分体超声结合射频的室内厘米级定位系统，所述定位系统分为两部分：第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络；所述第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声波发射端、射频交互端；所述的第二部分的多传感器节点网络设有底层和顶层，底层为分体超声接收端，顶层为射频交互端及定位服务仲裁端；

所述系统的定位方法包括以下步骤：

步骤一：装设传感器节点：将底层分体超声接收端装设在天花板上，所述底层分体超声接收端的数量大于等于四个；

步骤二：组建多传感器节点网络：将所有底层分体超声接收端与顶层射频交互端及定位服务仲裁端连接，使得顶层射频交互端及定位服务仲裁端与所有底层分体超声接收端交互信息；

步骤三：请求定位服务的移动机器人发送定位请求：机器人通过分体超声结合射频模块的射频交互端发送给多传感器节点网络中的顶层射频交互端及定位服务仲裁端；

步骤四：顶层射频交互端及定位服务仲裁端响应请求，开始定位：顶层射频交互端通过射频发送给请求定位服务的移动机器人信号，进行时间同步；同时定位服务仲裁端发送给所有底层为分体超声接收端开始计时，等待接收超声波信号；

步骤五：请求定位服务的移动机器人开始定位：机器人的射频交互端接收到顶层射频交互端的时间同步信号，启动超声波发射端，发送超声波；

步骤六：多传感器节点网络接收超声波信号并汇总至顶层为射频交互端及定位服务仲裁端：多传感器节点网络的各个分体超声接收端分别接收到移动机器人的超声波发射端发出的超声波，得到各个分体超声接收端相对与移动机器人的距离，并将数据发送至顶层为射频交互端及定位服务仲裁端；；

步骤七：顶层射频交互端及定位服务仲裁端计算目标位置：顶层接收到底层分体超声接收端发送来的位置信息后，查找是否存在有效数据，不存在，则本次定位失败，若存在，通过计算可得目标点位置的坐标；

步骤八：请求定位服务的移动机器人获得位置信息：顶层射频交互端及定位服务仲裁端计算出目标点位置的坐标后，通过射频模块发送给移动机器人，机器人获得自身的定位信息。

在采用上述技术方案的基础上，本发明还可采用以下进一步的技术方案：

所述超声波发射端由超声波发射探头、信号放大器及微控制单元组成，所述射频交互端由NRF24L01芯片及微控制单元组成。

所述底层的分体超声接收端由超声波接收探头，信号放大器，信号滤波器，信号整形器及微控制单元组成；所述顶层射频交互端及定位服务仲裁端由NRF24L01芯片及微控制单元组成；所述顶层和底层通过CAN总线连接。

所述定位方法步骤七中的目标点计算方法如下：

设点为A(X₁,Y₁,Z₁),B(X₂,Y₂,Z₂),C(X₃,Y₃,Z₃),D(X₄,Y₄,Z₄)，各个点到移动机器人发射端的距离为d₁,d₂,d₃,d₄，再假设被定位点的位置为M(x,y,z)，由勾股定理可得公式(2)：(x-X_i)²+(y-Y_i)²+(z-Z_i)²＝d_i ²i＝1,2,3,4整理式(2)可得式(3)：

将式(3)局部线性化得式(4)：

f_i(x₀,y₀,z₀)+(x-x₀)g_i1+(y-y₀)g_i2+(z-z_i)g_i3＝0i＝1,2,3,4；

通过线性超定方程式可得式(5)

通过超定方程组最小二乘解式可得式(6)：

式；

通过初值修正可得式(7)：

通过牛顿-高斯迭代可得式(8)

式(8)为目标点位置的坐标。经过5次迭代目标点基本收敛，即使得整个过程坐标的误差精度保持在厘米级。

所述系统的多传感器节点网络接收端在天花板朝下安装或者安装在地面上朝上安装。

请求定位服务的移动机器人为飞行器，所述飞行器底部装有方向向下的分体超声结合射频模块；本系统即可变为能对上方存在传感器节点信号的范围进行定位。

由于采用本发明的技术方案，本发明的有益效果为：本发明的系统及方法能再在保持厘米级精度的前提下，通过简单的设备实现，大幅度降低了系统成本，拓展了使用的空间。本发明在地面上可以很方便的布置多个接收端来增加定位精度。

具体实施方式

一种基于分体超声结合射频的室内厘米级定位系统，所述定位系统分为两部分：第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络；所述第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声波发射端、射频交互端；所述超声波发射端由超声波发射探头、信号放大器及微控制单元组成，所述射频交互端由NRF24L01芯片及微控制单元组成。所述的第二部分的多传感器节点网络设有底层和顶层，底层为分体超声接收端，顶层为射频交互端及定位服务仲裁端；所述底层的分体超声接收端由超声波接收探头，信号放大器，信号滤波器，信号整形器及微控制单元组成；所述顶层射频交互端及定位服务仲裁端由NRF24L01芯片及微控制单元组成；所述顶层和底层通过CAN总线连接。

所述系统的定位方法包括以下步骤：

步骤一、装设传感器节点：将底层分体超声接收端间隔1m装设在天花板上，令超声波接收探头垂直向下。所述系统的多传感器节点网络接收端在天花板朝下安装或者安装在地面上朝上安装。

步骤二、组建多传感器节点网络：将所有底层分体超声接收端通过CAN总线连接到顶层射频交互端及定位服务仲裁端。使用CAN总线能保证顶层射频交互端及定位服务仲裁端能在固定的时间内与所有底层分体超声接收端交互信息。本系统CAN总线采用1000k波特率发送数据帧，实测信息传输延时为固定的128us。

步骤三、请求定位服务的移动机器人发送定位请求：机器人通过分体超声结合射频模块的射频交互端发送给多传感器节点网络中的顶层射频交互端及定位服务仲裁端。有关射频模块的通讯延时，其中射频模块发送的电磁波为光速，在空间中传播的时间可以忽略不计，只需考虑程序在处理发送和接收的延时。实测本延时为恒定延时152us。

步骤四、顶层射频交互端及定位服务仲裁端响应请求，开始定位。顶层射频交互端通过射频发送给请求定位服务的移动机器人一段信号，进行时间同步。同时定位服务仲裁端通过CAN发送给所有底层为分体超声接收端开始计时，设时间点为t0，等待接收超声波信号。

步骤五、请求定位服务的移动机器人开始定位：机器人的射频交互端接收到顶层射频交互端的时间同步信号，启动超声波发射端，发送超声波，时间点同样为t0。

步骤六、多传感器节点网络接收超声波信号并汇总至顶层为射频交互端及定位服务仲裁端：多传感器节点网络的各个分体超声接收端分别接收到移动机器人的超声波发射端发出的超声波，记各个时间点为t1,t2...tn。由声速公式(1)得到各个距离d1，d2...dn，其中v＝340m/s。获得距离的传感器节点通过CAN将数据发送至顶层为射频交互端及定位服务仲裁端，部分节点因为位置关系无法接收到超声波则发送定位失败消息至顶层射频交互端及定位服务仲裁端。

d＝v*t(1)

步骤七、顶层射频交互端及定位服务仲裁端计算目标位置：顶层接收到底层分体超声接收端发送来的位置信息后，查找是否存在四个有效数据，且排布形式为正方形相邻的四个角点。若不存在，则本次定位失败，若存在，则设点为A(X₁,Y₁,Z₁),B(X₂,Y₂,Z₂),C(X₃,Y₃,Z₃),D(X₄,Y₄,Z₄)，各个点到移动机器人发射端的距离为d₁,d₂,d₃,d₄，再假设被定位点的位置为M(x,y,z)，由勾股定理可得公式(2)：(x-X_i)²+(y-Y_i)²+(z-Z_i)²＝d_i ²i＝1,2,3,4

整理式(2)可得式(3)：

将式(3)局部线性化得式(4)：

f_i(x₀,y₀,z₀)+(x-x₀)g_i1+(y-y₀)g_i2+(z-z_i)g_i3＝0i＝1,2,3,4；

通过线性超定方程式可得式(5)

通过超定方程组最小二乘解式可得式(6)：

式；

通过初值修正可得式(7)：

通过牛顿-高斯迭代可得式(8)

式(8)为目标点位置的坐标。

实验测得，经过5次迭代目标点基本收敛，即使得整个过程坐标的误差精度保持在厘米级。

步骤八、请求定位服务的移动机器人获得位置信息：顶层射频交互端及定位服务仲裁端计算出目标点位置的坐标后，通过射频模块发送给移动机器人，机器人获得自身的定位信息。

Claims

1.一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，所述定位系统分为两部分：第一部分为请求定位服务的移动机器人，第二部分为预先架设的多传感器节点网络；所述第一部分的移动机器人设有分体超声结合射频模块，所述分体超声结合射频模块包括超声波发射端、射频交互端；所述的第二部分的多传感器节点网络设有底层和顶层，底层为分体超声接收端，顶层为射频交互端及定位服务仲裁端；

所述系统的定位方法包括以下步骤：

2.如权利要求1所述的一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，所述超声波发射端由超声波发射探头、信号放大器及微控制单元组成，所述射频交互端由及微控制单元组成。

3.如权利要求1所述的一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，所述底层的分体超声接收端由超声波接收探头，信号放大器，信号滤波器，信号整形器及微控制单元组成；所述顶层射频交互端及定位服务仲裁端由NRF24L01芯片及微控制单元组成；所述顶层和底层通过CAN总线连接。

4.如权利要求1所述的一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，所述定位方法步骤七中的目标点计算方法如下：

f_{i} (x, y, z) = \sqrt{{(x - X_{i})}^{2} + {(y - Y_{i})}^{2} + {(z - Z_{i})}^{2}} - d_{i} = 0, i = 1, 2, 3, 4;

将式(3)局部线性化得式(4)：

f_i(x₀,y₀,z₀)+(x-x₀)g_i1+(y-y₀)g_i2+(z-z_i)g_i3＝0i＝1,2,3,4；

g_{i 1} = \frac{\partial f_{k}}{\partial x} |_{(x_{0}, y_{0}, z_{0})} = \frac{x_{0} - X_{i}}{\sqrt{{(x_{0} - X_{i})}^{2} + {(y_{0} - Y_{i})}^{2} + {(z_{0} - Z_{i})}^{2}}}

g_{i 2} = \frac{\partial f_{k}}{\partial y} |_{(x_{0}, y_{0}, z_{0})} = \frac{y_{0} - Y_{i}}{\sqrt{{(x_{0} - X_{i})}^{2} + {(y_{0} - Y_{i})}^{2} + {(z_{0} - Z_{i})}^{2}}}

g_{i 3} = \frac{\partial f_{k}}{\partial z} |_{(x_{0}, y_{0}, z_{0})} = \frac{z_{0} - Z_{i}}{\sqrt{{(x_{0} - X_{i})}^{2} + {(y_{0} - Y_{i})}^{2} + {(z_{0} - Z_{i})}^{2}}}

通过线性超定方程式可得式(5)

G_{0} [\begin{matrix} x - x_{0} \\ y - y_{0} \\ z - z_{0} \end{matrix}] = - F_{0} [\begin{matrix} g_{11} & g_{12} & g_{13} \\ g_{21} & g_{22} & g_{23} \\ g_{31} & g_{32} & g_{33} \\ g_{41} & g_{42} & g_{43} \end{matrix}] [\begin{matrix} x - x_{0} \\ y - y_{0} \\ z - z_{0} \end{matrix}] = - [\begin{matrix} f_{1} (x_{0}, y_{0}, z_{0}) \\ f_{2} (x_{1}, y_{1}, z_{1}) \\ f_{3} (x_{2}, y_{2}, z_{2}) \\ f_{4} (x_{3}, y_{3}, z_{3}) \end{matrix}];

通过超定方程组最小二乘解式可得式(6)：

[\begin{matrix} x - x_{0} \\ y - y_{0} \\ z - z_{0} \end{matrix}] = - {({G_{0}}^{T} G_{0})}^{- 1} {G_{0}}^{T} F_{0}

式；

通过初值修正可得式(7)：

[\begin{matrix} x \\ y \\ z \end{matrix}] = [\begin{matrix} x_{0} \\ y_{0} \\ z_{0} \end{matrix}] - {({G_{0}}^{T} G_{0})}^{- 1} {G_{0}}^{T} F_{0};

通过牛顿-高斯迭代可得式(8)

[\begin{matrix} x_{k + 1} \\ y_{k + 1} \\ z_{k + 1} \end{matrix}] = [\begin{matrix} x_{k} \\ y_{k} \\ z_{k} \end{matrix}] - {({G_{0}}^{T} G_{0})}^{- 1} {G_{0}}^{T} F_{0}, k = 0, 1, 2, ...;

式(8)为目标点位置的坐标。

5.如权利要求1所述的一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，所述系统的多传感器节点网络接收端在天花板朝下安装或者安装在地面上朝上安装。

6.如权利要求1所述的一种基于分体超声结合射频的室内厘米级定位系统，其特征在于，请求定位服务的移动机器人为飞行器，所述飞行器底部装有方向向下的分体超声结合射频模块。