Vehicle repositioning method, device and system and vehicle

Abstract

The invention provides a vehicle repositioning method, device and system and a vehicle. The method comprises the following steps: firstly, acquiring environment and motion state data of the vehicle; then, track node data are generated based on the environment and motion state data, and the track node data are used for representing the initial pose of the target vehicle and point cloud information of the environment where the target vehicle is located; next, point cloud matching is carried out based on the multiple pieces of preset sub-graph data and the track node data, and a first matching pose is obtained; and further, performing cerees nonlinear optimization processing on the first matching pose to obtain a second matching pose. And finally, obtaining a relocation result according to the second matching pose, and carrying out pose initialization according to the relocation result. According to the method, in combination with laser point cloud data, point cloud matching is carried out by searching near a real pose of a vehicle based on initial pose information. In this way, the vehicle can be repositioned efficiently and accurately.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of automatic driving, in particular to a vehicle repositioning method, device, system and vehicle. BACKGROUND

[0002] Three-dimensional simultaneous localization and mapping (3D SLAM) is a core technology in the field of automatic driving. This technology enables a vehicle to collect data through sensors (such as lidar and visual camera) in an unknown environment, and simultaneously achieve self-positioning and three-dimensional environment mapping. 3D SLAM repositioning refers to re-determining the accurate position and attitude of a vehicle in a map after the vehicle is restarted or SLAM positioning fails.

[0003] In the field of park-level automatic driving, a vehicle shuttles in indoor and outdoor environments. Generally, in the outdoor environment, the real-time kinematic (RTK) technology is used to initialize the integrated positioning, and then the SLAM system is initialized based on the accurate position and attitude of the integrated positioning, and repositioning is performed. However, since the parking position of the vehicle is mostly in an indoor or sheltered area, it is impossible to successfully initialize the SLAM system after power-on, i.e., it is impossible to obtain high-precision positioning information, thereby resulting in inefficient and inaccurate repositioning of the vehicle. SUMMARY

[0004] The technical problem to be solved by the present application is the problem of inefficient and inaccurate repositioning of the vehicle.

[0005] To solve the above technical problem, the present application provides a vehicle repositioning method, device, system and vehicle, which specifically adopts the following technical solutions:

[0006] In a first aspect, the application provides a vehicle repositioning method, which can be applied to a vehicle repositioning system. The method comprises the following steps. Firstly, environment and motion state data of a target vehicle are received, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle. Then, trajectory node data is generated based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located, under the condition that the target vehicle is determined to be in a stationary state. Next, point cloud matching is performed based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose. Secondly, ceres nonlinear optimization processing is performed on the first matching pose to obtain a second matching pose. Finally, pose initialization is performed according to the second matching pose to obtain a repositioning result of the target vehicle, so that the target vehicle is navigated and controlled according to the repositioning result.

[0007] The method first acquires environment and motion state data of a vehicle. Then, trajectory node data is generated based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located. Next, point cloud matching is performed based on a plurality of preset subgraph data and the trajectory node data to obtain a first matching pose. Further, ceres nonlinear optimization processing is performed on the first matching pose to obtain a second matching pose. Finally, a repositioning result can be obtained according to the second matching pose, so that pose initialization is performed according to the repositioning result. The method combines laser point cloud data for point cloud matching to determine the pose of the target vehicle, which can improve the accuracy of vehicle repositioning. Moreover, the initial pose can be used to represent the final pose of the target vehicle before power-off, or the initial pose information can be used to represent the pose of the target vehicle after parking due to SLAM positioning failure. Therefore, global search and matching of the full subgraph in the full map are not required, and search near the real pose can improve the efficiency of point cloud matching. In summary, the method can efficiently and accurately reposition the vehicle.

[0008] In combination with the first aspect, in an alternative implementation manner, each preset subgraph data in the plurality of preset subgraph data includes corresponding subgraph pose information. The point cloud matching based on the plurality of preset subgraph data and the trajectory node data to determine the first matching pose comprises the following steps. Firstly, a plurality of fast map matchers are constructed according to the number of the plurality of preset subgraph data. Then, the plurality of fast map matchers are called asynchronously through multi-threading, and the trajectory node data and the plurality of preset subgraph data are matched according to a preset matching method to determine a matching score value corresponding to each preset subgraph data in the plurality of preset subgraph data, the matching score value being used to represent a point cloud occupancy score. Finally, the first matching pose is determined based on the subgraph pose information corresponding to the preset subgraph data with the highest matching score value in the matching score values.

[0009] With reference to the first aspect, in a possible implementation form of the first aspect, the environment and motion state data of the target vehicle comprises first point cloud data and INS topic data. The trajectory node data comprises initial pose information, first resolution point cloud data, second resolution point cloud data and point cloud rotation histogram. The generating the trajectory node data based on the environment and motion state data comprises: firstly, performing time synchronization and merging processing on the point cloud data in the first point cloud data to obtain second point cloud data. Then, performing first voxel filtering on the second point cloud data to obtain third point cloud data. Performing point cloud clipping on the third point cloud data to remove point cloud data representing the target vehicle to obtain fourth point cloud data. Secondly, performing coordinate system conversion on the fourth point cloud data based on the INS topic data to obtain fifth point cloud data in a map coordinate system, the map coordinate system being a coordinate system in which the initial pose information is located. Next, performing second voxel filtering on the fifth point cloud data based on a first preset resolution to obtain the first resolution point cloud data. Performing third voxel filtering on the fifth point cloud data based on a second preset resolution to obtain the second resolution point cloud data, wherein the second preset resolution is smaller than the first preset resolution. Finally, generating a point cloud rotation histogram according to the fifth point cloud data, point cloud attitude information of the fifth point cloud data and a preset angle resolution, the point cloud rotation histogram being used to represent a spatial distribution of point cloud in the fifth point cloud data.

