汽车技术

名称	型号
机载电脑	树莓派4B+
激光雷达	思岚A2
底层控制器	STM32H743
主电源	4S锂电池

名称	型号
机载电脑	树莓派4B+
激光雷达	思岚A2
底层控制器	STM32H743
主电源	4S锂电池

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	2.4×10^-4g^-1	6.1×10^-2 (°)/s
灵敏度初始误差/%	±1	±1
灵敏度动态误差/%	±1.5	±2
非线性度（±8 g）/%	±0.3	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.04 g	±1 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 5 g/℃	±0.01 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	2.4×10^-4g^-1	6.1×10^-2 (°)/s
灵敏度初始误差/%	±1	±1
灵敏度动态误差/%	±1.5	±2
非线性度（±8 g）/%	±0.3	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.04 g	±1 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 5 g/℃	±0.01 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	4 096 LSB/g	16.4 LSB·(°)^-1·s
灵敏度初始误差/%	±2	±2
灵敏度动态误差/%	±1	±1.5
非线性度（±8 g）/%	±0.5	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.08 g	±5 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 75 g/℃	±0.05 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	4 096 LSB/g	16.4 LSB·(°)^-1·s
灵敏度初始误差/%	±2	±2
灵敏度动态误差/%	±1	±1.5
非线性度（±8 g）/%	±0.5	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.08 g	±5 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 75 g/℃	±0.05 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

算法	横滚角	俯仰角
ICM20689+STAUKF算法	5.62×10^-2	6.18×10^-2
ICM20602+ STAUKF算法	5.48×10^-2	5.53×10^-2
ICM20689+ICM20602加权平均	3.47×10^-2	2.16×10^-2
基于误差补偿的后验估计融合算法	1.10×10^-2	1.08×10^-2
基于信息融合的后验估计融合算法	1.10×10^-2	1.08×10^-2

算法	横滚角	俯仰角
ICM20689+STAUKF算法	5.62×10^-2	6.18×10^-2
ICM20602+ STAUKF算法	5.48×10^-2	5.53×10^-2
ICM20689+ICM20602加权平均	3.47×10^-2	2.16×10^-2
基于误差补偿的后验估计融合算法	1.10×10^-2	1.08×10^-2
基于信息融合的后验估计融合算法	1.10×10^-2	1.08×10^-2

算法	横滚角	俯仰角
ICM20689+STAUKF算法	3.9×10^-3	3.9×10^-3
ICM20602+STAUKF算法	2.5×10^-3	2.6×10^-3
ICM20689+ICM20602 加权平均	1.6×10^-3	1.6×10^-3
基于误差补偿的后验估计融合算法	8.395 5×10^-4	8.166 1×10^-4
基于信息融合的后验估计融合算法	8.393 8×10^-4	8.161 3×10^-4

算法	横滚角	俯仰角
ICM20689+STAUKF算法	3.9×10^-3	3.9×10^-3
ICM20602+STAUKF算法	2.5×10^-3	2.6×10^-3
ICM20689+ICM20602 加权平均	1.6×10^-3	1.6×10^-3
基于误差补偿的后验估计融合算法	8.395 5×10^-4	8.166 1×10^-4
基于信息融合的后验估计融合算法	8.393 8×10^-4	8.161 3×10^-4

算法	横滚角	俯仰角
ICM20689+STUKF算法	7.48×10^-2	7.62×10^-2
ICM20602+STUKF算法	6.97×10^-2	7.04×10^-2
ICM20689+ICM20602加权平均	4.76×10^-2	6.04×10^-2
基于误差补偿的后验估计融合算法	3.02×10^-2	2.89×10^-2
基于信息融合的后验估计融合算法	3.02×10^-2	2.89×10^-3

算法	横滚角	俯仰角
ICM20689+STUKF算法	7.48×10^-2	7.62×10^-2
ICM20602+STUKF算法	6.97×10^-2	7.04×10^-2
ICM20689+ICM20602加权平均	4.76×10^-2	6.04×10^-2
基于误差补偿的后验估计融合算法	3.02×10^-2	2.89×10^-2
基于信息融合的后验估计融合算法	3.02×10^-2	2.89×10^-3

算法	横滚角	俯仰角
ICM20689+STUKF算法	9.4×10^-3	8.6×10^-3
ICM20602+STUKF算法	8.3×10^-3	7.8×10^-3
ICM20689+ICM20602加权平均	7.1×10^-3	6.3×10^-3
基于误差补偿的后验估计融合算法	3.0×10^-3	2.8×10^-3
基于信息融合的后验估计融合算法	3.0×10^-3	2.8×10^-3

算法	横滚角	俯仰角
ICM20689+STUKF算法	9.4×10^-3	8.6×10^-3
ICM20602+STUKF算法	8.3×10^-3	7.8×10^-3
ICM20689+ICM20602加权平均	7.1×10^-3	6.3×10^-3
基于误差补偿的后验估计融合算法	3.0×10^-3	2.8×10^-3
基于信息融合的后验估计融合算法	3.0×10^-3	2.8×10^-3

基于粒子群优化的无人车双惯性测量单元姿态融合方法^*

PDF下载

马帅旗 , 贺海育 , 周雷金 , 王文妍

汽车技术 | 2024,(8): 38-46

收起

汽车技术 | 2024, (8): 38-46

基于粒子群优化的无人车双惯性测量单元姿态融合方法^*

全屏

马帅旗, 贺海育, 周雷金, 王文妍

作者信息

陕西理工大学，汉中 723000

Attitude Fusion Method of Unmanned Vehicle Dual IMU Based on PSO

Shuaiqi Ma, Haiyu He, Leijin Zhou, Wenyan Wang

Affiliations

Shaanxi University of Technology, Hanzhong 723000

出版时间: 2024-08-24 doi: 10.19620/j.cnki.1000-3703.20230744

文章导航

摘要

收起

为提高无人车系统中微机电惯性测量单元（MEMS IMU）的姿态角解算精度，提出了一种基于粒子群优化（PSO）算法和自适应强跟踪无迹卡尔曼滤波（STAUKF）算法的数据融合方法。首先，对两种不同精度的IMU模块通过STAUKF算法进行滤波，然后，利用构造的两类误差函数，引入PSO算法对两种IMU的后验估计进行融合，最后，在搭建的无人车平台上进行测试。试验结果表明，相较于两种单一IMU解算数据，所提出的方法解算获得的横滚轴与俯仰轴角度均方根误差分别减小了56.67%、58.94%，相较于冗余式双IMU系统直接加权平均所解算的数据分减小了36.55%、52.15%，解算精度更高、鲁棒性更强。

关键词

冗余传感器 / 数据融合 / 粒子群优化 / 强跟踪 / 卡尔曼滤波

Abstract

收起

In order to improve the attitude angle solving accuracy of Micro-Electro-Mechanical System Inertial Measurement Unit (MEMS IMU) in unmanned vehicle system, this paper proposed a Particle Swarm Optimization (PSO) based algorithm and a Strong Tracking Adaptive Unscented Kalman Filter (STAUKF) data fusion method. Firstly, two kinds of IMU modules with different precision were filtered by STAUKF algorithm. Secondly, two kinds of error functions were constructed and PSO algorithm was introduced to fuse the two kinds of IMU posterior estimation. Finally, the test was carried out on the built unmanned vehicle platform. Experimental results show that, compared with the data solved by two single IMU sensors, the root mean square error of the transverse roller shaft and pitch shaft angle solved by the proposed algorithm is reduced by 56.67% and 58.94%, respectively, and the data solved is reduced by 36.55% and 52.15% respectively compared with direct weighted average of the redundant dual IMU system. Therefore, the algorithm proposed in this paper is more accurate and robust.

Key words

Redundant sensor / Data fusion / Particle Swarm Optimization (PSO) / Strong tracking / Kalman filter

引用本文

马帅旗, 贺海育, 周雷金, 王文妍. 基于粒子群优化的无人车双惯性测量单元姿态融合方法^*. 汽车技术, 2024 , (8) : 38 -46 . DOI: 10.19620/j.cnki.1000-3703.20230744

Shuaiqi Ma, Haiyu He, Leijin Zhou, Wenyan Wang. Attitude Fusion Method of Unmanned Vehicle Dual IMU Based on PSO[J]. Automobile Technology, 2024 , (8) : 38 -46 . DOI: 10.19620/j.cnki.1000-3703.20230744

正文

收起

1 前言

收起

无人车自动导航技术的不断发展对无人车姿态解算速度和精度提出了更高的要求^[1]。目前，低成本微机电惯性测量单元（Micro-Electro-Mechanical System Inertial Measurement Unit，MEMS IMU）传感器存在精度低和可靠性差等问题，国内外学者对此进行了深入研究。文献[2]、文献[3]提出了利用改进的扩展卡尔曼滤波（Extended Kalman Filter，EKF）和简明误差卡尔曼滤波（Error State Kalman Filter，ESKF）算法提高机器人定位精度。文献[4]提出了利用自适应强跟踪误差状态扩展卡尔曼滤波（Adaptive Strong Tacking-Error-State Kalman Filter，AST-ESKF）算法对惯性导航模块与超宽带模块进行融合，提高了轨迹估计的精度。文献[5]、文献[6]分别提出了李群扩展卡尔曼滤波（Lie Group Extended Kalman Filter，LG-EKF）、无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法，提高了算法的估计精度。针对UKF无法满足高精度且易受系统模型影响的问题，文献[7]、文献[8]提出了自适应鲁棒无迹卡尔曼滤波（Adaptive Robust Unscented Kalman Filter，ARUKF）算法与改进的强跟踪自适应无迹卡尔曼滤波（Strong Tracking Adaptive Unscented Kalman Filter，STAUKF）算法。文献[9]提出了一种基于弯道自适应强跟踪卡尔曼滤波（Curve Adaptive Strong Tracking Kalman Filter，CASTKF）算法和侧向加速度传感器的可拓融合侧向坡度的估计算法，提高了车辆的侧向坡度估计精度。针对MEMS-IMU易受状态干扰和量测噪声影响的问题，文献[10]提出了模糊鲁棒自适应容积卡尔曼滤波（Fuzzy Robust Adaptive Cubature Kalman Filter，FRA-CKF）算法进行姿态估计，并提高了滤波精度。文献[11]提出了一种基于超宽带技术的自适应平方根容积卡尔曼滤波（Adaptive Square Root Cubature Kalman Filter，ASRCKF）算法，为无人车系统提供了更精准的定位数据。针对轮式车辆里程计标度变化较大的工况，文献[12]提出利用强跟踪容积卡尔曼滤波（Strong Tacking Cubature Kalman Filter，STCKF）对惯性导航模块和里程计进行信息融合，提高轮式车辆的定位精度。针对高超目标运动状态跟踪时精度低和收敛慢的不足，文献[13]提出自适应交互多模型（Adaptive Interacting Multiple-Model，AIMM）与STCKF融合的方法，提高了算法的收敛速度和精度。

当无人车运行环境发生变化时，单一传感器已无法保证导航系统的鲁棒性和可靠性^[14-16]。文献[17]提出了一种基于卡尔曼滤波（Kalman Filter，KF）的两级式冗余MEMS IMU信息融合方法，提高了姿态解算精度。考虑到不同类型传感器的耦合方式和解算方法不同，文献[18]、文献[19]采用不同精度的冗余传感器进行信息融合，提升了系统的容错率和解算精度。文献[20]、文献[21]利用反向传播（Back Propagation，BP）神经网络和多层感知机对冗余IMU阵列进行信息融合，提升了MEMS IMU的数据解算精度。

