Coarse-to-fine positioning initialization method and system

Abstract

The invention discloses a coarse-to-fine positioning initialization method and a coarse-to-fine positioning initialization system. The coarse-to-fine positioning initialization method comprises the following steps: generating a prior map; obtaining vehicle initialization information; obtaining a current radar point cloud frame, and carrying out voxel map matching on the current radar point cloud frame and the multi-resolution voxel map based on a branch and bound algorithm and the vehicle initialization information; when the effective point cloud proportion of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset proportion threshold value, taking the optimal matching position as a coarse registration initial value for positioning initialization and outputting the optimal matching position; based on a Small-GICP fine registration module, according to the coarse registration initial value and the point cloud map, obtaining a fine registration initial value of positioning initialization, and based on a DR module, according to current vehicle IMU data and current vehicle wheel speed data fusion, obtaining a pose of an odometer, and based on the fine registration initial value, carrying out dead reckoning according to the pose of the odometer. And obtaining a vehicle positioning initialization pose.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving positioning technology, and more specifically, to a coarse-to-fine positioning initialization method and system thereof. Background Art

[0002] As the technology of the autonomous driving industry matures, unmanned driving technology has brought revolutionary impacts to the sanitation industry, which can improve the operating efficiency and safety of sanitation vehicles, reduce labor costs and training costs, improve service quality and coverage, and promote the intelligence and digitalization of the sanitation industry. For unmanned vehicles such as unmanned sweepers, an accurate positioning initialization module is the basis and guarantee of the entire autonomous driving system. In the prior art, the common positioning initialization methods for outdoor scenes are: using GPS (Global Positioning System) to achieve positioning initialization and using historical track points to achieve positioning initialization. Among them, for unmanned vehicles operating outdoors, if the GPS signal is good, the GPS positioning initialization method can obtain a positioning result with high accuracy, but when the vehicle is in a tunnel or urban canyon with weak GPS signals or even no GPS signals, the positioning initialization method will fail. Unmanned vehicles can record the exact position of the vehicle in the map through historical track points. When the vehicle is hot-started again, the historical track point positioning initialization method can achieve positioning initialization according to the posture of the historical track points, but when the vehicle encounters a problem or the vehicle is cold-started without any prior information, the historical track will be completely invalid. Summary of the invention

[0003] The present application provides a coarse-to-fine positioning initialization method and system thereof, which solves the problems in the prior art of the positioning initialization method failing due to weak or even no GPS signal, and the inability to initialize positioning due to the failure of historical tracks.

[0004] The specific technical solutions are as follows:

[0005] In a first aspect, an embodiment of the present application provides a coarse-to-fine positioning initialization method, including:

[0006] Generate a priori map; wherein the priori map includes a point cloud map and a multi-resolution voxel map;

[0007] Acquire vehicle initialization information; wherein the vehicle initialization information includes vehicle initial position, vehicle initial Roll angle information and vehicle initial Pitch angle information;

[0008] Acquire a current radar point cloud frame, and perform voxel map matching between the current radar point cloud frame and the multi-resolution voxel map based on a branch and bound algorithm and the vehicle initialization information;

[0009] When the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset ratio threshold, the optimal matching position is used as a rough registration initial value for positioning initialization and output;

[0010] Based on the Small-GICP (Small-Generalized Iterative Closest Point) fine registration module, the fine registration initial value for positioning initialization is obtained according to the coarse registration initial value and the point cloud map, and based on the DR (Dead-reckoning) module, the odometer posture is obtained according to the fusion of the current vehicle IMU (Inertial Measurement Unit) data and the current vehicle wheel speed data, so as to perform dead reckoning based on the fine registration initial value and the posture of the odometer to obtain the vehicle positioning initialization posture.

[0011] In some embodiments of the present application, it also includes:

[0012] When all positions in the multi-resolution voxel map are traversed, and the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, a new current radar point cloud frame is reacquired, and voxel map matching is performed on the new current radar point cloud frame and the multi-resolution voxel map based on the branch and bound algorithm and the vehicle initialization information until the coarse alignment initial value for positioning initialization is output.

[0013] In some embodiments of the present application, generating a priori map specifically includes:

[0014] Acquire the point cloud map, and generate the multi-resolution voxel map according to the point cloud map;

[0015] The prior map is generated according to the point cloud map and the multi-resolution voxel map.

[0016] In some embodiments of the present application, obtaining vehicle initialization information specifically includes:

[0017] Acquire single-point GPS information, and obtain the initial position of the vehicle according to the single-point GPS information; wherein the initial position of the vehicle includes an initial X coordinate, an initial Y coordinate and an initial Yaw angle;

[0018] Acquire stationary state IMU data, and acquire the vehicle initial Roll angle information and the vehicle initial Pitch angle information according to the stationary state IMU data.

[0019] In some embodiments of the present application, the acquiring of the current radar point cloud frame and performing voxel map matching between the current radar point cloud frame and the multi-resolution voxel map based on a branch and bound algorithm and the vehicle initialization information specifically includes:

[0020] Acquire the current radar point cloud frame;

[0021] Based on the branch and bound algorithm, and using the vehicle initialization information, a search area of ​​the current radar point cloud frame in the multi-resolution voxel map is set, and an optimal position of the current radar point cloud frame in each layer is sequentially searched from a low-resolution layer to a high-resolution layer of the multi-resolution voxel map;

[0022] Obtaining the effective point cloud ratio of the highest resolution layer of the current radar point cloud frame in the current search area, and determining whether the effective point cloud ratio is greater than the preset ratio threshold;

[0023] If the effective point cloud ratio is greater than the preset ratio threshold, taking the optimal position of the current radar point cloud frame at the highest resolution layer of the current search area as the optimal matching position;

[0024] If the effective point cloud ratio is not greater than the preset ratio threshold, the adjacent area of ​​the current search area is reset as the search area of ​​the current radar point cloud frame in the multi-resolution voxel map, and the optimal position of the current radar point cloud frame in each layer is searched in sequence from the low-resolution layer to the high-resolution layer of the multi-resolution voxel map, and the effective point cloud ratio of the current radar point cloud frame in the highest resolution layer of the current search area is obtained, until the effective point cloud ratio is greater than the preset ratio threshold, and the traversal is terminated;

[0025] When all positions in the multi-resolution voxel map are traversed, if the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, it is determined that the initialization positioning of the current radar point cloud frame has failed, and a new current radar point cloud frame is reacquired for voxel map matching until the optimal matching position is obtained.

[0026] In some embodiments of the present application, the Small-GICP-based fine registration module obtains the initial fine registration value for positioning initialization according to the initial coarse registration value and the point cloud map, specifically including:

[0027] Based on the Small-GICP fine registration module, the point cloud of the current radar point cloud frame is projected to the MAP coordinate system (map coordinate system) of the point cloud map according to the rough registration initial value, the corresponding relationship between the corresponding point cloud between the current radar point cloud frame and the point cloud map is obtained, and the optimization result is obtained by iterative solution, and the optimization result is used as the fine registration initial value for positioning initialization and output.

[0028] In some embodiments of the present application, obtaining the optimization result by iterative solution specifically includes:

[0029] By iterative solution, when the residual between the corresponding relationships is not greater than a preset residual threshold, the optimization result is obtained.

[0030] In a second aspect, an embodiment of the present application provides a coarse-to-fine positioning initialization system, including:

[0031] A priori map generation module, used to generate a priori map; wherein the priori map includes a point cloud map and a multi-resolution voxel map;

[0032] An initialization information acquisition module is used to acquire vehicle initialization information; wherein the vehicle initialization information includes vehicle initial position, vehicle initial Roll angle information and vehicle initial Pitch angle information;

[0033] A coarse initialization positioning module is used to obtain a current radar point cloud frame, and based on a branch and bound algorithm and the vehicle initialization information, perform voxel map matching between the current radar point cloud frame and the multi-resolution voxel map, and when the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset ratio threshold, the optimal matching position is used as a coarse registration initial value for positioning initialization and output;

[0034] The fine initialization positioning module is used to obtain the initial fine registration value for positioning initialization based on the coarse registration initial value and the point cloud map based on the Small-GICP fine registration module, and to obtain the odometer posture based on the fusion of the current vehicle IMU data and the current vehicle wheel speed data based on the DR module, so as to perform dead reckoning based on the odometer posture based on the initial fine registration value to obtain the vehicle positioning initialization posture.

[0035] In a third aspect, an embodiment of the present application provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the coarse-to-fine positioning initialization method as described in the first aspect is implemented.

[0036] In a fourth aspect, an embodiment of the present application provides an electronic device, the electronic device comprising a processor and a memory;

[0037] The processor is coupled to the memory, the memory is used to store a computer program, and the computer program is executed by the processor to enable the electronic device to implement the coarse-to-fine positioning initialization method as described in the first aspect.

[0038] In a fifth aspect, an embodiment of the present application provides a computer program product, which includes instructions. When the instructions are executed on a computer or a processor, the computer or the processor executes the coarse-to-fine positioning initialization method as described in the first aspect.

[0039] The beneficial effects of the embodiments of the present application are as follows:

[0040] This positioning initialization method uses a multi-resolution voxel map for fuzzy initialization positioning to obtain the initial value of rough registration for positioning initialization. In this process, the search space of the initialization position and attitude is narrowed by single-point GPS information and static IMU data, and the point cloud map is used to initialize high-precision positioning. At the same time, the DR module is used to achieve high-frequency initialization of high-precision positioning, reducing the vehicle-side resource occupation, thereby solving the problem of high vehicle-side resource occupation during the positioning initialization stage, and greatly saving the cost of vehicle accessories, and optional or abandoned combined navigation. In addition, this positioning initialization method can achieve high-efficiency initialization positioning attitude output with high real-time performance, strong robustness, and high positioning accuracy, solving the problem of failure of the positioning initialization method due to weak or even no GPS signal, and the problem of failure of positioning initialization due to failure of historical trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following briefly introduces the drawings required for use in the embodiments or the prior art descriptions. Obviously, the drawings described below are only some embodiments of the present application, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0042] Figure 1 A flowchart of a method for initializing positioning from coarse to fine provided in an embodiment of the present application;

[0043] Figure 2 A block diagram of a coarse-to-fine positioning initialization system provided in an embodiment of the present application. DETAILED DESCRIPTION

[0044] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0045] It should be noted that, in the absence of conflict, the embodiments in the present application and the features in the embodiments can be combined with each other. The terms "including" and "having" in the embodiments of the present application and the accompanying drawings and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device comprising a series of steps or units is not limited to the listed steps or units, but optionally also includes steps or units that are not listed, or optionally also includes other steps or units inherent to these processes, methods, products or devices.

[0046] The embodiment of the present application discloses a method for initializing positioning from coarse to fine, which is highly robust and can achieve highly real-time and efficient positioning initialization posture output.

[0047] Figure 1 A coarse-to-fine positioning initialization method provided according to an embodiment of the present application is shown.

[0048] like Figure 1 As shown, the positioning initialization method includes the following steps:

[0049] Step S100, generating a priori map.

[0050] In the embodiment of the present application, the positioning initialization method mainly uses the generated prior map to perform fuzzy initialization positioning and initialize high-precision positioning. The prior map includes a point cloud map and a multi-resolution voxel map. The multi-resolution voxel map is used to implement the fuzzy initialization positioning of the positioning initialization method, and the point cloud map is used to implement the initialization high-precision positioning of the positioning initialization method.

[0051] In some embodiments, a point cloud map is obtained, and a multi-resolution voxel map is generated based on the point cloud map; a priori map is generated based on the point cloud map and the multi-resolution voxel map. Specifically, the positioning initialization method can directly obtain an already generated offline point cloud map, and then save the multi-resolution offline map point cloud distribution information by extracting voxel features, and construct voxel maps of several layers of resolution (i.e., multi-resolution voxel maps).

[0052] It should be noted that this application does not restrict the method of generating point cloud maps. Data can be collected through a collection vehicle, and the point cloud map can be expanded through the results of a high-precision combined navigation device. The simultaneous localization and mapping (SLAM) method can also be used to build an offline point cloud map by fusing sensor data such as IMU sensors, wheel speed sensors, combined navigation devices, and lidar.

[0053] Step S200, obtaining vehicle initialization information.

[0054] In the embodiment of the present application, the positioning initialization method also uses vehicle initialization information to narrow the search range to improve its initial positioning efficiency. The vehicle initialization information includes the vehicle initial position, the vehicle initial Roll angle (roll angle) information and the vehicle initial Pitch angle (pitch angle) information. The vehicle initial position is used to narrow the search range of the vehicle initial position, and the vehicle initial Roll angle information and the vehicle initial Pitch angle information are used to narrow the search range of the vehicle initial angle.

[0055] In some embodiments, single-point GPS information is obtained, and the initial position of the vehicle is obtained based on the single-point GPS information, wherein the initial position of the vehicle includes the initial X coordinate, the initial Y coordinate, and the initial Yaw angle (yaw angle); stationary IMU data is obtained, and the initial Roll angle information and the initial Pitch angle information of the vehicle are obtained based on the stationary IMU data. Specifically, the outdoor single-point GPS information of the vehicle can be obtained through the single-point GPS device on the vehicle, and the obtained outdoor single-point GPS information mainly includes the initial X coordinate, the initial Y coordinate, and the initial Yaw angle in the attitude angle, and the stationary IMU data of the vehicle is obtained through the IMU sensor on the vehicle to obtain the initial angle information of the Roll angle and the Pitch angle of the vehicle, so as to use the initial position and initial angle information of the vehicle to narrow the search range of the subsequent branch and bound voxel map matching.

[0056] Step S300, obtaining the current radar point cloud frame.

[0057] In some embodiments, the positioning initialization method obtains the current radar point cloud frame through the laser radar on the vehicle. In the specific implementation process, the obtained radar point cloud frame can also be sampled and other related processing is performed to facilitate the branch and bound voxel map matching of the radar point cloud frame in the subsequent step S400, which is not limited by this application.

[0058] Step S400 , based on the branch and bound algorithm and the vehicle initialization information, voxel map matching is performed between the current radar point cloud frame and the multi-resolution voxel map.

[0059] In an embodiment of the present application, the positioning initialization method performs voxel map matching based on a branch and bound algorithm, and narrows the search range in the matching search process through vehicle initialization information, thereby speeding up the search speed and improving the initialization positioning efficiency.

[0060] In some embodiments, based on the branch and bound algorithm, the search area of ​​the current radar point cloud frame in the multi-resolution voxel map is set using the vehicle initialization information, and the optimal position of the current radar point cloud frame in each layer is searched in sequence from the low-resolution layer to the high-resolution layer of the multi-resolution voxel map. Specifically, the positioning initialization method uses the branch and bound algorithm to complete the voxel map matching, and searches for the optimal initial position in the constructed multi-resolution voxel map of several layers of resolution from low resolution to high resolution. In this process, the search range of the vehicle's initial position is narrowed by the vehicle's initial position, and the search range of the vehicle's initial angle is narrowed by the vehicle's initial Roll angle information and the vehicle's initial Pitch angle information, so as to reduce the search calculation amount of branch and bound and reduce time consumption.

[0061] Step S500, determining whether the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset ratio threshold.

[0062] In the embodiment of the present application, the number of all points in the radar point cloud frame that fall within the voxels of the multi-resolution voxel map divided by the number of all points in its current body is the effective point cloud ratio of the radar point cloud frame in the multi-resolution voxel map, also known as the optimal solution score of the voxel map matching.