[0010] With reference to the first aspect, in a possible implementation form of the first aspect, the receiving the environment and motion state data of the target vehicle comprises: firstly, receiving initial pose information of the target vehicle and INS initial information, the initial pose information being used to represent a final pose of the target vehicle before power-on, or the initial pose information being used to represent a pose of the target vehicle after parking due to SLAM positioning failure. Then, generating INS topic data based on the initial pose information and the INS initial information, and receiving the INS topic data through a ROS topic. Collecting first point cloud data through a laser radar module, and receiving the first point cloud data through a ROS topic.

[0011] With reference to the first aspect, in a possible implementation form of the first aspect, the INS initial information comprises motion state information of the target vehicle after power-on, the motion state information being used to determine whether the target vehicle is in a stationary state.

[0012] In a second aspect, the present application provides a vehicle repositioning device, which comprises a 3D SLAM module and an INS module. The 3D SLAM module can be configured to receive environment and motion state data of a target vehicle, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generating trajectory node data based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; performing point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; and performing ceres non-linear optimization processing on the first matching pose to obtain a second matching pose. The INS module can be configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, and to perform navigation control on the target vehicle based on the repositioning result.

[0013] In combination with the second aspect, in an alternative implementation manner, the environment and motion state data comprises first point cloud data of the target vehicle and INS topic data; and the device further comprises a communication interaction module. The communication interaction module can be configured to receive initial pose information of the target vehicle and INS initial information, the initial pose information being used to represent a final pose of the target vehicle before power-off, or the initial pose information being used to represent a pose of the target vehicle after parking due to SLAM positioning failure; generate the INS topic data based on the initial pose information and the INS initial information, and publish the INS topic data through a ROS topic; and collect the first point cloud data through a laser radar module, and publish the first point cloud data through the ROS topic.

[0014] In combination with the second aspect, in an alternative implementation manner, the INS module is further configured to obtain the INS initial information, and publish the INS initial information through the ROS topic.

[0015] In combination with the second aspect, in an alternative implementation manner, each preset subgraph data in the plurality of preset subgraph data comprises corresponding subgraph pose information. The 3D SLAM module is specifically configured to: first, construct a plurality of fast map matchers according to a number of the plurality of preset subgraph data; then, perform matching between the trajectory node data and the plurality of preset subgraph data according to a preset matching method through multi-thread asynchronous calling of the plurality of fast map matchers, to determine a matching score value corresponding to each preset subgraph data in the plurality of preset subgraph data, the matching score value being used to represent a point cloud occupancy score; and finally, determine the first matching pose based on subgraph pose information corresponding to preset subgraph data with a highest matching score value in the matching score values.

[0016] In combination with the second aspect, in an alternative implementation manner, the environment and motion state data comprises: first point cloud data and INS topic data of the target vehicle. The trajectory node data comprises: initial pose information, first resolution point cloud data, second resolution point cloud data and point cloud rotation histogram. The 3D SLAM module is further specifically configured to: firstly, perform time synchronization and merging processing on the point cloud data in the first point cloud data to obtain second point cloud data. Then, performing first voxel filtering on the second point cloud data to obtain third point cloud data. The third point cloud data is subjected to point cloud clipping to remove the point cloud data used to represent the target vehicle to obtain fourth point cloud data. Secondly, performing coordinate system conversion on the fourth point cloud data based on the INS topic data to obtain fifth point cloud data in a map coordinate system, the map coordinate system being a coordinate system in which the initial pose information is located. Next, performing second voxel filtering processing on the fifth point cloud data based on a first preset resolution to obtain the first resolution point cloud data. Performing third voxel filtering processing on the fifth point cloud data based on a second preset resolution to obtain the second resolution point cloud data, wherein the second preset resolution is smaller than the first preset resolution. Finally, generating a point cloud rotation histogram according to the fifth point cloud data, point cloud attitude information of the fifth point cloud data and a preset segmentation angle resolution, the point cloud rotation histogram being used to represent the spatial distribution of the point cloud in the fifth point cloud data.

[0017] In a third aspect, the present application provides a vehicle repositioning system, comprising: an INS module, a communication interaction module, and a 3D SLAM module. The INS module can be configured to obtain INS initial information and publish the INS initial information through a ROS topic. The communication interaction module can be configured to receive initial pose information of a target vehicle and the INS initial information, the initial pose information being used to represent a final pose of the target vehicle before power-off, or the initial pose information being used to represent a pose of the target vehicle after parking due to SLAM positioning failure; generate INS topic data based on the initial pose information and the INS initial information, and publish the INS topic data through a ROS topic; and collect first point cloud data through a laser radar module and publish the first point cloud data through a ROS topic. The 3D SLAM module can be configured to receive environment and motion state data of the target vehicle, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generate trajectory node data based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; perform point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; perform ceres non-linear optimization processing on the first matching pose to obtain a second matching pose. The INS module is further configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, so as to perform navigation control on the target vehicle based on the repositioning result.

[0018] In a fourth aspect, the present application provides a vehicle, comprising: a vehicle body and a vehicle repositioning system provided on the vehicle body and configured as in the third aspect.

