精确空投导航装置快速初始定姿装置及方法与流程

本发明属于空投伞降的导航控制技术领域，涉及一种适用于精确空投系统空中投放载体上的载体初始姿态确定的装置及方法，尤其涉及一种精确空投载体投放后导航装置在空中加电启动条件下的短时间内快速确定载体初始姿态的简便易行的装置及方法。

背景技术：

空投技术利用降落伞将物资和人员投放到指定的地点，在特殊空投及民用救灾等领域内发挥着不可替代的作用。但是传统的空投技术，由于没有对投放的载体进行控制，使得物资等投放后，在降落伞的带动下，在空中的降落轨迹难以预测。空投物资的落点分散度很大，造成物资的集结费时费力。在特殊的地形和气候条件下，甚至空投行动难以开展。随着现代控制技术及导航技术的发展，可以对空投过程中的空投载体进行航迹控制，使得传统的空投技术进化成为精确空投技术。精确空投技术可以使运输机在远离投放点的时候就将投放载体投放出舱，载体自己向目标地点飞行并降落。同时物资的落点也得到控制，分散度降低，从而集结容易。抗气候和特殊地形的能力也增强了。提高了空投技术的应用范围。

在实施精确空投过程中，为了对投放载体实行飞行控制，就需要空投导航装置计算出载体的实时位置和姿态信息。而空投导航装置，在进行姿态和位置计算之前，初始姿态信息的取得是一个非常重要的问题，只有在相对精确的空投载体的初始姿态值的基础上，才能够完成后续的精确定位与姿态的计算，才能为空投控制装置提供所需要的位置及姿态信息。

更重要的是，在空投过程中，为了保证降低运输机的电磁干扰等需求，要求空投的物资载体在投放出舱后才能给导航及控制系统加电。由于空投载体在空中由降落伞引导着陆，在空中漂浮的时间有限。且需要在载体着陆前，在控制下降的过程中，完成对降落伞航迹的控制，因此就需要在导航及控制系统加电后，在很短时间内完成的初始姿态的获取，提供给导航控制系统初始的姿态参数，在此基础上才能完成后续的对伞的控制。

传统的初始姿态计算采用解析的方法，是在载体静止状态下，利用陀螺和加速度计敏感地球自转角速度和地球重力加速度，需要较长时间的计算才能够完成的载体初始姿态的计算。而在实际空投过程中，空投的物资载体随时都处于运动之中，且需要很短时间内就能够完成初始姿态的计算。这使得传统的初始姿态计算方法无法应用于精确空投系统中。

南京航空航天大学公开号为cn101256080的发明专利，“卫星/惯性组合导航系统的空中对准方法”，提出了一种卫星/惯性组合导航系统的空中对准方法，包括卫星导航接收机定位数据采集；卫星导航定位数据缓存；惯性测量单元数据采集；飞行器运动加速度计算；导航坐标系下三维比力计算；俯仰角和横滚角三角方程的快速数据求解；初始化捷联惯性导航系统的导航参数等步骤，可实现对载体动态飞行中的捷联惯性导航系统空中初始姿态的计算。

但是该初始定姿方法中，航向参数需要外界提供，且该初始姿态的计算方法中载体运动方向与载体航向坐标需要保持一致。而在空投过程中，由于安全性及电磁兼容等方面的要求，投放物资的导航和控制系统在空中才能加电启动，因此初始定姿期间没有可用的航向信息，且投放物资在空中下落过程中还会出现扭转、侧滑等情况，造成载体运动方向与航向坐标不一致。因此这种初始姿态的计算方法不能满足精确空投的导航系统的需求。

西安应用光学研究所公开号为cn103994777的发明专利，“空投物资组合导航装置用空中快速对准方法”，提出了一种由组合导航模块、磁航向计、陀螺加速度计、卫星导航模块等构成的导航装置，利用卫星模块提供位置信息、陀螺提供的角速率信息、加速度计提供的线加速度信息以及磁航向计提供的方向信息，完成精确空投系统在空中的快速初始姿态确定。

但是该方法中，所使用的磁航向计非常容易受到地理环境及运载货物的干扰，从而造成计算得到的载体的初始姿态精度很难保证。而且为了保证磁航向计的测量精度，磁航向计在使用前需要连同安装的载体一起进行现场标定，当投放的载体体积很大时，标定过程中需要占用很大的空间，且需要吊车等辅助设备。标定过程也非常耗时而且复杂，在空投物资很多的情况下，仅标定就需要耗费大量的人力和时间，很难适应具体应用对空投物资快速性、易操作性和精确性的需求。

由于精确空投系统的特殊性，要求空投载体在空中的初始姿态计算必须在很短时间内完成；且能够在载体处于空中漂浮下降的运动和扭转中完成初始姿态的计算；还要适应实际空投的使用需要，不能够造成姿态精度不可靠和使用不方便等问题。现有的载体初始姿态的获取方法，有的计算时间较长、有的不能适应空投载体的特殊运动状态，有的使用不方便，精度不可靠。因此需要一种简便易行，能够适应空投载体运动条件、短时间内快速完成、精度可靠性高的精确空投载体初始姿态的计算方法。

技术实现要素：

(一)发明目的

针对现有技术存在的问题，本发明提出一种精确空投导航装置快速初始定姿装置及方法，能够适用于载体投放后导航装置才加电启动、没有预先可用航向信息、空投载体降落中会出现侧滑及扭转等运动，使得的载体的方向与运动方向不一致等复杂情况下，在空投载体在空中降落飞行的有限时间内，用很短时间完成空投载体初始姿态信息的确定。

(二)技术方案

为了解决上述技术问题，本发明首先提供一种精确空投导航装置快速初始定姿装置，其包括初始姿态计算模块、三个陀螺仪、三个加速度计、卫星导航模块；初始姿态计算模块完成初始姿态计算的算法执行；陀螺仪和加速度计向初始姿态计算模块提供实时测得的载体的角加速度以及线加速度信息，作为进行初始姿态计算的输入量；卫星导航模块向初始姿态计算模块提供实时测得载体位置及速度信息，作为初始姿态计算的参考数据。

优选地，定姿装置中，导航装置随载体一起投放后，在空中加电启动，经过系统内各个电路预热后，所述的三个光纤陀螺仪开始对载体的右、前、上三个正交轴向的角度增量信息进行采集，并以一定的频率提供给所述初始姿态计算模块。

优选地，所述三个加速度计同时对载体在右、前、上三个正交轴向的线速度增量进行采集，并以一定的频率提供给所述初始姿态计算模块。

优选地，所述卫星导航模块预热后开始进行初始化及锁星等工作，系统加电后一段时间内，开始向所述初始姿态计算模块以固定频率提供载体的经度、纬度、高度等位置信息，以及东向速度、北向速度、天向速度等速度信息。

优选地，所述的初始姿态计算模块，在系统加电预热后，开始采集所述的三个陀螺仪传递来的载体三个正交轴向的角度增量信息、所述的三个加速度计测得的载体在三个正交轴向的线速度增量、所述的卫星导航模块送来的载体的经度、纬度、高度等位置信息，以及东向速度、北向速度、天向速度等速度信息，采用所述的快速初始定姿方法进行计算，在短时间内(30秒内)计算出载体初始航向、俯仰、横滚等信息，从而完成初始姿态信息的计算。

基于上述装置的一种精确空投导航装置快速初始定姿方法，其包括以下步骤：

步骤1：各个模块上电后，初始姿态计算模块向三个陀螺仪、三个加速度计以及卫星导航模块发送启动指令，并进行参数初始化：

初始化的内容包括初始姿态计算模块内的计时器t清零并开始计数，设置以下参数：初始化完成标识f＝0，初始纬度正弦s0＝0，初始纬度余弦cl0＝0，初始纬度lat0＝0，初始精度lon0＝0；

设4乘1矩阵

其中q1＝1，q2＝0,q3＝0,q4＝0；3乘1矩阵矩阵：

