一种多信息融合的伙伴机器人相对定位方法

摘要

本发明公开了一种多信息融合的伙伴机器人相对定位方法。为了克服单一的伙伴机器人定位技术都存在各自的弊端的问题；本发明包括以下步骤：S1：当前机器人使用毫米波雷达探测周围环境，获得反射信号形成的点云；S2：当前机器人通过UWB与伙伴机器人进行通信测距，获得伙伴机器人的实名测距结果；S3：当前机器人与伙伴机器人通过航位推算，分别获得各自的当前位置，当前机器人接收伙伴机器人的当前位置信息，获得两者相对位置的估计结果；S4：当前机器人根据同一时间的点云、实名测距结果和相对位置估计结果获得伙伴机器人的相对位置。通过将毫米波雷达和UWB结合，实现伙伴机器人之间点对点的定位，并且可以降低干扰物的影响。

说明书

技术领域

本发明涉及一种机器人定位领域，尤其涉及一种基于毫米波雷达和UWB结合的伙伴机器人相对定位方法。

背景技术

多机器人系统中，机器人之间的相对定位是实现多机器人协同作业的基础。超宽带(Ultra Wide Band，UWB)技术作为一种高精度定位技术，也在多机器人相对定位中得到广泛应用。

基于UWB的伙伴定位方法通常利用了UWB系统可以完成点对点的精确测距这一特性，对于二维空间，当系统中存在4台机器人时，借助UWB测距，就可以确定机器人之间的相对距离。基于这一原理，衍生出了TOF，TDOA等多种定位方法。但是这些方法都必须保证系统中4台以上机器人可以完成两两相互测距，如果部分机器人之间由于距离太远或者存在遮挡而无法完成测距时，算法失效。类似的方法包括在环境中部署UWB基站以辅助定位等。

例如，一种在中国专利文献上公开的“一种融合里程计与UWB的多机器人协作定位方法”，其公告号CN112833876B，，包括以下步骤：S1：利用里程计和UWB数据采集模块分别采集机器人的位姿信息和多组UWB节点之间的距离信息；S2：基于采集的多组UWB节点之间距离信息，通过非线性优化算法实现多机器人协作定位；S3：基于非线性优化后的多机器人定位信息，融合机器人里程计提供的信息，构建位姿图，并进行优化，实现多机器人协作的精确定位。

基于航位推算的方法是伙伴机器人之间相对定位的另外一种方法，这种方法的思路是首先通过初始化，确定两台机器人的初始相对位置，然后通过航位推算估计各机器人的运动量，最终更新机器人之间的相对位置。这类方法需要面对的挑战是航位推算所用的IMU存在累计误差，随着时间的推移，误差会逐渐变大。

基于探测的方法是伙伴机器人相对定位的另外一种方法，例如使用微波雷达、激光雷达、机器视觉等方法，从环境中主动识别出伙伴机器人，并确定伙伴机器人的相对位置。但是这种方法存在的问题是容易把环境中的干扰目标误认为是伙伴机器人，以及无法区分不同的伙伴机器人。

单一的伙伴机器人定位技术都存在各自的弊端，传统上使用UWB进行伙伴机器人定位需要4个以上机器人能够相互感知才能完成，而依赖微波雷达等基于主动探测的相对定位技术难以区分伙伴机器人和干扰物。

发明内容

本发明主要解决现有技术单一的伙伴机器人定位技术都存在各自的弊端的问题；提供一种多信息融合的伙伴机器人相对定位方法，通过将毫米波雷达和UWB结合，实现伙伴机器人之间点对点的定位，并且可以降低干扰物的影响。

本发明的上述技术问题主要是通过下述技术方案得以解决的：

一种多信息融合的伙伴机器人相对定位方法，包括以下步骤：

S1：当前机器人使用毫米波雷达探测周围环境，获得反射信号形成的点云；

S2：当前机器人通过UWB与伙伴机器人进行通信测距，获得伙伴机器人的实名测距结果；

S3：当前机器人与伙伴机器人通过航位推算，分别获得各自的当前位置，当前机器人接收伙伴机器人的当前位置信息，获得两者相对位置的估计结果；

S4：当前机器人根据同一时间的点云、实名测距结果和相对位置估计结果获得伙伴机器人的相对位置。

