Relocation methods and related devices for underground autonomous parking scenarios
By sampling and encoding historical trajectories and elements of semantic maps in underground autonomous parking scenarios, and utilizing odometry pose search and registration techniques, the problem of increased positioning errors in underground autonomous parking was solved, achieving high-precision repositioning.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- MOMENTA (SUZHOU) TECHNOLOGY CO LTD
- Filing Date
- 2024-08-28
- Publication Date
- 2026-07-31
AI Technical Summary
In underground autonomous parking scenarios, when the vehicle's driving route is not within the mapped area, the positioning and pose error caused by the odometer recursion gradually increases, leading to incorrect positioning and pose determination results.
By acquiring historical parking trajectories and map semantic elements from the semantic map, sampling and encoding map coded poses and features, storing mapping relationships, using odometry pose search for target map coded poses and features, calculating similarity, and performing registration to relocate vehicles.
It reduces positioning errors and improves the accuracy of positioning results. By using encoding and registration techniques, it quickly determines the match between map semantic elements and observed semantic elements, thereby reducing positioning errors.
Smart Images
Figure CN121632121B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of intelligent driving technology, and more specifically, to a relocation method and related apparatus for underground autonomous parking scenarios. Background Technology
[0002] The rise of intelligent driving technology is leading a transformation in the transportation industry. Through the coordinated operation of multiple modules such as positioning, perception, planning, and control, it transforms sensor outputs into vehicle control information, improving driving safety and comfort, and playing an increasingly important role in human society and economic activities. Autonomous parking, as a key component of intelligent driving, aims to solve the "last mile" problem. With the popularization of intelligent driving in people's lives, highly reliable autonomous parking technology has seen unprecedented development. Autonomous parking in underground scenarios typically requires learning the user's driving route and building a map of the target parking garage. When the user re-enters the garage, the vehicle can autonomously drive to a parking space based on the map. Therefore, autonomous parking in underground scenarios relies more heavily on high-precision map-based localization technology. Map-based localization technology typically relies on prior location, obtaining map information through prior location, and matching sensor observation information with its corresponding map semantic elements through methods such as iterative nearest neighbor points. This allows the construction of an observation model to solve for vehicle position and attitude information. However, when the vehicle's route is outside the mapped area, its pose is obtained by recursion from the odometer. The error accumulates with the recursion distance, causing the observation information to mismatch with the map's semantic features after returning to the map, resulting in incorrect positioning and pose determination results. Summary of the Invention
[0003] This application provides a relocation method and related device for underground autonomous parking scenarios, aiming to solve the problem that when the vehicle's driving route is not in the mapped area, the positioning pose error gradually increases with the error of the recursive distance when the vehicle's pose is recursively calculated by the odometer.
[0004] The specific technical solution is as follows:
[0005] In a first aspect, embodiments of this application provide a relocation method for underground autonomous parking scenarios, the method comprising:
[0006] Obtain historical parking trajectories and map semantic elements from the semantic map;
[0007] The historical parking trajectory is sampled, and each sampled point is used as a map coding pose. The map semantic elements within a preset range including each map coding pose are encoded to obtain the map coding features corresponding to each map coding pose. The mapping relationship between the map coding pose and the map coding features is stored in a preset storage space.
[0008] Encode the current observation semantic elements within the preset range, including vehicle reference points, to obtain current observation coding features, wherein the map semantic elements and the observation semantic elements contain the same type of semantic elements;
[0009] Based on the current odometer pose, search for N target map coding poses and N target map coding features from the preset storage space. The current odometer pose is the vehicle pose at the current time calculated based on the odometer recursive algorithm. The N target map coding poses are the N map coding poses that are closest to the current odometer pose. The target map coding features are the map coding features that correspond to the target map coding poses.
[0010] Calculate the similarity between the current observed coding feature and each of the target map coding features, and take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose;
[0011] Search the semantic map for map semantic features around the target pose, and register the searched map semantic features with the currently observed semantic features until a map semantic feature matching the currently observed semantic feature is obtained;
[0012] Based on the currently observed semantic elements and the matching map semantic elements, the vehicle is repositioned to obtain its current pose after repositioning.
[0013] As can be seen from the above scheme, the embodiments of this application can first encode the map semantic elements around the sampling points of the historical parking trajectory in the semantic map, and store the mapping relationship between the map encoded pose and the map encoded feature in a preset storage space. Then, the currently observed semantic elements around the vehicle reference point during vehicle driving are encoded to obtain the currently observed encoded features. Then, the N target map encoded poses and their corresponding target map encoded features that are closest to the current odometer pose are searched from the preset storage space. The map encoded pose with the highest similarity to the current observed encoded feature is selected from the N target map encoded poses as the target pose. Finally, the map semantic elements around the target pose are searched in the semantic map to quickly determine the map semantic elements that match the current observed semantic elements. The vehicle is repositioned based on the current observed semantic elements and the map semantic elements that match them. Therefore, compared with directly using the odometer to recursively deduce the vehicle pose as the final positioning result, the positioning error can be greatly reduced and the accuracy of the positioning result can be improved.
[0014] In one possible implementation, the coordinates of each of the map semantic elements in the semantic map are coordinates in a first coordinate system with the origin of the historical parking trajectory as the origin.
[0015] Encoding the map semantic elements within a preset range including each map coding pose to obtain map coding features corresponding to each map coding pose includes:
[0016] For each map coding pose, determine the map semantic elements within the preset range including the map coding pose in the semantic map;
[0017] A second coordinate system is established with each map coding pose as the origin. The second coordinate system is divided into sectors at preset distances and preset angles. The map semantic elements in each sector within the preset range are encoded to obtain the map coding feature value of each sector. The first coordinate system and the second coordinate system differ only in the rotation of the heading angle.
[0018] The feature matrix composed of the map coding feature values of all sectors within the preset range is used as the map coding feature corresponding to the map coding pose.
[0019] As can be seen from the above scheme, the embodiments of this application establish a second coordinate system with each sampling point as the origin, differing from the first coordinate system only by the heading angle, and divide the second coordinate system into sectors. The map semantic elements in each sector are encoded, and the feature matrix composed of the map encoding feature values in all sectors is used as the map encoding feature corresponding to the map encoding pose. This can improve the efficiency and accuracy of subsequent calculation of the similarity between the map encoding features and the currently observed encoding features.
[0020] In one possible implementation, the map semantic elements in each sector within the preset range are encoded to obtain a map encoding feature value for each sector, including:
[0021] According to the preset semantic element type order, each semantic element is encoded into the 0th to Pth bits of binary, where P+1 is the total number of semantic element types;
[0022] For each sector within the preset range, the semantic element type represented by each binary bit is judged sequentially. When the binary bit to be judged is included in the sector, the binary bit to be judged is set to 1; otherwise, the binary bit to be judged is set to 0, and the binary encoding result is converted into decimal as the map encoding feature value of the sector.
[0023] In one possible implementation, each row element in the feature matrix corresponds to the same distance interval, and each column element corresponds to the same angular interval; or,
[0024] Each row of the feature matrix corresponds to the same angular interval, and each column corresponds to the same distance interval.
[0025] In one possible implementation, the map coding feature and the current observation coding feature correspond to the same coding method;
[0026] The current observation coding feature is referred to as the current observation coding feature matrix, and each of the target map coding features is referred to as the target map coding feature matrix;
[0027] Calculate the similarity between the current observed coded feature and each of the target map coded features, including:
[0028] When each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angle interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by column of different lengths to obtain multiple shifted target map encoding feature matrices; or, when each row element in the feature matrix corresponds to the same angle interval and each column element corresponds to the same distance interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by row of different lengths to obtain multiple shifted target map encoding feature matrices.
[0029] Calculate the similarity between the current observation coding feature matrix and each translated target map coding feature matrix, and take the largest similarity as the similarity between the current observation coding feature and the target map coding feature.
[0030] As can be seen from the above scheme, since the map code is located in the first coordinate system and the observation code is located in the second coordinate system, the only difference between the two is the heading angle rotation. Since each column (or each row) in the feature matrix represents a different coding angle, the method of calculating similarity by shifting columns (or rows) instead of calculating the heading angle can improve the efficiency of similarity calculation.
[0031] In one possible implementation, the semantic map is searched for map semantic features surrounding the target pose, and the searched map semantic features are registered with the currently observed semantic features until a map semantic feature matching the currently observed semantic feature is obtained, including:
[0032] Using the target pose as the center, the nearest neighbor algorithm is used to search for the semantic features of the semantic map;
[0033] An iterative nearest-point algorithm is used to register the searched map semantic elements with the currently observed semantic elements until a map semantic element that matches the currently observed semantic element is obtained.
[0034] As can be seen from the above scheme, the embodiments of this application first use the target pose as the center and use the nearest neighbor algorithm to search for the map semantic elements in the semantic map, which can reduce the search range and improve the search efficiency. Then, the iterative nearest point algorithm is used to register the searched map semantic elements with the currently observed semantic elements, which can improve the accuracy of element registration.
[0035] In one possible implementation, the vehicle is relocalized based on the currently observed semantic features and the matching map semantic features to obtain the current pose after relocalization, including:
[0036] Based on the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the vehicle's current pose in the first coordinate system, the rotation matrix from the second coordinate system to the first coordinate system, and the current observation noise, a relocation observation model is established, wherein the first coordinate system and the second coordinate system differ only in the rotation of the heading angle.
[0037] The repositioning observation model is solved using the least squares method, and the current pose of the vehicle in the first coordinate system when the optimal solution is obtained is taken as the current pose of the vehicle after repositioning in the first coordinate system.
[0038] As can be seen from the above scheme, when establishing the relocation observation model, this application embodiment not only incorporates the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the current pose of the vehicle in the first coordinate system and the rotation matrix from the second coordinate system to the first coordinate system, but also incorporates the current observation noise, thereby reducing the error caused by the observation noise and further improving the accuracy of relocation.
[0039] Secondly, embodiments of this application provide a relocation device for underground autonomous parking scenarios, the device comprising:
[0040] The acquisition unit is used to acquire historical parking trajectories and map semantic elements from the semantic map;
[0041] A sampling unit is used to sample the historical parking trajectory and use each sampling point as a map-coded pose;
[0042] The encoding unit is used to encode the map semantic elements within a preset range including each map encoding pose to obtain the map encoding features corresponding to each map encoding pose.
[0043] A storage unit is used to store the mapping relationship between the map-coded pose and the map-coded features into a preset storage space;
[0044] The encoding unit is further configured to encode the currently observed semantic elements within the preset range, including the vehicle reference point, to obtain the current observation encoding features, wherein the map semantic elements and the observed semantic elements contain the same type of semantic elements.
[0045] The search unit is used to search for N target map coding poses and N target map coding features from the preset storage space based on the current odometer pose. The current odometer pose is the vehicle pose at the current time calculated based on the odometer recursive algorithm. The N target map coding poses are the N map coding poses that are closest to the current odometer pose. The target map coding features are the map coding features that correspond to the target map coding poses.
[0046] The calculation unit is used to calculate the similarity between the current observation coding feature and each of the target map coding features, and to take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose;
[0047] The registration unit is used to search for map semantic elements around the target pose in the semantic map, and register the searched map semantic elements with the currently observed semantic elements until a map semantic element matching the currently observed semantic element is obtained.
[0048] The relocation unit is used to relocate the vehicle based on the currently observed semantic elements and the map semantic elements that match them, so as to obtain the current pose after relocation.
[0049] In one possible implementation, the coordinates of each of the map semantic elements in the semantic map are coordinates in a first coordinate system with the origin of the historical parking trajectory as the origin.
[0050] The encoding unit includes:
[0051] A determination module is used to determine, for each map coding pose, the map semantic elements within the preset range including the map coding pose in the semantic map;
[0052] The partitioning module is used to establish a second coordinate system with each of the map-coded poses as the origin, and to divide the second coordinate system into sectors at preset distances and preset angles, wherein the first coordinate system and the second coordinate system differ only in the rotation of the heading angle;
[0053] The encoding module is used to encode the map semantic elements in each sector within the preset range to obtain the map encoding feature value of each sector;
[0054] The generation module is used to take the feature matrix composed of the map coding feature values of all sectors within the preset range as the map coding feature corresponding to the map coding pose.
[0055] In one possible implementation, the encoding module is configured to:
[0056] According to the preset semantic element type order, each semantic element is encoded into the 0th to Pth bits of binary, where P+1 is the total number of semantic element types;
[0057] For each sector within the preset range, the semantic element type represented by each binary bit is judged sequentially. When the binary bit to be judged is included in the sector, the binary bit to be judged is set to 1; otherwise, the binary bit to be judged is set to 0, and the binary encoding result is converted into decimal as the map encoding feature value of the sector.
[0058] In one possible implementation, each row element in the feature matrix corresponds to the same distance interval, and each column element corresponds to the same angular interval; or,
[0059] Each row of the feature matrix corresponds to the same angular interval, and each column corresponds to the same distance interval.
[0060] In one possible implementation, the map coding feature and the current observation coding feature correspond to the same coding method;
[0061] The current observation coding feature is referred to as the current observation coding feature matrix, and each of the target map coding features is referred to as the target map coding feature matrix;
[0062] The computing unit includes:
[0063] The translation module is used to perform column translation of each target map encoding feature matrix with different lengths, when each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angle interval, to obtain multiple translated target map encoding feature matrices; or, when each row element in the feature matrix corresponds to the same angle interval and each column element corresponds to the same distance interval, to perform row translation of each target map encoding feature matrix with different lengths, to obtain multiple translated target map encoding feature matrices.
[0064] The calculation module is used to calculate the similarity between the current observation coding feature matrix and each translated target map coding feature matrix, and take the largest similarity as the similarity between the current observation coding feature and the target map coding feature.
[0065] In one possible implementation, the registration unit includes:
[0066] The search module is used to search for the semantic elements of the semantic map using the nearest neighbor algorithm with the target pose as the center.
[0067] The registration module is used to register the searched map semantic features with the currently observed semantic features using an iterative nearest point algorithm until a map semantic feature matching the currently observed semantic feature is obtained.
[0068] In one possible implementation, the relocation unit includes:
[0069] A module is established to build a relocation observation model based on the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the current pose of the vehicle in the first coordinate system, the rotation matrix from the second coordinate system to the first coordinate system, and the current observation noise. The first coordinate system and the second coordinate system differ only in the rotation of the heading angle.
[0070] The repositioning module is used to solve the repositioning observation model using the least squares method, and to take the current pose of the vehicle in the first coordinate system when the optimal solution is obtained as the current pose of the vehicle after repositioning in the first coordinate system.
[0071] As can be seen from the above scheme, the embodiments of this application can first encode the map semantic elements around the sampling points of the historical parking trajectory in the semantic map, and store the mapping relationship between the map encoded pose and the map encoded feature in a preset storage space. Then, the currently observed semantic elements around the vehicle reference point during vehicle driving are encoded to obtain the currently observed encoded features. Then, the N target map encoded poses and their corresponding target map encoded features that are closest to the current odometer pose are searched from the preset storage space. The map encoded pose with the highest similarity to the current observed encoded feature is selected from the N target map encoded poses as the target pose. Finally, the map semantic elements around the target pose are searched in the semantic map to quickly determine the map semantic elements that match the current observed semantic elements. The vehicle is repositioned based on the current observed semantic elements and the map semantic elements that match them. Therefore, compared with directly using the odometer to recursively deduce the vehicle pose as the final positioning result, the positioning error can be greatly reduced and the accuracy of the positioning result can be improved.
[0072] Thirdly, embodiments of this application provide a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the method as described in any possible implementation of the first aspect.
[0073] Fourthly, embodiments of this application provide an electronic device, which includes:
[0074] One or more processors;
[0075] The processor is coupled to a storage device for storing one or more programs;
[0076] When one or more programs are executed by one or more processors, the electronic device performs the method as described in any possible implementation of the first aspect.
[0077] Fifthly, embodiments of this application provide a computer program product containing instructions that, when executed on a computer or processor, cause the computer or processor to perform the method described in any possible implementation of the first aspect. Attached Figure Description
[0078] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.
[0079] Figure 1 A flowchart illustrating a relocation method for an underground autonomous parking scenario provided in an embodiment of this application;
[0080] Figure 2 A flowchart illustrating another relocation method for underground autonomous parking scenarios provided in this application embodiment;
[0081] Figure 3 An example diagram of sector partitioning provided in this application embodiment;
[0082] Figure 4 A block diagram illustrating the composition of a relocation device for an underground autonomous parking scenario, provided in an embodiment of this application;
[0083] Figure 5 This is a schematic diagram of the structure of an electronic device or computer device provided in an embodiment of this application. Detailed Implementation
[0084] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.
[0085] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.
[0086] Figure 1 This is a flowchart illustrating a relocation method for underground autonomous parking scenarios. Figure 2 This is a flowchart illustrating another relocation method for underground autonomous parking scenarios. The following section combines... Figure 1 and Figure 2 This article provides a detailed explanation of the repositioning method:
[0087] This method can be applied to electronic devices or computer equipment, specifically to terminals such as vehicles, and includes:
[0088] S110: Obtain historical parking trajectories and map semantic elements from the semantic map.
[0089] During a user's first manual parking session, the system can record the user's historical parking trajectory and the semantic elements surrounding that trajectory. Based on the historical parking trajectory and semantic elements, a semantic map can be constructed. This semantic map can include a high-precision semantic map or a semantic map that meets the accuracy requirements of parking.
[0090] Historical parking trajectories include multiple historical parking poses, with poses including vehicle position and vehicle attitude. Map semantic elements include parking spaces, pillars, walls, ground arrows, zebra crossings, and speed bumps, each of which can be represented by a set of 3D points, assigned the aforementioned semantic element attributes. The coordinates of each map semantic element in the semantic map are in a first coordinate system with the origin of the historical parking trajectory as the origin. Each trajectory point in the historical parking trajectory can also be in a first coordinate system with the origin of the historical parking trajectory as the origin. The first coordinate system can be a northeast-sky coordinate system or any other coordinate system, as long as the first coordinate system differs from the subsequent second coordinate system only by a rotation of the heading angle. In the northeast-sky coordinate system, the x-axis points due east, the y-axis points due north, and the z-axis is perpendicular to the xy-plane and points upwards.
[0091] S120: Sample historical parking trajectories and use each sampled point as a map coding pose. Encode the map semantic elements within a preset range including each map coding pose to obtain the map coding features corresponding to each map coding pose. Store the mapping relationship between map coding poses and map coding features in a preset storage space.
[0092] After loading the historical parking trajectories and map semantic features from the semantic map, the electronic device can sample the historical parking trajectories uniformly or non-uniformly, for example, every k meters. Each sampling point is then used as a map coding pose to encode the map semantic features within a preset range including each map coding pose, and the encoding results are stored in a preset storage space, such as a database.
[0093] The specific implementation method of the step "encoding the map semantic elements within a preset range including each map coding pose to obtain the map coding features corresponding to each map coding pose" may include steps A1-A3:
[0094] A1: For each map-coded pose, determine the map semantic elements within a preset range that include the map-coded pose in the semantic map.
[0095] The area enclosed by the preset range can be circular, square, or any other shape. The map encoding pose can be the center point of the preset range, or it can be a non-center point, as long as the relative position of the map encoding pose within the preset range remains consistent with the relative position of the vehicle reference point within the preset range in subsequent steps. For example, map semantic elements within a circular area can be obtained using each map encoding pose as the center and r as the radius.
[0096] It should be noted that, when sampling uniformly, this embodiment of the application does not restrict the relationship between the sampling interval and the radius or length of the preset range, as long as the accuracy of the final repositioning meets the accuracy requirements.
[0097] A2: Establish a second coordinate system with each map coding pose as the origin. Divide the second coordinate system into sectors at preset distances and preset angles, and encode the map semantic elements in each sector within the preset range to obtain the map coding feature value of each sector.
[0098] The first coordinate system and the second coordinate system differ only in the rotation of the heading angle. For example, when the first coordinate system is a north-north-sky coordinate system, the second coordinate system can be a polar coordinate system or a Cartesian coordinate system.
[0099] The following explanation uses the first coordinate system as the northeast-central coordinate system and the second coordinate system as the polar coordinate system as an example to illustrate the coordinate relationship:
[0100] Since all map semantic elements are located on the ground, a planar polar coordinate system is established with the sampling points as the center, and each map semantic element is expressed in the planar polar coordinate system:
[0101]
[0102] In the formula, ρ and θ represent the distance and angle of the map semantic features in the polar coordinate system, respectively; atan is the arctangent function; x and y are vectors composed of the map semantic features and sampling points, calculated as follows:
[0103] x = x n -x c ,y=y n -y c
[0104] Where, x n y n x represents the coordinates of the map semantic features in the northeast celestial coordinate system. c y c The coordinates of the sampling center in the northeast celestial coordinate system are given.
[0105] After establishing the second coordinate system, it is divided into sectors at preset distances and preset angles, resulting in multiple sectors. Then, the map semantic elements within each sector within the preset range can be encoded to obtain the map coding feature value for each sector. The specific values of the preset distance and preset angle can be determined based on practical experience; for example, the preset distance could be 0.5m and the preset angle could be 6 degrees. Figure 3 As shown, a pre-defined circular area can be divided into 16 sectors.
[0106] The method for encoding each sector may include: encoding each semantic element into binary bits 0 to P according to a preset semantic element type order, where P+1 is the total number of semantic element types; for each sector within a preset range, judging the semantic element type represented by each binary bit in turn; when the binary bit to be judged is included in the sector, setting the binary bit to be judged to 1; otherwise, setting the binary bit to be judged to 0, and converting the binary encoding result into decimal as the map encoding feature value of the sector.
[0107] When there are multiple semantic element types, the preset order of semantic element types can be set arbitrarily.
[0108] The following example illustrates the sector coding process using the sequence "parking space, pillar, wall, ground arrow, zebra crossing, and speed bump":
[0109] For sector 1, which only includes parking spaces, walls, and speed bumps, the binary result is 100101, which is 37 in decimal.
[0110] A3: The feature matrix composed of map coding feature values of all sectors within the preset range is used as the map coding feature corresponding to the map coding pose.
[0111] In this feature matrix, each row corresponds to the same distance interval, and each column corresponds to the same angular interval; or,
[0112] Each row of the feature matrix corresponds to the same angular interval, and each column corresponds to the same distance interval.
[0113] If Figure 3 The eigenvalues of the 16 sectors are named a1-a1 respectively. 16 Then, when each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angular interval, the feature matrix can be represented as:
[0114]
[0115] When each row element in the feature matrix corresponds to the same angular interval, and each column element corresponds to the same distance interval, the feature matrix can be represented as:
[0116]
[0117] The above scheme establishes a second coordinate system with each sampling point as the origin, differing from the first coordinate system only by the heading angle. The second coordinate system is then divided into sectors, and the map semantic elements in each sector are encoded. The feature matrix composed of the map encoding feature values in all sectors is used as the map encoding feature corresponding to the map encoding pose. This can improve the efficiency and accuracy of subsequent calculations of the similarity between the map encoding features and the currently observed encoding features.
[0118] S130: Encode the semantic elements of the current observation within a preset range, including the vehicle reference point, to obtain the current observation coding features.
[0119] The map semantic elements and the observation semantic elements contain the same types of semantic elements, such as parking spaces, pillars, walls, ground arrows, zebra crossings, and speed bumps. The map coding features and the current observation coding features use the same coding method, which can be obtained by using the coding method described in steps A1-A3 above.
[0120] During vehicle operation, a vehicle reference point can be determined first, followed by the acquisition of currently observed semantic elements within a preset range including the vehicle reference point. This vehicle reference point can be any point on the vehicle, typically the midpoint of the rear axle or the vehicle's geometric center. The vehicle reference point can be the center point of the preset range, or it can be a different center point, as long as its relative position within the preset range matches the relative position of the map-encoded pose within the preset range obtained in the preceding steps.
[0121] The current observation semantic elements are the semantic elements perceived at the current moment by the vehicle's sensors. For example, the perception fusion results from image sensors and LiDAR can be used as the current observation semantic elements.
[0122] S140: Based on the current odometer pose, search for N target map coding poses and N target map coding features from the preset storage space.
[0123] Among them, the current odometer pose is the vehicle pose at the current moment calculated based on the odometer recursive algorithm, the N target map coding poses are the N map coding poses that are closest to the current odometer pose, and the target map coding features are the map coding features corresponding to the target map coding poses.
[0124] Odometer recursive algorithms typically involve calculating speed and pose. Speed calculation is based on wheel speed, using data collected by the encoder to calculate the angle rotated by the motor output shaft, thus obtaining the angular velocity, which is the foundation of odometer calculation. Pose calculation involves position and orientation, and is obtained by integrating speed data.
[0125] After obtaining the current odometry pose, the nearest neighbor algorithm can be used to search for the N target map coding poses that are closest to the current odometry pose in the preset storage space. Then, through the mapping relationship between map coding poses and map coding features, the N target map coding features corresponding to the N target map coding poses can be determined.
[0126] S150: Calculate the similarity between the current observed coding feature and each target map coding feature, and take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose.
[0127] Since encoded features can be represented by feature matrices, the currently observed encoded features can be called the current observed encoded feature matrix, and each target map encoded feature can be called the target map encoded feature matrix. In this case, methods for calculating the similarity between the currently observed encoded features and each target map encoded feature can include:
[0128] If each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angular interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by column of different lengths to obtain multiple shifted target map encoding feature matrices. Alternatively, if each row element in the feature matrix corresponds to the same angular interval and each column element corresponds to the same distance interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by row of different lengths to obtain multiple shifted target map encoding feature matrices. The similarity between the current observed encoding feature matrix and each shifted target map encoding feature matrix is calculated, and the largest similarity is taken as the similarity between the current observed encoding feature and the target map encoding feature.
[0129] Specifically, when performing column shifts on different lengths, all columns can be shifted to the right or to the left. When the feature matrix includes m rows and n columns, the shift step size ranges from shifting 0 columns to shifting n columns. When performing row shifts on different lengths, all rows can be shifted downwards or upwards. When the feature matrix includes m rows and n columns, the shift step size ranges from shifting 0 rows to shifting m rows.
[0130] Taking a rightward translation as an example, the feature matrix consists of m rows and n columns. The feature matrix f encodes the i-th target map. i Each time, shifting the target map by u columns to the right (0≤u≤n, 1≤i≤N) yields the translated target map encoding feature matrix. Encode the feature matrix of the target map after each translation. With the current observation encoded feature matrix f s Calculate similarity Similarity The calculation formula may include:
[0131]
[0132] Where j represents the j-th column in the characteristic matrix, and ||||2 represents the L2 norm. At this point, f i and f s The similarity is defined as
[0133] After obtaining the similarity between the currently observed coded features and the coded features of each target map, a similarity set S = {S 1,s S 2,s S N,s The target map pose can be obtained from the set of similarities by taking the target map coding feature with the highest similarity as the target pose.
[0134] Since the map encoding is located in the first coordinate system and the observation encoding is located in the second coordinate system, the only difference between the two is the heading angle rotation. Since each column (or each row) in the feature matrix represents a different encoding angle, the efficiency of calculating similarity can be improved by using column (or row) translation to calculate similarity instead of calculating the heading angle.
[0135] S160: Search for map semantic features around the target pose in the semantic map, and register the searched map semantic features with the currently observed semantic features until a map semantic feature matching the currently observed semantic feature is obtained.
[0136] To improve registration efficiency, the target pose can be used as the center to search for map semantic features in the semantic map using the nearest neighbor algorithm. Then, the Iterative Closest Point (ICP) algorithm is used to register the searched map semantic features with the currently observed semantic features until a map semantic feature matching the currently observed semantic feature is obtained.
[0137] S170: Based on the currently observed semantic elements and the matching map semantic elements, the vehicle is relocalized to obtain the current pose after relocalization.
[0138] The specific implementation methods for relocation may include:
[0139] Based on the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the vehicle's current pose in the first coordinate system, the rotation matrix from the second coordinate system to the first coordinate system, and the current observation noise, a relocation observation model is established, wherein the first coordinate system and the second coordinate system differ only in the rotation of the heading angle. The relocation observation model is solved using the least squares method, and the vehicle's current pose in the first coordinate system when the optimal solution is obtained is taken as the vehicle's current pose after relocation in the first coordinate system.
[0140] The relocation observation model includes:
[0141]
[0142] Where, p k P k r represents the currently observed semantic element and the map semantic element that matches the currently observed semantic element, respectively. b This indicates the vehicle's current pose in the first coordinate system. Let η represent the rotation matrix from the second coordinate system to the first coordinate system. k The current observation noise can be represented by an empirical value of Gaussian white noise, b represents the first coordinate system, and k represents the index of the observation semantic element.
[0143] In this relocation observation model, p k P k η k Given the parameters, the least squares method can be used to solve the problem. and r b This completes the vehicle repositioning.
[0144] In this embodiment of the application, when establishing the relocation observation model, not only are the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the current pose of the vehicle in the first coordinate system and the rotation matrix from the second coordinate system to the first coordinate system incorporated, but the current observation noise is also incorporated, which can reduce the error caused by the observation noise and further improve the accuracy of relocation.
[0145] The relocalization method for underground autonomous parking scenarios provided in this application first encodes the map semantic elements around the sampling points of historical parking trajectories in the semantic map, and stores the mapping relationship between the map encoded pose and the map encoded features in a preset storage space. Then, it encodes the currently observed semantic elements around the vehicle reference point during vehicle movement to obtain the currently observed encoded features. It then searches the preset storage space for the N target map encoded poses and their corresponding target map encoded features that are closest to the current odometer pose. From the N target map encoded poses, it selects the map encoded pose with the highest similarity to the currently observed encoded features as the target pose. Finally, it searches the semantic map for map semantic elements around the target pose to quickly determine the map semantic elements that match the currently observed semantic elements. Based on the currently observed semantic elements and the map semantic elements that match them, it relocalizes the vehicle. Compared with directly using the odometer to recursively deduce the vehicle pose as the final localization result, this method can greatly reduce localization errors and improve the accuracy of localization results.
[0146] Based on the above method embodiments, another embodiment of this application provides a relocation device for underground autonomous parking scenarios. This device can be applied to electronic devices or computer devices, specifically to terminals such as vehicles. Figure 4 As shown, the device includes:
[0147] Acquisition unit 210 is used to acquire historical parking trajectories and map semantic elements in the semantic map;
[0148] The sampling unit 220 is used to sample the historical parking trajectory and use each sampling point as a map-coded pose;
[0149] Encoding unit 230 is used to encode the map semantic elements within a preset range including each map coding pose to obtain map coding features corresponding to each map coding pose.
[0150] Storage unit 240 is used to store the mapping relationship between the map coding pose and the map coding features into a preset storage space;
[0151] The encoding unit 230 is further configured to encode the current observation semantic elements within the preset range, including the vehicle reference point, to obtain the current observation encoding features, wherein the map semantic elements and the observation semantic elements contain the same type of semantic elements.
[0152] Search unit 250 is used to search for N target map coding poses and N target map coding features from the preset storage space based on the current odometer pose. The current odometer pose is the vehicle pose at the current time calculated based on the odometer recursive algorithm. The N target map coding poses are the N map coding poses that are closest to the current odometer pose. The target map coding features are the map coding features that correspond to the target map coding poses.
[0153] The calculation unit 260 is used to calculate the similarity between the current observation coding feature and each of the target map coding features, and to take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose;
[0154] The registration unit 270 is used to search for map semantic elements around the target pose in the semantic map, and register the searched map semantic elements with the currently observed semantic elements until a map semantic element matching the currently observed semantic element is obtained.
[0155] The relocation unit 280 is used to relocate the vehicle based on the currently observed semantic elements and the map semantic elements that match them, so as to obtain the current pose after relocation.
[0156] In one possible implementation, the coordinates of each of the map semantic elements in the semantic map are coordinates in a first coordinate system with the origin of the historical parking trajectory as the origin.
[0157] The encoding unit 230 includes:
[0158] A determination module is used to determine, for each map coding pose, the map semantic elements within the preset range including the map coding pose in the semantic map;
[0159] The partitioning module is used to establish a second coordinate system with each of the map-coded poses as the origin, and to divide the second coordinate system into sectors at preset distances and preset angles, wherein the first coordinate system and the second coordinate system differ only in the rotation of the heading angle;
[0160] The encoding module is used to encode the map semantic elements in each sector within the preset range to obtain the map encoding feature value of each sector;
[0161] The generation module is used to take the feature matrix composed of the map coding feature values of all sectors within the preset range as the map coding feature corresponding to the map coding pose.
[0162] In one possible implementation, the encoding module is configured to:
[0163] According to the preset semantic element type order, each semantic element is encoded into the 0th to Pth bits of binary, where P+1 is the total number of semantic element types;
[0164] For each sector within the preset range, the semantic element type represented by each binary bit is judged sequentially. When the binary bit to be judged is included in the sector, the binary bit to be judged is set to 1; otherwise, the binary bit to be judged is set to 0, and the binary encoding result is converted into decimal as the map encoding feature value of the sector.
[0165] In one possible implementation, each row element in the feature matrix corresponds to the same distance interval, and each column element corresponds to the same angular interval; or,
[0166] Each row of the feature matrix corresponds to the same angular interval, and each column corresponds to the same distance interval.
[0167] In one possible implementation, the map coding feature and the current observation coding feature correspond to the same coding method;
[0168] The current observation coding feature is referred to as the current observation coding feature matrix, and each of the target map coding features is referred to as the target map coding feature matrix;
[0169] The computing unit 260 includes:
[0170] The translation module is used to perform column translation of each target map encoding feature matrix with different lengths, when each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angle interval, to obtain multiple translated target map encoding feature matrices; or, when each row element in the feature matrix corresponds to the same angle interval and each column element corresponds to the same distance interval, to perform row translation of each target map encoding feature matrix with different lengths, to obtain multiple translated target map encoding feature matrices.
[0171] The calculation module is used to calculate the similarity between the current observation coding feature matrix and each translated target map coding feature matrix, and take the largest similarity as the similarity between the current observation coding feature and the target map coding feature.
[0172] In one possible implementation, the registration unit 270 includes:
[0173] The search module is used to search for the semantic elements of the semantic map using the nearest neighbor algorithm with the target pose as the center.
[0174] The registration module is used to register the searched map semantic features with the currently observed semantic features using an iterative nearest point algorithm until a map semantic feature matching the currently observed semantic feature is obtained.
[0175] In one possible implementation, the relocation unit 280 includes:
[0176] A module is established to build a relocation observation model based on the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the current pose of the vehicle in the first coordinate system, the rotation matrix from the second coordinate system to the first coordinate system, and the current observation noise. The first coordinate system and the second coordinate system differ only in the rotation of the heading angle.
[0177] The repositioning module is used to solve the repositioning observation model using the least squares method, and to take the current pose of the vehicle in the first coordinate system when the optimal solution is obtained as the current pose of the vehicle after repositioning in the first coordinate system.
[0178] The relocalization device for underground autonomous parking scenarios provided in this application embodiment can first encode the map semantic elements around the sampling points of historical parking trajectories in the semantic map, and store the mapping relationship between the map encoded pose and the map encoded features in a preset storage space. Then, it encodes the currently observed semantic elements around the vehicle reference point during vehicle movement to obtain the currently observed encoded features. It then searches the preset storage space for the N target map encoded poses and their corresponding target map encoded features that are closest to the current odometer pose. From the N target map encoded poses, it selects the map encoded pose with the highest similarity to the currently observed encoded features as the target pose. Finally, it searches the semantic map for map semantic elements around the target pose to quickly determine the map semantic elements that match the currently observed semantic elements. Based on the currently observed semantic elements and the map semantic elements that match them, it relocalizes the vehicle. Compared with directly using the odometer to recursively deduce the vehicle pose as the final localization result, this device can greatly reduce localization errors and improve the accuracy of localization results.
[0179] Based on the above method embodiments, another embodiment of this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the above embodiments.
[0180] Based on the above method embodiments, another embodiment of this application provides an electronic device or computer device, such as... Figure 5 As shown, it includes:
[0181] One or more processors 310;
[0182] The processor 310 is coupled to a storage device 320, the storage device 320 being used to store one or more programs;
[0183] When the one or more programs are executed by the one or more processors 310, the electronic device or computer device performs the method as described in any of the above embodiments.
[0184] Based on the above embodiments, another embodiment of this application provides a computer program product, which includes instructions that, when executed on a computer or processor, cause the computer or processor to perform the method described in any of the above embodiments.
[0185] The above-described device and system embodiments correspond to the method embodiments and have the same technical effects. For detailed descriptions, please refer to the method embodiments. The device embodiments are derived from the method embodiments; detailed descriptions can be found in the method embodiments section, and will not be repeated here. Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing this application.
[0186] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.
[0187] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.
Claims
1. A relocation method for underground autonomous parking scenarios, characterized in that, The method includes: Obtain historical parking trajectories and map semantic elements from the semantic map; The historical parking trajectory is sampled, and each sampled point is used as a map coding pose. The map semantic elements within a preset range including each map coding pose are encoded to obtain the map coding features corresponding to each map coding pose. The mapping relationship between the map coding pose and the map coding features is stored in a preset storage space. Encode the current observation semantic elements within the preset range, including vehicle reference points, to obtain current observation coding features, wherein the map semantic elements and the observation semantic elements contain the same type of semantic elements; Based on the current odometer pose, search for N target map coding poses and N target map coding features from the preset storage space. The current odometer pose is the vehicle pose at the current time calculated based on the odometer recursive algorithm. The N target map coding poses are the N map coding poses that are closest to the current odometer pose. The target map coding features are the map coding features that correspond to the target map coding poses. Calculate the similarity between the current observed coding feature and each of the target map coding features, and take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose; Search the semantic map for map semantic features around the target pose, and register the searched map semantic features with the currently observed semantic features until a map semantic feature matching the currently observed semantic feature is obtained; Based on the currently observed semantic elements and the matching map semantic elements, the vehicle is repositioned to obtain its current pose after repositioning.
2. The method of claim 1, wherein, The coordinates of each of the map semantic elements in the semantic map are the coordinates in a first coordinate system with the starting point of the historical parking trajectory as the origin; Encoding the map semantic elements within a preset range including each map coding pose to obtain map coding features corresponding to each map coding pose includes: For each map coding pose, determine the map semantic elements within the preset range including the map coding pose in the semantic map; A second coordinate system is established with each map coding pose as the origin. The second coordinate system is divided into sectors at preset distances and preset angles. The map semantic elements in each sector within the preset range are encoded to obtain the map coding feature value of each sector. The first coordinate system and the second coordinate system differ only in the rotation of the heading angle. The feature matrix composed of the map coding feature values of all sectors within the preset range is used as the map coding feature corresponding to the map coding pose.
3. The method according to claim 2, characterized in that, Encoding the map semantic elements in each sector within the preset range to obtain the map coding feature value of each sector includes: According to the preset semantic element type order, each semantic element is encoded into the 0th to Pth bits of binary, where P+1 is the total number of semantic element types; For each sector within the preset range, the semantic element type represented by each binary bit is judged sequentially. When the binary bit to be judged is included in the sector, the binary bit to be judged is set to 1; otherwise, the binary bit to be judged is set to 0, and the binary encoding result is converted into decimal as the map encoding feature value of the sector.
4. The method of claim 2, wherein, Each row of the feature matrix corresponds to the same distance interval, and each column corresponds to the same angular interval; or, Each row of the feature matrix corresponds to the same angular interval, and each column corresponds to the same distance interval.
5. The method of claim 4, wherein, The map coding features and the current observation coding features correspond to the same coding method; The current observation coding feature is referred to as the current observation coding feature matrix, and each of the target map coding features is referred to as the target map coding feature matrix; Calculate the similarity between the current observed coded feature and each of the target map coded features, including: When each row element in the feature matrix corresponds to the same distance interval and each column element corresponds to the same angle interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by column of different lengths to obtain multiple shifted target map encoding feature matrices; or, when each row element in the feature matrix corresponds to the same angle interval and each column element corresponds to the same distance interval, for each target map encoding feature matrix, the target map encoding feature matrix is shifted by row of different lengths to obtain multiple shifted target map encoding feature matrices. Calculate the similarity between the current observation coding feature matrix and each translated target map coding feature matrix, and take the largest similarity as the similarity between the current observation coding feature and the target map coding feature.
6. The method of claim 1, wherein, Searching for map semantic features around the target pose in the semantic map, and registering the searched map semantic features with the currently observed semantic features, until a map semantic feature matching the currently observed semantic feature is obtained, including: Using the target pose as the center, the nearest neighbor algorithm is used to search for the semantic features of the semantic map; An iterative nearest-point algorithm is used to register the searched map semantic elements with the currently observed semantic elements until a map semantic element that matches the currently observed semantic element is obtained.
7. The method according to any one of claims 1 to 6, characterized in that, Based on the currently observed semantic features and the matching map semantic features, the vehicle is relocalized to obtain its current pose after relocalization, including: Based on the current observation semantic elements, the map semantic elements that match the current observation semantic elements, the vehicle's current pose in the first coordinate system, the rotation matrix from the second coordinate system to the first coordinate system, and the current observation noise, a relocation observation model is established, wherein the first coordinate system and the second coordinate system differ only in the rotation of the heading angle. The repositioning observation model is solved using the least squares method, and the current pose of the vehicle in the first coordinate system when the optimal solution is obtained is taken as the current pose of the vehicle after repositioning in the first coordinate system.
8. A relocation device for underground autonomous parking scenarios, characterized in that The device includes: The acquisition unit is used to acquire historical parking trajectories and map semantic elements from the semantic map; A sampling unit is used to sample the historical parking trajectory and use each sampling point as a map-coded pose; The encoding unit is used to encode the map semantic elements within a preset range including each map encoding pose to obtain the map encoding features corresponding to each map encoding pose. A storage unit is used to store the mapping relationship between the map-coded pose and the map-coded features into a preset storage space; The encoding unit is further configured to encode the currently observed semantic elements within the preset range, including the vehicle reference point, to obtain the current observation encoding features, wherein the map semantic elements and the observed semantic elements contain the same type of semantic elements. The search unit is used to search for N target map coding poses and N target map coding features from the preset storage space based on the current odometer pose. The current odometer pose is the vehicle pose at the current time calculated based on the odometer recursive algorithm. The N target map coding poses are the N map coding poses that are closest to the current odometer pose. The target map coding features are the map coding features that correspond to the target map coding poses. The calculation unit is used to calculate the similarity between the current observation coding feature and each of the target map coding features, and to take the target map coding pose corresponding to the target map coding feature with the highest similarity as the target pose; The registration unit is used to search for map semantic elements around the target pose in the semantic map, and register the searched map semantic elements with the currently observed semantic elements until a map semantic element matching the currently observed semantic element is obtained. The relocation unit is used to relocate the vehicle based on the currently observed semantic elements and the map semantic elements that match them, so as to obtain the current pose after relocation.
9. A computer readable storage medium having stored thereon a computer program, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-7.
10. An electronic device, comprising: The electronic device includes: One or more processors; The processor is coupled to a storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the electronic device performs the method as described in any one of claims 1-7.