CN111198496A - 一种目标跟随机器人及跟随方法 - Google Patents

Abstract

本发明公开了一种目标跟随机器人及跟随方法，所述方法包括如下步骤：步骤一，识别图像中的目标；步骤二，融合图像识别结果与雷达点云信息，得到所述目标的雷达点云信息；步骤三，对抗复杂环境下多个同类别目标干扰的处理；步骤四，PID算法得到跟随控制指令；所述机器人包括：相机、雷达、识别模块、控制模块、底盘、融合模块、抗干扰处理模块，所述相机与所述识别模块连接，所述融合模块分别于所述识别模块、所述雷达、所述抗干扰处理模块连接，所述控制模块分别与所述抗干扰处理模块与所述底盘连接。

Description

一种目标跟随机器人及跟随方法

技术领域

本发明涉及智能机器人领域，尤其是涉及一种目标跟随机器人及跟随方法。

背景技术

目标跟随机器人在日常生活和工业制造等领域有广泛的应用需求。例如在生产车间，物流仓库等环境下，有自主跟随功能的机器人可以跟随工人完成快捷可靠的搬运作业，提高生产和运输效率，甚至组成多机器人小队，逐一跟随，实现更高难度的要求。在购物中心，机场车站等场所下，能够克服干扰保持跟随的机器人也可以替人们完成商品、行李的收纳和运输。此外还有服务机器人，特种作业机器人等，也都有不少需要用到目标跟随功能的应用场景。

机器人如果需要保持对目标的跟随，就需要一个能够提供环境信息与位置反馈的感知系统。机器人环境感知的传感器中，摄像头的应用最为广泛。随着近年来深度学习技术的快速发展与广泛应用，基于摄像头图像信息和神经网络推断的感知方案势头迅猛，逐步取代传统图像处理算法，成为物体识别的主流方法，为机器人的整体环境感知能力带来巨大提升。目前，SSD与YOLO等知名算法，已经能够高速高精确性地实现障碍物识别，得到物体轮廓边界。

目前已有一些用于机器人跟随的方法，如专利(CN201710289168.2)利用RFID标签做定位，专利(CN201710455840.0)使用激光雷达的点云匹配做行人轨迹跟踪。专利(CN201610880972.3)利用超声波进行简单测距，三角定位后输出目标跟随数据。专利(201910602590.8)利用深度图像的识别处理，经过坐标系转换调整参数控制机器进行运动。然而单一传感器获得的环境信息有限，同时也易受到环境等因素的干扰，鲁棒性相比多传感器融合的方案有所欠缺。

发明内容

为解决现有技术的不足，实现抗干扰、提高鲁棒性的目的，本发明采用如下的技术方案：

一种目标跟随方法，包括如下步骤：

步骤一，识别图像中的目标；

利用神经网络完成目标识别，将图像识别结果输出；所述图像识别结果包括像素坐标、类别信息；

步骤二，融合图像识别结果与雷达点云信息，得到所述目标的雷达点云信息；降低了成本，毫米波雷达弥补了单目相机的距离探测能力与抗恶劣天气能力较弱的劣势；

(1)转换雷达点云坐标至像素点坐标；

雷达坐标系中点云坐标(x,y,z)与图像坐标系下对应像素点坐标(u,v)之间的转换关系表示如下：

相机的内参矩阵

相机与雷达间的外参

是已知参数，所述Z_c是所述像素点(u,v)所在成像平面与所述雷达坐标系中点云坐标(x,y,z)与之间的垂直距离，所述R是外参矩阵中的旋转部分，所述T是外参矩阵中的平移部分，所述f是相机焦距，所述(u₀,v₀)表示图像坐标系原点在像素坐标系中的坐标，所述dx、所述dy分别是每个像素在图像平面x和y方向上的物理尺寸；

通过合理的外参变换，毫米波雷达的点云坐标系与单目相机的图像坐标系顺利连接起来，使雷达和相机的数据互相匹配，图像中的物体就获得了对应的深度信息；

(2)匹配雷达点云与图像识别结果；

