一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法

摘要: 本发明涉及航天工程与空间机器人,尤其涉及一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法。背景技术:1、空间非合作目标捕获技术的发展,使得翻滚目标姿...
一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法

本发明涉及航天工程与空间机器人,尤其涉及一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法。

背景技术:

1、空间非合作目标捕获技术的发展,使得翻滚目标姿态运动参数估计成为核心技术,受限于星载计算资源约束,高精度实时估计成为工程应用的关键前提。姿态估计精度直接决定空间机械臂捕获的成功率与安全性。

2、而现有滤波方法难以同时兼顾估计精度与计算效率,强非线性下误差大或实时性差,且缺乏严格的收敛性理论保障,无法满足在轨实时任务需求,因此亟需研发高效可靠的姿态估计算法,保障非合作目标操作顺利实施,支撑航天技术工程化应用。

技术实现思路

1、针对现有技术的不足,一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法,克服了现有空间非合作翻滚目标姿态估计方法难以兼顾高精度与实时性、对时变噪声鲁棒性差且缺乏收敛性理论保证的缺陷,有效解决强非线性下估计误差大、计算耗时高及太空复杂环境干扰的问题。

2、一方面,本发明提供一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标参数估计方法,包括以下步骤:

3、步骤1:翻滚目标数学建模:提出假设,推导姿态运动学微分方程与无外力矩姿态动力学方程,构建包含姿态四元数、三轴角速度和惯量参数的13维状态向量,建立非线性离散状态空间方程与线性观测方程;

4、所述假设具体为:

5、(1)忽略重力的影响,不考虑轨道动力学;

6、(2)非合作翻滚目标被视为具有2个太阳能电池板和1个天线的立方体报废卫星,其质心cm位于几何中心,以某设定初速度绕其任意轴旋转;

7、(3)翻滚目标处于自由漂浮状态,外力或力矩忽略不计;

8、(4)翻滚目标是刚性的;

9、(5)复杂太空环境引起的系统噪声为时变高斯噪声。

10、步骤1.1:定义惯性参考坐标系与目标本体坐标系,采用单位四元数表征目标姿态,推导得到姿态运动学微分方程:;

11、其中为姿态四元数,是姿态四元数的时间变化率;为角速度对应的反对称矩阵,为目标本体坐标系下的三轴角速度;

12、步骤1.2:基于角动量定理,结合目标自由漂浮无外力矩的假设,推导得到无外力矩姿态动力学方程:;

13、其中,为翻滚目标的角加速度矢量,为翻滚目标关于的惯量;

14、步骤1.3:将惯量矩阵的6个独立元素,即3个主惯量和3个惯量积作为待估计参数,构建13维状态向量:;

15、其中,为惯性变量;

16、步骤1.4:将姿态运动学微分方程、无外力矩姿态动力学方程与惯量参数不变特性结合,构建连续状态空间方程:;

17、其中,为连续状态变化率,为姿态四元数对应的状态分量,为角速度对应的状态分量,为惯量对应的状态分量,为包含微小空间碎片对空间目标的运动干扰和外层空间不稳定气流的建模误差。

18、步骤1.5:采用欧拉法对连续状态方程进行离散化,得到非线性离散状态空间方程:

19、;

20、其中,为第k离散时刻的系统状态向量,,为第k离散时刻的连续状态变化率,δt为滤波迭代步长,为第k时刻离散过程噪声;

21、步骤1.6:基于姿态四元数、角速度和惯量参数可由传感器直接测量的特性,构建线性观测方程:

22、;

23、其中ztk为连续时间下系统的观测向量,包含由传感器测得目标姿态、角速度、惯量信息。为13阶单位矩阵,为第k离散时刻的测量噪声;

24、步骤2:基础滤波算法设计:分别实现扩展卡尔曼滤波ekf的预测-更新迭代流程,以及无迹卡尔曼滤波ukf的预测-更新迭代流程,从而分别获得当前时刻的状态后验估计值和误差协方差后验估计值;

