CN112975953A - 一种机械手运动轨迹规划方法及螺栓抓取检测系统 - Google Patents

Abstract

本发明涉及一种机械手运动轨迹规划方法及螺栓抓取检测系统。运动轨迹规划方法包括如下步骤：步骤1、以机械手自身坐标系为基础，建立环境模型坐标系，并对环境模型坐标系进行栅格化处理；步骤2、构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划。本发明通过中控机完成对机械手的运动轨迹规划。运动轨迹规划的目的是在完成抓取螺栓任务的前提下使得机械爪末端的抓取部尽可能按照设定的轨迹运行、且行程在最优范围内，能够显著提高运行效率、减少机械手从起点到终点处的运行时间，提高螺栓抓取节奏。

Description

一种机械手运动轨迹规划方法及螺栓抓取检测系统

技术领域

本发明涉及运动轨迹规划领域，具体涉及一种机械手运动轨迹规划方法及螺栓抓取 检测系统。

背景技术

螺栓是由头部和螺杆两部分组成的一类紧固件，需与螺母配合，用于紧固连接两个 带有通孔的零件。

在螺栓抓取检测领域，由于螺栓的形状特殊，传统的上下料装置通常由一组或多组 机械手组成，并通过预设好的轨迹运行。在应对较为复杂的场景时，如果不对路径进行规划，会严重降低运行效率、拖慢运行节奏。

发明内容

发明目的：提供一种基于动态图像识别的机械手运动轨迹规划系统、方法、设备及计算机存储介质，以解决现有技术存在的上述问题。

技术方案：第一个方面，提供一种机械手运动轨迹规划方法，包括如下步骤：

步骤1、以机械手自身坐标系为基础，建立环境模型坐标系，并对环境模型坐标系进行栅格化处理；

步骤2、构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划。

在第一方面的一些可实现方式中，步骤1中对环境模型坐标系进行栅格化处理的过 程如下：获取机械手的最大运行空间，将该最大运行空间沿X、Y、Z方向分解为多个 大小相同的网格单元；检查划分出来的网格单元内是否包含障碍物，若包含障碍物，则 判定为填充网格、并标记为不可通行网格；若不包含障碍物，则判定为未填充网格、并 标记为可通行网格；将不可通行网格记为0，可通行网格记为1，获得含有0和1的栅 格合集。

在第一方面的一些可实现方式中，步骤2中坐标变换矩阵如下：

式中，i表示驱动单元，j表示抓取部，θ表示驱动单元的各运行轨道的夹角，α表示抓取部相对于驱动单元的扭转角，d表示抓取部相对于驱动单元之间的距离；

在第一方面的一些可实现方式中，对抓取部的运动轨迹进行轨迹规划的过程进一步 包括：

步骤2-1、利用机械手上的多个工业相机采集实际环境，得出包含影像和景深信息的图像，建立环境模型：

式中，E(x)表示边缘函数，L(x)表示滤波函数，D表示带宽，σ表示噪声均方差，f(A)表示平滑边缘调节函数；

其中，

式中，R为归一化常数，J_s和J_r表示平滑解析度，x^s和x^r表示经过J_s和J_r平滑解析之后的图像；

步骤2-2、构建估价函数寻找起点到终点的最优路径：

f_n＝v(n)+w(n)

式中，v(n)表示从起点到当前节点n的实际距离，w(n)表示当前节点n到终点的估算距离；

步骤2-3、为当前节点n到终点的估算距离w(n)添加转向代价：

w(n)＝w′(n)+ε₀

其中，

式中，distance(1,n)表示从当前节点n到f值最小的节点m的前驱节点l之间的欧式距离；当

时，机械手先在X方向上平移预定距离、后在Y方向上 平移预定距离，此时转向代价ε₀＝1；当distance(1,n)＝2时，机械手不发生X方向和 Y方向上的平移，此时转向代价ε₀＝0；当distance(1,n)＝0时，机械手同时发生X、 Y、Z方向上的平移动作，此时转向代价ε₀＝2；w′(n)表示当前节点n到终点的曼哈顿 距离；

步骤2-4、构建当前节点n到终点endNode的曼哈顿距离，以曼哈顿距离反馈至估算距离中进行修正：

其中，曼哈顿距离表达式如下：

w′(n)＝d[(n.x)-(endNode.x)]+d[(n.y)-(endNode.y)] +d[(n.z)-(endNode.z)]

式中，(n.x)-(endNode.x)表示当前节点n到终点endNode在X方向上的位移量；(n.y)-(endNode.y)表示当前节点n到终点endNode在Y方向上的位移量； (n.z)-(endNode.z)表示当前节点n到终点endNode在Z方向上的位移量；d(*)表示对 空间内两点求曼哈顿距离；

修正后的距离表达式如下：

步骤2-5、根据修正后的距离，构建最优路径：

式中，

表示对一组j个路径点次序重新排列，

表示(1,2,…j)的一个置换，

表示

和

两点之间的曼哈顿距离，s表示机械手运动起点位置，

表示起点s到

之间的曼哈顿距离，e表示中途随机停止点，

表示停止点到

之 间的曼哈顿距离；

步骤2-6、在对环境模型坐标系进行栅格化处理的基础上，采用适应度函数生成初始种群：

式中，

表示从A点(x_i,y_i)到B点(x_j,y_j)的欧氏距离；A表示对路径安全性的衡量值，该衡量值由光电检测装置判断前方是否存在障碍得出，取值0 或1。

第二方面，提供一种螺栓抓取检测系统，包括：机械手组件；用于控制并驱动所述机械手组件以预定轨迹运行的轨迹规划单元；以及用于进行动态图像识别的图像识别单元；所述机械手组件包括驱动单元及抓取部。

在第二方面的一些可实现方式中，所述轨迹规划单元进一步包括用于建立环境模型 坐标系，并对环境模型坐标系进行栅格化处理的第一模块；用于构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划的第二模块；所述图像识别单元进 一步包括用于构建表面缺陷分析模型，并对其进行训练的第三模块；利用位于夹持部内 侧的周边视觉相机采集螺栓边缘的图像数据，利用上部视觉相机采集螺栓顶部的图像数 据，将两组图像数据分送至中控机的第四模块；根据接收得到的图像数据反哺至表面缺 陷分析模型中进行结果分析，对螺栓缺陷进行标记的第五模块。

第三方面，提供一种螺栓抓取检测设备，该设备包括：处理器以及存储有计算机程序指令的存储器；所述处理器读取并执行所述计算机程序指令，以实现第一方面或第一 方面的一些可实现方式中的运动轨迹规划方法，以及实现第二方面或第二方面的一些可 实现方式中的动态图像识别方法。

第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算 机程序指令，所述计算机程序指令被处理器执行时实现第一方面或第一方面的一些可实 现方式中的运动轨迹规划方法，以及实现第二方面或第二方面的一些可实现方式中的动 态图像识别方法。

有益效果：本发明涉及一种机械手运动轨迹规划方法及螺栓抓取检测系统。通过中 控机完成对机械手的运动轨迹规划。运动轨迹规划的目的是在完成抓取螺栓任务的前提 下使得机械爪末端的抓取部尽可能按照设定的轨迹运行、且行程在最优范围内，能够显著提高运行效率、减少机械手从起点到终点处的运行时间，提高螺栓抓取节奏。

附图说明

图1为本发明实施例中机械手运动轨迹规划方法的工作流程图。

图2为本发明实施例中螺栓抓取检测系统的结构示意图。

图3为螺栓抓取检测系统中回转盘的结构示意图。

图中各附图标记为：振筛盘1、下料道2、第一物料抓取装置3、回转盘4、安装座401、下导轨402、下固定铰支座403、下导块404、下活动铰支座405、上活动板本体 406、上固定铰支座407、上活动铰支座408、上导块409、连杆410、间歇式回转盘体 411、第二物料抓取装置5、上部视觉相机6、定位槽7。

具体实施方式

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而， 对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实 施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。

实施例一：

本实施例提供一种螺栓抓取检测系统，该抓取检测系统包括物料移栽机构和检测机 构两部分。物料移栽机构包括振筛盘1、下料道2、第一物料抓取装置3、回转盘4、第 二物料抓取装置5，振筛盘1与外部的生产线连通，下料道2与振筛盘1连通，第一物 料抓取装置3安装在下料道2的一端，回转盘4安装在第一物料抓取装置3的一侧，第 二物料抓取装置5设置在回转盘4的一侧，第二物料抓取装置5与第一物料抓取装置3 的延长线呈预定的夹角。回转盘4每个转动周期转动预定的角度并停止预定时长。

检测机构包括定位板、定位槽7、上部视觉相机6，定位板固定在第二物料抓取装置5的一侧，定位槽7开设在定位板上、且与被生产螺栓相适配，上部视觉相机6设置 在定位板的一侧。第一物料抓取装置3用于将已加工完成的螺栓从振筛盘1上抓取至回 转盘4上，第二物料抓取装置5用于将位于所述回转盘4上的螺栓抓取至定位板上。

第一物料抓取装置3和第二物料抓取装置5的结构相同，包括支撑座、第一滑轨、第一滑块、第一滚珠丝杆电动缸、X轴板、第二滚珠丝杆电动缸、Y轴板、第三滚珠丝 杆电动缸、Z轴板、抓取部、顶端连接部、顶部铰接座、爪头、缓冲垫、活动连接部、 活动铰接座、连接杆、伸缩杆。第一滑轨固定在支撑座上，第一滑块滑动套设在第一滑 轨上，第一滚珠丝杆电动缸固定在支撑座上，并且平行设置在第一滑轨的一侧。X轴板 与第一滚珠丝杆电动缸之间滑动连接；第二滚珠丝杆电动缸固定在X轴板上，Y轴板与 第二滚珠丝杆电动缸滑动连接；第三滚珠丝杆电动缸固定在Y轴板上；Z轴板与第三滚 珠丝杆电动缸滑动连接；抓取部固定在Z轴板上。

抓取部包括顶端连接部、顶部铰接座、爪头、缓冲垫、活动连接部、活动铰接座、 连接杆、伸缩杆；顶部铰接座呈圆周阵列在顶端连接部上，爪头铰接在每个顶部铰接座 上，缓冲垫设置在爪头的根部；顶端连接部内设有直线轴承座；伸缩杆活动穿插在直线 轴承座内；活动连接部同轴安装在伸缩杆的一端；活动铰接座呈圆周阵列在活动连接部 上，活动铰接座的数量与顶部铰接座的数量一致、并且一一对应。活动铰接座与爪头之 间通过连接杆铰接；活动连接部的底部分别安装有周边视觉相机和工业相机。振筛盘1 通过振动筛取出螺栓，并通过下料道2输送至第一物料抓取装置3的一侧，通过第一物 料抓取装置3将螺栓抓取至回转盘4上，再通过回转盘4按照预设的转动方式将螺栓转 动至靠近第二物料抓取装置5的一侧，通过第二物料抓取装置5将螺栓抓取至定位板上 进行后续的视觉检测工作。第一物料抓取装置3和第二物料抓取装置5分别在X轴、Y 轴、Z轴上具有两个方向上的运动自由度，保证螺栓的抓取灵活、放置准确。在爪头的 根部设置缓冲垫，当爪头加紧螺栓后能够具有缓冲效果，避免划伤螺栓表面。该抓取部 通过伸缩杆、铰接座、连接杆的设置，将伸缩杆在竖直方向上的运动轨迹转变为爪头的 收缩、张合两种状态，相比于两端推进式的夹爪，本抓取部更加适应螺栓这种体积较小 的工件。

实施例二：

在实施例一的基础之上，本实施例提出一种机械手运动轨迹规划方法，步骤如下：

步骤1、以机械手自身坐标系为基础，建立环境模型坐标系，并对环境模型坐标系进行栅格化处理；获取机械手的最大运行空间，将该最大运行空间沿X、Y、Z方向分 解为多个大小相同的网格单元；检查划分出来的网格单元内是否包含障碍物，若包含障 碍物，则判定为填充网格、并标记为不可通行网格；若不包含障碍物，则判定为未填充 网格、并标记为可通行网格；将不可通行网格记为0，可通行网格记为1，获得含有0 和1的栅格合集。

步骤2、构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划；

其中，坐标变换矩阵如下：

式中，i表示驱动单元，j表示抓取部，θ表示驱动单元的各运行轨道的夹角，α表示抓取部相对于驱动单元的扭转角，d表示抓取部相对于驱动单元之间的距离。

步骤2-1、利用机械手上的多个工业相机采集实际环境，得出包含影像和景深信息的图像，建立环境模型：

式中，E(x)表示边缘函数，L(x)表示滤波函数，D表示带宽，σ表示噪声均方差，f(A)表示平滑边缘调节函数；

其中，

式中，R为归一化常数，J_s和J_r表示平滑解析度，x^s和x^r表示经过J_s和J_r平滑解析之后的图像；

步骤2-2、构建估价函数寻找起点到终点的最优路径：

f_n＝v(n)+w(n)

式中，v(n)表示从起点到当前节点n的实际距离，w(n)表示当前节点n到终点的估算距离；

步骤2-3、为当前节点n到终点的估算距离w(n)添加转向代价：

w(n)＝w′(n)+ε₀

其中，

式中，distance(1,n)表示从当前节点n到f值最小的节点m的前驱节点l之间的欧式距离；当

时，机械手先在X方向上平移预定距离、后在Y方向上 平移预定距离，此时转向代价ε₀＝1；当distance(1,n)＝2时，机械手不发生X方向和 Y方向上的平移，此时转向代价ε₀＝0；当distance(1,n)＝0时，机械手同时发生X、 Y、Z方向上的平移动作，此时转向代价ε₀＝2；w′(n)表示当前节点n到终点的曼哈顿 距离；

步骤2-4、构建当前节点n到终点endNode的曼哈顿距离，以曼哈顿距离反馈至估算距离中进行修正：

其中，曼哈顿距离表达式如下：

w′(n)＝d[(n.x)-(endNode.x)]+d[(n.y)-(endNode.y)] +d[(n.z)-(endNode.z)]

式中，(n.x)-(endNode.x)表示当前节点n到终点endNode在X方向上的位移量；(n.y)-(endNode.y)表示当前节点n到终点endNode在Y方向上的位移量； (n.z)-(endNode.z)表示当前节点n到终点endNode在Z方向上的位移量；d(*)表示对 空间内两点求曼哈顿距离；

修正后的距离表达式如下：

步骤2-5、根据修正后的距离，构建最优路径：

式中，

表示对一组j个路径点次序重新排列，

表示(1,2,…j)的一个置换，

表示

和

两点之间的曼哈顿距离，s表示机械手运动起点位置，

表示起点s到

之间的曼哈顿距离，e表示中途随机停止点，

表示停止点到

之 间的曼哈顿距离；

步骤2-6、在对环境模型坐标系进行栅格化处理的基础上，采用适应度函数生成初始种群：

式中，

表示从A点(x_i,y_i)到B点(x_j,y_j)的欧氏距离；A表示对路径安全性的衡量值，该衡量值由光电检测装置判断前方是否存在障碍得出，取值0 或1。

实施例三：

