一种基于UWB阵列模块的位姿估计方法、系统及机器人与流程

本发明涉及机器人定位及位姿估计技术领域，特别涉及一种基于uwb阵列模块的位姿估计方法、系统及机器人。

背景技术：

目前，室内移动机器人的路径控制主要是磁导航、激光导航和视觉导航等方式。磁带导航需要在作业现场铺设磁条引导机器人行进，其不足之处是：(1)灵活性不足，路径若有变更需要重新铺设磁条；(2)对于多机器人的复杂路径或交叉路径，磁条引导方式较难实现。激光引导方式需要机器人配置较高性能的激光导航部件，成本较高。视觉导航方式主要基于图像背景的匹配定位原理，存在以下不足：(1)位置精度位差较大；(2)当作业环境的改变时需要重新布置图像背景；(3)光线变化(如逆光)对于匹配定位影响较大。

针对机器人的路径规划和定位功能需求，无线定位方式从不同角度弥补了上述不足之处。目前的无线定位技术包括wifi、rfid和蓝牙等，但这几种定位方式的精度均为米级水平，限制了这些技术在室内环境中的应用。

uwb(ultrawideband)技术是一种利用亚纳秒级超窄脉冲的无载波通信技术，相对于上述无线定位技术uwb技术具有通信距离远、高数据传输速率、抗干扰能力强、功耗小等优势，在实际应用中能控制误差在30cm内，相对其他定位技术使用uwb技术可以获得更高的定位精度和稳定性。现有的uwb技术中往往仅使用单点定位功能，无法判断机器人的方向性位姿信息，不利于机器人移动信息的全面掌控以及机器人之间的相互感知和协调移动或作业。

技术实现要素：

有鉴于此，本发明实施例提供了一种基于uwb阵列模块的位姿估计方法、系统及机器人。

第一方面，本发明提供一种基于uwb阵列模块的位姿估计系统，所述系统包括uwb阵列模块及位于所述uwb阵列模块外部的外部uwb基站，所述uwb阵列模块包括第一uwb子模块、第二uwb子模块，所述第一uwb子模块包括第一uwb标签，所述第二uwb子模块包括第二uwb标签，所述第一uwb标签和所述第二uwb标签与所述外部uwb基站通过到达角度测距法或这到达时间差法计算第一距离值、第二距离值、第一朝向角度以及第二朝向角度值，根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据，其中，所述第一距离值为所述第一uwb标签与所述外部uwb基站的距离值，所述第二距离值为所述第二uwb标签与所述外部uwb基站的距离值，所述第一朝向角度值为所述第一uwb标签与所述外部uwb基站的朝向角度值，所述第二朝向角度值为所述第二uwb标签与所述外部uwb基站的朝向角度值。

可选地，所述第一uwb子模块包括第一uwb基站，所述第二uwb子模块包括第二uwb基站，所述外部uwb基站具有外部uwb标签，所述第一uwb基站、所述第二uwb基站与所述外部uwb标签进行通讯连接。

可选地，所述外部uwb基站具有一个，所述第一uwb标签和所述第二uwb标签通过aoa方法或者tdoa方法估算出所述外部uwb基站相对所述第一uwb标签和所述第二uwb标签中心的距离值以及所述第一uwb标签和所述第二uwb标签连线的角度值。

可选地，所述外部uwb基站具有两个或两个以上，所述第一uwb标签和所述第二uwb标签通过aoa或者tdoa方法分别估算出所述第一uwb标签和所述第二uwb标签的位置坐标，再推算出所述第一uwb标签和所述第二uwb标签的中点的坐标，以及推算出所述第一uwb标签和所述第二uwb标签的朝向角度。

可选地，所述uwb阵列模块分别位于两个机器人，分别第一机器人和第二机器人。

第二方面，本发明提供一种基于uwb阵列模块的位姿估计方法，应用在如上项所述的基于uwb阵列模块的位姿估计系统，所述方法包括：

获取第一uwb标签与外部uwb基站的距离值；

获取第二uwb标签与外部uwb基站的距离值；

获取第一uwb标签与外部uwb基站的朝向角度值；

获取第二uwb标签与外部uwb基站的朝向角度值；

根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据。

第三方面，本发明提供一种机器人，所述机器人具有uwb阵列模块，所述uwb阵列模块包括第一uwb子模块、第二uwb子模块，所述第一uwb子模块包括第一uwb标签，所述第二uwb子模块包括第二uwb标签，所述第一uwb标签和所述第二uwb标签用于与所述外部uwb基站通过到达角度测距法或这到达时间差法计算第一距离值、第二距离值、第一朝向角度以及第二朝向角度值，根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据，其中，所述第一距离值为所述第一uwb标签与所述外部uwb基站的距离值，所述第二距离值为所述第二uwb标签与所述外部uwb基站的距离值，所述第一朝向角度值为所述第一uwb标签与所述外部uwb基站的朝向角度值，所述第二朝向角度值为所述第二uwb标签与所述外部uwb基站的朝向角度值。

可选地，所述第一uwb子模块包括第一uwb基站，所述第二uwb子模块包括第二uwb基站，所述外部uwb基站具有外部uwb标签，所述第一uwb基站、所述第二uwb基站与所述外部uwb标签进行通讯连接。

从以上技术方案可以看出，本发明实施例具有以下优点：

本发明提出在将uwb阵列模块布设于机器人本体，对于单uwb基站，实现机器人与该基站的距离和角度方位的估算；对于固定多uwb基站，实现机器人自身在地图中坐标稳健定位以及自身朝向位姿的稳健估算；对于多基站(基站距离固定)可移动单元，实现估算该可移动单元的朝向位姿的估算。

附图说明

图1是本发明实施例中的基于uwb阵列模块的位姿估计系统的结构框图。

具体实施方式

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

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

结合图1所示，本发明提供一种基于uwb阵列模块的位姿估计系统，所述系统包括uwb阵列模块及位于所述uwb阵列模块外部的外部uwb基站，所述uwb阵列模块包括第一uwb子模块、第二uwb子模块，所述第一uwb子模块包括第一uwb标签，所述第二uwb子模块包括第二uwb标签，所述第一uwb标签和所述第二uwb标签与所述外部uwb基站通过到达角度测距法或这到达时间差法计算第一距离值、第二距离值、第一朝向角度以及第二朝向角度值，根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据，其中，所述第一距离值为所述第一uwb标签与所述外部uwb基站的距离值，所述第二距离值为所述第二uwb标签与所述外部uwb基站的距离值，所述第一朝向角度值为所述第一uwb标签与所述外部uwb基站的朝向角度值，所述第二朝向角度值为所述第二uwb标签与所述外部uwb基站的朝向角度值。

可选地，所述第一uwb子模块包括第一uwb基站，所述第二uwb子模块包括第二uwb基站，所述外部uwb基站具有外部uwb标签，所述第一uwb基站、所述第二uwb基站与所述外部uwb标签进行通讯连接。

可选地，所述外部uwb基站具有一个，所述第一uwb标签和所述第二uwb标签通过aoa方法或者tdoa方法估算出所述外部uwb基站相对所述第一uwb标签和所述第二uwb标签中心的距离值以及所述第一uwb标签和所述第二uwb标签连线的角度值。

可选地，所述外部uwb基站具有两个或两个以上，所述第一uwb标签和所述第二uwb标签通过aoa或者tdoa方法分别估算出所述第一uwb标签和所述第二uwb标签的位置坐标，再推算出所述第一uwb标签和所述第二uwb标签的中点的坐标，以及推算出所述第一uwb标签和所述第二uwb标签的朝向角度。

可选地，所述uwb阵列模块分别位于两个机器人，分别第一机器人和第二机器人。

对于前文中提到的uwb定位方法，下面进行简单介绍：

一、接收信号强度定位法

接收信号强度定位法是通过信号强度和已知信号衰弱模型来估计接收点与待测点的距离，根据多个接收点距离待测点的距离值画出圆，多个圆的重叠部分就是待测物体。

