一种考虑伺服机构动态特性的运载器位姿一体化控制方法与流程

1.本发明涉及一种考虑伺服机构动态特性的运载器位姿一体化控制方法，属于飞行器动力学与控制技术领域。


背景技术：

2.以上面级、空间运输级、火箭末级为代表的运载器为实现入轨、行星际轨道转移、轨道机动等空间任务，往往配置一台或两台大推力液体发动机以确保足够的主动力在有限时间内达到所需的位置与速度，整个飞行过程中通过伺服机构或rcs调整运载器的姿态以实现推力矢量控制。
3.当前的运载器位姿控制系统的传统设计模式中，制导设计(也即位置控制设计)与姿态控制设计往往在各自性能指标分配后独立进行，所形成的控制指令再通过运载器6自由度仿真推演、反复迭代以满足运载器运动终端的轨道精度和飞行过程中的伺服机构约束(如幅值约束、动态性能约束等)。这种“分而治之”的位姿控制设计模式虽然易于子系统控制算法设计，但从原理上考虑，姿控子系统对制导指令的跟踪时延无疑会对轨道控制精度产生影响，而且，这种设计模式需要预先进行性能指标分配，使得最终设计指标存在保守性。


技术实现要素：

4.本发明的目的在于克服上述缺陷，提供一种考虑伺服机构动态特性的运载器位姿一体化控制方法，首先获取任务参数、运载器总体参数及执行机构配置矩阵、伺服系统动力学参数以及当前时刻运载器状态参数，后判断是否满足入轨要求，若满足，则控制过程结束，否则获取当前时刻伺服系统状态参数，并依次计算当前时刻发动机控制推力和指令姿态角、滤波指令姿态角及角速度，后判断姿态运动耦合矩阵是否奇异，如果不奇异则对矩阵进行修正，后继续计算当前时刻的伺服作动器指令位移、伺服作动器的滤波指令位移及速度、伺服作动器液压缸指令负载压力、伺服作动器液压缸的滤波指令负载压力及变化率、伺服作动器指令阀芯位移、伺服作动器阀芯滤波指令位移及变化率，以及伺服作动器输入控制电压矢量，最后输出控制电压矢量和推力矢量。本发明通过系统运载器“伺服-姿态-位置”耦合动力学，在反步设计框架下，结合滤波设计技术，有效补偿了姿控时延对位置控制的影响，从原理上提升了运载器位置控制精度，从模式上能够有效释放控制总体设计余量，从设计流程上能够大大提高设计效率。
5.为实现上述发明目的，本发明提供如下技术方案：
6.一种考虑伺服机构动态特性的运载器位姿一体化控制方法，包括如下步骤：
7.(1)获取运载器入轨任务参数；
8.(2)获取运载器总体参数，并根据运载器总体参数计算执行机构配置矩阵；
9.(3)获取伺服系统动力学参数；
10.(4)获取当前时刻tj的运载器状态参数，j＝0,1,2,
…
；
11.(5)根据步骤(4)所得当前时刻tj的运载器状态参数及步骤(1)所得运载器入轨任务参数，判断运载器是否满足入轨要求，若满足，则控制过程结束；否则，进入步骤(6)；
12.(6)获取当前时刻tj的伺服系统状态参数；
13.(7)根据步骤(1)所得运载器入轨任务参数和步骤(4)所得当前时刻tj的运载器状态参数计算当前时刻tj发动机控制推力和指令姿态角；
14.(8)根据步骤(7)所得当前时刻tj发动机的指令姿态角和步骤(1)所得运载器入轨任务参数，计算当前时刻tj运载器的滤波指令姿态角及滤波指令角速度；
15.(9)判断当前时刻tj运载器的姿态运动耦合矩阵g(θ)是否奇异，若奇异，进入步骤(10)，否则进入步骤(11)；
16.(10)对姿态运动耦合矩阵g(θ)进行修正，并进入步骤(11)；
17.(11)根据步骤(7)所得当前时刻tj发动机控制推力、步骤(8)所得当前时刻tj运载器的滤波指令姿态角及滤波指令角速度、步骤(9)或步骤(10)所得姿态运动耦合矩阵g(θ)、步骤(2)所得机构配置矩阵、以及步骤(4)所得当前时刻tj的运载器状态参数计算当前时刻tj的伺服作动器指令位移；
18.(12)根据步骤(11)所得当前时刻tj的伺服作动器指令位移、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻tj伺服作动器的滤波指令位移及滤波指令速度；
19.(13)根据步骤(12)所得当前时刻tj伺服作动器的滤波指令位移及滤波指令位移速度、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数，计算当前时刻tj伺服作动器液压缸指令负载压力；
20.(14)根据步骤(13)所得当前时刻tj伺服作动器液压缸指令负载压力、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻tj伺服作动器液压缸的滤波指令负载压力及滤波指令负载压力变化率；
21.(15)根据步骤(14)所得当前时刻tj伺服作动器液压缸的滤波指令负载压力及滤波指令负载压力变化率、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数计算当前时刻tj的伺服作动器指令阀芯位移；
22.(16)根据步骤(15)所得当前时刻tj的伺服作动器指令阀芯位移、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻tj伺服作动器阀芯滤波指令位移及位移变化率；
23.(17)根据步骤(16)所得当前时刻tj伺服作动器阀芯滤波指令位移及变化率、步骤(6)所得当前时刻tj的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数，计算当前时刻tj伺服作动器输入控制电压矢量；
24.(18)根据步骤(17)所得当前时刻tj伺服作动器输入控制电压矢量以及步骤(7)所得当前时刻tj发动机控制推力对运载器实施控制，并返回步骤(4)。
25.进一步的，所述步骤(1)中，运载器入轨任务参数包括入轨任务运载器的期望位置rd，期望速度vd，期望入轨精度e
ss
和控制周期t；
26.所述步骤(2)中，运载器总体参数包括运载器主发动机比冲i
sp
，运载器箭体半径r，伺服作动器与发动机中心线的距离d，伺服作动器上安装点构成的平面与运载器质心距离hg，发动机a和发动机b的安装点ag和bg距伺服作动器上安装点构成平面的距离lg；
27.所述步骤(3)中，伺服系统动力学参数包括负载质量矩阵m，液压缸横截面积a
p
，负载刚度k，阻尼b，液压缸的半容积v
t
，液压油体积弹性模量βe，液压缸泄露系数c
tl
，伺服阀流量系数cd，伺服阀面积梯度w，液压油密度ρ，液压泵供油压力ps，伺服阀响应时间常数t
sv
和伺服阀放大增益k
sv
；
28.所述步骤(4)中，当前时刻tj的运载器状态参数包括运载器质量m(tj)，运载器转动惯量j(tj)，运载器位置矢量r(tj)，运载器速度矢量v(tj)，运载器姿态角矢量θ(tj)和运载器姿态角速度矢量ω(tj)；
29.所述步骤(6)中，当前时刻tj的伺服系统状态参数包括负载力矩矢量f
l
(tj)，伺服作动器位移变化量δl(tj)，伺服作动器位移变化率液压缸负载压力矢量p(tj)和伺服阀阀芯位移矢量xv(tj)。
30.进一步的，所述步骤(2)中，执行机构配置矩阵h的计算公式为：
[0031][0032]
进一步的，所述步骤(5)中，判断运载器是否满足入轨要求的方法为：
[0033]
若满足||r(tj)-rd||≤e
ss
则运载器满足入轨要求，否则，运载器不满足入轨要求。
[0034]
进一步的，所述步骤(7)中，当前时刻tj发动机控制推力f(tj)和运载器指令姿态角的θc(tj)计算公式为：
[0035][0036]
其中，
[0037][0038]
式中，ξr为[0,1]区间内的实数，正实数kr和kv分别为位置控制增益和速度控制增益。
[0039]
进一步的，所述步骤(8)中，当前时刻tj运载器的滤波指令姿态角θf(tj)及滤波指令角速度的计算公式为：
[0040][0041]
式中，τ
ψ
和分别为偏航滤波时间常数和俯仰滤波时间常数，且ψc(t0)＝ψf(t0),t0为初始时间。
[0042]
进一步的，所述步骤(9)中，判断当前时刻tj运载器的姿态运动耦合矩阵g(θ)是否奇异的方法为，判断当前时刻tj运载器的姿态运动耦合矩阵g(θ)的秩rank(g(θ))是否小于3，若小于3，则表示g(θ)奇异，若不小于3，则表示g(θ)不奇异；
[0043]
所述步骤(10)中，对姿态运动耦合矩阵g(θ)进行修正的公式为：g(θ)＝g(θ) εe3，式中，ε为修正系数，ε小于等于0.001，e3为三阶单位矩阵。
[0044]
进一步的，所述步骤(11)中，当前时刻tj的伺服作动器指令位移δlc(tj)的计算公式为：
[0045][0046]
其中
[0047][0048]
式中，h为执行机构配置矩阵，为执行机构配置矩阵h的广义逆矩阵，ξ
θ
为[0,1]区间内的实数，正实数k
θ
和k
ω
分别为姿态控制增益和角速度控制增益，为姿态运动耦合矩阵g(θ)逆矩阵的导数矩阵。
[0049]
进一步的，所述步骤(12)中，当前时刻tj伺服作动器的滤波指令位移δlf(tj)及滤波指令速度的计算公式为：
[0050][0051]
式中，δlc(tj)为当前时刻tj的伺服作动器指令位移，τ
l
为伺服滤波时间常数，且δlf(t0)＝0,t0为初始时间。
[0052]
进一步的，所述步骤(13)中，当前时刻tj伺服作动器液压缸指令负载压力pc(tj)的计算公式为：
[0053][0054]
其中，a
p
为液压缸横截面积，k
l
和km分别为伺服位移增益矩阵和伺服位移速率增益矩阵，满足
[0055][0056]
式中，各参数取值满足如下关系：
[0057]
为负载质量矩阵m中的元素，j＝1,2,i＝a,b。
[0058]
进一步的，所述步骤(14)中，当前时刻tj伺服作动器液压缸的滤波指令负载压力pf(tj)及及滤波指令负载压力变化率的计算公式如下：
[0059][0060]
式中，τ
p
为液压缸滤波时间常数，且
[0061]
pf(t0)＝p(t0),t0为初始时间。
[0062]
式中，p(t0)为初始时间的液压缸负载压力矢量。
[0063]
进一步的，所述步骤(15)中，当前时刻tj的伺服作动器指令阀芯位移x
vc
(tj)的计算公式如下：
[0064][0065]
其中，k
p
为液压缸压力控制增益；
[0066][0067]
式中，对于发动机i的伺服作动器j，i＝a,b,j＝1,2，为伺服阀的阀芯位移，为的液压缸负载压力。
[0068]
进一步的，所述步骤(16)中，当前时刻tj伺服作动器阀芯滤波指令位移x
vf
(tj)及位移变化率的计算公式如下：
[0069][0070]
式中，τ
xv
为阀芯滤波时间常数，且x
vf
(t0)＝xv(t0),xv(t0)为初始时间的伺服作动器阀芯位移矢量，t0为初始时间。
[0071]
进一步的，所述步骤(17)中，当前时刻tj伺服作动器输入控制电压矢量u(tj)的计算公式为：
[0072][0073]
其中，ku为阀芯运动控制增益。
[0074]
本发明与现有技术相比具有如下有益效果：
[0075]
(1)本发明提出一种考虑伺服机构特性的运载器位姿一体化控制设计方法，综合考虑位置控制回路、姿态控制回路以及伺服运动回路，充分利用系统呈现的级联特点，在反步设计的框架下，实现一体化的控制指令设计，有效地补偿了姿控回路、伺服动态对轨道控制精度的影响；
[0076]
(2)本发明控制方法中，提出了一种控制耦合矩阵表示方法，以显式形式有效揭示了推力矢量对运载器姿态欧拉角的非线性依赖关系，据此完成了运载器“伺服-姿态-位置”耦合动力学系统的级联化表征，使位姿一体化设计成为可能；
[0077]
(3)本发明控制方法中，提出了一种滤波虚拟控制设计方法，在反步控制设计框架下，能够有效降低控制算法的复杂性，避免级数膨胀，提升控制方案的可解性。
附图说明
[0078]
图1为本发明的伺服作动器与主发动机的安装示意图；图1(a)为立体视图，图1(b)为为底视图；
[0079]
图2为本发明一种考虑伺服机构动态特性的运载器位姿一体化控制方法的工作流程图。
具体实施方式
[0080]
下面通过对本发明进行详细说明，本发明的特点和优点将随着这些说明而变得更为清楚、明确。
[0081]
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
[0082]
(1)获取任务参数，包括入轨任务运载器的期望位置rd和期望速度vd，期望入轨精度e
ss
，控制周期t。
[0083]
(2)获取运载器总体参数及执行机构配置矩阵，包括运载器主发动机比冲i
sp
，运载器箭体半径r，伺服作动器与发动机中心线的距离d，伺服作动器上安装点构成的平面与运载器质心距离hg，发动机a和发动机b的安装点ag和bg距该平面的距离lg，并计算执行机构配置矩阵h。
[0084]
(3)获取伺服系统动力学参数，包括负载质量矩阵m，液压缸横截面积a
p
，负载刚度k，阻尼b，液压缸的半容积v
t
，液压油体积弹性模量βe，液压缸泄露系数c
tl
，伺服阀流量系数cd，伺服阀面积梯度w，液压油密度ρ，液压泵供油压力ps，伺服阀响应时间常数t
sv
，伺服阀放大增益k
sv
。
[0085]
(4)获取当前时刻(tj时刻，j＝0,1,2,
…
)运载器状态参数，包括质量m(tj)，转动惯量j(tj)，位置矢量r(tj)，速度矢量v(tj)，姿态角矢量θ(tj)和姿态角速度矢量ω(tj)。
[0086]
(5)判断是否满足入轨要求，若满足，则控制过程结束；否则，进入步骤(6)。
[0087]
(6)获取当前时刻(tj时刻，j＝0,1,2,
…
)伺服系统状态参数，包括负载力矩矢量f
l
(tj)，伺服作动器位移变化量δl(tj)以及变化率液压缸负载压力矢量p(tj)，伺服阀阀芯位移矢量xv(tj)。
[0088]
(7)计算当前时刻(tj时刻，j＝0,1,2,
…
)发动机控制推力和指令姿态角。
[0089]
(8)计算当前时刻(tj时刻，j＝0,1,2,
…
)的滤波指令姿态角及角速度。
[0090]
(9)判断当前时刻(tj时刻，j＝0,1,2,
…
)的姿态运动耦合矩阵g(θ)是否奇异，若矩阵奇异，进入步骤(10)，否则进入步骤(11)。
[0091]
(10)修正姿态运动耦合矩阵。
[0092]
(11)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令位移。
[0093]
(12)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器的滤波指令位移及速度。
[0094]
(13)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸指令负载压力pc(tj)。
[0095]
(14)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸的滤波指令负载压力及变化率。
[0096]
(15)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令阀芯位移x
vc
(tj)。
[0097]
(16)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器阀芯滤波指令位移及变化率。
[0098]
(17)计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器输入控制电压矢量u(tj)。
[0099]
(18)输出当前时刻(tj时刻，j＝0,1,2,
…
)的控制电压矢量u(tj)和推力矢量f(tj)，实施控制，并返回步骤(4)。
[0100]
实施例1：
[0101]
本发明可应用于上面级、空间运输级、火箭末级等运载器入轨任务的高精度控制问题。如图1所示，o-xbybzb为运载器本体坐标系，主发动机为双机并联配置，通过电液伺服系统实现发动机的双向摇摆。发动机a通过伺服作动器sa1和sa2实现摇摆，发动机b通过伺服作动器sb1和sb2实现摇摆，伺服作动器sa1、sa2、sb1和sb2均竖直安装，且距离发动机中心线的距离为d。四个伺服作动器的上安装点分别为a12、a22、b12和b22，下安装点分别为a21、a11、b11和b21。伺服机构上安装点构成的平面距运载器质心距离为hg，而发动机a和发动机b的安装点ag和bg距该平面的距离为lg。
[0102]
下面结合附图对本发明作进一步详细说明：
[0103]
1)伺服作动器直线运动与发动机摇摆运动关系：
[0104]
发动机a与伺服作动器sa1和sa2的运动关系为：
[0105][0106]
其中，αa和βa分别为发动机a绕zb轴和yb轴转动的角度，和分别为伺服作动器sa1和sa2的直线位移变化。
[0107]
发动机b与伺服作动器sb1和sb2的运动关系为：
[0108][0109]
其中，αb和βb分别为发动机b绕zb轴和yb轴转动的角度，和分别为伺服作动器sb1和sb2的直线位移变化。
[0110]
2)伺服作动器的动力学模型
[0111]
液压伺服动力学模型表征如下：
[0112][0113]
其中，
[0114][0115][0116][0117]
式中，a
p
为液压缸横截面积，k为负载刚度，b为阻尼，v
t
为液压缸的半容积，βe为液压油体积弹性模量，c
tl
为液压缸泄露系数，cd为伺服阀流量系数，w为伺服阀面积梯度，ρ为液压油密度，ps为液压泵供油压力，t
sv
为伺服阀响应时间常数，k
sv
为伺服阀放大增益，对于发动机i的伺服作动器j(i＝a,b,j＝1,2)，为负载质量，为的液压缸负载压力，为伺服阀的阀芯位移，为负载力矩，为输入控制电压。
[0118]
3)运载器姿态动力学模型
[0119]
运载器本体坐标系相对于惯性系的姿态运动学与动力学模型表征如下：
[0120][0121]
其中，j为运载器转动惯量矩阵，θ和ω分别为运载器的姿态角矢量和姿态角速度矢量，描述如下
[0122][0123]
式中，γ为运载器滚转角，ψ为运载器偏航，为运载器俯仰角，ω
x
,ωy,ωz分别为运载器绕本体系xb轴、yb轴和zb轴的角速率，f为发动机推力，g(θ)为姿态运动耦合矩阵，h为配置矩阵，描述如下
[0124]
[0125][0126]
式中，r为运载器箭体半径。
[0127]
4)运载器位置动力学模型
[0128]
运载器在惯性系下的位置动力学模型表征如下：
[0129][0130]
其中，矢量r和v分别为运载器相对地心的位置矢量和速度矢量，m为运载器质量，μ为地球引力常数，r为矢量r的绝对值，矢量u(θ)可描述为：
[0131][0132]
5)如图2所示，一种考虑伺服机构动态特性的运载器位姿一体化控制方法包括以下步骤：
[0133]
(1)获取任务参数，包括入轨任务运载器的期望位置rd和期望速度vd，期望入轨精度e
ss
，控制周期t。
[0134]
(2)获取运载器总体参数及执行机构配置矩阵，包括运载器主发动机比冲i
sp
，运载器箭体半径r，伺服作动器与发动机中心线的距离d，伺服作动器上安装点构成的平面与运载器质心距离hg，发动机a和发动机b的安装点ag和bg距该平面的距离lg，并按下式计算执行机构配置矩阵h：
[0135][0136]
(3)获取伺服系统动力学参数，包括负载质量矩阵m，液压缸横截面积a
p
，负载刚度k，阻尼b，液压缸的半容积v
t
，液压油体积弹性模量βe，液压缸泄露系数c
tl
，伺服阀流量系数cd，伺服阀面积梯度w，液压油密度ρ，液压泵供油压力ps，伺服阀响应时间常数t
sv
，伺服阀放大增益k
sv
。
[0137]
(4)获取当前时刻(tj时刻，j＝0,1,2,
…
)运载器状态参数，包括质量m(tj)，转动惯量j(tj)，位置矢量r(tj)，速度矢量v(tj)，姿态角矢量θ(tj)和姿态角速度矢量ω(tj)。
[0138]
(5)按下式判断运载器是否满足入轨要求，即
[0139]
||r(tj)-rd||≤e
ss
[0140]
若满足，则控制过程结束；否则，进入步骤(6)。
[0141]
(6)获取当前时刻(tj时刻，j＝0,1,2,
…
)伺服系统状态参数，包括负载力矩矢量f
l
(tj)，伺服作动器位移变化量δl(tj)以及变化率液压缸负载压力矢量p(tj)，伺服阀阀芯位移矢量xv(tj)。
[0142]
(7)按下述公式计算当前时刻(tj时刻，j＝0,1,2,
…
)发动机控制推力，指令姿态角：
[0143][0144]
其中，
[0145][0146]
式中，ξr为[0,1]区间内的实数，可取为0.5，正实数kr和kv分别为位置控制增益和速度控制增益，r(tj)表示运载器位置的幅值。
[0147]
(8)根据步骤(7)所得当前时刻发动机指令姿态角θc(tj)，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)运载器的滤波指令姿态角及角速度：
[0148][0149]
式中，τ
ψ
和分别为偏航滤波时间常数和俯仰滤波时间常数，而且ψc(t0)＝ψf(t0),t0为初始时间。
[0150]
(9)判断当前时刻(tj时刻，j＝0,1,2,
…
)的姿态运动耦合矩阵g(θ)是否奇异，即，矩阵的秩rank(g(θ))是否小于3，若小于3，则表示矩阵奇异，进入步骤(10)，否则进入步骤(11)。
[0151]
(10)按下式修正姿态运动耦合矩阵，并进入步骤(11)。
[0152]
g(θ)＝g(θ) εe3[0153]
式中，ε为一较小的正实数，可取为0.001，e3为三阶单位矩阵。
[0154]
(11)根据步骤(7)所得发动机控制推力f(tj)，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令位移：
[0155][0156]
其中
[0157][0158]
式中，ξ
θ
为[0,1]区间内的实数，可取为0.5，正实数k
θ
和k
ω
分别为位置控制增益和速度控制增益，为矩阵g(θ)逆矩阵的导数矩阵。
[0159]
(12)根据当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令位移δlc(tj)，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器的滤波指令位移及速度：
[0160][0161]
式中，τ
l
为伺服滤波时间常数，而且且δlf(t0)＝0,t0为初始时间。
[0162]
(13)根据当前时刻(时刻，j＝0,1,2,
…
)的伺服作动器的滤波指令位移及速度，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸指令负载压力pc(tj)：
[0163][0164]
其中，a
p
为液压缸横截面积，k
l
和km分别为伺服位移增益矩阵和伺服位移速率增益矩阵，满足
[0165][0166]
式中，各参数取值满足如下关系：
[0167]
j＝1,2,i＝a,b
[0168]
(14)根据当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸指令负载压力pc(tj)，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸的滤波指令负载压力及变化率：
[0169][0170]
式中，τ
p
为液压缸滤波时间常数，而且
[0171]
pf(t0)＝p(t0),
[0172]
(15)根据当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器液压缸的滤波指令负载压力pf(tj)及变化率按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令阀芯位移x
vc
(tj)：
[0173][0174]
其中，k
p
为液压缸压力控制增益。
[0175]
(16)根据当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器指令阀芯位移x
vc
(tj)，按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器阀芯滤波指令位移及变化率：
[0176][0177]
式中，τ
xv
为阀芯滤波时间常数，且x
vf
(t0)＝xv(t0),xv(t0)为初始时间的伺服作动器阀芯位移矢量，t0为初始时间。
[0178]
(17)根据伺服作动器阀芯滤波指令位移x
vf
(tj)及位移变化率按下式计算当前时刻(tj时刻，j＝0,1,2,
…
)的伺服作动器输入控制电压矢量u(tj)：
[0179][0180]
其中，ku为阀芯运动控制增益。
[0181]
(18)输出当前时刻(tj时刻，j＝0,1,2,
…
)的控制电压矢量u(tj)和推力矢量f(tj)，实施控制，并返回步骤(4)。
[0182]
以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。本发明的保护范围以所附权利要求为准。
[0183]
本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。