一种机器人的近距离混合跟随定位系统及方法

1.本发明属于机器人目标定位领域，具体涉及一种机器人的近距离混合跟随定位系统及方法。


背景技术：

2.随着科学技术的发展，智能机器人已经慢慢的应用于人们的生活之中，机器人目标定位技术也成为了研究热点之一。在机器人目标定位领域，提升机器人的定位精度成为了一大研究热点。超声波以其指向性强、成本较低等优点在近距离定位中表现出良好的实用性。
3.《一种超声波室内定位方法及系统》介绍了一种由定位目标携带转发器，通过基站发射超声波信号，由定位目标转发至下一基站，通过处理超声波在各个基站间的飞行时间实现定位；
4.《一种基于复合型超声波信号的超声定位系统》介绍了一种通过发射载波编码信号由互相关算法计算超声波飞行时间，实现定位的方法。
5.上述两种方法均需要超声波收发时间同步，对超声波飞行时间计算有较高要求，在实际应用过程中，很难精确计算超声波飞行时间，故其很难实现精准定位。此外，《一种可辨识los/nlos声信号的智能手机室内定位系统和方法》介绍了一种采用智能手机发射伪超声信号，采用到达时间差测量算法(tdoa算法)作为定位算法，通过支持向量机(svm)与证据理论(ds)对测量数据进行识别丢弃，最终完成定位。利用tdoa算法虽然能有效减少对于时间同步的要求，但其提高了算法的复杂度，在实际应用过程中不易求解。


技术实现要素：