[0019] In a fifth aspect, the present application provides an electronic device, comprising: a memory and one or more processors; the memory is coupled to the processor; and the memory stores computer program codes, the computer program codes comprising computer instructions, when the computer instructions are executed by the processor, causing the electronic device to perform the method provided in the first aspect and any optional implementation manner thereof.

[0020] In a sixth aspect, the present application provides a computer readable storage medium, comprising computer instructions, when the computer instructions are run on an electronic device, causing the electronic device to perform the method provided in the first aspect and any optional implementation manner thereof.

[0021] It can be understood that the vehicle repositioning device provided by the second aspect, the vehicle repositioning system provided by the third aspect, the vehicle provided by the fourth aspect, the electronic device provided by the fifth aspect and the computer readable storage medium provided by the sixth aspect can achieve the beneficial effects of the first aspect and any possible design of the first aspect. The beneficial effects will not be repeated here. BRIEF DESCRIPTION OF DRAWINGS

[0022] Figure 1 Flowchart of the vehicle repositioning method provided by the embodiment of the present application;

[0023] Figure 2 Flowchart of the method for generating trajectory node data based on environment and motion state data provided by the embodiment of the present application;

[0024] Figure 3 Flowchart of the method for determining the first matching pose provided by the embodiment of the present application;

[0025] Figure 4 Structure diagram of the vehicle repositioning device provided by the embodiment of the present application. DETAILED DESCRIPTION

[0026] The embodiments will be described in detail below, and examples are shown in the accompanying drawings. When the following description refers to the accompanying drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following embodiments do not represent all embodiments consistent with the present application. They are only examples of systems and methods consistent with some aspects of the present application as described in detail in the claims.

[0027] In the present application, all actions related to the acquisition of signals, information or data are carried out in strict compliance with the relevant data protection regulations and policies of the place of residence, and in the case of obtaining the authorization of the owner of the corresponding device.

[0028] The owner refers to an individual or entity that owns or controls the relevant device (which can be equipment, system or other tool that can collect data).

[0029] In the field of intelligent connected vehicles, the "owner" mainly includes:

[0030] (1) Automobile manufacturers: as developers of vehicle hardware and systems, they control the underlying hardware and software platforms of vehicles and have management and control rights over the data generated by vehicle operation, such as driving and fault data.

[0031] (2) Parts suppliers: they provide key components for vehicles and have certain ownership of the data collected and processed by the components for product optimization and after-sales, such as data generated by sensors and chips.

[0032] (3) Car owner or user: the actual user of the vehicle, who has the decision-making power on the use and scope of the vehicle data, such as whether to share the data of driving track, driving habit, etc., and has the protection demand and right of privacy of the data related to himself.

[0033] (4) Service provider: providing services such as software and data analysis, having the right to use and manage the data obtained and processed under the agreement framework, but the ownership usually belongs to other subjects.

[0034] 3D SLAM is a core technology in the field of autonomous driving, which can enable the vehicle to collect data through sensors (such as lidar and visual camera) in an unknown environment, and simultaneously realize self-positioning and three-dimensional environment map construction. 3D SLAM repositioning refers to re-determining the accurate position and attitude of the vehicle in the map after the vehicle is restarted or SLAM positioning fails.

[0035] In the field of park-level autonomous driving, vehicles shuttle in indoor and outdoor environments. Usually, RTK technology is used to initialize combined positioning in outdoor environments, and then the SLAM system is initialized based on the accurate position and attitude of combined positioning, and repositioning is performed. However, since the parking position of the vehicle is mostly in an indoor or sheltered area, it is impossible to successfully initialize the SLAM system after power-on, i.e., it is impossible to obtain high-precision positioning information, which will affect the repositioning of the vehicle.

[0036] Cartographer algorithm is applied in global repositioning of 2D SLAM, which usually performs global matching based on the current frame point cloud of the vehicle, i.e., global search matching of the whole subgraph based on the whole map. However, if the map environment has a symmetrical structure, there is a probability of matching error in global repositioning, resulting in failure of global repositioning. Moreover, since the calculation amount of global search matching is very large, it will have a large time consumption and low efficiency. In global repositioning of 3D SLAM, the amount of subgraph data is large, and if the method of global search matching is used, the calculation amount will increase exponentially. It can be seen that the related technology cannot efficiently and accurately reposition the vehicle.

[0037] To solve the above problems, the embodiment of the present application provides a vehicle repositioning method, device, system and vehicle. The method is a 3D SLAM repositioning method based on laser. Specifically, the method first acquires the environment and motion state data of the vehicle. Then, the trajectory node data is generated based on the environment and motion state data, which is used to represent the initial pose of the target vehicle and the point cloud information of the environment where the target vehicle is located. Next, the point cloud matching is performed based on the preset subgraph data and the trajectory node data to obtain the coarse matching pose. Further, the coarse matching pose is optimized to obtain the fine matching pose. Finally, the repositioning result can be obtained according to the fine matching pose, so as to initialize the pose according to the repositioning result. The method combines laser point cloud data for point cloud matching to determine the pose of the target vehicle, which can improve the accuracy of vehicle repositioning. Moreover, the initial pose can be used to represent the final pose of the target vehicle before power-off, or the initial pose information is used to represent the pose of the target vehicle after SLAM positioning fails. Therefore, it is not necessary to perform global search matching in the whole graph, and searching near the real pose can improve the efficiency of point cloud matching. In summary, the method can efficiently and accurately reposition the vehicle

