%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波器在INS-GPS组合导航中应用仿真
% Author : lylogn
% Email : lylogn@gmail.com
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 参考文献:
% [1]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006.
clear all
%% 惯性-GPS组合导航模型参数初始化
we = 360/24/60/60*pi/180%地球自转角速度，弧度/s
psi = 10*pi/180 %psi角度 / 弧度
Tge = 0.12
Tgn = 0.10
Tgz = 0.10 %这三个参数的含义详见参考文献
sigma_ge=1
sigma_gn=1
sigma_gz=1
%% 连续空间系统状态方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0
-we*sin(psi) 0 0 0 1 0 0 1 0
we*cos(psi) 0 0 0 0 1 0 0 1
00 0 -1/Tge 0 0 0 0 0
00 0 0 -1/Tgn 0 0 0 0
00 0 0 0 -1/Tgz 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0]%状态转移矩阵
B=[00 0sigma_ge*sqrt(2/Tge) 0 0 0 0 0
00 00 sigma_gn*sqrt(2/Tgn) 0 0 0 0
00 00 0 sigma_gz*sqrt(2/Tgz) 0 0 0]'%输入控制矩阵
%% 转化为离散时间系统状态方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1
[F,G]=c2d(A,B,T)
H=[10 0 0 0 0 0 0 0
0 -sec(psi) 0 0 0 0 0 0 0]%观测矩阵
%% 卡尔曼滤波器参数初始化
t=0:T:50-T
length=size(t,2)
y=zeros(2,length)
Q=0.5^2*eye(3)%系统噪声协方差
R=0.25^2*eye(2) %测量噪声协方差
y(1,:)=2*sin(pi*t*0.5)
y(2,:)=2*cos(pi*t*0.5)
Z=y+sqrt(R)*randn(2,length) %生成的含有噪声的假定观测值，2维
X=zeros(9,length) %状态估计值，9维
X(:,1)=[0,0,0,0,0,0,0,0,0]' %状态估计初始值设定
P=eye(9) %状态估计协方差
%% 卡尔曼滤波算法迭代过程
for n=2:length
X(:,n)=F*X(:,n-1)
P=F*P*F'+ G*Q*G'
Kg=P*H'/(H*P*H'+R)
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n))
P=(eye(9,9)-Kg*H)*P
end
%% 绘图代码
figure(1)
plot(y(1,:))
hold on
plot(y(2,:))
hold off
title('理想的观测量')
figure(2)
plot(Z(1,:))
hold on
plot(Z(2,:))
hold off
title('带有噪声的观测量')
figure(3)
plot(X(1,:))
hold on
plot(X(2,:))
hold off
title('滤波后的观测量')
matlab 零速检测,一种基于车辆零速检测的惯性导航误差修正方法与流程 转载2021-03-22 01:52:56
2点赞
叫我xh
码龄4年
关注
548fcc13f82c0347f916e8e1394811ec.gif
本发明涉及车载导航与定位领域，尤其是涉及一种基于车辆零速检测的惯性导航误差修正方法。
背景技术：
惯性导航系统(inertialnavigationsystem，ins)能根据惯性传感器(陀螺仪、加速度计)提供的载体相对于惯性空间的线速度和角速度信息，成为车载组合定位系统关键部件。惯性导航系统具有独立工作、精度高、不易被电子干扰的优点，但最明显的缺点是其导航误差会随时间积累，因而长时间工作后会产生不同程度的积累误差。
零速修正技术是克服惯导定位定姿累计误差发散最为实用的一种约束方法，该技术包含零速区间探测和零速更新两个部分，其中，零速检测是零速更新的基础。此外，零速检测信息可以作为ins相对于地球静止的参考，通过准静态对准来标定ins的速度、姿态和imu(inertialmeasurementunit，惯性测量单元)误差。利用传统的ins零速检测技术可对惯性导航误差进行修正，传统的ins零速检测技术将车辆加速度计和陀螺仪采集的数据作为检测量，然而，该方法的检测的准确度不够高，且无法抑制导航误差的漂移。
技术实现要素：
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种修正准确率高的基于车辆零速检测的惯性导航误差修正方法。
本发明的目的可以通过以下技术方案来实现：
一种基于车辆零速检测的惯性导航误差修正方法，包括零速检测步骤和零速修正步骤：
零速检测步骤：
采用三轴加速度数据、三轴陀螺仪进行数据采集。
1.1、车辆启动后，采集车辆ins数据及轮速传感器的轮速数据，车辆ins数据包括加速度计数据和陀螺仪数据。
假设某一时刻k的所有传感器观测值为yk，式中，为k时刻三轴加速度计的观测向量，为k时刻三轴陀螺仪的观测向量，为k时刻轮速传感器的观测向量。选择观测窗口期为n至n+n-1，n为窗口期开始时间点的数据，n为窗口期数据数量，得到一组连续观测序列
1.2、基于采集的数据，在车辆静止时，利用零速检测方法对车辆零速进行实时检测，若检测出零速，则进入零速修正步骤。
121)基于采集的数据，构建分布参数的最大似然估计，计算所有传感器联合概率的广义似然比。具体地：
a)定义h0表示载体为运动状态，h1表示载体为静止状态，零速检测器的性能由检测概率pd＝p(h0|h1)和虚警概率pfa＝p(h1|h0)决定，根据neyman-pearson理论，对于给定的pfa＝α，使判断为h1状态时检测概率pd最大，需满足：
式中，l(qn)为观测序列qn的似然比，其中，阈值γ由下式决定：
b)考虑到假设h0下信号较难描述，但h1情况下车辆静止且比力只有重力，三轴加速度计输出应为当地重力加速度，三轴陀螺仪输出应为0，轮速传感器输出也应为0，则设定信号约束条件为：
式中，为方向为重力方向的单位矢量，观测窗口期为n至n+n-1，n为窗口期开始时间点的数据，n为窗口期数据数量，l为窗口期中任意的一个数据，g为重力加速度，分别为加速度计、陀螺仪及轮速传感器真实值；分别为加速度计、陀螺仪及轮速传感器的测量噪声。
c)结合步骤a)、b)获取所有传感器联合概率的广义似然比的表达式。
所有传感器联合概率的广义似然比的表达式为：
式中，γ'＝-2(lnγ)/n，γ为阈值；其中为k时刻加速度计的观测向量，为k时刻陀螺仪的观测向量，为k时刻轮速传感器的观测向量；分别为加速度计、陀螺仪及轮速传感器的噪声方差值。
d)计算imu及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比的表达式，则认为载体处于静止状态。
122)计算imu及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比，则认为载体处于静止状态，即检测出零速。
零速修正步骤：
2.1、利用零速状态下的加速度计数据，对车辆姿态角中的俯仰角θ和翻滚角φ的估计，其表达式为：
式中，ax、ay、az为三轴加速度计输出，ωn＝{l∈n:n≤l＜n-1}，观测窗口期为n至n+n-1，n为窗口期开始时间点的数据，n为窗口期数据数量，l为窗口期中任意的一个数据。
2.2、利用零速时各传感器的输出，对陀螺仪的零偏进行再估计。
假设陀螺仪零偏为[bxbybx]，利用零速时各传感器的输出，对陀螺仪的零偏进行再估计的表达式为：
式中，ωx,ωy,ωz分别为原始的三轴陀螺仪的输出角速度。
2.3、当车辆启动后，利用估计的车辆姿态及陀螺仪的零偏对ins误差进行修正。修正方法的表达式为：
式中，分别为修正后的三轴陀螺仪的输出角速度。
与现有技术相比，本发明具有以下优点：
一、本发明面向车载组合导航，提出了一种零速检测方法和惯导误差修正方法，该方法相比于传统的ins零速检测，加入了轮速传感器作为新的检测量，通过采集轮速传感器和ins数据，利用黎曼皮尔逊假设理论构造多传感器联合概率密度的最大似然比，对车辆零速状态进行检测，在检测出车辆零速后，通过零速信息对速度、姿态进行更新，并对陀螺仪零偏以及车辆这翻滚角、俯仰角进行估计，本发明方法提高了检测的准确度；
二、本发明方法考虑到车辆需要启停的特性，能够有效地应用在车辆行驶的过程中，利用零速检测和零速修正对导航误差的漂移进行抑制，并对导航累计误差进行有效的修正。
附图说明
图1为本发明方法的流程示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
本发明涉及一种基于车辆零速检测的惯性导航误差修正方法，本发明分为两个模块，分别是零速检测模块和零速修正模块。零速检测模块为零速修正模块的输入，提供车辆是否处于零速的判断，以下称零速flag信号。若车辆处于零速，则运行零速修正模块；若车辆不处于零速状态，则不允许零速修正模块。
零速检测模块包括以下步骤：
步骤1、车辆启动后，采集加速度计、陀螺仪数据和轮速传感器采集的轮速数据。
通过加速度方差检测器、加速度幅值检测器、角速度能量检测器，计算得到加速度方差、加速度幅值、角速度能量。优选地，采用三轴加速度数据、三轴陀螺仪进行数据采集。即车辆行驶过程中采集的数据包括惯性测量数据，即三轴加速度数据、三轴陀螺仪数据以及轮速传感器采集的轮速数据。
假设某一时刻k的所有传感器观测值为yk，式中，为k时刻三轴加速度计的观测向量，为k时刻三轴陀螺仪的观测向量，为k时刻轮速传感器的观测向量。选择观测窗口期为n至n+n-1，n为窗口期开始时间点的数据，n为窗口期数据数量，得到一组连续观测序列
步骤2、通过构建分布参数的最大似然估计，计算所有传感器联合概率的广义似然比。
假设传感器测量值可以用下式进行描述：
yk＝sk+vk
其中：
式中，分别表示三轴加速度计、三轴陀螺仪及轮速传感器真实值；分别表示三轴加速度计、三轴陀螺仪及轮速传感器的测量噪声。假设示三轴加速度计、三轴陀螺仪及轮速传感器的噪声项均为独立分布的高斯白噪声，即协方差矩阵为：
式中，i3表示三维单位矩阵，01×3(03×1)表示大小为1×3(3×1)的零矩阵；分别表示三轴加速度计、三轴陀螺仪及轮速传感器的噪声方差值。
零速检测可作为二元假设检验问题进行研究，并定义假设为：
h0:载体为运动状态
h1：载体为静止状态
零速检测器的性能由检测概率(pd＝p(h0|h1))和虚警概率(pfa＝p(h1|h0))决定。根据neyman-pearson理论，对于给定的pfa＝α，要使得判断为h1状态时检测概率pd最大，有：
式中，l(qn)为信号qn的似然比，该式即为似然比检验(likelihoodratiotest)。
式中，阈值γ由下式决定：
考虑到假设h0下信号较难描述，但h1情况下车辆静止且比力只有重力，三轴加速度计输出应为当地重力加速度，三轴陀螺仪输出应为0，轮速传感器输出也应为0。两种假设条件下的信号约束应为：
式中，为方向为重力方向的单位矢量，l表示窗口期中任意的一个数据，g为重力加速度。推导得到融合轮速的零速检测器似然比检验式如下：
式中，γ'＝-2(lnγ)/n。
计算imu及轮速传感器输出数据，若满足上式，则认为载体处于静止状态。
步骤3、通过广义似然比检验，比较广义似然比与设定的检验阈值，并输出零速检测的结果。即若零速检测不符合上式要求，零速检测输出的零速flag为fasle，则不运行零速修正模块，ins正常输出。若零速检测符合上式要求，零速检测输出的零速flag为true，则运行零速修正模块。
若运行零速修正模块，则零速修正模块首先检测零速flag信号，若零速flag信号为true，则运行零速修正模块；若零速flag信号为false，则停止运行零速修正模块。
零速修正模块包括以下三个步骤：
1)速度姿态位置更新：
利用零速信息，对速度和位置进行置零更新，即v＝0,[ωxωyωz]＝[000]，式中ωx、ωy、ωz分别为原始的三轴陀螺仪的角速度输出。此时由于速度和角速度输入均为0，则载体的位置和姿态会维持停止前状态，不会进行更新。此时陀螺仪噪声及加速度计噪声无法对车辆位姿造成影响。
利用零速状态下的加速度计，对车辆姿态进行重新估计。具体地：
假设加速度输出则姿态角中的俯仰角θ和翻滚角φ可以通过加速度进行计算：
2)误差估计：利用零速时候传感器的输出，对陀螺仪的零偏进行再估计。
假设陀螺仪零偏为[bxbybz]，利用零速状态下陀螺仪的输出对陀螺仪零偏重新进行估计，具体如下式：
3)零偏、姿态修正：当车辆启动后，利用步骤1)、步骤2)估计的车辆姿态和陀螺零偏对ins误差进行修正。修正方法如下式所示：
式中，分别表示修正后的三轴陀螺仪输出角速度。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）