机器人的定位导航系统及方法、存储介质、机器人
技术领域
本发明涉及机器人技术领域,尤其涉及一种机器人的定位导航系统及方法、存储介质、机器人、计算机程序。
背景技术
在机器人相关技术的研究中,在机器人的定位导航技术是研究的核心,也是机器人实现智能化及完全自主的关键技术。目前比较常用的定位导航技术有VSLAM(英文全称为:Visual Simultaneous Localization and Mapping,中文名称为:视觉同时定位与构图)、Wifi-SLAM(英文全称为:Wireless Fidelity-Simultaneous Localization andMapping,中文名称为:无线宽带同时定位与构图)、UWB(英文全称为:Ultra Wideband,中文名称为:超宽带)等。
其中,UWB技术被较多地用于高精度定位领域,如物流仓储、监狱、智能楼宇等领域,将UWB技术应用于机器人的定位导航领域,是目前定位导航的前沿技术。其基本原理为:在机器人活动的空间内布置至少3个基站,并在机器人身体上布置一个标签,基站的位置固定且已知,标签的位置未知,标签周期性地发送定位信号,基站接收到标签所发送的定位信号,通过一系列协同处理就能够计算确定标签(即机器人)的位置坐标。
但是上述UWB技术应用于机器人的定位导航存在如下缺点:采用UWB技术只是解决了机器人的定位问题,对于机器人的朝向的判断,必须辅助其他手段,一般可使用电子罗盘进行方向确定,但电子罗盘易受室内磁场变化等干扰影响,其指示的结果准确度较低。
发明内容
本发明实施例提供一种机器人的定位导航系统及方法、存储介质、机器人、计算机程序,以解决应用UWB技术进行机器人的定位导航所存在的朝向判断准确度较低的问题。
为达到上述目的,本发明实施例采用如下技术方案:
第一方面,本发明实施例提供了一种机器人的定位导航系统,该定位导航系统包括:分散安装于机器人身体上的若干标签,且各标签处于同一水平面上,所述标签用于发送定位信号;至少三个安装于机器人活动空间内的基站,所述基站用于接收所述定位信号;安装于机器人身体上的电子罗盘和陀螺仪;控制器,用于根据所述定位信号由所述标签传输至所述基站的时间数据,实时计算各标签的位置坐标,根据各标签的位置坐标,或电子罗盘的检测数据,或陀螺仪的检测数据,实时确定机器人的朝向角度。
第二方面,本发明实施例提供了一种机器人的定位导航方法,该定位导航方法应用于如第一方面所述的定位导航系统,该定位导航方法包括:步骤A:实时计算各标签的位置坐标;步骤B:实时确定机器人的朝向角度。其中步骤B包括:步骤B1:判断各标签的位置坐标数据是否可靠,如果是,则进入步骤B2,如果否,则进入步骤B3;步骤B2:确定各标签在机器人身体上的安装位置与机器人朝向之间的关系,结合各标签的位置坐标,计算确定机器人的朝向角度;步骤B3:判断当前时刻电子罗盘的检测数据是否可靠,如果是,则进入步骤B4,如果否,则进入步骤B5;步骤B4:根据电子罗盘的检测数据,计算确定机器人的朝向角度,并将陀螺仪的检测数据初始化为0;步骤B5:根据陀螺仪的检测数据,计算确定机器人的朝向角度。
第三方面,本发明实施例提供了一种存储介质,该存储介质存储有指令代码,该指令代码用于执行如第二方面所述的定位导航方法。
第四方面,本发明实施例提供了一种机器人,该机器人包括处理器和存储器;该存储器中存储有指令代码;该处理器用于执行该指令代码,以实现如第二方面所述的定位导航方法。
第五方面,本发明实施例提供了一种计算机程序,该计算机程序包括指令代码,该指令代码用于执行如第二方面所述的定位导航方法。
相较于现有技术,本发明实施例所提供的技术方案,通过在机器人身体上设置若干标签,使得在标签的位置坐标数据可靠时,能够根据这些标签在机器人身体上的安装位置,确定这些安装位置与机器人朝向之间的关系,从而可以结合这些标签的位置坐标确定机器人的朝向,实现了应用UWB技术不借助外部辅助设备就能确定机器人的朝向。并且在机器人身体上设置电子罗盘和陀螺仪,当标签的位置数据不可靠时,若电子罗盘的检测数据可靠,则根据电子罗盘的检测数据确定机器人的朝向;若电子罗盘的检测数据不可靠,则根据陀螺仪的检测数据确定机器人的朝向,从而有效地避免了电子罗盘受到干扰时检测的准确度较低的问题。通过上面的分析可见,本发明的技术方案通过采用标签的位置坐标、电子罗盘和陀螺仪三种方式结合,来对机器人的朝向进行判断,提高了朝向判断的准确度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例所提供的定位导航方法中确定机器人朝向的基本流程图;
图2为本发明实施例所提供的定位导航方法中计算各标签的位置坐标的基本流程图;
图3为本发明实施例所提供的定位导航方法中计算各标签的位置坐标和确定机器人朝向的具体流程图;
图4为本发明实施例所提供的定位导航方法导航时流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
由背景技术可知,现有技术中UWB技术应用于机器人的定位导航领域,只能对机器人进行定位,并不能判断机器人的朝向,而借助电子罗盘判断机器人的朝向,一旦电子罗盘受到干扰影响,其指示的准确度会较低。基于这一现状,本发明的发明人提出:将现有技术中在机器人身体上布置一个标签更改为布置若干标签,并且在机器人身体上安装电子罗盘和陀螺仪,根据各标签的位置坐标或电子罗盘的检测数据或陀螺仪的检测数据,实时确定机器人的朝向角度,这样通过三种方式结合判断机器人的朝向,能够实现提高机器人朝向不精准的目的。
基于上述发明思想,下面对本发明实施例的技术方案进行具体详细的介绍。
本发明实施例提供了一种机器人的定位导航系统,该定位导航系统包括:分散安装于机器人身体上的若干标签,且各标签处于同一水平面上;至少三个安装于机器人活动空间内的基站;安装于机器人身体上的电子罗盘和陀螺仪;控制器,用于实时计算各标签的位置坐标,根据各标签的位置坐标,或电子罗盘的检测数据,或陀螺仪的检测数据,实时确定机器人的朝向角度。
其中,标签用于发送定位信号,由于机器人处于运动状态,因此标签的位置坐标未知。标签的数量由现有技术中的一个更改为若干个,目的是为了能够利用多个标签的位置坐标和安装位置确定机器人的朝向。出于实现这一目的的考虑,需要将这些标签在机器人身体上分散安装,原则上各标签相互之间要尽量相互远离,以避免标签之间相互重叠,而无法准确确定标签的位置坐标。并且各标签需要处于同一水平面上,以确保能够通过标签的位置坐标和安装位置确定机器人的位置和朝向。此外各标签最好靠近机器人身体的上部安装,以很好地避免机器人在活动过程中自身四肢等部位对标签造成遮挡。作为一种可能的设计,可在机器人的头两侧或者两侧肩膀或者两侧手臂上各安装一个标签;进一步的,标签的数量可以仅为两个,这样既节省成本,又能简化算法。
基站用于接收定位信号,基站的位置坐标是可知的。针对一个标签,该标签发出一定位信号,可以通过记录各基站接收该定位信号分别需要的时间,将时间值转化为标签与各基站之间的距离值,或者通过记录各基站接收该定位信号的时间差,将时间差值转化为标签与各基站之间的距离差值,进而结合各基站的位置坐标得到该标签的位置坐标。因此基站的数量至少为三个,才能保证计算得到标签的位置坐标。作为一种可能的设计,基站的数量可为4个。
控制器实时计算各标签的位置坐标,所依据的是定位信号由标签传输至基站的时间数据,该时间数据例如可为某一定位信号由相应标签传输至各基站分别需要的时间值,又例如可为某一定位信号由相应标签传输至各基站的时间差值,在后续的描述中,会对控制器实时计算各标签的位置坐标的过程进行详述。
控制器除了用于实时确定机器人的朝向角度外,还可用于根据各标签的位置坐标,及各标签在机器人身体上的安装位置,实时确定机器人的位置坐标。
控制器可集成于机器人身体内部,也可外设于机器人身体外部,如设置于机器人的胸前。基于此,电子罗盘和陀螺仪可集成于控制器中,也可设置于机器人自身系统内,需要注意针对不同的设置方式,在进行机器人朝向角度的计算时,要做好电子罗盘和陀螺仪与机器人身体之间的角度转换。
基于上述机器人的定位导航系统,本发明实施例提供了一种机器人的定位导航方法,该定位导航方法包括:步骤A:实时计算各标签的位置坐标;步骤B:实时确定机器人的朝向角度。
其中,如图1所示,步骤B包括:
步骤B1:判断各标签的位置坐标数据是否可靠,如果是,则进入步骤B2;如果否,则进入步骤B3。
在上述步骤B1中,由于定位信号在由标签发出被基站接收的过程中可能被遮挡或阻碍,造成所确定的标签的位置坐标不准确,因此判断标签的位置坐标是否可靠,实质就是判断标签的位置坐标是否准确。如果标签的位置坐标准确,则可利用标签的位置坐标进行机器人朝向的确定;如果标签的位置坐标不准确,则不可利用标签的位置标签的位置坐标进行机器人朝向的确定,以保证所确定的机器人的朝向的准确度。
需要说明的是,判断各标签的位置坐标数据是否可靠所采用的判断机制需要根据标签的布置数量、布置位置等因素综合确定。在下面的具体实施例中会以一种情况为例对该判断机制进行示例性地介绍。
步骤B2:确定各标签在机器人身体上的安装位置与机器人朝向之间的关系,结合各标签的位置坐标,计算确定机器人的朝向角度。
在机器人活动空间的水平面内,将机器人的身体看作一线段,机器人的朝向即为垂直于该线段的一射线。由于各标签安装于机器人身体上,因此各标签与机器人身体之间的位置关系是固定不变的,也就是说,各标签与表征机器人身体的线段之间的位置关系是固定不变的,从而标签之间的连线与表征机器人朝向的射线之间存在一定的角度关系。基于上面的分析,在上述步骤B2中,确定各标签在机器人身体上的安装位置与机器人朝向之间的关系,即找出标签之间连线与表征机器人朝向的射线之间的角度关系,并且根据各标签的位置坐标能够求得任意两个标签之间连线的方向,进而可计算确定机器人的朝向角度。
通过上述步骤B2实现了在各标签的位置坐标数据可靠时,直接利用若干标签的位置坐标就能确定机器人的朝向,从而解决了应用UWB技术对机器人进行定位导航,不借助外部辅助设备就无法确定机器人朝向的问题。
步骤B3:判断当前时刻电子罗盘的检测数据是否可靠,如果是,则进入步骤B4;如果否,则进入步骤B5。
当标签的位置坐标不准确时,需要借助外部辅助设备进行机器人朝向的确定,比如借助电子罗盘,为了保证所确定的机器人的朝向的准确度,首先需要判断电子罗盘的检测数据是否可靠,如果可靠,则可利用电子罗盘确定机器人的朝向,如果不可靠,则不能利用电子罗盘确定机器人的朝向。
基于上述分析,作为一种可能的方案,步骤B3具体包括:获取当前时刻电子罗盘的准确程度值,判断该准确程度值是否小于或等于设定准确程度阈值,如果是,则判定当前时刻所述电子罗盘的检测数据可靠,进入步骤B4;如果否,则判定当前时刻电子罗盘的检测数据不可靠,进入步骤B5。需要说明的是,通过监听电子罗盘中地磁传感器的准确度报告事件,即调用on Accuracy Changed(准确性改变)函数,从该函数的参数中能够获取前述“准确程度值”。
步骤B4:根据电子罗盘的检测数据,计算确定机器人的朝向角度,并将陀螺仪的检测数据初始化为0。
为了获得准确的机器人朝向,在上述步骤B4之前包括:确定一基准方向,在机器人朝向该基准方向,且电子罗盘无干扰时,得到电子罗盘的检测数据,记该检测数据为校准角度。其中,基准方向可以是水平面中的任一方向,可依实际需要而定,例如正南、正北、正东、西南等方向。作为一种可能的方案,将基准方向定为正北方向,机器人朝向正北方向时的朝向角度为0°,此时电子罗盘的检测数据可能不为0,即校准角度可能不为0。
基于上面的预备步骤,上述步骤B4具体包括:将当前时刻电子罗盘的检测数据减去该校准角度,得到机器人相对于基准方向的朝向角度,并将陀螺仪的检测数据初始化为0。由于机器人从基准角度变为当前时刻的朝向角度,当前时刻的电子罗盘检测数据是从校准角度开始记录的,因此将当前时刻电子罗盘的检测数据减去校准角度,才能得到当前时刻准确的机器人朝向角度。
需要说明的是,在本步骤中,由于陀螺仪记录的检测数据为设备转向的角度,这一角度为一相对角度值,从其检测数据得到并不能直接得到机器人的朝向角度,因此在每次使用电子罗盘的检测数据时,均将陀螺仪的检测数据初始化为0,以备当要使用陀螺仪的检测数据时,其检测数据是从0开始记录的转向角度,该检测数据与最近一次陀螺仪的检测数据初始化为0时机器人的朝向角度加和,即可得到机器人当前时刻的朝向角度。
步骤B5:根据陀螺仪的检测数据,计算确定机器人的朝向角度。
基于上面的分析,上述步骤B5具体包括:将最近一次陀螺仪的检测数据初始化为0时机器人的朝向角度与当前时刻陀螺仪的检测数据加和,得到当前时刻机器人的朝向角度。
对于本实施例所提供的定位导航方法中的步骤A,如图2所示,其可包括以下步骤:
步骤A1:在机器人的活动空间的水平面上建立平面直角坐标系,确定各基站的位置坐标。
步骤A2:根据各基站的位置坐标,采用到TDOA(英文全称:Time Difference ofArrival,中文名称:到达时间差)定位技术或TOA(英文全称:Time of Arrival,中文名称:到达时间)定位技术,实时计算每个标签与各基站之间的距离。
在上述步骤A2中,对于某一标签而言,其发送一定位信号,各基站能够分别接收该定位信号,通过记录各基站接收该定位信号的时间或者时间差,已知定位信号的传输速率,就能够将所记录的时间或时间差转化为标签与各基站之间的距离值。需要说明的是,若采用TDOA定位技术,由于计算使用的是时间差值,为一相对值,因此计算结果的准确度更高。若采用TOA定位技术,由于计算使用的是时间值,直接表征标签与基站之间的距离,因此计算过程更简单。
步骤A3:根据每个标签与各基站之间的距离,采用三边测量法,实时计算相应标签的位置坐标。
在上述步骤A3中,对于某一标签而言,已知该标签与各基站之间的距离,以各基站为圆心,相应距离值为半径画圆,从而各个圆的相交点即为该标签所在位置点,进而可计算得到该标签的位置坐标。
对于本实施例所提供的定位导航方法,其还包括:步骤C:确定各标签在机器人身体上的安装位置与机器人位置之间的关系,结合各标签的位置坐标,实时计算确定机器人的位置坐标。
在机器人活动空间的水平面内,由于各标签安装于机器人身体上,因此各标签与机器人位置点之间的位置关系是固定不变的,从而标签之间的连线与机器人位置点之间存在一定的位置关系。基于上面的分析,在上述步骤C中,确定各标签在机器人身体上的安装位置与机器人位置之间的关系,即找出标签之间连线与机器人位置点之间的位置关系,进而可结合各标签的位置坐标求得机器人的位置坐标。
现有技术中,当标签与基站之间存在动态障碍时,如有人在标签与基站之间活动,或者在标签被机器人的身体某些部位或其它遮挡物遮挡时,就会发生标签所发送的定位信号绕过障碍物才被基站接收,或者定位信号到达基站的时间延迟等情况,导致最终得到的标签位置偏移,从而据此所确定的机器人位置坐标的准确度下降。本发明实施例中,由于机器人身体上安装了多个标签,且各标签的安装位置分散,因此能够大大减少动态障碍、标签遮挡等情况所带来的误差,从而提高了所确定的机器人位置坐标的准确度。
请参见图3,下面结合具体的应用场景对以上所述的步骤A、B、C进行示例性地详细介绍。
步骤A:实时计算各标签的位置坐标。具体包括以下步骤:
步骤A1:以机器人活动房间的西南角为原点,沿墙向东为X轴正方向,沿墙向北为Y轴正方向,建立平面直角坐标系。房间内架设4台基站,分别编号为1、2、3、4,并确定每台基站的位置坐标。
步骤A2:机器人的头两侧或者两侧肩膀或者两侧手臂上各安装有一个标签,分别记为第一标签T1和第二标签T2,采用TDOA定位技术计算第一标签T1与各基站之间的距离,及第二标签T2与各基站之间的距离。
步骤A3:采用三边测量法,计算得到第一标签T1的位置坐标(x1,y1),及第二标签T2的位置坐标(x2,y2)。
步骤B:实时确定机器人的朝向角度。具体包括以下步骤:
步骤B1:计算第一标签T1和第二标签T2之间的距离,判断该距离是否大于或等于设定距离阈值。若第一标签T1和第二标签T2分别位于机器人的头两侧,则设定距离阈值为机器人的头的最大宽度的一半;若第一标签T1和第二标签T2分别位于机器人的两侧肩膀上或者两侧手臂上,则设定距离阈值为机器人的身体的最大宽度的一半,例如机器人的身体的最大宽度为40cm,则设定距离阈值为20cm。如果第一标签T1和第二标签T2之间的距离大于或等于设定距离阈值,则判定第一标签T1和第二标签T2的位置坐标数据可靠,进入步骤B2。如果第一标签T1和第二标签T2之间的距离小于设定距离阈值,则判定第一标签T1和第二标签T2的位置坐标数据不可靠,进入步骤B3。
步骤B2:确定Y轴正方向(即正北方向)为基准方向,机器人朝向该基准方向时的朝向角度为0°,根据第一标签T1的位置坐标(x1,y1)和第二标签T2的位置坐标(x2,y2),利用三角函数,计算由第一标签T1指向第二标签T2的射线相对于基准方向的角度值,将该角度值减去90°,得到机器人相对于基准方向的朝向角度。需要说明的是,由于机器人的朝向与由第一标签T1指向第二标签T2的射线始终存在90°的差值,因此将由第一标签T1指向第二标签T2的射线的角度值减去90°就能够得到机器人的朝向角度。
步骤B3~B5的详细过程请参见上面对步骤B3~B5的相应描述,此处不再赘述。
步骤C:实时计算确定机器人的位置坐标。具体包括以下过程:
根据第一标签T1和第二标签T2的位置坐标(x1,y1)和(x2,y2),计算第一标签T1和第二标签T2的连线的中点的坐标((x1+x2)/2,(y1+y2)/2),将计算得到的坐标作为机器人的位置坐标。需要说明的是,由于第一标签T1和第二标签T2分别位于机器人的头两侧或者两侧肩膀或者两侧手臂上,因此二者连线的中点即可视为机器人所在位置点。
基于上述步骤C确定机器人位置坐标的算法,所确定的机器人的位置坐标的准确度较现有技术有大幅提升,请参见下面的表1:
表1
在上表1中,由于机器人的位置坐标是第一标签T1和第二标签T2的位置坐标的平均值,因此在第一标签T1和第二标签T2的位置坐标有偏移的情况下,机器人的位置坐标的偏移距离也是二者偏移距离的平均值,机器人的位置坐标的误差有50%的概率大幅消减,还有50%概率对误差进行了平均。另外可结合卡尔曼滤波的使用,进一步提升机器人的位置坐标的准确度。
以上内容介绍了如何实时确定机器人的位置坐标和朝向角度,下面将对如何使用所确定的位置坐标和朝向角度进行导航行进的过程进行介绍。
如图4所示,该定位导航方法进行导航时包括以下步骤:
步骤1:采用路径规划算法获取机器人行进的路径目标点的集合。机器人需要依次沿着这些路径目标点行进。
步骤2:实时确定机器人的位置坐标,根据当前时刻机器人的位置坐标和下一时刻路径目标点坐标,计算由当前时刻机器人所在位置点指向下一时刻路径目标点的射线的朝向角度,将该朝向角度记为下一时刻的目标角度。需要说明的是,在本步骤中,“实时确定机器人的位置坐标”可采用上面所介绍的步骤C的流程。计算“由当前时刻机器人所在位置点指向下一时刻路径目标点的射线的朝向角度”的方法,与计算由第一标签指向所述第二标签的射线的角度值的方法类似,可参照该方法进行计算。
步骤3:采用步骤B实时确定机器人的朝向角度,计算下一时刻的目标角度与当前时刻机器人的朝向角度的差值,判断该差值与0的关系,如果该差值大于0,则控制机器人向右旋转该差值的角度;如果该差值等于0,则控制机器人保持当前时刻的朝向角度;如果该差值小于0,则控制机器人向左旋转该差值的角度。需要说明的是,如果该差值大于0,说明机器人下一时刻的朝向相对于当前时刻的朝向偏右,机器人需向右旋转;如果该差值等于0,说明机器人下一时刻的朝向与当前时刻的朝向相同,机器人无需改变朝向;如果该差值小于0,说明机器人下一时刻的朝向相对于当前时刻的朝向偏左,机器人需向左旋转。
步骤4:计算当前时刻机器人所在位置点与下一时刻路径目标点之间的距离,控制机器人行进该距离。
步骤5:判断机器人是否到达最后一个路径目标点,如果是,则结束导航;如果否,则返回步骤2。
基于本实施例所提供的机器人的定位导航方法,本发明的实施例还提供了一种存储介质,该存储介质内存储有指令代码,该指令代码用于执行如本实施例所述的定位导航方法。该存储介质所能够带来的有益效果与本实施例所提供的定位导航方法的有益效果相同,此处不再赘述。
本发明的实施例还提供了一种机器人,该机器人包括处理器和存储器。其中,存储器中存储有指令代码;处理器用于执行该指令代码,以实现如本实施例所述的定位导航方法。该机器人所能够带来的有益效果与本实施例所提供的定位导航方法的有益效果相同,此处不再赘述。该机器人还包括电子罗盘和/或陀螺仪,以用于机器人朝向的确定。
本发明的实施例还提供了一种计算机程序,该计算机程序包括指令代码,该指令代码用于执行如本实施例所提述的定位导航方法。该计算机程序所能够带来的有益效果与本实施例所提供的定位导航方法的有益效果相同,此处不再赘述。
以上所述仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以所述权利要求的保护范围为准。