一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法
【專利摘要】本發(fā)明公開了一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法，通過主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置，經(jīng)過實(shí)驗(yàn)測試，配藥機(jī)器人在各種工況下都能夠?qū)崿F(xiàn)自動回零，且不會發(fā)生碰撞，很好地解決了配藥機(jī)器人在回零過程中的運(yùn)動干涉問題，并且縮短了機(jī)器人回零時間，提高了機(jī)器人的工作效率，并且可以很好地滿足機(jī)械臂之間協(xié)同工作的需求。
【專利說明】一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及醫(yī)用設(shè)備【技術(shù)領(lǐng)域】，特別涉及一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法。
【背景技術(shù)】
[0002]隨著機(jī)器人應(yīng)用領(lǐng)域的不斷擴(kuò)展，機(jī)器人工作環(huán)境越來越復(fù)雜，面臨的作業(yè)任務(wù)難度也越來越大，單臂機(jī)器人已經(jīng)不能滿足某些高難度任務(wù)需求，需要雙臂甚至多臂協(xié)同操作才能完成。
[0003]在醫(yī)院藥液配制過程中，對于醫(yī)用藥瓶(主要為安瓿瓶、西林瓶)的操作，如果采用配藥機(jī)器人進(jìn)行操作，配藥機(jī)器人上電后，需要進(jìn)行回零操作，將機(jī)器人各個軸回到初始位置。
[0004]請參閱圖1，其為配藥機(jī)器人的機(jī)械結(jié)構(gòu)示意圖，如圖所示，配藥機(jī)器人包括機(jī)械臂1、機(jī)械臂I1、藥瓶開啟裝置和藥品架等，其中機(jī)械臂I包含自由度I (也稱第一自由度)、自由度2 (也稱第二自由度)、手爪I (也稱第一手爪)及手爪2 (也稱第二手爪)，手爪I固定在絲杠I導(dǎo)軌末端；機(jī)械臂II包含自由度3 (也稱第三自由度)、自由度4 (也稱第四自由度)、自由度5 (也稱第五自由度)及手爪3 (也稱第三手爪)。配藥機(jī)器人上電后，需要先進(jìn)行回零操作，使機(jī)器人的各個軸回到初始位置。
[0005]現(xiàn)有的配藥機(jī)器人大多采用位置傳感器，通過設(shè)置回零開關(guān)來實(shí)現(xiàn)配藥機(jī)器人各軸的回零操作，但隨著配藥機(jī)器人機(jī)械結(jié)構(gòu)日趨復(fù)雜化，各關(guān)節(jié)間容易出現(xiàn)運(yùn)動干涉(特別是在按下急停開關(guān)及出現(xiàn)故障并修復(fù)后)，致使機(jī)器人發(fā)生碰撞損壞。作為一種雙臂機(jī)器人，配藥機(jī)器人在后續(xù)的作業(yè)過程中，需要雙臂協(xié)同工作才能有效完成任務(wù)，如果協(xié)同控制不當(dāng)，也易產(chǎn)生相互碰撞。
[0006]有鑒于此，現(xiàn)有技術(shù)還有待改進(jìn)和提高。

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

