发明内容
为解决上述问题,本发明公开了一种测速方法,包括以下步骤:
第1步、初始化:设定采样点t=0,设置加速度计与定位传感器的采样时间为T,设置陀螺仪的采样时间为N·T,其中N为正整数;根据实际情况确定权重系数wi,i=1,2,3,4;
第2步、采集数据:令t←t+1,从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t;从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt,其中为在采样点t的东向坐标的定位结果,为在采样点t的北向坐标的定位结果,Rt为一个2维的对角阵,并且满足
其中,表示以为中心,以Rt为包络矩阵的椭球,θt=(xt,yt)′表示真实的位置坐标;
第3步、计算速度观测值及其误差包络矩阵:利用第2步获取的与Rt计算在采样点t的速度观测值,如下:
其中,为2×1的向量,第一个元素表示计算的东向移动速度的观测值,第二个元素表示计算的北向移动速度的观测值;计算误差的包络矩阵如下:
其中
其中,tr表示矩阵的迹;
第4步、计算速度观测值误差系数:利用第2步获取的计算在采样点t的速度观测值误差系数λt,如下:
其中,wi为权重系数,符合μi具体如下:
其中,为示性函数,当括号内的公式成立时返回1,否则返回0;
第5步、计算速度估计值:利用第2步获取的ax,t、ay,t,第3步获取的第4步获取的λt,计算在时间t的速度估计值,如下:
第5.1步、进行先验估计,获取在时间t的状态先验估计值与先验估计误差包络矩阵如下:
其中,Qt为在采样点t的过程噪声包络矩阵,是一个四维对角阵,为在采样点t的后验估计误差包络矩阵,
其中,blockdiag表示块对角阵,表示在采样点t的东向、北向的移动速度的先验估计,表示在采样点t的加速度计在东向、北向的漂移的先验估计;
第5.2步、进行后验估计,获取被测对象在采样点t的增益Kt、后验状态估计值与后验估计误差包络矩阵如下:
其中,I4为四维单位阵,C=[1,0,1,0],
其中,maxsvd表示最大奇异值,表示被测对象在采样点t的东向、北向的移动速度的后验估计,表示在采样点t的加速度计在东向、北向的漂移的后验估计;
第6步、重复第2步至第5步,输出每个采样点的其中第1、3个元素即为东向、北向的速度估计值。
相比于已有技术,本发明具有如下优点:消除了基于惯性传感器积分的速度估计的累计误差;比单独使用定位系统进行差分运算得到的速度推算值更为准确,可靠性更高;通过对陀螺仪输出序列的分析,能够获取当前车况,进而获取速度观测值误差系数,实现了速度估计过程中增益的自动调整,进一步提升了估计的准确性。
本发明还公开了一种测速装置,包括:
加速度计:用以获取被测对象东向、北向的加速度;
陀螺仪:用以获取被测对象的偏航速率;
定位传感器:用于获取被测对象的位置坐标;
数据处理单元:用于执行速度估计程序,以获取被测对象的速度,所述速度估计程序在执行时实现上述第1步至第6步。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图与具体实施例对本发明进行详细说明。
如图1所示,本发明公开了一种测速方法,步骤如下:
第1步、初始化:设定采样点t=0,设置加速度计与定位传感器的采样时间为T,设置陀螺仪的采样时间为N·T,其中N为正整数;根据实际情况确定权重系数wi,i=1,2,3,4;
第2步、采集数据:令t←t+1,从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t;从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt,其中为在采样点t的东向坐标的定位结果,为在采样点t的北向坐标的定位结果,Rt为一个2维的对角阵,并且满足
其中,表示以为中心,以Rt为包络矩阵的椭球,θt=(xt,yt)′表示真实的位置坐标;
第3步、计算速度观测值及其误差包络矩阵:利用第2步获取的与Rt计算在采样点t的速度观测值,如下:
其中,的向量,第一个元素表示计算的东向移动速度的观测值,第二个元素表示计算的北向移动速度的观测值;计算误差的包络矩阵如下:
其中
其中,tr表示矩阵的迹;
第4步、计算速度观测值误差系数:利用第2步获取的计算在采样点t的速度观测值误差系数λt,如下:
其中,wi为权重系数,符合μi具体如下:
其中,为示性函数,当括号内的公式成立时返回1,否则返回0;
第5步、计算速度估计值:利用第2步获取的ax,t、ay,t,第3步获取的第4步获取的λt,计算在时间t的速度估计值,如下:
第5.1步、进行先验估计,获取在时间t的状态先验估计值与先验估计误差包络矩阵如下:
其中,Qt为在采样点t的过程噪声包络矩阵,是一个四维对角阵,为在采样点t的后验估计误差包络矩阵,
其中,blockdiag表示块对角阵,表示在采样点t的东向、北向的移动速度的先验估计,表示在采样点t的加速度计在东向、北向的漂移的先验估计;
第5.2步、进行后验估计,获取被测对象在采样点t的增益Kt、后验状态估计值与后验估计误差包络矩阵如下:
其中,I4为四维单位阵,C=[1,0,1,0],
其中,maxsvd表示最大奇异值,表示被测对象在采样点t的东向、北向的移动速度的后验估计,表示在采样点t的加速度计在东向、北向的漂移的后验估计;
第6步、重复第2步至第5步,输出每个采样点的其中第1、3个元素即为东向、北向的速度估计值。
本发明还公开了一种测速装置,包括:
加速度计:用以获取被测对象东向、北向的加速度;
陀螺仪:用以获取被测对象的偏航速率;
定位传感器:用于获取被测对象的位置坐标;
数据处理单元:用于执行速度估计程序,以获取被测对象的速度,所述速度估计程序在执行时实现以下步骤:
第1步、初始化:设定采样点t=0,设置加速度计与定位传感器的采样时间为T,设置陀螺仪的采样时间为N·T,其中N为正整数;根据实际情况确定权重系数wi,i=1,2,3,4;
第2步、采集数据:令t←t+1,从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t;从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt,其中为在采样点t的东向坐标的定位结果,为在采样点t的北向坐标的定位结果,Rt为一个2维的对角阵,并且满足
其中,表示以为中心,以Rt为包络矩阵的椭球,θt=(xt,yt)′表示真实的位置坐标;
第3步、计算速度观测值及其误差包络矩阵:利用第2步获取的与Rt计算在采样点t的速度观测值,如下:
其中,为2×1的向量,第一个元素表示计算的东向移动速度的观测值,第二个元素表示计算的北向移动速度的观测值;计算误差的包络矩阵如下:
其中
其中,tr表示矩阵的迹;
第4步、计算速度观测值误差系数:利用第2步获取的计算在采样点t的速度观测值误差系数λt,如下:
其中,wi为权重系数,符合μi具体如下:
其中,为示性函数,当括号内的公式成立时返回1,否则返回0;
第5步、计算速度估计值:利用第2步获取的ax,t、ay,t,第3步获取的第4步获取的λt,计算在时间t的速度估计值,如下:
第5.1步、进行先验估计,获取在时间t的状态先验估计值与先验估计误差包络矩阵如下:
其中,Qt为在采样点t的过程噪声包络矩阵,是一个四维对角阵,为在采样点t的后验估计误差包络矩阵,
其中,blockdiag表示块对角阵,表示在采样点t的东向、北向的移动速度的先验估计,表示在采样点t的加速度计在东向、北向的漂移的先验估计;
第5.2步、进行后验估计,获取被测对象在采样点t的增益Kt、后验状态估计值与后验估计误差包络矩阵如下:
其中,I4为四维单位阵,C=[1,0,1,0],
其中,maxsvd表示最大奇异值,表示被测对象在采样点t的东向、北向的移动速度的后验估计,表示在采样点t的加速度计在东向、北向的漂移的后验估计;
第6步、重复第2步至第5步,输出每个采样点的其中第1、3个元素即为东向、北向的速度估计值。
在上述方法与装置中,在初始化过程中需要确定权重系数wi,i=1,2,3,4,可以采用如下实验法:首先让被测对象,例如一辆车,进行各种形式的运动,包括直线、弯道、频繁变换车道、急变速等;在运动过程中采集传感器的数据,进而获取大量的(μ1,μ2,μ3,μ4)样本,然后用λt进行标记;最后用这些样例集合进行训练即可以获取权重系数wi,i=1,2,3,4。其中,训练样例中的λt可以采用如下方式获取:实验获取的速度观测误差包络矩阵的对角线元素之和除以的对角线元素之和。
图2为东向速度实验结果图,图3为北向速度实验结果图。从中可见本发明公开的一种测速方法与装置具有更高的精度。
提供以上实施例仅仅是为了描述本发明的目的,而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改,均应涵盖在本发明的范围之内。