Method, device and equipment for vehicle synchronous positioning and mapping

Abstract

The invention relates to the technical field of automatic driving, and discloses a method, device and equipment for vehicle synchronous positioning and mapping, and the method comprises the steps: obtaining inertial measurement unit data, wheel speed data and global positioning system data of a vehicle, based on the inertial measurement unit data, the wheel speed data and the global positioning system data, adopting an error state Kalman filtering algorithm 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; 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; and updating three-dimensional pose information based on the change of lane line information in the vehicle traveling process, and fitting lane lines in a two-dimensional map through a least square method.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method, device and equipment for simultaneous vehicle positioning and mapping. Background Art

[0002] Synchronous positioning and mapping is a key step in vehicle autonomous driving. It allows the vehicle to perceive its position and surrounding environment during operation, so that the vehicle's decision 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 positioning and mapping is an indispensable part of the vehicle's autonomous driving task, especially when the vehicle is running on a highway and facing ups and downs in road conditions.

[0003] In order to cope with road conditions with long-distance slope changes, the relevant technology uses a combination of carrier phase differential technology, cameras, lidar and high-precision maps to create a three-dimensional obstacle map, which is too costly. Summary of the invention

[0004] In view of this, the present invention provides a method, device and equipment for vehicle synchronous positioning and mapping to solve the problem that the related technology uses a combination of carrier phase difference technology, cameras, lidar and high-precision maps to create a three-dimensional obstacle map, but the cost is too high when the autonomous driving vehicle is in a high-speed section under road conditions with long-distance slope changes.

[0005] In a first aspect, the present invention provides a method for synchronous positioning and mapping of a vehicle, the method comprising: acquiring 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, the wheel speed data and the global positioning system data to obtain three-dimensional posture information of the vehicle; determining a gravity plane where the vehicle posture represented by the three-dimensional posture information is located based on 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; acquiring lane line information, obstacle position information and obstacle speed information perceived 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 posture information based on the transformation of the lane line information during the movement of the vehicle, fitting lane lines in the two-dimensional map by the least squares method, and the two-dimensional map including: the fitted lane lines, the position of the vehicle, the position of the obstacle and the speed of the obstacle.

[0006] In an optional 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 posture information of the vehicle, including: obtaining the angle increment of the vehicle represented by the inertial measurement unit data, and obtaining the three-dimensional posture information based on the accumulation of the angle increment; wherein the inertial measurement unit data includes: an acceleration signal of the vehicle and an angular velocity signal of the vehicle, integrating the acceleration signal to obtain the speed of the vehicle, integrating the speed to obtain the displacement of the vehicle; integrating the angular velocity signal to obtain the roll angle, pitch angle and yaw angle of the vehicle.

[0007] In an optional implementation, the three-dimensional posture information of the vehicle is obtained 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, and further includes: obtaining a position increment of the vehicle represented by the global positioning system data, and obtaining the three-dimensional posture information based on the accumulation of the position increments; and the observation equation updated based on the global positioning system data is as follows:

[0008] P GPS =P+δP GPS

[0009] Among them, P GPS is the position of the vehicle represented by the GPS data, P is the vehicle position at the current moment, δP GPS P GPS The difference in vehicle position between and P.

[0010] In an optional implementation, the three-dimensional posture information of the vehicle is obtained 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, further comprising: obtaining a speed increment of the vehicle represented by the wheel speed data, and obtaining the three-dimensional posture information based on the accumulation of the speed increments; and the observation equation updated based on the wheel speed data is as follows:

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

[0012] Among them, h w P eel is the wheel speed obtained based on the wheel speed pulse of the vehicle, P IG is the conversion relationship between the wheel speed pulse from the vehicle body coordinate system to the northeast sky coordinate system, V G is the vehicle speed in the northeast celestial coordinate system.

[0013] In an optional implementation, 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 by the following formula:

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

[0015] Among them, 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 transformation relationship of the two-dimensional obstacle position from the vehicle coordinate system to the three-dimensional northeast celestial coordinate system where the current vehicle is located. θ1 is the pitch angle, θ0 is the roll angle, T trans It represents the mapping relationship of the obstacle position from the three-dimensional northeast celestial coordinate system to the two-dimensional northeast coordinate system.

[0016] In an optional implementation, 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 by the following formula:

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

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

[0019] In an optional embodiment, fitting lane lines in the two-dimensional map by the least squares method includes: constructing a lane line fitting function, wherein 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 coefficients of each term in the lane line fitting function; and obtaining the fitted lane lines in the two-dimensional map based on the coefficients of each term.

