一種室內(nèi)視覺定位系統(tǒng)及方法
【專利摘要】本發(fā)明公開了一種室內(nèi)視覺定位系統(tǒng)及方法，包括電源模塊、傳感器組、信息處理模塊，還包括前置面板、無線通信模塊，并在前置面板上設(shè)置了定位燈組。室內(nèi)視覺定位裝置的兩個攝像機(jī)可以靈活部署于墻壁和移動載體上，具有雙目視覺和單目視覺兩種工作模式?；谠撗b置的定位方法，首先，利用攝像機(jī)的前置面板信息，標(biāo)定雙目視覺系統(tǒng)外參數(shù)；其次，利用雙目視覺測量準(zhǔn)確信息構(gòu)建地圖，為后續(xù)單目視覺定位模式提供先驗(yàn)知識；最后，運(yùn)用地圖中蘊(yùn)含的點(diǎn)、線、面、距離等知識，啟動單目視覺定位模式，實(shí)現(xiàn)高效快速定位。本發(fā)明解決了傳統(tǒng)視覺定位方法不能兼顧精度、速度的問題，提高了定位系統(tǒng)的靈活性和自動化程度。
【專利說明】一種室內(nèi)視覺定位系統(tǒng)及方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于視覺定位【技術(shù)領(lǐng)域】，尤其涉及一種室內(nèi)視覺定位系統(tǒng)及方法。

【背景技術(shù)】
[0002]傳統(tǒng)定位技術(shù)通常有全球定位系統(tǒng)（GPS)和航位推算方法。目前，運(yùn)用機(jī)器視 覺技術(shù)實(shí)現(xiàn)對運(yùn)動目標(biāo)定位或移動載體姿態(tài)估計(jì)已經(jīng)成為上述定位技術(shù)的有效補(bǔ)充，受 到國內(nèi)外學(xué)者的廣泛關(guān)注。根據(jù)使用攝像機(jī)方式的不同，基于視覺的定位方法主要分為 3類。一是立體視覺定位，這類方法能獲得較多的信息，如文 [Jirawinut R，Prak00nwU S,Cecelja F, etal visual odometer for pedestrian navigation. IEEE Transactions on Instrumentation and Measurement，2004,52(4):ll66-117 3]提出了一個應(yīng)用于室 內(nèi)環(huán)境的視覺導(dǎo)航系統(tǒng)，但是算法復(fù)雜，計(jì)算成本也較高。二是全維視覺定位，這類方法 可以獲得大范圍的信息，但圖像畸變較大，如文[王景順，陳衛(wèi)東，曹其新.基于全景視 覺與里程計(jì)的機(jī)器人自定位方法研究·機(jī)器人,2005, 27(1) :41-45.]介紹了一種應(yīng)用于 足球機(jī)器人的自定似方法。二是單目視覺定位，這類方法一般用于能獲得較多先驗(yàn)信息 的環(huán)境，如文[Dellaertt F，Thrun S，Thorpe C.Mosaicing a Large Number of Widely Dispersed, Noisy, and Distorted Images a Bayesian Approach[R], USA:Carnegie Mellon University, 1"9]介紹了一使用了最小二乘互相關(guān)算法和貝葉斯算法的自定位方法。 [0003]在現(xiàn)有技術(shù)中，提出了多種借助于視覺系統(tǒng)解決移動載體定位問題的技術(shù)方法， 彌補(bǔ)了傳統(tǒng)定位方法在室內(nèi)環(huán)境下的不足。中國專利CN 101441769A，
【公開日】2009年5月 27曰，申請?zhí)?00810204397.0,單目攝像機(jī)實(shí)時視覺定位方法。本發(fā)明只通過單個攝像機(jī) 就能實(shí)現(xiàn)自定位、導(dǎo)航作用，從而降低系統(tǒng)復(fù)雜度和成本。但該發(fā)明需要先建立目標(biāo)圖像特 征數(shù)據(jù)庫并進(jìn)行實(shí)時訓(xùn)練，實(shí)現(xiàn)相機(jī)姿態(tài)估計(jì)，環(huán)境適應(yīng)能力不足，定位精度受限。
[0004] 中國專利CN101033958A，
【公開日】2〇〇7年9月12日，申請?zhí)?00710051446. 7 -種 機(jī)器視覺定位方法。采用兩個普通攝像機(jī)共同完成檢測任務(wù)，包括用于攝取目標(biāo)物體的全 局圖像的遠(yuǎn)距離攝像機(jī)和用于攝取感興趣部位的近距離圖像的近距離攝像機(jī)。該發(fā)明利用 兩個普通攝像機(jī)實(shí)現(xiàn)測量的高精度要求，但是其配準(zhǔn)過程復(fù)雜，自動化程度不高。
[0005] 單一采用單目視覺定位環(huán)境適應(yīng)能力不足、定位精度受限于先驗(yàn)知識準(zhǔn)確度，而 單一采用雙目視覺定位方法配準(zhǔn)過程復(fù)雜、定位速度慢、靈活性差。
[0006] 因此，設(shè)計(jì)一個兼顧定位精度、系統(tǒng)復(fù)雜度、自動化程度等因素的視覺定位系統(tǒng)及 方法是一個值得探索的問題。


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

