[发明专利]一种机器人的近距离混合跟随定位系统及方法

权利要求书

1.一种机器人的近距离混合跟随定位系统，其特征在于：包括跟随者端模块和被跟踪目标端模块，所述被跟踪目标端模块包括电源电路A、无线通信模块A、超声波发射器和处理器A，所述电源电路A、无线通信模块A、超声波发射器和处理器A均安装在被跟踪目标端，所述电源电路A、无线通信模块A和超声波发射器分别与处理器A连接；所述跟随者端模块包括电源电路B、无线通信模块B、超声波接收阵列和处理器B，所述电源电路B、无线通信模块B、超声波接收阵列和处理器B均安装在跟随者端，所述电源电路B、无线通信模块B和超声波接收阵列分别与处理器B连接；所述无线通信模块A与无线通信模块B通过无线网络进行通信；所述超声波接收阵列包括n个超声波接收器，n≥2；

所述无线通信模块A发出的无线电信号中包含被跟踪目标端模块的状态以及电池状态信息；所述的被跟踪目标端模块的状态包括被跟踪目标端的时间、速度和方向；

所述超声波发射器在无线通信模块A接收到脉冲触发信号后发出频率为f_k的超声波脉冲；

频率f_k在超声波发射器自振频率f₀的附近，且满足f_k＝f₀+k·Δf，其中，k为整数，不同的被跟踪目标端超声波发射器所发出的超声脉冲频率k值不同，Δf为通过带通滤波器进行区分相邻频率的脉冲频率间隔；

超声波发射器每次发出频率为f_k的超声波脉冲持续时间为t_k，t_k大于超声波发射器发射超声波的起振时间t₁₀的2倍；

所述跟随者端的无线通信模块B接收被跟踪目标端的无线通信模块A发出的无线电信号；

所述跟随者端的无线通信模块B向被跟踪目标端的无线通信模块A发送的无线电信号包括：脉冲触发信号和状态查询信号；

所述跟随者端的无线通信模块B向被跟踪目标端的无线通信模块A发送脉冲触发信号的同时，处理器B对所有超声波接收器进行采样，采样频率为f_s，f_s≥2×f_k，采样时间为T_k，T_k≥2×t_k，以确保在所述跟随者和被跟踪目标距离范围内接收到被跟踪目标发出的超声波脉冲；

所述跟随者端固定的第i个超声波接收器在采样持续时间Δt_1ki后接收到被跟踪目标端超声波发射器所发出的超声波脉冲信号，所采样得到的超声波脉冲信号持续时间为St_ki，则对第i个超声波接收器来说有：t_k≤St_ki，且Δt_1ki+St_ki≤T_k；i≤n，对采样得到的St_ki时间内的超声波脉冲信号进行以f_k为中心频率、Δf为通带宽带的带通滤波，滤除接收到的附近其他被跟踪目标所发出的不同频率的超声波脉冲信号；

以f_k为中心频率、Δf为通带宽带的带通滤波在中心频率f_k造成的时间滞后为δt_k，以采集电压阈值v_t作为判断条件，则对第i个超声波接收器来说，若大于采集电压阈值v_t时刻为Δt_2ki，则Δt_1ki≤Δt_2ki，且Δt_2ki-δt_k为所述被跟踪目标端超声波发射器所发出的超声波脉冲在传输介质中飞行的时间；

对St_ki时间内采样得到的超声波脉冲信号进行快速傅里叶变换即FFT分析，记录在频率f_k处的幅值为At_ki；

对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，建立如下频率为f_k的超声波脉冲信号强度指示模型即RSSI模型R_i(d_i,θ_i)如下：

A_i＝R_i(d_i,α_i,β_i)       (1)

式中，A_i为第i个超声波接收器接收超声波脉冲信号的幅值，d_i为第i个超声波接收器与被跟踪目标端超声波发射器之间的距离，α_i和β_i分别为第i个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角和垂直方向夹角；

对第i个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α_i和垂直方向夹角β_i以及第j个超声波接收器与被跟踪目标端超声波发射器朝向的水平方向夹角α_j和垂直方向夹角β_j，满足下式：

式中，Δα_ij和Δβ_ij为常数；

对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，被跟踪目标和跟随者在恒定直线上运动且超声波发射器与超声波接收器正相对时，频率为f_k的超声波脉冲信号的RSSI模型R_i(x,α_i,β_i)有如下形式：

式中，a_i、b_i、c_i为第i个超声波接收器RSSI模型参数，并由离线实验数据拟合而得；

对频率为f_k的超声波脉冲信号RSSI模型，求解以下方程组获得已知测量超声波脉冲信号幅值时超声波发射器与超声波接收器之间的距离d_i、夹角α_i、夹角β_i，即：

对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，基于超声波脉冲信号RSSI模型的测量结果d_i和飞行时间t_ofi，通过选取神经网络NN_i和相应的离线训练数据，建立以下频率为f_k的超声波脉冲信号与距离之间的神经网络模型：

式中，r_i为经神经网络融合后的被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器之间的距离；

通过跟随者端固定的第i个超声波接收器在第k次测量所得到的RSSI模型距离d_ki和飞行时间Δt_2ki，求得跟随者与被跟踪目标端超声波发射器第k次测量的融合距离，即：

r_ik＝NN_i(d_ki,Δt_2ki)     (6)

对被跟踪目标端超声波发射器和跟随者端固定的第i个超声波接收器，被跟踪目标和跟随者在恒定直线上运动且超声波发射器与超声波接收器正相对时，则直接通过式(6)求得超声波发射器与超声波接收器之间的距离；

跟随者端固定的超声波接收器个数为2时，若被跟踪目标的超声波发射器与跟随者的两个超声波接收器安装在同一平面上且均在该平面上运动时，则在该平面上以跟随者端的超声波接收器为基准建立二维坐标系，通过求解式(7)方程组给出被跟踪目标的位置坐标(x,y)：

式中，(x₁,y₁)为所述跟随者端第1个超声波接收器的位置坐标，(x₂,y₂)为跟随者端第2个超声波接收器的位置坐标；

跟随者端固定的超声波接收器个数超过2时，即n≥3,则以跟随者端的超声波接收器为基准建立空间三维坐标系，通过求解式(8)方程组给出被跟踪目标的位置坐标(x,y,z)

式中，(x_i,y_i,z_i)为所述跟随者端第i个超声波接收器在空间三维坐标系中的位置坐标，i＝1,…,n。