[发明专利]一种基于FPGA及RTOS的多传感器融合导航装置

权利要求书

1.一种基于FPGA及RTOS的多传感器融合导航装置，其特征在于：包含数据接收模块、第一数据存储模块、状态选择滤波器模块、第二数据存储模块；

其中，数据接收模块包含北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪以及分别与北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪一一对应连接的北斗IP核模块、IMU IP核模块、三轴磁力计IP核模块、气压计IP核模块、激光测距仪IP核模块；

其中，数据接收模块，用于北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪数据的接收和解码；

第一数据存储模块，用于存储由北斗IP核模块、IMU IP核模块、三轴磁力计IP核模块、气压计IP核模块、激光测距仪IP核模块解码完成后的数据；

状态选择滤波器模块，用于根据实时高度确定相应的传感器使用策略，并对北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪解码完成后的数据进行局部最优估计，进而获取北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪状态向量的最优估计；

第二数据存储模块，用于存储状态向量的最优估计，进而通过对载体的导航信息进行校正，获取最优导航信息。

2.根据权利要求1所述的一种基于FPGA及RTOS的多传感器融合导航装置，其特征在于，北斗IP核模块、IMU IP核模块、三轴磁力计IP核模块、气压计IP核模块、激光测距仪IP核模块均通过运用VHDL语言编写。

3.根据权利要求1所述的一种基于FPGA及RTOS的多传感器融合导航装置，其特征在于：所述状态选择滤波器模块包含分布式卡尔曼滤波器、逻辑判断模块；所述逻辑判断模块用于设定传感器转换策略；所述分布式卡尔曼滤波器用于结合逻辑判断模块输出结果对各传感器的解码数据进行处理。

4.根据权利要求1所述的一种基于FPGA及RTOS的多传感器融合导航装置，其特征在于：所述第一数据存储模块和第二数据存储模块采用双端RAM。

5.根据权利要求2所述的一种基于FPGA及RTOS的多传感器融合导航装置，其特征在于：所述北斗接收机、六轴IMU惯性测量单元、三轴磁力计、气压计和激光测距仪的状态向量具体如下：

(1)六轴IMU惯性测量单元的状态向量为：

其中，δL、δλ、δh分别为IMU测量的载体的纬度误差、经度误差和高度误差；δv_E、δv_N、δv_U分别为东向、北向和垂向的速度误差；分别为东向、北向和垂向的姿态角误差；ε_x、ε_y、ε_z分别为x轴、y轴、z轴的陀螺漂移误差；分别为x轴、y轴、z轴的加速度计零位误差；

(2)北斗接收机的状态向量为：

XBD=δLBDδλBDδhBDδvEBDδvNBDδvUBDδtuδtrT]]>

其中，δL^BD、δλ^BD、δh^BD分别为北斗接收机测量的载体的纬度误差、经度误差和高度误差；分别为东向、北向和垂向的速度误差；δt_u为北斗接收机的时钟偏差，δt_r为北斗接收机的时钟随机漂移误差量；

(3)三轴磁力计的状态向量为：

X_MG＝[dy_MGE dy_MGN dy_MGU]

其中，dy_MGE、dy_MGN、dy_MGU为磁力计的磁东向、北向和垂向误差；

(4)气压计的状态向量为：

X_B＝[dh_B]

其中，dh_B为气压计的高度测量误差；

(5)激光测距仪的状态向量为：

X_LRF＝[dh_LRF]

其中，dh_LRF为激光测距仪的高度测量误差。