根据所述雷达点云坐标对应的所述像素点坐标(u,v)，匹配所述图像识别结果，为每个雷达点云赋予其对应的所述目标的类别信息，得到新的雷达点云信息，所述新的雷达点云信息包括位置信息(x,y,z)、类别信息。

步骤三，对抗复杂环境下多个同类别目标干扰的处理；

(1)分析所述目标附近识别结果；

设置搜索范围阈值，在所述阈值内搜索目标的雷达点云，并记录多个与所述目标类别相同的雷达点；

(2)利用雷达速度维信息区分相同类别的雷达点，得到所述目标位置；

雷达可以得到所述雷达点云速度信息，对所述目标雷达点云的速度信息进行历史记录，以最近时刻的速度为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置。

在小范围内有多个同类物体的场景下克服干扰，稳定跟随目标；

所述速度信息是径向速度。

步骤四，PID算法得到跟随控制指令；PID算法控制响应快速，有利于对目标保持稳定跟随；

(1)在前进速度方向上采用PID算法计算速度控制指令；

采用PID算法对机器人前进速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的前进速度V_t的转换关系，表述如下：

所述t是当前时刻，所述d_t是所述t时刻下所述机器人与所述目标之间的距离，所述d_i是i时刻下所述机器人与所述目标之间的距离，所述d_O是所述机器人跟随时期望目标保持的距离，所述K_p、K_i、K_d是控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标；速度指令被传递给所述机器人底盘，控制所述机器人对目标跟随的距离；

(2)在水平旋转方向上采用PID算法计算角速度控制指令；

采用PID算法对所述机器人旋转角速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的旋转角速度ω_t的转换关系，表述如下：

所述t是当前时刻，所述θ_t是t时刻下所述机器人前进方向与所述目标之间的水平夹角，θ_i是i时刻下所述机器人前进方向与所述目标之间的水平夹角，所述K_p、K_i、K_d是控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标；角速度指令被传递给机器人底盘，控制机器人在角度上完成目标跟随；

速度和角速度指令被机器人底盘同步执行，能够高速响应。

所述雷达是毫米波雷达。毫米波雷达抗环境干扰能力较强，能为视觉方案补充物体深度和速度信息，相比激光雷达，毫米波雷达具有可靠性好、环境鲁棒性强、成本合理等优势。

所述图像是通过单目相机获取的图像。

一种目标跟随机器人，包括：相机、雷达、识别模块、控制模块、底盘、融合模块、抗干扰处理模块，所述相机与所述识别模块连接，所述融合模块分别于所述识别模块、所述雷达、所述抗干扰处理模块连接，所述控制模块分别与所述抗干扰处理模块与所述底盘连接；

所述相机，用于获取图像；

所述识别模块，用于识别图像中的目标，得到图像识别结果，所述图像识别结果包括所述目标的像素坐标、类别信息；

所述雷达，用于获取目标的雷达点云信息，所述点云信息包括雷达点云坐标、雷达速度信息；

所述融合模块，用于转换雷达点云坐标至像素坐标点，匹配雷达点云与图像识别结果，得到带类别信息的新的雷达点云；

所述抗干扰处理模块，用于搜索所述目标附近的所述雷达点云，得到多个与所述目标类别相同的雷达点；利用雷达速度信息区分相同类别的雷达点，通过历史雷达速度信息，以最近时刻的速度为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置；

所述控制模块，用于根据所述目标位置计算所述机器人前进速度和水平旋转角速度，并向所述底盘发出控制指令。

所述雷达是毫米波雷达。毫米波雷达抗环境干扰能力较强，能为视觉方案补充物体深度和速度信息，相比激光雷达，毫米波雷达具有可靠性好、环境鲁棒性强、成本合理等优势。

所述相机是单目相机。

本发明的优势和有益效果在于：

