基于TMS320F206DSP的冗余度TT-VGT机器人的运动学求解

    摘要：提供了采用TMS320F206 DSP芯片进行冗余度TT-VGT机器人运动学计算方案。该方案充分利用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)为平面ACB和平面BCD的夹角，其相应的速度和加速度分别为qi,qi(i=1,2,…N)。

它们与li,li,li(i=1，2，…N)的关系如下[1]：

式中，d表示TT-VGT中不可伸缩构件的长度

li,l'i,l''I分别表示机器人可缩构件的长度、速度和加速度

相邻两个四面体单元的坐标系的建立如图3所示。坐标系XiYiZi相对于坐标系Xi-1Yi-1的变换矩阵可表示为：

对于机构自由度为N、任务自由度为L的冗余度TT-VGT机器人，其余四面体单元的结构与坐标系的建立与图3所示的相似。由文献[1]可知，其末端位姿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

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并行特性进行机器人位姿逆解计算，在程序设计中采用了多种技巧优化计算。仿真结果表明，该方案计算误差较小，实时性强。因此，可将其应用于机器人控制系统，实现机器人计算和控制任务一体化，从而大大缩小机器人体积、降低成本、增强灵活性、具有较强的先进性和实用性。