模塊化機(jī)器人嵌入式多核主控制器設(shè)計(jì)

摘要：針對模塊化機(jī)器人控制，提出一種基于FPGA的片上多核主控制器設(shè)計(jì)方案。利用SOPC技術(shù)在單一芯片上設(shè)計(jì)兩個完全不同結(jié)構(gòu)的核心：NiosII軟核處理器和協(xié)處理器。詳細(xì)介紹了機(jī)器人控制的路徑規(guī)劃流程、NiosII軟核體系、協(xié)處理器的構(gòu)架及接口以及基于SOPC的片上多核系統(tǒng)實(shí)現(xiàn)。實(shí)驗(yàn)結(jié)果驗(yàn)證了多核主控制器設(shè)計(jì)的可行性。

關(guān)鍵詞：模塊化機(jī)器人；多核控制器；協(xié)處理器；NiosII軟核處理器

引言

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

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

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

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

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

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

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

在得到了線速度和角速度之后就可以通過雅克比矩陣得出各個關(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那樣是由固定的芯片來實(shí)現(xiàn)，而是采用IP核的方式實(shí)現(xiàn)的，可以配置在滿足任何要求的AlteraFPGA器件中，因此，Nios II處理器給實(shí)際應(yīng)用帶來了很大的靈活性。只要芯片上有足夠的空間，就可以不斷進(jìn)行升級而不用修改電路結(jié)構(gòu)。另外，Nios II處理器作為一個標(biāo)準(zhǔn)的RISC處理器，具有執(zhí)行標(biāo)準(zhǔn)的C源代碼和程序的可移植性強(qiáng)等特點(diǎn)。

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

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

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

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

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

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

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

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

5 實(shí)驗(yàn)及結(jié)果分析

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

μs，而多核主控制器的協(xié)處理器只需要約10μs的時間?？梢钥闯鲈谙嗤到y(tǒng)時鐘下，協(xié)處理器進(jìn)行運(yùn)動學(xué)正解比單Nios II軟核處理器的效率提升了兩個數(shù)量級。

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

6 結(jié)論

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