【正文】 電機控制子程序 ........................................... 20 無線發(fā)送子程序 ........................................... 20 上位機軟件及通訊協(xié)議 .......................................... 21 6 實驗驗證及結(jié)果分析 .................................................. 22 試驗場地 ....................................................... 23 各項性能測試 .................................................. 24 速度 PID 測試 ............................................. 24 平移運動測試 ............................................. 24 原地旋轉(zhuǎn)測試 ............................................. 25 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 III 平移 +旋轉(zhuǎn)運動測試 ........................................ 26 影響因素分析 .................................................. 26 結(jié)束語 ................................................................ 28 參 考文獻 .............................................................. 29 附錄 .................................................................. 30 致謝 .................................................................. 42 基于 DSP 的全向運動控制系統(tǒng) 軟件設(shè)計 1 1 引言 隨 著 機器人技術(shù)的日新月異，機器人應(yīng)用領(lǐng)域也已從工業(yè)走向普通生活。全向運動機器人其高靈活度，勢必會在機器人領(lǐng)域發(fā)揮不可代替的作用。中國要想變成世界強國，也必須重視起機器人技術(shù)。 全向運動機器人是移動機器人中的一部分，其高靈活性和準(zhǔn)確性越來越突出其優(yōu)越性。 RoboCup 的最終目標(biāo)是在 2050 年成立一支完全自主的擬人機器人足球隊，能夠與人類進行一場真正意義上的足球賽。 對然全向運動機器人有了很多，但是很多都在實驗室，還不能在實際的環(huán)境使用，全向運動 機器人還需要很長的時間發(fā)展。 如圖 3 是 三種 全向機器人。 介紹此課題的現(xiàn)狀和未來發(fā)展前景。 第 3 章 ： DSP 及相關(guān)控制系統(tǒng)。 第 5 章：系統(tǒng)軟件設(shè)計。 2 全向運動控制系統(tǒng) 分析 全向運動控制系統(tǒng) 運動學(xué) 模型建立 在 引言 中介紹了全向 運動 控制的 幾種 結(jié)構(gòu)， 本設(shè)計 選用三輪結(jié)構(gòu)構(gòu)建 全向 運動系統(tǒng)[3][4]。 為實現(xiàn) 全向運動控制，首先對系統(tǒng)進行運動方式建模 [5]， ， 建立運動學(xué)模型。 iL (i=1,2,3)為機器人中心到 驅(qū)動輪 中心的距離 ,由于 本設(shè)計 使用的 瑞典 輪在旋轉(zhuǎn)的時候?qū)Φ孛娴慕佑|位置 一直在 變化， 會造成驅(qū)動輪與機器人中心的距離也一直在變化， 但變化值比較小 。 (a) 運動學(xué)建模 (b) 系統(tǒng)整體速度矢量分解 圖 5 運動學(xué)模型 根據(jù) 運動學(xué) 模型 分析出 各個驅(qū)動輪的速度 為下式 (1)： 1 si n( ) c os( )2 c os si n3 si n( ) c os( )XyXyXyV V V LV V V LV V V L? ? ?? ? ?? ? ????? ? ? ? ? ? ? ? ????? ? ???? ? ? ? ? ? ? ? ??? (1) 式 (1)中：由機械結(jié)構(gòu)可知 ? =30 o， ? 取 逆時針為正， ?? 為角速度且逆時針為正。 在實際中，路 徑規(guī)劃、位姿控制，通過矢量分解、坐標(biāo)變化，即可轉(zhuǎn)換為對機器人每個軸驅(qū)動電機的運動控制。 (4) 忽略驅(qū)動輪轉(zhuǎn)動過程中 L 的變化，取 L=L1=L2=L3。 平 移 運動 平移 是基本的也是比較常用的運動形式。 cos( )sin( )xyVVVV ?????? ??? (5) 由式 (4)和式 (5)我們可以得到三個驅(qū)動輪的速度 ，見式 (6)。 如圖 7， 其中圓是以驅(qū)動輪最大速度 maxV 大小為半徑的圓，紅色速度 V與 X 軸的角度為 γ ，隨著 γ 由 基于 DSP 的全向運動控制系統(tǒng) 軟件設(shè)計 7 0176。 圖 7 平移最大速度 模型 原地旋轉(zhuǎn)運動 全向 機器人不但要平移，而且很多時候都需要轉(zhuǎn)向，轉(zhuǎn)向 運動 分為原地旋轉(zhuǎn) 運動 和邊平移邊旋轉(zhuǎn) 運動 。 所以可以得知三個驅(qū)動 電機的矢量速度是相同的。在前文 中已經(jīng)推導(dǎo)出公式 (1)。 TMS320LF2407A 主要 特點 如下 ： ? 高性能、高速度：集成了高性能的 DSP 內(nèi)核和微處理器的片內(nèi)外設(shè)；每秒 4000萬條指令 (40MIPS)的處理速度 。 ? 片內(nèi)有高達 32K16位的 Flash程序存儲器 ： 高達 16位的數(shù)據(jù) /程序 RAM；544 字節(jié) 的 雙端口 RAM(DARAM)； 2K 字的單口 RAM(SARAM)。 ? 5 個外部中斷 (兩個驅(qū)動保護、復(fù)位和兩個可屏蔽中斷 ) [6][7] 。 DSP 下位機的無線模塊接收上位機發(fā)送的數(shù)據(jù)，經(jīng) RS232 通信協(xié)議發(fā)送給 DSP。 DSP 下位機將按照程序設(shè)定執(zhí)行上位機設(shè)定的運動方式， 只有上位機重新發(fā)送新的運動方式才會改變。 ? 電源模塊 在控制系統(tǒng)中，主要用到系統(tǒng)板電源模塊 、 DSP、外圍器件和 12V 電機供電 。一塊轉(zhuǎn)換為 12V單獨供電機，另一塊轉(zhuǎn)換 5V供外圍 5V器件和 DSP開發(fā)板。本設(shè)計是使用 ASWLCOM 無線模塊 。指南針模塊使用 232（ 9600bps）協(xié)議及 IIC協(xié)議。 、分辨率： 176。 DSP 2407A 中 SCI 已經(jīng)使用了無線模塊，所以 在本很設(shè)計中使用 IIC 通訊協(xié)議進行通信。） 通過 IIC 控制指南針命令， 磁偏角的值，范圍 03599（因為分辨率為 176。 圖 11 液晶顯示 ? 電機及驅(qū)動模塊 電機使用 Faulhaber 帶雙路編碼器減速電機 2230 12V220， 減速后速： 220RPM（轉(zhuǎn)每分鐘） ， 每圈脈沖： 512CPR（脈沖每圈） ，對于編碼器將在下章介紹。 L298N 功能模塊如表 4 所示 。在驅(qū)動輪的邊緣處有側(cè)滑的小輪，他們能夠使得輪子向前后提供驅(qū)動力，同時能夠使的側(cè)面與地面很小的摩擦力，實現(xiàn)側(cè)面移動。 電機 PID 控制 電機是運動系統(tǒng)的執(zhí)行機構(gòu)，不管 DSP 計算的多么正確，但是電機執(zhí)行達不到要求，同樣系統(tǒng)是無法實現(xiàn)全向運動的。 由于位置式 PID 是全量輸出，所以每次輸出均與過去的狀態(tài)有關(guān)，計算時要對 e（ k）進行累加， DSP 運算量大。對于 PID 控制反饋必不可少，在本設(shè)計使用的是 Faulhaber 帶雙路編碼器減速電機 ，即增量式光電編碼器 來做為反饋環(huán)節(jié) ，其工作原理如圖 14。 本設(shè)計使用輸入捕捉 CAP 功能對脈沖進行采集，將采集量進行相應(yīng)處理即得到 PID 控制的反饋 。綠色曲線為期望值，藍(lán)色曲線為 PID控制跟蹤曲線，紅色曲線為誤差曲線，實際速度可以通過編碼器反饋。運行結(jié)果如圖 16 所示，綠色曲線為期望曲線，藍(lán)色虛線為 PID控制跟蹤曲線，從圖中可以看出， PID 控制可以達到 系統(tǒng)的控制要求。 5 系統(tǒng)軟件設(shè)計 根據(jù)前幾章模型建立分析和硬件系統(tǒng)構(gòu)建，本章主要介紹全向運動控制系統(tǒng)程序編寫 。 安裝 CCS 后生成兩個部分， 即 CCStudio 和 Setup CCStudio 。燒寫 FLASH 即將編譯好的程序燒寫進 DSP 內(nèi)部FLASH 中，此種方法能夠是 DSP 獨立運行軟件，具有掉電保持功能。 運動控制 軟件 設(shè)計 由于本課題是基于 DSP 全向運動控制系統(tǒng)軟件設(shè)計，側(cè)重控制全向運動，故本設(shè)計將檢測部分做了精簡，只需要采集指南針和編碼脈沖。如接收到的信號是平移運動則進入平移運動相應(yīng)子程序，計算電機速度值并賦值給 PWM 寄存器，電機驅(qū)動控制驅(qū)動輪轉(zhuǎn)速。 子系統(tǒng)流程圖 如下圖 21 所示。 其中“屏幕顯示相應(yīng)運動界面”顯示的是運動狀態(tài)的界面。 電機 控制子程序 電機控制主要是將其他子程序計算好的速度值進行 PID 控制，這樣 能夠使得三個電機安裝設(shè)定的速度運行。 P I D 標(biāo) 志 位 清 零得 到 三 編 碼 器計 數(shù) 值計 算 三 驅(qū) 動 輪的 誤 差e i ( n ) , e i ( n 1 ) , e i ( n 2 )P I D 標(biāo) 志是 否 為 1根 據(jù) 增 量 式 P I D 公 式計 算 三 電 機 速 度返 回否是無 線 發(fā) 送子 程 序 圖 22 電機 控制子程序流程圖 PID 執(zhí)行周期為 3ms，利用定時器計時，當(dāng)時間到之后 PID 標(biāo)志位置 1，進入 PID子程序后清楚此標(biāo)志位。如沒有發(fā)送完畢則繼續(xù)發(fā)送，如果發(fā)送完畢則返回主程序。 同時方便不同電腦的串口情況需求，設(shè)計上位機軟件時設(shè)置了 4 個COM 口（ 14）。點擊開始，則上位機軟件將寫入的參數(shù)進行相關(guān)處理并按照一定的協(xié)議組織要發(fā)送的數(shù)據(jù)， 通過RS232 經(jīng)無線模塊發(fā)送出去。 演示 同速度合成平移 同速度合成平移 在上位機選擇不同的運動方式和相關(guān)參數(shù)，通過無線傳輸模塊傳送給 DSP，在 DSP上對接收到的數(shù)據(jù)根據(jù)協(xié)議進行處理，提取 出運動方式和相關(guān)參數(shù)，然后進行控制小車運動。 6 實驗 驗證 及 結(jié)果 分析 為驗證本設(shè)計的各種全向運動是否準(zhǔn)確，本章將對各種全向運動形式進行試驗驗證。為提供足夠摩擦力結(jié)合實驗室條件，選擇飛思卡爾賽道使用的 KT 板 ，做成 120CM*100CM 的試驗場地，并在場地上標(biāo)上邊距為 20cm的方格，同時為驗證轉(zhuǎn)角正確性標(biāo)上了相隔 45176。 速度 PID 測試 對電機 PID效果進行測試，測試方法為記錄從啟動到穩(wěn)定后一段時間 內(nèi)相隔時間段增量編碼器記錄數(shù)。能夠滿足要求，效果如下圖 27 所示。 圖 29 系統(tǒng)平移曲線 原地旋轉(zhuǎn) 測試 系統(tǒng)安裝有數(shù)字指南針，能夠根據(jù)地球磁場感知方位。的虛線 看是否轉(zhuǎn)到位，下圖 30 為試驗場地轉(zhuǎn)角 有 0176。到 45176。 下圖 31 為系統(tǒng)由（ 20 20）到（ 80 80）邊移動邊旋轉(zhuǎn)，黑色三角箭頭為系統(tǒng)方向。 (2） 外部磁場干擾 ，數(shù)字指南針能夠很靈敏的感應(yīng)磁場，所以外部稍強點的磁場就能干擾系統(tǒng)， 一方面是 金屬鐵，其很容易磁化，導(dǎo)致變成弱磁鐵。 去除干擾的方法只能經(jīng)過硬磁補償來減少很少部分干擾，大多數(shù)干擾無法去除。 其特殊的移動方式使得全向機器人越來越得到重視和應(yīng)用，其未來的實用領(lǐng)域和前景將更加寬廣。 (3) 運用 TMS320LF2407A 芯片為控制核心， 使 用了 其模塊 UART（ SCI） 、 IIC（模擬）、PWM、 I/O、 CAP、外部擴展 RAM。同時能夠在上位機顯示系統(tǒng)現(xiàn)在運動的狀態(tài)和三個電機速度值。 (6) 實現(xiàn)了全向運動控制系統(tǒng)的所有運動形式，且經(jīng)過試驗測試驗證了其準(zhǔn)確性。 PE2=PWM8 PE4=PWM10， PE6=PWM12三電機 */ /* PC01 PF23 PF45電機正反轉(zhuǎn) */ /******************************************************************************/ include include void SystemInit()。 void PWM_Init()。 /****************************************************/ /* 主程序 */ /****************************************************/ main() { SystemInit()。 //PWM初始化 ILI9163_init()。 /*定時器 2初始化 */ asm( CLRC INTM )。 // LCD_PutString(0,143,指南針 :,0x001f,0x07e0)。 /* 關(guān)閉總中斷 */ asm( CLRC