CN111595337B - 一种基于视觉建模的惯性定位的自校正方法 - Google Patents

Abstract

本发明涉及惯性定位技术领域，尤其涉及一种基于视觉建模的惯性定位的自校正方法。它包括以下步骤：S1、惯性设备与视觉建模设备同时采集数据；S2、得到状态方程；S3、得到观测方程；S4、将状态方程与观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。采用这种校正方法，在不需要事先做任何布置的前提下，让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果。

Description

一种基于视觉建模的惯性定位的自校正方法

技术领域

本发明涉及惯性定位技术领域，尤其涉及一种基于视觉建模的惯性定位的自校正方法。

背景技术

现下主流的室内定位技术中，惯性定位技术凭借着其成本低、不受信号强弱影响、不依赖外部辅助即可进行高精度定位的特点占据了一定的市场。然而，惯性定位由于其误差累积的特性，无法满足人员长时间或长距离定位的需求，因此需要一项技术来对惯性定位的结果进行不定时的校正，以弥补误差带来的精度失准。

常见的校正方式主要分为两种，一种是卫星定位校正，使用北斗/GPS定位对惯性定位结果进行修正，保持其定位精度；第二种是架设如UWB、蓝牙信标这类的基站，通过此类基站所得的人员位置来修正惯性定位的结果。

前者使用卫星定位来校正惯性定位结果，将使得惯性定位不再具有原本的不受信号强弱影响特点，一旦来到室内或者高楼附近这类卫星信号差的地区，卫星定位的修正效果就会失效。

后者虽然适用于室内等卫星信号无法覆盖的地方，但是由于对基站有要求，因此具有成本高、需要事先部署基站灵活性差等缺点。

发明内容

本发明所要解决的技术问题是：提供一种基于视觉建模的惯性定位的自校正方法，采用这种校正方法，在不需要事先做任何布置的前提下，让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果。

本发明所采用的技术方案是：一种基于视觉建模的惯性定位的自校正方法，它包括以下步骤：

S1、惯性设备与视觉建模设备同时采集数据；

S2、根据步骤S1采集到的数据计算得到惯性定位的误差向量与视觉建模的特征点坐标向量，然后根据误差向量与特征点向量得到动点的状态方程；

S3、根据采集到的数据得到视觉建模中的动点与特征点之间的距离，同时得到惯性定位中的动点与特征点的距离，然后根据两个距离的均方差得到动点的观测方程；

S4、将步骤S2得到的状态方程与步骤S3得到观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；

S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。

作为优选，步骤S5之后还需要进行平滑处理。

作为优选，所述平滑处理是指采用容积Rauch-Tung-Striebel平滑器进行处理。

作为优选，步骤S2中的误差向量包括姿态误差、位置误差、速度误差、加速度计误差以及陀螺仪误差。

作为优选，步骤S2中得到特征点的具体方法包括以下步骤：

S21、将采集的图像进行灰度化和模糊化处理；

S22、采用黑塞矩阵对处理后的图像中各像素点进行偏导处理，获得突变点；

S23、将突变点与其周围26个像素点进行比较，保留其中的极值点作为特征点。

作为优选，得到特征点后还需要进行特征点筛选。

采用以上方法与现有技术相比，本发明具有以下优点：采用视觉建模技术给惯导定位技术进行校正，这样并不需要事先布置基站，而能让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果，并且整套定位系统可以完全自主独立进行定位工作，不容易受到外界信号影响。

附图说明

图1为本发明一种基于视觉建模的惯性定位的自校正方法的原理图。

具体实施方式

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

本方法的主要原理就是使用卡尔曼滤波将视觉定位所获得的定位点和惯性技术所获得的定位点进行误差计算。

当来到一个陌生的环境时，集成在一起的惯性设备与视觉建模设备会同时开始采集数据并进行处理。以运行过程中的任意时间点t为例。在t时刻，惯性设备中的加速度计与陀螺仪都采集到了佩戴人员当前的数值，并用这些数值进行处理，得到了此时该人员对应的位置、速度、姿态。显然，使用带有误差的数值计算出来的结果也都含有误差，这里我们使用来表示t时刻时惯性技术解算出的15维误差向量。

其中，Ф_t表示t时刻的姿态误差向量，表示t时刻的位置误差向量，/>表示t时刻的速度误差向量，/>表示t时刻的加速度计误差向量，/>表示t时刻的陀螺仪误差向量，这些误差向量都包括3个元素(即x,y,z,轴或对应各轴的旋转角)。上标b表示该数值建立在载体坐标系上，上标n表示该数值建立在导航坐标系上。

而在惯性设备获取相对定位数值并处理的同时，视觉建模也在不同的采集现场画面并处理着。在t时刻，视觉建模设备采集到现场的画面图像，在对该图像进行灰度化与模糊化处理之后，使用黑塞矩阵对所得到的新图像中的各像素点进行偏导处理，以此初步获得一些突变点，然后用这些突变点与其周围的26个像素点进行比较，只保留其中的极值点作为最后的特征点。

而这些特征点不一定都能够达到自校正算法中用于标定的特征点的标准。因此，要对这些特征点进行进一步的筛选处理，即：根据所需要的特征点个数选择对应的对比度阈值，将这些特征点进行筛选处理，仅保留满足阈值要求的特征点，否则剔除掉。这样就能保证最后留下来的特征点都是满足要求的高对比度的点。将m个这种特征点的坐标向量记为

结合所获得这些特征点坐标向量与惯性定位的误差向量，得到的状态方程为：

其中，矩阵I表示单位矩阵。ω_t为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。/>为t时刻惯性定位系统的误差状态转移矩阵。其表达式为：

矩阵中的 分别表示导航坐标系下的东向，北向与天向，/>表示坐标系从载体坐标系b系到导航坐标系n系的转换矩阵，T为系统的采样周期。

而因为卡尔曼滤波在进行数值的最优化选择的时候还需要该点的观测值，所以还需要确定t时刻时，动点对应的观测方程。本方法采用特征点与动点之间的距离作为参考数据，然后对其进行均方误差计算减少其中的噪声误差，来确定最后的观测方程。

首先，记视觉建模中的动点与特征点之间的距离为根据距离计算公式，得：

式中的表示定点在视觉定位中的理论值，/>为第i个特征点的理论位置值，特征点取自前文中提到的保留的高对比度得特征点。

那么在同一时刻，该动点在惯性定位中与特征点的间距的表达式为：

那么相同点在两个系统中的间距的均方误差为：

由于本发明中要用视觉定位的结果来校正惯性定位的定位结果，因此这里先假定视觉定位所得结果精准，为惯性定位结果减去其误差后所得的最后结果，其表达式为：

式中的表示t时刻惯性定位推导的定点位置的误差。

将此关系式代入到均方误差表达式中，可得：

代入的表达式，得：

将等式左边的特征点坐标移至等式右边，使左边仅保留定点坐标值，即：

再代入视觉定位中的噪声ε，获得视觉定位得实际距离表达式：

可得最后的均方差表达式变为：

令

就可以获得最后化简得观测方程：

将得到的动点的状态方程与观测方程代入到计算预测值与实际值之间误差的协方差矩阵中，即可获得最后的校正结果。

假设，两个方程所得结果分别为P₁＝(x₁,y₁)，P₂＝(x₂,y₂)，而它们的高斯白噪声分别为Q₁,Q₂。那么经过校正后得到的定位点坐标为:

或/>

完成以上校正计算之后，还需要进行最后的算法优化。因为简单的卡尔曼滤波所得到的是一个线性方程A＝TB，T为转换矩阵。而作为一个描述定位曲线的方程，该定点的移动轨迹是无规则的，显然是一个非线性的方程，那么就需要代入多个特征点来对该方程进行一个多次的迭代逼近我们所需要的方程曲线。

首先，我们需要将这个曲线方程与时间的关系表示出来。假设非线性系统的离散时间模型为：

X_k＝f(X_k-1)+B_k-1ω_k-1

y_k＝h(X_k)+v_k

然后再将相邻定点之间的递归关系代入到协方差矩阵中，即：

其中，表示矩阵X_k在k位置的变化率，上标数字表示迭代次数。

通过对上面的协方差矩阵进行迭代运算，可以得到最后的递归关系式为：

上式为将迭代的关系式代入卡尔曼滤波方程后的表达式，/>与P_k|k是根据/>表达式推导出来的姿态与位置矩阵，其中/>

经过以上方法拟合得到的曲线可能存在节点不够平滑的问题，因此需要将取得的非线性系统用容积Rauch-Tung-Striebel平滑器来进行平滑处理，也就是将上面的递归关系式代入到下面这个正向数据处理方程组里：

其中

这样，我们最后得到的关于定位点的递归关系式就是经过优化后的最后的非线性曲线表达式：

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

Claims (6)

1.一种基于视觉建模的惯性定位的自校正方法，其特征在于，它包括以下步骤：

S1、惯性设备采集第一数据，同时视觉建模设备采集第二数据，所述第一数据包括人员对应的位置、速度以及姿态，所述第二数据包括现场的画面图像；

S2、根据步骤S1采集到的所述第一数据和所述第二数据，计算得到惯性定位的误差向量与视觉建模的特征点坐标向量，然后根据所述误差向量与所述特征点向量得到动点的状态方程；

S3、根据采集到的第一数据、所述第二数据以及状态方程，得到所述视觉建模中的动点与特征点之间的第一距离，同时得到惯性定位中的动点与特征点的第二距离，然后根据所述第一距离和所述第二距离的均方差得到动点的观测方程；

S4、将步骤S2得到的所述状态方程与步骤S3得到的所述观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；

S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。

2.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S5之后还需要进行平滑处理。

3.根据权利要求2所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：所述平滑处理是指采用容积Rauch-Tung-Striebel平滑器进行处理。

4.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S2中的所述误差向量包括姿态误差、位置误差、速度误差、加速度计误差以及陀螺仪误差。

5.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S2中得到的所述特征点的具体方法包括以下步骤：

S21、将采集的所述画面图像进行灰度化和模糊化处理；

S22、采用黑塞矩阵对处理后的所述画面图像中各像素点进行偏导处理，获得突变点；

S23、将突变点与其周围26个像素点进行比较，保留其中的极值点作为特征点。

6.根据权利要求5所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：得到所述特征点后还需要进行特征点筛选。