[0063] In some embodiments, the effective point cloud ratio of the highest resolution layer of the current radar point cloud frame in the current search area is obtained, and it is determined whether the effective point cloud ratio is greater than a preset ratio threshold. If the effective point cloud ratio is greater than the preset ratio threshold, it means that the search range meets the threshold requirement of the positioning initialization method, and the process proceeds from step S500 to step S600. If the effective point cloud ratio is not greater than the preset ratio threshold, it means that the current radar point cloud frame search fails, and the process proceeds from step S500 to step S800. It should be noted and understood that in step S500, "whether the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than the preset ratio threshold" refers to whether the effective point cloud ratio of the current radar point cloud frame in any search area in the multi-resolution voxel map is greater than the preset ratio threshold. Only if there is a search area and the effective point cloud ratio of the current radar point cloud frame is greater than the preset ratio threshold, then it is determined that the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than the preset ratio threshold. If the effective point cloud ratio of the current radar point cloud frame in any search area in the multi-resolution voxel map is not greater than the preset ratio threshold, then it is determined that the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, and the search of the current radar point cloud frame fails.

[0064] Step S600: The best matching position is used as the initial value of the rough registration for positioning initialization and outputted.

[0065] In some embodiments, if the effective point cloud ratio of the current radar point cloud frame in the current search area is greater than a preset ratio threshold, the optimal position of the current radar point cloud frame in the highest resolution layer of the current search area is used as the optimal matching position, that is, the initial value of the rough registration for positioning initialization, and the initial value of the rough registration for positioning initialization is subsequently used to initialize high-precision positioning. If the effective point cloud ratio of the current radar point cloud frame in the current search area is not greater than the preset ratio threshold, it means that the current search area does not meet the threshold requirement, and the adjacent area of ​​the current search area is reset as the search area of ​​the current radar point cloud frame in the multi-resolution voxel map, from the low-resolution layer to the high-resolution layer of the multi-resolution voxel map, the optimal position of the current radar point cloud frame in each layer is searched in sequence, and the effective point cloud ratio of the current radar point cloud frame in the highest resolution layer of the current search area is obtained, and compared with the preset ratio threshold. If it is greater than the preset ratio threshold, the initial value of the rough registration for positioning initialization is obtained. If it is still not greater than the preset ratio threshold, the unsearched range is reset as the new search range for matching search until the effective point cloud ratio is greater than the preset ratio threshold, the traversal is terminated, and the initial value of the rough registration for positioning initialization is output.

[0066] Step S700, based on the Small-GICP fine registration module, the initial fine registration value for positioning initialization is obtained according to the initial coarse registration value and the point cloud map, and based on the DR module, the odometer's posture is obtained according to the fusion of the current vehicle IMU data and the current vehicle wheel speed data, so as to perform dead reckoning based on the initial fine registration value and the posture of the odometer to obtain the vehicle positioning initialization posture.

[0067] In some embodiments, the positioning initialization method uses a Small-GICP fine registration module to perform point cloud matching fine registration. Specifically, based on the Small-GICP fine registration module, the point cloud of the current radar point cloud frame is projected to the MAP coordinate system of the point cloud map according to the initial value of the coarse registration, and the corresponding relationship between the current radar point cloud frame and the point cloud map is obtained. The optimization result is obtained by iterative solution, and the optimization result is used as the initial value of the fine registration for positioning initialization and output. Among them, in the iterative solution process, when the residual between the corresponding relationships is not greater than the preset residual threshold, the optimization result can be obtained. That is to say, the Small-GICP fine registration module makes the residual between the corresponding relationships as small as possible, thereby obtaining the optimization result, that is, the final matching result. Of course, the positioning initialization method can also use other methods to perform point cloud matching fine registration, and this application does not limit this.

[0068] In addition, the present application adopts the Small-GICP precision registration module in the prior art, which is lightweight, fast and highly accurate compared to the GICP precision registration module, further improving the initialization positioning efficiency of the present application.

[0069] The embodiment of the present application adopts a trained DR module, which is used to obtain an odometer that fuses IMU data and wheel speed data. Among them, the information obtained by the IMU sensor includes the acceleration and angular velocity of three axes, and the current vehicle IMU data in the embodiment of the present application mainly includes the angular velocity of three axes. The DR module fuses the angular velocity of the three axes with the speed information provided by the wheel speed meter to obtain the corresponding odometer posture, and performs dead reckoning based on the posture of the odometer to obtain the vehicle positioning initialization posture. At the same time, the DR module is set to high-frequency output, thereby realizing high-frequency initialization and high-precision positioning, reducing the vehicle-side resource occupancy, and solving the problem of high occupancy of vehicle-side resources in the positioning initialization stage in the prior art.

