TMS320F206DSP的冗余度TT-VGT机器人

　　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。[page]

　　3 TMS320F206 DSP运动学程序设计

　　对于TT-VGT机器人的位姿逆解,采用DSP汇编语言设计的程序流程进行求解,如图4所示。

　　为了保证该程序的执行速度和计算精度,采取了以下算法：

　　（1）由于汇编语言指令系统中没有三角函数等数学函数指令,这些函数的计算只有通过级数展开算法实现,但计算量太大。考虑到三角函数的周期性,建立了一人1024点的正弦函数和余弦函数表,其分辨率完全能满足精度要求。

　　（2）由于TMS320F206 DSP芯片为定点运算器件,因此需要将浮点运算转换为定点运算。为保证计算精度,将数据定标设定为可动态调节,数据表达能力为从Q13（-4～+3.9998779）到Q0（-32768～+32767）。

　　（3）采用并行指令,充分利用TMS320F206四级流水线操作,来提高程序运行速度。