一種基于fpga的七自由度力反饋主操作手控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，包括FPGA芯片、USB芯片、磁編碼器脈沖輸出電路、直流電機(jī)驅(qū)動(dòng)電路。其中FPGA包括：磁編碼器脈沖計(jì)數(shù)模塊、正運(yùn)動(dòng)學(xué)計(jì)算模塊、USB Slave FIFO讀寫模塊、初始化及回零控制模塊、直流電機(jī)控制模塊。本發(fā)明利用FPGA可編程邏輯體系結(jié)構(gòu)在芯片接口設(shè)計(jì)、編程及應(yīng)用中的靈活性，以及在并行運(yùn)行體系結(jié)構(gòu)設(shè)計(jì)中的優(yōu)勢(shì)，大大減少集成芯片數(shù)量，降低硬件電路設(shè)計(jì)及調(diào)試難度，工程實(shí)現(xiàn)容易。
【專利說(shuō)明】
一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)
技術(shù)領(lǐng)域
[0001]本發(fā)明涉及一種力反饋主操作手控制系統(tǒng)，并基于FPGA實(shí)現(xiàn)編碼器脈沖計(jì)數(shù)、正運(yùn)動(dòng)學(xué)計(jì)算及直流電機(jī)驅(qū)動(dòng)功能。
【背景技術(shù)】
[0002]力反饋主操作手是遙操作過(guò)程中的關(guān)鍵部件，是從端機(jī)械手進(jìn)行實(shí)時(shí)運(yùn)動(dòng)控制的輸入設(shè)備，又是反映從端機(jī)械手與環(huán)境之間相互作用力的一種力覺輸出設(shè)備。作為人與機(jī)器人之間建立緊密動(dòng)態(tài)耦合的重要接口，它可以向操作系統(tǒng)傳送位姿、速度等多種信息；同時(shí)可以接受控制系統(tǒng)發(fā)來(lái)的力/力矩等環(huán)境信息，以便為操作者提供力覺臨場(chǎng)感，實(shí)現(xiàn)對(duì)機(jī)器人的有效干預(yù)和控制。隨著遙操作機(jī)器人在外科手術(shù)中應(yīng)用的不斷發(fā)展，力反饋主手發(fā)揮著越來(lái)越重要的作用。
[0003]目前商業(yè)化比較成功的IntuitiveSurgical公司的達(dá)芬奇系統(tǒng)的主手為串聯(lián)型結(jié)構(gòu)，有8個(gè)運(yùn)動(dòng)自由度(7個(gè)自由度用來(lái)控制位置和姿態(tài)，I個(gè)自由度用來(lái)控制從手末端夾鉗的開合角度)。該主手運(yùn)動(dòng)靈活，運(yùn)動(dòng)空間大，但是沒有力反饋，醫(yī)生無(wú)法感受器械接觸到組織的物理特性，無(wú)法控制末端器械的力度。
[0004]哈爾濱工業(yè)大學(xué)基于國(guó)家“863”計(jì)劃項(xiàng)目“微創(chuàng)腹腔外科手術(shù)機(jī)器人”設(shè)計(jì)的力反饋主手為串并聯(lián)混合型7自由度結(jié)構(gòu)，并實(shí)現(xiàn)了4自由度力反饋(三維空間力反饋和夾持力反饋)，其控制系統(tǒng)采用傳統(tǒng)的工控主機(jī)+多軸運(yùn)動(dòng)控制卡+伺服驅(qū)動(dòng)器實(shí)現(xiàn)，系統(tǒng)不夠緊湊并且很難保證控制系統(tǒng)實(shí)時(shí)力反饋效果。
[0005]瑞士Force Dimens1n公司利用Delta并聯(lián)機(jī)構(gòu)原理成功開發(fā)出三自由度和六自由度力反饋設(shè)備，其控制系統(tǒng)實(shí)現(xiàn)中將運(yùn)動(dòng)控制和伺服電機(jī)驅(qū)動(dòng)集成到PCB板，使力反饋設(shè)備結(jié)構(gòu)緊湊且有很好的實(shí)時(shí)力反饋效果，但控制系統(tǒng)使用大量IC芯片，使硬件電路結(jié)構(gòu)復(fù)雜，調(diào)試?yán)щy且不便于產(chǎn)品后期擴(kuò)展升級(jí)。

【發(fā)明內(nèi)容】

