CN108810133A - 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统 - Google Patents

Abstract

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

Description

一种基于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锚节点、UWB Tag节点、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到控制锚节点之间的距离，其中：

r_i1＝ct_i1＝r_i-r₁,i＝2,3,…,M……………(2)

r_i1表示锚节点i与控制锚节点之间的距离，t_i1表示脉冲超声波信号传递到锚节点i和控制锚节点之间的到达时间差。

将式(2)代入式(1)可得：

其中i＝2,3,…,M，因为控制锚节点位于子网原点位置，所以

在方程(3)中，r_i1为已知量，x_i，y_i为已知量，因此方程(3)是有关x，y，r₁三个参数的线性方程，但是因为r₁是跟Tag相关的非线性变量，所以方程(3)是一组非线性方程。

为了从非线性方程组中求解出x，y，那么首先假设x，y，r₁三个变量为相互独立的变量，然后可以通过加权线性最小二乘法来求解，这是第一阶段求解；第二阶段求解还原为x，y，r₁是非线性相关的变量，但是其满足方程(4)，因此可以通过另外一个线性最小二乘法来求解，通过这两阶段求出最终的Tag节点的位置，其中，

(I)第一求解阶段

假设u_i＝[x，y，r₁]^T，因为TDOA算法是有噪声的，所以方程(3)的误差方程如下：

ε₁＝h-G₁u₁………………….(5)

其中

u_i的加权LS解是使得ε₁误差最小时的解，即：

这里Q为TDOA的M×M的协方差矩阵。

(II)第二求解阶段

使得方程(4)的方差最小，同时尽可能地接近u₁的值。u₂的求解方程如下：

ε₂＝h₂-G₂u₂

ε₂表示在u₂中非零方差所造成的误差，

使得ε₂的加权第二项最小化得出：

位置估计值最终值u＝[x,y]^T可由u₂的平方根表示，即：

P＝diag{sgn(u₁(1)),sgn(u₂(1))}。

一种基于UWB和TDOA算法的智能机器人的定位系统，其特征在于：包括覆盖智能机器人移动区域的UWB基站、UWB定位装置，以及根据权利要求1所述的一种基于UWB和TDOA算法定位的智能机器人定位方法进行定位的激光SLAM定位与导航系统。

可选的，所述UWB定位装置用于获取所述智能机器人的定位信息，所述UWB定位装置包括电连接所述智能机器人的UWB定位标签，用于在定位区域内同时与各个所述UWB基站进行通信，所述智能机器人上安装有一个所述UWB定位标签的Tag节点；所述UWB基站用于与所述UWB定位标签进行通信并获取所述UWB定位标签的位置信息；所述激光SLAM定位与导航系统包括UWB标签、激光SLAM雷达、电量计和IMU传感器，由工控机控制获得所述UWB标签、激光SLAM雷达和深度摄像头提供的定位数据，并由所述智能机器人的驱动器驱动的底盘控制器在所述IMU传感器和电量计的数据中和所述工控机之间进行交互控制，所述UWB标签会以一定的频率发出脉冲式超声波信号，智能机器人可以通过Android核心交互板的WIFI联网功能，向服务器获取UWB定位网络得出的定位数据，从而辅助智能机器人进行定位和导航。

本发明具有如下有益效果：

本发明的一种基于UWB和TDOA算法的智能机器人定位方法及定位系统，主要是将UWB定位和激光SLAM定位结合起来，用于大场景中的定位导航，且基于UWB定位和激光SLAM定位的机器人跟随和找人的方案，在本方案中，采用基于TDOA算法的UWB定位方法，将UWB锚节点、UWB Tag节点、UWB中心控制节点组成一个UWB通信网络，位置数据的计算是由后台服务器进行计算，机器人与后台进行通信，获知目标对象上UWB Tag节点的位置数据，从而机器人根据目标对象的位置数据进行目标跟随；当机器人跟丢了目标对象时，机器人通过获知目标对象的位置数据，规划一条符合条件的可达路径，机器人根据这条路径导航到目标对象所在位置。

在本方案中采用了一种TDOA方案的TSWLSS双阶段加权最小二乘法的非线性解算方法，由于传统移动机器人在导航过程中普遍使用的激光测距传感器由于其自身的非线性和由于车体颠簸的干扰因素的影响会造成其自身测量结果的不准确，从而造成其在局部地图构建中的误差，本申请针对上述问题提出加权最小二乘方法来解决移动机器人在局部地图构建过程中针对线特征的拟合问题，使得计算复杂度降低、定位精度提高，通过本申请的UWB激光测距传感器节点的TDOA的定位方案，给出了具体的拓扑部署方案，使得机器人在大场景应用中，也能准确地获取自身的数据，为下一步的应用打下了坚实的基础。

