摘 要： 提出了采用ti" title="ti">title="TMS320F206" title="TMS320F206">TMS320F206">TMS320F206 DSP" title="DSP">DSP芯片进行冗余度TT-VGT机器人" title="机器人">机器人运动学计算方案。该方案充分利用DSP 并行特性进行机器人位姿逆解计算,在程序设计中采用了多种技巧以实现优化计算,并对四重四面体变几何桁架(TT-VGT)机器人进行了仿真计算。
关键词： DSP 并行计算 TT—VGT机器人 运动学
20世纪90年代以来,数字信号处理器(DSP)在自动控制中得到了越来越广泛的应用。这主要是因为它具有以下优点:(1)并行体系结构和专用的硬件乘法器使得DSP运算能力极强;(2)高速特性使得DSP能实现实时处理和实时控制。
据调查,目前将DSP应用于机器人控制系统的方案,通常是将机器人位置控制中运动学计算任务交给PC机完成,PC机将计算结果(机器人各关节的转角)下载到以DSP芯片为核心的电机控制器,实现机器人控制?眼2?演。本文提出将机器人运动学计算任务直接交给DSP的控制方案,利用DSP 的并行性计算特点,提高了计算速度,缩小了控制系统的体积。仿真结果表明,该方案计算精度和实时性都较好。
1 TMS320F206 DSP结构特点
TMS320F206DSP基本结构特点包括:①哈佛结构;②流水线操作;③专用的硬件乘法器;④特殊的DSP指令;⑤快速的指令周期(25ns);⑥芯片内部集成了4.5KRAM和32K FLASH RAM,大多数程序及数据可存放在DSP芯片内。这些特点使得该芯片可以实现快速的DSP计算,并能使大部分运算能够在一个指令周期内完成。TMS320F206的并行性表现在以下两方面:
(1)哈佛结构是不同于传统的冯诺曼结构的并行体系结构,其主要特点是将程序和数据存储在不同的存储空间,因此取指令和执行能完全重叠运行。
(2)DSP芯片广泛采用流水线以减少指令执行时间。指令流水线由一系列总线操作组成。TMS320F206流水线具有4个独立的操作阶段:取指令、译码、取操作数和执行,如图1所示。由于4个操作阶段是独立的,因此,这些操作可以交叠地进行。
2 TT-VGT机器人的位姿逆解
TT-VGT(Tetrahedron- Tetrahedron—Variable Geometry Truss)机器人是由多个四面体组成的变几何桁架机器人,平面ABC为机器人的基础平台,基本单元中各杆之间由球铰连接,通过可伸缩构件li(i=1，2，…n)的长度变化,来改变机构的构形,如图2所示。
设冗余度TT-VGT机器人操作手由N个伸缩关节组成,图3所示为两个单元的TT-VGT机构。设变量qi(i=1，2…N)为平面ABC和平面BCD的夹角,其相应的速度和加速度分别为
。
它们与
(i=1，2，…N)的关系如下[1]:
式中， d表示TT-VGT中不可伸缩构件的长度
分别表示机器人可伸缩构件的长度、速度和加速度
相邻两个四面体单元的坐标系的建立如图3所示。坐标系XiYiZi相对于坐标系Xi-1Yi-1Zi-1的变换矩阵可表示为:
对于机构自由度为N、任务自由度为L的冗余度TT-VGT机器人,其余四面体单元的结构与坐标系的建立与图3所示的相似。由文献[1]可知,其末端位姿X是中间变量qi(i=1,2,…,N)的函数,有:
对式(3)求导,可得如下的运动学方程式:
将式(6)离散化,可得机器人运动轨迹上第k点各关节中间变量的dqk及位置qk:
将qk代入式(1),可求得TT-VGT机器人各伸缩构件的长度li。
3 TMS320F206 DSP运动学程序设计
对于TT-VGT机器人的位姿逆解,采用DSP汇编语言设计的程序流程进行求解,如图4所示。
为了保证该程序的执行速度和计算精度,采取了以下算法:
(1)由于汇编语言指令系统中没有三角函数等数学函数指令,这些函数的计算只有通过级数展开算法实现,但计算量太大。考虑到三角函数的周期性,建立了一个1024点的正弦函数和余弦函数表,其分辨率完全能满足精度要求。
(2)由于TMS320F206 DSP芯片为定点运算器件,因此需要将浮点运算转换为定点运算。为了保证计算精度,将数据定标设定为可动态调节,数据表达能力为从Q13(-4～+3.9998779)到Q0(-32768～+32767)。
(3)采用并行指令,充分利用TMS320F206四级流水线操作,来提高程序运行速度。
如:
MAC ;乘并累加
APAC ;累加
SACH*+,3,AR2 ;将计算结果左移3位后,存于当前辅助寄存器(AR)所指的存储器单元中,并将
AR内容加1,最后,将AR2设定为当前AR。
(4)对运算过程进行优化,既要减少计算量,又要防止计算溢出;在混合运算中采用“先除后加”、“加减交叉”的方法。
(5)尽量采用移位运算代替乘除运算,以提高运行速度和计算精度。
通过以上方法,实现了机器人运动学计算的实时性和准确性。
4 仿真计算
以四重四面体为例,建立如图5所示的基础坐标系XYZ,末端参考点H位于末端平台EFG的中点。设参考点H在基础坐标系中从点(0.522689，-0.818450， 0.472752)直线运动到点(0.771439，-0.965700，0.721502),只实现空间的位置运动,不实现姿态。运动的整个时间T设为5秒,运动轨迹分为等时间间隔的100个区间。设各定长构件长度为1m。
中间变量q曲线和中间变量q误差曲线如图6和图7所示。从误差曲线可看出,采用TMS329F206 DSP芯片进行的运动学计算精度较高。经过实测,该计算程序运行时间为34ms(TMS320F206芯片指令周期为25ns),可见其实时性较强。
本文提出的采用TMS320F206 DSP芯片进行冗余度TT-VGT机器人运动学计算方案,充分利用了DSP 并行特性进行机器人位姿逆解计算,在程序设计中采用了多种技巧优化计算。仿真结果表明,该方案计算误差较小,实时性强。因此,可将其应用于机器人控制系统,实现机器人计算和控制任务一体化,从而大大缩小机器人体积、降低成本、增强灵活性、具有较强的先进性和实用性。
参考文献
1 徐礼钜,李 宏,范守文.Analytical Model Method For Dynamics Of N-Celled Tetrahedron-Tetrahedron Variable
Geometry Truss Manipulators.ASME Mechanisms Conference，Atlanta，USA，1998
2 周学才,李卫平,李 强.开放式机器人通用控制系统.机器人.1998(1)
3 张雄伟,曹铁勇.DSP芯片的原理与开发应用(第2版).北京:电子工业出版社,2000.9
4 TMS320C2xx User’s Guide.TEXAS INSTUMENTS，1997