在移动装置中，接收信号强度常用接收信号强度指标rssi来表示，在理想环境中这种方式可以获得较为精确的定位。

二、到达时间定位法(timedifferenceofarrival到达时间差)

到达时间定位法和接收信号强度定位法类似，只不过计算接收点和测量点之间的距离是采用无线信号传播时间乘以无线信号传输速度。

三、接收信号角度定位法(angle-of-arrival：aoa)

接收信号角度定位法的工作原理：是利用定向天线量测出信号来源方向，多个接收点同时按照接收角度画出直线，直线的交点就是目标位置。

信号角度定位法的优点是：不需要每一个接收天线都做时间同步，误差来源主要是角度解析的误差。在距离越远的时候角度解析的误差影响越大；同时由于信号反射和折射，可能造成最终计算出来的被测点，是发射点而不是被测物体本身；由于定向天线的制作对测量角度的计算影响较大，在实际应用中会比较麻烦。

采用uwb进行定位具有系统容量大，带宽增加使信道容量的提高远远大于信号功率上升所带来的效应，这一点也正是提出超宽带技术的理论机理。超宽带无线电系统用户数量大大高于3g系统；高速的数据传输，uwb系统使用上吉赫兹的超宽频带，根据香农信道容量公式，即使把发送信号功率密度控制得很低，也可以实现高的信息速率。一般情况下，其最大数据传输速度可以达到几百兆比特每秒到吉比特每秒；多径分辨能力强，uwb由于其极高的工作频率和极低的占空比而具有很高的分辨率，窄脉冲的多径信号在时间上不易重叠，很容易分离出多径分量，所以能充分利用发射信号的能量。实验表明，对常规无线电信号多径衰落深达10～30db的多径环境，uwb信号的衰落最多不到5db；隐蔽性好，因为uwb的频谱非常宽，能量密度非常低，因此信息传输安全性高。另一方面，由于能量密度低，uwb设备对于其他设备的干扰就非常低；定位精确，冲激脉冲具有很高的定位精度，采用超宽带无线电通信，可在室内和地下进行精确定位，精度最高可达2厘米，一般精度在15厘米内。而gps定位系统只能工作在gps定位卫星的可视范围之内。与gps提供绝对地理位置不同，超短脉冲定位器可以给出相对位置，其定位精度可达厘米级；抗干扰能力强，uwb扩频处理增益主要取决于脉冲的占空比和发送每个比特所用的脉冲数。uwb的占空比一般为0.01～0.001，具有比其他扩频系统高得多的处理增益，抗干扰能力强。一般来说，uwb抗干扰处理增益在50db以上；低成本和低功耗，uwb无线通信系统接收机没有本振、功放、锁相环(pll)、压控振荡器(vco)、混频器等，因而结构简单，设备成本将很低。由于uwb信号无需载波，而是使用间歇的脉冲来发送数据，脉冲持续时间很短，一般在0.20～1.5ns之间，有很低的占空因数，所以它只需要很低的电源功率。一般uwb系统只需要50～70mw的电源，是蓝牙技术的十分之一。

无线uwb技术最基本的工作原理是发送和接收脉冲间隔严格受控的高斯单周期超短时脉冲，超短时单周期脉冲决定了信号的带宽很宽，接收机直接用一级前端交叉相关器就把脉冲序列转换成基带信号，省去了传统通信设备中的中频级，极大地降低了设备复杂性。

无线uwb技术采用脉冲位置调制ppm单周期脉冲来携带信息和信道编码，一般工作脉宽0.1-1.5ns(1纳秒＝一亿分之一秒)，重复周期在25-1000ns。

对应地，本发明提供一种基于uwb阵列模块的位姿估计方法，应用在如上项所述的基于uwb阵列模块的位姿估计系统，所述方法包括：

s101、获取第一uwb标签与外部uwb基站的距离值；

s102、获取第二uwb标签与外部uwb基站的距离值；

s103、获取第一uwb标签与外部uwb基站的朝向角度值；

