CN111693972B - Vehicle position and speed estimation method based on binocular sequence images - Google Patents

Vehicle position and speed estimation method based on binocular sequence images Download PDF

Info

Publication number
CN111693972B
CN111693972B CN202010475838.1A CN202010475838A CN111693972B CN 111693972 B CN111693972 B CN 111693972B CN 202010475838 A CN202010475838 A CN 202010475838A CN 111693972 B CN111693972 B CN 111693972B
Authority
CN
China
Prior art keywords
moving target
image
coordinates
moving
background
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
CN202010475838.1A
Other languages
Chinese (zh)
Other versions
CN111693972A (en
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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN202010475838.1A priority Critical patent/CN111693972B/en
Publication of CN111693972A publication Critical patent/CN111693972A/en
Application granted granted Critical
Publication of CN111693972B publication Critical patent/CN111693972B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/12—Systems for determining distance or velocity not using reflection or reradiation using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N20/00—Machine learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/20—Image enhancement or restoration using local operators
    • G06T5/30—Erosion or dilatation, e.g. thinning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/70—Denoising; Smoothing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/215—Motion-based segmentation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Software Systems (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Medical Informatics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Artificial Intelligence (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

The invention discloses a vehicle position and speed estimation method based on binocular sequence images, which comprises the following steps of: s1, acquiring a depth map and a point cloud map by using a ZED binocular camera; s2, background subtraction is achieved through a KNN algorithm, and a moving target and a static background environment in the sequence image are identified; s3, detecting edge points of the moving target, drawing a rectangular identification frame positioned on the moving target, and tracking the moving target in real time; s4, removing a rectangular recognition frame appearing on the non-moving target caused by error factors such as light and shadow; s5, calculating the pixel coordinate of the central point of the effective rectangular identification frame locked on the moving target; s6, obtaining the (X, Y, Z) three-dimensional space coordinates of the points; and S7, estimating the position and the speed of the next moment by using a Kalman filtering algorithm according to the three-dimensional space coordinates of the previous frame and the current frame. The method has the advantages of simple algorithm principle and low calculation complexity, is suitable for the technical field of simulation, and is used for detecting the position and the speed of the co-directional or opposite-directional vehicles by the unmanned vehicles.

Description

Vehicle position and speed estimation method based on binocular sequence images
Technical Field
The invention relates to the technical field of intelligent automobile positioning, in particular to a vehicle position and speed estimation method based on binocular sequence images.
Technical Field
The measurement, calculation and prediction of the position and the speed of the vehicle have important significance in the field of unmanned control. The method comprises the steps of calculating and predicting the position and the speed by using a sequence image acquired by a binocular camera, ensuring the relative accuracy of a predicted value, and controlling the use cost better. And estimating the vehicle speed and position based on the binocular image sequence, considering the sequence images acquired by using a binocular camera, realizing background subtraction by means of a KNN algorithm and performing relevant optimization adjustment, and acquiring relevant values and then performing processing calculation by using a Kalman filtering algorithm. Namely, the position and the speed of the next moment are predicted by using the position change of the characteristic points of the two frames before and after the real-time image.
At present, the existing mature vehicle speed detection technologies include ground magnetic induction coil detection, infrared laser ray vehicle speed detection, radar vehicle speed detection, video-based vehicle speed detection and the like. The video detection speed is relatively high, the amount of information which can be simultaneously obtained through the video is large, the detection technology cost is low, and the like, so that attention is paid to the video detection technology. In the video-based detection method, a related moving object needs to be obtained through a video frame, the position coordinate information of the related moving object is detected, and the speed is calculated. The relatively mature vehicle speed measuring method based on motion equation and KLT characteristic point tracking, which is proposed in 2010 by Madasu and Handlun and the like, can obtain the position information and the speed information of the vehicle.
In a similar video detection method, a monocular camera is commonly used, a real-time video is read through the monocular camera, and the position change of a moving target is obtained by comparing the known camera state with a two-dimensional image sequence of a front frame and a rear frame. However, this method cannot directly obtain three-dimensional coordinates, and has certain limitations in use.
Disclosure of Invention
In order to reduce the cost of a traditional vehicle-mounted positioning system mainly based on a laser radar or a combined navigation system, the invention provides a vehicle position and speed estimation method based on a binocular sequence image, which comprises the following steps:
s1, continuously acquiring sequence images in a visual range by using a ZED binocular camera to obtain a depth map and a point cloud map;
the ZED binocular camera can completely acquire the geometric information of the moving object. The traditional camera for vehicle speed measurement is generally a monocular camera, and although the mode of acquiring moving object information is similar to that of a binocular camera, the acquired information is extremely limited. Because of the lack of image depth information, a monocular camera can only acquire (x, y) coordinate information in an image coordinate system, but cannot acquire (x, y, z) three-dimensional coordinates of a certain moving object or a certain characteristic point in the three-dimensional coordinate system, so that the monocular camera has great limitations on calculation and prediction in the aspects of speed, position and distance, and has requirements on the placement angle and the like of the camera.
The binocular stereo vision adopted by the binocular camera can solve the problem, the three-dimensional position information of the moving object is completely recorded by utilizing the three-dimensional stereo coordinate system established at the camera, and the method has obvious help effect on the requirements of prediction and calculation of the position and the speed by utilizing a Kalman filtering method.
The binocular stereo vision is based on the parallax principle for measuring the coordinates of a target point, firstly, a three-dimensional stereo coordinate system with the central point of a camera as the origin is established, and the central distance b between the two cameras is determined. Then, images acquired from the two cameras are matched, and the projections M of the same characteristic point M in the coordinate systems of the two cameras are acquired 1 And M 2 Coordinate value (x) of 1 ，y 1 ) And (x) 2 ，y 2 ) And the three-dimensional coordinates (x, y, z) of the characteristic point in a camera coordinate system can be obtained through data calculation
The three-dimensional coordinates acquired through the ZED binocular camera are uniformly stored in a Point cloud type built in the ZED, and the coordinate system can be freely selected to be established based on a left eye or a right eye (the default is the right eye). Meanwhile, the transformation between the three-dimensional coordinate system and the image coordinate system can be realized, namely:
and (x, y) coordinates of the dynamic target in an image coordinate system, which are obtained through the image acquired by the left eye camera, are substituted into a retrieval function of the three-dimensional point cloud picture, so that the coordinate system conversion can be completed, and the (x, y, z) coordinates of the target point in the left eye camera coordinate system are returned for algebraic calculation in Kalman filtering.
S2, background subtraction is achieved through a KNN algorithm, and a moving target and a static background environment in the sequence image are identified;
the K nearest neighbor algorithm (KNN) belongs to the data mining classification technology and is characterized in that: and judging the class of the sample according to the class of the nearest neighbor sample of the sample, wherein the selected neighbor is the sample which is correctly classified.
The principle of the KNN algorithm in background modeling is as follows: the feature value type at one coordinate (x, y) is predicted, and the category of the feature value type is determined according to the feature value category of K points closest to the feature value type. Firstly according to a distance formula
Figure BDA0002515847790000031
And calculating the distance between the target point and the adjacent point, screening points for comparison through the K value, and finally determining the type of the target point.
The KNN algorithm has the advantages of high accuracy, stable effect, simple thought and clear process. Although the problem of no training process and complex calculation process exists, the complexity of the calculation process has no obvious influence on the camera pixel points with small sample capacity and can be ignored, and the application of the method is very suitable for classifying and distinguishing moving objects and backgrounds based on the characteristic of excellent classification capability, and a KNN algorithm is adopted.
Considering that the KNN algorithm belongs to the category of machine learning, in the first few frames when a program just starts to run, the algorithm needs to carry out adaptive training according to images, and the background and the moving object cannot be automatically separated until a proper background is found. However, the time of the adaptive training is very short, and the data processing content in the adaptive process is removed, so that the subsequent functions are not obviously influenced, the part has no influence on the tracking of the moving object and the subsequent coordinate calculation and Kalman filtering data processing process, and the task of tracking the moving object by background elimination modeling can be effectively completed.
The background subtraction algorithm is a very classical moving target detection means, and is characterized in that a background model under a static scene is obtained in a background modeling mode, then difference operation is carried out by means of image characteristics under the current frame and a previously stored background model, and an obtained region is stored as a moving target of a moving region, so that the identification and tracking of a moving object are completed. The principle of the background subtraction algorithm is
Figure BDA0002515847790000041
(x, y) -coordinate values of corresponding pixel points in the image coordinate system, f c (x, y) -feature value of pixel at (x, y) coordinate under current frame, f b (x, y) -eigenvalues of pixel points at (x, y) coordinates on background modeling, T h -a set threshold value, M, for determining moving objects Object (x, y) -image obtained after black-and-white binarization and corresponding processing are carried out on the current frame and the background difference image. And when the difference value between the characteristic value of the pixel point and the characteristic value of the background point is judged to be larger than the threshold value, the point is judged to be a moving object, otherwise, the point is judged to be the background point.
Background elimination modeling can separate moving objects from background characteristic points so as to obtain required parts (point cloud coordinates of the moving objects need to be obtained in the project), the project utilizes a KNN algorithm to realize background elimination, the background elimination is realized in C + + through an application function, and a relatively complete moving object and background separated image can be obtained after denoising processing of black and white binarization, corrosion and expansion (the moving objects are displayed in white, and the background is displayed in black).
S3, detecting edge points of the moving target, drawing a rectangular identification frame positioned on the moving target, and tracking the moving target in real time;
the images are analyzed through a Find contacts contour detection function in an OPENCV library, and the moving objects can be visualized through a rectangle function, so that the moving objects can be tracked in real time.
S4, setting a rectangular identification frame screening mechanism, and removing rectangular identification frames appearing on the non-moving target caused by error factors such as light and shadow;
the moving target identification is ensured to be accurate by setting a screening function to screen the identification range.
S5, calculating pixel coordinates X 'and Y' of the central point of the effective rectangular identification frame locked on the moving target;
the visualized image is saved based on the pixel coordinates (x, y) of the center point of the image coordinate system.
S6, acquiring corresponding real coordinates X and Y and a depth coordinate Z by using the obtained X 'and Y' and by means of a ZED point cloud coordinate, namely acquiring a (X, Y, Z) three-dimensional space coordinate of the point;
and converting the pixel coordinates (x, y) into three-dimensional coordinates (x, y, z) of the camera through the coordinate conversion mode in the step S1.
And S7, estimating the position and the speed of the next moment by using a Kalman filtering algorithm according to the three-dimensional space coordinates of the previous frame and the current frame.
The Kalman filtering algorithm has the core idea of prediction and measurement feedback and consists of two parts, wherein the first part is a linear system state prediction equation, and the second part is a linear system observation equation
The linear system state prediction equation can be expressed as:
x k ＝Ax k-1 +Bu k-1 +ω k-1
wherein: p (omega) -N (0, Q)
X in the equation k And x k-1 Represents the true value of the state at the time k and (k-1), u k-1 Indicates the control amount, ω, at the time (k-1) k-1 Representing process excitation noise, a represents the state transition coefficient matrix (n × n order), B represents the gain matrix of the optional control inputs, and Q represents the covariance matrix of the process excitation noise.
The linear system observation equation can be expressed as:
z k ＝Hx k +v k
wherein: p (v) to N (0, R)
Z in the equation k Is an observed value at time k, v k For observing noise, H denotes a measurement coefficient matrix (m × n order matrix) and R denotes a measurement noise covariance matrix.
To get the optimal estimate, the a posteriori covariance at time k must be known a priori the estimated covariance.
The expression of the posterior estimated covariance at the time k is as follows:
Figure BDA0002515847790000051
the a posteriori estimate of the k time can be expressed by a priori estimation and kalman gain:
Figure BDA0002515847790000052
thereby, it is possible to obtain:
Figure BDA0002515847790000061
the prior estimated covariance expression at time k is:
Figure BDA0002515847790000062
can be obtained by the following two formulas:
Figure BDA0002515847790000063
let the trace of the A posteriori estimated covariance matrix at time k be Tp k ]Thus, there are:
Figure BDA0002515847790000064
P k the sum of the diagonal elements is the mean square error. The mean square error is used for deriving the unknown quantity K, the value of Kalman gain K can be determined by making the derivation function equal to 0, and the minimum mean square error estimation of the model is completed, so that the error of the posterior estimation is small and is closer to the true value of the state.
The minimum mean square error is found to determine the expression for kalman gain:
Figure BDA0002515847790000065
Figure BDA0002515847790000066
Figure BDA0002515847790000067
as can be seen,
Figure BDA0002515847790000068
the larger, K k The larger the priori estimation error is, the more reliable measurement feedback is needed, and the Kalman gain is automatically increased by the model to carry out more accurate estimation;
Figure BDA0002515847790000069
is 0,K k The value is 0, namely the prior estimation has no error, the accurate posterior estimation can be obtained only through prediction, the feedback of measurement is not needed, and the weight value of the feedback of measurement is automatically set to be 0 by the model.
Therefore, a final equation of the Kalman filtering algorithm can be obtained:
(1) Time update equation
Forward reckoning state variables:
Figure BDA00025158477900000610
forward estimation error covariance:
Figure BDA00025158477900000611
(2) Equation of state update
Calculating a Kalman gain:
Figure BDA0002515847790000071
from an observed variable z k Updating estimation:
Figure BDA0002515847790000072
updating the error covariance:
Figure BDA0002515847790000073
the beneficial effects of the invention are as follows:
and aiming at the estimation of the position and the speed of the vehicle, a sequence image acquired by using a binocular camera is considered, background subtraction is realized by means of a KNN algorithm, relevant optimization adjustment is carried out, and after relevant numerical values are acquired, processing calculation is carried out by means of a Kalman filtering algorithm. Namely, the position and the speed of the next moment are predicted by using the position change of the characteristic points of the two frames before and after the real-time image. Experiments prove that the prediction method provided by the invention has higher reliability in a certain range. The method has the advantages of simple algorithm principle, low calculation complexity and low use cost by utilizing the binocular camera, is suitable for the technical field of simulation, can realize calculation and prediction of the speed and the position of the vehicle in a real environment, can be used for detecting the position and the speed of the co-directional or opposite-directional vehicle by the unmanned vehicle, can also be used for actual scenes such as intersection vehicle speed measurement and the like, and has good application prospect.
Drawings
FIG. 1 is a flow chart of the algorithm of the present invention;
FIG. 2 (a) is a comparison graph (X direction) of the real value and the predicted value of the three-dimensional space position of the experimental trolley of the invention;
FIG. 2 (b) is a comparison graph (Y direction) of the real value and the predicted value of the three-dimensional spatial position of the experimental trolley;
FIG. 2 (c) is a comparison graph (Z direction) of the real value and the predicted value of the three-dimensional space position of the experimental trolley of the invention;
FIG. 3 (a) is a comparison graph (X direction) of the real value and the predicted value of the three-dimensional direction speed of the experimental trolley;
FIG. 3 (b) is a comparison graph (Y direction) of the real value and the predicted value of the three-dimensional direction speed of the experimental trolley;
FIG. 3 (c) is a comparison graph (Z direction) of the real value and the predicted value of the three-dimensional direction speed of the experimental trolley of the invention;
fig. 4 is a working schematic diagram of the binocular camera of the present invention.
Examples
The present invention will be further illustrated with reference to the accompanying drawings and specific embodiments, which are to be understood as merely illustrative of the invention and not as limiting the scope of the invention.
The general design idea of the invention is as follows: the method comprises the steps of continuously acquiring sequence images by using a ZED binocular camera, obtaining a depth map and a point cloud map, realizing Background Subtraction (BSM) through a KNN algorithm, locking a moving target by using a rectangular identification frame, realizing accurate tracking of a moving vehicle, obtaining three-dimensional space coordinates of points on the moving target, obtaining a real-time moving state of the vehicle, and realizing more accurate prediction of the impending moving state of the vehicle by using a Kalman filtering algorithm.
As shown in the figure, the embodiment of the invention discloses a vehicle position and speed estimation method based on binocular sequence images, and the embodiment of the invention comprises the following steps:
s1, using a ZED binocular camera to continuously collect sequence images in a visual range to obtain a depth map and a point cloud map
The experiment dolly freely moves and passes in front of the camera, and the ZED binocular camera continuously acquires the image sequence, after processing and calculating, the three-dimensional coordinate that acquires through the ZED binocular camera is uniformly stored in the built-in Point cloud of ZED, and whether this coordinate system is established based on the left eye or the right eye (the default is the right eye) can be freely selected. And simultaneously, the conversion between a three-dimensional coordinate system and an image coordinate system is realized, and the (x, y) coordinates of the dynamic target in the image coordinate system, which are obtained through the image acquired by the left eye camera, are substituted into a retrieval function of the three-dimensional point cloud picture to complete the coordinate system conversion and return the (x, y, z) coordinates of the target point in the left eye camera coordinate system for algebraic calculation in Kalman filtering.
S2, background subtraction is achieved through a KNN algorithm, and a moving target and a static background environment in the sequence image are identified;
background elimination is realized by utilizing a KNN algorithm, separation of a background and a moving trolley is realized by an apply function, a background model under a static scene is obtained in a background modeling mode after denoising processing of black-white binarization, corrosion and expansion is carried out, then difference operation is carried out by means of image characteristics under a current frame and a previously stored background model, and an obtained area is used as a moving object of a moving area to be stored, so that identification and tracking of the moving object are completed, and a relatively complete image with the moving object separated from the background is obtained.
Background subtraction is realized by KNN algorithm, moving object identification is carried out by adopting the background subtraction method, namely background elimination modeling is realized by utilizing a Gaussian mixture model of an image segmentation mode or a KNN algorithm of machine learning,
the KNN algorithm predicts the characteristic value type under one coordinate (x, y) during background modeling, determines the category of the KNN algorithm according to the characteristic value category of K points closest to the KNN algorithm, and firstly determines the category of the KNN algorithm according to a distance formula
Figure BDA0002515847790000091
Calculating the distance between the target point and the adjacent point, screening points for comparison through a K value, and finally determining the type of the target point;
the background subtraction algorithm is specifically
Figure BDA0002515847790000092
(x, y) -coordinate values of corresponding pixel points in the image coordinate system, f c (x, y) -feature value of pixel at (x, y) coordinate under current frame, f b (x, y) -eigenvalues of pixel points at (x, y) coordinates on background modeling, T h Setting a threshold value for determining moving objects, M object (x, y) -obtaining an image after black-and-white binarization and corresponding processing are carried out on the current frame and the background difference image; and when the difference value between the characteristic value of the pixel point and the characteristic value of the background point is judged to be larger than the threshold value, the point is judged to be a moving object, otherwise, the point is judged to be the background point.
Background elimination is realized by utilizing a KNN algorithm in C + + through an apply function, and a relatively complete moving target and background separated image can be obtained after denoising processing of black and white binarization, corrosion and expansion.
S3, detecting edge points of the moving target, drawing a rectangular recognition frame positioned on the moving target, and tracking the moving target in real time;
the image is analyzed through a Find contacts contour detection function in an OPENCV library, and the moving target is processed visually through a rectangle function, so that the real-time tracking effect of the moving trolley is realized.
S4, setting a rectangular identification frame screening mechanism, and removing rectangular identification frames appearing on the non-moving target caused by error factors such as light and shadow;
the screening and identifying range is screened by setting the screening function, so that the identifying frame is accurately and correctly selected on the contour of the moving trolley without obvious deviation and error.
S5, calculating pixel coordinates X 'and Y' of the central point of the effective rectangular recognition frame locked on the moving target;
and saving the visual trolley tracking image based on the pixel coordinates (x, y) of the central point of the image coordinate system for later acquiring three-dimensional coordinates.
S6, acquiring corresponding real coordinates X and Y and a depth coordinate Z by using the obtained X 'and Y' and by means of a ZED point cloud coordinate, namely acquiring a (X, Y, Z) three-dimensional space coordinate of the point;
and (4) converting the pixel coordinates (x, y) of the center of the moving trolley into real three-dimensional coordinates (x, y, z) of the trolley position under the camera coordinate system through the coordinate conversion mode in the step (S1).
And S7, estimating the position and the speed of the next moment by using a Kalman filtering algorithm according to the three-dimensional space coordinates of the previous frame and the current frame.
Namely, the position and the speed of the next moment can be estimated by using a Kalman filtering algorithm;
the Kalman filtering algorithm consists of two parts, the first part is a linear system state prediction equation, and the second part is a linear system observation equation
The linear system state prediction equation can be expressed as:
x k ＝Ax k-1 +Bu k-1 +ω k-1
wherein: p (omega) to N (0, Q)
X in the equation k And x k-1 Showing the true values of the states at times k and (k-1), u k-1 Indicates the control amount, ω, at the time (k-1) k-1 Representing process excitation noise, a representing a state transition coefficient matrix (n × n order), B representing a gain matrix of the optional control input, and Q representing a covariance matrix of the process excitation noise;
the linear system observation equation can be expressed as:
z k ＝Hx k +v k
wherein: p (v) to N (0, R)
Z in the equation k Is an observed value at time k, v k For observing noise, H denotes a measurement coefficient matrix (m × n order matrix) and R denotes a measurement noise covariance matrix.
The expression of the posterior estimated covariance at the time k is as follows:
Figure BDA0002515847790000111
the posterior estimation of k time is expressed by prior estimation and Kalman gain:
Figure BDA0002515847790000112
thereby, it is possible to obtain:
Figure BDA0002515847790000113
the prior estimated covariance expression at time k is:
Figure BDA0002515847790000114
the two formulas can be obtained:
Figure BDA0002515847790000115
let us denote the trace of the a posteriori estimated covariance matrix at time k as T [ Pk ], so there is:
Figure BDA0002515847790000116
P k the sum of the diagonal elements is the mean square error. The mean square error is derived from the unknown quantity K, the value of the Kalman gain K can be determined by making the derivative function equal to 0, and the minimum mean square error estimation of the model is completed, so that the error of the posterior estimation is small and is closer to the true value of the state.
Solving the minimum mean square error to determine an expression of the Kalman gain:
Figure BDA0002515847790000117
Figure BDA0002515847790000118
Figure BDA0002515847790000121
using a final equation of a kalman filter algorithm:
(1) Time update equation
Forward reckoning state variables:
Figure BDA0002515847790000122
forward estimation error covariance:
Figure BDA0002515847790000123
(2) Equation of state update
Calculating a Kalman gain:
Figure BDA0002515847790000124
from an observed variable z k Updating estimation:
Figure BDA0002515847790000125
updating the error covariance:
Figure BDA0002515847790000126
and substituting the real coordinate value, the measurement speed value and the time interval of each frame, predicting the position and the speed of the trolley at the next moment, storing the predicted data, and comparing the predicted data with the real data calculated at the next moment and carrying out posterior correction.
As shown in fig. 4, the binocular stereo vision measures coordinates of a target point based on the principle of parallax, and a three-dimensional coordinate system with a camera center point as an origin is established to determine a center distance b between two cameras. Then, images acquired from the two cameras are matched, and the projections M of the same characteristic point M in the coordinate systems of the two cameras are acquired 1 And M 2 Coordinate value (x) of 1 ，y 1 ) And (x) 2 ，y 2 ) And three-dimensional coordinates (x, y, z) of the characteristic point in a camera coordinate system can be obtained through data calculation.
The technical means disclosed in the scheme of the invention are not limited to the technical means disclosed in the above embodiments, but also include the technical means formed by any combination of the above technical features. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and such improvements and modifications are also considered to be within the scope of the present invention.

