低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)及定位方法
【專利摘要】低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)及定位方法，涉及多個(gè)傳感器融合的原點(diǎn)精確定位技術(shù)。它是為了解決現(xiàn)有原點(diǎn)定位精度設(shè)備中如采用高精度傳感器存在成本高，裝配難度大，而如果采用低精度傳感器雖然能克服前述問題，但定位精度又不夠的矛盾。本發(fā)明在采用了低精度傳感器降低裝配難度和設(shè)備成本的同時(shí)，保證了定位精度；采用了位移傳感器和原點(diǎn)傳感器配合實(shí)現(xiàn)定位，在達(dá)到了采用高精度傳感器相同的定位精度的前提下，降低了對(duì)傳感器安裝精度的要求，進(jìn)而降低了傳感器的裝配難度，裝配難度同比降低了30%以上，設(shè)備成本降低了20%。本發(fā)明適用于多傳感器融合的原點(diǎn)精確定位技術(shù)。
【專利說明】低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)及定位方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及多傳感器融合的原點(diǎn)精確定位技術(shù)。
【背景技術(shù)】
[0002]隨著工業(yè)生產(chǎn)中自動(dòng)化設(shè)備的日益普及，對(duì)設(shè)備運(yùn)動(dòng)控制精度的要求也越來越高，而設(shè)備原點(diǎn)定位精度對(duì)設(shè)備運(yùn)動(dòng)控制精度影響甚大?，F(xiàn)行工業(yè)設(shè)備中，原點(diǎn)定位精度低往往是由于所采用的傳感器的精度低導(dǎo)致的，為了提高精度，往往采用依靠高精度的感應(yīng)式傳感器來實(shí)現(xiàn)，如SONY公司生產(chǎn)的的PK15，其定位精度能夠達(dá)到3um，但隨之而來的是設(shè)備研發(fā)制造成本的大幅增加，以及由于高裝配精度帶來的裝配難度。

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

[0003]本發(fā)明是為了解決現(xiàn)有原點(diǎn)定位精度設(shè)備中如采用高精度傳感器存在成本高，裝配難度大，而如果采用低精度傳感器雖然能克服前述問題，但定位精度又不夠的矛盾，進(jìn)而提供了一種低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)及定位方法。
[0004]低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)，它包括工控機(jī)1、DSP2、位置脈沖減計(jì)數(shù)器3、電機(jī)驅(qū)動(dòng)器4、電機(jī)5、移動(dòng)機(jī)構(gòu)、位移傳感器6、位移計(jì)數(shù)器7和原點(diǎn)傳感器8 ；
[0005]工控機(jī)I的位置指令信號(hào)輸出端連接DSP2的位置指令信號(hào)輸入端，DSP2將要發(fā)送的脈沖總數(shù)寫入位置脈沖減計(jì)數(shù)器3，所述DSP2的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器3的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器3的狀態(tài)信號(hào)輸出端連接DSP2的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器3用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器4的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器3的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器4的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器4的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)5的驅(qū)動(dòng)信號(hào)輸入端,所述電機(jī)5用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器6用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器6輸出表位移信息的脈沖信號(hào)給位移計(jì)數(shù)器7的脈沖信號(hào)輸入端，原點(diǎn)傳感器8用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器8的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器7的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器7的脈沖計(jì)數(shù)輸出端連接DSP2的位置脈沖信號(hào)輸入端；
[0006]在DSP2內(nèi)部的軟件實(shí)現(xiàn)的定位模塊，所述定位模塊包括以下單元:
[0007]初始化單元:用于啟動(dòng)電機(jī)5，電機(jī)5的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器7清零，位置脈沖減計(jì)數(shù)器3的脈沖數(shù)為maxval，maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器脈沖數(shù)；
[0008]判斷單元一:用于判斷移動(dòng)機(jī)構(gòu)是否到達(dá)原點(diǎn)傳感器8的感應(yīng)范圍且接收到位移傳感器6的Z相信號(hào)，如果是則啟動(dòng)關(guān)閉脈沖指令單元，否則重新啟動(dòng)該單元；
[0009]關(guān)閉脈沖指令單元:用于讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器7的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP2中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)5的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷單元二 ；否則重復(fù)啟動(dòng)該單元；[0010]判斷單元二:用于判斷電機(jī)5是否在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向沿軸的正方向，如果是則結(jié)束原點(diǎn)定位；如果否則啟動(dòng)校正單兀；
[0011]校正單元:到位標(biāo)志計(jì)數(shù)器清零，DSP2向位置脈沖減計(jì)數(shù)器3寫入正整數(shù)rglapls，設(shè)定電機(jī)5的運(yùn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸的正方向，然后啟動(dòng)電機(jī)5，然后重復(fù)查詢位置脈沖減計(jì)數(shù)器3的狀態(tài)，待位置脈沖減計(jì)數(shù)器3為O時(shí)，啟動(dòng)關(guān)閉脈沖指令單元。
[0012]低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，該方法是低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)實(shí)現(xiàn)的，所述低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)包括工控機(jī)1、DSP2、位置脈沖減計(jì)數(shù)器3、電機(jī)驅(qū)動(dòng)器4、電機(jī)5、移動(dòng)機(jī)構(gòu)、位移傳感器6、位移計(jì)數(shù)器7和原點(diǎn)傳感器8 ；
[0013]工控機(jī)I的位置指令信號(hào)輸出端連接DSP2的位置指令信號(hào)輸入端，DSP2將要發(fā)送的脈沖總數(shù)寫位置脈沖減計(jì)數(shù)器3，所述DSP2的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器3的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器3的狀態(tài)信號(hào)輸出端連接DSP2的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器3用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器4的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器3的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器4的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器4的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)5的驅(qū)動(dòng)信號(hào)輸入端,所述電機(jī)5用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器6用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器6輸出表位移信息的脈沖信號(hào)給位移計(jì)數(shù)器7的脈沖信號(hào)輸入端，原點(diǎn)傳感器8用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器8的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器7的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器7的脈沖計(jì)數(shù)輸出端連接DSP2的位置脈沖信號(hào)輸入端；
[0014]根據(jù)運(yùn)動(dòng)設(shè)備建立設(shè)備坐標(biāo)系，所述的原點(diǎn)精確定位方法是由嵌入在DSP2內(nèi)部的軟件實(shí)現(xiàn)的，該方法包括以下步驟:
[0015]初始化步驟:用于啟動(dòng)電機(jī)5,電機(jī)5的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器7清零，位置脈沖減計(jì)數(shù)器3的脈沖數(shù)為maxval，maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器6脈沖數(shù)，在該步驟結(jié)束后執(zhí)行判斷步驟一；
[0016]判斷步驟一:移動(dòng)機(jī)構(gòu)到達(dá)原點(diǎn)傳感器8的感應(yīng)范圍且接收到位移傳感器6的Z相信號(hào)，則執(zhí)行關(guān)閉脈沖指令步驟，否則重新執(zhí)行該步驟；
[0017]關(guān)閉脈沖指令步驟:讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器7的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP2中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)5的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷步驟二 ；否則重復(fù)執(zhí)行該步驟；
[0018]判斷步驟二:判斷電機(jī)5在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向，當(dāng)運(yùn)動(dòng)方向沿軸的正方向則結(jié)束原點(diǎn)定位；當(dāng)運(yùn)動(dòng)方向沿軸的負(fù)方向則執(zhí)行校正步驟；
[0019]校正步驟:到位標(biāo)志計(jì)數(shù)器清零，DSP2向位置脈沖減計(jì)數(shù)器3寫入正整數(shù)rglapls，設(shè)定電機(jī)5的運(yùn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸的正方向，然后啟動(dòng)電機(jī)5，然后重復(fù)查詢位置脈沖減計(jì)數(shù)器3的狀態(tài)，待位置脈沖減計(jì)數(shù)器3為O時(shí)，執(zhí)行步驟關(guān)閉脈沖指令步驟。
[0020]本發(fā)明的有益效果是:本發(fā)明在采用了低精度傳感器降低裝配難度和設(shè)備成本的同時(shí)，保證了定位精度。
[0021]本發(fā)明所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法中采用了位移傳感器和原點(diǎn)傳感器配合實(shí)現(xiàn)定位，其中原點(diǎn)傳感器用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，位移傳感器用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息，通過對(duì)兩個(gè)傳感器采集的數(shù)據(jù)進(jìn)行比較和計(jì)算進(jìn)而實(shí)現(xiàn)精確定位。
[0022]本發(fā)明所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)采用多個(gè)低精度傳感器實(shí)現(xiàn)聯(lián)合定位，在達(dá)到了采用高精度傳感器相同的定位精度的前提下，降低了對(duì)傳感器安裝精度的要求，進(jìn)而降低了傳感器的裝配難度，裝配難度同比降低了 30%以上，設(shè)備成本降低了 20%。
【專利附圖】

