用于室内机器人的定位方法及装置、机器人与流程

本申请涉及机器人室内定位领域，具体而言，涉及一种用于室内机器人的定位方法及装置。

背景技术：

常见的室内定位技术有通过wififootprint，uwb全局定位等技术。

发明人发现，在进行室内定位是容易受环境变化影响。同时还存在测距设备的量程限制。进一步，对光的依赖程度高。

针对相关技术中室内定位容易受到环境变化影响的问题，目前尚未提出有效的解决方案。

技术实现要素：

本申请的主要目的在于提供一种用于室内机器人的定位方法及装置，以解决室内定位容易受到环境变化影响的问题。

为了实现上述目的，根据本申请的一个方面，提供了一种用于室内机器人的定位方法。

根据本申请的于室内机器人的定位方法包括：获取机器人的里程计信息，预先安装至少一个uwb用于提供测距信息，确定初始化位置；根据实时的里程计信息和所述至少一个uwb中测距信息对定位位置进行更新，所述方法包括：确定初始化位置x，y、方向yaw、以及协方差矩阵p；获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态。

进一步地，计算x和p的估计值时，包括：

其中噪声w(wx，wa)表示线误差与角误差比例，x’为下一个位置。

进一步地，根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点包括：

根据实时获取的x结合p，生成确定性采样点：x0..x2l，更新所述确定性采样点为x‘0..x’2l，

x′＝x+vx*dt*cos(yaw)*(1+wx)

y′＝y+vx*dt*sin(yaw)*(1+wx)

yaw′＝yaw+va*dt*(1+wa)。

进一步地，方法还包括：

循环执行所述获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；所述根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的执行步骤。

进一步地，对于第k个uwb测距信息，其中k＝1..n，并采用ukf状态转移方程更新状态变量。

为了实现上述目的，根据本申请的另一方面，提供了一种用于室内机器人的定位装置。

根据本申请的用于室内机器人的定位装置包括：获取机器人的里程计信息，预先安装至少一个uwb用于提供测距信息，确定初始化位置；根据实时的里程计信息和所述至少一个uwb中测距信息对定位位置进行更新，所述方法包括：确定模块，用于确定初始化位置x，y、方向yaw、以及协方差矩阵p；获取模块，用于获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；确定性采样点模块，用于根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；更新模块，用于计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态。

进一步地，所述更新模块，用于

其中噪声w(wx，wa)表示线误差与角误差比例，x’为下一个位置。

进一步地，所述确定性采样点模块，用于

根据实时获取的x结合p，生成确定性采样点：x0..x2l，更新所述确定性采样点为x‘0..x’2l，

x′＝x+vx*dt*cos(yaw)*(1+wx)

y′＝y+vx*dt*sin(yaw)*(1+wx)

yaw′＝yaw+va*dt*(1+wa)。

进一步地，还包括：循环模块，用于

循环执行所述获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；

所述根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；

计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的执行步骤。

为了实现上述目的，根据本申请的又一方面，提供了一种机器人，用于执行所述的用于室内机器人的定位方法。

在本申请实施例中，采用确定初始化位置x，y、方向yaw、以及协方差矩阵p的方式，通过获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)，达到了根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点的目的，从而实现了计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的技术效果，进而解决了室内定位容易受到环境变化影响的技术问题。

此外，本申请中的一种用于室内机器人的定位方法，部署灵活，可以根据现场情况，部署一个或多个uwb，数目更多精度越高。且适用于空旷场合，光照无影响。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的用于室内机器人的定位方法流程示意图；

图2是根据本申请实施例的用于室内机器人的定位装置结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本申请中，术语“上”、“下”、“左”、“右”、“前”、“后”、“顶”、“底”、“内”、“外”、“中”、“竖直”、“水平”、“横向”、“纵向”等指示的方位或位置关系为基于附图所示的方位或位置关系。这些术语主要是为了更好地描述本申请及其实施例，并非用于限定所指示的装置、元件或组成部分必须具有特定方位，或以特定方位进行构造和操作。

