CN111121825B - 一种行人惯性导航系统中初始导航状态的确定方法及装置 - Google Patents

Abstract

本发明提供一种行人惯性导航系统中初始导航状态的确定方法及装置，包括将包含朝向的图像标志码安装固定在惯性传感器上，并将惯性传感器安装在行人足部，提前将包含位置和航向角的图像标志码安装在导航起始点附近地面上；初始化静止时段内，利用相机拍摄同时包含惯性传感器上和地面上的图像标志码的相片；根据地面上图像标志码的位置和航向，计算惯性传感器上图像标志码中选取点的实际导航坐标，确定惯性传感器的航向角和几何中心的位置；最后根据静止时段内的比力数据计算惯性传感器的初始横滚角和俯仰角，为行人惯性导航系统提供稳定可靠的初始导航状态。本发明实现了稳定可靠、不受干扰、极低成本、操作简单的行人惯性导航初始导航状态的确定。

Description

一种行人惯性导航系统中初始导航状态的确定方法及装置

技术领域

本发明涉及室内定位领域，具体涉及一种行人惯性导航系统中初始导航状态的确定方法及装置。

背景技术

全球卫星导航系统在开阔的室外环境中可提供可靠的定位服务；然而在城市峡谷、建筑物内部、地下矿井等室内环境中，由于信号受到建筑物或地面的遮挡，卫星导航无法提供定位服务。尽管WiFi、蓝牙、Beacon等技术可提供室内定位服务，然而这些技术均需要布设基准站，一方面基准站的布设和维护需要大量的人力、物力和时间成本，另一方面这些技术在消防救援、地下矿井等场景中将失去定位能力。行人惯性导航系统(PedestrianInertial Navigation System，PINS)是将重量轻的低成本的基于微机电系统(MicroElectroMechanical System，MEMS)的惯性测量单元(Inertial Measurement Unit，IMU)安装在行人的足部，在行走过程利用足部接触地面时速度为零的信息不断修正PINS的误差累积，从而推算行人的位置。PINS不依赖任何外部信息，具有完全的独立自主性，结合稀疏的控制点的情况下，可在长距离大区域的环境中提供可靠的导航定位服务。相较于其他室内定位技术，PINS在消防救援、应急抢险、大区域室内位置服务等应用领域中具有极大的应用前景。

目前PINS导航定位性能主要由两部分决定：初始导航状态的确定和零速更新算法。目前国内外大量的研究人员对零速更新算法进行了研究，主要集中在步态周期内零速状态的检测和优化误差估计算法，提升零速探测的可靠性，降低PINS中误差漂移的速度。在初始导航状态确定中，初始的速度为零，初始的位置可通过将起点作为原点或外部位置注入的形式获取，初始的横滚角和俯仰角可通过静止惯性传感器一段时间计算得到；然而由于MEMS陀螺仪的零偏不稳定性超过地球自转角速度，故无法通过静止传感器的方式获取初始航向角。尽管磁力计可用于确定初始的航向角，但PINS中惯性传感器被放置于行人足部，磁力计会受到地面下铁质建筑材料的干扰，导致难以获得稳定可靠的初始航向角。因此，可靠的初始导航状态(特别是航向角)确定方法将极大提升PINS的实时导航定位能力，拓宽PINS的应用领域。

发明内容

本发明旨在解决现有行人惯性导航系统中初始导航状态的确定中存在的上述技术问题。

为此，本发明的目的在于提出一种稳定可靠的行人惯性导航系统中初始导航状态的确定方法，从而满足行人定位中实时导航定位的需求。

为了实现上述目的，本发明提供了一种行人惯性导航系统中初始导航状态的确定方法，包括以下步骤：

步骤S1，将包含朝向信息的图像标志码安装固定在惯性传感器上，并将该惯性传感器安装在行人足部，且提前将包含位置和航向角信息的图像标志码安装在导航起始点附近的地面上；

步骤S2，行人实时导航初始化阶段，当行人站在起始点静止站立一段时间，在该静止时段内用相机拍摄同时包含惯性传感器上图像标志码和地面上图像标志码的相片；

步骤S3，利用相机所拍摄的相片和地面上图像标志码中所含的位置信息，在惯性传感器上图像标志码中选取至少两个特征点，并计算所选取点在导航定位中的实际坐标；

步骤S4，利用惯性传感器上图像标志码中所选取点的实际坐标计算惯性传感器的航向角和几何中心的位置；

