本發(fā)明涉及一種自主式探雷機器人系統(tǒng)及探雷方法，屬于探雷技術(shù)領(lǐng)域。

背景技術(shù)：

地雷是一種殺傷性較大的爆炸性武器，戰(zhàn)爭的參戰(zhàn)方一般均會使用大量的地雷，以此來削減敵方的軍事力量。但是由于地雷的濫布濫用，導(dǎo)致交戰(zhàn)國大片土地荒蕪，人民流離失所，嚴(yán)重影響了經(jīng)濟建設(shè)和人民的生活，因此探雷技術(shù)就成為這些交戰(zhàn)國迫切需要的技術(shù)。

申請?zhí)枮?01620688512.6的專利公開了一種適應(yīng)多場地的自平衡探雷車，其可以在多種場地中，完成各種情況下的探雷工作，保障了相關(guān)人員探雷工作的安全程度。但是該方案仍然存在以下問題：其僅僅公開了如果發(fā)現(xiàn)危險立刻停止，并發(fā)出報警，并未說明在探測到地雷后如何進(jìn)行處置，因而無法根據(jù)探雷情況實現(xiàn)快速拆雷。申請?zhí)枮?2100602.4的專利申請公開了一種地毯式快速探雷車，該方案在探測到地雷后直接引爆，對探雷車的破壞力非常大，直接導(dǎo)致探雷車的壽命大大縮減；另外，該探雷車的越障能力一般，導(dǎo)致使用范圍受限。此外，上述方案還存在以下問題：1、成本較高；2、探測設(shè)備的控制方式不夠靈活，且探測設(shè)備的探測面積大小完全取決于車體的移動速度大小，導(dǎo)致探雷車的探測范圍受限，且探雷效率較低。

另外，采用差分gps系統(tǒng)可以獲得機器人的經(jīng)緯度坐標(biāo)、海拔高度以及航向角，但是gps存在以下問題：1、gps系統(tǒng)數(shù)據(jù)的更新率不高，一般只能達(dá)到5hz，也就是200ms輸出一次數(shù)據(jù)，但是機器人系統(tǒng)要求更高的數(shù)據(jù)更新率；2、當(dāng)機器人進(jìn)入某些區(qū)域，如隧道、高樓之間時，可能會暫時失去gps信號，但還需要持續(xù)獲取位姿。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于，提供一種自主式探雷機器人系統(tǒng)及探雷方法，它可以有效解決現(xiàn)有技術(shù)中存在的問題，實現(xiàn)地雷的準(zhǔn)確、高效、無損探測和標(biāo)識，為后期排雷提供安全、可靠的保證。

為解決上述技術(shù)問題，本發(fā)明采用如下的技術(shù)方案：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)、機械臂探測系統(tǒng)和噴涂系統(tǒng)，移動底盤系統(tǒng)分別與機械臂探測系統(tǒng)和噴涂系統(tǒng)相連；所述的移動底盤系統(tǒng)包括移動平臺系統(tǒng)、動力系統(tǒng)和控制系統(tǒng)，機械臂探測系統(tǒng)包括機械臂運動系統(tǒng)和探測系統(tǒng)，其中，動力系統(tǒng)安裝于移動平臺系統(tǒng)的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)分別連接動力系統(tǒng)、械臂運動系統(tǒng)和探測系統(tǒng)；探測系統(tǒng)設(shè)于機械臂運動系統(tǒng)的前端。

優(yōu)選的，所述的噴涂系統(tǒng)包括伸縮機構(gòu)、擺臂機構(gòu)和噴涂機構(gòu)，伸縮機構(gòu)和擺臂機構(gòu)均與噴涂機構(gòu)連接；所述的噴涂機構(gòu)包括噴頭，從而達(dá)到噴涂系統(tǒng)扇形區(qū)域噴涂的效果(噴頭可達(dá)范圍是扇形的)。

更優(yōu)選的，所述的伸縮機構(gòu)包括：a電機、齒條和噴頭臂，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂連接，噴頭臂連接噴頭，不僅結(jié)構(gòu)簡單，成本低廉，而且可以靈活、準(zhǔn)確的控制噴涂機構(gòu)的伸出與收縮。

更優(yōu)選的，所述的擺臂機構(gòu)包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接，不僅結(jié)構(gòu)簡單，成本低廉，而且可以靈活、準(zhǔn)確的控制噴涂機構(gòu)的擺動。

更優(yōu)選的，所述的噴涂機構(gòu)還包括：空壓機以及噴漆泵，所述的空壓機與噴漆泵連接，噴漆泵與噴頭連接，從而可以使用油漆作為涂料進(jìn)行地雷的噴涂標(biāo)示；相較于利用液體罐中的液體如鋼筆墨水或利用涂料罐中的涂料粉末進(jìn)行標(biāo)示，本發(fā)明中采用油漆標(biāo)示更明顯，受外部環(huán)境影響更小，而且油漆不易進(jìn)入土壤，保持時間較久。

所述的噴漆泵可采用隔膜噴漆泵，從而可以實現(xiàn)在惡劣的環(huán)境中對地雷進(jìn)行更好的噴涂標(biāo)示，提高了噴涂效率，且使得噴涂的油漆保持的時間更長。

進(jìn)一步優(yōu)選的，所述的噴涂機構(gòu)還包括：二通電磁閥和/或三通電磁閥，所述的二通電磁閥分別與空壓機和噴頭連接，三通電磁閥分別與空壓機和噴頭連接，從而使得噴涂控制更加可靠、穩(wěn)定，而且三通電磁閥可以接兩個噴頭，每次可以噴更大面積，提高了噴涂效率，進(jìn)而提高了整個探雷機械裝置的探雷效率。

前述的自主式探雷機器人系統(tǒng)中，所述的機械臂運動系統(tǒng)采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)，另一端連接金屬機械臂；所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂或框架式機械臂，本發(fā)明中，所述的機械臂運動系統(tǒng)采用金屬機械臂與非金屬臂的組合臂，從而可以大大提高探測系統(tǒng)(如探地雷達(dá))的探測精度，降低干擾；此外，所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂，從而可以實現(xiàn)更靈活的控制探測設(shè)備，有效擴大探雷車的探測范圍和探測面積，從而進(jìn)一步提高了探雷效率；同時所述的金屬機械臂采用框架式機械臂，不僅結(jié)構(gòu)設(shè)計精巧簡單可靠，而且成本較低；另外，當(dāng)探測系統(tǒng)探測到地雷后，控制系統(tǒng)可以控制機械臂運動系統(tǒng)回收給噴涂機構(gòu)讓出空間，然后噴涂機構(gòu)伸出并前后擺動，使噴頭到達(dá)到地雷上方進(jìn)行噴涂標(biāo)示，從而可以有效縮小探雷裝置的體積，并且實現(xiàn)更加靈活、高效、準(zhǔn)確的控制噴涂系統(tǒng)進(jìn)行噴涂標(biāo)示；此外，所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂或框架結(jié)構(gòu)，從而可以起到較好的防塵效果，大大提高了探雷機械裝置的壽命。

本發(fā)明中，還包括設(shè)于移動底盤系統(tǒng)上的智能控制模塊和定位導(dǎo)航模塊，智能控制模塊分別與定位導(dǎo)航模塊和控制系統(tǒng)電連接，從而可以便于探雷機器人根據(jù)定位導(dǎo)航模塊提供的信息更合理的進(jìn)行路徑規(guī)劃。

優(yōu)選的，還包括遠(yuǎn)程控制系統(tǒng)及設(shè)于移動底盤系統(tǒng)上的遠(yuǎn)程通訊模塊，所述的遠(yuǎn)程通訊模塊分別與遠(yuǎn)程控制系統(tǒng)和智能控制模塊連接，探雷機器人通過遠(yuǎn)程通訊模塊與遠(yuǎn)程控制系統(tǒng)進(jìn)行數(shù)傳和圖傳通信，從而可以方便操作人員遠(yuǎn)距離實時觀察探雷機器人的工作狀態(tài)、檢測數(shù)據(jù)，并且遠(yuǎn)距離對探雷機器人的路徑規(guī)劃、行駛速度、工作狀態(tài)、避障等進(jìn)行控制，探測到地雷后噴涂標(biāo)示，從而可以實現(xiàn)更高效、高智能高精準(zhǔn)高可靠性的為后期排雷提供安全、可靠保證。

本發(fā)明中，所述的移動平臺系統(tǒng)為多履帶移動平臺，從而使得本發(fā)明的探雷機械裝置具有更好的越障能力，大大擴展了本發(fā)明的探雷機械裝置的應(yīng)用場合。

優(yōu)選的，所述的移動平臺系統(tǒng)包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括與至少一個第一主動輪連接的第二主動輪，所述的第二主動輪通過履帶與從動輪連接，從而使得移動平臺系統(tǒng)具有更好的支撐性能并為整個裝置提供更強的動力，使得本發(fā)明的探雷機械裝置具有更好的越障能力。

進(jìn)一步優(yōu)選的，所述的移動平臺系統(tǒng)包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括四個與第一主動輪一一對應(yīng)連接的第二主動輪(同軸連接)，所述的第二主動輪通過履帶與從動輪連接，從而可以進(jìn)一步提高探雷機器人的越障能力，使其適應(yīng)更復(fù)雜多變的環(huán)境。

前述的自主式探雷機器人系統(tǒng)中，還包括設(shè)于車體前后兩端的避障模塊，所述的避障模塊與智能控制模塊連接，從而實現(xiàn)探雷車在探雷過程中進(jìn)行自動避障，進(jìn)一步提高了探雷效率。

優(yōu)選的，所述的定位導(dǎo)航模塊包括：里程計、慣導(dǎo)和gps全球定位系統(tǒng)，所述的里程計、慣導(dǎo)和gps全球定位系統(tǒng)分別與智能控制模塊連接，里程計用于記錄相對位移，和慣導(dǎo)、gps全球定位系統(tǒng)一起實現(xiàn)室外高精度位置和姿態(tài)控制，從而可以獲得準(zhǔn)確的地理位置信息及周圍環(huán)境信息，實現(xiàn)更準(zhǔn)確、合理的路徑規(guī)劃以及更準(zhǔn)確的獲取地雷的位置，便于進(jìn)行標(biāo)識、排查。

優(yōu)選的，還包括：工業(yè)相機，所述的工業(yè)相機分別與遠(yuǎn)程通訊模塊和智能控制模塊連接，從而可以采集探雷機器人的周圍環(huán)境信息及探測系統(tǒng)周圍的環(huán)境信息，進(jìn)行監(jiān)控。

更優(yōu)選的，所述的工業(yè)相機包括至少3臺，分別設(shè)于機械臂運動系統(tǒng)上及車體的前后兩端，從而可以準(zhǔn)確的獲取探雷車周圍的環(huán)境信息及探測系統(tǒng)前端的路面情況信息，提供給后方排雷人員進(jìn)行監(jiān)控和記錄，另外也便于后期更準(zhǔn)確的排雷。

本發(fā)明中，所述的智能控制模塊采用一臺或多臺工控機，尤其是采用多臺工控機，可以將各種控制算法分別運行在不同的工控機上，提高了運行效率和通訊速度，例如，多臺工業(yè)相機的大量圖片數(shù)據(jù)可以單獨在一臺工控機上處理；另外，不同的工控機可以運行不同的操作系統(tǒng)(如linux和windows)，以適應(yīng)不同的設(shè)備驅(qū)動。

優(yōu)選的，所述的避障模塊由兩個單線式激光雷達(dá)組成；采用兩個激光雷達(dá)，分別裝在機器人的前部和后部，從而可以覆蓋機器人四周全部的環(huán)境，使得探雷機器人可以獲知前后左右附近所有的障礙，無論前進(jìn)還是后退時都可以避障；同時降低了系統(tǒng)成本。

前述的自主式探雷機器人系統(tǒng)中，所述的遠(yuǎn)程控制系統(tǒng)包括顯示屏、人機交互模塊和遠(yuǎn)程控制模塊，所述的遠(yuǎn)程控制模塊分別與顯示屏、人機交互模塊和遠(yuǎn)程通訊模塊連接，從而便于操作人員可以從遠(yuǎn)程控制系統(tǒng)實時觀察探雷機器人的工作狀態(tài)、檢測數(shù)據(jù)及多個相機拍攝的視頻，也可以便于操作人員手動控制探雷機器人進(jìn)行避障。

