一种基于UWB和TDOA算法的智能机器人定位方法及定位系统技术方案

本发明专利技术公开了一种基于UWB和TDOA算法定位的智能机器人定位方法，将UWB锚节点、UWB Tag节点、UWB中心控制节点组成一个UWB通信网络，位置数据的计算是由后台服务器进行计算，机器人与后台进行通信，获知目标对象上UWB Tag节点的位置数据，从而机器人根据目标对象的位置数据进行目标跟随；当机器人跟丢了目标对象时，机器人通过获知目标对象的位置数据，规划一条符合条件的可达路径，机器人根据这条路径导航到目标对象所在位置，本发明专利技术同时还公开了一种基于UWB和TDOA算法的智能机器人的定位系统，解决了现有技术中智能机器人在大场景、少特征的环境中难以进行有效定位的问题。

【技术实现步骤摘要】
一种基于UWB和TDOA算法的智能机器人定位方法及定位系统
本专利技术涉及一种UWB(一种无载波通信技术，利用纳秒至微秒级的非正弦波窄脉冲传输数据)和激光SLAM(即时定位与地图构建，或并发建图与定位)的相结合的机器人跟随及找人的方案，具体涉及一种基于UWB和TDOA算法的智能机器人定位方法及定位系统。
技术介绍
在现有的大型商场中，对机器人的需求越来越强烈，对于消费者来说，需要一个机器人助手来协助在超市中的购物。在现有的跟随解决方案中，大多数是基于视觉的方法来进行人体跟随，这种方法很难跟随特定的人，且容易跟丢目标，而且视觉受环境的影响大，测量距离相对比较近。当前，随着移动机器人技术的发展，越来越多的移动机器人产品进入消费品市场和商用市场。在商用市场领域，移动机器人主要用来导引、导购、讲解、介绍、迎宾、物品输送等。移动机器人作为一种移动平台，可移动性是其基本的功能。而移动机器人的定位功能则是移动机器人实现自主化的关键技术之一，移动机器人只有了解了自己在当前环境中所处的位置，才能决定下一步要去什么地方？和怎么样到达目的地？当前移动机器人的定位方法有很多中，诸如：WIFI定位、RFID定位、UWB定位、视觉定位、iBeacon定位、无线激光测距传感器定位、激光SLAM定位和超声波定位；这些方法各有各的有点，各有各的缺点；其中在大规模场景中，单纯靠视觉和激光SLAM定位进行定位很难取得很好的效果，需要与UWB定位进行融合定位。专利CN106211083A提出了的是一种基于UWB的手机室内定位方案，主要意图是搭建一套手机定位系统，通过这套系统可以查询在此区域内的所有标签的位置。专利CN105929365A公开了一种UWB高精度定位系统及定位方法，解决目前的定位系统同步方式复杂，同步难度大，且同步精度不可避免地受到影响的问题。但是目前，将UWB主要用于人员和目标物体的定位方面，没有将UWB和激光SLAM结合起来，用于大场景中机器人跟随和找人的应用，因此，本专利技术将UWB的定位首次应用于该领域。
技术实现思路
本专利技术目的是提供一种基于UWB和TDOA算法的智能机器人定位方法及定位系统，解决现有技术在大场景、少特征的环境中，进行有效定位的问题。本专利技术解决技术问题采用如下技术方案：一种基于UWB和TDOA算法的智能机器人定位方法，包括：A、智能机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和激光测距传感器数据进行自身定位，同时建造激光SLAM地图；B、标定UWB锚节点相对激光SLAM地图的位置；C、将UWB锚节点、UWBTag节点、UWB中心控制节点组成一个UWB通信网络，通过TDOA算法部署目标区域的Tag节点，并根据UWB通信网络定位智能机器人的自身位置，通过IMU传感器获取智能机器人的姿态数据；D、再次通过激光SLAM雷达获取机器人自身位置；E、将上述步骤C和步骤D中的定位数据进行融合；F、通过UWB通信网络获取目标Tag节点的位置；G、规划智能机器人的跟踪路径；H、驱动底盘到达目标对象的位置并跟随目标对象，通过判断，如果目标对象跟丢了，则回到步骤F，再次通过UWB通信网络获取目标Tag节点的位置，如果目标对象没跟丢，则继续跟随目标对象。可选的，在根据TDOA算法组网的过程中，根据目标部署场景的大小，划分成各个不同的子网；如果目标部署场景比较小，那单子网就可以完成所要求的位置定位服务；如果目标部署场景比较大，那么则需要多个子网才能完成所要求的位置定位服务，其中，在构建单子网网络的时候，控制锚节点位于部署于目标空间的中心位置，相对位置设置为(0,0)，锚节点位置相对控制锚节点的位置已知，标签Tag节点位置未知，位于目标部署空间的任意位置，控制锚节点控制目标部署空间的时间同步，控制锚节点同时还汇集标签Tag节点时间测量信息，并依据TDOA定位算法对标签Tag进行位置估计计算，控制锚节点将定位到的每个Tag节点的位置值通过网络发送给后台服务器进行位置显示、跟踪、查询、统计等工作，标签Tag节点可通过WIFI后台服务器获取自己本身的位置信息；在构建多子网UWB网络的时候，对于目标部署空间比较大的地方，采用部署多个子网来覆盖整个目标部署空间，各子网定位出来的各标签Tag节点的位置数据，通过位置转移矩阵转换成地图空间的全局位置，且计算服务由后台服务器完成。对于各控制锚节点可不需要进行时间同步。可选的，在构建多子网UWB网络的时候采用双阶加权最小二乘解算法，通过增加一个虚拟变量来线性化TDOA方程，假设在子网中有M个锚节点，其中有一个控制锚节点位于子网的中心原点位置，M个锚节点的位置分别为(xi，yi)，其中i＝1,…,M，目标Tag节点的位置为(x，y)，ri表示锚节点i到Tag节点之间的距离，ri1表示锚节点i到控制锚节点之间的距离，其中：ri1＝cti1＝ri-r1,i＝2,3,…,M……………(2)ri1表示锚节点i与控制锚节点之间的距离，ti1表示脉冲超声波信号传递到锚节点i和控制锚节点之间的到达时间差。将式(2)代入式(1)可得：其中i＝2,3,…,M，因为控制锚节点位于子网原点位置，所以在方程(3)中，ri1为已知量，xi，yi为已知量，因此方程(3)是有关x，y，r1三个参数的线性方程，但是因为r1是跟Tag相关的非线性变量，所以方程(3)是一组非线性方程。为了从非线性方程组中求解出x，y，那么首先假设x，y，r1三个变量为相互独立的变量，然后可以通过加权线性最小二乘法来求解，这是第一阶段求解；第二阶段求解还原为x，y，r1是非线性相关的变量，但是其满足方程(4)，因此可以通过另外一个线性最小二乘法来求解，通过这两阶段求出最终的Tag节点的位置，其中，(I)第一求解阶段假设ui＝[x，y，r1]T，因为TDOA算法是有噪声的，所以方程(3)的误差方程如下：ε1＝h-G1u1………………….(5)其中ui的加权LS解是使得ε1误差最小时的解，即：这里Q为TDOA的M×M的协方差矩阵。(II)第二求解阶段使得方程(4)的方差最小，同时尽可能地接近u1的值。u2的求解方程如下：ε2＝h2-G2u2ε2表示在u2中非零方差所造成的误差，使得ε2的加权第二项最小化得出：位置估计值最终值u＝[x,y]T可由u2的平方根表示，即：P＝diag{sgn(u1(1)),sgn(u2(1))}。一种基于UWB和TDOA算法的智能机器人的定位系统，其特征在于：包括覆盖智能机器人移动区域的UWB基站、UWB定位装置，以及根据权利要求1所述的一种基于UWB和TDOA算法定位的智能机器人定位方法进行定位的激光SLAM定位与导航系统。可选的，所述UWB定位装置用于获取所述智能机器人的定位信息，所述UWB定位装置包括电连接所述智能机器人的UWB定位标签，用于在定位区域内同时与各个所述UWB基站进行通信，所述智能机器人上安装有一个所述UWB定位标签的Tag节点；所述UWB基站用于与所述UWB定位标签进行通信并获取所述UWB定位标签的位置信息；所述激光SLAM定位与导航系统包括UWB标签、激光SLAM雷达、电量计和IMU传感器，由工控机控制获得所述UWB标签、激光SLAM雷达和深度摄像头提供的定位数据，并由所述智能机器人本文档来自技高网...

【技术保护点】
1.一种基于UWB和TDOA算法定位的智能机器人定位方法，包括：A、智能机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和激光测距传感器数据进行自身定位，同时建造激光SLAM地图；B、标定UWB锚节点相对激光SLAM地图的位置；C、将UWB锚节点、UWB Tag节点、UWB中心控制节点组成一个UWB通信网络，通过TDOA算法组网并部署目标区域的Tag节点，根据UWB通信网络定位智能机器人的自身位置，并通过IMU传感器获取智能机器人的姿态数据；D、再次通过激光SLAM雷达获取智能机器人的自身位置；E、将上述步骤C和步骤D中的定位数据进行融合；F、通过UWB通信网络获取目标Tag节点的位置；G、规划智能机器人的跟踪路径；H、驱动智能机器人的底盘到达目标对象的位置并跟随目标对象，通过判断，如果目标对象跟丢了，则回到步骤F，再次通过UWB通信网络获取目标Tag节点的位置，反之，如果目标对象没跟丢，则继续跟随目标对象。

【技术特征摘要】
1.一种基于UWB和TDOA算法定位的智能机器人定位方法，包括：A、智能机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和激光测距传感器数据进行自身定位，同时建造激光SLAM地图；B、标定UWB锚节点相对激光SLAM地图的位置；C、将UWB锚节点、UWBTag节点、UWB中心控制节点组成一个UWB通信网络，通过TDOA算法组网并部署目标区域的Tag节点，根据UWB通信网络定位智能机器人的自身位置，并通过IMU传感器获取智能机器人的姿态数据；D、再次通过激光SLAM雷达获取智能机器人的自身位置；E、将上述步骤C和步骤D中的定位数据进行融合；F、通过UWB通信网络获取目标Tag节点的位置；G、规划智能机器人的跟踪路径；H、驱动智能机器人的底盘到达目标对象的位置并跟随目标对象，通过判断，如果目标对象跟丢了，则回到步骤F，再次通过UWB通信网络获取目标Tag节点的位置，反之，如果目标对象没跟丢，则继续跟随目标对象。2.根据权利要求1所述的一种基于UWB和TDOA算法定位的智能机器人定位方法，其特征在于：在根据所述TDOA算法组网的过程中，根据目标部署场景的大小划分成各个不同的子网，如果目标部署场景比较小，那单子网网络就可以完成所要求的位置定位服务，如果目标部署场景比较大，那么则需要多个子网网络才能完成所要求的位置定位服务。3.根据权利要求2所述的一种基于UWB和TDOA算法定位的智能机器人定位方法，其特征在于：构建所述单子网网络的时候，控制锚节点位于部署于目标空间的中心位置，相对位置设置为(0,0)，所述锚节点位置相对控制锚节点的位置已知，标签Tag节点位置未知，位于目标部署空间的任意位置，控制所述锚节点控制目标部署空间的时间同步，且控制所述锚节点的同时还汇集了所述标签Tag节点时间测量信息，并依据所述TDOA定位算法对标签Tag进行位置估计计算，控制所述锚节点将定位到的每个Tag节点的位置值通过网络发送给后台服务器进行位置显示、跟踪、查询、统计等工作，所述标签Tag节点可通过WIFI后台服务器获取自己本身的位置信息。4.根据权利要求2所述的一种基于UWB和TDOA算法定位的智能机器人定位方法，其特征在于：构建所述多子网UWB网络的时候，对于目标部署空间比较大的地方，采用部署多个子网来覆盖整个目标部署空间，各子网定位出来的各标签Tag节点的位置数据通过位置转移矩阵转换成地图空间的全局位置，且计算服务由后台服务器完成，且对于各控制锚节点不需要进行时间同步。5.根据权利要求4所述的一种基于UWB和TDOA算法定位的智能机器人定位方法，其特征在于：在构建所述多子网UWB网络的时候，采用双阶加权最小二乘解算法，通过增加一个虚拟变量来线性化TDOA方程，假设在子网中有M个锚节点，其中有一个控制锚节点位于子网的中心原点位置，M个锚节点的位置分别为(xi，yi)，其中i＝1,…,M，目标Tag节点的位置...

【专利技术属性】
技术研发人员：李明明，
申请(专利权)人：深圳勇艺达机器人有限公司，
类型：发明
国别省市：广东,44