本發(fā)明涉及一種機(jī)器人技術(shù)領(lǐng)域，尤其是涉及一種機(jī)器人自主充電系統(tǒng)及控制方法。

背景技術(shù)：

目前，移動(dòng)機(jī)器人在各個(gè)行業(yè)正逐漸替代人去執(zhí)行一些繁重、復(fù)雜、枯燥、危險(xiǎn)的工作。這些機(jī)器人都是通過(guò)可充電電池為其工作提供能量，當(dāng)電池電能耗盡后，還需要人工去給機(jī)器人充電?？紤]到機(jī)器人的可持續(xù)工作，這些機(jī)器人必須要能夠在脫離人為干預(yù)情況下長(zhǎng)期自治，而對(duì)機(jī)器人自主充電技術(shù)的研究則可以使機(jī)器人幾乎完全不需要人工干預(yù)，不間斷地去執(zhí)行各項(xiàng)工作。

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

本發(fā)明的目的在于提出一種無(wú)需人工干預(yù)即可自行充電的機(jī)器人自主充電系統(tǒng)。

本發(fā)明的另一目的在于提出一種機(jī)器人自主充電系統(tǒng)的控制方法。

本發(fā)明的目的是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的：

一種機(jī)器人自主充電系統(tǒng)，所述系統(tǒng)包括：充電座控制單元、機(jī)器人充電單元、電機(jī)控制單元和上位機(jī)任務(wù)單元；

充電座控制單元包括充電座8、充電座電子羅盤(pán)、紅外發(fā)射單元1和充電座控制器；

充電座8包括充電口2和電源接口3，其中電源接口3與電源適配器相連；

充電座8前方布置有水平標(biāo)志帶，兩側(cè)布置有垂直標(biāo)志帶，水平標(biāo)志帶和兩個(gè)垂直標(biāo)志帶構(gòu)成矩形的標(biāo)志地帶9，標(biāo)志地帶9包圍充電座8；

充電座電子羅盤(pán)和充電座控制器設(shè)置在充電座8內(nèi)；

紅外發(fā)射單元1設(shè)置在充電座8上，所述紅外發(fā)射單元1內(nèi)設(shè)置三個(gè)紅外發(fā)射器，用于分別向不同的發(fā)射方向發(fā)射不同的紅外信號(hào)，每種信號(hào)的覆蓋范圍是60°，紅外發(fā)射單元1內(nèi)的三個(gè)紅外發(fā)射器的覆蓋范圍是180°；

機(jī)器人充電單元，包括超聲波傳感器、紅外接收器、紅外對(duì)管14和電子羅盤(pán)15；

機(jī)器人為矩形，在機(jī)器人前端由右到左依次安裝四個(gè)超聲波傳感器，分別為第一超聲波傳感器10、第二超聲波傳感器11、第三超聲波傳感器12和第四超聲波傳感器13，其中，第一超聲波傳感器10和第四超聲波傳感器13以及第二超聲波傳感器11和第三超聲波傳感器12分別關(guān)于機(jī)器人中心線對(duì)稱；

在機(jī)器人的右側(cè)、正前端和左側(cè)分別布置有右紅外接收器、前紅外接收器和左紅外接收器，每個(gè)紅外接收器的接收角度為120度；

電子羅盤(pán)15設(shè)置在機(jī)器人內(nèi)，機(jī)器人前端底部設(shè)置紅外對(duì)管14；

電機(jī)控制單元，包括電機(jī)控制器和電機(jī)，用于處理由機(jī)器人充電單元或上位機(jī)任務(wù)單元傳輸?shù)碾姍C(jī)控制信息，對(duì)電機(jī)進(jìn)行控制；

機(jī)器人充電單元和電機(jī)控制單元設(shè)置于機(jī)器人機(jī)體中。

標(biāo)志地帶9的兩個(gè)垂直標(biāo)志帶之間的距離是充電座8寬度的1～1.5倍。

標(biāo)志地帶9的兩個(gè)垂直標(biāo)志帶之間的距離是充電座8寬度的1.2倍。