s104、获取第二uwb标签与外部uwb基站的朝向角度值；

s105根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据。

为了在高精度无线定位技术上，实现机器人方向性位姿信息估算，本发明提出在将uwb阵列模块布设于机器人本体。对于单uwb基站，实现机器人与该基站的距离和角度方位的估算；对于固定多uwb基站，实现机器人自身在地图中坐标稳健定位以及自身朝向位姿的稳健估算；对于多基站(基站距离固定)可移动单元，实现估算该单元的朝向位姿的估算。

对应地，本发明提供一种机器人，所述机器人具有uwb阵列模块，所述uwb阵列模块包括第一uwb子模块、第二uwb子模块，所述第一uwb子模块包括第一uwb标签，所述第二uwb子模块包括第二uwb标签，所述第一uwb标签和所述第二uwb标签用于与所述外部uwb基站通过到达角度测距法或这到达时间差法计算第一距离值、第二距离值、第一朝向角度以及第二朝向角度值，根据第一距离值、第二距离值、第一朝向角度以及第二朝向角度值确定uwb阵列模块的姿态数据，其中，所述第一距离值为所述第一uwb标签与所述外部uwb基站的距离值，所述第二距离值为所述第二uwb标签与所述外部uwb基站的距离值，所述第一朝向角度值为所述第一uwb标签与所述外部uwb基站的朝向角度值，所述第二朝向角度值为所述第二uwb标签与所述外部uwb基站的朝向角度值。

可选地，所述第一uwb子模块包括第一uwb基站，所述第二uwb子模块包括第二uwb基站，所述外部uwb基站具有外部uwb标签，所述第一uwb基站、所述第二uwb基站与所述外部uwb标签进行通讯连接。

使用本发明中提出的uwb阵列模块，可以使用成本较低(相对激光传感器)的无线定位技术，实现精度较高的定位功能，同时还能估算出布设uwb阵列模块的机器人的位姿状态(即朝向角度)信息。便于对机器人进行后续的导航运动控制或跟随运动控制。对于布设有同样uwb阵列模块的多个机器人，可以进行机器人之间的位置和位姿状态感知，便于进行后续的协调工作控制。

当外界环境是单个uwb基站的应用场景时，可仅使用模块中的两个uwb标签t1和t2，通过aoa方法或者tdoa方法估算出该基站相对t1和t2中心的距离值以及t1和t2连线的角度值。

当外界环境是多个固定uwb基站的应用场景时，可仅使用模块中的两个uwb标签t1和t2，通过aoa或者tdoa方法分别估算出t1和t2的位置坐标，再推算出机器人中心即t1和t2的中点的坐标，以及推算出t1和t2的朝向角度。

当布设有同样uwb阵列模块am的机器人a与机器人b同在一个环境中时，阵列模块am-a和am-b中的基站bs1-a、bs2-a、bs1-b、bs2-b和标签t1-a、t2-a、t1-b、t2-b可以全部使用，使用t1-a、t2-a通过aoa方法或者tdoa方法对bs1-b和bs2-b(即机器人b)的朝向角度值和距离值进行估算；同理使用t1-b和t2-b通过aoa方法或者tdoa方法对bs1-a和bs2-a(即机器人a)的朝向角度值和距离值进行估算，实现机器人之间距离及朝向的相互感知功能，便于进行协调工作。

使用本发明中提出的uwb阵列模块，可以使用成本较低(相对激光传感器)的无线定位技术，实现精度较高的定位功能，同时还能估算出布设uwb阵列模块的机器人的位姿状态(即朝向角度)信息。便于对机器人进行后续的导航运动控制或跟随运动控制。对于布设有同样uwb阵列模块的多个机器人，可以进行机器人之间的位置和位姿状态感知，便于进行后续的协调工作控制。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(rom，readonlymemory)、随机存取存储器(ram，randomaccessmemory)、磁盘或光盘等。

以上对本发明所提供的一种基于uwb阵列模块的位姿估计方法、系统及机器人进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。