[发明专利]一种机械手运动轨迹规划方法及螺栓抓取检测系统

权利要求书

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

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

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

坐标变换矩阵如下：

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

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

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

式中，表示边缘函数，表示滤波函数，D表示带宽，表示噪声均方差，表示平滑边缘调节函数；

其中，

式中，为归一化常数，和表示平滑解析度，和表示经过和平滑解析之后的图像；

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

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

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

其中，

式中，表示从当前节点n到f值最小的节点m的前驱节点l之间的欧式距离；当时，机械手先在X方向上平移预定距离、后在Y方向上平移预定距离，此时转向代价；当时，机械手不发生X方向和Y方向上的平移，此时转向代价；当时，机械手同时发生X、Y、Z方向上的平移动作，此时转向代价；表示当前节点n到终点的曼哈顿距离；

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

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

式中，表示当前节点n到终点在X方向上的位移量；表示当前节点n到终点在Y方向上的位移量；表示当前节点n到终点在Z方向上的位移量；表示对空间内两点求曼哈顿距离；

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

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

式中，表示对一组j个路径点次序重新排列，表示的一个置换，表示和两点之间的曼哈顿距离，s表示机械手运动起点位置，表示起点s到之间的曼哈顿距离，e表示中途随机停止点，表示停止点到之间的曼哈顿距离；

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

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

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

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