CN113932809B - 基于智能粒子滤波的室内无人靶车定位方法 - Google Patents

Abstract

一种基于智能粒子滤波的室内无人靶车定位方法，实现步骤为：生成粒子滤波的初始粒子；确定每个定位时刻粒子滤波的状态坐标集；确定每个定位时刻粒子滤波的观测点坐标；计算每个定位时刻粒子滤波状态坐标集中每个粒子的权值；对每个定位时刻粒子滤波状态坐标集中的每个粒子进行重采样；计算每个定位时刻室内无人靶车的横、纵坐标值。本发明利用粒子滤波对惯性导航定位结果和UWB定位结果进行融合，并对粒子滤波状态坐标集中的粒子根据其权值大小进行选择、交叉、变异操作，有效的减少了粒子退化现象，与现有技术相比，提高了定位精度并延长了高精度定位时间。

Description

基于智能粒子滤波的室内无人靶车定位方法

技术领域

本发明属于通信技术领域，更进一步涉及无线跟踪技术领域中的一种基于智能粒子滤波的UWB(Ultra Wide Band)和惯性导航的室内无人靶车定位方法。本发明利用室内定位系统，估计室内无人靶车的二维位置。

背景技术

室内无人靶车是一种由人员在室内靶场进行射击训练时需要使用的一种可移动设备，随着科学技术的迅速发展，靶车上开始加载智能控制单元，为了方便对室内智能无人靶车的管理，必须要知道室内无人靶车所处的位置，即对无人靶车进行室内定位。应用于室内定位的方法有很多，其中基于基站的室内定位方法和基于惯性导航的室内定位方法是两种最为常见的室内定位方法。基于基站的室内定位方法利用信标接收到基站发送的短脉冲信号解算出目标物体在室内的具体位置。基于惯性导航的室内定位方法，利用搭载在目标物体上的惯性测量单元采集到的加速度和角速度解算出目标物体在室内的具体位置。两种室内定位方法都有其各自的优缺点，将这些定位方法进行合理地融合，将使系统的整体性能得到很大的提升，使室内定位结果更加准确。

电子科技大学在其申请的专利文献“基于粒子滤波算法的UWB/INS组合室内定位方法”(申请号：CN202010034636.3，申请公布号：CN111256695A)中公开了一种基于基站和惯导组合的室内定位方法。该方法首先通过UWB(超宽带)系统获取到待定位物体在室内的位置。然后通过INS(惯性导航系统)获取目标物体在运动过程中的三轴加速度、三轴角速度、以及三轴磁场强度，通过INS解算单元计算出目标在室内的位置。最后通过粒子滤波算法，将UWB系统计算结果与INS系统计算结果进行数据融合。达到了降低非视距复杂环境的影响、提高定位精度的目的。但是，该方法存在的不足之处是，由于其粒子滤波中用到的重要性采样方法只是根据归一化权值对粒子进行简单的复制和淘汰，容易随着时间的推移导致粒子贫化，最终使得融合算法失效，因此无法实现长期稳定的进行高精度定位。

发明内容

本发明的目的在于针对上述现有技术存在的缺陷，提出一种基于智能粒子滤波的室内靶车定位方法，用于解决现有技术中高精度定位不具有长期性和稳定性的问题。

实现本发明目的的思路是，本发明利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标，得到该定位时刻粒子滤波的状态坐标集。利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值，得到该定位时刻粒子滤波的观测点坐标。然后将每个时刻粒子滤波的状态坐标集和该时刻观测点坐标代入后续的粒子滤波操作中，得到该时刻最终的粒子滤波输出作为该时刻的融合定位结果，由于粒子滤波可以通过粒子分布模拟出方差最小的结果分布，因此以最终的粒子滤波输出作为融合定位结果具有跟踪二者中最佳定位结果的特点，克服了现有技术中使用其中任意一种定位方法导致室内无人靶车平均定位精度不高的不足。

本发明将粒子滤波状态坐标集中的粒子根据其权值大小分为两个群体，保留其中权值较大的群体，对权值小的群体进行交叉、变异操作，这样不但可以增大粒子在其他位置的可能性也可以使得粒子滤波中的大部分粒子自适应的分布在高似然区域内，使粒子滤波可以随着时间的推移自适应的保持粒子活性，克服了现有技术中粒子滤波会随着时间的推移产生粒子贫化现象而导致高精度定位结果失效的不足。

为实现上述目的，本发明采取的技术方案包括如下步骤：

步骤1，生成粒子滤波的初始粒子：

随机生成用于粒子滤波的M个初始粒子，在待定位靶车的区域内随机选择与每个粒子对应的坐标作为该初始粒子的坐标，M≥10；

步骤2，确定每个定位时刻粒子滤波的状态坐标集：

(2a)利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标；

(2b)将每个定位时刻所有粒子的坐标组成该定位时刻粒子滤波的状态坐标集；

步骤3，确定每个定位时刻粒子滤波的观测点坐标：

(3a)利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值；

(3b)将每个定位时刻无人靶车底座中心点的横、纵坐标初值作为该定位时刻粒子滤波观测点的横、纵坐标值；

步骤4，按照下式，计算每个定位时刻粒子滤波状态坐标集中每个粒子的权值：

其中，表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的权值，/>和/>表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的横、纵坐标值，/>分别表示第k个定位时刻粒子滤波观测点的横、纵坐标值；

步骤5，对每个定位时刻粒子滤波状态坐标集中的每个粒子进行重采样：

(5a)将在[0.6,0.8]中随机选取的一个小数W_T设定为阈值，将每个定位时刻粒子滤波状态坐标集中权值小于该阈值的粒子组成该定位时刻的衰败粒子集，其余部分组成该定位时刻的优秀粒子集；

(5b)利用下述交叉公式，对每个定位时刻衰败粒子集中的每个粒子进行交叉操作：

其中，表示第k个定位时刻衰败粒子集中的第l个粒子交叉后的粒子，/>表示第k个定位时刻衰败粒子集中的第l个粒子，/>表示第k个定位时刻优秀粒子集中的第l个粒子，α表示交叉系数，为在[0.6,0.8]中随机选取的一个小数；

(5c)利用如下变异公式，对交叉后的粒子进行变异操作：

其中，表示/>变异后的粒子，r^k表示第k个定位时刻的变异系数，为在[0,1]中随机选取的一个小数，p为(0,1)中随机选取的变异概率；

(5d)将每个定位时刻的优秀粒子集与该定位时刻所有经过变异后的粒子集合，作为该定位时刻重采样后的粒子滤波状态坐标集

步骤6，按照下式，计算每个定位时刻室内无人靶车的横、纵坐标值：

其中，和/>分别表示第k个定位时刻室内无人靶车底座中心点的横、纵坐标值，∑表示求和操作，/>和/>分别表示第k个定位时刻重采样后的粒子滤波状态坐标集中第i个粒子的横、纵坐标值。

本发明与现有技术相比，具有以下优点：

第一，由于本发明利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标，再利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值，克服了现有技术中使用其中任意一种定位方法导致室内无人靶车平均定位精度不高的不足，使得本发明提高了室内无人靶车的定位精度。

第二，本发明将粒子滤波状态坐标集中的粒子根据其权值大小分为两个群体，保留其中权值较大的群体，对权值小的群体进行交叉、变异操作，增大了粒子在其他位置的可能性，克服了现有技术中粒子滤波会随着时间的推移产生粒子贫化现象而导致高精度定位结果失效的不足，使得本发明高精度定位结果不会随着时间的推移而失效。

附图说明

图1为本发明的实现流程图；

图2为本发明与现有技术定位误差的仿真结果对比图。

具体实施方式

下面结合附图和具体实施例，对本发明作进一步详细描述。

参照图1，对本发明的实现步骤作进一步详细描述。

步骤1，生成粒子滤波的初始粒子。

