CN106385211A - 一种步进电机负载转矩估计方法 - Google Patents

Abstract

本发明公开了一种混合式步进电机的负载转矩估计方法。针对步进电机控制信号不连续的特点，建立输入周期矩形波电压的步进电机模型，并且以步进电机负载转矩作为该模型的状态变量之一；用扩展卡尔曼滤波器迭代计算出步进电机负载转矩估计值，解决了用物理传感器直接测量负载转矩成本、故障高和安装复杂等问题，为步进电机负载转矩测量提供了一种新方法；针对扩展卡尔曼滤波器中系统噪声协方差矩阵和测量噪声协方差矩阵难以选取的问题，将粒子群算法引入扩展卡尔曼滤波器中，对系统噪声矩阵和测量噪声矩阵用粒子群算法进行在线调整和优化，使系统噪声和测量噪声更加逼近真实噪声，从而获得更加精确的步进电机负载转矩估计值。

Description

一种步进电机负载转矩估计方法

技术领域

本发明涉及电机控制领域，特别涉及一种混合式步进电机的负载转矩估计方法。

背景技术

电机负载转矩大小可通过物理传感器测量或是软件算法估计而得到。负载转矩通过物理传感器直接测量成本较高，并且受到仪器精度影响较大，在一些场合容易收到安装条件的限制，以及有维护不方便等方面的不足。而通过软件算法来估计出负载转矩的大小，具有降低成本，便于电机安装等优点。因此利用软件算法来实现对负载转矩的估计的虚拟传感器研究成为了现代传动控制技术的重要研究方向，卡尔曼滤波器是估计负载转矩的一种重要方法。

利用扩展卡尔曼滤波器进行负载转矩估计存在的主要问题之一是噪声矩阵的选取，负载转矩估计的精度很大程度上取决于使用者对噪声的先验了解，所以对系统噪声矩阵和测量噪声矩阵的合理配置可以改善负载转矩估计精度。而对于噪声矩阵的选取，可以使用粒子群算法对噪声矩阵进行优化。利用粒子群算法的全局最优解对噪声矩阵进行优化，从而提高扩展卡尔曼滤波器的精度和总体性能，使得优化后的扩展卡尔曼滤波器对步进电机的负载转矩估计更加准确。

发明内容

本发明目的在于提供一种混合式步进电机负载转矩的估计方法，用虚拟力矩传感器对步进电机负载转矩进行测量。另外，在扩展卡尔曼滤波器进行力矩估计的过程中，针对初始噪声矩阵的不确定性，引入粒子群算法对噪声矩阵进行优化，提高扩展卡尔曼滤波器的估计精度。

本发明为解决上述技术问题所采用的技术方案是：利用扩展卡尔曼滤波器进行步进电机负载转矩估计时，利用粒子群算法对系统噪声矩阵和测量噪声矩阵进行优化，得到的优化值作为卡尔曼滤波器的输入参数进行负载转矩估计。本发明所述的步进电机负载转矩的估计方法主要包括以下步骤：

步骤1：建立两相混合式步进电机数学模型，由于u_a，u_b分别为A、B相电压，是周期性矩形波信号，其单个周期2τ内表达式为：

