一种基于无迹卡尔曼滤波的组合定位方法与流程

本发明涉及一种基于无迹卡尔曼滤波的组合定位方法，属于室内定位以及可见光定位技术领域。

背景技术：

近年来，随着智能设备的飞速发展，基于位置的服务也随之迅速发展，个人对于位置服务精细化、准确化、无缝化的要求日益提高，室内定位技术的发展也随之成为新的关注点。但由于gps等传统定位技术无法满足室内精细定位的需求，以可见光定位技术为代表的室内定位技术开始崭露头角。但可见光定位虽然以其精度高、成本低、易实现的特点倍受青睐，却容易受到多径反射、地图限制、信号遮挡等问题的影响，定位算法容易受到外界影响而极大地影响其短时定位精度，因此其实用性受到了极大限制。

惯性导航利用定位目标载体(以下简称载体)所搭载的传感器所测得的运动加速度值和角速度值或磁偏角计算出载体运动的方向和距离，由载体初始位置推导出实时位置坐标信息。由于其工作模式完全自主，短时精度优良，具有与生俱来的抗干扰特性，可以和可见光导航系统形成良好的互补。

本发明采用包括行人惯性导航技术(pdr)在内的惯性导航技术对可见光定位进行辅助定位，采用无迹卡尔曼滤波算法对可见光定位系统/惯性导航算法的数据进行融合，从而实现组合定位导航功能，其中无迹卡尔曼滤波缩写为ukf。实验表明，组合定位导航系统有效减小了可见光定位算法由于信号抖动、信号遮挡等外界干扰问题带来的误差，同时也极大地克服了纯惯性导航系统累积误差随时间增大的问题。整个系统既可以应用于机器人、agv(automatedguidedvehicle，自动导引运输车)等工业领域，也可以应用于行人导航等消费级电子产品领域

目前，惯性导航与其它导航的融合算法常被应用于如gps+惯性导航等室外导航领域。而在可见光定位配合惯性导航系统领域鲜有文章报道。

与本发明相关的专利共两篇，下文分别对其进行剖析：

(1)申请号cn201410067768.0，标题为基于惯性定位和vlc技术的室内混合定位系统及方法，主要解决了纯可见光定位系统的可靠性不高，无法在较为复杂的室内环境中应用的问题。该专利所述系统在接收器可以接收到led直射光线的室内环境下，定位结果由vlc室内定位系统提供；当接收器进入到室内没有直射光线的阴影区域时，定位结果由惯性定位系统提供，其中惯性定位系统所需的初始位置，由vlc室内定位系统提供；

(2)申请号cn201410831922.7，标题为基于手机惯性定位和vlc的室内混合定位系统及方法，它利用当前智能手机中集成的硬件设备实现了可见光定位与惯性导航系统的简单组合，同时简化了定位装置，也可以利用目标位置来实现位置相关信息的推送，具有一定的实用意义。该专利中所述的app模块把从通信接口获取的编码信号强度和阈值进行比较，当编码信号强度大于阈值时，计算出多个led相对强度关系，根据强度关系得到光传输的衰减距离，定位出vlc定位接收器中的光电探测器所在的基于多个led光源的相对位置关系；当编码信号强度低于阈值时，把惯性定位数值作为定位值；

以上专利虽然解决了在可见光信号受到遮挡情况下定位系统无法不间断工作的问题，但是由于其在工作时仅是单纯依赖是否接收到可见光信号作为选择不同定位系统的判据，因此无法解决可见光信号受到多径效应等干扰时的定位问题，也没有将惯性导航和可见光导航的定位数据进行深度融合。此外，由于行人在行走时加速度特征复杂多变，该系统惯性导航算法中采用的加速度积分来确定接收器移动距离的方法无法应用在行人定位领域。因此，可见光定位/惯性导航组合定位算法都具有很大的改进空间。本发明的目的即是致力于解决上述可见光定位/惯性导航组合定位算法的缺陷，提出基于无迹卡尔曼滤波的可见光定位/惯性导航组合定位算法。

技术实现要素：

本发明旨在解决已有的可见光定位与惯性导航组合定位系统无法在多径干扰区域进行定位的缺陷，提出了一种基于无迹卡尔曼滤波的组合定位方法。

本发明的技术方案为一种基于无迹卡尔曼滤波的组合定位方法，依托一种基于无迹卡尔曼滤波的组合定位系统，包括惯性导航单元、可见光定位单元、滤波与位置解算单元与系统初始化单元；

一种基于无迹卡尔曼滤波的组合定位系统各单元的连接方式如下：

惯性导航单元与可见光定位单元分别和滤波解算单元以及位置解算单元相连，连接方式包括但不限于串口及蓝牙为主的方式；系统初始化单元与位置解算单元相连；一种基于无迹卡尔曼滤波的组合定位系统的各模块功能如下：

惯性导航单元用于采集惯性导航运动数据；可见光定位单元用于采集可见光定位信号并解算可见光定位位置坐标数据；系统初始化单元用于初始化循环计数值、设置工作模式布尔值和循环计数最大值；滤波解算单元用于设置无迹卡尔曼滤波器各方程与各参数，并使用无迹卡尔曼滤波算法进行滤波；位置解算单元对载体的惯性运动数据和可见光定位坐标数据进行融合处理，得到滤波后的载体位置；

其中，无迹卡尔曼滤波算法，简称ukf算法；

本发明所述一种基于无迹卡尔曼滤波的组合定位方法，包括以下步骤：

步骤1：系统初始化单元初始化循环计数值、设置工作模式布尔值和循环计数最大值，设置k＝1时刻组合定位初始坐标值，设置初始协方差；

其中，循环计数值记为k、循环计数最大值记为kmax、工作模式布尔值记为bool；初始化k＝1；设置bool值和kmax值，当bool＝0时系统处于实时工作状态，kmax为无穷大值；bool＝1时系统处于离线工作状态，kmax为一常数；设置第1时刻组合定位位置的初始坐标值为(m,n)，设置初始协方差p＝0；

步骤2：惯性导航单元采集惯性导航运动参数，可见光定位单元解算可见光定位位置坐标，具体为：

步骤2.1：惯性导航单元采集载体的惯性运动数据；

其中，设当前时刻为k，所述载体的惯性运动数据根据本方法所应用的场景不同分为两种：

若为agv场景，则惯性运动数据包含k时刻的横轴方向加速度与纵轴方向加速度由于加速度数据容易受到外界环境的干扰，噪声较大，因此在进行下一个步骤之前需要对加速度数据进行低通滤波；

若为行人导航场景，则惯性运动数据包含k-1时刻到k时刻的平均运动距离sk与平均航向角θk；

步骤2.2：可见光定位单元采集并计算载体的可见光定位数据；

其中，可见光定位数据通过现有可见光定位算法进行计算；

现有可见光定位算法主要包括但不限于基于rss算法的可见光定位算法；

依然假设当前时刻为k，则所述可见光定位数据记为zk＝[xkyk]^t；xk、yk分别为载体的可见光定位位置横纵坐标，符号t表示矩阵转置；

步骤3：滤波与解算单元设置ukf各方程与各参数，对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的载体位置；

步骤3.1：设置无迹卡尔曼滤波器各方程包括滤波状态方程xk＝f(xk-1)与可见光定位观测方程zk＝hxk；其中xk为状态向量，zk为观测向量，h为观测矩阵；

设置无迹卡尔曼滤波器各参数包括根据系统所应用的场景而设置的系统噪声qk、观测噪声r、状态向量xk及观测向量zk；

其中，状态向量xk包含有载体的惯性运动数据，观测向量zk为可见光定位数据zk＝[xkyk]^t，xk、yk分别为载体的可见光定位位置横纵坐标；符号t表示矩阵转置；

步骤3.2：计算预测估计值xk+1/k；

所述计算预测估计值的方法为：

其中，加权系数

n为状态向量维数，在本方法中根据实际场景确定向量维数n；λ为尺度参数，计算方法为λ＝ε²(n+κ)-n，该式中ε为一小量，取值范围为[10^-4,1]，常量κ可取0或者3-n；

步骤3.3：计算预测协方差pk+1/k；

所述计算预测协方差的方法为：

符号t表示矩阵转置；

步骤3.4：计算预测测量值zk+1/k；

所述计算预测测量值的方法为：

步骤3.5：计算卡尔曼增益kk+1；

所述计算卡尔曼增益的方法为：

kk+1＝pxy，k+1|kpyy，k+1|k^-1，

其中，

步骤3.6：更新协方差pk+1/k+1；

所述更新协方差的方法为：

pk+1|k+1＝pk+1|k-kk+1pyy，k+1|kkk+1^t

符号t表示矩阵转置；

步骤3.7：更新状态向量xk+1；

所述更新状态向量的方法为：

xk+1＝xk+1|k+kk+1|k(zk+1-zk+1|k)。

上述步骤3.2-3.7为ukf算法；

步骤4：位置解算单元计算k时刻的组合定位位置坐标，具体为：

将步骤3.7中状态向量xk+1中包含的坐标值，作为k+1时刻组合定位位置坐标；