在实施例一的基础之上，回转盘4包括安装座401、下导轨402、下固定铰支座403、下导块404、下活动铰支座405、上活动板组件、间歇式回转盘4体；下导轨402相互 平行固定在安装座401的两侧；下固定铰支座403分别固定在安装座401上、且位于两 个下导轨402的一端；下导块404分别滑动设置在下导轨402上；下活动铰支座405分 别固定在两个下导轨402上；上活动板组件与下固定铰支座403和下活动铰支座405铰 接；间歇式回转盘4体设置在上活动板组件上。上活动板组件包括上活动板本体406、 上导轨、上导块409、上活动铰支座408、上固定铰支座407，上导轨固定在上活动板本 体406的下部两侧；上导块409滑动设置在上导轨上，上活动铰支座408分别固定在两 个上导块409上，上固定铰支座407分别固定在两个上导轨的一端。上固定铰支座407 和下固定铰支座403一一对应；上固定铰支座407和所述下活动铰支座405之间、以及 上活动铰支座408和所述下固定铰支座403之间通过连杆410连接，连杆410两两交叉 并通过销轴连接；安装座401上固定有直线气缸，直线气缸的输出杆连接至所述下导块 404。通过直线气缸推进下导块404沿着下导轨402滑动，从而带动下活动铰支座405 滑动，由于一端是活动铰支座、另一端是固定铰支座，并且两者之间通过连杆410连接， 上活动板组件同理，最终带动间歇式回转盘4体升降。间歇式回转盘4体包括驱动盘， 驱动盘的边缘处设有一段凸起，驱动盘的一侧转动设置有间歇盘，间歇盘的圆周阵列有 多个凹槽，凸起适配于凹槽，间歇盘上同轴安装有回转盘4本体。驱动盘每转动一圈， 其边缘处的凸起便拨动一次间歇盘，从而实现间歇盘的间歇式转动。驱动盘的转轴处连 接有伺服电机；位于间歇盘圆周上的凹槽数量等于驱动盘每转动一圈所间歇停留的次数； 间歇盘的转轴处连接有轴承座。

定位槽7包括一面设有螺旋槽的齿轮盘，设置在齿轮盘的螺旋槽一侧的多个夹持部， 固定在所述转动支座上、且与所述齿轮盘连接的步进电机，以及安装在所述夹持部的内 侧的多个周边视觉相机；多个夹持部之间行程预定空间的槽体，螺栓放置在所述槽体中； 该定位槽7可调，从而可以适应不同尺寸的螺栓；夹持部内部设有多个周边视觉相机能够感知当前是否夹持住螺栓。夹持部与所述齿轮盘接触的一面设有与所述螺旋槽匹配啮合的齿槽；夹持部呈阶梯状，步进电机的输出轴与所述齿轮盘的中心轴通过联轴器连接；步进电机电信连接于中控机上，由周边视觉相机感应是否存在螺栓，当判定存在螺栓， 则反馈信号至中控机，由中控机驱动步进电机对螺栓实施夹持动作。

实施例四：

本实施例提出一种螺栓抓取检测系统，该抓取检测系统包括机械手组件；用于控制 并驱动所述机械手组件以预定轨迹运行的轨迹规划单元；以及用于进行动态图像识别的 图像识别单元；所述机械手组件包括驱动单元及抓取部。

轨迹规划单元进一步包括用于建立环境模型坐标系，并对环境模型坐标系进行栅格 化处理的第一模块；用于构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划的第二模块；所述图像识别单元进一步包括用于构建表面缺陷分析模型， 并对其进行训练的第三模块；利用位于夹持部内侧的周边视觉相机采集螺栓边缘的图像 数据，利用上部视觉相机6采集螺栓顶部的图像数据，将两组图像数据分送至中控机的 第四模块；根据接收得到的图像数据反哺至表面缺陷分析模型中进行结果分析，对螺栓 缺陷进行标记的第五模块。

实施例五：

基于实施例一和实施例四，本实施例提出该抓取检测系统的工作方法：