[0038] The scheme provided by the embodiment of the present application will be introduced below in combination with the drawings.

[0039] Specifically, the vehicle repositioning method provided by the embodiment of the present application can be applied to a vehicle repositioning system, which can be arranged in the target vehicle. Specifically, the vehicle repositioning system includes an inertial navigation system (INS) module, a communication interaction module and a 3D SLAM module. The INS module can be an embedded device combined inertial navigation module, the communication interaction module can be a Communication node module, and the 3D SLAM module can be a Cartographer module. The INS module and the communication interaction module can perform data interaction through a user datagram protocol (UDP) communication module.

[0040] Figure 1 The flowchart of the vehicle repositioning method provided by the embodiment of the present application is shown in FIG. 1. Figure 1 The vehicle repositioning method provided by the embodiment of the present application includes the following steps S101-S105.

[0041] S101, receiving the environment and motion state data of the target vehicle.

[0042] In the embodiment of the present application, the environment and motion state data can be used to represent the environmental features in the target area where the target vehicle is located and the motion state of the target vehicle.

[0043] In some embodiments, the environment and motion state data specifically includes: first point cloud data of the target vehicle and INS topic data. Then, S101 receives the environment and motion state data of the target vehicle, which specifically can include the following steps S1011-S1013:

[0044] S1011, receive initial pose information and INS initial information of the target vehicle.

[0045] Specifically, the initial pose information of the target vehicle can be obtained by the communication interaction module in the vehicle repositioning system. The initial pose information specifically can include: x-axis coordinate value, y-axis coordinate value, z-axis coordinate value, roll angle, pitch angle and yaw angle. The initial pose information can be used to represent the final pose of the target vehicle before power-off, or the initial pose information can also be used to represent the pose of the target vehicle after parking due to SLAM positioning failure. In this way, the method can not only be applied to the repositioning of the target vehicle after power-off and initialization, but also be applied to the repositioning of the target vehicle after SLAM positioning failure (positioning drift). The current initial pose information can be used to quickly match and find the accurate pose of the target vehicle near the drift position, so as to ensure the continuity and consistency of the positioning of the target vehicle, and thus improve the application scenarios and practicality of the 3D SLAM repositioning method.

[0046] In an implementation manner, the communication interaction module can obtain the initial pose information by reading the local storage file of the target vehicle.

[0047] The initial pose information of the target vehicle can be obtained by the INS module in the vehicle repositioning system. The INS initial information can include motion state information of the target vehicle after power-on, which can be used to determine whether the target vehicle is in a stationary state for repositioning. For example, the motion state information can include: speed, acceleration, etc.

[0048] S1012, generate INS topic data based on the initial pose information and the INS initial information, and receive the INS topic data through the ROS topic.

[0049] Then, the initial pose information and the INS initial information can be combined and packaged to generate INS topic (INS_data) data, and the INS topic data is received through the ROS topic.

[0050] S1013, collect first point cloud data through the laser radar module, and receive the first point cloud data through the ROS topic.

[0051] The first point cloud data is used to represent the external environment of the target vehicle. The first point cloud data can be used to generate trajectory node data for point cloud matching.

[0052] S102, in the case where it is determined that the target vehicle is in a stationary state, generating trajectory node data based on the environment and motion state data.

[0053] In the embodiments of the present application, the target vehicle needs to be in a stationary state for repositioning. Therefore, motion state information can be extracted from the received INS topic data first. Then, based on the motion state information, it can be determined whether the target vehicle is in a stationary state. Further, in the case where it is determined that the target vehicle is in a stationary state, trajectory node data is generated based on the environment and motion state data for point cloud matching. The trajectory node data is used to represent the initial pose of the target vehicle and the point cloud information of the environment in which the target vehicle is located.

[0054] In some embodiments, the above-mentioned trajectory node data includes initial pose information, first resolution point cloud data, second resolution point cloud data, and point cloud rotation histogram. The first resolution point cloud data, the second resolution point cloud data, and the point cloud rotation histogram are used for point cloud matching with a plurality of preset subgraph data. The initial pose information can be used to determine the first matching pose.

[0055] The resolution of the first resolution point cloud data is greater than the resolution of the second resolution point cloud data. Specifically, the second resolution point cloud data and the point cloud rotation histogram can be used for first point cloud matching with a plurality of preset subgraph data, i.e., preliminary low-resolution matching. Next, the first resolution point cloud data and the point cloud rotation histogram can be used for second point cloud matching with the result of the first point cloud matching, i.e., further high-resolution fine matching. In this way, efficient and accurate point cloud matching can be achieved.

[0056] In some embodiments, based on the INS topic data and the first point cloud data, the above-mentioned trajectory node data can be obtained through data synchronization merging, point cloud filtering processing, cropping, pose processing and coordinate transformation, rotation histogram processing, etc. Specifically, Figure 2 The method flow diagram for generating trajectory node data based on environment and motion state data provided by the embodiments of the present application is shown in Figure 2 As shown in FIG. 1, S102, in the case where it is determined that the target vehicle is in a stationary state, generating trajectory node data based on the environment and motion state data, which can specifically include the following steps S1021-S1027:

[0057] S1021, performing time synchronization and merging processing on the point cloud data in the first point cloud data to obtain second point cloud data.

