引言

　　隨著科技的進步，機器人技術(shù)正在向智能機器和智能系統(tǒng)的方向發(fā)展，其發(fā)展趨勢主要為結(jié)構(gòu)的模塊化和可重構(gòu)化;控制技術(shù)的開放化、可配置化;伺服驅(qū)動技術(shù)的數(shù)字化和分散化;多傳感器融合技術(shù)的實用化。機器人的內(nèi)涵也已變?yōu)椤办`活應(yīng)用機器人技術(shù)的、具有實在動作功能的智能化系統(tǒng)”。近十幾年來，FPGA行業(yè)也在飛速的發(fā)展，它在實際科研開發(fā)中的地位也從處理簡單邏輯上升到了數(shù)字系統(tǒng)的核心處理器件。SOPC(可編程片上系統(tǒng))技術(shù)的出現(xiàn)，使得將CPU核與設(shè)備核以及系統(tǒng)軟件集成到單一芯片中成為可行，它能夠幫助用戶快速開發(fā)出所需要的產(chǎn)品。

　　本文結(jié)合實際項目以一個固定基座的單臂六自由度機器人作為研究對象，提出一種基于FPGA的模塊化機器人嵌入式多核主控制器設(shè)計方案。

　　1 機器人路徑規(guī)劃

　　設(shè)機械臂初始和終止位姿分別記為：Xe0=[Pe0，ψe0]，Xef=[Pef，ψef]。要求機械臂末端手爪沿Xe0到Xef的直線路徑運動，起點和終點分別為Pe0(x0.y0，z0，α0，β0，γ0)和Pef(xf，yf，***，αf，βf，γf)，機器人手抓運行軌跡如圖1所示。

?

　　由初始點和終止點坐標(biāo)計算出首末端直線距離長度為：

　　對于一個路徑段，由于兩端的過渡域具有相同的持續(xù)時間，因而在這兩個域中，采用相同的恒加速度值，只有符號相反，帶拋物線過渡的線性插值如圖2所示。

　　在求解加速度的基礎(chǔ)上可以得出末端運行線速度，末端線速度分為加速段、勻速段和減速段，其具體表達式為：

　　在得到了線速度和角速度之后就可以通過雅克比矩陣得出各個關(guān)節(jié)的關(guān)節(jié)角速度，再乘以固定的時間即可得出在每一步的關(guān)節(jié)轉(zhuǎn)角。

　　2 Nios II軟核體系結(jié)構(gòu)

　　Nios II是Altera公司的第二代用戶可配置的通用32位RISC軟核處理器，是Altera公司特有的基于通用FPGA架構(gòu)的CPU軟核。它具備完整的32位指令集、32位數(shù)據(jù)通道和地址空間;帶有32個通用寄存器;支持32個外部中斷源;單指令的32位與32位乘法和除法結(jié)果是32位;對于結(jié)果為64位或128位的乘法，提供專用指令;大多數(shù)指令可以在一個時鐘周期內(nèi)完成;帶有單指令桶形移位寄存器;可以訪問各種片上外設(shè)，提供與片外存儲器和外設(shè)的接口;處理器性能超過200 DMIPS。

　　Nios II是一個可配置的軟核處理器，用戶可以根據(jù)性能和成本的要求來增加或刪減處理器的功能。Nios II處理器不像ARM那樣是由固定的芯片來實現(xiàn)，而是采用IP核的方式實現(xiàn)的，可以配置在滿足任何要求的AlteraFPGA器件中，因此，Nios II處理器給實際應(yīng)用帶來了很大的靈活性。只要芯片上有足夠的空間，就可以不斷進行升級而不用修改電路結(jié)構(gòu)。另外，Nios II處理器作為一個標(biāo)準(zhǔn)的RISC處理器，具有執(zhí)行標(biāo)準(zhǔn)的C源代碼和程序的可移植性強等特點。

　　3 協(xié)處理器結(jié)構(gòu)

　　由于模塊化機器人的控制需要進行大量復(fù)雜的三角函數(shù)運算以及矩陣運算，單一Nios II軟核處理器在完成這些運算的同時并不能保證控制的快速性、實時性要求。因此本文設(shè)計一個專用的IP軟核作為協(xié)處理器，用來進行各種機器人運動學(xué)的解算。

　　協(xié)處理器總體設(shè)計框架如圖3所示。GDB、GAB、GCB分別表示外部的數(shù)據(jù)、地址和控制總線。協(xié)處理器主要包含輸入/輸出接口邏輯、機器人參數(shù)存儲區(qū)、協(xié)處理器運算控制模塊、運算處理模塊、命令字寄存器、協(xié)處理器狀態(tài)字寄存器和輸入/輸出FIFO構(gòu)成。

　　在本設(shè)計中，協(xié)處理器作為一個I/O設(shè)備與主處理器之間連接，協(xié)處理器完全接受主處理器的調(diào)度和支配。由于機器人控制算法的調(diào)度與計算是一個周期性過程，因此采用I/O設(shè)備方式即可滿足要求。采用此方式系統(tǒng)雖然增加了協(xié)處理器，但卻沒有增加復(fù)雜度，利于保證核心部件的可靠性且易于實現(xiàn)和管理，能夠簡化軟件設(shè)計。針對該接口模式的設(shè)計，協(xié)處理器與主處理器之間的硬件接口結(jié)構(gòu)框圖如圖4所示。

　　協(xié)處理器面向Nios II處理器接口分為參數(shù)區(qū)和寄存器區(qū)，通過寫命令寄存器來識別。通常情況下，給出的CMD為正常計算模式，默認(rèn)選擇寄存器區(qū);當(dāng)需要對基本參數(shù)進行訪問/修改時，給出的CMD為修改參數(shù)模式。其中寄存器區(qū)包括各類寄存器和輸入輸出FIFO。圖4中各信號含義在表1內(nèi)進行了介紹。

　　4 片上多核結(jié)構(gòu)

　　利用SOPC技術(shù)把Nios II軟核、專用協(xié)處理器、存儲器控制IP、CAN總線控制邏輯、內(nèi)部計數(shù)器等全部集成到FPGA芯片上?；赟OPC的多核控制器的系統(tǒng)結(jié)構(gòu)如圖5所示。

　　整個分布式控制系統(tǒng)結(jié)構(gòu)如圖6所示，多核主控制器通過CAN總線與各關(guān)節(jié)控制器通信并對反饋信息進行處理，進行路徑規(guī)劃，下發(fā)關(guān)節(jié)角。各關(guān)節(jié)控制器采集電機當(dāng)前角度信息并通過電機驅(qū)動模塊控制電機運動。

　　5 實驗及結(jié)果分析

　　實驗系統(tǒng)包括基于FPGA的嵌入式多核主控制器、機器人關(guān)節(jié)控制器以及單臂六自由度機器人等。運動學(xué)正解運算性能時間對比如表2所列。在保證控制準(zhǔn)確、有效的前提下分別采用單Nios II軟核處理器與協(xié)處理器進行運動學(xué)正解運算。單Nios II軟核處理的耗時約為2900

　　μs，而多核主控制器的協(xié)處理器只需要約10μs的時間。可以看出在相同系統(tǒng)時鐘下，協(xié)處理器進行運動學(xué)正解比單Nios II軟核處理器的效率提升了兩個數(shù)量級。

　　表3列出了一個控制周期(多核主控制器采用路徑規(guī)劃算法控制機器人從空間中一點到下一點)內(nèi)各階段的處理時間，整個控制周期所需時間約為24.97 ms，比之前單獨采用Nios II軟核處理器控制的100ms節(jié)約了大概3/4的時間。而且，用于CAN總線通信及數(shù)據(jù)采集的部分(獲取目標(biāo)位姿與當(dāng)前關(guān)節(jié)角、下發(fā)下一步關(guān)節(jié)角)所占用的時間為整個周期的76%。由此可以看出，該設(shè)計能極大地提高系統(tǒng)處理能力，并能夠很好解決嵌入式控制器運算能力對模塊化機器人控制系統(tǒng)的約束問題。

　　6 結(jié)論

　　本文對基于FPGA的模塊化機器人嵌入式多核主控制器進行了研究，重點討論了路徑規(guī)劃方法，分析了NiosII軟核體系結(jié)構(gòu);并對協(xié)處理器結(jié)構(gòu)設(shè)計、片上結(jié)構(gòu)設(shè)計做了詳細介紹。最終實驗結(jié)果也表明了該設(shè)計的可行性以及控制的快速性。在更改機器人結(jié)構(gòu)的時候，只需要分析連桿坐標(biāo)系，獲得D-H參數(shù)并傳遞給協(xié)處理器，該多核主控制器仍能可靠有效地工作。