本发明的实施例中，利用构建的一个搭载在无人靶车上的stm32微控制器型，随机生成100个用于粒子滤波的初始粒子，在待定位靶车的区域内随机选择与每个粒子对应的坐标作为该初始粒子的坐标。

步骤2，确定每个定位时刻粒子滤波的状态坐标集。

搭载在无人靶车上的微控制器利用中断控制的方式，控其上的MPU6050惯性传感器每隔1秒采集一次数据靶车运行的加速度和角速度，再利惯性导航积分算法，计算出每个定位时刻每个粒子的坐标值。并将每个定位时刻所有粒子的坐标组成该定位时刻粒子滤波的状态坐标集。

所述的利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标值如下：

其中，和/>分别表示第k个定位时刻第m个粒子的横、纵坐标值，cos和sin分别表示取余弦和取正弦操作，ω_k表示搭载在无人靶车上的惯性传感器在第k个定位时刻采集到的无人靶车的角速度，a_k表示搭载在无人靶车上的惯性传感器在第k个定位时刻采集到的无人靶车的加速度。

步骤3，确定每个定位时刻粒子滤波的观测点坐标。

搭载在无人靶车上的微控制器利用中断控制的方式，控其上的UWB信标每隔1秒采集一次由UWB基站发出的短脉冲信号飞行时间和脉冲到达角度，再利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值，并将该值作为该定位时刻粒子滤波观测点的横、纵坐标值。

所述的利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值如下：

其中，和/>分别表示第k个定位时刻无人靶车底座中心点的横、纵坐标初值，c₀表示电磁波在空气中的传播速度，τ_k表示搭载在无人靶车上的UWB信标在第k个定位时刻接收到的由UWB基站发出的短脉冲信号的飞行时间，θ_k表示短脉冲信号的到达角度，x_Y和y_Y分别表示室内UWB基站底座中心点的横、纵坐标值。

步骤4，按照下式，计算每个定位时刻粒子滤波状态坐标集中每个粒子的权值。

微控制器将步骤2得到的每个定位时刻粒子滤波状态坐标集和步骤3得到的该定位时刻粒子滤波的观测点坐标，根据下述粒子权值计算公式，计算出该定位时刻粒子滤波状态坐标集中每个粒子的权值。

其中，表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的权值，/>和/>表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的横、纵坐标值，/>分别表示第k个定位时刻粒子滤波观测点的横、纵坐标值。

步骤5，对每个定位时刻粒子滤波状态坐标集中的每个粒子进行重采样。

第一步，将在[0.6,0.8]中随机选取的一个小数W_T设定为阈值，本发明的实施例中W_T＝0.65,然后将每个定位时刻粒子滤波状态坐标集中权值小于该阈值的粒子组成该定位时刻的衰败粒子集，其余部分组成该定位时刻的优秀粒子集。

第二步，利用下述交叉公式，对每个定位时刻衰败粒子集中的每个粒子进行交叉操作：

其中，表示第k个定位时刻衰败粒子集中的第l个粒子交叉后的粒子，/>表示第k个定位时刻衰败粒子集中的第l个粒子，/>表示第k个定位时刻优秀粒子集中的第l个粒子，α表示交叉系数，为在[0.6,0.8]中随机选取的一个小数，本示例中α＝0.7。

第三步，利用如下变异公式，对交叉后的粒子进行变异操作：

其中，表示/>变异后的粒子，r^k表示第k个定位时刻的变异系数，为在[0,1]中随机选取的一个小数，p为(0,1)中随机选取的变异概率。本发明示例中，r^k为第k个定位时刻微处理器在[0,1]中随机选取的一个小数，p＝0.5。

第四步，将每个定位时刻的优秀粒子集与该定位时刻所有经过变异后的粒子集合，作为该定位时刻重采样后的粒子滤波状态坐标集

步骤6，按照下式，计算每个定位时刻室内无人靶车的横、纵坐标值。

