【正文】 .................................... 20 無線發(fā)送子程序 ........................................... 20 上位機軟件及通訊協(xié)議 .......................................... 21 6 實驗驗證及結(jié)果分析 .................................................. 22 試驗場地 ....................................................... 23 各項性能測試 .................................................. 24 速度 PID 測試 ............................................. 24 平移運動測試 ............................................. 24 原地旋轉(zhuǎn)測試 ............................................. 25 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 III 平移 +旋轉(zhuǎn)運動測試 ........................................ 26 影響因素分析 .................................................. 26 結(jié)束語 ................................................................ 28 參考文獻 .............................................................. 29 附錄 .................................................................. 30 致謝 .................................................................. 42 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 1 1 引言 隨 著 機器人技術(shù)的日新月異，機器人應(yīng)用領(lǐng)域也已從工業(yè)走向普通生活。 作為機器人中的 全向運動機器人 ，有著其特殊的運動形式，即 可以不改變姿勢向任何方向運動，找到最佳位置。全向運動機器人其高靈活度，勢必會在機器 人領(lǐng)域發(fā)揮不可代替的作用。通過控制各個輪子間相互協(xié)調(diào)運動達到全方位的運動效果，包括全方位平移、全方位邊平移邊旋轉(zhuǎn)和原地旋轉(zhuǎn)運動形式。中國要想變成世界強國，也必須重視起機器人技術(shù)。 只有這樣中國的機器人事業(yè)才會涌現(xiàn)大批的人才， 能夠加速機器人技術(shù)的發(fā)展。 全向運動機器人是移動機器人中的一部分，其高靈活性和準確性越來越突出其優(yōu)越性。1997 年成立于日本 ，足球機器人作為人工智能的一個重要組成部分，已是當(dāng)前機器人研究領(lǐng)域最為活躍的領(lǐng)域之一 [1]。 RoboCup 的最終目標是在 2050 年成立一支完全自主的擬人機器人足球隊，能夠與人類進行一場真正意義上的足球賽。要想實現(xiàn)足球機器人靈活自如的“踢足球”，傳統(tǒng)的三輪和四輪結(jié)構(gòu)移動不夠靈活已經(jīng)被淘汰，現(xiàn)在使用的是三輪和四輪結(jié)構(gòu)配備全向輪的全向運動機器人。 對然全向運動機器人有了很多，但是很多都在實驗室，還不能在實際的環(huán)境使用，全向運動 機器人還需要很長的時間發(fā)展。國外很多國家已經(jīng)研制出很多全方位機器人，應(yīng)用領(lǐng)域不斷擴大，如導(dǎo)游機器人、導(dǎo)購機器人、電動輪椅、平穩(wěn)的測量裝置、醫(yī)院巡視病房機器人和倉庫作業(yè)機器人等。 如圖 3 是 三種 全向機器人。其未來的應(yīng)用前景將十分廣闊 [2]。 介紹此課題的現(xiàn)狀和未來發(fā)展前景。建立全向運動數(shù)學(xué)模型，對模型進行力的分解合成，計算各種運動狀態(tài)下各電機運動公式。 第 3 章 ： DSP 及相關(guān)控制系統(tǒng)。 根據(jù)系統(tǒng)運動形式計算好各個電機矢量速度， 如何能讓電機穩(wěn)準快的執(zhí)行設(shè)定的矢量速度是運送形式是否實現(xiàn)的關(guān)鍵。 第 5 章：系統(tǒng)軟件設(shè)計。 第 6 章 ： 試驗驗證及結(jié)果分析。 2 全向運動控制系統(tǒng) 分析 全向運動控制系統(tǒng) 運動學(xué) 模型建立 在 引言 中介紹了全向 運動 控制的 幾種 結(jié)構(gòu)， 本設(shè)計 選用三輪結(jié)構(gòu)構(gòu)建 全向 運動系統(tǒng)[3][4]。 (a) 結(jié)構(gòu)圖 (b) 實物圖 圖 4 全向結(jié)構(gòu)圖和實物圖 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 4 其中三個驅(qū)動輪的旋轉(zhuǎn)軸互相成 120 176。 為實現(xiàn) 全向運動控制，首先對系統(tǒng)進行運動方式建模 [5]， ， 建立運動學(xué)模型。 要 由圖 5 中(a)圖所示， 小車運行在二維平面內(nèi)， 世界 坐標系 XY 和機器人坐標系 aa yx? 。 iL (i=1,2,3)為機器人中心到 驅(qū)動輪 中心的距 離 ,由于 本設(shè)計 使用的 瑞典 輪在旋轉(zhuǎn)的時候?qū)Φ孛娴慕佑|位置 一直在 變化， 會造成驅(qū)動輪與機器人中心的距離也一直在變化， 但變化值比較小 。 iV (i=1,2,3)為 驅(qū)動輪 i 提供沿驅(qū)動方向的速度 ， 規(guī)定 方向 逆時針為正 。 (a) 運動學(xué)建模 (b) 系統(tǒng)整體速度矢量分解 圖 5 運動學(xué)模型 根據(jù) 運動學(xué) 模型 分析出 各個驅(qū)動輪的速度 為下式 (1)： 1 si n( ) c os( )2 c os si n3 si n( ) c os( )XyXyXyV V V LV V V LV V V L? ? ?? ? ?? ? ????? ? ? ? ? ? ? ? ????? ? ???? ? ? ? ? ? ? ? ??? (1) 式 (1)中：由機械結(jié)構(gòu)可知 ? =30 o， ? 取 逆時針為正， ?? 為角速度且逆時針為正。 基于 DSP 的全向運動控制系統(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)。 在實際中，路 徑規(guī)劃 、位姿控制，通過矢量分解、坐標變化，即可轉(zhuǎn)換為對機器人每個軸驅(qū)動電機的運動控制。 (2) 小車的構(gòu)造及安裝位置應(yīng)盡量精確。 (4) 忽略驅(qū)動輪轉(zhuǎn)動過程中 L 的變化，取 L=L1=L2=L3。 不同運動方式 的運動特性 全向運動 的運動 形式大體 可分為 平移 運動 、原地旋轉(zhuǎn) 運動 、 邊平移邊旋轉(zhuǎn) 運動 三種運 動形式 。 平 移 運動 平移 是基本的也是比較常用的運動形式。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 6 (a) 平移模型 (b) 系統(tǒng)整體平移模型 圖 6 簡化后 運動 運動學(xué) 模型 單獨考慮平移運動故將 式 (3)進行簡化 為式 (4)： 131 222 1 03 01322xyL VVV L VVL?????? ???????? ? ???????? ???? ??????? (4) 假設(shè)小車整體平 移矢量速度為 V，與 X 軸成 γ 角。 cos( )sin( )xyVVVV ?????? ??? (5) 由式 (4)和式 (5)我們可以得到三個驅(qū)動輪的速度 ，見式 (6)。 maxV 的值則由電機性能決定。 如圖 7， 其中圓是以驅(qū)動輪最大速度 maxV 大小為半徑的圓，紅色速度 V與 X 軸的角度為 γ ， 隨著 γ 由 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 7 0176。 變化 V的軌跡為紅色 正 六邊形。 圖 7 平移最大速度 模型 原地旋轉(zhuǎn)運動 全向 機器人不但要平移，而且很多時候都需要轉(zhuǎn)向，轉(zhuǎn)向 運動 分為原地旋轉(zhuǎn) 運動 和邊平移邊旋轉(zhuǎn) 運動 。 原地旋轉(zhuǎn)運動時 V=0，即 xV 和 yV 都為 0，所以 由式 (2)可 的下式 (7)。 所以可以得知三個驅(qū)動電機的矢量速度是相同的。 圖 8 原地 旋轉(zhuǎn) 運動模型 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 8 邊平移邊旋轉(zhuǎn)運動 邊 平移 邊旋轉(zhuǎn)運動方式是全向運動中較為復(fù)雜的運動方式，即不做任何準備動作 的前提下往任何方向運動 并且可以 邊運動邊調(diào)整自身姿態(tài)。在前文 中已經(jīng)推導(dǎo)出公式 (1)。 3 基于 DSP 的硬件 系統(tǒng) 簡介 控制芯片選擇 作為 運動控制系統(tǒng) 的核心控制芯片，根據(jù)以往在實驗室的經(jīng)驗 ，其核心控制芯片當(dāng)屬DSP C2021 系類。 TMS320LF2407A 主要 特點 如下 ： ? 高性能、高速度：集成了高性能的 DSP 內(nèi)核和微處理器的片內(nèi)外設(shè)；每秒 4000萬條指令 (40MIPS)的處理速度 。 ? 兩個事件管理器模塊 EVA 和 EVB， 可以 提供完整的、高效的電機控制方案，提供所有的 PWM(8 個 16 位脈沖調(diào)制通道 )和 IO，可以控制所有類型的電機。 ? 片內(nèi)有高達 32K16位的 Flash程序存儲器 ： 高達 16位的數(shù)據(jù) /程序 RAM；544 字節(jié) 的 雙端口 RAM(DARAM)； 2K 字的單口 RAM(SARAM)。 ? 10 位 ADC 轉(zhuǎn)換器，其特性為 ： 最小轉(zhuǎn)換時間為 500ns、 8 個或 16 個多路復(fù)用的輸入通道，采集時間和轉(zhuǎn)換時間分開，提高了采樣率和輸入阻抗，并且支持自動順序采樣，不需 CPU干預(yù)。 ? 5 個外部中斷 (兩個驅(qū)動保護 、復(fù)位和兩個可屏蔽中斷 ) [6][7] 。 基于 DSP 的全向運動控制系統(tǒng)軟件設(shè)計 9 硬件系統(tǒng)結(jié)構(gòu)圖 通過第 2 章關(guān)于運動學(xué)模型的建立，為使得系統(tǒng)能夠按照建立的數(shù)學(xué)模型進行運動， 除了 節(jié)的 DSP 主控制芯片外還 需要 其它的 硬件的支持，本課題設(shè)計的硬件系統(tǒng)結(jié)構(gòu)圖如圖 9 所示。 DSP 下位機的無線模塊接收上位機發(fā)送的數(shù)據(jù)，經(jīng) RS232 通信協(xié)議發(fā)送給 DSP。為使得電機能夠穩(wěn)、準、快的執(zhí)行，本設(shè)計采用經(jīng)典 PID 控制，以增量式編碼器采集的值為反饋。 DSP 下位機將按照程序設(shè)定執(zhí)行 上位機設(shè)定的運動方式， 只有上位機重新發(fā)送新的運動方式才會改變。分為 電源模塊、無線通信 模塊、指南針模塊、液晶顯示模塊， DSP 下位機模塊、電 機 及 驅(qū)動模塊 和全向輪 。 ? 電源模塊 在控制系統(tǒng)中，主要用到系統(tǒng)板電源模塊 、 DSP、外圍器件和 12V 電機供電 。 電池選用 15V的鋰電池，需要將 15V轉(zhuǎn)換為 12V、 5V、 。一塊轉(zhuǎn)換為 12V單獨供電機，另一塊轉(zhuǎn)換 5V供外圍 5V器件和 DSP開發(fā)板。 ? 無線 通信模塊 DSP2407A 處理器內(nèi)嵌的異步串行接口 SCI 是一個標準的通用異步接收 /發(fā)送通訊接口，接收和發(fā)送都是雙緩沖的，有自己的是能和中斷位，支持半雙工和全雙工工作。本設(shè)計是使用 ASWLCOM 無線模塊 。 ASWLCOM 的鏈接方法如圖 10 所 示。指南針模塊使用 232（ 9600bps）協(xié)議及 IIC協(xié)議。測量范圍 ： 0176。 、分辨率： 176。 、重復(fù)精度 ： 1 176。 DSP 2407A 中 SCI 已經(jīng)使用了無線模塊，所以在本很設(shè)計中使用 IIC 通訊協(xié)議進行通信。 表 2 內(nèi)部數(shù)據(jù) 模塊內(nèi)部地址 地址的數(shù)據(jù)含義 0x00 未用到 0x01 角度值高 8 位 0x02 角度值低 8 位 0x03 磁偏角高 8 位 0x04 磁偏角低 8 位 0x05 未用到 0x06 未用到 0x07 校準等級值 實際當(dāng)前角度值為上表“角度值高 8 位”與“角度值低 8 位”合成的 16 位數(shù)據(jù)，數(shù)據(jù)范圍 03599（因為分辨率為 176。） 通過 IIC 控制指南針命令， 磁偏角的值，范圍 03599（因為分辨率為 176。其命令見表 3。 圖 11 液晶顯示 ? 電機及驅(qū)動模塊 電機使用 Faulhaber 帶雙路編碼器減速電機 2230 12V220， 減速后速： 220RPM（轉(zhuǎn)每分鐘） ， 每圈脈沖： 512CPR（脈沖每圈） ，對于編碼器將在下章介紹。它是一個 能夠 承受高電壓， 大 電流的雙 H 橋驅(qū)動。 L298N 功能模塊如表 4 所示 。 單個電機的控制方法 如下： 通過 IO口控制 IN1和 IN2，使能 ENA 為 PWM，則通過控制 IN1 和 IN2 來實現(xiàn)正反轉(zhuǎn)，通過 ENA 的 PWM 占空比調(diào)節(jié) 速度 快慢。在驅(qū)動輪的邊 緣處有