[0020] In a second aspect, the present invention provides a device for synchronous positioning and mapping of a vehicle, the device comprising: a data acquisition module, used to acquire 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, the wheel speed data and the global positioning system data to obtain three-dimensional posture information of the vehicle; a first positioning module, used to determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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, used to acquire lane line information, obstacle position information and obstacle speed information perceived 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, used to update the three-dimensional posture information based on the transformation of the lane line information during the movement of the vehicle, and fit the lane line in the two-dimensional map by the least squares method, and 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] In a third aspect, the present invention provides a computer device, comprising: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the method for synchronous vehicle positioning and mapping of the above-mentioned first aspect or any corresponding embodiment thereof by executing the computer instructions.

[0022] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to enable a computer to execute the method for simultaneous vehicle positioning and mapping of the first aspect or any corresponding embodiment thereof.

[0023] The method for simultaneous vehicle positioning and mapping provided in this embodiment is that when the vehicle is running on a high-speed road section and facing ups and downs in road conditions, a positioning result that meets the positioning requirements of the autonomous driving vehicle is obtained through IMU, GPS and wheel speed sensors, which can effectively save positioning costs; the three-dimensional posture information is updated based on the changes in lane line information during the vehicle's travel, which can improve the local objectivity of the created two-dimensional map; the obtained two-dimensional map including the vehicle's position can reduce the cost of using data in the vehicle's control decision planning module. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the related technologies, the drawings required for use in the specific embodiments or the related technical descriptions will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0025] Figure 1 A schematic diagram of a process for a method for synchronous positioning and mapping of a vehicle provided by an embodiment of the present invention is shown;

[0026] Figure 2 A schematic flow chart of a method for synchronous positioning and mapping of a vehicle according to an embodiment of the present invention is shown;

[0027] Figure 3 A schematic diagram showing the structure of a device for synchronous positioning and mapping of a vehicle according to an embodiment of the present invention is shown;

[0028] Figure 4 It is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0029] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0030] The synchronous positioning and mapping process in the related technology includes: using visual feature points, laser radar matching, matching with high-precision maps or using carrier phase difference technology for three-dimensional positioning to obtain a three-dimensional position; converting the three-dimensional obstacle information sensed by the vehicle through the three-dimensional position in the above steps to create a three-dimensional obstacle map; passing the three-dimensional vehicle information and obstacle information to the vehicle's control decision planning module. The related technology has the technical problem that the cost is too high and the three-dimensional information cannot be directly used by the vehicle's control decision planning module.

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

[0032] In this embodiment, a method for synchronous positioning and mapping of a vehicle is provided, which can be used for the above-mentioned autonomous driving vehicle. Figure 1 A schematic diagram of a process for synchronous vehicle positioning and mapping provided by an embodiment of the present invention is shown, such as Figure 1 As shown, the process includes the following steps:

[0033] Step S101, obtaining the vehicle's inertial measurement unit (IMU) data, wheel speed data and global positioning system (GPS) data, and using an error state Kalman filter algorithm based on the inertial measurement unit data, wheel speed data and GPS data to obtain the vehicle's three-dimensional posture information.

[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 the vehicle's latitude, longitude, altitude, speed and other positioning information. After the vehicle obtains GPS data for the first time, the origin of the east north up coordinate system (east north up) map can be obtained.

[0035] The error state Kalman filter algorithm is used to define the error state vector, including position error, velocity error, angle error, acceleration error and angular velocity error. The change of state is predicted based on IMU data, the change of velocity and position is calculated based on accelerometer data, and the change of angle is calculated based on gyroscope data.

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

[0037] Step S102, based on the angle information represented by the inertial measurement unit data, determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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] The vehicle's attitude relative to the gravity plane can be determined based on the pitch angle and yaw angle. If the pitch angle and yaw angle are both small, the vehicle is roughly located on the gravity plane. If the pitch angle or yaw angle is large, the vehicle has a certain tilt or rotation relative to the gravity plane.

[0039] The accelerometer data contains the acceleration of the vehicle in each axis, including the component of gravity acceleration. The accelerometer data can be used to calculate the vehicle's attitude angle, namely the pitch angle and roll angle. The pitch angle and roll angle describe the degree of inclination of the vehicle relative to the horizontal plane. According to the attitude angle, a plane perpendicular to the direction of gravity can be determined. The gravity plane is a plane perpendicular to the direction of gravity, usually approximately parallel to the surface of the earth. The vehicle's height information can be ignored when projecting the vehicle's position in three-dimensional space onto a two-dimensional map. A two-dimensional coordinate system is 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 longitude, latitude, and altitude of the vehicle's three-dimensional position are projected onto the gravity plane to obtain the vehicle's two-dimensional coordinates.

