基于TMS320F206 DSP芯片的冗余度TT—VGT機(jī)器人運(yùn)動(dòng)學(xué)設(shè)計(jì)方案

近年來自動(dòng)控制技術(shù)發(fā)展迅猛，特別是計(jì)算機(jī)技術(shù)、網(wǎng)絡(luò)和通信技術(shù)發(fā)展的突飛猛進(jìn)，使人們籍助于許多使能技術(shù)的進(jìn)步和一些開發(fā)工具的擴(kuò)大，將人們構(gòu)思的自動(dòng)操作得以付諸實(shí)現(xiàn)。如網(wǎng)絡(luò)控制技術(shù)、可編程控制器等均屬于自動(dòng)化控制技術(shù)中的使能技術(shù)。自動(dòng)控制技術(shù)正向著網(wǎng)絡(luò)化、集成化、分布化、節(jié)點(diǎn)節(jié)能化的方向發(fā)展。數(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ī)器人控制眼演。本文提出將機(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)

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

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

存儲(chǔ)器具有224K字可尋址存儲(chǔ)空間、544字片內(nèi)DRAM、4K字片內(nèi)SRAM或32K字片內(nèi)快閃存儲(chǔ)器。

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

外圍電路有軟件可編程定時(shí)器、軟件可編程等待狀態(tài)發(fā)生器、片內(nèi)鎖相環(huán)時(shí)鐘發(fā)生器、同步和異步系列串口。

2 TT-VGT機(jī)器人的位姿逆解

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

設(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)的速度和加速度分別為

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

對(duì)于機(jī)構(gòu)自由度為N、任務(wù)自由度為L的冗余度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)件的長度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è)各定長構(gòu)件長度為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í)用性。