近年來自動控制技術發展迅猛，特別是計算機技術、網絡和通信技術發展的突飛猛進，使人們籍助于許多使能技術的進步和一些開發工具的擴大，將人們構思的自動操作得以付諸實現。如網絡控制技術、可編程控制器等均屬于自動化控制技術中的使能技術。自動控制技術正向著網絡化、集成化、分布化、節點節能化的方向發展。數字信號處理器（DSP）在自動控制中得到了越來越廣泛的應用。這主要是因為它具有以下優點：（1）并行體系結構和專用的硬件乘法器使得DSP運算能力極強；（2）高速特性使得DSP能實現實時處理和實時控制。

據調查，目前將DSP應用于機器人控制系統的方案，通常是將機器人位置控制中運動學計算任務交給PC機完成，PC機將計算結果（機器人各關節的轉角）下載到以DSP芯片為核心的電機控制器，實現機器人控制眼演。本文提出將機器人運動學計算任務直接交給DSP的控制方案，利用DSP 的并行性計算特點，提高了計算速度，縮小了控制系統的體積。仿真結果表明，該方案計算精度和實時性都較好。

1 TMS320F206 DSP結構特點

TMS320F206是TI公司推出的一種DSP芯片，它是基于TMS320C5x之上的高速定點數字處理芯片，具有改進的哈佛結構并行分離的程序和數據總線、高性能CPU及高效的指令集等特點。其主要特性如下：

CPU具有32位CALU、32位累加器、16×16位并行乘法器、三個移位寄存器、八個16位輔助寄存器。

存儲器具有224K字可尋址存儲空間、544字片內DRAM、4K字片內SRAM或32K字片內快閃存儲器。

指令速度為50ns、35ns及25ns單指令周期。

外圍電路有軟件可編程定時器、軟件可編程等待狀態發生器、片內鎖相環時鐘發生器、同步和異步系列串口。

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

TT-VGT（Tetrahedron-Tetrahedron-Variable Geometry Truss）機器人是由多個四面體組成的變幾何桁架機器人，圖1所示為由N個四面體單元組成的冗余度TT-VGT機器人操作手，平面ABC為機器人的基礎平臺，基本單元中各桿之間由較鉸連接，通過可伸縮構件li（i=1,2,…，n）的長度變化改變機構的構形。圖2所示為其中的兩個單元的TT-VGT機構，設平面ABC和平面BCD的夾角用中間變量qi（i=1,2,…，n）表示，qi與li（I=1,2,…，n）的關系。

設冗余度TT-VGT機器人操作手由N個伸縮關節組成,圖3所示為兩個單元的TT-VGT機構。設變量qi(i=1，2…N)為平面ABC和平面BCD的夾角,其相應的速度和加速度分別為

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

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

對式(3)求導,可得如下的運動學方程式:

將式(6)離散化,可得機器人運動軌跡上第k點各關節中間變量的dqk及位置qk:

將qk代入式（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四級流水線操作，來提高程序運行速度。

如：

MAC ;乘并累加

APAC ;累加

SACH*+,3,AR2 ;將計算結果左移3位后，存于當前輔助寄存器（AR）所指的存儲器單元中，并將

AR內容加1,最后，將AR2設定為當前AR.

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

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

通過以上方法，實現了機器人運動學計算的實時性和準確性。

4 仿真計算

以四重四面體為例，建立如圖5所示的基礎坐標系XYZ,末端參考點H位于末端平臺EFG的中點。設參考點H在基礎坐標系中從點（0.522689,-0.818450, 0.472752）直線運動到點（0.771439,-0.965700,0.721502），只實現空間的位置運動，不實現姿態。運動的整個時間T設為5秒，運動軌跡分為等時間間隔的100個區間。設各定長構件長度為1m.

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

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