CN108648208A - 一种嵌入式裁片挂片机器人控制系统及机器人控制方法 - Google Patents

Abstract

本发明公开了一种嵌入式裁片挂片机器人控制系统，包括依次连接的硬件层、控制层和应用层，应用层包括若干个相互连接的PC上位机，PC上位机通过无线连接的方式与控制层相连；硬件层包括安装在机器人机械本体的若干个伺服电机，每个伺服电机连接有伺服驱动器，多个伺服驱动器之间通过CAN通信模块进行通信；控制层包括ARM嵌入式平台和各功能模块。本发明还公开了裁片挂片机器人的控制方法，求出机器人关节参数并通过运动控制芯片按照规划轨迹计算出末端执行器到达裁片位置的运动参数；驱动机器人抓取裁片后挂在衣架上。本发明的机器人控制系统，不需人工干预，自主识别裁片、确定裁片位置，准确有效的完成抓片和挂片动作。

Description

一种嵌入式裁片挂片机器人控制系统及机器人控制方法

技术领域

本发明属于嵌入式机器人运动控制装置技术领域，具体涉及一种嵌入式裁片挂片机器人控制系统，还涉及该裁片挂片机器人的控制方法。

背景技术

随着国民经济发展迈入新常态，传统的依赖资源消耗以及劳动力持续投入的发展模式已经难以为继，纺织服装业的内生动力必须要不断增强。“工业4.0”大幕的徐徐拉开以及人口红利的消失、机器人技术的逐渐成熟以及在各行业应用的普及，机器人代替人以减少用工，将最终成为纺织服装企业的现实选择。通过“机器换人”提升产业的信息化技术水平，应用信息化主动破解低效高耗难题，促进企业由劳动密集型向技术密集型转变，是纺织服装企业可持续发展的不二选择。国内企业对焊接、喷涂、搬运等行业的单工位或工艺联合通用型工业机器人的研发取得了一些成绩，但不能满足纺织服装企业对机器人的特殊要求。劳动密集型企业难以进口价格高昂的机器人，根据纺织服装企业“机器换人”的实际需求，研发制造价格相对较低、质量可靠、实用性强的单工位机器人甚至工艺联合机器人，是减轻企业“机器换人”成本负担的有效方法。2014年开始，单工位的铺布机器人、工艺联合型的纺织机器人以及印染机器人开始在青岛、石狮、绍兴等地进行研发，尚未投入使用。研发我国具有自主知识产权的高性能机器人控制系统及应用系统迫在眉睫。

因工业机器人配备PC工控机作为控制器导致体积大、占用空间大的问题，使用ARM嵌入式控制器配合运动控制芯片控制机器人，不但可以有效的缩小控制器体积而且实时稳定，具有明显的优势。目前，国外机器人公司已有的控制系统，如美国“PR2”的ROS控制系统、基于工业PLC的控制系统以及基于“IPC+运动控制器”的控制系统等都是基于PC机作为机器人控制器的运动控制系统；而目前国内尚无成熟的嵌入式机器人控制系统。现有的机器人控制系统都是基于PC机为运动控制器进行软件控制或以PC机为载体结合运动控制卡进行运动控制，这种设计虽然能够利用PC机丰富的资源，但造成运动控制器体积大，对机器人作业空间有很大的限制。而顺应控制系统小型化趋势的如基于嵌入式控制器的机器人控制系统，目前，国内尚无成熟的系统推出。

发明内容

本发明的目的是提供一种嵌入式裁片挂片机器人控制系统，有效的解决当前机器人由于控制器体积大，无法在狭小空间进行作业的问题。

本发明的目的还在于提供一种嵌入式裁片挂片机器人的控制方法。

本发明所采用的第一种技术方案是，一种嵌入式裁片挂片机器人控制系统，包括依次连接的硬件层、控制层和应用层，应用层包括若干个相互连接的具有热点功能的PC上位机，PC上位机通过无线连接的方式与控制层相连；硬件层包括安装在机器人机械本体的若干个伺服电机，每个伺服电机连接有伺服驱动器，多个伺服驱动器之间通过CAN通信模块进行通信；控制层包括ARM嵌入式平台和功能模块，功能模块包括DSP模块、运动控制芯片、视觉定位模块和射频识别模块；

ARM嵌入式平台分别与DSP模块、视觉定位模块、射频识别模块相连和运动控制芯片相连通信，ARM嵌入式平台还通过CAN总线的方式与CAN通信模块相连；ARM嵌入式平台通过无线连接的方式与PC上位机相连。

