【项目总结】基于磁强计、IMU的组合导航算法

基于卡尔曼滤波器的磁强计、IMU 组合导航算法研究

背景介绍

由三轴加速度计和三轴陀螺仪组成的惯性测量单元 (IMU) 广泛用于自主导航系统。然而，IMU 的漂移会导致位置和姿态测量存在显著的累积误差。IMU 测量的主要参数是加速度和角速率，其他参数例如速度、位移和旋转角度是通过对加速度或角速率随时间积分得到的。然而，漂移和积分误差导致长时间运动测量中存在大量累积误差，甚至导致结果发散。对于长时间稳定测量，需要额外的辅助传感器来补充 IMU 系统，以构建用于漂移补偿的组合系统。这些辅助传感器包括超声波传感器、激光测距传感器、相机、太阳传感器和磁强计。

磁导航系统和惯性导航系统的算法融合度较高，能够实现全态姿动态测量，同时运算量能够在微控制器平台实现，实现成本较低，是自主导航以及姿态检测方案中常用的组合系统。

在本项目中，将基于磁力计补偿惯性传感器的漂移和累积误差的方法来进行实时姿态估计。其工作主要包括卡尔曼滤波器的原理解释、组合导航应用仿真、结果分析与讨论。

磁强计 +IMU 导航简介

IMU 惯性导航系统是以陀螺仪和加速度计为敏感器件的导航参数解算系统。

地磁定位导航技术基于地磁场是一个矢量场, 其强度大小和方向是位置的函数。同时地磁场具有丰富的总强度、矢量强度、磁倾角、磁偏角和强度梯度等特征，为地磁匹配提供了充足的信息。因此，将地磁场作为一个天然的坐标系, 利用地磁场的测量信息实现定位导航。

该系统可以实现卫星的自主导航，不依靠外部信息，仅利用星上自备的测量设备实时地确定自身的位置和速度。

导航坐标系

地球：可简要分为：圆球体、大地面包围成的球体、椭球体 ；一般的做法会将地球等效成椭球体，不同维度下的曲率半径不同，重力加速度不同，但是有恒定的地球自转角速度。

惯性坐标系是无加速度的或者处于匀速直线运动状态。而宇宙空间中的所有物体都处在运动中，因此需要根据不同的导航对象来选取惯性坐标系。

地心惯性坐标系 ： 做匀速直线运动 忽略了行星公转 ；忽略太阳、月亮及其它星体的引力，以及由于这些引力而存在的地球轨道运动。

发射点惯性坐标系（简称 li 系）：发射时刻的发射点惯性坐标系作为测量该载体飞行位置的基准。

地球坐标系（简称 e 系）：地球坐标系是原点在地心，坐标轴固定在地球上的右手正交坐标系。近似认为它相对惯性坐标系固有转动角速率旋转。ＯＺ 轴指向北极，ＯＸ 和 ＯＹ 轴都在赤道平面内。

地理坐标系（简称 t 系）：相对于大地水准面定义的东北天坐标系。其中，东北天坐标系原点为载体质心在大地水准面上的投影。x 轴沿参考椭球卯酉圈方向指向东，y 轴沿参考椭球子无圈方向指向北。

载体坐标系（简称 b 系）固连在运载体的参考坐标系 坐标原点为运载体的重心，by 轴沿载体横轴指向右翼，bx 轴沿载体纵轴指向机头方向。

导航坐标系（简称 n 系）。导航坐标系是在根据导航的需要求解载体导航信息时选取的参考坐标系。对于捷联惯性导航系统来说，导航信息并不在载体坐标系内求解，但惯性传感器的测量姿态值是在载体坐标系内进行的。因此，必须将惯性传感器的输出值变换到导航坐标系中，再进行导航信息的计算。

姿态角与四元数

姿态角，也叫欧拉角，姿态角是载体坐标系和导航坐标系之间的三个夹角

航向角：载体纵轴 bx 与北向轴（N）之间的夹角，在水平面测量，顺时针为正；

俯仰角：载体纵轴 bx 与水平面之间的夹角，在垂直面中测量，抬头为正；

横滚角：载体横轴 by 与水平面之间的夹角，在横截面测量，左边抬头为正。

旋转矩阵：

从 n 系到 b 系，坐标点可以通过三个姿态角相关的旋转矩阵得到。

姿态矩阵 – 捷联姿态测量系统中姿态的更新就是根据惯性传感器在载体坐标系中测得值实时的计算出姿态矩阵，然后从姿态矩阵中提取出所需的载体姿态信息。

四元数：

测量器件本身跟随着载体既有平移又有旋转运动，使得问题描述与求解变得非常困难。四元数的理论可将此类问题归为刚体绕定点的转动问题，从而有效地解决了这一问题。

不是从三个旋转角的分量描述，而是找到一个转轴 μ ，可以用 ijk 三个坐标轴表示 μ 的方向。一个坐标或一个矢量相对于某一坐标系的旋转，转角为 θ：

q 是旋转四元数 ；R‘为旋转后的投影： R’ =q’Rq

可以和方向余弦矩阵法 都是表示旋转后坐标系之间的转换，本质上是等价的。

利用四元数求解姿态的具体算法：

求解四元数姿态微分方程式：

$$
dq/dt= 1/2 q × ω
$$

假设角速度的采样周期为 T，则四阶龙格-库塔法计算的形式如下：

在利用四元数进行载体姿态解算中通常需要进行正交化处理，从而消除非正交误差对姿态解算精度的影响，其主要目的是使采用四元数方法获得的姿态转移矩阵随机漂移误差达到最小，可以由四元数的四个参数的平方和与四元数的模相比的方法进行归一化处理。

卡尔曼滤波算法

卡尔曼滤波器的主要作用可以概括为在带有噪声的系统中寻找状态量的最优估计。

在设计滤波器时，可以选择估计值的权重和观测值的权重，来对结果进行修正。

经典的卡尔曼滤波器仅适用于线性系统，要求信号满足叠加性和齐次性，噪声满足正态分布。

状态空间表达式：状态方程和观测方程。

我们最关心的是 Xk，当前的所测量实际物理量。每一个状态和之前的状态以及过程噪声息息相关。观测就是说对当前状态好会有一个转移，然后会有一个误差。我们可以使用状态方程得到当前的估计值，然后再结合观测值，加权得到当前的最优估计值。

我们的噪声序列，假设是满足正态分布。

一维方差-噪声的方差，QK RK； 状态的方差 估计方差 Xt

如果状态是多维，则会有不同的噪声，需要用到协方差矩阵(协方差用来描述随机变量的相关性)

在实现过程中需要定义超参数：类似于 PID，一个模型很难算出来 PID，需要人工训练；超参数，需要人工训练，很难一开始就确定。在 kalman 滤波器中 Q R 的噪声方差也需要自己训练。

计算过程

使用上一次的结果预测当前值，同时使用观测值修正

上图中，上面是预测，下面是更新。Kt 是 kalman 增益，量测更新 Kt 是观测值的权重。Pt 是协方差矩阵在不断更新

主要的公式 or 过程：

  1. 先验估计 Xk 估=A Xk-1 估 +μ Bk，其中 A 决定了是不是线性系统
  2. 先验估计协方差 最优估计的协方差，会是上一个估计的协方差矩阵左乘右乘状态转移矩阵
    cov(Ax,Ax)=Acov(x,x)A^T
  3. 修正估计 右下角的公式，得到 kalman 滤波在最终值的值 Kk kalman 增益，决定了观测值的权
  4. 更新增益 Kk 左三公式。和 Q 和 R 都有关，性质就是 Q+R 作分母，那个方差小权重就高

整体过程就是不断地预测、然后修正、更新值再给到下一次作预测、如此循环迭代。

关键参数的选择

Q&R 过程噪声方差矩阵 Q 模型理想与否 观测噪声 R 取决于传感器性能

由于 X0 P0 会迭代，可以取 X0=0，P 小一点，方便收敛。

使用 kalman 滤波时的注意事项

  1. 选择状态量、观测量
  2. 构建迭代方程、
  3. 初始化参数
  4. 带入公式、进行迭代
  5. 适当调节超参数

kf 在惯性导航应用

IMU 信号输出：

  1. 选择状态量
    陀螺仪噪声协方差 Q_angle
    陀螺仪漂移噪声协方差 Q_bias
    角度测量噪声协方差 R_angle
    测量得的角速度 Newguro
    采样周期 Dt
  2. 方程构建
    先验估计：明确当前的角度值、漂移值的预测模型
    预测误差协方差矩阵：P=APA^T+Q ;多维协方差矩阵；注意不同噪声 大多数时候是独立的，cov =0
    测量方程： Z=CX+V 在实际应用的 C 代码中不用模拟噪声 V，本省的输出就是带噪的
    计算 kalman 增益：K=PC (CPC+R)^-1
    最优估计方程：X=X+K(Z-CX)
    更新协方差阵：P=[I-KC]P

有了数学方程，而且还是时域的，代码只是数学的简单延续。

SIN 仿真

四元数计算、欧拉角、位置速度参数转换

首先是基于上面坐标系、四元数等基础知识的数学函数，主要是四元数与方向向量、欧拉角、旋转矩阵之间的转换，四元数与向量的乘积，归一化等基础操作。

例如下面是从四元数到旋转矩阵的转换函数：

1
2
3
4
5
6
7
8
9
function Cnb = q2mat(qnb)
 q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
 q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4); 
 q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4); 
 q44 = qnb(4)*qnb(4);
 Cnb = [ q11+q22-q33-q44, 2*(q23-q14), 2*(q24+q13);
 2*(q23+q14), q11-q22+q33-q44, 2*(q34-q12);
 2*(q24-q13), 2*(q34+q12), q11-q22-q33+q44 ];

其余的基础函数实现不在这里给出，详情可以参考：

误差导入：

实际地球重力场中如果垂线偏差较大而又不能精确补偿，将会带来不利影响，它等效于加速度计偏值误差或者水平失准角误差。姿态需要初始化，对于 IMU 是根据测地球自转角速度以及重力加速度进行姿态初始化。而磁强计可以靠地磁场对姿态进行初始化。

1
2
phi = [0.1; 0.2; 3]*arcmin; qnb = qaddphi(qnb, phi); % 失准角

圆锥误差和划船误差是理论推导时存在的误差形式，学界已有较多补偿方式。这里采用的是最经典的基于泰勒展开的多子样补偿算法。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
% cnscl.m 圆锥误差和划船误差补偿
function [phim, dvbm] = cnscl(wm, vm)
 cs = [ [2, 0, 0, 0, 0 ]/3
 [9, 27, 0, 0, 0 ]/20
 [54, 92, 214, 0, 0 ]/105
 [250, 525, 650, 1375, 0 ]/504
 [2315, 4558, 7296, 7834, 15797]/4620 ]; % 2-6 子样补偿系数
 wmm = sum(wm,1); vmm = sum(vm,1); dphim = zeros(1,3); scullm = zeros(1,3);
 n = size(wm, 1); % 子样数
if n>1
 csw = cs(n-1,1:n-1)*wm(1:n-1,:); csv = cs(n-1,1:n-1)*vm(1:n-1,:);
 dphim = cross(csw,wm(n,:)); % 圆锥补偿量
 scullm = cross(csw,vm(n,:))+cross(csv,wm(n,:)); % 划船补偿量
 end