Claims (8)

1. A vehicle position and speed estimation method based on binocular sequence images is characterized by comprising the following steps: the method comprises the following steps:
s1, continuously acquiring sequence images in a visual range by using a ZED binocular camera to obtain a depth map and a point cloud map;
s2, background subtraction is achieved through a KNN algorithm, and a moving target and a static background environment in the sequence image are identified;
s3, detecting edge points of the moving target, drawing a rectangular identification frame positioned on the moving target, and tracking the moving target in real time;
s4, setting a rectangular recognition frame screening mechanism to remove a rectangular recognition frame appearing on the non-moving target caused by light and shadow error factors;
s5, calculating pixel coordinates X 'and Y' of the central point of the effective rectangular identification frame locked on the moving target;
s6, acquiring corresponding real coordinates X and Y and a depth coordinate Z by using the obtained X 'and Y' and by means of a ZED point cloud coordinate, namely acquiring a (X, Y, Z) three-dimensional space coordinate of the point;
and S7, estimating the position and the speed of the next moment by using a Kalman filtering algorithm according to the three-dimensional space coordinates of the previous frame and the current frame.
2. The method according to claim 1, wherein in the step S1, the ZED binocular camera completely acquires geometric information of a moving object;
the three-dimensional coordinate obtained through the ZED binocular camera is uniformly stored in a Point cloud type Point cloud built in the ZED, and the conversion between a three-dimensional coordinate system and an image coordinate system is realized, namely:
and (x, y) coordinates of the dynamic target in an image coordinate system, which are obtained through the image acquired by the left eye camera, are substituted into a retrieval function of the three-dimensional point cloud picture, namely, the coordinate system conversion is completed, and the (x, y, z) coordinates of the dynamic target point in the left eye camera coordinate system are returned for algebraic calculation in Kalman filtering.
3. The method of claim 1, wherein: in the step S2, background elimination is realized by using a KNN algorithm, separation of a background from a moving vehicle is realized by an apply function, denoising processing of black-and-white binarization, corrosion and expansion is performed, a background model in a static scene is obtained by a background modeling method, a difference operation is performed by using image characteristics in a current frame and a previously stored background model, and an obtained region is stored as a moving target in a moving region, so that identification and tracking of a moving object are completed, and a relatively complete image in which the moving target is separated from the background is obtained.
4. The method of claim 1, wherein: in the step S3, the image is analyzed by the Find thresholds contour detection function in the OPENCV library, and the moving object is visualized by the rectangle function, so as to realize real-time tracking of the moving object.
5. The method of claim 1, wherein: in the step S4, the identification range is screened by setting the screening function, so as to ensure that the moving object is accurately identified.
6. The method of claim 1, wherein: in step S5, the visualized image is stored based on the pixel coordinates (x, y) of the center point of the image coordinate system.
7. The method of claim 6, wherein: in step S6, the pixel coordinates (x, y) are converted into three-dimensional camera coordinates (x, y, z).
8. The method of claim 7, wherein: in the step S7, according to the three-dimensional coordinates (x, y, z) obtained in the step S6,
using a final equation of a kalman filter algorithm:
(1) Time update equation
Forward reckoning state variables:
Figure FDA0003842821070000021
forward estimation error covariance:
Figure FDA0003842821070000022
(2) Equation of state update
Calculating a Kalman gain:
Figure FDA0003842821070000023
from an observed variable z k Updating estimation:
Figure FDA0003842821070000024
updating the error covariance:
Figure FDA0003842821070000025
CN202010475838.1A 2020-05-29 2020-05-29 Vehicle position and speed estimation method based on binocular sequence images Active CN111693972B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010475838.1A CN111693972B (en) 2020-05-29 2020-05-29 Vehicle position and speed estimation method based on binocular sequence images

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010475838.1A CN111693972B (en) 2020-05-29 2020-05-29 Vehicle position and speed estimation method based on binocular sequence images

Publications (2)

Publication Number Publication Date
CN111693972A CN111693972A (en) 2020-09-22
CN111693972B true CN111693972B (en) 2022-11-15

Family

ID=72478862

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010475838.1A Active CN111693972B (en) 2020-05-29 2020-05-29 Vehicle position and speed estimation method based on binocular sequence images

Country Status (1)

Country Link
CN (1) CN111693972B (en)

Families Citing this family (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018176000A1 (en) 2017-03-23 2018-09-27 DeepScale, Inc. Data synthesis for autonomous control systems
US11893393B2 (en) 2017-07-24 2024-02-06 Tesla, Inc. Computational array microprocessor system with hardware arbiter managing memory requests
US11157441B2 (en) 2017-07-24 2021-10-26 Tesla, Inc. Computational array microprocessor system using non-consecutive data formatting
US10671349B2 (en) 2017-07-24 2020-06-02 Tesla, Inc. Accelerated mathematical engine
US11409692B2 (en) 2017-07-24 2022-08-09 Tesla, Inc. Vector computational unit
US11561791B2 (en) 2018-02-01 2023-01-24 Tesla, Inc. Vector computational unit receiving data elements in parallel from a last row of a computational array
US11215999B2 (en) 2018-06-20 2022-01-04 Tesla, Inc. Data pipeline and deep learning system for autonomous driving
US11361457B2 (en) 2018-07-20 2022-06-14 Tesla, Inc. Annotation cross-labeling for autonomous control systems
US11636333B2 (en) 2018-07-26 2023-04-25 Tesla, Inc. Optimizing neural network structures for embedded systems
US11562231B2 (en) 2018-09-03 2023-01-24 Tesla, Inc. Neural networks for embedded devices
WO2020077117A1 (en) 2018-10-11 2020-04-16 Tesla, Inc. Systems and methods for training machine models with augmented data
US11196678B2 (en) 2018-10-25 2021-12-07 Tesla, Inc. QOS manager for system on a chip communications
US11816585B2 (en) 2018-12-03 2023-11-14 Tesla, Inc. Machine learning models operating at different frequencies for autonomous vehicles
US11537811B2 (en) 2018-12-04 2022-12-27 Tesla, Inc. Enhanced object detection for autonomous vehicles based on field view
US11610117B2 (en) 2018-12-27 2023-03-21 Tesla, Inc. System and method for adapting a neural network model on a hardware platform
US11150664B2 (en) 2019-02-01 2021-10-19 Tesla, Inc. Predicting three-dimensional features for autonomous driving
US10997461B2 (en) 2019-02-01 2021-05-04 Tesla, Inc. Generating ground truth for machine learning from time series elements
US11567514B2 (en) 2019-02-11 2023-01-31 Tesla, Inc. Autonomous and user controlled vehicle summon to a target
US10956755B2 (en) 2019-02-19 2021-03-23 Tesla, Inc. Estimating object properties using visual image data
CN112472074A (en) * 2020-11-27 2021-03-12 吉林农业科技学院 Sitting gait data acquisition and analysis system based on acceleration sensor
CN112268548B (en) * 2020-12-14 2021-03-09 成都飞机工业（集团）有限责任公司 Airplane local appearance measuring method based on binocular vision
CN112698380A (en) * 2020-12-16 2021-04-23 南京大学 Beam section processing method suitable for low-energy proton beam under strong background noise
CN113112524B (en) * 2021-04-21 2024-02-20 智道网联科技(北京)有限公司 Track prediction method and device for moving object in automatic driving and computing equipment
CN114034303A (en) * 2021-11-11 2022-02-11 华南农业大学 Moving target object positioning method and device based on Kalman filtering
CN114567726A (en) * 2022-02-25 2022-05-31 苏州安智汽车零部件有限公司 Human-eye-like self-adaptive shake-eliminating front-view camera
CN114684568A (en) * 2022-04-29 2022-07-01 天地（常州）自动化股份有限公司 Coal flow velocity and coal flow measuring system and measuring method thereof

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108731587A (en) * 2017-04-14 2018-11-02 中交遥感载荷（北京）科技有限公司 A kind of the unmanned plane dynamic target tracking and localization method of view-based access control model
CN109934848B (en) * 2019-03-07 2023-05-23 贵州大学 Method for accurately positioning moving object based on deep learning
CN109949364B (en) * 2019-04-01 2023-04-11 上海淞泓智能汽车科技有限公司 Vehicle attitude detection precision optimization method based on road side monocular camera
CN110570453B (en) * 2019-07-10 2022-09-27 哈尔滨工程大学 Binocular vision-based visual odometer method based on closed-loop tracking characteristics
CN111199556B (en) * 2019-12-31 2023-07-04 同济大学 Indoor pedestrian detection and tracking method based on camera

Also Published As

Publication number Publication date
CN111693972A (en) 2020-09-22

Similar Documents

Publication Publication Date Title
CN111693972B (en) Vehicle position and speed estimation method based on binocular sequence images
CN110163904B (en) Object labeling method, movement control method, device, equipment and storage medium
CN110988912B (en) Road target and distance detection method, system and device for automatic driving vehicle
US11461912B2 (en) Gaussian mixture models for temporal depth fusion
Zhou et al. Efficient road detection and tracking for unmanned aerial vehicle
US9846812B2 (en) Image recognition system for a vehicle and corresponding method
EP2858008B1 (en) Target detecting method and system
Ferryman et al. Visual surveillance for moving vehicles
Fang et al. On-road vehicle tracking using part-based particle filter
US9361696B2 (en) Method of determining a ground plane on the basis of a depth image
CN107452015B (en) Target tracking system with re-detection mechanism
Sucar et al. Bayesian scale estimation for monocular slam based on generic object detection for correcting scale drift
CN115049700A (en) Target detection method and device
CN111738033B (en) Vehicle driving information determination method and device based on plane segmentation and vehicle-mounted terminal
CN113848545A (en) Fusion target detection and tracking method based on vision and millimeter wave radar
Głowacz et al. Video detection algorithm using an optical flow calculation method
Omar et al. Detection and localization of traffic lights using YOLOv3 and Stereo Vision
US20200057905A1 (en) Point group data processing device, point group data processing method, point group data processing program, vehicle control device, and vehicle
Cigla et al. Image-based visual perception and representation for collision avoidance
Chun-Zhao et al. Drivable road boundary detection for intelligent vehicles based on stereovision with plane-induced homography
García-García et al. 3D visual odometry for road vehicles
EP3896651A1 (en) Method and apparatus for evaluating temporal characteristics of semantic image segmentation
Du CAMShift-Based Moving Object Tracking System
Chumerin et al. Cue and sensor fusion for independent moving objects detection and description in driving scenes
Zwemer et al. 3D Detection of Vehicles from 2D Images in Traffic Surveillance.

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
GR01 Patent grant
GR01 Patent grant