加工完成的螺栓通过振筛盘1振筛至下料道2，并由下料道2将其传输至第一物料抓取装置3的一侧。随后第一物料抓取装置3启动，第一滚珠丝杆电动缸推动第一滑块 沿着第一滑轨滑动，从而带动X轴板在X轴方向直线运动；第二滚珠丝杆电动缸安装 在X轴板上，由第二滚珠丝杆电动缸推动Y轴板在Y轴方向直线运动；第三滚珠丝杆 电动缸安装在Y轴板上，由第三滚珠丝杆电动缸推动Z轴板沿着Z轴方向升降，由此 实现三个方向的运动自由度。抓取部固定在Z轴板上，通过直线气缸或其他具备直线驱 动能力的机构驱动伸缩杆，由伸缩杆拉动与之铰接的连接杆动作、并由连接杆将动作反 馈至与之铰接的爪头，从而使得爪头收缩或张开。活动连接部的底部分别安装有周边视 觉相机和工业相机，由工业相机感应是否夹取到螺栓，由周边视觉相机判断当前抓取部 距离螺栓的距离。当第一物料抓取装置3上的抓取部抓取到螺栓后，通过上述X轴板、 Y轴板、Z轴板的相互配合，将螺栓放置在回转盘4上。放置到位后，第一物料抓取装 置3移开，回转盘4工作，回转盘4每转动60度便停止5秒钟，在这5秒钟的周期内， 第一物料抓取装置3继续从下料道2上抓取螺栓并放置在下一个工位上，如此往复。当 螺栓被回转盘4转动至离检测机构最近的位置时，第二物料抓取装置5启动，第二物料 抓取装置5的工作方式与第一物料抓取装置3一致，将螺栓移栽至定位板上准备视觉检 测。

在本发明的实施例中，回转盘4可以在预定升降行程内升降，通过直线气缸推进下导块404沿着下导轨402滑动，从而带动下活动铰支座405滑动，由于一端是活动铰支 座、另一端是固定铰支座，并且两者之间通过连杆410连接，上活动板组件同理，最终 带动间歇式回转盘4体升降。

在本发明的实施例中，回转盘4的本体为间歇式回转盘4体，包括驱动盘，驱动盘的边缘处设有一段凸起，驱动盘的一侧转动设置有间歇盘，间歇盘的圆周阵列有多个凹槽，凸起适配于凹槽，间歇盘上同轴安装有回转盘4本体。驱动盘每转动一圈，其边缘 处的凸起便拨动一次间歇盘，从而实现间歇盘的间歇式转动。

在本发明的实施例中，定位板上设有与被生产螺栓相适配的定位槽7，定位槽7包括一面设有螺旋槽的齿轮盘，齿轮盘的螺旋槽一侧设有多个夹持部，齿轮盘的一端连接 步进电机，夹持部的内测设有多个周边视觉相机。多个夹持部之间行程预定空间的槽体， 螺栓放置在所述槽体中；该定位槽7可调，从而可以适应不同尺寸的螺栓；夹持部内部 设有多个周边视觉相机能够感知当前是否夹持住螺栓。夹持部与所述齿轮盘接触的一面 设有与所述螺旋槽匹配啮合的齿槽；夹持部呈阶梯状，步进电机的输出轴与所述齿轮盘 的中心轴通过联轴器连接；步进电机电信连接于中控机上，由周边视觉相机感应是否存 在螺栓，当判定存在螺栓，则反馈信号至中控机，由中控机驱动步进电机对螺栓实施夹 持动作。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为 对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上做出各种变化。

Claims (7)

1.一种机械手运动轨迹规划方法，其特征在于，包括如下步骤：

步骤1、以机械手自身坐标系为基础，建立环境模型坐标系，并对环境模型坐标系进行栅格化处理；

步骤2、构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划。

2.根据权利要求1所述的一种机械手运动轨迹规划方法，其特征在于：步骤1中对环境模型坐标系进行栅格化处理的过程进一步为：

获取机械手的最大运行空间，将该最大运行空间沿X、Y、Z方向分解为多个大小相同的网格单元；检查划分出来的网格单元内是否包含障碍物，若包含障碍物，则判定为填充网格、并标记为不可通行网格；若不包含障碍物，则判定为未填充网格、并标记为可通行网格；将不可通行网格记为0，可通行网格记为1，获得含有0和1的栅格合集。

3.根据权利要求1所述的一种机械手运动轨迹规划方法，其特征在于：步骤2中坐标变换矩阵如下：

式中，i表示驱动单元，j表示抓取部，θ表示驱动单元的各运行轨道的夹角，α表示抓取部相对于驱动单元的扭转角，d表示抓取部相对于驱动单元之间的距离；

对抓取部的运动轨迹进行轨迹规划的过程进一步包括：

