【正文】 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 26 通過指南針顯示能夠很清晰的看到，最終轉(zhuǎn)到的角度已經(jīng)達(dá)到了設(shè)定角度，誤差在? 1176。 在室內(nèi)鋼筋結(jié)構(gòu)的環(huán)境下各個(gè)位置受到的磁干擾不同，經(jīng)測(cè)試最大可影響角度達(dá) 30176。本設(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)。并實(shí)現(xiàn)了通過游戲手柄來遙控系統(tǒng)的各種運(yùn)動(dòng)狀態(tài)。 void Timer1Init()。 //系統(tǒng)初始化 IOinit()。 display(0xff07,0xffe0)。 /* 禁止符號(hào)位擴(kuò)展 */ asm( CLRC CNF )。 /* 清除中斷標(biāo)志 */ IMR=0x000B。 //速度函數(shù) } } /************************************************/ /* 系統(tǒng)初始化 */ /************************************************/ void SystemInit() { asm( SETC INTM )。 /*定時(shí)器 4初始化 */ Timer2Init()。 void Timer2Init()。 ： /******************************************************************************/ /* 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è)速。使得系統(tǒng)能夠經(jīng)過 選擇相關(guān)運(yùn)動(dòng)形式， 修改上位機(jī) 的相關(guān)參數(shù)來實(shí)現(xiàn)運(yùn)動(dòng)系統(tǒng)的全向運(yùn)動(dòng)。 基于 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)角。導(dǎo)致系統(tǒng)不能很好的按照設(shè)定的形式運(yùn)動(dòng)。 圖 30 0176。 圖 28 留痕裝置 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 25 下圖 29 為 系統(tǒng)沿 x 軸負(fù)方向平移的曲線圖，起始位置為（ 110 20），結(jié)束位置為（ 10 ），即走過 1m的距離誤差△ e=，最大 。設(shè)計(jì)制作試驗(yàn)裝置，分別對(duì)各種性能測(cè)試。 *112021582105*： 即 分別控制三電機(jī)， 電機(jī) 1 速度正轉(zhuǎn) 120，電機(jī) 2 正轉(zhuǎn) 58，電機(jī) 3反轉(zhuǎn) 105。 選擇 COM 并打開，選中要運(yùn)行的運(yùn)動(dòng)類型并在相關(guān)位置寫上各種參數(shù)。 發(fā)送 的數(shù)據(jù)不是一個(gè)數(shù)據(jù)，而 DSP 每次只能發(fā)送 8 位數(shù)據(jù)，故需要發(fā)送多次并檢測(cè)是否發(fā)送完畢。這樣能夠通過 TFT 液晶顯示出運(yùn)行方式和參數(shù)，有利于調(diào)試程序和查找錯(cuò)誤。 三種 基本 運(yùn)動(dòng)狀態(tài) 子 程序 當(dāng)接收到的數(shù)據(jù)判斷出要運(yùn)行那種運(yùn)動(dòng)方式后，將進(jìn)入相應(yīng)子程序中 。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 18 通過仿真或者燒寫 FLASH 都可以在線監(jiān)控 DSP 各個(gè)寄存器和變量的值，有利于排除各種錯(cuò)誤。 軟件開發(fā)平臺(tái) 及 仿真器 本設(shè)計(jì)的核心控制芯片為 TMS320LF2407A，其軟件開發(fā)平臺(tái)使用官方工具 CCS ，仿真器選用 。（ ?45?? ）的方向從靜止開始，勻加速運(yùn)動(dòng) 2 秒，達(dá)到 ，然后勻速運(yùn)動(dòng) 2 秒，再在 1 秒之內(nèi)以勻減速將速度減為 0，從啟動(dòng)到停止共運(yùn)行 5s。對(duì) AB 相的采集是電機(jī)閉環(huán)控制的反饋量，關(guān)乎電機(jī) PID 控制是 否能達(dá)到穩(wěn)準(zhǔn)快的要求。 數(shù)字 PID 可分為位置式 PID 和增量式 PID 控制。 N1N2N3N4E1E2Input 15Input 27Input 310Input 412Enable A6Enable B11GND8Vss9Vs4motor 112motor 123motor 2113motor 2214Sense 11Sense 215*1L298NGNDGNDD1 D2D8D7D3 D4D6D5MB1MotorMB2Motor5V 12VGND100uFC2C1C4GNDGND100uFC3GND 圖 12 L298N 電機(jī)驅(qū)動(dòng)電路原理圖 ? 全向輪 作為全向 運(yùn)動(dòng)的驅(qū)動(dòng)部分，驅(qū)動(dòng)輪與普通驅(qū)動(dòng)輪有著重要的區(qū)別。 基于 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)出來。 、響應(yīng)頻率： 30 HZ。 基于 DSP 的全向運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì) 11 圖 10 無線發(fā)送接收模塊 連接圖 ? 指南針模塊 采用 CY— 26 平面數(shù)字指南針模塊圖具有磁偏角補(bǔ)償功能，通過硬磁補(bǔ) 償和設(shè)置地磁夾角能夠表示成全向機(jī)器人的夾角。電機(jī)需要大電流在此選用兩塊 2576 開關(guān)電源進(jìn)行處理。 在進(jìn)行的過程中液晶模塊實(shí)時(shí)顯示運(yùn)動(dòng)方式和參數(shù)及指南針的值，同時(shí)將編碼器的值通過無線模塊傳送給 PC 上位機(jī)。 ? CAN 總線控制器，可以為控制器、傳感器、激勵(lì)源以及其它節(jié)點(diǎn)提供良好的通訊，特別適用于工業(yè)現(xiàn)場(chǎng)和汽車等強(qiáng)噪聲和惡劣的環(huán)境中。 C2021 系類對(duì)于運(yùn)動(dòng)控制有著諸多的優(yōu)點(diǎn)，本設(shè)計(jì)采用 C2021 系類TMS320LF2407A 的控制板。 123VLVLVL??????? ??? ???? ?? (7) 其中 ?? 為角速度，逆時(shí)針為正。通過式 (6)我們可以反向推導(dǎo)出平移速度 V的速度范圍。 三種運(yùn)動(dòng)形式有著不同的運(yùn)動(dòng)特性，以下將分三小節(jié)對(duì)三種運(yùn)動(dòng)形式進(jìn)行單獨(dú)分析。 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 ????? ????。 ? 為 X與 ax 的夾角， ? 為 驅(qū)動(dòng)輪 與 ay 的 夾角， 驅(qū)動(dòng)輪之間的夾角 為 120o 角。通過前幾章設(shè)計(jì)出了全向機(jī)器人，本章主要是通過試驗(yàn)驗(yàn)證各種運(yùn)動(dòng)形式是否達(dá)到要求，并對(duì)結(jié)果進(jìn)行分析。分別分析了全向運(yùn)動(dòng)三種基本運(yùn)動(dòng)形式：平移運(yùn)動(dòng)、原地旋轉(zhuǎn)運(yùn)動(dòng)、平移 +旋轉(zhuǎn)運(yùn)動(dòng)。這些機(jī)器人 很大程度上代替了人大勞動(dòng)，提高了人的生活質(zhì)量。 RoboCup 以機(jī)器人足球作為中心研究課題，通過舉 辦機(jī)器人足球比賽，旨在促進(jìn)人工智能、機(jī)器人技術(shù)及其相關(guān)學(xué)科的發(fā)展。 圖 1 三輪和四輪的結(jié)構(gòu)模型 全向運(yùn)動(dòng)控制系統(tǒng)發(fā)展現(xiàn)狀 在機(jī)器人高速發(fā)展的今天，機(jī)器人的使用越來越廣泛，如用于軍事、勘探、工業(yè)和家居等等。 MATLAB simulation。 南 陽 理 工 學(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ì)。 mathematical modeling。通過控制各個(gè)輪子間相互協(xié)調(diào)運(yùn)動(dòng)達(dá)到全方位的運(yùn)動(dòng)效果，包括全方位平移、全方位邊平移邊旋轉(zhuǎn)和原地旋轉(zhuǎn)運(yùn)動(dòng)形式。1997 年成立于日本 ，足球機(jī)器人作為人工智能的一個(gè)重要組成部分，已是當(dāng)前機(jī)器人研究領(lǐng)域最為活躍的領(lǐng)域之一 [1]。國外很多國家已經(jīng)研制出很多全方位機(jī)器人，應(yīng)用領(lǐng)域不斷擴(kuò)大，如導(dǎo)游機(jī)器人、導(dǎo)購機(jī)器人、電動(dòng)輪椅、平穩(wěn)的測(cè)量裝置、醫(yī)院巡視病房機(jī)器人和倉庫作業(yè)機(jī)器人等。建立全向運(yùn)動(dòng)數(shù)學(xué)模型，對(duì)模型進(jìn)行力的分解合成，計(jì)算各種運(yùn)動(dòng)狀態(tài)下各電機(jī)運(yùn)動(dòng)公式。 第 6 章 ： 試驗(yàn)驗(yàn)證及結(jié)果分析。 要 由圖 5 中(a)圖所示， 小車運(yùn)行在二維平面內(nèi)， 世界 坐標(biāo)系 XY 和機(jī)器人坐標(biāo)系 aa yx? 。 基于 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)。 不同運(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)形式 。 maxV 的值則由電機(jī)性能決定。 原地旋轉(zhuǎn)運(yùn)動(dòng)時(shí) V=0，即 xV 和 yV 都為 0，所以 由式 (2)可 的下式 (7)。 3 基于 DSP 的硬件 系統(tǒng) 簡(jiǎn)介 控制芯片選擇 作為 運(yùn)動(dòng)控制系統(tǒng) 的核心控制芯片，根據(jù)以往在實(shí)驗(yàn)室的經(jīng)驗(yàn) ，其核心控制芯片當(dāng)屬DSP C2021 系類。 ? 10 位 ADC 轉(zhuǎn)換器，其特性為 ： 最小轉(zhuǎn)換時(shí)間為 500ns、 8 個(gè)或 16 個(gè)多路復(fù)用的輸入通道，采集時(shí)間和轉(zhuǎn)換時(shí)間分開，提高了采樣率和輸入阻抗，并且支持自動(dòng)順序采樣，不需 CPU干預(yù)。為使得電機(jī)能夠穩(wěn)、準(zhǔn)、快的執(zhí)行，本設(shè)計(jì)采用經(jīng)典 PID 控制，以增量式編碼器采集的值為反饋。 電池選用 15V的鋰電池，需要將 15V轉(zhuǎn)換為 12V、 5V、 。 ASWLCOM 的鏈接方法如圖 10 所 示。 、重復(fù)精度 ： 1 176。其命令見表 3。 單個(gè)電機(jī)的控制方法 如下： 通過 IO口控制 IN1和 IN2，使能 ENA 為