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

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

【技术实现步骤摘要】
基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
本专利技术涉及组合定位
，具体的说是基于自适应卡尔曼滤波器的室内移动机器人组合定位方法。
技术介绍
移动机器人定位是其实现导航及其他任务的基础，定位的精确度直接影响到机器人完成任务的质量。近年来，随着机器人技术的飞速发展，人们对机器人在室内环境下完成任务的需求越来越多，尤其是复杂室内环境下的移动机器人定位技术是该领域的一个重要研究方向。由于室内环境的封闭性，卫星导航系统(GlobalNavigationSatelliteSystem，GNSS)往往难以提供一致性和长期稳定的位置信息。惯性导航系统(InertialNavigationSystem，INS)在室内定位中应用广泛，虽然其不受外界环境变化的干扰，但存在累积误差，不适合长距离精确定位。在室内环境下，无线传感器网络(WirelessSensorNetwork,WSN)被广泛应用于室内定位，如WiFi，蓝牙，射频识别(Radiofrequencyidentification，RFID)，超宽带(UltraWideband，UWB)等技术。其中，UWB较其他技术具有发射功率低、测量精度高等优点，近年来受到越来越多关注。但由于室内环境的复杂性产生的多径效应、非视距因素(Non-LineofSight，NLOS)会对UWB信号造成干扰，进而引起较大的定位误差。综上所述，单一的定位技术往往不能满足复杂室内环境下的定位需求。组合定位是一种融合了相对定位和绝对定位的技术。在组合定位系统中，两种定位技术特性互补，克服了各自的缺点。最近，INS/UWB组合定位技术逐渐成为研究热点，扩展卡尔曼滤波(ExtendedKalmanfilter，EKF)，无迹卡尔曼滤波器(UnscentedKalmanfilter,UKF),容积卡尔曼滤波器(CubatureKalmanfilter,CKF)等非线性滤波器被广泛应用于INS/UWB组合定位中。上述非线性滤波器假定过程和量测噪声协方差为常数，但是，在复杂室内环境下，UWB量测数据会受到多种因素(动态环境、多径效应、非视距因素等)的影响，导致量测噪声具有时变特性，并且可能在量测值中出现野值，不仅使状态估计失去了最优性，甚至可能导致滤波器的发散，进而出现较大的定位误差，不能满足室内移动机器人的任务需求。为解决上述问题，Fan等人使用简化的Sage-Husa自适应滤波器对量测噪声协方差进行自适应估计。Zhong等人将强跟踪滤波器应用到组合定位系统中，以应对无法预测的动态因素。然而，这些组合定位方法没有考虑到野值对定位系统性能带来的影响。为了提高组合定位系统的鲁棒性，文献“ZhenW,ZengS,SobererS.Robustlocalizationandlocalizabilityestimationwitharotatinglaserscanner[C].IEEEInternationalConferenceonRobotics&Automation.IEEE,2017:6240-6245.”中使用误差状态卡尔曼滤波器(ESKF)来融合异质传感器数据，对无法预测的误差因素进行了有效补偿，但是没有考虑时变噪声的情况。文献“HanH,XuT,WangJ.TightlycoupledintegrationofGPSambiguityfixedprecisepointpositioningandMEMS-INSthroughatroposphere-constrainedadaptiveKalmanfilter[J].Sensors,2016,16(7):1057.”中使用简化的Sage-Husa自适应滤波器(SHAF)对量测噪声协方差进行自适应估计，但是该方法没有考虑到野值对定位系统性能带来的影响。
技术实现思路
为了解决现有技术中的不足，本专利技术提供一种基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，具有高定位精度和高鲁棒性。为了实现上述目的，本专利技术采用的具体方案为：基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括如下步骤：S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；S3、对量测值中的野值进行处理，得到修正值；S4、使用自适应估计算法估计修正值的噪声协方差；S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。作为一种优选方案，S1的具体方法为：S1.1、设定状态向量并且赋初值，状态向量包括标称状态和误差状态，标称状态表示为其中为空间位置向量，为速度向量，为角度向量，为加速度偏置向量，为角速度偏置向量，误差状态δp为空间位置误差量向量，δv为速度误差量向量，δθ为角度误差量向量，δab为加速度偏置误差量向量，δωb为角速度偏置误差量向量；S1.2、基于状态向量构建惯导运动学模型，惯导运动学模型的状态转移方程为：其中为控制量，am,k为惯性导航系统中三轴加速度计的量测值，ωm,k为惯性导航系统中陀螺仪的量测值，k表示时刻，C为室内机器人机体坐标系到惯性导航系统坐标系的旋转矩阵，q为C的四元数形式，g为重力向量，I3为3×3的单位矩阵，Δt为采样时间，q{·}表示将角度变化量转换为四元数形式，表示四元数乘法，[·]×表示斜对称算子，F为误差状态转移矩阵；S1.3、构建超宽带量测模型的量测方程：en,k＝||pn-pt,k||2-dn,k,n＝1,2,...,N；其中，dn,k表示超宽带定位标签到第n个超宽带定位锚点之间的距离，pm,k为超宽带定位系统解算出的室内机器人位置，en,k为量测距离与真实距离之间的误差，pn＝[xn,yn,zn]T为超宽带定位锚点在惯性导航坐标系下位置，n∈{1,2,3,…,N}表示超宽带定位锚点编号，N为超宽带定位锚点总数，pt,k为超宽带定位标签的真实位置，vm,k为超宽带定位系统计算出的室内机器人速度；S1.4、初始化自适应卡尔曼滤波器：其中Pk|k-1为误差状态协方差的预测值，Γn为噪声驱动矩阵，Qn为过程噪声的协方差矩阵。作为一种优选方案，S2中，组合量测方程为：其中zk为量测值，pINS,k为惯性导航系统递推出的室内机器人位置，vINS,k为惯性导航系统递推出的室内机器人速度，为量测噪声向量，R为量测噪声协方差阵，为误差状态的量测矩阵。作为一种优选方案，S3的具体方法为：S3.1、估计新息协方差：εk＝zk-Hδxk|k-1；其中εk为新息，Sk为通过带衰减因子的滑窗估计法求得的新息协方差的估计值，l为滑动窗口大小，σi＝ak-i(1-a)/(1-al)为衰减系数，a表示衰减率，并且有0.95≤a≤0.99，εi为时刻i时的新息；S3.2、根据新息正交性理论计算量测值方差估计值和量测值方差理论值：S3.3、根据量测值方差估计值与量测值方差理论值之间的差异判断量测值是否为野值，如果本文档来自技高网...

【技术保护点】
1.基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括如下步骤：S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；S3、对量测值中的野值进行处理，得到修正值；S4、使用自适应估计算法估计修正值的噪声协方差；S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。

【技术特征摘要】
1.基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：使用惯性导航系统和超宽带定位系统实现，其中惯性导航系统设置在室内机器人上，超宽带定位系统包括设置在室内机器人上的超宽带定位标签和设置在室内的超宽带定位锚点，组合定位方法包括如下步骤：S1、构建室内机器人的惯导运动学模型和超宽带量测模型，并且初始化自适应卡尔曼滤波器；S2、基于惯导运动学模型和超宽带量测模型生成组合量测方程，并且利用组合量测方程对室内机器人的状态进行量测，得到量测值；S3、对量测值中的野值进行处理，得到修正值；S4、使用自适应估计算法估计修正值的噪声协方差；S5、将噪协方差代入到自适应卡尔曼滤波器中对量测值进行更新，得到优化值。2.如权利要求1的基于自适应卡尔曼滤波器的室内移动机器人组合定位方法，其特征在于：S1的具体方法为：S1.1、设定状态向量并且赋初值，状态向量包括标称状态和误差状态，标称状态表示为其中为空间位置向量，为速度向量，为角度向量，为加速度偏置向量，为角速度偏置向量，误差状态δp为空间位置误差量向量，δv为速度误差量向量，δθ为角度误差量向量，δab为加速度偏置误差量向量，δωb为角速度偏置误差量向量；S1.2、基于状态向量构建惯导运动学模型，惯导运动学模型的状态转移方程为：其中为控制量，am,k为惯性导航系统中三轴加速度计的量测值，ωm,k为惯性导航系统中陀螺仪的量测值，k表示时刻，C为室内机器人机体坐标系到惯性导航系统坐标系的旋转矩阵，q为C的四元数形式，g为重力向量，I3为3×3的单位矩阵，Δt为采样时间，q{·}表示将角度变化量转换为四元数形式，表示四元数乘法，[·]×表示斜对称算子，F为误差状态转移矩阵；S1.3、构建超宽带量测模型的量测方程：en,k＝||pn-pt,k||2-dn,k,n＝1,2,...,N；其中，dn,k表示超宽带定位标签到第n个超宽带定位锚点之间的距离，pm,k为超宽带定位系统解算出的室内机器人位置，en,k为量测距离与真实距离之间的误差，pn＝[xn,yn,zn]T为超宽带定位锚点在惯性导航坐标系下位置，n∈{1,2,3,…,N}表示超宽带定位锚点编号，N为超宽带定位锚点总数，pt,k为超宽带定位标签的真实位置，vm,k为超宽带定位系统计算出的室内机器人速度；S1.4、初始化自适应卡尔曼滤波器：其中Pk|k-1为误差状态协方差的预测值，Γn为噪声驱动矩阵，Qn为过程噪声的协方差...

【专利技术属性】
技术研发人员：孙力帆，刘剑锋，普杰信，常玉婷，俞皓芳，付主木，陶发展，
申请(专利权)人：河南科技大学，
类型：发明
国别省市：河南,41