基于DSP與FPGA的機器人運動控制系統設計

李 疆，游有鵬

（南京航空航天大學機電學院，江蘇 南京210016）

0 引言

隨著電子技術和計算機技術的發展，機器人已經在眾多領域得到了日益廣泛的應用。作為機器人的核心部分，機器人控制器是影響機器人性能的關鍵部分之一。傳統的機器人控制系統基本采用專用計算機和專用機器人語言、專用微處理器的封閉式控制系統結構。這種結構限制了機器人系統的擴展性和靈活性，不利于進一步的擴展和開發。而通用、可靠、開放和實時動態的開放式控制系統，已是機器人控制系統發展的必然趨勢。

利用DSP和FPGA設計了開放式架構的運動控制系統。以PC機為依托，利用它的強大處理能力，完成一些非實時任務，并通過PCI總線與下位機核心處理器DSP進行數據交互。下位機運動控制卡對機械臂進行精插補規劃和實時控制等任務，并在其中嵌入了智能控制算法，同時克服了封閉式系統可擴展性差的缺陷［1］。

1 硬件平臺設計

1．1 主控單元

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

圖1 硬件架構

1．2 上下位機通信接口

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

1．3 電機驅動接口

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

1．4 編碼器反饋信號

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

圖2 編碼器反饋原理

1．5 開關量接口

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

圖3 開關量接口原理

2 控制系統軟件設計

系統軟件主要劃分為3個層次，包括上位機軟件、PCI驅動程序以及運動控制DSP軟件。上位機軟件作為人機交互部分，主要實現狀態顯示、軌跡規劃、軌跡優化、示教再現和參數管理等功能。PCI驅動程序主要完成上位機軟件與運動控制卡之間的數據交互，包括命令的發送接收。運動控制DSP軟件負責機器人控制系統的實時控制部分，主要實現速度規劃、位置控制、I／O控制和參數設置等功能［2］。控制系統總體架構如圖4所示。

圖4 系統軟件架構

上位機采用Visual C＋＋6．0開發。狀態顯示用來監測機器人控制系統的運行狀態，并根據監測結果進行跟蹤處理。軌跡規劃主要完成理論曲線的速度前瞻控制，確保了運動的平穩性；軌跡優化則是研究時間最小的軌跡規劃算法，使機器人的作業效率達到最優。示教再現指機器人通過記錄機器人被操作路徑上的點坐標，實現軌跡復現的過程。參數管理用以設置各個關節的運動參數，包括速度、加速度和加加速度的限制等。

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

下位機的控制程序流程如圖5所示。為了促使軟件模塊化，提高開發效率，軟件采用了DSP／BIOS實時操作系統作為整個系統的控制核心，來完成整個控制系統的任務調度。前后臺系統中，循環程序通過中斷來觸發處理事件，其他所有后臺任務都被認為具有相同的執行優先級，這就導致那些實時性要求高的任務得不到及時調度，影響整個系統的運行效率。DSP／BIOS提供可搶占式線程調度，通過高優先級線程搶占低優先級線程，實現了線程資源的優化配置［3］。

圖5 下位機程序流程

3 加減速規劃

機器人的軌跡插補不僅是對位置的插補，也是對速度和加速度的插補［4］。機械臂的啟停平穩性直接影響到整體的控制精度，不合理的速度曲線勢必會造成電機的震蕩。機械臂是大慣量的執行機構，加速度是電機力矩輸出的直接結果，所以巨大、突變的加速度會給予系統過大的負擔，會導致不能及時響應，加劇機械的磨損。

機器人的運動分為點位（PTP）運動和連續軌跡（CP）。點位運動的軌跡插補比較簡單，只需要在規定的時間里面到達目標位置。連續軌跡在笛卡爾空間進行規劃，插值的方法是把目標曲線上的點細化成一組帶位置和速度信息的結點，然后映射到關節空間完成速度曲線規劃［5］。

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

給定連續2個數據點的“位置（s1，s2）、速度（v1，v2）和時間（t1，t2）”參數，代入函數得：

求解此方程組可以得到a，b，c，d。因此，連續2個數據點的運動規律就可以確定下來。為方便解方程，特令t1＝0，解得：

也可推出速度、加速度、加加速度計算方程為：

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

圖6 PVT插補加減速控制

4 位置控制

機器人的位置控制，關鍵在于控制機器人的各個關節到達給定位置。對于多關節機器人，隨著關節遞增，對應負載電機會隨著重力和機械慣性影響而變化，構成一個非線性強耦合系統，因此，傳統PID控制器很難保證系統的動態響應品質，使系統性能下降。為此，引入模糊PID控制策略構建關節空間位置控制系統［7］，原理如圖7所示。

圖7 位置控制系統

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

這種位置控制策略的主要優勢在于：不同的位置對應使用不同的PID參數，不需要精確的數學模型，而且能滿足機器人位置控制系統的要求。

5 結束語

根據工業機器人的實際要求，利用DSP和FPGA技術，設計了開放式機器人運動控制系統。上位機軟件層次化設計，具有人機交互、示教和軌跡規劃功能。下位機軟件借助DSP／BIOS操作系統模塊化設計，支持點位和連續軌跡控制的插補算法，以及神經網絡PID控制算法，設計簡單，功能強大。

［1］ 馬瓊雄，吳向磊，李 琳，等．基于IPC的開放式工業機器人控制系統研究［J］．機電產品開發與創新，2008，21（1）：15－17．

［2］ 郭 釗．基于PC的PCB鉆孔機數控系統的研究與開發［D］．南京：南京航空航天大學，2012．

［3］ 孫禾風．基于DSP／BIOS的運動控制器軟件開發［D］．南京：南京航空航天大學，2009．

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

［5］ 熊有倫．機器人技術基礎［M］．武漢：華中科技大學出版社，1996．

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

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