CN111605737A - 一种航天器三超控制多级协同规划与敏捷机动方法 - Google Patents

Abstract

一种航天器三超控制多级协同规划与敏捷机动方法，适用于航天器相对运动控制领域。在追踪航天器与目标航天器相对姿态较大时，采用轨道外推获得追踪航天器和目标航天器的初始相对姿态，设计追踪航天器星体一级控制器实现敏捷机动以对目标航天器进行快速指向。当追踪航天器与目标航天器相对姿态较小时，通过光学相机进行载荷目标姿态规划。设计载荷二级姿态控制器，以光学载荷的测量信息为反馈，实现载荷光轴对目标航天器高精度指向控制。同时，针对追踪航天器星体和载荷控制器周期不同的问题，设计追踪航天器多级协同规划方法，利用卫星平台发送的姿态进行轨迹插值，实现载荷对目标姿态的高精度跟踪。

Description

一种航天器三超控制多级协同规划与敏捷机动方法

技术领域

本发明属于航天器姿态控制领域，涉及一种航天器三超控制多级协同规划与敏捷机动方法，能够有效实现追踪航天器高精度指向目标航天器的敏捷机动控制。

背景技术

当前航天器对跟踪控制提出了三超(超高精度、超高稳定度、超敏捷)的要求，目标航天器指向跟踪任务期间，要求追踪航天器中的光学载荷能够指向目标航天器，并进行高精度姿态跟踪与保持控制，以获取清晰稳定的目标航天器图像信息。而在整个跟踪过程中需要尽可能的保持追踪航天器y轴(太阳翼)指向太阳进行能力获取，满足整星能源供应要求；在整个追踪航天器指向目标航天器的过程中，追踪航天器和目标航天器都处于快速的运动状态，指向过程中存在指向速度快、指向精度高的需求以及相对姿态变化快的矛盾问题。为实现追踪航天器高精度指向目标航天器，追踪航天器往往采用星体一级控制系统和载荷二级控制系统。在追踪航天器敏捷机动指向目标航天器的动态过程中，如何实现追踪航天器两级系统的控制协调和敏捷机动动态规划，是实现对目标航天器高精度观测的前提。现有追踪航天器敏捷机动指向目标航天器控制方法存在以下不足：

1、难以实现追踪航天器动态轨迹规划

由于现有的追踪航天器对目标航天器观测时，更多的采用直接观测的方法，不进行对目标航天器指向控制。所设计轨迹规划方法更多的侧重于期望姿态固定的敏捷机动。而在整个追踪航天器指向目标航天器过程中，追踪航天器与目标航天器都处于实时运动状态，且对指向控制精度要求较高。现有的轨迹规划方法，难以满足这类指向控制精度要求高且相对姿态动态变化的需求。

2、难以实现两级控制系统的动态轨迹规划

现有在轨追踪航天器对目标航天器观测都采用星体平台一级系统，其轨迹规划方法仅能给出追踪航天器星体一级控制系统的目标轨迹规划方法。当追踪航天器实现对目标航天器的粗指向后，此时光学相机敏感器能够实时测量目标航天器的相对姿态。现有的轨迹规划方法，难以实现基于光学相机测量的载荷二级目标轨迹动态规划。

3、难以实现两级控制系统的控制协调规划

目前常规的追踪航天器姿态控制系统中只有星体一级姿态控制，不包含载荷二级姿态控制系统。追踪航天器在整星敏捷机动过程中，仅需规划出星体一级目标姿态。由于追踪航天器星体和载荷控制周期不同，星体控制周期远大于载荷控制周期，采用现有姿态规划方法仅能实现追踪航天器星体目标姿态规划，无法实现载荷二级目标姿态规划。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供了一种航天器三超控制多级协同规划与敏捷机动方法，能够实现追踪航天器整星敏捷机动过程中星体和载荷姿态的协同规划，并通过轨道外推信息以及光学相机测量信息实现追踪航天器两级系统的动态轨迹规划。

本发明的技术解决方案是：一种航天器三超控制多级协同规划与敏捷机动方法，包括如下步骤：

(1)建立追踪航天器星体-主动指向超静平台-载荷两级姿态动力学模型；

(2)通过轨道外推信息获得追踪航天器与目标航天器的相对姿态和相对角速度信息；

(3)计算指向过程初始时刻t₀时，追踪航天器机动欧拉轴、欧拉角以及机动时间；

(4)在时间t为：t₀≤t≤t₀+t_f时，计算追踪航天器指向目标航天器的实时目标姿态q_r，目标角速度ω_r和目标角加速度a_r，之后进入步骤(5)；其中，t_f为总机动时间；

(5)采用多项式插值计算载荷的目标姿态q_pr，载荷的目标角速度ω_pr和载荷的目标角加速度a_pr，之后进入步骤(10)；

(6)在时间t为：t>t₀+t_f时，计算追踪航天器载荷视线与目标航天器的相对位置和相对姿态在本体坐标系下的表达；相对姿态包括方位角α、俯仰角β；相对姿态角速度包括方位角速度ω_α和俯仰角速度ω_β；

(7)通过步骤(6)得到的相对姿态进行姿态判别，若成立的进入步骤(7)，否则返回步骤(2)，迭代计算追踪航天器指向目标航天器的目标姿态；

(8)引入光学相机的测量值，将相机的测量值方位角α_m，俯仰角β_m作为载荷二级控制的目标姿态；

(9)通过光学相机的测量值方位角α_m、俯仰角β_m以及载荷和星体之间的位移测量相对姿态θ_bpx、θ_bpy计算星体的目标姿态、和目标角速度，之后进入步骤(10)；

(10)计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]和角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]；计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]和角速度控制误差ω_errb＝[ω_errpx ω_errpy ω_errpz]；

(11)采用PID形式的控制器，计算星体姿态控制力矩，进行星体姿态控制；采用PID形式的控制器，计算载荷姿态控制力矩，进行载荷姿态控制，从而实现航天器三超控制多级协同规划与敏捷机动。

进一步的，所述步骤(1)建立追踪航天器星体-主动指向超静平台-载荷两级姿态动力学模型，采用状态方程表示为：

其中，

x为状态变量，由追踪航天器星体和载荷的广义位移

及广义速度

构成；A为系统矩阵，由追踪航天器惯性参数阵M、主动指向超静平台对追踪航天器星体和载荷质心的刚度阵K、阻尼阵C以及单位矩阵I构成；u为输入向量，由载荷控制向量u_p＝f_Lpa、追踪航天器星体控制量u_b＝[0,τ_bb]^T构成；τ_bb为星体三轴姿态控制力矩；f_Lpa为主动指向超静平台控制力；B为输入变换矩阵，由主动指向超静平台对追踪航天器星体质心的Jacobi阵J_b和对载荷质心的Jacobi阵J_p构成；C为输出矩阵；y为追踪航天器星体和载荷的广义位移和广义速度输出量。

进一步的，所述步骤(2)通过轨道外推信息获得追踪航天器与目标航天器的相对姿态和相对角速度信息，具体为：

追踪航天器与目标航天器之间的相对位置ρ_i与速度

表达如下：

ρ_i＝r_s-r_b

其中，r_b、v_b分别为追踪航天器在地心惯性系下位置矢量和速度矢量，r_s、v_s分别为目标航天器在地心惯性系下的位置矢量和速度矢量，ω_bi为追踪航天器本体坐标系相对于地心惯性系的角速度在惯性系下的表达；(ω_bi)^×为ω_bi反对称阵；

计算追踪航天器与目标航天器之间的指向初始时刻t₀的相对姿态：

追踪航天器指向目标航天器的目标姿态方向余弦阵表示为：

C_si＝[x y z]^T

其中，z＝ρ_i/||ρ_i||，y＝ρ_i×s_i/||ρ_i×s_i||，x＝y×z，s_i为太阳光线矢量在地心惯性系投影；

追踪航天器指向目标航天器的相对目标姿态在地心惯性系下的表达为：

q_sb＝dcm2quat(C_sb)

其中，dcm2quat为方向余弦阵与四元数之间的转换函数；q_sb为追踪航天器指向目标航天器的目标四元数，C_bi是航天器本体系与惯性系的方向余弦阵。

进一步的，所述步骤(3)计算指向过程初始时刻t₀时，追踪航天器机动欧拉轴、欧拉角以及机动时间，具体为：

(3-1)设置追踪航天器初始姿态四元数为q₀，目标姿态四元数为q_sb，则追踪航天器三轴机动的误差四元数表示为q_m：

其中，q_m＝[q_m1,q_m2,q_m3,q_m4]^T为机动误差四元数，其中q_m4为标量部分；

(3-2)机动时绕欧拉轴的转角θ_m计算为：

θ_m＝2arccos(q_m4)；

(3-3)机动的欧拉轴e＝[e_x,e_y,e_z]^T计算为：

(3-4)采用正弦路径规划方法对动态规划转角θ计进行规划：设计追踪航天器敏捷机动最大角加速度a_max＝3°/s²、敏捷机动最大角速度ω_max＝6°/s，动态规划转角θ，采用正弦路径规划方法对动态规划转角θ进行规划:

计算匀加/减速段时长t_a：t_a＝ω_max/2πa_max

计算匀速段时长t_c：t_c＝θ/2πa_maxt_a-t_a

计算总机动时间t_f：t_f＝t_c+2t_a。

进一步的，所述步骤(4)在时间t为：t₀≤t≤t₀+t_f时，计算追踪航天器指向目标航天器的实时目标姿态q_r，目标角速度ω_r和目标角加速度a_r，具体为：

(4-1)计算动态规划转角θ：

目标角加速度a：

目标角速度ω：

动态规划转角θ：

(4-2)计算追踪航天器本体坐标系下的目标姿态q_r、目标角速度ω_r和目标角加速度a_r，具体为：

