本發(fā)明涉及物體在三維空間的跟蹤定位技術(shù)領(lǐng)域，具體涉及一種三維空間跟蹤定位系統(tǒng)及方法。

背景技術(shù)：

三維空間的跟蹤定位是虛擬現(xiàn)實VR、增強(qiáng)現(xiàn)實AR和其它一些機(jī)器人領(lǐng)域的關(guān)鍵技術(shù)。常見的定位方案有激光定位、光學(xué)定位以及基于計算機(jī)視覺的定位，其中光學(xué)定位又分為主動式和被動式兩種。激光定位和光學(xué)定位存在成本高、可檢測范圍小、難以實現(xiàn)多目標(biāo)定位等問題。

傳統(tǒng)的基于計算機(jī)視覺的定位方法，分為標(biāo)志物法(Marker)，并行跟蹤映射法(PTAM)，關(guān)鍵點模板跟蹤法(Keypoint Template Tracking)，單應(yīng)矩陣法(Homography-based)等。這些方法或?qū)τ趫鼍坝邢拗埔螅驅(qū)τ谠O(shè)備的計算能力有較高的要求，并且容易受到環(huán)境光的影響，不能處理遮擋，精確度和魯棒性都不夠高，因此難以做到商用級別。

因此，如何設(shè)計一種能夠解決上述問題的三維空間跟蹤定位系統(tǒng)或方法，是亟待解決的問題。

技術(shù)實現(xiàn)要素：

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明提供一種三維空間跟蹤定位系統(tǒng)及方法，實現(xiàn)了對三維空間的高效且可靠的跟蹤定位，且對定位場景及設(shè)備計算能力均無特殊要求，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

為解決上述技術(shù)問題，本發(fā)明提供以下技術(shù)方案：

一方面，本發(fā)明提供了一種三維空間跟蹤定位系統(tǒng)，所述系統(tǒng)包括：圖像獲取單元、固定設(shè)置在目標(biāo)定位室內(nèi)的二維編碼顯示單元、以及，與所述圖像獲取單元通信連接的三維空間位置獲取單元；

所述圖像獲取單元用于實時獲取所述目標(biāo)定位室的室內(nèi)圖像，并將所述室內(nèi)圖像發(fā)送至所述三維空間位置獲取單元；

所述二位編碼顯示單元用于顯示二維編碼圖像；

所述三維空間位置獲取單元用于識別所述室內(nèi)圖像，并在確定所述室內(nèi)圖像中包含所述二維編碼圖像時，根據(jù)該二維編碼圖像獲取所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

進(jìn)一步的，所述三維空間位置獲取單元包括：圖像識別子單元和圖像位置生成子單元；

所述圖像識別子單元用于接收所述圖像獲取單元發(fā)送的室內(nèi)圖像，并在識別所述室內(nèi)圖像中是否包含有完整或部分的所述二維編碼圖像時，將包含有該二維編碼圖像的室內(nèi)圖像發(fā)送至所述圖像位置生成子單元；

所述圖像位置生成子單元用于根據(jù)接收的室內(nèi)圖像中的所述二維編碼圖像，獲取所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

進(jìn)一步的，所述系統(tǒng)還包括：慣性測量單元，且所述慣性測量單元與圖像獲取單元均設(shè)置在虛擬現(xiàn)實設(shè)備上，且所述慣性測量單元與所述三維空間位置獲取單元通信連接；

所述慣性測量單元用于對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測，并將檢測結(jié)果發(fā)送至所述三維空間位置獲取單元。

進(jìn)一步的，所述三維空間位置獲取單元還包括：傳感位置生成子單元和三維空間位置發(fā)送子單元；

所述傳感位置生成子單元用于接收所述慣性測量單元發(fā)送的所述檢測結(jié)果，并根據(jù)所述檢測結(jié)果獲取所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息；

所述三維空間位置發(fā)送子單元用于將所述圖像位置生成子單元和所述傳感位置生成子單元分別生成的三維空間位置信息按時間順序進(jìn)行匯總，得到所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置匯總信息，并將所述三維空間位置匯總信息發(fā)送至所述虛擬現(xiàn)實設(shè)備或外部顯示設(shè)備進(jìn)行顯示。

進(jìn)一步的，所述二位編碼顯示單元為印設(shè)有所述二維編碼圖像的紙板，且該紙板設(shè)置在所述目標(biāo)定位室的天花板上；

所述二維編碼圖像由N*N個單元格組成，且N為大于或等于5的整數(shù)；

其中，不同的所述二維編碼圖像中相同位置的單元格組的組成形式不同，所述單元格組由X*Y個單元格組成，且4≤X<N，4≤Y<N。

另一方面，本發(fā)明還提供了一種三維空間跟蹤定位方法，所述方法包括：

實時獲取虛擬現(xiàn)實設(shè)備的圖像獲取單元拍攝的當(dāng)前目標(biāo)定位室的室內(nèi)圖像；

識別所述室內(nèi)圖像，并在確定室內(nèi)圖像中包含所述二維編碼圖像時，對所述二維編碼圖像進(jìn)行定位和方向檢測，其中，所述二維編碼圖像由N*N個單元格組成，且N為大于或等于5的整數(shù)，不同的所述二維編碼圖像中相同位置的單元格組的組成形式不同；

以及，根據(jù)所述二維編碼圖像的定位和方向檢測的結(jié)果確定所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

進(jìn)一步的，所述方法還包括：

根據(jù)設(shè)置在所述虛擬現(xiàn)實設(shè)備的慣性測量單元對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測，并根據(jù)所述慣性測量單元的檢測結(jié)果獲取所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息；

在經(jīng)識別確定所述室內(nèi)圖像中包含所述二維編碼圖像時，將所述圖像獲取單元的測量誤差值調(diào)至第一閾值以下，并將所述慣性測量單元的測量誤差值調(diào)至第二閾值以上，且所述第一閾值小于第二閾值；

以及，在經(jīng)識別確定所述室內(nèi)圖像未包含所述二維編碼圖像時，將所述圖像獲取單元的測量誤差值調(diào)至第二閾值以上，并將所述慣性測量單元的測量誤差值調(diào)至第第一閾值以下。

進(jìn)一步的，所述方法還包括：

將分別由所述二維編碼圖像以及所述慣性測量單元獲取的三維空間位置信息按時間順序進(jìn)行匯總，得到所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置匯總信息；

以及，將所述三維空間位置匯總信息發(fā)送至所述虛擬現(xiàn)實設(shè)備或外部顯示設(shè)備進(jìn)行顯示。

進(jìn)一步的，所述在確定室內(nèi)圖像中包含所述二維編碼圖像時，對所述二維編碼圖像進(jìn)行定位和方向檢測，包括：

在識別到某一室內(nèi)圖像中包含所述二維編碼圖像時，獲取所述二維編碼圖像的單元格組，其中，所述單元格組由X*Y個單元格組成，且4≤X<N，4≤Y<N；

根據(jù)所述二位編碼圖案的所述單元格組中的單元格分布狀態(tài)，確定所述二位編碼圖案的三維空間位置信息；

以及，根據(jù)所述圖像獲取單元的內(nèi)置參數(shù)和所述二位編碼圖案的三維空間位置信息，確定所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

進(jìn)一步的，所述根據(jù)慣性測量單元對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測，包括：

根據(jù)慣性測量單元及擴(kuò)展卡爾曼濾波器對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測及外部信號矯正。

由上述技術(shù)方案可知，本發(fā)明所述的一種三維空間跟蹤定位系統(tǒng)及方法，系統(tǒng)包括圖像獲取單元、固定設(shè)置在目標(biāo)定位室內(nèi)的二維編碼顯示單元、以及，與圖像獲取單元通信連接的三維空間位置獲取單元；方法包括：實時獲取虛擬現(xiàn)實設(shè)備的圖像獲取單元拍攝的當(dāng)前目標(biāo)定位室的室內(nèi)圖像；識別室內(nèi)圖像，并在確定室內(nèi)圖像中包含二維編碼圖像時，對二維編碼圖像進(jìn)行定位和方向檢測；根據(jù)二維編碼圖像的定位和方向檢測的結(jié)果確定圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。本發(fā)明實現(xiàn)了對三維空間的高效且可靠的跟蹤定位，且對定位場景及設(shè)備計算能力均無特殊要求，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

附圖說明

為了更清楚地說明本發(fā)明實施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見，下面描述中的附圖是本發(fā)明的一些實施例，對于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1是本發(fā)明實施例一的一種三維空間跟蹤定位系統(tǒng)的第一種具體實施方式的結(jié)構(gòu)示意圖；

圖2是本發(fā)明實施例二的一種三維空間跟蹤定位系統(tǒng)的第二種具體實施方式的結(jié)構(gòu)示意圖；

圖3是本發(fā)明實施例三的定位系統(tǒng)中三維空間位置獲取單元30的具體實施方式的結(jié)構(gòu)示意圖；

圖4是本發(fā)明實施例四中的一種三維空間跟蹤定位方法的第一種具體實施方式的流程示意圖；

圖5是本發(fā)明實施例五中的一種三維空間跟蹤定位方法的第二種具體實施方式的流程示意圖；

圖6是本發(fā)明實施例六中的一種三維空間跟蹤定位方法的第三種具體實施方式的流程示意圖；

圖7是本發(fā)明實施例七中的定位方法中的步驟200的流程示意圖；

圖8是本發(fā)明應(yīng)用實例中的一種二維編碼圖案的舉例示意圖；

圖9是本發(fā)明應(yīng)用實例中的人為將世界坐標(biāo)系定義在二維編碼圖案的中心的舉例示意圖；

圖10是本發(fā)明應(yīng)用實例中的攝像頭拍攝到的帶有二維編碼圖案的識別示意圖。

具體實施方式

為使本發(fā)明實施例的目的、技術(shù)方案和優(yōu)點更加清楚，下面將結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進(jìn)行清楚、完整的描述，顯然，所描述的實施例是本發(fā)明一部分實施例，而不是全部的實施例。基于本發(fā)明中的實施例，本領(lǐng)域普通技術(shù)人員在沒有作出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護(hù)的范圍。

本發(fā)明的實施例一提供了一種三維空間跟蹤定位系統(tǒng)的第一種具體實施方式，參見圖1，該三維空間跟蹤定位系統(tǒng)具體包括如下內(nèi)容：

圖像獲取單元10、固定設(shè)置在目標(biāo)定位室內(nèi)的二維編碼顯示單元20、以及，與所述圖像獲取單元10通信連接的三維空間位置獲取單元30。

在圖像獲取單元10中，所述圖像獲取單元10用于實時獲取所述目標(biāo)定位室的室內(nèi)圖像，并將所述室內(nèi)圖像發(fā)送至所述三維空間位置獲取單元30；其中，圖像獲取單元10可以為設(shè)置在所述虛擬現(xiàn)實設(shè)備上的攝像頭，而本發(fā)明所述的虛擬現(xiàn)實設(shè)備可以為虛擬現(xiàn)實VR、增強(qiáng)現(xiàn)實AR和其它一些機(jī)器人設(shè)備。

在二維編碼顯示單元20中，所述二位編碼顯示單元20用于顯示二維編碼圖像21，所述二位編碼顯示單元為印設(shè)有所述二維編碼圖像的紙板，且該紙板設(shè)置在所述目標(biāo)定位室的天花板上；所述二維編碼圖像21由N*N個單元格組成，為根據(jù)實際設(shè)置需求自定義的特殊二維編碼圖案，所述單元格的填充顏色為黑或白，也可以為其他形式的填充，且N為大于或等于5的整數(shù)，不同的所述二維編碼圖像21中相同位置的單元格組的組成形式不同，所述單元格組由X*Y個單元格組成，且4≤X<N，4≤Y<N；在本實施例中，二維編碼圖案可以為“De Bruijn(德布魯因)”二維編碼圖，這種編碼圖案的中任意一個固定大小的單元X*Y(不一定是正方形單元)的圖案都是唯一的；因此只要攝像機(jī)拍到含有一個單元以上的圖案，即能進(jìn)行識別跟定位。另外，印設(shè)有所述二維編碼圖像的紙板可以根據(jù)應(yīng)用目標(biāo)定位室的房間面積而改變大小，即相對較大的房間設(shè)置包含較大的二維編碼圖像21的紙張，相對較小的房間設(shè)置包含較小的二維編碼圖像21的紙張。

在三維空間位置獲取單元30中，所述三維空間位置獲取單元30用于識別所述室內(nèi)圖像，并在確定所述室內(nèi)圖像中包含所述二維編碼圖像21時，根據(jù)該二維編碼圖像21獲取所述圖像獲取單元10當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息，其中，所述識別到所述二維編碼圖像21為完全識別到所述二維編碼圖像21，或者由于所述二維編碼圖像21被部分遮擋而識別到部分所述二維編碼圖像21。

從上述描述可知，本發(fā)明的實施例實現(xiàn)了對三維空間的高效且可靠的跟蹤定位，且對定位場景及設(shè)備計算能力均無特殊要求，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例二提供了一種三維空間跟蹤定位系統(tǒng)的第二種具體實施方式，參見圖2，該三維空間跟蹤定位系統(tǒng)還具體包括如下內(nèi)容：

慣性測量單元40，且所述慣性測量單元40與圖像獲取單元10均設(shè)置在虛擬現(xiàn)實設(shè)備上，且所述慣性測量單元40與所述三維空間位置獲取單元30通信連接。

在所述慣性測量單元40中，所述慣性測量單元40用于對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測，并將檢測結(jié)果發(fā)送至所述三維空間位置獲取單元30。

從上述描述可知，本發(fā)明的實施例能夠在無法識別到所述二維編碼圖像時，仍能夠根據(jù)慣性測量單元獲取虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息，以及，能夠獲取圖像獲取單元拍攝的2幀之間的中間幀的虛擬現(xiàn)實設(shè)備的對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例三提供了上述定位系統(tǒng)中三維空間位置獲取單元30的具體實施方式，參見圖3，該三維空間位置獲取單元30看具體包括如下內(nèi)容：

圖像識別子單元31、圖像位置生成子單元32、傳感位置生成子單元33和三維空間位置發(fā)送子單元34。

在圖像識別子單元31中，所述圖像識別子單元31用于接收所述圖像獲取單元發(fā)送的室內(nèi)圖像，并在識別所述室內(nèi)圖像中是否包含有完整或部分的所述二維編碼圖像21時，將包含有該二維編碼圖像21的室內(nèi)圖像發(fā)送至所述圖像位置生成子單元32。

在圖像位置生成子單元32中，所述圖像位置生成子單元32用于根據(jù)接收的室內(nèi)圖像中的所述二維編碼圖像21，獲取所述圖像獲取單元10當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

在傳感位置生成子單元33中，所述傳感位置生成子單元33用于接收所述慣性測量單元40發(fā)送的所述檢測結(jié)果，并根據(jù)所述檢測結(jié)果獲取所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

在三維空間位置發(fā)送子單元34中，所述三維空間位置發(fā)送子單元34用于將所述圖像位置生成子單元32和所述傳感位置生成子單元33分別生成的三維空間位置信息按時間順序進(jìn)行匯總，得到所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置匯總信息，并將所述三維空間位置匯總信息發(fā)送至所述虛擬現(xiàn)實設(shè)備或外部顯示設(shè)備進(jìn)行顯示。

從上述描述可知，本發(fā)明的實施例利用自定義的特殊二維編碼圖案，基于計算機(jī)視覺技術(shù)，以慣性測量單元作為輔助，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例四提供了應(yīng)用上述定位系統(tǒng)實現(xiàn)的一種三維空間跟蹤定位方法的第一種具體實施方式，參見圖4，該三維空間跟蹤定位方法具體包括如下內(nèi)容：

步驟100：實時獲取虛擬現(xiàn)實設(shè)備的圖像獲取單元拍攝的當(dāng)前目標(biāo)定位室的室內(nèi)圖像。

步驟200：識別所述室內(nèi)圖像，并在確定室內(nèi)圖像中包含所述二維編碼圖像時，對所述二維編碼圖像進(jìn)行定位和方向檢測，其中，所述二維編碼圖像由N*N個單元格組成，所述單元格的填充顏色為黑或白，且N為大于或等于5的整數(shù)，不同的所述二維編碼圖像中相同位置的單元格組的組成形式不同。

步驟300：根據(jù)所述二維編碼圖像的定位和方向檢測的結(jié)果確定所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

從上述描述可知，本發(fā)明的實施例實現(xiàn)了對三維空間的高效且可靠的跟蹤定位，且對定位場景及設(shè)備計算能力均無特殊要求，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例五提供了上述定位方法的第二種具體實施方式，參見圖5，該定位方法還具體包括如下內(nèi)容：

步驟400：根據(jù)設(shè)置在所述虛擬現(xiàn)實設(shè)備的慣性測量單元對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測，并根據(jù)所述慣性測量單元的檢測結(jié)果獲取所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

其中的根據(jù)設(shè)置在所述虛擬現(xiàn)實設(shè)備的慣性測量單元對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測為根據(jù)慣性測量單元及擴(kuò)展卡爾曼濾波器對所述虛擬現(xiàn)實設(shè)備的運(yùn)動軌跡和方向進(jìn)行檢測及外部信號矯正。

步驟500：在經(jīng)識別確定所述室內(nèi)圖像中包含所述二維編碼圖像時，將所述圖像獲取單元的測量誤差值調(diào)至第一閾值以下，并將所述慣性測量單元的測量誤差值調(diào)至第二閾值以上，且所述第一閾值小于第二閾值。

步驟600：在經(jīng)識別確定所述室內(nèi)圖像未包含所述二維編碼圖像時，將所述圖像獲取單元的測量誤差值調(diào)至第二閾值以上，并將所述慣性測量單元的測量誤差值調(diào)至第第一閾值以下。

從上述描述可知，本發(fā)明的實施例能夠在無法識別到所述二維編碼圖像時，仍能夠根據(jù)慣性測量單元獲取虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息，以及，能夠獲取圖像獲取單元拍攝的2幀之間的中間幀的虛擬現(xiàn)實設(shè)備的對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例六提供了上述定位方法的第三種具體實施方式，參見圖6，該定位方法還具體包括如下內(nèi)容：

步驟700：將分別由所述二維編碼圖像以及所述慣性測量單元獲取的三維空間位置信息按時間順序進(jìn)行匯總，得到所述虛擬現(xiàn)實設(shè)備當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置匯總信息。

步驟800：將所述三維空間位置匯總信息發(fā)送至所述虛擬現(xiàn)實設(shè)備或外部顯示設(shè)備進(jìn)行顯示。

從上述描述可知，本發(fā)明的實施例利用自定義的特殊二維編碼圖案，基于計算機(jī)視覺技術(shù)，以慣性測量單元作為輔助，實現(xiàn)了大范圍、不限人數(shù)的高精度、高魯棒性的空間定位技術(shù)。

本發(fā)明的實施例七提供了上述定位方法中步驟200的具體實施方式，參見圖7，該步驟200具體包括如下內(nèi)容：

步驟201：識別所述室內(nèi)圖像。

步驟202：在識別到某一室內(nèi)圖像中包含所述二維編碼圖像時，獲取所述二維編碼圖像的單元格組。

在步驟202中，所述單元格組由M*M個單元格組成，且4≤M<N。

步驟203：根據(jù)所述二位編碼圖案的所述單元格組中的單元格分布狀態(tài)，確定所述二位編碼圖案的三維空間位置信息。

步驟204：根據(jù)所述圖像獲取單元的內(nèi)置參數(shù)和所述二位編碼圖案的三維空間位置信息，確定所述圖像獲取單元當(dāng)前對應(yīng)在目標(biāo)定位室內(nèi)的三維空間位置信息。

從上述描述可知，本發(fā)明的實施例提供了對所述二維編碼圖像進(jìn)行定位和方向檢測的具體方式，實現(xiàn)了對三維空間的高效且可靠的跟蹤定位。

為進(jìn)一步的說明本方案，本發(fā)明還提供了應(yīng)用上述三維空間跟蹤定位系統(tǒng)實現(xiàn)三維空間跟蹤定位方法的應(yīng)用實例，以所述單元格的填充顏色為黑或白進(jìn)行說明，該應(yīng)用實例具體包括如下內(nèi)容：

(1)基于攝像頭的精準(zhǔn)定位技術(shù)

1、取得攝像頭的圖像。

2、判斷圖像中是否有二維編碼圖案。

3、對二維編碼圖案進(jìn)行定位和方向檢測。

4、通過相機(jī)的內(nèi)參和二維編碼圖案的位置和方向信息確定相機(jī)的三維空間坐標(biāo)。

(2)傳感器融合技術(shù)

單一攝像頭難以解決圖像被完全遮擋的問題，這時候需要利用IMU來對運(yùn)動軌跡和方向進(jìn)行預(yù)測。但是IMU長時間工作會產(chǎn)生累計誤差，需要外部的信號矯正。因此，利用傳感器融合技術(shù)結(jié)合攝像頭和IMU的優(yōu)點，可以實現(xiàn)精準(zhǔn)又穩(wěn)定的定位方案。

具體實現(xiàn)過程如下：

(1)基于圖像匹配的攝像頭姿態(tài)估計

如圖8所示，這是多種二維編碼圖案中的一種。所使用的二維編碼圖案不具有旋轉(zhuǎn)對稱性，可以通過攝像機(jī)所獲取的帶有二維編碼圖案的圖像來確定二維編碼圖案的方向；并且，二位編碼圖案每個子塊(最小到4*4)的分布是唯一的，所以對于被遮擋的二維編碼圖案，仍然可以通過識別子塊，確定子塊在二維編碼圖案中的位置，來確定整個二維編碼圖案的三維空間位置。

