A method and system for extracting vehicle front elevation information based on multi-sensor fusion

Abstract

The application discloses a kind of vehicle front elevation information extraction method and system based on multi-sensor fusion, belong to multi-sensor fusion positioning and mapping technical field, the method includes: obtaining integrated navigation data;Establish the trajectory prediction model of wheel;Space-time synchronization is carried out to sensor;Laser radar mapping and point cloud search and slope correction algorithm;The preliminary positioning of vehicle is completed using integrated navigation data, the geometric model in the two cases of straight driving and steering driving of vehicle is considered respectively to realize the prediction of wheel trajectory, space-time synchronization is carried out using laser radar and integrated navigation, to establish the point cloud map in front of vehicle, based on the above predicted trajectory model, search is completed to point cloud, and original elevation information is obtained, but elevation information is accumulated error by ground slope, so sliding window and random sampling consistent ground slope algorithm are used to correct the accumulated error, and accurate and reliable elevation information is obtained.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-sensor fusion positioning and mapping, and particularly relates to a vehicle front elevation information extraction method and system based on multi-sensor fusion. BACKGROUND

[0002] With the development of science and technology, automobiles are developing towards intelligentization, electrification, networking and other diversification. Unmanned driving, new energy technology, intelligent networking and the like are gradually entering our lives. The continuous improvement of the automation degree of automobiles provides conditions for the improvement of their performance, and the research on the driving smoothness and stability of vehicles is of great significance, and the extraction accuracy of vehicle front elevation information is a key to the driving smoothness and stability of vehicles.

[0003] At present, the acquisition of vehicle front elevation information mainly relies on traditional GPS, IMU and other sensors, and the existing sensor fusion scheme has problems of low accuracy and being susceptible to interference. The extracted elevation information is not accurate enough, and the data is unreliable. Meanwhile, the influence of road slope on vehicle front elevation information has cumulative error, and the driving distance of the vehicle directly affects the accuracy of the vehicle front elevation information. SUMMARY

[0004] In view of the above problems, the application provides a vehicle front elevation information extraction method and system, which realizes the extraction of elevation information under the fusion of a laser radar-based multi-sensor, and the accuracy is reliable.

[0005] To achieve the above object, the method comprises the following steps:

[0006] Step S1: acquiring combined navigation data,

[0007] Step S2: acquiring the orientation and steering angle of the vehicle wheel based on the combined navigation data and the vehicle CAN bus, and establishing a trajectory prediction model of the vehicle wheel,

[0008] Step S3: synchronizing the time of the laser radar and the combined navigation system,

[0009] Step S4: performing coordinate conversion on the sensor system to establish a point cloud map,

[0010] Step S5: using a slope algorithm on the point cloud map to search for point clouds based on the predicted trajectory of the vehicle wheel to obtain original elevation information of a specific position,

[0011] Step S6: obtaining final elevation information after the elevation information is corrected by the slope algorithm,

[0012] Optionally, the trajectory prediction model of the vehicle wheel is realized together with the positioning function, and the specific steps comprise:

[0013] Step S21: regarding the vehicle model as a rectangle with the center point of the vehicle body as the rotation center and the heading angle as the rotation angle, the top vertices of the rectangle are regarded as the four wheels of the vehicle, and the wheel positioning function is realized;

[0014] Step S22: based on the heading angle, wheel rotation angle and coordinate information of the vehicle, the predicted path of the vehicle in the current pose state is derived using geometric relationships.

[0015] Optionally, the time of each sensor needs to be synchronized, and high-precision time synchronization is achieved by using 1PPS+TOD protocol, and a time synchronization device is used to give timestamps to each sensor.

[0016] Preferably, based on the time synchronization of the above sensors, the laser radar should be unified with the coordinate axis of the integrated navigation, and the specific steps include:

[0017] Step S41: converting the GNSS and INS measurement results from the latitude-longitude-height coordinate system to the geocentric terrestrial coordinate system, and converting from the geocentric terrestrial coordinate system to the northeast celestial coordinate system;

[0018] Step S42: converting all point cloud data obtained by the laser radar from the radar coordinate system to the northeast celestial coordinate system, and aligning with the sensor coordinate system; through the above steps, the sensors are synchronized in time and space;

[0019] Step S43: placing each frame of point cloud data of the laser radar into the point cloud map, and applying a filtering algorithm to filter out irrelevant points and only keep useful point cloud information.

[0020] Preferably, the specific steps of extracting the original elevation information of the specific point in the point cloud map are as follows:

[0021] Step S51: in the point cloud map, searching for point clouds according to the predicted trajectory of the vehicle to obtain local point cloud data. Then, the point cloud data is processed to obtain the elevation information of the local area.

[0022] Optionally, the original elevation information is affected by the slope, so the present application proposes a slope correction algorithm to eliminate the influence.

[0023] Step S61: performing a random sample consensus algorithm on the point cloud map to only keep the ground points to generate a ground point cloud map;

[0024] Step S62: extracting elevation information on the ground point cloud map, calculating the distance between the point and the coordinate origin, and placing the data into a sliding window with a certain capacity; the data in the sliding window is subjected to a random sample consensus algorithm to fit a straight line, and the slope of the straight line can be regarded as the slope of the ground at that point; based on the slope and the distance, the elevation information unaffected by the ground slope is calculated.

[0025] The application also discloses a vehicle front elevation information extraction system based on multi-sensor fusion.

[0026] A sensor time synchronization module is used to unify the time dimensions of the sensors and provide necessary conditions for data collection.

[0027] A sensor coordinate conversion module is used to align the spatial dimensions between the sensors and adapt the data to the system.

[0028] A wheel track prediction module is used to predict the path to be taken by the vehicle at the current pose based on the above sensors and collect the vehicle front elevation information.

[0029] A laser radar point cloud mapping module is used to generate a point cloud map.

[0030] A point cloud search module is used to collect original elevation information.

[0031] A slope correction module is used to correct the influence of the slope on the elevation information and obtain the final elevation information.

[0032] According to the specific embodiments provided by the application, the following technical effects are disclosed.

[0033] The application discloses a vehicle front elevation information extraction method and system, wherein the method comprises the following steps: synchronizing the time and space of sensor data; obtaining the pose information of the vehicle through geometric relationship to obtain the predicted trajectory of the vehicle; preprocessing the point cloud data from the laser radar and storing it in the point cloud map; performing point cloud search in the point cloud map based on the predicted trajectory of the vehicle, obtaining local point cloud data, and performing data processing to obtain original elevation information; to solve the cumulative error problem of the slope on the elevation information, the random sample consensus algorithm is used to fit the ground point cloud, the ground points are filtered out, the elevation information is extracted again on the ground point cloud map, and the distance between the corresponding points and the coordinate origin is calculated, and the data is stored in a sliding window with a certain capacity; for the data in the sliding window, the random sample consensus algorithm is used again to fit a straight line, and the slope of the straight line can be used as the slope of the current road.

[0034] The application proposes a solution to the multiple synchronization problems of multi-sensor fusion, fully considers the influence of the slope on the elevation information, proposes a slope algorithm, and thus obtains more accurate elevation information. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 The flowchart of the vehicle front elevation information extraction of the embodiments of the application;

[0036] Figure 2Trajectory prediction schematic diagram for a vehicle in straight driving according to an embodiment of the present application;

[0037] Figure 3 Trajectory prediction schematic diagram for a vehicle in turning driving according to an embodiment of the present application;

[0038] Figure 4 Schematic diagram of sensor coordinate conversion according to an embodiment of the present application;

[0039] Figure 5 Flowchart of slope algorithm according to an embodiment of the present application. DETAILED DESCRIPTION

[0040] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0041] The purpose of the present application is to provide a vehicle front elevation information extraction method and system based on multi-sensor fusion.

[0042] In order to make the above-mentioned purposes, characteristics and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0043] A vehicle front elevation information extraction system based on multi-sensor fusion comprises the following modules:

[0044] A sensor time synchronization module is used to unify the time dimension of each sensor and provide necessary conditions for data collection;

[0045] A sensor coordinate conversion module is used to align the spatial dimension between each sensor and adapt the data to the system;

[0046] A wheel trajectory prediction module predicts the path to be taken by the vehicle under the current pose based on the sensor and is used for collecting the vehicle front elevation information;

[0047] A laser radar point cloud mapping module is used for generating a point cloud map;

[0048] A point cloud search module is used for collecting original elevation information;

[0049] A slope correction module is used to correct the influence of slope on elevation information and obtain the final elevation information.

[0050] The wheel trajectory prediction module is specifically: regarding the vehicle model as a rectangle with the center point of the vehicle body as the rotation center and the heading angle as the rotation angle, regarding the four vertices of the rectangle as the four wheels of the vehicle, and realizing the wheel positioning function; using the heading angle, wheel rotation angle and coordinates of the vehicle, and based on the geometric relationship, obtaining the predicted path under the current pose state of the vehicle.

[0051] The sensor coordinate conversion module is specifically: converting the GNSS and INS from the latitude-longitude-height coordinate system to the geocentric geodetic coordinate system, and then from the geocentric geodetic coordinate system to the northeast sky coordinate system; converting all points of the laser radar from the radar coordinate system to the northeast sky coordinate system, and aligning the above sensor coordinate axes.

[0052] The slope correction module is specifically: first, performing a random sample consensus algorithm on the generated point cloud map, since only the points that have an impact on the elevation information are retained, after the random sample consensus algorithm, the point cloud only retains the ground points, and the non-ground points such as obstacles on the ground are filtered out by the algorithm, thereby generating a ground point cloud map;

[0053] On the ground point cloud map, the above elevation information extraction is performed again, and the distance of the point from the coordinate origin is calculated, the data is put into a sliding window with a certain capacity, and the data located in the sliding window is again subjected to a random sample consensus algorithm with a straight line as a model, and a straight line is fitted, and the slope of the straight line can be regarded as the slope of the ground at this place, and based on the slope and the distance, the elevation information that is not affected by the slope of the ground can be calculated.

[0054] As shown in Figure 1 The present application provides a vehicle front elevation information extraction method, which comprises:

[0055] Step S1: acquiring combined navigation data;

[0056] Step S2: based on the combined navigation data and the vehicle CAN bus, acquiring the orientation and steering angle of the wheels, and establishing a trajectory prediction model of the wheels;

[0057] Step S3: synchronizing the time of the laser radar and the combined navigation;

[0058] Step S4: performing coordinate conversion on the sensor system, and establishing a point cloud map;

[0059] Step S5: using a slope algorithm on the point cloud map, searching for point clouds based on the predicted trajectory of the wheels, and obtaining the original elevation information of the specific position;

[0060] Step S6: correcting the elevation information by the slope algorithm to obtain the final elevation information;

[0061] Some steps are described in detail as follows:

[0062] Step S2: based on the integrated navigation data and vehicle CAN bus, the orientation and steering angle of the wheel are obtained, and a trajectory prediction model of the wheel is established;

[0063] Step S21: the vehicle model is regarded as a rectangle with the center point of the vehicle body as the rotation center and the heading angle as the rotation angle, and the top points of the rectangle are regarded as the four wheels of the vehicle, so as to realize the wheel positioning function;

[0064] Step S22: using the heading angle, wheel steering angle and coordinate of the vehicle, the predicted path under the current pose state of the vehicle is obtained based on the geometric relationship.

[0065] Regarding the predicted path, the present application discusses two cases, the first one is the straight driving of the vehicle, as shown in Figure 2 , and the second one is the steering driving of the vehicle, as shown in Figure 3 .

[0066] When the vehicle drives straight, the wheel takes the heading angle of the vehicle as the slope angle, and takes the center point coordinate of the wheel as a point on the straight line, and draws a straight line in the point-slope form, which is the predicted trajectory of the vehicle. That is

[0067]

[0068] Among them, is the heading angle of the vehicle, which can be obtained by integrated navigation, x0 and y0 are the coordinates of the wheel, which can be obtained by integrated navigation and vehicle coordinate calculation.

[0069] When the vehicle drives straight, the steering condition conforms to the Ackerman steering geometry,

[0070] Ignoring the vehicle slip, and the rear wheel steering angle δ r is approximately 0, the geometric relationship can be obtained as

[0071] δ f / L=1 / R

[0072] The steering angles δ i and δ o of the inner and outer wheels are obtained from the CAN bus, and the steering radii of the inner and outer wheels are obtained.

[0073] The calculation of the steering center of the wheel is that the normal vectors of the advancing directions of the front and rear wheels on the same side intersect at a point, which is the steering center of the vehicle.

[0074] The trajectory prediction when steering is obtained as follows:

[0075] (X-x0) 2 +(Y-y0) 2 =R 2

[0076] Step S4: After the data from each sensor has undergone time synchronization processing, the spatial coordinate system needs to be unified. This invention adopts the Northeast-Northeast coordinate system. The coordinate unification of each system is as follows: Figure 4 As shown:

[0077]

[0078]

[0079]

[0080] Then transform from the Earth-centered Earth-fixed coordinate system to the Northeast-Upper-Heaven coordinate system (ENU):

[0081] P enu =R enu (P ecef -O ecef )

[0082] After this processing, the point cloud and wheel pose data are unified under the ENU coordinate system. At this time, each frame of point cloud from the LiDAR is first processed by voxel grid filtering to reduce the point cloud density to ensure the real-time performance of the entire system. Each point is then placed into the map, and as the vehicle position is updated, points far away from the vehicle are cleared, leaving only the rich point cloud in front of the vehicle for subsequent point cloud search.

[0083] Step S51: In the point cloud map, perform point cloud search based on the predicted trajectory of the wheel to obtain local points, and then process the data to obtain elevation information.

[0084] By selecting a point on the predicted trajectory of the wheel, the wheel profile can be regarded as an ellipse. In the point cloud map, all points inside the ellipse are considered to have an impact on the elevation. These points are weighted by the distance from the center point, with the farther away having a lower weight, and the elevation information under the influence of the slope is calculated.

[0085] This invention also provides a slope algorithm, the specific steps of which are as follows:

[0086] Step S61: Initially, when the vehicle begins to move, a preliminary point cloud map is established. This paper uses a random sampling consensus algorithm to fit the ground point cloud map. First, assuming the ground model is planar, a subset of sample points is randomly selected from the point cloud map to fit the model, finding all points P within the tolerance range. ransac The number of record points N ransac Then, randomly select some points again and repeat the above operation until the iteration ends, finally finding the point with the most points within the tolerance range. planar model α ransac This algorithm eliminates non-ground point clouds, preventing obstacles from interfering with the ground slope. The ground point cloud is then generated. The ground point cloud planar model is as follows:

[0087] ax + by + cz + d = 0

[0088] Step S62: On the ground point cloud map, the above-mentioned elevation information extraction is performed again to obtain Z ground and the distance D of the corresponding point and the coordinate origin ground , and the data in a certain capacity sliding window Ω. Since the vehicle corner is small and the steering track curvature is low, the vehicle track can be regarded as a straight line. The data in Ω is fitted into a straight line again using the random sampling consensus algorithm. The slope k slope of the straight line can be used as the current road slope. The actual ground elevation is

[0089] Z slope = Z - k slope * D slope

[0090] Each embodiment in the specification is described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts between the embodiments can be referred to each other.

[0091] The principles and implementation manners of the present application are described by applying specific examples in this paper. The above-mentioned embodiment is only used to help understand the method of the present application and its core idea. Meanwhile, for the general technical personnel in the field, the specific implementation manner and application range of the present application will be changed according to the idea of the present application. In conclusion, the content of the specification should not be understood as the limitation of the present application.

Claims

1. A method for extracting vehicle front elevation information based on multi-sensor fusion, characterized in that: The method includes the following steps: Step S1: Obtain integrated navigation data; Step S2: Based on the integrated navigation data and the vehicle CAN bus, obtain the wheel orientation and steering angle, and establish a wheel trajectory prediction model; Step S3: Synchronize the time of the lidar and the integrated navigation system; Step S4: Perform coordinate transformation on the sensor system to create a point cloud map; Step S5: Use the slope algorithm on the point cloud map to perform point cloud search based on the predicted trajectory of the wheel to obtain the original elevation information of the specific location; Step S6: After correcting the elevation information using a slope algorithm, the final elevation information is obtained; The specific steps of the point cloud search and slope algorithm include: Step S61: First, when the vehicle begins to move, a preliminary point cloud map is established. A random sampling consensus algorithm is used to fit the ground point cloud map. First, assuming the ground model is planar, a portion of sample points are randomly selected from the point cloud map to fit the model, and all points P within the tolerance range are found. ransac The number of record points N ransac Then, randomly select some points again and repeat the above operation until the iteration ends, finally finding the point with the most points within the tolerance range. planar model α ransac This algorithm eliminates non-ground point clouds, preventing obstacles from interfering with the ground slope. The ground point cloud is then generated accordingly, and its planar model is as follows: ax + by + cz + d = 0 Step S62: On the ground point cloud map, perform the above elevation information extraction again to obtain Z. ground and the distance D between the corresponding point and the origin of the coordinate system ground The data is stored in a sliding window Ω of a certain capacity. The random sampling consensus algorithm is then applied again to the data in Ω to fit a straight line, the slope of which is k. slope This can be used as the current road slope, then the actual ground elevation is... Z slope =Z ground -k slope *D ground 。 2. The method for extracting vehicle front elevation information based on multi-sensor fusion according to claim 1, characterized in that: Based on the fitted ground point cloud planar model in step S61, determine whether each point cloud data in each dataset is a ground point or a non-ground point. Specific steps include: Step S21: Treat the vehicle model as a rectangle with the center point of the vehicle body as the rotation center and the heading angle as the rotation angle. The vertices of the rectangle are regarded as the four wheels of the vehicle to realize the wheel positioning function. Step S22: Using the vehicle's heading angle, wheel rotation angle, and coordinate information, obtain the predicted path based on geometric relationships in the vehicle's current pose state.