进一步的，所述步骤(5)采用多项式插值计算载荷的目标姿态q_pr，载荷的目标角速度ω_pr，具体为(5-1)载荷姿态机动缓存序列计数标志N增加1；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[3][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；ω_pseq[k][N_max]＝ω_b(t)，θ_pseq[k][N_max]＝θ_b(t)，t_seq[N_max]＝t；

其中，ω_pseq[k][i]为载荷第k个轴第i时刻的缓存角速度序列，θ_pseq[k][i]为载荷第k个轴第i时刻的缓存姿态角序列，t_seq[k][i]为载荷第k个轴第i时刻对应的时间序列；N_max为载荷缓存序列中的最大个数；N为载荷缓存序列计数标志；

θ_b(t)＝quat2angle(q_r),ω_b(t)＝ω_r；

(5-2)判断载荷缓存序列计数标志N是否大于N_max，若N≤N_max，则进行步骤(5-1)；若N>N_max，则进行下一步；

(5-3)载荷姿态机动计数标志N保持不变；设置航天器机动时刻为t＝t+Δt₁，采用步骤(4)方法计算载荷在机动时刻t的目标角速度ω_pr(t)、目标姿态q_pr(t)；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[k][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；

ω_pseq[k][N_max]＝ω_br(t)，θ_pseq[k][N_max]＝quat2angle(q_r(t))，t_seq[N_max]＝t；其中quat2angle为姿态四元数转欧拉角标准函数；

(5-4)采用多项式插值法计算载荷控制的实时载荷目标姿态：

计算多项式中的各阶差商：

其中，t_p为载荷姿态机动序列数组t_seq中第p个值，t为当前实时间；

则N次拉格朗日插值多项式值P_N(t)解为：

其中，y_p为t_p时刻对应的载荷姿态机动序列数组；

将载荷姿态机动序列数组t_seq、以及ω_pseq[0][]、ω_pseq[1][]、ω_pseq[2][]、θ_pseq[0][]、θ_pseq[1][]、θ_pseq[2][]以及载荷机动当前实时间t代入上述多项式插值公式；采用ω_pseq[0][i]、ω_pseq[1][i]、ω_pseq[2][i]、替代多项式插值中的x_i,进行求解P_N(t)，即为载荷目标角速度ω_pr(t)的插值结果；采用θ_pseq[0][i]、θ_pseq[1][i]、θ_pseq[2][i]替代多项式插值中的x_i,进行求解P_N(t)，计算得到θ_pr，则载荷实时目标姿态q_pr(t)＝angle2quat(θ_pr)的插值结果。

进一步的，所述步骤(6)在时间t为：t>t₀+t_f时，计算追踪航天器载荷视线与目标航天器的相对位置和相对姿态在本体坐标系下的表达，具体为：

(6-1)计算追踪航天器与目标航天器之间的相对位置在本体坐标系下的表达为：

其中，ρ_b＝[x_b y_b z_b]^T为追踪航天器与目标航天器之间的相对位置在追踪航天器本体坐标系的表达；

为追踪航天器与目标航天器之间的相对速度在追踪航天器本体坐标系的表达；

(6-2)相对姿态计算为：

视线角表示为

视线角速度表示为

进一步的，所述通过步骤(7)得到的相对姿态进行姿态判别，具体为：

判断α≤α_min、β≤β_min、ω_α≤ω_αmin、ω_β≤ω_βmin是否均成立，若成立进行步骤(8)；否则，将t₀+t_f赋值为t₀，返回步骤(2)，迭代计算追踪航天器指向目标航天器的目标指标；其中，阈值α_min＝0.05°，β_min＝0.05°，ω_αmin＝0.1°/s，ω_βmin＝0.1°/s。

进一步的，所述步骤(9)通过光学相机的测量值方位角α_m、俯仰角β_m以及载荷和星体之间的位移测量相对姿态θ_bpx、θ_bpy计算星体的目标姿态、和目标角速度，具体为：

其中，θ_brx、θ_bry、θ_brz为星体的目标姿态，ω_brx、ω_bry、ω_brz为星体的目标角速度，θ_bz为由星体陀螺积分获得。

进一步的，计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]和角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]和角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为：

(10-1)在时间t为：t₀≤t≤t₀+t_f，计算星体姿态控制误差θ_errb＝[θ_errbx θ_errbyθ_errbz]、星体角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，具体为

其中，q_errb＝[q_errb1,q_errb2,q_errb3,q_errb4]^T为姿态控制误差四元数，其中q_errb4为标量部分；q_bm为星体姿态四元数，由星敏感器测量；

星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差ω_errb＝[ω_errbxω_errby ω_errbz]，计算为

θ_errbx＝q_errb1

θ_errby＝q_errb2

θ_errbz＝q_errb3

ω_errb＝ω_r-ω_bm

其中，ω_errb＝[ω_errbx,ω_errbyω_errbz]^T为角速度控制误差，ω_bm为星体角速度，由陀螺测量；

(10-2)在时间t为：t₀≤t≤t₀+t_f，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpyθ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为

其中，q_errp＝[q_errp1,q_errp2,q_errp3,q_errp4]^T为姿态控制误差四元数，其中q_errp4为标量部分；q_pm为载荷姿态四元数，由星敏感器测量；

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、角速度控制误差ω_errp＝[ω_errpxω_errpy ω_errpz]，具体为

θ_errpx＝q_errp1

θ_errpy＝q_errp2

θ_errpz＝q_errp3

ω_errp＝ω_pr-ω_pm

其中，ω_errp＝[ω_errpx,ω_errpxω_errpx]^T为载荷角速度控制误差，ω_pm为载荷角速度，由陀螺测量；

(10-3)在时间t为：t>t₀+t_f，计算星体和载荷姿态控制误差计算为星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差

ω_errb＝[ω_errbx ω_errby ω_errbz]，计算为

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为

进一步的，采用PID形式的控制器，计算姿态控制力矩，进行姿态控制，具体为：

星体姿态控制力矩为：

τ_bb＝I_bα_r+k_bpθ_errb+k_bdω_errb+k_bi∫θ_errb

其中，τ_bb为星体三轴姿态控制力矩；k_bp为星体三轴控制器的比例系数，k_bd为星体三轴控制器的微分系数，k_bi为星体三轴控制器的积分系数，I_b为星体惯量阵；

载荷姿态控制力矩为：

τ_pp＝k_ppθ_errp+k_pdω_errp+k_pi∫θ_errp

其中，τ_pp为载荷三轴姿态控制力矩；k_pp为载荷三轴控制器的比例系数，k_pd为载荷三轴控制器的微分系数，k_pi为载荷三轴控制器的积分系数。

本发明与现有技术相比的优点在于：

1、能够实现追踪航天器动态轨迹规划

由于现有的追踪航天器对目标航天器观测的轨迹规划方法侧重于期望姿态固定的敏捷机动规划。而在整个追踪航天器指向目标航天器过程中，追踪航天器与目标航天器都处于实时运动状态，且对指向控制精度要求较高。现有的轨迹规划方法，难以满足这类指向控制精度要求高且相对姿态动态变化的需求。本发明在考虑了追踪航天器光学载荷指向目标航天器、太阳翼尽可能对日等约束情况，通过判断敏捷机动后载荷视线指向目标航天器的方位角α和俯仰角β是否满足最小约束，进行了追踪航天器目标姿态的动态迭代规划方法，实现了追踪航天器指向目标航天器的动态轨迹规划。当追踪航天器实现对目标航天器的粗指向后，此时光学相机敏感器能够实时测量目标航天器的相对姿态。现有的轨迹规划方法，难以实现基于光学相机测量的载荷二级目标轨迹动态规划。本发明在光学相机有测量值，给出了载荷二级控制的目标姿态，在此基础上，通过光学相机的测量值方位角α_m，俯仰角β_m以及载荷和星体之间的位移测量的相对姿态θ_bpx，θ_bpy计算星体的目标姿态，实现了追踪航天器指向目标航天器的动态轨迹规划。

2、实现追踪航天器星体-载荷两级目标姿态解算与姿态控制

在目前的追踪航天器控制系统仿真模型中只有星体一级模型，不包含主动指向超静平台数学模型，难以实现载荷二级的目标姿态解算和载荷二级姿态控制。本发明利用轨道外推数据以及光学相机测量数据，设计了追踪航天器星体一级目标姿态和载荷二级目标姿态，通过粗精两级不同目标姿态的解算，实现星体-载荷两级姿态控制。受限于测量视场以及精度等原因，现有相对姿态测量敏感器难以满足大范围的相对姿态测量。本发明设计的基于追踪航天器与目标航天器之间的视线角和视线角速度预估的方法，实现大范围内相对姿态的解算。

3、能够实现星体和载荷对同一目标姿态高精度跟踪控制

载荷二级控制周期较短，通常在毫秒量级；而卫星平台的控制周期较长，通常在百毫秒量级。为了在载荷二级控制的每个控制周期内，以尽可能小的计算量获取与卫星平台同步且光滑的目标姿态，本法明设计了追踪航天器多级协同规划方法，利用卫星平台发送的姿态进行轨迹插值，设计了简洁的载荷目标姿态规划方法，能够实现载荷对目标姿态的高精度跟踪。

附图说明

图1为目标航天器指向的星体一级控制误差；

图2为载荷光轴指向视线角；

具体实施方式

本发明提出一种航天器三超控制多级协同规划与敏捷机动方法，适用于航天器相对运动控制领域。追踪航天器指向目标航天器时，存在指向速度快、指向精度高的需求以及相对姿态变化快的矛盾问题，所设计的航天器多级复合控制系统包括星体平台一级姿态控制、主动指向超静平台、载荷二级控制等部分。主动指向超静平台安装于载荷和星体平台之间。主动指向超静平台中安装位移敏感器，用于测量载荷和星体之间的相对姿态。在追踪航天器与目标航天器相对姿态较大时，采用轨道外推获得追踪航天器和目标航天器的初始相对姿态，设计追踪航天器星体一级控制器实现敏捷机动以对目标航天器进行快速指向。当追踪航天器与目标航天器相对姿态较小时，通过光学相机进行载荷目标姿态规划。设计载荷二级姿态控制器，以光学载荷的测量信息为反馈，实现载荷光轴对目标航天器高精度指向控制。同时，针对追踪航天器星体和载荷控制器周期不同的问题，设计追踪航天器多级协同规划方法，利用卫星平台发送的姿态进行轨迹插值，实现载荷对目标姿态的高精度跟踪。

具体的，本发明提出的航天器三超控制多级协同规划与敏捷机动方法包括步骤如下：

(1)建立追踪航天器星体-主动指向超静平台-载荷两级姿态动力学模型，采用状态方程表示为：

其中，

x为状态变量，由追踪航天器星体和载荷的广义位移

及广义速度

构成；A为系统矩阵，由追踪航天器惯性参数阵M、主动指向超静平台对追踪航天器星体和载荷质心的刚度阵K、阻尼阵C以及单位矩阵I构成；u为输入向量，由载荷控制向量u_p＝f_Lpa、追踪航天器星体控制量u_b＝[0,τ_bb]^T构成；τ_bb为星体三轴姿态控制力矩；f_Lpa为主动指向超静平台控制力；B为输入变换矩阵，由主动指向超静平台对追踪航天器星体质心的Jacobi阵J_b和对载荷质心的Jacobi阵J_p构成；C为输出矩阵；y为追踪航天器星体和载荷的广义位移和广义速度输出量。

本发明实施例给出：

M＝diag([M_p,M_b])；M_p＝diag(m_p,m_p,m_p,I_px,I_py,I_pz)，m_p为载荷质量，I_p＝diag(I_px,I_py,I_pz)分别为载荷x轴、y轴、z轴的惯量在载荷质心本体系下的表达。M_b＝diag(m_b,m_b,m_b,I_bx,I_by,I_bz)，m_b为星体质量，I_b＝diag(I_bx,I_by,I_bz)分别为星体x轴、y轴、z轴的惯量在星体质心本体系下的表达。

m_b＝3500kg,m_p＝100kg,

I_b＝diag(10000,10000,8000)kgm²,

I_p＝diag(140,140,130)kgm²,

I_f＝diag(0.001,0.001)kgm²。

K＝[K_pp，K_pb；K_bp，K_bb]；C＝[C_pp，C_pb；C_bp，C_bb]；

K_f0＝diag(k_f1,…,k_f4)，k_f1,…,k_f4为快反镜四个作动器的刚度系数。C_f0＝diag(c_f1,…,c_f4)，c_f1,…,c_f4为快反镜四个作动器的阻尼系数。k_f1＝k_f2＝…＝k_f4＝5(Nm/rad),c_f1＝c_f2＝…＝c_f4＝0.001(Nm/(rad/s))。

主动指向超静平台对星体的雅克比矩阵为

主动指向超静平台对载荷的雅克比矩阵为：

(2)通过轨道外推信息获得追踪航天器与目标航天器的相对姿态和相对角速度信息。

追踪航天器与目标航天器之间的相对位置ρ_i与速度

表达如下：

ρ_i＝r_s-r_b

其中，r_b、v_b分别为追踪航天器在地心惯性系下位置矢量和速度矢量，r_s、v_s分别为目标航天器在地心惯性系下的位置矢量和速度矢量，ω_bi为追踪航天器本体坐标系相对于地心惯性系的角速度在惯性系下的表达；(ω_bi)^×为ω_bi反对称阵；

