CN116252581A - System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition - Google Patents
System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition Download PDFInfo
- Publication number
- CN116252581A CN116252581A CN202310249330.3A CN202310249330A CN116252581A CN 116252581 A CN116252581 A CN 116252581A CN 202310249330 A CN202310249330 A CN 202310249330A CN 116252581 A CN116252581 A CN 116252581A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- vehicle body
- vertical
- module
- speed
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 15
- 230000001133 acceleration Effects 0.000 claims abstract description 55
- 238000004364 calculation method Methods 0.000 claims description 54
- 238000006073 displacement reaction Methods 0.000 claims description 45
- 238000001914 filtration Methods 0.000 claims description 31
- 238000012937 correction Methods 0.000 claims description 16
- 238000000605 extraction Methods 0.000 claims description 13
- 239000000284 extract Substances 0.000 claims description 3
- 239000000725 suspension Substances 0.000 abstract description 8
- 230000000007 visual effect Effects 0.000 abstract description 2
- 230000036544 posture Effects 0.000 description 38
- 239000011159 matrix material Substances 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
- B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
- B60G17/018—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the use of a specific signal treatment or control method
- B60G17/0182—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the use of a specific signal treatment or control method involving parameter estimation, e.g. observer, Kalman filter
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
- B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
- B60G17/016—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by their responsiveness, when the vehicle is travelling, to specific motion, a specific condition, or driver input
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
- B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
- B60G17/019—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the type of sensor or the arrangement thereof
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G17/00—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load
- B60G17/015—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements
- B60G17/019—Resilient suspensions having means for adjusting the spring or vibration-damper characteristics, for regulating the distance between a supporting surface and a sprung part of vehicle or for locking suspension during use to meet varying vehicular or surface conditions, e.g. due to speed or load the regulating means comprising electric or electronic elements characterised by the type of sensor or the arrangement thereof
- B60G17/01933—Velocity, e.g. relative velocity-displacement sensors
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
- B60G2400/05—Attitude
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
- B60G2400/20—Speed
- B60G2400/204—Vehicle speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60G—VEHICLE SUSPENSION ARRANGEMENTS
- B60G2400/00—Indexing codes relating to detected, measured or calculated conditions or factors
- B60G2400/80—Exterior conditions
- B60G2400/82—Ground surface
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Image Analysis (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention relates to a system and a method for estimating vertical and pitching motion information of a vehicle body under a straight running condition, wherein the system comprises an image information capturing module for capturing road information in front of a vehicle, a vehicle speed estimating module for acquiring a running speed, a vehicle body vertical position and pitching angle estimating module for estimating a vehicle body vertical position and pitching angle, and a vehicle body vertical information and pitching angle information estimating and displaying module; the vehicle body vertical information and pitching angle information estimation and display module is used for calculating the vehicle vertical speed, the vehicle vertical acceleration, the pitching angle speed and the pitching angle acceleration, and displaying the vehicle vertical speed, the vertical acceleration, the pitching angle speed and the pitching angle acceleration on the central control display. According to the invention, the visual sensor is utilized to obtain the gesture of the vehicle, the vertical height, the vertical speed, the vertical acceleration, the pitching angle speed and the pitching angle acceleration of the vehicle body can be accurately measured in real time without adding a vertical acceleration sensor and a corner sensor of the vehicle, and information reference is provided for the control of the active suspension and the semi-active suspension of the vehicle.
Description
Technical Field
The invention belongs to the technical field of intelligent sensing and control of automobiles, and particularly relates to a monocular vision-based estimation system and an estimation method for vertical and pitching motion information of a vehicle body under a straight running condition.
Background
With the development of computer technology, instrument technology and electronic control technology, intelligent automobiles gradually enter the line of sight of people. More and more advanced sensors are used in smart cars to provide more information to the vehicle. However, the more sensors employed, the more effective information the intelligent vehicle can obtain, and the higher the vehicle cost. How to obtain as much effective information as possible without adding sensors is one of the key points of current intelligent automobile research.
In addition to sensing environmental information, another important use of vehicle sensors is for estimating vehicle driving conditions and dynamics. The vertical height and the pitching angle of the automobile are important parameters for controlling the active suspension and the semi-active suspension of the automobile, and influence the safety, smoothness and operation stability of the automobile. However, the inertial sensor equipped on the current mass production vehicle can only measure the acceleration of the x and y axes and the yaw rate of the z axis of the vehicle, and other sensors are added to measure the vertical height and the pitching angle of the vehicle, which increases the cost of the vehicle. The camera is a sensor element frequently adopted by the current intelligent automobile, and if an estimation system and an estimation method for the vertical height information and the pitching angle information of the automobile estimated by the vehicle-mounted camera can be established, the camera is very important for controlling the active suspension of the automobile and the smoothness, the maneuverability, the comfort and the safety of the automobile.
Disclosure of Invention
The invention aims to provide a monocular vision-based system for estimating vertical and pitching motion information of a vehicle body under a straight running working condition, and also provides a monocular vision-based method for estimating vertical and pitching motion information of the vehicle body under the straight running working condition, so as to solve the problem that in the prior art, the vertical and pitching motion postures of the vehicle body cannot be calculated through a monocular vision system, and other sensors are required to be added for active suspension control, so that the cost is increased.
The invention aims at realizing the following technical scheme:
a vehicle body vertical and pitching motion information estimation system under a straight running working condition comprises an image information capturing module 1, a vehicle speed estimation module 2, a vehicle body vertical position and pitching angle estimation module 3 and a vehicle body vertical information and pitching angle information estimation and display module 4;
the image information capturing module 1 is configured to capture road information in front of a roadway, and includes: a monocular camera 11, an image acquisition module 12, and a feature extraction and tracking module 13; the monocular camera 11 sends the original image information to the image acquisition module 12, and sends the original image information to the feature extraction and tracking module 13 after distortion correction;
the vehicle speed estimation module 2 is used for acquiring the running speed;
the vehicle body vertical position and pitching angle estimation module 3 is used for estimating the vehicle body vertical position and pitching angle, and comprises a driving displacement calculation module 31, a vehicle posture calculation module 32 and a vehicle posture filtering module 33;
the vehicle speed estimation module 2 sends the driving speed information to the driving displacement calculation module 31;
the vehicle posture calculation module 32 is configured to calculate a vertical displacement and a pitch angle of a vehicle body under a current frame, and includes a previous frame pixel coordinate recording module 321, a previous frame vehicle body posture recording module 322, a current frame pixel coordinate recording module 323, and a current frame vehicle body posture calculation module 324;
the feature extraction and tracking module 13 sends the pixel coordinates of the feature point relative to the camera and the pixel coordinate information of the feature point on the new adjacent frame to the current frame pixel coordinate recording module 323;
the previous frame pixel coordinate recording module 321, the previous frame vehicle body posture recording module 322 and the current frame pixel coordinate recording module 323 respectively send the pixel coordinates of the characteristic points of the previous frame, the vertical displacement and pitching angle of the vehicle body of the previous frame and the pixel coordinate information of the characteristic points of the current frame to the current frame vehicle body posture calculating module 324; the current frame body posture calculation module 324 sends the calculated vertical displacement and pitching angle information of the current frame body to the vehicle posture filtering module 33;
the vehicle attitude filtering module 33 sends the current vertical position and the pitch angle of the vehicle to the vehicle body vertical information and the pitch angle information estimating and displaying module 4;
the vehicle vertical information and pitch angle information estimation and display module 4 is configured to calculate a vehicle vertical speed, a vehicle vertical acceleration, a vehicle pitch angle speed, and a vehicle pitch angle acceleration according to a vehicle vertical position and a vehicle pitch angle, and display the vehicle vertical speed, the vehicle vertical acceleration, the vehicle pitch angle speed, and the vehicle pitch angle acceleration on the central control display 41.
Further, the monocular camera 11 is used for acquiring original image information of a road in front of the vehicle through a camera of the camera; the image acquisition module 12 is used for acquiring original image information and performing distortion correction on the acquired image; the feature extraction and tracking module 13 extracts feature points of the image after distortion correction by using an ORB algorithm, acquires pixel coordinates of the feature points relative to the camera, and tracks three-dimensional feature points on adjacent frames to acquire pixel coordinates of the feature points on new adjacent frames.
Further, the driving displacement calculating module 31 calculates the driving distance of the vehicle in the time interval corresponding to two continuous frames of images according to the driving speed.
Further, the previous frame pixel coordinate recording module 321 is configured to record the pixel coordinates of the feature points of the previous frame; the previous frame body posture recording module 322 is used for recording the vertical displacement and the pitching angle of the previous frame body; the current frame pixel coordinate recording module 323 is configured to record the pixel coordinates of the feature points of the current frame; the current frame body posture calculating module 324 is configured to combine the pixel coordinates of the feature points of the previous frame, the vertical displacement and the pitching angle of the body of the previous frame, the pixel coordinates of the feature points of the current frame, and the vehicle running distance in the time interval corresponding to the two continuous frames of images, so as to calculate the vertical displacement and the pitching angle of the body of the current frame.
Further, the vehicle posture filtering module 33 performs filtering processing on the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle posture calculating module by using a kalman filtering algorithm, so as to obtain the current vertical position and the pitching angle of the vehicle.
Further, the vehicle body vertical information and pitching angle information estimation and display module 4 comprises a vehicle body vertical speed calculation module, a vehicle body vertical acceleration calculation module, a vehicle body pitching angle speed calculation module, a vehicle body pitching angle acceleration calculation module and a vehicle body vertical information and pitching information display module.
Further, the vehicle vertical speed calculation module estimates the vehicle vertical speed by combining the vehicle vertical position of two adjacent frames and the interval time of the adjacent frames; the vehicle body pitch angle speed calculation module is used for estimating the vehicle body pitch angle speed by combining the vehicle body pitch angles of two adjacent frames and the interval time of the adjacent frames; the vehicle vertical acceleration calculation module is used for estimating the vehicle vertical acceleration by combining the vehicle vertical speed of two adjacent frames and the interval time of the adjacent frames; the vehicle body pitch angle acceleration calculation module is used for estimating vehicle body pitch angle acceleration by combining the vehicle body pitch angle speed of two adjacent frames and the interval time of the adjacent frames; the vehicle body vertical information and pitching information display module is used for displaying the vehicle body vertical speed, the vehicle body vertical acceleration, the vehicle body pitching angle speed and the vehicle body pitching angle acceleration on the central control display.
A monocular vision-based estimation method for vertical and pitching motion information of a vehicle body under a straight running working condition comprises the following steps:
step S1, acquiring an original image of a road surface in front of a vehicle by using a monocular camera 11, acquiring image information of the camera by an image acquisition module 12, and performing distortion correction on the acquired image;
s2, extracting characteristic points of the image after distortion correction by using an ORB algorithm, acquiring pixel coordinates of the characteristic points relative to the camera, tracking three-dimensional characteristic points on adjacent frames, and acquiring pixel coordinates of the characteristic points on new adjacent frames;
s3, acquiring a driving speed by using a vehicle speed estimation module 2, and calculating a vehicle driving distance in a time interval corresponding to two continuous frames of images;
step S4, calculating the vertical displacement and the pitching angle of the vehicle body of the current frame by combining the pixel coordinates of the characteristic points of the previous frame, the vertical displacement and the pitching angle of the vehicle body of the previous frame and the pixel coordinates of the characteristic points of the current frame through the vehicle posture calculation module 32;
s5, filtering the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle posture calculation module by using a Kalman filtering algorithm through the vehicle posture filtering module 33 to obtain the vertical position and the pitching angle of the vehicle corresponding to the current frame;
s6, respectively estimating the vertical speed and the pitch angle speed of the vehicle body by combining the vertical displacement and the pitch angle of the vehicle body of two adjacent frames and the interval time of the adjacent frames;
and S7, respectively estimating the vertical acceleration and the pitch angle acceleration of the vehicle body by combining the vertical speed and the pitch angle speed of the vehicle body of two adjacent frames and the interval time of the adjacent frames.
Compared with the prior art, the invention has the beneficial effects that:
according to the invention, the visual sensor is utilized to obtain the gesture of the vehicle, the vertical height, the vertical speed, the vertical acceleration, the pitching angle speed and the pitching angle acceleration of the vehicle body can be accurately measured in real time without adding a vertical acceleration sensor and a corner sensor of the vehicle, and information reference is provided for the control of the active suspension and the semi-active suspension of the vehicle.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a block diagram of a vehicle body vertical height and pitch angle estimation system of the present invention;
FIG. 2 is a flow chart of a method of estimating vertical height and pitch angle of a vehicle body according to the present invention;
fig. 3 is a schematic illustration of the driving of the present invention.
In the figure, an image information capturing module 2, a vehicle speed estimating module 3, a vehicle vertical position and pitching angle estimating module 4, a vehicle vertical information and pitching angle information estimating and displaying module 11, a monocular camera 12, an image acquisition module 13, a feature extraction and tracking module 31, a driving displacement calculating module 32, a vehicle posture calculating module 33, a vehicle posture filtering module 321, a previous frame pixel coordinate recording module 322, a previous frame vehicle posture recording module 323, a current frame pixel coordinate recording module 324, a current frame vehicle posture calculating module 41 and a central control display.
Detailed Description
The invention is further illustrated by the following examples:
the invention is described in further detail below with reference to the drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting thereof. It should be further noted that, for convenience of description, only some, but not all of the structures related to the present invention are shown in the drawings.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures. Meanwhile, in the description of the present invention, the terms "first", "second", and the like are used only to distinguish the description, and are not to be construed as indicating or implying relative importance.
According to the invention, the monocular camera and the vehicle speed sensor are used for calculating the vehicle movement information, the height and the pitching angle of the vehicle body, then the Kalman filtering algorithm is used for calculating the height and the pitching angle of the vehicle body, and then the vertical speed, the vertical acceleration, the pitching angle speed and the pitching angle acceleration of the vehicle body are calculated in sequence, and are dynamically displayed on the central control display.
Example 1
As shown in fig. 1, the system for estimating vertical and pitching motion information of a vehicle body under a straight running condition comprises an image information capturing module 1, a vehicle speed estimating module 2, a vehicle body vertical position and pitching angle estimating module 3 and a vehicle body vertical information and pitching angle information estimating and displaying module 4.
The image information capturing module 1 is configured to capture road information in front of a roadway, and includes: a monocular camera 11, an image acquisition module 12 and a feature extraction and tracking module 13.
The monocular camera 11 is used for acquiring original image information of a road in front of a vehicle through a camera of the camera; the image acquisition module 12 is used for acquiring original image information and performing distortion correction on the acquired image; the feature extraction and tracking module 13 extracts feature points of the image after distortion correction by using an ORB algorithm, acquires pixel coordinates of the feature points relative to the camera, and tracks three-dimensional feature points on adjacent frames to acquire pixel coordinates of the feature points on new adjacent frames.
The monocular camera 11 sends the original image information to the image acquisition module 12, and after distortion correction, the original image information is sent to the feature extraction and tracking module 13.
The vehicle speed estimation module 2 is configured to obtain a vehicle speed. The vehicle body vertical position and pitching angle estimation module 3 is configured to estimate a vehicle body vertical position and a pitching angle, and includes a driving displacement calculation module 31, a vehicle posture calculation module 32, and a vehicle posture filtering module 33.
The vehicle speed estimation module 2 sends the driving speed information to the driving displacement calculation module 31.
The driving displacement calculation module 31 calculates the driving distance of the vehicle in the time interval corresponding to two continuous frames of images according to the driving speed; the vehicle posture calculation module 32 is configured to calculate a vertical displacement and a pitch angle of a vehicle body under a current frame, and includes a previous frame pixel coordinate recording module 321, a previous frame vehicle body posture recording module 322, a current frame pixel coordinate recording module 323, and a current frame vehicle body posture calculation module 324.
The previous frame pixel coordinate recording module 321 is configured to record the pixel coordinates of the feature points of the previous frame;
the previous frame body posture recording module 322 is used for recording the vertical displacement and the pitching angle of the previous frame body;
the current frame pixel coordinate recording module 323 is configured to record the pixel coordinates of the feature points of the current frame;
the current frame body posture calculating module 324 is configured to combine the pixel coordinates of the feature points of the previous frame, the vertical displacement and the pitching angle of the body of the previous frame, the pixel coordinates of the feature points of the current frame, and the vehicle running distance in the time interval corresponding to the two continuous frames of images, so as to calculate the vertical displacement and the pitching angle of the body of the current frame.
The feature extraction and tracking module 13 sends the pixel coordinates of the feature point relative to the camera and the pixel coordinate information of the feature point on the new adjacent frame to the current frame pixel coordinate recording module 323.
The previous frame pixel coordinate recording module 321, the previous frame vehicle body posture recording module 322, and the current frame pixel coordinate recording module 323 respectively send the pixel coordinates of the feature points of the previous frame, the vertical displacement and pitching angle of the vehicle body of the previous frame, and the pixel coordinate information of the feature points of the current frame to the current frame vehicle body posture calculating module 324.
The current frame body posture calculation module 324 sends the calculated vertical displacement and pitch angle information of the current frame body to the vehicle posture filtering module 33.
The vertical displacement and pitching angle of the vehicle body relate to coordinate conversion of the monocular camera, which is a mutual conversion between the world coordinate system and the pixel coordinate system of the feature points.
As shown in fig. 3, xw, yw, zw are coordinate axes of the world coordinate system, and Xc, yc, zc are coordinate axes of the camera coordinate system (pixel coordinates are two-dimensional planes, and pixel coordinates after distortion correction are considered to be parallel to the camera coordinates, denoted by μ and ν). In particular, the camera coordinate system is located in front of the vehicle, the origin of the world coordinate system is located on the road surface in front of the vehicle, and the origin of the camera coordinate system is located on the Zw axis of the world coordinate system, and the camera rotates only about the Xc axis thereof.
The process of converting world coordinates of an object to pixel coordinates satisfies the following relation:
wherein s is a proportionality coefficient, K is an internal reference matrix of the camera, is determined by the characteristics of the camera, does not change along with the position of the camera, and can be measured by calibration. R is a rotation matrix of a camera coordinate system, and T is a translation matrix of the camera coordinate system.
Where f is the camera focal length, (μ) 0 ,ν 0 ) Is the offset of the center point of the image coordinate system to the center point of the pixel coordinate system, dx is the length of x to one pixel, and dy is the length of y to one pixel.
The process of converting world coordinates of an object to pixel coordinates can also be represented by the following formula:
wherein R is a rotation coincidence matrix, which is a conversion matrix converted from a world coordinate system to a camera coordinate system without considering the rotation of a camera around the axis of the camera, tc is the position of the origin of the world coordinate system under the camera coordinate system, and Rc is the conversion matrix under the condition that the camera coordinate system rotates around the axis of the camera. K is the camera reference matrix.
The origin of the camera coordinate system is located on the Zw axis of the world coordinate system, when the height from the ground is h,
the height h is the height of the camera relative to the ground, and the change in h reflects the change in the vertical height of the vehicle body.
When the camera is rotated only by the angle theta around its Xc axis,
when the camera is fixed on the automobile, the pitching angle of the automobile is known as the angle theta of the camera rotating around the Xc axis of the camera according to theoretical mechanics.
From the above, it is possible to:
therefore, the functional relationship between the world coordinates of the object and the pixel coordinates corresponding to the imaging of the object includes two variables, i.e., h and θ.
The world coordinates are back-deduced from the pixel coordinate system, and the following can be obtained:
wherein K, R are known matrices.
Is provided with
To counteract the influence of the proportionality coefficient s
Then there is
Because the characteristic points captured by the method are on the road surface, the world coordinate Z of the characteristic points w ＝0。
Is provided with
For a monocular camera that determines parameters, the pixel coordinates (μ, ν) of the feature point can be determined by determining them, and G, R, g is the determined value.
Thus there is
X w ＝tan(g+θ)h
In two continuous frames of images, the coordinates of the characteristic points of the previous frame are set as (mu) 1 ,ν 1 ) The current frame feature point coordinates are (μ) 2 ,ν 2 ) The coordinates of the characteristic points at two moments are (X) w1 ,Y w1 ,Z w1 ) And (X) w2 ,Y w2 ,Z w2 )。
Since the captured feature points are located on the road surface, Z is considered to be w1 ＝Z w2 ＝0。
Since the car is moving straight, consider Y w1 ＝Y w2 ,ΔY w ＝Y w1 -Y w2 ＝0。
ΔX w ＝X w1 -X w2 ,ΔX w Is the distance travelled by the vehicle in the interval of two consecutive frames, and is calculated by the driving displacement calculation module 31.
Thus there is
ΔX w ＝X w1 -X w2 ＝tan(g 1 +θ 1 )h 1 -tan(g 2 +θ 2 )h 2
The formula is a Markov process, which can be determined from the vehicle pose (h 1 ,θ 1 ) Calculate the attitude (h) 2 ,θ 2 )。
The vehicle posture filtering module 33 performs filtering processing on the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle posture calculating module by adopting a kalman filtering algorithm, and is used for obtaining the current vertical position and pitching angle of the vehicle.
The initial state of the vehicle is defined as (h 0 ,θ 0 ) And the method is obtained by calibration measurement of the automobile.
Consider h 2 Is the height of the car body in the current frame.
Consider (θ) 2 -θ 0 ) Is the pitch angle of the automobile body of the current frameDegree.
From the Kalman filtering, the Kalman filtering value of the single target system can be calculated by the following formula:
wherein,,
is the Kalman filter value at time n, < >>
Is the system prediction value at time n, +.>
Is the observed value at time n, b m Is system state noise, b s Is the measurement noise of the system. When calculating the vertical height h, x=h, and when calculating the pitch angle θ, x=θ.
Since the posture is surrounded (h) when the vehicle is moving 0 ,θ 0 ) Wave motion, so that
h n And theta n And (5) obtaining the product through iterative calculation.
The vehicle attitude filtering module 33 sends the current vehicle vertical position and pitch angle to the vehicle body vertical information and pitch angle information estimation and display module 4.
The vehicle vertical information and pitch angle information estimation and display module 4 is configured to calculate a vehicle vertical speed, a vehicle vertical acceleration, a vehicle pitch angle speed, and a vehicle pitch angle acceleration according to a vehicle vertical position and a vehicle pitch angle, and display the vehicle vertical speed, the vehicle vertical acceleration, the vehicle pitch angle speed, and the vehicle pitch angle acceleration on the central control display 41. The vehicle body vertical information and pitching angle information estimation and display module 4 comprises a vehicle body vertical speed calculation module, a vehicle body vertical acceleration calculation module, a vehicle body pitching angle speed calculation module, a vehicle body pitching angle acceleration calculation module and a vehicle body vertical information and pitching information display module. The vehicle vertical speed calculation module estimates the vehicle vertical speed by combining the vehicle vertical positions of two adjacent frames and the interval time of the adjacent frames.
The vehicle body pitch angle speed calculation module is used for estimating the vehicle body pitch angle speed by combining the vehicle body pitch angles of two adjacent frames and the interval time of the adjacent frames.
The vehicle vertical acceleration calculation module estimates the vehicle vertical acceleration by combining the vehicle vertical speed of two adjacent frames and the interval time of the adjacent frames.
The vehicle body pitch angle acceleration calculation module is used for estimating the vehicle body pitch angle acceleration by combining the vehicle body pitch angle speed of two adjacent frames and the interval time of the adjacent frames.
The vehicle body vertical information and pitching information display module is used for displaying the vehicle body vertical speed, the vehicle body vertical acceleration, the vehicle body pitching angle speed and the vehicle body pitching angle acceleration on the central control display.
Example 2
As shown in fig. 2, the method for estimating the vertical and pitching motion information of the vehicle body based on the monocular vision straight running working condition specifically comprises the following steps:
step S1, acquiring an original image of a road surface in front of a vehicle by using a monocular camera 11, acquiring image information of the camera by using an image acquisition module 1, and performing distortion correction on the acquired image;
and S2, extracting characteristic points of the image after distortion correction by using an ORB algorithm, acquiring pixel coordinates of the characteristic points relative to the camera, tracking the three-dimensional characteristic points on adjacent frames, and acquiring pixel coordinates of the characteristic points on new adjacent frames.
And S3, acquiring the driving speed by using the vehicle speed estimation module 2, and calculating the vehicle driving distance in the time interval corresponding to the two continuous frames of images.
Step S4, calculating the vertical displacement and the pitching angle of the vehicle body of the current frame by combining the pixel coordinates of the characteristic points of the previous frame, the vertical displacement and the pitching angle of the vehicle body of the previous frame and the pixel coordinates of the characteristic points of the current frame through the vehicle posture calculation module 32.
And S5, filtering the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle posture calculation module by using a Kalman filtering algorithm through the vehicle posture filtering module 33, and obtaining the vertical position and the pitching angle of the vehicle corresponding to the current frame.
And S6, respectively estimating the vertical speed and the pitch angle speed of the vehicle body by combining the vertical displacement and the pitch angle of the vehicle body of two adjacent frames and the interval time of the adjacent frames.
And S7, respectively estimating the vertical acceleration and the pitch angle acceleration of the vehicle body by combining the vertical speed and the pitch angle speed of the vehicle body of two adjacent frames and the interval time of the adjacent frames.
Note that the above is only a preferred embodiment of the present invention and the technical principle applied. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, while the invention has been described in connection with the above embodiments, the invention is not limited to the embodiments, but may be embodied in many other equivalent forms without departing from the spirit or scope of the invention, which is set forth in the following claims.
Claims (8)
1. A straight line driving working condition automobile body vertical and pitching motion information estimation system which is characterized in that: the vehicle body vertical position and pitching angle estimation device comprises an image information capturing module (1), a vehicle speed estimation module (2), a vehicle body vertical position and pitching angle estimation module (3) and a vehicle body vertical information and pitching angle information estimation and display module (4);
the image information capturing module (1) for capturing road information in front of a roadway, comprising: a monocular camera (11), an image acquisition module (12) and a feature extraction and tracking module (13); the monocular camera (11) sends original image information to the image acquisition module (12) and sends the original image information to the feature extraction and tracking module (13) after distortion correction;
the vehicle speed estimation module (2) is used for acquiring the running speed;
the vehicle body vertical position and pitching angle estimation module (3) is used for estimating the vehicle body vertical position and pitching angle and comprises a driving displacement calculation module (31), a vehicle posture calculation module (32) and a vehicle posture filtering module (33);
the vehicle speed estimation module (2) sends the driving speed information to the driving displacement calculation module (31);
the vehicle attitude calculation module (32) is used for calculating the vertical displacement and the pitching angle of the vehicle body under the current frame and comprises a previous frame pixel coordinate recording module (321), a previous frame vehicle body attitude recording module (322), a current frame pixel coordinate recording module (323) and a current frame vehicle body attitude calculation module (324);
the characteristic extraction and tracking module (13) sends the pixel coordinates of the characteristic points relative to the camera and the pixel coordinate information of the characteristic points on the new adjacent frame to the current frame pixel coordinate recording module (323);
the previous frame pixel coordinate recording module (321), the previous frame vehicle body posture recording module (322) and the current frame pixel coordinate recording module (323) respectively send the pixel coordinates of the characteristic points of the previous frame, the vertical displacement and pitching angle of the vehicle body of the previous frame and the pixel coordinate information of the characteristic points of the current frame to the current frame vehicle body posture calculating module (324); the current frame body posture calculation module (324) sends the calculated vertical displacement and pitching angle information of the current frame body to the vehicle posture filtering module (33);
the vehicle attitude filtering module (33) sends the current vertical position and the pitch angle of the vehicle to the vehicle body vertical information and pitch angle information estimating and displaying module (4);
the vehicle body vertical information and pitching angle information estimation and display module (4) is used for calculating the vehicle vertical speed, vertical acceleration, pitching angle speed and pitching angle acceleration according to the vehicle vertical position and pitching angle, and displaying the vehicle vertical speed, vertical acceleration, pitching angle speed and pitching angle acceleration on the central control display (41).
2. The system for estimating vertical and pitch motion information of a vehicle body under straight running conditions according to claim 1, wherein: the monocular camera (11) is used for acquiring original image information of a road in front of a vehicle through a camera of the camera; the image acquisition module (12) is used for acquiring original image information and carrying out distortion correction on the acquired image; the feature extraction and tracking module (13) extracts feature points of the image after distortion correction by adopting an ORB algorithm, acquires pixel coordinates of the feature points relative to the camera, tracks three-dimensional feature points on adjacent frames, and acquires pixel coordinates of the feature points on new adjacent frames.
3. The system for estimating vertical and pitch motion information of a vehicle body under straight running conditions according to claim 1, wherein: the driving displacement calculation module (31) calculates the driving distance of the vehicle in the time interval corresponding to the two continuous frames of images through the driving speed.
4. The system for estimating vertical and pitch motion information of a vehicle body under straight running conditions according to claim 1, wherein: the previous frame pixel coordinate recording module (321) is used for recording the pixel coordinates of the feature points of the previous frame; the previous frame body posture recording module (322) is used for recording the vertical displacement and the pitching angle of the previous frame body; the current frame pixel coordinate recording module (323) is used for recording the pixel coordinates of the characteristic points of the current frame; the current frame body posture calculation module (324) is configured to combine the pixel coordinates of the feature points of the previous frame, the vertical displacement and the pitching angle of the body of the previous frame, the pixel coordinates of the feature points of the current frame, and the vehicle running distance in the corresponding time interval of the two continuous frames of images, so as to calculate the vertical displacement and the pitching angle of the body of the current frame.
5. The system for estimating vertical and pitch motion information of a vehicle body under straight running conditions according to claim 1, wherein: the vehicle attitude filtering module (33) is used for filtering the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle attitude calculation module by adopting a Kalman filtering algorithm and is used for obtaining the current vertical position and pitching angle of the vehicle.
6. The system for estimating vertical and pitch motion information of a vehicle body under straight running conditions according to claim 1, wherein: the vehicle body vertical information and pitching angle information estimation and display module (4) comprises a vehicle body vertical speed calculation module, a vehicle body vertical acceleration calculation module, a vehicle body pitching angle speed calculation module, a vehicle body pitching angle acceleration calculation module and a vehicle body vertical information and pitching information display module.
7. The system for estimating vertical and pitch motion information of a vehicle under straight-line driving conditions according to claim 6, wherein: the vehicle body vertical speed calculation module is used for estimating the vehicle body vertical speed by combining the vehicle body vertical positions of two adjacent frames and the interval time of the adjacent frames; the vehicle body pitch angle speed calculation module is used for estimating the vehicle body pitch angle speed by combining the vehicle body pitch angles of two adjacent frames and the interval time of the adjacent frames; the vehicle vertical acceleration calculation module is used for estimating the vehicle vertical acceleration by combining the vehicle vertical speed of two adjacent frames and the interval time of the adjacent frames; the vehicle body pitch angle acceleration calculation module is used for estimating vehicle body pitch angle acceleration by combining the vehicle body pitch angle speed of two adjacent frames and the interval time of the adjacent frames; the vehicle body vertical information and pitching information display module is used for displaying the vehicle body vertical speed, the vehicle body vertical acceleration, the vehicle body pitching angle speed and the vehicle body pitching angle acceleration on the central control display.
8. The monocular vision-based estimation method for the vertical and pitching motion information of the vehicle body under the straight running working condition is characterized by comprising the following steps of:
s1, acquiring an original image of a road surface in front of a vehicle by using a monocular camera (11), acquiring image information of the camera by using an image acquisition module (12), and performing distortion correction on the acquired image;
s2, extracting characteristic points of the image after distortion correction by using an ORB algorithm, acquiring pixel coordinates of the characteristic points relative to the camera, tracking three-dimensional characteristic points on adjacent frames, and acquiring pixel coordinates of the characteristic points on new adjacent frames;
s3, acquiring the driving speed by using a vehicle speed estimation module (2), and calculating the vehicle driving distance in the time interval corresponding to two continuous frames of images;
s4, calculating the vertical displacement and the pitching angle of the vehicle body of the current frame by combining the pixel coordinates of the characteristic points of the previous frame, the vertical displacement and the pitching angle of the vehicle body of the previous frame and the pixel coordinates of the characteristic points of the current frame through a vehicle posture calculation module (32);
s5, filtering the vertical displacement and the pitching angle of the vehicle body obtained by the vehicle posture calculation module by using a Kalman filtering algorithm through a vehicle posture filtering module (33) to obtain the vertical position and the pitching angle of the vehicle corresponding to the current frame;
s6, respectively estimating the vertical speed and the pitch angle speed of the vehicle body by combining the vertical displacement and the pitch angle of the vehicle body of two adjacent frames and the interval time of the adjacent frames;
and S7, respectively estimating the vertical acceleration and the pitch angle acceleration of the vehicle body by combining the vertical speed and the pitch angle speed of the vehicle body of two adjacent frames and the interval time of the adjacent frames.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310249330.3A CN116252581B (en) | 2023-03-15 | 2023-03-15 | System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310249330.3A CN116252581B (en) | 2023-03-15 | 2023-03-15 | System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116252581A true CN116252581A (en) | 2023-06-13 |
| CN116252581B CN116252581B (en) | 2024-01-16 |
Family
ID=86679198
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310249330.3A Active CN116252581B (en) | 2023-03-15 | 2023-03-15 | System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116252581B (en) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070154068A1 (en) * | 2006-01-04 | 2007-07-05 | Mobileye Technologies, Ltd. | Estimating Distance To An Object Using A Sequence Of Images Recorded By A Monocular Camera |
| JP2012046081A (en) * | 2010-08-27 | 2012-03-08 | Daihatsu Motor Co Ltd | Running condition determination device |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| JP2015184956A (en) * | 2014-03-25 | 2015-10-22 | 日産自動車株式会社 | Self-position calculation device and self-position calculation method |
| US20170024617A1 (en) * | 2014-02-24 | 2017-01-26 | Nissan Motor Co., Ltd. | Self-Location Calculating Device and Self-Location Calculating Method |
| CN106525049A (en) * | 2016-11-08 | 2017-03-22 | 山东大学 | Quadruped robot body posture tracking method based on computer vision |
| CN107833249A (en) * | 2017-09-29 | 2018-03-23 | 南京航空航天大学 | A kind of carrier-borne aircraft landing mission attitude prediction method of view-based access control model guiding |
| CN110386142A (en) * | 2018-04-17 | 2019-10-29 | 百度（美国）有限责任公司 | Pitch angle calibration method for automatic driving vehicle |
| WO2021035669A1 (en) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | Pose prediction method, map construction method, movable platform, and storage medium |
| CN112639502A (en) * | 2018-09-07 | 2021-04-09 | 华为技术有限公司 | Robot pose estimation |
| CN114910927A (en) * | 2021-02-10 | 2022-08-16 | 福特全球技术公司 | Event-based vehicle attitude estimation using monochromatic imaging |
-
2023
- 2023-03-15 CN CN202310249330.3A patent/CN116252581B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070154068A1 (en) * | 2006-01-04 | 2007-07-05 | Mobileye Technologies, Ltd. | Estimating Distance To An Object Using A Sequence Of Images Recorded By A Monocular Camera |
| JP2012046081A (en) * | 2010-08-27 | 2012-03-08 | Daihatsu Motor Co Ltd | Running condition determination device |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| US20170024617A1 (en) * | 2014-02-24 | 2017-01-26 | Nissan Motor Co., Ltd. | Self-Location Calculating Device and Self-Location Calculating Method |
| JP2015184956A (en) * | 2014-03-25 | 2015-10-22 | 日産自動車株式会社 | Self-position calculation device and self-position calculation method |
| CN106525049A (en) * | 2016-11-08 | 2017-03-22 | 山东大学 | Quadruped robot body posture tracking method based on computer vision |
| CN107833249A (en) * | 2017-09-29 | 2018-03-23 | 南京航空航天大学 | A kind of carrier-borne aircraft landing mission attitude prediction method of view-based access control model guiding |
| CN110386142A (en) * | 2018-04-17 | 2019-10-29 | 百度（美国）有限责任公司 | Pitch angle calibration method for automatic driving vehicle |
| CN112639502A (en) * | 2018-09-07 | 2021-04-09 | 华为技术有限公司 | Robot pose estimation |
| WO2021035669A1 (en) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | Pose prediction method, map construction method, movable platform, and storage medium |
| CN114910927A (en) * | 2021-02-10 | 2022-08-16 | 福特全球技术公司 | Event-based vehicle attitude estimation using monochromatic imaging |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116252581B (en) | 2024-01-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2020119140A1 (en) | Method, apparatus and smart device for extracting keyframe in simultaneous localization and mapping | |
| CN110745140B (en) | Vehicle lane change early warning method based on continuous image constraint pose estimation | |
| US6535114B1 (en) | Method and apparatus for environment recognition | |
| CN111595333A (en) | Modularized unmanned vehicle positioning method and system based on visual inertial laser data fusion | |
| JP4355341B2 (en) | Visual tracking using depth data | |
| CN107121132B (en) | Method and device for obtaining vehicle environment image and method for identifying object in environment | |
| JP5804185B2 (en) | Moving object position / orientation estimation apparatus and moving object position / orientation estimation method | |
| CN103411621B (en) | A kind of vision/INS Combinated navigation method of the optical flow field towards indoor mobile robot | |
| Jeong et al. | Adaptive inverse perspective mapping for lane map generation with SLAM | |
| US20180075614A1 (en) | Method of Depth Estimation Using a Camera and Inertial Sensor | |
| JP6708730B2 (en) | Mobile | |
| CN115371665B (en) | Mobile robot positioning method based on depth camera and inertial fusion | |
| CN113503873B (en) | Visual positioning method for multi-sensor fusion | |
| CN109345591B (en) | Vehicle posture detection method and device | |
| WO2019156072A1 (en) | Attitude estimating device | |
| CN113701750A (en) | Fusion positioning system of underground multi-sensor | |
| CN116442707B (en) | System and method for estimating vertical and pitching motion information of vehicle body based on binocular vision | |
| CN109878528A (en) | Head movement attitude detection system towards vehicle-mounted stereo visual system | |
| JP6768554B2 (en) | Calibration device | |
| CN110720113A (en) | Parameter processing method and device, camera equipment and aircraft | |
| JP4256992B2 (en) | Obstacle detection device | |
| CN110197104B (en) | Distance measurement method and device based on vehicle | |
| CN116252581B (en) | System and method for estimating vertical and pitching motion information of vehicle body under straight running working condition | |
| Park et al. | A novel line of sight control system for a robot vision tracking system, using vision feedback and motion-disturbance feedforward compensation | |
| CN107248171B (en) | Triangulation-based monocular vision odometer scale recovery method |
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 |