標(biāo)志地帶9的兩根垂直標(biāo)志帶與充電座8兩端的距離相等，充電座8與前方水平標(biāo)志帶的距離d為：

其中，a為機(jī)器人的長(zhǎng)度，b為機(jī)器人的寬度。

充電口2包括上、下兩個(gè)口，上口是正極，下口是負(fù)極。

一種如所述的機(jī)器人自主充電系統(tǒng)的控制方法，其包括如下步驟：

a.機(jī)器人執(zhí)行當(dāng)前任務(wù)；

b.判斷機(jī)器人電量是否小于設(shè)定值，如果電量小于設(shè)定值，則執(zhí)行步驟c；如果電量大于設(shè)定值，則返回步驟a，繼續(xù)執(zhí)行當(dāng)前任務(wù)；

c.判斷機(jī)器人是否接收到紅外信號(hào)，如果機(jī)器人未接收到紅外信號(hào)，則機(jī)器人原地旋轉(zhuǎn)，并重新判斷機(jī)器人是否接收到紅外信號(hào)；如果機(jī)器人收到紅外信號(hào)，則進(jìn)入步驟d；

d.機(jī)器人根據(jù)接收到的紅外信號(hào)選擇相應(yīng)的運(yùn)動(dòng)策略靠近充電座8；

安裝在充電座8中的三個(gè)不同的紅外發(fā)射管發(fā)射三種不同的紅外信號(hào)，每種信號(hào)的覆蓋范圍是60°；

機(jī)器人位于紅外信號(hào)覆蓋范圍內(nèi)時(shí)，根據(jù)機(jī)器人上的三個(gè)紅外接收器接收到的紅外信號(hào)，判斷當(dāng)前機(jī)器人所在的區(qū)域與姿態(tài)，并確定機(jī)器人接下來(lái)的運(yùn)動(dòng)狀態(tài)；

e.判斷機(jī)器人是否到達(dá)對(duì)接區(qū)域，如果機(jī)器人未到達(dá)對(duì)接區(qū)域，則返回步驟d；如果機(jī)器人已經(jīng)到達(dá)對(duì)接區(qū)域，則進(jìn)入步驟f；

f.調(diào)用對(duì)接算法；

設(shè)置于機(jī)器人中的機(jī)器人充電單元中的電子羅盤(pán)15測(cè)量機(jī)器人的方位角度進(jìn)而消除角度誤差，當(dāng)機(jī)器人的紅外對(duì)管14檢測(cè)到標(biāo)志地帶9后，機(jī)器人向充電座控制器發(fā)送獲取充電座方位角度的指令，充電座控制器收到該指令后，將充電座電子羅盤(pán)測(cè)到的自身方位角度通過(guò)無(wú)線串口發(fā)回機(jī)器人充電單元，機(jī)器人充電單元將機(jī)器人中的電子羅盤(pán)15測(cè)到的機(jī)器人的角度與機(jī)器人收到的充電座的角度進(jìn)行對(duì)比，得到電機(jī)控制信息，將該信息輸出至電機(jī)控制單元，進(jìn)而控制機(jī)器人調(diào)整姿態(tài)直至機(jī)器人的角度與充電座的角度一致，此刻機(jī)器人的前端面與標(biāo)志地帶9中的水平標(biāo)志帶平行；

當(dāng)機(jī)器人檢測(cè)到標(biāo)志地帶9并將機(jī)器人角度調(diào)整到與標(biāo)志地帶9中的水平標(biāo)志帶平行后，四個(gè)超聲波傳感器開(kāi)始工作，通過(guò)比較四個(gè)超聲波傳感器所測(cè)量的距離信息，判斷機(jī)器人與充電座8的相對(duì)位置，調(diào)用對(duì)接算法進(jìn)行對(duì)接；

g.對(duì)接成功，開(kāi)始充電。

在步驟d中，所述運(yùn)動(dòng)策略為：

機(jī)器人的右側(cè)、正前端、左側(cè)分別布置有右紅外接收器4、前紅外接收器5和左紅外接收器6，I、II、III分別表示充電座8的紅外發(fā)射單元1內(nèi)的三個(gè)紅外發(fā)射器從左至右發(fā)出的三種不同的紅外信號(hào)，每種紅外信號(hào)的覆蓋范圍是60°：

