CN102109349A - 一种具有ecef模型的mimu系统 - Google Patents

Abstract

本发明公开了一种具有ECEF模型的MIMU系统，该MIMU系统包括有信号调理模块(1)、模数转换模块(2)、平滑滤波模块(3)、模式设置模块(4)、误差模型模块(5)、模式解码模块(6)、ECEF模型(7)、惯性与GPS同步信息产生模块(8)和状态控制字产生模块(9)；MIMU系统是基于DSP和FPGA处理器的嵌入式平台，实现了以GPS数据为基准来进行惯性数据的速度与位置的高速融合。本发明融合后的数据能够辅助GPS接收机对环路的跟踪，以及提高跟踪环路的相对带宽，从而提高了GPS接收机的抗干扰能力。

Description

一种具有ECEF模型的MIMU系统

技术领域

本发明涉及一种微型惯性测量组合(MIMU)系统，更特别地说，是指一种具有ECEF模型的MIMU系统，以及用本发明MIMU系统辅助GPS跟踪环路的方法。

背景技术

微型惯性测量组合(MIMU，miniature inertial measurement unit)是一种重要的MEMS，它是由微型陀螺仪、微型加速度计，专用集成电路(ASIC)、嵌入式微机及相应的导航软件组成，可以提供运动载体的位置、速度和姿态信息。

2010年第2期总第244期的《计算机与数字工程》中公开了“北斗双星/SINS组合导航中的捷联惯导算法研究”。在该文章的图1中介绍了一种由计算机、导航计算机、控制显示、加速度计组合和陀螺组合形成的捷联惯导系统(INS)。该INS是基于地理坐标系作为导航坐标系的。

2008年2月第37卷第1期《测绘学报》中公开了“MEMS IMU辅助的高性能GPS接收机设计”。为了改善GPS接收机在高动态条件下或低信噪比条件下的性能，在GPS跟踪环路中引入MIMU辅助信息。MIMU辅助的卫星接收机示意图如图1所示。

发明内容

本发明的目的是提供一种具有ECEF模型的MIMU系统，该MIMU系统基于ECEF模型，利用磁通、加表和GPS的参数得到初始对准的姿态矩阵，通过利用陀螺输出的角速度和加表输出的比力信号，获得三轴速度

和三轴位置

作为辅助GPS接收机跟踪环路的信息。本发明融合后的数据能够辅助GPS接收机对环路的跟踪，以及提高跟踪环路的相对带宽，从而提高了GPS接收机的抗干扰能力；同时本发明设计MIMU系统也解决了光纤惯导辅助GPS接收机平台体积大、成本高的问题。

本发明的一种具有ECEF模型的MIMU系统，MIMU系统用于接收加速度计组合(10)输出的载体加速度信息AF₁₀＝{AFX₁₀，AFY₁₀，AFZ₁₀}、陀螺组合(20)输出的载体角速度信息AG₂₀＝{AGX₂₀，AGY₂₀，AGZ₂₀}、以及磁力计组合(30)输出的磁通信息AH₃₀＝{AHX₃₀，AHY₃₀，AHZ₃₀}；该MIMU系统包括有信号调理模块(1)、模数转换模块(2)、平滑滤波模块(3)、模式设置模块(4)、误差模型模块(5)、模式解码模块(6)、ECEF模型(7)、惯性与DPS同步信息产生模块(8)、状态控制字产生模块(9)；AFX₁₀表示加速度计的X轴输出的载体加速度信息，AFY₁₀表示加速度计的Y轴输出的载体加速度信息，AFZ₁₀表示加速度计的Z轴输出的载体加速度信息，AGX₂₀表示陀螺的X轴输出的载体角速度信息，AGY₂₀表示陀螺的Y轴输出的载体角速度信息，AGZ₂₀表示陀螺的Z轴输出的载体角速度信息，AHX₃₀表示磁力计的X轴输出的磁通信息，AHY₃₀表示磁力计的Y轴输出的磁通信息，AHZ₃₀表示磁力计的Z轴输出的磁通信息；

信号调理模块(1)用于对载体加速度信息AF₁₀＝{AFX₁₀，AFY₁₀，AFZ₁₀}、载体角速度信息AG₂₀＝{AGX₂₀，AGY₂₀，AGZ₂₀}、以及磁通信息AH₃₀＝{AHX₃₀，AHY₃₀，AHZ₃₀}进行噪声抑制滤波，得到调理后的调理-陀螺AG₁、调理-加表AF₁、以及调理-磁通AH₁；

模数转换模块(2)对接收的AG₁、AF₁和AH₁进行模数转换，得到数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂；

平滑滤波模块(3)对接收的数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂进行野点剔除和两级平滑滤波处理，得到平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃；

模式设置模块(4)用于接收上位机下发的模式命令，然后根据该命令进行不同模式启动，并将模式设置命令M₄输出给模式解码模块(6)；所述的模式命令包括有松组合模式切换命令、紧组合模式切换命令、辅助解算模式切换命令和GPS更新率；

误差模型(5)用于接收上位机下发的MIMU误差模型参数，然后根据该MIMU误差模型参数进行不同模式启动，并将误差模型数据M₅输出给模式解码模块(6)；该MIMU误差模型参数主要包括有三轴陀螺和三轴加表的误差模型参数；

模式解码模块(6)用于对接收的模式设置命令M₄和误差模型数据M₅进行解码和校验，然后输出CRC校验后命令MD₆给ECEF模型(7)；

状态控制字产生模块(9)用于产生状态控制字命令ST₉，并将该状态控制字命令ST₉输出给ECEF模型(7)；

惯性与GPS同步信息产生模块(8)对接收的平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃和GPS数据

进行数据状态字、GPS数据和惯性传感器数据的整合，产生与接收信息相同步的同步陀螺信息

同步加表信息同步磁通信息Mag^b、ECEF下初始三轴速度

和ECEF下初始三轴位置

然后将生产的同步信息输出给ECEF模型(7)；所述的数据状态字分别为系统状态字、传感器数据标识状态字、GPS数据标识控制字；

ECEF模型(7)依据CRC校验后的命令MD₆、状态控制字命令ST₉对接收到的三轴陀螺数据

三轴加表数据

三轴磁通数据Mag^b、ECEF下初始三轴速度

ECEF下初始三轴位置

GPS初始纬度L、GPS初始经度λ信息进行ECEF模型解算，得到ECEF的速度信息

和位置信息

然后将ECEF的速度信息

和位置信息

传送给GPS接收机进行跟踪环路的辅助，提高GPS接收机的抗干扰能力。

本发明具有ECEF模型的MIMU系统的优点在于：

1.以ECEF模型，并结合GPS数据对GPS接收机跟踪环路进行辅助，从而提高了GPS接收机的抗干扰能力。

2.以GPS数据为基准来进行惯性数据的速度与位置的高速融合。

3.采用野点剔除和两级平滑滤波相结合的方法对陀螺组合数据AG₂、加速度计组合数据AF₂和磁力计组合数据AH₂进行处理，处理后数据的方差比原始数据的方差降低了10倍左右，从而提高了惯性传感器数据的信噪比，从而从根源上提高了导航解算的精度。

4.惯性与GPS同步信息产生模块利用系统状态字、传感器数据标识状态字和GPS数据标识控制字对GPS数据和惯性数据进行打包同步控制，以作为第二DSP解算的控制标志。

5.通过MD₆和ST₉对第二DSP处理器进行设置，能够灵活控制MIMU系统的模式选择，以及GPS接收机数据更新率的选择，以及MIMU数据的模型参数。

6.采用在ECEF下获取初始姿态矩阵

实现了MIMU系统在ECEF坐标下的姿态矩阵的实时更新。

7.本发明采用DSP和FPGA处理器的硬件平台，其尺寸小(由四块电路板组成，分别为97mm×70mm、92mm×65mm、45mm×25mm、35mm×25mm)、成本低(由微机械传感器芯片组成，相对光纤惯性传感器成本较低)。

8.在硬件平台上，作为系统逻辑和时序控制核心的FPGA不仅是系统的总控制器，同时能在FPGA内部完成传感器数据的平滑滤波，实现传感器数据的消噪。

附图说明

