Method, apparatus, and device for vehicle simultaneous localization and mapping

Abstract

The present invention relates to the field of autonomous driving technology, and discloses a method, device, and equipment for vehicle simultaneous localization and mapping. The method includes: obtaining inertial measurement unit data, wheel speed data, and global positioning system data of the vehicle, and using an error-state Kalman filter algorithm based on the inertial measurement unit data, wheel speed data, and global positioning system data to obtain the three-dimensional pose information of the vehicle; determining the gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establishing a two-dimensional map based on the gravity plane, and determining the position of the vehicle in the two-dimensional map; determining the position of the obstacle in the two-dimensional map based on the obstacle position information, and determining the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle; updating the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel, and fitting the lane line in the two-dimensional map by the least squares method.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and particularly to a method, device, and equipment for vehicle simultaneous localization and mapping. Background Art

[0002] Simultaneous localization and mapping is a key step in vehicle autonomous driving, which enables the vehicle to perceive its position and the surrounding environment during operation, so that the vehicle's decision-making module can guide the vehicle's next action based on the vehicle's position information and environmental information, and then complete the set autonomous driving task. Accurate simultaneous localization and mapping is an essential part of the vehicle autonomous driving task, especially when the vehicle is running in a highway section scenario and facing undulating road conditions.

[0003] In order to cope with the road conditions with long-distance slope changes, in the related art, a technical solution combining carrier phase differential technology, cameras, lidar, and high-precision maps is adopted to create a three-dimensional obstacle map, but the cost is too high. Summary of the Invention

[0004] In view of this, the present invention provides a method, device, and equipment for vehicle simultaneous localization and mapping to solve the problem that in the related art, a technical solution combining carrier phase differential technology, cameras, lidar, and high-precision maps is adopted to create a three-dimensional obstacle map, but the cost is too high when the autonomous driving vehicle is in a highway section scenario with long-distance slope changes.

[0005] In a first aspect, the present invention provides a method for vehicle simultaneous localization and mapping, the method comprising: obtaining inertial measurement unit data, wheel speed data, and global positioning system data of the vehicle; using an error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain three-dimensional pose information of the vehicle; determining a gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establishing a two-dimensional map based on the gravity plane, and determining the position of the vehicle in the two-dimensional map; obtaining lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determining the position of the obstacle in the two-dimensional map based on the obstacle position information, and determining the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle; updating the three-dimensional pose information based on the change of the lane line information during the vehicle's travel, and fitting the lane line in the two-dimensional map by the least squares method, where the two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

[0006] In an alternative embodiment, an error-state Kalman filter algorithm is used based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain the three-dimensional pose information of the vehicle, including: obtaining the angular increment of the vehicle represented by the inertial measurement unit data, and accumulating based on the angular increment to obtain the three-dimensional pose information; wherein, the inertial measurement unit data includes: the acceleration signal of the vehicle and the angular velocity signal of the vehicle, performing an integration operation on the acceleration signal to obtain the speed of the vehicle, performing an integration operation on the speed to obtain the displacement of the vehicle; performing an integration operation on the angular velocity signal to obtain the roll angle, pitch angle, and yaw angle of the vehicle.

[0007] In an alternative embodiment, using the error-state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain the three-dimensional pose information of the vehicle further includes: obtaining the position increment of the vehicle represented by the global positioning system data, and accumulating based on the position increment to obtain the three-dimensional pose information; the observation equation updated based on the global positioning system data is as follows:

[0008] P GPS = P + δP GPS

[0009] wherein, P GPS is the position of the vehicle represented by the global positioning system data, P is the vehicle position at the current moment, and δP GPS is the vehicle position difference between P GPS and P.

[0010] In an alternative embodiment, using the error-state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain the three-dimensional pose information of the vehicle further includes: obtaining the speed increment of the vehicle represented by the wheel speed data, and accumulating based on the speed increment to obtain the three-dimensional pose information; the observation equation updated based on the wheel speed data is as follows:

[0011] V w h eel = R IG V G

[0012] wherein, h w P eel is the wheel speed obtained based on the wheel speed pulses of the vehicle, P IG is the conversion relationship of the wheel speed pulses from the body coordinate system to the north-east-down coordinate system, and V G is the vehicle body speed in the north-east-down coordinate system.

[0013] In an alternative embodiment, determining the position of an obstacle in the two-dimensional map based on the obstacle position information includes: determining the position of the obstacle in the two-dimensional map by the following formula:

[0014] P EN =T trans R GI P I

[0015] where P EN is the two-dimensional obstacle position in the two-dimensional map, P I is the two-dimensional obstacle position in the two-dimensional space represented by the obstacle position information, R GI is the conversion relationship of the two-dimensional obstacle position from the vehicle body coordinate system to the current vehicle body's three-dimensional northeast-up coordinate system, θ1 is the pitch angle, θ0 is the roll angle, and T trans represents the mapping relationship of the obstacle position from the three-dimensional northeast-up coordinate system to the two-dimensional northeast coordinate system.