并且，上述部分术语除了可以用于表示方位或位置关系以外，还可能用于表示其他含义，例如术语“上”在某些情况下也可能用于表示某种依附关系或连接关系。对于本领域普通技术人员而言，可以根据具体情况理解这些术语在本申请中的具体含义。

此外，术语“安装”、“设置”、“设有”、“连接”、“相连”、“套接”应做广义理解。例如，可以是固定连接，可拆卸连接，或整体式构造；可以是机械连接，或电连接；可以是直接相连，或者是通过中间媒介间接相连，又或者是两个装置、元件或组成部分之间内部的连通。对于本领域普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

本申请实施例中的目的在于提供一种室内机器人的定位方案，包括位置和朝向信息。通过本申请实施例中的方法采用结合机器人里程计以及uwb的融合定位方案，可以取长补短，给出低成本，低计算负荷，相对可靠的全局定位方案，也可以作为位置找回的手段。

如图1所示，在执行本申请实施例中的定位方法时，首先需要获取机器人的里程计信息，预先安装至少一个uwb用于提供测距信息，确定初始化位置；根据实时的里程计信息和所述至少一个uwb中测距信息对定位位置进行更新。

具体地，本申请的实施例中使用到机器人的里程计信息(即线速度和角速度)，以及事先安装的n(n>＝1)个uwb提供的测距信息，本申请的实施例中需要初始化位置，然后根据实时的里程计信息和uwb测距信息来更新定位位置。

方法还包括如下的步骤s100至步骤s103：

步骤s100，确定初始化位置x，y、方向yaw、以及协方差矩阵p；

步骤s101，获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；

步骤s102，根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；

步骤s103，计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态。

具体地，确定初始化位置x，y、方向yaw、以及协方差矩阵p，得到状态x(x,y,yaw)。包括：位置(x,y)以及朝向yaw以及协方差矩阵p。还包括噪声w(wx，wa)，用于作为线误差与角误差比例。

获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt；

根据该些信息对位置进行更新初始状态x(x，y，yaw)；

根据实时获取的状态x结合协方差矩阵p，生成确定性采样点，并更新所述确定性采样点。

需要注意的是，可以通过设置初始化位置(x,y)和方向(yaw)，以及可根据经验设定p的初始值。

需要注意的是，上述生成确定性采样点，并更新所述确定性采样点是指，根据现有的x，p产生确定性采样点为sigmapoints：x0..x2l。然后更新确定性采样点sigmapoints为x‘0..x’2l。

计算估计值，并根据已知的uwb位置uxi，uyi，测距误差noise_d，获取n个uwb测距信息为d1..dn，依次更新状态。

从以上的描述中，可以看出，本申请实现了如下技术效果：

在本申请的实施例中，通过初始化位置，然后根据实时的里程计信息和uwb测距信息来更新定位位置。

在本申请实施例中，采用确定初始化位置x，y、方向yaw、以及协方差矩阵p的方式，通过获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)，达到了根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点的目的，从而实现了计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的技术效果，进而解决了室内定位容易受到环境变化影响的技术问题。基于上述室内机器人的定位方案，包括位置和朝向信息。

根据本申请实施例，作为本实施例中的优选该方法包括：

计算x和p的估计值时，包括：

其中噪声w(wx,wa)表示线误差与角误差比例，x’为下一个位置。

根据本申请实施例，作为本实施例中的优选，根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点包括：

根据实时获取的x结合p，生成确定性采样点：x0..x2l，更新所述确定性采样点为x‘0..x’2l，

x'＝x+vx*dt*cos(yaw)*(1+wx)

y'＝y+vx*dt*sin(yaw)*(1+wx)

yaw'＝yaw+va*dt*(1+wa)。

根据本申请实施例，作为本实施例中的优选，还包括：

循环执行所述获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；

所述根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；

计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的执行步骤。

优选地，对于第k个uwb测距信息，其中k＝1..n，并采用ukf状态转移方程更新状态变量。

需要注意的是，ukf状态转移方程本领域技术人员可以进行自行选择，并通过ukf状态转移方程更新在uwb测距信息中的状态变量。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