步骤S5，利用静止时段内比力数据计算惯性传感器的初始横滚角和俯仰角，将计算得到的初始位置和初始姿态角作为PINS的初始导航状态，为PINS实时导航定位提供稳定可靠的初始导航状态。

而且，步骤S1中所述的图像标志码是指具有特定结构且能被计算机识别、区分并获取内部附加信息的图形或物体，包括但不限于：带编码信息的二维码、带编码的广告和海报、字画，带编码信息的物体。

而且，步骤S1的实现方式包括以下子步骤，

步骤S11，将包含朝向信息的图像标志码安装固定在惯性传感器外壳上时，提前标定图像标志码与惯性传感器航向角之间的夹角；

步骤S12，将惯性传感器安装在行人足部时，使相机能够拍摄到被固定惯性传感器上的图像标志码；

步骤S13，固定在地面上图像标志码的位置信息为真实世界中全局坐标系或局部坐标系下的坐标。

而且，步骤S2中，所述的相机是指具有拍摄和保存照片的设备，包括但不限于：手机、平板电脑、工业相机、数码相机和单反设备，用相机拍摄相片时惯性传感器上的图像标志码和固定在地面上的图像标志码同时出现在所拍摄的单张相片内。

而且，步骤S3中利用相机的相片和地面上图像标志码的位置计算惯性传感器上图像标志码所选取至少两个点的实际位置时包括以下子步骤，

首先定义以下坐标系，

导航坐标系是指运动目标跟踪和解算的参考坐标系，记为n系；相机坐标系是指以相机镜头中心所构成的右手直角坐标系，记为c系；像素坐标系是指二维码横轴和纵轴构成的右手直角坐标系，记为p系；

步骤S31，将拍摄的相片转为灰度图像，对灰度图像进行去畸变处理，获得校正之后的灰度图像，然后识别被安装于惯性传感器外壳上和被固定在地面上的图像标志码，以像素为单位获取灰度图片中被固定在地面上图像标志码中至少四个特征点的像素坐标，以像素为单位获取灰度图片中被安装在惯性传感器上图像标志码中方向轴线方向至少两个特征点的像素坐标；

步骤S32，通过相机的内参数矩阵将两个图像标志码中各自选取的特征点的像素坐标转换至相机坐标系中；

步骤S33，通过被固定在地面上图像标志码的n系中的真实坐标和航向角计算图像标志码所选取特征点的导航实际坐标，然后结合相机坐标系中被固定在地面上图像标志码四个角特征点的坐标，计算相机坐标系转换到导航坐标系的旋转矩阵和平移向量；

步骤S34，利用计算得到的相机坐标系转换到导航坐标系的旋转矩阵和平移向量，将固定在惯性传感器上的图像标志码中所选取点在相机坐标系中的坐标转换至导航坐标系中，求得惯性传感器上图像标志码中所选取点的实际导航坐标。

而且，步骤S4的实现包括以下子步骤，

步骤S41，利用惯性传感器上图像标志码中所选取点的实际坐标计算图像标志码的航向角和几何中心的位置；

步骤S42，利用提前标定的图像标志码与惯性传感器航向角之间的夹角，结合所计算的图像标志码的航向角和几何中心的位置，计算惯性传感器的航向角和几何中心的位置。

本发明还提供一种行人惯性导航系统中初始导航状态的确定装置，用于执行如上所述的行人惯性导航系统中初始导航状态的确定方法。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明所提供的方法中，将包含朝向信息的图像标志码安装固定在惯性传感器上，并将该传感器安装在行人足部，且提前将包含位置和航向角信息的图像标志码安装在导航起始点附近的地面上；行人实时导航初始化阶段，行人站在起始点静止站立一段时间，且在该静止时段内用相机拍摄同时包含惯性传感器上图像标志码和地面上图像标志码的相片；然后利用所拍摄的相片和地面上图像标志码中所含的位置信息，在惯性传感器上图像标志码中选取至少两个特征点，计算所选取点在导航定位中的实际坐标；之后利用惯性传感器上图像标志码中所选取点的实际坐标计算惯性传感器的航向角和几何中心的位置；最后根据静止时段内比力数据的平均值计算惯性传感器的初始横滚角和俯仰角，将计算得到的初始位置和初始姿态角作为PINS的初始导航状态，为PINS实时导航定位提供稳定可靠的初始导航状态。本发明能很好地解决现有行人惯性导航系统初始导航状态确定中存在的航向角难以可靠确定的技术问题，具有安装简单、无需维护、不受环境干扰等特点。

附图说明

图1是本发明实施例中一种行人惯性导航系统中初始导航状态的确定方法的总体流程图。

图2是本发明实施例中一种行人惯性导航系统中初始导航状态的确定方法中两个图像标志码示意图。

图3是本发明实施例中一种行人惯性导航系统中初始导航状态的确定方法的坐标系示意图。

图4是本发明实施例中一种行人惯性导航系统中初始导航状态的确定方法中地面上四个角特征点坐标计算的示意图。

图5是本发明实施例中一种行人惯性导航系统中初始导航状态的确定方法中惯性传感器的位置和航向角计算的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面结合优选实施例和附图对本发明的技术方案做进一步的详细说明。下面描述中的附图仅仅是本发明实施例中的一些特定实施方式，来表示实施本发明实施例的原理的方式。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，可以根据这些描述获得其他的实施方式，不应以此限制本发明的保护范围。

如图1所示，本发明的一个实施例提供了一种行人惯性导航系统中初始导航状态的确定方法，包括以下步骤：

步骤S1，将包含朝向信息的图像标志码安装固定在惯性传感器上，并将该传感器安装在行人足部，且提前将包含位置和航向角信息的图像标志码安装在导航起始点附近的地面上。

具体地，所述步骤S1中的图像标志码是指具有特定结构且能被计算机识别、区分并获取内部附加信息的特殊图形或特殊物体，包括但不限于：带编码信息的二维码、特殊编码的广告和海报、特殊的字画、带编码信息的物体等，且步骤S1具体实现过程包括：

步骤S11，将包含朝向信息的图像标志码安装固定在惯性传感器外壳上时，需提前标定图像标志码与惯性传感器航向角之间的夹角。

具体来说，被固定在惯性传感器上的图像标志码需包含明确的朝向信息，即能通过视觉的方式直接识别图像标志码的朝向标记，包括但限于：箭头指示、轴向方块等。在本实施例中，以具有特殊编码的黑白二维码作为图像标志码，二维码中白色方块可用于确定二维码的朝向，所述二维码如图2所示。

为了确定惯性传感器的航向角信息，需要提前标定图像标志码与惯性传感器航向角之间的夹角，从而将图像标志码与惯性传感器的水平轴对齐，标定后的夹角将保持固定不变直到更换图像标志码。在具体安装过程中，安装的形式不作具体要求，但是需要确保图像标志码和惯性传感器是紧密固连在一起的，既可以在惯性传感器的生产过程中将带方向信息的图像标志码永久雕刻在惯性传感器外壳上，也可以将图像标志码粘贴在惯性传感器外壳上，在本实施例中，将图像标志码直接粘贴在惯性传感器外壳上。

步骤S12，将惯性传感器安装在行人足部时，需要满足相机能够拍摄到被固定惯性传感器上的图像标志码。

具体来说，将带图像标志码的惯性传感器安装在行人足部时，需要满足在初始静止时段内相机能够拍摄和识别到图像标志码，具体实施过程中该惯性传感器可以安装在行人鞋子的前鞋面处、鞋背处或者侧面处；此外，惯性传感器安装在行人足部的过程中需要保证惯性传感器和行人足部是固连在一起的，避免出现安装不牢固、安装松弛等现象。

步骤S13，固定在地面上图像标志码的位置信息为真实世界中全局坐标系或局部坐标系下的坐标。

具体来说，提前在起始点附件的地面上安装包含位置和航向角的图像标志码时，可将该图像标志码可以作为一个精确的地面控制点，并将图像标志码附着在纸张、玻璃板或金属板的形式固定在地面上；该图像标志码中的位置和航向角信息可直接编码到图像标志码中，通过计算机进行识别和读取，在本实施例中，图像标志码几何中心的位置和航向角被编码成特殊的二维码，如图2所示；图像标志码中的位置为实际导航定位所需坐标系中的坐标，该坐标系可以为全局坐标系或局部坐标系，具体的坐标系定义将在之后的步骤S3中进行统一定义和说明。

步骤S2，行人实时导航初始化阶段，行人站在起始点静止站立一段时间，且在该静止时段内用相机拍摄同时包含惯性传感器上图像标志码和地面上图像标志码的相片。

具体地，步骤S12和步骤S2中所述的相机是指具有拍摄和保存照片的设备，包括但不限于：手机、平板电脑、工业相机、数码相机、单反等设备，而且在步骤S2中用相机拍摄相片时需满足惯性传感器上的图像标志码和固定在地面上的图像标志码同时出现在所拍摄的单张相片内。在本实施例中，利用智能手机作为相机设备。

具体来说，在行人导航初始化阶段之前，惯性传感器需开始正常运行，在行人导航初始化阶段，行人需要站立在导航定位的起始点，并保持行人足部静止，即保证惯性传感器静止；在该静止时段内，惯性传感器将正常采集惯性数据(包括三轴的角速度信息和三轴的比力数据)；同时在该静止时段内，相机需拍摄同时包含至少一张惯性传感器上的图像标志码和地面上提前安装的图像标志码的相片，在本实施例中，相片的拍摄过程可通过安卓手机程序方式自动实现。

步骤S3，利用相机所拍摄的相片和地面上图像标志码中所含的位置信息，在惯性传感器上图像标志码中选取至少两个特征点，并计算所选取点在导航定位中的实际坐标。

具体地，所述步骤S3中利用相机拍摄的相片和地面上图像标志码的位置计算惯性传感器上图像标志码所选取至少两个点的实际位置时包括以下子步骤，

首先定义以下坐标系：导航坐标系是指运动目标跟踪和解算的参考坐标系，该参考系通常为地心地固坐标系或者当地地理坐标系(北东地坐标系或东北天坐标系)，记为n系；相机坐标系是指以相机镜头中心所构成的右手直角坐标系(前右下坐标系或者右前上坐标系)，记为c系；像素坐标系是指二维码横轴和纵轴构成的右手直角坐标系，记为p系，其中假设像素坐标系中两个图像标志码的深度信息为0(即两个图像标志码在实际导航坐标系中的高度相同)。

具体来说，导航坐标系定义中的地心地固坐标系是指以地球质心为坐标原点，Z轴为地球自转轴指向北极，X轴指向赤道与零度子午线的交点，Y轴与X、Z轴构成右手直角坐标系；导航坐标系中定义的当地地理坐标系是指以运动载体中心作为坐标原点，Z轴沿地球椭球的法线方向指向下，X轴沿经度方向指向北，Y轴沿纬度方向并指向东所构成的右手直角坐标系(或者X轴沿纬度方向并指向东，Y轴沿经度方向指向北，Z轴沿地球椭球的法线方向指向上)；本实施例中以当地地理坐标系作为导航坐标系；相机坐标系是指以相机镜头中心作为坐标原点，以相机的前向作为X轴，以相机的右向作为Y轴，Z轴与X、Y轴所形成的右手直角坐标系(或者以相机的右向为X轴，相机的前上为Y轴)，图3给出了本实施例中坐标系的示意图。

步骤S31，将拍摄的相片转为灰度图像，对灰度图像进行去畸变处理，获得校正之后的灰度图像，然后识别被安装于惯性传感器外壳上和被固定在地面上的图像标志码，以像素为单位获取灰度图片中被固定在地面上图像标志码中至少四个特征点(通常包括四个角特征点)的像素坐标，以像素为单位获取灰度图片中被安装在惯性传感器上图像标志码中方向轴线方向至少两个特征点的像素坐标。

具体来说，利用地面上图像标志码中至少四个特征点的像素坐标和实际的导航坐标可以计算像素坐标系与导航坐标系的转换关系；在计算惯性传感器的航向角时，至少需要两个朝向轴线上的特征点；在灰度图像中，惯性传感器上图像标志码的朝向信息用于将所选取点的像素和实际中的位置一一对应起来。在本实施例中，地面上图像标志码选取四个角特征点作为特征点，如图2中所示的M1、M2、M3和M4，尺寸为3cm×3cm，惯性传感器上图像标志码也选取四个角特征点作为特征点，如图2中所示的P1、P2、P3和P4，尺寸为10cm×10cm。

步骤S32，通过相机的内参数矩阵(通常相机的内参数矩阵固定不变)将两个图像标志码中各自选取的特征点的像素坐标转换至相机坐标系中。

具体来说，相机的内参数矩阵是指由相机焦距和相片相对于成像平面的主点坐标所构成的矩阵。定义相片中一个点的像素坐标为(x^p,y^p)，该点的在相机坐标系中的坐标为(x^c,y^c,z^c)，那么该点的像素坐标和相机坐标之间的关系可以表示为：

其中，f_x和f_y为相机在X轴和Y轴上的焦距，(u_x,u_y)为相机相对于成像平面的主点坐标，K为相机的内参数矩阵。通过公式(1)可以将某一点的坐标从像素坐标转换至相机坐标系中。

步骤S33，通过被固定在地面上图像标志码的真实坐标(n系中的坐标)和航向角计算图像标志码所选取特征点的导航实际坐标，然后结合相机坐标系中被固定在地面上图像标志码四个角特征点的坐标，计算相机坐标系转换到导航坐标系的旋转矩阵和平移向量。

具体来说，选取被固定在地面上图像标志码的四个角特征点，如图4所示。定义被固定在地面上图像标志码的中心坐标为

图像标志码的航向角为ψ₁，由于航向角为0度下四个角特征点逆时针旋转了ψ度得到了航向角为ψ₁下的四个角特征点，那么第一个角特征点在导航坐标系中的坐标为：

第二个角特征点在导航坐标系中的坐标为：

第三个角特征点在导航坐标系中的坐标为：

第四个角特征点在导航坐标系中的坐标为：

其中，L为图像标志码的实际长度，W为图像标志码的实际宽度。通过公式(2)～(5)则可计算图像标志码四个角特征点在导航坐标系下的坐标。

利用地面上图像标志码中四个角特征点在相机坐标系中的坐标和在导航坐标系中的坐标，计算相机坐标系到导航坐标系的变换矩阵和平移向量，定义地面上图像标志码中四个角特征点在相机坐标系中的坐标为

那么相机坐标系转换到导航坐标系中的关系为：

其中，

为相机坐标系转换到导航坐标系的3×3的旋转矩阵，

为相机坐标系转换到导航坐标系的3×1的平移向量。

步骤S34，利用计算得到的相机坐标系转换到导航坐标系的旋转矩阵和平移向量，将固定在惯性传感器上的图像标志码中所选取点在相机坐标系中的坐标转换至导航坐标系中，求得惯性传感器上图像标志码中所选取点的实际导航坐标。

具体来说，利用步骤S33中计算得到的平移向量和旋转矩阵将惯性传感器上图像标志码中所选取的特征点的坐标从相机坐标系转换至导航坐标系中，定义惯性传感器上图像标志码所选取的特征点在相机坐标系中的坐标为

其在导航坐标系中的坐标

为：

其中，

和

分别为利用地面上图像标志码所选取点的实际导航坐标与相机坐标一起估算得到的旋转矩阵和平移向量。

步骤S4，利用惯性传感器上图像标志码中所选取点的实际坐标计算惯性传感器的航向角和几何中心的位置。

具体地，步骤S4的实现包括以下子步骤，

步骤S41，利用惯性传感器上图像标志码中所选取点的实际坐标计算图像标志码的航向角和几何中心的位置。

具体来说，如图5所示，通过步骤S3所计算的惯性传感器上图像标志码中四个角特征点的实际坐标为

那么惯性传感器上图像标志码几何中心的位置

为：

惯性传感器上图像标志码的航向角ψ₂为：

其中，W为惯性传感器上图像标志码的宽度。

步骤S42，利用提前标定的图像标志码与惯性传感器航向角之间的夹角，结合所计算的图像标志码的航向角和几何中心的位置，计算惯性传感器的航向角和几何中心的位置。

具体来说，提前标定的图像标志码与惯性传感器航向角之间的夹角被定义为δψ，那么惯性传感器的航向角ψ为：

ψ＝ψ₂-δψ (10)

由于行人惯性导航系统中惯性传感器的厚度很小，不会影响实际导航定位的精度和性能，可以将惯性传感器上图像标志码的几何中心直接作为惯性传感器的初始位置。

步骤S5，利用静止时段内比力数据计算惯性传感器的初始横滚角和俯仰角，将计算得到的初始位置和初始姿态角作为PINS的初始导航状态，为PINS实时导航定位提供稳定可靠的初始导航状态信息。

具体来说，利用静止时段内的比力数据的平均值计算惯性传感器的初始横滚角和俯仰角时，定义静止时段内一共采集了N个历元的比力数据f_k＝(f_k,x,f_k,y,f_k,z),k＝1,2,…,N，那么惯性传感器的横滚角φ和俯仰角θ为：