步骤5：位置解算单元判断循环计数值是否已经达到循环计数最大值，并决定是否完成本方法，具体为：

步骤5.1：若是，即k＝kmax，则位置解算单元输出k时刻的组合定位位置坐标，完成了本方法；

步骤5.2：若否，即k＜kmax，则位置解算单元输出k时刻的组合定位位置坐标，并令k＝k+1，跳至步骤2；

至此，经过步骤1到步骤5，完成了一种基于无迹卡尔曼滤波的组合定位方法。

有益效果

一种基于无迹卡尔曼滤波的组合定位方法，与现有技术相比，具有如下有益效果：

1、由于可见定位算法容易受到外界影响与干扰，其在单独工作时非线性度大，相比较与ekf等其他滤波算法，采用ukf能够更好地适应可见光定位环境，可以有效减小了可见光定位系统的信号抖动、受可见光覆盖区域限制、信号遮挡等问题带来的误差；

2、本方法相较于单纯依靠选择滤波来对可见光定位数据和惯性导航定位数据进行融合的方式相比，融合效果更好，融合后误差更小；

3、相较于其他滤波器，本方法所采用的ukf无需选择滤波等其他滤波器辅助，整个算法只需要一个ukf滤波器工作，可靠性更强，稳定性更好；

4、本发明所提系统既可以应用于机器人、agv等工业领域，也可以应用于行人导航等消费级电子产品领域，应用广泛，实用性强，所需硬件设备成熟，成本低廉，易于推广。

附图说明

图1为本发明“一种基于无迹卡尔曼滤波的组合定位方法”流程示意图；

图2为本发明“一种基于无迹卡尔曼滤波的组合定位方法”实施例3中本方法与现有其他定位方法的定位结果对比图；

图3为本发明“一种基于无迹卡尔曼滤波的组合定位方法”实施例3中本方法与现有其他定位方法定位结果对比局部放大图；

图4为本发明“一种基于无迹卡尔曼滤波的组合定位方法”实施例3中本方法与现有其他定位方法的定位结果误差对比图；

图5为本发明“一种基于无迹卡尔曼滤波的组合定位方法”实施例3中本方法与现有其他定位方法的定位结果误差分布对比。

具体实施方式

以下实施例结合附图对本发明进行了详细的说明，但本发明的具体实施形式并不局限于此。

实施例1

以agv可见光/惯性组合定位系统为例，采用本发明“一种基于无迹卡尔曼滤波的可见光定位/惯性导航组合定位方法”实现可见光定位/惯性定位组合定位，具体步骤如图1所示。

步骤a：系统初始化单元初始化循环计数值k、设置工作模式布尔值bool；设置循环最大值kmax；设置k＝1时刻组合定位初始坐标值，设置初始协方差；

具体到本实施例，k被初始化为1；设置初始协方差p＝0；

步骤b：判断bool值是否为0，并进行相应操作：

步骤b.1若是，对应图1中的“bool＝0？”输出的y，设置循环最大值kmax为无穷大；

步骤b.2若否，对应图1中的“bool＝0？”输出的n，循环最大值kmax为一常数；

具体到本实施例，对应于b.1，bool＝0，循环最大值kmax设置为无穷大；系统处在实时工作模式；

若非本实施例bool＝0情况，则对应于b.2，bool＝1，循环最大值kmax为常数，系统处在离线工作模式；

步骤c：惯性导航单元采集惯性导航运动参数，可见光定位单元解算可见光定位位置坐标；

步骤c.1：假设当前时刻为k，惯性导航单元采样周期为t，惯性导航单元输出的k时刻的原始横向和纵向加速度数据通过低通滤波器进行滤波处理；其中，为防止低通滤波器滤掉有用信号，低通滤波器截止频率不能过低，具体到本实施例，设定低通滤波器截止频率5hz；

步骤c.2：载体在k时刻的可见光定位数据通过现有可见光定位算法进行计算，得到载体的可见光定位位置，并将其记为zk＝[xkyk]^t，其中符号t表示矩阵转置；

步骤d：滤波与解算单元设置ukf各方程与各参数，使用ukf对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的载体位置；具体到本实施例，其方法说明如下：

步骤d.1：设定状态向量其中上标t表示矩阵转置，其中，xk为该时刻载体横坐标，yk为该时刻载体纵坐标，为该时刻载体横轴方向速度，为该时刻载体纵轴方向速度，为该时刻载体横轴方向加速度，为该时刻载体横轴方向加速度；同时设定滤波状态方程为：

设定系统噪声为：

设定观测噪声为：

步骤d.2：开始进行ukf滤波，具体与步骤3.2-3.7相同；

步骤e：判断是否kmax<k，并决定是否完成本方法，具体为：

步骤e.1若是，对应图1中的“kmax<k？”输出的y，则输出本时刻位置坐标(xk,yk)，并令k＝k+1，跳至步骤c；

步骤e.2若否，对应图1中的“kmax<k？”输出的n，则输出本时刻位置坐标(xk,yk)，完成了本方法；

至此，从步骤a到e，完成了本实施例一种基于无迹卡尔曼滤波的可见光组合定位方法。

实施例2

以行人导航可见光/惯性组合定位系统为例，采用本发明“一种基于无迹卡尔曼滤波的可见光定位/惯性导航组合定位装置”实现可见光定位/惯性定位组合定位：

步骤a：系统初始化单元初始化循环计数值、设置工作模式布尔值和循环计数最大值，设置k＝1时刻组合定位初始坐标值，设置初始协方差，具体与步骤a相同；

步骤b：判断bool值是否为0，并进行相应操作，具体与步骤b相同：

步骤c：惯性导航单元采集惯性导航运动参数，可见光定位单元解算可见光定位位置坐标；

步骤c.1：假使当前时刻为k，计算周期为行人步行的周期，惯性导航单元输出k-1时刻到k时刻这一周期内的行人行走距离sk、平均航向变化角δθk；

步骤c.2:行人的可见光定位数据通过现有可见光定位算法进行计算，得到载体的可见光定位位置；具体与步骤c.2相同；

步骤d：滤波与解算单元设置ukf各方程与各参数，使用ukf对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的行人位置；具体到本实施例，其方法说明如下：

步骤d.1：设定状态向量xk＝[θkxkyk]^t，θk为该时刻行人行走方向航向角，xk、yk分别为行人该时刻的横纵坐标，其中上标t表示矩阵转置；设定滤波状态方程为：

xk+1＝[θk+δθkxk+skcosθkyk+sksinθk]^t；

设定可见光观测方程中观测矩阵为：

设定系统噪声为：

其中，vθ为航向角测量方差，在本实施例中取0.01²；vs为步长估计方差，在本实施例中取0.01²。

设定观测噪声为：

步骤d.2：具体与步骤d.2相同；

步骤e：判断循环计数值是否已经达到计数最大值，并决定是否完成本方法，具体与步骤e相同；

至此，从步骤a到e，完成了本实施例一种基于无迹卡尔曼滤波的可见光组合定位方法。

实施例3

本实施例按照实施例1所述的参数具体阐述了执行本发明步骤a到步骤c所得的组合定位位置结果，同时与现有可见光定位方法、惯性导航方法所得定位位置结果进行比较，比较结果如图2。

图2中，横轴、纵轴方向的单位均为米；虚线“实际路线”为agv小车的真实运动轨迹；十字划线“ukf滤波结果”为经本发明所提出的“一种基于无迹卡尔曼滤波的可见光组合定位方法”步骤a到步骤c所得的组合定位位置轨迹解算结果；星划线“惯性导航结果”为采用传统惯性导航方法得到的定位位置轨迹；圆圈划线“可见光定位结果”为背景技术中专利(1)中提到的定位方法所得定位位置轨迹；需要说明的是：由于专利(1)和专利(2)核心技术相似，因此我们仅选取专利(1)中所述技术与本专利的定位效果进行对比。

从图2中可以看出，“ukf滤波结果”曲线与“可见光定位结果”和“惯性导航结果”两条曲线相比，与“实际路线”轨迹更为接近，且“ukf滤波结果”曲线抖动更小，定位误差更小。

为了更好的看清楚本方法的优势，将图2中横坐标(0.95,1.02)，纵坐标(0,0.2)内的区域放大，放大后的结果图如图3所示。由图3可以清晰的看出，该区域内中pf曲线抖动较小，与真实轨迹更加吻合；

为了更好地说明本发明对于减小可见光定位方法与惯性导航方法定位误差的作用，我们将图2中“ukf滤波结果”曲线、“可见光定位结果”曲线、“惯性导航结果”相对于“实际路线”曲线之间的误差、误差分布分别作图，如图4、图5所示。由图4、图5可以看出，该区域内“ukf滤波结果”曲线所表明的误差相比有其他两条曲线所表明的有大幅减小，充分证明了本方法可以有效减小可见光定位系统的定位误差。

需要说明的是，本说明书所述的只是本发明的较佳具体实施例，以上实施例仅用于说明本发明的技术方案而非对本发明的限制。凡本领域技术人员依本发明的构思通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在本发明的范围之内。