[發明專利]一種基于影像空間和全景輔助的室內定位導航方法

有效

• 申請號: 201811091574.9
• 申請日: 2018-09-19
• 公開（公告）號: CN109141432B
• 公開（公告）日: 2020-04-10
• 發明（設計）人: 張春森;張奇源
• 申請（專利權）人: 西安科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 西安創知專利事務所 61213
• 代理人: 譚文琰

1.一種基于影像空間和全景輔助的室內定位導航方法，其特征在于，該方法包括以下步驟：

步驟一、控制點的布設及世界坐標系的建立：

步驟101、在室內每個墻壁上設置至少兩個控制點；

步驟102、以室內任意一點為原點O_s，以過原點O_s且指向正北為X_s軸，以過原點O_s且指向正東為Y_s軸，以過原點O_s且垂直于由X_s軸和Y_s軸形成的平面O_sX_sY_s指向天為Z_s軸，建立世界坐標系O_sX_sY_sZ_s；

步驟103、采用全站儀測量多個所述控制點的坐標，則獲得在世界坐標系O_sX_sY_sZ_s下多個所述控制點的空間坐標，通過所述參數輸入模塊(17)輸入，并存儲至存儲器(11)中，形成控制點空間坐標數據庫；

步驟二、攝像機的校正及室內序列影像的獲取：

步驟201、第一控制器(1)調取相機檢校模塊對攝像機(2)進行相機檢校處理，得到攝像機(2)的參數矩陣其中，f表示攝像機(2)的焦距，f/d_x表示攝像機(2)在圖像坐標系u軸上的歸一化焦距，f/d_y表示攝像機(2)在像素坐標系v軸上的歸一化焦距，1/d_x表示像素坐標系u軸上單位像素的尺寸大小，1/d_y表示像素坐標系v軸上單位像素的尺寸大小，u₀表示像主點在像素坐標系u軸上的偏移量，v₀表示像主點在像素坐標系v軸上的偏移量；

步驟202、采用攝像機(2)對室內拍攝序列影像，并將拍攝到的序列影像發送至第一控制器(1)，第一控制器(1)將接收到的序列影像通過第一無線通信模塊(3)和第二無線通信模塊(7)發送至計算機(9)；其中，相鄰兩幅影像的旁向重疊度不低于50％；

步驟三、特征點提取與匹配：

步驟301、計算機(9)調取SIFI特征提取算法對每一幅影像分別進行特征點提取，得到多個特征點描述子歸一化向量；

步驟302、計算機(9)調取特征點匹配模塊對相鄰兩幅所述影像中的特征點進行匹配，得到多組匹配點對；

步驟四、特征點的剔除：

步驟401、計算機(9)將任一組匹配點對記作第k幅影像中第j′個特征匹配點和第k+1幅影像中第j′個特征匹配點，計算機(9)將第k幅影像中第j′個特征匹配點的特征點描述子歸一化向量記作計算機(9)將第k+1幅影像中第j′個特征匹配點的特征點描述子歸一化向量記作其中，j′為正整數，且j′的取值范圍為1～N_p，N_p為匹配點對的對數；表示第k幅影像中第j′個特征匹配點的特征點描述子歸一化向量的第i個分量，表示第k+1幅影像中第j′個特征匹配點的特征點描述子歸一化向量的第i個分量；

步驟402、計算機(9)根據公式得到第k幅影像中第j′個特征匹配點的特征向量平均值；計算機(9)根據公式得到第k+1幅影像中第j′個特征匹配點的特征向量平均值；

步驟403、計算機(9)根據公式得到第k幅影像中第j′個特征匹配點和第k+1幅影像中第j′個特征匹配點的相關系數X_gxs；

步驟404、計算機(9)根據判定條件X_gxs≤0.6進行判定，當X_gxs≤0.6成立，執行步驟405；否則，執行步驟406；

步驟405、當X_gxs≤0.6成立，則為誤匹配點對，計算機(9)將誤匹配誤匹配點對剔除；

步驟406、當X_gxs>0.6成立，則為正確匹配點對，計算機(9)將正確匹配點對保存；

步驟407、多次重復步驟402至步驟406，完成多組匹配點對的一次剔除，并獲得剔除后的多組匹配點對；

步驟五、特征點空間坐標的計算：

步驟501、計算機(9)將剔除后的任一組匹配點對記作第k幅影像中第j″個剔除后特征匹配點和第k+1幅影像中第j″個剔除后特征匹配點，計算機(9)將第k幅影像中第j″個剔除后特征匹配點的特征點描述子歸一化向量記作計算機(9)將第k+1幅影像中第j″個剔除后特征匹配點的特征點描述子歸一化向量記作計算機(9)將第k幅影像中第j″個剔除后特征匹配點的像素坐標記作計算機(9)將第k+1幅影像中第j″個剔除后特征匹配點的像素坐標記作其中，j″為正整數，且j″的取值范圍為1～N′_p，N′_p為剔除后的匹配點對的對數；表示第k幅影像中第j″個剔除后特征匹配點的特征點描述子歸一化向量的第i個分量，表示第k+1幅影像中第j″個剔除后特征匹配點的特征點描述子歸一化向量的第i個分量；

步驟502、計算機(9)按照拍攝先后順序在第k幅影像的拍攝處建立第k個相機坐標系，在第k+1幅影像的拍攝處建立第k+1個相機坐標系O_z(k+1)X_z(k+1)Y_z(k+1)Z_z(k+1)；其中，在攝像機(2)對室內拍攝序列影像的第一個位置處即第一副影像的拍攝處建立第一相機坐標系O_z1X_z1Y_z1Z_z1；

步驟503、計算機(9)調取相機旋轉矩陣和平移向量計算模塊，從剔除后的多組匹配點對中選擇四對匹配點輸入，得到第k+1個相機坐標系到第k個相機坐標系的相機旋轉矩陣R_k+1,k和平移向量t_k+1,k；其中，相機旋轉矩陣R_k+1,k為3×3矩陣，平移向量t_k+1,k為3×1向量；

步驟504、計算機(9)根據公式R_k+1,1＝R_k+1,k×R_k,k-1×...×R_2,1，得到第k+1個相機坐標系到第一個相機坐標系的相機旋轉矩陣R_k+1,1；其中，R_k,k-1表示第k個相機坐標系到第k-1個相機坐標系的相機旋轉矩陣，R_2,1表示第二個相機坐標系到第一個相機坐標系的相機旋轉矩陣；計算機(9)根據公式t_k+1,1＝t_k+1,k+t_k,k-1+...+t_2,1，得到第k+1個相機坐標系到第一個相機坐標系的平移向量t_k+1,1，其中，t_k,k-1表示第k個相機坐標系到第k-1個相機坐標系的平移向量，t_2,1表示第二個相機坐標系到第一個相機坐標系的平移向量；計算機(9)根據公式得第k+1個相機坐標系到第一個相機坐標系的投影矩陣P_k+1,1；

步驟505、計算機(9)根據公式得到第k幅影像中第j″個剔除后特征匹配點的圖像坐標P_k,j″(x_k,j″,y_k,j″)，計算機(9)根據公式得到第k+1幅影像中第j″個剔除后特征匹配點的圖像坐標P_k+1,j″(x_k+1,j″,y_k+1,j″)；計算機(9)根據公式得到深度參數Z_c；其中，B表示第k+1個相機坐標系原點到第k個相機坐標系原點的距離，f表示攝像機(2)的焦距；

步驟507、計算機(9)根據公式得到在第一個相機坐標系下第k+1幅影像中第j″個剔除后特征匹配點的坐標P_k+1,j″(X_k+1,j″,Y_k+1,j″,Z_k+1,j″)；

步驟508、計算機(9)判斷剔除后的多組匹配點對所在的相鄰兩幅所述影像中是否存在匹配的控制點像點，當相鄰兩幅所述影像中存在匹配的控制點像點，從步驟509執行；否則，執行步驟5010；

步驟509、當剔除后的多組匹配點對所在的相鄰兩幅所述影像中存在匹配的控制點像點，計算機(9)調取圖形標記模塊在相鄰兩幅影像中匹配的控制點像點處標記，計算機(9)調取像素坐標提取模塊對匹配的控制點像點的像素坐標進行提取，獲取匹配的控制點像點的像素坐標；

步驟5010、計算機(9)按照步驟四和步驟501至步驟508所述的方法，對剔除后的多組匹配點對所在的下一個相鄰兩幅影像進行處理；

