基于強跟蹤濾波的機器人運動軌跡控制系統設計

霍延軍，崔 巍

(1.延安職業技術學院 人事處，陜西 延安 716000；2.延安大學 物理與電子信息學院，陜西 延安 716000)

0 引言

在工業生產中，機器人常常遇到起動或停止時手抖動、機器人末端執行器不能移動到指定位置等問題。這種問題的出現，大大提高了機器人機構損壞概率[1]。這就要求機器人系統在運動過程中必須遵循盡可能平穩的原則，避免位置突然轉變而導致速度和加速度的改變[2]。若移動不穩定，則會增加機械零件磨損，振動和沖擊機器人系統。為保證機器人能夠到達指定位置，需提高整個機器人系統運行穩定性，因此，需先設計出運行軌跡，再借助仿真技術進行仿真分析以避免不良現象發生[3]。軌跡規劃優劣，直接影響機器人操作效率。

以往采用拋物過渡法來控制該系統，在節點空間進行軌跡規劃，計算簡單[4]。但由于關節空間的軌跡規劃路徑不唯一，關節角度函數不具代表性，且運動軌跡易漏檢；而空間路徑規劃方法嚴格要求運動路徑和姿態瞬時變化規律為動作路徑和姿態，必須在直角空間執行軌跡規劃，用關節角函數來描述機器人運動軌跡，即機器人末端執行機構運動軌跡由關節變量直接確定[5]。但是，直角空間路徑規劃需要大量計算，導致控制范圍過大。根據傳統方法存在的問題，設計了基于強跟蹤濾波的機器人運動軌跡控制系統。將強跟蹤濾波方法應用于軌跡控制中，實驗結果說明該系統設計的有效性。

1 系統總體架構設計

提出的基于強跟蹤濾波的機器人軌跡規劃方法，應用在機器人軌跡控制系統中，并在圖1中得到具體描述。

圖1 系統總體架構

由圖1可知，該系統通過軟硬件的協同工作，實現了伺服控制。使用軌跡規劃器，將輸出規劃位置輸入軸資源AXIS之中；資源軸是指控制并輸出驅動報警和停止信號軟、硬件組合[6]。控制脈沖經等效變換后送至電機控制器，再通過比較規劃位置和實際接收脈沖信號，獲取誤差，及時調整，最終獲得機器人軌跡[7]。

2 硬件結構設計

通過拉伸直流伺服電機提供機器人各關節的轉矩，DC電機線性調速特性好，過載能力強[8]。采用三菱 IPM電機驅動，通過強大跟蹤濾波控制整個系統。硬件結構如圖2所示。

圖2 硬件結構

根據指令和感知信息控制機器人，可完成某一特定動作或任務的軌跡控制[9]。該系統采用多CPU結構及分布式控制方式，分為上下位機控制系統兩個層次[10]。上位機負責管理整個系統，下位機由許多單片機組成，單片機控制機器人關節動作，上下位機的CPU通過并行總線相連[11]。

2.1 PCI-485接口卡

上下位機與外部設備連接的總線就是PCI總線，具有即插即用優勢，保證數據穩定、高效傳輸。在各種與計算機連接的設備中得到廣泛應用[12]。考慮到 PCI總線協議復雜性，設計利用 PLX專用PCI9052接口控制 PCI總線，以實現 PCI上、下位機的通訊。整個數據傳輸系統的結構如圖3所示。

圖3 數據傳輸系統結構

MAX485卡插入計算機的 PCI插槽，主要包括專用 PCI芯片 EXRXR17D158，該芯片由整流控制器93LC46提供額外存儲空間，并被連接到DART通道，SP485微分信號直接與DSPMAX485芯片控制卡連接，通過LF2407模塊的DSP控制芯片串行通信，實現上位機與下位機之間數據傳輸[13]。

2.2 上下位機

上位機為普通臺式計算機，安裝了 Windows XP操作系統，主要分析機器人運動軌跡。其作用是向下位機發送各種任務中斷信息，刺激下位機調用相應程序來完成操作任務[14]。

采用專用PCI-485板完成上下位機通訊，并將RS-485模塊差分信號接入下位機之中，實現可靠數據傳輸。下位機采用TMS320LF2407A型號 DSP芯片，主要完成脈沖信號的輸出、位置反饋信號的采集與處理。將輸出脈沖調制 PWM連接到電機驅動端，要求增加光電隔離，其功能是隔離強弱電，驅動裝置和馬達連接。馬達輸出的扭矩由傳動裝置帶動各個關節運動。

2.3 DSP控制器

