一种3+4新构型双臂协作机器人加工系统及方法
技术领域
[0001] 本
发明属于智能制造领域，更具体地，涉及一种3+4新构型双臂
协作机器人加工系统及方法。
背景技术
[0002]
叶片加工
抛光作主要用于去除叶片表面刀纹、提高叶片表面光度、修正型面等，其加工效果直接影响叶片的
气动性能、作业效率和服役寿命。
[0003] 当前，手工/六轴数控磨抛是复杂叶片主要的磨抛手段。手工磨抛应用最为广泛(应用占比90％以上)，但存在加工效率和
精度低、表面一致性差、加工要求难以得到保证、环境污染和危害工人健康等问题。
[0004] 六轴数控磨床相对于手工磨抛可以显著提高加工效率和
质量，但也存在明显缺点：
[0005] (1)成本高，成套精密机床需要几百万人民币；
[0006] (2)加工模式单一，针对粗磨、精磨和抛光需要手动更换
砂带；
[0007] (3)不具备
指定磨抛余量的变
力自适应加工能力，不具备微米级原位测量的高性价比手段，无法构成补偿加工闭环回路。
[0008] 机器人加工则为磨抛提供了新思路。相对于六轴数控磨床，机器人具有成本低、柔性好、智能化、效率高、操作空间大等优势。同时，机器人常用的
感知功能，如
机器视觉、力觉以及相应的视觉伺服和力位混合控制技术日臻成熟。以机器人为装备本体，配以强大的感知功能，可基于工艺知识与多
传感器反馈信息对运行参数进行实时滚动优化。这将突破传统多轴磨抛装备仅关注运动轴
位置和速度控制的局限，形成装备对工艺过程的主动控制，在磨抛加工中具有显著优势。
[0009] 但是，当前主流的
串联/闭链结构六
自由度机器人的
定位精度难以满足高精磨削加工精度要求，主要是由于机器人
运动链较长导致其整体
刚度差、定位精度难以提高，影响磨抛精度。并且，传统磨抛设备属于专用设备，并不能用于
焊接、磨抛、钻孔、激光加工等工艺，对于日益增长的自动化加工需求而言，仍显不足。
[0010] 因此，亟需一种新构型叶片加工系统及方法来解决上述问题。
发明内容
[0011] 针对
现有技术的以上
缺陷或改进需求，本发明提供了一种3+4新构型双臂协作机器人加工系统及方法，其目的在于，由三旋转一平动的
工件夹持臂和三旋转的工具夹持臂组成3+4新构型双臂协作机器人，从而缩短运动链长度、提升系统刚度、保证加工精度，解决现有技术的整体刚度及加工精度不足的技术问题。
[0012] 为了实现上述目的，按照本发明的一个方面，提供了一种3+4新构型双臂协作机器人加工系统，用于工件的加工，包括：工件夹持臂直线滑轨、底座、加工工具、工具夹持臂、机器人
控制器以及工件夹持臂；
[0013] 所述工件夹持臂直线滑轨、所述工具夹持臂及所述机器人控制器均安装于所述底座上，所述工件夹持臂滑动安装于所述工件夹持臂直线滑轨上；
[0014] 所述工件夹持臂直线滑轨
水平布置，所述工件夹持臂包括三个相互垂直的回
转轴，其中一个回转轴竖直布置；所述工具夹持臂包括一个竖直回转轴和两个水平回转轴；所述工件夹持臂末端用于安装所述工件；所述工具夹持臂末端通过
法兰安装所述加工工具，用于加工所述工件；
[0015] 所述机器人控制器用于控制所述工件夹持臂进行三转动一平动的4自由度运动，以及控制所述工具夹持臂进行3自由度转动，从而实现3+4双臂协作加工。
[0016] 进一步地，还包括三坐标测量设备；所述机器人控制器用于控制所述工件夹持臂将所述工件送入所述三坐标测量设备，以检测所述工件是否合格，若不合格，则控制所述工件夹持臂将所述工件转移至加工工具的
工作空间内，然后对不合格部位进行3+4双臂协作加工。
[0017] 进一步地，所述工具夹持臂包括第一轴减速机、第一轴
电机、第二轴电机、第二轴减速机、机器人转座、第三轴减速机、第三轴电机、第一臂部、第二臂部和机器人底座；
[0018] 所述机器人底座安装于所述底座上，所述第三轴电机安装于所述机器人底座上，且通过所述第三轴减速机连接所述机器人转座，所述第三轴减速机的转轴竖直布置；所述第二轴减速机水平安装于所述机器人转座上，其
输出轴与所述第一臂部的一端固定连接；所述第一轴减速机水平安装于所述机器人转座上，其输出轴与所述第一臂部的另一端
枢接且与所述第二臂部的一端固定连接，所述第二臂部的另一端安装所述加工工具；所述第二轴电机固定于所述第一臂部上，且其输出轴连接所述第二轴减速机的
输入轴；所述第一轴电机固定于所述第一臂部上，且其输出轴连接所述第一轴减速机的输入轴；
[0019] 所述机器人控制器用于控制所述第一轴电机、所述第二轴电机、所述第三轴电机的运动，从而控制所述加工工具的运动轨迹。
[0020] 进一步地，所述工件夹持臂直线滑轨包括直
线轴导轨和直线轴电机；
[0021] 所述工件夹持臂包括第四轴减速机、第四轴电机、第五轴减速机、第五轴电机、第六轴减速机、第六轴电机、末端法兰、机器人底座、第三臂部和第四臂部；
[0022] 所述第四轴电机安装于所述机器人底座上，且通过所述第四轴减速机连接所述第三臂部的一端，所述第四轴减速机的转轴竖直布置；所述第五轴减速机水平设置，其输出轴与所述第三臂部的另一端枢接，且与所述第四臂部的一端固定连接；所述第六轴减速机垂直于所述第五轴减速机布置，且固定于所述第四臂部的另一端，所述第六轴减速机的输出轴安装所述末端法兰，所述末端法兰用于安装所述工件；所述第五轴电机固定于所述第三臂部上，且其输出轴连接所述第五轴减速机的输入轴；所述第六轴电机安装于所述第四臂部上，且其输出轴连接所述第六轴减速机的输入轴；
[0023] 所述直线轴导轨水平安装于所述底座上，所述机器人底座安装于所述直线轴导轨上，所述直线轴电机用于驱动所述机器人底座沿所述直线轴导轨滑动。
[0024] 为了实现上述目的，按照本发明的另一方面，提供了一种+新构型双臂协作机器人加工系统的加工方法，将所述工件通过夹具安装于所述末端法兰上，通过所述机器人控制器控制所述工件夹持臂和所述工具夹持臂运动，以按照预设轨迹对所述工件进行双臂协作加工；在加工完成后，通过所述机器人控制器控制所述工件夹持臂将所述工件转移至所述三坐标测量设备，以检测和判断所述工件的加工表面是否满足加工要求：
[0025] 若不满足加工要求，则所述机器人控制器继续控制所述工件夹持臂将所述工件转移至所述加工工具的工作空间，并控制所述工件夹持臂和所述工具夹持臂协作运动，以对所述工件不满足加工要求的部位再次进行加工直至满足加工要求；若满足加工要求，则加工结束。
[0026] 进一步地，采用所述的工具夹持臂与所述的工件夹持臂进行双臂协作加工；其中，双臂协作加工的控制流程如下：
[0027] S1，获取所述工具夹持臂、所述工件夹持臂的平移轴和回转轴的配置参数，得到工具
坐标系的具体表示；记所述第一轴减速机、所述第二轴减速机、所述第三轴减速机、所述第四轴减速机、所述第五轴减速机、所述第六轴减速机相对于自身零位的实轴转
角分别为θ1、θ2、θ3、θ4、θ5、θ6；
[0028] S2，在工件坐标系中对所述工件进行加工路径规划，得到加工的路径点，包括路径点的位置和
姿态；
[0029] S3，根据所述工件的实际装夹位置与所述工件夹持臂的末端法兰坐标系的偏差，将步骤S2规划的路径点转化为所述工件夹持臂的末端法兰盘坐标系中的刀位点，获得各个刀位点的位置和姿态；
[0030] S4，按照当前刀位点的位置和姿态，以及所述加工工具的实际装夹位置与所述工具夹持臂的末端法兰坐标系的偏差，通过运动学逆解的方式，计算并输出θ1、θ2、θ3、θ4、θ5、θ6。
[0031] 进一步地，步骤S4包括如下子步骤：
[0032] S4.1，先建立运动学方程，通过运动学逆解求解θ3，
[0033] S4.2，将θ3的所有可行解与上一姿态下对应的θ3′进行比较，将差值大的解舍弃，保留差值小的解作为θ3的正确解；
[0034] S4.3，将θ3的正确解代入运动学方程计算出θ2的所有可行解；
[0035] S4.4，将θ2的所有可行解与上一姿态下对应的θ2′进行比较，将差值大的解舍弃，保留差值小的解作为θ2的正确解；
[0036] S4.5，将θ3、θ2的正确解代入运动学方程计算出θ1的所有可行解；
[0037] S4.6，将θ1的所有可行解于上一姿态的θ1′进行比较，将差值大的解舍弃，保留差值小的解作为θ1的正确解；
[0038] S4.7，将θ3、θ2、θ1的正确解代入运动学方程计算出θ4、θ5的所有可行解；
[0039] S4.8，将θ4、θ5的所有可行解分别于上一姿态的θ4′、θ5′进行比较，将差值大的解舍弃，保留差值小的解作为θ4、θ5的正确解；
[0040] S4.9，将θ3、θ2、θ1、θ4的正确解代入运动学方程计算出θ6的所有可行解；
[0041] S4.10，将θ6的所有可行解于上一姿态的θ6′进行比较，将差值大的解舍弃，保留差值小的解作为θ6的正确解；
[0042] S4.11，输出θ1、θ2、θ3、θ4、θ5、θ6。
[0043] 进一步地，记所述第一轴减速机、所述第二轴减速机、所述第三轴减速机、所述第四轴减速机、所述第五轴减速机、所述第六轴减速机分别为回转关节1～回转关节6，对应的坐标系分别为坐标系1～坐标系6，基坐标系为坐标系0；
[0044] 步骤S1中，所述工具坐标系如下：
[0045] 坐标系t表示机器人工具夹持臂的末端法兰坐标系，原点在所述工具夹持臂的末端法兰中心处，坐标系t的Xt方向与世界坐标系Z方向同向，Yt方向与世界坐标系X方向同向，Zt方向与世界坐标系Y方向同向；
[0046] 坐标系0原点在机器人回转关节1回转轴与回转关节3回转轴交点处，坐标系0的X0方向与世界坐标系Z方向同向，Y0方向与世界坐标系X方向同向，Z0方向与世界坐标系Y方向同向；
[0047] 坐标系1与坐标系2的原点重合，位于机器人回转关节2回转轴与机器人回转关节3回转轴交点处；坐标系1的X1方向与世界坐标系Z方向同向，Y1方向与世界坐标系X方向同向，Z1方向与世界坐标系Y方向同向；坐标系2的X2方向与世界坐标系X方向同向，Y2方向与世界坐标系Y方向同向，Z2方向与世界坐标系Z方向同向；
[0048] 坐标系3、坐标系4、坐标系5、坐标系6的原点重合，位于机器人回转关节5回转轴与回转关节6回转轴交点处；坐标系3的X3方向与世界坐标系X方向同向，Y3方向与世界坐标系Y方向同向，Z3方向与世界坐标系Z方向同向；坐标系4的X4方向与世界坐标系X方向同向，Y4方向与世界坐标系Z负方向同向，Z4方向与世界坐标系Y方向同向；坐标系5的X5方向与世界坐标系Z负方向同向，Y5方向与世界坐标系Y方向同向，Z5方向与世界坐标系X方向同向；坐标系6的X6方向与世界坐标系Z负方向同向，Y6方向与世界坐标系Y方向同向，Z6方向与世界坐标系X方向同向；
[0049] 坐标系f表示机器人工件夹持臂的末端法兰坐标系，原点在所述末端法兰的末端中心处，坐标系f的Xf方向与世界坐标系Z负方向同向，Yf方向与世界坐标系Y方向同向，Zf方向与世界坐标系X方向同向。
[0050] 进一步地，步骤S1中，所述平移轴和回转轴的配置参数如下：
[0051] 回转关节1的回转轴与回转关节2的回转轴在世界坐标系Z方向上的偏移量为a1；
[0052] 回转关节1的回转轴与机器人工具夹持臂末端法兰中心在世界坐标系Z方向上的偏移量为a2；
[0053] 回转关节2的回转轴与机器人工件夹持臂末端法兰中心在世界坐标系Z方向上的偏移量为a3；
[0054] 回转关节1的回转轴与机器人工具夹持臂末端法兰中心在世界坐标系X方向上的偏移量为d1；
[0055] 回转关节5的回转轴与机器人工件夹持臂末端法兰中心在世界坐标系X方向上的偏移量为d2；
[0056] 回转关节3的回转轴与回转关节4的回转轴在世界坐标系X方向上的偏移量为b。
[0057] 进一步地，步骤S4.1中，先建立运动学方程如下：
[0058]
[0059] 其中，
[0060]
[0061]
[0062]
[0063]
[0064]
[0065]
[0066]
[0067] 其中， 是坐标系i在坐标系j下的表示，θi表示机器人各个回转关节轴i的实轴转角，i＝1～6，j＝0～5；r11、r12、r13、r21、r22、r23、r31、r32、r33、Px、Py、Pz分别是矩阵 中对应元素；
[0068] 在运动学逆解过程中， 为已知量，设
[0069]
[0070] 中各对应元素已知， 中对应元素则根据 求出，根据 两者之间的关系：
[0071]
[0072] 计算得：
[0073]
[0074] 进而得到：r11＝r′11、r12＝r′12、r13＝r′13、r21＝r′21、r22＝r′22、r23＝r′23、r31＝r′31、r32＝r′32、r33＝r′33、Px＝2′x+a2+d2r13、Py＝P′y+d1+d2r23、Pz＝P′z+d2r33；
[0075] 基于上述运动学方程，通过运动学逆解得到θ3如下：
[0076]
[0077] 步骤S4.3中，令 有
[0078]
[0079] 步骤S4.5中，令L2＝a3sinθ2+b cosθ3cosθ2+a1，有
[0080]
[0081] 步骤S4.7中，
[0082] θ4＝Atan2[r33cosθ1-r13sinθ3cos(θ1+θ2)-r23sinθ3sin(θ1+θ2)，r33sinθ3+r13cosθ3cos(θ1+θ2)+r23cosθ3sin(θ1+θ2)]
[0083] θ5＝arccos[r13sin(θ1+θ2)-r23cos(θ1+θ2)]
[0084] 步骤S4.9中，
[0085] θ6＝Atan2[r31cos(θ3+θ4)-r21sin(θ1+θ2)sin(θ3+θ4)-r11cos(θ1+θ2)sin(θ3+θ4)，r32cos(θ3+θ4)-r22sin(θ1+θ2)sin(θ3+θ4)-r12cos(θ1+θ2)sin(θ3+θ4)]。
[0086] 总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
[0087] 1、本发明由三旋转一平动的工件夹持臂和三旋转的加工装置夹持臂组成3+4新构型双臂协作机器人，能够缩短运动链长度，从而提升系统刚度及加工精度；工具夹持臂末端采用法兰与加工工具连接，则可以根据不同的加工需求更换不同的加工工具，从而一套加工系统可以适用于不同加工工艺，例如焊接、磨抛、钻孔、激光加工等。
[0088] 2、本发明能够实现“上下料-磨削加工-检测”自动化，形成“磨削加工-检测-磨削加工”的加工模式，从而进一步提高加工效率和加工精度；
[0089] 3、通过本发明实现机器加工替换手工加工，将工人从恶劣的作业环境中解放出来，提高了效率，降低了成本，保证了精度。
[0090] 4、本发明在运动学逆解过程中，通过差值取小的方式筛选正确解，可以得到
机械臂运动路径最短的、最快的最优解，提高加工效率；此外，按照本发明的特定顺序进行各个实轴角度的求解，能够降低控制器运算负荷及响应延迟，从而进一步提升加工效率及加工精度。
附图说明
[0091] 图1是本发明优选
实施例的立体示意图；
[0092] 图2是图1的俯视图；
[0093] 图3是图1中磨抛装置夹持臂的内部构造示意图；
[0094] 图4是图1中工件夹持臂的内部构造示意图；
[0095] 图5是图1中磨抛装置夹持臂的坐标系建立示意图；
[0096] 图6是图1中工件夹持臂的坐标系建立示意图；
[0097] 图7是图1中磨抛装置夹持臂和工件夹持臂的设计参数示意图；
[0098] 图8是图1中磨抛装置夹持臂和工件夹持臂的运动角度参数示意图；
[0099] 图9是本发明优选实施例的运动控制
流程图。
[0100] 在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：
[0101] 1-三坐标测量设备，2-工件夹持臂直线滑轨，3-底座，4-加工工具，5-工具夹持臂，6-工件，7-机器人控制器，8-工件夹持臂，9-夹具，10-第一轴减速机，11-第一
同步带轮传动机构，12-第一轴电机，13-第二轴电机，14-第二轴减速机，15-机器人转座，16-第三轴减速机，17-第三轴电机，18-第一
信号接口，19-线缆航插，20-机器人底座，21-末端法兰，22-第六轴减速机，23-第六轴电机，24-第五轴减速机，25-第五轴电机，26-第二信号接口，27-第四轴减速机，28-第四轴电机，29-第二同步带轮传动机构，30-机器人底座，31-直线轴电机，
32-直线轴导轨，33-第一臂部，34-第二臂部，35-第三臂部，36-第四臂部。
具体实施方式
[0102] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0103] 如图1～4所示，本发明优选实施例的一种3+4新构型双臂协作机器人加工系统，以工件6的磨抛加工为例，包括：三坐标测量设备1、工件夹持臂直线滑轨2、底座3、加工工具4、磨抛装置夹持臂5、机器人控制器7、工件夹持臂8以及夹具9。本实施例的加工工具4为打磨头，为叙述方便，下文直接表述为打磨头4。
[0104] 所述工件夹持臂直线滑轨2、所述磨抛装置夹持臂5及所述机器人控制器7均安装于所述底座3上，所述工件夹持臂8滑动安装于所述工件夹持臂直线滑轨2上。
[0105] 本实施例中，所述工件夹持臂直线滑轨2沿世界坐标系的Y轴方向水平布置，所述工件夹持臂8包括三个相互垂直的回转轴，其中一个回转轴沿世界坐标系的Z轴方向竖直布置。所述磨抛装置夹持臂5包括一个沿世界坐标系的Z轴方向竖直布置的回转轴和两个水平回转轴。所述工件夹持臂8末端通过所述夹具9安装所述工件6。所述磨抛装置夹持臂5末端通过法兰安装所述打磨头4，用于磨抛所述工件6。
[0106] 所述机器人控制器7用于控制所述工件夹持臂8进行三转动一平动的4自由度运动，以及控制所述磨抛装置夹持臂5进行3自由度转动，从而实现3+4双臂协作磨抛。
[0107] 优选地，为了提高加工效率及精度，实现“磨削加工-检测-磨削加工”的加工模式实，本实施例中在双机械臂旁边设置了所述三坐标测量设备1，便于利用所述机器人控制器7控制所述工件夹持臂8将所述工件6送入所述三坐标测量设备1，以检测所述工件6是否合格，若不合格，则控制所述工件夹持臂8将所述工件6转移至打磨头4的工作空间内，然后对不合格部位再次进行3+4双臂协作磨抛。在其他实施例中(未图示)，所述三坐标测量设备1还可以替换成产线的流水线传送带、其他加工或运输机器人、其他检测或测量设备、置物台等，可在当前加工完成后将工件6直接转移过去，从而实现加工、运输和/或检测的一体化流水作业。此外，若将打磨头4更换成其他加工工具，例如
焊枪、
钻头、激光刀等，则还可以进行焊接、钻孔、激光加工。
[0108] 优选地，如图3所示，本实施例中所述磨抛装置夹持臂5包括第一轴减速机10、第一同步带轮传动机构11、第一轴电机12、第二轴电机13、第二轴减速机14、机器人转座15、第三轴减速机16、第三轴电机17、第一信号接口18、线缆航插19、机器人底座20、第一臂部33和第二臂部34。
[0109] 所述机器人底座20安装于所述底座3上，所述第三轴电机17安装于所述机器人底座20上，且通过所述第三轴减速机16连接所述机器人转座15，所述第三轴减速机16的转轴沿世界坐标系的Z轴方向竖直布置。所述第二轴减速机14水平安装于所述机器人转座15上，其输出轴与所述第一臂部33的一端固定连接。所述第一轴减速机10水平安装于所述机器人转座15上，其输出轴与所述第一臂部33的另一端枢接且与所述第二臂部34的一端固定连接，所述第二臂部34的另一端安装所述打磨头4。所述第二轴电机13固定于所述第一臂部33上，且其输出轴通过一个第一同步带轮传动机构11连接所述第二轴减速机14的输入轴。所述第一轴电机12固定于所述第一臂部33上，且其输出轴通过另一个第一同步带轮传动机构11连接所述第一轴减速机10的输入轴。本实施例采用第一轴减速机10、第一轴电机12、第二轴电机13、第二轴减速机14四轴平行布置的方式，且使第一轴电机12、第二轴电机13位于第一轴减速机10和第二轴减速机14之间，可以最大限度地利用第一臂部33占用的空间，缩短运动链长度。
[0110] 第一信号接口18、线缆航插19用于供各个电机连接所述机器人控制器7，所述机器人控制器7用于控制所述第一轴电机12、所述第二轴电机13、所述第三轴电机17的运动，从而控制所述打磨头4的运动轨迹。
[0111] 优选地，如图4所示，本实施例中所述工件夹持臂直线滑轨2包括机器人底座30、直线轴电机31和直线轴导轨32。所述直线轴导轨32沿世界坐标系的Y轴方向水平安装于所述底座3上，所述机器人底座30安装于所述直线轴导轨32上，所述直线轴电机31用于驱动所述机器人底座30沿所述直线轴导轨32滑动。所述工件夹持臂直线滑轨2的直线驱动形式包括直线电机、滚珠
丝杆、
齿轮齿条等。
[0112] 所述工件夹持臂8包括末端法兰21、第六轴减速机22、第六轴电机23、第五轴减速机24、第五轴电机25、第二信号接口26、第四轴减速机27、第四轴电机28、第二同步带轮传动机构29、第三臂部35和第四臂部36。
[0113] 所述第四轴电机28安装于所述机器人底座30上，且通过所述第四轴减速机27连接所述第三臂部35的一端，所述第四轴减速机27的转轴沿世界坐标系的Z轴方向竖直布置。所述第五轴减速机24水平设置，其输出轴与所述第三臂部35的另一端枢接，且与所述第四臂部36的一端固定连接。所述第六轴减速机22垂直于所述第五轴减速机24布置，且固定于所述第四臂部36的另一端，所述第六轴减速机22的输出轴安装所述末端法兰21，所述末端法兰21用于安装所述工件6。所述第五轴电机25固定于所述第三臂部35上，且其输出轴连接所述第五轴减速机24的输入轴。所述第六轴电机23安装于所述第四臂部36上，且其输出轴连接所述第六轴减速机22的输入轴。
[0114] 本发明的主要操作步骤如下：
[0115] 步骤一、所述夹具9安装于机器人三旋转一平动的工件夹持臂8法兰上，所述的工件6安装于三旋转一平动的工件夹持臂8的夹具9上；
[0116] 步骤二、所述打磨头4安装于机器人三旋转的磨抛装置夹持臂5末端；
[0117] 步骤三、所述的机器人控制器7集成有
机器人运动学及动力学
算法程序模
块，可按照预设的控制流程来控制工件夹持臂8和磨抛装置夹持臂5运动；
[0118] 步骤四、所述打磨头4开始打磨所述工件，且在打磨完成后，通过三旋转一平动的工件夹持臂8将工件6装夹至三坐标测量设备1，对该工件6进行检测以判断所述工件6的磨抛表面是否满足要求，若不满足要求，则所述三旋转一平动的工件夹持臂8将工件6移动至打磨头4附近，所述打磨头4对不符合要求的部位再次进行打磨直至满足要求；若满足要求，则打磨结束。
[0119] 由上述结构介绍及主要操作步骤可知，本发明能够实现“测量-操作-加工”于一体的自动化流水作业，由此提高了加工精度及效率。其采用的机械臂构造与传统单臂六关节结构相比，大大缩短了运动链长度，提升了系统刚度，取代了人工作用，提高了效率，降低了成本，保证了精度。
[0120] 下面结合图5～图9，对本发明的控制流程及运动学算法程序模块部分进行更为详细的介绍。
[0121] 如图5、图6所示，记所述第一轴减速机、所述第二轴减速机、所述第三轴减速机、所述第四轴减速机、所述第五轴减速机、所述第六轴减速机分别为回转关节1～回转关节6，对应的坐标系分别为坐标系1～坐标系6，基坐标系为坐标系0，本发明优选实施例的工具坐标系建立如下：
[0122] (1)坐标系t表示机器人工具夹持臂末端法兰坐标系，原点在法兰末端中心处，坐标系t的Xt方向与世界坐标系Z方向同向，Yt方向与世界坐标系X方向同向，Zt方向与世界坐标系Y方向同向；
[0123] (2)坐标系0原点在机器人回转关节1回转轴与回转关节3回转轴交点处，坐标系0的X0方向与世界坐标系Z方向同向，Y0方向与世界坐标系X方向同向，Z0方向与世界坐标系Y方向同向；
[0124] (3)坐标系1与坐标系2的原点重合，位于机器人回转关节2回转轴与机器人回转关节3回转轴交点处。坐标系1的X1方向与世界坐标系Z方向同向，Y1方向与世界坐标系X方向同向，Z1方向与世界坐标系Y方向同向；坐标系2的X2方向与世界坐标系X方向同向，Y2方向与世界坐标系Y方向同向，Z2方向与世界坐标系Z方向同向；
[0125] (4)坐标系3、坐标系4、坐标系5、坐标系6的原点重合，位于机器人回转关节5回转轴与回转关节6回转轴交点处。坐标系3的X3方向与世界坐标系X方向同向，Y3方向与世界坐标系Y方向同向，Z3方向与世界坐标系Z方向同向；坐标系4的X4方向与世界坐标系X方向同向，Y4方向与世界坐标系Z负方向同向，Z4方向与世界坐标系Y方向同向；坐标系5的X5方向与世界坐标系Z负方向同向，Y5方向与世界坐标系Y方向同向，Z5方向与世界坐标系X方向同向；坐标系6的X6方向与世界坐标系Z负方向同向，Y6方向与世界坐标系Y方向同向，Z6方向与世界坐标系X方向同向；
[0126] (5)坐标系f表示机器人工件夹持臂末端法兰坐标系，原点在法兰末端中心处，坐标系f的Xf方向与世界坐标系Z负方向同向，Yf方向与世界坐标系Y方向同向，Zf方向与世界坐标系X方向同向。
[0127] 如图7所示，一些基本的配置参数说明如下：
[0128] (1)a1表示回转关节1的回转轴与回转关节2的回转轴在世界坐标系Z方向上的偏移量；
[0129] (2)a2表示回转关节1的回转轴与机器人工具夹持臂末端法兰中心在世界坐标系Z方向上的偏移量；
[0130] (3)a3表示回转关节2的回转轴与机器人工件夹持臂末端法兰中心在世界坐标系Z方向上的偏移量；
[0131] (4)d1表示回转关节1的回转轴与机器人工具夹持臂末端法兰中心在世界坐标系X方向上的偏移量；
[0132] (5)d2表示回转关节5的回转轴与机器人工件夹持臂末端法兰中心在世界坐标系X方向上的偏移量；
[0133] (6)b表示回转关节3的回转轴与回转关节4的回转轴在世界坐标系X方向上的偏移量。
[0134] 注：图中的所有参数a1、a2、a3、d1、d2、b等在机器人设计制造完成后均为已知量。
[0135] 如图8所示，θ1、θ2、θ3、θ4、θ5、θ6分别对应机器人回转关节1到回转6相对零位时转动的实轴角度。
[0136] 由于机器人逆运动学求解具有多解性，选择合适的求解结果将直接影响运动路径的合理性，并且这在多机器人协调运动中尤为重要，本实施例通过引入机器人上一位置(姿态亦可，不影响计算过程及结论)的实轴角度数据，实现对机器人运动学反解的结果进行选解。如图9所示，双臂协作打磨的控制流程如下：
[0137] S1，获取所述磨抛装置夹持臂、所述工件夹持臂的平移轴和回转轴的配置参数，得到工具坐标系的具体表示；记所述第一轴减速机、所述第二轴减速机、所述第三轴减速机、所述第四轴减速机、所述第五轴减速机、所述第六轴减速机相对于自身零位的实轴转角分别为θ1、θ2、θ3、θ4、θ5、θ6；
[0138] S2，在工件坐标系中对所述工件进行加工路径规划，得到加工的路径点，包括路径点的位置和姿态；
[0139] S3，根据所述工件的实际装夹位置与所述工件夹持臂的末端法兰坐标系的偏差，将步骤S2规划的路径点转化为所述工件夹持臂8的末端法兰盘坐标系中的刀位点，获得各个刀位点的位置和姿态；
[0140] S4，按照当前刀位点的位置和姿态以及所述加工工具4的实际装夹位置与所述工具夹持臂5的末端法兰坐标系的偏差，通过运动学逆解的方式，计算并输出θ1、θ2、θ3、θ4、θ5、θ6。具体地，步骤S4包括如下子步骤：
[0141] S4.1，先建立运动学方程，通过运动学逆解求解θ3，过程如下：
[0142] S4.1.1，运动学方程的建立：
[0143]
[0144]
[0145]
[0146]
[0147]
[0148]
[0149]
[0150] 其中， 表示坐标系i在坐标系j下的表示，θi表示机器人各个回转关节轴i的实轴转角；r11、r12、r13、r21、r22、r23、r31、r32、r33、Px、Py、Pz分别指矩阵 中对应元素，在运动学正解中该矩阵是需要求解的未知量，在运动学逆解中该矩阵为已知量。
[0151] 建立机器人的运动学方程为：
[0152]
[0153] S4.1.2，运动学正解：
[0154] 已知θ1、θ2、θ3、θ4、θ5、θ6，求 过程如下：
[0155] 根据运动学方程计算：
[0156]
[0157]
[0158] r11＝cosθ6[cosθ5cos(θ1+θ2)cos(θ3+θ4)-sinθ5sin(θ1+θ2)]-sinθ6cos(θ1+θ2)sin(θ3+θ4)
[0159] r12＝-cosθ6cos(θ1+θ2)sin(θ3+θ4)-sinθ6[cosθ5cos(θ1+θ2)cos(θ3+θ4)-sinθ5sin(θ1+θ2)]
[0160] r13＝sinθ5cos(θ1+θ2)cos(θ3+θ4)+cosθ5sin(θ1+θ2)
[0161] Px＝a3sin(θ1+θ2)+a1cosθ1+bcosθ3cos(θ1+θ2)
[0162] r21＝cosθ6[cosθ5sin(θ1+θ2)cos(θ3+θ4)+sinθ5cos(θ1+θ2)]-sinθ6sin(θ1+θ2)sin(θ3+θ4)
[0163] r22＝-cosθ6sin(θ1+θ2)sin(θ3+θ4)-sinθ6[cosθ5sin(θ1+θ2)cos(θ3+θ4)+sinθ5cos(θ1+θ2)]
[0164] r23＝sinθ5sin(θ1+θ2)cos(θ3+θ4)-cosθ5cos(θ1+θ2)
[0165] Py＝a1sinθ1-a3cos(θ1+θ2)+b cosθ3sin(θ1+θ2)
[0166] r31＝sinθ6cos(θ3+θ4)+cosθ5cosθ6sin(θ3+θ4)
[0167] r32＝cosθ6cos(θ3+θ4)-cosθ5sinθ6sin(θ3+θ4)
[0168] r33＝sinθ5sin(θ3+θ4)
[0169] Pz＝b sinθ3
[0170] 则坐标系6在坐标系t下的表示为：
[0171]
[0172] 坐标系f在坐标系t下的表示为：
[0173]
[0174] 结果即为运动学正解结果，是工件端法兰盘坐标系在工具端法兰盘坐标系下的表示。
[0175] S4.1.3，运动学逆解：
[0176] 已知 求解算出 再求θ1、θ2、θ3、θ4、θ5、θ6，过程如下：
[0177] 设 其中 中所有变量已知。
[0178] 根据 两者之间的关系：
[0179]
[0180] 计算得：
[0181]
[0182] 进而可得：r11＝r′11、r12＝r′12、r13＝r′13、r21＝r′21、r22＝r′22、r23＝r′23、r31＝r′31、r32＝r′32、r33＝r′33、Px＝P′x+a2+d2r13、Py＝P′y+d1+d2r23、Pz＝P′z+d2r33。
[0183] 联立S4.1.1的运动学方程可以解出：
[0184]
[0185] S4.2，将θ3的所有可行解与上一姿态下对应的θ3′进行比较，将差值大的解舍弃，保留差值小的解作为θ3的正确解；
[0186] S4.3，将θ3的正确解代入运动学方程计算出θ2的所有可行解；
[0187] 令 有
[0188]
[0189] S4.4，将θ2的所有可行解与上一姿态下对应的θ2′进行比较，将差值大的解舍弃，保留差值小的解作为θ2的正确解；
[0190] S4.5，将θ3、θ2的正确解代入运动学方程计算出θ1的所有可行解；
[0191] 令L2＝a3sinθ2+b cosθ3cosθ2+a1，有
[0192]
[0193] S4.6，将θ1的所有可行解于上一姿态的θ1′进行比较，将差值大的解舍弃，保留差值小的解作为θ1的正确解；
[0194] S4.7，将θ3、θ2、θ1的正确解代入运动学方程计算出θ4、θ5的所有可行解；
[0195] θ4＝Atan2[r33cosθ1-r13sinθ3cos(θ1+θ2)-r23sinθ3sin(θ1+θ2)，r33sinθ3+r13cosθ3cos(θ1+θ2)+r23cosθ3sin(θ1+θ2)]
[0196] θ5＝arccos[r13sin(θ1+θ2)-r23cos(θ1+θ2)]
[0197] S4.8，将θ4、θ5的所有可行解分别于上一姿态的θ4′、θ5′进行比较，将差值大的解舍弃，保留差值小的解作为θ4、θ5的正确解；
[0198] S4.9，将θ3、θ2、θ1、θ4的正确解代入运动学方程计算出θ6的所有可行解；
[0199]
[0200] S4.10，将θ6的所有可行解于上一姿态的θ6′进行比较，将差值大的解舍弃，保留差值小的解作为θ6的正确解；
[0201] S4.11，输出θ1、θ2、θ3、θ4、θ5、θ6。
[0202] 为了方便计算关节转角，在上述公式中引入了双参变量的反正切函数Atan2(y，x)，x、y表示上述各个公式中对应位置的参数项，例如：
[0203] 中，x＝L2、 Atan2(Px，Py)中，x＝Px、y＝Py；
[0204] 函数Atan2(y，x)在计算 时可以根据x和y的符号判别求得角所在的象限。
[0205] 在上述步骤S3、S4中，由于工件的实际装夹位置与所述工件夹持臂的末端法兰坐标系的偏差，以及加工工具的实际装夹位置与所述工具夹持臂的末端法兰坐标系的偏差，均是简单的平移换算关系，该偏差计算是本领域技术人员的基本技能，故未做特别介绍，而在本发明的实施例中直接以工件夹持臂的末端法兰坐标系和工具夹持臂的末端法兰坐标系进行运动学方程的建立及求解，以期简明扼要地介绍本发明的主要控制流程。并且，由于本发明并不仅限于进行磨抛加工，还适用于焊接、磨抛、钻孔、激光加工等，而不同工件、不同加工工具对应的实际偏差量数值也会不同，直接以工件夹持臂的末端法兰坐标系和工具夹持臂的末端法兰坐标系进行运动学方程的建立及求解更能反映本发明的精神实质，具体的偏差换算则由本领域技术人员按常规方式计算即可。
[0206] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
