本文以二维为例,讲解惯性导航(IMU与GPS传感器融合)。
如下图所示为GPS的轨迹图,(运行utility/showgps.m), 横坐标x为东向,纵坐标为北向.
如下图所示为imu的波形图,可以看出50s处(从上图中看出,车辆右转),角速度为负,局部坐标系应是x朝右,y超前,顺时针为负.
惯性导航的三大优势:
惯性导航方程描述的是估计变量与IMU量测值之间的关系,首先将加速度计观测到的局部观测值,转换到全局坐标系下去:
a G = R a L a^G = Ra^L aG=RaL
其中 a L a^L aL是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 δθ=(yawrate+yawbias)∗δt
状态的更新直接使用惯性导航方程
x = [ θ , v , s , b i a s ] T x = [\theta, v, s, bias]^T x=[θ,v,s,bias]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ˉ=FTPF+Q
其中P是一个66的协方差矩阵,f函数的输入是61, 输出为61, u是一个0为均值,Q为方差的过程噪声.F是一个66的雅克比矩阵:
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(θ)]aLδt...1/2[−sin(θ)cos(θ)−cos(θ)−sin(θ)]aLδt20010I2∗2δt0001...0000I2∗20000...0δt0001⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
在组合惯性导航中,GPS是一个非常理想的可以与IMU配合使用的器件.IMU以100hz输出信息,GPS以10HZ输出信息.具体的,在EKF中,每次GPS的观测作为measurement包含到整体系统中.更新三板斧:计算残差,计算卡尔曼增益,计算状态量与方差.
首先残差可以定义为:
y = z − H x ˉ y = z - H\bar{x} y=z−Hxˉ
其中z为gps的21维的观测值, x ˉ \bar{x} xˉ为状态向量.H为26观测矩阵:
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=[000000100100]
卡尔曼增益(5*2)如下:
K = P H T ( H P H T + R ) − 1 K = PH^T(HPH^T + R)^{-1} K=PHT(HPHT+R)−1
计算状态量:
x = x ˉ + K y x = \bar{x} + Ky x=xˉ+Ky
计算方差
P = ( I − K H ) P ˉ P = (I - KH)\bar{P} P=(I−KH)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积分轨迹
将一处转弯处放大的结果如下: