煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法技术

本发明专利技术公开了一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，包括以下步骤：基于煤矸视觉检测单元获取煤矸流中的矸石位置信息和目标矸石模板；根据目标矸石模板通过动态目标匹配识别单元和动态目标精准跟踪单元实现目标矸石的精确实时定位；基于主控子系统内的动态分拣轨迹规划单元将目标矸石实时位置和当前机械臂末端之间的空间相对位置信息作为输入信息进行机械臂动态轨迹规划，所得的机械臂动态轨迹规划通过主控子系统接口发送至机械臂控制子系统，机械臂根据接收到的轨迹信息通过机械臂控制器完成目标矸石的分拣。应用本发明专利技术替代人工分拣，结合对应的机械臂同步跟踪煤矸石轨迹规划方法能够极大地提高煤矿智能化水平。地提高煤矿智能化水平。地提高煤矿智能化水平。

【技术实现步骤摘要】
煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法


[0001]本专利技术涉及分拣
，具体涉及一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法。

技术介绍

[0002]利用机器人进行分拣已经成为一种新的研究方向，目前工业上的分拣机器人多是生产线上的分拣，是针对小质量、低速度目标进行分拣，在机械臂达到抓取点时只需确保位置精度，无需速度同步。而对于机器人分拣煤矸石来讲，煤矸石随带式输送机运动具有高速度特性，并且矸石平均质量大，如果机械臂末端在抓取时只要求位置同步而对速度不做要求，则会因目标和机械臂之间速度差产生载荷冲击，严重时会造成机械臂损坏，降低机器人安全性和可靠性。

技术实现思路