根据本申请实施例，还提供了一种用于实施上述方法的用于室内机器人的定位装置，获取机器人的里程计信息，预先安装至少一个uwb用于提供测距信息，确定初始化位置；根据实时的里程计信息和所述至少一个uwb中测距信息对定位位置进行更新，如图2所示，该装置包括：确定模块10，用于确定初始化位置x，y、方向yaw、以及协方差矩阵p；获取模块20，用于获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；确定性采样点模块30，用于根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；更新模块40，用于计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态。

在本申请实施例的上述模块中，具体地，确定初始化位置x，y、方向yaw、以及协方差矩阵p，得到状态x(x,y,yaw)。包括：位置(x,y)以及朝向yaw以及协方差矩阵p。还包括噪声w(wx，wa)，用于作为线误差与角误差比例。

获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt；

根据该些信息对位置进行更新初始状态x(x，y，yaw)；

根据实时获取的状态x结合协方差矩阵p，生成确定性采样点，并更新所述确定性采样点。

需要注意的是，可以通过设置初始化位置(x,y)和方向(yaw)，以及可根据经验设定p的初始值。

需要注意的是，上述生成确定性采样点，并更新所述确定性采样点是指，根据现有的x，p产生确定性采样点为sigmapoints：x0..x2l。然后更新确定性采样点sigmapoints为x‘0..x’2l。

计算估计值，并根据已知的uwb位置uxi，uyi，测距误差noise_d，获取n个uwb测距信息为d1..dn，依次更新状态。

具体地，所述更新模块，用于

其中噪声w(wx,wa)表示线误差与角误差比例，x’为下一个位置。

具体地，所述确定性采样点模块，用于

根据实时获取的x结合p，生成确定性采样点：x0..x2l，更新所述确定性采样点为x‘0..x’2l，

x'＝x+vx*dt*cos(yaw)*(1+wx)

y'＝y+vx*dt*sin(yaw)*(1+wx)

yaw′＝yaw+va*dt*(1+wa)。

具体地，模块还包括：循环模块，用于

循环执行所述获取里程计信息中的角速度vx、线速度va，刷新时间间隔dt，并根据该些信息对位置进行更新初始状态x(x，y，yaw)；

所述根据实时获取的x结合p，生成确定性采样点，并更新所述确定性采样点；

计算x和p的估计值，获取n个uwb测距信息为d1..dn，并依次更新状态的执行步骤。

本申请的实现原理如下：

本申请实施例中使用到机器人的里程计信息(即线速度和角速度)，以及事先安装的n(n＞＝1)个uwb测距信息提供的测距信息，本申请实施例中需要初始化位置，然后根据实时的里程计信息和uwb测距信息来更新定位位置。基本步骤如下。

具体地，以ukf实现为例：

状态表示为x(x，y，yaw)，分别为位置(x，y)以及朝向yaw，协方差矩阵p，噪声w(wx，wa)(线误差与角误差比例)。

设置初始化位置(x，y)和方向(yaw)，以及p的初始值(可根据经验设定)循环执行如下步骤：

获取里程计信息，角速度和线速度vx，va，刷新时间间隔dt，根据此信息来更新(x，y，yaw)。

根据现有的x，p产生sigmaρoints：x0..x2l，更新sigmapoints为：

x‘0..x’2l：

x‘＝x+vx*dt*cos(yaw)*(1+wx)

y′＝y+vx*dt*sin(yaw)*(1+wx)

yaw′＝yaw+va*dt*(1+wa)

其中，状态变量包括x坐标，y坐标和角度。

误差wx,wa为里程计线速度和角速度数据的误差估计。

计算估计值如下：

已知uwb位置uxi，uyi，测距误差noise_d，获取n个uwb测距信息为d1..dn，依次更新状态。对于第k个uwb，采用ukf状态转移方程更新状态变量。

显然，本领域的技术人员应该明白，上述的本申请的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本申请不限制于任何特定的硬件和软件结合。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。