優(yōu)選的，所述的遠(yuǎn)程控制系統(tǒng)可采用背負(fù)式手提箱遙控器，從而可以降低系統(tǒng)成本，而且該背負(fù)式手提箱遙控器體積小，重量輕，便于攜帶，適合復(fù)雜地形的野外作業(yè)。

利用前述系統(tǒng)進(jìn)行探雷的方法，包括以下步驟：

s1，控制車體移動至道面指定的位置；

s2，計算待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)，并(可通過遠(yuǎn)程控制系統(tǒng))設(shè)定探測區(qū)域；

s3，對待探測區(qū)域進(jìn)行“z”字形探測路徑自主規(guī)劃；

s4，控制車體沿規(guī)劃路徑行進(jìn)進(jìn)行探雷(可同時通過遠(yuǎn)程通訊模塊回傳探測系統(tǒng)的探測數(shù)據(jù)及定位導(dǎo)航模塊拍攝的視頻信息)；當(dāng)機械臂探測系統(tǒng)探測到地雷后，車體停下，中斷探雷工作，記錄地雷位置，并計算噴涂機構(gòu)的運動軌跡；機械臂運動系統(tǒng)收回，并控制噴涂系統(tǒng)對地雷點進(jìn)行噴涂標(biāo)示；

s5，對地雷點標(biāo)示后，車體按照規(guī)劃路徑繼續(xù)探測工作直至探測完整個待探測區(qū)域，或者對地雷點標(biāo)示后，車體返回原點，待排雷后繼續(xù)規(guī)劃探測。

優(yōu)選的，步驟s2中所述的計算待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)包括以下步驟：

s21，選取待探測區(qū)域中的不在同一直線上的三個點(一般選取區(qū)域邊緣的點)，分別(可通過gps)獲取這三個點的經(jīng)緯度坐標(biāo)及機器人本體的經(jīng)緯度坐標(biāo)和航向角(經(jīng)緯度坐標(biāo)是顯示給操作者看，或者操作者下達(dá)指令用)；

s22，建立世界直角坐標(biāo)系(機器人移動時采用的是世界直角坐標(biāo)系)：以其中一個點為原點，另外兩個點中的一個點與原點之間的直線作為世界直角坐標(biāo)系的x軸；在地平面中垂直x軸且經(jīng)過原點的直線作為世界直角坐標(biāo)系的y軸；垂直地平面且向上的矢量作為世界直角坐標(biāo)系的z軸；

s23，通過坐標(biāo)系轉(zhuǎn)換獲得待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)以及機器人本體在世界直角坐標(biāo)系中的位置直角坐標(biāo)和姿態(tài)角。

每次開始工作之前都更新一次世界直角坐標(biāo)系，能保證整個工作中的位置精度；且僅采用三個點就能確定一個世界直角坐標(biāo)系。選定世界直角坐標(biāo)系后，機器人所有的運動操作全部都映射到這個坐標(biāo)系中執(zhí)行，簡化了計算，提高了探雷效率。

優(yōu)選的，步驟s3包括以下步驟：

s31，將待探測區(qū)域在世界直角坐標(biāo)系中對應(yīng)的設(shè)置為矩形(如果人為設(shè)定為不規(guī)則多邊形，則機器人自動將其填充為包含所設(shè)定區(qū)域的矩形)；

s32，在世界直角坐標(biāo)系中對該矩形進(jìn)行“z”字形探測路徑自主規(guī)劃，直到行走區(qū)域覆蓋整個矩形平面。

本發(fā)明中將用戶設(shè)定的不同形狀的探測區(qū)域統(tǒng)一轉(zhuǎn)化為矩形，從而簡化了問題的復(fù)雜度，提高了路徑自主規(guī)劃的效率。

優(yōu)選的，步驟s4中所述的計算噴涂機構(gòu)的運動軌跡包括以下步驟：

s41，建立機器人坐標(biāo)系(噴涂需要機器人坐標(biāo)系和工具坐標(biāo)系)：以機器人本體幾何中心為原點，以機器人正前方作為機器人坐標(biāo)系y軸正方向，以機器人正右方作為機器人坐標(biāo)系x軸正方向；

s42，獲取地雷在探測系統(tǒng)(如地雷探測雷達(dá))的覆蓋區(qū)域中的相對位置；

s43，根據(jù)探測系統(tǒng)和車體之間的固定位置關(guān)系，由坐標(biāo)平移換算得到地雷在機器人坐標(biāo)系中的坐標(biāo)；

s44，建立噴涂工具極坐標(biāo)系：將擺臂機構(gòu)的擺角頂點作為原點，以機器人坐標(biāo)系y軸作為噴涂工具極坐標(biāo)系x軸，極坐標(biāo)系平面與機器人坐標(biāo)系平面平行；

s45，根據(jù)擺臂機構(gòu)和車體之間的固定位置關(guān)系，獲得噴涂工具極坐標(biāo)系與機器人坐標(biāo)系的轉(zhuǎn)化關(guān)系矩陣，進(jìn)而獲得地雷在噴涂工具極坐標(biāo)系中的坐標(biāo)(ρ，θ)；

s46，所述的地雷在噴涂工具極坐標(biāo)系中的坐標(biāo)即為擺臂機構(gòu)的擺角θ及擺臂需要伸出的長度ρ。

通過采用上述方法，則在噴涂時，不需要世界直角坐標(biāo)系，也不需要定位導(dǎo)航系統(tǒng)，僅知道地雷在雷達(dá)覆蓋區(qū)域的位置就可以實現(xiàn)精準(zhǔn)定位噴涂了，噴涂效率較高。

更優(yōu)選的，還包括：根據(jù)慣導(dǎo)及里程計輸出的位置偏移和姿態(tài)角偏移信息，對步驟s23中已獲取的機器人本體在世界直角坐標(biāo)系中的位置直角坐標(biāo)和姿態(tài)角進(jìn)行數(shù)據(jù)融合修正，得到更為精準(zhǔn)的機器人本體的位置直角坐標(biāo)和姿態(tài)角；具體包括以下步驟：

s231，利用慣導(dǎo)輸出三軸加速度和三軸角加速度，對所述的三軸加速度和三軸角加速度進(jìn)行積分得三軸線速度和三軸角速度；

