[0033] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。
[0034] 对于一个在二维平面运动的目标，例如机器人、车辆、船舶、行人等等，其在采样点t的坐标可以用θt＝(xt,yt)′表示，为了获取他们的移动速度而又缺乏速度检测器的情况下，可以从定位结果中推算得到，如下
[0035]
[0036]
[0037] 其中，T为采样时间，τ为时延系数， 分别表示东向、北向在采样点t的平均移动速度。也可以整合为一个等式
[0038]
[0039] 其中，
[0040] 然而，定位系统存在一定的偏差。令 表示定位结果， 表示速度推算结果，可以得到观测方程
[0041]
[0042] 假设
[0043]
[0044] 其中，ε (θt,Rt)表示以θt为中心，以Rt为包络矩阵的椭球。
[0045] 因此，可得
[0046]
[0047]
[0048]
[0049] 其中，Qt(τ)为速度推算结果的误差包络矩阵。
[0050] 由上可见，椭球 为 与 的直减运算的外包椭球。为了得到迹最小化的外包椭球，Qt(τ)可以通过以下运算得到
[0051]
[0052]
[0053] 进而可知
[0054]
[0055]
[0056] 其中， 表示 中第i个元素，Qt(τ)(i,i)表示Qt(τ)中对角线第i个元素。因此，可得东向、北向的推算速度的相对误差的最大值
[0057]
[0058]
[0059] 显然，当 较小时， 往往较大，因此为了控制ζt(τ)的上限，即
[0060] ζt(τ)≤[μ,μ]′, (15)
[0061] 其中μ为设定的阈值，我们需要动态地调整τ，以满足这个条件；当这个条件无法满足时，要保证ζt(τ)尽可能小。具体方法如下：
[0062] 1.设定τ为1，集合Zt为空；
[0063] 2.计算 以及其相应的ζt(τ)，如果满足
[0064] ζt(τ)≤[μ,μ]′, (16)
[0065] 则停止运算，输出 为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
[0066] 3.如果Zt的元素个数等于t-1，则停止运算，输出 为最终推算的速度，其中[0067]
[0068] 其中，L(·)表示二范数；否则，转入步骤2。
[0069] 综上所述，我们提出了一种目标速度自适应估计方法，如图1所示，包含以下步骤：
[0070] 第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间T与判定阈值μ；
[0071] 第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果 与相应的误差包络矩阵Rt，其中符号′表示转置， 为东向坐标的定位结果，为北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
[0072]
[0073] 其中， 表示以 为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；
[0074] 第3步、根据第2步获取的 与Rt，推算移动速度，具体如下
[0075] 3.1设定时延系数τ为1，集合Zt为空；
[0076] 3.2推算在采样点t的移动速度 如下
[0077]
[0078] 其中， 为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，T表示采样时间；
[0079] 3.3计算 误差的包络矩阵
[0080]
[0081] 其中
[0082]
[0083] 其中，tr表示矩阵的迹；
[0084] 3.4计算 相对误差的最大值 其中
[0085]
[0086]
[0087] 其中，Qt(τ)(i,i)表示Qt(τ)对角线第i个元素， 表示 的第i个元素；
[0088] 3.4如果满足
[0089]
[0090] 其中μ为判定阈值，则停止运算，输出 为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
[0091] 3.5如果Zt的元素个数等于t-1，则停止运算，输出 为最终推算的速度，其中[0092]
[0093] 其中，L(·)表示二范数；否则，转入步骤3.2；
[0094] 第4步、重复第2步至第3步，输出每个采样点的速度估计值。
[0095] 提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。
