一种着陆器动力下降液体晃动抑制方法与流程

本发明涉及一种着陆器动力下降姿态控制方法，尤其涉及一种着陆器动力下降期间的液体晃动抑制姿态控制方法，属于航天器姿态控制技术领域。

背景技术：

探测器动力下降期间，制导系统根据探测器位置速度信息给出目标姿态，姿态控制系统根据姿态确定给出的当前姿态，控制探测器趋向于目标姿态，最终软着陆在月球表面。

当着陆器推进剂贮箱为表面张力贮箱时，在着陆过程中，除了发动机产生干扰力矩外，液体推进剂容易晃动而产生晃动力矩，危机着陆安全。

技术实现要素：

本发明的技术解决问题是：克服现有技术的不足，针对动力下降期间具有液体推进剂的着陆器姿态控制，提出一种动力下降液体晃动抑制方法，提高了姿态控制精度。

本发明的技术解决方案是：

一种着陆器动力下降液体晃动抑制方法，基于隆伯格观测器方法可以估计着陆器的角速度和干扰力矩，且隆伯格观测器的角速度系数和干扰力矩系数范围均为0～1；

当隆伯格观测器的干扰力矩系数大于零小于0.5时，估计结果为缓慢变化的发动机干扰力矩以及对应的角速度，定义为慢变干扰力矩和慢变角速度，此时的隆伯格观测器系数定义为慢变角速度系数和慢变干扰力矩系数；

当隆伯格观测器的干扰力矩系数大于等于0.5小于1时，估计结果为发动机干扰力矩与晃动干扰力矩之和以及对应的角速度，定义为快变干扰力矩和快变角速度，此时的隆伯格观测器系数定义为快变角速度系数和快变干扰力矩系数；

根据快变干扰力矩与慢变干扰力矩的差值在一段时间的统计特性，决定采用快变干扰力矩或者慢变干扰力矩作为姿态控制的前馈补偿力矩；

该方法基于隆伯格观测器估计着陆器的角速度和干扰力矩，具体步骤包括：

(1)设定着陆器的初始状态时，快变角速度ω_e1(0)＝[000]^t，快变干扰力矩md_e1(0)＝[000]^t；慢变角速度ω_e2(0)＝[000]^t，慢变干扰力矩md_e2(0)＝[000]^t；初始姿态控制力矩mc(0)＝[000]^t；

(2)第i(i≥1)个控制周期时，估计快变角速度ω_e1(i)，ω_e1(i)＝ω_e1(i-1)+j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt+l11·(ω(i)-ω_e1(i-1)-j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt)；

其中：ω_e1(i-1)为上周期快变角速度，md_e1(i-1)为上周期快变干扰力矩，l11为快变角速度系数，mc(i-1)为上周期姿态控制力矩，ω(i)为第i个控制周期测量得到姿态角速度，j为着陆器惯量矩阵，δt为控制周期；

(3)第i个控制周期时，估计快变干扰力矩md_e1(i)，md_e1(i)＝md_e1(i-1)+l21·j·(ω(i)-ω_e1(i-1)-j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt)/δt；

其中：l21为快变干扰力矩系数；

(4)第i个控制周期时，估计慢变角速度ω_e2(i)，ω_e2(i)＝ω_e2(i-1)+j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt+l12·(ω(i)-ω_e2(i-1)-j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt)；

其中：ω_e2(i-1)为上周期慢变角速度，md_e2(i-1)为上周期慢变干扰力矩，l12为慢变角速度系数；

(5)第i个控制周期时，估计慢变干扰力矩md_e2(i)，md_e2(i)＝md_e2(i-1)+l22·j·(ω(i)-ω_e2(i-1)-j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt)/δt；

其中：l22为慢变干扰力矩系数；

(6)第i个控制周期时，获取力矩差历史值矩阵dmde，矩阵dmde为3行nw列的矩阵，nw为统计周期数；获取的方法是，将步骤(3)的md_e1(i)减去步骤(5)的md_e2(i)的差值md_e1(i)-md_e2(i)，赋值给力矩差历史值矩阵dmde的第mod(i,nw)列，其中：函数mod()为求余数函数；

(7)第i个控制周期时，若i<nw，则：前馈补偿力矩δmd(i)＝[000]^t；

