CN104050355B - 一种基于h∞滤波的机械臂外力估计方法 - Google Patents

Abstract

本发明所述的基于H_∞滤波的机械臂外力估计方法，包括：采用模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的动力学模型；将外力看作是状态空间方程的状态变量，基于H_∞滤波估计外力；再用双线性变换的方法设计IIR数字滤波器，从而实现在实际机械臂系统中应用。

Description

一种基于H∞滤波的机械臂外力估计方法

(一)技术领域

本发明属于H_∞滤波和数字滤波技术在机械臂外力估计中的应用。

(二)背景技术

近年来，随着机器人技术的发展，机器人被应用于工业和服务业的许多领域。机器人的任务可以大致分为两类:一类是机器人在运行过程中不需要与环境相接触，单纯的位置控制就可以解决问题；另一类是要求机器人与环境相接触，例如打磨，抛光，修边等，对于这类任务单纯的位置控制已经不能胜任。这类任务一般对接触力的大小有严格的要求，因为机器人末端微小的位置误差都可能导致巨大的接触力，易对机器人和目标物造成损伤。工业机器人对接触力进行控制，首先需要测量机械臂在运行过程中末端受到的外力信息。工业应用中一般在机器人末端执行器安装力传感器来测量所受到的外力，但是力传感器的价格昂贵，且在高温和潮湿环境下可能无法正常工作。由于成本和使用条件的限制，考虑基于机械臂动力学方程，由机械臂关节驱动器的力矩去估计机械臂末端受到的外力将变得非常有实际应用价值。

(三)发明内容

本发明针对实际的工业机器人中通常没有安装关节加速度传感器，使用数字低通滤波技术在基于动力学方程估计末端受到外力时避免直接使用加速度的信息，并平滑夹杂高频噪声的关节驱动器力矩信号。

本发明由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的动力学模型，将外力看作是状态空间方程的状态变量，基于H_∞滤波估计外力；再用双线性变换的方法设计IIR数字滤波器，从而实现在实际机器人系统中应用。

一种基于H_∞滤波的机械臂外力估计方法，其特征在于，具体步骤如下：

(1)由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；

串联机械臂的动力学方程的一般形式如下式:

q是关节角度的矢量,M(q)为机械臂的惯量矩阵,是离心力和哥氏力矢量,G(q)是重力矢量,τ_d表示未知的扰动力,J(q)是机械臂的雅克比矩阵,F_ext代表施加在机械臂末端的外力，τ是关节驱动器的输出力矩，一般是由测量电机电流计算得到，所以夹杂着噪声。

以下都以s表示拉普拉斯变换复变量，z表示Z变换复变量，选用的模拟低通滤波器的传递函数为

ω_c是模拟低通滤波器的3dB截止频率，冲激响应为

滤波之后的(1)式如下所示

上式中r代表时间积分变量,由分部积分公式

∫udv＝uv-∫vdu

设u＝f(t-r)M(q),可得

假设机械臂的初始状态是静止的,即而f(0)＝ω_c,将(5)代入(4),可得

(6)式中出现了新的卷积

令

假设我们用符号<x(t)>_F(s)表示信号x(t)经过传递函数为F(s)的滤波器滤波之后的结果，则由(6)可导出不显含加速度的机械臂动力学模型：

(2)用双线性变换方法由模拟低通滤波器设计IIR数字滤波器；

双线性变换实现从s平面到z平面的映射为

T表示采样周期，则传递函数为F(s)的滤波器经过双线性变换后的系统函数为

同理，

设期望的数字滤波器截止频率Ω_c,由s域的频率变量ω和z域的频率变量Ω的关系

计算得到参数ω_c，得到IIR滤波器的系统函数F(z)，使设计的数字滤波器的截止频率可以映射到Ω_c；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得

如果外力F_ext变化相对平缓,上式可以写成

令

<J(q)^T>_F(z)＝H_k

-<τ_d>_F(z)＝v_k

k＝0,1,2...，代表各个时刻的离散值，可以得到

加入外力随机游动情况下，外力F_ext的状态方程

可得状态空间方程

其中，w_k,v_k分别为状态空间方程的系统噪声和量测噪声；

(3)基于建立的状态空间方程，采用H_∞滤波方法估计状态变量F_ext,选择估计变量

其中

算法步骤为

并且在每个时刻k必须满足条件

设计H_∞滤波中初始估计误差,系统噪声w_k和量测噪声v_k的权重矩阵P₀,Q_k和R_k，使代价函数的界1/θ满足条件。

再由双线性变换的方法由模拟低通滤波器设计IIR数字滤波器方法，其特征在于：步骤(2)中，由模拟滤波器的传递函数F(s)通过双线性变换设计IIR滤波器F(z)，由s域的频率变量ω和z域的频率变量Ω的关系，设定期望的数字滤波器截止频率Ω_c得到参数ω_c，使IIR滤波器的截止频率映射到期望截止频率。

步骤(3)中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于状态空间方程设计H_∞滤波器,实时估计外力。

本发明中不显含加速度的动力学模型推导过程为：

串联机械臂的动力学方程的一般形式如下式:

q是关节角度的矢量,M(q)为机械臂的惯量矩阵,是离心力和哥氏力矢量,G(q)是重力矢量,τ_d表示未知的扰动力,J(q)是机械臂的雅克比矩阵,F_ext代表施加在机械臂末端的外力,τ是关节驱动器的输出力矩,一般是由测量电机电流计算得到,所以夹杂着噪声.

以下都以s表示拉普拉斯变换复变量，z表示Z变换复变量。我们使用模拟低通滤波器,它的传递函数为

ω_c是模拟低通滤波器的3dB截止频率，冲激响应为

由卷积定理可知,在频率域与(2)相乘等于在时域与(3)卷积,滤波之后的(1)式如下所示

上式中r代表时间积分变量,由分部积分公式

∫udv＝uv-∫vdu

动力学方程(1)只有等式左边第一项包含着加速度的项,设u＝f(t-r)M(q), 可得

此时上式已经不显含加速度的项,我们假设机械臂的初始状态是静止的,即而f(0)＝ω_c,将(5)代入(4),可得

(7)式中出现了新的卷积,由

得到第二个卷积的传递函数。由于(6)式中项比较多,为了表示方便,令

假设我们用符号<x(t)>_F(s)表示信号x(t)经过传递函数为F(s)的滤波器滤波之后的结果，则由(6)可导出不显含加速度的机械臂动力学模型：

我们利用双线性变换的方法去设计IIR滤波器。双线性变换实现从s平面到z平面的映射为

上式中T表示采样周期,则传递函数为F(s)的滤波器经过双线性变换后的系统函数为

同理,

s域的频率变量ω和z域的频率变量Ω有如下关系

数字滤波器的截止频率对结果会有影响。理想情况下,截止频率应大于关节力矩信号的最高频率,但是过高的截止频率会影响滤除高频噪声的效果。数字滤波器的3dB截止频率设计方法为：由期望的数字滤波器截止频率Ω_c，代入(8)计算得到参数ω_c，即可得到截止频率为Ω_c数字滤波器的系统函数F(z)。

经过数字滤波器后,再整理(7),我们可以得到

如果外力F_ext变化相对平缓,上式可以写成

令

<J(q)^T>_F(z)＝H_k

-<τ_d>_F(z)＝v_k

可以得到

k＝0,1,2...代表各个时刻的离散值，上式可以看成是状态空间的观测方程,v_k是量测误差,又由于外力F_ext随时间变化,只考虑外力是随机游动的情况,可以设状态方程为

合并(9)和(10)可以得状态空间方程

其中，w_k,v_k分别为状态空间方程的系统噪声和量测噪声。

