1.一种基于变分推断的机器人状态估计方法,其特征在于,包括:
构建机器人运动过程的非线性系统动态模型;
将机器人状态参数作为粒子滤波算法中的粒子,所述机器人状态参数包括但不限于机器人基站、关节臂、臂角;设定每个粒子初始时刻的粒子状态和粒子权重,所述粒子状态包括但不限于粒子的速度、角度、加速度;设定所述非线性系统动态模型的参数初始值为设定初始控制输入u0;设定Inverse‑Wishart分布中的参数值包括设定调节参数τ,对角线元素数值为τ、维数为dy×dy的对角矩阵B,设定自由度α和维度为dy×dy的尺度矩阵ψ的初始值;
将粒子n‑1时刻的粒子状态输入所述非线性系统动态模型,预测n时刻粒子的粒子状态;根据n‑1时刻的自由度αn‑1计算n时刻Inverse‑Wishart分布下的自由度预测值 根据n‑1时刻的尺度矩阵ψn‑1计算n时刻Inverse‑Wishart分布下的尺度矩阵预测值 根据n时刻的自由度和尺度矩阵预测值确定n时刻的自由度更新值αn和尺度矩阵更新值ψn;
对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新,每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制,完成预设次数的迭代更新后,输出每个粒子的粒子权重迭代值和尺度矩阵迭代值;
根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理;
根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值,根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
2.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,所述机器人运动过程的非线性系统动态模型为:xn=fn(xn‑1,un‑1)+Wn
yn=gn(xn)+Vn
其中,n表示时间索引,xn表示n时刻的系统状态,xn‑1表示n‑1时刻的系统状态,un‑1表示n‑1时刻的系统控制输入,yn表示n时刻的测量向量,维数为dy,fn为已知的非线性状态转移函数,gn为已知的非线性输出映射函数,Wn~N(0,Qn)为服从均值为0,协方差矩阵为Qn的过程噪声,Vn~N(0,Rn)为服从均值为0,协方差矩阵为Rn的测量噪声。
3.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,所述将粒子n‑1时刻的粒子状态输入所述非线性系统动态模型,预测n时刻粒子的粒子状态;根据n‑1时刻的自由度αn‑1计算n时刻Inverse‑Wishart分布下的自由度预测值 根据n‑1时刻的尺度矩阵ψn‑1计算n时刻Inverse‑Wishart分布下的尺度矩阵预测值 根据n时刻的自由度和尺度矩阵预测值确定n时刻的自由度更新值αn和尺度矩阵更新值ψn包括:将粒子n‑1时刻的粒子状态输入所述非线性系统动态模型,预测n时刻粒子的粒子状态:其中, 表示n时刻第i个粒子的粒子状态预测值,fn为已知的非线性状态转移函数,表示n‑1时刻第i个粒子的粒子状态,un‑1表示n‑1时刻的系统控制输入, 表示n‑1时刻第i个粒子的过程噪声;
根据n‑1时刻的自由度αn‑1计算n时刻Inverse‑Wishart分布下的自由度预测值其中,τ为调节参数,dy为测量向量yn的维数;
根据n‑1时刻的尺度矩阵ψn‑1计算n时刻Inverse‑Wishart分布下的尺度矩阵预测值其中,B为对角线元素数值为τ、维数为dy×dy的对角矩阵;
根据n时刻的自由度和尺度矩阵预测值确定n时刻的自由度更新值αn和尺度矩阵更新值ψn:
4.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,所述对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新,每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制,完成预设次数的迭代更新后,输出每个粒子的粒子权重迭代值和尺度矩阵迭代值包括:对n时刻的尺度矩阵更新值进行迭代更新,其迭代更新公式为:
其中, 表示n时刻第l次迭代时的尺度矩阵,ψn为n时刻尺度矩阵更新值, 表示n时刻第l次迭代时第i个粒子的权重,yn表示n时刻的测量向量,gn为已知的非线性输出映射函数, 表示n时刻第i个粒子的粒子状态预测值,N表示粒子个数;
对n时刻的粒子权重进行迭代更新,其迭代更新公式为:
其中, 表示第l次迭代时第i个粒子的粒子权重, 为n‑1时刻第i个粒子的粒子权重,exp(·)表示指数运算, 的计算公式为:其中,αn为n时刻自由度更新值,dy为测量向量yn的维数, 为n时刻第l‑1次迭代时的尺度矩阵;
每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制,完成预设次数的迭代更新后,输出每个粒子的粒子权重迭代值 和尺度矩阵迭代值ψ'n:其中,L表示预设的迭代次数。
5.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,所述根据粒子权重迭代值对粒子进行重采样的公式为:其中, 表示n时刻第i个粒子的状态更新值, 为粒子权重迭代值,xn表示n时刻的系统状态, 表示n时刻第i个粒子的粒子状态预测值,N表示粒子个数,δ(·)为狄拉克函数;
所述对粒子权重迭代值进行归一化处理的公式为:
其中, 为n时刻第i个粒子的粒子权重,N为粒子滤波算法的粒子个数。
6.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,根据重采样后每个粒子的粒子状态 和归一化处理后的粒子权重 计算n时刻机器人状态的估计值 其中,N表示粒子个数;
根据自由度更新值αn和尺度矩阵迭代值ψ'n计算n时刻测量噪声协方差矩阵的估计值其中dy为测量向量yn的维数。
7.根据权利要求1所述的基于变分推断的机器人状态估计方法,其特征在于,所述每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制时,淘汰的粒子数和复制的粒子数相同。
8.一种基于变分推断的机器人状态估计系统,其特征在于,包括:
模型构建模块:用于构建机器人运动过程的非线性系统动态模型;
数据初始模块:用于将机器人状态参数作为粒子滤波算法中的粒子,所述机器人状态参数包括但不限于机器人基站、关节臂、臂角;设定每个粒子初始时刻的粒子状态和粒子权重,所述粒子状态包括但不限于粒子的速度、角度、加速度;设定所述非线性系统动态模型的参数初始值为设定初始控制输入u0;设定Inverse‑Wishart分布中的参数值包括设定调节参数τ,对角线元素数值为τ、维数为dy×dy的对角矩阵B,设定自由度α和维度为dy×dy的尺度矩阵ψ的初始值;
数据预测模块:用于将粒子n‑1时刻的粒子状态输入所述非线性系统动态模型,预测n时刻粒子的粒子状态;根据n‑1时刻的自由度αn‑1计算n时刻Inverse‑Wishart分布下的自由度预测值 根据n‑1时刻的尺度矩阵ψn‑1计算n时刻Inverse‑Wishart分布下的尺度矩阵预测值 根据n时刻的自由度和尺度矩阵预测值确定n时刻的自由度更新值αn和尺度矩阵更新值ψn;
迭代更新模块:用于对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新,每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制,完成预设次数的迭代更新后,输出每个粒子的粒子权重迭代值和尺度矩阵迭代值;
重采样及归一化模块:用于根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理;
数据测算模块:用于根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值,根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
9.一种基于变分推断的机器人状态估计设备,其特征在于,包括:
存储器:用于存储计算机程序;
处理器:用于执行所述计算机程序时实现权利要求1‑7任一项所述的基于变分推断的机器人状态估计方法的步骤。