An automatic driving vehicle pose updating method, device, equipment and medium

Abstract

The application relates to the technical field of automatic driving cars, and discloses an automatic driving car pose updating method, device, equipment and medium, the method comprising the following steps: acquiring driving environment image data, driving environment laser point cloud data and vehicle original pose information of an automatic driving car; extracting first feature points in the driving environment image data and second feature points in the driving environment laser point cloud data, performing depth assignment on the first feature points according to the second feature points to obtain feature points with depth information; performing pose estimation according to the feature points with depth information and the vehicle original pose information to obtain a pose estimation result; and updating the vehicle original pose information according to the pose estimation result to obtain new vehicle pose information. The image data in a vision system and the point cloud data in a laser radar system are combined, and the vehicle original pose information is used for pose estimation together with the combined data, so that the positioning accuracy, the stability and the robustness of the positioning system are improved.

Description

Technical Field

[0001] This application relates to the field of autonomous vehicle technology, specifically to an autonomous vehicle pose update method, device, equipment, and medium. Background Technology

[0002] Location positioning plays an irreplaceable role in autonomous driving and has promising future development. Currently, location positioning in autonomous driving relies on RTK (Real-Time Kinematic) combined with high-precision maps, which adds considerable cost and difficulty to the deployment of autonomous driving. In the field of autonomous driving technology, the car does not need to know its global high-precision positioning and the detailed environment around it; it only needs a global navigation path and the vehicle's position on that path. Determining the car's position on the path is the key technology of SLAM (Simultaneous Localization and Mapping).

[0003] SLAM is a fundamental capability required for many mobile robot navigation tasks and is now widely used. SLAM has achieved great success in real-time state estimation and mapping using a single perception sensor (such as LiDAR or a camera) in challenging environments. LiDAR-based methods can capture environmental details at long distances; however, they often fail in environments with weak structure, such as long corridors or flat, open fields where line or surface features are not clearly defined. Vision-based methods are particularly well-suited for position recognition and perform well in textured environments, but their performance is highly sensitive to changes in lighting, rapid movement, and initialization. Both of these methods, when used individually, are affected by environmental structure or texture, leading to inaccurate localization in autonomous vehicles and lower stability and robustness of the localization system.

[0004] Therefore, improving the accuracy of positioning for autonomous vehicles, as well as the stability and robustness of the positioning system, is an urgent problem that needs to be solved. Summary of the Invention

[0005] In view of the shortcomings of the prior art described above, the present invention provides a method, apparatus, device and medium for updating the pose of an autonomous vehicle, in order to solve the above-mentioned technical problems.

[0006] In one embodiment of the present invention, an autonomous vehicle pose update method is provided, the method being applied to an on-board diagnostic device, the method comprising:

[0007] Acquire image data of the driving environment of autonomous vehicles, laser point cloud data of the driving environment, and original vehicle pose information;

[0008] Extract the first feature point from the driving environment image data and the second feature point from the driving environment laser point cloud data, and assign a depth value to the first feature point based on the second feature point to obtain a feature point with depth information;

[0009] Based on the feature points with depth information and the original vehicle pose information, pose estimation is performed to obtain the pose estimation result.

[0010] The original vehicle pose information is updated based on the pose estimation result to obtain new vehicle pose information.

[0011] In one embodiment of the present invention, the step of assigning a depth value to the first feature point based on the second feature point to obtain a feature point with depth information includes:

[0012] Project the first and second feature points onto a unit sphere centered at the camera;

[0013] The positions of the first and second feature points are represented in polar coordinates.

[0014] The depth information is obtained by searching the three nearest second feature points of each first feature point using a search tree, and the length of the line between the intersection of the line from the camera optical center to each first feature point and the plane determined by the three nearest second feature points is used as the depth information.

[0015] Each first feature point is assigned a value based on the depth information to obtain the feature point with depth information.

[0016] In one embodiment of the present invention, after acquiring the driving environment image data, driving environment laser point cloud data, and original vehicle pose information of the autonomous vehicle, the method further includes:

[0017] The reprojection error of the driving environment image data and the measurement error of the vehicle's original pose information are obtained.

[0018] The reprojection error and measurement error are nonlinearly optimized by a pre-configured sliding window estimator to reduce the reprojection error and measurement error.

[0019] Based on the reduced reprojection error and measurement error, visual feature tracking is performed on the driving environment image data to obtain visual feature tracking feature points, and distorted point cloud data in the driving environment laser point cloud data is removed based on the visual feature tracking feature points.

[0020] In one embodiment of the present invention, before performing pose estimation based on the feature points with depth information and the original vehicle pose information to obtain the pose estimation result, the method further includes:

[0021] The driving environment laser point cloud data is calibrated based on the zero bias of the original vehicle pose information, and the edge features and surface features of the calibrated driving environment laser point cloud data are extracted.

[0022] The edge features and surface features are matched with the feature images in a pre-configured sliding window to obtain the initial estimated state;

[0023] Based on the timestamps of the image frames in the driving environment image data, the zero bias and initial estimated state of the vehicle's original pose information are interpolated and associated with the image frames in each driving environment image data, and an association value is determined. The association value is used to initialize the visual inertial system for pose estimation.

[0024] In one embodiment of the present invention, pose estimation is performed based on the feature points with depth information and the original vehicle pose information to obtain a pose estimation result, including:

[0025] The initial estimated state is transferred to the camera coordinate system to obtain the estimated state in the camera coordinate system;

[0026] The visual-inertial system is initialized based on the estimated state in the camera coordinate system and the associated value;

[0027] The original vehicle pose information is input into the initialized visual inertial system, and the original vehicle pose information is calculated by the initialized visual inertial system to obtain the high-frequency pose.

[0028] Based on the high-frequency pose, the estimated state in the camera coordinate system, and the feature points of the depth information, pose estimation is performed to obtain the pose estimation result.

[0029] In one embodiment of the present invention, after performing pose estimation based on the feature points with depth information and the original vehicle pose information to obtain the pose estimation result, the method further includes:

[0030] Extract key image frames from the pose estimation results and transmit the key image frames to a pre-built image frame dictionary and / or the driving environment image data;

[0031] In the image frame dictionary and / or the driving environment image data, the key image frames are filtered to determine the candidate image frames that match the key image frames.

[0032] In one embodiment of the present invention, after determining the candidate image frame that matches the key image frame, the method further includes:

[0033] The edge and surface features of the calibrated driving environment laser point cloud data are matched with the point and surface features of the pre-constructed local map system to obtain the map system pose information corresponding to the current frame.

[0034] The frame pose or estimated state in the camera coordinate system in the original pose information of the vehicle is used as the initial value of the map system pose information, and the map system pose information with the initial value is matched with the point cloud by the iterative nearest point algorithm to obtain the map system pose information after point cloud matching.

[0035] The map pose information after point cloud matching is optimized by a pre-constructed loop closure detection factor to obtain optimized map pose information. The optimized map pose information is used to update the original vehicle pose information to obtain new vehicle pose information.

[0036] In one embodiment of the present invention, an autonomous vehicle pose update device is also provided, the device comprising:

[0037] The data acquisition module is used to acquire image data of the driving environment of autonomous vehicles, laser point cloud data of the driving environment, and the original pose information of the vehicle.

[0038] The feature point extraction and depth assignment module is used to extract the first feature point in the driving environment image data and the second feature point in the driving environment laser point cloud data, and to assign the depth value of the first feature point based on the second feature point to obtain the feature point with depth information.

[0039] The pose estimation module is used to perform pose estimation based on the feature points with depth information and the original pose information of the vehicle to obtain the pose estimation result.

[0040] The pose information update module is used to update the original pose information of the vehicle based on the pose estimation result to obtain new vehicle pose information.

[0041] In one embodiment of the present invention, an electronic device is also provided, the electronic device comprising:

[0042] One or more processors;

[0043] A storage device for storing one or more programs that, when executed by one or more processors, cause the electronic device to implement the autonomous vehicle pose update method as described above.

[0044] In one embodiment of the present invention, a computer-readable storage medium is also provided, on which a computer program is stored, which, when executed by a computer processor, causes the computer to perform the autonomous vehicle pose update method as described above.

[0045] The beneficial effects of this invention are:

[0046] First, image data of the autonomous vehicle's driving environment, laser point cloud data of the driving environment, and the vehicle's original pose information are acquired. Then, a first feature point is extracted from the driving environment image data and a second feature point is extracted from the driving environment laser point cloud data. The first feature point is then assigned a depth value based on the second feature point to obtain a feature point with depth information. Next, pose estimation is performed based on the feature point with depth information and the vehicle's original pose information to obtain a pose estimation result. Finally, the vehicle's original pose information is updated based on the pose estimation result to obtain new vehicle pose information. In this invention, image data from the vision system and point cloud data from the LiDAR system are combined, and the vehicle's original pose information is used in conjunction with the combined data for pose estimation, further improving the accuracy of autonomous vehicle positioning, the stability of the positioning system, and its robustness.

[0047] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0048] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application. It is obvious that the drawings described below are merely some embodiments of this application, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort. In the drawings:

[0049] Figure 1 This is a schematic diagram illustrating the implementation environment of an autonomous vehicle pose update method, as shown in an exemplary embodiment of this application.

[0050] Figure 2 This is a flowchart illustrating an exemplary embodiment of the present application of an autonomous vehicle pose update method;

