CN109490923A - Vehicle driving camber angle real-time detection method based on adaptive least square fitting - Google Patents
Vehicle driving camber angle real-time detection method based on adaptive least square fitting Download PDFInfo
- Publication number
- CN109490923A CN109490923A CN201811377485.0A CN201811377485A CN109490923A CN 109490923 A CN109490923 A CN 109490923A CN 201811377485 A CN201811377485 A CN 201811377485A CN 109490923 A CN109490923 A CN 109490923A
- Authority
- CN
- China
- Prior art keywords
- fitting
- straight line
- threshold
- error
- adaptive
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a kind of vehicle driving camber angle real-time detection methods based on adaptive least square fitting.This method can accurately calculate the camber angle of each position in vehicle driving in real time, this makes it possible to obtain the degree of deflection when turning degree turned when vehicle drive and straight trip, can be used to judge whether the driving behavior of a people meets regulation.The realization of this method is by, to direction and vehicle forward direction, finally obtaining angle of turn after fitting trajectory calculation vehicle.The present invention relies on the thought of least square method, has carried out some innovations to this thought, adaptively looks for the point set around each point, and the angle of turn of this point is calculated according to the point set found.This method has extremely low complexity and error, substantially increases the real-time and accuracy of detection.
Description
Technical field
The present invention relates to vehicle driving safety field of auxiliary, in particular to the vehicle row based on adaptive least square fitting
Sail camber angle real-time detection method.
Background technique
Detect vehicle steering angle it is most important to the safety of vehicle and driver because fatal accident often by
Caused by danger turns to.According to statistics, cause in dead road traffic accident, 23.1% is related with lane change, 7.7% with fall
It is related, it is related with the go to action of vehicle to amount to 30.8%.The existing method for calculating turning angle of vehicle has using steering
Angle transducer is calculated, is calculated using magnetometric sensor and calculated using the front camera and rear camera of smart phone.However these sides
Method suffers from respective limitation, can not use in reality well.
The steering angle sensor of vehicle can detecte the rotation angle and rotation speed of steering wheel, on this basis again into one
Step calculates the actual angle of turn of vehicle.The angle of turn error that this method calculates is smaller, but the position of the sensor is in direction
In steering column below disk, therefore revenue passenger is inconvenient using the sensor, and is fitted without the traveling of the sensor
Tool is not available this method then.
Magnetometric sensor can immediately arrive at the angle of turning, but this method stability is poor, vulnerable to interference.This method
Principle is that magnetometric sensor can directly read angle between mobile phone Y-axis and the earth's magnetic field arctic, therefore according to the change of this angle
Change can directly obtain angle of turn.But when magnetometric sensor nearby has stronger magnetic field, magnetometric sensor just be will receive
Extreme influence, therefore exceptional value will often occur in obtained angle.For needing to calculate the reality scene of angle of turn and
It says, this uncontrollable exceptional value can not be all allowed in most situations.
It the use of the method that the front camera and rear camera of smart phone is calculated is to first pass through camera to capture road object, so
It is analyzed it afterwards using image procossing and obtains angle of turn.But this method is affected by environment larger, in insufficient light
Place, especially night this method can substantially completely fail, additionally by weather, the influence of the factors such as pavement behavior.These are all
Limit application of this method in reality.
Summary of the invention
The purpose of the present invention is to provide the vehicle driving camber angle real-time detection sides being fitted based on adaptive least square
Method, to solve the above problems.
To achieve the above object, the invention adopts the following technical scheme:
Vehicle driving camber angle real-time detection method based on adaptive least square fitting, comprising the following steps:
Step 1, vehicle raw GPS data is projected to obtain k binary group by coordinate sequence；
Step 2, given threshold obtains two fitting a straight lines by least square fitting to k binary group in step 1
Intercept and slope；
Step 3, the penalty values of digital simulation straight line carry out in next step if penalty values are greater than threshold value, by intercept and tiltedly
Rate calculates separately the direction vector of two straight lines；Step 2 is returned to if penalty values are less than threshold value to recalculate；
Step 4, the angle for calculating two rectilinear direction vectors, judges turn direction, by the direction to two straight lines to
Amount does dot product, if its value is greater than 0, then it represents that rotation counterclockwise, if its value is less than 0, then it represents that rotate clockwise and return；If 0
It is then expressed as not turning.
Further, in step 1, by raw GPS data (x ' [1], y ' [1]), (x ' [2], y ' [2]), (x ' [3], y '
[3]) ... (x ' [k], y ' [k]) projects to obtain (x [1], y [1]) by coordinate sequence, (x [2], y [2]) ... (x [n], y
[n]) ... (x [k], y [k]) this k binary group.
Further, in step 2, to the intercept and slope of direction straight line after calculating:
Set a parameter threshold threshold；Using a, b indicates the intercept and slope of fitting a straight line, sets two
Variable numerator and denominator indicates the molecule and denominator of slope；Q come indicate participate in fitting point number；q
2 are initialized as, according to least square fitting, the calculation formula for obtaining numertor and denominator is as follows:
B and a are calculated again:
B=numerator/denominator
A=y [n]-b*x [n].
Further, in step 3, to direction linear vector after calculating:
Using error be used to indicate the penalty values of each fitting a straight line, this value and original point to fitting a straight line distance
It is positively correlated, and working as penalty values is more than threshold value, just illustrates that be fitted straight line and original GPS data point gap are excessive, is fitted
More points are added in Shi Buying；Penalty values are calculated according to range formula
If error is less than threshold, the value of q adds 1, and repeats step 2, will make (x [n+q], y [n+q])
This point participates in fitting；If error is more than or equal to threshold, straight line is the backward direction within the scope of coincidence loss
Straight line, to the direction vector of straight line after obtaining
Further, in step 2, the intercept and slope of forward direction straight line are calculated:
Setting threshold ' be threshold value, the intercept and slope of a ', b ' expression straight line, numerator ' and
The molecule and denominator of denominator ' expression slope；P, error ' come indicate participate in fitting point number and penalty values；
Initialization: p=2, a '=0, b '=0, error '=0
Further, in step 3, forward direction linear vector is calculated:
It is used to indicate the preceding penalty values to straight line of fitting using error, penalty values is calculated according to range formula
If error is less than threshold, the value of p adds 1, and repeats step 4, will make (x [n-p], y [n-p]) this
A point participates in fitting；If error ' is greater than equal threshold ', straight line is that the forward direction within the scope of coincidence loss is straight
Line, to the direction vector of straight line before obtaining
Further, in step 4, by arccos function and the angle of both direction is acquired, to obtain n-th
The camber camber angle of point is a [n]；
Compared with prior art, the present invention has following technical effect:
The present invention, which passes through, to be fitted after trajectory calculation vehicle to direction and vehicle forward direction, and angle of turn is finally obtained, according to
The thought in least square method is held in the palm, adaptively looks for the point set around each point, and this is calculated according to the point set found
The angle of turn of a point.The present invention can quickly and accurately calculate Ackermann steer angle in the case where only GPS data
Camber angle, and can be used to judge when vehicle turns.Be conducive to after safety accident or public order incident occur, more
Accurately extracts the turning feature of vehicle and judge the psychological condition of driver.
Accuracy of the present invention is high, substantially not affected by environment, and extremely low based on least-squares calculation cost.
Detailed description of the invention
Fig. 1 shows the effect picture of fitting in practice；
Fig. 2 is the flow chart for calculating angle of turn.
Specific embodiment
Below in conjunction with attached drawing, the present invention is further described:
Vehicle driving camber angle real-time detection method based on adaptive least square fitting, comprising the following steps:
Step 1, vehicle raw GPS data is projected to obtain k binary group by coordinate sequence；
Step 2, given threshold obtains two fitting a straight lines by least square fitting to k binary group in step 1
Intercept and slope；
Step 3, the penalty values of digital simulation straight line carry out in next step if penalty values are greater than threshold value, by intercept and tiltedly
Rate calculates separately the direction vector of two straight lines；Step 2 is returned to if penalty values are less than threshold value to recalculate；
Step 4, the angle for calculating two rectilinear direction vectors, judges turn direction, by the direction to two straight lines to
Amount does dot product, if its value is greater than 0, then it represents that rotation counterclockwise, if its value is less than 0, then it represents that rotate clockwise and return；If 0
It is then expressed as not turning.
In step 1, by raw GPS data (x ' [1], y ' [1]), (x ' [2], y ' [2]), (x ' [3], y ' [3]) ...
(x ' [k], y ' [k]) projects to obtain (x [1], y [1]) by coordinate sequence, (x [2], y [2]) ... (x [n], y [n]) ...
(x [k], y [k]) this k binary group.
In step 2, to the intercept and slope of direction straight line after calculating:
Set a parameter threshold threshold；Using a, b indicates the intercept and slope of fitting a straight line, sets two
Variable numerator and denominator indicates the molecule and denominator of slope；Q come indicate participate in fitting point number；q
2 are initialized as, according to least square fitting, the calculation formula for obtaining numertor and denominator is as follows:
B and a are calculated again:
B=numerator/denominator
A=y [n]-b*x [n].
In step 3, to direction linear vector after calculating:
Using error be used to indicate the penalty values of each fitting a straight line, this value and original point to fitting a straight line distance
It is positively correlated, and working as penalty values is more than threshold value, just illustrates that be fitted straight line and original GPS data point gap are excessive, is fitted
More points are added in Shi Buying；Penalty values are calculated according to range formula
If error is less than threshold, the value of q adds 1, and repeats step 2, will make (x [n+q], y [n+q])
This point participates in fitting；If error is more than or equal to threshold, straight line is the backward direction within the scope of coincidence loss
Straight line, to the direction vector of straight line after obtaining
In step 2, the intercept and slope of forward direction straight line are calculated:
Setting threshold ' be threshold value, the intercept and slope of a ', b ' expression straight line, numerator ' and
The molecule and denominator of denominator ' expression slope；P, error ' come indicate participate in fitting point number and penalty values；
Initialization: p=2, a '=0, b '=0, error '=0
In step 3, forward direction linear vector is calculated:
It is used to indicate the preceding penalty values to straight line of fitting using error, penalty values is calculated according to range formula
If error is less than threshold, the value of p adds 1, and repeats step 4, will make (x [n-p], y [n-p]) this
A point participates in fitting；If error ' is greater than equal threshold ', straight line is that the forward direction within the scope of coincidence loss is straight
Line, to the direction vector of straight line before obtaining
In step 4, by arccos function and the angle of both direction is acquired, so that it is curved to obtain n-th point of camber
Degree angle is a [n]；
Claims (7)
1. the vehicle driving camber angle real-time detection method based on adaptive least square fitting, which is characterized in that including following
Step:
Step 1, vehicle raw GPS data is projected to obtain k binary group by coordinate sequence；
Step 2, given threshold obtains the intercept of two fitting a straight lines by least square fitting to k binary group in step 1
And slope；
Step 3, the penalty values of digital simulation straight line carry out in next step, passing through intercept and slope point if penalty values are greater than threshold value
Not Ji Suan two straight lines direction vector；Step 2 is returned to if penalty values are less than threshold value to recalculate；
Step 4, the angle for calculating two rectilinear direction vectors, judges turn direction, is done by the direction vector to two straight lines
Dot product, if its value is greater than 0, then it represents that rotation counterclockwise, if its value is less than 0, then it represents that rotate clockwise and return；It is indicated if 0
Not turn.
2. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, in step 1, by raw GPS data (x ' [1], y ' [1]), (x ' [2], y ' [2]), (x ' [3], y ' [3]) ...
(x ' [k], y ' [k]) projects to obtain (x [1], y [1]) by coordinate sequence, (x [2], y [2]) ... (x [n], y [n]) ... (x
[k], y [k]) this k binary group.
3. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, in step 2, to the intercept and slope of direction straight line after calculating:
Set a parameter threshold threshold；Using a, b indicates the intercept and slope of fitting a straight line, sets two variables
Numerator and denominator indicates the molecule and denominator of slope；Q come indicate participate in fitting point number；Q is initial
2 are turned to, according to least square fitting, the calculation formula for obtaining numertor and denominator is as follows:
B and a are calculated again:
B=numerator/denominator
A=y [n]-b*x [n].
4. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, in step 3, to direction linear vector after calculating:
Using error be used to indicate the penalty values of each fitting a straight line, this value and original point to fitting a straight line distance at just
Correlation, and when penalty values are more than threshold value, just illustrate that be fitted straight line and original GPS data point gap are excessive, when fitting not
More points should be added；Penalty values are calculated according to range formula
If error is less than threshold, the value of q adds 1, and repeats step 2, will make (x [n+q], y [n+q]) this
Point participates in fitting；If error is more than or equal to threshold, straight line is the backward direction straight line within the scope of coincidence loss,
To the direction vector of straight line after obtaining
5. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, calculating the intercept and slope of forward direction straight line in step 2:
Setting threshold ' is threshold value, the intercept and slope of a ', b ' expression straight line, numerator ' and denominator ' table
Show the molecule and denominator of slope；P, error ' come indicate participate in fitting point number and penalty values；
Initialization: p=2, a '=0, b '=0, error '=0
6. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, calculating forward direction linear vector in step 3:
It is used to indicate the preceding penalty values to straight line of fitting using error, penalty values is calculated according to range formula
If error ' is less than threshold ', the value of p adds 1, and repeats step 4, will make (x [n-p], y [n-p]) this
Point participates in fitting；If error ' is greater than equal threshold ', straight line is the forward direction straight line within the scope of coincidence loss,
To the direction vector of straight line before obtaining
7. the vehicle driving camber angle real-time detection method according to claim 1 based on adaptive least square fitting,
It is characterized in that, in step 4, by arccos function and the angle of both direction is acquired, to obtain n-th point of camber
Camber angle is an；
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811377485.0A CN109490923A (en) | 2018-11-19 | 2018-11-19 | Vehicle driving camber angle real-time detection method based on adaptive least square fitting |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811377485.0A CN109490923A (en) | 2018-11-19 | 2018-11-19 | Vehicle driving camber angle real-time detection method based on adaptive least square fitting |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109490923A true CN109490923A (en) | 2019-03-19 |
Family
ID=65697113
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811377485.0A Pending CN109490923A (en) | 2018-11-19 | 2018-11-19 | Vehicle driving camber angle real-time detection method based on adaptive least square fitting |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109490923A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115480275A (en) * | 2022-09-15 | 2022-12-16 | 中华人民共和国广东海事局 | Motion state acquisition method and device, computer equipment and storage medium |
-
2018
- 2018-11-19 CN CN201811377485.0A patent/CN109490923A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115480275A (en) * | 2022-09-15 | 2022-12-16 | 中华人民共和国广东海事局 | Motion state acquisition method and device, computer equipment and storage medium |
| CN115480275B (en) * | 2022-09-15 | 2023-08-08 | 中华人民共和国广东海事局 | Motion state acquisition method and device, computer equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Andrade et al. | A novel strategy for road lane detection and tracking based on a vehicle’s forward monocular camera | |
| EP3514032B1 (en) | Adjusting velocity of a vehicle for a curve | |
| CN103761737B (en) | Robot motion's method of estimation based on dense optical flow | |
| He et al. | A lane detection method for lane departure warning system | |
| CN107133985A (en) | A kind of vehicle-mounted vidicon automatic calibration method for the point that disappeared based on lane line | |
| Zhang et al. | A real-time curb detection and tracking method for UGVs by using a 3D-LIDAR sensor | |
| CN107284455B (en) | A kind of ADAS system based on image procossing | |
| Zhang et al. | Robust inverse perspective mapping based on vanishing point | |
| JP6756101B2 (en) | Object recognition device | |
| CN109917359B (en) | Robust vehicle distance estimation method based on vehicle-mounted monocular vision | |
| CN107415951A (en) | A kind of road curvature method of estimation based on this car motion state and environmental information | |
| Liu et al. | Development of a vision-based driver assistance system with lane departure warning and forward collision warning functions | |
| Kellner et al. | Road curb detection based on different elevation mapping techniques | |
| CN105513056A (en) | Vehicle-mounted monocular infrared camera external parameter automatic calibration method | |
| JP6171849B2 (en) | Moving body position / posture angle estimation apparatus and moving body position / posture angle estimation method | |
| CN112927309A (en) | Vehicle-mounted camera calibration method and device, vehicle-mounted camera and storage medium | |
| CN109490923A (en) | Vehicle driving camber angle real-time detection method based on adaptive least square fitting | |
| CN103453875A (en) | Real-time calculating method for pitch angle and roll angle of unmanned aerial vehicle | |
| Abadi et al. | Detection of cyclist’s crossing intention based on posture estimation for autonomous driving | |
| CN109920001A (en) | Method for estimating distance based on pedestrian head height | |
| Shu et al. | Vision based lane detection in autonomous vehicle | |
| CN106601076B (en) | A kind of automobile self training device and method based on inertial navigation and area array cameras | |
| Yang | Estimation of vehicle's lateral position via the Lucas-Kanade optical flow method | |
| Damon et al. | Inverse perspective mapping roll angle estimation for motorcycles | |
| CN108615242B (en) | High-speed guardrail tracking 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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190319 |
|
| RJ01 | Rejection of invention patent application after publication |