步骤2-1、利用机械手上的多个工业相机采集实际环境，得出包含影像和景深信息的图像，建立环境模型：

式中，E(x)表示边缘函数，L(x)表示滤波函数，D表示带宽，σ表示噪声均方差，f(A)表示平滑边缘调节函数；

其中，

式中，R为归一化常数，J_s和J_r表示平滑解析度，x^s和x^r表示经过J_s和J_r平滑解析之后的图像；

步骤2-2、构建估价函数寻找起点到终点的最优路径：

f_n＝v(n)+w(n)

式中，v(n)表示从起点到当前节点n的实际距离，w(n)表示当前节点n到终点的估算距离；

步骤2-3、为当前节点n到终点的估算距离w(n)添加转向代价：

w(n)＝w′(n)+ε₀

其中，

式中，distance(1，n)表示从当前节点n到f值最小的节点m的前驱节点1之间的欧式距离；当

时，机械手先在X方向上平移预定距离、后在Y方向上平移预定距离，此时转向代价ε₀＝1；当distance(1，n)＝2时，机械手不发生X方向和Y方向上的平移，此时转向代价ε₀＝0；当distance(1，n)＝0时，机械手同时发生X、Y、Z方向上的平移动作，此时转向代价ε₀＝2；w′(n)表示当前节点n到终点的曼哈顿距离；

步骤2-4、构建当前节点n到终点endNode的曼哈顿距离，以曼哈顿距离反馈至估算距离中进行修正：

其中，曼哈顿距离表达式如下：

w′(n)＝d[(n.x)-(endNode.x)]+d[(n.y)-(endNode.y)]+d[(n.z)-(endNode.z)]

式中，(n.x)-(endNode.x)表示当前节点n到终点endNode在X方向上的位移量；(n.y)-(endNode.y)表示当前节点n到终点endNode在Y方向上的位移量；(n.z)-(endNode.z)表示当前节点n到终点endNode在z方向上的位移量；d(*)表示对空间内两点求曼哈顿距离；

修正后的距离表达式如下：

步骤2-5、根据修正后的距离，构建最优路径：

式中，

表示对一组j个路径点次序重新排列，

表示(1，2，...j)的一个置换，

表示

和

两点之间的曼哈顿距离，s表示机械手运动起点位置，

表示起点s到

之间的曼哈顿距离，e表示中途随机停止点，

表示停止点到

之间的曼哈顿距离；

步骤2-6、在对环境模型坐标系进行栅格化处理的基础上，采用适应度函数生成初始种群：

式中，

表示从A点(x_i，y_i)到B点(x_j，y_j)的欧氏距离；A表示对路径安全性的衡量值，该衡量值由光电检测装置判断前方是否存在障碍得出，取值0或1。

4.一种螺栓抓取检测系统，其特征是包括：

机械手组件；

用于控制并驱动所述机械手组件以预定轨迹运行的轨迹规划单元；

用于进行动态图像识别的图像识别单元；

所述机械手组件包括驱动单元及抓取部。

5.根据权利要求4所述的一种螺栓抓取检测系统，其特征在于：

所述轨迹规划单元进一步包括用于建立环境模型坐标系，并对环境模型坐标系进行栅格化处理的第一模块；

用于构建驱动单元与抓取部之间的变换矩阵，对抓取部的运动轨迹进行轨迹规划的第二模块；

所述图像识别单元进一步包括用于构建表面缺陷分析模型，并对其进行训练的第三模块；

利用位于夹持部内侧的周边视觉相机采集螺栓边缘的图像数据，利用上部视觉相机采集螺栓顶部的图像数据，将两组图像数据分送至中控机的第四模块；

根据接收得到的图像数据反哺至表面缺陷分析模型中进行结果分析，对螺栓缺陷进行标记的第五模块。

6.一种螺栓抓取检测设备，其特征在于，所述设备包括：

处理器以及存储有计算机程序指令的存储器；

所述处理器读取并执行所述计算机程序指令，以实现如权利要求1-3任意一项所述的运动轨迹规划方法。

7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现如权利要求1-3任意一项所述的运动轨迹规划方法。

Images