TMS320F206DSP的冗余度TT-VGT機(jī)器人

　　2 TT-VGT機(jī)器人的位姿逆解

　　TT-VGT（Tetrahedron-Tetrahedron-Variable Geometry Truss）機(jī)器人是由多個(gè)四面體組成的變幾何桁架機(jī)器人,平面ABC為機(jī)器人的基礎(chǔ)平臺(tái),基本單元中各桿之間由球校連接,通過可伸縮構(gòu)件li(i=1,2,…n)的長度,來改變機(jī)構(gòu)的構(gòu)形,如圖2所示。

　　設(shè)冗余度TT-VGT機(jī)器人操作手由N個(gè)伸縮關(guān)節(jié)組成,圖3所示為兩個(gè)單元的TT-VGT構(gòu)成。設(shè)變量qi(i=1,2,…N)為平面ACB和平面BCD的夾角,其相應(yīng)的速度和加速度分別為qi,qi(i=1,2,…N)。

　　它們與li,li,li(i=1,2,…N)的關(guān)系如下[1]：

　　式中,d表示TT-VGT中不可伸縮構(gòu)件的長度

　　li,l''i,l''''I分別表示機(jī)器人可縮構(gòu)件的長度、速度和加速度

　　相鄰兩個(gè)四面體單元的坐標(biāo)系的建立如圖3所示。坐標(biāo)系XiYiZi相對于坐標(biāo)系Xi-1Yi-1的變換矩陣可表示為：

　　對于機(jī)構(gòu)自由度為N、任務(wù)自由度為L的冗余度TT-VGT機(jī)器人,其余四面體單元的結(jié)構(gòu)與坐標(biāo)系的建立與圖3所示的相似。由文獻(xiàn)[1]可知,其末端位姿X是中間變量qi(i=1,2,…,N)的函數(shù),有：

　　X=f(q)　　 (3)

　　對式（3）求導(dǎo),可得如下的運(yùn)動(dòng)學(xué)方程式：

　　X=Jq　　 （4）

　　式中,X=(x1,x2,…,xL) T∈R L

　　q=(q1,q2,…,qN)T∈R N

　　LN

　　J為機(jī)器人的雅可比矩陣,

　　由式（4）可得：

　　q=J + X (6)

　　式中,J+為雅可比矩陣的偽逆,

　　J+=J T(JJT)-1 (7)

　　將式(6)離散化,可得機(jī)器人運(yùn)動(dòng)軌跡上第k點(diǎn)各關(guān)節(jié)中間變量的dqk及位置qk：

　　dqk=J+dXk (8)

　　q k=q (k-1)+dq k (9)

　　將q k代入式（1）,可求得TT-VGT機(jī)器人各伸縮構(gòu)件的長度li。