本发明公开了一种测速方法与装置，包含以下步骤：初始化、采集数据、计算速度观测值及误差包络矩阵、计算速度观测值误差系数、计算速度估计值，最终能够输出每个采样点的速度估计值。相比于已有技术，本发明具有如下优点：消除了基于惯性传感器积分的速度估计的累计误差；比单独使用定位系统进行差分运算得到的速度推算值更为准确，可靠性更高；通过对陀螺仪输出序列的分析，能够获取当前车况，进而获取速度观测值误差系数，实现了速度估计过程中增益的自动调整，进一步提升了估计的准确性。
第1步、初始化：设定采样点t＝0，设置加速度计与定位传感器的采样时间为T，设置陀螺仪的采样时间为N·T，其中N为正整数；根据实际情况确定权重系数ωi,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的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算 误差的包络矩阵 如下：其中
第4步、计算速度观测值误差系数：利用第2步获取的 计算在采样点t的速度观测值误差系数λt，如下：其中，ωi为权重系数，符合 μi具体如下：
其中， 为示性函数，当括号内的公式成立时返回1，否则返回0；
第5步、计算速度估计值：利用第2步获取的ax,t、ay,t，第3步获取的 第4步获取的λt，计算在时间t的速度估计值，如下：第5.1步、进行先验估计，获取在时间t的状态先验估计值 与先验估计误差包络矩阵如下：其中，Qt为在采样点t的过程噪声包络矩阵，是一个四维对角阵， 为在采样点t-1的后验估计误差包络矩阵，αt＝[ax,t,ay,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为正整数；根据实际情况确定权重系数ωi,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的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算 误差的包络矩阵 如下：其中
第4步、计算速度观测值误差系数：利用第2步获取的 计算在采样点t的速度观测值误差系数λt，如下：其中，ωi为权重系数，符合 μi具体如下：
其中， 为示性函数，当括号内的公式成立时返回1，否则返回0；
第5步、计算速度估计值：利用第2步获取的ax,t、ay,t，第3步获取的 第4步获取的λt，计算在时间t的速度估计值，如下：第5.1步、进行先验估计，获取在时间t的状态先验估计值 与先验估计误差包络矩阵如下：其中，Qt为在采样点t的过程噪声包络矩阵，是一个四维对角阵， 为在采样点t-1的后验估计误差包络矩阵，αt＝[ax,t,ay,t]′,
其中，blockdiag表示块对角阵， 表示在采样点t的东向、北向的移动速度的先验估计， 表示在采样点t的加速度计在东向、北向的漂移的先验估计；
第5.2步、进行后验估计，获取被测对象在采样点t的增益Kt、后验状态估计值 与后验估计误差包络矩阵 如下：其中，I4为四维单位阵，C＝[1,0,1,0]，其中，maxsvd表示最大奇异值， 表示被测对象在采样点t的东向、北向的移动速度的后验估计， 表示在采样点t的加速度计在东向、北向的漂移的后验估计；
第6步、重复第2步至第5步，输出每个采样点的 其中第1、3个元素即为东向、北向的速度估计值。
一种测速方法与装置
技术领域
[0001] 本发明涉及信号处理技术，特别是涉及一种测速方法与装置。
背景技术
[0002] 在很多应用场合中，需要实时地获取目标的运动速度，即其相对于东、北方向的速度分量值。一种常用的手段是利用安装在目标上的速度检测器来获取速度。文献“牛英明、肖骁、夏夕盛、李超，一种测速系统，申请号：CN201310714583.X，申请日：2013-12-20”公开了一种测速系统，用于对车辆进行速度检测，包括至少一个测速子系统，每个测速子系统包括：多个驱动模块，多个输入表决模块，多个单元处理模块，速度融合模块，输出表决模块。该发明提供的测速系统中，对传感器采集的车辆速度信号，进行多次判决保证采集到的速度信号可用，然后利用可用的速度信号进行融合，可以提高速度检测的准确性、可靠性和可用性。文献“赖鸿庆，测速方法以及测速装置，申请号：CN201610620233.0，申请日：2016-08-
01”公开了一种测速方法以及测速装置，其中，测速装置包含：一影像撷取装置，用以撷取一待测物的至少一待测物影像；以及一速度计算装置，用以根据该待测物影像的残影长度来计算该待测物的一移动速度。文献“程赟、来先家，一种测速设备及测速系统，申请号：
CN201810299178.9，申请日：2018-04-04”公开了一种测速设备及测速系统，原理如下：前端采集设备采集第一监控区域的第一图像信息，识别第一图像信息中的第一车牌信息；测速设备接收第一车牌信息及第一图像信息，获取第一车牌信息的接收时间，并采集第二监控区域的第二图像信息，识别第二图像信息中的第二车牌信息，获取第二车牌信息的获得时间，若第二车牌信息与第一车牌信息相同，根据接收时间与获得时间的时间差及前端采集设备与测速设备的距离计算第二车牌信息对应车辆的车速，若车速超过设定车速，将超速车辆的图像信息、车牌信息及车速发送至监控终端。文献“K·A·奥德亚、J·里尤、F·纳迪、H·谭，GPS增强的车辆速度估计，申请号：CN201010548781.X，申请日：2010-11-12”公开了一种使用单天线全球定位系统的车辆估计车辆速度的方法。使用所述单天线GPS测量所述车辆的绝对速度和航向角。所述绝对速度和所述侧滑角的函数来确定所述车辆速度。将所述车辆速度提供给车辆动态控制应用。用惯性传感器的积分来进行速度估计的误差随着时间的推移而增加，用定位结果来进行差分运算得到的速度受到定位传感器精度的影响，已有的技术文献没有解决这些问题。
发明内容
[0003] 为解决上述问题，本发明公开了一种测速方法，包括以下步骤：
[0004] 第1步、初始化：设定采样点t＝0，设置加速度计与定位传感器的采样时间为T，设置陀螺仪的采样时间为N·T，其中N为正整数；根据实际情况确定权重系数wi,i＝1,2,3,4；
[0005] 第2步、采集数据：令t←t+1，从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t；从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt，其中 为在采样点t的东向坐标的定位结果， 为在采样点t的北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
[0006]
[0007] 其中， 表示以 为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标；
[0008] 第3步、计算速度观测值及其误差包络矩阵：利用第2步获取的 与Rt计算在采样点t的速度观测值，如下：
[0009]
[0010] 其中， 为2×1的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算 误差的包络矩阵 如下：
[0011]
[0012] 其中
[0013]
[0014] 其中，tr表示矩阵的迹；
[0015] 第4步、计算速度观测值误差系数：利用第2步获取的 计算在采样点t的速度观测值误差系数λt，如下：
[0016]
[0017] 其中，wi为权重系数，符合 μi具体如下：
[0018]
[0019]
[0020] 其中， 为示性函数，当括号内的公式成立时返回1，否则返回0；
[0021] 第5步、计算速度估计值：利用第2步获取的ax,t、ay,t，第3步获取的 第4步获取的λt，计算在时间t的速度估计值，如下：
[0022] 第5.1步、进行先验估计，获取在时间t的状态先验估计值 与先验估计误差包络矩阵 如下：
[0023]
[0024]
[0025] 其中，Qt为在采样点t的过程噪声包络矩阵，是一个四维对角阵， 为在采样点t-1的后验估计误差包络矩阵，
[0026]
[0027]
[0028]
[0029] 其中，blockdiag表示块对角阵， 表示在采样点t的东向、北向的移动速度的先验估计， 表示在采样点t的加速度计在东向、北向的漂移的先验估计；
[0030] 第5.2步、进行后验估计，获取被测对象在采样点t的增益Kt、后验状态估计值 与后验估计误差包络矩阵 如下：
[0031]
[0032]
[0033]
[0034] 其中，I4为四维单位阵，C＝[1,0,1,0]，
[0035]
[0036]
[0037] 其中，maxsvd表示最大奇异值， 表示被测对象在采样点t的东向、北向的移动速度的后验估计， 表示在采样点t的加速度计在东向、北向的漂移的后验估计；
[0038] 第6步、重复第2步至第5步，输出每个采样点的 其中第1、3个元素即为东向、北向的速度估计值。
[0039] 相比于已有技术，本发明具有如下优点：消除了基于惯性传感器积分的速度估计的累计误差；比单独使用定位系统进行差分运算得到的速度推算值更为准确，可靠性更高；通过对陀螺仪输出序列的分析，能够获取当前车况，进而获取速度观测值误差系数，实现了速度估计过程中增益的自动调整，进一步提升了估计的准确性。
[0040] 本发明还公开了一种测速装置，包括：
[0041] 加速度计：用以获取被测对象东向、北向的加速度；
[0042] 陀螺仪：用以获取被测对象的偏航速率；
[0043] 定位传感器：用于获取被测对象的位置坐标；
[0044] 数据处理单元：用于执行速度估计程序，以获取被测对象的速度，所述速度估计程序在执行时实现上述第1步至第6步。
附图说明
[0045] 图1为本发明流程图；
[0046] 图2为东向速度实验结果图；
[0047] 图3为北向速度实验结果图。
具体实施方式
[0048] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。
[0049] 如图1所示，本发明公开了一种测速方法，步骤如下：
[0050] 第1步、初始化：设定采样点t＝0，设置加速度计与定位传感器的采样时间为T，设置陀螺仪的采样时间为N·T，其中N为正整数；根据实际情况确定权重系数wi,i＝1,2,3,4；
[0051] 第2步、采集数据：令t←t+1，从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t；从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt，其中 为在采样点t的东向坐标的定位结果， 为在采样点t的北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
[0052]
[0053] 其中， 表示以 为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标；
[0054] 第3步、计算速度观测值及其误差包络矩阵：利用第2步获取的 与Rt计算在采样点t的速度观测值，如下：
[0055]
[0056] 其中， 为2×1的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算 误差的包络矩阵 如下：
[0057]
[0058] 其中
[0059]
[0060] 其中，tr表示矩阵的迹；
[0061] 第4步、计算速度观测值误差系数：利用第2步获取的 计算在采样点t的速度观测值误差系数λt，如下：
[0062]
[0063] 其中，wi为权重系数，符合 μi具体如下：
[0064]
[0065]
[0066] 其中， 为示性函数，当括号内的公式成立时返回1，否则返回0；
[0067] 第5步、计算速度估计值：利用第2步获取的ax,t、ay,t，第3步获取的 第4步获取的λt，计算在时间t的速度估计值，如下：
[0068] 第5.1步、进行先验估计，获取在时间t的状态先验估计值 与先验估计误差包络矩阵 如下：
[0069]
[0070]
[0071] 其中，Qt为在采样点t的过程噪声包络矩阵，是一个四维对角阵， 为在采样点t-1的后验估计误差包络矩阵，
[0072]
[0073]
[0074]
[0075] 其中，blockdiag表示块对角阵， 表示在采样点t的东向、北向的移动速度的先验估计， 表示在采样点t的加速度计在东向、北向的漂移的先验估计；
[0076] 第5.2步、进行后验估计，获取被测对象在采样点t的增益Kt、后验状态估计值 与后验估计误差包络矩阵 如下：
[0077]
[0078]
[0079]
[0080] 其中，I4为四维单位阵，C＝[1,0,1,0]，
[0081]
[0082]
[0083] 其中，maxsvd表示最大奇异值， 表示被测对象在采样点t的东向、北向的移动速度的后验估计， 表示在采样点t的加速度计在东向、北向的漂移的后验估计；
[0084] 第6步、重复第2步至第5步，输出每个采样点的 其中第1、3个元素即为东向、北向的速度估计值。
[0085] 本发明还公开了一种测速装置，包括：
[0086] 加速度计：用以获取被测对象东向、北向的加速度；
[0087] 陀螺仪：用以获取被测对象的偏航速率；
[0088] 定位传感器：用于获取被测对象的位置坐标；
[0089] 数据处理单元：用于执行速度估计程序，以获取被测对象的速度，所述速度估计程序在执行时实现以下步骤：
[0090] 第1步、初始化：设定采样点t＝0，设置加速度计与定位传感器的采样时间为T，设置陀螺仪的采样时间为N·T，其中N为正整数；根据实际情况确定权重系数wi,i＝1,2,3,4；
[0091] 第2步、采集数据：令t←t+1，从加速度计中获取在采样点t的被测对象东向、北向的加速度ax,t与ay,t；从陀螺仪中获取从采样点t-1至t之间的被测对象的偏航速率序列从定位传感器中获取在采样点t的被测对象的位置坐标与相应的误差包络矩阵Rt，其中 为在采样点t的东向坐标的定位结果， 为在采样点t的北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
[0092]
[0093] 其中， 表示以 为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标；
[0094] 第3步、计算速度观测值及其误差包络矩阵：利用第2步获取的 与Rt计算在采样点t的速度观测值，如下：
[0095]
[0096] 其中，为2×1的向量，第一个元素表示计算的东向移动速度的观测值，第二个元素表示计算的北向移动速度的观测值；计算 误差的包络矩阵 如下：
[0097]
[0098] 其中
[0099]
[0100] 其中，tr表示矩阵的迹；
[0101] 第4步、计算速度观测值误差系数：利用第2步获取的 计算在采样点t的速度观测值误差系数λt，如下：
[0102]
[0103] 其中，wi为权重系数，符合 μi具体如下：
[0104]
[0105]
[0106] 其中， 为示性函数，当括号内的公式成立时返回1，否则返回0；
[0107] 第5步、计算速度估计值：利用第2步获取的ax,t、ay,t，第3步获取的 第4步获取的λt，计算在时间t的速度估计值，如下：
[0108] 第5.1步、进行先验估计，获取在时间t的状态先验估计值 与先验估计误差包络矩阵 如下：
[0109]
[0110]
[0111] 其中，Qt为在采样点t的过程噪声包络矩阵，是一个四维对角阵， 为在采样点t-1的后验估计误差包络矩阵，
[0112]
[0113]
[0114]
[0115] 其中，blockdiag表示块对角阵， 表示在采样点t的东向、北向的移动速度的先验估计， 表示在采样点t的加速度计在东向、北向的漂移的先验估计；
[0116] 第5.2步、进行后验估计，获取被测对象在采样点t的增益Kt、后验状态估计值 与后验估计误差包络矩阵 如下：
[0117]
[0118]
[0119]
[0120] 其中，I4为四维单位阵，C＝[1,0,1,0]，
[0121]
[0122]
[0123] 其中，maxsvd表示最大奇异值， 表示被测对象在采样点t的东向、北向的移动速度的后验估计， 表示在采样点t的加速度计在东向、北向的漂移的后验估计；
[0124] 第6步、重复第2步至第5步，输出每个采样点的 其中第1、3个元素即为东向、北向的速度估计值。
[0125] 在上述方法与装置中，在初始化过程中需要确定权重系数wi,i＝1,2,3,4，可以采用如下实验法：首先让被测对象，例如一辆车，进行各种形式的运动，包括直线、弯道、频繁变换车道、急变速等；在运动过程中采集传感器的数据，进而获取大量的(μ1,μ2,μ3,μ4)样本，然后用λt进行标记；最后用这些样例集合进行训练即可以获取权重系数wi,i＝1,2,3,4。其中，训练样例中的λt可以采用如下方式获取：实验获取的速度观测误差包络矩阵的对角线元素之和除以 的对角线元素之和。
[0126] 图2为东向速度实验结果图，图3为北向速度实验结果图。从中可见本发明公开的一种测速方法与装置具有更高的精度。
[0127] 提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。
