一种惯性导航系统的传感器数据获取方法及装置与流程

本发明涉及惯性导航技术领域，具体涉及一种惯性导航系统的传感器数据获取方法及装置。

背景技术：

传感器作为一种检测装置，能感受到被测量的信息，并能将感受到的信息，按一定规律变换成为电信号或其他所需形式的信息输出，以满足信息的传输、处理、存储、显示、记录和控制等要求。传感器在进行数据采集时通常会出现延时现象，出现这种现象的主要原因有：传感器支持的采样频率小；传感器采集的原始数据含有噪声，波动大，实际使用时一般会进行低通滤波处理，截止频率越低，滤波后的数据越平滑，但是相位滞后也越大，造成数据延时。

惯性导航是一种通过测量飞行器的加速度，并自动进行积分运算，获得飞行器瞬时速度和瞬时位置数据的技术。在惯性导航中，常用的传感器有加速度计，超声波，气压计，gps，光流模块等。其中，超声波，气压计，gps，光流模块等静态特性比较好，可较准确测量距离，高度，位置，速度等数据，但是受限于采样频率，数据延时较大，而传感器的延时较大则会造成估计数据的不准确。

技术实现要素：

有鉴于此，本发明实施例提供了一种惯性导航系统的传感器数据获取方法及装置，以解决惯性系统中传感器采集数据延时造成估计数据不准确的问题。

本发明实施例提供的技术方案如下：

本发明实施例第一方面提供一种惯性导航系统的传感器数据获取方法，该传感器数据获取方法包括：获取所述惯性导航系统历史状态的估计值及当前状态的测量值，其中，所述历史状态的估计值为根据所述传感器的延时时间记录的历史估计数据；根据所述惯性导航系统上一状态的估计值和上一状态的控制量计算得到当前状态的预测值；计算所述惯性导航系统当前状态的卡尔曼增益；根据所述惯性导航系统当前状态的预测值、所述历史状态的估计值、所述当前状态的测量值及所述当前状态的卡尔曼增益计算得到当前状态的估计值。

进一步地，该惯性导航系统的传感器数据获取方法通过以下步骤计算所述惯性导航系统当前状态的卡尔曼增益：获取上一状态估计值的协方差；根据所述上一状态估计值的协方差和所述惯性导航系统状态变化过程的噪声的协方差计算得到当前状态预测值的协方差；根据所述当前状态预测值的协方差和所述惯性导航系统测量的噪声的协方差计算得到所述当前状态的卡尔曼增益。

进一步地，所述获取上一状态估计值的协方差，包括：根据上一状态预测值的协方差和上一状态的卡尔曼增益计算得到上一状态的估计值的协方差。

进一步地，所述惯性导航系统的传感器数据获取方法还包括：获取所述的惯性导航系统的状态变量，其中，所述状态变量包括高度、速度和加速度偏差。

进一步地，所述惯性导航系统的传感器数据获取方法还包括：根据所述状态变量确定所述上一状态的估计值，其中，所述上一状态的估计值用下列公式表示：

其中xk-1|k-1表示上一状态的估计值，hk-1、vk-1及abiask-1表示状态变量。

本发明实施例第二方面提供一种惯性导航系统的传感器数据获取装置，该传感器数据获取装置包括：数据获取模块，用于获取所述惯性导航系统历史状态的估计值及当前状态的测量值，其中，所述历史状态的估计值为根据所述传感器的延时时间记录的历史估计数据；预测值计算模块，用于根据所述惯性导航系统上一状态的估计值和上一状态的控制量计算得到当前状态的预测值；卡尔曼增益计算模块，用于计算所述惯性导航系统当前状态的卡尔曼增益；估计值计算模块，用于根据所述惯性导航系统当前状态的预测值、所述历史状态的估计值、所述当前状态的测量值及所述当前状态的卡尔曼增益计算得到当前状态的估计值。

进一步地，所述卡尔曼增益计算模块还包括：协方差获取模块，用于获取上一状态估计值的协方差；预测值协方差计算模块，用于根据所述上一状态估计值的协方差和所述惯性导航系统状态变化过程的噪声的协方差计算得到当前状态预测值的协方差；卡尔曼增益计算模块，用于根据所述当前状态预测值的协方差和所述惯性导航系统测量的噪声的协方差计算得到所述当前状态的卡尔曼增益。

进一步地，所述协方差获取模块包括：估计值的协方差计算模块，用于根据上一状态预测值的协方差和上一状态的卡尔曼增益计算得到上一状态的估计值的协方差。

