一种单制造技术

本发明专利技术提供了一种单

【技术实现步骤摘要】
一种单NLOS状态下的UWB室内机器人定位方法


[0001]本专利技术涉及室内机器人定位
，特别是涉及一种单
NLOS
状态下的
UWB
室内机器人定位方法
。

技术介绍

[0002]随着小型化与可穿戴智能电子设备的流行，更多研究者考虑将惯性测量单元
(Inertial Measurement Unit
，
IMU)
与无线通信设备通过滤波算法进行数据融合，以实现室内外稳健的无缝定位
。
贝叶斯框架下的经典滤波算法主要有卡尔曼滤波器
(Kalman Filter
，
KF)、
扩展卡尔曼滤波器
(Enhance Kalman Filter
，
EKF)、
无迹卡尔曼滤波器
(Unscented Kalman Filter
，
UKF)
，以及粒子滤波器
(Particle Filter
，
PF)。KF
在解决线性优化问题上独具优势，
EKF、UKF
和
PF
则在非线性问题中应用更加广泛
。
[0003]已有研究提出了基于超声定位系统与
IMU
的
EKF/FIR
联合定位算法，通过设置加权系数，将次优的
EKF
滤波器与更加稳健的
FIR
滤波器进行融合，可以在非视距
(NLOS)
条件下达到比单一滤波器更高的精度
。
另一研究针对
UWB/INS
定位时粒子滤波器粒子贫化的问题，提出了一种基于遗传重采样的粒子滤波器，在重采样阶段引入遗传算法优化
。
通过粒子的自我寻优，将粒子集导向到高似然区域
。
但显著增加了滤波定位解算时间
。
在融合定位时，如果滤波算法中的状态噪声选择不对，滤波器可能存在滤波发散的风险
。
鉴于此，有学者提出了一种自适应的智能
UKF
滤波算法，可以在
NLOS
环境下滤波估计出现故障时，实现自我恢复，而且其中的故障诊断策略具有良好的可移植性
。
[0004]从上述研究中可知，结合滤波算法进行融合定位的方案，是有望减轻室内
NLOS
误差影响的有效手段
。
但如果目标长时间处于
NLOS
频发的环境中，滤波算法相对于更依赖状态方程对目标位置进行估计，而状态方程的误差却是随时间累积的
。
因此，设计一种单
NLOS
状态下的
UWB
室内机器人定位方法是十分有必要的
。

技术实现思路

