一种基于UWBLOC技术的机器人自主跟随人的方法

UWBLOC跟随模块

机器人小车

第一步 如下图所示在机器人小车上安装上3个华星智控的UWBLOC跟随模块，三个模块安装为等边三角形，安装在同一个平面上。人和机器人处在同样环境中，身上穿戴有一个UWBLOC跟随模块传感器作为标签，假设人初始时刻已被机器人锁定。3个基站按等边三角形三个顶点位置固定在移动机器人上，用来发射UWB信号，标签接收到信号后，通过TOA算法可算出标签分别距3个基站的距离，并通过几何运算得到人相对于机器人的位置。

第2步 机器人和人所处的环境是三维空间，以机器人本身为参考系建立一个局部坐标系统。如图下图所示，定义3个基站位置距离AB、BC、AC均为a；定义BC边上的高所在延长线为y轴，y轴正向与机器人自身的方向一致；3个基站构成的等边三角形AB边的中点为O（0，0，0）；标签坐标为M（x，y，z），其中z为标签到基站所在水平面的高度；等边三角形的中心为H。

第3步 根据3个基站到标签的距离，通过解析几何可以算出标签M在等边三角形所在平面的投影M点到等边三角形中心的距离：L=MH=（d14+d24+d34-d12d22-d12d32-d22d32）÷（3a2）（1）标签M在等边三角形所在平面的投影M′点与等边三角形中心H点的连线，即MH与y轴的夹角即方位角：θ=∠MHA=arctan（3（d32-d22）÷（-2d12+d22+d32））（2）由此，人的位置坐标可以表示为（L，θ），即确定出人相对于机器人的位置信息，所得位置误差范围在±100mm以内，位置信息基于机器人和人直接计算，不存在累积误差。

第4步 利用人相对于机器人的坐标，机器人实现跟随的步骤为：①机器人根据人的位置坐标，解算出人在以机器人本身为参考坐标系中x，y方向的偏移量；②机器人在x方向上移动，最终使人在坐标系中x方向的偏移量为零；③机器人在y方向上移动，最终使人在坐标系中y方向的偏移量为零；④当人在坐标系中x，y方向的偏移量都为零时，即机器人此时到达人的位置，从而实现跟随。