[0058] Specifically, the first point cloud data usually includes point cloud data collected by multiple lidar sensors. Therefore, the point cloud data in the first point cloud data is first time-synchronized and merged to obtain complete second point cloud data after merging.

[0059] S1022, performing first voxel filtering on the second point cloud data to obtain third point cloud data.

[0060] Then, the first voxel filtering is performed on the second point cloud data to remove noise and outliers in the second point cloud data and retain the geometric features of the object, to obtain third point cloud data after first voxel filtering.

[0061] S1023, performing point cloud cropping on the third point cloud data to remove point cloud data for representing the target vehicle, to obtain fourth point cloud data.

[0062] S1024, performing coordinate system conversion on the fourth point cloud data based on the INS topic data to obtain fifth point cloud data in a map coordinate system.

[0063] Specifically, the global pose of the target vehicle (i.e., based on the initial pose information) can be applied to the point cloud data, i.e., the fourth point cloud data in the lidar sensor coordinate system is converted to the map coordinate system to obtain the fifth point cloud data after conversion in the map coordinate system. The map coordinate system is the coordinate system in which the initial pose information is located.

[0064] S1025, performing second voxel filtering on the fifth point cloud data based on a first preset resolution to obtain first resolution point cloud data.

[0065] S1026, performing third voxel filtering on the fifth point cloud data based on a second preset resolution to obtain second resolution point cloud data.

[0066] The second preset resolution is smaller than the first preset resolution, i.e., the first resolution point cloud data is high-resolution point cloud data, and the second resolution point cloud data is low-resolution point cloud data. In this way, the first resolution point cloud data and the second resolution point cloud data are used for multi-resolution point cloud matching with the preset subgraph data, to improve the efficiency and accuracy of point cloud matching.

[0067] S1027, generating a point cloud rotation histogram according to the fifth point cloud data, point cloud pose information of the fifth point cloud data, and a preset segmentation angle resolution.

[0068] The point cloud rotation histogram is used to represent the spatial distribution of the point cloud in the fifth point cloud data. In this way, the point cloud rotation histogram can be combined to more accurately perform point cloud matching.

[0069] In an implementation manner, the point cloud posture information can be a rotation matrix for representing a point cloud posture, based on which the point cloud corresponding to the fifth point cloud data can be coordinate-transformed to be unified into a reference coordinate system, so as to eliminate the influence of the posture error. Then, [-π, π] is divided into multiple angle intervals based on a preset segmentation angle resolution (for example, 5°). Finally, the distribution of the point cloud in each angle interval in the fifth point cloud data is determined to generate a point cloud rotation histogram.

[0070] S103, performing point cloud matching based on the multiple preset subgraph data and the trajectory node data to determine a first matching pose.

[0071] Further, the multiple preset subgraph data can be matched with the trajectory node data determined in S102, that is, coarse matching is performed to determine the first matching pose. The multiple preset subgraph data are subgraph data constituting a global map in which the target vehicle is located, and the preset subgraph data can include grid point cloud data for representing the environment in the map. Since the initial pose represented by the initial pose information in the trajectory node data is near the real pose of the target vehicle, the search range of the corresponding global matching method is effectively reduced, and the first matching pose can be quickly determined.

[0072] In some embodiments, the multiple preset subgraph data are a preset number of preset subgraph data closest to the initial pose corresponding to the initial pose information. The preset number can be set based on prior knowledge or application requirements.

[0073] For example, the preset subgraph data constituting the global map can be subgraph screened based on the initial pose information, and a preset number (for example, 3) of preset subgraph data closest to the initial pose information can be selected for point cloud matching. In this way, the matching success rate is not affected, and the algorithmic demand for invalid subgraph matching is greatly reduced. The method is also applicable in a low algorithmic hardware and a large-scale map scenario, thereby improving the practicability of the 3D SLAM repositioning method.

[0074] In some embodiments, each of the multiple preset subgraph data further includes corresponding subgraph pose information. Figure 3 A method flowchart for determining the first matching pose provided by the embodiments of the present application is shown in FIG. 3. Figure 3 As shown in FIG. 3, S103 performs point cloud matching based on the multiple preset subgraph data and the trajectory node data to determine the first matching pose, which can specifically include the following steps S1031-S1033.

[0075] S1031, constructing multiple fast map matchers according to the number of the multiple preset subgraph data.

[0076] The fast map matcher can be used to match the trajectory node data with the plurality of preset subgraph data. For example, the fast map matcher can be a fast_scan_match fast map matcher. The number of fast map matchers can be the same as the number of the plurality of preset subgraph data, so that the plurality of fast map matchers can correspond to the plurality of preset subgraph data one by one, and the corresponding preset subgraph data and the trajectory node data can be simultaneously matched by the plurality of fast map matchers to improve the efficiency of point cloud matching.

[0077] In S1032, the plurality of fast map matchers are called asynchronously through multi-threading, and the trajectory node data is matched with the plurality of preset subgraph data according to a preset matching method to determine a matching score value corresponding to each preset subgraph data in the plurality of preset subgraph data.

[0078] For example, the fast map matcher is a fast_scan_match fast map matcher. The match function in the plurality of fast map matchers is called asynchronously through multi-threading to determine the matching score value corresponding to each preset subgraph data in the plurality of preset subgraph data to realize point cloud matching. The matching score value is used to represent the point cloud occupancy score. In this way, the fast map matchers are constructed based on the number of preset subgraph data, and the fast matching is realized through asynchronous calling of multi-threading. The number of threads of the multi-threading can be determined by hardware threads, so that the fast matching of the plurality of preset subgraph data can be realized, and the matching time is shortened.

