CN115070765A - 一种基于变分推断的机器人状态估计方法及系统 - Google Patents
一种基于变分推断的机器人状态估计方法及系统 Download PDFInfo
- Publication number
- CN115070765A CN115070765A CN202210733585.2A CN202210733585A CN115070765A CN 115070765 A CN115070765 A CN 115070765A CN 202210733585 A CN202210733585 A CN 202210733585A CN 115070765 A CN115070765 A CN 115070765A
- Authority
- CN
- China
- Prior art keywords
- particle
- value
- state
- time
- scale matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 74
- 239000002245 particle Substances 0.000 claims abstract description 260
- 239000011159 matrix material Substances 0.000 claims abstract description 98
- 238000009826 distribution Methods 0.000 claims abstract description 27
- 238000005259 measurement Methods 0.000 claims abstract description 25
- 238000012952 Resampling Methods 0.000 claims abstract description 23
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 13
- 238000012545 processing Methods 0.000 claims description 12
- 238000010606 normalization Methods 0.000 claims description 8
- 230000008030 elimination Effects 0.000 claims description 7
- 238000003379 elimination reaction Methods 0.000 claims description 7
- 238000012546 transfer Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000001105 regulatory effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 9
- 238000012544 monitoring process Methods 0.000 description 4
- JWZVDINGVWOBKH-OUAUKWLOSA-N (3R,4R)-3-[(1S)-1-hydroxyheptyl]-4-(hydroxymethyl)oxolan-2-one Chemical compound CCCCCC[C@H](O)[C@H]1[C@H](CO)COC1=O JWZVDINGVWOBKH-OUAUKWLOSA-N 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013398 bayesian method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000000528 statistical test Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Feedback Control In General (AREA)
Abstract
本发明涉及一种基于变分推断的机器人状态估计方法及系统，包括：构建机器人状态估计的非线性系统动态模型；将机器人状态估计的参数作为粒子滤波算法中的粒子，设定每个粒子的状态和粒子权重以及相关参数的初始值；对当前时刻的粒子状态以及Inverse‑Wishart分布参数进行预测，再对当前时刻粒子权重和Inverse‑Wishart分布的参数值进行预设次数的迭代更新，输出最后一次迭代的粒子权重迭代值以及Inverse‑Wishart分布的参数迭代值；根据粒子权重迭代值对粒子进行重采样并对粒子权重进行归一化处理；最后获得机器人状态的估计值以及测量噪声协方差矩阵的估计值。本发明使用基于分贝叶斯理论的粒子滤波算法实现了在非线性系统中对机器人状态和测量噪声协方差的定量描述，提高了估计精度。
Description
技术领域
本发明涉及机器人监测技术领域，尤其是指一种基于变分推断的机器人状态估计方法及系统。
背景技术
对机器人位置和状态信息定位的过程属于机器人定位与监测技术领域，此过程需要相关传感器持续提供相关信息，在状态估计领域，卡尔曼滤波方法利用观测值和预测值之差所得到的残差信息为决策提供残差分析或统计测试，但卡尔曼滤波仅适用于对线性系统进行估计，为了解决非线性系统下的状态估计问题，学者们提出了从实时测量中学习噪声协方差的方法，该方法可在一定程度上降低异常信息丢失的概率，但是这些方法仅用于估计目的，不能用于传感器监测，目前，没有针对非线性系统的状态估计方法可以实现对异常或故障信息进行定量描述。
综上所述，现有的机器人状态估计方法中无法对机器人状态及测量噪声协方差进行定量描述且对机器人状态估计的估计精度不高。
发明内容
为此，本发明所要解决的技术问题在于克服现有的机器人状态估计方法中无法对机器人状态及测量噪声协方差进行定量描述且估计精度不高的问题。
为解决上述技术问题，本发明提供了一种基于变分推断的机器人状态估计方法，包括：
构建机器人运动过程的非线性系统动态模型；
将机器人状态参数作为粒子滤波算法中的粒子，设定每个粒子初始时刻的粒子状态和粒子权重，设定所述非线性系统动态模型的参数初始值和Inverse-Wishart分布中的参数值；
利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态，计算当前时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据当前时刻的自由度和尺度矩阵预测值确定当前时刻自由度和尺度矩阵的更新值；
对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值；
根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理；
根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值，根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
在本发明的一个实施例中，所述机器人运动过程的非线性系统动态模型为：
xn＝fn(xn-1,un-1)+Wn
yn＝gn(xn)+Vn
其中，n表示时间索引，xn表示n时刻的系统状态，xn-1表示n-1时刻的系统状态，un-1表示n-1时刻的系统控制输入，yn表示n时刻的测量向量，fn为已知的非线性状态转移函数，gn为已知的非线性输出映射函数，Wn～N(0,Qn)为服从均值为0，协方差矩阵为Qn的过程噪声，Vn～N(0，Rn)为服从均值为0，协方差矩阵为Rn的测量噪声。
在本发明的一个实施例中，所述机器人状态参数包括但不限于机器人基站、关节臂、臂角；
所述粒子状态包括但不限于粒子的速度、角度、加速度；
所述设定非线性系统动态模型的参数初始值为设定初始控制输入u0；
所述设定Inverse-Wishart分布中的参数值包括设定调节参数τ，对角线元素数值为τ、维数为dy×dy的对角矩阵B，设定自由度α和维度为dy×dy的尺度矩阵ψ的初始值。
在本发明的一个实施例中，所述利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态，计算当前时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据当前时刻的自由度和尺度矩阵预测值确定当前时刻自由度和尺度矩阵的更新值包括：
将粒子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：
在本发明的一个实施例中，所述对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值包括：
对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表示预设的迭代次数。
在本发明的一个实施例中，所述根据粒子权重迭代值对粒子进行重采样的公式为：
其中，
表示n时刻第i个粒子的状态更新值，
为粒子权重迭代值，xn表示n时刻的系统状态，
表示n时刻第i个粒子的粒子状态预测值，N表示粒子个数，δ(·)为狄拉克函数；
所述对粒子权重迭代值进行归一化处理的公式为：
其中，
为n时刻第i个粒子的粒子权重，N为粒子滤波算法的粒子个数。
在本发明的一个实施例中，根据重采样后每个粒子的粒子状态
和归一化处理后的粒子权重
计算n时刻机器人状态的估计值
其中，N表示粒子个数；
根据自由度更新值αn和尺度矩阵迭代值ψ'n计算n时刻测量噪声协方差矩阵的估计值
其中dy为测量向量yn的维数。
在本发明的一个实施例中，所述每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制时，淘汰的粒子数和复制的粒子数相同。
本发明还提供了一种基于变分推断的机器人状态估计系统，包括：
模型构建模块：用于构建机器人运动过程的非线性系统动态模型；
数据初始模块：用于将机器人状态参数作为粒子滤波算法中的粒子，设定每个粒子初始时刻的粒子状态和粒子权重，设定所述非线性系统动态模型的参数初始值和Inverse-Wishart分布中的参数值；
数据预测模块：用于利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态，计算当前时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据当前时刻的自由度和尺度矩阵预测值确定当前时刻自由度和尺度矩阵的更新值；
迭代更新模块：用于对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值；
重采样及归一化模块：用于根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理；
数据测算模块：用于根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值，根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
本发明还提供了一种基于变分推断的机器人状态估计设备，包括：
存储器：用于存储计算机程序；
处理器：用于执行所述计算机程序时实现上述基于变分推断的机器人状态估计方法的步骤。
本发明所述的基于变分推断的机器人状态估计方法，包括构建机器人运动过程的非线性系统动态模型，采用粒子滤波算法对机器人各粒子的粒子状态以及Inverse-Wishart分布的参数值进行预测更新，并对每个粒子的粒子权重和Inverse-Wishart分布的参数值进行预定次数的迭代更新，迭代后淘汰可信度低的粒子，对可信度高的粒子进行重采样，根据重采样后粒子的粒子状态及粒子权重得到机器人状态的估计值，根据迭代更新后的Inverse-Wishart分布参数值得到测量噪声协方差矩阵的估计值。本发明所述的基于变分推断的机器人状态估计方法在进行机器人状态估计过程中采用基于变分贝叶斯理论的粒子滤波算法实现了在非线性系统中对机器人状态和测量噪声协方差的定量描述，提高了机器人状态的估计精度。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
图1为基于变分推断的机器人状态估计方法流程图；
图2为旋转柔性关节各状态变量的真实值和测量值随时间的变化图；
图3为旋转柔性关节底角和臂角速度的状态估计随时间的变化图；
图4为旋转柔性关节定位过程状态估计的测量噪声协方差的均方根误差随时间变化的箱型图；
图5为旋转柔性关节定位过程状态估计的测量噪声协方差的均方根误差随粒子逼近数N的变化图；
图6为旋转柔性关节使用VB-PF方法和VB-D方法的跟踪精度随时间的变化图；
图7为基于变分推断的机器人状态估计系统图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
实施例1：
参照图1所示，为本发明的基于变分推断的机器人状态估计方法的流程图，包括：
S1：构建机器人运动过程的非线性系统动态模型：
xn＝fn(xn-1,un-1)+Wn
yn＝gn(xn)+Vn
其中，n表示时间索引，xn表示n时刻的系统状态，xn-1表示n-1时刻的系统状态，un-1表示n-1时刻的系统控制输入，yn表示n时刻的测量向量，fn为已知的非线性状态转移函数，gn为已知的非线性输出映射函数，Wn～N(0,Qn)为服从均值为0，协方差矩阵为Qn的过程噪声，Vn～N(0，Rn)为服从均值为0，协方差矩阵为Rn的测量噪声。
S2：将机器人状态参数作为粒子滤波算法的粒子，设定每个粒子初始时刻的粒子状态和粒子权重，表示为
其中，
表示0时刻第i个粒子的状态，
表示0时刻第i个粒子的权重，N表示粒子个数；设定机器人运动过程的非线性系统动态模型初始控制输入u0和Inverse-Wishart分布中的调节参数τ，对角线元素数值为τ、维数为dy×dy的对角矩阵B，设定自由度α和维度为dy×dy的尺度矩阵ψ的初始值；
其中，所述机器人状态参数为机器人基站、柔性关节臂、底角、臂角等；所述粒子的状态不限于各粒子的速度、角度、加速度。
S3：利用机器人状态估计的非线性系统动态模型预测n时刻每个粒子的粒子状态，计算n时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据自由度和尺度矩阵的预测值确定n时刻的自由度和尺度矩阵更新值，具体包括以下步骤：
S30：将N个粒子n-1时刻的粒子状态输入非线性系统动态模型预测n时刻的粒子状态：
其中，
表示n时刻第i个粒子的粒子状态预测值，
表示n-1时刻第i个粒子的粒子状态，
表示n-1时刻第i个粒子的过程噪声；
S31：根据n-1时刻的自由度αn-1预测n时刻自由度的值，公式为：
S32：根据n-1时刻的尺度矩阵ψn-1预测n时刻尺度矩阵的值，公式为：
S33：根据自由度和尺度矩阵的预测值确定n时刻自由度和尺度矩阵的更新值：
S4：对n时刻的尺度矩阵更新值和n时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值，具体包括以下步骤：
S40：对n时刻尺度矩阵更新值进行迭代更新，其公式为：
其中，
表示n时刻第l次迭代时的尺度矩阵，
表示n时刻第l次迭代时第i个粒子的权重；
S41：对n时刻的粒子权重进行迭代更新，其迭代更新公式为：
其中，exp(·)表示指数运算，
的计算公式为：
S42：每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，其中每次淘汰的粒子个数和复制的粒子个数相同，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值：
其中，L表示预设的迭代次数。
S5：根据每个粒子的粒子权重迭代值对粒子进行重采样，重采样公式为：
其中，
表示n时刻第i个粒子的状态更新值，δ(·)为狄拉克函数；
对粒子权重迭代值进行归一化处理的公式为：
S6：根据重采样后每个粒子的粒子状态
和归一化处理后的粒子权重
计算n时刻机器人状态的估计值
根据自由度更新值αn和尺度矩阵迭代值ψ'n计算n时刻测量噪声协方差矩阵的估计值
实施例2：
基于实施例1所述的基于变分推断的机器人状态估计方法，本实施例采用机器人旋转柔性关节所收集的测量数据进行仿真验证：
引入四维状态向量
其中θn和εn表示在n时刻基站的角度和柔性关节臂的角度，
和
表示在n时刻对应的底角速度和臂角速度。
步骤1：建立旋转柔性关节机器人运动状态估计的非线性系统动态模型：
xn＝fn(xn-1,un-1)+Wn
yn＝gn(xn)+Vn
其中，旋转柔性关节的非线性状态转移函数fn＝Anxn+Hnun+0.8sin(2θn)，非线性测量转移函数gn＝Cnxn，系统的过程噪声协方差矩阵为Q＝10-4diag(1111)，实际测量噪声Vn～N(0,Rn)如图2所示，
其中，
步骤2：设定初始值：
设置0时刻的系统状态向量初始值x0＝[0 0 0 0]T，协方差矩阵初始值P0＝[0 0 00]T，Inverse-Wishart分布下的尺度矩阵初始值
其中ψ0，1＝[2 0]，ψ0，2＝[0 1]；自由度初始值α0＝10，参数τ＝0.99，矩阵B＝τIdy×dy，粒子数N＝100，迭代次数L＝3。
将初始值输入旋转柔性关节机器人运动状态估计的非线性系统动态模型中基于实施例1所述的方法步骤进行仿真，仿真基于10次蒙特卡洛运行，且时间为20s；
旋转柔性关节底角和臂角速度的状态估计随时间的变化如图3所示，系统的状态估计均方误差随时间变化的箱型图以及算法随粒子逼近数N的变化如图4和图5所示。
除此之外，本实施例还将本发明所提出的方法(记为VB-PF)与现有的处理非线性系统状态估计问题的变分贝叶斯方法(记为VB-D)进行了比较，其中旋转柔性关节系统使用VB-PF方法和使用VB-D方法的跟踪精度随时间的变化图如图6所示，从图中可以看出，使用本方法得到的估计值与状态估计同时监测非线性状态空间中的传感器噪声协方差表现出比现有解决方案更好的性能，对系统状态进行估计时可以保持较高的估计精度，提升了算法在实时传感器中的监测效率。
本发明的具体实施例还提供了一种用于实现前述的基于变分推断的机器人状态估计方法的系统，如图7所示，包括：
模型构建模块10：用于构建机器人运动过程的非线性系统动态模型；
数据初始模块20：用于将机器人状态参数作为粒子滤波算法中的粒子，设定每个粒子初始时刻的粒子状态和粒子权重，设定所述非线性系统动态模型的参数初始值和Inverse-Wishart分布中的参数值；
数据预测模块30：用于利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态，计算当前时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据当前时刻的自由度和尺度矩阵预测值确定当前时刻自由度和尺度矩阵的更新值；
迭代更新模块40：用于对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值；
重采样及归一化模块50：用于根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理；
数据测算模块60：用于根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值，根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
本实施例的基于基于变分推断的机器人状态估计系统用于实现前述基于变分推断的机器人状态估计方法，因此基于变分推断的机器人状态估计系统的具体实施方式可见前文中的基于变分推断的机器人状态估计方法的实施例部分，例如，模型构建模块10用于实现上述基于变分推断的机器人状态估计方法中步骤S1；数据初始模块20用于实现上述基于变分推断的机器人状态估计方法中步骤S2；数据预测模块30用于实现上述基于变分推断的机器人状态估计方法中步骤S3；迭代更新模块40用于实现上述基于变分推断的机器人状态估计方法中步骤S4；重采样及归一化模块50用于实现上述基于变分推断的机器人状态估计方法中步骤S5；数据测算模块60用于实现上述基于变分推断的机器人状态估计方法中步骤S6，所以其具体实施方式可以参照相应的实施例的描述，在此不再赘述。
本发明的具体实施例还提供了一种基于变分推断的机器人状态估计设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于变分推断的机器人状态估计方法的步骤。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。
Claims (10)
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个粒子的过程噪声；
根据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表示预设的迭代次数。
6.根据权利要求1所述的基于变分推断的机器人状态估计方法，其特征在于，所述根据粒子权重迭代值对粒子进行重采样的公式为：
其中，
表示n时刻第i个粒子的状态更新值，
为粒子权重迭代值，xn表示n时刻的系统状态，
表示n时刻第i个粒子的粒子状态预测值，N表示粒子个数，δ(·)为狄拉克函数；
所述对粒子权重迭代值进行归一化处理的公式为：
其中，
为n时刻第i个粒子的粒子权重，N为粒子滤波算法的粒子个数。
7.根据权利要求1所述的基于变分推断的机器人状态估计方法，其特征在于，根据重采样后每个粒子的粒子状态
和归一化处理后的粒子权重
计算n时刻机器人状态的估计值
其中，N表示粒子个数；
根据自由度更新值αn和尺度矩阵迭代值ψ'n计算n时刻测量噪声协方差矩阵的估计值
其中dy为测量向量yn的维数。
8.根据权利要求1所述的基于变分推断的机器人状态估计方法，其特征在于，所述每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制时，淘汰的粒子数和复制的粒子数相同。
9.一种基于变分推断的机器人状态估计系统，其特征在于，包括：
模型构建模块：用于构建机器人运动过程的非线性系统动态模型；
数据初始模块：用于将机器人状态参数作为粒子滤波算法中的粒子，设定每个粒子初始时刻的粒子状态和粒子权重，设定所述非线性系统动态模型的参数初始值和Inverse-Wishart分布中的参数值；
数据预测模块：用于利用所述非线性系统动态模型预测当前时刻每个粒子的粒子状态，计算当前时刻Inverse-Wishart分布下的自由度和尺度矩阵预测值，并根据当前时刻的自由度和尺度矩阵预测值确定当前时刻自由度和尺度矩阵的更新值；
迭代更新模块：用于对当前时刻的尺度矩阵更新值和当前时刻每个粒子的粒子权重进行迭代更新，每次迭代更新后根据粒子权重按照预设比例对粒子进行淘汰复制，完成预设次数的迭代更新后，输出每个粒子的粒子权重迭代值和尺度矩阵迭代值；
重采样及归一化模块：用于根据粒子权重迭代值对粒子进行重采样并对粒子权重迭代值进行归一化处理；
数据测算模块：用于根据重采样后每个粒子的粒子状态和归一化处理后的粒子权重计算机器人状态的估计值，根据自由度更新值和尺度矩阵迭代值计算测量噪声协方差矩阵的估计值。
10.一种基于变分推断的机器人状态估计设备，其特征在于，包括：
存储器：用于存储计算机程序；
处理器：用于执行所述计算机程序时实现权利要求1-8任一项所述的基于变分推断的机器人状态估计方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210733585.2A CN115070765B (zh) | 2022-06-27 | 2022-06-27 | 一种基于变分推断的机器人状态估计方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210733585.2A CN115070765B (zh) | 2022-06-27 | 2022-06-27 | 一种基于变分推断的机器人状态估计方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115070765A true CN115070765A (zh) | 2022-09-20 |
| CN115070765B CN115070765B (zh) | 2023-06-13 |
Family
ID=83255760
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210733585.2A Active CN115070765B (zh) | 2022-06-27 | 2022-06-27 | 一种基于变分推断的机器人状态估计方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115070765B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103345577A (zh) * | 2013-06-27 | 2013-10-09 | 江南大学 | 变分贝叶斯概率假设密度多目标跟踪方法 |
| CN103686923A (zh) * | 2013-12-26 | 2014-03-26 | 华北电力大学 | 面向目标跟踪的无线传感器聚簇能量管理方法 |
| CN103902819A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 基于变分滤波的粒子优化概率假设密度多目标跟踪方法 |
| CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
| CN109253727A (zh) * | 2018-06-22 | 2019-01-22 | 东南大学 | 一种基于改进迭代容积粒子滤波算法的定位方法 |
| CN110162871A (zh) * | 2019-05-16 | 2019-08-23 | 东北大学 | 一种基于无迹卡尔曼粒子滤波的电力系统动态估计方法 |
| CN110957011A (zh) * | 2019-11-25 | 2020-04-03 | 江南大学 | 连续搅拌反应器在未知时变测量噪声下的在线生产参数估计方法 |
| CN111609878A (zh) * | 2020-06-10 | 2020-09-01 | 江南大学 | 三自由度直升机系统传感器运行状态监测方法 |
| US20200372352A1 (en) * | 2019-05-22 | 2020-11-26 | Royal Bank Of Canada | System and method for machine learning architecture with variational hyper-rnn |
| CN114519728A (zh) * | 2022-01-25 | 2022-05-20 | 河南大学 | 未知噪声下的变分贝叶斯强跟踪phd多目标跟踪方法 |
| CN114662535A (zh) * | 2022-03-15 | 2022-06-24 | 中国矿业大学 | 一种基于变分贝叶斯学习的井下传感器网络目标跟踪方法 |
-
2022
- 2022-06-27 CN CN202210733585.2A patent/CN115070765B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103345577A (zh) * | 2013-06-27 | 2013-10-09 | 江南大学 | 变分贝叶斯概率假设密度多目标跟踪方法 |
| CN103686923A (zh) * | 2013-12-26 | 2014-03-26 | 华北电力大学 | 面向目标跟踪的无线传感器聚簇能量管理方法 |
| CN103902819A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 基于变分滤波的粒子优化概率假设密度多目标跟踪方法 |
| CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
| CN109253727A (zh) * | 2018-06-22 | 2019-01-22 | 东南大学 | 一种基于改进迭代容积粒子滤波算法的定位方法 |
| CN110162871A (zh) * | 2019-05-16 | 2019-08-23 | 东北大学 | 一种基于无迹卡尔曼粒子滤波的电力系统动态估计方法 |
| US20200372352A1 (en) * | 2019-05-22 | 2020-11-26 | Royal Bank Of Canada | System and method for machine learning architecture with variational hyper-rnn |
| CN110957011A (zh) * | 2019-11-25 | 2020-04-03 | 江南大学 | 连续搅拌反应器在未知时变测量噪声下的在线生产参数估计方法 |
| CN111609878A (zh) * | 2020-06-10 | 2020-09-01 | 江南大学 | 三自由度直升机系统传感器运行状态监测方法 |
| CN114519728A (zh) * | 2022-01-25 | 2022-05-20 | 河南大学 | 未知噪声下的变分贝叶斯强跟踪phd多目标跟踪方法 |
| CN114662535A (zh) * | 2022-03-15 | 2022-06-24 | 中国矿业大学 | 一种基于变分贝叶斯学习的井下传感器网络目标跟踪方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115070765B (zh) | 2023-06-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111178385B (zh) | 一种鲁棒在线多传感器融合的目标跟踪方法 | |
| Most et al. | Metamodel of Optimal Prognosis-an automatic approach for variable reduction and optimal metamodel selection | |
| US8452423B2 (en) | Methods and systems for the design and implementation of optimal multivariable model predictive controllers for fast-sampling constrained dynamic systems | |
| CN111597647B (zh) | 一种面向工业生产过程的弹簧阻尼系统滤波故障诊断方法 | |
| Zhang et al. | Identification of continuous-time nonlinear systems: The nonlinear difference equation with moving average noise (NDEMA) framework | |
| Selby et al. | Learning of causal observable functions for Koopman-DFL lifting linearization of nonlinear controlled systems and its application to excavation automation | |
| Li et al. | A wiener-based remaining useful life prediction method with multiple degradation patterns | |
| CN115070765B (zh) | 一种基于变分推断的机器人状态估计方法及系统 | |
| CN116662733A (zh) | 基于高斯过程回归的变分滤波机动目标跟踪方法 | |
| CN113884914B (zh) | 一种渐消并行卡尔曼滤波动力电池荷电状态估计方法 | |
| CN114136538B (zh) | 一种基于随机变分贝叶斯学习的压力传感器标定装置温度建模方法 | |
| CN114093433B (zh) | 基于观测器的精馏过程单吨能耗预报精度评价方法及系统 | |
| Lippiello et al. | Objects motion estimation via BSP tree modeling and Kalman filtering of stereo images | |
| Hernández et al. | On the precision evaluation in non-linear sensor network design | |
| CN114186190A (zh) | 坐标转换矩阵的计算方法、装置、设备及可读存储介质 | |
| CN113408057A (zh) | 一种基于em算法的飞机气流角高精度离线估计方法 | |
| CN114139461B (zh) | 一种航天器系统不确定性预测方法、装置及存储介质 | |
| CN117268381B (zh) | 一种航天器状态的判断方法 | |
| CN115342817B (zh) | 无人履带车辆轨迹跟踪状态监测方法、系统、设备、介质 | |
| Politi et al. | Comparing data assimilation filters for parameter estimation in a neuron model | |
| CN117381805B (zh) | 一种面向冲突应对的机械臂运行控制方法和系统 | |
| CN117634101B (zh) | 芯片表面形态确定方法、装置、计算机设备和存储介质 | |
| CN113191082B (zh) | 基于机器学习的模型参数获取方法、系统及可读介质 | |
| WO2024095540A1 (en) | System and method for training of neural network model for control of high dimensional physical systems | |
| Wang et al. | The identification of multivariable systems based on a new two-stage hierarchical least-squares iterative algorithm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |