Positioning method based on multi-feature information and electronic equipment

Abstract

The invention provides a multi-feature information-based positioning method and electronic equipment, and the method comprises the steps: obtaining point cloud data obtained by a laser radar in a motion process of a carrier, and extracting feature points in the point cloud data; and constructing a residual error from the feature points to a map, and performing iterative optimization based on the residual error to calculate and obtain the laser odometer. Dead reckoning information is obtained through calculation based on speed information obtained by the millimeter wave radar and pre-integration information obtained by the IMU, whether the laser speedometer degenerates or not is judged according to the dead reckoning information and the laser speedometer, and if the laser speedometer degenerates, pose updating of the carrier is carried out based on the dead reckoning information so as to achieve positioning of the carrier. According to the scheme, under the condition of degradation of the laser odometer, the laser odometer is optimized by combining a millimeter wave and IMU information fused odometer, so that the accuracy of the carrier pose is improved, and the safety and accuracy of driving control are guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of navigation control technology, and in particular to a positioning method and electronic equipment based on multi-feature information. Background Art

[0002] LiDAR technology uses laser beams to emit and receive reflected signals to obtain three-dimensional information about target objects. This technology has been widely used in autonomous driving, robot navigation, and geographic information systems.

[0003] Currently, the best-performing and most widely used simultaneous localization and mapping (SLAM) solutions are based on a series of solutions using real-time lidar odometry and mapping (LOAM). LOAM is a positioning and mapping solution designed primarily for multi-line lidars. In a LOAM solution, the front-end typically extracts features from the current lidar frame, and the back-end combines information from other sensors to match the current frame to the map for positioning.

[0004] In existing implementation methods, in some point cloud degraded scenarios, such as subway tunnel scenarios and highway scenarios, the geometric features of the point cloud data in these scenarios are almost the same or have no geometric features, which leads to easy mismatching when the point cloud data and map are matched and optimized, and thus it is impossible to accurately estimate the vehicle's own position based on the laser point cloud, which in turn affects the safety and accuracy of driving control. Summary of the Invention

[0005] The purpose of the embodiments of the present invention is to provide a positioning method and electronic device based on multi-feature information, so as to improve the accuracy of the carrier's position and posture and ensure the safety and accuracy of driving control.

[0006] In a first aspect, the present invention provides a positioning method based on multi-feature information, the method comprising:

[0007] During the movement of the carrier, point cloud data obtained by the laser radar is acquired, and feature points in the point cloud data are extracted;

[0008] Constructing a residual from the feature point to a pre-established map, and performing iterative optimization based on the residual to calculate a laser odometry;

[0009] The dead reckoning information is calculated based on the velocity information obtained by the millimeter-wave radar and the pre-integrated information obtained by the IMU;

[0010] Determining whether the laser odometry is degraded is performed based on the dead reckoning information and the laser odometry. If degradation occurs, updating the position and posture of the carrier is performed based on the dead reckoning information to achieve positioning of the carrier.

[0011] In an optional embodiment, the feature points include reflectivity feature points, and the reflectivity feature points are extracted from the point cloud data in the following manner:

[0012] Dividing a plurality of points in the point cloud data into a plurality of segments;

[0013] For each of the sections, obtaining the reflection intensity of each point within the section;

[0014] Based on the reflection intensity of each point, points that meet the reflectivity requirements are extracted as reflectivity feature points.

[0015] In an optional embodiment, the step of extracting points that meet reflectivity requirements based on the reflection intensity of each point includes:

[0016] Extract points whose reflection intensity ranks at the front preset position and whose reflection intensity is greater than the first preset intensity, or extract points whose reflectivity change rate ranks at the front preset position and whose reflectivity change rate is greater than the preset change rate and whose reflection intensity is greater than the second preset intensity, wherein the reflectivity change rate is calculated based on the emission intensity of each point and the points in its neighborhood.

[0017] In an optional embodiment, the feature points include line feature points, surface feature points and reflectivity feature points;

[0018] The step of constructing the residual from the feature point to the pre-established map comprises:

[0019] Residuals of the line feature points, the surface feature points and the reflectivity feature points to the line feature map, the surface feature map and the reflectivity feature map in the pre-established map are constructed respectively.

[0020] In an optional embodiment, the step of constructing a residual between the reflectivity feature point and a reflectivity feature map in a pre-established map includes:

[0021] Converting the reflectivity feature points into a map coordinate system of a reflectivity feature map in a pre-established map;

[0022] For each converted reflectivity feature point, searching for a reflectivity feature point adjacent to the reflectivity feature point in the reflectivity feature map;

[0023] constructing a reference plane based on the adjacent reflectivity feature points;

[0024] The distance between the reflectivity feature point and the reference plane is obtained as the residual to be optimized.

[0025] In an optional embodiment, the step of determining whether the laser odometry is degraded based on the dead reckoning information and the laser odometry includes:

[0026] Calculating, according to the laser odometry, a first displacement increment between two adjacent postures determined based on the point cloud data;

[0027] Calculating, according to the dead reckoning information, a second displacement increment between two adjacent postures determined based on the velocity information and the pre-integration information;

[0028] It is detected whether the difference between the first displacement increment and the second displacement increment is greater than a preset threshold value. If the difference is greater than the preset threshold value, it is determined that the laser odometer is degraded.

[0029] In an optional embodiment, the method further comprises:

[0030] Constructing a track vector map based on track positions in a pre-established map;

[0031] Determining whether the current posture needs to be optimized based on the trajectory vector map and the current posture of the carrier;

[0032] If it is determined that the current posture needs to be optimized, the optimization of the current posture is achieved based on the orbit information in the orbit vector map.

[0033] In an optional embodiment, the step of determining whether the current posture needs to be optimized based on the trajectory vector map and the current posture of the carrier includes:

[0034] Obtaining the spatial extent of the track and the centerline of the track in the track vector map;

[0035] Detecting whether position information in a current posture of the carrier exceeds a spatial range of the track, or detecting whether an angular difference between a heading angle in the current posture and a deflection angle determined by a centerline of the track exceeds a preset angular difference;

[0036] If the position information exceeds the spatial range, or the angle difference exceeds the preset angle difference, it is determined that the current posture needs to be optimized.

[0037] In an optional embodiment, the step of optimizing the current posture based on the orbit information in the orbit vector map includes:

[0038] Determine an intersection point between a circle defined by the position in the current posture as an origin and the displacement increment as a radius and a center line of the track in the track vector map, and use position information of the intersection point as optimized position information;

[0039] Determine two points on the center line of the track that are adjacent to the position in the current posture, calculate a deflection angle of the center line on a horizontal plane based on a line connecting the two points, and use the deflection angle as the optimized heading angle.

[0040] In a second aspect, the present invention provides an electronic device comprising a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor can execute the machine-executable instructions to implement any of the methods described in the foregoing embodiments.

[0041] The present invention provides a positioning method and electronic device based on multi-feature information. During the movement of a carrier, point cloud data obtained by a laser radar is acquired, and feature points in the point cloud data are extracted. The residual from the feature point to the map is constructed, and iterative optimization is performed based on the residual to calculate a laser odometry. Dead reckoning information is calculated based on the speed information obtained by the millimeter-wave radar and the pre-integration information obtained by the IMU. Based on the dead reckoning information and the laser odometry, it is determined whether the laser odometry has degraded. If degradation has occurred, the carrier's posture is updated based on the dead reckoning information to achieve the positioning of the carrier. In this solution, when the laser odometry has degraded, the odometry that combines millimeter-wave and IMU information is used to optimize the laser odometry, thereby improving the accuracy of the carrier's posture and ensuring the safety and accuracy of driving control. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. It should be understood that the following drawings only illustrate certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without paying any creative work.

[0043] Figure 1 A structural block diagram of an electronic device provided by an embodiment of the present invention;

[0044] Figure 2 A flowchart of a positioning method based on multi-feature information provided by an embodiment of the present invention;

[0045] Figure 3 is image information in a tunnel scene in an embodiment of the present invention;

[0046] Figure 4 This is point cloud information in a tunnel scene according to an embodiment of the present invention;

[0047] Figure 5 This is a flowchart of an optimization method in a positioning method based on multiple feature information according to an embodiment of the present invention;

[0048] Figure 6 This is a functional module block diagram of a positioning device based on multi-feature information provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0049] The technical solutions in the embodiments of the present invention will be described below with reference to the accompanying drawings in the embodiments of the present invention.

[0050] See also Figure 1 , is an electronic device provided in an embodiment of the present invention, and the multi-feature information-based positioning method provided in an embodiment of the present invention can be applied to this electronic device. The electronic device includes a memory, a processor, and a communication module. The memory, processor, and communication module are electrically connected to each other directly or indirectly to enable data transmission or exchange. For example, these components can be electrically connected to each other via one or more communication buses or signal lines.

[0051] Memory is used to store programs or data. Memory can be, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), and electrically erasable programmable read-only memory (EEPROM).

[0052] The processor is used to read / write data or programs stored in the memory and execute corresponding functions.

[0053] The communication module is used to establish a communication connection between the electronic device and other communication terminals through the network, and is used to send and receive data through the network.

[0054] It should be understood that Figure 1 The structure shown is only a schematic diagram of the structure of the electronic device. The electronic device may also include Figure 1 More or fewer components than shown, or with Figure 1 Different configurations shown. Figure 1 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0055] In some embodiments, the electronic device can be loaded into a carrier equipped with an inertial measurement unit (IMU), millimeter-wave radar, and lidar, and can serve as the carrier's control system or data processing system to execute the multi-feature information-based positioning method provided by the embodiments of the present invention, update the carrier's position and posture in real time, and ensure the accuracy of the carrier's positioning to facilitate subsequent better applications. The carrier can be, but is not limited to, a subway, a robot, a car, an aircraft, etc.

[0056] The following, combined Figure 2 The positioning method based on multi-feature information provided by the embodiment of the present invention is described. Figure 2 : is a flowchart of a positioning method based on multi-feature information provided by an embodiment of the present invention, the positioning method based on multi-feature information includes the following steps:

[0057] S11, during the movement of the carrier, obtaining point cloud data obtained by the laser radar and extracting feature points in the point cloud data.

[0058] S12, constructing the residual from the feature point to the pre-established map, and performing iterative optimization based on the residual to calculate the laser odometry.

[0059] S13, calculating dead reckoning information based on the velocity information obtained by the millimeter-wave radar and the pre-integration information obtained by the IMU.

[0060] S14, judging whether the laser odometer is degraded based on the dead reckoning information and the laser odometer, and if so, executing the following step S15, and if not, executing the following step S16.

[0061] S15: Update the position of the carrier based on the dead reckoning information to achieve positioning of the carrier.

[0062] S16, positioning the carrier based on the position determined by the laser odometry.

[0063] In this embodiment, the carrier is equipped with devices such as a laser radar, millimeter-wave radar, and an IMU. As the carrier moves, each device performs real-time detection to obtain relevant data. The laser radar measures the distance to surrounding objects by emitting a laser beam and receiving reflected signals. Each reflected signal from the laser beam forms a three-dimensional coordinate point, which in turn forms point cloud data of the surrounding environment.

[0064] In addition, a map is pre-established, and the way this map is established is similar to the real-time positioning method when the carrier is moving. That is, the point cloud data of the surrounding environment needs to be obtained during the real-time positioning process when the carrier is moving, and this point cloud data is also the data required for map construction.

[0065] The point cloud data obtained during the carrier's motion is matched against a pre-established map. Feature points are extracted from the point cloud data for matching. Feature points with distinct characteristics can be extracted, such as those on a plane, on a line, or in a highlighted area. Using these distinct feature points for matching against the pre-established map improves matching efficiency and accuracy.

[0066] When matching feature points with the map, residuals from the feature points to the map are constructed. The laser odometry is determined by iteratively optimizing the residuals from the feature points to the map in consecutive adjacent lidar frames. This iterative optimization can employ an iterative optimization method that combines the scan2map and LM (Levenberg-Marquardt) algorithms commonly used in the SLAM field. The LM algorithm is an optimization algorithm that combines the Gauss-Newton method and the gradient descent method. It is a commonly used iterative optimization algorithm in this field and will not be described in detail in this embodiment. The laser odometry can be understood as the position information of the carrier estimated based on continuous point cloud data.

[0067] When in scenes such as subway tunnels and highways, the geometric features of the point cloud data of the surrounding environment change very little. Therefore, the point cloud features are degraded, and the pose information of the carrier estimated based on the point cloud data may also be degraded, that is, the pose information is not accurate enough.

[0068] Furthermore, the millimeter-wave radar onboard the vehicle can obtain velocity information, which can be the relative velocity of the vehicle relative to its surroundings. The onboard IMU can detect and obtain acceleration and angular velocity information during the vehicle's motion. The acceleration and angular velocity information of consecutive frames is integrated and accumulated to generate pre-integrated information.

[0069] Combining the velocity information provided by the millimeter-wave radar with the pre-integrated information from the IMU, we can calculate dead reckoning information. Dead reckoning is a method that uses velocity information, relative displacement, and time intervals to estimate the next moment's pose from a known initial position. Here, dead reckoning information can be understood as the pose information derived from this method.

[0070] As can be seen from the above, the pose information estimated from point cloud data may be inaccurate. In this embodiment, the obtained dead reckoning information can be combined to determine whether the laser odometry is degraded. In other words, whether the pose information estimated based on the point cloud data is degraded. For example, by comparing the dead reckoning information with the laser odometry information, the size of the difference can be used to determine whether the laser odometry is degraded.

[0071] When it is determined that the laser odometer is degraded, the position and posture of the carrier can be updated based on the obtained dead reckoning information, and the positioning information of the carrier can be determined based on the updated position and posture.

[0072] In the positioning method based on multi-feature information provided in this embodiment, when the laser odometry is degraded, the odometry that combines millimeter wave and IMU information fusion is used to optimize the laser odometry, thereby improving the accuracy of the carrier's position and posture, and ensuring the safety and accuracy of driving control.

[0073] Furthermore, when this positioning method is applied in a tunnel scenario, since tunnels are enclosed spaces, carrier positioning is limited to the track within the tunnel. Therefore, this embodiment also introduces a track vector map constructed based on a map built from point cloud data, i.e., a vector map primarily containing track information. This track vector map is used to detect whether the position information determined in the aforementioned manner satisfies the actual track conditions in the track vector map. If not, the position can be further optimized based on the track information in the track vector map. This further improves the rationality and accuracy of the positioning results.

[0074] The specific implementation of each of the above steps will be described in detail below.

[0075] In existing methods, when extracting feature points from point cloud data and matching them with a map, matching is generally performed by extracting line feature points and surface feature points from the point cloud data.

[0076] Line feature points are points in a point cloud that can reflect the edge, outline, or straight line structure of an object. By calculating the curvature of the points in the point cloud, points with significant curvature changes can be identified as line feature points. Surface feature points are points in a point cloud that can reflect the plane of an object's surface. These points are typically located in the same plane. By calculating the covariance matrix of the points in the point cloud, we can identify a set of points with significantly different eigenvalue distributions. The points in this set are considered surface feature points.

[0077] However, in scenarios such as tunnels, line feature points and surface feature points cannot reflect the uniqueness of features. Therefore, when performing map matching based on line feature points and surface feature points, incorrect matching may occur, thereby affecting the accuracy of mapping and positioning.

[0078] Tunnel scenes generally have infrastructure such as signboards, which generally have highly reflective surfaces. Therefore, such infrastructure is relatively unique in the scene, which is beneficial for map matching. Figure 3 and Figure 4 In the figure, there are images of tunnel scenes and point cloud data, respectively. The area enclosed by the box is the area with high reflective properties.

[0079] Based on this, reflectivity feature points are introduced in this embodiment. Reflectivity feature points are feature points corresponding to such highly reflective infrastructure. Map matching is achieved by combining reflectivity feature points, line feature points and surface feature points.

[0080] In this embodiment, in the step of extracting feature points from the point cloud data, the reflectivity feature points are extracted from the point cloud data in the following manner:

[0081] Multiple points in the point cloud data are divided into multiple segments; for each segment, the reflection intensity of each point in the segment is obtained; based on the reflection intensity of each point, points that meet the reflectivity requirements are extracted as reflectivity feature points.

[0082] The laser beam emitted by a LiDAR consists of multiple lines and spans a certain angle range in the horizontal field of view. To ensure that the reflectivity feature points ultimately extracted are evenly distributed within the field of view, the multiple points in the point cloud data can be divided into multiple segments.

[0083] The segmentation can be based on the line bundle and its horizontal field of view. For example, if the line bundle consists of 256 lines and the horizontal field of view is 120 degrees, 256*6 segments can be obtained. Reflectivity feature points are then extracted in each segment. This ensures that the extracted reflectivity feature points are evenly distributed across the point cloud.

[0084] When extracting reflectivity feature points in each segment, the reflection intensity of each point within the segment can be obtained. The reflection intensity can be determined based on the power of the echo signal after the laser radar emits a laser beam and receives the reflected echo signal.

[0085] The reflection intensity can reflect the reflectivity and other properties of an object. Therefore, based on the reflection intensity of each point, points that meet the reflectivity requirements can be screened out as reflectivity feature points.

[0086] As a possible implementation manner, a point whose reflection intensity ranks in a front preset position and whose reflection intensity is greater than a first preset intensity may be extracted as a reflectivity feature point.

[0087] Specifically, the reflection intensities of the points within the segment can be sorted from largest to smallest, and points with reflection intensities greater than a first preset intensity are determined. The first preset intensity can be, for example, 128, etc. Based on the determined points, points ranked in a front preset position are screened out. The front preset position can be, for example, the top 10, etc. The points screened out in this manner can be determined as reflectivity feature points.

[0088] In addition, as another possible implementation, points whose reflectivity change rate ranks first in a preset position, whose reflectivity change rate is greater than a preset change rate, and whose reflection intensity is greater than a second preset intensity can be extracted as reflectivity feature points. The reflectivity change rate is calculated based on the reflection intensity of each point and the points in its neighborhood.

[0089] Specifically, each point in the obtained point cloud data is generally numbered according to a specified numbering scheme, for example, 1-n, where n is the number of points in the point cloud. For point i in segment a, the reflection intensity intensity_i of point i can be obtained, and the reflection intensities of the points within the neighborhood of point i can be obtained. The points within the neighborhood of point i can refer to points within a certain range centered on point i, for example, points i-5 to i-1, and points i+1 to i+5.

[0090] Based on the reflection intensity of point i and its neighborhood, the reflectivity change rate of point i is calculated as follows:

[0091]

[0092] The reflectivity change rates of each point within the segment are calculated using the above method and can be arranged in descending order. Points with a reflection intensity greater than a second preset intensity are identified. The second preset intensity can be, for example, 32, etc. Based on this, points with a reflectivity change rate greater than a preset change rate are screened out. The preset change rate can be, for example, 10,000, etc. Finally, points with a reflectivity change rate ranked in the top preset positions can be extracted as reflectivity feature points. The top preset positions can be, for example, the top 10, etc., etc.

[0093] Since the number of points in the point cloud obtained by the lidar is huge, in order to avoid the problem of excessive workload, when using any of the above methods to extract reflectivity feature points, if the reflectivity feature point is determined within the segment, the points in the area around the reflectivity feature point can be masked, and the masked points will not participate in the subsequent extraction of reflectivity feature points.

[0094] In this way, the number of points that need to be detected can be reduced. Moreover, when the reflectivity feature points have been determined within a certain area, the determined reflectivity feature points can be used for map matching without combining them with other reflectivity feature points in the surrounding area. Therefore, the mask operation does not reduce the accuracy of the matching.

[0095] Based on the extraction of line feature points, surface feature points, and reflectivity feature points from the point cloud data, map matching can be performed by combining the line feature points, surface feature points, and reflectivity feature points. In this embodiment, in the above-mentioned step of constructing the residual from the feature points to the pre-established map, this can be achieved by the following methods:

[0096] Residuals of the line feature points, surface feature points and reflectivity feature points to the line feature map, surface feature map and reflectivity feature map in the pre-established map are constructed respectively.

[0097] In this embodiment, the pre-established maps can be divided into line feature maps, surface feature maps, and reflectivity feature maps. Among them, the line feature map is a map constructed based on line feature points, the surface feature map is a map constructed based on surface feature points, and the reflectivity feature map is a map constructed based on reflectivity feature points.

[0098] During map matching, the linear feature residuals from line feature points to the linear feature map, the surface feature residuals from surface feature points to the surface feature map, and the reflectivity feature residuals from reflectivity feature points to the reflectivity feature map are constructed. Finally, these three residuals are combined and iteratively optimized to calculate the current laser odometry.

[0099] Among them, constructing the residual from the reflectivity feature point to the reflectivity feature map can be achieved in the following ways:

[0100] The reflectivity feature points are converted into the map coordinate system of the reflectivity feature map in the pre-established map; for each reflectivity feature point after conversion, the reflectivity feature points adjacent to the reflectivity feature point are searched in the reflectivity feature map; a reference plane is constructed based on the adjacent reflectivity feature points; and the distance between the reflectivity feature point and the reference plane is obtained as the residual to be optimized.

[0101] In this embodiment, a reflectivity feature point is first converted to a map coordinate system. A reflectivity feature map is then searched for multiple reflectivity feature points that are closest to the reflectivity feature point, which are used as neighboring reflectivity feature points. For example, three neighboring reflectivity feature points can be searched and determined. A plane is constructed based on the three neighboring reflectivity feature points identified in the search, serving as a reference plane. The method for constructing a plane based on multiple points can be implemented using existing methods, which are not detailed in this embodiment.

[0102] Based on the position of the reflectivity feature point and the plane equation of the reference plane, the distance from the reflectivity feature point to the reference plane is determined. This distance is the residual to be optimized.

[0103] In addition, the method for obtaining the residual from the line feature point to the line feature map and the residual from the surface feature point to the surface feature map is similar to the method for obtaining the residual from the reflectivity feature point to the reflectivity feature map, and is not repeated here in this embodiment.

[0104] After obtaining the residuals from the reflectivity feature points to the reflectivity feature map, the residuals from the line feature points to the line feature map, and the residuals from the surface feature points to the surface feature map, the scan2map method in SLAM can be used for multiple iterative optimizations to determine the position and posture of the carrier, that is, the current laser odometry.

[0105] In addition, the dead reckoning information is calculated by combining the velocity information detected by the millimeter-wave radar and the pre-integration information obtained by the IMU.

[0106] During a vehicle's motion, the majority of the signals returned by millimeter-wave radar detection are environmental information, such as those from tunnel walls, the ground, and surrounding static infrastructure. In addition, a small amount of return signals are also from dynamic targets, such as passengers waiting at stations and vehicles on public roads adjacent to open-air sections. Therefore, the velocity information detected by millimeter-wave radar is the relative velocity of the vehicle.

[0107] Among them, the relative speed information of different static targets and dynamic targets may not be consistent. In this embodiment, by constructing a histogram of the obtained relative speed information, the relative speed information in the statistical interval with the largest number of relative speed information is used as the current relative speed information of the carrier.

[0108] The IMU's pre-integrated information provides relative displacement and attitude changes. The vehicle's relative velocity is converted to relative velocity in the map coordinate system using the Euler angles from the IMU. The relative velocity and displacement information are added to the initial position to calculate the current position. This information, combined with the Euler angles from the IMU's pre-integrated information, is used to obtain dead reckoning information, including attitude and position information.

[0109] The following example shows the pseudo code for obtaining dead reckoning information:

[0110]

[0111] On this basis, the above step of determining whether the laser odometry is degraded based on the dead reckoning information and the laser odometry can be achieved by the following methods:

[0112] A first displacement increment between two adjacent postures determined based on point cloud data is calculated based on the laser odometry; a second displacement increment between two adjacent postures determined based on velocity information and pre-integration information is calculated based on dead reckoning information; and a difference between the first displacement increment and the second displacement increment is detected to determine whether it is greater than a preset threshold. If so, it is determined that the laser odometry has degraded.

[0113] In this embodiment, the displacement increment determined based on the point cloud data and the displacement increment determined based on the velocity information and pre-integration information are calculated separately. When the difference between the two displacement increments exceeds a preset threshold, it indicates that the pose determined based on the point cloud data may be degraded, that is, the laser odometry is degraded. The preset threshold can be set based on needs, for example, it can be 0.2.

[0114] If the laser odometry is degraded, the current pose can be updated based on the calculated dead reckoning information. Based on this, the scan2map LM algorithm can be used for iterative optimization based on the updated current pose. The number of iterations for this iterative optimization can be set to a small number, for example, 1. The purpose of performing LM optimization after updating the pose based on dead reckoning information is to prevent inconsistencies between the current point cloud and the map after the dead reckoning information updates the current pose. Performing iterative optimization ensures consistency between the current point cloud and the map. The low number of iterations is intended to prevent over-optimization caused by incorrect association of point cloud features in degraded scenarios such as tunnels.

[0115] The following is an example of pseudo code for determining whether the laser odometer is degraded:

[0116]

[0117] lastLaserPose=currentLaserPose;

[0118] As can be seen from the above, this embodiment also introduces a track vector map to further optimize the current posture. Figure 5 The positioning method based on multi-feature information provided in this embodiment may further include the following steps:

[0119] S17, constructing a track vector map based on the track positions in the pre-established map.

[0120] S18, based on the track vector map and the current posture of the carrier, determine whether the current posture needs to be optimized. If it is determined that the current posture needs to be optimized, execute the following step S19, otherwise exit the optimization process.

[0121] S19, optimizing the current position based on the orbit information in the orbit vector map.

[0122] In the tunnel scenario, the vehicle can only travel on the lane, so the track vector map is constructed based on the track position in the map, such as Figure 3 and Figure 4 The figure shows the point cloud map and track vector map in the tunnel scene, where the solid line part is the constructed track vector map.

[0123] When judging whether the current posture needs to be optimized based on the orbit vector map, it is mainly achieved by judging whether the position of the carrier exceeds the orbit range in the orbit vector map, whether the carrier's forward direction deviates too much from the direction of the orbit, etc.

[0124] Specifically, based on the trajectory vector map and the current posture of the carrier, the step of determining whether the current posture needs to be optimized can be achieved by:

[0125] Obtain the spatial range of the track and the centerline of the track in the track vector map; detect whether the position information in the current posture of the carrier exceeds the spatial range of the track, or detect whether the angle difference between the heading angle in the current posture and the deflection angle determined by the centerline of the track exceeds the preset angle difference; if the position information exceeds the spatial range, or the angle difference exceeds the preset angle difference, it is determined that the current posture needs to be optimized.

[0126] The spatial extent of a track in a track vector map refers to the range between the edge lines on both sides of the track, and the centerline of the track is the line midway between the edge lines on both sides. The current position of the carrier includes position information and attitude information, where the attitude information includes the heading angle.

[0127] Based on the position information of the carrier and the position range defined by the spatial range of the track, it can be determined whether the position information of the carrier exceeds the spatial range of the track. Since the carrier can only travel on the track, if the position information of the carrier exceeds the spatial range of the track, it indicates that the currently determined position information may have errors. In addition, the heading angle of the carrier indicates the direction of the carrier's advance, and the deflection angle determined by the center line of the track can characterize the direction of the track. The deflection angle determined by the heading angle of the carrier and the center line can be compared. If the angle difference between the two exceeds the preset angle difference, it indicates that the deviation between the forward direction of the carrier and the direction pointed by the center line is too large. At this time, it indicates that the heading angle in the current posture of the carrier may be deviated.

[0128] When determining the deflection angle based on the center line, two points on the center line with a certain distance between them can be located, and the deflection angle can be determined based on the angle between the line connecting the two points and the coordinate axis on the horizontal plane.

[0129] If it is determined that the position information of the carrier is deviated, or the deflection angle is deviated, it is determined that the current posture needs to be optimized.

[0130] In this embodiment, the step of optimizing the current posture based on the track information in the track vector map can be achieved by:

[0131] Determine the intersection of a circle with the position in the current posture as the origin and the displacement increment as the radius and the center line of the track in the track vector map, and use the position information of the intersection as the optimized position information; determine two points on the center line of the track that are adjacent to the position in the current posture, calculate the deflection angle of the center line in the horizontal plane based on the line connecting the two points, and use the deflection angle as the optimized heading angle.

[0132] In this embodiment, when optimizing the position information of the carrier, a circle can be drawn with the position in the current posture as the origin and the displacement increment as the radius. The circle has an intersection with the center line of the track in the track vector map, and the position information of the intersection can be used as the optimized position information.

[0133] In addition, when optimizing the heading angle of the carrier, the two points closest to the position can be found on the center line of the track based on the position information in the current posture, and the two points found are connected. The angle between the connecting line and the coordinate axis on the horizontal plane is the deflection angle, which can be used as the optimized heading angle.

[0134] Finally, the carrier can be positioned based on the optimized position information and heading angle.

[0135] The following is an example of pseudo code for performing pose optimization based on a trajectory vector map:

[0136]

[0137]

[0138] The positioning method based on multi-feature information provided in this embodiment, in order to solve the mapping and positioning problems in scenarios with degraded point cloud features, first extracts reflectivity feature points from the point cloud, and then improves the accuracy of matching the feature points with the map based on the relative uniqueness of the reflectivity feature points, thereby improving the accuracy of positioning.

[0139] In addition, the odometry that combines millimeter wave and IMU information fusion is used to optimize the degraded laser odometry, avoiding the problem of deviation in the pose determined by the laser odometry due to the degradation of point cloud features.

[0140] Furthermore, considering that the carrier generally only travels on the track, the track vector map is introduced, and the carrier posture is further optimized based on the track vector map, thereby further improving the accuracy and robustness of carrier positioning.

[0141] In order to execute the corresponding steps in the above-mentioned positioning method embodiment and various possible methods, a method for implementing a positioning device based on multiple feature information is given below. Optionally, the positioning device based on multiple feature information can adopt the above-mentioned Figure 1 The device structure of the electronic device shown.

[0142] Further, see Figure 6 , Figure 6 This is a functional module diagram of a positioning device based on multi-feature information provided by an embodiment of the present invention. It should be noted that the basic principles and technical effects of the positioning device based on multi-feature information provided by this embodiment are the same as those of the corresponding method embodiments described above. For the sake of simplicity, any parts not mentioned in this embodiment can be referred to the corresponding contents of the corresponding method embodiments described above. The positioning device based on multi-feature information includes:

[0143] An extraction module is used to obtain point cloud data obtained by the laser radar during the movement of the carrier and extract feature points in the point cloud data;

[0144] A construction module is used to construct the residuals from feature points to a pre-established map and perform iterative optimization based on the residuals to calculate the laser odometry;

[0145] A calculation module is used to calculate dead reckoning information based on the velocity information obtained by the millimeter wave radar and the pre-integrated information obtained by the IMU;

[0146] A judgment module, used to judge whether the laser odometry is degraded based on the dead reckoning information and the laser odometry;

[0147] The updating module is used to update the position and posture of the carrier based on the dead reckoning information when the judging module determines that degradation has occurred, so as to achieve the positioning of the carrier.

[0148] As a possible implementation, the feature points include reflectivity feature points, and the extraction module is used to extract the reflectivity feature points in the following manner:

[0149] Divide multiple points in the point cloud data into multiple segments;

[0150] For each section, obtain the reflection intensity of each point in the section;

[0151] Based on the reflection intensity of each point, points that meet the reflectivity requirements are extracted as reflectivity feature points.

[0152] As a possible implementation method, the above extraction module is used to extract points that meet the reflectivity requirements in the following way:

[0153] Extract points whose reflection intensity ranks at the front preset position and whose reflection intensity is greater than the first preset intensity, or extract points whose reflectivity change rate ranks at the front preset position and whose reflectivity change rate is greater than the preset change rate and whose reflection intensity is greater than the second preset intensity, wherein the reflectivity change rate is calculated based on the emission intensity of each point and the points in its neighborhood.

[0154] As a possible implementation method, feature points include line feature points, surface feature points, and reflectivity feature points. The above construction module is used to construct the residual from feature points to the map in the following way:

[0155] Residuals of the line feature points, surface feature points and reflectivity feature points to the line feature map, surface feature map and reflectivity feature map in the pre-established map are constructed respectively.

[0156] As a possible implementation, the above construction module is used to construct the residual between the reflectivity feature points and the reflectivity feature map in the following manner:

[0157] Converting the reflectivity feature points into a map coordinate system of a reflectivity feature map in a pre-established map;

[0158] For each converted reflectivity feature point, searching for adjacent reflectivity feature points of the reflectivity feature point in the reflectivity feature map;

[0159] Construct a reference plane based on neighboring reflectivity feature points;

[0160] The distance between the reflectivity feature point and the reference plane is obtained as the residual to be optimized.

[0161] As a possible implementation, the above-mentioned determination module is used to determine whether the laser odometer is degraded by:

[0162] A first displacement increment between two adjacent poses determined based on the point cloud data is calculated according to the laser odometry;

[0163] Calculating, according to the dead reckoning information, a second displacement increment between two adjacent postures determined based on the velocity information and the pre-integration information;

[0164] It is detected whether the difference between the first displacement increment and the second displacement increment is greater than a preset threshold value. If the difference is greater than the preset threshold value, it is determined that the laser odometer is degraded.

[0165] As a possible implementation, the positioning device based on multi-feature information may further include an optimization module, which may be used to:

[0166] Constructing a track vector map based on track positions in a pre-established map;

[0167] According to the orbit vector map and the current position of the carrier, determine whether the current position needs to be optimized;

[0168] If it is determined that the current posture needs to be optimized, the current posture is optimized based on the orbit information in the orbit vector map.

[0169] As a possible implementation method, the above optimization module determines whether the current posture needs to be optimized in the following way:

[0170] Get the spatial extent and centerline of the track in the track vector map;

[0171] Detecting whether the position information in the current posture of the carrier exceeds the spatial range of the track, or detecting whether the angle difference between the heading angle in the current posture and the deflection angle determined by the centerline of the track exceeds a preset angle difference;

[0172] If the position information exceeds the spatial range, or the angle difference exceeds the preset angle difference, it is determined that the current posture needs to be optimized.

[0173] As a possible implementation method, the above optimization module is used to optimize the current posture in the following way:

[0174] Determine the intersection of a circle with the current position as the origin and the displacement increment as the radius and the center line of the track in the track vector map, and use the position information of the intersection as the optimized position information;

[0175] Determine two points on the centerline of the track that are adjacent to the position in the current pose, calculate the deflection angle of the centerline on the horizontal plane based on the line connecting the two points, and use the deflection angle as the optimized heading angle.

[0176] Optionally, the above modules can be stored in the form of software or firmware. Figure 1 The memory shown in FIG. 1 or the operating system (OS) of the electronic device may be fixed and may be Figure 1 Meanwhile, the data and program codes required to execute the above modules may be stored in the memory.

[0177] In the several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architectures, functions, and operations of the devices, methods, and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, program segment, or part of the code, which contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the boxes can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, as well as the combination of boxes in the block diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified functions or actions, or can be implemented using a combination of dedicated hardware and computer instructions.

[0178] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0179] If the function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the various embodiments of the present invention. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0180] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A positioning method based on multi-feature information, characterized in that: The method comprises: During the movement of the carrier, point cloud data obtained by the laser radar is acquired, and feature points in the point cloud data are extracted; Constructing a residual from the feature point to a pre-established map, and performing iterative optimization based on the residual to calculate a laser odometry; The dead reckoning information is calculated based on the velocity information obtained by the millimeter-wave radar and the pre-integrated information obtained by the IMU; Determining whether the laser odometry is degraded is performed based on the dead reckoning information and the laser odometry. If degradation occurs, updating the position and posture of the carrier is performed based on the dead reckoning information to achieve positioning of the carrier.

2. The positioning method based on multi-feature information according to claim 1, characterized in that: The feature points include reflectivity feature points, which are extracted from the point cloud data in the following manner: Dividing a plurality of points in the point cloud data into a plurality of segments; For each of the sections, obtaining the reflection intensity of each point within the section; Based on the reflection intensity of each point, points that meet the reflectivity requirements are extracted as reflectivity feature points.

3. The positioning method based on multi-feature information according to claim 2, characterized in that: The step of extracting points that meet reflectivity requirements based on the reflection intensity of each point includes: Extract points whose reflection intensity ranks at the front preset position and whose reflection intensity is greater than the first preset intensity, or extract points whose reflectivity change rate ranks at the front preset position and whose reflectivity change rate is greater than the preset change rate and whose reflection intensity is greater than the second preset intensity, wherein the reflectivity change rate is calculated based on the emission intensity of each point and the points in its neighborhood.

4. The positioning method based on multi-feature information according to claim 1, characterized in that: The feature points include line feature points, surface feature points and reflectivity feature points; The step of constructing the residual from the feature point to the pre-established map comprises: Residuals of the line feature points, the surface feature points and the reflectivity feature points to the line feature map, the surface feature map and the reflectivity feature map in the pre-established map are constructed respectively.

5. The positioning method based on multi-feature information according to claim 4, characterized in that: The step of constructing the residual between the reflectivity feature point and the reflectivity feature map in the pre-established map comprises: Converting the reflectivity feature points into a map coordinate system of a reflectivity feature map in a pre-established map; For each converted reflectivity feature point, searching for a reflectivity feature point adjacent to the reflectivity feature point in the reflectivity feature map; constructing a reference plane based on the adjacent reflectivity feature points; The distance between the reflectivity feature point and the reference plane is obtained as the residual to be optimized.

6. The positioning method based on multi-feature information according to claim 1, characterized in that: The step of determining whether the laser odometry is degraded based on the dead reckoning information and the laser odometry comprises: Calculating, according to the laser odometry, a first displacement increment between two adjacent postures determined based on the point cloud data; Calculating, according to the dead reckoning information, a second displacement increment between two adjacent postures determined based on the velocity information and the pre-integration information; It is detected whether the difference between the first displacement increment and the second displacement increment is greater than a preset threshold value. If the difference is greater than the preset threshold value, it is determined that the laser odometer is degraded.

7. The positioning method based on multi-feature information according to claim 1, characterized in that: The method further comprises: Constructing a track vector map based on track positions in a pre-established map; Determining whether the current posture needs to be optimized based on the trajectory vector map and the current posture of the carrier; If it is determined that the current posture needs to be optimized, the optimization of the current posture is achieved based on the orbit information in the orbit vector map.

8. The positioning method based on multi-feature information according to claim 7, characterized in that: The step of determining whether the current posture needs to be optimized based on the track vector map and the current posture of the carrier includes: Obtaining the spatial extent of the track and the centerline of the track in the track vector map; Detecting whether position information in a current posture of the carrier exceeds a spatial range of the track, or detecting whether an angular difference between a heading angle in the current posture and a deflection angle determined by a centerline of the track exceeds a preset angular difference; If the position information exceeds the spatial range, or the angle difference exceeds the preset angle difference, it is determined that the current posture needs to be optimized.

9. The positioning method based on multi-feature information according to claim 7, characterized in that: The step of optimizing the current posture based on the orbit information in the orbit vector map includes: Determine an intersection point between a circle defined by the position in the current posture as an origin and the displacement increment as a radius and a center line of the track in the track vector map, and use position information of the intersection point as optimized position information; Determine two points on the center line of the track that are adjacent to the position in the current posture, calculate a deflection angle of the center line on a horizontal plane based on a line connecting the two points, and use the deflection angle as the optimized heading angle.

10. An electronic device, characterized in that: The invention comprises a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor can execute the machine-executable instructions to implement the method according to any one of claims 1 to 9.

Images