d1、機(jī)器人的三個(gè)紅外接收器未收到任何信號(hào)時(shí)，機(jī)器人原地旋轉(zhuǎn)；

d2、當(dāng)只有左紅外接收器6收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于a狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d3、當(dāng)前紅外接收器5和左紅外接收器6收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于b狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d4、當(dāng)右紅外接收器4收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于c狀態(tài)，此時(shí)機(jī)器人直行；

d5、當(dāng)左紅外接收器6收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于d狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d6、當(dāng)只有前紅外接收器5收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于e狀態(tài)，機(jī)器人直行；

d7、當(dāng)右紅外接收器4收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于f狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d8、當(dāng)只有右紅外接收器4收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于i狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d9、當(dāng)右紅外接收器4和前紅外接收器5收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于h狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d10、當(dāng)左紅外接收器6收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于g狀態(tài)，此時(shí)機(jī)器人直行。

在步驟f中，所述對(duì)接算法為：

機(jī)器人中心點(diǎn)與充電座8中心軸線的垂直距離是L₁，d₁表示第一超聲波傳感器10測(cè)得的與正前方的距離，d₂表示第二超聲波傳感器11測(cè)得的與正前方的距離，d₃表示第三超聲波傳感器12測(cè)得的與正前方的距離，d₄表示第四超聲波傳感器13測(cè)得的與正前方的距離，d表示充電座8的厚度，L₂為充電座寬度，d₅為兩個(gè)相鄰超聲波傳感器的中軸線之間的距離，

Δ1＝||d₁-d₂|-d|

Δ2＝||d₂-d₃|-d|

Δ3＝||d₃-d₄|-d|

找出Δ₁，Δ₂，Δ₃中的最小值m，

當(dāng)m＝Δ₁且d₁>d₂時(shí)，則機(jī)器人偏右，且垂直距離L₁＝L₂/2-d₅；

當(dāng)m＝Δ₁且d₁<d₂時(shí)，則機(jī)器人偏左，且垂直距離L₁＝L₂/2+d₅；

當(dāng)m＝Δ₂且d₂>d₃時(shí)，則機(jī)器人偏右，且垂直距離L₁＝L₂/2；

當(dāng)m＝Δ₂且d₂<d₃時(shí)，則機(jī)器人偏左，且垂直距離L₁＝L₂/2；

當(dāng)m＝Δ₃且d₃>d₄時(shí)，則機(jī)器人偏右，且垂直距離L₁＝L₂/2+d₅；

當(dāng)m＝Δ₃且d₃<d₄時(shí)，則機(jī)器人偏左，且垂直距離L₁＝L₂/2-d₅；

如果機(jī)器人偏右，則機(jī)器人順時(shí)針旋轉(zhuǎn)60°，機(jī)器人沿其軸線方向后退距離L，L＝L₁/sin60°，隨后，機(jī)器人逆時(shí)針旋轉(zhuǎn)60°，機(jī)器人朝向充電座8直行直至與充電座8完成對(duì)接；

如果機(jī)器人偏左，則機(jī)器人逆時(shí)針旋轉(zhuǎn)60°，機(jī)器人沿其軸線方向后退距離L，L＝L₁/sin60°，隨后，機(jī)器人順時(shí)針旋轉(zhuǎn)60°，然后機(jī)器人朝向充電座8直行直至完成與充電座8的對(duì)接。

本發(fā)明的有益效果在于：

1.經(jīng)過(guò)實(shí)驗(yàn)證明，該機(jī)器人自主充電系統(tǒng)有較好的可重復(fù)性和較高的可靠性。

2.該機(jī)器人自主充電系統(tǒng)采用超聲波傳感器、紅外發(fā)射器等廉價(jià)的傳感器，成本較低、控制簡(jiǎn)單。

3.該機(jī)器人自主充電系統(tǒng)適于室內(nèi)機(jī)器人、AGV等移動(dòng)平臺(tái)。

附圖說(shuō)明

圖1是本發(fā)明的機(jī)器人自主充電系統(tǒng)的控制方法的流程圖；

圖2是本發(fā)明的機(jī)器人自主充電系統(tǒng)的充電站結(jié)構(gòu)示意圖；

圖3是本發(fā)明的機(jī)器人自主充電系統(tǒng)的機(jī)器人與充電座相對(duì)位置示意圖；

圖4是本發(fā)明的機(jī)器人自主充電系統(tǒng)的對(duì)接示意圖；

圖5a-5e是本發(fā)明的機(jī)器人自主充電系統(tǒng)的橫向誤差消除策略示意圖；

圖6a-6g是本發(fā)明的機(jī)器人的狀態(tài)代表示意圖；

圖7a-7h是本發(fā)明的機(jī)器人處于臨界位置的示意圖。

附圖標(biāo)記：

1紅外發(fā)射單元 2充電口 3電源接口

4右紅外接收器 5前紅外接收器 6左紅外接收器

8充電座 9標(biāo)志帶 10第一超聲波傳感器

11第二超聲波傳感器 12第三超聲波傳感器 13第四超聲波傳感器

14紅外對(duì)管 15電子羅盤(pán)

具體實(shí)施方式

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

一種機(jī)器人自主充電系統(tǒng)，包括：充電座控制單元、機(jī)器人充電單元、電機(jī)控制單元和上位機(jī)任務(wù)單元。每個(gè)單元獨(dú)立運(yùn)行，各單元之間采用固定的通訊協(xié)議。

充電座控制單元，包括充電座8、充電座電子羅盤(pán)(圖中未示出)、紅外發(fā)射單元1和充電座控制器，用于發(fā)射紅外信號(hào)以及向機(jī)器人發(fā)送自身方位角度，它與機(jī)器人充電單元之間通過(guò)紅外通訊和無(wú)線串口通訊。

其中，如圖2所示，充電座8包括充電口2和電源接口3，其中電源接口3與電源適配器相連。

充電口2包括上、下兩個(gè)口，上口是正極，下口是負(fù)極。

如圖4所示，充電座8前方布置有水平標(biāo)志帶，兩側(cè)布置有垂直標(biāo)志帶，水平標(biāo)志帶和兩個(gè)垂直標(biāo)志帶構(gòu)成矩形的標(biāo)志地帶9，標(biāo)志地帶9包圍充電座8，優(yōu)選地，標(biāo)志地帶9的兩個(gè)垂直標(biāo)志帶之間的距離是充電座8寬度的1～1.5倍，優(yōu)選為1.2倍，標(biāo)志地帶9的兩根垂直標(biāo)志帶與充電座8兩端的距離相等，充電座8與前方水平標(biāo)志帶的距離d為：

其中，a為機(jī)器人的長(zhǎng)度，b為機(jī)器人的寬度。

充電座電子羅盤(pán)和充電座控制器設(shè)置在充電座8內(nèi)。

紅外發(fā)射單元1設(shè)置在充電座8上，所述紅外發(fā)射單元1內(nèi)設(shè)置三個(gè)紅外發(fā)射器，用于分別向不同的發(fā)射方向發(fā)射不同的紅外信號(hào)，每種信號(hào)的覆蓋范圍是60°，紅外發(fā)射單元1內(nèi)的三個(gè)紅外發(fā)射器的覆蓋范圍是180°。

機(jī)器人充電單元，包括超聲波傳感器、紅外接收器、紅外對(duì)管14和電子羅盤(pán)15，用于通過(guò)采集超聲波傳感器、紅外接收器、紅外對(duì)管14和電子羅盤(pán)15的信息，處理后得到電機(jī)控制信息。

如圖3和圖4所示，機(jī)器人為矩形，在機(jī)器人前端由右到左依次安裝四個(gè)超聲波傳感器，分別為第一超聲波傳感器10、第二超聲波傳感器11、第三超聲波傳感器12和第四超聲波傳感器13，其中，第一超聲波傳感器10和第四超聲波傳感器13以及第二超聲波傳感器11和第三超聲波傳感器12分別關(guān)于機(jī)器人中心線對(duì)稱。

在機(jī)器人的右側(cè)、正前端和左側(cè)布置有右紅外接收器、前紅外接收器和左紅外接收器，每個(gè)紅外接收器的接收角度為120度。

電子羅盤(pán)15設(shè)置在機(jī)器人內(nèi)，機(jī)器人前端底部設(shè)置紅外對(duì)管14。

電機(jī)控制單元，包括電機(jī)控制器和電機(jī)，用于處理通過(guò)串口由機(jī)器人充電單元或上位機(jī)任務(wù)單元傳輸過(guò)來(lái)的電機(jī)控制信息，對(duì)電機(jī)進(jìn)行控制。

機(jī)器人充電單元和電機(jī)控制單元設(shè)置于機(jī)器人機(jī)體中。

上位機(jī)任務(wù)單元，包括上位機(jī)，用于監(jiān)測(cè)機(jī)器人的狀態(tài)信息，并在機(jī)器人電量充足的情況下將機(jī)器人的運(yùn)動(dòng)任務(wù)通過(guò)無(wú)線串口發(fā)送至電機(jī)控制單元，控制機(jī)器人完成相應(yīng)任務(wù)。

如圖1所示，一種機(jī)器人自主充電系統(tǒng)的控制方法，針對(duì)的是機(jī)器人自主充電系統(tǒng)在近距離的自主充電，即機(jī)器人初始位于充電座控制單元的紅外發(fā)射單元1發(fā)射的紅外信號(hào)覆蓋的區(qū)域內(nèi)，包括如下步驟：

a.機(jī)器人執(zhí)行當(dāng)前任務(wù)；

b.判斷機(jī)器人電量是否小于設(shè)定值，如果電量小于設(shè)定值，則執(zhí)行步驟c；如果電量大于設(shè)定值，則返回步驟a，繼續(xù)執(zhí)行當(dāng)前任務(wù)；

其中，機(jī)器人的電機(jī)控制單元接收并執(zhí)行上位機(jī)發(fā)送的任務(wù)命令，同時(shí)不斷檢測(cè)機(jī)器人電量；

c.判斷機(jī)器人是否接收到紅外信號(hào)，如果機(jī)器人未接收到紅外信號(hào)，則機(jī)器人原地旋轉(zhuǎn)，并重新判斷機(jī)器人是否接收到紅外信號(hào)；如果機(jī)器人收到紅外信號(hào)，則進(jìn)入步驟d；

d.機(jī)器人根據(jù)接收到的紅外信號(hào)選擇相應(yīng)的運(yùn)動(dòng)策略靠近充電座8；

安裝在充電座8中的三個(gè)不同的紅外發(fā)射管發(fā)射三種不同的紅外信號(hào)，每種信號(hào)的覆蓋范圍是60°。

機(jī)器人位于紅外信號(hào)覆蓋范圍內(nèi)時(shí)，根據(jù)機(jī)器人上的三個(gè)紅外接收器接收到的紅外信號(hào)，綜合判斷當(dāng)前機(jī)器人所在的區(qū)域與姿態(tài)，并確定機(jī)器人接下來(lái)的運(yùn)動(dòng)狀態(tài)。

如圖3所示，圖中數(shù)字4、5、6分別表示布置于機(jī)器人的右側(cè)、正前端、左側(cè)的右紅外接收器、前紅外接收器和左紅外接收器，I、II、III分別表示充電座8的紅外發(fā)射單元1內(nèi)的三個(gè)紅外發(fā)射器從左至右發(fā)出的三種不同的紅外信號(hào)，每種紅外信號(hào)的覆蓋范圍是60°，三個(gè)紅外發(fā)射器可覆蓋范圍是180°。a、b、c、d、e、f、g、h、i分別表示機(jī)器人的9種不同的姿態(tài)，根據(jù)機(jī)器人不同的位置姿態(tài)，制定了不同的運(yùn)動(dòng)策略：

d1、機(jī)器人的三個(gè)紅外接收器未收到任何信號(hào)時(shí)，機(jī)器人原地旋轉(zhuǎn)；

d2、當(dāng)只有左紅外接收器6收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于a狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d3、當(dāng)前紅外接收器5和左紅外接收器6收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于b狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d4、當(dāng)右紅外接收器4收到信號(hào)I時(shí)，則斷定機(jī)器人當(dāng)前處于c狀態(tài)，此時(shí)機(jī)器人直行；

d5、當(dāng)左紅外接收器6收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于d狀態(tài)，此時(shí)機(jī)器人逆時(shí)針旋轉(zhuǎn)；

d6、當(dāng)只有前紅外接收器5收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于e狀態(tài)，機(jī)器人直行；

d7、當(dāng)右紅外接收器4收到信號(hào)II時(shí)，則斷定機(jī)器人當(dāng)前處于f狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d8、當(dāng)只有右紅外接收器4收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于i狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d9、當(dāng)右紅外接收器4和前紅外接收器5收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于h狀態(tài)，此時(shí)機(jī)器人順時(shí)針旋轉(zhuǎn)；

d10、當(dāng)左紅外接收器6收到信號(hào)III時(shí)，則斷定機(jī)器人當(dāng)前處于g狀態(tài)，此時(shí)機(jī)器人直行。

e.判斷機(jī)器人是否到達(dá)對(duì)接區(qū)域，如果機(jī)器人未到達(dá)對(duì)接區(qū)域，則返回步驟d；如果機(jī)器人已經(jīng)到達(dá)對(duì)接區(qū)域，則進(jìn)入步驟f；

即：根據(jù)步驟d中的幾種情況，機(jī)器人不斷靠近充電座8，直到機(jī)器人底部的紅外對(duì)管14檢測(cè)到充電座8前方的標(biāo)志地帶9，機(jī)器人進(jìn)入對(duì)接階段。

f.調(diào)用對(duì)接算法；

設(shè)置于機(jī)器人中的機(jī)器人充電單元中的電子羅盤(pán)15測(cè)量機(jī)器人的方位角度進(jìn)而消除角度誤差。當(dāng)機(jī)器人的紅外對(duì)管14檢測(cè)到標(biāo)志地帶9后，機(jī)器人向充電座控制器發(fā)送獲取充電座方位角度的指令，充電座控制器收到該指令后，將充電座電子羅盤(pán)測(cè)到的自身方位角度通過(guò)無(wú)線串口發(fā)回機(jī)器人充電單元。機(jī)器人充電單元將機(jī)器人中的電子羅盤(pán)15測(cè)到的機(jī)器人的角度與機(jī)器人收到的充電座的角度進(jìn)行對(duì)比，得到電機(jī)控制信息，將該信息輸出至電機(jī)控制單元，進(jìn)而控制機(jī)器人調(diào)整姿態(tài)直至機(jī)器人的角度與充電座的角度一致。此刻機(jī)器人的前端面與標(biāo)志地帶9中的水平標(biāo)志帶平行。

如圖5a～5e所示，當(dāng)機(jī)器人檢測(cè)到標(biāo)志地帶9并將機(jī)器人角度調(diào)整到與標(biāo)志地帶9的水平標(biāo)志帶平行后，四個(gè)超聲波傳感器開(kāi)始工作，通過(guò)比較四個(gè)超聲波傳感器所測(cè)量的距離信息，判斷機(jī)器人與充電座8的相對(duì)位置，調(diào)用對(duì)接算法進(jìn)行對(duì)接；

如圖6a～6g和圖7a～圖7h所示，根據(jù)四個(gè)超聲波傳感器的分布情況選取對(duì)接時(shí)機(jī)器人處于的8個(gè)臨界位置：

機(jī)器人處于臨界位置圖7a與臨界位置圖7b之間的狀態(tài)由機(jī)器人處于圖6a的狀態(tài)代表；

機(jī)器人處于臨界位置圖7b與臨界位置圖7c之間的狀態(tài)由機(jī)器人處于圖6b的狀態(tài)代表；

機(jī)器人處于臨界位置圖7c與臨界位置圖7d之間的狀態(tài)由機(jī)器人處于圖6c的狀態(tài)代表；

機(jī)器人處于臨界位置圖7d與臨界位置圖7e之間的狀態(tài)由機(jī)器人處于圖6d的狀態(tài)代表；

機(jī)器人處于臨界位置圖7e與臨界位置圖7f之間的狀態(tài)由機(jī)器人處于圖6e的狀態(tài)代表；

機(jī)器人處于臨界位置圖7f與臨界位置圖7g之間的狀態(tài)由機(jī)器人處于圖6f的狀態(tài)代表；

機(jī)器人處于臨界位置圖7g與臨界位置圖7h之間的狀態(tài)由機(jī)器人處于圖6g的狀態(tài)代表。

對(duì)接算法如下：

機(jī)器人中心點(diǎn)與充電座8中心軸線的垂直距離是L₁，d₁表示第一超聲波傳感器10測(cè)得的與正前方的距離，d₂表示第二超聲波傳感器11測(cè)得的與正前方的距離，d₃表示第三超聲波傳感器12測(cè)得的與正前方的距離，d₄表示第四超聲波傳感器13測(cè)得的與正前方的距離，d表示充電座8的厚度，L₂為充電座寬度，d₅為兩個(gè)相鄰超聲波傳感器的中軸線之間的距離。

Δ1＝||d₁-d₂|-d|

Δ2＝||d₂-d₃|-d|

Δ3＝||d₃-d₄|-d|

找出Δ₁，Δ₂，Δ₃中的最小值m，

當(dāng)m＝Δ₁且d₁>d₂時(shí)，如圖6c所示，則機(jī)器人偏右，且垂直距離L₁＝L₂/2-d₅；

當(dāng)m＝Δ₁且d₁<d₂時(shí)，如圖6e所示，則機(jī)器人偏左，且垂直距離L₁＝L₂/2+d₅；

當(dāng)m＝Δ₂且d₂>d₃時(shí)，如圖6b所示，則機(jī)器人偏右，且垂直距離L₁＝L₂/2；

當(dāng)m＝Δ₂且d₂<d₃時(shí)，如圖6f所示，則機(jī)器人偏左，且垂直距離L₁＝L₂/2；

當(dāng)m＝Δ₃且d₃>d₄時(shí)，如圖6a所示，則機(jī)器人偏右，且垂直距離L₁＝L₂/2+d₅；

當(dāng)m＝Δ₃且d₃<d₄時(shí)，如圖6e所示，則機(jī)器人偏左，且垂直距離L₁＝L₂/2-d₅；

如果機(jī)器人偏右，則機(jī)器人順時(shí)針旋轉(zhuǎn)60°，機(jī)器人沿其軸線方向后退距離L，L＝L1/sin60°，隨后，機(jī)器人逆時(shí)針旋轉(zhuǎn)60°，機(jī)器人朝向充電座8直行直至與充電座8完成對(duì)接。

如果機(jī)器人偏左，則機(jī)器人逆時(shí)針旋轉(zhuǎn)60°，機(jī)器人沿其軸線方向后退距離L，L＝L1/sin60°，隨后，機(jī)器人順時(shí)針旋轉(zhuǎn)60°，然后機(jī)器人朝向充電座8直行直至完成與充電座8的對(duì)接。

如圖5a所示，此時(shí)Δ₃最小且d₃>d₄，可知當(dāng)前機(jī)器人偏右，且L₁＝L₂/2+d₅。如圖5b所示，機(jī)器人順時(shí)針旋轉(zhuǎn)60°。如圖5c所示，然后機(jī)器人沿其軸線方向后退距離L，L＝L₁/sin60°。如圖5d所示，機(jī)器人逆時(shí)針旋轉(zhuǎn)60°。如圖5e，然后機(jī)器人前進(jìn)直至完成對(duì)接。

當(dāng)Δ₃最小且d₃<d₄，可知當(dāng)前機(jī)器人偏左，且橫向偏移量L₁＝L₂/2-d₅，機(jī)器人逆時(shí)針旋轉(zhuǎn)60°，機(jī)器人沿其軸線方向后退距離L(L＝L₁/sin60°)，然后機(jī)器人順時(shí)針旋轉(zhuǎn)60°，然后機(jī)器人前進(jìn)直至完成對(duì)接。

g.對(duì)接成功，開(kāi)始充電。