CN109085376B - Target speed self-adaptive estimation method - Google Patents
Target speed self-adaptive estimation method Download PDFInfo
- 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
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
The invention discloses a target speed self-adaptive estimation method, which comprises three basic steps of initialization, reading a positioning result and calculating the moving speed; the global positioning system is utilized to actively consider the positioning error obtained at each moment when calculating the moving speed, so that the error of the moving speed obtained by the differential operation of the positioning results at two different moments is calculated, and the real-time evaluation of the precision of the speed estimation is realized; by adaptively adjusting the delay coefficient, the upper bound control of the error of the mobile speed estimation is realized, or the upper bound is minimized. The invention can be applied to the estimation of the moving speed of land vehicles, field robots, underwater robots, ships, aircrafts and pedestrians.
Description
Technical Field
The present invention relates to signal processing technology, and in particular, to a target speed adaptive estimation method.
Background
In many applications, it is necessary to obtain the moving speed of the object, i.e. the speed component values thereof relative to the east and north directions, in real time. One common approach is to acquire velocity using a velocity detector mounted on the target. For a ground mobile robot, as shown in the document "duyingcai, wangxiejun, wangsujie, luck, lianglihui" incremental encoder automatic detection system [ J ]. electronic measurement and instrument bulletin, 2012,26(11):993 + 998 ", we can use an encoder to detect the rotation speed of a motor, and further calculate the moving speed of the robot relative to each direction. For non-wheel moving objects such as ships, aircrafts and the like, such as the documents' Huangsheng, Huyang, a non-water resistance ship speed measuring instrument, the application number is as follows: 02138887.3 ", the velocity may be integrated using an accelerometer. Since the accelerometer has some error, its integral amplifies the error, so its velocity estimate will gradually shift from its true value over time. In the document "Chenfengxiang, shu Ling, Zhoosu, octopus, a method and a device for measuring the ship speed of a brushless direct current motor ship, the application number is as follows: 201210097040.3 discloses a method for estimating the speed of a ship by using the rotation speed of a propulsion motor of the ship, which is easily affected by the environment of the water area and has weak robustness to environmental changes. A relative movement speed detection method of a ship is disclosed in the literature ' Wangbang, Dian nature spring, Zhao Zhen Qiang ' research on electromechanical equipment, 2011,28(05):26-28 ', and the scheme provides a new relative movement speed detection method of the ship based on a correlation ultrasonic sensor by analyzing advantages and disadvantages of a photoelectric encoder detection method and a laser ranging detection method, and particularly designs a relative time difference speed measurement scheme for improving the detection precision of the method.
The above method utilizes an internal sensor mounted on the target to detect or estimate the moving speed, and in some cases, we can only obtain the moving speed of the target through an external sensor, for example: and acquiring the moving speed of the pedestrian by using the camera. In the document, "Lizhong, Wujunjie, Sun Yangyao, Yanhai, Huangyulin, Yangjiayu, bistatic forward-looking SAR moving target imaging method and moving target speed estimation method, application number: 201410136027.3 discloses a double-base forward-looking SAR moving target imaging method and a moving target speed estimation method based on mismatch compression, which can complete the estimation of the moving target speed and obtain the distance direction moving speed and the azimuth direction moving speed. In the document "he yutong, zhao shan lin, a target object velocity calculation method based on monitoring equipment, application No.: 201611078538.X "discloses a target speed calculation method based on a monitoring device, which is used for calculating the speed of a target according to a video image shot by the monitoring device installed on a running vehicle, acquiring the relative position of the target and the running vehicle, and finally calculating the running speed of the target according to the change situation of the relative position of the target and the running vehicle. In the document "hu shui, li de min, pai ren lin, wu schen. mobile user velocity estimation and optimization design of location area [ J ] microcomputer information, 2005(20):71-72+ 192", a method is disclosed for predicting the velocity of a mobile user in real time by data acquisition and parameter estimation using a gaussian-markov model as a motion model.
The difference operation using the positioning result is a commonly used speed estimation method, and since a positioning system has a certain error and has a large influence on the speed estimation, the parameters of the speed estimation need to be adjusted in real time to control the upper limit of the speed estimation error. However, the prior art documents do not take this problem into account.
Disclosure of Invention
In order to solve the above problems, the present invention discloses a target speed adaptive estimation method, which comprises the following steps:
step 1, initialization: setting a sampling point T to be 0, and determining sampling time T and a judgment threshold value mu according to actual conditions;
step 2, the sampling point is automatically increased by 1, and then the positioning result at the sampling point t is read from the positioning equipmentWith corresponding error envelope matrix RtWherein the symbol' represents a transpose,as a result of the positioning of the east coordinates,for the positioning result of the north coordinates, RtIs a 2-dimensional diagonal matrix and satisfies
Wherein the content of the first and second substances,is shown inCentered on RtEllipsoid, theta, of envelope matrixt=(xt,yt) ' denotes the true position coordinates, xtRepresenting true east coordinates, ytRepresenting true north coordinates;
3.1 set delay coefficient τ to 1, set ZtIs empty;
Wherein the content of the first and second substances,a vector of 2 × 1, the first element representing the estimated east movement velocity, the second element representing the estimated north movement velocity, and T representing the sample time;
Wherein
Wherein tr represents a trace of the matrix;
Wherein Q ist(τ)(i,i)Represents Qt(τ) diagonal to the ith element,to representThe ith element of (1);
3.4 if satisfied
Wherein mu is a determination threshold, stopping the operation, and outputtingThe final estimated speed; otherwise, let τ increase by 1 and ζt(τ) is stored in Zt;
Wherein, L (·) represents a two-norm; otherwise, turning to step 3.2;
and 4, repeating the steps 2 to 3, and outputting the speed estimation value of each sampling point.
Compared with the prior art, the invention has the following advantages: the global positioning system is utilized to actively consider the positioning error obtained at each moment when calculating the moving speed, so that the error of the moving speed obtained by the differential operation of the positioning results at two different moments is calculated, and the real-time evaluation of the precision of the speed estimation is realized; by adaptively adjusting the delay coefficient, the upper bound control of the error of the mobile speed estimation is realized, or the upper bound is minimized. The invention can be applied to the estimation of the moving speed of land vehicles, field robots, underwater robots, ships, aircrafts and pedestrians.
Drawings
FIG. 1 is a flow chart of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in detail with reference to the accompanying drawings and specific embodiments.
For an object moving in a two-dimensional plane, such as a robot, a vehicle, a ship, a pedestrian, etc., the coordinate of the object at the sampling point t can be represented by thetat=(xt,yt) ' means that in the absence of a speed detector in order to obtain their moving speed, it can be derived from the positioning results as follows
Wherein T is sampling time, tau is time delay coefficient,mean moving speeds of the east and north directions at the sampling point t are respectively represented. Can also be integrated into one equation
however, there is a certain deviation of the positioning system. Order toThe result of the positioning is shown,expressing the speed calculation result to obtain an observation equation
Suppose that
Wherein (theta)t,Rt) Expressed in thetatCentered on RtEllipsoid of the envelope matrix.
Thus, it is possible to obtain
Wherein Q istAnd (tau) is an error envelope matrix of the speed estimation result.
From the top, ellipsoidIs composed ofAndthe direct subtraction of (2) is an outer ellipsoid. To obtain an outer-wrapped ellipsoid of trace minimization, Qt(τ) can be obtained by the following operation
Further, it can be seen that
Wherein the content of the first and second substances,to representThe ith element of (1), Qt(τ)(i,i)Represents QtThe ith element of the diagonal in (τ). Therefore, the maximum value of the relative error of the estimated speed in the east and north directions can be obtained
It is obvious thatWhen the ratio of the water to the oil is small,tends to be large and therefore to control ζtUpper limit of (τ), i.e.
ζt(τ)≤[μ,μ]′, (15)
Where μ is a set threshold, we need to dynamically adjust τ to meet this condition; when this condition cannot be satisfied, ζ is guaranteedt(τ) is as small as possible. The specific method comprises the following steps:
1. setting τ to 1, set ZtIs empty;
ζt(τ)≤[μ,μ]′, (16)
Stopping the operation and outputtingThe final estimated speed; otherwise, let τ increase by 1 and ζt(τ) is stored in Zt;
3. If Z istIs equal to t-1, the operation is stopped and outputIs the final estimated speed, wherein
Wherein, L (·) represents a two-norm; otherwise, go to step 2.
In summary, we propose a target speed adaptive estimation method, as shown in fig. 1, including the following steps:
step 1, initialization: setting a sampling point T to be 0, and determining sampling time T and a judgment threshold value mu according to actual conditions;
step 2, the sampling point is automatically increased by 1, and then the positioning result at the sampling point t is read from the positioning equipmentWith corresponding error envelope matrix RtWherein the symbol' represents a transpose,as a result of the positioning of the east coordinates,for the positioning result of the north coordinates, RtIs a 2-dimensional diagonal matrix and satisfies
Wherein the content of the first and second substances,is shown inCentered on RtEllipsoid, theta, of envelope matrixt=(xt,yt) ' denotes the true position coordinates, xtRepresenting true east coordinates, ytRepresenting true north coordinates;
3.1 set delay coefficient τ to 1, set ZtIs empty;
Wherein the content of the first and second substances,a vector of 2 × 1, the first element representing the estimated east movement velocity, the second element representing the estimated north movement velocity, and T representing the sample time;
Wherein
Wherein tr represents a trace of the matrix;
Wherein Q ist(τ)(i,i)Represents Qt(τ) diagonal to the ith element,to representThe ith element of (1);
3.4 if satisfied
Wherein mu is a determination threshold, stopping the operation, and outputtingThe final estimated speed; otherwise, let τ increase by 1 and ζt(τ) is stored in Zt;
Wherein, L (·) represents a two-norm; otherwise, turning to step 3.2;
and 4, repeating the steps 2 to 3, and outputting the speed estimation value of each sampling point.
The above examples are provided only for the purpose of describing the present invention, and are not intended to limit the scope of the present invention. The scope of the invention is defined by the appended claims. Various equivalent substitutions and modifications can be made without departing from the spirit and principles of the invention, and are intended to be within the scope of the invention.
Claims (1)
1. A method for adaptively estimating a target speed, comprising the steps of:
step 1, initialization: setting a sampling point T to be 0, and determining sampling time T and a judgment threshold value mu according to actual conditions;
step 2, the sampling point is automatically increased by 1, and then the positioning result at the sampling point t is read from the positioning equipmentWith corresponding error envelope matrix RtWherein the symbol' represents a transpose,as a result of the positioning of the east coordinates,positioning results for north coordinates,RtIs a 2-dimensional diagonal matrix and satisfies
Wherein the content of the first and second substances,is shown inCentered on RtEllipsoid, theta, of envelope matrixt=(xt,yt) ' denotes the true position coordinates, xtRepresenting true east coordinates, ytRepresenting true north coordinates;
3.1 set delay coefficient τ to 1, set ZtIs empty;
Wherein the content of the first and second substances,a vector of 2 × 1, the first element representing the estimated east movement velocity, the second element representing the estimated north movement velocity, and T representing the sample time;
Wherein
Wherein tr represents a trace of the matrix;
Wherein Q ist(τ)(i,i)Represents Qt(τ) diagonal to the ith element,to representThe ith element of (1);
3.4 if satisfied
Wherein mu is a determination threshold, stopping the operation, and outputtingThe final estimated speed; otherwise, let τ increase by 1 and ζt(τ) is stored in Zt;
Wherein, L (·) represents a two-norm; otherwise, turning to step 3.2;
and 4, repeating the steps 2 to 3, and outputting the speed estimation value of each sampling point.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810946930.4A CN109085376B (en) | 2018-08-20 | 2018-08-20 | Target speed self-adaptive estimation method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810946930.4A CN109085376B (en) | 2018-08-20 | 2018-08-20 | Target speed self-adaptive estimation method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109085376A CN109085376A (en) | 2018-12-25 |
CN109085376B true CN109085376B (en) | 2020-09-18 |
Family
ID=64794032
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810946930.4A Active CN109085376B (en) | 2018-08-20 | 2018-08-20 | Target speed self-adaptive estimation method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109085376B (en) |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101661048A (en) * | 2008-08-29 | 2010-03-03 | 索尼株式会社 | Velocity calculation device,velocity calculation method, and navigation device |
CN102298070A (en) * | 2010-06-22 | 2011-12-28 | 鹦鹉股份有限公司 | Method for assessing the horizontal speed of a drone, particularly of a drone capable of hovering on automatic pilot |
CN102353802A (en) * | 2011-07-01 | 2012-02-15 | 哈尔滨工程大学 | Method for measuring angular velocity of motion carrier based on total acceleration meter |
CN103900613A (en) * | 2014-03-31 | 2014-07-02 | 哈尔滨工程大学 | Micro-electromechanical system (MEMS) error estimation method based on magnetometer and N step interval detection |
CN204631065U (en) * | 2015-05-26 | 2015-09-09 | 重庆大学 | A kind of road speed and acceleration measurement device |
KR101813698B1 (en) * | 2016-12-21 | 2018-01-30 | 한국항공대학교산학협력단 | Apparatus and method for estimating velocity of aerial robots |
CN107991110A (en) * | 2017-11-29 | 2018-05-04 | 安徽省通信息科技有限公司 | A kind of caterpillar type robot slides parameter detection method |
CN108020855A (en) * | 2017-11-29 | 2018-05-11 | 安徽省通信息科技有限公司 | The pose and instantaneous center of rotation combined estimation method of a kind of glide steering robot |
CN108036789A (en) * | 2017-11-29 | 2018-05-15 | 安徽省通信息科技有限公司 | A kind of field robot reckoning method |
CN108121941A (en) * | 2016-11-30 | 2018-06-05 | 上海联合道路交通安全科学研究中心 | A kind of object speed calculation method based on monitoring device |
CN108362282A (en) * | 2018-01-29 | 2018-08-03 | 哈尔滨工程大学 | A kind of inertia pedestrian's localization method based on the adjustment of adaptive zero-speed section |
-
2018
- 2018-08-20 CN CN201810946930.4A patent/CN109085376B/en active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101661048A (en) * | 2008-08-29 | 2010-03-03 | 索尼株式会社 | Velocity calculation device,velocity calculation method, and navigation device |
CN102298070A (en) * | 2010-06-22 | 2011-12-28 | 鹦鹉股份有限公司 | Method for assessing the horizontal speed of a drone, particularly of a drone capable of hovering on automatic pilot |
CN102353802A (en) * | 2011-07-01 | 2012-02-15 | 哈尔滨工程大学 | Method for measuring angular velocity of motion carrier based on total acceleration meter |
CN103900613A (en) * | 2014-03-31 | 2014-07-02 | 哈尔滨工程大学 | Micro-electromechanical system (MEMS) error estimation method based on magnetometer and N step interval detection |
CN204631065U (en) * | 2015-05-26 | 2015-09-09 | 重庆大学 | A kind of road speed and acceleration measurement device |
CN108121941A (en) * | 2016-11-30 | 2018-06-05 | 上海联合道路交通安全科学研究中心 | A kind of object speed calculation method based on monitoring device |
KR101813698B1 (en) * | 2016-12-21 | 2018-01-30 | 한국항공대학교산학협력단 | Apparatus and method for estimating velocity of aerial robots |
CN107991110A (en) * | 2017-11-29 | 2018-05-04 | 安徽省通信息科技有限公司 | A kind of caterpillar type robot slides parameter detection method |
CN108020855A (en) * | 2017-11-29 | 2018-05-11 | 安徽省通信息科技有限公司 | The pose and instantaneous center of rotation combined estimation method of a kind of glide steering robot |
CN108036789A (en) * | 2017-11-29 | 2018-05-15 | 安徽省通信息科技有限公司 | A kind of field robot reckoning method |
CN108362282A (en) * | 2018-01-29 | 2018-08-03 | 哈尔滨工程大学 | A kind of inertia pedestrian's localization method based on the adjustment of adaptive zero-speed section |
Non-Patent Citations (2)
Title |
---|
"膛内高速运动目标的速度测量技术研究";杨梦玲;《中国优秀硕士学位论文全文数据库》;20171231;全文 * |
"频率步进雷达运动扩展目标速度估计与距离像抽取研究";聂丹;《中国优秀硕士学位论文全文数据库》;20101231;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN109085376A (en) | 2018-12-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110531357B (en) | Method for estimating speed of moving target in horizontal plane and radar detection system | |
CN107990891B (en) | Underwater robot combined navigation method based on long baseline and beacon online calibration | |
CN109239709B (en) | Autonomous construction method for local environment map of unmanned ship | |
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 | |
EP1975646A2 (en) | Lader-based motion estimation for navigation | |
US20210213962A1 (en) | Method for Determining Position Data and/or Motion Data of a Vehicle | |
Ko et al. | Comparison of Kalman filter and particle filter used for localization of an underwater vehicle | |
CN114599995A (en) | Estimating in-plane velocity from radar returns of stationary roadside objects | |
KR101890612B1 (en) | Method and apparatus for detecting object using adaptive roi and classifier | |
KR101789188B1 (en) | An underwater integrated navigation system for tracking underwater moving objects | |
CN109579850B (en) | Deepwater intelligent navigation method based on auxiliary inertial navigation to water velocity | |
Xu et al. | Robust inertial-aided underwater localization based on imaging sonar keyframes | |
Han et al. | GPS-less coastal navigation using marine radar for USV operation | |
CN109582026B (en) | Autonomous underwater vehicle path tracking control method based on self-tuning sight line and drift angle compensation | |
CN109085376B (en) | Target speed self-adaptive estimation method | |
CN114061565A (en) | Unmanned ship SLAM and application method thereof | |
Sawada et al. | Mapping and localization for autonomous ship using LiDAR SLAM on the sea | |
CN117630911A (en) | Unmanned ship radar photoelectric fusion method based on Singer model | |
KR101790482B1 (en) | Wave measurement system and method using marine rader | |
CN113091756A (en) | Position estimation device and position estimation method | |
CN110908404A (en) | AUV intelligent observation motion method based on data driving | |
Subramanian et al. | Shoreline mapping using an omni-directional camera for autonomous surface vehicle applications | |
McConnell et al. | Perception for Underwater Robots | |
CN111446898B (en) | Low-cost AUV speed estimation method based on fuzzy logic and extended state observer |
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 |