摘 要： 提出了采用ti" title="ti">title="TMS320F206" title="TMS320F206">TMS320F206">TMS320F206 DSP" title="DSP">DSP芯片進行冗余度TT-VGT機器人" title="機器人">機器人運動學計算方案。該方案充分利用DSP 并行特性進行機器人位姿逆解計算,在程序設計中采用了多種技巧以實現優化計算,并對四重四面體變幾何桁架(TT-VGT)機器人進行了仿真計算。
關鍵詞： DSP 并行計算 TT—VGT機器人 運動學
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)為平面ABC和平面BCD的夾角,其相應的速度和加速度分別為
。
它們與
(i=1，2，…N)的關系如下[1]:
式中， d表示TT-VGT中不可伸縮構件的長度
分別表示機器人可伸縮構件的長度、速度和加速度
相鄰兩個四面體單元的坐標系的建立如圖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 并行特性進行機器人位姿逆解計算,在程序設計中采用了多種技巧優化計算。仿真結果表明,該方案計算誤差較小,實時性強。因此,可將其應用于機器人控制系統,實現機器人計算和控制任務一體化,從而大大縮小機器人體積、降低成本、增強靈活性、具有較強的先進性和實用性。
參考文獻
1 徐禮鉅,李 宏,范守文.Analytical Model Method For Dynamics Of N-Celled Tetrahedron-Tetrahedron Variable
Geometry Truss Manipulators.ASME Mechanisms Conference，Atlanta，USA，1998
2 周學才,李衛平,李 強.開放式機器人通用控制系統.機器人.1998(1)
3 張雄偉,曹鐵勇.DSP芯片的原理與開發應用(第2版).北京:電子工業出版社,2000.9
4 TMS320C2xx User’s Guide.TEXAS INSTUMENTS，1997