一种室内运动标签定位和测速方法与流程

本发明属于电子通信领域，涉及一种室内运动标签定位和测速方法。主分类号为G01S5/06。

背景技术：

随着智能技术的发展,目前越来越多的移动式机器人应用于餐厅服务、流配送等领域。机器人的导航技术是智能机器人领域的一个重要研究方向，而位置和速度信息又是导航的基本要素。

因为速度是一个矢量信息，目前主流的室内动标签的测速方法主要为在标签上加入额外的速度或者加速度传感器来计算目标速度，加入额外的电子罗盘或方向传感器来确定方向，比如专利申请号201610321161.X《室内导航方法及装置》利用陀螺仪和方向传感器进行实时卡尔曼滤波获取融合导航信息；专利申请号201521043024.X《一种基于UWB技术的室内机器人自主定位系统》，利用UWB坐标终端通过自带的电子罗盘计算出当前机器人的航向角；专利申请号201510121270.2《一种腰带式可穿戴室内移动定位终端》利用惯性导航模块的加速度计和角速度计各自感知加速度和角速度分别进行积分速度和角度信息。这种方式测速方式，增加了标签的成本，增大了标签体积和复杂度。另一种对室内运动标签的定位方法为利用定位系统得到的航迹信息做航迹预测处理，比如专利申请号201310469003.5《一种室内移动机器人定位方法》利用超声波定位获得不同时刻的绝对距离信息和航迹推算得到相对距离信息实现对运动机器人的定位；这种方法由于定位精度不高导致距离信息误差大，得到的航迹信息误差也比较大，另外这并不是一个实时测速方法，获取的速度信息也是不准确的。

目前，室内标签定位一般都采用专门的高精度室内定位系统，其中线性调频连续波是一种常用的信号形式，目前主要相关技术文献有：文献1，《基于线性调频信号的无线定位系统设计与实现》（郑州大学，武霄泳，硕士论文）；文献2，专利《一种基于线性调频连续波技术精确定位及设备》（专利申请号201310538331.6）；文献3，专利《室内定位装置及方法》（专利申请号：201410416423.1）。前两个文献采用的是线性调频连续波信号体制，但他们都是距离测量系统，标签需要分时与各个基站距离测量，另外在测距时基站和标签（文献2，设备A与设备B）也需要分时双向测距，这些非实时性都给运动标签带来了定位和测速误差。文献3是一个距离差系统，定位精度和实时性都要优于前两种，但是对于运动标签，单一的正扫频或者下扫频无法解决速度距离耦合现象从而造成定位误差。

技术实现要素：

针对现有室内定位系统中对运动目标定位和测速精度不足，需要附件额外的速度或加速度以及方向传感器的问题，本发明提出了一种室内运动标签的定位与测速方法。

本发明公开了一种室内运动标签的定位与测速方法一种室内运动标签定位和测速方法,具体包括如下步骤：

步骤1：待测标签与基站同步产生线性调频连续波信号；所述线性调频连续波信号的调制方式为三角波，对各基站得到的数字中频信号按正、负扫频分别做FFT处理；

步骤2：对FFT处理结果做CFAR判决，得到正负扫频对应的正中频频率f₊和负中频频率f_-；

步骤3：利用步骤2得到的正中频频率f₊和负中频频率f_-解算出待测标签相对各基站的伪距R，以及待测标签相对各基站的速度分量v_i；下标表示不同基站；

步骤4：将步骤3得到的伪距两两作差得到基站间真实距离差，计算出标签的位置；

步骤5：将步骤4得到的标签位置计算出标签相对于各基站的角度；

步骤6：利用步骤3计算出的速度分量和步骤5得出的角度解算出标签的速度大小和方向。

具体的,所述步骤2中解算标签相对于各基站的速度分量：

v_i= c（f₊ + f_- - 2f_IF）/2f₀

其中f₀为该基站的载波起始频率，f_IF为该基站和标签调频连续波信号的起始频率差。

具体的,所述步骤3中伪距

R= c（f₊ - f_-）/2µ

其中c为光速，µ为连续波的调频斜率。

具体的,所述步骤5标签相对于各基站的角度计算方法如下：

建立XY二维坐标系，以x轴正方向为参考，其中基站坐标为（x₀,y₀）,由所述步骤4得到的标签的坐标为（x,y），标签相对于基站与x轴正方向的夹角为α₀：

α₀=arctan((y-y₀)/(x-x₀))。

具体的,所述步骤6中解算标签运动速度和方向的具体方法如下：

任选所述步骤3中的得到的两个速度分量v_a、v_b，计算出v_a与v_b的夹角α，选出速度较大的速度分量，这里设，标签与v_b的夹角记作θ，偏向速度v_a为正方向；

标签的运动角度θ为：

标签速度：

v=v_b/cos(θ)

进一步的,所述夹角α的计算方法为：

若v_a>0，v_a与x轴正方向的夹角α_a=α₁，v_a<=0，v_a与x轴正方向的夹角α_a=α₁-π；

若v_b>0，v_b与x轴正方向的夹角α_b=α₂，v_b<=0，v_b与x轴正方向的夹角α_b=α₂-π；

则α=α_b-α_a；

α₁与α₂分别为v_a与v_b对应的基站与x轴正方向的夹角。

具体的,所述步骤4中得到基站间真实距离差后，利用双曲定位原理计算出标签的位置信息。

优选的,所述步骤1中基站得到数字中频信号为基站将线性调频连续波经过混频、滤波、放大后得到。

本发明的有益之处在于只利用定位信道实现室内运动标签的位置和速度解算，具有高精度和实时性的特点，可以直接应用于运动标签的导航。

附图说明

图1是能够实现本发明所述方法的一种硬件系统的具体实施方式示意图；

图2是图1中的标签、基站、控制器的一种具体实施方式结构示意图；

图3是本发明的一个完整工作流程具体实施方式示意图；

图4是本发明所述速度解算方法示意图；

图5是实施例1中定位区域、定位装置分布示意图；

图6是实施例1中标签位置解算结果示意图。

具体实施方式

下面结合附图，对本发明的具体实施方式作进一步的详细说明。

本发明采用现有的测距硬件系统实现，特别是在中国专利《室内定位装置及方法》（专利申请号：201410416423.1）中已公开的室内定位装置。

如图1至3所示，给出了本发明所述室内运动标签的定位和测速方法所适用的室内定位装置的具体实施方式。

图1为本发明所适用的一室内定位装置结构示意图，所述定位系统装置由控制器、至少三个不共线的基站和若干定位标签组成。所述控制器和标签设置通信模块，所述控制器具有数字信号处理功能，所述控制器与各基站通过有线或无线方式控制同步信号。

图2为图1中控制器、基站和标签的具体结构图。所述控制器包含同步控制模块，ZigBee模块、和信号处理模块，控制器的同步控制模块通过有线连接各基站的MCU；所述基站由天线、放大器、混频器、线性调频发生器、滤波放大器、ADC，基站ADC采集到的数据通过有线传送给数字信号处理器，所述基站接收到标签发射的线性调频信号流向为天线、放大器、混频器、滤波放大器、ADC；所述标签由天线、线性调频发生器，标签MCU和ZigBee模块组成，所述标签MCU通过ZigBee模块与控制器同步后，控制线性调频发生器产生线性调频信号，经过天线发送出去。

图2的具体实施方式中，以MCU控制通信信号的同步；以上控制器中的ZigBee模块为控制器通信模块，标签中的ZigBee模块为标签通信模块，ZigBee是基于IEEE802.15.4标准的低功耗局域网协议，是一种短距离、低功耗的无线通信技术，为本领域技术人员公知。

如图3所示给出本发明的一个完整实施方式流程示意图，具体流程如下所述：

待测标签与基站同步产生线性调频连续波信号。所述线性调频连续波信号的调制方式为三角波，带宽为B，调频周期为T，同步误差为τ₀。

所述各基站的本振信号如下：

（6）

其中，t表示以时间为变量，T_up表示正程扫频阶段时间，T_down表示负程扫频阶段时间；A₀表示本振信号幅度；其中f_S为该基站的载波起始频率；f_H=f_S+B；发射信号的调频斜率为µ=2B/T。

待测标签发射的信号如下：