[0070] The positioning initialization method in the embodiment of the present application obtains initialized high-precision positioning through lightweight point cloud matching and precise registration, and then fuses the current vehicle IMU data and the current vehicle wheel speed data through the DR module to achieve high-frequency output of high-precision positioning.

[0071] Step S800, reacquire a new current radar point cloud frame.

[0072] In some embodiments, if after traversing all positions in the multi-resolution voxel map, the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, it is determined that the initialization positioning of the current radar point cloud frame has failed, and a new current radar point cloud frame is reacquired, and the process returns from step S800 to step S400. Based on the branch and bound algorithm and the vehicle initialization information, the new current radar point cloud frame and the multi-resolution voxel map are voxel-map matched. After that, it is re-determined whether the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than the preset ratio threshold. If it is not greater than the preset ratio threshold, a new current radar point cloud frame is reacquired again until the optimal matching position is obtained, and the process proceeds from step S500 to step S600 to output the initial value of the rough registration for positioning initialization.

[0073] Corresponding to the above method embodiment, the present application embodiment also provides a coarse-to-fine positioning initialization system, which is used to execute the steps of the coarse-to-fine positioning initialization method in the above embodiment. Figure 2 As shown, the positioning initialization system includes a priori map generation module 1, an initialization information acquisition module 2, a coarse initialization positioning module 3 and a fine initialization positioning module 4.

[0074] Specifically, the prior map generation module 1 is used to generate a prior map. The prior map includes a point cloud map and a multi-resolution voxel map. The prior map generation module 1 is specifically used to obtain an offline point cloud map that has been generated, and then save the multi-resolution offline map point cloud distribution information by extracting voxel features, and construct a multi-resolution voxel map of several layers of resolution.

[0075] The initialization information acquisition module 2 is used to obtain vehicle initialization information. The vehicle initialization information includes the vehicle initial position, the vehicle initial Roll angle information and the vehicle initial Pitch angle information. The initialization information acquisition module 2 is specifically used to obtain single-point GPS information, and obtain the vehicle initial position according to the single-point GPS information, and to obtain stationary IMU data, and obtain the vehicle initial Roll angle information and the vehicle initial Pitch angle information according to the stationary IMU data.

[0076] The coarse initialization positioning module 3 is used to obtain the current radar point cloud frame, and based on the branch and bound algorithm and the vehicle initialization information, the current radar point cloud frame and the multi-resolution voxel map are voxel matched, and it is used to determine whether the optimal solution score is greater than the preset ratio threshold. If so, the optimal matching position is used as the coarse alignment initial value for positioning initialization and output. If not, a new current radar point cloud frame is re-acquired until the coarse alignment initial value for positioning initialization is output.

[0077] The fine initialization positioning module 4 is used to obtain the fine registration initial value for positioning initialization based on the coarse registration initial value and the point cloud map based on the Small-GICP fine registration module, and is used to obtain the odometer's posture based on the DR module according to the fusion of the current vehicle IMU data and the current vehicle wheel speed data, so as to perform dead reckoning based on the odometer's posture based on the fine registration initial value to obtain the vehicle positioning initialization posture.

[0078] It should be noted that the coarse-to-fine positioning initialization system provided in the embodiment of the present application is based on the same concept as the coarse-to-fine positioning initialization method embodiment of the present application, and the technical effect it brings is the same as the coarse-to-fine positioning initialization method embodiment of the present application. For specific contents, please refer to the description in the coarse-to-fine positioning initialization method embodiment of the present application, which will not be repeated here.

[0079] In summary, the present application discloses a coarse-to-fine positioning initialization method and system thereof, which uses a multi-resolution voxel map for fuzzy initialization positioning to obtain a coarse alignment initial value for positioning initialization, and in this process, the search space for the initialization position and attitude is narrowed by single-point GPS information and stationary IMU data, and the point cloud map is used to initialize high-precision positioning, while the DR module is used to achieve high-frequency initialization high-precision positioning, reducing vehicle-side resource occupancy, thereby solving the problem of high vehicle-side resource occupancy during the positioning initialization stage, and greatly saving the cost of vehicle accessories, and optionally or abandoning combined navigation. This positioning initialization method can achieve high-efficiency initialization positioning attitude output with high real-time performance, strong robustness, and high positioning accuracy, solving the problem of failure of the positioning initialization method due to weak or even no GPS signal, and the problem of failure of positioning initialization due to failure of historical trajectory.

[0080] Based on the above method embodiment, another embodiment of the present application provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method described in the above coarse-to-fine positioning initialization method embodiment is implemented.

[0081] Based on the above method embodiment, another embodiment of the present application provides an electronic device, which includes a processor and a memory, the processor is coupled to the memory, the memory is used to store a computer program, and the computer program is executed by the processor, so that the electronic device implements the method described in the above coarse-to-fine positioning initialization method embodiment.

[0082] At the same time, based on the above embodiments, another embodiment of the present application provides a computer program product, which contains instructions. When the instructions are executed on a computer or a processor, the computer or the processor executes the method described in the above coarse-to-fine positioning initialization method embodiment.

[0083] Those skilled in the art will understand that the accompanying drawings are only schematic diagrams of an embodiment, and the modules or processes in the accompanying drawings are not necessarily necessary for implementing the present application. In addition, the modules in the devices in the embodiments may be distributed in the devices in the embodiments according to the description of the embodiments, or may be changed accordingly and located in one or more devices different from the present embodiments. The modules in the above embodiments may be combined into one module, or may be further split into multiple sub-modules.

[0084] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it. Although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A coarse-to-fine positioning initialization method, characterized in that: include: Generate a priori map; wherein the priori map includes a point cloud map and a multi-resolution voxel map; Acquire vehicle initialization information; wherein the vehicle initialization information includes vehicle initial position, vehicle initial Roll angle information and vehicle initial Pitch angle information; Acquire a current radar point cloud frame, and perform voxel map matching between the current radar point cloud frame and the multi-resolution voxel map based on a branch and bound algorithm and the vehicle initialization information; When the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset ratio threshold, the optimal matching position is used as a rough registration initial value for positioning initialization and output; Based on the Small-GICP fine registration module, the fine registration initial value for positioning initialization is obtained according to the coarse registration initial value and the point cloud map, and based on the DR module, the odometer posture is obtained according to the fusion of the current vehicle IMU data and the current vehicle wheel speed data, so as to obtain the vehicle positioning initialization posture by performing dead reckoning based on the fine registration initial value and the odometer posture.

2. The method for initializing positioning from coarse to fine according to claim 1, characterized in that: Also includes: When all positions in the multi-resolution voxel map are traversed, and the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, a new current radar point cloud frame is reacquired, and voxel map matching is performed on the new current radar point cloud frame and the multi-resolution voxel map based on the branch and bound algorithm and the vehicle initialization information until the coarse alignment initial value for positioning initialization is output.

3. The method for initializing positioning from coarse to fine according to claim 1, characterized in that: The generating of the prior map specifically includes: Acquire the point cloud map, and generate the multi-resolution voxel map according to the point cloud map; The prior map is generated according to the point cloud map and the multi-resolution voxel map.

4. The method for initializing positioning from coarse to fine according to claim 1, characterized in that: The obtaining of vehicle initialization information specifically includes: Acquire single-point GPS information, and obtain the initial position of the vehicle according to the single-point GPS information; wherein the initial position of the vehicle includes an initial X coordinate, an initial Y coordinate and an initial Yaw angle; Acquire stationary state IMU data, and acquire the vehicle initial Roll angle information and the vehicle initial Pitch angle information according to the stationary state IMU data.