[0051] Figure 3 This is a schematic diagram illustrating feature depth association, as shown in an exemplary embodiment of this application;

[0052] Figure 4 This is a schematic diagram illustrating the workflow of a lidar visual inertial odometry system, as shown in an exemplary embodiment of this application.

[0053] Figure 5 This is a block diagram illustrating an autonomous vehicle pose update device according to an exemplary embodiment of this application;

[0054] Figure 6 A schematic diagram of the structure of a computer system suitable for implementing the electronic device of the present application is shown. Detailed Implementation

[0055] The embodiments of the present invention will be described below with reference to the accompanying drawings and preferred embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be understood that the preferred embodiments are only for illustrating the present invention and not for limiting the scope of protection of the present invention.

[0056] It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Therefore, the drawings only show the components related to the present invention and are not drawn according to the actual number, shape and size of the components in the actual implementation. In the actual implementation, the form, quantity and proportion of each component can be arbitrarily changed, and the layout of the components may also be more complex.

[0057] In the following description, numerous details are explored to provide a more thorough explanation of embodiments of the invention. However, it will be apparent to those skilled in the art that embodiments of the invention may be practiced without these specific details. In other embodiments, well-known structures and devices are shown in block diagram form rather than in detail to avoid obscuring embodiments of the invention.

[0058] First, it's important to note that because LiDAR-based localization methods often fail in environments with limited structure, and vision-based methods are sensitive to the richness of environmental texture, both LiDAR-based and vision-based methods are typically combined with an IMU (Inertial Measurement Unit) to improve their robustness and accuracy. LiDAR-based inertial odometry systems help correct point cloud distortions and maintain a certain level of accuracy even when features are scarce for short periods. Scene scale and pose can be recovered through IMU measurements, thus assisting vision-inertial systems.

[0059] Visual-inertial odometry (VIO) can be divided into two main categories: filter-based methods and optimization-based methods. Filter-based methods typically use an Extended Kalman Filter (EKF) to propagate the system state from camera and IMU observations. Optimization-based methods use a sliding window estimator and minimize visual reprojection errors and IMU measurement errors. Chinese patent CN112683305A discloses a visual-inertial odometry state estimation method based on point and line features. It tracks and reconstructs 3D features by acquiring monocular camera image frames and IMU data, and uses a nonlinear optimization method to construct a loss function to reduce pose and observation errors. This invention achieves odometry functionality while reducing processing time and improving system accuracy.

[0060] Lidar Inertial Odometry (LIO) can be divided into two main categories: loosely coupled methods and tightly coupled methods. LOAM and LeGO-LOAM are loosely coupled systems that do not use IMU measurements during optimization. Tightly coupled systems typically offer higher accuracy and stability and are currently the main focus of research. Chinese patent CN113740875A discloses a localization method for autonomous vehicles based on Lidar odometry and point cloud descriptor matching. It designs a fusion method based on Lidar and point cloud descriptor matching localization, fusing odometry localization results with global matching localization results to improve the adaptability of matching localization to the environment. Simultaneously, it introduces a sliding window of Lidar keyframes to limit computational complexity and utilizes factor graphs for joint IMU and Lidar constraint optimization. This invention solves the problems of low pose discrimination of matching information and reduced matching localization accuracy of autonomous vehicles when the environment changes, significantly improving the localization accuracy of odometry systems.

[0061] In this embodiment, the advantages of VIO and LIO are combined to construct a tightly coupled system framework for a lidar visual inertial odometry system, so that VIO and LIO in the system framework complement each other for real-time pose estimation and map building.

[0062] The following provides a detailed description of the autonomous vehicle pose update method, apparatus, device, and medium in this application.

[0063] Please refer to the following first. Figure 1 , Figure 1This is a schematic diagram illustrating the implementation environment of an autonomous vehicle pose update method according to an exemplary embodiment of this application. The implementation environment includes a sensing device 101, a communication medium 102, and an in-vehicle terminal device 103. The sensing device 101 includes a lidar and a camera, used to acquire driving environment image data and driving environment lidar point cloud data of the autonomous vehicle during driving; the communication medium 102 includes vehicle transmission media such as a CAN network, used to transmit the driving environment image data and driving environment lidar point cloud data acquired by the sensing device 101 to the in-vehicle terminal device 103; the in-vehicle diagnostic device 103 is used to execute the autonomous vehicle pose update method in this embodiment of the application.

[0064] For example, the on-board diagnostic device 103 acquires driving environment image data, driving environment laser point cloud data, and vehicle original pose information of an autonomous vehicle; then it extracts a first feature point from the driving environment image data and a second feature point from the driving environment laser point cloud data, assigns a depth value to the first feature point based on the second feature point to obtain a feature point with depth information; then it performs pose estimation based on the feature point with depth information and the vehicle original pose information to obtain a pose estimation result; finally, it updates the vehicle original pose information based on the pose estimation result to obtain new vehicle pose information.