6.为克服现有技术中存在的上述缺陷，本发明的目的是提出一种能有效解决多个传感器信号间的互扰问题并提高近距离跟随系统相对定位精度的机器人的近距离混合跟随定位系统及方法。
7.为了实现上述目的，本发明的技术方案如下：
8.一种机器人的近距离混合跟随定位系统，包括跟随者端模块和被跟踪目标端模块，所述被跟踪目标端模块包括电源电路a、无线通信模块a、超声波发射器和处理器a，所述电源电路a、无线通信模块a、超声波发射器和处理器a均安装在被跟踪目标端，所述电源电路a、无线通信模块a和超声波发射器分别与处理器a连接；所述跟随者端模块包括电源电路b、无线通信模块b、超声波接收阵列和处理器b，所述电源电路b、无线通信模块b、超声波接收阵列和处理器b均安装在跟随者端，所述电源电路b、无线通信模块b和超声波接收阵列分别与处理器b连接；所述无线通信模块a与无线通信模块b通过无线网络进行通信；所述超声波接收阵列包括n个超声波接收器，n≥2；
9.所述无线通信模块a发出的无线电信号中包含被跟踪目标端模块的状态以及电池状态信息；所述的被跟踪目标端模块的状态包括被跟踪目标端的时间、速度和方向；
10.所述超声波发射器在无线通信模块a接收到脉冲触发信号后发出频率为f
k
的超声波脉冲；
11.频率f
k
在超声波发射器自振频率f0的附近，且满足f
k
＝f0+k
·
δf，其中，k为整数，不同的被跟踪目标端超声波发射器所发出的超声脉冲频率k值不同，δf为通过带通滤波器进行区分相邻频率的脉冲频率间隔；
12.超声波发射器每次发出频率为f
k
的超声波脉冲持续时间为t
k
，t
k
大于超声波发射器发射超声波的起振时间t
10
的2倍；
13.所述跟随者端的无线通信模块b接收被跟踪目标端的无线通信模块a发出的无线电信号；
14.所述跟随者端的无线通信模块b向被跟踪目标端的无线通信模块a发送的无线电信号包括：脉冲触发信号和状态查询信号；
15.所述跟随者端的无线通信模块b向被跟踪目标端的无线通信模块a发送脉冲触发信号的同时，处理器b对所有超声波接收器进行采样，采样频率为f
s
，f
s
≥2
×
f
k
，采样时间为t
k
，t
k
≥2
×
t
k
，以确保在所述跟随者和被跟踪目标距离范围内接收到被跟踪目标发出的超声波脉冲；
16.所述跟随者端固定的第i个超声波接收器在采样持续时间δt
1ki
后接收到被跟踪目标端超声波发射器所发出的超声波脉冲信号，所采样得到的超声波脉冲信号持续时间为st
ki
，则对第i个超声波接收器来说有：t
k
≤st
ki
，且δt
1ki
+st
ki
≤t
k
；i≤n，对采样得到的st
ki
时间内的超声波脉冲信号进行以f
k
为中心频率、δf为通带宽带的带通滤波，滤除接收到的附近其他被跟踪目标所发出的不同频率的超声波脉冲信号；
17.以f
k
为中心频率、δf为通带宽带的带通滤波在中心频率f
k
造成的时间滞后为δt
k
，以采集电压阈值v
t
作为判断条件，则对第i个超声波接收器来说，若大于采集电压阈值v
t
时刻为δt
2ki
，则δt
1ki
≤δt
2ki
，且δt
2ki
‑
δt
k
为所述被跟踪目标端超声波发射器所发出的超声波脉冲在传输介质中飞行的时间；
18.对st
ki
时间内采样得到的超声波脉冲信号进行快速傅里叶变换即fft分析，记录在频率f
k
处的幅值为at
ki
；
19.对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，建立如下频率为f
k
的超声波脉冲信号强度指示模型即rssi模型r
i
(d
i
,θ
i
)如下：
20.a
i
＝r
i
(d
i
,α
i
,β
i
)
ꢀꢀꢀ
(1)
21.式中，a
i
为第i个超声波接收器接收超声波脉冲信号的幅值，d
i
为第i个超声波接收器与被跟踪目标端超声波发射器之间的距离，α
i
和β
i
分别为第i个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角和垂直方向夹角；
22.对第i个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α
i
和垂直方向夹角β
i
以及第j个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α
j
和垂直方向夹角β
j
，满足下式：
[0023][0024]
式中，δα
ij
和δβ
ij
为常数。
[0025]
对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，被跟踪目
标和跟随者在恒定直线上运动且超声波发射器与超声波接收器正相对时，频率为f
k
的超声波脉冲信号的rssi模型r
i
(x,α
i
,β
i
)有如下形式：
[0026][0027]
式中，a
i
、b
i
、c
i
为第i个超声波接收器rssi模型参数，并由离线实验数据拟合而得；
[0028]
对频率为f
k
的超声波脉冲信号rssi模型，求解以下方程组获得已知测量超声波脉冲信号幅值时超声波发射器与超声波接收器之间的距离d
i
、夹角α
i
、夹角β
i
，即：
[0029][0030]
对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，基于超声波脉冲信号rssi模型的测量结果d
i
和飞行时间t
ofi
，通过选取神经网络nn
i
和相应的离线训练数据，建立以下频率为f
k
的超声波脉冲信号与距离之间的神经网络模型：
[0031][0032]
式中，r
i
为经神经网络融合后的被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器之间的距离；
[0033]
通过跟随者端固定的第i个超声波接收器在第k次测量所得到的rssi模型距离d
ki
和飞行时间δt
2ki
，求得跟随者与被跟踪目标端超声波发射器第k次测量的融合距离，即：
[0034]
r
ik
＝nn
i
(d
ki
,δt
2ki
)
ꢀꢀꢀ
(6)
[0035]
对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，被跟踪目标和跟随者在恒定直线上运动且超声波发射器与超声波接收器正相对时，则直接通过式(6)求得超声波发射器与超声波接收器之间的距离；
[0036]
跟随者端固定的超声波接收器个数为2时，若被跟踪目标的超声波发射器与跟随者的两个超声波接收器安装在同一平面上且均在该平面上运动时，则在该平面上以跟随者端的超声波接收器为基准建立二维坐标系，通过求解式(7)方程组给出被跟踪目标的位置坐标(x,y)：
[0037][0038]
式中，(x1,y1)为所述跟随者端第1个超声波接收器的位置坐标，(x2,y2)为跟随者端第2个超声波接收器的位置坐标；
[0039]
跟随者端固定的超声波接收器个数超过2时，即n≥3,则以跟随者端的超声波接收器为基准建立空间三维坐标系，通过求解式(8)方程组给出被跟踪目标的位置坐标(x,y,z)
[0040][0041]
式中，(x
i
,y
i
,z
i
)为所述跟随者端第i个超声波接收器在空间三维坐标系中的位置坐标，i＝1,
…
,n。
[0042]
进一步地，所述处理器a和处理器b为stm32单片机或其他具有不低于stm32单片机计算能力和兼容接口的处理器。
[0043]
进一步地，所述无线通信模块a和无线通信模块b均为无线电信号收发器，无线电信号收发器包括红外收发器、蓝牙收发器、wifi收发器、zigbee收发器、rfid收发器或nfc收发器。
[0044]
一种机器人的近距离混合跟随定位方法，利用近距离混合跟随定位系统进行定位，包括以下步骤：
[0045]
步骤1、初始化系统，初始化被跟踪目标端与机器人跟随者端的设备，初始化无线通信模块a和无线通信模块b；
[0046]
步骤2、由被跟踪目标通过无线通信模块a向机器人跟随者发送无线信号，或机器人跟随者通过无线通信模块b向被跟踪目标发送无线信号，建立二者无线通信模块连接，实现跟随者端与被跟踪目标端实时通信，被跟踪目标等待机器人跟随者发送无线触发信号；
[0047]
步骤3、若双方未接收到对方发送的无线信号，则返回步骤2；
[0048]
步骤4、若双方连接已经建立，则机器人跟随者向被跟踪目标发送启动超声波发送指令，同时记录该时刻，并启动采样，采样频率为f
s
，启动定时周期为t
k
的定时器；
[0049]
步骤5、被跟踪目标接收到启动超声波发送指令后，立即发送频率为f
k
的超声波信号，并持续发送t
k
时间后停止发送；
[0050]
步骤6、若机器人跟随者在采样周期t
k
内采集到超声信号的时间为st
ki
，对采样得到的st
ki
时间内的超声波脉冲信号进行以f
k
为中心频率、δf为通带宽带的带通滤波，滤除接收到的附近其他被跟踪目标所发出的不同频率的超声波脉冲信号；
[0051]
步骤7、机器人跟随者以f
k
为中心频率、δf为通带宽带的带通滤波在中心频率f
k
造成的时间滞后为δt
k
，以采集电压阈值v
t
作为判断条件，则对第i个超声波接收器来说，若大于采集电压阈值v
t
时刻为δt
2ki
，则δt
1ki
≤δt
2ki
，且δt
2ki
‑
δt
k
为所述被跟踪目标端超声波发射器所发出的超声波脉冲在传输介质中飞行的时间；
[0052]
步骤8、机器人跟随者对采样所得的超声波脉冲信号进行快速傅里叶变换即fft分析，记录在频率f
k
处的幅值为at
ki
；
[0053]
步骤9、对被跟踪目标端超声波发射器和机器人跟随者端固定的第i个超声波接收器，在处理器中建立如(1)式给出的频率为f
k
的超声波脉冲信号强度指示模型，利用该模型求取第i个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α
i
和垂直方向夹角β
i
以及第j个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α
j
和垂直方向夹角β
j
；
[0054]
步骤10、机器人跟随者对由(1)式给出的频率为f
k
的超声波脉冲信号rssi模型，求(4)式给出的方程组获得已知测量超声波脉冲信号幅值时超声波发射器与超声波接收器之间的距离d
i
、夹角α
i
、夹角β
i
；
[0055]
步骤11、机器人跟随者对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，基于超声波脉冲信号rssi模型的测量结果d
i
和飞行时间t
ofi
，根据训练所得的神经网络模型(6)式，求取融合后的被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器之间的距离；
[0056]
步骤12、机器人跟随者根据其所带的接收器数目选择(7)式或(8)式求解被跟踪目标空间坐标(x,y,z)的当前值，并显示；
[0057]
步骤13、返回步骤5进行下一次被跟踪目标空间坐标(x,y,z)计算，直至人为退出上述过程。
[0058]
与现有技术相比，本发明具有以下有益效果：
[0059]
1、本发明融合了toa算法与rssi算法并通过神经网络训练，最终计算机器人与被跟踪目标端之间的位置关系。相较于传统的使用单一定位算法，实现了较高的定位精度。
[0060]
2、本发明考虑了多机器人同时工作时信号串扰的问题，提出了每个机器人发送不同频率超声波信号，并通过滤波实现机器人与其对应目标之间的特异性识别。
[0061]
3、本发明可实现一维、二维、三维定位，对超声波接受阵列布局无特殊限制，具有结构简单，成本较低等优点。
附图说明
[0062]
图1为本发明的原理示意图。
[0063]
图2为本发明的程序流程示意图。
[0064]
图3为本发明的定位方法示意图。
[0065]
图4为本发明的bp神经网络模型示意图。
具体实施方式
[0066]
为了更清楚的解释本发明的实施方式，下面结合附图，将本发明实施例中的技术方案进行清楚完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
[0067]
如图1所示，一种机器人的近距离混合跟随定位系统包括机器人部分即跟随端和被跟踪目标端。在被跟踪目标端上装有电源电路、无线通信模块、超声波发射器和stm32单片机。在机器人端装有电源电路、无线通信模块、超声波接收阵列和stm32单片机。
[0068]
本实例以三维平面定位说明。本实例的无线通信模块采用蓝牙模块为例，超声波模块采用t/r40
‑
16型号超声波传感器，采用1个超声波发射器(如图3所示o点)，4个超声波接收器组成超声波接收阵列(如图3所示a,b,c,d点)，以此为例对本发明进行具体说明。
[0069]
考虑多个机器人同时工作时，容易发生信号串扰，故不同机器人目标端的stm32单片机应发射以40khz为中心频率(谐振频率附近)
±
1khz(以0.1khz为单位分频)的持续时长为15ms的方波作为超声波发射器的触发信号(此处以40.4khz为例)，并触发蓝牙模块与机器人端进行通信。机器人端的蓝牙模块收到对应目标端蓝牙通讯信号后，以400khz的采样频率接收并记录4个超声波接收器所接收到的超声波信号，接收时间持续50ms。
[0070]
在多个机器人同时工作时，接收到的超声波信号易发生串扰，故需采用带通滤波
器，分别对4个超声波接收器接收到的超声波信号进行滤波，滤去其他频率的超声波信号，保留与发射频率相对应的40.4khz的超声波信号。同时，设置阈值电压v，当滤波后的超声波信号大于阈值电压v时，记录该时刻，作为超声波的飞行时间t1、t2、t3、t4。
[0071]
对4个超声波接收器滤波后的超声波信号做fft分析，得到频率在40.4khz时的幅值，分别为a1、a2、a3、a4,根据所拟合的rssi模型如下式，可计算出其对应的距离。
[0072][0073]
其中，d1、d2、d3、d4分别代表四个超声波接收器由模型计算出的距被跟随目标的距离，α1、α2、α3、α4和β1、β2、β3、β4分别代表四个超声波接收器与超声波发射器在水平方向和垂直方向间的夹角，a1、a2、a3、a4表示四个超声波接收器所接收到的超声波信号的幅值。
[0074]
通过计算得到的电压幅值与超声波的飞行时间，可以训练如图4所示的bp神经网络模型nn
i
，该网络模型由输入层、两个隐层结构和输出层构成。网络模型的输入是超声波接收器接收到的电压幅值与飞行时间(d
i
,t
i
),网络的输出是超声波接收器与超声波发射器的距离r
i
。
[0075]
根据rssi模型求得的距离d1、d2、d3、d4以及超声波飞行时间t1、t2、t3、t4，通过离线训练的bp神经网络模型nn1、nn2、nn3、nn4，可得到四个超声波接收器与被跟随目标之间的距离。其中，r1、r2、r3、r4代表有神经网络计算出的四个超声波接收器与被跟踪目标上超声波发射器之间的距离。
[0076][0077]
如图3
‑
4所示，根据bp神经网络模型计算结果，可以获得四个超声波接收器与被跟踪目标上超声波发射器之间的距离r1、r2、r3、r4。根据下式，可计算出以机器人上的超声波接收器为基准建立三维平面坐标系，其中被跟踪目标的的坐标为(x,y,z)。(x1,y1),(x2,y2),(x3,y3),(x4,y4)分别代表四个超声波接收器的位置坐标。
[0078][0079]
如果是二维平面定位，那么超声波发射器与超声波接收器在同一平面，故仅采用2个超声波接收器(如图3所示a,b点)和一个超声波发射器(如图3所示o点)。
[0080]
与三维空间一样，在二维平面中，采用带通滤波器，分别对2个超声波接收器接收到的超声波信号进行滤波，保留与发射频率相对应的40.4khz的超声波信号。同时，设置阈值电压v，当滤波后的超声波信号大于阈值电压v时，记录该时刻，作为超声波的飞行时间t1、t2。对2个超声波接收器滤波后的超声波信号做fft分析，得到频率在40.4khz时的幅值，分别为a1、a2,根据所拟合的rssi模型如下式，可计算出其对应的距离如下：
[0081][0082]
其中，d1、d2分别代表2个超声波接收器由模型计算出的距被跟随目标的距离，α1、α2分别代表2个超声波接收器与超声波发射器的夹角，a1、a2表示2个超声波接收器所接收到的超声波信号的幅值。
[0083]
根据rssi模型求得的距离d1、d2以及超声波飞行时间t1、t2，通过训练bp神经网络模型nn1、nn2，可得到2个超声波接收器与被跟随目标之间的距离。其中，r1、r2代表由bp神经网络计算出的2个超声波接收器与被跟踪目标上超声波发射器之间的距离：
[0084][0085]
根据2个超声波接收器与被跟踪目标上超声波发射器之间的距离r1、r2，可计算出以机器人上的超声波接收器为基准建立二维平面坐标系，其中被跟踪目标的的坐标为(x,y)。(x1,y1),(x2,y2),分别代表a、b这两个超声波接收器的位置坐标。
[0086][0087]
以上所述仅为本发明的一种实例，并不用以限制本发明，凡在本发明的精神和原则之内，做出任何修改、同等替换、改进等，均应包含在本发明的保护范围之内。