[0040] Step S103, obtaining lane line information, obstacle position information and obstacle speed information perceived 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 obstacle speed information.

[0041] In this step, the vehicle's perception system, such as hardware such as radar or a vehicle-mounted camera, or computer vision technology, can be used to acquire and process information such as the position and speed of lane lines or obstacles in the traffic environment. The obstacles can be vehicles or pedestrians.

[0042] Step S104, updating the three-dimensional pose information based on the transformation of the lane line information during the vehicle's movement, fitting the lane line in the two-dimensional map by the least squares method, and 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] A number of discrete points sensed during the vehicle's movement are obtained, and based on the position transformation of the discrete points in the two-dimensional map, a rotation matrix and a translation matrix representing the position transformation relationship are obtained, and the three-dimensional posture information of the vehicle is updated based on the rotation matrix and the translation matrix.

[0044] The method for simultaneous vehicle positioning and mapping provided in this embodiment is that when the vehicle is running on a high-speed road section and facing ups and downs in road conditions, a positioning result that meets the positioning requirements of the autonomous driving vehicle is obtained through IMU, GPS and wheel speed sensors, which can effectively save positioning costs; the three-dimensional posture information is updated based on the changes in lane line information during the vehicle's travel, which can improve the local objectivity of the created two-dimensional map; the obtained two-dimensional map including the vehicle's position can reduce the cost of using data in the vehicle's control decision planning module.

[0045] In this embodiment, a method for synchronous positioning and mapping of a vehicle is provided, which can be used for various types of vehicles. Figure 2 A schematic diagram of a process for synchronous vehicle positioning and mapping according to an embodiment of the present invention is shown. Figure 2 As shown, the process includes the following steps:

[0046] Step S201, acquiring the inertial measurement unit data, wheel speed data and global positioning system data of the vehicle, and using 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 posture information of the vehicle.

[0047] The vehicle's position and posture can be calculated using the 15-dimensional state quantity Among them, δp T is the position offset, δv T is the velocity bias, δθ T is the angle offset, is the acceleration bias, is the angular velocity bias.

[0048] Specifically, the above step S201 includes:

[0049] Step S2011, obtaining the angle increment of the vehicle represented by the inertial measurement unit data, and obtaining the three-dimensional posture 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.

[0050] Obtain the acceleration signal and angular velocity signal of the vehicle; integrate the acceleration signal to obtain the vehicle's speed, integrate the speed to obtain the vehicle's displacement; integrate the angular velocity signal to obtain the vehicle's roll angle, pitch angle, and yaw angle. The acceleration signal of the vehicle in the independent three-axis of the carrier coordinate system can be detected by the accelerometer. The acceleration signal includes: the vehicle's own motion acceleration and gravity acceleration. The angular velocity signal of the vehicle relative to the navigation coordinate system can be detected by the gyroscope, and the rotational motion of the vehicle can be obtained by measuring the angular velocity.

[0051] Step S2012, integrating the acceleration signal to obtain the speed of the vehicle, integrating the speed to obtain the displacement of the vehicle; integrating the angular velocity signal to obtain the roll angle, pitch angle and yaw angle of the vehicle.

[0052] Angular state quantities usually use attitude angles, such as pitch, yaw, and roll, to describe the rotation state of the vehicle relative to the northeast sky coordinate system. The attitude angle defines 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 degree of 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 degree of rotation of the vehicle relative to the north direction. When the yaw angle of the vehicle is zero, the vehicle is pointing to the due north direction.

[0053] Step S2013, based on the vehicle's displacement, vehicle speed, roll angle, yaw angle, pitch angle, acceleration signal and angular velocity signal, an error state Kalman filter algorithm is used to obtain the vehicle's three-dimensional posture information.

[0054] Step S202: Based on the angle information represented by the inertial measurement unit data, determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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 will not be described in detail here.

[0055] Step S203, obtaining lane line information, obstacle position information and obstacle speed information perceived 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 obstacle speed information. Figure 1 Step S103 of the illustrated embodiment will not be described in detail here.

[0056] Step S204: Update the 3D position information based on the changes in lane information during the vehicle's movement, and fit the lane in the 2D map using the least squares method. The 2D map includes: the fitted lane, the vehicle's position, the obstacle's position, and the obstacle's speed. Figure 1 Step S104 of the illustrated embodiment will not be described in detail here.

[0057] The method for simultaneous vehicle positioning and mapping provided in this embodiment, the error state Kalman filter algorithm can make full use of the advantages of the three data sources of IMU, GPS and wheel speed, and can provide more accurate three-dimensional posture information; when the GPS signal is blocked or interfered, the IMU and wheel speed can serve as supplements to provide reliable positioning information, thereby enhancing the robustness of the applicability of the method; the high-frequency characteristics of the IMU data can respond quickly 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 optional embodiments, the three-dimensional position information of the vehicle is obtained 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, and further includes: obtaining the position increment of the vehicle represented by the global positioning system data, and obtaining the three-dimensional position information based on the accumulation of the position increment; the observation equation updated based on the global positioning system data is as follows:

[0059] P GPS =P+δP GPS

[0060] Among them, P GPS is the vehicle position represented by the GPS data, P is the vehicle position at the current moment, δP GPS P GPS The difference in vehicle position between A and P. The data collected on site can be updated and processed in real time, which improves the decision-making efficiency of the vehicle's control decision-making planning module and improves the safety of autonomous driving vehicles to a certain extent.

[0061] In some optional embodiments, obtaining the three-dimensional position 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 further includes: obtaining a speed increment of the vehicle represented by the wheel speed data, and obtaining the three-dimensional position information based on the accumulation of the speed increments;

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

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

[0064] Among them, V wheel is the wheel speed obtained based on the vehicle's wheel speed pulse, R IG is the conversion relationship between the wheel speed pulse from the vehicle body coordinate system to the northeast sky coordinate system, V G is the vehicle speed in the northeast celestial coordinate system.

[0065] Wheel speed data provides the linear speed information of the vehicle at each moment. The wheel speed sensor can obtain the rotation speed information of the vehicle tire in real time, and then calculate the linear speed of the vehicle. Combined with the steering information and time dimension of the vehicle, the linear speed increment is accumulated to estimate the position and direction change of the vehicle in three-dimensional space. An integration method, such as pre-integration technology, can be used to process the wheel speed data to obtain a more accurate and smooth vehicle posture estimate. In this way, the nonlinear characteristics of sensor noise and vehicle motion are taken into account, and the robustness and accuracy of vehicle state estimation are improved.

[0066] In some optional implementations, 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 by the following formula:

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

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

[0069] Get multiple lane line discrete points transmitted by the vehicle perception model, and use formula P EN The position p of multiple lane line discrete points is obtained by transformation curr_map_i_en , calculate p curr_map_i_en and the discrete point p extracted from the two-dimensional map map_i_en The Euclidean distance ε between , ε is calculated as follows:

[0070]

[0071] Among them, R map_cur is the rotation matrix, t map_cur is the translation matrix.

[0072] Based on the transformation of lane line information during vehicle movement, the three-dimensional pose information can be updated using the following formula:

[0073] P map =P+δP map

[0074] Among them, P map It is a deep learning perception module that takes image information as input and outputs the vehicle position represented by lane line information. P is the vehicle position at the current moment, and δP map P map The difference in vehicle position between and P.

[0075] The data perceived by the vehicle is the local plane corresponding to the vehicle slope at that time, which has an angle difference with the plane where the two-dimensional map is located. EN By converting , we can get the location information of obstacles in the two-dimensional map.

[0076] In some optional implementations, 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 by the following formula:

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

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

[0079] In some optional implementations, fitting lane lines in a two-dimensional map by a least squares method includes:

[0080] A lane line fitting function is constructed, which is a third-order polynomial. An error function is constructed based on the error between the fitting value and the observed value of the lane line fitting function, and the minimum value of the error function is solved to obtain the coefficients of each term in the lane line fitting function. Based on the coefficients of each term, the fitted lane line in the two-dimensional map is obtained.

[0081] The lane line represented 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 horizontal coordinate of the point on the lane line. The error between the polynomial fitting value and the observed value is calculated 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 )},y i where i∈(1, n) 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 using the least squares method. Functions in MATLAB, such as polyfit, can be used to directly solve the linear equations to obtain the optimal solution for each polynomial coefficient. The calculated w0, w1, w2 and w3 are substituted into f(x) to obtain the equation for fitting the lane line. The lane line can be represented by drawing the lane line fitting function on a two-dimensional map.

[0086] The lane line is fitted by the least square method. The calculation process is relatively simple, and the calculation result fits the lane line shape and is highly practical.

