CN108189754B - A kind of automobile anti-collision method of multipoint mode active probe - Google Patents

Abstract

The invention discloses a multi-point active detection automobile anti-collision method, comprising the steps of: 1. establishing a multi-point active detection automobile anti-collision system; 2. constructing an independent coordinate system based on the vehicle body; 3. acquiring moving obstacles Real-time position; 4. Fit and predict the trajectory of moving obstacles; 5. Display and early warning of moving obstacle trajectories. The present invention utilizes no less than three radar detectors on the four side panels of the automobile to actively detect moving obstacles in real time, and performs identification, detection and tracking of static and dynamic objects on the moving obstacles, and utilizes the moving obstacles and the car body The distance and the azimuth of the car body are used to obtain the coordinates of the moving obstacle, and the coordinate accuracy of the moving obstacle is improved by taking the average value of multiple calculations, and the trajectory of the moving obstacle is obtained by curve fitting, so that the driver can be aware of it in advance. to possible dangers.

Description

A Multi-point Active Detection Vehicle Collision Avoidance Method

technical field

The invention belongs to the technical field of automobile anti-collision, and in particular relates to a multi-point active detection automobile anti-collision method.

Background technique

As far as the current development of automobile technology is concerned, blind spots in driving vision are still unavoidable. The angle of view of the interior rearview mirror and left and right rearview mirrors is limited. Even if reversing images and reversing sensors are installed, they can only play an auxiliary role, especially for novices. , Inexperienced driving, can only rely on rearview mirrors, reversing images and radar, unable to accurately judge the situation in blind spots, there is a great potential safety hazard, the existing traffic conditions are becoming more and more complicated, accidents happen from time to time, single point The traditional radar detection method cannot meet the potential safety hazards brought by any direction around the car; therefore, there is a lack of a car collision avoidance method with reasonable design and simple steps. The three radar detectors actively detect moving obstacles in real time, and identify, detect and track static and dynamic objects for moving obstacles, so as to let the driver be aware of possible dangers in advance.

Contents of the invention

The technical problem to be solved by the present invention is to provide a multi-point active detection automobile collision avoidance method for the deficiencies in the above-mentioned prior art. Detect moving obstacles, identify, detect and track static and dynamic objects for moving obstacles, use the distance between moving obstacles and the car body and the azimuth angle of the car body to obtain the coordinates of moving obstacles, and obtain them through multiple calculations The average method improves the coordinate accuracy of moving obstacles, and the trajectory of moving obstacles is obtained by curve fitting, so that drivers can be aware of possible dangers in advance, which is convenient for popularization and use.

In order to solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a multi-point active detection automobile collision avoidance method, characterized in that the method comprises the following steps:

Step 1. Establish a multi-point active detection automobile collision avoidance system: install no less than three points on the front side panel, rear side panel, left side panel and right side panel of the car body for active detection of moving obstacles. position radar detectors, and establish a vehicle anti-collision subsystem for processing the data detected by each radar detector. The vehicle anti-collision subsystem and radar detectors form a multi-point active detection vehicle anti-collision system system; the automobile anti-collision subsystem includes a vehicle-mounted controller and an electronic vertical angle measuring instrument installed on the chassis of the automobile body for detecting the vertical angle of the automobile body, and is connected with the output terminal of the vehicle-mounted controller The connected display screen and the alarm for prompting the driver to avoid moving obstacles, the electronic vertical angle measuring instrument and the radar detector are all connected with the input end of the on-board controller;

The radar detectors installed on the front side panel, rear side panel, left side panel and right side panel of the car body are all located on the same plane;

Step 2. Build an independent coordinate system based on the body: take the geometric center position of the plane where multiple radar detectors on the same plane are located as the coordinate origin O, take the front of the car body moving straight ahead as the positive direction of the Y axis, and take The direction perpendicular to the Y axis and pointing from the left side panel to the right side panel is the positive direction of the X axis, and the direction vertically perpendicular to the Y axis and pointing from the chassis of the car body to the roof of the car body is the positive direction of the Z axis. The independent coordinate system O-XYZ of the body;

Step 3. Obtain the real-time position of the moving obstacle, the process is as follows:

Step 301, determine effective radar detectors: use multiple radar detectors to simultaneously detect the distances of moving obstacles from multiple radar detectors, and transmit multiple distance data to the on-board controller in real time, and the on-board controller calculates the distance between multiple distances The data is sorted from small to large, and the radar detectors corresponding to the first three distance data after sorting are set to be effective radar detectors, and the three effective radar detectors are three consecutive radar detectors. The radar detectors are the first effective radar detector A, the second effective radar detector B and the third effective radar detector C in turn, and the position of the moving obstacle is set as point P;

Step 302, according to the formula Calculate the reference azimuth angle between the second effective radar detector B and the first effective radar detector A Among them, X _A is the X-axis coordinate of the first effective radar detector A, Y _A is the Y-axis coordinate of the first effective radar detector A, X _B is the X-axis coordinate of the second effective radar detector B, and Y _B is The Y-axis coordinate of the second effective radar detector B,

According to the formula Calculate the reference azimuth angle between the third effective radar detector C and the second effective radar detector B Wherein, X _C is the X-axis coordinate of the third effective radar detector C, and Y _C is the Y-axis coordinate of the third effective radar detector C,

Step 303. Determine the azimuth α _BA between the second effective radar detector B and the first effective radar detector A:

When X _A -X _B >0 and Y _A -Y _B >0, the azimuth

When X _A -X _B <0 and Y _A -Y _B >0, the azimuth

When X _A -X _B <0 and Y _A -Y _B <0, the azimuth

When X _A -X _B >0 and Y _A -Y _B <0, the azimuth

Determine the azimuth α _CB between the third effective radar detector C and the second effective radar detector B:

When X _B -X _C >0 and Y _B -Y _C >0, the azimuth

When X _B -X _C <0 and Y _B -Y _C >0, the azimuth

When X _B -X _C <0 and Y _B -Y _C <0, the azimuth

When X _B -X _C >0 and Y _B -Y _C <0, the azimuth

Step 304, according to the formula Calculate the azimuth α _AP between the first effective radar detector A and point P and the initial azimuth α' _BP between the second effective radar detector B and point P, where ∠BAP is the angle between L _BA and L _AP and ∠ABP is the angle between L _BA and L _BP and L _BA is the line segment from the second effective radar detector B to the first effective radar detector A, L _AP is the line segment from the first effective radar detector A to point P, and L _BP is the line segment from the second effective radar detector B to point P , α _AB is the azimuth angle from the first effective radar detector A to the second effective radar detector B;

According to the formula Calculate the updated azimuth α' _B ' _P between the second effective radar detector B and point P and the azimuth α _CP between the third effective radar detector C and point P, where ∠CBP is the angle between L _CB and L _BP and ∠BCP is the angle between L _CB and L _CP and L _CB is the line segment from the third effective radar detector C to the second effective radar detector B, L _CP is the line segment from the third effective radar detector C to point P, α _BC is the line segment from the second effective radar detector B to the third the azimuth of the effective radar detector C;

According to the formula Calculate the azimuth α _BP between the second effective radar detector B and the point P;

Step 305, according to the formula Calculate the orientation angle β _{AP of L AP ;}

According to the formula Calculate the orientation angle β _{BP of L BP ;}

According to the formula Calculate the orientation angle β _{CP of L CP ;}

Step 306, according to the formula Calculate the direction cosine γ _APx of the first effective radar detector A and point P on the X axis, the direction cosine γ _APy of the first effective radar detector A and point P on the Y axis, the first effective radar detector A and point P The direction cosine γ _APz of P in the Z axis, wherein, V is the vertical angle of the automobile body measured by the electronic vertical angle measuring instrument;

According to the formula Calculate the direction cosine γ _BPx of the second effective radar detector B and point P on the X axis, the direction cosine γ _BPy of the second effective radar detector B and point P on the Y axis, the second effective radar detector B and point P The direction cosine γ _BPz of P in the Z axis;

According to the formula Calculate the direction cosine γ _CPx of the third effective radar detector C and point P on the X axis, the direction cosine γ _CPy of the third effective radar detector C and point P on the Y axis, the third effective radar detector C and point P The direction cosine γ _CPz of P in the Z axis;

Step 307, according to the formula Calculate the initial first coordinates (X _AP , Y _AP , Z _AP ) of point P, where ΔX _AP is the component of L _AP on the X axis and ΔX _AP = L _AP *γ _APx , and ΔY _AP is the component of L _AP on the Y axis Component and ΔY _AP =L _AP *γ _APy , ΔZ _AP is the component of L _AP on the Z axis and ΔZ _AP =L _AP *γ _APz ;

According to the formula Calculate the initial second coordinates (X _BP , Y _BP , Z _BP ) of point P, where ΔX _BP is the component of L _BP on the X axis and ΔX _BP =L _BP *γ _BPx , ΔY _BP is the component of L _BP on the Y axis Component and ΔY _BP =L _BP *γ _BPy , ΔZ _BP is the component of L _BP on the Z axis and ΔZ _BP =L _BP *γ _BPz ;

According to the formula Calculate the initial third coordinate (X _CP , Y _CP , Z _CP ) of point P, where ΔX _CP is the component of L _CP on the X axis and ΔX _CP =L _CP *γ _CPx , ΔY _CP is the component of L _CP on the Y axis Component and ΔY _CP =L _CP *γ _CPy , ΔZ _CP is the component of L _CP on the Z axis and ΔZ _CP =L _CP *γ _CPz ;

Step 308, according to the formula Calculate the updated coordinates (X _P , Y _P , Z _P ) of point P;

Step 309, use the first effective radar detector A, the second effective radar detector B and the third effective radar detector C to actively detect the moving obstacle distance n times, and loop steps 304 to 308 for n times to obtain n times different The updated coordinates of point P at each time, to obtain the real-time position of moving obstacles, and the updated coordinates of point P at n times at different times constitute the observation data set Q={(x ₁ ,y ₁ ,z ₁ ),(x ₂ ,y ₂ ,z ₂ ),...,( _xi ,y _i , _zi ),...,(x _n ,y _n ,z _n )}, where n is a positive integer not less than 4, and i is a valid The detection times of the radar detector are numbered and 1≤i≤n, ( _xi , y, _{zi )} is the update coordinate of the point P calculated by the on-board controller for the ith time;

Step 4. Fitting and predicting the trajectory of moving obstacles, the process is as follows:

Step 401, Obtain the effective observation data set Q': use the on-board controller to obtain the observation data set Q={(x ₁ ,y ₁ ,z ₁ ),(x ₂ ,y ₂ ,z ₂ ),...,(x _i ,y _i ,z _i ),...,(x _n ,y _n ,z _n )} do dimension reduction processing, select the X-axis coordinate value and Y-axis coordinate value of the update coordinates of point P to form an effective observation data set Q', effective observation data set Q'={(x ₁ ,y ₁ ),(x ₂ ,y ₂ ),...,( _xi ,y _i ),...,(x _n ,y _n ) };

Step 402, determine the basis function family and construct the quadratic polynomial: the on-board controller sets the basis function family, and the basis function family includes δ ₀ (x)=1, δ ₁ (x)=x and δ ₂ (x)= x ² , the on-board controller constructs the quadratic polynomial s(x)=a ₀ δ ₀ (x)+a ₁ δ ₁ (x)+a ₂ δ ₂ (x), where a ₀ is the quadratic polynomial s(x ), a ₁ is the coefficient of δ ₁ ( _x ) in the quadratic polynomial s(x), a ₂ is the coefficient of δ ₂ (x) in the quadratic polynomial s(x);

