Disclosure of Invention
The invention aims to solve the problems of low ranging accuracy caused by system uncertainty, environmental interference and failure data in the existing distance measurement mode and the ranging error caused by the influence of system errors of a single ranging system.
A distance measurement method based on the fusion of TOF cameras and binocular vision data comprises the following steps:
S1, acquiring data of ranging of a target to be measured by a TOF camera and a binocular vision ranging platform, wherein the acquired data corresponds to n times of measurement, namely the TOF camera and the binocular vision camera respectively acquire n distance values Di, i=1, 2, & gt, n;
The TOF camera and the binocular vision ranging platform comprise a TOF camera and a binocular vision camera, wherein the binocular vision camera comprises a binocular vision right camera and a binocular vision left camera;
S2, respectively carrying out the following processing on the TOF camera and binocular vision:
s2.1, determining a Gaussian distribution function of the random measured value d:
Wherein, sigma is the standard deviation, D is the random measurement value in n distance values Di, mu is the mathematical expected value, sigma2 is the variance corresponding to n distance values Di;
s2.2, determining a critical value of a selectable value range and a Gaussian distribution function:
wherein u is the lower threshold of the gaussian distribution function;
when the value of the Gaussian distribution function is larger than u, the measured value is considered to be a high probability occurrence value, and m is an average value corresponding to n distance values Di;
S2.3, selecting the ranging values Xi which are reserved after being screened by a Gaussian model from the ranging initial values, wherein the number of the ranging values Xi is r, and obtaining the ranging optimal values:
Wherein Xi is the i-th value meeting the requirements, i=1, 2,..r, r is the number meeting the requirements;
S3, judging whether any one of the distance measurement optimal value distances obtained by the TOF method and the binocular method is located between a first distance threshold and a second distance threshold, and if so, executing S4;
S4, fusing the TOF ranging average value and the binocular vision ranging average value according to weights to obtain a final ranging value:
S4.1, marking a TOF camera as p, marking data screened by a Gaussian model as Xp, and marking the mean value as
The binocular vision camera is marked as q, the data screened by the Gaussian model is marked as Xq, and the average value is
S4.2, assuming that the real distance is Xture, recording the observation errors of TOF ranging average value and binocular vision ranging as Vp and Vq, and then having Xp＝Xture+Vp and Xq＝Xture+Vq, wherein the observation errors Vp and Vq are regarded as zero-mean stationary noise;
The variance of the data measured by camera p is σ2＝E(Vp2), where E (·) is desired;
s4.3, as R data are obtained for both cameras after being processed by the Gaussian model, a cross-correlation function Rpq between the camera p and the camera q and an autocorrelation function Rpp of the camera p are obtained:
s4.4 determining the variance of the camera pAnd variance of camera q:
S4.5, determining weighting factors Wp and Wq for camera p and camera q:
S4.5, fusing by using weighting factors Wp and Wq, wherein the estimated value of the fused distance X is as follows:
Wherein, theAndAnd (5) screening the data mean value of the TOF camera and the binocular vision through a Gaussian model.
Further, the determining process of the first distance threshold and the second distance threshold is as follows:
A1, acquiring data for ranging a target to be measured by a TOF camera and a binocular vision ranging platform, wherein the acquired data correspond to L/k measuring positions, and each position corresponds to n times of measuring data;
in the range L of the measured object, the measured object is approached to the measured object by the measuring position with the interval distance of k and is measured in sequence, each measuring position is measured n times, and L/k measuring positions are all used;
A2, respectively processing the TOF camera and the binocular vision, and processing each group of data by using a Gaussian model to obtain a ranging optimal value;
A3, determining distance of the distance measurement optimal value and corresponding distance errors based on L/k measurement positions, respectively obtaining error-distance curves of the TOF and binocular vision methods in the distance measurement range 0~L, drawing the error-distance curves of the TOF and binocular vision methods together, and obtaining the intersection point corresponding to the distance minimum value and the intersection point corresponding to the distance maximum value in the two graphs as a first distance threshold value and a second distance threshold value.
Further, the processing procedure of step A2 is the same as that of step S2.
Preferably, the first distance threshold and the second distance threshold are 1m and 3.5m, respectively.
Further, the method comprises the following steps:
Taking the distance within the first distance threshold as a short distance segment and the distance outside the second distance threshold as a long distance segment based on the target position;
In the S3 process of judging whether any one of the distance measurement optimal value distances obtained by the TOF method and the binocular method is located between the first distance threshold value and the second distance threshold value, if the distance measurement optimal value distances obtained by the TOF method and the binocular method are in a long distance section, selecting a distance measurement average value corresponding to a camera with smaller distance error in the long distance section in the TOF camera and the binocular vision camera as a final distance measurement value, and if the distance measurement optimal value distances obtained by the TOF method and the binocular method are in a short distance section, selecting a distance measurement average value corresponding to a camera with smaller distance error in the short distance section in the TOF camera and the binocular vision camera as the final distance measurement value.
Further, the distance error in the camera with smaller distance error in the long distance section is determined in the process of determining the distance between the distance measurement optimal value and the corresponding distance error in the A3.
Further, the distance error in the camera with smaller distance error in the short distance section is determined in the process of determining the distance between the distance measurement optimal value and the corresponding distance error in the A3.
Further, in the short distance section, the ranging average value corresponding to the binocular vision camera is selected as the final ranging value, and in the long distance section, the ranging average value corresponding to the TOF camera is selected as the final ranging value.
A computer storage medium having stored therein at least one instruction loaded and executed by a processor to implement the ranging method based on a TOF camera fused with binocular vision data.
The ranging device based on the fusion of the TOF camera and the binocular vision data comprises a processor and a memory, wherein at least one instruction is stored in the memory, and the at least one instruction is loaded and executed by the processor to realize the ranging method based on the fusion of the TOF camera and the binocular vision data.
The beneficial effects are that:
The invention provides a distance measurement method based on the fusion of TOF camera and binocular vision data, wherein the TOF camera and the binocular vision distance measurement have advantages and disadvantages in different measurement ranges and measurement scenes, and the distance measurement is carried out on the same target parameter by two sensors, so that the advantages are complementary, and the distance measurement precision can be improved.
The invention provides a data fusion method, which comprises the steps of firstly processing two groups of data sets through a Gaussian model, removing a value with larger error, then carrying out self-adaptive weighted fusion, and obtaining an optimal value through distributing different weights. The two data fusion methods are used for advantage integration, so that the negative influence of uncertainty of a system, environmental interference and failure data on state estimation can be effectively reduced;
Most of the existing depth information fusion methods related to the TOF cameras need to generate the parallax images of the TOF cameras, the parallax images of the TOF depth images do not need to be generated, pixel-level fusion processing does not need to be carried out, a novel data fusion method is provided, algorithm efficiency is high, and processing time is fast.
Detailed Description
The invention provides a ranging method based on TOF camera and binocular vision data fusion, which fuses a TOF camera and a binocular vision ranging result through two fusion methods of Gaussian model processing and self-adaptive weighting, inhibits the influence of errors, expands the use condition of a sensor and improves the reliability and the accuracy. The invention is further described below with reference to the drawings and detailed description.
Detailed description of the embodiments first the present embodiment is described with reference to figure 1,
The present embodiment is a ranging method based on the integration of TOF camera and binocular vision data, and the ranging method based on the integration of TOF camera and binocular vision data includes ranging, first data optimization, second data optimization, and final estimated value output.
The ranging method based on the fusion of the TOF camera and the binocular vision data comprises the following steps:
And 1, building a binocular vision ranging platform, calibrating internal parameters and external parameters of left and right cameras of binocular vision, and obtaining a transformation relation between the internal parameters of the cameras and the two cameras.
As shown in fig. 2, a black-and-white checkered picture on a plane is first prepared, the left and right cameras shoot pictures at the same time, a plurality of groups of pictures are shot, the corner points of the pictures are detected through simulation software, and internal parameters and external parameters of the cameras, namely internal parameters and relative transformation relations of the left and right cameras, are solved.
The method comprises the steps of 2, building a TOF camera and binocular vision ranging platform for ranging a target, namely a measured object (6), wherein the TOF camera and binocular vision ranging platform comprises a camera support (1), a camera platform (2), a binocular vision right camera (3), a TOF camera (4) and a binocular vision left camera (5), the binocular vision right camera (3), the binocular vision left camera (5) and the TOF camera (4) are arranged on the camera platform, the centers of the TOF camera and the binocular vision left camera are on the same straight line, the binocular vision right camera (3) and the binocular vision left camera (5) are arranged on two sides of the TOF camera (4), and the camera platform (2) is arranged on the camera support (1) as shown in fig. 4.
The TOF camera and the left and right cameras of binocular vision are centered on a straight line, and the straight line is perpendicular to the connecting line of the platform and the measured object.
In the range L, the measuring positions with the interval distance k are close to the measured target, and the measured target is photographed and measured in sequence, so that a measured image is obtained, and L/k measuring positions are taken in total. At each measuring position, the TOF camera and binocular vision are measured n times (n is more than or equal to 20);
And 3, in order to measure the distance of a certain point on the target, a measured point is firstly selected on the target, the distance value of the point can be directly obtained on the TOF depth image, binocular vision ranging is required to calculate a parallax image through three-dimensional correction and three-dimensional matching (comprising four stages of preprocessing, cost calculation, dynamic programming and post processing) through a semi-global three-dimensional matching algorithm SGBM, and then the distance value of the measured point is obtained in the parallax image.
At this time, L/k measurement positions are shared in the whole range, so that the TOF camera and binocular vision form L/k groups of data respectively, each group of numbers has n distance values at each measurement position, and a distance measuring platform is not required to be moved when the measurement is carried out at the same distance measuring position;
The L/k measuring positions are used for respectively obtaining error-distance curves of TOF phase and binocular vision methods in a 0~L ranging range, and when the method is actually applied, the TOF and binocular vision methods only need to measure n (n is more than or equal to 20) times at a certain position.
And 4, processing the TOF camera and the binocular vision respectively, processing each group of data by using a Gaussian model, eliminating the distance value with overlarge deviation, namely, the distance value which does not meet the distance value in the step 4.2, selecting the distance value subjected to Gaussian model screening as a high probability distance value, selecting the high probability distance value, and calculating an average value to obtain the distance measurement optimal value.
Taking a certain group of data of a certain camera as an example, the specific steps are as follows:
step 4.1, the Gaussian distribution function of the random measurement value d is as follows:
Wherein sigma is a standard deviation, D is a random measurement value in n distance values Di, mu is a mathematical expected value, and m and sigma2 are the average value and the variance corresponding to n distance values Di respectively;
the mean m and variance σ2 of the measured data are:
Where Di is the i-th initial measurement, i=1, 2,..n. n is the total number of data;
Step 4.2, determining a selectable value range and a critical value of a Gaussian distribution function:
Where u is a lower threshold of the Gaussian distribution function, and is generally selected from 0.6-0.8. When the value of the Gaussian distribution function is smaller than u, the measured value is regarded as a small probability error value, and the measured value can be omitted;
And 4.3, selecting the ranging values Xi which are reserved after the Gaussian model screening from the ranging initial values, wherein the number of the ranging values Xi is r. Obtaining the distance measurement optimal value:
Wherein Xi is the i-th value meeting the requirements, i=1, 2,..r, r is the number meeting the requirements;
The Gaussian model solves the problem that an error value caused by a small probability event affects the overall ranging accuracy in the ranging process, and the ranging accuracy and the stability of the system are improved;
Determining distance of the distance measurement optimal value and corresponding distance errors based on the L/k measurement positions to respectively obtain error-distance curves of the TOF and binocular vision methods in the 0~L distance measurement range, and determining application ranges of the TOF camera and the binocular vision based on the error-distance curves of the TOF and binocular vision methods:
The error-distance curves of the TOF and binocular vision methods are drawn together, an intersection point corresponding to a distance minimum value and an intersection point corresponding to a distance maximum value in the two curves are recorded as a first distance threshold value and a second distance threshold value, a distance within the first distance threshold value is used as a short distance segment, a distance outside the second distance threshold value is used as a long distance segment, and a distance between the first distance threshold value and the second distance threshold value is used as an intermediate distance segment.
In this embodiment, the first distance threshold and the second distance threshold were found to be 1m and 3.5m, respectively.
Taking the camera with smaller error in the short distance section as a short distance section dominant camera;
Step 5, when in actual use, firstly judging the distance between the distance measurement optimal values obtained by the TOF method and the binocular method;
if the distance measurement optimal value distances of the two are located in the long-distance section, adopting a distance measurement average value corresponding to the advantage camera in the long-distance section as a final distance measurement value;
if the distance measurement optimal value distances of the two are positioned in the short-distance section, adopting a distance measurement average value corresponding to the advantage camera in the short-distance section as a final distance measurement value;
If any distance between the two distance measurement optimal values is located in the middle distance section, executing the step 6;
in the embodiment, in the short distance section of 0-1 m, the binocular vision camera is dominant, the average binocular vision ranging value is the last ranging value, and in the long distance section of more than 3.5m, the TOF camera is dominant, the average TOF camera ranging value is the last ranging value;
Step 6, in the middle distance section, the distance measurement results of the binocular vision and the TOF camera are unstable, and both the two can influence the distance measurement accuracy at a certain position due to system uncertainty, environmental interference and failure data, so that different weights are distributed to the TOF distance measurement average value and the binocular vision distance measurement average value, the distance measurement average values of the TOF camera and the binocular vision are multiplied by the weights respectively and then summed, and the sum is used as the final distance measurement value of the middle distance section as shown in fig. 3.
Taking a certain measurement position as an example, the specific steps are as follows:
In the step 6.1, the TOF camera is marked as p, the data after being screened by a Gaussian model is Xp, the total number of the data is r, and the average value isThe binocular vision camera is marked as q, the data after being screened by a Gaussian model is Xq, the total number of the binocular vision cameras is r, and the average value is
Step 6.2. In general, these measurements consist of the true signal X and the observed errors Vp and Vq, denoted Xp＝X+Vp and Xq＝X+Vq. The observed errors Vp and Vq can be regarded as zero-mean stationary noise. Based on this, at a certain measurement position, the variance of the data measured by the camera p is σ2＝E(Vp2), where E (·) is desired. Since the TOF and the binocular vision are mutually independent, the ranging process is not affected, so that the observation errors between the 2 cameras are uncorrelated, the average value of the observation errors is 0, and the observation errors are uncorrelated with the real signal. The formula for the cross correlation function Rpq between any camera p and q, and the autocorrelation function Rpp for camera p is as follows:
Rpq＝E(XpXq)＝E(X2)
Rpp＝E(XpXp)＝E(X2)+E(Vp2)
Step 6.3, since r data are obtained for both cameras after the gaussian model processing, the calculation formula of the autocorrelation and cross correlation functions is as follows:
wherein r is the number of sampling points, namely the number of data;
Step 6.4 variance of camera pAnd the variances of camera q are respectively:
And 6.5, solving an extremum theory according to a multiple function, wherein when the total root mean square error is minimum, the weighting factors Wp and Wq corresponding to p and q are respectively as follows:
and 6.5, calculating the following formula of the fused estimated value X:
Wherein, theAndAnd (5) screening the data mean value of the TOF camera and the binocular vision through a Gaussian model.
In summary, the invention provides a ranging method based on the fusion of TOF camera and binocular vision data, which fuses the TOF camera and the binocular vision ranging result through two fusion methods of Gaussian model processing and self-adaptive weighting, suppresses the influence of errors, enlarges the use condition of the camera, and improves the reliability and accuracy.
The second embodiment is as follows:
the embodiment is a computer storage medium, in which at least one instruction is stored, where the at least one instruction is loaded and executed by a processor to implement the ranging method based on the integration of a TOF camera and binocular vision data.
It should be understood that the instructions comprise a computer program product, software, or computerized method corresponding to any of the methods described herein, which instructions may be used to program a computer system, or other electronic device. Computer storage media may include readable media having instructions stored thereon, and may include, but is not limited to, magnetic storage media, optical storage media, magneto-optical storage media including read-only memory ROM, random access memory RAM, erasable programmable memory (e.g., EPROM and EEPROM), and flash memory layers, or other types of media suitable for storing electronic instructions.
And a third specific embodiment:
The embodiment is a distance measuring device based on the integration of a TOF camera and binocular vision data, the device comprises a processor and a memory, and it should be understood that the device comprising any of the devices comprising the processor and the memory described in the invention can also comprise other units and modules for performing display, interaction, processing, control and other functions through signals or instructions;
the memory stores at least one instruction, and the at least one instruction is loaded and executed by the processor to realize the ranging method based on the fusion of the TOF camera and the binocular vision data.
The above examples of the present invention are only for describing the calculation model and calculation flow of the present invention in detail, and are not limiting of the embodiments of the present invention. Other variations and modifications of the above description will be apparent to those of ordinary skill in the art, and it is not intended to be exhaustive of all embodiments, all of which are within the scope of the invention.