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四級流水線操作,來提高程序運行速度。