Step 403, according to the formula Compute the coefficient matrix of the quadratic polynomial s(x) And into the quadratic polynomial s(x)=a ₀ +a ₁ x+a ₂ x ² , the on-board controller obtains the fitting curve to realize the prediction of the trajectory of moving obstacles, where δ ₀ is defined by δ ₀ (x) = n-dimensional vector composed of 1 and δ ₀ =[1,1,L,1,L,1] ^T , δ ₁ is an n-dimensional vector composed of δ ₁ (x)=x and δ ₁ =[x ₁ , x ₂ ,L, _xi ,L,x _n ] ^T , δ ₂ is an n-dimensional vector composed of δ ₂ (x)=x ² and

Step 5. Track display and early warning of moving obstacles: Use the display screen to display the moving tracks of moving obstacles in real time, and set the safety distance threshold between moving obstacles and the car body through the on-board controller. When the distance is not greater than the safety distance threshold, the on-board controller controls the alarm to give an early warning.

The automobile collision avoidance method of above-mentioned a kind of multi-point type active detection is characterized in that: in step 304, according to the formula Calculate the reference azimuth angle between the first effective radar detector A and the second effective radar detector B Determine the azimuth α _AB of the first effective radar detector A and the second effective radar detector B:

When X _B -X _A >0 and Y _B -Y _A >0, the azimuth

When X _B -X _A <0 and Y _B -Y _A >0, the azimuth

When X _B -X _A <0 and Y _B -Y _A <0, the azimuth

When X _B -X _A >0 and Y _B -Y _A <0, the azimuth

In step 304 according to the formula Calculate the reference azimuth angle of the second effective radar detector B and the third effective radar detector C Determine the azimuth α _BC of the second effective radar detector B and the third effective radar detector C:

When X _C -X _B >0 and Y _C -Y _B >0, the azimuth

When X _C -X _B <0 and Y _C -Y _B >0, the azimuth

When X _C -X _B <0 and Y _C -Y _B <0, the azimuth

When X _C -X _B >0 and Y _C -Y _B <0, the azimuth

The above-mentioned a kind of multi-point type active detection automobile anti-collision method is characterized in that: the alarm includes a voice prompter and an LED flashing light, and the voice prompter and the LED flashing light are all connected to the vehicle-mounted controller The output terminals are connected.

The above-mentioned multi-point active detection vehicle collision avoidance method is characterized in that: the safety distance threshold is 0.5m-1m.

Compared with the prior art, the present invention has the following advantages:

1. The present invention actively detects the position of moving obstacles by installing no less than three radar detectors on the front side panel, rear side panel, left side panel and right side panel of the automobile body, and satisfies the requirements of the vehicle in any direction around the vehicle. The potential safety hazards caused by moving obstacles can reduce the situation that the blind spots of cars cannot accurately identify moving obstacles, which is convenient for popularization and use.

2. The present invention uses the vehicle-mounted controller to sort a plurality of distance data from small to large, and sets the radar detectors corresponding to the first three distance data after sorting to be effective radar detectors. The two adjacent radar detectors form a triangle relationship with the moving obstacle, obtain the coordinates of the moving obstacle, use the method of calculating the average of the coordinates to obtain the updated coordinates of the moving obstacle, and then obtain the real-time position of the moving obstacle, which is reliable and stable. The effect is good.

3. The steps of the method of the present invention are simple, and the update coordinates of the moving obstacles are curve-fitted by means of multi-point fitting curves, so as to obtain the trajectory of the moving obstacles and predict the trajectory of the moving obstacles, and realize the detection and monitoring of the moving obstacles. Tracking, so that drivers can be aware of possible dangers in advance, which is convenient for popularization.

To sum up, the present invention utilizes no less than three radar detectors on the four side panels of the automobile to actively detect moving obstacles in real time, to identify, detect and track static and dynamic objects on moving obstacles, and utilize moving The distance between the obstacle and the car body and the azimuth angle of the car body are used to obtain the coordinates of the moving obstacle, and the coordinate accuracy of the moving obstacle is improved by means of multiple calculations and average values, and the trajectory of the moving obstacle is obtained by curve fitting, so that Let drivers be aware of possible dangers in advance, which is convenient for promotion and use.

The technical solutions of the present invention will be described in further detail below with reference to the accompanying drawings and embodiments.

Description of drawings

Fig. 1 is a schematic diagram of the top view positional relationship among the vehicle body, the radar detector and the moving obstacle in the independent coordinate system O-XYZ based on the vehicle body of the present invention.

Fig. 2 is a circuit block diagram of the multi-point active detection automobile collision avoidance system of the present invention.

Fig. 3 is a flow chart of the method of the present invention.

Explanation of reference signs:

1—radar detector; 2—vehicle controller; 3—display screen;

4—alarm; 5—car body; 6—moving obstacles;

7—Electronic vertical angle measuring instrument.

Detailed ways

A kind of automobile anti-collision method of multi-point active detection as shown in Fig. 1, Fig. 2 and Fig. 3, comprises the following steps:

Step 1. Establish a multi-point active detection automobile collision avoidance system: install no less than three points on the front side panel, rear side panel, left side panel and right side panel of the car body 5 for actively detecting movement obstacles. The radar detector 1 at the position of the object 6, and establishes the automobile anti-collision subsystem for processing the data detected by each radar detector 1 at the same time, and the automobile anti-collision subsystem and the radar detector 1 form a multi-point active The automobile anti-collision system of detection; Described automobile anti-collision subsystem comprises vehicle-mounted controller 2 and is installed on the chassis of automobile body 5 and is installed on the chassis of automobile body 5 and is used to detect the electronic vertical angle measuring instrument 7 of automobile body 5 vertical angles, and The display screen 3 that is all connected with the output terminal of the vehicle-mounted controller 2 and the alarm 4 that is used to prompt the driver to avoid the moving obstacle 6, the electronic vertical angle measuring instrument 7 and the radar detector 1 are all connected with the vehicle-mounted controller 2. The input terminals are connected;

The radar detectors 1 installed on the front side panel, rear side panel, left side panel and right side panel of the automobile body 5 are all located on the same plane;

It should be noted that no less than three radar detectors 1 are respectively installed on the front side panel, rear side panel, left side panel and right side panel of the automobile body 5, and the purpose of actively detecting the position of the moving obstacle 6 is to realize Potential safety hazards caused by moving obstacles in any direction around the car, reduce the situation that the blind spot of the car cannot accurately identify moving obstacles, realize the multi-point active detection mode, and use the on-board controller 2 to process the data detected by the radar detector 1 , the display screen 3 can be used to display the positional relationship between the car body 5 and the moving obstacle 6 in real time, which is convenient for the driver to check, intuitive and reliable, and the alarm 4 can be used to effectively prompt the driver or the people on the moving obstacle 6 to keep Safe distance, good use effect; the radar detector 1 installed on the front side panel, the rear side panel, the left side panel and the right side panel of the automobile body 5 is all located on the same plane, which is convenient to establish an independent coordinate system based on the vehicle body, and improves The detection accuracy of the radar detector 1 simplifies the calculation process.

Step 2. Build an independent coordinate system based on the body: take the geometric center position of the plane where multiple radar detectors 1 are located on the same plane as the coordinate origin O, and take the straight ahead of the car body 5 as the positive direction of the Y axis , take the direction perpendicular to the Y axis and pointing from the left panel to the right panel as the positive direction of the X axis, and take the direction perpendicular to the Y axis and pointing from the chassis of the automobile body 5 to the top plate of the automobile body 5 as the Z axis Establish an independent coordinate system O-XYZ based on the body in the positive direction;

Step 3. Obtain the real-time position of the moving obstacle, the process is as follows:

Step 301, determine the effective radar detector: use multiple radar detectors 1 to simultaneously detect the distances of the moving obstacles 6 from the multiple radar detectors 1, and transmit the multiple distance data to the on-board controller 2 in real time, and the on-board controller 2 Sorting multiple distance data from small to large, setting the radar detectors corresponding to the first three distance data after sorting are all effective radar detectors, and the three effective radar detectors are three consecutive radar detectors 1. The three continuous radar detectors 1 are the first effective radar detector A, the second effective radar detector B and the third effective radar detector C in turn, and the position of the moving obstacle 6 is set as point P;

It should be noted that the on-board controller 2 sorts multiple distance data from small to large, and the purpose of setting the radar detectors 1 corresponding to the first three distance data after sorting to be effective radar detectors is to simplify the calculation process. The data of the moving obstacle 6 detected by the radar detector 1 on the same side of the car panel are close, and the first three data with the smallest distances are selected. It is to simplify the calculation and avoid a large amount of data from causing a large calculation load to the on-board controller 2, resulting in slow data calculation speed and satisfying the accuracy of data acquisition. In actual use, the three effective radar detectors are three consecutive radar detectors 1, Three consecutive radar detectors 1 are respectively named as the first effective radar detector A, the second effective radar detector B and the third effective radar detector C, for any selected three effective radar detectors according to the The locations where the automobile body 5 is installed are sequentially named as the first effective radar detector A, the second effective radar detector B and the third effective radar detector C.

Step 302, according to the formula Calculate the reference azimuth angle between the second effective radar detector B and the first effective radar detector A Among them, X _A is the X-axis coordinate of the first effective radar detector A, Y _A is the Y-axis coordinate of the first effective radar detector A, X _B is the X-axis coordinate of the second effective radar detector B, and Y _B is The Y-axis coordinate of the second effective radar detector B,

According to the formula Calculate the reference azimuth angle between the third effective radar detector C and the second effective radar detector B Wherein, X _C is the X-axis coordinate of the third effective radar detector C, and Y _C is the Y-axis coordinate of the third effective radar detector C,

Step 303. Determine the azimuth α _BA between the second effective radar detector B and the first effective radar detector A:

When X _A -X _B >0 and Y _A -Y _B >0, the azimuth

When X _A -X _B <0 and Y _A -Y _B >0, the azimuth

When X _A -X _B <0 and Y _A -Y _B <0, the azimuth

When X _A -X _B >0 and Y _A -Y _B <0, the azimuth

Determine the azimuth α _CB between the third effective radar detector C and the second effective radar detector B:

When X _B -X _C >0 and Y _B -Y _C >0, the azimuth

When X _B -X _C <0 and Y _B -Y _C >0, the azimuth

When X _B -X _C <0 and Y _B -Y _C <0, the azimuth

When X _B -X _C >0 and Y _B -Y _C <0, the azimuth

It should be noted that since the first effective radar detector A, the second effective radar detector B and the third effective radar detector C are fixed at the positions installed on the vehicle body 5, the second effective radar detector B is the same as the first effective radar detector. The azimuth angle of the radar detector A is a definite value, and the azimuth angles of the third effective radar detector C and the second effective radar detector B are definite values.

Step 304, according to the formula Calculate the azimuth α _AP between the first effective radar detector A and point P and the initial azimuth α' _BP between the second effective radar detector B and point P, where ∠BAP is the angle between L _BA and L _AP and ∠ABP is the angle between L _BA and L _BP and L _BA is the line segment from the second effective radar detector B to the first effective radar detector A, L _AP is the line segment from the first effective radar detector A to point P, and L _BP is the line segment from the second effective radar detector B to point P , α _AB is the azimuth angle from the first effective radar detector A to the second effective radar detector B;

In step 304 according to the formula Calculate the reference azimuth angle between the first effective radar detector A and the second effective radar detector B Determine the azimuth α _AB of the first effective radar detector A and the second effective radar detector B:

When X _B -X _A >0 and Y _B -Y _A >0, the azimuth

When X _B -X _A <0 and Y _B -Y _A >0, the azimuth

When X _B -X _A <0 and Y _B -Y _A <0, the azimuth

When X _B -X _A >0 and Y _B -Y _A <0, the azimuth

According to the formula Calculate the updated azimuth α' _B ' _P between the second effective radar detector B and point P and the azimuth α _CP between the third effective radar detector C and point P, where ∠CBP is the angle between L _CB and L _BP and ∠BCP is the angle between L _CB and L _CP and L _CB is the line segment from the third effective radar detector C to the second effective radar detector B, L _CP is the line segment from the third effective radar detector C to point P, α _BC is the line segment from the second effective radar detector B to the third the azimuth of the effective radar detector C;

In step 304 according to the formula Calculate the reference azimuth angle of the second effective radar detector B and the third effective radar detector C Determine the azimuth α _BC of the second effective radar detector B and the third effective radar detector C:

When X _C -X _B >0 and Y _C -Y _B >0, the azimuth

When X _C -X _B <0 and Y _C -Y _B >0, the azimuth

When X _C -X _B <0 and Y _C -Y _B <0, the azimuth

When X _C -X _B >0 and Y _C -Y _B <0, the azimuth

According to the formula Calculate the azimuth α _BP between the second effective radar detector B and the point P;

It should be noted that since the first effective radar detector A, the second effective radar detector B and the third effective radar detector C are fixed at the positions installed on the vehicle body 5, the first effective radar detector A and the second effective radar detector The radar detector B is adjacent, and the second effective radar detector B is adjacent to the third effective radar detector C, so the first effective radar detector A, the second effective radar detector B and point P are selected to form a triangle structure, The second effective radar detector B, the third effective radar detector C and point P are selected to form another triangle structure, and the distance between the first effective radar detector A and the third effective radar detector C is relatively far away. Therefore, in order to improve Calculating accuracy, reducing data interference, discarding the triangle structure formed by the first effective radar detector A, the third effective radar detector C and point P.

Step 305, according to the formula Calculate the orientation angle β _{AP of L AP ;}

According to the formula Calculate the orientation angle β _{BP of L BP ;}

According to the formula Calculate the orientation angle β _{CP of L CP ;}

Step 306, according to the formula Calculate the direction cosine γ _APx of the first effective radar detector A and point P on the X axis, the direction cosine γ _APy of the first effective radar detector A and point P on the Y axis, the first effective radar detector A and point P The direction cosine γ _APz of P in the Z axis, wherein, V is the vertical angle of the automobile body measured by the electronic vertical angle measuring instrument 7;

According to the formula Calculate the direction cosine γ _BPx of the second effective radar detector B and point P on the X axis, the direction cosine γ _BPy of the second effective radar detector B and point P on the Y axis, the second effective radar detector B and point P The direction cosine γ _BPz of P in the Z axis;

According to the formula Calculate the direction cosine γ _CPx of the third effective radar detector C and point P on the X axis, the direction cosine γ _CPy of the third effective radar detector C and point P on the Y axis, the third effective radar detector C and point P The direction cosine γ _CPz of P in the Z axis;

It should be noted that since the electronic vertical angle measuring instrument 7 is installed on the chassis of the automobile body 5 and moves synchronously with the automobile body 5, the radar detector 1 is installed on the automobile body 5, and the electronic vertical angle measuring instrument 7 The vertical angle of the measured automobile body 5 and the vertical angle of the radar detector 1 are the same vertical angle, utilize the electronic vertical angle measuring instrument 7 to measure the vertical angle of the automobile body 5, calculate the current coordinates of the point P, When the automobile body 5 moves, the position of the radar detector 1 and the moving obstacle 6 changes, and the vertical angle of the automobile body 5 changes along with the change of the position, and the current vertical angle measured by the electronic vertical angle measuring instrument 7 is Synchronized with radar detector 1 detection data.

Step 307, according to the formula Calculate the initial first coordinates (X _AP , Y _AP , Z _AP ) of point P, where ΔX _AP is the component of L _AP on the X axis and ΔX _AP = L _AP *γ _APx , and ΔY _AP is the component of L _AP on the Y axis Component and ΔY _AP =L _AP *γ _APy , ΔZ _AP is the component of L _AP on the Z axis and ΔZ _AP =L _AP *γ _APz ;

According to the formula Calculate the initial second coordinates (X _BP , Y _BP , Z _BP ) of point P, where ΔX _BP is the component of L _BP on the X axis and ΔX _BP =L _BP *γ _BPx , ΔY _BP is the component of L _BP on the Y axis Component and ΔY _BP =L _BP *γ _BPy , ΔZ _BP is the component of L _BP on the Z axis and ΔZ _BP =L _BP *γ _BPz ;

According to the formula Calculate the initial third coordinate (X _CP , Y _CP , Z _CP ) of point P, where ΔX _CP is the component of L _CP on the X axis and ΔX _CP =L _CP *γ _CPx , ΔY _CP is the component of L _CP on the Y axis Component and ΔY _CP =L _CP *γ _CPy , ΔZ _CP is the component of L _CP on the Z axis and ΔZ _CP =L _CP *γ _CPz ;

It should be noted that the coordinates of point P are calculated once using the positional relationship between the first effective radar detector A and point P, and the coordinates of point P are calculated again using the positional relationship between the second effective radar detector B and point P. The positional relationship between the three effective radar detectors C and the point P is calculated for the third time to avoid the interference error in the single calculation.

Step 308, according to the formula Calculate the updated coordinates (X _P , Y _P , Z _P ) of point P;

It should be noted that using the average value of the initial coordinates of P calculated three times as the updated coordinates of point P has high data accuracy and good calculation reliability.

Step 309, use the first effective radar detector A, the second effective radar detector B and the third effective radar detector C to actively detect the distance of the moving obstacle 6 n times, and loop steps 304 to 308 n times to obtain n times The updated coordinates of point P at different times obtain the real-time position of the moving obstacle 6, and the updated coordinates of point P at n times at different times constitute the observation data set Q={(x ₁ ,y ₁ ,z ₁ ),(x ₂ , y ₂ ,z ₂ ),...,( _xi ,y _i , _zi ),...,(x _n ,y _n ,z _n )}, where n is a positive integer not less than 4, i is the detection number of effective radar detectors and 1≤i≤n, ( _xi , y, _{zi )} is the updated coordinates of point P calculated by the on-board controller 2 for the ith time;

It should be noted that the first effective radar detector A, the second effective radar detector B and the third effective radar detector C are used to actively detect the distance of the moving obstacle 6 n times, and the update of the point P at different times is continuously calculated The coordinates can obtain discrete points of n points P to obtain the real-time position of the moving obstacle 6 .

Step 4. Fitting and predicting the trajectory of moving obstacles, the process is as follows:

Step 401, Obtain the effective observation data set Q': use the on-board controller 2 to pair the observation data set Q={(x ₁ ,y ₁ ,z ₁ ),(x ₂ ,y ₂ ,z ₂ ),...,( x _i ,y _i , _zi ),...,(x _n ,y _n ,z _n )} do dimension reduction processing, select the X-axis coordinate value and Y-axis coordinate value of the update coordinates of point P to form effective observation data Set Q', effective observation data set Q'={(x ₁ ,y ₁ ),(x ₂ ,y ₂ ),...,( _xi ,y _i ),...,(x _n ,y _n )};

In actual use, since the vehicle body 5 and the moving obstacle 6 are three-dimensional objects moving on the ground, the Z-axis value of the moving obstacle 6 and the radar detector 1 at the same height will not affect the trajectory change of the moving obstacle 6 moving forward and backward. Therefore, only the X-axis coordinates and Y-axis coordinates of the updated coordinates of the point P are used to fit the trajectory.

Step 402, determine the basis function family and construct the quadratic polynomial: the on-board controller 2 sets the basis function family, and the basis function family includes δ ₀ (x)=1, δ ₁ (x)=x and δ ₂ (x) = x ² , the on-board controller 2 constructs a quadratic polynomial s(x)=a ₀ δ ₀ (x)+a ₁ δ ₁ (x)+a ₂ δ ₂ (x), where a ₀ is the quadratic polynomial s (x) is the coefficient of δ ₀ (x), a ₁ is the coefficient of δ ₁ (x) in the quadratic polynomial s(x), a ₂ is the coefficient of δ ₂ (x) in the quadratic polynomial s(x);

Step 403, according to the formula Compute the coefficient matrix of the quadratic polynomial s(x) And bring in the quadratic polynomial s(x)=a ₀ +a ₁ x+a ₂ x ² , the on-board controller 2 obtains the fitting curve, and realizes the prediction of the trajectory of the moving obstacle 6, wherein, δ ₀ is determined by δ ₀ ( x)=1 is an n-dimensional vector composed of δ ₀ =[1,1,L,1,L,1] ^T , δ ₁ is an n-dimensional vector composed of δ ₁ (x)=x and δ ₁ =[x ₁ ,x ₂ ,L, _xi ,L,x _n ] ^T , δ ₂ is an n-dimensional vector composed of δ ₂ (x)=x ² and

It should be noted that using the updated coordinates of point P at different times in step 3 can only obtain the current position of the moving obstacle 6, and the position of the moving obstacle 6 at the next moment cannot be predicted. Therefore, the purpose of this step is to use the known The point uses the quadratic polynomial fitting curve to obtain the movement trajectory of the moving obstacle 6, and realizes the prediction of the position of the moving obstacle 6 at the next moment, so that the driver can be aware of the possible danger in advance, and according to the update of the point P The continuous acquisition of coordinates corrects the trajectory of the moving obstacle 6 in time. In actual calculation, the trajectory of the moving obstacle 6 is relative to the independent coordinate system based on the vehicle body, and the real-time performance and relativity are good.

Step 5, track display and early warning of moving obstacles: use the display screen 3 to display the moving track of moving obstacles 6 in real time, set the safety distance threshold between moving obstacles 6 and car body 5 through on-board controller 2, when moving obstacles 6. When the distance from the vehicle body 5 is not greater than the safety distance threshold, the on-board controller 2 controls the alarm 4 to give an early warning.

In this embodiment, the alarm device 4 includes a voice prompter and an LED flashing light, and both the voice prompter and the LED flashing light are connected to the output end of the vehicle controller 2 .

In this embodiment, the safety distance threshold is 0.5m˜1m.

It should be noted that the display screen 3 is used to display the movement track of the moving obstacle 6 in real time. When there is a moving obstacle 6 approaching the safety distance threshold, the alarm 4 can prompt the driver to be dangerous through the voice prompter, and at the same time, the LED can flash The lamp sends a strong prompting signal, prompting the people of the moving obstacle 6 to pay attention to the driving direction and driving speed, and plays a safety protection role in many aspects.

The above are only preferred embodiments of the present invention, and do not limit the present invention in any way. All simple modifications, changes and equivalent structural changes made to the above embodiments according to the technical essence of the present invention still belong to the technical aspects of the present invention. within the scope of protection of the scheme.

Claims (4)

