本发明涉及定位导航技术领域，更为具体来说，本发明为一种基于多核dsp的定位导航方法。
背景技术：
目前，随着技术的不断发展，人们对定位导航设备的精度提出了越来越高的要求。为提高定位导航设备的精度，常规的方案主要有两种：(1)建立更准确的系统模型、使用更高的采样频率；(2)基于惯性导航、gps、里程计等多源信息融合的方式来提高定位导航设备的精度。
对于方案(1)，建立更准确的系统模型意味着更高的系统阶数，这会使运算量呈指数级的增长；而更高的采样频率意味着需要更快的处理速度，这就提高了对处理器的性能要求，极大地增加了成本。
对于方案(2)，如果采用多源信息融合的方法，运算量会随着信息量的增大而增大，一般的硬件无法保证定位导航的实时性和准确性，而通过提高硬件能力的方式保证定位导航精度则必然大大地提高了成本，则无法推广使用。
因此，在保证定位导航精度的基础上，如何降低导航运算的复杂度、降低成本投入，成为了本领域技术人员亟待解决的技术问题和始终研究的重点。
技术实现要素：
为解决现有提高定位导航精度的方法存在的运算量大、成本高、实时性差、准确性差等问题，本发明创新地提出了一种基于多核dsp的定位导航方法，一方面选用具有并行运算能力强、内存空间大、成本低等优点的多核dsp作为硬件基础，另一方面通过巧妙地降低频率的方式对定位导航算法进行优化，从而实现在保证定位导航精度的同时降低了定位导航运算的复杂度。
为实现上述技术目的，本发明公开了一种基于多核dsp的定位导航方法，所述多核dsp具有第一内核和第二内核；所述定位导航方法包括如下步骤：
步骤1，通过第一内核采集惯性测量单元、里程计、高程计以及卫星设备发出的数据，并将采集的数据放入共享数据区中；
步骤2，通过第二内核读取放入共享数据区中的数据、将读取的数据用于定位导航解算；
步骤3，所述第二内核将定位导航解算结果发送至显示装置。
基于上述的技术方案，本发明通过第一内核和第二内核并行进行数据采集和导航解算的方式，从而有效提高运算效率、增强了运算实时性，以解决现有技术存在的成本高、实时性差、准确性差等问题，
进一步地，步骤1中，还包括通过所述第一内核对采集的数据进行预处理的步骤，然后将预处理后的数据写入共享数据区。
进一步地，步骤1中，所述预处理后的数据被写入共享数据区后，所述第一内核向第二内核发送核间中断信号；步骤2中，所述第二内核接收到所述核间中断信号后，才读取放入共享数据区中的数据。
进一步地，步骤2中，采用如下方式将读取的数据用于定位导航解算；
步骤21，设置状态量初值，根据先验数据对第二内核中的滤波器参数进行初始化操作，所述滤波器参数包括协方差矩阵、状态噪声矩阵及观测噪声矩阵；
步骤22，按照第一频率读取共享数据区中的惯性测量单元数据、里程计数据、高程计数据及卫星设备数据，并对读取的数据进行修正；
步骤23，利用修正后的惯性测量单元数据和初始位置姿态信息进行纯惯性导航解算，得到惯性姿态、速度及位置结果；
步骤24，利用状态量、惯性测量单元数据、里程计数据、高程计数据以及卫星数据得到用于组合导航的滤波观测矩阵和滤波增益矩阵，为组合导航滤波解算做准备；
步骤25，基于所述滤波观测矩阵和滤波增益矩阵，按照小于所述第一频率的第二频率进行组合导航滤波解算，根据组合导航滤波解算结果更新惯性姿态、速度及位置结果，将更新后的姿态、速度及位置结果作为定位导航解算结果；对未进行组合导航滤波解算的数据进行卡尔曼滤波解算，根据卡尔曼滤波结果更新状态量和协方差矩阵，将步骤23中得到的惯性姿态、速度及位置结果作为定位导航解算结果；
步骤26，返回步骤22。
基于上述的改进的技术方案，本发明通过降低了组合导航滤波解算的频率的方式，从而在保障定位导航精度的同时降低了算法运算的复杂度，从而解决现有技术存在的运算量大、成本高、实时性差、准确性差等问题。
进一步地，在步骤22中，包括判断是否收到停止导航指令的步骤，如果是，则停止导航；如果否，则读取共享数据区中的数据。
进一步地，在步骤23和步骤24之间，包括按照低于所述第一频率的第三频率对状态噪声矩阵进行离散化的步骤，然后执行步骤24。
进一步地，所述第一频率为1khz，所述第二频率为1hz，所述第三频率为100hz。
进一步地，步骤25中，通过计数器判断是否对当前待解算的数据进行组合导航滤波解算。
进一步地，在步骤23和步骤24之间，通过计数器判断是否对当前状态噪声矩阵进行离散化。
进一步地，步骤25中，组合导航滤波解算后，还包括修正惯组参数的步骤。
本发明的有益效果为：本发明一方面通过多核dsp中的第一内核和第二内核并行进行数据采集和导航解算的方式，从而有效提高运算效率、增强了运算实时性；本发明另一方面通过降低对滤波器矩阵离散化的频率和组合导航滤波解算的频率的方式，从而在保障定位导航精度的同时降低了算法运算的复杂度。
附图说明
图1为实施本发明的定位导航方法的硬件组成示意图。
图2为基于多核dsp的定位导航方法流程示意图。
具体实施方式
下面结合说明书附图对本发明的基于多核dsp的定位导航方法进行详细的解释和说明。
如图1、2所示，本发明公开了一种基于多核dsp的定位导航方法，根据功能要求和多核dsp的硬件资源，将原始数据采集任务和导航解算任务分配给两个独立的核来并行执行。本实施例中的多核dsp具有第一内核和第二内核；定位导航方法包括如下步骤：
步骤1，通过第一内核(core1)以固定频率采集惯性测量单元(imu)、里程计、高程计以及卫星设备发出的数据，通过第一内核对采集的数据进行预处理的步骤，然后将预处理后的数据写入共享数据区；预处理后的数据被写入共享数据区后，第一内核向第二内核发送核间中断信号。
步骤2，通过第二内核(core2)读取放入共享数据区中的数据、将读取的数据用于定位导航解算；本实施例中，只有第二内核接收到核间中断信号后，才读取放入共享数据区中的数据。具体地，采用如下方式将读取的数据用于定位导航解算。
步骤21，设置状态量初值，根据先验数据对第二内核中的滤波器参数进行初始化操作，滤波器参数包括协方差矩阵(p)、状态噪声矩阵(q)及观测噪声矩阵(r)。
步骤22，判断是否收到停止导航指令，如果是，则停止导航；如果否，则继续导航流程；按照第一频率读取共享数据区中的惯性测量单元数据、里程计数据、高程计数据及卫星设备数据，并对读取的数据进行修正。比如，可通过1ms定时中断的方式从共享数据区中读取惯性测量单元数据、里程计数据、高程计数据及卫星设备数据，且还可对上述数据进行预处理。
步骤23，利用修正后的惯性测量单元数据和初始位置姿态信息进行纯惯性导航解算，得到惯性姿态、速度及位置结果；还根据状态量和惯性测量单元数据求取状态转移矩阵(φ)；本实施例中，通过计数器判断是否对当前状态噪声矩阵进行离散化，并在满足条件时按照低于第一频率的第三频率对状态噪声矩阵进行离散化，本实施例还可对状态转移矩阵进行离散化，然后执行步骤24。
步骤24，利用状态量、惯性测量单元数据、里程计数据、高程计数据以及卫星数据得到用于组合导航的滤波观测矩阵(h)和滤波增益矩阵(k)，为组合导航滤波解算做准备；
步骤25，本实施例中，通过计数器判断是否对当前待解算的数据进行组合导航滤波解算，并在满足条件时：基于滤波观测矩阵和滤波增益矩阵，按照小于第一频率的第二频率进行组合导航滤波解算，根据组合导航滤波解算结果更新惯性姿态、速度及位置结果，将更新后的姿态、速度及位置结果作为定位导航解算结果，组合导航滤波解算后，还包括修正惯组参数的步骤；对未进行组合导航滤波解算的数据进行卡尔曼(kalman)滤波解算，根据卡尔曼滤波结果更新状态量和协方差矩阵，将步骤23中得到的惯性姿态、速度及位置结果作为定位导航解算结果；
步骤26，返回步骤22，循环执行上述步骤。
另外，上述的定位导航解算过程中，其时间更新过程为：
其中，x表示状态向量，φ表示状态转移矩阵，p表示协方差矩阵，q表示状态噪声矩阵。
另外，上述的定位导航解算过程中，其测量更新过程为：
其中，x表示状态向量，k表示滤波增益矩阵，h表示滤波观测矩阵，z表示观测量矩阵；p表示协方差矩阵，r表示观测噪声矩阵。
步骤3，第二内核将定位导航解算结果发送至显示装置，使操作者可获悉定位导航的最终结果。
本实施例中，涉及到的第一频率为1khz，即第二内核采用1khz进行原始数据采样及纯惯性解算；第二频率为1hz，即第二内核采用1hz进行组合导航滤波解算；第三频率为100hz，即第二内核采用100hz进行滤波器矩阵离散化。
另外，本发明可通过全局变量dzcount进行采样计数，以实现本发明需要使用的计数器功能。比如在步骤23中，判断dzcount％10是否为0(100hz)，若是则对q矩阵进行离散化，然后继续流程，若否跳过离散化直接继续流程；再比如在步骤25中，判断dzcount％1000是否为0(1hz)，若是，则进行组合导航滤波解算，更新协方差矩阵和状态向量矩阵，然后更新导航信息并修正惯组参数，若否，则进行简单kalman滤波，更新协方差矩阵和状态向量矩阵，不做导航信息更新和惯组参数修正。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本说明书的描述中，参考术语“本实施例”、“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明实质内容上所作的任何修改、等同替换和简单改进等，均应包含在本发明的保护范围之内。