【附圖說明】
[0023]圖1為低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)的整體結(jié)構(gòu)圖；
[0024]圖2為低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法的流程圖。
【具體實(shí)施方式】
[0025]【具體實(shí)施方式】一:下面結(jié)合圖1說明本實(shí)施方式，本實(shí)施方式所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)，它包括工控機(jī)1、DSP2、位置脈沖減計(jì)數(shù)器3、電機(jī)驅(qū)動(dòng)器
4、電機(jī)5、移動(dòng)機(jī)構(gòu)、位移傳感器6、位移計(jì)數(shù)器7和原點(diǎn)傳感器8 ；
[0026]工控機(jī)I的位置指令信號(hào)輸出端連接DSP2的位置指令信號(hào)輸入端，DSP2將要發(fā)送的脈沖總數(shù)寫位置脈沖減計(jì)數(shù)器3，所述DSP2的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器3的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器3的狀態(tài)信號(hào)輸出端連接DSP2的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器3用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器4的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器3的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器4的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器4的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)5的驅(qū)動(dòng)信號(hào)輸入端,所述電機(jī)5用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器6用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器6輸出表位移信息的脈沖信號(hào)給位移計(jì)數(shù)器7的脈沖信號(hào)輸入端，原點(diǎn)傳感器8用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器8的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器7的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器7的脈沖計(jì)數(shù)輸出端連接DSP2的位置脈沖信號(hào)輸入端；
[0027]在DSP2內(nèi)部的軟件實(shí)現(xiàn)的定位模塊，所述定位模塊包括以下單元:
[0028]初始化單元:用于啟動(dòng)電機(jī)5，電機(jī)5的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器7清零，位置脈沖減計(jì)數(shù)器3的脈沖數(shù)為maxval，maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器脈沖數(shù)；
[0029]判斷單元一:用于判斷移動(dòng)機(jī)構(gòu)是否到達(dá)原點(diǎn)傳感器8的感應(yīng)范圍且接收到位移傳感器6的Z相信號(hào)，如果是則啟動(dòng)關(guān)閉脈沖指令單元，否則重新啟動(dòng)該單元；
[0030]關(guān)閉脈沖指令單元:用于讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器7的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP2中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)5的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷單元二 ；否則重復(fù)啟動(dòng)該單元；
[0031]判斷單元二:用于判斷電機(jī)5是否在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向沿軸的正方向，如果是則結(jié)束原點(diǎn)定位；如果否則啟動(dòng)校正單兀；
[0032]校正單元:到位標(biāo)志計(jì)數(shù)器清零，DSP2向位置脈沖減計(jì)數(shù)器3寫入正整數(shù)rglapls，設(shè)定電機(jī)5的運(yùn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸的正方向，然后啟動(dòng)電機(jī)5，然后重復(fù)查詢位置脈沖減計(jì)數(shù)器3的狀態(tài)，待位置脈沖減計(jì)數(shù)器3為O時(shí)，啟動(dòng)關(guān)閉脈沖指令單元。
[0033]【具體實(shí)施方式】二:本實(shí)施方式對(duì)【具體實(shí)施方式】一所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)作進(jìn)一步限定，本實(shí)施方式中，位移傳感器6采用磁柵尺或光柵尺實(shí)現(xiàn)。
[0034]【具體實(shí)施方式】三:下面結(jié)合圖1和圖2說明本實(shí)施方式，本實(shí)施方式所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，該方法是低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)實(shí)現(xiàn)的，所述低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)包括工控機(jī)1、DSP2、位置脈沖減計(jì)數(shù)器3、電機(jī)驅(qū)動(dòng)器4、電機(jī)5、移動(dòng)機(jī)構(gòu)、位移傳感器6、位移計(jì)數(shù)器7和原點(diǎn)傳感器8 ；
[0035]工控機(jī)I的位置指令信號(hào)輸出端連接DSP2的位置指令信號(hào)輸入端，DSP2將要發(fā)送的脈沖總數(shù)寫位置脈沖減計(jì)數(shù)器3，所述DSP2的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器3的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器3的狀態(tài)信號(hào)輸出端連接DSP2的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器3用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器4的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器3的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器4的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器4的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)5的驅(qū)動(dòng)信號(hào)輸入端,所述電機(jī)5用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器6用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器6輸出表位移信息的脈沖信號(hào)給位移計(jì)數(shù)器7的脈沖信號(hào)輸入端，原點(diǎn)傳感器8用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器8的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器7的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器7的脈沖計(jì)數(shù)輸出端連接DSP2的位置脈沖信號(hào)輸入端；
[0036]根據(jù)運(yùn)動(dòng)設(shè)備建立設(shè)備坐標(biāo)系，所述的原點(diǎn)精確定位方法是由嵌入在DSP2內(nèi)部的軟件實(shí)現(xiàn)的，該方法包括以下步驟:
[0037]初始化步驟:用于啟動(dòng)電機(jī)5，電機(jī)5的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器7清零，位置脈沖減計(jì)數(shù)器3的脈沖數(shù)為maxval，maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器脈沖數(shù)，在該步驟結(jié)束后執(zhí)行判斷步驟一；
[0038]判斷步驟一:判斷移動(dòng)機(jī)構(gòu)是否到達(dá)原點(diǎn)傳感器8的感應(yīng)范圍且接收到位移傳感器6的Z相信號(hào)，如果是則執(zhí)行關(guān)閉脈沖指令步驟，如果否則重新執(zhí)行該步驟；
[0039]關(guān)閉脈沖指令步驟:讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器7的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP2中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)5的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷步驟二 ；否則重復(fù)執(zhí)行該步驟；
[0040]判斷步驟二:判斷電機(jī)5是否在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向沿軸的正方向，如果是則結(jié)束原點(diǎn)定位；如果否則執(zhí)行校正步驟；
[0041]校正步驟:到位標(biāo)志計(jì)數(shù)器清零，DSP2向位置脈沖減計(jì)數(shù)器3寫入正整數(shù)rglapls，設(shè)定電機(jī)5的運(yùn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸的正方向，然后啟動(dòng)電機(jī)5，然后重復(fù)查詢位置脈沖減計(jì)數(shù)器3的狀態(tài)，待位置脈沖減計(jì)數(shù)器3為O時(shí)，執(zhí)行步驟關(guān)閉脈沖指令步驟。
[0042]本實(shí)施方式中，初始化步驟中位移計(jì)數(shù)器清零后不是真正的把該計(jì)數(shù)器置零，而是根據(jù)設(shè)備在該軸方向上行程而定的值midval，該值稍大于整個(gè)行程對(duì)應(yīng)的最大位移脈沖計(jì)數(shù)。這樣避免因?yàn)樵O(shè)備上電就反向移動(dòng)而造成的位移計(jì)數(shù)器反向溢出。
[0043]在設(shè)備回原點(diǎn)過程中將位置脈沖減計(jì)數(shù)器初值設(shè)為maxval，該值為走完整個(gè)行程所需要發(fā)送給電機(jī)驅(qū)動(dòng)器的最大位置脈沖數(shù)，這樣將起到保護(hù)作用因?yàn)楫?dāng)減計(jì)數(shù)器為O的時(shí)候，位置脈沖輸出將被關(guān)斷，一定程度避免了因某設(shè)備故障，導(dǎo)致設(shè)備不能到原點(diǎn)區(qū)域，但是位置脈沖一直在發(fā)送，進(jìn)而發(fā)生危險(xiǎn)。
[0044]關(guān)閉脈沖指令步驟中，當(dāng)發(fā)送給電機(jī)的位置脈沖信號(hào)關(guān)斷之后，由于慣性，以及驅(qū)動(dòng)器根據(jù)收到的位置脈沖數(shù)調(diào)整電機(jī)位置，運(yùn)動(dòng)設(shè)備不可能立即停止。本發(fā)明采用計(jì)數(shù)濾波的方式，當(dāng)在20個(gè)采樣周期內(nèi)，位置計(jì)數(shù)器的值都沒有變，那么可以認(rèn)為運(yùn)動(dòng)設(shè)備確實(shí)已經(jīng)停止。這樣就在盡可能短的時(shí)間內(nèi)可靠的得到運(yùn)動(dòng)設(shè)備是否停止的信息。
[0045]校正步驟中，在原點(diǎn)位置關(guān)斷脈沖，并等待系統(tǒng)穩(wěn)定后，向位置脈沖減計(jì)數(shù)器寫入正整數(shù)，并設(shè)定運(yùn)動(dòng)設(shè)備運(yùn)動(dòng)方向?yàn)樽鴺?biāo)軸正方向。即讓設(shè)備再正向運(yùn)動(dòng)正整數(shù)個(gè)脈沖。正整數(shù)的取值與運(yùn)動(dòng)設(shè)備慣性和位置脈沖延遲有關(guān)，要保證該過程結(jié)束后，運(yùn)動(dòng)設(shè)備停止的位置在原點(diǎn)位置的正方向一側(cè)。
[0046]【具體實(shí)施方式】四:本實(shí)施方式對(duì)【具體實(shí)施方式】三所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法作進(jìn)一步限定，本實(shí)施方式中，位移傳感器6采用磁柵尺或光柵尺實(shí)現(xiàn)。
[0047]本實(shí)施方式中，在需要定位操作的設(shè)備中，位移傳感器是必不可少的，以磁柵尺和光柵尺最為常見，而且這些位移傳感器大多為增量式的，其輸出信號(hào)都符合統(tǒng)一的標(biāo)準(zhǔn)規(guī)范，即以A/B/Z的方式輸出位移增量脈沖。
[0048]【具體實(shí)施方式】五:本實(shí)施方式對(duì)【具體實(shí)施方式】三所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法作進(jìn)一步限定，本實(shí)施方式中，DSP2通過位移計(jì)數(shù)器7接收位移傳感器6發(fā)送的表示當(dāng)前位置的脈沖信息號(hào)。
[0049]【具體實(shí)施方式】六:本實(shí)施方式對(duì)【具體實(shí)施方式】三所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法作進(jìn)一步限定，本實(shí)施方式中，判斷步驟一的位移傳感器6的Z相信號(hào)為固定間隔a的脈沖信號(hào)，原定傳感器的感應(yīng)區(qū)域?qū)挾葁大于等于a且小于2a。
[0050]本實(shí)施方式中，所選用的位移傳感器的精度一定是要符合設(shè)備的定位精度的，其輸出信號(hào)的Z相為固定間隔a大于傳感器精度輸出的脈沖信號(hào)。所選用的低精度傳感器應(yīng)滿足其感應(yīng)區(qū)域?qū)挾葁滿足a〈=w〈2a，這樣就保證了在原點(diǎn)定位傳感器感應(yīng)區(qū)域內(nèi)有且只有一個(gè)位移傳感器的Z相脈沖。
【權(quán)利要求】
1.低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)，其特征在于:它包括工控機(jī)(I)、DSP(2)、位置脈沖減計(jì)數(shù)器(3)、電機(jī)驅(qū)動(dòng)器(4)、電機(jī)(5)、移動(dòng)機(jī)構(gòu)、位移傳感器(6)、位移計(jì)數(shù)器(7)和原點(diǎn)傳感器(8); 工控機(jī)(I)的位置指令信號(hào)輸出端連接DSP (2)的位置指令信號(hào)輸入端，DSP (2)將要發(fā)送的脈沖總數(shù)寫位置脈沖減計(jì)數(shù)器(3)，所述DSP (2)的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器(3)的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器(3)的狀態(tài)信號(hào)輸出端連接DSP(2)的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器(3)用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器(4)的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器(3)的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器(4)的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器(4)的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)(5)的驅(qū)動(dòng)信號(hào)輸入端，所述電機(jī)(5)用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器(6)用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器(6)輸出表示位移信息的脈沖信號(hào)給位移計(jì)數(shù)器(7)的脈沖信號(hào)輸入端，原點(diǎn)傳感器(8)用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器(8)的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器(7)的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器(7)的脈沖計(jì)數(shù)輸出端連接DSP (2)的位置脈沖信號(hào)輸入端； 在DSP (2)內(nèi)部的軟件實(shí)現(xiàn)的定位模塊，所述定位模塊包括以下單元: 初始化單兀:用于啟動(dòng)電機(jī)(5),電機(jī)(5)的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器(7)清零,位置脈沖減計(jì)數(shù)器(3)的脈沖數(shù)為maxval, maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器脈沖數(shù)； 判斷單元一:用于判斷移動(dòng)機(jī)構(gòu)是否到達(dá)原點(diǎn)傳感器(8)的感應(yīng)范圍且接收到位移傳感器(6)的Z相信號(hào) ，如果是則執(zhí)行關(guān)閉脈沖指令單元，否則重新執(zhí)行該單元； 關(guān)閉脈沖指令單元:用于讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器(7)的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP (2)中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)(5)的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷單元二 ；否則重復(fù)執(zhí)行該單元； 判斷單元二:用于判斷電機(jī)(5)是否在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向沿軸的正方向，如果是則結(jié)束原點(diǎn)定位；如果否則執(zhí)行校正單元； 校正單元:到位標(biāo)志計(jì)數(shù)器清零，DSP (2)向位置脈沖減計(jì)數(shù)器(3)寫入正整數(shù)rglapls,設(shè)定電機(jī)(5)的運(yùn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸的正方向,然后啟動(dòng)電機(jī)(5),然后重復(fù)查詢位置脈沖減計(jì)數(shù)器(3)的狀態(tài)，待位置脈沖減計(jì)數(shù)器(3)為O時(shí)，執(zhí)行步驟關(guān)閉脈沖指令單兀。
2.根據(jù)權(quán)利要求1所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)，其特征在于:位移傳感器(6)采用磁柵尺或光柵尺實(shí)現(xiàn)。
3.低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，該方法是低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)實(shí)現(xiàn)的，所述低精度多個(gè)傳感器融合的原點(diǎn)精確定位系統(tǒng)包括工控機(jī)(I)、DSP (2)、位置脈沖減計(jì)數(shù)器(3)、電機(jī)驅(qū)動(dòng)器(4)、電機(jī)(5)、移動(dòng)機(jī)構(gòu)、位移傳感器(6)、位移計(jì)數(shù)器(7)和原點(diǎn)傳感器(8); 工控機(jī)(I)的位置指令信號(hào)輸出端連接DSP (2)的位置指令信號(hào)輸入端，DSP (2)將要發(fā)送的脈沖總數(shù)寫位置脈沖減計(jì)數(shù)器(3)，所述DSP (2)的脈沖信號(hào)輸出端連接位置脈沖減計(jì)數(shù)器(3)的脈沖信號(hào)輸入端，所述位置脈沖減計(jì)數(shù)器(3)的狀態(tài)信號(hào)輸出端連接DSP(2)的狀態(tài)信號(hào)輸入端，位置脈沖減計(jì)數(shù)器(3)用于控制發(fā)送給電機(jī)驅(qū)動(dòng)器(4)的位置脈沖總數(shù)，位置脈沖減計(jì)數(shù)器(3)的位置脈沖信號(hào)輸出端連接電機(jī)驅(qū)動(dòng)器(4)的位置脈沖信號(hào)輸入端，所述電機(jī)驅(qū)動(dòng)器(4)的驅(qū)動(dòng)信號(hào)輸出端連接電機(jī)(5)的驅(qū)動(dòng)信號(hào)輸入端，所述電機(jī)(5)用于驅(qū)動(dòng)移動(dòng)機(jī)構(gòu)移動(dòng),位移傳感器(6)用于檢測(cè)移動(dòng)機(jī)構(gòu)的位移信息,位移傳感器(6)輸出表示位移信息的脈沖信號(hào)給位移計(jì)數(shù)器(7)的脈沖信號(hào)輸入端，原點(diǎn)傳感器(8)用于檢測(cè)移動(dòng)機(jī)構(gòu)是否位于原點(diǎn)范圍的狀態(tài)，原點(diǎn)傳感器(8)的檢測(cè)信號(hào)輸出端連接位移計(jì)數(shù)器(7)的原點(diǎn)檢測(cè)信號(hào)輸入端，所述位移計(jì)數(shù)器(7)的脈沖計(jì)數(shù)輸出端連接DSP (2)的位置脈沖信號(hào)輸入端； 其特征在于:根據(jù)運(yùn)動(dòng)設(shè)備建立設(shè)備坐標(biāo)系，所述的原點(diǎn)精確定位方法是由嵌入在DSP (2)內(nèi)部的軟件實(shí)現(xiàn)的，該方法包括以下步驟: 初始化步驟:用于啟動(dòng)電機(jī)(5),電機(jī)(5)的轉(zhuǎn)動(dòng)方向?yàn)樵O(shè)備坐標(biāo)系中軸向的負(fù)方向；位移計(jì)數(shù)器(7)清零,位置脈沖減計(jì)數(shù)器(3)的脈沖數(shù)為maxval, maxval表示該軸整個(gè)運(yùn)動(dòng)范圍所對(duì)應(yīng)的位移傳感器脈沖數(shù)，在該步驟結(jié)束后執(zhí)行判斷步驟一； 判斷步驟一:判斷移動(dòng)機(jī)構(gòu)是否到達(dá)原點(diǎn)傳感器(8)的感應(yīng)范圍且接收到位移傳感器(6)的Z相信號(hào)，如果是則執(zhí)行關(guān)閉脈沖指令步驟，否則重新執(zhí)行該步驟； 關(guān)閉脈沖指令步驟:讀取系統(tǒng)每個(gè)脈沖周期上位移計(jì)數(shù)器(7)的值，讀取的當(dāng)前值與上個(gè)周期的讀取的值相同時(shí)，則DSP (2)中的到位標(biāo)志計(jì)數(shù)器加1，否則到位標(biāo)志計(jì)數(shù)器清零，判斷位標(biāo)志計(jì)數(shù)器是否大于等于20，如果是則關(guān)閉電機(jī)(5)的脈沖指令，使移動(dòng)機(jī)構(gòu)停止運(yùn)動(dòng)，然后執(zhí)行判斷步驟二 ；否則重復(fù)執(zhí)行該步驟； 判斷步驟二:判斷電機(jī)(5)是否在設(shè)備坐標(biāo)系的運(yùn)動(dòng)方向沿軸的正方向，如果是則結(jié)束原點(diǎn)定位；如果否則執(zhí)行校正步驟； 校正步驟:到位標(biāo)志計(jì)數(shù)器清零，DSP (2)向位置脈沖減計(jì)數(shù)器(3)寫入正整數(shù)rglapls,設(shè)定電機(jī)(5)的運(yùn)動(dòng)方向 為設(shè)備坐標(biāo)系中軸的正方向，然后啟動(dòng)電機(jī)(5),然后重復(fù)查詢位置脈沖減計(jì)數(shù)器(3)的狀態(tài)，待位置脈沖減計(jì)數(shù)器(3)為O時(shí)，執(zhí)行步驟關(guān)閉脈沖指令步驟。
4.根據(jù)權(quán)利要求4所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，其特征在于:位移傳感器(6)采用磁柵尺或光柵尺實(shí)現(xiàn)。
5.根據(jù)權(quán)利要求4所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，其特征在于:DSP (2)通過位移計(jì)數(shù)器(7)接收位移傳感器(6)發(fā)送的表示當(dāng)前位置的脈沖信號(hào)。
6.根據(jù)權(quán)利要求4所述的低精度多個(gè)傳感器融合的原點(diǎn)精確定位方法，其特征在于:判斷步驟一的位移傳感器(6)的Z相信號(hào)為固定間隔a的脈沖信號(hào)，原定傳感器的感應(yīng)區(qū)域?qū)挾葁大于等于a且小于2a。
【文檔編號(hào)】G05D3/20GK103760921SQ201410028037
【公開日】2014年4月30日 申請(qǐng)日期:2014年1月22日 優(yōu)先權(quán)日:2014年1月22日
【發(fā)明者】孫光輝, 任雨, 高會(huì)軍, 于金泳, 孫一勇, 張?jiān)鼋? 申請(qǐng)人:哈爾濱工業(yè)大學(xué)