一種基于單視覺的機器人室內(nèi)定位和導航方法
【技術(shù)領域】
[0001] 本發(fā)明涉及一種基于單視覺的機器人室內(nèi)定位和導航方法。
【背景技術(shù)】
[0002] 機器人定位、導航與路徑規(guī)劃功能在機器人領域非常重要，目前大部分導航技術(shù) 都要通過專業(yè)的輔助設備來進行，這些設備有的價格昂貴，有的對環(huán)境要求非常高，這在很 大程度上提高了該領域的門檻，使機器人的成本居高不下，且降低了環(huán)境的適應性。

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

[0003] 針對上述問題，本發(fā)明提供一種基于單視覺的機器人室內(nèi)定位和導航方法，機器 人可以在室內(nèi)完成路徑規(guī)劃、定位導航等功能，成本低，方便控制，對環(huán)境的適應性更好。
[0004] 為實現(xiàn)上述技術(shù)目的，達到上述技術(shù)效果，本發(fā)明通過以下技術(shù)方案實現(xiàn)：
[0005] -種基于單視覺的機器人室內(nèi)定位和導航方法，其特征在于，具體包括如下步 驟：
[0006] 步驟Sl :初次使用時，需要創(chuàng)建房間的室內(nèi)地圖，具體步驟如下：
[0007] A):機器人設置有單目攝像頭、電子羅盤和聲納傳感器，且在室內(nèi)屋頂設置有三個 可識別的標記點，機器人以室內(nèi)的充電點為原點創(chuàng)建室內(nèi)坐標系，標定三個標記點的坐標， 記錄三個標記點方向信息，并標定電子羅盤方向；
[0008] B):在機器人單目攝像頭中，以攝像頭中心為原點，建立攝像頭直角坐標系，其中， 初始化生成的攝像頭直角坐標系中，攝像頭直角坐標系的y軸與室內(nèi)坐標系的Y軸及機器 人正方向F相同；
[0009] C):控制機器人在室內(nèi)移動，通過聲納傳感器檢測房間邊緣，生成房間的室內(nèi)地 圖；
[0010] 步驟 S2:
[0011] 對于室內(nèi)定位：當機器人在房間內(nèi)的任一點時，通過單目攝像頭測量三個標記點 并結(jié)合攝像頭直角坐標系和室內(nèi)坐標系，計算出機器人所在的位置和方向數(shù)據(jù)；
[0012] 對于自主導航：設機器人目標點為M2,機器人通過室內(nèi)定位和目標點定位進行自 主導航。
[0013] 優(yōu)選，自主導航步驟中，設機器人在Ml點，首先計算出Ml點機器人的坐標和方向 角度，再計算Ml到M2的連線，可得出機器人的轉(zhuǎn)動方向角度和移動距離，機器人轉(zhuǎn)向目標 方向后向目標點移動預定距離，到達目標點之后根據(jù)目標位置的目標方向進行轉(zhuǎn)動，完成 導航。
[0014] 優(yōu)選，在自主導航過程中，機器人通過計算碼盤和電子羅盤數(shù)據(jù)校準自己的行程 路線。
[0015] 本發(fā)明的有益效果是：機器人通過單目攝像頭視覺，利用室內(nèi)屋頂三個標記點的 位置信息，計算機器人的坐標和方向，同時通過路程校準和方向校準措施確保定位導航的 準確性，實現(xiàn)了機器人室內(nèi)的地圖創(chuàng)建、路徑規(guī)劃，定位導航，自主充電等功能。
【附圖說明】
[0016] 圖1是本發(fā)明機器人移動過程中與室內(nèi)屋頂三個標記點的空間位置示意圖；
[0017] 圖2是本發(fā)明室內(nèi)地圖創(chuàng)建的流程圖；
[0018] 圖3是本發(fā)明機器人的位置及三個標記點的俯視圖；
[0019] 圖4是本發(fā)明攝像頭直角坐標系的不意圖；
[0020] 圖5是本發(fā)明室內(nèi)尚度h與攝像頭焦距f的關(guān)系不意圖；
[0021] 圖6是本發(fā)明通過聲納傳感器檢測房間邊緣的示意圖；
[0022] 圖7是本發(fā)明當機器人移動到某位置時的攝像頭直角坐標系的示意圖；
[0023] 圖8是本發(fā)明通過A、B的坐標和距離計算機器人的坐標的示意圖；
[0024] 圖9是本發(fā)明自主導航的示意圖；
[0025] 圖10是本發(fā)明機器人輪胎直徑1、軸徑L和旋轉(zhuǎn)角度q的參數(shù)示意圖。
【具體實施方式】
[0026] 下面結(jié)合附圖和具體的實施例對本發(fā)明技術(shù)方案作進一步的詳細描述，以使本領 域的技術(shù)人員可以更好的理解本發(fā)明并能予以實施，但所舉實施例不作為對本發(fā)明的限 定。
[0027] -種基于單視覺的機器人室內(nèi)定位和導航方法，其中，機器人設置有單目攝像頭、 電子羅盤和聲納傳感器，在室內(nèi)屋頂設置有三個可識別的標記點，如圖1所示，為了使機器 人定位、導航與路徑規(guī)劃功能更便利的實現(xiàn)和安裝，通過單目攝像頭和屋頂三個可識別的 標記點（或標簽）實現(xiàn)機器人的定位、導航和路徑規(guī)劃等功能。
[0028] 其中，初次使用時，需要創(chuàng)建房間的室內(nèi)地圖，如圖2所示，具體步驟如下：
[0029] A):機器人以室內(nèi)的充電點為原點創(chuàng)建室內(nèi)坐標系，標定三個標記點的坐標，記錄 三個標記點方向信息，并標定電子羅盤方向；如圖3所示，機器人M的位置及三個標記點A、 B、C，機器人正方向為F，機器人電子羅盤的標定方向為N，測量室內(nèi)高度為h，攝像頭焦距為 f〇
[0030] B):在機器人單目攝像頭中，以攝像頭中心為原點，建立攝像頭直角坐標系，其中， 初始化生成的攝像頭直角坐標系中，攝像頭直角坐標系的y軸與室內(nèi)坐標系的Y軸及機器 人正方向F相同；初始化生成的坐標系如圖4所示，其作為室內(nèi)地圖的基準坐標系，機器人 的位置不同，與三個標記點A、B、C的距離也不同，故可根據(jù)三個標記點A、B、C來定位機器 人的位置。
[0031] 可以通過攝像頭內(nèi)的直角坐標系計算出三個標記點距攝像頭中心的距離，比如，C 點距坐標系原點的距離為d。，C點與原點連線與X軸的夾角為Θ。。如圖5所示，室內(nèi)高度 為h，攝像頭焦距為f，則有IVd c= h/f，可計算出標記點C距坐標系原點的距離D通過Θ c 計算出C點的坐標為（X。，Yj，其中Xc= D ^cos Θ。，Y。= D CX sin Θ。。同理可計算出A點 坐標為（Xa，Xb)，B點的坐標為（Xb，Y b)。
[0032] 計算A點，B點連線在室內(nèi)坐標系中的角度數(shù)據(jù)，A點與B點的連線以A為正方向， 可計算線BA的矢量角0 ab，線BA的斜率角為Kab，可計算得0ab=yb>y a? (JI+Kab):Kab(即 當 yb> y a時，θab= π+κΛ，當 yb<ya時，θ ab= Kab);若 yb= 丫3時，則 θ ab= Xb> Xa? 0: π。其中，yb、yjp X b、13分別表示B點和A點在室內(nèi)坐標系中的y軸和x軸坐標。
[0033] 線BA與房間內(nèi)坐標系中的矢量角Θ ab保持不變，后面可用于定位時機器人方向的 計算。電子羅盤初始方向為N，標定電子羅盤方向的矢量角為α，有W n= α且在房間坐標 系中保持不變，可用于定位時方向校準，Wn表示電子羅盤在坐標系中的矢量角，即N這條帶 方向的線和X軸之間的夾角。
[0034] C):控制機器人在室內(nèi)移動，通過聲納傳感器檢測房間邊緣，生成房間的室內(nèi)地 圖；如圖6所示，機器人首先計算自己的位置和方向數(shù)據(jù)，之后將聲納傳感器檢測到的邊緣 信息繪制到房間的室內(nèi)地圖中。
[0035] 如圖7所示，當機器人移動到某位置時，可計算出當前機器人與C點的距離D。' = h/fXd。'，d。'是C點距當前攝像頭直角坐標系原點的距離，同理計算出A點的距離為Da'， B點的距離為Db'。
[0036] 通過初始化時標定的A，B兩個點的坐標:A(Xa，Xb)，B(X b，Yb)和當前A點，B點