本发明实施例第三方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行如本发明实施例第一方面任一项所述的惯性导航系统的传感器数据获取方法。

本发明实施例第四方面提供一种惯性导航系统的传感器数据获取设备，其特征在于，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行如本发明实施例第一方面任一项所述的惯性导航系统的传感器数据获取方法。

本发明实施例提供的技术方案，具有如下效果：

本发明实施例提供的惯性导航系统的传感器数据获取方法及装置，对常规的卡尔曼估计流程进行改进，由于当前状态的估计值是通过当前状态的预测值和当前状态的测量值计算得到的，而当前状态的测量值是通过传感器进行采集，传感器在采集数据受限于采样频率和低通滤波等因素，采集的数据会出现延时的问题，因此传感器采集的当前状态的测量值实际是与当前状态相差延时时间的测量值，由此本发明实施例中将卡尔曼历史估计值保存，在计算当前状态的估计值时选择传感器采集的当前状态的测量值和保存的历史状态的估计值，该历史状态的估计值选择与传感器延时时间相应的状态的估计值，通过该方法计算的当前状态的估计值，因为采用的当前状态的测量值和历史状态的估计值在时间上表示同一状态，，使得计算结果更加准确，从而解决了传感器延时造成的估计数据的不准确和滞后问题。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是根据本发明实施例的惯性导航系统的传感器数据获取方法的流程图；

图2是根据本发明另一实施例的惯性导航系统的传感器数据获取方法的流程图；

图3是根据本发明实施例的惯性导航系统的传感器数据获取方法的数据处理曲线图；

图4是根据本发明另一实施例的惯性导航系统的传感器数据获取方法的数据处理曲线图；

图5是根据本发明实施例的惯性导航系统的传感器数据获取装置的结构框图；

图6是根据本发明另一实施例的惯性导航系统的传感器数据获取装置的结构框图；

图7是根据本发明实施例提供的惯性导航系统的传感器数据获取终端的硬件结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供一种惯性导航系统的传感器数据获取方法，如图1所示，该处理方法包括如下步骤：

步骤s101：获取惯性导航系统历史状态的估计值及当前状态的测量值。具体地，当前状态的测量值可以用zk表示，本发明实施例中可以采用气压计进行测量，也可以采用其他装置测量，本发明实施例对此不做限定。其中，当前状态的测量值可以采用气压计测量的高度表示，即可以用公式(1)表示：

zk＝hk气压公式(1)

历史状态的估计值是根据传感器的延时时间记录的历史数据，可以将历史的估计值保存在一个数组中，根据传感器的延时时间t，确定与当前状态前相差延时时间t的历史的估计值，即为本发明实施例中历史状态的估计值，假设传感器的延时时间为100ms，则本发明实施例中历史状态的估计值即为100ms前的估计值。

步骤s102：根据惯性导航系统上一状态的估计值和上一状态的控制量计算得到当前状态的预测值；具体地，该步骤可以用公式(2)表示：

xk|k-1＝axk-1|k-1+buk-1公式(2)

其中，xk|k-1表示当前状态的预测值，xk-1|k-1表示上一状态的估计值，uk-1表示上一状态的控制量，a和b为惯性导航系统的参数，a可以用公式(3)表示：

其中，t表示卡尔曼递推公式的时间间隔，t可以是5ms。

b可以用公式(4)表示：

上一状态的控制量可以用垂直方向的加速度表示，即可以用公式(5)表示：

uk-1＝ak-1公式(5)

其中，ak-1表示上一状态垂直方向的加速度。

步骤s103：计算惯性导航系统当前状态的卡尔曼增益。

步骤s104：根据惯性导航系统当前状态的预测值、历史状态的估计值、当前状态的测量值及当前状态的卡尔曼增益计算得到当前状态的估计值。具体地，该步骤可以用公式(6)表示：

xk|k＝xk|k-1+kk(zk-hkxhistory)公式(6)

其中，xk|k表示当前状态的估计值，xhistory表示历史状态的估计值，kk表示当前状态的卡尔曼增益，hk表示测量系统的参数，本发明实施例中，hk的值可以为1。