[0007]鑒于上述現(xiàn)有技術(shù)的不足之處，本發(fā)明的目的在于提供一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法，以解決現(xiàn)有技術(shù)中配藥機(jī)器人采用位置傳感器進(jìn)行回零操作，容易導(dǎo)致配藥機(jī)器人發(fā)生碰撞損壞的問題。
[0008]為了達(dá)到上述目的，本發(fā)明采取了以下技術(shù)方案:
一種配藥機(jī)器人的回零控制系統(tǒng)，其中，包括:主控制器、若干個可編程運(yùn)動控制卡、伺服驅(qū)動器及中繼端子板；
其中，所述主控制器通過PCI總線與可編程運(yùn)動控制卡通信，所述可編程運(yùn)動控制卡通過中繼端子板與各伺服驅(qū)動器連接，每一伺服驅(qū)動器均配備有電池；
主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置； 同時，配備有電池的伺服驅(qū)動器還用于在斷電后保存、記錄與伺服驅(qū)動器連接的電機(jī)的位置信息。
[0009]所述的配藥機(jī)器人的回零控制系統(tǒng)，其中，所述主控制器為工業(yè)控制計算機(jī)。
[0010]所述的配藥機(jī)器人的回零控制系統(tǒng)，其中，所述可編程運(yùn)動控制卡采用運(yùn)動控制卡與數(shù)據(jù)采集卡替代。
[0011]所述的配藥機(jī)器人的回零控制系統(tǒng)，其中，所述可編程運(yùn)動控制卡包括依次連接的脈沖發(fā)送模塊、編碼反饋模塊及開關(guān)量輸入/輸出模塊。
[0012]一種上述的配藥機(jī)器人的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其中，所述方法包括:
Ml、預(yù)先根據(jù)用戶指令設(shè)定為絕對位置控制模式，并設(shè)定好第一、第二、第三、第四和第五自由度的零點(diǎn)位置；且絲杠傳動中，靠近電機(jī)的運(yùn)動方向?yàn)檎?，遠(yuǎn)離電機(jī)的運(yùn)動方向?yàn)樨?fù)；
M2、配藥機(jī)器人通電，初始化后，獲取與第一、第二、第三、第四和第五自由度的分別對應(yīng)的位置數(shù)據(jù):第一位置數(shù)據(jù)S1、第二位置數(shù)據(jù)S2、第三位置數(shù)據(jù)S3、第四位置數(shù)據(jù)S4和第五位置數(shù)據(jù)S5 ；
M3、獲取藥瓶開啟裝置上頂面與第五自由度的零點(diǎn)位置的距離Hl后，比較Hl與第S5的大小:若S5大于H1，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；同時，第三自由度運(yùn)動到S位置處，所述S位置為第三手爪的中心點(diǎn)與第三自由度的零點(diǎn)的距離，然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置；
M4、若S5大于H1，則獲取第一手爪的下底面與第五自由度的零點(diǎn)位置的距離H2 ;并比較H2與S5的大小，若S5大于-H2，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置；
M5、若S5小于-H2，則第一自由度、第二自由度、第三自由度、第四自由度和第五自由度依次回到零點(diǎn)位置。
[0013]所述的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其中，所述第一自由度為單圈旋轉(zhuǎn)運(yùn)動，當(dāng)?shù)谝皇肿ι稀⑾旅嫠讲⑶业诙杂啥忍幱谙路綍r，即為第一自由度的零點(diǎn)位置。
[0014]所述的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其中，所述第四自由度為單圈旋轉(zhuǎn)運(yùn)動，第三手爪上、下面水平并且第三手爪的手指在上方時為第四自由度的零點(diǎn)位置。
[0015]相較于現(xiàn)有技術(shù)，本發(fā)明提供的配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法，通過主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置，經(jīng)過實(shí)驗(yàn)測試，配藥機(jī)器人在各種工況下都能夠?qū)崿F(xiàn)自動回零，且不會發(fā)生碰撞，很好地解決了配藥機(jī)器人在回零過程中的運(yùn)動干涉問題，并且縮短了機(jī)器人回零時間，提高了機(jī)器人的工作效率，并且可以很好地滿足機(jī)械臂之間協(xié)同工作的需求。
【專利附圖】

【附圖說明】
[0016]圖1為配藥機(jī)器人的機(jī)械結(jié)構(gòu)的示意圖。
[0017]圖2為本發(fā)明的配藥機(jī)器人的回零控制系統(tǒng)的結(jié)構(gòu)框圖。[0018]圖3為設(shè)置有本發(fā)明的配藥機(jī)器人的回零控制系統(tǒng)的配藥機(jī)器人的零點(diǎn)位置及尺寸標(biāo)定的示意圖。
[0019]圖4為本發(fā)明的配藥機(jī)器人的回零控制的流程圖。
【具體實(shí)施方式】
[0020]本發(fā)明提供一種配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法，為使本發(fā)明的目的、技術(shù)方案及效果更加清楚、明確，以下參照附圖并舉實(shí)施例對本發(fā)明進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅用以解釋本發(fā)明，并不用于限定本發(fā)明。
[0021]請參閱圖2，其為本發(fā)明的配藥機(jī)器人的回零控制系統(tǒng)，如圖所示，其包括:主控制器、若干個可編程運(yùn)動控制卡(在本實(shí)施例中，所述可編程運(yùn)動控制卡的個數(shù)為兩個:可編程運(yùn)動控制卡I和可編程運(yùn)動控制卡2)、伺服驅(qū)動器及中繼端子板；其中，所述主控制器通過PCI總線與可編程運(yùn)動控制卡通信，所述可編程運(yùn)動控制卡通過中繼端子板與各伺服驅(qū)動器連接，每一伺服驅(qū)動器均配備有電池(如圖所示:伺服驅(qū)動器1、伺服驅(qū)動器2到伺服驅(qū)動器N)。
[0022]具體來說，主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置。在本實(shí)施中，所述主控制器為工業(yè)控制計算機(jī)；可編程運(yùn)動控制卡包含脈沖發(fā)送模塊、編碼反饋模塊及開關(guān)量輸入/輸出模塊，并且可通過其提供的庫函數(shù)讀取開關(guān)量輸入/輸出點(diǎn)的電平狀態(tài)；伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定可將自由度當(dāng)前的位置數(shù)據(jù)保存在驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置，若配備電池，機(jī)器人斷電后，亦可保存、記錄電機(jī)的位置信息。
[0023]經(jīng)過實(shí)驗(yàn)測試，配藥機(jī)器人在各種工況下都能夠?qū)崿F(xiàn)自動回零，且不會發(fā)生碰撞，很好地解決了配藥機(jī)器人在回零過程中的運(yùn)動干涉問題。
[0024]請繼續(xù)參閱圖3，圖3的結(jié)構(gòu)配藥機(jī)器人正視簡化圖，圖中對機(jī)器人各個自由度零點(diǎn)位置和一些關(guān)鍵尺寸進(jìn)行標(biāo)定。配藥機(jī)器人機(jī)械結(jié)構(gòu)及控制系統(tǒng)硬件平臺搭建好之后，已經(jīng)將各自由度控制模式設(shè)定為絕對位置控制模式，并且通過零點(diǎn)設(shè)定功能設(shè)定好各自由度的零點(diǎn)位置。在本實(shí)施例中，自由度I (即第一自由度，下同)為單圈旋轉(zhuǎn)運(yùn)動，當(dāng)手爪I上、下面水平并且自由度2處于下方時，即為自由度I的原點(diǎn)位置；自由度2、自由度3及自由度5的零點(diǎn)位置如圖3所示；自由度4為單圈旋轉(zhuǎn)運(yùn)動，手爪3上、下面水平并且手爪的手指在上方時(如圖所示)為自由度4的零點(diǎn)位置。藥瓶開啟裝置上頂面與零點(diǎn)5的距離為H1，手爪I的下底面與零點(diǎn)5的距離為H2，為保證手爪3不與藥瓶開啟裝置發(fā)生碰撞，設(shè)定手爪3中心點(diǎn)與零點(diǎn)3的安全距離為S。31、52、53、54和55依次為自由度1、自由度2、自由度3、自由度4、自由度5的位置數(shù)據(jù)。絲杠傳動中，靠近電機(jī)的運(yùn)動方向?yàn)檎?，遠(yuǎn)離電機(jī)的運(yùn)動方向?yàn)樨?fù)。
[0025]上述的配藥機(jī)器人的回零控制系統(tǒng)的實(shí)現(xiàn)方法如下:
首先，預(yù)先根據(jù)用戶指令主設(shè)定為絕對位置控制模式，并設(shè)定好第一、第二、第三、第四和第五自由度的零點(diǎn)位置；且絲杠傳動中，靠近電機(jī)的運(yùn)動方向?yàn)檎?，遠(yuǎn)離電機(jī)的運(yùn)動方向?yàn)樨?fù)；
然后，配藥機(jī)器人通電，初始化后，獲取與第一、第二、第三、第四和第五自由度的分別對應(yīng)的位置數(shù)據(jù):第一位置數(shù)據(jù)S1、第二位置數(shù)據(jù)S2、第三位置數(shù)據(jù)S3、第四位置數(shù)據(jù)S4和第五位置數(shù)據(jù)S5 ；
獲取藥瓶開啟裝置上頂面與第五自由度的零點(diǎn)位置的距離Hl后，比較Hl與第S5的大小:若S5大于H1，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；同時，第三自由度運(yùn)動到S位置處，所述S位置為第三手爪的中心點(diǎn)與第三自由度的零點(diǎn)的距離，然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置；
若S5大于H1，則獲取第一手爪的下底面與第五自由度的零點(diǎn)位置的距離H2 ;并比較H2與S5的大小，若S5大于-H2，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置；
若S5小于-H2，則第一自由度、第二自由度、第三自由度、第四自由度和第五自由度依次回到零點(diǎn)位置。
[0026]概括來說，如圖4所示，具體實(shí)施方案如下所述:
步驟1:配藥機(jī)器人上電后，初始化系統(tǒng)。
[0027]步驟2:獲取自由度1、自由度2、自由度3、自由度4、自由度5的位置數(shù)據(jù)S1、S2、S3、S4 和 S5。
[0028]步驟3:若S5>H1，則自由度I運(yùn)動致絲杠I處于水平位置，并且自由度2處于外側(cè)；自由度3運(yùn)動到S位置處；然后自由度5、自由度3、自由度4、自由度1、自由度2依次回到零點(diǎn)位置。否則，則執(zhí)行步驟4。
[0029]步驟4:若S5>- H2,則自由度I運(yùn)動致絲杠I處理水平位置，并且自由度2處于外側(cè)；自由度3、自由度4、自由度5、自由度1、自由度2依次回到時零點(diǎn)位置。否則，則執(zhí)行步驟5。
[0030]步驟5:自由度1、自由度2、自由度3、自由度4、自由度5依次回到零點(diǎn)位置。
[0031]步驟6:結(jié)束。
[0032]本發(fā)明可以輕易地避免配藥機(jī)器人在回零點(diǎn)過程中出現(xiàn)的運(yùn)動干涉問題，避免機(jī)器人發(fā)生碰撞，并且縮短了機(jī)器人回零時間，提高了機(jī)器人的工作效率，并且可以很好地滿足機(jī)械臂之間協(xié)同工作的需求。
[0033]進(jìn)一步地，本發(fā)明的配藥機(jī)器人的回零控制系統(tǒng)所用到的主控制器除了工業(yè)控制計算機(jī)，也可以用其他控制器，如普通臺式機(jī)等；發(fā)明中的可編程運(yùn)動控制卡也可以用運(yùn)動控制卡與數(shù)據(jù)采集卡替代等；發(fā)明中的伺服驅(qū)動器也可以為其他可配備電池的伺服驅(qū)動器。本發(fā)明還可以用在一些上電后，條件不允許進(jìn)行回零操作的多關(guān)節(jié)串聯(lián)機(jī)器人上，如點(diǎn)焊機(jī)器人、弧焊機(jī)器人等，這里就不一一展開描述了。
[0034]本發(fā)明的配藥機(jī)器人的回零控制系統(tǒng)及其實(shí)現(xiàn)方法，通過主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置，經(jīng)過實(shí)驗(yàn)測試，配藥機(jī)器人在各種工況下都能夠?qū)崿F(xiàn)自動回零，且不會發(fā)生碰撞，很好地解決了配藥機(jī)器人在回零過程中的運(yùn)動干涉問題，并且縮短了機(jī)器人回零時間，提高了機(jī)器人的工作效率，并且可以很好地滿足機(jī)械臂之間協(xié)同工作的需求。[0035]可以理解的是，對本領(lǐng)域普通技術(shù)人員來說，可以根據(jù)本發(fā)明的技術(shù)方案及其發(fā)明構(gòu)思加以等同替換或改變，而所有這些改變或替換都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護(hù)范圍。
【權(quán)利要求】
1.一種配藥機(jī)器人的回零控制系統(tǒng)，其特征在于，包括:主控制器、若干個可編程運(yùn)動控制卡、伺服驅(qū)動器及中繼端子板； 其中，所述主控制器通過PCI總線與可編程運(yùn)動控制卡通信，所述可編程運(yùn)動控制卡通過中繼端子板與各伺服驅(qū)動器連接，每一伺服驅(qū)動器均配備有電池； 主控制器根據(jù)用戶指令將各自由度控制模式設(shè)定為絕對位置控制模式，伺服驅(qū)動器在絕對位置控制模式下，通過零點(diǎn)設(shè)定將自由度當(dāng)前的位置數(shù)據(jù)保存在伺服驅(qū)動器的EEP-ROM存儲器中作為零點(diǎn)位置； 同時，配備有電池的伺服驅(qū)動器還用于在斷電后保存、記錄與伺服驅(qū)動器連接的電機(jī)的位置信息。
2.根據(jù)權(quán)利要求1所述的配藥機(jī)器人的回零控制系統(tǒng)，其特征在于，所述主控制器為工業(yè)控制計算機(jī)。
3.根據(jù)權(quán)利要求1所述的配藥機(jī)器人的回零控制系統(tǒng)，其特征在于，所述可編程運(yùn)動控制卡采用運(yùn)動控制卡與數(shù)據(jù)采集卡替代。
4.根據(jù)權(quán)利要求1所述的配藥機(jī)器人的回零控制系統(tǒng)，其特征在于，所述可編程運(yùn)動控制卡包括依次連接的脈沖發(fā)送模塊、編碼反饋模塊及開關(guān)量輸入/輸出模塊。
5.一種采用權(quán)利要求1所述的配藥機(jī)器人的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其特征在于，所述方法包括: Ml、預(yù)先根據(jù)用戶指令主設(shè)定為絕對位置控制模式，并設(shè)定好第一、第二、第三、第四和第五自由度的零點(diǎn)位置；且絲杠傳動中，靠近電機(jī)的運(yùn)動方向?yàn)檎?，遠(yuǎn)離電機(jī)的運(yùn)動方向?yàn)樨?fù)； M2、配藥機(jī)器人通電，初始化后，獲取與第一、第二、第三、第四和第五自由度的分別對應(yīng)的位置數(shù)據(jù):第一位置數(shù)據(jù)S1、第二位置數(shù)據(jù)S2、第三位置數(shù)據(jù)S3、第四位置數(shù)據(jù)S4和第五位置數(shù)據(jù)S5 ； M3、獲取藥瓶開啟裝置上頂面與第五自由度的零點(diǎn)位置的距離Hl后，比較Hl與第S5的大小:若S5大于H1，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；同時，第三自由度運(yùn)動到S位置處，所述S位置為第三手爪的中心點(diǎn)與第三自由度的零點(diǎn)的距離，然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置； M4、若S5大于H1，則獲取第一手爪的下底面與第五自由度的零點(diǎn)位置的距離H2 ;并比較H2與S5的大小，若S5大于-H2，則第一自由度運(yùn)動致第一絲杠處于水平位置，并且第二自由度處于外側(cè)；然后，第五自由度、第三自由度、第四自由度、第一自由度和第二自由度依次回到零點(diǎn)位置； M5、若S5小于-H2，則第一自由度、第二自由度、第三自由度、第四自由度和第五自由度依次回到零點(diǎn)位置。
6.根據(jù)權(quán)利要求5所述的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其特征在于，所述第一自由度為單圈旋轉(zhuǎn)運(yùn)動，當(dāng)?shù)谝皇肿ι稀⑾旅嫠讲⑶业诙杂啥忍幱谙路綍r，即為第一自由度的零點(diǎn)位置。
7.根據(jù)權(quán)利要求5所述的回零控制系統(tǒng)的實(shí)現(xiàn)方法，其特征在于，所述第四自由度為單圈旋轉(zhuǎn)運(yùn)動，第三手爪上、下面水平并且第三手爪的手指在上方時為第四自由度的零點(diǎn)位置。
【文檔編號】G01D1/10GK103543677SQ201310464304
【公開日】2014年1月29日 申請日期:2013年9月30日 優(yōu)先權(quán)日:2013年9月30日
【發(fā)明者】肖德明, 何玉成, 胡穎, 雷鳴, 張宏, 曾暉, 李偉, 丁小容, 段麗芳 申請人:北京大學(xué)深圳醫(yī)院