在双IMU传感器数据融合中，还未出现计算成本低、精度高且信息利用率高的融合算法。为此，本文提出利用粒子群优化（Particle Swarm Optimization，PSO）算法对STAUKF算法的后验估计及其输出的后验误差协方差进行寻优，从而对两个IMU单元的数据进行融合。

2 冗余式双MEMS IMU系统结构

收起

冗余式双MEMS IMU系统结构如所示，系统主要由IMU-STAUKF模块Ⅰ、IMU-STAUKF模块Ⅱ和PSO信息融合模块组成。2个IMU-STAUKF模块采用精度不同的六轴陀螺仪加速度计传感器，分别通过STAUKF算法进行滤波和姿态解算。数据融合模块分别计算冗余式双IMU传感器的误差函数，并利用PSO算法搜索最佳权重因子，实现冗余式双传感器数据融合。

3 自适应强跟踪无迹卡尔曼滤波算法

收起

3.1 无迹卡尔曼滤波算法

根据惯性传感器解算系统模型构建的非线性7维状态方程和10维量测方程为：

（1）

X K = f (X K - 1) + W K - 1 Z K = h (X K) + V K

式中：X_K=[q₀,q₁,q₂,q₃,ω_X,ω_Y,ω_Z]^T为K时刻的系统状态向量，Z_K=[q₀,q₁,q₂,q₃,ω_X,ω_Y,ω_Z,a_X,a_Y,a_Z]^T为K时刻的量测向量，q₀、q₁、q₂、q₃为四元数，ω_X、ω_Y、ω_Z分别为X、Y、Z轴角速度，a_X、a_Y、a_Z分别为X、Y、Z轴加速度，f( )、h( )分别为系统状态向量、量测向量的非线性传播函数，W_K_-1~N(0,Q_K)、V_K~N(r_K,R_K)分别为(K-1)时刻系统噪声和K时刻量测噪声，Q_K为W_K_-1在K时刻更新后的方差矩阵，r_K、R_K分别为K时刻V_K的期望矩阵与方差矩阵。

3.1.1 UKF的状态向量更新

UKF算法将先验误差协方差矩阵进行三角分解，对状态变量进行无迹变换（Unscented Transformation，UT），并通过f( )对Sigma点进行非线性传播。选取的Sigma点集和Sigma点非线性传播模型可表示为：

（2）

χ K - 1 = X^K - 1 X^K - 1 n + γ P X, K - 1 X^K - 1 n - γ P X, K - 1 T χ i, K K - 1 * = f (χ i, K - 1)

式中：P_X,K_-1为(K-1)时刻状态变量的后验误差协方差矩阵，

χ i, K K - 1 *

为(K-1)时刻所计算的K时刻非线性传播后的状态变量的第i个Sigma点，χ_i,K_-1为(K-1)时刻所选取的第i个Sigma点。

状态预测值及均方差矩阵可以表示为：

（3）

X^K K - 1 = ∑ i = 0 2 n W i m χ i, K K - 1 * P X, K K - 1 = ∑ i = 0 2 n W i c χ i, K K - 1 * - X^K K - 1 χ i, K K - 1 * - X^K K - 1 T + Q K - 1

式中：χ_K_-1为(K-1)时刻的Sigma点集；

X^K - 1

为(K-1)时刻的后验估计值；

X^K K - 1

为利用(K-1)时刻所估计K时刻的状态变量的预测值；P_X,K|K_-1为状态变量的先验误差协方差矩阵；W₀^m、W_i^c为求和权重，且

W 0 m = λ / γ 2 W i m = 1 / (2 γ 2)

，

W 0 c = λ / γ 2 + 1 - α 2 + β W i c = 1 / (2 γ 2)

；

γ = n + λ

；λ=α²(n+κ)-n；n为系统状态变量的维数；α=1为Sigma点分布因子；κ=0、β=2为比例因子。

3.1.2 UKF的量测向量更新

根据式（3）中

X^K K - 1

和P_X,K|K_-1进行第2次无迹变换，量测向量的Sigma点集和Sigma点非线性传播模型可表示为：

（4）

χ K - 1 = X^K - 1 X^K K - 1 n + γ P X, K K - 1 X^K K - 1 n - γ P X, K K - 1 T η i, K K - 1 = h (χ i, K K - 1)

式中：η_i,K|K_-1为量测向量的Sigma点集，h( )表示Sigma点的非线性传播。

量测预测值、均方差矩阵及协方差矩阵可表示为：

（5）

Y^K K - 1 = ∑ i = 0 2 n W i m η i, K K - 1 P Y, K | K - 1 = ∑ i = 0 2 n W i c η i, K K - 1 - Y^K K - 1 η i, K K - 1 - Y^K K - 1 T + R K P X Y, K | K - 1 = ∑ i = 0 2 n W i c χ i, K K - 1 - X^K K - 1 η i, K K - 1 - Y^K K - 1 T

式中：χ_K|K_-1为量测向量的Sigma点集，

Y^K K - 1

为量测向量预测值，P_Y,K|K_-1为量测向量的均方差矩阵，P_XY,K|K_-1为状态变量与量测向量的协方差矩阵。

因而，卡尔曼增益、后验估计值和先验误差协方差矩阵可表示为：

（6）

K K = P X Y, K | K - 1 P Y, K | K - 1 - 1

（7）

X^K = X^K K - 1 + K K (Z K - Y^K K - 1)

（8）

P X, K = P X, K K - 1 - K K P X Y, K | K - 1 K K T

式中：K_K为卡尔曼增益，

X^K

为状态变量的后验估计值，P_X,K为更新后的先验误差协方差矩阵。

3.2 自适应强跟踪滤波

强跟踪滤波源于量测故障的检测，可通过引入渐消因子λ_K对先验误差协方差矩阵P_X,K|K_-1进行修正，并实时调整卡尔曼增益。利用式（5）及当前传感器量测值Z_K计算的新息阵ε_K及其方差阵Γ_K可表示为：

（9）

ε K = Z K - Y^K K - 1

（10）

Γ K = ε K ε K T (1 - β K) Γ K - 1 + β K ε K ε K T

（11）

β K = β K - 1 β K - 1 + b, 0 b 1

式中：β_K为指数渐消因子，且β₀=1，β_∞=1-b；b为渐消因子，通常取0.95~0.99。

由于量测噪声矩阵R_K在非线性系统中不易获取，通过在STUKF算法中引入Sage-Husa自适应方法对其进行次优估计，则量测噪声可表示为：

（12）

R^K = (1 - β K) R^K - 1 + β K (ε K ε K T - P Y, K | K - 1)

由于强跟踪滤波基于线性系统推导，将强跟踪滤波引入UKF算法^[22]中，则单重渐消因子λ_K可表示为：

（13）

λ K = m a x 1, t r (N K) t r (M K)

式中：N_K=Γ_K-R_K，

M K = ∑ i = 0 2 n W i c η i, K K - 1 - Y^K K - 1 η i, K K - 1 - Y^K K - 1 T

。

则修正后的后验估计P_X,K可表示为：

（14）

P X, K = Λ K P X, K K - 1 Λ K - K K P Y, K K - 1 K K T

式中：

Λ K = d i a g (λ K, λ K, λ K, ⋯) α

，

α = d i a g (a 1, a 2, a 3, ⋯)

为预设的经验渐消比例系数，a₁~a₃分别为α中第1行~第3行的经验渐消比例系数。

综上，在双冗余MEMS IMU数据解算系统中，UKF算法的量测噪声不易建模，且估计精度低，可利用以下方法解决此问题：

a. 对量测系统的噪声进行实时估计，引入渐消Sage-Husa自适应，在滤波的同时对量测噪声进行估计；

b. 为提高滤波精度及对量测噪声的识别精度，引入多重渐消因子，修正UKF中的先验误差协方差矩阵。

4 基于粒子群优化的姿态数据融合

收起

4.1 姿态角后验估计融合

由于单一IMU传感器的精度一定，可以通过增加传感器的数量来提高量测系统的鲁棒性和数据解算精度。而精度的提升即意味着输出数据与真值间的均方差减小，故可利用求取均方差的极小值实现姿态的最优估计。

4.1.1 基于误差补偿的后验估计方法

2个IMU-STAUKF模块的量测相互独立时，则后验估计可表示为：

（15）

X^K 1,2 = X^K 1 + K 1,2 (X^K 2 - X^K 1)

式中：

X^K 1,2

为系统信息融合后的后验估计；

X^K 1

、

X^K 2

分别为IMU-STAUKF模块Ⅰ、IMU-STAUKF模块Ⅱ的后验估计值；K_1,2为权重因子，实现模块Ⅱ对模块Ⅰ的补偿。

对式（15）两端同时求方差可得：

（16）

P X, K 1,2 = (1 - K 1,2) 2 P X, K 1 + K 1,2 2 P X, K 2

式中：

P X, K 1,2

为信息融合后验估计的方差，

P X, K 1

、

P X, K 2

分别为模块Ⅰ与模块Ⅱ后验估计的方差。

4.1.2 基于信息融合后验估计方法

当模块Ⅰ与模块Ⅱ的量测相互独立时，依据信息融合的方法可得^[23]：

（17）

X^K 1,2 = K 1 P X, K 1 P X, K 1 + P X, K 2 X^K 1 + K 2 P X, K 2 P X, K 1 + P X, K 2 X^K 2

对式（17）两端同时求方差可得：

（18）

P X, K 1,2 = P X, K 1 P X, K 1 + P X, K 2 2 P X, K 1 K 12 + P X, K 2 P X, K 1 + P X, K 2 2 P X, K 2 K 22

式中：K₁、K₂为权重因子。

4.2 权重因子计算

粒子群优化算法中提出以下假设：

a. 每个粒子只拥有自身速度和位置这2种属性；

b. 每个粒子在函数解中运动，只通过速度来改变方向和位置；

c. 每个粒子会跟踪最优粒子移动，以通过较少的代数寻求最优解。

粒子群优化算法的计算函数为4.1节中所构造的误差函数，因而优化的权重因子为

P X, K 1,2

极小值点处的取值。每个粒子都会跟踪个体历史最优位置和种群历史最优位置，其算法可表示为：

（19）

x i j (t + 1) = x i j (t) + v i j (t + 1)

（20）

v i j (t + 1 = ω v i j (t) + c 1 r 1 (t) p i j (t) - x i j (t) + c 2 r 2 (t) p g j (t) - x i j (t)

式中：x_ij(t)为第t代（上一代）第j个维度的第i个粒子的位置；v_ij(t+1)为当前代粒子移动的速度；p_ij(t)为第t代粒子的个体极值（局部最优）；p_g_j(t)为第t代粒子的全局极值（全局最优）；ω=0.8为惯性权重；c₁、c₂为学习因子，取值均为0.5；r₁(t)、r₂(t)为(0,1)范围内的随机数。

式（20）右侧第1项为所保留的当前粒子个体的运动惯性信息，第2项为当前粒子的飞行经验对其当前速度的修正，第3项为种群所有粒子飞行经验对当前粒子速度的修正。

粒子群优化算法具体步骤如下：

a. 设定粒子搜索的空间边界、粒子飞行速度的上下限、最大迭代次数及c₁、c₂、ω，初始化粒子速度与位置；

b. 计算粒子的适应度并保存个体和种群的历史最优位置；

c. 由式（20）更新每个粒子的速度与位置；

d. 重复步骤b并更新个体粒子的历史最优位置；

e. 对比每个粒子与种群的最优位置和最佳适应度，并对种群最优位置和最佳适应度进行更新；

f. 判断所得出极值是否满足最大迭代次数或达到输出精度要求，若满足则输出最佳权重因子，否则重复步骤c~步骤f。

PSO计算完成后，将权重因子分别代入式（15）和式（17），即可得到融合后误差较小的姿态角数据。

综上，基于冗余式双IMU系统，首先利用STAUKF算法进行数据滤波和姿态解算并提出以下方法进行后验姿态融合：

a. 基于信息融合后验估计方法，依据PSO算法对两种精度不同的传感器信息进行姿态融合，获得系统的最优估计；

b. 基于误差补偿后验估计方法，依据PSO算法对两种精度不同的传感器信息进行姿态融合，获得系统的最优估计。

5 试验对比及数据分析

收起

5.1 试验平台

为验证所提出算法的正确性，利用STM32H743微控制器、六轴陀螺仪加速度计ICM20602及ICM20689搭建双余度系统测试平台，用于搭载冗余式双IMU系统的无人车如图2所示，车载硬件如表1所示。

其中，六轴陀螺加速度计ICM20602和ICM20689的参数如表2、表3所示。

5.2 算法对比

5.2.1 基于静态测量时的数据对比

将搭载有冗余式双IMU系统的无人车静止于某一平面时，对单一传感器ICM20602和ICM20689使用STAUKF算法解算，并利用数据平滑后加权平均的方式进行数据融合。再利用3.1节中的两种融合方法、PSO以及STAUKF进行联合解算。其中，横滚轴、俯仰轴分别采样2 000个采样点。以多算法线程在同一组点处多次采样后的均值作为基准计算数据的均方根误差，评判输出数据的精度。基于STAUKF算法及数据融合算法的横滚轴的姿态角和角度误差如图3所示，俯仰轴的姿态角和角度误差如图4所示。

试验算法的均方根误差如表4所示，两种后验估计融合算法所得到的姿态角均方根误差最小。基于误差补偿的后验估计融合算法以及基于信息融合的后验估计融合算法所解算的横滚轴与俯仰轴角度均方根误差，相较于两种单一传感器数据解算分别平均减小了79.92%、82.09%，相较于冗余式双IMU系统直接加权平均所解算的数据分别平均减小了68.29%、50.00%。

由于此时的角度基准为均值基准，故引入数据方差再次评判数据精度，各算法所解算数据方差如表5所示。

由表5可以看出，无论是横滚轴或俯仰轴，在静态时各算法输出数据中基于信息融合后验估计方法方差最小，基于误差补偿后验估计方法次之，直接对两个模块加权平均后所得数据方差较大，单一传感器利用STAUKF直接滤波输出后的方差最大。综上，由表4、表5综合评判可知：在静态测量时冗余式双IMU系统测量数据误差小于单一IMU传感器测量数据的误差，应用PSO与STAUKF联合解算的基于误差补偿的后验估计方法、基于信息融合的后验估计方法的测量数据误差小于直接对双IMU模块进行加权平均后所得到的数据，表明在静态正常测量时本文方法更具优势。

5.2.2 基于无人车运动时的测量数据对比

在搭载有冗余式双IMU系统的无人车平面运动时，对单一传感器ICM20602和ICM20689使用STAUKF算法解算，并利用数据平滑后加权平均的方式进行数据融合。再利用3.1节中的两种融合方法、PSO以及STAUKF进行联合解算。其中，横滚轴、俯仰轴分别采样2 000个采样点。以多算法线程在同一组点处多次采样后的均值作为基准计算数据的均方根误差，评判输出数据的精度。基于STAUKF算法及数据融合算法的横滚轴的姿态角和角度误差如图5所示，俯仰轴的姿态角和角度误差如图6所示。

由图5、图6可以看出，动态条件下数据离散程度较大，在某些点处可能会出现“野值”，但从图中可以直观地看到，本文所提出的方法很好地抑制了“野值”的出现，即便在较为复杂的运动环境中也可保持精度较高的姿态数据输出。

试验算法的均方根误差如表6所示，通过两种后验估计融合算法所得到的角度均方根误差最小。相较于两种单一IMU传感器解算数据，基于误差补偿的后验估计融合算法以及基于信息融合的后验估计融合算法所解算的横滚轴与俯仰轴角度均方根误差分别平均减小了56.67%、58.94%，相较于冗余式双IMU系统直接加权平均所解算的数据分别平均减小了36.55%、52.15%。

由于此时的角度基准为均值基准，故引入数据方差再次评判数据精度，各算法所解算数据方差如表7所示。

由表7可以看出，在无人车正常行驶状态下，各算法输出横滚轴与俯仰轴数据中基于信息融合后验估计方法与基于误差补偿后验估计方法的方差最小，直接对两个模块加权平均后所得数据方差较大，单一传感器利用STAUKF直接滤波输出后的方差最大。综上，由表6与表7综合评判可知：在无人车正常行驶时，冗余式双IMU系统测量数据误差小于单一IMU传感器测量数据的误差；应用PSO与STAUKF联合解算的基于误差补偿后验估计方法、基于信息融合后验估计方法的测量数据误差小于直接对双IMU模块进行加权平均后所得到的数据误差。试验数据说明冗余式双IMU系统满足辅助无人车建图导航需求，本文方法提高了双IMU系统的解算精度，并充分利用双IMU数据输出，增加了系统的鲁棒性。

5.2.3 无人车二维建图数据对比

利用无人车搭载冗余式双IMU系统并以Karto算法在实验室走廊进行二维建图测试，试验场景如图7所示，无人车建图测试结果对比如图8所示。

在路面不平整的情况下，采用冗余式双IMU系统对激光雷达所扫描的数据进行校正。由图8对比可知：在建图算法中引入IMU数据后所建地图的边界更为清晰；由冗余式双IMU系统应用信息融合后验估计方法处理后的数据所建地图相较直接将其加权平均后的地图边界更为平滑，少见重影与偏转；应用误差补偿后验估计方法处理后的数据所建地图效果相较于直接加权平均的方法优势明显，但还存在少量重影。试验结果证实了所提出的两种融合方法满足无人车建图需要，且在实际场景应用中基于信息融合后验估计方法更具优势。

6 结束语

收起

本文利用PSO算法对STAUKF算法所解算出的角度以误差寻优的方式进行融合，从而提升冗余式双IMU系统的数据精度与利用率，并通过无人车硬件平台对所提出的算法进行建图试验。通过多种算法的比较，可以看出本文所提出的融合方法较加权平均或单一传感器更具优势，且通过实测建图，可以证实所提出的方法满足低成本冗余MEMS IMU系统在无人车定位与导航系统中应用，并且两种方法精度差距不大，都提高了系统信息利用率以及系统鲁棒性，但在实测中建议使用基于信息融合后验估计方法，其精度最高。

基金

收起

*陕西省重点研发计划项目(2023-YBNY-205)
陕西省教育厅科研计划项目(18JK0146)

参考文献

收起

文献

收起

参考文献引证文献

排序方式：

[1]

李娜, 贺海育, 贾伟, 等. 基于2D建图算法的导航方法[J]. 无线电工程, 2022, 52(4): 612-616.

, HE

H Y

, JIA

, et al. Navigation Method Based on 2D Mapping Algorithm[J]. Radio Engineering, 2022, 52(4): 612-616.

[2]

仇海涛, 陈晓东, 张峰. 基于改进ESKF滤波和自适应滤波的组合导航算法研究[J]. 电光与控制, 2023, 30(5): 1-5.

QIU

H T

, CHEN

X D

, ZHANG

. Research on Integrated Navigation Algorithm Based on Improved ESKF Filter and Adaptive Filter[J]. Electronics Optics and Control, 2023, 30(5): 1-6.

[3]

周萌萌, 张冰, 赵强, 等. 基于自适应渐消Sage-Husa扩展卡尔曼滤波的协同定位算法[J]. 中国舰船研究, 2022, 17(4): 92-97.

ZHOU

M M

, ZHANG

, ZHAO

, et al. Cooperative Localization Algorithm Based on Adaptive Fading Sage-Husa Extended Kalman Filter[J]. China Ship Research, 2022, 17(4): 92-97.

[4]

张雯涛, 吴飞, 朱海. 自适应强跟踪AST-ESKF无人车室内导航算法[J]. 导航定位学报, 2022, 10(4): 34-42.

ZHANG

W T

, WU

, ZHU

. Indoor Navigation Algorithm for Unmanned Vehicle Based on Adaptive Strong Tracking AST-ESKF[J]. Journal of Navigation and Positioning, 2022, 10(4): 34-42.

[5]

CUI

J R

, WANG

M S

, WU

W Q

, et al. Lie Group Based Nonlinear State Errors for MEMS-IMU/GNSS/Magnetometer Integrated Navigation[J]. Journal of Navigation, 2021, 74(4): 887-900.

[6]

JULIER

, UHLMANN

, DURRANT-WHYTE

H F

. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477-482.

[7]

李明, 柴洪洲, 靳凯迪, 等. 改进的强跟踪自适应UKF算法及其在大方位失准角对准中的应用[J]. 导航定位学报, 2022, 10(6): 165-172.

, CHAI

H Z

, JIN

K D

, et al. Improved Strong Tracking Adaptive UKF Algorithm and Its Application in Large Azimuth Misalignment[J]. Journal of Navigation and Positioning, 2022, 10(6): 165-172.

[8]

许万, 程兆, 夏瑞东, 等. 一种基于动态残差的自适应鲁棒无迹卡尔曼滤波器定位算法[J]. 中国机械工程, 2023, 34(21): 2607-2614.

, CHENG

, XIA

R D

, et al. An Adaptive Robust Unscented Kalman Filter Localization Algorithm Based on Dynamic Residual[J]. China Mechanical Engineering, 2023, 34(21): 2607-2614.

[9]

刘轶材, 范志先, 王翔宇, 等. 基于弯道自适应强跟踪卡尔曼滤波的侧向坡度估计算法[J]. 同济大学学报(自然科学版), 2021, 49(增刊1): 148-154.

LIU

Y C

, FAN

Z X

, WANG

X Y

, et al. Road Bank Estimation Based on Curve Adaptive Strong Tracking Kalman Filter[J]. Journal of Tongji University (Natural Science Edition), 2021, 49(S1): 148-154.

[10]

乔美英, 高翼飞, 李宛妮, 等. 基于模糊鲁棒自适应CKF算法的MEMS-IMU姿态估计[J]. 中国惯性技术学报, 2022, 30(3): 296-303.

QIAO

M Y

, GAO

Y F

, LI

W N

, et al. Attitude Estimation of MEMS-IMU Based on Fuzzy Robust Adaptive CKF Algorithm[J]. Chinese Journal of Inertial Technology, 2022, 30(3): 296-303.

[11]

赵宏强, 邓文斌, 辛涛, 等. 基于UWB和KF的无人车目标跟踪方法[J]. 传感器与微系统, 2022, 41(10): 34-38.

ZHAO

H Q

, DENG

W B

, XIN

, et al. Target Tracking Method for Unmanned Ground Vehicle Based on UWB and KF[J]. Sensors and Microsystems, 2022, 41(10): 34-38.

[12]

李士心, 黄凤荣, 邱时前, 等. 基于自适应强跟踪滤波的捷联惯导/里程计组合导航方法[J]. 中国惯性技术学报, 2018, 26(2): 156-161.

S X

, HUANG

F R

, QIU

S Q

, et al. SINS/Odometer Integrated Navigation Method Based on Adaptive Strong Tracking Filter[J]. Chinese Journal of Inertial Technology, 2018, 26(2): 156-161.

[13]

罗亚伦, 廖育荣, 李兆铭, 等. 高超目标强跟踪CKF自适应交互多模型跟踪算法[J]. 北京航空航天大学学报(2022-09-28)[2023-10-24]. https://doi.org/10.13700/j.bh.1001-5965.2022.0587.

LUO

Y L

, LIAO

Y R

, LI

Z M

, et al. Strong Tracking CKF Adaptive Interacting Multiple-Model Algorithm Based on Maneuvering Hypersonic-Target Tracking[J]. Journal of Beijing University of Aeronautics and Astronautics(2022-09-28)[2023-10-24]. https://doi.org/10.13700/j.bh.1001-5965.2022.0587.

[14]

蔡英凤, 陆子恒, 李祎承, 等. 基于多传感器融合的紧耦合SLAM系统[J]. 汽车工程, 2022, 44(3): 350-361.

CAI

Y F

, LU

Z H

, LI

Y C

, et al. Tightly Coupled SLAM System Based on Multi-Sensor Fusion[J]. Automotive Engineering, 2022, 44(3): 350-361.

[15]

许智宾, 李宏伟, 张斌, 等. 双目视觉与惯导融合的移动机器人定位方法[J]. 测绘学报, 2021, 50(11): 1512-1521.

Z B

, LI

H W

, ZHANG

, et al. Localization Method of Mobile Robot Based on Binocular Vision and Inertial Navigation[J]. Journal of Surveying and Mapping, 2021, 50(11): 1512-1521.

[16]

许万, 朱力, 张宇豪, 等. 基于自适应UKF的移动机器人室内定位算法研究[J]. 传感器与微系统, 2022, 41(11): 44-47.

, ZHU

, ZHANG

Y H

, et al. Research on Indoor Localization Algorithm for Mobile Robot Based on Adaptive UKF[J]. Sensors and Microsystems, 2022, 41(11): 44-47.

[17]

XUE

, YANG

X G

, et al. A Redundant Fused MIMU Attitude System Algorithm Based on Two-Stage Data Fusion of MEMS Gyro Clusters Array[J]. Measurement, 2021, 184.

[18]

WANG

, WU

W Q

, WEI

, et al. Navigation Information Fusion in a Redundant Marine Rotational Inertial Navigation System Configuration[J]. Journal of Navigation, 2018, 71(6): 1531-1552.

[19]

XING

, TU

X W

, QIAN

W X

, et al. Performance Enhancement Method for Angular Rate Measurement Based on Redundant MEMS IMUs[J]. Micromachines, 2019, 10(8): 514.

[20]

乔文超, 王红雨, 王鸿东. 基于BP神经网络的无人机IMU多传感器冗余的补偿算法[J]. 电子测量与仪器学报, 2020, 34(12): 19-28.

QIAO

W C

, WANG

H Y

, WANG

H D

. Compensation Algorithm for UAV IMU Multi-Sensor Redundancy Based on BP Neural Network[J]. Journal of Electronic Measurement and Instrumentation, 2020, 34(12): 19-28.

[21]

廖亚桢, 刘昱, 张立强, 等. 面向PDR定位系统的多层感知机IMU阵列数据融合方法[J]. 电子测量与仪器学报, 2022, 36(8): 35-42.

LIAO

Y Z

, LIU

, ZHANG

L Q

, et al. Multi-Layer Sensor IMU Array Data Fusion Method for PDR Positioning System[J]. Journal of Electronic Measurement and Instrumentation, 2022, 36(8): 35-42.

[22]

蒋凯, 周建江, 吕瑞广, 等. 基于IMM-JPDA-ISTUKF的车载毫米波雷达多目标跟踪算法[J]. 现代雷达(2022-10-14)[2023-10-24]. http://kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html. http://kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html

JIANG

, ZHOU

J J

, LÜ

R G

, et al. Multi-Target Tracking Algorithm of Vehicle-Mounted Millimeter-Wave Radar Based on IMM-JPDA-ISTUKF[J]. Modern Radar(2022-10-14)[2023-10-24]. http://kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html. http://kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html

[23]

彦恭敏, 翁浚. 捷联惯导算法于组合导航原理[M]. 西安: 西北工业大学出版社, 2019.

YAN

G M

, WENG

. Strapdown Inertial Navigation Algorithm and Integrated Navigation Principle[M]. Xi’an: Northwestern Polytechnical University Press, 2019.

2024年第卷第8期

PDF下载

224

引用本文

BibTeX

文章信息

doi: 10.19620/j.cnki.1000-3703.20230744

首发时间：2025-12-22
出版时间：2024-08-24

补充材料

相关文章

文章信息

作者

出版历史

基金

*陕西省重点研发计划项目(2023-YBNY-205)

陕西省教育厅科研计划项目(18JK0146)

作者信息

陕西理工大学，汉中 723000

参考文献

分享链接

https://castjournals.cast.org.cn/joweb/qcjs/CN/10.19620/j.cnki.1000-3703.20230744

分享至

全文二维码

扫描看全文

引用本文

BibTeX

本文的引用情况

2种不同金属材料的力学参数

科 Family	属数 Number of genus	种数 Number of species	占总种数比例 Percentage of total species (%)	属 Genus	种数 Number of species	占总种数比例 Percentage of total species (%)
鹅膏菌科Amanitaceae	2	11	5.26	鹅膏菌属 Amanita	10	4.78
小菇科 Mycenaceae	2	12	5.74	丝盖伞属 Inocybe	5	2.39
多孔菌科 Polyporaceae	8	14	6.70	蜡蘑属 Laccaria	5	2.39
红菇科 Russulaceae	3	23	11.00	小皮伞属 Marasmius	6	2.87
				小菇属 Mycena	11	5.26
				光柄菇属 Pluteus	5	2.39
				红菇属 Russula	17	8.13
				栓菌属 Trametes	5	2.39

关闭全屏

BibTeX
EndNote
RefWorks
TxT

名称	型号
机载电脑	树莓派4B+
激光雷达	思岚A2
底层控制器	STM32H743
主电源	4S锂电池

名称

型号

机载电脑

树莓派4B+

激光雷达

思岚A2

底层控制器

STM32H743

主电源

4S锂电池

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	2.4×10^-4g^-1	6.1×10^-2 (°)/s
灵敏度初始误差/%	±1	±1
灵敏度动态误差/%	±1.5	±2
非线性度（±8 g）/%	±0.3	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.04 g	±1 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 5 g/℃	±0.01 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

参数类型

加速度计

陀螺仪

量程

±8 g

2 000 (°)/s

分辨率

2.4×10^-4g^-1

6.1×10^-2 (°)/s

灵敏度初始误差/%

±1

灵敏度动态误差/%

±1.5

±2

非线性度（±8 g）/%

±0.3

±0.1

初始公差（25 ℃）/初始零漂（25 ℃）

±0.04 g

±1 (°)/s

零漂温度系数（-40 ℃，85 ℃）

±0.000 5 g/℃

±0.01 (°)·s^-1·℃

最大输出频率（低噪声）/kHz

参数类型	加速度计	陀螺仪
量程	±8 g	2 000 (°)/s
分辨率	4 096 LSB/g	16.4 LSB·(°)^-1·s
灵敏度初始误差/%	±2	±2
灵敏度动态误差/%	±1	±1.5
非线性度（±8 g）/%	±0.5	±0.1
初始公差（25 ℃）/初始零漂（25 ℃）	±0.08 g	±5 (°)/s
零漂温度系数（-40 ℃，85 ℃）	±0.000 75 g/℃	±0.05 (°)·s^-1·℃
最大输出频率（低噪声）/kHz	4	8

参数类型

加速度计

陀螺仪

量程

±8 g

2 000 (°)/s

分辨率

4 096 LSB/g

16.4 LSB·(°)^-1·s

灵敏度初始误差/%

±2

灵敏度动态误差/%

±1

±1.5

非线性度（±8 g）/%

±0.5

±0.1

初始公差（25 ℃）/初始零漂（25 ℃）

±0.08 g

±5 (°)/s

零漂温度系数（-40 ℃，85 ℃）

±0.000 75 g/℃

±0.05 (°)·s^-1·℃

最大输出频率（低噪声）/kHz

算法	横滚角	俯仰角
ICM20689+STAUKF算法	5.62×10^-2	6.18×10^-2
ICM20602+ STAUKF算法	5.48×10^-2	5.53×10^-2
ICM20689+ICM20602加权平均	3.47×10^-2	2.16×10^-2
基于误差补偿的后验估计融合算法	1.10×10^-2	1.08×10^-2
基于信息融合的后验估计融合算法	1.10×10^-2	1.08×10^-2

算法

横滚角

俯仰角

ICM20689+STAUKF算法

5.62×10^-2

6.18×10^-2

ICM20602+ STAUKF算法

5.48×10^-2

5.53×10^-2

ICM20689+ICM20602加权平均

3.47×10^-2

2.16×10^-2

基于误差补偿的后验估计融合算法

1.10×10^-2

1.08×10^-2

基于信息融合的后验估计融合算法

1.10×10^-2

1.08×10^-2

算法	横滚角	俯仰角
ICM20689+STAUKF算法	3.9×10^-3	3.9×10^-3
ICM20602+STAUKF算法	2.5×10^-3	2.6×10^-3
ICM20689+ICM20602 加权平均	1.6×10^-3	1.6×10^-3
基于误差补偿的后验估计融合算法	8.395 5×10^-4	8.166 1×10^-4
基于信息融合的后验估计融合算法	8.393 8×10^-4	8.161 3×10^-4

算法

横滚角

俯仰角

ICM20689+STAUKF算法

3.9×10^-3

ICM20602+STAUKF算法

2.5×10^-3

2.6×10^-3

ICM20689+ICM20602
加权平均

1.6×10^-3

基于误差补偿的后验估计融合算法

8.395 5×10^-4

8.166 1×10^-4

基于信息融合的后验估计融合算法

8.393 8×10^-4

8.161 3×10^-4

算法	横滚角	俯仰角
ICM20689+STUKF算法	7.48×10^-2	7.62×10^-2
ICM20602+STUKF算法	6.97×10^-2	7.04×10^-2
ICM20689+ICM20602加权平均	4.76×10^-2	6.04×10^-2
基于误差补偿的后验估计融合算法	3.02×10^-2	2.89×10^-2
基于信息融合的后验估计融合算法	3.02×10^-2	2.89×10^-3

算法

横滚角

俯仰角

ICM20689+STUKF算法

7.48×10^-2

7.62×10^-2

ICM20602+STUKF算法

6.97×10^-2

7.04×10^-2

ICM20689+ICM20602加权平均

4.76×10^-2

6.04×10^-2

基于误差补偿的后验估计融合算法

3.02×10^-2

2.89×10^-2

基于信息融合的后验估计融合算法

3.02×10^-2

2.89×10^-3

算法	横滚角	俯仰角
ICM20689+STUKF算法	9.4×10^-3	8.6×10^-3
ICM20602+STUKF算法	8.3×10^-3	7.8×10^-3
ICM20689+ICM20602加权平均	7.1×10^-3	6.3×10^-3
基于误差补偿的后验估计融合算法	3.0×10^-3	2.8×10^-3
基于信息融合的后验估计融合算法	3.0×10^-3	2.8×10^-3

算法

横滚角

俯仰角

ICM20689+STUKF算法

9.4×10^-3

8.6×10^-3

ICM20602+STUKF算法

8.3×10^-3

7.8×10^-3

ICM20689+ICM20602加权平均

7.1×10^-3

6.3×10^-3

基于误差补偿的后验估计融合算法

3.0×10^-3

2.8×10^-3

基于信息融合的后验估计融合算法

3.0×10^-3

2.8×10^-3