[0065] It should be noted that the autonomous vehicle pose update method in this embodiment is implemented through a tightly coupled LiDAR visual inertial odometry system framework built in the on-board diagnostic device 103. The LiDAR visual inertial odometry system framework includes two subsystems, namely VIS (Visual Identity System) and LIS (LiDAR Inertial System). The two subsystems can work independently, and at the same time, they can use each other's information to improve the positioning accuracy and stability of the LiDAR visual inertial odometry system framework, avoiding the situation where the system cannot be used due to the failure of a single subsystem, and improving the robustness of the entire system.

[0066] The input sources for VIS are driving environment image data and IMU measurements. It employs visual feature tracking for visual odometry, and can also optionally use feature depth information extracted from LiDAR frames as the input source. By optimizing the visual reprojection error and IMU measurement residuals, the visual odometry output can be used as the initial value for LiDAR scan matching, and constraints are introduced into the factor graph.

[0067] LIS uses IMU measurements to calibrate the laser point cloud, then extracts the edge and surface features of the laser point cloud, and matches these features with the feature image maintained in the sliding window. The final estimated state can be sent to VIS to facilitate system initialization.

[0068] Figure 2 This is a flowchart illustrating an exemplary embodiment of an autonomous vehicle pose update method according to this application. The method provided in this application embodiment can be executed by any electronic device with computing power; for example, the method can be executed by the aforementioned... Figure 1 The on-board diagnostic device 103 in the embodiment performs the execution. In the following embodiment, the on-board diagnostic device 103 is used as an example for illustration, but this disclosure is not limited thereto.

[0069] Reference Figure 2 The autonomous vehicle pose update method provided in this application includes at least the following steps S210 to S240.

[0070] In step S210, the driving environment image data, driving environment laser point cloud data, and vehicle original pose information of the autonomous vehicle are acquired.

[0071] For example, during the driving of an autonomous vehicle, the camera in the sensing device 101 acquires image data of the driving environment, the lidar in the sensing device 101 acquires laser point cloud data of the driving environment, and the IMU measurement values ​​acquire the original pose information of the vehicle.

[0072] In step S220, a first feature point is extracted from the driving environment image data and a second feature point is extracted from the driving environment laser point cloud data. The first feature point is then assigned a depth value based on the second feature point to obtain a feature point with depth information.

[0073] For example, the first feature point of vision and the second feature point of laser are associated. After the VIS and LIS are initialized, the estimated visual odometry is used to register the LiDAR frame to the camera frame. Data from multiple LiDAR frames are stacked to obtain a dense depth map.

[0074] In step S230, pose estimation is performed based on feature points with depth information and the vehicle's original pose information to obtain the pose estimation result.

[0075] For example, the LIS used in this embodiment of the invention maintains a global pose optimization factor graph, which incorporates four constraints: pre-integration constraint, visual odometry constraint, lidar odometry constraint, and closed-loop constraint. These four constraints are used to optimize the pose estimation of feature points with depth information and the vehicle's original pose information to obtain the pose estimation result.

[0076] In step S240, the original vehicle pose information is updated based on the pose estimation result to obtain new vehicle pose information.

[0077] For example, after optimizing the pose estimation of feature points with depth information and the original vehicle pose information, the lidar visual inertial odometry system framework outputs the pose estimation result at the frequency of the IMU and updates the original vehicle pose information with the pose estimation result.

[0078] As can be seen from steps S210 to S240 above, the solution proposed in this embodiment combines image data from the vision system and point cloud data from the lidar system, and uses the vehicle's original pose information and the combined data to perform pose estimation, which further improves the accuracy of autonomous vehicle positioning, the stability of the positioning system, and the robustness.

[0079] In one embodiment of this application, Figure 2 Step S220, which assigns depth values ​​to the first feature point based on the second feature point to obtain a feature point with depth information, includes the following steps:

[0080] Project the first and second feature points onto a unit sphere centered at the camera;

[0081] The positions of the first and second feature points are represented in polar coordinates.

[0082] The depth information is obtained by searching the three nearest second feature points of each first feature point using a search tree, and the length of the line between the intersection of the line from the camera optical center to each first feature point and the plane determined by the three nearest second feature points is used as the depth information.

[0083] Each first feature point is assigned a value based on the depth information to obtain the feature point with depth information.

[0084] For example, the VIS used in this embodiment of the invention employs a corner detector to extract visual feature points and uses the KLT optical flow method for feature point tracking. See [link to documentation]. Figure 3 , Figure 3 This is a schematic diagram illustrating feature depth association in an exemplary embodiment of this application. To associate a first feature point with a depth value, the visual feature point and the downsampled laser point cloud depth point are first projected onto a sphere centered at the camera (O). COn a unit sphere, the positions of the two types of feature points are represented in polar coordinates. Then, a search tree is used to search for the three nearest laser point cloud depth points near each visual feature point. Finally, the length of the line connecting the straight line r from the camera optical center to the feature point (on the normalized plane) and the plane determined by the three point cloud points is taken as the depth value of the visual feature point. Through the method in this embodiment, visual feature points and laser point cloud depth points are associated, resulting in a more accurate depth estimation of the feature points.

[0085] In one embodiment of this application, Figure 2 After obtaining the driving environment image data, driving environment laser point cloud data, and vehicle original pose information in step S210, the following steps are also included:

[0086] The reprojection error of the driving environment image data and the measurement error of the vehicle's original pose information are obtained.

[0087] The reprojection error and measurement error are nonlinearly optimized by a pre-configured sliding window estimator to reduce the reprojection error and measurement error.

[0088] Based on the reduced reprojection error and measurement error, visual feature tracking is performed on the driving environment image data to obtain visual feature tracking feature points, and distorted point cloud data in the driving environment laser point cloud data is removed based on the visual feature tracking feature points.

[0089] For example, the optimization-based method uses a sliding window estimator and minimizes visual reprojection error and IMU measurement error. Point cloud distortion correction and sorting are achieved through point cloud distortion correction, enabling point cloud ordering and IMU-based LiDAR distortion correction. Since motion distortion occurs during LiDAR scanning due to carrier movement, it needs to be eliminated based on the motion. This can be achieved by using odometry motion output from the IMU or VIS to eliminate distortion and perform sorting.

[0090] It should be noted that in this embodiment, corner points and face points are extracted from the distortion-free point cloud through laser point cloud feature extraction. First, the smoothness of the point cloud is calculated, unstable points are screened and marked, and feature extraction is performed on the remaining point cloud.

[0091] In one embodiment of this application, in Figure 2 Before step S230, which involves performing pose estimation based on the feature points with depth information and the vehicle's original pose information, to obtain the pose estimation result, the following steps are also included:

[0092] The driving environment laser point cloud data is calibrated based on the zero bias of the original vehicle pose information, and the edge features and surface features of the calibrated driving environment laser point cloud data are extracted.

[0093] The edge features and surface features are matched with the feature images in a pre-configured sliding window to obtain the initial estimated state;

[0094] Based on the timestamps of the image frames in the driving environment image data, the zero bias and initial estimated state of the vehicle's original pose information are interpolated and associated with the image frames in each driving environment image data, and an association value is determined. The association value is used to initialize the visual inertial system for pose estimation.

[0095] For example, the initialization steps are divided into pure visual monocular initialization, visual-inertial joint initialization, and laser mileage index (LIS) initialization. Since VIS initialization is prone to failure, redundant LIS is considered to assist visual initialization. First, the LIS is initialized to obtain the system state and the zero bias of the IMU. Then, the above data is interpolated and associated with each image keyframe based on the timestamp of the image frame. Finally, the associated values ​​are used to initialize the VIS, thereby improving the speed and robustness of VIS initialization.

[0096] In one embodiment of this application, pose estimation is performed based on the feature points with depth information and the original vehicle pose information to obtain a pose estimation result, including:

[0097] The initial estimated state is transferred to the camera coordinate system to obtain the estimated state in the camera coordinate system;

[0098] The visual-inertial system is initialized based on the estimated state in the camera coordinate system and the associated value;

[0099] The original vehicle pose information is input into the initialized visual inertial system, and the original vehicle pose information is calculated by the initialized visual inertial system to obtain the high-frequency pose.

[0100] Based on the high-frequency pose, the estimated state in the camera coordinate system, and the feature points of the depth information, pose estimation is performed to obtain the pose estimation result.

[0101] For example, the information from the lidar odometry is obtained and transferred to the camera coordinate system for VIS initialization; the raw information from the IMU is obtained, and the result of the pose calculation is used for the high-frequency pose before the next frame of the image arrives; finally, by processing the IMU, lidar odometry, and image feature point information, the VIO fusion is completed to estimate the pose.

[0102] In one embodiment of this application, after performing pose estimation based on the feature points with depth information and the original vehicle pose information to obtain the pose estimation result, the method further includes:

[0103] Extract key image frames from the pose estimation results and transmit the key image frames to a pre-built image frame dictionary and / or the driving environment image data;

[0104] In the image frame dictionary and / or the driving environment image data, the key image frames are filtered to determine the candidate image frames that match the key image frames.

[0105] For example, keyframes are created for loop closure detection. The keyframes are added to a dictionary or historical matching frames are searched to obtain the image information of the current frame image and the historical closed loop frames that have been successfully matched.

[0106] In one embodiment of this application, after determining the candidate image frame that matches the key image frame, the method further includes:

[0107] The edge and surface features of the calibrated driving environment laser point cloud data are matched with the point and surface features of the pre-constructed local map system to obtain the map system pose information corresponding to the current frame.

[0108] The frame pose or estimated state in the camera coordinate system in the original pose information of the vehicle is used as the initial value of the map system pose information, and the map system pose information with the initial value is matched with the point cloud by the iterative nearest point algorithm to obtain the map system pose information after point cloud matching.

[0109] The map pose information after point cloud matching is optimized by a pre-constructed loop closure detection factor to obtain optimized map pose information. The optimized map pose information is used to update the original vehicle pose information to obtain new vehicle pose information.

[0110] For example, the loop closure detection process first involves VIS identifying and filtering candidate frames, followed by LIS for further optimization. In the factor graph, the constraints of visual odometry, laser odometry, IMU pre-integration, and loop closure are jointly optimized, as are the IMU, laser odometry, visual odometry, and loop closure constraint information. First, the point and surface features extracted from the laser point cloud are matched with the local map point and surface features of the map system to obtain the pose of the current frame relative to the map system. The pose transformations of the two frames calculated by the IMU or VIS are used as initial values ​​for the ICP matching algorithm; then, loop closure detection factor optimization is added to fuse and calculate the pose of the current frame, and the optimized result is used to update the keyframe pose.

[0111] In one embodiment of this application, a failure detection mode is also added, namely: different failure detection logics are used for VIS and LIS respectively. When one subsystem triggers failure detection, the other subsystem will be switched immediately for seamless connection. This can avoid the failure of a single sensor causing the entire system to crash and greatly improve the robustness of the system.

[0112] In one embodiment of this application, see Figure 4 , Figure 4 This is a schematic diagram illustrating the workflow of a lidar visual inertial odometry system, as shown in an exemplary embodiment of this application. The workflow of the lidar visual inertial odometry system includes the following steps:

[0113] Step 1, Feature Tracking, mainly involves extracting, tracking, and assigning depth values ​​to feature points in the driving environment image data acquired by the camera. This includes acquiring laser point cloud data of the driving environment to prepare for depth registration of feature points in the driving environment image data. Feature points are extracted and tracked from the driving environment image data, and the depth information of the feature points is obtained by associating and registering the driving environment laser point cloud data.

[0114] Step 2, Visual estimation: By acquiring the information from the LiDAR odometry, the coordinates are transferred to the camera coordinate system for VIS initialization; the raw IMU information is acquired, and the pose calculation results are used for high-frequency pose estimation before the next frame arrives; finally, by processing the IMU, LiDAR odometry, and image feature point information, VIO fusion is completed to estimate the pose.

[0115] Step 3: Loop closure detection. Create keyframes to perform loop closure detection. Add the keyframes to the dictionary or search for historical matching frames to obtain the image information of the current frame image and the historical closed loop frames that have been successfully filtered and matched.

[0116] Step 4, Point Cloud Distortion Reduction and Sorting, mainly involves ordering the LiDAR point cloud data acquired by the LiDAR (Light Detection and Ranging) system for the driving environment and performing IMU-based LiDAR distortion reduction. Since motion distortion occurs during LiDAR scanning due to the movement of the vehicle, it needs to be eliminated based on the motion. This can be achieved by using the odometer motion output from the IMU or VIS to eliminate distortion and sort the data.

[0117] Step 5: Feature extraction from laser point cloud data of the driving environment, mainly by extracting corner points and face points from the distortion-free point cloud data. First, the smoothness of the point cloud is calculated, unstable points are screened and marked, and features are extracted from the remaining point cloud.

[0118] Step 6: Map Optimization. Map optimization is performed by jointly optimizing IMU, laser odometry, visual odometry, and loop closure constraint information. First, point and area features extracted from the laser point cloud data of the driving environment are matched with local map point and area features in the map system to obtain the pose of the current frame relative to the map system. Then, the pose transformation between the two frames calculated by IMU or VIS is used as initial values ​​for ICP matching algorithm. Finally, a loop closure detection factor is added to optimize and fuse the calculation of the current frame pose, and the optimization result is used to update the keyframe pose.

[0119] The pose information obtained after optimization in steps 7 and 6 is used to jointly optimize and solve the pose information with the IMU pre-integration pose information, and the original vehicle pose information is updated according to the optimization results to obtain new vehicle pose information.

[0120] Figure 5 This is a block diagram illustrating an autonomous vehicle pose update device according to an exemplary embodiment of this application. The device can be applied to... Figure 1 The implementation environment shown is not limited to this embodiment. This device can also be applied to other exemplary implementation environments and specifically configured in other devices. This embodiment does not limit the implementation environment to which the device is applicable.