本发明克服了毫米波雷达点云角分辨率过低，密度不足以识别物体的缺点，也克服了单目相机快速获得物体位置的难题，同时还克服了复杂场景下的同类目标物干扰。充分利用了两种传感器各自的优势，将多维信息有机融合，克服干扰，快速、准确地实现对目标的定位与跟随控制。克服多个物体小范围内聚团时的干扰问题，保持对目标的持续跟踪，更稳定、准确地控制机器人完成目标跟随任务。降低成本，提高机器人的感知能力、抗干扰能力，提升机器人的鲁棒性、可靠性，更快速、准确地完成对目标的跟随。

附图说明

图1是本发明的流程图。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

如图1所示，一种目标跟随方法，包括如下步骤：

步骤一，单目相机识别图像中的目标；

利用卷积神经网络完成视野内的目标识别，根据实际的应用场景，运行迁移学习优化后的物体识别神经网络模型，快速、准确地在单目相机图像上完成推断，将各图像识别结果输出；所述图像识别结果包括中心像素坐标、识别框长宽、类别标签；

步骤二，融合图像识别结果与雷达点云信息，得到所述目标的雷达点云信息；降低了成本，毫米波雷达弥补了单目相机的距离探测能力与抗恶劣天气能力较弱的劣势；

(1)根据已知的传感器外参，转换雷达点云坐标至像素点坐标；

雷达坐标系中点云坐标(x,y,z)与图像坐标系下对应像素点坐标(u,v)之间的转换关系表示如下：

相机的内参矩阵

相机与雷达间的外参

是已知参数，所述Z_c是所述像素点(u,v)所在成像平面与所述雷达坐标系中点云坐标(x,y,z)与之间的垂直距离，所述R是外参矩阵中的旋转部分，所述T是外参矩阵中的平移部分，所述f是相机焦距，所述(u₀,v₀)表示图像坐标系原点在像素坐标系中的坐标，所述dx、所述dy分别是每个像素在图像平面x和y方向上的物理尺寸；

通过合理的外参变换，毫米波雷达的点云坐标系与单目相机的图像坐标系顺利连接起来，使雷达和相机的数据互相匹配，图像中的物体就获得了对应的深度信息；

(2)匹配雷达点云与图像识别结果；

根据所述雷达点云坐标对应的所述像素点坐标(u,v)，匹配步骤一中的所述图像识别结果，从而为每个雷达点云赋予其对应的所述目标的类别标签，得到新的雷达点云信息，所述新的雷达点云信息包括位置信息(x,y,z)、类别标签Obj_label。

步骤三，对抗复杂环境下多个同类别目标干扰的处理；

(1)分析所述目标附近识别结果；

设置搜索范围阈值，在所述阈值内搜索目标的雷达点云，并记录所述雷达点云的类别标签，当有多个与所述目标类别相同的雷达点时，表示有混淆跟踪目标位置的风险，需要利用毫米波雷达采集到的速度信息对抗干扰；

(2)利用雷达速度维信息区分相同类别的雷达点，得到所述目标位置；

雷达可以得到每个所述雷达点云速度信息velocity，对当前所述目标雷达点云的速度信息velocity进行历史记录，以最近时刻的速度值为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置。

在小范围内有多个同类物体的场景下克服干扰，稳定跟随目标；例如，在机器人被要求跟随人的场景下，即使目标人与其他人相遇，交错，依然保持对目标人的跟随，不会跟随到错误的干扰目标上；

所述速度信息是径向速度。

步骤四，PID算法得到跟随控制指令；PID算法控制响应快速，有利于对目标保持稳定跟随；

(1)在前进速度方向上采用PID算法计算速度控制指令；

采用PID算法对机器人前进速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的前进速度V_t的转换关系，表述如下：

所述t是当前时刻，即从0时刻开始到当前时刻的计数值，所述d_t是所述t时刻下所述机器人与所述目标之间的距离，所述d_i是i时刻下所述机器人与所述目标之间的距离，通过求和公式将i从0遍历至t是为了求从0时刻开始到t时刻的累积距离差值，所述d_O是所述机器人跟随时期望目标保持的距离，所述K_p、K_i、K_d是优选的控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标，即所述(x,y,z)中的z和x；速度指令被传递给所述机器人底盘，控制所述机器人对目标跟随的距离；