[0007] 為了解決現(xiàn)上述問題，本發(fā)明提供了一種室內(nèi)視覺定位系統(tǒng)，定位快速、系統(tǒng)簡 單、使用靈活。
[0008] 本發(fā)明所要解決的技術(shù)問題是通過以下技術(shù)方案實(shí)現(xiàn)的：
[0009] 一種室內(nèi)視覺定位系統(tǒng)，包括固定攝像機(jī)與移動攝像機(jī)，所述固定攝像機(jī)懸掛于 室內(nèi)墻壁上，移動攝像機(jī)安裝于移動載體上，所述固定攝像機(jī)與移動攝像機(jī)上均設(shè)有前置 面板、電源模塊、傳感器組、通信模塊以及信息處理模塊，所述前置面板上還設(shè)有定位燈組。
[0010] 進(jìn)一步的，所述傳感器組包括C⑶傳感器、傾角測量傳感器、方向角傳感器。
[0011] 進(jìn)一步的，所述定位燈組包括6個顏色各不相同的定位燈，在前置面板上呈正六 邊形排布，移動攝像機(jī)與固定攝像機(jī)的鏡頭均設(shè)置在正六邊形的中心位置。
[0012] 一種室內(nèi)視覺定位方法，包括以下步驟：
[0013] 1)系統(tǒng)自檢；
[0014] 2)固定攝像機(jī)與移動攝像機(jī)進(jìn)行無線握手通信，啟動雙目視覺工作模式；
[0015] 3)固定攝像機(jī)與移動攝像機(jī)互以對方前置面板的定位燈組為參照，標(biāo)定雙目視覺 系統(tǒng)；
[0016] 4)固定攝像機(jī)對固定攝像機(jī)與移動攝像機(jī)各自拍攝的場景圖像進(jìn)行處理，然后構(gòu) 建初始環(huán)境地圖并將該地圖發(fā)送給移動攝像機(jī)；
[0017] 5)固定攝像機(jī)與移動攝像機(jī)均進(jìn)入單目工作模式；
[0018] 6)固定攝像機(jī)跟蹤移動攝像機(jī)定位燈組，計(jì)算移動攝像機(jī)的空間坐標(biāo)，并將計(jì)算 結(jié)果發(fā)送給移動攝像機(jī)；
[0019] 7)移動攝像機(jī)以環(huán)境地圖的點(diǎn)、線距離知識為基礎(chǔ)獲取新的點(diǎn)、線距離特征，并將 其發(fā)送給固定攝像機(jī)，若固定攝像機(jī)無法對新的點(diǎn)、線距離特征進(jìn)行匹配，則再切換到雙目 視覺工作模式，進(jìn)行地圖更新。
[0020] 進(jìn)一步的，所述雙目視覺工作模式下移動攝像機(jī)的工作流程包括以下步驟：
[0021] 5A、控制云臺旋轉(zhuǎn)，拍攝場景圖像PT，并記錄姿態(tài)信息；
[0022] 5B、從圖像PT中搜索識別固定攝像機(jī)的面板上的特征點(diǎn)，若識別成功則繼續(xù)步驟 5C，否則返回到步驟5A;
[0023] 5C、停止云臺旋轉(zhuǎn)，記錄移動攝像機(jī)當(dāng)前的姿態(tài)信息；
[0024] 5D、向固定攝像機(jī)發(fā)送雙目配準(zhǔn)請求，若收到固定攝像機(jī)的同步命令則執(zhí)行步驟 5E，否則返回步驟5A;
[0025] 5E、移動攝像機(jī)以固定攝像機(jī)發(fā)送的同步命令中的同步時間戳為準(zhǔn)，拍攝場景圖 像PM;
[0026] 5F、向固定攝像機(jī)發(fā)送包含圖像PM及移動攝像機(jī)姿態(tài)的報(bào)文；
[0027] 5G、移動攝像機(jī)接收固定攝像機(jī)發(fā)送來的移動攝像機(jī)當(dāng)前位置坐標(biāo)及環(huán)境地圖信 息。
[0028] 進(jìn)一步的，所述雙目視覺工作模式下固定攝像機(jī)的工作流程包括以下步驟：
[0029] 6A、接收移動攝像機(jī)發(fā)送的雙目配準(zhǔn)請求；
[0030] 6B、向移動攝像機(jī)發(fā)送雙目配準(zhǔn)同步時間戳；
[0031] 6C、以步驟6B中的時間戳為準(zhǔn)，拍攝場景圖像PW;
[0032] 6D、接收移動攝像機(jī)發(fā)送的圖像以及位姿報(bào)文；
[0033] 6E、解析步驟6D中的報(bào)文，得到圖像PM以及移動攝像機(jī)的位姿參數(shù)；
[0034] 6F、分析圖像PW、PM確定兩幅圖像中的特征點(diǎn)位置；
[0035] 6G、分別以圖像Μ以及PM上特征點(diǎn)為參照來確定固定攝像機(jī)與移動攝像機(jī)的位 姿；
[0036] 6H、統(tǒng)一以固定攝像機(jī)坐標(biāo)系作為兩個攝像機(jī)的世界坐標(biāo)系，匹配位姿數(shù)據(jù)，若所 得的位姿數(shù)據(jù)一致則執(zhí)行步驟 61，否則執(zhí)行步驟6B ;
[0037] 61、根據(jù)視覺以及傳感器所得位姿數(shù)據(jù)計(jì)算固定攝像機(jī)與移動攝像機(jī)在世界坐標(biāo) 系中的位姿；
[0038] 6J、檢測并匹配圖像PW、PM中的角點(diǎn)、邊緣特征；
[0039] 6K、根據(jù)視覺三角測量原理計(jì)算角點(diǎn)、直線的空間坐標(biāo)；
[0040] 6L、繪制初始地圖并將地圖發(fā)送給移動攝像機(jī)。
[0041] 進(jìn)一步的，所述單目視覺工作模式下固定攝像機(jī)的工作流程包括以下步驟：
[0042] 7A、固定攝像機(jī)定時采集場景圖像PW ;
[0043] 7B、檢測圖像PW中是否包含移動攝像機(jī)面板上的所有特征點(diǎn)信息，若檢測到所有 特征點(diǎn)信息則執(zhí)行步驟7C，否則跳到步驟7F ;
[0044] 7C、計(jì)算移動攝像頭的空間坐標(biāo)；
[0045] 7D、將移動攝像機(jī)的空間坐標(biāo)發(fā)送給移動攝像機(jī)；
[0046] 7E、結(jié)束；
[0047] 7F、向移動攝像機(jī)發(fā)送超范圍指令；
[0048] 7G、接收移動攝像機(jī)的地圖更新請求，在圖像PW中檢測識別需要更新的點(diǎn)線特 征，若檢測到這些點(diǎn)、線特征則執(zhí)行步驟7H，否則執(zhí)行步驟71 ;
[0049] 7H、向移動攝像機(jī)發(fā)送地圖更新確認(rèn)信息，跳到步驟7E ;
[0050] 71、向移動攝像機(jī)發(fā)送地圖更新失敗信息；
[0051] 7J、啟動雙目視覺工作模式，配準(zhǔn)地圖，跳到步驟7E。
[0052] 進(jìn)一步的，所述單目視覺工作模式下移動攝像機(jī)的工作流程包括以下步驟：
[0053] 8A、接收固定攝像機(jī)發(fā)送的定位信息；
[0054] 8B、拍攝場景圖像PW;
[0055] 8C、檢測圖像PW中角點(diǎn)、直線特征信息；
[0056] 8D、利用圖中已知的點(diǎn)、線、距離知識計(jì)算新特征的空間坐標(biāo)；
[0057] 8E、將新特征補(bǔ)充到環(huán)境地圖；
[0058] 8F、將環(huán)境地圖發(fā)送到固定攝像機(jī)確認(rèn)，若確認(rèn)成功則執(zhí)行步驟8G，否則執(zhí)行步驟 8H ；
[0059] 8G、刪除舊環(huán)境地圖，以新環(huán)境地圖指導(dǎo)后續(xù)運(yùn)動，跳到步驟81 ;
[0060] 8H、用新環(huán)境地圖指導(dǎo)移動攝像頭后期運(yùn)動，啟動雙目視覺配準(zhǔn)模式；
[0061] 81、結(jié)束。
[0062]本發(fā)明所達(dá)到的有益效果是：（1)本發(fā)明的室內(nèi)視覺定位系統(tǒng)及方法，在攝像機(jī) 的前置面板上配置了 6個位置精確固定、相對位置關(guān)系己知、顏色不同的LED定位指示燈， 提供定位參考信息，使系統(tǒng)工作于雙目視覺模式時，能夠自標(biāo)定，不需要系統(tǒng)之外的額外靶 標(biāo)，系統(tǒng)工作于單目視覺模式時，能夠快速定位跟蹤。
[0063] (2)本發(fā)明的室內(nèi)視覺定位系統(tǒng)及方法，配置了無線通信模塊，攝像機(jī)間能夠?qū)崟r 通信，使系統(tǒng)的部署更加靈活，使用更加方便。
[0064] (3)本發(fā)明的室內(nèi)視覺定位系統(tǒng)及方法，在構(gòu)建地圖時采用雙目視覺工作模式，測 距更準(zhǔn)確，而移動載體運(yùn)動中，系統(tǒng)可工作于單目視覺模式，定位更快速。

