CN101930216B - 基于主从参考模型的遥操作机器人自适应控制方法 - Google Patents

Abstract

一种基于主从参考模型的遥操作机器人自适应控制方法，包括：主边回路，从边回路以及通讯时延环节，主边回路由操作者、主机器人和主边环境模型构成，从边回路由环境、从机器人、从边环境模型、模型参数修正模块和模拟时延模块构成。从边环境模型提供参考力信号，与环境反馈的力信号输入模型参数修正模块进行比较，利用其误差，输出可调增益p_c(τ)，p_c(τ)从模型参数修正模块中输出，经过通讯时延环节后调节主边环境模型，由主边环境模型为操作者提供反馈力信号，同时，可调增益p_c(τ)经过模拟时延模块后调节从边环境模型，不断的进行循环，主边环境模型和从边环境模型就不断地逼近真实的环境模型，形成克服时延影响的遥操作，并且使系统获得稳定的控制。

Description

基于主从参考模型的遥操作机器人自适应控制方法

技术领域

本发明属于遥操作机器人控制技术领域，具体涉及用于克服时延影响的基于主从参考模型的遥操作机器人自适应控制方法。

背景技术

遥操作系统是实现空间危险环境中作业的有力手段，是人类感知和行为能力的有效延伸、拓展以至超越。由于操作者所在位置与作业位置之间的通讯时延，造成了遥操作系统的不稳定和操作性能的降低，而且时延会严重破坏系统的透明性，导致操作者做出错误的判断。

在很多情况下，我们对遥操作所要面对的环境是有一定了解的，如核电站里核材料的搬运，空间站内部的科学实验等，都是一个结构已知的环境，我们称之为结构化环境。现有的遥操作系统的大多是在结构化环境下作业的，其特点是通讯时延基本稳定、环境对象已知，如核电站中对于核原料的搬运，原料的大小、位置、路径都是已知的；再如空间无人值守环境下进行科学实验，实验的过程、步骤都是已知的。所以，我们可以根据环境的已知条件，得到较好的环境模型。

自适应控制系统是利用它的可调系统的输入信号、状态和输出变量来度量某个性能指标，然后根据实测性能指标值与给定性能指标值相比较的结果，由自适应机构修正可调系统的参数或者产生一个辅助信号，以保持系统的性能指标接近给定的性能指标。对于基于模型的自适应控制系统，其设计性能指标是以模型的形式来表示的，因此简单直观，符合工程实践，并且有着多种结构可以利用。将自适应控制技术运动的遥操作机器人系统中，为解决遥操作系统由于时延影响而造成的失稳问题提供较好的解决途径。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于主从参考模型的遥操作机器人自适应控制方法，使遥操作系统具有更好的稳定性。

本发明采用如下技术方案：

所述基于主从参考模型的遥操作机器人系统包括：主边回路，从边回路以及通讯时延环节，主边回路由操作者、主机器人和主边环境模型构成，从边回路由环境、从机器人、从边环境模型、模型参数修正模块和模拟时延模块构成，主边环境模型与从边环境模型结构一致，

设t为控制系统的时间变量，T为通讯时延环节确定的时延量，引入时间标识t_n，n＝0，1，2…，对各控制量进行说明，所述t_n的意义为：设某一时刻为t₀，则t₁为t₀之后经过了时延量T的时间值，t₂为t₁之后经过了时延量T的时间值，即为t₀之后经过了时延量2T的时间值，以此类推，t_n为t₀之后经过了时延量nT的时间值。

步骤1操作者通过主机器人发出主边位置信号x_m(t₀)，所述主边位置信号x_m(t₀)进入通讯时延环节时延1T后形成从边位置信号x_s(t₁)，

步骤2时延后的从边位置信号x_s(t₁)输入从机器人，使从机器人运动并产生环境位置信号x_e(t₁)，作用于环境，同时环境位置信号x_e(t₁)作为从边环境模型的输入量进入从边环境模型，从边环境模型根据环境位置信号x_e(t₁)产生从边模型输出力f_cs(t₁)，即：