3. The method for extracting vehicle front elevation information based on multi-sensor fusion according to claim 1, characterized in that: The specific steps for coordinate transformation and point cloud map creation using the sensor system include: Step S41: Convert the integrated navigation data from the latitude, longitude and altitude coordinate system to the geocentric coordinate system, and then from the geocentric coordinate system to the northeast-sky coordinate system; Step S42: Transform all points of the lidar from the radar coordinate system to the northeast-sky coordinate system and align them with the coordinate axes of the northeast-sky coordinate system of the integrated navigation system; Step S43: Place each frame of the LiDAR point cloud into a point cloud map, use a filtering algorithm to filter out irrelevant points, and retain useful points.

4. A system for extracting vehicle front elevation information based on multi-sensor fusion, characterized in that: The system for implementing the extraction method according to any one of claims 1-3, the system comprising: The sensor time synchronization module is used to unify the time dimension of each sensor; The sensor coordinate transformation module is used to align the spatial dimensions between the sensors, so that the data can be adapted to the system. The wheel trajectory prediction module predicts the path the vehicle will take in its current pose based on the sensors, and is used to collect elevation information in front of the vehicle. The LiDAR point cloud mapping module is used to generate point cloud maps. The point cloud search module is used for collecting raw elevation information; The slope correction module is used to correct the impact of slope on elevation information and obtain the final elevation information.

5. The vehicle front elevation information extraction system based on multi-sensor fusion according to claim 4, characterized in that: The wheel trajectory prediction module specifically involves: treating the vehicle model as a rectangle with the vehicle's center point as the rotation center and the heading angle as the rotation angle, with the vertices of the rectangle representing the four wheels of the vehicle, thus achieving wheel positioning; and using the vehicle's heading angle, wheel rotation angle, and coordinate information, obtaining the predicted path of the vehicle in its current pose state based on geometric relationships.

6. The vehicle front elevation information extraction system based on multi-sensor fusion according to claim 4, characterized in that: The sensor coordinate transformation module specifically transforms GNSS and INS from the latitude, longitude, and altitude coordinate system to the geocentric coordinate system, and then from the geocentric coordinate system to the northeast-sky coordinate system; it also transforms all points of the lidar from the radar coordinate system to the northeast-sky coordinate system, aligning them with the sensor coordinate axes of the northeast-sky coordinate system of the integrated navigation system.

7. The vehicle front elevation information extraction system based on multi-sensor fusion according to claim 4, characterized in that: The slope correction module specifically includes: First, a random sampling consensus algorithm is applied to the point cloud map generated above. Since only points that affect the elevation information are retained, after the random sampling consensus algorithm, only ground points are retained in the point cloud. Obstacles and non-ground points on the ground are filtered out by the algorithm, thus generating a ground point cloud map. On the ground point cloud map, the above elevation information extraction is performed again to obtain Z. ground and the distance D between the corresponding point and the origin of the coordinate system ground The data is placed into a sliding window of a certain capacity. The data in the sliding window are then subjected to a random sampling consensus algorithm with a straight line as the model. After fitting a straight line, the slope of the straight line can be regarded as the slope of the ground area corresponding to the sliding window data. Based on this slope and distance, the elevation information that is not affected by the ground slope can be calculated.

Images