基于TMS320F206DSP的冗余度TT-VGT機器人的運動學(xué)求解

摘要：提供了采用TMS320F206 DSP芯片進行冗余度TT-VGT機器人運動學(xué)計算方案。該方案充分利用DSP并行特性進行機器人位姿逆解計算，在程序設(shè)計中采用了多種技巧以實現(xiàn)優(yōu)化計算，并對四重四面體變幾何桁架（TT-VGT）機器人進行了仿真計算。

關(guān)鍵詞：DSP 并行計算 TT-VGT機器人 運動學(xué)

20世紀(jì)90年代以來，數(shù)字信號處理器（DSP）在自動控制中得到越來越廣泛的應(yīng)用。這主要是因為它具有以下優(yōu)點：（1）并行體系結(jié)構(gòu)和專用的硬件乘法器使得DSP運算能力極強；（2）高速特性使得DSP能實現(xiàn)實時處理和實時控制。

據(jù)調(diào)查，目前將DSP應(yīng)用于機器人控制系統(tǒng)的方案，通常是將機器人位置控制中運動學(xué)計算任務(wù)交給PC機完成，PC機將計算結(jié)果（機器人各關(guān)節(jié)的轉(zhuǎn)角）下載到以DSP芯片為核心的電機控制器，實現(xiàn)機器人控制[2]。本文提出將機器人運動學(xué)計算任務(wù)直接交給DSP的控制方案，利用DSP的并行性計算特點，提高了計算速度，縮小了控制系統(tǒng)的體積。仿真結(jié)果表明，該方案計算精度和實時性都較好。

1 TMS320F206 DSP結(jié)構(gòu)特點

TMS320F206DSP基本結(jié)構(gòu)特點包括：①哈佛結(jié)構(gòu)；②流水線操作；③專用的硬件乘法器；④特殊的DSP指令；⑤快速的指令周期（25ns）；⑥芯片內(nèi)部集成了4.5KRAM和32K FLASH RAM，大多數(shù)程序及數(shù)據(jù)可存放在DSP芯片內(nèi)。這些特點使得該芯片可以實現(xiàn)快速的DSP計算，并能使大部分運算能夠在一個指令周期內(nèi)完成。TMS320F206的并行性表現(xiàn)在以下兩方面：

（1）哈佛結(jié)構(gòu)是不同于傳統(tǒng)的馮諾曼結(jié)構(gòu)的并行體系結(jié)構(gòu)，其主要特點是將程序和數(shù)據(jù)存儲在不同的存儲空間，因此取指令和執(zhí)行能完全重疊運行。

（2）DSP芯片廣泛采用流水線以減少指令執(zhí)行時間。指令流水線由一系列總線操作組成。TMS320F206流水線具有4個獨立的操作階段：取指令、譯碼、取操作數(shù)和執(zhí)行，如圖1所示。由于4個操作階段是獨立的，因此，這些操作可交疊地進行。

2 TT-VGT機器人的位姿逆解

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

設(shè)冗余度TT-VGT機器人操作手由N個伸縮關(guān)節(jié)組成，圖3所示為兩個單元的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分別表示機器人可縮構(gòu)件的長度、速度和加速度

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

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

X=f(q) (3)

對式（3）求導(dǎo)，可得如下的運動學(xué)方程式：

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點各關(guān)節(jié)中間變量的dqk及位置qk：

dqk=J+dXk (8)

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

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

3 TMS320F206 DSP運動學(xué)程序設(shè)計

對于TT-VGT機器人的位姿逆解，采用DSP匯編語言設(shè)計的程序流程進行求解，如圖4所示。

為了保證該程序的執(zhí)行速度和計算精度，采取了以下算法：

（1）由于匯編語言指令系統(tǒng)中沒有三角函數(shù)等數(shù)學(xué)函數(shù)指令，這些函數(shù)的計算只有通過級數(shù)展開算法實現(xiàn)，但計算量太大。考慮到三角函數(shù)的周期性，建立了一人1024點的正弦函數(shù)和余弦函數(shù)表，其分辨率完全能滿足精度要求。

（2）由于TMS320F206 DSP芯片為定點運算器件，因此需要將浮點運算轉(zhuǎn)換為定點運算。為保證計算精度，將數(shù)據(jù)定標(biāo)設(shè)定為可動態(tài)調(diào)節(jié)，數(shù)據(jù)表達(dá)能力為從Q13（-4～+3.9998779）到Q0（-32768～+32767）。

（3）采用并行指令，充分利用TMS320F206四級流水線操作，來提高程序運行速度。

如：

MAC ；乘并累加

APAC ；累加

SACH +，3，AR2 ；將計算結(jié)果左移3位后，存于當(dāng)前輔助寄存器（AR）所指的存儲器單元中，并將AR內(nèi)容加1，最后，將AR2設(shè)定為當(dāng)有AR。

（4）對運算過程進行優(yōu)化，既要減少計算量，又要防止計算溢出；在混合運算中采用“先除后加”、“加減交叉”的方法。

（5）盡量采用移位運算代替乘除運算，以提高運行速度和計算精度。

通過以上方法，實現(xiàn)了機器人運行學(xué)計算的實時性和準(zhǔn)確性。

4 仿真計算

以四重四面體為例，建立如圖5所示的基礎(chǔ)坐標(biāo)系XYZ，末端參考點H位于末端平臺EFG的中點。設(shè)參考點H在基礎(chǔ)坐標(biāo)系中從點（0.522689，-0.818450，0.472752）直線運動到點（0.771439，-0.965700，0.721502），只實現(xiàn)空間的位置運動，不實現(xiàn)姿態(tài)。動態(tài)的整個時間T設(shè)為5秒，運動軌跡分為等時間間隔的100個區(qū)間。設(shè)各定長構(gòu)件長度為1m。

中間變量q曲線和中間變量q誤差曲線如圖6和圖7所示。從誤差曲線可看出，采用TMS329F206DSP芯片進行的運動學(xué)計算精度較高。經(jīng)過實測，該計算程序運行時間為34ms。（TMS320F206芯片指令周期為25ns），可見其實時性較強。

本文提出的采用TMS320F206 DSP的芯片進行冗余度TT-VGT機器人運動學(xué)計算方案，充分利用了DSP并行特性進行機器人位姿逆解計算，在程序設(shè)計中采用了多種技巧優(yōu)化計算。仿真結(jié)果表明，該方案計算誤差較小，實時性強。因此，可將其應(yīng)用于機器人控制系統(tǒng)，實現(xiàn)機器人計算和控制任務(wù)一體化，從而大大縮小機器人體積、降低成本、增強靈活性、具有較強的先進性和實用性。