CN107192387A - 一种基于无迹卡尔曼滤波的组合定位方法 - Google Patents

一种基于无迹卡尔曼滤波的组合定位方法 Download PDF

Info

Publication number
CN107192387A
CN107192387A CN201710365995.5A CN201710365995A CN107192387A CN 107192387 A CN107192387 A CN 107192387A CN 201710365995 A CN201710365995 A CN 201710365995A CN 107192387 A CN107192387 A CN 107192387A
Authority
CN
China
Prior art keywords
mrow
visible ray
unit
msub
unscented kalman
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
CN201710365995.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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201710365995.5A priority Critical patent/CN107192387A/zh
Publication of CN107192387A publication Critical patent/CN107192387A/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/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

一种基于无迹卡尔曼滤波的组合定位方法，属于室内定位以及可见光定位技术领域。包括：步骤1：系统初始化单元初始化循环计数值、设置工作模式布尔值和循环计数最大值，设置k＝1时刻组合定位初始坐标值，设置初始协方差；步骤2：惯性导航单元采集惯性导航运动参数，可见光定位单元解算可见光定位位置坐标；步骤3：滤波与解算单元设置UKF各方程与各参数，对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的载体位置；步骤4：位置解算单元计算k时刻的组合定位位置坐标；步骤5：位置解算单元判断循环计数值是否已达循环计数最大值，并决定是否完成本方法。本方法有效减小了可见光定位误差，且可靠性、稳定性以及实用性更强。

Description