本发明推导出不显含加速度的动力学模型，并加入随时间变化外力的状态方程，基于状态空间方程设计H_∞滤波器估计外力F_ext。

采用H_∞滤波方法估计状态变量F_ext,选择估计变量

其中

H_∞滤波的代价函数为

上式中z_k表示变量真值，表示估计值，即向量的加权内积,S_k为对应时刻状态向量的权重矩阵,其余加权内积说明同上,代价函数可以被设定为小于1/θ,即

算法步骤为

并且在每个时刻k必须满足条件

H_∞滤波中的P₀,Q_k,R_k分别是初始估计误差,系统噪声w_k和量测噪声v_k的权重矩阵,1/θ是代价函数的界,增大θ可以提高估计精度,但是必须满足(12),所以应根据情况折中选择θ。

本发明的优点是：可以有效地滤除关节驱动器力矩信号的高频成分，平滑力矩信号；基于不显含动力学方程估计外力，不需要直接测量机器人关节的加速度信号；运用H_∞滤波的方法可以在系统存在干扰的情况下，能有效抑制噪声干扰对力估计精度的影响。在实际应用中，本发明不依赖机械臂关节加速度信号，由关节驱动器力矩信号估计机械臂末端执行器受到的外力，可替代昂贵的力传感器；可降低机械臂力控制成本，在机器人打磨、抛光、装配和雕刻等加工领域具有应用前景。

附图说明

图1为在MATLAB中搭建的二自由度机械臂仿真模型图。Env是整个仿真模型的环境模块,在分析模式中我们选择Inverse dynamics。External Force模块中是施加在机械臂末端的外力信号。Joint1Motion和Joint2Motion模块中是驱动关节运动的信号,包括关节角位置,速度和加速度。SensorQ1和SensorQ2是位置传感器,输出关节1和关节2的关节角度。SensorV1和SensorV2是速度传感器,输出关节1和关节2的关节角速度。SensorF1和SensorF2是关节力矩传感器,输出关节1和关节2的力矩值。

图2为本发明实施例中施加在机械臂末端的外力信号。

图3为本发明实施例中在高斯白噪声均值为0，方差为0.1时，估计外力的效果图。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步描述。

一种基于H_∞滤波的机械臂外力估计方法，其特征在于，具体步骤如下：

(1)由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；

串联机械臂的动力学方程的一般形式如下式:

q是关节角度的矢量,M(q)为机械臂的惯量矩阵,是离心力和哥氏力矢量,G(q)是重力矢量,τ_d表示未知的扰动力,J(q)是机械臂的雅克比矩阵,F_ext代表施加在机械臂末端的外力，τ是关节驱动器的输出力矩，一般是由测量电机电流计算得到，所以夹杂着很强的噪声；

以下都以s表示拉普拉斯变换复变量，z表示Z变换复变量，选用的模拟低通滤波器的传递函数为

ω_c是模拟低通滤波器的3dB截止频率，冲激响应为

滤波之后的(1)式如下所示

上式中r代表时间积分变量,由分部积分公式

∫udv＝uv-∫vdu

设u＝f(t-r)M(q),可得

假设机械臂的初始状态是静止的,即而f(0)＝ω_c,将(5)代入(4),可得

(8)式中出现了新的卷积

令

假设我们用符号<x(t)>_F(s)表示信号x(t)经过传递函数为F(s)的滤波器滤波之后的结果，则由(6)可导出不显含加速度的机械臂动力学模型：

(2)用双线性变换方法由模拟低通滤波器设计IIR数字滤波器；

双线性变换实现从s平面到z平面的映射为

T表示采样周期，则传递函数为F(s)的滤波器经过双线性变换后的系统函数为

同理，

设期望的数字滤波器截止频率Ω_c,由s域的频率变量ω和z域的频率变量Ω的关系

计算得到参数ω_c，得到IIR滤波器的系统函数F(z)，使设计的数字滤波器的截止频率可以映射到Ω_c；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得

如果外力F_ext变化相对平缓,上式可以写成

令

<J(q)^T>_F(z)＝H_k

-<τ_d>_F(z)＝v_k

k＝0,1,2...，代表各个时刻的离散值，可以得到

加入外力随机游动情况下，外力F_ext的状态方程

可得状态空间方程

其中，w_k,v_k分别为状态空间方程的系统噪声和量测噪声；

(3)基于建立的状态空间方程，采用H_∞滤波方法估计状态变量F_ext,选择估计变量

其中

算法步骤为

并且在每个时刻k必须满足条件

设计H_∞滤波中初始估计误差,系统噪声w_k和量测噪声v_k的权重矩阵P₀,Q_k和R_k，使代价函数的界1/θ满足条件。

再由双线性变换的方法由模拟低通滤波器设计IIR数字滤波器方法，其特征在于：步骤(2)中，由模拟滤波器的传递函数F(s)通过双线性变换设计IIR滤波器F(z)，由s域的频率变量ω和z域的频率变量Ω的关系，设定期望的数字滤波器截止频率Ω_c得到参数ω_c，使IIR滤波器的截止频率映射到期望截止频率。

步骤(3)中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于状态空间方程设计H_∞滤波器,实时估计外力。

考虑如图1所示的二自由度机械臂仿真系统,在Joint1Motion和Joint2Motion模块中输入驱动信号,使机械臂运行一段轨迹。τ＝[τ₁ τ₂]^T即为SensorF1和SensorF2输出关节1和关节2的力矩值,在力矩信号上叠加高斯白噪声,即量测误差v_k是高斯白噪声。令量测误差v_k的均值为0，设采样周期T＝0.01s。其中连杆1的长度l₁＝0.4m,质量m₁＝15kg,连杆2的长度l₂＝0.2m,质量m₂＝5kg,重力加速度g＝9.81m/s²。External Force模块中所施加的外力信号如图2所示。为了简化二自由度机械臂的动力学方程,假设机械臂每个连杆的质量都集中在连杆的末端,因此每个连杆质心的惯性张量为零矩阵。根据(1)式可以推出动力学方程为

二自由度平面机械臂受到的外力矢量为

F_ext＝[F_x F_y]^T

具体实施本发明的步骤为：

(1)设期望的数字滤波器截止频率Ω_c＝20.5πrad/s,由(8)得ω_c＝2/T,计算得到IIR滤波器的系统函数为

(2)用设计出的IIR数字滤波器对二自由度的机械臂动力学方程信号进行滤波，建立不显含加速度的动力学模型，得到y_k和H_k。

(3)设计H_∞滤波的S_k,P₀,Q_k和R_k，本例中选择

机械臂从静止状态开始运行一段轨迹,开始阶段不受到任何外力,令

为了满足(12),通过预先确定机械臂关节运动轨迹可以得到H_k的取值区间,性能上界参数θ应取得适当小,选择θ＝1。由算法步骤在机械臂运行时实时估计机械臂末端受到外力。在噪声方差为0.1时，外力估计效果如图3所示。

本发明在外力恒定和外力快速变化阶段都能较精确地估计机械臂末端执行器受到的外力,而且能有效抑制噪声干扰对力估计精度的影响。在实际应用中，本发明不依赖机械臂关节加速度信号，由关节驱动器力矩信号估计机械臂末端执行器受到的外力，可替代昂贵的力传感器；可降低机械臂力控制成本，在机器人打磨、抛光、装配和雕刻等加工领域具有应用前景。

Claims (3)

1.一种基于H_∞滤波的机械臂外力估计方法，其特征在于，具体步骤如下：

(1)由模拟低通滤波器对动力学方程中信号进行滤波，推导出不显含加速度的机械臂动力学模型；

串联机械臂的动力学方程的一般形式如下式:

$\mathrm{\&tau;}=M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)+{\mathrm{\&tau;}}_d-J{\left(q\right)}^T{F}_{ext}---\left(1\right)$

