CN109085376B - 一种目标速度自适应估计方法 - Google Patents

一种目标速度自适应估计方法 Download PDF

Info

Publication number
CN109085376B
CN109085376B CN201810946930.4A CN201810946930A CN109085376B CN 109085376 B CN109085376 B CN 109085376B CN 201810946930 A CN201810946930 A CN 201810946930A CN 109085376 B CN109085376 B CN 109085376B
Authority
CN
China
Prior art keywords
speed
positioning
coordinates
moving speed
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810946930.4A
Other languages
English (en)
Other versions
CN109085376A (zh
Inventor
李泽琦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dongyang Weichuang industrial product design Co., Ltd
Original Assignee
Dongyang Weichuang Industrial Product Design Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dongyang Weichuang Industrial Product Design Co Ltd filed Critical Dongyang Weichuang Industrial Product Design Co Ltd
Priority to CN201810946930.4A priority Critical patent/CN109085376B/zh
Publication of CN109085376A publication Critical patent/CN109085376A/zh
Application granted granted Critical
Publication of CN109085376B publication Critical patent/CN109085376B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/64—Devices characterised by the determination of the time taken to traverse a fixed distance

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

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

Description

一种目标速度自适应估计方法
技术领域
本发明涉及信号处理技术，特别是涉及一种目标速度自适应估计方法。
背景技术
在很多应用场合中，需要实时地获取目标的运动速度，即其相对于东、北方向的速度分量值。一种常用的手段是利用安装在目标上的速度检测器来获取速度。对于地面移动机器人，如文献“杜颖财,王希军,王树洁,卢新然,梁立辉.增量式编码器自动检测***[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的定位结果
Figure GDA0002501261890000021
与相应的误差包络矩阵Rt，其中符号′表示转置，
Figure GDA0002501261890000022
为东向坐标的定位结果，
Figure GDA0002501261890000023
为北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
Figure GDA0002501261890000024
其中，
Figure GDA0002501261890000025
表示以
Figure GDA0002501261890000026
为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；
第3步、根据第2步获取的
Figure GDA0002501261890000027
与Rt，推算移动速度，具体如下
3.1设定时延系数τ为1，集合Zt为空；
3.2推算在采样点t的移动速度
Figure GDA0002501261890000028
如下
Figure GDA0002501261890000029
其中，
Figure GDA00025012618900000210
为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，T表示采样时间；
3.3计算
Figure GDA00025012618900000211
误差的包络矩阵
Figure GDA00025012618900000212
其中
Figure GDA00025012618900000213
其中，tr表示矩阵的迹；
3.4计算
Figure GDA00025012618900000214
相对误差的最大值
Figure GDA00025012618900000215
其中
Figure GDA00025012618900000216
Figure GDA00025012618900000217
其中，Qt(τ)(i,i)表示Qt(τ)对角线第i个元素，
Figure GDA00025012618900000218
表示
Figure GDA00025012618900000219
的第i个元素；
3.4如果满足
Figure GDA00025012618900000220
其中μ为判定阈值，则停止运算，输出
Figure GDA00025012618900000221
为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
3.5如果Zt的元素个数等于t-1，则停止运算，输出
Figure GDA0002501261890000031
为最终推算的速度，其中
Figure GDA0002501261890000032
其中，L(·)表示二范数；否则，转入步骤3.2；
第4步、重复第2步至第3步，输出每个采样点的速度估计值。
本发明与已有的技术相比，具有以下优点：利用全球定位***推算移动速度时主动地考虑每个时刻获取的定位误差，进而推算出由两个不同时刻定位结果差分运算得到的移动速度的误差，实现了速度估计的精度实时评价；通过自适应地调整时延系数，实现了移动速度估计的误差上界控制，或者上界最小化。本发明可以应用于陆地车辆、野外机器人、水下机器人、船舶、飞行器以及行人的移动速度估计。
附图说明
图1为本发明流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。
对于一个在二维平面运动的目标，例如机器人、车辆、船舶、行人等等，其在采样点t的坐标可以用θt＝(xt,yt)′表示，为了获取他们的移动速度而又缺乏速度检测器的情况下，可以从定位结果中推算得到，如下
Figure GDA0002501261890000033
Figure GDA0002501261890000034
其中，T为采样时间，τ为时延系数，
Figure GDA0002501261890000035
分别表示东向、北向在采样点t的平均移动速度。也可以整合为一个等式
Figure GDA0002501261890000036
其中，
Figure GDA0002501261890000037
然而，定位***存在一定的偏差。令
Figure GDA0002501261890000038
表示定位结果，
Figure GDA0002501261890000039
表示速度推算结果，可以得到观测方程
Figure GDA00025012618900000310
假设
Figure GDA00025012618900000311
其中，ε (θt,Rt)表示以θt为中心，以Rt为包络矩阵的椭球。
因此，可得
Figure GDA0002501261890000041
Figure GDA0002501261890000042
Figure GDA0002501261890000043
其中，Qt(τ)为速度推算结果的误差包络矩阵。
由上可见，椭球
Figure GDA0002501261890000044
为
Figure GDA0002501261890000045
与
Figure GDA0002501261890000046
的直减运算的外包椭球。为了得到迹最小化的外包椭球，Qt(τ)可以通过以下运算得到
Figure GDA0002501261890000047
Figure GDA0002501261890000048
进而可知
Figure GDA0002501261890000049
Figure GDA00025012618900000410
其中，
Figure GDA00025012618900000411
表示
Figure GDA00025012618900000412
中第i个元素，Qt(τ)(i,i)表示Qt(τ)中对角线第i个元素。因此，可得东向、北向的推算速度的相对误差的最大值
Figure GDA00025012618900000413
Figure GDA00025012618900000414
显然，当
Figure GDA00025012618900000415
较小时，
Figure GDA00025012618900000416
往往较大，因此为了控制ζt(τ)的上限，即
ζt(τ)≤[μ,μ]′, (15)
其中μ为设定的阈值，我们需要动态地调整τ，以满足这个条件；当这个条件无法满足时，要保证ζt(τ)尽可能小。具体方法如下：
1.设定τ为1，集合Zt为空；
2.计算
Figure GDA00025012618900000417
以及其相应的ζt(τ)，如果满足
ζt(τ)≤[μ,μ]′, (16)
则停止运算，输出
Figure GDA00025012618900000418
为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
3.如果Zt的元素个数等于t-1，则停止运算，输出
Figure GDA0002501261890000051
为最终推算的速度，其中
Figure GDA0002501261890000052
其中，L(·)表示二范数；否则，转入步骤2。
综上所述，我们提出了一种目标速度自适应估计方法，如图1所示，包含以下步骤：
第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间T与判定阈值μ；
第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果
Figure GDA0002501261890000053
与相应的误差包络矩阵Rt，其中符号′表示转置，
Figure GDA0002501261890000054
为东向坐标的定位结果，
Figure GDA0002501261890000055
为北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
Figure GDA0002501261890000056
其中，
Figure GDA0002501261890000057
表示以
Figure GDA0002501261890000058
为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；
第3步、根据第2步获取的
Figure GDA0002501261890000059
与Rt，推算移动速度，具体如下
3.1设定时延系数τ为1，集合Zt为空；
3.2推算在采样点t的移动速度
Figure GDA00025012618900000510
如下
Figure GDA00025012618900000511
其中，
Figure GDA00025012618900000512
为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，T表示采样时间；
3.3计算
Figure GDA00025012618900000513
误差的包络矩阵
Figure GDA00025012618900000514
其中
Figure GDA00025012618900000515
其中，tr表示矩阵的迹；
3.4计算
Figure GDA00025012618900000516
相对误差的最大值
Figure GDA00025012618900000517
其中
Figure GDA00025012618900000518
Figure GDA00025012618900000519
其中，Qt(τ)(i,i)表示Qt(τ)对角线第i个元素，
Figure GDA00025012618900000520
表示
Figure GDA00025012618900000521
的第i个元素；
3.4如果满足
Figure GDA0002501261890000061
其中μ为判定阈值，则停止运算，输出
Figure GDA0002501261890000062
为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
3.5如果Zt的元素个数等于t-1，则停止运算，输出
Figure GDA0002501261890000063
为最终推算的速度，其中
Figure GDA0002501261890000064
其中，L(·)表示二范数；否则，转入步骤3.2；
第4步、重复第2步至第3步，输出每个采样点的速度估计值。
提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (1)

1.一种目标速度自适应估计方法，其特征在于包含以下步骤：
第1步、初始化：设定采样点t＝0，根据实际情况确定采样时间T与判定阈值μ；
第2步、采样点自增1，然后从定位设备读取在采样点t的定位结果
Figure FDA0002501261880000011
与相应的误差包络矩阵Rt，其中符号′表示转置，
Figure FDA0002501261880000012
为东向坐标的定位结果，
Figure FDA0002501261880000013
为北向坐标的定位结果，Rt为一个2维的对角阵，并且满足
Figure FDA0002501261880000014
其中，
Figure FDA0002501261880000015
表示以
Figure FDA0002501261880000016
为中心，以Rt为包络矩阵的椭球，θt＝(xt,yt)′表示真实的位置坐标，xt表示真实的东向坐标，yt表示真实的北向坐标；
第3步、根据第2步获取的
Figure FDA0002501261880000017
与Rt，推算移动速度，具体如下
3.1设定时延系数τ为1，集合Zt为空；
3.2推算在采样点t的移动速度
Figure FDA0002501261880000018
如下
Figure FDA0002501261880000019
其中，
Figure FDA00025012618800000110
为2×1的向量，第一个元素表示推算的东向移动速度，第二个元素表示推算的北向移动速度，T表示采样时间；
3.3计算
Figure FDA00025012618800000111
误差的包络矩阵
Figure FDA00025012618800000112
其中
Figure FDA00025012618800000113
其中，tr表示矩阵的迹；
3.4计算
Figure FDA00025012618800000114
相对误差的最大值
Figure FDA00025012618800000115
其中
Figure FDA00025012618800000116
Figure FDA00025012618800000117
其中，Qt(τ)(i,i)表示Qt(τ)对角线第i个元素，
Figure FDA00025012618800000118
表示
Figure FDA00025012618800000119
的第i个元素；
3.4如果满足
Figure FDA00025012618800000120
其中μ为判定阈值，则停止运算，输出
Figure FDA00025012618800000121
为最终推算的速度；否则，令τ自增1，并将ζt(τ)存入Zt；
3.5如果Zt的元素个数等于t-1，则停止运算，输出
Figure FDA00025012618800000122
为最终推算的速度，其中
Figure FDA00025012618800000123
其中，L(·)表示二范数；否则，转入步骤3.2；
第4步、重复第2步至第3步，输出每个采样点的速度估计值。
CN201810946930.4A 2018-08-20 2018-08-20 一种目标速度自适应估计方法 Active CN109085376B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810946930.4A CN109085376B (zh) 2018-08-20 2018-08-20 一种目标速度自适应估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810946930.4A CN109085376B (zh) 2018-08-20 2018-08-20 一种目标速度自适应估计方法

Publications (2)

Publication Number Publication Date
CN109085376A CN109085376A (zh) 2018-12-25
CN109085376B true CN109085376B (zh) 2020-09-18

Family

ID=64794032

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810946930.4A Active CN109085376B (zh) 2018-08-20 2018-08-20 一种目标速度自适应估计方法

Country Status (1)

Country Link
CN (1) CN109085376B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101661048A (zh) * 2008-08-29 2010-03-03 索尼株式会社 速度计算装置、速度计算方法和导航装置
CN102298070A (zh) * 2010-06-22 2011-12-28 鹦鹉股份有限公司 估算无人机，尤其是能够在自动驾驶下执行悬停飞行的无人机的水平速度的方法
CN102353802A (zh) * 2011-07-01 2012-02-15 哈尔滨工程大学 一种基于全加速度计的运动载体角速度测量方法
CN103900613A (zh) * 2014-03-31 2014-07-02 哈尔滨工程大学 一种基于磁力计n阶距检测的mems***误差估计方法
CN204631065U (zh) * 2015-05-26 2015-09-09 重庆大学 一种行车速度及加速度测量装置
KR101813698B1 (ko) * 2016-12-21 2018-01-30 한국항공대학교산학협력단 공중 로봇의 이동 속도 추정 장치 및 방법
CN107991110A (zh) * 2017-11-29 2018-05-04 安徽省通信息科技有限公司 一种履带式机器人滑动参数检测方法
CN108020855A (zh) * 2017-11-29 2018-05-11 安徽省通信息科技有限公司 一种滑移转向机器人的位姿和转动瞬心联合估计方法
CN108036789A (zh) * 2017-11-29 2018-05-15 安徽省通信息科技有限公司 一种野外机器人航迹推算方法
CN108121941A (zh) * 2016-11-30 2018-06-05 上海联合道路交通安全科学研究中心 一种基于监控设备的目标物速度计算方法
CN108362282A (zh) * 2018-01-29 2018-08-03 哈尔滨工程大学 一种基于自适应零速区间调整的惯性行人定位方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101661048A (zh) * 2008-08-29 2010-03-03 索尼株式会社 速度计算装置、速度计算方法和导航装置
CN102298070A (zh) * 2010-06-22 2011-12-28 鹦鹉股份有限公司 估算无人机，尤其是能够在自动驾驶下执行悬停飞行的无人机的水平速度的方法
CN102353802A (zh) * 2011-07-01 2012-02-15 哈尔滨工程大学 一种基于全加速度计的运动载体角速度测量方法
CN103900613A (zh) * 2014-03-31 2014-07-02 哈尔滨工程大学 一种基于磁力计n阶距检测的mems***误差估计方法
CN204631065U (zh) * 2015-05-26 2015-09-09 重庆大学 一种行车速度及加速度测量装置
CN108121941A (zh) * 2016-11-30 2018-06-05 上海联合道路交通安全科学研究中心 一种基于监控设备的目标物速度计算方法
KR101813698B1 (ko) * 2016-12-21 2018-01-30 한국항공대학교산학협력단 공중 로봇의 이동 속도 추정 장치 및 방법
CN107991110A (zh) * 2017-11-29 2018-05-04 安徽省通信息科技有限公司 一种履带式机器人滑动参数检测方法
CN108020855A (zh) * 2017-11-29 2018-05-11 安徽省通信息科技有限公司 一种滑移转向机器人的位姿和转动瞬心联合估计方法
CN108036789A (zh) * 2017-11-29 2018-05-15 安徽省通信息科技有限公司 一种野外机器人航迹推算方法
CN108362282A (zh) * 2018-01-29 2018-08-03 哈尔滨工程大学 一种基于自适应零速区间调整的惯性行人定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"膛内高速运动目标的速度测量技术研究";杨梦玲;《中国优秀硕士学位论文全文数据库》;20171231;全文 *
"频率步进雷达运动扩展目标速度估计与距离像抽取研究";聂丹;《中国优秀硕士学位论文全文数据库》;20101231;全文 *

Also Published As

Publication number Publication date
CN109085376A (zh) 2018-12-25

Similar Documents

Publication Publication Date Title
US11062475B2 (en) Location estimating apparatus and method, learning apparatus and method, and computer program products
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
CN109239709B (zh) 一种无人船的局部环境地图自主构建方法
Franchi et al. A forward-looking SONAR and dynamic model-based AUV navigation strategy: Preliminary validation with FeelHippo AUV
Ma et al. Radar image-based positioning for USV under GPS denial environment
CN110531357A (zh) 估计移动目标在水平面中速度大小的方法和雷达检测***
US20210213962A1 (en) Method for Determining Position Data and/or Motion Data of a Vehicle
KR101890612B1 (ko) 적응적 관심영역 및 탐색창을 이용한 객체 검출 방법 및 장치
CN109579850B (zh) 基于对水速度辅助惯导的深水智能导航方法
CN111402328B (zh) 一种基于激光里程计的位姿计算方法及装置
Han et al. GPS-less coastal navigation using marine radar for USV operation
Tan et al. Drag coefficient estimation model to simulate dynamic control of autonomous underwater vehicle (AUV) motion
CN109085376B (zh) 一种目标速度自适应估计方法
CN114061565A (zh) 一种无人船舶slam及其应用方法
CN117606491A (zh) 一种自主式水下航行器的组合定位导航方法及装置
CN117630911A (zh) 一种基于Singer模型的无人艇雷达光电融合方法
CN110908404B (zh) 一种基于数据驱动的auv智能观测运动方法
KR101790482B1 (ko) 항해용 레이더를 이용한 파고 측정 시스템 및 측정 방법
CN113091756A (zh) 位置推定装置、以及位置推定方法
Subramanian et al. Shoreline mapping using an omni-directional camera for autonomous surface vehicle applications
McConnell et al. Perception for Underwater Robots
CN111446898B (zh) 基于模糊逻辑和扩张状态观测器的低成本auv速度估计方法
Miller et al. AUV navigation with seabed acoustic sensing
CN107272713A (zh) 一种水下机器人不确定流场环境自适应控制***
Spears et al. Determining underwater vehicle movement from sonar data in relatively featureless seafloor tracking missions

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20200825

Address after: No.2, building 15, Dongfu community, houtang community, Jiangbei street, Dongyang City, Jinhua City, Zhejiang Province

Applicant after: Dongyang Weichuang industrial product design Co., Ltd

Address before: 230601 Room 110, Public Rental Building 1 # Building, West Qinglongtan Road, East Feiguang Road, North Export Processing Zone, Binhe District, Hefei Economic and Technological Development Zone, Anhui Province

Applicant before: HEFEI YOUKONG TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant