一種機器人所處環(huán)境與地圖的匹配識別方法及系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供一種機器人所處環(huán)境與地圖的匹配識別方法，包括如下步驟：S10利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位置信息；S20計算出所述機器人所處環(huán)境與讀取到的地圖的匹配度；S30判斷所述機器人所處環(huán)境與所述地圖的匹配度是否滿足匹配要求；S40若所述機器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則所述機器人所處環(huán)境與所述地圖不匹配；否則，所述機器人所處環(huán)境與所述地圖匹配。本發(fā)明利用粒子濾波器對機器人所處環(huán)境進行定位，計算出機器人所處環(huán)境與地圖的匹配度，判斷匹配度是否滿足匹配要求，從而識別出環(huán)境與地圖是否匹配，從而提高機器人對復(fù)雜環(huán)境的適應(yīng)性和機器人自身的安全性。
【專利說明】
一種機器人所處環(huán)境與地圖的匹配識別方法及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及機器人技術(shù)領(lǐng)域，尤其是指機器人所處環(huán)境與地圖的匹配識別方法及 系統(tǒng)。
【背景技術(shù)】
[0002] 隨著科學(xué)技術(shù)不斷進步，移動機器人的種類越來越多，被廣泛應(yīng)用于工業(yè)、農(nóng)業(yè)、 消防、服務(wù)等行業(yè)，深受廣大人們的喜愛。在移動機器人自主完成各種任務(wù)之前，給出地圖 以及移動機器人在地圖上行走的規(guī)劃路徑，移動機器人按照規(guī)劃路徑行走。
[0003] 然而，移動機器人在行走的過程中，很有可能會遇到突發(fā)狀況，機器人所處環(huán)境變 化大會影響機器人定位的準確度，如果機器人定位失敗，機器人可能會失控。因此，移動機 器人對環(huán)境適應(yīng)性的要求越來越高，需要及時判斷移動機器人所處環(huán)境與地圖是否匹配， 以避免移動機器人發(fā)生不必要的撞擊。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明為了解決移動機器人所處環(huán)境與地圖是否匹配的問題，提供一種機器人所 處環(huán)境與地圖的匹配識別方法及系統(tǒng)。該匹配識別方法及系統(tǒng)識別出機器人所處環(huán)境與地 圖是否匹配，從而提高機器人對復(fù)雜環(huán)境的適應(yīng)性和機器人自身的安全性。
[0005] 為了實現(xiàn)本發(fā)明以上發(fā)明目的，本發(fā)明是通過以下技術(shù)方案實現(xiàn)的：
[0006] 本發(fā)明提供一種機器人所處環(huán)境與地圖的匹配識別方法，包括如下步驟:S10利用 粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位置信息；S20計算出所述機器 人所處環(huán)境與讀取到的地圖的匹配度；S30判斷所述機器人所處環(huán)境與所述地圖的匹配度 是否滿足匹配要求;S40若所述機器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則所 述機器人所處環(huán)境與所述地圖不匹配;否則，所述機器人所處環(huán)境與所述地圖匹配。
[0007] 進一步，本機器人所處環(huán)境與地圖的匹配識別方法，還包括步驟:S01利用激光傳 感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激光接收總條數(shù)、激光發(fā)射 總條數(shù)、激光長度以及激光角度。
[0008] 進一步，所述步驟S30還包括如下步驟:S31根據(jù)所述激光傳感器的激光接收總條 數(shù)，計算激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例;S32判斷激光接收總條數(shù)占激光發(fā)射總 條數(shù)的比例是否大于激光合法閾值;S33若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于 激光合法閾值，則判斷計算出的匹配度是否小于預(yù)設(shè)匹配閾值;S34若計算出的匹配度小于 預(yù)設(shè)匹配閾值，則計算出所述匹配度低于所述預(yù)設(shè)匹配閾值的持續(xù)時間，記錄所述機器人 的初始位姿和當(dāng)前位姿;S35判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值;S36若計算出 的持續(xù)時間大于預(yù)設(shè)時間閾值，則根據(jù)所述機器人的初始位姿和當(dāng)前位姿，計算出所述機 器人的位移差；否則，跳轉(zhuǎn)至步驟S31;S37判斷計算出的位移差是否大于預(yù)設(shè)位移閾值;若 計算出的位移差大于預(yù)設(shè)位移閾值，則所述機器人所處環(huán)境與所述地圖不匹配;否則，所述 機器人所處環(huán)境與所述地圖匹配。
[0009] 進一步，所述步驟S10還包括步驟:S11利用粒子濾波器對機器人所處環(huán)境進行定 位，得到所述機器人的位姿;其定位公式如下：
[0010] x(t) = f (x(t~l) ,u(t))；
[0011] y(t)=g(x(t)，z(t)，m);
[0012] 其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t_l時刻的位姿， u(t)是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是 觀測噪音模擬值，m是地圖信息。
[0013] 進一步，所述步驟S20還包括步驟:S21采用高斯分布密度函數(shù)計算出t時刻單個粒 子的權(quán)重P，將所有粒子權(quán)重累計即為所述機器人所處環(huán)境與所述地圖的匹配度;單個粒子 權(quán)重計算公式如下：
[0014] p = 〇e~l ^ + c, c = \!2d ;
[0015] 其中，a是高斯分布系數(shù)，d是所述激光數(shù)據(jù)中的激光長度，0是高斯分布的方差，1 是所述機器人所處環(huán)境中的障礙物與所述地圖中的障礙物之間的距離。
[0016] 進一步，本機器人所處環(huán)境與地圖的匹配識別方法，還包括步驟:S50當(dāng)所述機器 人所處環(huán)境與所述地圖匹配時，所述機器人按照規(guī)劃路徑行走;S60當(dāng)所述機器人所處環(huán)境 與所述地圖不匹配時，所述機器人報警并停止按照規(guī)劃路徑行走。
[0017] 進一步，本機器人所處環(huán)境與地圖的匹配識別方法。還包括步驟:S51當(dāng)所述機器 人按照規(guī)劃路徑行走時，判斷所述機器人是否到達終點位置;S52若所述機器人到達終點位 置，則所述機器人停止行走;否則，跳轉(zhuǎn)至步驟S01。
[0018] 進一步，本機器人所處環(huán)境與地圖的匹配識別方法的系統(tǒng)，包括：定位模塊，用于 利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位姿;計算模塊，所述計算 模塊與所述定位模塊電連接，用于計算出所述機器人所處環(huán)境與讀取到的地圖的匹配度； 判斷模塊，所述判斷模塊與所述計算模塊電連接，用于判斷所述機器人所處環(huán)境與所述地 圖的匹配度是否滿足匹配要求;匹配識別模塊，所述匹配識別模塊與所述判斷模塊電連接， 用于若所述機器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則所述機器人所處環(huán)境 與所述地圖不匹配;否則，所述機器人所處環(huán)境與所述地圖匹配。
[0019] 進一步，本機器人所處環(huán)境與地圖的匹配識別系統(tǒng)，包括:數(shù)據(jù)獲取模塊，用于利 用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激光接收總條數(shù)、 激光發(fā)射總條數(shù)、激光長度以及激光角度。
[0020] 進一步，所述計算模塊還用于根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光 接收總條數(shù)占激光發(fā)射總條數(shù)的比例;所述判斷模塊還用于判斷激光接收總條數(shù)占激光發(fā) 射總條數(shù)的比例是否大于激光合法閾值;若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于 激光合法閾值，則所述判斷模塊還用于判斷計算出的匹配度是否小于預(yù)設(shè)匹配閾值;若計 算出的匹配度小于預(yù)設(shè)匹配閾值，則所述計算模塊還用于計算出所述匹配度低于所述預(yù)設(shè) 匹配閾值的持續(xù)時間，記錄模塊用于記錄所述機器人的初始位姿和當(dāng)前位姿;所述判斷模 塊還用于判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值;若計算出的持續(xù)時間大于預(yù)設(shè)時 間閾值，則所述計算模塊還用于根據(jù)所述機器人的初始位姿和當(dāng)前位姿，計算出所述機器 人的位移差;所述判斷模塊還用于判斷計算出的位移差是否大于預(yù)設(shè)位移閾值;若計算出 的位移差大于預(yù)設(shè)位移閾值，則所述匹配識別模塊用于識別出所述機器人所處環(huán)境與所述 地圖不匹配;否則，所述匹配識別模塊用于識別出所述機器人所處環(huán)境與所述地圖匹配。
[0021] 本發(fā)明至少具有以下有益效果之一
[0022] 1.本發(fā)明利用粒子濾波器對機器人所處環(huán)境進行定位，計算出機器人所處環(huán)境與 地圖的匹配度，判斷匹配度是否滿足匹配要求，從而識別出環(huán)境與地圖是否匹配，從而提高 機器人對復(fù)雜環(huán)境的適應(yīng)性和機器人自身的安全性。
[0023] 2、本發(fā)明在識別出機器人所處環(huán)境與地圖是否匹配后，控制機器人是否按照規(guī)劃 路徑行走，從而為應(yīng)對實時變化的環(huán)境給出處理策略，例如遇到障礙物時匹配滿足要求可 選擇繞行，否則匹配度不滿足要求可重新規(guī)劃路徑等。
[0024] 3、本發(fā)明不僅適用于機器人在規(guī)劃路徑上行走時，在復(fù)雜的環(huán)境中，識別出其環(huán) 境與地圖不匹配;還適用于判斷出機器人所在環(huán)境是否與給出的地圖一致，可提示用戶是 否選錯地圖。
【附圖說明】
[0025]下面結(jié)合附圖和【具體實施方式】對本發(fā)明作進一步詳細說明：
[0026] 圖1為一種機器人所處環(huán)境與地圖的匹配識別方法的示意圖；
[0027] 圖2為機器人所處環(huán)境與地圖的匹配識別方法的一部分示意圖；
[0028] 圖3為另一種機器人所處環(huán)境與地圖的匹配識別方法的示意圖；
[0029] 圖4為再一種機器人所處環(huán)境與地圖的匹配識別方法的示意圖；
[0030] 圖5為機器人所處環(huán)境與地圖的匹配識別方法的另一部分示意圖；
[0031] 圖6為一種機器人所處環(huán)境與地圖的匹配識別系統(tǒng)的組成結(jié)構(gòu)示意圖；
[0032] 圖7為另一種機器人所處環(huán)境與地圖的匹配識別系統(tǒng)的組成結(jié)構(gòu)示意圖；
[0033] 圖中：
[0034] 10、定位模塊，11、數(shù)據(jù)獲取模塊，20、計算模塊，21、記錄模塊，30、判斷模塊，40、匹 配識別模塊，50、控制模塊，60、報警模塊。
【具體實施方式】
[0035] 為了更清楚地說明本發(fā)明實施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實施例或現(xiàn) 有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見地，以下說明和附圖對于本發(fā)明 是示例性的，并且不應(yīng)被理解為限制本發(fā)明。以下說明描述了眾多具體細節(jié)以方便對本發(fā) 明理解。然而，在某些實例中，熟知的或常規(guī)的細節(jié)并未說明，以滿足說明書簡潔的要求。
[0036] 如圖1所示，根據(jù)本發(fā)明的一個實施例，一種機器人所處環(huán)境與地圖的匹配識別方 法，包括如下步驟：
[0037] S10利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位置信息；
[0038] S20計算出所述機器人所處環(huán)境與讀取到的地圖的匹配度；
[0039] S30判斷所述機器人所處環(huán)境與所述地圖的匹配度是否滿足匹配要求；
[0040] 若所述機器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則執(zhí)行S40所述機 器人所處環(huán)境與所述地圖不匹配；
[0041] 若所述機器人所處環(huán)境與所述地圖的匹配度滿足匹配要求，則執(zhí)行S41所述機器 人所處環(huán)境與所述地圖匹配。
[0042]如圖1、圖2所示，根據(jù)本發(fā)明的另一個實施例，一種機器人所處環(huán)境與地圖的匹配 識別方法，包括如下步驟：
[0043] S01利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激 光接收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度;優(yōu)選的，所述激光傳感器的型號 可以是RPLIDARA1M1;
[0044] S10利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位置信息；
[0045] S20計算出所述機器人所處環(huán)境與讀取到的地圖的匹配度；
[0046] S31根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總條數(shù)占激光發(fā)射總 條數(shù)的比例；
[0047] S32判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合法閾值，所述 激光合法閾值是指遇到障礙物時，激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例；
[0048] S33若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則判斷計算 出的匹配度是否小于預(yù)設(shè)匹配閾值;否則，跳轉(zhuǎn)至步驟S41;
[0049] S34若計算出的匹配度小于預(yù)設(shè)匹配閾值，則計算出所述匹配度低于所述預(yù)設(shè)匹 配閾值的持續(xù)時間，記錄所述機器人的初始位姿和當(dāng)前位姿;否則，跳轉(zhuǎn)至步驟S41;
[0050] S35判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值；
[0051] S36若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則根據(jù)所述機器人的初始位姿和當(dāng) 前位姿，計算出所述機器人的位移差;否則，跳轉(zhuǎn)至步驟S31;
[0052] S37判斷計算出的位移差是否大于預(yù)設(shè)位移閾值；
[0053]若計算出的位移差大于預(yù)設(shè)位移閾值，則執(zhí)行S40所述機器人所處環(huán)境與所述地 圖不匹配；
[0054]若計算出的位移差小于預(yù)設(shè)位移閾值，則執(zhí)行S41所述機器人所處環(huán)境與所述地 圖匹配。
[0055] 具體的，激光接收總條數(shù)是指:反射回來的激光條數(shù)，利用激光傳感器探測機器人 所處環(huán)境，如果所處環(huán)境較空闊，在激光測量范圍之外，則反射回來的激光條數(shù)占激光發(fā)射 總數(shù)的比例將小于激光合法閾值;如果環(huán)境信息豐富，例如障礙物分布復(fù)雜，大部分激光都 能發(fā)射回來，則激光傳感器中反射回來的激光條數(shù)占激光發(fā)射總數(shù)的比例大于激光合法閾 值；
[0056] 判斷機器人所處環(huán)境與地圖的匹配度，判斷該匹配度是否小于預(yù)設(shè)匹配閾值，若 匹配度小于預(yù)設(shè)匹配閾值，則表示該障礙物不存在于地圖中；若匹配度大于等于預(yù)設(shè)匹配 閾值，則表示該障礙物存在于地圖中，機器人所處環(huán)境與地圖匹配；
[0057] 再判斷匹配度小于預(yù)設(shè)匹配閾值的持續(xù)時間是否大于預(yù)設(shè)時間閾值，若持續(xù)時間 大于預(yù)設(shè)時間閾值，則表示該障礙物在短時間內(nèi)不會消失，會阻礙機器人行走;若持續(xù)時間 不大于預(yù)設(shè)時間閾值，則表示該障礙物在短時間內(nèi)會消失，不阻礙機器人行走，機器人所處 環(huán)境與地圖還是匹配的；
[0058]再判斷機器人的位移差是否大于預(yù)設(shè)位移閾值，若位移差大于預(yù)設(shè)位移閾值，則 表示機器人在運動一定距離后依然檢測出環(huán)境變化大，排除機器人在固定的位置處于低匹 配度且持續(xù)一定時間，其實環(huán)境變化不大的情況，機器人所處環(huán)境與地圖不匹配，需要對機 器人進行相關(guān)控制；若位移差不大于預(yù)設(shè)位移閾值，則表示機器人所處環(huán)境與地圖還是匹 配的。
[0059] 根據(jù)本發(fā)明的再一個實施例，一種機器人所處環(huán)境與地圖的匹配識別方法，包括 如下步驟：
[0060] S01利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激 光接收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度；
[0061] S11利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位姿;其定位 公式如下：
[0062] x(t) = f (x(t~l) ,u(t))；
[0063] y(t)=g(x(t)，z(t)，m);
[0064] 其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t_l時刻的位姿， u(t)是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是 〇~1之間的隨機觀測噪音模擬值，m是地圖信息;在本發(fā)明中z(t)就是權(quán)重計算公式中的c;
[0065] S21采用高斯分布密度函數(shù)計算出t時刻單個粒子的權(quán)重p，將所有粒子權(quán)重累計 即為所述機器人所處環(huán)境與所述地圖的匹配度;單個粒子權(quán)重計算公式如下：
[0066] p = ae~l2^ + c, c = V2d ;
[0067]其中，a是高斯分布系數(shù)，d是所述激光數(shù)據(jù)中的激光長度，0是高斯分布的方差，1 是所述機器人所處環(huán)境中的障礙物與所述地圖中的障礙物之間的距離；其中，1的取值越 小，匹配度越高，環(huán)境變化越??；
[0068] S31根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總條數(shù)占激光發(fā)射總 條數(shù)的比例；
[0069] S32判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合法閾值，所述 激光合法閾值是指遇到障礙物時，激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例；
[0070] S33若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則判斷計算 出的匹配度是否小于預(yù)設(shè)匹配閾值；
[0071] S34若計算出的匹配度小于預(yù)設(shè)匹配閾值，則計算出所述匹配度低于所述預(yù)設(shè)匹 配閾值的持續(xù)時間，記錄所述機器人的初始位姿和當(dāng)前位姿；
[0072] S35判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值；
[0073] S36若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則根據(jù)所述機器人的初始位姿和當(dāng) 前位姿，計算出所述機器人的位移差;否則，跳轉(zhuǎn)至步驟S31;
[0074] S37判斷計算出的位移差是否大于預(yù)設(shè)位移閾值；
[0075] 若計算出的位移差大于預(yù)設(shè)位移閾值，則執(zhí)行S40所述機器人所處環(huán)境與所述地 圖不匹配；
[0076] 若計算出的位移差小于預(yù)設(shè)位移閾值，則執(zhí)行S41所述機器人所處環(huán)境與所述地 圖匹配。
[0077]具體的，在本發(fā)明中簡單介紹了粒子濾波定位原理，其粒子濾波定位的具體原理 可參照申請?zhí)枮?01310424318.8的基于分布式邊緣無味粒子濾波的同步定位與地圖構(gòu)建 方法。
[0078]如圖3所示，根據(jù)本發(fā)明的又一個實施例，一種機器人所處環(huán)境與地圖的匹配識別 方法，包括如下步驟：
[0079] SOI利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激 光接收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度；
[0080] S11利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位姿;其定位 公式如下：
[0081] x(t) = f (x(t~l) ,u(t))；
[0082] y(t)=g(x(t)，z(t)，m);
[0083]其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t_l時刻的位姿， u(t)是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是 觀測噪音模擬值，m是地圖信息；
[0084] S21采用高斯分布密度函數(shù)計算出t時刻單個粒子的權(quán)重p，將所有粒子權(quán)重累計 即為所述機器人所處環(huán)境與所述地圖的匹配度;單個粒子權(quán)重計算公式如下：
[0085] p = ae~l1^ +c t c = H2d ;
[0086] 其中，a是高斯分布系數(shù)，d是所述激光數(shù)據(jù)中的激光長度，0是高斯分布的方差，1 是所述機器人所處環(huán)境中的障礙物與所述地圖中的障礙物之間的距離；
[0087] S31根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總條數(shù)占激光發(fā)射總 條數(shù)的比例；
[0088] S32判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合法閾值，所述 激光合法閾值是指遇到障礙物時，激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例；
[0089] S33若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則判斷計算 出的匹配度是否小于預(yù)設(shè)匹配閾值；
[0090] S34若計算出的匹配度小于預(yù)設(shè)匹配閾值，則計算出所述匹配度低于所述預(yù)設(shè)匹 配閾值的持續(xù)時間，記錄所述機器人的初始位姿和當(dāng)前位姿；
[0091 ] S35判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值；
[0092] S36若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則根據(jù)所述機器人的初始位姿和當(dāng) 前位姿，計算出所述機器人的位移差;否則，跳轉(zhuǎn)至步驟S31;
[0093] S37判斷計算出的位移差是否大于預(yù)設(shè)位移閾值；
[0094] 若計算出的位移差大于預(yù)設(shè)位移閾值，則執(zhí)行S40所述機器人所處環(huán)境與所述地 圖不匹配；
[0095] 若計算出的位移差小于預(yù)設(shè)位移閾值，則執(zhí)行S41所述機器人所處環(huán)境與所述地 圖匹配；
[0096] S50當(dāng)所述機器人所處環(huán)境與所述地圖匹配時，所述機器人按照規(guī)劃路徑行走；
[0097] 優(yōu)選的，還包括步驟S51當(dāng)所述機器人按照規(guī)劃路徑行走時，判斷所述機器人是否 到達終點位置；
[0098] 優(yōu)選的，還包括步驟S52若所述機器人到達終點位置，則所述機器人停止行走；否 貝1J，跳轉(zhuǎn)至步驟S01;
[0099] S60當(dāng)所述機器人所處環(huán)境與所述地圖不匹配時，所述機器人報警并停止行走。 [0100]如圖6所示，根據(jù)本發(fā)明的一個實施例，一種機器人所處環(huán)境與地圖的匹配識別系 統(tǒng)，包括：
[0101]定位模塊10,用于利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人 的位姿；
[0102]計算模塊20,所述計算模塊20與所述定位模塊10電連接，用于計算出所述機器人 所處環(huán)境與讀取到的地圖的匹配度；
[0103]判斷模塊30,所述判斷模塊30與所述計算模塊20電連接，用于判斷所述機器人所 處環(huán)境與所述地圖的匹配度是否滿足匹配要求；
[0104] 匹配識別模塊40,所述匹配識別模塊40與所述判斷模塊30電連接，用于若所述機 器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則所述機器人所處環(huán)境與所述地圖不 匹配;否則，所述機器人所處環(huán)境與所述地圖匹配。
[0105] 如圖7所示，根據(jù)本發(fā)明的另一個實施例，一種機器人所處環(huán)境與地圖的匹配識別 系統(tǒng)，包括：
[0106] 數(shù)據(jù)獲取模塊11，用于利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所 述激光數(shù)據(jù)包括激光接收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度；
[0107] 定位模塊10,用于利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人 的位姿;其定位公式如下：
[0108] x(t) = f (x(t~l) ,u(t))；
[0109] y(t)=g(x(t)，z(t)，m);
[0110] 其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t-1時刻的位姿， u(t)是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是 觀測噪音模擬值，m是地圖信息；
[0111] 計算模塊20，用于采用高斯分布密度函數(shù)計算出t時刻單個粒子的權(quán)重p，將所有 粒子權(quán)重累計即為所述機器人所處環(huán)境與所述地圖的匹配度；單個粒子權(quán)重計算公式如 下：
[0112] p = aeTl"p + c，c = /2d ;
[0113] 其中，a是高斯分布系數(shù)，d是所述激光數(shù)據(jù)中的激光長度，0是高斯分布的方差，1 是所述機器人所處環(huán)境中的障礙物與所述地圖中的障礙物之間的距離；
[0114] 所述計算模塊20還用于根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總 條數(shù)占激光發(fā)射總條數(shù)的比例；
[0115] 判斷模塊30,用于判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合 法閾值；
[0116] 若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則所述判斷模塊 30還用于判斷計算出的匹配度是否小于預(yù)設(shè)匹配閾值；
[0117] 若計算出的匹配度小于預(yù)設(shè)匹配閾值，則所述計算模塊20還用于計算出所述匹配 度低于所述預(yù)設(shè)匹配閾值的持續(xù)時間，記錄模塊21用于記錄所述機器人的初始位姿和當(dāng)前 位姿；
[0118] 所述判斷模塊30還用于判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值；
[0119] 若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則所述計算模塊20還用于根據(jù)所述機器 人的初始位姿和當(dāng)前位姿，計算出所述機器人的位移差；
[0120] 所述判斷模塊30還用于判斷計算出的位移差是否大于預(yù)設(shè)位移閾值；
[0121] 若計算出的位移差大于預(yù)設(shè)位移閾值，則所述匹配識別模塊40用于識別出所述機 器人所處環(huán)境與所述地圖不匹配；否則，所述匹配識別模塊40用于識別出所述機器人所處 環(huán)境與所述地圖匹配；
[0122] 控制模塊50,用于當(dāng)所述機器人所處環(huán)境與所述地圖匹配時，所述機器人按照規(guī) 劃路徑行走；
[0123] 優(yōu)選的，所述判斷模塊30還用于當(dāng)所述機器人按照規(guī)劃路徑行走時，判斷所述機 器人是否到達終點位置；
[0124] 優(yōu)選的，若所述機器人到達終點位置，則所述控制模塊50還用于控制所述機器人 停止行走；
[0125] 報警模塊60。用于當(dāng)所述機器人所處環(huán)境與所述地圖不匹配時，所述機器人報警， 所述控制模塊50用于控制機器人停止行走。
[0126] 如圖4、圖5所示，根據(jù)本發(fā)明的一個實施例，一種機器人所處環(huán)境與地圖的匹配識 別方法，包括如下步驟：
[0127] S100:讀取全局地圖數(shù)據(jù)，這里默認建圖完畢；
[0128] S200:讀取激光傳感器獲取到的激光數(shù)據(jù)，機器人通過激光傳感器感知外界環(huán)境， 激光傳感器安裝在機器人前端，激光數(shù)據(jù)包括激光角度和對應(yīng)的激光長度，激光傳感器的 型號是RPLIDAR A1M1;
[0129] S300:獲取機器人的行走路徑，并控制機器人行走，通過全局路徑規(guī)劃和控制算法 使機器人正常工作；
[0130] S400:利用粒子濾波算法進行定位;粒子濾波器定位原理，
[0131] x(t) =f(x(t-l) ,u(t)) ； (1)
[0132] y(t)=g(x(t)，z(t)，m); (2)
[0133] 其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t-1時刻的位姿， u(t)是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是 觀測噪音模擬值，m是地圖信息；
[0134] 公式1為狀態(tài)方程，公式2為觀測方程，其原理是將前一時刻的樣本集代入到狀態(tài) 方程中，可預(yù)測出一個新的樣本集，也稱為粒子集(機器人的位姿），將新的粒子集代入觀測 方程中，將獲得的觀測值(激光數(shù)據(jù))與地圖信息對比，可獲得粒子的權(quán)重(也稱匹配度）；
[0135] S410:進行粒子初始化，在地圖中創(chuàng)建均勻分布的粒子(樣本）、權(quán)重相等，采用高 斯概率密度函數(shù)進行粒子分布；
[0136] S420:利用高斯分布函數(shù)進行采樣，獲得粒子集；
[0137] S420:根據(jù)采樣后的粒子和激光觀測數(shù)據(jù)得到每個粒子的權(quán)重，計算所有粒子的 總權(quán)重(匹配度）；權(quán)重的計算公式為：
[0138] P = qb-/2//^ +C , c = /2d ; (3)
[0139] 其中，a是高斯分布系數(shù)，例如a = 0.5;d是所述激光數(shù)據(jù)中的激光最大長度，例如d =6m，c = 1 /12; 0是高斯分布的方差，例如0 = 0.08; 1是所述機器人所處環(huán)境中的障礙物與 所述地圖中的障礙物之間的距離，距離越小說明匹配度越高，環(huán)境變化越?。?br>[0140] S430:將權(quán)重歸一化，最后進行重采樣獲取機器人的最優(yōu)位置；
[0141] 根據(jù)公式3可計算出所有粒子的總權(quán)重進而定位出機器人的實際位置;初始狀態(tài)x (〇)為一個均勻分布的樣本集，特別說明，粒子濾波算法實現(xiàn)機器人的定位還包括一些其他 的步驟這里不作贅述，如采樣和重采樣；
[0142] S500:計算機器人環(huán)境與地圖匹配度，匹配度的影響因子包括：激光數(shù)據(jù)是否合 法，粒子的總權(quán)重，總權(quán)重持續(xù)時間和機器人位移，如果激光數(shù)據(jù)合法（即大部分激光數(shù)據(jù) 都能反射回來，否則激光數(shù)據(jù)在不可靠的情況下繼續(xù)進行匹配度計算沒有意義），粒子低權(quán) 重持續(xù)一定時間，且在機器人有位移的前提下可認為機器人所處環(huán)境與地圖上的信息不匹 配。這里設(shè)定了六個閾值，即為激光合法閾值（1&8 61'_(3011111：_1：11代(1)，總權(quán)重閾值（1：(^&1_ weight_thred)，時間閾值（last_time_thred)，機器人位移閾值（x_thred，y_thred，theta_ thred)，其中機器人的位移僅需滿足位移閾值中的任一個即可；
[0143] S501:計算激光接收總條數(shù)占總發(fā)射的比例，主要是為了解決空闊地帶激光數(shù)返 回較少，總權(quán)重持續(xù)較低的引起的誤判；
[0144] S502:判斷激光接收總條數(shù)所占的比例是否滿足要求，即是否大于激光合法閾值 (laser_count_thred)，若是轉(zhuǎn)Sf5〇 3;否則轉(zhuǎn)Sf504;
[0145] S503:根據(jù)公式3獲取粒子的總權(quán)重；
[0146] S504:退出匹配度識別；
[0147] S505:判斷總權(quán)重是否滿足要求，若是說明總權(quán)重低于設(shè)定的權(quán)重閾值（total_ weight_thred)轉(zhuǎn) S506，否則轉(zhuǎn) S504;
[0148] S506:獲取機器人的位姿，轉(zhuǎn)S507;
[0149] S507:判斷是否第一次滿足總權(quán)重小于閾值，若是轉(zhuǎn)S508，否則轉(zhuǎn)S509;
[0150] S508:記錄此刻為初始時刻。此時機器人的位姿為匹配度識別的初始位姿；
[0151] S509:記錄當(dāng)前的時間和機器人的位姿；
[0152] S510:計算S508和S509的時間差，判斷持續(xù)時間是否達到設(shè)定的時間閾值（last_ time_thred)，若是轉(zhuǎn)S511，否則轉(zhuǎn)S501;
[0153] S511:計算S508和S509的位移差，判斷位移是否大于設(shè)定的位移閾值(x_thred，y_ thred，theta_thred)，若是轉(zhuǎn)S512，否則轉(zhuǎn)S504;
[0154] S512:環(huán)境與地圖不匹配，輸出結(jié)果；
[0155] S600:判斷匹配度是否滿足要求，如果滿足要求則按原規(guī)劃的路徑繼續(xù)行走，即 S800,否則轉(zhuǎn) S700;
[0156] S700:報警并停止行走；
[0157] S800:按原路徑繼續(xù)行走，轉(zhuǎn)S900;
[0158] S900:判斷機器人行走是否結(jié)束，若沒有轉(zhuǎn)S200;
[0159] 應(yīng)當(dāng)說明的是，上述實施例均可根據(jù)需要自由組合。以上所述僅是本發(fā)明的優(yōu)選 實施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提 下，還可以做出若干改進和潤飾，這些改進和潤飾也應(yīng)視為本發(fā)明的保護范圍。
【主權(quán)項】
1. 一種機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，包括如下步驟： SlO利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位置信息； S20計算出所述機器人所處環(huán)境與讀取到的地圖的匹配度； S30判斷所述機器人所處環(huán)境與所述地圖的匹配度是否滿足匹配要求； S40若所述機器人所處環(huán)境與所述地圖的匹配度不滿足匹配要求，則所述機器人所處 環(huán)境與所述地圖不匹配;否則，所述機器人所處環(huán)境與所述地圖匹配。2. 根據(jù)權(quán)利要求1所述的機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，還包括 步驟： SOl利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光數(shù)據(jù)包括激光接 收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度。3. 根據(jù)權(quán)利要求2所述的機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，所述步 驟S30還包括如下步驟： S31根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總條數(shù)占激光發(fā)射總條數(shù) 的比例； S32判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合法閾值； S33若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則判斷計算出的 匹配度是否小于預(yù)設(shè)匹配閾值； S34若計算出的匹配度小于預(yù)設(shè)匹配閾值，則計算出所述匹配度低于所述預(yù)設(shè)匹配閾 值的持續(xù)時間，記錄所述機器人的初始位姿和當(dāng)前位姿； S35判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值； S36若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則根據(jù)所述機器人的初始位姿和當(dāng)前位 姿，計算出所述機器人的位移差;否則，跳轉(zhuǎn)至步驟S31; S37判斷計算出的位移差是否大于預(yù)設(shè)位移閾值;若計算出的位移差大于預(yù)設(shè)位移閾 值，則所述機器人所處環(huán)境與所述地圖不匹配；否則，所述機器人所處環(huán)境與所述地圖匹 配。4. 根據(jù)權(quán)利要求2所述的機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，所述步 驟S10還包括步驟： Sll利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位姿;其定位公式 如下： x(t)=f (x(t-l) ,u(t))； y(t) = g(x(t)，z(t)，m); 其中，x(t)是所述機器人在t時刻的位姿，x(t-l)是所述機器人在t-1時刻的位姿，u(t) 是所述機器人在t時刻的控制輸入量;y(t)是所述機器人所處環(huán)境的激光數(shù)據(jù)，z(t)是觀測 噪音模擬值，m是地圖信息。5. 根據(jù)權(quán)利要求4所述的機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，所述步 驟S20還包括步驟： S21采用高斯分布密度函數(shù)計算出t時刻單個粒子的權(quán)重p，將所有粒子權(quán)重累計即為 所述機器人所處環(huán)境與所述地圖的匹配度;單個粒子權(quán)重計算公式如下：其中，α是高斯分布系數(shù)，d是所述激光數(shù)據(jù)中的激光長度，β是高斯分布的方差，1是所 述機器人所處環(huán)境中的障礙物與所述地圖中的障礙物之間的距離。6. 根據(jù)權(quán)利要求1~5中任意一項所述的機器人所處環(huán)境與地圖的匹配識別方法，其特 征在于，還包括步驟： S50當(dāng)所述機器人所處環(huán)境與所述地圖匹配時，所述機器人按照規(guī)劃路徑行走； S60當(dāng)所述機器人所處環(huán)境與所述地圖不匹配時，所述機器人報警并停止按照規(guī)劃路 徑行走。7. 根據(jù)權(quán)利要求6所述的機器人所處環(huán)境與地圖的匹配識別方法，其特征在于，還包括 步驟： S51當(dāng)所述機器人按照規(guī)劃路徑行走時，判斷所述機器人是否到達終點位置； S52若所述機器人到達終點位置，則所述機器人停止行走;否則，跳轉(zhuǎn)至步驟SO 1。8. -種應(yīng)用在如權(quán)利要求1~7中任意一項所述的機器人所處環(huán)境與地圖的匹配識別 方法的系統(tǒng)，其特征在于，包括： 定位模塊，用于利用粒子濾波器對機器人所處環(huán)境進行定位，得到所述機器人的位姿； 計算模塊，所述計算模塊與所述定位模塊電連接，用于計算出所述機器人所處環(huán)境與 讀取到的地圖的匹配度； 判斷模塊，所述判斷模塊與所述計算模塊電連接，用于判斷所述機器人所處環(huán)境與所 述地圖的匹配度是否滿足匹配要求； 匹配識別模塊，所述匹配識別模塊與所述判斷模塊電連接，用于若所述機器人所處環(huán) 境與所述地圖的匹配度不滿足匹配要求，則所述機器人所處環(huán)境與所述地圖不匹配;否則， 所述機器人所處環(huán)境與所述地圖匹配。9. 根據(jù)權(quán)利要求8所述的機器人所處環(huán)境與地圖的匹配識別系統(tǒng)，其特征在于，包括： 數(shù)據(jù)獲取模塊，用于利用激光傳感器獲取所述機器人所處環(huán)境的激光數(shù)據(jù)，所述激光 數(shù)據(jù)包括激光接收總條數(shù)、激光發(fā)射總條數(shù)、激光長度以及激光角度。10. 根據(jù)權(quán)利要求9所述的機器人所處環(huán)境與地圖的匹配識別系統(tǒng)，其特征在于： 所述計算模塊還用于根據(jù)所述激光傳感器的激光接收總條數(shù)，計算激光接收總條數(shù)占 激光發(fā)射總條數(shù)的比例； 所述判斷模塊還用于判斷激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例是否大于激光合 法閾值； 若激光接收總條數(shù)占激光發(fā)射總條數(shù)的比例大于激光合法閾值，則所述判斷模塊還用 于判斷計算出的匹配度是否小于預(yù)設(shè)匹配閾值； 若計算出的匹配度小于預(yù)設(shè)匹配閾值，則所述計算模塊還用于計算出所述匹配度低于 所述預(yù)設(shè)匹配閾值的持續(xù)時間，記錄模塊用于記錄所述機器人的初始位姿和當(dāng)前位姿； 所述判斷模塊還用于判斷計算出的持續(xù)時間是否大于預(yù)設(shè)時間閾值； 若計算出的持續(xù)時間大于預(yù)設(shè)時間閾值，則所述計算模塊還用于根據(jù)所述機器人的初 始位姿和當(dāng)前位姿，計算出所述機器人的位移差； 所述判斷模塊還用于判斷計算出的位移差是否大于預(yù)設(shè)位移閾值； 若計算出的位移差大于預(yù)設(shè)位移閾值，則所述匹配識別模塊用于識別出所述機器人所
【文檔編號】G06K9/62GK105892461SQ201610244332
【公開日】2016年8月24日
【申請日】2016年4月13日
【發(fā)明人】徐清霞, 張小*, 章征貴
【申請人】上海物景智能科技有限公司