以及3乘3矩阵c0，c1；

步骤2：判断计时器t是否增加了t1秒的时间间隔，如果没有达到则继续计数，如果达到了就进入步骤3；

步骤3：采集当前时间间隔内的三个陀螺仪输出的载体在右、前、上三个方向上的角增量值δθx、δθy、δθz将他们组成角增量矩阵δθ，记为：

三个加速度计输出的载体在右、前、上三个方向上的速度增量值δvx、δvy、δvz，将他们组成速度增量矩阵δv，记为：

步骤4：完成向量vb的计算：

设变量n1、n2，n3，t，c，s，q2，q3，c2，vb1，及δθ^t＝[δθxδθyδθz]，设矩阵顺序进行如下运算：

(1)利用矩阵乘法计算n1＝δθ^t*δθ，对n1进行开方运算得到

(2)计算n3＝n2/2；

(3)对n3进行三角函数运算c＝cos(n3)*i，s＝sin(n3)/n2；

(4)计算出t＝s*d，得到q2＝(c+t)*q1；

(5)并设q2的转置矩阵为计算出更新后的

(6)设q11＝q1*q1，q12＝q1*q2，q13＝q1*q3，q14＝q1*q4，q22＝q2*q2，q23＝q2*q3，q24＝q2*q4，q33＝q3*q3，q34＝q3*q4，q44＝q4*q4，并计算：

(7)计算c2＝c1*δθ，然后计算并更新vb1，使得vb＝vb1；

步骤5：判断计时器t是否增加了t2秒的时间间隔，如果没有达到则返回步骤2，如果达到了就进入步骤6；

步骤6：采集卫星导航模块发来的位置及速度信息：

当前载体所在的经度，记为lon；纬度，记为lat；高度，记为hi；

当前载体的速度信息，东向速度，记为ve；北向速度，记为vn；天向速度，记为vu组成矩阵

如果f＝0则令f＝1并计算变量,令lat0＝lat,lon0＝lon，s0＝sin(lat),cl0＝cos(lat)；然后转入步骤2；如果f＝1则进入步骤7；

步骤7：设置变量s1，s2，s3，gn,sd，cd，det，矩阵gn，依次完成如下计算：

(1)计算s1＝sin(lat)，然后利用s1计算s2＝s1*s1，再计算s3＝s2*s2。以及det＝lon-lon0+7.2921151467e-5*t2；

(2)计算矩阵c0的当前值

(3)计算重力加速度:

gn＝9.7803267714*(1+5.27094e-3*s2+2.32718e-5*s4)-3.086e-6*hi，

组成矩阵计算：

(4)更新计算：v1＝v1+c0*wie×v*t2其中×表示矩阵的叉乘运算；

(5)更新计算：v2＝v2-c0*gn*t2；

(6)计算出当前矩阵vn：vn＝c0*v+v1+v2；

步骤8：判断计数器t是否到达第第一个时间点t1，如果t不等于t1则返回步骤2；如果t＝t1则对当前计算出来的向量vb和vn进行记录，即v0b＝vb，v0n＝vn，然后进入步骤9；

步骤9：判断计时器是否到达要求初始姿态计算完成的时间t2，如果t不等于t2，则返回步骤2执行；如果t＝t2则对当前计算出来的向量vb和vn的进行记录，即v1b＝vb，v1n＝vn，然后进入步骤10；

步骤10：设矩阵ct1，ct1r，ct2，ct3，cbn，及cbn中的元素

c11,c12,c13,c21,c22,c23,c31,c32,c33即依次完成如下计算：

(1)计算其中×表示矩阵的叉乘运算。并计算出ct1的逆矩阵ct1r，即：

(2)计算ct2，算法为：

(3)计算ct3＝ct1r*ct2；

(4)计算出ct4＝ct3*c1；

(5)计算矩阵cbn＝c0^-1*ct4，其中c0^-1是对矩阵c0的求逆矩阵运算；

(6)利用反三角函数计算最后确定的当前时刻的姿态值，单位为弧度：载体俯仰值pitch的计算方法为：pitch＝arcsin(c32)；

载体横滚值roll的计算方法为：roll＝atan2(-c31,c33)；

载体航向值hdg的计算方法为：hdg＝atan2(-c12,c22)；

确定上述载体姿态值后，初始姿态的计算过程完成。

(三)有益效果

上述技术方案所提供的精确空投导航装置快速初始定姿装置及方法，具有以下有益效果：

(1)本发明提出的方法，采用两个不同时间点测量到的加速度与载体姿态的关系完成对载体初始姿态矩阵的计算，因此能够在系统启动后很短时间内计算出载体初始姿态，满足了精确空投过程中空投载体在空中停留时间有限，需要快速获取初始姿态的使用条件。

(2)本发明利用载体动态过程中加速度的变化与载体姿态变化的关系来求解载体的初始姿态，解决了精确空投过程中，空中投放载体导航控制系统在空中加电启动，在没有预先可用信息的情况下，在运动、晃动、旋转等过程中的载体初始姿态确定的问题。

(3)本发明在确定载体初始姿态过程中，仅仅使用了卫星导航模块、陀螺仪、加速度计等设备，安装方便，操作简单，精度指标稳定。避免了使用其他辅助设备如磁航向计等而造成的操作复杂、可靠性低、精度易受到使用环境影响等问题。

附图说明

图1快速初始定姿系统组成示意图。

图2空投载体坐标系示意图。

图3快速初始定姿算法流程。

具体实施方式

为使本发明的目的、内容和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本发明优选实施例提供的快速初始姿态确定的方法，用于精确空投系统中装有惯性与卫星组合导航装置的物资载体的初始姿态值的获取。

本发明采用如图1所示的三轴陀螺仪、三轴加速度计、卫星导航模块、初始姿态计算模块等单元组成。其中三轴陀螺仪中三个陀螺的敏感轴x、y、z如图2相互垂直构成迪卡尔坐标系，分别与空投物资载体的右、前、上三个方向平行，构成了空投载体坐标系。其功能是实时向初始姿态计算模块和组合导航模块提供空投物资载体降落过程中的右、前、上三个方向的角加速度；三个加速度计的敏感轴与陀螺的敏感轴即空投载体坐标系重合，其功能是实时向初始姿态计算模块和组合导航模块提供空投物资载体降落过程中的右、前、上三个方向的线加速度信息；卫星导航模块的功能是向初始姿态计算模块和组合导航模块提供空投物资载体当前的经度、纬度、高度及东向、北向速度值；初始姿态计算模块根据卫星导航模块及三个陀螺、三个加速度计的输出信息计算出空投物资坐标系在地理坐标系内的姿态初始值，即初载体始的航向角、俯仰角、横滚角的值。初始姿态计算模块是本发明的核心，在本实施例中初始姿态计算模块采用dsp处理计算机电路板。

当空投物资载体被投放出机舱且各单体加电后，初始姿态计算按图3所示的流程执行以下操作步骤。

第一步：各个模块上电后，初始姿态计算模块向三个陀螺仪、三个加速度计以及卫星导航模块发送启动指令，并进行参数初始化。初始化的内容包括初始姿态计算模块内的计时器t清零并开始计数，设置以下参数：初始化完成标识f＝0，初始纬度正弦s0＝0，初始纬度余弦cl0＝0，初始纬度lat0＝0，初始精度lon0＝0；设4乘1矩阵其中q1＝1，q2＝0,q3＝0,q4＝0；3乘1矩阵矩阵以及3乘3矩阵c0，c1。

第二步：判断计时器t是否增加了t1秒的时间间隔，如果没有达到则继续计数，如果达到了就进入第三步。在本实施例中，t1选择为0.01秒。

第三步：采集当前时间间隔内的三个陀螺仪输出的载体在右、前、上三个方向上的角增量值δθx、δθy、δθz将他们组成角增量矩阵δθ，记为：

三个加速度计输出的载体在右、前、上三个方向上的速度增量值δvx、δvy、δvz，将他们组成速度增量矩阵δv，记为：

