CN108225330A - 一种基于卡尔曼滤波的可见光动态定位方法 - Google Patents

一种基于卡尔曼滤波的可见光动态定位方法 Download PDF

Info

Publication number
CN108225330A
CN108225330A CN201810003561.5A CN201810003561A CN108225330A CN 108225330 A CN108225330 A CN 108225330A CN 201810003561 A CN201810003561 A CN 201810003561A CN 108225330 A CN108225330 A CN 108225330A
Authority
CN
China
Prior art keywords
state
moment
information
visible ray
dynamic positioning
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.)
Pending
Application number
CN201810003561.5A
Other languages
English (en)
Inventor
吴玉香
刘幸烽
关伟鹏
谢灿宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN201810003561.5A priority Critical patent/CN108225330A/zh
Publication of CN108225330A publication Critical patent/CN108225330A/zh
Pending legal-status Critical Current

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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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

Abstract

本发明公开了一种基于卡尔曼滤波的可见光动态定位方法，包括以下步骤：LED阵列发送ID信息；接收端利用接收到LED阵列发送的ID信息和接收信号强度得到二维位置信息，实现初步定位；获取接收端的运动状态信息，包括速度信息，加速度信息；利用获得的数据，分别建立状态模型和观测模型；根据卡尔曼滤波方程，利用所得模型和前一次状态的估计值来预测和修正，得到当前状态的最优估计值，并与当前状态观测值比较，实现动态定位；输出当前动态定位状态的最优估计值。本发明通过采用卡尔曼滤波，提高了可见光的动态定位精度，实现接收端的追踪导航。

Description

一种基于卡尔曼滤波的可见光动态定位方法
技术领域
本发明涉及室内定位领域，尤其涉及一种基于卡尔曼滤波的可见光动态定位方法。
背景技术
随着时代的发展，室内定位技术在机器人定位导航、大型商场导购、大型工厂机器追踪等领域得到了广泛应用，成为重点研究对象之一。尽管全球定位系统(GPS)在室外环境中能够实现较高精度的定位，但在室内环境下，由于多径衰落的影响、其他无线设备的干扰以及建筑物的遮挡，GPS信号在室内被严重削弱，导致GPS定位精度低，稳定性差。为满足室内定位需求，国内外各机构展开了大量研究，涌现了很多新的定位技术，如红外线、超声波、蓝牙、超宽带、射频识别、Wi‐Fi等室内无线定位技术。然而，大多数基于无线通信的定位系统容易受到电磁干扰的影响，需要搭建复杂的定位设施环境，不仅成本高，定位精度和安全性也得不到有效保障。
目前，LED在室内外照明系统中得到了广泛应用，具有电压低、功率低、寿命长和易于小型化等优点；同时，白光LED具有高速调制及响应时间短等特性，从而使得LED的应用从照明领域扩展到通讯领域，能够同时实现照明和通信功能。基于白光LED的可见光通信(VLC)具有无电磁干扰、附加模块少、保密性好等优点。因此，将可见光应用于室内定位系统被视为有效的选择。近期，可见光定位技术飞速发展，定位精度已达到厘米级别。然而，多数可见光定位系统是静态定位，在定位目标运动的情况下，很难达到较高的定位精度，难以满足动态定位精度需求。
发明内容
为克服现有技术的不足，本发明提出了一种基于卡尔曼滤波的可见光动态定位方法，该方法通过采用卡尔曼滤波，提高了可见光的动态定位精度，实现接收端的追踪导航。
为达到上述目的，本发明采用的技术方案如下：
一种基于卡尔曼滤波的可见光动态定位方法，包括以下步骤：
步骤1、LED阵列发送身份识别信息；
步骤2、接收端利用接收到LED阵列发送的身份识别信息和接收信号强度得到二维位置信息(x，y)，实现初步定位；
步骤3、获取接收端的运动状态信息，包括速度信息(vx,vy)，加速度信息(ax,ay)；
步骤4、利用步骤2和3获得的数据，分别建立状态模型和观测模型；
步骤5、根据卡尔曼滤波方程，利用步骤4所得模型和前一次状态的估计值来预测和修正，得到当前状态的最优估计值，并与当前状态观测值比较，实现动态定位；
步骤6、输出当前动态定位状态的最优估计值。
具体地，步骤1所述的身份识别信息原始ID信息经过通断键控调制形成调制信号。
具体地，步骤2包括以下步骤：
步骤21、接收端的光电器件接收LED阵列发送的身份识别信息，检测不同LED的接收信号强度；
步骤22、解码接收到的身份识别信息，并计算接收端到LED阵列中各LED的距离；
步骤23、根据步骤22所得信息，运用接收信号强度三角定位算法得到接收端的二维坐标。
具体地，步骤3所述运动状态信息由陀螺仪、加速度传感器、方向传感器获得。
具体地，步骤4所述状态模型表示为：
X(k)＝A*X(k-1)+W(k-1)
式中，X(k)＝[x,y,vx,vy,ax,ay]T为k时刻系统的状态变量，A为系统6×6维状态转移矩阵，X(k-1)为(k-1)时刻接收端的状态变量，W(k-1)为(k-1)时刻系统过程噪声。
具体地，步骤4所述观测模型表示为：
Y(k)＝H*X(k)+V(k)
Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为表示k时刻系统观测变量，X(k)＝[x,y,x,vy,vx,ay]T为k时刻系统的状态变量，H为系统6×6维观测矩阵，V(k)为k时刻系统观测噪声。
具体地，步骤5中，预测过程如下：
其中，为k时刻预测状态变量，A为系统6×6维状态转移矩阵，为(k-1)时刻滤波最优估计状态变量，为k时刻预测估计值的协方差矩阵，P(k-1)为(k-1)时刻滤波估计状态向量的协方差矩阵，Q(k-1)为系统过程噪声W(k-1)的协方差矩阵。
具体地，步骤5中，修正过程如下：
Kg(k)＝P(k)*H*R(k)-1
其中，Kg(k)为k时刻的滤波增益矩阵，P(k)为k时刻滤波估计状态向量的协方差矩阵，H为系统6×6维观测矩阵，R(k)为系统观测噪声V(k的协方差矩阵，为k时刻预测估计值的协方差矩阵，为k时刻滤波最优估计状态变量，为k时刻预测状态变量，Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为k时刻系统观测变量。
本发明对于现有技术具有如下的优点及效果：
本发明只需要知道初始时刻的状态值和每个时刻的观测值，就能计算出每个时刻的最优估计值，通过卡尔曼滤波器将位置、速度、加速度信息融合起来，并且通过预测与更新来消除噪声，从而达到提高动态定位精度的目的。
附图说明
图1为可见光定位的原理示意图
图2为基于卡尔曼滤波的可见光动态定位方法步骤框图
图3为接收端移动平面示意图
具体实施方式
下面结合实施例及附图对本发明做进一步详细的解释，但本发明的实施方式不限于此。
实施例
目前现有技术的动态定位精度差，原因主要是接收端在移动过程中接收的信号掺了大量的噪声。要提高动态定位精度，可利用卡尔曼滤波器滤去噪声。卡尔曼滤波是一种递归估计,通过前一个时刻的最优估计值和当前的观测值就可以得到当前状态的最优估计值。
图1示出了一种基于卡尔曼滤波的可见光动态定位方法的系统模型。根据模型要求，室内需布置3盏以上的LED灯具，这些灯具构成LED阵列，LED阵列发送信号，接收端接收后，经过信号处理得到接收端的位置信息。
本实施例的可见光动态定位方法在动态定位初始阶段，将系统初次定位结果、速度信息、加速度信息设为系统初始值，接收端移动一定距离后，接收端通过接收信号强度三角定位方法进行初步定位，同时获取接收端的运动信息，作为当前时刻的观测值，输入到卡尔曼滤波器当中，经过预测和修正，得到当前状态的最优估计值，经过一定时间后，再次定位，如此重复，从而实现动态定位。
如图2所示，本实施例的可见光动态定位方法包括如下步骤：
步骤1、LED阵列发送ID(身份识别)信息。
具体地，初始的ID信息经过OOK(On‐Off Keying，OOK，通断键控)调制后，形成调制信号，调制信号再经过驱动电路驱动LED灯发射可见光信号。
步骤2、接收端利用接收到的ID信息和接收信号强度实现初步定位，从而得到二维位置信息(x,y)。
具体地，接收端的光电检测器件检测接收光强，解码，得到三个LED信息，通过相应公式计算得到接收端到三个LED的距离。根据已知信号，运用三角定位算法得到接收端的二维坐标。
步骤3、获取接收端的运动状态信息，包括速度信息(vx,vy)，加速度信息(ax,ay)。
具体地，运动信息由接收端的陀螺仪、加速度传感器、方向传感器得到。
步骤4、利用步骤2和3获得数据，分别建立状态模型和观测模型。
具体地，在本例中，接收端的运动模型是线性的，则有：
式中，X(k)＝[x,y,vx,vy,ax,ay]T为k时刻系统的状态变量，
为系统状态转移矩阵，X(k-1)为(k-1)时刻接收端的状态变量，W(k-1)为(k-1)时刻系统过程噪声，Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为k时刻系统观测变量，
为系统的观测矩阵，V(k)为k时刻系统观测噪声。
步骤5、根据卡尔曼滤波方程，利用步骤4所得模型和前一次状态的估计值来预测当前状态的最优估计值，并与当前状态观测值比较，实现动态定位。
具体地，步骤5中，预测过程如下：
其中，为k时刻预测状态变量，A为系统6×6维状态转移矩阵，具体可参见上述步骤4的A矩阵，为(k-1)时刻滤波最优估计状态变量，为k时刻预测估计值的协方差矩阵，P(k-1)为(k-1)时刻滤波估计状态向量的协方差矩阵，Q(k-1)为系统过程噪声W(k-1)的协方差矩阵。
修正过程如下：
Kg(k)＝P(k)*H*R(k)-1
其中，Kg(k)为k时刻的滤波增益矩阵，P(k)为k时刻滤波估计状态向量的协方差矩阵，H为系统6×6维观测矩阵，具体可参见上述步骤4的H矩阵，R(k)为系统观测噪声V(k)的协方差矩阵，为k时刻预测估计值的协方差矩阵，为k时刻滤波最优估计状态变量，为k时刻预测状态变量，Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为k时刻系统观测变量。
步骤6、输出当前状态的最优估计值。
综上所述，本发明只需要知道初始时刻的状态值和每个时刻的观测值，就能计算出每个时刻的最优估计值，通过卡尔曼滤波器将位置、速度、加速度信息融合起来，并且通过预测与更新来消除噪声，从而达到提高动态定位精度的目的。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (8)

1.一种基于卡尔曼滤波的可见光动态定位方法，其特征在于，包括以下步骤：
步骤1、LED阵列发送身份识别信息；
步骤2、接收端利用接收到LED阵列发送的身份识别信息和接收信号强度得到二维位置信息(x，y)，实现初步定位；
步骤3、获取接收端的运动状态信息，包括速度信息(vx,vy)，加速度信息(ax,ay)；
步骤4、利用步骤2和3获得的数据，分别建立状态模型和观测模型；
步骤5、根据卡尔曼滤波方程，利用步骤4所得模型和前一次状态的估计值来预测和修正，得到当前状态的最优估计值，并与当前状态观测值比较，实现动态定位；
步骤6、输出当前动态定位状态的最优估计值。
2.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤1所述的身份识别信息原始ID信息经过通断键控调制形成调制信号。
3.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤2包括以下步骤：
步骤21、接收端的光电器件接收LED阵列发送的身份识别信息，检测不同LED的接收信号强度；
步骤22、解码接收到的身份识别信息，并计算接收端到LED阵列中各LED的距离；
步骤23、根据步骤22所得信息，运用接收信号强度三角定位算法得到接收端的二维坐标。
4.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤3所述运动状态信息由陀螺仪、加速度传感器、方向传感器获得。
5.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤4所述状态模型表示为：
X(k)＝A*X(k-1)+W(k-1)
式中，X(k)＝[x,y,vx,vy,ax,ay]T为k时刻系统的状态变量，A为系统6×6维状态转移矩阵，X(k-1)为(k-1)时刻接收端的状态变量，W(k-1)为(k-1)时刻系统过程噪声。
6.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤4所述观测模型表示为：
Y(k)＝H*X(k)+V(k)
Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为表示k时刻系统观测变量，X(k)＝[x,y,vx,vy,ax,ay]T为是k时刻系统的状态变量，H为是系统6×6维观测矩阵，V(k)为是k时刻系统观测噪声。
7.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤5中，预测过程如下：
其中，为k时刻预测状态变量，A为系统6×6维状态转移矩阵，为(k-1)时刻滤波最优估计状态变量，为k时刻预测估计值的协方差矩阵，P(k-1)为(k-1)时刻滤波估计状态向量的协方差矩阵，Q(k-1)为系统过程噪声W(k-1)的协方差矩阵。
8.根据权利要求1所述的基于卡尔曼滤波的可见光动态定位方法，其特征在于，步骤5中，修正过程如下：
Kg(k)＝P(k)*H*R(k)-1
其中，Kg(k)为k时刻的滤波增益矩阵，P(k)为k时刻滤波估计状态向量的协方差矩阵，H为系统6×6维观测矩阵，R(k)为系统观测噪声V(k)的协方差矩阵，为k时刻预测估计值的协方差矩阵，为k时刻滤波最优估计状态变量，为k时刻预测状态变量，Y(k)＝[x′,y′,vx′,vy′,ax′,ay′]T为k时刻系统观测变量。
CN201810003561.5A 2018-01-03 2018-01-03 一种基于卡尔曼滤波的可见光动态定位方法 Pending CN108225330A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810003561.5A CN108225330A (zh) 2018-01-03 2018-01-03 一种基于卡尔曼滤波的可见光动态定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810003561.5A CN108225330A (zh) 2018-01-03 2018-01-03 一种基于卡尔曼滤波的可见光动态定位方法

Publications (1)

Publication Number Publication Date
CN108225330A true CN108225330A (zh) 2018-06-29

Family

ID=62642706

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810003561.5A Pending CN108225330A (zh) 2018-01-03 2018-01-03 一种基于卡尔曼滤波的可见光动态定位方法

Country Status (1)

Country Link
CN (1) CN108225330A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109129482A (zh) * 2018-08-29 2019-01-04 武汉理工大学 一种动态补偿机器人直线导轨运动误差的方法
CN109507639A (zh) * 2018-12-30 2019-03-22 广东大仓机器人科技有限公司 一种提高超宽带传感器的定位精度的方法
CN110673721A (zh) * 2019-08-29 2020-01-10 江苏大学 基于视觉与意念信号协同控制的机器人看护系统
CN112740759A (zh) * 2018-09-28 2021-04-30 谷歌有限责任公司 用于在可用网络之间主动切换的方法和装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101853243A (zh) * 2010-04-01 2010-10-06 西北工业大学 系统模型未知的自适应卡尔曼滤波方法
CN105674986A (zh) * 2016-04-05 2016-06-15 中国电子科技集团公司第二十研究所 一种可见光与惯性组合的室内定位方法
CN105871462A (zh) * 2016-05-17 2016-08-17 华南理工大学 基于cdma调制的可见光通信定位方法及其定位系统
CN106483500A (zh) * 2016-11-17 2017-03-08 华南理工大学 一种基于cdma调制的可见光通信三维定位方法
CN106872943A (zh) * 2017-01-03 2017-06-20 东南大学 一种基于可见光通信的移动物体室内跟踪定位方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101853243A (zh) * 2010-04-01 2010-10-06 西北工业大学 系统模型未知的自适应卡尔曼滤波方法
CN105674986A (zh) * 2016-04-05 2016-06-15 中国电子科技集团公司第二十研究所 一种可见光与惯性组合的室内定位方法
CN105871462A (zh) * 2016-05-17 2016-08-17 华南理工大学 基于cdma调制的可见光通信定位方法及其定位系统
CN106483500A (zh) * 2016-11-17 2017-03-08 华南理工大学 一种基于cdma调制的可见光通信三维定位方法
CN106872943A (zh) * 2017-01-03 2017-06-20 东南大学 一种基于可见光通信的移动物体室内跟踪定位方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
TANER MUTLU等: "《基于卡尔曼滤波的大气数据 GPS组合导航系统》", 30 June 2016, 国防工业出版社 *
YE CAI, ET AL: "Indoor High Precision Three-Dimensional Positioning System Based on Visible Light Communication Using Particle Swarm Optimization", 《IEEE PHOTONICS JOURNAL》 *
邹霞等: "《麦克风阵列信号处理》", 31 January 2016, 国防工业出版社 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109129482A (zh) * 2018-08-29 2019-01-04 武汉理工大学 一种动态补偿机器人直线导轨运动误差的方法
CN109129482B (zh) * 2018-08-29 2021-05-25 武汉理工大学 一种动态补偿机器人直线导轨运动误差的方法
CN112740759A (zh) * 2018-09-28 2021-04-30 谷歌有限责任公司 用于在可用网络之间主动切换的方法和装置
CN109507639A (zh) * 2018-12-30 2019-03-22 广东大仓机器人科技有限公司 一种提高超宽带传感器的定位精度的方法
CN110673721A (zh) * 2019-08-29 2020-01-10 江苏大学 基于视觉与意念信号协同控制的机器人看护系统
CN110673721B (zh) * 2019-08-29 2023-07-21 江苏大学 基于视觉与意念信号协同控制的机器人看护系统

Similar Documents

Publication Publication Date Title
CN108225330A (zh) 一种基于卡尔曼滤波的可见光动态定位方法
Hossan et al. A novel indoor mobile localization system based on optical camera communication
KR100686735B1 (ko) 이동 단말기를 이용한 실내 위치 추적 방법 및 시스템
CN106483500B (zh) 一种基于cdma调制的可见光通信三维定位方法
US7612324B1 (en) Distributed display composed of active fiducials
Chai et al. An indoor positioning algorithm using bluetooth low energy RSSI
JP2007155699A (ja) カメラ及び標識を用いた移動ロボットの測位システム及び方法
Naz et al. Single LED ceiling lamp based indoor positioning system
Shahjalal et al. An implementation approach and performance analysis of image sensor based multilateral indoor localization and navigation system
CN106872943B (zh) 一种基于可见光通信的移动物体室内跟踪定位方法
JP2006220465A (ja) 位置特定システム
CN104865552A (zh) 一种基于双图像传感器的可见光定位系统及方法
Miah et al. Intelligent range-only mapping and navigation for mobile robots
CN108692701A (zh) 基于粒子滤波器的移动机器人多传感器融合定位方法
CN107959925A (zh) 一种智能家居设备辅助定位方法
KR100749923B1 (ko) 카메라와 표식을 이용한 이동 로봇의 측위 시스템 및 방법
US20190353749A1 (en) Ray-surface positioning systems and methods
CN107356905B (zh) 一种基于混沌粒子群优化的可见光定位方法及其系统
CN106101980A (zh) 蓝牙系统、设备以及蓝牙信号的方向识别方法
CN107919910A (zh) 一种抗nlos干扰的可见光定位方法
Jeong et al. Fuzzy logic vehicle positioning system based on chromaticity and frequency components of LED illumination
CN206892313U (zh) 一种抗nlos干扰的可见光定位硬件系统
Salem et al. Visible light technologies for the industrial internet of things
CN105783906A (zh) 室内定位系统
Rahman et al. Performance analysis of indoor positioning system using visible light based on two-LEDs and image sensor for different handhold situation of mobile phone

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20180629