一种基于UWB的跟随系统及方法与流程

本发明涉及智能跟随技术领域，具体涉及一种基于uwb的跟随系统及方法。

背景技术：

智能跟随产品为人们的生活带来了更多的便利，如超市中的跟随推车、智能跟随行李箱、智能跟随婴儿车等。

目前的智能跟随技术主要有基于定位标签和基于机器视觉两种。其中，基于定位标签的智能跟随技术，则于成本低、算法简单，应用最为广泛。

中国发明专利cn105828431a公开了一种基于uwb的自主跟随机器人定位方法及系统，包括uwb基站和uwb定位标签，其中uwb基站设置为三个，且距uwb定位标签的分别为d1、d2、d3，建立坐标系，根据d1、d2、d3三角关系获得uwb定位标签的x、y位置坐标，进而求解其距离和偏角，具体的方法为：uwb基站定时发送uwb信号，uwb定位标签接收信号，且标签与基站时间同步，利用toa算法，根据uwb信号在基站和标签之间的传播时间t，由d＝c*t，c为光速,得到三个uwb基站分别于uwb定位标签的距离am,bm,cm为d1,d2,d3，根据已知的ab,bc,ac的距离以及am,bm,cm的距离可以确定一个交点，这个交点是分别以a,b,c为圆心，d1,d2,d3为半径的三个球体的交点，该交点就是uwb定位标签所在的位置；定义三个uwb基站位置距离ab,bc,ac均为a；定义bc边上的高所在延长线为y轴，y轴正向与机器人自身的方向一致；三个uwb基站构成的等边三角形的边ab的中点为o(0,0,0)，标签的坐标为m(x,y,z),其中z为标签到基站所在水平面的高度；等边三角形的中心为h；根据三个uwb基站到uwb定位标签的距离，通过解析几何算出：标签m在等边三角形所在平面的投影m’点到等边三角形中心的距离：标签m在等边三角形所在平面的投影m’点与等边三角形的中心h点的连线，即m’h与y轴的夹角即方位角：由上述步骤得到的(l,θ)即可确定特定目标的具体所在位置信息。该方案的实现，需要三个uwb基站和一个uwb定位标签，成本较高。

有鉴于此，急需对现有的基于uwb的跟随系统进行改进，减少uwb基站的数量，降低跟随系统的成本，同时，简化跟随算法。

技术实现要素：

本发明所要解决的技术问题是现有的跟随系统存在的成本高和对跟随目标的距离和偏角计算不够简洁精准的问题。

为了解决上述技术问题，本发明所采用的技术方案是提供一种基于uwb的跟随系统，包括：

第一uwb发射接收模块，设置在被跟随的目标物体上，所述第一uwb发射接收模块上设有uwb标签；

第二、第三uwb发射接收模块，水平间隔地设置在跟随物体上，根据所述uwb标签获得与所述第一uwb发射接收模块之间的距离l2和l3；

计算单元，根据所述uwb标签获得与所述第一uwb发射接收模块之间的距离l2和l3，计算得到所述跟随物体与所述第一uwb发射接收模块之间的距离和夹角；

控制单元，根据所述跟随物体与所述第一uwb发射接收模块之间的距离和夹角，控制设置在所述跟随物体上的驱动装置，使所述跟随物体朝向所述目标物体，并跟随所述目标物体的移动速度、距离和转动角度。

在上述方案中，所述第一uwb发射接收模块上设有：

速度传感器，用于实时获取被跟随的所述目标物体的速度；

三轴加速度传感器，用于实时获取被跟随的所述目标物体的加速度；

所述控制单元，根据被跟随的所述目标物体的速度、加速度以及所述跟随物体与所述第一uwb发射接收模块之间的距离，控制所述跟随物体加速、减速或匀速，使所述跟随物体与所述目标物体保持设定的距离。

在上述方案中，所述第一uwb发射接收模块包括id芯片，所述id芯片存储有唯一的地址信息，所述跟随物体上存储有启动跟随的id编码以及用于读取所述id芯片的无线射频读卡器，所述控制单元根据读取到的所述id芯片与存储的所述id编码的匹配结果，启动或不启动所述驱动装置。

所述目标物体为穿戴设备，所述跟随物体为装载设备。

在上述方案中，所述第一、第二和第三uwb发射接收模块的工作频率为6.5ghz。

本发明还提供了一种基于uwb的跟随系统的跟随方法，包括以下步骤：

根据uwb测距原理，得到第二、第三uwb发射接收模块与第一uwb发射接收模块之间的距离，分别为l2和l3；

定义x轴与第二、第三uwb发射接收模块的连线平行，定义y轴上任意点到第二、第三uwb发射接收模块的距离相等，且y轴与x轴相交垂直，定义y轴与x轴构成的平面为参考面，参考面与水平面平行，根据l2和l3的大小，判断第三uwb发射接收模块在参考面上的投影在y轴的左侧、右侧或落在y轴上；

定义第三uwb发射接收模块在参考面上的投影与x轴和y轴交点的连线为d,根据三角关系，得到d的长度，以及d与y轴的夹角；

控制单元先控制跟随物体转动，转动角度为α，使d与y轴重合，再控制跟随物体朝向被跟随的目标物体移动指定距离。

在上述方案中，根据l2与l3比较的结果，求解α和d，方法如下：

如果l2＜l3，获得l2与x轴的夹角θ，求解的公式为：

根据夹角θ获得d的长度，求解的公式为：

根据d的长度获得d与x轴所成的钝角β1，求解的公式为：

根据角β1获得d与y轴的夹角α，求解的公式为：

α＝β1-90°；

如果l3＜l2，获得l3与x轴的夹角θ，求解的公式为：

根据夹角θ获得d的长度，求解的公式为：

根据d的长度获得d与x轴所成的锐角β2，求解的公式为：

根据角β2获得d与y轴的夹角α，求解的公式为：

α＝90°-2；

如果l2＝l3，则α＝0°，获得d的长度，求解的公式为：

在上述方案中，

每一套跟随系统中的两个uwb模块和一个标签各自都有独立地址，在第一uwb发射接收模块与第二、第三uwb发射接收模块信息传输前，进行地址检测，如果是指定的地址，才会进行传输信息，进行相互测距；

第一uwb发射接收模块上设有全球唯一id芯片，跟随物体上存储有启动跟随的id编码以及用于读取id芯片的无线射频读卡器，根据读取到的id芯片与存储的id编码的匹配结果，启动或不启动驱动装置。

在上述方案中，第一uwb发射接收模块通过速度传感器和三轴加速度传感器检测目标物体的移动信息，并将该移动信息传递给第二、第三uwb发射接收模块，通过控制单元，控制跟随物体加速、减速或匀速，使跟随物体与目标物体保持设定的距离。

与现有技术相比，本发明，通过第二、第三uwb发射接收模块实现对第一uwb发射接收模块距离和角度的测量，相对现有的定位跟随方案(基于uwb、wifi、蓝牙、zigbee模块等)来说，算法精简高效，而且测量可靠，跟随精度高，此外，降低了成本，具有较好的用户体验。

附图说明

图1为本发明中跟随模型的示意图；

图2为本发明中第一uwb发射接收模块的示意图；

图3为本发明中第二或第三uwb发射接收模块的示意图；

图4为本发明中具体实施例一的示意图；

图5为本发明中具体实施例二的示意图；

图6为本发明中具体实施例三的示意图。

具体实施方式

下面结合说明书附图和具体实施方式对本发明做出详细说明。

如图1～3所示，本发明提供了一种基于uwb的跟随系统，包括第一、第二和第三uwb发射接收模块，第一uwb发射接收模块设置在被跟随的目标物体上，第二、第三uwb发射接收模块设置在跟随物体上，且水平设置，第二、第三uwb发射接收模块用于实现对第一uwb发射接收模块与跟随物体之间的距离和夹角的测量，并根据该测量结果实现实时跟随。

