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