專利名稱：一種基于fpga的模塊化機(jī)器人嵌入式多核主控制器的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人控制領(lǐng)域，具體涉及一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器。
背景技術(shù)：
在機(jī)器人控制技術(shù)領(lǐng)域里，機(jī)器人的控制問(wèn)題一直是控制領(lǐng)域眾多學(xué)者的研究課題，現(xiàn)代控制理論所提供的幾乎所有方法都在其控制系統(tǒng)上作過(guò)嘗試。隨著機(jī)器人應(yīng)用領(lǐng)域的不斷擴(kuò)大(裝配、焊接、醫(yī)療外科、太空作業(yè))，對(duì)機(jī)器人的作業(yè)性能也提出了越來(lái)越高的要求，這也就對(duì)機(jī)器人的控制系統(tǒng)性能也提出了更高的要求。其中，機(jī)器人主控制器是整個(gè)控制系統(tǒng)中最關(guān)鍵的一部分。因此，如何開(kāi)發(fā)出具有開(kāi)放式結(jié)構(gòu)的標(biāo)準(zhǔn)化、模塊化的機(jī)器人控制系統(tǒng)無(wú)疑對(duì)提高機(jī)器人性能和自主能力、推動(dòng)機(jī)器人技術(shù)的發(fā)展具有重大意義。因此，研究模塊化機(jī)器人主控制器具有更重要的意義。目前國(guó)內(nèi)主流機(jī)器人控制大多采用上下位機(jī)的方式或者ARM處理器配合各種硬件協(xié)處理器的方式，這樣不利于實(shí)現(xiàn)高集成，低功耗，而且體積相對(duì)龐大。本論文研究的嵌入式多核主控制器的意義在于保證控制穩(wěn)定可靠的前提下，能耗更小，體積更小，功能可配置，適應(yīng)不同模塊化機(jī)器人控制需求。這也是未來(lái)模塊化機(jī)器人主控制器的發(fā)展趨勢(shì)。