[0016] In an alternative embodiment, determining the speed of an obstacle in the two-dimensional map based on the speed information of the obstacle includes: determining the speed of the obstacle in the two-dimensional map by the following formula:

[0017] V EN =T trans R GI V I

[0018] where V EN is the obstacle speed in the two-dimensional map, and V I is the obstacle speed in the three-dimensional space represented by the obstacle speed information.

[0019] In an alternative embodiment, fitting a lane line in the two-dimensional map by the least squares method includes: constructing a lane line fitting function, which is a third-order polynomial; constructing an error function based on the error between the fitting value and the observed value of the lane line fitting function, solving the minimum value of the error function to obtain the coefficients of each term in the lane line fitting function; and obtaining the fitted lane line in the two-dimensional map based on the coefficients of each term.

[0020] Second aspect, the present invention provides a device for vehicle simultaneous localization and mapping. The device includes: a data acquisition module, configured to acquire inertial measurement unit data, wheel speed data, and global positioning system data of a vehicle, and obtain three-dimensional pose information of the vehicle by using an error-state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data; a first positioning module, configured to determine a gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establish a two-dimensional map based on the gravity plane, and determine the position of the vehicle in the two-dimensional map; a second positioning module, configured to acquire lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determine the position of an obstacle in the two-dimensional map based on the obstacle position information, and determine the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle; a mapping module, configured to update the three-dimensional pose information based on the change of the lane line information during the vehicle's travel, and fit the lane line in the two-dimensional map by using the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

[0021] Third aspect, the present invention provides a computer device, including: a memory and a processor, which are communicatively connected to each other. The memory stores computer instructions, and the processor executes the computer instructions to execute the method for vehicle simultaneous localization and mapping according to the first aspect or any corresponding implementation thereof.

[0022] Fourth aspect, the present invention provides a computer-readable storage medium, on which computer instructions are stored. The computer instructions are used to cause a computer to execute the method for vehicle simultaneous localization and mapping according to the first aspect or any corresponding implementation thereof.

[0023] The method for vehicle simultaneous localization and mapping provided in this embodiment is applicable to the scenario where the vehicle is running on a highway section with undulating road conditions. By using an IMU, a GPS, and a wheel speed sensor, a positioning result that meets the positioning requirements of an autonomous vehicle can be obtained, effectively saving the positioning cost; updating the three-dimensional pose information based on the change of the lane line information during the vehicle's travel can improve the local objectivity of the created two-dimensional map; the obtained two-dimensional map including the vehicle position can reduce the data usage cost of the vehicle's control decision-making and planning module. Description of the Drawings

[0024] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the related art, the following will briefly introduce the drawings required for use in the description of the specific embodiments or the related art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0025] Figure 1 The flowchart shows the method for vehicle simultaneous localization and mapping provided by the embodiments of the present invention;

[0026] Figure 2 The flowchart shows the method for vehicle simultaneous localization and mapping provided by the embodiments of the present invention;

[0027] Figure 3 The structural diagram shows the device for vehicle simultaneous localization and mapping provided by the embodiments of the present invention;

[0028] Figure 4 It is the hardware structural diagram of the computer device of the embodiments of the present invention. Specific Embodiments

[0029] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of the present invention.

[0030] The simultaneous localization and mapping process in the related art includes: using visual feature points, lidar matching, matching with a high-precision map, or using carrier phase differential technology for three-dimensional positioning to obtain the three-dimensional position; converting the three-dimensional obstacle information sensed by the vehicle through the three-dimensional position in the foregoing steps to create a three-dimensional obstacle map; and transmitting the three-dimensional vehicle information and obstacle information to the control decision-making and planning module of the vehicle. There are technical problems in the related art such as excessive cost and the inability to directly use the three-dimensional information for the control decision-making and planning module of the vehicle.

[0031] According to the embodiments of the present invention, a method embodiment for vehicle simultaneous localization and mapping is provided. It should be noted that the steps shown in the flowchart of the drawings can be executed in a computer system such as a set of computer-executable instructions. And although the logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order than here.

[0032] In this embodiment, a method for vehicle simultaneous localization and mapping is provided, which can be used for the above-mentioned autonomous vehicles. Figure 1 The flowchart of the method for vehicle simultaneous localization and mapping provided by the embodiment of the present invention is shown, as Figure 1 shown. The process includes the following steps:

[0033] Step S101, obtain the inertial measurement unit (IMU) data, wheel speed data, and Global Positioning System (GPS) data of the vehicle, and use the error state Kalman filter algorithm based on the IMU data, wheel speed data, and GPS data to obtain the three-dimensional pose information of the vehicle.

[0034] In this step, the IMU is a device that can measure the three-axis attitude angle (or angular velocity) and acceleration of the vehicle. The IMU usually includes three single-axis accelerometers and three single-axis gyroscopes. The wheel speed data provides the speed information of each wheel of the vehicle. The GPS data provides positioning information such as the longitude, latitude, altitude, and speed of the vehicle. After the vehicle obtains the GPS data for the first time, the origin of the east north up map can be obtained.

[0035] Using the error state Kalman filter algorithm, define the error state vector, including: position error, speed error, angle error, acceleration error, and angular velocity error. Predict the change of the state based on the IMU data, calculate the change of speed and position based on the accelerometer data, and calculate the change of angle based on the gyroscope data.

[0036] Use the wheel speed data and GPS data to update the state estimation. The wheel speed data can provide more accurate mileage information, and the GPS data can provide absolute position information. Define the process noise and observation noise and their covariance matrices. Update the state estimation and covariance matrix based on the IMU data, and correct the state estimation and covariance matrix based on the wheel speed data and GPS data. Obtain the three-dimensional pose information of the vehicle, including: position, speed, and attitude, where the position includes precision, dimension, and altitude, the speed is a three-dimensional speed vector, and the attitude includes: pitch angle, roll angle, and yaw angle.

[0037] Step S102, based on the angle information characterized by the IMU data, determine the gravity plane where the vehicle pose characterized by the three-dimensional pose information is located, establish a two-dimensional map based on the gravity plane, and determine the position of the vehicle in the two-dimensional map.

[0038] Based on the pitch angle and yaw angle, the attitude of the vehicle relative to the gravity plane can be determined. If both the pitch angle and yaw angle are small, the vehicle is approximately located on the gravity plane. If the pitch angle or yaw angle is large, the vehicle has a certain inclination or rotation relative to the gravity plane.

[0039] The accelerometer data contains the acceleration of the vehicle in each axis, which also includes the component of the gravitational acceleration. The attitude angles of the vehicle, namely the pitch angle and roll angle, which describe the inclination of the vehicle relative to the horizontal plane, can be calculated using the accelerometer data. Based on the attitude angles, a plane perpendicular to the direction of gravity can be determined. The gravity plane is a plane perpendicular to the direction of gravity and is usually approximately parallel to the Earth's surface. When projecting the vehicle position in three-dimensional space onto a two-dimensional map, the height information of the vehicle can be ignored. A two-dimensional coordinate system can be established in the gravity plane. The initial position of the vehicle can be selected as the origin, or a fixed global coordinate system can be set. The three-dimensional position of the vehicle, longitude, latitude, and height, is projected onto the gravity plane to obtain the two-dimensional coordinates of the vehicle.

[0040] Step S103: Obtain the lane line information, obstacle position information, and obstacle speed information sensed by the vehicle. Based on the obstacle position information, determine the position of the obstacle in the two-dimensional map, and based on the obstacle speed information, determine the speed of the obstacle in the two-dimensional map.

[0041] In this step, the lane line in the traffic environment or the position and speed information of the obstacle, etc., can be obtained and processed through the vehicle's sensing system, such as hardware like radar or in-vehicle camera, or computer vision technology. The obstacle can be a vehicle or a pedestrian, etc.

[0042] Step S104: Update the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel. Fit the lane line in the two-dimensional map by the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

[0043] Obtain a number of discrete points sensed during the vehicle's travel. Based on the position transformation of the number of discrete points in the two-dimensional map, obtain the rotation matrix and translation matrix representing the position transformation relationship, and update the three-dimensional pose information of the vehicle based on the rotation matrix and translation matrix.

[0044] The method for vehicle simultaneous localization and mapping provided in this embodiment is applicable when the vehicle is running in a highway scenario with undulating road conditions. By using an IMU, GPS, and wheel speed sensors, a positioning result that meets the positioning requirements of an autonomous vehicle can be obtained, effectively saving positioning costs. Updating the three-dimensional pose information based on the transformation of lane line information during vehicle movement can improve the local objectivity of the created 2D map. The obtained 2D map including the vehicle's position can reduce the cost of data used by the vehicle's control decision-making and planning module.

[0045] In this embodiment, a method for vehicle simultaneous localization and mapping is provided, which can be used for various types of vehicles. Figure 2 The flowchart of the method for vehicle simultaneous localization and mapping according to an embodiment of the present invention is shown, as Figure 2 shown, and the process includes the following steps:

[0046] Step S201: Obtain the inertial measurement unit data, wheel speed data, and global positioning system data of the vehicle, and use the error-state Kalman filter algorithm based on the inertial measurement unit data, wheel speed data, and global positioning system data to obtain the three-dimensional pose information of the vehicle.

[0047] The pose of the vehicle can be calculated using a 15-dimensional state vector. where δp T is the position offset, δv T is the velocity offset, δθ T is the angle offset, is the acceleration offset, is the angular velocity offset.

[0048] Specifically, the above step S201 includes:

[0049] Step S2011: Obtain the angle increment of the vehicle represented by the inertial measurement unit data, and obtain the three-dimensional pose information based on the accumulation of the angle increment; where the inertial measurement unit data includes: the acceleration signal of the vehicle and the angular velocity signal of the vehicle.

