[發(fā)明專利]一種基于紅外圖形的機器人定位的方法

在審

• 申請?zhí)枺?/td>: 202210030634.6
• 申請日: 2022-01-13
• 公開（公告）號: CN114440887A
• 公開（公告）日: 2022-05-06
• 發(fā)明（設(shè)計）人: 余程;袁榮炎
• 申請（專利權(quán)）人: 蘇州山滬和智能科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01B11/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于紅外圖形的機器人定位的方法，其特征在于：所述的一種基于紅外圖形的機器人定位的方法由R個機器人和N個檢測點實施；其實施步驟如下：

A.1、構(gòu)建三維地圖M3；

構(gòu)建三維地圖M3的步驟如下：

A.1.1、機器人r在所述的室內(nèi)僅有固定物體時，建立用于機器人自主行走的二維地圖M2；r是機器人的編號，r∈RS；RS是機器人編號的集合，RS＝{r|r＝1，2，...，R}，R是機器人的數(shù)量，R≥1；機器人是指室內(nèi)機器人；固定物體是指在所述的室內(nèi)固定不動的物體；

A.1.2、處理器C_r得到三維地圖M3；

處理器C_r得到三維地圖M3的步驟如下：

A.1.2.1、處理器C_r設(shè)置如下數(shù)據(jù)：(1)記錄數(shù)據(jù)編號i的初值i＝0；(2)檢測點n編號n的初值n＝1；處理器C_r是機器人r中的處理器；每個檢測點n均由處理器D_n、采集設(shè)備n、無線網(wǎng)絡(luò)設(shè)備WD_n組成，NS是檢測點編號n的集合，NS＝{n|n＝1，2，...，N}，N是檢測點的數(shù)量，N≥1；每個采集設(shè)備n是采集圖像的設(shè)備，采集圖像的范圍是區(qū)域n，圖像是指視頻圖像和紅外圖像，并且每個區(qū)域n一一對應(yīng)范圍n，

A.1.2.2、使機器人r處在行走范圍n的起點(X_n0，Y_n0)；范圍n是機器人規(guī)劃路徑全部范圍中的一個部分范圍或是全部的范圍；范圍n的編號是機器人r按二維地圖M2依方向WD行走，根據(jù)先后到達范圍的順序進行的編號，方向WD是順時針方向或是逆時針方向；范圍1，范圍2，...，范圍N組成機器人規(guī)劃路徑的全部范圍；

A.1.2.3、在所述的室內(nèi)僅有固定物體時，控制機器人r進行遍及范圍n的行走；當(dāng)機器人r在進行所述的行走期間，處理器C_r在每個時刻T_i均控制激光器r向上發(fā)射一個紅外圖形G，G的外形尺寸為G_size；向上是指向機器人r的正上方的天花板，正上方是指機器人r建立二維地圖M2時機器人r中心的正上方；天花板是指能在其上形成紅外圖形G的室內(nèi)天花板；紅外圖形是指由紅外光形成的圖形；GS_min≤G_size≤GS_max，GS_min是每個檢測點n從區(qū)域n上的圖像中均能識別出G的最小外形尺寸，GS_max是機器人r處在二維地圖M2中不同坐標(biāo)點時，每個檢測點n均能從圖像中識別出G且能生成不同的三維地圖數(shù)據(jù)的最大外形尺寸，每個區(qū)域n的范圍均包含機器人在范圍n行走時發(fā)射全部紅外圖形G的范圍，激光器r每次發(fā)射圖形G的時長為Tc，發(fā)射的時間間隔Ts＝T_i+1-T_i，2*ft≤Tc＜Ts，ft是采集設(shè)備n采集一幀圖像的時長；激光器r是指機器人r中的紅外激光器；

A.1.2.4、處理器C_r從檢測點n得到圖像數(shù)據(jù)(XG_i，YG_i)；

A.1.2.5、處理器C_r得到一個三維地圖數(shù)據(jù)；

處理器C_r得到一個三維地圖數(shù)據(jù)的步驟如下：

A.1.2.5.1、處理器C_r從導(dǎo)航系統(tǒng)r中獲取機器人r所處位置在二維地圖M2中的坐標(biāo)點，并記錄該坐標(biāo)點為(X_i，Y_i)；導(dǎo)航系統(tǒng)r是指機器人r中的導(dǎo)航系統(tǒng)；

A.1.2.5.2、處理器C_r記錄三維地圖數(shù)據(jù)[X_i，Y_i，XG_i，YG_i，n]；n是區(qū)域的編號；

A.1.2.6、處理器C_r對i加1；

A.1.2.7、如果處理器C_r判斷機器人r已經(jīng)完成遍及范圍n的行走，則轉(zhuǎn)步驟A.1.2.8；否則轉(zhuǎn)步驟A.1.2.3；機器人r已經(jīng)完成遍及范圍n的行走是指機器人r已經(jīng)行走完范圍n上的K_n條邊界路徑，或者還包括已經(jīng)行走完范圍n上的M_n條范圍內(nèi)路徑；

A.1.2.8、處理器C_r對n加1；

A.1.2.9、如果處理器C_r判斷n＞N，則轉(zhuǎn)步驟A.1.2.10；否則轉(zhuǎn)步驟A.1.2.2；

A.1.2.10、處理器C_r設(shè)置記錄三維地圖數(shù)據(jù)的數(shù)量I＝i；

A.1.2.11、處理器C_r結(jié)束得到三維地圖M3的處理；

A.1.3、處理器C_r用無線網(wǎng)絡(luò)設(shè)備WE_r通過網(wǎng)絡(luò)將二維地圖M2和三維地圖M3傳輸給其它的機器人t，t≠r，t＝1，2，...，R；三維地圖M3是指記錄三維地圖數(shù)據(jù)的數(shù)量I和每個記錄數(shù)據(jù)[X_i，Y_i，XG_i，YG_i，n]，i＝0，1，...，I-1，n是區(qū)域的編號；無線網(wǎng)絡(luò)設(shè)備WE_r是機器人r中的無線網(wǎng)絡(luò)設(shè)備；

A.2、機器人的定位處理；

機器人的定位處理的步驟如下：

A.2.1、機器人r按二維地圖M2自主行走；r∈RS；

A.2.2、機器人r在自主行走期間，如果處理器C_r判斷導(dǎo)航數(shù)據(jù)相差較大，則轉(zhuǎn)步驟A.2.3；否則轉(zhuǎn)步驟A.2.1；導(dǎo)航數(shù)據(jù)相差較大是指導(dǎo)航系統(tǒng)r上的傳感器在機器人r現(xiàn)在所處位置檢測到的環(huán)境數(shù)據(jù)與建立二維地圖M2時所檢測到的環(huán)境數(shù)據(jù)相差較大；

A.2.3、如果處理器C_r判斷機器人r與檢測點n通訊有效，則轉(zhuǎn)步驟A.2.4；否則轉(zhuǎn)步驟A.2.1；機器人r與檢測點n通訊有效是指機器人r與檢測點n進行通訊，并在檢測圖形的完整性后，處理器C_r仍保存著所接收的檢測點n的回復(fù)信息R_n；

A.2.4、機器人r得到定位數(shù)據(jù)；

機器人r得到定位數(shù)據(jù)的步驟如下：

A.2.4.1、處理器C_r控制機器人r停止行走；

A.2.4.2、處理器C_r從導(dǎo)航系統(tǒng)r中獲取機器人r現(xiàn)在所處位置在二維地圖M2中的坐標(biāo)點，并記錄該坐標(biāo)點為(X_p，Y_p)；

A.2.4.3、處理器C_r控制激光器r向上發(fā)射一個紅外圖形G，發(fā)射紅外圖形G的時長為Tc，發(fā)射的時間間隔為Ts，發(fā)射的次數(shù)是Q_f，1≤Q_f≤100；

A.2.4.4、處理器C_r從檢測點n得到圖像數(shù)據(jù)(XG_r，YG_r)；

A.2.4.5、處理器C_r用公式(1)計算出機器人r在二維地圖M2中的實際坐標(biāo)點(X_r，Y_r)；

X_r＝X_i1+Dx

Y_r＝Y(jié)_i1+Dy

α＝tan^-1[|(XG_r-XG_i1)/(YG_r-YG_i1)|]

Sv＝[(X_i2-X_i1)²+(Y_i2-Y_i1)²]^1/2*[(XG_r-XG_i1)²+(YG_r-YG_i1)²]^1/2/[(XG_i2-XG_i1)²+(YG_i2-YG_i1)²]^1/2 (1)

(1)式中，int[]是取整運算；X_i1，Y_i1，XG_i1，YG_i1是三維地圖M3中記錄點[X_i1，Y_i1，XG_i1，YG_i1，n]中的數(shù)據(jù)；X_i2，Y_i2，XG_i2，YG_i2是三維地圖M3中記錄點[X_i2，Y_i2，XG_i2，YG_i2，n]中的數(shù)據(jù)；(XG_i1，YG_i1)與(XG_i2，YG_i2)是(XG_r，YG_r)8鄰域中對稱區(qū)里到(XG_r，YG_r)距離最近的兩個點，并且D1≤D2；D1是(XG_i1，YG_i1)到(XG_r，YG_r)距離，D2是(XG_i2，YG_i2)到(XG_r，YG_r)距離；8鄰域是指與點相鄰的周圍8個點所圍成的區(qū)域，對稱區(qū)是指對稱點區(qū)域，對稱點是指8鄰域中的處在對角的兩個點，或處在X方向上的兩個點，或處在Y方向上的兩個點，對稱點區(qū)域是指對稱點和8鄰域中與對稱點相鄰的兩個點所圍區(qū)域；

A.2.5、如果處理器C_r判斷(X_r，Y_r)到(X_p，Y_p)的距離D_rp≥MinD，則轉(zhuǎn)步驟A.2.6；否則轉(zhuǎn)步驟A.2.7；Rc≤MinD≤3*Rc；Rc是二維地圖M2中的一個坐標(biāo)單位；

A.2.6、處理器C_r用(X_r，Y_r)修改機器人r的導(dǎo)航數(shù)據(jù)中的原坐標(biāo)點(X_p，Y_p)；

A.2.7、處理器r控制機器人r重新開始自主行走；本步驟執(zhí)行后轉(zhuǎn)步驟A.2.1。