本发明的第一uwb发射接收模块和第二、第三uwb发射接收模块是基于uwb模块进行改良的，其集成现有的uwb模块、stm32f103单片机、测速传感器模块、三轴加速度模块，并内置改良天线、flash内存和电路。其中，第一uwb发射接收模块上设有uwb标签，本发明还包括计算单元和控制单元，计算单元根据uwb标签获得第二、第三uwb发射接收模块与第一uwb发射接收模块之间的距离l2和l3，并计算得到跟随物体与第一uwb发射接收模块之间的距离和夹角。控制单元根据跟随物体与第一uwb发射接收模块之间的距离和夹角，控制设置在跟随物体上的驱动装置，使跟随物体朝向目标物体，并跟随目标物体的移动速度、距离和转动角度。

stm32f103单片机和第二、第三uwb发射接收模块通过spi方式编程通信。这样改良过的第二、第三uwb发射接收模块，可以独立工作，通过编程来实现第二、第三uwb发射接收模块与第一uwb发射接收模块之间的测距、传输信息。控制单元为stm32f103单片机，第一uwb发射接收模块和第二、第三uwb发射接收模块包括id芯片，id芯片(rfid标签)存储有唯一的地址信息。第二、第三uwb发射接收模块上设有无线射频读卡器。第一uwb发射接收模块和第二、第三uwb发射接收模块之间在通讯过程中遵守自定义的协议。自定义的协议：通信前，先检测另一个模块发送的地址，如果是指定的地址(每一套跟随设备中的第二、第三uwb发射接收模块和第一uwb发射接收模块都有独立地址，而且只有同一套设备中的模块允许通信)，才会进行传输信息，进行相互测距。不同设备的地址不同，不能进行通信。避免造成设备错跟指定的对象，由此实现专属跟随。

每一套跟随系统中的两个uwb模块和一个标签各自都有独立地址，在第一uwb发射接收模块与第二、第三uwb发射接收模块信息传输前，进行地址检测，如果是指定的地址，才会进行传输信息，进行相互测距；第一uwb发射接收模块上设有全球唯一id芯片，跟随物体上存储有启动跟随的id编码以及用于读取id芯片的无线射频读卡器，根据读取到的id芯片与存储的id编码的匹配结果，启动或不启动驱动装置。

由于现有的uwb模块的工作频率在3.1ghz～10.6ghz(分频段)，考虑到其他设备(wifi、蓝牙、nb-iot、zigbee等)的频率干扰，本发明采用的改良后的第一、第二和第三uwb发射接收模块的工作频率设定在6.5ghz,提高了抗干扰性。本发明中，目标物体可以为穿戴设备，跟随物体可以为装载设备。例如，穿戴设备可以为手环，装载设备可以为旅行箱、购物车等，可避免人手推拉，解放了人的双手，为生活提供了便利。

本发明，相对于通过雷达技术等实现跟随和定位的系统，所提供的跟随系统其核心硬件是三个uwb发射接收模块，与其他的跟随设备相比，具有较低的成本，可以为更多的科技物品爱好者带来较好的使用体验。

如图4～6所示，本发明还提供了一种基于uwb的跟随方法，采用上述的一种基于uwb的跟随系统，包括以下步骤：

建立跟随模型，第二、第三uwb发射接收模块分别标记为uwba和uwbb，第一uwb发射接收模块标记为uwbc，使两个第二、第三uwb发射接收模块之间的距离为l，根据uwb测距原理，获得第二、第三uwb发射接收模块与第一uwb发射接收模块之间的距离，分别为l2和l3。建立坐标系，该坐标系包括x轴和有y轴，令x轴与第二、第三uwb发射接收模块的连线平行，y轴上任意一点到第二、第三uwb发射接收模块的距离相等，y轴与x轴相交与o点，y轴与x轴相互垂直，定义y轴与x轴构成的平面为参考面，参考面与水平面平行。令第一uwb发射接收模块在参考面上的投影与o点的连线为d，d与y轴的夹角为α，在实际跟随过程中，第一uwb发射接收模模块落在y轴意外的区域时，会使d与x轴形成钝角θ，当第一uwb发射接收模模块落在y轴上，则θ为90度，夹角为α为零度。

