專利名稱：移動(dòng)體系統(tǒng)和移動(dòng)體的到位判定方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種移動(dòng)體系統(tǒng)，特別涉及移動(dòng)體是否能夠停止在容許范圍內(nèi)(到 位)的判定。
背景技術(shù)：
對(duì)于多軸的移動(dòng)體，如下情況較多，S卩，以第1軸的位置進(jìn)入規(guī)定范圍內(nèi)(到位范 圍內(nèi))為條件，使第2軸動(dòng)作。例如在橋式吊車中，當(dāng)行走方向位置進(jìn)入規(guī)定范圍內(nèi)時(shí)，升 降臺(tái)開始升降或者橫向進(jìn)給。在堆垛機(jī)和無人搬送車中，當(dāng)行走方向位置(以及堆垛機(jī)中 的升降方向位置)進(jìn)入規(guī)定范圍內(nèi)時(shí)，使滑叉等移載裝置動(dòng)作。另外在機(jī)床等中，當(dāng)?shù)?軸 的χ方向位置或者xy面內(nèi)位置進(jìn)入規(guī)定范圍內(nèi)時(shí)，沿著第2軸的ζ方向使工具移動(dòng)，并開 始加工。對(duì)第1軸和第2軸的順序動(dòng)作應(yīng)用到位判定，當(dāng)?shù)?軸的位置進(jìn)入到位范圍內(nèi)時(shí)， 開始進(jìn)行第2軸的動(dòng)作。例如專利文獻(xiàn)1(JP2000-231412A)公開如下技術(shù)，S卩，當(dāng)在xy面 內(nèi)的移動(dòng)后接著進(jìn)行ζ軸方向的移動(dòng)時(shí)，進(jìn)行關(guān)于xy面內(nèi)的合成移動(dòng)方向的到位判定，對(duì) 于二維移動(dòng)進(jìn)行一維中的到位判定。但是，當(dāng)只使用當(dāng)前位置是否在到位范圍內(nèi)來判定時(shí)，判定為到位后，移動(dòng)體可能 由于過沖而從到位范圍脫離。通過圖6、圖7說明此情況。圖6表示移動(dòng)體不振動(dòng)而停止的 狀況，圖7表示移動(dòng)體由于振動(dòng)而產(chǎn)生過沖的狀況。圖6、圖7中的a)表示位置的軌跡，b) 表示速度和位置在相位面上的軌跡，c)表示粗略和精確這2個(gè)階段的到位判定結(jié)果。在圖 6中，移動(dòng)體不振動(dòng)地朝向目的位置減速，不發(fā)生過沖。與之相對(duì)，在圖7中，位置和速度振 動(dòng)，相位面中的軌跡為螺旋狀，有必要在中途取消暫時(shí)成立的到位判定。

發(fā)明內(nèi)容
本發(fā)明的課題在于迅速且正確地判定有無由于過沖而從到位范圍脫離的可能性。本發(fā)明是當(dāng)移動(dòng)體進(jìn)入了到位范圍內(nèi)時(shí)進(jìn)行到位判定的系統(tǒng)，其特征在于，設(shè)置 有傳感器和計(jì)算機(jī)構(gòu)，上述傳感器用于求得移動(dòng)體的位置、速度和加速度，上述計(jì)算機(jī)構(gòu)用 于基于所求得的位置、速度和加速度而推定移動(dòng)體的停止位置是否在到位范圍內(nèi)。在本發(fā)明中，根據(jù)當(dāng)前位置在到位范圍內(nèi)、以及推定停止位置也在到位范圍內(nèi)這 兩者進(jìn)行判定。因此，如果移動(dòng)體有由于過沖等從到位范圍脫離的可能性，則不進(jìn)行到位判 定，可以進(jìn)行具有可靠性的判定。另外，在本發(fā)明中，根據(jù)移動(dòng)體的實(shí)際位置、速度和加速度 來判定，不需要使用移動(dòng)體的模型。因此，沒有基于移動(dòng)體的模型化的誤差。上述計(jì)算機(jī)構(gòu)為，根據(jù)移動(dòng)體的位置的時(shí)間序列數(shù)據(jù){Pi}求得速度的時(shí)間序列 數(shù)據(jù){vi}，并且根據(jù)求得的速度的時(shí)間序列數(shù)據(jù)求得加速度的時(shí)間序列數(shù)據(jù)Iai)，在這 里，i為表示時(shí)間序列的附注，i表示當(dāng)前，并且從當(dāng)前位置Pi到停止位置的距離實(shí)質(zhì)上 以-vi7ai求得。在這里，“實(shí)質(zhì)上”的意思是，-Vi2Aii乘以0.8 1.2左右的常數(shù)亦可， 或者加減到位范圍的1/10 1/100左右的補(bǔ)償亦可。實(shí)質(zhì)上，由于-vi7ai的距離相當(dāng)于到停止位置的距離的上限，所以，如果從當(dāng)前位置實(shí)質(zhì)上只前進(jìn)-Vi2Aii的位置在到位范圍 內(nèi)，則可以判定移動(dòng)體由于過沖等從到位范圍脫離的可能性實(shí)質(zhì)上是沒有的。而且此判定 可以通過簡(jiǎn)單的計(jì)算而迅速進(jìn)行。優(yōu)選地，上述傳感器作為求得移動(dòng)體的位置的線性傳感器，在短周期內(nèi)正確測(cè)定 第1軸方向的位置。優(yōu)選地，當(dāng)通過上述傳感器求得的當(dāng)前位置和通過上述計(jì)算機(jī)構(gòu)推定的停止位置 都在到位范圍內(nèi)時(shí)，判定為到位。本發(fā)明的移動(dòng)體的到位判定方法是當(dāng)移動(dòng)體進(jìn)入了到位范圍內(nèi)時(shí)判定到位的方 法，其特征在于，設(shè)置有求得步驟，通過傳感器求得移動(dòng)體的位置、速度和加速度；推定步驟，用于通過計(jì)算機(jī)構(gòu)基于所求得的位置、速度和加速度來推定移動(dòng)體的 停止位置；以及，判定步驟，用于通過判定機(jī)構(gòu)判定所推定的停止位置是否在到位范圍內(nèi)。本說明書中，關(guān)于移動(dòng)體系統(tǒng)的記載直接完全適合于移動(dòng)體的到位判定方法，反 過來，關(guān)于移動(dòng)體的到位判定方法的記載直接完全適合于移動(dòng)體系統(tǒng)。優(yōu)選地，在上述判定步驟中，當(dāng)通過上述傳感器求得的當(dāng)前位置和通過上述計(jì)算 機(jī)構(gòu)推定的停止位置都在到位范圍內(nèi)時(shí)，判定為到位。


圖1是實(shí)施例的移動(dòng)體系統(tǒng)的主要部分的框圖。圖2是實(shí)施例中的到位判定部的框圖。圖3是表示實(shí)施例中的到位判定算法的流程圖。圖4是表示實(shí)施例中的停止位置的推定算法的流程圖。圖5是表示實(shí)施例中的、應(yīng)用了相位面的停止位置的推定的附圖。圖6表示現(xiàn)有例中的到位判定，a)表示移動(dòng)體不振動(dòng)地接近目的位置時(shí)的軌跡，b)表示移動(dòng)體的相位面上的軌跡，c)表示粗略到位的判定信號(hào)和精確到位的判定信號(hào)。圖7表示現(xiàn)有例中的到位判定，a)表示移動(dòng)體一邊過沖一邊接近目的位置時(shí)的軌跡，b)表示移動(dòng)體的相位面上的軌跡，c)表示粗略到位的判定信號(hào)和精確到位的判定信號(hào)。附圖標(biāo)記說明2移動(dòng)體系統(tǒng)4、10 控制器6、12伺服放大器8、14線性傳感器16到位判定部20位置數(shù)據(jù)存儲(chǔ)部
21速度數(shù)據(jù)存儲(chǔ)部22加速度數(shù)據(jù)存儲(chǔ)部23計(jì)算部24停止推定位置存儲(chǔ)部M1、M2 馬達(dá)
具體實(shí)施例方式下面示出實(shí)施本發(fā)明的最佳實(shí)施例。本發(fā)明的范圍應(yīng)該基于所要求保護(hù)的范圍的 記載，參考說明書的記載和本領(lǐng)域的公知技術(shù)，根據(jù)本領(lǐng)域技術(shù)人員的理解來確定。在圖1 圖5中，表示實(shí)施例的移動(dòng)體系統(tǒng)2。在各圖中，4為第1軸控制器，10為 第2軸控制器，經(jīng)由各個(gè)伺服放大器6、12驅(qū)動(dòng)馬達(dá)M1、M2。線性傳感器8、14分別求得第1 軸方向的移動(dòng)體的位置和第2軸方向的移動(dòng)體的位置，并輸入到控制器4、10。到位判定部16以第1軸方向的根據(jù)線性傳感器8得到的位置的時(shí)間序列數(shù)據(jù)為 基礎(chǔ)，生成速度的時(shí)間序列數(shù)據(jù)，并根據(jù)速度的時(shí)間序列數(shù)據(jù)生成加速度的時(shí)間序列數(shù)據(jù)。 而且通過當(dāng)前位置和當(dāng)前速度、以及當(dāng)前加速度來推定停止位置，并判定其是否在到位范 圍內(nèi)。在當(dāng)前位置在到位范圍內(nèi)并且推定停止位置也在到位范圍內(nèi)時(shí)，到位判定部16判定 為到位，基于此，第2軸控制器10啟動(dòng)馬達(dá)M2?？刂破? 到位判定部16設(shè)置在移動(dòng)體上，例如在馬達(dá)Ml、M2為地面1次的線性 馬達(dá)并且在移動(dòng)體上設(shè)置馬達(dá)Ml、M2的2次側(cè)的情況下，在地面?zhèn)仍O(shè)置控制器4 到位判 定部16。線性傳感器8、14可以設(shè)置在移動(dòng)體上，亦可設(shè)置在地面?zhèn)龋€性傳感器8、14例如 由多個(gè)線圈構(gòu)成，根據(jù)線圈的電感變化檢測(cè)出與被檢測(cè)用的磁標(biāo)記相對(duì)應(yīng)的位置。在圖2中，表示到位判定部16的結(jié)構(gòu)。位置數(shù)據(jù)存儲(chǔ)部20存儲(chǔ)根據(jù)線性傳感器 8得到的位置的時(shí)間序列數(shù)據(jù){Pi}，計(jì)算部23根據(jù)位置的時(shí)間序列數(shù)據(jù)的差分生成速度的 時(shí)間序列數(shù)據(jù)Ivi}，速度數(shù)據(jù)存儲(chǔ)部21存儲(chǔ)時(shí)間序列數(shù)據(jù){vi}。計(jì)算部23根據(jù)速度的時(shí) 間序列數(shù)據(jù)生成加速度的時(shí)間序列數(shù)據(jù){ai}，并由加速度數(shù)據(jù)存儲(chǔ)部22存儲(chǔ)。計(jì)算部23利 用當(dāng)前位置Pi和當(dāng)前速度vi以及當(dāng)前加速度ai，以Pi-vi7ai來求得停止位置。在這里， 負(fù)號(hào)對(duì)應(yīng)于減速時(shí)加速度為負(fù)的情況。推定停止位置不需要嚴(yán)格地為Pi_vi2/ai，實(shí)質(zhì)上為 Pi-vi2/ai即可。例如在vi2/ai的項(xiàng)中，乘以0. 8 1. 2左右的系數(shù)亦可，或者在Pi_vi2/ ai的項(xiàng)中加減到位范圍的幅度的1/10 1/100左右的補(bǔ)償亦可。計(jì)算部23存儲(chǔ)停止推定 位置。當(dāng)當(dāng)前位置和停止推定位置都在到位范圍內(nèi)時(shí)，計(jì)算部23判定為到位。在圖3 圖5中表示到位判定的方法。在這里，由粗略到位和精確到位這2個(gè)階 段來判定到位，但是1個(gè)階段亦可，或者3個(gè)階段以上亦可。當(dāng)當(dāng)前位置在粗略到位范圍內(nèi) 并且推定停止位置也在粗略到位范圍內(nèi)時(shí)，判定為粗略到位。然后，當(dāng)當(dāng)前位置在精確到位 范圍內(nèi)并且推定停止位置也在精確到位范圍內(nèi)時(shí)，判定為精確到位。表示停止位置的推定機(jī)制。根據(jù)位置Pi的時(shí)間序列數(shù)據(jù)得到速度的時(shí)間序列數(shù) 據(jù)vi，并據(jù)此得到加速度的時(shí)間序列數(shù)據(jù)ai。停止推定位置定為Pi_vi2/ai，如上述那樣， vi2/ai的項(xiàng)乘以0. 8 1. 2的系數(shù)亦可，或者加減到位范圍的1/10 1/100左右的補(bǔ)償亦 可。另外，雖然位置Pi的時(shí)間序列數(shù)據(jù)可以在短周期內(nèi)正確地從線性傳感器得到，但也可 以從測(cè)定周期長的激光距離傳感器得到。
5
在圖5中表示Pi-Vi2Aii的項(xiàng)的意思。圖5表示位置Pi和速度vi的相位面，相 位面上的坐標(biāo)按照Q0、QU Q2、Q3的順序接近目標(biāo)停止位置。另外，在目標(biāo)停止位置上，位 置和速度都為0。在這里，在進(jìn)入了粗略到位范圍內(nèi)的Ql時(shí)刻，例如求得連接其與緊之前的 點(diǎn)QO的軌跡的連線(圖5的虛線)與位置軸的截距，來判定其是否在粗略到位范圍內(nèi)。另 外，在當(dāng)前位置進(jìn)入了精確到位范圍內(nèi)的Q3時(shí)刻，求得連接緊之前的點(diǎn)Q2與點(diǎn)Q3的連線 與位置軸的截距，來判定其是否在精確到位范圍內(nèi)。此處，從當(dāng)前位置與緊之前的兩個(gè)點(diǎn)產(chǎn) 生連線，但是，例如也可以以點(diǎn)Q3、Q2的中點(diǎn)作為當(dāng)前位置，以點(diǎn)Q1、Q0的中點(diǎn)作為一個(gè)之 前的位置，從四個(gè)點(diǎn)產(chǎn)生連線。對(duì)這樣得到的截距的意義進(jìn)行說明。QO Q3等的點(diǎn)由線性傳感器得到，而不是根 據(jù)移動(dòng)體的控制上的模型得到。另外，由于對(duì)移動(dòng)體賦予加速度從而使之停止在目的位置， 所以，實(shí)際上如圖5的白圓的排列那樣，停止在比圖5的接線更接近目的位置的一側(cè)。例如 在等加速度運(yùn)動(dòng)中使移動(dòng)體減速的情況下，停止位置為Pi-vi72ai，-vi2/ai的項(xiàng)假定為從 當(dāng)前位置僅以等加速度運(yùn)動(dòng)情況的2倍前進(jìn)并停止。這樣，在圖5中與位置軸的截距為在 最壞的情況下估算的移動(dòng)體的停止位置。在圖5的評(píng)價(jià)中，根據(jù)現(xiàn)實(shí)的移動(dòng)體的位置的時(shí)間序列數(shù)據(jù)推定停止位置，不包 含移動(dòng)體的控制上的模型。因此，不受對(duì)移動(dòng)體模型化時(shí)的誤差的影響。因此，可以推定距 目的停止位置的偏差的上限。另外，由于在進(jìn)行圖5的推定的時(shí)刻，移動(dòng)體的位置在粗略到 位范圍內(nèi)，或者在精確到位范圍內(nèi)，并進(jìn)行減速運(yùn)動(dòng)，因此不可能停止在到位范圍的近前一 側(cè)(到位范圍之外、且靠近該到位范圍的位置)。由于這些原因，可以通過正確并且簡(jiǎn)單的 計(jì)算迅速地判定移動(dòng)體是否停止在到位范圍內(nèi)。當(dāng)正確并且迅速地判定移動(dòng)體是否停止在到位范圍內(nèi)時(shí)，不僅可以更正確地定 位，而且還能夠更迅速地使接下來的第2軸的運(yùn)動(dòng)開始。
權(quán)利要求
1.一種移動(dòng)體系統(tǒng)，是在移動(dòng)體進(jìn)入了到位范圍內(nèi)時(shí)進(jìn)行到位判定的系統(tǒng)，其特征在 于，具有傳感器，用于求得移動(dòng)體的位置、速度和加速度；以及，計(jì)算機(jī)構(gòu)，用于基于所求得的位置、速度和加速度來推定移動(dòng)體的停止位置是否在到 位范圍內(nèi)。
2.如權(quán)利要求1所述的移動(dòng)體系統(tǒng)，其特征在于，上述計(jì)算機(jī)構(gòu)根據(jù)移動(dòng)體的位置的 時(shí)間序列數(shù)據(jù){Pi}求得速度的時(shí)間序列數(shù)據(jù)Ivi}，并且，根據(jù)所求得的速度的時(shí)間序列數(shù) 據(jù)求得加速度的時(shí)間序列數(shù)據(jù)lai}，在這里i為表示時(shí)間序列的附注，并且i表示當(dāng)前，從當(dāng)前位置Pi到停止位置的距離實(shí)質(zhì)上以-Vi2Aii求得。
3.如權(quán)利要求1所述的移動(dòng)體系統(tǒng)，其特征在于，上述傳感器為求得移動(dòng)體的位置的 線性傳感器。
4.如權(quán)利要求2所述的移動(dòng)體系統(tǒng)，其特征在于，還設(shè)置有判定機(jī)構(gòu)，在通過上述傳感 器求得的當(dāng)前位置和通過上述計(jì)算機(jī)構(gòu)推定的停止位置都在到位范圍內(nèi)時(shí)，該判定機(jī)構(gòu)判 定為到位。
5.一種移動(dòng)體的到位判定方法，是在移動(dòng)體進(jìn)入了到位范圍內(nèi)時(shí)判定為到位的方法， 其特征在于，具有求得步驟，通過傳感器求得移動(dòng)體的位置、速度和加速度；推定步驟，用于通過計(jì)算機(jī)構(gòu)基于所求得的位置、速度和加速度來推定移動(dòng)體的停止 位置；以及，判定步驟，用于通過判定機(jī)構(gòu)判定所推定的停止位置是否在到位范圍內(nèi)。
6.權(quán)利要求5所述的移動(dòng)體的到位判定方法，其特征在于，在上述判定步驟中，當(dāng)通過 上述傳感器求得的當(dāng)前位置和通過上述計(jì)算機(jī)構(gòu)推定的停止位置都在到位范圍內(nèi)時(shí)，判定 為到位。
全文摘要
本發(fā)明提供一種移動(dòng)體系統(tǒng)和移動(dòng)體的到位判定方法。設(shè)置有傳感器和計(jì)算機(jī)構(gòu)，上述傳感器用于求得移動(dòng)體的位置、速度和加速度，上述計(jì)算機(jī)構(gòu)基于所求得的位置、速度和加速度，判定移動(dòng)體的停止位置是否在到位范圍內(nèi)。
文檔編號(hào)G05B19/19GK101995846SQ20101023398
公開日2011年3月30日 申請(qǐng)日期2010年7月20日 優(yōu)先權(quán)日2009年8月18日
發(fā)明者佐藤元 申請(qǐng)人:村田機(jī)械株式會(huì)社