[发明专利]一种多信息融合的伙伴机器人相对定位方法

权利要求书

1.一种多信息融合的伙伴机器人相对定位方法，其特征在于，包括以下步骤：

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

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

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

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

2.根据权利要求1所述的一种多信息融合的伙伴机器人相对定位方法，其特征在于，所述的当前机器人和伙伴机器人均搭载有相对定位模块；

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

3.根据权利要求1或2所述的一种多信息融合的伙伴机器人相对定位方法，其特征在于，对毫米波雷达探测到的所有点筛选，仅保留伙伴机器人潜在存在的区域中的点；

D_f＝{d|l-l_ε＜d＜l+l_ε，d∈D}

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

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

l_ε为UWB测距的一般经验误差；

D_f为筛选后的子集；

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

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

4.根据权利要求3所述的一种多信息融合的伙伴机器人相对定位方法，其特征在于，连续采集若干个定位周期的信息，获得在每个定位周期始末伙伴机器人的潜在位置组成的集合，构建在若干次迭代中，伙伴机器人相对当前机器人的所有潜在运动轨迹；

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

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

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

5.根据权利要求3所述的一种多信息融合的伙伴机器人相对定位方法，其特征在于，在每个周期，执行如下操作：

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

2计算聚类结果集合C_p中各元素到p_last的距离，以最接近伙伴机器人在这一周期内运动的距离l_neighbor的元素作为伙伴机器人当前相对位置的估计值，记录为

3更新

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

6.根据权利要求5所述的一种多信息融合的伙伴机器人相对定位方法，其特征在于，计算和之间的差值，当偏差大于一定阈值时，判断机器人之间存在障碍物，以航位推算的结果作为相对定位的结果；

但判断机器人之间不存在障碍物时，以作为相对定位结果，同时，利用相对定位结果更新IMU基准方向之间的偏差θ_error。