附图说明

图1为本发明单子网UWB网络的示意图；

图2为本发明多子网UWB网络的的示意图；

图3为本发明激光SLAM定位和导航框图；

图4为本发明智能机器人定位方法及定位系统的流程图。

具体实施方式

下面结合实施例及附图对本发明的技术方案作进一步阐述。

实施例

本实施例提供了一种基于UWB和TDOA算法定位的智能机器人定位方法，智能机器人从所处的未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和激光测距传感器数据进行自身定位，同时建造激光SLAM地图,其中可以采用视觉SLAM，并包括如下操作步骤：数据采集、视觉里程计、后端优化、建图和闭环检测，采集视觉数据后，利用一个图像序列或者一个视频流，计算摄像机的方向和位置的过程，一般包括图像获取后、畸变校正、特征检测匹配或者直接匹配对应像素、通过对极几何原理估计摄像机的旋转矩阵和平移向量，如果视觉里程计模块估计的相机的旋转矩阵R和平移向量t都正确的话就能得到完美的定位和建图，反之可以采用采用捆集调整(BA)、卡尔曼滤波(EKF)、图优化等方式来解决，其中基于图优化的后端优化效果最好，Graph-based SLAM一般使用g2o求解器，进行图优化计算。因此，从任意位置开始，智能机器人能够通过以上的方法用其激光测距传感器探索环境，获得有关环境的知识，解释场景并构建合适的地图，且在目前的自主移动机器人领域，地图构建的方法大致可分为三类：栅格表示、几何信息表示和拓扑图表示，本申请中的智能机器人可以采用基于几何信息表示方法把真实环境中连续地图的表示成2D区段，采用简单的点簇聚类和分段的办法直接对激光测距传感器的原始数据进行提取，例如激光测距传感器LMS200的分辨率可达到0.5°，距离范围为0.05-20m，每秒可以进行25次180°范围内的扫描。

标定UWB锚节点相对激光SLAM地图的位置，常用的标定UWB室内定位系统精度的方法是采用静态校准和动态校准，可以采用Optitrack光学运动捕捉系统标定UWB锚节点。

将UWB锚节点、UWB Tag节点、UWB中心控制节点组成一个UWB通信网络，并采用TDOA的定位方法进行定位，其中，TDOA定位的优点是不需要信号发送节点和信号接收节点之间的同步，而且，UWB时间同步电路的高度保真性是的数据在传输过程中的高精度，UWB良好的时间分辨率能够提供精确的TDOA位置估计和可靠的位置追踪性能；TDOA网络本质是一个分层的网络，第一层为UWB脉冲信号发送节点，即为标签(Tag)节点层，第二层为UWB脉冲信号接收节点，即为锚(Anchor)节点，第三层为锚节点控制节点，主要控制锚节点的时间严格同步性，即为控制锚节点。

在TDOA组网过程中，根据目标部署场景的大小，划分成各个不同的子网，如果目标部署场景比较小，那单子网就可以完成所要求的位置定位服务，如图1所示，如果目标部署场景比较大，那么则需要多个子网才能完成所要求的位置定位服务，如图2所示；对于单子网网络来说，控制锚节点位于部署于目标空间的中心位置，相对位置设置为(0,0)，锚节点位置相对控制锚节点的位置已知，标签Tag节点位置未知，位于目标部署空间的任意位置，TDOA定位方案为二维空间的定位方法，因此不考虑空间z方面的定位位置服务。

控制锚节点控制目标部署空间的时间同步，因为只有时间同步程度高，才能算出精准的位置，控制锚节点同时还汇集标签Tag节点时间测量信息，并依据TDOA定位算法对标签Tag进行位置估计计算，控制锚节点将定位到的每个Tag节点的位置值通过网络发送给后台服务器进行位置显示、跟踪、查询、统计等工作，标签Tag节点可通过WIFI后台服务器获取自己本身的位置信息；对于目标部署空间比较大的地方，采用部署多个子网来覆盖整个目标部署空间，如图2所示，在图2中有4个子网覆盖整个目标部署空间。假设子网1、子网2、子网3和子网4的本地局部坐标系分别用Ci(i＝1,…,4)表示，目标地图空间坐标系用Cm表示。在部署时，局部坐标系Ci(i＝1,…,4)相对地图坐标系Cm的位置转移矩阵为Mi(i＝1,…,4)，M转移矩阵为3*3矩阵，各子网定位出来的各标签Tag节点的位置数据，通过位置转移矩阵转换成地图空间的全局位置，这个计算服务由后台服务器完成。对于各控制锚节点可不需要进行时间同步。

