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

　　LN

　　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。