確的實現過門的運動。
[0099] 本領域內的技術人員應明白，本發明的實施例可提供為方法、系統、或計算機程序 產品。因此，本發明可采用完全硬件實施例、完全軟件實施例、或結合軟件和硬件方面的實 施例的形式。而且，本發明可采用在一個或多個其中包含有計算機可用程序代碼的計算機 可用存儲介質（包括但不限于磁盤存儲器和光學存儲器等）上實施的計算機程序產品的形 式。
[0100] 本發明是參照根據本發明實施例的方法、設備（系統）、和計算機程序產品的流程 圖和/或方框圖來描述的。應理解可由計算機程序指令實現流程圖和/或方框圖中的每一流 程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結合。可提供這些計算機程序 指令到通用計算機、專用計算機、嵌入式處理機或其他可編程數據處理設備的處理器以產 生一個機器，使得通過計算機或其他可編程數據處理設備的處理器執行的指令產生用于實 現在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的裝置。
[0101] 這些計算機程序指令也可存儲在能引導計算機或其他可編程數據處理設備以特 定方式工作的計算機可讀存儲器中，使得存儲在該計算機可讀存儲器中的指令產生包括指 令裝置的制造品，該指令裝置實現在流程圖一個流程或多個流程和/或方框圖一個方框或 多個方框中指定的功能。
[0102] 這些計算機程序指令也可裝載到計算機或其他可編程數據處理設備上，使得在計 算機或其他可編程設備上執行一系列操作步驟以產生計算機實現的處理，從而在計算機或 其他可編程設備上執行的指令提供用于實現在流程圖一個流程或多個流程和/或方框圖一 個方框或多個方框中指定的功能的步驟。
[0103] 顯然，本領域的技術人員可以對本發明進行各種改動和變型而不脫離本發明的精 神和范圍。這樣，倘若本發明的這些修改和變型屬于本發明權利要求及其等同技術的范圍 之內，則本發明也意圖包含這些改動和變型在內。
【主權項】
1. 一種基于視覺的機器人導航過門方法，其特征在于，包括以下步驟： 5101、 根據里程計或全局定位傳感器得到機器人當前時刻在全局坐標系中的位置和姿 態參數(x，y，9)，x為機器人相對于原點的橫坐標，y為機器人相對于原點的縱坐標，Θ為機器 人朝向相對于X軸的夾角，逆時針旋轉為正值角度夾角，順時針旋轉為負值角度夾角； 5102、 根據用戶指令確定機器人所要通過的門； 5103、 確定機器人過門之前開啟攝像頭的區域，每個開啟攝像頭區域為:Areai=(xj, yk)(l< _]_^1<1^1〇，其中1代表門的編號4和1表示某個門前區域的范圍； 5104、 計算此次導航終點； 5105、 起點和終點確定后，由A*算法計算機器人行走路徑，控制機器人運動到導航終 占. 5106、 根據里程計返回的航位推測信息（&，7^0〇，計算機器人相對門的位置和姿態， 求出機器人朝向和機器人與門中點連線的夾角△ Θ，控制機器人轉動△ Θ使機器人朝向門； 5107、 機器人靜止，同時下位機向上位機發送請求開啟攝像頭命令，上位機打開位于機 器人胸前的雙目攝像頭，并開始采集圖像，進行門識別； 5108、 根據圖像處理返回的門左右兩邊框的坐標11，71^1^，71^(11〈11^71〈71〇計算門中心 坐標(XQ，yo)，其中，5109、 以XQ，y。為原點，機器人當前朝向為基準，以從門左邊框指向右邊框為X軸正向建 立笛卡爾坐標系XOY; 5110、 求出機器人在XOY坐標系下的坐標(乂^乂^^^:八并求出機器人相對過門過程 中第一個矯正點(xii，yn)的距離Δ di和角度Δ Θ:，其中， -200mm<xn<200mm? -I400mm<yn<-1000mm； 5111、 控制機器人直行AcU，然后轉彎ΔΘ:，根據航位推測信息求出機器人當前朝向與 門中垂線夾角Α θ':，控制機器人轉過Δ θ':角度對正門； 5112、 求出機器人在XOY坐標系下的坐標(1\2,7\2，0\2)，求出機器人相對過門過程中 第二個矯正點(xi2，yi2)的距離Δ d2和角度Δ θ2,其中， -200mm<xi2<200mm? -1000mm<yi2<_600mm ； 5113、 控制機器人直行Acb，然后轉彎Δθ2,根據航位推測信息計算出機器人當前朝向 與門中垂線夾角Α θ' 2,使機器人轉過Δ θ' 2對正門； 5114、 求出機器人在ΧΟΥ坐標系下的坐標(1\3,7\3，0\3)，求出機器人相對過門過程中 第三個矯正點(xi3，yi3)的距離Δ d3和角度Δ θ3,其中， -200mm<xi3<200mm? -600mm<yi3<-300mm ； 5115、 控制機器人直行Δ d3,然后轉彎Δ θ3,根據航位推測信息求出機器人當前朝向與 門中垂線夾角Α θ' 3,使機器人轉過Δ θ' 3對正門； S116、控制機器人前進Δ d，其中800mm〈 Δ d〈 1200mm，使機器人完成進門操作。2. 根據權利要求1所述的方法，其特征在于，在步驟S104中，所述計算此次導航終點，進 一步包括： 對參考區域進行搜索，求出參考區域中每個參考點到機器人當前位置的歐幾里得距離 艮I其中X，y代表某個柵格在柵格地圖中的坐標； 對所有求得的cU排序求得cU的最小值； CU最小值所相應的柵格位置便是機器人此次導航的終點。3. 根據權利要求1所述的方法，其特征在于，在步驟S107中，所述進行門識別，進一步包 括： 5201、 上位機對雙目攝像頭采集到的左右兩幅RBG圖像h、Ir分別進行灰度化處理，得到 灰度圖像Gray 1、Gray r; 5202、 采用圖像全局二值化算法(如閾值法、OSTU大律法、Shanbhag法等)對得到的灰度 圖像Grayi、Grayr分別進行二值化處理得到二值圖像Binaryi、Binary r; 5203、 使用形狀為矩形(a〈b，且a取值為1或2，b取值范圍為b〈h&b>h.2,其中h為采集得 到圖像的高）的結構元素，進行圖像形態學的腐蝕、膨脹處理獲得二值圖像Binary、、 Binary^*的垂直條狀區±?,該區域即可能 為門框或門扇存在的區域； 5204、 以步驟S203中得到的垂直條狀區域為匹配基元，對辦和心中的垂直條狀區域進行 匹配； 5205、 對步驟S204中得到的垂直條狀區域對左右兩側對應點應用雙目測距原理得出該 點在機器人坐標系下的位置信息(X和y)，同時得到圖像中各個垂直條狀區域的實際寬度信 息5206、 結合家居環境下門的實際寬度為850mm-1200mm，可得到符合實際門寬度的垂直 條狀區域(一個垂直條狀區域（門扇+左右兩側門框）、兩個垂直條狀區域（門框和門框、門框 和門扇）），進一步把得到的垂直條狀區域認為是門，并將門左右兩邊框在機器人坐標系下 的坐標11，71，&，^返回下位機（當為一個垂直條狀區域（門扇）時，返回垂直條狀區域兩側 的坐標；當為兩個垂直條狀區域（門框和門框)時，返回兩個垂直條狀區域外側的坐標;當為 兩個垂直條狀區域（門框和門扇)時，返回兩個垂直條狀區域內側的坐標）。4. 根據權利要求3所述的方法，其特征在于，在步驟S204中，所述對辦和心中的垂直條狀 區域進行匹配，進一步包括： S301、根據雙目立體匹配原則中同一物體在左右兩個攝像頭中的成像存在視差，且對 于光軸平行的雙目測距模型，其在左攝像頭圖像坐標系下成像點的XI值大于其在右攝像頭 圖像坐標系下成像點的Xr，即Ri中的元素構對應R沖的匹配項蹲滿足<4,其中硌為 沒|、區域中心點的X值，4為4區域中心點的X值，選擇4 <4的成像點。 5302、 根據雙目立體匹配原則中幾何相似性約束，即心中的元素 Ι?對應Rr中的匹配項 茜足其幾何特征相差不大，包括但不限于區域寬度w、顏色(RGB色彩空間、HSV色彩空間、 YUV色彩空間等）、紋理(LBP等)特征，通過對上述特征相似度進行度量： 對于寬度特征'分別為/的寬度，η為Rr滿足約束的 區域總數量； 對其他特征：采用直方圖統計，并采用相似性度量（如歐式距離、馬氏距離、巴氏距離 等)的方式進行相似性計算可得特征對應相似度Sf(3ature3; 最終可得其相似度為U = ΣΡ=〇 Q · A Q = 1); 選擇相似度最大的蔥I為翁f的匹配項。 5303、 返回S301，重復S30US302,得到左右圖像中所有的匹配區域。
【專利摘要】本發明公開了一種基于視覺的機器人導航過門方法，該方法包括：根據傳感器得到機器人在全局坐標系中的位置和姿態參數；確定機器人所要通過的門；確定機器人開啟攝像頭的區域；計算導航終點；機器人運動到導航終點；機器人開啟雙攝像頭進行門識別；以門中心為原點建立笛卡爾坐標；控制機器人分別到達3個矯正點；穿過門完成過門操作。本發明技術方案根據視覺返回的信息計算門和機器人的相對位置關系，根據里程計返回的信息計算機器人的全局位置信息，對機器人的位置和姿勢參數進行有效的獲取，從而控制機器人精確的實現過門的運動。
【IPC分類】G05D1/02, G01C21/20
【公開號】CN105717928
【申請號】CN201610266610
【發明人】王曉剛, 王玉亮, 喬濤, 薛林, 王巍
【申請人】北京進化者機器人科技有限公司
【公開日】2016年6月29日
【申請日】2016年4月26日