CN111595332B

CN111595332B - 一种融合惯性技术与视觉建模的全环境定位方法

Info

Publication number: CN111595332B
Application number: CN202010284018.4A
Authority: CN
Inventors: 许凯; 鲍翊平; 王栋
Original assignee: Ningbo Shenxun Information Technology Co ltd
Current assignee: Zhejiang Shenxun Technology Co ltd
Priority date: 2020-04-13
Filing date: 2020-04-13
Publication date: 2023-05-09
Anticipated expiration: 2040-04-13
Also published as: CN111595332A

Abstract

本发明涉及定位方法技术领域，尤其涉及一种融合惯性技术与视觉建模的全环境定位方法。它包括以下步骤：S1、设置惯性测量单元与图像采集设备的位置，使两者相对静止；S2、通过惯性测量单元采集的数据进行定位，得到定位坐标；同时通过图像采集设备采集的数据进行视觉建模，得到视觉模型；S3、将视觉模型与定位坐标的坐标系进行统一，然后进行时钟同步，最后将定位坐标显示在视觉模型内。采用这种定位方法可以不依赖外部设备在陌生环境中使用，成本较低，且定位准确度较高。

Description

一种融合惯性技术与视觉建模的全环境定位方法

技术领域

本发明涉及定位方法技术领域，尤其涉及一种融合惯性技术与视觉建模的全环境定位方法。

背景技术

现有技术惯性导航定位技术因为其自带漂移累积特性，所以会随着时间和距离的增加，定位精度将越来越低，进而无法满足生活中的定位需要，所以需要其他定位技术来配合惯性导航来使用，一般为卫星定位以及UWB定位。

但是针对于卫星定位技术，其最大的缺陷就是受信号的影响太大，不谈室内定位，即使是在室外，一旦遇到高楼林立的情况，就无法配合惯导导航做到准确定位；而UWB定位的缺陷是需要布置大量的基站，不适合陌生环境，而且成本也较高。

发明内容

本发明所要解决的技术问题是：提供一种融合惯性技术与视觉建模的全环境定位方法，采用这种定位方法可以不依赖外部设备在陌生环境中使用，成本较低，且定位准确度较高。

本发明所采用的技术方案是：一种融合惯性技术与视觉建模的全环境定位方法，它包括以下步骤：

S1、设置惯性测量单元与图像采集设备的位置，使两者相对静止；

S2、通过惯性测量单元采集的数据进行定位，得到定位坐标；同时通过图像采集设备采集的数据进行视觉建模，得到视觉模型；

S3、将视觉模型与定位坐标的坐标系进行统一，然后进行时钟同步，最后将定位坐标显示在视觉模型内。

作为优选，步骤S3中的时钟同步是指通过将偏差在逐次优化中不断收敛到零。

作为优选，步骤S2中通过惯性测量单元进行定位的具体包括以下步骤：

SA1、从惯导测量单元中测得水平加速度与重力加速度；

SA2、通过水平加速度与重力加速度得到移动方向与速度，并且根据初始位置得到定位位置；

SA3、计算误差进行校正。

作为优选，步骤SA3中误差包括速度误差、位置误差以及姿态误差。

作为优选，步骤S2中通过图像采集设备采集的数据进行视觉建模的具体包括以下步骤：

SB1、将图像采集设备采集的图像进行预处理，将图像转换为带有主方向的特征点；

SB2、进行闭环检测；

SB3、确定各特征点的深度数据，得到各特征点的立体坐标；

SB4、通过特征点匹配得到相对位姿；

SB5、进行2D-3D转换，实现三维视觉建模。

作为优选，步骤SB1包括以下具体步骤：

SB11、将图像采集设备采集到的图像进行灰度处理；

SB12、对灰度处理后的图像使用高斯滤波处理；

SB13、对高斯滤波后的图像使用黑塞矩阵对各像素点进行偏导处理，从而找到图像中的边缘点；

SB14、筛选边缘点，取得最后的特征点；

SB15、赋予最后得到的特征点的主方向。

