基于ＡＶＲ平台的六自由度仿人机械手臂控制算法研究ｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉ
基于ＡＶＲ平台的六自由度
仿人机械手臂控制算法研究
Ｒｅｓｅａｒｃｈｏｆ６一ＤＯＦＨｕｍａｎｏｉｄＲｏｂｏｔｉｃＡｒｍ
ＣｏｎｔｒｏｌＡｌｇｏｒｉｔｈｍＢａｓｅｄ
ｏｎ
ＡＶＲ
沈长青孔凡让刘永斌刘
方
（中国科学技术大学精密机械与精密仪器系，合肥２３００２７）
摘要：传统机械手臂各关节常用电机来驱动，其驱动系统较为复杂，设计难度较大。提出以ＡＶＲ为控制平台，以舵机作为机械臂各关节驱动，并着重介绍了基于舵机特有的工作方式而提出的一种可以对该机械臂各关
节运动速度、方向和运动冒：进行控制的算法。该算法同时还具备可扩展性等优点，即可以对由舵机驱动的更多自
由度的机械手臂进行实时控制。
关键词：ＡＶＲ平台
仿人机械手臂可控速算法
ＤｏＩ：１０．３９６９／ｊ．ｉｓｓｎ．１００７—０８０Ｘ．２０１０．０５．００７
Ａｂｓｔｒａｃｔ：Ｔｒａｄｉｔｉｏｎａｌｒｏｂｏｔｉｃ
ｅｓｓａｙ，ｉｔｒｅｆｅｒｅｄｔｏｔａｋｅＡＶＲ
ａ
ａｓ
ａｒｍｗａｓｄｒｉｖｅｎ
ｂｙｍｏｔｏｒｓ，ｗｈｏｓｅｓｙｓｔｅｍａｔｉｃｄｅｓｉｇｎｏｆｄｒｉｖｅｉｓｔｏｕｇｈ．Ｉｎｔｈｉｓ
ｕｓｅ
ｓｅｒｖｏ
ａｓ
ｔｈｅｃｏｎｔｒｏｌｐｌａｔｆｏｒｍａｎｄ
ｔｈｅｄｒｉｖｅｏｆｅａｃｈｓｅｃｔｏｒ，ｂｅｓｉｄｅｓ，ｉｔｐｒｏｐｏｓｅｄ
ａ
ｃｏｎｔｒｏｌａｌｇｏｒｉｔｈｍｔｈａｔ
ｃａｎ
ｃｏｎｔｒｏｌｔｈｅｓｐｅｅｄ，ｄｉｒｅｃｔｉｏｎａｎｄａｍｏｕｎｔ
ａｓ
ａｓ
ｆｏｃｕｓｂａｓｅｄ
ｏｎ
ｉｔｓｕｎｉｑｕｅｗｏｒｋｓｔｙｌｅ．Ａｎｄ
ｔｈｅｎｔｈｉｓｃｏｎｔｒｏｌｍｅｔｈｏｄｈａｄａｄｖａｎｔａｇｅｓｓｕｃｈｆｒｅｅｄｏｍ．
Ｋｅｙｗｏｒｄｓ：ＡＶＲｐｌａｔｆｏｒｍ
ｅｘｐａｎｓｉｂｉｌｉｔｙａｎｄ
ｃａｎ
ｃｏｎｔｒｏｌｔｈｅｉｎｓｔｉｔｕｔｉｏｎｓｗｉｔｈｍｏｒｅｄｅｇｒｅｅｓｏｆ
ｈｕｍａｎｏｉｄｒｏｂｏｔｉｃａｒｍ
ｓｐｅｅｄ－ｃｏｎｔｒｏｌｌａｂｌｅａｌｇｏｒｉｔｈｍ
０引言位置的干涉问题，因此，机器人机械手臂关节驱动系统的运动分析和控制方法已经成为研究中的重点问题之一。
本文在分析人体手臂各关节结构及长度比例分布的基础上，研制了一款六自由度仿人机械手臂，并利用ＡＶＲ控制平台设计了相应的控制系统，实现了对其运动速度、方向和运动量的实时控制。文中重点描述了对该机械手臂运动进行控制的算法思想、控制平台以及程序实现等。
目前机械手臂是机械人技术领域中应用最广泛的
自动化机械装置，在工业制造、医学治疗、娱乐服务、军事以及太空探索等领域都能见到它的身影。尽管它们的形态各有不同，但它们都有一个共同的特点，就是能够接受指令，较为精确地定位到三维（或二维）空间上的某一点进行作业。
运动控制在机械手臂研制过程中可谓一大难点，需综合考虑机械臂各部分的质量、转动惯量，合理控制其运动角速度以及加速度，同时还要避免各关节运动
作者简介：沈长青１９８７年生，硕士研究生。研究方向为智能机器人。
孙凡让教授，博士生导师。研究方向为状态检测与故障诊断等。
柏机电一体化Ｉ
２０１０．５
万方数据
嗣麓ｉ嗣ｉｉｉｉ蕊ｉｉｉｉ嗣窝ｉ焉嗣葡蕊嗣赫Ｒｅｓｅａｒｃｈ．Ｄｅｖｅｌｏｐｍｅｎｔｉ团圆嘲
ｌ仿人机械手臂简介
该六自由度仿人机械手臂根据人体手臂关节构造来布局自由度，其结构示意图如图１所示，主要分为肩部、肘部以及腕部三个部分共计六个自由度。与传统机械臂不同的是，各关节均通过可控旋转角度的舵机作为驱动电机实现，在驱动系统的设计上大大降低了难度，同时舵机０．８度的旋转精度满足普通场合下机械手臂的精度要求。机械手臂主体材料采用硬
铝、尼龙等轻质材料，以降低对舵机驱动扭矩的要求，
该机械臂在肩部设计了同定孔，用于与其他平台的衔
接同定。为了满足手臂各关节实现平稳运动所需的
驱动力，肩部两个自由度采用４２ｋｇｃｍ的大扭矩舵机，其他自由度采用１３ｋｇｃｍ的小型舵机，实物如图２
所示。
圈ｌ六自由度仿人机械
图２六自由度仿人
手臂自由度示意图
机械手臂实物
２算法设计２．１电机控制原理
舵机的控制信号通常是周期为２０ｍｓ的ＰＷＭ（ｐｕｌｓｅ－ｗｉｄｔｈ－ｍｏｄｕｌａｔｉｏｎ）方波信号，其中高电平的持续时间对应为舵机的转角，舵机与对应转角的关系如图３、图４所示。
圈３脉宽一转角关系圈
万方数据
∞∞∞加∞∞∞加加Ｏ
（ｍｓ）
图４舵机控制波形
２．２
算法思想
舵机的运动量和方向可以通过脉冲命令间的占空比来控制。但是就速度控制而言，如果我们以普通的
方式进行控制，例如让它运动到２００后再发一个５０。的
脉冲命令，它就由２０。直接以最大速度旋转至５００，而不
存在所谓的速度控制，而且由于负载等因素的影响，往
往会对舵机和运动机构造成很大的损害。为了实现这个旋转过程的柔和速度控制，我们让舵机按照某条曲线进行角度运动。为此，我们设计了图５的曲线，在从
ｎ到口所占用的时间间隔△ｔ内，舵机运动速度先由快变慢，然后由慢变快，该曲线对应一个关于时间ｆ的三
■
次函数：
ｐ（ｔ）＝ａｔ３＋ｂｔ２＋ｃｔ＋０。
（１）
圈５理想运动曲线
在理想情况下，ｔ。时刻的脉冲命令对应角度口。应该是ｐ（ｔ。），但是脉冲命令是离散量，而且脉冲之间有一定的时间间隔限制，即表现出舵机的追随特性（图
６）。因此，它所发出的角度命令只能在ｔ时刻逼近ｏ（ｔ）
２０１０．５Ｉ机电一体化们
基于ＡＶＲ平台的六自由度仿人机械手臂控制算法研究ｉｉ—＂——Ｉ—Ｉ————————Ｉ————ＩＩ————ＩＩ—ＩＩＩ～ｉＩＩｉ
的值。分析图６，当△中越小，所需要的追随时间ＡＴ
越短。
田６舵机追随特性圈
鉴于此，从微分的思想出发，将角度由ａ到口的时间△‘分为Ⅳ份，则第１｜｝份时间对应的角度应为
ｐ（ｔ。＋ｋＡｔ／Ｎ）
（２）
当Ⅳ＿００，这一系列的离散点无限逼近实际曲线，调整』、ｒ值，舵机从ａ角度运动到口所花的时间也相应发生了变化。具体做法如下：如果希望舵机在角度变化过程中按照图５曲线，先快后慢，然后再快地运动到角度终点／３，我们将其“微元化”，见图７。设符合图５曲线趋势且满足边界条件的三次函数为
吣，：竺｛过＋字㈩
圈７舵机运动理想曲线“徽元化。
经过图７的“微元化”，同时以舵机初始运动时刻为０时刻，即第Ｊ｜Ｉ步对应的时间为
…等
㈩
●０机电一体化Ｉ２０１０．５
万方数据
其中Ｊ『、ｒ表示从ａ角度运动到卢角度所经过的“步数”，将
去＝专
一＝一Ｉ】●
、’
△ｒⅣ
㈣
代入
吣，：竺｛过＋字㈤
得到
吣卜一＋字：迎盟掣＋掣吨（７）２Ｎ
２
’
根据这样的设计，还可产生其他多种曲线运动，例
如先快后慢、先慢后快等多种运动方式均可实现（见图
＝一＋～：仃
Ｉ，－
圈８先慢后快再慢运动方式
圈９先快后慢运动方式
盔．．
ＩｌｌＩＩＩＩＩＩＩｌｌｌｎｉｉｉｉｉｉｉｉｉｉ
赫Ｒｅｓｅａｒｃｈ．Ｄｅｖｅｌｏｐｍｅｎｔ
ｌ豳瞳—圜圜
图７和图８运动方式关于时间ｔ的三次函数０（ｔ）分别为：
ｏ（ｔ）＝（ｆｉ－ｏｔ）ｘ（３—２忐）（忐）＋Ｏｒ（８）ｏ（ｔ）＝（口一卢）×（忐一１）州．（９）
第ｋ步对应的角度
卟盟型型≥墨堕＋ｎ‘
（１０）
矿
盱堕型要生丛＋卢‘
盯
（１１）
３程序实现３．１控制平台
由ＡＴＭＥＬ公司研发出的ＡＶＲ单片机具有高速度、高保密性、低功耗的特点，已广泛地应用于军事、工
业、家用电器、智能玩具、便携式智能仪表和机器人制
作等领域。本系统中采用ＡＶＲ系列单片机中的ＡＴｍｅｇａｌ６Ｌ作为控制核心，内含可反复编程的Ｆｌａｓｈ程
序存储器、ＳＲＡＭ和ＥＥＰＲＯＭ两种数据存储器、定时器／计数器、方向可定义的Ｉ／Ｏ口及ＰＷＭ等丰富的内
部资源，算术运算功能强，软件编程灵活，自由度大，可
用软件编程实现各种算法和逻辑控制。３．２关键程序
针对ＡＴｍｅｇａｌ６Ｌ芯片的处理特点，我们采用每次驱动四只舵机和分批驱动的方式对机械手臂进行驱动，驱动函数定义如下：
ｒｏｔａｔｅ（ｓｔｒｕｃｔｓｅｒｖｏ
ｓｅｒｖｏ一１，ｕｎｓｉｇｎｅｄ
ｉｎｔ
ａｎｇｌｅ一１，……）其中：ｓｔｒｕｃｔ
８ｅ／＇ｖｏ
ｓｅｌ＇ｖｏ一１为待转动的舵机变量，
ａｎｇｌｅ一１为舵机的旋转角度对应的计数器值。ａｎｇｌｅ—ｎ
参数计算公式如下：
ａｎｇｌｅ—ｎ＝ＴＩＭＥｌ一ＢＥＧＩＮ＋ＴＩＭＥｌ—５００ＭＳ＋ａｎｇｌｅ宰１７８
单片机中计时通过计数器实现，上式中ＴＩＭＥｌ一ＢＥＧＩＮ对应计数器３ｍｓ溢出时间，ＴＩＭＥｌ＿５００ＭＳ对应
计数器０．５ｍｓ时间，２ｍｓ时间对应计数器的计数值为
ＴＩＭＥｌ＿．５００ＭＳ幸４（３２０４０，计数器１分频），然后将该值
万方数据
分成１８０份，每份是１７８，于是得出上式。
为了方便对多只舵机的动作调试，可以先定义一
个二维矩阵：
ｃｏｎｓｔ
ｐｒｏｇ＿ｕｅｈａｒ
ｍｏｒｅｌ—ｍａｔｒｉｘ［
］［ＣＯＬＵＭＮ］２
｛．…・・｝；
对于Ｍ只舵机的驱动函数，即可定义一个数组为肼＋２列的矩阵，其中前Ｍ列对应各时刻各舵机的角度（０—１８０度），第肘＋ｌ列表示由该行角度运动到下一行角度所分的步数，第肘＋２列对应由该行角度运动
到下一行角度的速度属性（即对先快后慢、先慢后快等
运动方式的设定）。执行程序时即可根据这些数据判
断第ｋ步需要给什么样的脉冲命令。
最后建立矩阵扫描函数如下：
ｓｃａｎ（ｅｏｎｓｔ
ｐｒｏｇ—ｕｃｈａｒ
ｍａｔｒｉｘ［］［］，ｕｎｓｉｇｎｅｄ
ｃｈａｒ
ｂｅｇｉｎ，ｕｎｓｉｇｎｅｄｃｈａｒｅｎｄ）
三个参数分别对应保存运动数据的数组名称、起始扫描行、结束扫描行。
上述程序表示六自由度机械臂依次按照Ｍ１，Ｍ２，
Ｍ４，Ｍ３的运动方式（Ｍｎ代表的方式已先定义好）从
第一行数据对应角度依次运行到第五行数据对应角度，从而形成ｍｏｖｅｌ运动。其他运动的控制依靠重新
定义数组和运动函数即可实现。
综合以上分析，单片机程序执行流程图如图１０所示。
。
ｊ’初始ｆ￡毽
《扁动ｉ铷一端口一
“
‰帝存嚣锈；
图ｌＯ单片机程序执行流程图
这样，反复执行这个函数若干次就可以实现对多
只舵机的并行控制。
４
结语
通过以上的分析及程序构架，大大方便了对多自由度机械臂运动的控制，而且易于扩展，该控制程序利用通用ＡＶＲ处理器的片内资源，发挥了它潜在的计算能力，大大降低代码冗余度的过程。目前该机械臂运行平稳，响应速度快，能很好地满足科研要求。
２０１０．５Ｉ机电一体化柏
基于AVR平台的六自由度仿人机械手臂控制算法研究
作者：作者单位：刊名：英文刊名：年，卷(期)：
沈长青， 孔凡让， 刘永斌， 刘方
中国科学技术大学精密机械与精密仪器系,合肥,230027机电一体化MECHATRONICS 2010(5)
本文链接：http://d.wanfangdata.com.cn/Periodical_jdyth201005009.aspx
基于ＡＶＲ平台的六自由度仿人机械手臂控制算法研究ｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉｉ
基于ＡＶＲ平台的六自由度
仿人机械手臂控制算法研究
Ｒｅｓｅａｒｃｈｏｆ６一ＤＯＦＨｕｍａｎｏｉｄＲｏｂｏｔｉｃＡｒｍ
ＣｏｎｔｒｏｌＡｌｇｏｒｉｔｈｍＢａｓｅｄ
ｏｎ
ＡＶＲ
沈长青孔凡让刘永斌刘
方
（中国科学技术大学精密机械与精密仪器系，合肥２３００２７）
摘要：传统机械手臂各关节常用电机来驱动，其驱动系统较为复杂，设计难度较大。提出以ＡＶＲ为控制平台，以舵机作为机械臂各关节驱动，并着重介绍了基于舵机特有的工作方式而提出的一种可以对该机械臂各关
节运动速度、方向和运动冒：进行控制的算法。该算法同时还具备可扩展性等优点，即可以对由舵机驱动的更多自
由度的机械手臂进行实时控制。
关键词：ＡＶＲ平台
仿人机械手臂可控速算法
ＤｏＩ：１０．３９６９／ｊ．ｉｓｓｎ．１００７—０８０Ｘ．２０１０．０５．００７
Ａｂｓｔｒａｃｔ：Ｔｒａｄｉｔｉｏｎａｌｒｏｂｏｔｉｃ
ｅｓｓａｙ，ｉｔｒｅｆｅｒｅｄｔｏｔａｋｅＡＶＲ
ａ
ａｓ
ａｒｍｗａｓｄｒｉｖｅｎ
ｂｙｍｏｔｏｒｓ，ｗｈｏｓｅｓｙｓｔｅｍａｔｉｃｄｅｓｉｇｎｏｆｄｒｉｖｅｉｓｔｏｕｇｈ．Ｉｎｔｈｉｓ
ｕｓｅ
ｓｅｒｖｏ
ａｓ
ｔｈｅｃｏｎｔｒｏｌｐｌａｔｆｏｒｍａｎｄ
ｔｈｅｄｒｉｖｅｏｆｅａｃｈｓｅｃｔｏｒ，ｂｅｓｉｄｅｓ，ｉｔｐｒｏｐｏｓｅｄ
ａ
ｃｏｎｔｒｏｌａｌｇｏｒｉｔｈｍｔｈａｔ
ｃａｎ
ｃｏｎｔｒｏｌｔｈｅｓｐｅｅｄ，ｄｉｒｅｃｔｉｏｎａｎｄａｍｏｕｎｔ
ａｓ
ａｓ
ｆｏｃｕｓｂａｓｅｄ
ｏｎ
ｉｔｓｕｎｉｑｕｅｗｏｒｋｓｔｙｌｅ．Ａｎｄ
ｔｈｅｎｔｈｉｓｃｏｎｔｒｏｌｍｅｔｈｏｄｈａｄａｄｖａｎｔａｇｅｓｓｕｃｈｆｒｅｅｄｏｍ．
Ｋｅｙｗｏｒｄｓ：ＡＶＲｐｌａｔｆｏｒｍ
ｅｘｐａｎｓｉｂｉｌｉｔｙａｎｄ
ｃａｎ
ｃｏｎｔｒｏｌｔｈｅｉｎｓｔｉｔｕｔｉｏｎｓｗｉｔｈｍｏｒｅｄｅｇｒｅｅｓｏｆ
ｈｕｍａｎｏｉｄｒｏｂｏｔｉｃａｒｍ
ｓｐｅｅｄ－ｃｏｎｔｒｏｌｌａｂｌｅａｌｇｏｒｉｔｈｍ
０引言位置的干涉问题，因此，机器人机械手臂关节驱动系统的运动分析和控制方法已经成为研究中的重点问题之一。
本文在分析人体手臂各关节结构及长度比例分布的基础上，研制了一款六自由度仿人机械手臂，并利用ＡＶＲ控制平台设计了相应的控制系统，实现了对其运动速度、方向和运动量的实时控制。文中重点描述了对该机械手臂运动进行控制的算法思想、控制平台以及程序实现等。
目前机械手臂是机械人技术领域中应用最广泛的
自动化机械装置，在工业制造、医学治疗、娱乐服务、军事以及太空探索等领域都能见到它的身影。尽管它们的形态各有不同，但它们都有一个共同的特点，就是能够接受指令，较为精确地定位到三维（或二维）空间上的某一点进行作业。
运动控制在机械手臂研制过程中可谓一大难点，需综合考虑机械臂各部分的质量、转动惯量，合理控制其运动角速度以及加速度，同时还要避免各关节运动
作者简介：沈长青１９８７年生，硕士研究生。研究方向为智能机器人。
孙凡让教授，博士生导师。研究方向为状态检测与故障诊断等。
柏机电一体化Ｉ
２０１０．５
万方数据
嗣麓ｉ嗣ｉｉｉｉ蕊ｉｉｉｉ嗣窝ｉ焉嗣葡蕊嗣赫Ｒｅｓｅａｒｃｈ．Ｄｅｖｅｌｏｐｍｅｎｔｉ团圆嘲
ｌ仿人机械手臂简介
该六自由度仿人机械手臂根据人体手臂关节构造来布局自由度，其结构示意图如图１所示，主要分为肩部、肘部以及腕部三个部分共计六个自由度。与传统机械臂不同的是，各关节均通过可控旋转角度的舵机作为驱动电机实现，在驱动系统的设计上大大降低了难度，同时舵机０．８度的旋转精度满足普通场合下机械手臂的精度要求。机械手臂主体材料采用硬
铝、尼龙等轻质材料，以降低对舵机驱动扭矩的要求，
该机械臂在肩部设计了同定孔，用于与其他平台的衔
接同定。为了满足手臂各关节实现平稳运动所需的
驱动力，肩部两个自由度采用４２ｋｇｃｍ的大扭矩舵机，其他自由度采用１３ｋｇｃｍ的小型舵机，实物如图２
所示。
圈ｌ六自由度仿人机械
图２六自由度仿人
手臂自由度示意图
机械手臂实物
２算法设计２．１电机控制原理
舵机的控制信号通常是周期为２０ｍｓ的ＰＷＭ（ｐｕｌｓｅ－ｗｉｄｔｈ－ｍｏｄｕｌａｔｉｏｎ）方波信号，其中高电平的持续时间对应为舵机的转角，舵机与对应转角的关系如图３、图４所示。
圈３脉宽一转角关系圈
万方数据
∞∞∞加∞∞∞加加Ｏ
（ｍｓ）
图４舵机控制波形
２．２
算法思想
舵机的运动量和方向可以通过脉冲命令间的占空比来控制。但是就速度控制而言，如果我们以普通的
方式进行控制，例如让它运动到２００后再发一个５０。的
脉冲命令，它就由２０。直接以最大速度旋转至５００，而不
存在所谓的速度控制，而且由于负载等因素的影响，往
往会对舵机和运动机构造成很大的损害。为了实现这个旋转过程的柔和速度控制，我们让舵机按照某条曲线进行角度运动。为此，我们设计了图５的曲线，在从
ｎ到口所占用的时间间隔△ｔ内，舵机运动速度先由快变慢，然后由慢变快，该曲线对应一个关于时间ｆ的三
■
次函数：
ｐ（ｔ）＝ａｔ３＋ｂｔ２＋ｃｔ＋０。
（１）
圈５理想运动曲线
在理想情况下，ｔ。时刻的脉冲命令对应角度口。应该是ｐ（ｔ。），但是脉冲命令是离散量，而且脉冲之间有一定的时间间隔限制，即表现出舵机的追随特性（图
６）。因此，它所发出的角度命令只能在ｔ时刻逼近ｏ（ｔ）
２０１０．５Ｉ机电一体化们
基于ＡＶＲ平台的六自由度仿人机械手臂控制算法研究ｉｉ—＂——Ｉ—Ｉ————————Ｉ————ＩＩ————ＩＩ—ＩＩＩ～ｉＩＩｉ
的值。分析图６，当△中越小，所需要的追随时间ＡＴ
越短。
田６舵机追随特性圈
鉴于此，从微分的思想出发，将角度由ａ到口的时间△‘分为Ⅳ份，则第１｜｝份时间对应的角度应为
ｐ（ｔ。＋ｋＡｔ／Ｎ）
（２）
当Ⅳ＿００，这一系列的离散点无限逼近实际曲线，调整』、ｒ值，舵机从ａ角度运动到口所花的时间也相应发生了变化。具体做法如下：如果希望舵机在角度变化过程中按照图５曲线，先快后慢，然后再快地运动到角度终点／３，我们将其“微元化”，见图７。设符合图５曲线趋势且满足边界条件的三次函数为
吣，：竺｛过＋字㈩
圈７舵机运动理想曲线“徽元化。
经过图７的“微元化”，同时以舵机初始运动时刻为０时刻，即第Ｊ｜Ｉ步对应的时间为
…等
㈩
●０机电一体化Ｉ２０１０．５
万方数据
其中Ｊ『、ｒ表示从ａ角度运动到卢角度所经过的“步数”，将
去＝专
一＝一Ｉ】●
、’
△ｒⅣ
㈣
代入
吣，：竺｛过＋字㈤
得到
吣卜一＋字：迎盟掣＋掣吨（７）２Ｎ
２
’
根据这样的设计，还可产生其他多种曲线运动，例
如先快后慢、先慢后快等多种运动方式均可实现（见图
＝一＋～：仃
Ｉ，－
圈８先慢后快再慢运动方式
圈９先快后慢运动方式
盔．．
ＩｌｌＩＩＩＩＩＩＩｌｌｌｎｉｉｉｉｉｉｉｉｉｉ
赫Ｒｅｓｅａｒｃｈ．Ｄｅｖｅｌｏｐｍｅｎｔ
ｌ豳瞳—圜圜
图７和图８运动方式关于时间ｔ的三次函数０（ｔ）分别为：
ｏ（ｔ）＝（ｆｉ－ｏｔ）ｘ（３—２忐）（忐）＋Ｏｒ（８）ｏ（ｔ）＝（口一卢）×（忐一１）州．（９）
第ｋ步对应的角度
卟盟型型≥墨堕＋ｎ‘
（１０）
矿
盱堕型要生丛＋卢‘
盯
（１１）
３程序实现３．１控制平台
由ＡＴＭＥＬ公司研发出的ＡＶＲ单片机具有高速度、高保密性、低功耗的特点，已广泛地应用于军事、工
业、家用电器、智能玩具、便携式智能仪表和机器人制
作等领域。本系统中采用ＡＶＲ系列单片机中的ＡＴｍｅｇａｌ６Ｌ作为控制核心，内含可反复编程的Ｆｌａｓｈ程
序存储器、ＳＲＡＭ和ＥＥＰＲＯＭ两种数据存储器、定时器／计数器、方向可定义的Ｉ／Ｏ口及ＰＷＭ等丰富的内
部资源，算术运算功能强，软件编程灵活，自由度大，可
用软件编程实现各种算法和逻辑控制。３．２关键程序
针对ＡＴｍｅｇａｌ６Ｌ芯片的处理特点，我们采用每次驱动四只舵机和分批驱动的方式对机械手臂进行驱动，驱动函数定义如下：
ｒｏｔａｔｅ（ｓｔｒｕｃｔｓｅｒｖｏ
ｓｅｒｖｏ一１，ｕｎｓｉｇｎｅｄ
ｉｎｔ
ａｎｇｌｅ一１，……）其中：ｓｔｒｕｃｔ
８ｅ／＇ｖｏ
ｓｅｌ＇ｖｏ一１为待转动的舵机变量，
ａｎｇｌｅ一１为舵机的旋转角度对应的计数器值。ａｎｇｌｅ—ｎ
参数计算公式如下：
ａｎｇｌｅ—ｎ＝ＴＩＭＥｌ一ＢＥＧＩＮ＋ＴＩＭＥｌ—５００ＭＳ＋ａｎｇｌｅ宰１７８
单片机中计时通过计数器实现，上式中ＴＩＭＥｌ一ＢＥＧＩＮ对应计数器３ｍｓ溢出时间，ＴＩＭＥｌ＿５００ＭＳ对应
计数器０．５ｍｓ时间，２ｍｓ时间对应计数器的计数值为
ＴＩＭＥｌ＿．５００ＭＳ幸４（３２０４０，计数器１分频），然后将该值
万方数据
分成１８０份，每份是１７８，于是得出上式。
为了方便对多只舵机的动作调试，可以先定义一
个二维矩阵：
ｃｏｎｓｔ
ｐｒｏｇ＿ｕｅｈａｒ
ｍｏｒｅｌ—ｍａｔｒｉｘ［
］［ＣＯＬＵＭＮ］２
｛．…・・｝；
对于Ｍ只舵机的驱动函数，即可定义一个数组为肼＋２列的矩阵，其中前Ｍ列对应各时刻各舵机的角度（０—１８０度），第肘＋ｌ列表示由该行角度运动到下一行角度所分的步数，第肘＋２列对应由该行角度运动
到下一行角度的速度属性（即对先快后慢、先慢后快等
运动方式的设定）。执行程序时即可根据这些数据判
断第ｋ步需要给什么样的脉冲命令。
最后建立矩阵扫描函数如下：
ｓｃａｎ（ｅｏｎｓｔ
ｐｒｏｇ—ｕｃｈａｒ
ｍａｔｒｉｘ［］［］，ｕｎｓｉｇｎｅｄ
ｃｈａｒ
ｂｅｇｉｎ，ｕｎｓｉｇｎｅｄｃｈａｒｅｎｄ）
三个参数分别对应保存运动数据的数组名称、起始扫描行、结束扫描行。
上述程序表示六自由度机械臂依次按照Ｍ１，Ｍ２，
Ｍ４，Ｍ３的运动方式（Ｍｎ代表的方式已先定义好）从
第一行数据对应角度依次运行到第五行数据对应角度，从而形成ｍｏｖｅｌ运动。其他运动的控制依靠重新
定义数组和运动函数即可实现。
综合以上分析，单片机程序执行流程图如图１０所示。
。
ｊ’初始ｆ￡毽
《扁动ｉ铷一端口一
“
‰帝存嚣锈；
图ｌＯ单片机程序执行流程图
这样，反复执行这个函数若干次就可以实现对多
只舵机的并行控制。
４
结语
通过以上的分析及程序构架，大大方便了对多自由度机械臂运动的控制，而且易于扩展，该控制程序利用通用ＡＶＲ处理器的片内资源，发挥了它潜在的计算能力，大大降低代码冗余度的过程。目前该机械臂运行平稳，响应速度快，能很好地满足科研要求。
２０１０．５Ｉ机电一体化柏
基于AVR平台的六自由度仿人机械手臂控制算法研究
作者：作者单位：刊名：英文刊名：年，卷(期)：
沈长青， 孔凡让， 刘永斌， 刘方
中国科学技术大学精密机械与精密仪器系,合肥,230027机电一体化MECHATRONICS 2010(5)
本文链接：http://d.wanfangdata.com.cn/Periodical_jdyth201005009.aspx