基于POS数据与框架角联合解算的姿态控制方法与流程

本发明涉及导航制导领域，具体涉及一种基于高精度POS(Position and Orientation System,位置与姿态系统)数据与框架角联合解算的姿态控制方法。

背景技术：

随着精确制导武器技术的不断发展，航空光电吊舱已成为战机精确打击系统中的重要组成部分，是集光学、机械、微电子、自动控制和通讯技术为一体的高科技产品，其研制开发备受各国的重视。

高精度的位置和姿态测量系统是光电吊舱的核心部件，用于隔离载体的姿态运动，从而辅助光电传感器完成对目标的搜索、侦察等任务。位置和姿态测量系统中包括光纤陀螺和加速度计、GNSS，根据其在光电吊舱内部安装位置的不同，可以支持不同的工作模式。

当前高精度多框架光电吊舱广泛应用于边界巡逻、海岸巡防、安全搜救、执法安全、森林防火、环境监视、输油管道检查、高压线检查、影视航拍等工作。多框架光电吊舱采用多框架架构技术，基于陀螺稳定原理，通过数字化和先进驱动技术，通常利用内中外三层框架实现稳定控制，有效隔离载体运动向内部载荷平台的传导。机载多框架光电吊舱适用于直升机、固定翼等多种类型的有人/无人飞机，在强振动、大风阻等不利扰动环境下，保持吊舱内部载荷平台微弧级精度稳定。机载多框架光电吊舱根据配备的载荷(如高清摄像机、红外传感器、激光测距仪、激光指示器等)，完成影视拍摄、昼夜侦察、目标跟踪与定位及武器导引等功能。

采用多框架稳定的机载光电吊舱，需要科学准确地估算各框架的当前角速率，本领域亟待研究更优秀的相关技术方案。

技术实现要素：

本发明的目的就在于克服现有技术存在的问题和不足，提供一种满足姿态控制需求的高精度POS数据与框架角联合解算技术，实时为光电载荷提供高频率高精度的姿态控制信息。

本发明所采用的技术方案是一种基于POS数据与框架角联合解算的姿态控制方法，用于多框架光电吊舱的姿态控制，所述多框架光电吊舱包括内框、中框和外框，所述多框架光电吊舱中安装有位置和姿态测量系统，将位置和姿态测量系统固连于多框架光电吊舱的内框轴，姿态控制包括以下步骤，

步骤1，将位置和姿态测量系统中光纤陀螺的陀螺数据与多框架光电吊舱的框架角数据同步；

步骤2，在任意时刻k，获取陀螺三轴角速率信息

步骤3，根据位置与姿态系统在任意时刻k输出的位置信息计算此时地球自转引起的角速度信息

步骤4，根据多框架光电吊舱在时刻k前输出的框架角信息进行线性拟合，获得任意时刻k三轴框架角信息，包括内框框架角α₀(k)、中框框架角α₁(k)和外框框架角α₂(k)；

步骤5，根据位置与姿态系统在任意时刻k输出的速度和位置信息计算此时载体线运动引起的角速度信息

步骤6，根据位置与姿态系统在任意时刻k输出的姿态信息获取此时载体角运动引起的角速度信息

步骤7，利用以上步骤所得结果计算任意时刻k光电吊舱系统的三个框架角速率信息；

步骤8，根据步骤7所得结果输出多框架光电吊舱的三个框架角控制信息，输出位置与姿态系统所得载体的速度、位置和姿态信息。

而且，利用位置和姿态测量系统中内置的晶振计数器实现同步。

而且，根据位置和姿态测量系统在任意时刻k输出的地理纬度信息计算此时地球自转引起的角速度信息计算公式如下，

其中，ω_ie是地球自转角速率值，n表示导航坐标系。

而且，步骤5中，根据位置和姿态测量系统在任意时刻k输出的速度V_E和V_N和地理纬度信息计算此时载体线运动引起的角速度信息计算公式如下：

其中，V_E和V_N分别是载体在地理东向和北向速度分量，R是地球半径。

而且，步骤7中，计算任意时刻k光电吊舱系统的三个框架角速率信息包括以下步骤，

步骤7.1，根据步骤4所得光电吊舱的三轴框架角信息分别计算载体坐标系与内框坐标系之间的旋转矩阵

步骤7.2，结合POS系统输出的载体系与导航系之间的旋转矩阵计算内框系与导航系之间的旋转矩阵计算公式如下，

步骤7.3，根据步骤3、5、6所得和进行坐标变换，转换到内框系，得到陀螺三轴角速率信息中的地球自转角速度分量载体线运动引起的角速度分量载体角运动引起的分量公式如下，

步骤7.4，计算光电吊舱的三个框架角速率信息，计算公式如下，

其中，分别是所对应的角速率在时刻k的值。

本发明具有下列优点和积极效果：

①提取的框架角速率精度高，稳定性好；

②输出频率更高；

③充分利用高精度位置与姿态系统的输出数据，设计紧凑，节约成本，具有重要的市场价值。

附图说明

图1是本发明实施例的吊舱系统框架结构示意图，其中，Xb、Yb、Zb分别是载体坐标系的三个轴、X2、Y2、Z2分别是外框坐标系的三个轴、X1、Y1、Z1是中框坐标系的三个轴、X0、Y0、Z0是内框坐标系的三个轴；

图2是本发明实施例的高精度POS数据与框架角联合解算技术流程图。

具体实施方式

下面结合附图和实施例对本发明技术方案详细说明：

本发明为提供姿态控制，实现高精度POS数据与框架角联合解算，通过光电吊舱输出的三个框架角和固连于光电吊舱内框轴的高精度POS系统提供的陀螺角速率信息，共同解算高精度高频率的框架角速率信息。通过框架角信息与陀螺角速度信息的高精度同步和联合解算，可以提供高精度的位置和姿态信息的同时，为载荷提供高频框架角速率信息。

本发明的工作原理：

与光电吊舱内框固连的高精度位置与姿态系统在输出固定频率的载体位置与姿态信息的同时，还输出了高精度高频率的角速率信息，该角速率是光电吊舱内框相对于惯性系的相对旋转角速度矢量在内框坐标系下的投影分量，记为其中：角标i是惯性坐标系，0是内框坐标系。该角速率分量包含了地球自转角速度分量载体线运动引起的角速度分量载体角运动引起的分量和框架角运动引起的分量其中：e是地球坐标系、n是导航坐标系、b是载体坐标系。根据位置与姿态系统提供的载体位置、速度和姿态信息以及各角速率信息之间的相对关系，可以较高的频率实时解算高精度的框架角速率信息。

参见图1，其中，Xb、Yb、Zb分别是载体坐标系的三个轴、X2、Y2、Z2分别是外框坐标系的三个轴、X1、Y1、Z1是中框坐标系的三个轴、X0、Y0、Z0是内框坐标系的三个轴，Ob、O0、O1、O2分别是各坐标系的原点，Ob与O2重合，附图中仅显示O2。

本发明将光电吊舱输出的框架角信息和高精度的位置和姿态测量系统信息同步，通过欧拉角与框架角之间的转换关系，实时输出高频框架角速率信息和载荷的位置姿态信息。实施例中，位置和姿态测量系统简称POS系统。参见图2，本发明实施例为满足姿态控制需求，进行高精度POS数据与框架角联合解算，其中可以直接利用POS系统中通过光纤陀螺和加速度计捷联解算输出的位置信息、速度信息、姿态信息，具体包括以下步骤：

①将陀螺数据与框架角数据同步：实施例利用POS系统内置的高精度的晶振计数器(优选地，精度高于10^-9)，将POS系统中光纤陀螺的陀螺数据与光电吊舱框架系统的框架角数据同步；

②在任意时刻k，获取高频率的(优选地，大于2kHz)陀螺三轴角速率信息

③根据POS系统在任意时刻k输出的地理纬度信息计算此时地球自转引起的角速度信息设ω_ie是地球自转角速率值，n是导航坐标系，计算公式如下：

④根据多框架光电吊舱在时刻k前若干个时刻的框架角信息进行线性拟合，获得任意时刻k三轴框架角信息α₀(k)(内框框架角)、α₁(k)(中框框架角)和α₂(k)(外框框架角)；

⑤根据POS系统在任意时刻k输出的速度V_E和V_N和地理纬度信息计算此时载体线运动引起的角速度信息计算公式如下：

其中，V_E和V_N分别是载体在地理东向和北向速度分量，R是地球半径。

⑥根据POS系统在任意时刻k输出的姿态信息获取此时载体角运动引起的角速度信息具体实施时可在POS系统的载体捷联解算过程中直接输出，本流程直接使用该结果；

⑦利用以上信息计算任意时刻k，光电吊舱系统的三个框架角速率信息，具体步骤如下：

a、根据步骤④所得多框架光电吊舱的三个框架角系信息分别计算载体坐标系与内框坐标系之间的旋转矩阵计算公式如下：

其中，内框框架角α₀(k)、中框框架角α₁(k)和外框框架角α₂(k)分别是在时刻k，内框系相对于中框系转过的角度、中框系相对于外框系转过的角度和外框系绕载体系转过的角度，由线性拟合所得。

b、结合POS系统输出的载体系与导航系之间的旋转矩阵计算内框系与导航系之间的旋转矩阵计算公式如下：

c、对步骤③、⑤、⑥所得和进行坐标变换，将其转换到内框系，得到陀螺三轴角速率信息中的地球自转角速度分量载体线运动引起的角速度分量载体角运动引起的分量公式如下：

d、计算多框架光电吊舱的三个框架角速率信息，计算公式如下：

其中，分别是所对应的角速率在具体时刻k的值。

⑧输出多框架光电吊舱的三个框架角控制信息(即根据步骤⑦所得和POS系统所得载体的速度、位置和姿态信息，具体所述时可将输出反馈给光电吊舱控制执行系统。

具体实施时，以上步骤可采用计算机软件技术实现自动流程运行。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。