1. a kind of automobile anti-collision method of multi-point active detection, it is characterized in that, the method comprises the following steps: Step 1. Establish a multi-point active detection automobile collision avoidance system: install no less than three points for active detection on the front side panel, rear side panel, left side panel and right side panel of the car body (5). A radar detector (1) at the position of the moving obstacle (6), and an automobile anti-collision subsystem for processing the data detected by each radar detector (1) is established at the same time, and the automobile anti-collision subsystem is connected with the radar The detector (1) forms a multi-point active detection automobile anti-collision system; the automobile anti-collision subsystem includes an on-board controller (2) and is installed on the chassis of the automobile body (5) for detecting (5) the electronic vertical angle measuring instrument (7) of vertical angle, and the display screen (3) that all connects with the output end of vehicle-mounted controller (2) and be used for prompting driver to avoid moving obstacle (6) The alarm (4), the electronic vertical angle measuring instrument (7) and the radar detector (1) are all connected with the input end of the vehicle-mounted controller (2); The radar detectors (1) installed on the front side panel, rear side panel, left side panel and right side panel of the automobile body (5) are all located on the same plane; Step 2. Build an independent coordinate system based on the vehicle body: take the geometric center position of the plane where multiple radar detectors (1) on the same plane are located as the coordinate origin O, and take the straight ahead of the vehicle body (5) as The positive direction of the Y-axis is the positive direction of the X-axis with the direction perpendicular to the Y-axis and directed to the right panel by the left side panel, and the positive direction of the Y-axis is vertically perpendicular to the Y-axis and directed to the car body ( 5) The direction of the top plate is the positive direction of the Z-axis to establish an independent coordinate system O-XYZ based on the body; Step 3. Obtain the real-time position of the moving obstacle, the process is as follows: Step 301, determine effective radar detectors: use multiple radar detectors (1) to simultaneously detect the distances of moving obstacles (6) from multiple radar detectors (1), and transmit multiple distance data to the vehicle control in real time device (2), the on-board controller (2) sorts multiple distance data from small to large, and sets the radar detectors (1) corresponding to the first three distance data after sorting to be effective radar detectors, and the three The effective radar detectors are three consecutive radar detectors (1), and the three consecutive radar detectors (1) are successively the first effective radar detector A, the second effective radar detector B and the third effective radar detector Device C, set the position of moving obstacle (6) as point P; Step 302, according to the formula Calculate the reference azimuth angle between the second effective radar detector B and the first effective radar detector A Among them, X _A is the X-axis coordinate of the first effective radar detector A, Y _A is the Y-axis coordinate of the first effective radar detector A, X _B is the X-axis coordinate of the second effective radar detector B, and Y _B is The Y-axis coordinate of the second effective radar detector B, According to the formula Calculate the reference azimuth angle between the third effective radar detector C and the second effective radar detector B Wherein, X _C is the X-axis coordinate of the third effective radar detector C, and Y _C is the Y-axis coordinate of the third effective radar detector C, Step 303. Determine the azimuth α _BA between the second effective radar detector B and the first effective radar detector A: When X _A -X _B >0 and Y _A -Y _B >0, the azimuth When X _A -X _B <0 and Y _A -Y _B >0, the azimuth When X _A -X _B <0 and Y _A -Y _B <0, the azimuth When X _A -X _B >0 and Y _A -Y _B <0, the azimuth Determine the azimuth α _CB between the third effective radar detector C and the second effective radar detector B: When X _B -X _C >0 and Y _B -Y _C >0, the azimuth When X _B -X _C <0 and Y _B -Y _C >0, the azimuth When X _B -X _C <0 and Y _B -Y _C <0, the azimuth When X _B -X _C >0 and Y _B -Y _C <0, the azimuth Step 304, according to the formula Calculate the azimuth α _AP between the first effective radar detector A and point P and the initial azimuth α' _BP between the second effective radar detector B and point P, where ∠BAP is the angle between L _BA and L _AP and ∠ABP is the angle between L _BA and L _BP and L _BA is the line segment from the second effective radar detector B to the first effective radar detector A, L _AP is the line segment from the first effective radar detector A to point P, and L _BP is the line segment from the second effective radar detector B to point P , α _AB is the azimuth angle from the first effective radar detector A to the second effective radar detector B; According to the formula Calculate the updated azimuth α″ _BP between the second effective radar detector B and point P and the azimuth α _CP between the third effective radar detector C and point P, where ∠CBP is the angle between L _CB and L _BP and ∠BCP is the angle between L _CB and L _CP and L _CB is the line segment from the third effective radar detector C to the second effective radar detector B, L _CP is the line segment from the third effective radar detector C to point P, α _BC is the line segment from the second effective radar detector B to the third the azimuth of the effective radar detector C; According to the formula Calculate the azimuth α _BP between the second effective radar detector B and the point P; Step 305, according to the formula Calculate the orientation angle β _{AP of L AP ;} According to the formula Calculate the orientation angle β _{BP of L BP ;} According to the formula Calculate the orientation angle β _{CP of L CP ;} Step 306, according to the formula Calculate the direction cosine γ _APx of the first effective radar detector A and point P on the X axis, the direction cosine γ _APy of the first effective radar detector A and point P on the Y axis, the first effective radar detector A and point P The direction cosine γ _APz of P in Z axial direction, wherein, V is the vertical angle of the automobile body (5) that electronic vertical angle measuring instrument (7) measures; According to the formula Calculate the direction cosine γ _BPx of the second effective radar detector B and point P on the X axis, the direction cosine γ _BPy of the second effective radar detector B and point P on the Y axis, the second effective radar detector B and point P The direction cosine γ _BPz of P in the Z axis; According to the formula Calculate the direction cosine γ _CPx of the third effective radar detector C and point P on the X axis, the direction cosine γ _CPy of the third effective radar detector C and point P on the Y axis, the third effective radar detector C and point P The direction cosine γ _CPz of P in the Z axis; Step 307, according to the formula Calculate the initial first coordinates (X _AP , Y _AP , Z _AP ) of point P, where ΔX _AP is the component of L _AP on the X axis and ΔX _AP = L _AP *γ _APx , and ΔY _AP is the component of L _AP on the Y axis Component and ΔY _AP =L _AP *γ _APy , ΔZ _AP is the component of L _AP on the Z axis and ΔZ _AP =L _AP *γ _APz ; According to the formula Calculate the initial second coordinates (X _BP , Y _BP , Z _BP ) of point P, where ΔX _BP is the component of L _BP on the X axis and ΔX _BP =L _BP *γ _BPx , ΔY _BP is the component of L _BP on the Y axis Component and ΔY _BP =L _BP *γ _BPy , ΔZ _BP is the component of L _BP on the Z axis and ΔZ _BP =L _BP *γ _BPz ; According to the formula Calculate the initial third coordinate (X _CP , Y _CP , Z _CP ) of point P, where ΔX _CP is the component of L _CP on the X axis and ΔX _CP =L _CP *γ _CPx , ΔY _CP is the component of L _CP on the Y axis Component and ΔY _CP =L _CP *γ _CPy , ΔZ _CP is the component of L _CP on the Z axis and ΔZ _CP =L _CP *γ _CPz ; Step 308, according to the formula Calculate the updated coordinates (X _P , Y _P , Z _P ) of point P; Step 309, n times utilize the first effective radar detector A, the second effective radar detector B and the third effective radar detector C to actively detect the distance of the moving obstacle (6), and loop step 304 to step 308 n times to obtain The updated coordinates of point P at n times at different times obtain the real-time position of the moving obstacle (6), and the updated coordinates of point P at n times at different times constitute the observation data set Q={(x ₁ , y ₁ , z ₁ ), (x ₂ ,y ₂ ,z ₂ ),...,( _xi ,y _i , _zi ),...,(x _n ,y _n ,z _n )}, where n is not less than 4 Positive integer, i is the detection number of valid radar detectors and 1≤i≤n, (x _i , y _i , z _i ) is the updated coordinates of point P calculated by the on-board controller (2) for the ith time; Step 4. Fitting and predicting the trajectory of moving obstacles, the process is as follows: Step 401. Acquire effective observation data set Q': use the on-board controller (2) to obtain observation data set Q={(x ₁ ,y ₁ ,z ₁ ),(x ₂ ,y ₂ ,z ₂ ),... ,(x _i ,y _i , _zi ),...,(x _n ,y _n ,z _n )} is used for dimension reduction processing, and the X-axis coordinate value and Y-axis coordinate value of the update coordinates of point P are selected to form a valid Observation data set Q', effective observation data set Q'={(x ₁ ,y ₁ ),(x ₂ ,y ₂ ),...,( _xi ,y _i ),...,(x _n , y _n )}; Step 402, determine the basis function family and construct the quadratic polynomial: the on-board controller (2) sets the basis function family, and the basis function family includes δ ₀ (x)=1, δ ₁ (x)=x and δ ₂ ( x)=x ² , the on-board controller (2) constructs a quadratic polynomial s(x)=a ₀ δ ₀ (x)+a ₁ δ ₁ (x)+a ₂ δ ₂ (x), where a ₀ is The coefficient of δ ₀ (x) in the quadratic polynomial s(x), a ₁ is the coefficient of δ ₁ (x) in the quadratic polynomial s(x), a ₂ is the coefficient of δ ₂ (x) in the quadratic polynomial s(x) ) coefficient; Step 403, according to the formula Compute the coefficient matrix of the quadratic polynomial s(x) And bring in the quadratic polynomial s(x)=a ₀ +a ₁ x+a ₂ x ² , the on-board controller (2) obtains the fitting curve, and realizes the prediction of the trajectory of the moving obstacle (6), wherein, δ ₀ is An n-dimensional vector composed of δ ₀ (x)=1 and δ ₀ =[1,1,L,1,L,1] ^T , δ ₁ is an n-dimensional vector composed of δ ₁ (x)=x and δ ₁ =[x ₁ ,x ₂ ,L, _xi ,L,x _n ] ^T , δ ₂ is an n-dimensional vector composed of δ ₂ (x)=x ² and Step 5. Track display and early warning of the moving obstacle: use the display screen (3) to display the moving track of the moving obstacle (6) in real time, and set the distance between the moving obstacle (6) and the vehicle body (5) through the on-board controller (2). ), when the distance between the moving obstacle (6) and the vehicle body (5) is not greater than the safe distance threshold, the vehicle-mounted controller (2) controls the alarm (4) to give an early warning. 2. according to the automobile collision avoidance method of a kind of multi-point type active detection described in claim 1, it is characterized in that: in step 304, according to the formula Calculate the reference azimuth angle between the first effective radar detector A and the second effective radar detector B Determine the azimuth α _AB of the first effective radar detector A and the second effective radar detector B: When X _B -X _A >0 and Y _B -Y _A >0, the azimuth When X _B -X _A <0 and Y _B -Y _A >0, the azimuth When X _B -X _A <0 and Y _B -Y _A <0, the azimuth When X _B -X _A >0 and Y _B -Y _A <0, the azimuth In step 304 according to the formula Calculate the reference azimuth angle of the second effective radar detector B and the third effective radar detector C Determine the azimuth α _BC of the second effective radar detector B and the third effective radar detector C: When X _C -X _B >0 and Y _C -Y _B >0, the azimuth When X _C -X _B <0 and Y _C -Y _B >0, the azimuth When X _C -X _B <0 and Y _C -Y _B <0, the azimuth When X _C -X _B >0 and Y _C -Y _B <0, the azimuth 3. according to the automobile anti-collision method of a kind of multi-point type active detection described in claim 1, it is characterized in that: described alarm (4) comprises voice prompter and LED strobe light, and described voice prompter and all The above-mentioned LED flashing lights are all connected with the output terminals of the vehicle-mounted controller (2). 4 . The multi-point active detection vehicle collision avoidance method according to claim 1 , wherein the safety distance threshold is 0.5m˜1m.