(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210694242.X (22)申请日 2022.06.17 (71)申请人 西安科技大 学 地址 710054 陕西省西安市雁塔区雁塔中 路58号 (72)发明人 马宏伟　王鹏　孙那新　曹现刚　 张烨　李亚坤　 (74)专利代理 机构 北京鼎德宝 专利代理事务所 (特殊普通 合伙) 11823 专利代理师 钟西飞 (51)Int.Cl. B25J 9/16(2006.01) B25J 15/08(2006.01) G06V 20/00(2022.01) G06V 10/74(2022.01)G06V 10/82(2022.01) G06T 7/277(2017.01) (54)发明名称 煤矸分拣机器人视觉伺服的机械臂动态目 标跟踪轨 迹规划方法 (57)摘要 本发明公开了一种煤矸分拣机器人视觉伺 服的机械臂动态目标跟踪轨迹规划方法， 包括以 下步骤： 基于煤矸视觉检测单元获取煤矸流中的 矸石位置信息和目标矸石模板； 根据目标矸石模 板通过动态目标匹配识别单元和动态目标精准 跟踪单元实现目标矸石的精确实时定位； 基于主 控子系统内的动态分拣轨迹规划单元将目标矸 石实时位置和当前机械臂末端之间的空间相对 位置信息作为输入信息进行机械臂动态轨迹规 划， 所得的机械臂动态轨迹规划通过主控子系统 接口发送至机械臂控制子系统， 机械臂根据接收 到的轨迹信息通过机械臂控制器完成目标矸石 的分拣。 应用本发明替代人工分拣， 结合对应的 机械臂同步跟踪煤矸石轨迹规划方法能够极大 地提高煤矿智能化水平。 权利要求书3页 说明书6页 附图2页 CN 114932556 A 2022.08.23 CN 114932556 A 1.一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法， 其特征在于， 包括如下步骤： S1、 基于煤矸视觉检测单元获取图像信息， 并获取煤矸的识别结果， 得到煤矸流中的矸 石位置信息和目标矸石模板； S2、 基于动态目标匹配识别单元采用HU不变矩匹配算法实现目标矸石模板的处理， 进 行目标矸石匹配识别， 得到目标矸石的初始位置信息； S3、 动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信 息作 为输入， 采用卡尔曼滤波扩展的FDSST跟踪算法快速跟踪目标矸石， 得到目标矸石的实时位 置信息； S4、 识别定位子系统接口将所述动态目标精确跟踪单元获取的目标矸石位置信息， 发 送至主控子系统进行目标矸石信息预处 理， 即矸石位置坐标计算； S5、 动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息， 采用三环PID控 制算法进行机 械臂末端轨 迹规划， 获得机 械臂末端 ‑目标矸石任务轨 迹规划； S6、 通过主控子系统接口将获得的机械臂末端 ‑目标矸石任务轨迹规划发送至机械臂 控制子系统， 机 械臂控制器根据轨 迹信息执 行并完成矸石的分拣任务。 2.根据权利要求1所述的煤矸分拣机器人视觉伺 服的机械臂动态目标跟踪轨迹规划方 法， 其特征在于， 步骤S1中， 煤矸视觉检测单元采用双目立体相机获取图像信息， 采用FCNN 网络获取煤矸的识别结果。 3.根据权利要求1所述的煤矸分拣机器人视觉伺 服的机械臂动态目标跟踪轨迹规划方 法， 其特征在于， 步骤S4中， 所述 矸石位置坐标计算 流程如下： 第一步： 根据步骤S3中所述得到的目标矸石几何形心像素坐标位置信息(x， y)， 单位为 pixel， 获取目标矸石在机 械臂末端坐标系下的坐标值(XM， YM， ZM)， 单位为m， 表示公式如下： 其中， M1为相机内参矩阵， 通 过相机标定获取； 分别为相机坐标系相对于机械臂 末端坐标系的旋转矩阵和平移矩阵， ZC为相机坐标系原点距目标矸石的垂直距离， 单位为 m； 第二步： 计算得到 机器人全局坐标系下目标矸石位置坐标值， 具体如下式： 其中， XTar、 YTar和ZTar分别表示 目标矸石在机器人全局 坐标系下的X、 Y、 Z坐标， XMal、 YMal 和ZMal分别表示通过读取机器人控制器获取的机器人全局坐标系下的机械臂末端位置坐标 值信息。 4.根据权利要求1所述的煤矸分拣机器人视觉伺 服的机械臂动态目标跟踪轨迹规划方 法， 其特征在于， 步骤S5中， 所述机械臂末端轨迹规划通过三环PID控制算法进行规划实现 的， 以机械臂上相机捕捉到目标矸石的当前位置(期 望位置)， 作为位置环控制器的输入， 位权　利　要　求　书 1/3 页 2 CN 114932556 A 2置环控制器的输出作为速度环控制器的输入， 速度环控制器的输出作加速度 环控制器的输 入， 最后综合得到位置环控制变量通过转换输出叠加到伺服电机上， 共同作用控制 机械臂 运动， 使机 械臂末端与目标煤矸石达 到位置速度同步 运动的效果， 实现平稳 快速抓取。 5.根据权利要求1所述的煤矸分拣机器人视觉伺 服的机械臂动态目标跟踪轨迹规划方 法， 其特征在于， 步骤S 5中， 所述机械臂末端轨迹规划通过三环PID控制算法进 行规划实现， 包括如下步骤： 第一步： 确定目标矸石在机械臂可抓取范围内， 结合步骤S3和S4得到实时机器人全局 坐标系下的目标矸石坐标位置信息 第二步： 根据当前目标矸石位置坐标 当前机械臂末端位置坐 标 分别可分为X、 Y、 Z三轴轴向， 计算 当前目标矸石 位置与当前机 械臂末端位置的位置偏差值ΔSk； 计算为当前的位置偏差值ΔSk与上一次末端位置偏差值 ΔSlast之间的偏差值ΔS ′； 计算累计末端的位置偏差值ΔSsum； 具体如下式： 式中， Sk为末端当前 更新的位置， Slast为上一次末端的位置； 第三步： 根据当前目标矸石位置坐标 当前机械臂末端位置坐 标 分别可分为X、 Y、 Z三轴轴向， 计算当前目标矸石位置与当前机 械臂末端位置的速度偏差值ΔVk； 计算为当前的速度偏差值ΔVk与上一次末端速度偏差值 ΔVlast之间的偏差值ΔV ′； 计算累计末端的速度偏差值ΔVsum； 具体如下式： 其中： 式中， Vk为末端当前更新的速度， Vlast为上一次末端的速度； dt为时间步长， Vmax为最大 速度； 第四步： 根据当前目标矸石位置坐标 当前机械臂末端位置坐 标 分别可分为X、 Y、 Z三轴轴向， 计算当前目标矸石加速度与当前 机械臂末端加速度的加速度偏差值ΔAk； 计算为当前的加速度偏差值ΔAk与上一次末端加 速度偏差值ΔAlast之间的偏差值ΔA ′； 计算累计末端的加速度偏差值ΔAsum； 具体如下式： 其中：权　利　要　求　书 2/3 页 3 CN 114932556 A 3