專利名稱：預(yù)測碰撞可能性的方法和系統(tǒng)、防碰撞控制方法和系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及控制工程領(lǐng)域，具體地，涉及一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞 可能性的方法和系統(tǒng)以及一種防碰撞控制方法和系統(tǒng)。
背景技術(shù)：
在堆取料作業(yè)過程中，在一個(gè)堆場中經(jīng)常有需要使用多個(gè)取料機(jī)進(jìn)行作業(yè)的情 況，例如，圖1為堆取料機(jī)在堆場作業(yè)的示意圖，其中在一個(gè)堆場10中可能有不止一個(gè)取料 機(jī)20各沿著一個(gè)行走軌道30行走，而在行走過程中，多個(gè)取料機(jī)20的大臂之間有可能會(huì)發(fā)生碰撞。
目前一直沒有有效的方法來使得工作人員在堆取料機(jī)20之間將要發(fā)生碰撞前提 前得知可能要發(fā)生碰撞，而是只能通過人工觀察，這樣就浪費(fèi)了人力。堆取料機(jī)空間防碰撞 問題也一直是堆取料作業(yè)過程中的難題，舉例來說包括黃驊港在內(nèi)等煤炭碼頭中均無法實(shí) 現(xiàn)對(duì)碰撞問題的有效避免。具體來說，一種現(xiàn)有的防碰撞方法中需要結(jié)合堆取料機(jī)的行走 數(shù)據(jù)，而現(xiàn)有的方法獲取的堆取料機(jī)的行走數(shù)據(jù)經(jīng)常不準(zhǔn)，具體來說，由于堆取料機(jī)的行走 距離很長，一般都在1500m以上，而用于獲取堆取料機(jī)的行走數(shù)據(jù)的行走編碼器一般都安 裝在行走輪上，由于堆取料機(jī)的行走軌道與堆取料機(jī)的行走輪之間的摩擦力不均，一旦堆 料機(jī)的行走輪出現(xiàn)打滑現(xiàn)象，行走數(shù)據(jù)就會(huì)出現(xiàn)不準(zhǔn)確的情況，長時(shí)間累積會(huì)造成誤差越 來越大，即使通過行走定點(diǎn)校正對(duì)編碼器進(jìn)行修正，但不準(zhǔn)確因素仍無法徹底克服。其次， 對(duì)大臂的空間位置計(jì)算過程非常復(fù)雜，該計(jì)算過程需要結(jié)合行走、俯仰、回轉(zhuǎn)三個(gè)編碼器的 數(shù)值進(jìn)行空間建模，而這三個(gè)編碼器都有不同程度的誤差，這就造成累積誤差，故大臂空間 坐標(biāo)的準(zhǔn)確性不高。另一種現(xiàn)有的防碰撞方法一般是根據(jù)兩個(gè)堆取料機(jī)是否處于一個(gè)場垛 進(jìn)行判斷，如果兩個(gè)堆取料機(jī)不在同一個(gè)場垛就可以正常作業(yè)，一旦兩個(gè)堆取料機(jī)進(jìn)入一 個(gè)場垛進(jìn)行作業(yè)，就對(duì)其中一個(gè)堆取料機(jī)進(jìn)行鎖定，使其不能工作，由此避免堆取料機(jī)之間 發(fā)生碰撞，但是這嚴(yán)重影響了堆取料機(jī)的工程作業(yè)。
由于以上原因，現(xiàn)有的防碰撞方法無法有效避免目前國內(nèi)煤炭碼頭的堆取料機(jī)空 間防碰撞問題，使得兩個(gè)堆取料機(jī)無法同時(shí)在同一個(gè)場垛中作業(yè)，嚴(yán)重影響效率。發(fā)明內(nèi)容
本發(fā)明的目的是針對(duì)現(xiàn)有技術(shù)中無法自動(dòng)提前得知堆取料機(jī)發(fā)生碰撞的可能性 的問題，提供一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法和系統(tǒng)、以及一種防碰 撞控制方法和系統(tǒng)。
本發(fā)明提供一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法，該方法包括 檢測設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆取料機(jī)的大臂頭部中心的流動(dòng)站基于基 準(zhǔn)站的位置信息；根據(jù)對(duì)每個(gè)堆取料機(jī)檢測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩 個(gè)堆取料機(jī)的大臂之間的最小距離；以及根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生 碰撞的可能性。
本發(fā)明提供一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制方法，該方法包括根據(jù) 上述用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法判斷堆場中多個(gè)堆取料機(jī)的碰撞可 能性；以及根據(jù)可能性來控制堆取料機(jī)的動(dòng)作。
本發(fā)明提供一種用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)，該系統(tǒng)包 括基準(zhǔn)站；多個(gè)流動(dòng)站，與基準(zhǔn)站通信，分別設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆 取料機(jī)的大臂頭部中心處，以檢測堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和堆取料機(jī)的大臂頭部中心基 于基準(zhǔn)站的位置信息；以及處理單元，用于接收來自每個(gè)流動(dòng)站的位置信息，根據(jù)對(duì)每個(gè)堆 取料機(jī)檢測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料機(jī)的大臂之間的最小 距離，并且根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可能性。
本發(fā)明還提供一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制系統(tǒng)，該系統(tǒng)包括上 述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)；以及控制單元，根據(jù)所述用于預(yù)測 堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)判斷出的可能性來控制堆取料機(jī)的動(dòng)作。
根據(jù)本發(fā)明提供的用于堆取料機(jī)的防碰撞控制方法和系統(tǒng)，通過獲得堆取料機(jī)上 的流動(dòng)站的位置信息，得出兩個(gè)堆取料機(jī)之間的最小距離從而可以判斷出堆取料機(jī)發(fā)生碰 撞的可能性，使得工作人員根據(jù)該可能性進(jìn)行相應(yīng)處理，解決了行走編碼器不準(zhǔn)確、誤差累 積的問題，可以實(shí)時(shí)計(jì)算出堆取料機(jī)的大臂的相對(duì)位置和距離，可以實(shí)現(xiàn)兩臺(tái)甚至多臺(tái)堆 取料機(jī)在同一個(gè)場垛中安全作業(yè)。根據(jù)本發(fā)明提供的防碰撞控制方法和系統(tǒng)也具有上述優(yōu) 點(diǎn)，并且可以根據(jù)堆取料機(jī)碰撞的可能性采取相應(yīng)的措施，從而避免碰撞的發(fā)生。
本發(fā)明的其他特征和優(yōu)點(diǎn)將在隨后的具體實(shí)施方式
部分予以詳細(xì)說明。


附圖是用來提供對(duì)本發(fā)明的進(jìn)一步理解，并且構(gòu)成說明書的一部分，與下面的具 體實(shí)施方式一起用于解釋本發(fā)明，但并不構(gòu)成對(duì)本發(fā)明的限制。在附圖中
圖1是堆取料機(jī)在堆場作業(yè)的示意圖2是根據(jù)本發(fā)明的一種實(shí)施方式提供的用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可 能性的方法的流程圖3是流動(dòng)站在堆取料機(jī)上的安裝位置示意圖4(a)是兩個(gè)取料機(jī)共面時(shí)的狀態(tài)示意圖4(b)是兩個(gè)堆取料機(jī)異面時(shí)的狀態(tài)示意圖5是本發(fā)明的一種實(shí)施方式提供的用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性 的系統(tǒng)的結(jié)構(gòu)框圖。
具體實(shí)施方式
以下結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式
進(jìn)行詳細(xì)說明。應(yīng)當(dāng)理解的是，此處所描 述的具體實(shí)施方式
僅用于說明和解釋本發(fā)明，并不用于限制本發(fā)明。
本發(fā)明提供一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法，如圖2所示， 該方法包括在步驟201，檢測設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆取料機(jī)的大臂 頭部中心的流動(dòng)站基于基準(zhǔn)站的位置信息；在步驟202，根據(jù)對(duì)每個(gè)堆取料機(jī)檢測到的所 述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料機(jī)的大臂之間的最小距離；以及在步驟5203，根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可能性。從而能夠判斷出堆取 料機(jī)之間發(fā)生碰撞的可能性，以便于工作人員根據(jù)該可能性進(jìn)行下一步處理。
其中在步驟201中，檢測設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆取料機(jī) 的大臂頭部中心的流動(dòng)站基于基準(zhǔn)站的位置信息的過程可以通過使用如RTK系統(tǒng)中的 流動(dòng)站和基準(zhǔn)站來實(shí)現(xiàn)。這是因?yàn)镽TK系統(tǒng)采用了載波相位動(dòng)態(tài)實(shí)時(shí)差分(Real-time kinematic)方法，能夠在野外實(shí)時(shí)得到厘米級(jí)定位精度的測量。高精度的GPS測量必須采 用載波相位觀測值，RTK定位技術(shù)是基于載波相位觀測值的實(shí)時(shí)動(dòng)態(tài)定位技術(shù)，能夠?qū)崟r(shí)地 提供測站點(diǎn)在指定坐標(biāo)系中的三維定位結(jié)果，并達(dá)到厘米級(jí)精度。在使用RTK系統(tǒng)作業(yè)時(shí)， 基準(zhǔn)站通過數(shù)據(jù)鏈將其觀測值和測站坐標(biāo)信息一起傳送給流動(dòng)站。流動(dòng)站不僅通過數(shù)據(jù)鏈 接收來自基準(zhǔn)站的數(shù)據(jù)，還要采集GPS觀測數(shù)據(jù)，并在系統(tǒng)內(nèi)組成差分觀測值進(jìn)行實(shí)時(shí)處 理，同時(shí)給出厘米級(jí)定位結(jié)果，歷時(shí)不到一秒鐘。
其中可以首先在每臺(tái)堆取料機(jī)上設(shè)置兩臺(tái)流動(dòng)站，例如可以分別安裝在大機(jī)回轉(zhuǎn) 中心50 (即大臂轉(zhuǎn)動(dòng)所沿著的中心軸線處)和大臂頭部中心40處，流動(dòng)站安裝位置如圖3 所示，基準(zhǔn)站的位置可以根據(jù)具體需要設(shè)置在任意位置，例如可以在堆取料機(jī)的中控室內(nèi) 安裝基準(zhǔn)站。
兩個(gè)流動(dòng)站實(shí)時(shí)檢測空間的三維坐標(biāo)信息，這樣根據(jù)兩點(diǎn)確定一條直線原理，就 可以實(shí)時(shí)地知道堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和堆取料機(jī)的大臂頭部中心所在的直線。
通過使用流動(dòng)站和基準(zhǔn)站來檢測的大臂位置信息可以使得檢測結(jié)果精確較高 (可以達(dá)到厘米級(jí))，并且不受堆取料機(jī)自身行走輪打滑和其他編碼器累積誤差的影響，因 此比現(xiàn)有的防碰撞方法更加準(zhǔn)確高效?？朔爽F(xiàn)有技術(shù)中由于大臂空間位置反饋都是采用 行走、回轉(zhuǎn)、俯仰三個(gè)編碼器的數(shù)值計(jì)算得出的，而造成的誤差累積問題。
在步驟202中可以通過多種方法來根據(jù)所述位置信息確定多個(gè)堆取料機(jī)中任意 兩個(gè)堆取料機(jī)的大臂之間的最小距離，其中由于堆取料機(jī)的大臂較長，兩個(gè)堆取料機(jī)之間 的距離可以近似看作是兩個(gè)堆取料機(jī)的大臂之間的距離。
其中所述位置信息可以為三維坐標(biāo)，優(yōu)選地，兩個(gè)堆取料機(jī)之間的最小距離可以 為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī)回 轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段之間的最小距離。
其中當(dāng)兩個(gè)堆取料機(jī)的大臂共面時(shí)，所述最小距離可以為一堆取料機(jī)的大臂頭部 中心到另一個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的垂直距離或兩個(gè) 堆取料機(jī)的大臂頭部中心之間的最小距離；當(dāng)兩個(gè)堆取料機(jī)的大臂異面時(shí)，所述最小距離 可以為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī) 回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的公垂線段的長度或兩個(gè)堆取料機(jī)的大臂頭部中 心之間的最小距離。
優(yōu)選地，所述預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法還可以包括將所述最 小距離與第一預(yù)定距離和第二預(yù)定距離進(jìn)行比較；以及當(dāng)所述最小距離小于第一預(yù)定距 離，判斷可能性為較高；當(dāng)所述最小距離小于第一預(yù)定距離而大于第二預(yù)定距離時(shí)，判斷可 能性為中等；當(dāng)所述最小距離大于第二預(yù)定距離時(shí)，判斷可能性為較低。
可選地，所述預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法還可以包括當(dāng)判斷可 能性為較高時(shí)，進(jìn)行碰撞報(bào)警，使得工作人員得知堆取料機(jī)之間即將發(fā)生碰撞，可以進(jìn)行停機(jī)等處理；當(dāng)判斷可能性為中等時(shí)，進(jìn)行減速報(bào)警，使得工作人員得知堆取料機(jī)之間可能要 發(fā)生碰撞，需要減慢堆取料機(jī)運(yùn)行速度；當(dāng)判斷可能性為較低時(shí)，不進(jìn)行報(bào)警，堆取料機(jī)可 以安全地進(jìn)行作業(yè)。
其中第一預(yù)定距離可以小于第二預(yù)定距離，并且都可以根據(jù)需要預(yù)先設(shè)定，舉 例來說，第一預(yù)定距離取值范圍可以為3-6m，例如為: ，第二預(yù)定距離取值范圍可以為 6-20m，例如為8m。
下面通過舉例描述一種可以用于在步驟202中用于確定最小距離方法的過程。
采集到各臺(tái)堆取料機(jī)的位置信息后，可以通過以下過程來建立堆取料機(jī)大機(jī)回轉(zhuǎn) 中心和大臂頭部中心所構(gòu)成的線段的空間直線方程，現(xiàn)在以兩個(gè)堆取料機(jī)為例具體介紹， 兩個(gè)堆取料機(jī)可以稱為第一堆取料機(jī)和第二堆取料機(jī)。
設(shè)第一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心A和大臂頭部中心B處的三維坐標(biāo)分別為A (xl， yl,zl)、B(x2，y2，z2)，第二堆取料機(jī)的大機(jī)回轉(zhuǎn)中心C和大臂頭部中心D處的三維坐標(biāo)分 別為C(x3, y3，z3)、D(x4，y4，z4)。也就是說通過RTK采集了四個(gè)點(diǎn)的坐標(biāo)，每兩個(gè)點(diǎn)可以 確定一條線段。根據(jù)空間立體幾何，兩條直線線段的關(guān)系有兩種情況共面和異面。所謂共 面是指兩條直線在一個(gè)平面內(nèi)，而異面則指兩條直線不在一個(gè)平面內(nèi)，現(xiàn)在分別介紹兩種 情況下如何兩個(gè)堆取料機(jī)之間的最小距離。
如圖4(a)所示，兩條直線共面存在兩種情況，一種是兩條直線平行，一種是兩條 直線相交。兩條直線平行的情況是指兩個(gè)堆取料機(jī)的大臂在空間坐標(biāo)中平行時(shí)的狀態(tài)；而 兩條直線相交在實(shí)際中則不能發(fā)生，因?yàn)榇蟊凼菍?shí)體，不可能出現(xiàn)相交的情況，由于大臂的 長度有限，所以在共面的情況下，除了平行，另一個(gè)狀態(tài)則是不平行，有相交的趨勢(shì)。在這兩 種情況下，出現(xiàn)兩個(gè)堆取料機(jī)的大臂相碰撞的可能情形有以下三種一是第一堆取料機(jī)的 大臂的頭部碰到第二堆取料機(jī)的大臂的頭部，二是第一堆取料機(jī)的大臂的頭部碰到第二堆 取料機(jī)的大臂的上下左右側(cè)部，三是第二堆取料機(jī)的大臂的頭部碰到第一堆取料機(jī)的大臂 的上下左右側(cè)部。在共面的情況下，不可能出現(xiàn)兩個(gè)大臂的側(cè)部相碰。
所以共面情況下算法的原理是求出一個(gè)大臂的頭部中心到另一個(gè)大機(jī)回轉(zhuǎn)中心 和大臂頭部中心所構(gòu)成的線段的垂直距離，當(dāng)每個(gè)大臂頭部中心到另一個(gè)堆取料機(jī)的大機(jī) 回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段都沒有垂線時(shí)，可直接求出兩個(gè)大臂頭部中心之間 的最小距離，當(dāng)該最小距離小于第一預(yù)定距離而大于第二預(yù)定距離例如小于8m而大于: 時(shí)，這時(shí)將兩個(gè)堆取料機(jī)的行走、俯仰、回轉(zhuǎn)的速度降至低速運(yùn)行，并且可以發(fā)出警告；當(dāng)最 小距離小于第一預(yù)定距離例如小于:3m時(shí)，兩個(gè)堆取料機(jī)同時(shí)停止各種動(dòng)作，并發(fā)出碰撞報(bào)警。
如圖4(b)所示，在異面的情況下，求兩個(gè)大臂的最小距離可以采用兩條直線間計(jì) 算公垂線段長度的方法來實(shí)現(xiàn)，但是由于兩條大臂的長度有限，有可能不存在公垂線，這時(shí) 則需要將兩條大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段延長為兩條直線，求出直線的公 垂線和兩條大臂的交點(diǎn)pl、p2，首先檢查交點(diǎn)pi有沒有在第一堆取料機(jī)的大臂上，如果在 則選定Pl為第一個(gè)點(diǎn)，如果不在則選擇第一堆取料機(jī)的大臂頭部的點(diǎn)A作為第一點(diǎn)；然后 檢查P2點(diǎn)有沒有在第二堆取料機(jī)的大臂上，同理如果在，則作為第二個(gè)點(diǎn)，如果不在，則選 中第二堆取料機(jī)的大臂頭部的點(diǎn)C作為第二點(diǎn)。兩點(diǎn)選擇完畢后計(jì)算兩點(diǎn)之間的最小距 離，當(dāng)最小距離小于第一預(yù)定距離而大于第二預(yù)定距離例如小于8m而大于: 時(shí)，兩個(gè)堆取料機(jī)進(jìn)入慢速區(qū)間運(yùn)行并發(fā)出警告，當(dāng)最小距離小于第一預(yù)定距離例如小于: !時(shí)，兩個(gè)堆 取料機(jī)停止各種動(dòng)作，并進(jìn)行停機(jī)報(bào)警。
根據(jù)以上原理，可以按照如下過程來確定最小距離。
首先需要判斷A、B、C、D四個(gè)點(diǎn)是否在同一個(gè)平面即共面還是異面，
具體過程如下
計(jì)算A(xl，yl, zl)、B(x2，y2, z2)、C(x3，y3, z3)、D(x4，y4, z4)四個(gè)點(diǎn)構(gòu)成的四 個(gè)向量 AB(al，bl，cl)、AC(a2，b2，c2)、AD(a3，b3，c3)、CD(a4，b4，c4)分別為
AB :al = xl_x2 ；bl = yl-y2 ；cl = zl_z2
AC :a2 = xl_x3 ；b2 = yl-y3 ；c2 = zl_z3
AD :a3 = xl_x4 ；b3 = yl-y4 ；c3 = zl_z4
CD :a4 = x4_x3 ；b4 = y4-y3 ；c4 = z4_z3。
首先，需要判斷A、B、C、D四個(gè)點(diǎn)是否處于一個(gè)平面內(nèi)。根據(jù)四點(diǎn)共面定理，若三 個(gè)向量的混合積(AB，AC，AD) = 0，即如果 al*b2*c3+a2*b3*cl+a3*bl*c2-a3*b2*cl-al*b3 *c2-a2*bl*c3 = 0，則四點(diǎn)共面，否則，說明不共面即異面。
在判斷出共面的情況下，
如果
(a2*a4+b2*b4+c2*c4)/(sqrt(a2*a2+b2*b2+c2*c2)*sqrt(a4*a4+b4*b4+c4*c4) >=0，說明兩個(gè)向量之間的夾角為0-90°，則第一堆取料機(jī)的大臂頭部中心到第二堆取 料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段有垂線。其中sqrtO代表平方根運(yùn)算。
根據(jù)兩點(diǎn)確定一條直線原則，第二堆取料機(jī)的大臂的直線方程可以設(shè)為(x-x3)/ a4 = (y-y3) /b4 = (z_z3)/c4，
則過已知點(diǎn)A(xl，yl，zl)且與已知直線⑶相垂直的平面的方程為 a4 (x-xl)+b4(y-yl)+c4 (z-zl) = 0，可以通過以上兩個(gè)方程求出第二堆取料機(jī)的直線與垂 直平面的交點(diǎn)(x5，y5, z5)，所以，可以通過兩點(diǎn)之間的距離計(jì)算出第一堆取料機(jī)的大臂頭 部中心與第二堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的距離為
第一堆取料機(jī)的大臂頭部中心與第二堆取料機(jī)的大臂頭部中心距離，即點(diǎn)A、C之 間的距離為
Distl = sqrt((xl_x5)*(xl_x5) + (yl_y5)*(yl_y5) + (zl_z5)*(zl_z5))0
否則，如果
(a2*a4+b2*b4+c2*c4)/(sqrt(a2*a2+b2*b2+c2*c2)氺sqrt(a4*a4+b4*b4+c4*c4) < 0,則計(jì)算 Dist3 = sqrt (a2*a2+b2*b2+c2*c2)。
同理，如果
(al*a2+bl*b2+cl*c2) / (sqrt (al氺al+bl氺bl+cl氺cl)氺sqrt (a2*a2+b2*b2+c2*c2)) >=0，說明兩個(gè)向量之間的夾角為0-90°，則第二堆取料機(jī)的大臂頭部中心到第一堆取 料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段有垂線。
根據(jù)兩點(diǎn)確定一條直線原則，第一堆取料機(jī)的大臂的直線方程為(x-xl)/al = (y-yl)/bl = (z-zl)/cl，
則過已知點(diǎn)C(x3，y3，z3)且與已知直線AB相垂直的平面的方程為 al(x-x3)+bl(y-y3)+cl(z-z3) = 0，可以通過以上兩個(gè)方程求出第二堆取料機(jī)的直線與垂直平面的交點(diǎn)(x6，y6, z6)，所以，可以通過兩點(diǎn)之間的距離計(jì)算出第一堆取料機(jī)的大臂頭 部中心與第二堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的距離為
Dist2 = sqrt((x3_x6)*(x3_x6+(y3-y6)*(y3-y6)+(z3_z6)*(z3_z6))。
否則，如果<0，則計(jì)算第一堆取料機(jī)的大臂頭部中心與第二堆取料機(jī)的大臂頭部 中心距離為
Dist3 = sqrt(a2氺a2+b2氺b2+c2氺c2)。
然后選取上面所計(jì)算的三個(gè)距離Distl，Dist2，Dist3中最小的距離計(jì)算Mindist =MIN(Distl，Dist2，Dist3)為兩個(gè)堆取料機(jī)之間的最小距離。
在判斷異面的情況下
首先求出AB、⑶所在直線的公垂線與AB、⑶所在直線的交點(diǎn)即公垂點(diǎn)pi、p2，計(jì) 算過程如下
設(shè)兩根大臂直線的方程如下
第一堆取料機(jī)的大臂所在直線方程為(X-Xl)/al = (y_yl)/bl = (z~zl)/cl,
第二堆取料機(jī)的大臂所在直線方程為(x-x3) /a4 = (y-y3) /b4 = (z-z3) /c4。
設(shè) pi, p2 的坐標(biāo)分別為 pl(al*tl+xl，bl*tl+yl，cl*tl+zl)，p2(a4*t2+x3， b4*t2+y3，c4*t2+z3)；
因?yàn)閜i、p2點(diǎn)所在的直線垂直于AB所在直線、并垂直于⑶所在直線，所以
(a4*t2+x3-al*tl+xl, b4氺t2+y3_bl氺tl+yl, c4氺t2+z3_cl氺tl+zl)氺(al, bl, cl)= 0，
(a4*t2+x3-al*tl+xl, b4氺t2+y3_bl氺tl+yl, c4氺t2+z3_cl氺tl+zl)氺(a4, b4, c4)= 0，
由此可以求出11和12，從而可以求出ρ 1和p2兩點(diǎn)的坐標(biāo)ρ 1 (x7，y7，ζ7)，p2 (x8， y8, z8)。
然后判斷點(diǎn)pi和p2是否分別在線段AB和線段CD中，具體過程如下
如果同時(shí)滿足xl < x7 < x2 或 x2 < x7 < xl、并且 x3 < x8 < x4 或 x4 < x8 < x3， 表明都在，則此時(shí)兩公垂點(diǎn)Pl、p2之間的距離為兩個(gè)堆取料機(jī)之間的最小距離Mindist2 = sqrt((x8-x7)*(x8_x7+ (y8-y7)* (y8-y7) + (z8_z7)* (z8_z7))。
否則，如果滿足xl < x7 < x2 或 x2 < x7 < xl、或者 x3 < x8 < x4 或 x4 < x8 <x3，表明點(diǎn)pi在線段AB中或p2在線段⑶中，則大臂頭部中心與另一個(gè)公垂點(diǎn)的距離 為兩個(gè)堆取料機(jī)之間的最小距離。其中，如果僅滿足xl < x7 < x2或x2 < x7 < xl，表明 點(diǎn) P1 在線段 AB 中而 p2 不在線段 CD 中，則 Mindi st2 = sqrt ((x3_x7) * (x3_x7+ (y3_y7) * ( y3-y7) + (z3-z7)*(z3-z7))；如果僅滿足 x3 < x8 < x4 或 x4 < x8 < x3，表明點(diǎn) pi 不在線 段 AB 中而 p2 在線段 CD 中，則 Mindist2 = sqrt ((x8_xl) * (x8_xl+ (y8_yl) * (y8_yl) + (z8_ zl)*(z8-zl))。
如果都不在，則求出兩個(gè)大臂頭部中心的距離Mindist2 = sqrt ((x3-xl) * (x3_xl + (y3-yl)*(y3-y 1) + (z3_z 1)*(z3-zl))為兩個(gè)堆取料機(jī)之間的最小距離。
本發(fā)明還提供一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制方法，該方法包括根 據(jù)上述用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法判斷堆場中多個(gè)堆取料機(jī)的碰撞 可能性；以及根據(jù)可能性來控制堆取料機(jī)的動(dòng)作。
其中所述堆取料機(jī)的動(dòng)作可以包括例如大臂的旋轉(zhuǎn)、俯仰以及堆取料機(jī)的行走等寸。
優(yōu)選地，該用于堆場中多個(gè)堆取料機(jī)的防碰撞控制方法還可以包括當(dāng)判斷可能 性為較高時(shí)，控制所述堆取料機(jī)停機(jī)；當(dāng)判斷可能性為中等時(shí)；控制所述堆取料機(jī)減速，當(dāng) 判斷可能性較低時(shí)，可以控制堆取料機(jī)正常作業(yè)。從而可以在堆取料機(jī)之間有碰撞可能性 時(shí)，及時(shí)防止碰撞發(fā)生。
本發(fā)明還提供了一種用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)，該系統(tǒng) 包括基準(zhǔn)站501 ；多個(gè)流動(dòng)站502，與基準(zhǔn)站501通信，分別設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回 轉(zhuǎn)中心和該堆取料機(jī)的大臂頭部中心處，以檢測堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和堆取料機(jī)的大 臂頭部中心基于基準(zhǔn)站501的位置信息；處理單元503，用于接收來自每個(gè)流動(dòng)站502的位 置信息，根據(jù)對(duì)每個(gè)堆取料機(jī)檢測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料 機(jī)的大臂之間的最小距離，并且根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可 能性。從而能夠判斷出堆取料機(jī)之間發(fā)生碰撞的可能性，以便于工作人員根據(jù)該可能性進(jìn) 行下一步處理。
其中處理單元503可以由集成電路、PLC等實(shí)現(xiàn)，并且其可以通過多種方式來確定 兩個(gè)堆取料機(jī)的之間的距離，上面通過舉例描述的確定兩個(gè)堆取料機(jī)的之間的距離的計(jì)算 方法同樣適用于本發(fā)明所提供的用于堆取料機(jī)的防碰撞控制系統(tǒng)。處理單元503通過多種 方式接收來自流動(dòng)站502的位置信息，例如通過網(wǎng)絡(luò)如以太網(wǎng)等接收來自流動(dòng)站502的位直fe息。
其中兩個(gè)堆取料機(jī)之間的最小距離可以為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭 部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段之間的最小距離。
當(dāng)兩個(gè)堆取料機(jī)的大臂共面時(shí)，所述最小距離可以為一堆取料機(jī)的大臂頭部中心 到另一個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的垂直距離或兩個(gè)堆取 料機(jī)的大臂頭部中心之間的最小距離；當(dāng)兩個(gè)堆取料機(jī)的大臂異面時(shí)，所述最小距離可以 為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī)回轉(zhuǎn) 中心和大臂頭部中心所構(gòu)成的線段的公垂線段的長度或兩個(gè)堆取料機(jī)的大臂頭部中心之 間的最小距離。
優(yōu)選地，處理單元503還可以將所述最小距離與第一預(yù)定距離和第二預(yù)定距離進(jìn) 行比較；并且當(dāng)所述最小距離小于第一預(yù)定距離，處理單元503判斷可能性為較高；當(dāng)所述 最小距離小于第一預(yù)定距離而大于第二預(yù)定距離時(shí)，處理單元503判斷可能性為中等；當(dāng) 所述最小距離大于第二預(yù)定距離時(shí)，處理單元503判斷可能性為較低。
優(yōu)選地，所述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)還可以包括報(bào)警 單元504，該報(bào)警單元504與處理單元503耦合，并且當(dāng)處理單元503判斷可能性為較高時(shí)， 所述處理單元503控制所述報(bào)警單元504發(fā)出減速報(bào)警信號(hào)；當(dāng)判斷可能性為中等時(shí)，所述 處理單元503控制所述報(bào)警單元504發(fā)出碰撞報(bào)警信號(hào)；當(dāng)處理單元503判斷可能性為較 低時(shí)，所述處理單元503不控制所述報(bào)警單元504報(bào)警。
其中所述第一預(yù)定距離取值范圍可以3_6m，例如為3m，第二預(yù)定距離取值范圍可 以為6-20m，例如為8m。10
本發(fā)明還提供一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制系統(tǒng)，該系統(tǒng)可以包 括上述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)；以及控制單元，根據(jù)所述用 于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)判斷出的可能性來控制堆取料機(jī)的動(dòng)作。
其中所述堆取料機(jī)的動(dòng)作可以包括例如大臂的旋轉(zhuǎn)、俯仰以及堆取料機(jī)的行走等等。
優(yōu)選地，當(dāng)所述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)中的處理單元 503判斷可能性為較高時(shí)，所述控制單元可以控制所述堆取料機(jī)停機(jī)；當(dāng)處理單元503判斷 可能性為中等時(shí)，所述控制單元可以控制所述堆取料機(jī)減速；當(dāng)處理單元503判斷可能性 為較低時(shí)，所述堆取料機(jī)可以正常作業(yè)。從而可以在堆取料機(jī)之間有碰撞可能性時(shí)，及時(shí)防止碰撞發(fā)生。
根據(jù)本發(fā)明提供的用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法和系統(tǒng)以及 一種防碰撞控制方法和系統(tǒng)，解決了行走打滑的問題，而且不需要使用編碼器數(shù)據(jù)，降低了 誤差；同時(shí)，計(jì)算過程相對(duì)簡單、精度高，可以實(shí)現(xiàn)兩個(gè)堆取料機(jī)同場作業(yè)，并且能夠?qū)崟r(shí)檢 測兩大臂之間的最小距離，提高了安全性和作業(yè)效率，可以用于多種類型的堆取料機(jī)。
以上結(jié)合附圖詳細(xì)描述了本發(fā)明的優(yōu)選實(shí)施方式，但是，本發(fā)明并不限于上述實(shí) 施方式中的具體細(xì)節(jié)，在本發(fā)明的技術(shù)構(gòu)思范圍內(nèi)，可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行多種簡 單變型，這些簡單變型均屬于本發(fā)明的保護(hù)范圍。例如，雖然在上面的描述過程中將本發(fā)明 提供的用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)中的處理單元503和報(bào)警單元 504描述為兩個(gè)獨(dú)立的元件，但是也可以將二者集成在一個(gè)模塊中由單獨(dú)的元件實(shí)現(xiàn)。
另外需要說明的是，在上述具體實(shí)施方式
中所描述的各個(gè)具體技術(shù)特征，在不矛 盾的情況下，可以通過任何合適的方式進(jìn)行組合。為了避免不必要的重復(fù)，本發(fā)明對(duì)各種可 能的組合方式不再另行說明。
此外，本發(fā)明的各種不同的實(shí)施方式之間也可以進(jìn)行任意組合，只要其不違背本 發(fā)明的思想，其同樣應(yīng)當(dāng)視為本發(fā)明所公開的內(nèi)容。
權(quán)利要求
1.一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法，該方法包括檢測設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆取料機(jī)的大臂頭部中心的流動(dòng)站基 于基準(zhǔn)站的位置信息；根據(jù)對(duì)每個(gè)堆取料機(jī)檢測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料機(jī) 的大臂之間的最小距離；以及根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可能性。
2.根據(jù)權(quán)利要求1所述的方法，其中兩個(gè)堆取料機(jī)之間的最小距離為一堆取料機(jī)的 大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部 中心所構(gòu)成的線段之間的最小距離。
3.根據(jù)權(quán)利要求1或2所述的方法，其中當(dāng)兩個(gè)堆取料機(jī)的大臂共面時(shí)，所述最小距離 為一堆取料機(jī)的大臂頭部中心到另一個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成 的線段的垂直距離或兩個(gè)堆取料機(jī)的大臂頭部中心之間的最小距離；當(dāng)兩個(gè)堆取料機(jī)的大 臂異面時(shí)，所述最小距離為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與 另一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的公垂線段的長度或兩個(gè)堆 取料機(jī)的大臂頭部中心之間的最小距離。
4.根據(jù)權(quán)利要求1或2所述的方法，該方法還包括將所述最小距離與第一預(yù)定距離 和第二預(yù)定距離進(jìn)行比較；以及當(dāng)所述最小距離小于第一預(yù)定距離，判斷可能性為較高； 當(dāng)所述最小距離小于第一預(yù)定距離而大于第二預(yù)定距離時(shí)，判斷可能性為中等； 當(dāng)所述最小距離大于第二預(yù)定距離時(shí)，判斷可能性為較低。
5.根據(jù)權(quán)利要求4所述的方法，該方法還包括 當(dāng)判斷可能性為較高時(shí)，進(jìn)行碰撞報(bào)警； 當(dāng)判斷可能性為中等時(shí)，進(jìn)行減速報(bào)警。
6.根據(jù)權(quán)利要求4所述的方法，其中所述第一預(yù)定距離取值范圍為3-6m，第二預(yù)定距 離取值范圍為6-20m。
7.一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制方法，該方法包括根據(jù)權(quán)利要求1-6中任一項(xiàng)權(quán)利要求所述的方法判斷堆場中多個(gè)堆取料機(jī)的碰撞可 能性；以及根據(jù)可能性來控制堆取料機(jī)的動(dòng)作。
8.根據(jù)權(quán)利要求7所述的方法，其中所述堆取料機(jī)的動(dòng)作包括大臂的旋轉(zhuǎn)、俯仰以及 堆取料機(jī)的行走。
9.根據(jù)權(quán)利要求7或8所述的方法，該方法還包括 當(dāng)判斷可能性為較高時(shí)，控制所述堆取料機(jī)停機(jī)；當(dāng)判斷可能性為中等時(shí)，控制所述堆取料機(jī)減速。
10.一種用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)，該系統(tǒng)包括 基準(zhǔn)站(501)；多個(gè)流動(dòng)站(502)，與基準(zhǔn)站(501)通信，分別設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和 該堆取料機(jī)的大臂頭部中心處，以檢測堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和堆取料機(jī)的大臂頭部中 心基于基準(zhǔn)站(501)的位置信息；以及處理單元(503)，用于接收來自每個(gè)流動(dòng)站(502)的位置信息，根據(jù)對(duì)每個(gè)堆取料機(jī)檢 測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料機(jī)的大臂之間的最小距離，并且 根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可能性。
11.根據(jù)權(quán)利要求10所述的系統(tǒng)，其中兩個(gè)堆取料機(jī)之間的最小距離為一堆取料機(jī) 的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段、與另一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭 部中心所構(gòu)成的線段之間的最小距離。
12.根據(jù)權(quán)利要求10或11所述的系統(tǒng)，其中當(dāng)兩個(gè)堆取料機(jī)的大臂共面時(shí)，所述最 小距離為一堆取料機(jī)的大臂頭部中心到另一個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心 所構(gòu)成的線段的垂直距離或兩個(gè)堆取料機(jī)的大臂頭部中心之間的最小距離；當(dāng)兩個(gè)堆取料 機(jī)的大臂異面時(shí)，所述最小距離為一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線 段、與另一堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和大臂頭部中心所構(gòu)成的線段的公垂線段的長度或兩 個(gè)堆取料機(jī)的大臂頭部中心之間的最小距離。
13.根據(jù)權(quán)利要求10或11所述的系統(tǒng)，其中處理單元(503)將所述最小距離與第一預(yù) 定距離和第二預(yù)定距離進(jìn)行比較；并且當(dāng)所述最小距離小于第一預(yù)定距離，處理單元(503)判斷可能性為較高；當(dāng)所述最小距離小于第一預(yù)定距離而大于第二預(yù)定距離時(shí)，處理單元(503)判斷可能 性為中等；當(dāng)所述最小距離大于第二預(yù)定距離時(shí)，處理單元(503)判斷可能性為較低。
14.根據(jù)權(quán)利要求13所述的系統(tǒng)，該系統(tǒng)還包括報(bào)警單元(504)，該報(bào)警單元(504)與 處理單元(503)耦合，并且當(dāng)處理單元(50 判斷可能性為較高時(shí)，所述處理單元(50 控制所述報(bào)警單元(504) 發(fā)出減速報(bào)警信號(hào)；當(dāng)處理單元(50 判斷可能性為中等時(shí)，所述處理單元(50 控制所述報(bào)警單元(504)發(fā)出碰撞報(bào)警信號(hào)。
15.根據(jù)權(quán)利要求13所述的系統(tǒng)，其中所述第一預(yù)定距離取值范圍為3-6m，第二預(yù)定 距離取值范圍為6-20m。
16.一種用于堆場中多個(gè)堆取料機(jī)的防碰撞控制系統(tǒng)，該系統(tǒng)包括根據(jù)權(quán)利要求10-15中任一權(quán)利要求所述的系統(tǒng)；以及控制單元，根據(jù)所述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)判斷出的可能 性來控制堆取料機(jī)的動(dòng)作。
17.根據(jù)權(quán)利要求16所述的系統(tǒng)，其中所述堆取料機(jī)的動(dòng)作包括大臂的旋轉(zhuǎn)、俯仰以 及堆取料機(jī)的行走。
18.根據(jù)權(quán)利要求16或17所述的系統(tǒng)，其中當(dāng)所述用于預(yù)測堆場中多個(gè)堆取料機(jī)的碰撞可能性的系統(tǒng)中的處理單元(503)判斷 可能性為較高時(shí)，所述控制單元控制所述堆取料機(jī)停機(jī)；當(dāng)處理單元(50 判斷可能性為 中等時(shí)，所述控制單元控制所述堆取料機(jī)減速。
全文摘要
本發(fā)明公開了一種用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法和系統(tǒng)、以及一種防碰撞控制方法和系統(tǒng)，該用于預(yù)測堆場中多個(gè)堆取料機(jī)碰撞可能性的方法包括檢測設(shè)置在每個(gè)堆取料機(jī)的大機(jī)回轉(zhuǎn)中心和該堆取料機(jī)的大臂頭部中心的流動(dòng)站基于基準(zhǔn)站的位置信息；根據(jù)對(duì)每個(gè)堆取料機(jī)檢測到的所述位置信息確定多個(gè)堆取料機(jī)中任意兩個(gè)堆取料機(jī)的大臂之間的最小距離；以及根據(jù)該最小距離判斷該兩個(gè)堆取料機(jī)的大臂發(fā)生碰撞的可能性。從而能夠判斷出堆取料機(jī)之間發(fā)生碰撞的可能性，以便于工作人員根據(jù)該可能性進(jìn)行下一步處理。
文檔編號(hào)G01S19/42GK102033237SQ20101059213
公開日2011年4月27日 申請(qǐng)日期2010年12月16日 優(yōu)先權(quán)日2010年12月16日
發(fā)明者李長安, 李靖宇, 董傳博 申請(qǐng)人:中國神華能源股份有限公司, 神華黃驊港務(wù)有限責(zé)任公司