基于DSP與FPGA的機(jī)器人運(yùn)動控制系統(tǒng)設(shè)計(jì)

李 疆，游有鵬

（南京航空航天大學(xué)機(jī)電學(xué)院，江蘇 南京210016）

0 引言

隨著電子技術(shù)和計(jì)算機(jī)技術(shù)的發(fā)展，機(jī)器人已經(jīng)在眾多領(lǐng)域得到了日益廣泛的應(yīng)用。作為機(jī)器人的核心部分，機(jī)器人控制器是影響機(jī)器人性能的關(guān)鍵部分之一。傳統(tǒng)的機(jī)器人控制系統(tǒng)基本采用專用計(jì)算機(jī)和專用機(jī)器人語言、專用微處理器的封閉式控制系統(tǒng)結(jié)構(gòu)。這種結(jié)構(gòu)限制了機(jī)器人系統(tǒng)的擴(kuò)展性和靈活性，不利于進(jìn)一步的擴(kuò)展和開發(fā)。而通用、可靠、開放和實(shí)時動態(tài)的開放式控制系統(tǒng)，已是機(jī)器人控制系統(tǒng)發(fā)展的必然趨勢。

利用DSP和FPGA設(shè)計(jì)了開放式架構(gòu)的運(yùn)動控制系統(tǒng)。以PC機(jī)為依托，利用它的強(qiáng)大處理能力，完成一些非實(shí)時任務(wù)，并通過PCI總線與下位機(jī)核心處理器DSP進(jìn)行數(shù)據(jù)交互。下位機(jī)運(yùn)動控制卡對機(jī)械臂進(jìn)行精插補(bǔ)規(guī)劃和實(shí)時控制等任務(wù)，并在其中嵌入了智能控制算法，同時克服了封閉式系統(tǒng)可擴(kuò)展性差的缺陷［1］。

1 硬件平臺設(shè)計(jì)

1．1 主控單元

作為下位機(jī)運(yùn)動控制卡，承擔(dān)了工業(yè)機(jī)器人實(shí)時控制的任務(wù)，主要工作是進(jìn)行速度規(guī)劃、位置控制和上位機(jī)的通訊以及任務(wù)調(diào)度等。因此，為滿足工業(yè)機(jī)器人運(yùn)動控制系統(tǒng)的要求，選取TI公司DSP，型號為TMS320F28335。同時，為實(shí)現(xiàn)多路PWM輸出和光電編碼器的反饋，以完成高精度機(jī)械臂的位置控制，選取Altera公司cycloneⅡ型號的FPGA作為輔助CPU，處理與上位機(jī)的通訊、PWM輸出、編碼器反饋和I／O量控制等功能。機(jī)器人控制系統(tǒng)硬件由工控機(jī)和基于DSP的運(yùn)動控制器組成，其硬件架構(gòu)如圖1所示。

圖1 硬件架構(gòu)

1．2 上下位機(jī)通信接口

基于整個系統(tǒng)通信的要求，必須綜合考慮速度、開發(fā)復(fù)雜度和穩(wěn)定性等因素，系統(tǒng)選擇了PCI方式進(jìn)行高速數(shù)據(jù)交換。它是一種高性能的32／64位數(shù)據(jù)／地址復(fù)用總線，通信速度最快可達(dá)133 MB／s，滿足控制系統(tǒng)的速率要求。系統(tǒng)采用FPGA作為PCI總線目標(biāo)設(shè)備接口控制器，實(shí)現(xiàn)了下位控制器與客戶端之間的高速實(shí)時通信，采用中斷防沖突方式，把FPGA中集成的雙端口RAM作為上下交互數(shù)據(jù)區(qū)域，增強(qiáng)了系統(tǒng)的集成性。

1．3 電機(jī)驅(qū)動接口

面向機(jī)器人的高精度位置控制系統(tǒng)設(shè)計(jì)，采用交流伺服驅(qū)動器輸入模擬量的方式完成電機(jī)的閉環(huán)控制。考慮到硬件成本、多通道控制和電壓輸出精度，系統(tǒng)采用PWM＋濾波的方式實(shí)現(xiàn)模擬電壓的輸出。借助FPGA時鐘計(jì)數(shù)器和比較器，輸出對應(yīng)電壓占空比的差分信號，經(jīng)過運(yùn)算放大電路后即可得到－10～＋10 V的電壓。

1．4 編碼器反饋信號

編碼器具有三相六路信號XA＋／XA－、XB＋／XB－、XZ＋／XZ－。首先，經(jīng)過濾波電路消除雜波，然后，輸入差分接收芯片26LS32。輸出的信號直接連接到74HC14整形器，它可以將緩慢變化的輸入信號轉(zhuǎn)換成清晰無抖動的輸出信號，Z相零位信號則用于回零的檢測。編碼器反饋接收原理如圖2所示。

圖2 編碼器反饋原理

1．5 開關(guān)量接口

FPGA的開關(guān)量控制借助了74HC595芯片，它通過移位操作接收／發(fā)送8位串行數(shù)據(jù)至8個引腳并行輸出。考慮到電平匹配和保護(hù)電路，信號由FPGA輸出后，也要經(jīng)過光耦隔離和電平轉(zhuǎn)換，再經(jīng)由BTS716輸出，以保障其輸出具有足夠的驅(qū)動能力。對于開關(guān)量輸入信號，為增強(qiáng)它的抗干擾能力，I／O接口部分需要使用光耦隔離，并且信號輸入FPGA后仍要進(jìn)行濾波，才能供運(yùn)動控制卡使用。開關(guān)量信號輸入／輸出原理如圖3所示。

圖3 開關(guān)量接口原理

2 控制系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件主要劃分為3個層次，包括上位機(jī)軟件、PCI驅(qū)動程序以及運(yùn)動控制DSP軟件。上位機(jī)軟件作為人機(jī)交互部分，主要實(shí)現(xiàn)狀態(tài)顯示、軌跡規(guī)劃、軌跡優(yōu)化、示教再現(xiàn)和參數(shù)管理等功能。PCI驅(qū)動程序主要完成上位機(jī)軟件與運(yùn)動控制卡之間的數(shù)據(jù)交互，包括命令的發(fā)送接收。運(yùn)動控制DSP軟件負(fù)責(zé)機(jī)器人控制系統(tǒng)的實(shí)時控制部分，主要實(shí)現(xiàn)速度規(guī)劃、位置控制、I／O控制和參數(shù)設(shè)置等功能［2］。控制系統(tǒng)總體架構(gòu)如圖4所示。

圖4 系統(tǒng)軟件架構(gòu)

上位機(jī)采用Visual C＋＋6．0開發(fā)。狀態(tài)顯示用來監(jiān)測機(jī)器人控制系統(tǒng)的運(yùn)行狀態(tài)，并根據(jù)監(jiān)測結(jié)果進(jìn)行跟蹤處理。軌跡規(guī)劃主要完成理論曲線的速度前瞻控制，確保了運(yùn)動的平穩(wěn)性；軌跡優(yōu)化則是研究時間最小的軌跡規(guī)劃算法，使機(jī)器人的作業(yè)效率達(dá)到最優(yōu)。示教再現(xiàn)指機(jī)器人通過記錄機(jī)器人被操作路徑上的點(diǎn)坐標(biāo)，實(shí)現(xiàn)軌跡復(fù)現(xiàn)的過程。參數(shù)管理用以設(shè)置各個關(guān)節(jié)的運(yùn)動參數(shù)，包括速度、加速度和加加速度的限制等。

