CN109848964B - 基于光学动作捕捉的示教机器人数据采集器*** - Google Patents

Abstract

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

Description

基于光学动作捕捉的示教机器人数据采集器***

技术领域

本发明涉及一种基于光学动作捕捉的示教机器人数据采集***，是一种通过捕捉光学动作数据，实时控制六自由度机器人的数据采集***。

背景技术

为了使机器人产生特定轨迹的运动,通常做法是采用一种操作设备，由示教人员对该操作设备进行手动操作，使机器人按照所指定的动作运动，同时，将机器人的位置路径、机械手停止位置角度及高度等一系列信息记录于控制器中，最后基于所记录的数据生成机器人的动作程序。在利用操作设备对机器人进行示教时，传统的示教方法需要将动作分解为多个静态动作，然后根据各静态动作的位置、高度和角度信息来确定机器人的动作信息，这样做不仅工作量大，而且由于分解动作精度的限制，并不能充分反映示教人员手臂末端***的信息，示教相对稍复杂的动作，示教完成较困难，使机器人示教受到一些示教动作或者其它外部条件的限制；例如，在示教机器人使用打磨机打磨工件的动作时，由于在打磨过程中需要不断调整打磨片的角度，如果利用传统的示教方法，很难将这些动作精确分解并确定动作的各种信息，无法精确的完成示教任务。

基于光学动作捕捉的示教机器人采集器***，张涛、杨新、任为提出了一种机器人示教方法、装置及***(张涛，杨新，任为.一种机器人示教方法、装置及***:中国，106182003[P]，2016-12-07)，使用的是利用惯性测量单元IMU采集示教动作过程中数据，虽然提高了精确度和效率，但IMU易受到各种有干扰因素，尤其是位置误差，从而影响到***的精度。于建均、徐骢驰、阮晓钢等提出具有模仿学习机制的手把手示教机器人***及方法(于建均，徐骢驰，阮晓钢等.具有模仿学习机制的手把手示教机器人***及方法:中国，104924313[P].2015-09-23),使用搭载在机器人上的动作检测装置采集各连杆的状态信息，将信息应用到模仿学习算法，指导机器人***模仿学习示教行为，但是操作过程复杂，使用的学习算法性能不太高，用户操作繁琐不便。郭颜京天等提出一种工业机器人示教***和方法(郭颜京天，程凡宇，刘昊.一种工业机器人示教***和方法:中国，108214495[P].2018.06.29)，该***采用基于红外激光扫描定位的手持***来获取空间数据，并将空间数据发送到数据处理服务器。但是基于光学动作捕捉相比，得到的数据不够稳定，红外激光扫描距离不够远。

发明内容

针对现有技术的不足，本发明提出一种基于光学动作捕捉的示教机器人数据采集***，其通过实时在线捕捉光学动作数据，从而能够大幅提高识别范围和识别精度，提高示教效率和效果。

本发明为解决现有技术问题所采用的技术方案是：

基于光学动作捕捉的示教机器人数据采集***，其特征在于：数据处理模块通过交换机通信模块对标定模块发送采集命令，由监控模块监控各个模块，实现示教机器人运动控制数据的实时在线采集，具体包括：

标定模块，标定模块包括光学动作捕捉***、作业工具和示教机器人，标定模块中光学动作捕捉***通过电缆线与交换机通信模块相连接，光学动作捕捉***中摄像头接收到通过交换机通信模块中传输来的数据采集命令，在光学动作捕捉***中开始对作业工具进行光学动作捕捉，同时通过交换机通信模块传输采集到的作业工具的运动轨迹数据；由于作业工具是基于光学动作捕捉***坐标系，而机器人是处于世界坐标系下，因此需要将机器人置于光学动作捕捉***中进行标定后保存机器人在光学动作捕捉***坐标系中的相对位置，使得光学动作捕捉***中可识别出机器人并对其进行操作，坐标变换过程如下：

其中，{V}为光学动作捕捉***坐标系，{mar}为桌面参考物坐标系，{UR}为机器人基座坐标系，{tool}为作业工具坐标系，

为4×4为UR坐标系相对于V坐标系的齐次变换，

为mar坐标系相对于V坐标系的齐次变换，

为UR坐标系相对于mar坐标系的齐次变换，

为tool坐标系相对于UR坐标系的齐次变换，

为

的逆变换，

为tool坐标系相对于V坐标系的齐次变换；

表示了一般的旋转和平移，来定义一个坐标系，

为3×3旋转矩阵，是{UR}相对于{V}的表达；标量r_ij，其中i＝1，2，3，j＝1，2,3，表示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)

接着对比公式(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接入数据处理模块，接受在数据处理模块中末端轨迹姿态转换为各个关节角的角度值，使得机器人完成与作业工具动作相对应的动作。

所述的标定模块包括光学动作捕捉***、作业工具和示教机器人，光学动作捕捉***包括八个摄像头及捕捉***主机；对示教机器人在光学动作捕捉***中进行标定，使得示教机器人完成与作业工具动作相对应的动作。标定模块中光学动作捕捉***提供的实时光学数据，可以应用于实时在线的运动捕捉和分析；光学动作捕捉***的摄像头镜头通过收集作业工具上的反射光，在摄像头的传感器平面上形成聚焦图像，增加捕捉距离和捕捉稳定性。

所述的数据处理模块与标定模块之间采用ROS发布/订阅消息通信机制，每个模块以ROS中节点的形式存在，将传递的数据以消息的形式发布到相应的主题上，其他的功能模块通过订阅该主题获取到消息，以此达到模块之间的数据传输。

本发明的优点和积极效果是：精度很高，示教操作简单易行，只需通过挥动作业工具即可完成高精度的示教，避免了直接接触机器人保证了操作人员的安全性，同时不在需要手动记录示教点，***自动记录轨迹大大提升了工作效率，可以满足各种不同类型的需求。该***通过能够及时反馈工作状态和参数，让使用者可以直观地监控和跟踪整个示教***，使用了相机捕捉***，精度等级高，数据传输高效稳定，对运动轨迹的捕捉高效流畅，同时采用WiFi连接，工作空间得以扩展，能够符合高精度高效率的工业示教工作要求。一种基于光学动作捕捉的示教机器人数据采集器***是进行喷涂、焊接、抛光、打磨等复杂工作的行业的优良选择，专业化、精度高、易操作、通用性强等特点方便了所有使用者。

附图说明

图1为本发明的平台组成示意图。

图2为本发明的***控制关系。

图3为本发明的监控模块。

图4位本发明的标定模块的坐标系关系图。

具体实施方式

以下结合附图对本发明做进一步详述。

一种基于光学动作捕捉的示教机器人数据采集***，如图1所示，平台组成主要包括：示教机器人1、机器人示教装置处理终端2、交换机3、光学动作捕捉***摄像头4、作业工具5。利用光学动作捕捉***摄像头4对作业工具和示教机器人进行标定，标定完成后，使用机器人示教装置处理终端2向光学动作捕捉***摄像头发送采集命令，对作业工具5的位置、姿态数据进行采集；机器人示教装置处理终端2通过交换机3与光学动作捕捉***摄像头4进行数据传输，采集接收来自光学动作捕捉***摄像头4中传输的作业工具5的位置、姿态数据信息，并对接收到的位置、姿态数据进行坐标变换和生成机器人可以执行的运动指令，将此指令发送到示教机器人1，以使示教机器人完成与示教动作相同动作从而实现示教，在整个***示教过程中，通过机器人示教装置处理终端2中的监控模块负责向各个模块分发控制命令并对整个***运行状况进行监控。

该发明设计了一种基于光学动作捕捉的示教机器人数据采集***，机器人示教装置处理终端上装有交互软件，用以发送采集命令并将运动轨迹数据和空间姿态数据生成运动指令。按照对图1、图3的描述搭好机器人示教平台：

1)首先用户将八个摄像头固定在安装平台上，对每个摄像头，将RJ-45电缆线一端***摄像头插口，另一端***到交换机的一个插口内；摄像头是专门设计的运动捕捉相机，使用多个高速处理器来执行实时专有图像处理，当相机设置为正常的运动捕捉模式时，它们会输出作业工具上贴有的反射球体运动信息，发射球体反射出相机闪光灯单元的光线，摄像头镜头收集场景中的反射光，形成聚焦相机传感器平面上的图像；以太网交换机为八个摄像机提供电源和数据通信，并管理数据流通过RJ-45电缆线将数据传输到交换机通信模块中，标定模块通过交换机通信模块与数据处理模块、监控模块、示教机器人模块组成局域网，监控模块向局域网内的各个模块发送控制命令。

2)在标定模块中对机器人进行标定，用到的机器人是UR5机器人，将光学动作捕捉***世界坐标系X，Y，Z方向同机器人基坐标系X，Y，Z方向设置成一致，在机器人桌面上贴光学动作捕捉***可识别的marker球，测量marker球与机器人基座相对位姿关系，将测量后的数据封装成一个ROS里的launch文件，并利用ROS的tf工具，将launch文件中机器人桌面贴有的marker球与机器人基座的位姿关系发布出去，在ROS中利用tf工具监听到光学动作捕捉***坐标系、机器人底座桌面上标记物的坐标系，而机器人基座坐标系亦可通过tf监听到，因此根据发布的标记物与机器人基座的位置关系，通过坐标变换可得到光学动作捕捉***坐标系与机器人基座坐标系的相对位置关系，从而实现了机器人在光学动作捕捉***坐标系的标定，标定模块的坐标系关系图如图2所示，变换公式如下：

其他齐次变换矩阵求法一样，在标定模块中的光学动作捕捉***中挥动作业工具，让作业工具开始做示教动作，ROS中通过编写C++数据采集程序获取示教动作数据，采集到的数据为作业工具的轨迹信息，通过坐标变换将作业工具的坐标系相对于动作捕捉***下的坐标系转换成作业工具坐标系在机器人基坐标系下的坐标系；