微控制器利用每个定位时刻重采样后的粒子滤波状态坐标集根据下述室内无人靶车每个定位时刻横、纵坐标的计算公式，计算出每个定位时刻室内无人靶车的坐标，并在微控制器的显示屏上显示出来。

其中，和/>分别表示第k个定位时刻室内无人靶车底座中心点的横、纵坐标值，∑表示求和操作，/>和/>分别表示第k个定位时刻重采样后的粒子滤波状态坐标集中第i个粒子的横、纵坐标值。

下面结合仿真实验对本发明的效果做进一步的说明：

1.仿真条件。

本发明的仿真实验的硬件平台为：型号为FW-001的室内无人靶车、UWB基站、处理器为Intel i7-6700的笔记本电脑、设置在室内无人靶车上搭载有MPU6050传感器和UWB信标的STM32F103微控制器。

仿真软硬件环境：Windows 10，MATLAB R2019a。

本发明仿真实验所用到的数据，采集自西安电子科技大学西大楼实验室，采集时间为2021年10月，样本集的内容为100秒内每一秒的室内定位数据。

2.仿真内容及其结果分析。

本发明仿真实验是采用本发明和三个现有技术(基于惯性导航的室内定位方法、基于UWB的室内定位方法、基于粒子滤波算法的UWB/INS组合室内定位方法)分别对本发明所采集的100秒内每一秒的室内定位数据进行位置解算，得到室内无人靶车的定位结果，再将此定位结果除以实际的定位结果，得到定位精度并绘制成定位精度图，如图2所示。

在仿真实验中，采用的三个现有技术是指：

现有技术基于惯性导航的室内定位方法是指王旭凤在其发表的论文“基于惯性导航的室内定位系统的研究”([D].吉林大学,2015)中提到的室内定位方法。

现有技术基于UWB的室内定位方法是指，徐劲松等人在其发表的论文“基于UWB的室内定位系统设计与仿真”([J].全国时间频率学术会议，2009.)中提到的室内定位方法。

现有技术基于粒子滤波算法的UWB/INS组合室内定位方法是指，电子科技大学在其申请的专利文献“基于粒子滤波算法的UWB/INS组合室内定位方法”(申请号：CN202010034636.3，申请公布号：CN111256695A)中提到的室内定位方法。

下面结合图2的仿真图对本发明的效果做进一步的描述。

图2中的横坐标表示定位时间，纵坐标表示定位精度。图2中以“×”标示的曲线表示基于惯性导航的室内定位方法的仿真曲线，图2中以“○”标示的曲线表示基于UWB的室内定位方法的仿真曲线，图2中以“Δ”标示的曲线表示基于粒子滤波算法的UWB/INS组合室内定位方法的仿真曲线，图2中以“+”标示的曲线表示本发明所提到的基于智能粒子滤波的室内无人靶车定位方法的仿真曲线。由图2可以看出，在100秒的定位时间内，本发明所提出的方法定位精度稳定在95％附近；基于惯性导航的室内定位方法在前30秒内定位精度维持在90％附近，然后定位精度便随着时间推移越来越低；基于UWB的室内定位方法在100秒内，定位精度始终维持在90％附近；基于粒子滤波算法的UWB/INS组合室内定位方法前80秒内定位精度维持在95％附近，在后20秒定位精度随着时间推移变得越来越低。

以上仿真实验表明：由于本发明方法利用智能化粒子滤波将基于惯性导航的内定位方法结果和基于UWB的室内定位方法结果进行融合，提高了室内无人靶车的定位精度。同时本发明将粒子滤波状态坐标集中的粒子根据其权值大小分为两个群体，保留其中权值较大的群体，对权值小的群体进行交叉、变异操作，使粒子滤波可以随着时间的推移自适应的保持粒子活性，解决了现有技术中粒子滤波会随着时间的推移产生粒子贫化现象而导致高精度定位结果失效的的问题，是一种非常实用的室内无人靶车定位方法。

Claims (3)