步驟5011、多次重復步驟5010，得到多對匹配的控制點像點的像素坐標，從多個匹配的控制點像點的像素坐標中選擇四對匹配的控制點像點的像素坐標，并將選擇的第f對匹配的控制點像點記作第k幅影像中匹配的控制點像點和第k+1幅影像中匹配的控制點像點，且第k幅影像中匹配的控制點像點的像素坐標記作第k+1幅影像中匹配的控制點像點的像素坐標記作f＝1,2，3，4；

步驟5012、計算機(9)按照步驟504到步驟507所述的方法，得到第f對匹配的控制點像點對應的控制點在第一個相機坐標系下的坐標和計算機(9)從步驟103中控制點空間坐標數據庫中得到第f對匹配的控制點像點對應的控制點的空間坐標和其中，表示第k幅影像中匹配的控制點像點對應的控制點在第一個相機坐標系下的坐標，表示第k+1幅影像中匹配的控制點像點對應的控制點在第一個相機坐標系下的坐標，表示第k幅影像中匹配的控制點像點對應的控制點在世界坐標系下的空間坐標，表示第k+1幅影像中匹配的控制點像點對應的控制點在世界坐標系下的空間坐標；

步驟5012、計算機(9)調取相機坐標系到世界坐標系的旋轉矩陣和平移向量計算模塊，并輸入四對匹配的控制點像點對應的控制點在第一個相機坐標系下的坐標和在世界坐標系下的空間坐標，得到式子

其中，ω′表示第一相機坐標系O_z1X_z1Y_z1Z_z1中Y_z1O_z1Z_z1平面繞O_z1X_z1軸旋轉至Y_z1O_z1Z_z1平面與Y_sO_sZ_s平面平行時的旋轉角度，表示第一相機坐標系O_z1X_z1Y_z1Z_z1中X_z1O_z1Z_z1平面繞O_z1Y_z1軸旋轉至X_z1O_z1Z_z1平面與X_sO_sZ_s平面平行時的旋轉角度，κ′表示第一相機坐標系O_z1X_z1Y_z1Z_z1中X_z1O_z1Y_z1平面繞O_z1Z_z1軸旋轉至X_z1O_z1Y_z1平面與X_sO_sY_s平面平行時的旋轉角度；

步驟5013、計算機(9)調取最小二乘計算模塊，并設置初始值λ＝1.0,ΔX＝ΔY＝ΔZ＝0.0，對式子和進行計算，得到λ尺度因子、旋轉矩陣和平移向量

步驟5014、計算機(9)根據式子得到在世界坐標系O_sX_sY_sZ_s下第k+1幅影像中第j″個剔除后特征匹配點的空間坐標

步驟5015、計算機(9)將第k+1幅影像中第j″個剔除后特征匹配點的特征點描述子歸一化向量記作第k+1幅影像中第j″個剔除后特征匹配點的圖像坐標P_k+1,j″(x_k+1,j″,y_k+1,j″)和第k+1幅影像中第j″個剔除后特征匹配點的空間坐標存儲至與計算機(9)相接的數據存儲器(11)中，形成特征點數據庫；

步驟六、室內全景影像的獲取：

步驟601、將室內劃分為商鋪場景和過道場景；其中，室內每個商鋪稱為一個商鋪場景，將室內過道按照預先設定的場景距離將室內過道劃分為多個過道場景；所述預先設定的場景距離為20米～30米；

步驟602、采用魚眼相機模塊(4)對商鋪場景進行全景拍攝，并將拍攝的多幅商鋪場景拍攝影像發送至第二控制器(5)，第二控制器(5)將接收到的多幅商鋪場景拍攝影像通過第三無線通信模塊(6)和第二無線通信模塊(7)發送至計算機(9)；并采用全站儀測量魚眼相機模塊(4)的坐標，則獲取在世界坐標系O_sX_sY_sZ_s下魚眼相機模塊(4)對商鋪場景進行全景拍攝時的坐標，并將在世界坐標系O_sX_sY_sZ_s下魚眼相機模塊(4)對商鋪場景進行全景拍攝時的坐標稱作商鋪場景定位坐標，通過所述參數輸入模塊(17)輸入商鋪場景定位坐標和商鋪名稱，并存儲至存儲器(11)中，計算機(9)調取全景影像拼接模塊，并輸入多幅商鋪場景拍攝影像進行拼接，形成商鋪場景全景影像，得到一個商鋪場景全景影像數據庫；

步驟603、采用魚眼相機模塊(4)對每個過道場景進行全景拍攝，并將拍攝的多幅過道場景拍攝影像發送至第二控制器(5)，第二控制器(5)將接收到的多幅過道場景拍攝影像通過第三無線通信模塊(6)和第二無線通信模塊(7)發送至計算機(9)；并采用全站儀測量魚眼相機模塊(4)的坐標，則獲取在世界坐標系O_sX_sY_sZ_s下魚眼相機模塊(4)對過道場景進行全景拍攝時的坐標，并將在世界坐標系O_sX_sY_sZ_s下魚眼相機模塊(4)對過道場景進行全景拍攝時的坐標稱作過道場景定位坐標，通過所述參數輸入模塊(17)輸入過道場景定位坐標，并存儲至存儲器(11)中；計算機(9)調取全景影像拼接模塊，并輸入多幅過道場景拍攝影像進行拼接，形成過道場景全景影像，得到一個過道場景全景影像數據庫；其中，商鋪場景拍攝影像和過道場景拍攝影像的旁向重疊度均不低于30％；

步驟604、多次重復步驟602至步驟603，得到多個商鋪場景全景影像數據庫和多個過道場景全景影像數據庫，并將多個商鋪場景全景影像數據庫和多個過道場景全景影像數據庫按照拍攝先后順序進行存儲，則將多個商鋪場景全景影像數據庫和多個過道場景全景影像數據庫形成的數據庫稱作場景影像數據庫，將商鋪場景定位坐標和過道場景定位坐標均稱作場景定位坐標；

步驟七、室內定位：

步驟701、采用手機(8)采集用戶當前所在位置的一幅定位影像并通過第二無線通信模塊(7)發送至計算機(9)；

步驟702、計算機(9)按照步驟301所述的方法對定位影像進行特征點提取，獲取定位影像中多個特征點描述子歸一化向量；

步驟703、計算機(9)按照步驟302所述的方法將定位影像中多個特征點描述子歸一化向量與步驟5015中形成的特征點數據庫進行匹配，得到定位影像的匹配點對數M_dp；

步驟704、計算機(9)根據判定條件M_dp≥8進行判斷，當M_dp≥8成立，從步驟705開始執行；否則，從步驟701執行，重新拍一副定位影像；

步驟705、當M_dp≥8成立，計算機(9)調取相機Exif信息讀取模塊讀取定位影像中的Exif信息，得到手機(8)中相機拍攝時的相機焦距f_d；

步驟706、計算機(9)調取投影矩陣計算模塊，從定位影像的匹配點對數中選擇四對定位影像的匹配點輸入，得到定位影像的投影矩陣P′，計算機(9)調取QR分解模塊，對定位影像的投影矩陣P′進行QR分解，得到定位影像的外方位元素初值ω,κ,X_S0,Y_S0,Z_S0；其中，ω表示手機相機的相機坐標系O_dpX_dpY_dpZ_dp中Y_dpO_dpZ_dp平面繞O_dpX_dp軸旋轉至Y_dpO_dpZ_dp平面與Y_sO_sZ_s平面平行時的旋轉角度，表示手機相機的相機坐標系O_dpX_dpY_dpZ_dp中X_dpO_dpZ_dp平面繞O_dpY_dp軸旋轉至X_dpO_dpZ_dp平面與X_sO_sZ_s平面平行時的旋轉角度，κ表示手機相機的相機坐標系O_dpX_dpY_dpZ_dp中X_dpO_dpY_dp平面繞O_dpZ_dp軸旋轉至X_dpO_dpY_dp平面與X_sO_sY_s平面平行時的旋轉角度，X_S0,Y_S0,Z_S0分別表示手機(8)中相機光心在世界坐標系O_sX_sY_sZ_s的X_s軸方向的初始坐標、Y_s軸方向的初始坐標和Z_s軸方向的初始坐標；

步驟707、計算機(9)將M_dp對定位影像的匹配點對中位于步驟5015中特征點數據庫中的匹配特征點記作定位影像數據庫匹配特征點，得到定位影像數據庫匹配特征點的特征點描述子歸一化向量、定位影像數據庫匹配特征點的像素坐標和定位影像數據庫匹配特征點的空間坐標；其中，所述定位影像數據庫匹配特征點的數量不少于8個；

步驟708、計算機(9)從多個定位影像數據庫匹配特征點中選擇三個定位影像數據庫匹配特征點并記作定位特征點，三個定位特征點分別記作第一定位特征點、第二定位特征點和第三定位特征點，計算機(9)從步驟5015中形成的特征點數據庫中，得到三個定位特征點的圖像坐標，并將第一定位特征點的圖像坐標記作P_d,1(x_d,1,y_d,1)，所述第二定位特征點的圖像坐標記作P_d,2(x_d,2,y_d,2)，所述第三定位特征點的圖像坐標記作P_d,3(x_d,3,y_d,3)；且計算機(9)從步驟5015中形成的特征點數據庫中，得到三個定位特征點的空間坐標，并分別將第一定位特征點的空間坐標記作第二定位特征點的空間坐標記作第三定位特征點的空間坐標記作

步驟709、計算機(9)調取共線方程模塊并輸入第一定位特征點、第二定位特征點和第三定位特征點的圖像坐標和空間坐標，得到共線方程式其中，

步驟7010、計算機(9)調取最小二乘計算模塊，并輸入手機(8)中相機光心在世界坐標系O_sX_sY_sZ_s的X_s軸方向的初始坐標X_S0、Y_s軸方向的初始坐標Y_S0和Z_s軸方向的初始坐標Z_S0分別作為X′_S0、Y′_S0和Z′_S0的初始值，并對步驟709中共線方程式進行最小二乘計算，得到手機(8)中相機光心在世界坐標系O_sX_sY_sZ_s的X_s軸方向的校正坐標X′_S0、Y_s軸方向的校正坐標Y′_S0和Z_s軸方向的校正坐標Z′_S0，則得到用戶當前的定位坐標

步驟八、室內導航：

步驟801、計算機(9)將第l個商鋪場景全景影像數據庫的商鋪場景定位坐標記作P_sp,l(X_sp,l,Y_sp,l,Z_sp,l)；計算機(9)將第h個過道場景全景影像數據庫中的過道場景定位坐標記作P_gd,h(X_gd,h,Y_gd,h,Z_gd,h)；l為正整數，h為正整數；

步驟802、用戶通過參數輸入模塊(17)輸入用戶最終抵達的商鋪名稱作為目標商鋪，且計算機(9)通過商鋪場景全景影像數據庫，得到目標商鋪的商鋪場景定位坐標，并記作

步驟803、計算機(9)根據公式得到用戶當前位置與第l個商鋪場景全景影像數據庫的商鋪場景定位坐標之間的距離d_sp,l；計算機(9)根據公式得到用戶當前位置與第h個過道場景全景影像數據庫的過道場景定位坐標之間的距離d_gd,h；

步驟804、多次重復步驟803，得到用戶當前位置與多個商鋪場景全景影像數據庫的商鋪場景定位坐標之間的距離和用戶當前位置與多個場景全景影像數據庫的過道場景定位坐標之間的距離，并將用戶當前位置與多個場景影像數據庫的商鋪場景定位坐標之間的距離和用戶當前位置與多個場景全景影像數據庫的過道場景定位坐標之間的距離按照從小到大的順序排序，得到最小距離和次小距離，并從步驟604中場景定位坐標中得到最小距離所對應的場景定位坐標并記作P_zg,1(X_zg,1,Y_zg,1,Z_zg,1)和次小距離所對應的場景定位坐標并記作P_zg,2(X_zg,2,Y_zg,2,Z_zg,2)；

步驟805、計算機(9)根據公式進行判斷，當成立時，執行步驟806；否則，執行步驟807；

步驟806、當成立時，則次小距離所對應的場景定位坐標作為下一個過渡坐標，且下一個過渡坐標所對應的影像作為下一個過渡影像，計算機(9)從場景影像數據庫中得到下一個過渡影像，并通過第二無線通信模塊(7)發送至手機(8)；

步驟807、當不成立時，則最小距離所對應的場景定位坐標作為下一個過渡坐標，且下一個過渡坐標所對應的影像作為下一個過渡影像，計算機(9)從場景影像數據庫中得到下一個過渡影像，并通過第二無線通信模塊(7)發送至手機(8)；

步驟808、用戶根據計算機(9)發送的下一個過渡影像，抵達下一個過渡坐標，重復步驟七、步驟803至步驟805，得到下下一個過渡坐標和下下一個過渡影像，直至抵達目標商鋪。