[0050] Obtain the acceleration signal and angular velocity signal of the vehicle; integrate the acceleration signal to obtain the velocity of the vehicle, integrate the velocity to obtain the displacement of the vehicle; integrate the angular velocity signal to obtain the roll angle, pitch angle, and yaw angle of the vehicle. The acceleration signal of the vehicle in the independent three axes of the carrier coordinate system can be detected by an accelerometer. The acceleration signal includes: the motion acceleration of the vehicle itself and the gravitational acceleration. The angular velocity signal of the vehicle relative to the navigation coordinate system can be detected by a gyroscope. By measuring the angular velocity, the rotational motion of the vehicle can be obtained.

[0051] In step S2012, perform an integration operation on the acceleration signal to obtain the vehicle speed, and perform an integration operation on the speed to obtain the vehicle displacement; perform an integration operation on the angular velocity signal to obtain the vehicle's roll angle, pitch angle, and yaw angle.

[0052] The angular state quantity usually uses attitude angles, such as the pitch angle, yaw angle, and roll angle, to describe the rotation state of the vehicle relative to the northeast celestial coordinate system. The attitude angles define the direction of the vehicle, that is, how the vehicle rotates relative to the geographic coordinate system. The pitch angle is used to describe the up and down inclination of the vehicle relative to the horizontal plane. When the vehicle is completely horizontal, the pitch angle is zero. The yaw angle is used to describe the rotation degree of the vehicle relative to the north direction. When the yaw angle of the vehicle is zero, the vehicle points directly north.

[0053] In step S2013, based on the vehicle displacement, vehicle speed, roll angle, yaw angle, pitch angle, acceleration signal, and angular velocity signal, use the error state Kalman filter algorithm to obtain the vehicle's three-dimensional pose information.

[0054] In step S202, based on the angle information characterized by the inertial measurement unit data, determine the gravity plane where the vehicle pose characterized by the three-dimensional pose information is located, establish a two-dimensional map based on the gravity plane, and determine the position of the vehicle in the two-dimensional map. For details, please refer to Figure 1 step S102 of the illustrated embodiment, which will not be elaborated here.

[0055] In step S203, obtain the lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determine the position of the obstacle in the two-dimensional map based on the obstacle position information, and determine the speed of the obstacle in the two-dimensional map based on the obstacle speed information. For details, please refer to Figure 1 step S103 of the illustrated embodiment, which will not be elaborated here.

[0056] In step S204, update the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel, fit the lane line in the two-dimensional map by the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle. For details, please refer to Figure 1 step S104 of the illustrated embodiment, which will not be elaborated here.

[0057] The method for vehicle simultaneous localization and mapping provided in this embodiment. The error-state Kalman filter algorithm can make full use of the advantages of three data sources, namely IMU, GPS, and wheel speed, and can provide more accurate three-dimensional pose information. In the case of GPS signal occlusion or interference, IMU and wheel speed can be used as supplements to provide reliable positioning information, enhancing the robustness of this method in application. The high-frequency characteristics of IMU data can quickly respond to the dynamic changes of the vehicle, and the error-state Kalman filter algorithm can further smooth the IMU data to improve the stability and accuracy of positioning.

[0058] In some alternative embodiments, when using the error-state Kalman filter algorithm based on inertial measurement unit data, wheel speed data, and global positioning system data to obtain the three-dimensional pose information of the vehicle, it further includes: obtaining the position increment of the vehicle represented by the global positioning system data, and accumulating based on the position increment to obtain the three-dimensional pose information; the observation equation updated based on the global positioning system data is as follows:

[0059] P GPS = P + δP GPS

[0060] where P GPS is the position of the vehicle represented by the global positioning system data, P is the vehicle position at the current moment, and δP GPS is the difference in vehicle position between P GPS and P. It is possible to update and process the data collected on-site in real time, improving the decision-making efficiency of the vehicle's control decision-making and planning module, and to a certain extent improving the safety of autonomous vehicles.

[0061] In some alternative embodiments, when using the error-state Kalman filter algorithm based on inertial measurement unit data, wheel speed data, and global positioning system data to obtain the three-dimensional pose information of the vehicle, it further includes: obtaining the speed increment of the vehicle represented by the wheel speed data, and accumulating based on the speed increment to obtain the three-dimensional pose information;

[0062] The observation equation updated based on the wheel speed data is as follows:

[0063] V w h eel = R IG V G

[0064] where V wheel is the wheel speed obtained based on the wheel speed pulses of the vehicle, R IG is the conversion relationship of the wheel speed pulses from the body coordinate system to the north-east-down coordinate system, and V G is the vehicle body speed in the north-east-down coordinate system.

[0065] The wheel speed data provides the linear speed information of the vehicle at each moment. Through the wheel speed sensor, the rotational speed information of the vehicle tires can be obtained in real time, and then the linear speed of the vehicle can be calculated. Combining the steering information and time dimension of the vehicle, and accumulating the linear speed increments, the position and orientation changes of the vehicle in three-dimensional space can be estimated. Integration methods, such as pre-integration techniques, can be used to process the wheel speed data to obtain a more accurate and smooth vehicle pose estimation. In this way, considering the sensor noise and the non-linear characteristics of vehicle motion, the robustness and accuracy of vehicle state estimation are improved.

[0066] In some alternative embodiments, determining the position of an obstacle in a two-dimensional map based on the obstacle position information includes: determining the position of the obstacle in the two-dimensional map through the following formula:

[0067] P EN = T trans R GI P I

[0068] where P EN is the two-dimensional obstacle position in the two-dimensional map, P I is the two-dimensional obstacle position in the two-dimensional space represented by the obstacle position information, R GI is the conversion relationship of the two-dimensional obstacle position from the vehicle body coordinate system to the current three-dimensional northeast-up coordinate system where the vehicle body is located, θ1 is the pitch angle, θ0 is the roll angle, and T trans represents the mapping relationship of the obstacle position from the three-dimensional northeast-up coordinate system to the two-dimensional northeast coordinate system.

[0069] Obtain multiple lane line discrete points transmitted by the vehicle perception model. Through the conversion of the formula P EN , obtain the positions p curr_map_i_en of the multiple lane line discrete points, and calculate the Euclidean distance ε between p curr_map_i_en and the discrete points p map_i_en extracted from the two-dimensional map. The calculation method of ε is as follows:

[0070]

[0071] where R map_cur is the rotation matrix, and t map_cur is the translation matrix.

[0072] Updating the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel can be achieved through the following formula:

[0073] P map = P + δP map

[0074] Among them, P map is a deep learning perception module. Taking the image information as the input, the vehicle position characterized by the lane line information output, P is the vehicle position at the current moment, and δP map is the vehicle position difference between P map and P

[0075] The data sensed by the vehicle is the local plane corresponding to the vehicle slope at that time, and there is an angular difference between the plane where the vehicle is located and the plane of the two-dimensional map. Through the conversion of the formula P EN the position information of the obstacle in the two-dimensional map can be obtained.

[0076] In some alternative embodiments, determining the speed of an obstacle in a two-dimensional map based on the speed information of the obstacle includes: determining the speed of the obstacle in the two-dimensional map by the following formula:

[0077] V EN = T trans R GI V I

[0078] Among them, V EN is the speed of the obstacle in the two-dimensional map, and V I is the speed of the obstacle in the three-dimensional space characterized by the speed information of the obstacle.

[0079] In some alternative embodiments, fitting a lane line in a two-dimensional map by the least squares method includes:

[0080] Constructing a lane line fitting function, the lane line fitting function is a third-order polynomial; constructing an error function based on the error between the fitting value and the observed value of the lane line fitting function, solving the minimum value of the error function, and obtaining the coefficients of each term in the lane line fitting function; obtaining the fitted lane line in the two-dimensional map based on the coefficients of each term.

[0081] The lane line characterized by the lane line information can be represented based on the lane line fitting function f(x).

[0082] f(x)= w0 + w1x + w2x 2 + w3x 3

[0083] w0, w1, w2, and w3 are polynomial coefficients, and x is the abscissa of the point on the lane line. Calculate the error between the polynomial fitting value and the observed value based on the error function E(w); the position of the lane line includes the data point set {(x1, y1), (x2, y2),..., (x n , y n )}, where y i where i ∈ (1, n) in is the ordinate of the lane line in the data point set, and the error function E(w) is as follows:

[0084]

[0085] The coefficients w0, w1, w2, and w3 that minimize the error function E(w) can be calculated by the least squares method. Functions in Matlab, such as polyfit, can be used to directly solve the linear equations to obtain the optimal solutions of each polynomial coefficient. Substituting the calculated w0, w1, w2, and w3 into f(x) gives the equation for fitting the lane line, and the lane line fitting function can be plotted in a two-dimensional map to represent the lane line.

[0086] Lane line fitting by the least squares method has a relatively simple calculation process, and the calculation results conform to the lane line shape, with strong practicability.

[0087] In this embodiment, a device for vehicle simultaneous localization and mapping is also provided. This device is used to implement the above embodiments and preferred implementation manners, and those that have been described will not be repeated. As used hereinafter, the term "module" can be a combination of software and / or hardware that realizes a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, implementation in hardware, or a combination of software and hardware is also possible and contemplated.

[0088] Figure 3 The structural schematic diagram of the device for vehicle simultaneous localization and mapping according to the embodiment of the present invention is shown. This embodiment provides a device for vehicle simultaneous localization and mapping, as Figure 3 shown, including:

[0089] A data acquisition module 501, configured to acquire inertial measurement unit data, wheel speed data, and global positioning system data of the vehicle, and based on the inertial measurement unit data, wheel speed data, and global positioning system data, use the error state Kalman filter algorithm to obtain the three-dimensional pose information of the vehicle.

[0090] A first positioning module 502, configured to determine the gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establish a two-dimensional map based on the gravity plane, and determine the position of the vehicle in the two-dimensional map.

[0091] A second positioning module 503, configured to acquire lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determine the position of the obstacle in the two-dimensional map based on the obstacle position information, and determine the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle.

[0092] The map building module 504 is used to update the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel, and fit the lane line in the two-dimensional map by the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

[0093] In some alternative embodiments, the data acquisition module 501 includes:

[0094] The first data acquisition unit is used to obtain the angle increment of the vehicle characterized by the inertial measurement unit data, and obtain the three-dimensional pose information based on the accumulation of the angle increment; wherein, the inertial measurement unit data includes: the acceleration signal of the vehicle and the angular velocity signal of the vehicle.

[0095] The second data acquisition unit is used to perform an integration operation on the acceleration signal to obtain the speed of the vehicle, perform an integration operation on the speed to obtain the displacement of the vehicle; perform an integration operation on the angular velocity signal to obtain the roll angle, pitch angle, and yaw angle of the vehicle.

[0096] The third data acquisition unit is used to obtain the three-dimensional pose information of the vehicle by using the error state Kalman filter algorithm based on the displacement of the vehicle, the speed of the vehicle, the roll angle, the yaw angle, the pitch angle, the acceleration signal, and the angular velocity signal.

[0097] In some alternative embodiments, the data acquisition module 501 further includes:

[0098] The fourth data acquisition unit is used to obtain the three-dimensional pose information of the vehicle by using the error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data, and further includes:

[0099] Obtain the position increment of the vehicle characterized by the global positioning system data, and obtain the three-dimensional pose information based on the accumulation of the position increment;

[0100] The observation equation updated based on the global positioning system data is as follows:

[0101] P GPS = P + δP GPS

[0102] wherein, P GPS is the position of the vehicle characterized by the global positioning system data, P is the vehicle position at the current moment, and δP GPS is the vehicle position difference between P GPS and P.

[0103] In some alternative embodiments, the data acquisition module 501 further includes:

[0104] The fifth unit for data acquisition, which is used to obtain the three-dimensional pose information of the vehicle by using the error state Kalman filter algorithm based on inertial measurement unit data, wheel speed data, and global positioning system data, further includes: obtaining the speed increment of the vehicle represented by the wheel speed data, and obtaining the three-dimensional pose information based on the accumulation of the speed increment; the observation equation updated based on the wheel speed data is as follows:

[0105] V wheel = R IG V G

[0106] where V wheel is the wheel speed obtained based on the wheel speed pulses of the vehicle, R IG is the conversion relationship of the wheel speed pulses from the vehicle body coordinate system to the north-east-down coordinate system, and V G is the vehicle body speed in the north-east-down coordinate system.

[0107] In some alternative embodiments, the second positioning module 503 includes:

[0108] The first unit of the second positioning, which is used to determine the position of the obstacle in the two-dimensional map based on the obstacle position information, includes: determining the position of the obstacle in the two-dimensional map through the following formula:

[0109] P EN = T trans R GI P I

[0110] where P EN is the two-dimensional obstacle position in the two-dimensional map, P I is the two-dimensional obstacle position in the two-dimensional space represented by the obstacle position information, R GI is the conversion relationship of the two-dimensional obstacle position from the vehicle body coordinate system to the current three-dimensional north-east-down coordinate system where the vehicle body is located, θ1 is the pitch angle, θ0 is the roll angle, and T trans represents the mapping relationship of the obstacle position from the three-dimensional north-east-down coordinate system to the two-dimensional north-east coordinate system.

[0111] In some alternative embodiments, the second positioning module 503 further includes:

[0112] The second unit of the second positioning, which is used to determine the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle, includes: determining the speed of the obstacle in the two-dimensional map through the following formula:

[0113] V EN = T trans R GI V I

[0114] Among them, V EN is the speed of the obstacle in the two-dimensional map, and V I is the speed of the obstacle in the three-dimensional space represented by the speed information of the obstacle.

[0115] In some optional embodiments, the mapping module 504 includes:

[0116] The first mapping unit is used to fit the lane line in the two-dimensional map by the least squares method, including: constructing a lane line fitting function, where the lane line fitting function is a third-order polynomial; constructing an error function based on the error between the fitting value and the observed value of the lane line fitting function, solving the minimum value of the error function to obtain the coefficients of each term in the lane line fitting function; and obtaining the fitted lane line in the two-dimensional map based on the coefficients of each term.

[0117] The device for vehicle simultaneous localization and mapping provided in this embodiment is when the vehicle is running in a high-speed section scenario and facing undulating road conditions. By using the IMU, GPS, and wheel speed sensors, a positioning result that meets the positioning requirements of an autonomous vehicle can be obtained, which can effectively save the positioning cost; based on the transformation of the lane line information during the vehicle's travel, the three-dimensional pose information can be updated, which can improve the local objectivity of the created two-dimensional map; the obtained two-dimensional map including the vehicle position can reduce the cost of using data by the vehicle's control decision-making and planning module.