计算追踪航天器与目标航天器之间的指向初始时刻t₀的相对姿态：

追踪航天器指向目标航天器的目标姿态方向余弦阵表示为：

C_si＝[x y z]^T

其中，z＝ρ_i/||ρ_i||，y＝ρ_i×s_i/||ρ_i×s_i||，x＝y×z，s_i为太阳光线矢量在地心惯性系投影；

追踪航天器指向目标航天器的相对目标姿态在地心惯性系下的表达为：

q_sb＝dcm2quat(C_sb)

其中，dcm2quat为方向余弦阵与四元数之间的转换函数；q_sb为追踪航天器指向目标航天器的目标四元数，C_bi是航天器本体系与惯性系的方向余弦阵。

(3)计算指向过程初始时刻t₀时，追踪航天器机动欧拉轴和欧拉角以及机动时间等参数。

具体为：

(3-1)设置追踪航天器初始姿态四元数为q₀，目标姿态四元数为q_sb，则追踪航天器三轴机动的误差四元数表示为q_m：

其中，q_m＝[q_m1,q_m2,q_m3,q_m4]^T为机动误差四元数，其中q_m4为标量部分；

(3-2)机动时绕欧拉轴的转角θ_m计算为：

θ_m＝2arccos(q_m4)；

(3-3)机动的欧拉轴e＝[e_x,e_y,e_z]^T计算为：

(3-4)采用正弦路径规划方法对动态规划转角θ计进行规划：设计追踪航天器敏捷机动最大角加速度a_max＝3°/s²、敏捷机动最大角速度ω_max＝6°/s，动态规划转角θ，采用正弦路径规划方法对动态规划转角θ进行规划:

计算匀加/减速段时长t_a：t_a＝ω_max/2πa_max

计算匀速段时长t_c：t_c＝θ/2πa_maxt_a-t_a

计算总机动时间t_f：t_f＝t_c+2t_a。

(4)在时间t为：t₀≤t≤t₀+t_f，计算追踪航天器指向目标航天器的实时目标姿态q_r，目标角速度ω_r和目标角加速度a_r。具体计算为：

(4-1)计算动态规划转角θ：

目标角加速度a：

目标角速度ω：

动态规划转角θ：

(4-2)计算追踪航天器本体坐标系下的目标姿态q_r、目标角速度ω_r和目标角加速度a_r，具体为：

(5)在时间t为：t₀≤t≤t₀+t_f，计算追踪航天器载荷指向目标航天器的实时目标姿态q_pr，目标角速度ω_pr。具体计算为：

(5-1)载荷姿态机动缓存序列计数标志N增加1；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[3][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；ω_pseq[k][N_max]＝ω_b(t)，θ_pseq[k][N_max]＝θ_b(t)，t_seq[N_max]＝t；

其中，ω_pseq[k][i]为载荷第k个轴第i时刻的缓存角速度序列，θ_pseq[k][i]为载荷第k个轴第i时刻的缓存姿态角序列，t_seq[k][i]为载荷第k个轴第i时刻对应的时间序列；N_max为载荷缓存序列中的最大个数；N为载荷缓存序列计数标志；

θ_b(t)＝quat2angle(q_r),ω_b(t)＝ω_r；

(5-2)判断载荷缓存序列计数标志N是否大于N_max，若N≤N_max，则进行步骤(5-1)；若N>N_max，则进行下一步；

(5-3)载荷姿态机动计数标志N保持不变；设置航天器机动时刻为t＝t+Δt₁，采用步骤(4)方法计算载荷在机动时刻t的目标角速度ω_pr(t)、目标姿态q_pr(t)；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[k][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；

ω_pseq[k][N_max]＝ω_br(t)，θ_pseq[k][N_max]＝quat2angle(q_r(t))，t_seq[N_max]＝t；其中quat2angle为姿态四元数转欧拉角标准函数；

(5-4)采用多项式插值法计算载荷控制的实时载荷目标姿态：

计算多项式中的各阶差商：

其中，t_p为载荷姿态机动序列数组t_seq中第p个值，t为当前实时间；

则N次拉格朗日插值多项式值P_N(t)解为：

其中，y_p为t_p时刻对应的载荷姿态机动序列数组；

将载荷姿态机动序列数组t_seq、以及ω_pseq[0][]、ω_pseq[1][]、ω_pseq[2][]、θ_pseq[0][]、θ_pseq[1][]、θ_pseq[2][]以及载荷机动当前实时间t代入上述多项式插值公式；采用ω_pseq[0][i]、ω_pseq[1][i]、ω_pseq[2][i]、替代多项式插值中的x_i,进行求解P_N(t)，即为载荷目标角速度ω_pr(t)的插值结果；采用θ_pseq[0][i]、θ_pseq[1][i]、θ_pseq[2][i]替代多项式插值中的x_i,进行求解P_N(t)，计算得到θ_pr，则载荷实时目标姿态q_pr(t)＝angle2quat(θ_pr)的插值结果。

(6)在时间t为：t>t₀+t_f时，计算追踪航天器载荷视线与目标航天器的相对姿态和相对角速度在本体系下的表达。相对姿态为方位角α，俯仰角β以及相对姿态角速度ω_α和ω_β。具体计算为：

(6-1)计算追踪航天器与目标航天器之间的相对位置在本体坐标系下的表达为：

其中，ρ_b＝[x_b y_b z_b]^T为追踪航天器与目标航天器之间的相对位置在追踪航天器本体坐标系的表达；

为追踪航天器与目标航天器之间的相对速度在追踪航天器本体坐标系的表达；

(6-2)相对姿态计算为：

视线角表示为

视线角速度表示为

(7)判断α≤α_min、β≤β_min、ω_α≤ω_αmin、ω_β≤ω_βmin是否均成立，若成立进行步骤(8)；否则，将t₀+t_f赋值为t₀，返回步骤(2)，迭代计算追踪航天器指向目标航天器的目标指标；其中，阈值α_min＝0.05°，β_min＝0.05°，ω_αmin＝0.1°/s，ω_βmin＝0.1°/s。

(8)引入光学相机的测量值，将相机的测量值方位角α_m，俯仰角β_m作为载荷二级控制的目标姿态。

(9)通过光学相机的测量值方位角α_m、俯仰角β_m以及载荷和星体之间的位移测量相对姿态θ_bpx、θ_bpy计算星体的目标姿态和目标角速度，具体为：

其中，θ_brx、θ_bry、θ_brz为星体的目标姿态，ω_brx、ω_bry、ω_brz为星体的目标角速度。θ_bz为由星体陀螺积分获得。

(10)计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]和角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]和角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为：

(10-1)在时间t为：t₀≤t≤t₀+t_f，计算星体姿态控制误差θ_errb＝[θ_errbx θ_errbyθ_errbz]、星体角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，具体为

其中，q_errb＝[q_errb1,q_errb2,q_errb3,q_errb4]^T为姿态控制误差四元数，其中q_errb4为标量部分；q_bm为星体姿态四元数，由星敏感器测量；

星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差ω_errb＝[ω_errbxω_errby ω_errbz]，计算为

θ_errbx＝q_errb1

θ_errby＝q_errb2

θ_errbz＝q_errb3

ω_errb＝ω_r-ω_bm

其中，ω_errb＝[ω_errbx,ω_errbyω_errbz]^T为角速度控制误差，ω_bm为星体角速度，由陀螺测量；

(10-2)在时间t为：t₀≤t≤t₀+t_f，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpyθ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为

其中，q_errp＝[q_errp1,q_errp2,q_errp3,q_errp4]^T为姿态控制误差四元数，其中q_errp4为标量部分；q_pm为载荷姿态四元数，由星敏感器测量；

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、角速度控制误差ω_errp＝[ω_errpxω_errpy ω_errpz]，具体为

θ_errpx＝q_errp1

θ_errpy＝q_errp2

θ_errpz＝q_errp3

ω_errp＝ω_pr-ω_pm

其中，ω_errp＝[ω_errpx,ω_errpxω_errpx]^T为载荷角速度控制误差，ω_pm为载荷角速度，由陀螺测量；

(10-3)在时间t为：t>t₀+t_f，计算星体和载荷姿态控制误差计算为星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，计算为

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为

(11)采用PID形式的控制器，计算星体和载荷姿态控制力矩，进行航天器多级协同规划和控制。具体为

星体姿态控制力矩具体为：

τ_bb＝I_bα_r+k_bpθ_errb+k_bdω_errb+k_bi∫θ_errb

其中，τ_bb为星体三轴姿态控制力矩；k_bp分别为星体三轴控制器的比例系数，k_bd分别为星体三轴控制器的微分系数，k_bi分别为星体三轴控制器的积分系数，I_b为星体惯量阵；

载荷姿态控制力矩具体为：

τ_pp＝k_ppθ_errp+k_pdω_errp+k_pi∫θ_errp

其中，τ_pp为载荷三轴姿态控制力矩；k_pp分别为载荷三轴控制器的比例系数，k_pd分别为载荷三轴控制器的微分系数，k_pi分别为载荷三轴控制器的积分系数。

实施例：

目标航天器指向的两级协同规划与敏捷机动性能校核。

定义载荷姿态机动序列数组长度N＝5，采用拉格朗日多项式插值法计算载荷控制周期时间的实时星体姿态：星体控制器、载荷控制器进行追踪航天器两级协同控制的目标航天器敏捷机动指向控制。图1给出了目标航天器指向过程中的动态轨迹规划。图2给出了在整个敏捷机动指向过程中追踪航天器星体和载荷两级的指向控制误差。通过星体一级控制能够实现对目标航天器约70角秒的指向控制，通过二级控制能够实现对目标航天器亚角秒级指向控制。

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

Claims (11)

1.一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于包括如下步骤：

(1)建立追踪航天器星体-主动指向超静平台-载荷两级姿态动力学模型；

(2)通过轨道外推信息获得追踪航天器与目标航天器的相对姿态和相对角速度信息；

(3)计算指向过程初始时刻t₀时，追踪航天器机动欧拉轴、欧拉角以及机动时间；

(4)在时间t为：t₀≤t≤t₀+t_f时，计算追踪航天器指向目标航天器的实时目标姿态q_r，目标角速度ω_r和目标角加速度a_r，之后进入步骤(5)；其中，t_f为总机动时间；

(5)采用多项式插值计算载荷的目标姿态q_pr，载荷的目标角速度ω_pr和载荷的目标角加速度a_pr，之后进入步骤(10)；

(6)在时间t为：t>t₀+t_f时，计算追踪航天器载荷视线与目标航天器的相对位置和相对姿态在本体坐标系下的表达；相对姿态包括方位角α、俯仰角β；相对姿态角速度包括方位角速度ω_α和俯仰角速度ω_β；

(7)通过步骤(6)得到的相对姿态进行姿态判别，若成立的进入步骤(7)，否则返回步骤(2)，迭代计算追踪航天器指向目标航天器的目标姿态；

(8)引入光学相机的测量值，将相机的测量值方位角α_m，俯仰角β_m作为载荷二级控制的目标姿态；

(9)通过光学相机的测量值方位角α_m、俯仰角β_m以及载荷和星体之间的位移测量相对姿态θ_bpx、θ_bpy计算星体的目标姿态、和目标角速度，之后进入步骤(10)；

(10)计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]和角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]；计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]和角速度控制误差ω_errb＝[ω_errpx ω_errpy ω_errpz]；

(11)采用PID形式的控制器，计算星体姿态控制力矩，进行星体姿态控制；采用PID形式的控制器，计算载荷姿态控制力矩，进行载荷姿态控制，从而实现航天器三超控制多级协同规划与敏捷机动。

2.根据权利要求1所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(1)建立追踪航天器星体-主动指向超静平台-载荷两级姿态动力学模型，采用状态方程表示为：

其中，

x为状态变量，由追踪航天器星体和载荷的广义位移

及广义速度

构成；A为系统矩阵，由追踪航天器惯性参数阵M、主动指向超静平台对追踪航天器星体和载荷质心的刚度阵K、阻尼阵C以及单位矩阵I构成；u为输入向量，由载荷控制向量u_p＝f_Lpa、追踪航天器星体控制量u_b＝[0,τ_bb]^T构成；τ_bb为星体三轴姿态控制力矩；f_Lpa为主动指向超静平台控制力；B为输入变换矩阵，由主动指向超静平台对追踪航天器星体质心的Jacobi阵J_b和对载荷质心的Jacobi阵J_p构成；C为输出矩阵；y为追踪航天器星体和载荷的广义位移和广义速度输出量。

3.根据权利要求2所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(2)通过轨道外推信息获得追踪航天器与目标航天器的相对姿态和相对角速度信息，具体为：

追踪航天器与目标航天器之间的相对位置ρ_i与速度

表达如下：

ρ_i＝r_s-r_b

其中，r_b、v_b分别为追踪航天器在地心惯性系下位置矢量和速度矢量，r_s、v_s分别为目标航天器在地心惯性系下的位置矢量和速度矢量，ω_bi为追踪航天器本体坐标系相对于地心惯性系的角速度在惯性系下的表达；(ω_bi)^×为ω_bi反对称阵；

计算追踪航天器与目标航天器之间的指向初始时刻t₀的相对姿态：

追踪航天器指向目标航天器的目标姿态方向余弦阵表示为：

C_si＝[x y z]^T

其中，z＝ρ_i/||ρ_i||，y＝ρ_i×s_i/||ρ_i×s_i||，x＝y×z，s_i为太阳光线矢量在地心惯性系投影；

追踪航天器指向目标航天器的相对目标姿态在地心惯性系下的表达为：

q_sb＝dcm2quat(C_sb)

其中，dcm2quat为方向余弦阵与四元数之间的转换函数；q_sb为追踪航天器指向目标航天器的目标四元数，C_bi是航天器本体系与惯性系的方向余弦阵。

4.根据权利要求3所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(3)计算指向过程初始时刻t₀时，追踪航天器机动欧拉轴、欧拉角以及机动时间，具体为：

(3-1)设置追踪航天器初始姿态四元数为q₀，目标姿态四元数为q_sb，则追踪航天器三轴机动的误差四元数表示为q_m：

其中，q_m＝[q_m1,q_m2,q_m3,q_m4]^T为机动误差四元数，其中q_m4为标量部分；

(3-2)机动时绕欧拉轴的转角θ_m计算为：

θ_m＝2arccos(q_m4)；

(3-3)机动的欧拉轴e＝[e_x,e_y,e_z]^T计算为：

(3-4)采用正弦路径规划方法对动态规划转角θ计进行规划：设计追踪航天器敏捷机动最大角加速度a_max＝3°/s²、敏捷机动最大角速度ω_max＝6°/s，动态规划转角θ，采用正弦路径规划方法对动态规划转角θ进行规划:

计算匀加/减速段时长t_a：t_a＝ω_max/2πa_max

计算匀速段时长t_c：t_c＝θ/2πa_maxt_a-t_a

计算总机动时间t_f：t_f＝t_c+2t_a。

5.根据权利要求4所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(4)在时间t为：t₀≤t≤t₀+t_f时，计算追踪航天器指向目标航天器的实时目标姿态q_r，目标角速度ω_r和目标角加速度a_r，具体为：

(4-1)计算动态规划转角θ：

目标角加速度a：

目标角速度ω：

动态规划转角θ：

(4-2)计算追踪航天器本体坐标系下的目标姿态q_r、目标角速度ω_r和目标角加速度a_r，具体为：

6.根据权利要求5所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(5)采用多项式插值计算载荷的目标姿态q_pr，载荷的目标角速度ω_pr，具体为

(5-1)载荷姿态机动缓存序列计数标志N增加1；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[3][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；ω_pseq[k][N_max]＝ω_b(t)，θ_pseq[k][N_max]＝θ_b(t)，t_seq[N_max]＝t；

其中，ω_pseq[k][i]为载荷第k个轴第i时刻的缓存角速度序列，θ_pseq[k][i]为载荷第k个轴第i时刻的缓存姿态角序列，t_seq[k][i]为载荷第k个轴第i时刻对应的时间序列；N_max为载荷缓存序列中的最大个数；N为载荷缓存序列计数标志；

θ_b(t)＝quat2angle(q_r),ω_b(t)＝ω_r；

(5-2)判断载荷缓存序列计数标志N是否大于N_max，若N≤N_max，则进行步骤(5-1)；若N>N_max，则进行下一步；

(5-3)载荷姿态机动计数标志N保持不变；设置航天器机动时刻为t＝t+Δt₁，采用步骤(4)方法计算载荷在机动时刻t的目标角速度ω_pr(t)、目标姿态q_pr(t)；载荷姿态机动序列数组依次左移一列，即ω_pseq[k][i]＝ω_pseq[k][i+1]，θ_pseq[k][i]＝θ_pseq[k][i+1]，t_seq[i]＝t_seq[i+1]，i＝1,2,N_max-1，k＝0,1,2；

ω_pseq[k][N_max]＝ω_br(t)，θ_pseq[k][N_max]＝quat2angle(q_r(t))，t_seq[N_max]＝t；其中quat2angle为姿态四元数转欧拉角标准函数；

(5-4)采用多项式插值法计算载荷控制的实时载荷目标姿态：

计算多项式中的各阶差商：

其中，t_p为载荷姿态机动序列数组t_seq中第p个值，t为当前实时间；

则N次拉格朗日插值多项式值P_N(t)解为：

其中，y_p为t_p时刻对应的载荷姿态机动序列数组；

将载荷姿态机动序列数组t_seq、以及ω_pseq[0][]、ω_pseq[1][]、ω_pseq[2][]、θ_pseq[0][]、θ_pseq[1][]、θ_pseq[2][]以及载荷机动当前实时间t代入上述多项式插值公式；采用ω_pseq[0][i]、ω_pseq[1][i]、ω_pseq[2][i]、替代多项式插值中的x_i,进行求解P_N(t)，即为载荷目标角速度ω_pr(t)的插值结果；采用θ_pseq[0][i]、θ_pseq[1][i]、θ_pseq[2][i]替代多项式插值中的x_i,进行求解P_N(t)，计算得到θ_pr，则载荷实时目标姿态q_pr(t)＝angle2quat(θ_pr)的插值结果。

7.根据权利要求6所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(6)在时间t为：t>t₀+t_f时，计算追踪航天器载荷视线与目标航天器的相对位置和相对姿态在本体坐标系下的表达，具体为：

(6-1)计算追踪航天器与目标航天器之间的相对位置在本体坐标系下的表达为：

其中，ρ_b＝[x_b y_b z_b]^T为追踪航天器与目标航天器之间的相对位置在追踪航天器本体坐标系的表达；

为追踪航天器与目标航天器之间的相对速度在追踪航天器本体坐标系的表达；

(6-2)相对姿态计算为：

视线角表示为

视线角速度表示为

8.根据权利要求7所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述通过步骤(7)得到的相对姿态进行姿态判别，具体为：

判断α≤α_min、β≤β_min、ω_α≤ω_αmin、ω_β≤ω_βmin是否均成立，若成立进行步骤(8)；否则，将t₀+t_f赋值为t₀，返回步骤(2)，迭代计算追踪航天器指向目标航天器的目标指标；其中，阈值α_min＝0.05°，β_min＝0.05°，ω_αmin＝0.1°/s，ω_βmin＝0.1°/s。

9.根据权利要求8所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：所述步骤(9)通过光学相机的测量值方位角α_m、俯仰角β_m以及载荷和星体之间的位移测量相对姿态θ_bpx、θ_bpy计算星体的目标姿态、和目标角速度，具体为：

其中，θ_brx、θ_bry、θ_brz为星体的目标姿态，ω_brx、ω_bry、ω_brz为星体的目标角速度，θ_bz为由星体陀螺积分获得。

10.根据权利要求9所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]和角速度控制误差ω_errb＝[ω_errbxω_errby ω_errbz]，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]和角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为：

(10-1)在时间t为：t₀≤t≤t₀+t_f，计算星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、星体角速度控制误差ω_errb＝[ω_errbx ω_errby ω_errbz]，具体为

其中，q_errb＝[q_errb1,q_errb2,q_errb3,q_errb4]^T为姿态控制误差四元数，其中q_errb4为标量部分；q_bm为星体姿态四元数，由星敏感器测量；

星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差ω_errb＝[ω_errbx ω_errbyω_errbz]，计算为

θ_errbx＝q_errb1

θ_errby＝q_errb2

θ_errbz＝q_errb3

ω_errb＝ω_r-ω_bm

其中，ω_errb＝[ω_errbx,ω_errbyω_errbz]^T为角速度控制误差，ω_bm为星体角速度，由陀螺测量；

(10-2)在时间t为：t₀≤t≤t₀+t_f，计算载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpx ω_errpy ω_errpz]，具体为

其中，q_errp＝[q_errp1,q_errp2,q_errp3,q_errp4]^T为姿态控制误差四元数，其中q_errp4为标量部分；q_pm为载荷姿态四元数，由星敏感器测量；

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、角速度控制误差ω_errp＝[ω_errpx ω_errpyω_errpz]，具体为

θ_errpx＝q_errp1

θ_errpy＝q_errp2

θ_errpz＝q_errp3

ω_errp＝ω_pr-ω_pm

其中，ω_errp＝[ω_errpx,ω_errpxω_errpx]^T为载荷角速度控制误差，ω_pm为载荷角速度，由陀螺测量；

(10-3)在时间t为：t>t₀+t_f，计算星体和载荷姿态控制误差计算为

星体姿态控制误差θ_errb＝[θ_errbx θ_errby θ_errbz]、角速度控制误差ω_errb＝[ω_errbx ω_errbyω_errbz]，计算为

载荷姿态控制误差θ_errp＝[θ_errpx θ_errpy θ_errpz]、载荷角速度控制误差ω_errp＝[ω_errpxω_errpy ω_errpz]，具体为

11.根据权利要求10所述的一种航天器三超控制多级协同规划与敏捷机动方法，其特征在于：采用PID形式的控制器，计算姿态控制力矩，进行姿态控制，具体为：

星体姿态控制力矩为：

τ_bb＝I_bα_r+k_bpθ_errb+k_bdω_errb+k_bi∫θ_errb

其中，τ_bb为星体三轴姿态控制力矩；k_bp为星体三轴控制器的比例系数，k_bd为星体三轴控制器的微分系数，k_bi为星体三轴控制器的积分系数，I_b为星体惯量阵；

载荷姿态控制力矩为：

τ_pp＝k_ppθ_errp+k_pdω_errp+k_pi∫θ_errp

其中，τ_pp为载荷三轴姿态控制力矩；k_pp为载荷三轴控制器的比例系数，k_pd为载荷三轴控制器的微分系数，k_pi为载荷三轴控制器的积分系数。

Images