通过上述步骤s101至步骤s103，本发明实施例提供的惯性导航系统的传感器数据获取方法，对常规的卡尔曼估计流程进行改进，由于当前状态的估计值是通过当前状态的预测值和当前状态的测量值计算得到的，而当前状态的测量值是通过传感器进行采集，传感器在采集数据受限于采样频率和低通滤波等因素，采集的数据会出现延时的问题，因此传感器采集的当前状态的测量值实际是与当前状态相差延时时间的测量值，由此本发明实施例中将卡尔曼历史估计值保存在一个数组中，在计算当前状态的估计值时选择传感器采集的当前状态的测量值和保存的历史状态的估计值，该历史状态的估计值选择与传感器延时时间相应的状态的估计值，这样根据上述公式(6)，对当前状态的估计值进行计算时zk表示传感器采集的当前状态的测量值，该当前状态的测量值是与当前状态相差延时时间的测量值，xhistory表示历史状态的估计值，该历史状态的估计值是与当前状态相差延时时间的估计值，因此通过该方法计算的当前状态的估计值采用的当前状态的测量值和历史状态的估计值在时间上表示同一状态，计算的估计值的结果更加准确，解决了传感器延时造成的估计数据的不准确和滞后问题。

作为本发明实施例的一种可选的实施方式，如图2所示，该惯性导航系统的传感器数据获取方法可以通过以下步骤计算惯性导航系统当前状态的卡尔曼增益：

步骤s201：获取上一状态估计值的协方差；具体地，可以根据上一状态预测值的协方差和上一状态的卡尔曼增益计算得到上一状态的估计值的协方差。

步骤s202：根据上一状态估计值的协方差和惯性导航系统状态变化过程的噪声的协方差计算得到当前状态预测值的协方差；具体地，该步骤可以用公式(7)表示：

pk|k-1＝apk-1|k-1a^t+q公式(7)

其中，pk|k-1表示当前状态预测值的协方差，pk-1|k-1表示上一状态估计值的协方差，q表示惯性导航系统状态变化过程的噪声的协方差，a^t表示矩阵a的转置矩阵。

步骤s203：根据当前状态预测值的协方差和惯性导航系统测量的噪声的协方差计算得到当前状态的卡尔曼增益。具体地，该步骤可以通过公式(8)表示：

其中，表示矩阵hk的转置矩阵，r表示惯性导航系统测量的噪声的协方差。

作为本发明实施例的一种可选的实施方式，该惯性导航系统的传感器数据获取方法还包括：

获取惯性导航系统的状态变量，状态变量包括高度、速度和加速度偏差。具体地，可以根据状态变量确定上一状态的估计值，用公式(9)表示：

其中xk-1|表示上一状态的估计值，hk-1表示上一状态状态变量高度值、vk-1表示上一状态状态变量速度值，abiask-1表示上一状态状态变量加速度偏差值。本发明实施例中是以高度、速度和加速度偏差作为状态变量，利用加速度和气压计进行高度融合估计，输入信号为垂直方向的加速度，即本发明实施例中的控制量，观测信号为气压计高度值，即本发明实施例中的测量值。本发明实施例提供的惯性导航系统的传感器数据获取方法可以对垂直方向的高度进行估计，也可以对水平方向的位置进行估计，本发明实施例对此不做限定。

图3所示为传感器获取的垂直方向高度的数据，单位为cm，曲线1为采用气压计测量时(20ms采样周期)原始数据换算得到的气压高度，大约有50cm的高度值波动，曲线2为使用本发明估计得到的高度，数据平滑且相位超前于曲线1传感器原始高度，曲线3为巴特沃斯1hz截止频率获得的气压高度数据，从图中可见经过巴特沃斯数字滤波后的高度值有明显的时间滞后。相对于惯性导航主导传感器加速度计而言(2ms采样周期)，上述延时已经比较大，如果用上述巴特沃斯数字滤波后的延时数据和加速度数据进行高度融合，将会出现如图4所示的结果。由图4可以看出，曲线2的估计值准确性明显低于图3中的曲线2，图4中曲线2估计值有一段缓慢的向气压传感器靠近的过程，原因即在于传感器数据的延迟，造成估计值结果偏差。上述图3和图4所示的曲线验证了将气压计高度原始值(20ms采样延时)经过巴特沃斯滤波后进行估计会造成大约100ms的延时，用100ms的延时数据当成高度估计时的观测值，会造成结果准确性降低。

本发明实施例还提供一种惯性导航系统的传感器数据获取装置，如图5所示，该传感器数据获取装置包括：

数据获取模块1，用于获取惯性导航系统历史状态的估计值及当前状态的测量值，其中，历史状态的估计值为根据传感器的延时时间记录的历史数据；详细内容参见上述方法实施例中步骤s101的相关描述。