1.一种基于智能粒子滤波的室内靶车定位方法，其特征在于，根据粒子权值大小将所有粒子分为两个群体，保留其中权值较大的群体，对权值小的群体进行交叉、变异操作；该室内靶车定位方法的具体步骤包括如下：

步骤1，生成粒子滤波的初始粒子：

随机生成用于粒子滤波的M个初始粒子，在待定位靶车的区域内随机选择与每个粒子对应的坐标作为该初始粒子的坐标，M≥10；

步骤2，确定每个定位时刻粒子滤波的状态坐标集：

(2a)利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标；

(2b)将每个定位时刻所有粒子的坐标组成该定位时刻粒子滤波的状态坐标集；

步骤3，确定每个定位时刻粒子滤波的观测点坐标：

(3a)利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值；

(3b)将每个定位时刻无人靶车底座中心点的横、纵坐标初值作为该定位时刻粒子滤波观测点的横、纵坐标值；

步骤4，按照下式，计算每个定位时刻粒子滤波状态坐标集中每个粒子的权值：

其中，表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的权值，/>和/>表示第k个定位时刻粒子滤波状态坐标集中第m个粒子的横、纵坐标值，/>分别表示第k个定位时刻粒子滤波观测点的横、纵坐标值；

步骤5，对每个定位时刻粒子滤波状态坐标集中的每个粒子进行重采样：

(5a)将在[0.6,0.8]中随机选取的一个小数W_T设定为阈值，将每个定位时刻粒子滤波状态坐标集中权值小于该阈值的粒子组成该定位时刻的衰败粒子集，其余部分组成该定位时刻的优秀粒子集；

(5b)利用下述交叉公式，对每个定位时刻衰败粒子集中的每个粒子进行交叉操作：

其中，表示第k个定位时刻衰败粒子集中的第l个粒子交叉后的粒子，/>表示第k个定位时刻衰败粒子集中的第l个粒子，/>表示第k个定位时刻优秀粒子集中的第l个粒子，α表示交叉系数，为在[0.6,0.8]中随机选取的一个小数；

(5c)利用如下变异公式，对交叉后的粒子进行变异操作：

其中，表示/>变异后的粒子，r^k表示第k个定位时刻的变异系数，为在[0,1]中随机选取的一个小数，p为(0,1)中随机选取的变异概率；

(5d)将每个定位时刻的优秀粒子集与该定位时刻所有经过变异后的粒子集合，作为该定位时刻重采样后的粒子滤波状态坐标集

步骤6，按照下式，计算每个定位时刻室内无人靶车的横、纵坐标值：

其中，和/>分别表示第k个定位时刻室内无人靶车底座中心点的横、纵坐标值，∑表示求和操作，/>和/>分别表示第k个定位时刻重采样后的粒子滤波状态坐标集中第i个粒子的横、纵坐标值。

2.根据权利要求1所述的基于智能粒子滤波的室内靶车定位方法，其特征在于，步骤(2a)中所述的利用惯性导航积分算法，确定每个定位时刻每个粒子的坐标值如下：

其中，和/>分别表示第k个定位时刻第m个粒子的横、纵坐标值，cos和sin分别表示取余弦和取正弦操作，ω_k表示搭载在无人靶车上的惯性传感器在第k个定位时刻采集到的无人靶车的角速度，a_k表示搭载在无人靶车上的惯性传感器在第k个定位时刻采集到的无人靶车的加速度。

3.根据权利要求1所述的基于智能粒子滤波的室内靶车定位方法，其特征在于，步骤(3a)中所述的利用UWB定位算法，获取每个定位时刻无人靶车底座中心点的横、纵坐标初值如下：

其中，和/>分别表示第k个定位时刻无人靶车底座中心点的横、纵坐标初值，c₀表示电磁波在空气中的传播速度，τ_k表示搭载在无人靶车上的UWB信标在第k个定位时刻接收到的由UWB基站发出的短脉冲信号的飞行时间，θ_k表示短脉冲信号的到达角度，x_Y和y_Y分别表示室内UWB基站底座中心点的横、纵坐标值。