【專利附圖】

【附圖說明】 _
[0065] 圖1為本發(fā)明中固定攝像機(jī)與移動攝像機(jī)的結(jié)構(gòu)示意圖；
[0066] 圖2為本發(fā)明中固定攝像機(jī)與移動攝像機(jī)的位直部署不意圖；
[0067] 圖3為本發(fā)明中固定攝像機(jī)與移動攝像機(jī)的前置面板結(jié)構(gòu)示意圖；
[0068] 圖4為本發(fā)明中固定攝像機(jī)與移動攝像機(jī)功能框架示意圖；
[0069] 圖5為本發(fā)明的工作流程圖；
[0070] 圖6為本發(fā)明中移動攝像機(jī)的初始化流程圖；
[0071] 圖7為本發(fā)明中固定攝像機(jī)的初始化流程圖；
[0072] 圖8為本發(fā)明的室內(nèi)視覺定位系統(tǒng)在雙目視覺工作模式下移動攝像機(jī)的工作流 程圖；
[0073] 圖9為本發(fā)明的室內(nèi)視覺定位系統(tǒng)在雙目視覺工作模式下固定攝像機(jī)的工作流 程圖；
[0074] 圖10為本發(fā)明的室內(nèi)視覺定位系統(tǒng)在單目視覺工作模式下固定攝像機(jī)的工作流 程圖；
[0075] 圖11為實(shí)施例提供的室內(nèi)視覺定位系統(tǒng)在雙目視覺工作模式下CM攝像機(jī)工作流 程圖。

【具體實(shí)施方式】
[0076]為了進(jìn)一步描述本發(fā)明的技術(shù)特點(diǎn)和效果，以下結(jié)合附圖和【具體實(shí)施方式】對本發(fā) 明做進(jìn)一步描述。
[0077] 參照圖1_11所不，~種室內(nèi)視覺定位系統(tǒng)，米用兩個冋構(gòu)攝像機(jī)共同完成定位監(jiān) 測任務(wù)，其中，一個為位置固定的攝像機(jī)，稱為固定攝像機(jī)CW,可以懸掛于室內(nèi)墻壁上，用于 檢測環(huán)境全局信息及指導(dǎo)另一攝像機(jī)的移動；另一個攝像機(jī)安裝于移動載體上，稱為移動 攝像機(jī)CM，用于檢測環(huán)境局部信息及配合完成雙目視覺任務(wù)。所述固定攝像機(jī)CW和移動 攝像機(jī)CM的硬件結(jié)構(gòu)是一樣的，包括前置面板20、電源模塊10、傳感器組30、通信模塊40、 信息處理模塊50。所述的前置面板20上裝有定位燈組21，定位燈組21由6個位置精確固 定、相對位置關(guān)系已知、顏色不同的LED燈組成，分別是紅色指示燈211、黃色指示燈212、綠 色指示燈213、紫色指示燈214、藍(lán)色指示燈215、白色指示燈216,6個LH)燈在呈正六邊形 排布，邊長為4cm，將這6個LED燈作為特征點(diǎn)。用作雙目視覺工作模式的自標(biāo)定以及單目 工作模式的運(yùn)動目標(biāo)跟蹤標(biāo)志，兩個攝像機(jī)的鏡頭311均位于六邊形的中心位置。所述的 電源模塊10包括鋰電池11和充放電保護(hù)模塊12兩部分，鋰電池11為攝像機(jī)系統(tǒng)提供5V 工作電壓，充放電保護(hù)模塊12負(fù)責(zé)在電池充電時將220V電壓變換成充電電壓，在電池組放 電時提供放電保護(hù)及實(shí)時告警，必要時斷掉電源。所述傳感器組由CCD傳感器31、傾角測量 傳感器 32、方向角傳感器33組成，傾角測量傳感器32負(fù)責(zé)測量攝像機(jī)與鉛垂線的傾角，用 作位姿配準(zhǔn)數(shù)據(jù)（即攝像機(jī)坐標(biāo)系與世界坐標(biāo)系之間的）方向角傳感器33用于測量攝像 機(jī)相對于正南方向的方向角，C⑶傳感器31用于采集場景圖像。所述通信模塊40具有無 線發(fā)送模塊41與無線接收模塊42,是攝像機(jī)間通信的接口，用于發(fā)送/接收控制命令、場景 圖像、地圖數(shù)據(jù)等信息。所述信息處理模塊50由信息采集子模塊51、信息處理子模塊 52、 控制輸出子模塊53組成。信息采集子模塊51負(fù)責(zé)采集來自傳感器組的數(shù)據(jù)，這些數(shù)據(jù)傳 遞給信息處理子模塊52。信息處理子模塊52對傳感器組的數(shù)據(jù)以及通信模塊的數(shù)據(jù)進(jìn)行 融合處理和判斷決策?？刂戚敵鲎幽K 53根據(jù)信息處理模塊的判斷決策向移動載體輸出 控制信號。
[0078] 本發(fā)明的定位主要是針對移動攝像機(jī)的定位，移動攝像機(jī)CM安裝在移動載體上， 固定攝像機(jī)CW安裝在墻壁上，兩攝像機(jī)都有根據(jù)對方面板上的定位燈組21為參考計(jì)算^ 身位置的能力，最后通過固定攝像機(jī)CW通過坐標(biāo)變換將位置坐標(biāo)統(tǒng)一到同一個世界坐標(biāo) 系中。
[0079] 本發(fā)明的室內(nèi)視覺定位系統(tǒng)可以靈活工作在雙目視覺模式和單目視覺模式下，充 分發(fā)揮雙目視覺定位的準(zhǔn)確性以及單目視覺定位的高效率。
[0080] 本發(fā)明的室內(nèi)視覺定位方法總體工作流程如圖5所示：第1步進(jìn)行系統(tǒng)自檢；第2 步，固定攝像機(jī)CW與移動攝像機(jī)CM無線握手通信，啟動雙目視覺工作模式；第3步，固定攝 像機(jī)CW與移動攝像機(jī)CM，互以對方前置面板的6個LED燈為參照，標(biāo)定雙目視覺系統(tǒng)（即 標(biāo)定自身在對方定位燈組坐標(biāo)下的坐標(biāo)）；第4步，固定攝像機(jī)CW對兩攝像機(jī)各自拍攝的 場景圖像進(jìn)行處理，處理內(nèi)容包括預(yù)處理、特征提取、特征匹配，構(gòu)建初始環(huán)境地圖，發(fā)送給 移動攝像機(jī)CM ;第5步，兩攝像機(jī)CW、CM各自進(jìn)入單目工作模式；第6步，固定攝像機(jī)CW跟 蹤移動攝像機(jī)CM的6個LED燈，計(jì)算移動攝像機(jī)CM的空間坐標(biāo)，并無線發(fā)送給移動攝像機(jī) CM ;第7步，移動攝像機(jī)CM在固定攝像機(jī)CW的指令下，以環(huán)境地圖的點(diǎn)、線、面、距離知識為 基礎(chǔ)，獲取新特征，發(fā)送給固定攝像機(jī)CW，若固定攝像機(jī)CW無法對新的點(diǎn)、線距離特征進(jìn)行 匹配，則再切換到雙目視覺工作模式，進(jìn)行地圖更新。。
[0081] 其中步驟3中的標(biāo)定雙目視覺系統(tǒng)采用ΡΘΡ方法，所述P6P方法說明如下：
[0082] 攝像機(jī)的成像過程可以用針孔模型描述，通常建立世界坐標(biāo)系〇JwYwZw、攝像 機(jī)坐標(biāo)系0〇\Υ〇Ζ。、圖像坐標(biāo)系0UV三個坐標(biāo)系描述空間點(diǎn)與像點(diǎn)之間的關(guān)系。士空間 點(diǎn)在世界坐標(biāo)系下的齊次坐標(biāo)系記為（x w，yw,zw, 1),該點(diǎn)在攝像機(jī)坐標(biāo)系下的坐標(biāo)記為 (X。，y。，z。, 1)，其對應(yīng)像點(diǎn)在圖像坐標(biāo)系下記為（u, V，1)，則圖像坐標(biāo)系與攝像機(jī)坐標(biāo)系的 關(guān)系可以描述為： Γ"1 Γι? 「·、'1 . 「Μ 「:（
[0083] .? V =氣 Α ⑴ _c = ￠.2::): χ LU 1 J Ζ?. Ζ,ν -」 L J i i i 'fu ^ ?ο]
[0084] 其中，Min是攝像機(jī)的內(nèi)參數(shù)，M,n= 0 /v va。其中，fu、fv分別為圖像平面上 _〇 P IJ u軸和V軸上的尺度因子，U。、V。為主點(diǎn)坐標(biāo)，是圖像平面與光軸的交點(diǎn)。本發(fā)明所用攝像 機(jī)已標(biāo)定，即Min已知。
[0085] 攝像機(jī)的外參數(shù)，是世界坐標(biāo)系在攝像機(jī)坐標(biāo)中的描述。R、T分別表示旋轉(zhuǎn)矩 陣和肀移向量。R= [η 0 a]T，T= 向量n、〇、a分別表示世界坐標(biāo)系坐標(biāo)軸 Χ"軸、Yw軸、4軸在攝像機(jī)坐標(biāo)系〇ΑΥΛ中的方向向量，而向量[tl，t2, t3]τ表示0J JwZw的 坐標(biāo)原點(diǎn)在攝像機(jī)坐標(biāo)系OJJJ。中的坐標(biāo)。
[0086]攝像機(jī)的位姿，是指攝像機(jī)坐標(biāo)系在世界坐標(biāo)系中的描述，g卩t'M,,.1。因此，求得攝 像機(jī)的外參數(shù)，隨即可得攝像機(jī)的位姿。
[0087]由式⑴、⑵知，每一個圖像點(diǎn)可以建立兩個關(guān)于R、T的方程，。Mw中共有12個未 知數(shù)，因此，已知6個空間點(diǎn)的世界坐標(biāo)（即6個LED定位燈），即可線性求解攝像機(jī)的位 姿。
[0088]本發(fā)明的系統(tǒng)自檢過程主要完成攝像機(jī)的狀態(tài)檢測，使其進(jìn)入合適的工作狀態(tài)。 系統(tǒng)中的兩個攝像機(jī)各自完成自己的自檢過程。如圖6所示，移動攝像機(jī)CM的自檢過程包 括如下步驟：第1步，進(jìn)行基本輸入輸出檢測；第2步，進(jìn)行慣性傳感器模塊檢測；第3步， 檢測云臺控制模塊；第4步，檢測傾斜角；第5步，檢測視覺系統(tǒng)各模塊是否工作正常。攝像 機(jī)CW的自檢過程和攝像機(jī)CM的略有不同，如圖7所示，攝像機(jī)CW不需要檢測云臺控制模 塊。
[0089]本發(fā)明的室內(nèi)視覺定位系統(tǒng)完成自檢后，進(jìn)入雙目視覺工作模式，完成場景地圖 構(gòu)建，為單目視覺工作模式提供先驗(yàn)知識。雙目視覺模式首先要標(biāo)定外參數(shù)，本發(fā)明使用兩 攝像機(jī)的前置面板上6個相對位置關(guān)系確定的指示燈為參照完成標(biāo)定。雙目視覺工作模式 下，攝像機(jī)CM的工作流程如圖8所示：第1步，控制云臺旋轉(zhuǎn)，拍攝場景圖片ρτ，并記錄方 向角及傾斜角等姿態(tài)信息；第2步，分析圖片PT，搜索識別相機(jī)CW的面板6特征點(diǎn)；若識別 成功（即準(zhǔn)確標(biāo)定出6個LED的位置），則執(zhí)行第3步，否則執(zhí)行第1步；第3步，停止云臺 旋轉(zhuǎn)，記錄移動攝像機(jī)CM的當(dāng)前姿態(tài)信息（姿態(tài)信息包括傾角、方向角狀態(tài)）；第4步，以 無線通信方式，向固定攝像機(jī)CW發(fā)送雙目配準(zhǔn)請求;若收到CW的同步命令，同步命令中包 含同步時間戳，則執(zhí)行第 5步，否則，執(zhí)行第1步；第5步，以固定攝像機(jī)CW發(fā)送的同步時間 戳為準(zhǔn)，拍攝場景圖像PM ;第6步，向固定攝像機(jī)CW發(fā)送包含圖片PM及移動攝像機(jī)CM姿 態(tài)的報(bào)文；第7步，接收固定攝像機(jī)CW發(fā)送來的當(dāng)前位置坐標(biāo)及環(huán)境地圖信息。
[0090]同樣地，雙目視覺工作模式下，固定攝像機(jī)CW的工作流程如圖9所示：第1步，接 收移動攝像機(jī)CM發(fā)送的雙目配準(zhǔn)請求；第2步，向移動攝像機(jī)CM發(fā)送雙目配準(zhǔn)同步時間 戳；第3步，以此時間戳為準(zhǔn)，拍攝場景圖片PW ;第4步，接收移動攝像機(jī)CM發(fā)送的圖片及 位姿報(bào)文，報(bào)文中包含拍攝時間、傾斜角、方位角及以固定攝像機(jī)CW面板LED指示燈為參照 的空間三維坐標(biāo)；第5步，解析報(bào)文，得到圖片PM及移動攝像頭CM的位姿參數(shù)；第6步，分 析圖片PW、PM，檢測兩幅圖像中的6特征點(diǎn)；第7步，以對方面板的6特征點(diǎn)為參照，運(yùn)用 ΡδΡ方法，確定固定攝像機(jī)CW與移動攝像機(jī)CM的位姿；第8步，統(tǒng)一以CW相機(jī)坐標(biāo)系作為 兩相機(jī)的世界坐標(biāo)系，匹配位姿數(shù)據(jù)；若所得位姿數(shù)據(jù)一致，則執(zhí)行第 9步，否則，執(zhí)行第2 步；第9步，運(yùn)用視覺所得位姿數(shù)據(jù)及其他傳感器得到的位姿數(shù)據(jù)，計(jì)算雙目視覺系統(tǒng)的外 參數(shù)，即兩攝像機(jī)在世界坐標(biāo)系中的位姿；第10步，檢測圖片PW、PM中的角點(diǎn)、邊緣特征， 匹配兩幅圖像中的角點(diǎn)、邊緣特征（請問交點(diǎn)、邊緣特征以及立體匹配具體內(nèi)容是什么）； 第11步，根據(jù)立體視覺三角測量原理，計(jì)算角點(diǎn)、直線的空間坐標(biāo)；第12步，繪制初始地圖 (初始地圖是移動攝像機(jī)CM移動前的地圖），并將該地圖發(fā)送給移動攝像機(jī)CM。
[0091]本發(fā)明通過雙目視覺模式構(gòu)建了蘊(yùn)含豐富先驗(yàn)知識的環(huán)境地圖后，進(jìn)入快速便捷 的單目視覺工作模式。單目工作模式下，固定攝像機(jī)CW的工作流程如圖10所示：第1步， 固定攝像機(jī)CW定時（每隔3秒）采集場景圖像PW ;第2步，檢測圖像PW中是否包含移動 攝像機(jī)CM面板的6個特征點(diǎn)信息，若檢測到6個特征點(diǎn)信息，則執(zhí)行第3步，否則，執(zhí)行第 6步；第3步，計(jì)算移動攝像機(jī)CM的空間坐標(biāo)；第4步，將該坐標(biāo)以無線通信方式發(fā)送給移 動攝像機(jī)CM ;第5步，結(jié)束；第6步，向CM發(fā)送超范圍指令；第7步，接收移動攝像機(jī)CM地 圖更新請求，在PW圖像中檢測識別需要更新的點(diǎn)、線特征，若檢測到這些點(diǎn)、線特征，則執(zhí) 行第8步，否則執(zhí)行第9步；第8步，向CM發(fā)送地圖更新確認(rèn)信息，執(zhí)行第5步；第9步,發(fā) 送地圖更新失敗信息；第10步，啟動雙目視覺工作模式，配準(zhǔn)地圖，執(zhí)行第5步。
[0092] 同樣地，單目工作模式，攝像機(jī)CM的工作流程如圖11所示：第1步，接收固定攝像 機(jī)CW發(fā)送的定位信息；第2步，拍攝場景圖像PW ;第3步，檢測角點(diǎn)、直線等特征信息；第4 步，利用地圖中已知的點(diǎn)、線、面、距離等知識，計(jì)算新特征的空間坐標(biāo)；第5步，將新特征補(bǔ) 充到環(huán)境地圖；第6步，將新環(huán)境地圖發(fā)送到固定攝像機(jī)CW，等待確認(rèn)；若確認(rèn)成功（確認(rèn) 主要包括更新的點(diǎn)、線位置坐標(biāo)及時間戳），則執(zhí)行第 7步，否則，執(zhí)行第8步；第7步，以新 地圖指導(dǎo)后期運(yùn)動，刪除舊地圖，執(zhí)行第9步；第8步，新地圖臨時指導(dǎo)后期運(yùn)動，啟動雙目 視覺配準(zhǔn)模式；第9步，結(jié)束。
[0093]上述實(shí)施例不以任何形式限定本發(fā)明，凡采取等同替換或等效變換的形式所獲得 的技術(shù)方案，均落在本發(fā)明的保護(hù)范圍之內(nèi)。
【權(quán)利要求】
1. 一種室內(nèi)視覺定位系統(tǒng)，其特征在于：包括固定攝像機(jī)與移動攝像機(jī)，所述固定攝 像機(jī)懸掛于室內(nèi)墻壁上，移動攝像機(jī)安裝于移動載體上，所述固定攝像機(jī)與移動攝像機(jī)上 均設(shè)有前置面板（20)、電源模塊（10)、傳感器組（30)、通信模塊（40)以及信息處理模塊 (50)，所述前置面板（20)上還設(shè)有定位燈組（21)。
2. 根據(jù)權(quán)利要求1所述的一種室內(nèi)視覺定位系統(tǒng)，其特征在于：所述傳感器組（30)包 括CCD傳感器（31)、傾角測量傳感器（32)、方向角傳感器（33)。
3. 根據(jù)權(quán)利要求1所述的一種室內(nèi)視覺定位系統(tǒng)，其特征在于：所述定位燈組（21)包 括6個顏色各不相同的定位燈，在前置面板（20)上呈正六邊形排布，移動攝像機(jī)與固定攝 像機(jī)的鏡頭（311)均設(shè)置在正六邊形的中心位置。
4. 一種室內(nèi)視覺定位方法，其特征在于，包括以下步驟： 1) 系統(tǒng)自檢； 2) 固定攝像機(jī)與移動攝像機(jī)進(jìn)行無線握手通信，啟動雙目視覺工作模式； 3) 固定攝像機(jī)與移動攝像機(jī)互以對方前置面板（20)的定位燈組（21)為參照，標(biāo)定雙 目視覺系統(tǒng)； 4) 固定攝像機(jī)對固定攝像機(jī)與移動攝像機(jī)各自拍攝的場景圖像進(jìn)行處理，然后構(gòu)建初 始環(huán)境地圖并將該地圖發(fā)送給移動攝像機(jī)； 5) 固定攝像機(jī)與移動攝像機(jī)均進(jìn)入單目工作模式； 6) 固定攝像機(jī)跟蹤移動攝像機(jī)定位燈組（21)，計(jì)算移動攝像機(jī)的空間坐標(biāo)，并將計(jì)算 結(jié)果發(fā)送給移動攝像機(jī)； 7) 移動攝像機(jī)以環(huán)境地圖的點(diǎn)、線距離知識為基礎(chǔ)獲取新的點(diǎn)、線距離特征，并將其發(fā) 送給固定攝像機(jī),若固定攝像機(jī)無法對新的點(diǎn)、線距離特征進(jìn)行匹配，則再切換到雙目視覺 工作模式，進(jìn)行地圖更新。
5. 根據(jù)權(quán)利要求4所述的一種室內(nèi)視覺定位方法，其特征在于，所述雙目視覺工作模 式下移動攝像機(jī)的工作流程包括以下步驟： 5A、控制云臺旋轉(zhuǎn)，拍攝場景圖像PT，并記錄姿態(tài)信息； 5B、從圖像PT中搜索識別固定攝像機(jī)的面板上的特征點(diǎn)，若識別成功則繼續(xù)步驟5C， 否則返回到步驟5A; 5C、停止云臺旋轉(zhuǎn)，記錄移動攝像機(jī)當(dāng)前的姿態(tài)信息； 5D、向固定攝像機(jī)發(fā)送雙目配準(zhǔn)請求，若收到固定攝像機(jī)的同步命令則執(zhí)行步驟5E，否 則返回步驟5A ; 5E、移動攝像機(jī)以固定攝像機(jī)發(fā)送的同步命令中的同步時間戳為準(zhǔn)，拍攝場景圖像 PM； 5F、向固定攝像機(jī)發(fā)送包含圖像PM及移動攝像機(jī)姿態(tài)的報(bào)文； 5G、移動攝像機(jī)接收固定攝像機(jī)發(fā)送來的移動攝像機(jī)當(dāng)前位置坐標(biāo)及環(huán)境地圖信息。
6. 根據(jù)權(quán)利要求4所述的一種室內(nèi)視覺定位方法，其特征在于，所述雙目視覺工作模 式下固定攝像機(jī)的工作流程包括以下步驟： 6A、接收移動攝像機(jī)發(fā)送的雙目配準(zhǔn)請求； 6B、向移動攝像機(jī)發(fā)送雙目配準(zhǔn)同步時間戳； 6C、以步驟6B中的時間戳為準(zhǔn)，拍攝場景圖像PW ; 6D、接收移動攝像機(jī)發(fā)送的圖像以及位姿報(bào)文； 6E、解析步驟6D中的報(bào)文，得到圖像PM以及移動攝像機(jī)的位姿參數(shù)； 6F、分析圖像PW、PM確定兩幅圖像中的特征點(diǎn)位置； 6G、分別以圖像PW以及PM上特征點(diǎn)為參照來確定固定攝像機(jī)與移動攝像機(jī)的位姿； 6H、統(tǒng)一以固定攝像機(jī)坐標(biāo)系作為兩個攝像機(jī)的世界坐標(biāo)系，匹配位姿數(shù)據(jù)，若所得的 位姿數(shù)據(jù)一致則執(zhí)行步驟61，否則執(zhí)行步驟6B ; 61、根據(jù)視覺以及傳感器所得位姿數(shù)據(jù)計(jì)算固定攝像機(jī)與移動攝像機(jī)在世界坐標(biāo)系中 的位姿； 6J、檢測并匹配圖像PW、PM中的角點(diǎn)、邊緣特征； 6K、根據(jù)視覺三角測量原理計(jì)算角點(diǎn)、直線的空間坐標(biāo)； 6L、繪制初始地圖并將地圖發(fā)送給移動攝像機(jī)。
7.根據(jù)權(quán)利要求4所述的一種室內(nèi)視覺定位方法，其特征在于，所述單目視覺工作模 式下固定攝像機(jī)的工作流程包括以下步驟： 7A、固定攝像機(jī)定時采集場景圖像PW ; 7B、檢測圖像PW中是否包含移動攝像機(jī)面板上的所有特征點(diǎn)信息，若檢測到所有特征 點(diǎn)信息則執(zhí)行步驟7C，否則跳到步驟7F; 7C、計(jì)算移動攝像頭的空間坐標(biāo)； 7D、將移動攝像機(jī)的空間坐標(biāo)發(fā)送給移動攝像機(jī)； 7E、結(jié)束； 7F、向移動攝像機(jī)發(fā)送超范圍指令； 7G、接收移動攝像機(jī)的地圖更新請求，在圖像PW中檢測識別需要更新的點(diǎn)線特征，若 檢測到這些點(diǎn)、線特征則執(zhí)行步驟7H，否則執(zhí)行步驟71 ; 7H、向移動攝像機(jī)發(fā)送地圖更新確認(rèn)信息，跳到步驟7 E ; 71、向移動攝像機(jī)發(fā)送地圖更新失敗信息； 7J、啟動雙目視覺工作模式，配準(zhǔn)地圖，跳到步驟7E。 8_根據(jù)權(quán)利要求4所述的一種室內(nèi)視覺定位方法，其特征在于，所述單目視覺工作模 式下移動攝像機(jī)的工作流程包括以下步驟： 8A、接收固定攝像機(jī)發(fā)送的定位信息； 8B、拍攝場景圖像PW; 8C、檢測圖像PW中角點(diǎn)、直線特征信息； 8D、利用圖中已知的點(diǎn)、線、距離知識計(jì)算新特征的空間坐標(biāo)； 8E、將新特征補(bǔ)充到環(huán)境地圖； 8F、將環(huán)境地圖發(fā)送到固定攝像機(jī)確認(rèn)，若確認(rèn)成功則執(zhí)行步驟8G，否則執(zhí)行步驟8H ; 8G、刪除舊環(huán)境地圖，以新環(huán)境地圖指導(dǎo)后續(xù)運(yùn)動，跳到步驟81 ; 8H、用新環(huán)境地圖指導(dǎo)移動攝像頭后期運(yùn)動，啟動雙目視覺配準(zhǔn)模式； 81、結(jié)束。
【文檔編號】G06T7/00GK104217439SQ201410504875
【公開日】2014年12月17日 申請日期:2014年9月26日 優(yōu)先權(quán)日:2014年9月26日
【發(fā)明者】韓磊, 徐夢溪, 鄭勝男, 黃陳蓉 申請人:南京工程學(xué)院