【正文】 // LCD_PutString(0,143,指南針 :,0x001f,0x07e0)。 //PWM初始化 ILI9163_init()。 void PWM_Init()。 (6) 實(shí)現(xiàn)了全向運(yùn)動(dòng)控制系統(tǒng)的所有運(yùn)動(dòng)形式，且經(jīng)過(guò)試驗(yàn)測(cè)試驗(yàn)證了其準(zhǔn)確性。 (3) 運(yùn)用 TMS320LF2407A 芯片為控制核心， 使 用了 其模塊 UART（ SCI） 、 IIC（模擬）、PWM、 I/O、 CAP、外部擴(kuò)展 RAM。 去除干擾的方法只能經(jīng)過(guò)硬磁補(bǔ)償來(lái)減少很少部分干擾，大多數(shù)干擾無(wú)法去除。 下圖 31 為系統(tǒng)由（ 20 20）到（ 80 80）邊移動(dòng)邊旋轉(zhuǎn)，黑色三角箭頭為系統(tǒng)方向。的虛線 看是否轉(zhuǎn)到位，下圖 30 為試驗(yàn)場(chǎng)地轉(zhuǎn)角 有 0176。能夠滿足要求，效果如下圖 27 所示。為提供足夠摩擦力結(jié)合實(shí)驗(yàn)室條件，選擇飛思卡爾賽道使用的 KT 板 ，做成 120CM*100CM 的試驗(yàn)場(chǎng)地，并在場(chǎng)地上標(biāo)上邊距為 20cm的方格，同時(shí)為驗(yàn)證轉(zhuǎn)角正確性標(biāo)上了相隔 45176。 演示 同速度合成平移 同速度合成平移 在上位機(jī)選擇不同的運(yùn)動(dòng)方式和相關(guān)參數(shù)，通過(guò)無(wú)線傳輸模塊傳送給 DSP，在 DSP上對(duì)接收到的數(shù)據(jù)根據(jù)協(xié)議進(jìn)行處理，提取 出運(yùn)動(dòng)方式和相關(guān)參數(shù)，然后進(jìn)行控制小車(chē)運(yùn)動(dòng)。 同時(shí)方便不同電腦的串口情況需求，設(shè)計(jì)上位機(jī)軟件時(shí)設(shè)置了 4 個(gè)COM 口（ 14）。 P I D 標(biāo) 志 位 清 零得 到 三 編 碼 器計(jì) 數(shù) 值計(jì) 算 三 驅(qū) 動(dòng) 輪的 誤 差e i ( n ) , e i ( n 1 ) , e i ( n 2 )P I D 標(biāo) 志是 否 為 1根 據(jù) 增 量 式 P I D 公 式計(jì) 算 三 電 機(jī) 速 度返 回否是無(wú) 線 發(fā) 送子 程 序 圖 22 電機(jī) 控制子程序流程圖 PID 執(zhí)行周期為 3ms，利用定時(shí)器計(jì)時(shí)，當(dāng)時(shí)間到之后 PID 標(biāo)志位置 1，進(jìn)入 PID子程序后清楚此標(biāo)志位。 其中“屏幕顯示相應(yīng)運(yùn)動(dòng)界面”顯示的是運(yùn)動(dòng)狀態(tài)的界面。如接收到的信號(hào)是平移運(yùn)動(dòng)則進(jìn)入平移運(yùn)動(dòng)相應(yīng)子程序，計(jì)算電機(jī)速度值并賦值給 PWM 寄存器，電機(jī)驅(qū)動(dòng)控制驅(qū)動(dòng)輪轉(zhuǎn)速。燒寫(xiě) FLASH 即將編譯好的程序燒寫(xiě)進(jìn) DSP 內(nèi)部FLASH 中，此種方法能夠是 DSP 獨(dú)立運(yùn)行軟件，具有掉電保持功能。 5 系統(tǒng)軟件設(shè)計(jì) 根據(jù)前幾章模型建立分析和硬件系統(tǒng)構(gòu)建，本章主要介紹全向運(yùn)動(dòng)控制系統(tǒng)程序編寫(xiě) 。綠色曲線為期望值，藍(lán)色曲線為 PID控制跟蹤曲線，紅色曲線為誤差曲線，實(shí)際速度可以通過(guò)編碼器反饋。對(duì)于 PID 控制反饋必不可少，在本設(shè)計(jì)使用的是 Faulhaber 帶雙路編碼器減速電機(jī) ，即增量式光電編碼器 來(lái)做為反饋環(huán)節(jié) ，其工作原理如圖 14。 電機(jī) PID 控制 電機(jī)是運(yùn)動(dòng)系統(tǒng)的執(zhí)行機(jī)構(gòu)，不管 DSP 計(jì)算的多么正確，但是電機(jī)執(zhí)行達(dá)不到要求，同樣系統(tǒng)是無(wú)法實(shí)現(xiàn)全向運(yùn)動(dòng)的。 L298N 功能模塊如表 4 所示 。） 通過(guò) IIC 控制指南針命令， 磁偏角的值，范圍 03599（因?yàn)榉直媛蕿?176。 、分辨率： 176。本設(shè)計(jì)是使用 ASWLCOM 無(wú)線模塊 。 ? 電源模塊 在控制系統(tǒng)中，主要用到系統(tǒng)板電源模塊 、 DSP、外圍器件和 12V 電機(jī)供電 。 DSP 下位機(jī)的無(wú)線模塊接收上位機(jī)發(fā)送的數(shù)據(jù)，經(jīng) RS232 通信協(xié)議發(fā)送給 DSP。 ? 片內(nèi)有高達(dá) 32K16位的 Flash程序存儲(chǔ)器 ： 高達(dá) 16位的數(shù)據(jù) /程序 RAM；544 字節(jié) 的 雙端口 RAM(DARAM)； 2K 字的單口 RAM(SARAM)。在前文 中已經(jīng)推導(dǎo)出公式 (1)。 圖 7 平移最大速度 模型 原地旋轉(zhuǎn)運(yùn)動(dòng) 全向 機(jī)器人不但要平移，而且很多時(shí)候都需要轉(zhuǎn)向，轉(zhuǎn)向 運(yùn)動(dòng) 分為原地旋轉(zhuǎn) 運(yùn)動(dòng) 和邊平移邊旋轉(zhuǎn) 運(yùn)動(dòng) 。 cos( )sin( )xyVVVV ?????? ??? (5) 由式 (4)和式 (5)我們可以得到三個(gè)驅(qū)動(dòng)輪的速度 ，見(jiàn)式 (6)。 (4) 忽略驅(qū)動(dòng)輪轉(zhuǎn)動(dòng)過(guò)程中 L 的變化，取 L=L1=L2=L3。 (a) 運(yùn)動(dòng)學(xué)建模 (b) 系統(tǒng)整體速度矢量分解 圖 5 運(yùn)動(dòng)學(xué)模型 根據(jù) 運(yùn)動(dòng)學(xué) 模型 分析出 各個(gè)驅(qū)動(dòng)輪的速度 為下式 (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)中：由機(jī)械結(jié)構(gòu)可知 ? =30 o， ? 取 逆時(shí)針為正， ?? 為角速度且逆時(shí)針為正。 為實(shí)現(xiàn) 全向運(yùn)動(dòng)控制，首先對(duì)系統(tǒng)進(jìn)行運(yùn)動(dòng)方式建模 [5]， ， 建立運(yùn)動(dòng)學(xué)模型。 第 5 章：系統(tǒng)軟件設(shè)計(jì)。 介紹此課題的現(xiàn)狀和未來(lái)發(fā)展前景。 對(duì)然全向運(yùn)動(dòng)機(jī)器人有了很多，但是很多都在實(shí)驗(yàn)室，還不能在實(shí)際的環(huán)境使用，全向運(yùn)動(dòng) 機(jī)器人還需要很長(zhǎng)的時(shí)間發(fā)展。 全向運(yùn)動(dòng)機(jī)器人是移動(dòng)機(jī)器人中的一部分，其高靈活性和準(zhǔn)確性越來(lái)越突出其優(yōu)越性。全向運(yùn)動(dòng)機(jī)器人其高靈活度，勢(shì)必會(huì)在機(jī)器人領(lǐng)域發(fā)揮不可代替的作用。 By the whole motion control system software to write and online calibration with relevant hardware, the system realized the omnidirectional movement we expected. Through the results analysis and experimental verification, the accuracy have already meet requirements of various sports state. Key Words: DSP。通過(guò)試驗(yàn) 驗(yàn)證和結(jié)果分析，各種運(yùn)動(dòng)狀態(tài)的準(zhǔn)確性 已達(dá)到 性能的基本 要求 。 機(jī)器人技術(shù)已經(jīng)在人們的生活中越來(lái)越發(fā)揮著重要的作用。同時(shí)機(jī)器人技術(shù) 不能只靠少量研究人員，必須從學(xué)生時(shí)代培養(yǎng)機(jī)器人技術(shù)。 RoboCup 的最終目標(biāo)是：到 21 世紀(jì)中葉，一支完全自治的人形機(jī)器人基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 2 足球隊(duì)?wèi)?yīng)該能在遵循國(guó)際足聯(lián)正式規(guī)則的比賽中，戰(zhàn)勝最 近的人類(lèi)世界杯冠軍隊(duì)。 全向運(yùn)動(dòng)機(jī)器人在社會(huì)服務(wù)、教育、娛樂(lè)、軍事和環(huán)境探測(cè)領(lǐng)域都將發(fā)揮著不可代替的作用。選擇 TMS320LF2407A 為核心控制芯片，選擇電機(jī)、驅(qū)動(dòng)模塊、顯示模塊、無(wú)線通訊模塊、電源模塊等等，組建硬件系統(tǒng) 第 4 章 ： 系統(tǒng)運(yùn)動(dòng)控制部分的設(shè)計(jì)和 MATLAB 仿真 。 其結(jié)構(gòu)圖和實(shí)物付如圖 4 所示 。 為方便 建立運(yùn)動(dòng)學(xué)模型及 計(jì)算取 L=L1=L2=L3。 為使模型成立， 機(jī)器人應(yīng) 該符合以下幾點(diǎn) 要求 ： (1) 驅(qū)動(dòng)輪與地面有足夠的摩擦力，不存在打滑現(xiàn)象。 由于是 平移 不考慮旋轉(zhuǎn)運(yùn)動(dòng)，我們將模型進(jìn)行簡(jiǎn)化 ，其簡(jiǎn)化后運(yùn)動(dòng)學(xué)模型圖如圖 6 所示 。 ~360176。為了表述直觀，建立原地旋轉(zhuǎn)運(yùn)動(dòng)模型，其模型圖如圖 8 所示。 ? 高可靠性、可編程性： TMS320LF2407A 的 16 位定點(diǎn) DSP 內(nèi)核為模擬系統(tǒng)的設(shè)計(jì)者提供了一個(gè)不犧牲系統(tǒng)精度和性能的數(shù)字解決方案。 為了滿足機(jī)器人大量擴(kuò)展傳感器的需要和減少處理器外圍數(shù)字邏輯器件的使用數(shù)量，該運(yùn)動(dòng)控制卡采用 Altera 公司的復(fù)雜可編程邏輯器件（ CPLD）來(lái)擴(kuò)展 DSP 的 I/O端口和實(shí)現(xiàn)外圍數(shù)字邏輯電路設(shè)計(jì) [8]。 P C 上 位 機(jī)無(wú) 線 通 信模 塊R S 2 3 2無(wú) 線 通 信模 塊D S P 下 位 機(jī)指 南 針 模 塊電 源 模 塊電機(jī)驅(qū)動(dòng)模塊電 機(jī) 1增 量 編 碼 器電 機(jī) 2增 量 編 碼 器電 機(jī) 3增 量 編 碼 器液 晶 顯 示R S 2 3 2圖 9 硬件系統(tǒng)結(jié)構(gòu)圖 根據(jù)系統(tǒng)要求分配系統(tǒng)配置（見(jiàn)表 1）： 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 10 表 1 DSP2407A系統(tǒng)資源分配 功能模塊 名稱(chēng) 實(shí)現(xiàn)功能 通訊模塊 SCI（ PA0、 PA1） 通過(guò)無(wú)線模塊 與電腦上位機(jī)通訊 (2 個(gè) I/O) 電機(jī)調(diào)速 電機(jī)轉(zhuǎn)向 PWM ： PE PE PE6 3 個(gè) PWM 控制控制調(diào)速，（ 3 個(gè) I/O） 正反轉(zhuǎn)： PC0 PF2 PF45 每個(gè)電機(jī)兩個(gè)正反轉(zhuǎn)控制信號(hào)控制正反轉(zhuǎn) (6 個(gè) I/O) 編碼值讀取 CAP： PF0、 PF PA3 利用 CAP 脈沖捕捉功能獲取編碼器脈沖，得到速度 (3 個(gè) I/O) 液晶顯示 PF PA47 液晶使能控制 IO 口 (5 個(gè) I/O) PB07 液晶數(shù)據(jù) 8 位數(shù)據(jù)口 (8 個(gè) I/O) 指南針模塊 IIC:PE PE3 軟件模擬 IIC 與指 南針通信， PE1 是 SCL , PE3 是SDA(2 個(gè) I/O) 硬件系統(tǒng) 基本 模塊 根據(jù)圖 9 硬件系統(tǒng)結(jié)構(gòu)圖 所示，本節(jié)將分析幾個(gè)重要的模塊。在開(kāi)發(fā)板上 選用 LM1117 作為系統(tǒng)板電源模塊的主芯片，把輸入的 +5V 變?yōu)?。本設(shè)計(jì)中采用 IIC 通信協(xié)議。 IIC 通信模塊地址及數(shù)據(jù)含義見(jiàn)表 2。 對(duì)于電機(jī)驅(qū)動(dòng)本設(shè)計(jì)選用 L298N，它 是一款具有 15 管腳 Multiwatt 封裝的大功率集成驅(qū)動(dòng)芯片。 4 系統(tǒng)運(yùn)動(dòng)控制 的 MATLAB仿真 本設(shè)計(jì)的 全向運(yùn)動(dòng)控制系統(tǒng) 使用三軸全向控制 ，即控制 3 個(gè)電機(jī)相互協(xié)調(diào)運(yùn)動(dòng)來(lái)實(shí)現(xiàn)全向輪動(dòng)。 而增量式PID 控制的優(yōu)點(diǎn)是只輸出 增量，計(jì)算誤差動(dòng)作時(shí)造成的影響小，同時(shí)對(duì)控制算法不需要基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 14 累加，增量只跟最近幾次的偏差采樣值有關(guān)，易獲得較好的控制效果 [9]。 ? ?10 00( 1 ) ( 1 ) ( ) ( 1 ) ( 2 )k dpiiTTu k K e k e i e k e k????? ? ? ? ? ? ? ??????? ? ? ? ? ?( 1 ) ( ) ( 1 )( ) ( 1 ) ( ) 2 ( 1 ) ( 2 )p i dk u k u kk e k e k k e k k e k e k e k? ? ? ? ?? ? ? ? ? ? ? ? ?( ) ( 1 ) ( )u k u k u k? ? ? ?基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 15 14 增量式編碼器輸出信號(hào)波形 MATLAB 仿真 在三軸控制系統(tǒng)中， PID 是應(yīng)用最廣泛的控制算法。 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 0 . 200 . 20 . 40 . 60 . 811 . 20 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 0 . 200 . 20 . 40 . 60 . 811 . 21 . 41 . 6 圖 15 基于單軸驅(qū)動(dòng)電機(jī)實(shí)驗(yàn) 圖 16 三軸運(yùn)動(dòng)控制系統(tǒng)的速度合成實(shí)驗(yàn) 接著將此運(yùn)動(dòng)通過(guò)運(yùn)運(yùn)學(xué)分解，將速度分解至三個(gè)輪子上， 并分別對(duì)每個(gè)輪子的驅(qū)動(dòng)軸單獨(dú)控制，三個(gè)輪子相應(yīng)跟蹤曲線如圖 17 所示。其中利用 Setup CCStudio 對(duì)開(kāi)發(fā)工具參考硬件進(jìn)行相應(yīng)配置 ，基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 17 利用 CCStudio 建立工程 編寫(xiě)軟件 ,其開(kāi)發(fā)周期示意圖為圖 18 所示 [12]。其全向運(yùn)動(dòng)方式經(jīng)過(guò)上位機(jī)無(wú)線傳輸指令，然后 DSP 接收指令進(jìn)行處理最后控制電機(jī)協(xié)調(diào)運(yùn)動(dòng)，根據(jù)指南針和編碼器進(jìn)行閉環(huán)控制， 以 達(dá)到最佳效果。 標(biāo) 志 位 清 零對(duì) 接 收 參 數(shù) 值 轉(zhuǎn) 換A S C I I 碼 轉(zhuǎn) 十 進(jìn) 制上 次 旋 轉(zhuǎn) 運(yùn) 動(dòng) 狀態(tài) 是 否 改 變屏 幕 顯 示相 應(yīng) 運(yùn) 動(dòng) 界 面否是按 照 協(xié) 議 得 到各 種 參 數(shù) 值利 用 指 南 針 及 相 關(guān)公 式 得 到 三 電 機(jī) 值屏 幕 顯 相 關(guān) 參 數(shù)及 指 南 針 角 度返 回 圖 21 運(yùn)動(dòng)方式 子程序流程圖 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 20 本課題設(shè)計(jì)的軟件有三種運(yùn)動(dòng)方式，即有三種運(yùn)動(dòng)狀態(tài)控制子程序。電機(jī)的穩(wěn)、準(zhǔn)、快的運(yùn)行將會(huì)使運(yùn)動(dòng)方式更接近理論目標(biāo)。 上位機(jī)軟件及通訊協(xié)議 上位機(jī)軟件是利用 的 MSOCOMM 控件編寫(xiě)。 本設(shè)計(jì)系統(tǒng)通訊協(xié)議為簡(jiǎn)單通訊協(xié)議 [15]。設(shè)計(jì)試驗(yàn)場(chǎng)地，記錄測(cè)試試驗(yàn)數(shù)據(jù)，對(duì)結(jié)果進(jìn)行分析。在系統(tǒng)中每隔 3 ms 記錄一次 3ms 內(nèi)記錄數(shù)。其快速性和精確性為系統(tǒng)準(zhǔn)確的方位提供可能。轉(zhuǎn)角示意圖 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng) 軟件設(shè)計(jì) 26 通過(guò)指南針顯示能夠很清晰的看到，最終轉(zhuǎn)到的角度已經(jīng)達(dá)到了設(shè)定角度，誤差在? 1176。 在室內(nèi)鋼筋結(jié)構(gòu)的環(huán)境下各個(gè)位置受到的磁