發(fā)明內(nèi)容
為了解決當(dāng)前機(jī)器人主控制器集成度低，能耗高，體積相對(duì)龐大的問(wèn)題，本發(fā)明提供了一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，把不同功能的處理器核集成到單一的FPGA芯片上，采用CAN總線與各關(guān)節(jié)控制器通信。本發(fā)明實(shí)現(xiàn)了機(jī)器人主控制器的高集成度，低功耗，小體積，高運(yùn)算能力，低成本，功能可配置，廣泛適用于各種模塊化機(jī)器人。本發(fā)明的基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，它包括Nios II軟核處理器，DSP協(xié)處理器模塊，Avalon總線模塊，CAN總線控制模塊，系統(tǒng)時(shí)鐘模塊，看門(mén)狗及復(fù)位系統(tǒng)模塊，JTAG接口模塊，存儲(chǔ)器控制模塊等。主控制模塊為Nios II軟核處理器，通過(guò)Avalon總線模塊與DSP協(xié)處理器、系統(tǒng)時(shí)鐘模塊、看門(mén)狗及復(fù)位系統(tǒng)模塊、JTAG接口模塊、CAN總線控制模塊、存儲(chǔ)器控制模塊進(jìn)行訪問(wèn)控制。同時(shí)它在控制過(guò)程中負(fù)責(zé)機(jī)器人的路徑規(guī)劃，并通過(guò)CAN總線與各關(guān)節(jié)控制器通信，給出關(guān)節(jié)角度。主控制模塊將數(shù)據(jù)(當(dāng)前關(guān)節(jié)角度和目標(biāo)位置)傳輸給DSP協(xié)處理器，同時(shí)也把控制信息傳遞給DSP協(xié)處理器。DSP協(xié)處理器模塊主要包括CORDIC算法，矩陣基本運(yùn)算，求解變換矩陣和雅克比矩陣三部分功能。它負(fù)責(zé)根據(jù)Mos II軟核處理器的控制信息將輸入的數(shù)據(jù)進(jìn)行運(yùn)算，然后把得到的數(shù)據(jù)(到達(dá)目標(biāo)位置所需的關(guān)節(jié)角)傳遞給Nios II軟核處理器，由它下發(fā)給各關(guān)節(jié)控制器。本發(fā)明中，協(xié)處理器作為一個(gè)I/O設(shè)備，與主處理器之間采用松耦合的方式連接，協(xié)處理器完全接受主處理器的調(diào)度和支配。采用此方法基于如下原因1)機(jī)器人運(yùn)動(dòng)控制算法的調(diào)度、計(jì)算不是一個(gè)連續(xù)的流水線過(guò)程，而是周期性的過(guò)程。采用I/O設(shè)備方式進(jìn)行實(shí)現(xiàn)完全可以滿足需要。2)實(shí)際機(jī)器人工作要求系統(tǒng)的結(jié)構(gòu)要盡量簡(jiǎn)單，以保證系統(tǒng)基本性能的可靠性。采用該方式實(shí)現(xiàn)，主控制器系統(tǒng)模塊化較好，雖然增加了協(xié)處理器，但是并沒(méi)有過(guò)多的增加系統(tǒng)的復(fù)雜度，保證系統(tǒng)核心部件的可靠性和基本任務(wù)的運(yùn)行。3)采用獨(dú)立I/O設(shè)備方式實(shí)現(xiàn)協(xié)處理器比較簡(jiǎn)單且易于實(shí)現(xiàn)和管理，可以大大簡(jiǎn)化軟件設(shè)計(jì)。系統(tǒng)時(shí)鐘模塊負(fù)責(zé)把外部晶振產(chǎn)生的時(shí)鐘進(jìn)行處理給FPGA內(nèi)部各個(gè)模塊使用。看門(mén)狗及復(fù)位系統(tǒng)模塊負(fù)責(zé)檢測(cè)Nios II軟核處理器模塊是否鎖死，在Nios II 軟核處理器出現(xiàn)故障的時(shí)候控制電源短暫停止向其供電，實(shí)現(xiàn)Nios II軟核處理器的重啟。JTAG接口模塊在開(kāi)發(fā)時(shí)實(shí)現(xiàn)軟件系統(tǒng)的在線調(diào)試以及最終的程序燒入等功能。CAN總線控制模塊把Nios II軟核處理器傳遞的信息進(jìn)行處理通過(guò)CAN總線傳輸。 它實(shí)現(xiàn)了信息在基于FPGA的模塊化機(jī)器人嵌入式多核主控制器和各個(gè)關(guān)節(jié)控制器之間的傳輸。存儲(chǔ)器控制模塊包括FLASH存儲(chǔ)器、SDRAM存儲(chǔ)器、EPCS存儲(chǔ)器、FPGA片上存儲(chǔ)器，其中FLASH存儲(chǔ)器用于存儲(chǔ)系統(tǒng)程序，SDRAM存儲(chǔ)器用于程序的運(yùn)行，EPCS存儲(chǔ)器用于存儲(chǔ)系統(tǒng)FPGA的配置文件，F(xiàn)PGA片上存儲(chǔ)器用于存儲(chǔ)機(jī)器人控制參數(shù)。本發(fā)明采用uClinux操作系統(tǒng)作為多核主控制器的軟件開(kāi)發(fā)平臺(tái)。uClinux是一個(gè)優(yōu)秀的嵌入式Linux版本，其秉承了標(biāo)準(zhǔn)Linux的優(yōu)良特性，并經(jīng)過(guò)各方面的改造，形成了一個(gè)高度優(yōu)化的、代碼緊湊的嵌入式Linux，非常適合于嵌入式系統(tǒng)。本發(fā)明工作時(shí)，Nios II軟核處理器通過(guò)CAN總線模塊從各關(guān)節(jié)控制器獲得當(dāng)前的關(guān)節(jié)角度，然后把得到的角度值發(fā)送給DSP協(xié)處理器模塊進(jìn)行運(yùn)算，得到當(dāng)前術(shù)端的位姿，將該數(shù)據(jù)傳輸給Nios II軟核處理器；Nios II軟核處理器得到當(dāng)前位姿和目標(biāo)位姿， 通過(guò)路徑規(guī)劃算法給DSP協(xié)處理器發(fā)送每一步規(guī)劃的目標(biāo)位姿和命令；DSP協(xié)處理器進(jìn)行運(yùn)算得出實(shí)現(xiàn)該步驟每個(gè)關(guān)節(jié)角所需達(dá)到的角度值并返回給Mos II軟核處理器；最后， Nios II軟核處理器通過(guò)CAN總線把各關(guān)節(jié)角度值發(fā)送給關(guān)節(jié)控制器；如此反復(fù)直到當(dāng)前的末端位姿與目標(biāo)位姿一致。在上述過(guò)程中，Nios II軟核處理器作為路徑規(guī)劃與信息處理核心，而DSP處理器作為運(yùn)算核心，兩個(gè)主要模塊協(xié)同工作，完成控制流程。本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)1、本發(fā)明是基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，F(xiàn)PGA芯片具有體積小、能耗低、設(shè)計(jì)方式靈活等優(yōu)點(diǎn)，片上的多核系統(tǒng)減少了元器件數(shù)據(jù)(包括核心器件以及外圍器件)，縮小了體積，使得該主控制器的體積有了明顯的縮小，適合用于各種模塊化機(jī)器人的嵌入式控制場(chǎng)合。同時(shí)這種設(shè)計(jì)方式也降低了成本，縮短了開(kāi)發(fā)周期。2、本發(fā)明采用Nios II軟核處理器作為路徑規(guī)劃與信息處理核心，Nios II處理器具有功能可配置、擴(kuò)展性強(qiáng)、豐富的標(biāo)準(zhǔn)IP核接口支持等優(yōu)點(diǎn)。如果更改機(jī)器人構(gòu)型只需要更改軟件程序傳遞不同的機(jī)器人控制參數(shù)，解決了以往的機(jī)器人主控制器需要隨機(jī)器人構(gòu)型重新設(shè)計(jì)的問(wèn)題。3、本發(fā)明采用自己設(shè)計(jì)的基于FPGA的DSP協(xié)處理器作為運(yùn)算核心，該DSP協(xié)處理器核心算法為CORDIC算法，該數(shù)值逼近算法比傳統(tǒng)的超越函數(shù)求解更適合硬件實(shí)現(xiàn)，而且求解更快，精度更高。硬件語(yǔ)言實(shí)現(xiàn)的協(xié)處理器保證了運(yùn)算的快速性，控制的穩(wěn)定性、可靠性，使得整個(gè)主控制器能夠滿足高速運(yùn)動(dòng)控制的要求的同時(shí)保證控制系統(tǒng)的穩(wěn)定可靠。
4、本發(fā)明采用uClinux操作系統(tǒng)作為多核主控制器的軟件開(kāi)發(fā)平臺(tái)，與以往的控制系統(tǒng)所采用的嵌入式Linux有所不同。uClinux內(nèi)核同標(biāo)準(zhǔn)Linux內(nèi)核相比要小得多，一般uClinux的內(nèi)核大小在500KB左右，如果加上一些基本的應(yīng)用，也就在900KB左右，非常適合于嵌入式系統(tǒng)。于此同時(shí)，uClinux仍然保留了 Linux操作系統(tǒng)的主要優(yōu)點(diǎn)穩(wěn)定性、 良好的移植性、優(yōu)異的網(wǎng)絡(luò)功能、支持多種文件系統(tǒng)以及標(biāo)準(zhǔn)豐富的API等。這些保證了控制系統(tǒng)軟件運(yùn)行的穩(wěn)定性、可靠性，并且可以根據(jù)不同應(yīng)用場(chǎng)合進(jìn)行二次開(kāi)發(fā)及修改。