phim = (wmm+dphim)';
dvbm = (vmm+0.5*cross(wmm,vmm)+scullm)';

IMU 的误差系统为无阻尼振荡系统，这里直接给出了一组精度较好的近似解析解，它全面包括了陀螺常值漂移误差、加速度计常值偏值误差、角度随机游走误差、速度随机游走误差。

1
2
3
4
5
6
7
8
9
10
11
12
function [wm, vm] = imuadderr(wm, vm, eb, web, db, wdb, ts)
 m = size(wm,1); sts = sqrt(ts);
 wm = wm + [ ts*eb(1) + sts*web(1)*randn(m,1), ...
 ts*eb(2) + sts*web(2)*randn(m,1), ...
 ts*eb(3) + sts*web(3)*randn(m,1) ];
 vm = vm + [ ts*db(1) + sts*wdb(1)*randn(m,1), ...
 ts*db(2) + sts*wdb(2)*randn(m,1), ...
 ts*db(3) + sts*wdb(3)*randn(m,1) ];

%在这里考虑了陀螺随机常值漂移误差 eb、角度随机游走误差 web，
%以及加速度计随机常值偏值误差 db、速度随机游走误差 wdb。

SINS 参数更新：

ins 的参数更新主要是基于角速度四元数微分方程的姿态更新算法；从 g 系变换到 n 系，在进行一次积分的速度更新算法；再积分一次得到的位置更新算法。

1
2
3
4
5
6
7
8
9
10
11
function [qnb, vn, pos, eth] = insupdate(qnb, vn, pos, wm, vm, ts)
 nn = size(wm,1); nts = nn*ts;
 [phim, dvbm] = cnscl(wm, vm); % 圆锥误差/划船误差补偿
 eth = earth(pos, vn); % 地球相关参数计算
 vn1 = vn + rv2m(-eth.wnin*nts/2)*qmulv(qnb,dvbm) + eth.gcc*nts; % 速度更新
 vn = (vn+vn1)/2;
 pos = pos + [vn(2)/eth.RMh;vn(1)/eth.clRNh;vn(3)]*nts; % 位置更新
 vn = vn1; 
 qnb = qmul(rv2q(-eth.wnin*nts), qmul(qnb, rv2q(phim))); % 姿态更新
 qnb = qnormlz(qnb);

kalman 滤波器实现：

  1. 先验估计 Xk 估=A Xk-1 估 +μ Bk，其中状态转移矩阵决定了是不是线性系统
  2. 先验估计协方差 最优估计的协方差，会是上一个估计的协方差矩阵左乘右乘状态转移矩阵
    cov(Ax,Ax)=Acov(x,x)A^T
  3. 修正估计 右下角的公式，得到 kalman 滤波在最终值的值 Kk kalman 增益，决定了观测值的权
  4. 更新增益 Kk 左三公式。和 Q 和 R 都有关，性质就是 Q+R 作分母，那个方差小权重就高

整体过程就是不断地预测、然后修正、更新值再给到下一次作预测、如此循环迭代。Q&R 过程噪声方差矩阵 Q 模型理想与否 观测噪声 R 取决于传感器性能。由于 X0 P0 会迭代，可以取 X0=0，P 小一点，方便收敛。

使用 kalman 滤波时的注意事项

  1. 选择状态量、观测量
  2. 构建迭代方程
  3. 初始化参数
  4. 带入公式、进行迭代
  5. 适当调节超参数

初始化：

1
2
3
4
5
6
7
8
function kf = kfinit(Qk, Rk, P0, Phikk_1, Hk, Tauk)
 [kf.m, kf.n] = size(Hk);
 kf.Qk = Qk; kf.Rk = Rk; kf.Pk = P0; kf.Xk = zeros(kf.n,1);
 kf.Phikk_1 = Phikk_1; kf.Hk = Hk;
 if nargin<6, kf.Tauk = eye(kf.n);
 else kf.Tauk = Tauk; 
 end

更新：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
function kf = kfupdate(kf, Zk, TimeMeasBoth)
 if nargin==1, TimeMeasBoth = 'T';
 elseif nargin==2, TimeMeasBoth = 'B'; end
 if TimeMeasBoth=='T' || TimeMeasBoth=='B' % 时间更新
 kf.Xkk_1 = kf.Phikk_1*kf.Xk;
 kf.Pkk_1 = kf.Phikk_1*kf.Pk*kf.Phikk_1' + kf.Tauk*kf.Qk*kf.Tauk';
 else % TimeMeasBoth=='M'
 kf.Xkk_1 = kf.Xk;
 kf.Pkk_1 = kf.Pk; 
 end
 if TimeMeasBoth=='M' || TimeMeasBoth=='B' % 量测更新
 kf.PXZkk_1 = kf.Pkk_1*kf.Hk';
 kf.PZkk_1 = kf.Hk*kf.PXZkk_1 + kf.Rk;
 kf.Kk = kf.PXZkk_1/kf.PZkk_1;
 kf.Xk = kf.Xkk_1 + kf.Kk*(Zk-kf.Hk*kf.Xkk_1);
 kf.Pk = kf.Pkk_1 - kf.Kk*kf.PZkk_1*kf.Kk';
 else % TimeMeasBoth=='T'
 kf.Xk = kf.Xkk_1;
 kf.Pk = kf.Pkk_1; 
 end
 kf.Pk = (kf.Pk+kf.Pk')/2; % P阵对称化

SINS 仿真主程序：

首先是初始化采样时间、初始导航参数，然后引入误差，并进行一定时长的导航参数仿真。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
nn = 2; ts = 0.1; nts = nn*ts; % 子样数和采样时间
att = [0; 0; 30]*arcdeg; 
vn = [0;0;0]; 
pos = [40*arcdeg; 116*arcdeg; 100];
qnb = a2qua(att); % 姿态、速度和位置初始化

eth = earth(pos, vn);
wm = qmulv(qconj(qnb),eth.wnie)*ts; vm = qmulv(qconj(qnb),-eth.gn)*ts;
wm = repmat(wm', nn, 1); vm = repmat(vm', nn, 1); 
phi = [0.1; 0.2; 3]*arcmin; qnb = qaddphi(qnb, phi); % 失准角
eb = [0.01;0.015;0.02]*dph; web = [0.001;0.001;0.001]*dpsh; % 陀螺常值零偏，角度随机游走系数
db = [80;90;100]*ug; wdb = [1;1;1]*ugpsHz; % 加速度计常值偏值，速度随机游走系数
qnb = qaddphi(qnb, phi);% 仿真静态IMU数据

len = fix(3600/ts); % 仿真时长
avp = zeros(len, 10); kk = 1; t = 0; % 记录导航结果 [att, vn, pos, t]
for k=1:nn:len
 t = t + nts;
 [wm, vm] = imuadderr(wm, vm, eb, web, db, wdb, ts);
 [qnb, vn, pos] = insupdate(qnb, vn, pos, wm, vm, ts); 
 vn(3) = 0;
avp(kk,:) = [q2att(qnb); vn; pos; t]'; 
kk = kk+1;
end

加入磁强计定位和卡拉曼滤波：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
nn = 2; ts = 0.1; nts = nn*ts; % 子样数和采样时间
att0 = [0; 0; 30]*arcdeg; qnb0 = a2qua(att0); vn0 = [0;0;0]; pos0 = [34*arcdeg; 108*arcdeg; 100];
qnb = qnb0; vn = vn0; pos = pos0; % 姿态、速度和位置初始化
eth = earth(pos, vn);
wm = qmulv(qconj(qnb),eth.wnie)*ts; vm = qmulv(qconj(qnb),-eth.gn)*ts;
wm = repmat(wm', nn, 1); vm = repmat(vm', nn, 1); % 静态IMU数据

%phi = [0.1; 0.2; 3]*arcmin; qnb = qaddphi(qnb, phi); % 失准角
eb = [0.01;0.015;0.02]*dph; web = [0.001;0.001;0.001]*dpsh; % 陀螺常值零偏，角度随机游走系数
db = [80;90;100]*ug; wdb = [1;1;1]*ugpsHz; % 加速度计常值偏值，速度随机游走系数

Qk = diag([web; wdb; zeros(9,1)])^2*nts;
rk = [[0.1;0.1;0.1];[[10;10]/Re;10]]; Rk = diag(rk)^2;%kalman 误差矩阵
P0 = diag([[0.1;0.1;10]*arcdeg; [1;1;1]; [[10;10]/Re;10]; [0.1;0.1;0.1]*dph; [100;100;100]*ug])^2;
Hk = [zeros(6,3),eye(6),zeros(6)];%初始增益矩阵
kf = kfinit(Qk, Rk, P0, zeros(15), Hk); % kf滤波器初始化

len = fix(3600/ts); % 仿真时长
avp = zeros(len, 10); xkpk = zeros(len, 2*kf.n+1); kk = 1; t = 0; % 记录导航结果
for k=1:nn:len
 t = t + nts;
 [wm1, vm1] = imuadderr(wm, vm, eb, web, db, wdb, ts);
 [qnb, vn, pos, eth] = insupdate(qnb, vn, pos, wm1, vm1, ts);
 kf.Phikk_1 = eye(15) + kfft15(eth, q2mat(qnb), sum(vm1,1)'/nts)*nts;
 kf = kfupdate(kf);
 if mod(t,1)<nts
 mag = [vn0; pos0] + rk.*randn(6,1); % 速度位置仿真
 kf = kfupdate(kf, [vn;pos]-mag, 'M');
 vn(3) = vn(3) - kf.Xk(6); kf.Xk(6) = 0; % ·反馈
 end
 avp(kk,:) = [qq2phi(qnb,qnb0); vn; pos; t]';
 xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t]; kk = kk+1;
end

结果讨论

kalman 滤波效果

未使用 kalman 滤波时的导航结果：

可以看到无论是方向角还是速度、位置参数，都是发散较大的，结果几乎无法使用。

在引入磁强计信号定期更新观测矩阵的 kalman 滤波器后，可以看到导航的结果有较好的收敛：误差量级比之前小了几个量级。

修正参数精度对估计影响

用于修正累积误差的磁强计传感器不同的精度，会导致修正结果的不同。主要结论是：修正的结果都能收敛，但是稳定后的误差限以及收敛速度不同。磁强计精度越高，收敛越快，收敛后误差越小。

误差矩阵更新度对结果的影响

在实际的计算过程中，实时更新误差矩阵能带来更好的估计。在仿真代码中不进行 QR 矩阵更新，也能收敛，但是收敛效果不好。

不足

  1. 没有基于磁强计参数计算初始姿态以及实时航向角信息。
    在仿真过程中，磁强计的修正结果直接以精确值 + 随机误差的形式给出，而不是通过三轴磁强参数通过一定的计算方法给出，距离实际应用的算法还有待改进。
  2. 由于没有计算磁强计的组合系统，系统仍是线性的，没有用到 EKF、UKF 等算法。

实际组合导航系统的量测矩阵会是非线性的，目前也有很多关于非线性卡尔曼滤波器的研究。但是目前的仿真系统作了一定的简化，没有用到扩展卡尔曼滤波器等方法。

  1. 误差都是理论的正态分布随机误差，是仿真模型，没有获得实际传感器信号输入。
#课程学习
【项目总结】基于磁强计、IMU的组合导航算法
http://example.com/2023/03/25/【项目总结】基于磁强计、IMU的组合导航算法/
作者
Chery Young
发布于
2023年3月25日
许可协议