專利名稱：基于視覺測(cè)量的清潔機(jī)器人系統(tǒng)及其測(cè)控方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種基于視覺測(cè)量的清潔機(jī)器人系統(tǒng)及其測(cè)控方法，屬于機(jī)器人領(lǐng)域。
背景技術(shù)：
近年來(lái)，室內(nèi)清潔機(jī)器人備受關(guān)注，人們對(duì)其智能化程度的要求也越來(lái)越高。傳統(tǒng)室內(nèi)清潔機(jī)器人通常有非地圖導(dǎo)航和基于地圖導(dǎo)航兩種路徑規(guī)劃方式，基于地圖導(dǎo)航代表清潔機(jī)器人的發(fā)展方向，其地圖構(gòu)建方法則體現(xiàn)了清潔機(jī)器人智能程度。常見地圖構(gòu)建方法的環(huán)境監(jiān)測(cè)手段有紅外傳感器、超聲傳感器、雙目立體視覺、結(jié)構(gòu)光機(jī)器視覺等，機(jī)器人行進(jìn)中的自定位技術(shù)往往借助于編碼器、陀螺儀等計(jì)算單位時(shí)間內(nèi)機(jī)器人的方位變化。在現(xiàn)有技術(shù)中，中國(guó)專利公開號(hào)CN 101446832 A，
公開日2009年6月3日，發(fā)明名稱是機(jī)器人吸塵器自動(dòng)避障及其控制方法，該發(fā)明提出設(shè)置一個(gè)基站，吸塵本體與基站之間保持通信聯(lián)系，執(zhí)行吸塵、避障，并能自主回到基站充電。該發(fā)明的不足在于吸塵本體和基站之間采用紅外方式通信，易受環(huán)境影響，且需要額外維護(hù)基站。中國(guó)專利申請(qǐng)公布號(hào)CNlO 1852609 A，申請(qǐng)公布日2010年10年6日，發(fā)明名稱是一種基于機(jī)器人雙目立體視覺的地面障礙物檢測(cè)方法，該發(fā)明公開了一種基于機(jī)器人雙目立體視覺的地面障礙物檢測(cè)方法，在地面視差值的基礎(chǔ)上，通過(guò)反投影模型計(jì)算出某像素對(duì)應(yīng)場(chǎng)景的三維坐標(biāo)，從而初步判斷該像素屬于障礙物或是地面點(diǎn)，對(duì)障礙物和地面點(diǎn)賦予不同的顏色；最后控制機(jī)器人實(shí)施避障。該發(fā)明的缺點(diǎn)是測(cè)距受光照等條件影響較大，不及超聲測(cè)距準(zhǔn)確，地圖構(gòu)建以及機(jī)器人行進(jìn)無(wú)方向參照。中國(guó)專利申請(qǐng)公布號(hào)CN 102138769 A，申請(qǐng)公布日2011年8月3日，發(fā)明名稱是清潔機(jī)器人及其清掃方法，該發(fā)明地圖邊界中標(biāo)識(shí)可清掃區(qū)域，在首次遍歷的同時(shí)或之后，按照預(yù)設(shè)清掃方式進(jìn)行清掃，提高了清掃效率。但該方法以機(jī)器人原始位置為坐標(biāo)原點(diǎn)，以機(jī)器人行走距離為基礎(chǔ)構(gòu)建地圖，容易積累誤差，且缺少對(duì)地圖適時(shí)調(diào)整的功能。因此，設(shè)計(jì)一個(gè)無(wú)基站定位、環(huán)境適應(yīng)能力強(qiáng)、地圖構(gòu)建準(zhǔn)確靈活的室內(nèi)清潔機(jī)器人系統(tǒng)是一個(gè)亟待解決的問(wèn)題。

發(fā)明內(nèi)容
發(fā)明目的本發(fā)明提出一種基于視覺測(cè)量的清潔機(jī)器人系統(tǒng)及其測(cè)控方法，解決了現(xiàn)有清潔機(jī)器人易受環(huán)境影響，需要額外基站支持以及地圖構(gòu)建不夠靈活的缺點(diǎn)。技術(shù)方案本發(fā)明采用的技術(shù)方案為一種基于視覺測(cè)量的清潔機(jī)器人系統(tǒng)，包括進(jìn)行數(shù)據(jù)處理的中央子系統(tǒng)，拍攝圖像的視覺子系統(tǒng)，測(cè)量與障礙物距離的測(cè)量子系統(tǒng)，行走子系統(tǒng)，吸塵子系統(tǒng)，以及給本裝置所有子系統(tǒng)提供電力供應(yīng)的電源子系統(tǒng)。所述中央子系統(tǒng)由信息采集模塊、信息處理模塊、控制輸出模塊、人機(jī)交互模塊組成。信息采集模塊負(fù)責(zé)采集來(lái)自測(cè)量子系統(tǒng)和視覺子系統(tǒng)的傳感器數(shù)據(jù)，這些數(shù)據(jù)傳遞給信息處理模塊。信息處理模塊對(duì)傳感器數(shù)據(jù)進(jìn)行信息處理、數(shù)據(jù)融合和判斷決策?？刂戚敵瞿K根據(jù)信息處理模塊的判斷決策向吸塵子系統(tǒng)、行走子系統(tǒng)輸出控制信號(hào)。人機(jī)交互模塊提供系統(tǒng)設(shè)置、系統(tǒng)標(biāo)定等人機(jī)交互接ロ。所述電源子系統(tǒng)包括鋰電池組和充放電保護(hù)模塊兩部分，鋰電池組為清潔機(jī)器人提供48V工作電壓，充放電保護(hù)模塊負(fù)責(zé)在電池組充電時(shí)將220V電壓變換成充電電壓，在電池組放電時(shí)提供放電保護(hù)及實(shí)時(shí)告警，必要時(shí)并斷掉電源。所述吸塵子系統(tǒng)由起塵模塊、濾塵模塊、吸塵模塊、儲(chǔ)塵模塊組成。所述行走子系統(tǒng)包括減速機(jī)構(gòu)、車輪、電機(jī)驅(qū)動(dòng)、步進(jìn)電機(jī)等模塊。所述測(cè)量子系統(tǒng)由方向角測(cè)量、傾角測(cè)量、超聲波測(cè)距、高度測(cè)量等模塊組成。方向角測(cè)量的核心部件為電子指南針，測(cè)量視覺子系統(tǒng)的方向角。傾角測(cè)量負(fù)責(zé)測(cè)量機(jī)器人系統(tǒng)的傾角，用作位姿判定依據(jù)。超聲波測(cè)距由超聲波傳感器實(shí)現(xiàn)，其原理是通過(guò)發(fā)射聲波與接收聲波的時(shí)間差計(jì)算距離。所述視覺子系統(tǒng)包括結(jié)構(gòu)光發(fā)射器和雙目CCD傳感器兩部分，結(jié)構(gòu)光發(fā)射器向前方投射線結(jié)構(gòu)光，雙目CCD傳感器采集場(chǎng)景圖像。一種機(jī)器人地圖構(gòu)建方法，包括以下步驟
1)將左右雙目CXD拍攝到的圖像分別記為匕、Pe,以線結(jié)構(gòu)光為界，將匕和Pk兩幅圖像分別分為左右兩部分，即Pu與Pu^ Pel與PKK，并提取圖像的邊緣、角點(diǎn)等幾何特征；
2)以Pui最靠近線結(jié)構(gòu)光部分的幾何特征為基準(zhǔn)，在Pkk中尋求立體匹配，若找到匹配特征則轉(zhuǎn)入第6)步，若未找到匹配特征則說(shuō)明左CXD或右CXD前有近距離障礙，繼續(xù)進(jìn)入第3)步進(jìn)行判定；
3)首先檢查左CCD前是否有近距離障礙，啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)逆時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于の，在每ー步旋轉(zhuǎn)后都使用超聲波進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcosO)的障礙，說(shuō)明左CXD前無(wú)近距離障礙，轉(zhuǎn)入第4)步對(duì)右CXD前是否有近距離障礙作檢測(cè)，若在某一步中檢測(cè)到距離小于bcosO)的障礙，則說(shuō)明左CXD前有近距離障礙，記錄該點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，控制云臺(tái)歸位后再順時(shí)針旋轉(zhuǎn)15°，啟動(dòng)雙目視覺測(cè)距，轉(zhuǎn)入第I)歩；
4)控制云臺(tái)歸位，啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)順時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于の，在每ー步旋轉(zhuǎn)后都進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcosの的障礙，說(shuō)明右CXD前無(wú)近距離障礙，`轉(zhuǎn)入第5)步，若在某一步中檢測(cè)到距離小于bcosO)的障礙，則說(shuō)明右CXD前有近距離障礙，轉(zhuǎn)入第6)步；
5)控制云臺(tái)歸位，然后順時(shí)針旋轉(zhuǎn)15°，轉(zhuǎn)入第I)歩；
6)記錄該障礙點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，計(jì)算該點(diǎn)方向角，據(jù)此方向角控制云臺(tái)旋轉(zhuǎn)，使視覺子系統(tǒng)朝向該特征點(diǎn)，執(zhí)行第7)步；
7)啟動(dòng)方向角計(jì)算，若方向角偏正南方向大于15°或特征點(diǎn)數(shù)小于24個(gè)，則轉(zhuǎn)入第2)步，否則控制云臺(tái)面向正南方向，根據(jù)獲取的特征點(diǎn)信息，繪制柵格地圖，結(jié)束地圖構(gòu)建。一種機(jī)器人行進(jìn)中參考點(diǎn)的選取方法，其特征在于，包括以下步驟
1)獲得室內(nèi)清潔機(jī)器人初始位置正南方向的左右兩張CXD圖像和Pk；
2)對(duì)I\、PK進(jìn)行局部不變特征檢測(cè)，獲取圖像中不隨光照、方向變化的局部特征；
3)對(duì)特征點(diǎn)進(jìn)行立體視覺匹配，并進(jìn)行視覺測(cè)距，選取最近的特征點(diǎn)計(jì)算其方向角；
4)根據(jù)第3)步得到的方向角控制云臺(tái)將視覺系統(tǒng)正對(duì)特征點(diǎn)，啟動(dòng)超聲測(cè)距；
5)若超聲測(cè)距與視覺測(cè)距在誤差范圍之內(nèi)，則選定該特征點(diǎn)作為參考點(diǎn)并結(jié)束，否則選其他特征點(diǎn)嘗試以上過(guò)程，直到找到特征點(diǎn)。有益效果本發(fā)明構(gòu)建的地圖具有距離準(zhǔn)確、細(xì)粒度、信息豐富的特點(diǎn)，利用超聲測(cè)距和線結(jié)構(gòu)光雙目測(cè)距的方法，避免了光照顏色等易變化的外界因素對(duì)地圖構(gòu)建和特征點(diǎn)選取的影響。自定位方法以視覺圖像的局部不變特征作為參考點(diǎn)，提高了清潔機(jī)器人在室內(nèi)非結(jié)構(gòu)化環(huán)境中的適應(yīng)能力和清潔效率，且無(wú)需額外基站。


圖1為實(shí)施例提供的室內(nèi)清潔機(jī)器人功能模塊示意 圖2為實(shí)施例提供的室內(nèi)清潔機(jī)器人視覺子系統(tǒng)的結(jié)構(gòu)示意 圖3為實(shí)施例提供的室內(nèi)清潔機(jī)器人初始化流程 圖4為實(shí)施例提供的線結(jié)構(gòu)光視覺系統(tǒng)的視場(chǎng)模型示意 圖5為實(shí)施例提供的初始地圖構(gòu)建流程 圖6為實(shí)施例提供的自定位技術(shù)的參考點(diǎn)選取流程 圖7為實(shí)施例提供的行進(jìn)檢測(cè)邏輯 圖8為實(shí)施例提供的地圖修正流程圖。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施例，進(jìn)一步闡明本發(fā)明，應(yīng)理解這些實(shí)施例僅用于說(shuō)明本發(fā)明而不用于限制本發(fā)明的范圍，在閱讀了本發(fā)明之后，本領(lǐng)域技術(shù)人員對(duì)本發(fā)明的各種等價(jià)形式的修改均落于本申請(qǐng)所附權(quán)利要求所限定的范圍。如圖1所示，本發(fā)明提出了一種基于機(jī)器視覺和多傳感器信息融合的室內(nèi)清潔機(jī)器人，包括進(jìn)行數(shù)據(jù)處理的中央子系統(tǒng)，拍攝圖像的視覺子系統(tǒng)，測(cè)量與障礙物距離的測(cè)量子系統(tǒng)，行走子系統(tǒng)，吸塵子系統(tǒng)，以及給本裝置所有子系統(tǒng)提供電力供應(yīng)的電源子系統(tǒng)。所述中央子系統(tǒng)01由信息采集模塊11、信息處理模塊12、控制輸出模塊13、人機(jī)交互模塊14組成。信息采集模塊11負(fù)責(zé)采集來(lái)自測(cè)量子系統(tǒng)和視覺子系統(tǒng)的傳感器數(shù)據(jù)，這些數(shù)據(jù)傳遞給信息處理模塊12。信息處理模塊12對(duì)傳感器數(shù)據(jù)進(jìn)行信息處理、數(shù)據(jù)融合和判斷決策。控制輸出模塊13根據(jù)信息處理模塊12的判斷決策向吸塵子系統(tǒng)03、行走子系統(tǒng)04輸出控制信號(hào)。人機(jī)交互模塊14提供系統(tǒng)設(shè)置、系統(tǒng)標(biāo)定等人機(jī)交互接口。所述電源子系統(tǒng)02包括鋰電池組15和充放電保護(hù)模塊16兩部分，鋰電池組15為清潔機(jī)器人提供48V工作電壓，充放電保護(hù)模塊16負(fù)責(zé)在電池組充電時(shí)將220V電壓變換成充電電壓，在電池組放電時(shí)提供放電保護(hù)及實(shí)時(shí)告警，必要時(shí)并斷掉電源。所述吸塵子系統(tǒng)03由起塵模塊17、濾塵模塊18、吸塵模塊19、儲(chǔ)塵模塊20組成。所述行走子系統(tǒng)04包括減速機(jī)構(gòu)21、車輪22、電機(jī)驅(qū)動(dòng)23、步進(jìn)電機(jī)24等模塊。所述測(cè)量子系統(tǒng)05由方向角測(cè)量25、傾角測(cè)量26、超聲波測(cè)距27、高度測(cè)量28等模塊組成。方向角測(cè)量25的核心部件為電子指南針，測(cè)量視覺子系統(tǒng)的方向角。傾角測(cè)量26負(fù)責(zé)測(cè)量機(jī)器人系統(tǒng)的傾角，用作位姿判定依據(jù)。超聲波測(cè)距27由超聲波傳感器實(shí)現(xiàn)，其原理是通過(guò)發(fā)射聲波與接收聲波的時(shí)間差計(jì)算距離。所述視覺子系統(tǒng)05包括結(jié)構(gòu)光發(fā)射器29和雙目CCD傳感器30兩部分，結(jié)構(gòu)光發(fā)射器29向前方投射線結(jié)構(gòu)光，雙目CCD傳感器30采集場(chǎng)景圖像。圖2是視覺子系統(tǒng)的結(jié)構(gòu)示意圖。該裝置安裝有雙目CXD傳感器30、線結(jié)構(gòu)光投射器29、超聲測(cè)距傳感器27、方向角傳感器25，整個(gè)裝置固定在云臺(tái)31上，云臺(tái)31可以在中央子系統(tǒng)01的控制下旋轉(zhuǎn)指定角度。
本發(fā)明清潔機(jī)器人系統(tǒng)上電后，執(zhí)行系統(tǒng)自檢。自檢流程如圖3所示，第I步進(jìn)行電源子系統(tǒng)自檢，檢測(cè)電池組當(dāng)前電壓及每節(jié)電池的工作狀態(tài)是否正常；第2步檢測(cè)行走子系統(tǒng)，獲取電機(jī)工作信號(hào)；第3步進(jìn)行基本輸入輸出自檢，檢測(cè)人機(jī)交互接ロ工作是否正常；第4步檢測(cè)傾斜角模塊工作狀態(tài)；第5步進(jìn)行電子指南針模塊檢測(cè);第6步高度傳感器檢測(cè)；第7步視覺系統(tǒng)檢測(cè)，測(cè)試CCD傳感器及線結(jié)構(gòu)光投射器工作情況；第8步進(jìn)行超聲模塊檢測(cè)，測(cè)試超聲發(fā)射與接收是否正常。清潔機(jī)器人自檢完成后，系統(tǒng)啟動(dòng)地圖構(gòu)建程序，檢測(cè)非結(jié)構(gòu)化環(huán)境信息，建立清掃區(qū)域的初始地圖。本發(fā)明構(gòu)建初始地圖的基本思想是結(jié)合線結(jié)構(gòu)光雙目視覺測(cè)距與超聲測(cè)距的優(yōu)勢(shì)，運(yùn)用方向角傳感器(電子指南針)提供的方向信息，建立具有明確方向信息、細(xì)粒度距離信息、圖像特征信息的柵格地圖。線結(jié)構(gòu)光雙目視覺系統(tǒng)的模型如圖4所示，實(shí)心黒點(diǎn)分別表示左CXD和右CXD的小孔成像模型，間距為b，點(diǎn)線相間虛線表示兩CXD的視場(chǎng)，視場(chǎng)角為2の，點(diǎn)虛線表示線結(jié)構(gòu)光，顯然，出現(xiàn)在兩CCD視場(chǎng)非相交區(qū)域的障礙物不能在兩CXD圖像中找到立體匹配，因此在建立地圖時(shí)，需要考慮此類現(xiàn)象對(duì)立體匹配的影響。地圖構(gòu)建方法的程序流程如圖5所示，首先進(jìn)行視覺測(cè)量，使用磁阻傳感器測(cè)量視覺子系統(tǒng)的方向角，據(jù)此方向角，控制云臺(tái)旋轉(zhuǎn)使視覺子系統(tǒng)面向正南方向。然后啟動(dòng)線結(jié)構(gòu)光雙目測(cè)距得到距離SA1，以及超聲測(cè)距得到距離SA2。由于線結(jié)構(gòu)光雙目測(cè)距易受光照等因素的影響不一定能準(zhǔn)確測(cè)量障礙物距離，因此將Sai和Sa2距離比對(duì)，若兩個(gè)距離之差在誤差范圍內(nèi)，則記錄線結(jié)構(gòu)光特征點(diǎn)的地圖信息，開始進(jìn)行地圖構(gòu)建。接下來(lái)進(jìn)行細(xì)粒度的旋轉(zhuǎn)測(cè)距，獲取建立地圖所需的至少24個(gè)特征點(diǎn)的圖像特征及距離信息，涉及雙目立體視覺特征提取、特征匹配等技術(shù)，具體地可分為如下步驟
1)將左右雙目CXD拍攝到的圖像分別記為匕、Pe,以線結(jié)構(gòu)光為界，將匕和Pk兩幅圖像分別分為左右兩部分，即Pu與Pu^ Pel與PKK，并提取圖像的邊緣、角點(diǎn)等幾何特征； 2)以Pui最靠近線結(jié)構(gòu)光部分的幾何特征為基準(zhǔn)，在Pkk中尋求立體匹配，若找到匹配特征則轉(zhuǎn)入第6)步，若未找到匹配特征則說(shuō)明左CXD或右CXD前有近距離障礙，繼續(xù)進(jìn)入第3)步進(jìn)行判定；
3)首先檢查左CCD前是否有近距離障礙，啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)逆時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于の，在每ー步旋轉(zhuǎn)后都使用超聲波進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcosO)的障礙，說(shuō)明左CXD前無(wú)近距離障礙，轉(zhuǎn)入第4)步對(duì)右CXD前是否有近距離障礙作檢測(cè)，若在某一步中檢測(cè)到距離小于bcosO)的障礙，則說(shuō)明左CXD前有近距離障礙，記錄該點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，控制云臺(tái)歸位后再順時(shí)針旋轉(zhuǎn)15°，啟動(dòng)雙目視覺測(cè)距，轉(zhuǎn)入第I)歩；
4)控制云臺(tái)歸位，啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)順時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于の，在每ー步旋轉(zhuǎn)后都進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcosの的障礙，說(shuō)明右C⑶前無(wú)近距離障礙，轉(zhuǎn)入第5)步，若在某一步中檢測(cè)到距離小于bcosO)的障礙，則說(shuō)明右CXD前有近距離障礙，轉(zhuǎn)入第6)步；
5)控制云臺(tái)歸位，然后順時(shí)針旋轉(zhuǎn)15°，轉(zhuǎn)入第I)歩；
6)記錄該障礙點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，計(jì)算該點(diǎn)方向角，據(jù)此方向角控制云臺(tái)旋轉(zhuǎn)，使視覺子系統(tǒng)朝向該特征點(diǎn)，執(zhí)行第7)步；
7)啟動(dòng)方向角計(jì)算，若方向角偏正南方向大于15°或特征點(diǎn)數(shù)小于24個(gè)，則轉(zhuǎn)入第2)步，否則控制云臺(tái)面向正南方向，根據(jù)獲取的特征點(diǎn)信息，繪制柵格地圖，結(jié)束地圖構(gòu)建。本發(fā)明清潔機(jī)器人在行進(jìn)中不僅可以檢測(cè)到明確的方向信息，還可以借助于參考點(diǎn)獲取準(zhǔn)確的行進(jìn)距離。關(guān)于參考點(diǎn)的選取，本發(fā)明采用局部不變特征檢測(cè)技術(shù)獲取局部不變特征作為室內(nèi)清潔機(jī)器人行進(jìn)距離計(jì)算的參考點(diǎn)。所謂局部不變特征即參考點(diǎn)選取流程如圖6所示第I步，獲得室內(nèi)清潔機(jī)器人初始位置正南方向的兩C⑶圖像I\、PK ;第2步，對(duì)I\、PK進(jìn)行局部不變特征檢測(cè)，獲取圖像中不隨光照、方向變化的局部特征；第3步，對(duì)特征點(diǎn)進(jìn)行立體視覺匹配，并進(jìn)行視覺測(cè)距，選取最近的特征點(diǎn)計(jì)算其方向角；第4步，根據(jù)第3步得到的方向角控制云臺(tái)將視覺系統(tǒng)正對(duì)特征點(diǎn)，啟動(dòng)超聲測(cè)距；第5步，若超聲測(cè)距與視覺測(cè)距在誤差范圍之內(nèi)，則選定該特征點(diǎn)作為參考點(diǎn)并結(jié)束，否則選其他特征點(diǎn)嘗試以上過(guò)程。為降低生成行進(jìn)策略的運(yùn)算量，在機(jī)器人行進(jìn)過(guò)程中，本發(fā)明不使用視覺測(cè)距，而啟用前方、左側(cè)、右側(cè)超聲測(cè)距和下視高度測(cè)量，行進(jìn)檢測(cè)所涉及的符號(hào)的含義如下
Dl—超聲測(cè)距得到的前方障礙距離；
D2—超聲測(cè)距得到的左側(cè)障礙距離；
D3—超聲測(cè)距得到的右側(cè)障礙距離；
D4——高度測(cè)量模塊檢測(cè)得到的高度值；
At 減速慢行臨界值；
K—下視高度臨界值；
——系統(tǒng)根據(jù)當(dāng)前地圖及行進(jìn)距離計(jì)算得到的前方障礙距離取值范圍； UJ——系統(tǒng)根據(jù)當(dāng)前地圖及行進(jìn)距離計(jì)算得到的左側(cè)障礙距離取值范圍；
—系統(tǒng)根據(jù)當(dāng)前地圖及行進(jìn)距離計(jì)算得到的右側(cè)障礙距離取值范圍。如圖7所示，室內(nèi)清潔機(jī)器人在行進(jìn)中，使用多傳感器不斷檢測(cè)前方、左側(cè)、右側(cè)、下視距離，并根據(jù)當(dāng)前地圖和行進(jìn)距離計(jì)算上述距離的取值范圍，然后做出行動(dòng)決策。正常情況下，Dl應(yīng)在前方障礙距離取值范圍內(nèi)，D2在左側(cè)障礙距離取值范圍內(nèi)，D3在右側(cè)障礙距離取值范圍內(nèi)。當(dāng)Dl、D2、D3任意一個(gè)不在其取值范圍或者D4大于下視高度臨界值時(shí)，說(shuō)明當(dāng)前地圖與實(shí)際環(huán)境不符，系統(tǒng)啟動(dòng)地圖修正程序；當(dāng)D1、D2、D3每個(gè)都在其取值范圍并且D4小于等于下視高度臨界值，但Dl大于減速慢行臨界值時(shí)，說(shuō)明地圖準(zhǔn)確且前方距離安全，室內(nèi)清潔機(jī)器人勻速前進(jìn)；當(dāng)Dl、D2、D3每個(gè)都在其取值范圍并且D4小于等于下視高度臨界值，但Dl也小于等于減速慢行臨界值時(shí)，說(shuō)明地圖準(zhǔn)確但前方已逼近障礙物，室內(nèi)清潔機(jī)器人減速前進(jìn)。在室內(nèi)清潔機(jī)器人行進(jìn)中，若發(fā)現(xiàn)實(shí)際環(huán)境與當(dāng)前地圖不一致，則應(yīng)啟動(dòng)地圖修正程序，其流程如圖8所示第I步，根據(jù)方向角數(shù)據(jù)，控制云臺(tái)旋轉(zhuǎn)，使視覺系統(tǒng)面向正南方向，獲取左右CCD圖像PpPK ;第2步，對(duì)I\、PK進(jìn)行參考點(diǎn)檢測(cè)，若檢測(cè)到參考點(diǎn)，則轉(zhuǎn)第3步，否則轉(zhuǎn)第5步；第3步，計(jì)算參考點(diǎn)方向角，控制云臺(tái)使視覺系 統(tǒng)正對(duì)參考點(diǎn)，啟動(dòng)線結(jié)構(gòu)光雙目視覺測(cè)距^和前方超聲測(cè)距Ds，比較兩者是否在誤差范圍相等，若相等則轉(zhuǎn)入第4步，否則發(fā)出系統(tǒng)標(biāo)定警告；第4步，根據(jù)行進(jìn)距離和當(dāng)前地圖計(jì)算室內(nèi)清潔機(jī)器人當(dāng)前位置距參考點(diǎn)的距離Dm，若該距離與超聲測(cè)距在誤差內(nèi)相等，則直接轉(zhuǎn)第5步，否則修正當(dāng)前位置地圖坐標(biāo)后轉(zhuǎn)第5歩；第5步，控制云臺(tái)旋轉(zhuǎn)，使視覺系統(tǒng)面向距離越限方向，并啟動(dòng)視覺測(cè)距和圖像特征提取，以此新特征信息修正當(dāng)前地圖，結(jié)束。
權(quán)利要求
1.一種清潔機(jī)器人系統(tǒng)，其特征在于，包括進(jìn)行數(shù)據(jù)處理的中央子系統(tǒng)，拍攝圖像的視覺子系統(tǒng)，測(cè)量與障礙物距離的測(cè)量子系統(tǒng)，行走子系統(tǒng)，吸塵子系統(tǒng)，以及給本裝置所有子系統(tǒng)提供電力供應(yīng)的電源子系統(tǒng)； 其中行走子系統(tǒng)和吸塵子系統(tǒng)受到中央子系統(tǒng)的控制，視覺子系統(tǒng)將其拍攝的圖像發(fā)送到中央子系統(tǒng)，測(cè)量子系統(tǒng)將測(cè)量的數(shù)據(jù)發(fā)送給中央子系統(tǒng)。
2.根據(jù)權(quán)利要求1所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述中央子系統(tǒng)由信息采集模塊、信息處理模塊、控制輸出模塊和人機(jī)交互模塊組成。
3.根據(jù)權(quán)利要求2所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述視覺子系統(tǒng)由雙目CCD和結(jié)構(gòu)光發(fā)射器組成。
4.根據(jù)權(quán)利要求3所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述測(cè)量子系統(tǒng)由超聲波測(cè)距、傾角測(cè)量、方向角測(cè)量和高度測(cè)量組成。
5.根據(jù)權(quán)利要求4所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述電源子系統(tǒng)由鋰電池組合充放電保護(hù)模塊組成。
6.根據(jù)權(quán)利要求5所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述行走子系統(tǒng)由電機(jī)驅(qū)動(dòng)、步進(jìn)電機(jī)、減速機(jī)構(gòu)和車輪組成。
7.根據(jù)權(quán)利要求6所述的清潔機(jī)器人系統(tǒng)，其特征在于，所述吸塵子系統(tǒng)由起塵模塊、濾塵模塊、吸塵模塊和儲(chǔ)塵模塊組成。
8.一種機(jī)器人地圖構(gòu)建方法，其特征在于，包括以下步驟 1)將左右雙目CXD拍攝到的圖像分別記為匕、Pe,以線結(jié)構(gòu)光為界，將匕和Pk兩幅圖像分別分為左右兩部分，即Pu與Pu^ Pel與Ρκκ，并提取圖像的幾何特征； 2)以Pui最靠近線結(jié)構(gòu)光部分的幾何特征為基準(zhǔn)，在Pkk中尋求立體匹配，若找到匹配特征則轉(zhuǎn)入第6)步，若未找到匹配特征則說(shuō)明左CXD或右CXD前有近距離障礙，繼續(xù)進(jìn)入第3)步進(jìn)行判定； 3)啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)逆時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于Φ，在每一步旋轉(zhuǎn)后都進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcosO的障礙，說(shuō)明左CCD前無(wú)近距離障礙，轉(zhuǎn)入第4)步，若在某一步中檢測(cè)到距離小于bcosO的障礙，則說(shuō)明左CXD前有近距離障礙，記錄該點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，控制云臺(tái)歸位后再順時(shí)針旋轉(zhuǎn)15°，啟動(dòng)雙目視覺測(cè)距，轉(zhuǎn)入第I)步； 4)控制云臺(tái)歸位，啟動(dòng)超聲測(cè)距，控制云臺(tái)以3°為步長(zhǎng)順時(shí)針旋轉(zhuǎn)直到累計(jì)偏轉(zhuǎn)角度大于等于Φ，在每一步旋轉(zhuǎn)后都進(jìn)行障礙物探測(cè)，若最終未檢測(cè)到距離小于bcoscD的障礙，說(shuō)明右CXD前無(wú)近距離障礙，轉(zhuǎn)入第5)步，若在某一步中檢測(cè)到距離小于bcosO的障礙，則說(shuō)明右CXD前有近距離障礙，轉(zhuǎn)入第6)步； 5)控制云臺(tái)歸位，然后順時(shí)針旋轉(zhuǎn)15°，轉(zhuǎn)入第I)步； 6)記錄該障礙點(diǎn)的圖像特征及距離信息作為特征點(diǎn)，計(jì)算該點(diǎn)方向角，據(jù)此方向角控制云臺(tái)旋轉(zhuǎn)，使視覺子系統(tǒng)朝向該特征點(diǎn)，執(zhí)行第7)步； 7)啟動(dòng)方向角計(jì)算，若方向角偏正南方向大于15°或特征點(diǎn)數(shù)小于24個(gè)，則轉(zhuǎn)入第2)步，否則控制云臺(tái)面向正南方向，根據(jù)獲取的特征點(diǎn)信息，繪制柵格地圖，結(jié)束地圖構(gòu)建。
9.一種機(jī)器人行進(jìn)中參考點(diǎn)的選取方法，其特征在于，包括以下步驟 O獲得室內(nèi)清潔機(jī)器人初始位置正南方向的左右兩張CXD圖像和Pk ；2)對(duì)Pl、Pr進(jìn)行局部不變特征檢測(cè)，獲取圖像中不隨光照、方向變化的局部特征； 3)對(duì)特征點(diǎn)進(jìn)行立體視覺匹配，并進(jìn)行視覺測(cè)距，選取最近的特征點(diǎn)計(jì)算其方向角； 4)根據(jù)第3)步得到的方向角控制云臺(tái)將視覺系統(tǒng)正對(duì)特征點(diǎn)，啟動(dòng)超聲測(cè)距； 5)若超聲測(cè)距與視覺測(cè)距在誤差范圍之內(nèi)，則選定該特征點(diǎn)作為參考點(diǎn)并結(jié)束，否則選其他特征點(diǎn)嘗試以上過(guò)程，直到找到特征點(diǎn)。
全文摘要
本發(fā)明公開了一種基于視覺測(cè)量的清潔機(jī)器人系統(tǒng)及其測(cè)控方法，該室內(nèi)清潔機(jī)器人系統(tǒng)不僅包括中央子系統(tǒng)、吸塵子系統(tǒng)、行進(jìn)子系統(tǒng)、測(cè)量子系統(tǒng)和電源子系統(tǒng)，還包括視覺子系統(tǒng)，特別地，測(cè)量子系統(tǒng)中還引入了方向角傳感器(電子指南針)。室內(nèi)清潔機(jī)器人的測(cè)控方法包括地圖構(gòu)建方法、行進(jìn)檢測(cè)機(jī)制、地圖修正方法、自定位方法等。結(jié)合線結(jié)構(gòu)光雙目立體視覺和超聲兩種測(cè)距手段，融合視覺圖像特征構(gòu)建地圖，使地圖具有準(zhǔn)確、細(xì)粒度、信息豐富的特點(diǎn)；自定位方法以視覺圖像的局部不變特征作為參考點(diǎn)，無(wú)需額外基站。本發(fā)明提高了室內(nèi)清潔機(jī)器人對(duì)非結(jié)構(gòu)化環(huán)境的適應(yīng)能力、行進(jìn)估計(jì)準(zhǔn)確度和清潔效率。
文檔編號(hào)A47L11/24GK103054522SQ20121058868
公開日2013年4月24日 申請(qǐng)日期2012年12月31日 優(yōu)先權(quán)日2012年12月31日
發(fā)明者韓磊, 徐立中, 王鑫, 沈潔 申請(qǐng)人:河海大學(xué)