组合导航装置的任务调度方法及系统

技术领域

本申请涉及导航领域，具体而言，涉及一种组合导航装置的任务调度方法及系统。

背景技术

现代卫星导航系统两次定位之间的时间间隔长，每次定位还需要10分钟以上的跟踪，不能连续提供无人机位置信息，因此，会常与惯性导航系统组合。

采用卫星全球定位系统能使无人机在任何地区实时得到位置和速度信息。但串行处理的单核工作导航逻辑导致导航效率较低。

针对上述的问题，目前尚未提出有效的解决方案。

发明内容

本申请实施例提供了一种组合导航装置的任务调度方法及系统，以至少解决串行处理的单核工作导航逻辑导致效率较低的的技术问题。

根据本申请实施例的一个方面，提供了一种组合导航装置的任务调度方法，包括：通过多个惯性测量单元进行数据采集，并采用多线程来对所采集的数据进行补偿处理；对补偿后的所述数据进行预处理，并基于预处理后的数据进行对准；在对准之后，采用多任务模块来对所述组合导航装置进行捷联任务调度。

根据本申请实施例的另一方面，还提供了一种组合导航装置的任务调度系统，包括：采集模块，被配置为通过多个惯性测量单元进行数据采集，并采用多线程来对所采集的数据进行补偿处理；对准模块，被配置为对补偿后的所述数据进行预处理，并基于预处理后的数据进行对准；调度模块，被配置在对准之后，采用多任务模块来对所述组合导航装置进行捷联任务调度。

在本申请实施例中，通过多个惯性测量单元进行数据采集，并采用多线程来对所采集的数据进行补偿处理；对补偿后的所述数据进行预处理，并基于预处理后的数据进行对准；在对准之后，采用多任务模块来对所述组合导航装置进行捷联任务调度，从而解决了串行处理的单核工作导航逻辑导致效率较低的的技术问题。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的一种组合导航装置的任务调度方法的流程图；

图2A是根据本申请实施例的初始化方法的流程图；

图2B是根据本申请实施例的并行处理方法的流程图；

图2C是根据本申请实施例的地理坐标系的示意图；

图2D是根据本申请实施例的松耦合结构的示意图；

图3是根据本申请实施例的卡尔曼滤波误差修正方法的流程图；

图4是根据本申请实施例的捷联惯性测量单元系统组成示意图；

图5是根据本申请实施例的惯性测量组件的硬件设计电路图；

图6是根据本申请实施例的导航计算机的硬件设计电路图；

图7是根据本申请实施例的二次电源模块的硬件设计电路图；

图8是根据本申请实施例的卫星接收机的硬件设计电路图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

根据本申请实施例，提供了一种组合导航装置的任务调度方法，如图1所示，该方法包括：

步骤S 102，通过多个惯性测量单元进行数据采集，并采用多线程来对所采集的数据进行补偿处理。

首先，对以下至少之一进行初始化：系统变量、存储信息、报文信息、存储参数、用户参数、补偿参数和配置参数。

接着，通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息，对采用多线程来对所采集的数据进行补偿处理。

步骤S 104，对补偿后的所述数据进行预处理，并基于预处理后的数据进行对准；

步骤S106，在对准之后，采用多任务模块来对所述组合导航装置进行捷联任务调度。

首先，通过捷联惯性测量单元获取所述无人机的加速度信息和角速度信息之后，基于所述角速度信息，利用四元数法进行姿态解算，得到所述载体的姿态角。具体地，利用四元数来描述所述姿态信息，并基于所述描述确定弹体坐标系到地理坐标系的转换关系式与所述四元数之间的关系；利用角速度增量，基于所确定的所述转换关系式与所述四元数之间的关系，来求解描述所述姿态信息的所述四元数；基于所求解出的所述四元数，来进行姿态解算，得到所述载体的姿态角。

接着，基于所述加速度信息，进行捷联解算，得到所述载体的速度信息和位置信息。具体地，基于地速在惯性坐标系下的变化率、所述加速度信息中的比力矢量、以及地球的自转角速度，确定所述地速在导航坐标系的变化率；基于所述地速在导航坐标系的变化率，确定导航方程，并基于所述导航方程，确定在所述导航坐标系中所述载体的速度沿真北、东向和当地垂线方向的分量。例如，基于所述载体的经度和纬度的变化率，确定地理坐标系相对于地球固连坐标系的转动角速率；基于所述转动角速率，确定所述载体的当地重力矢量；基于由于重力异常造成的当地重力矢量方向相对于当地垂线方向的角度偏差、当前维度、当前经度和当前距地球表面的高度、以及所述当地重力矢量，确定在所述导航坐标系中所述载体的速度沿真北、东向和当地垂线方向的分量。

接收地面站以预设的频率发送的卫星差分修正信息，并利用所述卫星差分修正信息，来修正所述位姿信息。

首先，基于所述卫星差分修正信息，利用卡尔曼滤波器来估计位置、速度和姿态的误差。具体地，将来自导航卫星系统的导航解的位置、速度和姿态作为量测信息输入到所述卡尔曼滤波器，作为初始估计值；所述卡尔曼滤波器在预测阶段，对所述初始估计值进行线性化处理，并基于线性化处理后的所述初始估计值来确定误差协方差；基于所述误差协方差来确定卡尔曼增益，并基于所述卡尔曼增益重新确定所述误差协方差；基于重新确定的所述误差协方差，来估计捷联解算的所述位置、速度和姿态的误差。

然后，基于所估计出的误差，来修正所述位姿信息。

最后，基于修正后的所述位姿信息，来对所述无人机进行导航。

在本申请实施例中，增加多任务模式，方便程序维护与升级，并且，提高了软件模块化程度，适合并行开发，从而提高了效率。

此外，本实施例中，接收地面站以预设的频率发送的卫星差分修正信息，并利用所述卫星差分修正信息，来修正所述位姿信息；基于修正后的所述位姿信息，来对所述无人机进行导航，从而提高了导航的精确度。

实施例2

根据本申请实施例，还提供了组合导航装置的任务调度方法，该方法包括：

步骤S202，初始化。

具体如图2A所示，惯导上电后自动开始初始化和自检，检查设备的陀螺、加速度计及导航计算机工作状态，自检超时时间10s。

惯导以5ms周期向综合控制计算机反馈当前状态，更新结果字、惯导工作状态、导航时间、软件版本号等。惯导自检开始时刻，更新数据包中″结果字″为″自检过程中″；自检完成后，更新″结果字″为″自检正常″或″自检异常″，继续周期性上报数据包，等待综合控制计算机下一步命令。

综合控制计算机根据结果字判定惯导初始化及自检结果，若供电10s后，惯导仍未完成自检，则判定自检超时异常。

步骤S204，并行处理。

任务并行处理具体如图2B所示。

综合控制计算机在收到终端控制平台测试设备发送的对准指令及装订数据后，向惯导发送″装订及对准指令″，惯导响应该命令开始对准。同时，综合控制计算机以5ms周期向惯导转发舰船母惯导装订及对准数据，该数据更新根据″基准惯导帧计数″确定有效数据。

惯导收到指令后，根据装定及对准类型，进入″正常对准″、″快速对准″分支，上报″正在装订″，接收到连续5帧合理装订数据，按照此参数进行封装；若连续2.5s不满足装订正常条件，上报″装订异常″；若装订过程中，出现惯测单元设备异常或1s未收到装订数据或连续判断基准信息无效次数大于等于50，上报″装订异常″。

惯导装订正常后，自动进入对准流程，根据当前所处分支进入″正常对准″或″快速对准″，上报″对准过程中″。在快速对准过程中，自检异常、连续1s未收到装订数据或连续判断基准信息无效次数大于等于50，则上报″快速对准异常″；若根据装定及对准数据进行了大于等于1次水平修正，则上报″快速对准正常″。在正常对准程中，根据装定及对准数据进行了大于等于20次水平修正，则上报″对准正常″；若惯测单元在此期间出现异常或者连续1s未收到装订数据、或连续判断基准信息无效次数大于等于50，则上报″对准异常″；在结果字中，明确″对准及校准精度″。

在惯导对准正常后，自动进入校准状态。在校准过程中，惯测单元检测异常、连续1s未收到装订数据、或连续基准信息无效次数大于等于50，上报″校准异常″；在″惯导系统工作状态字″中，明确对准及校准精度，包括：正常对准精度、快速对准精度、低于快速对准精度。

综合控制计算机从收到终端控制平台测试设备″对准指令″开始计时，正常对准时间不大于3min，快速对准不大于10s

具体算法如下：

1)坐标系设计。

导航坐标系采用北-天-东坐标系。坐标原点取在飞行器质心，ox

载体坐标系坐标原点在飞行器质心，ox

采用正欧拉可用真航向角ψ

在使用正欧拉时地理坐标系和载体坐标系的俯仰角和滚转角定义相同，偏航角和真航向角的关系如下式：

其中，A表示角度偏差。

本实施例中，采用上述方式对坐标系进行转换，能够避免坐标转换时存在的误差，从而使得后续的导航解算更为准确。

2)姿态解算。

本实施采用四元数法，利用四个元素来描述全部姿态信息，四元数q可表示为：

q＝a+bi+cj+dk(1)

其中，a，b，c，d为四元数的实部，i、j、k为虚数单位。

设有b系中矢量v

式中q

由上式可得

采用四元数姿态解算，需要求解如下方程：

其中p＝[0，ω

式中

由角增量计算解四元数，其解可表示为：

其中，q

带入W，令

最终可得解为：

式中，

由此可通过四元数的求解得到姿态角，公式如下：

式中矩阵C即为式(3)。

本实施例中，采用上述方式解算姿态角，可以使得解算出的姿态角更为精确，从而为后续导航提供了精确的基础数据。

3)捷联解算。

导航方程可表示为如下形式：

其中，r表示位置矢量，f表示比力，通过一次积分得到速度，二次积分得到位置。

其中，v

其中，

导航方程可表示为如下形式：

速度沿真北、东向和当地垂线方向分量为：

f

f

是当地地理坐标系中地球的自转角速度：

其中，

使

式(19)中，R

是当地重力矢量，它由地球的质量引力(g)和地球转动产生的向心加速度(ω

其中，Ω表示地球自转角速率，导航方程可以表示为如下分量形式：

式中：ξ、η为由于重力异常造成的当地重力矢量方向相对于当地垂线方向的角度偏差。

纬度、经度和距地球表面的高度由下列公式给出：

其中，

本实施例中，在解算速度信息时，不仅考虑了纬度、经度和距地球表面的高度，还引入了由于重力异常造成的当地重力矢量方向相对于当地垂线方向的角度偏差，从而使得计算出的速度信息更为准确。

4)组合导航。

由微捷联惯性测量单元和移动站卫星接收机(GNSS)构成组合导航，GNSS负责重置惯性导航在一定时间周期内的累计误差。采用松组合结构，被估计的位置、速度和姿态误差用于修正惯性导航的解，具体架构如图2D所示。

本实施例的松组合结构是一个串联系统，来自GNSS的导航解输出位置和速度作为量测信息输入到卡尔曼滤波器中，用卡尔曼滤波器来估计捷联解算的误差。扩展Kalman滤波器(EKF)方法的具体流程如图3所示，包括以下步骤：步骤S302，建立系统模型和量测模型；步骤S304，输入初始估计值；步骤S306，在预测阶段进行线性化处理，估计误差协方差；步骤S308，计算卡尔曼增益，基于卡尔曼增益重新估计误差协方差。

本实施例提供的松耦合集成的主要优点是简单和冗余性，可以用于任何微惯性测量单元(即捷联惯性测量单元)和GNSS设备，特别适合改进算法应用，在松耦合的结构中，存在一个独立的GNSS可用导航解，其在组合导航解之外。在开环微惯性测量单元修正执行时，也存在一个独立微惯性测量单元导航解，这支持基本的并行导航解。

在本实施例的微惯性测量单元误差模型中，是基于小失准角的误差模型，通过一个小扰动表示误差因素影响，推导出微惯性测量单元误差的非线性微分方程。根据微惯性测量单元和GNSS的误差状态方程，组合系统的状态变量，由微惯性测量单元和GNSS的误差状态变量共同组合而成，可用式(27)表示。

其中，

车载微惯性测量单元的误差包括X轴方向滚转角偏差δα，Y轴方向俯仰角偏差δ

由误差模型对其简化处理得到状态转移矩阵F

其中，F

姿态角误差通过车载参考系到导航参考系的方向余弦矩阵和陀螺的误差联系用式(31)表示。

其中，C表示方向余弦矩阵元素。

F矩阵其中元素如式(32)到(43)表示：

其中R是地球半径，V

组合系统的量测模型体现在矩阵H中，将量测模型与滤波状态联系起来。本实施例的观测信息是GNSS系统的速度、位置和姿态，姿态包括偏航角α和俯仰角β，GNSS无法提供滚转角。GNS提供车辆的当前位置信息，以地理参考系为导航参考系，经度L

GNSS提供的位置表示为地理下的真值和误差之和，如(44)表示：

其中，L

其中，L

其中，V

上式中

其中，HDOP表示位置精度因子，

微惯性测量单元姿态量测表示为导航系下真值与相应姿态误差，GNSS姿态量测表示为N系下真值与相应姿态误差，航向角和俯仰角测量方程为(48)：

