CN107289933B - 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 - Google Patents
基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 Download PDFInfo
- Publication number
- CN107289933B CN107289933B CN201710505805.5A CN201710505805A CN107289933B CN 107289933 B CN107289933 B CN 107289933B CN 201710505805 A CN201710505805 A CN 201710505805A CN 107289933 B CN107289933 B CN 107289933B
- Authority
- CN
- China
- Prior art keywords
- moment
- state
- follows
- vector
- ekf
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
本发明公开了一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法，包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块以及测姿定位双卡尔曼滤波器；对于测姿滤波器，基于惯导机械编排的误差方程作为系统方程，观测方程包括加速度计观测量更新和磁力计观测量更新，输出姿态信息给VLC定位模块和PDR定位模块以校正姿态的影响。对于定位滤波器，二维平面的位置信息作为系统状态向量，基于行人航位推算的误差方程作为系统方程，而VLC的定位结果为观测方程。本发明的技术方案解决VLC定位容易受设备姿态影响以及在光信号被遮挡情况下定位不连续的问题，消除姿态对VLC定位的影响。
Description
技术领域
本发明涉及导航定位装置和方法，特别是涉及一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法。
背景技术
随着室内定位技术的发展和应用，基于可见光通信的室内定位技术也在迅速兴起并得到广泛关注。在充足室内光源的环境下，通过光传感器等设备检测得到经过复用协议调制的光信号，通过信号解调技术可以将不同光源信号数据分离，从而结合环境参数可以计算出定位目标相对每个光源的距离或角度信息，最后通过定位算法如三边定位可以完成目标定位。
然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对VLC定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。
但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差。3)在信号被遮挡的情况频繁发生的场景中，仅通过VLC数据加滤波器的定位系统推测的目标位置仍然与实际位置有较大偏差，VLC定位结果不连续、不平滑；另外，在惯导定位技术中，多采用一个卡尔曼滤波来进行噪声处理和定位，然而这样通过单一滤波器同时定位和测姿会造成姿态和位置相互影响。
发明内容
发明目的：为解决现有技术的不足，提供一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法。
技术方案：基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置，包括 MEMS传感器、INS模块、PDR定位模块、VLC定位模块、测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；
测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ 方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；其中，加速度信息和角速度信息按时间轴顺序输入至测姿扩展卡尔曼滤波器；输出包括传递给 PDR 定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪声补偿的陀螺偏差向量；
定位扩展卡尔曼滤波器的输入包括PDR定位模块输出的PDR位置信息和VLC定位模块输出的位置信息，PDR定位模块的输入包括采集的接收器在XYZ方向的加速度信息和所述测姿扩展卡尔曼滤波器输出的姿态信息；定位扩展卡尔曼滤波器输出接收器的定位信息。
一种基于所述导航装置的导航方法，包括以下步骤：
(1)估计接收器姿态信息
(11)建立A-EKF的状态向量；
(12)建立A-EKF的系统模型；
(13)建立A-EKF的观测模型；
(14)S-EKF滤波输出姿态信息；
(2)估计接收器位置信息
(21)建立L-EKF的状态向量；
(22)建立L-EKF的系统模型；
(23)建立L-EKF的观测模型；
(24)L-EKF滤波输出定位信息。
进一步的，所述步骤(11)中A-EKF的状态向量定义为：
x＝[ψ bg]T
其中：ψ为姿态误差向量；bg为陀螺偏差向量。
进一步的，所述步骤(12)中先对姿态矩阵进行坐标系变换，其坐标转移方程为：
其中，是从载体坐标系到导航坐标系的方向余弦矩阵，即姿态矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；
A-EKF的系统模型为具体展开公式如下：
其中，和分别表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。
进一步的，所述步骤(13)中A-EKF的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为Z＝HX+V，其中，
加速度计观测模型具体为：
其中， 是加速度计读数矢量,fn＝-gn＝[0 0 -g]T是导航坐标系中的特定力矢量，fb是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差, I是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gn×]表示gn矢量的斜对称矩阵，此时，H＝-[gn×]，
磁力计观测模型具体为：
其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声，此时，H＝[mn×]，
进一步的，所述步骤(14)包括：
(a)将系统模型根据卡尔曼滤波的模型形式进行变换成如下形式：
其中，
(b)观测模型Z＝HX+V对应以下方程：
加速计观测方程为：
此时，H＝[-[gn×] 03×3]，
磁力计观测方程为：
此时，H＝[[mn×] 03×3]，
其中，两个观测方程根据时间轴顺序参与到算法中；
(c)测姿扩展卡尔曼滤波递推步骤如下
①设置初值
令协方差矩阵P的初始值为：
P0＝diag([var(ψ0) var(bg0)])
其中，ψ0和bg0分别表示向量ψ和bg的初始值，P0表示各个向量初始值的协方差构成的对角矩阵；
设状态量X初始值为0向量；
②利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
Pk′＝FPk-1FT+Q
其中，Pk-1表示k-1时刻的协方差矩阵，Pk′表示k时刻的协方差矩阵的预测值， Q＝E[(GW)(GW)T]；
③利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，表示k-1时刻的状态量，表示k时刻状态量的预测值；
④计算k时刻的卡尔曼增益Kk′，计算公式如下：
Kk′＝Pk′HT(HPk′HT+R)-1
其中，R＝E[VVT]；
⑤利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量则：
⑥利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，则：
Pk＝(I-KkH)Pk′；
⑦输出k时刻状态量中的姿态信息给VLC定位模块和PDR定位模块，状态量中的陀螺偏差信息反馈给陀螺仪，同时，令k＝k+1，返回步骤②继续循环滤波不断更新状态量。
进一步的，所述步骤(21)中L-EKF的状态向量定义为：
其中，和λ分别为接收器纬度信息和经度信息。
进一步的，所述步骤(22)中使用PDR定位模块处理加速度计读数获得步数和步长信息，并从A-EKF获得方位角信息，然后PDR通过上一时刻的位置接收器方位角和接收器步长来计算当前时刻新的位置其系统模型为：
其中和是k-1时刻的方位角，sk是k时刻的步长，下标k-1和k代表步数，Rm和 Rn分别是是地球的主垂直线中的子午线和曲率的曲率半径，h是高度。
进一步的，所述步骤(23)中L-EKF的观测模型直接使用VLC输出的位置信息建立，观测模型如下：
其中，和分别为PDR预测的纬度和经度，和分别为VLC输出的纬度和经度，和nλ是测量噪声。
进一步的，所述步骤(24)包括：
(a)L-EKF系统模型的简化形式为：
xk＝f(xk-1,uk,wk)
其中，
(b)L-EKF观测模型的简化形式为：
zk＝h(xk,vk)
其中，
(c)定位扩展卡尔曼滤波的递推步骤如下：
①设置初值
令协方差矩阵P的初始值为：
其中，P0为状态量和状态量λk的方差构成的对角矩阵；设状态量x初始为0向量；
②利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，xk-1表示k-1时刻的状态量，表示k时刻状态量的预测值；
③利用雅可比矩阵进行更新系统模型，公式如下：
④利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
其中，Pk-1表示k-1时刻的协方差矩阵，Pk′表示k时刻的协方差矩阵的预测值， Qk＝E[(GW)(GW)T]，GW＝wk；
⑤利用雅可比矩阵进行更新观测模型，公式如下：
⑥计算k时刻的卡尔曼增益Kk
其中，Kk表示k时刻的卡尔曼增益，Rk＝E[VVT]，V＝vk；
⑦利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量
其中，
⑧利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，计算公式为：
Pk＝(I-KkHk)Pk′。
⑨最终输出接收器的定位信息，并令k＝k+1，返回步骤②继续循环滤波不断更新状态量。
有益效果：与现有技术相比，本发明具有以下优点：1)将测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器融合首次在VLC定位领域使用融合测姿准确估计VLC接收器的姿态信息，并消除姿态对VLC定位的影响；2)该融合滤波器设计可弥补VLC定位结果不连续、不平滑的缺点；3)该融合滤波器设计可在VLC信号被遮挡的情况下使用 MEMS传感器信息提供定位结果。另外，测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器融合的双滤波器架构具有的优势是将姿态和位置分开估计，避免它们的误差相互影响。同时，每个滤波器的结构又相对简单，便与调试；算法简单效率高。
附图说明
图1是测姿定位双卡尔曼滤波器导航装置结构示意图；
图2是俯仰角0°的仿真定位结果CDF曲线；
图3是俯仰角5°的仿真定位结果CDF曲线；
图4是俯仰角8°的仿真定位结果CDF曲线；
图5是接收设备平放、倾斜5°、倾斜8°时的定位误差分析结果。
具体实施方式
下面结合附图对本发明的技术方案进行详细的描述。
如图1所示，基于MEMS传感器和VLC定位融合的双卡尔曼滤波器导航装置，其特征在于：包括MEMS传感器、惯性导航系统(Inertial Navigation System，INS)模块、行人航位推算(Pedestrian Dead Reckoning，PDR)定位模块、可见光通信(Visible LightCommunication，VLC)定位模块、测姿扩展卡尔曼滤波器(Attitude Extended KalmanFilter，A-EKF)和定位扩展卡尔曼滤波器(Location Extended Kalman Filter，L-EKF)；所述MEMS传感器包括加速度计、陀螺仪以及磁力计。
陀螺仪测得的接收器在XYZ方向的角速度信息传递给INS模块，通过陀螺机械编排算法处理数据得到INS姿态信息。加速度计测得的接收器在XYZ方向的加速度信息传递给PDR定位模块进行位置估计。另一方面，加速度计测得的接收器在XYZ方向的加速度信息、磁力计测得的接收器相对东南西北方向的角度信息与INS姿态信息三者作为测姿扩展卡尔曼滤波器的输入量。测姿扩展卡尔曼滤波器输出包含姿态误差向量和陀螺偏差向量。陀螺偏差向量反馈给陀螺仪进行噪声补偿，而姿态误差向量传递给VLC 定位模块和PDR定位模块帮助定位。VLC定位模块和PDR定位模块通过各自的定位算法分别输出接收器位置信息给定位扩展卡尔曼滤波器。定位扩展卡尔曼滤波器会持续更新状态量并输出接收器的定位信息(位置、速度)。该装置将测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器融合形成融合滤波器。
系统中有两点需要注意：对于A-EKF，磁力计和加速计的读数会按时间轴顺序参与到测姿扩展卡尔曼滤波器的观测方程中，并非同时参与滤波；对于L-EKF，从PDR定位模块获得的位置信息作为定位扩展卡尔曼滤波器预测和更新的状态量，而从VLC定位模块获得的位置信息作为观测量参与到定位扩展卡尔曼滤波器中。
测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ 方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；输出包括传递给PRD定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪声补偿的陀螺偏差向量。
定位扩展卡尔曼滤波器的输入包括PDR定位模块输出的PDR位置信息和VLC定位模块输出的位置信息，PDR定位模块的输入包括采集的接收器在XYZ方向的加速度信息和所述测姿扩展卡尔曼滤波器输出的姿态信息；定位扩展卡尔曼滤波器输出接收器的定位信息。
本发明基于INS惯导机械编排的误差方程作为融合滤波器的系统方程，观测方程包括VLC定位信息更新、PDR定位信息更新和磁力计观测量更新。融合滤波器输出VLC 接收器的姿态信息给VLC定位模块，输出PDR接收器的姿态信息给PDR定位模块以校正姿态的影响。
一种基于所述导航装置的导航方法，包括以下步骤：
步骤1：估计接收器姿态信息
A-EKF主要用来估计接收器的姿态信息(即：横滚角、俯仰角和方位角)。
(1)建立A-EKF的状态向量
A-EKF的状态向量定义为：
x＝[ψ bg]T (1)
其中：ψ为姿态误差向量；bg为陀螺偏差向量。
(2)建立A-EKF的系统模型
先对姿态矩阵进行坐标系变换，其坐标转移方程为：
其中，是从载体坐标系(b-frame)到导航坐标系(n-frame)的方向余弦矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系(b-frame) 相对于惯性坐标(i-frame)的转动角速度和导航坐标系(n-frame)相对于惯性坐标(i-frame)的转动角速度。
A-EKF的系统模型为具体展开公式如下：
其中，和表示地球自传的角速度和导航坐标系(n-frame)相对于地心地固坐标系(ECEF坐标系，e-frame)的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。
(3)建立A-EKF的观测模型
A-EKF的观测模型直接使用加速度计和磁力计读数建立。本发明通过使用加速度计读数来构建观测模型，而不是应用加速度计导出的横滚角和俯仰角。这个选择对于具有任意接收器姿态的行人应用是重要的，因为它避免了当俯仰角达到±90°时的奇点问题。
A-EKF的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为 Z＝HX+V其中：
加速度计观测模型具体为：
其中， 是加速度计读数矢量,fn＝-gn＝[0 0 -g]T是导航坐标系中的特定力矢量，fb是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差, I是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gn×]表示gn矢量的斜对称矩阵，此时H＝-[gn×]，
A-EKF使用磁力计读数建立观测模型的主要挑战在于存在频繁的磁扰动。典型类型的磁扰动是局部磁场(Local Magnetic Field,LMF)的方向和强度都改变，但是该改变在有限的空间(或时间段)内是稳定的。LMF稳定的周期可以称为准静态磁场(Quasi-StaticMagnetic Field,QSMF)周期，并且可以通过使用磁力计读数的大小来检测。本发明所使用的算法将使用磁力计在QSMF期间的观测值，假设完全不知道LMF参数，并通过算法在每个QSMF周期的开始阶段校准LMF。第k个QSMF周期期间的LMF向量下列公式计算：
其中，是在第k个QSMF周期的第一个时期的开始处的磁力计读数，然后将计算的用作第k个QSMF周期期间的参考。本发明直接使用磁力计读数建立观测模型，该策略避免了调平步骤(即：使用加速度计读数来计算横滚角和俯仰角)。因此，下面列出的磁力计测量模型与加速度计测量值无关。
磁力计观测模型具体为：
其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声，此时，H＝[mn×]，
(4)A-EKF滤波输出姿态信息
为了进一步实现扩展卡尔曼滤波，需要将系统模型(公式(3))和观测模型(公式(4)和(6))代入到扩展卡尔曼滤波的时间更新方程和测量更新方程。首先先将上述中系统模型和观测模型根据卡尔曼滤波的模型形式进行变换，从而与模型中的变量一一对应。
(a)将系统模型根据卡尔曼滤波的模型形式进行变换成如下形式：
其中，
(b)观测模型Z＝HX+V对应以下方程：
加速计观测方程为：
此时，H＝[-[gn×] 03×3]，
磁力计观测方程为：
此时，H＝[[mn×] 03×3]，
其中，由于加速度计和磁力计从测量到返回读数不一定同步，且和二者的读数频率有关，所以二者作为输入量在时间上是错开的。同时A-EKF滤波器在一个时刻只能接收一个观测方程，即两个观测方程根据时间轴顺序参与到算法中。
确定好各个变量以后，就可以按照扩展卡尔曼滤波的时间更新方程和测量更新方程递推进行滤波。
(c)测姿扩展卡尔曼滤波递推步骤如下
①设置初值
令协方差矩阵P的初始值为：
P0＝diag([var(ψ0) var(bg0)]) (10)
其中，ψ0和bg0分别表示向量ψ和bg的初始值，P0表示各个向量初始值的协方差构成的对角矩阵；
设状态量X初始值为0向量；
②利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
Pk′＝FPk-1FT+Q (11)
其中，Pk-1表示k-1时刻的协方差矩阵，Pk′表示k时刻的协方差矩阵的预测值， Q＝E[(GW)(GW)T]；
③利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，表示k-1时刻的状态量，表示k时刻状态量的预测值；
④计算k时刻的卡尔曼增益Kk′，计算公式如下：
Kk′＝Pk′HT(HPk′HT+R)-1 (13)
其中，R＝E[VVT]；
⑤利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量则：
⑥利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，则：
Pk＝(I-KkH)Pk′ (15)
⑦输出k时刻状态量中的姿态信息给VLC定位模块和PDR定位模块，状态量中的陀螺偏差信息反馈给陀螺仪，同时，令k＝k+1，返回步骤②继续循环滤波不断更新状态量。
其中，步骤②、③中的方程为时间更新方程，④、⑤、⑥中为测量更新方程。
步骤2：估计接收器位置信息
L-EKF主要用来估计接收器的二维位置信息(即：纬度和经度)。图1显示L-EKF 的输入为1)PDR得出的位置信息和2)VLC得出的位置信息；输出为接收器的位置信息。
(1)建立L-EKF的状态向量
L-EKF的状态向量定义为：
其中，和λ分别为接收器纬度信息和经度信息。
(2)建立L-EKF的系统模型
使用行人航位推算PDR定位模块处理加速度计读数获得步数和步长信息，并从 A-EKF获得方位角信息，然后PDR通过上一时刻的位置接收器方位角和接收器步长来计算当前时刻的位置其系统模型为：
其中表示k-1时刻的方位角，sk表示k时刻的步长，Rm和Rn分别表示地球的主垂直线中的子午线和曲率的曲率半径，h是高度，和wλ为位置噪声。
(3)建立L-EKF的观测模型
L-EKF的观测模型直接使用VLC输出的位置信息建立，观测模型如下：
其中，和分别为PDR预测的纬度和经度，和分别为VLC输出的纬度和经度，和nλ是测量噪声。
(4)L-EKF滤波输出定位信息
为了进一步实现扩展卡尔曼滤波，需要将L-EKF系统模型公式和观测模型公式代入到定位扩展卡尔曼滤波的时间更新方程和测量更新方程。
L-EKF系统模型公式是对离散过以后的形式，所以滤波过程与A-EKF略有差异。滤波过程包括确定系统模型和观测模型并进行递推滤波步骤：
(41)L-EKF系统模型的简化形式为：
xk＝f(xk-1,uk,wk) (19)
其中，
(42)L-EKF观测模型的简化形式为：
zk＝h(xk,vk) (20)
其中，
(43)定位扩展卡尔曼滤波的递推步骤如下：
(a)设置初值
令协方差矩阵P的初始值为：
其中，P0为状态量和状态量λk的方差构成的对角矩阵；设状态量x初始为0向量；
(b)利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，xk-1表示k-1时刻的状态量，表示k时刻状态量的预测值；
(c)利用雅可比矩阵进行更新系统模型，公式如下：
(d)利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
其中，Pk-1表示k-1时刻的协方差矩阵，Pk′表示k时刻的协方差矩阵的预测值， Qk＝E[(GW)(GW)T]，GW＝wk；
(e)利用雅可比矩阵进行更新观测模型，公式如下：
(f)计算k时刻的卡尔曼增益Kk
其中，Kk表示k时刻的卡尔曼增益，Rk＝E[VVT]，V＝vk；
(g)利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k 时刻的状态量
其中，
(h)利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，计算公式为：
Pk＝(I-KkHk)Pk′ (28)
(i)最终输出接收器的定位信息，并令k＝k+1，返回步骤(b)继续循环滤波不断更新状态量。
其中，从步骤(b)到(d)为时间更新过程，步骤(c)到(h)为测量更新方程.
技术原理：
在VLC定位模块中，定位算法一般采用三边定位算法。其原理是利用室内顶端的多盏灯(一般至少有三盏LED灯)作为发射源，通过接收器实时测得的光信号强度代入光信号传播模型估计出目标点到每盏灯的距离，最后通过联立方程估算出定位位置。然而，当光信号被遮挡时，探测器无法获取全部信号，求解出的位置会严重超出误差范围，这种情况会通过阈值进行排除，所以系统在该情况下不输出位置信息，导致定位结果不连续。另外，考虑到VLC系统往往存在散粒噪声和热噪声等随机噪声，距离的估计往往存在一定误差，因此估得的位置形成的运动轨迹会存在陡峭变化等特征，这往往不符合实际目标的运动特征。由于在目标跟踪时，先验知识应当表示定位轨迹平滑，目标当前时刻的状态与上一时刻的状态相关，而滤波方法可以将先验值考虑进来，从而使得定位轨迹更加平滑。
一般的VLC定位往往假设设备是平放姿态，从而简化了定位算法。然而在实际情况中设备会产生抖动、倾斜。设备的随机倾斜会改变光信号的入射角，从而导致测得的光信号强度发生变化。如果无法准确估计设备的姿态，这部分的光信号强度变化将成为系统误差的一部分，导致定位误差变大。为了说明姿态误差对于VLC定位精度的影响程度，我们通过对比仿真了不同俯仰角的情况下VLC定位的精度。该仿真实验对接收器发生倾斜，但定位算法不校正的情况进行了模拟，对所有接收器仿真了两种不同的倾斜情况，定位结果CDF曲线分别如图3和图4所示(图2表示设备未发生倾斜的定位结果)。实验结果显示接收器俯仰角为5°和8°时最大定位误差分别约为0.13m和0.21m。对比接收器倾斜和平放时的定位结果(如图5所示)，可以看出当接收器发生了倾斜，如果不对接收器姿态进行修正，定位误差会显著的增大。该仿真结果证明准确估计接收器的姿态对提高可见光定位系统的性能具有重要意义。因此，引入MEMS姿态估计模块，将准确的姿态信息传递给VLC定位模块进行姿态校准，能够显著的减小姿态对VLC 定位结果的影响。
由于光信号在被遮挡的情况下VLC定位无法进行，因此引入PDR模块进行定位。PDR定位可以完全利用MEMS传感器信号来计算步长和方向，利用行人上一时刻的位置推测出行人当前的位置。通过反复迭代，即可得到行人的运动轨迹。因此，当VLC 定位发生信号遮挡时(无法输出位置结果)，融合系统会采用PDR模块的定位结果进行弥补。
单滤波器结构姿态和位置误差间相互影响：由于滤波器中的系统模型利用了惯导定位的位置预测公式。而在惯导中，速度误差矢量的状态转移包含姿态变量，位置误差矢量的状态转移又和速度相关。因此，当采用单滤波器的架构时，姿态和位置信息同时预测估计，必然会造成姿态的预测估计误差影响位置信息的预测。所以本发明通过采用双滤波器架构将姿态和位置的预测估计过程解耦，避免两者之间相互影响。
Claims (10)
1.基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块、测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；
测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；其中，加速度信息和角速度信息按时间轴顺序输入至测姿扩展卡尔曼滤波器；输出包括传递给PDR 定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪声补偿的陀螺偏差向量；
定位扩展卡尔曼滤波器的输入包括PDR定位模块输出的PDR位置信息和VLC定位模块输出的位置信息，PDR定位模块的输入包括采集的接收器在XYZ方向的加速度信息和所述测姿扩展卡尔曼滤波器输出的姿态信息；定位扩展卡尔曼滤波器输出接收器的定位信息。
2.一种基于权利要求1所述导航装置的导航方法，其特征在于，包括以下步骤：
(1)估计接收器姿态信息
(11)建立测姿扩展卡尔曼滤波器A-EKF的状态向量；
(12)建立测姿扩展卡尔曼滤波器A-EKF的系统模型；
(13)建立测姿扩展卡尔曼滤波器A-EKF的观测模型；
(14)测姿扩展卡尔曼滤波器A-EKF滤波输出姿态信息；
(2)估计接收器位置信息
(21)建立定位扩展卡尔曼滤波器L-EKF的状态向量；
(22)建立定位扩展卡尔曼滤波器L-EKF的系统模型；
(23)建立定位扩展卡尔曼滤波器L-EKF的观测模型；
(24)定位扩展卡尔曼滤波器L-EKF滤波输出定位信息。
3.根据权利要求2所述的导航方法，其特征在于，所述步骤(11)中A-EKF的状态向量定义为：
x＝[ψ bg]T
其中：ψ为姿态误差向量；bg为陀螺偏差向量。
4.根据权利要求3所述的导航方法，其特征在于，所述步骤(12)中先对姿态矩阵进行坐标系变换，其坐标转移方程为：
其中，是从载体坐标系到导航坐标系的方向余弦矩阵，即姿态矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；
A-EKF的系统模型为具体展开公式如下：
其中，和分别表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。
5.根据权利要求4所述的导航方法，其特征在于，所述步骤(13)中A-EKF的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为Z＝HX+V，其中，
加速度计观测模型具体为：
其中， 是加速度计读数矢量,fn＝-gn＝[0 0 -g]T是导航坐标系中的特定力矢量，fb是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差,I是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gn×]表示gn矢量的斜对称矩阵，此时，H＝-[gn×]，
磁力计观测模型具体为：
其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声，此时，H＝[mn×]，
6.根据权利要求5所述的导航方法，其特征在于，所述步骤(14)包括：
(a)将系统模型根据卡尔曼滤波的模型形式进行变换成如下形式：
其中，
(b)观测模型Z＝HX+V对应以下方程：
加速计观测方程为：
此时，H＝[-[gn×] 03×3]，
磁力计观测方程为：
此时，H＝[[mn×] 03×3]，
其中，两个观测方程根据时间轴顺序参与到算法中；
(c)测姿扩展卡尔曼滤波递推步骤如下
①设置初值
令协方差矩阵P的初始值为：
P0＝diag([var(ψ0) var(bg0)])
其中，ψ0和bg0分别表示向量ψ和bg的初始值，P0表示各个向量初始值的协方差构成的对角矩阵；
设状态量X初始值为0向量；
②利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
P′k＝FPk-1FT+Q
其中，Pk-1表示k-1时刻的协方差矩阵，P′k表示k时刻的协方差矩阵的预测值，Q＝E[(GW)(GW)T]；
③利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，表示k-1时刻的状态量，表示k时刻状态量的预测值；
④计算k时刻的卡尔曼增益K′k，计算公式如下：
K′k＝P′kHT(HP′kHT+R)-1
其中，R＝E[VVT]；
⑤利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量则：
⑥利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，则：
Pk＝(I-KkH)P′k；
⑦输出k时刻状态量中的姿态信息给VLC定位模块和PDR定位模块，状态量中的陀螺偏差信息反馈给陀螺仪，同时，令k＝k+1，返回步骤②继续循环滤波不断更新状态量。
7.根据权利要求6所述的导航方法，其特征在于，所述步骤(21)中L-EKF的状态向量定义为：
其中，和λ分别为接收器纬度信息和经度信息。
8.根据权利要求7所述的导航方法，其特征在于，所述步骤(22)中使用PDR定位模块处理加速度计读数获得步数和步长信息，并从A-EKF获得方位角信息，然后PDR通过上一时刻的位置接收器方位角和接收器步长来计算当前时刻新的位置其系统模型为：
其中和是k-1时刻的方位角，sk是k时刻的步长，下标k-1和k代表步数，Rm和Rn分别是是地球的主垂直线中的子午线和曲率的曲率半径，h是高度。
9.根据权利要求8所述的导航方法，其特征在于，所述步骤(23)中L-EKF的观测模型直接使用VLC输出的位置信息建立，观测模型如下：
其中，和分别为PDR预测的纬度和经度，和分别为VLC输出的纬度和经度，和nλ是测量噪声。
10.根据权利要求9所述的导航方法，其特征在于，所述步骤(24)包括：
(a)L-EKF系统模型的简化形式为：
xk＝f(xk-1,uk,wk)
其中，
(b)L-EKF观测模型的简化形式为：
zk＝h(xk,vk)
其中，
(c)定位扩展卡尔曼滤波的递推步骤如下：
①设置初值
令协方差矩阵P的初始值为：
其中，P0为状态量和状态量λk的方差构成的对角矩阵；设状态量x初始为0向量；
②利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
其中，xk-1表示k-1时刻的状态量，表示k时刻状态量的预测值；
③利用雅可比矩阵进行更新系统模型，公式如下：
④利用k-1时刻的协方差矩阵对k时刻的协方差矩阵进行预测，预测公式为：
其中，Pk-1表示k-1时刻的协方差矩阵，P′k表示k时刻的协方差矩阵的预测值，Qk＝E[(GW)(GW)T]，GW＝wk；
⑤利用雅可比矩阵进行更新观测模型，公式如下：
⑥计算k时刻的卡尔曼增益Kk
其中，Kk表示k时刻的卡尔曼增益，Rk＝E[VVT]，V＝vk；
⑦利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量
其中，
⑧利用k时刻的卡尔曼增益估计k时刻的协方差矩阵的最优值，令该最优值作为k时刻的协方差矩阵Pk，计算公式为：
Pk＝(I-KkHk)P′k；
⑨最终输出接收器的定位信息，并令k＝k+1，返回步骤②继续循环滤波不断更新状态量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710505805.5A CN107289933B (zh) | 2017-06-28 | 2017-06-28 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710505805.5A CN107289933B (zh) | 2017-06-28 | 2017-06-28 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107289933A CN107289933A (zh) | 2017-10-24 |
| CN107289933B true CN107289933B (zh) | 2019-08-20 |
Family
ID=60099371
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710505805.5A Active CN107289933B (zh) | 2017-06-28 | 2017-06-28 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107289933B (zh) |
Families Citing this family (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108507561A (zh) * | 2018-03-05 | 2018-09-07 | 华南理工大学 | 一种基于移动终端的vlc与imu融合定位方法 |
| CN108279008A (zh) * | 2018-03-07 | 2018-07-13 | 华南理工大学 | 一种基于卡尔曼滤波的复杂场景下的led灯追踪方法及其系统 |
| CN110361003B (zh) * | 2018-04-09 | 2023-06-30 | 中南大学 | 信息融合方法、装置、计算机设备和计算机可读存储介质 |
| CN109655057B (zh) * | 2018-12-06 | 2021-05-25 | 深圳市吉影科技有限公司 | 一种六推无人机加速器测量值的滤波优化方法及其系统 |
| CN109765523B (zh) * | 2018-12-21 | 2022-11-01 | 山东省科学院海洋仪器仪表研究所 | 基于自适应akf的单应答器斜距水声定位方法及系统 |
| CN110672095A (zh) * | 2019-10-17 | 2020-01-10 | 北京首贝科技发展有限公司 | 一种基于微惯导的行人室内自主定位算法 |
| CN111086001B (zh) * | 2019-12-25 | 2021-09-14 | 广东省智能制造研究所 | 一种足式机器人多模态感知的状态估计方法及系统 |
| CN113438615A (zh) * | 2020-03-04 | 2021-09-24 | 北京京东乾石科技有限公司 | 定位方法和装置 |
| CN111854762A (zh) * | 2020-08-05 | 2020-10-30 | 翟瑞永 | 一种基于卡尔曼滤波算法的三维定位方法及其定位系统 |
| CN111896008A (zh) * | 2020-08-20 | 2020-11-06 | 哈尔滨工程大学 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
| CN112179332A (zh) * | 2020-09-30 | 2021-01-05 | 劢微机器人科技(深圳)有限公司 | 无人叉车混合定位方法及系统 |
| CN112362052B (zh) * | 2020-10-27 | 2022-09-16 | 中国科学院计算技术研究所 | 一种融合定位方法及系统 |
| CN112737547A (zh) * | 2020-12-18 | 2021-04-30 | 北京邮电大学 | 一种基于卡尔曼滤波的信号补偿方法及装置 |
| CN113267178B (zh) * | 2021-03-25 | 2023-07-07 | 浙江大学 | 一种基于多传感器融合的模型位姿测量系统与方法 |
| CN114383605B (zh) * | 2021-12-03 | 2024-04-02 | 理大产学研基地(深圳)有限公司 | 基于mems传感器和稀疏地标点的室内定位及优化方法 |
| CN114485574B (zh) * | 2021-12-21 | 2023-03-21 | 武汉大学 | 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法 |
| CN114413885A (zh) * | 2021-12-22 | 2022-04-29 | 华人运通(上海)自动驾驶科技有限公司 | 基于多传感器融合定位的时间同步方法及系统 |
| CN114877892A (zh) * | 2022-07-11 | 2022-08-09 | 泉州通维科技有限责任公司 | 一种用于光伏机器人的融合定位方法 |
| CN115061175A (zh) * | 2022-07-28 | 2022-09-16 | 知微空间智能科技(苏州)有限公司 | Gnss rtk与ins半紧组合定位导航方法、装置和系统 |
| CN115790615B (zh) * | 2023-01-30 | 2023-05-05 | 安徽蔚来智驾科技有限公司 | 基于自动驾驶的多传感器融合方法、装置、介质及车辆 |
| CN117419745A (zh) * | 2023-10-13 | 2024-01-19 | 南京理工大学 | 一种基于循环ekf的惯性辅助地磁在线标定方法及系统 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103033186A (zh) * | 2012-12-30 | 2013-04-10 | 东南大学 | 一种用于水下滑翔器的高精度组合导航定位方法 |
| CN103175529A (zh) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | 基于室内磁场特征辅助的行人惯性定位系统 |
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN105043385A (zh) * | 2015-06-05 | 2015-11-11 | 北京信息科技大学 | 一种行人自主导航定位的自适应卡尔曼滤波方法 |
| CN105222772A (zh) * | 2015-09-17 | 2016-01-06 | 泉州装备制造研究所 | 一种基于多源信息融合的高精度运动轨迹检测系统 |
| WO2016040166A1 (en) * | 2014-09-08 | 2016-03-17 | Invensense, Inc. | Method and apparatus for using map information aided enhanced portable navigation |
| CN105424041A (zh) * | 2016-01-18 | 2016-03-23 | 重庆邮电大学 | 一种基于bd/ins紧耦合的行人定位算法 |
| CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位系统及方法 |
| CN105652306A (zh) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法 |
| CN106225784A (zh) * | 2016-06-13 | 2016-12-14 | 国家海洋局第二海洋研究所 | 基于低成本多传感器融合行人航位推算方法 |
-
2017
- 2017-06-28 CN CN201710505805.5A patent/CN107289933B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103033186A (zh) * | 2012-12-30 | 2013-04-10 | 东南大学 | 一种用于水下滑翔器的高精度组合导航定位方法 |
| CN103175529A (zh) * | 2013-03-01 | 2013-06-26 | 上海美迪索科电子科技有限公司 | 基于室内磁场特征辅助的行人惯性定位系统 |
| WO2016040166A1 (en) * | 2014-09-08 | 2016-03-17 | Invensense, Inc. | Method and apparatus for using map information aided enhanced portable navigation |
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN105043385A (zh) * | 2015-06-05 | 2015-11-11 | 北京信息科技大学 | 一种行人自主导航定位的自适应卡尔曼滤波方法 |
| CN105222772A (zh) * | 2015-09-17 | 2016-01-06 | 泉州装备制造研究所 | 一种基于多源信息融合的高精度运动轨迹检测系统 |
| CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位系统及方法 |
| CN105652306A (zh) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法 |
| CN105424041A (zh) * | 2016-01-18 | 2016-03-23 | 重庆邮电大学 | 一种基于bd/ins紧耦合的行人定位算法 |
| CN106225784A (zh) * | 2016-06-13 | 2016-12-14 | 国家海洋局第二海洋研究所 | 基于低成本多传感器融合行人航位推算方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107289933A (zh) | 2017-10-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107289933B (zh) | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 | |
| CN107270898B (zh) | 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法 | |
| CN107289932B (zh) | 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法 | |
| CN104121905B (zh) | 一种基于惯性传感器的航向角获取方法 | |
| CN110398257A (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
| Huang et al. | Synergism of INS and PDR in self-contained pedestrian tracking with a miniature sensor module | |
| CN106225784B (zh) | 基于低成本多传感器融合行人航位推算方法 | |
| CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
| CN105180937B (zh) | 一种mems‑imu初始对准方法 | |
| CN103389506B (zh) | 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法 | |
| CN107246872B (zh) | 基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法 | |
| Diaz et al. | Evaluation of AHRS algorithms for inertial personal localization in industrial environments | |
| CN110398245A (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
| CN105589064A (zh) | Wlan位置指纹数据库快速建立和动态更新系统及方法 | |
| CN109470241B (zh) | 一种具备重力扰动自主补偿功能的惯性导航系统及方法 | |
| CN108344415A (zh) | 一种组合导航信息融合方法 | |
| CN109931955A (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
| CN109916395A (zh) | 一种姿态自主冗余组合导航算法 | |
| US20110153266A1 (en) | Augmented vehicle location system | |
| CN108562917A (zh) | 附加正交函数拟合条件的约束滤波解算方法及装置 | |
| CN108931791A (zh) | 卫惯紧组合钟差修正系统和方法 | |
| CN110595503A (zh) | 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法 | |
| CN109945895A (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
| CN110440746A (zh) | 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法 | |
| CN110057356A (zh) | 一种隧道内车辆定位方法及装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |