摘 要： 提出了采用ti" title="ti">title="TMS320F206" title="TMS320F206">TMS320F206">TMS320F206 DSP" title="DSP">DSP芯片進(jìn)行冗余度TT-VGT機(jī)器人" title="機(jī)器人">機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算方案。該方案充分利用DSP 并行特性進(jìn)行機(jī)器人位姿逆解計(jì)算,在程序設(shè)計(jì)中采用了多種技巧以實(shí)現(xiàn)優(yōu)化計(jì)算,并對(duì)四重四面體變幾何桁架(TT-VGT)機(jī)器人進(jìn)行了仿真計(jì)算。
關(guān)鍵詞： DSP 并行計(jì)算 TT—VGT機(jī)器人 運(yùn)動(dòng)學(xué)
20世紀(jì)90年代以來,數(shù)字信號(hào)處理器(DSP)在自動(dòng)控制中得到了越來越廣泛的應(yīng)用。這主要是因?yàn)樗哂幸韵聝?yōu)點(diǎn):(1)并行體系結(jié)構(gòu)和專用的硬件乘法器使得DSP運(yùn)算能力極強(qiáng);(2)高速特性使得DSP能實(shí)現(xiàn)實(shí)時(shí)處理和實(shí)時(shí)控制。
據(jù)調(diào)查,目前將DSP應(yīng)用于機(jī)器人控制系統(tǒng)的方案,通常是將機(jī)器人位置控制中運(yùn)動(dòng)學(xué)計(jì)算任務(wù)交給PC機(jī)完成,PC機(jī)將計(jì)算結(jié)果(機(jī)器人各關(guān)節(jié)的轉(zhuǎn)角)下載到以DSP芯片為核心的電機(jī)控制器,實(shí)現(xiàn)機(jī)器人控制?眼2?演。本文提出將機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算任務(wù)直接交給DSP的控制方案,利用DSP 的并行性計(jì)算特點(diǎn),提高了計(jì)算速度,縮小了控制系統(tǒng)的體積。仿真結(jié)果表明,該方案計(jì)算精度和實(shí)時(shí)性都較好。
1 TMS320F206 DSP結(jié)構(gòu)特點(diǎn)
TMS320F206DSP基本結(jié)構(gòu)特點(diǎn)包括:①哈佛結(jié)構(gòu);②流水線操作;③專用的硬件乘法器;④特殊的DSP指令;⑤快速的指令周期(25ns);⑥芯片內(nèi)部集成了4.5KRAM和32K FLASH RAM,大多數(shù)程序及數(shù)據(jù)可存放在DSP芯片內(nèi)。這些特點(diǎn)使得該芯片可以實(shí)現(xiàn)快速的DSP計(jì)算,并能使大部分運(yùn)算能夠在一個(gè)指令周期內(nèi)完成。TMS320F206的并行性表現(xiàn)在以下兩方面:
(1)哈佛結(jié)構(gòu)是不同于傳統(tǒng)的馮諾曼結(jié)構(gòu)的并行體系結(jié)構(gòu),其主要特點(diǎn)是將程序和數(shù)據(jù)存儲(chǔ)在不同的存儲(chǔ)空間,因此取指令和執(zhí)行能完全重疊運(yùn)行。
(2)DSP芯片廣泛采用流水線以減少指令執(zhí)行時(shí)間。指令流水線由一系列總線操作組成。TMS320F206流水線具有4個(gè)獨(dú)立的操作階段:取指令、譯碼、取操作數(shù)和執(zhí)行,如圖1所示。由于4個(gè)操作階段是獨(dú)立的,因此,這些操作可以交疊地進(jìn)行。
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)的長(zhǎng)度變化,來改變機(jī)構(gòu)的構(gòu)形,如圖2所示。
設(shè)冗余度TT-VGT機(jī)器人操作手由N個(gè)伸縮關(guān)節(jié)組成,圖3所示為兩個(gè)單元的TT-VGT機(jī)構(gòu)。設(shè)變量qi(i=1，2…N)為平面ABC和平面BCD的夾角,其相應(yīng)的速度和加速度分別為
。
它們與
(i=1，2，…N)的關(guān)系如下[1]:
式中， d表示TT-VGT中不可伸縮構(gòu)件的長(zhǎng)度
分別表示機(jī)器人可伸縮構(gòu)件的長(zhǎng)度、速度和加速度
相鄰兩個(gè)四面體單元的坐標(biāo)系的建立如圖3所示。坐標(biāo)系XiYiZi相對(duì)于坐標(biāo)系Xi-1Yi-1Zi-1的變換矩陣可表示為:
對(duì)于機(jī)構(gòu)自由度為N、任務(wù)自由度為L(zhǎng)的冗余度TT-VGT機(jī)器人,其余四面體單元的結(jié)構(gòu)與坐標(biāo)系的建立與圖3所示的相似。由文獻(xiàn)[1]可知,其末端位姿X是中間變量qi(i=1,2,…,N)的函數(shù),有:
對(duì)式(3)求導(dǎo),可得如下的運(yùn)動(dòng)學(xué)方程式:
將式(6)離散化,可得機(jī)器人運(yùn)動(dòng)軌跡上第k點(diǎn)各關(guān)節(jié)中間變量的dqk及位置qk:
將qk代入式(1),可求得TT-VGT機(jī)器人各伸縮構(gòu)件的長(zhǎng)度li。
3 TMS320F206 DSP運(yùn)動(dòng)學(xué)程序設(shè)計(jì)
對(duì)于TT-VGT機(jī)器人的位姿逆解,采用DSP匯編語言設(shè)計(jì)的程序流程進(jìn)行求解,如圖4所示。
為了保證該程序的執(zhí)行速度和計(jì)算精度,采取了以下算法:
(1)由于匯編語言指令系統(tǒng)中沒有三角函數(shù)等數(shù)學(xué)函數(shù)指令,這些函數(shù)的計(jì)算只有通過級(jí)數(shù)展開算法實(shí)現(xiàn),但計(jì)算量太大?？紤]到三角函數(shù)的周期性,建立了一個(gè)1024點(diǎn)的正弦函數(shù)和余弦函數(shù)表,其分辨率完全能滿足精度要求。
(2)由于TMS320F206 DSP芯片為定點(diǎn)運(yùn)算器件,因此需要將浮點(diǎn)運(yùn)算轉(zhuǎn)換為定點(diǎn)運(yùn)算。為了保證計(jì)算精度,將數(shù)據(jù)定標(biāo)設(shè)定為可動(dòng)態(tài)調(diào)節(jié),數(shù)據(jù)表達(dá)能力為從Q13(-4～+3.9998779)到Q0(-32768～+32767)。
(3)采用并行指令,充分利用TMS320F206四級(jí)流水線操作,來提高程序運(yùn)行速度。
如:
MAC ;乘并累加
APAC ;累加
SACH*+,3,AR2 ;將計(jì)算結(jié)果左移3位后,存于當(dāng)前輔助寄存器(AR)所指的存儲(chǔ)器單元中,并將
AR內(nèi)容加1,最后,將AR2設(shè)定為當(dāng)前AR。
(4)對(duì)運(yùn)算過程進(jìn)行優(yōu)化,既要減少計(jì)算量,又要防止計(jì)算溢出;在混合運(yùn)算中采用“先除后加”、“加減交叉”的方法。
(5)盡量采用移位運(yùn)算代替乘除運(yùn)算,以提高運(yùn)行速度和計(jì)算精度。
通過以上方法,實(shí)現(xiàn)了機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算的實(shí)時(shí)性和準(zhǔn)確性。
4 仿真計(jì)算
以四重四面體為例,建立如圖5所示的基礎(chǔ)坐標(biāo)系XYZ,末端參考點(diǎn)H位于末端平臺(tái)EFG的中點(diǎn)。設(shè)參考點(diǎn)H在基礎(chǔ)坐標(biāo)系中從點(diǎn)(0.522689，-0.818450， 0.472752)直線運(yùn)動(dòng)到點(diǎn)(0.771439，-0.965700，0.721502),只實(shí)現(xiàn)空間的位置運(yùn)動(dòng),不實(shí)現(xiàn)姿態(tài)。運(yùn)動(dòng)的整個(gè)時(shí)間T設(shè)為5秒,運(yùn)動(dòng)軌跡分為等時(shí)間間隔的100個(gè)區(qū)間。設(shè)各定長(zhǎng)構(gòu)件長(zhǎng)度為1m。
中間變量q曲線和中間變量q誤差曲線如圖6和圖7所示。從誤差曲線可看出,采用TMS329F206 DSP芯片進(jìn)行的運(yùn)動(dòng)學(xué)計(jì)算精度較高。經(jīng)過實(shí)測(cè),該計(jì)算程序運(yùn)行時(shí)間為34ms(TMS320F206芯片指令周期為25ns),可見其實(shí)時(shí)性較強(qiáng)。
本文提出的采用TMS320F206 DSP芯片進(jìn)行冗余度TT-VGT機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算方案,充分利用了DSP 并行特性進(jìn)行機(jī)器人位姿逆解計(jì)算,在程序設(shè)計(jì)中采用了多種技巧優(yōu)化計(jì)算。仿真結(jié)果表明,該方案計(jì)算誤差較小,實(shí)時(shí)性強(qiáng)。因此,可將其應(yīng)用于機(jī)器人控制系統(tǒng),實(shí)現(xiàn)機(jī)器人計(jì)算和控制任務(wù)一體化,從而大大縮小機(jī)器人體積、降低成本、增強(qiáng)靈活性、具有較強(qiáng)的先進(jìn)性和實(shí)用性。
參考文獻(xiàn)
1 徐禮鉅,李 宏,范守文.Analytical Model Method For Dynamics Of N-Celled Tetrahedron-Tetrahedron Variable
Geometry Truss Manipulators.ASME Mechanisms Conference，Atlanta，USA，1998
2 周學(xué)才,李衛(wèi)平,李 強(qiáng).開放式機(jī)器人通用控制系統(tǒng).機(jī)器人.1998(1)
3 張雄偉,曹鐵勇.DSP芯片的原理與開發(fā)應(yīng)用(第2版).北京:電子工業(yè)出版社,2000.9
4 TMS320C2xx User’s Guide.TEXAS INSTUMENTS，1997