CN109916407A - 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法 - Google Patents

Abstract

基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，包括：S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；S3、对量测值中的野值进行处理，得到修正值；S4、使用自适应估计算法估计修正值的噪声协方差；S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。本发明提供一种基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，具有高定位精度和高鲁棒性。

Description

基于自适应卡尔曼滤波器的室内移动机器人组合定位方法

技术领域

本发明涉及组合定位技术领域，具体的说是基于自适应卡尔曼滤波器的室内移动机器人组合定位方法。

背景技术

移动机器人定位是其实现导航及其他任务的基础，定位的精确度直接影响到机器人完成任务的质量。近年来，随着机器人技术的飞速发展，人们对机器人在室内环境下完成任务的需求越来越多，尤其是复杂室内环境下的移动机器人定位技术是该领域的一个重要研究方向。

由于室内环境的封闭性，卫星导航系统(Global Navigation Satellite System，GNSS)往往难以提供一致性和长期稳定的位置信息。惯性导航系统(Inertial NavigationSystem，INS)在室内定位中应用广泛，虽然其不受外界环境变化的干扰，但存在累积误差，不适合长距离精确定位。在室内环境下，无线传感器网络(Wireless SensorNetwork,WSN)被广泛应用于室内定位，如WiFi，蓝牙，射频识别(Radio frequencyidentification，RFID)，超宽带(UltraWideband，UWB)等技术。其中，UWB较其他技术具有发射功率低、测量精度高等优点，近年来受到越来越多关注。但由于室内环境的复杂性产生的多径效应、非视距因素(Non-LineofSight，NLOS)会对UWB信号造成干扰，进而引起较大的定位误差。综上所述，单一的定位技术往往不能满足复杂室内环境下的定位需求。

组合定位是一种融合了相对定位和绝对定位的技术。在组合定位系统中，两种定位技术特性互补，克服了各自的缺点。最近，INS/UWB组合定位技术逐渐成为研究热点，扩展卡尔曼滤波(ExtendedKalmanfilter，EKF)，无迹卡尔曼滤波器(UnscentedKalman filter,UKF),容积卡尔曼滤波器(Cubature Kalman filter,CKF)等非线性滤波器被广泛应用于INS/UWB组合定位中。上述非线性滤波器假定过程和量测噪声协方差为常数，但是，在复杂室内环境下，UWB量测数据会受到多种因素(动态环境、多径效应、非视距因素等)的影响，导致量测噪声具有时变特性，并且可能在量测值中出现野值，不仅使状态估计失去了最优性，甚至可能导致滤波器的发散，进而出现较大的定位误差，不能满足室内移动机器人的任务需求。为解决上述问题，Fan等人使用简化的Sage-Husa自适应滤波器对量测噪声协方差进行自适应估计。Zhong等人将强跟踪滤波器应用到组合定位系统中，以应对无法预测的动态因素。然而，这些组合定位方法没有考虑到野值对定位系统性能带来的影响。

为了提高组合定位系统的鲁棒性，文献“Zhen W,Zeng S,Soberer S.Robustlocalization and localizability estimation with a rotating laser scanner[C].IEEE International Conference on Robotics&Automation.IEEE,2017:6240-6245.

”中使用误差状态卡尔曼滤波器(ESKF)来融合异质传感器数据，对无法预测的误差因素进行了有效补偿，但是没有考虑时变噪声的情况。文献“Han H,Xu T,WangJ.Tightly coupled integration of GPS ambiguity fixed precise pointpositioning and MEMS-INS through a troposphere-constrained adaptive Kalmanfilter[J].Sensors,2016,16(7):1057.”中使用简化的Sage-Husa自适应滤波器(SHAF)对量测噪声协方差进行自适应估计，但是该方法没有考虑到野值对定位系统性能带来的影响。

发明内容

为了解决现有技术中的不足，本发明提供一种基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，具有高定位精度和高鲁棒性。

为了实现上述目的，本发明采用的具体方案为：基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括如下步骤：

S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；

S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；

S3、对量测值中的野值进行处理，得到修正值；

S4、使用自适应估计算法估计修正值的噪声协方差；

S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。

作为一种优选方案，S1的具体方法为：

S1.1、设定状态向量并且赋初值，状态向量包括标称状态和误差状态，标称状态表示为其中为空间位置向量，为速度向量，为角度向量，为加速度偏置向量，为角速度偏置向量，误差状态δp为空间位置误差量向量，δv为速度误差量向量，δθ为角度误差量向量，δa_b为加速度偏置误差量向量，δω_b为角速度偏置误差量向量；

S1.2、基于状态向量构建惯导运动学模型，惯导运动学模型的状态转移方程为：

其中为控制量，a_m,k为惯性导航系统中三轴加速度计的量测值，ω_m,k为惯性导航系统中陀螺仪的量测值，k表示时刻，C为室内机器人机体坐标系到惯性导航系统坐标系的旋转矩阵，q为C的四元数形式，g为重力向量，I₃为3×3的单位矩阵，Δt为采样时间，q{·}表示将角度变化量转换为四元数形式，表示四元数乘法，[·]_×表示斜对称算子，F为误差状态转移矩阵；

S1.3、构建超宽带量测模型的量测方程：

e_n,k＝||p_n-p_t,k||₂-d_n,k,n＝1,2,...,N；

其中，d_n,k表示超宽带定位标签到第n个超宽带定位锚点之间的距离，p_m,k为超宽带定位系统解算出的室内机器人位置，e_n,k为量测距离与真实距离之间的误差，p_n＝[x_n,y_n,z_n]^T为超宽带定位锚点在惯性导航坐标系下位置，n∈{1,2,3,…,N}表示超宽带定位锚点编号，N为超宽带定位锚点总数，p_t,k为超宽带定位标签的真实位置，v_m,k为超宽带定位系统计算出的室内机器人速度；

S1.4、初始化自适应卡尔曼滤波器：

其中P_k|k-1为误差状态协方差的预测值，Γ_n为噪声驱动矩阵，Q_n为过程噪声的协方差矩阵。作为一种优选方案，S2中，组合量测方程为：

其中z_k为量测值，p_INS,k为惯性导航系统递推出的室内机器人位置，v_INS,k为惯性导航系统递推出的室内机器人速度，为量测噪声向量，R为量测噪声协方差阵，为误差状态的量测矩阵。

作为一种优选方案，S3的具体方法为：

S3.1、估计新息协方差：

ε_k＝z_k-Hδx_k|k-1；

其中ε_k为新息，S_k为通过带衰减因子的滑窗估计法求得的新息协方差的估计值，l为滑动窗口大小，σ_i＝a^k-i(1-a)/(1-a^l)为衰减系数，a表示衰减率，并且有0.95≤a≤0.99，ε_i为时刻i时的新息；

S3.2、根据新息正交性理论计算量测值方差估计值和量测值方差理论值：

S3.3、根据量测值方差估计值与量测值方差理论值之间的差异判断量测值是否为野值，如果是则进行修正，反之则不进行处理；

M_j,k＝G_j,k/D_j,k；

z′_k＝fr_k×z_k；

其中，G_j,k为中的第j个对角元素，D_j,k为D_k中的第j个对角元素，fr_k＝diag[fr_1,k,fr_2,k…fr_rank(R),k]为野值修正矩阵。

作为一种优选方案，S4的具体方法为：

S4.1、计算新息的估计值与理论值：

S_k＝HP_k|k-1H^T+R_k；

其中S_k为新息的理论值，为新息的估计值；

S4.2、计算自适应调节因子：

其中Tr(·)为求矩阵的迹，fuzzy(·)表示模糊推理系统，r_k为模糊推理系统的输入，s_k为模糊推理系统的输出，k_s为模糊推理系统起作用的时刻，模糊推理系统的推理规则为

S4.3、计算噪声协方差估计值：

其中d_k＝(λ-b)/(λ-b^k+1)为新息贡献权值，λ为大于等于1的常数，s_k为d_k的自适应调节因子，α为s_k的缩放系数。

作为一种优选方案，S5的具体方法为：

S5.1、将噪声协方差代入到自适应卡尔曼滤波器的增益计算公式中求得卡尔曼增益：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1；

S5.2、利用卡尔曼增益更新误差状态和误差状态协方差：

δx_k＝K_kε_k；

P_k＝(I-K_kH)P_k|k-1；

S5.3、将更新后的误差状态代入到标称状态中对标称状态进行更新，并且重置误差状态为0：

δx_k＝0。

有益效果：本发明通过野值处理和噪声协方差自适应估计的共同作用，有效提高了定位精度和鲁棒性。

附图说明

图1是本发明的流程图；

图2是本发明模糊推理系统输入的隶属度函数示意图；

图3是本发明模糊推理系统输出的隶属度函数示意图；

图4是仿真实验中本发明与几种现有技术的定位轨迹示意图；

图5是仿真实验中本发明与几种现有技术的定位误差示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括S1至S5。

S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器。S1的具体方法包括S1.1至S1.3。

S1.1、设定状态向量并且赋初值，状态向量包括标称状态和误差状态，标称状态表示为其中为空间位置向量，为速度向量，为角度向量，为加速度偏置向量，为角速度偏置向量，误差状态δp为空间位置误差量向量，δv为速度误差量向量，δθ为角度误差量向量，δa_b为加速度偏置误差量向量，δω_b为角速度偏置误差量向量。

S1.2、基于状态向量构建惯导运动学模型，惯导运动学模型的状态转移方程为：

其中为控制量，a_m,k为惯性导航系统中三轴加速度计的量测值，ω_m,k为惯性导航系统中陀螺仪的量测值，k表示时刻，C为室内机器人机体坐标系到惯性导航系统坐标系的旋转矩阵，q为C的四元数形式，g为重力向量，I₃为3×3的单位矩阵，Δt为采样时间，q{·}表示将角度变化量转换为四元数形式，表示四元数乘法，[·]_×表示斜对称算子，F为误差状态转移矩阵。

S1.3、构建超宽带量测模型的量测方程：

e_n,k＝||p_n-p_t,k||₂-d_n,k,n＝1,2,...,N；

其中，d_n,k表示超宽带定位标签到第n个超宽带定位锚点之间的距离，p_m,k为超宽带定位系统解算出的室内机器人位置，e_n,k为量测距离与真实距离之间的误差，p_n＝[x_n,y_n,z_n]^T为超宽带定位锚点在惯性导航坐标系下位置，n∈{1,2,3,…,N}表示超宽带定位锚点编号，N为超宽带定位锚点总数，p_t,k为超宽带定位标签的真实位置，v_m,k为超宽带定位系统计算出的室内机器人速度；

S1.4、初始化自适应卡尔曼滤波，本发明中自适应卡尔曼滤波器选择为Sage-Husa模糊自适应滤波器：

其中P_k|k-1为误差状态协方差的预测值，Γ_n为噪声驱动矩阵，Q_n为过程噪声的协方差矩阵。

S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值。组合量测方程为：

其中z_k为量测值，p_INS,k为惯性导航系统递推出的室内机器人位置，v_INS,k为惯性导航系统递推出的室内机器人速度，为量测噪声向量，R为量测噪声协方差阵，为误差状态的量测矩阵。

S3、对量测值中的野值进行处理，得到修正值。S3的具体方法包括S3.1至S3.3。

S3.1、估计新息协方差：

ε_k＝z_k-Hδx_k|k-1；

其中ε_k为新息，S_k为通过带衰减因子的滑窗估计法求得的新息协方差的估计值，l为滑动窗口大小，σ_i＝a^k-i(1-a)/(1-a^l)为衰减系数，a表示衰减率，并且有0.95≤a≤0.99，ε_i为时刻i时的新息。

S3.2、根据新息正交性理论计算量测值方差估计值和量测值方差理论值：

S3.3、根据量测值方差估计值与量测值方差理论值之间的差异判断量测值是否为野值，如果是则进行修正，反之则不进行处理；

M_j,k＝G_j,k/D_j,k；

z′_k＝fr_k×z_k；

其中，G_j,k为中的第j个对角元素，D_j,k为D_k中的第j个对角元素，fr_k＝diag[fr_1,k,fr_2,k…fr_rank(R),k]为野值修正矩阵。

S4、使用自适应估计算法估计修正值的噪声协方差。S4的具体方法包括S4.1至S4.3。

S4.1、计算新息的估计值与理论值：

S_k＝HP_k|k-1H^T+R_k；

其中S_k为新息的理论值，为新息的估计值。

S4.2、计算自适应调节因子：

其中Tr(·)为求矩阵的迹，fuzzy(·)表示模糊推理系统，r_k为模糊推理系统的输入，s_k为模糊推理系统的输出，输入与输出的隶属度函数如图2和3所示，k_s为模糊推理系统起作用的时刻，模糊推理系统的推理规则为

S4.3、计算噪声协方差估计值：

其中d_k＝(λ-b)/(λ-b^k+1)为新息贡献权值，λ为大于等于1的常数，s_k为d_k的自适应调节因子，α为s_k的缩放系数。

S5、将噪声协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。S5的具体方法包括S5.1至S.3。

S5.1、将噪声协方差代入到自适应卡尔曼滤波器的增益计算公式中求得卡尔曼增益：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1。

S5.2、利用卡尔曼增益更新误差状态和误差状态协方差：

δx_k＝K_kε_k；

P_k＝(I-K_kH)P_k|k-1。

S5.3、将更新后的误差状态代入到标称状态中对标称状态进行更新，并且重置误差状态为0：

δx_k＝0。

本发明(以下简称SHFAF)通过野值处理和噪声协方差自适应估计的共同作用，有效提高了定位精度和鲁棒性。为了验证本发明的效果，通过仿真实验将本发明与几种现有技术进行对比，具体仿真结果如图4和5所示，可以看出，SHFAF的绝对定位误差基本在0.2m以内，而ESKF由于没有自适应估计能力，所以在遇到野值时会产生较大的定位误差，SHAF因为具有一定的自适应估计能力，所以相较于ESKF具有更小的定位误差，但是与SHFAF相比其定位误差更大，也说明SHAF的自适应估计能力没有SHFAF强。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括如下步骤：

S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；

S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；

S3、对量测值中的野值进行处理，得到修正值；

S4、使用自适应估计算法估计修正值的噪声协方差；

S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。

2.如权利要求1的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S1的具体方法为：

S1.1、设定状态向量并且赋初值，状态向量包括标称状态和误差状态，标称状态表示为其中为空间位置向量，为速度向量，为角度向量，为加速度偏置向量，为角速度偏置向量，误差状态δp为空间位置误差量向量，δv为速度误差量向量，δθ为角度误差量向量，δa_b为加速度偏置误差量向量，δω_b为角速度偏置误差量向量；

S1.2、基于状态向量构建惯导运动学模型，惯导运动学模型的状态转移方程为：

其中为控制量，a_m,k为惯性导航系统中三轴加速度计的量测值，ω_m,k为惯性导航系统中陀螺仪的量测值，k表示时刻，C为室内机器人机体坐标系到惯性导航系统坐标系的旋转矩阵，q为C的四元数形式，g为重力向量，I₃为3×3的单位矩阵，Δt为采样时间，q{·}表示将角度变化量转换为四元数形式，表示四元数乘法，[·]_×表示斜对称算子，F为误差状态转移矩阵；

S1.3、构建超宽带量测模型的量测方程：

e_n,k＝||p_n-p_t,k||₂-d_n,k,n＝1,2,...,N；

其中，d_n,k表示超宽带定位标签到第n个超宽带定位锚点之间的距离，p_m,k为超宽带定位系统解算出的室内机器人位置，e_n,k为量测距离与真实距离之间的误差，p_n＝[x_n,y_n,z_n]^T为超宽带定位锚点在惯性导航坐标系下位置，n∈{1,2,3,…,N}表示超宽带定位锚点编号，N为超宽带定位锚点总数，p_t,k为超宽带定位标签的真实位置，v_m,k为超宽带定位系统计算出的室内机器人速度；

S1.4、初始化自适应卡尔曼滤波器：

其中P_k|k-1为误差状态协方差的预测值，Γ_n为噪声驱动矩阵，Q_n为过程噪声的协方差矩阵。

3.如权利要求2的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S2中，组合量测方程为：

其中z_k为量测值，p_INS,k为惯性导航系统递推出的室内机器人位置，v_INS,k为惯性导航系统递推出的室内机器人速度，为量测噪声向量，R为量测噪声协方差阵，为误差状态的量测矩阵。

4.如权利要求4的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S3的具体方法为：

S3.1、估计新息协方差：

ε_k＝z_k-Hδx_k|k-1；

其中ε_k为新息，S_k为通过带衰减因子的滑窗估计法求得的新息协方差的估计值，l为滑动窗口大小，σ_i＝a^k-i(1-a)/(1-a^l)为衰减系数，a表示衰减率，并且有0.95≤a≤0.99，ε_i为时刻i时的新息；

S3.2、根据新息正交性理论计算量测值方差估计值和量测值方差理论值：

S3.3、根据量测值方差估计值与量测值方差理论值之间的差异判断量测值是否为野值，如果是则进行修正，反之则不进行处理；

M_j,k＝G_j,k/D_j,k；

z′_k＝fr_k×z_k；

其中，G_j,k为中的第j个对角元素，D_j,k为D_k中的第j个对角元素，fr_k＝diag[fr_1,k,fr_2,k…fr_rank(R),k]为野值修正矩阵。

5.如权利要求4的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S4的具体方法为：

S4.1、计算新息的估计值与理论值：

S_k＝HP_k|k-1H^T+R_k；

其中S_k为新息的理论值，为新息的估计值；

S4.2、计算自适应调节因子：

其中Tr(·)为求矩阵的迹，fuzzy(·)表示模糊推理系统，r_k为模糊推理系统的输入，s_k为模糊推理系统的输出，k_s为模糊推理系统起作用的时刻，模糊推理系统的推理规则为

S4.3、计算噪声协方差估计值：

其中d_k＝(λ-b)/(λ-b^k+1)为新息贡献权值，λ为大于等于1的常数，s_k为d_k的自适应调节因子，α为s_k的缩放系数。

6.如权利要求5的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S5的具体方法为：

S5.1、将噪声协方差代入到自适应卡尔曼滤波器的增益计算公式中求得卡尔曼增益：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1；

S5.2、利用卡尔曼增益更新误差状态和误差状态协方差：

δx_k＝K_kε_k；

P_k＝(I-K_kH)P_k|k-1；

S5.3、将更新后的误差状态代入到标称状态中对标称状态进行更新，并且重置误差状态为0：

δx_k＝0。