本方案将毫米波雷达与UWB技术结合，可以使当前机器人在不依赖辅助机器人或额外的基站的情况下，就能够对伙伴机器人进行定位，实现点对点的相对定位。使得多台机器人可以排列成单线阵型，以接力的形式进行相互定位，最终使得末端的机器人可以获得首端机器人的位置，从而对巷道、洞穴等狭窄空间进行探索，有助于拓展多机器人系统的协同作业应用。

作为优选，所述的当前机器人和伙伴机器人均搭载有相对定位模块；

所述的相对定位模块包括用于探测周围环境的毫米波雷达、增强雷达信号反射强度的角反射器、用于机器人之间相对测距的UWB测距模块、用于航位推算的惯性传感器以及无线电通信模块。

将毫米波雷达-UWB-IMU整合成独立的协同定位模块，可以方便的安装于不同的机器人上，易于使用。使多机器人系统实现一对一的伙伴定位，而不需要依赖额外的基站，也不需要依赖多个(大于2)机器人才能完成定位，一方面提高了系统的鲁棒性，另外一方面也可以使多机器人系统以链式队形深入洞穴、巷道等狭窄的空间，进行区域探索。

作为优选，对毫米波雷达探测到的所有点筛选，仅保留伙伴机器人潜在存在的区域中的点；

D

其中，D为毫米波雷达探测到的所有点构成的集合；

l为通过UWB探测到的距离伙伴机器人的距离；

l

D

对筛选后保留下来的点使用DBSCAN算法进行聚类；聚类结果所表示的位置为伙伴机器人的潜在位置；

根据航位推算的信息，对聚类的结果进行多次迭代筛选，最终确定伙伴机器人的位置。

本方案的目的是减少计算量，从而加快定位速度。对于复杂环境，尤其是存在大量干扰源的环境，能有效定位。

作为优选，连续采集若干个定位周期的信息，获得在每个定位周期始末伙伴机器人的潜在位置组成的集合，构建在若干次迭代中，伙伴机器人相对当前机器人的所有潜在运动轨迹；

在每次迭代中，基于两台机器人航位推算的结果，计算伙伴机器人的移动距离；

以运动轨迹各段的移动距离与伙伴机器人真实移动距离的偏差为依据，对伙伴机器人的每条潜在运动轨迹进行评分，以偏差最小的轨迹作为伙伴机器人的真实运动轨迹；据此可以获得伙伴机器人在连续若干个定位周期中的位置，以及获得在当前时刻，伙伴机器人相对于当前机器人的相对位置；

结合当前机器人和伙伴机器人的航位推算结果，获得两台机器人IMU的基准方向偏差角度。

作为优选，在每个周期，执行如下操作：

<1>在当前机器人的载体坐标系内计算伙伴机器人当前位置p

<2>计算聚类结果集合C

<3>更新

<4>基于航位推算更新伙伴机器人的相对位置，得到伙伴机器人当前相对位置的估计值，记录为

初始化完成后，具体执行计算获取伙伴机器人的相对位置。

作为优选，计算

但判断机器人之间不存在障碍物时，以

当两机器人之间有障碍物遮挡时，基于UWB和毫米波雷达的定位结果变得不准确。但是单纯依赖航位推算，定位结果存在累积误差。因此使用短时间内的航位推算结果对基于UWB和毫米波雷达的定位结果进行监测，判断机器人之间是否有障碍物。同时使用UWB和毫米波雷达的定位结果修正航位推算的累积误差，

本发明的有益效果是：

1.将毫米波雷达与UWB技术结合，可以使当前机器人在不依赖辅助机器人或额外的基站的情况下，就能够对伙伴机器人进行定位，实现点对点的相对定位。

2.使得多台机器人可以排列成单线阵型，以接力的形式进行相互定位，最终使得末端的机器人可以获得首端机器人的位置，从而对巷道、洞穴等狭窄空间进行探索，有助于拓展多机器人系统的协同作业应用。

3.使用毫米波雷达对伙伴机器人进行探测，可以保证定位效果不受环境光的影响，同时综合UWB的测距结果从毫米波雷达的探测结果中筛选伙伴机器人的位置，可以识别各伙伴机器人的身份。