(2)在水平旋转方向上采用PID算法计算角速度控制指令；

采用PID算法对所述机器人旋转角速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的旋转角速度ω_t的转换关系，表述如下：

所述t是当前时刻，所述θ_t是t时刻下所述机器人前进方向与所述目标之间的水平夹角，θ_i是i时刻下所述机器人前进方向与所述目标之间的水平夹角，通过求和公式将i从0遍历至t是为了求从0时刻开始到t时刻的累计夹角值，所述K_p、K_i、K_d是优选的控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标，即所述(x,y,z)中的z和x；角速度指令被传递给机器人底盘，控制机器人在角度上完成目标跟随；

速度和角速度指令被机器人底盘同步执行，直到下一刻新的指令传达，能够高速响应。

所述雷达是毫米波雷达。毫米波雷达抗环境干扰能力较强，能为视觉方案补充物体深度和速度信息，相比激光雷达，毫米波雷达具有可靠性好、环境鲁棒性强、成本合理等优势，在大目标移动场景下有其独特的优势。

所述图像是通过单目相机获取的图像。

一种目标跟随机器人，包括：相机、雷达、识别模块、控制模块、底盘、融合模块、抗干扰处理模块，所述相机与所述识别模块连接，所述融合模块分别于所述识别模块、所述雷达、所述抗干扰处理模块连接，所述控制模块分别与所述抗干扰处理模块与所述底盘连接；

所述相机，用于获取图像；

所述识别模块，用于识别图像中的目标，得到图像识别结果，所述图像识别结果包括所述目标的中心像素坐标、识别框长宽、类别标签；

所述雷达，用于获取目标的雷达点云信息，所述点云信息包括雷达点云坐标、雷达速度信息；

所述融合模块，用于转换雷达点云坐标至像素坐标点，匹配雷达点云与图像识别结果，得到带类别标签的新的雷达点云；

所述抗干扰处理模块，用于搜索所述目标附近的所述雷达点云，得到多个与所述目标类别相同的雷达点；利用雷达速度维信息区分相同类别的雷达点，通过历史雷达速度维信息，以最近时刻的速度为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置；

所述控制模块，用于根据所述目标位置计算所述机器人前进速度和水平旋转角速度，并向所述底盘发出控制指令。

所述雷达是毫米波雷达。毫米波雷达抗环境干扰能力较强，能为视觉方案补充物体深度和速度信息，相比激光雷达，毫米波雷达具有可靠性好、环境鲁棒性强、成本合理等优势，在大目标移动场景下有其独特的优势。

所述相机是单目相机。

利用单目相机获得图像，根据图像物体识别结果，通过毫米波雷达提供的速度维信息，对视野中临近易混淆的同类物体进行区分，对抗复杂环境下多个同类物体的干扰，将目标的位置反馈给机器人跟随控制算法，采用PID算法得到机器人底盘的控制指令，最终快速、准确地完成对目标的跟随。

Claims (8)

1.一种目标跟随方法，其特征在于，包括如下步骤：

步骤一，识别图像中的目标；

利用神经网络完成目标识别，将图像识别结果输出；所述图像识别结果包括像素坐标、类别信息；

步骤二，融合图像识别结果与雷达点云信息，得到所述目标的雷达点云信息；

(1)转换雷达点云坐标至像素点坐标；

雷达坐标系中点云坐标(x,y,z)与图像坐标系下对应像素点坐标(u,v)之间的转换关系表示如下：

相机的内参矩阵

相机与雷达间的外参

是已知参数，所述Z_c是所述像素点(u,v)所在成像平面与所述雷达坐标系中点云坐标(x,y,z)与之间的垂直距离，所述R是外参矩阵中的旋转部分，所述T是外参矩阵中的平移部分，所述f是相机焦距，所述(u₀,v₀)表示图像坐标系原点在像素坐标系中的坐标，所述dx、所述dy分别是每个像素在图像平面x和y方向上的物理尺寸；

