[发明专利]一种测速方法与装置

摘要

本发明公开了一种测速方法与装置，包含以下步骤：初始化、采集数据、计算速度观测值及误差包络矩阵、计算速度观测值误差系数、计算速度估计值，最终能够输出每个采样点的速度估计值。相比于已有技术，本发明具有如下优点：消除了基于惯性传感器积分的速度估计的累计误差；比单独使用定位系统进行差分运算得到的速度推算值更为准确，可靠性更高；通过对陀螺仪输出序列的分析，能够获取当前车况，进而获取速度观测值误差系数，实现了速度估计过程中增益的自动调整，进一步提升了估计的准确性。

主权项

1.一种测速方法，其特征在于包含以下步骤：第1步、初始化：设定采样点t＝0，设置加速度计与定位传感器的采样时间为T，设置陀螺仪的采样时间为N·T，其中N为正整数；根据实际情况确定权重系数ω_i,i＝1,2,3,4；第2步、采集数据：令t←t+1，从加速度计中获取在采样点t的被测对象东向、北向的加速度a_x,t与a_y,t；从陀螺仪中获取从采样点t‑1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵R_t，其中为在采样点t的东向坐标的定位结果，为在采样点t的北向坐标的定位结果，R_t为一个2维的对角阵，并且满足其中，表示以为中心，以R_t为包络矩阵的椭球，θ_t＝(x_t,y_t)′表示真实的位置坐标；第3步、计算速度观测值及其误差包络矩阵：利用第2步获取的与R_t计算在采样点t的速度观测值，如下：其中，为2×1的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算误差的包络矩阵如下：其中其中，tr表示矩阵的迹；第4步、计算速度观测值误差系数：利用第2步获取的计算在采样点t的速度观测值误差系数λ_t，如下：其中，ω_i为权重系数，符合μ_i具体如下：其中，为示性函数，当括号内的公式成立时返回1，否则返回0；第5步、计算速度估计值：利用第2步获取的a_x,t、a_y,t，第3步获取的第4步获取的λ_t，计算在时间t的速度估计值，如下：第5.1步、进行先验估计，获取在时间t的状态先验估计值与先验估计误差包络矩阵如下：其中，Q_t为在采样点t的过程噪声包络矩阵，是一个四维对角阵，为在采样点t‑1的后验估计误差包络矩阵，α_t＝[a_x,t,a_y,t]′,其中，blockdiag表示块对角阵，表示在采样点t的东向、北向的移动速度的先验估计，表示在采样点t的加速度计在东向、北向的漂移的先验估计；第5.2步、进行后验估计，获取被测对象在采样点t的增益K_t、后验状态估计值与后验估计误差包络矩阵如下：其中，I₄为四维单位阵，C＝[1,0,1,0]，其中，maxsvd表示最大奇异值，表示被测对象在采样点t的东向、北向的移动速度的后验估计，表示在采样点t的加速度计在东向、北向的漂移的后验估计；第6步、重复第2步至第5步，输出每个采样点的其中第1、3个元素即为东向、北向的速度估计值。