一种目标速度自适应估计方法与流程

本发明涉及信号处理技术，特别是涉及一种目标速度自适应估计方法。

背景技术

在很多应用场合中，需要实时地获取目标的运动速度，即其相对于东、北方向的速度分量值。一种常用的手段是利用安装在目标上的速度检测器来获取速度。对于地面移动机器人，如文献“杜颖财,王希军,王树洁,卢新然,梁立辉.增量式编码器自动检测系统[j].电子测量与仪器学报,2012,26(11):993-998”所示，我们可以采用编码器检测电机的转速，进而推算出机器人的相对于各个方向的移动速度。对于船舶、航空器等无轮运动物体，如文献“黄胜初、胡阳，一种无水阻船速测量仪，申请号：02138887.3”所示，可以采用加速度计进行积分得到速度。由于加速度计存在一定的误差，所以其积分将误差放大，因此其速度估计值也会随时间推移而逐渐偏移其真实值。在文献“陈凤祥、俞林炯、周苏、章桐，一种无刷直流电机船只的船速测量方法及装置，申请号：201210097040.3”中提出了一种利用船只推进电机转速推算船速的方法，该方法容易受到水域环境的影响，对环境变化的鲁棒性较弱。在文献“王争荣,翟性泉,赵珍强.船舶相对运动速度检测方法研究[j].机电设备,2011,28(05):26-28.”中公开了一种船舶相对运动速度检测方法，该方案通过分析光电编码器检测法和激光测距检测法的优缺点，提出了基于对射型超声波传感器的船舶相对运动速度检测新方法，并且为提高该方法的检测精度，特别设计了相对时差测速方案。

以上方法利用了安装在目标上的内部传感器来检测或者估计移动速度，在某些场合下，我们只能通过外部传感器来获取目标的移动速度，例如：利用摄像头获取行人的移动速度。在文献“李中余、武俊杰、孙稚超、杨海光、黄钰林、杨建宇，双基前视sar动目标成像方法与动目标速度估计方法，申请号：201410136027.3”中公开了一种基于失配压缩的双基前视sar动目标成像方法与动目标速度估计方法，能够完成动目标速度的估计，得到距离向运动速度与方位向运动速度。在文献“何宇桐、赵山林，一种基于监控设备的目标物速度计算方法，申请号：201611078538.x”中，公开了一种基于监控设备的目标物速度计算方法，用于根据安装于行驶车辆上的监控设备所拍摄的视频图像计算目标物的速度，通过获取目标物与行驶车辆的相对位置，然后根据目标物与行驶车辆的相对位置的变化情况，最终计算得到目标物的行驶速度。在文献“胡水,李德敏,裴仁林,吴施恩.移动用户速度估计与位置区的优化设计[j].微计算机信息,2005(20):71-72+192.”公布了一种采用高斯-马尔可夫模型作为移动模型，通过数据采集和参数估计来实时预测移动用户的速度。

利用定位结果来进行差分运算是一种常用的速度估计方法，由于定位系统存在一定误差，且对速度估计会产生较大的影响，所以需要实时地调整速度估计的参数，以控制速度估计误差的上限。然而，已有的技术文献没有考虑这个问题。

技术实现要素：

为解决上述问题，本发明公开了一种目标速度自适应估计方法，包括以下步骤：

第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间t与判定阈值μ；

第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果与相应的误差包络矩阵rt，其中符号′表示转置，为东向坐标的定位结果，为北向坐标的定位结果，rt为一个2维的对角阵，并且满足

其中，表示以为中心，以rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；

第3步、根据第2步获取的与rt，推算移动速度，具体如下

3.1设定时延系数τ为1，集合zt为空；

3.2推算在采样点t的移动速度如下

其中，为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，t表示采样时间；

3.3计算误差的包络矩阵

其中

其中，tr表示矩阵的迹；

3.4计算相对误差的最大值其中

其中，qt(τ)^(i,i)表示qt(τ)对角线第i个元素，表示的第i个元素；

3.4如果满足

其中μ为判定阈值，则停止运算，输出为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入zt；

3.5如果zt的元素个数等于t-1，则停止运算，输出为最终推算的速度，其中

其中，l(·)表示二范数；否则，转入步骤3.2；

第4步、重复第2步至第3步，输出每个采样点的速度估计值。

本发明与已有的技术相比，具有以下优点：利用全球定位系统推算移动速度时主动地考虑每个时刻获取的定位误差，进而推算出由两个不同时刻定位结果差分运算得到的移动速度的误差，实现了速度估计的精度实时评价；通过自适应地调整时延系数，实现了移动速度估计的误差上界控制，或者上界最小化。本发明可以应用于陆地车辆、野外机器人、水下机器人、船舶、飞行器以及行人的移动速度估计。

附图说明

图1为本发明流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。

对于一个在二维平面运动的目标，例如机器人、车辆、船舶、行人等等，其在采样点t的坐标可以用θt＝(xt,yt)′表示，为了获取他们的移动速度而又缺乏速度检测器的情况下，可以从定位结果中推算得到，如下

其中，t为采样时间，τ为时延系数，分别表示东向、北向在采样点t的平均移动速度。也可以整合为一个等式

其中，

然而，定位系统存在一定的偏差。令表示定位结果，表示速度推算结果，可以得到观测方程

假设

其中，ε(θt,rt)表示以θt为中心，以rt为包络矩阵的椭球。

因此，可得

其中，qt(τ)为速度推算结果的误差包络矩阵。

由上可见，椭球qt(τ))为与的直减运算的外包椭球。为了得到迹最小化的外包椭球，qt(τ)可以通过以下运算得到

进而可知

其中，表示中第i个元素，qt(τ)^(i,i)表示qt(τ)中对角线第i个元素。因此，可得东向、北向的推算速度的相对误差的最大值

显然，当较小时，往往较大，因此为了控制ζt(τ)的上限，即

ζt(τ)≤[μ,μ]′,(15)

其中μ为设定的阈值，我们需要动态地调整τ，以满足这个条件；当这个条件无法满足时，要保证ζt(τ)尽可能小。具体方法如下：

1.设定τ为1，集合zt为空；

2.计算以及其相应的ζt(τ)，如果满足

ζt(τ)≤[μ,μ]′,(16)

则停止运算，输出为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入zt；

3.如果zt的元素个数等于t-1，则停止运算，输出为最终推算的速度，其中

其中，l(·)表示二范数；否则，转入步骤2。

综上所述，我们提出了一种目标速度自适应估计方法，如图1所示，包含以下步骤：

第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间t与判定阈值μ；

第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果与相应的误差包络矩阵rt，其中符号′表示转置，为东向坐标的定位结果，为北向坐标的定位结果，rt为一个2维的对角阵，并且满足

其中，表示以为中心，以rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；

第3步、根据第2步获取的与rt，推算移动速度，具体如下

3.1设定时延系数τ为1，集合zt为空；

3.2推算在采样点t的移动速度如下

其中，为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，t表示采样时间；

3.3计算误差的包络矩阵

其中

其中，tr表示矩阵的迹；

3.4计算相对误差的最大值其中

其中，qt(τ)^(i,i)表示qt(τ)对角线第i个元素，表示的第i个元素；

3.4如果满足

其中μ为判定阈值，则停止运算，输出为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入zt；

3.5如果zt的元素个数等于t-1，则停止运算，输出为最终推算的速度，其中

其中，l(·)表示二范数；否则，转入步骤3.2；

第4步、重复第2步至第3步，输出每个采样点的速度估计值。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。