[发明专利]一种目标跟随机器人及跟随方法

权利要求书

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所述的一种目标跟随方法，其特征在于，所述图像是通过单目相机获取的图像。