[发明专利]一种嵌入式裁片挂片机器人控制系统及机器人控制方法

权利要求书

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θ₄的缩写；

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