据调查,目前将DSP应用于 机器人控制系统的方案,通常是将机器人位置控制中运动学计算任务交给PC机完成,PC机将计算结果（机器人各关节的转角）下载到以DSP芯片为核心的 电机控制器,实现机器人控制。本文提出将机器人运动学计算任务直接交给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)为平面ACB和平面BCD的夹角,其相应的速度和加速度分别为qi,qi(i=1,2,…N)。
它们与li,li,li(i=1,2,…N)的关系如下：
式中,d表示TT-VGT中不可伸缩构件的长度
li,l''i,l''''I分别表示机器人可缩构件的长度、速度和加速度
相邻两个四面体单元的坐标系的建立如图3所示。坐标系XiYiZi相对于坐标系Xi-1Yi-1的变换矩阵可表示为：
对于机构自由度为N、任务自由度为L的冗余度TT-VGT机器人,其余四面体单元的结构与坐标系的建立与图3所示的相似。由文献可知,其末端位姿X是中间变量qi(i=1,2,…,N)的函数,有：
X=f(q) (3)
对式（3）求导,可得如下的运动学方程式：
X=Jq （4）
式中,X=(x1,x2,…,xL) T∈R L
q=(q1,q2,…,qN)T∈R N
L＜N
J为机器人的雅可比矩阵,
由式（4）可得：
q=J + X (6)
式中,J+为雅可比矩阵的伪逆,
J+=J T(JJT)-1 (7)
将式(6)离散化,可得机器人运动轨迹上第k点各关节中间变量的dqk及位置qk：
dqk=J+dXk (8)
q k=q (k-1)+dq k (9)
将q k代入式（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所示。从误差曲线可看出,采用TMS329F206DSP芯片进行的运动学计算精度较高。经过实测,该计算程序运行时间为34ms。（TMS320F206芯片指令周期为25ns）,可见其实时性较强。
本文提出的采用TMS320F206 DSP的芯片进行冗余度TT-VGT机器人运动学计算方案,充分利用了DSP并行特性进行机器人位姿逆解计算,在程序设计中采用了多种技巧优化计算。仿真结果表明,该方案计算误差较小,实时性强。因此,可将其应用于机器人控制系统,实现机器人计算和控制任务一体化,从而大大缩小机器人体积、降低成本、增强灵活性、具有较强的先进性和实用性。