(8)第i个控制周期时，若i≥nw，则：计算滚动力矩差均值meanx(i)，计算滚动力矩差方差stdx(i)，其中：j＝1～nw；若|dmde(1,i)-meanx(i)|>stdx(i)，则：将md_e2(i)的第1个元素赋值给δmd(i)的第1个元素；若|dmde(1,i)-meanx(i)|≤stdx(i)，则：将md_e1(i)的第1个元素赋值给δmd(i)的第1个元素；

(9)第i个控制周期时，若i≥nw，则：计算俯仰力矩差均值meany(i)，计算俯仰力矩差方差stdy(i)，其中：j＝1～nw；若|dmde(2,i)-meany(i)|>stdy(i)，则：将md_e2(i)的第2个元素赋值给δmd(i)的第2个元素；若|dmde(2,i)-meany(i)|≤stdy(i)，则：将md_e1(i)的第2个元素赋值给δmd(i)的第2个元素；

(10)第i个控制周期时，若i≥nw，则：计算偏航力矩差均值meanz(i)，计算偏航力矩差方差stdz(i)，其中：j＝1～nw；若|dmde(3,i)-meanz(i)|>stdz(i)，则：将md_e2(i)的第3个元素赋值给δmd(i)的第3个元素；若|dmde(3,i)-meanz(i)|≤stdz(i)，则：将md_e1(i)的第3个元素赋值给δmd(i)的第3个元素；

(11)第i个控制周期时，计算姿态控制力矩mc(i)，mc(i)＝tpid(i)+δmd(i)，其中：tpid(i)为pid控制力矩。

本发明与现有技术相比的有益效果是：

本发明姿态控制方法，针对动力下降段液体晃动抑制问题，采用滤波系数不同的隆伯格观测器，分别估计缓慢变化的发动机干扰力矩和发动机干扰力矩与晃动干扰力矩之和。根据两种估计干扰力矩一段时间的历史值，判定干扰力矩状态和观测器估计状态，选择一种干扰力矩估值，进行姿控控制前馈补偿，提高了姿态控制精度。本发明公开了一种着陆器动力下降姿态控制方法，该方法针对动力下降段液体晃动抑制问题，采用两个滤波系数不同的观测器，分别估计缓慢变化的发动机干扰力矩和发动机干扰力矩与晃动干扰力矩之和。根据两种估计干扰力矩一段时间的历史值，判定干扰力矩状态和观测器估计状态，选择一种干扰力矩估值，进行姿控控制前馈补偿，提高了姿态控制精度。

具体实施方式

本发明的实现步骤如下：

(1)设定着陆器的初始状态时，快变角速度ω_e1(0)＝[000]^t，快变干扰力矩md_e1(0)＝[000]^t；慢变角速度ω_e2(0)＝[000]^t，慢变干扰力矩md_e2(0)＝[000]^t；初始姿态控制力矩mc(0)＝[000]^t；

(2)第i(i≥1)个控制周期时，估计快变角速度ω_e1(i)，ω_e1(i)＝ω_e1(i-1)+j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt+l11·(ω(i)-ω_e1(i-1)-j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt)；

其中：ω_e1(i-1)为上周期快变角速度，md_e1(i-1)为上周期快变干扰力矩，l11为快变角速度系数，mc(i-1)为上周期姿态控制力矩，ω(i)为第i个控制周期测量得到姿态角速度，j为着陆器惯量矩阵，δt为控制周期；

(3)第i个控制周期时，估计快变干扰力矩md_e1(i)，md_e1(i)＝md_e1(i-1)+l21·j·(ω(i)-ω_e1(i-1)-j^-1·(mc(i-1)+md_e1(i-1)-cross(ω_e1(i-1),j·ω_e1(i-1)))·δt)/δt；

其中：l21为快变干扰力矩系数；

(4)第i个控制周期时，估计慢变角速度ω_e2(i)，ω_e2(i)＝ω_e2(i-1)+j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt+l12·(ω(i)-ω_e2(i-1)-j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt)；

其中：ω_e2(i-1)为上周期慢变角速度，md_e2(i-1)为上周期慢变干扰力矩，l12为慢变角速度系数；

