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

权利要求书

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个元素即为东向、北向的速度估计值。

2.一种测速装置，其特征在于，包括：

加速度计：用以获取被测对象东向、北向的加速度；

陀螺仪：用以获取被测对象的偏航速率；

定位传感器：用于获取被测对象的位置坐标；

数据处理单元：用于执行速度估计程序，以获取被测对象的速度，所述速度估计程序在执行时实现以下步骤：

第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个元素即为东向、北向的速度估计值。