技术领域
[0001] 本
发明涉及一种机械臂示教系统，尤其是一种无线控制的六自由度机械臂示教系统。
背景技术
[0002] 随着各行业对工业
机器人的广泛使用，他们期望机器人可以在最短时间，以更简单的方式，完成更加复杂的任务，尤其是
喷涂、
焊接、
抛光打磨等轨迹规划复杂的行业。目前
工业机器人的使用，距离实现完全智能化还有一段距离，大多数任务的完成主要依靠示教——再现形式实现，那么如何实现示教依然是机器人执行加工任务前的一项重要工作。示教——再现中的“示教”顾名思义为示范教导的意思，即在机器人正常加工之前，操作者利用某种交互方式示范执行加工任务时所需动作，并且将示教过程记录在
存储器中。“再现”即对示教的过程重现，利用存储器中的数据，经过一定的转换，并经由机器人的
驱动器、
控制器实现机器人对
工件的加工动作。示教-再现的工作，一般包括示教轨迹点的
位置、
姿态，运动过程中的速度、
加速度。
[0003] 目前工业机器人多数采用示教盒示教，对于复杂曲面的加工，示教任务非常繁重，任务的苛刻所带来的问题使示教盒形式的示教过程以及前期的轨迹规划需要耗费大量的时间、资源。示教复杂轨迹任务需获取大量点位信息。示教点
位姿的获取，依靠按示教盒的指令键来
移动机器人，人眼查看机器人末端位姿与工件的位姿关系来获得，每个点的获得都需要较长的调整时间，示教点数的增多使示教周期更长，对于轨迹运动的规划也变得复杂，导致示教工作繁重，大大降低了机器人的加工效率。用示教盒
键盘对于机器人进行示教，需要操作者具备专业的机器人操作技能和编程经验，而且现在不同厂家制造的示教盒规格是不同的，操作培训周期较长。
[0004] 因此需要用直接示教来延续技术工人加工工件的经验，记录其加工过程的轨迹，于此同时，也免去了前期复杂的轨迹规划工作。但是，工件加工技术的执行人员大多是未接受过编程技术、操作
机器人技术知识教育，并且再学习能
力较低的人群。那么，如何延续技术工人的加工经验与控制机器人运动完成示教以及实现编程处理等工作，两者成为矛盾。若把技术经验的延续和示教操作的实施由两类人合作完成，则增加了机器人的使用成本，长此以往，也不可取。总之，局限于编程、经验积累同时进行的示教模式，是无法维持高效生产的，故亟需一种快速示教并且兼容技术工人示教经验的示教系统来实现工业机器人的示教作业。
[0005] 而传统的机械臂示教系统没有使用WiFi无线通信，因此需要复杂的接线工作，并且
连接线长度也限制了示教工作的空间。传统的机械臂示教系统采集
频率不够高，且没有引入轨迹规划
算法，最终工作机械臂执行的示教轨迹不平滑，机械臂末端工作速度变化不均匀，不能达到预期的工作要求。大部分机械臂示教系统都没有良好
人机交互性能的PC端APP，无法实时反馈采集数据，
修改采集频率和轴地址不方便，因此不能让用户有良好的操作体验，无法达到较好的示教效果。
[0006] 目前，针对工业机械臂示教系统的设计主要集中在机械臂结构、有线通信和示教控制器等方面，针对无线通信、APP控制和示教轨迹优化方面的研究较少。姜声华、丁文力、李晗提出了一种工业机器人示教系统(姜声华，丁文力，李晗.一种工业机器人示教系统：中国，205852817[P].2017-01-04)，给出了工业机械臂的一种机械结构设计及其设计原理，但没有示教控制工具，用户交互操作极其不便；朱茂娟、王林
冰、曾奇等提出一种机器人及其示教通信系统(朱茂娟，王林冰，曾奇等.机器人及其示教通信系统:中国，205620710[P].2016-01-04),实现了示教采集器与控制器之间的数据通信，但是通信方式是有线的，具有空间局限性且缺少
保密性；陈恳、任书楠、王国磊等提出了一种工业机器人示教系统(陈恳，任书楠，王国磊等.工业机器人示教系统:中国，104700705[P].2015-06-10)，该系统能够完成示教采集、数据通信和示教控制，但是没有对示教轨迹进行优化，不能满足高
精度的工业生产要求；夏泽洋、邓豪、翁少葵等提出一种机器人示教系统及其控制方法(夏泽洋，邓豪，翁少葵等.机器人示教系统及其控制方法:中国，105058396[P].2015-11-18)，详细介绍了机器人的示教过程，但是没有设计良好交互性能的APP，用户操作繁琐不便。
发明内容
[0007] 本发明要克服
现有技术的上述缺点，提出一种六自由度机械臂示教系统。
[0008] 首先该系统具有人性化的操作界面，让不同的用户都能够方便地进行示教过程的各种操作和设置，并显示各种数据和参数供用户直观
跟踪整个示教过程；其次该系统使用了WiFi无线通信方式，实现了无线控制，建立连接方便，数据的传输高效稳定，没有了数据线的限制，
工作空间得以扩展；再次该系统引入了笛卡尔轨迹规划算法，使得最终的示教轨迹更加平滑，
末端执行器速度变化更加均匀；最后该系统可以生成控制工作机械臂执行示教轨迹的代码文件，一次示教即可供多台工作机械臂无限次执行该示教作业。
[0009] 本发明为解决现有技术问题所采用的技术方案是：
[0010] 一种无线控制的六自由度机械臂示教系统，其特征在于：PC端APP安装在用户的Windows电脑上，
数据采集器与PC端APP通过WiFi无线连接，数据采集器与示教机械臂通过RS485
电路连接。
[0011] 所述的PC端APP包括依次连接的交互界面模
块、APP主控模块、轨迹优化模块、控制文件生成模块，控制文件生成模块通过文件传输协议FTP将控制代码文件下载到一个或多个工作机械臂中，工作机械臂完成优化后的示教工作。
[0012] 数据采集器连接MCU主控模块，MCU主控模块通过WiFi无线连接PC端，数据采集器与示教机械臂关节的六个绝对式
编码器通过RS485电路进行双向通信连接，六个
绝对式编码器分别对应机械臂的六个自由度。数据采集器输出数据给MCU主控模块，MCU主控模块输出数据给PC端，PC端输出操作指令给MCU主控模块，MCU主控模块输出操作指令给数据采集器。数据采集器和示教机械臂、MCU主控模块都连接外接电源模块。
[0013] 各模块的具体构成是：
[0014] 供电模块，其输入为+24V交流
电压，电压输入后首先经过保护电路，保护电路可以防止
电流回流对电源产生影响和破坏。之后+24V交流电压进入稳压滤波电路，再将稳压滤波后的+24V交流电压作为六自由度示教机械臂关节中的绝对式编码器的供电电源。然后+24V交流电压经过降压电路、整流电路和稳压滤波电路分别转变为+5.4V直流电压和+3.3V直流电压，+5.4V直流电压用于给部分数据采集电路供电，+3.3V直流电压用于给MCU和部分数据采集电路供电。
[0015] 数据采集模块，通过RS485电路与六自由度示教机械臂关节中的绝对式编码器连接，通过串口与MCU的主芯片连接。当数据采集模块收到MCU主控模块发来的控制指令后，通过RS485电路对六个绝对式编码器发送广播指令或查询指令代码，编码器根据相应指令返回某个关节的实时
角度数据或者六个关节的实时同步运动角度数据，采集模块再将角度数据通过串口发送给MCU主控模块。
[0016] MCU主控模块，由主芯片和其它辅助电路组成，负责通信功能的配置以及指令和数据的处理。MCU主控模块控制数据采集器通过WiFi通信模块与PC端建立连接；将接收到的一个或多个编码器的角度信息进行整合处理，并通过WiFi通信模块发送给PC端APP；通过WiFi通信模块接收来自PC端APP的各种指令，做出相应处理，包括向PC端APP发送连接响应、采集准备就绪
信号、停止采集信号、采集频率设置成功或失败响应、采集频率和采集周期信息、修改各轴地址成功或失败响应、各轴地址信息等，和向数据采集模块发送开始采集指令、停止采集指令等，以及修改各轴地址、修改采集频率等。
[0017] WiFi通信模块，由下位机数据采集器的WiFi模块开启WiFi热点，PC端通过无线网络连接接入热点，采用UDP通信协议模式建立无线连接，并通过WiFi透传模式快速传输数据。通过WiFi通信模块，PC端APP可以向数据采集器发送各种控制指令，数据采集器可以向PC端APP发送角度和参数信息。其数据传输方式采用UDP通信协议，数据采集器与PC端APP互相
锁定了对方的IP地址和端口号，为一对一的通信，第三方无法再接入连接，保证了数据传输的保密性；对指令、参数、角度数据都做了校验，保证了数据传输的正确性；采用WiFi透传模式，数据的无线收发几乎无延迟，保证了数据传输的高效性；所用的WiFi模块传输距离远穿墙能力强，并且一旦锁定了双方的IP地址和端口号后，无论是数据采集器或是PC端单方断电掉线重连后都能直接继续进行数据传输，保证了数据传输的
稳定性。
[0018] 交互界面模块，用于用户对整个示教系统进行设置和操作，以及显示示教系统的各种相关参数和数据；通过PC端APP的交互界面，用户可以设置要连接的WiFi热点的IP地址和网络端口号、示教机械臂各个关节内的编码器的正反向标定、角度数据的采集频率、示教机械臂各个轴的轴地址、所使用的机械臂型号以及控制代码文件的下载路径等，并将这些设置参数传给APP主控模块处理；用户还能在PC端APP的交互界面进行WiFi通信的连接和断开、角度数据采集的开始和停止、生成控制代码文件、查询数据采集器就绪状态、查询角度数据采集频率和周期以及查询示教机械臂各轴的轴地址等操作，这些操作指令会传给APP主控模块进行处理；交互界面还会对所有能设置的参数和采集得到的角度数据进行显示，这些参数和数据从APP主控模块得到；
[0019] APP主控模块，是PC端APP的通信中枢和控制中枢，对各种数据、参数和指令进行收发和处理。APP主控模块可以控制PC端APP通过WiFi通信模块与数据采集器建立通信连接。可以从交互界面模块接收到参数设置指令和操作指令，进行处理后生成相应通信协议的控制指令，并通过WiFi通信模块发送给MCU主控模块和通过网络通信控制仿真模块。能够通过WiFi通信模块接收来自数据采集器的角度数据和参数信息，将实时角度数据和参数信息传给交互界面显示，并在示教结束后(即停止数据采集后)生成示教机械臂的各个关节的运动角度数据包，将角度数据包传给轨迹优化模块进行优化和通过网络通信发给仿真模块进行仿真。
[0020] 轨迹优化模块，接收来自APP主控模块的示教机械臂的各个关节的运动角度数据包，结合当前所使用的机械臂型号的结构参数，根据D-H参数建模方法建立机械臂每个杆件在各个关节处的笛卡尔
坐标系，用正运动学公式求解末端执行器相对固定参考坐标系的位置和姿态，求出多组解后即得到了示教轨迹。再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化。最后利用逆运动学公式结合当前所使用的机械臂型号的结构参数对优化后的轨迹求其关节空间中各关节的关节变量，即可获得到每个采集时刻优化后的运动角度数据包。可将角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。所述的正逆运动学求解和笛卡尔轨迹规划算法如下：
[0021] 1)选择示教机械臂六个关节的D-H参数(即关节1≤i≤6的
连杆长度ai、连杆转角αi、
连杆偏距di和关节角θi)作为输入量，选择工作机械臂各个关节的关节角θi作为输出量。
[0022] 2)首先对示教机械臂构建其D-H参数模型，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，用以下四个参数来描述杆件：
[0023] (1)连杆长度ai：关节轴i和关节轴i+1之间公垂线的长度；
[0024] (2)连杆转角αi：作一个与两关节轴之间的公垂线垂直的平面，将关节轴i和关节轴i+1投影到该平面，在平面内轴i按右手法则绕ai
转向轴i+1，其转角即为连杆转角；
[0025] (3)连杆偏距di：公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i+1的交点的有向距离长度；
[0026] (4)关节角θi：ai-1的延长线与ai之间绕关节轴i旋转所形成的夹角，即采集得到的示教机械臂
关节角度。
[0027] 得到示教机械臂六个关节的D-H连杆参数表1所示：
[0028] 表1示教机械臂D-H连杆参数表
[0029]关节i 连杆长度ai 连杆转角αi 连杆偏距di 关节角θi
1 a1 α1 d1 θ1
2 a2 α2 d2 θ2
3 a3 α3 d3 θ3
4 a4 α4 d4 θ4
5 a5 α5 d5 θ5
6 a6 α6 d6 θ6
[0030] 3)以
基座坐标系作为T0坐标系，设 表示i关节坐标系相对i-1关节坐标系的位置和姿态，则：
[0031]
[0032] 其中，sθi＝sinθi，cθi＝cosθi，sαi＝sinαi，cαi＝cosαi。
[0033] 4)对于六自由度机械臂，获得机械臂末端执行器位姿相对固定参考坐标系的变换矩阵(即机械臂末端位置点坐标的变换矩阵)。
[0034]
[0035] 5)求得每个采集时刻的示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹。
[0036] 6)对于圆弧位置插补，取任务空间中p1(x1,y1,z1)、p2(x2,y2,z2)和p3(x3,y3,z3)三点，这三点组成的平面方程为：
[0037] Ax+By+Cz＝1
[0038] 由下式可解得平面方程参数A、B和C的值：
[0039]
[0040] 7)求出p1p2和p2p3两条线段的中点p12(x12,y12,z12)，p23(x23,y23,z23)，设圆心坐标为po(xo,yo,zo)，由平面几何关系获得圆心满足下列方程：
[0041]
[0042] 求解获得圆心坐标，以圆心为球心，r为半径的球参数方程为：
[0043]
[0044] 8)将上式代入平面方程式Ax+By+Cz＝1中，得到：
[0045] A(rcosφcosα+xo)+B(rcosφsinα+yo)+C(rsinα+zo)＝1
[0046] 因为圆心po(xo,yo,zo)也在平面上，所以：
[0047] Arcosφcosα+Brcosφsinα+Crsinα＝0
[0048] 从上式可以解得：
[0049]
[0050] 从而可获得空间圆的参数方程为：
[0051]
[0052] 9)取矢量 为X1轴向量，平面法向量为Z1轴向量，建立T1坐标系，由于姿态变换矩阵为单位
正交矩阵，则有Y1＝X1×Z1，进而可以求得Y1轴向量，其姿态变换矩阵R和平移向量T为：
[0053]
[0054] 设 则有：
[0055]
[0056] 姿态变换矩阵R和平移向量T可以得到坐标变换矩阵为：
[0057]
[0058] 10)取末端轨迹在T1的位置坐标为p'(x',y',z')，通过姿态变换矩阵和平移向量可以求得末端轨迹在T0中的位置p(x,y,z)，即：
[0059] [x y z 1]T＝Q·[x' y' z' 1]T
[0060] 在T1中，独立参数α圆弧轨迹为：
[0061]
[0062] 其中，α∈(0,αmax)，αmax为圆弧的弧角值。
[0063] 对α进行运动规划，得到轨迹p'(x',y',z')后，通过变换得到(x,y,z)，即为圆弧插补的规划轨迹。
[0064] 11)在优化后的轨迹上选择需要的点，并列出每个插补点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵：
[0065]
[0066] 12)对每个优化后的插补点p(x,y,z)进行逆运动学求解，其中rij取与插补点最近的末端坐标点的变换矩阵中的值，则可按顺序求解得到：
[0067]
[0068]
[0069]
[0070] θ23＝A tan2[(-a3-a2c3)pz-(c1px+s1py)(d4-a2s3)
[0071] (a2s3-d4)pz-(a3+a2c3)(c1px+s1py)]
[0072] θ2＝θ23-θ3
[0073] θ4＝A tan2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23)
[0074] s5＝-[r13(c1c23c4+s1s4)+r23(s1c23c4-c1s4)-r33(s23c4)]
[0075] c5＝r13(-c1s23)+r23(-s1s23)+r33(-c23)
[0076] θ5＝A tan2(s5,c5)
[0077] s6＝-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31(s23s4)
[0078] c6＝r11[(c1c23c4+s1s4)c5-c1s23s5]+r21[(s1c23c4-c1s4)c5-s1s23s5]-r31(s23c4c5+c23s5)
[0079] θ6＝A tan2(s6,c6)
[0080] 其中，A tan2为反正切函数，si＝sinθi，ci＝cosθi。
[0081] 13)由于各个关节角是用反三角函数计算出来的，丢失了函数的周期信息，因此在每个关节角度求出后，需要在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值。求出每个优化后的末端位姿对应的各个关节角，
整理成角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。
[0082] 仿真模块，使用RoboDK仿真
软件，模拟工作机械臂的工作轨迹执行情况。PC端APP可以将采集得到的示教过程中示教机械臂的各个关节的运动角度数据包直接通过网络通信传输给RoboDK，RoboDK软件中的虚拟工作机械臂会执行相应的示教运动轨迹。能够先对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包在轨迹优化模块中进行优化后，再通过网络通信将优化后的运动角度数据包传输给RoboDK，RoboDK软件中的虚拟工作机械臂就会执行相应的优化后的示教运动轨迹。当确定RoboDK软件中执行的优化后的示教运动轨迹是满足工作需求的后，再让控制文件生成模块生成控制实际工作机械臂完成工作所需的控制代码文件。
[0083] 控制文件生成模块，生成控制实际工作机械臂完成工作所需的控制代码文件。其输入是轨迹优化模块对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包进行优化后的运动角度数据包。若仿真模块肯定了优化方案的可行性，则控制文件生成模块在收到优化后的运动角度数据包后，就将其转化为可以控制实际工作机械臂执行工作的控制代码文件。再通过FTP(文件传输协议)将控制代码文件下载到一个或多个工作机械臂中，由工作机械臂完成优化后的示教工作。
[0084] 用户使用PC端APP无线控制数据采集器。对六自由度机械臂进行直接示教，数据采集器同步采集安置在示教臂六个关节中的绝对式编码器的角度信息，并将角度信息发送给PC端APP。PC端APP无线接收来自数据采集器的角度信息，结合正逆运动学公式和笛卡尔轨迹规划算法对示教轨迹进行优化，并生成控制代码文件用以控制工作臂完成优化后的示教作业。用户在PC端APP的交互界面可以查看各种参数和数据，并且可以进行各种设置和操作。
[0085] 本发明的优点和积极效果是：
[0086] 首先该系统具有人性化的操作界面，让不同的用户都能够方便地进行示教过程的各种操作和设置，并显示各种数据和参数供用户直观跟踪整个示教过程；其次该系统使用了WiFi无线通信方式，实现了无线控制，建立连接方便，数据的传输高效稳定，没有了数据线的限制，工作空间得以扩展；再次该系统引入了笛卡尔轨迹规划算法，使得最终的示教轨迹更加平滑，末端执行器速度变化更加均匀；最后该系统可以生成控制工作机械臂执行示教轨迹的代码文件，一次示教即可供多台工作机械臂无限次执行该示教轨迹。
[0087] 本发明采用了PC端APP来控制角度数据采集器，相较于传统的用户终端控制器，PC端APP可安装在用户的电脑上，节省了用户终端控制器带来的器械制作成本；无需担心用户终端控制器带来的机械故障；也不用为用户终端控制器花费过多的
电池开销；APP可以在线升级，方便开发者对其进行问题修复和产品升级；控制器以APP的形式出现，也顺应了现今电器智能化的趋势，更容易为用户所接受。
[0088] 本发明比较其它类别的机械臂示教系统，成本低廉，结构简单，搭建方便，便于操作，可以满足各种不同类型的用户需求。本发明相较于传统的机械臂示教系统通过用户终端控制器进行操作，使用PC端APP并结合笛卡尔轨迹规划算法优化示教轨迹，采用信息化的界面显示，便捷化的无线操控，简单化的按键控制，并可以一键生成控制代码文件，让用户获得相对数据化的参数信息、便易化的操作体验。现在的示教盒示教操作过于繁琐，培养代价较大，直接示教的轨迹又不够光滑，速度变化不均，一种无线控制的六自由度机械臂示教系统是进行喷涂、焊接、抛光、打磨等复杂工作的行业的优良选择，专业化、低成本、便安置、易操作、通用性强等特点方便了所有厂家和操作者。
附图说明
[0089] 图1为本发明的平台组成示意图。
[0091] 图3a为本发明的PC端APP的WiFi连接操作界面图。
[0092] 图3b为本发明的PC端APP的机械臂型号选择界面图。
[0093] 图3c为本发明的PC端APP的关节角度正反标定界面图。
[0094] 图3d为本发明的PC端APP的轴地址设置界面图。
[0095] 图3e为本发明的PC端APP的采集频率设置界面图。
[0096] 图3f为本发明的PC端APP的采集及控制代码生成界面图。
[0097] 图3g为本发明的PC端APP的文件下载设置界面图。
具体实施方式
[0098] 以下结合附图对本发明做进一步详述。
[0099] 一种无线控制的六自由度机械臂示教系统，如图1所示，平台组成主要包括PC端APP1、六自由度机械臂2和数据采集器3。PC端APP1安装在用户的电脑上，便于用户进行各种设置和操作，也可以显示参数信息和角度数据；PC端APP1与六自由度机械臂2之间用数据线连接，此时六自由度机械臂2作为工作机械臂，PC端APP1可以将控制代码文件下载给六自由度机械臂2，让其执行优化后的示教轨迹；PC端APP1与数据采集器3之间通过WiFi模块无线连接，PC端APP1可以给数据采集器3下达各种设置和操作指令，数据采集器3将参数和角度信息反馈给PC端APP1；数据采集器3与六自由度机械臂2之间通过RS485电路连接，此时六自由度机械臂2作为示教机械臂，数据采集器3可以对六自由度机械臂2发送广播和查询指令，六自由度机械臂2向数据采集器3返回相应的角度数据信息；电源模块4外接+24V电源，数据采集器3与电源模块4通过电线连接，电源模块4给数据采集器3提供所需的工作电压。
[0100] 结合图2和图3，本发明
专利的具体实施方式如下：
[0101] PC端APP安装在用户的电脑上，数据采集器连接电源模块。按照对图1的描述搭好机械臂示教平台后，用户即可进行六自由度机械臂的示教操作。
[0102] 用户将供电模块外接+24V电源，供电模块采用了LM2576S系列降压芯片和B0505S系列电源芯片，供电模块对外接电压进行降压稳压之后分别给MCU模块、RS485电路和编码器提供+3.3V、+5.4V和+24V电源。
[0103] MCU主控模块采用了STM32F103系列主芯片，需要+3.3V外部供电，由供电模块提供。MCU主控模块可以控制WiFi通信模块开启WiFi热点，并建立数据采集器与PC端APP的WiFi通信连接；MCU主控模块接收到APP主控模块发来的读取编码器轴地址指令后，可以读取当前各关节中的编码器的轴地址，并通过WiFi通信模块向APP主控模块反馈当前各个编码器的轴地址；接收到APP主控模块发来的修改编码器轴地址指令后，完成该编码器轴地址的修改，并向APP主控模块反馈修改成功响应；接收到APP主控模块发来的修改采集频率指令后，完成采集频率和周期的修改，并向APP主控模块反馈修改成功响应；接收到APP主控模块发来的查询采集频率和周期指令后，向APP主控模块反馈当前采集频率和周期；接收到APP主控模块发来的开始采集指令后，控制采集模块进行示教角度数据采集，并向APP主控模块发送角度数据；接收到APP主控模块发来的停止采集指令后，向APP主控模块反馈停止采集响应。
[0104] WiFi模块采用了ESP8266 WiFi模块，MCU模块上电后开始自动设置ESP8266WiFi模块的WiFi热点的账号和密码并开启WiFi热点，设置数据传输协议为UDP协议，设置通信方式为透传模式，设置数据采集器的本机IP地址和端口号，并锁定PC端APP的IP地址和端口号；PC端使用无线网络连接功能接入该WiFi热点。
[0105] 在PC端APP的交互界面，打开PC端APP的WiFi连接操作界面，设置PC端APP的本机IP地址和端口号，并锁定数据采集器的IP地址和端口号，点击打开按钮即可建立数据通信连接；打开PC端APP的机械臂型号选择界面，选择当前使用的机械臂型号，获取相关连杆参数；打开PC端APP的关节角度正反标定界面，根据安装在机械臂各个关节中的编码器的位置姿态标定其正反，保证每个编码器的角度正变化都是同一个方向；打开PC端APP的轴地址设置界面，点击读取地址按钮后APP主控模块通过WiFi通信向MCU主控模块发送读取编码器轴地址指令，可以读取当前各关节中的编码器的轴地址；也可以根据机械臂各个关节的顺序设置该关节的编码器的轴地址，以便按顺序采集到对应关节的编码器角度数据；打开PC端APP的采集频率设置界面，可以设置此次示教的采集频率和周期；也可直接查询当前的采集频率和周期；打开PC端APP的采集及控制代码生成界面，点击开始
采样后APP主控模块通过WiFi通信向MCU主控模块发送开始采集指令；点击停止采样，APP主控模块通过WiFi通信向MCU主控模块发送停止采集指令，并将保存的角度数据包传给轨迹优化模块。
[0106] 数据采集模块电路主要由串口和RS485电路组成，MCU主控模块收到开始采集指令后控制数据采集模块执行采集功能，数据采集模块根据采集周期定时按关节顺序向各个编码器发送广播指令和查询指令；采集期间由操作人员手持示教机械臂末端进行示教操作，数据采集模块收到六个编码器的角度数据传给MCU主控模块整合后，通过WiFi通信模块向APP主控模块发送实时角度数据，APP主控模块将收到的角度数据发送给交互界面模块显示并保存成角度数据包；
[0107] 在采集过程中，采集得到的角度数据会直接通过网络通信同步发到RoboDK仿真软件，因此RoboDK软件中的虚拟工作机械臂会跟随操作人员同步执行示教轨迹的仿真，当采集结束时RoboDK仿真软件的虚拟工作机械臂停止执行示教轨迹；轨迹优化之后，在采集及控制代码生成界面点击生成RoboDK程序按钮，便可将采集得到的角度数据包发给RoboDK仿真软件，RoboDK仿真软件根据收到的角度数据包仿真工作机械臂执行优化后的示教轨迹的过程并将轨迹以曲线的形式具象化，用户通过观察轨迹曲线的情况判断该优化后的示教轨迹是否可以用于实际工作机械臂的生产工作中。
[0108] 轨迹优化模块收到示教角度数据包后，结合当前所使用的机械臂型号的结构参数得到示教轨迹，再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化，最后利用逆运动学公式结合当前所使用的机械臂型号的结构参数得到每个采集时刻优化后的运动角度数据包，具体步骤如下：
[0109] 1)选择示教机械臂六个关节的D-H参数(即关节1≤i≤6的连杆长度ai、连杆转角αi、连杆偏距di和关节角θi)作为输入量，选择工作机械臂各个关节的关节角θi作为输出量；
[0110] 2)首先对示教机械臂构建其D-H参数模型，以PUMA560机器人为例，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，取第n个采集时刻示教机械臂六个关节的D-H连杆参数表2所示：
[0111] 表2示教机械臂D-H连杆参数表
[0112]关节i 连杆长度ai 连杆转角αi 连杆偏距di 关节角θi
1 0 0 0 30°
2 -90° 0 0 -30°
3 0 3 6 -60°
4 -90° 4 8 -90°
5 90° 0 0 60°
6 -90° 0 0 90°
[0113] 由i关节坐标系相对i-1关节坐标系的位置和姿态 的表达式
[0114]
[0115] 和 其中sθi＝sinθi，cθi＝cosθi，sαi＝sinαi，cαi＝cosαi，可以求得示教机械臂末端执行器在第n个采集时刻的位姿相对固定参考坐标系的变换矩阵为：
[0116]
[0117] 3)可以得到示教机械臂末端执行器在第n个采集时刻相对固定参考坐标系的位置坐标为pn(6.178,10.495,5.5)；
[0118] 同理求得每个采集时刻的示教臂的末端执行器相对固定参考坐标系的位置坐标后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹；
[0119] 4)对于圆弧位置插补，取任务空间中p1(x1,y1,z1)、p2(x2,y2,z2)和p3(x3,y3,z3)三点，令p1(2,0,0)、p2(2,2,0)和p3(0,2,0)，根据三点组成的平面方程
[0120]
[0121] 获得：
[0122] A＝0,B＝0,C＝4
[0123] 由p1p2和p2p3两条线段的中点p12(x12,y12,z12)，p23(x23,y23,z23)，可以得到平面内同时经过p1、p2和p3的唯一圆的圆心po(xo,yo,zo)和半径r：
[0124]
[0125] 5)以圆心为原点，参照圆所在的平面，建立新的坐标系O'X'Y'Z'(即T1)：坐标原点为圆弧圆心po(xo,yo,zo)，所在平面的法线为Z'轴方向，O'p1的方向为X'轴方向，在通过右手定则确定Y'轴方向。可知，新坐标到基座标系的齐次变换矩阵为：
[0126]
[0127] 其中，
[0128]
[0129] 6)在新坐标系O'X'Y'Z'中，圆弧轨迹为：
[0130]
[0131] 其中，α∈(0,αmax)，αmax为圆弧的弧角值，得到轨迹p'(x',y',z')后，通过变换[x y T Tz 1]＝Q·[x' y' z' 1]得到(x,y,z)，即为圆弧优化后的规划轨迹；
[0132] 7)包括圆弧的起点和终点，均匀地取四个点作为新的机械臂末端坐标，即(2,0,0)，(2.366,1.366,0)，(1.366,2.366)，(0,2,0)，则机械臂在这四点间的运动轨迹既平滑又匀速；
[0133] 同理对示教轨迹的坐标点中的每三个点为一组进行优化，则可以得到优化后的示教轨迹的离散坐标点；
[0134] 8)对每个优化后的坐标点p(x,y,z)进行逆运动学求解，列出每个点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵
[0135]
[0136] 其中rij取与插补点最近的末端坐标点的变换矩阵中的值，以PUMA560机器人为例，结合工作机械臂的D-H参数并使用下面的公式进行求解：
[0137]
[0138]
[0139]
[0140] θ23＝A tan2[(-a3-a2c3)pz-(c1px+s1py)(d4-a2s3)
[0141] (a2s3-d4)pz-(a3+a2c3)(c1px+s1py)]
[0142] θ2＝θ23-θ3
[0143] θ4＝A tan2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23)
[0144] s5＝-[r13(c1c23c4+s1s4)+r23(s1c23c4-c1s4)-r33(s23c4)]
[0145] c5＝r13(-c1s23)+r23(-s1s23)+r33(-c23)
[0146] θ5＝A tan2(s5,c5)
[0147] s6＝-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31(s23s4)
[0148] c6＝r11[(c1c23c4+s1s4)c5-c1s23s5]+r21[(s1c23c4-c1s4)c5-s1s23s5]-r31(s23c4c5+c23s5)
[0149] θ6＝A tan2(s6,c6)
[0150] 表3工作机械臂D-H连杆参数表
[0151]关节i 连杆长度ai 连杆转角αi 连杆偏距di 关节角θi
1 0 0 0 θ1
2 -90° 0 0 θ2
3 0 3 6 θ3
4 -90° 4 8 θ4
5 90° 0 0 θ5
6 -90° 0 0 θ6
[0152] 由于各个关节角是用反三角函数计算出来的，丢失了函数的周期信息，因此在每个关节角度求出后，需要在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值，令优化得到的插补点的坐标为p(6.1782,10.4952,5.5)，与插补点最近的末端坐标点的变换矩阵为
[0153]
[0154] 以及工作机械臂的D-H参数如表3所示，则可以求得p点所对应的工作机械臂的各个关节的最优角度(弧度制)为θ1＝0.5236,θ2＝-0.5236,θ3＝-1.0472，θ4＝-1.5708，θ5＝1.0472，θ6＝1.5708；
[0155] 9)按8)中的步骤同理求出每个优化后的末端坐标和姿态对应的工作臂的各个关节角，整理成角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成；
[0156] 当用户判断优化后的示教轨迹可以用于实际工作机械臂的生产工作中后，在采集及控制代码生成界面点击生成机器人程序，即可自动生成控制对应型号的工作机械臂的控制代码文件；打开PC端APP的文件下载设置界面，可以设置生成的控制对应型号的工作机械臂的代码文件的下载路径，控制代码文件将在该路径中永久保存，之后用户可以将该控制代码文件传输给任意对应型号的机械臂去完成该示教工作内容。
[0157] 以上是整个系统的控制情况，PC端APP的交互界面模块和WiFi模块的存在，让用户的操作变得简单方便，轨迹优化模式让示教轨迹更加平滑，示教运动速度变化更加均匀，控制文件生成模块让示教轨迹的应用时间更加长久、应用对象数量更加庞大。
[0158] 要强调的是，本发明所述的
实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的类似的其它实施方式，同样属于本发明的保护范围。