下位機(jī)DSP控制器借助TI公司官方軟件CCS3．3進(jìn)行開發(fā)。通訊模塊包含程序解釋和狀態(tài)反饋。程序解釋指DSP通過FPGA的雙端口RAM下載上位機(jī)發(fā)送的指令，并按照相關(guān)協(xié)議譯碼，得到控制指令信息，狀態(tài)反饋則是把下位控制器的相關(guān)信息發(fā)送給PC端，供用戶監(jiān)測。速度規(guī)劃部分的作用是避免機(jī)器人的沖擊、振動，并在盡量減少系統(tǒng)運(yùn)算負(fù)荷的情況下，使得插補(bǔ)曲線平滑。位置控制任務(wù)完成了機(jī)器人平穩(wěn)、高精度的控制，尤其在連續(xù)路徑控制時，保證實(shí)際軌跡和目標(biāo)曲線重合。

下位機(jī)的控制程序流程如圖5所示。為了促使軟件模塊化，提高開發(fā)效率，軟件采用了DSP／BIOS實(shí)時操作系統(tǒng)作為整個系統(tǒng)的控制核心，來完成整個控制系統(tǒng)的任務(wù)調(diào)度。前后臺系統(tǒng)中，循環(huán)程序通過中斷來觸發(fā)處理事件，其他所有后臺任務(wù)都被認(rèn)為具有相同的執(zhí)行優(yōu)先級，這就導(dǎo)致那些實(shí)時性要求高的任務(wù)得不到及時調(diào)度，影響整個系統(tǒng)的運(yùn)行效率。DSP／BIOS提供可搶占式線程調(diào)度，通過高優(yōu)先級線程搶占低優(yōu)先級線程，實(shí)現(xiàn)了線程資源的優(yōu)化配置［3］。

圖5 下位機(jī)程序流程

3 加減速規(guī)劃

機(jī)器人的軌跡插補(bǔ)不僅是對位置的插補(bǔ)，也是對速度和加速度的插補(bǔ)［4］。機(jī)械臂的啟停平穩(wěn)性直接影響到整體的控制精度，不合理的速度曲線勢必會造成電機(jī)的震蕩。機(jī)械臂是大慣量的執(zhí)行機(jī)構(gòu)，加速度是電機(jī)力矩輸出的直接結(jié)果，所以巨大、突變的加速度會給予系統(tǒng)過大的負(fù)擔(dān)，會導(dǎo)致不能及時響應(yīng)，加劇機(jī)械的磨損。

機(jī)器人的運(yùn)動分為點(diǎn)位（PTP）運(yùn)動和連續(xù)軌跡（CP）。點(diǎn)位運(yùn)動的軌跡插補(bǔ)比較簡單，只需要在規(guī)定的時間里面到達(dá)目標(biāo)位置。連續(xù)軌跡在笛卡爾空間進(jìn)行規(guī)劃，插值的方法是把目標(biāo)曲線上的點(diǎn)細(xì)化成一組帶位置和速度信息的結(jié)點(diǎn)，然后映射到關(guān)節(jié)空間完成速度曲線規(guī)劃［5］。

基于上述分析，采用了基于PVT速度規(guī)劃的位置插補(bǔ)，速度和加速度曲線連續(xù)無跳變［6］。PVT插補(bǔ)模式，即給定位移、速度和時間，用戶可以利用這些參數(shù)直接控制軌跡曲線的形狀。位置、速度和時間關(guān)系為：

給定連續(xù)2個數(shù)據(jù)點(diǎn)的“位置（s1，s2）、速度（v1，v2）和時間（t1，t2）”參數(shù)，代入函數(shù)得：

求解此方程組可以得到a，b，c，d。因此，連續(xù)2個數(shù)據(jù)點(diǎn)的運(yùn)動規(guī)律就可以確定下來。為方便解方程，特令t1＝0，解得：

也可推出速度、加速度、加加速度計(jì)算方程為：

根據(jù)上述2組計(jì)算方程即可得到3個量。將機(jī)器人軌跡規(guī)劃所得的位移、速度和時間輸入PVT插補(bǔ)器，即可求解出三次方程。最后，利用差分公式得到每個周期的插補(bǔ)位移。在給定的位移和時間條件下，選取合適的速度參數(shù)，可以得到類似S形曲線的效果，最終得到加減速控制如圖6所示。