其中，

将位置、速度和姿态量(45)(46)(48)测合并表示如公式49所示：

其中，Z表示量测方程，Z

本实施例，采用速度、位置和姿态误差的方式建立组合系统的量测方程，比直接采用状态值更适合动态系统的变化。

5)初始对准。

利用卫星基准站测量的基准信息：经度、纬度、高度、姿态角、俯仰角和航向角信息。将相关的基准信息传递给捷联惯性测量单元，捷联惯性测量单元接收到基准信息，利用建立的EKF滤波器，结合自身惯性、卫星数据，完成速度、位置匹配下的传递对准。从而实现无人机端的初始对准。

本申请解决了现有技术中分立式的惯性、卫星导航系统体积大、成本高的技术问题，具有体积小、成本低的有益效果。

需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必须的。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本申请各个实施例所述的方法。

实施例3

根据本申请实施例，提供了一种超小型无人机用组合导航系统，包括捷联惯性测量单元及卫星差分定位系统。

捷联惯性测量单元，如图4所示，包括惯性测量组件42、导航计算机44、二次电源模块40、晶振48以及对外连接器46、移动站卫星接收机49。

1)惯性测量组件

惯性测量组件42主要进行载体的加速度和角速度信息获取，通过四线制SPI接口，将数据上传给导航计算机44。惯性测量组件42内置3轴陀螺仪和3轴加速度计，其具体指标如表1所示。

表1

硬件电路设计图如图5所示。芯片通过四线制SPI接口与导航计算机44相连接，供电电源为3.3V，配有独立的旁路电容进行滤波处理。

2)导航计算机

导航计算机44主要进行导航算法解算，本实施例中，将导航计算机44的工作主频设置为160MHz，配备浮点计算单元，自身存储程序空间为2MB，内存800KB，能够满足5ms一个周期的导航解算要求。此外，导航计算机44还具有丰富的DMA通道，用于进行片外芯片操作与逻辑运算。具体资源分配如下表2所示。

表2

具体硬件设计如图6所示。该芯片的最小系统包括：启动电容、复位电路和时钟源等。

3)二次电源模块

二次电源模块40如图7所示，主要是进行内部供电的调整与匹配。本实施例中，放置了两个独立芯片进行电源输出，一路用于提供给移动站卫星接收机49，一路提供给板上其他数字电路。为提高电源性能，减少设备间的干扰，在电源设计过程中，输入、输出均进行了π型滤波器设计。

4)晶振

本实施中晶振48具有温度特性好，可靠性高等特点，其稳定性≤20ppm。

5)对外连接器

本实施例中，对外连接器46的间距为1.0mm的表贴插针，并镀金，这样，在保证稳定性的前提下，减少接触阻抗。

星差分定位系统包括卫星差分定位系统、移动站卫星导航系统、移动站卫星机、移动站卫星接收天线、移动站卫星接收馈线、基准站卫星导航系统、基准站卫星接收机、基准站卫星接收天线、基准站卫星接收天线、基准站卫星接收馈线、基准站卫星接收馈线、基准站卫星天线连接器、基准站卫星天线连接器、二次电源模块、接口转换模块和数据融合单元。

1)移动站卫星导航系统

移动站卫星导航系统的具体指标如表3所示。

/>

表3

具体硬件设计电路如图8所示。

2)基准站卫星导航系统

基准站卫星导航系统同时具备定位和姿态测量功能，配合高精度惯性测量单元，实现高精度基准位姿获取。主要技术指标如下表4所示。

表4

卫星接收机用于接收高精度卫星定位数据，实现高精度的航空级定位精度。结合高精度的惯性测量单元和数据融合单元，实现数据有效融合，完成最终的位置、姿态初始测量。

实施例4

根据本申请实施例，还提供了一种组合导航装置的任务调度系统，包括：采集模块，被配置为通过多个惯性测量单元进行数据采集，并采用多线程来对所采集的数据进行补偿处理；对准模块，被配置为对补偿后的所述数据进行预处理，并基于预处理后的数据进行对准；调度模块，被配置在对准之后，采用多任务模块来对所述组合导航装置进行捷联任务调度。

可选地，本实施例中的具体示例可以参考上述实施例1和实施例2中所描述的示例，本实施例在此不再赘述。

实施例5

本申请的实施例还提供了一种存储介质。该存储介质被设置为存储用于执行以上实施例1和2中的方法的程序代码。

可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。

上述实施例中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。

在本申请的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

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