Map construction method and device, electronic equipment and storage medium
By acquiring and optimizing the position and local map of the bicycle, the problem of low online map construction accuracy is solved, high-precision online map construction and bicycle positioning is achieved, and decision-making accuracy of the intelligent driving system is improved.
Patent Information
- Application Number
- CN202410984881.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-04-24
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-04-24
AI Technical Summary
The existing online vector map construction method is not very accurate in real-time map construction, especially the location of map elements is inaccurate, making it difficult to achieve high-precision online map construction.
By obtaining the position of the bicycle at the current moment and the local map of the previous moment, using sensor data to build a detection map, and gradually update the location of the local map through coordinate system conversion and matching optimization to achieve accurate construction of online maps.
Improve the accuracy of real-time built online maps, integrate pre- and post-perception results and other sensor data, and enhance the accuracy of bicycle positioning and decision planning.
Smart Images

Figure CN118960716A_ABST
Abstract
Description
[0001] This application is a divisional application. The application date of the original application is April 24, 2024, the application number of the original application is 202410496267.8, and the name of the invention of the original application is: a map construction method, device, electronic device and storage medium. Technical Field
[0002] The present invention relates to the technical field of online map construction, and in particular to a map construction method, device, electronic equipment and storage medium. Background Art
[0003] Currently, the technology for building and positioning lane-level maps for autonomous driving is about achieving high-precision, stable and reliable transmission of maps and positioning for mass-produced autonomous driving vehicles on drivable roads. This technology mainly involves the following aspects: offline high-precision maps, crowdsourced maps, online maps, etc.
[0004] However, although the current end-to-end online vector map construction method can effectively solve the modeling and matching problems of map elements, and the accuracy has been greatly improved, the fluctuation is still large, and the accuracy of the map constructed online in real time is not high, for example, the location of the map elements is inaccurate. Therefore, how to improve the accuracy of the online map constructed in real time is a technical problem that the present invention urgently needs to solve. Summary of the invention
[0005] Based on the above technical problems, the present invention provides a map construction method, device, electronic device and storage medium to improve the accuracy of online maps constructed in real time.
[0006] A first aspect of the present invention provides a map construction method, the method comprising:
[0007] Obtaining the position and posture of the vehicle at the current moment in the first coordinate system, and obtaining the local map of the vehicle at the previous moment in the first coordinate system; wherein the "vehicle" in the present invention means: the current vehicle;
[0008] According to the sensor data at the current moment, a detection map in the second coordinate system at the current moment is obtained;
[0009] According to the current position of the vehicle in the first coordinate system, the detection map in the second coordinate system at the current moment is converted to the first coordinate system to obtain the detection map in the first coordinate system at the current moment;
[0010] Matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result;
[0011] According to the first matching result, correct the current position and posture of the vehicle in the first coordinate system to obtain an updated position and posture of the vehicle in the first coordinate system at the current moment;
[0012] According to the updated posture of the vehicle at the current moment in the first coordinate system, an updated detection map in the first coordinate system at the current moment is obtained, and then matched with the local map in the first coordinate system at the previous moment to obtain a second matching result;
[0013] According to the second matching result, the position of the map element in the local map in the first coordinate system at the previous moment is updated.
[0014] A second aspect of the present invention provides a map construction device, the device comprising:
[0015] An acquisition module is used to obtain the position and posture of the vehicle in the first coordinate system at the current moment, and to obtain the local map in the first coordinate system at the previous moment;
[0016] A detection module, used to obtain a detection map in a second coordinate system at the current moment according to sensor data at the current moment;
[0017] A conversion module, used for converting the detection map in the second coordinate system at the current moment into the first coordinate system according to the position of the vehicle at the current moment in the first coordinate system, so as to obtain the detection map in the first coordinate system at the current moment;
[0018] A first matching module, used for matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result;
[0019] A posture updating module, used for correcting the posture of the vehicle at the current moment in the first coordinate system according to the first matching result, to obtain an updated posture of the vehicle at the current moment in the first coordinate system;
[0020] A second matching module is used to obtain an updated detection map in the first coordinate system at the current moment according to the updated posture of the vehicle in the first coordinate system at the current moment, and then match it with the local map in the first coordinate system at the previous moment to obtain a second matching result;
[0021] A map updating module is used to update the position of the map element in the local map in the first coordinate system at the previous moment according to the second matching result.
[0022] The third aspect of the present invention provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the map construction method of the first aspect of the present invention.
[0023] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the map construction method of the first aspect of the present invention is implemented.
[0024] Through the map construction method of the present invention, the vehicle posture and detection map of each moment are obtained in real time, and the local map of the previous moment is obtained; after the detection map of the current moment is converted to the same coordinate system based on the vehicle posture, the detection map of the current moment in the same coordinate system can be matched with the local map of the previous moment for the first time, so as to optimize the vehicle posture based on the first matching result; then, based on the optimized vehicle posture, the detection map is converted to the same coordinate system again, and then the updated detection map is matched with the local map for the second time, and the local map is updated based on the second matching result, so as to optimize the local map at each moment, realize the accurate construction of the online map, and improve the accuracy of the online map constructed in real time. BRIEF DESCRIPTION OF THE DRAWINGS
[0025] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative labor.
[0026] Figure 1 is a flow chart of a map construction method shown in one embodiment of the present invention;
[0027] Figure 2 It is a schematic diagram of a residual calculation process in map element matching proposed in an embodiment of the present invention;
[0028] Figure 3 It is a topology relationship optimization model diagram provided by an embodiment of the present invention;
[0029] Figure 4 It is a simple schematic diagram of a model between map elements of a graph optimization matching provided by an embodiment of the present invention;
[0030] Figure 5 is a flowchart of a map construction and positioning method according to an embodiment of the present invention;
[0031] Figure 6 is a structural block diagram of a map construction device provided by an embodiment of the present invention;
[0032] Figure 7 It is a schematic diagram of an electronic device shown in an embodiment of the present invention. DETAILED DESCRIPTION
[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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 ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0034] Reference Figure 1 , Figure 1 It is a flowchart of a map construction method shown in one embodiment of the present invention.
[0035] like Figure 1 As shown, the method may include the following steps:
[0036] Step S11: Obtain the current position and posture of the vehicle in the first coordinate system, and obtain the local map in the first coordinate system at the previous moment.
[0037] In this embodiment, during the process of vehicle driving, such as the process of automatic driving, the position of the vehicle at each moment can be tracked inferred in the first coordinate system according to the real-time driving information of the vehicle, so as to obtain the position of the vehicle at the current moment in the first coordinate system. Among them, the vehicle is the current vehicle, which can be an automatic driving vehicle. The vehicle of this embodiment can display the map constructed by this embodiment; the first coordinate system is a coordinate system based on the absolute position of the real physical world, and the track inference is to use the position of the carrier at a certain moment, and according to the heading and speed information, to calculate the position information of the vehicle at the current moment.
[0038] For example, in one embodiment, the position of the vehicle at the current moment in the first coordinate system can be determined by using an IMU (Inertial Measurement Unit) and a wheel speed sensor Kalman filter to calculate the relative displacement (i.e., the vehicle's posture). First, the inertial measurement unit IMU can provide the acceleration and angular velocity information of the vehicle at the current moment. By integrating the acceleration and angular velocity information of the vehicle, the speed and displacement of the vehicle at the current moment can be obtained. In addition, the wheel speed sensor can directly measure the rotation speed of the vehicle's wheels, and the actual driving distance of the vehicle at the current moment can be obtained through the radius and rotation speed of the wheel.
[0039] However, since the integration process of the inertial measurement unit IMU will accumulate errors, the displacement information obtained after a long period of integration may have a large deviation. In addition, the measurement of the wheel speed sensor is relatively accurate, but its measurement value may also be affected in cases such as wheel slippage or tire wear. Based on this, in this embodiment, in order to comprehensively utilize the advantages of IMU and wheel speed sensor, Kalman filter algorithm can be used for data fusion. Kalman filter is an efficient recursive filter, which can obtain the optimal state estimate by fusing multiple observation data in the presence of uncertainty.
[0040] When calculating the posture of the ego vehicle by fusion, the speed and displacement provided by the IMU can be used as the state variables of the Kalman filter, and the actual driving distance provided by the wheel speed sensor can be used as the observation value. Through the recursive process of the Kalman filter, the posture of the ego vehicle in the first coordinate system at the current moment that integrates the information of the IMU and wheel speed sensor can be obtained.
[0041] Assuming that the current time is time t, the trajectory inference result at time t is: the current position of the vehicle in the first coordinate system T 1,t and the error of the vehicle δT 1,t ,in R is the rotation matrix and t is the translation vector.
[0042] In this embodiment, the position and posture of the vehicle can be determined by using the high-frequency data of the IMU to provide continuous displacement information, and the integral error of the IMU can be corrected by the measurement value of the wheel speed sensor, so as to obtain a more accurate position and posture of the vehicle.
[0043] And, in this embodiment, a local map in the first coordinate system at the previous moment is also obtained. Among them, the local map in this embodiment is a map constructed based on the map elements detected previously, which can be understood as a true value. In this embodiment, the local map in the first coordinate system at the previous moment is a local map obtained by updating and optimizing the detection map at the previous moment. Among them, the local map coordinate system is the first coordinate system. If the current moment is the first moment (i.e., the first frame), the local map can be initialized.
[0044] Step S12: Obtaining a detection map in the second coordinate system at the current moment according to the sensor data at the current moment.
[0045] In this embodiment, during the driving process of the vehicle, real-time map elements are extracted according to the sensor data at the current moment to obtain a detection map in the second coordinate system at the current moment, wherein the second coordinate system is the vehicle coordinate system, the equipment coordinate system or the BEV space coordinate system.
[0046] Map elements are detected based on sensor data. For example, in some embodiments, a model can be built through an end-to-end online vector map, with reference to maptr / maptrv2, and lane line detection and map element extraction can be completed through the BEV-transformer technical solution. At the same time, multimodal data (such as camera images and lidar point clouds) can also be integrated, and key points in map elements can be predicted through the Transformer network structure. Among them, the key points in the map elements can refer to 2D key points or 3D key points.
[0047] In addition, the attention module can also be used to model the topological connection relationship between key points, which can further improve the extraction accuracy and efficiency of map elements. In some other embodiments, a time frame can be added to improve the extraction accuracy of map elements. It should be noted that the map elements of this embodiment include but are not limited to: lane center lines, lane lines, lane line types, lane markings, stop lines, sidewalks, and poles. Finally, a detection map is constructed based on a set of detected map element instances (ie, map elements), where the detection map is an ordered set of key points, and each map element is an ordered set of points.
[0048] In an optional example, assuming that the current time is time t, the kth key point result in the jth map element instance detected at time t is: P in the second coordinate system 2,j,k (x, y, 0), where this embodiment takes a 2D key point as an example, and (x, y, 0) refers to the 2D plane where the detection result is 0 on the z axis. And, the error δP corresponding to the key point can be set according to the confidence and the distance from the vehicle. 2,j (The larger the confidence s, the smaller the error; the larger the distance d from the vehicle, the larger the error). For example: δP 2,j ~N(0,∑P 2,j ),∑P 2,j =k(d / (s+1))+b, k=0.01, b=0.1; where s is the confidence level, and d is the distance from the vehicle; k and b are respectively set empirical values to meet the requirement that the larger the confidence level s, the smaller the error; the larger the distance d from the vehicle, the larger the error.
[0049] Furthermore, this embodiment can perform straight line judgment on three consecutive key points detected. When the distance from the middle key point to the line segments of the front and rear key points is less than a preset threshold, the middle point is deleted to obtain a map element without the middle point, and a detection map is constructed, thereby deleting points close to the straight line to reduce the number of key points and reduce the amount of calculation in the subsequent optimization process.
[0050] Step S13: According to the current position and posture of the vehicle in the first coordinate system, the detection map in the second coordinate system at the current moment is converted to the first coordinate system to obtain the detection map in the first coordinate system at the current moment.
[0051] In this embodiment, since the detection map is in the second coordinate system, it is necessary to convert the detection map in the second coordinate system to the first coordinate system, and then match the detection map with the local map. Specifically, the detection map in the second coordinate system at the current moment can be converted to the first coordinate system according to the current position of the vehicle in the first coordinate system, so as to obtain the detection map in the first coordinate system at the current moment.
[0052] For example, the current time is time t, and the position of the kth key point in the i-th map feature instance at time t in the first coordinate system is P 1,j,k,t With error δP 1,j,k,t , then based on the position t of the vehicle in the first coordinate system at time t 1,t and the error of the vehicle δt 1,t The conversion process can refer to the following formula:
[0053]
[0054] Among them, R 1,t is the rotation matrix, δR is R 1,t The error is, θ is the rotation matrix R 1,t Lie algebra, δθ is the error of θ, δR=exp(δθ), R 1,t There is a one-to-one correspondence between θ and δθ, and δR also corresponds to δθ. 2,j,k ,δP 2,j,k are the position and error of the kth key point in the jth map feature instance in the second coordinate system, P 2,j,k The antisymmetric matrix of the first coordinate system is δP, and the error of the key points of the map elements in the detection map is 1,j,k,t ~N(0,∑P 1,j,k,t ), Where T is the transpose.
[0055] Step S14: Matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result.
[0056] In this embodiment, after obtaining the detection map in the first coordinate system at the current moment, the detection map in the first coordinate system at the current moment may be matched for the first time with the local map in the first coordinate system at the previous moment to obtain a first matching result.
[0057] Step S15: According to the first matching result, the current position and posture of the vehicle in the first coordinate system are corrected to obtain an updated position and posture of the vehicle in the first coordinate system at the current moment.
[0058] In this embodiment, according to the first matching result obtained in the first matching, the current position and posture of the vehicle in the first coordinate system can be corrected based on the first matching result to obtain an updated position and posture of the vehicle in the first coordinate system at the current moment.
[0059] Step S16: According to the updated posture of the vehicle in the first coordinate system at the current moment, an updated detection map in the first coordinate system at the current moment is obtained, and then matched with the local map in the first coordinate system at the previous moment to obtain a second matching result.
[0060] In this embodiment, after obtaining the updated posture of the vehicle in the first coordinate system at the current moment, the detection map in the second coordinate system at the current moment can be reconverted to the first coordinate system according to the updated posture of the vehicle in the first coordinate system at the current moment to obtain the updated detection map in the first coordinate system at the current moment.
[0061] After obtaining the updated detection map in the first coordinate system at the current moment, the updated detection map in the first coordinate system at the current moment can be matched for the second time with the local map in the first coordinate system at the previous moment to obtain a second matching result. In this step, the re-conversion method is the same or similar to the method in the aforementioned step S13, and the second matching method is the same or similar to the first matching method, which will not be repeated.
[0062] Step S17: updating the position of the map element in the local map in the first coordinate system at the previous moment according to the second matching result.
[0063] In this embodiment, after obtaining the second matching result, the position of the map element in the local map in the first coordinate system at the previous moment can be updated according to the second matching result to generate the local map in the first coordinate system at the current moment, thereby completing the update of the local map and realizing the real-time construction of the online map. In addition, the local map in the first coordinate system at the current moment can be used for the map construction at the next moment.
[0064] In this embodiment, the vehicle posture and detection map of each moment are obtained in real time, and the local map of the previous moment is obtained; after the detection map of the current moment is converted to the same coordinate system based on the vehicle posture, the detection map of the current moment in the same coordinate system can be matched with the local map of the previous moment for the first time, so as to optimize the vehicle posture based on the first matching result; then, the detection map is converted to the same coordinate system again based on the optimized vehicle posture, and the updated detection map is matched with the local map for the second time, and the local map is updated based on the second matching result, so as to optimize the local map at each moment, realize the accurate construction of the online map, and improve the accuracy of the online map constructed in real time, so as to integrate the previous and next perception results (detection results) with other sensor odometers, improve the accuracy of the constructed map and the vehicle positioning (vehicle posture), and then improve the accuracy of decision-making and planning in the intelligent driving system.
[0065] In combination with the above embodiments, in one implementation, the present invention further provides a map construction method. In this method, the above step S14 may specifically include step S21, and the above step S15 may specifically include step S22:
[0066] Step S21: matching the map elements in the detection map in the first coordinate system at the current moment with the map elements in the local map in the first coordinate system at the previous moment to obtain the first matching result.
[0067] In this embodiment, for the first matching, the map elements in the detection map in the first coordinate system at the current moment can be matched with the map elements in the local map in the first coordinate system at the previous moment to obtain the first matching result. It can be understood that the purpose of matching is to match the map elements of the detection map of the same type with the map elements in the local map one by one, and to eliminate missed detections and false detections as much as possible. In this embodiment, each map element in the detection map can be matched with each map element in the local map respectively, so as to obtain the first matching result.
[0068] The first matching result at least includes: the matched first matching map element and the residual corresponding to the first matching map element. And in this embodiment, the second matching method is still the same or similar to the first matching method.
[0069] Step S22: with the goal of minimizing the residual corresponding to the first matching map element, correct the position and posture of the vehicle at the current moment in the first coordinate system to obtain an updated position and posture of the vehicle at the current moment in the first coordinate system.
[0070] In this embodiment, after obtaining the matching first matching map element and the residual corresponding to the first matching map element, when updating the vehicle's posture, the posture of the vehicle in the first coordinate system at the current moment can be corrected with the goal of minimizing the residual corresponding to the first matching map element to obtain the updated posture of the vehicle in the first coordinate system at the current moment.
[0071] For example, the residual corresponding to the first matching map element can be made as small as possible, and when the residual reaches a certain value (a preset residual threshold), the corresponding position of the vehicle is the updated position of the vehicle in the first coordinate system at the current moment.
[0072] In an optional example, the ray from the current key point to the next key point in the map element of the local map can be calculated, and the key point position is P 1,j,k,t-1 The error is δP 1,j,k,t-1 , the direction vector is l and the error δl, where the error δl can be calculated through the error propagation model in three-dimensional space, so that the ray distance d to the nearest key point in the map element of the detection map and the error δd can be calculated, and the self-position can be corrected so that ∑ j=0 ∑ k=0 weight*d j,k minimum, where j is a map feature, k is a key point, and d j,k is the ray distance from the kth key point to the nearest key point in the jth map feature instance, weight is the weight value, which is affected by δd. You can refer to the observation model in plane uncertainty estimation in voxelmap and iekf pose estimation.
[0073] In combination with the above embodiments, in one implementation, the present invention further provides a map construction method. In the method, before the above step S21, a step S31 is further included, and the above step S21 may specifically include a step S32:
[0074] Step S31: Eliminate map elements whose first probability is less than a first threshold in the local map in the first coordinate system at the previous moment, to obtain remaining map elements.
[0075] In this embodiment, each map element in the local map corresponds to a first probability, and the first probability represents the probability that the map element in the local map actually exists. Since this embodiment uses track inference to predict the position of the vehicle, the position difference between the map elements in the detection map and the local map will not be too large, so the map elements with a first probability less than the first threshold can be eliminated from the local map in the first coordinate system at the last moment, and the remaining map elements in the local map in the first coordinate system at the last moment are obtained. Among them, the first threshold of this embodiment is a threshold freely set according to needs or experience, and there is no restriction on the value of the first threshold.
[0076] Step S32: matching the map elements in the detection map in the first coordinate system at the current moment with the remaining map elements to obtain the first matching result.
[0077] In this embodiment, when matching the local map with the detection map, the map elements in the detection map in the first coordinate system at the current moment are matched with the remaining map elements in the local map in the first coordinate system at the previous moment to obtain a first matching result. Similarly, in this embodiment, the second matching is to match the map elements in the updated detection map in the first coordinate system at the current moment with the remaining map elements to obtain a second matching result.
[0078] In combination with the above embodiments, the present invention further provides a map construction method, in which, in addition to the above steps, steps S41 to S43 may also be included:
[0079] Step S41: Based on the residual corresponding to the second matching map element, increase the first probability corresponding to the second matching map element in the local map.
[0080] In this embodiment, all map elements in the local map correspond to a first probability, and the first probability represents the probability that the map elements in the local map actually exist. The second matching result of this embodiment includes: the matched second matching map element and the residual corresponding to the second matching map element, the map element without a match in the local map, and the map element without a match in the detection map. Among them, the map element without a match in the local map represents that the map element is in the local map and does not match the map element in the detection map; the map element without a match in the detection map represents that the map element is in the detection map and does not match the map element in the local map.
[0081] It can be understood that the first matching result in this embodiment includes not only the matched first matching map elements and the residuals corresponding to the first matching map elements, but also the map elements that are not matched in the local map in the first matching, and the map elements that are not matched in the detection map in the first matching. This embodiment performs two matches, and in addition to the successfully matched matching map elements and the residuals corresponding to the matched map elements, it also includes the unsuccessful matching: the map elements that are not matched in the local map, and the map elements that are not matched in the detection map. The map elements that are not matched in the local map and the map elements that are not matched in the detection map obtained from the two matches can be the same or different, which is related to the actual matching situation.
[0082] In this embodiment, after obtaining the second matching result, the first probability corresponding to the second matching map element in the local map of the first coordinate system at the previous moment can be increased according to the residual corresponding to the second matching map element in the second matching result. For example, a probability can be calculated according to the residual size corresponding to the second matching map element, and the calculated probability can be added to the first probability corresponding to the previous one, so that the first probability corresponding to the second matching map element is increased, and the smaller the residual, the greater the probability; it is also possible to directly use the calculated probability as the first probability corresponding to the second matching map element, so that the first probability corresponding to the second matching map element is increased, and the smaller the residual, the greater the probability.
[0083] Step S42: reducing the first probability that the local map has no matching map element in the local map.
[0084] In this embodiment, after determining that there is no matching map element in the local map in the second matching result, the first probability corresponding to the map element in the local map in the first coordinate system at the previous moment that has no matching map element in the local map may be reduced.
[0085] Step S43: adding map elements that have no match in the detection map to the local map, and initializing first probabilities corresponding to the map elements that have no match in the detection map.
[0086] In this embodiment, for the map elements with no match in the detection map in the second matching result, the map elements with no match in the detection map can be added to the local map in the first coordinate system at the previous moment, and the first probability corresponding to the newly added map elements with no match in the detection map in the local map can be initialized.
[0087] In this embodiment, with respect to the second matching result at the current moment, the first probability of each map element in the local map is updated for use in matching between map elements at the next moment.
[0088] In combination with the above embodiments, the present invention further provides a map construction method, in which the above step S21 may specifically include steps S51 to S56:
[0089] Step S51: Determine a second key point that is closest to a first key point in a first map element in the detection map.
[0090] In this embodiment, the second key point closest to the first key point in the first map element of the detection map can be determined first. Among them, the map element to be matched in the detection map in the first coordinate system at the current moment is the first map element; the map element to be matched in the local map in the first coordinate system at the previous moment is the second map element; the first map element and the second map element are two map elements to be matched. The key point in the first map element is the first key point; the second key point is the key point in the second map element of the local map in the first coordinate system at the previous moment that is closest to the first key point.
[0091] Step S52: Determine the straight-line distance between the first key point and the second key point.
[0092] In this embodiment, after determining the second key point corresponding to the first key point, that is, determining the second key point closest to the first key point, the straight-line distance D between the first key point and the second key point can be determined.
[0093] Step S53: traverse all first key points in the first map element to obtain the straight-line distances corresponding to all first key points.
[0094] In this embodiment, there are multiple first key points in the first map element, and all the first key points in the first map element can be traversed to determine the second key points corresponding to all the first key points, thereby obtaining the straight-line distances corresponding to all the first key points. The straight-line distances corresponding to all the first key points are: the straight-line distances corresponding to all the first key points and the second key points corresponding to them.
[0095] Step S54: based on the straight-line distances corresponding to all the first key points, obtain the corresponding residual between the first map element and the second map element.
[0096] In this embodiment, after obtaining the straight-line distances corresponding to all the first key points in the first map element, the corresponding residual between the first map element and the second map element can be determined based on the straight-line distances corresponding to all the first key points. In an optional example, the straight-line distances corresponding to all the first key points can be averaged to obtain the corresponding residual between the first map element and the second map element.
[0097] Step S55: retaining the matching relationship between the first map element and the second map element when the residual is less than a second threshold.
[0098] In this embodiment, a second threshold is preset, and the second threshold can be set arbitrarily according to demand or experience, and is not limited to this. When it is determined that the corresponding residual between the first map element and the second map element is less than the second threshold, the matching relationship between the first map element and the second map element can be retained.
[0099] Step S56: Eliminate redundant matching relationships in the matching relationships to obtain one-to-one matching map elements and residuals corresponding to the one-to-one matching map elements.
[0100] In this embodiment, after retaining the matching relationship between the first map element and the second map element, since one first map element may retain a matching relationship with multiple second map elements, and one second map element may retain a matching relationship with multiple first map elements, based on this, this embodiment uses the Hungarian matching algorithm to remove redundant matching relationships from the retained matching relationships, thereby obtaining one-to-one matching matching map elements and the residuals corresponding to the one-to-one matching matching map elements.
[0101] It can be understood that, during the first matching, the one-to-one matching map elements and the residuals corresponding to the one-to-one matching map elements are: the matched first matching map elements and the residuals corresponding to the first matching map elements; during the second matching, the one-to-one matching map elements and the residuals corresponding to the one-to-one matching map elements are: the matched second matching map elements and the residuals corresponding to the second matching map elements.
[0102] For example, in one embodiment, Figure 2 As shown, Figure 2 FIG. 1 is a schematic diagram of a residual calculation process in a map element matching method proposed in an embodiment of the present invention. Figure 2 In the figure, the upper solid curve is the map element in the detection map, the lower dotted curve is the map element in the local map, the points on the curves are the ordered points in the map elements, that is, the ordered key points in the map elements, and the line between the two curves is the distance from the key point in the map element of the detection map to the nearest line segment of the map element of the local map.
[0103] In combination with the above embodiments, the present invention further provides a map construction method, in which the above step S17 may specifically include steps S61 to S69:
[0104] Step S61: Determine key points to be optimized based on the second matching result and the local map in the first coordinate system at the previous moment.
[0105] In this embodiment, after determining the second matching result, the key point to be optimized can be determined based on the second matching result and the local map in the first coordinate system at the previous moment. The key point to be optimized is a key point in the map element that needs to be updated in position.
[0106] In this embodiment, in order to optimize the topological relationship between map elements, the curve shape of each map element itself is also optimized. Since the key points of the map elements in the local map and the detection map are not one-to-one matching, and on a curve, the intersections in each map element are fixed, it is a good choice to use the intersections and the points in the matching map elements as constraints on the topological relationship in the local map.
[0107] Step S62: determining the head and tail points of the map elements among the key points to be optimized as first-category points, and determining the head and tail points of each map element as first-category point pairs.
[0108] In this embodiment, the head and tail points of the map elements among the key points to be optimized can be determined as first-class points, and the head and tail points of each map element can be determined as a first-class point pair. Taking the map element as a lane line as an example, the head and tail points of the map element are the two vertices of the lane line.
[0109] Step S63: Among the key points to be optimized, the key points where the head and tail points of the map elements coincide with the intersection points are determined as second-category points.
[0110] In this embodiment, the intersection points in each map element among the key points to be optimized can be determined first: the head and tail points in all map elements are traversed, and the nearest points in other map elements are searched within a distance threshold (a threshold arbitrarily specified according to requirements or experience) according to each head and tail point. The nearest points in multiple map elements may be found, and the nearest point in the nearest points in multiple map elements that is closest to the head and tail points is regarded as an intersection point.
[0111] After the intersection is determined, the key points to be optimized, where the head and tail points of the map elements coincide with the intersection, can be determined as the second type of points. Figure 3 As shown, Figure 3 It is a topology relationship optimization model diagram provided by an embodiment of the present invention. Figure 3 In the figure, the dot on the top rectangle is the intersection point within the threshold of the head and tail points, the horizontal ellipse points in the bottom row are the head and tail points, the vertical ellipse points in the middle row and the vertical ellipse points in the fifth column of the line segment are the key points where the head and tail points coincide with the intersection point.
[0112] Step S64: Determine the intersection points within the third threshold of the head and tail points of the map element among the key points to be optimized as third-category points, and determine the intersection points in the third-category points and the corresponding head and tail points of the map element as third-category point pairs.
[0113] In this embodiment, the intersection points within the third threshold of the head and tail points of the map elements among the key points to be optimized can be determined as the third type of points, and the head and tail points and the intersection points are the third type of point pairs. The third threshold can be set arbitrarily according to needs or experience, and the specific value is not limited. In other words, the intersection points in the third type of points and the head and tail points of the corresponding map elements are the third type of point pairs.
[0114] It should be noted that the "point pair" in the first type of point pair and the third type of point pair in this embodiment refers to a pair of points consisting of two points; wherein the first type of point pair consists of the two head and tail points of each map element, and the third type of point pair consists of the intersection points in the third type of points, and the head and tail points of the map elements corresponding to the intersection points in the third type of points.
[0115] Step S65: Among the key points to be optimized, the key point in each map element that is closest to the vehicle is determined as a fourth type of point.
[0116] In this embodiment, among the key points to be optimized, the key point closest to the vehicle in each map element is also determined as a fourth type of point.
[0117] Step S66: Determine the key points to be optimized, except the first type of points, the second type of points, the third type of points and the fourth type of points, as fifth type of points, and determine the fifth type of points in the same map element as a fifth type of point set.
[0118] In this embodiment, after determining the first, second, third and fourth category points, the key points to be optimized, except for the first, second, third and fourth category points, can be determined as fifth category points; and the fifth category points in the same map element are determined as a fifth category point set, and the fifth category point set is a sequential set.
[0119] Step S67: Define the key points to be optimized as nodes in the graph optimization model, define the two key points in the first type of point pairs as edges, define the two key points in the third type of point pairs as edges, define the two adjacent key points in the fifth type of point set as edges, and establish prior edges for the nodes of the third type of points.
[0120] In this embodiment, a graph optimization model can be established, and all key points to be optimized (first-class points, second-class points, third-class points, fourth-class points, and fifth-class points) are defined as nodes in the graph optimization model, and two key points in the first-class point pair are defined as edges, two key points in the third-class point pair are defined as edges, two adjacent key points in the fifth-class point set are defined as edges, and prior edges are established for the nodes of the third-class points.
[0121] Step S68: The distance and direction of the two adjacent key points are taken as observations, the prior edge of the first node in front of the vehicle posture is the position of the first node in the detection map at the current moment as the observation, and the prior edge of the second node behind the vehicle posture is the position of the second node in the local map at the previous moment as the observation, and the key points to be optimized are processed by the graph optimization model to obtain the positions and errors of the optimized key points.
[0122] In this embodiment, the distance and direction of two adjacent key points can be used as observations. The second matched map element includes: a map element in the local map and a map element in the detection map, all key points of the map elements in the second matched map element and the local map are defined as edges, and the distance from the key points in the map elements in the second matched map element and the detection map to the nearest line segment is used as an observation, and the observation and error of the line segment length and direction are deduced based on the position and error of the two key points of the nearest line segment.
[0123] And, the prior edge of the first node in front of the ego-vehicle posture is observed by the position of the first node in the detection map at the current moment, wherein the first node is the node in front of the ego-vehicle posture in the detection map. The prior edge of the second node behind the ego-vehicle posture is observed by the position of the second node in the local map at the previous moment, wherein the second node is the node behind the ego-vehicle posture in the local map. It should be noted that the observation of this embodiment is to keep the observed value as unchanged as possible during the optimization process.
[0124] After determining the nodes, edges, and observations, this embodiment can process the key points to be optimized through the graph optimization model to obtain the positions and errors of the key points after optimization, that is, the positions and errors of all nodes after optimization.
[0125] For example, Figure 4 As shown, Figure 4 This is a simple schematic diagram of a model for optimizing the matching of map elements provided by an embodiment of the present invention. Figure 4 In the figure, the dotted line connecting the largest dots is the a priori edge between the local map and the detection map, the other ellipse vertex connected by the a priori edge is the third type point, the dotted rectangular box above is the map element in the local map, the solid rectangular box below is the map element in the detection map, the points in the two rectangular boxes are the ordered points (fifth type points) in the map elements, the line between the fifth type points is the edge of length and direction between the points, and the short line between the two rectangular boxes is the edge between the map elements.
[0126] Step S69: Based on the positions and errors of the optimized key points, the positions of the map elements in the local map in the first coordinate system at the previous moment are updated to generate a local map in the first coordinate system at the current moment.
[0127] In this embodiment, after obtaining the positions and errors of the optimized key points, the positions of the map elements in the local map in the first coordinate system at the previous moment can be updated based on the positions and errors of the optimized key points, and the local map in the first coordinate system at the current moment can be generated to realize the map construction at the current moment.
[0128] In an optional example, specifically, the positions and errors of all nodes are obtained based on optimization, the key points in the detection map are inserted between the two key points of the nearest line segment in the local map, and then a straight line judgment is made through three adjacent key points (the same or similar to the straight line judgment in the aforementioned step S12), and the redundant key points in the middle are deleted to form a new key point set, thereby constructing a local map in the first coordinate system at the current moment based on the new key point set.
[0129] The map construction method provided in this embodiment can be deployed on the vehicle side for map-free intelligent driving to reduce costs. It can also be used for cloud crowdsourcing map construction (vector map with map element probabilities and point errors). The vehicle-side vector map matching and positioning method with errors can quickly promote L3 and above intelligent driving.
[0130] In combination with the above embodiments, the present invention further provides a map construction method, in which the above step S61 may specifically include step S71 and step S72:
[0131] Step S71: Determine as a third key point a key point whose error corresponding to a key point in a map element in the local map in the first coordinate system at the previous moment is greater than a fourth threshold.
[0132] In this embodiment, a threshold judgment may be performed on the error corresponding to the key point in the map element in the local map under the first coordinate system at the previous moment, and the error corresponding to the key point of the map element in the local map is compared with a preset fourth threshold (which can be arbitrarily specified based on demand or experience), and the key point whose error corresponding to the key point of the map element in the local map is greater than the fourth threshold is determined as the third key point.
[0133] Step S72: at least the key points of the map elements that have no matching in the detection map and the third key points in the second matching result are determined as key points to be optimized.
[0134] In this embodiment, the map elements with no match in the detection map are those that appear in the detection map but not in the local map, and they may be newly detected map elements; the map elements with no match in the local map are those that appear in the local map but not in the detection map, and they may be falsely detected map elements. These are all map elements that have not been successfully matched and need to be further optimized.
[0135] Therefore, in this embodiment, at least the key points of the map elements that have no match in the detection map in the second matching result and the determined third key points can be determined as the key points to be optimized; the key points of the map elements that have no match in the detection map in the second matching result, the key points of the map elements that have no match in the local map in the second matching result, and the determined third key points can be determined as the key points to be optimized, so as to optimize the position of the key points to be optimized.
[0136] In this embodiment, key points with smaller errors are excluded from the key points to be optimized. It can be understood that smaller errors indicate that the key points are already relatively accurate and do not need to be optimized. Therefore, in order to further save computing resources, key points with errors greater than a certain threshold and key points in map elements that have not been successfully matched before are determined as key points to be optimized and their positions are optimized.
[0137] In one embodiment, if Figure 5 As shown, Figure 5 FIG. 1 is a flow chart of a map construction and positioning method according to an embodiment of the present invention. Figure 5 In the process, after starting, first, set the first coordinate system at the absolute position of the ego vehicle in the physical world, and establish the ego vehicle state filter, such as establishing a Kalman filter for the ego vehicle to predict and update the ego vehicle posture. For example, you can refer to the use of IEKF Kalman filter in the laser radar and inertial measurement unit IMU fusion positioning algorithm.
[0138] Secondly, the vehicle posture transformation and error are obtained based on the track inference in the first coordinate system. It is assumed that the result of the track inference at time t is the vehicle posture and error. Also, map element detection is performed in the second coordinate system at each moment, so that the map element detection is performed in the second coordinate system at time t, and the map element of the detection map in the second coordinate system at time t is obtained. Among them, the vehicle coordinate system, the device coordinate system or the BEV space are collectively referred to as the second coordinate system. At the beginning, the transformation matrix between the first coordinate system and the second coordinate system is T 1,t And, when time t is the first frame, the local map is initialized.
[0139] Then, based on the vehicle's position and error obtained by inferring the track at time t, the map elements in the detection map in the second coordinate system at time t are transformed to obtain the position and error of the map elements in the detection map at time t in the first coordinate system.
[0140] Next, based on the position and error of the map elements in the detection map at time t in the first coordinate system and the map elements in the local map updated at time t-1, matching detection is performed through the Hungarian algorithm to determine the first matching result between the map elements of the detection map and the map elements of the local map.
[0141] Finally, for the pose residuals of the successfully matched map elements in the first matching result, the vehicle state filter is optimized and updated to obtain the optimized pose and error of the vehicle. Based on the optimized pose and error of the vehicle, the pose and error of the map elements in the detection map in the second coordinate system at time t are re-converted to update the pose and error of the map elements in the detection map at time t in the first coordinate system. Finally, based on the pose and error of the map elements in the detection map at time t in the first coordinate system, they are re-matched with the local map elements, and the local map at time t is updated based on the unmatched map elements in the second matching result and the map elements with large errors in the local map.
[0142] In this embodiment, the map elements in the current frame are detected, and the map elements in the current detection map and the map elements in the local map in the same coordinate system are Hungarian matched. The matched detection map elements are used as observations of the map elements in the local map with a probability greater than a threshold, and the odometer information of other sensors is integrated to optimize the positioning of the vehicle, thereby correcting the first probability of the map elements in the local map and the key point positions and errors. The unmatched detection map instances are added to the local map for management, forming a cycle, and finally reaching a convergence state.
[0143] It should be noted that, for the sake of simplicity, the method embodiments are described as a series of action combinations, but those skilled in the art should be aware that the embodiments of the present invention are not limited by the order of the actions described, because according to the embodiments of the present invention, certain steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also be aware that the embodiments described in the specification are all preferred embodiments, and the actions involved are not necessarily required by the embodiments of the present invention.
[0144] Based on the same inventive concept, an embodiment of the present invention provides a map construction device 600. Figure 6 , Figure 6 FIG. 1 is a structural block diagram of a map construction device provided by an embodiment of the present invention. Figure 6 As shown, the device 600 includes:
[0145] An acquisition module 601 is used to acquire the position and posture of the vehicle in the first coordinate system at the current moment, and to acquire a local map in the first coordinate system at the previous moment;
[0146] The detection module 602 is used to obtain a detection map in the second coordinate system at the current moment according to the sensor data at the current moment;
[0147] A conversion module 603 is used to convert the detection map in the second coordinate system at the current moment into the first coordinate system according to the posture of the vehicle at the current moment in the first coordinate system, so as to obtain the detection map in the first coordinate system at the current moment;
[0148] A first matching module 604 is used to match the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result;
[0149] A posture updating module 605 is used to correct the posture of the vehicle at the current moment in the first coordinate system according to the first matching result to obtain an updated posture of the vehicle at the current moment in the first coordinate system;
[0150] A second matching module 606 is used to obtain an updated detection map in the first coordinate system at the current moment according to the updated position of the vehicle in the first coordinate system at the current moment, and then match it with the local map in the first coordinate system at the previous moment to obtain a second matching result;
[0151] The map updating module 607 is used to update the position of the map element in the local map in the first coordinate system at the last moment according to the second matching result.
[0152] Optionally, the first matching module 604 includes:
[0153] The first matching submodule is used to match the map elements in the detection map in the first coordinate system at the current moment with the map elements in the local map in the first coordinate system at the previous moment to obtain the first matching result; the first matching result at least includes: the matched first matching map elements and the residual corresponding to the first matching map elements;
[0154] The posture updating module 605 comprises:
[0155] The posture updating submodule is used to correct the posture of the vehicle at the current moment in the first coordinate system with the goal of minimizing the residual corresponding to the first matching map element, so as to obtain the updated posture of the vehicle at the current moment in the first coordinate system.
[0156] Optionally, the map element in the local map corresponds to a first probability, and the first probability represents the probability that the map element in the local map actually exists; the device 600 further includes:
[0157] a removal module, configured to remove the map elements in the local map under the first coordinate system at the previous moment whose first probability is less than a first threshold value in the local map under the first coordinate system at the previous moment before matching the map elements in the detection map under the first coordinate system at the current moment with the map elements in the local map under the first coordinate system at the previous moment to obtain the first matching result, so as to obtain the remaining map elements;
[0158] The first matching submodule includes:
[0159] The map element matching submodule is used to match the map elements in the detection map in the first coordinate system at the current moment with the remaining map elements to obtain the first matching result.
[0160] Optionally, the map element in the local map corresponds to a first probability, and the first probability represents the probability that the map element in the local map actually exists; the second matching result includes: the matched second matching map element and the residual corresponding to the second matching map element, the map element without a match in the local map, and the map element without a match in the detection map; the map element without a match in the local map represents that the map element is in the local map and does not match the map element in the detection map; the map element without a match in the detection map represents that the map element is in the detection map and does not match the map element in the local map;
[0161] The device 600 further includes:
[0162] A first probability updating module, configured to increase a first probability corresponding to the second matching map element in the local map based on a residual corresponding to the second matching map element;
[0163] a second probability updating module, configured to reduce a first probability corresponding to a map element in the local map that has no matching map element in the local map;
[0164] The third probability updating module is used to add the map elements that have no match in the detection map to the local map, and initialize the first probability corresponding to the map elements that have no match in the detection map.
[0165] Optionally, the first matching submodule includes:
[0166] a key point determination submodule, configured to determine a second key point closest to a first key point in a first map element in the detection map, wherein the second key point is in a second map element in the local map; the first map element and the second map element are map elements to be matched;
[0167] A straight-line distance determination submodule, used to determine the straight-line distance between the first key point and the second key point;
[0168] A traversal submodule, used for traversing all the first key points of the first map element to obtain the straight-line distances corresponding to all the first key points;
[0169] A residual determination submodule, configured to obtain a residual corresponding to the first map element and the second map element based on the straight-line distances corresponding to all the first key points;
[0170] a retaining relationship submodule, configured to retain the matching relationship between the first map element and the second map element when the residual is less than a second threshold;
[0171] The elimination submodule is used to eliminate redundant matching relationships in the matching relationships to obtain one-to-one matching map elements and residuals corresponding to the one-to-one matching map elements.
[0172] Optionally, the map updating module 607 includes:
[0173] An optimization key point determination submodule, used to determine the key points to be optimized based on the second matching result and the local map in the first coordinate system at the previous moment;
[0174] A first-category point determination submodule, used to determine the head and tail points of the map elements among the key points to be optimized as first-category points, and determine the head and tail points of each map element as first-category point pairs;
[0175] A second type point determination submodule is used to determine the key points where the head and tail points of the map elements coincide with the intersection points among the key points to be optimized as second type points;
[0176] A third-category point determination submodule is used to determine, among the key points to be optimized, intersection points within a third threshold from the head and tail points of the map element as third-category points, and determine the intersection points among the third-category points and the corresponding head and tail points of the map element as third-category point pairs;
[0177] The fourth type point determination submodule is used to determine the key point closest to the vehicle in each map element among the key points to be optimized as the fourth type point;
[0178] A fifth-category point determination submodule is used to determine the key points to be optimized, except the first-category points, the second-category points, the third-category points and the fourth-category points, as fifth-category points, and determine the fifth-category points in the same map element as a fifth-category point set;
[0179] An edge determination submodule, used to define the key points to be optimized as nodes in a graph optimization model, define two key points in the first type of point pairs as edges, define two key points in the third type of point pairs as edges, define two adjacent key points in the fifth type of point set as edges, and establish prior edges for nodes of the third type of points;
[0180] An observation determination submodule, configured to take the distance and direction of the two adjacent key points as observations, the a priori edge of the first node in front of the ego-vehicle posture by the position of the first node in the detection map at the current moment as observation, and the a priori edge of the second node behind the ego-vehicle posture by the position of the second node in the local map at the previous moment as observation, and process the key points to be optimized through the graph optimization model to obtain the positions and errors of the optimized key points;
[0181] The position updating submodule is used to update the position of the map elements in the local map in the first coordinate system at the previous moment based on the position and error of the optimized key point, and generate the local map in the first coordinate system at the current moment.
[0182] Optionally, the optimization key point determination submodule includes:
[0183] A third key point determination submodule is used to determine a key point in the local map in the first coordinate system at the previous moment, the key point corresponding to which is greater than a fourth threshold value, as a third key point;
[0184] The key point determination submodule for optimization is used to determine at least the key point of the map element that has no match in the detection map in the second matching result and the third key point as the key point to be optimized.
[0185] Based on the same inventive concept, another embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps in the map construction method described in any of the above embodiments of the present invention are implemented.
[0186] Based on the same inventive concept, another embodiment of the present invention provides an electronic device 700, such as Figure 7 shown. Figure 77 is a schematic diagram of an electronic device according to an embodiment of the present invention. The electronic device includes a memory 702, a processor 701, and a computer program stored in the memory and executable on the processor, and when executed by the processor, the steps in the map construction method according to any of the above embodiments of the present invention are implemented.
[0187] As for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.
[0188] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.
[0189] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, devices, or computer program products. Therefore, embodiments of the present invention may take the form of complete hardware embodiments, complete software embodiments, or embodiments combining software and hardware. Furthermore, embodiments of the present invention may take the form of computer program products implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.
[0190] The embodiments of the present invention are described with reference to the flowcharts and / or block diagrams of the methods, terminal devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of the processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing terminal device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing terminal device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.
[0191] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing terminal device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.
[0192] These computer program instructions can also be loaded onto a computer or other programmable data processing terminal device so that a series of operating steps are executed on the computer or other programmable terminal device to produce a computer-implemented process, thereby providing instructions for executing on the computer or other programmable terminal device to implement the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.
[0193] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the embodiments of the present invention.
[0194] Finally, it should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or terminal device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or terminal device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or terminal device including the elements.
[0195] The map construction method, device, electronic device and storage medium provided by the present invention are introduced in detail above. The principles and implementation methods of the present invention are explained in this article using specific examples. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as limiting the present invention.
Claims
1. A map construction method, characterized in that: The method comprises: Obtain the current position of the vehicle in the first coordinate system, and obtain the local map of the first coordinate system at the previous moment; According to the sensor data at the current moment, a detection map in the second coordinate system at the current moment is obtained; According to the current position of the vehicle in the first coordinate system, the detection map in the second coordinate system at the current moment is converted to the first coordinate system to obtain the detection map in the first coordinate system at the current moment; Matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result; According to the first matching result, correct the current position and posture of the vehicle in the first coordinate system to obtain an updated position and posture of the vehicle in the first coordinate system at the current moment; According to the updated posture of the vehicle at the current moment in the first coordinate system, an updated detection map in the first coordinate system at the current moment is obtained, and then matched with the local map in the first coordinate system at the previous moment to obtain a second matching result; Determine the key points to be optimized according to the second matching result and the local map in the first coordinate system at the last moment, and update the positions of the map elements in the local map in the first coordinate system at the last moment based on the key points to be optimized; The key points to be optimized include at least: third-category points; the third-category points are intersection points among the key points to be optimized that are within a third threshold value from the head and tail points of the map element.
2. The map construction method according to claim 1, characterized in that: Matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result, including: Matching the map elements in the detection map in the first coordinate system at the current moment with the map elements in the local map in the first coordinate system at the previous moment to obtain the first matching result; the first matching result at least includes: the matched first matching map elements and the residual corresponding to the first matching map elements; According to the first matching result, the current position and posture of the vehicle in the first coordinate system are corrected to obtain an updated position and posture of the vehicle in the first coordinate system at the current moment, including: With the goal of minimizing the residual corresponding to the first matching map element, the position and posture of the vehicle at the current moment in the first coordinate system are corrected to obtain an updated position and posture of the vehicle at the current moment in the first coordinate system.
3. The map construction method according to claim 1, characterized in that: The map element in the local map corresponds to a first probability, and the first probability represents the probability that the map element in the local map actually exists; the second matching result includes: the matched second matching map element and the residual corresponding to the second matching map element, the map element without a match in the local map, and the map element without a match in the detection map; the map element without a match in the local map represents that the map element is in the local map and does not match the map element in the detection map; the map element without a match in the detection map represents that the map element is in the detection map and does not match the map element in the local map; The method further comprises: increasing a first probability corresponding to the second matching map element in the local map based on the residual corresponding to the second matching map element; reducing a first probability in the local map that the local map has no matching map element corresponding thereto; Map elements that have no match in the detection map are added to the local map, and first probabilities corresponding to the map elements that have no match in the detection map are initialized.
4. The map construction method according to claim 2, characterized in that: Matching the map elements in the detection map in the first coordinate system at the current moment with the map elements in the local map in the first coordinate system at the previous moment to obtain the first matching result, including: Determine a second key point closest to a first key point in a first map element in the detection map, wherein the second key point is in a second map element in the local map; the first map element and the second map element are map elements to be matched; Determine a straight-line distance between the first key point and the second key point; Traversing all the first key points of the first map element to obtain the straight-line distances corresponding to all the first key points; Obtaining a corresponding residual between the first map element and the second map element based on the straight-line distances corresponding to all the first key points; retaining the matching relationship between the first map element and the second map element when the residual is less than a second threshold; Redundant matching relationships in the matching relationships are eliminated to obtain one-to-one matching map elements and residuals corresponding to the one-to-one matching map elements.
5. The map construction method according to any one of claims 1 to 4, characterized in that: The updating of the position of the map element in the local map in the first coordinate system at the last moment based on the key point to be optimized includes: The key point to be optimized is defined as a node in the graph optimization model, at least the a priori edge of the first node in front of the ego-vehicle posture is observed by the position of the first node in the detection map at the current moment, and / or the a priori edge of the second node behind the ego-vehicle posture is observed by the position of the second node in the local map at the previous moment, and the key point to be optimized is processed by the graph optimization model to obtain the position and error of the optimized key point; wherein the a priori edge is established by the node of the third type of point; Based on the positions and errors of the optimized key points, the positions of the map elements in the local map in the first coordinate system at the previous moment are updated.
6. The map construction method according to claim 5, characterized in that: The method further comprises: Determine the head and tail points of the map elements among the key points to be optimized as first-class points, and determine the head and tail points of each map element as first-class point pairs; Among the key points to be optimized, key points where the head and tail points of the map element coincide with the intersection points are determined as second-category points; Determine the intersection points within the third threshold from the head and tail points of the map element among the key points to be optimized as the third type of points, and determine the intersection points among the third type of points and the corresponding head and tail points of the map element as the third type of point pairs; Among the key points to be optimized, the key point in each map element that is closest to the vehicle is determined as a fourth type of point; Determine the key points to be optimized, except the first type of points, the second type of points, the third type of points and the fourth type of points, as fifth type of points, and determine the fifth type of points in the same map element as a fifth type of point set; The two key points in the first type of point pair are defined as edges, the two key points in the third type of point pair are defined as edges, the two adjacent key points in the fifth type of point set are defined as edges, and the nodes of the third type of points are established with prior edges; The method of at least taking the position of the first node in the detection map at the current moment as an observation of the prior edge of the first node in front of the ego-vehicle posture, and / or taking the position of the second node in the local map at the previous moment as an observation of the prior edge of the second node behind the ego-vehicle posture, and processing the key points to be optimized by the graph optimization model to obtain the positions and errors of the key points after optimization, includes: The distance and direction of the two adjacent key points are used as observations, and the a priori edge of the first node in front of the ego-vehicle posture is the position of the first node in the detection map at the current moment as observation; or, the distance and direction of the two adjacent key points are used as observations, and the a priori edge of the second node behind the ego-vehicle posture is the position of the second node in the local map at the previous moment as observation; the key points to be optimized are processed by the graph optimization model to obtain the positions and errors of the optimized key points; The updating of the position of the map element in the local map in the first coordinate system at the last moment based on the position and error of the optimized key point comprises: Based on the positions and errors of the optimized key points, the positions of the map elements in the local map in the first coordinate system at the previous moment are updated to generate a local map in the first coordinate system at the current moment.
7. The map construction method according to claim 1, characterized in that: Determining the key points to be optimized according to the second matching result and the local map in the first coordinate system at the previous moment includes: Determine a key point in the local map in the first coordinate system at the last moment, the key point corresponding to the key point in the map element having an error greater than a fourth threshold, as a third key point; At least the key points of the map elements that have no matching in the detection map and the third key points in the second matching result are determined as key points to be optimized.
8. A map construction device, characterized in that: The device comprises: An acquisition module is used to obtain the position and posture of the vehicle in the first coordinate system at the current moment, and to obtain the local map in the first coordinate system at the previous moment; A detection module, used to obtain a detection map in a second coordinate system at the current moment according to sensor data at the current moment; A conversion module, used for converting the detection map in the second coordinate system at the current moment into the first coordinate system according to the position of the vehicle at the current moment in the first coordinate system, so as to obtain the detection map in the first coordinate system at the current moment; A first matching module, used for matching the detection map in the first coordinate system at the current moment with the local map in the first coordinate system at the previous moment to obtain a first matching result; A posture updating module, used for correcting the posture of the vehicle at the current moment in the first coordinate system according to the first matching result, to obtain an updated posture of the vehicle at the current moment in the first coordinate system; A second matching module is used to obtain an updated detection map in the first coordinate system at the current moment according to the updated posture of the vehicle in the first coordinate system at the current moment, and then match it with the local map in the first coordinate system at the previous moment to obtain a second matching result; A map updating module, configured to determine key points to be optimized according to the second matching result and the local map in the first coordinate system at the last moment, and update the positions of map elements in the local map in the first coordinate system at the last moment based on the key points to be optimized; The key points to be optimized include at least: third-category points; the third-category points are intersection points among the key points to be optimized that are within a third threshold value from the head and tail points of the map element.
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the computer program is executed by the processor, the map construction method according to any one of claims 1 to 7 is implemented.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the map construction method according to any one of claims 1 to 7 is implemented.
Citation Information
Patent Citations
Robot simultaneous positioning and mapping method and device and readable medium
CN115265523A
Map construction method and device, computer equipment and storage medium
CN117288178A
Map construction method and device based on robot and storage medium
CN117870702A
Pose prediction method, map construction method, movable platform, and storage medium
WO2021035669A1
Ground texture information-based map construction method and system, and mobile robot
WO2022179519A1
