基于联邦卡尔曼滤波的双足机器人行走质心状态估计方法

1.本发明涉及双足机器人技术领域，具体涉及一种基于联邦卡尔曼滤波的双足机器人行走质心状态估计方法。


背景技术：

2.双足机器人采用腿足式运动机构实现行走运动，具有与人相似的运动特点。双足机器人作为移动机器人的分支，相比轮履式机器人，在行走过程中可采取避让、跨越等多种形式适应复杂环境，具有更为强大的复杂非结构化环境适应潜力，因此应用前景十分广阔。
3.但双足机器人全身自由度多，非线性程度高，行走稳定控制的难度较大。双足机器人行走过程中的质心运动状态作为稳定控制的闭环反馈信息，是实现双足机器人稳定行走的重要基础。由于受到环境及传感器噪声的影响，机器人行走过程中质心运动状态的在线估计，其估计可靠性和计算速度是影响稳定控制效果的关键因素。
4.目前方法机器人行走过程中质心运动状态的在线估计以集中式卡尔曼滤波技术为主，使用单个滤波器处理多个传感器的量测信息，虽然能实现行走质心状态估计，但状态向量维数高，计算量较大，难以保证滤波器的实时性；如果其中一个子系统量测更新出现故障，将会直接污染整个系统，导致状态估计出错，偏差较大。


技术实现要素：

