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

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

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