5. The method for initializing positioning from coarse to fine according to claim 1, characterized in that: The obtaining of the current radar point cloud frame and performing voxel map matching between the current radar point cloud frame and the multi-resolution voxel map based on a branch and bound algorithm and the vehicle initialization information specifically includes: Acquire the current radar point cloud frame; Based on the branch and bound algorithm, and using the vehicle initialization information, a search area of ​​the current radar point cloud frame in the multi-resolution voxel map is set, and an optimal position of the current radar point cloud frame in each layer is sequentially searched from a low-resolution layer to a high-resolution layer of the multi-resolution voxel map; Obtaining the effective point cloud ratio of the highest resolution layer of the current radar point cloud frame in the current search area, and determining whether the effective point cloud ratio is greater than the preset ratio threshold; If the effective point cloud ratio is greater than the preset ratio threshold, taking the optimal position of the current radar point cloud frame at the highest resolution layer of the current search area as the optimal matching position; If the effective point cloud ratio is not greater than the preset ratio threshold, the adjacent area of ​​the current search area is reset as the search area of ​​the current radar point cloud frame in the multi-resolution voxel map, and the optimal position of the current radar point cloud frame in each layer is searched in sequence from the low-resolution layer to the high-resolution layer of the multi-resolution voxel map, and the effective point cloud ratio of the current radar point cloud frame in the highest resolution layer of the current search area is obtained, until the effective point cloud ratio is greater than the preset ratio threshold, and the traversal is terminated; When all positions in the multi-resolution voxel map are traversed, if the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is not greater than the preset ratio threshold, it is determined that the initialization positioning of the current radar point cloud frame has failed, and a new current radar point cloud frame is reacquired for voxel map matching until the optimal matching position is obtained.

6. The method for initializing positioning from coarse to fine according to claim 1, characterized in that: The Small-GICP-based fine registration module obtains the initial fine registration value for positioning initialization according to the initial coarse registration value and the point cloud map, specifically including: Based on the Small-GICP fine registration module, the point cloud of the current radar point cloud frame is projected to the MAP coordinate system of the point cloud map according to the rough registration initial value, the corresponding relationship between the corresponding point clouds of the current radar point cloud frame and the point cloud map is obtained, and the optimization result is obtained by iterative solution, and the optimization result is used as the fine registration initial value for positioning initialization and output.

7. The method for initializing positioning from coarse to fine according to claim 6, characterized in that: The optimization result obtained by iterative solution specifically includes: By iterative solution, when the residual between the corresponding relationships is not greater than a preset residual threshold, the optimization result is obtained.

8. A coarse-to-fine positioning initialization system, characterized in that: include: A priori map generation module, used to generate a priori map; wherein the priori map includes a point cloud map and a multi-resolution voxel map; An initialization information acquisition module is used to acquire vehicle initialization information; wherein the vehicle initialization information includes vehicle initial position, vehicle initial Roll angle information and vehicle initial Pitch angle information; A coarse initialization positioning module is used to obtain a current radar point cloud frame, and based on a branch and bound algorithm and the vehicle initialization information, perform voxel map matching between the current radar point cloud frame and the multi-resolution voxel map, and when the effective point cloud ratio of the current radar point cloud frame in the multi-resolution voxel map is greater than a preset ratio threshold, the optimal matching position is used as a coarse registration initial value for positioning initialization and output; The fine initialization positioning module is used to obtain the initial fine registration value for positioning initialization based on the coarse registration initial value and the point cloud map based on the Small-GICP fine registration module, and to obtain the odometer posture based on the fusion of the current vehicle IMU data and the current vehicle wheel speed data based on the DR module, so as to perform dead reckoning based on the odometer posture based on the initial fine registration value to obtain the vehicle positioning initialization posture.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the coarse-to-fine positioning initialization method described in any one of claims 1-7 is implemented.

10. An electronic device, characterized in that: The electronic device comprises a processor and a memory; The processor is coupled to the memory, the memory is used to store a computer program, and the computer program is executed by the processor to enable the electronic device to implement the coarse-to-fine positioning initialization method described in any one of claims 1-7.

Images