[0079] In some embodiments, the preset matching method can be a branch and bound method. Specifically, the branch and bound method is used in the matching process of the trajectory node data and the plurality of preset subgraph data, a large number of candidate poses can be generated in a small angle and a small translation range to perform point cloud occupancy score statistics, and then the matching score value is determined. In this way, the matching result of the trajectory node data and the plurality of preset subgraph data can be effectively and accurately determined.

[0080] In S1033, the first matching pose is determined based on the subgraph pose information of the preset subgraph data corresponding to the highest matching score value in the matching score value.

[0081] Finally, the preset subgraph data corresponding to the highest matching score value in the matching score value can be used as the target subgraph data with the highest matching degree with the trajectory node data. Further, the first matching pose can be determined based on the subgraph pose information of the target subgraph data.

[0082] In an implementation manner, the relative pose relationship can be determined in the process of matching the trajectory node data with the preset subgraph data. Then, the subgraph pose information of the target subgraph data is adjusted based on the relative pose relationship to obtain the first matching pose.

[0083] S104, performing ceres nonlinear optimization processing on the first matching pose to obtain a second matching pose.

[0084] Next, ceres nonlinear optimization processing can be performed on the first matching pose of the coarse matching in S103 to obtain a fine matching pose, i.e., a second matching pose. In this way, the accuracy of the target vehicle repositioning can be further improved.

[0085] S105, performing pose initialization according to the second matching pose to obtain a repositioning result of the target vehicle, so as to control the target vehicle according to the repositioning result.

[0086] Finally, the INS module in the vehicle repositioning system can perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, i.e., initialize the INS module to realize repositioning. After the initialization of the INS module is completed, the real-time pose can be sent to the communication interaction module for updating the pose information of the target vehicle, for example, updating the pose information at a frequency of 1s to ensure real-time maintenance of the latest pose.

[0087] The vehicle repositioning method provided by the above-mentioned embodiments of the present application first acquires the environment and motion state data of the vehicle. Then, trajectory node data is generated based on the environment and motion state data, and the trajectory node data is used to represent the initial pose of the target vehicle and the point cloud information of the environment in which the target vehicle is located. Next, point cloud matching is performed based on the plurality of preset subgraph data and the trajectory node data to obtain a first matching pose. Further, ceres nonlinear optimization processing is performed on the first matching pose to obtain a second matching pose. Finally, a repositioning result can be obtained according to the second matching pose, and pose initialization is performed according to the repositioning result. This method combines laser point cloud data for point cloud matching to determine the pose of the target vehicle, which can improve the accuracy of vehicle repositioning. Moreover, the initial pose can be used to represent the final pose of the target vehicle before power-off, or the initial pose information can be used to represent the pose of the target vehicle after SLAM positioning fails and the vehicle is parked. Therefore, it is not necessary to perform global search matching in the entire map, and searching in the vicinity of the real pose can improve the efficiency of point cloud matching. In summary, this method can efficiently and accurately realize repositioning of the vehicle.

[0088] The present application also provides a vehicle repositioning device, specifically, Figure 4 The structure diagram of the vehicle repositioning device provided by the embodiments of the present application is shown in Figure 4 The vehicle repositioning device 400 includes a 3D SLAM module 401 and an INS module 402.

[0089] The 3D SLAM module 401 can be configured to receive environment and motion state data of the target vehicle, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generating trajectory node data based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; performing point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; and performing ceres non-linear optimization processing on the first matching pose to obtain a second matching pose.

[0090] The INS module 402 can be configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, and perform navigation control on the target vehicle based on the repositioning result.

[0091] In some embodiments, the environment and motion state data includes first point cloud data of the target vehicle and INS topic data. Figure 4 As shown in FIG. 4, the vehicle repositioning apparatus 400 further includes a communication interaction module 403.

[0092] Specifically, the communication interaction module 403 can be configured to receive initial pose information of the target vehicle and INS initial information, the initial pose information being used to represent a final pose of the target vehicle before power-off, or the initial pose information being used to represent a pose of the target vehicle after parking due to SLAM positioning failure; generate INS topic data based on the initial pose information and the INS initial information, and publish the INS topic data through a ROS topic; and collect first point cloud data through a laser radar module, and publish the first point cloud data through a ROS topic.

[0093] In some embodiments, the INS module 402 can be further configured to obtain INS initial information, and publish the INS initial information through a ROS topic.

[0094] In some embodiments, each preset subgraph data in the plurality of preset subgraph data includes corresponding subgraph pose information. In this case, the 3D SLAM module 401 is specifically configured to: first, construct a plurality of fast map matchers according to a number of the plurality of preset subgraph data; then, perform matching between the trajectory node data and the plurality of preset subgraph data according to a preset matching method through multi-thread asynchronous calling of the plurality of fast map matchers, determine a matching score value corresponding to each preset subgraph data in the plurality of preset subgraph data, and the matching score value is used to represent a point cloud occupancy score; and finally, determine the first matching pose based on subgraph pose information corresponding to a preset subgraph data with a highest matching score value in the matching score values.