q是关节角度的矢量,M(q)为机械臂的惯量矩阵,是离心力和哥氏力矢量,G(q)是重力矢量,τ_d表示未知的扰动力,J(q)是机械臂的雅克比矩阵,F_ext代表施加在机械臂末端的外力，τ是关节驱动器的输出力矩，一般是由测量电机电流计算得到，所以夹杂着噪声；

以下都以s表示拉普拉斯变换复变量，z表示Z变换复变量，选用的模拟低通滤波器的传递函数为

$F\left(s\right)=\frac{{\mathrm{\&omega;}}_c}{s+{\mathrm{\&omega;}}_c}---\left(2\right)$

ω_c是模拟低通滤波器的3dB截止频率，冲激响应为

$f\left(t\right)=L^{-1}\left\{F\left(s\right)\right\}={\mathrm{\&omega;}}_c{e}^{-{\mathrm{\&omega;}}_{c}t}---\left(3\right)$

滤波之后的(1)式如下所示

${\&Integral;}_0^{t}f(t-r)\{M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)+{\mathrm{\&tau;}}_d-J{\left(q\right)}^T{F}_{ext}\}dr={\&Integral;}_0^{t}f(t-r)\mathrm{\&tau;}\left(r\right)dr---\left(4\right)$

上式中r代表时间积分变量,由分部积分公式

∫udv＝uv-∫vdu

设u＝f(t-r)M(q),可得

$\begin{array}{c}{\&Integral;}_0^{t}f(t-r)\left\{M\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}\right\}dr=f\left(0\right)M\left(q\right)\stackrel{\&CenterDot;}{q}-f\left(t\right)M\left(q\right(0\left)\right)\stackrel{\&CenterDot;}{q}\left(0\right)\\ -{\&Integral;}_0^t\{f(t-r)\stackrel{\&CenterDot;}{M}\left(q\right)\stackrel{\&CenterDot;}{q}-\stackrel{\&CenterDot;}{f}(t-r)M\left(q\right)\stackrel{\&CenterDot;}{q}\}dr\end{array}---\left(5\right)$

假设机械臂的初始状态是静止的,即而f(0)＝ω_c,将(5)代入(4),可得

$\begin{array}{c}{\&Integral;}_0^{t}f(t-r)\{C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)+{\mathrm{\&tau;}}_d-J{\left(q\right)}^T{F}_{ext}-\stackrel{\&CenterDot;}{M}\left(q\right)\stackrel{\&CenterDot;}{q}\}dr\\ +{\mathrm{\&omega;}}_{c}M\left(q\right)\stackrel{\&CenterDot;}{q}+{\&Integral;}_0^t\stackrel{\&CenterDot;}{f}(t-r)M\left(q\right)\stackrel{\&CenterDot;}{q}dr={\&Integral;}_0^{t}f(t-r)\mathrm{\&tau;}\left(r\right)dr\end{array}---\left(6\right)$

(6)式中出现了新的卷积

$F_2\left(s\right)=L\left\{\stackrel{\&CenterDot;}{f}\left(t\right)\right\}=L\{-{{\mathrm{\&omega;}}_c}^2{e}^{-{\mathrm{\&omega;}}_{c}t}\}=-\frac{{{\mathrm{\&omega;}}_c}^2}{{\mathrm{\&omega;}}_c+s}$

令

$M\left(q\right)\stackrel{\&CenterDot;}{q}=H_1(q,\stackrel{\&CenterDot;}{q})$

$\stackrel{\&CenterDot;}{M}\left(q\right)\stackrel{\&CenterDot;}{q}=H_2(q,\stackrel{\&CenterDot;}{q})$

