一种基于椭球等高分布的机器人关节状态估计方法

本发明属于机器人领域，尤其涉及一种基于椭球等高分布的机器人关节状态估计方法。

背景技术：

1、在机器人控制领域中，特别是多关节机器人，无论是基于模型的控制方法或是基于学习的控制方法均需要依赖于机器人当前状态信息，如关节位置、速度和加速度，而关节端一般仅配有测量位置的角度编码器，无法准确获取速度和加速度信息。

2、在状态估计领域，卡尔曼滤波器是方差最小意义下的最优滤波器，通过预测与测量更新实现状态向量的估计，但最优性的实现基于噪声的高斯假设。相比于运动较为平稳的传统机械臂，足式机器人需要通过肢体的快速运动实现外界扰动后的平衡维持，此时肢体运动表现出高机动性，过程噪声变化剧烈，较大的噪声值出现的概率增加，破坏了过程噪声的高斯假设，表现为具有重尾特性的非高斯噪声。在传统卡尔曼滤波框架下，如果采用固定的过程噪声协方差矩阵，较小的过程噪声协方差矩阵无法快速跟踪状态的相应变化，使估计精度降低，而增大过程噪声协方差矩阵虽然能够想实现一定的改善，但可能会造成滤波的发散，使得估计失效。

3、基于变分推断的滤波方法能够通过选取合理的分布形式实现重尾噪声下较为准确的状态估计，但需要相关参数的共轭先验，且求解时间较长，无法满足足式机器人高控制频率的实时性要求。目前还未见到针对该问题的相关研究，因此有必要研究针对重尾过程噪声的快速鲁棒滤波方法，以提升重尾过程噪声情况下机器人关节状态的估计精度。

4、综上所述，现有的机器人关节状态估计方法无法对具有重尾过程噪声的高机动关节实现快速鲁棒状态估计，存在滤波精度差导致对机器人关节状态估计准确率低的问题。

技术实现思路

1、本发明目的是解决现有机器人关节状态估计方法中针对高机动机器人关节状态估计准确率低的问题。我们提出了一种基于椭球等高分布的机器人关节状态估计方法。

2、s1、构建交互式imm滤波器；所述交互式imm滤波器包括m1个滤波器；

3、s2、采用传感器采集当前时间步k时的机器人关节位置数据，并根据时间步k时的机器人关节位置数据得到当前时间步k时的观测向量zk；

4、s3、获取上一时间步k-1时imm滤波器中m1个滤波器的模型概率、后验概率密度函数、后验状态均值以及后验协方差矩阵；所述后验状态均值包括：关节位置、速度、加速度；

5、s4、根据s3得到的上一时间步k-1时imm滤波器中m1个滤波器的模型概率、后验概率密度函数、后验状态均值以及后验协方差矩阵计算得到当前时间步k时m1个滤波器的初始概率密度函数；并根据m1个滤波器的初始概率密度函数得到m1个滤波器的初始状态均值以及初始协方差矩阵；

6、s5、将s2获得的当前时间步k的观测向量zk以及s4得到的当前时间步k时m1个滤波器的初始状态均值以及初始协方差矩阵输入交互式imm滤波器，并行运行m1个滤波器，得到当前时间步k时的m1个滤波器的测量似然概率密度函数、后验状态均值以及后验协方差矩阵；

7、s6、根据s5得到的m1个滤波器的测量似然概率密度函数更新当前时间步k时的模型概率；

8、s7、根据s6更新得到的m1个滤波器在时间步k时的模型概率，以及s5获得的当前时间步k时的m1个滤波器的后验状态均值以及后验协方差矩阵计算得到当前时间步k时的融合估计结果。

9、本发明的有益效果为：

10、本发明提出的基于椭球等高分布的机器人关节状态估计方法，在重尾过程噪声情况下，对比现有估计方法具有估计准确，求解快速，鲁棒性高等特点，满足机器人领域对实时性和准确性的需求。

11、2、本发明提出的基于椭球等高分布的机器人关节状态估计方法，在变分贝叶斯求解方法下，无需提供椭球等高分布尺度参数的先验分布，通过合理的近似求解尺度参数的期望。

12、3、本发明提出的基于椭球等高分布的机器人关节状态估计方法，基于位置测量值实现对速度和加速度的快速准确估计，也可以扩展到基于位置和速度测量值的应用中，具有良好的扩展性。

13、4、本发明提出的基于椭球等高分布的机器人关节状态估计方法中包含的各项公式均已解析的给出，适用于计算机编程，具有良好的应用性。

技术特征：

1.一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，所述s1中构建交互式imm滤波器中第m1∈m1个滤波器的具体过程为：

3.根据权利要求2所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，所述s1.1.1中机器人关节系统动态方程与测量方程用公式表示为：

4.根据权利要求3所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，所述s1.1.3.1中先验概率密度函数p(xk|z1:k-1)服从的椭球等高分布形式，包括：高斯椭球等高分布、对数椭球等高分布和指数椭球等高分布；

5.根据权利要求4所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，所述s1.1.4中构建似然概率密度函数p(zk|xk)的具体过程为：

6.根据权利要求5所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，

7.根据权利要求6所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，

8.根据权利要求7所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，

9.根据权利要求8所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，

10.根据权利要求9所述的一种基于椭球等高分布的机器人关节状态估计方法，其特征在于，

技术总结
本发明涉及机器人领域，尤其涉及一种基于椭球等高分布的机器人关节状态估计方法，本发明提出的基于椭球等高分布的机器人关节状态估计方法，在重尾过程噪声情况下，对比现有估计方法具有估计准确，求解快速，鲁棒性高等特点，满足机器人领域对实时性和准确性的需求。本发明提出的基于椭球等高分布的机器人关节状态估计方法，在变分贝叶斯求解方法下，无需提供椭球等高分布尺度参数的先验分布，通过合理的近似求解尺度参数的期望。本发明提出的基于椭球等高分布的机器人关节状态估计方法，基于位置测量值实现对速度和加速度的快速准确估计，也可以扩展为基于位置和速度测量值的应用中，具有良好的扩展性。

技术研发人员：齐彬,付宜利,任秉银,张松源
受保护的技术使用者：哈尔滨工业大学
技术研发日：
技术公布日：2024/9/23