一种基于无迹卡尔曼滤波的组合定位方法
技术领域
本发明涉及一种基于无迹卡尔曼滤波的组合定位方法，属于室内定位以及可见光定位技术领域。
背景技术
近年来，随着智能设备的飞速发展，基于位置的服务也随之迅速发展，个人对于位置服务精细化、准确化、无缝化的要求日益提高，室内定位技术的发展也随之成为新的关注点。但由于GPS等传统定位技术无法满足室内精细定位的需求，以可见光定位技术为代表的室内定位技术开始崭露头角。但可见光定位虽然以其精度高、成本低、易实现的特点倍受青睐，却容易受到多径反射、地图限制、信号遮挡等问题的影响，定位算法容易受到外界影响而极大地影响其短时定位精度，因此其实用性受到了极大限制。
惯性导航利用定位目标载体(以下简称载体)所搭载的传感器所测得的运动加速度值和角速度值或磁偏角计算出载体运动的方向和距离，由载体初始位置推导出实时位置坐标信息。由于其工作模式完全自主，短时精度优良，具有与生俱来的抗干扰特性，可以和可见光导航系统形成良好的互补。
本发明采用包括行人惯性导航技术(PDR)在内的惯性导航技术对可见光定位进行辅助定位，采用无迹卡尔曼滤波算法对可见光定位系统/惯性导航算法的数据进行融合，从而实现组合定位导航功能，其中无迹卡尔曼滤波缩写为UKF。实验表明，组合定位导航系统有效减小了可见光定位算法由于信号抖动、信号遮挡等外界干扰问题带来的误差，同时也极大地克服了纯惯性导航系统累积误差随时间增大的问题。整个系统既可以应用于机器人、AGV(Automated Guided Vehicle，自动导引运输车)等工业领域，也可以应用于行人导航等消费级电子产品领域
目前，惯性导航与其它导航的融合算法常被应用于如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＝[Xk Yk]T；Xk、Yk分别为载体的可见光定位位置横纵坐标，符号T表示矩阵转置；
步骤3：滤波与解算单元设置UKF各方程与各参数，对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的载体位置；
步骤3.1：设置无迹卡尔曼滤波器各方程包括滤波状态方程Xk＝f(Xk-1)与可见光定位观测方程Zk＝HXk；其中Xk为状态向量，Zk为观测向量，H为观测矩阵；
设置无迹卡尔曼滤波器各参数包括根据系统所应用的场景而设置的系统噪声Qk、观测噪声R、状态向量Xk及观测向量Zk；
其中，状态向量Xk包含有载体的惯性运动数据，观测向量Zk为可见光定位数据Zk＝[Xk Yk]T，Xk、Yk分别为载体的可见光定位位置横纵坐标；符号T表示矩阵转置；
步骤3.2：计算预测估计值Xk+1/k；
所述计算预测估计值的方法为：
其中，加权系数
n为状态向量维数，在本方法中根据实际场景确定向量维数n；λ为尺度参数，计算方法为λ＝ε2(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＝[Xk Yk]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＝[θk xk yk]T，θk为该时刻行人行走方向航向角，xk、yk分别为行人该时刻的横纵坐标，其中上标T表示矩阵转置；设定滤波状态方程为：
Xk+1＝[θk+Δθk xk+Skcosθk yk+Sksinθk]T；
设定可见光观测方程中观测矩阵为：
设定系统噪声为：
其中，Vθ为航向角测量方差，在本实施例中取0.012；Vs为步长估计方差，在本实施例中取0.012。
设定观测噪声为：
步骤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滤波结果”曲线所表明的误差相比有其他两条曲线所表明的有大幅减小，充分证明了本方法可以有效减小可见光定位系统的定位误差。
需要说明的是，本说明书所述的只是本发明的较佳具体实施例，以上实施例仅用于说明本发明的技术方案而非对本发明的限制。凡本领域技术人员依本发明的构思通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在本发明的范围之内。

Claims (7)

1.一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：依托一种基于无迹卡尔曼滤波的组合定位系统，包括惯性导航单元、可见光定位单元、滤波与位置解算单元与系统初始化单元；
一种基于无迹卡尔曼滤波的组合定位系统各单元的连接方式如下：
惯性导航单元与可见光定位单元分别和滤波解算单元以及位置解算单元相连，连接方式包括但不限于串口及蓝牙为主的方式；系统初始化单元与位置解算单元相连；一种基于无迹卡尔曼滤波的组合定位系统的各模块功能如下：
惯性导航单元用于采集惯性导航运动数据；可见光定位单元用于采集可见光定位信号并解算可见光定位位置坐标数据；系统初始化单元用于初始化循环计数值、设置工作模式布尔值和循环计数最大值；滤波解算单元用于设置无迹卡尔曼滤波器各方程与各参数，并使用无迹卡尔曼滤波算法进行滤波；位置解算单元对载体的惯性运动数据和可见光定位坐标数据进行融合处理，得到滤波后的载体位置；
其中，无迹卡尔曼滤波算法，简称UKF算法。
2.根据权利要求1所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：包括以下步骤：
步骤1：系统初始化单元初始化循环计数值、设置工作模式布尔值和循环计数最大值，设置k＝1时刻组合定位初始坐标值，设置初始协方差；
步骤2：惯性导航单元采集惯性导航运动参数，可见光定位单元解算可见光定位位置坐标；
步骤3：滤波与解算单元设置UKF各方程与各参数，对载体的惯性运动数据和可见光定位数据进行融合滤波处理，得到滤波后的载体位置；
步骤4：位置解算单元计算k时刻的组合定位位置坐标；
步骤5：位置解算单元判断循环计数值是否已经达到循环计数最大值，并决定是否完成本方法；
至此，经过步骤1到步骤5，完成了一种基于无迹卡尔曼滤波的组合定位方法。
3.根据权利要求2所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：步骤1中，循环计数值记为k、循环计数最大值记为kmax、工作模式布尔值记为Bool；初始化k＝1；设置Bool值和kmax值，当Bool＝0时系统处于实时工作状态，kmax为无穷大值；Bool＝1时系统处于离线工作状态，kmax为一常数；设置第1时刻组合定位位置的初始坐标值为(m,n)，设置初始协方差P＝0。
4.根据权利要求2所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：步骤2，具体为：
步骤2.1：惯性导航单元采集载体的惯性运动数据；
其中，设当前时刻为k，所述载体的惯性运动数据根据本方法所应用的场景不同分为两种：
若为AGV场景，则惯性运动数据包含k时刻的横轴方向加速度与纵轴方向加速度由于加速度数据容易受到外界环境的干扰，噪声较大，因此在进行下一个步骤之前需要对加速度数据进行低通滤波；
若为行人导航场景，则惯性运动数据包含k-1时刻到k时刻的平均运动距离Sk与平均航向角θk；
步骤2.2：可见光定位单元采集并计算载体的可见光定位数据；
其中，可见光定位数据通过现有可见光定位算法进行计算；
现有可见光定位算法主要包括但不限于基于RSS算法的可见光定位算法；
依然假设当前时刻为k，则所述可见光定位数据记为Zk＝[Xk Yk]T；Xk、Yk分别为载体的可见光定位位置横纵坐标，符号T表示矩阵转置。
5.根据权利要求2所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：步骤3，具体为：
步骤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；
所述计算预测估计值的方法为：
<mrow> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>W</mi> <mi>i</mi> </msub> <msub> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> </mrow>
其中，加权系数
n为状态向量维数，在本方法中根据实际场景确定向量维数n；λ为尺度参数，计算方法为λ＝ε2(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，
其中，
<mrow> <msub> <mi>P</mi> <mrow> <mi>X</mi> <mi>Y</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>W</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <mi>R</mi> <mo>;</mo> </mrow>
步骤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算法。
6.根据权利要求2所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：步骤4，具体为：
将步骤3.7中状态向量Xk+1中包含的坐标值，作为k+1时刻组合定位位置坐标。
7.根据权利要求2所述的一种基于无迹卡尔曼滤波的组合定位方法，其特征在于：步骤5，具体为：
步骤5.1：若是，即k＝kmax，则位置解算单元输出k时刻的组合定位位置坐标，完成了本方法；
步骤5.2：若否，即k＜kmax，则位置解算单元输出k时刻的组合定位位置坐标，并令k＝k+1，跳至步骤2。
CN201710365995.5A 2017-05-23 2017-05-23 一种基于无迹卡尔曼滤波的组合定位方法 Pending CN107192387A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710365995.5A CN107192387A (zh) 2017-05-23 2017-05-23 一种基于无迹卡尔曼滤波的组合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710365995.5A CN107192387A (zh) 2017-05-23 2017-05-23 一种基于无迹卡尔曼滤波的组合定位方法

Publications (1)

Publication Number Publication Date
CN107192387A true CN107192387A (zh) 2017-09-22

Family

ID=59875824

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710365995.5A Pending CN107192387A (zh) 2017-05-23 2017-05-23 一种基于无迹卡尔曼滤波的组合定位方法

Country Status (1)

Country Link
CN (1) CN107192387A (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107861501A (zh) * 2017-10-22 2018-03-30 北京工业大学 地下污水处理厂智能机器人自主定位导航系统
CN108225330A (zh) * 2018-01-03 2018-06-29 华南理工大学 一种基于卡尔曼滤波的可见光动态定位方法
CN108387205A (zh) * 2018-01-20 2018-08-10 西安石油大学 基于多传感器数据融合的钻具姿态测量系统的测量方法
CN108616982A (zh) * 2018-04-12 2018-10-02 南京信息工程大学 一种智能建筑微区域内被动式人员定位及统计方法
CN109883416A (zh) * 2019-01-23 2019-06-14 中国科学院遥感与数字地球研究所 一种结合可见光通信定位和惯导定位的定位方法及装置
CN110068832A (zh) * 2019-04-23 2019-07-30 科罗玛特自动化科技（苏州）有限公司 一种激光导航agv的高精度定位方法
CN110225458A (zh) * 2019-01-14 2019-09-10 北京理工大学 一种基于混合滤波的uwb定位系统与方法
CN110296706A (zh) * 2019-07-11 2019-10-01 北京云迹科技有限公司 用于室内机器人的定位方法及装置、机器人
CN110940999A (zh) * 2019-12-13 2020-03-31 合肥工业大学 一种基于误差模型下的自适应无迹卡尔曼滤波方法
CN111780769A (zh) * 2020-07-29 2020-10-16 深圳市南科信息科技有限公司 单灯可见光定位方法
CN112595320A (zh) * 2020-11-23 2021-04-02 北京联合大学 基于ros的室内智能轮椅高精定位自主导航方法及系统
CN113884098A (zh) * 2021-10-15 2022-01-04 上海师范大学 一种基于具体化模型的可迭代的卡尔曼滤波定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105674986A (zh) * 2016-04-05 2016-06-15 中国电子科技集团公司第二十研究所 一种可见光与惯性组合的室内定位方法
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105674986A (zh) * 2016-04-05 2016-06-15 中国电子科技集团公司第二十研究所 一种可见光与惯性组合的室内定位方法
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HENRIQUE M. T. MENEGAZ: "A Systematization of the Unscented Kalman Filter Theory", 《IEEE TRANSACTIONS ON AUTOMATIC CONTROL》 *
董渊科: "基于非线性技术的AUV组合导航新方法", 《火力与指挥控制》 *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107861501A (zh) * 2017-10-22 2018-03-30 北京工业大学 地下污水处理厂智能机器人自主定位导航系统
CN108225330A (zh) * 2018-01-03 2018-06-29 华南理工大学 一种基于卡尔曼滤波的可见光动态定位方法
CN108387205A (zh) * 2018-01-20 2018-08-10 西安石油大学 基于多传感器数据融合的钻具姿态测量系统的测量方法
CN108387205B (zh) * 2018-01-20 2021-01-01 西安石油大学 基于多传感器数据融合的钻具姿态测量系统的测量方法
CN108616982A (zh) * 2018-04-12 2018-10-02 南京信息工程大学 一种智能建筑微区域内被动式人员定位及统计方法
CN110225458A (zh) * 2019-01-14 2019-09-10 北京理工大学 一种基于混合滤波的uwb定位系统与方法
CN110225458B (zh) * 2019-01-14 2020-10-09 北京理工大学 一种基于混合滤波的uwb定位系统与方法
CN109883416A (zh) * 2019-01-23 2019-06-14 中国科学院遥感与数字地球研究所 一种结合可见光通信定位和惯导定位的定位方法及装置
CN110068832A (zh) * 2019-04-23 2019-07-30 科罗玛特自动化科技（苏州）有限公司 一种激光导航agv的高精度定位方法
CN110296706A (zh) * 2019-07-11 2019-10-01 北京云迹科技有限公司 用于室内机器人的定位方法及装置、机器人
CN110940999A (zh) * 2019-12-13 2020-03-31 合肥工业大学 一种基于误差模型下的自适应无迹卡尔曼滤波方法
CN111780769A (zh) * 2020-07-29 2020-10-16 深圳市南科信息科技有限公司 单灯可见光定位方法
CN112595320A (zh) * 2020-11-23 2021-04-02 北京联合大学 基于ros的室内智能轮椅高精定位自主导航方法及系统
CN113884098A (zh) * 2021-10-15 2022-01-04 上海师范大学 一种基于具体化模型的可迭代的卡尔曼滤波定位方法
CN113884098B (zh) * 2021-10-15 2024-01-23 上海师范大学 一种基于具体化模型的可迭代的卡尔曼滤波定位方法

Similar Documents

Publication Publication Date Title
CN107192387A (zh) 一种基于无迹卡尔曼滤波的组合定位方法
CN107289933B (zh) 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN103499350B (zh) Gps盲区下融合多源信息的车辆高精度定位方法及装置
CN107796391A (zh) 一种捷联惯性导航系统/视觉里程计组合导航方法
CN106597017B (zh) 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN107478214A (zh) 一种基于多传感器融合的室内定位方法及系统
CN108759833A (zh) 一种基于先验地图的智能车辆定位方法
CN105652306A (zh) 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
CN108871336A (zh) 一种车辆位置估算系统及方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN108592914A (zh) 无gps情况下的复杂区域巡视机器人定位、导航及授时方法
CN109443348A (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN107289932A (zh) 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN111880207A (zh) 一种基于小波神经网络的视觉惯性卫星紧耦合定位方法
CN107702712A (zh) 基于惯性测量双层wlan指纹库的室内行人组合定位方法
CN108759823A (zh) 基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法
CN107270898A (zh) 基于mems传感器和vlc定位融合的双粒子滤波导航装置和方法
CN108627864A (zh) 基于汽车钥匙的定位方法及系统、无人驾驶汽车系统
CN109741372A (zh) 一种基于双目视觉的里程计运动估计方法
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
Chiang et al. Semantic proximity update of GNSS/INS/VINS for seamless vehicular navigation using smartphone sensors
CN108387228A (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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20170922