s232，對所述的三軸線速度進(jìn)行積分，得三軸方向的位置偏移；對z軸角速度進(jìn)行積分，得姿態(tài)角偏移；里程計輸出姿態(tài)角方向上的位置偏移，所述的姿態(tài)角方向上的位置偏移在世界直角坐標(biāo)系中線性分解為xy軸兩個方向的位置偏移；

s233，將gps數(shù)據(jù)計算出的機器人本體的直角坐標(biāo)與慣導(dǎo)、里程計計算出的機器人本體的位置偏移采用卡爾曼濾波器進(jìn)行融合(從而可以輸出更為精確的機器人本體的位置直角坐標(biāo)數(shù)據(jù))；將gps計算出的機器人本體的姿態(tài)角與慣導(dǎo)計算出的機器人本體的姿態(tài)角偏移采用卡爾曼濾波器進(jìn)行融合(從而可以輸出機器人本體更為精確的姿態(tài)角數(shù)據(jù))。

與現(xiàn)有技術(shù)相比，本發(fā)明通過利用移動底盤系統(tǒng)搭載機械臂探測系統(tǒng)和噴涂系統(tǒng)對雷區(qū)雷點進(jìn)行智能、準(zhǔn)確、高效、無損探測并標(biāo)示，可替代高強度高危險人工探測工作，從而為后期排雷提供安全、可靠的保證；另外，本發(fā)明采用無損檢測技術(shù)探測地雷，安全性好，無論對機器人還是操作人員都沒有傷害；而且本發(fā)明的探雷機器人進(jìn)行地雷標(biāo)識，壽命較長。此外，本發(fā)明提出的探雷技術(shù)，自動化程度高，從探測開始到發(fā)現(xiàn)、標(biāo)記地雷，均不需要人員參與操作，人員只需要在遠(yuǎn)處監(jiān)控機器人的工作狀態(tài)即可，既能提高工作效率，又能減輕排雷人員的工作負(fù)擔(dān)。另外，本發(fā)明中，所述的機械臂運動系統(tǒng)采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)，另一端連接金屬機械臂；所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂或框架結(jié)構(gòu)，從而可以實現(xiàn)更靈活的控制探測設(shè)備，有效擴大探雷車的探測范圍和探測面積，從而進(jìn)一步提高了探雷效率；同時所述的機械臂運動系統(tǒng)采用金屬機械臂與非金屬臂的組合臂，可以大大提高探測系統(tǒng)(如探地雷達(dá))的探測精度，降低干擾；另外，當(dāng)探測系統(tǒng)探測到地雷后，控制系統(tǒng)可以控制機械臂運動系統(tǒng)回收給噴涂機構(gòu)讓出空間，然后噴涂機構(gòu)伸出并前后擺動，使噴頭到達(dá)到地雷上方進(jìn)行噴涂標(biāo)示，從而可以有效縮小探雷裝置的體積，并且實現(xiàn)更加靈活、高效、準(zhǔn)確的控制噴涂系統(tǒng)進(jìn)行噴涂標(biāo)示；此外，所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂或框架式機械臂，從而可以起到較好的防塵效果，大大提高了探雷機械裝置的壽命。此外，本發(fā)明中，所述的移動平臺系統(tǒng)采用多履帶移動平臺，從而使得本發(fā)明的探雷機械裝置具有更好的越障能力，大大擴展了本發(fā)明的探雷機械裝置的應(yīng)用場合，可以適用于各種環(huán)境和地形；特別是所述的移動平臺系統(tǒng)包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括四個與第一主動輪一一對應(yīng)連接的第二主動輪(同軸連接)，所述的第二主動輪通過履帶與從動輪連接，從而使得移動平臺系統(tǒng)具有更好的支撐性能并為整個裝置提供更強的動力，使得本發(fā)明的探雷機械裝置具有更好的越障能力。通過利用本發(fā)明的探雷方法，尤其是對待探測區(qū)域進(jìn)行“z”字形探測路徑自主規(guī)劃并控制車體沿規(guī)劃路徑行進(jìn)，從而可以實現(xiàn)地毯式的探雷，防止遺漏，提高了探測的精確性。此外，本發(fā)明中，所述的伸縮機構(gòu)包括：a電機、齒條和噴頭臂，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂連接，噴頭臂連接噴頭，不僅結(jié)構(gòu)簡單，成本低廉，而且可以靈活、準(zhǔn)確的控制噴涂機構(gòu)的伸出與收縮；所述的擺臂機構(gòu)包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接，不僅結(jié)構(gòu)簡單，成本低廉，而且可以靈活、準(zhǔn)確的控制噴涂機構(gòu)的擺動。最后，本發(fā)明采用將gps、慣導(dǎo)以及里程計的數(shù)據(jù)進(jìn)行融合，從而可以獲得更高精度、更高數(shù)據(jù)刷新率的位姿數(shù)據(jù)。

附圖說明

圖1是本發(fā)明的探雷機器人系統(tǒng)的整體結(jié)構(gòu)示意圖；

圖2是移動底盤系統(tǒng)的結(jié)構(gòu)示意圖；

圖3是雙履帶移動平臺的結(jié)構(gòu)示意圖；

圖4是輪式移動平臺的結(jié)構(gòu)示意圖；

圖5是多履帶移動平臺的一種結(jié)構(gòu)示意圖；

圖6是多履帶移動平臺的另一種結(jié)構(gòu)示意圖；

圖7是機械臂探測系統(tǒng)的結(jié)構(gòu)示意圖；

圖8是噴涂系統(tǒng)的結(jié)構(gòu)示意圖；

圖9是機械臂運動系統(tǒng)收回的示意圖；

圖10是噴涂系統(tǒng)伸出的主視圖；

圖11是噴涂系統(tǒng)伸出擺臂中間位置的俯視圖；

圖12是噴涂系統(tǒng)伸出擺臂前后位置的俯視圖；

圖13為噴涂機構(gòu)的一種實現(xiàn)方式的示意圖；

圖14為噴涂機構(gòu)的另一種實現(xiàn)方式的示意圖；

圖15為機械臂運動系統(tǒng)中金屬機械臂采用雙關(guān)節(jié)機械臂的示意圖；

圖16為多關(guān)節(jié)機械臂示意圖；

圖17為連桿機構(gòu)式機械臂示意圖；

