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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

s1，控制車體移動(dòng)至道面指定的位置；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

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

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

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

圖9是機(jī)械臂運(yùn)動(dòng)系統(tǒng)收回的示意圖；

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

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

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

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

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

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

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

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

圖18為金屬框架式機(jī)械臂示意圖；

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

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

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

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

s1，控制車體移動(dòng)至道面指定的位置；

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

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

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

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

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

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

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

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

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

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

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

步驟s3包括以下步驟：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

位姿包括位置和姿態(tài)，位置即機(jī)器人在平面上運(yùn)動(dòng)的坐標(biāo)，因?yàn)闄C(jī)器人只在地面運(yùn)動(dòng)，所以姿態(tài)只需要航向角即可。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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