图1是本发明具有ECEF模型的MIMU系统的结构框图。

图2是本发明ECEF模型的结构框图。

具体实施方式

下面将结合附图对本发明做进一步的详细说明。

参见图1所示，本发明的一种具有ECEF模型的MIMU系统，该MIMU系统包括有信号调理模块1、模数转换模块2、平滑滤波模块3、模式设置模块4、误差模型模块5、模式解码模块6、ECEF模型7、惯性与DPS同步信息产生模块8、状态控制字产生模块9。

本发明MIMU系统用于接收加速度计组合10输出的载体加速度信息AF₁₀、陀螺组合20输出的载体角速度信息AG₂₀、以及磁力计组合30输出的磁通信息AH₃₀；

本发明MIMU系统用于输出ECEF(Earth Centered Earth Fixed，地固地心直角坐标系)坐标系下的三轴速度

三轴位置

给GPS接收机40。该三轴速度

和三轴位置

用于辅助GPS接收机40中的跟踪环路，从而提高了GPS接收机的抗干扰能力。

在本发明中，平滑滤波模块3、模式解码模块6、惯性与DPS同步信息产生模块8和状态控制字产生模块9由FPGA处理器实现。FPGA芯片用Verlog HDL硬件描述语言编程。

在本发明中，模式设置模块4和误差模型5由第一DSP处理器实现。ECEF模型7由第二DSP处理器实现。DSP芯片上用C语言编程。

下面将对本发明具有ECEF模型的MIMU系统中的各个模块实现的功能进行详细说明。

(一)信号调理模块1

信号调理模块主要是对传感器数据进行噪声抑制滤波。输入三轴陀螺数据AG₂₀(即AG₂₀＝{AGX₂₀，AGY₂₀，AGZ₂₀}，X、Y和Z表示陀螺的三个轴)、三轴加表数据AF₁₀(即AF₁₀＝{AFX₁₀，AFY₁₀，AFZ₁₀}，X、Y和Z表示加表的三个轴)和三轴磁通数据AG₃₀(即AH₃₀＝{AHX₃₀，AHY₃₀，AHZ₃₀}，X、Y和Z表示磁通的三个轴)，经过信号调理输出调理后的三轴陀螺数据AG₁(简称为调理-陀螺AG₁)、三轴加表数据AF₁(简称为调理-加表AF₁)、以及三轴磁通数据AH₁(简称为调理-磁通AH₁)。在本发明中，AG₁、AF₁和AH₁中分别也包括了X、Y和Z三轴，即9路信息，为了方便说明，此后的文字不作X、Y和Z三轴的分别叙述。

由于传感器输出的原始信号存在高频噪声，若直接进入模数转换模块会导致误差较大，所以要对传感器数据输出的数据进行调理。在进行模数转换前选用TI公司的运算放大器OPA2350，构成一阶有源低通滤波器，不仅可以消除高频噪声，而且还可以解决输入输出阻抗不匹配的问题。OPA2350及其外围电路组成的低通滤波电路的截止频率应为MIMU信号带宽的5倍，这里通过OPA2350附加外围电路设计的一阶有源低通滤波的截止频率是1000HZ，这样有效的滤除了高频噪声的干扰。

(二)模数转换模块2

模数转换模块2对AG₁、AF₁和AH₁进行模数转换，得到数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂，并将AG₂、AF₂和AH₂输出给平滑滤波模块3。

模数转换模块2选用三片TI公司的ADS1274芯片，其最大的采集速率是144kSPS，24位高精度数据输出。

(三)平滑滤波模块3

平滑滤波模块3对数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂进行野点剔除和两级平滑滤波处理，得到平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃，并将AG₃、AF₃和AH₃输出给惯性与DPS同步信息产生模块8。

微惯性传感器输出的原始值噪声较大，若直接用原始数据进行导航解算会大大影响解算的精度，通过(一)的信号调理模块从硬件设计上已经对噪声进行了抑制，在利用传感器数据进行解算前在FPGA处理器中还需要软件方面的平滑滤波。为了消除传输过程中个别点受到干扰以及提高信噪比，本发明提出了野点剔除和两级平滑滤波相结合的方法。

上述平滑滤波模块是在FPGA中完成的，利用了FPGA处理器的强大的并行处理能力，经过模数转换后的数据AG₂、AF₂和AH₂数据分别同时进入平滑滤波模块进行并行处理，经过平滑滤波后的数据输出并行的AG₃、AF₃和AH₃，下面以陀螺输出的角信息进行平滑滤波为例进行平滑滤波步骤说明，依据同样的方法能够计算出加表、磁通的阈值。

步骤3-1：首先记录下惯性传感器前后两次采样的阈值。具体方法为：由于陀螺的量程为±300°/s，以10kHz的前端采样率进行采样，正常情况下两个数之间转过的最大角度为300/10k，由于陀螺的刻度系数为0.05，根据陀螺的数据手册换算到3δ(3倍方差)的电压值计算可以得到阈值大约等于18。

步骤3-2：FPGA中设置一个存储寄存器存储上一个采样点的数据，下一个数据到来后和前一个做差，若其差的绝对值大于第一步求得的阈值，即认为是野值，则野点剔除模块输出的值是上一个正确的IMU；若当前的IMU值不是野值，则将当前IMU值输出，并且存储寄存器的值更新为当前的IMU值。

步骤3-3：对步骤3-2野点剔除输出的数据每10个进行一次相加取平均值，将这个平均值作为最终数据输出给步骤3-4，通过这步操作，相当于将10k的采集速率降为1000Hz。

步骤3-4：对步骤3-3输出的数据进行窗口长度为8的滑窗平滑。所谓的窗口类似长度为8的FIFO队列，每次把窗口中的8个数据进行加权平均，然后把最早到窗口的数据移出窗口，其它窗口数据依次移位，收取一个当前新的数据到窗口的末尾。

步骤3-5：对步骤3-4输出的窗口输出数据每10个再进行一次相加取平均值，通过这步操作，将这个平均值作为最终数据输出给步骤3-6，相当于将步骤3-4输出的数据采集速率降为100Hz。

步骤3-6：对步骤3-5输出的数据进行同步骤3-4的窗口长度为8的滑窗平滑。并将平滑后的数据作为最终的平滑滤波输出数据。

通过以上几个操作步骤，将原始数据的干扰点剔除，并且消除了传感器数据的原始噪声，使数据更加平滑数据平滑，经过实际证明，经过野点剔除和两级平滑滤波模块的数据其方差比原始数据的方差降低了10倍左右，从而提高了惯性传感器数据的信噪比，从而从根源上提高了导航解算的精度。

(四)模式设置模块4

模式设置模块4用于接收上位机下发的模式命令，然后根据该命令进行不同模式启动(简称为模式设置命令M₄)，并将模式设置命令M₄输出给模式解码模块6。该模式设置命令M₄主要包括有模式切换命令(松组合、紧组合和辅助解算)和GPS更新率。

在本发明中，模式设置模块4在第一DSP处理器上实现，该第一DSP处理器通过RS232总线接受上位机的模式设置命令。上位机(即计算机)是一种能够按照事先存储的程序，自动、高速地进行大量数值计算和各种信息处理的现代化智能电子设备。最低配置为CPU 2GHz，内存2GB，硬盘180GB；操作系统为windows2000/2003/XP。接收的上位机的命令格式如下面所示：

每一个数据包中采用的数据类型描述如下：

BYTE：8-bit无符号整数。

SINGLE：32-bit(4-byte)单精度浮点数，采用″ANSI/IEEE Std 754SINGLE″格式。发送的第一、二个字节发送的是小数的低16位，第三个字节包含偏移阶码的最低位和小数的高7位；第四个字节包含尾数符号位和偏移阶码的高7位。实际阶码＝偏移阶码-127。加上隐含的小数点前面的1，SINGLE尾数实际包含24-bit有效位。低字节先发送。

信息的发送和接收，都采用下面的数据包结构，每一个数据包顺序以下面两个字节开头：

<DLE>ASCII字符，1-byte，＝10h

<id>数据包标志符，1-byte，可取00h～FFh中除10h(ASCII<DLE>)和03h(ASCII<ETX>)中任何值，其取值和定义见后面说明。

每一个数据包顺序以下面两个字符结束：

<DLE>ASCII字符，1-byte，＝10h

<ETX>ASCII字符，1-byte，＝03h

模式命令M₄格式采用集合形式表达为：

M₄＝{<DLE>，<id>，<DATA>，<CRC>，<DLE>，<ETX>}。

数据包中每个“data byte”可取任何8-bit数，但如果data byte＝10h(<DLE>)，则该byte必须发两次。

<CRC>帧校验字，2bytes，校验算法采用CRC校验，对id和DATA进行校验，但多发的DLE不参与校验。如果CRC byte＝10h(<DLE>)，则该byte必须发两次。

本系统规定系统模式切换命令的id号06H，其具体含义如下：

DATA	参数	类型
			1	组合模式切换(I)	BYTE
2	GPS更新率(II)	BYTE

(I)“01H”表示浅组合，“02H”表示深组合，03H表示为辅助解算

(II)“01H”、“02H”、“03H”和“04H”分别表示GPS的更新率为1Hz，5Hz，10Hz，20Hz

(五)误差模型5

误差模型5用于接收上位机下发的MIMU误差模型参数，然后根据该MIMU误差模型参数进行不同模式启动，并将误差模式的数据命令格式M₅(简称为模型误差数据M₅)输出给模式解码模块6。该MIMU误差模型参数主要包括有三轴陀螺和三轴加表的误差模型参数。

误差模型5在第一DSP处理器上实现，该第一DSP处理器通过RS232接收上位机的MIMU误差模型参数。误差模式的数据命令格式M₅与模式设置模块4中的命令格式一致，仅增加了id号。

5-1：三轴陀螺参数：id号为04H

其数据包定义如下：

DATA	参数	类型
			1～4	X轴第一陀螺参数	SINGLE
5～8	X轴第二陀螺参数	SINGLE
			9～12	X轴第三陀螺参数	SINGLE
13～16	Y轴第一陀螺参数	SINGLE
			17～20	Y轴第二陀螺参数	SINGLE
21～24	Y轴第三陀螺参数	SINGLE
			25～28	Z轴第一陀螺参数	SINGLE
29～32	Z轴第二陀螺参数	SINGLE
			33～36	Z轴第三陀螺参数	SINGLE

5-2：三轴加速计数：id号为05H

其数据包定义如下：

DATA	参数	类型
			1～4	X轴第一加速计参数	SINGLE
5～8	X轴第二加速计参数	SINGLE
			9～12	X轴第三加速计参数	SINGLE
13～16	Y轴第一加速计参数	SINGLE
			17～20	Y轴第二加速计参数	SINGLE
21～24	Y轴第三加速计参数	SINGLE
			25～28	Z轴第一加速计参数	SINGLE
29～32	Z轴第二加速计参数	SINGLE
			33～36	Z轴第三加速计参数	SINGLE

(六)模式解码模块6

模式解码模块6用于对接收的模式设置命令M₄和误差模型数据M₅进行解码和校验，然后输出CRC校验后命令MD₆给ECEF模型7。

模式解码模块6是在FPGA中实现的，主要是对模式设置模块4中的模式设置命令M₄和误差模型数据M₅进行解码和校验，并将模式设置命令和误差模型参数写进Flash，若下次系统上电后上位机不发送命令，则FPGA将上次写进Flash的命令和模型参数读出，作为此次默认的命令和参数。FPGA对模式设置命令M₄和误差模型数据M₅的数据进行解码，做CRC校验后形成CRC校验后命令MD₆发送给ECEF模型7(第二DSP处理器)。

(七)状态控制字产生模块9

状态控制字产生模块9用于产生状态控制字命令ST₉，并将该状态控制字命令ST₉输出给ECEF模型7。

状态控制字产生模块9在FPGA处理器上实现，是由FPGA内部计时控制产生的状态控制字命令ST₉，上电开始后的前2分钟为数据准备阶段，在这一阶段，通知设备不做任何操作，相当于系统的预热时间，在这一时间给状态标志位置为001。数据准备后的3分钟为初始对准阶段，在这一阶段，给状态标志位置为010，通知第二DSP此阶段为初始对准阶段，通知第二DSP进行初始对准算法，求出初始姿态矩阵。此后的阶段为辅助解算阶段，在这一阶段，给状态标志位置100，通知ECEF模型7(第二DSP处理器)进行辅助解算，算得辅助信息，进行跟踪环路的辅助。

(八)惯性与GPS同步信息产生模块8

惯性与GPS同步信息产生模块8对接收的平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃和GPS数据

进行数据状态字、GPS数据和惯性传感器数据的整合，产生与接收信息相同步的同步陀螺信息

同步加表信息

同步磁通信息Mag^b、ECEF下初始三轴速度

和ECEF下初始三轴位置

然后将生产的同步信息输出给ECEF模型7(第二DSP处理器)。

惯性与GPS同步信息产生模块8在FPGA处理器中实现，其需要将传感器(三轴陀螺、三轴加速度计和三轴磁通计)数据

Mag^b和GPS接收机的数据

进行系统状态字、GPS数据和惯性传感器数据的整合，产生同步信息，传给第二DSP能够识别的具有实际意义的数据包，以进行解算。

在本发明中，所述的数据状态字分别为系统状态字、传感器数据标识状态字、GPS数据标识控制字。ECEF模型7(第二DSP处理器)通过这些状态标识控制字识别当前状态和区分数据。引入秒脉冲信号(PPS)来控制惯性与GPS同步信息产生模块8的处理方式。

系统系统状态字：最高位第15位为1表示GPS数据的CRC校验错误，为0表示校验通过，第12到第10位表示当前状态，为001.表示当前为数据准备阶段，010表示为初始对准阶段，100表示为辅助解算阶段。低8位表示GPS更新率，这里暂时未用。

传感器数据更新率较高，经过数据处理后的更新率为100HZ；GPS的更新率比较低。经过数据同步打包模块传递给第二DSP的数据是传感器数据和GPS数据的组合。要求传感器数据和GPS数据是同一时刻的数据，这就要求给第二DSP传递一包数据的时候要有包号标识，第二DSP通过这些标识区分不同时刻和同一时刻的数据。

以GPS的更新率为5HZ为例，每两个传感器之间的数据间隔为10ms，PPS到来和GPS信号到来之间是有一个时间延迟，延迟时间大约为15ms。根据传感器数据标识状态字、GPS数据标识控制字，16位传感器数据标识状态字的高8位和16位GPS数据标识控制字的高8位代表的是当前第几个PPS，每来一个PPS信号，PPS的状态标识位加1，加到255后PPS状态标识位清零，此后，PPS到来后状态标识位再重新加1，一直这样循环；16位传感器数据标识状态字的低8位表示是第几个传感器数据，每两个PPS之间传感器数据数据一共有200个，每来一个新的PPS，传感器数据包号清零，重新开始计数。

在本发明中，传感器数据与GPS同步信息产生模块8中的系统状态字作如下说明：

8-1：初始的高8位是系统状态控制位：

低8位是GPS数据更新率：

“01H”、“02H”、“03H”和“04H”分别表示GPS的更新率为1Hz，5Hz，10Hz，20Hz

8-2：16位传感器数据标识状态字

15～8	7～0
		标识PPS信号包号	标识这个PPS下的第几个传感器数据

说明：这里的PPS信号的包号标识的是第几个PPS信号，一共有8位，最大能表示的是数是255，也就是表征PPS信号的寄存器4分钟左右更新一次，重新开始计数。后8位标识是第几个传感器数据，每来一个PPS信号，传感器数据包号清零一次，第二DSP通过这两个标识来识别确定同一时刻的传感器数据和GPS数据。

8-3：16位GPS数据标识控制字

15～8	7～0
		标识PPS信号包号	标识这个PPS的GPS数据包号

说明：这里的PPS信号的包号标识的是第几个PPS信号，一共有8位，最大能表示的是数是255，也就是表征PPS信号的寄存器4分钟左右更新一次，重新开始计数。后8位是GPS的数据包号，每个PPS清零一次重新开始计数包号。第二DSP通过包号来识别确定同一时刻的传感器数据和GPS数据。

(九)ECEF模型7

参见图2所示，ECEF模型7在第二DSP处理器中完成，ECEF模型7依据CRC校验后的命令MD₆、状态控制字命令ST₉对接收到的三轴陀螺数据

三轴加表数据

三轴磁通数据Mag^b、ECEF下初始三轴速度

ECEF下初始三轴位置

GPS初始纬度L、GPS初始经度λ信息进行ECEF模型解算，得到ECEF的速度信息

和位置信息

然后将ECEF的速度信息和位置信息

传送给GPS接收机进行跟踪环路的辅助，提高GPS接收机的抗干扰能力。

在本发明中，ECEF模型解算执行下列步骤：

步骤7-1：依据MD₆、ST₉，进行自身程序初始化控制；

步骤7-2：在初始对准单元中对L、λ、Mag^b、

信息进行初始化，获取当前时刻T下载体坐标系b到ECEF坐标系e的坐标转换矩阵的初始姿态矩阵

在本发明中，载体姿态中的航向角记为ψ、俯仰角记为θ和横滚角记为φ；载体的具体航迹推算为：

推导步骤一：求初始航向角

初始对准要求磁力计传感器测得的值在水平面内，如果不在水平面内，应当进行坐标变换，变换方法为：

$\left[\begin{array}{c}{\mathrm{Mag}}_x^b\\ {\mathrm{Mag}}_y^b\\ {\mathrm{Mag}}_z^b\end{array}\right]=C_g^b\left[\begin{array}{c}{\mathrm{Mag}}_x^g\\ {\mathrm{Mag}}_y^g\\ {\mathrm{Mag}}_z^g\end{array}\right]---\left(1\right)$

其中：Mag^b的下角标的x，y，z表示磁力计在载体坐标系下的三个轴，即X轴、Y轴和Z轴，b表示载体坐标系；

Mag^g的下角标的x，y，z表示磁力计在地理坐标系下的三个轴，即X轴、Y轴和Z轴，g表示地理坐标系；

表示地理坐标系g到载体坐标系b的磁通坐标转换矩阵。

在载体姿态条件下，坐标转换矩阵

为：

$C_g^b=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& -\sin \mathrm{\&phi;}\\ 0& 1& 0\\ \sin \mathrm{\&phi;}& 0& \cos \mathrm{\&phi;}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ 0& -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& \sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(2\right)$

的正交矩阵记为

即

T表示矩阵转置符号(常用数学规定)，所以由式(2)可以得到载体系到地理系的坐标转换矩阵

的表达式为：

$C_b^g=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& \sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(3\right)$

由联立式(1)和(3)可得：

${\mathrm{Mag}}_x^g={\mathrm{Mag}}_z^b\sin \mathrm{\&phi;}+{\mathrm{Mag}}_x^b\cos \mathrm{\&phi;}$

${\mathrm{Mag}}_y^g={\mathrm{Mag}}_x^b\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}+{\mathrm{Mag}}_y^b\cos \mathrm{\&theta;}-{\mathrm{Mag}}_z^b\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}---\left(4\right)$

表示在地理坐标系g下载体X轴上的磁通量；

表示在地理坐标系g下载体Y轴上的磁通量；

表示在地理坐标系g下载体Z轴上的磁通量；

由式(4)和载体姿态的航向角ψ可得航向角为：

$\mathrm{\&psi;}={\tan }^{-1}\frac{{\mathrm{Mag}}_x^g}{{\mathrm{Mag}}_y^g}---\left(5\right)$

推导步骤二：求初始俯仰角和横滚角

对初始的得到的同步加表信息

进行误差补偿，从而得到标定后的三轴加表数据

(i表示惯性坐标系)，由坐标转换关系可得：

$f_{\mathrm{ib}}^b=C_n^b{f}^n---\left(6\right)$

其中，fⁿ为导航坐标系n下比力，

为导航坐标系n到载体坐标系b的坐标转换矩阵，由坐标转换关系得坐标转换矩阵

的表达式为：

$C_n^b=\left[\begin{array}{ccc}\cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)-\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)+\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& -\sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\\ -\cos \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&theta;}\right)\\ \sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)+\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)-\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\end{array}\right]---\left(7\right)$

由联立式(6)和式(7)可以得到：

$\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b\\ f_{\mathrm{iby}}^b\\ f_{\mathrm{ibz}}^b\end{array}\right]=\left[\begin{array}{ccc}\cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)-\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)+\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& -\sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\\ -\cos \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&theta;}\right)\\ \sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)+\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)-\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -G\end{array}\right]---\left(8\right)$

的下角标的x，y，z表示加速度计在载体坐标系b下的三个轴，即X轴、Y轴和Z轴；

表示在载体坐标系b下X轴的加速度信息；

表示在载体坐标系b下Y轴的加速度信息；

表示在载体坐标系b下Z轴的加速度信息；

在导航坐标系n下 $f^n=\left[\begin{array}{c}0\\ 0\\ -G\end{array}\right];$

由化简式(8)得到：

$f_{\mathrm{ibx}}^b=G\sin \mathrm{\&phi;}\cos \mathrm{\&theta;},$ $f_{\mathrm{iby}}^b=-G\sin \mathrm{\&theta;},$ $f_{\mathrm{ibz}}^b=-G\cos \mathrm{\&phi;}\cos \mathrm{\&theta;}---\left(9\right)$

G表示重力加速度；

由式(9)可得初始俯仰角

和初始横滚角

推导步骤三：求初始ECEF下的姿态矩阵

由坐标转换矩阵转换关系可得：

$C_n^e=\left(\begin{array}{ccc}-\sin \mathrm{\&lambda;}& -\sin L\cos \mathrm{\&lambda;}& \cos L\cos \mathrm{\&lambda;}\\ \cos \mathrm{\&lambda;}& -\sin L\sin \mathrm{\&lambda;}& \cos L\sin \mathrm{\&lambda;}\\ 0& \cos L& \sin L\end{array}\right)---\left(10\right)$

表示导航坐标系n到ECEF坐标系e下的坐标转换矩阵；

$C_b^n=\left(\begin{array}{ccc}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}-\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&phi;}\cos \mathrm{\&psi;}+\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\\ \cos \mathrm{\&phi;}\sin \mathrm{\&psi;}+\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&phi;}\sin \mathrm{\&psi;}-\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\cos \mathrm{\&phi;}\end{array}\right)---\left(11\right)$

通过初始时刻GPS接收机的纬度L、经度λ，代入式(10)可得初始坐标变换矩阵

通过推导步骤一和推导步骤二中求得的姿态角ψ、θ和φ，代入式(11)，可得坐标变换矩阵通过由关系

可以得到

将此时得到的

作为初值；

表示在当前时刻T时，载体坐标系b到ECEF坐标系e下的坐标转换矩阵。

步骤7-3：

对初始的得到的三轴陀螺数据进行误差补偿，从而得到标定后的

利用和步骤7-2求得的姿态矩阵的初始值

后来进行ECEF下姿态矩阵

的更新，具体步骤如下：

时间导数

为：

${\stackrel{\&CenterDot;}{C}}_b^e=C_b^e{\mathrm{\&Omega;}}_{\mathrm{ib}}^b-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{C}_b^e---\left(12\right)$

式(12)，是角速度的斜对称矩阵，且 ${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left(\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right);$

式(12)，

是地球旋转矢量的斜对称矩阵，且

ω_ie为地球自转角速度。

式(12)，

的下角标的x，y，z表示陀螺在载体坐标系b下的三个轴，即X轴、Y轴和Z轴；

对式(12)进行积分得：

其中，τ表示时间间隔，T+τ表示下一时刻，

为姿态增量的斜对称矩阵，I3表示单位矩阵；

表示下一时刻T+τ的载体坐标系b到ECEF坐标系e下的坐标转换矩阵。

对式(13)的幂级数进行一阶泰勒展开，并在时间间隔τ内角速度是不变的，即角速度增量

由此可以得到坐标变换矩阵更新后的最终形式：

$C_b^e(T+\mathrm{\&tau;})\&ap;C_b^e\left(T\right)(I_3+{\mathrm{\&Omega;}}_{\mathrm{ib}}^b\mathrm{\&tau;})-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{C}_b^e\left(T\right)\mathrm{\&tau;}---\left(14\right)$

步骤7-4：进行ECEF下的比力更新

由不同坐标系下比力的转换关系可得：

$f_{\mathrm{ib}}^e\left(T\right)=C_b^e\left(T\right)f_{\mathrm{ib}}^b\left(T\right)\&ap;\frac{1}{2}(C_b^e\left(T\right)+C_b^e(T+\mathrm{\&tau;}))f_{\mathrm{ib}}^b\left(T\right)---\left(15\right)$

表示当前时刻T在ECEF坐标系下相对载体坐标系b下的比力，i表示惯性坐标系；

表示当前时刻T在载体坐标系b下的比力；

由不同坐标系下速度转换关系可以得到：

${\mathrm{\&upsi;}}_{\mathrm{ib}}^e\&ap;\frac{1}{2}(C_b^e\left(T\right)+C_b^e(T+\mathrm{\&tau;})){\mathrm{\&upsi;}}_{\mathrm{ib}}^b\left(T\right)---\left(16\right)$

其中

为ECEF坐标系相对载体系下的速度，

为当前时刻T载体坐标系下速度；

步骤7-5：ECEF下速度更新

由速度、加速度和位置之间的关系可以得到下式：

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=a_{\mathrm{eb}}^e={\stackrel{\&CenterDot;\&CenterDot;}{r}}_{\mathrm{eb}}^e---\left(17\right)$

$r_{\mathrm{eb}}^e=r_{\mathrm{ib}}^e-r_{\mathrm{ie}}^e---\left(18\right)$

其中，

为ECEF坐标系下速度的微分，

为ECEF坐标系下加速度，

为ECEF坐标系下位置的双重微分，

为ECEF坐标系下的位置，

为ECEF坐标系相对载体坐标系的位置，

为ECEF坐标系相对惯性坐标系下的位置。

由于地球自转角速度

为常值，则

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{ib}}^e-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\stackrel{\&CenterDot;}{r}}_{\mathrm{eb}}^e+a_{\mathrm{ib}}^e---\left(19\right)$

表示

和重力矢量引起的加速度

的和；

将式(18)及位置、速度之间的关系

(表示ECEF坐标系下的速度)，代入式(19)中得到：

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e+a_{\mathrm{ib}}^e---\left(20\right)$

其中

是

和重力矢量引起的加速度

的和；ECEF下的重力模型

是重力矢量引起的加速度

与离心加速度

的和，即

$a_{\mathrm{ib}}^e=f_{\mathrm{ib}}^e\left(T\right)+{\mathrm{\&gamma;}}_{\mathrm{ib}}^e$

$g_b^e={\mathrm{\&gamma;}}_{\mathrm{ib}}^e-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e---\left(21\right)$

将(20)式代入(21)中得到

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=f_{\mathrm{ib}}^e\left(T\right)+g_b^e\left(r_{\mathrm{eb}}^e\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e---\left(22\right)$

在一个采样时间间隔内速率比力

和重力模型

是不变的，由此对(22)进行积分运算得到下一时刻的速度为：

$v_{\mathrm{eb}}^e(T+\mathrm{\&tau;})\&ap;v_{\mathrm{eb}}^e\left(T\right)+(f_{\mathrm{ib}}^e\left(T\right)+g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e\left(T\right))\mathrm{\&tau;}$ (23)

$=v_{\mathrm{eb}}^e\left(T\right)+{\mathrm{\&upsi;}}_{\mathrm{ib}}^e+(g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e\left(T\right))\mathrm{\&tau;}$

在导航坐标系下的重力模型可以看做是关于纬度函数，WGS-84的重力数值式为G＝978.03267714×(1+0.00193185138639sin²L)/(1-000669437999013sin²L)^1/2其中L为当地纬度。导航坐标系下的重力G通过乘以坐标变换矩阵

得到ECEF下的重力模型，即

由于

其中L为纬度，λ为经度，正弦下的

正弦下的

余弦下的 $\cos \mathrm{\&lambda;}=\frac{x}{\sqrt{x^2+y^2}}.$

在本发明中，x、y、z为ECEF坐标系中的坐标值。将正弦下的

正弦下的

余弦下的

代入

中，得到：

$C_n^e=\left(\begin{array}{ccc}-\frac{y}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{x}{\sqrt{x^2+y^2}}& \frac{x}{\sqrt{x^2+y^2+z^2}}\\ \frac{x}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{y}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2+z^2}}\\ 0& \frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}}& \frac{z}{\sqrt{x^2+y^2+z^2}}\end{array}\right)---\left(24\right)$

根据式(24)以及坐标之间的转换关系可以得到：

$g_b^e=C_n^e\&times;G=\left(\begin{array}{ccc}-\frac{y}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{x}{\sqrt{x^2+y^2}}& \frac{x}{\sqrt{x^2+y^2+z^2}}\\ \frac{x}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{y}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2+z^2}}\\ 0& \frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}}& \frac{z}{\sqrt{x^2+y^2+z^2}}\end{array}\right)\&times;\left(\begin{array}{c}0\\ 0\\ -G\end{array}\right)---\left(25\right)$

$x=-\frac{G}{\sqrt{x^2+y^2+z^2}}\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$

步骤7-6：ECEF下位置更新

在ECEF坐标系中，由位置和速度之间的关系

对位置进行积分，因为在采样时间间隔内速度

是线性变化的，则有下一时刻的位置为：

$r_{\mathrm{eb}}^e(T+\mathrm{\&tau;})=r_{\mathrm{eb}}^e\left(T\right)+(v_{\mathrm{eb}}^e\left(T\right)+v_{\mathrm{eb}}^e(T+\mathrm{\&tau;}))\frac{\mathrm{\&tau;}}{2}$

$\&ap;r_{\mathrm{eb}}^e\left(T\right)+v_{\mathrm{eb}}^e\left(T\right)\mathrm{\&tau;}+(f_{\mathrm{ib}}^e\left(T\right)+g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e)\frac{{\mathrm{\&tau;}}^2}{2}---\left(26\right)$

在本发明中，通过ECEF模型中得到的三轴位置

和三轴速度

辅助信息，通过输出接口再传送回给FPGA，FPGA传送给GPS接收机进行辅助。

本发明设计的MIMU系统主要是由4块电路板组成，分别是母板、导航板、Y板和Z板。

本发明的母板主要由电源模块、单轴微机械陀螺芯片ADXRS610BBGZ、双轴磁阻传感器芯片HMC1022、双轴加速度计(ADXL320)、3片ADS1274芯片和一片DSP(TMS320F28335)及其外围电路组成。主要功能是提供为整个系统供电和完成传感器的模拟信号到数字信号的采集，并且第一DSP处理器用于发送模式设置指令和误差模型参数。

导航板是整个系统的导航信息融合处理板，主要放置了FPGA(XC6SLX150-2FGG484CES)及其配置Flash(XCF32P)和第二DSP(TMS320C6727)及其外围电路组成，SDRAM(MT48LC4M32B2P-6IT，4M×32，166MHz)作为第二DSP的数据存储器，Flash(LH28F800BJE-PTTL90，512K×16)作为第二DSP的程序存储器。

Y板是传感器板，主要放置单轴微机械陀螺芯片(ADXRS610BBGZ)、单轴磁阻传感器(HMC1021S)、双轴加速度计(ADXL320)。

Z板是传感器板，主要放置了单轴微机械陀螺芯片(ADXRS610BBGZ)。

本发明系统微惯性传感器输出存在高频噪声，为了消除高频信号的干扰，在进行A/D转换前，选用OPA2350构成一阶有源滤波电路，不仅可以消除高频噪声，还可以解决输入输出阻抗不匹配的问题。传感器输出的模拟信号经过3片A/D转换后缓存在FPGA中，FPGA对缓存的数据进行野点剔除和平滑滤波，进一步抑制噪声。A/D芯片选用TI公司的ADS1274，最大的采集速率是144kSPS，24位高精度数据输出。本设计选用了3片ADS1274组成一个9通道的A/D转换电路。FPGA是整个系统的逻辑和时序控制中心，基于系统对FPGA的要求，选择XILINX公司Spartan-6系列的XC6SLX150，其内部逻辑资源丰富，并有大量的DSP硬核乘法器，以便在FPGA内部完成微惯性传感器数据的降噪滤波。配置芯片选用XILINX公司32M的PlatformFlash，采用主时钟并行配置模式。FPGA同步控制3片A/D，为每片A/D芯片分别提供采样时钟和串行输出时钟，采样时钟是20M，串行输出时钟是2M，FPGA通过控制3片A/D的同步采集。

本系统的中心计算处理器第二DSP选择TI公司的TMS320C6727，其主频300MHz。它提供了32位外部存储器接口(EMIF)，使SDRAM的速度有了较大的提升，通过了EMIF口和FPGA实现了无缝链接。由于程序和数据量比较大，对第二DSP外扩了存储器，FLASH存储器用来存放第二DSP系统的程序，SDRAM作为动态随机存储器。

辅助信息要求以较高的更新率传送给GPS接收机，为了使第二DSP能够专注于导航解算，本系统用FPGA作为与GPS接收机通信的桥梁，要求第二DSP解算的信息能够高速、可靠的传递给FPGA，通过FPGA传递给GPS接收机辅助其跟踪环路。所以，核心控制器之间的接口设计格外重要，FPGA与第二DSP通过并行口进行通信，与第一DSP通过SPI总线通信，其中，第一DSP工作在主模式，FPGA工作在从模式。

第二DSP用EMIF口的CS2空间与FPGA通信，DSP的复位信号通过FPGA给出，它们之间的信号线包括：16位数据线，12位地址线以及控制信号线。数据读取过程描述：由FPGA采集数据并缓存后，向DSP发出中断信号(DSP_INT)，DSP响应中断，将读信号DSP_OE置高，进行数据读取。读取方式：DSP把FPGA作为外部扩展存储器，FPGA事先把数据存在规定的地址空间中，DSP通过这些寄存器读入采集的数据，其中第一个寄存器为16位的标志寄存器，用来放置标志信息。数据发送过程描述：当DSP内部解算完毕后，向FPGA发出写信号DSP_WE，然后将解算结果写进固定的寄存器中。

第一DSP主要为了接收上位机指令和误差模型参数。它是一款主频达150M的单精度浮点DSP，拥有丰富的片上存储资源，搭建最小系统简单，如果第二DSP资源紧张，可用它作为辅助处理器进行相关解算。

接口模块包括辅助信息接口和外界接口。辅助信息接口是辅助信息传递的桥梁，有三个方面的要求：一是要求数据传输高速可靠；二是要求信号的驱动能力强；三是要求传输方式尽量简单。辅助接口总线通过高速、低噪的异步双向驱动芯片SN74LVTH 16245进行驱动。

为了传送高速、稳定、可靠的辅助信息，辅助信息传送接口用了两套对称的单向接口：包括1条发送时钟线，1条发送使能线，8条发送数据线，1条接收时钟线，1条接收使能线，8条接收数据线。辅助信息的存储是利用FPGA内部的RAM资源。

外界接口包括上位机接口，扩展接口和显示接口以及普通接收机的接口。最终辅助完成后的解算结果要传输给外界显示设备，考虑到其通常传输距离较远，为增加抗干扰能力，保证数据传输的可靠性，通过RS422总线传输。

第一DSP和上位机之间通过RS232总线进行通信，接收上位机的指令，FPGA还预留了一路与外界的RS232接口，MAX3232作为电平转换芯片。通过外界接口模块，实现了上控制机构和外界显示设备与本系统之间的通信。

Claims (4)

1.一种具有ECEF模型的MIMU系统，MIMU系统一方面用于接收加速度计组合(10)输出的载体加速度信息AF₁₀＝{AFX₁₀，AFY₁₀，AFZ₁₀}、陀螺组合(20)输出的载体角速度信息AG₂₀＝{AGX₂₀，AGY₂₀，AGZ₂₀}、以及磁力计组合(30)输出的磁通信息AH₃₀＝{AHX₃₀，AHY₃₀，AHZ₃₀}，MIMU系统另一方面用于输出ECEF坐标系下的三轴速度

三轴位置给GPS接收机(40)，实现对GPS接收机(40)的跟踪环路进行跟踪；其特征在于：该MIMU系统包括有信号调理模块(1)、模数转换模块(2)、平滑滤波模块(3)、模式设置模块(4)、误差模型模块(5)、模式解码模块(6)、ECEF模型(7)、惯性与DPS同步信息产生模块(8)、状态控制字产生模块(9)；AFX₁₀表示加速度计的X轴输出的载体加速度信息，AFY₁₀表示加速度计的Y轴输出的载体加速度信息，AFZ₁₀表示加速度计的Z轴输出的载体加速度信息，AGX₂₀表示陀螺的X轴输出的载体角速度信息，AGY₂₀表示陀螺的Y轴输出的载体角速度信息，AGZ₂₀表示陀螺的Z轴输出的载体角速度信息，AHX₃₀表示磁力计的X轴输出的磁通信息，AHY₃₀表示磁力计的Y轴输出的磁通信息，AHZ₃₀表示磁力计的Z轴输出的磁通信息；

信号调理模块(1)用于对载体加速度信息AF₁₀＝{AFX₁₀，AFY₁₀，AFZ₁₀}、载体角速度信息AG₂₀＝{AGX₂₀，AGY₂₀，AGZ₂₀}、以及磁通信息AH₃₀＝{AHX₃₀，AHY₃₀，AHZ₃₀}进行噪声抑制滤波，得到调理后的调理-陀螺AG₁、调理-加表AF₁、以及调理-磁通AH₁；

模数转换模块(2)对接收的AG₁、AF₁和AH₁进行模数转换，得到数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂；

平滑滤波模块(3)对接收的数字-陀螺AG₂、数字-加表AF₂和数字-磁通AH₂进行野点剔除和两级平滑滤波处理，得到平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃；

模式设置模块(4)用于接收上位机下发的模式命令，然后根据该命令进行不同模式启动，并将模式设置命令M₄输出给模式解码模块(6)；所述的模式命令包括有松组合模式切换命令、紧组合模式切换命令、辅助解算模式切换命令和GPS更新率；

误差模型(5)用于接收上位机下发的MIMU误差模型参数，然后根据该MIMU误差模型参数进行不同模式启动，并将误差模型数据M₅输出给模式解码模块(6)；该MIMU误差模型参数主要包括有三轴陀螺和三轴加表的误差模型参数；

模式解码模块(6)用于对接收的模式设置命令M₄和误差模型数据M₅进行解码和校验，然后输出CRC校验后命令MD₆给ECEF模型(7)；

状态控制字产生模块(9)用于产生状态控制字命令ST₉，并将该状态控制字命令ST₉输出给ECEF模型(7)；

惯性与GPS同步信息产生模块(8)对接收的平滑后-陀螺AG₃、平滑后-加表AF₃和平滑后-磁通AH₃和GPS数据进行数据状态字、GPS数据和惯性传感器数据的整合，产生与接收信息相同步的同步陀螺信息同步加表信息

同步磁通信息Mag^b、ECEF下初始三轴速度

和ECEF下初始三轴位置

然后将生产的同步信息输出给ECEF模型(7)；所述的数据状态字分别为系统状态字、传感器数据标识状态字、GPS数据标识控制字；

ECEF模型(7)依据CRC校验后的命令MD₆、状态控制字命令ST₉对接收到的三轴陀螺数据

三轴加表数据

三轴磁通数据Mag^b、ECEF下初始三轴速度

ECEF下初始三轴位置

GPS初始纬度L、GPS初始经度λ信息进行ECEF模型解算，得到ECEF的速度信息和位置信息

然后将ECEF的速度信息和位置信息

传送给GPS接收机进行跟踪环路的辅助，提高GPS接收机的抗干扰能力。

2.根据权利要求1所述的具有ECEF模型的MIMU系统，其特征在于：平滑滤波模块(3)、模式解码模块(6)、惯性与DPS同步信息产生模块(8)和状态控制字产生模块(9)由FPGA处理器实现；模式设置模块(4)和误差模型(5)由第一DSP处理器实现；ECEF模型(7)由第二DSP处理器实现。