预测值计算模块2，用于根据惯性导航系统上一状态的估计值和上一状态的控制量计算得到当前状态的预测值；详细内容参见上述方法实施例中步骤s102的相关描述。

卡尔曼增益计算模块3，用于计算惯性导航系统当前状态的卡尔曼增益；详细内容参见上述方法实施例中步骤s103的相关描述。

估计值计算模块4，用于根据惯性导航系统当前状态的预测值、历史状态的估计值、当前状态的测量值及当前状态的卡尔曼增益计算得到当前状态的估计值；详细内容参见上述方法实施例中步骤s104的相关描述。

通过上述各个组成部分之间的协同配合，本发明实施例提供的惯性导航系统的传感器数据获取装置，对常规的卡尔曼估计流程进行改进，由于当前状态的估计值是通过当前状态的预测值和当前状态的测量值计算得到的，而当前状态的测量值是通过传感器进行采集，传感器在采集数据受限于采样频率和低通滤波等因素，采集的数据会出现延时的问题，因此传感器采集的当前状态的测量值实际是与当前状态相差延时时间的测量值，由此本发明实施例中将卡尔曼历史估计值保存在一个数组中，在计算当前状态的估计值时选择传感器采集的当前状态的测量值和保存的历史状态的估计值，该历史状态的估计值选择与传感器延时时间相应的状态的估计值，这样根据上述公式(6)，对当前状态的估计值进行计算时zk表示传感器采集的当前状态的测量值，该当前状态的测量值是与当前状态相差延时时间的测量值，xhistory表示历史状态的估计值，该历史状态的估计值是与当前状态相差延时时间的估计值，因此通过该装置计算的当前状态的估计值采用的当前状态的测量值和历史状态的估计值在时间上表示同一状态，计算的估计值的结果更加准确，解决了传感器延时造成的估计数据的不准确和滞后问题。

作为本发明实施例的一种可选的实施方式，如图6所示，该惯性导航系统的传感器数据获取装置中卡尔曼增益计算模块3包括：

协方差获取模块31，用于获取上一状态估计值的协方差；详细内容参见上述方法实施例中步骤s201的相关描述。

预测值协方差计算模块32，用于根据上一状态估计值的协方差和惯性导航系统状态变化过程的噪声的协方差计算得到当前状态预测值的协方差；详细内容参见上述方法实施例中步骤s202的相关描述。

卡尔曼增益计算模块33，用于根据当前状态预测值的协方差和惯性导航系统测量的噪声的协方差计算得到当前状态的卡尔曼增益。详细内容参见上述方法实施例中步骤s203的相关描述。

作为本发明实施例的一种可选的实施方式，该惯性导航系统的传感器数据获取装置中协方差获取模块31还包括：

估计值的协方差计算模块，用于根据上一状态预测值的协方差和上一状态的卡尔曼增益计算得到上一状态的估计值的协方差。

本发明实施例提供的惯性导航系统的传感器数据获取装置的功能描述详细参见上述实施例中惯性导航系统的传感器数据获取方法描述。

本发明实施例还提供了一种惯性导航系统的传感器数据获取终端，如图7所示，该惯性导航系统的传感器数据获取终端可以包括处理器51和存储器52，其中处理器51和存储器52可以通过总线或者其他方式连接，图7中以通过总线连接为例。

处理器51可以为中央处理器(centralprocessingunit，cpu)。处理器51还可以为其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现场可编程门阵列(field－programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。

存储器52作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明实施例中的惯性导航系统的传感器数据获取装置对应的程序指令/模块(例如，图5所示的数据获取模块1、预测值计算模块2、估计值计算模块3)。处理器51通过运行存储在存储器52中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法实施例中的惯性导航系统的传感器数据获取方法。

存储器52可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储处理器51所创建的数据等。此外，存储器52可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器52可选包括相对于处理器51远程设置的存储器，这些远程存储器可以通过网络连接至处理器51。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述一个或者多个模块存储在所述存储器52中，当被所述处理器51执行时，执行如图1、图4所示实施例中的惯性导航系统的传感器数据获取方法。

上述惯性导航系统的传感器数据获取终端具体细节可以对应参阅图1、图4所示的实施例中对应的相关描述和效果进行理解，此处不再赘述。

本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(read－onlymemory，rom)、随机存储记忆体(randomaccessmemory，ram)、快闪存储器(flashmemory)、硬盘(harddiskdrive，缩写：hdd)或固态硬盘(solid－statedrive，ssd)等；所述存储介质还可以包括上述种类的存储器的组合。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。