(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210595176.0 (22)申请日 2022.05.28 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 朱威　韩慧　王国强　何德峰　 郑雅羽　 (74)专利代理 机构 杭州赛科专利代理事务所 (普通合伙) 33230 专利代理师 汪爱平 (51)Int.Cl. G06T 7/246(2017.01) G06T 7/73(2017.01) G06T 7/80(2017.01) H04B 17/318(2015.01)H04N 13/204(2018.01) H04W 64/00(2009.01) (54)发明名称 一种基于视觉和UWB定位的移动机器人跟随 方法 (57)摘要 本发明涉及一种基于视觉和UWB定位的移动 机器人跟随方法， 基于U WB定位技术， 使得移动机 器人在多目标场景下确定跟随的目标， 并以RGBD 相机对确定的目标进行跟随。 本发明通过UWB定 位确定RGBD相机视野内的跟随目标， 有效地解决 了移动机器人在复杂环境下无法确定跟随目标 的问题。 权利要求书3页 说明书9页 附图3页 CN 115239759 A 2022.10.25 CN 115239759 A 1.一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在于： 所述方法基于UWB定 位技术， 使得移动机器人在多目标场景下确定跟随的目标， 并以RGBD相机对确定的目标进 行跟随。 2.根据权利要求1所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 所述方法包括以下步骤： 步骤1： 配置UWB定位系统， 基于UWB定位系统进行 数据采集和预处 理； 步骤2： 对RGBD相机进行 标定， 并获得相机坐标系与像素坐标系的坐标转 化关系； 步骤3： 以RGBD相机采集图像， 以目标检测器检测目标， 得到每个目标的外接矩形坐标， 并确认所述外接矩形在像素坐标系下的左上角像素坐标(UMIN,VMIN)和右下角像素坐标 (UMAX,VMAX)； 步骤4： 从已检测目标中选择跟随 目标； 步骤5： 采用相关滤波器更新跟随 目标并确定跟随距离； 步骤6： 采用PID算法对移动机器人的线速度和角速度进行控制， 进 而实现目标的跟随。 3.根据权利要求2所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 所述步骤1中， UWB定位系统包括搭载于移动机器人的基站及 对应每个目标设置的标签， 基站与移动机器人的主控制器配合设置， 主控制器采集每个标签到基站的距离Dis、 角度 Angle、 信号强度RS SI。 4.根据权利要求3所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 对于采集的数据的预处 理包括： 判断在当前时刻基站所接收在信 号范围内标签的信 号强度RSSI是否在阈值THRSSI范围 内， 若是， 则将距离Dis和角度Angle按式(1)和(2)转化为标签在基站坐标系下的坐标信息 (XT‑A,YT‑A)， 否则循环等待； 其中， THRSSI的取值范围为[6,10]， 单位 为db。 5.根据权利要求2所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 所述步骤2中， RGBD相机水平 放置， 标定相机的内参， 如式(3)获得相机坐标系P的坐标数 据(X,Y,Z)与像素坐标系Puv的像素数据(U,V)的转 化关系， 其中， U、 V表示RGBD图像各个像素点的行列号， cx、 cy表示图像中心像素点行列号， Z表示 深度图像中各个像素点到相机平面的深度距离， fx、 fy表示相机水平、 垂直方向的焦距， K为 相机的内参矩阵。 6.根据权利要求2所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在权　利　要　求　书 1/3 页 2 CN 115239759 A 2于： 所述步骤4包括以下步骤： 步骤4.1： 若步骤3只 检测到一个目标， 则直接将该目标作 为跟随目标， 并进行步骤5， 否 则进行下一步； 步骤4.2： 根据步骤2中的相机坐标系与像素坐标系的转换关系， 将所有 的像素坐标系 下的目标的矩形外接框的两个像素点坐标(UMIN,VMIN)和(UMAX,VMAX)转化为相机坐标系下的 二维坐标(XMIN,YMIN)和(XMAX,YMAX)； 步骤4.3： 根据步骤1中的跟随目标的标签在基站坐标系下的坐标信息(XT‑A,YT‑A)， 判断 出跟随目标在基站坐标系下的坐标位置； 步骤4.4： 根据步骤2中各传感器与移动机器人的坐标转换关系， 通过ROS中的TF树， 将 跟随目标在基站坐标系下的坐标信息(XT‑A,YT‑A)转化为相机坐标下的二维坐标信息(XTag, YTag)； 步骤4.5： 将XTag与每个目标的XMIN和XMAX进行比较， 通过判断XTag是否位于相机坐标系下 的目标的坐标 范围[XMIN,XMAX]内， 从而确定最终的跟随 目标。 7.根据权利要求6所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 若XTag在多个目标的坐标范围内， 则选择XTag与 差距最小的目标作为最终 的跟随目标。 8.根据权利要求2所述的一种基于视觉和UWB定位的移动机器人跟随方法， 其特征在 于： 所述步骤5包括以下步骤： 步骤5.1： 由YOLO目标检测器检测出真实的跟 随目标， 在当前帧进行采样处理， 用目标 和背景信息作为训练分类器， 得到相关滤波器， 考虑到速度和精度两方面因素， 采用核相关 滤波算法KCF作为相关滤波器； 将下一帧图像与相关滤波器做卷积 计算， 响应最大的区域作 为跟随的预测目标， 并进行目标的更新迭代； 步骤5.2： 在预测目标位置利用与彩色图相对齐的深度图获取跟随目标与相机的距离， 取当前帧内的跟随目标的外接矩形框内中心像素点的深度值， 和以中心像素点为中心、 外 接矩形框的高度值和宽度值各三分之一为边长构建的矩形的四个顶点的像素点的深度值 做均值处 理， 如式(4)， 其中， ratio为常数； x,y为当前帧内的跟随目标的外接矩形框左上角坐标的横坐标值 和纵坐标值； h,w为当前帧内的跟随目标的外接矩形框的高度值和宽度值。 d1,d2为构建的矩权　利　要　求　书 2/3 页 3 CN 115239759 A 3