基于自适应核宽卡尔曼滤波的机器人定位方法

本发明涉及机器人定位，具体而言，涉及基于自适应核宽卡尔曼滤波的机器人定位方法。

背景技术：

1、随着科技技术不断创新，机器人及相关领域得到了大力发展，其中，特种机器人逐渐代替人工而广泛应用于仓储，工业，医疗等领域。在复杂的工作环境中，特种机器人如何确定自身的位置信息，是高效完成特殊任务的首要前提。就目前而言，为克服惯性定位导航方法的缺点，现有技术通常采用的技术手段为：加装编码器等器件获得另一组位置信息，采用基于最小均方误差准则的卡尔曼滤波算法对惯性测量单元位置信息进行校正。然而由于特种机器人的工作环境较为复杂，其传感器会受到各种时变噪声的影响，容易导致算法性能会急速下降，致使特种机器人的定位精确度不高。基于此，我们亟需一种能够更准确地定位特种机器人的算法。

技术实现思路

1、本发明的目的在于提供基于自适应核宽卡尔曼滤波的机器人定位方法，其根据特种机器人配备的惯性测量单元和编码器，获取理论位置和观测位置，并使用自适合核宽的卡尔曼滤波算法实现位置信息的预估与校正，能够结合惯性定位单元和编码器数据信息，去除外界变化的过程噪声和观测噪声的影响，实现特种机器人更准确的定位。

2、本发明的实施例通过以下技术方案实现：

3、基于自适应核宽卡尔曼滤波的机器人定位方法，该方法的步骤包括：

4、基于特种机器人配备的惯性定位单元与编码器，构建特种机器人的运动学方程与观测方程，所述特种机器人的运动学方程表征为理论位置数据，所述特种机器人的观测方程表征为观测位置数据；

5、将理论位置数据及观测位置数据带入基于自适应核宽的最大相关熵卡尔曼滤波算法进行校正计算，完成特种机器人的精确定位，其中，自适应核宽通过kl散度优化求得。

6、可选的，所述特种机器人的运动学方程如下：

7、xk＝akxk-1+wk

8、

9、其中，k为第k个时刻，xk∈rp×1为k时刻的状态估计值，ak∈rp×p为状态转移矩阵，wk∈rp×1为过程噪声，wk为零均值噪声，e为期望算子，t为转置符号，qk为过程噪声的协方差矩阵。

10、可选的，所述特种机器人的观测方程如下：

11、yk＝ckxk+vk

12、

13、

14、其中，yk∈rq×1为k时刻的观测值，ck∈rq×p为观测矩阵，vk∈rq×1为观测噪声，vk为零均值噪声，rk为观测噪声的协方差矩阵。

15、可选的，所述自适应核宽的求解过程如下：

16、设定初始核宽σ、正数∈以及初始状态估计值和初始协方差矩阵p0|0；

17、通过第一计算公式求解下一时刻的预测值

18、通过第二计算公式求解下一时刻的预测误差协方差矩阵pk|k-1；

19、分别对pk|k-1与rk进行楚列斯基分解，求得bpk与brk；

20、更新预设的增广系统的参数，并判断k与设定的时间窗口n之间的大小关系，若k>n，则通过固定点迭代公式求解核宽，进入下一步骤，否则，取初始核宽σ，并通过固定点迭代估计状态值，进入下一步骤；

21、直至满足迭代条件后，迭代停止，并通过第三计算公式更新后验估计协方差矩阵，获取最优核宽。

22、可选的，所述第一计算公式具体为：

23、

24、其中，为k-1时刻状态的最佳估计值，为k时刻状态的预测值。

25、可选的，所述第二计算公式具体为：

26、pk|k-1＝akpk-1|k-1akt+qk-1

27、其中，pk|k-1为k时刻真实值与预测值的协方差矩阵，即先验估计协方差矩阵，pk-1|k-1为k-1时刻真实值与估计值的协方差矩阵，qk-1为k-1时刻过程噪声的协方差矩阵。

28、可选的，所述分别对pk|k-1与rk进行楚列斯基分解，其计算公式如下：

29、bpk＝chol(pk|k-1)t

30、brk＝chol(rk)t

31、其中，bpk为变量，brk为变量。

32、可选的，所述更新预设的增广系统的参数，其具体为：

33、dk＝wkxk+ek

34、

35、

36、

37、

38、其中，dk、wk、bk均为增广系统变量，ek为增广系统的误差变量。

39、可选的，所述固定点迭代公式具体为：

40、

41、

42、其中，σk为k时刻的高斯核函数核宽，n为取样时间窗口大小，i为第i时刻，gk为中间变量，j为第j时刻，ei为第i时刻增广系统的误差大小，ej为第j时刻增广系统的误差大小；

43、其迭代条件具体为：

44、||σk+1-σk||/||σk||≤∈σ

45、其中，∈σ为设定的正数；

46、所述固定点迭代估计状态值具体为：

47、

48、

49、其中，gσ为高斯核函数，t为迭代次数，分别为第t次迭代后k时刻状态预测值和估计值，kk为卡尔曼滤波增益，为k时刻真实值与预测值的广义协方差矩阵，为k时刻观测噪声的广义协方差矩阵，ek(i)为k时刻增广误差的第i个值，diag[]为提取对角元素；

50、其迭代条件具体为：

51、

52、可选的，所述第三计算公式具体为：

53、

54、其中，pk|k为k时刻的后验估计协方差矩阵，i为单位矩阵。

55、本发明实施例的技术方案至少具有如下优点和有益效果：

56、本发明实施例根据特种机器人配备的惯性测量单元和编码器，获取理论位置和观测位置，并使用自适合核宽的卡尔曼滤波算法实现位置信息的预估与校正，能够结合惯性定位单元和编码器数据信息，去除外界变化的过程噪声和观测噪声的影响，实现特种机器人更准确的定位。

技术特征：

1.基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，该方法的步骤包括：

2.根据权利要求1所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述特种机器人的运动学方程如下：

3.根据权利要求2所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述特种机器人的观测方程如下：

4.根据权利要求1-3任一项所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述自适应核宽的求解过程如下：

5.根据权利要求4所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述第一计算公式具体为：

6.根据权利要求5所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述第二计算公式具体为：

7.根据权利要求6所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述分别对pk|k-1与rk进行楚列斯基分解，其计算公式如下：

8.根据权利要求7所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述更新预设的增广系统的参数，其具体为：

9.根据权利要求8所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述固定点迭代公式具体为：

10.根据权利要求9所述的基于自适应核宽卡尔曼滤波的机器人定位方法，其特征在于，所述第三计算公式具体为：

技术总结
本发明涉及特种机器人定位技术领域，具体而言，涉及基于自适应核宽卡尔曼滤波的机器人定位方法，该方法的步骤包括：基于特种机器人配备的惯性定位单元与编码器，构建特种机器人的运动学方程与观测方程，所述特种机器人的运动学方程表征为理论位置数据，所述特种机器人的观测方程表征为观测位置数据；将理论位置数据及观测位置数据带入基于自适应核宽的最大相关熵卡尔曼滤波算法进行校正计算，完成特种机器人的精确定位，其中，自适应核宽通过KL散度优化求得。本发明能够结合惯性定位单元和编码器数据信息，去除外界变化的过程噪声和观测噪声的影响，实现特种机器人更准确的定位。

技术研发人员：高松,马浩然,彭倍,刘凯,陈润乾,何嘉诚,王刚
受保护的技术使用者：电子科技大学
技术研发日：
技术公布日：2024/1/15