5.针对现有技术中存在不足，本发明提供了一种基于联邦卡尔曼滤波的双足机器人行走质心状态估计方法，实现快速而准确的双足机器人行走质心状态在线估计，并且通过信息分配系数的自适应调节，进一步提高系统的容错性能和可靠性。
6.本发明是通过以下技术手段实现上述技术目的的。
7.一种基于联邦卡尔曼滤波的双足机器人行走质心状态估计方法，包括：
8.惯性滤波器，处理惯性测量单元的量测信息经卡尔曼滤波时间更新和量测更新，输出次优质心状态估计向量和对应的误差协方差矩阵p1；
9.运动学滤波器，处理各关节码盘的量测信息经卡尔曼滤波时间更新和量测更新，输出次优质心状态估计向量和对应的误差协方差矩阵p2；
10.线性倒立摆滤波器，处理力传感器的量测信息经卡尔曼滤波时间更新和量测更新，输出次优质心状态估计向量和对应的误差协方差矩阵p3；
11.主滤波器，进行卡尔曼滤波时间更新，得到次优质心状态估计向量和对应的误差协方差矩阵pm；
12.主滤波器基于上述各子滤波器及自身的次优质心状态估计向量和误差协方差矩阵进行最优融合，获得当前时刻最优质心状态估计向量和对应的误差协方差矩阵pg，所述作用于机器人；
13.利用所述pg和信息分配系数β对各滤波器进行反馈重置，重复上述步骤，得到下一时刻的最优质心状态估计向量，继续作用于机器人；
14.重复上述过程，直至机器人停止行走运动。
15.进一步地，所述信息分配系数包括各子滤波器的信息分配系数和主滤波器的信息分配系数，所述主滤波器的信息分配系数为定常值，所述各子滤波器的信息分配系数满足：
[0016][0017]
其中：为容错信息分配系数，为精度信息分配系数，βm为主滤波器分配系数，ρ为权重分配因子，且0≤ρ≤1；
[0018]
所述容错信息分配系数满足：
[0019][0020]
式中cond(ri)为第i个子滤波器的量测噪声方差矩阵ri的条件数；
[0021]
所述精度信息分配系数满足：
[0022][0023]
其中‖pi‖f为第i个子滤波器的误差协方差矩阵pi的frobenius范数。
[0024]
进一步地，所述次优质心状态估计向量是通过以下方式得到的：
[0025][0026]
其中：是t
k+1
时刻各子滤波器的卡尔曼增益，是t
k+1
时刻各子滤波器的次优状态估计，是t
k+1
时刻各子滤波器与次优状态估计对应的误差协方差矩阵，是t
k+1
时刻各子滤波器的量测矩阵，为t
k+1
时刻各子滤波器量测噪声方差矩阵，为各子滤波器t
k+1
时刻对应传感器的量测信息，和分别是t
k+1
时刻各子滤波器的质心状态先验估计和质心状态先验估计误差协方差矩阵。
[0027]
更进一步地，所述最优融合采用如下方式：
[0028][0029]
其中：为t
k+1
时刻质心状态估计器的全局误差协方差矩阵，为t
k+1
时刻质心状态估计器的全局最优质心状态估计向量。
[0030]
进一步地，所述惯性测量单元的量测信息为：
[0031][0032]
其中和为在世界坐标系x方向和y方向的分量，且：
[0033][0034]
其中：为tk时刻滤波后的世界坐标系下加速度量测向量，为t
k-1
时刻滤波后的世界坐标系下加速度量测向量，
wak
为世界坐标系下tk时刻惯性测量单元的加速度量测向量，μ为量测加速度一阶惯性滤波系数，且有0＜μ＜1。
[0035]
进一步地，所述各关节码盘的量测信息为：
[0036][0037]
其中和为在x方向和y方向的分量，和为在x方向和y方向的分量，所述为质心位置的量测值，为质心速度的量测值，且：
[0038][0039][0040]
其中：wp
foot
(tk)是tk时刻支撑腿落脚点在世界坐标系下的位置，rk是tk时刻支撑腿落脚点局部坐标系相对于世界坐标系的姿态旋转矩阵，t为离散周期，forwardkinematic(
·
)是基于denavit-hartenberg方法得到的双足机器人单腿正运动学计算函数。
[0041]
进一步地，所述力传感器的量测信息为：
[0042][0043]
其中：g为重力加速度常值，zc为质心高度常值，为质心位置的量测的平面分量，wp
zmp
(tk)为tk时刻世界坐标系下的零力矩点位置。
[0044]
更进一步地，所述tk时刻世界坐标系下的zmp位置满足：
[0045][0046]
其中：为tk时刻世界坐标系下左脚脚底板的位置，为tk时刻世界坐标系下右脚脚底板的位置，α(tk)为tk时刻两脚受到的地面竖直作用力的分配占比，且：
[0047][0048]
其中：和分别为tk时刻经一阶惯性滤波处理后的左脚和右脚世界坐标系z方向的地面作用力数值。
[0049]
进一步地，所述主滤波器和各子滤波器进行时间更新：
[0050][0051]
其中为各滤波器t
k+1
时刻的质心状态先验估计，p
ik+1，k
为各滤波器t
k+1
时刻的质心状态先验估计误差协方差矩阵，f
ik+
′
，k
为各滤波器的系统矩阵，g
ik+
′
，k
为各滤波器的控制矩阵，为各滤波器tk时刻的控制输入，是tk时刻各滤波器与其次优状态估计对应的误差协方差矩阵，γ
ik
为各滤波器tk时刻的噪声驱动矩阵，是各滤波器的过程噪声方差矩阵。
[0052]
更进一步地，所述各滤波器的系统矩阵为：
[0053][0054]
其中：t为离散周期；
[0055]
所述各滤波器的控制矩阵为：
[0056][0057]
本发明的有益效果为：
[0058]
(1)本发明利用惯性滤波器、运动学滤波器和线性倒立摆滤波器三个子滤波器，分别处理惯性测量单元、关节码盘和力传感器的量测信息，为分散式滤波结构，状态维数低，计算速度快，能够保证质心状态估计的实时性；
[0059]
(2)本发明对各滤波器进行反馈重置时的信息分配系数可自适应调节，通过调节权重分配因子能够动态调整状态估计的容错性能和估计精度，当传感器出现故障时可提高状态估计的容错性能，当传感器正常工作时可提高状态估计的估计精度，保证了状态估计的可靠性。
附图说明
[0060]
图1为本发明所述行走质心状态估计器结构示意图；
[0061]
图2为本发明所述质心状态估计所用传感器机载位姿及坐标系定义说明图；
[0062]
图3为本发明所述行走质心状态估计流程图。
具体实施方式
[0063]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0064]
联邦卡尔曼滤波属于分散化滤波技术，联邦卡尔曼滤波器是一种两级滤波器，包含子滤波器和主滤波器。子滤波器相互独立并行，分别处理不同的传感器的量测信息，可得到局部次优状态估计，主滤波器对各局部估计进行最优融合，得到全局最优状态估计。联邦卡尔曼滤波使用信息分配原则来保证全局估计的最优性，使用方差上界技术来保证各子滤
波器估计的不相关性。
[0065]
本发明的双足机器人行走质心状态估计器结构如图1所示。估计器的输入是根据机载传感器实时测量数据计算得到的量测信息，包括惯性测量单元(imu)的量测信息双足机器人下肢各关节码盘(joint encoder)的量测信息和足端力传感器(force sensor)的量测信息采用分散化滤波方式，基于卡尔曼滤波原理，分别设计了三个子滤波器独立处理三种传感器的量测信息，其中惯性滤波器(第一个子滤波器)处理惯性测量单元的量测信息输出次优质心状态估计向量和对应的误差协方差矩阵p1；运动学滤波器(第二个子滤波器)处理各关节码盘的量测信息输出次优质心状态估计向量和对应的误差协方差矩阵p2；线性倒立摆(lipm)滤波器(第三个子滤波器)处理力传感器的量测信息输出次优质心状态估计向量和对应的误差协方差矩阵p3；主滤波器不接收量测信息，仅进行时间更新，得到次优质心状态估计向量和对应的误差协方差矩阵pm。随后，主滤波器基于各子滤波器及自身的次优状态估计向量和对应的误差协方差矩阵进行最优融合，获得当前时刻最优质心状态估计向量和对应的误差协方差矩阵pg。在子滤波器和主滤波器进行状态估计前，基于上一时刻质心最优状态估计向量对应的误差协方差矩阵pg和信息分配系数β对子滤波器进行反馈重置，惯性滤波器的信息分配系数为β1，运动学滤波器的信息分配系数为β2，线性倒立摆滤波器的信息分配系数为β3，主滤波器的信息分配系数为βm。
[0066]
本发明的行走质心状态估计器用于在线估计双足机器人行走过程中世界坐标系下的质心前进(x)方向和横移(y)方向的位置、速度和加速度。采用三维线性倒立摆(3d-lipm)模型表征双足机器人行走动力学，将双足机器人行走时的横移方向运动与前进方向运动解耦，基于本发明提出的行走质心状态估计器，将x和y两个方向的运动状态合并，同时估计x和y方向的质心运动状态。
[0067]
因此，定义行走质心状态估计器中各滤波器的质心状态向量为：
[0068][0069]
其中：其中：表示实数域，表示实数域上的6维线性空间，其余表示以此类推；推；分别为对应滤波器世界坐标系下质心x方向和y方向的估计位置，分别为对应滤波器世界坐标系下质心x方向和y方向的估计速度，分别为对应滤波器世界坐标系下质心x方向和y方向的估计加速度。
[0070]
以北京理工大学bhr6p双足机器人为例，说明用于质心状态估计的各传感器位置分布及坐标系定义。如图2所示，bhr6p双足机器人全身共23个自由度，其中上肢共8个自由度，腰部共3个自由度，下肢共12个自由度。六维力传感器位于机器人两条腿足端踝关节处，惯性测量单元(imu)位于机器人颈肩部，关节码盘位于各关节处，其数量与关节数一致。质心(com)坐标系原点位于两髋中心处，质心坐标系x方向朝机器人前方，y方向朝机器人左方，z方向竖直向上，该坐标系固连于机器人。世界(world)坐标系原点是机器人启动运动前质心坐标系原点向地面的竖直投影，位于两脚正中位置，其坐标系方向与初始质心坐标系朝向一致，该坐标系固连于地。imu坐标系原点位于imu测量单元正中心，其朝向与初始质心坐标系朝向一致，该坐标系固连于机器人。质心坐标系和imu坐标系的相对位姿可通过测量
工具进行标定。左脚落脚点(lfoot)坐标系和右脚落脚点(rfoot)坐标系原点分别位于左踝关节和右踝关节向各自脚底板的竖直投影处，左脚落脚点坐标系和右脚落脚点坐标系固连于机器人，其初始朝向与世界坐标系相同。
[0071]
本发明的双足机器人行走质心状态估计器的状态估计流程如图3所示。状态估计过程主要分为五个步骤，分别是系统的初始化、信息分配、时间更新、量测更新和最优融合。
[0072]
(1)系统的初始化部分，需要配置质心最优状态估计向量对应误差协方差矩阵pg，各滤波器信息分配系数βi(i＝1,2,3,m)的初值，全局过程噪声矩阵qg和量测噪声矩阵ri(i＝1,2,3)的测定值；机器人开始运动前，质心位于世界坐标系正上方，则质心最优状态估计向量初值为：
[0073][0074]
对各传感器量测噪声，依据传感器实测数据及量测计算进行标定，可得到惯性测量单元量测噪声矩阵测定值关节码盘量测噪声矩阵测定值和力传感器量测噪声矩阵测定值误差协方差矩阵pg和过程噪声矩阵qg依据工程经验配置初值，可通过多组实验进行参数调整，直至估计结果收敛；
[0075]
对于各滤波器信息分配系数βi(i＝1,2,3,m)，采用联邦卡尔曼滤波器变比例模式，对系数进行平均分配，则各滤波器信息分配系数的初值：
[0076][0077]
(2)信息分配部分，基于上一时刻最优质心状态估计结果或估计初始值，对子滤波器进行反馈重置，用最优质心状态估计向量重置各滤波器次优状态估计向量
[0078][0079]
其中为tk时刻各滤波器的次优状态估计向量，为tk时刻的最优质心状态估计向量；
[0080]
基于信息分配系数和上一时刻最优估计结果(或最优质心状态估计向量初值)反馈重置各滤波器误差协方差矩阵pi和过程噪声矩阵qi：
[0081][0082][0083]
信息分配系数始终服从信息守恒原则，即：
[0084][0085]
传统的联邦卡尔曼滤波器采用恒定的信息分配系数，难以应对复杂环境变化，当传感器发生故障时，容易导致估计受到污染而发散失效，难以保证质心状态估计的精度，因此，本发明提出一种信息分配系数自适应调节的方法，可以降低故障传感器对全局最优质心状态估计的影响，提高质心状态估计的容错性和可靠性，保证状态估计的精度。
[0086]
主滤波器只进行时间更新，不进行量测更新，其估计精度不会受到传感器故障的影响，因此，根据主滤波器过程模型与实际双足机器人运动的符合程度设定主滤波器信息
分配系数为定常值，即：
[0087][0088]
对子滤波器，设计如下信息分配系数自适应调节规则：
[0089][0090]
为容错信息分配系数，其计算公式为：
[0091][0092]
式中cond(ri)为第i个子滤波器的量测噪声矩阵ri的条件数，其计算公式为：
[0093][0094]
其中‖ri‖为量测噪声矩阵ri的范数，为量测噪声矩阵ri的逆矩阵的范数，cond-1
(ri)为量测噪声矩阵ri条件数的倒数。
[0095]
cond(ri)可反映故障传感器对全局最优质心状态估计的污染程度，cond(ri)越小，则全局污染程度越大。为提高子滤波器的容错性能，设置容错信息分配系数与cond(ri)成反比。当传感器发生故障，使得全局污染加重、cond(ri)变小时，则应当增大容错信息分配系数，使其他无故障传感器对应的子滤波器的信息分配系数减小，增强系统容错性能。
[0096]
为精度信息分配系数，其计算公式为：
[0097][0098]
其中‖pi‖f为第i个子滤波器的误差协方差矩阵pi的frobenius范数，其计算公式为：
[0099][0100]
为矩阵的迹，为误差协方差矩阵pi的frobenius范数的倒数。
[0101]
‖pi‖f可度量误差协方差矩阵所反映的估计误差大小，估计误差越大，该子滤波器的估计精度越低，则分配的信息系数越小，即精度信息分配系数与估计误差成反比，从而提高全局最优质心状态估计的精度。
[0102]
对容错信息分配系数和精度信息分配系数显然有：
[0103][0104][0105]
即本发明提出的信息分配系数自适应调节方法仍满足信息守恒原理(式(7))。
[0106]
式(9)中ρ为权重分配因子，其取值范围为0≤ρ≤1，用于调节容错信息分配系数和精度信息分配系数在信息分配系数βi中的占比，当ρ
→
0时，信息分配系数自适应调节方法倾向于提高系统的容错能力，当ρ
→
1时，信息分配系数自适应调节方法倾向于提高系
统的估计精度。权重分配因子ρ可根据各传感器故障率和实际测试情况确定。
[0107]
(3)时间更新部分，基于三维线性倒立摆(3d-lipm)模型进行时间更新，各滤波器基于上一时刻的质心状态估计结果、误差协方差矩阵和过程模型进行一步预测，得到当前时刻质心状态先验估计及其先验误差协方差矩阵：
[0108][0109]
其中为各滤波器t
k+1
时刻的质心状态先验估计，p
ik+1,k
为各滤波器t
k+1
时刻的质心状态先验估计误差协方差矩阵，f
ik+1,k
为各滤波器的系统矩阵，g
ik+1,k
为各滤波器的控制矩阵，为各滤波器tk时刻的控制输入，是tk时刻各滤波器与其次优状态估计对应的误差协方差矩阵，γ
ik
为各滤波器tk时刻的噪声驱动矩阵，是各滤波器的过程噪声方差矩阵。
[0110]
针对双足机器人的行走运动，采用经典三维线性倒立摆动力学模型，可得系统的状态方程(过程模型)为：
[0111][0112]
x,y为世界坐标系下双足机器人质心在x和y方向的位置，为世界坐标系下双足机器人在x和y方向的速度，为世界坐标系下双足机器人在x和y方向的加速度，u
x
和uy为系统x和y方向的控制输入。
[0113]
取该状态方程作为子滤波器和主滤波器的过程模型，并将状态方程离散化，可得：
[0114]
[0115][0116]
其中：t为离散周期，即t
k+1-tk；
[0117]
由于各子滤波器和主滤波器具有相同的估计状态，噪声驱动阵为单位矩阵，即：
[0118][0119]
(4)量测更新部分，各子滤波器先依据各自传感器信息计算量测信息，然后计算卡尔曼增益，基于tk时刻的质心状态先验估计及其先验估计误差协方差矩阵量测更新得到t
k+1
时刻各自的次优状态估计和对应的误差协方差矩阵
[0120]
对惯性滤波器，采用imu的测量加速度作为量测信息进行该滤波器的量测更新，惯性滤波器用于量测更新加速度，其卡尔曼滤波量测模型为：
[0121][0122]
其中为惯性滤波器的量测向量，为惯性滤波器的量测矩阵，为惯性滤波器的质心状态向量，为惯性滤波器的量测噪声矩阵，且
[0123]
以tk时刻为例，将tk时刻惯性测量单元(imu)读取得到的imu坐标系下的加速度量测向量
imuak
经姿态变换转换到世界坐标系下描述，获得世界坐标系下tk时刻的加速度量测向量
wak
，即：
[0124]
wak
＝
wrimuimuak
ꢀꢀꢀ
(22)
[0125]
其中
wrimu
为imu坐标系相对于世界坐标系的旋转矩阵，且有为imu坐标系相对于世界坐标系的旋转矩阵，且有
[0126]
对
wak
进行一阶惯性滤波，得到tk时刻滤波后的世界坐标系下加速度量测向量即：
[0127][0128]
其中为t
k-1
时刻(上一时刻)滤波后的世界坐标系下加速度量测向量，μ为量测加速度一阶惯性滤波系数，且有0《μ《1，
[0129]
则惯性滤波器tk时刻的惯性测量单元的量测信息为：
[0130][0131]
其中和为在世界坐标系下x方向和y方向的分量。
[0132]
对运动学滤波器，采用单腿正运动学计算浮动基相对于支撑脚的位置及差分线速度作为量测信息进行量测更新；运动学滤波器用于量测更新质心位置和速度，其卡尔曼滤波量测模型为：
[0133][0134]
其中为运动学滤波器的量测向量，为运动学滤波器的量测矩阵，为运动学滤波器的质心状态向量，为运动学滤波器的量测噪声矩阵，且为运动学滤波器的量测噪声矩阵，且
[0135]
双足机器人行走时与人相似，两只脚交替与地面接触，存在单腿支撑期和摆动期，当支撑腿与地面保持接触时，可根据支撑腿的位姿和腿部各关节码盘数据量测浮动基(质心)的位姿及其运动增量。
[0136]
以t
k-1
时刻和tk时刻为例，记t
k-1
时刻从关节码盘读取的支撑腿关节角向量为q
k-1
，tk时刻从关节码盘读取的支撑腿关节角向量为qk，bhr6p机器人单腿共6个关节角，则，bhr6p机器人单腿共6个关节角，则世界坐标系下落脚点位置和浮动基位置的关系式为：
[0137]wp
foot
(t
k-1
)＝p
k-1
+r
k-1
*forwardkinematic(q
k-1
)
ꢀꢀꢀ
(26)
[0138]wp
foot
(tk)＝pk+rk*forωardkinematic(qk)
ꢀꢀꢀ
(27)
[0139]
其中wp
foot
(t
k-1
)是t
k-1
时刻支撑腿落脚点在世界坐标系下的位置，p
k-1
为t
k-1
时刻浮动基在世界坐标系下的位置，r
k-1
是t
k-1
时刻支撑腿落脚点局部坐标系相对于世界坐标系的姿态旋转矩阵，wp
foot
(tk)是tk时刻支撑腿落脚点在世界坐标系下的位置，pk为tk时刻浮动基在世界坐标系下的位置，rk是tk时刻支撑腿落脚点局部坐标系相对于世界坐标系的姿态旋转矩阵，且forwardkinematic(
·
)是基于denavit-hartenberg方法(为现有技术)得到的双足机器人单腿正运动学计算函数，该函数以双足机器人单腿关节角向量为输入，输出为支撑腿落脚点相对于质心坐标系的三维位置向量。
[0140]
当支撑腿保持与地面稳定接触时，在该支撑期内，落脚点在世界坐标系下的位置保持不变，即：
[0141]wp
foot
(t
k-1
)＝wp
foot
(tk)
ꢀꢀꢀ
(28)
[0142]
质心速度量测可由位置差分得到，即：
[0143][0144]
将式(27)-(28)代入(29)，可得质心速度量测公式为：
[0145][0146]
为质心速度的量测值，
[0147]
质心位置量测可由式(27)得到，其公式为：
[0148][0149]
为质心位置的量测值，为质心位置的量测值，和rk由外部定位传感器或双足机器人运动规划技术得到(现有技术)。
[0150]
由式(30)、(31)可得运动学滤波器tk时刻的关节码盘的量测信息为：
[0151][0152]
其中和为在x方向和y方向的分量，和为在x方向和y方向的分量。
[0153]
对lipm滤波器，采用基于线性倒立摆的零力矩点(zmp)动力学对质心加速度进行量测更新，其卡尔曼滤波量测模型为：
[0154][0155]
其中为lipm滤波器的量测向量，为lipm滤波器的量测矩阵，为lipm滤波器的质心状态向量，为lipm滤波器的量测噪声矩阵，且为lipm滤波器的量测噪声矩阵，且
[0156]
lipm滤波器需使用两条腿踝关节的六维力传感器量测信息计算zmp。假设双足机器人为点足，则机器人每只脚的zmp位置与落脚点位置相同，双足机器人行走时的zmp二维位置为：
[0157][0158]
其中wp
zmp
(tk)为tk时刻世界坐标系下的zmp位置，为tk时刻世界坐标系下左脚脚底板的位置，为tk时刻世界坐标系下右脚脚底板的位置，时刻世界坐标系下右脚脚底板的位置，α(tk)为tk时刻两脚受到的地面竖直作用力的分配占比，其计算公式为：
[0159][0160]
和分别为tk时刻经一阶惯性滤波处理后的左脚和右脚世界坐标系z方向(竖直向上)的地面作用力数值，其计算方法为：
[0161][0162]
其中和分别为tk时刻左脚和右脚力传感器测量的世界坐标系z方向(竖直向上)的地面作用力数值，和分别为t
k-1
时刻经一阶惯性滤波处理后的左脚和右脚世界坐标系z方向(竖直向上)的地面作用力数值，σ为力传感器一阶惯性滤波系数，且有0《σ《1。
[0163]
依据线性倒立摆的零力矩点(zmp)动力学，计算量测加速度作为力传感器的量测信息：
[0164][0165]
其中为tk时刻lipm滤波器的力传感器的量测信息，g为重力加速度常值，zc为质心高度常值，为质心位置量测的平面分量，即：
[0166][0167]
各子滤波器依据传感器数据计算得到各自量测信息后，基于时间更新得到的tk时刻的质心状态先验估计及其先验误差协方差矩阵p
ik+1，k
，进行量测更新：
[0168][0169]
其中是t
k+1
时刻各子滤波器的卡尔曼增益，是t
k+1
时刻各子滤波器的次优状态估计，是t
k+1
时刻各子滤波器与次优状态估计对应的误差协方差矩阵，是t
k+1
时刻各子滤波器的量测矩阵，为t
k+1
时刻各子滤波器量测噪声方差矩阵，为各子滤波器t
k+1
时刻的对应传感器的量测信息。
[0170]
对主滤波器，因只进行卡尔曼滤波时间更新，无需进行量测更新，取时间更新得到的主滤波器的质心状态先验估计及其先验误差协方差矩阵分别作为主滤波器的次优状态估计和与次优状态估计对应的误差协方差矩阵即：
[0171][0172]
(5)最优融合部分，依据各滤波器的次优状态估计和对应的误差协方差矩阵可按下式计算全局最优质心状态估计向量和对应的全局误差协方差矩阵：
[0173][0174]
其中为t
k+1
时刻质心状态估计器的全局误差协方差矩阵，为t
k+1
时刻质心状态估计器的全局最优质心状态估计向量。
[0175]
是质心状态估计器的输出，可作为双足机器人行走稳定控制的反馈输入用于保持机器人行走的稳定性，也可用于实时校正双足机器人运动导航路径规划中机器人实际运动状态。
[0176]
当双足机器人仍在行走时，时间向前累进，进行下一时刻的信息分配、时间更新、量测更新和最优融合步骤，得到下一时刻的全局最优质心状态估计向量。当双足机器人停止行走运动后，行走质心状态估计器停止工作。
[0177]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换
或变型均属于本发明的保护范围。