如圖9所示，我們?nèi)藶橐?guī)定世界坐標(biāo)系的原點在二維編碼圖案的中心，并且圖案的邊長為1。在此處采用右手坐標(biāo)系，所以，根據(jù)右手定則，Z軸應(yīng)該是垂直于平面向里的。圖中標(biāo)出了二維編碼圖案所在的坐標(biāo)系以及角點的坐標(biāo)值。

在規(guī)定了圖9中世界坐標(biāo)的基礎(chǔ)上，根據(jù)相已知的攝像機(jī)內(nèi)參矩陣K和二位編碼圖案的投影坐標(biāo)x，二維編碼圖案的世界坐標(biāo)X，可以計算出。攝像機(jī)相對于當(dāng)前使用的二維編碼圖案的外參矩陣[R|T]。

x＝K×[R|T]×X

在計算出了攝像機(jī)相對于當(dāng)前使用的二維編碼圖案的外參矩陣[R|T]后，對于不同視角看到的二維編碼圖案，根據(jù)上式，可以很快地計算出攝像機(jī)的三維坐標(biāo)。

圖10為對攝像機(jī)采集到的圖像進(jìn)行二維編碼圖案識別的中間過程。通過算法確定各個小方格子的中心位置，從而精準(zhǔn)定位二維編碼圖案。在之后的圖像中，結(jié)合上一幀圖像，對二維編碼圖像進(jìn)行快速跟蹤。

(2)基于擴(kuò)展卡爾曼濾波的傳感器融合

擴(kuò)展卡爾曼濾波器(EKF)是傳感器融合最常用的工具，主要由預(yù)測和矯正兩部分組成。是卡爾曼濾波器的工作流程包括：

首先用上一狀態(tài)，通過狀態(tài)轉(zhuǎn)移矩陣對當(dāng)前狀態(tài)進(jìn)行估計；

用當(dāng)前狀態(tài)的估計值，通過觀測矩陣對當(dāng)前測量進(jìn)行估計；

計算卡爾曼增益，它代表狀態(tài)或者測量的可信度；

得到當(dāng)前時刻的實際測量值，通過卡爾曼增益對當(dāng)前狀態(tài)進(jìn)行糾正。

要將不同傳感器的數(shù)據(jù)進(jìn)行融合，主要有兩個問題：

運(yùn)動模型的建立：

運(yùn)動模型決定了狀態(tài)轉(zhuǎn)移矩陣，在預(yù)測階段主要依靠狀態(tài)轉(zhuǎn)移矩陣來實現(xiàn)前后狀態(tài)之間的變換。

參數(shù)的動態(tài)調(diào)整：

在實際應(yīng)用中，不同情況下不同傳感器的數(shù)據(jù)的重要性不一樣，需要動態(tài)調(diào)整一些參數(shù)。當(dāng)攝像頭沒有被遮擋的時候，我們把攝像頭的測量誤差設(shè)置得很低，把IMU的測量誤差調(diào)高。此時，主要依靠攝像頭的實際測量值。反之，當(dāng)攝像頭被遮擋時，我們增大攝像頭的測量誤差，減少IMU的測量誤差。此時，主要依靠IMU的測量數(shù)據(jù)，通過EKF公式推導(dǎo)出攝像頭的姿態(tài)。

另外，不同傳感器有不同的幀率，一般來說，IMU的幀率要高于攝像頭的幀率，因此在攝像頭兩幀之間，需要依靠IMU的數(shù)據(jù)進(jìn)行預(yù)測，以保證整個系統(tǒng)的輸出幀率足夠高。

從上述描述可知，本發(fā)明的應(yīng)用實例通過在攝像機(jī)圖像中快速地定位并跟蹤二維編碼圖案，從而可以計算出攝像機(jī)的三維空間坐標(biāo)。并且采用了傳感器融合技術(shù)，很好的結(jié)合了攝像頭的精準(zhǔn)性和IMU的預(yù)測能力，使得定位算法能夠很好地處理環(huán)境光的影響，并且在被遮擋時也能準(zhǔn)確地定位。整套定位方案具有低成本，高效率，集準(zhǔn)確性和魯棒性于一體。

以上實施例僅用于說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述實施例對本發(fā)明進(jìn)行了詳細(xì)的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其依然可以對前述各實施例所記載的技術(shù)方案進(jìn)行修改，或者對其中部分技術(shù)特征進(jìn)行等同替換；而這些修改或替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明各實施例技術(shù)方案的精神和范圍。