[0003]针对机器人分拣煤矸石时，因输送带打滑、左右摆动而造成矸石定位不准确、机械臂末端抓取失败和载荷冲击等问题，提出了一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，可满足对高速度目标的同步跟踪、精准抓取要求。
[0004]为实现上述目的，本专利技术采取的技术方案为：
[0005]一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，包括如下步骤：
[0006]S1、基于煤矸视觉检测单元获取图像信息，并获取煤矸的识别结果，得到煤矸流中的矸石位置信息和目标矸石模板；
[0007]S2、基于动态目标匹配识别单元采用HU不变矩匹配算法实现目标矸石模板的处理，进行目标矸石匹配识别，得到目标矸石的初始位置信息；
[0008]S3、动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信息作为输入，采用卡尔曼滤波扩展的FDSST跟踪算法快速跟踪目标矸石，得到目标矸石的实时位置信息；
[0009]S4、识别定位子系统接口将所述动态目标精确跟踪单元获取的目标矸石位置信息，发送至主控子系统进行目标矸石信息预处理，即矸石位置坐标计算；
[0010]S5、动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息，采用三环PID控制算法进行机械臂末端轨迹规划，获得机械臂末端
‑
目标矸石任务轨迹规划；
[0011]S6、通过主控子系统接口将获得的机械臂末端
‑
目标矸石任务轨迹规划发送至机械臂控制子系统，机械臂控制器根据轨迹信息执行并完成矸石的分拣任务。
[0012]作为本方案的进一步地设计，步骤S1中，煤矸视觉检测单元采用双目立体相机获取图像信息，采用FCNN网络获取煤矸的识别结果。
[0013]作为本方案的进一步地设计，步骤S4中，所述矸石位置坐标计算流程如下：
[0014]第一步：根据步骤S3中所述目标矸石几何形心像素坐标位置信息(x，y)，单位为pixel，获取目标矸石在机械臂末端坐标系下的坐标值(X
M
，Y
M
，Z
M
)，单位为m，表示公式如下：
[0015][0016]其中，M1为相机内参矩阵，通过相机标定获取；分别为相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移矩阵。Z
C
为相机坐标系原点距目标矸石的垂直距离，单位为m；
[0017]第二步：计算得到机器人全局坐标系下目标矸石位置坐标值，具体如下式：
[0018][0019]其中，X
Tar
、Y
Tar
和Z
Tar
分别表示目标矸石在机器人全局坐标系下的X、Y、Z坐标，X
Mal
、Y
Mal
和Z
Mal
分别表示通过读取机器人控制器获取的机器人全局坐标系下的机械臂末端位置坐标值信息。
[0020]作为本方案的进一步地设计，步骤S5中，所述机械臂末端轨迹规划通过三环PID控制算法进行规划实现的，以机械臂上相机捕捉到目标矸石的当前位置(期望位置)，作为位置环控制器的输入，位置环控制器的输出作为速度环控制器的输入，速度环控制器的输出作加速度环控制器的输入，最后综合得到位置环控制变量通过转换输出叠加到伺服电机上，共同作用控制机械臂运动，使机械臂末端与目标煤矸石达到位置速度同步运动的效果，实现平稳快速抓取。
[0021]作为本方案的进一步地设计，步骤S5中，所述机械臂末端轨迹规划通过三环PID控制算法进行规划实现，包括如下步骤：
[0022]第一步：确定目标矸石在机械臂可抓取范围内，结合步骤S3和S4得到实时机器人全局坐标系下的目标矸石坐标位置信息
[0023]第二步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为X、Y、Z三轴轴向，计算当前目标矸石位置与当前机械臂末端位置的位置偏差值ΔS
k
；计算为当前的位置偏差值ΔS
k
与上一次末端位置偏差值ΔS
last
之间的偏差值ΔS
′
；计算累计末端的位置偏差值ΔS
sum
；具体如下式：
[0024][0025]式中，S
k
为末端当前更新的位置，S
last
为上一次末端的位置；
[0026]第三步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为X、Y、Z三轴轴向，计算当前目标矸石速度与当前机械臂末端速度的速度偏差值ΔV
k
；计算为当前的速度偏差值ΔV
k
与上一次末端速度偏
差值ΔV
last
之间的偏差值ΔV
′
；计算累计末端的速度偏差值ΔV
sum
；具体如下式：
[0027][0028]其中：
[0029][0030]式中，V
k
为末端当前更新的速度，V
last
为上一次末端的速度；dt为时间步长，V
max
为最大速度；
[0031]第四步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为X、Y、Z三轴轴向，计算当前目标矸石加速度与当前机械臂末端加速度的加速度偏差值ΔA
k
；计算为当前的加速度偏差值ΔA
k
与上一次末端加速度偏差值ΔA
last
之间的偏差值ΔA
′
；计算累计末端的加速度偏差值ΔA
sum
；具体如下式：
[0032][0033]其中：
[0034][0035]其中，A
k
为末端当前更新的加速度；A
last
为上一次末端的加速度；A
max
为最大加速度；
[0036]第五步：结合第二、三、四步中公式，计算机械臂末端加速度、速度、位置控制调整量A(k)，V(k)，S(k)，表达式如下：
[0037][0038]式中：K
P
为比例系数；K
I
为积分系数；K
D
为微分系数；
[0039]第六步：更新下一时刻机械臂末本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，其特征在于，包括如下步骤：S1、基于煤矸视觉检测单元获取图像信息，并获取煤矸的识别结果，得到煤矸流中的矸石位置信息和目标矸石模板；S2、基于动态目标匹配识别单元采用HU不变矩匹配算法实现目标矸石模板的处理，进行目标矸石匹配识别，得到目标矸石的初始位置信息；S3、动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信息作为输入，采用卡尔曼滤波扩展的FDSST跟踪算法快速跟踪目标矸石，得到目标矸石的实时位置信息；S4、识别定位子系统接口将所述动态目标精确跟踪单元获取的目标矸石位置信息，发送至主控子系统进行目标矸石信息预处理，即矸石位置坐标计算；S5、动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息，采用三环PID控制算法进行机械臂末端轨迹规划，获得机械臂末端
‑
目标矸石任务轨迹规划；S6、通过主控子系统接口将获得的机械臂末端
‑
目标矸石任务轨迹规划发送至机械臂控制子系统，机械臂控制器根据轨迹信息执行并完成矸石的分拣任务。2.根据权利要求1所述的煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，其特征在于，步骤S1中，煤矸视觉检测单元采用双目立体相机获取图像信息，采用FCNN网络获取煤矸的识别结果。3.根据权利要求1所述的煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，其特征在于，步骤S4中，所述矸石位置坐标计算流程如下：第一步：根据步骤S3中所述得到的目标矸石几何形心像素坐标位置信息(x，y)，单位为pixel，获取目标矸石在机械臂末端坐标系下的坐标值(X
M
，Y
M
，Z
M
)，单位为m，表示公式如下：其中，M1为相机内参矩阵，通过相机标定获取；分别为相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移矩阵，Z
C
为相机坐标系原点距目标矸石的垂直距离，单位为m；第二步：计算得到机器人全局坐标系下目标矸石位置坐标值，具体如下式：其中，X
Tar
、Y
Tar
和Z
Tar
分别表示目标矸石在机器人全局坐标系下的X、Y、Z坐标，X
Mal
、Y
Mal
和Z
Mal
分别表示通过读取机器人控制器获取的机器人全局坐标系下的机械臂末端位置坐标值信息。4.根据权利要求1所述的煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，其特征在于，步骤S5中，所述机械臂末端轨迹规划通过三环PID控制算法进行规划实现的，以机械臂上相机捕捉到目标矸石的当前位置(期望位置)，作为位置环控制器的输入，位
置环控制器的输出作为速度环控制器的输入，速度环控制器的输出作加速度环控制器的输入，最后综合得到位置环控制变量通过转换输出叠加到伺服电机上，共同作用控制机械臂运动，使机械臂末端与目标煤矸石达到位置速度同步运动的效果，实现平稳快速抓取...

【专利技术属性】
技术研发人员：马宏伟，王鹏，孙那新，曹现刚，张烨，李亚坤，
申请(专利权)人：西安科技大学，
类型：发明
国别省市：