$u_a\left(t\right)=\left\{\begin{array}{cc}E& \left(\right|t|\&le;\frac{\mathrm{\&tau;}}{2})\\ 0& \left(\right|t|>\frac{\mathrm{\&tau;}}{2})\end{array}\right.,u_b=\left\{\begin{array}{cc}E& \left(\right|t|\&GreaterEqual;\frac{\mathrm{\&tau;}}{2})\\ 0& \left(\right|t|<\frac{\mathrm{\&tau;}}{2})\end{array}\right.$

其中，E为相电压幅值，τ为矩形波宽度，A、B两相电压各自周期为2τ。

混合式步进电机沿时间轴宏观表现为在A、B两相交替施加端电压，在定子线圈上产生磁极，和转子磁极互相吸引，进而带动步进电机转子旋转，通过改变电机相电压频率而改变步进电机转速；在微观上，即在某相导通期间，转子在通电定子线圈产生的磁极吸引下旋转，根据电磁学原理，可得定子绕组电压方程，进而得到步进电机电磁学方程。再根据电机动力学方程，将步进电机相电流i_a，i_b，转子角速度ω，转子位置θ作为状态变量，构造两相步进电机的数学模型(1)

$\left\{\begin{array}{c}\frac{{\mathrm{di}}_a}{dt}=-\frac{R}{L}{i}_a+\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_a}{L}\\ \frac{{\mathrm{di}}_b}{dt}=-\frac{R}{L}{i}_b-\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_b}{L}\\ \frac{d\mathrm{\&omega;}}{dt}=-\frac{K_m}{J}{i}_{a}sin\left(N\mathrm{\&theta;}\right)+\frac{K_m}{J}{i}_b\cos \left(N\mathrm{\&theta;}\right)-\frac{K_v}{J}\mathrm{\&omega;}-\frac{T_l}{J}\\ \frac{d\mathrm{\&theta;}}{dt}=\mathrm{\&omega;}\end{array}\right.---\left(1\right)$

式中，i_a，i_b分别为两相步进电机A、B相电流；u_a，u_b分别为A、B相电压；R为相电阻，L为相电感；K_m为电机转矩常数；K_v为粘滞摩擦系数；J为转动惯量；T_l为负载转矩；N为转子齿数。

将模型(1)进行d-q坐标变换得模型(2)

$\left\{\begin{array}{c}\frac{{\mathrm{di}}_d}{dt}=-\frac{R}{L}{i}_d+{\mathrm{N\&omega;i}}_q+\frac{u_d}{L}\\ \frac{{\mathrm{di}}_q}{dt}=-\frac{R}{L}{i}_q-{\mathrm{N\&omega;i}}_d-\frac{K_m}{L}\mathrm{\&omega;}+\frac{u_q}{L}\\ \frac{d\mathrm{\&omega;}}{dt}=-\frac{K_m}{J}{i}_q-\frac{K_v}{J}\mathrm{\&omega;}-\frac{T_l}{J}\\ \frac{d\mathrm{\&theta;}}{dt}=\mathrm{\&omega;}\end{array}\right.---\left(2\right)$

式中i_d，i_q为分别为变换后d，q轴电流，u_d，u_q分别为变换后d，q轴电压。由模型(2)可得以下模型(3)形式。

$\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=f(x,u)\\ y=h\left(x\right)\end{array}---\left(3\right)$

式中，x＝[i_d i_q ω θ]^T，y＝[i_d i_q]^T

由模型(3)得离散化模型

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\&Gamma;}\&CenterDot;f(x_k,u_k)+w_k\\ y_k={\mathrm{hx}}_k+v_k\end{array}\right.---\left(4\right)$

在采样时间内，近似认为负载转矩不变，有负载转矩方程

T_l(k+1)＝T_lk (5)

带入离散化模型，组成五阶离散化模型，其中

步骤2：在粒子群算法过程中，卡尔曼滤波器的噪声矩阵Q和R的每组参数对应一个粒子。在最初的时候需要对噪声矩阵Q和R进行初始化，在算法开始时需要随机初始化粒子的速度向量和位置向量。由于电机模型中有五个状态变量和两个输出变量，所以分别取系统噪声矩阵Q＝diag(Q₁₁ Q₂₂ Q₃₃ Q₄₄ Q₅₅)，测量噪声矩阵R＝diag(R₁₁R₂₂)。为方便计算，令f(Q,R)＝[Q₁₁ Q₂₂ Q₃₃ Q₄₄ Q₅₅ R₁₁ R₂₂]^T。

判断粒子的适应度函数为

$fitness(Q,R)=\frac{1}{k}\underset{i=1}{\overset{k}{\&Sigma;}}{({\stackrel{\&OverBar;}{T}}_{li}-T_{li})}^2---\left(6\right)$

其中，T_li分别对应于时刻i的滤波器输出理想负载转矩值和估计负载转矩值，k对应于仿真时间与采样周期的比值。

步骤3：根据下列公式(7)公式(8)更新粒子的速度和位置：

v_id(t+1)＝wv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (7)

x_id(t+1)＝x_id(t)+v_id(t+1) (8)

式中，p_id为每个粒子自己搜索到的最优解；p_gd为整个粒子群搜索到的最优解；v_id表示第i个粒子在第d维上的速度；x_id表示第i个粒子在第d维上的位置；w为惯性权重；r₁，r₂为分布在[0,1]之间的随机数；c₁，c₂为学习因子，通常c₁＝c₂＝2；粒子更新速度和位置的过程为：每个粒子的适应度值分别和个体极值p_id和全局极值p_gd进行比较，若较好则用适应度值替换个体极值或全局极值。这样粒子通过不断学习更新，最终到达最优解所在位置，整个搜索过程结束。最后输出的p_gd就是全局最优解。

步骤4：在每个采样周期内，运用扩展卡尔曼滤波算法进行迭代计算，将步骤3得到的优化过后的Q和R作为带入扩展卡尔曼滤波算法进行负载转矩估计。迭代过程如下，状态预测公式为：

${\hat{x}}_{k+1|k}={\hat{x}}_{k|k}+\mathrm{\&Gamma;}\&CenterDot;f({\hat{x}}_{k|k},u_k)---\left(9\right)$

协方差矩阵估计公式为：

$P_{k+1|k}={\mathrm{Fd}}_k\&CenterDot;P_{k|k}\&CenterDot;{{\mathrm{Fd}}_k}^T+Q_k---\left(10\right)$

其中，

卡尔曼增益公式为：

K_k+1＝P_k+1|k·H_k ^T·(H_k·P_k+1|k·H_k ^T+R_k)^-1 (12)

其中，

状态更新公式为：

${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K_{k+1}(y_{k+1}-H_k\&CenterDot;{\hat{x}}_{k+1|k})---\left(14\right)$

协方差更新公式为：

P_k+1|k+1＝P_k+1|k-K_k+1·H_k·P_k+1|k (15)

根据式(9)至式(15)和递推初值及优化后的噪声矩阵，可以进行扩展卡尔曼滤波迭代计算，从而估计出负载转矩。

本发明一种步进电机负载转矩的估计方法有益效果在于：针对电机负载转矩用物理传感器测量成本高，安装复杂等问题，提出一种用扩展卡尔曼滤波算法估计负载转矩的方法，可以有效降低成本，并且提高适用性。另外针对影响卡尔曼滤波算法的噪声矩阵问题，加入了对噪声矩阵优化的算法，利用粒子群算法对噪声矩阵进行优化，从而提高扩展卡尔曼滤波器的精度，进而提高基于扩展卡尔曼滤波算法的步进电机负载转矩估计方法的总体性能。

附图说明

图1是基于粒子群算法优化的扩展卡尔曼滤波器结构图；

图2是粒子群算法的算法流程图；

具体实施方式

以下将结合附图和实施例对本发明做进一步详细说明。

本发明所涉及的一种步进电机负载转矩的估计方法主要是由扩展卡尔曼滤波器，目标函数以及粒子群算法三部分组成，其结构图如图1所示。扩展卡尔曼滤波器根据系统各时刻的测量值y_k，输入值u_k及选定的滤波参数值，通过更新迭代得到系统状态的后验估计根据各时刻当前的目标函数值和滤波参数值，使用粒子群算法对滤波参数Q和R进行寻优，将修正值作为扩展卡尔曼滤波器的输入参数进行参数估计，直至得到优化的状态估计值

建立两相混合式步进电机数学模型，由于u_a，u_b分别为A、B相电压，是周期性矩形波信号，其单个周期2τ内表达式为：

$u_a\left(t\right)=\left\{\begin{array}{cc}E& \left(\right|t|\&le;\frac{\mathrm{\&tau;}}{2})\\ 0& \left(\right|t|>\frac{\mathrm{\&tau;}}{2})\end{array}\right.,u_b=\left\{\begin{array}{cc}E& \left(\right|t|\&GreaterEqual;\frac{\mathrm{\&tau;}}{2})\\ 0& \left(\right|t|<\frac{\mathrm{\&tau;}}{2})\end{array}\right.,$

其中，E为相电压幅值，τ为矩形波宽度，A、B两相电压各自周期为2τ。

步进电机沿时间轴宏观表现为给A、B两相交替施加端电压，在定子线圈上产生磁极，和转子磁极互相吸引，进而带动步进电机转子旋转，通过改变电机相电压频率而改变步进电机转速；在微观上，即在某相导通期间，转子在通电定子线圈产生的磁极吸引下旋转，根据电磁学原理，可得定子绕组电压方程，进而得到步进电机电磁学方程。再根据电机动力学方程，将步进电机相电流i_a，i_b，转子角速度ω，转子位置θ作为状态变量，构造两相步进电机的数学模型(1)

$\left\{\begin{array}{c}\frac{{\mathrm{di}}_a}{dt}=-\frac{R}{L}{i}_a+\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_a}{L}\\ \frac{{\mathrm{di}}_b}{dt}=-\frac{R}{L}{i}_b-\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_b}{L}\\ \frac{d\mathrm{\&omega;}}{dt}=-\frac{K_m}{J}{i}_{a}sin\left(N\mathrm{\&theta;}\right)+\frac{K_m}{J}{i}_b\cos \left(N\mathrm{\&theta;}\right)-\frac{K_v}{J}\mathrm{\&omega;}-\frac{T_l}{J}\\ \frac{d\mathrm{\&theta;}}{dt}=\mathrm{\&omega;}\end{array}\right.---\left(1\right)$

式中，i_a，i_b分别为两相步进电机A、B相电流；u_a，u_b分别为A、B相电压；R为相电阻，L为相电感；K_m为电机转矩常数；K_v为粘滞摩擦系数；J为转动惯量；T_l为负载转矩；N为转子齿数。

将模型(1)进行d-q坐标变换得模型(2)，其中变换矩阵为

$\left\{\begin{array}{c}\frac{{\mathrm{di}}_d}{dt}=-\frac{R}{L}{i}_d+{\mathrm{N\&omega;i}}_q+\frac{u_d}{L}\\ \frac{{\mathrm{di}}_q}{dt}=-\frac{R}{L}{i}_q-{\mathrm{N\&omega;i}}_d-\frac{K_m}{L}\mathrm{\&omega;}+\frac{u_q}{L}\\ \frac{d\mathrm{\&omega;}}{dt}=-\frac{K_m}{J}{i}_q-\frac{K_v}{J}\mathrm{\&omega;}-\frac{T_l}{J}\\ \frac{d\mathrm{\&theta;}}{dt}=\mathrm{\&omega;}\end{array}\right.---\left(2\right)$

式中i_d，i_q为分别为变换后d，q轴电流，u_d，u_q分别为变化后d，q轴电压，ω为转子角速度，θ为转子位置，N为转子齿数。由模型(2)可得以下模型(3)形式。

$\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=f(x,u)\\ y=h\left(x\right)\end{array}---\left(3\right)$

式中，x＝[i_d i_q ω θ]^T，y＝[i_d i_q]^T

由模型(3)得离散化模型

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\&Gamma;}\&CenterDot;f(x_k,u_k)+w_k\\ y_k={\mathrm{hx}}_k+v_k\end{array}\right.---\left(4\right)$

其中Γ为采样时间，w_k，v_k分别为系统噪声和测量噪声，他们对应的协方差矩阵分别为Q和R。在采样时间内，近似认为负载转矩不变，有

T_l(k+1)＝T_lk (5)

带入离散化模型，组成五阶离散化模型，其中

卡尔曼滤波器的噪声矩阵Q和R的每组参数对应一个粒子。在最初的时候需要对噪声矩阵Q和R进行初始化，在算法开始时需要随机初始化粒子的速度向量和位置向量。由于电机模型中有五个状态变量和两个输出变量，所以分别取系统噪声矩阵Q＝diag(Q₁₁ Q₂₂Q₃₃ Q₄₄ Q₅₅)，测量噪声矩阵R＝diag(R₁₁R₂₂)。为方便计算，令f(Q,R)＝[Q₁₁ Q₂₂ Q₃₃ Q₄₄ Q₅₅R₁₁ R₂₂]^T。

判断粒子的适应度函数为：

$fitness(Q,R)=\frac{1}{k}\underset{i=1}{\overset{k}{\&Sigma;}}{({\stackrel{\&OverBar;}{T}}_{li}-T_{li})}^2---\left(6\right)$

其中，T_li分别对应于时刻i的滤波器输出理想负载转矩值和估计负载转矩值，k对应于仿真时间与采样周期的比值。

根据下列公式(7)公式(8)更新粒子的速度和位置：

v_id(t+1)＝wv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (7)

x_id(t+1)＝x_id(t)+v_id(t+1) (8)

式中，p_id为每个粒子自己搜索到的最优解；p_gd为整个粒子群搜索到的最优解；v_id表示第i个粒子在第d维上的速度；x_id表示第i个粒子在第d维上的位置；_w为惯性权重；r₁，r₂为分布在[0,1]之间的随机数；c₁，c₂为学习因子，通常c₁＝c₂＝2；粒子更新速度和位置的过程为：每个粒子的适应度值分别和个体极值p_id和全局极值p_gd进行比较，若较好则用适应度值替换个体极值或全局极值。这样粒子通过不断学习更新，最终到达最优解所在位置，整个搜索过程结束。最后输出的p_gd就是全局最优解。

每个采样周期内，扩展卡尔曼滤波器进行迭代过程，得到修正值Q和R作为扩展卡尔曼滤波器的输入参数进行负载转矩估计。状态预测公式为：

${\hat{x}}_{k+1|k}={\hat{x}}_{k|k}+\mathrm{\&Gamma;}\&CenterDot;f({\hat{x}}_{k|k},u_k)---\left(9\right)$

协方差矩阵计算公式为

P_k+1|k＝Fd_k·P_k|k·Fd_k ^T+Q_k (10)

式中：

本实施例中有

卡尔曼增益公式为：

K_k+1＝P_k+1|k·H_k ^T·(H_k·P_k+1|k·H_k ^T+R_k)^-1 (12)

式中：

本实施例中有

状态更新公式为：

${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K_{k+1}(y_{k+1}-H_k\&CenterDot;{\hat{x}}_{k+1|k})---\left(14\right)$

协方差矩阵更新公式为：

P_k+1|k+1＝P_k+1|k-K_k+1·H_k·P_k+1|k (15)

根据式(9)到式(15)和递推初值及更新的Q和R，就可以进行扩展卡尔曼滤波迭代运算，从而估计出步进电机的负载转矩。

以上内容是结合具体实施例对本发明所作的进一步详细说明，不能认定本发明具体实施只局限于这些说明。对于本发明所属技术领域的人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明由所提交的权利要求书确定的专利保护范围。

Claims (3)

1.一种混合式步进电机负载转矩估计方法，其特征在于，具体包括以下步骤：

步骤1：建立静态坐标系下步进电机数学模型，包括系统电磁学方程和动力学方程，其中电磁学方程中A、B两相电压为周期性矩形波信号；运用坐标变换获得在d-q坐标系下的步进电机模型并将模型离散化；将上述模型中d、q轴电流i_d、i_q，转子速度ω，转子位置θ作为状态变量，d、q轴电压u_d、u_q为不连续电压信号作为系统输入；增加负载转矩方程，并将负载转矩T_l作为状态变量，构造含有负载转矩为状态变量的步进电机离散模型；

步骤2：初始化系统参数，包括步进电机状态x(0)，扩展卡尔曼滤波器中协方差P(0)、噪声矩阵Q和R，其中Q和R的每组参数对应粒子群算法中的一个粒子，随机初始化粒子的速度向量、位置向量，计算适应度函数；

步骤3：引入粒子群算法，更新粒子的速度和位置、个体极值点、全局极值点，进行粒子群算法的递归迭代，得到最优解；

步骤4：根据扩展卡尔曼滤波算法，将优化后的噪声矩阵Q和R作为扩展卡尔曼滤波器的输入参数，经过扩展卡尔曼滤波器迭代计算，估计出步进电机负载转矩。

2.根据权利要求1所述的基于粒子群优化扩展卡尔曼滤波器的混合式步进电机负载转矩估计方法中，其特征在于：

所述步骤1中，A、B两相电压为周期矩形波电压，其单个周期2τ内表达式有：

$u_a\left(t\right)=\left\{\begin{array}{cc}E& \left(\left|t\right|\&le;\frac{\mathrm{\&tau;}}{2}\right)\\ 0& \left(\left|t\right|>\frac{\mathrm{\&tau;}}{2}\right)\end{array}\right.,u_b\left(t\right)=\left\{\begin{array}{cc}E& \left(\right|t|\&GreaterEqual;\frac{\mathrm{\&tau;}}{2})\\ 0& \left(\right|t|<\frac{\mathrm{\&tau;}}{2})\end{array}\right.$

其中：u_a、u_b分别是A、B两相电压，E为相电压幅值，τ为矩形波宽度。A、B两相电压各自周期为2τ。

混合式步进电机沿时间轴宏观表现为给A、B两相交替施加端电压，在定子线圈上产生磁极，和转子磁极互相吸引，进而带动步进电机转子旋转，通过改变电机相电压频率而改变步进电机转速。在微观上，即在某相导通期间，可得定子绕组电压方程，进而得到步进电机电磁学方程。再根据电机动力学方程，得到步进电机模型：

$\left\{\begin{array}{c}\frac{{\mathrm{di}}_a}{dt}=-\frac{R}{L}{i}_a+\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_a}{L}\\ \frac{{\mathrm{di}}_b}{dt}=-\frac{R}{L}{i}_b-\frac{K_m}{L}\&CenterDot;\mathrm{\&omega;}sin\left(N\mathrm{\&theta;}\right)+\frac{u_b}{L}\\ \frac{d\mathrm{\&omega;}}{dt}=-\frac{K_m}{J}{i}_{a}sin\left(N\mathrm{\&theta;}\right)+\frac{K_m}{J}{i}_b\cos \left(N\mathrm{\&theta;}\right)-\frac{K_v}{J}\mathrm{\&omega;}-\frac{T_l}{J}\\ \frac{d\mathrm{\&theta;}}{dt}=\mathrm{\&omega;}\end{array}\right.$

式中，i_a，i_b分别为两相步进电机A、B相电流；u_a，u_b分别为A、B相电压；R为相电阻，L为相电感；K_m为电机转矩常数；K_v为粘滞摩擦系数；J为转动惯量；T_l为负载转矩；N为转子齿数；ω为转子角速度；θ为转子位置。

3.根据权利要求2所述的基于粒子群优化的扩展卡尔曼滤波器的混合式步进电机负载转矩估计方法中，其特征在于：

所述步骤1中，含有负载转矩为状态变量的步进电机离散模型：

$\left\{\begin{array}{c}{x}_{k+1}=x_k+\mathrm{\&Gamma;}\&CenterDot;f\left(x_k,u_k\right)+w_k\\ y_k={\mathrm{hx}}_k+v_k\end{array}\right.---\left(1\right)$

其中：Γ为采样时间。w_k，v_k分别为系统噪声和测量噪声，其对应协方差矩阵分别为Q，R。系统输入为u_d，u_q，由相电压经d-q变换后而来。式中，i_d，i_q分别对应于直轴和交轴电流，ω对应于转子角速度，θ对应于转子位置，T_l对应于电机负载转矩；

所述步骤2中，由于步进电机离散状态方程中有五个状态变量和两个输出变量，所以分别取Q＝diag(Q₁₁Q₂₂Q₃₃Q₄₄Q₅₅)，R＝diag(R₁₁R₂₂)，适应度函数为：

$fitness(Q,R)=\frac{1}{k}\underset{i=1}{\overset{k}{\&Sigma;}}{({\stackrel{\&OverBar;}{T}}_{li}-T_{li})}^2---\left(2\right)$

其中，T_li分别对应于时刻i的滤波器输出理想负载转矩值和估计负载转矩值，k对应于仿真时间与采样周期的比值；

所述步骤3中，目标函数为适应度函数fitness(Q,R)，每个粒子对应于一组Q和R的参数，根据式(3)和(4)更新粒子的速度和位置：

v_id(t+1)＝wv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (3)

x_id(t+1)＝x_id(t)+v_id(t+1) (4)

式中，p_id为每个粒子自己搜索到的最优解；p_gd为整个粒子群搜索到的最优解；v_id表示第i个粒子在第d维上的速度；x_id表示第i个粒子在第d维上的位置；w为惯性权重；r₁，r₂为分布在[0,1]之间的随机数；c₁，c₂为学习因子,通常c₁＝c₂＝2；粒子更新速度和位置的过程为：每个粒子的适应度值分别和个体极值p_id和全局极值p_gd进行比较，若较好则用适应度值替换个体极值或全局极值。这样粒子通过不断学习更新，最终到达最优解所在位置，整个搜索过程结束。最后输出的p_gd就是全局最优解。

所述步骤4中，扩展卡尔曼滤波器进行迭代过程，状态预测公式为：

${\hat{x}}_{k+1|k}={\hat{x}}_{k|k}+\mathrm{\&Gamma;}\&CenterDot;f({\hat{x}}_{k|k},u_k)---\left(5\right)$

协方差矩阵计算公式为

P_k+1|k＝Fd_k·P_k|k·Fd_k ^T+Q_k (6)

式中：

卡尔曼增益公式为：

K_k+1＝P_k+1|k·H_k ^T·(H_k·P_k+1|k·H_k ^T+R_k)^-1 (8)

式中：

状态更新公式为：

${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K_{k+1}(y_{k+1}-H_k\&CenterDot;{\hat{x}}_{k+1|k})---\left(10\right)$

协方差矩阵更新公式为：

P_k+1|k+1＝P_k+1|k-K_k+1·H_k·P_k+1|k (11)

根据式(5)到式(11)和递推初值及更新的Q和R，就可以进行扩展卡尔曼滤波迭代运算，从而估计出步进电机的负载转矩。