[0121] like Figure 5 As shown, this exemplary autonomous vehicle pose update device includes:

[0122] The data acquisition module 501 is used to acquire driving environment image data, driving environment laser point cloud data and vehicle original pose information of autonomous vehicles;

[0123] The feature point extraction and depth assignment module 502 is used to extract a first feature point from the driving environment image data and a second feature point from the driving environment laser point cloud data, and to assign a depth value to the first feature point based on the second feature point to obtain a feature point with depth information.

[0124] The pose estimation module 503 is used to perform pose estimation based on the feature points with depth information and the original pose information of the vehicle to obtain the pose estimation result.

[0125] The pose information update module 504 is used to update the original pose information of the vehicle according to the pose estimation result to obtain new vehicle pose information.

[0126] In this exemplary autonomous vehicle pose update device, image data from the vision system and point cloud data from the lidar system are combined, and pose estimation is performed using the vehicle's original pose information and the combined data, which further improves the accuracy of autonomous vehicle positioning, the stability of the positioning system, and the robustness.

[0127] It should be noted that the autonomous vehicle pose update device and the autonomous vehicle pose update method provided in the above embodiments belong to the same concept. The specific methods by which each module and unit performs operations have been described in detail in the method embodiments and will not be repeated here. In practical applications, the autonomous vehicle pose update device provided in the above embodiments can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. This is not a limitation here.

[0128] Embodiments of this application also provide an electronic device, including: one or more processors; and a storage device for storing one or more programs, which, when executed by the one or more processors, cause the electronic device to implement the autonomous vehicle pose update method provided in the above embodiments.

[0129] Figure 6 A schematic diagram of a computer system suitable for implementing the embodiments of this application is shown. It should be noted that... Figure 6 The computer system 600 of the electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0130] like Figure 6 As shown, the computer system 600 includes a Central Processing Unit (CPU) 601, which can perform various appropriate actions and processes based on programs stored in Read-Only Memory (ROM) 602 or programs loaded from Storage Unit 608 into Random Access Memory (RAM) 603, such as performing the methods described in the above embodiments. The RAM 603 also stores various programs and data required for system operation. The CPU 601, ROM 602, and RAM 603 are interconnected via a bus 604. An Input / Output (I / O) interface 605 is also connected to the bus 604.

[0131] The following components are connected to I / O interface 605: an input section 606 including a keyboard, mouse, etc.; an output section 607 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 608 including a hard disk, etc.; and a communication section 609 including a network interface card such as a LAN (Local Area Network) card, modem, etc. The communication section 609 performs communication processing via a network such as the Internet. A drive 610 is also connected to I / O interface 605 as needed. A removable medium 611, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on drive 610 as needed so that computer programs read from it can be installed into storage section 608 as needed.

[0132] Specifically, according to embodiments of this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program including a computer program for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 609, and / or installed from removable medium 611. When the computer program is executed by central processing unit (CPU) 601, it performs various functions defined in the system of this application.

[0133] It should be noted that the computer-readable medium shown in the embodiments of this application can be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium can be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying a computer-readable computer program. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. Computer-readable signal media can also be any computer-readable medium other than computer-readable storage media, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The computer program contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to wireless, wired, etc., or any suitable combination thereof.

[0134] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0135] The units described in the embodiments of this application can be implemented in software or hardware, and the described units can also be located in a processor. The names of these units do not necessarily limit the specific unit itself.

[0136] Another aspect of this application provides a computer-readable storage medium storing a computer program that, when executed by a computer's processor, causes the computer to perform the autonomous vehicle pose update method as described above. This computer-readable storage medium may be included in the electronic device described in the above embodiments, or it may exist independently and not incorporated into the electronic device.

[0137] Another aspect of this application provides a computer program product or computer program including computer instructions stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the autonomous vehicle pose update method provided in the various embodiments described above.

[0138] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the invention. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in the present invention should still be covered by the claims of the present invention.

Claims

1. A pose update method for an autonomous vehicle, characterized in that, The method includes: Acquire image data of the driving environment of autonomous vehicles, laser point cloud data of the driving environment, and original vehicle pose information; Extract the first feature point from the driving environment image data and the second feature point from the driving environment laser point cloud data, and assign a depth value to the first feature point based on the second feature point to obtain a feature point with depth information; Based on the feature points with depth information and the original vehicle pose information, pose estimation is performed to obtain the pose estimation result. The original vehicle pose information is updated based on the pose estimation result to obtain new vehicle pose information; The step of assigning depth values ​​to the first feature points based on the second feature points to obtain feature points with depth information includes: projecting the first and second feature points onto a unit sphere centered at the camera; representing the positions of the first and second feature points in polar coordinates; searching for three neighboring second feature points near each first feature point using a search tree; using the length of the line connecting the camera's optical center to each first feature point and the plane determined by the three neighboring second feature points as the depth information; and assigning values ​​to each first feature point based on the depth information to obtain the feature points with depth information.

2. The autonomous vehicle pose update method according to claim 1, characterized in that, After acquiring the driving environment image data, driving environment laser point cloud data, and original vehicle pose information of the autonomous vehicle, the process further includes: The reprojection error of the driving environment image data and the measurement error of the vehicle's original pose information are obtained. The reprojection error and measurement error are nonlinearly optimized by a pre-configured sliding window estimator to reduce the reprojection error and measurement error. Based on the reduced reprojection error and measurement error, visual feature tracking is performed on the driving environment image data to obtain the visual feature tracking feature points, and the distorted point cloud data in the driving environment laser point cloud data is removed based on the visual feature tracking feature points.

3. The autonomous vehicle pose update method according to claim 1, characterized in that, Before obtaining the pose estimation result by performing pose estimation based on the feature points with depth information and the original vehicle pose information, the method further includes: The driving environment laser point cloud data is calibrated based on the zero bias of the original vehicle pose information, and the edge features and surface features of the calibrated driving environment laser point cloud data are extracted. The edge features and surface features are matched with the feature images in a pre-configured sliding window to obtain the initial estimated state; Based on the timestamps of the image frames in the driving environment image data, the zero bias and initial estimated state of the vehicle's original pose information are interpolated and associated with the image frames in each driving environment image data, and an association value is determined. The association value is used to initialize the visual inertial system for pose estimation.

4. The autonomous vehicle pose update method according to claim 3, characterized in that, Based on the feature points with depth information and the original vehicle pose information, pose estimation is performed to obtain the pose estimation result, including: The initial estimated state is transferred to the camera coordinate system to obtain the estimated state in the camera coordinate system; The visual-inertial system is initialized based on the estimated state in the camera coordinate system and the associated value; The original vehicle pose information is input into the initialized visual inertial system, and the original vehicle pose information is calculated by the initialized visual inertial system to obtain the high-frequency pose. Based on the high-frequency pose, the estimated state in the camera coordinate system, and the feature points of the depth information, pose estimation is performed to obtain the pose estimation result.

5. The autonomous vehicle pose update method according to claim 4, characterized in that, Based on the feature points with depth information and the original vehicle pose information, pose estimation is performed. After obtaining the pose estimation result, the method further includes: Extract key image frames from the pose estimation results and transmit the key image frames to a pre-built image frame dictionary and / or the driving environment image data; In the image frame dictionary and / or the driving environment image data, the key image frames are filtered to determine the candidate image frames that match the key image frames.

6. The autonomous vehicle pose update method according to claim 5, characterized in that, After determining the candidate image frame that matches the key image frame, the method further includes: The edge and surface features of the calibrated driving environment laser point cloud data are matched with the point and surface features of the pre-constructed local map system to obtain the map system pose information corresponding to the current frame. The frame pose or estimated state in the camera coordinate system in the original vehicle pose information is used as the initial value of the map system pose information, and the map system pose information with the initial value is matched with the point cloud by the iterative nearest point algorithm to obtain the map system pose information after point cloud matching. The map pose information after point cloud matching is optimized by a pre-constructed loop closure detection factor to obtain optimized map pose information. The optimized map pose information is used to update the original vehicle pose information to obtain new vehicle pose information.

7. An autonomous vehicle pose update device, characterized in that, The device includes: The data acquisition module is used to acquire image data of the driving environment of autonomous vehicles, laser point cloud data of the driving environment, and the original pose information of the vehicle. The feature point extraction and depth assignment module is used to extract the first feature point in the driving environment image data and the second feature point in the driving environment laser point cloud data, and to assign the depth value of the first feature point based on the second feature point to obtain the feature point with depth information. The pose estimation module is used to perform pose estimation based on the feature points with depth information and the original pose information of the vehicle to obtain the pose estimation result. The pose information update module is used to update the original vehicle pose information according to the pose estimation result to obtain new vehicle pose information. The feature point extraction and depth assignment module is specifically used to project the first feature point and the second feature point onto a unit sphere centered at the camera; represent the positions of the first feature point and the second feature point in polar coordinates; search for three neighboring second feature points near each first feature point using a search tree; and use the length of the line connecting the line from the camera's optical center to each first feature point to the plane determined by the three neighboring second feature points as the depth information; and assign a value to each first feature point based on the depth information to obtain the feature point with depth information.

8. An electronic device, characterized in that, The electronic device includes: One or more processors; A storage device for storing one or more programs that, when executed by one or more processors, cause the electronic device to implement the autonomous vehicle pose update method as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, It stores a computer program that, when executed by the computer's processor, causes the computer to perform the autonomous vehicle pose update method according to any one of claims 1 to 6.

Images