【文章內(nèi)容簡(jiǎn)介】 顯示運(yùn)動(dòng)方式和參數(shù)及指南針的值，同時(shí)將編碼器的值通過(guò)無(wú)線模塊傳送給 PC 上位機(jī)。 DSP 下位機(jī)將按照程序設(shè)定執(zhí)行 上位機(jī)設(shè)定的運(yùn)動(dòng)方式， 只有上位機(jī)重新發(fā)送新的運(yùn)動(dòng)方式才會(huì)改變。 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)資源分配 功能模塊 名稱 實(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è)重要的模塊。分為 電源模塊、無(wú)線通信 模塊、指南針模塊、液晶顯示模塊， DSP 下位機(jī)模塊、電 機(jī) 及 驅(qū)動(dòng)模塊 和全向輪 。其中 DSP 下位機(jī) 節(jié)已經(jīng)介紹，在此不再 敘述 。 ? 電源模塊 在控制系統(tǒng)中，主要用到系統(tǒng)板電源模塊 、 DSP、外圍器件和 12V 電機(jī)供電 。TMS320LF2407A 不采用 5V供電而是采用低電壓 供電方式 。 電池選用 15V的鋰電池，需要將 15V轉(zhuǎn)換為 12V、 5V、 。電機(jī)需要大電流在此選用兩塊 2576 開(kāi)關(guān)電源進(jìn)行處理。一塊轉(zhuǎn)換為 12V單獨(dú)供電機(jī)，另一塊轉(zhuǎn)換 5V供外圍 5V器件和 DSP開(kāi)發(fā)板。在開(kāi)發(fā)板上 選用 LM1117 作為系統(tǒng)板電源模塊的主芯片，把輸入的 +5V 變?yōu)?。 ? 無(wú)線 通信模塊 DSP2407A 處理器內(nèi)嵌的異步串行接口 SCI 是一個(gè)標(biāo)準(zhǔn)的通用異步接收 /發(fā)送通訊接口，接收和發(fā)送都是雙緩沖的，有自己的是能和中斷位，支持半雙工和全雙工工作。通訊接口有 SCITXD 和 SCIRXD 的 TTL 電平信號(hào)。本設(shè)計(jì)是使用 ASWLCOM 無(wú)線模塊 。本設(shè)計(jì)用的 是 RS232 電平接口 ， 通訊波特率設(shè)置 9600bps，無(wú)奇偶校驗(yàn)位， 8 位數(shù)據(jù)位和一個(gè)停止位 。 ASWLCOM 的鏈接方法如圖 10 所 示。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 11 圖 10 無(wú)線發(fā)送接收模塊 連接圖 ? 指南針模塊 采用 CY— 26 平面數(shù)字指南針模塊圖具有磁偏角補(bǔ)償功能，通過(guò)硬磁補(bǔ) 償和設(shè)置地磁夾角能夠表示成全向機(jī)器人的夾角。指南針模塊使用 232（ 9600bps）協(xié)議及 IIC協(xié)議。本設(shè)計(jì)中采用 IIC 通信協(xié)議。測(cè)量范圍 ： 0176。~ 360176。 、分辨率： 176。、測(cè)量精度 ： 1 176。 、重復(fù)精度 ： 1 176。 、響應(yīng)頻率： 30 HZ。 DSP 2407A 中 SCI 已經(jīng)使用了無(wú)線模塊，所以在本很設(shè)計(jì)中使用 IIC 通訊協(xié)議進(jìn)行通信。 IIC 通信模塊地址及數(shù)據(jù)含義見(jiàn)表 2。 表 2 內(nèi)部數(shù)據(jù) 模塊內(nèi)部地址 地址的數(shù)據(jù)含義 0x00 未用到 0x01 角度值高 8 位 0x02 角度值低 8 位 0x03 磁偏角高 8 位 0x04 磁偏角低 8 位 0x05 未用到 0x06 未用到 0x07 校準(zhǔn)等級(jí)值 實(shí)際當(dāng)前角度值為上表“角度值高 8 位”與“角度值低 8 位”合成的 16 位數(shù)據(jù)，數(shù)據(jù)范圍 03599（因?yàn)榉直媛蕿?176。） 實(shí)際當(dāng)前磁偏角值為“磁偏角高 8 位”與“磁偏角低 8 位”合成的 16 位數(shù)據(jù)，數(shù)據(jù)范圍 03599（因?yàn)榉直媛蕿?176。） 通過(guò) IIC 控制指南針命令， 磁偏角的值，范圍 03599（因?yàn)榉直媛蕿?176。） ，是由 2個(gè) 8位的數(shù)據(jù)組成，當(dāng)修改模塊磁偏角時(shí)，分為高 8 位值，低 8位值，寫(xiě)入模塊。其命令見(jiàn)表 3。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 12 表 3 模塊命令表 命令值 作用 0x00+0x31 角度測(cè)量 0x00+0xC0 校準(zhǔn) 0x00+0XC1 停止校準(zhǔn) 0x00+(0xA0+0xAA+0XA5+0XC5) 恢復(fù)出廠設(shè)置 0x00+(0xA0+0xAA+0XA5+IIC_ADDR) IIC 地址修改 0x03+磁偏高 8 位值 磁偏角修改 0x03+磁偏高 8 位值 磁偏角修改 ? 液晶顯示模塊 使用 英寸 TFT 全彩屏，顯示運(yùn)動(dòng)過(guò)程中各種狀態(tài)和參數(shù) 如圖 11， 通過(guò)顯示模塊能夠很好的顯示運(yùn)動(dòng)狀態(tài)，同時(shí)系統(tǒng)出現(xiàn)問(wèn)題也能通 過(guò)顯示反應(yīng)出來(lái)。 圖 11 液晶顯示 ? 電機(jī)及驅(qū)動(dòng)模塊 電機(jī)使用 Faulhaber 帶雙路編碼器減速電機(jī) 2230 12V220， 減速后速： 220RPM（轉(zhuǎn)每分鐘） ， 每圈脈沖： 512CPR（脈沖每圈） ，對(duì)于編碼器將在下章介紹。 對(duì)于電機(jī)驅(qū)動(dòng)本設(shè)計(jì)選用 L298N，它 是一款具有 15 管腳 Multiwatt 封裝的大功率集成驅(qū)動(dòng)芯片。它是一個(gè) 能夠 承受高電壓， 大 電流的雙 H 橋驅(qū)動(dòng)。 L298N 可驅(qū)動(dòng) 2 個(gè)電機(jī)， OUT1， OUT2 和 OUT3， OUT4 之間可分 別接電機(jī)。 L298N 功能模塊如表 4 所示 。 表 4 L298N 功能模塊 ENA IN1 IN2 運(yùn)轉(zhuǎn)狀態(tài) 0 X X 停止 1 1 0 正轉(zhuǎn) 1 0 1 反轉(zhuǎn) 1 1 1 剎停 1 0 0 停止 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 13 L298N驅(qū)動(dòng)電路如圖 12所示。 單個(gè)電機(jī)的控制方法 如下： 通過(guò) IO口控制 IN1和 IN2，使能 ENA 為 PWM，則通過(guò)控制 IN1 和 IN2 來(lái)實(shí)現(xiàn)正反轉(zhuǎn)，通過(guò) ENA 的 PWM 占空比調(diào)節(jié) 速度 快慢。 N1N2N3N4E1E2Input 15Input 27Input 310Input 412Enable A6Enable B11GND8Vss9Vs4motor 112motor 123motor 2113motor 2214Sense 11Sense 215*1L298NGNDGNDD1 D2D8D7D3 D4D6D5MB1MotorMB2Motor5V 12VGND100uFC2C1C4GNDGND100uFC3GND 圖 12 L298N 電機(jī)驅(qū)動(dòng)電路原理圖 ? 全向輪 作為全向 運(yùn)動(dòng)的驅(qū)動(dòng)部分，驅(qū)動(dòng)輪與普通驅(qū)動(dòng)輪有著重要的區(qū)別。在驅(qū)動(dòng)輪的邊 緣處有側(cè)滑的小輪，他們能夠使得輪子向前后提供驅(qū)動(dòng)力，同時(shí)能夠使的側(cè)面與地面很小的摩擦力，實(shí)現(xiàn)側(cè)面移動(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)。電機(jī) 的 控制對(duì)整個(gè) 系統(tǒng)是至關(guān)重要的。 本章主要分析系統(tǒng)的 PID 控制和 MATLAB仿真。 電機(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)的。為使得電機(jī)能夠穩(wěn)、準(zhǔn)、快的執(zhí)行 ， 而 PID 是根據(jù)偏差的比例、積分、微分的線性 組合進(jìn)行反饋控制，是迄今為止工業(yè)中應(yīng)用最為廣泛的一種控制方法。 所以本設(shè)計(jì) 采用 PID 分別對(duì)三個(gè)電機(jī)進(jìn)行閉環(huán)控制。 數(shù)字 PID 可分為位置式 PID 和增量式 PID 控制。 由于位置式 PID 是全量輸出，所以每次輸出均與過(guò)去的狀態(tài)有關(guān)，計(jì)算時(shí)要對(duì) e（ k）進(jìn)行累加， DSP 運(yùn)算量大。 而增量式PID 控制的優(yōu)點(diǎn)是只輸出增量，計(jì)算誤差動(dòng)作時(shí)造成的影響小，同時(shí)對(duì)控制算法不需要基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 14 累加，增量只跟最近幾次的偏差采樣值有關(guān)，易獲得較好的控制效果 [9]。 本設(shè)計(jì)采用增量式 PID，其閉環(huán)系統(tǒng)見(jiàn)圖 13所示。 P I D 調(diào) 節(jié) 器P W M脈 寬 調(diào) 制電 機(jī) 驅(qū) 動(dòng) 電 機(jī)編 碼 器s ( t )+e ( t )_V ( t ) 圖 13 電機(jī)閉環(huán)控制系統(tǒng) 已知模擬 PID 控制算法的傳遞函數(shù) 為式 (8)： ? ? ? ?? ? 11PdiUSD S K S T sE S T??? ? ? ????? (8) 數(shù)字式 PID 前一刻輸出的值 為式 (9)： (9) 前一時(shí)刻減去后一時(shí)刻的值 為式 (10)： (10) 數(shù)字 PID最終輸出量 為式 (11)： (11) 轉(zhuǎn)速檢測(cè) 本設(shè)計(jì)選用增量式 PID 即 節(jié)中所闡述的 PID。對(duì)于 PID 控制反饋必不可少，在本設(shè)計(jì)使用的是 Faulhaber 帶雙路編碼器減速電機(jī) ，即增量式光電編碼器 來(lái)做為反饋環(huán)節(jié) ，其工作原理如圖 14。光源經(jīng)過(guò)碼盤(pán)和檢測(cè)光柵照射的信號(hào) 通過(guò)光電檢測(cè)器件檢測(cè)，檢測(cè)出的正弦值經(jīng)轉(zhuǎn)換電路轉(zhuǎn)換成矩形波，最后出來(lái) A 相 B 相 Z 相。 A 相 B 相相差 90度，通過(guò)軟件能夠很好的得到此時(shí)電機(jī)的速度和方向。對(duì) AB 相的采集是電機(jī)閉環(huán)控制的反饋量，關(guān)乎電機(jī) PID 控制是 否能達(dá)到穩(wěn)準(zhǔn)快的要求。 本設(shè)計(jì)使用輸入捕捉 CAP 功能對(duì)脈沖進(jìn)行采集，將采集量進(jìn)行相應(yīng)處理即得到 PID 控制的反饋 。 ? ?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)用最廣泛的控制算法。針對(duì)電機(jī)模型，這里設(shè)定電機(jī)的模型為2 3() 2 3 4Gs ss? ??，針對(duì)此電機(jī)模型進(jìn)行 MATLAB 仿真，觀察 PID 控制算法的控制效果 [10][11]。控制結(jié)果比較曲線如圖 15 所示。綠色曲線為期望值，藍(lán)色曲線為 PID控制跟蹤曲線，紅色曲線為誤差曲線，實(shí)際速度可以通過(guò)編碼 器反饋?？梢钥闯?PID 控制方式的響應(yīng)速度、跟蹤精度可以達(dá)到系統(tǒng)要求。 針對(duì) 三軸運(yùn)動(dòng)控制系統(tǒng) 設(shè)計(jì)以下速度合成實(shí)驗(yàn)，假設(shè)機(jī)器人坐標(biāo)系和場(chǎng)地坐標(biāo)系具有同樣的方位，要求機(jī)器人（作為整體）沿 45176。（ ?45?? ）的方向從靜止開(kāi)始，勻加速運(yùn)動(dòng) 2 秒，達(dá)到 ，然后勻速運(yùn)動(dòng) 2 秒，再在 1 秒之內(nèi)以勻減速將速度減為 0，從啟動(dòng)到停止共運(yùn)行 5s。運(yùn)行結(jié)果如圖 16 所示，綠色曲線為期望曲線，藍(lán)色虛線為 PID控制跟蹤曲線，從圖中可以看出， PID 控制可以達(dá)到 系統(tǒ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 所示。綠色曲線為期望曲線，藍(lán)色曲線基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 16 為 PID 控制跟蹤曲線，從曲線中可以看出， PID 控制可以滿足系統(tǒng)的控制要求。 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 0 . 4 5