CN106885523B - A kind of vehicle route tracking error vision measurement optimization method - Google Patents
A kind of vehicle route tracking error vision measurement optimization method Download PDFInfo
- Publication number
- CN106885523B CN106885523B CN201710171955.7A CN201710171955A CN106885523B CN 106885523 B CN106885523 B CN 106885523B CN 201710171955 A CN201710171955 A CN 201710171955A CN 106885523 B CN106885523 B CN 106885523B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- point
- following
- monocular
- distance
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 67
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000005457 optimization Methods 0.000 title claims abstract description 8
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 15
- 238000000354 decomposition reaction Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 8
- 238000007781 pre-processing Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- 230000009977 dual effect Effects 0.000 abstract description 2
- 238000002156 mixing Methods 0.000 abstract 1
- 238000000691 measurement method Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 102100037651 AP-2 complex subunit sigma Human genes 0.000 description 1
- 101000806914 Homo sapiens AP-2 complex subunit sigma Proteins 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/02—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness
- G01B11/03—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness by measuring coordinates of points
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
- Measurement Of Optical Distance (AREA)
Abstract
The present invention relates to a kind of vehicle route tracking error vision measurement optimization methods.It first passes through vehicle towing point monocular camera monocular indirect measurement systems acquisition vehicle and follows a little side offset distance y relative to vehicle towing point driving path；Pass through vehicle towing point monocular camera again and follow the direct measuring system of the binocular of a monocular camera, obtains vehicle and follow a little side offset distance y relative to vehicle towing point driving pathDC；The dual system blending algorithm based on trackless Kalman filtering is finally carried out, follows the side offset distance y a little relative to vehicle towing point driving path to be modified on the vehicle of monocular indirect measurement systems output.The present invention can obtain more accurate vehicle tail side offset distance measured value in bigger measurement range, this measured value can be used as the input of rear axle active front steering system controller, to improve the passability of long wheelbase vehicle.
Description
Technical Field
The invention relates to the field of vehicle control, in particular to a vehicle path following error visual measurement optimization method.
Background
Vehicles or trains with long wheelbase, such as buses, commercial vehicles, heavy vehicles and trailer vehicles, have higher mass centers and longer body lengths, so that the controllability and the low-speed passing performance are poorer. Under low speed turning conditions, such a vehicle rear portion may be laterally offset from the vehicle front portion to the inside of the turning radius. The longer the vehicle body length, the smaller the turning radius, the greater the lateral offset distance, and the poorer the vehicle trafficability. To improve the low speed safety performance of such vehicles, the application of some rear axle active steering systems may allow the entire vehicle to better follow the driver's desired travel path. The lateral offset distance of the tail part of the vehicle relative to the front part of the vehicle is an important index for measuring the performance of the system, the parameter is difficult to directly obtain through measurement of a sensing system, and the parameter is usually estimated through a vehicle kinematic model according to state observation of a vehicle course angle, a yaw rate and the like.
The partial vision system can derive the lateral offset distance of the tail of the vehicle by a direct measurement method or an indirect measurement method according to the road surface characteristics. The indirect measurement method can measure the speed, the yaw angle and the yaw velocity of a vehicle traction point according to the monocular camera, and calculate the path of a vehicle following point by utilizing a traction point position shift register and vehicle geometric parameters, so that the lateral offset distance of the tail of the vehicle is obtained. The method has the advantages that the measuring range is not limited by the lateral offset distance, but the precision is easily influenced by the accumulated error of the integral of the yaw angular velocity. The direct measurement method can directly determine the lateral offset distance between the traction point camera and the following point camera according to the characteristic matching of the driving path of the traction point camera and the following point camera. The method has high precision, but the measuring range is easily influenced by the overlapping range of the visual fields of the two cameras. For the calculation method based on the inertial sensor (gyroscope), the following measurement error of the tail path of the vehicle is large under the working conditions of a smooth road surface, the existence of a longitudinal slope and the existence of a lateral slope.
Accurately measuring the lateral offset distance of the tail of the vehicle relative to the front of the vehicle is of great importance for the application of rear axle active steering systems.
Disclosure of Invention
In order to solve the technical problems, the invention provides a vehicle path following error vision measurement optimization method, which combines two vision sensing-based vehicle tail lateral offset distance measurement methods by using a sensor fusion technology, measures the lateral offset distance of a vehicle following point relative to a vehicle traction point driving path in real time, can obtain a more accurate vehicle tail lateral offset distance measurement value in a larger measurement range, and can be used as a rear axle active steering system controller input to improve the trafficability of a long-wheelbase vehicle.
The technical problem of the invention is mainly solved by the following technical scheme: the invention comprises the following steps:
①, acquiring the lateral offset distance y of the vehicle following point relative to the driving path of the vehicle traction point through the monocular indirect measurement system of the vehicle traction point monocular camera;
② through the binocular direct measurement system of the vehicle traction point monocular camera and the following point monocular camera, the lateral offset distance y of the vehicle following point relative to the vehicle traction point driving path is obtainedDC；
③, a dual-system fusion algorithm based on the trackless Kalman filtering is carried out, and the lateral offset distance y of the vehicle following point output by the monocular indirect measurement system relative to the running path of the vehicle traction point is corrected.
By utilizing an information fusion technology, the measuring method of the monocular indirect measuring system and the binocular direct measuring system is combined, the measuring result is corrected through the dual-system fusion algorithm based on the trackless Kalman filtering, a more accurate vehicle tail lateral offset distance measuring value can be obtained in a larger measuring range, and the measuring value can be input as a rear axle active steering system controller so as to improve the trafficability of the long-wheelbase vehicle.
Preferably, the step ① includes the following steps:
(11) preprocessing an image acquired by a towing point monocular camera, extracting FAST (FAST fourier transform) feature points, generating SURF feature description vectors, performing feature matching on the SURF feature description vectors extracted from two adjacent frames by using a FLANN (FLANN feature matching library), selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
(12) performing singular value decomposition on the calculated homographic matrix to obtain a planeThe translation information is used for calculating a traction point deflection angle βfAnd absolute velocity vfThe yaw angle psi of the traction point is calculated by using the rotation informationf；
(13) According to the vehicle kinematic model of the horizontal swing plane, the vehicle running distance S is calculated according to the following formulafAnd tow point location (X)f，Yf) And following point position (X)r，Yr) Global position information of (2):
Sf＝∫vf dt
γf＝ψf+βf
Xf＝∫vf cos(γf)dt
Yf＝∫vf sin(γf)dt
Xr＝Xf-l cos(ψf)
Yr＝Yf-l sin(ψf)
wherein, γfIs a course angle, and l is the distance between the traction point and the following point;
(14) SURF characteristics, towing point position information and vehicle driving distance S extracted by the towing point monocular camerafAnd storing the data into a road characteristic memory buffer area, reading the global coordinate of the current position of the following point corresponding to the running of the traction point according to the distance l between the traction point and the following point, carrying out coordinate transformation, converting the coordinate into a vehicle coordinate system at the current moment, and calculating the lateral offset distance y of the vehicle following point relative to the running path of the vehicle traction point.
Preferably, the step ② includes the following steps:
(21) preprocessing an image acquired by a following point monocular camera, extracting FAST (FAST fourier transform) feature points, generating SURF feature description vectors, matching the generated SURF feature vectors with SURF features read from a road feature memory buffer area by using a FLANN (FLANN feature matching) library, selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
(22) performing singular value decomposition on the calculated homograph matrix to obtain translation information, converting the translation information from a camera coordinate system to a vehicle coordinate system, wherein a Y-direction component of a following point is a lateral offset distance Y of the vehicle following point relative to a vehicle traction point driving pathDCThe X-component of the following point is used to correct the distance l between the tow point and the following point.
The Y-direction component is a lateral component of a following point under the vehicle coordinate system, and the X-direction component is a longitudinal component of the following point under the vehicle coordinate system.
Preferably, the step ③ is performed when the binocular direct measuring system measures the lateral offset y of the vehicle following point relative to the vehicle towing point travel pathDCWhen the lateral deviation distance is smaller than the set value, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, so that the lateral deviation distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral deviation distance z is obtainedk(ii) a The UKF trackless Kalman filtering updating and measurement equation used is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd a tow point vehicle speed VSC，k；
Quantity of state
Input quantity
The state quantity refers to an objectively existing vehicle state, such as a vehicle speed, and does not refer to a measurement value obtained by a specific sensor, and the state quantity refers to a state of the vehicle and is estimated by an observer from an input quantity and an observation model. The input quantity refers to a measured value obtained from a sensor, such as a vehicle speed value measured by a wheel speed sensor, and the subscript SC of the input quantity indicates that the measured value originated from a monocular vision system.
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kProcess noise, obtainable by manual adjustment;
covariance matrix PkUpdating:
wherein λ is α2(L+κ)-L
α, kappa and L are UKF parameters, and the default values are 0.01, 2, 0 and 4 respectively;
defining the calculation process of the lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point, which is described in the step (13) and the step (14), as an observation equation F, the corrected lateral offset distance is represented by the following formula:
zk＝F(xk)
and (3) observation updating:
and (3) Kalman gain K calculation:
state and covariance correction:
xk+1＝x(k+1|k)+K(yDC-z(k+1|k))
Pk+1＝P(k+1|k)-K Pzz，(k+1|k)KT
wherein, yDCThe lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement system.
Preferably, step ③ includes measuring the lateral offset y of the vehicle tracking point relative to the vehicle tow point travel path as measured by the binocular direct measurement systemDCWhen the lateral deviation distance is larger than the set value, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, the lateral deviation distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral deviation distance z is obtainedkThe correction process is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd a tow point vehicle speed VSC，k；
Quantity of state
Input quantity
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kIs process noise;
substituting each parameter after the state updating into an observation equation F to obtain a corrected lateral offset distance:
zk＝F(xk)。
the set value used for judgment in the technical scheme is the upper limit of the measurement range of the binocular direct measurement system, is related to the internal and external parameters of the camera, and is obtained according to experimental calibration. If the installation height of the camera is 0.8 m and the focal length is 3mm, the set value is 0.3 m.
The invention has the beneficial effects that: by utilizing the information fusion method, the problems existing in the application process of the single vision system can be effectively eliminated. Compared with a monocular vision system, the integrated vision system effectively eliminates the integral error of the yaw angular velocity, and the measurement precision is higher; compared with a binocular vision system, the fused vision system can effectively estimate the lateral offset distance of a large measurement value by utilizing the filtered vehicle state, and the measurement range is larger. The fused vision system can effectively measure the lateral offset distance of the long-wheelbase vehicle tail under the working condition of the existence of a smooth road surface, a longitudinal slope or a lateral slope, and can obtain a more accurate measured value of the lateral offset distance of the vehicle tail in a larger measuring range, so that the low-speed passing performance of the long-wheelbase vehicle is accurately described, and the measured value can be used as the input of a rear axle active steering system controller to improve the passing performance of the vehicle.
Drawings
FIG. 1 is a general flow chart of the algorithm of the present invention.
FIG. 2 is a schematic top view of the vehicle under low speed cornering conditions in accordance with the present invention.
FIG. 3 is a flowchart of the monocular indirect measurement system algorithm of the tow point monocular camera of the present invention.
Fig. 4 is a flow chart of the algorithm of the binocular direct measurement system of the present invention.
In the figure, 1 is a vehicle, 2 is a vehicle traction point, and 3 is a vehicle following point.
Detailed Description
The technical scheme of the invention is further specifically described by the following embodiments and the accompanying drawings.
Example (b): in the method for optimizing the vehicle path following error vision measurement according to the embodiment, an algorithm flow general diagram is shown in fig. 1, and the method includes the following steps:
①, acquiring the lateral offset distance y of the vehicle following point relative to the driving path of the vehicle traction point through the monocular indirect measurement system of the vehicle traction point monocular camera;
② Dual of monocular camera and follower point monocular camera via vehicle tow pointA direct vision measuring system for obtaining the lateral offset distance y of the vehicle following point relative to the running path of the vehicle traction pointDC；
③, a dual-system fusion algorithm based on the trackless Kalman filtering is carried out, and the lateral offset distance y of the vehicle following point output by the monocular indirect measurement system relative to the running path of the vehicle traction point is corrected.
The images acquired by the two monocular cameras are the input of the method, and the lateral offset distance of the vehicle following point 3 (vehicle tail) relative to the driving path of the vehicle traction point 2 (vehicle front) is the output of the method. As shown in fig. 2, the towing point monocular camera is installed at the front end (usually at the towing point position) of the vehicle 1, the following point monocular camera is installed at the rear end (usually at the following point position) of the vehicle, both monocular cameras are installed in the direction of the road surface vertically downward, and the ground clearance of the monocular camera in this embodiment is about 0.5 m. The dashed line in fig. 2 is the vehicle tow point travel path, i is the distance between the tow point and the following point, and D is the lateral offset distance of the vehicle following point relative to the vehicle tow point travel path that actually occurs at this time.
The optimization method comprises the following specific steps:
a monocular indirect measurement system, as shown in fig. 3:
(11) preprocessing (graying and distortion removal) an image acquired by a traction point monocular camera, extracting FAST (FAST Fourier transform) feature points, extracting plane features of the front surface or the side surface of a vehicle, generating SURF feature description vectors, performing feature matching on the SURF feature description vectors extracted from two adjacent frames by using a FLANN (hybrid automatic repeat network) feature matching library, selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
through M cycles, randomly selecting 4 matching features, calculating a homograph matrix, scoring the remaining features according to the matrix matching result, if the pixel point matching distance is smaller than a certain threshold value M, selecting the homograph matrix with the highest score, and recalculating to obtain the final homograph matrix by using all correct matching feature pairs corresponding to the homograph matrix if the pixel point matching distance is smaller than the threshold value M; the middle cycle number M and the distance threshold value M are preset values;
(12) performing singular value decomposition on the calculated homograph matrix to obtain translation information and rotation information, and calculating a traction point deflection angle β by using the translation informationfAnd absolute velocity vfThe yaw angle psi of the traction point is calculated by using the rotation informationf；
For the calculated homographic matrixSingular value decomposition is carried out to obtain camera translation information T and rotation information R; order:
∑＝diag(σ1，σ2，σ3)，V＝[v1，v2，v3]
this is toThe singular value decomposition result, sigma is a diagonal matrix, V is a vector, and sigma 1, sigma 2, sigma 3, V1, V2 and V3 are corresponding numerical values;
the above singular value decomposition theoretically has four sets of solutions as follows:
solution 1:
solution 2:
solution 3:
solution 4:
selecting the set of solutions corresponding to the normal vector N with the direction closest to [0, 0, 1 ];
by the formula:calculating to obtain the absolute value V of the real-time vehicle speed Vf，vfNamely translation information;
by the formula:calculating a real-time slip angle β for the vehiclef；
By the formula:calculating the yaw rate Ψ of the vehiclef；
In the formula: t isxThe real-time translation speed of the monocular camera is the traction point in the x-axis direction; t isyThe real-time translation speed of the monocular camera is the traction point in the y-axis direction; rzThe rotation component of the traction point monocular camera around the z axis is obtained; t is tsIs a unit time step length;
(13) according to the vehicle kinematic model of the horizontal swing plane, the vehicle running distance S is calculated according to the following formulafAnd tow point location (X)f，Yf) And following point position (X)r，Yr) Global position information of (2):
Sf＝∫vf dt
γf＝ψf+βf
Xf＝∫vf cos(γf)dt
Yf＝∫vf sin(γf)dt
Xr＝Xf-l cos(ψf)
Yr＝Yf-l sin(ψf)
wherein, γfIs a course angle, and l is the distance between the traction point and the following point;
(14) SURF characteristics, towing point position information and vehicle driving distance S extracted by the towing point monocular camerafAnd storing the data into a road characteristic memory buffer area, reading the global coordinate of the current position of the following point corresponding to the running of the traction point according to the distance l between the traction point and the following point, carrying out coordinate transformation, converting the coordinate into a vehicle coordinate system at the current moment, and calculating the lateral offset distance y of the vehicle following point relative to the running path of the vehicle traction point.
Secondly, a binocular direct measurement system, as shown in fig. 4:
(21) preprocessing (graying and distortion removal) an image acquired by a following point monocular camera, extracting FAST (FAST fourier transform) feature points, generating SURF feature description vectors, matching the generated SURF feature vectors with SURF features read from a road feature memory buffer area by using a FLANN (sparse least squares) feature matching library, namely performing feature matching on an image at the tail of a vehicle at the current moment and a vehicle front-end image at a corresponding position stored in the memory by using the FLANN feature matching library, selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
(22) performing singular value decomposition on the calculated homograph matrix to obtain translation information, converting the translation information from a camera coordinate system to a vehicle coordinate system, wherein a Y-direction component of a following point is a lateral offset distance Y of the vehicle following point relative to a vehicle traction point driving pathDCThe X-component of the following point is used to correct the distance l between the tow point and the following point.
Thirdly, a dual-system fusion algorithm based on trackless Kalman filtering:
when the lateral deviation distance y of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement systemDCWhen the lateral deviation distance is smaller than the set value and is in a first mode state, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, so that the lateral deviation distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral deviation distance z is obtainedk(ii) a The UKF trackless Kalman filtering updating and measurement equation used is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd a tow point vehicle speed VSC，k；
Quantity of state
Input quantity
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kIs process noise;
covariance matrix PkUpdating:
wherein λ is α2(L+κ)-L
α, kappa and L are UKF parameters, and the default values are 0.01, 2, 0 and 4 respectively;
defining the calculation process of the lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point, which is described in the step (13) and the step (14), as an observation equation F, the corrected lateral offset distance is represented by the following formula:
zk＝F(xk)
and (3) observation updating:
and (3) Kalman gain K calculation:
state and covariance correction:
xk+1＝x(k+1|k)+K(yDC-z(k+1|k))
Pk+1＝P(k+1|k)-K Pzz，(k+1|k)KT
wherein, yDCThe lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement system.
When the lateral deviation distance y of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement systemDCWhen the measured value is larger than the set value and is in a second mode state, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, the lateral offset distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral offset distance z is obtainedkThe correction process is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd a tow point vehicle speed VSC，k；
Quantity of state
Input quantity
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kIs process noise;
substituting each parameter after the state updating into an observation equation F to obtain a corrected lateral offset distance:
zk＝F(xk)。
namely, in the second mode state, Kalman gain calculation and state correction are not carried out in the optimization process until the vehicle is restored to the first mode state again.
The purpose of the method is to offset the measured lateral offset z by a distance zkCloser to the actually occurring lateral offset distance D.
Noun interpretation of related art terms:
FAST: the FAST feature detection algorithm is derived from the definition of kernel, which detects a circle of pixel values around a candidate feature point based on the gray value of the image around the feature point, and if there are enough pixel points in the neighborhood around the candidate point and the gray value difference of the candidate point is large enough, the candidate point is considered as a feature point. The feature point detection is a recognized and rapid feature point detection method, and the feature points can be obtained only by utilizing the comparison information of surrounding pixels, so that the method is simple and effective. The method is mostly used for corner detection.
SURF: a feature description algorithm with scale and rotation feature invariance is strong in description and high in speed.
FLANN: a fast approximate nearest neighbor search function library automatically selects the optimal algorithm of two approximate nearest neighbor algorithms.
RANSAC: a robust regression method for excluding mismatched feature information.
Homograph: projection transformation matrix of corresponding matching characteristic points in two images
SIFT: the Scale Invariant Feature Transform (SIFT) algorithm is a method of feature extraction. The extreme point is searched in the scale space, the position, scale and rotation invariants of the extreme point are extracted, the extreme point is used as a feature point, and a neighborhood of the feature point is used for generating a feature vector. The SIFT algorithm is quite tolerant to light, noise, and small viewing angle changes, and has a high recognition phase rate for partially occluded objects.
According to the invention, a sensor fusion technology is utilized, two vehicle tail lateral offset distance measuring methods based on visual sensing are combined, and compared with the method of independently applying one visual system, the method can obtain more accurate vehicle tail lateral offset distance in a larger measuring range. The method has the advantages of small computation amount, strong transportability, good real-time performance and no need of introducing additional hardware investment.
Claims (2)
1. A vehicle path following error visual measurement optimization method is characterized by comprising the following steps:
①, acquiring the lateral offset distance y of the vehicle following point relative to the driving path of the vehicle traction point through the monocular indirect measurement system of the vehicle traction point monocular camera;
the step ① includes the following steps:
(11) preprocessing an image acquired by a towing point monocular camera, extracting FAST (FAST fourier transform) feature points, generating SURF feature description vectors, performing feature matching on the SURF feature description vectors extracted from two adjacent frames by using a FLANN (FLANN feature matching library), selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
(12) performing singular value decomposition on the calculated homograph matrix to obtain translation information and rotation information, and calculating a traction point deflection angle β by using the translation informationfAnd absolute velocity vfThe yaw angle psi of the towing point is calculated by using the rotation informationf；
(13) According to the vehicle kinematic model of the horizontal swing plane, the vehicle running distance S is calculated according to the following formulafAnd tow point location (X)f，Yf) And following point position (X)r，Yr) Global position information of (2):
Sf＝∫vfdt
γf＝ψf+βf
Xf＝∫vfcos(γf)dt
Yf＝∫vfsin(γf)dt
Xr＝Xf-l cos(ψf)
Yr＝Yf-l sin(ψf)
wherein, γfIs a course angle, and l is the distance between the traction point and the following point;
(14) SURF characteristics, towing point position information and vehicle driving distance S extracted by the towing point monocular camerafStoring the data into a road characteristic memory buffer area, reading the global coordinate of the current position of the following point corresponding to the driving of the traction point according to the distance l between the traction point and the following point, carrying out coordinate transformation, converting the coordinate into a vehicle coordinate system at the current moment, and calculating the lateral offset distance y of the vehicle following point relative to the driving path of the vehicle traction point;
② through the binocular direct measurement system of the vehicle traction point monocular camera and the following point monocular camera, the lateral offset distance y of the vehicle following point relative to the vehicle traction point driving path is obtainedDC；
The step ② includes the following steps:
(21) preprocessing an image acquired by a following point monocular camera, extracting FAST (FAST fourier transform) feature points, generating SURF feature description vectors, matching the generated SURF feature vectors with SURF features read from a road feature memory buffer area by using a FLANN (FLANN feature matching) library, selecting correct matching samples by using RANSAC (random sample consensus), and calculating a homograph matrix;
(22) performing singular value decomposition on the calculated homograph matrix to obtain translation information, converting the translation information from a camera coordinate system to a vehicle coordinate system, wherein a Y-direction component of a following point is a lateral offset distance Y of the vehicle following point relative to a vehicle traction point driving pathDCThe X-direction component of the following point is used for correcting the distance l between the traction point and the following point;
③, performing a dual-system fusion algorithm based on trackless Kalman filtering, and correcting the lateral offset distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point;
the step ③ is:
when the lateral deviation distance y of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement systemDCWhen the lateral deviation distance is smaller than the set value, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, so that the lateral deviation distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral deviation distance z is obtainedk(ii) a The UKF trackless Kalman filtering updating and measurement equation used is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd tractionPoint vehicle speed VSC，k；
Quantity of state xk：
Input quantity uk：
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kIs process noise;
covariance matrix PkUpdating:
wherein,
α，β，l is a UKF parameter, and the default values are 0.01, 2, 0 and 4 respectively;
defining the calculation process of the lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point, which is described in the step (13) and the step (14), as an observation equation F, the corrected lateral offset distance is represented by the following formula:
zk＝F(xk)
and (3) observation updating:
and (3) Kalman gain K calculation:
state and covariance correction:
xk+1＝x(k+1|k)+K(yDC-z(k+1|k))
Pk+1＝P(k+1|k)-K Pzz，(k+1|k)KT
wherein, yDCThe lateral offset distance of the vehicle following point relative to the driving path of the vehicle traction point is measured by the binocular direct measurement system.
2. The method of claim 1, wherein the step ③ comprises measuring the lateral offset y of the vehicle tracking point relative to the vehicle tow point travel path as measured by the binocular direct measurement systemDCWhen the lateral deviation distance is larger than the set value, the yaw angle measurement error of the monocular indirect measurement system is predicted by using the output of the binocular direct measurement system, the lateral deviation distance y of the vehicle following point output by the monocular indirect measurement system relative to the driving path of the vehicle traction point is corrected, and the corrected lateral deviation distance z is obtainedkThe correction process is as follows:
the state quantity including the yaw angle psi of the vehiclekYaw rate errorTow point slip angle βkAnd velocity Vk(ii) a The input quantity is provided by a traction point monocular indirect measuring system, and the input quantity comprises the yaw angular speed of the vehicleTow point slip angle βSC，kAnd a tow point vehicle speed VSC，k；
Quantity of state xk：
Input quantity uk：
And (3) updating the state:
β(k+1|k)＝βSC，k+w3，k
V(k+1|k)＝VSC，k+w4，k
where Δ t is the sampling time interval, wi，kIs process noise;
substituting each parameter after the state updating into an observation equation F to obtain a corrected lateral offset distance:
zk＝F(xk)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710171955.7A CN106885523B (en) | 2017-03-21 | 2017-03-21 | A kind of vehicle route tracking error vision measurement optimization method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710171955.7A CN106885523B (en) | 2017-03-21 | 2017-03-21 | A kind of vehicle route tracking error vision measurement optimization method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106885523A CN106885523A (en) | 2017-06-23 |
| CN106885523B true CN106885523B (en) | 2019-03-08 |
Family
ID=59180874
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710171955.7A Active CN106885523B (en) | 2017-03-21 | 2017-03-21 | A kind of vehicle route tracking error vision measurement optimization method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106885523B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111506126B (en) * | 2020-03-17 | 2023-06-20 | 陕西雷神智能装备有限公司 | Multidirectional movement equipment drive-by-wire signal generating device and drive-by-wire movement equipment |
| CN111661048B (en) * | 2020-06-10 | 2023-04-07 | 中车株洲电力机车有限公司 | Multi-articulated vehicle and track following control method and system thereof |
| CN112622903B (en) * | 2020-10-29 | 2022-03-08 | 东北大学秦皇岛分校 | Longitudinal and transverse control method for autonomous vehicle in vehicle following driving environment |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001357403A (en) * | 2000-06-14 | 2001-12-26 | Public Works Research Institute Ministry Of Land Infrastructure & Transport | Method and device for tracking vehicle |
| CN1940591A (en) * | 2005-09-26 | 2007-04-04 | 通用汽车环球科技运作公司 | System and method of target tracking using sensor fusion |
| CN103373354A (en) * | 2012-04-30 | 2013-10-30 | 通用汽车环球科技运作有限责任公司 | Vehicle turn assist system and method |
| KR20140098609A (en) * | 2013-01-31 | 2014-08-08 | 한국기술교육대학교 산학협력단 | Road condition estimating system and a method for estimating a road condition using the same |
| CN106295651A (en) * | 2016-07-25 | 2017-01-04 | 浙江零跑科技有限公司 | A kind of vehicle route follower method based on double vertical view cameras Yu rear axle steering |
| CN106327433A (en) * | 2016-08-01 | 2017-01-11 | 浙江零跑科技有限公司 | Monocular downward view camera and rear axle steering-based vehicle path following method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3521860B2 (en) * | 2000-10-02 | 2004-04-26 | 日産自動車株式会社 | Vehicle travel path recognition device |
-
2017
- 2017-03-21 CN CN201710171955.7A patent/CN106885523B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001357403A (en) * | 2000-06-14 | 2001-12-26 | Public Works Research Institute Ministry Of Land Infrastructure & Transport | Method and device for tracking vehicle |
| CN1940591A (en) * | 2005-09-26 | 2007-04-04 | 通用汽车环球科技运作公司 | System and method of target tracking using sensor fusion |
| CN103373354A (en) * | 2012-04-30 | 2013-10-30 | 通用汽车环球科技运作有限责任公司 | Vehicle turn assist system and method |
| KR20140098609A (en) * | 2013-01-31 | 2014-08-08 | 한국기술교육대학교 산학협력단 | Road condition estimating system and a method for estimating a road condition using the same |
| CN106295651A (en) * | 2016-07-25 | 2017-01-04 | 浙江零跑科技有限公司 | A kind of vehicle route follower method based on double vertical view cameras Yu rear axle steering |
| CN106327433A (en) * | 2016-08-01 | 2017-01-11 | 浙江零跑科技有限公司 | Monocular downward view camera and rear axle steering-based vehicle path following method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106885523A (en) | 2017-06-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106096525B (en) | A kind of compound lane recognition system and method | |
| CN103630122B (en) | Monocular vision lane line detection method and distance measurement method thereof | |
| CN106295560B (en) | Lane keeping method based on vehicle-mounted binocular camera and segmented PID control | |
| CN110745140B (en) | Vehicle lane change early warning method based on continuous image constraint pose estimation | |
| CN109435942A (en) | A kind of parking stall line parking stall recognition methods and device based on information fusion | |
| CN106156723B (en) | A kind of crossing fine positioning method of view-based access control model | |
| CN106885523B (en) | A kind of vehicle route tracking error vision measurement optimization method | |
| CN103593671B (en) | The wide-range lane line visible detection method worked in coordination with based on three video cameras | |
| CN105678787A (en) | Heavy-duty lorry driving barrier detection and tracking method based on binocular fisheye camera | |
| EP3009789A1 (en) | Monocular-motion-stereo distance estimation method, and monocular-motion-stereo distance estimation apparatus | |
| EP2126843A2 (en) | Method and system for video-based road lane curvature measurement | |
| CN103196418A (en) | Measuring method of vehicle distance at curves | |
| CN107389084A (en) | Planning driving path planing method and storage medium | |
| WO2018179281A1 (en) | Object detection device and vehicle | |
| CN108108667A (en) | A kind of front vehicles fast ranging method based on narrow baseline binocular vision | |
| CN107796373A (en) | A kind of distance-finding method of the front vehicles monocular vision based on track plane geometry model-driven | |
| CN114998452A (en) | On-line calibration method and system for vehicle-mounted camera | |
| CN105300390B (en) | The determination method and device of obstructing objects movement locus | |
| CN106327433B (en) | A kind of vehicle route follower method based on single vertical view camera and rear axle steering | |
| CN106295651B (en) | A kind of vehicle route follower methods based on double vertical view cameras and rear axle steering | |
| CN113221739B (en) | Monocular vision-based vehicle distance measuring method | |
| CN106408589A (en) | Vehicle-mounted overlooking camera based vehicle movement measurement method | |
| CN105787455A (en) | Lane-keeping control algorithm introducing image virtual guide line | |
| CN117746357A (en) | Lane line identification method and device and electronic equipment | |
| CN116878542A (en) | Laser SLAM method for inhibiting height drift of odometer |
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 | ||
| CP01 | Change in the name or title of a patent holder | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 6 / F, Xintu building, 451 Internet of things street, Binjiang District, Hangzhou City, Zhejiang Province, 310051 Patentee after: Zhejiang Zero run Technology Co.,Ltd. Address before: 6 / F, Xintu building, 451 Internet of things street, Binjiang District, Hangzhou City, Zhejiang Province, 310051 Patentee before: ZHEJIANG LEAPMOTOR TECHNOLOGY Co.,Ltd. |