25、所述扩展卡尔曼滤波ekf的预测-更新迭代流程包括以下步骤:

26、步骤a2.1:设置初始状态后验估计值和初始误差协方差矩阵,定义过程噪声和观测噪声为零均值高斯白噪声,分别设置其协方差矩阵和;

27、步骤a2.2:非线性系统离散时间状态转移方程线性化为,其中,通过计算非线性状态转移函数在上一时刻后验估计值处的雅可比矩阵,得到k步到k+1步的状态转移矩阵;基于上一时刻的后验估计值,计算当前时刻的状态先验预测值和误差协方差先验预测值;

28、步骤a2.3:计算卡尔曼增益矩阵:;

29、其中为观测矩阵;

30、结合当前时刻的观测值更新得到当前时刻的状态后验估计值;误差协方差后验估计值,其中是第k步迭代的卡尔曼增益矩阵,离散观测函数为第k步的先验误差协方差矩阵;

31、所述无迹卡尔曼滤波ukf的预测-更新迭代流程包括以下步骤:

32、步骤b2.1:初始化:配置无迹变换缩放参数,κ是二次缩放参数,设为0,是无迹变换专门用于利用状态分布先验统计信息的调参量,设为0,计算缩放因子及sigma点权重:

33、;

34、其中,l为状态向量维数, 为第i个 sigma 点对应的均值权重,为第i个sigma 点对应的协方差权重;

35、步骤b2.2:无迹变换与状态预测:根据当前时刻的状态估计值和误差协方差矩阵,生成2l+1个sigma点:

36、;

37、其中,为第i个由无迹变换生成的sigma采样点,为状态向量的均值,为状态向量的误差协方差矩阵,()i表示取括号内矩阵的第i行;

38、将每个sigma点代入非线性状态转移函数进行传播,得到第i个 sigma 点经过一步状态转移后的预测值;计算状态先验预测值和误差协方差先验预测值;

39、步骤b2.3:观测预测:将传播后的sigma点代入观测函数;计算观测先验预测、观测误差协方差矩阵以及状态与观测的互相关矩阵;

40、步骤b2.4:结合当前时刻的观测值,更新得到当前时刻的状态后验估计值和误差协方差后验估计值;

41、步骤3:混合切换规则设计:提出基于误差协方差矩阵迹的动态切换策略,设定混合阈值系数与参考协方差矩阵,根据当前估计误差大小自适应选择滤波算法,连续执行ekf时更新参考协方差矩阵以加快收敛;

42、步骤3.1:预设混合策略核心参数:设定混合阈值系数用于控制ekf与ukf的执行比例;初始化参考协方差矩阵,作为估计误差大小判断的基准阈值;

43、步骤3.2:实时计算估计误差整体指标:在每一个离散周期的滤波迭代中,计算当前时刻后验误差协方差矩阵的迹,用于量化表征当前 13 维状态估计的整体误差水平;

44、步骤3.3:基于误差阈值的自适应算法切换:

45、如果,执行无迹卡尔曼滤波ukf的预测-更新迭代流程;

46、如果,执行扩展卡尔曼滤波ekf的预测-更新迭代流程;

47、步骤3.4:连续ekf执行时的参考协方差更新:当连续判定执行扩展卡尔曼滤波ekf的预测-更新迭代流程时,按照实时更新参考协方差矩阵,逐步降低混合阈值,维持ekf的持续执行状态;

48、步骤4:时变高斯噪声自适应处理:引入遗忘因子限制算法记忆长度,针对ekf与ukf分别设计对应的噪声统计特性实时更新公式,在线迭代估计过程噪声与观测噪声的均值和协方差矩阵,抑制太空环境不均匀光照与碎片干扰引起的时变噪声;

49、步骤4.1:建立时变噪声模型:将太空噪声假设为时变独立的高斯噪声,定义为:

50、;

51、;

52、其中,为过程噪声均值,为观测噪声,,过程噪声协方差矩阵,观测噪声均值,观测噪声协方差矩阵;

53、步骤4.2:引入遗忘因子机制:设置遗忘因子,取值范围为0.960-0.99,计算第k个离散时刻的自适应加权系数;

54、步骤4.3:当执行扩展卡尔曼滤波ekf的预测-更新迭代流程时,采用一阶线性化误差驱动的噪声统计特性更新公式:

55、;

56、;

57、;

58、;

59、当无迹卡尔曼滤波ukf的预测-更新迭代流程时,采用无迹变换误差驱动的噪声统计特性更新公式:

60、;

61、;

62、;

63、步骤4.4:每完成一次滤波迭代后,根据当前执行的预测-更新迭代流程,自动调用对应的噪声更新公式,在线迭代更新过程噪声和观测噪声的统计特性,用于下一次滤波迭代的预测和更新计算;通过令k=k+1循环执行,返回步骤2,实现对目标姿态运动参数的连续实时估计。

64、另一方面,所述一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标姿态运动参数估计方法,通过一种空间非合作目标在轨捕获系统实现对目标状态进行实时解算,包括视觉测量单元、星载计算单元、空间机械臂控制单元和目标捕获执行单元;

65、所述视觉测量单元获取翻滚目标的图像、点云原始观测数据,完成目标识别、特征提取;输出相对运动测量信息;

66、所述星载计算单元接收视觉测量单元输出的相对运动测量信息,解算得到翻滚目标高精度状态;执行任务逻辑判断、机械臂轨迹规划、基座姿态耦合动力学解算,生成机械臂期望运动轨迹,并把规划好的机械臂期望运动轨迹下发给空间机械臂控制单元;

67、所述空间机械臂控制单元接收星载计算单元下发的机械臂期望运动轨迹,输出捕获动作指令,管控机械臂本体;

68、所述目标捕获执行单元为机械臂末端执行器;接收空间机械臂控制单元输出的捕获动作指令,完成任务作业。

69、所述一种空间非合作目标在轨捕获系统通过自适应混合卡尔曼滤波动态切换扩展卡尔曼滤波ekf的预测-更新迭代流程,以及无迹卡尔曼滤波ukf的预测-更新迭代流程,在线自适应抑制太空时变测量噪声,实时输出绕任意轴旋转的翻滚目标的姿态四元数、三轴角速度与惯量参数,为空间机械臂的抓捕提供运动参数输入。

70、采用上述技术方案所产生的有益效果在于:

71、本发明提供一种基于自适应混合卡尔曼滤波的空间非合作翻滚目标姿态运动参数估计方法,包括以下有益效果:

72、1.本发明能够有效兼顾姿态估计精度与计算效率,结合扩展卡尔曼滤波的快速性与无迹卡尔曼滤波的高精度特性,姿态和角速度估计精度接近无迹卡尔曼滤波,次均cpu时间仅为其35.8%,解决了星载计算资源受限下的实时性难题。

73、2.本发明采用时变噪声自适应处理机制,引入遗忘因子实时更新噪声统计特性,相较于传统固定噪声滤波方法,大幅提升了太空不均匀光照、空间碎片干扰等复杂环境下的鲁棒性,有效抑制了时变测量噪声对估计精度的影响。

74、3.本发明已被严格证明了滤波器的局部收敛充分条件,填补了混合卡尔曼滤波收敛性理论空白,从数学上保证了算法不会发散,为工程应用提供了可靠的理论支撑。

75、4.本发明方法适用于绕任意轴旋转的刚性翻滚目标,克服了现有方法仅能处理绕惯性主轴旋转目标的局限,参数可根据任务需求灵活配置,工程易实现,可拓展应用于太空垃圾清理、在轨维修、交会对接等各类空间非合作目标操作任务。