$C(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+G\left(q\right)=H_3(q,\stackrel{\&CenterDot;}{q})$

假设我们用符号<x(t)>_F(s)表示信号x(t)经过传递函数为F(s)的滤波器滤波之后的结果，则由(6)可导出不显含加速度的机械臂动力学模型：

$\begin{array}{c}{\mathrm{\&omega;}}_c{H}_1(q,\stackrel{\&CenterDot;}{q})+<H_3(q,\stackrel{\&CenterDot;}{q})-H_2(q,\stackrel{\&CenterDot;}{q}){>}_{F\left(s\right)}+<{\mathrm{\&tau;}}_d{>}_{F\left(s\right)}\\ -<J{\left(q\right)}^T{F}_{ext}{>}_{F\left(x\right)}+<H_1(q,\stackrel{\&CenterDot;}{q}){>}_{F_2\left(s\right)}=<\mathrm{\&tau;}{>}_{F\left(s\right)}\end{array}$

(2)用双线性变换方法由模拟低通滤波器设计IIR数字滤波器；

双线性变换实现从s平面到z平面的映射为

$s=\frac{2}{T}\left(\frac{1-z^{-1}}{1+z^{-1}}\right)$

T表示采样周期，则传递函数为F(s)的滤波器经过双线性变换后的系统函数为

$F\left(z\right)=\frac{\frac{{\mathrm{\&omega;}}_{c}T}{2+{\mathrm{\&omega;}}_{c}T}(1+z^{-1})}{1+\frac{{\mathrm{\&omega;}}_{c}T-2}{{\mathrm{\&omega;}}_{c}T+2}{z}^{-1}}$

同理，

$F_2\left(z\right)=-{\mathrm{\&omega;}}_{c}F\left(z\right)=\frac{\frac{-{{\mathrm{\&omega;}}_c}^{2}T}{2+{\mathrm{\&omega;}}_{c}T}(1+z^{-1})}{1+\frac{{\mathrm{\&omega;}}_{c}T-2}{{\mathrm{\&omega;}}_{c}T+2}{z}^{-1}}$

设期望的数字滤波器截止频率Ω_c,由s域的频率变量ω和z域的频率变量Ω的关系

${\mathrm{\&omega;}}_c=\frac{2}{T}tan\frac{{\mathrm{\&Omega;}}_c}{2}$

计算得到参数ω_c，得到IIR滤波器的系统函数F(z)，使设计的数字滤波器的截止频率可以映射到Ω_c；用设计出的IIR数字滤波器对动力学方程信号进行滤波，得

${\mathrm{\&omega;}}_c{H}_1(q,\stackrel{\&CenterDot;}{q})+<H_3(q,\stackrel{\&CenterDot;}{q})-H_2(q,\stackrel{\&CenterDot;}{q}){>}_{F\left(z\right)}+<H_1(q,\stackrel{\&CenterDot;}{q}){>}_{F_2\left(z\right)}$

$-<\mathrm{\&tau;}{>}_{F\left(z\right)}=<J{\left(q\right)}^T{F}_{ext}{>}_{F\left(z\right)}-<{\mathrm{\&tau;}}_d{>}_{F\left(z\right)}$

如果外力F_ext变化相对平缓,上式可以写成

${\mathrm{\&omega;}}_c{H}_1(q,\stackrel{\&CenterDot;}{q})+<H_3(q,\stackrel{\&CenterDot;}{q})-H_2(q,\stackrel{\&CenterDot;}{q}){>}_{F\left(z\right)}+<H_1(q,\stackrel{\&CenterDot;}{q}){>}_{F_2\left(z\right)}$

$-<\mathrm{\&tau;}{>}_{F\left(z\right)}=<J{\left(q\right)}^T{>}_{F\left(z\right)}{F}_{ext}-<{\mathrm{\&tau;}}_d{>}_{F\left(z\right)}$

令

${\mathrm{\&omega;}}_c{H}_1(q,\stackrel{\&CenterDot;}{q})+<H_3(q,\stackrel{\&CenterDot;}{q})-H_2(q,\stackrel{\&CenterDot;}{q}){>}_{F\left(z\right)}+<H_1(q,\stackrel{\&CenterDot;}{q}){>}_{F_2\left(z\right)}-<\mathrm{\&tau;}{>}_{F\left(z\right)}=y_k$

<J(q)^T>_F(z)＝H_k

-<τ_d>_F(z)＝v_k

k＝0,1,2...，代表各个时刻的离散值，可以得到

$y_k=H_k{F}_{{\mathrm{ext}}_k}+v_k$

加入外力随机游动情况下，外力F_ext的状态方程

$F_{{\mathrm{ext}}_{k+1}}=F_{{\mathrm{ext}}_k}+w_k$

可得状态空间方程

$\left\{\begin{array}{c}{F}_{{\mathrm{ext}}_{k+1}}=F_{{\mathrm{ext}}_k}+w_k\\ y_k=H_k{F}_{{\mathrm{ext}}_k}+v_k\end{array}\right.$

其中，w_k,v_k分别为状态空间方程的系统噪声和量测噪声；

(3)基于建立的状态空间方程，采用H_∞滤波方法估计状态变量F_ext,选择估计变量

$z_k=L_k{F}_{{\mathrm{ext}}_k}$

其中

$L_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

H_∞滤波的代价函数为

$J_1=\frac{{\mathrm{\&Sigma;}}_{k=0}^{N-1}\left|\right|z_k-{\hat{z}}_k|{|}_{S_k}^2}{\left|\right|F_{{\mathrm{ext}}_0}-{\hat{F}}_{{\mathrm{ext}}_0}|{|}_{{P_0}^{-1}}^2+{\mathrm{\&Sigma;}}_{k=0}^{N-1}\left(\right|\left|w_k\right|{|}_{Q_k^{-1}}^2+\left|\right|v_k\left|{|}_{R_k^{-1}}^2\right)}$

上式中P₀，Q_k和R_k分别是初始估计误差、系统噪声w_k和量测噪声v_k的权重矩阵，z_k和，表示变量z_k估计值，即向量的加权内积,S_k为对k时刻状态向量的权重矩阵,其余加权内积说明同上,设代价函数的上界为即

算法步骤为

${\stackrel{\&OverBar;}{S}}_k=L_k^T{S}_k{L}_k$

$K_k=P_k{\&lsqb;I-\mathrm{\&theta;}{\stackrel{\&OverBar;}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k\&rsqb;}^{-1}{H}_k^T{R}_k^{-1}$

${\hat{F}}_{{\mathrm{ext}}_{k+1}}={\hat{F}}_{{\mathrm{ext}}_k}+K_k(y_k-H_k{\hat{F}}_{{\mathrm{ext}}_k})$

$P_{k+1}=P_k{\&lsqb;I-\mathrm{\&theta;}{\stackrel{\&OverBar;}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k\&rsqb;}^{-1}+Q_k$

其中：在每个时刻k必须满足条件

$P_k^{-1}-\mathrm{\&theta;}{\stackrel{\&OverBar;}{S}}_k+H_k^T{R}_k^{-1}{H}_k>0$

设计H_∞滤波中初始估计误差,系统噪声w_k和量测噪声v_k的权重矩阵分别为P₀,Q_k和R_k，使代价函数的界1/θ满足条件，1/θ是代价函数的界。

2.根据权利要求1所述的方法，其特征在于：步骤(2)中，由模拟滤波器的传递函数F(s)通过双线性变换设计IIR滤波器F(z)，由s域的频率变量ω和z域的频率变量Ω的关系，设定期望的数字滤波器截止频率Ω_c得到参数ω_c，使IIR滤波器的截止频率映射到期望截止频率。

3.根据权利要求1所述的方法，其特征在于：步骤(3)中，将经过数字滤波器的动力学模型看作观测方程，加入外力变化的状态方程，基于状态空间方程设计H_∞滤波器,实时估计外力。