基于Nios II的機器人視覺(jué)伺服控制器的研究與設計

　　引言

　　Altera公司的Nios II處理器是可編程邏輯器件的軟核處理器。NiosII軟核處理器和存儲器、I/O接口等外設可嵌入到FPGA中，組成一個(gè)可編程單芯片系統(SOPC)，大大降低了系統的成本、體積和功耗。適合網(wǎng)絡(luò )、電信、數據通信、嵌入式和消費市場(chǎng)等各種嵌入式應用場(chǎng)合。

　　本文提出一個(gè)基于Nios II處理器結構的系統用于實(shí)現機器人實(shí)時(shí)運動(dòng)檢測跟蹤，使用線(xiàn)性卡爾曼濾波器算法來(lái)快速完成運動(dòng)估計及進(jìn)一步分析和校正，算法中的乘除利用MATLAB/DSP Builder生成的模塊作為Nios II處理器的自定義指令的設計方法。

　　機器人視覺(jué)伺服控制器的研究與設計

　　機器人視覺(jué)伺服控制就是用各種成像系統代替視覺(jué)器官作為輸入的敏感手段，并由高速處理器替代大腦完成相應的處理和解釋?zhuān)渥罱K研究目標就是使機器人視覺(jué)伺服控制器能像人那樣通過(guò)視覺(jué)觀(guān)察和理解世界，具有自主適應環(huán)境的能力，可依據視覺(jué)敏感和反饋，以某種程度的智能完成一定的任務(wù)。

　　系統硬件實(shí)現

　　基于SOPC的機器人視覺(jué)伺服控制器，主要由FPGA、存儲器和外設三個(gè)部分，如圖1所示。

　　攝像頭位置固定，它所能采集圖像的范圍稱(chēng)為視覺(jué)區域，調整攝像機使視覺(jué)區域覆蓋機器人的工作空間，即機器人要跟蹤的曲線(xiàn)在該視覺(jué)區域內。利用攝像機采集圖像，而后系統對采集的圖像進(jìn)行處理，分析、提取出離散的采樣點(diǎn)序列，最后再根據采樣點(diǎn)序列規劃機器人的運動(dòng)路徑。其中，FPGA部分核心是Nios II處理器Core。在一般的嵌入式系統開(kāi)發(fā)中，當需要新的外設模塊時(shí)往往需要在PCB上加入相應的外設芯片或者換用更高檔的CPU，而SOPC設計可以同一個(gè)FPGA芯片內加入相應的外設模塊核，并通過(guò)在片上的Avalon總線(xiàn)與NiosⅡ處理器Core相連，因而不需要在PCB這個(gè)層面上作更多的修改。成像采集裝置從目標對象場(chǎng)景中采集圖像序列，保存在SOPC的片外存儲器中，然后利用NiosⅡ處理器和定制的乘法、除法等DSP運算指令來(lái)實(shí)現線(xiàn)形卡爾曼濾波器的算法，從而實(shí)現運動(dòng)目標的識別與跟蹤。

　　系統軟件的實(shí)現

　　離散線(xiàn)性卡爾曼濾波算法

　　線(xiàn)性卡爾曼濾波是美國工程師Kalman在線(xiàn)性最小方差估計的基礎上，提出的數學(xué)結構上比較簡(jiǎn)單的最優(yōu)線(xiàn)性遞推濾波方法，具有計算量、存儲量低，實(shí)時(shí)性高的優(yōu)點(diǎn)。特別是經(jīng)歷了初始濾波的過(guò)渡狀態(tài)后，濾波效果非常好。

　　線(xiàn)性卡爾曼濾波基本算法如下：設一隨機動(dòng)態(tài)系統，其數學(xué)模型為：

　　公式1中x(k)為系統狀態(tài)矢量，w(k)為系統噪音矢量,φ(k),Г(k)為系統矩陣,公式2中Z(k)為系統觀(guān)測矢量，H(k)為系統觀(guān)測矩陣，V(k)為系統觀(guān)測噪音矩陣。

　　關(guān)于系統的隨機性，本文假定，系統噪音和觀(guān)測噪音是不相關(guān)的零均值高斯白噪聲。隨機系統的狀態(tài)估計問(wèn)題，就是根據選定的估計準則和獲取的測量信息對系統狀態(tài)進(jìn)行估計，卡爾曼濾波的估計準則是：

　　其中

　　預測誤差方程為：

　　增益矩陣方程為：

　　濾波誤差方差陣為：

　　上述公式中I是單位矩陣，Q為w(k)自協(xié)方差方差陣，R為V(k)自協(xié)方差方差陣?？柭鼮V波采用遞推算法，計算最優(yōu)濾波值時(shí)，K(k+1)由P(k+1|k)來(lái)確定，P(k+1|k)由P(k)來(lái)確定，P(k+1)由P(k+1|k)和K(k+1)來(lái)確定，如此反復遞推運算。

　　系統程序流程

　　本系統的主要功能是完成運動(dòng)目標的鎖定，并控制運動(dòng)平臺對目標進(jìn)行實(shí)時(shí)跟蹤。使用高性能NiosII處理器作為控制器控制著(zhù)整個(gè)系統的實(shí)現流程和高效的線(xiàn)性卡爾曼濾波算法對目標運動(dòng)參數的估計，有效地提高了圖像處理速度，實(shí)現了運動(dòng)目標的快速跟蹤。以下是系統流程：

　　1.系統初始化：由SOPC通過(guò)控制總線(xiàn)設置USB接口微型攝像頭工作模式，并初始化其主控程序變量。

　　2.確定運動(dòng)區域：由Nios II處理器根據圖像序列鎖定運動(dòng)物體，根據被跟蹤物體確定運動(dòng)跟蹤區間，接下來(lái)的跟蹤操作都是在這個(gè)跟蹤窗口中進(jìn)行。

　　3.預測計算：利用線(xiàn)形卡爾曼濾波器方程進(jìn)行計算。

　　4.濾波：預測和濾波是相互作用的，即由濾波得到預測而由預測又可得到濾波。

　　5.輸出：SOPC發(fā)出控制信號給隨動(dòng)平臺。

　　該系統采用集成了Nios軟核處理器的Stratix高密度FPGA，控制器通過(guò)攝像頭記錄每一時(shí)刻運動(dòng)目標的位置和速度作為觀(guān)測值。然后按照公式(3)、(4)、(5)、(6)進(jìn)行最佳狀態(tài)估計，得到每一時(shí)刻運動(dòng)目標的位置和速度的預測值。由于各種干擾因素的存在，經(jīng)過(guò)七八個(gè)時(shí)間段后預測位置與觀(guān)測位置相當接近，即可實(shí)現準確的狀態(tài)預測。

　　結語(yǔ)

　　本文建立了一套基于SOPC結構的多關(guān)節機器人視覺(jué)伺服系統，主要應用線(xiàn)性卡爾曼濾波算法成功的預測了運動(dòng)目標的狀態(tài)參數功能是鎖定運動(dòng)目標，實(shí)現對目標進(jìn)行實(shí)時(shí)跟蹤。由于應用卡爾曼濾波后極大地縮小了搜索空間，減少了系統的圖像處理時(shí)間，可以有效地提高系統的實(shí)時(shí)性，實(shí)現對運動(dòng)目標的快速跟蹤。