一种室内外连续定位方法及装置与流程
本发明属于待监测载体融合定位领域，特别是以惯性导航为基础的室内外融合辅助定位方向。本发明涉及一种待监测载体定位方法，尤其涉及一种基于惯性导航与卫星，场景视觉技术相融合的室内外连续定位方法和装置。
背景技术：
为了顺应新世纪国防信息现代化的建设要求，提升部队综合作战能力，实现打赢未来现代化局部战争的要求，智能单兵战术定位系统成为了未来陆军作战的能力倍增器，可以满足单兵在户外以及城市多种环境下的普通作战和特种作战要求。
近些年来，随着我国北斗卫星导航定位技术产业的高速发展，通过对gnss高精度定位理论与方法的深入研究已经为智能化单兵定位提供了一种有效而可靠的实现途径。gnss具有全球性、全天候导航能力，误差长期稳定，接收机成本较低等优势，但由于卫星信号易受到地理环境干扰、遮挡等因素的影响，从而导致无法捕获卫星信号而丧失定位功能，且无法实现室内环境下的定位需求。并且gnss通常只能输出载体的位置坐标与速度信息，且输出带宽低。
微惯性导航定位技术具有自主性强、短时精度高、输出带宽高、硬件成本低、导航信息全等优势，但其也面临着导航误差随时间积累、长时间精度低的劣势。
考虑到在室内环境下gnss无法有效提供可靠的定位信息这一问题，而如何解决室内定位问题也成为了当下十分热门的方向。目前室内定位技术普遍是基于人的手机终端，采用蓝牙、wifi指纹、zigbee、超宽带uwb等单一定位方式。这些定位方法普遍存在定位精度不高或者单独定位时硬件布设量大等问题，不适合快速部署使用。
场景视觉分析技术是基于摄影测量原理，利用拍摄到的场景照片进行定位。场景视觉分析技术定位精度高、稳定性好。但是，由于视觉场景分析技术需要场景中包含明显的易于配准的特征，但是在单独利用场景分析进行定位的过程中，若需要进行室内、室外环境切换，则可能遇到场景环境中不包含明显特征点的情况，会出现无法定位的盲区，无法实现室内连续定位及室内外连续定位，导致定位的失败。
因此，现有的定位技术无法满足现有的室内连续定位及室内外连续定位的要求。
技术实现要素：
本发明针对室内环境下gnss无法有效提供可靠的定位信息及无法实现室内连续定位的问题，提供一种室内外连续定位方法及装置。
为解决上述技术问题，本发明采用的技术方案是：一种室内外连续定位方法，待监测载体上设置有用于采集惯性测量数据的惯性测量单元和用于采集场景的视觉图像的视觉图像采集单元；所述待监测载体位于al点时，所述视觉图像采集单元采集第l幅视觉图像image(al)，l的初始值为1；所述惯性测量数据用于解算al点的位置坐标；待监测载体的位置坐标、惯性测量单元的位置坐标、视觉图像采集单元的位置坐标相同或具有固定的相互关系；所述室内外连续定位方法包括视觉图像与惯性导航融合定位方法；所述视觉图像与惯性导航融合定位方法包括如下步骤：
(a-1)采集视觉图像image(al)、视觉图像image(al+1)，并确定视觉图像image(al)和视觉图像image(al+1)的同名像点集sl,l+1；
(a-2)利用同名像点集sl,l+1在视觉图像image(al)中的像素坐标、同名像点集sl,l+1在视觉图像image(al+1)中的像素坐标、al点的位置坐标、al+1点的位置坐标计算并保存同名像点集sl,l+1的位置坐标，优选al点的位置坐标、al+1点的位置坐标均由所述惯性测量数据解算得到；
(a-3)采集视觉图像image(al+2)，并确定视觉图像image(al+1)和视觉图像image(al+2)的同名像点集sl+1,l+2；
(a-4)利用下式确定sl,l+1与sl+1,l+2的共同同名像点集sl,l+2
sl,l+2＝sl,l+1∩sl+1,l+2，
其中sl,l+1为视觉图像image(al)和视觉图像image(al+1)的同名像点集；
(a-5)利用共同同名像点集sl,l+2的位置坐标、共同同名像点集sl,l+2在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标；
(a-7)利用同名像点集sl+1,l+2在视觉图像image(al+1)中的像素坐标、同名像点集sl+1,l+2在视觉图像image(al+2)中的像素坐标、al+1点的位置坐标、al+2点的位置坐标计算并保存同名像点集sl+1,l+2的位置坐标；
(a-8)利用卡尔曼滤波法方法，根据第一坐标与第二坐标计算修正后待监测载体(10)在al+2点的位置坐标，且令l＝l+1，跳转到步骤(a-3)，所述第一坐标是步骤(a-5)中计算得到的al+2点的位置坐标，所述第二坐标是由惯性测量数据解算的al+2点的位置坐标。
本发明中，在刚刚进行视觉图像采集时，需要借助惯性测量数据解算的位置坐标给出前两个点的位置坐标，然后依次进行第一个点和第二个点的同名像点集的位置坐标的计算、第三个点的位置坐标的计算、第二个点和第三个点的同名像点集的位置坐标的计算，然后根据卡尔曼滤波方法，利用惯性测量数据解算的第三个点的位置坐标和视觉图像解算得到的第三个点的位置坐标计算修正后第三个点的位置坐标，然后依次进行迭代运算。本发明实现了惯性测量数据与视觉图像测量的融合定位，通过视觉图像解算得到的定位结果对惯性测量数据进行修正，得到更为准确的修正后的定位结果，大大降低了单纯惯导测量中的累积误差。步骤(a-5)中计算得到的al+2点的位置坐标即为视觉图像解算的位置坐标。在步骤(a-5)中，由于
上述技术方案中，还包括将采集的视觉图像保存到场景特征库中；所述步骤(a-3)中，还包括执行第一判断，若第一判断的结果为否，则跳转到步骤(a-4)，若第一判断的结果为是，则执行如下步骤(a-6)以代替步骤(a-4)-(a-5)；所述第一判断为：判断在场景特征库中是否存在一幅视觉图像，使得该视觉图像与视觉图像image(al+1)和视觉图像image(al+2)有共同同名像点集b且该共同同名像点集b由至少m对所保存的同名像点构成，优选m≥3，更优选m≥5；
(a-6)利用共同同名像点集b的位置坐标、共同同名像点集b在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标，且跳转到步骤(a-7)。
本发明中，利用卡尔曼滤波方法，结合惯性测量数据解算的位置坐标和视觉图像解算的位置坐标计算修正后待监测载体在al+2点的位置坐标。由于惯性测量数据解算的位置坐标也会受到累积误差的影响，因此，随着时间的增加，惯性测量数据解算的位置坐标的误差也会逐渐变大，也会影响修正后待监测载体的位置坐标的计算。因此，时间较早时计算的数据的误差要小于时间较晚时计算的数据。因此，可以将将采集的视觉图像保存到场景特征库中，通过判断所保存的视觉图像与视觉图像image(al+1)和视觉图像image(al+2)是否有共同同名像点集，且该共同同名像点集是否包含至少3对所保存的同名像点，来确定是否再次进入之前曾经进入过的场景，若判断为是，则可利用所保存的同名像点集的位置坐标进行后续计算。这样，在判断再次进入之前曾经进入过的场景时，可以利用保存的时间较早的数据来计算，由此可以减少惯性测量数据造成的位置坐标的累积误差，提高计算精度，实现对位置坐标的修正。
上述技术方案中，所述步骤(a-3)中，若第一判断的结果为是，则执行步骤(a-6)以代替步骤(a-4)-(a-5)，且不执行第二判断，若第一判断的结果为否，则执行第二判断；所述第二判断为：判断视觉图像image(al+1)和视觉图像image(al+2)的同名像点集是否由不少于h对同名像点构成，优选h＝3，更优选h＝5；
若第一判断的结果为否且第二判断的结果为是，则跳转到步骤(a-4)；
若第一判断的结果为否且第二判断的结果为否，则利用惯性测量数据计算al+2点的位置坐标，且令l＝l+1，跳转到步骤(a-1)。
实际计算过程中，可能出现相邻两幅图像的特征点较少、特征点不显著或特征点无法匹配的情况，即同名像点集中同名像点的个数较少的情况。在步骤(a-3)中，若判断第一判断的结果为是，即判断再次进入之前曾经进入过的场景，则利用保存的数据进行位置计算。若判断第一判断的结果为否，即判断进入新的场景，则进行第二判断，即需要判断视觉图像image(al+1)和视觉图像image(al+2)的同名像点集是否由不少于h对同名像点构成。若第一判断的结果为否，即两幅视觉图像中说明无法通过视觉图像中的同名像点集来实现对位置坐标的解算，因此返回到步骤(a-1)，重新进行计算。
上述技术方案中，所述场景特征库中，任意相邻两幅视觉图像重合的面积不大于一幅图像面积的p％，优选p≤20。
在本发明的视觉测量过程中，所保存的各幅视觉图像可以用于后续的处理，例如用于已记录的同名像点坐标的提取。由于定位系统是携带于待监测载体或行进的物体上，存储空间有限，出于有效利用存储空间的需求，不需要保存每一幅采集的图像，可以间隔几幅图像，对采集到的图像进行保存。通过设置相邻两幅视觉图像重合的面积不大于一幅图像面积的p％，优选p≤20，可以存储较少的视觉图像，有效节省存储空间。使得场景库由尽可能少的图像构成。
上述技术方案中，所述步骤(a-1)包括如下子步骤：
(a-1-1)采集视觉图像image(al)、视觉图像image(al+1)，提取两幅视觉图像中的特征点；
(a-1-2)判断步骤(a-1-1)中的特征点是否具有显著性，优选利用robert算子判断所述特征点是否具有显著性；
(a-1-3)对两幅视觉图像中具有显著性的特征点进行配准，得到同名像点集。
上述技术方案中，所述步骤(a-1-1)中，利用moravec算子方法或harris算子方法或susan算子方法或forstner算子方法或sift方法或surf方法提取两幅视觉图像中的特征点。
上述技术方案中，所述步骤(a-9)中，选取状态量
上述技术方案中，待监测载体上设置有用于接收卫星测量数据的卫星接收模块，所述室内外连续定位方法包括：判断待监测载体是否位于室外，若判断待监测载体位于室外，则利用卫星与惯性导航融合定位方法计算待监测载体的位置坐标，否则，则利用所述视觉图像与惯性导航融合定位方法计算待监测载体的位置坐标；所述卫星与惯性导航融合定位方法包括：
利用惯性测量数据解算的待监测载体在导航坐标系下的位置坐标、速度、航向角作为卡尔曼滤波的状态量，利用卫星解算的待监测载体的位置坐标、速度和航向角作为卡尔曼滤波的观测量，利用卡尔曼滤波方法对待监测载体的位置坐标进行修正，其中，卫星解算的待监测载体的位置坐标、速度和航向角均由所述卫星测量数据解算得到。
本发明中，可首先判断待监测载体是否位于室外，若位于室外，则卫星信号较好，可利用卫星与惯性导航融合定位方法计算待监测载体的位置坐标。若判断待监测载体位于室内，则在卫星信号较差的室内环境，利用所述视觉图像与惯性导航融合定位方法计算待监测载体的位置坐标。本发明中，为了克服gnss定位存在的不足，将微惯性导航定位技术与gnss相结合。因此将两者相互结合可以克服两者各自的不足，提高户外和室内的整体定位精度和可靠性。本方案采用场景视觉分析技术与惯导定位手段的相融合，提高惯导定位的准确性和精度的同时避免单一场景视觉定位存在的问题。在室外场景中，可以根据室外实际环境，利用视觉检测与惯性测量结合或采用卫星定位与惯性测量结合。
上述技术方案中，若q_satt＞4且loc_rest＜threshold_res，则判断待监测载体位于室外，其中q_satt为有效卫星个数，loc_rest为卫星定位残差，threshold_res为卫星定位残差阈值，所述有效卫星个数、卫星定位残差均由所述卫星测量数据解算得到。
本发明中，如果有效卫星数q_satt大于4则认为当前有足够的卫星可提供有效定位信息；反之卫星定位数据无效；如果卫星定位残差小于threshold_res，则定位解算位置精度高；反之则精度低；依据上述卫星信号质量判断，实现室内与室外模式的切换，在室外采用卫星定位与惯性测量结果融合的定位模式；在室内采用视觉图像与惯性测量结果融合的定位模式。
本发明还提供一种实现上述所述的室内外连续定位方法的室内外连续定位装置，包括：惯性测量单元：用于采集惯性测量数据；
视觉图像采集单元：用于采集视觉图像image(al)、视觉图像image(al+1)、视觉图像image(al+2)；信号处理模块：用于确定视觉图像image(al)和视觉图像image(al+1)的同名像点集sl,l+1，利用同名像点集sl,l+1在视觉图像image(al)中的像素坐标、同名像点集sl,l+1在视觉图像image(al+1)中的像素坐标、al点的位置坐标、al+1点的位置坐标计算并保存同名像点集sl,l+1的位置坐标，确定视觉图像image(al+1)和视觉图像image(al+2)的同名像点集sl+1,l+2，确定sl,l+1与sl+1,l+2的共同同名像点集sl,l+2，利用共同同名像点集sl,l+2的位置坐标、共同同名像点集sl,l+2在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标，利用同名像点集sl+1,l+2在视觉图像image(al+1)中的像素坐标、同名像点集sl+1,l+2在视觉图像image(al+2)中的像素坐标、al+1点的位置坐标、al+2点的位置坐标计算并保存同名像点集sl+1,l+2的位置坐标，利用卡尔曼滤波法方法，根据第一坐标与第二坐标计算修正后待监测载体在al+2点的位置坐标；
所述惯性测量单元、视觉图像采集单元均设置于待监测载体上且均与所述信号处理模块连接；待监测载体的位置坐标、惯性测量单元的位置坐标、视觉图像采集单元的位置坐标相同或具有固定的相互关系。
本发明具有的优势和积极效果：本发明实现了室内外环境下的连续定位，充分利用高精度的卫星定位数据，以及室内视觉场景的高特征性，结合惯性测量单元的定位结果，比现有的室内视觉定位更具有使用灵活性、更精确的位置信息、更高的定位精度。本发明集成使用了多种观测信息的室内外融合定位方法，其使用室外环境精度高的北斗卫星定位信息，不受外界环境因素干扰的惯性导航定位信息，可闭环修正的场景视觉定位信息。本发明所提的方法具有适用性强，通用性好的优点，可以为单兵提供长短时的高定位精度与可靠性。本发明可以满足单兵在室内环境以及室内和室外环境下可靠性、高精度的定位需要。。
附图说明：
图1为本发明的室内外连续定位的流程示意图；
图2为本发明的室内外连续定位装置的结构示意图；
图3为本发明的实施例三的视觉图像与惯性导航融合定位方法的流程图；
图4为本发明的实施例四的视觉图像与惯性导航融合定位方法的流程图；
图5为本发明的实施例五的视觉图像与惯性导航融合定位方法的流程图；
图6为本发明的视觉图像采集单元(3)初始化标校框图；
图7为本发明的场景图像同名点计算流程图；
图8为本发明的视觉图像采集单元(3)在新位置下视觉位姿计算框图；
图9为本发明的场景视觉位置与惯性定位融合计算框图；
图10为本发明的场景视觉闭环修正计算框图；
图11为本发明的场景视觉使用摄影测量分析定位技术的原理图；
图12为对本发明的视觉图像采集单元所拍摄的相邻两幅视觉图像的特征点配准的实验示意图；
图中，1、惯性测量单元，2、卫星接收模块，3、视觉图像采集单元，4、信号处理模块，5、北斗卫星，10、待监测载体。
具体实施方式
下面结合附图对本发明做更详细地解释。
如图1-10所示，本发明提出一种室内外连续定位方法，采用无缝融合定位方案，是一种基于北斗卫星、惯性导航和场景视觉图像随遇校准技术的单兵连续定位方法，是集成使用了多种观测信息的室内外融合定位方法，其使用室外环境精度高的北斗卫星5的定位信息，不受外界环境因素干扰的惯性导航定位信息，可闭环修正的场景视觉定位信息。本发明所提的方法具有适用性强，通用性好的优点，可以为单兵提供长短时的高定位精度与可靠性。
图1为本发明的室内外连续定位方法的设计框图。卫星信号可采用北斗卫星信号。本发明中，待监测载体10可为人员。待监测载体10也可以是其他行进的装置。视觉图像采集单元3可采用视觉相机。若待监测载体10为人员，则视觉图像采集单元3设置在人员的肩部或腰部或其他合适的部位，用于拍摄前视视图，惯性测量单元1设置于足部或腰部或其他合适的部位。视觉图像采集单元3、惯性测量单元1可设置在相同的位置，也可设置在不同的位置。若视觉图像采集单元3、惯性测量单元1设置在相同的位置，则认为二者在同一时刻的位置坐标、姿态角相同，若二者设置在不同位置，则二者均为固定设置，且二者之间的距离为定值，可通过惯性测量单元1的位置坐标、姿态角计算得到视觉图像采集单元3的位置坐标和姿态角。本发明的视觉图像采集单元3采用普通相机即可。
在待监测载体10行进中，利用视觉图像采集单元3采集场景的视觉图像，视觉图像采集单元3采集第l幅视觉图像image(al)时在al点，且l＝1,2,3,……；视觉图像采集单元3的视角可设置为所需要的角度，优选设置为不小于120°；惯性测量单元1的采样时间不大于5s，可根据实际需要设置，例如可设置为0.01s、0.1s、1s。视觉图像采集单元3的采样时间可根据实际需要设置，例如可设置为1s或0.5s。
待监测载体10上固定设置有用于采集惯性测量数据的惯性测量单元1和用于采集场景的视觉图像的视觉图像采集单元3；所述待监测载体10位于al点时，所述视觉图像采集单元3采集第l幅视觉图像image(al)，l的初始值为1；所述惯性测量数据用于解算al点的位置坐标；待监测载体10的位置坐标、惯性测量单元1的位置坐标、视觉图像采集单元3的位置坐标相同或具有固定的相互关系。
视觉图像采集单元3的姿态角包括视觉图像采集单元3的航向角、俯仰角、横滚角。
卫星接收单元从卫星获取卫星接收数据，卫星接收数据包括卫星定位结果、接收时钟差，其中接收时钟差反映了卫星接收数据与惯性测量数据的时钟差。
本发明中，同名像点是指具有明显特征的对应点，同名像点集指的是视觉环境中同一个点在不同影像上成的像点集。共同同名像点集指两个同名像点集之中相同的同名像点的集合。
如图2所示，本发明提出的室内外连续定位装置包括信号处理模块4和设置于待监测载体10上的惯性测量单元1、卫星接收模块2、视觉图像采集单元3，所述惯性测量单元1、卫星接收模块2、视觉图像采集单元3均与所述信号处理模块4连接。信号处理模块4可采用单片机、dsp或fpga构成。
本发明提出的室内外连续定位装置，包括：
惯性测量单元1：用于采集惯性测量数据；
视觉图像采集单元3：用于采集视觉图像image(al)、视觉图像image(al+1)、视觉图像image(al+2)；信号处理模块4：用于确定视觉图像image(al)和视觉图像image(al+1)的同名像点集sl,l+1，利用同名像点集sl,l+1在视觉图像image(al)中的像素坐标、同名像点集sl,l+1在视觉图像image(al+1)中的像素坐标、al点的位置坐标、al+1点的位置坐标计算并保存同名像点集sl,l+1的位置坐标，确定视觉图像image(al+1)和视觉图像image(al+2)的同名像点集sl+1,l+2，确定sl,l+1与sl+1,l+2的共同同名像点集sl,l+2，利用共同同名像点集sl,l+2的位置坐标、共同同名像点集sl,l+2在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标，利用同名像点集sl+1,l+2在视觉图像image(al+1)中的像素坐标、同名像点集sl+1,l+2在视觉图像image(al+2)中的像素坐标、al+1点的位置坐标、al+2点的位置坐标计算并保存同名像点集sl+1,l+2的位置坐标，利用卡尔曼滤波法方法，根据第一坐标与第二坐标计算修正后待监测载体10在al+2点的位置坐标。
所述惯性测量单元1、视觉图像采集单元3均设置于待监测载体10上且均与所述信号处理模块4连接；待监测载体10的位置坐标、惯性测量单元1的位置坐标、视觉图像采集单元3的位置坐标相同或具有固定的相互关系。
惯性测量单元1包括加速度计、陀螺仪。惯性测量数据包括陀螺仪测量数据、加速度计测量数据。惯性测量单元1可采用微惯导。
本发明中，建立任意空间坐标系，al点的位置坐标表示al点在所述空间坐标系中的位置坐标，同名像点集的位置坐标表示同名像点集中各个点在所述空间坐标系中的位置坐标。
室内外连续定位方法包括：室内、室外环境判断方法、卫星与惯性导航融合定位方法、视觉图像与惯性导航融合定位方法。
实施例一：室内、室外环境判断方法
若判断待监测载体10位于室外，则利用卫星与惯性导航融合定位方法计算待监测载体10的位置坐标，否则，则利用所述视觉图像与惯性导航融合定位方法计算待监测载体10的位置坐标。
若q_satt＞4且loc_rest＜threshold_res，则判断卫星信号满足定位条件，其中q_satt为有效卫星个数，loc_rest为卫星定位残差，threshold_res为卫星定位残差阈值。所述有效卫星个数、卫星定位残差均由所述卫星测量数据解算得到。
利用单兵随身穿戴的北斗接收模块，获取单兵当前环境下北斗卫星5的有效星数q_satt，卫星定位残差loc_rest，依据如下策略对单兵所处环境进行评估切换。
基于获取的室内、室外评估划分，从而选择不同参考数据进行融合。若判断为是室外，则利用卫星与惯性导航融合定位方法计算待监测载体10的位置坐标，否则利用视觉图像与惯性导航融合定位方法计算待监测载体10的位置坐标，即进行如下判断：若同时满足q_satt＞4及loc_rest＜threshold_res，则利用卫星与惯性导航融合定位方法计算待监测载体10的位置坐标；否则，则利用所述视觉图像与惯性导航融合定位方法计算待监测载体10的位置坐标。根据卫星测量数据，即可得到q_satt，也可得到卫星定位残差loc_rest。
如果同时满足如下两个条件，则判断卫星信号质量满足上述要求：第一条件：有效卫星数q_satt大于4；第二条件：卫星定位残差小于threshold_res。其中，q_satt为有效卫星数；loc_rest为卫星定位残差，threshold_res为卫星定位残差阈值。如果有效卫星数q_satt大于4，则认为当前有足够的卫星可提供有效定位信息；如果卫星定位残差小于threshold_res，则说明定位解算位置精度满足要求。这个卫星定位残差越小越好，越小说明当前卫星的分布越有利于获取高精度的位置坐标。loc_rest的取值范围为0.5～99.9米。threshold_res取值范围为小于20米。
若满足第一条件、第二条件，即说明待监测载体位于室外环境，可以在室外采用gnss卫星与惯性导航融合的定位模式。若判断卫星信号质量不满足上述要求，则卫星定位数据无效，即说明待监测载体处于室内环境，可以在室内采用视觉图像与惯性导航融合的定位模式。
若判断待监测载体10位于室外或室内，则进行相应的计算。若处于室外环境，则可每间隔1s、5s、10s或20s进行一次判断，判断待监测载体10位于室外或室内。若处于室内环境，则可每采集5幅、10幅、20幅图像进行一次判断，判断待监测载体10位于室外或室内。这样的考虑时，人员在室内、室外环境的切换需要一定的时间。为了使得程序执行的效率最大化，可以设定每间隔一定时间再进行室内、室外的判断。
因此，通过上述对有效卫星数和卫星定位残差的判断，可以实现室内与室外模式的切换。
实施例二：卫星与惯性导航融合定位方法
所述卫星与惯性导航融合定位方法包括：利用惯性测量数据解算的待监测载体10在导航坐标系下的位置坐标、速度、航向角作为卡尔曼滤波的状态量，利用卫星解算的待监测载体10的位置坐标、速度和航向角作为卡尔曼滤波的观测量，利用卡尔曼滤波方法对待监测载体10的位置坐标进行修正。
步骤(b-1)计算惯性测量数据解算的数据和卫星解算的数据
利用惯性测量数据解算待监测载体10的姿态角、姿态变化矩阵、在导航坐标系下的位置坐标、在导航坐标系下的速度。
卫星接收模块2接收来自北斗卫星5的卫星测量数据，卫星测量数据解算得到卫星解算的待监测载体(10)的位置坐标(λbd,lbd)、速度(ve,vn)和航向角ψbd。
依据惯性测量单元1初始静止阶段的加速度计测量数据计算初始俯仰角和横滚角，构成惯导解算初始姿态，并构造姿态变化矩阵。依据惯性测量单元1的陀螺仪测量数据更新姿态变化矩阵，利用加速度计测量数据积分解算得到惯性测量数据解算的待监测载体(10)的速度vk＝[ve,vn,vu]、位置坐标pk＝[λ,l,height]。其中λ、l代表平面位置，height代表高度位置，其中n、v代表地球坐标系，u代表经纬度坐标。
步骤(b-2)时间同步
在利用北斗卫星5的定位数据、微惯性测量数据进行计算之前，需要先将卫星接收信号与惯性测量数据的时间进行同步。通过北斗gnss实时伪距单点定位解算的接收机钟差改正数获得高精度接收机钟时间。利用单兵手持端进行授时同步，保证手持端的系统时间与北斗同步；因此基于手持端的惯性定位时间系统也同北斗时保持一致。由于北斗数据更新率低，而惯导更新率高，而造成的不同步问题，则通过插值方式实现对北斗的观测数据进行拆分，并保证与惯导定位观测时刻相对应。若卫星接收模块2接收的数据的采样率小于惯性测量单元1采集的数据的采样率，则利用插值方式对北斗的观测数据进行拆分。
步骤(b-3)gnss数据有效性筛选
本步骤中，对卫星信号进行数据筛选。
步骤(b-4)利用北斗定位信息与惯性导航定位结果，通过卡尔曼滤波器得出最终定位信息。
利用导航坐标系的待监测载体10的水平位置信息、速度信息、航向角度信息作为卡尔曼滤波状态量，利用卫星导航信息的水平位置信息，以及水平速度信息，相对方向角度作为卡尔曼滤波的观测量,，利用卡尔曼滤波方法对待监测载体10的位置坐标进行修正。
选择在东、北、天坐标系(导航坐标系)的待监测载体10的水平位置信息、速度信息、航向角度信息作为卡尔曼滤波的状态量
实施例三：视觉图像与惯性导航融合定位方法
图3为实施例三的视觉图像与惯性导航融合定位方法的流程图。
本专利采用新的场景视觉技术与惯性导航定位相融合。即在实际使用时从室外场景到室内场景使用时，采样场景视觉技术与惯性导航定位相融合的方法。
待监测载体10上固定设置有用于采集惯性测量数据的惯性测量单元1和用于采集场景的视觉图像的视觉图像采集单元3；所述待监测载体10位于al点时，所述视觉图像采集单元3采集第l幅视觉图像image(al)，l的初始值为1；所述惯性测量数据用于解算al点的位置坐标；待监测载体10的位置坐标、惯性测量单元1的位置坐标、视觉图像采集单元3的位置坐标相同或具有固定的相互关系；所述视觉图像与惯性导航融合定位方法包括如下步骤：
步骤(a-1)：确定同名像点集s1,2
采集视觉图像image(al)、视觉图像image(al+1)，并确定视觉图像image(al)和视觉图像image(al+1)的同名像点集sl,l+1。
步骤(a-1)包括如下子步骤：
子步骤(a-1-1)提取特征点
采集视觉图像image(al)、视觉图像image(al+1)，提取两幅视觉图像中的特征点。当l＝1时，待监测载体10在a1点、a2点时，两个位置处视觉图像采集单元3所拍摄的图像分别为image(a1)和image(a2)。采集a1点的视觉图像image(a1)和a2点的视觉图像image(a2)。
对两幅视觉图像image(a1)和image(a2)的特征点进行提取和配准，得到两幅视觉图像中对某个或多个物体表征的像素点，即同名像点集s1,2。
提取视觉图像image(al)和视觉图像image(al+1)中的特征点；提取相邻两幅视觉图像中的特征点；常用的特征点提取方法包括：moravec算子方法、harris算子方法、susan算子方法、forstner算子方法、sift方法、surf方法等方法。
子步骤(a-1-2)判断所述特征点是否具有显著性：判断步骤(a-1-1)中提取的特征点是否具有显著性，优选利用robert算子判断所述特征点是否具有显著性。
子步骤(a-1-2-1)对提取的特征点进行robert’s梯度提取
子步骤(a-1-2-2)计算以点为中心指定窗口大小的灰度的协方差矩阵
子步骤(a-1-2-3)利用下式计算兴趣值q
其中detn代表矩阵n的行列式，trn代表矩阵n的迹。
当视觉图像中有超过5个点满足q＞tq，则说明视觉图像中具有显著的特征点。一般情况下，0.6≤tq≤0.75。
在本发明中，两幅视觉图像的同名像点集中至少需要包括3对同名像点，优选包括5对同名像点。在本发明中，利用所有同名像点进行计算。
子步骤(a-1-3)特征点的配准
对视觉图像image(a1)和image(a2)中具有显著性的特征点进行配准，得到同名像点集。
特征点的提取即在各幅图像中提取一定数量具有明显特征的点。当两幅视觉图像的特征点都提取出来后，即需要寻找其对应关系。两幅视觉图像之间的差异是由于摄像时的观察点不同引起的，因此两幅视觉图像之间产生差异的根本原因是视点的不同，从而引起环境中物体与周围的环境的相互位置关系在视觉图像中发生了变化。特征点的配准即是寻找两幅视觉图像中相互对应的特征点，若判定两幅视觉图像中相互对应的特征点均为空间同一物体上同一点的投影，则两幅视觉图像中相互对应的特征点为同名像点。实际运用中，可利用有内在关联的像素点有序集合进行配准。
步骤(a-2)：计算同名像点集sl,l+1的位置坐标
利用同名像点集sl,l+1在视觉图像image(al)中的像素坐标、同名像点集sl,l+1在视觉图像image(al+1)中的像素坐标、al点的位置坐标、al+1点的位置坐标计算并保存同名像点集sl,l+1的位置坐标且待监测载体10在a1点、a2点视觉图像采集单元的姿态角，优选al点的位置坐标、al+1点的位置坐标均由所述惯性测量数据解算得到。
视觉场景初始化，即a1点和a2点的参数初始化。利用惯性测量单元1定位结果给出相邻两个位置的位置坐标。如图6、图7所示，利用在a1点和a2点的惯性测量单元1解算的待监测载体10的位置坐标对a1点和a2点的位置坐标进行初始化。在a1点和a2点，分别利用惯性测量单元1进行解算，得到惯性测量单元1解算的待监测载体10的位置，由于视觉图像采集单元3、惯性测量单元1设置在相同的位置，或二者具有确定的位置关系，因此可根据惯性测量单元1解算的待监测载体位置坐标计算得到a1点和a2点的位置坐标。
根据摄影测量技术，利用同名像点集sl,l+1在视觉图像image(al)中的像素坐标、同名像点集sl,l+1在视觉图像image(al+1)中的像素坐标、al点的位置坐标、al+1点的位置坐标计算并保存同名像点集sl,l+1的位置坐标、待监测载体10位于al点时视觉图像采集单元3的姿态角、待监测载体10位于al+1点时视觉图像采集单元3的姿态角，并将同名像点集的位置坐标和视觉图像image(a1)保存到场景特征库中。场景特征库可采用缓存器或存储器，为本领域公知技术。优选al点的位置坐标、al+1点的位置坐标均由所述惯性测量数据解算得到。
如图6、图7所示，利用最初从室外到室内阶段内的短时间惯性测量单元1定位信息对视觉图像采集单元3进行初始化。由于短时间内惯性测量数据计算得到的定位结果的累积的误差有限，因此利用惯性测量数据计算在a1点、a2点的位置坐标的相对误差比较小，可以满足对视觉图像采集单元3的初始化标校。如图6-7所示，待监测载体10在a1点、a2点时，惯性测量单元1解算的待监测载体10位置的位置坐标分别为
根据如图11所示的摄影测量原理图，计算出两幅视觉图像中同名像点集s1,2所对应的场景点的三维坐标集pos(s1,2)，并保存到场景特征库中，图11中，s点为焦点，a_point、b_point、c_point为成像点。
拍摄得到两幅视觉图像后，可以通过摄影测量方法解算得到物体的位置坐标。摄影测量方法包括空间后方——前方交会法、相对定向法、绝对定向法、光线束平差解法、直接线性变换法等，均可利用现有技术实现。摄影测量方法中的各个关键技术，例如摄像机标定、像对定向、核线(极线)计算等，均可利用现有技术中的方法实现。
步骤(a-3)新的同名像点集的计算
本步骤中，采集视觉图像image(al+2)，并确定视觉图像image(al+1)和视觉图像image(al+2)的同名像点集sl+1,l+2；其中image(al+1)为待监测载体10在al+1点时视觉图像采集单元3所采集的视觉图像，image(al+2)为待监测载体10在al+2点时视觉图像采集单元3所采集的视觉图像。
确定视觉图像image(al+1)和视觉图像image(al+2)的同名像点集的方法与步骤(a-1)中相同。可参照步骤(a-1)中确定同名像点集的方法。
子步骤(a-3-1)采集视觉图像image(al+1)、视觉图像image(al+2)，提取两幅视觉图像中的特征点。
子步骤(a-3-2)判断所述特征点是否具有显著性，优选利用robert算子判断所述特征点是否具有显著性。
子步骤(a-3-3)对两幅视觉图像中具有显著性的特征点进行配准，得到同名像点集。
利用moravec算子方法或harris算子方法或susan算子方法或forstner算子方法或sift方法或surf方法提取相邻两幅视觉图像中的特征点。
图12为对本发明的视觉图像采集单元3所拍摄的相邻两幅视觉图像的特征点配准的实验示意图，将两幅视觉图像中的各个抽屉的把手以及柜子的边缘区域及柜子上的钥匙上提取了多个特征点，并实现了两幅视觉图像中各个特征点的配准，由此可以得到同名像点集。
步骤(a-4)共同同名像点集的计算
利用下式确定sl,l+1与sl+1,l+2的共同同名像点集sl,l+2
sl,l+2＝sl,l+1∩sl+1,l+2，
其中sl,l+1为image(al)和image(al+1)的同名像点集，image(al)为待监测载体10在al点时视觉图像采集单元3所采集的视觉图像。在完成视觉图像采集单元3的初始化后，在新位置al+2点处对场景进行成像，所成的像与上一位置al+1点处图像进行配准，提取同名像点集sl+1,l+2，并提取与上一时刻得到同名像点的共同同名像点sl,l+2。共同同名像点集中不少于3对同名像点，优选不少于5对同名像点。
步骤(a-5)新的点的位置坐标的获取
根据摄影测量技术，利用共同同名像点集sl,l+2的位置坐标、共同同名像点集sl,l+2在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标(xl+2,yl+2)、待监测载体10位于al+2点时视觉图像采集单元3的姿态角。
在步骤(a-5)中，当l＝1时，由于
如图7、图8所示，在新位置al+2点对场景完成视觉成像，将新的视觉图像与上一幅图像进行配准，提取两幅视觉图像之间的同名像点，并提取与上一个同名像点集的共同同名像点；利用得到的共同同名像点的三维坐标，反求出待监测载体10的新的位置坐标以及视觉图像采集单元3的姿态角；由新得到的待监测载体10的视觉图像采集单元3的位姿，通过摄影测量原理对新位姿下的其他同名像点进行三维坐标求解，并添加到场景特征库中。图7给出了对a3点的位置坐标等参数的计算示意图。
计算共同同名像点在al+2点处的空间三维坐标，利用共同同名像点的空间三维坐标计算该位置下待监测载体10的视觉图像采集单元3的位置坐标和姿态。然后再利用求解得到的待监测载体10的视觉图像采集单元3的位姿信息，利用摄影测量原理对该位置下同名像点集sl+1,l+2中非共同同名像点进行三维坐标求解，并添加到场景特征库中，丰富场景特征库。其中共同同名像点集sl,l+2的位置坐标已在al+1点时计算得到。
步骤(a-7)同名像点集的位置坐标的计算
根据摄影测量技术，利用同名像点集sl+1,l+2在视觉图像image(al+1)中的像素坐标、同名像点集sl+1,l+2在视觉图像image(al+2)中的像素坐标、al+1点的位置坐标、al+2点的位置坐标计算并保存同名像点集sl+1,l+2的位置坐标，并保存同名像点集的位置坐标。
步骤(a-8)视觉图像与惯性导航的融合
利用卡尔曼滤波法方法，根据第一坐标与第二坐标计算修正后待监测载体10在al+2点的位置坐标，且令l＝l+1，跳转到步骤(a-3)，所述第一坐标是步骤(a-5)中计算得到的al+2点的位置坐标，所述第二坐标是由惯性测量数据解算的al+2点的位置坐标。
选取状态量
当新捕获到的视觉图像中出现了之前已经经过的场景时，就可以利用已经存在的场景特征库和相机对相同区域所成的像进行闭环比对，利用比对差异对待监测载体10的单兵惯性定位结果进行修正。若待监测载体10所处环境场景比较单一，图像中判断未找到明显特征点，则认为视觉图像无法提供持续定位，必须利用惯性测量数据对待监测载体10进行定位。否则，若判断找到明显特征点，则只需利用视觉图像和惯性测量数据对待监测载体10进行定位。可利用场景特征库中的图像进行计算。
如图10所示，对新得到的视觉场景特征库中保存的历史场景特征进行比对完成不定期修正。不定期修正是当检测到的目标再次进入已经经过的历史场景时，利用已经存在的场景特征库和视觉成像图像进行配准，实现对目标位置的修正。实际使用过程中，如果待监测载体10一直在各个房间里走的时候，无法接收到卫星信号，从而一直利用惯性测量数据和视觉图像的方法对待监测载体10进行定位。
可利用场景特征库中的位置替代计算的位置，也可利用融合技术将两个位置值进行融合，例如可将两个位置的值取均值，得到修正后的位置值。
如图9所示，为本发明的场景视觉位置与惯性定位融合计算框图。获取到待监测载体10的视觉图像采集单元3在新位置下的位姿指示信息，同时惯性测量单元1也可以给出新位置的位置指示信息，将视觉场景信息与惯性测量数据进行融合得到最终位置信息进行输出，从而利用视觉位置对惯性位置进行校正。
视觉场景与微惯性定位信息融合的卡尔曼滤波选取待监测载体10的位置坐标，速度作为状态量x＝[λk,lk,vek,vnk]；选取由相邻两位置间的共同同名像点计算的该位置下待监测载体10的视觉图像采集单元3的位置与单兵惯性定位位置之差作为观测量
实施例四：包含第一判断的视觉图像与惯性导航融合定位方法
图4为实施例四的视觉图像与惯性导航融合定位方法的流程图。
本实施例与实施例三的区别是：还包括将采集的视觉图像保存到场景特征库中；所述步骤(a-3)中，还包括执行第一判断，若第一判断的结果为否，则跳转到步骤(a-4)，若第一判断的结果为是，则执行如下步骤(a-6)以代替步骤(a-4)-(a-5)；所述第一判断为：判断在场景特征库中是否存在一幅视觉图像，使得该视觉图像与视觉图像image(al+1)和视觉图像image(al+2)有共同同名像点集b且该共同同名像点集b由至少m对所保存的同名像点构成，优选m≥3，更优选m≥5。
步骤(a-6)：利用共同同名像点集b的位置坐标、共同同名像点集b在视觉图像image(al+2)中的像素坐标计算al+2点的位置坐标，且跳转到步骤(a-7)。
所述场景特征库中，任意相邻两幅视觉图像重合的面积不大于一幅图像面积的p％，优选p≤20。各幅视觉图像的大小相同。一般是，根据行走速度或姿态决定，如果行走速度、姿态变化不大，则可设置每5幅图片时，将前后的图片进行对比，若两幅图片的配准的内容占比例较小，则存储下一幅图片，即将路过场景中的所有信息都存储下来。若姿态变化较大，例如转向到另一侧，则每隔一幅即需要存储。
如图10所示，由于惯性测量数据的误差的累积，也可能对定位结果产生一定影响。因而，在本专利中，若待监测载体10在行进过程中回到原来已经走过的位置，例如又回到已经经过的房间，则可利用原有的对待监测载体10的定位对后续再次回到此处时的定位进行修正。
实施例五：包含第一判断与第二判断的视觉图像与惯性导航融合定位方法
图5为实施例五的视觉图像与惯性导航融合定位方法的流程图。
本实施例与实施例三、四的区别是：所述步骤(a-3)中，若第一判断的结果为是，则执行步骤(a-6)以代替步骤(a-4)-(a-5)，且不执行第二判断，若第一判断的结果为否，则执行第二判断；所述第二判断为：判断视觉图像image(al+1)和视觉图像image(al+2)的同名像点集是否由不少于h对同名像点构成，优选h＝3，更优选h＝5；若第一判断的结果为否且第二判断的结果为是，则跳转到步骤(a-4)；若第一判断的结果为否且第二判断的结果为否，则利用惯性测量数据计算al+2点的位置坐标，且令l＝l+1，跳转到步骤(a-1)。
若第一判断的结果为否且第二判断的结果为是，则跳转到步骤(a-4)。若同名像点集由不少于3对同名像点构成，则说明仍可利用采集的视觉图像进行迭代，对待监测载体10进行定位，因此，跳转到步骤(a-4)。
若第一判断的结果为否且第二判断的结果为否，则利用惯性测量数据计算al+2点的位置坐标，且令l＝l+1，跳转到步骤(a-1)。
若采集的视觉图像中不包含所保存的同名像点，且同名像点集由少于3对同名像点构成，则说明无法利用图像进行待监测载体10的位置的计算。
实施例六：室内外环境均利用视觉图像定位方法
若行走环境的室外环境的特征较为明显时，也可不对卫星接收信号是否满足定位条件进行判断，在室内外均利用视觉图像与惯性导航融合定位方法实现待监测载体10的定位。室外环境中，将室内中的视觉图像与惯性导航融合定位方法类比即可得到，不再赘述。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。在阅读了本发明之后，本领域技术对本发明的各种等价形式的修改均落入本申请所附权利要求所限定的范围。在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
- 还没有人留言评论。精彩留言会获得点赞！