4.将毫米波雷达-UWB-IMU整合成独立的协同定位模块，可以方便的安装于不同的机器人上，易于使用。

附图说明

图1是本发明的伙伴机器人相对定位方法流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：

本实施例的一种多信息融合的伙伴机器人相对定位方法应用于多机器人系统。

本实施所展示的场景为，使用由两台机器人组成的多机器人系统对目标区域进行探索，这两台机器人中，第一机器人搭载采样设备，而第二机器人搭载探测传感器。当第二机器人发现疑似目标后，将由第一机器人进行抵近采样。为实现这一目标，第一机器人必须能够随时获得第二机器人相对于自身的位置，从而保持对第二机器人的跟随，实现这一双机器人系统的协同作业。

为实现这一目标，每台机器人都搭载有一台伙伴机器人相对定位模块，每个相对定位模块包括毫米波雷达、角反射器、超宽带(UWB)测距模块，惯性传感器(IMU)，无线电通信设备以及嵌入式计算机。

其中毫米波雷达用于对周围环境进行探测，识别出包括伙伴机器人在内的周围所有目标。

角反射器用于增强雷达信号反射强度，从而保证被伙伴机器人的毫米波雷达探测时，反射的信号足够强，不会被淹没在背景噪音中。

UWB测距模块用于实现伙伴机器人之间的相对测距，是区分伙伴机器人和干扰目标的关键。

IMU用于完成航位推算，即获取在两次定位的间隔中，机器人的运动量是多少。这一信息有助于提高相对定位的准确性。

无线电通信设备用于将自身的航位推算结果发送给伙伴机器人，作为其进行相对定位的依据。

相对定位模块自带的嵌入式计算机用于完成相对定位所需的计算。

基于上述相对定位模块，第一机器人按照下面的过程获得第二机器人的相对位置。同样的，第二机器人也可以遵循类似的步骤获得第一机器人相对于其的位置。第一机器人与第二机器人互为伙伴机器人。

如图1所示，本实施例的一种多信息融合的伙伴机器人相对定位方法包括以下步骤：S1：当前机器人使用毫米波雷达探测周围环境，获得反射信号形成的点云。

在本实施例中，在两台机器人刚被部署的那一刻，第一机器人可以通过无线电通信得知第二机器人的存在，但是并不知道第二机器人的初始位置。同样，由于两台机器人的初始状态不同，各自搭载的IMU的基准方向也不同，此时需要通过初始化建立两台机器人之间的联系。

第一机器人使用毫米波雷达对周围环境进行扫描，获得了周围所有目标的反射信号构成的点云，这些点中包含第二机器人的反射信号，但是第一机器人无法识别出来。

S2：当前机器人通过UWB与伙伴机器人进行通信测距，获得伙伴机器人的实名测距结果。

当前机器人使用UWB模块与携带UWB模块的伙伴机器人进行通信测距，从而获得到伙伴机器人的精确测距结果，称为实名测距结果。第一机器人通过UWB测距获得了与第二机器人的相对距离，这一测距结果较为准确，误差通常为分米级。

基于测距结果，考虑测距误差后，对采集到的点云进行筛选，仅保留到第一机器人的距离与测距结果相近的点，可以认为第二机器人的反射信号就在保留下来的这些点中。

设毫米波雷达探测到的所有点构成的集合为D，通过UWB探测到的距离伙伴机器人的距离为l，UWB测距的一般经验误差为l

对集合D中的点进行筛选，得到其子集D

D

即对于毫米波雷达探测到的所有点，只保留到伙伴机器人可能存在的这一环形区域中的点。这一操作的目的是减少计算量，从而加快定位速度。对于复杂环境，尤其是存在大量干扰源的环境，能有效定位。

对筛选后保留下来的点使用DBSCAN算法进行聚类，聚类结果所表示的位置就是第二机器人的潜在位置；此时的聚类结果可能有多个。

对集合D

聚类结果形成集合C

其中，p

n为集合C

默认毫米波雷达的朝向与IMU的基准方向一致。

然后借助航位推算的信息，对聚类的结果进行筛选，最终确定第二机器人的位置。该过程通过多次迭代实现。

S3：当前机器人与伙伴机器人通过航位推算，分别获得各自的当前位置，当前机器人接收伙伴机器人的当前位置信息，获得两者相对位置的估计结果。

在本实施例中，借助航位推算的信息，对聚类的结果进行筛选，最终确定第二机器人的位置。该过程通过多次迭代实现。

其基本思路是，连续采集若干个定位周期的信息，获得在每个定位周期始末第二机器人的潜在位置组成的集合，最终构建在若干次迭代中，第二机器人的所有可能运动轨迹(相对于第一机器人)。

同时，在每次迭代中，基于两台机器人航位推算的结果，计算第二机器人的移动距离。对第二机器人的每条可能运动轨迹进行评分，评分标准是运动轨迹各段的移动距离和第二机器人真实移动距离的偏差情况，以偏差最小的那条轨迹作为第二机器人的真实运动轨迹，据此可以获得第二机器人在连续若干个定位周期中的位置，以及获得在当前时刻，第二机器人相对于第一机器人的相对位置。进而结合第一机器人和第二机器人的航位推算结果，可以获得两台机器人IMU的基准方向偏差角度。

当前机器人和待定位的伙伴机器人均搭载有IMU模块，可以对各自的运动情况进行航位推算。

假定机器人定位周期为ΔT，则在T

其中，L

θ

L

θ

由于两个机器人所搭载的IMU基准方向不同，因此两个角度θ

两机器人启动后，首先进入初始化阶段。

对于连续两个定位周期T

假设集合

对上述两个集合中所有点运用这一假设，得到集合K：

其中，

dis(·)为求解两个位置之间距离的函数。

根据l

上述过程连续进行若干周期，在本实施例中进行3个周期，在每个周期都将得到一个上述集合K，分别为K

从集合K

按照上述方法构建所有序列，以评分最低的序列的终点作为伙伴机器人的当前位置，记录为p

基于p

S4：当前机器人根据同一时间的点云、实名测距结果和相对位置估计结果获得伙伴机器人的相对位置。

完成初始化后，算法进入正常运行过程。在每个周期，执行如下操作：

<1>在当前机器人的载体坐标系内计算p

其中，

<2>计算C

<3>更新

<4>基于航位推算更新伙伴机器人的相对位置，得到伙伴机器人当前相对位置的估计值，记录为

定位结果监测和累积误差消除：

当两机器人之间有障碍物遮挡时，基于UWB和毫米波雷达的定位结果变得不准确。但是单纯依赖航位推算，定位结果存在累积误差。因此使用短时间内的航位推算结果对基于UWB和毫米波雷达的定位结果进行监测，判断机器人之间是否有障碍物。同时使用UWB和毫米波雷达的定位结果修正航位推算的累积误差，具体为：

在每一拍，计算

如果认为机器人之间不存在障碍物，则以

在完成初始化后，可以获得当前时刻，第二机器人相对于第一机器人的相对位置，以及第一机器人和第二机器人所搭载IMU的基准方向偏差角度。

在之后的每个定位周期，基于毫米波雷达和UWB反馈的信息，可以得到一组第二机器人的潜在位置。结合上一个周期末第二机器人的相对位置，计算这组潜在位置所代表的第二机器人在本周期的移动距离。根据航位推算提供的第二机器人的真实移动距离，从这一组潜在位置中筛选出最符合的那一个作为第二机器人相对位置的一个估计值

同时，基于第一机器人和第二机器人的航位推算结果，可以得到第二机器人相对位置的另外一个估计值

当机器人之间存在障碍物遮挡时，毫米波雷达的探测失效，且UWB的测距结果也会出现较大的偏差。基于上述两个位置估计值，可以判断当前两个机器人之间是否有障碍物遮挡，以选择较为准确的那个作为定位结果。

当

同时，根据连续两拍的第二机器人相对定位结果，第一机器人和第二机器人的航位推算结果，对两台机器人的IMU基准方向偏差进行重新计算，以消除累积误差。

当

同时，定位模块重新进入初始化过程。当初始化结束，两个机器人之间仍然有遮挡时，初始化的结果和航位推算的结果会表现出偏差大于阈值，因此模块又将回到初始化过程。直到两个机器人之间的障碍物消失，此时初始化的结果和航位推算的结果偏差会小于阈值，此时系统才会重新回到正常定位阶段。

应理解，实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。