[0118] The further function descriptions of the above-mentioned various modules and units are the same as those in the corresponding embodiments above, and will not be repeated here.

[0119] The device for vehicle simultaneous localization and mapping in this embodiment is presented in the form of functional units. Here, the unit refers to an Application Specific Integrated Circuit (ASIC) circuit, a processor and a memory that execute one or more software or fixed programs, and / or other devices that can provide the above functions.

[0120] The embodiment of the present invention also provides a computer device having the above Figure 3 shown device for vehicle simultaneous localization and mapping.

[0121] Please refer to Figure 4 , Figure 4 which is a schematic structural diagram of a computer device provided by an optional embodiment of the present invention. As shown in Figure 4As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including a high-speed interface and a low-speed interface. Each component communicates with each other using different buses and can be installed on a common motherboard or in other ways as needed. The processor can process instructions executed within the computer device, including instructions stored in the memory or on the memory to display graphical information of a graphical user interface on an external input / output device (such as a display device coupled to the interface). In some alternative embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (such as an array of servers, a set of blade servers, or a multi-processor system). Figure 4 Take one processor 10 as an example in Figure 4 .

[0122] The processor 10 can be a central processing unit, a network processor, or a combination thereof. Among them, the processor 10 can further include a hardware chip. The above hardware chip can be an application-specific integrated circuit, a programmable logic device, or a combination thereof. The above programmable logic device can be a complex programmable logic device, a field programmable gate array, a generic array logic, or any combination thereof.

[0123] Among them, the aforementioned memory 20 stores instructions executable by at least one processor 10, so that the at least one processor 10 executes the method shown in the above embodiments.

[0124] The memory 20 can include a program storage area and a data storage area. Among them, the program storage area can store an operating system and application programs required for at least one function; the data storage area can store data created according to the use of the computer device. In addition, the memory 20 can include a high-speed random access memory and can also include a non-transitory memory, such as at least one disk storage device, a flash memory device, or other non-transitory solid-state storage devices. In some alternative embodiments, the memory 20 can optionally include a memory remotely set relative to the processor 10, and these remote memories can be connected to the computer device through a network. Examples of the above network include but are not limited to the Internet, an enterprise intranet, a local area network, a mobile communication network, and combinations thereof.

[0125] The memory 20 can include a volatile memory, such as a random access memory; the memory can also include a non-volatile memory, such as a flash memory, a hard disk, or a solid-state drive; the memory 20 can also include a combination of the above types of memories.

[0126] The computer device further includes an input device 30 and an output device 40. The processor 10, the memory 20, the input device 30, and the output device 40 may be connected through a bus or other means. Figure 4 Take the connection through the bus as an example.

[0127] The input device 30 can receive input digital or character information and generate key signal inputs related to the user settings and function controls of the computer device, such as a touch screen, a keypad, a mouse, a trackpad, a touchpad, a pointing stick, one or more mouse buttons, a trackball, a joystick, etc. The output device 40 may include a display device, an auxiliary lighting device (such as a light-emitting diode), and a tactile feedback device (such as a vibration motor), etc. The above display device includes, but is not limited to, a liquid crystal display, a light-emitting diode, a display, and a plasma display. In some alternative embodiments, the display device may be a touch screen.

[0128] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or be implemented as computer code that can be recorded on a storage medium, or be implemented as computer code that is originally stored in a remote storage medium or a non-transitory machine-readable storage medium and downloaded through a network and will be stored in a local storage medium, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory, a random access memory, a flash memory, a hard disk, or a solid-state drive, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by the computer, the processor, or the hardware, the method shown in the above embodiment is implemented.

[0129] A part of the present invention can be applied as a computer program product, such as computer program instructions. When executed by a computer, through the operation of the computer, the method and / or technical solution according to the present invention can be called or provided. Those skilled in the art should be able to understand that the forms of existence of computer program instructions in a computer-readable medium include, but are not limited to, source files, executable files, installation package files, etc. Correspondingly, the ways in which computer program instructions are executed by a computer include, but are not limited to: the computer directly executes the instructions, or the computer compiles the instructions and then executes the corresponding compiled program, or the computer reads and executes the instructions, or the computer reads and installs the instructions and then executes the corresponding installed program. Here, the computer-readable medium can be any available computer-readable storage medium or communication medium accessible to the computer.

[0130] Although embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations fall within the scope defined by the appended claims.

Claims

1. A method for vehicle simultaneous localization and mapping, characterized in that, The method includes: Obtaining inertial measurement unit data, wheel speed data, and global positioning system data of a vehicle, and using an error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain three-dimensional pose information of the vehicle; Determining a gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establishing a two-dimensional map based on the gravity plane, and determining the position of the vehicle in the two-dimensional map; Obtaining lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determining the position of the obstacle in the two-dimensional map based on the obstacle position information, and determining the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle; Updating the three-dimensional pose information based on the transformation of the lane line information during the vehicle's travel, and fitting the lane line in the two-dimensional map by the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

2. The method according to claim 1, characterized in that, Using an error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain three-dimensional pose information of the vehicle includes: Obtaining the angle increment of the vehicle represented by the inertial measurement unit data, and obtaining the three-dimensional pose information based on the accumulation of the angle increment; Wherein, the inertial measurement unit data includes: the acceleration signal of the vehicle and the angular velocity signal of the vehicle. Performing an integration operation on the acceleration signal to obtain the speed of the vehicle, and performing an integration operation on the speed to obtain the displacement of the vehicle; performing an integration operation on the angular velocity signal to obtain the roll angle, pitch angle, and yaw angle of the vehicle.

3. The method according to claim 1 or 2, characterized in that, Using an error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain three-dimensional pose information of the vehicle further includes: Obtaining the position increment of the vehicle represented by the global positioning system data, and obtaining the three-dimensional pose information based on the accumulation of the position increment; The observation equation updated based on the global positioning system data is as follows: P GPS = P + δP GPS where P GPS is the position of the vehicle represented by the global positioning system data, P is the vehicle position at the current moment, and δP GPS is the difference in vehicle position between P GPS and P.

4. The method according to claim 3, wherein Using an error state Kalman filter algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain three-dimensional pose information of the vehicle further includes: Obtaining the speed increment of the vehicle represented by the wheel speed data, and obtaining the three-dimensional pose information based on the accumulation of the speed increment; The observation equation updated based on the wheel speed data is as follows: V wheel = R IG V G Among them, V wheel is the wheel speed obtained based on the wheel speed pulses of the vehicle, and R IG is the conversion relationship of the wheel speed pulses from the vehicle body coordinate system to the north-east-up coordinate system, and V G is the vehicle body speed in the north-east-up coordinate system.

5. The method according to claim 2, wherein Determining the position of the obstacle in the two-dimensional map based on the obstacle position information includes: Determining the position of the obstacle in the two-dimensional map through the following formula: P EN = T trans R GI P I where, P EN is the two-dimensional obstacle position in the two-dimensional map, and P I is the two-dimensional obstacle position in the two-dimensional space represented by the obstacle position information. R GI is the conversion relationship of the two-dimensional obstacle position from the vehicle body coordinate system to the current three-dimensional northeast-up coordinate system where the vehicle body is located. θ1 is the pitch angle, θ0 is the roll angle, and T trans represents the mapping relationship of the obstacle position from the three-dimensional northeast-up coordinate system to the two-dimensional northeast coordinate system.

6. The method according to claim 5, wherein Determining the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle includes: Determining the speed of the obstacle in the two-dimensional map through the following formula: V EN = T trans R GI V I Among them, V EN is the speed of the obstacle in the two-dimensional map, and V I is the speed of the obstacle in the three-dimensional space characterized by the speed information of the obstacle.

7. The method according to claim 5, characterized in that, Fitting the lane line in the two-dimensional map by the least squares method includes: Construct a lane line fitting function, where the lane line fitting function is a third-order polynomial; Construct an error function based on the error between the fitting value and the observed value of the lane line fitting function, and solve the minimum value of the error function to obtain the coefficients of each term in the lane line fitting function; Obtain the fitted lane line in the two-dimensional map based on the coefficients of each term.

8. A device for vehicle simultaneous localization and mapping, characterized in that, The device includes: A data acquisition module, configured to acquire the inertial measurement unit data, wheel speed data, and global positioning system data of the vehicle, and use the error state Kalman filtering algorithm based on the inertial measurement unit data, the wheel speed data, and the global positioning system data to obtain the three-dimensional pose information of the vehicle; A first positioning module, configured to determine the gravity plane where the vehicle pose represented by the three-dimensional pose information is located based on the angle information represented by the inertial measurement unit data, establish a two-dimensional map based on the gravity plane, and determine the position of the vehicle in the two-dimensional map; A second positioning module, configured to acquire the lane line information, obstacle position information, and obstacle speed information sensed by the vehicle, determine the position of the obstacle in the two-dimensional map based on the obstacle position information, and determine the speed of the obstacle in the two-dimensional map based on the speed information of the obstacle; A mapping module, configured to update the three-dimensional pose information based on the change of the lane line information during the vehicle's travel, and fit the lane line in the two-dimensional map by the least squares method. The two-dimensional map includes: the fitted lane line, the position of the vehicle, the position of the obstacle, and the speed of the obstacle.

9. A computer device, characterized in that, Including: A memory and a processor, which are communicatively connected to each other. The memory stores computer instructions, and the processor executes the computer instructions to execute the method for vehicle simultaneous localization and mapping according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, Computer instructions are stored on the computer-readable storage medium, and the computer instructions are used to cause a computer to execute the method for vehicle simultaneous localization and mapping according to any one of claims 1 to 7.

Images