Disclosure of Invention
In order to solve at least one of the above problems, according to an aspect of the present invention, there is provided a train obstacle detection method including: collecting full-line point cloud data by a laser radar along a full line; calculating a full-line limit boundary point off-line based on the full-line point cloud data; collecting real-time point cloud data on line by the laser radar; and monitoring the obstacle online based on the full-line limit boundary point and point cloud cluster data obtained from the real-time point cloud data.
In one embodiment, wherein monitoring the obstacle online comprises determining whether a boundary point of the point cloud cluster data is within a rectangular bounding box of the full-line limit boundary point.
In one embodiment, when the proportion of the boundary points of the point cloud cluster data falling within the rectangular bounding box of the full-line limit boundary point to the total boundary points of the point cloud cluster data exceeds a threshold value, the object represented by the point cloud cluster data is determined to be an obstacle.
In one embodiment, wherein obtaining the point cloud cluster data comprises setting different clustering radii with the lidar as an origin, and partitioning within the clustering radii using the same clustering threshold distance.
In one embodiment, the method further comprises extracting orbit point cloud data based on the real-time point cloud data.
In one embodiment, wherein the orbit point cloud data is extracted by a principal component analysis algorithm.
In one embodiment, the method further comprises screening the real-time point cloud data based on a region of interest.
In one embodiment, the region of interest is selected by means of pass-through filtering.
In one embodiment, wherein acquiring the full line point cloud data comprises acquiring a real-time pose of the train by an inertial measurement unit, thereby transforming trajectory lines in the full line point cloud data from a lidar coordinate system to a global coordinate system.
In one embodiment, the offline calculation of the all-line limit boundary includes enclosing the body limit with a rectangular enclosing frame on the basis of the body limit of the train, a margin is left between the rectangular enclosing frame and the body limit, and a node of the rectangular enclosing frame is the all-line limit boundary.
In one embodiment, the step of calculating the full-line limit boundary off-line comprises the step of acquiring the real-time pose of the train by an inertial measurement unit, so as to obtain the full-line limit boundary under a global coordinate system.
In one embodiment, wherein acquiring the real-time point cloud data online comprises comparing the real-time point cloud data obtained online with the full-line point cloud data using a point cloud registration algorithm to determine a current location of the train.
In one embodiment, wherein the lidar is a 3D multiline lidar.
In one embodiment, the full line point cloud data includes three-dimensional coordinates, reflection intensity, and trajectory line of the train as it travels.
In one embodiment, the lidar is mounted at a front center of a head end of the train.
According to another aspect of the invention, a non-transitory computer readable medium is proposed, having computer program instructions stored thereon, characterized in that the computer program instructions, when executed by one or more processors, implement the above-mentioned method.
The method provided by the invention can independently detect the obstacles in the small-radius curve and other road sections where the cameras and the millimeter wave radar are easy to have monitoring blind areas, and can reduce the difficulty of judging the obstacles and improve the efficiency of on-line obstacle detection.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, exemplary embodiments according to the present invention will be described in detail below with reference to the accompanying drawings. It is to be understood that the described embodiments are merely a subset of embodiments of the invention and not all embodiments of the invention, with the understanding that the invention is not limited to the example embodiments described herein. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the invention described herein without inventive step, shall fall within the scope of protection of the invention.
Fig. 1 shows a comparison of detection ranges of a camera, a millimeter wave radar, and a laser radar in obstacle detection in a small radius curve section.
As shown in fig. 1, in a rail transit scene, since the types of foreign objects intruding into a traffic area are random and difficult to predict in advance, detection omission may occur in camera detection. In the obstacle detection of the small-radius curve section, due to the fact that the horizontal monitoring areas of the camera and the millimeter wave radar are limited, missing detection of the obstacle can also occur. And the lidar can acquire data horizontally around, and can play a main detection role in the situation.
The invention provides a rail transit obstacle detection method based on a laser radar, which can detect obstacles existing in a front road section in the rail transit driving process in real time, and can independently detect the obstacles in the road sections where cameras such as small radius curves and millimeter wave radars are easy to have monitoring blind areas. The characteristic of fixed line structure of rail transit is utilized in the calculation, and the mode of combining off-line calculation and on-line calculation is adopted, so that the difficulty of judging the barrier is reduced, and the efficiency of detecting the barrier on line is improved.
Next, a train obstacle detection method according to an embodiment of the present invention will be described in detail with reference to fig. 2.
As shown in fig. 2, a method for detecting a train obstacle includes: collecting full-line point cloud data by a laser radar along a full line; calculating a full-line limit boundary point off-line based on the full-line point cloud data; collecting real-time point cloud data on line by the laser radar; and monitoring the obstacle online based on the full-line limit boundary point and point cloud cluster data obtained from the real-time point cloud data.
In one embodiment, the lidar is a 3D multiline lidar. The multiline laser radar is a laser rotating range radar which simultaneously transmits and receives a plurality of laser beams. There are mainly 4, 8, 16, 32, 64 and 128 lines on the market today. The multiline lidar can identify height information of objects and acquire 3D scans of the surrounding environment.
The train obstacle detection method based on the laser radar combines the offline calculation of the full-line point cloud map and the full-line limit boundary point and the online detection of the rail obstacle.
The full-line point cloud map is full-line point cloud data acquired after a train is provided with a 3D multi-line laser radar and runs for a certain number of times along the full line of a line in service of the train. The full-line point cloud data can be used for positioning when the train runs formally. The full-line limiting boundary point is a rectangular enclosing frame obtained by simplifying the train body limit. In the on-line detection, if the real-time point cloud of the object invades into the full-line limit boundary range corresponding to the position of the object, the real-time point cloud of the object is judged as an obstacle. And the calculation of the full-line limit boundary point is obtained by off-line calculation of the train running track acquired in the full-line point cloud map acquisition process.
The on-line detection of the rail obstacle comprises real-time point cloud data acquisition, real-time point cloud data screening and filtering, rail point cloud extraction, object point cloud segmentation and clustering and obstacle judgment, and is obtained by on-line calculation in the process that the train is provided with a 3D multi-line laser radar to run along a service line.
Next, an installation position of the laser radar according to the embodiment of the present invention is explained with reference to fig. 3.
In one embodiment, the lidar is mounted at a front center of the head end of the train, as shown in fig. 3. In a specific embodiment, in view of simplifying calculation processing of point cloud data obtained by detection and ensuring a detection range of a laser radar, an optimal installation position of the laser radar is located at a front central position of a head end of a rail transit train, and the laser radar can be arranged on a holder so as to adjust an angle. Considering the situation of back-turning and end-changing in urban rail transit, two laser radars can be respectively assembled at the head end and the tail end of a train.
Hereinafter, the principle of the train obstacle detection method according to the embodiment of the present invention will be described with reference to fig. 4 to 6.
S1: and the laser radar acquires the full-line map information along the full line. The full-line point cloud map is full-line point cloud data acquired after a train is provided with a 3D multi-line laser radar and runs for a certain number of times along the full line of a line in service of the train. In one embodiment, the full line point cloud data includes three-dimensional coordinates, reflection intensity, and trajectory line of the train as it travels. Specifically, the full-line point cloud data comprises three-dimensional coordinates and reflection intensity of radar scanning points, and is mainly used for providing positioning information for the formal operation of the train. In addition, the point cloud map also includes the track line of the train when the train runs on the line. The trajectory line is a curve in space and represents a series of coherent three-dimensional discrete points, and the point set is used for calculating limit points and extracting a track point cloud.
In one embodiment, acquiring the full line point cloud data includes acquiring a real-time pose of the train by an inertial measurement unit, thereby converting trajectory lines in the full line point cloud data from a lidar coordinate system to a global coordinate system.
In particular, in order to facilitate the transformation of the trajectory data from the lidar local coordinate system to the global coordinate system, the train is required to be equipped with an IMU (Inertial Measurement Unit: a device for measuring the three-axis attitude angle (or angular rate) and acceleration of an object) to obtain the real-time pose of the train, thereby completing the transformation of the trajectory from the lidar coordinate system → the train coordinate system → the global coordinate system. The conversion formula is as follows:
wherein
Being the coordinates of the trajectory line in the global coordinate system,
is the coordinate of the origin of the train coordinate system under the global coordinate system,
is the coordinate of the trajectory line in the radar coordinate system,
is the coordinate of the origin of the radar coordinate system under the train coordinate system, R
BIs a rotation matrix from a radar coordinate system to a train coordinate system and is determined by the installation position of the radar on the train, R
PA rotation matrix from the train coordinate system to the global coordinate system is provided by the IMU.
S2: and calculating the full-line limit boundary point offline. Delimitation refers to the line of dimensions of the contour that must not be exceeded, as specified for trains and buildings and equipment close to the line, in order to ensure the safety of the trains running on the railway line, preventing them from striking buildings and equipment adjacent to the line. The full-line limit boundary point is a boundary standard for judging whether the object point cloud is an obstacle. In one embodiment, calculating the all-line limit boundary off-line includes surrounding a body boundary of the train with a rectangular surrounding frame on the basis of the body boundary, a margin being left between the rectangular surrounding frame and the body boundary, and a node of the rectangular surrounding frame being the all-line limit boundary.
Specifically, in consideration of convenience of calculation and storage, as shown in fig. 4, the limit may be surrounded by a rectangular frame on the basis of the limit of the vehicle body, and a certain margin may be left between the rectangular frame and the limit. The larger the margin, the higher the safety, but also the increased risk of false alarms. And four nodes of the rectangular surrounding frame are the limiting boundary points.
In one embodiment, the off-line calculation of the full-line limit boundary comprises acquiring a real-time pose of the train by an inertial measurement unit, so as to obtain the full-line limit boundary in a global coordinate system.
Specifically, after the trajectory line in the global coordinate system is obtained through S1, a full-line limit point set in the global coordinate system can be obtained through spatial geometric calculation in combination with the train real-time pose data acquired by the IMU. Each element in the set corresponds to a point in the trajectory line, each element comprises four bounding points, and each bounding point comprises spatial coordinate data. During specific calculation, defining a limit, namely determining a distance H1 from a track center line to the top of the limit, a distance H2 from the track center line to the bottom of the limit and a distance W from the track center line to two sides of the limit; dividing the obtained trajectory line into a plurality of sections according to the shape of the obtained trajectory line, wherein each section can be approximately straight, estimating the direction vector of the central line of the section of the trajectory line, and calculating the coordinates of each node one by one from the midpoint of the central line of the section of the trajectory line according to the direction vector and the simultaneous equation of the distance from the point to the straight line.
Specifically, the Track is divided into n sections of Track ═ TrackiEach track (i ═ 1,2, …, n), and each track segmentiCan be approximated as straight segments. trackiIs a set of spatial points, i.e. for a track comprising m pointsiFor tracki={Pj(xj,yj,zj) J ═ 1,2, …, m. For any trackiThe centroid coordinate W (x) can be calculated by PCA algorithmw,yw,zw) Front three main directions (i.e. the track)iX, y, z direction vector of) px、py、pzAnd trackiThe dimension l in these three directionsx、ly、lz. The problem of finding a limiting boundary point can be abstracted to a coordinate (x) passing through point 11,y1,z1) The vector (direction vector) coordinate p composed of point 2 and point 1d(dx,dy,dz) And distance l between point 1 and point 212Determining the coordinates (x) of point 22,y2,z2). The solution method is to solve a set of simultaneous equations:
in the calculation of pdIn turn take px、py、pzCorresponding to l12Taken as l in sequencexThe coordinates of 8 limit points can be solved by the method of the following steps of/2, W, H1 and H2.
S3: and the laser radar acquires data on line. The 3d multi-line laser radar carried by the train scans and acquires real-time point cloud data in real time in the process of running on a given line. And obtaining the real-time point cloud data and the positioning information corresponding to the real-time point cloud data. And the positioning information is the point data of the current position of the laser radar corresponding to the trajectory line. The method of positioning is not unique. In one embodiment, acquiring the real-time point cloud data online includes comparing the real-time point cloud data obtained online to the full-line point cloud data using a point cloud registration algorithm to determine a current location of the train. Other positioning methods, such as GPS positioning, wheel speed positioning, etc., may also be employed.
S4: and screening and filtering the real-time point cloud data. In one embodiment, the method further comprises screening the real-time point cloud data based on a region of interest. Specifically, in order to avoid unnecessary calculation expense for each frame of the obtained real-time point cloud data, a Region of Interest (ROI: a Region to be processed manually selected from the original data) may be selected according to the train positioning information corresponding to the frame of the real-time point cloud.
The method of selection of the ROI is not unique. In one embodiment, the region of interest is selected by means of pass-through filtering. Specifically, after the positioning information of the train is obtained, the straight-through filtering method can obtain the information of the flat longitudinal section of the track in front of the train, such as the gradient and the radian, according to the track line data. Therefore, the laser radar can be used as a center, the sections in the x (longitudinal), y (transverse) and z (vertical) directions are defined, and points outside the defined sections can be discarded. If the front track has a slope, the section in the z (vertical) direction needs to be correspondingly longer to cover the track with the slope; if the front track has a radian, the section in the y (transverse) direction needs to be correspondingly defined to be longer so as to cover the radian track; the x (longitudinal) direction may be segmented according to the effective detection range of the lidar used.
In addition, ground point cloud data is more, the characteristics of the ground point cloud data are obvious, barrier information does not exist, the ground point cloud data are easy to remove, and calculation after removal is convenient, so that the ground point cloud data are removed together. The aforementioned ROI selection method can reject the ground point cloud when the track is a certain distance from the ground. If the track is close to the ground, the ground plane is removed by using a plane segmentation algorithm. The plane division algorithm is not exclusive, and a RanSAC (random sample consensus) algorithm may be preferably used. The basic steps of the algorithm are that a certain seed point is randomly selected from the point cloud, a model equation is fitted by using the seed point, whether the model can adapt to more points under the condition of meeting an error threshold value is calculated, if so, the model is recorded as a result, and if not, a new seed point is selected for cycle iteration. The method can quickly screen the main plane in the real-time point cloud data.
Only the obstacle in the ROI needs to be concerned in the calculation, and since the lidar scans 360 °, data at the rear position of the train or at a position far above the train does not need to be concerned, for example, taking x (longitudinal) [0:150], y (lateral) [ -50:50], and z (vertical) [ -5:5] as the center means that only data within the range of 150m in front of the lidar, 50m in the left side, 50m in the right side, 5m in the bottom, and 5m in the top are calculated.
S5: and (5) point cloud segmentation and clustering. The method for segmenting the clusters is not unique, and preferably, a segmentation clustering algorithm based on Euclidean distance can be adopted. The segmentation clustering algorithm based on the Euclidean distance can be combined with the characteristic that the laser radar wire harnesses are not uniformly distributed. As shown in fig. 5, in one embodiment, obtaining the point cloud cluster data includes setting different clustering radii with the lidar as an origin, and performing partitioning within the clustering radii using the same clustering threshold distance. Specifically, different clustering radii (Radius) within which the same clustering threshold distance (Tolerance) is used are set with the radar as the origin. The larger the cluster radius, the larger the cluster threshold distance. The clustering threshold distance indicates that two points are considered to belong to the same cluster when the Euclidean distance between the two points is less than or equal to the value.
S6: and extracting the track point cloud. In one embodiment, orbit point cloud data is extracted based on the real-time point cloud data. The track point cloud extraction can refer to the existing track line point cloud extraction algorithm of a road train, and the most direct method is to screen out the track by using the reflection intensity value in the returned data of the laser radar to carry out direct filtering. In one embodiment, the orbit point cloud data is extracted by a principal component analysis algorithm.
In particular, a Principal Component Analysis (PCA) algorithm can accurately extract an orbit point cloud. The PCA algorithm can extract k-dimensional orthogonal features (i.e., k principal components) from the n-dimensional data. The calculation process is as follows:
calculating the mean value of each dimension data, and subtracting the mean value from each dimension data.
Solving a characteristic covariance matrix:
and thirdly, solving eigenvalues and eigenvectors of the covariance matrix, and arranging the eigenvalues from large to small, wherein the first k eigenvalues are k-dimensional principal characteristics, and the corresponding eigenvectors are principal directions.
Referring to fig. 6, it is noted that the main direction corresponding to the largest eigenvalue is the x direction, and since the track structure is slender and continuous, the longitudinal extension direction thereof should be the x direction, so that it can be determined whether the current point cloud cluster is a track by calculating whether the deviation value between the x main direction of the point cloud cluster and the x main direction of the track point cloud cluster in the previous radius is smaller than the threshold value in each radius range according to the sequence of the cluster radii from small to large in S5 (when searching in the first radius, the deviation value with the x direction of the radar coordinate system is calculated instead).
S7: and judging the cloud of the obstacle points. And (4) after the orbit point clouds are obtained according to the S6, all the orbit point clouds in the S5 result are removed, and residual point cloud clusters are obtained. In one embodiment, online monitoring of the obstacle includes determining whether a boundary point of the point cloud cluster data is within a rectangular bounding box of the full-line limit boundary point. Specifically, the remaining point cloud clusters are projected to the xOy plane, then the boundary of each point cloud cluster on the xOy plane is extracted, and the boundary is subjected to down-sampling. And traversing the points in the boundary of the object after the down-sampling, inquiring the corresponding vehicle body limit boundary points which are calculated off line according to the x coordinates of the points, and judging whether the points are positioned in the rectangular surrounding frame of the limit boundary points.
In one embodiment, when the proportion of the boundary points of the point cloud cluster data falling within the rectangular bounding box of the full-line limit boundary point to the total boundary points of the point cloud cluster data exceeds a threshold value, the object represented by the point cloud cluster data is determined to be an obstacle. Specifically, if the proportion of the boundary points of the point cloud cluster data falling in the rectangular surrounding frame of the full-line limit boundary point to the total boundary points of the point cloud cluster exceeds a certain threshold, it is determined that the object represented by the point cloud cluster is an obstacle and invades the normal driving area of the train, and the safe driving is affected.
The present invention also provides a non-transitory computer readable medium having computer program instructions stored thereon. The computer program instructions, when executed by one or more processors, implement the above-described methods.
In conclusion, the method and the device have the advantages that the characteristic of fixed line structure of the rail transit is utilized, the mode of combining the offline calculation of the safe driving area and the online calculation of the position of the object is adopted, the difficulty of the obstacle judgment algorithm is reduced, and the efficiency of online obstacle detection is improved.
Although the illustrative embodiments have been described herein with reference to the accompanying drawings, it is to be understood that the foregoing illustrative embodiments are merely exemplary and are not intended to limit the scope of the invention thereto. Various changes and modifications may be effected therein by one of ordinary skill in the pertinent art without departing from the scope or spirit of the present invention. All such changes and modifications are intended to be included within the scope of the present invention as set forth in the appended claims.
In the description provided herein, numerous specific details are set forth. It is understood, however, that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
Similarly, it should be appreciated that in the description of exemplary embodiments of the invention, various features of the invention are sometimes grouped together in a single embodiment, figure, or description thereof for the purpose of streamlining the invention and aiding in the understanding of one or more of the various inventive aspects. However, the method of the present invention should not be construed to reflect the intent: that the invention as claimed requires more features than are expressly recited in each claim. Rather, as the following claims reflect, inventive aspects lie in less than all features of a single disclosed embodiment. Thus, the claims following the detailed description are hereby expressly incorporated into this detailed description, with each claim standing on its own as a separate embodiment of this invention.
It will be understood by those skilled in the art that all of the features disclosed in this specification (including any accompanying claims, abstract and drawings), and all of the processes or elements of any method or apparatus so disclosed, may be combined in any combination, except combinations where such features are mutually exclusive. Each feature disclosed in this specification (including any accompanying claims, abstract and drawings) may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise.
Furthermore, those skilled in the art will appreciate that while some embodiments described herein include some features included in other embodiments, rather than other features, combinations of features of different embodiments are meant to be within the scope of the invention and form different embodiments. For example, in the claims, any of the claimed embodiments may be used in any combination.
The above description is only for the specific embodiment of the present invention or the description thereof, and the protection scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and the changes or substitutions should be covered within the protection scope of the present invention. The protection scope of the present invention shall be subject to the protection scope of the claims.