[发明专利]一种基于影像空间和全景辅助的室内定位导航方法

摘要

本发明公开了一种基于影像空间和全景辅助的室内定位导航方法，包括以下步骤：一、控制点的布设及世界坐标系的建立；二、摄像机的校正及室内序列影像的获取；三、特征点提取与匹配；四、特征点的剔除；五、特征点空间坐标的计算；六、室内全景影像的获取；七、室内定位；八、室内导航。本发明步骤简单、设计合理且定位导航准确，使用效果好，减少辅助设备安装，成本低，提高定位精度，实用性强。

主权项

1.一种基于影像空间和全景辅助的室内定位导航方法，其特征在于，该方法包括以下步骤：步骤一、控制点的布设及世界坐标系的建立：步骤101、在室内每个墙壁上设置至少两个控制点；步骤102、以室内任意一点为原点Os，以过原点Os且指向正北为Xs轴，以过原点Os且指向正东为Ys轴，以过原点Os且垂直于由Xs轴和Ys轴形成的平面OsXsYs指向天为Zs轴，建立世界坐标系OsXsYsZs；步骤103、采用全站仪测量多个所述控制点的坐标，则获得在世界坐标系OsXsYsZs下多个所述控制点的空间坐标，通过所述参数输入模块(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)根据判定条件Xgxs≤0.6进行判定，当Xgxs≤0.6成立，执行步骤405；否则，执行步骤406；步骤405、当Xgxs≤0.6成立，则为误匹配点对，计算机(9)将误匹配误匹配点对剔除；步骤406、当Xgxs>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个相机坐标系Oz(k+1)Xz(k+1)Yz(k+1)Zz(k+1)；其中，在摄像机(2)对室内拍摄序列影像的第一个位置处即第一副影像的拍摄处建立第一相机坐标系Oz1Xz1Yz1Zz1；步骤503、计算机(9)调取相机旋转矩阵和平移向量计算模块，从剔除后的多组匹配点对中选择四对匹配点输入，得到第k+1个相机坐标系到第k个相机坐标系的相机旋转矩阵Rk+1,k和平移向量tk+1,k；其中，相机旋转矩阵Rk+1,k为3×3矩阵，平移向量tk+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)的坐标，则获取在世界坐标系OsXsYsZs下鱼眼相机模块(4)对商铺场景进行全景拍摄时的坐标，并将在世界坐标系OsXsYsZs下鱼眼相机模块(4)对商铺场景进行全景拍摄时的坐标称作商铺场景定位坐标，通过所述参数输入模块(17)输入商铺场景定位坐标和商铺名称，并存储至存储器(11)中，计算机(9)调取全景影像拼接模块，并输入多幅商铺场景拍摄影像进行拼接，形成商铺场景全景影像，得到一个商铺场景全景影像数据库；步骤603、采用鱼眼相机模块(4)对每个过道场景进行全景拍摄，并将拍摄的多幅过道场景拍摄影像发送至第二控制器(5)，第二控制器(5)将接收到的多幅过道场景拍摄影像通过第三无线通信模块(6)和第二无线通信模块(7)发送至计算机(9)；并采用全站仪测量鱼眼相机模块(4)的坐标，则获取在世界坐标系OsXsYsZs下鱼眼相机模块(4)对过道场景进行全景拍摄时的坐标，并将在世界坐标系OsXsYsZs下鱼眼相机模块(4)对过道场景进行全景拍摄时的坐标称作过道场景定位坐标，通过所述参数输入模块(17)输入过道场景定位坐标，并存储至存储器(11)中；计算机(9)调取全景影像拼接模块，并输入多幅过道场景拍摄影像进行拼接，形成过道场景全景影像，得到一个过道场景全景影像数据库；其中，商铺场景拍摄影像和过道场景拍摄影像的旁向重叠度均不低于30％；步骤604、多次重复步骤602至步骤603，得到多个商铺场景全景影像数据库和多个过道场景全景影像数据库，并将多个商铺场景全景影像数据库和多个过道场景全景影像数据库按照拍摄先后顺序进行存储，则将多个商铺场景全景影像数据库和多个过道场景全景影像数据库形成的数据库称作场景影像数据库，将商铺场景定位坐标和过道场景定位坐标均称作场景定位坐标；步骤七、室内定位：步骤701、采用手机(8)采集用户当前所在位置的一幅定位影像并通过第二无线通信模块(7)发送至计算机(9)；步骤702、计算机(9)按照步骤301所述的方法对定位影像进行特征点提取，获取定位影像中多个特征点描述子归一化向量；步骤703、计算机(9)按照步骤302所述的方法将定位影像中多个特征点描述子归一化向量与步骤5015中形成的特征点数据库进行匹配，得到定位影像的匹配点对数Mdp；步骤704、计算机(9)根据判定条件Mdp≥8进行判断，当Mdp≥8成立，从步骤705开始执行；否则，从步骤701执行，重新拍一副定位影像；步骤705、当Mdp≥8成立，计算机(9)调取相机Exif信息读取模块读取定位影像中的Exif信息，得到手机(8)中相机拍摄时的相机焦距fd；步骤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)将Mdp对定位影像的匹配点对中位于步骤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个商铺场景全景影像数据库的商铺场景定位坐标记作Psp,l(Xsp,l,Ysp,l,Zsp,l)；计算机(9)将第h个过道场景全景影像数据库中的过道场景定位坐标记作Pgd,h(Xgd,h,Ygd,h,Zgd,h)；l为正整数，h为正整数；步骤802、用户通过参数输入模块(17)输入用户最终抵达的商铺名称作为目标商铺，且计算机(9)通过商铺场景全景影像数据库，得到目标商铺的商铺场景定位坐标，并记作步骤803、计算机(9)根据公式得到用户当前位置与第l个商铺场景全景影像数据库的商铺场景定位坐标之间的距离d_sp,l；计算机(9)根据公式得到用户当前位置与第h个过道场景全景影像数据库的过道场景定位坐标之间的距离d_gd,h；步骤804、多次重复步骤803，得到用户当前位置与多个商铺场景全景影像数据库的商铺场景定位坐标之间的距离和用户当前位置与多个场景全景影像数据库的过道场景定位坐标之间的距离，并将用户当前位置与多个场景影像数据库的商铺场景定位坐标之间的距离和用户当前位置与多个场景全景影像数据库的过道场景定位坐标之间的距离按照从小到大的顺序排序，得到最小距离和次小距离，并从步骤604中场景定位坐标中得到最小距离所对应的场景定位坐标并记作Pzg,1(Xzg,1,Yzg,1,Zzg,1)和次小距离所对应的场景定位坐标并记作Pzg,2(Xzg,2,Yzg,2,Zzg,2)；步骤805、计算机(9)根据公式进行判断，当成立时，执行步骤806；否则，执行步骤807；步骤806、当成立时，则次小距离所对应的场景定位坐标作为下一个过渡坐标，且下一个过渡坐标所对应的影像作为下一个过渡影像，计算机(9)从场景影像数据库中得到下一个过渡影像，并通过第二无线通信模块(7)发送至手机(8)；步骤807、当不成立时，则最小距离所对应的场景定位坐标作为下一个过渡坐标，且下一个过渡坐标所对应的影像作为下一个过渡影像，计算机(9)从场景影像数据库中得到下一个过渡影像，并通过第二无线通信模块(7)发送至手机(8)；步骤808、用户根据计算机(9)发送的下一个过渡影像，抵达下一个过渡坐标，重复步骤七、步骤803至步骤805，得到下下一个过渡坐标和下下一个过渡影像，直至抵达目标商铺。