机械臂远程控制的设计与实现

毕业设计机械臂远程控制的设计与实现Design and Realization of Remote Control of Manipulator专 业：自动化 摘 要由于当下人工捕捞海参存在成本高、危险性高、人员短缺等问题，用机械臂去代替人捕捞海参已成为必然的趋势。而目前工业用的机械臂大都属于第一代机械臂，即人工开环控制下的机械臂，且大多数机械臂只能完成单一重复性的工作。由于海底捕捞环境复杂、海参位置不固定、机械臂防水程度不够、抓取操作复杂等因素，第一代机械臂不能直接用于捕捞海参。为了满足水下捕捞作业的需求，需要对现有机械臂从结构、控制算法到控制系统进行改进。本文基于6自由度机械手臂（乐幻索尔科技）和7自由度机械手臂，设计此类机械臂控制算法并加以验证和扩展。为了实现机械臂的远程控制和日后的二次开发，采用了美国国家仪器开发的LabVIEW，利用其庞大的函数库和便捷的用户界面编写的特性，搭建了6自由度机械手臂和7自由度机械手臂的远程人工控制系统，加入网络摄像头用于实时画面传输，加入手柄来对机械臂控制进行操作，通过网络传输信号实现了远程控制，使操作人员在远处通过实时画面来控制机械臂简洁迅速的抓取某一物体。本文创新之处在于机械臂控制算法。现有的由舵机旋转控制运动的机械臂大多以单舵机控制为主，即机械臂上所由舵机被分别控制，操作复杂费时。本文设计了6自由度机械手臂和7自由度机械手臂的简化控制、机械臂末端定点控制和机械臂末端平动控制的算法并加以归纳和验证。其中机械臂末端定点控制算法，参考机械臂逆运动学的理念，摆脱传统的矩阵逆乘解析法等计算复杂的算法，从空间几何的角度进行设计的。先单独计算控制机械臂底盘旋转舵机，这样将机械臂控制的空间问题转化为平面问题，再用多边形与机械臂姿态对应，在已知目标点的情况下，加入合理的约束条件，求出多边形的各个角度，即机械臂各个关节对应的舵机旋转角度，从而实现了机械臂末端定点控制。在机械臂末端定点控制算法的基础上，实现了机械臂末端平动控制算法，使此类机械臂人工控制更加直观简洁。 通过实验验证，6自由度机械臂和7自由度机械臂的控制算法可以满足控制需求。使用LabVIEW编写6自由度机械臂和7自由度机械臂的远程人工控制系统已基本完成，随着手柄和网络摄像头的加入，已经能够实现操作人员实时依据返回机械臂工作环境的画面，使用手柄远程控制机械臂对给定目标进行抓取。关键词：机械臂控制；机械臂逆运动学；远程控制- II -Design and Realization of Remote Control of ManipulatorAbstract Due to the high cost, high risk and shortage of artificial sea cucumber, it is an inevitable trend to use robotic arm instead of sea cucumber. The current industrial robots are mostly the first generation of robotic arm, that is, artificial ring under the control of the arm, and most of the arm in the completion of a single repetitive work. Due to the complexity of the seabed fishing environment, the location of the sea cucumber is not fixed, the arm arm is not enough water level, the crawling operation is complicated and so on. The first generation manipulator can not be directly used for artificial sea cucumber. In order to meet the needs of underwater fishing operations, the need for existing robots from the structure, control algorithms to control the system to transform.Based on 6-DOF robotic arm and 7-DOF manipulator, this algorithm is designed and validated and expanded. In order to achieve the remote control of the robot and the future of the secondary development, the use of the United States National Instruments developed LabVIEW, the use of its huge library and convenient user interface to write the characteristics of the 6-DOF mechanical arm and 7 degrees of freedom machinery Arm remote control system, adding a web camera for real-time picture transmission, adding the handle to manipulate the arm control, and then realize the remote control, so that the operator in the distance through the real-time screen to control the robot simple and quick grasp An object.The innovation of this paper lies in the robot arm control algorithm. The existing robotic machine to control the movement of the robot is mostly controlled by a single steering gear, that is, the arm on the steering gear were controlled, the operation of complex time-consuming. In this paper, the simplified control of the 6-DOF manipulator and the 7-DOF manipulator, the algorithm of the fixed-point control of the manipulator and the translational control of the end of the manipulator are designed and summarized. The algorithm of manipulating the end of the manipulator is based on the idea of inverse kinematics of the manipulator, which is designed from the perspective of spatial geometry from the traditional matrix inverse multiplication method. The control of the robot arm chassis rotation of the steering wheel, so that the robot arm control space problem into a plane problem, and then the polygon and arm attitude corresponding to the known target point in the case, adding reasonable constraints, The angle of the polygon, that is, the rotation angle of the steering gear corresponding to each joint of the manipulator, thus realizing the fixed point control of the arm end. On the basis of the control algorithm of the end of the manipulator, the algorithm of the end of the manipulator is realized, which makes the artificial control of the manipulator more intuitive and concise.Experiments show that the 6-DOF manipulator and the 7-DOF manipulator control algorithm can meet the control requirements. The use of LabVIEW to write 6-DOF robotic arm and 7-DOF robotic arm remote control system has been basically completed, with the joystick and web camera to join, has been able to achieve real-time operation according to the robot back to the robot working environment, through the handle remote The control arm scans the given target.Key Words：Robot arm control; manipulator inverse kinematics; remote control- VI -目 录摘 要IAbstractII1 引言11.1 课题研究背景11.2 设计的目的与意义11.3 主要设计要求21.4 国内外研究现状21.5 设计方案及主要设计内容42 机械臂控制算法的设计52.1 单舵机控制52.2 6自由度机械臂控制算法的设计52.2.1 简化控制62.2.2 末端定点坐标控制72.2.3 末端平动控制102.3 7自由度机械臂控制算法的设计112.3.1 简化控制122.3.2 末端定点坐标控制122.3.3 末端平动控制152.4 多自由度机械臂控制算法的设计152.5 机械臂控制算法的验证173 机械臂远程控制的实现183.1 机械臂基本组成及功能183.2 机械臂控制算法的实现193.3 机械臂远程控制实现233.4 控制手柄的加入243.5 误差分析264 机械臂设计的远景规划274.1 机械臂的姿态反馈274.2 机械臂的防水284.3 图像定位与机械臂控制算法的结合284.4 机械臂的避障算法的设计29设计总结30参 考 文 献31致 谢33机械臂远程控制的设计与实现1 引言1.1 课题研究背景当下海参捕捞主要靠人工完成，由海参捕捞员穿上潜水衣下潜到水面下七八米的深度进行捕捞作业，如图1.1所示。这种人工捕捞的方式有多种弊端。第一，海参捕捞员这份工作对水下工作技能要求很高，而且这种工作危险系数大，十分考验工作人员的毅力和勇气。第二，海参捕捞主要在冬季，冬季海水寒冷，工作环境很恶劣。第三，人工捕捞费用极高，海参捕捞员平均一个月酬金3万余元，增加了海参的成本，且即使如此高的工资，海参捕捞员也是供不应求。图1.1 人工捕捞海参现状基于上述弊端，人工捕捞海参的方式将逐渐被机器捕捞海参取代。而目前虽然我国深海载人潜水器“蛟龙号”成绩斐然1，水下技术颇有建树，但是至今没有专门用在水下捕捞海参的相关机器。于是，为了解决海参捕捞问题，促进海参养殖行业的发展，拟设计并开发一款用于水下海参捕捞的机械臂，海参捕捞人员将船开到指定海域，投下机械臂等相关装置，在船上通过水下摄像头返回的图像操作机械臂进行海参捕捞。后期可与水下潜行器相结合，利用图像识别，自主完成海参捕捞，将人彻底从海参捕捞这种繁重危险的工作中解放出来。而完成这宏伟目标的第一步，是对机械臂进行远程控制。无论人工控制还是智能识别控制，如何控制机械臂才能直观，简洁，不易出错，是本文主要研究设计的方向。1.2 设计的目的与意义现有由舵机旋转控制运动的机械臂大多以单舵机控制为主，即机械臂上所有舵机被独立控制。以主流的6自由度旋转机械臂为例，需要控制6路舵机。控制输入较多，且操作复杂，因为每调整一路舵机时，机械臂末端都会以不同的轴按不同的半径旋转，这样到达目标点之前需要对多个舵机进行反复调整，既影响工作效率，又不符合人们直观控制的思维习惯，即上、下、左、右、前、后直线调整。现有的流水线上的多自由度旋转机械臂大多以动作组的方式进行作业，本质是将动作预先编成指令进行存储，再让机械臂一遍遍读取指令进行动作。这种方式只适用于重复性较高，且目标点固定的场合，不适用于人机交互、目标点不确定等场合2。所以，设计一套简单直接的多自由度旋转机械臂的控制算法具有很深远的意义。本文设计目的是设计一套简单直接普适的多自由度机械臂的控制算法，并使用LabVIEW设计开发6自由度和7自由度旋转机械臂的远程控制系统。1.3 主要设计要求使用LabVIEW设计开发机械臂远程控制系统，实现对6自由度机械臂或7自由度机械臂的控制。控制模式包括单舵机控制、简化控制、机械臂末端定点坐标自主控制和机械臂末端平动控制。控制模式可以任意切换，并可以通过远程连接对机械臂进行操作。控制算法自主设计，并且要求开发平台界面友好，操作方便，具有较好的数据分析方法与手段。完成后需反复测试，确保可靠。本设计开发平台为Windows 7，开发工具为LabVIEW2014或以上版本，硬件工具有PC机，6自由度机械手臂（乐幻索尔科技）及其相关开发套件。1.4 国内外研究现状机械臂属于机器人的一种，指能够按照程序或指令对物体完成夹取、传递等操作的机械设备，其历史可追溯到1913年。第一个机器手是美国福特公司于1913年研制出来，用于汽车生产线的上下料。从那时至今，机械臂由于工作效率高，重复性工作能力强，能够在恶劣的环境下工作等优点，被广泛地应用在工业生产中。机械臂的控制技术也随之得到国内外很多机构和个人的探究。工业机械臂大致分为三个阶段。第一代机械臂主要用人工进行控制，控制是开环的，机械臂本身不具备识别物体的能力3。当下大多数机械臂仍处于此阶段。第二代机械臂集合了多种传感器进行反馈，具备视觉、触觉、听觉和一定程度的智能，有其独立的控制系统，这些都能辅助工作人员更好的控制机械臂。第三代机械臂能够彻底摆脱人工控制，独立完成工作中各种问题，将完全实现智能。机械臂有很多种，按运动坐标形式分类，有直角坐标式机械臂，圆柱坐标式机械臂，球坐标式机械臂和多关节式机械臂大致四类，而且每一类中的机械臂也有多种，所以机械臂五花八门，这给机械臂控制算法的探究造成了一定的困难。很难有哪种机械臂的控制算法能满足多种类甚至全部种类的机械臂。所以，对机械臂控制算法的研究应针对机械臂的种类而具体探究设计。不同种类的机械臂，在不同的工作要求下，对应的最适合的控制算法也是不同的，但机械臂控制算法的研究思想却可以互相借鉴。机械臂逆运动学的探究式机械臂控制算法的主要组成部分。机械臂的正向运动学，是给出机械臂的各个关节的角度从而确定整个机械臂的姿势，正向运动学相对简单，因为各个关节的角度和机械臂的姿势是一对一的关系。而机械臂的逆运动学是在机械臂姿势的某种或某些约束下（如给定机械臂末端位置）求解各个关节的角度。求出的解多数情况并不唯一，所以机械臂的逆运动学往往涉及到寻找最优解的问题。近年来，国内对机械臂逆运动学的研究取得了很多成果。王海，蔡英凤，张为公提出一种逆运动学解析算法在传统6维位姿矩阵的基础上，额外提出一个elbo wangle新约束，利用位姿目标矩阵和该elbow angle共7维约束推导出7自由度机械臂逆运动学的一种可求得冗余解的解析算法4。徐军福利用Matlab仿真平台的Robotics Toolbox机器人工具箱进行仿真和验证逆运动学求解算法的正确性5。李宇飞，高朝辉和申麟使用了立体相机，对空间目标的位姿进行测量从而控制机械臂的抓取6。崔泽、张世兴和崔玉乾为了降低冗余自由度的逆运动学研究的难度，以7自由度机械臂为研究对象，采用SolidWorks/Motion模块的虚拟仿真，应用轨迹规划算法实现避障优化7。任晓琳和李洪文在D-H参数法中引入虚拟关节建立运动学模型，使用四种PSO算法进行计算，使机械臂的逆运动学计算更加有效8。张栩曼，张中哲，王燕波等人提出一种适用于大多数串联型节械臂的数值迭代逆运动学求解方法9。国外机械臂逆运动学成果斐然。 B Siciliano提出了基于Jacobian的闭环直接运动学（CLDK）算法来解决沿给定轨迹的直接运动学问题10。P Chang使用拉格朗日乘数法得到具有冗余度的机械手逆运动学的闭式解决方案公式。在提出的方法中，解析运动方法中的重复性问题将不再存在11。B Karlik ，S Aydin使用ANN来实现六自由度机器人机械手的运动学解决方案12。AT Hasan ，AMS Hamouda ，N Ismail ，HMAA Al-Assadi提出了一种使用人工神经网络ANN的收养学习策略，用于控制6D.O.F机器人机器人的运动，克服了反向运动学问题，这主要是手臂配置中的奇异点和不确定性13。Simas, H ; Guenther, R ; Da Cruz, D. F. M ; Martins, D设计一种基于数值积分求解并联机器人逆运动学的数值算法14。Jess Rubio, Jos ; Aquino, Vctor ; Figueroa, Maricela提出了两种获取移动机器人逆运动学的方法并进行了模拟验证15。 Cavdar, Tugrul ; Mohammad, M. ; Milani, R. Alavi提出了一种基于改进的人造蜂群（ABC）的新方法来解决机器人手臂的逆运动学问题，并使用一些统计分析方法来确定修改后的ABC的权衡参数16。VADS Marques，RS Gonalves，JCM Carvalho，M Pfurner讨论了轨迹插值和运动表示的奇异性的一些问题，并且使用双四元数表示作为避免这些问题的替代方案，并且获得确保运动轨迹的一般空间分析的模型17。1.5 设计方案及主要设计内容本文拟走理论与实践相结合的研究设计路线，即理论上的机械臂控制算法的设计与实际用单片机和LabVIEW对机械臂控制算法加以实现相结合。前期查阅相关资料、了解并选择购买机械臂、完成机械臂的组装调试、设计6自由度机械臂控制算法并用单片机加以验证。中期学习LabVIEW18、用LabVIEW设计6自由度机械臂控制系统、增加自由度改装成7自由度机械臂、设计7自由度机械臂控制算法并用LabVIEW设计7自由度机械臂控制系统。后期对机械臂控制算法进行归纳和扩展、实现远程控制并完成毕业论文。主要设计内容是多自由度机械臂逆运动学解算和机械臂控制系统的设计。2 机械臂控制算法的设计2.1 单舵机控制对于由舵机实现自由度的机械臂而言，单舵机控制是最基础的控制，且所有此类机械臂的控制算法都是基于单舵机控制实现的。每一个舵机，对应机械臂的一个自由度。单舵机控制，即对机械臂上的舵机分别控制，使其旋转到指定角度，从而带动机械臂完成相应的动作。由于每个舵机控制独立，在无障碍的情况下，单舵机控制可以在机械臂工作范围内实现任意姿态（机械结构冲突除外），机械臂末端可以到达任意位置。综上所述，单舵机控制使机械臂控制灵活，同时也使机械臂控制复杂。以挖掘机为例，多数液压挖掘机是5自由度，虽然挖掘机自由度是由液压带动实现的，其操作本质和单舵机控制下5自由度机械臂相似。挖掘机操作杆如图2.1所示。挖掘机驾驶操作难度不言而喻，由此类比，可见机械臂单舵机控制操作的复杂性。所以，为了更简单的控制多自由度机械臂，必须用算法对机械臂控制进行简化。图2.1 某液压挖掘机操作杆示意图 2.2 6自由度机械臂控制算法的设计6自由度旋转机械臂有6路舵机，如图2.2所示，舵机6-1控制机械爪张合，舵机6-2控制机械爪旋转，舵机6-6控制底盘旋转，舵机6-3、舵机6-4和舵机6-5控制竖直平面内机械臂肢节运动。机械臂各个肢节的长度已知，舵机6-4和舵机6-5旋转轴距离OD1为L1，舵机6-3和舵机6-4旋转轴距离D1D2为L2，机械臂末端机械爪中心到舵机6-3旋转轴距离D2D3为L3。工作范围是以图示原点O为圆心，L（L=L1+L2+L3）为半径的半球域（由于转台限制，转台下的工作范围并不是完整的半球域）。（注：由于购买的6自由度机械臂臂长L1=L3，所以之后部分6自由度机械臂控制算法是基于此条件设计的。）图2.2 6自由度旋转机械臂舵机及建系示意图2.2.1 简化控制为了初步简化6自由度机械臂在无障碍工作环境下的控制过程，令舵机6-4与舵机6-3旋转角度时时刻刻相同，即D3D2D1=D2D1O。下面从工作范围和姿态两个角度对简化控制进行分析。从工作范围来看，由于D3D2D1=D2D1O，基于实际条件L1=L3，反映机械臂姿态的四边形OD1D2D3为等腰梯形，目标点到原点O的距离，即为OD3的长度。当目标点为O点时，D3点与O点重合，此时等腰梯形OD1D2D3变为等腰三角形O(D3)D1D2。因为L1=L3，所以此等腰三角形是可以存在的，即目标点到原点O距离为0是可以实现的。当D3D2D1=D2D1O=180时，O点、D1点、D2点和D3点四点共线，达到了目标点到原点的最大距离L，也是此机械臂的最大工作半径。由于D3D2D1是连续变化的，目标点到原点O的距离从0到L也是连续的，所以，简化控制目标点到原点的距离范围和单舵机控制相同。但是，由于转台原因，机械臂肢节OD1不能转动到XOY平面以下，所以简化控制工作范围比单舵机控制工作范围略小。从姿态来看，单舵机控制时，机械臂末端，即目标点固定，机械臂可以有无数种姿态，即四边形OD1D2D3没有全约束。而简化控制，当目标点给定时，机械臂只有一种姿态与其对应，即等腰梯形OD1D2D3被全约束。简化控制虽然牺牲了机械爪到达同一目标点的姿态的多样性和小部分工作范围，但是简化了控制过程，具体体现为控制输入数量减少，控制比单舵机控制简洁，易于人工操作。2.2.2 末端定点坐标控制为了进一步简化6自由度机械臂的控制，本文在无障碍的工作环境下，又设计了末端定点坐标控制的算法，实现机械臂自动调整姿态，使其末端与目标点重合。以舵机6-5旋转轴中心为原点坐标O，建立如图2.2所示的三维直角坐标系，其中XOY平面始终与机械臂底座平面平行。当目标点已知为(x0,y0,z0)时，即D3点坐标为(x0,y0,z0)，将此坐标转换为三维空间内的极坐标(r0,0,0)（r0L1+L2+L3）。此时，四边形OD1D2D3形状不固定，每一种形状都对应着机械臂的机械爪到达目标点时的姿态。有无数种四边形OD1D2D3的形状，也有无数种抓取姿态，无法进行相应的姿态计算和姿态控制。所以，我们需要添加约束条件，使四边形OD1D2D3全约束，即每个目标点的对应姿态唯一确定，这样才能对其进行计算控制。添加约束条件的方法有很多，这里提出两种比较成熟的方案。（1）方案一当有前提条件L1=L3时，在OD3线段上取两点H1和H2，使 (2.1) (2.2)加入约束条件于H2，于H1，此时，四边形OD1D2D3全约束,且H1H2平行且等于D1D2，如图2.3所示，求舵机6-3、舵机6-4和舵机6-5的旋转角度即求D3D2D1、D2D1O和D1OD3（-D1OD3即为舵机6-5的旋转角，而已知），结果如下： (2.3) (2.4) (2.5) 图2.3 6自由度旋转机械臂方案一模型简化示意图舵机6-6旋转角度即为0。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了6自由度旋转机械臂自动到给定的三维空间坐标点进行作业。添加方案一的约束条件，6自由度旋转机械臂的控制方法计算简单，误差较小，工作范围与简化控制的工作范围大致相同，且经过实验验证此方法是可以实现的，但是需要在机械臂设计制造过程中注意对称机械臂肢节长度相等，其相等程度将影响机械爪到目标点的误差大小。（2）方案二当机械臂臂长并不对称相等，且无明显规律时，可采用方案二：连接D3D1，加入约束条件D3D1=kr0,其中,此时四边形OD1D2D3全约束，如图2.4所示。求舵机6-3、舵机6-4和舵机6-5的旋转角度即求D3D2D1、D2D1O和D1OD3（-D1OD3即为舵机6-5的旋转角，而已知），结果如下： (2.6) (2.7) (2.8) 图2.4 6自由度旋转机械臂方案二模型简化示意图 舵机6-6旋转角度即为0。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了6自由度旋转机械臂自动到给定的三维空间坐标点进行作业。当r0=L1+L2+L3时，D3D1=kr0=L2+L3=D1D2+D2D3=D3O-D1O,即D3、D2、D1和O四点共线，机械臂得以完全伸展。如k取其他值，则机械臂的机械爪不能完全伸展，即不能在最大工作半径下作业，缩小了其作业空间。应注意D1D3+D3OD1O,即,也就是说，目标点到原点的距离不能小于。添加方案二的约束条件，6自由度旋转机械臂的控制方法适用性好，计算简单，误差很小，尤其适用于各肢节长度无规律的机械臂控制。工作空间比单舵机控制下的工作区间小，因为存在隐性约束条件，所以使目标点到原点的距离不能小于一个与机械臂肢节长度相关的定值。但是因为这一定值可以通过人为改变机械臂某肢节长度进行调整，且通常这个机械爪不能到达的目标点的区域较小，不影响机械臂的主要工作区域，所以方案二不失为添加约束条件的较好的选择。 2.2.3 末端平动控制在末端定点坐标控制算法的基础上，可以实现6自由度机械臂末端平动控制。这里末端平动控制包含两种模式。第一种模式，是在如图2.2所示的三维直角坐标系中，当已知当前6自由度旋转机械臂机械爪的中心坐标(x0,y0,z0)时，读取输入状态，如收到z0减少的指令，则目标点坐标为(x0,y0,z0-P)（P为步长，可以编程调节步长大小实现机械爪运动速度的调节），再通过上述的6自由度旋转机械臂定点运动的算法，使机械臂的机械爪到达目标点。此时目标点将作为z0减少后6自由度旋转机械臂机械爪的中心坐标(x0,y0,z0)，即刷新坐标，之后每一时刻都重复上述步骤，沿其余坐标轴增减同理，从而实现了6自由度机械臂末端机械爪中心沿直线运动。第二种模式，是将底盘舵机的旋转单独考虑，即将底盘舵机旋转到目标点的方向，使其工作平面与目标点共面，然后再此二维平面内进行平动控制。具体来说，是调整0角、z轴坐标和。0角的调整是直接调整底盘旋转舵机6-6，z轴坐标的调整是对目标点极坐标0的调整，z轴坐标减少，0增大。的调整，可以利用调整前后不变的z轴坐标和调整后的计算新的目标点的r0和0。每次调整都如上刷新极坐标，再按6自由度机械臂的末端定点控制计算出各个舵机的旋转角度，从而实现6自由度机械臂末端机械爪中心沿直线运动。（注：在实际编程中，由于所购买的6自由度旋转机械臂满足方案一的前提条件，即L1=L3 ，所以采取方案一对6自由度旋转机械臂控制加以约束。）2.3 7自由度机械臂控制算法的设计7自由度旋转机械臂有7路舵机，如图2.5所示，舵机7-1控制机械爪张合，舵机7-2控制机械爪旋转，舵机7-7控制底盘旋转，舵机7-3、舵机7-4、舵机7-5和舵机7-6控制竖直平面内机械臂肢节运动。机械臂各个肢节的长度已知，舵机7-5和舵机7-6旋转轴距离OD1为L1，舵机7-4和舵机7-5旋转轴距离D1D2为L2，舵机7-3和舵机7-4旋转轴距离D2D3为L3，机械爪中心到舵机7-3旋转轴距离D3D4为L4，工作范围是以图示原点O为圆心，L（L=L1+L2+L3+L4）为半径的半球域（由于转台限制，转台下的工作范围并不是完整的半球域）。（注：由于组装的7自由度机械臂臂长L1=L4，L2=L3，所以之后部分7自由度机械臂控制算法是基于此条件设计的。）图2.5 7自由度旋转机械臂舵机及建系示意图2.3.1 简化控制为了初步简化7自由度机械臂在无障碍工作环境下的控制过程，令舵机7-4与舵机7-3旋转角度时时刻刻相同，舵机7-6与舵机7-5旋转角度时时刻刻相同,即D4D3D2=D3D2D1，D2D1D0=D1OZ。基于实际条件L1=L4，L2=L3，分析结果与6自由度机械臂的简化控制类似，优缺点同6自由度机械臂的简化控制。2.3.2 末端定点坐标控制为了进一步简化7自由度机械臂的控制，本文在无障碍的工作环境下，扩展了6自由度机械臂末端定点坐标的算法，实现机7自由度械臂自动调整姿态，使其末端与目标点重合。以舵机7-6旋转轴中心为原点坐标O，建立如图所示的三维直角坐标系，其中XOY平面始终与机械臂底座平面平行。当目标点已知为(x0,y0,z0)时，即D4点坐标为(x0,y0,z0)，将此坐标转换为三维空间内的极坐标(r0,0,0)（r0L1+L2+L3+L4）。此时，五边形OD1D2D3D4形状不固定，需要添加约束条件，使五边形OD1D2D3D4全约束，即每个目标点的对应姿态唯一确定，这样才能对其进行计算控制。（1）方案一当有前提条件L1=L4，L2=L3时，在OD4线段上取三点H3、H2和H1，使 (2.9) (2.10) (2.11) (2.12) 加入约束条件D3H3D4O于H3，D2H2D4O于H2，D1H1D4O于H1，如图2.6所示。此时，五边形OD1D2D3D4全约束，且线段D4O对称轴即为线段D2H2所在直线。求舵机7-3、舵机7-4、舵机7-5和舵机7-6的旋转角度即求D4D3D2、D3D2D1、D2D1O和D1OD3（-D1OD3即为舵机7-6的旋转角，而已知），结果如下： (2.13) (2.14)(2.15) (2.16)图2.6 7自由度旋转机械臂方案一模型简化示意图 舵机7-7旋转角度即为0。根据舵机7-3到舵机7-7的旋转角度，便可以确定这五个舵机的PWM值，将信号同时发送给这五个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了7自由度旋转机械臂自动到给定的三维空间坐标点进行作业。 （2）方案二当机械臂臂长并不对称相等，且无明显规律时，可采用方案二。连接D4D1和D4D2，加入约束条件，其中,此时五边形OD1D2D3D4全约束，如图2.7所示。求舵机7-3、舵机7-4、舵机7-5和舵机7-6的旋转角度即求D4D3D2、D3D2D1、D2D1O和D1OD3（-D1OD3即为舵机6-5的旋转角，而已知），结果如下： (2.16) (2.17)(2.18) (2.19)图2.7 7自由度旋转机械臂方案二模型简化示意图舵机7-7旋转角度即为0。根据舵机7-3到舵机7-7的旋转角度，便可以确定这五个舵机的PWM值，将信号同时发送给这五个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了7自由度旋转机械臂自动到给定的三维空间坐标点进行作业。应注意D1D4+D4OD1O,D2D4+D4D1D2D1，即且。2.3.3 末端平动控制当不考虑机械臂末端爪子抓取姿态时，7自由度机械臂末端平动控制思想同6自由度机械臂末端平动控制，即不断根据输入量刷新目标点的坐标，再用上述末端定点坐标控制来实现机械臂末端的平动。（注：在实际编程中，由于所购买的7自由度旋转机械臂满足方案一的前提条件，即L1=L4，L2=L3 ，所以采取方案一对7自由度旋转机械臂控制加以约束。）当考虑机械臂末端爪子抓取姿态时，舵机7-3将不再用于走位，而用来调整机械爪的抓取方向。此时，用于走位的舵机数量为三，其定点坐标控制和平动控制可使用6自由度机械臂的相关控制算法，只是目标点不再是与机械臂末端机械爪中心重合，而是与舵机7-3转轴的中心重合。（注：在实际编程中，由于所购买的7自由度旋转机械臂在不考虑舵机7-3参与走位时，不满足方案一的前提条件，即L1L3 ，所以采取方案二对7自由度旋转机械臂控制加以约束。）2.4 多自由度机械臂控制算法的设计对上述6自由度机械臂和7自由度机械臂控制算法进行归纳总结，可得出一种具有普适意义的多自由度旋转机械臂的控制算法，这里指的多自由度旋转机械臂有N路舵机（N6），当不考虑机械爪抓取物体方向时，舵机N-1控制机械爪张合，舵机N-2控制机械爪旋转，舵机N-N控制底盘旋转，其余舵机控制竖直平面内机械臂肢节运动。舵机N-1和舵机N-2为机械爪抓取控制，不影响机械爪运动，所以单独考虑。以舵机N-(N-1)旋转轴中心为原点坐标O，建立三维直角坐标系，其中XOY平面始终与机械臂底座平面平行。机械臂各个肢节的长度已知，舵机N-(N-2)和舵机N-(N-1)旋转轴距离OD1为L1，舵机N-(N-3)和舵机N-(N-2)旋转轴距离D1D2为L2，以此类推，机械爪中心到舵机N-3旋转轴距离DnDn-1为Ln。当目标点已知为(x0,y0,z0)时，即Dn点坐标为(x0,y0,z0)，将此坐标转换为三维空间内的极坐标(r0,0,0)（r0L1+L2+Ln）。此时，多边形OD1D2Dn形状不固定，每一种形状都对应着机械臂的机械爪到达目标点时的姿态。为了使其全约束，提出以下添加约束条件的方案：（1）方案一当机械臂臂长对称相等时，即Li=Ln+1-i(1in-1,i为整数)时,可采取方案一：当n为奇数时，在ODn线段上取n-1个点H1、H2、Hn-1，使,，,(),加入约束条件于Hi(1in-1,i为整数)。此时，多边形OD1D2Dn全约束，且由对称可得，该线段与DnO平行并二者有相同的对称轴。当n为偶数时，在DnO线段上取n-1个点H1、H2、Hn-1，使,加入约束条件于Hi(1in-1,i为整数)。此时，多边形OD1D2Dn全约束，且线段DnO对称轴即为线段所在直线。当多边形OD1D2Dn全约束后，可以求出控制竖直平面内机械臂肢节运动的舵机的旋转角度，即多边形OD1D2Dn的各个内角。舵机N-N旋转角度即为0。根据舵机N-3到舵机N-N的旋转角度，便可以确定这N-2个舵机的PWM值，将信号同时发送给这N-2个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了多自由度旋转机械臂自动到给定的三维空间坐标点进行作业。（2）方案二当机械臂臂长并不对称相等，且无明显规律时，可采用方案二：在多边形OD1D2Dn中连接若干顶点将其分割成互不重叠的n-1个三角形，并对连接的这些线段的长度加以约束，是各个三角形都全约束，那么多边形OD1D2Dn也被全约束。分割为三角形的方案有多种，由于Dn-1DnO不涉及舵机旋转角度，也不用计算其大小，为了简化后期计算，连接Dn与其他不相邻顶点，则有线段DnD1，DnD2，DnDn-2，且每段线段长度为：，其中,1in-2,i为整数，且i=1时D0即为原点O。这里ki的选择是有原因的，因为当r0=L1+L2+Ln时，即机械臂机械爪要达到最大工作半径，只有ki这样取值才能使机械臂每个肢节完全伸展开。此种方案还有隐形的限制条件，即DnDi-1+DnDiDi-1Di,(1in-2,i为整数，且i=1时D0即为原点O),计算得出如采用方案二，目标点到原点的坐标需大于某一定值。添加线段并约束其长度后，每个三角形均被全约束，且每个三角形边长都已知，根据边长即可求出三角形各个内角，进而求得了多边形OD1D2Dn的内角，即控制竖直平面内机械臂肢节运动的舵机的旋转角度。2.5 机械臂控制算法的验证在算法设计之后，用单片机编程对控制算法进行验证，分别实现了6自由度机械臂的单舵机控制、简化控制、机械臂末端定点坐标自主控制和机械臂末端平动控制。选择并购买STM32F103单片机开发板，用mbed开发平台完成单片机编程和调试（注：mbed是一个基于ARM Cortex-M系列的单片机开发平台）。单舵机控制、简化控制和机械臂末端平动控制是采集按键键值完成对应操作，而机械臂末端定点坐标自主控制是通过串口中断更改程序中目标点的坐标数组来完成对应操作。每当按键按下时，舵机都按预计旋转走位，控制准确。当从串口发送目标点的极坐标时，机械臂能在2秒内迅速到达目标点。从而得出结论：算法对6自由度机械臂的适应性很好。同时算法的准确性的确认也为下一步LabVIEW编程实现打下基础。 3 机械臂远程控制的实现3.1 机械臂基本组成及功能机械臂的商家，即乐幻索尔科技除了提供机械臂散件和舵机。舵机为数字舵机LD-1501MG，精度高，虚位小，180度可定位，有17KG扭力。该舵机是PWM脉宽型调节角度，周期20ms，0.5ms2.5ms的脉宽高电平对应舵机0度180度角度范围，且成线性关系。舵机驱动控制板采用5002500数值对应舵机控制输出角度的0.5ms2.5ms高电平脉冲，舵机驱动板数值与舵机旋转的角度有如下关系： (3.1)为舵机旋转角度，PWM为舵机驱动板数值。这样舵机的控制精度是3s，在2000个脉宽范围内最小控制精度能达到0.3度。十六路舵机驱动板如图3.1所示。舵机驱动板上有二次开发用的TTL串口。波特率9600，向串口发送数据帧，可以控制一个或几个舵机旋转角度和旋转到此角度的时间。通信协议如表3.1。图3.1 十六路舵机驱动板表3.1 控制板通信协议帧头数据长度指令参数0x55 0x55LengthCmdPrm 1Prm N 注1）帧头：连续收到两个0x55，表示有数据包到达。 注2）数据长度：等于参数个数N加一个指令加数据长度本身占用的一个字节长度，即数据长度Length等于参数N+2。 注3）指令：各种控制指令。 注4）参数：除指令外需要补充的控制信息。在诸多指令中，只用到了一条指令，即CMD_SERVO_MOVE，指令值3。该指令控制任意个舵机的转动。数据Length=控制舵机的个数3+5。Prm 1：要控制的舵机个数。Prm 2：时间低8位。Prm 3：时间高8位。Prm 4：舵机ID号。Prm 5：角度位置低8位。Prm 6：角度位置高8位。Prm ：格式与参数4、5、6相同，控制不同ID的角度位置。数字舵机控制的线性和舵机驱动板二次开发的通信协议和指令格式的提供，极大的方便了机械臂控制算法的实现。只需要将算法算出的各个舵机的旋转角度按照公式(3.1)计算出其PWM数值，再按指令格式将其发送到舵机驱动板的串口，即可实现机