【正文】 /* 初始化看門狗 */ IFR=0xFFFF。 /* B0塊映射為 onchip DARAM*/ asm( CLRC OVM )。 //判斷并讀取 SCI數(shù)據(jù) speedPV()。 LCD_PutString(40,03,李海清 ,0x001f,0x07e0)。 /*定時(shí)器 1初始化 */ Timer4Init()。 //IO口初始化 SCI_Init()。 void Timer4Init()。 void ILI9163_init()。由于程序比較大， 在此只附錄上 和 兩個(gè)文件。 (5) 對(duì)多電機(jī)實(shí)現(xiàn)了數(shù)字增量式 PID 控制， 三個(gè)電機(jī)各自 PID 控制且互不影響，并通過 MATLAB 進(jìn)行了仿真。 (4) 使用了 的 MSComm 控件編寫上位機(jī)軟件，設(shè)計(jì)上位機(jī)界面和功能，編寫通信協(xié)議。 (2) 對(duì)三軸運(yùn)動(dòng)控制結(jié)構(gòu)進(jìn)行了數(shù)學(xué)建模，進(jìn)行了運(yùn)動(dòng)學(xué)分析。同時(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ā)生誤差。角，嚴(yán)重影響了系統(tǒng)性能。 影響因素 分析 影響系統(tǒng)性能的因素大部分來源于外部，大致分為以下幾種 情況： （ 1） 電池電量不足，導(dǎo)致系統(tǒng)電機(jī)啟動(dòng)或大的變化時(shí)電流不足。而且此誤差是為了避免 轉(zhuǎn)到角度出現(xiàn)來回校準(zhǔn)而設(shè)置的死區(qū)，故原地旋轉(zhuǎn)運(yùn)動(dòng)能夠準(zhǔn)確的執(zhí)行。時(shí)的情況。驗(yàn)證指南針轉(zhuǎn)角是否正確比較簡(jiǎn)單，直接觀看 TFT 液晶顯示的指南針角度就可以知道準(zhǔn)確性。在驗(yàn)證平移性能的時(shí)，采用在系統(tǒng)上安裝自制鉛筆 留痕 裝置 （如圖 28） ，能夠跟隨系統(tǒng)運(yùn)動(dòng)時(shí)留下系統(tǒng)運(yùn)動(dòng)的軌跡，依此來采集系統(tǒng)平移是的各項(xiàng)性能。在這個(gè)試驗(yàn)中測(cè)試占空比為 200（占空比 2/5）的上升曲線。 (a) 場(chǎng)地 規(guī)劃 (b) 實(shí)際場(chǎng)地 圖 26 試驗(yàn)場(chǎng)地 基于 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)。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 23 試驗(yàn)場(chǎng)地 由于本設(shè)計(jì)使用的驅(qū) 動(dòng)輪為 尼龍 材質(zhì)。 例如讓系統(tǒng)往 [0 50 45]T運(yùn)動(dòng)，即系統(tǒng)往正 Y 軸以 50 的速度前進(jìn)且旋轉(zhuǎn)，那么發(fā)送的協(xié)議 為： *05010450100*。 協(xié)議格式 見表 5 所示。最后加上一個(gè)“演示”模塊，即設(shè)定幾個(gè)特殊方向角利用圖片直觀表示出來，能夠?qū)崿F(xiàn)平移和邊平移邊旋轉(zhuǎn)的運(yùn)動(dòng)，能夠?qū)φ麄€(gè)設(shè)計(jì)系統(tǒng)有個(gè)更直觀的操作。 上位機(jī)軟件界面如圖 24 所示。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 21 將 三 組 數(shù) 值進(jìn) 行 相 應(yīng) 轉(zhuǎn) 換是 否 發(fā) 送 完 畢寫 入 S C I發(fā) 送 寄 存 器返 回是否 圖 23 無線發(fā)送子程序 為使得上位機(jī) 更 好的監(jiān)控系統(tǒng)，同時(shí)有利于 PID 調(diào)試，故將得到的編碼值經(jīng)無線傳輸模塊發(fā)送給上位機(jī)軟件，并在相應(yīng)位置顯示出來。故將電機(jī)控制部分單獨(dú)設(shè)為子程序進(jìn) 行敘述。 顯示的其他參數(shù)則實(shí)時(shí)變化。由于三種運(yùn)動(dòng)狀態(tài)子程序其流程大致相同，不同之處在于對(duì)接收到的數(shù)據(jù)的處理和相應(yīng)的計(jì)算以及相關(guān)顯 示部分。通過軟件的反復(fù)運(yùn)動(dòng)則完成了整個(gè)系統(tǒng)的運(yùn)動(dò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 所示。而燒寫 FLASH 則比較復(fù)雜 燒寫 時(shí)間比較長，不利于開發(fā)軟件階段使用 [13]。圖 19 即為CCStudio 開發(fā)環(huán)境。最后一節(jié)介紹上位機(jī)部分，編寫上位機(jī)軟件部分和上位機(jī)下位機(jī)使用的通訊協(xié)議。綠色曲線為期望曲線，藍(lán)色曲線基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 16 為 PID 控制跟蹤曲線，從曲線中可以看出， PID 控制可以滿足系統(tǒng)的控制要求。 針對(duì) 三軸運(yùn)動(dòng)控制系統(tǒng) 設(shè)計(jì)以下速度合成實(shí)驗(yàn)，假設(shè)機(jī)器人坐標(biāo)系和場(chǎng)地坐標(biāo)系具有同樣的方位，要求機(jī)器人（作為整體）沿 45176。針對(duì)電機(jī)模型，這里設(shè)定電機(jī)的模型為2 3() 2 3 4Gs ss? ??，針對(duì)此電機(jī)模型進(jìn)行 MATLAB 仿真，觀察 PID 控制算法的控制效果 [10][11]。 A 相 B 相相差 90度，通過軟件能夠很好的得到此時(shí)電機(jī)的速度和方向。 本設(shè)計(jì)采用增量式 PID，其閉環(huán)系統(tǒng)見圖 13所示。 所以本設(shè)計(jì) 采用 PID 分別對(duì)三個(gè)電機(jī)進(jìn)行閉環(huán)控制。電機(jī) 的 控制對(duì)整個(gè) 系統(tǒng)是至關(guān)重要的。 單個(gè)電機(jī)的控制方法 如下： 通過 IO口控制 IN1和 IN2，使能 ENA 為 PWM，則通過控制 IN1 和 IN2 來實(shí)現(xiàn)正反轉(zhuǎn)，通過 ENA 的 PWM 占空比調(diào)節(jié) 速度 快慢。它是一個(gè) 能夠 承受高電壓， 大 電流的雙 H 橋驅(qū)動(dòng)。其命令見表 3。 表 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。 、重復(fù)精度 ： 1 176。測(cè)量范圍 ： 0176。 ASWLCOM 的鏈接方法如圖 10 所 示。 ? 無線 通信模塊 DSP2407A 處理器內(nèi)嵌的異步串行接口 SCI 是一個(gè)標(biāo)準(zhǔn)的通用異步接收 /發(fā)送通訊接口，接收和發(fā)送都是雙緩沖的，有自己的是能和中斷位，支持半雙工和全雙工工作。 電池選用 15V的鋰電池，需要將 15V轉(zhuǎn)換為 12V、 5V、 。分為 電源模塊、無線通信 模塊、指南針模塊、液晶顯示模塊， DSP 下位機(jī)模塊、電 機(jī) 及 驅(qū)動(dòng)模塊 和全向輪 。為使得電機(jī)能夠穩(wěn)、準(zhǔn)、快的執(zhí)行，本設(shè)計(jì)采用經(jīng)典 PID 控制，以增量式編碼器采集的值為反饋。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 9 硬件系統(tǒng)結(jié)構(gòu)圖 通過第 2 章關(guān)于運(yùn)動(dòng)學(xué)模型的建立，為使得系統(tǒng)能夠按照建立的數(shù)學(xué)模型進(jìn)行運(yùn)動(dòng)， 除了 節(jié)的 DSP 主控制芯片外還 需要 其它的 硬件的支持，本課題設(shè)計(jì)的硬件系統(tǒng)結(jié)構(gòu)圖如圖 9 所示。 ? 10 位 ADC 轉(zhuǎn)換器，其特性為 ： 最小轉(zhuǎn)換時(shí)間為 500ns、 8 個(gè)或 16 個(gè)多路復(fù)用的輸入通道，采集時(shí)間和轉(zhuǎn)換時(shí)間分開，提高了采樣率和輸入阻抗，并且支持自動(dòng)順序采樣，不需 CPU干預(yù)。 ? 兩個(gè)事件管理器模塊 EVA 和 EVB， 可以 提供完整的、高效的電機(jī)控制方案，提供所有的 PWM(8 個(gè) 16 位脈沖調(diào)制通道 )和 IO，可以控制所有類型的電機(jī)。 3 基于 DSP 的硬件 系統(tǒng) 簡(jiǎn)介 控制芯片選擇 作為 運(yùn)動(dòng)控制系統(tǒng) 的核心控制芯片，根據(jù)以往在實(shí)驗(yàn)室的經(jīng)驗(yàn) ，其核心控制芯片當(dāng)屬DSP C2021 系類。 圖 8 原地 旋轉(zhuǎn) 運(yùn)動(dòng)模型 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 8 邊平移邊旋轉(zhuǎn)運(yùn)動(dòng) 邊 平移 邊旋轉(zhuǎn)運(yùn)動(dòng)方式是全向運(yùn)動(dòng)中較為復(fù)雜的運(yùn)動(dòng)方式，即不做任何準(zhǔn)備動(dòng)作 的前提下往任何方向運(yùn)動(dòng) 并且可以 邊運(yùn)動(dòng)邊調(diào)整自身姿態(tài)。 原地旋轉(zhuǎn)運(yùn)動(dòng)時(shí) V=0，即 xV 和 yV 都為 0，所以 由式 (2)可 的下式 (7)。 變化 V的軌跡為紅色 正 六邊形。 maxV 的值則由電機(jī)性能決定。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 6 (a) 平移模型 (b) 系統(tǒng)整體平移模型 圖 6 簡(jiǎn)化后 運(yùn)動(dòng) 運(yùn)動(dòng)學(xué) 模型 單獨(dú)考慮平移運(yùn)動(dòng)故將 式 (3)進(jìn)行簡(jiǎn)化 為式 (4)： 131 222 1 03 01322xyL VVV L VVL?????? ???????? ? ???????? ???? ??????? (4) 假設(shè)小車整體平 移矢量速度為 V，與 X 軸成 γ 角。 不同運(yùn)動(dòng)方式 的運(yùn)動(dòng)特性 全向運(yùn)動(dòng) 的運(yùn)動(dòng) 形式大體 可分為 平移 運(yùn)動(dòng) 、原地旋轉(zhuǎn) 運(yùn)動(dòng) 、 邊平移邊旋轉(zhuǎn) 運(yùn)動(dòng) 三種運(yùn) 動(dòng)形式 。 (2) 小車的構(gòu)造及安裝位置應(yīng)盡量精確。 基于 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)。 iV (i=1,2,3)為 驅(qū)動(dòng)輪 i 提供沿驅(qū)動(dòng)方向的速度 ， 規(guī)定 方向 逆時(shí)針為正 。 要 由圖 5 中(a)圖所示， 小車運(yùn)行在二維平面內(nèi)， 世界 坐標(biāo)系 XY 和機(jī)器人坐標(biāo)系 aa yx? 。 (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。 第 6 章 ： 試驗(yàn)驗(yàn)證及結(jié)果分析。 根據(jù)系統(tǒng)運(yùn)動(dòng)形式計(jì)算好各個(gè)電機(jī)矢量速度， 如何能讓電機(jī)穩(wěn)準(zhǔn)快的執(zhí)行設(shè)定的矢量速度是運(yùn)送形式是否實(shí)現(xiàn)的關(guān)鍵。建立全向運(yùn)動(dòng)數(shù)學(xué)模型，對(duì)模型進(jìn)行力的分解合成，計(jì)算各種運(yùn)動(dòng)狀態(tài)下各電機(jī)運(yùn)動(dòng)公式。其未來的應(yīng)用前景將十分廣闊 [2]。國外很多國家已經(jīng)研制出很多全方位機(jī)器人，應(yīng)用領(lǐng)域不斷擴(kuò)大，如導(dǎo)游機(jī)器人、導(dǎo)購機(jī)器人、電動(dòng)輪椅、平穩(wěn)的測(cè)量裝置、醫(yī)院巡視病房機(jī)器人和倉庫作業(yè)機(jī)器人等。要想實(shí)現(xiàn)足球機(jī)器人靈活自如的“踢足球”，傳統(tǒng)的三輪和四輪結(jié)構(gòu)移動(dòng)不夠靈活已經(jīng)被淘汰，現(xiàn)在使用的是三輪和四輪結(jié)構(gòu)配備全向輪的全向運(yùn)動(dòng)機(jī)器人。1997 年成立于日本 ，足球機(jī)器人作為人工智能的一個(gè)重要組成部分，已是當(dāng)前機(jī)器人研究領(lǐng)域最為活躍的領(lǐng)域之一 [1]。 只有這樣中國的機(jī)器人事業(yè)才會(huì)涌現(xiàn)大批的人才， 能夠加速機(jī)器人技術(shù)的發(fā)展。通過控制各個(gè)輪子間相互協(xié)調(diào)運(yùn)動(dòng)達(dá)到全方位的運(yùn)動(dòng)效果，包括全方位平移、全方位邊平移邊旋轉(zhuǎn)和原地旋轉(zhuǎn)運(yùn)動(dòng)形式。 作為機(jī)器人中的 全向運(yùn)動(dòng)機(jī)器人 ，有著其特殊的運(yùn)動(dòng)形式，即 可以不改變姿勢(shì)向任何方向運(yùn)動(dòng)，找到最佳位置。 mathematical modeling。 [關(guān)鍵詞 ] DSP；全向 運(yùn)動(dòng)控制 ； 數(shù)學(xué)建模 ； MATLAB 仿真 ； 串行通信 。 南 陽 理 工 學(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è)：