其中，

和

分别为静止时段内比力向量在三轴上的平均值。

在初始化阶段中，惯性传感器的初始速度为零，将计算所得到的初始位置和初始姿态角(横滚角、俯仰角和航向角)作为行人惯性导航系统的初始导航状态。

具体实施时，可采用软件技术实现以上流程的自动运行。运行相应方法的相应装置也应当在本发明的保护范围内。

本发明实施例的行人惯性导航系统中初始航向角的确定方法中，将包含朝向信息的图像标志码安装固定在惯性传感器上，并将该传感器安装在行人足部，且提前将包含位置和航向角信息的图像标志码安装在导航起始点附近的地面上；行人实时导航初始化阶段，行人站在起始点静止站立一段时间，且在该静止时段内用相机拍摄同时包含惯性传感器上图像标志码和地面上图像标志码的相片；然后利用所拍摄的相片和地面上图像标志码中所含的位置信息，在惯性传感器上图像标志码中选取至少两个特征点，计算所选取点在导航定位中的实际坐标；之后利用惯性传感器上图像标志码中所选取点的实际坐标计算惯性传感器的航向角和几何中心的位置；最后根据静止时段内比力数据的平均值计算惯性传感器的初始横滚角和俯仰角，将计算得到的初始位置和初始姿态角作为PINS的初始导航状态，为PINS实时导航定位提供稳定可靠的初始导航状态信息。在本发明实施例中，利用图像标志码辅助PINS获取惯性传感器的初始导航状态，不需要后期的维护，操作简单方便，不受环境等因素的干扰。解决了现有行人惯性导航中初始导航状态(特别是航向角)难以稳定可靠确定的技术问题。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

显然，本发明的上述实施例只是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于本领域的普通技术人员来说，在上述说明的基础上还可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (7)

1.一种行人惯性导航系统中初始导航状态的确定方法，其特征在于，包括以下步骤：

步骤S1，将包含朝向信息的图像标志码安装固定在惯性传感器上，并将该惯性传感器安装在行人足部，且提前将包含位置和航向角信息的图像标志码安装在导航起始点附近的地面上；

步骤S2，行人实时导航初始化阶段，当行人站在起始点静止站立一段时间，在该静止时段内用相机拍摄同时包含惯性传感器上图像标志码和地面上图像标志码的相片；

步骤S3，利用相机所拍摄的相片和地面上图像标志码中所含的位置信息，在惯性传感器上图像标志码中选取至少两个特征点，并计算所选取点在导航定位中的实际坐标；

实现方式包括利用地面上图像标志码中四个角特征点在相机坐标系中的坐标和在导航坐标系中的坐标，计算相机坐标系到导航坐标系的变换矩阵和平移向量，定义地面上图像标志码中四个角特征点在相机坐标系中的坐标为

那么相机坐标系转换到导航坐标系中的关系为：

其中，

为相机坐标系转换到导航坐标系的3×3的旋转矩阵，

为相机坐标系转换到导航坐标系的3×1的平移向量；

利用计算得到的平移向量和旋转矩阵将惯性传感器上图像标志码中所选取的特征点的坐标从相机坐标系转换至导航坐标系中，定义惯性传感器上图像标志码所选取的特征点在相机坐标系中的坐标为

其在导航坐标系中的坐标

为：

其中，

和

分别为利用地面上图像标志码所选取点的实际导航坐标与相机坐标一起估算得到的旋转矩阵和平移向量；步骤S4，利用惯性传感器上图像标志码中所选取点的实际坐标计算惯性传感器的航向角和几何中心的位置；

实现方式包括通过步骤S3所计算的惯性传感器上图像标志码中四个角特征点的实际坐标为

那么惯性传感器上图像标志码几何中心的位置

为：

惯性传感器上图像标志码的航向角ψ₂为：

其中，W为惯性传感器上图像标志码的宽度；

提前标定的图像标志码与惯性传感器航向角之间的夹角被定义为δψ，那么惯性传感器的航向角ψ为：

ψ＝ψ₂-δψ

由于行人惯性导航系统中惯性传感器的厚度很小，不会影响实际导航定位的精度和性能，将惯性传感器上图像标志码的几何中心直接作为惯性传感器的初始位置；

