CN107314766B - 基于超声和磁力计的机器人室内定位系统及方法 - Google Patents

Abstract

本发明公开了基于超声和磁力计的机器人室内定位系统及方法，包括位于天花板上的信标节点和位于机器人上的接收节点，所述信标节点的数量至少为一个，每个信标节点由一个超声波发射探头组成，每个接收节点包括至少三个超声波接收探头，在机器人上还设置有地磁传感器，所述超声波接收探头和地磁传感器均与接收控制系统连接；本发明实现了一种结构简单、计算量低，能准确定位移动物体在室内的位置，能避免电磁干扰的系统及方法的目的。

Description

基于超声和磁力计的机器人室内定位系统及方法

技术领域

本发明涉及机器人室内定位技术领域，具体涉及基于超声和磁力计的机器人室内定位系统及方法。

背景技术

超声定位系统中比较典型的系统是Active Bat和Cricket系统。其中前者接收节点发射RF信号和US信号，位置固定的信标节点接收RF信号和US信号；Cricket系统则相反，信标节点作为信号发射节点，待定位的接收节点用于信号的接收，然后根据RF信号和US信号的到达时间差进行分布式目标定位。

Active Bat是低功耗，无线室内定位系统，精度可达3cm。利用三角定位法，利用在天花板上嵌入的超声接收器，测量TOF。

Active Bat系统中，中心控制主机集中控制多个接收节点，而待定的接收节点作为发射节点是不可控的，尤其是当接收节点数量较多的情况下，必然会引起各组RF和US信号之间的串扰。从而导致错误的定位，影响系统的稳定性。Cricket系统具有很好的可扩展性，但其信标节点采用随机发射信号的方式，仍然不能有效解决信号串扰的问题。

在洪林的硕士毕业论文《移动机器人超声波室内定位系统研究》中提出了十字阵单发射多接收系统，该系统依然要通过同步来解决测距问题。

通过分析目前系统中存在的主要问题包括：

1)系统定位过程中需要同步，一般采用射频同步的方法，增加了系统的复杂度；

2)系统定位采用的是三角定位法，因此需要至少三个信标节点和一个接收节点才能完成定位，增加了系统中信标节点个数，也就意味着成本的增加；

3)射频同步的模式，在公共频段，与WIFI等同频，增加了电磁干扰，同时，系统也容易受到电磁干扰的影响。

发明内容

本发明所要解决的技术问题是克服以上技术问题，目的在于提供基于超声和磁力计的机器人室内定位系统及方法，实现一种结构简单、计算量低，能准确定位移动物体在室内的位置，能避免电磁干扰的系统及方法的目的。

本发明通过下述技术方案实现：

基于超声和磁力计的机器人室内定位系统，包括位于天花板上的信标节点和位于机器人上的接收节点，所述信标节点的数量至少为一个，每个信标节点由一个超声波发射探头组成，每个接收节点包括至少三个超声波接收探头，在机器人上还设置有地磁传感器，所述超声波接收探头和地磁传感器均与接收控制系统连接，其中：

超声波发射探头：发射超声波信号给超声波接收探头；

地磁传感器：探测地磁方向信号，并将地磁方向信号传输给接收控制系统；

超声波接收探头：接收超声波发射探头发射的超声波信号，并将超声波信号传输给接收控制系统；

接收控制系统：接收超声波接收探头传输的超声波信号和地磁传感器传输的地磁方向信号。

进一步的，本发明开创性的设计了一种用于机器人的超声波室内定位系统，在天花板上均匀安装多个信标节点，每个信标节点只包括一个超声波发射探头，超声波发射探头和超声波接收探头通信并利用现有的列阵测向技术可得到机器人的水平角，可得到机器人在室内的相对位置，然后再利用地磁传感器测向可得到机器人在水平面的偏离角度，以此矫正相对位置，通过这种两维测角阵列最终能机器人的实际位置，相比现有技术，本发明结构更加简单，减少了系统中信标节点个数，有效降低了定位成本。并且，本发明无需发射射频信号，有效避免了系统定位过程中需要同步，采用射频同步的方法增加了系统的复杂度的问题，同时也避免了电磁干扰。本发明安装在机器人身上的地磁传感器为一种专用于探测地磁方向的探测器，类似指南针，可时刻测量地磁方向信号。超声波发射探头与发射控制系统连接。超声波接收探头与接收控制系统，由接收控制系统处理定位信息，该接收控制系统为现有结构，包括与超声波接收探头依次连接的放大器、BPF、MCU模块等，放大器将超声波接收探头传输的超声波信号进行功率放大后传输给BPF；BPF接收放大器传输的超声波信号，按其所在频率滤波后传输给MCU模块进行处理，MCU模块将超声波信号进行数模转换和回波信号处理后输出移动设备的定位信息。