本发明的特点还在于，

ARM嵌入式平台通过HPI接口与DSP模块相连进行通信、通过RS232串口分别与视觉定位模块和射频识别模块相连、通过配有ISA插槽的端子板与运动控制芯片相连，ARM嵌入式平台还通过CAN总线的方式与CAN通信模块相连。

ARM嵌入式平台是以S3C6410芯片为核心，运动控制芯片是PCL6045BL控制芯片。

本发明的所采用的第二种技术方案是，一种嵌入式裁片挂片机器人的控制方法，具体操作步骤如下：

步骤1、对视觉定位模块的双目摄像机进行标定，获得双目摄像机的内参、外参以及畸变参数；

步骤2、通过视觉定位模块的双目摄像机采集服装裁片的图像信号，然后通过ARM嵌入式平台实时在线传送给DSP模块；

步骤3、DSP模块单元获得裁片的数字图像后，通过图像分割、边缘检测等一系列图像处理技术，获得图像的边缘信息；

步骤4、根据步骤1获得双目摄像机的内参、外参以及畸变参数和步骤3的图像边缘信息，确定裁片的实际抓取位置，也就是机器人末端手爪要到达的位置；

步骤5、根据机器人末端手爪要到达的位置，在笛卡尔坐标下进行轨迹规划；

步骤6、运动控制芯片根据裁片的实际抓取位置与步骤5的轨迹规划结果进行计算，将裁片的实际抓取位置转化为机器人每一个关节的位置与运动速度，驱动机器人末端执行器运动到指定位置；

步骤7、射频识别模块经电磁感应识别衣架标签，确定衣架位置，将此位置上传到ARM嵌入式平台，ARM嵌入式平台将衣架位置信息发送给运动控制卡，机器人按照步骤5与步骤6，将裁片挂在对应的衣架上。

本发明的特点还在于，

步骤6具体为：

步骤6.1、根据轨迹规划的结果，将轨迹分解并计算出每个节点的位置、速度以及加速度等运动参数；

步骤6.2、将裁片的位置设置为机器人手爪终端位置，此位置经过机器人运动学反解，转换为各关节的运动参数，机器人根据对应的各个关节角进而驱动关节电机实施抓取与放置动作，各个关节角求取如下：

①关节角θ₁

其中，p_x表示机器人手爪坐标系原点作为机器人基础坐标系中的位置矢量p在机器人基础坐标系x轴上的投影、p_y表示p在y轴上的投影、d₂表示关节1和关节2的x轴方向距离；

②关节角θ₃

其中

其中，a₂表示连杆2的长度，a₃表示连杆3的长度；d₄表示关节3和关节4的x轴方向距离；

③关节角θ₂

θ₂₃＝arctan2((-a₃-a₂c₃)p_z+(c₁p_x+s₁p_y)(a₂s₃-d₄),(-d₄+a₂s₃)p_z-(c₁p_x+s₁p_y)(-a₂c₃-a₃)) (8)

其中：θ₂＝θ₂₃-θ₃

其中，p_z表示机器人手爪坐标系原点作为机器人基础坐标系中的位置矢量p在z轴上的投影；s₁是sinθ₁的缩写，s₂是sinθ₂的缩写,s₃是sinθ₂的缩写；c₁是cosθ₁的缩写、c₂是cosθ₂的缩写、c₃是cosθ₁的缩写；

④关节角θ₄

θ₄＝arctan2(-a_xs₁+a_yc₁,-a_xc₁c₂₃-a_ys₁c₂₃+a_zs₂₃) (9)

其中，a_x、a_y、a_z分别表示机器人手爪坐标系z轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；c₂₃表示cos(θ₂+θ₃)，s₂₃表示sin(θ₂+θ₃)；

⑤关节角θ₅

θ₅＝arctan 2(s₅,c₅) (11)

其中，s₅是sinθ₅的缩写，c₅是cosθ₅的缩写；

⑥关节角θ₆

θ₆＝arctan 2(s₆,c₆) (13)

其中，

其中：s₆是sinθ₆的缩写，c₆是cosθ₆的缩写；n_x、n_y、n_z分别表示机器人手爪坐标系x轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；c₄是cosθ₄的缩写；

以上步骤求解出机器人运动学逆解参数后，在机器人控制过程中通过机器人末端的位姿，获得各个关节的角度，驱动机器人末端执行器运动到指定位置。

本发明的有益效果是：

①本发明一种嵌入式裁片挂片机器人控制系统，采用ARM嵌入式平台代替PC机作为机器人的控制器，进而缩小控制器体积。

