CN107363812B - 无线控制的六自由度机械臂示教*** - Google Patents
无线控制的六自由度机械臂示教*** Download PDFInfo
- Publication number
- CN107363812B CN107363812B CN201710665078.9A CN201710665078A CN107363812B CN 107363812 B CN107363812 B CN 107363812B CN 201710665078 A CN201710665078 A CN 201710665078A CN 107363812 B CN107363812 B CN 107363812B
- Authority
- CN
- China
- Prior art keywords
- teaching
- module
- mechanical arm
- joint
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005457 optimization Methods 0.000 claims abstract description 25
- 230000002452 interceptive effect Effects 0.000 claims abstract description 19
- 238000012546 transfer Methods 0.000 claims abstract description 4
- 230000007175 bidirectional communication Effects 0.000 claims abstract description 3
- 230000006854 communication Effects 0.000 claims description 61
- 238000004891 communication Methods 0.000 claims description 59
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000000034 method Methods 0.000 claims description 27
- 230000009466 transformation Effects 0.000 claims description 26
- 238000012545 processing Methods 0.000 claims description 24
- 238000004088 simulation Methods 0.000 claims description 23
- 230000005540 biological transmission Effects 0.000 claims description 22
- 230000008569 process Effects 0.000 claims description 18
- 239000012636 effector Substances 0.000 claims description 17
- 239000013598 vector Substances 0.000 claims description 16
- 230000006870 function Effects 0.000 claims description 10
- 230000004044 response Effects 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 9
- 230000009471 action Effects 0.000 claims description 4
- 230000008901 benefit Effects 0.000 claims description 4
- 230000003993 interaction Effects 0.000 claims description 4
- 230000000087 stabilizing effect Effects 0.000 claims description 4
- 230000002441 reversible effect Effects 0.000 claims description 3
- 230000000149 penetrating effect Effects 0.000 claims description 2
- 230000009467 reduction Effects 0.000 claims description 2
- 230000006641 stabilisation Effects 0.000 claims description 2
- 238000011105 stabilization Methods 0.000 claims description 2
- 230000001360 synchronised effect Effects 0.000 claims description 2
- 238000004804 winding Methods 0.000 claims description 2
- 229920000535 Tan II Polymers 0.000 description 9
- 238000010586 diagram Methods 0.000 description 9
- 101150053844 APP1 gene Proteins 0.000 description 6
- 101100189105 Homo sapiens PABPC4 gene Proteins 0.000 description 6
- 102100039424 Polyadenylate-binding protein 4 Human genes 0.000 description 6
- 230000004048 modification Effects 0.000 description 6
- 238000012986 modification Methods 0.000 description 6
- 238000013480 data collection Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 206010063385 Intellectualisation Diseases 0.000 description 2
- 238000000227 grinding Methods 0.000 description 2
- 230000000737 periodic effect Effects 0.000 description 2
- 238000005498 polishing Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000005507 spraying Methods 0.000 description 2
- 238000003466 welding Methods 0.000 description 2
- 241001083548 Anemone Species 0.000 description 1
- 101100264195 Caenorhabditis elegans app-1 gene Proteins 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000011022 operating instruction Methods 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000003825 pressing Methods 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/0081—Programme-controlled manipulators with master teach-in means
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
无线控制的六自由度机械臂示教***，PC端APP安装在用户的Windows电脑上，PC端APP包括依次连接的交互界面模块、APP主控模块、轨迹优化模块、控制文件生成模块，控制文件生成模块通过文件传输协议FTP将控制代码文件下载到一个或多个工作机械臂中，工作机械臂完成优化后的示教工作；数据采集器连接MCU主控模块，MCU主控模块通过WiFi无线连接PC端，数据采集器与示教机械臂关节的六个绝对式编码器通过RS485电路进行双向通信连接，六个绝对式编码器分别对应机械臂的六个自由度；数据采集器输出数据给MCU主控模块，MCU主控模块输出数据给PC端，PC端输出操作指令给MCU主控模块，MCU主控模块输出操作指令给数据采集器；数据采集器和示教机械臂、MCU主控模块都连接外接电源模块。
Description
技术领域
本发明涉及一种机械臂示教***，尤其是一种无线控制的六自由度机械臂示教***。
背景技术
随着各行业对工业机器人的广泛使用，他们期望机器人可以在最短时间，以更简单的方式，完成更加复杂的任务，尤其是喷涂、焊接、抛光打磨等轨迹规划复杂的行业。目前工业机器人的使用，距离实现完全智能化还有一段距离，大多数任务的完成主要依靠示教——再现形式实现，那么如何实现示教依然是机器人执行加工任务前的一项重要工作。示教——再现中的“示教”顾名思义为示范教导的意思，即在机器人正常加工之前，操作者利用某种交互方式示范执行加工任务时所需动作，并且将示教过程记录在存储器中。“再现”即对示教的过程重现，利用存储器中的数据，经过一定的转换，并经由机器人的驱动器、控制器实现机器人对工件的加工动作。示教-再现的工作，一般包括示教轨迹点的位置、姿态，运动过程中的速度、加速度。
目前工业机器人多数采用示教盒示教，对于复杂曲面的加工，示教任务非常繁重，任务的苛刻所带来的问题使示教盒形式的示教过程以及前期的轨迹规划需要耗费大量的时间、资源。示教复杂轨迹任务需获取大量点位信息。示教点位姿的获取，依靠按示教盒的指令键来移动机器人，人眼查看机器人末端位姿与工件的位姿关系来获得，每个点的获得都需要较长的调整时间，示教点数的增多使示教周期更长，对于轨迹运动的规划也变得复杂，导致示教工作繁重，大大降低了机器人的加工效率。用示教盒键盘对于机器人进行示教，需要操作者具备专业的机器人操作技能和编程经验，而且现在不同厂家制造的示教盒规格是不同的，操作培训周期较长。
因此需要用直接示教来延续技术工人加工工件的经验，记录其加工过程的轨迹，于此同时，也免去了前期复杂的轨迹规划工作。但是，工件加工技术的执行人员大多是未接受过编程技术、操作机器人技术知识教育，并且再学习能力较低的人群。那么，如何延续技术工人的加工经验与控制机器人运动完成示教以及实现编程处理等工作，两者成为矛盾。若把技术经验的延续和示教操作的实施由两类人合作完成，则增加了机器人的使用成本，长此以往，也不可取。总之，局限于编程、经验积累同时进行的示教模式，是无法维持高效生产的，故亟需一种快速示教并且兼容技术工人示教经验的示教***来实现工业机器人的示教作业。
而传统的机械臂示教***没有使用WiFi无线通信，因此需要复杂的接线工作，并且连接线长度也限制了示教工作的空间。传统的机械臂示教***采集频率不够高，且没有引入轨迹规划算法，最终工作机械臂执行的示教轨迹不平滑，机械臂末端工作速度变化不均匀，不能达到预期的工作要求。大部分机械臂示教***都没有良好人机交互性能的PC端APP，无法实时反馈采集数据，修改采集频率和轴地址不方便，因此不能让用户有良好的操作体验，无法达到较好的示教效果。
目前，针对工业机械臂示教***的设计主要集中在机械臂结构、有线通信和示教控制器等方面，针对无线通信、APP控制和示教轨迹优化方面的研究较少。姜声华、丁文力、李晗提出了一种工业机器人示教***(姜声华，丁文力，李晗.一种工业机器人示教***：中国，205852817[P].2017-01-04)，给出了工业机械臂的一种机械结构设计及其设计原理，但没有示教控制工具，用户交互操作极其不便；朱茂娟、王林冰、曾奇等提出一种机器人及其示教通信***(朱茂娟，王林冰，曾奇等.机器人及其示教通信***:中国，205620710[P].2016-01-04),实现了示教采集器与控制器之间的数据通信，但是通信方式是有线的，具有空间局限性且缺少保密性；陈恳、任书楠、王国磊等提出了一种工业机器人示教***(陈恳，任书楠，王国磊等.工业机器人示教***:中国，104700705[P].2015-06-10)，该***能够完成示教采集、数据通信和示教控制，但是没有对示教轨迹进行优化，不能满足高精度的工业生产要求；夏泽洋、邓豪、翁少葵等提出一种机器人示教***及其控制方法(夏泽洋，邓豪，翁少葵等.机器人示教***及其控制方法:中国，105058396[P].2015-11-18)，详细介绍了机器人的示教过程，但是没有设计良好交互性能的APP，用户操作繁琐不便。
发明内容
本发明要克服现有技术的上述缺点，提出一种六自由度机械臂示教***。
首先该***具有人性化的操作界面，让不同的用户都能够方便地进行示教过程的各种操作和设置，并显示各种数据和参数供用户直观跟踪整个示教过程；其次该***使用了WiFi无线通信方式，实现了无线控制，建立连接方便，数据的传输高效稳定，没有了数据线的限制，工作空间得以扩展；再次该***引入了笛卡尔轨迹规划算法，使得最终的示教轨迹更加平滑，末端执行器速度变化更加均匀；最后该***可以生成控制工作机械臂执行示教轨迹的代码文件，一次示教即可供多台工作机械臂无限次执行该示教作业。
本发明为解决现有技术问题所采用的技术方案是：
一种无线控制的六自由度机械臂示教***，其特征在于：PC端APP安装在用户的Windows电脑上，数据采集器与PC端APP通过WiFi无线连接，数据采集器与示教机械臂通过RS485电路连接。
所述的PC端APP包括依次连接的交互界面模块、APP主控模块、轨迹优化模块、控制文件生成模块，控制文件生成模块通过文件传输协议FTP将控制代码文件下载到一个或多个工作机械臂中，工作机械臂完成优化后的示教工作。
数据采集器连接MCU主控模块，MCU主控模块通过WiFi无线连接PC端，数据采集器与示教机械臂关节的六个绝对式编码器通过RS485电路进行双向通信连接，六个绝对式编码器分别对应机械臂的六个自由度。数据采集器输出数据给MCU主控模块，MCU主控模块输出数据给PC端，PC端输出操作指令给MCU主控模块，MCU主控模块输出操作指令给数据采集器。数据采集器和示教机械臂、MCU主控模块都连接外接电源模块。
各模块的具体构成是：
供电模块，其输入为+24V交流电压，电压输入后首先经过保护电路，保护电路可以防止电流回流对电源产生影响和破坏。之后+24V交流电压进入稳压滤波电路，再将稳压滤波后的+24V交流电压作为六自由度示教机械臂关节中的绝对式编码器的供电电源。然后+24V交流电压经过降压电路、整流电路和稳压滤波电路分别转变为+5.4V直流电压和+3.3V直流电压，+5.4V直流电压用于给部分数据采集电路供电，+3.3V直流电压用于给MCU和部分数据采集电路供电。
数据采集模块，通过RS485电路与六自由度示教机械臂关节中的绝对式编码器连接，通过串口与MCU的主芯片连接。当数据采集模块收到MCU主控模块发来的控制指令后，通过RS485电路对六个绝对式编码器发送广播指令或查询指令代码，编码器根据相应指令返回某个关节的实时角度数据或者六个关节的实时同步运动角度数据，采集模块再将角度数据通过串口发送给MCU主控模块。
MCU主控模块，由主芯片和其它辅助电路组成，负责通信功能的配置以及指令和数据的处理。MCU主控模块控制数据采集器通过WiFi通信模块与PC端建立连接；将接收到的一个或多个编码器的角度信息进行整合处理，并通过WiFi通信模块发送给PC端APP；通过WiFi通信模块接收来自PC端APP的各种指令，做出相应处理，包括向PC端APP发送连接响应、采集准备就绪信号、停止采集信号、采集频率设置成功或失败响应、采集频率和采集周期信息、修改各轴地址成功或失败响应、各轴地址信息等，和向数据采集模块发送开始采集指令、停止采集指令等，以及修改各轴地址、修改采集频率等。
WiFi通信模块，由下位机数据采集器的WiFi模块开启WiFi热点，PC端通过无线网络连接接入热点，采用UDP通信协议模式建立无线连接，并通过WiFi透传模式快速传输数据。通过WiFi通信模块，PC端APP可以向数据采集器发送各种控制指令，数据采集器可以向PC端APP发送角度和参数信息。其数据传输方式采用UDP通信协议，数据采集器与PC端APP互相锁定了对方的IP地址和端口号，为一对一的通信，第三方无法再接入连接，保证了数据传输的保密性；对指令、参数、角度数据都做了校验，保证了数据传输的正确性；采用WiFi透传模式，数据的无线收发几乎无延迟，保证了数据传输的高效性；所用的WiFi模块传输距离远穿墙能力强，并且一旦锁定了双方的IP地址和端口号后，无论是数据采集器或是PC端单方断电掉线重连后都能直接继续进行数据传输，保证了数据传输的稳定性。
交互界面模块，用于用户对整个示教***进行设置和操作，以及显示示教***的各种相关参数和数据；通过PC端APP的交互界面，用户可以设置要连接的WiFi热点的IP地址和网络端口号、示教机械臂各个关节内的编码器的正反向标定、角度数据的采集频率、示教机械臂各个轴的轴地址、所使用的机械臂型号以及控制代码文件的下载路径等，并将这些设置参数传给APP主控模块处理；用户还能在PC端APP的交互界面进行WiFi通信的连接和断开、角度数据采集的开始和停止、生成控制代码文件、查询数据采集器就绪状态、查询角度数据采集频率和周期以及查询示教机械臂各轴的轴地址等操作，这些操作指令会传给APP主控模块进行处理；交互界面还会对所有能设置的参数和采集得到的角度数据进行显示，这些参数和数据从APP主控模块得到；
APP主控模块，是PC端APP的通信中枢和控制中枢，对各种数据、参数和指令进行收发和处理。APP主控模块可以控制PC端APP通过WiFi通信模块与数据采集器建立通信连接。可以从交互界面模块接收到参数设置指令和操作指令，进行处理后生成相应通信协议的控制指令，并通过WiFi通信模块发送给MCU主控模块和通过网络通信控制仿真模块。能够通过WiFi通信模块接收来自数据采集器的角度数据和参数信息，将实时角度数据和参数信息传给交互界面显示，并在示教结束后(即停止数据采集后)生成示教机械臂的各个关节的运动角度数据包，将角度数据包传给轨迹优化模块进行优化和通过网络通信发给仿真模块进行仿真。
轨迹优化模块，接收来自APP主控模块的示教机械臂的各个关节的运动角度数据包，结合当前所使用的机械臂型号的结构参数，根据D-H参数建模方法建立机械臂每个杆件在各个关节处的笛卡尔坐标系，用正运动学公式求解末端执行器相对固定参考坐标系的位置和姿态，求出多组解后即得到了示教轨迹。再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化。最后利用逆运动学公式结合当前所使用的机械臂型号的结构参数对优化后的轨迹求其关节空间中各关节的关节变量，即可获得到每个采集时刻优化后的运动角度数据包。可将角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。所述的正逆运动学求解和笛卡尔轨迹规划算法如下：
1)选择示教机械臂六个关节的D-H参数(即关节1≤i≤6的连杆长度ai、连杆转角αi、连杆偏距di和关节角θi)作为输入量，选择工作机械臂各个关节的关节角θi作为输出量。
2)首先对示教机械臂构建其D-H参数模型，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，用以下四个参数来描述杆件：
(1)连杆长度ai：关节轴i和关节轴i+1之间公垂线的长度；
(2)连杆转角αi：作一个与两关节轴之间的公垂线垂直的平面，将关节轴i和关节轴i+1投影到该平面，在平面内轴i按右手法则绕ai转向轴i+1，其转角即为连杆转角；
(3)连杆偏距di：公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i+1的交点的有向距离长度；
(4)关节角θi：ai-1的延长线与ai之间绕关节轴i旋转所形成的夹角，即采集得到的示教机械臂关节角度。
得到示教机械臂六个关节的D-H连杆参数表1所示：
表1示教机械臂D-H连杆参数表
| 关节i | 连杆长度a<sub>i</sub> | 连杆转角α<sub>i</sub> | 连杆偏距d<sub>i</sub> | 关节角θ<sub>i</sub> |
| 1 | a<sub>1</sub> | α<sub>1</sub> | d<sub>1</sub> | θ<sub>1</sub> |
| 2 | a<sub>2</sub> | α<sub>2</sub> | d<sub>2</sub> | θ<sub>2</sub> |
| 3 | a<sub>3</sub> | α<sub>3</sub> | d<sub>3</sub> | θ<sub>3</sub> |
| 4 | a<sub>4</sub> | α<sub>4</sub> | d<sub>4</sub> | θ<sub>4</sub> |
| 5 | a<sub>5</sub> | α<sub>5</sub> | d<sub>5</sub> | θ<sub>5</sub> |
| 6 | a<sub>6</sub> | α<sub>6</sub> | d<sub>6</sub> | θ<sub>6</sub> |
3)以基座坐标系作为T0坐标系，设
表示i关节坐标系相对i-1关节坐标系的位置和姿态，则：
其中，sθi＝sinθi，cθi＝cosθi，sαi＝sinαi，cαi＝cosαi。
4)对于六自由度机械臂，获得机械臂末端执行器位姿相对固定参考坐标系的变换矩阵(即机械臂末端位置点坐标的变换矩阵)。
5)求得每个采集时刻的示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹。
6)对于圆弧位置插补，取任务空间中p1(x1,y1,z1)、p2(x2,y2,z2)和p3(x3,y3,z3)三点，这三点组成的平面方程为：
Ax+By+Cz＝1
由下式可解得平面方程参数A、B和C的值：
7)求出p1p2和p2p3两条线段的中点p12(x12,y12,z12)，p23(x23,y23,z23)，设圆心坐标为po(xo,yo,zo)，由平面几何关系获得圆心满足下列方程：
求解获得圆心坐标，以圆心为球心，r为半径的球参数方程为：
其中，φ表示球面上任意一点与球心的连线与其在X-Y平面的投影的夹角，
α表示球面上任意一点与球心的连线在X-Y平面的投影与X轴的夹角；
8)将上式代入平面方程式Ax+By+Cz＝1中，得到：
A(rcosφcosα+xo)+B(rcosφsinα+yo)+C(rsinα+zo)＝1
因为圆心po(xo,yo,zo)也在平面上，所以：
Arcosφcosα+Brcosφsinα+Crsinα＝0
从上式可以解得：
从而可获得空间圆的参数方程为：
9)取矢量
为X1轴向量，平面法向量为Z1轴向量，建立T1坐标系，由于姿态变换矩阵为单位正交矩阵，则有Y1＝X1×Z1，进而可以求得Y1轴向量，其姿态变换矩阵R和平移向量T为：
其中，xp，yp，zp分别表示T1坐标系中的Y1轴向量在参考坐标系的X、Y、Z三个轴方向的投影分量的值；
设
则有：
姿态变换矩阵R和平移向量T可以得到坐标变换矩阵为：
10)取末端轨迹在T1的位置坐标为p'(x',y',z')，通过姿态变换矩阵和平移向量可以求得末端轨迹在T0中的位置p(x,y,z)，即：
[x y z 1]T＝Q·[x' y' z' 1]T
在T1中，独立参数α圆弧轨迹为：
其中，α∈(0,αmax)，αmax为圆弧的弧角值。
对α进行运动规划，得到轨迹p'(x',y',z')后，通过变换得到(x,y,z)，即为圆弧插补的规划轨迹。
11)在优化后的轨迹上选择需要的点，并列出每个插补点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵：
12)对每个优化后的插补点p(x,y,z)进行逆运动学求解，其中rij取与插补点最近的末端坐标点的变换矩阵中的值，则可按顺序求解得到：
θ23＝A tan 2[(-a3-a2c3)pz-(c1px+s1py)(d4-a2s3)
(a2s3-d4)pz-(a3+a2c3)(c1px+s1py)]
θ2＝θ23-θ3
θ4＝A tan 2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23)
s5＝-[r13(c1c23c4+s1s4)+r23(s1c23c4-c1s4)-r33(s23c4)]
c5＝r13(-c1s23)+r23(-s1s23)+r33(-c23)
θ5＝A tan 2(s5,c5)
s6＝-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31(s23s4)
c6＝r11[(c1c23c4+s1s4)c5-c1s23s5]+r21[(s1c23c4-c1s4)c5-s1s23s5]-r31(s23c4c5+c23s5)
θ6＝A tan 2(s6,c6)
其中，A tan 2为反正切函数，si＝sinθi，ci＝cosθi。
13)由于各个关节角是用反三角函数计算出来的，丢失了函数的周期信息，因此在每个关节角度求出后，需要在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值。求出每个优化后的末端位姿对应的各个关节角，整理成角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。
仿真模块，使用RoboDK仿真软件，模拟工作机械臂的工作轨迹执行情况。PC端APP可以将采集得到的示教过程中示教机械臂的各个关节的运动角度数据包直接通过网络通信传输给RoboDK，RoboDK软件中的虚拟工作机械臂会执行相应的示教运动轨迹。能够先对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包在轨迹优化模块中进行优化后，再通过网络通信将优化后的运动角度数据包传输给RoboDK，RoboDK软件中的虚拟工作机械臂就会执行相应的优化后的示教运动轨迹。当确定RoboDK软件中执行的优化后的示教运动轨迹是满足工作需求的后，再让控制文件生成模块生成控制实际工作机械臂完成工作所需的控制代码文件。
控制文件生成模块，生成控制实际工作机械臂完成工作所需的控制代码文件。其输入是轨迹优化模块对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包进行优化后的运动角度数据包。若仿真模块肯定了优化方案的可行性，则控制文件生成模块在收到优化后的运动角度数据包后，就将其转化为可以控制实际工作机械臂执行工作的控制代码文件。再通过FTP(文件传输协议)将控制代码文件下载到一个或多个工作机械臂中，由工作机械臂完成优化后的示教工作。
用户使用PC端APP无线控制数据采集器。对六自由度机械臂进行直接示教，数据采集器同步采集安置在示教臂六个关节中的绝对式编码器的角度信息，并将角度信息发送给PC端APP。PC端APP无线接收来自数据采集器的角度信息，结合正逆运动学公式和笛卡尔轨迹规划算法对示教轨迹进行优化，并生成控制代码文件用以控制工作臂完成优化后的示教作业。用户在PC端APP的交互界面可以查看各种参数和数据，并且可以进行各种设置和操作。
本发明的优点和积极效果是：
首先该***具有人性化的操作界面，让不同的用户都能够方便地进行示教过程的各种操作和设置，并显示各种数据和参数供用户直观跟踪整个示教过程；其次该***使用了WiFi无线通信方式，实现了无线控制，建立连接方便，数据的传输高效稳定，没有了数据线的限制，工作空间得以扩展；再次该***引入了笛卡尔轨迹规划算法，使得最终的示教轨迹更加平滑，末端执行器速度变化更加均匀；最后该***可以生成控制工作机械臂执行示教轨迹的代码文件，一次示教即可供多台工作机械臂无限次执行该示教轨迹。
本发明采用了PC端APP来控制角度数据采集器，相较于传统的用户终端控制器，PC端APP可安装在用户的电脑上，节省了用户终端控制器带来的器械制作成本；无需担心用户终端控制器带来的机械故障；也不用为用户终端控制器花费过多的电池开销；APP可以在线升级，方便开发者对其进行问题修复和产品升级；控制器以APP的形式出现，也顺应了现今电器智能化的趋势，更容易为用户所接受。
本发明比较其它类别的机械臂示教***，成本低廉，结构简单，搭建方便，便于操作，可以满足各种不同类型的用户需求。本发明相较于传统的机械臂示教***通过用户终端控制器进行操作，使用PC端APP并结合笛卡尔轨迹规划算法优化示教轨迹，采用信息化的界面显示，便捷化的无线操控，简单化的按键控制，并可以一键生成控制代码文件，让用户获得相对数据化的参数信息、便易化的操作体验。现在的示教盒示教操作过于繁琐，培养代价较大，直接示教的轨迹又不够光滑，速度变化不均，一种无线控制的六自由度机械臂示教***是进行喷涂、焊接、抛光、打磨等复杂工作的行业的优良选择，专业化、低成本、便安置、易操作、通用性强等特点方便了所有厂家和操作者。
附图说明
图1为本发明的平台组成示意图。
图2为本发明的控制原理框图。
图3a为本发明的PC端APP的WiFi连接操作界面图。
图3b为本发明的PC端APP的机械臂型号选择界面图。
图3c为本发明的PC端APP的关节角度正反标定界面图。
图3d为本发明的PC端APP的轴地址设置界面图。
图3e为本发明的PC端APP的采集频率设置界面图。
图3f为本发明的PC端APP的采集及控制代码生成界面图。
图3g为本发明的PC端APP的文件下载设置界面图。
具体实施方式
以下结合附图对本发明做进一步详述。
一种无线控制的六自由度机械臂示教***，如图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提供所需的工作电压。
结合图2和图3，本发明专利的具体实施方式如下：
PC端APP安装在用户的电脑上，数据采集器连接电源模块。按照对图1的描述搭好机械臂示教平台后，用户即可进行六自由度机械臂的示教操作。
用户将供电模块外接+24V电源，供电模块采用了LM2576S系列降压芯片和B0505S系列电源芯片，供电模块对外接电压进行降压稳压之后分别给MCU模块、RS485电路和编码器提供+3.3V、+5.4V和+24V电源。
MCU主控模块采用了STM32F103系列主芯片，需要+3.3V外部供电，由供电模块提供。MCU主控模块可以控制WiFi通信模块开启WiFi热点，并建立数据采集器与PC端APP的WiFi通信连接；MCU主控模块接收到APP主控模块发来的读取编码器轴地址指令后，可以读取当前各关节中的编码器的轴地址，并通过WiFi通信模块向APP主控模块反馈当前各个编码器的轴地址；接收到APP主控模块发来的修改编码器轴地址指令后，完成该编码器轴地址的修改，并向APP主控模块反馈修改成功响应；接收到APP主控模块发来的修改采集频率指令后，完成采集频率和周期的修改，并向APP主控模块反馈修改成功响应；接收到APP主控模块发来的查询采集频率和周期指令后，向APP主控模块反馈当前采集频率和周期；接收到APP主控模块发来的开始采集指令后，控制采集模块进行示教角度数据采集，并向APP主控模块发送角度数据；接收到APP主控模块发来的停止采集指令后，向APP主控模块反馈停止采集响应。
WiFi模块采用了ESP8266 WiFi模块，MCU模块上电后开始自动设置ESP8266 WiFi模块的WiFi热点的账号和密码并开启WiFi热点，设置数据传输协议为UDP协议，设置通信方式为透传模式，设置数据采集器的本机IP地址和端口号，并锁定PC端APP的IP地址和端口号；PC端使用无线网络连接功能接入该WiFi热点。
在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主控模块发送停止采集指令，并将保存的角度数据包传给轨迹优化模块。
数据采集模块电路主要由串口和RS485电路组成，MCU主控模块收到开始采集指令后控制数据采集模块执行采集功能，数据采集模块根据采集周期定时按关节顺序向各个编码器发送广播指令和查询指令；采集期间由操作人员手持示教机械臂末端进行示教操作，数据采集模块收到六个编码器的角度数据传给MCU主控模块整合后，通过WiFi通信模块向APP主控模块发送实时角度数据，APP主控模块将收到的角度数据发送给交互界面模块显示并保存成角度数据包；
在采集过程中，采集得到的角度数据会直接通过网络通信同步发到RoboDK仿真软件，因此RoboDK软件中的虚拟工作机械臂会跟随操作人员同步执行示教轨迹的仿真，当采集结束时RoboDK仿真软件的虚拟工作机械臂停止执行示教轨迹；轨迹优化之后，在采集及控制代码生成界面点击生成RoboDK程序按钮，便可将采集得到的角度数据包发给RoboDK仿真软件，RoboDK仿真软件根据收到的角度数据包仿真工作机械臂执行优化后的示教轨迹的过程并将轨迹以曲线的形式具象化，用户通过观察轨迹曲线的情况判断该优化后的示教轨迹是否可以用于实际工作机械臂的生产工作中。
轨迹优化模块收到示教角度数据包后，结合当前所使用的机械臂型号的结构参数得到示教轨迹，再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化，最后利用逆运动学公式结合当前所使用的机械臂型号的结构参数得到每个采集时刻优化后的运动角度数据包，具体步骤如下：
1)选择示教机械臂六个关节的D-H参数(即关节1≤i≤6的连杆长度ai、连杆转角αi、连杆偏距di和关节角θi)作为输入量，选择工作机械臂各个关节的关节角θi作为输出量；
2)首先对示教机械臂构建其D-H参数模型，以PUMA560机器人为例，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，取第n个采集时刻示教机械臂六个关节的D-H连杆参数表2所示：
表2示教机械臂D-H连杆参数表
| 关节i | 连杆长度a<sub>i</sub> | 连杆转角α<sub>i</sub> | 连杆偏距d<sub>i</sub> | 关节角θ<sub>i</sub> |
| 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° |
由i关节坐标系相对i-1关节坐标系的位置和姿态
的表达式
和
其中sθi＝sinθi，cθi＝cosθi，sαi＝sinαi，cαi＝cosαi，可以求得示教机械臂末端执行器在第n个采集时刻的位姿相对固定参考坐标系的变换矩阵为：
3)可以得到示教机械臂末端执行器在第n个采集时刻相对固定参考坐标系的位置坐标为pn(6.178,10.495,5.5)；
同理求得每个采集时刻的示教臂的末端执行器相对固定参考坐标系的位置坐标后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹；
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)，根据三点组成的平面方程
获得：
A＝0,B＝0,C＝4
由p1p2和p2p3两条线段的中点p12(x12,y12,z12)，p23(x23,y23,z23)，可以得到平面
内同时经过p1、p2和p3的唯一圆的圆心po(xo,yo,zo)和半径r：
5)以圆心为原点，参照圆所在的平面，建立新的坐标系O'X'Y'Z'(即T1)：坐标原点为圆弧圆心po(xo,yo,zo)，所在平面的法线为Z'轴方向，O'p1的方向为X'轴方向，在通过右手定则确定Y'轴方向。可知，新坐标到基座标系的齐次变换矩阵为：
其中，
6)在新坐标系O'X'Y'Z'中，圆弧轨迹为：
其中，α∈(0,αmax)，αmax为圆弧的弧角值，得到轨迹p'(x',y',z')后，通过变换[x yz 1]T＝Q·[x' y' z'1]T得到(x,y,z)，即为圆弧优化后的规划轨迹；
7)包括圆弧的起点和终点，均匀地取四个点作为新的机械臂末端坐标，即(2,0,0)，(2.366,1.366,0)，(1.366,2.366)，(0,2,0)，则机械臂在这四点间的运动轨迹既平滑又匀速；
同理对示教轨迹的坐标点中的每三个点为一组进行优化，则可以得到优化后的示教轨迹的离散坐标点；
8)对每个优化后的坐标点p(x,y,z)进行逆运动学求解，列出每个点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵
其中rij取与插补点最近的末端坐标点的变换矩阵中的值，以PUMA560机器人为例，结合工作机械臂的D-H参数并使用下面的公式进行求解：
θ23＝A tan 2[(-a3-a2c3)pz-(c1px+s1py)(d4-a2s3)
(a2s3-d4)pz-(a3+a2c3)(c1px+s1py)]
θ2＝θ23-θ3
θ4＝A tan 2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23)
s5＝-[r13(c1c23c4+s1s4)+r23(s1c23c4-c1s4)-r33(s23c4)]
c5＝r13(-c1s23)+r23(-s1s23)+r33(-c23)
θ5＝A tan 2(s5,c5)
s6＝-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31(s23s4)
c6＝r11[(c1c23c4+s1s4)c5-c1s23s5]+r21[(s1c23c4-c1s4)c5-s1s23s5]-r31(s23c4c5+c23s5)
θ6＝A tan 2(s6,c6)
表3工作机械臂D-H连杆参数表
| 关节i | 连杆长度a<sub>i</sub> | 连杆转角α<sub>i</sub> | 连杆偏距d<sub>i</sub> | 关节角θ<sub>i</sub> |
| 1 | 0 | 0 | 0 | θ<sub>1</sub> |
| 2 | -90° | 0 | 0 | θ<sub>2</sub> |
| 3 | 0 | 3 | 6 | θ<sub>3</sub> |
| 4 | -90° | 4 | 8 | θ<sub>4</sub> |
| 5 | 90° | 0 | 0 | θ<sub>5</sub> |
| 6 | -90° | 0 | 0 | θ<sub>6</sub> |
由于各个关节角是用反三角函数计算出来的，丢失了函数的周期信息，因此在每个关节角度求出后，需要在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值，令优化得到的插补点的坐标为p(6.1782,10.4952,5.5)，与插补点最近的末端坐标点的变换矩阵为
以及工作机械臂的D-H参数如表3所示，则可以求得p点所对应的工作机械臂的各个关节的最优角度(弧度制)为θ1＝0.5236,θ2＝-0.5236,θ3＝-1.0472，θ4＝-1.5708，θ5＝1.0472，θ6＝1.5708；
9)按8)中的步骤同理求出每个优化后的末端坐标和姿态对应的工作臂的各个关节角，整理成角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成；
当用户判断优化后的示教轨迹可以用于实际工作机械臂的生产工作中后，在采集及控制代码生成界面点击生成机器人程序，即可自动生成控制对应型号的工作机械臂的控制代码文件；打开PC端APP的文件下载设置界面，可以设置生成的控制对应型号的工作机械臂的代码文件的下载路径，控制代码文件将在该路径中永久保存，之后用户可以将该控制代码文件传输给任意对应型号的机械臂去完成该示教工作内容。
以上是整个***的控制情况，PC端APP的交互界面模块和WiFi模块的存在，让用户的操作变得简单方便，轨迹优化模式让示教轨迹更加平滑，示教运动速度变化更加均匀，控制文件生成模块让示教轨迹的应用时间更加长久、应用对象数量更加庞大。
要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的类似的其它实施方式，同样属于本发明的保护范围。
Claims (3)
1.一种无线控制的六自由度机械臂示教***，其特征在于：PC端APP安装在用户的Windows电脑上，数据采集器与PC端APP通过WiFi无线连接，数据采集器还与示教机械臂通过RS485电路连接；
所述的PC端APP包括依次连接的交互界面模块、APP主控模块、轨迹优化模块、控制文件生成模块，控制文件生成模块通过文件传输协议FTP将控制代码文件下载到一个或多个工作机械臂中，工作机械臂完成优化后的示教工作；
数据采集器连接MCU主控模块，MCU主控模块通过WiFi无线连接PC端，数据采集器与示教机械臂关节的六个绝对式编码器通过RS485电路进行双向通信连接，六个绝对式编码器分别对应机械臂的六个自由度；数据采集器输出数据给MCU主控模块，MCU主控模块输出数据给PC端，PC端输出操作指令给MCU主控模块，MCU主控模块输出操作指令给数据采集器；数据采集器和示教机械臂、MCU主控模块都连接外接电源模块；
各模块的具体构成是：
供电模块，其输入为+24V交流电压，电压输入后首先经过保护电路，保护电路可以防止电流回流对电源产生影响和破坏；之后+24V交流电压进入稳压滤波电路，再将稳压滤波后的+24V交流电压作为六自由度示教机械臂关节中的绝对式编码器的供电电源；然后+24V交流电压经过降压电路、整流电路和稳压滤波电路分别转变为+5.4V直流电压和+3.3V直流电压，+5.4V直流电压用于给部分数据采集电路供电，+3.3V直流电压用于给MCU和部分数据采集电路供电；
数据采集模块，通过RS485电路与六自由度示教机械臂关节中的绝对式编码器连接，通过串口与MCU的主芯片连接；当数据采集模块收到MCU主控模块发来的控制指令后，通过RS485电路对六个绝对式编码器发送广播指令或查询指令代码，编码器根据相应指令返回某个关节的实时角度数据或者六个关节的实时同步运动角度数据，采集模块再将角度数据通过串口发送给MCU主控模块；
MCU主控模块，由主芯片和其它辅助电路组成，负责通信功能的配置以及指令和数据的处理；MCU主控模块控制数据采集器通过WiFi通信模块与PC端建立连接；将接收到的一个或多个编码器的角度信息进行整合处理，并通过WiFi通信模块发送给PC端APP；通过WiFi通信模块接收来自PC端APP的各种指令，做出相应处理，包括向PC端APP发送连接响应、采集准备就绪信号、停止采集信号、采集频率设置成功或失败响应、采集频率和采集周期信息、修改各轴地址成功或失败响应、各轴地址信息，和向数据采集模块发送开始采集指令、停止采集指令，以及修改各轴地址、修改采集频率；
WiFi通信模块，由下位机数据采集器的WiFi模块开启WiFi热点，PC端通过无线网络连接接入热点，采用UDP通信协议模式建立无线连接，并通过WiFi透传模式快速传输数据；通过WiFi通信模块，PC端APP可以向数据采集器发送各种控制指令，数据采集器可以向PC端APP发送角度和参数信息；
交互界面模块，用于用户对整个示教***进行设置和操作，以及显示示教***的各种相关参数和数据；通过PC端APP的交互界面，用户可以设置要连接的WiFi热点的IP地址和网络端口号、示教机械臂各个关节内的编码器的正反向标定、角度数据的采集频率、示教机械臂各个轴的轴地址、所使用的机械臂型号以及控制代码文件的下载路径，并将这些设置参数传给APP主控模块处理；用户还能在PC端APP的交互界面进行WiFi通信的连接和断开、角度数据采集的开始和停止、生成控制代码文件、查询数据采集器就绪状态、查询角度数据采集频率和周期以及查询示教机械臂各轴的轴地址的操作，这些操作指令会传给APP主控模块进行处理；交互界面还会对所有能设置的参数和采集得到的角度数据进行显示，这些参数和数据从APP主控模块得到；
APP主控模块，是PC端APP的通信中枢和控制中枢，对各种数据、参数和指令进行收发和处理；APP主控模块可以控制PC端APP通过WiFi通信模块与数据采集器建立通信连接；可以从交互界面模块接收到参数设置指令和操作指令，进行处理后生成相应通信协议的控制指令，并通过WiFi通信模块发送给MCU主控模块和通过网络通信控制仿真模块；能够通过WiFi通信模块接收来自数据采集器的角度数据和参数信息，将实时角度数据和参数信息传给交互界面显示，并在示教结束后生成示教机械臂的各个关节的运动角度数据包，将角度数据包传给轨迹优化模块进行优化和通过网络通信发给仿真模块进行仿真；
轨迹优化模块，接收来自APP主控模块的示教机械臂的各个关节的运动角度数据包，结合当前所使用的机械臂型号的结构参数，根据D-H参数建模方法建立机械臂每个杆件在各个关节处的笛卡尔坐标系，利用正运动学公式求解末端执行器相对固定参考坐标系的位置和姿态，求出多组解后即得到了示教轨迹；再对示教轨迹使用笛卡尔轨迹规划算法进行轨迹优化；最后利用逆运动学公式结合当前所使用的机械臂型号的结构参数对优化后的轨迹求其关节空间中各关节的关节变量，即可获得到每个采集时刻优化后的运动角度数据包；可将角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成；
仿真模块，使用RoboDK仿真软件，模拟工作机械臂的工作轨迹执行情况；PC端APP可以将采集得到的示教过程中示教机械臂的各个关节的运动角度数据包直接通过网络通信传输给RoboDK，RoboDK软件中的虚拟工作机械臂会执行相应的示教运动轨迹；能够对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包在轨迹优化模块中进行优化后，再通过网络通信将优化后的运动角度数据包传输给RoboDK，RoboDK软件中的虚拟工作机械臂就会执行相应的优化后的示教运动轨迹；当确定RoboDK软件中执行的优化后的示教运动轨迹是满足工作需求的后，再让控制文件生成模块生成控制实际工作机械臂完成工作所需的控制代码文件；
控制文件生成模块，生成控制实际工作机械臂完成工作所需的控制代码文件；其输入是轨迹优化模块对采集得到的示教过程中示教机械臂的各个关节的运动角度数据包进行优化后的运动角度数据包；若仿真模块肯定了优化方案的可行性，则控制文件生成模块在收到优化后的运动角度数据包后，就将其转化为可以控制实际工作机械臂执行工作的控制代码文件；再通过FTP将控制代码文件下载到一个或多个工作机械臂中，由工作机械臂完成优化后的示教工作。
2.根据权利要求1所述的无线控制的六自由度机械臂示教***，其特征在于：所述的轨迹优化模块从PC端APP主控模块接收到示教角度数据包后，结合了正逆运动学求解和笛卡尔轨迹规划算法来优化示教轨迹，再将优化后的角度数据包发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成，所述的正逆运动学求解和笛卡尔轨迹规划算法如下：
2.1)选择示教机械臂六个关节的D-H参数作为输入量，选择工作机械臂各个关节的关节角θi作为输出量，D-H参数包括关节1≤i≤6的连杆长度ai、连杆转角αi、连杆偏距di和关节角θi；
2.2)首先对示教机械臂构建其D-H参数模型，由于各杆件相对参考坐标系有转动和平移两个动作，因此对每个杆件沿关节轴建立一个关节坐标系，用以下四个参数来描述杆件：
(1)连杆长度ai：关节轴i和关节轴i+1之间公垂线的长度；
(2)连杆转角αi：作一个与两关节轴之间的公垂线垂直的平面，将关节轴i和关节轴i+1投影到该平面，在平面内轴i按右手法则绕ai转向轴i+1，其转角即为连杆转角；
(3)连杆偏距di：公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i+1的交点的有向距离长度；
(4)关节角θi：ai-1的延长线与ai之间绕关节轴i旋转所形成的夹角，即采集得到的示教机械臂关节角度；
2.3)以基座坐标系作为T0坐标系，设
表示i关节坐标系相对i-1关节坐标系的位置和姿态，则：
其中，sθi＝sinθi，cθi＝cosθi，sαi＝sinαi，cαi＝cosαi；
2.4)对于六自由度机械臂，获得机械臂末端执行器位姿相对固定参考坐标系的变换矩阵，即机械臂末端位置点坐标的变换矩阵：
2.5)求得每个采集时刻的示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵后，即可获得到由每个采集时刻示教臂的末端执行器位姿相对固定参考坐标系的坐标点组成的离散示教轨迹；
2.6)使用笛卡尔圆弧插补法，先取示教轨迹中的p1(x1,y1,z1)、p2(x2,y2,z2)和p3(x3,y3,z3)三点，这三点组成的平面方程为Ax+By+Cz＝1，则可由下式解得平面方程参数A、B和C的值：
2.7)求出p1p2和p2p3两条线段的中点p12(x12,y12,z12)，p23(x23,y23,z23)，设圆心坐标为po(xo,yo,zo)，求解获得圆心坐标，以圆心为球心，r为半径的球参数方程为：
其中，φ表示球面上任意一点与球心的连线与其在X-Y平面的投影的夹角，
α表示球面上任意一点与球心的连线在X-Y平面的投影与X轴的夹角；
2.8)将上式代入平面方程式Ax+By+Cz＝1中，得到：
A(rcosφcosα+xo)+B(rcosφsinα+yo)+C(rsinα+zo)＝1
从而获得空间圆的参数方程为：
2.9)取矢量
为X1轴向量，平面法向量为Z1轴向量，建立T1坐标系，由于姿态变换矩阵为单位正交矩阵，则有Y1＝X1×Z1，进而可以求得Y1轴向量，其姿态变换矩阵R和平移向量T为：
其中，xp，yp，zp分别表示T1坐标系中的Y1轴向量在参考坐标系的X、Y、Z三个轴方向的投影分量的值；
设
则有：
姿态变换矩阵R和平移向量T可以得到坐标变换矩阵为：
2.10)取末端轨迹在T1的位置坐标为p'(x',y',z')，通过姿态变换矩阵和平移向量可以求得末端轨迹在T0中的位置p(x,y,z)，即为圆弧插补的规划轨迹；
2.11)在优化后的轨迹上选择需要的插补点，并列出每个点p(x,y,z)在该时刻示教臂的末端执行器位姿相对固定参考坐标系的变换矩阵：
2.12)对每个优化后的插补点p(x,y,z)进行逆运动学求解，其中rij取与插补点最近的末端坐标点的变换矩阵中的值，则可按顺序求解得到：
θ23＝Atan2[(-a3-a2c3)pz-(c1px+s1py)(d4-a2s3)(a2s3-d4)pz-(a3+a2c3)(c1px+s1py)]
θ2＝θ23-θ3
θ4＝Atan2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23)
s5＝-[r13(c1c23c4+s1s4)+r23(s1c23c4-c1s4)-r33(s23c4)]
c5＝r13(-c1s23)+r23(-s1s23)+r33(-c23)
θ5＝Atan2(s5,c5)
s6＝-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31(s23s4)
c6＝r11[(c1c23c4+s1s4)c5-c1s23s5]+r21[(s1c23c4-c1s4)c5-s1s23s5]-r31(s23c4c5+c23s5)
θ6＝Atan2(s6,c6)
其中，Atan2为反正切函数，si＝sinθi，ci＝cosθi；
2.13)在每个关节角度求出后，接着在每个关节可活动的范围内通过对关节角加减360°，找出所有的可能值，再利用连续性原则从所有的可能值里选择唯一合理值，即与上一时刻该关节的角度值最接近的值；求出每个优化后的末端位姿对应的各个关节角，整理成角度数据包再发给仿真模块进行仿真，并且发给控制文件生成模块进行控制代码文件生成。
3.根据权利要求1所述的无线控制的六自由度机械臂示教***，其特征在于：所述的数据采集器与PC端APP之间的通信方式采用了WiFi无线通信，实现了数据的无线交互；其数据传输方式采用UDP通信协议，数据采集器与PC端APP互相锁定了对方的IP地址和端口号，为一对一的通信，第三方无法再接入连接，保证了数据传输的保密性；对指令、参数、角度数据都做了校验，保证了数据传输的正确性；采用WiFi透传模式，数据的无线收发几乎无延迟，保证了数据传输的高效性；所用的WiFi模块传输距离远穿墙能力强，并且一旦锁定了双方的IP地址和端口号后，无论是数据采集器或是PC端单方断电掉线重连后都能直接继续进行数据传输，保证了数据传输的稳定性。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710665078.9A CN107363812B (zh) | 2017-08-07 | 2017-08-07 | 无线控制的六自由度机械臂示教*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710665078.9A CN107363812B (zh) | 2017-08-07 | 2017-08-07 | 无线控制的六自由度机械臂示教*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107363812A CN107363812A (zh) | 2017-11-21 |
| CN107363812B true CN107363812B (zh) | 2020-11-13 |
Family
ID=60309433
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710665078.9A Active CN107363812B (zh) | 2017-08-07 | 2017-08-07 | 无线控制的六自由度机械臂示教*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107363812B (zh) |
Families Citing this family (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108318581B (zh) * | 2017-12-08 | 2020-12-18 | 中国兵器科学研究院宁波分院 | 一种不用装夹定位的圆弧面工件超声波c扫描自动检测方法 |
| CN109927056B (zh) * | 2017-12-19 | 2020-11-10 | 合肥欣奕华智能机器有限公司 | 一种示教摇杆及机器人示教方法、机器人控制*** |
| CN108015776B (zh) * | 2018-01-19 | 2023-10-27 | 广东海洋大学 | 一种基于CompactRIO的机器人控制*** |
| CN108340352A (zh) * | 2018-02-09 | 2018-07-31 | 巨轮中德机器人智能制造有限公司 | 基于示教关节臂的工业机器人远程实时操控方法 |
| CN108469821B (zh) * | 2018-03-23 | 2021-03-30 | 广东工业大学 | 喷涂机器人轨迹修正方法、装置、设备及可读存储介质 |
| CN108500966A (zh) * | 2018-03-30 | 2018-09-07 | 阜阳师范学院 | 一种新型无线机械臂及控制方法 |
| CN109032070B (zh) * | 2018-07-19 | 2020-11-03 | 西南交通大学 | 一种采用电涡流位移传感器的非接触式R-test测量仪标定方法 |
| CN108994851B (zh) * | 2018-08-07 | 2020-07-24 | 王保山 | 一种机械手的智能打磨方法及智能打磨机械手*** |
| CN108748115B (zh) * | 2018-08-24 | 2022-05-06 | 北京密塔网络科技有限公司 | 一种多连杆自检协作机械臂及控制方法 |
| CN109093634A (zh) * | 2018-09-20 | 2018-12-28 | 埃夫特智能装备股份有限公司 | 一种拖动编程的无线示教手柄装置 |
| CN109446670B (zh) * | 2018-11-01 | 2023-01-24 | 金陵科技学院 | 一种机械手臂的行为识别的数据生成以及存储*** |
| CN109799771B (zh) * | 2018-12-29 | 2021-01-05 | 深圳市越疆科技有限公司 | 一种工业机器人的控制***、方法及装置 |
| CN110421559B (zh) * | 2019-06-21 | 2021-01-05 | 国网安徽省电力有限公司淮南供电公司 | 配网带电作业机器人的遥操作方法和动作轨迹库构建方法 |
| CN110497391B (zh) * | 2019-09-04 | 2020-11-03 | 湖南三一快而居住宅工业有限公司 | 机械臂示教方法及机械臂示教装置 |
| CN110587612A (zh) * | 2019-10-09 | 2019-12-20 | 福州旺星人智能科技有限公司 | 一种机器人智能编码*** |
| CN111026038A (zh) * | 2019-12-06 | 2020-04-17 | 北京和利时智能技术有限公司 | 一种控制方法、装置、存储介质及电子设备 |
| CN112936265B (zh) * | 2021-01-29 | 2022-09-20 | 山东莱钢永锋钢铁有限公司 | 一种远程调控abb机械臂的*** |
| JP2022122729A (ja) * | 2021-02-10 | 2022-08-23 | セイコーエプソン株式会社 | 教示装置、教示方法および教示プログラム |
| CN113794409B (zh) * | 2021-09-24 | 2024-03-15 | 广东麦珂威尔健康科技有限公司 | 基于磁编码器的健身器材驱动装置及其控制方法 |
| CN113858215A (zh) * | 2021-11-24 | 2021-12-31 | 江苏汇博机器人技术股份有限公司 | 一种六轴机器人自动装配仿真*** |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2006000264A1 (en) * | 2004-06-24 | 2006-01-05 | Abb Ab | Device and method for controlling a robot by means of a wireless teach pendant unit |
| CN200970771Y (zh) * | 2006-11-10 | 2007-11-07 | 南京工程学院 | 五自由度多关节机器人智能遥控器 |
| CN102681465A (zh) * | 2012-05-23 | 2012-09-19 | 浙江大学 | 具备运动规划功能的工业机器人示教规划器及其控制方法 |
| CN203070065U (zh) * | 2013-01-18 | 2013-07-17 | 沧州得丰机械设备有限公司 | 工业机器人无线示教控制器 |
| CN203954118U (zh) * | 2014-06-03 | 2014-11-26 | 王维 | 一种人体单侧下肢外骨骼装置 |
| CN204932175U (zh) * | 2015-09-22 | 2016-01-06 | 浙江科惠医疗器械股份有限公司 | 一种适用于被动式运动关节康复器上的角度传感器 |
| CN105128030B (zh) * | 2015-09-29 | 2017-11-07 | 中国科学院深圳先进技术研究院 | 关节机构 |
| CN105549417A (zh) * | 2015-12-10 | 2016-05-04 | 南京熊猫电子股份有限公司 | 一种机器人教学***及其方法 |
| CN106625683A (zh) * | 2017-02-27 | 2017-05-10 | 哈尔滨工业大学深圳研究生院 | 一种基于wifi的可重构人机协作机械臂*** |
-
2017
- 2017-08-07 CN CN201710665078.9A patent/CN107363812B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN107363812A (zh) | 2017-11-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107363812B (zh) | 无线控制的六自由度机械臂示教*** | |
| CN107544299B (zh) | 用于六自由度机械臂示教控制的pc端app*** | |
| CN107932504B (zh) | 基于PyQt的机械臂运行控制*** | |
| CN107901039B (zh) | 基于Python的桌面级机器人离线编程仿真*** | |
| CN111381815B (zh) | 离线编程后置代码转换方法和基于该方法的双机器人协同智造***、方法 | |
| US9311608B2 (en) | Teaching system and teaching method | |
| CN106003034B (zh) | 一种主从式机器人控制***及控制方法 | |
| CN109848964B (zh) | 基于光学动作捕捉的示教机器人数据采集器*** | |
| CN109262609A (zh) | 基于虚拟现实技术的机械臂远程控制***及方法 | |
| CN104786221A (zh) | 一种基于以太网的开放式机械手控制方法 | |
| CN203449306U (zh) | 一种主从式双工业机器人协调操作控制*** | |
| CN107309884B (zh) | 机器人标定***及方法 | |
| CN108356796A (zh) | 一种可适配多种工业机器人的示教*** | |
| CN107838922A (zh) | 一种机器人免重复示教的方法 | |
| US20170197308A1 (en) | Teaching data generating device and teaching data-generating method for work robot | |
| CN110216683A (zh) | 一种基于游戏手柄的协作机器人示教方法 | |
| CN112873204B (zh) | 机器人标定方法、装置、设备及计算机可读存储介质 | |
| CN107363835A (zh) | 运动控制部件的配置方法、装置、介质和机器人*** | |
| CN108527320A (zh) | 一种基于三维鼠标的协作机器人引导示教方法 | |
| CN107932510A (zh) | 基于动作采集的nao机器人*** | |
| CN113211447A (zh) | 一种基于双向rrt*算法的机械臂实时感知规划方法及*** | |
| Jan et al. | Smartphone based control architecture of teaching pendant for industrial manipulators | |
| Banduka | Remote monitoring and control of industrial robot based on android device and wi-fi communication | |
| Diachenko et al. | Industrial collaborative robot Digital Twin integration and control using Robot Operating System | |
| JP2012099082A (ja) | 多軸同期動作機械のプログラム変換モジュール及びプログラム変換方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |