【正文】 PID 增量式控制方式及 MALTAB 仿真來實(shí)現(xiàn)?？梢圆蛔鋈魏螠?zhǔn)備的情況下朝球移動，并且可以邊移動邊調(diào)整踢 球姿態(tài)已達(dá)到最近效果。 同時 也可以邊往某個方向運(yùn)動，邊調(diào)整自身的姿勢達(dá)到最佳運(yùn)動效果 ， 可以在平面內(nèi)完美的往任何方向做三自由度運(yùn)動。 [關(guān)鍵詞 ] DSP；全向 運(yùn)動控制 ； 數(shù)學(xué)建模 ； MATLAB 仿真 ； 串行通信 。 只有這樣中國的機(jī)器人事業(yè)才會涌現(xiàn)大批的人才， 能夠加速機(jī)器人技術(shù)的發(fā)展。其未來的應(yīng)用前景將十分廣闊 [2]。 (a) 結(jié)構(gòu)圖 (b) 實(shí)物圖 圖 4 全向結(jié)構(gòu)圖和實(shí)物圖 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 4 其中三個驅(qū)動輪的旋轉(zhuǎn)軸互相成 120 176。 (2) 小車的構(gòu)造及安裝位置應(yīng)盡量精確。 變化 V的軌跡為紅色 正 六邊形。 ? 兩個事件管理器模塊 EVA 和 EVB， 可以 提供完整的、高效的電機(jī)控制方案，提供所有的 PWM(8 個 16 位脈沖調(diào)制通道 )和 IO，可以控制所有類型的電機(jī)。分為 電源模塊、無線通信 模塊、指南針模塊、液晶顯示模塊， DSP 下位機(jī)模塊、電 機(jī) 及 驅(qū)動模塊 和全向輪 。測量范圍 ： 0176。它是一個 能夠 承受高電壓， 大 電流的雙 H 橋驅(qū)動。 本設(shè)計采用增量式 PID，其閉環(huán)系統(tǒng)見圖 13所示。綠色曲線為期望曲線，藍(lán)色曲線基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 16 為 PID 控制跟蹤曲線，從曲線中可以看出， PID 控制可以滿足系統(tǒng)的控制要求。 主程序 開 始系 統(tǒng) 初 始 化接 收 到停 止 信 號處 理 接 收 到 的 數(shù) 據(jù)判 斷 運(yùn) 動 方 式平 移 + 旋 轉(zhuǎn) 子 程 序電 機(jī) 控 制 子 程 序停 止 電 機(jī)結(jié) 束原 地 旋 轉(zhuǎn) 子 程 序平 移 子 程 序是否原 地 旋 轉(zhuǎn)平 移 + 旋 轉(zhuǎn)平 移 運(yùn) 動 圖 20 主程序 流程圖 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 19 根據(jù)前幾章的設(shè)計和分析，設(shè)計出軟件系統(tǒng)流程圖，如上圖 20 所示。故將電機(jī)控制部分單獨(dú)設(shè)為子程序進(jìn) 行敘述。 協(xié)議格式 見表 5 所示。在這個試驗(yàn)中測試占空比為 200（占空比 2/5）的上升曲線。而且此誤差是為了避免 轉(zhuǎn)到角度出現(xiàn)來回校準(zhǔn)而設(shè)置的死區(qū)，故原地旋轉(zhuǎn)運(yùn)動能夠準(zhǔn)確的執(zhí)行。 (2) 對三軸運(yùn)動控制結(jié)構(gòu)進(jìn)行了數(shù)學(xué)建模，進(jìn)行了運(yùn)動學(xué)分析。 void ILI9163_init()。 LCD_PutString(40,03,李海清 ,0x001f,0x07e0)。 /* 初始化看門狗 */ IFR=0xFFFF。 /*定時器 1初始化 */ Timer4Init()。由于程序比較大， 在此只附錄上 和 兩個文件。同時由于驅(qū)動輪滾動會造成電機(jī)力矩發(fā)生變化，L=12 或 ，及內(nèi)外兩個側(cè)滑輪著地時會造成力矩 的變化，會使得系統(tǒng)轉(zhuǎn)動發(fā)生誤差。時的情況。 (a) 場地 規(guī)劃 (b) 實(shí)際場地 圖 26 試驗(yàn)場地 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 24 各項(xiàng)性能測試 需要試驗(yàn)驗(yàn)證 測試 的 有： PID 驗(yàn)證、平移驗(yàn)證、原地旋轉(zhuǎn)驗(yàn)證、邊平移邊旋轉(zhuǎn)運(yùn)動。最后加上一個“演示”模塊，即設(shè)定幾個特殊方向角利用圖片直觀表示出來，能夠?qū)崿F(xiàn)平移和邊平移邊旋轉(zhuǎn)的運(yùn)動，能夠?qū)φ麄€設(shè)計系統(tǒng)有個更直觀的操作。 顯示的其他參數(shù)則實(shí)時變化。而燒寫 FLASH 則比較復(fù)雜 燒寫 時間比較長，不利于開發(fā)軟件階段使用 [13]。 針對 三軸運(yùn)動控制系統(tǒng) 設(shè)計以下速度合成實(shí)驗(yàn)，假設(shè)機(jī)器人坐標(biāo)系和場地坐標(biāo)系具有同樣的方位，要求機(jī)器人（作為整體）沿 45176。 所以本設(shè)計 采用 PID 分別對三個電機(jī)進(jìn)行閉環(huán)控制。其命令見表 3。 ASWLCOM 的鏈接方法如圖 10 所 示。為使得電機(jī)能夠穩(wěn)、準(zhǔn)、快的執(zhí)行，本設(shè)計采用經(jīng)典 PID 控制，以增量式編碼器采集的值為反饋。 3 基于 DSP 的硬件 系統(tǒng) 簡介 控制芯片選擇 作為 運(yùn)動控制系統(tǒng) 的核心控制芯片，根據(jù)以往在實(shí)驗(yàn)室的經(jīng)驗(yàn) ，其核心控制芯片當(dāng)屬DSP C2021 系類。 maxV 的值則由電機(jī)性能決定。 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 5 1 s i n ( ) c o s ( )662 c o s s i n3 s i n ( ) c o s ( )66XyXyXyV V V LV V V LV V V L??? ? ?? ? ???? ? ????? ? ? ? ? ? ???? ? ? ???? ? ? ? ? ? ??? (2) 由式 (2)寫出矩陣形式 ，即式 (3)。 第 6 章 ： 試驗(yàn)驗(yàn)證及結(jié)果分析。國外很多國家已經(jīng)研制出很多全方位機(jī)器人，應(yīng)用領(lǐng)域不斷擴(kuò)大，如導(dǎo)游機(jī)器人、導(dǎo)購機(jī)器人、電動輪椅、平穩(wěn)的測量裝置、醫(yī)院巡視病房機(jī)器人和倉庫作業(yè)機(jī)器人等。通過控制各個輪子間相互協(xié)調(diào)運(yùn)動達(dá)到全方位的運(yùn)動效果，包括全方位平移、全方位邊平移邊旋轉(zhuǎn)和原地旋轉(zhuǎn)運(yùn)動形式。 南 陽 理 工 學(xué) 院 本 科 畢 業(yè) 設(shè) 計（論文） 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 Software Design of Omnidirectional Motion Control System Based on DSP 學(xué) 院（系）： 電子與電氣工程系 專 業(yè)： 自動化 學(xué) 生 姓 名： 學(xué) 號： 96107031 指 導(dǎo) 教 師（職稱）： 評 閱 教 師： 完 成 日 期： 南陽理工學(xué)院 Nanyang Institute of Technology基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 I 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 [摘 要 ] 本文 基于 DSP C2021系列 TMS320LF2407A核心控制芯片，以 CCStudio 軟件為開發(fā)平臺， 在了解 RoboCup 中型組足球機(jī)器人和其他全向機(jī)器人的基礎(chǔ)上， 主要完成了全向運(yùn)動控制系統(tǒng)的軟件設(shè)計。 圖 1 三輪和四輪的結(jié)構(gòu)模型 全向運(yùn)動控制系統(tǒng)發(fā)展現(xiàn)狀 在機(jī)器人高速發(fā)展的今天，機(jī)器人的使用越來越廣泛，如用于軍事、勘探、工業(yè)和家居等等。這些機(jī)器人 很大程度上代替了人大勞動，提高了人的生活質(zhì)量。通過前幾章設(shè)計出了全向機(jī)器人，本章主要是通過試驗(yàn)驗(yàn)證各種運(yùn)動形式是否達(dá)到要求，并對結(jié)果進(jìn)行分析。 si n( ) c os( )1 662 c os si n3 si n( ) c os( )66xyL VVV L VV L?????????? ???? ??? ? ? ????? ???? ????? ???????? ? ? ? ???? (3) 所以在任意時刻，小車的運(yùn)動形態(tài)就是 TxyV V V ????? ????。通過式 (6)我們可以反向推導(dǎo)出平移速度 V的速度范圍。 C2021 系類對于運(yùn)動控制有著諸多的優(yōu)點(diǎn)，本設(shè)計采用 C2021 系類TMS320LF2407A 的控制板。 在進(jìn)行的過程中液晶模塊實(shí)時顯示運(yùn)動方式和參數(shù)及指南針的值，同時將編碼器的值通過無線模塊傳送給 PC 上位機(jī)。 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 11 圖 10 無線發(fā)送接收模塊 連接圖 ? 指南針模塊 采用 CY— 26 平面數(shù)字指南針模塊圖具有磁偏角補(bǔ)償功能，通過硬磁補(bǔ) 償和設(shè)置地磁夾角能夠表示成全向機(jī)器人的夾角。 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 12 表 3 模塊命令表 命令值 作用 0x00+0x31 角度測量 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)動過程中各種狀態(tài)和參數(shù) 如圖 11， 通過顯示模塊能夠很好的顯示運(yùn)動狀態(tài)，同時系統(tǒng)出現(xiàn)問題也能通 過顯示反應(yīng)出來。 數(shù)字 PID 可分為位置式 PID 和增量式 PID 控制。（ ?45?? ）的方向從靜止開始，勻加速運(yùn)動 2 秒，達(dá)到 ，然后勻速運(yùn)動 2 秒，再在 1 秒之內(nèi)以勻減速將速度減為 0，從啟動到停止共運(yùn)行 5s。 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 18 通過仿真或者燒寫 FLASH 都可以在線監(jiān)控 DSP 各個寄存器和變量的值，有利于排除各種錯誤。這樣能夠通過 TFT 液晶顯示出運(yùn)行方式和參數(shù)，有利于調(diào)試程序和查找錯誤。 選擇 COM 并打開，選中要運(yùn)行的運(yùn)動類型并在相關(guān)位置寫上各種參數(shù)。設(shè)計制作試驗(yàn)裝置，分別對各種性能測試。 圖 30 0176。 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 28 結(jié)束語 全向 運(yùn)動控系統(tǒng)在全向機(jī)器人上的應(yīng)用 可實(shí)現(xiàn)機(jī)器人向任意方向做直線運(yùn)動而不需要事先作旋轉(zhuǎn)運(yùn)動，并且在以直線運(yùn)動到達(dá)目標(biāo)點(diǎn)的過程中同時可以做自身旋轉(zhuǎn)運(yùn)動，以調(diào)整機(jī)器人的姿態(tài)，從而達(dá)到終態(tài)所需的姿態(tài)角。 ： /******************************************************************************/ /* File Name : */ /* Autor : Li Haiqing */ /* Date : 04/01/2021 */ /* Description : This file provides main function, provide initialization */ /* 串口發(fā)送與中斷接收程序 ,波特率設(shè)置為 9600 */ /* IO口占用：： PE1PE6（ PWM） PA3PA6和 PB0PB7和 PE7液晶屏使用 */ /* PF0=CAP5,PF1=cap6,PA3=cap1三電機(jī)測速。 /*定時器 4初始化 */ Timer2Init()。 /* 清除中斷標(biāo)志 */ IMR=0x000B。 display(0xff07,0xffe0)。 void Timer1Init()。本設(shè)計完成的主要工作為以下幾點(diǎn)： (1) 對目前的幾種運(yùn)動結(jié)構(gòu)進(jìn)行了研究分析， 設(shè)計了互成 120 度的三軸運(yùn)動結(jié)構(gòu)。轉(zhuǎn)角示意圖 基于 DSP 的全向運(yùn)動控制系統(tǒng)軟件設(shè)計 26 通過指南針顯示能夠很清晰的看到，最終轉(zhuǎn)到的角度已經(jīng)達(dá)到了設(shè)定角度，誤差在? 1176。在系統(tǒng)中每隔 3 ms 記錄一次 3ms 內(nèi)記錄數(shù)。 本設(shè)計系統(tǒng)通訊協(xié)議為簡單通訊協(xié)議 [15]。電機(jī)的穩(wěn)、準(zhǔn)、快的運(yùn)行將會使運(yùn)動方式更接近理論目標(biāo)。其全向運(yùn)動方式經(jīng)過上位機(jī)無線傳輸指令，然后 DSP 接收指令進(jìn)行處理最后控制電機(jī)協(xié)調(diào)運(yùn)動，根據(jù)指南針和編碼器進(jìn)行閉環(huán)控制， 以 達(dá)到最佳效果。 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ū)動電機(jī)實(shí)驗(yàn) 圖 16 三軸運(yùn)動控制系統(tǒng)的速度合成實(shí)驗(yàn) 接著將此運(yùn)動通過運(yùn)運(yùn)學(xué)分解，將速度分解至三個輪子上， 并分別對每個輪子的驅(qū)動軸單獨(dú)控制，三個輪子相應(yīng)跟蹤曲線如圖 17 所示。 而增量式PID 控制的優(yōu)點(diǎn)是只輸出增量，計算誤差動作時造成的影響小，同時對控制算法不需要基于 DSP 的全向運(yùn)動控制