采用以上方法与现有技术相比，本发明具有以下优点：通过将惯性导航定位技术结合三维视觉建模，这样可以不依赖于外部辅助设备就能实现定位，而且可以实时生成陌生环境的实际地图，为指挥人员提供现场实时环境数据以供决策参考，并且以视觉建模地图校正惯性定位结果，可以保证惯性定位长时间的高精度、不漂移。

附图说明

图1为本发明一种融合惯性技术与视觉建模的全环境定位方法中的积分像素值的示意图。

图2为本发明一种融合惯性技术与视觉建模的全环境定位方法中高斯滤波模板与盒子滤波器的示意图。

图3为本发明一种融合惯性技术与视觉建模的全环境定位方法中三层图像示意图。

图4为本发明一种融合惯性技术与视觉建模的全环境定位方法中Haar小波滤波器的示意图。

图5为本发明一种融合惯性技术与视觉建模的全环境定位方法中对极约束的解释图。

图6为本发明一种融合惯性技术与视觉建模的全环境定位方法中时钟同步的示意图。

具体实施方式

以下通过具体实施方式对本发明做进一步描述，但是本发明不仅限于以下具体实施方式。

本发明主要从惯性技术与视觉建模两个角度来实现最终的定位计算。

需要将惯性测量单元(也可以成为惯导模块，主要包括三个加速度计)和图像采集设备(主要包括相机)的位置进行固定，使两者保持相对静止。

首先，从惯性测量单元内部的三个加速度计中分别测得佩戴人员的各方向比力分量f^b，用于后续的定位计算。

为了能够将比力在矢量上拆分成为水平加速度与重力加速度，需要将载体坐标系(b系)内的比力转换成为导航坐标系(n系)，这样才能得到真实的地面加速度值，进行有效的地面定位。

而坐标系的转换需要借助余弦矩阵

即：

为了获得这个

的实时值，就要知道两坐标系之间的夹角：

艏向角φ：载体的纵向轴y_b在当时位置点相对于地球的切平面的投影与地理北向轴y_t的夹角，以北偏东方向为正向，定义域范围0°～360°。

纵摇角θ：载体的纵向轴与载体横向轴在水平面投影的夹角，向上为正方向，定义域范围-90°～90°。

横摇角γ：载体的横向轴x_b与载体横向轴在水平面投影的夹角，以右倾方向为正，定义域范围-180°～180°。

则有载体坐标系(b系)与导航坐标系(n系)之间的转换关系为：

为了方便之后的算式书写，记姿态矩阵

在得到了转换矩阵

后，可通过转换f^b获得fⁿ，用于接下来的定位计算。然而，直接使用fⁿ进行下一步计算显然是不合理的，因为载体处于地球环境中，其势必会受到地心引力与地球自转产生的影响，因此考虑导航坐标***的时候，需要将这一数据添加到考虑中，不然所得的定位结果会存在巨大的误差。

将地心引力与地球自转纳入到考虑范围后，根据向量的绝对变率和相对变率的关系及牛顿第二定律，可以得到惯性导航***的基本方程为：

其中：

——导航坐标系内的加速度矢量

fⁿ——导航坐标系的比力矢量(加速度计的输出量)

——地球自转角速度

——载体相对地球运动产生的角速度

gⁿ——重力加速度矢量

——哥式加速度与地球的向心加速度的和

哥式加速度的定义为：当一动点相对某一动参考系作相对线运动，同时该动系又在作转动运动时，动点的相对速度发生变化，产生了哥式加速度。简言之，哥式加速度是由相对运动与牵连转动的共同作用形成的。

此方程计算的是载体的速度，其中

R_m和R_n为地球参考椭球体的主曲率半径，即：

将上述fⁿ、

gⁿ、

的公式带入到速度方程中，可解出最后的三个轴向速度

得到三个轴向速度之后就可以计算得到了人员的移动方向与速度后，只需要将速度代入到积分方程中，加到人员的初始位置中，即可获得人员最后的位置数据，且这个位置可根据速度变化实时更新。

其中L₀和λ₀分别为零时刻是载体的纬度和经度，T表示经过的时间。

不可避免的，任何***总是有着误差的存在，为了进一步提高惯导的定位精度，需要进行最后的误差计算，用以优化定位结果。

假设实际中的导航坐标系n系与计算机解算出来的导航坐标系

系存在的微小角度差分别为

则有n系和

系的坐标变换矩阵：

下面，将使用这个变换矩阵来分别求速度、位置、姿态的误差。

计算速度误差：

理想情况下的水平通道的速度计算方程为：

其中，

为实际速度值；

为实际比力值；L^c为实际纬度值。

并且，很容易得到一组实际速度的表达公式：

其中，i＝x、y、z，

表示速度差，▽_i表示加速度计零位偏差。

联立方程，得速度误差的表达式为：

计算位置误差：

由载体的位置计算公式

可得实际含误差情况下的经纬度变化率为：

又有实际误差表达式：

联立上述方程，可得误差的表达式为：

计算姿态误差：

已知存在角度偏差为

写成负反对称矩阵的形式为：

则之前的转换矩阵可以改成：

已知惯性测量单元坐标系s系相对计算机解算出的导航坐标系

系的转换矩阵为：

对此等式左右两边进行求导，得：

再将余弦矩阵中所含性质

代入上述方程，得

两边同乘以方向余弦矩阵

并代入到

矩阵中，得：

其中，

表示陀螺仪在静止时三个轴的漂移误差，令x,y,z得漂移误差分别为ε_x、ε_y、ε_z，即

由速度计算中获得的角速度计算矩阵进行求导，可得对应的漂移：

联立上述方程，可得姿态误差表达式为：

通过以上定位解算与误差排除算法，可以极大程度上减小角度误差给定位带来的精度影响。然而，惯性导航还有一个特性，即——误差累积。虽然，经过修正后的惯性导航具有相对误差3‰的精度，但是它的精度会随着运行时间之间下降。

本发明采用视觉建模的方式来校正惯导定位结果，依靠建模来约束定位点的漂移，从而长时间维持高精度定位。即：在惯性测量单元获取比力分量的同时，配备在人员身上的镜头组在实时采集着现场的画面进行建模。

首先，通过配备在身上的镜头组采集现场画面，并将该画面进行灰度化处理，使图像内部的各像素值以灰度值的形式表示。同时，以图像的左上角为坐标原点，向右为x轴正向，向下为y轴正向建立坐标系。则任意像素点(x_i,y_i)与坐标原点所围成的矩形区域中所有像素值的合就是——积分像素值。

通过积分像素值，想要获得任意矩形的积分像素值，如图1所示，只要取该矩形的四个顶角位置的积分像素值进行加减运算即可得到：Σ＝A-B-C+D，A为左上点，B为右上点，C为左下点，D为右下点。由于整个矩形运算不再需要对矩形内的各像素点注意累积，因此大大减少了图像处理的运算量。

获得各像素点的像素值之后，需要使用高斯滤波来对获得的图像进行处理，以确保我们最后获得的特征点具备尺度无关性。(即：无论场景的尺度大小如何变化，机器视觉都能够判断是否是同一场景的图像。这关乎于后期建模过程中的闭环检测的实现。)

高斯滤波是一种对图像像素值进行加权平均的过程，每个像素点的值，都是由其本身的值和领域内的其他像素值进行加权平均后得到的。越靠近的点权重越高，反之则权重越低。对图像进行模糊化处理，以此来模拟机器在距离物体由远到近时的成像过程。

二维高斯滤波的公式为:

其中，x,y表示像素点的坐标，σ²表示像素值的方差。

对图像进行高斯模糊之后，即可使用黑塞矩阵对各像素点进行偏导处理，从而找到图像中的边缘点(突变点)。

Hessian矩阵的判别式为：

由于高斯核是服从正态分布的，SURF算法使用盒式滤波器来近似替代高斯滤波器，以此提高运算速度。如图2所示。

得到一个黑塞矩阵的近似值：

det(H)＝D_xx*D_yy-(0.9*D_xy)²。

其中，D_xy乘上的系数0.9是为了平衡使用盒式滤波器所带来的误差。

通过上述步骤找到图像中的所有突变点后，要对这些突变点进行逐一筛选，取得最后的特征点。由于特征点是图像中的极值点，且这个极值不仅仅是与周围8个点比较，还包括相邻两层的9个点。

如图3所示，中间的检测点要和其所在图像的3×3邻域8个像素点，以及其相邻的上下两层的3×3领域18个像素点，共26个像素点进行比较。所以，为了使每个突变点都能够有上下两层图像进行比对，只有一个图像是远远不够的，这时候就需要进行尺度空间的构建。

构建的方法主要是：保持原图像尺寸大小不变，仅仅改变盒式滤波器的模板尺寸大小来获得不同尺度的图像，再使用获得的图像来组成一个金字塔模型。

先从9*9尺寸的盒子滤波器开始，此时盒子滤波器对应的是σ＝1.2的二阶高斯滤波器。由于一个尺度空间是由若干个组构成，而每一组又包含若干层。层与层之间是由不同尺寸的滤波器生成的，而组与组之间是起始与间隔的尺寸大小不同生成的。而滤波模板尺寸与σ的对应关系式为：

例：

值得注意的是，为了保持尺度空间的连续性，以避免出现每组的第一层和最后一层没有上下层进行比较的情况，相邻组之间必须有部分层的重叠，同时每组中的盒子滤波器的尺寸都是逐渐增大的。

在构建完的尺度空间中即可对获得的突变点进行筛选，获得其中的极值点，即——特征点。然而，我们还需要根据实际情况，对这些特征点进行相应的择优处理。

如果获得的特征点较少，则不做择优处理，直接使用这些特征点。

若特征点较多，则需要进行下一步筛选，仅保留对比度较大的特征点。具体方法如下：

设定阈值，记候选特征点x，其偏移量定义为Δx，其对比度为D(x)的绝对值∣D(x)∣，对D(x)应用泰勒展开:

得，

由于x是D(x)的极值点，所以对上式求导并令其为0，得到

然后再把求得的Δx代入到D(x)的泰勒展开式中

设对比度的阈值为T，若

则该特征点保留，否则剔除掉。这样就能保证留下来的特征点都是高对比度的点。

在实际选择阀值时，要根据实际应用中对特征点数量和精确度的要求改变阀值。阀值越大，得到的特征点的鲁棒性越好，但获得的特征点数量越少。在处理场景简单的图像时，其阀值可以适当的调低。在复杂的图像中，图像经旋转或者模糊后特征点变化的数量较大，测试需要适当提高阀值。

在确定下来最后的特征点后，要赋予这些特征点主方向，使其具备旋转不变性，即：两幅旋转角不同的图像进行匹配，依然能够通过每个特征点的主方向来进行匹配。这一步同样关乎于后期建模过程中的闭环检测。

主要步骤如下：

Ⅰ兴趣点方向的确认

首先赋予每一个兴趣点方向特征。我们以某个特征点为圆心，以6S(S为该兴趣点对应的尺度)为半径的圆形领域里，用尺寸为4S的Haar小波模板对图像进行处理，求x,y两个方向的Haar小波响应。

Haar小波的模板如图4所示：

其中左侧模板表示X方向的响应，右侧模板表示y方向的响应，黑色表示-1，白色表示+1。用其对圆形领域进行处理后，就得到了该领域内每个点对应的x,y方向的响应，然后用以兴趣点为中心的高斯函数(σ＝2S)对这些响应进行加权。

在这个圆形领域内做一个60度的扇形区域，统计这个扇形区域内的haar小波特征总和，然后转动扇形区域，再统计小波特征总和。小波特征总和最大的方向为主方向。

Ⅱ生成特征点描述子

在特征点周围选取一个正方形框，方向为关键点的主方向，边长为20S。将其划分为16个区域(边长为5S)，每个区域统计25个像素的水平方向和垂直方向的Haar小波特性(均相对于正方形框的主方向确定的)，

该小波特征包括水平方向值之和，水平方向绝对值之和，垂直方向值之和和垂直方向绝对值之和(为了把强度变化的极性信息也包括描述符中，所以对绝对值进行累加)。

以上所有步骤即为图像的预处理，将镜头组采集的图像转化成一个个带有主方向的特征点，接下来就是对这些特征点的应用。

首先是闭环检测。由于整个算法中仅考虑相邻时间上的关联，因此不可避免的会遇到误差累积的问题，而随着时间的不断延长，整个VSLAM的轨迹漂移将越来越严重。此时，就需要使用闭环检测来修正轨迹，将漂移的点拉回到初始位置。从上述算法中获得特征点后，采用KD-Tree算法对最近邻的特征点进行匹配，识别两图像是否处于同一场景，从而判断是否形成闭环。

KD-Tree算法主要是通过不断得将数据按照大小分成两份。左支上的所有值均要小于根值，而右支上的所有值均要大于根值，以此为条件不断将整个数值进行划分，直至所有子集都不能划分为止。

当遇到类似像素位置这样的二维数据时，要先对x,y分别求方差，然后对方差大的方向进行划分，因为方差大表示这些数据在这个方向离散度更高，划分更明显。

例：有{(4,7),(2,3),(7,2),(8,1),(5,4),(9,6)}6个像素位置。

分别对x,y求方差。得x方向得方差更大。因此先以x方向划分：2,4,5,7,8,9。取中间值7，则有左支(2,4,5)与右支(8,9)。

然后再在y方向上进行划分，3,4,7取中值4，而1,6取6即可得到二叉树。

这样的话，当进行特征点匹配时，就能直接在这个二叉树上根据数值的大小进行最短路径的搜索配对，实现最高效的图像配对。

但是，如果只有单向的搜索，那么所得结果不一定是距离Q点最近的点，还需要进行回溯操作，即，判断未被访问过的(或者已访问过的)树分支中是否还有离Q更近的点，就是判断"Q与未被访问的树分支的距离|Q(k)-m|"是否小于"Q到当前的最近邻点的距离"。从几何空间上来看，就是判断以Q为中心，以dis为半径超球面是否与未被访问的树分支代表的超矩形相交。若相交则继续计算Q与分支代表的距离，反之则直接略过不考虑。

如果出现相交说明回溯点距离Q点更近，反之，则说明结果点已经是最邻近点。然后通过比对最临近点与Q点间的像素值，因为一张图像中有数十个Q的存在，那就形成了数十个比对关系，当这种对应关系中的值与特征描述相近，且相比较于该图像相邻几帧更相似时，那么就可以认为此时形成了一个闭环。

解决了轨迹漂移问题后，然后就是确定各特征点的深度数据(相对于镜头组的距离)。

这里我们采用对极约束的原理。如图5所示，a，c是相机的中心，x是图像中的像点，那么可以知道实物X一定在直线cx的延长线上，此时如果有另一个镜头的图像数据，就可以同理做出一条延长线，两线的交点就是实物的真实位置，从而确定具体实物距离机器的远近。且在此基础上，可以以第一个相机作为坐标原点，假设其未平移和旋转，再通过像点移动来判断第二个相机相对于第一个相机的旋转与平移，从而知道相机的位姿。

推导公式如下：

设以第一个相机作为坐标系三维空间的点P＝[X,Y,Z]，其在两个相机的像点分别为p₁,p₂。由于第一个相机的中心作为世界坐标系的原点，也就是说第一个相机没有旋转和平移，通过小孔相机模型可得：

p₁＝KP，p₂＝K(RP+t)

其中,K是相机的内参，R,t是第二个相机相对于第一个相机的旋转和平移。

从p₁＝KP可以得到P＝K^-1p₁，带入第二个式子可得到：

p₂＝K(RK^-1p₁+t)

两边同时左乘K^-1，得到

K^-1p₂＝RK^-1p₁+t

设x₁＝K^-1p₁,x₂＝K^-1p₂，代入

x₂＝Rx₁+t

两边同时左乘向量t的反对称矩阵t×,由于t×t＝0，消除t

t×x₂＝t×Rx₁

两边再同时左乘

由于t×x₂是向量t和向量x₂的叉积，同时垂直于向量t和向量x₂，所以左边

得到

再将x₁,x₂换掉

上式是对极约束的另一种表示，该式子中仅包含像点，相机的旋转和平移，中间的矩阵就是基础矩阵F。

其中F＝K^-Tt×RK^-1

由式子F＝K^-Tt×RK^-1，可知假如相机的内参数K是已知的，提取中间的矩阵可到

E＝t×R

E被称为本质矩阵，其和基础矩阵相差相机的内参K。通过匹配的点对计算相机的位姿。

通过上面的知道，对于匹配的像点p1,p2以及基础矩阵F有如下关系：

其中F＝K^-Tt×RK^-1

也就是说，仅通过匹配的点对(最少7对)可以计算出两视图的基础矩阵F，然后再从F中分解得到相机的位姿势。

相机的相对位姿可以通过特征点匹配估计出来：

1、提取两幅图像的特征点，并进行匹配

2、利用匹配得像点计算两视图的基础矩阵F

3、从基础矩阵F中分解得到相机的旋转矩阵R和平移向量t

通过上述方法采集到空间点与图像中的像素点对应关系后，即可进行最后的2D-3D坐标转换，实现VSLAM的三维建模。

即，一个空间点[x,y,z]和它在图像中的像素坐标[u,v,d](d深度数据)的对应关系是这样的：

其中，f_x,f_y指相机在x,y两个轴上的焦距，cx,cy指相机的光圈中心，s指深度图的缩放因子。这个公式是从(x,y,z)推到(u,v,d)的。反之，我们也可以把它写成已知(u,v,d)，推导(x,y,z)的方式。

根据这个公式就可以构建点云模型了。

通常，我们会把f_x,f_y,cx,cy这四个参数定义为相机的内参矩阵C，也就是相机做好之后就不会变的参数。给定内参K之后，每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了：

其中R和t是相机的姿态。R代表旋转矩阵，t代表位移矢量。因为我们现在做的是单幅点云，认为相机没有旋转和平移。所以，把R设成单位矩阵I，把t设成了零。s是深度图里给的数据与实际距离的比例。由于深度图给的都是short(mm单位)，s通常为1000。

本发明通过统一坐标系与进行时钟同步两个步骤来结合这两项技术。

坐标***一

通常情况下，两个技术都有着相互独立的坐标系，为了让惯性定位的点能够在视觉建模生成的模型中进行高精度地定位显示，就要保证视觉建模与惯性定位这两项技术处于同一个坐标系内。只有这样，空间中的一个定点的移动轨迹在这两个技术的坐标系内才能重合到一起。

本发明将视觉建模和惯性定位的功能模块进行集成设计，即：组装于单个产品中，使得两者保持相对静止。这就意味着视觉建模和惯性定位所对应的相机坐标系与载体坐标系的夹角在产品制造完成后就固定了。相机坐标系的z轴为摄像机光轴，x,y轴分别与成像平面的x,y轴平行，原点位于相机的光心；载体坐标系则为对应惯性定位模块中的三个加速度计组成的坐标系。

那么在已知两坐标系的情况下，将其代入到载体坐标系与导航坐标系的转换公式当中去，可以得到对应的转换矩阵T＝

其中，

θ_x：相机坐标系的纵向轴y轴在载体坐标系的xy平面上的投影与载体坐标系的y轴的夹角，以顺时针方向为正向，定义域为0°～360°。

θ_y：相机坐标系的y轴与其y轴在载体坐标系xy平面中的投影的夹角，以向上为正向，定义域为-90°～90°。

θ_z：相机坐标系的x轴与x轴在载体坐标系xy平面上的投影的夹角，以右倾方向为正，定义域为-180°～180°。

那么就可以通过公式：A＝TB，将相机坐标系代入到B中，所获得的新坐标系即可与惯性坐标系重合，从而确保两个技术的定位轨迹重合，定位轨迹精准显示在实时建模的地图模型中。

时钟同步

