智能搬运机器人运动控制算法实现与路径规划实现毕业论文.doc

智能搬运机器人运动控制算法实现与路径规划实现毕业论文目 录中文摘要英文摘要1 绪论11.1 背景介绍11.2课题现状11.3 论文的研究思路2 1.4 论文的研究内容22 桁架式机器人23 程序代码53.1 二轴主程序63.2 三轴主程序37I1.绪论1.1 背景简介传统的工业机器人常用于搬运、喷漆、焊接和装配工作。工业现场的很多重体力劳动必将由机器代替,这一方面可以减轻工人的劳动强度,另一方面可以大大提高劳动生产率。搬运机器人是可以进行自动化搬运作业的工业机器人。最早的搬运机器人出现在1960年的美国，Versatran和Unimate两种机器人首次用于搬运作业。搬运作业是指用一种设备握持工件，是指从一个加工位置移到另一个加工位置。搬运机器人可安装不同的末端执行器以完成各种不同形状和状态的工件搬运工作，大大减轻了人类繁重的体力劳动。目前世界上使用的搬运机器人逾10万台，被广泛应用于机床上下料、冲压机自动化生产线、自动装配流水线、码垛搬运、集装箱等的自动搬运。部分发达国家已制定出人工搬运的最大限度，超过限度的必须由搬运机器人来完成。搬运机器人是近代自动控制领域出现的一项高新技术，涉及到了力学，机械学，电器液压气压技术，自动控制技术，传感器技术，单片机技术和计算机技术等学科领域，已成为现代机械制造生产体系中的一项重要组成部分。它的优点是可以通过编程完成各种预期的任务，在自身结构和性能上有了人和机器的各自优势，尤其体现出了人工智能和适应性。 应用搬运机器人进行工作，这是直接减少人力的一个侧面，同时由于应用搬运机气人可以连续的工作，这是减少人力的另一个侧面。因此，在自动化机床的综合加工自动线上，目前几乎都有搬运机气人，以减少人力和更准确的控制生产的节拍，便于有节奏的进行工作生产。可见，有效的应用搬运机器人，是发展机械工业的必然趋势机械手是提高劳动生产率,改善劳动条件,减轻工人劳动强度和实现工业生产自动化的一个重要手段,国内外都很重视它的应用和发展。搬运机器人是典型的机电一体化数字化装备，技术附加值很高，应用范围很广，作为先进制造业的支撑技术和信息化社会的新兴产业，将对未来生产和社会发展起越来越重要的作用。1.2 课题现状移动搬运机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标自主运动，从而完成一定功能的机器人系统。理想的自主移动机器人可以不需人的干预在各种环境中自主完成规定任务，具有较高的智能水平，但目前全自主的移动机器人还大多处于实验阶段，进入实用的多为自主移动机器人，通过人的干预在特定环境中执行各种任务，而遥控机器人则完全离不开人的干预。 智能搬运机器人是一类能够通过传感器、感知环境和自身状态，实现在有障碍物的环境中面向目标自主运动，从而完成一定功能的机器人系统。移动机器人技术研究综合了路径规划、导航定位、路径跟踪与运动控制等技术。涉及包括距离探测、视频采集、温湿度以及声光等多种外部传感器，作为移动机器人的输入信息。移动机器人的运动控制主要是完成移动机器人的运动平台，提供一种移动机器人的控制方式。性能良好的移动机器人运动控制系统是移动机器人运行的基础，能够服务于移动机器人研究的通用开发平台。1.3 论文的研究思路首先对桁架式机器人有较深刻的理解。对机器人码垛动作做路径规划。具体工作包括首先对货盘书贴包的尺寸进行测量，建立模型。之后根据模型设置机械爪的路径点。还将对搬运机器人手爪运动进行动力学建模与控制仿真。之后在机器人上验证动力学控制算法。1.4 论文的研究内容首先对搬运机器人进行建模。得出桁架式搬运机器人的正解与反解，建立搬运机器人的运动学模型。之后利用拉格朗日法建立机器人的动力学模型。使其能够适用于对不同载荷下的机器人。之后将根据建立的动力学模型，建立相应的控制器，对机器人运行的速度、位置进行控制。使其能够在最短时间内达到目标位置。本课题将对运动学正反解进行MATLAB计算仿真。验证运动学模型的准确性。还将对控制器进行MATLAB计算仿真。验证动力学模型的准确性与控制算法的有效性。最后将首先根据货盘与货物的位置设置机器人的路径，实现货物的搬运。2. 桁架式机器人桁架式机器人是以XYZ直角坐标系统为基本数学模型，以伺服电机、步进电机为驱动的单轴机械臂为基本工作单元，以滚珠丝杆、同步皮带、齿轮齿条为常用的传动方式所架构起来的机器人系统，可以完成在XYZ三维坐标系中任意一点的到达和遵循可控的运动轨迹。桁架式机器人机器人采用运动控制系统实现对其的驱动及编程控制，直线、曲线等运动轨迹的生成为多点插补方式，操作及编程方式为引导示教编程方式或坐标定位方式。机器人特点：1、自由度运动，每个运动自由度之间的空间夹角为直角；2、自动控制的，可重复编程，所有的运动均按程序运行；3、一般由控制系统、驱动系统、机械系统、操作工具等组成。4、灵活，多功能，因操作工具的不同功能也不同。5、高可靠性、高速度、高精度。6、可用于恶劣的环境，可长期工作，便于操作维修。 3. 程序代码3.1 二轴主程序sub_readset读取设置sub_readprg读取FLASH文件modbus寄存器初始化modbus_reg(415) = g_state状态显示modbus_reg(416) = runerror状态显示modbus_long(432) = g_totaltimes运行次数modbus_long(420) = g_maxloop最大循环次数 y以前是408modbus_long(434) = g_loops已加工次数modbus_long(444) = g_curline + 1当前点数modbus_long(446) = g_totallines点数modbus_reg(414) = g_speedper速度modbus_reg(450) = 10 窗口号modbus_long(826) = g_totallines运行时间计时runtime = vr(0)modbus_long(425)=runtimemodbus_reg(823) =zhktime -rsttime=100modbus_reg(830) = rsttime-modbus_reg(832) = sonmodbus_reg(834) = resetdonemodbus_reg(808) = danqimodbus_reg(708) = 3 低速modbus_reg(200) = 50 手动速度百分比modbus_bit(201) = 0sub_doguidespsub_upsetsub_setregtosub_showeditTrace enter task_hmiin输入扫描任务 ，主任务，扫描触摸屏状态，根据状态调用对应子过程处理task_hmiin: If (modbus_reg(400) 0) Then触摸屏有命令输入或外部IO输入 0- 3 curcmd = modbus_reg(400)读取触摸屏命令寄存器,暂时保存至curcmd if modbus_reg(400) = 11 or modbus_reg(400) = 21 or (1104 = modbus_reg(400) = 1100) then Fsd = 0 endif modbus_reg(400) = 0将命令寄存器清零 If curcmd = 30 Then手动速度变化通知命令 sub_doguidesp ElseIf curcmd = 31 Then ID变化通知命令 ElseIf curcmd = 11 Then 手动命令 调试完了 加 and runtj=1- If g_state = 1 Then modbus_reg(450) = 20跳转至手动界面 g_state = 4设置控制器状态 modbus_reg(415) = g_state End If if Fsd = 0 then modbus_reg(708) = 1设置为低速 g_guidespdPer = setlowsp trace g_guidespdPer=g_guidespdPer Fsd = 1 End If ElseIf curcmd = 14 Then 参数命令 If g_state = 1 Then modbus_reg(450) = 50跳转至参数设置界面 g_state = 7设置控制器状态 modbus_reg(415) = g_state sub_setregto调用子过程sub_setregto，更新参数 End If ElseIf curcmd = 21 Then 编辑命令 sub_doguidesp If g_state = 1 Theng_cureditline = 0 sub_togedit跳转至编辑界面 g_state = 6 modbus_reg(415) = g_state设置控制器状态 sub_showedit调用子过程sub_showedit End IfElseIf curcmd =18 Then 浏览命令If g_state = 1 or g_state = 6 Then 1-待机；2-回零；3-运行；4-手动；5-暂停；6-编辑；7-设置sub_savefile()sub_readprg()modbus_reg(450)=140 调至浏览界面g_state = 8modbus_reg(415)=g_state 设置控制器状态g_pagelines=0sub_look 调用子过程Endif ElseIf curcmd = 15 Then 返回命令 If g_state = 6 Then sub_readprg g_cureditline = 0 End If modbus_reg(450) = 10 If g_state 3 and g_state 5 Then g_state = 1 modbus_reg(415) = g_state设置控制器状态 End If ElseIf curcmd = 400 and resetdone =0 and in(3) = 0 and in(13) = 1 Then 启动 Trace enter home If g_state = 1 Then 待机下 g_state = 2 3月31添加 stoptask 1 runtask 1, task_home启动任务 End If ElseIf curcmd = 400 and runtj=0 and resetdone =1 and in(3) = 0 and in(13) = 1 Then 启动 Trace enter start OP(1,ON) stoptask 2 能否不来回停止再执行，任务2老是频繁启动停止 runtask 2, task_io DELAY(500) sub_start调用启动子过程 zhua=0 runtj=0 ElseIf curcmd = 420 and runtj=0 and resetdone =1 and in(3) = 0 Then 单启动 空运行 Trace enter start OP(1,ON) stoptask 2 能否不来回停止再执行，任务2老是频繁启动停止 runtask 2, task_io DELAY(500) krun = 1 空运行标志 sub_start调用启动子过程 zhua=0 ElseIf curcmd = 402 Then 暂停 If g_state = 3 Theng_command=1Trace enter pauseendif ElseIf curcmd = 410 and in(18)=1 ThenSON Trace enter stop OP(0,ON) ElseIf curcmd = 401 Then停止 krun = 0 空运行标志 liao1 = 0 liao2 = 0 quliao = 0 jiajin1 = 0 in17ok = 0 tuiok = 0 liaonum = 0 qi1 = 0 wei1 = 0 wei2 = 0 op(3,off) 关闭启动冲床 op(4,off) 关闭输出下一台可取料 AB1 = 0 stoptask 1 stoptask 2 rapidstop(2) Trace enter stop sub_stop调用停止子过程 stoptask 2 stoptask 1 ElseIf curcmd = 412 and in(3) = 0 and in(13) = 1 Then 回零 Trace enter home If g_state = 1 Then 待机下 g_state = 2 3月31添加 stoptask 1 runtask 1, task_home启动任务 End If ElseIf curcmd 400 Then sub_doset调用子过程 ElseIf curcmd 700 Then sub_dofileset调用子过程 ElseIf curcmd 800 Then sub_doedit调用子过程，编辑示教点 ElseIf curcmd 100 thenmodbus_reg(414) = 100 g_speedper = 100 endif if runtime vr(0) then vr(0) = runtime endif if modbus_bit(201) = 1 then runtime = 0 modbus_long(425)=runtime endif sub_scanio sub_scanid sub_guide GoTo task_hmiinEndsub sub_scanio()tempin = in(0,3)if lastin tempin thenif read_bit2(0,lastin)=0 and read_bit2(0,tempin)=1 and in(3) =0 and in(13)=1 then zhua =1modbus_reg(400) = 400 启动, elseif read_bit2(1,lastin)=0 and read_bit2(1,tempin)=1 thenmodbus_reg(400) = 402 暂停elseif read_bit2(2,lastin)=0 and read_bit2(2,tempin)=1 and in(3) =0 and in(13)=1 thenmodbus_reg(400) = 412 复位elseif read_bit2(3,lastin)=0 and read_bit2(3,tempin)=1 thenmodbus_reg(400) = 401 急停end iflastin = tempin当多个IO同时来的时候，可能会扫描丢失endif son=op(0)end sub回零任务，此处可以加入回零处理：如各轴的回零顺序等Sub task_home() 上电自动复位 AB1 = 0 For j = 0 To AXISNUM - 1 base (j)选择参与运动的轴 cancel停止 wait idle Next resetdone=0 modbus_reg(834) = resetdone modbus_ieee(80) = 0 modbus_ieee(82) = 0 modbus_ieee(84) = 0 modbus_ieee(86) = 0 g_zeropos(0) = 0 g_zeropos(1) = 0 g_zeropos(2) = 0 g_zeropos(3) = 0 setstartpos(0) = 0 setstartpos(1) = 0 setstartpos(2) = 0 setstartpos(3) = 0 base (1) fanzhuan speed(1) = g_setzerosp(1) datum(14) wait until idle(1) mpos(1) = mpos(1) - g_zeropos(1) base (0) fanzhuan speed(0) = g_setzerosp(0) datum(14) wait until idle(0) mpos(0) = mpos(0) - g_zeropos(0) g_state = 1 modbus_reg(415) = g_state resetdone=1 modbus_reg(834) = resetdone AB = 0 if fuwei2 = 1 and cishu1(2) = 1 then modbus_reg(400) = 400 启动, 上跳沿 ab11 = 1 endif AB = 0 fuwei2 = 0 modbus_long(432) = g_totaltimes运行次数 if rstnum1=111 then zhua=1 首次复位完成后，不能自动抓，当复位次数相等时的复位，要自动抓= rstnum1=0 endif rstnum=0 每次复位 End 停止任务End Sub刷新运动程序号Sub sub_scanid()If g_state = 1 And g_curfileid modbus_reg(412) Then当前文件号和触摸屏文件号不一致，而且当前状态为待机状态 If modbus_reg(412) 100 Or modbus_reg(412) 1 Then范围1-16：判断文件号是否在1-7范围内 ，0用于 存储控制参数 modbus_reg(412) = g_curfileid文件号不再范围内，置为当前文件号 Else g_curfileid = modbus_reg(412)文件号在范围内，写入当前文件号全局变量g_curline = 0modbus_reg(444)=g_curline +1 sub_readprg运行读文件任务：从FLASH中读取数据 End IfElse modbus_reg(412) = g_curfileid刷新HMI文件号显示End IfEnd Sub手动运动扫描Sub sub_guide()If g_state 3 and in(3) = 0 Then For k = 0 To AXISNUM - 1 tempk = k * 2 base (k)if modbus_bit(305+ tempk) then手动前进, 16以上的输入做示教 speed = g_guidespdPer * setspeed(k) * modbus_reg(200) / 10000手动速度计算 示教*最高速度 vmove(1)BASE轴列表正向运动 g_ifguide(k) = Trueelseif modbus_bit(306+tempk) then手动后退 speed = g_guidespdPer * setspeed(k) * modbus_reg(200) / 10000手动速度计算 vmove(-1)BASE轴列表反向运动 g_ifguide(k) = True ElseIf g_ifguide(k) Then停止 cancelBASE轴列表减速停止 g_ifguide(k) = False End If NextEnd IfEnd Sub其他子函数Sub sub_readprg()读取数据：从Flash 段中读取存取的数据fileflag = 0 判断是否有读取正确的文件flash_read g_curfileid, fileflag读取当前文件的文件标志If fileflag 561234 Then文件标志不正确fileflag = 561234文件标志g_totallines = 0modbus_long(446) = g_totallines点数Else 文件标志正确，读取文件Trace read fileflash_read g_curfileid, fileflag, g_totallines, g_curfilename, g_atype, g_apara1, g_apara2, g_apara3, g_apara4, g_apara5,g_apara6modbus_long(446) = g_totallinesEnd IfEnd Sub其他子函数Sub sub_savefile()写入数据至FlashTrace save filefileflag = 561234置文件标号，在读取文件时需要检查文件标号是否正确flash_write g_curfileid, fileflag, g_totallines, g_curfilename, g_atype, g_apara1, g_apara2, g_apara3, g_apara4, g_apara5,g_apara6参数写入当前文件End SubSub sub_readset()读取参数设置：setflag=0flash_read 0, setflag读取当前文件的文件标志If setflag 561234 Then文件标志不正确,初始化对应参数 setflag = 561234 For i = 0 To AXISNUM - 1 setspeed(i) = 100 setacc(i) = 500 setdec(i) = 500 g_setzerosp(i) = 20 setstartpos(i) = 0 Next初始化插补参数 setvspeed = 100 setvacc = 500 setvdec = 500 sethighsp = 100 setmidsp = 40 setlowsp = 20Else 文件标志正确，读取参数 Trace read set flash_read 0, setflag, setspeed, setacc, setdec, g_setzerosp, setvspeed, setvacc, setvdec, setstartpos, sethighsp, setmidsp, setlowsp,zhktime,rsttime,cishu1,keep11End IfEnd Sub参数读取任务结束更新参数，单轴速度和插补速度要不断的设置Sub sub_upset() For i = 0 To AXISNUM - 1 base (i) accel = setacc(i)设置BASE轴加速度 decel = setdec(i)设置BASE轴减速度 Next sub_startposEnd Sub设置起始位置，修改坐标来实现, 不能在运动中调用Sub sub_startpos() For i = 0 To AXISNUM - 1 If g_zeropos(i) setstartpos(i) Then mpos(i) = mpos(i) - (setstartpos(i) - g_zeropos(i)直接修改至工件坐标 g_zeropos(i) = setstartpos(i) End If NextEnd Sub保存参数，与存储时的要一致Sub sub_saveset() Trace set saved setflag = 561234参数标志 flash_write 0, setflag, setspeed, setacc, setdec, g_setzerosp, setvspeed, setvacc, setvdec, setstartpos, sethighsp, setmidsp, setlowsp,zhktime,rsttime,cishu1,keep11 参数写入至当前文件号End SubSub sub_doguidesp()示教速度切换 If modbus_reg(708) = 1 Then当前为低速 modbus_reg(708) = 2点击后切换为中速 g_guidespdPer = setmidsp ElseIf modbus_reg(708) = 2 Then当前为中速 modbus_reg(708) = 3点击后切换为高速 g_guidespdPer = sethighsp Else当前为高速 modbus_reg(708) = 1设置为低速 g_guidespdPer = setlowsp End If trace g_guidespdPer=g_guidespdPerEnd SubSub sub_setregto()更新触摸屏参数显示 For i = 0 To AXISNUM - 1 modbus_long(16 + (i * 2) = setacc(i) modbus_long(144 + (i * 2) = setdec(i) modbus_long(32 + (i * 2) = setspeed(i) modbus_long(48 + (i * 2) = g_setzerosp(i) modbus_ieee(80 + (i * 2) = setstartpos(i) Next modbus_long(68) = setvspeed 轨迹速度 modbus_long(70) = setvacc 加速度 modbus_long(72) = setvdec 减速度 modbus_reg(129) = sethighsp modbus_reg(131) = setmidsp modbus_reg(133) = setlowsp modbus_reg(823) =zhktime modbus_reg(830) = rsttime modbus_reg(832) = son modbus_reg(834) = resetdoneEnd SubSub sub_setregfrom()从触摸屏读取参数至对应变量，更新参数设置 For i = 0 To AXISNUM - 1 setacc(i) = modbus_long(16 + (i * 2) setdec(i) = modbus_long(144 + (i * 2) setspeed(i) = modbus_long(32 + (i * 2) g_setzerosp(i) = modbus_long(48 + (i * 2) setstartpos(i) = modbus_ieee(80 + (i * 2) Next setvspeed = modbus_long(68) setvacc = modbus_long(70) setvdec = modbus_long(72) sethighsp = modbus_reg(129) setmidsp = modbus_reg(131) setlowsp = modbus_reg(133) zhktime=modbus_reg(823) rsttime=modbus_reg(830) son=modbus_reg(832) resetdone=modbus_reg(834) End Sub显示当前点Sub sub_showedit()If g_cureditline = g_totallines Then modbus_long(802) = g_cured