3)在数据处理模块中通过交换机通信模块接收到坐标变换后的机器人基坐标系参考系下的作业工具的轨迹信息，将作业工具的轨迹信息通过坐标变换转换成机器人末端轨迹信息，通过逆运动学变换后得到机器人各个关节角度值，首先利用公式(1-9)计算得到基座第一关节角(θ₁)，公式(1-14)得到手腕2第五关节角θ₅，公式(1-15)得到手腕3第六关节角θ₆，公式(1-18)得到(θ₂+θ₃+θ₄)，联立公式(1-18)，(1-19)，(1-20)得到肩部第二关节角θ₂，将解出的肩部第二关节代入到联立后的公式(1-19)、(1-20)中得到肘部第三关节角θ₃，手腕1第四关节角θ₄。通过以上过程求得机器人六个关节角θ₁、θ₂、θ₃、θ₄、θ₅、θ₆，将六个关节角发送到示教机器人模块，使得示教机器人做出与示教动作相同动作。

4)监控模块如图4所示，通过WiFi经由交换机与机器人，光学动作捕捉***组成局域网，负责向各个模块分发控制命令，同时带有监视功能，将机器人、光学动作捕捉***工作状态显示出来以供用户观察整个***是否顺利运行，也有助于在***运行出现问题时，能帮助用户顺利定位到出问题的具体模块。

以上是整个***的控制情况，标定模块中光学动作捕捉***的使用，使得***操作简单，所提供的实时、高质量的光学数据，可以被应用于实时在线或者离线的运动捕捉、分析；交换机通过RJ-45电缆线连接到光学动作捕捉***的摄像头，并向所有的摄像机直接供电来捕捉、分析数据，这样得到的数据非常稳定，捕捉距离也更远。与现有技术相比，具有极高示教精度和效率的优点，可远程对机器人进行示教。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (3)

1.一种基于光学动作捕捉的示教机器人数据采集***，其特征在于：数据处理模块通过交换机通信模块对标定模块发送采集命令，监控模块向各个模块分发控制命令并监视整个***，实现机器人运动控制数据的实时在线采集，各模块的具体构成是：

标定模块，标定模块包括光学动作捕捉***、作业工具和示教机器人，标定模块中光学动作捕捉***通过电缆线与交换机通信模块相连接，光学动作捕捉***中摄像头接收到通过交换机通信模块中传输来的数据采集命令，在光学动作捕捉***中开始对作业工具进行光学动作捕捉，同时通过交换机通信模块传输采集到的作业工具的运动轨迹数据；由于作业工具是基于光学动作捕捉***坐标系，而机器人是处于世界坐标系下，因此需要将机器人置于光学动作捕捉***中进行标定后保存机器人在光学动作捕捉***坐标系中的相对位置，使得光学动作捕捉***中可识别出机器人并对其进行操作，坐标变换过程如下：

其中，{V}为光学动作捕捉***坐标系，{mar}为桌面参考物坐标系，{UR}为机器人基座坐标系，{tool}为作业工具坐标系，

为4×4为UR坐标系相对于V坐标系的齐次变换，

为mar坐标系相对于V坐标系的齐次变换，

为UR坐标系相对于mar坐标系的齐次变换，

为tool坐标系相对于UR坐标系的齐次变换，

为

的逆变换，

为tool坐标系相对于V坐标系的齐次变换；

表示了一般的旋转和平移，来定义一个坐标系，

为3×3旋转矩阵，是{UR}相对于{V}的表达；标量r_ij，其中i＝1，2，3，j＝1，2,3，表示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的每个矢量在参考坐标系固定坐标系0中的y单位方向上的投影，a_x、a_y、a_z为末端坐标系6的每个矢量在参考坐标系固定坐标系0中的z单位方向上的投影，根据式

可以求出θ₁，可以表示为：

另外：

其中，其中c代表cos，s代表sin，θ_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的每个矢量在参考坐标系固定坐标系1中的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接入数据处理模块，接受在数据处理模块中末端轨迹姿态转换为各个关节角的角度值，使得机器人完成与作业工具动作相对应的动作。

2.根据权利要求1所述的基于光学动作捕捉的示教机器人采集***，其特征在于：所述的标定模块包括光学动作捕捉***、作业工具和示教机器人，光学动作捕捉***包括八个摄像头及捕捉***主机；对示教机器人在光学动作捕捉***中进行标定，使得示教机器人完成与作业工具动作相对应的动作；标定模块中光学动作捕捉***提供的实时光学数据，可以应用于实时在线的运动捕捉和分析；光学动作捕捉***的摄像头镜头通过收集作业工具上的反射光，在摄像头的传感器平面上形成聚焦图像，增加捕捉距离和捕捉稳定性。

3.根据权利要求1所述的基于光学动作捕捉的示教机器人采集***，其特征在于：所述的数据处理模块与标定模块之间采用ROS发布/订阅消息通信机制，每个模块以ROS中节点的形式存在，将传递的数据以消息的形式发布到相应的主题上，其他的功能模块通过订阅该主题获取到消息，以此达到模块之间的数据传输。

Images