一种基于粒子滤波的可见光组合定位系统和方法与流程

本发明涉及一种基于粒子滤波的可见光组合定位系统和方法，属于室内定位以及可见光定位
技术领域：
。
背景技术：
：随着智能设备技术的快速发展，人们对于基于位置的服务需求日益增加。但由于GPS等传统定位技术无法满足室内精细定位的需求，以蓝牙、WiFi、可见光定位技术为代表的室内定位技术开始崭露头角。其中，可见光定位以其精度高、成本低、易实现的特点倍受青睐。但可见光定位技术容易受到多径反射、地图限制、信号遮挡等问题的影响，因此限制了其实用性。惯性导航利用陀螺仪和加速度传感器所测得的载体加速度值和角速度值(可能还包括磁偏角值)计算出载体运动的方向和距离，由物体初始位置推导出实时位置坐标信息。由于其工作模式完全自主，短时精度优良，具有与生俱来的抗干扰特性，可以和可见光导航系统形成良好的互补。本发明采用包括行人惯性导航技术(PDR)在内的惯性导航技术对可见光定位进行辅助定位，采用粒子滤波算法对可见光定位系统/惯性导航系统的数据进行融合，从而实现组合定位导航功能。实验表明，组合定位导航系统有效减小了可见光定位系统的信号抖动、地图限制、信号遮挡等问题带来的误差，同时也极大地克服了纯惯性导航系统累积误差随时间增大的问题。整个系统既可以应用于机器人、AGV等工业领域，也可以应用于行人导航等消费级电子产品领域。与本发明相关的专利共两篇，下文分别对其进行剖析：专利(1)：申请号CN201410067768.0，标题为基于惯性定位和VLC技术的室内混合定位系统及方法，主要解决了纯可见光定位系统的可靠性不高，无法在较为复杂的室内环境中应用的问题。该专利所述系统在接收器可以接收到LED直射光线的室内环境下，定位结果由VLC室内定位系统提供；当接收器进入到室内没有直射光线的阴影区域时，定位结果由惯性定位系统提供，其中惯性定位系统所需的初始位置，由VLC室内定位系统提供，即可见光定位位置数据与惯性导航定位数据不进行融合只进行选择。专利(2)：申请号CN201410831922.7，标题为基于手机惯性定位和VLC的室内混合定位系统及方法，它利用当前智能手机中集成的硬件设备实现了可见光定位与惯性导航系统的简单组合，同时简化了定位装置，也可以利用目标位置来实现位置相关信息的推送，具有一定的实用意义。该专利中所述的APP模块把从通信接口获取的编码信号强度和阈值进行比较，当编码信号强度大于阈值时，计算出多个LED相对强度关系，根据强度关系得到光传输的衰减距离，定位出VLC定位接收器中的光电探测器所在的基于多个LED光源的相对位置关系；当编码信号强度低于阈值时，把惯性定位数值作为定位值。该专利与上述专利类似，并没有对可见光定位位置信息与惯性导航位置信息做深度融合，仅仅根据某阈值做选择处理。以上专利虽然解决了在可见光信号受到遮挡情况下定位系统无法正常工作的问题，但是由于其在工作时仅是单纯依赖是否接收到可见光信号作为选择不同定位系统数据的判据，因此并没有将惯性导航和可见光导航的定位数据进行深度融合，如果可见光信号强度正常但受到干扰或者信号错误的情况，例如可见光信号在靠近墙壁、立柱等区域受到多径干扰、或者由于LED布设区域限制载体位于可见光正常定位区域之外时，以上专利所提出的方法并不能减小误差，甚至不能正常工作。因此，可见光定位/惯性导航组合定位方法都具有很大的改进空间。本发明的目的即是致力于解决上述可见光定位/惯性导航组合定位方法的缺陷，提出基于粒子滤波的可见光定位/惯性导航组合定位系统。技术实现要素：本发明旨在解决已有的可见光定位与惯性导航组合定位系统无法在多径干扰区域以及部分可见光信号未覆盖区无法进行定位的缺陷，提出了一种基于粒子滤波的可见光组合定位系统和方法；一种基于粒子滤波的可见光组合定位系统和方法包括一种基于粒子滤波的可见光组合定位系统(简称本系统)和一种基于粒子滤波的可见光组合定位方法(简称本方法)；其中，本系统包括系统初始化单元、可见光定位位置采集解算单元、惯性导航运动参数采集单元、粒子产生与粒子滤波状态方程确定单元、权重确定单元、重采样单元以及预测单元；本方法，具体步骤如下：步骤1：系统初始化单元初始化循环计数值、设置工作模式布尔值和循环计数最大值，设置k＝1时刻组合定位位置的初始坐标值；其中，循环计数值记为k、循环计数最大值记为kmax、工作模式布尔值记为Bool；初始化k＝1；设置Bool值和kmax值，当Bool＝0时系统处于实时工作状态，kmax为无穷大值；Bool＝1时系统处于离线工作状态，kmax为一常数；设置第1时刻组合定位位置的初始坐标值为(m,n)；步骤2：粒子产生与粒子滤波状态方程确定单元产生粒子，具体为：按预先设定的先验概率p(x1)分布产生粒子群其中，其中，代表第1时刻粒子群中第i个粒子；Ns是粒子总数，粒子群中的每一个粒子权值为其中，Ns的范围为10～1000；其中，先验概率p(x1)的概率分布为在组合定位覆盖区域范围内的均匀分布，x1代表第1时刻的粒子；将第k时刻第i个粒子的坐标记为的取值范围为组合定位覆盖区域范围；步骤3：可见光定位位置采集解算单元解算可见光定位位置坐标，惯性导航运动参数采集单元采集惯性导航运动参数输入本系统的粒子产生与粒子滤波状态方程确定单元，并确定粒子滤波器状态方程；其中，可见光定位位置采集解算单元解算出的k时刻可见光定位位置坐标记为Zk，Zk为两行一列矩阵，第一行与第二行分别为横坐标值xk与纵坐标值yk；惯性导航运动参数包括运动距离和航向角，其中，将k-1时刻到k时刻内的运动距离记为Sk；将k-1时刻到k时刻内的平均航向角记为θk；其中，粒子滤波器状态方程为：X[1,k+1]=X[1,k]+Sk×cosθk+vkX[2,k+1]=X[2,k]+Sk×sinθk+vk---(1)]]>其中，X为k时刻的组合定位位置，X[1,k]、X[2,k]分别表示k时刻的X的横坐标值和纵坐标值，X[1,k+1]、X[2,k+1]分别表示k+1时刻的X的横坐标值和纵坐标值；cosθk代表余弦操作代表对k时刻的航向角取余弦；sinθk代表对k时刻的航向角取正弦；vk为k时刻的过程噪声；步骤4：权重确定单元按照设定的权重规则计算每个粒子的权重并归一化；其中，设定的权重规则如公式(2)所示：ωki=h(Zk-xki)),i=1,2...Ns---(2)]]>其中，为第i个粒子在k时刻的权重，h为该粒子坐标和k时刻可见光定位位置坐标之间距离的函数，具体为公式(3)；ωki=2πR×exp(-(dki)2R2)---(3)]]>其中，R为系统观测噪声；d为k时刻第i个粒子的坐标与可见光定位位置坐标Zk距离，计算公式为公式(4):dki=(X_xki-xk)2+(Y_yki-yk)2---(4)]]>求出后，使用公式(4)把归一化为ω*ki=ωkiΣi=1Nsωki---(5);]]>步骤5：重采样单元按照设定的重采样规则对粒子进行重采样，得到新的粒子替换原有粒子，得出新粒子群；所述的新粒子群，记为其中，设定的重采样规则包括但不限于：如文献1中2.1节所述的简单随机重采样方法；如文献1中2.2节所述的分层重采样方法、如文献1中2.3节所述的系统重采样以及如文献1中2.4节所述的残余重采样方法；以上四种重采样方法参见文献1：范澎湃,隋立芬,牟忠凯.粒子滤波常用重采样算法分析比较[C]//通信理论与信号处理学术年会.2008.；其中，文献1中所述的每个粒子的归一化权重为qi，对应本专利中的步骤6：计算k时刻的组合定位位置坐标；具体为：将步骤5中输出的新粒子群中所有粒子的平均位置作为k时刻的组合定位位置坐标Xk；步骤7：判断循环计数值是否已经达到计数最大值，并决定是否完成本方法，具体为：7.1若是，则输出k时刻的组合定位位置坐标Xk，完成了本方法；7.2若否，则输出k时刻的组合定位位置坐标Xk，并令k＝k+1，跳至步骤3；至此，从步骤1到步骤7，完成了一种基于粒子滤波的可见光组合定位方法。有益效果一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法，与现有技术相比，本方法具有如下有益效果：1、本方法极大地减小了可见光定位系统的信号抖动、地图限制、信号遮挡等问题带来的误差；2、本方法相较于单纯依靠选择滤波来对可见光定位数据和惯性导航定位数据进行融合的方式相比，融合效果更好，融合后误差更小；3、相较于其他滤波器，本方法所采用的粒子滤波器无需选择滤波等其他滤波器辅助，整个算法只需要一个粒子滤波器工作，可靠性更强，稳定性更好；4、本方法适用于不同形式的可见光定位与惯性定位的融合场景，既可以应用于机器人、AGV等工业领域，也可以应用于行人导航等消费级电子产品领域，应用广泛，实用性强，易于推广。附图说明图1为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”中的本方法及实施例1中的流程示意图；图2为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”实施例2中的重采样流程；图3为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”实施例3中的组合定位位置解算图；图4为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”实施例3中组合定位系统在靠近立柱区域，可见光信号由于多径效应干扰从而引起可见光定位位置抖动时，本发明与现有可见光定位系统的组合定位位置解算结果对比；图5为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”实施例3中组合定位系统在靠近立柱区域，可见光信号由于多径效应干扰从而引起可见光定位位置抖动时，本发明与现有可见光定位系统的组合定位位置解算结果误差对比；图6为本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”实施例3中组合定位系统在可见光信号没有覆盖情况下组合定位系统和现有可见光定位系统的解算结果对比。具体实施方式下面根据附图及实施例对本发明进行详细说明，但本发明的具体实施形式并不局限于此。实施例1本实施例阐述了将本发明“一种基于粒子滤波的可见光定位与惯性定位组合定位系统与方法”应用于以行人为目标的定位识别场景下的流程。图1为本方法的算法流程图以及本实施例的流程图。从图中可以看出，本方法包含如下步骤：步骤A：设置并初始化循环计数值k、工作模式布尔值Bool；设置循环最大值kmax；具体到本实施例，k被初始化为1；步骤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：产生粒子；具体到本实施例，粒子产生与粒子滤波状态方程确定单元在预定的组合定位区域范围内按照均匀分布的规则产生100个粒子；步骤D：解算可见光定位位置坐标，采集惯性导航运动参数以及确定粒子滤波器状态方程；解算过程与步骤3相同，具体到本实施例，k时刻的过程噪声为0.02，k＝1...kmax；步骤E：确定粒子权重并归一化；具体与步骤4相同，本实施例中设置系统观测噪声R为0.01；步骤F：对粒子进行重采样；具体为：重采样单元按照设定的重采样规则对粒子进行重采样，得到新的粒子替换原有粒子，得出新粒子群；步骤G：求粒子平均位置Xk；步骤H：判断是否k＜kmax，并决定是否完成本方法，具体为：H.1若是，对应图1中的“k＜kmax？”输出的Y，则输出本时刻位置Xk，并令k＝k+1，跳至步骤D；H.2若否，对应图1中的“k＜kmax？”输出的N，则输出本时刻位置Xk，完成了本方法；至此，从步骤A到H，完成了本实施例一种基于粒子滤波的可见光组合定位方法。实施例2本实施例具体阐述了本发明步骤5中叙述的简单随机重采样方法及实施例1中步骤F的重采样算法，算法流程如图2所示。从图2中可以看出，重采样算法的具体步骤为：步骤F.1：初始化粒子计数i＝1；步骤F.2：设定权重阈值ωmax与随机抽取粒子权重ωindex；其中，权重阈值为ωmax＝2×max(ω)×rand，rand为0-1间均匀分布的随机数，max(ω)为k时刻所有100个粒子中最大的权值；其中：随机抽取粒子权重ωindex的方法是从第i个到第100个粒子中任意抽取第index个粒子，将该粒子的权值记为ωindex；步骤F.3：判断是否ωmax＞ωindex，并进行相应操作，具体为：F.31：若是，对应图2中“ωmax＞ωindex”输出的Y，则令ωmax＝ωmax-ωindex，index＝index+1；F.32：若否，对应图2中“ωmax＞ωindex”输出的N，则跳至步骤F.5；步骤F.4：判断是否index>100，并进行相应操作，具体为F.41：若是，对应图2中“index>100”输出的Y，则令index＝1；F.42：若否，对应图2中“index>100”输出的N，则跳至步骤F.5；步骤F.5：令粒子步骤F.6：判断是否i＝100并进行相应操作，具体为：F.61：若否，对应图2中“i＝100”输出的N，令i＝i+1，跳至步骤F.2；F.62：若是，对应图2中“i＝100”输出的Y，重采样算法停止；至此，从步骤F.1到步骤F.6，完成了实施例1中步骤F的重采样算法。实施例3本实施例按照实施例1所述的参数及实施例2中所述的重采样方法，具体阐述了执行本发明步骤1到步骤7所得的kmax个组合定位位置结果，同时与现有可见光定位方法所得定位位置结果进行比较，比较结果如图3。图3中，XAxis表示横坐标，其单位为米；YAxis表示纵坐标，其单位为米；图3中虚线，即：“TrueTrack”为行人的真实轨迹；星划线，即：“PF”为经本发明所提出的“一种基于粒子滤波的可见光组合定位方法(简称本方法)”步骤1到步骤7所得的组合定位位置轨迹解算结果；十字划线，即“VLC”为
背景技术：
中专利(1)中提到的定位方法所得定位位置轨迹；需要说明的是：由于专利(1)和专利(2)核心技术相似，因此我们仅选取专利(1)中所述技术与本专利的定位效果进行对比。从图3中可见，本实施例中，定位区域的横坐标范围为(0,3)，纵坐标范围为(0,3.5)；行人按照(0.2,1)→(2.4,1)→(2.4,3.1)→(0.2,3.1)的矩形路径运动；其中，(2,1)→(2.4,1)→(2.4,2)路径受到多径干扰；(0.2,1)→(2.4,1)→(2.4,3.1)→(0.2,3.1)未被可见光信号覆盖。从图3中可以看出，PF曲线与VLC曲线相比，与TureTrack轨迹更为接近，且PF曲线相比与VLC曲线其抖动更小，定位误差更小。为了更好的看清楚本方法的优势，将图3中横坐标(2,2.5)，纵坐标(0.2,8)内的区域放大，放大后的结果图如图4所示。由图4可以清晰的看出，该区域内中PF曲线抖动较小，与真实轨迹更加吻合，PF曲线与TureTrack曲线之间最大偏差量为0.1m；而VLC曲线抖动较大，且与TureTrack曲线相比偏差较大，距离TureTrack曲线的最大偏差量达到了0.19m，表明了本发明有效减小了可见光定位系统的信号抖动问题；为了更好地说明本发明对于减小多径效应导致的定位位置抖动的有效性，我们将图4中PF曲线与VLC曲线相对于TureTrack曲线之间的误差作图，如图5所示。由图5可以看出，该区域内PF曲线的平均误差与VLC曲线的平均误差相比有大幅减小，计算可知采用本发明可以使定位位置误差减小48％，充分证明了本发明有效减小了可见光定位系统的信号抖动问题；为了更好的说明本发明对于克服系统地图限制、信号遮挡问题的作用，将图3中横坐标(0,1)、纵坐标(0,3.5)以内的区域进行放大，放大结果如图6所示。由图6可以看出，PF曲线与TureTrack曲线之间的误差更小，而VLC曲线在横坐标为(0,1)之内的区域与TureTrack曲线走向完全不同，存在很大误差，这也证明了本系统及本方法能够有效克服系统地图限制、信号遮挡等问题带来的误差。需要说明的是，本说明书所述的只是本发明的较佳具体实施例，以上实施例仅用于说明本发明的技术方案而非对本发明的限制。凡本领域技术人员依本发明的构思通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在本发明的范围之内。当前第1页1 2 3