[发明专利]一种目标速度自适应估计方法

摘要

本发明公开了一种目标速度自适应估计方法，包含初始化、读取定位结果、推算移动速度三个基本步骤；利用全球定位系统推算移动速度时主动地考虑每个时刻获取的定位误差，进而推算出由两个不同时刻定位结果差分运算得到的移动速度的误差，实现了速度估计的精度实时评价；通过自适应地调整时延系数，实现了移动速度估计的误差上界控制，或者上界最小化。本发明可以应用于陆地车辆、野外机器人、水下机器人、船舶、飞行器以及行人的移动速度估计。

主权项

1.一种目标速度自适应估计方法，其特征在于包含以下步骤：第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间T与判定阈值μ；第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果与相应的误差包络矩阵R_t，其中符号′表示转置，为东向坐标的定位结果，为北向坐标的定位结果，R_t为一个2维的对角阵，并且满足其中，表示以为中心，以R_t为包络矩阵的椭球，θ_t＝(x_t,y_t)′表示真实的位置坐标，x_t表示真实的东向坐标，y_t表示真实的北向坐标；第3步、根据第2步获取的与R_t，推算移动速度，具体如下3.1设定时延系数τ为1，集合Zt为空；3.2推算在采样点t的移动速度如下其中，为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，T表示采样时间；3.3计算误差的包络矩阵其中其中，tr表示矩阵的迹；3.4计算相对误差的最大值其中其中，Q_t(τ)^(i,i)表示Q_t(τ)对角线第i个元素，表示的第i个元素；3.4如果满足其中μ为判定阈值，则停止运算，输出为最终推算的速度；否则，令τ自增1，并将ζ_t(τ)存入Z_t；3.5如果Z_t的元素个数等于t‑1，则停止运算，输出为最终推算的速度，其中其中，l(·)表示二范数；否则，转入步骤3.2；第4步、重复第2步至第3步，输出每个采样点的速度估计值。