(2)匹配雷达点云与图像识别结果；

根据所述雷达点云坐标对应的所述像素点坐标(u,v)，匹配所述图像识别结果，为每个雷达点云赋予其对应的所述目标的类别信息，得到新的雷达点云信息，所述新的雷达点云信息包括位置信息(x,y,z)、类别信息；

步骤三，对抗复杂环境下多个同类别目标干扰的处理；

(1)分析所述目标附近识别结果；

设置搜索范围阈值，在所述阈值内搜索目标的雷达点云，并记录多个与所述目标类别相同的雷达点；

(2)利用雷达速度维信息区分相同类别的雷达点，得到所述目标位置；

雷达可以得到所述雷达点云速度信息，对所述目标雷达点云的速度信息进行历史记录，以最近时刻的速度为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置。

2.根据权利要求1所述的一种目标跟随方法，其特征在于，所述速度信息是径向速度。

3.根据权利要求1所述的一种目标跟随方法，其特征在于，还包括步骤四，PID算法得到跟随控制指令；

(1)在前进速度方向上采用PID算法计算速度控制指令；

采用PID算法对机器人前进速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的前进速度V_t的转换关系，表述如下：

所述t是当前时刻，所述d_t是所述t时刻下所述机器人与所述目标之间的距离，所述d_i是i时刻下所述机器人与所述目标之间的距离，所述d_O是所述机器人跟随时期望目标保持的距离，所述K_p、K_i、K_d是控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标；速度指令被传递给所述机器人底盘，控制所述机器人对目标跟随的距离；

(2)在水平旋转方向上采用PID算法计算角速度控制指令；

采用PID算法对所述机器人旋转角速度进行控制，所述目标位置坐标(x,y,z)与所述机器人跟随的旋转角速度ω_t的转换关系，表述如下：

所述t是当前时刻，所述θ_t是t时刻下所述机器人前进方向与所述目标之间的水平夹角，θ_i是i时刻下所述机器人前进方向与所述目标之间的水平夹角，所述K_p、K_i、K_d是控制参数，所述z_t和x_t分别是t时刻下所述目标的z坐标与x坐标；角速度指令被传递给机器人底盘，控制机器人在角度上完成目标跟随；

速度和角速度指令被机器人底盘同步执行。

4.根据权利要求1所述的一种目标跟随方法，其特征在于，所述雷达是毫米波雷达。

5.根据权利要求1所述的一种目标跟随方法，其特征在于，所述图像是通过单目相机获取的图像。

6.一种目标跟随机器人，包括：相机、雷达、识别模块、控制模块、底盘，其特征在于，还包括融合模块、抗干扰处理模块，所述相机与所述识别模块连接，所述融合模块分别于所述识别模块、所述雷达、所述抗干扰处理模块连接，所述控制模块分别与所述抗干扰处理模块与所述底盘连接；

所述相机，用于获取图像；

所述识别模块，用于识别图像中的目标，得到图像识别结果，所述图像识别结果包括所述目标的像素坐标、类别信息；

所述雷达，用于获取目标的雷达点云信息，所述点云信息包括雷达点云坐标、雷达速度信息；

所述融合模块，用于转换雷达点云坐标至像素坐标点，匹配雷达点云与图像识别结果，得到带类别信息的新的雷达点云；

所述抗干扰处理模块，用于搜索所述目标附近的所述雷达点云，得到多个与所述目标类别相同的雷达点；利用雷达速度信息区分相同类别的雷达点，通过历史雷达速度信息，以最近时刻的速度为基准，寻找所述类别相同的雷达点中速度最接近的雷达点，判断出下一时刻的目标位置；

所述控制模块，用于根据所述目标位置计算所述机器人前进速度和水平旋转角速度，并向所述底盘发出控制指令。

7.根据权利要求6所述的一种目标跟随机器人，其特征在于，所述雷达是毫米波雷达。

8.根据权利要求6所述的一种目标跟随机器人，其特征在于，所述相机是单目相机。

Images