圖6 PVT插補(bǔ)加減速控制

4 位置控制

機(jī)器人的位置控制，關(guān)鍵在于控制機(jī)器人的各個關(guān)節(jié)到達(dá)給定位置。對于多關(guān)節(jié)機(jī)器人，隨著關(guān)節(jié)遞增，對應(yīng)負(fù)載電機(jī)會隨著重力和機(jī)械慣性影響而變化，構(gòu)成一個非線性強(qiáng)耦合系統(tǒng)，因此，傳統(tǒng)PID控制器很難保證系統(tǒng)的動態(tài)響應(yīng)品質(zhì)，使系統(tǒng)性能下降。為此，引入模糊PID控制策略構(gòu)建關(guān)節(jié)空間位置控制系統(tǒng)［7］，原理如圖7所示。

圖7 位置控制系統(tǒng)

由于關(guān)節(jié)位置是一個時變量，從而使被控對象的參數(shù)變化，相應(yīng)也要改變控制器的參數(shù)以滿足系統(tǒng)要求。模糊推理器以實(shí)際位置反饋和它的變化率作為輸入，按照一定的模糊控制規(guī)則導(dǎo)出PID控制器參數(shù)KP，KI，KD，輸入給PID控制環(huán)節(jié)運(yùn)算。前饋環(huán)節(jié)是為了提高系統(tǒng)的跟蹤控制能力。位置環(huán)的PID控制器輸出和前饋通道的速度輸出，合成后作為總輸出并以速度控制指令輸出。

這種位置控制策略的主要優(yōu)勢在于：不同的位置對應(yīng)使用不同的PID參數(shù)，不需要精確的數(shù)學(xué)模型，而且能滿足機(jī)器人位置控制系統(tǒng)的要求。

5 結(jié)束語

根據(jù)工業(yè)機(jī)器人的實(shí)際要求，利用DSP和FPGA技術(shù)，設(shè)計(jì)了開放式機(jī)器人運(yùn)動控制系統(tǒng)。上位機(jī)軟件層次化設(shè)計(jì)，具有人機(jī)交互、示教和軌跡規(guī)劃功能。下位機(jī)軟件借助DSP／BIOS操作系統(tǒng)模塊化設(shè)計(jì)，支持點(diǎn)位和連續(xù)軌跡控制的插補(bǔ)算法，以及神經(jīng)網(wǎng)絡(luò)PID控制算法，設(shè)計(jì)簡單，功能強(qiáng)大。

［1］ 馬瓊雄，吳向磊，李 琳，等．基于IPC的開放式工業(yè)機(jī)器人控制系統(tǒng)研究［J］．機(jī)電產(chǎn)品開發(fā)與創(chuàng)新，2008，21（1）：15－17．

［2］ 郭 釗．基于PC的PCB鉆孔機(jī)數(shù)控系統(tǒng)的研究與開發(fā)［D］．南京：南京航空航天大學(xué)，2012．

［3］ 孫禾風(fēng)．基于DSP／BIOS的運(yùn)動控制器軟件開發(fā)［D］．南京：南京航空航天大學(xué)，2009．

［4］ 劉鵬飛，楊孟興，宋 科，等．‘S’型加減速曲線在機(jī)器人軌跡插補(bǔ)算法中的應(yīng)用研究［J］．制造業(yè)自動化，2012，34（20）：4－8，11．

［5］ 熊有倫．機(jī)器人技術(shù)基礎(chǔ)［M］．武漢：華中科技大學(xué)出版社，1996．

［6］ 舒志兵，嚴(yán)彩忠．PVT插補(bǔ)及位置伺服變加減速處理［J］．電氣應(yīng)用，2007，26（4）：86－89．

［7］ 譚 民，徐 德，候增廣，等．先進(jìn)機(jī)器人控制［M］．北京：高等教育出版社，2007．