②本发明一种嵌入式裁片挂片机器人控制系统，采用分层分布控制并通过CAN总线获取机器人各关节节点的轴信息和坐标信息，在应用层及控制层设置运动控制参数控制机器人运动。

③本发明一种嵌入式裁片挂片机器人控制系统，可以自主识别要抓取的裁片、裁片的位置和衣架的位置并按照规划的运动轨迹完成抓片和挂片动作。

附图说明

图1是本发明一种嵌入式裁片挂片机器人控制系统的结构图；

图2是本发明一种嵌入式裁片挂片机器人控制系统的硬件平台设计图；

图3是本发明一种嵌入式裁片挂片机器人控制系统工作流程图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种嵌入式裁片挂片机器人控制系统，结构如图1所示，包括依次连接的硬件层、控制层和应用层，应用层包括若干个相互连接的具有热点功能的PC上位机，PC上位机通过无线连接的方式与控制层相连；硬件层包括安装在机器人机械本体的若干个伺服电机，每个伺服电机连接有伺服驱动器，多个伺服驱动器之间通过CAN通信模块进行通信；控制层以ARM嵌入式平台为核心集中接收和处理各个功能模块的数据信息，进而发送指令完成对机器人的运动控制，功能模块主要包括DSP模块、运动控制芯片、视觉定位模块和射频识别模块；

ARM嵌入式平台通过HPI接口与DSP模块相连进行通信、通过RS232串口分别与视觉定位模块和射频识别模块相连、通过配有ISA插槽的端子板与运动控制芯片相连，ARM嵌入式平台还通过CAN总线的方式与CAN通信模块相连，获取机器人各轴各节点的数据信息。

ARM嵌入式平台是以S3C6410芯片为核心，运动控制芯片是PCL6045BL控制芯片。

一种嵌入式裁片挂片机器人控制系统的搭建方法，具体按照以下步骤实施：

步骤1、设计机器人控制系统

搭建ARM嵌入式平台并构建运动控制器，即以ARM芯片为核心的嵌入式平台配合以运动控制芯片为核心的运动控制卡构成机器人运动控制器；ARM根据DSP模块的图像处理结果以及摄像机标定结果，计算出裁片中心位置；同时，ARM应用射频模块发送的数据，识别衣架的位置信息，此两个位置信息，提供给运动控制卡，以实现抓取裁片与悬挂裁片到衣架的功能。

步骤2、如图2所示，搭建ARM嵌入式软件平台

开发PCL6045BL驱动、CAN模块驱动并应用Qt完成PC上位机人机交互和智能监控界面的设计，完成嵌入式控制板与DSP控制器的HPI接口通信，完成运动控制器与CAN总线模块的通信，获取机器人各关节点信息并通过CAN总线传送到ARM嵌入式平台，完成ARM嵌入式平台与应用层PC上位机WIFI通信，便于控制层通过软件控制机器人运动；

一种嵌入式裁片挂片机器人控制方法，具体按照以下步骤实施：

步骤1、对视觉定位模块的双目摄像机进行标定，获得双目摄像机的内参、外参以及畸变参数；

步骤2、如图3所示，通过视觉定位模块的双目摄像机采集服装裁片的图像信号，然后通过ARM嵌入式平台实时在线传送给DSP模块；

步骤3、DSP模块单元获得裁片的数字图像后，通过图像分割、边缘检测等一系列图像处理技术，获得图像的边缘信息；

步骤4、根据步骤1获得双目摄像机的内参、外参以及畸变参数和步骤3的图像边缘信息，确定裁片的实际抓取位置，也就是机器人末端手爪要到达的位置。

步骤5、根据机器人末端手爪要到达的位置，在笛卡尔坐标下进行轨迹规划；

步骤6、运动控制芯片根据裁片的实际抓取位置与步骤5的轨迹规划结果进行计算，将裁片的实际抓取位置转化为机器人每一个关节的位置与运动速度，驱动机器人末端执行器运动到指定位置；

步骤6.1、根据轨迹规划的结果，将轨迹分解并计算出每个节点的位置、速度以及加速度等运动参数；

步骤6.2、将裁片的位置设置为机器人手爪终端位置，此位置经过机器人运动学反解，转换为各关节的运动参数，机器人实施抓取与放置动作，其运动学反解参数求取过程如下：

机器人正运动学方程表示如下：

其中：n_x、n_y、n_z分别表示机器人手爪坐标系x轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；o_x、o_y、o_z分别表示机器人手爪坐标系y轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；a_x、a_y、a_z分别表示机器人手爪坐标系z轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；p_x表示机器人手爪坐标系原点作为机器人基础坐标系中的位置矢量p在机器人基础坐标系x轴上的投影、p_y表示p在y轴上的投影、p_z表示p在z轴上的投影；

其中

其中：s₁是sinθ₁的缩写，s₂是sinθ₂的缩写，以此类推；同样c₁是cosθ₁的缩写；其中θ₁是连杆1的转角，θ₂是连杆2的转角，以此类推；a₁表示连杆1的长度，a₂表示连杆2的长度，以此类推；d₂表示关节1和关节2的x轴方向距离，d₄表示关节3和关节4的x轴方向距离，

在获知机器人手爪终端的位置和姿态的基础上，求解机器人对应的各个关节角进而驱动关节电机，使手爪的姿态符合抓取要求，逆运动学具有多解性，本发明具体的机器人逆运动学求解如下：

①求关节角θ₁

用的逆矩阵左乘公式(1)，得到公式(2)：

由矩阵内对应元素对等，可得：

-s₁p_x+c₁p_y＝d₂ (3)

通过三角代换：

其中其中:将极坐标方程转换成直角坐标系方程中，三角代换通常构造x²+y²＝ρ²类比于本公式为进而有

带入公式(3)中，得：

解得：

②求关节角θ₃

令公式(2)左右矩阵中(1,4)和(2,4)元素相等，得：

对公式(3)和公式(4)两边平方后相加，整理得：

-s₃d₄+c₃a₃＝k

其中

同求解θ₁相似，通过三角代换，解得：

③求关节角θ₂

用的逆矩阵左乘公式(1)，得到公式(6)：

将公式(5)展开，由方程两端对应元素相等可得：

其中：c₂₃表示cos(θ₂+θ₃)，s₂₃表示sin(θ₂+θ₃)，

进而求得：

θ₂₃＝arctan2((-a₃-a₂c₃)p_z+(c₁p_x+s₁p_y)(a₂s₃-d₄),(-d₄+a₂s₃)p_z-(c₁p_x+s₁p_y)(-a₂c₃-a₃)) (8)

其中：θ₂＝θ₂₃-θ₃

④求关节变量θ₄

将公式(6)中左右矩阵中的(1,3)和(3,3)相等，得到

解得：

θ₄＝arctan2(-a_xs₁+a_yc₁,-a_xc₁c₂₃-a_ys₁c₂₃+a_zs₂₃) (9)

⑤求关节角θ₅

用的逆矩阵左乘公式(1)，得到：

将上式展开后左右矩阵(1,3)和(3,3)元素对应相等，得：

即θ₅＝arctan 2(s₅,c₅) (11)

⑥求关节角θ₆

用的逆矩阵左乘公式(1)，得到公式(12)：

将上式展开后左右矩阵(3,1)和(1,1)元素对应相等，得：

求得θ₆＝arctan 2(s₆,c₆) (13)

以上步骤求解出机器人运动学逆解参数后，在机器人控制过程中通过机器人末端的位姿，可以获得各个关节的角度，驱动机器人末端执行器运动到指定位置。

步骤7、射频识别模块经电磁感应识别衣架标签，确定衣架位置，将此位置上传到ARM嵌入式平台，ARM嵌入式平台将衣架位置信息发送给运动控制卡，机器人按照步骤5与步骤6，将裁片挂在对应的衣架上。

Claims (5)

1.一种嵌入式裁片挂片机器人控制系统，其特征在于，包括依次连接的硬件层、控制层和应用层，所述应用层包括若干个相互连接的具有热点功能的PC上位机，所述PC上位机通过无线连接的方式与所述控制层相连；所述硬件层包括安装在机器人机械本体的若干个伺服电机，每个所述伺服电机连接有伺服驱动器，多个所述伺服驱动器之间通过CAN通信模块进行通信；所述控制层包括ARM嵌入式平台和功能模块，所述功能模块包括DSP模块、运动控制芯片、视觉定位模块和射频识别模块；

所述ARM嵌入式平台分别与DSP模块、视觉定位模块、射频识别模块相连和运动控制芯片相连通信，所述ARM嵌入式平台还通过CAN总线的方式与CAN通信模块相连；所述ARM嵌入式平台通过无线连接的方式与PC上位机相连。

2.根据权利要求1所述的一种嵌入式裁片挂片机器人控制系统，其特征在于，所述ARM嵌入式平台通过HPI接口与DSP模块相连进行通信、通过RS232串口分别与视觉定位模块和射频识别模块相连、通过配有ISA插槽的端子板与运动控制芯片相连，所述ARM嵌入式平台还通过CAN总线的方式与CAN通信模块相连。

3.根据权利要求1所述的一种嵌入式裁片挂片机器人控制系统，其特征在于，所述ARM嵌入式平台是以S3C6410芯片为核心，所述运动控制芯片是PCL6045BL控制芯片。

4.一种嵌入式裁片挂片机器人的控制方法，其特征在于，具体按照以下步骤实施：

步骤1、对视觉定位模块的双目摄像机进行标定，获得双目摄像机的内参、外参以及畸变参数；

步骤2、通过视觉定位模块的双目摄像机采集服装裁片的图像信号，然后通过ARM嵌入式平台实时在线传送给DSP模块；

步骤3、DSP模块单元获得裁片的数字图像后，通过图像分割、边缘检测等一系列图像处理技术，获得图像的边缘信息；

步骤4、根据步骤1获得双目摄像机的内参、外参以及畸变参数和步骤3的图像边缘信息，确定裁片的实际抓取位置，也就是机器人末端手爪要到达的位置；

步骤5、根据机器人末端手爪要到达的位置，在笛卡尔坐标下进行轨迹规划；

步骤6、运动控制芯片根据裁片的实际抓取位置与步骤5的轨迹规划结果进行计算，将裁片的实际抓取位置转化为机器人每一个关节的位置与运动速度，驱动机器人末端执行器运动到指定位置；

步骤7、射频识别模块经电磁感应识别衣架标签，确定衣架位置，将此位置上传到ARM嵌入式平台，ARM嵌入式平台将衣架位置信息发送给运动控制卡，机器人按照步骤5与步骤6，将裁片挂在对应的衣架上。

5.根据权利要求4所述的一种嵌入式裁片挂片机器人的控制方法，其特征在于，步骤6具体为：

步骤6.1、根据轨迹规划的结果，将轨迹分解并计算出每个节点的位置、速度以及加速度等运动参数；

步骤6.2、将裁片的位置设置为机器人手爪终端位置，此位置经过机器人运动学反解，转换为各关节的运动参数，机器人根据对应的各个关节角进而驱动关节电机实施抓取与放置动作，各个关节角求取如下：

①关节角θ₁

其中，p_x表示机器人手爪坐标系原点作为机器人基础坐标系中的位置矢量p在机器人基础坐标系x轴上的投影、p_y表示p在y轴上的投影、d₂表示关节1和关节2的x轴方向距离；

②关节角θ₃

其中

其中，a₂表示连杆2的长度，a₃表示连杆3的长度；d₄表示关节3和关节4的x轴方向距离；

③关节角θ₂

θ₂₃＝arctan2((-a₃-a₂c₃)p_z+(c₁p_x+s₁p_y)(a₂s₃-d₄),(-d₄+a₂s₃)p_z-(c₁p_x+s₁p_y)(-a₂c₃-a₃)) (8)

其中：θ₂＝θ₂₃-θ₃

其中，p_z表示机器人手爪坐标系原点作为机器人基础坐标系中的位置矢量p在z轴上的投影；s₁是sinθ₁的缩写，s₂是sinθ₂的缩写,s₃是sinθ₂的缩写；c₁是cosθ₁的缩写、c₂是cosθ₂的缩写、c₃是cosθ₁的缩写；

④关节角θ₄

θ₄＝arctan 2(-a_xs₁+a_yc₁,-a_xc₁c₂₃-a_ys₁c₂₃+a_zs₂₃) (9)

其中，a_x、a_y、a_z分别表示机器人手爪坐标系z轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；c₂₃表示cos(θ₂+θ₃)，s₂₃表示sin(θ₂+θ₃)；

⑤关节角θ₅

θ₅＝arctan 2(s₅,c₅) (11)

其中，s₅是sinθ₅的缩写，c₅是cosθ₅的缩写；

⑥关节角θ₆

θ₆＝arctan 2(s₆,c₆) (13)

其中，

其中：s6是sinθ₆的缩写，c₆是cosθ₆的缩写；n_x、n_y、n_z分别表示机器人手爪坐标系x轴相对于机器人基础坐标系的x轴、y轴以及z轴的旋转位置；c₄是cosθ₄的缩写；

以上步骤求解出机器人运动学逆解参数后，在机器人控制过程中通过机器人末端的位姿，获得各个关节的角度，驱动机器人末端执行器运动到指定位置。