3.根据权利要求1所述的具有ECEF模型的MIMU系统，其特征在于：平滑滤波模块(3)对陀螺输出的角信息进行平滑滤波包括有下列处理步骤：

步骤3-1：首先记录下惯性传感器前后两次采样的阈值；具体方法为：由于陀螺的量程为±300°/s，以10kHz的前端采样率进行采样，正常情况下两个数之间转过的最大角度为300/10k，由于陀螺的刻度系数为0.05，根据陀螺的数据手册换算到3δ的电压值计算可以得到阈值大约等于18；

步骤3-2：FPGA中设置一个存储寄存器存储上一个采样点的数据，下一个数据到来后和前一个做差，若其差的绝对值大于第一步求得的阈值，即认为是野值，则野点剔除模块输出的值是上一个正确的IMU；若当前的IMU值不是野值，则将当前IMU值输出，并且存储寄存器的值更新为当前的IMU值；

步骤3-3：对步骤3-2野点剔除输出的数据每10个进行一次相加取平均值，将这个平均值作为最终数据输出给步骤3-4，通过这步操作，相当于将10k的采集速率降为1000Hz；

步骤3-4：对步骤3-3输出的数据进行窗口长度为8的滑窗平滑；所谓的窗口类似长度为8的FIFO队列，每次把窗口中的8个数据进行加权平均，然后把最早到窗口的数据移出窗口，其它窗口数据依次移位，收取一个当前新的数据到窗口的末尾；

步骤3-5：对步骤3-4输出的窗口输出数据每10个再进行一次相加取平均值，通过这步操作，将这个平均值作为最终数据输出给步骤3-6，相当于将步骤3-4输出的数据采集速率降为100Hz；

步骤3-6：对步骤3-5输出的数据进行同步骤3-4的窗口长度为8的滑窗平滑；并将平滑后的数据作为最终的平滑滤波输出数据。

4.根据权利要求1所述的具有ECEF模型的MIMU系统，其特征在于ECEF模型解算执行下列步骤：

步骤7-1：依据MD₆、ST₉，进行自身程序初始化控制；

步骤7-2：在初始对准单元中对L、λ、Mag^b、

信息进行初始化，获取载体坐标系到ECEF下的坐标转换矩阵的初始姿态矩阵

载体姿态：航向角记为ψ、俯仰角记为θ和横滚角记为φ；具体航迹推算为：

推导步骤一：求初始航向角

初始对准要求磁阻传感器测得的值在水平面内，如果不在水平面内，应当进行坐标变换，变换方法为：

$\left[\begin{array}{c}{\mathrm{Mag}}_x^b\\ {\mathrm{Mag}}_y^b\\ {\mathrm{Mag}}_z^b\end{array}\right]=C_g^b\left[\begin{array}{c}{\mathrm{Mag}}_x^g\\ {\mathrm{Mag}}_y^g\\ {\mathrm{Mag}}_z^g\end{array}\right]---\left(1\right)$

其中：Mag^b的下角标的x，y，z表示磁力计在载体坐标系下的三个轴，即X轴、Y轴和Z轴，b表示载体坐标系；

Mag^g的下角标的x，y，z表示磁力计在地理坐标系下的三个轴，即X轴、Y轴和Z轴，g表示地理坐标系；

表示地理坐标系g到载体坐标系b的一个坐标转换矩阵。

在载体姿态条件下，坐标转换矩阵

为：

$C_g^b=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& -\sin \mathrm{\&phi;}\\ 0& 1& 0\\ \sin \mathrm{\&phi;}& 0& \cos \mathrm{\&phi;}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ 0& -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& \sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(2\right)$

的正交矩阵记为

即

T表示矩阵转置，所以由式(2)可以得到载体系到地理系的坐标转换矩阵

的表达式为：

$C_b^g=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& 0& \sin \mathrm{\&phi;}\\ \sin \mathrm{\&phi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& -\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]---\left(3\right)$

由联立式(1)和(3)可得：

${\mathrm{Mag}}_x^g={\mathrm{Mag}}_z^b\sin \mathrm{\&phi;}+{\mathrm{Mag}}_x^b\cos \mathrm{\&phi;}$

${\mathrm{Mag}}_y^g={\mathrm{Mag}}_x^b\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}+{\mathrm{Mag}}_y^b\cos \mathrm{\&theta;}-{\mathrm{Mag}}_z^b\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}---\left(4\right)$

由式(4)和载体姿态的航向角ψ可得航向角为：

$\mathrm{\&psi;}={\tan }^{-1}\frac{{\mathrm{Mag}}_x^g}{{\mathrm{Mag}}_y^g}---\left(5\right)$

推导步骤二：求初始俯仰角和横滚角

对初始的得到的进行误差补偿，从而得到标定后的三轴加表数据

由坐标转换关系可得：

$f_{\mathrm{ib}}^b=C_n^b{f}^n---\left(6\right)$

其中，fⁿ为导航坐标系n下比力，

为导航坐标系n到载体坐标系b的坐标转换矩阵，由坐标转换关系得坐标转换矩阵的表达式为：

$C_n^b=\left[\begin{array}{ccc}\cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)-\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)+\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& -\sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\\ -\cos \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&theta;}\right)\\ \sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)+\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)-\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\end{array}\right]---\left(7\right)$

由联立式(6)和式(7)可以得到：

$\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b\\ f_{\mathrm{iby}}^b\\ f_{\mathrm{ibz}}^b\end{array}\right]=\left[\begin{array}{ccc}\cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)-\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)+\sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& -\sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\\ -\cos \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&theta;}\right)\\ \sin \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&psi;}\right)+\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\sin \left(\mathrm{\&psi;}\right)& \sin \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&psi;}\right)-\cos \left(\mathrm{\&phi;}\right)\sin \left(\mathrm{\&theta;}\right)\cos \left(\mathrm{\&psi;}\right)& \cos \left(\mathrm{\&phi;}\right)\cos \left(\mathrm{\&theta;}\right)\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -G\end{array}\right]---\left(8\right)$

的下角标的x，y，z表示加速度计在载体坐标系b下的三个轴，即X轴、Y轴和Z轴；在导航坐标系n下

由化简式(8)得到：

$f_{\mathrm{ibx}}^b=G\sin \mathrm{\&phi;}\cos \mathrm{\&theta;},$ $f_{\mathrm{iby}}^b=-G\sin \mathrm{\&theta;},$ $f_{\mathrm{ibz}}^b=-G\cos \mathrm{\&phi;}\cos \mathrm{\&theta;}---\left(9\right)$

G表示重力加速度；

由式(9)可得初始俯仰角

和初始横滚角

推导步骤三：求初始ECEF下的姿态矩阵

由坐标转换矩阵转换关系可得：

$C_n^e=\left(\begin{array}{ccc}-\sin \mathrm{\&lambda;}& -\sin L\cos \mathrm{\&lambda;}& \cos L\cos \mathrm{\&lambda;}\\ \cos \mathrm{\&lambda;}& -\sin L\sin \mathrm{\&lambda;}& \cos L\sin \mathrm{\&lambda;}\\ 0& \cos L& \sin L\end{array}\right)---\left(10\right)$

表示导航坐标系n到ECEF坐标系e下的坐标转换矩阵；

$C_b^n=\left(\begin{array}{ccc}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}-\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&phi;}\cos \mathrm{\&psi;}+\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\\ \cos \mathrm{\&phi;}\sin \mathrm{\&psi;}+\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&phi;}\sin \mathrm{\&psi;}-\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&phi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\cos \mathrm{\&phi;}\end{array}\right)---\left(11\right)$

通过初始时刻GPS接收机的纬度L、经度λ，代入式(10)可得初始坐标变换矩阵

通过推导步骤一和推导步骤二中求得的姿态角ψ、θ和φ，代入式(11)，可得坐标变换矩阵

通过由关系可以得到

将此时得到的

作为初值；

表示在当前时刻T时，载体坐标系b到ECEF坐标系e下的坐标转换矩阵。

步骤7-3：

对初始的得到的进行误差补偿，从而得到标定后的

利用

和步骤7-2求得的姿态矩阵的初始值

后来进行ECEF下姿态矩阵

的更新，具体步骤如下：

时间导数为：

${\stackrel{\&CenterDot;}{C}}_b^e=C_b^e{\mathrm{\&Omega;}}_{\mathrm{ib}}^b-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{C}_b^e---\left(12\right)$

式(12)，是角速度的斜对称矩阵，且 ${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left(\begin{array}{ccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right);$

式(12)，

是地球旋转矢量的斜对称矩阵，且

ω_ie为地球自转角速度。

式(12)，

的下角标的x，y，z表示陀螺在载体坐标系b下的三个轴，即X轴、Y轴和Z轴；

对式(12)进行积分得：

其中，τ表示时间间隔，T+τ表示下一时刻，

为姿态增量的斜对称矩阵，I₃表示单位矩阵。

对式(13)的幂级数进行一阶泰勒展开，并假定在时间间隔τ内角速度是不变的，即角速度增量

由此可以得到坐标变换矩阵更新后的最终形式：

$C_b^e(T+\mathrm{\&tau;})\&ap;C_b^e\left(T\right)(I_3+{\mathrm{\&Omega;}}_{\mathrm{ib}}^b\mathrm{\&tau;})-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{C}_b^e\left(T\right)\mathrm{\&tau;}---\left(14\right)$

步骤7-4：进行ECEF下的比力更新

由不同坐标系下比力的转换关系可得：

$f_{\mathrm{ib}}^e\left(T\right)=C_b^e\left(T\right)f_{\mathrm{ib}}^b\left(T\right)\&ap;\frac{1}{2}(C_b^e\left(T\right)+C_b^e(T+\mathrm{\&tau;}))f_{\mathrm{ib}}^b\left(T\right)---\left(15\right)$

其中

为ECEF坐标系下相对载体坐标系下比力的表达式，由不同坐标系下速度转换关系可以得到：

${\mathrm{\&upsi;}}_{\mathrm{ib}}^e\&ap;\frac{1}{2}(C_b^e\left(T\right)+C_b^e(T+\mathrm{\&tau;})){\mathrm{\&upsi;}}_{\mathrm{ib}}^b\left(T\right)---\left(16\right)$

其中

为ECEF坐标系相对载体系下的速度，

为载体坐标系下速度；

步骤7-5：ECEF下速度更新

由速度、加速度和位置之间的关系可以得到下式：

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=a_{\mathrm{eb}}^e={\stackrel{\&CenterDot;\&CenterDot;}{r}}_{\mathrm{eb}}^e---\left(17\right)$

$r_{\mathrm{eb}}^e=r_{\mathrm{ib}}^e-r_{\mathrm{ie}}^e---\left(18\right)$

其中，

为ECEF坐标系下速度的微分，

为ECEF坐标系下加速度，为ECEF坐标系下位置的双重微分，

为ECEF坐标系下的位置，

为ECEF坐标系相对载体坐标系的位置，为ECEF坐标系相对惯性坐标系下的位置。

由于地球自转角速度

为常值，则

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{ib}}^e-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\stackrel{\&CenterDot;}{r}}_{\mathrm{eb}}^e+a_{\mathrm{ib}}^e---\left(19\right)$

表示

和重力矢量引起的加速度

的和；

将式(18)及位置、速度之间的关系

(表示ECEF坐标系下的速度)，代入式(19)中得到：

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e+a_{\mathrm{ib}}^e---\left(20\right)$

其中

是

和重力矢量引起的加速度

的和；ECEF下的重力模型

是重力矢量引起的加速度与离心加速度

的和，即

$a_{\mathrm{ib}}^e=f_{\mathrm{ib}}^e\left(T\right)+{\mathrm{\&gamma;}}_{\mathrm{ib}}^e$ (21)

$g_b^e={\mathrm{\&gamma;}}_{\mathrm{ib}}^e-{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{r}_{\mathrm{eb}}^e$

将(20)式代入(21)中得到

${\stackrel{\&CenterDot;}{v}}_{\mathrm{eb}}^e=f_{\mathrm{ib}}^e\left(T\right)+g_b^e\left(r_{\mathrm{eb}}^e\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e---\left(22\right)$

在一个采样时间间隔内速率

比力

和重力模型

是不变的，由此对(22)进行积分运算得到下一时刻的速度为：

$v_{\mathrm{eb}}^e(T+\mathrm{\&tau;})\&ap;v_{\mathrm{eb}}^e\left(T\right)+(f_{\mathrm{ib}}^e\left(T\right)+g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e\left(T\right))\mathrm{\&tau;}$ (23)

$=v_{\mathrm{eb}}^e\left(T\right)+{\mathrm{\&upsi;}}_{\mathrm{ib}}^e+(g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e\left(T\right))\mathrm{\&tau;}$

在导航坐标系下的重力模型可以看做是关于纬度函数，WGS-84的重力数值式为G＝978.03267714×(1+0.00193185138639sin²L)/(1-0.00669437999013sin²L)^1/2其中L为当地纬度。导航坐标系下的重力G通过乘以坐标变换矩阵

得到ECEF下的重力模型，即

由于

其中L为纬度，λ为经度，正弦下的

正弦下的

余弦下的

$\cos \mathrm{\&lambda;}=\frac{x}{\sqrt{x^2+y^2}}.$

在本发明中，x、y、z为ECEF坐标系中的坐标值。将正弦下的

正弦下的

余弦下的

$\cos \mathrm{\&lambda;}=\frac{x}{\sqrt{x^2+y^2}}$ 代入 $C_n^e=\left(\begin{array}{ccc}-\sin \mathrm{\&lambda;}& -\sin L\cos \mathrm{\&lambda;}& \cos L\cos \mathrm{\&lambda;}\\ \cos \mathrm{\&lambda;}& -\sin L\sin \mathrm{\&lambda;}& \cos L\sin \mathrm{\&lambda;}\\ 0& \cos L& \sin L\end{array}\right)$ 中，得到：

$C_n^e=\left(\begin{array}{ccc}-\frac{y}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{x}{\sqrt{x^2+y^2}}& \frac{x}{\sqrt{x^2+y^2+z^2}}\\ \frac{x}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{y}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2+z^2}}\\ 0& \frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}}& \frac{z}{\sqrt{x^2+y^2+z^2}}\end{array}\right)---\left(24\right)$

根据式(24)以及坐标之间的转换关系可以得到：

$g_b^e=C_n^e\&times;G=\left(\begin{array}{ccc}-\frac{y}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{x}{\sqrt{x^2+y^2}}& \frac{x}{\sqrt{x^2+y^2+z^2}}\\ \frac{x}{\sqrt{x^2+y^2}}& -\frac{z}{\sqrt{x^2+y^2+z^2}}\frac{y}{\sqrt{x^2+y^2}}& \frac{y}{\sqrt{x^2+y^2+z^2}}\\ 0& \frac{\sqrt{x^2+y^2}}{\sqrt{x^2+y^2+z^2}}& \frac{z}{\sqrt{x^2+y^2+z^2}}\end{array}\right)\&times;\left(\begin{array}{c}0\\ 0\\ -G\end{array}\right)---\left(25\right)$

$=-\frac{G}{\sqrt{x^2+y^2+z^2}}\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$

步骤7-6：ECEF下位置更新

在ECEF坐标系中，由位置和速度之间的关系

对位置进行积分，因为在采样时间间隔内速度

是线性变化的，则有下一时刻的位置为：

$r_{\mathrm{eb}}^e(T+\mathrm{\&tau;})=r_{\mathrm{eb}}^e\left(T\right)+(v_{\mathrm{eb}}^e\left(T\right)+v_{\mathrm{eb}}^e(T+\mathrm{\&tau;}))\frac{\mathrm{\&tau;}}{2}$ (26)

$\&ap;r_{\mathrm{eb}}^e\left(T\right)+v_{\mathrm{eb}}^e\left(T\right)\mathrm{\&tau;}+(f_{\mathrm{ib}}^e+g_b^e\left(r_{\mathrm{eb}}^e\left(T\right)\right)-2{\mathrm{\&Omega;}}_{\mathrm{ie}}^e{v}_{\mathrm{eb}}^e\left(T\right))\frac{{\mathrm{\&tau;}}^2}{2}$

Images