DSP控制器實現了程序空間與數據空間的完全分離，并可同時獲得指令和操作數，實現不同程序空間之間數據傳輸。TMS32OF240XDS具有獨立的內部數據結構和程序總線結構的 CPU內核[15]。

程序總線分為6個16位總線，即：程序地址總線，它提供讀寫程序空間的地址；數據讀地址總線，提供數據空間讀地址；數據寫地址總線，提供寫數據空間地址；程序讀總線是從程序空間向 CPU傳輸代碼、直接操作信息；數據讀總線是從數據空間向中央邏輯單元傳輸數據；數據寫總線是從程序空間和數據空間傳輸數據。兩者都是獨立的地址總線，CPU能夠在同一機器運行周期中同時讀寫數據。

TMS320f240xds流水線有4個不同階段：指令獲取、指令解碼、操作數獲取和指令執行。提取指令占用 PAB和 PRDB；解碼指令不占用數據總線；獲得操作數時會占用 DRAB和 DRDB；指令執行時會將執行結果寫回數據內存，使指令運行速度大大提高。

2.4 光電隔離驅動器

選擇CAN-TTLG型號單片機光隔離超遠程驅動器，具有光速發射，光速接收和信號放大優勢。由輸入電信號驅動 LED 發射出一定波長的光，然后由光電檢測器接收，產生光電流，然后放大輸出。因為光電隔離的輸入與輸出是相互隔離的，所以電信號傳輸是單向性的，所以電絕緣性能好，抗干擾能力強。光電隔離驅動器輸入端為低阻抗元件，工作于電流模式，所以共模抑制能力強。因此，將其作為終端隔離驅動器能夠提高信噪比。

TXD、RXD通訊口要實現遠距離通訊，通常采用RS422接口，以此收發控制信號。選擇CA-TTLG光電隔離超遠程驅動器，無需收發控制信號，可延伸單片機通訊距離，最長可達到10 km，便于單片機組網。單片機的 TXD和 RXD信號由 TTL級驅動電路驅動，而信號自接收抑制電路則保證了設備不會接收到自己發出的信號，CA總線通過光電隔離輸出 CA信號。采用隔離電源模塊，實現信號與電氣的隔離，確保系統抗干擾性能。

2.5 直流電機

選用100 W綜合交流AGV低壓直流電動機，轉換直流功率為機械能，或選用旋轉電動機轉換機械能。該電動機可以實現直流電與機械能的轉換，可將電能轉化為機械能，也可將機械能轉化為電能。

直流電機結構如圖4所示。

圖4 直流電機結構

電源接口具有3個端子號，分別是PE/VCC/GD，其對應的端子定義為接地端子、電源正、電源負。對于PE端子號連接機柜大地，線纜截面積保持0.75 mm2以上；VCC/GD端子號主電源輸入端，電壓為直流30.5～38.5 V，這兩個端子嚴格按照電源正負連接，嚴禁反接。

電機接口具有8個端子號，分別是DIR+、DIR-、PUL+、PUL-、ENA+、ENA-、ALM+、ALM-，其對應的端子定義為方向輸入正、方向輸入負、脈沖輸入正、脈沖輸入負、使能輸入正、使能輸入負、報警輸出正、報警輸出負。對于DIR+、DIR-端子號光耦隔離輸入，最大輸入電壓為6 V，在輸入同時應串聯2.4 K電阻限流，并在脈沖信號停止至少5 μs之后再切換方向信號；對于PUL+、PUL-端子號最大輸入頻率為200 kHz，在信號切換完成5 μs之后，再輸出脈沖信號；對于ENA+、ENA-端子號最大輸入頻率為1 kHz，當配置為高有效時，該端口無輸入使能；當配置為低有效時，該端口有輸入使能。在使能信號有效至少10 ms后，再輸入脈沖指令；對于ALM+、ALM-端子號最大耐壓為30 V，最大帶負載能力為30 mA，設置為有報警輸出和無報警輸出。

3 系統軟件部分實現

以VC++為面向對象的上層系統開發工具，完成控制界面程序基于MFC的API功能設計，結合MallControl卡，以及操作者與信息交互之間的關系，可促使系統執行相關任務。該系統集成了機械手運動控制，實現了對機器人運動動作的控制。

主機程序通過VC提供的MS通信控制，從端口以事件驅動方式獲取數據。如果緩沖區收到字符，則會觸發通訊任務。根據光電隔離驅動器的串行通信協議，通過串行通信控制接口，實現了事件響應功能。

如果參數 Rthreshold為0，緩沖區將收到字符，已分配給程序，沒有產生通訊任務。如果參數 Rthreshold為1，緩沖區中將收到字符，每一個字符都會觸發一項通訊任務。

系統軟件包括主界面軟件和連續 PT運動控制軟件，主要接口接收目標姿態數據，可進行點到點運動和各軸狀態檢測。依據檢測結果確定各關節的絕對位置，判斷是否超出各關節的活動范圍，并在主界面上實時顯示實際位置和計劃位置，便于用戶及時作出決定。通過強跟蹤濾波方法，對機器人運動控制進行重力補償，保證各個關節之間非線性。

設機器人運動方程為：

(1)

e=sd-s

(2)

基于對話框的機器人軌跡規劃軟件，采用 Visual c++6.0的 MFC核心編寫。計算機與機器人之間的通信可通過菜單命令和功能按鈕完成，調用參數設置窗口，退出機器人控制系統，包括控制機器人姿態、讀數等。基于強跟蹤濾波方法在機器人運動軌跡控制步驟如圖5所示。

圖5 機器人運動軌跡控制步驟

因為系統是非線性的，狀態轉換矩陣不確定，所以要實現系統穩定運行，就要將強跟蹤濾波應用其中，通過引入弱化因子，在線調整濾波增益，最大程度提取有效信息，根據提前預測軌跡，設計具體運行軌跡，由此完成系統軟件部分設計。

4 實驗分析

為驗證基于強跟蹤濾波的機器人運動軌跡控制系統的性能，設計仿真實驗。采用MATLAB仿真軟件作為實驗平臺，根據機器人的實際尺寸進行三維仿真，因為下位機和上位機之間的通信也是通過 VC進行的，所以通過 VC實現三維仿真，在 VC接口上直接進行實時驗證，有利于整個系統調試。利用 VC++6.0開發平臺，在 MFC的基礎上，引入 OpenGL動態鏈接庫，對系統的設計進行驗證。

實驗參數設置如表1所示。

表1 實驗參數

在表1所示的實驗參數下，以機器人抓取物品的過程為例，選取抓取物品曲線起始點、終止點和兩點之間結點，共5個地址變量，如圖6所示。

圖6 機器人抓取物品曲線連接示意圖

由圖6可知：該機器人抓取物品的運動軌跡具有不規則性，因此針對這5個節點，分別采用拋物過渡法、空間路徑規劃方法設計的系統和基于強跟蹤濾波控制系統對運動軌跡進行對比分析，結果如圖7所示。

圖7 三種系統運動軌跡對比結果

由圖7可知：使用拋物過渡系統5個節點均不在實際機器人運動所在位置，誤差較大；使用空間路徑規劃系統5個節點中有2個節點在實際機器人運動所在位置，其余3個與實際位置有所偏差；基于強跟蹤濾波控制系統5個節點均在實際機器人運動所在位置，無偏差。

在此基礎上測試采用不同控制系統機器人多次抓取物品的軌跡控制時間，得到軌跡控制耗時對比結果如圖8所示。

圖8 三種系統運動軌跡對比結果

通過圖8可以看出，在10次抓取實驗過程中，采用拋物過渡系統的機器人運動軌跡控制耗時平均為12.1s，采用空間路徑規劃系統的機器人運動軌跡控制耗時平均為7.5s，而采用所提基于強跟蹤濾波的機器人運動軌跡控制系統的2.3。由此可見，所提方法能夠快速對機器人運動信息做出采集，并及時規劃最短運動軌跡，有效提升機器人的工作效率。

5 結束語

為了解決現有機器人運動軌跡控制系統的不足，提出了一種基于強跟蹤濾波的機器人運動軌跡控制系統設計方案，并將該方案應用到機器人時間優化軌跡規劃中，極大地提高了工作效率。針對機器人的實際工況，在強跟蹤濾波技術支持下，給出了機器人運動軌跡，并用 Matlab在實驗條件下進行了仿真分析，以驗證系統設計的合理性。由驗證結果可知，該系統實現了機器人運動軌跡的精準控制。

受到實驗條件限制，系統設計仍存在不足之處，仍需后續繼續完善。

1)所設計的軌跡控制系統僅僅是基于機器人最優時間進行軌跡優化的，對能量優化或時間-能量優化軌跡設計的進一步研究將是今后工作方向。

2)根據實驗情況，可以考慮在該機器人機械臂的末端安裝一個觸覺傳感器，以抓住較硬物體。對機器人運動姿態而言，應采用何種姿態對不同形狀物體進行抓取，還有進一步考慮。

3)在實驗研究基礎上，該視覺反饋裝置還可加入實現三維圖像處理的爪子位置操縱器，并進一步研究基于機器人軌跡最佳控制方案。