Rail obstacle identification method based on millimeter wave radar
1. A rail obstacle identification method based on a millimeter wave radar is characterized by comprising the following steps:
s1: setting GPS longitude and latitude information of the millimeter wave radar before the millimeter wave radar enters a scene without GPS signalsG 1(lo, la) is the initial position of the track, the Cartesian coordinates are P (0,0), and the Cartesian coordinates P (x, y) of the millimeter-wave radar at other moments are obtained through GPS calculation;
after the millimeter wave radar enters a scene without GPS signals, calculating and recording Cartesian coordinates of the millimeter wave radar through an inertial navigation systemP n (x, y)；
Dividing the whole track line into a plurality of track segments by using track point cloud coordinates acquired by a millimeter wave radar, wherein the distance of each track segment in the vertical direction is S meters;
recording x, y coordinates measured by millimeter wave radar relative to the starting point of each track segment at regular intervals, and comparing these coordinates with corresponding Cartesian coordinatesP n (x, y) Associating;
calculating and recording the deflection angle theta of each track segment from the starting position to the ending position in the horizontal direction;
after the track information is completely recorded, the process proceeds to step S2;
s2: and the millimeter wave radar enters the same track line again, the millimeter wave radar acquires the current position according to the inertial navigation system after entering the scene without the GPS signal, the track information far exceeding S meters is spliced by combining the currently identified track segmentation information and the stored track segmentation information, and whether the detected obstacle is in the track line or not is judged.
2. The millimeter wave radar-based track obstacle recognition method according to claim 1, wherein the step S2 includes the steps of:
s21: the millimeter wave radar obtains the Cartesian coordinates of itself through the inertial navigation systemP a (x a , y a ) And obtaining recorded Cartesian coordinates of the starting position of the track segment closest to the current positionP b (x b , y b ) Then identify fromP a ToP b The track line position of this segment;
s22: extracting recorded data from memoryP a ToP b The track segment position information of the segment is calculated to be on the track line currently identifiedxSum of on-axis variancesvar sum When is coming into contact withvar sum <factor n, wherein factor is a coefficient,nexecuting the next step for the number of the coordinates;
s23: computer radar slaveP a Run toP b Angle theta of deflection relative to current position at time of positiona→b =；
S24: for a first segment of track segment of a splice, there is a recorded start position of the track segment and a track segment descriptionkGroup coordinates (x k(1…)，y k(1…)) Calculating the coordinates relative to the current position of the radarxThe coordinates arex k(1…)/cosθa→b，yThe coordinates arey k(1…) +(y b-y a ) (ii) a For the second track segment of the splice, the coordinates are calculated relative to the current position of the radarxThe coordinates arex k(1…)/cos(θa→b+θb→c)，yCoordinate is S +y k(1…) + (y b- y a ) (ii) a For the third track segment of the splice, the coordinates are calculated relative to the current position of the radarxThe coordinates arex k(1…) / cos(θa→b+θb→c+θc→d)，yCoordinate is 2 + S +y k(1…) + (y b- y a ) And so on, splicing a track line far exceeding the distance of S meters;
s25: and the millimeter wave radar judges whether the obstacle is in the track line or not according to the spliced track line and the detected obstacle.
3. The method for recognizing a track obstacle based on a millimeter wave radar according to claim 1, wherein the method of calculating the angle θ by which each track section of the radar is deflected in the horizontal direction from the start position to the end position is: assuming relative cartesian coordinates of the starting position of the track segmentPThe coordinates of (0,0) areP c (x c , y c ) Relative Cartesian coordinates of the end positionPThe coordinates of (0,0) areP d (x d , y d ) Then thetac→d = 。
4. The millimeter wave radar-based obstacle recognition method of claim 1, wherein the calculation is in-line with a currently recognized orbit linexThe step of summing the variances on the axes is as follows: if it isP a ToP b Is a distance ofmMeter, this for the current radar measurementmMeter rail line co-selectionn = m / iGroup coordinates of respectively: (x 1, y 1)...(x n , y n ) This is recorded in the memorymOf ricenGroup coordinates are respectively (x’ 1, y’ 1)...(x’ n , y’ n ) Calculating two sets of coordinates atxSum of variance of axesvar sum = (x’ 1-x 1, y’ 1- y 1)^2 + (x’ 2-x 2, y’ 2- y 2)^2 + ... + (x’ n -x n , y’ n - y n )^2。
5. The method as claimed in claim 1, wherein the millimeter wave radar records the GPS latitude and longitude information of the start time of operation when the millimeter wave radar first operates on the track lineG 1(lo, la) is recorded as the initial position of the track, and the position P (in Cartesian coordinate system) of the millimeter wave radar is in the presence of GPS signalsx, y) From its current time GPS latitude and longitude informationG 2 (lo, la) - G 1(lo, la) is obtained;
after the millimeter wave radar enters a scene without GPS signals, the position information finally output by the GPS is transmittedP GPS (x, y) The speed V corresponding to the moment is used as the input of inertial navigation, and the running distance of the train without GPS signals is calculated through the integration of a gyroscope and an accelerometer of the inertial navigationP INS (x, y) Position P of the millimeter-wave radar in a cartesian coordinate system (bx, y) = P GPS (x, y) + P INS (x, y)。
6. The method of claim 1 wherein the method further comprises identifying the obstacle based on millimeter wave radariThe coordinates of the track line are recorded at intervals of meters, so that the track line of S meters needs to be recordedkGroup coordinate information, whereink = S / i。
Background
The application of the millimeter wave radar in the track mainly lies in detecting whether an obstacle exists in front of the track, and the track needs to be identified so as to determine whether the obstacle is located in the track, so that false alarm is reduced. Because the surface of the track is smooth, the energy reflected by electromagnetic waves is low, and the farther the distance is, the higher the requirement on the radar angular resolution is, and all factors are synthesized, the identification distance of the traditional identification method to the track is usually short, so that the requirement on remote identification is difficult to meet. Other sensors, such as a camera, are greatly influenced by conditions such as weather and illumination; lidar is too costly and detection performance in rainy and foggy weather is also difficult to meet requirements.
Disclosure of Invention
In view of the above, the present invention provides a method for identifying a rail obstacle based on a millimeter wave radar, which includes the following steps:
s1: setting GPS longitude and latitude information of the millimeter wave radar before the millimeter wave radar enters a scene without GPS signalsG 1(lo, la) is the initial position of the track, the Cartesian coordinates are P (0,0), and the Cartesian coordinates P (x, y) of the millimeter-wave radar at other moments are obtained through GPS calculation;
after the millimeter wave radar enters a scene without GPS signals, calculating and recording Cartesian coordinates of the millimeter wave radar through an inertial navigation systemP n (x, y)；
Dividing the whole track line into a plurality of track segments by using track point cloud coordinates acquired by a millimeter wave radar, wherein the distance of each track segment in the vertical direction is S meters;
recording x, y coordinates measured by millimeter wave radar relative to the starting point of each track segment at regular intervals, and comparing these coordinates with corresponding Cartesian coordinatesP n (x, y) Associating;
calculating and recording the deflection angle theta of each track segment from the starting position to the ending position in the horizontal direction;
after the track information is completely recorded, the process proceeds to step S2;
s2: and the millimeter wave radar enters the same track line again, the millimeter wave radar acquires the current position according to the inertial navigation system after entering the scene without the GPS signal, the track information far exceeding S meters is spliced by combining the currently identified track segmentation information and the stored track segmentation information, and whether the detected obstacle is in the track line or not is judged.
Further, step S2 includes the steps of:
s21: the millimeter wave radar obtains the Cartesian coordinates of itself through the inertial navigation systemP a (x a , y a ) And obtaining recorded Cartesian coordinates of the starting position of the track segment closest to the current positionP b (x b , y b ) Then identify fromP a ToP b The track line position of this segment;
s22: extracting recorded data from memoryP a ToP b The track segment position information of the segment is calculated to be on the track line currently identifiedxSum of on-axis variancesvar sum When is coming into contact withvar sum <factor n, wherein factor is a coefficient,nexecuting the next step for the number of the coordinates;
s23: computer radar slaveP a Run toP b Angle theta of deflection relative to current position at time of positiona→b =；
S24: for a first segment of track segment of a splice, there is a recorded start position of the track segment and a track segment descriptionkGroup coordinates (x k(1…)，y k(1…)) Calculating the coordinates relative to the current position of the radarxThe coordinates arex k(1…)/cosθa→b，yThe coordinates arey k(1…) +(y b-y a ) (ii) a For the second track segment of the splice, the coordinates are calculated relative to the current position of the radarxThe coordinates arex k(1…)/cos(θa→b+θb→c)，yCoordinate is S +y k(1…) + (y b- y a ) (ii) a For the third track segment of the splice, the coordinates are calculated relative to the current position of the radarxThe coordinates arex k(1…) / cos(θa→b+θb→c+θc→d)，yCoordinate is 2 + S +y k(1…) + (y b- y a ) And so on, splicing a track line far exceeding the distance of S meters;
s25: and the millimeter wave radar judges whether the obstacle is in the track line or not according to the spliced track line and the detected obstacle.
Further, the method for calculating the deflection angle theta of each track section from the starting position to the ending position of the radar in the horizontal direction comprises the following steps: assuming relative cartesian coordinates of the starting position of the track segmentPThe coordinates of (0,0) areP c (x c , y c ) Relative Cartesian coordinates of the end positionPThe coordinates of (0,0) areP d (x d , y d ) Then thetac→d = 。
Further, the current identified track line is calculated to be onxThe step of summing the variances on the axes is as follows: if it isP a ToP b Is a distance ofmMeter, this for the current radar measurementmMeter rail line co-selectionn = m / iGroup coordinates of respectively: (x 1, y 1)...(x n , y n ) This is recorded in the memorymOf ricenGroup coordinates are respectively (x’ 1, y’ 1)...(x’ n , y’ n ) Meter for measuringCalculate two sets of coordinates inxSum of variance of axesvar sum = (x’ 1-x 1, y’ 1- y 1)^2 + (x’ 2-x 2, y’ 2- y 2)^2 + ... + (x’ n -x n , y’ n - y n )^2。
Further, when the millimeter wave radar operates on the track line for the first time, the GPS longitude and latitude information of the operation starting time is recordedG 1(lo, la) is recorded as the initial position of the track, and the position P (in Cartesian coordinate system) of the millimeter wave radar is in the presence of GPS signalsx, y) From its current time GPS latitude and longitude informationG 2 (lo, la) - G 1(lo, la) is obtained;
after the millimeter wave radar enters a scene without GPS signals, the position information finally output by the GPS is transmittedP GPS (x, y) The speed V corresponding to the moment is used as the input of inertial navigation, and the running distance of the train without GPS signals is calculated through the integration of a gyroscope and an accelerometer of the inertial navigationP INS (x, y) Position P of the millimeter-wave radar in a cartesian coordinate system (bx, y) = P GPS (x, y) +P INS (x, y)。
Further, if soiThe coordinates of the track line are recorded at intervals of meters, so that the track line of S meters needs to be recordedkGroup coordinate information, whereink = S / i。
The invention has the following beneficial effects:
the problem that the millimeter wave radar is limited by the detection capability and the angular resolution of the millimeter wave radar to the track, and long-distance track identification is difficult to realize is solved.
By combining the position information of the track line, the millimeter wave radar can judge whether the front target is positioned in the track without data fusion with other sensors (such as a camera, a laser radar and the like) so as to determine whether alarm information needs to be sent, thereby greatly reducing the probability of misinformation and maximizing the detection performance of the millimeter wave radar.
Drawings
FIG. 1 is a flow chart of a method of track obstacle identification of the present invention;
FIG. 2 is a schematic diagram of the point cloud distribution of the S-meter long trajectory line of the present invention;
fig. 3 is a schematic diagram of a method for splicing track lines at the current position by the millimeter wave radar of the present invention.
Detailed Description
The invention is further described with reference to the accompanying drawings, but the invention is not limited in any way, and any alterations or substitutions based on the teaching of the invention are within the scope of the invention.
As shown in fig. 1, the method for identifying a rail obstacle based on a millimeter wave radar disclosed by the invention comprises the following steps:
when the radar runs on the track line for the first time, the GPS longitude and latitude information of the running time will be startedG 1(lo, la) is recorded as the start position of the current track, expressed in Cartesian coordinates as(0,0), then the position P (x, y) of the radar in the Cartesian coordinate system at any moment can be determined by the GPS longitude and latitude information of the current momentG 2 (lo, la) - G 1(lo, la).
If the radar enters a tunnel and other scenes without GPS signals, the GPS is output for the last timeP GPS (x, y) The speed V corresponding to the moment is used as the input of inertial navigation, and then the running distance of the train without GPS signals is calculated through the integration of a gyroscope and an accelerometer of the inertial navigationP INS (x, y) At this time P: (x, y) = P GPS (x, y) + P INS (x, y)。
As shown in fig. 2, assuming that the detection capability of the millimeter wave radar for the track line is S meters, the point cloud coordinate of the track line of S meters is obtained by using the point cloud information of the radar, and the whole track line is divided into a plurality of track segments with S meters as a unit.
The millimeter wave radar measures the S-meter track line relative to the radar at the starting point of each track segmentx、yCoordinates of in orderiThe coordinates are recorded at intervals of meters,idepending on the minimum radius of curvature of the track segment, the smaller the radius of curvature,ithe smaller the value. S m track segment needs to record k sets of coordinate information (k = S/i), and is selected from radar point cloud, such as the second recordjAnd when the orbit data of the meter is obtained, selecting the point cloud closest to the position of the j meter. The coordinates are related to the corresponding Cartesian coordinate system positionsP n (x, y) And (4) associating.
And calculating and recording the angle theta of the radar which will deflect from the starting position to the ending position of the S-meter track segment in the horizontal direction. Let the coordinates of the starting position of S m relative to the Cartesian coordinates P (0,0) beP c (x c , y c ) The coordinates of the end position relative to the Cartesian coordinates P (0,0) areP d (x d , y d ) Then thetac→d = 。
After the track information is completely recorded, the next stage is started, and at this time, the radar can splice the track information far exceeding S meters by combining the current position information of the radar, the currently identifiable track information of S meters and the track information stored in the radar, as shown in fig. 3, the specific steps are as follows:
the radar firstly obtains the position of the radar itself through GPS/inertial navigationP a (x a , y a ) And obtaining the recorded starting position of a section of track line closest to the current positionP b (x b , y b ) Then identify fromP a ToP b The position of the trajectory line of this segment.
Extracting recorded data from memoryP a ToP b Track segment position information for this segment, the sum of the variances from the currently identified track line on the x-axis is calculated: if it isP a ToP b Is a distance ofmMeter, this for the current radar measurementmMeter rail line co-selectionn = m / iGroup coordinates of respectively: (x 1, y 1)...(x n , y n ) This is recorded in the memorymOf ricenGroup coordinates are respectively (x’ 1, y’ 1)...(x’ n , y’ n ) CalculatingnGroup ofy = y’ - (S - m) Time of flightxSum of variance ofvar sum = (x’ 1-x 1, y’ 1- y 1)^2 + (x’ 2-x 2, y’ 2- y 2)^2 + ... + (x’ n -x n , y’ n - y n ) 2 whenvar sum <factor n (factor is a coefficient), the next step is performed.
Computer radar slaveP a Run toP b Angle theta of deflection relative to current position at time of positiona→b = 。
For a first segment of track segment spliced to describe the relative position of the track line to the radarkGroup coordinate data (x k(1…)，y k(1…)) Calculating the coordinates relative to the current position of the radarxCoordinates are cosx k(1…)/cosθa→b，yThe coordinates arey k(1…) +(y b-y a ) (ii) a The second track segment being spliced, the coordinates being calculated with respect to the current position of the radarxThe coordinates arex k(1…)/cos(θa→b+θb→c)，yCoordinate is S +y k(1…) + (y b- y a ) (ii) a A third section of track spliced, the coordinates of which are calculated with respect to the current position of the radarxThe coordinates arex k(1…) / cos(θa→b+θb→c+θc→d)，yCoordinate is 2 + S +y k(1…) + (y b- y a ) And so on.
When the distance requirement for target detection in the track is R meters, the number Num of track segments needing to be spliced is = R/S, and Num is rounded up.
After coordinate data of the track line are stored in the millimeter wave radar memory, the millimeter wave radar enters the same track line again, after the millimeter wave radar enters a scene without GPS signals, the millimeter wave radar obtains the current position according to the inertial navigation system, track information far exceeding S meters is spliced by combining currently identified track segmentation information and a plurality of stored track segmentation information, and whether the detected obstacle is in the track line or not can be judged.
The method is exemplified by splicing and detecting the millimeter wave radar from one side of the track, and the splicing and detecting are performed from the other side, which is the same as above and is not described again.
The invention has the following beneficial effects:
the problem that the millimeter wave radar is limited by the detection capability and the angular resolution of the millimeter wave radar to the track, and long-distance track identification is difficult to realize is solved.
By combining the position information of the track line, the millimeter wave radar can judge whether the front target is positioned in the track without data fusion with other sensors (such as a camera, a laser radar and the like) so as to determine whether alarm information needs to be sent, thereby greatly reducing the probability of misinformation and maximizing the detection performance of the millimeter wave radar.
The above embodiment is an embodiment of the present invention, but the embodiment of the present invention is not limited by the above embodiment, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be regarded as equivalent replacements within the protection scope of the present invention.
