專利名稱：一種多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人控制技術(shù)領(lǐng)域。具體涉及一種基于網(wǎng)絡(luò)的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器。
背景技術(shù)：
在多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃中，需要進(jìn)行大量的數(shù)學(xué)計(jì)算如坐標(biāo)變換、正逆運(yùn)動(dòng)學(xué)方程求解、高次插補(bǔ)運(yùn)算等，這一系列的運(yùn)算均需采用高速的處理器才能滿足運(yùn)動(dòng)規(guī)劃控制的實(shí)時(shí)性要求?！盎诜植际娇刂频募床寮从脵C(jī)械臂系統(tǒng)”(CN1586829)專利技術(shù)，采用8位單片機(jī)來完成運(yùn)動(dòng)規(guī)劃控制，運(yùn)算能力有限，難以滿足高速的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃控制要求。
在多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃中，機(jī)器人控制器與各自由度伺服驅(qū)動(dòng)器之間需要進(jìn)行大量的數(shù)據(jù)交換。機(jī)器人及其控制系統(tǒng)(周志紅，周國容.PR機(jī)器人及其控制系統(tǒng).機(jī)電一體化.2004第1期)，由一個(gè)DSP控制器對機(jī)器人的4個(gè)關(guān)節(jié)進(jìn)行插補(bǔ)運(yùn)算和伺服控制，控制器與各關(guān)節(jié)之間采用點(diǎn)對點(diǎn)的傳輸方式，其傳輸速度和傳輸效率有待進(jìn)一步提高，同時(shí)控制器與每個(gè)關(guān)節(jié)之間連接都需要設(shè)計(jì)接口電路和數(shù)條電纜，使得整個(gè)控制系統(tǒng)結(jié)構(gòu)較為復(fù)雜。
另外，隨著機(jī)器人應(yīng)用領(lǐng)域的擴(kuò)展，如危險(xiǎn)環(huán)境下機(jī)器人作業(yè)，本地控制已無法滿足其控制要求，需要機(jī)器人系統(tǒng)具有遠(yuǎn)程操作能力。采用自下而上的設(shè)計(jì)思想，設(shè)計(jì)了基于PC、TMS320VC5410DSP和TMS320LF240DSP的三層控制系統(tǒng)(王沫楠，孫立寧.多運(yùn)動(dòng)方式移動(dòng)機(jī)器人控制系統(tǒng)設(shè)計(jì).電機(jī)與控制學(xué)報(bào).2005第9卷第4期)，其結(jié)構(gòu)較為合理，但由于PC與DSP5410之間采用串口通訊方式，最大波特率為115200bps，系統(tǒng)的實(shí)時(shí)規(guī)劃性能受到一定的限制，尤其是在系統(tǒng)網(wǎng)絡(luò)化和多機(jī)器人系統(tǒng)擴(kuò)展時(shí)，通訊速度以及控制的靈活性有待進(jìn)一步提升。以ARM和FPGA為核心的機(jī)器人分布式CAN總線控制系統(tǒng)體系(劉淼等.一種機(jī)器人嵌入式網(wǎng)絡(luò)化控制系統(tǒng)體系結(jié)構(gòu)的研究.機(jī)器人.2006第28卷第2期)便具有一定的網(wǎng)絡(luò)控制功能，但是其用于完成運(yùn)動(dòng)規(guī)劃算法的控制器主頻為67.5MHz，運(yùn)算速度較不高，限制了系統(tǒng)運(yùn)動(dòng)規(guī)劃的實(shí)時(shí)性。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種結(jié)構(gòu)簡單、體積小、運(yùn)算能力強(qiáng)、可遠(yuǎn)程操作、具有運(yùn)動(dòng)實(shí)時(shí)規(guī)劃能力及快速數(shù)據(jù)交換能力的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器。該控制器可用于工業(yè)機(jī)器人、機(jī)械臂、空間機(jī)器人、類人機(jī)器人等具有多個(gè)自由度模塊的機(jī)器人控制系統(tǒng)。
為了實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案是本發(fā)明包括電源轉(zhuǎn)換模塊、DSP芯片、以太網(wǎng)控制器、以太網(wǎng)連接器(內(nèi)置以太網(wǎng)變壓器)、CAN總線連接器、CAN總線接口芯片。外部直流電源與電源轉(zhuǎn)換模塊的輸入端連接，電源轉(zhuǎn)換模塊的輸出端分別與DSP芯片、以太網(wǎng)控制器、以太網(wǎng)連接器、CAN總線接口芯片連接，DSP芯片分別與以太網(wǎng)控制器、CAN總線接口芯片連接，以太網(wǎng)控制器與以太網(wǎng)連接器連接，CAN總線接口芯片與CAN總線連接器連接。多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件集成在DSP芯片中。其中電源轉(zhuǎn)換模塊包括開關(guān)電源模塊、雙路輸出電壓調(diào)整器、三極管。電源轉(zhuǎn)換模塊的輸入端為24V、COM，輸出端為5V、RESET、1.9V、3.3V。開關(guān)電源模塊的VIN+、VIN-引腳分別與24V、COM輸入端連接，開關(guān)電源模塊的VOUT+引腳分別與雙路輸出電壓調(diào)整器的1IN、2IN引腳連接，開關(guān)電源模塊的VOUT+引腳與5V輸出端連接，開關(guān)電源模塊的VOUT+引腳通過電阻R1與雙路輸出電壓調(diào)整器的1EN引腳連接，開關(guān)電源模塊的VOUT-引腳接地。雙路輸出電壓調(diào)整器的1EN引腳與三極管的集電極連接，三極管的基極通過電阻R5與雙路輸出電壓調(diào)整器的2OUT引腳連接，三極管的發(fā)射極與雙路輸出電壓調(diào)整器的2EN引腳分別接地。雙路輸出電壓調(diào)整器的1RESET、2RESET引腳分別與RESET輸出端連接，雙路輸出電壓調(diào)整器的1OUT引腳與1.9V輸出端連接，雙路輸出電壓調(diào)整器的1FB/SENSE、1OUT引腳間通過電阻R3連接，雙路輸出電壓調(diào)整器的1FB/SENSE引腳通過R4接地，雙路輸出電壓調(diào)整器的2OUT引腳與3.3V輸出端連接，雙路輸出電壓調(diào)整器的2OUT、1RESET引腳通過電阻R2連接。上述電阻R1、R2、R3、R4、R5的阻值分別為5～15KΩ、5～15KΩ、15～25KΩ、25～35KΩ、5～15KΩ。
電源轉(zhuǎn)換模塊的24V、COM輸入端與直流電源連接，電源轉(zhuǎn)換模塊的3.3V輸出端分別與DSP芯片的VDDIO引腳、以太網(wǎng)控制器的VDD引腳、以太網(wǎng)連接器的TX_CT引腳連接。電源轉(zhuǎn)換模塊的RESET、1.9V輸出端分別與DSP芯片的XRS、VDD引腳連接，電源轉(zhuǎn)換模塊的5V輸出端與CAN總線接口芯片的VCC引腳連接。
DSP芯片的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引腳分別與以太網(wǎng)控制器的OSC1、CS、INT、WOL、SI、SO、SCK引腳連接；DSP芯片的CANTX、CANRX引腳分別與CAN總線接口芯片的TXD、RXD引腳連接。
以太網(wǎng)控制器的TPOUT+、TPOUT-、TPIN+、TPIN-引腳分別與以太網(wǎng)連接器的TP_OUT+、TP_OUT-、TP_IN+、TP-IN-引腳連接，以太網(wǎng)連接器的J1、J2、J3、J4、J5、J6、J7、J8輸出端通過雙絞線與上位機(jī)連接。
CAN總線接口芯片的CANH、CANL引腳與CAN總線連接器連接。
本發(fā)明的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件的主流程為首先初始化DSP控制器片內(nèi)各功能模塊，包括設(shè)置系統(tǒng)控制寄存器、初始化中斷向量表、配置CAN控制器模塊、配置SPI控制器模塊。再初始化以太網(wǎng)控制器，包括初始化設(shè)備標(biāo)識符、數(shù)據(jù)傳輸標(biāo)準(zhǔn)、協(xié)議、波特率及其它相關(guān)參數(shù)。然后進(jìn)入判斷是否接收到上位機(jī)指令的循環(huán)，若未接收到指令則繼續(xù)循環(huán)，若接收到指令則首先讀取CAN總線反饋信息，再計(jì)算出機(jī)器人當(dāng)前位姿。然后結(jié)合接收到的上位機(jī)指令進(jìn)行軌跡規(guī)劃，完成軌跡規(guī)劃后，先發(fā)送指令至CAN總線，再發(fā)送反饋信息至以太網(wǎng)，然后判斷是否有以太網(wǎng)接收信息標(biāo)志，有則返回至判斷是否接收到上位機(jī)指令，沒有則返回至讀取CAN總線反饋信息。
軌跡規(guī)劃的流程為首先判斷是否為連續(xù)路徑模式，是連續(xù)路徑模式則先進(jìn)行位置插補(bǔ)計(jì)算，再進(jìn)行姿態(tài)插補(bǔ)計(jì)算，然后計(jì)算插補(bǔ)點(diǎn)目標(biāo)位姿，計(jì)算完畢啟動(dòng)多點(diǎn)路徑模式；不是連續(xù)路徑模式則判斷是否為示教模式，是示教模式則進(jìn)入判斷是否為期望位姿的循環(huán)，不是期望位姿則繼續(xù)循環(huán)，是期望位姿則計(jì)算目標(biāo)點(diǎn)位姿，計(jì)算完畢后啟動(dòng)多點(diǎn)路徑模式；不是示教模式則直接啟動(dòng)多點(diǎn)路徑模式；啟動(dòng)多點(diǎn)路徑模式后，首先進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，再判斷是否運(yùn)動(dòng)學(xué)方程有解，運(yùn)動(dòng)學(xué)方程有解則進(jìn)行可行解、最優(yōu)解計(jì)算，然后進(jìn)行多項(xiàng)式軌跡規(guī)劃，規(guī)劃完畢發(fā)送各自由度運(yùn)動(dòng)控制指令，程序結(jié)束；運(yùn)動(dòng)學(xué)方程無解則提示目標(biāo)點(diǎn)錯(cuò)誤，程序結(jié)束。
由于采用上述技術(shù)方案，本發(fā)明采用DSP芯片作為核心處理器，它是一款高速的數(shù)字信號處理芯片，不僅具有數(shù)字信號處理器的優(yōu)點(diǎn)，同時(shí)又具有微控制器的特點(diǎn)，無論在運(yùn)算精度和數(shù)據(jù)的處理能力上都可以滿足運(yùn)動(dòng)規(guī)劃控制的高實(shí)時(shí)性要求，為完成復(fù)雜的實(shí)時(shí)軌跡規(guī)劃算法提供了可靠的平臺。
本發(fā)明與上位機(jī)通訊采用以太網(wǎng)方式，不但解決了傳統(tǒng)串行通訊方式存在的傳輸速率低、通訊距離短和組網(wǎng)能力差等問題，且具有信息量大、兼容性強(qiáng)、編址靈活方便等優(yōu)點(diǎn)。還可以通過以太網(wǎng)通訊模塊將系統(tǒng)接入局域網(wǎng)或者因特網(wǎng)，從而實(shí)現(xiàn)系統(tǒng)的遠(yuǎn)程控制，提高了系統(tǒng)控制的靈活性和使用價(jià)值。
本發(fā)明與多自由度機(jī)器人的各個(gè)自由度模塊通訊采用CAN總線方式，CAN總線通訊具有多主機(jī)方式、傳輸距離遠(yuǎn)、傳輸速度快、抗干擾能力強(qiáng)、應(yīng)用靈活等諸多優(yōu)點(diǎn)，解決了傳統(tǒng)控制器點(diǎn)對點(diǎn)數(shù)據(jù)傳輸方式中的傳輸速率低、接口復(fù)雜等問題。
本發(fā)明主要由一個(gè)電源模塊和三個(gè)芯片組成，結(jié)構(gòu)簡單、體積小、運(yùn)算能力強(qiáng)、可遠(yuǎn)程操作、具有實(shí)時(shí)規(guī)劃能力和快速數(shù)據(jù)交換能力，可滿足多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制的要求。
本發(fā)明可廣泛用于工業(yè)機(jī)器人、機(jī)械臂、空間機(jī)器人、類人機(jī)器人等具有多個(gè)自由度模塊的機(jī)器人控制系統(tǒng)。


圖1是本發(fā)明的一種電路連接示意圖；圖2是圖1中電源轉(zhuǎn)換模塊1的電路連接示意圖；圖3是本發(fā)明的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件的主流程圖；圖4是圖3中軌跡規(guī)劃的流程圖。
具體實(shí)施例方式下面結(jié)合附圖對本發(fā)明做進(jìn)一步的描述一種多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其硬件部分如圖1所示，包括電源轉(zhuǎn)換模塊1、DSP芯片2、以太網(wǎng)控制器3、以太網(wǎng)連接器(內(nèi)置以太網(wǎng)變壓器)4、CAN總線連接器5、CAN總線接口芯片6。外部直流電源7與電源轉(zhuǎn)換模塊1的輸入端連接，電源轉(zhuǎn)換模塊1的輸出端分別與DSP芯片2、以太網(wǎng)控制器3、以太網(wǎng)連接器4、CAN總線接口芯片6連接，DSP芯片2分別與以太網(wǎng)控制器3、CAN總線接口芯片6連接，以太網(wǎng)控制器3與以太網(wǎng)連接器4連接，CAN總線接口芯片6與CAN總線連接器5連接；多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件集成在DSP芯片2中。在本實(shí)施例中電源轉(zhuǎn)換模塊1如圖2所示，包括開關(guān)電源模塊8、雙路輸出電壓調(diào)整器9、三極管10，電源轉(zhuǎn)換模塊1的輸入端為24V、COM，輸出端為5V、RESET、1.9V、3.3V；開關(guān)電源模塊8的VIN+、VIN-引腳分別與24V、COM輸入端連接，開關(guān)電源模塊8的VOUT+引腳分別與雙路輸出電壓調(diào)整器9的1IN、2IN引腳連接，開關(guān)電源模塊8的VOUT+引腳與5V輸出端連接，開關(guān)電源模塊8的VOUT+引腳通過電阻R1與雙路輸出電壓調(diào)整器9的1EN引腳連接，開關(guān)電源模塊8的VOUT-引腳接地，雙路輸出電壓調(diào)整器9的1EN引腳與三極管10的集電極連接，三極管10的基極通過電阻R5與雙路輸出電壓調(diào)整器9的2OUT引腳連接，三極管10的發(fā)射極與雙路輸出電壓調(diào)整器9的2EN引腳分別接地；雙路輸出電壓調(diào)整器9的1RESET、2RESET引腳分別與RESET輸出端連接，雙路輸出電壓調(diào)整器9的1OUT引腳與1.9V輸出端連接，雙路輸出電壓調(diào)整器9的1FB/SENSE、1OUT引腳通過電阻R3連接，雙路輸出電壓調(diào)整器9的1FB/SENSE引腳通過R4接地，雙路輸出電壓調(diào)整器9的2OUT引腳與3.3V輸出端連接，雙路輸出電壓調(diào)整器9的2OUT、1RESET引腳間通過電阻R2連接；上述的電阻R1、R2、R3、R4、R5的阻值分別為10KΩ、10KΩ、18.2KΩ、30.1KΩ、10KΩ。
電源轉(zhuǎn)換模塊1的24V、COM輸入端與直流電源7連接，電源轉(zhuǎn)換模塊1的3.3V輸出端分別與DSP芯片2的VDDIO引腳、以太網(wǎng)控制器3的VDD引腳、以太網(wǎng)連接器4的TX_CT引腳連接；電源轉(zhuǎn)換模塊1的RESET、1.9V輸出端分別與DSP芯片2的XRS、VDD引腳連接；電源轉(zhuǎn)換模塊1的5V輸出端與CAN總線接口芯片6的VCC引腳連接。
如圖1所示，DSP芯片2的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引腳分別與以太網(wǎng)控制器3的OSC1、CS、INT、WOL、SI、SO、SCK引腳連接；DSP芯片2的CANTX、CANRX引腳分別與CAN總線接口芯片6的TXD、RXD引腳連接。
以太網(wǎng)控制器3的TPOUT+、TPOUT-、TPIN+、TPIN-引腳分別與以太網(wǎng)連接器4的TP_OUT+、TP_OUT-、TP_IN+、TP_IN-引腳連接；以太網(wǎng)連接器4的J1、J2、J3、J4、J5、J6、J7、J8輸出端通過雙絞線與上位機(jī)連接。
CAN總線接口芯片6的CANH、CANL引腳與CAN總線連接器5連接。
本實(shí)施例的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件的主流程如圖3所示首先初始化DSP控制器，再初始化以太網(wǎng)控制器，然后進(jìn)入循環(huán)，判斷是否接收到上位機(jī)指令，若未接收到指令則繼續(xù)循環(huán)，若接收到指令則進(jìn)入下一步；首先讀取CAN總線反饋信息，再計(jì)算出機(jī)器人當(dāng)前位姿，然后結(jié)合接收到的上位機(jī)指令進(jìn)行軌跡規(guī)劃；完成軌跡規(guī)劃后，先發(fā)送指令至CAN總線，再發(fā)送反饋信息至以太網(wǎng)，然后判斷是否有以太網(wǎng)接收信息標(biāo)志，有則返回至判斷是否接收到上位機(jī)指令，沒有則返回至讀取CAN總線反饋信息。
軌跡規(guī)劃的流程如圖4所示首先判斷是否為連續(xù)路徑模式，是連續(xù)路徑模式則先進(jìn)行位置插補(bǔ)計(jì)算，再進(jìn)行姿態(tài)插補(bǔ)計(jì)算，然后計(jì)算插補(bǔ)點(diǎn)目標(biāo)位姿，計(jì)算完畢啟動(dòng)多點(diǎn)路徑模式；不是連續(xù)路徑模式則判斷是否為示教模式，是示教模式則進(jìn)入判斷是否為期望位姿的循環(huán)，不是期望位姿則繼續(xù)循環(huán)，是期望位姿則計(jì)算目標(biāo)點(diǎn)位姿，計(jì)算完畢后啟動(dòng)多點(diǎn)路徑模式；不是示教模式則直接啟動(dòng)多點(diǎn)路徑模式；啟動(dòng)多點(diǎn)路徑模式后，首先進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，再判斷是否運(yùn)動(dòng)學(xué)方程有解，運(yùn)動(dòng)學(xué)方程有解則進(jìn)行可行解、最優(yōu)解計(jì)算，然后進(jìn)行多項(xiàng)式軌跡規(guī)劃，規(guī)劃完畢發(fā)送各自由度運(yùn)動(dòng)控制指令，程序結(jié)束；運(yùn)動(dòng)學(xué)方程無解則提示目標(biāo)點(diǎn)錯(cuò)誤，程序結(jié)束。
本實(shí)施例采用TI公司的DSP芯片TMS320F2812作為核心處理器，它是一款高速的數(shù)字信號處理芯片，不僅具有數(shù)字信號處理的優(yōu)點(diǎn)，同時(shí)又具有微控制器的特點(diǎn)，無論在運(yùn)算精度和數(shù)據(jù)的處理能力上都可以滿足運(yùn)動(dòng)規(guī)劃控制的高實(shí)時(shí)性要求，為完成復(fù)雜的實(shí)時(shí)軌跡規(guī)劃算法提供了可靠的平臺。
本實(shí)施例與上位機(jī)通訊采用以太網(wǎng)方式，以太網(wǎng)控制器芯片采用MicrochipTechnology(美國微芯科技公司)近期推出的28引腳獨(dú)立以太網(wǎng)控制器ENC28J60，它采用標(biāo)準(zhǔn)的SPI串行接口，只需4根導(dǎo)線即可實(shí)現(xiàn)與TMS320F2812連接，有利于在系統(tǒng)上實(shí)現(xiàn)以太網(wǎng)功能。采用以太網(wǎng)通訊方式不但解決了傳統(tǒng)串行通訊方式存在的傳輸速率低、通訊距離短和組網(wǎng)能力差等問題，具有信息量大、兼容性強(qiáng)、編址靈活方便等優(yōu)點(diǎn)，而且可以通過以太網(wǎng)通訊模塊將系統(tǒng)接入局域網(wǎng)或者因特網(wǎng)，從而實(shí)現(xiàn)系統(tǒng)的遠(yuǎn)程控制，提高了系統(tǒng)控制的靈活性和使用價(jià)值。
本實(shí)施例與多自由度機(jī)器人的各個(gè)自由度模塊通訊采用CAN總線方式，CAN總線通訊具有多主機(jī)方式、傳輸距離遠(yuǎn)、傳輸速度快、抗干擾能力強(qiáng)、應(yīng)用靈活等諸多優(yōu)點(diǎn)，解決了傳統(tǒng)控制器點(diǎn)對點(diǎn)數(shù)據(jù)傳輸方式中的傳輸速率低、接口復(fù)雜等問題。
本實(shí)施例主要由一個(gè)電源模塊和三個(gè)芯片組成，結(jié)構(gòu)簡單、體積小、運(yùn)算能力強(qiáng)、可遠(yuǎn)程操作、具有實(shí)時(shí)規(guī)劃能力和快速數(shù)據(jù)交換能力，可滿足多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制的要求。
本裝置可廣泛用于工業(yè)機(jī)器人、機(jī)械臂、空間機(jī)器人、類人機(jī)器人等具有多個(gè)自由度模塊的機(jī)器人控制系統(tǒng)。
權(quán)利要求
1.一種多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于電源轉(zhuǎn)換模塊1的輸入端與外部直流電源7連接，電源轉(zhuǎn)換模塊1的輸出端分別與DSP芯片2、以太網(wǎng)控制器3、以太網(wǎng)連接器4、CAN總線接口芯片6連接，DSP芯片2分別與以太網(wǎng)控制器3、CAN總線接口芯片6連接，以太網(wǎng)控制器3與以太網(wǎng)連接器4連接，CAN總線接口芯片6與CAN總線連接器5連接；多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件集成在DSP芯片2中。
2.根據(jù)權(quán)利要求1所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的電源轉(zhuǎn)換模塊1包括開關(guān)電源模塊8、雙路輸出電壓調(diào)整器9、三極管10，電源轉(zhuǎn)換模塊1的輸入端為24V、COM，輸出端為5V、RESET、1.9V、3.3V；開關(guān)電源模塊8的VIN+、VIN-引腳分別與24V、COM輸入端連接，開關(guān)電源模塊8的VOUT+引腳分別與雙路輸出電壓調(diào)整器9的1IN、2IN引腳連接，開關(guān)電源模塊8的VOUT+引腳與5V輸出端連接，開關(guān)電源模塊8的VOUT+引腳通過電阻R1與雙路輸出電壓調(diào)整器9的1EN引腳連接，開關(guān)電源模塊8的VOUT-引腳接地，雙路輸出電壓調(diào)整器9的1EN引腳與三極管10的集電極連接，三極管10的基極通過電阻R5與雙路輸出電壓調(diào)整器9的2OUT引腳連接，三極管10的發(fā)射極與雙路輸出電壓調(diào)整器9的2EN引腳分別接地；雙路輸出電壓調(diào)整器9的1RESET、2RESET引腳分別與RESET輸出端連接，雙路輸出電壓調(diào)整器9的1OUT引腳與1.9V輸出端連接，雙路輸出電壓調(diào)整器9的1FB/SENSE、1OUT引腳間通過電阻R3連接，雙路輸出電壓調(diào)整器9的1FB/SENSE引腳通過R4接地，雙路輸出電壓調(diào)整器9的2OUT引腳與3.3V輸出端連接，雙路輸出電壓調(diào)整器9的2OUT、1RESET引腳間通過電阻R2連接；電源轉(zhuǎn)換模塊1的24V、COM輸入端與直流電源7連接，電源轉(zhuǎn)換模塊1的3.3V輸出端分別與DSP芯片2的VDDIO引腳、以太網(wǎng)控制器3的VDD引腳、以太網(wǎng)連接器4的TX_CT引腳連接；電源轉(zhuǎn)換模塊1的RESET、1.9V輸出端分別與DSP芯片2的XRS、VDD引腳連接；電源轉(zhuǎn)換模塊1的5V輸出端與CAN總線接口芯片6的VCC引腳連接。
3.根據(jù)權(quán)利要求2所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的電阻R1、R2、R3、R4、R5的阻值分別為5～15KΩ、5～15KΩ、15～25KΩ、25～35KΩ、5～15KΩ。
4.根據(jù)權(quán)利要求1所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的DSP芯片2的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引腳分別與以太網(wǎng)控制器3的OSC1、CS、INT、WOL、SI、SO、SCK引腳連接；DSP芯片2的CANTX、CANRX引腳分別與CAN總線接口芯片6的TXD、RXD引腳連接。
5.根據(jù)權(quán)利要求1所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的以太網(wǎng)控制器3的TPOUT+、TPOUT-、TPIN+、TPIN-引腳分別與以太網(wǎng)連接器4的TP_OUT+、TP_OUT-、TP_IN+、TP_IN-引腳連接；以太網(wǎng)連接器4的J1、J2、J3、J4、J5、J6、J7、J8輸出端通過雙絞線與上位機(jī)連接。
6.根據(jù)權(quán)利要求1所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的CAN總線接口芯片6的CANH、CANL引腳與CAN總線連接器5連接。
7.根據(jù)權(quán)利要求1所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件的主流程為首先初始化DSP控制器，再初始化以太網(wǎng)控制器，然后進(jìn)入循環(huán)，判斷是否接收到上位機(jī)指令，若未接收到指令則繼續(xù)循環(huán)，若接收到指令則進(jìn)入下一步；先讀取CAN總線反饋信息，再計(jì)算機(jī)器人當(dāng)前位姿，然后結(jié)合接收到的上位機(jī)指令進(jìn)行軌跡規(guī)劃；完成軌跡規(guī)劃后，發(fā)送指令至CAN總線，再發(fā)送反饋信息至以太網(wǎng)，然后判斷是否有以太網(wǎng)接收信息標(biāo)志，有則返回至判斷是否接收到上位機(jī)指令，沒有則返回至讀取CAN總線反饋信息。
8.根據(jù)權(quán)利要求7所述的多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器，其特征在于所述的軌跡規(guī)劃的流程為首先判斷是否為連續(xù)路徑模式，是連續(xù)路徑模式則先進(jìn)行位置插補(bǔ)計(jì)算，再進(jìn)行姿態(tài)插補(bǔ)計(jì)算，然后計(jì)算插補(bǔ)點(diǎn)目標(biāo)位姿，計(jì)算完畢啟動(dòng)多點(diǎn)路徑模式；不是連續(xù)路徑模式則判斷是否為示教模式，是示教模式則進(jìn)入判斷是否為期望位姿的循環(huán)，不是期望位姿則繼續(xù)循環(huán)，是期望位姿則計(jì)算目標(biāo)點(diǎn)位姿，計(jì)算完畢后啟動(dòng)多點(diǎn)路徑模式；不是示教模式則直接啟動(dòng)多點(diǎn)路徑模式；啟動(dòng)多點(diǎn)路徑模式后，首先進(jìn)行逆運(yùn)動(dòng)學(xué)計(jì)算，再判斷是否運(yùn)動(dòng)學(xué)方程有解，運(yùn)動(dòng)學(xué)方程有解則進(jìn)行可行解、最優(yōu)解計(jì)算，然后進(jìn)行多項(xiàng)式軌跡規(guī)劃，規(guī)劃完畢發(fā)送各自由度運(yùn)動(dòng)控制指令，程序結(jié)束；運(yùn)動(dòng)學(xué)方程無解則提示目標(biāo)點(diǎn)錯(cuò)誤，程序結(jié)束。
全文摘要
本發(fā)明涉及一種多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制器。其技術(shù)方案是外部電源7與電源轉(zhuǎn)換模塊1的輸入端連接，電源轉(zhuǎn)換模塊1的輸出端分別與DSP芯片2、以太網(wǎng)控制器3、以太網(wǎng)連接器4、CAN總線接口芯片6連接，DSP芯片2分別與以太網(wǎng)控制器3、CAN總線接口芯片6連接，以太網(wǎng)控制器3與以太網(wǎng)連接器4連接，CAN總線接口芯片6與CAN總線連接器5連接；多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制軟件集成在DSP芯片2中。本發(fā)明主要由一個(gè)電源模塊和三個(gè)芯片組成，結(jié)構(gòu)簡單、體積小、運(yùn)算能力強(qiáng)、可遠(yuǎn)程操作、具有實(shí)時(shí)規(guī)劃能力和快速數(shù)據(jù)交換能力，可滿足多自由度機(jī)器人運(yùn)動(dòng)規(guī)劃控制的要求。
文檔編號B25J13/00GK101045298SQ20071005184
公開日2007年10月3日 申請日期2007年4月12日 優(yōu)先權(quán)日2007年4月12日
發(fā)明者吳懷宇, 方恒, 程磊, 許春山, 趙偉, 張巍, 胡琳萍, 劉亮 申請人:武漢科技大學(xué)