(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210733585.2 (22)申请日 2022.06.27 (71)申请人 江南大学 地址 214122 江苏省无锡市滨湖区蠡湖大 道1800号 (72)发明人 赵顺毅　慈航　栾小丽　刘飞　 (74)专利代理 机构 苏州市中南伟业知识产权代 理事务所(普通 合伙) 32257 专利代理师 李柏柏 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于变分推断的机器人状态估计方法 及系统 (57)摘要 本发明涉及一种基于变分推断的机器人状 态估计方法及系统， 包括： 构建机器人状态估计 的非线性系统动态模型； 将机器人状态估计的参 数作为粒子滤波算法中的粒子， 设定每个粒子的 状态和粒子权重以及相关参数的初始值； 对当前 时刻的粒子状态以及Inverse ‑Wishart分布参数 进行预测， 再对当前时刻粒子权重和Inverse ‑ Wishart分布的参数值进行预设次数的迭代更 新， 输出最后一次迭代的粒子权重迭代值以及 Inverse‑Wishart分布的参数迭代值； 根据粒子 权重迭代值对粒子进行重采样并对粒子权重进 行归一化处理； 最后获得机器人状态的估计值以 及测量噪声协方差矩阵的估计值。 本发明使用基 于分贝叶斯理论的粒子滤波算法实现了在非线 性系统中对机器人状态和测量噪声协方差的定 量描述， 提高了估计精度。 权利要求书4页 说明书9页 附图5页 CN 115070765 A 2022.09.20 CN 115070765 A 1.一种基于变分推断的机器人状态估计方法， 其特 征在于， 包括： 构建机器人运动过程的非线性系统动态模型； 将机器人状态参数作为粒子滤波算法中的粒子， 设定每个粒子初始时刻的粒子状态和 粒子权重， 设定所述非线性系统动态模型的参数初始值和Inverse ‑Wishart分布中的参数 值； 利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态， 计算当前时刻 Inverse‑Wishart分布下的自由度和尺度矩阵预测值， 并根据当前时刻的自由度和尺度矩 阵预测值确定当前时刻自由度和尺度矩阵的更新 值； 对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新， 每次迭 代更新后根据粒子权重按照预设比例对粒子进行淘汰复制， 完成预设次数 的迭代更新后， 输出每个粒子的粒子 权重迭代值和尺度矩阵迭代值； 根据粒子 权重迭代值对粒子进行重采样并对粒子 权重迭代值进行归一 化处理； 根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估 计值， 根据自由度更新 值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。 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所述的基于变分推断的机器人状态估计方法， 其特征在于， 所述机器 人状态参数包括但不限于 机器人基站、 关节 臂、 臂角； 所述粒子状态包括但不限于粒子的速度、 角度、 加速度； 所述设定非线性系统动态模型的参数初始值 为设定初始控制输入u0； 所述设定Inverse ‑Wishart分布中的参数值包括设定调节参数τ， 对角线元素数值为τ、 维数为dy×dy的对角矩阵B， 设定自由度α 和维度为dy×dy的尺度矩阵ψ 的初始值。 4.根据权利要求1所述的基于变分推断的机器人状态估计方法， 其特征在于， 所述利用 所述非线性系统动态模型预测当前时刻每个粒子的粒子状态， 计算当前时刻Inverse ‑ Wishart分布下的自由度和尺度矩阵预测值， 并根据当前时刻的自由度和尺度矩阵预测值 确定当前时刻自由度和尺度矩阵的更新 值包括： 将粒子n‑1时刻的粒子状态输入所述非线性系统动态模型， 预测n时刻粒子的粒子状 态： 其中， 表示n时刻第i个粒子的粒子状态预测值， fn为已知的非线性状态转移函数， 表示n‑1时刻第i个粒子的粒子状态， un‑1表示n‑1时刻的系统控制输入， 表示n‑1时 刻第i个粒子的过程噪声；权　利　要　求　书 1/4 页 2 CN 115070765 A 2根据n‑1时刻的自由度αn‑1计算n时刻Inverse‑Wishart分布下的自由度预测值 其中， τ 为调节参数， dy为测量向量yn的维数； 根据n‑1时刻的尺度矩阵ψn‑1计算n时刻Inverse ‑Wishart分布下的尺度矩阵预测值 其中， B为对角线元 素数值为 τ、 维数为dy×dy的对角矩阵； 根据n时刻的自由度和尺度矩阵预测值确定n时刻的自 由度更新值αn和尺度矩阵更新值 ψn： 5.根据权利要求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表示预设的迭代次数。权　利　要　求　书 2/4 页 3 CN 115070765 A 3