采用TDOA的方案，被定位的Tag节点的数量不受限制，在整个网络中可以同时存在上千个Tag节点，在整个UWB通信网络中，2D位置定位服务需要锚节点和控制节点的总数必须是3个以上，这样TDOA算法才有意义。

双阶加权最小二乘解算法(TSWLSS)能够消除传统的迭代计算的初始化和收敛问题。双阶加权最小二乘解算法(TSWLSS)是通过增加一个虚拟变量来线性化TDOA方程。采用一个额外的变量，将原始的非线性TDOA方程变换成一组线性方程，但是这个变量的引入会需要额外的接收节点。本算法在低噪声水平时，几乎达到了TDOA高斯噪声的Cramer-Rao下界，取得了一个基本的次优解。

考虑二维目标部署区域的TDOA方案，假设在子网中有M个锚节点，其中有一个控制锚节点位于子网的中心原点位置，M个锚节点的位置分别为(xi，yi)，其中i＝1,…,M，目标Tag节点的位置为(x，y)，ri表示锚节点i到Tag节点之间的距离，ri1表示锚节点i到控制锚节点之间的距离，其中，

r_i1＝ct_i1＝r_i-r₁,i＝2,3,…,M…………….(2)

r_i1表示锚节点i与控制锚节点之间的距离，t_i1表示脉冲超声波信号传递到锚节点i和控制锚节点之间的到达时间差。

将式(2)代入式(1)可得：

其中i＝2,3,…,M，因为控制锚节点位于子网原点位置，所以

在方程(3)中，r_i1为已知量，x_i，y_i为已知量，因此方程(3)是有关x，y，r₁三个参数的线性方程，但是因为r₁是跟Tag相关的非线性变量，所以方程(3)是一组非线性方程。

为了从非线性方程组中求解出x，y，那么首先假设x，y，r₁三个变量为相互独立的变量，然后可以通过加权线性最小二乘法来求解，这是第一阶段求解；第二阶段求解还原为x，y，r₁是非线性相关的变量，但是其满足方程(4)，因此可以通过另外一个线性最小二乘法来求解，通过这两阶段求出最终的Tag节点的位置。这种方法的好处是只需要标准的二乘法计算，而不需要假设初始位置。

(I)第一求解阶段

假设u_i＝[x，y，r₁]^T，因为TDOA估计是有噪声的，所以方程(3)的误差方程如下：

ε₁＝h-G₁u₁………………….(5)

其中

u_i的加权LS解是使得ε₁误差最小时的解，即：

这里Q为TDOA的M×M的协方差矩阵。

(II)第二求解阶段

第二阶段的误差最小化优化主要是提升位置估计的精度。最终的估计值应使得方程(4)的方差最小，同时尽可能地接近u1的值。u₂的求解方程如下：

ε₂＝h₂-G₂u₂

ε₂表示在u₂中非零方差所造成的误差，

使得ε₂的加权第二项最小化得出：

位置估计值最终值u＝[x,y]^T可由u₂的平方根表示，即：

P＝diag{sgn(u₁(1)),sgn(u₂(1))}

通过UWB通信网络及根据上述算法定位智能机器人的自身位置，并通过IMU(惯性测量单元)传感器获取智能机器人的姿态数据，通过IMU的三个单轴的加速度计和三个单轴的陀螺仪测量目标在坐标系统中三个轴的加速度信号及目标在坐标系统中运动的角速度大小，最后根据得到的一共6个数据解算物体的姿态，从而定位智能机器人的自身位置。

再次通过激光SLAM雷达获取机器人自身位置，基于激光SLAM定位与导航的框图如图3所示，在图3中，构建了基于UWB标签、激光SLAM雷达、电量计和IMU传感器构建多传感器融合的定位和导航系统，由工控机控制获得的UWB标签、激光SLAM雷达和深度摄像头提供的定位数据，并由智能机器人的驱动器驱动的底盘控制器在IMU传感器和电量计的数据中和工控机之间进行交互控制；但是在大场景的应用中，激光SLAM雷达也会出现定位丢失的情况，因此需要辅助采用TDOA方式进行的定位的UWB定位节点进行定位；在如图3中，UWB标签会以一定的频率发出脉冲式超声波信号，智能机器人可以通过Android核心交互板的WIFI联网功能，向服务器获取UWB定位网络得出的定位数据，从而辅助智能机器人进行定位和导航。

将通过UWB通信网络及根据上述算法定位智能机器人的自身位置和通过激光SLAM雷达获取机器人自身位置的定位数据进行融合，通过UWB通信网络获取目标Tag节点的位置；规划智能机器人的跟踪路径后，驱动底盘到达目标对象的位置并跟随目标对象，通过判断，如果目标对象跟丢了，则继续通过UWB通信网络获取目标Tag节点的位置，如果目标对象没跟丢，则继续跟随目标对象。

本申请的基于UWB和TDOA算法定位的智能机器人定位方法可以通过图4对所述步骤进行完整的演示。

另外，本发明还提供了一种基于上述UWB和TDOA算法的智能机器人的定位系统，包括覆盖智能机器人移动区域的UWB基站、UWB定位装置以及激光SLAM定位与导航系统，其中，UWB定位装置用于获取智能机器人的定位信息，UWB定位装置包括电连接智能机器人的UWB定位标签，用于在定位区域内同时与各个UWB基站进行通信，智能机器人上安装有一个UWB定位标签的Tag节点，UWB基站用于与UWB定位标签进行通信并获取UWB定位标签的位置信息。

以上实施例的先后顺序仅为便于描述，不代表实施例的优劣。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

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节点的位置，反之，如果目标对象没跟丢，则继续跟随目标对象。

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节点的位置为(x，y)，ri表示锚节点i到Tag节点之间的距离，ri 1表示锚节点i到控制锚节点之间的距离，其中：

r_i1＝ct_i1＝r_i-r₁,i＝2,3,…,M……………(2)

r_i1表示锚节点i与控制锚节点之间的距离，t_i1表示脉冲超声波信号传递到锚节点i和控制锚节点之间的到达时间差。

将式(2)代入式(1)可得：

其中i＝2,3,…,M，因为控制锚节点位于子网原点位置，所以

在方程(3)中，r_i1为已知量，x_i，y_i为已知量，因此方程(3)是有关x，y，r₁三个参数的线性方程，但是因为r₁是跟Tag相关的非线性变量，所以方程(3)是一组非线性方程。

为了从非线性方程组中求解出x，y，那么首先假设x，y，r₁三个变量为相互独立的变量，然后可以通过加权线性最小二乘法来求解，这是第一阶段求解；第二阶段求解还原为x，y，r₁是非线性相关的变量，但是其满足方程(4)，因此可以通过另外一个线性最小二乘法来求解，通过这两阶段求出最终的Tag节点的位置，其中，

(I)第一求解阶段

假设u_i＝[x，y，r₁]^T，因为TDOA算法是有噪声的，所以方程(3)的误差方程如下：

ε₁＝h-G₁u₁………………….(5)

其中

u_i的加权LS解是使得ε₁误差最小时的解，即：

这里Q为TDOA的M×M的协方差矩阵。

(II)第二求解阶段

使得方程(4)的方差最小，同时尽可能地接近u₁的值。u₂的求解方程如下：

ε₂＝h₂-G₂u₂

ε₂表示在u₂中非零方差所造成的误差，

使得ε₂的加权第二项最小化得出：

位置估计值最终值u＝[x,y]^T可由u₂的平方根表示，即：

P＝diag{sgn(u₁(1)),sgn(u₂(1))}。

6.一种基于UWB和TDOA算法的智能机器人的定位系统，其特征在于：包括覆盖智能机器人移动区域的UWB基站、UWB定位装置，以及根据权利要求1所述的一种基于UWB和TDOA算法定位的智能机器人定位方法进行定位的激光SLAM定位与导航系统。

7.根据权利要求6所述的一种基于UWB定位的智能机器人的定位系统，其特征在于：所述UWB定位装置用于获取智能机器人的定位信息，所述UWB定位装置包括电连接所述智能机器人的UWB定位标签，用于在定位区域内同时与各个所述UWB基站进行通信，所述智能机器人上安装有一个所述UWB定位标签的Tag节点；所述UWB基站用于与所述UWB定位标签进行通信并获取所述UWB定位标签的位置信息；所述激光SLAM定位与导航系统包括UWB标签、激光SLAM雷达、电量计和IMU传感器，由工控机控制获得所述UWB标签、激光SLAM雷达和深度摄像头提供的定位数据，并由所述智能机器人的驱动器驱动的底盘控制器在所述IMU传感器和电量计的数据中和所述工控机之间进行交互控制，所述UWB标签会以一定的频率发出脉冲式超声波信号，智能机器人可以通过Android核心交互板的WIFI联网功能，向服务器获取UWB定位网络得出的定位数据，从而辅助智能机器人进行定位和导航。