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

技术特征：
1.一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，包括如下步骤：(1)获取运载器入轨任务参数；(2)获取运载器总体参数，并根据运载器总体参数计算执行机构配置矩阵；(3)获取伺服系统动力学参数；(4)获取当前时刻t
j
的运载器状态参数，j＝0,1,2,
…
；(5)根据步骤(4)所得当前时刻t
j
的运载器状态参数及步骤(1)所得运载器入轨任务参数，判断运载器是否满足入轨要求，若满足，则控制过程结束；否则，进入步骤(6)；(6)获取当前时刻t
j
的伺服系统状态参数；(7)根据步骤(1)所得运载器入轨任务参数和步骤(4)所得当前时刻t
j
的运载器状态参数计算当前时刻t
j
发动机控制推力和指令姿态角；(8)根据步骤(7)所得当前时刻t
j
发动机的指令姿态角和步骤(1)所得运载器入轨任务参数，计算当前时刻t
j
运载器的滤波指令姿态角及滤波指令角速度；(9)判断当前时刻t
j
运载器的姿态运动耦合矩阵g(θ)是否奇异，若奇异，进入步骤(10)，否则进入步骤(11)；(10)对姿态运动耦合矩阵g(θ)进行修正，并进入步骤(11)；(11)根据步骤(7)所得当前时刻t
j
发动机控制推力、步骤(8)所得当前时刻t
j
运载器的滤波指令姿态角及滤波指令角速度、步骤(9)或步骤(10)所得姿态运动耦合矩阵g(θ)、步骤(2)所得机构配置矩阵、以及步骤(4)所得当前时刻t
j
的运载器状态参数计算当前时刻t
j
的伺服作动器指令位移；(12)根据步骤(11)所得当前时刻t
j
的伺服作动器指令位移、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻t
j
伺服作动器的滤波指令位移及滤波指令速度；(13)根据步骤(12)所得当前时刻t
j
伺服作动器的滤波指令位移及滤波指令位移速度、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数，计算当前时刻t
j
伺服作动器液压缸指令负载压力；(14)根据步骤(13)所得当前时刻t
j
伺服作动器液压缸指令负载压力、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻t
j
伺服作动器液压缸的滤波指令负载压力及滤波指令负载压力变化率；(15)根据步骤(14)所得当前时刻t
j
伺服作动器液压缸的滤波指令负载压力及滤波指令负载压力变化率、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数计算当前时刻t
j
的伺服作动器指令阀芯位移；(16)根据步骤(15)所得当前时刻t
j
的伺服作动器指令阀芯位移、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(1)所得运载器入轨任务参数，计算当前时刻t
j
伺服作动器阀芯滤波指令位移及位移变化率；(17)根据步骤(16)所得当前时刻t
j
伺服作动器阀芯滤波指令位移及变化率、步骤(6)所得当前时刻t
j
的伺服系统状态参数以及步骤(3)所得伺服系统动力学参数，计算当前时刻t
j
伺服作动器输入控制电压矢量；(18)根据步骤(17)所得当前时刻t
j
伺服作动器输入控制电压矢量以及步骤(7)所得当
前时刻t
j
发动机控制推力对运载器实施控制，并返回步骤(4)。2.根据权利要求1所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(1)中，运载器入轨任务参数包括入轨任务运载器的期望位置r
d
，期望速度v
d
，期望入轨精度e
ss
和控制周期t；所述步骤(2)中，运载器总体参数包括运载器主发动机比冲i
sp
，运载器箭体半径r，伺服作动器与发动机中心线的距离d，伺服作动器上安装点构成的平面与运载器质心距离h
g
，发动机a和发动机b的安装点a
g
和b
g
距伺服作动器上安装点构成平面的距离l
g
；所述步骤(3)中，伺服系统动力学参数包括负载质量矩阵m，液压缸横截面积a
p
，负载刚度k，阻尼b，液压缸的半容积v
t
，液压油体积弹性模量β
e
，液压缸泄露系数c
tl
，伺服阀流量系数c
d
，伺服阀面积梯度w，液压油密度ρ，液压泵供油压力p
s
，伺服阀响应时间常数t
sv
和伺服阀放大增益k
sv
；所述步骤(4)中，当前时刻t
j
的运载器状态参数包括运载器质量m(t
j
)，运载器转动惯量j(t
j
)，运载器位置矢量r(t
j
)，运载器速度矢量v(t
j
)，运载器姿态角矢量θ(t
j
)和运载器姿态角速度矢量ω(t
j
)；所述步骤(6)中，当前时刻t
j
的伺服系统状态参数包括负载力矩矢量f
l
(t
j
)，伺服作动器位移变化量δl(t
j
)，伺服作动器位移变化率液压缸负载压力矢量p(t
j
)和伺服阀阀芯位移矢量x
v
(t
j
)。3.根据权利要求2所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(2)中，执行机构配置矩阵h的计算公式为：4.根据权利要求2所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(5)中，判断运载器是否满足入轨要求的方法为：若满足||r(t
j
)-r
d
||≤e
ss
则运载器满足入轨要求，否则，运载器不满足入轨要求。5.根据权利要求2所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(7)中，当前时刻t
j
发动机控制推力f(t
j
)和运载器指令姿态角的θ
c
(t
j
)计算公式为：其中，
式中，ξ
r
为[0,1]区间内的实数，正实数k
r
和k
v
分别为位置控制增益和速度控制增益。6.根据权利要求5所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(8)中，当前时刻t
j
运载器的滤波指令姿态角θ
f
(t
j
)及滤波指令角速度的计算公式为：式中，τ
ψ
和分别为偏航滤波时间常数和俯仰滤波时间常数，且ψ
c
(t0)＝ψ
f
(t0),t0为初始时间。7.根据权利要求1或2所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(9)中，判断当前时刻t
j
运载器的姿态运动耦合矩阵g(θ)是否奇异的方法为，判断当前时刻t
j
运载器的姿态运动耦合矩阵g(θ)的秩rank(g(θ))是否小于3，若小于3，则表示g(θ)奇异，若不小于3，则表示g(θ)不奇异；所述步骤(10)中，对姿态运动耦合矩阵g(θ)进行修正的公式为：g(θ)＝g(θ) εe3，式中，ε为修正系数，ε小于等于0.001，e3为三阶单位矩阵。8.根据权利要求6所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(11)中，当前时刻t
j
的伺服作动器指令位移δl
c
(t
j
)的计算公式为：其中式中，h为执行机构配置矩阵，为执行机构配置矩阵h的广义逆矩阵，ξ
θ
为[0,1]区间
内的实数，正实数k
θ
和k
ω
分别为姿态控制增益和角速度控制增益，为姿态运动耦合矩阵g(θ)逆矩阵的导数矩阵。9.根据权利要求2或8所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(12)中，当前时刻t
j
伺服作动器的滤波指令位移δl
f
(t
j
)及滤波指令速度的计算公式为：式中，δl
c
(t
j
)为当前时刻t
j
的伺服作动器指令位移，τ
l
为伺服滤波时间常数，且δl
f
(t0)＝0,t0为初始时间。10.根据权利要求9所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(13)中，当前时刻t
j
伺服作动器液压缸指令负载压力p
c
(t
j
)的计算公式为：其中，a
p
为液压缸横截面积，k
l
和k
m
分别为伺服位移增益矩阵和伺服位移速率增益矩阵，满足式中，各参数取值满足如下关系：为负载质量矩阵m中的元素，11.根据权利要求10所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(14)中，当前时刻t
j
伺服作动器液压缸的滤波指令负载压力p
f
(t
j
)及及滤波指令负载压力变化率的计算公式如下：式中，τ
p
为液压缸滤波时间常数，且p
f
(t0)＝p(t0),t0为初始时间。式中，p(t0)为初始时间的液压缸负载压力矢量。12.根据权利要求11所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(15)中，当前时刻t
j
的伺服作动器指令阀芯位移x
vc
(t
j
)的计算公式如下：其中，k
p
为液压缸压力控制增益；
式中，对于发动机i的伺服作动器j，i＝a,b,j＝1,2，为伺服阀的阀芯位移，为的液压缸负载压力。13.根据权利要求12所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(16)中，当前时刻t
j
伺服作动器阀芯滤波指令位移x
vf
(t
j
)及位移变化率的计算公式如下：式中，τ
xv
为阀芯滤波时间常数，且x
vf
(t0)＝x
v
(t0),x
v
(t0)为初始时间的伺服作动器阀芯位移矢量，t0为初始时间。14.根据权利要求13所述的一种考虑伺服机构动态特性的运载器位姿一体化控制方法，其特征在于，所述步骤(17)中，当前时刻t
j
伺服作动器输入控制电压矢量u(t
j
)的计算公式为：其中，k
u
为阀芯运动控制增益。

技术总结
本发明公开了考虑伺服机构动态特性的运载器位姿一体化控制方法，首先获取任务参数、运载器总体参数及执行机构配置矩阵、伺服系统动力学参数以及当前时刻运载器状态参数，在不满足入轨要求时获取当前时刻伺服系统状态参数，依次计算发动机控制推力和指令姿态角、滤波指令姿态角及角速度，后在判断姿态运动耦合矩阵不奇异时对矩阵修正，继续计算当前时刻伺服作动器指令位移、伺服作动器滤波指令位移及速度、伺服作动器液压缸指令负载压力、伺服作动器液压缸的滤波指令负载压力等，最后输出控制电压矢量和推力矢量。本发明通过系统运载器“伺服-姿态-位置”耦合动力学，在反步设计框架下，结合滤波设计技术，有效补偿了姿控时延对位置控制的影响。位置控制的影响。位置控制的影响。


技术研发人员：张烽 张柳 汪小卫 吴胜宝 高朝辉 李扬 刘丙利 唐琼 胡冬生 王书廷 张雪梅 任宽 童科伟 刁萌
受保护的技术使用者：中国运载火箭技术研究院
技术研发日：2021.12.14
技术公布日：2022/4/5