圖18為金屬框架式機械臂示意圖；

圖19為本發(fā)明的電路連接框圖；

圖20是對待探測區(qū)域進(jìn)行“z”字形探測路徑自主規(guī)劃的示意圖；

圖21是機器人坐標(biāo)系和工具坐標(biāo)系的關(guān)系示意圖；

圖22是將航向角轉(zhuǎn)換成世界直角坐標(biāo)系中的姿態(tài)角的示意圖。

附圖標(biāo)記：1-遠(yuǎn)程控制系統(tǒng)，2-移動底盤系統(tǒng)，3-機械臂探測系統(tǒng)，4-噴涂系統(tǒng)，5-移動平臺系統(tǒng)，6-動力系統(tǒng)，7-控制系統(tǒng)，8-機械臂運動系統(tǒng)，9-探測系統(tǒng)，10-伸縮機構(gòu)，11-擺臂機構(gòu)，12-噴涂機構(gòu)，13-空壓機，14-噴頭，15-噴漆泵，16-涂料罐，18-二通電磁閥，19-三通電磁閥，20-水泵，21-液體罐，22-第二主動輪，23-從動輪，24-噴頭臂，25-避障模塊，26-工業(yè)相機，27-里程計，28-慣導(dǎo)，29-gps全球定位系統(tǒng)，30-顯示屏，31-人機交互模塊，32-遠(yuǎn)程控制模塊，33-定位導(dǎo)航模塊，34-智能控制模塊，35-遠(yuǎn)程通訊模塊。

下面結(jié)合附圖和具體實施方式對本發(fā)明作進(jìn)一步的說明。

具體實施方式

本發(fā)明的實施例1：一種自主式探雷機器人系統(tǒng)，如圖1所示，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；如圖2所示，所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，如圖7、圖9所示，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。如圖8、圖10～圖12所示，所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的伸縮機構(gòu)10包括：a電機、齒條和噴頭臂24，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂24連接，噴頭臂24連接噴頭14。所述的擺臂機構(gòu)11包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接。如圖13所示，所述的噴涂機構(gòu)12還包括：空壓機13以及噴漆泵15，所述的空壓機13與噴漆泵15連接，噴漆泵15與噴頭14連接。所述的噴涂機構(gòu)12還包括：二通電磁閥18和/或三通電磁閥19，所述的二通電磁閥18分別與空壓機13和噴頭14連接，三通電磁閥19分別與空壓機13和噴頭14連接。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用多關(guān)節(jié)機械臂(如圖16所示)。如圖19所示，還包括設(shè)于移動底盤系統(tǒng)2上的智能控制模塊34和定位導(dǎo)航模塊33，智能控制模塊34分別與定位導(dǎo)航模塊33和控制系統(tǒng)7電連接。還包括遠(yuǎn)程控制系統(tǒng)1及設(shè)于移動底盤系統(tǒng)2上的遠(yuǎn)程通訊模塊35，所述的遠(yuǎn)程通訊模塊35分別與遠(yuǎn)程控制系統(tǒng)1和智能控制模塊34連接。所述的移動平臺系統(tǒng)5采用多履帶移動平臺。所述的移動平臺系統(tǒng)5包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括與至少一個第一主動輪連接的第二主動輪22，所述的第二主動輪22通過履帶與從動輪23連接(比如如圖5、圖6所示，具體可以為：所述的移動平臺系統(tǒng)5包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括四個與第一主動輪一一對應(yīng)連接的第二主動輪22(同軸連接)，所述的第二主動輪22通過履帶與從動輪23連接?；蛘咚龅囊苿悠脚_系統(tǒng)5包括：分別設(shè)于兩側(cè)的第一主動輪，每側(cè)設(shè)有兩個，且每側(cè)的兩個第一主動輪通過履帶連接；還包括與每側(cè)其中一個第一主動輪連接的第二主動輪22，所述的第二主動輪22通過履帶與從動輪23連接；所述的兩個第二主動輪22位于同一端)。還包括設(shè)于車體前后兩端的避障模塊25，所述的避障模塊25與智能控制模塊34連接。所述的定位導(dǎo)航模塊33包括：里程計27、慣導(dǎo)28和gps全球定位系統(tǒng)29，所述的里程計27、慣導(dǎo)28和gps全球定位系統(tǒng)29分別與智能控制模塊34連接。還包括：工業(yè)相機26，所述的工業(yè)相機26分別與遠(yuǎn)程通訊模塊35和智能控制模塊34連接。所述的工業(yè)相機26包括至少3臺，分別設(shè)于機械臂運動系統(tǒng)8上及車體的前后兩端。所述的智能控制模塊34采用一臺或多臺工控機。所述的避障模塊25由兩個單線式激光雷達(dá)組成。所述的遠(yuǎn)程控制系統(tǒng)1包括顯示屏30、人機交互模塊31和遠(yuǎn)程控制模塊32，所述的遠(yuǎn)程控制模塊32分別與顯示屏30、人機交互模塊31和遠(yuǎn)程通訊模塊35連接。所述的遠(yuǎn)程控制系統(tǒng)1可采用背負(fù)式手提箱遙控器。

實施例2：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。還包括設(shè)于移動底盤系統(tǒng)2上的智能控制模塊34和定位導(dǎo)航模塊33，智能控制模塊34分別與定位導(dǎo)航模塊33和控制系統(tǒng)7電連接。還包括遠(yuǎn)程控制系統(tǒng)1及設(shè)于移動底盤系統(tǒng)2上的遠(yuǎn)程通訊模塊35，所述的遠(yuǎn)程通訊模塊35分別與遠(yuǎn)程控制系統(tǒng)1和智能控制模塊34連接。如圖3所示，所述的移動平臺系統(tǒng)5可采用雙履帶移動平臺。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用雙關(guān)節(jié)機械臂(如圖15所示)。

實施例3：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用雙關(guān)節(jié)機械臂或多關(guān)節(jié)機械臂。如圖4所示，所述的移動平臺系統(tǒng)5可采用輪式移動平臺(包括普通四輪和全向輪)。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用連桿機構(gòu)(如圖17所示)。

實施例4：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的噴涂機構(gòu)12還包括：空壓機13以及噴漆泵15，所述的空壓機13與噴漆泵15連接，噴漆泵15與噴頭14連接。所述的噴涂機構(gòu)12還包括：二通電磁閥18和/或三通電磁閥19，所述的二通電磁閥18分別與空壓機13和噴頭14連接，三通電磁閥19分別與空壓機13和噴頭14連接。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用金屬框架式機械臂(如圖18所示)。

實施例5：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的擺臂機構(gòu)11包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接。

實施例6：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的伸縮機構(gòu)10包括：a電機、齒條和噴頭臂24，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂24連接，噴頭臂24連接噴頭14。

實施例7：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的伸縮機構(gòu)10可采用現(xiàn)有的方式實現(xiàn)，比如：電動推桿(成本高)。所述的擺臂機構(gòu)11可采用現(xiàn)有的方式實現(xiàn)，比如：電動推桿(成本高)。如圖14所示，所述的噴涂機構(gòu)12還包括：水泵20和液體罐21，所述的水泵20分別與液體罐21和噴頭14連接。或者所述的噴涂機構(gòu)12還可以利用以下方式實現(xiàn)：包括空壓機13和涂料罐16，涂料罐16分別與空壓機13和噴頭14連接。

實施例8：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。

實施例9：一種自主式探雷機器人系統(tǒng)，包括車體，所述車體上設(shè)有移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4，移動底盤系統(tǒng)2分別與機械臂探測系統(tǒng)3和噴涂系統(tǒng)4相連；所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7，機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，其中，動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個裝置提供動力源并做配重用；控制系統(tǒng)7分別連接動力系統(tǒng)6、械臂運動系統(tǒng)8和探測系統(tǒng)9；探測系統(tǒng)9設(shè)于機械臂運動系統(tǒng)8的前端。所述的噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12，伸縮機構(gòu)10和擺臂機構(gòu)11均與噴涂機構(gòu)12連接；所述的噴涂機構(gòu)12包括噴頭14。所述的伸縮機構(gòu)10包括：a電機、齒條和噴頭臂24，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂24連接，噴頭臂24連接噴頭14。所述的擺臂機構(gòu)11包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接。所述的噴涂機構(gòu)12還包括：空壓機13以及噴漆泵15，所述的空壓機13與噴漆泵15連接，噴漆泵15與噴頭14連接。所述的噴涂機構(gòu)12還包括：二通電磁閥18和/或三通電磁閥19，所述的二通電磁閥18分別與空壓機13和噴頭14連接，三通電磁閥19分別與空壓機13和噴頭14連接。所述的機械臂運動系統(tǒng)8采用金屬機械臂與非金屬臂的組合臂，所述的非金屬臂的一端連接探測系統(tǒng)9，另一端連接金屬機械臂；所述的金屬機械臂采用雙關(guān)節(jié)機械臂、多關(guān)節(jié)機械臂或框架式機械臂。還包括設(shè)于移動底盤系統(tǒng)2上的智能控制模塊34和定位導(dǎo)航模塊33，智能控制模塊34分別與定位導(dǎo)航模塊33和控制系統(tǒng)7電連接。還包括遠(yuǎn)程控制系統(tǒng)1及設(shè)于移動底盤系統(tǒng)2上的遠(yuǎn)程通訊模塊35，所述的遠(yuǎn)程通訊模塊35分別與遠(yuǎn)程控制系統(tǒng)1和智能控制模塊34連接。

利用實施例1～實施例9所述系統(tǒng)進(jìn)行探雷的方法，包括以下步驟：

s1，控制車體移動至道面指定的位置；

s2，計算待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)，并設(shè)定探測區(qū)域；

s3，對待探測區(qū)域進(jìn)行“z”字形探測路徑自主規(guī)劃；

s4，控制車體沿規(guī)劃路徑行進(jìn)進(jìn)行探雷；當(dāng)機械臂探測系統(tǒng)探測到地雷后，車體停下，中斷探雷工作，記錄地雷位置，并計算噴涂機構(gòu)的運動軌跡；機械臂運動系統(tǒng)收回，并控制噴涂系統(tǒng)對地雷點進(jìn)行噴涂標(biāo)示；

s5，對地雷點標(biāo)示后，車體按照規(guī)劃路徑繼續(xù)探測工作直至探測完整個待探測區(qū)域，或者對地雷點標(biāo)示后，車體返回原點，待排雷后繼續(xù)規(guī)劃探測。

步驟s2中所述的計算待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)包括以下步驟：

s21，選取待探測區(qū)域中的不在同一直線上的三個點(一般選取區(qū)域邊緣的點)，分別(可通過gps)獲取這三個點的經(jīng)緯度坐標(biāo)及機器人本體的經(jīng)緯度坐標(biāo)和航向角(經(jīng)緯度坐標(biāo)是顯示給操作者看，或者操作者下達(dá)指令用)；

s22，建立世界直角坐標(biāo)系(機器人移動時采用的是世界直角坐標(biāo)系)：以其中一個點為原點，另外兩個點中的一個點與原點之間的直線作為世界直角坐標(biāo)系的x軸；在地平面中垂直x軸且經(jīng)過原點的直線作為世界直角坐標(biāo)系的y軸；垂直地平面且向上的矢量作為世界直角坐標(biāo)系的z軸；

s23，通過坐標(biāo)系轉(zhuǎn)換獲得待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)以及機器人本體在世界直角坐標(biāo)系中的位置直角坐標(biāo)和姿態(tài)角。

步驟s23中，可以通過以下方式將航向角轉(zhuǎn)換成世界直角坐標(biāo)系中的姿態(tài)角：

如圖22所示，機器人實際航向角是正北方，真航向角0°，世界直角坐標(biāo)系x軸指向n60e，真航向角60°，y軸指向n30w，真航向角330°，從圖中不難看出，機器人指向方向與世界直角坐標(biāo)系x軸的夾角是60°，所以機器人在世界直角坐標(biāo)系中的姿態(tài)角就是60°。

假設(shè)世界直角坐標(biāo)系x軸的真航向角是a°，機器人的真航向角是r°，那么機器人在世界直角坐標(biāo)系中的姿態(tài)角可由a-r計算得出。

步驟s3包括以下步驟：

s31，將待探測區(qū)域在世界直角坐標(biāo)系中對應(yīng)的設(shè)置為矩形(如果人為設(shè)定為不規(guī)則多邊形，則機器人自動將其填充為包含所設(shè)定區(qū)域的矩形)；

s32，在世界直角坐標(biāo)系中對該矩形進(jìn)行“z”字形探測路徑自主規(guī)劃，直到行走區(qū)域覆蓋整個矩形平面。

步驟s4中所述的計算噴涂機構(gòu)的運動軌跡包括以下步驟，如圖21所示：

s41，建立機器人坐標(biāo)系(噴涂需要機器人坐標(biāo)系和工具坐標(biāo)系)：以機器人本體幾何中心為原點，以機器人正前方作為機器人坐標(biāo)系y軸正方向，以機器人正右方作為機器人坐標(biāo)系x軸正方向；

s42，獲取地雷在探測系統(tǒng)(9)(如地雷探測雷達(dá))的覆蓋區(qū)域中的相對位置；

s43，根據(jù)探測系統(tǒng)(9)和車體之間的固定位置關(guān)系，由坐標(biāo)平移換算得到地雷在機器人坐標(biāo)系中的坐標(biāo)；

s44，建立噴涂工具極坐標(biāo)系：將擺臂機構(gòu)的擺角頂點作為原點，以機器人坐標(biāo)系y軸作為噴涂工具極坐標(biāo)系x軸，極坐標(biāo)系平面與機器人坐標(biāo)系平面平行；

s45，根據(jù)擺臂機構(gòu)和車體之間的固定位置關(guān)系，獲得噴涂工具極坐標(biāo)系與機器人坐標(biāo)系的轉(zhuǎn)化關(guān)系矩陣，進(jìn)而獲得地雷在噴涂工具極坐標(biāo)系中的坐標(biāo)(ρ，θ)；

s46，所述的地雷在噴涂工具極坐標(biāo)系中的坐標(biāo)即為擺臂機構(gòu)的擺角θ及擺臂需要伸出的長度ρ。

噴涂時，首先，控制擺臂機構(gòu)的電機轉(zhuǎn)動，使得擺臂角度等于地雷極坐標(biāo)角度，然后控制伸縮機構(gòu)的電機轉(zhuǎn)動，伸出噴頭，使得擺臂長度等于地雷極坐標(biāo)矢量長度，即可使得噴頭達(dá)到地雷正上方，然后打開電磁閥門進(jìn)行噴涂(如噴漆)。

可選的，還包括：根據(jù)慣導(dǎo)及里程計輸出的位置偏移和姿態(tài)角偏移信息，對步驟s23中已獲取的機器人本體在世界直角坐標(biāo)系中的位置直角坐標(biāo)和姿態(tài)角進(jìn)行數(shù)據(jù)融合修正，得到更為精準(zhǔn)的機器人本體的位置直角坐標(biāo)和姿態(tài)角；具體包括以下步驟：

s231，利用慣導(dǎo)輸出三軸加速度和三軸角加速度，對所述的三軸加速度和三軸角加速度進(jìn)行積分得三軸線速度和三軸角速度；

s232，對所述的三軸線速度進(jìn)行積分，得三軸方向的位置偏移；對z軸角速度進(jìn)行積分，得姿態(tài)角偏移；里程計輸出姿態(tài)角方向上的位置偏移，所述的姿態(tài)角方向上的位置偏移在世界直角坐標(biāo)系中線性分解為xy軸兩個方向的位置偏移；

s233，將gps數(shù)據(jù)計算出的機器人本體的直角坐標(biāo)與慣導(dǎo)、里程計計算出的機器人本體的位置偏移采用卡爾曼濾波器進(jìn)行融合(從而可以輸出更為精確的機器人本體的位置直角坐標(biāo)數(shù)據(jù))；將gps計算出的機器人本體的姿態(tài)角與慣導(dǎo)計算出的機器人本體的姿態(tài)角偏移采用卡爾曼濾波器進(jìn)行融合(從而可以輸出機器人本體更為精確的姿態(tài)角數(shù)據(jù))。

位姿包括位置和姿態(tài)，位置即機器人在平面上運動的坐標(biāo)，因為機器人只在地面運動，所以姿態(tài)只需要航向角即可。

本發(fā)明的一種實施例的工作原理：

本發(fā)明的探雷機器人系統(tǒng)，由智能控制模塊34對移動底盤系統(tǒng)2、機械臂探測系統(tǒng)3和噴涂系統(tǒng)4進(jìn)行控制，包括移動平臺系統(tǒng)5的移動路徑、速度、方向，機械臂運動系統(tǒng)8的前伸與回收，噴涂機構(gòu)12的伸縮、擺臂、噴涂標(biāo)示等。

具體的說，本發(fā)明利用遠(yuǎn)程控制系統(tǒng)1中的遠(yuǎn)程控制模塊32通過遠(yuǎn)程通訊模塊35遠(yuǎn)程控制車體上的智能控制模塊34或者近距離直接操作智能控制模塊34，控制車體的移動底盤系統(tǒng)2移動至道面指定的位置；利用gps計算待探測區(qū)域的關(guān)鍵點在世界直角坐標(biāo)系中的坐標(biāo)，并(可利用遠(yuǎn)程控制系統(tǒng)1)設(shè)定探測區(qū)域；智能控制模塊34根據(jù)定位導(dǎo)航模塊33(如里程計27、慣導(dǎo)28、gps全球定位系統(tǒng)29)的數(shù)據(jù)對待探測區(qū)域進(jìn)行探測路徑自主規(guī)劃；控制車體沿規(guī)劃路徑行進(jìn)，同時通過遠(yuǎn)程通訊模塊35將工業(yè)相機26采集的視頻信息傳回到遠(yuǎn)程控制系統(tǒng)1供技術(shù)人員通過顯示屏查看探雷車周圍的環(huán)境情況和探測系統(tǒng)9周圍的環(huán)境情況。當(dāng)機械臂探測系統(tǒng)3探測到地雷后，將數(shù)據(jù)傳給智能控制模塊34，智能控制模塊4記錄地雷的位置，然后發(fā)送信息給控制系統(tǒng)7，控制系統(tǒng)7控制機械臂運動系統(tǒng)8收回給噴涂機構(gòu)12讓出空間，然后噴涂機構(gòu)12伸出并前后擺動，使噴頭14到達(dá)到地雷上方進(jìn)行噴涂標(biāo)示；對地雷點標(biāo)示后，車體按照規(guī)劃路徑繼續(xù)探測工作直至探測完整個待探測區(qū)域，或者對地雷點標(biāo)示后，車體返回原點，待排雷后繼續(xù)規(guī)劃探測；從而可以實現(xiàn)更高效、高智能高精準(zhǔn)高可靠性的為后期排雷提供安全、可靠保證。在探雷過程中，如果避障模塊25發(fā)現(xiàn)障礙物，則機器人系統(tǒng)停下并試圖繞過，如障礙無法繞過，則切換成遠(yuǎn)程遙控模式由操作人員手動遙控人機交互模塊31(所述的人機交互模塊31可包括多個三軸工業(yè)手柄、按鈕、旋鈕、指示燈等，用戶可以通過人機交互模塊給移動底盤系統(tǒng)上的智能控制模塊發(fā)送控制信息，還可以通過指示燈獲得機器人系統(tǒng)的主要狀態(tài)、錯誤信息等)繞過障礙。

