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

1.本发明涉及分拣技术领域，具体涉及一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法。


背景技术：

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


技术实现要素：

3.针对机器人分拣煤矸石时，因输送带打滑、左右摆动而造成矸石定位不准确、机械臂末端抓取失败和载荷冲击等问题，提出了一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，可满足对高速度目标的同步跟踪、精准抓取要求。
4.为实现上述目的，本发明采取的技术方案为：
5.一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，包括如下步骤：
6.s1、基于煤矸视觉检测单元获取图像信息，并获取煤矸的识别结果，得到煤矸流中的矸石位置信息和目标矸石模板；
7.s2、基于动态目标匹配识别单元采用hu不变矩匹配算法实现目标矸石模板的处理，进行目标矸石匹配识别，得到目标矸石的初始位置信息；
8.s3、动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信息作为输入，采用卡尔曼滤波扩展的fdsst跟踪算法快速跟踪目标矸石，得到目标矸石的实时位置信息；
9.s4、识别定位子系统接口将所述动态目标精确跟踪单元获取的目标矸石位置信息，发送至主控子系统进行目标矸石信息预处理，即矸石位置坐标计算；
10.s5、动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息，采用三环pid控制算法进行机械臂末端轨迹规划，获得机械臂末端-目标矸石任务轨迹规划；
11.s6、通过主控子系统接口将获得的机械臂末端-目标矸石任务轨迹规划发送至机械臂控制子系统，机械臂控制器根据轨迹信息执行并完成矸石的分拣任务。
12.作为本方案的进一步地设计，步骤s1中，煤矸视觉检测单元采用双目立体相机获取图像信息，采用fcnn网络获取煤矸的识别结果。
13.作为本方案的进一步地设计，步骤s4中，所述矸石位置坐标计算流程如下：
14.第一步：根据步骤s3中所述目标矸石几何形心像素坐标位置信息(x，y)，单位为pixel，获取目标矸石在机械臂末端坐标系下的坐标值(xm，ym，zm)，单位为m，表示公式如下：
[0015][0016]
其中，m1为相机内参矩阵，通过相机标定获取；分别为相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移矩阵。zc为相机坐标系原点距目标矸石的垂直距离，单位为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三轴轴向，计算当前目标矸石位置与当前机械臂末端位置的位置偏差值δsk；计算为当前的位置偏差值δsk与上一次末端位置偏差值δs
last
之间的偏差值δs
′
；计算累计末端的位置偏差值δs
sum
；具体如下式：
[0024][0025]
式中，sk为末端当前更新的位置，s
last
为上一次末端的位置；
[0026]
第三步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为x、y、z三轴轴向，计算当前目标矸石速度与当前机械臂末端速度的速度偏差值δvk；计算为当前的速度偏差值δvk与上一次末端速度偏
差值δv
last
之间的偏差值δv
′
；计算累计末端的速度偏差值δv
sum
；具体如下式：
[0027][0028]
其中：
[0029][0030]
式中，vk为末端当前更新的速度，v
last
为上一次末端的速度；dt为时间步长，v
max
为最大速度；
[0031]
第四步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为x、y、z三轴轴向，计算当前目标矸石加速度与当前机械臂末端加速度的加速度偏差值δak；计算为当前的加速度偏差值δak与上一次末端加速度偏差值δa
last
之间的偏差值δa
′
；计算累计末端的加速度偏差值δa
sum
；具体如下式：
[0032][0033]
其中：
[0034][0035]
其中，ak为末端当前更新的加速度；a
last
为上一次末端的加速度；a
max
为最大加速度；
[0036]
第五步：结合第二、三、四步中公式，计算机械臂末端加速度、速度、位置控制调整量a(k)，v(k)，s(k)，表达式如下：
[0037][0038]
式中：k
p
为比例系数；ki为积分系数；kd为微分系数；
[0039]
第六步：更新下一时刻机械臂末端位置s
k+1
，控制公式如下：
[0040]sk+1
＝sk+(a(k)dt+v(k))dt
[0041]
第七步：将下一时刻机械臂末端位置，通过主控子系统接口发送至机械臂控制子系统，各机械臂控制器根据末端位置信息执行并完成矸石的分拣任务，实现全闭环跟踪抓取。
[0042]
与现有技术相比，本发明具有以下有益效果：本发明基于煤矸石分拣机器人系统，通过目标矸石匹配识别和精准跟踪，识别跟踪带式输送机上煤矸流中的目标矸石，获得目标矸石位置坐标信息，结合由机器人控制器读取到的机器人全局坐标系下机械臂末端的位置信息，得到机器人全局坐标系下目标矸石位置坐标信息。通过基于视觉伺服的三环pid控制算法实现机械臂动态目标跟踪轨迹规划，实现位置、速度和加速度的最优控制，使得机械
臂末端和目标矸石到达同步，最终实现机械手拟静态抓取矸石，提高了抓取成功率，解放人工分选的劳动力，提高煤炭生产的智能化水平。
附图说明
[0043]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0044]
图1为本发明实施例中煤矸分拣机器人系统的系统框图。
[0045]
图2为本发明实施例煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法的流程图。
具体实施方式
[0046]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
[0047]
实施例1
[0048]
如图1所示，煤矸分拣机器人系统包括煤矸视觉检测单元、动态目标匹配识别单元、动态目标精确跟踪单元、识别定位子系统接口、配置有主控子系统和动态分拣轨迹规划单元的主控子系统、配置有机械臂控制子系统接口和机械臂控制器的机械臂分拣子系统；煤矸视觉检测单元采用双目立体相机获取图像信息，采用fcnn网络获取煤矸的识别结果，动态目标匹配识别单元采用hu不变矩匹配算法实现目标矸石模板的处理，动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信息作为输入，采用卡尔曼滤波扩展的fdsst跟踪算法快速跟踪目标矸石，动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息，采用三环pid控制算法进行机械臂末端轨迹规划，获得机械臂末端-目标矸石任务轨迹规划。
[0049]
实施例2
[0050]
如图2所示，一种煤矸分拣机器人视觉伺服的机械臂动态目标跟踪轨迹规划方法，该规划方法基于实施例1所述的煤矸分拣机器人系统实现，包括如下步骤：
[0051]
s1、原煤流中矸石通过带式输送机运载，经过煤矸视觉检测区域，基于煤矸视觉检测单元获取图像信息，并获取煤矸的识别结果，得到煤矸流中的矸石位置信息和目标矸石模板；
[0052]
s2、基于动态目标匹配识别单元采用hu不变矩匹配算法实现目标矸石模板的处理，进行目标矸石匹配识别，得到目标矸石的初始位置信息；
[0053]
s3、动态目标精确跟踪单元将动态目标匹配识别单元获取的目标矸石初始位置信息作为输入，采用卡尔曼滤波扩展的fdsst跟踪算法快速跟踪目标矸石，得到目标矸石的实时位置信息；
[0054]gk
{x，y}
[0055]
s4、识别定位子系统接口将所述动态目标精确跟踪单元获取的目标矸石位置信息，发送至主控子系统进行目标矸石信息预处理，即矸石位置坐标计算；
[0056]
第一步：根据步骤三中所述目标矸石几何形心像素坐标位置信息(x，y)，单位为pixel，获取目标矸石在机械臂末端坐标系下的坐标值(xm，ym，zm)，单位为m，表示公式如下：
[0057][0058]
其中，m1为相机内参矩阵，通过相机标定获取；分别为相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移矩阵。zc为相机坐标系原点距目标矸石的垂直距离，单位为m；
[0059]
第二步：计算得到机器人全局坐标系下目标矸石位置坐标值，具体如下式：
[0060][0061]
其中，x
tar
、y
tar
和z
tar
分别表示目标矸石在机器人全局坐标系下的x、y、z坐标，x
mal
、y
mal
和z
mal
分别表示通过读取机器人控制器获取的机器人全局坐标系下的机械臂末端位置坐标值信息。
[0062]
s5、动态分拣轨迹规划单元根据接收到预处理后的目标矸石位置信息，采用三环pid控制算法进行机械臂末端轨迹规划，获得机械臂末端-目标矸石任务轨迹规划；
[0063]
第一步：确定目标矸石在机械臂可抓取范围内，结合步骤s3和s4得到实时机器人全局坐标系下的目标矸石坐标位置信息
[0064]
第二步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为x、y、z三轴轴向，计算当前目标矸石位置与当前机械臂末端位置的位置偏差值δsk；计算为当前的位置偏差值δsk与上一次末端位置偏差值δs
last
之间的偏差值δs
′
；计算累计末端的位置偏差值δs
sum
；具体如下式：
[0065][0066]
式中，sk为末端当前更新的位置，s
last
为上一次末端的位置；
[0067]
第三步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为x、y、z三轴轴向，计算当前目标矸石速度与当前机械臂末端速度的速度偏差值δvk；计算为当前的速度偏差值δvk与上一次末端速度偏差值δv
last
之间的偏差值δv
′
；计算累计末端的速度偏差值δv
sum
；具体如下式：
[0068][0069]
其中：
[0070][0071]
式中，vk为末端当前更新的速度，v
last
为上一次末端的速度；dt为时间步长，v
max
为最大速度；
[0072]
第四步：根据当前目标矸石位置坐标当前机械臂末端位置坐标分别可分为x、y、z三轴轴向，计算当前目标矸石加速度与当前机械臂末端加速度的加速度偏差值δak；计算为当前的加速度偏差值δak与上一次末端加速度偏差值δa
last
之间的偏差值δa
′
；计算累计末端的加速度偏差值δa
sum
；具体如下式：
[0073][0074]
其中：
[0075][0076]
其中，ak为末端当前更新的加速度；a
last
为上一次末端的加速度；a
max
为最大加速度；
[0077]
第五步：结合第二、三、四步中公式，计算机械臂末端加速度、速度、位置控制调整量a(k)，v(k)，s(k)，表达式如下：
[0078][0079]
式中：k
p
为比例系数；ki为积分系数；kd为微分系数；
[0080]
第六步：更新下一时刻机械臂末端位置s
k+1
，控制公式如下：
[0081]sk+1
＝sk+(a(k)dt+v(k))dt
[0082]
第七步：将下一时刻机械臂末端位置，通过主控子系统接口发送至机械臂控制子系统，各机械臂控制器根据末端位置信息执行并完成矸石的分拣任务，实现全闭环跟踪抓取。
[0083]
s6、通过主控子系统接口将获得的机械臂末端-目标矸石任务轨迹规划发送至机械臂控制子系统，机械臂控制子系统接口接收主控子系统接口发送的机械臂末端-目标矸石任务轨迹规划信息，经机械臂控制器根据轨迹信息执行并完成矸石的分拣任务。
[0084]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。