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误差进行修正。修正方法如下式所示：
式中，分别表示修正后的三轴陀螺仪输出角速度。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
饮酒驾车一 、摘要
本文建立了一个为确定司机酒后驾车时酒精含量是否超标提供参考的数学模型。首先，将酒精含量视为血药浓度，借用药物动力学的喊雀房室模型，将酒精在肠胃的吸收过程和在血液中的分解过程抽象为吸收室和中心室里所发生的作用，运用微分方程理论推导出了吸收速率和分解速率随时间变化的规律（,），并用回归分析方法结合题述经验数据具体导出一人在未喝过酒的情况下，饮入2瓶啤酒的血液中酒精含量与时间的关系模型()，以此为基础回答题述的五个问题。
本文的最大特点是将房室模型灵活运用于司机各种不同的饮酒方式，使模型的应用范围和解释力都得到了郑顷早加强。
二、问题的重述
据报载，2003年全国道路交通事故死亡人数为10.4372万，其中因饮酒驾车造成的占有相当的比例。
针对这种严重的道路交通情况，国家质量监督检验检疫局2004年5月31日发布了新的《车辆驾驶人员血液、呼气酒精含量阈值与检验》国家标准，新标准规定，车辆驾驶人员血液中的酒精含量大于或等于20毫克／百毫升，小于80毫克／百毫升为饮酒驾车（原标准是小于100毫克／百毫升），血液中的酒精含量大于或等于80毫克／百毫升为醉酒驾车（原标准是大于或等于100毫克／百毫升）。
大李在中午12点喝了一瓶啤酒，下午6点检查时符合新的驾车标准，紧接着他在吃晚饭时又喝了一瓶啤酒，为了保险起见他呆到凌晨2点才驾车回家，又一次遭遇检查时却被定为饮酒驾车，这让他既懊恼又困惑，为什么喝同样多的酒，两次检查结乎燃果会不一样呢？
请你参考下面给出的数据（或自己收集资料）建立饮酒后血液中酒精含量的数学模型，并讨论以下问题：
1. 对大李碰到的情况做出解释；
2. 在喝了3瓶啤酒或者半斤低度白酒后多长时间内驾车就会违反上述标准，在以下情况下回答：
酒是在很短时间内喝的；
酒是在较长一段时间（比如2小时）内喝的。
3. 怎样估计血液中的酒精含量在什么时间最高。
4. 根据你的模型论证：如果天天喝酒，是否还能开车？
5. 根据你做的模型并结合新的国家标准写一篇短文，给想喝一点酒的司机如何驾车提出忠告。
参考数据
1. 人的体液占人的体重的65%至70%，其中血液只占体重的7%左右；而药物（包括酒精）在血液中的含量与在体液中的含量大体是一样的。
2. 体重约70kg的某人在短时间内喝下2瓶啤酒后，隔一定时间测量他的血液中酒精含量（毫克／百毫升），得到数据如下：
时间(小时) 0.25 0.5 0.75 1 1.5 2 2.5 3 3.5 4 4.5 5
酒精含量 30 68 75 82 82 77 68 68 58 51 50 41
时间(小时) 6 7 8 9 10 11 12 13 14 15 16
酒精含量 38 35 28 25 18 15 12 10 7 7 4
三、问题分析.
该部分包含于 五、模型的建立与分析 中.
四、模型假设与符号约定
假设人体密度是均匀的，而且酒精进入体液的时候马上均匀分布，且血液和体液的酒精浓度是一样的，都以c2 表示。
为了方便计算，保守计啤酒的酒精浓度为5%（g/ml）。
机体分为吸收室和中心室，两个室的的容积(v1,v2),即肠胃容积和体液（血液是其中的一部分）的体积在过程中保持不变。两室的酒精总量分别为x1, x2 。两室的酒精含量分别以浓度c1，c2表示。
酒精从一室向另一室的转移速率（速率系数k1），及中心室分解酒精的速率（速率系数k2）,与该室的酒精浓度成正比。
吸收室从外界补给酒精，并将其吸收和散布在体液中；中心室把体液中的酒精分解并将其产物排出中心室外的环境。两室某一时刻的酒精吸收速率或酒精的分解速率分别为,。
g0为酒精的补给速率，是啤酒补给量对时间的导数。
7、喝入2瓶啤酒的酒精总量为D0 。
在这些假设下的一种二室模型示意图如图所示。
中心室（2室）
x2 (t) ,c2(t),v2
吸收室（1室）
x1 (t) ,c1 (t),v1
K2
K1
G0
五、模型的建立与分析
5．1． 下面建立一人在未喝过酒的情况下，短时间内饮入2瓶啤酒的酒精含量与时间的关系模型。
由假设条件与上图，可以写出两个房室中酒精量所满足的微分方程。
的变化率由1室向2室的转移, 1室的酒精补给速率组成；的变化率由由1室向2室的转移及2室向体液外的转移组成。于是有
-------------------------------------------（1）
--------------------------------------------（2）
-------------------------------------------------（3）
由 （1）、（2）、（3）式，得
---------------------------（4）
-----------------------------（5）
（4）、（5）两式构成线性常系数非齐次微分方程，
一人在未喝过酒的情况下，短时间内饮入2瓶啤酒，g0=0，即一下子喝完啤酒，（4）式可化为
=------------------------------------（6）
同时，有c1(0)= ，c2(0)=0 ， 由（5）、（6）联合，通过MATLAB可解得
c1(t)=---------------------------------(7)
c2(t)= ---------------(8)
令A =，则(8)可化为
c2(t)= A—A-----------------------------（9）
不妨设<，
第一、当t很大时，（9）可看成是
c2(t)= A ------------------------------- (10)
两边取对数,使得(10)变为线性,得
ln c2(t)=lnA-k2t-------------------------------(11)
通过运用MATLAB进行线性回归分析, 得
lnA=4.9078
A=135.34
k2=0.20507
第二、由（9）得A= A—c2(t) ，在第一步的基础上代入k2，A，t可得出A—c2(t)的一系列具体的值，所以
ln（A）=lnA-k1t=ln（A—c2(t)）
就可以通过MATLAB做线性回归分析得出k1=1.4976 ，lnA=4.9425 ，A=140.1201
由第一步和第二步的出的lnA有点误差，但从它们的线性残差分析可见，这两个结果都可用，把上面得出的结果代入（9）式，得
c2(t)= ---------------（12）
这是一人在未喝过酒的情况下，短时间内饮入2瓶啤酒的酒精含量与时间的关系模型，（12）式作图如下：
下面是题目给出的数据作出的散点图，对比一下上图，可见上图拟合的结果是比较精确的。
5. 2 下面建立一人在未喝过酒的情况下，短时间内饮入n 瓶啤酒的酒精含量与时间的关系模型.
---------------------------（6）
-----------------------------（5）
c1(0)= ，c2(0)=0
c1(t)=-------------------------------------(13)
c2(t)= ---------------------------(14)
其中, A=135.34 , k2=0.20507 , k1=1.4976
5. 3. 当一人喝酒前肠胃（吸收室）里没有酒精或酒精已被吸收完了，而体液里（中心室）的酒精仍有残余，这时他立即在短时间内喝完n瓶酒，吸收室酒精总量为nD0/2 ,此时体液酒精浓度（设为初始浓度）c2(0)=a ,吸收室酒精初始浓度为c1(0)= nD0/2/v1，结合以下两式
---------------------（5）
=-----------------------（6）
c2(0)=a , c1(0)= nD0/2/v1
用MATLAB解之并化简得
-------------------------------(13)
---------------(15)
其中, A=135.34 , k2=0.20507 , k1=1.4976
5. 4 当一人吸收室酒精含量为0,中心室亦为0的时候,先用2小时持续地以均匀速率给酒n瓶,在2小时后给酒停住.在这种情况下,c1(0)=0, c2(0)=0 。率为g0= nD0/4.
此时的微分方程组的形式为:
-----------------------------（5）
---------------------------（16）
c1(0)=0, c2(0)=0
用MATLAB解之并化简得到:
-----------------------------------(17)
---------------(18)
其中, A=135.34 , k2=0.20507 , k1=1.4976
六、问题的解答
6．1. 对大李碰到的情况做出解释
大李在中午12点喝了一瓶啤酒，下午6点检查时符合新的驾车标准。因此符合5.1.的模型的条件。运用5.2.的模型，代入t=6，n=1
c2(6)=19.763<20,
符合大李在中午12点喝了一瓶啤酒，下午6点检查时符合新的驾车标准。
紧接着他在吃晚饭时又喝了一瓶啤酒，为了保险起见他呆到凌晨2点才驾车回家，又一次遭遇检查时却被定为饮酒驾车。根据5.1.的模型
c1(t)=-------------------------------------(7)
代入k1=1.4976， =25
（根据经验，可假设大李开始喝酒那一刻的吸收室酒精含量为普通啤酒酒精浓度的一半）
得c1(6)=0.003130
忽略不记。因此他的情况是下午6点吃饭时吸收室酒精浓度为0。按正常情况，大李可能属于5.3 模型，但计算 5.3 模型的 c2（8 ）=16.950<20，不符合超标的实际情况。
所以，大李的情况必然是晚饭时酒非一饮而尽，且其体液（即中心室）中有酒精残余。
但这种情况不包含在第五点所建立的四种情况的模型中。设他喝2小时酒，我们可以忽略着两个小时体液内的酒精消耗，设他从第8个小时开始一下子喝完，用5. 3.得 c2（6）=23.5955〉20
所以超标
6．2在喝了3瓶啤酒或者半斤低度白酒后多长时间内驾车就会违反上述标准，在以下情况下回答：
（1）酒是在很短时间内喝的
（2）酒是在较长一段时间（比如2小时）内喝的。
对（1），运用5.2.的模型，代入n=3，
c2(t)= ---------------------------(14)
其中, A=135.34 , k2=0.20507 , k1=1.4976
解不等式
得
同理，解不等式
得
因此，若酒是在很短时间内喝的，以开始喝酒那一时刻计时间，
内驾车就会违反上述“80mg”标准， 内驾车就会违反上述“20mg”标准。
对（2），运用5.4的模型，代入n=3，
----------------(18)
其中, A=135.34 , k2=0.20507 , k1=1.4976
解不等式
得
同理，解不等式
得
因此，若酒是在较长一段时间（比如2小时）内喝的，以开始喝酒那一时刻计时间，内驾车就会违反上述“80mg”标准，内驾车就会违反上述“20mg”标准。
6. 3 估计血液中的酒精含量在什么时间最高，需要对各种具体的饮酒方式的
血液中酒精含量与时间的关系模型()求最植，下面列举一些简单的结果。
对符合5。1 模型的情况，时候酒精含量最高；
对符合5。2模型的情况，时候酒精含量最高；
对符合5。3模型的情况，何时酒精含量最高与喝酒前血液（中心室）中酒精残余含量有关。
对符合5。4模型的情况，时候酒精含量最高。
6. 4 随着生活的快节奏，我们假设酒都是一下子喝完的，利用5. 2
c2(t)= <20
代入t=12 得出n=4 即每天不得超过4瓶,否则不能开车。
可见,只要掌握好了饮酒的方式和驾车的时机, 天天喝酒还是能开车的.
6 .5短文:
掌握好了饮酒的方式和驾车的时机
------- 给想喝一点酒的司机如何驾车提出忠告
司机朋友们,
掌握好了饮酒的方式和驾车的时机,既有利于公民对国家法规的遵守,又有利于公民人身安全的有效保障.在此,我们向各位司机朋友门提出几点忠告.
若你想喝2瓶啤酒,并且是短时间喝, 我们劝您10小时后方驾车上路.
若你想喝1瓶啤酒,并且是短时间喝, 我们劝您6小时后方驾车上路.
为了您和他人的安全与幸福, 请您掌握好了科学的饮酒的方式和适当的驾车的时机.
七. 参考文献
[1] 梁炼 数学建模 广州: 华南理工大学出版社, 2003.
[2] 姜启源 谢金星 叶俊 数学模型 (第三版) 北京: 高等教育出版社, 2003
[3] 韩伯棠 管理运筹学 北京: 高等教育出版社, 2003
[4] WILLAM F. LUCAS主编 微分方程模型 湖南: 国防科技大学出版社,1988
八. 附录
MATLAB命令在建模中的使用
[b,bint,r,rint,stats]=regress(y,x,0.05)
用MATLAB Statistics Toolbox 中的regress命令完成求解,其格式为
[b,bint,r,rint,stats]=regress(y,x,0.05)
其中输入的y为模型中的因变量数据n维向量，x为对应于回归系数α
=（α0, α1）的数据矩阵[ 1,x1 ] （n*2矩阵，其中第一列为全一向量），0.05为置信水平；输出b为的估计值。bint为b的置信区间，r为残差向量，rint为r 的置信区间。Stats为回归模型的检验统计量，有3个值，第一是回归方程的决定系数R2（R是相关系数），第二是F统计量，第三个是与F统计量对应的概率值p。
例如得到模型的回归系数估计值及置信区间，检验统计量R2，，F，p的结果见下表。
参数 参数估计值 置信区间
α0 108.62 [103.64，113.6]
α1 5.2994 [4.9362，5.6626]
R2=0.9777 F=920.77 P=0.000
上表显示，R2=0.9777指因变量y的97.77%可由模型（1）来决定，F远远超过检验的临界值，P远小于0.05。因而该模型整体来看是可用的。
[x,y]=dsolve('Dx=-k1*x+1/v1*g','Dy=k1*v1/v2*x-k2*y','t')
用来求解微分方程(组)的命令,例如上式表示求解微分方程组
---------------------------（4）
-----------------------------（5）
其中t为自变量, 因变量c1(t), c2(t), 在命令中以x,y 表示.
Plot(x, y)
绘图命令,描绘的图象.
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）