CN110320497A - 基于vlc与imu的粒子滤波融合定位方法 - Google Patents

Abstract

本发明公开了一种基于VLC与IMU的粒子滤波融合定位方法，通过粒子滤波融合定位方法针对可能的观测值，对VLC和IMU所得到的定位结果进行加权修正，近似估计待定位装置的近似状态，从而实现对两种定位结果的融合，得到较为理想的定位结果。将该基于VLC与IMU的粒子滤波融合定位方法应用于可见光定位系统中，克服VLC自身技术所固有的缺陷，构建一种具备双传感器的具备较强鲁棒性的定位技术方案，可以在不借助外界信标的条件下，在一定时间段内仍保持较精确的定位，具有广阔的应用前景。

Description

基于VLC与IMU的粒子滤波融合定位方法

技术领域

本发明涉及可见光通信定位与惯性导航技术技术领域，具体涉及一种基于VLC与IMU的粒子滤波融合定位方法。

背景技术

近年来，随着科技的发展，各种定位技术层出不穷，包括蓝牙、RFDI、WIFI等技术已经日趋成熟，但由于环境的复杂性，设计的成本问题在一定程度上阻碍了技术的进一步推广。目前LED灯具的普及程度已经很高，利用已有的设施进行室内定位不失为一种好的选择，于是可见光定位技术(VLC)应运而生。VLC有如下优势：首先，光源成本低廉，位置在安装后相对稳定；其次，利用人眼不可见的闪烁，使得在照明系统上添加信息传输成为可能，而且在照明的同时对周围没有干扰，实现照明、通信、定位一体化的目标。

但是该技术亦存在缺陷：LED灯具发出的光线受到遮挡导致部分通信链路被阻隔，接受者就没有办法接收到足够的信标信号，从而影响定位的稳定性。

惯性导航技术(IMU)在光信号较弱时可以发挥自身优势，通过定位终端自身的相关数据对定位结果加以校正。测量在采样时间内的移动距离和倾斜角的变化，通过积分原理推算出原始位置。但是无法长时间精确定位，因为定位误差随着时间的推移逐渐增加。因此，考虑使用粒子滤波融合定位方法降低定位误差。

粒子滤波融合定位方法并不是一种定位技术，但是是一种重要的数据处理方式，对于优化定位的结果有很大的作用，针对所有可能的观测值，通过状态空间随机生成的一些粒子，每个粒子利用贝叶斯准则进行加权修正，以近似估计实际的系统状态。

发明内容

本发明的目的是为了解决现有技术中的上述缺陷，提供一种基于VLC与IMU的粒子滤波融合定位方法。

本发明的目的可以通过采取如下技术方案达到：

一种基于VLC与IMU的粒子滤波融合定位方法，该方法包括以下步骤：

S1、定位终端利用IMU进行定位。定位终端使用定位终端所携带的传感器测量相邻时间内定位终端移动距离、倾斜角的变化，利用定位终端内IMU定位程序包进行k次采样，得到定位终端相对初始点(x₀,y₀)的位置(x₁,y₁)(x₂,y₂)…(x_k,y_k)；

S2、定位终端利用VLC进行定位。假设现在共有A、B、C三盏LED灯具，三盏LED灯具安装在天花板上传输其位置信息，每一盏LED灯具具备有不同的ID，定位终端可通过光电检测器件接受光信号，并根据LED灯具已知的ID和固定位置，采用基于三角定位法的程序包推算出定位终端所在位置；

S3、定位终端利用粒子滤波方法实现融合定位，获得校正后的定位。定位终端对IMU定位过程中定位终端的运动行为进行建模，将步骤S2中所获得的定位数据作为粒子滤波的观测数据，实现状态估计，对步骤S1所获得的位置信息进行校正，得到新的定位结果。

进一步地，所述的步骤S1中，假设定位终端在t₀时刻的位置为(X_k,Y_k)，则定位终端在下一时刻t(t₀和t的时间间隔较短)的坐标位置为(X_k+1,Y_k+1)，设该过程中x坐标变化量为ΔX_k，y坐标变化量为ΔY_k，连接两点所形成的直线的倾斜角为α，则两坐标具有如下关系：

设两点间线段距离为l_k,则有：

从而得到系统状态方程为：

进行k次采样，就得到定位终端相对初始点(x₀,y₀)的位置(x₁,y₁)(x₂,y₂)…(x_k,y_k)。

进一步地，所述的步骤S2中，包括以下步骤：

S201、定位终端根据光电检测器件接收到的光信号，求出LED灯具与光电检测器件的水平距离。

基于兰伯模型得到LED灯具到定位终端的光电检测器件的信道增益：

其中A是定位终端用于接受光信号的光电检测器件的物理面积，该器件的位置即为定位终端的位置。是LED灯具的发射角，θ是LED灯具的入射角，d代表定位终端中光电检测器件与LED灯具的直线距离，m代表兰伯排放的顺序，可用如下公式计算：

其中代表LED灯具的半功率角，通常m＝1,h为光电检测器件至LED灯具所在水平面的距离。

定义LED灯具的光强度为I_t，光电检测器件接受的光强度为I_r，其中C是常量，则有：

设r为LED灯具与光电检测器件的水平距离，由上一步骤的数据，利用以下两个公式得到r：

S202、定位终端获取灯具图像。定位终端利用光电检测器对光电检器件接收到的信号进行灯具识别，在低曝光度和极短曝光时间之下，获取灯具的图像，其中不同的灯具所形成的条纹密度不同；

S203、定位终端进行图像处理，求出光电检测器件的位置。定位终端利用基于OpenCV编写的程序包对步骤S202获取的图像进行切割，得到单个LED图像，计算出每张图片的条纹数目，从而得到LED灯具的ID信息，进而得到三个LED灯管A、B、C的位置(x_A,y_A)(x_B,y_B)(x_C,y_C)。因此，光电检测器件的位置(x,y)可以由如下公式获得：

其中三盏LED灯具的坐标为(x_A,y_A)(x_B,y_B)(x_C,y_C)，r_A、r_B、r_C代表图像传感器与三盏LED灯具的水平距离。

进一步地，在所述的步骤S3中，包括以下步骤：

S301、粒子的选取和初始化。在步骤S1的结果的基础上，设粒子的集合为H＝{Aⁱ|i＝1,2,…,N_s}，N_s为粒子个数，在步骤S1中系统状态空间对应的粒子的状态空间为Aⁱ＝(xⁱ,yⁱ)^T，(xⁱ,yⁱ)即粒子i的位置。初始粒子的状态空间其中i＝1,2,…,N_s，且粒子的初始权重为1/N_s,经过k次采样计算后粒子i的状态空间为

对应的系统状态空间A_k＝(X_k，Y_k)^T；

S302、对粒子权重进行计算。当使用IMU在第k次采样计算获得新的观测数据之后，需要对所有粒子的权重进行更新，对于粒子i权重的计算如下：

其中为粒子i第k次采样时坐标与VLC定位结果得到的坐标的(x,y)距离，可由下面公式计算，R为常数。

最终，对粒子权重进行归一化，得到

S303、粒子重采样，得到校正后的定位信息。系统状态根据步骤S302的对应权重，保留权重较大的粒子，抛弃权重较小的粒子，可得校正后的系统状态空间A′_k为：

VLC校准航迹推算的定位时，将A′_k取代原本的定位结果A_k＝(X_k，Y_k)^T，并作为下一次航迹推算的初始位置，以固定或者不固定的时间间隔循环，或者当误差范围较大时执行循环，定位结果即为A′₀,…A′_k，A′_k+1。以上运算将航迹推算定位的进度控制在一个需要的范围以内。

本发明相对于现有技术具有如下的优点及效果：

1、本发明提出将VLC定位与惯性导航技术(IMU)通过粒子滤波的方式进行融合，构建一种双传感器的定位技术方案，并通过粒子滤波方法去修正定位误差，提升了定位方法的鲁棒性、准确性。

2、本发明克服了原有的VLC定位技术过度依赖光信号的不足，即使不能接受到足够的光信号，通过惯性导航定位技术在一段时间内仍可进行定位，同时降低了IMU定位的累计误差。。

附图说明

图1是本发明实施例中实现VLC三角定位方法示意图；

图2是本发明实施例中公开的基于VLC与IMU的粒子滤波融合定位方法的结构框图；

图3是本发明实施例中图像处理过程的图像切割效果图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

本实施例公开了一种基于VLC与IMU的粒子滤波融合定位方法，如图2所示，该粒子滤波融合定位方法基于粒子滤波融合定位系统为实施对象，该粒子滤波融合定位系统具体包括LED灯信号发射子系统、定位终端。

其中,LED灯信号发射子系统包括28V恒压源、STM32系统板、LED驱动电路、LED灯具。通过STM32系统板向LED灯具发送不同频率的脉冲序列，控制LED灯具的驱动电路，每一盏LED灯具都有不同的频率，实现LED灯ID信息的生成。

定位终端包括光电检测器、惯性导航模块、图像识别模块。惯性导航模块可以使用ROS机器人系统中IMU节点所发出的信息实现定位，光电检测器件获取LED灯的ID信号，经过基于OpenCV的图像处理模块获得VLC定位信息，粒子滤波融合定位模块对IMU和VLC的定位结果进行融合，得到校正后的定位结果。

本实施例公开的一种基于VLC与IMU的粒子滤波融合定位方法包括以下步骤：

S1、定位终端利用IMU进行定位。基于ROS系统的移动机器人本身具备含有IMU功能的节点，利用该类型的移动机器人作为定位终端进行定位可以简化程序的开发过程，因此给定移动机器人的初始点(x₀,y₀)，利用移动机器人内IMU定位程序包及其所携带的传感器得到移动机器人的相对初始点(x₀,y₀)的位置(x₁,y₁)(x₂,y₂)…(x_n,y_n)，实现IMU定位。

S2、定位终端利用VLC进行定位。如图1所示，r为LED灯具与移动机器人的光电检测器件的水平距离，h为光电检测器件至LED灯具所在水平面的距离，d代表光电检测器件与LED灯具的直线距离。假设现在共有A、B、C三盏LED灯具，事先对每一个LED灯具输入不同的频率，即每一盏LED灯具具备有不同的ID，这样移动机器人才能够识别不同位置的LED灯具，根据LED灯具已知的ID和固定位置，采用基于三角定位法的程序包推算出移动机器人所在位置；

S3、移动机器人利用粒子滤波方法实现融合定位，获得校正后的定位。ROS系统提供了带有粒子滤波算法的节点ACML，即自适应蒙特卡洛定位，编写程序对S1中定位终端的运动行为进行建模，将S2中所获得的定位数据作为粒子滤波的观测数据，实现状态估计，对S1所获得位置进行修正，得到新的定位结果。

在步骤S1中，假设定位终端在t₀时刻的位置为(X_k,Y_k)。则定位终端在下一时刻t(t₀和t的时间间隔较短)的坐标位置为(X_k+1,Y_k+1)。设该过程中x坐标变化量为ΔX_k，y坐标变化量为ΔY_k，连接两点所形成的直线的倾斜角为α，则两坐标具有如下关系：

设两点间线段距离为l_k,则有

从而得到系统状态方程为

进行k次采样，就得到定位终端相对初始点(x₀,y₀)的位置(x₁,y₁)(x₂,y₂)…(x_k,y_k)；

在步骤S2中，包括以下步骤：

S201、基于兰伯模型得到LED灯具到定位终端的光电检测器件的信道增益：

其中A是定位终端用于接受可见光信号的的光电检测器件的物理面积，该器件的位置即为定位终端的位置。是LED灯具的发射角，θ是LED灯具的入射角，d代表定位终端中光电检测器件与LED灯具的直线距离，m代表兰伯排放的顺序，可用如下公式计算：

其中代表LED灯具的半功率角，通常m＝1,h为光电检测器件至LED灯具所在水平面的距离，r为LED灯具与光电检测器件的水平距离，定义LED灯具的光强度为I_t，光电检测器件接受的光强度为I_r，其中C是常量，则有：

由上一步骤的数据，利用以下两个公式得到r：

S202、定位终端利用CMOS摄像头对接收到的信号进行灯具识别，在低曝光度和极短曝光时间之下，获取灯具的图像，其中不同的灯具所形成的条纹密度不同；

S203、对获取的图像进行图像处理，求出光电检测器件的位置。每一盏LED灯具的条纹数目不同，故需要基于OpenCV编写的程序包对图像进行切割，得到单个LED图像，所获得的LED图像效果如图3所示，可以利用移动机器人的程序包计算出每张图片的条纹数目，得到LED灯具的ID信息，进而得到三个LED灯管A、B、C的位置(x_A,y_A)(x_B,y_B)(x_C,y_C)。

则光电检测器件的位置(x,y)可以由如下公式获得：

其中三盏LED灯具的坐标为(x_A,y_A)(x_B,y_B)(x_C,y_C)，分别代表三个LED灯管A、B、C的位置，r_A、r_B、r_C代表图像传感器与三盏LED灯具的水平距离。

在步骤S3中，包括以下步骤：

S301、粒子的选取和初始化，在S1的结果的基础上，设粒子的集合为H＝{Aⁱ|i＝1,2,…,N_s}，N_s为粒子个数，S1系统状态空间为A＝(X,Y)^T，则粒子的状态空间为Aⁱ＝(xⁱ,yⁱ)^T，(xⁱ,yⁱ)即粒子i的位置。初始粒子的状态空间其中i＝1,2,…,N_s，且粒子i的初始权重为1/N_s,经过k次采样计算后粒子i的状态空间对应的系统状态空间A_k＝(X_k，Y_k)^T；

S302、对粒子权重进行计算。当使用IMU在第k次采样计算获得一个新的观测数据之后，需要对所有粒子的权重进行更新，对于粒子i权重的计算如下式：

其中为粒子i第k次采样时坐标与VLC定位结果得到的坐标的(x,y)距离，可由下面公式计算，R为常数。

最终，对粒子权重进行归一化，得到

S303、粒子重采样，得到校正后的定位信息。系统状态根据S302的对应权重，保留权重较大的粒子，抛弃权重较小的粒子，可得校正后的系统状态空间为：

VLC校准航迹推算的定位时，将A′_k取代原本的定位结果A_k＝(X_k，Y_k)^T，并作为下一次航迹推算的初始位置，以固定或者不固定的时间间隔循环，或者当误差范围较大时执行循环，定位结果即为A′₀,…A′_k，A′_k+1。以上运算将航迹推算定位的进度控制在一个需要的范围以内。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的粒子滤波融合定位方法包括以下步骤：

S1、定位终端利用IMU进行定位，定位终端使用定位终端所携带的传感器测量相邻时间内定位终端移动距离、倾斜角的变化，利用定位终端内IMU定位程序包进行k次采样，得到定位终端相对初始点(x₀,y₀)的位置(x₁,y₁)(x₂,y₂)…(x_k,y_k)；

S2、定位终端利用VLC进行定位，假设现在共有A、B、C三盏LED灯具，三盏LED灯具安装在天花板上传输其位置信息，每一盏LED灯具具备有不同的ID，定位终端可通过光电检测器件接受光信号，并根据LED灯具已知的ID和固定位置，采用基于三角定位法的程序包推算出定位终端所在位置；

S3、定位终端利用粒子滤波方法实现融合定位，获得校正后的定位。定位终端对IMU定位过程中定位终端的运动行为进行建模，将步骤S2中所获得的定位数据作为粒子滤波的观测数据，实现状态估计，对步骤S1所获得的位置信息进行校正，得到新的定位结果。

2.根据权利要求1所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S2包含如下步骤：

S201、定位终端求出LED灯具与光电检测器件的水平距离；

S202、定位终端获取灯具图像；

S203、定位终端进行图像处理，求出光电检测器件的位置。

3.根据权利要求1所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S3包含如下步骤：

S301、粒子的选取和初始化；

S302、对粒子权重进行计算；

S303、粒子重采样，得校正后的定位信息。

4.根据权利要求2所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S201中定位终端求出LED灯具与光电检测器件的水平距离为r：

其中d代表定位终端中光电检测器件与LED灯具的直线距离，h为光电检测器件至LED灯具所在水平面的距离，C是常量，LED灯具的光强度为I_t，光电检测器件接受的光强度为I_r。

5.根据权利要求2所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S203中光电检测器件的位置(x,y)可由下式获得：

其中三盏LED灯具的坐标为(x_A,y_A)(x_B,y_B)(x_C,y_C)，分别代表三个LED灯管A、B、C的位置，r_A、r_B、r_C代表图像传感器与三盏LED灯具的水平距离。

6.根据权利要求3所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S302中对粒子权重进行计算过程如下：

对于粒子i权重的计算如下：

其中为粒子i第k次采样时坐标与VLC定位结果得到的坐标的(x,y)距离，可由下面公式计算，R为常数，

最终，对粒子权重进行归一化，得到

其中，N_s为粒子个数。

7.根据权利要求6所述的基于VLC与IMU的粒子滤波融合定位方法，其特征在于，所述的步骤S303过程如下：

系统状态根据步骤S302的对应权重，保留权重较大的粒子，抛弃权重较小的粒子，得校正后的系统状态空间A′_k为：

当VLC校准航迹推算的定位时，将A′_k取代原本的定位结果A_k＝(X_k，Y_k)^T，并作为下一次航迹推算的初始位置，以固定或者不固定的时间间隔循环，或者当误差范围较大时执行循环，定位结果即为A′₀,…A′_k，A′_k+1。