捷联惯性导航算法研究

捷联惯性导航算法研究（共3篇）

捷联惯性导航算法研究 篇1

1 系统组成框图（图1)

其中，主要讨论姿态测量模块，本模块由加速度计和陀螺仪组成，本课题主要讨论对加速度计的标定。

2 实现方案

传感器总会存在误差，测量的值总会受到线性度、温度、零点漂移的影响，所以在使用传感器之前需要对传感器进行预处理。对加速度计的处理，比较有效方法是利用重力加速度。把物体以不同方向摆放，然后把测量到的数据拟合到当地重力加速度，这样就完成了对加速度计的标定了。

3 实现原理

设x’,y’,z’为测量到的加速度分量，G0为当地重力加速度值。理想情况下有下面的等式：

由于测量存在误差，设测量值为[x,y,z]，则测定值和实际值可以利用下面的公式建立关系。拟合方式为线性拟合，其中a',b',c'是比例系数，d',e',f'为偏移系数。通过这条公式，可以把测量值“变成”实际值，标定加速度计的目标，就是确定[a',b',c',d',e',f']这六个参数，使估计值更接近实际值。

代入理想情况的等式，有：

展开得：

这实际上就是个椭球方程。

为方便推导和运算，写成如下形式：

比较以上两式，有：（注意加上系数k)

反过来，有：

在静止状态下测量得一些数据，记作[xn,yn,zn]，每组都对应重力加速度G0。为了衡量参数的精度，引入误差函数Δn，表示第n个估计值与实际值的偏差。其表达式为：

Δn是第n组数据的偏差，我们要做的是使整体偏差降到最小，寻找最优的参数，所以引入整体数据偏差I，其数值为所有数据的偏差的平方和。

为了使整体估计值更为接近实际值，需要找出一组最优参数，常常是曲线或者曲面的一些极值点。即已知[xn,yn,zn]，求使I最小的[a',b',c',d',e',f']。利用[a,b,c,d,e,f,g]求[a',b',c',d',e',f']，这就是我们引入这些参数的原因。利用高等数学中的偏导函数，用I分别对[a,b,c,d,e,f,g]求偏导数，然后通通使其为0，该方法即是拉格朗日数乘法，于是得到下面7条方程：

仔细观察以上的方程，过于繁杂，但其形式基本相同，还可以写成更简洁的形式。引入列向量v和pn，如下所示：

v为变量组，pn为系数组，方程组可以用一个式子表示，再经过一些矩阵运算，得到的矩阵形式A。而且矩阵A相当利于程序实现。

4 编写软件

首先要提取出[xn,yn,zn]，然后用[xn,yn,zn]算出向量pn,pn转置，pn*pn累加成矩阵A，接着求以A为系数矩阵的齐次线性方程组，算出[a,b,c,d,e,f,g]，这样就可以得到最佳的[a',b',c',d',e',f']了。

5采集的数据

捷联惯性导航算法研究 篇2

1 Sage-Husa自适应算法

Sage-Husa自适应滤波算法是利用量测数据进行滤波的同时, 通过时变噪声统计估计器实时的估计出系统噪声和量测噪声的统计特性, 同时对其进行校正。

设随机线性离散系统的基本方程为:

式中, Xk是系统的n维状态向量, Zk是系统的m维观测矩阵, Wk是p维系统过程噪声序列, Vk是m维量测噪声序列, (37) k, k-1是系统的nn维状态转移矩阵, (38) k, k-1是np维噪声驱动阵, Hk是mn维量测矩阵。

假定系统过程噪声和观测噪声的统计特性未知时变的, 其统计特性为:

则有带时变系统的时变噪声统计指数加权估计器为:

式中, 为遗忘因子, 一般选取b的范围为 之间。采用遗忘因子可以限制滤波器的记忆长度, 加重新近观测数据对现时估计的作用。b的选择必须对时变参数的跟踪性能和噪声的不敏感性考虑。虚拟噪声补偿方法, 将系统模型误差造成的影响考虑成系统噪声和观测噪声, 通过对噪声的实时估计, 实现对模型误差的补偿, 从而提高滤波精度。

2 联合自适应Kalman滤波器设计

组合导航的本质就是采用组合算法对导航系统的误差进行估计修正, 提高组合导航系统精度。在实际系统中, 系统噪声方差阵和量测噪声方差阵事先是不知道的, 本文设计了联合自适应Kalman滤波器。本滤波器采用捷联系统作为主参考系统, 捷联系统和GPS系统的位置差值作为一组量测值, 速度差值作为另一组量测值, 子滤波器采用Kalman滤波器。位置信息自适应Kalman滤波器和速度信息Kalman滤波器利用观测值不断地修正预测值, 同时对不确切知道的噪声统计参数进行估计。

滤波增益矩阵

一步预测误差方差阵:

估计误差方差阵:

遗忘因子:

主滤波器采用如下方程对子滤波器的信息进行融合来获得全局的估计值:

通过上述各式进行不断往返递推, 得到全局的估计值。

3 SINS/GPS组合系统滤波模型

状态方程如下:

式中: 为系统的噪声; (37) 是系统的传递矩阵。X是系统状态变量。两个子滤波器的状态方程相同。

在此组合系统中, 其量测方程有两个, 以GPS输出的位置和相应的SINS输出位置之差作为位置子滤波器的量测信息, 量测方程为:

以GPS输出的速度和相应的SINS输出速度之差作为速度子滤波器的量测信息, 量测方程为:

式中:

4 系统仿真

选定仿真环境为MATLAB R2008a, 设三坐标系轴正交安装三个陀螺仪和加速度计的特性完全相同。惯性测量元件的采样时间为20ms, 仿真时间为2000s, 遗忘因子选为。仿真时系统的量测噪声取为经验值的30倍。初始条件如表1所示。误差曲线如图2、图3、图4。

通过捷联惯性组合导航系统的仿真图可以看出, 设计的联合自适应Kalman滤波器有较好的滤波精度。位置误差控制在5m以内, 速度误差控制在以内。

4 结论

捷联惯导系统和GPS的组合系统性能好、结构简单。常用的滤波方法已经不能满足系统实时性和稳定性的要求。本文设计的联合自适应Kalman滤波器能够通过在线实时校正系统的噪声统计特性。该滤波器不仅保留了基本联合Kalman滤波器的冗余性能好、实时性强、可靠性高的特点, 还有效提高了滤波精度。在实际应用的组合导航系统中具有较高的应用价值。

参考文献

[1]章燕申.高精度导航系统[M].北京:中国宇航出版社, 2005.

[2]邓自立.自校正滤波理论及其应用—现代时间序列分析方法[M].哈尔滨:哈尔滨工业大学出版社, 2003.

[3]段玉波.最优估计理论及其应用[M].哈尔滨:哈尔滨工程大学出版社, 1994.

[4]Carlson, N.A.Federated Filter for Computer--Efficient Near--Optimal GPS Integration, IEEE PLANS, 1996.

捷联惯性导航算法研究 篇3

1 高性能DSP选取

DSP (Digital Signal Processing) 即数字信号处理[4], 与其他处理器相比, 它是以数字信号来处理信息的器件, 具有独特性。在捷联惯性导航中, 三个陀螺仪和三个加速计是系统的核心部件。如何选取高性能适用的DSP, 实现对这6个传感器的高速采样、计算、信息输出呢?

数据采集选用DSP数字信号处理, 兼顾单精度浮点运算, 接口丰富, 使用方便, 选用支持32位浮点运算DSP28335。DSP28335速率速度可达150MHz, 有两个可设CAN口、3个UART口, 2个SPI口 (其中一个I2C) , 12位ADC, 3个32位定时器等, 可以满足三个陀螺仪、三个加速度计和其他信号的采集, 而且数据输入、输出接口较多, 有利于数据输出到监控平台, 也可接入惯性组合导航系统中其他设备。

高速实时导航解算选用性能优越的DSP6747, DSP6747频率为300MHz, 支持双精度浮点运算, 2个UART口, FLASH容量512MB。由于捷联惯性导航系统姿态解算、卡尔曼滤波、组合导航等运算量大, 而且要满足双精度浮点运算, 对CPU的频率要求特别高, ARM系列芯片主要以软浮点为主, 满足不了要求。采用PC104工控板, 运行频率在300M以上, 可基本满足计算要求, 但数据输入输出只能通过RS232串口或CAN口, 传输速率太慢, 延迟至少超过10ms。因此, 选用DSP6747可以满足复杂浮点运算和大容量程序装载, 数据传输和交换采用双口RAM, 构建满足捷联惯性导航系统的运算平台。

2 数据采集

数据采集是高速实时系统的关键环节, 直接影响系统输出是否能真实反应载体或者设定系统的运动状态。一般数据采集涉及传感器较少, 实时性要求不高, 直接采用A/D模数转换即可实现数据采集。不过, 对于复杂的惯性导航系统, 既要考虑6个传感器的数据采集, 还要保证同一时间点读取数据, 选用模数转换器既要考虑多通道, 而且要考虑转换频率和精度。AD7608是内置18位、8通道双极性同步采样频率高达200KSPS模数转换。选择DSP28335作为信号处理CPU, 通过AD7608可以实现三个陀螺仪和三个加速度计的直接数据采样, 另外两个通道对实时采样要求不高, 通过逻辑开关选通满足温度传感器和其他数据采样, 过采样位解码OS[2:0]选择为010, 采样速率达50KHz, 而且稳定可靠。

DSP28335支持单精度浮点运算, 而且有丰富的外围接口。通过软件设置DSP28335定时器、输入输出口、中断触发方式 (电平触发或边沿触发) 等, 采样周期设置为定时周期的偶数倍, 以便设置引脚高低电平, 引发指定周期周期性中断。数据采样设置在中断服务程序中, 一次中断响应同时读取三个陀螺仪和三个加速度计传感器数据, 保证传感器数据采集的实时性和同步性, 以便实时响应和敏感载体的运动姿态。捷联惯性导航系统加速度计数据的采集频率很高[5], 要保证采样数据不丢失跟踪状态, 采用硬件电路积分;陀螺仪数据采用敏感角速度脉冲通过FPGA求和, 采样周期均为2000Hz;数据采集后, 经过温度补偿和量纲转换, 以及一定浮点运算后, 通过双口RAM与DSP6747实时高速数据交换, 其周期与采样周期一致。数据读取采用并行独立模式, 数据采样按照所有通道同步采样。

3 FPGA选取和应用

FPGA (Field-Programmable Gate Array) 现场可编程门阵列, 是在PAL、GAL等逻辑器件的基础上发展起来的, 规模更大, 可以实现任何的数字功能器件, 具有设计灵活、用户定制、保密性强、功能密集度、容量大等优点, 适合于实现数据密集型的系统。FPGA可编程方式缩短了工程技术人员设计硬件逻辑电路的周期, 而且提供比PLD和EPLD器件足够大的有效逻辑容量密度, 大量减少硬件电路板逻辑电路布线, 并且可以反复编程, 重复使用, 还不容易被别人复制。

捷联惯性导航系统要求系统小型化, 控制电路较多, 而且比较复杂, 还要考虑组合导航系统辅助设备GPS或北斗、里程计、高度计等, 选用FPGA型号A3P250, 可以大量减少逻辑电路, 将系统需要保密和控制核心部分通过FPGA固化到内部, 同时该型号支持4个I/O组, 可提供157个I/O接口, 1024位可配置非挥发性Flash内存和6个时钟, 其中一个带有PLL机载锁相环, 待机功耗低于3m A, 并配有128位AES加密和内置加密存储Flash。实时数据采集系统使用该技术优势, 通过DSP28335定时器引发中断, 中断信号连接到FPGA引脚, 从而保持两边时钟同步。为了保持数据采样时间的一致性, 在同一周期内需同时读取捷联惯性导航系统三个陀螺仪和三个角速度计的值。其采样周期达2000Hz, 待数据处理完毕后, 通过电平转换告知FPGA, 从而引发中断, 通知DSP6747接收数据, 接收完毕后清除数据。数据从发送到接收不超过10ns, 大大节省了CPU数据交换时间, 提高了数据采集、处理、传输的实时性。

4 双口RAM设计思想

双口RAM设计需要完成在多个相同或不同CPU的数据共享, 通过控制线、数据线和地址线访问对方内部RAM资源。双口RAM能够对同一地址单元访问的时序控制;存储单元数据块的访问权限分配:信令交换逻辑 (例如中断信号) 等。目前, 使用较多芯片有Actel公司的Pro ASIC3系列、Cypress公司CYxxxxxx系列、IDT公司IDTxxxxx系列等, 以捷联惯性导航系统采用A3P250芯片为例, 实现双口RAM数据交换。该系统信号采集和处理由DSP28335完成, 航姿解算和组合导航由DSP6747完成, 与用户数据交互由专用时钟同步模块负责。三个独立的分系统协同工作, 保证从数据采集、处理、运算、输出整个过程延迟不超过1ms, 也就是说, 数据更新频率最高可达1000Hz, 这样可以有效跟踪载体姿态。三个系统通过写数据, 按照控制时序, 引发中断, 通知对方读取数据, 然后清除数据缓存的方式, 从而实现数据交互, 数据读存时间小于5ns。如果采样传统方式RS232串口数据交换, 以最高频率115200bps计算, 传输采样数据50字节需要200ms;如果采用CAN传输, 以最高频率1M计算, 最多能提高10倍, 也需要20ms。如果在双口RAM中读/写存取器时, 既可以在指定的RAM读取数据, 也可以可保存数据, 如果主动不清除数据, 只要电源存在, 信息仍然保存。因此, 采用双口RAM设计, 可以大大缩短数据交换时间, 满足高速实时系统要求。

5 实时采集系统与外部数据交换

目前, 一般系统数据输交换广泛采用RS232串口、CAN总线接口, 有的采用网络、USB接口, 如果需要高频率输出500Hz以上数据, 且不影响CPU解算周期, 难度非常高。捷联惯性导航系统与外部数据交换传统方式, 直接由导航计算机RS232串口或CAN总线接口输入和输出, 导致大部分时间用于数据发送和接收, 姿态和坐标更新频率非常低, 甚至更新周期以秒级为单位, 超过100ms都非常困难, 而且导致计算与数据接收、输出超出CPU单位周期内得承载能力, 很容易丢失数据, 造成系统数据不能反应运动的真实状态。因此, 捷联惯性导航系统与外部交换采用单独CPU来完成, DSP6747上电启动, 等待信号处理器DSP28335周期性合法传感器数据, 然后经双口RAM和FPGA, 发送至另一数据交换处理器DSP28335, 再DSP28335与外部控制端或操作计算机进行指令和数据的交互。

6 结束语

文章采用高性能DSP+FPGA+双口RAM技术, 探讨了高速实时系统数据从采集、解算、传输等一体化过程, 实现多CPU并行处理, 数据通过双口RAM共享, 克服了实时系统因数据传输时间过长的瓶颈, 为高速实时捷联惯性导航系统数据采集和传输提供了解决方案, 具有很强的适用性和参考价值。

参考文献

[1]马秀娟, 考丽, 赵国良.基于FPGA和DSP的高速数据采集实时处理系统的设计[J].电子器件, 2007, 3:1009-1013.

[2]秦鸿刚, 刘京科, 吴迪.基于FPGA的双口RAM实现及应用[J].电子设计工程, 2010, 2:72-74.

[3]秦鸿刚, 刘京, 科吴迪.激光陀螺捷联惯性导航系统中惯性器件误差补偿技术[J].上海交通大学学报, 2003, 37 (11) .

[4]高源, 罗秋凤.基于DSP28335程序移植方法的研究与实现[J].电子测量技术, 2013, 3:84-88.