在跟随动作前，先对第一uwb发射接收模块在参考面上的投影位置进行判断，判断其在y轴左侧、右侧或落在y轴上，而后根据判断结果采用不同的计算方法，下面结合具体的实施例分别说明。

具体实施例一：

第一uwb发射接收模块在参考面上的投影位于y轴左侧时：根据余弦定理，计算出∠θ：

根据余弦定理，计算出d：

根据余弦定理，计算出∠β1：

根据反余弦公式，得到∠β1＝arccos(m)；

根据三角形关系，得到∠α＝∠β1-90°；

化简得到：

具体实施例二：

第一uwb发射接收模块在参考面上的投影位于y轴右侧时：根据余弦定理，计算出∠θ：

根据余弦定理，计算出d：

根据余弦定理，计算出∠β2：

根据反余弦公式，得到∠β2＝arccos(m)；

根据三角形关系，得到∠α＝90°-∠β2；

化简得到：

具体实施例三：

第一uwb发射接收模块在参考面上的投影落在y轴上时：

根据三角形关系，得到∠α＝0°，

第一uwb发射接收模块通过速度传感器和三轴加速度传感器检测目标物体的移动信息，并将该移动信息传递给第二、第三uwb发射接收模块，通过控制单元，控制跟随物体加速、减速或匀速，使跟随物体与目标物体保持设定的距离。

在述步骤中，第二、第三uwb发射接收模块与第一uwb发射接收模块信息传输前，进行地址检测。

本发明，通过第一uwb发射接收模块和第二、第三uwb发射接收模块，可每秒对目标物体的位置完成10次更新，计算目标物体的位置误差小于15cm。这样跟随物体就可以精准地跟随着目标物体移动，通过第二、第三uwb发射接收模块的精准测距技术，可使测距误差小于10cm，优选的，当目标物为手环，跟随物体为旅行箱时，通过连续的数据更新，可以精准地测出用户与旅行箱之间的距离，具有较高的安全性和便利性。

为更好的适应旅行箱、购物车等物品的使用习惯，本发明可实现变速跟随，以旅行箱为例：本发明会根据用户与旅行箱之间地距离、用户的速度和旅行箱的当前速度，对会对旅行箱的跟随速度做出适应性调整，具体的，在第一uwb发射接收模块上，通过速度传感器和加速度传感器实时监测用户的行走速度，通过第一、第二和第三uwb发射接收模块通信，将用户的行走速度和加速度信息传输至第二、第三uwb发射接收模块。同时，在旅行箱上也有速度传感器，测出旅行箱轮子的速度。通过对比用户的行走速度和旅行箱轮子的速度，来检判断出旅行箱此时应该加速、减速或匀速。判断依据：如果用户的行走速度>旅行箱轮子的速度,则旅行箱可以适当加速；如果用户的行走速度<跟随物体(旅行箱)轮子的速度,则旅行箱可以适当减速，优选的，加速和减速可以设置相应的等级，例如，可根据旅行箱与用户之间的距离设置分为3个等级，当两者的距离到达一定的阈值时，采用一级加速和减速。两者的距离到达一定的阈值时，采用二级加速和减速等。同时，通过加速度传感器还可实时监测用户行走的加速度和旅行箱的加速度，将旅行箱的加速度和用户的加速度对比，结合两者的距离，控制旅行箱的加速度增大或减小，可使旅行箱与用户之间实时保证一定的相对距离，使旅行箱跟随在用户身边，保证旅行箱的安全。

与现有技术相比，本发明，通过第二、第三uwb发射接收模块实现对第一uwb发射接收模块距离和角度的测量，相对现有的定位跟随方案(基于uwb、wifi、蓝牙、zigbee模块等)来说，算法精简高效，而且测量可靠，跟随精度高，此外，降低了成本，具有较好的用户体验。

本发明并不局限于上述最佳实施方式，任何人应该得知在本发明的启示下做出的结构变化，凡是与本发明具有相同或相近的技术方案，均落入本发明的保护范围之内。