CN104554277A - Automobile lane changing recognition method based on gravity vector and angular speed vector - Google Patents

Abstract

The invention discloses an automobile lane changing recognition method based on a gravity vector and an angular speed vector. The automobile lane changing recognition method includes acquiring the gravity vector and the angular speed vector through a six-axial sensor fixed on the automobile, reading the quaternion and an angular speed vector of the six-axial sensor, calculating gravity vector of the six-axial sensor, performing the horizontal plane and upslope face calibration to obtain a conversion parameter rotating from a six-axial sensor coordinate system to an automobile coordinate system, mapping the gravity vector and the angular speed vector of the six-axial sensor through the conversion parameter into the gravity vector and the angular speed vector of the automobile, judging whether or not the drive direction is off course through the angular speed vector and calculating the off-course period features; performing off-course period filtering to search possible lane changing combinations; matching the optimal lane changing operation and outputting the lane changing types. By the use of the automobile lane changing recognition method, automobile space states such as horizon, inclining, upslope and downslope are calculated through the gravity vector, the lane changing is recognized by the aid of the angular speed vector, and driving safety and skill is improved.

Description

A kind of automobile lane change recognition methods based on gravity vector and angular velocity vector

Technical field

The present invention relates to the field of research of automobile lane change identification, particularly a kind of automobile lane change recognition methods based on gravity vector and angular velocity vector.

Background technology

Along with increasing gradually of city vehicle quantity, the vehicle of road also gets more and more, and traffic more and more blocks, on vehicle congested traffic road, vehicle may carry out lane change with multiple situation, may be front construction needs, may be the traffic accident in front, also may be only because want to overtake other vehicles.Illegal lane change is very common a kind of traffic violations event, and have the traffic accident exceeding half all to have relation with illegal lane change according to statistics, its harmfulness is huge as can be seen here.So the detection of illegal lane change has important practical significance.Traditional traffic incident detecting system need be buried a large amount of ring coil detector underground or install ultrasonic detector in roadside by physics below road.Their shortcoming ring coil detector needs brokenly the existing road surface of ring to install ground induction coil, all there is the problem of work life, environment for use and reliability deficiency.

Summary of the invention

Main purpose of the present invention is to overcome the shortcoming of prior art with not enough, provides a kind of automobile lane change recognition methods based on gravity vector and angular velocity vector.

In order to achieve the above object, the present invention is by the following technical solutions:

Based on an automobile lane change recognition methods for gravity vector and angular velocity vector, comprise the steps:

(1) obtain gravity vector and angular velocity vector by fixing six axle sensors onboard, read quaternion and the angular velocity vector of six axle sensors;

(2) gravity vector of six axle sensors is calculated according to quaternion;

(3) carry out horizontal surface calibration, make the horizontal surface of the space coordinates of six axle sensors and the space coordinates plane-parallel of automobile;

(4) carry out upper domatic calibration, the system of axes of horizontal surface is overlapped;

(5) after calibration, obtain gravity vector and the angular velocity vector of automobile, judge whether direction goes off course, if then go to step (6), then go to step if not (1);

(6) calculate driftage characteristics of time interval, the described driftage period is change to from automobile steering to come back to the period of straight-line travelling;

(7) period of going off course filters, and the candidate that the sequencing occurred according to the period obtains lane change goes off course sequence;

(8) find possible lane change combination, filter out the combination of all possible driftage period, obtain lane change candidate combinations;

(9) lane change optimum operation is mated;

(10) export the type of lane change, jump to step (1).

Preferably, in step (2), suppose that the quaternion of six axle sensors is for (q1, q2, q3, q4), angular velocity vector is (rx, ry, rz), obtains gravity vector (gx, gy, gz) by following formula:

gx＝2*(q2*q4-q1*q3)；

gy＝-2*(q1*q2+q3*q4)；

gz＝q1*q1-q2*q2-q3*q3+q4*q4；

Wherein read a secondary data every fixing duration.

Preferably, in step (3), the concrete steps of horizontal surface calibration are:

(3.1) automobile parking on level ground, record the gravity vector (cg of six axle sensors _x0, cg _y0, cg _z0), remember that the gravity vector of now automobile is for (vg _x0, vg _y0, vg _z0), wherein vg _x0=0, vg _y0=0, vg _z0=-g, g represent gravitational unit's value, and the gravity vector of six axle sensors rotates the gravity vector obtaining automobile by two steps:

(3.1.1) around Z axis anglec of rotation α, the gravity vector of six axle sensors is made to be $(0,\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2},{\mathrm{cg}}_{z0}),$ Therefore obtain:

$\begin{array}{c}0={\mathrm{cg}}_{x0}*\cos \left(\mathrm{\α}\right)+{\mathrm{cg}}_{y0}*\sin \left(\mathrm{\α}\right)\\ \sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}=-{\mathrm{cg}}_{x0}*\sin \left(\mathrm{\α}\right)+{\mathrm{cg}}_{y0}*\cos \left(\mathrm{\α}\right)\end{array}---\left(1\right)$

Solving equation group (1) obtains anglec of rotation α;

(3.1.2) after (3.1.1), around X-axis anglec of rotation β, the gravity vector of six axle sensors is made to be $(\mathrm{0,0},-\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Wherein $g=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Therefore obtain:

$\begin{array}{c}0=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\cos \left(\mathrm{\β}\right)+{\mathrm{cg}}_{z0}*\sin \left(\mathrm{\β}\right)\\ -\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}=-\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\sin \left(\mathrm{\β}\right)+{\mathrm{cg}}_{z0}*\cos \left(\mathrm{\β}\right)\end{array}---\left(2\right)$

Solving equation group (2) obtains anglec of rotation β;

(3.2) horizontal surface rotation is carried out:

After obtaining horizontal surface collimation angle α, β, under other any times, by the gravity vector (cg of six axle sensors _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(α)+cg _y*sin(α)

vg _y＝(-cg _x*sin(α)+cg _y*cos(α))*cos(β)+cg _z*sin(β)

vg _z＝(cg _x*sin(α)-cg _y*cos(α))*sin(β)+cg _z*cos(β)

vr _x＝cr _x*cos(α)+cr _y*sin(α) (3)

vr _y＝(-cr _x*sin(α)+cr _y*cos(α))*cos(β)+cr _z*sin(β)

vr _z＝(cr _x*sin(α)-cr _y*cos(α))*sin(β)+cr _z*cos(β) 。

Preferably, in step (4), the concrete steps of upper domatic calibration are:

(4.1) storing cycle on slope, keep car the right and left level, record the gravity vector (cg of six axle sensors _x1, cg _y1, cg _z1), after the horizontal surface of step (3.2) rotates, the gravity vector of assembly is designated as (cg _x2, cg _y2, cg _z2), when automobile be placed in upper domatic time, the gravity vector of vehicle is six axle sensor gravity vector can obtain automobile gravity vector after Z axis rotates θ angle, therefore:

$\begin{array}{c}0={\mathrm{cg}}_{x2}*\cos \left(\mathrm{\θ}\right)+{\mathrm{cg}}_{y2}*\sin \left(\mathrm{\θ}\right)\\ \sqrt{{{\mathrm{cg}}_{x2}}^2+{{\mathrm{cg}}_{y2}}^2}=-{\mathrm{cg}}_{x2}*\sin \left(\mathrm{\θ}\right)+{\mathrm{cg}}_{y2}*\cos \left(\mathrm{\θ}\right)\end{array}---\left(4\right)$

Solving equation group (4) obtains anglec of rotation θ;

(4.2) upper domatic rotation;

After selected angle θ on obtaining during domatic calibration, under other any times, by rotating the six axle sensor gravity vector (cg obtained through horizontal surface _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(θ)+cg _y*sin(θ)

vg _y＝-cg _x*sin(θ)+cg _y*cos(θ)

vg _z＝cg _z

vr _x＝cr _x*cos(θ)+cr _y*sin(θ) (5)

vr _y＝-cr _x*sin(θ)+cr _y*cos(θ)

vr _z＝cr _z。

Preferably, in step (5), judge that the method whether direction has gone off course carries out state modeling, its concrete grammar is:

First cireular frequency Z axis component cr is chosen _zsequence, anticlockwise direction is cr _zpositive dirction, following process is done to Z axis sequence:

${\mathrm{cr}}_z=\left\{\begin{array}{c}{\mathrm{cr}}_z,\left|{\mathrm{cr}}_z\right|\≥r_{\min }\\ 0,\mathrm{otherwise}\end{array}\right.---\left(6\right)$

Wherein r _mina cireular frequency limit value through optimizing, when | cr _z| <r _mintime think that the change of cireular frequency causes due to device measuring error, now car is in straight travel state; When | cr _z|>=r _mintime think that automobile steering there occurs change.

Preferably, in step (6), the method calculating driftage characteristics of time interval is:

Obtain the driftage period sequence of vehicle travel according to good state model, for driftage period i, the note time opening is tb _i, the end time is te _i, during this period of time vehicle level cireular frequency maxim is designated as max_cr _i, cireular frequency integrated value is designated as total_cr _i, driftage time length is tl _i=te _i-tb _i, represent yaw direction with dire, work as total_cr _ileft drift dire=1 is designated as during >0; Work as total_cr _iright avertence boat dire=2 is designated as during <0.

Preferably, in step (7), the method that the driftage period filters is:

In normal driving procedure, the yaw angle of automobile in lane change and time length have certain value range, set the minimum limit value min_tl of suitable time length, threshold limit value max_tl, the minimum limit value m_cr of driftage maximum angular rate, driftage maximum angle integration mt_cr, when the period of going off course meets following all conditions, judge that this driftage period goes off course the period as lane change candidate:

tl _i≥min_tl

tl _i≤max_tl

max_cr _i≥m_cr (7)

total_cr _i≤mt_cr

The driftage period, the candidate that the sequencing occurred according to the period obtains lane change went off course sequence <mod after filtering ₁..., mod _i>.

Preferably, in step (8), two driftage the period likely form lane change operation necessary condition be:

1) two driftage periods are adjacent block;

2) direction of two driftage periods is contrary, i.e. dire _i≠ dire _i+1;

3) two driftage periods cover total duration and are no more than duration △ T ₁, i.e. te _i+1-tb _i≤ △ T ₁;

4) interval time of two driftage periods is no more than duration △ T ₂, i.e. tb _i+1-te _i≤ △ T ₂;

5) the cireular frequency integrated absolute sum of two driftage periods exceedes the suitable limit value minAll of setting, namely | and total_cr _i|+total_cr _i+1|>=minAll;

6) the cireular frequency absolute value only poor suitable limit value max_d being no more than setting of two driftage periods, namely || total_cr _i|-total_cr _i+1||≤max_d;

Through above judgement necessary condition, filter out all possible driftage period combinations, obtain lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) >.

Preferably, step (9) is specially:

For appearing at lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) mod in > _i, when only there is (mod _i, mod _i+1) or (mod _i-1, mod _i) time, then (mod _i, mod _i+1) or (mod _i-1, mod _i) be directly judged to be lane change; As (mod _i, mod _i+1), (mod _i-1, mod _i) when all existing, if mod _iwith mod _i+1cireular frequency integration differential than mod _iwith mod _i-1cireular frequency integration differential hour, judge (mod _i, mod _i+1) be lane change, otherwise judge (mod _i-1, mod _i) be lane change.

Preferably, in step (10), obtaining lane change (mod _i, mod _i+1) after, work as mod _iduring for the left drift period, be lane change left, otherwise be right lane change; The lane change time opening is tb _i, the end time is te _i+1.

Principle of work of the present invention is: obtain gravity vector and angular velocity vector by fixing six axle sensors onboard, read quaternion (a kind of corner describing method of six axle sensors, by rotating shaft vector (x, y, z) and corner w form) and angular velocity vector, the gravity vector of six axle sensors is calculated according to quaternion, carry out horizontal surface calibration again and upper domatic calibration obtains the conversion parameter that six axle sensor system of axess rotate to vehicle axis system, in driving procedure, by conversion parameter, the gravity vector of six axle sensors and angular velocity vector are mapped to gravity vector and the angular velocity vector of vehicle, use cireular frequency judge drive direction whether go off course-and calculate go off course characteristics of time interval, then carry out the driftage period to filter, find and may lane change combine, finally mate optimum lane change operation, the type of-output lane change.The present invention uses gravity vector can calculate the spatiality of automobile as level, inclination, upward slope, descending etc., uses angular velocity vector identification vehicle lane change.By identifying the lane change operation of chaufeur, the lane change behavior for danger is pointed out, and can improve traffic safety and driving quality.

Compared with prior art, tool has the following advantages and beneficial effect in the present invention:

The present invention detects the lane change behavior of automobile by gravity vector and angular velocity vector, by installing six axle sensors of existing maturation on the market, to be fixedly installed on automobile and to carry out horizontal surface and dip plane calibration, making six axle sensor system of axess overlap with vehicle chassis system of axes and obtain gravity vector and the angular velocity vector of automobile.Use gravity vector can calculate the spatiality of automobile as level, inclination, upward slope, descending etc., use angular velocity vector identification vehicle lane change.By identifying the lane change operation of chaufeur, the lane change behavior for danger is pointed out, and can improve traffic safety and driving quality.Compared with the method based on image recognition, the present invention is not by the impact of the factors such as mist, rain, snow, sand and dust, hail.

Accompanying drawing explanation

Fig. 1 is diagram of circuit of the present invention.

Detailed description of the invention

Below in conjunction with embodiment and accompanying drawing, the present invention is described in further detail, but embodiments of the present invention are not limited thereto.

Embodiment

As shown in Figure 1, the present invention is based on the automobile lane change recognition methods of gravity vector and angular velocity vector, comprise three parts: horizontal surface calibration, upper domatic calibration and lane change identification, wherein horizontal surface calibration and upper domatic be aligned in vehicle stationary state under complete; Lane change is identified in the normal driving procedure of automobile and completes, and starts data flow and read during automobile starting, stops identifying at the end of stroke; Its concrete steps are as follows:

1. the acquisition of gravity vector and angular velocity vector data

Obtain gravity vector and angular velocity vector by six axle sensors, read quaternion (q1, q2, the q3 of six axle sensors, and angular velocity vector (rx, ry, rz) q4), gravity vector (gx, gy, gz) is obtained by following formula:

gx＝2*(q2*q4-q1*q3)；

gy＝-2*(q1*q2+q3*q4)；

gz＝q1*q1-q2*q2-q3*q3+q4*q4。

A secondary data is read every fixing duration.

2. calibrate

The vector read from six axle sensors is the numerical value under the space coordinates of assembly, and the space coordinates of automobile do not overlap with the space coordinates of six axle sensors, the object of calibration is that gravity vector and angular velocity vector are rotated the space coordinates to automobile from the space coordinates of assembly.

Comprise horizontal surface calibration and upper domatic calibration from six axle sensor space coordinates rotations to the calibration process of motor space system of axes, be divided into 4 steps:

2-1. obtains horizontal surface calibration parameter

The object of horizontal surface calibration is the horizontal surface of the space coordinates making six axle sensors and the space coordinates plane-parallel of automobile.Rotate from six axle sensor space coordinates and need to know Z axis anglec of rotation α and X-axis anglec of rotation β to motor space system of axes.First automobile parking on level ground, record the gravity vector (cg of six axle sensors _x0, cg _y0, cg _z0), remember that in the gravity vector of this moment automobile be (vg _x0, vg _y0, vg _z0), wherein vg _x0=0, vg _y0=0, vg _z0=-g, g represent gravitational unit's value.The gravity vector of six axle sensors rotates the gravity vector obtaining automobile by two steps:

1) around Z axis anglec of rotation α, the gravity vector of six axle sensors is made to be therefore obtain:

$\begin{array}{c}0={\mathrm{cg}}_{x0}*\cos \left(\mathrm{\&alpha;}\right)+{\mathrm{cg}}_{y0}*\sin \left(\mathrm{\&alpha;}\right)\\ \sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}=-{\mathrm{cg}}_{x0}*\sin \left(\mathrm{\&alpha;}\right)+{\mathrm{cg}}_{y0}*\cos \left(\mathrm{\&alpha;}\right)\end{array}---\left(1\right)$

Solving equation group (1) obtains anglec of rotation α.

2) through 1) after, around X-axis anglec of rotation β, make the gravity vector of six axle sensors to be $(\mathrm{0,0},-\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Wherein $g=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Therefore obtain:

$\begin{array}{c}0=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\cos \left(\mathrm{\&beta;}\right)+{\mathrm{cg}}_{z0}*\sin \left(\mathrm{\&beta;}\right)\\ -\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}=-\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\sin \left(\mathrm{\&beta;}\right)+{\mathrm{cg}}_{z0}*\cos \left(\mathrm{\&beta;}\right)\end{array}---\left(2\right)$

Solving equation group (2) obtains anglec of rotation β.

2-2. horizontal surface rotates

After obtaining horizontal surface collimation angle α, β, under other any times, by the gravity vector (cg of six axle sensors _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(α)+cg _y*sin(α)

vg _y＝(-cg _xvsin(α)+cg _y*cos(α))*cos(β)+cg _z*sin(β)

vg _z＝(cg _x*sin(α)-cg _y*cos(α))*sin(β)+cg _z*cos(β)

(3)

vr _x＝cr _x*cos(α)+cr _y*sin(α)

vr _y＝(-cr _x*sin(α)+cr _y*cos(α))*cos(β)+cr _z*sin(β)

vr _z＝(cr _x*sin(α)-cr _y*cos(α))*sin(β)+cr _z*cos(β)

Domatic calibration parameter in 2-3. acquisition

Six axle sensor system of axes horizontal surfaces and vehicle axis system plane-parallel after rotating through horizontal surface, the system of axes of right horizontal surface is still for overlapping, the object of upper domatic calibration is that the system of axes of horizontal surface is also overlapped, and namely obtains the anglec of rotation θ of six axle sensors and vehicle level plane coordinates system.

Storing cycle on slope, keep car the right and left level, record the gravity vector (cg of six axle sensors _x1, cg _y1, cg _z1), after the horizontal surface of 2-2 rotates, the gravity vector of six axle sensors is designated as (cg _x2, cg _y2, cg _z2), when automobile be placed in upper domatic time, the gravity vector of vehicle is six axle sensor gravity vector can obtain automobile gravity vector after Z axis rotates θ angle, therefore:

$\begin{array}{c}0={\mathrm{cg}}_{x2}*\cos \left(\mathrm{\&theta;}\right)+{\mathrm{cg}}_{y2}*\sin \left(\mathrm{\&theta;}\right)\\ \sqrt{{{\mathrm{cg}}_{x2}}^2+{{\mathrm{cg}}_{y2}}^2}=-{\mathrm{cg}}_{x2}*\sin \left(\mathrm{\&theta;}\right)+{\mathrm{cg}}_{y2}*\cos \left(\mathrm{\&theta;}\right)\end{array}---\left(4\right)$

Solving equation group (4) obtains anglec of rotation θ.

The upper domatic rotation of 2-4.

After selected angle θ on obtaining during domatic calibration, under other any times, by rotating the six axle sensor gravity vector (cg obtained through horizontal surface _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(θ)+cg _y*sin(θ)

vg _y＝-cg _x*sin(θ)+cg _y*cos(θ)

vg _z＝cg _z(5)

vr _x＝cr _x*cos(θ)+cr _y*sin(θ)

vr _y＝-cr _x*sin(θ)+cr _y*cos(θ)

vr _z＝cr _z

3. automobile departs from working direction identification

Gravity vector and the angular velocity vector of automobile is obtained, wherein cireular frequency Z axis component statement automobile cireular frequency in the horizontal plane after calibration.Before automobile lane change, automobile working direction can depart from, and after lane change terminates, working direction keeps again, and the identification that automobile departs from working direction is the period in order to cut out lane change, comprises following three steps:

The modeling of 3-1. state

First cireular frequency Z axis component cr is chosen _zsequence, anticlockwise direction is cr _zpositive dirction, following process is done to Z axis sequence:

${\mathrm{cr}}_z=\left\{\begin{array}{c}{\mathrm{cr}}_z,\left|{\mathrm{cr}}_z\right|\&GreaterEqual;r_{\min }\\ 0,\mathrm{otherwise}\end{array}\right.---\left(6\right)$

Wherein r _mina cireular frequency limit value through optimizing, when | cr _z| <r _mintime think that the change of cireular frequency causes due to device measuring error, now car is in straight travel state; When | cr _z|>=r _mintime think that automobile steering there occurs change.

3-2. driftage characteristics of time interval is selected

Claiming changes to from automobile steering comes back to straight-line travelling during this period of time for going off course the period.The state model built up according to 3-1 obtains the driftage period sequence of vehicle travel, and for driftage period i, the note time opening is tb _i, the end time is te _i, during this period of time vehicle level cireular frequency maxim (referring to absolute value) is designated as max_cr _i, cireular frequency integrated value is designated as total_cr _i, driftage time length is tl _i=te _i-tb _i, represent yaw direction with dire, work as total_cr _ileft drift dire=1 is designated as during >0; Work as total_cr _iright avertence boat dire=2 is designated as during <0.

The 3-3. driftage period filters

In normal driving procedure, the yaw angle of automobile in lane change and time length have certain value range, set the minimum limit value min_tl of suitable time length, threshold limit value max_tl, the minimum limit value m_cr of driftage maximum angular rate, driftage maximum angle integration mt_cr, when the period of going off course meets following all conditions, judge that this driftage period goes off course the period as lane change candidate:

tl _i≥min_tl

tl _i≤max_tl (7)

max_cr _i≥m_cr

total_cr _i≤mt_cr

The driftage period, the candidate that the sequencing occurred according to the period obtains lane change went off course sequence <mod after filtering ₁..., mod _i>.

4. based on the lane change identification of optimal characteristics coupling

In normal lane change left, vehicle there will be two driftage periods: have a right avertence boat period after first having a left drift period; In like manner, lane change to the right also there will be two driftage periods: have a left drift period after first having a right avertence boat period.Therefore identify that the target of lane change operation is to combine the optimum adjacent driftage period.

4-1. finds possible lane change combination

Two driftage the period likely form lane change operation necessary condition be:

1) two driftage periods are adjacent block;

2) direction of two driftage periods is contrary, i.e. dire _i≠ dire _i+1;

3) two driftage periods cover total duration and are no more than duration △ T ₁, i.e. te _i+1-tb _i≤ △ T ₁;

4) interval time of two driftage periods is no more than duration △ T ₂, i.e. tb _i+1-te _i≤ △ T ₂;

5) the cireular frequency integrated absolute sum of two driftage periods exceedes the suitable limit value minAll of setting, namely | and total_cr _i|+total_cr _i+1|>=minAll;

6) the cireular frequency absolute value only poor suitable limit value max_d being no more than setting of two driftage periods, namely || total_cr _i|-total_cr _i+1||≤max_d.

Through above judgement necessary condition, filter out all possible driftage period combinations, obtain lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) >.

The driftage period that 4-2. coupling is optimum

For appearing at lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) mod in > _i, when only there is (mod _i, mod _i+1) or (mod _i-1, mod _i) time, then (mod _i, mod _i+1) or (mod _i-1, mod _i) be directly judged to be lane change; As (mod _i, mod _i+1), (mod _i-1, mod _i) when all existing, if mod _iwith mod _i+1cireular frequency integration differential than mod _iwith mod _i-1cireular frequency integration differential hour, judge (mod _i, mod _i+1) be lane change, otherwise judge (mod _i-1, mod _i) be lane change.

4-3. lane change result exports

Obtaining lane change (mod _i, mod _i+1) after, work as mod _iduring for the left drift period, be lane change left, otherwise be right lane change; The lane change time opening is tb _i, the end time is te _i+1.

Above-described embodiment is the present invention's preferably embodiment; but embodiments of the present invention are not restricted to the described embodiments; change, the modification done under other any does not deviate from Spirit Essence of the present invention and principle, substitute, combine, simplify; all should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (10)

1., based on an automobile lane change recognition methods for gravity vector and angular velocity vector, it is characterized in that, comprise the steps:

(1) obtain gravity vector and angular velocity vector by fixing six axle sensors onboard, read quaternion and the angular velocity vector of six axle sensors;

(2) gravity vector of six axle sensors is calculated according to quaternion;

(3) carry out horizontal surface calibration, make the horizontal surface of the space coordinates of six axle sensors and the space coordinates plane-parallel of automobile;

(4) carry out upper domatic calibration, the system of axes of horizontal surface is overlapped;

(5) after calibration, obtain gravity vector and the angular velocity vector of automobile, judge whether direction goes off course, if then go to step (6), then go to step if not (1);

(6) calculate driftage characteristics of time interval, the described driftage period is change to from automobile steering to come back to the period of straight-line travelling;

(7) period of going off course filters, and the candidate that the sequencing occurred according to the period obtains lane change goes off course sequence;

(8) find possible lane change combination, filter out the combination of all possible driftage period, obtain lane change candidate combinations;

(9) lane change optimum operation is mated;

(10) export the type of lane change, jump to step (1).

2. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 1, is characterized in that, in step (2), suppose that the quaternion of six axle sensors is for (q1, q2, q3, q4), angular velocity vector is (rx, ry, rz), obtain gravity vector (gx by following formula, gy, gz):

gx＝2*(q2*q4-q1*q3)；

gy＝-2*(q1*q2+q3*q4)；

gz＝q1*q1-q2*q2-q3*q3+q4*q4；

Wherein read a secondary data every fixing duration.

3. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 1, is characterized in that, in step (3), the concrete steps of horizontal surface calibration are:

(3.1) automobile parking on level ground, record the gravity vector (cg of six axle sensors _x0, cg _y0, cg _z0), remember that the gravity vector of now automobile is for (vg _x0, vg _y0, vg _z0), wherein vg _x0=0, vg _y0=0, vg _z0=-g, g represent gravitational unit's value, and the gravity vector of six axle sensors rotates the gravity vector obtaining automobile by two steps:

(3.1.1) around Z axis anglec of rotation α, the gravity vector of six axle sensors is made to be $(0,\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2},{\mathrm{cg}}_{z0}),$ Therefore obtain:

0＝cg _x0*cos(α)+cg ₀₀*sin(α)

(1)

$\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}=-c{g}_{x0}*\sin \left(\mathrm{\&alpha;}\right)+{\mathrm{cg}}_{y0}*\cos \left(\mathrm{\&alpha;}\right)$

Solving equation group (1) obtains anglec of rotation α;

(3.1.2) after (3.1.1), around X-axis anglec of rotation β, the gravity vector of six axle sensors is made to be $(\mathrm{0,0},-\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Wherein $g=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{z0}}^2}),$ Therefore obtain:

$\begin{array}{c}0=\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\cos \left(\mathrm{\&beta;}\right)+{\mathrm{cg}}_{z0}*\sin \left(\mathrm{\&beta;}\right)\\ -\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2+{{\mathrm{cg}}_{z0}}^2}=-1\sqrt{{{\mathrm{cg}}_{x0}}^2+{{\mathrm{cg}}_{y0}}^2}*\sin \left(\mathrm{\&beta;}\right)+{\mathrm{cg}}_{z0}*\cos \left(\mathrm{\&beta;}\right)\end{array}---\left(2\right)$

Solving equation group (2) obtains anglec of rotation β;

(3.2) horizontal surface rotation is carried out:

After obtaining horizontal surface collimation angle α, β, under other any times, by the gravity vector (cg of six axle sensors _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(α)+cg _y*sin(α)

vg _y＝(-cg _x*sin(α)+cg _y*cos(α))*cos(β)+cg _z*sin(β)

vg _z＝(cg _x*sin(α)-cg _y*cos(α))*sin(β)+cg _z*cos(β)

vr _x＝cr _x*cos(α)+cr _y*sin(α) (3)

vr _y＝(-cr _x*sin(α)+cr _y*cos(α))*cos(β)+cr _z*sin(β)

vr _z＝(cr _x*sin(α)-cr _y*cos(α))*sin(β)+cr _z*cos(β)

。

4. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 3, it is characterized in that, in step (4), the concrete steps of upper domatic calibration are:

(4.1) storing cycle on slope, keep car the right and left level, record the gravity vector (cg of six axle sensors _x1, cg _y1, cg _z1), after the horizontal surface of step (3.2) rotates, the gravity vector of assembly is designated as (cg _x2, cg _y2, cg _z2), when automobile be placed in upper domatic time, the gravity vector of vehicle is six axle sensor gravity vector can obtain automobile gravity vector after Z axis rotates θ angle, therefore:

0＝cg _x2*cos(θ)+cg _y2*sin(θ)

(4)

$\sqrt{{{\mathrm{cg}}_{x2}}^2+{{\mathrm{cg}}_{y2}}^2}=-{\mathrm{cg}}_{x2}*\sin \left(\mathrm{\&theta;}\right)+{\mathrm{cg}}_{y2}*\cos \left(\mathrm{\&theta;}\right)$

Solving equation group (4) obtains anglec of rotation θ;

(4.2) upper domatic rotation;

After selected angle θ on obtaining during domatic calibration, under other any times, by rotating the six axle sensor gravity vector (cg obtained through horizontal surface _x, cg _y, cg _z) and angular velocity vector (cr _x, cr _y, cr _z) obtain the gravity vector (vg of automobile _x, vg _y, vg _z) and angular velocity vector (cr _x, cr _y, cr _z):

vg _x＝cg _x*cos(θ)+cg _y*sin(θ)

vg _y＝-cg _x*sin(θ)+cg _y*cos(θ)

vg _z＝cg _z

vr _x＝cr _x*cos(θ)+cr _y*sin(θ) (5)

vr _y＝-cr _x*sin(θ)+cr _y*cos(θ)

vr _z＝cr _z。

5. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 4, it is characterized in that, in step (5), judge that the method whether direction has gone off course carries out state modeling, its concrete grammar is:

First cireular frequency Z axis component cr is chosen _zsequence, anticlockwise direction is cr _zpositive dirction, following process is done to Z axis sequence:

${\mathrm{cr}}_z=\left\{\begin{array}{c}{\mathrm{cr}}_z,\left|{\mathrm{cr}}_z\right|\&GreaterEqual;r_{\min }\\ 0,\mathrm{otherwise}\end{array}\right.---\left(6\right)$

Wherein r _mina cireular frequency limit value through optimizing, when | cr _z| <r _mintime think that the change of cireular frequency causes due to device measuring error, now car is in straight travel state; When | cr _z|>=r _mintime think that automobile steering there occurs change.

6. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 5, is characterized in that, in step (6), the method calculating driftage characteristics of time interval is:

Obtain the driftage period sequence of vehicle travel according to good state model, for driftage period i, the note time opening is tb _i, the end time is te _i, during this period of time vehicle level cireular frequency maxim is designated as max_cr _i, cireular frequency integrated value is designated as total_cr _i, driftage time length is tl _i=te _i-tb _i, represent yaw direction with dire, work as total_cr _ileft drift dire=1 is designated as during >0; Work as total_cr _iright avertence boat dire=2 is designated as during <0.

7. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 5, is characterized in that, in step (7), the method that the driftage period filters is:

In normal driving procedure, the yaw angle of automobile in lane change and time length have certain value range, set the minimum limit value min_tl of suitable time length, threshold limit value max_tl, the minimum limit value m_cr of driftage maximum angular rate, driftage maximum angle integration mt_cr, when the period of going off course meets following all conditions, judge that this driftage period goes off course the period as lane change candidate:

tl _i≥min_tl

tl _i≤max_tl

max_cr _i≥m_cr (7)

total_cr _i≤mt_cr

The driftage period, the candidate that the sequencing occurred according to the period obtains lane change went off course sequence <mod after filtering ₁..., mod _i>.

8. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 1, is characterized in that, in step (8), the necessary condition that two driftage periods likely form lane change operation is:

1) two driftage periods are adjacent block;

2) direction of two driftage periods is contrary, i.e. dire _i≠ dire _i+1;

3) two driftage periods cover total duration and are no more than duration △ T ₁, i.e. te _i+1-tb _i≤ △ T ₁;

4) interval time of two driftage periods is no more than duration △ T ₂, i.e. tb _i+1-te _i≤ △ T ₂;

5) the cireular frequency integrated absolute sum of two driftage periods exceedes the suitable limit value min All of setting, namely | and total_cr _i|+total_cr _i+1|>=min All;

6) the cireular frequency absolute value only poor suitable limit value max_d being no more than setting of two driftage periods, namely || total_cr _i|-total_cr _i+1||≤max_d;

Through above judgement necessary condition, filter out all possible driftage period combinations, obtain lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) >.

9. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 1, it is characterized in that, step (9) is specially:

For appearing at lane change candidate combinations < (mod _i, mod _i+1) ..., (mod _j, mod _j+1) mod in > _i, when only there is (mod _i, mod _i+1) or (mod _i-1, mod _i) time, then (mod _i, mod _i+1) or (mod _i-1, mod _i) be directly judged to be lane change; As (mod _i, mod _i+1), (mod _i-1, mod _i) when all existing, if mod _iwith mod _i+1cireular frequency integration differential than mod _iwith mod _i-1cireular frequency integration differential hour, judge (mod _i, mod _i+1) be lane change, otherwise judge (mod _i-1, mod _i) be lane change.

10. the automobile lane change recognition methods based on gravity vector and angular velocity vector according to claim 1, is characterized in that, in step (10), obtaining lane change (mod _i, mod _i+1) after, work as mod _iduring for the left drift period, be lane change left, otherwise be right lane change; The lane change time opening is tb _i, the end time is te _i+1.