[发明专利]基于光学动作捕捉的示教机器人数据采集器系统

摘要

一种基于光学动作捕捉的示教机器人数据采集系统，模块主要划分为标定模块、交换机通信模块、数据处理模块、监控模块以及示教机器人模块。数据处理模块通过交换机模块向标定模块发送采集命令，来获取示教动作数据，示教动作数据为标定模块中采集到的作业工具在示教动作过程中的位姿，通过数据处理模块获取作业工具的位姿并进行坐标变换和生成机器人可以执行的运动指令，将此指令通过WiFi发送到机器人模块，监控模块向各个模块分发控制命令并监视整个系统，以使示教机器人完成与示教动作相同的动作从而实现示教。系统采用光学动作捕捉系统进行实时采集，与现有技术相比，具有良好的远程控制性能，采集到的数据具有高精度，示教具有稳定性。

主权项

1.一种基于光学动作捕捉的示教机器人数据采集系统，其特征在于：数据处理模块通过交换机通信模块对标定模块发送采集命令，监控模块向各个模块分发控制命令并监视整个系统，实现机器人运动控制数据的实时在线采集，各模块的具体构成是：标定模块，标定模块包括光学动作捕捉系统、作业工具和示教机器人，标定模块中光学动作捕捉系统通过电缆线与交换机模块相连接，光学动作捕捉系统中摄像头接收到通过交换机模块中传输来的数据采集命令，在光学动作捕捉系统中开始对作业工具进行光学动作捕捉，同时通过交换机模块传输采集到的作业工具的运动轨迹数据；由于作业工具是基于光学捕捉系统坐标系，而机器人是处于世界坐标系下，因此需要将机器人置于光学动作捕捉系统中进行标定后保存机器人在光学动作捕捉系统坐标系中的相对位置，使得光学动作捕捉系统中可识别出机器人并对其进行操作，坐标变换过程如下：其中，{V}为光学动作捕捉系统原点坐标系，{mar}为桌面参考物坐标系，{UR}为机器人坐标系，{tool}为作业工具坐标系，为4×4为UR坐标系相对于V坐标系的齐次变换，为mar坐标系相对于坐标系的齐次变换，为UR坐标系相对于mar坐标系的齐次变换，为tool坐标系相对于UR坐标系的齐次变换，为的逆变换，为tool坐标系相对于V坐标系的齐次变换；表示了一般的旋转和平移，来定义一个坐标系，为3×3旋转矩阵，是{UR}相对于{V}的表达；标量r_ij，其中i＝1，2…9，j＝1，2…9，表示UR中每个矢量在其参考系V中单位方向上投影的分量来表示；^AP_URORG为确定坐标系{UR}的原点的位置矢量；其余齐次变换与类似，表示用坐标系V表达时的坐标系UR主轴方向的单位矢量；在ROS中利用tf工具监听到光学动作捕捉系统原点坐标系、机器人底座桌面上标记物的坐标系，而机器人基座坐标系亦可通过tf工具监听到，因此根据发布的标记物与机器人基座的位置关系，通过坐标变换可得到光学动作捕捉系统原点坐标系与机器人基座坐标系的相对位置关系，从而实现了机器人在光学动作捕捉系统原点坐标系的标定，标定完成后，标定模块采集接收作业工具的运动轨迹数据信息并将采集到的信息通过交换机模块发送到数据处理模块；数据处理模块，数据处理模块通过WiFi与交换机模块相连，接收标定模块中采集到的作业工具的运动轨迹数据和空间姿态数据信息，将采集到的位姿信息写入文件中，在ROS系统中读写文件，发送个机器人末端，通过逆运动学求解，得到各个关节角度信息，机器人逆运动学求解过程如下：机械臂的关节角度分别为基座为第一关节角θ₁，肩部为第二关节角θ₂，肘部为第三关节角θ₃，手腕1为第四关节角θ₄，手腕2为第五关节角θ₅，手腕3为第六关节角θ₆；具体的计算方法如下：当机器人末端位姿的数值已知时，通过下列方程解出角度θ_i，i＝1，2，3，4，5，6；其中，是末端坐标系6到基座固定坐标系0的变换矩阵，它的数值是已知的，n_x、n_y、n_z为末端坐标系6的每个矢量在参考坐标系固定坐标系0中x单位方向上的投影，o_x、o_y、o_z为末端坐标系6的每个矢量在参考坐标系固定坐标系中的y单位方向上的投影，a_x、a_y、a_z为末端坐标系6的每个矢量在参考坐标系固定坐标系0中的z单位方向上的投影，根据式可以求出θ₁，可以表示为：另外：其中，其中c代表cos，s代表sin，α_i：绕x_i‑1坐标轴，从z_i‑1旋转到z_i的角度；a_i：沿x_i‑1坐标轴，从z_i‑1移动到z_i的角度；θ_i：绕z_i坐标轴，从x_i‑1旋转到x_i的角度；d_i：沿z_i坐标轴，从x_i‑1移动到x_i的角度；是末端坐标系6到基座固定坐标系1的变换矩阵，它的数值是已知的，¹n_x、¹n_y、¹n_z为末端坐标系6的每个矢量在参考坐标系固定坐标系1中x单位方向上的投影，¹o_x、¹o_y、¹o_z为末端坐标系6的每个矢量在参考坐标系固定坐标系中的y单位方向上的投影，¹a_x、¹a_y、¹a_z为末端坐标系6的每个矢量在参考坐标系固定坐标系1中的z单位方向上的投影，对比式(1‑6)和式(1‑7)，两个式子第2行第4列的值为常数，即可获得求解θ₁需要的一元方程：‑p_xs₁+p_yc₁＝d₂+d₃+d₄                        (1‑8)根据三角函数的万能公式，可以求出θ₁：θ₁＝atan2(p_y,p_x)‑atan2(E,±F)                        (1‑9)E＝d₂+d₃+d₄；接着对比公式(1‑6)和(1‑7)，可以求解θ₅和θ₆；由矩阵的第二行的前三列的元素可以求解：‑n_xs₁+n_yc₁＝‑s₅c₆                         (1‑10)‑o_xs₁+o_yc₁＝s₅c₆                           (1‑11)‑a_xs₁+a_yc₁＝‑c₅                           (1‑12)联立(1‑10)、(1‑11)，消除变量，得：求出θ₅：θ₅＝atan2(s₅,a_xs₁‑a_yc₁)                      (1‑14)当s₅＝0时，即θ₅＝0或θ₅＝π时，机构会发生奇异性，这样就不能求出θ₆的角度了；当s₅≠0，即θ₅≠0而且θ₅≠π时，可以求出θ₆：对比公式(1‑6)和(1‑7)，根据矩阵第一行第三列的元素和第三行第三列元素，这两列元素可以推导出以下的方程：a_xc₁+a_ys₁＝‑c₂₃₄s₅                            (1‑16)a_z＝s₂₃₄s₅                          (1‑17)当s₅＝0时，即θ₅＝0或θ₅＝π时，机构会发生奇异性，这样就不能求出θ₂+θ₃+θ₄的角度了；当s₅≠0，即θ₅≠0而且θ₅≠π时，可以求出θ₂+θ₃+θ₄：对比公式(1‑6)和(1‑7)，由矩阵第一行第四列和第三行第四列这两列的元素可以推导出以下方程：p_xc₁+p_ys₁＝a₃c₂₃+a₂c₂‑d₅s₂₃₄               (1‑19)p_z‑d₁＝‑a₃s₂₃‑a₂s₂‑d₅c₂₃₄              (1‑20)消除θ₂+θ₃，从而可以求解出θ₂的值：解出θ₂之后，再次联立(1‑19)和(1‑20)，可以求出θ₂+θ₃因此求出θ₃与θ₄；由以上的逆运动学的求解公式，就可以求出每个关节角的角度；交换机通信模块，交换机通信模块包括以太网交换机、路由器和电缆线，以太网交换机通过电缆线接收数据处理模块发送的采集命令，并将标定模块采集到的轨迹及姿态数据信息发送到数据处理模块，交换机通信模块通过WiFi与数据处理模块及标定模块建立无线连接，由路由器开启WiFi热点，采用TCP通信协议模式建立无线连接，并通过WiFi透传模式快速传输数据；交换机通信模块为光学动作捕捉系统中的摄像头提供电源及数据通信，并将采集完成的数据通过WiFi发送到数据处理模块；监控模块，监控模块通过WiFi经由交换机通信模块与机器人、光学捕捉系统组成局域网，负责向各个模块分发控制命令，同时带有监视功能，将各模块工作状态显示出来用于观察整个系统是否顺利运行，也有助于在系统运行出现问题时，能帮助用户顺利定位到出问题的模块；示教机器人模块，示教机器人模块通过WiFi接入数据处理模块，接受在数据处理模块中末端轨迹姿态转换为各个关节角的角度值，使得机器人完成与作业工具动作相对应的动作。