[0095] In some embodiments, the environment and motion state data comprises: first point cloud data of the target vehicle and INS topic data. The trajectory node data comprises: initial pose information, first resolution point cloud data, second resolution point cloud data and point cloud rotation histogram. Then, the 3D SLAM module 401 is further specifically configured to: firstly, perform time synchronization and merging processing on the point cloud data in the first point cloud data to obtain second point cloud data. Then, perform first voxel filtering on the second point cloud data to obtain third point cloud data. Perform point cloud clipping on the third point cloud data to remove the point cloud data used to represent the target vehicle to obtain fourth point cloud data. Secondly, perform coordinate system conversion on the fourth point cloud data based on the INS topic data to obtain fifth point cloud data in a map coordinate system, and the map coordinate system is a coordinate system in which the initial pose information is located. Next, perform second voxel filtering processing on the fifth point cloud data based on a first preset resolution to obtain the first resolution point cloud data. Perform third voxel filtering processing on the fifth point cloud data based on a second preset resolution to obtain the second resolution point cloud data, wherein the second preset resolution is smaller than the first preset resolution. Finally, generate a point cloud rotation histogram according to the fifth point cloud data, point cloud attitude information of the fifth point cloud data and a preset segmentation angle resolution, and the point cloud rotation histogram is used to represent the spatial distribution of the point cloud in the fifth point cloud data.

[0096] The vehicle repositioning device provided by the above-mentioned embodiments of the present application first receives the environment and motion state data of the vehicle through the 3D SLAM module. Then, the trajectory node data is generated based on the environment and motion state data, and the trajectory node data is used to represent the initial pose of the target vehicle and the point cloud information of the environment in which the target vehicle is located. Next, the point cloud matching is performed based on the plurality of preset subgraph data and the trajectory node data to obtain the first matching pose. Further, the ceres nonlinear optimization processing is performed on the first matching pose to obtain the second matching pose. Finally, the repositioning result can be obtained according to the second matching pose through the INS module, so as to perform the pose initialization according to the repositioning result. The device combines the laser point cloud data to perform the point cloud matching and determine the pose of the target vehicle, which can improve the accuracy of the vehicle repositioning. Moreover, the initial pose can be used to represent the final pose of the target vehicle before power-off, or the initial pose information is used to represent the pose of the target vehicle after the SLAM positioning fails and the vehicle is parked. Therefore, the global search and matching of the full subgraph in the full map is not needed, and the search near the real pose can improve the efficiency of the point cloud matching. In summary, the vehicle repositioning device can efficiently and accurately reposition the vehicle.

[0097] The embodiments of the present application also provide a vehicle repositioning system, which comprises: an INS module, a communication interaction module and a 3D SLAM module.

[0098] The INS module can be configured to acquire INS initial information and publish the INS initial information via a ROS topic.

[0099] The communication interaction module can be configured to receive initial pose information of the target vehicle and INS initial information, the initial pose information being used to represent a final pose of the target vehicle before power-off or a pose of the target vehicle after parking due to SLAM positioning failure; generate INS topic data based on the initial pose information and the INS initial information, and publish the INS topic data via a ROS topic; and collect first point cloud data via the laser radar module and publish the first point cloud data via a ROS topic.

[0100] The 3D SLAM module can be configured to receive environment and motion state data of the target vehicle, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generate trajectory node data based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; perform point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; and perform ceres nonlinear optimization processing on the first matching pose to obtain a second matching pose.

[0101] The INS module can also be configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, so as to perform navigation control on the target vehicle based on the repositioning result.

[0102] The embodiment of the present application also provides a vehicle, which includes a vehicle body and a vehicle repositioning system as described above.

[0103] The embodiment of the present application also provides an electronic device, which can include a display screen, a memory and one or more processors. The display screen, the memory and the processors are coupled. The memory is configured to store computer program codes, the computer program codes including computer instructions. When the processors execute the computer instructions, the electronic device can execute each method or step executed in the vehicle repositioning method embodiments described above. Of course, the electronic device includes but is not limited to the display screen, the memory and the one or more processors.

[0104] The embodiment of the present application also provides a computer readable storage medium for storing computer instructions for running the vehicle repositioning method described above.

[0105] Through the description of the above embodiments, those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above functional modules is taken as an example, and in actual application, the above functions can be completed by different functional modules according to needs, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above.

[0106] In the description of the present application, it should be understood that the terms "first", "second" are only for the purpose of description, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of indicated technical features. Therefore, the features defined with "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "a plurality of" is at least two, for example, two, three, etc., unless otherwise specifically limited.

[0107] In the description of the present application, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In the present application, the illustrative description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any appropriate manner in any one or more embodiments or examples. In addition, the skilled in the art can combine and combine the different embodiments or examples described in the present application and the features of the different embodiments or examples without contradiction.

[0108] The similar parts between the embodiments provided in the present application can be referred to each other, and the specific embodiments provided above are only a few examples of the general concept of the present application, and do not constitute a limitation of the protection scope of the present application. For those skilled in the art, any other embodiments extended according to the present application scheme without creative labor are within the protection scope of the present application.

Claims

1. A vehicle repositioning method characterized by, Applied to a vehicle repositioning system, the method comprises: receiving environment and motion state data of a target vehicle, the environment and motion state data being used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generating trajectory node data based on the environment and motion state data, the trajectory node data being used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; performing point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; performing ceres nonlinear optimization processing on the first matching pose to obtain a second matching pose; performing pose initialization according to the second matching pose to obtain a repositioning result of the target vehicle, so as to control navigation of the target vehicle according to the repositioning result.

