大连理工大学本科毕业设计
机械臂远程控制的设计与实现
DesignandRealizationofRemoteControlofManipulator
学院（系）：电子信息与电气工程工程学部
专业：自动化
学生姓名：付群
学号：201382079
指导教师：唐达
评阅教师：
完成日期：2017.5.20
大连理工大学
DalianUniversityofTechnology
摘要
由于当下人工捕捞海参存在成本高、危险性高、人员短缺等问题，用机械臂去代替人捕捞海参已成为必然的趋势。而目前工业用的机械臂大都属于第一代机械臂，即人工开环控制下的机械臂，且大多数机械臂只能完成单一重复性的工作。由于海底捕捞环境复杂、海参位置不固定、机械臂防水程度不够、抓取操作复杂等因素，第一代机械臂不能直接用于捕捞海参。为了满足水下捕捞作业的需求，需要对现有机械臂从结构、控制算法到控制系统进行改进。
本文基于6自由度机械手臂（乐幻索尔科技）和7自由度机械手臂，设计此类机械臂控制算法并加以验证和扩展。为了实现机械臂的远程控制和日后的二次开发，采用了美国国家仪器开发的LabVIEW，利用其庞大的函数库和便捷的用户界面编写的特性，搭建了6自由度机械手臂和7自由度机械手臂的远程人工控制系统，加入网络摄像头用于实时画面传输，加入手柄来对机械臂控制进行操作，通过网络传输信号实现了远程控制，使操作人员在远处通过实时画面来控制机械臂简洁迅速的抓取某一物体。
本文创新之处在于机械臂控制算法。现有的由舵机旋转控制运动的机械臂大多以单舵机控制为主，即机械臂上所由舵机被分别控制，操作复杂费时。本文设计了6自由度机械手臂和7自由度机械手臂的简化控制、机械臂末端定点控制和机械臂末端平动控制的算法并加以归纳和验证。其中机械臂末端定点控制算法，参考机械臂逆运动学的理念，摆脱传统的矩阵逆乘解析法等计算复杂的算法，从空间几何的角度进行设计的。先单独计算控制机械臂底盘旋转舵机，这样将机械臂控制的空间问题转化为平面问题，再用多边形与机械臂姿态对应，在已知目标点的情况下，加入合理的约束条件，求出多边形的各个角度，即机械臂各个关节对应的舵机旋转角度，从而实现了机械臂末端定点控制。在机械臂末端定点控制算法的基础上，实现了机械臂末端平动控制算法，使此类机械臂人工控制更加直观简洁。
通过实验验证，6自由度机械臂和7自由度机械臂的控制算法可以满足控制需求。使用LabVIEW编写6自由度机械臂和7自由度机械臂的远程人工控制系统已基本完成，随着手柄和网络摄像头的加入，已经能够实现操作人员实时依据返回机械臂工作环境的画面，使用手柄远程控制机械臂对给定目标进行抓取。
关键词：机械臂控制；机械臂逆运动学；远程控制
DesignandRealizationofRemoteControlofManipulator
Abstract
Duetothehighcost,highriskandshortageofartificialseacucumber,itisaninevitabletrendtouseroboticarminsteadofseacucumber.Thecurrentindustrialrobotsaremostlythefirstgenerationofroboticarm,thatis,artificialringunderthecontrolofthearm,andmostofthearminthecompletionofasinglerepetitivework.Duetothecomplexityoftheseabedfishingenvironment,thelocationoftheseacucumberisnotfixed,thearmarmisnotenoughwaterlevel,thecrawlingoperationiscomplicatedandsoon.Thefirstgenerationmanipulatorcannotbedirectlyusedforartificialseacucumber.Inordertomeettheneedsofunderwaterfishingoperations,theneedforexistingrobotsfromthestructure,controlalgorithmstocontrolthesystemtotransform.
Basedon6-DOFroboticarmand7-DOFmanipulator,thisalgorithmisdesignedandvalidatedandexpanded.Inordertoachievetheremotecontroloftherobotandthefutureofthesecondarydevelopment,theuseoftheUnitedStatesNationalInstrumentsdevelopedLabVIEW,theuseofitshugelibraryandconvenientuserinterfacetowritethecharacteristicsofthe6-DOFmechanicalarmand7degreesoffreedommachineryArmremotecontrolsystem,addingawebcameraforreal-timepicturetransmission,addingthehandletomanipulatethearmcontrol,andthenrealizetheremotecontrol,sothattheoperatorinthedistancethroughthereal-timescreentocontroltherobotsimpleandquickgraspAnobject.
Theinnovationofthispaperliesintherobotarmcontrolalgorithm.Theexistingroboticmachinetocontrolthemovementoftherobotismostlycontrolledbyasinglesteeringgear,thatis,thearmonthesteeringgearwerecontrolled,theoperationofcomplextime-consuming.Inthispaper,thesimplifiedcontrolofthe6-DOFmanipulatorandthe7-DOFmanipulator,thealgorithmofthefixed-pointcontrolofthemanipulatorandthetranslationalcontroloftheendofthemanipulatoraredesignedandsummarized.Thealgorithmofmanipulatingtheendofthemanipulatorisbasedontheideaofinversekinematicsofthemanipulator,whichisdesignedfromtheperspectiveofspatialgeometryfromthetraditionalmatrixinversemultiplicationmethod.Thecontroloftherobotarmchassisrotationofthesteeringwheel,sothattherobotarmcontrolspaceproblemintoaplaneproblem,andthenthepolygonandarmattitudecorrespondingtotheknowntargetpointinthecase,addingreasonableconstraints,Theangleofthepolygon,thatis,therotationangleofthesteeringgearcorrespondingtoeachjointofthemanipulator,thusrealizingthefixedpointcontrolofthearmend.Onthebasisofthecontrolalgorithmoftheendofthemanipulator,thealgorithmoftheendofthemanipulatorisrealized,whichmakestheartificialcontrolofthemanipulatormoreintuitiveandconcise.
Experimentsshowthatthe6-DOFmanipulatorandthe7-DOFmanipulatorcontrolalgorithmcanmeetthecontrolrequirements.TheuseofLabVIEWtowrite6-DOFroboticarmand7-DOFroboticarmremotecontrolsystemhasbeenbasicallycompleted,withthejoystickandwebcameratojoin,hasbeenabletoachievereal-timeoperationaccordingtotherobotbacktotherobotworkingenvironment,throughthehandleremoteThecontrolarmscansthegiventarget.
KeyWords：Robotarmcontrol;manipulatorinversekinematics;remotecontrol
目录
摘要 I
Abstract II
1引言 1
1.1课题研究背景 1
1.2设计的目的与 1
1.3主要设计要求 2
1.4国内外研究现状 2
1.5设计方案及主要设计内容 4
2机械臂控制算法的设计 5
2.1单舵机控制 5
2.26自由度机械臂控制算法的设计 5
2.2.1简化控制 6
2.2.2末端定点坐标控制 7
2.2.3末端平动控制 10
2.37自由度机械臂控制算法的设计 11
2.3.1简化控制 12
2.3.2末端定点坐标控制 12
2.3.3末端平动控制 15
2.4多自由度机械臂控制算法的设计 15
2.5机械臂控制算法的验证 17
3机械臂远程控制的实现 18
3.1机械臂基本组成及功能 18
3.2机械臂控制算法的实现 19
3.3机械臂远程控制实现 23
3.4控制手柄的加入 24
3.5误差分析 26
4机械臂设计的远景规划 27
4.1机械臂的姿态反馈 27
4.2机械臂的防水 28
4.3图像定位与机械臂控制算法的结合 28
4.4机械臂的避障算法的设计 29
设计总结 30
参考文献 31
致谢 33
1引言
1.1课题研究背景
当下海参捕捞主要靠人工完成，由海参捕捞员穿上潜水衣下潜到水面下七八米的深度进行捕捞作业，如图1.1所示。这种人工捕捞的方式有多种弊端。第一，海参捕捞员这份工作对水下工作技能要求很高，而且这种工作危险系数大，十分考验工作人员的毅力和勇气。第二，海参捕捞主要在冬季，冬季海水寒冷，工作环境很恶劣。第三，人工捕捞费用极高，海参捕捞员平均一个月酬金3万余元，增加了海参的成本，且即使如此高的工资，海参捕捞员也是供不应求。
图1.1人工捕捞海参现状
基于上述弊端，人工捕捞海参的方式将逐渐被机器捕捞海参取代。而目前虽然我国深海载人潜水器“蛟龙号”成绩斐然[1]，水下技术颇有建树，但是至今没有专门用在水下捕捞海参的相关机器。于是，为了解决海参捕捞问题，促进海参养殖行业的发展，拟设计并开发一款用于水下海参捕捞的机械臂，海参捕捞人员将船开到指定海域，投下机械臂等相关装置，在船上通过水下摄像头返回的图像操作机械臂进行海参捕捞。后期可与水下潜行器相结合，利用图像识别，自主完成海参捕捞，将人彻底从海参捕捞这种繁重危险的工作中解放出来。而完成这宏伟目标的第一步，是对机械臂进行远程控制。无论人工控制还是智能识别控制，如何控制机械臂才能直观，简洁，不易出错，是本文主要研究设计的方向。
1.2设计的目的与PC机，6自由度机械手臂（乐幻索尔科技）及其相关开发套件。
1.4国内外研究现状
机械臂属于机器人的一种，指能够按照程序或指令对物体完成夹取、传递等操作的机械设备，其历史可追溯到1913年。第一个机器手是美国福特公司于1913年研制出来，用于汽车生产线的上下料。从那时至今，机械臂由于工作效率高，重复性工作能力强，能够在恶劣的环境下工作等优点，被广泛地应用在工业生产中。机械臂的控制技术也随之得到国内外很多机构和个人的探究。
工业机械臂大致分为三个阶段。第一代机械臂主要用人工进行控制，控制是开环的，机械臂本身不具备识别物体的能力[3]。当下大多数机械臂仍处于此阶段。第二代机械臂集合了多种传感器进行反馈，具备视觉、触觉、听觉和一定程度的智能，有其独立的控制系统，这些都能辅助工作人员更好的控制机械臂。第三代机械臂能够彻底摆脱人工控制，独立完成工作中各种问题，将完全实现智能。
机械臂有很多种，按运动坐标形式分类，有直角坐标式机械臂，圆柱坐标式机械臂，球坐标式机械臂和多关节式机械臂大致四类，而且每一类中的机械臂也有多种，所以机械臂五花八门，这给机械臂控制算法的探究造成了一定的困难。很难有哪种机械臂的控制算法能满足多种类甚至全部种类的机械臂。所以，对机械臂控制算法的研究应针对机械臂的种类而具体探究设计。不同种类的机械臂，在不同的工作要求下，对应的最适合的控制算法也是不同的，但机械臂控制算法的研究思想却可以互相借鉴。
机械臂逆运动学的探究式机械臂控制算法的主要组成部分。机械臂的正向运动学，是给出机械臂的各个关节的角度从而确定整个机械臂的姿势，正向运动学相对简单，因为各个关节的角度和机械臂的姿势是一对一的关系。而机械臂的逆运动学是在机械臂姿势的某种或某些约束下（如给定机械臂末端位置）求解各个关节的角度。求出的解多数情况并不唯一，所以机械臂的逆运动学往往涉及到寻找最优解的问题。
近年来，国内对机械臂逆运动学的研究取得了很多成果。王海，蔡英凤，张为公提出一种逆运动学解析算法．在传统6维位姿矩阵的基础上，额外提出一个elbowangle新约束，利用位姿目标矩阵和该elbowangle共7维约束推导出7自由度机械臂逆运动学的一种可求得冗余解的解析算法[4]。徐军福利用Matlab仿真平台的RoboticsToolbox机器人工具箱进行仿真和验证逆运动学求解算法的正确性[5]。李宇飞，高朝辉和申麟使用了立体相机，对空间目标的位姿进行测量从而控制机械臂的抓取[6]。崔泽、张世兴和崔玉乾为了降低冗余自由度的逆运动学研究的难度，以7自由度机械臂为研究对象，采用SolidWorks/Motion模块的虚拟仿真，应用轨迹规划算法实现避障优化[7]。任晓琳和李洪文在D-H参数法中引入虚拟关节建立运动学模型，使用四种PSO算法进行计算，使机械臂的逆运动学计算更加有效[8]。张栩曼，张中哲，王燕波等人提出一种适用于大多数串联型节械臂的数值迭代逆运动学求解方法[9]。
国外机械臂逆运动学成果斐然。BSiciliano提出了基于Jacobian的闭环直接运动学（CLDK）算法来解决沿给定轨迹的直接运动学问题[10]。PChang使用拉格朗日乘数法得到具有冗余度的机械手逆运动学的闭式解决方案公式。在提出的方法中，解析运动方法中的重复性问题将不再存在[11]。BKarlik，SAydin使用ANN来实现六自由度机器人机械手的运动学解决方案[12]。ATHasan，AMSHamouda，NIsmail，HMAAAl-Assadi提出了一种使用人工神经网络ANN的收养学习策略，用于控制6D.O.F机器人机器人的运动，克服了反向运动学问题，这主要是手臂配置中的奇异点和不确定性[13]。Simas,H;Guenther,R;DaCruz,D.F.M;Martins,D设计一种基于数值积分求解并联机器人逆运动学的数值算法[14]。JesúsRubio,José;Aquino,Víctor;Figueroa,Maricela提出了两种获取移动机器人逆运动学的方法并进行了模拟验证[15]。Cavdar,Tugrul;Mohammad,M.;Milani,R.Alavi提出了一种基于改进的人造蜂群（ABC）的新方法来解决机器人手臂的逆运动学问题，并使用一些统计分析方法来确定修改后的ABC的权衡参数[16]。VADSMarques，RSGon?alves，JCMCarvalho，MPfurner讨论了轨迹插值和运动表示的奇异性的一些问题，并且使用双四元数表示作为避免这些问题的替代方案，并且获得确保运动轨迹的一般空间分析的模型[17]。
1.5设计方案及主要设计内容
本文拟走理论与实践相结合的研究设计路线，即理论上的机械臂控制算法的设计与实际用单片机和LabVIEW对机械臂控制算法加以实现相结合。前期查阅相关资料、了解并选择购买机械臂、完成机械臂的组装调试、设计6自由度机械臂控制算法并用单片机加以验证。中期学习LabVIEW[18]、用LabVIEW设计6自由度机械臂控制系统、增加自由度改装成7自由度机械臂、设计7自由度机械臂控制算法并用LabVIEW设计7自由度机械臂控制系统。后期对机械臂控制算法进行归纳和扩展、实现远程控制并完成毕业论文。
主要设计内容是多自由度机械臂逆运动学解算和机械臂控制系统的设计。
2机械臂控制算法的设计
2.1单舵机控制
对于由舵机实现自由度的机械臂而言，单舵机控制是最基础的控制，且所有此类机械臂的控制算法都是基于单舵机控制实现的。每一个舵机，对应机械臂的一个自由度。单舵机控制，即对机械臂上的舵机分别控制，使其旋转到指定角度，从而带动机械臂完成相应的动作。由于每个舵机控制独立，在无障碍的情况下，单舵机控制可以在机械臂工作范围内实现任意姿态（机械结构冲突除外），机械臂末端可以到达任意位置。综上所述，单舵机控制使机械臂控制灵活，同时也使机械臂控制复杂。
以挖掘机为例，多数液压挖掘机是5自由度，虽然挖掘机自由度是由液压带动实现的，其操作本质和单舵机控制下5自由度机械臂相似。挖掘机操作杆如图2.1所示。挖掘机驾驶操作难度不言而喻，由此类比，可见机械臂单舵机控制操作的复杂性。所以，为了更简单的控制多自由度机械臂，必须用算法对机械臂控制进行简化。
图2.1某液压挖掘机操作杆示意图
2.26自由度机械臂控制算法的设计
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.26自由度旋转机械臂舵机及建系示意图
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.36自由度旋转机械臂方案一模型简化示意图
舵机6-6旋转角度即为φ0。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了6自由度旋转机械臂自动到给定的三维空间坐标点进行作业。
添加方案一的约束条件，6自由度旋转机械臂的控制方法计算简单，误差较小，工作范围与简化控制的工作范围大致相同，且经过实验验证此方法是可以实现的，但是需要在机械臂设计制造过程中注意对称机械臂肢节长度相等，其相等程度将影响机械爪到目标点的误差大小。
（2）方案二
当机械臂臂长并不对称相等，且无明显规律时，可采用方案二：连接D3D1，加入约束条件D3D1=k×r0,其中,此时四边形OD1D2D3全约束，如图2.4所示。求舵机6-3、舵机6-4和舵机6-5的旋转角度即求∠D3D2D1、∠D2D1O和∠D1OD3（θ-∠D1OD3即为舵机6-5的旋转角，而θ已知），结果如下：
(2.6)
(2.7)
(2.8)
图2.46自由度旋转机械臂方案二模型简化示意图
舵机6-6旋转角度即为φ0。根据舵机6-3到舵机6-6的旋转角度，便可以确定这四个舵机的PWM值，将信号同时发送给这四个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了6自由度旋转机械臂自动到给定的三维空间坐标点进行作业。当r0=L1+L2+L3时，D3D1=k×r0=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.37自由度机械臂控制算法的设计
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.57自由度旋转机械臂舵机及建系示意图
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)
加入约束条件D3H3⊥D4O于H3，D2H2⊥D4O于H2，D1H1⊥D4O于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.67自由度旋转机械臂方案一模型简化示意图
舵机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.77自由度旋转机械臂方案二模型简化示意图
舵机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）。此时，多边形OD1D2…Dn形状不固定，每一种形状都对应着机械臂的机械爪到达目标点时的姿态。为了使其全约束，提出以下添加约束条件的方案：
（1）方案一
当机械臂臂长对称相等时，即Li=Ln+1-i(1in-1,i为整数)时,可采取方案一：当n为奇数时，在ODn线段上取n-1个点H1、H2、……Hn-1DnO平行并二者有相同的对称轴。当n为偶数时，在DnO线段上取n-1个点H1、H2、…Hn-1，使,…,,…,,加入约束条件于Hi(1in-1,i为整数)。此时，多边形OD1D2…Dn全约束，且线段DnO对称轴即为线段所在直线。当多边形OD1D2…Dn全约束后，可以求出控制竖直平面内机械臂肢节运动的舵机的旋转角度，即多边形OD1D2…Dn的各个内角。
舵机N-N旋转角度即为φ0。根据舵机N-3到舵机N-N的旋转角度，便可以确定这N-2个舵机的PWM值，将信号同时发送给这N-2个舵机，使其各自转动到对应的理论计算角度，此时机械爪中心与目标点重合，实现了多自由度旋转机械臂自动到给定的三维空间坐标点进行作业。
（2）方案二
当机械臂臂长并不对称相等，且无明显规律时，可采用方案二：在多边形OD1D2…Dn中连接若干顶点将其分割成互不重叠的n-1个三角形，并对连接的这些线段的长度加以约束，是各个三角形都全约束，那么多边形OD1D2…Dn也被全约束。分割为三角形的方案有多种，由于∠Dn-1DnO不涉及舵机旋转角度，也不用计算其大小，为了简化后期计算，连接Dn与其他不相邻顶点，则有线段DnD1，DnD2，…，DnDn-26自由度机械臂的单舵机控制、简化控制、机械臂末端定点坐标自主控制和机械臂末端平动控制。选择并购买STM32F103单片机开发板，用mbed开发平台完成单片机编程和调试（注：mbed是一个基于ARMCortex-M系列的单片机开发平台）。单舵机控制、简化控制和机械臂末端平动控制是采集按键键值完成对应操作，而机械臂末端定点坐标自主控制是通过串口中断更改程序中目标点的坐标数组来完成对应操作。每当按键按下时，舵机都按预计旋转走位，控制准确。当从串口发送目标点的极坐标时，机械臂能在2秒内迅速到达目标点。从而得出结论：算法对6自由度机械臂的适应性很好。同时算法的准确性的确认也为下一步LabVIEW编程实现打下基础。
3机械臂远程控制的实现
3.1机械臂基本组成及功能
机械臂的商家，即乐幻索尔科技除了提供机械臂散件和舵机。舵机为数字舵机LD-1501MG，精度高，虚位小，180度可定位，有17KG扭力。该舵机是PWM脉宽型调节角度，周期20ms，0.5ms~2.5ms的脉宽高电平对应舵机0度~180度角度范围，且成线性关系。舵机驱动控制板采用500~2500数值对应舵机控制输出角度的0.5ms~2.5ms高电平脉冲，舵机驱动板数值与舵机旋转的角度有如下关系：
(3.1)
θ为舵机旋转角度，PWM为舵机驱动板数值。这样舵机的控制精度是3μs，在2000个脉宽范围内最小控制精度能达到0.3度。
十六路舵机驱动板如图3.1所示。舵机驱动板上有二次开发用的TTL串口。波特率9600，向串口发送数据帧，可以控制一个或几个舵机旋转角度和旋转到此角度的时间。通信协议如表3.1。
图3.1十六路舵机驱动板
表3.1控制板通信协议
帧头 数据长度 指令 参数 0x550x55 Length Cmd Prm1…PrmN
注1）0x55，表示有数据包到达。
注2）Length等于参数N+2。
注3）CMD_SERVO_MOVE，指令值3。该指令控制任意个舵机的转动。数据Length=控制舵机的个数3+5。
Prm1：要控制的舵机个数。
Prm2：时间低8位。
Prm3：时间高8位。
Prm4：舵机ID号。
Prm5：角度位置低8位。
Prm6：角度位置高8位。
Prm…：格式与参数4、5、63.2所示。二是通过串口将各个舵机PWM值按照舵机驱动板二次开发串口协议实时发送到驱动板的串口上，流程图如图3.3所示。
图3.2机械臂控制算法实现流程图
图3.3串口发送流程图
以6自由度机械臂的控制算法的实现为例，6自由度机械臂简化控制的实现如图3.4所示。图3.4中上图是舵机6-4与舵机6-3时时刻刻旋转相同角度，下图是舵机6-5的旋转。可见机械臂简化控制的实现是成功的。
图3.46自由度机械臂简化控制的实现
6自由度机械臂末端定点坐标控制的实现如图3.5所示。其中，输入的极坐标的数值分别是，到原点距离150mm，平面旋转角60°，仰角75°，从图中可知，机械臂已运动到对应位置，且机械臂末端机械爪的中心处正处在输入的极坐标的位置，可见机械臂末端定点坐标控制的实现是成功的。
图3.56自由度机械臂末端定点坐标控制的实现
6自由度机械臂末端平动控制的实现如图3.6所示。图3.6三个图中，机械臂末端机械爪中心分别沿z轴，y轴，x轴进行平动，而且平动轨迹有很好的线性度，可见机械臂末端平动控制的实现是成功的。
图3.66自由度机械臂末端平动控制的实现
3.3机械臂远程控制实现
因为需要远程控制机械臂，所以需要摄像头对机械臂工作画面进行采集并实时传输，使操作人员了解机械臂的工作环境和目标点的位置从而发出相应的控制指令。购买网络摄像头，将图像用网络进行传输。客户端程序大体分为三个线程：一是对网络摄像头的图像进行读取，二是根据操作人员输入量选择控制算法并进行计算，并将结果转换为各个舵机的PWM值，三是通过网络将各个舵机PWM值按照舵机驱动板二次开发串口协议实时发送到网口转串口的模块，该模块串口输出连接到驱动板的串口上。工作示意图如图3.4所示。
图3.4机械臂远程控制实现示意图
6自由度机械臂控制系统界面和7自由度远程控制系统界面如图3.5所示。
3.4控制手柄的加入
为了使机械臂控制更加简洁，更加人性化，加入了无线控制手柄，手柄如图3.6所示。手柄一共有14个按键。同一时刻只能返回一个按键的键值，即同时按多个键时，只有一个键是有效的。手柄是用2.4G无线将信号传到接收器，手柄接收器如图3.7所示。接收器将信号转成串口输出，再使用USB转串口模块将信号传到电脑，连接关系图如图3.8所示。
图3.6手柄示意图
图3.7手柄接收器示意图
图3.8手柄连接关系示意图
通过上位机软件可以修改手柄的键值。手柄控制下，7自由度机械臂控制系统有两个模式：单舵机控制和末端平动（考虑机械爪抓取姿态）。两种模式可以无缝切换，且任何时候都可以对机械臂进行复位。
单舵机控制的模式下，一共有7个舵机，每个舵机都有两个旋转方向，即顺时针和逆时针旋转，也是舵机PWM值的增和减。如各用一个按键，则需要14个按键，每次调整的步长也需要2个按键，加上复位和模式切换，一共需要18个按键，多于实际按键数量，所以此种按键定义不适合。另一种按键定义的方式是：用7个按键分别表示7个舵机，用2个键表示舵机PWM的增和减，用2个键表示舵机PWM调整的步长增和减。每次对某个舵机进行调整时，需要先按表示舵机号的按键，再按表示PWM增减的键调整该舵机的旋转角度。再加上复位和模式切换，一共需要13个键，实际按键数量正好能满足。
末端平动控制的模式下，一共有三个轴需要调整，处于直观控制的角度考虑，用6个按键来实现平动。和机械爪抓取相关的舵机控制参照单舵机控制模式下的按键定义。
3.5误差分析
机械臂的误差来源主要有：理论上等长的臂长的误差、舵机校对的误差、舵机的旋转角度与理论值的误差、机械结构的摩擦和松弛、计算和取值时四舍五入的误差。为了减少误差对机械臂精度的影响，需要选择合适扭力的舵机并进行仔细校对，机械结构尺寸精细，连接紧密，关节处活动灵活，计算时多保留几位小数（本设计在实现时计算保留10位小数）。底部舵机（如6自由度机械臂的舵机6-5和7自由度机械臂的舵机7-6）承受的力矩最大，长时间工作下最容易出现松动或偏差。如没有合适扭力的舵机，可以在该位置同时使用两个相同的舵机进行驱动，这两个舵机控制信号一致，旋转角度一致，这样可以平分力矩，既减小误差，又能延长舵机使用寿命。在实验中，对于给定的坐标点，机械臂能在2秒内准确到达该点，误差不超过2mm，完全满足需要。
4机械臂设计的远景规划
4.1机械臂的姿态反馈
现在虽然实现了6自由度和7自由度机械臂的远程控制，但是整个机械臂的控制系统是开环控制系统。通过算法计算出各个舵机的角度，并将角度信号发给舵机，信号流动一直是单向的，也就是说我们并不知道舵机是否按照给定的信号旋转到了理论的角度。因为舵机是消耗品，随工作时间越长，出现偏差的可能性越来越大。距离底盘近的舵机出现偏差后，会对机械臂末端的精度造成很大影响，而这些偏差，是很难仅通过网络摄像头传过来的实时图像判断出来。再者，当机械臂在工作中遇到了障碍，使其卡在某一位置，某个或某些舵机堵转发热。不仅使机械臂不能正常工作，又会缩短舵机的使用寿命。
为了解决上述问题，实时了解机械臂的工作状况，必须使用传感器对机械臂的工作状态进行信息采集反馈。本文提供一种传感器采集反馈方案，即使用六轴惯导模块固定在末端机械臂肢节上，采集其倾角数据并与理论值做比较，如果与理论值偏差过大，则机械臂遇障或某舵机已损坏。之所以将惯导固定在末端肢节上，是因为机械臂末端肢节的倾角是所有走位舵机倾角的叠加。其中任一个走位舵机与理论角度偏差较大都会影响到机械臂末端肢节的倾角与其理论角度的偏差。此种方案可以检测出绝大多数机械臂运行故障，而且成本较低，可以列入日后的开发计划中。
六轴惯导模块的选择如图4.1，是淘宝商家提供的MPU6050模块，其体积小，可以很方便地固定在机械臂末端肢节上，精度高，通过串口将角度数据传给单片机，单片机与理论值比较后再将比较结果传给上位机，完成机械臂工作状态的信息反馈。
图4.1六轴惯导模块实物图
4.2机械臂的防水
由于以后的研发目的是为了将机械臂用于水下的海参捕捞，所以机械臂的防水将是以后重点探究环节之一。防水要求主要有以下两点：
机械臂全部应该都能浸泡在20米水深下进行工作。海参养殖的水域比较浅，大约是10米水深左右。机械臂在水下工作，要防止水侵入到舵机或其他电路元件中，损坏机械臂及其控制系统。
机械臂各关节不能因防水而缩小其活动范围或降低各关节的灵活性。这就需要在做防水时要以机械臂能在水下正常工作为前提。由于舵机的扭矩有限，在水下工作受水压影响，如果防水措施也会增加舵机旋转阻力，那么舵机的负载将会加大，不利于机械臂的正常工作和舵机的使用寿命。
防水目前主要有两个方向。第一个方向是使用大扭矩的防水舵机，这样舵机防水不需要考虑，只需要考虑连接线和舵机驱动板的防水即可。目前市面上的舵机防水等级恐无法实现其完全浸泡在10米深的水下进行工作，而且防水舵机最大扭力25KG，在实验前不知是否能满足在水下工作的需要。但是，随着科技的进步，舵机制造业的发展，相信迟早会有防水等级更高，扭力更大的舵机问世。第二个方向是将机械臂整体包装起来，就像给机械臂带上加长手套。这种方式下，舵机的防水将不再重要，重要的是“手套”的材质，必须柔韧，紧密，不易破碎，具有一定的拉伸性，耐水泡。“手套”必须做的大些，这样可以使机械臂活动不受手套束缚。需注意海底尖锐物体划破“手套”或机械臂本身运动关节夹伸“手套”导致其破裂。
4.3图像定位与机械臂控制算法的结合
为了使机械臂能自主工作，不再依赖人工控制，需要对被抓取的目标进行定位。定位的方式有很多种，如回声定位、蓝牙定位等。但是考虑到以后机械臂在水下抓取海参的工作环境，定位必须在对目标识别的基础上进行，而图像识别能较好的区分出海参和其所处的背景环境。所以，定位宜采取图像识别后定位的方式。常见的图像定位方式是双目视觉定位，用两个摄像头从不同角度对同一物体进行图像采集，就像人的双眼，在对图像信息交互中可得知物体的方位、大小和形状等信息。由双目视觉定位可以给出目标点的位置信息，进而引导机械臂对目标点进行抓取。
目前，图像处理已发展了数年，有比较成熟的开发工具。OpenCV是一个基于BSD许可（开源）发行的跨平台计算机视觉库，被广泛应用于人机互动、物体识别、运动跟踪、机器视觉等领域，OpenCV提供的视觉处理算法非常丰富，并且它部分以C语言编写，加上其开源的特性，可以满足机械臂二次开发的需要。LabVIEW本身也提供的Vision库函数，可以直接调用编程。
如定位的精度不能满足机械臂直接到指定坐标点进行作业，可以使用模糊的定位方式引导机械臂。即图像定位不能提供准确的目标点的坐标，但是可以判断出机械臂末端与目标点的相对位置，再使用机械臂末端平动或机械臂简化控制的模式不断调整机械臂，辅以图像识别不断纠正，使机械臂末端与目标点重合。这种方式对定位的精度较低，定位算法相对简单，缺点是机械臂不能一步到位，工作效率低。
如定位的精度可以到达误差在2mm之内，则可以由图像定位直接提供目标点相对于机械臂坐标系下的三维坐标，再由机械臂按照机械臂末端定点坐标控制的算法直接一步到位，如目标是运动的并且图像定位处理较快，则可以实现机械臂的目标跟随。
4.4机械臂的避障算法的设计
本文所有对机械臂控制算法都是在无障碍情况下设计的，而现实中，机械臂在水下工作，不可能是无障碍的理想情况。本文在7自由度机械臂末端相关算法中，牺牲了用于走位的舵机7-3，将其用于末端机械爪的抓取姿态的调整，这将解决部分目标点受附近障碍影响的抓取问题。但是面对更加复杂的障碍时，人工控制下只有单舵机控制才能满足规避各种障碍，因为虽然单舵机控制复杂繁琐，但是只有单舵机控制才能发挥机械臂的所有灵活性，使其实现任意姿态。
若让机械臂在自主工作时实现避障，首先图像识别定位必须在准确提供目标点坐标信息的同时，提供障碍物的坐标，形状，尺寸并准确判断其轮廓。之后由计算机模拟机械臂抓取目标点的姿态，与无障碍不同的是，计算机还必须模拟计算出机械臂完成抓取目标点的姿态的路径规划。在计算出不同阶段各个舵机的角度及其变化，发送给各个舵机，实现避障。
设计总结
本设计包括了6自由度机械臂和7自由度机械臂的控制算法的设计及其归纳扩展出的此类机械臂的控制算法。同时，本设计基本全部实现了此类6自由度机械臂和7自由度机械臂的人工控制，其中简化控制和末端平动控制将大大降低机械臂的操作难度，节约操作时间，尤其是在舵机数量较多，且工作环境简单的情况下。而末端定点坐标控制将会被应用于之后的智能控制的探究中，只要解决目标识别和定位，机械臂会自动到该点进行作业。
参考文献
徐芑南,张海燕.蛟龙号载人潜水器的研制及应用[J].科学:上海,2014,66(2):11-13.
王振华,王萍萍,马兴瑞,等.平面2R机械臂周期性运动分析与仿真[J].振动工程学报,2006,19(2):189-194.
郑珊珊.基于ARM处理器的钻孔机械臂控制系统研究[D].江西理工大学,2009.
王海,蔡英凤,张为公.一种7DOF机械臂逆运动学解析算法及其应用[J].江苏大学学报自然科学版,2011,32(3):254-259.
徐军福.一种实用的六自由度机械臂逆运动学解算与验证[J].机械工业标准化与质量,2016(1):28-30.
李宇飞,高朝辉,申麟.基于视觉的机械臂空间目标抓取策略研究[J].中国科学:技术科学,2015,45(1):31-35.
崔泽,张世兴,崔玉乾.冗余度机器人运动轨迹规划问题的仿真研究[J].计算机仿真,2016,33(3):268-273.
任晓琳,李洪文.复杂多关节机械臂建模及逆运动学比较分析[J].吉林大学学报(信息科学版),2016,34(6):753-760.
张栩曼,张中哲,王燕波,等.基于空间六自由度机械臂的逆运动学数值解法[J].导弹与航天运载技术,2016(3):81-84.
SicilianoB.TheTriceptrobot:Inversekinematics,manipulabilityanalysisandclosed-loopdirectkinematicsalgorithm[J].Robotica,1999,17(17):437-445.
ChangP.Aclosed-formsolutionforinversekinematicsofrobotmanipulatorswithredundancy[J].Robotics&AutomationIEEEJournalof,1987,3(5):393-403.
KarlikB,AydinS.Animprovedapproachtothesolutionofinversekinematicsproblemsforrobotmanipulators[J].EngineeringApplicationsofArtificialIntelligence,2000,13(2):159-164.
HasanAT,HamoudaAMS,IsmailN,etal.Anadaptive-learningalgorithmtosolvetheinversekinematicsproblemofa6DOFserialrobotmanipulator[J].AdvancesinEngineeringSoftware,2006,37(7):432-438.
SimasH,GuentherR,CruzDFMD,etal.AnewmethodtosolverobotinversekinematicsusingAssurvirtualchains[J].Robotica,2009,27(7):1017-1026.
RubioJDJ,AquinoV,FigueroaM.Inversekinematicsofamobilerobot[J].NeuralComputingandApplications,2013,23(1):187-194.
?avdar,Tu?rul,MohammadM,MilaniRA.ANewHeuristicApproachforInverseKinematicsofRobotArms[J].JournalofComputational&TheoreticalNanoscience,2013,19(1):329–333(3).
MarquesVADS,Gon?alvesRS,CarvalhoJCM,etal.HumanoidRobot''sInverseKinematicsUsingAlgebraicGeometry[C]//IEEE,2015.
吴成东,孙秋野,盛科.LabVIEW虚拟仪器程序设计及应用[M].人民邮电出版社,2008.
致谢
时间飞逝，转眼间就要毕业了。在大连理工大学攻读学士的时光是我人生中这二十几年
在大学期间，唐达老师是我的领路人，无论是在科研、项目锻炼还是生活上唐老师都教会我许多方法和道理我的，唐老师的教导。
最后感谢我的父母亲人，你们一直是我的坚强后盾，无论是精神还是物质，你们都让我在研究生期间没有后顾之忧，以后我也会努力工作，积极乐观的生活，回报你们的付出。
大连理工大学毕业设计（论文）格式规范
论文辅导老师扣扣：依零叁零武贰溜零武叁
-II-
-I-
论文辅导老师扣扣：依零叁零武贰溜零武叁
-IV-
大连理工大学毕业设计（论文）格式规范
机械臂远程控制的设计与实现
2
–3–
|
|