（7）

其中，T_up表示正程扫频阶段，T_down表示负程扫频阶段，标签载波起始频率为f₀，标签载波终止频率为f₁=f₀+B, A₀表示发射信号幅度，发射信号的调频斜率为µ=2B/T。

各基站经过混频、滤波、放大后得到中频信号为：

（8）

和表示中频信号的固定相位部分，记作：

（9）

以系统中有A、B、C三个基站为例，若为A基站，f_IF=f_A- f₀，Δτ=τ_a-τ₀，f_d=f_da，τ_x=τ_a；其中，τ_a=R_a/c表示待测标签信号到达基站A的传播时延，f_da为待测标签相对于A基站产生的多普勒频率，γ=γ_a表示待测标签接收到A基站信号的衰减系数。若为B基站f_IF=f_B- f₀，Δτ=τ_b-τ₀，f_d=f_db，τ_x=τ_b；其中，τ_b=R_b/c表示标签信号到基站B的传播时延, f_db为待测标签相对于B基站产生的多普勒频率，γ=γ_b表示标签接收到B基站信号的衰减系数。若为C基站f_IF=f_C- f₀，Δτ=τ_c-τ₀，f_d=f_dc，τ_x=τ_c；其中，τ_c=R_c/c表示标签信号到基站C的传播时延，f_dc为待测标签相对于C基站产生的多普勒频率，γ=γ_c表示标签接收到C基站信号的衰减系数。所述c为光速，R_a、R_b、R_c分别为标签到基站A、B、C的距离。

步骤1中对各基站得到的数字中频信号按正、负扫频分别做FFT（（Fast Fourier Transformation，即快速付利叶变换）处理后，进入步骤2。

步骤2：对FFT处理结果做CFAR判决，得到正负扫频对应的中频频率f₊和f_-。

（10）

其中µ为线性调频连续波的调频斜率，f_IF为基站和标签调频连续波信号的起始频率差，f_d为待测标签相对于基站产生的多普勒频率。CFAR即Constant False-Alarm Rate，恒虚警率检测是雷达目标检测的常用判定方法。

步骤3：利用步骤2得到f₊和f_-解算出待测标签相对各基站的伪距，以及待测标签相对各个基站的速度分量，计算公式为

R= c（f₊ - f_-）/2µ （11）

v= c（f₊ + f_- - 2f_IF）/2f₀ （12）

步骤3中解算出的速度分量包含正负符号，其中正号代表标签相对于基站速度分量的方向朝向基站的方向，负号代表标签相对于基站速度分量的方

向朝向基站的反方向。

步骤4：将步骤3得到的伪距两两作差得到基站间真实距离差，计算出标签的位置信息。可以利用双曲定位原理进行计算，利用双曲定位原理计算前应先对定位系统建立坐标系，对于二维平面的定位，首先选取定位区域里的一点作为坐标原点，在选取x轴和y轴建立二维平面坐标系，解算出标签的坐标为（x,y）。

步骤4中基站间的真实距离差包含正负符号，利用距离差的正负来筛选双曲线交点。

步骤5：将步骤4得到的标签位置计算出标签相对于各基站的角度。根据标签位置，结合各个基站的位置，容易得到该待测标签相对于各个基站的角度。例如以下给出一个具体算法：

以x轴正方向为参考，其中基站的坐标为（x₀,y₀）,步骤4中得到的待测标签坐标为（x,y），则标签相对于基站与x轴正方向的夹角：

α₁=arctan((y-y₀)/(x-x₀))。 （13）

步骤6：利用步骤3计算出的速度分量和步骤5得出的角度信息解算出标签的速度大小和方向。

下面给出步骤6中计算标签速度大小和方向的一种具体实施方式：

任选步骤3中的得到的待测标签相对A,B两个基站的速度分量v_a、v_b，下标分别表示基站A和B，根据步骤5得到待测标签相对于各基站的角度计算出v_a与v_b的夹角α。选出速度较大的速度分量，假设，标签与v_b的夹角记作θ，速度v_b为参考,偏向于v_a的方向为正方向。图4中分别给出了两种θ与速度分量v_a、v_b的空间关系示意图，其中a部分表示θ>0的一个示例，b部分表示θ<0的一个示例。

则标签的角度信息θ为：

（14）

标签速度大小为：

v=v_b/cos(θ) （15）

本发明所述夹角α的计算方法为：判断v_a与v_b的正负，

若v_a>0，v_a与x轴正方向的夹角α_a=α₁，v_a<=0，v_a与x轴正方向的夹角α_a=α₁-π；

若v_b>0，v_b与x轴正方向的夹角α_b=α₂，v_b<=0，v_b与x轴正方向的夹角α_b=α₂-π；

则α=α_b-α_a。

其中α₁和α₂分别为步骤5中得到的该待测标签相对基站的角度。

最后可以由图2所示的控制器将解算得到的待测标签位置和速度信息通过通信链路发送。

以下给出本发明的一个具体实施例

本实施例中，采用如图2所示结构的室内定位装置，包括待测标签、基站、控制器。所述标签、基站、控制器结构功能图如图5所示。标签与控制器通信采用ZigBee模块，同时该通信信道也是粗同步信道，其中控制器中的同步控制模块采用直接产生本振信号通过等长传输线传输至各基站的方式来进行基站同步。所述标签具有线性调频连续波发射功能，可以控制线性调频连续波的发射时刻、起始频率和调频斜率。所述基站包括基站天线、基站放大器、线性调频产生器、混频器、滤波放大器、ADC；控制器结构包括同步控制模块、ZigBee模块和信号处理模块。

本实施例中是在一个边长为100米的正方形房间，设置3个基站，位置分布如图5中基站A、基站B、基站C所示，以A基站为坐标原点，以房间两条边为x、y轴建立直角坐标系。待测标签在位置坐标为（40m，75m），速度大小为3.8m/s，速度方向延x轴正向夹角为103度。

待测标签和控制器粗同步之后，控制器通过有线方式控制三个基站完全同步，基站，控制器约定标签和各个基站在同一时刻控制线性调频连续波产生器产生线性调频连续波。标签线性调频连续波调制周期T=5ms，标签载波起始频率f₀=5.2GHz，调频带宽B=500MHz。

各基站本振信号的线性调频连续波调制周期T=5ms，载波起始频率f_s=5.201GHz,即f_A= f_B= f_C=5.201GHz，调频带宽B=500MHz，其中，f_IF=1MHz，中频信号采样率f_c=4MHz，FFT点数通过补零增加为N=2¹⁸，信噪比10dB。

各个基站经过混频、滤波、放大、ADC采样得到数字中频信号，各基站将得到的数字中频信号传送给控制器，控制器的数字信号处理器对所述数字中频信号解算得到位置和速度信息。

由步骤4解算得到的距离差结果如下：

标签到基站B与标签到基站A的距离差为-37.83（米）

标签到基站C与标签到基站B的距离差为17.83（米）

标签到基站C与标签到基站A的距离差为-20.00（米）

由步骤4得到标签的坐标为（39.94,75.07），定位误差为0.088（米）定位结果如图6所示。图6中横，纵坐标分别为X、Y坐标，三条不同轨迹的交点为最终得到的标签坐标。

由步骤3解算得到标签相对于各个基站的速度分量如下：

待测标签相对于A基站的速度分量为：-2.87m/s

待测标签相对于B基站的速度分量为：2.67m/s

待测标签相对于C基站的速度分量为：0.64m/s

由步骤5解算标签到各个基站与x轴正方向的夹角为：A基站241.91度、B基站148.02度、C基站22.62度。

由步骤6解算到标签的速度大小为3.79m/s，与x轴夹角为102.74度，速度误差为0.01m/s，角度误差为0.26度。

前文所述的为本发明的各个优选实施例，各个优选实施例中的优选实施方式如果不是明显自相矛盾或以某一优选实施方式为前提，各个优选实施方式都可以任意叠加组合使用，所述实施例以及实施例中的具体参数仅是为了清楚表述发明人的发明验证过程，并非用以限制本发明的专利保护范围，本发明的专利保护范围仍然以其权利要求书为准，凡是运用本发明的说明书及附图内容所作的等同结构变化，同理均应包含在本发明的保护范围内。