[0006]本發(fā)明的目的是提供一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)。
[0007]本發(fā)明的目的是通過(guò)這樣的技術(shù)方案實(shí)現(xiàn)的，一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，包括FPGA芯片、USB芯片、磁編碼器脈沖輸出電路和直流電機(jī)驅(qū)動(dòng)電路，所述，所述FPGA芯片包括:磁編碼器脈沖計(jì)數(shù)模塊、正運(yùn)動(dòng)學(xué)計(jì)算模塊、USB Slave FIFO讀寫模塊、初始化及回零控制模塊和直流電機(jī)控制模塊；
[0008]磁編碼器脈沖輸出電路根據(jù)電機(jī)正反轉(zhuǎn)方向輸出A、B兩相相差±90°的脈沖信號(hào)；直流電機(jī)驅(qū)動(dòng)電路根據(jù)輸入的PWM控制信號(hào)輸出大電流電機(jī)驅(qū)動(dòng)信號(hào)，通過(guò)改變PWM控制信號(hào)占空比控制直流電機(jī)轉(zhuǎn)速；
[0009]所述磁編碼器脈沖計(jì)數(shù)模塊對(duì)磁編碼器脈沖輸出電路輸出的脈沖計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)加減脈沖計(jì)數(shù)器值；
[0010]所述正運(yùn)動(dòng)學(xué)計(jì)算模塊根據(jù)脈沖計(jì)數(shù)器值、編碼器單位脈沖對(duì)應(yīng)的電機(jī)旋轉(zhuǎn)弧度、主操作手機(jī)械結(jié)構(gòu)尺寸以及正運(yùn)動(dòng)學(xué)公式計(jì)算出末端點(diǎn)空間位置坐標(biāo)及姿態(tài)值；
[0011]所述USB Slave FIFO讀寫模塊實(shí)現(xiàn)FPGA芯片與USB芯片之間數(shù)據(jù)的傳輸；
[0012]所述初始化及回零控制模塊在控制系統(tǒng)接收到上位機(jī)發(fā)送的回零命令后，控制電機(jī)按一定方向旋轉(zhuǎn)使主操作手回到機(jī)械零點(diǎn)并清零脈沖計(jì)數(shù)器的值；
[0013]所述直流電機(jī)控制模塊輸出上位機(jī)經(jīng)過(guò)逆運(yùn)動(dòng)學(xué)計(jì)算后發(fā)送來(lái)的電機(jī)的PWM控制信號(hào)到直流電機(jī)驅(qū)動(dòng)電路。
[0014]進(jìn)一步，電機(jī)正轉(zhuǎn)時(shí)，A相脈沖上升沿時(shí)B相脈沖為低電平；電機(jī)反轉(zhuǎn)時(shí)，A相脈沖上升沿時(shí)B相脈沖為高電平。
[0015]進(jìn)一步，所述磁編碼器脈沖計(jì)數(shù)模塊對(duì)磁編碼器脈沖輸入電路的A相脈沖上升沿計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)時(shí)B相電平高低加減脈沖計(jì)數(shù)器值。
[0016]由于采用上述技術(shù)方案，本發(fā)明具有如下的優(yōu)點(diǎn):
[0017]本發(fā)明所述的七自由度力反饋主操作手控制系統(tǒng)，采用與目前力反饋主操作手控制系統(tǒng)不一樣的系統(tǒng)結(jié)構(gòu)，提供一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，利用FPGA可編程邏輯體系結(jié)構(gòu)在芯片接口設(shè)計(jì)、編程及應(yīng)用中的靈活性，以及在并行運(yùn)行體系結(jié)構(gòu)設(shè)計(jì)中的優(yōu)勢(shì)，大大減少集成芯片數(shù)量，降低硬件電路設(shè)計(jì)及調(diào)試難度，使控制系統(tǒng)結(jié)構(gòu)緊湊，并能夠滿足力反饋實(shí)時(shí)性效果。
【附圖說(shuō)明】
[0018]為了使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的詳細(xì)描述，其中:
[0019]圖1為基于FPGA的七自由度力反饋主操作手控制系統(tǒng)的結(jié)構(gòu)示意圖；
[0020]圖2為磁編碼器輸出脈沖信號(hào)示意圖；
[0021 ]圖3為直流電機(jī)PffM占空比控制信號(hào)示意圖。
【具體實(shí)施方式】
[0022]為了使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的詳細(xì)描述。
[0023]圖1為本發(fā)明所述的基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，包括FPGA芯片、USB芯片、磁編碼器脈沖輸出電路、直流電機(jī)驅(qū)動(dòng)電路。其中FPGA芯片包括:磁編碼器脈沖計(jì)數(shù)模塊、正運(yùn)動(dòng)學(xué)計(jì)算模塊、USB Slave FIFO讀寫模塊、初始化及回零控制模塊、直流電機(jī)控制模塊。
[0024]磁編碼器脈沖輸出電路根據(jù)電機(jī)正反轉(zhuǎn)方向輸出A、B兩相相差±90°的脈沖信號(hào)；直流電機(jī)驅(qū)動(dòng)電路根據(jù)輸入的PWM控制信號(hào)輸出大電流電機(jī)驅(qū)動(dòng)信號(hào)，通過(guò)改變PWM控制信號(hào)占空比控制直流電機(jī)轉(zhuǎn)速；
[0025]所述磁編碼器脈沖計(jì)數(shù)模塊對(duì)磁編碼器脈沖輸出電路輸出的脈沖計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)加減脈沖計(jì)數(shù)器值；
[0026]所述正運(yùn)動(dòng)學(xué)計(jì)算模塊根據(jù)脈沖計(jì)數(shù)器值、編碼器單位脈沖對(duì)應(yīng)的電機(jī)旋轉(zhuǎn)弧度、主操作手機(jī)械結(jié)構(gòu)尺寸以及正運(yùn)動(dòng)學(xué)公式計(jì)算出末端點(diǎn)空間位置坐標(biāo)及姿態(tài)值；
[0027]所述USB Slave FIFO讀寫模塊實(shí)現(xiàn)FPGA芯片與USB芯片之間數(shù)據(jù)的傳輸；即從USB芯片端點(diǎn)2讀取從上位機(jī)發(fā)送的數(shù)據(jù)，并往USB芯片端點(diǎn)6寫入七路編碼器計(jì)數(shù)器值和末端點(diǎn)空間位置及姿態(tài)值。
[0028]所述初始化及回零控制模塊在控制系統(tǒng)接收到上位機(jī)發(fā)送的回零命令后，控制電機(jī)按一定方向旋轉(zhuǎn)使主操作手回到機(jī)械零點(diǎn)并清零脈沖計(jì)數(shù)器的值；
[0029]所述直流電機(jī)控制模塊輸出上位機(jī)經(jīng)過(guò)逆運(yùn)動(dòng)學(xué)計(jì)算后發(fā)送來(lái)的電機(jī)的PWM控制信號(hào)到直流電機(jī)驅(qū)動(dòng)電路。
[0030]如圖2所示給出了電機(jī)正反轉(zhuǎn)時(shí)磁編碼器輸出的A、B兩相脈沖信號(hào)，信號(hào)相位相差90°，所使用的磁編碼器型號(hào)為Maxon MR編碼器，旋轉(zhuǎn)一周脈沖數(shù)為512個(gè)。正轉(zhuǎn)時(shí)，A相脈沖上升沿時(shí)B相脈沖為低電平;反轉(zhuǎn)時(shí)，A相脈沖上升沿時(shí)B相脈沖為高電平。由于本發(fā)明用于七自由度力反饋主操作手控制系統(tǒng)，因此磁編碼器脈沖計(jì)數(shù)模塊分別對(duì)此七路磁編碼器脈沖輸入電路的A相脈沖上升沿計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)時(shí)B相電平高低加減脈沖計(jì)數(shù)器值。
[0031]USB Slave FIFO讀寫模塊能夠?qū)崿F(xiàn)FPGA芯片與USB芯片之間數(shù)據(jù)的傳輸，數(shù)據(jù)通訊頻率為4KHz，并以時(shí)間片形式實(shí)現(xiàn)數(shù)據(jù)讀寫;上位機(jī)通過(guò)USB芯片發(fā)送7路電機(jī)PffM控制數(shù)據(jù)共16Byte到USB端點(diǎn)2FIF0，F(xiàn)PGA在讀時(shí)間片檢測(cè)到端點(diǎn)2非空標(biāo)志后讀取端點(diǎn)2FIF0中的數(shù)據(jù)并立即更新電機(jī)控制寄存器;上位機(jī)讀取端點(diǎn)6FIF0數(shù)據(jù)后標(biāo)志位會(huì)置空，F(xiàn)PGA在寫時(shí)間片檢測(cè)到端點(diǎn)6空標(biāo)志后往端點(diǎn)6FIF0寫入七路編碼器計(jì)數(shù)器值以及末端點(diǎn)空間位置和姿態(tài)值共32Byte。
[0032]直流電機(jī)控制模塊根據(jù)電機(jī)控制寄存器輸出七個(gè)電機(jī)的PWM控制信號(hào)到直流電機(jī)驅(qū)動(dòng)電路，直流電機(jī)驅(qū)動(dòng)電路使用集成的電機(jī)驅(qū)動(dòng)芯片L6202，根據(jù)輸入的PWM控制信號(hào)能夠輸出最大1.5A的電流信號(hào)，能夠驅(qū)動(dòng)15W直流電機(jī)，通過(guò)改變HVM控制信號(hào)占空比來(lái)控制直流電機(jī)轉(zhuǎn)速，如圖3所示。
[0033]最后說(shuō)明的是，以上實(shí)施例僅用以說(shuō)明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實(shí)施例對(duì)本發(fā)明進(jìn)行了詳細(xì)說(shuō)明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍當(dāng)中。
【主權(quán)項(xiàng)】
1.一種基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，其特征在于:包括FPGA芯片、USB芯片、磁編碼器脈沖輸出電路和直流電機(jī)驅(qū)動(dòng)電路，所述FPGA芯片包括:磁編碼器脈沖計(jì)數(shù)模塊、正運(yùn)動(dòng)學(xué)計(jì)算模塊、USB Slave FIFO讀寫模塊、初始化及回零控制模塊和直流電機(jī)控制模塊； 磁編碼器脈沖輸出電路根據(jù)電機(jī)正反轉(zhuǎn)方向輸出A、B兩相相差±90°的脈沖信號(hào);直流電機(jī)驅(qū)動(dòng)電路根據(jù)輸入的PWM控制信號(hào)輸出大電流電機(jī)驅(qū)動(dòng)信號(hào)，通過(guò)改變PWM控制信號(hào)占空比控制直流電機(jī)轉(zhuǎn)速； 所述磁編碼器脈沖計(jì)數(shù)模塊對(duì)磁編碼器脈沖輸出電路輸出的脈沖計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)加減脈沖計(jì)數(shù)器值； 所述正運(yùn)動(dòng)學(xué)計(jì)算模塊根據(jù)脈沖計(jì)數(shù)器值、編碼器單位脈沖對(duì)應(yīng)的電機(jī)旋轉(zhuǎn)弧度、主操作手機(jī)械結(jié)構(gòu)尺寸以及正運(yùn)動(dòng)學(xué)公式計(jì)算出主操作手末端點(diǎn)空間位置坐標(biāo)及姿態(tài)值；所述USB Slave FIFO讀寫模塊實(shí)現(xiàn)FPGA芯片與USB芯片之間數(shù)據(jù)的傳輸； 所述初始化及回零控制模塊在控制系統(tǒng)接收到上位機(jī)發(fā)送的回零命令后，控制電機(jī)按一定方向旋轉(zhuǎn)使主操作手回到機(jī)械零點(diǎn)并清零脈沖計(jì)數(shù)器的值； 所述直流電機(jī)控制模塊輸出上位機(jī)經(jīng)過(guò)逆運(yùn)動(dòng)學(xué)計(jì)算后發(fā)送來(lái)的電機(jī)的PWM控制信號(hào)到直流電機(jī)驅(qū)動(dòng)電路。2.根據(jù)權(quán)利要求1所述的基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，其特征在于:電機(jī)正轉(zhuǎn)時(shí)4相脈沖上升沿時(shí)B相脈沖為低電平；電機(jī)反轉(zhuǎn)時(shí)4相脈沖上升沿時(shí)B相脈沖為高電平。3.根據(jù)權(quán)利要求2所述的基于FPGA的七自由度力反饋主操作手控制系統(tǒng)，其特征在于:所述磁編碼器脈沖計(jì)數(shù)模塊對(duì)磁編碼器脈沖輸入電路的A相脈沖上升沿計(jì)數(shù)，并根據(jù)電機(jī)正反轉(zhuǎn)時(shí)B相電平高低加減脈沖計(jì)數(shù)器值。
【文檔編號(hào)】B25J9/16GK106003051SQ201610589812
【公開日】2016年10月12日
【申請(qǐng)日】2016年7月25日
【發(fā)明人】熊亮, 李耀
【申請(qǐng)人】重慶中科博恩思醫(yī)療機(jī)器人有限公司