优选的，当每个接收节点由三个超声波接收探头组成时，三个超声波接收探头围成一个三角形。进一步的，三个超声波接收探头可形成任意形状三阵元平面，三阵元平面在相同孔径的阵列中，具有线阵有较好的定向精度；当,阵列孔径越大，其定距精度越高。在无任何目标位置的先验信息时，为避免整个平面的定位精度不会出现某些点特别大的问题，可采用正三角阵。

优选的，当每个接收节点由四个超声波接收探头组成时，四个超声波接收探头位于同一平面，且四个探头中相对的两个探头的连线与另外两个相对的探头的连线相互垂直。进一步的，该结构排列成十字阵列，也可称为正方阵列或圆形阵列，这种阵列的距离参数只有一个，相比现有技术，数据处理起来更加简单。

优选的，所述接收控制系统内的存储器采用FIFO存储器。本发明采用的FIFO存储器用于存储超声波接收探头接收的数据，当数据存储满了之后，自动覆盖掉最初始的数据。进一步的，采用这样的方式，接收端不用一直进行信号处理，而是等接收到信号超过一定门限之后，表示有发射信号接收到，同时开始进行处理。

基于超声和磁力计的机器人室内定位方法，包括以下步骤：

A：接收节点接收信标节点发射的超声波信号，根据阵列测向理论，测得信标节点到接收节点的俯仰角水平角θ；

B：测量出信标节点所在平面到接收节点所在平面的垂直高度h，根据信标节点到接收节点的俯仰角通过/>计算得到接收节点距离信标节点在水平面上的投影长度l；

C：地磁传感器探测地磁方向信号，获得机器人在水平面的偏离角度θ₀；设定两个超声波接收探头的基线方向与地磁传感器的探测方向重合时的方向为初始方向，根据信标节点到接收节点的水平角θ，可计算出机器人与信标节点之间的真实角度θ′＝θ₀±θ；

D：由于信标节点的坐标已知，设为(x₀,y₀)，再根据方程组x₁＝x₀+l·cosθ′，y₁＝y₀+l·sinθ′，可计算得到机器人的坐标为(x₁,y₁)。

进一步的，本发明的定位系统的具体定位方法如上述所示，利用阵列测向理论，可测得信标节点到接收节点的俯仰角水平角θ；通常室内的天花板与地面平行，那么机器人上的接收节点距离天花板的垂直高度固定不变，本利用高度已知信息、俯仰角/>水平角θ和信标节点的坐标就可获得机器人的坐标位置，但由于机器人在移动的过程中会接收到多个信标节点发射的超声波信号，所以所测得水平角并不是绝对水平角，根据该水平角计算得出的坐标位置为一种相对位置，即机器人相对每个一个信标节点的位置，系统无法从若干个相对位置点判别出机器人的实际位置在哪。所以本发明再利用地磁传感器来对机器人的相对水平角进行校正，测量出机器人在水平面的偏离角度θ₀，通过θ′＝θ₀±θ计算得出θ′，获得机器人与信标节点之间的真实水平角度，最终计算出机器人的实际坐标。相比现有技术，本发明采用的方法计算量低，且不需要系统同步，降低了系统复杂度。

具体位置校正原理如下：1、选取接收节点中任意两个超声波接收探头，设定两个超声波接收探头的基线(基线指两个超声波接收探头的连线)方向与地磁传感器的探测方向重合时的方向为初始方向，此时所述的信标节点位于两个超声波接收探头的基线的中垂线上，此时测得的信标节点到接收节点的水平角θ为0°，地磁传感器的测量方向与初始方向重合，测得的偏离角度θ₀也为0°，其机器人与信标节点之间的真实角度θ′＝θ₀±θ为0°，该时刻信标节点所测得水平角θ就是机器人实际位置测得的水平角。2、随着机器人的移动，两个超声波接收探头的基线的中垂线会再次经过信标节点，此时测得的信标节点到接收节点的水平角θ仍为0°，该水平角θ不是绝对水平角，所以需要地磁传感器进行校正，由于地磁传感器探测的地磁方向是一直不变的，此时地磁传感器测得的相对于基线方向的偏离角度θ₀为45°，则机器人实际位置测得的水平角θ′＝θ₀±θ为45°。3、由于机器人是随意移动的，当机器人的两个超声波接收探头的基线的中垂线不经过信标节点时，此刻根据阵列测向理论测得的信标节点到接收节点的水平角θ就不为0°，地磁传感器测得的偏离角度θ₀也不为0°，实际得到的机器人与信标节点之间的真实角度就为θ′＝θ₀±θ。

本发明与现有技术相比，具有如下的优点和有益效果：

1、本发明基于超声和磁力计的机器人室内定位系统及方法，利用超声波探头和地磁传感器同时测向，获得机器人与信标节点之间的真实水平角度实现了对机器人的最终定位，相比现有技术，结构更加简单，减少了系统中信标节点个数，有效降低了定位成本；

2、本发明基于超声和磁力计的机器人室内定位系统及方法，无需发射射频信号，有效避免了系统定位过程中采用射频同步的方法增加了系统的复杂度的问题，同时也避免了电磁干扰；

3、本发明基于超声和磁力计的机器人室内定位系统及方法，定位方法计算量低，可实现对机器人的准确定位。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。在附图中：

图1为本发明超声波室内定位位置示意图；

图2为本发明的位置校正测向示意图；

图3为本发明十字接收阵列示意图；

图4为本发明的接收控制系统结构图。

附图中标记及对应的零部件名称：

1-天花板，2-地面，3-超声波发射探头，4-超声波接收探头，5-地磁传感器。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实施例1

如图1、4所示，本发明基于超声和磁力计的机器人室内定位系统，包括位于天花板1上的信标节点和位于机器人上的接收节点，所述信标节点的数量至少为一个，每个信标节点由一个超声波发射探头3组成，每个接收节点包括至少三个超声波接收探头4，在机器人上还设置有地磁传感器5，所述超声波接收探头4和地磁传感器5均与接收控制系统连接，其中：

超声波发射探头3：发射超声波信号给超声波接收探头4；

地磁传感器5：探测地磁方向信号，并将地磁方向信号传输给接收控制系统；

超声波接收探头4：接收超声波发射探头3发射的超声波信号，并将超声波信号传输给接收控制系统；

接收控制系统：接收超声波接收探头4传输的超声波信号和地磁传感器5传输的地磁方向信号。所述接收控制系统内的存储器采用FIFO存储器；接收控制系统包括依次连接的放大器、BPF、MCU模块，放大器与超声波接收探头4连接，且放大器、BPF的数量均与超声波接收探头4的数量一一对应，其中：超声波接收探头4接收信标节点发送的超声波信号，并将超声波信号传输给放大器；放大器接收超声波接收探头4传输的超声波信号，将功率放大后的超声波信号传输给BPF；BPF接收放大器传输的超声波信号，并将超声波信号传输给MCU模块；MCU模块接收所有的BPF传输的超声波信号，将超声波信号进行数模转换和回波信号处理后输出移动设备的定位信息。

基于超声和磁力计的机器人室内定位方法，包括以下步骤：

A：接收节点接收信标节点发射的超声波信号，根据阵列测向理论，测得信标节点到接收节点的俯仰角水平角θ；

B：测量出信标节点所在平面到接收节点所在平面的垂直高度h，根据信标节点到接收节点的俯仰角通过/>计算得到接收节点距离信标节点在水平面上的投影长度l；

C：地磁传感器探测地磁方向信号，获得机器人在水平面的偏离角度θ₀；设定两个超声波接收探头的基线方向与地磁传感器的探测方向重合时的方向为初始方向，根据信标节点到接收节点的水平角θ，可计算出机器人与信标节点之间的真实角度θ′＝θ₀±θ；

D：由于信标节点的坐标已知，设为(x₀,y₀)，再根据方程组x₁＝x₀+l·cosθ′，y₁＝y₀+l·sinθ′，可计算得到机器人的坐标为(x₁,y₁)。

实施例2

如图1～4所示，本发明基于超声和磁力计的机器人室内定位系统，当每个接收节点由四个超声波接收探头4组成时，四个超声波接收探头4位于同一平面，且四个探头中相对的两个探头的连线与另外两个相对的探头的连线相互垂直。四个接收探头排列成十字形状，其中为机器人正前方向，/>为机器人正前方向垂直的轴。每个轴中的两个阵元间距为d，中心交于一点，假设中心点即为机器人的位置奇点，如图3所示。

当信标节点在天花板1上发射信号时，四个探头可以接收到同一个发射信号，根据阵列测向理论，四个超声波接收探头4可以测量出发射信号的俯仰角水平角θ。由于天花板1的高度h已知，根据俯仰角可以求出目标距离超声波发射探头3在地面2上的投影长度为

地磁传感器5的输出为目标在水平面的偏离角度θ₀，假设以北向为初始方向，则根据阵列获得的水平角可以得到目标与信源的真实角度θ′。由于超声波发射信源的坐标已知，设为(x₀,y₀)，则机器人的位置为(x₁,y₁)，利用x₁＝x₀+l·cosθ′，y₁＝y₀+l·sinθ′可计算得出。

