|
20世紀90年代以來(lái),數字信號處理器（DSP）在自動(dòng)控制中得到越來(lái)越廣泛的應用。這主要是因為它具有以下優(yōu)點(diǎn)：（1）并行體系結構和專(zhuān)用的硬件乘法器使得DSP運算能力極強;（2）高速特性使得DSP能實(shí)現實(shí)時(shí)處理和實(shí)時(shí)控制。 據調查,目前將DSP應用于機器人控制系統的方案,通常是將機器人位置控制中運動(dòng)學(xué)計算任務(wù)交給PC機完成,PC機將計算結果（機器人各關(guān)節的轉角）下載到以DSP芯片為核心的電機控制器,實(shí)現機器人控制。本文提出將機器人運動(dòng)學(xué)計算任務(wù)直接交給DSP的控制方案,利用DSP的并行性計算特點(diǎn),提高了計算速度,縮小了控制系統的體積。仿真結果表明,該方案計算精度和實(shí)時(shí)性都較好。 1 TMS320F206 DSP結構特點(diǎn) TMS320F206DSP基本結構特點(diǎn)包括：①哈佛結構;②流水線(xiàn)操作;③專(zhuān)用的硬件乘法器;④特殊的DSP指令;⑤快速的指令周期（25ns）;⑥芯片內部集成了4.5KRAM和32K FLASH RAM,大多數程序及數據可存放在DSP芯片內。這些特點(diǎn)使得該芯片可以實(shí)現快速的DSP計算,并能使大部分運算能夠在一個(gè)指令周期內完成。TMS320F206的并行性表現在以下兩方面： （1）哈佛結構是不同于傳統的馮諾曼結構的并行體系結構,其主要特點(diǎn)是將程序和數據存儲在不同的存儲空間,因此取指令和執行能完全重疊運行。 （2）DSP芯片廣泛采用流水線(xiàn)以減少指令執行時(shí)間。指令流水線(xiàn)由一系列總線(xiàn)操作組成。TMS320F206流水線(xiàn)具有4個(gè)獨立的操作階段：取指令、譯碼、取操作數和執行,如圖1所示。由于4個(gè)操作階段是獨立的,因此,這些操作可交疊地進(jìn)行。 2 TT-VGT機器人的位姿逆解 TT-VGT（Tetrahedron-Tetrahedron-Variable Geometry Truss）機器人是由多個(gè)四面體組成的變幾何桁架機器人,平面ABC為機器人的基礎平臺,基本單元中各桿之間由球校連接,通過(guò)可伸縮構件li(i=1,2,…n)的長(cháng)度,來(lái)改變機構的構形,如圖2所示。 設冗余度TT-VGT機器人操作手由N個(gè)伸縮關(guān)節組成,圖3所示為兩個(gè)單元的TT-VGT構成。設變量qi(i=1,2,…N)為平面ACB和平面BCD的夾角,其相應的速度和加速度分別為qi,qi(i=1,2,…N)。 它們與li,li,li(i=1,2,…N)的關(guān)系如下： 式中,d表示TT-VGT中不可伸縮構件的長(cháng)度 li,l''i,l''''I分別表示機器人可縮構件的長(cháng)度、速度和加速度 相鄰兩個(gè)四面體單元的坐標系的建立如圖3所示。坐標系XiYiZi相對于坐標系Xi-1Yi-1的變換矩陣可表示為： 對于機構自由度為N、任務(wù)自由度為L(cháng)的冗余度TT-VGT機器人,其余四面體單元的結構與坐標系的建立與圖3所示的相似。由文獻可知,其末端位姿X是中間變量qi(i=1,2,…,N)的函數,有： X=f(q) (3) 對式（3）求導,可得如下的運動(dòng)學(xué)方程式： X=Jq （4） 式中,X=(x1,x2,…,xL) T∈R L q=(q1,q2,…,qN)T∈R N L＜N J為機器人的雅可比矩陣, 由式（4）可得： q=J + X (6) 式中,J+為雅可比矩陣的偽逆, J+=J T(JJT)-1 (7) 將式(6)離散化,可得機器人運動(dòng)軌跡上第k點(diǎn)各關(guān)節中間變量的dqk及位置qk： dqk=J+dXk (8) q k=q (k-1)+dq k (9) 將q k代入式（1）,可求得TT-VGT機器人各伸縮構件的長(cháng)度li。 3 TMS320F206 DSP運動(dòng)學(xué)程序設計 對于TT-VGT機器人的位姿逆解,采用DSP匯編語(yǔ)言設計的程序流程進(jìn)行求解,如圖4所示。 為了保證該程序的執行速度和計算精度,采取了以下算法： （1）由于匯編語(yǔ)言指令系統中沒(méi)有三角函數等數學(xué)函數指令,這些函數的計算只有通過(guò)級數展開(kāi)算法實(shí)現,但計算量太大�？紤]到三角函數的周期性,建立了一人1024點(diǎn)的正弦函數和余弦函數表,其分辨率完全能滿(mǎn)足精度要求。 （2）由于TMS320F206 DSP芯片為定點(diǎn)運算器件,因此需要將浮點(diǎn)運算轉換為定點(diǎn)運算。為保證計算精度,將數據定標設定為可動(dòng)態(tài)調節,數據表達能力為從Q13（-4～+3.9998779）到Q0（-32768～+32767）。 （3）采用并行指令,充分利用TMS320F206四級流水線(xiàn)操作,來(lái)提高程序運行速度。 如： MAC ;乘并累加 APAC ;累加 SACH +,3,AR2 ;將計算結果左移3位后,存于當前輔助寄存器（AR）所指的存儲器單元中,并將AR內容加1,最后,將AR2設定為當有AR。 （4）對運算過(guò)程進(jìn)行優(yōu)化,既要減少計算量,又要防止計算溢出;在混合運算中采用“先除后加”、“加減交叉”的方法。 （5）盡量采用移位運算代替乘除運算,以提高運行速度和計算精度。 通過(guò)以上方法,實(shí)現了機器人運行學(xué)計算的實(shí)時(shí)性和準確性。 4 仿真計算 以四重四面體為例,建立如圖5所示的基礎坐標系XYZ,末端參考點(diǎn)H位于末端平臺EFG的中點(diǎn)。設參考點(diǎn)H在基礎坐標系中從點(diǎn)（0.522689,-0.818450,0.472752）直線(xiàn)運動(dòng)到點(diǎn)（0.771439,-0.965700,0.721502）,只實(shí)現空間的位置運動(dòng),不實(shí)現姿態(tài)。動(dòng)態(tài)的整個(gè)時(shí)間T設為5秒,運動(dòng)軌跡分為等時(shí)間間隔的100個(gè)區間。設各定長(cháng)構件長(cháng)度為1m。 中間變量q曲線(xiàn)和中間變量q誤差曲線(xiàn)如圖6和圖7所示。從誤差曲線(xiàn)可看出,采用TMS329F206DSP芯片進(jìn)行的運動(dòng)學(xué)計算精度較高。經(jīng)過(guò)實(shí)測,該計算程序運行時(shí)間為34ms。（TMS320F206芯片指令周期為25ns）,可見(jiàn)其實(shí)時(shí)性較強。 本文提出的采用TMS320F206 DSP的芯片進(jìn)行冗余度TT-VGT機器人運動(dòng)學(xué)計算方案,充分利用了DSP并行特性進(jìn)行機器人位姿逆解計算,在程序設計中采用了多種技巧優(yōu)化計算。仿真結果表明,該方案計算誤差較小,實(shí)時(shí)性強。因此,可將其應用于機器人控制系統,實(shí)現機器人計算和控制任務(wù)一體化,從而大大縮小機器人體積、降低成本、增強靈活性、具有較強的先進(jìn)性和實(shí)用性。 |
