基于mems惯导的双四元数动中通天线控制方法及系统的制作方法
【专利摘要】基于MEMS惯导的双四元数动中通天线控制方法，在导航姿态四元数基础上引入天线控制四元数。在导航计算机的每个中断周期，都用陀螺测得的载体系相对于理想平台坐标系的旋转矢量更新两种四元数。在每个滤波周期都用卡尔曼滤波修正导航姿态四元数的误差。根据由两种姿态四元数所确定的姿态之间的关系，确定天线控制指令角速度。最后由天线控制四元数姿态换算出的天线伺服控制角驱动伺服系统转动。基于MEMS惯导的双四元数动中通天线控制系统，以所述控制方法为控制流程，可以实现对动中通天线指向的精确控制。本发明可以有效避免卡尔曼滤波暂态过程中导航姿态四元数不稳定对动中通天线伺服系统带来的冲击，有效缩短动中通系统的对星时间。
【专利说明】基于MEMS惯导的双四元数动中通天线控制方法及系统
【技术领域】
[0001] 本发明涉及一种动中通天线控制方法。
【背景技术】
[0002] 同步卫星的移动通信应用系统俗称"动中通"，是当前卫星通信领域需求旺盛、发 展迅速的应用。"动中通"除了具有卫星通信覆盖区域广、不受地形地域限制、传输线路稳定 可靠的优点外，真正实现了宽带、移动通信的目的。
[0003] 目前，国内基于MEMS惯性导航（简称为MEMS惯导）的动中通系统尚且没有较为 成熟的方案。随着MEMS惯性技术的发展，MEMS惯导的精度稳步提升。目前国内动中通系 统应用越来越广泛，市场对降低动中通系统的成本提出了迫切需求。因此研究MEMS惯导对 动中通天线伺服系统的控制方法具有十分重要的意义。
[0004] 通常的做法，是利用由捷联惯导解算的导航姿态四元数直接控制天线，在卡尔曼 滤波组合导航过程中，状态估计若得到较大的误差估计量，那么在修正估计量的同时，会通 过给伺服系统瞬间输入非常大的电流来驱动伺服系统瞬间转动一个较大的角度（角度大 小为卡尔曼滤波估计得到的误差估计量），这样势必会对天线伺服系统造成较大的电学和 力学冲击。为了避免这种冲击，只能花费大量时间等待卡尔曼滤波稳定，直到伺服系统能够 承受由卡尔曼滤波估计的到的误差修正量所带来的冲击时再启动动中通伺服系统，显然这 是有悖于市场对动中通的快速对星需求的。
【发明内容】
[0005] 本发明的技术解决问题是：克服现有技术的不足，提供了一种基于MEMS惯导的双 四元数动中通天线控制方法及系统，通过在传统导航姿态四元数的基础上引入了新的天线 控制四元数，从而隔离了卡尔曼滤波修正姿态误差时对动中通天线伺服系统带来的冲击， 可以显著缩短动中通系统的对星时间。
[0006] 本发明的技术解决方案是：一种基于MEMS惯导的双四元数动中通天线控制方法， 包括如下步骤：
[0007] (1)在载体上同时安装MEMS惯导、GPS和动中通，其中MEMS惯导和GPS构成组合 导航系统；
[0008] (2)设定天线控制四元数，天线控制四元数的形式为[q' ^ q' i q' 2 q' 3]，天线控制四 元数中每个参数的含义与捷联惯导解算中获取的导航姿态四元数[% qi q2 q3]对应一致，天 线控制四元数的初值与导航姿态四元数相同；
[0009] (3)在捷联惯导导航计算机的每个中断周期里，用载体系相对于理想平台坐标系 的旋转矢量，分别更新导航姿态四元数和天线控制四元数；
[0010] (4)在所述组合导航系统的每个滤波周期内，利用卡尔曼滤波组合导航算法修正 MEMS惯导的导航姿态中的水平姿态误差，从而修正导航姿态四元数；
[0011] (5)在捷联惯导导航计算机的每个中断周期里，将由导航姿态四元数确定的载体 姿态角与由天线控制四元数确定的载体姿态角对应相减，得到姿态角差值，并根据姿态角 差值产生用于校正天线控制四元数的三轴指令角速度旋转矢量，具体为：
[0012] a.若由天线控制四元数确定的航向角大于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取正的修正指令角速度；
[0013] b.若由天线控制四元数确定的航向角小于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取负的修正指令角速度；
[0014] c.若由天线控制四元数确定的俯仰角大于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取正的修正指令角速度；
[0015] d.若由天线控制四元数确定的俯仰角小于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取负的修正指令角速度；
[0016] e.若由天线控制四元数确定的横滚角大于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取正的修正指令角速度；
[0017] f.若由天线控制四元数确定的横滚角小于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取负的修正指令角速度；
[0018] (6)利用三轴指令角速度旋转矢量校正天线控制四元数，并在校正以后的下一个 捷联惯导导航计算机的中断周期，利用校正后的天线控制四元数，解算得到动中通天线的 伺服方位角、伺服仰角和伺服极化角，由此获得三个姿态方向所对应的控制量控制动中通 天线转动。
[0019] GPS:测量获取载体的速度和位置信息并送至动中通天线控制器中的滤波单元；
[0020] MEMS陀螺：测量获取载体在三维空间内的角速度信息并送至动中通天线控制器 中的惯导解算单元和天线控制四元数计算单元；
[0021] MEMS加速度计：测量获取载体在三维空间内的比力信息并送至动中通天线控制 器中的惯导解算单元；
[0022] 动中通天线控制器：包括惯导解算单元、滤波单元、天线控制四元数计算单元、天 线控制指令生成单元、天线控制四元数校正指令角速度生成单元，其中 ：
[0023] 惯导解算单元：将MEMS陀螺测量获取的载体在三维空间内的角速度信息，扣除由 地球自转、载体沿地球表面运动带来的角速度后，得到载体坐标系相对于地理坐标系的三 轴旋转矢量 ;将MEMS加速度计测量获取的载体在三维空间内的比力信息，扣除重力加速 度、哥氏加速度后，得到载体的对地加速度；利用载体坐标系相对于地理坐标系的三轴旋转 角速度和载体的对地加速度，经过惯导解算得到载体的姿态、位置和速度信息并送至滤波 单元；将载体坐标系相对于地理坐标系的三轴旋转矢量ΛΛ,.以及首次惯导解算直接得到的 载体姿态所对应的姿态四元数[q0 q2 q3]送至天线控制四元数计算单元；从滤波单元获取 修正后的载体姿态信息，利用载体坐标系相对于地理坐标系的三轴旋转矢量更新与修 正后的载体姿态信息所对应的姿态四元数作为导航姿态四元数送至天线控制四元数校正 指令角速度生成单元；
[0024] 滤波单元：利用GPS输出的载体速度和位置信息，以及惯导解算单元输出的载体 速度和位置信息，通过卡尔曼滤波组合导航算法，以固定的滤波周期修正惯导解算单元输 出载体姿态中的水平姿态误差并将修正后的结果送至惯导解算单元；
[0025] 天线控制四元数计算单元：生成天线控制四元数，所述的天线控制四元数的形式 为[q' ^ q' i q' 2 q' 3]，天线控制四元数中每个参数的含义与惯导解算单元获取的姿态四元数 [qQ qi q2 q3]对应一致，且[q' Q q' i q' 2 q' 3]的初值为[qQ qi q2 q3];从惯导解算单元每接收到 一次载体坐标系相对于地理坐标系的三轴旋转矢量狀，就利用载体坐标系相对于地理坐 标系的三轴旋转矢量以更新天线控制四元数[q' 0 q' i q' 2 q' 3]并送至天线控制四元数校正 指令角速度生成单元；从天线控制四元数校正指令角速度生成单元获取三轴指令角速度旋 转矢量，并用所述三轴指令角速度旋转矢量再次更新天线控制四元数[q' 0 q' i q' 2 3]并送 至天线控制指令生成单元；
[0026] 天线控制四元数校正指令角速度生成单元：分别从惯导解算单元和天线控制四元 数计算单元获取导航姿态四元数和天线控制四元数，将由导航姿态四元数确定的载体姿态 角与由天线控制四元数确定的载体姿态角对应相减，得到姿态角差值，并根据姿态角差值 生成用于校正天线控制四元数的三轴指令角速度旋转矢量并送至天线控制四元数计算单 元，三轴指令角速度旋转矢量中各元素的取值方法如下：
[0027] a.若由天线控制四元数确定的航向角大于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取正的修正指令角速度；
[0028] b.若由天线控制四元数确定的航向角小于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取负的修正指令角速度；
[0029] c.若由天线控制四元数确定的俯仰角大于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取正的修正指令角速度；
[0030] d.若由天线控制四元数确定的俯仰角小于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取负的修正指令角速度；
[0031] e.若由天线控制四元数确定的横滚角大于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取正的修正指令角速度；
[0032] f.若由天线控制四元数确定的横滚角小于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取负的修正指令角速度；
[0033] 天线控制指令生成单元：从天线控制四元数计算单元接收最新的天线控制四元 数，根据天线控制四元数解算得到动中通天线的伺服方位角、伺服仰角和伺服极化角，送至 动中通天线伺服机构；
[0034] 动中通天线伺服机构：包括方位向、俯仰向和极化向的电机驱动器和相应的电机， 三个方向的电机驱动器根据天线控制指令生成单元传来的伺服方位角、伺服仰角和伺服极 化角分别驱动相应方向的电机，由此控制动中通天线的三轴转动。
[0035] 所述的修正指令角速度，在a和b两种情况下，大小至少是天线控制四元数确定的 航向角与导航姿态四元数确定的航向角之差再除以组合导航滤波周期，并且不大于动中通 天线每秒所允许的最大对星角度误差；在c和d两种情况下，大小至少是天线控制四元数确 定的俯仰角与导航姿态四元数确定的俯仰角之差再除以组合导航滤波周期，并且不大于动 中通天线每秒所允许的最大对星角度误差；在e和f两种情况下，大小至少是天线控制四元 数确定的横滚角与导航姿态四元数确定的横滚角之差再除以组合导航滤波周期，并且不大 于动中通天线每秒所允许的最大对星角度误差。
[0036] 本发明与现有技术相比的优点在于：
[0037] (1)本发明方法中引入了天线控制四元数。由于组合导航算法对导航姿态四元数 的误差修正使得由导航姿态四元数解算得到的姿态产生跳跃，这种跳跃将会对天线伺服系 统带来冲击。独立于导航姿态四元数的天线控制四元数隔离了组合导航误差修正时导航姿 态四元数震荡所导致的天线伺服系统震荡，从而保证了在组合导航算法对惯导误差进行修 正时，天线伺服系统的平稳运行；
[0038] (2)本发明方法中，通过利用天线控制四元数以较小的指令角速度逼近导航姿态 四元数，初始时刻可用导航姿态四元数初值直接初始化天线控制四元数，天线控制四元数 被初始化后可立刻驱动天线完成对星功能；天线控制四元数初始化完成后，即可由天线控 制四元数独立控制天线伺服系统平滑的转动，隔离导航姿态误差修正对天线伺服系统带来 的冲击，从而使天线始终稳定的对准卫星；
[0039] (3)本发明系统中，惯导解算单元采集MEMS陀螺和MEMS加表测量的载体转动角速 率信息和加速度信息，完成惯导解算；滤波单元通过采集GPS的速度信息和位置信息完成 对惯导解算的误差修正；天线控制四元数计算单元隔离了滤波单元对惯导解算进行误差修 正时对天线伺服系统带来的冲击；天线控制四元数校正指令角速度生成单元通过对导航姿 态四元数对应的姿态和天线控制四元数对应的姿态进行比较，产生了用于修正天线控制四 元数的三轴指令角速度旋转矢量；天线控制指令生成单元通过实时解算天线控制四元数对 应的姿态来进一步计算动中通天线伺服系统所需要的伺服方位角、伺服仰角、伺服极化角， 进而通过动中通天线伺服机构完成对动中通天线的实时控制；实现了动中通天线的实时、 平滑、稳定控制，提高了对星精度。
【专利附图】
【附图说明】
[0040] 图1为本发明方法的原理框图；
[0041] 图2为天线指向偏差随时间变化的曲线示意图；
[0042] 图3为本发明系统的组成原理框图。
【具体实施方式】
[0043] 如图1所示，为本发明方法的原理图。本发明方法采用导航姿态四元数和天线控 制四元数协同控制载体上的动中通的天线伺服系统。
[0044] 本发明方法中引入天线控制四元数的概念。天线控制四元数的形式为[q' ^ q' i q' 2 q' 3]，每个参数的含义与捷联惯导解算中的四元数[q0 q2 q3]对应一致。在导航初始时刻， 天线控制四元数与导航姿态四元数相等。天线控制四元数在每个导航周期需要经历两次更 新，一次由载体系b相对于理想平台系T的旋转矢量在载体系b下旋转矢量更新，一次由恒 定的三轴小指令角速度更新。第一次更新用于跟踪载体姿态变化。第二次更新的目的是使 天线控制四元数虚拟的数学平台以很小的角速度追赶导航姿态四元数虚拟的数学平台，而 不因瞬间较大幅度地修正导航姿态四元数使得天线伺服姿态角发生剧烈变化。
[0045] 天线控制四元数和导航姿态四元数的相同之处是它们都用同样的旋转矢量更新 来跟踪载体姿态变化。它们的不同之处是导航姿态四元数用卡尔曼滤波在到达滤波时间时 一次性修正估计误差（会冲击天线伺服系统），而天线控制四元数则以较小的指令角速度 缓慢逼近导航姿态四元数（不会冲击伺服系统）。
[0046] 如图2所示，是天线指向偏差随时间变化的曲线示意图。实线AEBGCID表示假设 由导航姿态四元数控制天线转动时，天线指向误差曲线。虚线AEFGHI表示由天线控制四元 数控制天线转动时，天线的指向偏差曲线。AB、BC、CD分别表示组合导航算法对导航姿态四 元数的误差修正周期。在A点处，天线控制四元数和导航姿态四元数同时初始化。AE段实 线虚线重合表示用相同的旋转矢量同时更新导航姿态四元数和天线控制四元数，由于 导航姿态四元数和天线控制四元数初值相同，因此在ΑΒ段内，二者始终相等，所以ΑΕ段实 线和虚线重合。EB(GC、ID)段实线表示组合导航算法对导航姿态四元数进行修正，导航姿 态误差归零，由导航姿态四元数控制的天线指向误差由非零值瞬间变为0,对天线伺服系统 带来震荡。BG(CI)段实线表示随着时间推移和惯性器件误差累积，天线指向偏差逐渐增大。 EF(GH)虚线表示天线控制四元数姿态以较小的指令角速度缓慢追赶导航姿态四元数的过 程。FG(HI)虚线表示天线控制四元数姿态与导航姿态四元数相等后，二者以相同的旋转矢 量更新，二者对应的天线指向误差发展状况相同。
[0047] 本发明方法的主要步骤如下：
[0048] (1)设置天线控制四元数的初值。因独立设计天线控制四元数的目的是隔离载体 姿态误差修正时对动中通伺服系统带来的力学和电学冲击，天线控制四元数实际上和导航 姿态四元数一样描述了载体姿态，因此在系统启动时刻，可使其与捷联惯导解算四元数法 中获取的导航姿态四元数相同。
[0049] (2)在捷联惯导的导航计算机的每个中断周期里，用载体系相对于理想平台坐标 系的旋转矢量，分别更新导航姿态四元数和天线控制四元数。
[0050] (3)在每个滤波周期内，利用卡尔曼滤波修正MEMS惯导的导航姿态四元数，速度 和位置，从而保证MEMS惯导的长时间导航精度。
[0051] (4)在每个中断周期，将由导航姿态四元数确定的载体姿态角与由天线控制四元 数确定的载体姿态角对应相减，得到姿态角的差值；
[0052] (5)在每个中断周期，根据姿态角的差值确定天线控制方式；基本原则是以天线 控制四元数逼近导航姿态四元数，在此引入用于校正天线控制四元数的三轴指令角速度旋 转矢量，分为如下几种情况 ：
[0053] a.若由天线控制四元数确定的航向角大于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取正的修正指令角速度；
[0054] b.若由天线控制四元数确定的航向角小于由导航姿态四元数确定的航向角，则三 轴指令角速度旋转矢量的第三个元素取负的修正指令角速度；
[0055] 修正指令角速度的大小取值方法为：为了使修正天线控制四元数带给天线伺服系 统的冲击量达到最小，三轴指令角速度旋转矢量的第三个元素的修正指令角速度的大小为 天线控制四元数航向角与导航姿态四元数航向角之差除以组合导航滤波周期，当该指令角 速度的大小为天线控制四元数航向角与导航姿态四元数航向角之差除以组合导航滤波周 期时，该指令角速度恰好使天线控制四元数的航向角误差与导航姿态四元数的航向角误差 最大值相等。考虑到系统运行中的不确定因素，应使该指令角速度大于天线控制四元数航 向角与导航姿态四元数航向角之差除以组合导航滤波周期，但其最大值不应超过每秒动中 通天线允许的最大对星角度误差。
[0056] c.若由天线控制四元数确定的俯仰角大于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取正的修正指令角速度；
[0057] d.若由天线控制四元数确定的俯仰角小于由导航姿态四元数确定的俯仰角，则三 轴指令角速度旋转矢量的第一个元素取负的修正指令角速度；
[0058] 上述两种情况下修正指令角速度的大小取值方法与a，b两种情况原理相同，不同 的是由于是第一个元素，与其对应的为俯仰角，因此应将a，b两种情况对应的航向角替换 为俯仰角进行计算即可。
[0059] e.若由天线控制四元数确定的横滚角大于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取正的修正指令角速度；
[0060] f.若由天线控制四元数确定的横滚角小于由导航姿态四元数确定的横滚角，则三 轴指令角速度旋转矢量的第二个元素取负的修正指令角速度；
[0061] 上述两种情况下修正指令角速度的大小取值方法与a，b两种情况原理相同，不同 的是由于是第二个元素，与其对应的为横滚角，因此应将a，b两种情况对应的航向角替换 为横滚角进行计算即可。
[0062] (6)根据天线控制四元数对应的姿态角计算动中通天线的伺服方位角、伺服仰角、 伺服极化角，驱动动中通伺服系统对天线进行控制。
[0063] 如图3所示，为本发明基于MEMS惯导的双四元数动中通天线控制系统的组成原理 图。主要包括：动中通天线控制器、GPS、MEMS陀螺、MEMS加速度计和动中通天线伺服机构。
[0064] GPS主要是测量获取载体的速度和位置信息并送至动中通天线控制器中的滤波单 J Li 〇
[0065] MEMS陀螺主要是测量获取载体在三维空间内的角速度信息并送至动中通天线控 制器中的惯导解算单元和天线控制四元数计算单元。
[0066] MEMS加速度计主要是测量获取载体在三维空间内的比力信息并送至动中通天线 控制器中的惯导解算单元。
[0067] 动中通天线伺服机构：包括方位向、俯仰向和极化向的电机驱动器和相应的电机， 三个方向的电机驱动器根据天线控制指令生成单元传来的伺服方位角、伺服仰角和伺服极 化角分别驱动相应方向的电机，由此控制动中通天线的三轴转动。
[0068] 动中通天线控制器是本发明系统的核心部分，主要包括惯导解算单元、滤波单元、 天线控制四元数计算单元、天线控制指令生成单元、天线控制四元数校正指令角速度生成 单元，其中：
[0069] 惯导解算单元：将MEMS陀螺测量获取的载体在三维空间内的角速度信息，扣除由 地球自转、载体沿地球表面运动带来的角速度后，得到载体坐标系相对于地理坐标系的三 轴旋转矢量将MEMS加速度计测量获取的载体在三维空间内的比力信息，扣除重力加速 度、哥氏加速度后，得到载体的对地加速度；利用载体坐标系相对于地理坐标系的三轴旋转 角速度和载体的对地加速度，经过惯导解算得到载体的姿态、位置和速度信息并送至滤波 单元；将载体坐标系相对于地理坐标系的三轴旋转矢量以及首次惯导解算直接得到的 载体姿态所对应的姿态四元数[q0 q2 q3]送至天线控制四元数计算单元；从滤波单元获取 修正后的载体姿态信息，利用载体坐标系相对于地理坐标系的三轴旋转矢量ΛΛ更新与修 正后的载体姿态信息所对应的姿态四元数作为导航姿态四元数送至天线控制四元数校正 指令角速度生成单元。
[0070] 滤波单元：将GPS输出的载体速度和位置信息，以及惯导解算单元输出的载体速 度和位置信息进行组合，将GPS获取的载体的东向速度和北向速度，以及惯导解算单元输 出的载体的东向速度和北向速度分别做差，将两个差值构成卡尔曼滤波的量测量，通过卡 尔曼滤波组合导航算法，以固定的滤波周期修正惯导解算单元输出的载体姿态信息并将修 正后的结果送至惯导解算单元。卡尔曼滤波组合导航算法具体可参见2012年西北工业 大学出版社出版的，由秦永元、张洪钺、王淑华编著的《卡尔曼滤波与组合导航原理（第二 版）》一书。本发明中，选取其中东向和北向速度误差、东向和北向失准角、右向和前向陀螺 的漂移，以及右向和前向加速度计的偏置量这八个量作为状态变量。
[0071] 天线控制四元数计算单元：生成天线控制四元数，所述的天线控制四元数的形式 为[q' ^ q' i q' 2 q' 3]，天线控制四元数中每个参数的含义与惯导解算单元获取的姿态四元数 [qQ qi q2 q3]对应一致，且[q' Q q' i q' 2 q' 3]的初值为[qQ qi q2 q3];从惯导解算单元每接收到 一次载体坐标系相对于地理坐标系的三轴旋转矢量就利用载体坐标系相对于地理坐 标系的三轴旋转矢量更新天线控制四元数[q' 0 q' i q' 2 q' 3]并送至天线控制四元数校正 指令角速度生成单元；从天线控制四元数校正指令角速度生成单元获取三轴指令角速度旋 转矢量，并用所述三轴指令角速度旋转矢量再次更新天线控制四元数[q' 0 q' i q' 2 3]并送 至天线控制指令生成单元。
[0072] 天线控制四元数校正指令角速度生成单元：分别从惯导解算单元和天线控制四元 数计算单元获取导航姿态四元数和天线控制四元数，将由导航姿态四元数确定的载体姿态 角与由天线控制四元数确定的载体姿态角对应相减，得到姿态角差值，并根据姿态角差值 生成用于校正天线控制四元数的三轴指令角速度旋转矢量并送至天线控制四元数计算单 J Li 〇
[0073] 天线控制指令生成单元：从天线控制四元数计算单元接收最新的天线控制四元 数，根据天线控制四元数解算得到动中通天线的伺服方位角、伺服仰角和伺服极化角，送至 动中通天线伺服机构。
[0074] 在惯导解算单元中运行的主要是捷联惯导算法。在捷联惯导解算算法中，分别进 行了姿态解算、速度解算、位置解算。其中，姿态信息的具体数学载体是导航姿态四元数；导 航姿态四元数是天线控制四元数变化的参考量；速度信息用于与GPS得到的速度信息构成 滤波单元的滤波量测量；通过位置信息和天线控制四元数对应的姿态信息来计算动中通天 线的伺服方位角、伺服仰角和伺服极化角。
[0075] 伺服方位角、伺服仰角和伺服极化角的计算方法如下：
[0076] 计算中有导航系n，载体坐标系b，天线坐标系v和地球坐标系e。其中导航系η取 地理坐标系（X-东，y-北，Ζ-天）；载体坐标系的X轴、y轴、ζ轴分别指向载体的右、前、上； 天线坐标系v中y轴与天线指向一致，Z轴指向天线方位轴向上，X轴与另外两轴构成右手 系；地球坐标系e，原点位于地心，x轴穿越本初子午线与赤道的交点，ζ轴穿越地球北极点， y轴穿越东经90°子午线与赤道的交点，该坐标系与地球固连。
[0077] 根据上述坐标系的定义，可以方便的计算出各坐标系之间的转换矩阵：天线坐标 系至载体坐标系的转换矩阵为 <，载体坐标系至导航坐标系的转换矩阵为?：，导航坐标系 至载体坐标系的转换矩阵为，天线坐标系至导航坐标系的转换矩阵为，地球坐标系至 导航坐标系的转换矩阵为c：。
[0078] 对于天线伺服方位角和伺服仰角，可由卫星经度λ s得到卫星在地球直角坐标系 下的坐标(I： f <)，同时易得载体在地球直角坐标系下的坐标为ρ? κ z〗），则载 体到卫星的矢量为
【权利要求】
1. 基于MEMS惯导的双四元数动中通天线控制方法，其特征在于包括如下步骤： ⑴在载体上同时安装MEMS惯导、GPS和动中通，其中MEMS惯导和GPS构成组合导航 系统； ⑵设定天线控制四元数，天线控制四元数的形式为[q' ^ q' i q' 2 q' 3]，天线控制四元数 中每个参数的含义与捷联惯导解算中获取的导航姿态四元数[% q2 q3]对应一致，天线控 制四元数的初值与导航姿态四元数相同； (3) 在捷联惯导导航计算机的每个中断周期里，用载体系相对于理想平台坐标系的旋 转矢量，分别更新导航姿态四元数和天线控制四元数； (4) 在所述组合导航系统的每个滤波周期内，利用卡尔曼滤波组合导航算法修正MEMS 惯导的导航姿态中的水平姿态误差，从而修正导航姿态四元数； (5) 在捷联惯导导航计算机的每个中断周期里，将由导航姿态四元数确定的载体姿态 角与由天线控制四元数确定的载体姿态角对应相减，得到姿态角差值，并根据姿态角差值 产生用于校正天线控制四元数的三轴指令角速度旋转矢量，具体为： a. 若由天线控制四元数确定的航向角大于由导航姿态四元数确定的航向角，则三轴指 令角速度旋转矢量的第三个元素取正的修正指令角速度； b. 若由天线控制四元数确定的航向角小于由导航姿态四元数确定的航向角，则三轴指 令角速度旋转矢量的第三个元素取负的修正指令角速度； c. 若由天线控制四元数确定的俯仰角大于由导航姿态四元数确定的俯仰角，则三轴指 令角速度旋转矢量的第一个元素取正的修正指令角速度； d. 若由天线控制四元数确定的俯仰角小于由导航姿态四元数确定的俯仰角，则三轴指 令角速度旋转矢量的第一个元素取负的修正指令角速度； e. 若由天线控制四元数确定的横滚角大于由导航姿态四元数确定的横滚角，则三轴指 令角速度旋转矢量的第二个元素取正的修正指令角速度； f. 若由天线控制四元数确定的横滚角小于由导航姿态四元数确定的横滚角，则三轴指 令角速度旋转矢量的第二个元素取负的修正指令角速度； (6) 利用三轴指令角速度旋转矢量校正天线控制四元数，并在校正以后的下一个捷联 惯导导航计算机的中断周期，利用校正后的天线控制四元数，解算得到动中通天线的伺服 方位角、伺服仰角和伺服极化角，由此获得三个姿态方向所对应的控制量控制动中通天线 转动。
2. 根据权利要求1所述的基于MEMS惯导的双四元数动中通天线控制方法，其特征在 于：所述步骤（5)中的修正指令角速度，在a和b两种情况下，大小至少是天线控制四元数 确定的航向角与导航姿态四元数确定的航向角之差再除以组合导航滤波周期，并且不大于 动中通天线每秒所允许的最大对星角度误差；在c和d两种情况下，大小至少是天线控制四 元数确定的俯仰角与导航姿态四元数确定的俯仰角之差再除以组合导航滤波周期，并且不 大于动中通天线每秒所允许的最大对星角度误差；在e和f两种情况下，大小至少是天线控 制四元数确定的横滚角与导航姿态四元数确定的横滚角之差再除以组合导航滤波周期，并 且不大于动中通天线每秒所允许的最大对星角度误差。
3. 基于MEMS惯导的双四元数动中通天线控制系统，其特征在于包括：动中通天线控制 器、GPS、MEMS陀螺、MEMS加速度计和动中通天线伺服机构，其中： GPS:测量获取载体的速度和位置信息并送至动中通天线控制器中的滤波单元； MEMS陀螺：测量获取载体在三维空间内的角速度信息并送至动中通天线控制器中的 惯导解算单元和天线控制四元数计算单元； MEMS加速度计：测量获取载体在三维空间内的比力信息并送至动中通天线控制器中 的惯导解算单元； 动中通天线控制器：包括惯导解算单元、滤波单元、天线控制四元数计算单元、天线控 制指令生成单元、天线控制四元数校正指令角速度生成单元，其中： 惯导解算单元：将MEMS陀螺测量获取的载体在三维空间内的角速度信息，扣除由地球 自转、载体沿地球表面运动带来的角速度后，得到载体坐标系相对于地理坐标系的三轴旋 转矢量;将MEMS加速度计测量获取的载体在三维空间内的比力信息，扣除重力加速度、 哥氏加速度后，得到载体的对地加速度；利用载体坐标系相对于地理坐标系的三轴旋转角 速度和载体的对地加速度，经过惯导解算得到载体的姿态、位置和速度信息并送至滤波单 元；将载体坐标系相对于地理坐标系的三轴旋转矢量以及首次惯导解算直接得到的载 体姿态所对应的姿态四元数[q〇 q2 q3]送至天线控制四元数计算单元；从滤波单元获取修 正后的载体姿态信息，利用载体坐标系相对于地理坐标系的三轴旋转矢量ΛΛ更新与修正 后的载体姿态信息所对应的姿态四元数作为导航姿态四元数送至天线控制四元数校正指 令角速度生成单元； 滤波单元：利用GPS输出的载体速度和位置信息，以及惯导解算单元输出的载体速度 和位置信息，通过卡尔曼滤波组合导航算法，以固定的滤波周期修正惯导解算单元输出载 体姿态中的水平姿态误差并将修正后的结果送至惯导解算单元； 天线控制四元数计算单元：生成天线控制四元数，所述的天线控制四元数的形式为 [q' ^ q' i q' 2 q' 3]，天线控制四元数中每个参数的含义与惯导解算单元获取的姿态四元数[(? qi % (?]对应一致，且[q' cl q' 1 q' 2 q' 3]的初值为[%ι I % ;从惯导解算单元每接收到一 次载体坐标系相对于地理坐标系的三轴旋转矢量β/?就利用载体坐标系相对于地理坐标 系的三轴旋转矢量0Λ,更新天线控制四元数[q' 〇 q' i q' 2 q' 3]并送至天线控制四元数校正指 令角速度生成单元；从天线控制四元数校正指令角速度生成单元获取三轴指令角速度旋转 矢量，并用所述三轴指令角速度旋转矢量再次更新天线控制四元数[q' 〇 q' i q' 2 q' 3]并送至 天线控制指令生成单元； 天线控制四元数校正指令角速度生成单元：分别从惯导解算单元和天线控制四元数计 算单元获取导航姿态四元数和天线控制四元数，将由导航姿态四元数确定的载体姿态角与 由天线控制四元数确定的载体姿态角对应相减，得到姿态角差值，并根据姿态角差值生成 用于校正天线控制四元数的三轴指令角速度旋转矢量并送至天线控制四元数计算单元，三 轴指令角速度旋转矢量中各元素的取值方法如下： a. 若由天线控制四元数确定的航向角大于由导航姿态四元数确定的航向角，则三轴指 令角速度旋转矢量的第三个元素取正的修正指令角速度； b. 若由天线控制四元数确定的航向角小于由导航姿态四元数确定的航向角，则三轴指 令角速度旋转矢量的第三个元素取负的修正指令角速度； C.若由天线控制四元数确定的俯仰角大于由导航姿态四元数确定的俯仰角，则三轴指 令角速度旋转矢量的第一个元素取正的修正指令角速度； d. 若由天线控制四元数确定的俯仰角小于由导航姿态四元数确定的俯仰角，则三轴指 令角速度旋转矢量的第一个元素取负的修正指令角速度； e. 若由天线控制四元数确定的横滚角大于由导航姿态四元数确定的横滚角，则三轴指 令角速度旋转矢量的第二个元素取正的修正指令角速度； f. 若由天线控制四元数确定的横滚角小于由导航姿态四元数确定的横滚角，则三轴指 令角速度旋转矢量的第二个元素取负的修正指令角速度； 天线控制指令生成单元：从天线控制四元数计算单元接收最新的天线控制四元数，根 据天线控制四元数解算得到动中通天线的伺服方位角、伺服仰角和伺服极化角，送至动中 通天线伺服机构； 动中通天线伺服机构：包括方位向、俯仰向和极化向的电机驱动器和相应的电机，三个 方向的电机驱动器根据天线控制指令生成单元传来的伺服方位角、伺服仰角和伺服极化角 分别驱动相应方向的电机，由此控制动中通天线的三轴转动。
4.根据权利要求3所述的基于MEMS惯导的双四元数动中通天线控制系统，其特征在 于：所述的天线控制四元数校正指令角速度生成单元生成的修正指令角速度，在a和b两种 情况下，大小至少是天线控制四元数确定的航向角与导航姿态四元数确定的航向角之差再 除以组合导航滤波周期，并且不大于动中通天线每秒所允许的最大对星角度误差；在c和d 两种情况下，大小至少是天线控制四元数确定的俯仰角与导航姿态四元数确定的俯仰角之 差再除以组合导航滤波周期，并且不大于动中通天线每秒所允许的最大对星角度误差；在 e和f两种情况下，大小至少是天线控制四元数确定的横滚角与导航姿态四元数确定的横 滚角之差再除以组合导航滤波周期，并且不大于动中通天线每秒所允许的最大对星角度误 差。
【文档编号】H01Q3/02GK104064869SQ201410265808
【公开日】2014年9月24日 申请日期:2014年6月13日 优先权日:2014年6月13日
【发明者】于清波, 门吉卓, 赵书伦, 郎嵘, 刘晓滨, 杨春香 申请人:北京航天控制仪器研究所