(5)第i个控制周期时，估计慢变干扰力矩md_e2(i)，md_e2(i)＝md_e2(i-1)+l22·j·(ω(i)-ω_e2(i-1)-j^-1·(mc(i-1)+md_e2(i-1)-cross(ω_e2(i-1),j·ω_e2(i-1)))·δt)/δt；

其中：l22为慢变干扰力矩系数；

(6)第i个控制周期时，获取力矩差历史值矩阵dmde，矩阵dmde为3行nw列的矩阵，nw为统计周期数；获取的方法是，将步骤(3)的md_e1(i)减去步骤(5)的md_e2(i)的差值md_e1(i)-md_e2(i)，赋值给力矩差历史值矩阵dmde的第mod(i,nw)列，其中：函数mod()为求余数函数；

(7)第i个控制周期时，若i<nw，则：前馈补偿力矩δmd(i)＝[000]^t；

(8)第i个控制周期时，若i≥nw，则：计算滚动力矩差均值meanx(i)，计算滚动力矩差方差stdx(i)，其中：j＝1～nw；若|dmde(1,i)-meanx(i)|>stdx(i)，则：将md_e2(i)的第1个元素赋值给δmd(i)的第1个元素；若|dmde(1,i)-meanx(i)|≤stdx(i)，则：将md_e1(i)的第1个元素赋值给δmd(i)的第1个元素；

(9)第i个控制周期时，若i≥nw，则：计算俯仰力矩差均值meany(i)，计算俯仰力矩差方差stdy(i)，其中：j＝1～nw；若|dmde(2,i)-meany(i)|>stdy(i)，则：将md_e2(i)的第2个元素赋值给δmd(i)的第2个元素；若|dmde(2,i)-meany(i)|≤stdy(i)，则：将md_e1(i)的第2个元素赋值给δmd(i)的第2个元素；

(10)第i个控制周期时，若i≥nw，则：计算偏航力矩差均值meanz(i)，计算偏航力矩差方差stdz(i)，其中：j＝1～nw；若|dmde(3,i)-meanz(i)|>stdz(i)，则：将md_e2(i)的第3个元素赋值给δmd(i)的第3个元素；若|dmde(3,i)-meanz(i)|≤stdz(i)，则：将md_e1(i)的第3个元素赋值给δmd(i)的第3个元素；

(11)第i个控制周期时，计算姿态控制力矩mc(i)，mc(i)＝tpid(i)+δmd(i)，其中：tpid(i)为pid控制力矩。

实施例

(1)着陆器惯量矩阵控制周期δt＝0.128。快变角速度系数l11＝0.9，快变干扰力矩系数l21＝0.5，慢变角速度系数l12＝0.9，慢变干扰力矩系数l22＝0.02。统计周期数nw＝50。

(2)控制周期数小于nw时，以控制周期数等于1为例。估计快变角速度ω_e1(1)＝[0.0090.180.27]^t、快变干扰力矩md_e1(1)＝[0.172.623.93]^t、慢变角速度ω_e2(1)＝[0.0090.180.27]^t、慢变干扰力矩md_e2(1)＝[0.0070.10.16]^t；力矩差历史值矩阵dmde的第1列为[0.1632.523.77]^t；计算姿态控制力矩mc(1)＝[0.010.070.08]^t。

(3)控制周期数大于等于nw时，以控制周期数等于50为例。快变角速度ω_e1(50)＝[0.050.220.32]^t，快变干扰力矩md_e1(50)＝[0.051526]^t，慢变角速度ω_e2(50)＝[0.060.20.3]^t，慢变干扰力矩md_e2(50)＝[0.021020]^t；力矩差历史值矩阵dmde的第50列为[0.0356]^t；滚动力矩差均值meanx(50)＝0.02，滚动力矩差方差stdx(50)＝0.06，俯仰力矩差均值meany(50)＝0.2，俯仰力矩差方差stdy(50)＝10，偏航力矩差均值meanz(50)＝0.3，偏航力矩差方差stdz(50)＝11，δmd(50)＝[0.051525]^t，pid控制力矩tpid(50)＝[0.010.40.5]^t。姿态控制力矩mc(50)＝[0.0615.425.5]^t。数学仿真表明，将姿态控制力矩作用于着陆器，抑制着陆器推进剂液体晃动，实现动力下降过程的高精度姿态控制。上述角速度的单位为°/s，力矩的单位为nm。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。