[发明专利]一种非等间隔联邦滤波多源组合导航系统及方法

权利要求书

1.一种非等间隔联邦滤波多源组合导航系统，其特征在于，包括计算机、程控直流电源、数据采集与处理单元和传感器单元，其中数据采集与处理单元包括多路计算器采集卡、RS422串口服务器、RS232串口服务器，传感器单元包括捷联惯导SINS、卫导GNSS、高度计ALT、磁强计MAG、里程计ODO、激光雷达LIDAR和双目视觉VIS7种传感器；

所述数据采集与处理单元一端与计算机连接，另一端分别接入传感器单元中的每个传感器产品；所述程控直流电源包含可编程直流电源模块，为计算机和传感器单元提供电源。

2.根据权利要求1所述的非等间隔联邦滤波多源组合导航系统，其特征在于，所述计算机采用Linux操作系统平台。

3.根据权利要求1所述的非等间隔联邦滤波多源组合导航系统，其特征在于，所述计算机采用安装Ubuntu18.04的英伟达板卡。

4.根据权利要求1所述的非等间隔联邦滤波多源组合导航系统，其特征在于，所述多路计算器采集卡的控制芯片、A/D芯片和存储器均安装于PCB板上，支持Linux操作系统。

5.根据权利要求1所述的非等间隔联邦滤波多源组合导航系统，其特征在于，所述多路计算器采集卡板载的RS422串口服务器、RS232串口服务器串口支持最高921.6kbps的传输速率，板上控制器提供全调制解调器控制信号，以确保与各种串口周边设备兼容。

6.根据权利要求1所述的非等间隔联邦滤波多源组合导航系统，其特征在于，程控直流电源每个通道的电压和电流门限独立设置，并具备过压和过流保护功能，供各类传感器使用；

每路电源电压独立设置，并具备电压、电流过载保护功能，每路通道功率为100W，每路输出电压调节范围0～24V，每路电流输出上限≥5A，纹波噪声峰峰值小于30mV。

7.一种非等间隔联邦滤波多源组合导航方法，其特征在于，包括以下步骤：

步骤1、对定位车辆搭载的7种传感器进行自检，确保各传感器正常工作；

步骤2、配置程控直流电源并进行断电自检，保障工作环境的安全性；

步骤3、采集各类传感器的原始数据进行解包，并转换为导航数据；

步骤4、将导航系统预热，预热结束后取60s陀螺仪的角速度输出数据w_i和加速度计输出的加速度数据A_i，然后计算此段时间各轴的角速度输出平均值w_b和加速度输出平均值A_b作为零偏；

步骤5、去除零偏后，取60s加速度计输出的加速度计数据进行俯仰角θ₀和滚转角γ₀的初始对准，通过其他各类传感器获取定位车辆航向角；

步骤6、对各传感器的输出数据进行解包，采用分段重叠的方法对量测数据进行处理，进而将数据处理转化为东北天坐标系或载体系下的数据，并进行非等间隔联邦滤波；

步骤7、对基于非等间隔联邦滤波后的各状态误差值进行反馈，并在终端进行实时显示。

8.根据权利要求7所述的非等间隔联邦滤波多源组合导航方法，其特征在于，所述步骤4具体如下：

设定数据总数为N，则有：

9.根据权利要求7所述的非等间隔联邦滤波多源组合导航方法，其特征在于，所述步骤5具体如下：

俯仰角：

横滚角：

其中：

式中，分别为加速度计在x、y、z上输出的平均加速度信息，A_xi、A_yi、A_zi分别为加速度计在x、y、z上输出的加速度速度信息，g₀为地球重力加速度。

10.根据权利要求7所述的非等间隔联邦滤波多源组合导航方法，其特征在于，所述步骤6具体如下：

系统的状态方程为：

式中，为由SINS误差变量构成的18维状态向量，F(t)为系统的状态转移矩阵，G(t)为系统的噪声驱动矩阵，W(t)为系统的噪声随机误差向量；

其中：

式中，F_N(t)为对应9个基本导航参数的系统阵，F_M是加速度计和陀螺仪随机游走组成的斜对角矩阵，F_S由单位阵和0矩阵构成，F_G为一特定矩阵；

系统噪声矩阵G(t)为：

式中，旋转矩阵，I为单位矩阵；

系统的状态向量为

其中，φ_e、φ_n、φ_u分别为捷联式惯性导航系统在东向、北向、天向上的姿态角误差；δV_e、δV_n、δV_u分别为捷联式惯性导航系统在东向、北向、天向上的速度误差；δL、δλ、δh分别为捷联式惯性导航系统纬度、经度、高度的位置误差；ε_bxε_byε_bz分别为陀螺仪在x、y、z三个轴向上的随机常值误差；ε_rx、ε_ry、ε_rz分别为陀螺仪在x、y、z三个轴向上的一阶马尔可夫偏移误差；分别为加速度计在x、y、z三个轴向上的一阶马尔可夫偏移误差；

各子滤波器的量测公式如下：

子滤波器1：将SINS输出的位置、速度与GNSSG输出的位置、速度作差作为系统量测向量，系统量测方程为：

式中，L_sins、λ_sins、h_sins是SINS输出的纬经高信息，V_sinse、V_sinsn、V_sinsu是SINS输出的地理系下载体在x、y、z上的速度信息，L_gps、λ_gps、h_gps是GNSS输出的纬经高信息，V_gpse、V_gpsn、V_gpsu是GNSS输出的在地理系下在x、y、z上的速度信息，H₁(t)是量测矩阵，V₁(t)是量测噪声；

子滤波器2：将SINS输出的高度与高度计输出的高度差值作为系统量测向量，系统量测方程为：

Z₂(t)＝[h_sins-h_alt]＝H₂(t)X(t)+V₂(t)

式中，h_alt是高度计输出的高度信息，H₂(t)是量测矩阵，V₂(t)是量测噪声；

子滤波器3：将SINS输出的航向角与磁强计输出的航向角差值作为系统量测向量，系统量测方程为：

Z₃(t)＝[ψ_sins-ψ_mag]＝H₃(t)X(t)+V₃(t)

式中，ψ_sins是SINS输出的航向角信息，ψ_mag是磁强计输出的航向角信息，H₃(t)是量测矩阵，V₃(t)是量测噪声；

子滤波器4：将SINS输出速度和里程计处理输出的速度差值作为系统量测向量，系统量测方程为：

式中，V_odoe、V_odon、V_odou是里程计输出的在地理系下在x、y、z上的速度信息；

子滤波器5：将SINS输出的位置、姿态与激光雷达处理输出的位置、姿态差值作为系统量测向量，系统量测方程为：

式中，L_lider、λ_lider、h_lider是激光雷达输出的纬经高信息，ψ_lider是激光雷达处理输出的航向角信息；

子滤波器6：将SINS输出的位置、姿态与双目视觉处理输出的位置、姿态差值作为系统量测向量，系统量测方程为：

式中，L_vis、λ_vis、h_vis是双目视觉输出的纬经高信息，ψ_vis是双目视觉处理输出的航向角信息；

分别对上述6个子滤波器进行Kalman滤波，时间更新为：

式中，是k时刻的状态向量，Φ_kk-1为k-1到k时刻的系统一步转移矩阵，是k-1时刻的状态向量，P_kk-1为k时刻的一步预测均方差误差，P_k-1为k-1时刻的一步预测均方差误差，Γ_k-1为系统噪声矩阵，Q_k-1为系统噪声；

量测更新为：

式中，K_k为k时刻卡尔曼滤波增益，R_k为量测噪声，I为单位矩阵，H_k为观测矩阵，Z_k为系统观测向量；

设第i个传感器的量测周期为T_i，i＝1，2，3…m，主滤波器的计算周期为T_c，融合周期为T_f，T_c和T_f定义如下：

其中，G(*)表示求取最大公约数，L(*)表示求取最小公倍数，定义k为各计算周期T_c划分的时间刻度，有T_i＝k_iT_c，k_i为互质的自然数；当各子滤波器同时有量测更新时，即T_f＝T_c时，完全满足联邦卡尔曼滤波算法要求；

对子滤波器而言：

(1)当k＝k_p时，即此时主滤波器有量测更新时：

(2)当k≠k_p时，即此时主滤波器没有量测更新时，子滤波器只进行时间更新：

对主滤波器而言：

(1)当T_f＝T_c，即此时主滤波器有量测更新时，主滤波器进行信息分配：

(2)当T_f≠T_c时，即此时主滤波器没有量测更新时，主滤波器不进行任何操作；

其中，k_p为子滤波器有新的量测更新的时刻，如果当前子滤波器没有新的量测信息时，即满足k≠k_p时，子滤波器只进行时间更新；如果当前子滤波器有新的量测信息时，即满足k＝k_p，子滤波器同时进行时间更新和量测更新；对于主滤波器，只有当主滤波器有量测更新时，主滤波器进行信息分配。