$f_{\mathrm{cs}}\left(t\right)=p_c(t+2T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，所述环境根据环境位置信号x_e(t₁)产生环境反作用力f_e(t₁)，即：

$f_e\left(t\right)=m_e\stackrel{\·\·}{x}\left(t\right)+d_e\stackrel{\·}{x}\left(t\right)+k_{e}x\left(t\right)$

其中，m_e为环境的惯性系数，d_e为模型的阻尼系数，k_e为模型的弹性系数，且所述环境反作用力f_e(t₁)反作用于从机器人，同时环境反作用力f_e(t₁)进入到模型参数修正模块，模型参数修正模块根据从边环境模型输出力f_cs(t₁)及环境反作用力f_e(t₁)输出可调增益p_c(t₁)，可调增益p_c(t)的产生方法为：

所述从边环境模型输出力f_cs(t)作为模型参数修正模块的输出力f_cs(t)，环境反作用力f_e(t)作为环境的环境输出力，模型参数修正模块的控制律为

具体步骤如下：

Step1设定调整律γ，设γ＝0.01～2；

Step2模型参数修正模块的两个输入f_e(t)和f_cs(t)，得到广义误差e(t)，e(t)＝f_e(t)-f_cs(t)；

Step3利用

计算出p_c(t)；

Step4t→t+h，t为时间变量，h为计算步长，h的取值视具体计算条件确定，取值范围为(10^-7s～0.5s)，s为秒，返回Step2，继续循环。

令可调增益p_c(t₁)＝p_ct1，设τ为新的时间变量，相对于t₀时刻，τ的起始点为t₀+T，所述时间变量τ表征可调增益p_ct1输出之后的变化，即有p_ct1(τ)，

步骤3所述可调增益p_ct1(τ)经过通讯时延环节的1T时延后，得到p_ct1(τ-T)，主边环境模型根据p_ct1(τ-T)以及主机器人此时输出的主边位置信号x_m(t₂)，输出主边环境模型输出力f_cm(t₂)，即：

$f_{\mathrm{cm}}\left(t\right)=p_c(t+T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，所述主边环境模型输出力f_cm(t₂)最终作用于主机器人，主机器人输出力f_h(t₂)再作用于操作者，即f_h(t₂)＝f_cm(t₂)，使操作者感受到作用力，形成下一步的控制的参考，

步骤4根据主机器人的输出力f_h(t₂)在t₂时刻形成新的主边位置信号，但此时的主边位置信号与步骤3中的主边位置信号x_m(t₂)已不同，所以新的主边位置信号用x_m(t₂₊)表示，所述主边位置信号x_m(t₂₊)进入通讯时延环节时延1T后形成从边位置信号x_s(t₃)，

步骤5时延后的从边位置信号x_s(t₃)输入从机器人，使从机器人运动并产生环境位置信号x_e(t₃)，作用于环境和从边环境模型，与此同时，步骤2中所述的可调增益值p_ct1(τ)经过模拟时延模块模拟时延2T后，得到p_ct1(τ-2T)，从边环境模型根据p_ct1(τ-2T)以及从机器人此时输出的从边位置信号值x_m(t₃)，输出从边环境模型输出力f_cs(t₃)，作用于模型参数修正模块，此时可调增益值p_ct1(τ)的运行周期终止，可调增益p_ct1(τ)消失，模型参数修正模块根据环境位置信号x_e(t₃)作用于环境产生的环境反作用力f_e(t₃)以及从边环境模型输出力f_cs(t₃)生成新的可调增益p_c(t₃)，设p_c(t₃)＝p_ct3，由于前的可调增益值p_ct1(τ)消失，设τ为新的时间变量，此时相对于t₀时刻，τ的起始点为t₀+3T，所述时间变量τ表征可调增益p_ct3输出之后的变化，即有p_ct3(τ)，

步骤6所述可调增益p_ct3(τ)经过通讯时延环节的1T时延后，得到p_ct3(τ-T)，主边环境模型根据p_ct3(τ-T)以及主机器人此时输出的主边位置信号x_m(t₄)，输出主边环境模型输出力f_cm(t₄)，所述主边环境模型输出力f_cm(t₄)最终作用于主机器人，主机器人输出力f_h(t₄)再作用于操作者，即f_h(t₄)＝f_cm(t₄)，使操作者感受到作用力，形成下一步的控制的参考，

步骤7返回步骤1，

以此循环，实现遥操作机器人系统的连续控制。

本发明的有益效果在于：

1.本发明实现了遥操作机器人的双边控制。

2.本发明在遥操作机器人系统的主边进入环境模型，并且由该环境模型提供反馈的力信号，这样就避免了时延造成的失稳问题，使操作者感受到稳定的反馈力。

3.本发明在遥操作系统的主从双边引入环境模型，对于同一个位置信号，所面对的主从环境模型是一样的，这种结构的功能就是可以使操作者感受到的力信号与真实环境反馈的力信号进行比较，在调整可调增益，这样提高了遥操作机器人系统的控制准确性。

4.本发明提高了结构化环境下遥操作机器人系统的控制稳定性，能极大地拓宽遥操作机器人的应用范围。

附图说明

图1是本发明的遥操作机器人系统控制模型图。

图2是控制系统中的时间关系示意图。

图3是本发明控制系统的仿真结果图。

具体实施方式

所述基于主从参考模型的遥操作机器人自适应控制方法包括：主边回路1，从边回路2以及通讯时延环节3，主边回路1由操作者11、主机器人12和主边环境模型13构成，从边回路2由环境21、从机器人22、从边环境模型23、模型参数修正模块24和模拟时延模块25构成，主边环境模型13与从边环境模型23结构一致，

结构化环境下，已知通讯时延环节3有时延，设T为通讯时延环节3的时延量，仿真中T＝1s。在结构化环境下对所要面对的环境有一定的了解，所以可以建立较为准确的环境模型，但不可能完全一致。本发明中的主边环境模型13和从边环境模型23都是在控制开始时已建立好的近似环境模型，这两个模型建立时是一样的，控制过程中参数调整也一样，两个模型都在不断地逼近真实的环境模型，但从边环境模型23比从边环境模型在参数调整上滞后1T。

设t为控制系统的时间变量，引入时间标识t_n，n＝0，1，2…，对各控制量进行说明，所述t_n的意义为：设某一时刻为t₀，则t₁为t₀之后经过了时延量T的时间值，t₂为t₁之后经过了时延量T的时间值，即为t₀之后经过了时延量2T的时间值，以此类推，t_n为t₀之后经过了时延量nT的时间值。

具体步骤如下：

步骤1操作者11通过主机器人12发出主边位置信号x_m(t₀)，所述主边位置信号x_m(t₀)进入通讯时延环节3时延1T后形成从边位置信号x_s(t₁)，

步骤2时延后的从边位置信号x_s(t₁)输入从机器人22(令从机器人22的动力学模型为

其中f_s为从机器人22的输出力，m_s为惯性系数，d_s为阻尼系数，k_s为弹性系数，x是位置量，仿真中取m_s＝2，d_s＝0.001，k_s＝0)，使从机器人22运动并产生环境位置信号x_e(t₁)，作用于环境21，同时环境位置信号x_e(t₁)作为从边环境模型23的输入量进入从边环境模型23，从边环境模型23根据环境位置信号x_e(t₁)产生从边模型输出力f_cs(t₁)，即：

$f_{\mathrm{cs}}\left(t\right)=p_c(t+2T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，仿真中取m_c＝0.6，d_c＝0.01，k_c＝0，所述环境根据环境位置信号x_e(t₁)产生环境反作用力f_e(t₁)，即：

$f_e\left(t\right)=m_e\stackrel{\·\·}{x}\left(t\right)+d_e\stackrel{\·}{x}\left(t\right)+k_{e}x\left(t\right)$

其中，m_e为环境的惯性系数，d_e为模型的阻尼系数，k_e为模型的弹性系数，仿真中取m_e＝0.8，d_e＝0.01，k_e＝0，且所述环境反作用力f_e(t₁)反作用于从机器人22，同时环境反作用力f_e(t₁)进入到模型参数修正模块24，模型参数修正模块24根据从边环境模型输出力f_cs(t₁)及环境反作用力f_e(t₁)输出可调增益p_c(t₁)。

模型参数修正模块24中所计算p_c(t)的算法为基于梯度法的自适应律，设从边环境模型的输出参考力信号f_cs与真实环境输出的力信号f_e之差为e(t)，即e(t)＝f_e(t)-f_cs(t)，采用的自适应律为

γ为调整律，其取值范围为γ＝0.01～2，当主边反馈给操作者的力需要较大时，γ取较大值，当主边反馈给操作者的力需要较小时，γ取较小值，在仿真中取γ＝0.1。当在计算机仿真中，采用的算法为：

Step1设定调整率γ；

Step2采样模型参数修正模块的两个输入f_e(t)和f_cs(t)，得到误差e(t)；

Step3利用

计算出p_c(t)；

Step4t→t+h，h为计算步长，返回Step2，继续循环。

h的取值视具体计算条件(如所需要的精度，精度要求越高h越小)确定(如所需要的精度，精度要求越高h越小)，取值范围为(10^-7s～0.5s)，在仿真中取h＝1/30s。

令可调增益p_c(t₁)＝p_ct1，设τ为新的时间变量，相对于t₀时刻，τ的起始点为t₀+T，所述时间变量τ表征可调增益p_ct1输出之后的变化，即有p_ct1(τ)，

步骤3所述可调增益p_ct1(τ)经过通讯时延环节3的1T时延后，得到p_ct1(τ-T)，主边环境模型13根据p_ct1(τ-T)以及主机器人12此时输出的主边位置信号x_m(t₂)，输出主边环境模型输出力f_cm(t₂)，即：

$f_{\mathrm{cm}}\left(t\right)=p_c(t+T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，仿真中取m_c＝0.6，d_c＝0.01，k_c＝0，各参数与之前从边环境模型一致，这说明两模型结构一致。所述主边环境模型13输出力f_cm(t₂)最终作用于主机器人12，主机器人输出力f_h(t₂)再作用于操作者11，即f_h(t₂)＝f_cm(t₂)，使操作者11感受到作用力，形成下一步的控制的参考，

步骤4根据主机器人的输出力f_h(t₂)在t₂时刻形成新的主边位置信号，新的主边位置信号用x_m(t₂₊)表示，所述主边位置信号x_m(t₂₊)进入通讯时延环节3时延1T后形成从边位置信号x_s(t₃)，

步骤5时延后的从边位置信号x_s(t₃)输入从机器人22，使从机器人22运动并产生环境位置信号x_e(t₃)，作用于环境21和从边环境模型23，与此同时，步骤2中所述的可调增益值p_ct1(τ)经过模拟时延模块25模拟时延2T后，得到p_ct1(τ-2T)，从边环境模型23根据p_ct1(τ-2T)以及从机器人22此时输出的从边位置信号值x_m(t₃)，输出从边环境模型输出力f_cs(t₃)，作用于模型参数修正模块24，此时可调增益值p_ct1(τ)的运行周期终止，可调增益p_ct1(τ)消失，模型参数修正模块24根据环境位置信号x_e(t₃)作用于环境21产生的环境反作用力f_e(t₃)以及从边环境模型输出力f_cs(t₃)生成新的可调增益p_c(t₃)，设p_c(t₃)＝p_ct3，由于之前的可调增益p_ct1(τ)消失，所以可以设τ为新的时间变量，此时相对于t₀时刻，τ的起始点为t₀+3T，所述时间变量τ表征可调增益p_ct3输出之后的变化，即有p_ct3(τ)，

步骤6所述可调增益p_ct3(τ)经过通讯时延环节3的1T时延后，得到p_ct3(τ-T)，主边环境模型13根据p_ct3(τ-T)以及主机器人12此时输出的主边位置信号x_m(t₄)，输出主边环境模型输出力f_cm(t₄)，所述主边环境模型输出力f_cm(t₄)最终作用于主机器人12，主机器人输出力f_h(t₄)再作用于操作者11，即f_h(t₄)＝f_cm(t₄)，使操作者11感受到作用力，形成下一步的控制的参考，

步骤7返回步骤1，

这样，不断的进行循环，主边环境模型和从边环境模型就不断地逼近真实的环境模型，形成克服时延影响的遥操作机器人控制。

图2为本发明控制系统中的时间关系示意图。三条实线箭头分别表示环境21、主边环境模型13和从边环境模型23在输出量上的时间标度；三条虚线表示时间间隔为T；虚线箭头表示可调增益p_c(τ)的作用位置。这样可以清晰地看出可调增益对各输出的影响。在a点形成的p_c(τ)在1T后影响主边环境模型13的输出，2T后影响从边环境模型23的输出。操作者感受到的一直是1T前修正的模型输出的力，与环境21输出f_e比较的一直是1T前操作者提供的位置信号所形成的模型参数修正模块24的参考力信号f_cs，所以本发明改变了时延对系统稳定性的影响，提高了系统的稳定性。

图3为本发明的仿真结果图，以一个在t＝3时刻触发的斜坡信号作为主边位置信号x_m进入通讯延时环节，即图中实线所表示的控制量。虚线表示的是环境反作用于从机器人的力信号f_e，可以看出f_e很好地跟随了x_m。点线表示的从边环境模型输出的力信号f_cs，f_cs在经历了初期的延时之后快速且较好的跟踪了f_e，使f_e与f_cs的差e始终保持最小。仿真结果说明本发明可以遥操作机器人系统实线准确、稳定地控制。

Claims (2)

1.一种基于主从参考模型的遥操作机器人自适应控制方法，其特征在于，

包括：主边回路(1)，从边回路(2)以及通讯时延环节(3)，主边回路(1)由操作者(11)、主机器人(12)和主边环境模型(13)构成，从边回路(2)由环境(21)、从机器人(22)、从边环境模型(23)、模型参数修正模块(24)和模拟时延模块(25)构成，主边环境模型(13)与从边环境模型(23)结构一致，

设t为控制系统的时间变量，T为通讯时延环节(3)确定的时延量，引入时间标识t_n，n＝0，1，2…，对各控制量进行说明，所述t_n的意义为：设某一时刻为t₀，则t₁为t₀之后经过了时延量T的时间值，t₂为t₁之后经过了时延量T的时间值，即为t₀之后经过了时延量2T的时间值，以此类推，t_n为t₀之后经过了时延量nT的时间值，

步骤1操作者(11)通过主机器人(12)发出主边位置信号x_m(t₀)，所述主边位置信号x_m(t₀)进入通讯时延环节(3)时延1T后形成从边位置信号x_s(t₁)，

步骤2时延后的从边位置信号x_s(t₁)输入从机器人(22)，使从机器人(22)运动并产生环境位置信号x_e(t₁)，作用于环境(21)，同时环境位置信号x_e(t₁)作为从边环境模型(23)的输入量进入从边环境模型(23)，从边环境模型(23)根据环境位置信号x_e(t₁)产生从边环境模型输出力f_cs(t₁)，即：

$f_{\mathrm{cs}}\left(t\right)=p_c(t+2T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，所述环境根据环境位置信号x_e(t₁)产生环境反作用力f_e(t₁)，即：

$f_e\left(t\right)=m_e\stackrel{\·\·}{x}\left(t\right)+d_e\stackrel{\·}{x}\left(t\right)+k_{e}x\left(t\right)$

其中，m_e为环境的惯性系数，d_e为模型的阻尼系数，k_e为模型的弹性系数，且所述环境反作用力f_e(t₁)反作用于从机器人(22)，同时环境反作用力f_e(t₁)进入到模型参数修正模块(24)，模型参数修正模块(24)根据从边环境模型输出力f_cs(t₁)及环境反作用力f_e(t₁)输出可调增益p_c(t₁)，令可调增益p_c(t₁)＝p_ct1，设τ为新的时间变量，相对于t₀时刻，τ的起始点为t₀+T，所述时间变量τ表征可调增益p_ct1输出之后的变化，即有p_ct1(τ)，

步骤3所述可调增益p_ct1(τ)经过通讯时延环节(3)的1T时延后，得到p_ct1(τ-T)，主边环境模型(13)根据p_ct1(τ-T)以及主机器人(12)此时输出的主边位置信号x_m(t₂)，输出主边环境模型输出力f_cm(t₂)，即：

$f_{\mathrm{cm}}\left(t\right)=p_c(t+T)\·(m_c\stackrel{\·\·}{x}\left(t\right)+d_c\stackrel{\·}{x}\left(t\right)+k_{c}x\left(t\right))$

其中，p_c(t)为可调增益，m_c为模型的惯性系数，d_c为模型的阻尼系数，k_c为模型的弹性系数，所述主边环境模型(13)输出力f_cm(t₂)最终作用于主机器人(12)，主机器人输出力f_h(t₂)再作用于操作者(11)，即f_h(t₂)＝f_cm(t₂)，使操作者(11)感受到作用力，形成下一步的控制的参考，

步骤4根据主机器人的输出力f_h(t₂)在t₂时刻形成新的主边位置信号，新的主边位置信号用x_m(t₂₊)表示，所述主边位置信号x_m(t₂₊)进入通讯时延环节(3)时延1T后形成从边位置信号x_s(t₃)，

步骤5时延后的从边位置信号x_s(t₃)输入从机器人(22)，使从机器人(22)运动并产生环境位置信号x_e(t₃)，作用于环境(21)和从边环境模型(23)，与此同时，步骤2中所述的可调增益值p_ct1(τ)经过模拟时延模块(25)模拟时延2T后，得到p_ct1(τ-2T)，从边环境模型(23)根据p_ct1(τ-2T)以及从机器人(22)此时输出的环境位置信号x_e(t₃)，输出从边环境模型输出力f_cs(t₃)，作用于模型参数修正模块(24)，此时可调增益值p_ct1(τ)的运行周期终止，可调增益p_ct1(τ)消失，模型参数修正模块(24)根据环境位置信号x_e(t₃)作用于环境(21)产生的环境反作用力f_e(t₃)以及从边环境模型输出力f_cs(t₃)生成新的可调增益p_c(t₃)，设p_c(t₃)＝p_ct3，设τ为新的时间变量，此时相对于t₀时刻，τ的起始点为t₀+3T，所述时间变量τ表征可调增益p_ct3输出之后的变化，即有p_ct3(τ)，

步骤6所述可调增益p_ct3(τ)经过通讯时延环节(3)的1T时延后，得到p_ct3(τ-T)，主边环境模型(13)根据p_ct3(τ-T)以及主机器人(12)此时输出的主边位置信号x_m(t₄)，输出主边环境模型输出力f_cm(t₄)，所述主边环境模型输出力f_cm(t₄)最终作用于主机器人(12)，主机器人输出力f_h(t₄)再作用于操作者(11)，即f_h(t₄)＝f_cm(t₄)，使操作者(11)感受到作用力，形成下一步的控制的参考，

步骤7返回步骤1，

以此循环，实现连续控制。

2.根据权利要求1所述的基于主从参考模型的遥操作机器人自适应控制方法，其特征在于可调增益p_c(t)的产生方法为：

所述从边环境模型输出力f_cs(t)作为从边环境模型(23)的输出力f_cs(t)，环境反作用力f_e(t)作为环境(21)的环境输出力，模型参数修正模块(24)的控制律为 ${\stackrel{\·}{p}}_c\left(t\right)=\frac{{\mathrm{dp}}_c\left(t\right)}{\mathrm{dt}}=\mathrm{\γe}\left(t\right)f_e\left(t\right),$ 具体步骤如下：

Step1设定调整律γ，设γ＝0.01～2；

Step2模型参数修正模块的两个输入f_e(t)和f_cs(t)，得到广义误差e(t)，e(t)＝f_e(t)-f_cs(t)；

Step3利用 ${\stackrel{\·}{p}}_c\left(t\right)=\frac{{\mathrm{dp}}_c\left(t\right)}{\mathrm{dt}}=\mathrm{\γe}\left(t\right)f_e\left(t\right),$ 计算出p_c(t)；

Step4t→t+h，t为时间变量，h为计算步长，h的取值视具体计算条件确定，取值范围为10^-7s～0.5s，s为秒，返回Step2，继续循环。

Images