[0087] In this embodiment, a device for synchronous positioning and mapping of vehicles is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and will not be repeated hereafter. As used below, the term "module" may be a combination of software and / or hardware that implements a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0088] Figure 3 FIG. 1 is a schematic diagram showing a structure of a device for synchronous positioning and mapping of a vehicle according to an embodiment of the present invention. This embodiment provides a device for synchronous positioning and mapping of a vehicle, such as Figure 3 As shown, including:

[0089] The data acquisition module 501 is used to acquire the inertial measurement unit data, wheel speed data and global positioning system data of the vehicle, and obtain the three-dimensional position information of the vehicle based on the inertial measurement unit data, wheel speed data and global positioning system data using the error state Kalman filter algorithm.

[0090] The first positioning module 502 is used to determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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] The second positioning module 503 is used to obtain lane line information, obstacle position information and obstacle speed information perceived 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.

[0092] The mapping module 504 is used to update the three-dimensional posture information based on the transformation of the lane line information during the vehicle's movement, and fit the lane line in the two-dimensional map through 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 optional implementations, the data acquisition module 501 includes:

[0094] The first data acquisition unit is used to acquire the angle increment of the vehicle represented by the inertial measurement unit data, and obtain the three-dimensional posture 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 integrate the acceleration signal to obtain the speed of the vehicle, integrate the speed 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.

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

[0097] In some optional implementations, the data acquisition module 501 further includes:

[0098] The fourth data acquisition unit is used to obtain the three-dimensional position 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 also includes:

[0099] Obtaining a position increment of the vehicle represented by global positioning system data, and obtaining three-dimensional posture information based on the accumulation of the position increments;

[0100] The observation equation updated based on GPS data is as follows:

[0101] P GPS =P+δP GPS

[0102] Among them, P GPS is the vehicle position represented by the GPS data, P is the vehicle position at the current moment, δP GPS P GPS The difference in vehicle position between and P.

[0103] In some optional implementations, the data acquisition module 501 further includes:

[0104] The fifth data acquisition unit is used to obtain the three-dimensional posture 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: obtaining the speed increment of the vehicle represented by the wheel speed data, and obtaining the three-dimensional posture 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] Among them, V wheel is the wheel speed obtained based on the vehicle's wheel speed pulse, R IG is the conversion relationship between the wheel speed pulse from the vehicle body coordinate system to the northeast sky coordinate system, V G is the vehicle speed in the northeast celestial coordinate system.

[0107] In some optional implementations, the second positioning module 503 includes:

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

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

[0110] Among them, P EN is the 2D obstacle position in the 2D map, P I is the two-dimensional obstacle position in the two-dimensional space represented by the obstacle position information, R GI is the transformation relationship of the two-dimensional obstacle position from the vehicle coordinate system to the three-dimensional northeast celestial coordinate system where the current vehicle is located. θ1 is the pitch angle, θ0 is the roll angle, T trans It represents the mapping relationship of the obstacle position from the three-dimensional northeast celestial coordinate system to the two-dimensional northeast coordinate system.

[0111] In some optional implementations, the second positioning module 503 further includes:

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

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

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

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

[0116] The first mapping unit is used to fit lane lines in a two-dimensional map by the least squares method, including: 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 observation 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; and obtaining the fitted lane line in the two-dimensional map based on the coefficients of each term.

[0117] The device for simultaneous vehicle positioning and mapping provided in this embodiment is a device for obtaining positioning results that meet the positioning requirements of the autonomous driving vehicle through IMU, GPS and wheel speed sensors when the vehicle is running on a high-speed road section and facing ups and downs in road conditions, which can effectively save positioning costs; the three-dimensional posture information is updated based on the changes in lane line information during the vehicle's travel, which can improve the local objectivity of the created two-dimensional map; the obtained two-dimensional map including the vehicle's position can reduce the cost of using data in the vehicle's control decision planning module.

[0118] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0119] The device for simultaneous vehicle positioning and mapping in this embodiment is presented in the form of a functional unit, where the unit refers to an application specific integrated circuit (ASIC) circuit, a processor and memory that executes 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 The device shown is used for simultaneous vehicle positioning and mapping.

[0121] See also Figure 4 , Figure 4 is a schematic diagram of the structure of a computer device provided by an optional embodiment of the present invention, such as Figure 4As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process instructions executed in the computer device, including instructions stored in 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 an interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 4 A processor 10 is taken as an example.

[0122] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

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

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

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

[0126] The computer device also 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 via a bus or other means. Figure 4 The example of connecting through bus is taken in the following.

[0127] The input device 30 can receive input digital or character information, and generate key signal input related to the user settings and function control of the computer device, such as a touch screen, a keypad, a mouse, a track pad, a touch pad, an indicator rod, 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-mentioned display device includes but is not limited to a liquid crystal display, a light emitting diode, a display and a plasma display. In some optional embodiments, the display device can 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 can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, 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 storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, 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 a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0129] A part of the present invention may be applied as a computer program product, such as a computer program instruction, which, when executed by a computer, can call or provide the method and / or technical solution according to the present invention through the operation of the computer. Those skilled in the art should understand that the existence of the computer program instruction in a computer-readable medium includes, but is not limited to, a source file, an executable file, an installation package file, etc., and accordingly, the way in which the computer program instruction is executed by the computer includes, but is not limited to: the computer directly executes the instruction, or the computer compiles the instruction and then executes the corresponding compiled program, or the computer reads and executes the instruction, or the computer reads and installs the instruction and then executes the corresponding installed program. Here, the computer-readable medium may be any available computer-readable storage medium or communication medium accessible to the computer.

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

Claims

1. A method for simultaneous vehicle positioning and mapping, characterized in that: The method comprises: Acquire inertial measurement unit data, wheel speed data and global positioning system data of the vehicle, and use 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 position information of the vehicle; Based on the angle information represented by the inertial measurement unit data, determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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; Acquiring lane line information, obstacle position information, and obstacle speed information perceived 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 obstacle speed information; The three-dimensional posture information is updated based on the transformation of the lane line information during the movement of the vehicle, and the lane line is fitted 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 The three-dimensional position information of the vehicle is obtained 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, including: Acquire the angle increment of the vehicle represented by the inertial measurement unit data, and obtain the three-dimensional posture information based on the accumulation of the angle increment; Among them, the inertial measurement unit data includes: the acceleration signal of the vehicle and the angular velocity signal of the vehicle. The acceleration signal is integrated to obtain the speed of the vehicle, and the speed is integrated to obtain the displacement of the vehicle; the angular velocity signal is integrated 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 the three-dimensional posture information of the vehicle further includes: Acquire the position increment of the vehicle represented by the global positioning system data, and obtain the three-dimensional posture information based on the accumulation of the position increment; The updated observation equation based on the GPS data is as follows: P GPS =P+δP GPS Among them, P GPS is the position of the vehicle represented by the GPS data, P is the vehicle position at the current moment, δP GPS P GPS The difference in vehicle position between and P.

4. The method according to claim 3, 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 the three-dimensional posture information of the vehicle further includes: Acquire a speed increment of the vehicle represented by the wheel speed data, and obtain the three-dimensional posture information based on 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 pulse of the vehicle, R IG is the conversion relationship between the wheel speed pulse from the vehicle body coordinate system to the northeast sky coordinate system, V G is the vehicle speed in the northeast celestial coordinate system.

5. The method according to claim 2, characterized in that: Determining the position of the obstacle in the two-dimensional map based on the obstacle position information includes: The position of the obstacle is determined in the two-dimensional map by the following formula: P EN =T trans R GI P I Among them, 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 transformation relationship of the two-dimensional obstacle position from the vehicle coordinate system to the three-dimensional northeast celestial coordinate system where the current vehicle is located. θ1 is the pitch angle, θ0 is the roll angle, T trans It represents the mapping relationship of the obstacle position from the three-dimensional northeast celestial coordinate system to the two-dimensional northeast coordinate system.

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

7. The method according to claim 5, characterized in that Fitting lane lines in the two-dimensional map by the least squares method includes: Constructing a lane line fitting function, wherein 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 coefficients of each term in the lane line fitting function; The fitted lane lines in the two-dimensional map are obtained based on the coefficients of the various items.

8. A device for simultaneous vehicle positioning and mapping, characterized in that: The device comprises: A data acquisition module, used to acquire inertial measurement unit data, wheel speed data and global positioning system data of the vehicle, and to obtain three-dimensional position 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 is used to determine the gravity plane where the vehicle posture represented by the three-dimensional posture 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 obtain lane line information, obstacle position information, and obstacle speed information perceived 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; A mapping module is used to update the three-dimensional pose information based on the transformation of the lane line information during the movement of the vehicle, 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: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the method for synchronous positioning and mapping of a vehicle according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method for simultaneous vehicle positioning and mapping according to any one of claims 1 to 7.

Images