圖1是本發(fā)明提出的片上多核主控制器內(nèi)部結(jié)構(gòu)圖。圖2是本發(fā)明的整體系統(tǒng)結(jié)構(gòu)框圖。圖3是本發(fā)明的路徑規(guī)劃流程圖。圖4是本發(fā)明中的協(xié)處理器總體設(shè)計(jì)框架。圖5是本發(fā)明中的協(xié)處理器硬件接口邏輯。圖6是本發(fā)明的軟件系統(tǒng)構(gòu)架設(shè)計(jì)。
具體實(shí)施例方式下面結(jié)合附圖對(duì)本發(fā)明的實(shí)施方式作詳細(xì)說(shuō)明，本實(shí)施例在以本發(fā)明技術(shù)方案為前提下進(jìn)行實(shí)施，給出了詳細(xì)的實(shí)施方式和具體的操作過(guò)程。參見(jiàn)圖1所示為，片上多核主控制器內(nèi)部結(jié)構(gòu)包括Nios II軟核處理器，DSP協(xié)處理器模塊，Avalon總線模塊，CAN總線控制模塊，系統(tǒng)時(shí)鐘模塊，看門(mén)狗及復(fù)位系統(tǒng)模塊， JTAG接口模塊，存儲(chǔ)器控制模塊(FLASH存儲(chǔ)器、SDRAM存儲(chǔ)器、EPCS存儲(chǔ)器)。其中，主控制模塊為Nios II軟核處理器，它通過(guò)Avalon總線模塊與DSP協(xié)處理器、系統(tǒng)時(shí)鐘模塊、看門(mén)狗及復(fù)位系統(tǒng)模塊、JTAG接口模塊、CAN總線控制模塊、存儲(chǔ)器控制模塊相連。參見(jiàn)圖2所示的整體系統(tǒng)結(jié)構(gòu)，其中FPGA芯片采用Altera公司的EP3C25F324， CAN總線控制器芯片采用SJA1000T，CAN總線驅(qū)動(dòng)器芯片采用PCA82C250T，F(xiàn)LASH存儲(chǔ)器采用64M的JS^F640，SDRAM存儲(chǔ)器采用64M的K4S641632N-LI60，EPCS存儲(chǔ)器采用16M的 M25P16。各關(guān)節(jié)控制器包括A/D數(shù)據(jù)采集、電機(jī)驅(qū)動(dòng)模塊、CAN總線控制模塊、Nios II軟核處理器模塊等。主控制器與各關(guān)節(jié)控制器通過(guò)CAN總線通信，數(shù)據(jù)主要為關(guān)節(jié)角度值，對(duì)于有手眼視覺(jué)的機(jī)器人數(shù)據(jù)部分還可以包括當(dāng)前的末端位姿反饋。參見(jiàn)圖3所示，詳細(xì)介紹本發(fā)明的路徑規(guī)劃流程。設(shè)機(jī)器人初始和目標(biāo)位姿分別記為:xe0= [Pe0, ve。]，xef= [pef, Vef]。機(jī)器人術(shù)端沿Xetl到Xef的直線路徑運(yùn)動(dòng)，初始點(diǎn)和目標(biāo)點(diǎn)分別為 PeO (X0，y〇，Ζ0，α 0，3 0，Yo)禾口 Pef (Xf，Yf' Zf，α f，3 f，Yf)0系統(tǒng)啟動(dòng)，主控制器得到當(dāng)前的初始關(guān)節(jié)角度和目標(biāo)位姿，首先求出初始點(diǎn)和目標(biāo)點(diǎn)的直線距離長(zhǎng)度為d = JX (P/ J-Po,, )2 式(1)根據(jù)需要的總時(shí)間tf以及按梯形法規(guī)劃其運(yùn)行路徑得到的加速時(shí)間ts確定末端運(yùn)動(dòng)的最大速度為
權(quán)利要求
1.一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，其特征在于本模塊化機(jī)器人嵌入式多核主控制器是一種基于FPGA的片上多核系統(tǒng)，包括Nios II軟核處理器，DSP協(xié)處理器模塊，Avalon總線模塊，CAN總線控制模塊，系統(tǒng)時(shí)鐘模塊，看門(mén)狗及復(fù)位系統(tǒng)模塊， JTAG接口模塊，存儲(chǔ)器控制模塊等。
2.根據(jù)權(quán)利要求1所述的一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，其特征在于主控制模塊為。Nios II軟核處理器，通過(guò)Avalon總線模塊與DSP協(xié)處理器、系統(tǒng)時(shí)鐘模塊、看門(mén)狗及復(fù)位系統(tǒng)模塊、JTAG接口模塊、CAN總線控制模塊、存儲(chǔ)器控制模塊進(jìn)行訪問(wèn)控制及中斷響應(yīng)。主運(yùn)算模塊為Verilog語(yǔ)言編寫(xiě)的DSP協(xié)處理器，主要包括CORDIC 算法，矩陣基本運(yùn)算，求解變換矩陣和雅克比矩陣三部分功能。它負(fù)責(zé)根據(jù)主控制模塊的控制信息將輸入的數(shù)據(jù)進(jìn)行運(yùn)算，然后把得到的數(shù)據(jù)反饋給主控制模塊并下發(fā)給各關(guān)節(jié)控制ο
3.根據(jù)權(quán)利要求1所述的一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，其特征在于系統(tǒng)時(shí)鐘模塊負(fù)責(zé)把外部晶振產(chǎn)生的時(shí)鐘進(jìn)行處理給FPGA內(nèi)部各個(gè)模塊使用；看門(mén)狗及復(fù)位系統(tǒng)模塊負(fù)責(zé)檢測(cè)主控制模塊是否鎖死，在主控制模塊出現(xiàn)故障的時(shí)候控制電源短暫停止向其供電，實(shí)現(xiàn)主控制模塊的重啟；JTAG接口模塊在開(kāi)發(fā)時(shí)實(shí)現(xiàn)軟件系統(tǒng)的在線調(diào)試以及最終的程序燒入等功能；CAN總線控制模塊把主控制模塊傳遞的信息進(jìn)行處理通過(guò)CAN總線傳輸。它實(shí)現(xiàn)了信息在基于FPGA的模塊化機(jī)器人嵌入式多核主控制器和各個(gè)關(guān)節(jié)控制器之間的傳輸；存儲(chǔ)器控制模塊包括FLASH存儲(chǔ)器、SDRAM存儲(chǔ)器、EPCS存儲(chǔ)器、FPGA 片上存儲(chǔ)器，其中FLASH存儲(chǔ)器用于存儲(chǔ)系統(tǒng)程序，SDRAM存儲(chǔ)器用于程序的運(yùn)行，EPCS存儲(chǔ)器用于存儲(chǔ)系統(tǒng)FPGA的配置文件，F(xiàn)PGA片上存儲(chǔ)器用于存儲(chǔ)機(jī)器人控制參數(shù)。
4.根據(jù)權(quán)利要求1所述的一種基于FPGA的模塊化機(jī)器人嵌入式多核主控制器，其特征在于采用uClinux操作系統(tǒng)作為多核主控制器的軟件開(kāi)發(fā)平臺(tái)。
全文摘要
一種機(jī)器人控制技術(shù)領(lǐng)域的基于FPGA芯片的嵌入式多核主控制器。本發(fā)明中，主控制器集成Nios II軟核處理器與DSP協(xié)處理器兩個(gè)主要模塊。Nios II軟核處理器通過(guò)Avalon總線模塊與系統(tǒng)時(shí)鐘模塊、看門(mén)狗及復(fù)位系統(tǒng)模塊、JTAG接口模塊、CAN總線控制模塊、存儲(chǔ)器控制模塊通信；DSP協(xié)處理器作為硬件計(jì)算核心通過(guò)Avalon總線模塊與Nios II軟核通信。Nios II軟核負(fù)責(zé)機(jī)器人的運(yùn)動(dòng)規(guī)劃及與各個(gè)關(guān)節(jié)控制器通信，而協(xié)處理器模塊負(fù)責(zé)進(jìn)行運(yùn)動(dòng)學(xué)的解算。本發(fā)明是采用Nios II軟核作為通信核心，協(xié)處理器作為計(jì)算核心，實(shí)現(xiàn)了快速穩(wěn)定控制的同時(shí)，降低了能耗，縮小了體積。
文檔編號(hào)G05B19/04GK102393656SQ20111038761
公開(kāi)日2012年3月28日 申請(qǐng)日期2011年11月29日 優(yōu)先權(quán)日2011年11月29日
發(fā)明者葉平, 孫漢旭, 張丹, 曾祥宇, 賈慶軒, 魏楠哲 申請(qǐng)人:北京郵電大學(xué)