本文以二维为例,讲解惯性导航(IMU与GPS传感器融合)。
一,坐标系与数据
如下图所示为GPS的轨迹图,(运行utility/showgps.m), 横坐标x为东向,纵坐标为北向. 如下图所示为imu的波形图,可以看出50s处(从上图中看出,车辆右转),角速度为负,局部坐标系应是x朝右,y超前,顺时针为负.
二,惯性导航方程
惯性导航的三大优势:
不受外界干扰
高频
6自由度估计
惯性导航方程描述的是估计变量与IMU量测值之间的关系,首先将加速度计观测到的局部观测值,转换到全局坐标系下去:
a
G
=
R
a
L
a^G = Ra^L
a G = R a L 其中
a
L
a^L
a L 是2D的局部加速度,R是姿态.陀螺仪的观测值角速度并没有全局局部之分,直接可以使用时间,将角度的增量求出来:
δ
θ
=
(
y
a
w
r
a
t
e
+
y
a
w
b
i
a
s
)
∗
δ
t
\delta \theta = (yaw_{rate} + yaw_{bias})*\delta t
δ θ = ( y a w r a t e + y a w b i a s ) ∗ δ t
姿态的更新
θ
t
=
θ
t
−
1
+
δ
θ
\theta_t = \theta_{t-1} + \delta \theta
θ t = θ t − 1 + δ θ
速度的更新
v
t
=
v
t
−
1
+
a
G
δ
t
=
v
t
−
1
+
R
∗
a
L
δ
t
v_t = v_{t-1} + a^G\delta t = v_{t-1} + R*a^L\delta t
v t = v t − 1 + a G δ t = v t − 1 + R ∗ a L δ t
位置的更新
s
t
=
s
t
−
1
+
v
t
−
1
δ
t
+
1
/
2
a
G
δ
t
2
=
s
t
−
1
+
v
t
−
1
δ
t
+
1
/
2
R
∗
a
L
δ
t
2
s_t = s_{t-1} + v_{t-1}\delta t + 1/2 a^G \delta t^2 = s_{t-1} + v_{t-1}\delta t + 1/2 R*a^L \delta t^2
s t = s t − 1 + v t − 1 δ t + 1 / 2 a G δ t 2 = s t − 1 + v t − 1 δ t + 1 / 2 R ∗ a L δ t 2
bias的更新
b
i
a
s
t
=
b
i
a
s
t
−
1
bias_{t} = bias_{t-1}
b i a s t = b i a s t − 1 其中变量维度:姿态,速度,位置,陀螺仪bias, 维度为1+2+2+1共6维
三,Prediction in EKF
状态的更新直接使用惯性导航方程
x
=
[
θ
,
v
,
s
,
b
i
a
s
]
T
x = [\theta, v, s, bias]^T
x = [ θ , v , s , b i a s ] T
x
ˉ
(
t
)
=
f
(
x
(
t
−
1
)
)
+
u
\bar{x}(t) = f(x(t-1)) + u
x ˉ ( t ) = f ( x ( t − 1 ) ) + u
P
ˉ
=
F
T
P
F
+
Q
\bar{P} = F^TPF + Q
P ˉ = F T P F + Q 其中P是一个66的协方差矩阵,f函数的输入是6 1, 输出为61, u是一个0为均值,Q为方差的过程噪声.F是一个6 6的雅克比矩阵:
F
=
∂
f
∂
x
=
[
1
0
0
0
0
δ
t
[
−
sin
(
θ
)
−
cos
(
θ
)
cos
(
θ
)
−
sin
(
θ
)
]
a
L
δ
t
1
0
0
0
0
.
.
.
0
1
0
0
0
1
/
2
[
−
sin
(
θ
)
−
cos
(
θ
)
cos
(
θ
)
−
sin
(
θ
)
]
a
L
δ
t
2
I
2
∗
2
δ
t
.
.
.
I
2
∗
2
.
.
.
0
0
0
0
0
0
1
]
F = \frac{\partial f}{\partial x } \\ = \left[ \begin{matrix}1 & 0 &0 &0 &0&\delta t \\ \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t &1&0&0&0&0\\ ...&0&1&0&0&0\\ 1/2 \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t^2 &I_{2*2}\delta t &...&I_{2*2}&...&0\\ 0 & 0 &0 &0 &0&1 \end{matrix}\right ]
F = ∂ x ∂ f = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 1 [ − sin ( θ ) cos ( θ ) − cos ( θ ) − sin ( θ ) ] a L δ t . . . 1 / 2 [ − sin ( θ ) cos ( θ ) − cos ( θ ) − sin ( θ ) ] a L δ t 2 0 0 1 0 I 2 ∗ 2 δ t 0 0 0 1 . . . 0 0 0 0 I 2 ∗ 2 0 0 0 0 . . . 0 δ t 0 0 0 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
四,Update using GPS
在组合惯性导航中,GPS是一个非常理想的可以与IMU配合使用的器件.IMU以100hz输出信息,GPS以10HZ输出信息.具体的,在EKF中,每次GPS的观测作为measurement包含到整体系统中.更新三板斧:计算残差,计算卡尔曼增益,计算状态量与方差. 首先残差可以定义为:
y
=
z
−
H
x
ˉ
y = z - H\bar{x}
y = z − H x ˉ 其中z为gps的21维的观测值,
x
ˉ
\bar{x}
x ˉ 为状态向量.H为2 6观测矩阵:
H
=
[
0
0
0
1
0
0
0
0
0
0
1
0
]
H = \left[ \begin{matrix}0&0&0&1&0&0\\0&0&0&0&1&0\end{matrix}\right]
H = [ 0 0 0 0 0 0 1 0 0 1 0 0 ] 卡尔曼增益(5*2)如下:
K
=
P
H
T
(
H
P
H
T
+
R
)
−
1
K = PH^T(HPH^T + R)^{-1}
K = P H T ( H P H T + R ) − 1 计算状态量:
x
=
x
ˉ
+
K
y
x = \bar{x} + Ky
x = x ˉ + K y 计算方差
P
=
(
I
−
K
H
)
P
ˉ
P = (I - KH)\bar{P}
P = ( I − K H ) P ˉ
五,实验
状态转换函数f(x)以及其雅克比F定义如下:
theta_t = theta_t_1 + yawrate*delta_t;
v_t = v_t_1 + to_R2d(theta_t_1)*a_L*delta_t;
s_t = s_t_1 + v_t_1*delta_t+ 1/2*to_R2d(theta_t_1)*a_L*delta_t*delta_t;
fx =[theta_t;v_t;s_t];
dR_theta = [-sin(theta_t_1),-cos(theta_t_1);cos(theta_t_1),-sin(theta_t_1)];
F = eye(5,5);
F(2:3, 1) = dR_theta*a_L*delta_t;
F(4:5, 1) =1/2*dR_theta*a_L*delta_t*delta_t;
F(4:5, 2:3) = eye(2,2)*delta_t;
预测部分:
[X_bar, F] = get_state_transition_F(delta_t,yaw_rate,aL,X(1),X(2:3),X(4:5));
Q = get_Q();
P_bar = F'*P*F + Q;
更新部分:
H = zeros(2,5);
H(1:2,4:5) = eye(2,2);
R = 4*eye(2,2);
z = data2d.GNSS.pos_EN(:,it_gps);
y = z - H*X_bar;
K = P_bar*H'*inv(H*P_bar*H' + R);
X = X_bar + K*y;
P = (eye(5,5) - K*H)*P_bar;
实验得到如下位置结果: 可以看出方向大致正确,因为GPS 1HZ的原因,导致在接收GPS的瞬间,并不是特别连续. 代码见:github 2d case . run: run_kf_INS.m.
接着使用tutlebot仿真数据 ,该数据有三个优点:1,IMU数据漂移较小。2,GPS为10HZ,3,坐标轴无歧义。 全局轨迹结果如下,其中红色为gps观测,蓝色为imu积分轨迹 将一处转弯处放大的结果如下:
扫描二维码关注公众号,回复:
8907047 查看本文章