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