[0005]本专利技术的目的是提供一种单
NLOS
状态下的
UWB
室内机器人定位方法，结合分布式粒子滤波方案，对目标的状态与
NLOS
误差来源进行判别，实现了在目标处于单基站处于
NLOS
状态时的有效定位
。
[0006]一种单
NLOS
状态下的
UWB
室内机器人定位方法，包括如下步骤：
[0007]步骤1：判断粒子滤波是否因粒子贫化导致跟踪失败；
[0008]步骤2：若未发生跟踪失败，则不做处理，若发生跟踪失败，则基于差分进化算法对粒子集进行优化；
[0009]步骤3：基于粒子集判断目标的状态，并判断测量之中是否含有
NLOS
误差以及
NLOS
误差的来源，并对粒子集进行误差优化；
[0010]步骤4：基于误差优化后的粒子集进行机器人定位
。
[0011]可选的，步骤1中，判断粒子滤波是否因粒子贫化导致跟踪失败，具体为：
[0012]设
k
时刻重采样之后的粒子集为通过各基站的测量距离对目标做一个初步测距，结合
PF
得到一个滤波估计值为将代入到测量方程
H(
·
)
之中，计算相对其几个定位基站的距离：
[0013][0014][0015]式中，
M
为基站个数，定义
Dist
为与
Y
uwb,k
之间的后验马氏距离，为：
[0016][0017]式中，
Y
uwb,k
为
UWB
系统得到的测距值，
R
k
为
k
时刻的测量噪声的协方差矩阵，通过卡方分布表评价当前滤波值的可信度，取可信概率为
0.01
，使得卡方值
χ2作为阈值，若
Dist
大于阈值
χ2，则判断发生跟踪失败，基于差分进化算法对粒子集进行优化，否则，则将作为输出值传递到下一时刻，并将粒子集也传递到下一时刻
。
[0018]可选的，步骤2中，基于差分进化算法对粒子集进行优化，具体为：
[0019]设
k
时刻经过状态方程
f(
·
)
得来的原始粒子集为为：
[0020][0021]此时还未进行重采样操作，代表
k
时刻粒子集状态的原始分布，若跟踪失败，则将权重函数作为差分进化算法的适应度函数，计算粒子集中各粒子的权重值为：
[0022][0023]式中，是将第
i
个粒子将其带入到测量方程
H(
·
)
所得到的测量值，
i
＝
1,2,...
，
N
，
R
k
为
k
时刻的测量噪声的协方差矩阵；
[0024]计算每个粒子的后验马氏距离，为：
[0025][0026]设置可信度区间的上界下界为每个粒子设置一个指示值
index(x
i
)
，并初始两个空集
gp
与
mp
，设置
gp
的大小为三分之二
N
，
mp
的大小为三分之一
N
，当
Dist
小于认为此粒子为可信粒子，将该粒子置于
gp
中，并把相应的指示值
index(x
i
)
置为1，当
Dist
位于之间，则认为粒子为次优粒子，将该粒子置于
mp
中，并把相应的指示值
index(x
i
)
置为2，当
Dist
大于时，则认为该粒子为不可信粒子，将该粒子的指示值
index(x
i
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种单
NLOS
状态下的
UWB
室内机器人定位方法，其特征在于，包括如下步骤：步骤1：判断粒子滤波是否因粒子贫化导致跟踪失败；步骤2：若未发生跟踪失败，则不做处理，若发生跟踪失败，则基于差分进化算法对粒子集进行优化；步骤3：基于粒子集判断目标的状态，并判断测量之中是否含有
NLOS
误差以及
NLOS
误差的来源，并对粒子集进行误差优化；步骤4：基于误差优化后的粒子集进行机器人定位
。2.
根据权利要求1所述的单
NLOS
状态下的
UWB
室内机器人定位方法，其特征在于，步骤1中，判断粒子滤波是否因粒子贫化导致跟踪失败，具体为：设
k
时刻重采样之后的粒子集为通过各基站的测量距离对目标做一个初步测距，结合
PF
得到一个滤波估计值为将代入到测量方程
H(
·
)
之中，计算相对其几个定位基站的距离：个定位基站的距离：式中，
M
为基站个数，定义
Dist
为与
Y
uwb,k
之间的后验马氏距离，为：式中，
Y
uwb,k
为
UWB
系统得到的测距值，
R
k
为
k
时刻的测量噪声的协方差矩阵，通过卡方分布表评价当前滤波值的可信度，取可信概率为
0.01
，使得卡方值
χ2作为阈值，若
Dist
大于阈值
χ2，则判断发生跟踪失败，基于差分进化算法对粒子集进行优化，否则，则将作为输出值传递到下一时刻，并将粒子集也传递到下一时刻
。3.
根据权利要求2所述的单
NLOS
状态下的
UWB
室内机器人定位方法，其特征在于，步骤2中，基于差分进化算法对粒子集进行优化，具体为：设
k
时刻经过状态方程
f(
·
)
得来的原始粒子集为为：此时还未进行重采样操作，代表
k
时刻粒子集状态的原始分布，若跟踪失败，则将权重函数作为差分进化算法的适应度函数，计算粒子集中各粒子的权重值为：式中，是将第
i
个粒子将其带入到测量方程
H(
·
)
所得到的测量值，
i
＝
1,2,...
，
N
，
R
k
为
k
时刻的测量噪声的协方差矩阵；计算每个粒子的后验马氏距离，为：设置可信度区间的上界下界为每个粒子设置一个指示值
index(x
i
)
，并初始两个空集
gp
与
mp
，设置
gp
的大小为三分之二
N
，
mp
的大小为三分之一
N
，当
Dist
小于认为此粒子为可信粒子，将该粒子置于
gp
中，并把相应的指示值
index(x
i
)
置为1，当
Dist
位于之间，则认为粒子为次优粒子，将该粒子置于
mp
中，并把相应的指示值
index(x
i
)
置为2，当
Dist
大于时，则认为该粒子为不可信粒子，将该粒子的指示值
index(x
i
)
置为0；对粒子集中的离子采用差分算法优化，初始化迭代此处
G
，对指示值
index(x
i
)
为1或2的粒子不进行变异操作，对指示值
index(x
i
)
为0的粒子进行变异操作，将其中的
F1及
F2设置成方差相同，均值不同的正态分布的随机数，为：
F1＝
normrnd(0.5,1)F2＝
normrnd(0,1)
在
gp
中选择两个随机粒子
x
r1
及
x
r2
，选择当前时刻适应度最高的粒子
x
best
，得到变异粒子后，计算变异粒子的适应度值，通过选择操作比较变异粒子与原粒子的权重，保留更优粒子，计算更优粒子的后验马氏距离，并根据后验马氏距离所处的区间更新更优粒子的指示值，将指示值为1的优质粒子置于
gp
中，将指示值为2的优质粒子置于
mp
中，在
gp
中元素满后，将粒子与
mp
中的离子交叉，使其向
mp
中转移，直至两个离子集均达到峰值，整合为一个总集输出；结束后，对粒子的权重进行归一化，按照加权求和的方式计算此时刻的滤波估计值，为：式中，为经过差分采样后的粒子
。4.
根据权利要求3所述的单
NLOS
状态下的
UWB
室内机器人定位方法，其特征在于，步骤3中，基于粒子集判断目标的状态，并判断测量之中是否含有
NLOS
误差以及
NLOS
误差的来源，并对粒子集进行误差优化，具体为：设
k
‑1时刻的粒子集为健康的，且
...

【专利技术属性】
技术研发人员：杨永辉，许函铭，
申请(专利权)人：深圳市微能信息科技有限公司，
类型：发明
国别省市：