第四步：完成向量vb的计算。设变量n1、n2，n3，t，c，s，q2，q3，c2，vb1，及δθ^t＝[δθxδθyδθz]，设矩阵顺序进行如下运算：

(1)利用矩阵乘法计算n1＝δθ^t*δθ，对n1进行开方运算得到

(2)计算n3＝n2/2；

(3)对n3进行三角函数运算c＝cos(n3)*i，s＝sin(n3)/n2；

(4)计算出t＝s*d，得到q2＝(c+t)*q1；

(5)并设q2的转置矩阵为计算出更新后的

(6)设q11＝q1*q1，q12＝q1*q2，q13＝q1*q3，q14＝q1*q4，q22＝q2*q2，q23＝q2*q3，q24＝q2*q4，q33＝q3*q3，q34＝q3*q4，q44＝q4*q4，并计算：

(1)计算c2＝c1*δθ，然后计算并更新vb1，使得vb＝vb1。

上述计算完成后进入第五步。

第五步：判断计时器t是否增加了t2秒的时间间隔，如果没有达到则返回第二步，如果达到了就进入第六步。在本实施例中，t2选择为1秒。

第六步：采集卫星导航模块发来的位置及速度信息。当前载体所在的经度，记为lon；纬度，记为lat；高度，记为hi。当前载体的速度信息，东向速度，记为ve；北向速度，记为vn；天向速度，记为vu组成矩阵如果f＝0则令f＝1并计算变量,令lat0＝lat,lon0＝lon，s0＝sin(lat),cl0＝cos(lat)。然后转入第二步；如果f＝1则进入第七步。

第七步：设置变量s1，s2，s3，gn,sd，cd，det，矩阵gn，依次完成如下计算：

(1)计算s1＝sin(lat)，然后利用s1计算s2＝s1*s1，再计算s3＝s2*s2。以及det＝lon-lon0+7.2921151467e-5*t2；

(2)计算矩阵c0的当前值

(3)计算重力加速度:

gn＝9.7803267714*(1+5.27094e-3*s2+2.32718e-5*s4)-3.086e-6*hi，

组成矩阵计算：

(4)更新计算：v1＝v1+c0*wie×v*t2其中×表示矩阵的叉乘运算；

(5)更新计算：v2＝v2-c0*gn*t2；

(6)计算出当前矩阵vn：vn＝c0*v+v1+v2。

完成上述计算后进入第八步。

第八步：判断计数器t是否到达第第一个时间点t1，如果t不等于t1则返回第二步。如果t＝t1则对当前计算出来的向量vb和vn进行记录，即v0b＝vb，v0n＝vn，然后进入第九步。本实施例中根据使用要求t1选择为15秒。

第九步：判断计时器是否到达要求初始姿态计算完成的时间t2，如果t不等于t2，则返回第二步执行。如果t＝t2则对当前计算出来的向量vb和vn的进行记录，即v1b＝vb，v1n＝vn，然后进入第十步。本实施例中根据使用要求t2选择为30秒。

第十步：设矩阵ct1，ct1r，ct2，ct3，cbn，及cbn中的元素c11,c12,c13,c21,c22,c23,c31,c32,c33即，依次完成如下计算：

(1)计算其中×表示矩阵的叉乘运算。并计算出ct1的逆矩阵ct1r，即：(2)计算ct2，算法为：

(3)计算ct3＝ct1r*ct2。

(4)计算出ct4＝ct3*c1。

(5)计算矩阵cbn＝c0^-1*ct4，其中c0^-1是对矩阵c0的求逆矩阵运算。

(6)利用反三角函数计算最后确定的当前时刻的姿态值，单位为弧度：载体俯仰值pitch的计算方法为：pitch＝arcsin(c32)；

载体横滚值roll的计算方法为：roll＝atan2(-c31,c33)；

载体航向值hdg的计算方法为：hdg＝atan2(-c12,c22)。

确定上述载体姿态值后，初始姿态的计算过程完成。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。