以上所述机器人与信标节点之间的真实角度θ′具体计算过程如下，具体示意图见图2，该图为天花板指向地面的俯视图：首先在十字阵列的四个超声波接收探头4中选取任意两个超声波接收探头，设定两个超声波接收探头的基线方向与地磁传感器的探测方向重合时的方向为初始方向，在图2中表示为L1，此时所述的信标节点位于两个超声波接收探头的基线的中垂线上，此时测得的信标节点到接收节点的水平角θ为0°，地磁传感器的测量方向与初始方向重合，测得的偏离角度θ₀也为0°，其机器人与信标节点之间的真实角度θ′＝θ₀±θ为0°，该时刻信标节点所测得水平角θ就是机器人实际位置测得的水平角。随着机器人的移动，两个超声波接收探头的基线的中垂线会再次经过信标节点，在图2中表示为L2，此时测得的信标节点到接收节点的水平角θ仍为0°，该水平角θ不是绝对水平角，所以需要地磁传感器进行校正，由于地磁传感器探测的地磁方向是一直不变的，此时地磁传感器测得的相对于基线方向的偏离角度θ₀为45°，则机器人实际位置测得的水平角θ′＝θ₀±θ为45°。由于机器人是随意移动的，当机器人的两个超声波接收探头的基线的中垂线不经过信标节点时，在图2中表示为L3，此刻根据阵列测向理论测得的信标节点到接收节点的水平角θ为15°，地磁传感器测得的偏离角度θ₀为45°，实际得到的机器人与信标节点之间的真实角度就为θ′＝θ₀±θ为60°或30度。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.基于超声与磁力计的机器人室内定位方法，方法应用于室内定位系统，所述系统包括位于天花板上的信标节点和位于机器人上的接收节点，其特征在于，所述信标节点的数量至少为一个，每个信标节点由一个超声波发射探头组成，每个接收节点包括至少三个超声波接收探头，在机器人上还设置有地磁传感器，所述超声波接收探头和地磁传感器均与接收控制系统连接，其中：

超声波发射探头：发射超声波信号给超声波接收探头；

地磁传感器：探测地磁方向信号，并将地磁方向信号传输给接收控制系统；

超声波接收探头：接收超声波发射探头发射的超声波信号，并将超声波信号传输给接收控制系统；

接收控制系统：接收超声波接收探头传输的超声波信号和地磁传感器传输的地磁方向信号；

当每个接收节点由三个超声波接收探头组成时，三个超声波接收探头围成三角形接收阵列，三个超声波接收探头可形成任意形状三阵元平面；

当每个接收节点由四个超声波接收探头组成时，四个超声波接收探头位于同一平面，且四个探头中相对的两个探头的连线与另外两个相对的探头的连线相互垂直；

包括以下方法步骤：

A：接收节点接收信标节点发射的超声波信号，根据阵列测向理论，测得信标节点到接收节点的俯仰角水平角θ，首先在十字阵列的四个超声波接收探头中选取任意两个超声波接收探头，设定两个超声波接收探头的基线方向与地磁传感器的探测方向重合时的方向为初始方向，此时所述的信标节点位于两个超声波接收探头的基线的中垂线上，此时测得的信标节点到接收节点的水平角θ为0°，地磁传感器的测量方向与初始方向重合，测得的偏离角度θ₀也为0°，其机器人与信标节点之间的真实角度θ′＝θ₀±θ为0°，该时刻信标节点所测得水平角θ就是机器人实际位置测得的水平角θ′；

B：测量出信标节点所在平面到接收节点所在平面的垂直高度h，根据信标节点到接收节点的俯仰角通过/>计算得到接收节点距离信标节点在水平面上的投影长度l；

C：地磁传感器探测地磁方向信号，获得机器人在水平面的偏离角度θ₀；设定两个超声波接收探头的基线方向与地磁传感器的探测方向重合时的方向为初始方向，根据信标节点到接收节点的水平角θ，可计算出机器人与信标节点之间的真实角度θ′＝θ₀±θ；随着机器人的移动，两个超声波接收探头的极限的中垂线会再次经过信标节点，此时测得的信标节点到接收节点的水平角θ仍为0°，通过地磁传感器进行校正，地磁传感器测得的相对于基线方向的偏离角度θ₀为45°，则机器人实际位置测得的水平角θ′＝θ₀±θ为45°；由于机器人是随意移动的，当机器人的两个超声波接收探头的基线的中垂线不经过信标节点时，此刻根据阵列测向理论测得的信标节点到接收节点的水平角θ就不为0°，地磁传感器测得的偏离角度θ₀也不为0°，实际得到的机器人与信标节点之间的真实角度就为θ′＝θ₀±θ；

D：由于信标节点的坐标已知，设为(x₀,y₀)，再根据方程组x₁＝x₀+l·cosθ′，y₁＝y₀+l·sinθ′，可计算得到机器人的坐标为(x₁,y₁)。

2.根据权利要求1所述的基于超声与磁力计的机器人室内定位方法，其特征在于，所述接收控制系统内的存储器采用FIFO存储器。