CN110166571A - 一种基于移动机器人的自动跟随方法和装置 - Google Patents
一种基于移动机器人的自动跟随方法和装置 Download PDFInfo
- Publication number
- CN110166571A CN110166571A CN201910491979.XA CN201910491979A CN110166571A CN 110166571 A CN110166571 A CN 110166571A CN 201910491979 A CN201910491979 A CN 201910491979A CN 110166571 A CN110166571 A CN 110166571A
- Authority
- CN
- China
- Prior art keywords
- uwb
- mobile robot
- label
- node
- positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000033001 locomotion Effects 0.000 claims abstract description 11
- 238000004891 communication Methods 0.000 claims description 20
- 238000005259 measurement Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 description 9
- 239000011159 matrix material Substances 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 238000005457 optimization Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000012546 transfer Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 231100000572 poisoning Toxicity 0.000 description 3
- 230000000607 poisoning effect Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000006855 networking Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000011017 operating method Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/12—Target-seeking control
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
- H04L67/00—Network arrangements or protocols for supporting network services or applications
- H04L67/01—Protocols
- H04L67/10—Protocols in which an application is distributed across nodes in the network
- H04L67/1095—Replication or mirroring of data, e.g. scheduling or transport for data synchronisation between network nodes
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
- H04L67/00—Network arrangements or protocols for supporting network services or applications
- H04L67/50—Network services
- H04L67/52—Network services specially adapted for the location of the user terminal
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于移动机器人的自动跟随方法和装置,其中所述方法包括,定位移动机器人的第一位置;获取用户所处的第二位置;根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离,进而解决了现有技术中智能机器人在大场景、少特征的环境中难以进行有效定位的问题。
Description
技术领域
本发明涉及智能机器人领域,具体涉及一种基于移动机器人的自动跟随方法和装置。
背景技术
随着移动机器人技术的发展,越来越多的移动机器人产品进入消费品市场和商用市场。在商用市场领域,移动机器人主要用来导引、导购、讲解、介绍、迎宾、物品输送等。移动机器人作为一种移动平台,可移动性是其基本的功能。而移动机器人的定位功能则是移动机器人实现自主化的关键技术之一,移动机器人只有了解了自己在当前环境中所处的位置,才能决定下一步要去什么地方。
在现有的跟随解决方案中,大多数是基于视觉的方法来进行人体跟随,这种方法很难跟随特定的人,且容易跟丢目标,而且视觉受环境的影响大,测量距离相对比较近。
当前移动机器人的定位方法有很多中,诸如:WIFI定位、RFID定位、UWB定位、视觉定位、iBeacon定位、无线激光测距传感器定位、激光SLAM定位和超声波定位;这些方法各有各的有点,各有各的缺点;其中在大规模场景中,单纯靠视觉和激光SLAM定位进行定位很难取得很好的效果。
发明内容
本发明目的是提供一种基于移动机器人的自动跟随方法和装置,解决现有技术在大场景、少特征的环境中,进行有效定位的问题。
本发明解决技术问题采用如下技术方案:一种基于移动机器人的自动跟随方法
定位移动机器人的第一位置;
获取用户所处的第二位置;
根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离。
可选的,所述方法还包括:
在丢失目标的情况下,根据所述第一位置和第二位置确定移动机器人的第二移动路径,以使所述移动机器人移动至所述第二位置。
可选的,移动机器人有第一UWB标签、惯性测量传感器和激光雷达;
其中,所述定位移动机器人的第一位置,包括:
基于第一UWB标签、惯性测量传感器、以及预设的位置坐标转换规则,确定移动机器人的第一子位置;
基于激光雷达定位移动机器人的第二子位置;
融合第一子位置和所述第二子位置,得到所述第一位置。
可选的,所述用户设置有第二UWB标签,所述自动跟随方法基于UWB通信网络实现,所述UWB通信网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成;
其中,所述获取用户所处的第二位置,包括:
基于所述第二UWB标签定位所述第二位置;
通过从锚节点和主锚节点,将所述第二位置发送至所述中心控制节点;
从中心控制节点中获取第二位置。
可选的,所述UWB通信网络包括多个UWB通信子网络,所述UWB通信子网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成。
可选的,预先构建所述UWB定位地图。
可选的,基于最小二乘解算法确定所述第一位置。
根据本发明的另一方面,还提供了一种基于移动机器人的自动跟随装置,包括:
定位模块,用于定位移动机器人的第一位置;
获取模块,用于获取用户所处的第二位置;
确定模块,用于根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离。
可选的,自动跟随方法还包括: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到控制锚节点之间的距离,其中:
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雷达和深度摄像头提供的定位数据,并由所述智能机器人的驱动器驱动的底盘控制器在所述IMU传感器和电量计的数据中和所述工控机之间进行交互控制,所述UWB标签会以一定的频率发出脉冲式超声波信号,智能机器人可以通过Android核心交互板的WIFI联网功能,向服务器获取UWB定位网络得出的定位数据,从而辅助智能机器人进行定位和导航。
本发明具有如下有益效果:
本发明的一种基于移动机器人的自动跟随方法,主要是将UWB定位和激光SLAM定位结合起来,用于大场景中的定位导航,且基于UWB定位和激光SLAM定位的机器人跟随和找人的方案,在本方案中,采用基于TDOA算法的UWB定位方法,将UWB锚节点、UWB Tag节点、UWB中心控制节点组成一个UWB通信网络,位置数据的计算是由后台服务器进行计算,机器人与后台进行通信,获知目标对象上UWB Tag节点的位置数据,从而机器人根据目标对象的位置数据进行目标跟随;当机器人跟丢了目标对象时,机器人通过获知目标对象的位置数据,规划一条符合条件的可达路径,机器人根据这条路径导航到目标对象所在位置。
在本方案中采用了一种TDOA方案的TSWLSS双阶段加权最小二乘法的非线性解算方法,由于传统移动机器人在导航过程中普遍使用的激光测距传感器由于其自身的非线性和由于车体颠簸的干扰因素的影响会造成其自身测量结果的不准确,从而造成其在局部地图构建中的误差,本申请针对上述问题提出加权最小二乘方法来解决移动机器人在局部地图构建过程中针对线特征的拟合问题,使得计算复杂度降低、定位精度提高,通过本申请的UWB激光测距传感器节点的TDOA的定位方案,给出了具体的拓扑部署方案,使得机器人在大场景应用中,也能准确地获取自身的数据,为下一步的应用打下了坚实的基础。
附图说明
图1为本发明提供的基于移动机器人的自动跟随方法流程图;
图2为本发明单子网UWB网络的示意图;
图3为本发明多子网UWB网络的的示意图;
图4为本发明激光SLAM定位和导航框图;
图5为本发明智能机器人定位方法及定位系统的流程图。
具体实施方式
下面结合实施例及附图对本发明的技术方案作进一步阐述。
实施例
图1为本发明提供的基于移动机器人的自动跟随方法流程图。如图1所示,本实施例提供了一种基于移动机器人的自动跟随方法,包括:
S110、定位移动机器人的第一位置。
其中,第一位置为所述移动机器人所处的位置,具体可以根据位置估计和激光测距传感器数据进行自身定位。
S120、获取用户所处的第二位置。
其中,第二位置为所述移动机器人所处的位置。第二位置可以通过多种方式获取得到,本实现方式中,对此不做限定。
S130、根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离。
其中,所述第一移动路径可以是移动机器人的行驶路径,以使得移动机器人与用户保持预设距离,在用户发生移动时,移动机器人也实时进行移动。
可选的,所述方法还包括:在丢失目标的情况下,根据所述第一位置和第二位置确定移动机器人的第二移动路径,以使所述移动机器人移动至所述第二位置。
可选的,移动机器人有第一UWB标签、惯性测量传感器(IMU)和激光雷达;其中,所述定位移动机器人的第一位置,包括:基于第一UWB标签、惯性测量传感器、以及预设的位置坐标转换规则,确定移动机器人的第一子位置;
基于激光雷达定位移动机器人的第二子位置;融合第一子位置和所述第二子位置,得到所述第一位置。
可选的,所述用户设置有第二UWB标签,所述自动跟随方法基于UWB通信网络实现,所述UWB通信网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成;
其中,所述获取用户所处的第二位置,包括:基于所述第二UWB标签定位所述第二位置;通过从锚节点和主锚节点,将所述第二位置发送至所述中心控制节点;从中心控制节点中获取第二位置。
可选的,所述UWB通信网络包括多个UWB通信子网络,所述UWB通信子网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成。
可选的,预先构建所述UWB定位地图。
可选的,基于最小二乘解算法确定所述第一位置。
作为一个优选实施例,移动机器人从所处的未知环境中从一个未知位置(第一位置)开始移动,在移动过程中根据位置估计和激光测距传感器数据进行自身定位,同时建造激光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锚节点(主锚节点)、UWBTag节点(第一标签节点或第二标签节点)、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到控制锚节点之间的距离,其中,
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通信网络及根据上述算法定位智能机器人的自身位置,并通过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 (8)
1.一种基于移动机器人的自动跟随方法,其特征在于,包括:
定位移动机器人的第一位置;
获取用户所处的第二位置;
根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离。
2.根据权利要求1所述的方法,其特征在于,所述方法还包括:
在丢失目标的情况下,根据所述第一位置和第二位置确定移动机器人的第二移动路径,以使所述移动机器人移动至所述第二位置。
3.根据权利要求1所述的方法,其特征在于,移动机器人有第一UWB标签、惯性测量传感器和激光雷达;
其中,所述定位移动机器人的第一位置,包括:
基于第一UWB标签、惯性测量传感器、以及预设的位置坐标转换规则,确定移动机器人的第一子位置;
基于激光雷达定位移动机器人的第二子位置;
融合第一子位置和所述第二子位置,得到所述第一位置。
4.根据权利要求3所述的方法,其特征在于,所述用户设置有第二UWB标签,所述自动跟随方法基于UWB通信网络实现,所述UWB通信网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成;
其中,所述获取用户所处的第二位置,包括:
基于所述第二UWB标签定位所述第二位置;
通过从锚节点和主锚节点,将所述第二位置发送至所述中心控制节点;
从中心控制节点中获取第二位置。
5.根据权利要求4所述的方法,其特征在于:
所述UWB通信网络包括多个UWB通信子网络,所述UWB通信子网络由第一UWB标签的标签节点、第二UWB标签的标签节点、从锚节点、主锚节点和中心控制节点组成。
6.根据权利要求4所述的方法,其特征在于,所述方法还包括:
预先构建所述UWB定位地图。
7.根据权利要求1-6中任意一项所述的方法,其特征在于:
基于最小二乘解算法确定所述第一位置。
8.一种基于移动机器人的自动跟随装置,其特征在于,包括:
定位模块,用于定位移动机器人的第一位置;
获取模块,用于获取用户所处的第二位置;
确定模块,用于根据所述第一位置和第二位置确定所述移动机器人的第一移动路径,以使移动机器人与所述用户保持预设距离。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2018105892573 | 2018-06-08 | ||
CN201810589257.3A CN108810133A (zh) | 2018-06-08 | 2018-06-08 | 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110166571A true CN110166571A (zh) | 2019-08-23 |
Family
ID=64088086
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810589257.3A Pending CN108810133A (zh) | 2018-06-08 | 2018-06-08 | 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统 |
CN201910491979.XA Pending CN110166571A (zh) | 2018-06-08 | 2019-06-06 | 一种基于移动机器人的自动跟随方法和装置 |
Family Applications Before (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810589257.3A Pending CN108810133A (zh) | 2018-06-08 | 2018-06-08 | 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统 |
Country Status (1)
Country | Link |
---|---|
CN (2) | CN108810133A (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110531769A (zh) * | 2019-08-28 | 2019-12-03 | 深圳勇艺达机器人有限公司 | 用于机器人移动路径确定方法及装置 |
CN111432334A (zh) * | 2020-03-31 | 2020-07-17 | 中通服创立信息科技有限责任公司 | 一种轨道式巡检机器人的跟随监控方法及系统 |
CN112904845A (zh) * | 2021-01-15 | 2021-06-04 | 珠海市一微半导体有限公司 | 基于无线测距传感器的机器人卡住检测方法、系统及芯片 |
CN113093215A (zh) * | 2021-04-01 | 2021-07-09 | 南京航空航天大学 | 一种基于激光测距的移动平台跟踪方法 |
Families Citing this family (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109633537A (zh) * | 2018-12-27 | 2019-04-16 | 浙江绿晶环境服务有限公司 | 一种基于uwb测距的员工定位系统 |
CN109443350B (zh) * | 2018-12-27 | 2023-09-01 | 仝人智能科技(江苏)有限公司 | 基于神经网络的蓝牙/光电/ins组合导航装置及方法 |
CN109600713A (zh) * | 2019-01-09 | 2019-04-09 | 哈尔滨理工大学 | 儿童定位监测系统 |
CN109541535A (zh) * | 2019-01-11 | 2019-03-29 | 浙江智澜科技有限公司 | 一种基于uwb和视觉slam的agv室内定位及导航的方法 |
CN111665470A (zh) * | 2019-03-07 | 2020-09-15 | 阿里巴巴集团控股有限公司 | 一种定位方法及装置和机器人 |
CN110244715B (zh) * | 2019-05-23 | 2022-09-30 | 西安理工大学 | 一种基于超宽带技术的多移动机器人高精度协同跟踪方法 |
CN110427055A (zh) * | 2019-08-05 | 2019-11-08 | 厦门大学 | 一种舞台追光灯自动控制系统及方法 |
CN110647089A (zh) * | 2019-10-28 | 2020-01-03 | 天津中德应用技术大学 | 智能仓储物流机器人控制系统及控制方法 |
CN111947659B (zh) * | 2020-07-07 | 2022-05-24 | 华南理工大学 | 面向移动机器人的声光电多模分布协作式定位与导航系统 |
CN111988739B (zh) * | 2020-08-06 | 2023-05-02 | 普玄物联科技(杭州)有限公司 | 一种高精度定位的商场导购系统及其应用方法 |
CN114485656A (zh) * | 2020-11-11 | 2022-05-13 | Oppo广东移动通信有限公司 | 室内定位方法及相关装置 |
CN112578800A (zh) * | 2021-02-25 | 2021-03-30 | 湖南擎谱数字科技有限公司 | 一种移动机器人室内自动定位系统及方法 |
GB202102939D0 (en) * | 2021-03-02 | 2021-04-14 | Ubisense Ltd | Environmental characterisation |
CN113324544B (zh) * | 2021-04-19 | 2023-02-28 | 北京化工大学 | 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 |
CN117590858B (zh) * | 2024-01-19 | 2024-04-16 | 潍坊现代农业山东省实验室 | 大棚无人车导航方法和大棚无人车导航系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120044355A1 (en) * | 2010-08-18 | 2012-02-23 | Nearbuy Systems, Inc. | Calibration of Wi-Fi Localization from Video Localization |
CN204495357U (zh) * | 2015-04-13 | 2015-07-22 | 梁步阁 | 一种多基多模组网融合室内人员导航定位系统 |
CN105807775A (zh) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | 具有自主跟随避障功能的移动机器人 |
CN106211083A (zh) * | 2016-07-15 | 2016-12-07 | 北京深蓝织梦科技有限公司 | 一种手机室内定位系统 |
CN206105880U (zh) * | 2016-09-12 | 2017-04-19 | 上海仙知机器人科技有限公司 | 一种机器人控制系统 |
CN107065863A (zh) * | 2017-03-13 | 2017-08-18 | 山东大学 | 一种基于人脸识别技术的导览解说机器人及方法 |
CN107608345A (zh) * | 2017-08-26 | 2018-01-19 | 深圳力子机器人有限公司 | 一种机器人及其跟随方法和系统 |
-
2018
- 2018-06-08 CN CN201810589257.3A patent/CN108810133A/zh active Pending
-
2019
- 2019-06-06 CN CN201910491979.XA patent/CN110166571A/zh active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120044355A1 (en) * | 2010-08-18 | 2012-02-23 | Nearbuy Systems, Inc. | Calibration of Wi-Fi Localization from Video Localization |
CN204495357U (zh) * | 2015-04-13 | 2015-07-22 | 梁步阁 | 一种多基多模组网融合室内人员导航定位系统 |
CN105807775A (zh) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | 具有自主跟随避障功能的移动机器人 |
CN106211083A (zh) * | 2016-07-15 | 2016-12-07 | 北京深蓝织梦科技有限公司 | 一种手机室内定位系统 |
CN206105880U (zh) * | 2016-09-12 | 2017-04-19 | 上海仙知机器人科技有限公司 | 一种机器人控制系统 |
CN107065863A (zh) * | 2017-03-13 | 2017-08-18 | 山东大学 | 一种基于人脸识别技术的导览解说机器人及方法 |
CN107608345A (zh) * | 2017-08-26 | 2018-01-19 | 深圳力子机器人有限公司 | 一种机器人及其跟随方法和系统 |
Non-Patent Citations (2)
Title |
---|
JEEWOONG PARK ET AL.: "A BIM and UWB integrated Mobile Robot Navigation System for Indoor Position Tracking Applications", 《KICEM JOURNAL OF CONSTRUCTION ENGINEERING AND PROJECT MANAGEMENT》 * |
方晨晨: "基于UWB的自主跟随机器人定位方法", 《软件导刊》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110531769A (zh) * | 2019-08-28 | 2019-12-03 | 深圳勇艺达机器人有限公司 | 用于机器人移动路径确定方法及装置 |
CN111432334A (zh) * | 2020-03-31 | 2020-07-17 | 中通服创立信息科技有限责任公司 | 一种轨道式巡检机器人的跟随监控方法及系统 |
CN111432334B (zh) * | 2020-03-31 | 2022-05-27 | 中通服创立信息科技有限责任公司 | 一种轨道式巡检机器人的跟随监控方法及系统 |
CN112904845A (zh) * | 2021-01-15 | 2021-06-04 | 珠海市一微半导体有限公司 | 基于无线测距传感器的机器人卡住检测方法、系统及芯片 |
CN113093215A (zh) * | 2021-04-01 | 2021-07-09 | 南京航空航天大学 | 一种基于激光测距的移动平台跟踪方法 |
CN113093215B (zh) * | 2021-04-01 | 2022-10-14 | 南京航空航天大学 | 一种基于激光测距的移动平台跟踪方法 |
Also Published As
Publication number | Publication date |
---|---|
CN108810133A (zh) | 2018-11-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110166571A (zh) | 一种基于移动机器人的自动跟随方法和装置 | |
Ayyalasomayajula et al. | Deep learning based wireless localization for indoor navigation | |
CN110446159B (zh) | 一种室内无人机精确定位与自主导航的系统及方法 | |
CN106931961B (zh) | 一种自动导航方法及装置 | |
CN105547305B (zh) | 一种基于无线定位和激光地图匹配的位姿解算方法 | |
Paul et al. | RSSI-based indoor localization and tracking using sigma-point Kalman smoothers | |
Wang et al. | A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors | |
CN107544501A (zh) | 一种智能机器人智慧行走控制系统及其方法 | |
CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
WO2020223689A1 (en) | Wireless device localization | |
Yang et al. | High-precision UWB-based localisation for UAV in extremely confined environments | |
Tian et al. | HiQuadLoc: A RSS fingerprinting based indoor localization system for quadrotors | |
CN112857370A (zh) | 一种基于时序信息建模的机器人无地图导航方法 | |
Liu et al. | An autonomous positioning method for fire robots with multi-source sensors | |
Chen et al. | Submap-based indoor navigation system for the fetch robot | |
Doer et al. | x-rio: Radar inertial odometry with multiple radar sensors and yaw aiding | |
Ivancsits et al. | Visual navigation system for small unmanned aerial vehicles | |
Barrile et al. | Self-localization by laser scanner and GPS in automated surveys | |
Chen et al. | Decimeter-accuracy positioning for drones using two-stage trilateration in a GPS-denied environment | |
Lin et al. | Drift-free visual slam for mobile robot localization by integrating uwb technology | |
Liu et al. | A Review of Sensing Technologies for Indoor Autonomous Mobile Robots | |
US11561553B1 (en) | System and method of providing a multi-modal localization for an object | |
An et al. | Efficient rigid body localization based on euclidean distance matrix completion for AGV positioning under harsh environment | |
Sotelo et al. | Low level controller for a POMDP based on WiFi observations | |
CN113825100A (zh) | 定位寻物方法及系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190823 |
|
RJ01 | Rejection of invention patent application after publication |