2. The method of claim 1, wherein, Each of the plurality of preset subgraph data includes corresponding subgraph pose information; The point cloud matching based on the plurality of preset subgraph data and the trajectory node data to determine the first matching pose comprises: constructing a plurality of fast map matchers according to the number of the plurality of preset subgraph data; determining a matching score value corresponding to each of the plurality of preset subgraph data by asynchronously calling the plurality of fast map matchers through multi-threading and matching the trajectory node data and the plurality of preset subgraph data according to a preset matching method, the matching score value being used to represent a point cloud occupancy score; determining the first matching pose based on the subgraph pose information corresponding to the preset subgraph data with the highest matching score value in the matching score values.

3. The method of claim 2, wherein, The environment and motion state data includes first point cloud data and INS topic data of the target vehicle; the trajectory node data includes initial pose information, first resolution point cloud data, second resolution point cloud data, and a point cloud rotation histogram; The generation of the trajectory node data based on the environment and motion state data comprises: performing time synchronization and merging processing on point cloud data in the first point cloud data to obtain second point cloud data; performing first voxel filtering on the second point cloud data to obtain third point cloud data; performing point cloud cropping on the third point cloud data to remove point cloud data used to represent the target vehicle to obtain fourth point cloud data; performing coordinate system conversion on the fourth point cloud data based on the INS topic data to obtain fifth point cloud data in a map coordinate system, the map coordinate system being a coordinate system where the initial pose information is located; performing second voxel filtering processing on the fifth point cloud data based on a first preset resolution to obtain the first resolution point cloud data; performing third voxel filtering processing on the fifth point cloud data based on a second preset resolution to obtain the second resolution point cloud data, wherein the second preset resolution is smaller than the first preset resolution; The point cloud rotation histogram is generated according to the fifth point cloud data, point cloud posture information of the fifth point cloud data and a preset segmentation angle resolution, and the point cloud rotation histogram is used to represent a spatial distribution of the point cloud in the fifth point cloud data.

4. The method of claim 3, wherein, The environment and motion state data of the target vehicle includes: The initial pose information of the target vehicle is used to represent a final pose of the target vehicle before power-off, or the initial pose information is used to represent a pose of the target vehicle after parking due to SLAM positioning failure. The INS topic data is generated based on the initial pose information and the INS initial information, and the INS topic data is received through a ROS topic. The first point cloud data is collected through a laser radar module, and the first point cloud data is received through a ROS topic.

5. The method of claim 4, wherein, The INS initial information includes motion state information of the target vehicle after power-on, and the motion state information is used to determine whether the target vehicle is in a stationary state.

6. A vehicle repositioning apparatus characterized by, The device includes a 3D SLAM module and an INS module. The 3D SLAM module is configured to receive environment and motion state data of a target vehicle, the environment and motion state data being used to represent environmental features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, trajectory node data is generated based on the environment and motion state data, the trajectory node data being used to represent initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; point cloud matching is performed based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, the plurality of preset subgraph data including a preset number of preset subgraph data closest to the initial pose; ceres non-linear optimization processing is performed on the first matching pose to obtain a second matching pose. The INS module is configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, and to perform navigation control on the target vehicle based on the repositioning result.

7. The apparatus of claim 6, wherein, The environment and motion state data of the target vehicle includes first point cloud data and INS topic data, and the device further includes a communication interaction module. The communication interaction module is configured to receive initial pose information and INS initial information of a target vehicle, the initial pose information being used to represent a final pose of the target vehicle before power-off, or the initial pose information being used to represent a pose of the target vehicle after parking due to SLAM positioning failure; the INS topic data is generated based on the initial pose information and the INS initial information, and the INS topic data is published through a ROS topic; the first point cloud data is collected through a laser radar module, and the first point cloud data is published through a ROS topic.

8. The apparatus of claim 7, wherein, The INS module is further configured to obtain the INS initial information and publish the INS initial information through a ROS topic.

9. A vehicle repositioning system characterized by, The system comprises an INS module, a communication interaction module and a 3D SLAM module, wherein The INS module is configured to acquire INS initial information and publish the INS initial information through a ROS topic; The communication interaction module is configured to receive initial pose information of a target vehicle and the INS initial information, wherein the initial pose information is used to represent a final pose of the target vehicle before power-off, or the initial pose information is used to represent a pose of the target vehicle after parking due to SLAM positioning failure; generate INS topic data based on the initial pose information and the INS initial information, and publish the INS topic data through a ROS topic; collect the first point cloud data through a laser radar module, and publish the first point cloud data through a ROS topic; The 3D SLAM module is configured to receive environment and motion state data of the target vehicle, wherein the environment and motion state data are used to represent environment features in a target area where the target vehicle is located and a motion state of the target vehicle; in a case where it is determined that the target vehicle is in a stationary state, generate trajectory node data based on the environment and motion state data, wherein the trajectory node data are used to represent an initial pose of the target vehicle and point cloud information of an environment where the target vehicle is located; perform point cloud matching based on a plurality of preset subgraph data and the trajectory node data to determine a first matching pose, wherein the plurality of preset subgraph data comprise a preset number of preset subgraph data closest to the initial pose; perform ceres nonlinear optimization processing on the first matching pose to obtain a second matching pose; The INS module is further configured to perform pose initialization based on the second matching pose to obtain a repositioning result of the target vehicle, and perform navigation control on the target vehicle based on the repositioning result.

10. A vehicle characterized by comprising: Comprise: A vehicle body and a vehicle repositioning system as claimed in claim 9 arranged on the vehicle body.

Images