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

权利要求书

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

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

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

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

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

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

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的缩放系数。