【正文】 分 析實(shí) 時(shí) 調(diào) 試 、統(tǒng) 計(jì) 和 跟 蹤 圖 18 CCS 開(kāi)發(fā)周期示意圖 圖 19 CCStudio 開(kāi)發(fā)環(huán)境 仿真器 的作用：通過(guò) 編譯的程序下載 DSP 上兩種方式，一個(gè)是燒寫(xiě) FLASH，一個(gè)是仿真。其中利用 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]。 軟件開(kāi)發(fā)平臺(tái) 及 仿真器 本設(shè)計(jì)的核心控制芯片為 TMS320LF2407A，其軟件開(kāi)發(fā)平臺(tái)使用官方工具 CCS ，仿真器選用 。前兩節(jié)介紹了下位機(jī)全向運(yùn)動(dòng)系統(tǒng)使用的程序編寫(xiě)平臺(tái)軟件和整個(gè)系統(tǒng)流程圖及各部分子程序的流程圖。 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 0 . 4 5 0 . 4 0 . 3 5 0 . 3 0 . 2 5 0 . 2 0 . 1 5 0 . 1 0 . 0 500 . 0 5 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 0 . 200 . 20 . 40 . 60 . 811 . 21 . 41 . 6 (a) １號(hào)電機(jī) (b) ２號(hào)電機(jī) 0 0 . 5 1 1 . 5 2 2 . 5 3 3 . 5 4 4 . 5 5 1 . 21 0 . 8 0 . 6 0 . 4 0 . 200 . 2 (c) ３號(hào)電機(jī) 圖 17 三個(gè)電機(jī)相應(yīng)跟蹤曲線(xiàn) 從上面的速度以及位移跟蹤曲線(xiàn)的仿真結(jié)果可以看出， PID控制能夠達(dá)到三軸運(yùn)動(dòng)控 制系統(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)跟蹤曲線(xiàn)如圖 17 所示。（ ?45?? ）的方向從靜止開(kāi)始，勻加速運(yùn)動(dòng) 2 秒，達(dá)到 ，然后勻速運(yùn)動(dòng) 2 秒，再在 1 秒之內(nèi)以勻減速將速度減為 0，從啟動(dòng)到停止共運(yùn)行 5s?？梢钥闯?PID 控制方式的響應(yīng)速度、跟蹤精度可以達(dá)到系統(tǒng)要求?？刂平Y(jié)果比較曲線(xiàn)如圖 15 所示。 ? ?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ì) AB 相的采集是電機(jī)閉環(huán)控制的反饋量，關(guān)乎電機(jī) PID 控制是 否能達(dá)到穩(wěn)準(zhǔn)快的要求。光源經(jīng)過(guò)碼盤(pán)和檢測(cè)光柵照射的信號(hào) 通過(guò)光電檢測(cè)器件檢測(cè)，檢測(cè)出的正弦值經(jīng)轉(zhuǎn)換電路轉(zhuǎn)換成矩形波，最后出來(lái) A 相 B 相 Z 相。 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。 而增量式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ù)字 PID 可分為位置式 PID 和增量式 PID 控制。為使得電機(jī)能夠穩(wěn)、準(zhǔn)、快的執(zhí)行 ， 而 PID 是根據(jù)偏差的比例、積分、微分的線(xiàn)性 組合進(jìn)行反饋控制，是迄今為止工業(yè)中應(yīng)用最為廣泛的一種控制方法。 本章主要分析系統(tǒng)的 PID 控制和 MATLAB仿真。 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)。 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ū)別。 表 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所示。 L298N 可驅(qū)動(dòng) 2 個(gè)電機(jī)， OUT1， OUT2 和 OUT3， OUT4 之間可分 別接電機(jī)。 對(duì)于電機(jī)驅(qū)動(dòng)本設(shè)計(jì)選用 L298N，它 是一款具有 15 管腳 Multiwatt 封裝的大功率集成驅(qū)動(dòng)芯片。 基于 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ù)出廠(chǎng)設(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)。） ，是由 2個(gè) 8位的數(shù)據(jù)組成，當(dāng)修改模塊磁偏角時(shí)，分為高 8 位值，低 8位值，寫(xiě)入模塊。） 實(shí)際當(dāng)前磁偏角值為“磁偏角高 8 位”與“磁偏角低 8 位”合成的 16 位數(shù)據(jù)，數(shù)據(jù)范圍 03599（因?yàn)榉直媛蕿?176。 IIC 通信模塊地址及數(shù)據(jù)含義見(jiàn)表 2。 、響應(yīng)頻率： 30 HZ。、測(cè)量精度 ： 1 176。~ 360176。本設(shè)計(jì)中采用 IIC 通信協(xié)議。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 11 圖 10 無(wú)線(xiàn)發(fā)送接收模塊 連接圖 ? 指南針模塊 采用 CY— 26 平面數(shù)字指南針模塊圖具有磁偏角補(bǔ)償功能，通過(guò)硬磁補(bǔ) 償和設(shè)置地磁夾角能夠表示成全向機(jī)器人的夾角。本設(shè)計(jì)用的 是 RS232 電平接口 ， 通訊波特率設(shè)置 9600bps，無(wú)奇偶校驗(yàn)位， 8 位數(shù)據(jù)位和一個(gè)停止位 。通訊接口有 SCITXD 和 SCIRXD 的 TTL 電平信號(hào)。在開(kāi)發(fā)板上 選用 LM1117 作為系統(tǒng)板電源模塊的主芯片，把輸入的 +5V 變?yōu)?。電機(jī)需要大電流在此選用兩塊 2576 開(kāi)關(guān)電源進(jìn)行處理。TMS320LF2407A 不采用 5V供電而是采用低電壓 供電方式 。其中 DSP 下位機(jī) 節(jié)已經(jīng)介紹，在此不再 敘述 。 P C 上 位 機(jī)無(wú) 線(xiàn) 通 信模 塊R S 2 3 2無(wú) 線(xiàn) 通 信模 塊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ú)線(xiàn)模塊與電腦上位機(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è)重要的模塊。 在進(jìn)行的過(guò)程中液晶模塊實(shí)時(shí)顯示運(yùn)動(dòng)方式和參數(shù)及指南針的值，同時(shí)將編碼器的值通過(guò)無(wú)線(xiàn)模塊傳送給 PC 上位機(jī)。 DSP 接收到數(shù)據(jù)進(jìn)行相應(yīng)的處理，分析出要進(jìn)行的運(yùn)動(dòng)形式及各種參數(shù)，同時(shí)利用 I2C 協(xié)議采集指南針的數(shù)據(jù)，將這些信息通過(guò)第 2 章的全向運(yùn)動(dòng)學(xué)模型分析的各種公式轉(zhuǎn)換成各個(gè)電機(jī)的速度值。 其主要的工作方式為： PC 上位機(jī)選擇相應(yīng)的運(yùn)動(dòng)方式，經(jīng)過(guò)RS232 協(xié)議 連接無(wú)線(xiàn)模塊，由無(wú)線(xiàn)模塊發(fā)送數(shù)據(jù)。 為了滿(mǎn)足機(jī)器人大量擴(kuò)展傳感器的需要和減少處理器外圍數(shù)字邏輯器件的使用數(shù)量，該運(yùn)動(dòng)控制卡采用 Altera 公司的復(fù)雜可編程邏輯器件（ CPLD）來(lái)擴(kuò)展 DSP 的 I/O端口和實(shí)現(xiàn)外圍數(shù)字邏輯電路設(shè)計(jì) [8]。 ? CAN 總線(xiàn)控制器，可以為控制器、傳感器、激勵(lì)源以及其它節(jié)點(diǎn)提供良好的通訊，特別適用于工業(yè)現(xiàn)場(chǎng)和汽車(chē)等強(qiáng)噪聲和惡劣的環(huán)境中。 ? 可擴(kuò)展的外部存儲(chǔ)器總共具有 192K16 位的空間，分別為 64K 字 的 程序存儲(chǔ)器空間、 64K 字的數(shù)據(jù)存儲(chǔ)空間和 64K 字的 I/O 空間。 ? 采用高性能靜態(tài) CMOS 技術(shù)，使得供電電壓降為 ，減少了控制器的損耗。 ? 高可靠性、可編程性： TMS320LF2407A 的 16 位定點(diǎn) DSP 內(nèi)核為模擬系統(tǒng)的設(shè)計(jì)者提供了一個(gè)不犧牲系統(tǒng)精度和性能的數(shù)字解決方案。 C2021 系類(lèi)對(duì)于運(yùn)動(dòng)控制有著諸多的優(yōu)點(diǎn)，本設(shè)計(jì)采用 C2021 系類(lèi)TMS320LF2407A 的控制板。此處不再過(guò)多敘述。 這種運(yùn)動(dòng)形式是 平移 與旋轉(zhuǎn)的合成。為了表述直觀(guān)，建立原地旋轉(zhuǎn)運(yùn)動(dòng)模型，其模型圖如圖 8 所示。 123VLVLVL??????? ??? ???? ?? (7) 其中 ?? 為角速度，逆時(shí)針為正。原地旋轉(zhuǎn)是旋轉(zhuǎn)的最基本運(yùn)動(dòng)形式 ，邊 平移 邊 旋轉(zhuǎn) 運(yùn)動(dòng)將在下節(jié) 中闡述 ，本節(jié)主要分析原地旋轉(zhuǎn)運(yùn)動(dòng)形式。其中最大速度為max233 V。 ~360176。通過(guò)式 (6)我們可以反向推導(dǎo)出平移速度 V的速度范圍。 11223313 sin( )c os( ) sin( )622c os( ) c os( )13 sin( )c os( ) sin( )622VVV V VV V V VVVV V V? ???? ???? ?? ? ?? ? ?? ?? ?