步骤S5，利用静止时段内比力数据计算惯性传感器的初始横滚角和俯仰角，将计算得到的初始位置和初始姿态角作为行人惯性导航系统的初始导航状态，为行人惯性导航系统实时导航定位提供稳定可靠的初始导航状态，实现方式如下，

用静止时段内的比力数据的平均值计算惯性传感器的初始横滚角和俯仰角时，定义静止时段内一共采集了N个历元的比力数据f_k＝(f_k,x,f_k,y,f_k,z),k＝1,2,…,N，那么惯性传感器的横滚角φ和俯仰角θ为：

其中，f_x、f_y和f_z分别为静止时段内比力向量在三轴上的平均值；

在初始化阶段中，惯性传感器的初始速度为零，将计算所得到的初始位置和初始姿态角作为行人惯性导航系统的初始导航状态。

2.如权利要求1所述的行人惯性导航系统中初始导航状态的确定方法，其特征在于：步骤S1中所述的图像标志码是指具有特定结构且能被计算机识别、区分并获取内部附加信息的图形或物体，包括带编码信息的二维码、带编码的广告和海报、字画。

3.如权利要求1所述的行人惯性导航系统中初始导航状态的确定方法，其特征在于：步骤S1的实现方式包括以下子步骤，

步骤S11，将包含朝向信息的图像标志码安装固定在惯性传感器外壳上时，提前标定图像标志码与惯性传感器航向角之间的夹角；

步骤S12，将惯性传感器安装在行人足部时，使相机能够拍摄到被固定惯性传感器上的图像标志码；

步骤S13，固定在地面上图像标志码的位置信息为真实世界中全局坐标系或局部坐标系下的坐标。

4.如权利要求1所述的行人惯性导航系统中初始导航状态的确定方法，其特征在于：步骤S2中，所述的相机是指具有拍摄和保存照片的设备，包括手机、平板电脑、工业相机、数码相机和单反设备，用相机拍摄相片时惯性传感器上的图像标志码和固定在地面上的图像标志码同时出现在所拍摄的单张相片内。

5.如权利要求1所述的行人惯性导航系统中初始导航状态的确定方法，其特征在于：步骤S3中利用相机的相片和地面上图像标志码的位置计算惯性传感器上图像标志码所选取至少两个点的实际位置时包括以下子步骤，

首先定义以下坐标系，

导航坐标系是指运动目标跟踪和解算的参考坐标系，记为n系；相机坐标系是指以相机镜头中心所构成的右手直角坐标系，记为c系；像素坐标系是指二维码横轴和纵轴构成的右手直角坐标系，记为p系；

步骤S31，将拍摄的相片转为灰度图像，对灰度图像进行去畸变处理，获得校正之后的灰度图像，然后识别被安装于惯性传感器外壳上和被固定在地面上的图像标志码，以像素为单位获取灰度图片中被固定在地面上图像标志码中至少四个特征点的像素坐标，以像素为单位获取灰度图片中被安装在惯性传感器上图像标志码中方向轴线方向至少两个特征点的像素坐标；

步骤S32，通过相机的内参数矩阵将两个图像标志码中各自选取的特征点的像素坐标转换至相机坐标系中；

步骤S33，通过被固定在地面上图像标志码的n系中的真实坐标和航向角计算图像标志码所选取特征点的导航实际坐标，然后结合相机坐标系中被固定在地面上图像标志码四个角特征点的坐标，计算相机坐标系转换到导航坐标系的旋转矩阵和平移向量；

步骤S34，利用计算得到的相机坐标系转换到导航坐标系的旋转矩阵和平移向量，将固定在惯性传感器上的图像标志码中所选取点在相机坐标系中的坐标转换至导航坐标系中，求得惯性传感器上图像标志码中所选取点的实际导航坐标。

6.如权利要求1所述的行人惯性导航系统中初始导航状态的确定方法，其特征在于：步骤S4的实现包括以下子步骤，

步骤S41，利用惯性传感器上图像标志码中所选取点的实际坐标计算图像标志码的航向角和几何中心的位置；

步骤S42，利用提前标定的图像标志码与惯性传感器航向角之间的夹角，结合所计算的图像标志码的航向角和几何中心的位置，计算惯性传感器的航向角和几何中心的位置。

7.一种行人惯性导航系统中初始导航状态的确定装置，其特征在于：用于执行如权利要求1至6任一项所述的行人惯性导航系统中初始导航状态的确定方法。

Images