在进行坐标系转换实现两个坐标系重合之后，理论上两个技术的所得的定位点应该是重合，然而实际上，在不考虑漂移的情况下，两项技术所得的结果依然存在差别。这是因为所有的硬件***都会存在触发延时、传输延时和没有准确同步时钟等问题，导致每个设备无法做到同时获得定位结果，使得相互之间的结果存在偏差。如图6所示，以相机和IMU数据时间戳上的偏差作为状态变量的一部分，记为t_d，令t_IMU＝t_cam+t_d，使得惯性测量单元与图像采集设备(相机)之间的时间戳能够重合。

设第n帧对应的时间戳为t_n，则其对应的真实采样时间点为t_n-t_d。此时，该帧图像上上的一个特征点I_k,其对应坐标为[x_k y_k]^T,那么该帧和真实特征点坐标的关系为：

I_k真＝[x_k y_k]^T-t_dV_k

其中V_k表示t_d时间段内特征点的移动速度，因为正常情况下t_d是非常小的，所以可以将特征点视为匀速运动，即V_k为一个常量。

那么将这一情况代入到三维坐标系中，用I_k真来替换I_k并将时间偏差参数t_d引入到优化方程相机残差项，即重投影误差可以写为：

e_k＝I_k真-π(R_k(P_l-p_k))

其中，P_l表示三维坐标[x_l y_l z_l]^T,(R_k p_k)表示相机的姿态。

因为即使是误差，它也并不是一个固定的值，所以需要取误差中的最小值(前文提过，正常情况下误差不应该很大)，则有：

其中，e_p-H_pχ表示先验因子，e_B(I_k+1,χ)表示惯性***产生的误差项，B表示惯性设备所得到的各个测量值，

表示视觉建模产生的误差项，C表示视觉建模采集到的至少被观测到两次的特征点，这些误差项中包含了t_d的信息。

最后将所得到的最小t_d代回到最开始的时间偏差中t'_cam＝t_cam-t_d,然后用新得到的t'_cam建立新的时间戳再次进行以上操作，使得偏差在逐次优化中将不断收敛到零，最终实现时钟同步。

至此，两项技术就同时满足了坐标***一与时钟同步，即可实现同时同点的精准定位了。视觉建模技术获得的实时建模同时回传至后台指控端，为指挥人员提供现场真实环境模拟。真正意义上实现了在不依赖外部基站的情况下的陌生环境长时间高精度定位与地图信息模拟回传。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的技术人员应当理解，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行同等替换；而这些修改或者替换，并不使相应的技术方案的本质脱离本发明各实施例技术方案的精神与范围。

Claims

1.一种融合惯性技术与视觉建模的全环境定位方法，其特征在于，它包括以下步骤：

S3、将视觉模型与定位坐标的坐标系进行统一，然后进行时钟同步，最后将定位坐标显示在视觉模型内；

步骤S2中通过惯性测量单元进行定位的具体包括以下步骤：

SA1、从惯导测量单元中测得水平加速度与重力加速度；

SA3、计算误差进行校正；

步骤S2中通过图像采集设备采集的数据进行视觉建模的具体包括以下步骤：

SB2、进行闭环检测；

SB3、确定各特征点的深度数据，得到各特征点的立体坐标；

SB4、通过特征点匹配得到相对位姿；

SB5、进行2D-3D转换，实现三维视觉建模；

步骤SB1包括以下具体步骤：

SB11、将图像采集设备采集到的图像进行灰度处理；

SB12、对灰度处理后的图像使用高斯滤波处理；

SB14、筛选边缘点，取得最后的特征点；

SB15、赋予最后得到的特征点的主方向。

2.根据权利要求1所述的一种融合惯性技术与视觉建模的全环境定位方法，其特征在于：步骤S3中的时钟同步是指通过将偏差在逐次优化中不断收敛到零实现的。

3.根据权利要求1所述的一种融合惯性技术与视觉建模的全环境定位方法，其特征在于：步骤S3中的坐标系进行统一是指：通过已知的惯性测量单元的坐标系与图像采集设备的坐标系来得到转换矩阵，然后通过转换矩阵来进行坐标统一。

4.根据权利要求1所述的一种融合惯性技术与视觉建模的全环境定位方法，其特征在于：步骤SA3中误差包括速度误差、位置误差以及姿态误差。