更具體的說，所述的移動底盤系統(tǒng)2包括移動平臺系統(tǒng)5、動力系統(tǒng)6和控制系統(tǒng)7。動力系統(tǒng)6安裝于移動平臺系統(tǒng)5的后端，為整個系統(tǒng)提供動力源并做配重用，用以平衡整個探雷機械裝置的重量；控制系統(tǒng)7安裝于移動平臺系統(tǒng)5的上部，控制整個智能探雷裝置的路徑規(guī)劃、行駛速度、工作狀態(tài)等：控制系統(tǒng)7控制動力系統(tǒng)6帶動移動平臺系統(tǒng)5(可以為輪式移動平臺、雙履帶移動平臺或多履帶移動平臺等)將車體移動至道面指定的位置。

機械臂探測系統(tǒng)3包括機械臂運動系統(tǒng)8和探測系統(tǒng)9，所述的探測系統(tǒng)9位于機械臂運動系統(tǒng)8前端。控制系統(tǒng)7控制車體沿規(guī)劃路徑行進(jìn)并控制機械臂運動系統(tǒng)8帶動探測系統(tǒng)9進(jìn)行地雷探測。探測時，機械臂運動系統(tǒng)8的機械臂前伸(帶動非金屬臂和探測系統(tǒng))，機械臂在控制系統(tǒng)7的協(xié)調(diào)下，將探測系統(tǒng)9置于探測區(qū)域，探測結(jié)束后機械臂向后收回，帶動探測系統(tǒng)9回到移動平臺系統(tǒng)5上，完成探測。本發(fā)明中，所述的機械臂運動系統(tǒng)8可采用電機、減速器、鏈條、齒輪齒條傳動組合，從而帶動移動支撐臂前后往復(fù)移動，實現(xiàn)機械臂探測系統(tǒng)3的回收和探測工作。

噴涂系統(tǒng)4包括伸縮機構(gòu)10、擺臂機構(gòu)11和噴涂機構(gòu)12。當(dāng)機械臂探測系統(tǒng)3探測到地雷后，探測系統(tǒng)收回，噴涂機構(gòu)12由伸縮機構(gòu)10和擺臂機構(gòu)11帶動，運行到地雷上方，然后通過噴頭14對地雷及周邊區(qū)域進(jìn)行噴涂標(biāo)示。本發(fā)明中，所述的伸縮機構(gòu)10包括：a電機、齒條和噴頭臂24，所述a電機上設(shè)有a齒輪，a齒輪與齒條連接，齒條與噴頭臂24連接，噴頭臂24連接噴頭14。a電機驅(qū)動齒輪齒條機構(gòu)帶動噴頭臂24，并沿導(dǎo)軌運行，從而達(dá)到噴頭向前或向后移動的效果。本發(fā)明中，所述的擺臂機構(gòu)11包括：b電機和弧形齒條，所述b電機上設(shè)有b齒輪，b齒輪與弧形齒條連接，弧形齒條與噴頭臂連接。采用b電機驅(qū)動齒輪與弧形齒條實現(xiàn)噴頭臂24的前后擺動，從而達(dá)到噴涂系統(tǒng)4扇形區(qū)域噴涂的效果。本發(fā)明中，所述的噴涂機構(gòu)12還包括：空壓機13以及噴漆泵15，所述的空壓機13與噴漆泵15連接，噴漆泵15與噴頭14連接。所述的噴涂機構(gòu)12還包括：二通電磁閥18和/或三通電磁閥19，所述的二通電磁閥18分別與空壓機13和噴頭14連接，三通電磁閥19分別與空壓機13和噴頭14連接?；蛘咚龅膰娡繖C構(gòu)12還包括：水泵20和液體罐21，所述的水泵20分別與液體罐21和噴頭14連接；或者所述的噴涂機構(gòu)12還包括：空壓機13以及涂料罐16，所述的空壓機13與涂料罐16連接，涂料罐16與噴頭14連接。從而可以吸出油漆、有色液體(如紅色鋼筆墨水)或粉末作為噴涂物料，由噴頭14噴出進(jìn)行地雷標(biāo)示。

對待探測區(qū)域進(jìn)行“z”字形探測路徑自主規(guī)劃的工作原理：

將待探測區(qū)域在機器人坐標(biāo)系中對應(yīng)的設(shè)置為矩形(如果人為設(shè)定為不規(guī)則多邊形，則機器人自動將其填充為包含所設(shè)定區(qū)域的矩形)。

如圖20所示，首先以矩形某個頂點作為起點，圖中以a點為例，沿機器人坐標(biāo)系x軸方向行進(jìn)至b點；

左轉(zhuǎn)90度，沿平行y軸方向行進(jìn)一個機器人身位；

再左轉(zhuǎn)90度，沿x軸反方向行進(jìn)至ad邊；

右轉(zhuǎn)90度，沿y軸方向行進(jìn)一個機器人身位；

再右轉(zhuǎn)90度，沿x軸方向行進(jìn)至bc邊；

如此循環(huán)，直到行走區(qū)域覆蓋整個矩形平面。

由于矩形各頂點各邊位置坐標(biāo)已知，只需要計算z字形軌跡上多點坐標(biāo)，然后機器人依次序走到這些坐標(biāo)位置就可完成。