Pose estimation method and device of equipment, electronic equipment and storage medium

Abstract

The application provides a device pose estimation method and device, electronic equipment and storage medium, which are related to the technical field of automatic navigation, and a camera and a laser radar are arranged on the device; the method comprises the following steps: performing visual pose estimation on the device based on image data collected by the camera to obtain a first pose estimation result; performing laser radar pose estimation on the device based on laser point cloud data collected by the laser radar to obtain a second pose estimation result; and obtaining a target pose estimation result of the device based on the first pose estimation result and the second pose estimation result. In the embodiment of the application, the target pose estimation result of the device can be obtained based on the pose estimation results obtained by the camera and the laser radar respectively, and the pose estimation results of the two are combined. Since the camera has the characteristics of low cost, light weight and rich texture information in images, compared with a single laser SLAM positioning method, the positioning accuracy can be effectively improved.

Description

Technical Field

[0001] This invention relates to the field of automatic navigation technology, and in particular to a method, apparatus, electronic device, and storage medium for estimating the pose of a device. Background Technology

[0002] With the continuous development of autonomous driving technology, driverless cars (i.e., unmanned vehicles) are showing great application prospects in intelligent transportation, logistics, fire fighting, and military fields. Unmanned vehicles are complex mechanical products that integrate multiple technologies. Among them, the application of positioning technology is a prerequisite for unmanned vehicles to complete complex tasks such as path planning and autonomous navigation algorithms, and it is a research hotspot in the field of artificial intelligence.

[0003] Currently, autonomous vehicles typically use the Simultaneous Localization and Mapping (SLAM) method based on lidar for localization.

[0004] However, the effective measurement distance of the laser SLAM positioning method based on lidar is relatively small, making it unsuitable for open environments, resulting in low positioning accuracy. Summary of the Invention

[0005] This invention provides a method, apparatus, electronic device, and storage medium for estimating the pose of a device, in order to solve the problem of low positioning accuracy of devices in the prior art.

[0006] This invention provides a pose estimation method for a device, wherein the device is equipped with a camera and a lidar; the method includes:

[0007] Visual pose estimation of the device is performed based on the image data acquired by the camera to obtain the first pose estimation result;

[0008] Based on the laser point cloud data collected by the lidar, the device is subjected to lidar pose estimation to obtain a second pose estimation result;

[0009] Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device is obtained.

[0010] According to a device pose estimation method provided by the present invention, obtaining a target pose estimation result of the device based on a first pose estimation result and a second pose estimation result includes:

[0011] The first pose estimation result is input into the state equation in the pre-set extended Kalman filter equation to obtain the pose prediction value of the device; wherein, the system transfer matrix F of the state equation includes a second-order Taylor series;

[0012] The pose prediction value is input into the observation equation in the extended Kalman filter equation to obtain the predicted observation value of the device; wherein, the observation matrix H of the observation equation includes a second-order Taylor series;

[0013] The second pose estimation result is used as the actual observation value of the device;

[0014] Based on the predicted observations and the actual observations, the target pose estimation result of the device is obtained.

[0015] According to a device pose estimation method provided by the present invention, the step of performing visual pose estimation on the device based on image data acquired by the camera to obtain a first pose estimation result includes:

[0016] Based on a pre-set bucketing method, the image data is divided into N rectangles as N buckets, where N is an integer greater than 1;

[0017] Feature points are extracted from the N buckets respectively to obtain the ORB feature points in the image data;

[0018] Initialize a local map based on the image data;

[0019] Based on the ORB feature points and the feature points in the last frame of the local map, the device performs visual pose estimation to obtain the first pose estimation result.

[0020] According to a device pose estimation method provided by the present invention, the step of performing visual pose estimation on the device based on the ORB feature points and feature points in the last frame image of the local map to obtain a first pose estimation result includes:

[0021] Based on the survival age of each ORB feature point, the average inter-frame error of each ORB feature point is calculated.

[0022] ORB feature points with an average inter-frame error less than a first threshold are selected as target feature points;

[0023] Calculate the error between the target feature point and the feature points in the last frame of the local map, and construct an error function;

[0024] Visual pose estimation of the device is performed based on the error function to obtain the first pose estimation result.

[0025] According to a device pose estimation method provided by the present invention, the step of calculating the average inter-frame error of each ORB feature point based on the survival age of each ORB feature point includes:

[0026] Based on the survival age of each ORB feature point, the average inter-frame error (Error) of each ORB feature point is calculated using formula (1):

[0027]

[0028] Among them, P k P represents the k-th feature point. k-1 The (k-1)th feature point, Age sur Characteristic point P k-1 Survival age, M k-1|k The true value of the motion matrix, P k -M k-1|k ·P k-1 Characterizes the single-step inter-frame tracking error.

[0029] According to a device pose estimation method provided by the present invention, the step of performing lidar pose estimation on the device based on lidar point cloud data collected by the lidar to obtain a second pose estimation result includes:

[0030] Based on a pre-set wavelet transform method, the laser point cloud data is preprocessed to obtain denoised laser point cloud data.

[0031] Environmental feature points are extracted from the denoised laser point cloud data, and the laser radar pose estimation is performed on the device based on the environmental feature points to obtain the second pose estimation result.

[0032] According to the present invention, a device pose estimation method is provided, wherein the threshold function ω of the wavelet transform method is... m It is obtained using formula (2):

[0033]

[0034] Wherein, ω represents the distance value of the laser point cloud in the laser point cloud data, and λ represents the fixed threshold.

[0035] The present invention also provides a pose estimation device for a device, comprising: a camera and a lidar disposed on the device; the device comprising:

[0036] The first pose estimation module is used to perform visual pose estimation of the device based on the image data acquired by the camera, and obtain the first pose estimation result.

[0037] The second pose estimation module is used to perform lidar pose estimation on the device based on the lidar point cloud data collected by the lidar, and obtain the second pose estimation result.

[0038] The acquisition module is used to obtain the target pose estimation result of the device based on the first pose estimation result and the second pose estimation result.

[0039] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the pose estimation method of any of the devices described above.

[0040] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the pose estimation method of any of the above-described devices.

[0041] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the pose estimation method of any of the devices described above.

[0042] The present invention provides a device pose estimation method, apparatus, electronic device, and storage medium. Using image data and laser point cloud data acquired by a camera and LiDAR respectively, visual and LiDAR pose estimation is performed on the device to obtain a first pose estimation result and a second pose estimation result. Based on the first and second pose estimation results, a target pose estimation result for the device is obtained. Embodiments of the present invention can obtain the target pose estimation result for the device by combining the pose estimation results obtained from the camera and LiDAR respectively. Since cameras have the advantages of low cost, light weight, and images containing rich texture information, compared with single-laser SLAM positioning methods, the positioning accuracy can be effectively improved. Attached Figure Description

[0043] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0044] Figure 1 This is one of the flowcharts illustrating the pose estimation method for the device provided by the present invention;

[0045] Figure 2 This is the second flowchart illustrating the pose estimation method for the device provided by the present invention;

[0046] Figure 3 This is the third flowchart illustrating the pose estimation method for the device provided by the present invention;

[0047] Figure 4This is the fourth flowchart illustrating the pose estimation method for the device provided by the present invention;

[0048] Figure 5 This is a schematic diagram of the path trajectory curve drawn by the pose estimation method of the device provided by the present invention;

[0049] Figure 6 This is a schematic diagram of the Euclidean distance error curves at various positions obtained by the pose estimation method of the device provided by the present invention;

[0050] Figure 7 This is a schematic diagram of the pose estimation device of the equipment provided by the present invention;

[0051] Figure 8 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0052] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0053] The pose estimation method, apparatus, electronic device, and storage medium of the present invention are described below with reference to the accompanying drawings.

[0054] The embodiments of the present invention provide a solution to the problem of low positioning accuracy of devices. To facilitate a clearer understanding of the embodiments of the present invention, some related technical knowledge is first introduced as follows.

[0055] Autonomous vehicle positioning systems can be categorized into three levels based on different positioning requirements: road network level, a level between road network and lane level, and lane level. Reliable and accurate real-time positioning is fundamental to the safe driving and high operational efficiency of autonomous vehicles.

[0056] There are many types of autonomous vehicle positioning technologies, and more and more autonomous vehicles are adopting positioning methods such as camera-based visual SLAM positioning and lidar-based lidar SLAM positioning.

[0057] For lidar-based SLAM localization methods, lidar is mainly used to measure the environmental information around the autonomous vehicle. This information is represented as a series of dispersed point clouds with accurate angle and distance information. By matching and comparing the point clouds, the relative motion of the lidar can be calculated, thereby completing the localization of the autonomous vehicle. Lidar SLAM theoretical research is relatively mature, and there are many commercially available products. However, lidar is relatively expensive, has a limited effective range, and is not suitable for open environments.

[0058] Compared to LiDAR, cameras offer advantages such as lower cost, lighter weight, and richer texture information in their images. Therefore, camera-based visual SLAM localization methods can easily distinguish similar scenes that are easily confused by LiDAR, demonstrating significant advantages in autonomous vehicle relocalization, loop closure detection, and scene classification. While visual SLAM has a wider range of applications, it is highly dependent on lighting and scene conditions, failing to function in dark areas or textureless regions. It also suffers from other robustness issues, resulting in fewer commercially viable products. However, both LiDAR and visual methods have their limitations, such as noise and poor localization accuracy in complex environments.

[0059] Figure 1 This is one of the flowcharts illustrating the pose estimation method for the device provided by the present invention. The device is equipped with a camera and a lidar, such as... Figure 1 As shown, the device pose estimation method includes steps 101 to 103; wherein:

[0060] Step 101: Perform visual pose estimation on the device based on the image data acquired by the camera to obtain the first pose estimation result;

[0061] Step 102: Based on the laser point cloud data collected by the lidar, perform lidar pose estimation on the device to obtain a second pose estimation result;

[0062] Step 103: Based on the first pose estimation result and the second pose estimation result, obtain the target pose estimation result of the device.

[0063] Specifically, existing technologies rely solely on laser SLAM positioning methods based on lidar for device positioning. However, lidar has a limited effective measurement range and is not suitable for open environments, resulting in low device positioning accuracy.

[0064] In this embodiment of the invention, a camera-based visual SLAM positioning method is combined with a laser radar-based laser SLAM positioning method. Specifically, a camera and a laser radar can be set up on the device at the same time, so that the camera and the laser radar can respectively collect image data and laser point cloud data. Based on the collected image data and laser point cloud data, the device is visually and by laser radar to estimate the pose, so as to obtain a first pose estimation result and a second pose estimation result. Then, based on the first pose estimation result and the second pose estimation result, the target pose result of the device is obtained.

[0065] Optionally, the aforementioned equipment may include driverless cars, unmanned vehicles, and robots.

[0066] It should be noted that pose estimation refers to obtaining the position coordinates and attitude information of the device in the scene captured by the current acquisition device, which can be understood as locating the device.

[0067] Optionally, to unify the coordinate systems of the image data and laser point cloud data acquired by the camera and LiDAR, the laser point cloud data acquired by the LiDAR can be transformed into the camera coordinate system. This facilitates subsequent visual and LiDAR pose estimation of the device based on the same coordinate system, resulting in a first pose estimation result and a second pose estimation result. Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device in the unified coordinate system can be obtained.

[0068] In this embodiment of the invention, image data and laser point cloud data acquired by a camera and a LiDAR, respectively, are used to perform visual and LiDAR pose estimation on the device to obtain a first pose estimation result and a second pose estimation result. The target pose estimation result of the device is then obtained based on the first and second pose estimation results. This embodiment of the invention can obtain the target pose estimation result of the device based on the pose estimation results obtained by the camera and LiDAR respectively, and combine the two pose estimation results. Since cameras have the advantages of low cost, light weight, and images containing rich texture information, they can effectively improve positioning accuracy compared to single-laser SLAM positioning methods.

[0069] Optionally, the method for obtaining the target pose estimation result of the device based on the first pose estimation result and the second pose estimation result may include:

[0070] The first pose estimation result is input into the state equation in the pre-set extended Kalman filter equation to obtain the pose prediction value of the device; wherein, the system transfer matrix F of the state equation includes a second-order Taylor series;

[0071] The pose prediction value is input into the observation equation in the extended Kalman filter equation to obtain the predicted observation value of the device; wherein, the observation matrix H of the observation equation includes a second-order Taylor series;

[0072] The second pose estimation result is used as the actual observation value of the device;

[0073] Based on the predicted observations and the actual observations, the target pose estimation result of the device is obtained.

[0074] Specifically, we can first perform a second-order Taylor expansion on the state equation and observation equation in the extended Kalman filter equation. Specifically, based on formula (3), we can obtain the expanded state equation and observation equation. Specifically, formula (4) represents the expanded state equation, and formula (5) represents the expanded observation equation.

[0075]

[0076]

[0077]

[0078] Where F represents the system transfer matrix, X(k) represents the state vector, and C * Let Z(k) represent the second-order Taylor series, Z(k) represent the observation vector, H represent the observation matrix, ε(k) represent the system noise w(k), and C represent the system noise. * The sum, λ(k), characterizes the observation noise v(k) and C. * The sum of these two values, ε(k) and λ(k), characterize virtual noise. U(k) can be expressed as: y(k) can be expressed as:

[0079]

[0080] The first pose estimation result obtained by visually estimating the device based on the image data acquired by the camera is used as U(k) to predict the current pose of the device, thus obtaining the device's pose prediction value. See formula (6) for details:

[0081]

[0082] The above obtained device pose prediction values Substituting these values ​​into the observation equation, we can obtain the predicted observation values ​​of the equipment, as shown in formula (7):

[0083]

[0084] The second pose estimation result obtained by performing lidar pose estimation on the device based on the lidar point cloud data collected by lidar is used as the actual observation value of the device.

[0085] Specifically, the second pose estimation result is used as the actual observation value Z(k+1), and matched with the environmental features corresponding to the predicted observation value. Assuming there are m pairs of matched environmental features, the gain of each pair is calculated using formula (8):

[0086]

[0087] Where K represents the gain, P represents the covariance matrix, and S represents the variance.

[0088] Then, based on the actual observations and predicted observations of the equipment, the state is updated to obtain the laser-visual odometry. Specifically, the state update can be performed using formula (9):

[0089]

[0090] Then, the predicted covariance matrix is ​​updated using formula (10):

[0091]

[0092] The final target pose estimation result of the device can be obtained by using the extended Kalman filter method described above.

[0093] Alternatively, the above-mentioned extended Kalman filtering method can be implemented using an extended Kalman filter (EKF).

[0094] Optionally, the method for performing visual pose estimation on the device based on the image data acquired by the camera to obtain the first pose estimation result may include:

[0095] Based on a pre-set bucketing method, the image data is divided into N rectangles as N buckets, where N is an integer greater than 1;

[0096] Feature points are extracted from the N buckets respectively to obtain ORB (Oriented Fast and Rotated BRIEF) feature points in the image data;

[0097] Initialize a local map based on the image data;

[0098] Based on the ORB feature points and the feature points in the last frame of the local map, the device performs visual pose estimation to obtain the first pose estimation result.

[0099] Specifically, the ORB feature extraction method can be used to extract ORB feature points from each image to be localized and initialize the local map.

[0100] For example, suppose It is a point in the k-th frame under the image projection coordinate system {I}. If a point is located in the k-th frame of the world coordinate system {W}, then according to the properties of the homogeneous coordinate system: Where, π () T represents the transformation from homogeneous coordinates to non-homogeneous coordinates. k yes Homogeneous coordinate format of Δ Δ and Δ represent rotation and translation, respectively. The first local map point can be obtained according to the above formula.

[0101] The image data (the image to be located) can also be segmented into 50×50 rectangles as buckets using a bucketing method. All features used for ego-motion estimation can be selected from the buckets.

[0102] Optionally, the aforementioned camera can be a stereo camera. For example, the LeEco Astrapro 3-in-1 camera can be used as an environmental perception sensor. The camera collects a set of images of the surrounding environment and inputs them into the visual SLAM system as images. Before using the camera to collect image data, the camera needs to be calibrated. For example, a checkerboard-based method can be used to complete the camera calibration.

[0103] Optionally, the method for performing visual pose estimation on the device based on the ORB feature points and feature points in the last frame of the local map to obtain the first pose estimation result may include:

[0104] Based on the survival age of each ORB feature point, the average inter-frame error of each ORB feature point is calculated.

[0105] ORB feature points with an average inter-frame error less than a first threshold are selected as target feature points;

[0106] Calculate the error between the target feature point and the feature points in the last frame of the local map, and construct an error function;

[0107] Visual pose estimation of the device is performed based on the error function to obtain the first pose estimation result.

[0108] Specifically, after extracting ORB feature points from the image data, the average inter-frame error of each feature point can be calculated based on the age of the feature points, and features with smaller errors can be selected for visual pose estimation. Specifically, feature points with average inter-frame errors less than a first threshold can be selected as target feature points, and the first threshold can be set by technicians as needed.

[0109] It should be noted that the larger the first threshold is set, the fewer feature points will be filtered out, the more feature matching needs to be performed, the greater the processing burden on the processor, and the slower the processing speed, but the higher the matching accuracy. Conversely, the smaller the threshold is, the fewer feature points will be filtered out, the more feature matching needs to be performed, the greater the processing burden on the processor, and the slower the processing speed, but the higher the matching accuracy. Technicians can set the first threshold as needed.

[0110] Optionally, the method for calculating the average inter-frame error of each ORB feature point based on its survival age may include:

[0111] Based on the survival age of each ORB feature point, the average inter-frame error (Error) of each ORB feature point is calculated using formula (1):

[0112]

[0113] Among them, P k P represents the k-th feature point. k-1 The (k-1)th feature point, Age sur Characteristic point P k-1 Survival age, M k-1|k The true value of the motion matrix, P k -M k-1|k ·P k-1 Characterizes the single-step inter-frame tracking error.

[0114] Specifically, if P k-1 If a point is in the (k-1)th frame, then its corresponding measurement feature in the kth frame is P. k Feature point P k-1 The average inter-frame error Error can be calculated using the above formula (1).

[0115] It should be noted that after calculating the average inter-frame error, the feature points can be sorted according to the magnitude of the average inter-frame error. Feature points with smaller errors are considered to have higher motion estimation priority, so target feature points with higher motion estimation priority are used for visual pose estimation first.

[0116] Optionally, the feature points corresponding to the image data can be matched with the points updated in the last frame of the local map to construct an error function, which can then be solved to obtain the target pose estimation result.

[0117] For example, suppose and These are features with precise depth values ​​and features without depth values, respectively. Features The target pose estimation result T can be obtained by using formula (11) from the corresponding matching points in the local map. * :

[0118]

[0119] Using the Levenberg-Marquadt (LM) method, we can obtain T. * .

[0120] It should be noted that each time an estimation is performed, the local map can be continuously updated and optimized based on these matched feature points.

[0121] Optionally, the method for performing lidar pose estimation on the device based on the lidar point cloud data collected by the lidar to obtain a second pose estimation result may include:

[0122] Based on a pre-set wavelet transform method, the laser point cloud data is preprocessed to obtain denoised laser point cloud data.

[0123] Environmental feature points are extracted from the denoised laser point cloud data, and the laser radar pose estimation is performed on the device based on the environmental feature points to obtain the second pose estimation result.

[0124] Specifically, this embodiment of the invention employs an improved wavelet filter to preprocess the laser point cloud data acquired by the lidar. During lidar data acquisition, errors generated by the lidar system, noise from the material of the object being measured, and errors during the scanning process significantly impact subsequent feature extraction and lidar pose estimation. Therefore, a suitable algorithm is needed to denoise the point cloud, providing high-quality point cloud data for subsequent work.

[0125] The output data of the lidar includes information such as distance, angle, reflection intensity, and timestamp. This embodiment of the invention mainly studies the distance value.

[0126] First, a suitable wavelet basis function dbN and decomposition layer number N are selected to decompose the original signal (laser point cloud data) acquired by the lidar into N layers. Second, an improved threshold function is used to quantize the detail coefficients of each layer through different thresholds. Finally, the lidar signal is reconstructed through inverse wavelet transform to obtain the denoised laser point cloud data.

[0127] Environmental feature points are then extracted from single-frame laser point cloud data after denoising. Based on a kinematic model, the coordinates of these multi-frame detected environmental feature points can be transformed to the camera coordinate system. Given the known inherent parameters of the camera, this embodiment of the invention performs joint calibration of the camera and lidar. To achieve coordinate system unification, the laser point cloud data can be transformed to the camera coordinate system.

[0128] Specifically, feature matching and lidar pose estimation can be performed on point cloud data of two adjacent frames according to the rules for selecting feature points. Specifically, points with sharp edges and spots on planar surfaces can be selected as key points, and the smoothness and number of channels of these key points can be calculated to match the lidar point cloud in each scan.

[0129] Feature matching can employ Iterative Closest Point (ICP), which allows us to obtain the second pose estimation result using formula (12).

[0130]

[0131] in, Used to represent a point in the k-th frame in the camera coordinate system {C}. Used to represent a point in the (k-1)th frame in the camera coordinate system {C}.

[0132] The solution can be obtained using LM nonlinear optimization.

[0133] Optionally, the aforementioned lidar can use the LeiShen C16 lidar as an environmental perception sensor. Its signal transmission method is unlimited power and signal transmission, and it has 16 laser transmitting and receiving devices. It has a 360-degree horizontal viewing angle and a 30-degree vertical viewing angle, a maximum range of 150m, and a ranging accuracy of 3cm.

[0134] Optionally, the LeiShen C16 LiDAR can be mounted on the top of the device, such as on the top of an autonomous vehicle.

[0135] Optionally, the threshold function ω of the wavelet transform method m We can obtain it using formula (2):

[0136]

[0137] Wherein, ω represents the distance value of the laser point cloud in the laser point cloud data, and λ represents a fixed threshold. The larger the threshold, the more obvious the denoising effect; conversely, the smaller the threshold, the closer it is to the original signal.

[0138] Optionally, λ can be selected based on the laser point cloud data and experimental environment; for example, λ = 30 can be selected.

[0139] The following example illustrates the pose estimation method for the device provided in the embodiments of the present invention.

[0140] Figure 2 This is a second schematic flowchart of the pose estimation method for the device provided by the present invention, as shown below. Figure 2 As shown, the second-order Taylor series of the system transfer matrix F and the observation matrix H are retained. The first pose estimation result (camera pose) is used as the system input. The autonomous vehicle pose prediction value is obtained through the state equation, and the predicted observation value of the autonomous vehicle is obtained according to the observation equation.

[0141] The second pose estimation result (LiDAR position information) is then used as the actual observation value of the unmanned vehicle, and the unmanned vehicle state is updated based on the predicted observation value and the actual observation value to obtain the best pose estimation as the LiDAR-visual odometry, which is used as the target pose estimation result.

[0142] Figure 3 This is the third flowchart illustrating the pose estimation method for the device provided by the present invention, as shown below. Figure 3 As shown, the laser SLAM localization method based on wavelet analysis in the device pose estimation method includes steps 301 to 303; wherein:

[0143] Step 301: Obtain raw laser point cloud data from the laser scanner and perform preprocessing operations using an improved wavelet filtering method;

[0144] Step 302: Extract environmental feature points from the preprocessed single-frame laser point cloud data, and transform the coordinates of the environmental feature points detected in multiple frames to the camera coordinate system according to the kinematic model.

[0145] Step 303: According to the rules for selecting feature points, perform feature matching and lidar pose estimation on the point cloud data of two adjacent frames to obtain the second pose estimation result.

[0146] Figure 4 This is the fourth flowchart illustrating the pose estimation method for the device provided by the present invention, as shown below. Figure 4 As shown, the visual SLAM localization method in the device pose estimation method includes steps 401 to 403; wherein:

[0147] Step 401: Acquire image data from the stereo camera and initialize the local map;

[0148] Step 402: According to the bucketing method, the image data is divided into 50×50 rectangles as buckets. All features used for self-motion estimation are selected from the buckets. ORB feature points are extracted from the image data, and the average inter-frame error of each feature point is calculated based on age. Target feature points with smaller errors are selected for visual pose estimation.

[0149] Step 403: Match the target feature points corresponding to the image data with the points updated in the last frame of the local map, construct an error function, and solve it to obtain the first pose estimation result.

[0150] This invention can be implemented using the Robot Operating System (ROS) unmanned vehicle platform. The ROS unmanned vehicle is equipped with hardware devices such as LiDAR, cameras, and NANO development boards, and can be remotely controlled via a computer to move according to program instructions.

[0151] During the movement of the autonomous vehicle, LiDAR and cameras collect LiDAR point cloud data and image data, respectively. After preprocessing, feature matching, pose estimation, and data fusion algorithms, the final pose of the autonomous vehicle is obtained, and the path trajectory curve is plotted. Figure 5 This invention provides a method for estimating the pose of a device and plotting a path trajectory curve, as shown in the diagram. Figure 5 As shown.

[0152] By comparing the estimated and actual values ​​of the turning points, start and end points in the trajectory, the Euclidean distance error at each location is obtained. Figure 6 This is a schematic diagram of the Euclidean distance error curves at various positions obtained by the pose estimation method of the device provided by this invention, as shown in the figure. Figure 6 As shown in the figure, the asterisks represent the pose estimation method of the device provided in the embodiment of the present invention, and the cross and circles represent existing algorithms used for comparison. Figure 6 As can be seen from the above, the pose estimation method of the device provided in the embodiments of the present invention has a smaller positioning error and more accurate positioning.

[0153] The above embodiments use the ROS unmanned vehicle platform. This invention can also be applied to other mobile robots and other fields, such as inspection robots and automated guided vehicles (AGV) robots.

[0154] Traditional LiDAR-based laser SLAM methods for autonomous vehicle localization do not consider LiDAR noise and cannot obtain accurate position and attitude information. This invention provides a device pose estimation method based on an improved EKF and wavelet filtering, which can accurately locate the vehicle even in large and complex environments.

[0155] Compared with the prior art, the embodiments of the present invention have the following advantages:

[0156] 1. Preprocessing the raw data of lidar using an improved wavelet transform method can improve the quality of the sensor's extraction of features of the surrounding environment.

[0157] 2. The features corresponding to the image to be located are filtered, and the PnP (Perspective-n-Points)-Iterative Closest Point (ICP) combination method is used to realize feature matching and pose estimation, making feature matching more effective.

[0158] 3. Multi-sensor fusion based on improved extended Kalman filtering can avoid errors caused by linearization, reduce odometer drift, and improve the positioning accuracy of unmanned vehicles.

[0159] The pose estimation device of the device provided by the present invention is described below. The pose estimation device of the device described below can be referred to in correspondence with the pose estimation method of the device described above.

[0160] Figure 7 This is a schematic diagram of the pose estimation device of the equipment provided by the present invention, wherein a camera and a lidar are provided on the device; as shown Figure 7 As shown, the pose estimation device 700 of the device includes:

[0161] The first pose estimation module 701 is used to perform visual pose estimation on the device based on the image data acquired by the camera, and obtain the first pose estimation result.

[0162] The second pose estimation module 702 is used to perform laser radar pose estimation on the device based on the laser point cloud data collected by the laser radar, and obtain the second pose estimation result.

[0163] The acquisition module 703 is used to obtain the target pose estimation result of the device based on the first pose estimation result and the second pose estimation result.

[0164] In this embodiment of the invention, a first pose estimation module and a second pose estimation module respectively perform visual and lidar pose estimation on the device using image data and lidar point cloud data acquired by the camera and lidar to obtain a first pose estimation result and a second pose estimation result. An acquisition module then uses these first and second pose estimation results to obtain the target pose estimation result of the device. This embodiment of the invention can obtain the target pose estimation result of the device based on the pose estimation results obtained from the camera and lidar respectively, and combine these two results. Since cameras have the advantages of low cost, light weight, and images containing rich texture information, they can effectively improve positioning accuracy compared to single-laser SLAM positioning methods.

[0165] Optionally, module 703 is specifically used for:

[0166] The first pose estimation result is input into the state equation in the pre-set extended Kalman filter equation to obtain the pose prediction value of the device; wherein, the system transfer matrix F of the state equation includes a second-order Taylor series;

[0167] The pose prediction value is input into the observation equation in the extended Kalman filter equation to obtain the predicted observation value of the device; wherein, the observation matrix H of the observation equation includes a second-order Taylor series;

[0168] The second pose estimation result is used as the actual observation value of the device;

[0169] Based on the predicted observations and the actual observations, the target pose estimation result of the device is obtained.

[0170] Optionally, the first pose estimation module 701 is specifically used for:

[0171] Based on a pre-set bucketing method, the image data is divided into N rectangles as N buckets, where N is an integer greater than 1;

[0172] Feature points are extracted from the N buckets respectively to obtain the ORB feature points in the image data;

[0173] Initialize a local map based on the image data;

[0174] Based on the ORB feature points and the feature points in the last frame of the local map, the device performs visual pose estimation to obtain the first pose estimation result.

[0175] Optionally, the first pose estimation module 701 is also specifically used for:

[0176] Based on the survival age of each ORB feature point, the average inter-frame error of each ORB feature point is calculated.

[0177] ORB feature points with an average inter-frame error less than a first threshold are selected as target feature points;

[0178] Calculate the error between the target feature point and the feature points in the last frame of the local map, and construct an error function;

[0179] Visual pose estimation of the device is performed based on the error function to obtain the first pose estimation result.

[0180] Optionally, the first pose estimation module 701 is also specifically used for:

[0181] Based on the survival age of each ORB feature point, the average inter-frame error (Error) of each ORB feature point is calculated using formula (1):

[0182]

[0183] Among them, P k P represents the k-th feature point. k-1 The (k-1)th feature point, Age sur Characteristic point P k-1 Survival age, M k-1|k The true value of the motion matrix, P k -M k-1|k ·P k-1 Characterizes the single-step inter-frame tracking error.

[0184] Optionally, the second pose estimation module 702 is specifically used for:

[0185] Based on a pre-set wavelet transform method, the laser point cloud data is preprocessed to obtain denoised laser point cloud data.

[0186] Environmental feature points are extracted from the denoised laser point cloud data, and the laser radar pose estimation is performed on the device based on the environmental feature points to obtain the second pose estimation result.

[0187] Optionally, the threshold function ω of the wavelet transform method m It is obtained using formula (2):

[0188]

[0189] Wherein, ω represents the distance value of the laser point cloud in the laser point cloud data, and λ represents the fixed threshold.

[0190] Figure 8 This is a schematic diagram of the structure of the electronic device provided by the present invention, such as... Figure 8 As shown, the electronic device 800 may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call logical instructions in the memory 830 to execute a pose estimation method for the device. The device is equipped with a camera and a lidar. The method includes:

[0191] Visual pose estimation of the device is performed based on the image data acquired by the camera to obtain the first pose estimation result;

[0192] Based on the laser point cloud data collected by the lidar, the device is subjected to lidar pose estimation to obtain a second pose estimation result;

[0193] Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device is obtained.

[0194] Furthermore, the logical instructions in the aforementioned memory 830 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0195] On the other hand, the present invention also provides a computer program product, the computer program product comprising a computer program that can be stored on a non-transitory computer-readable storage medium, wherein when the computer program is executed by a processor, the computer is able to execute the pose estimation method of the device provided by the above methods, wherein the device is equipped with a camera and a lidar, and the method includes:

[0196] Visual pose estimation of the device is performed based on the image data acquired by the camera to obtain the first pose estimation result;

[0197] Based on the laser point cloud data collected by the lidar, the device is subjected to lidar pose estimation to obtain a second pose estimation result;

[0198] Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device is obtained.

[0199] In another aspect, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a pose estimation method for a device provided by the methods described above, wherein the device is equipped with a camera and a lidar, and the method includes:

[0200] Visual pose estimation of the device is performed based on the image data acquired by the camera to obtain the first pose estimation result;

[0201] Based on the laser point cloud data collected by the lidar, the device is subjected to lidar pose estimation to obtain a second pose estimation result;

[0202] Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device is obtained.

[0203] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0204] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0205] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for estimating the pose of a device, characterized in that, The device is equipped with a camera and a lidar; the method includes: Visual pose estimation of the device is performed based on the image data acquired by the camera to obtain a first pose estimation result. This process includes: dividing the image data into N rectangles (where N is an integer greater than 1) using a pre-set bucketing method; extracting feature points from each of the N buckets to obtain ORB feature points in the image data; initializing a local map based on the image data; and performing visual pose estimation of the device based on the ORB feature points and feature points in the last frame of the local map to obtain the first pose estimation result. Based on the laser point cloud data collected by the lidar, the device is subjected to lidar pose estimation to obtain a second pose estimation result; Based on the first pose estimation result and the second pose estimation result, the target pose estimation result of the device is obtained.

2. The pose estimation method for the device according to claim 1, characterized in that, The step of obtaining the target pose estimation result of the device based on the first pose estimation result and the second pose estimation result includes: The first pose estimation result is input into the state equation in the pre-set extended Kalman filter equation to obtain the pose prediction value of the device; wherein, the system transfer matrix F of the state equation includes a second-order Taylor series; The pose prediction value is input into the observation equation in the extended Kalman filter equation to obtain the predicted observation value of the device; wherein, the observation matrix H of the observation equation includes a second-order Taylor series; The second pose estimation result is used as the actual observation value of the device; Based on the predicted observations and the actual observations, the target pose estimation result of the device is obtained.

3. The pose estimation method for the device according to claim 1, characterized in that, The first pose estimation result is obtained by performing visual pose estimation on the device based on the ORB feature points and the feature points in the last frame of the local map, including: Based on the survival age of each ORB feature point, the average inter-frame error of each ORB feature point is calculated. ORB feature points with an average inter-frame error less than a first threshold are selected as target feature points; Calculate the error between the target feature point and the feature points in the last frame of the local map, and construct an error function; Visual pose estimation of the device is performed based on the error function to obtain the first pose estimation result.

4. The pose estimation method for the device according to claim 3, characterized in that, The calculation of the average inter-frame error of each ORB feature point based on its survival age includes: Based on the survival age of each ORB feature point, the average inter-frame error (Error) of each ORB feature point is calculated using formula (1): Among them, P k P represents the k-th feature point. k-1 The (k-1)th feature point, Age sur Characteristic point P k-1 Survival age, M k-1|k The true value of the motion matrix, P k -M k-1|k ·P k-1 Characterizes the single-step inter-frame tracking error.

5. The pose estimation method for the device according to claim 1 or 2, characterized in that, The process of performing lidar pose estimation on the device based on the lidar point cloud data collected by the lidar to obtain a second pose estimation result includes: Based on a pre-set wavelet transform method, the laser point cloud data is preprocessed to obtain denoised laser point cloud data. Environmental feature points are extracted from the denoised laser point cloud data, and the laser radar pose estimation is performed on the device based on the environmental feature points to obtain the second pose estimation result.

6. The pose estimation method for the device according to claim 5, characterized in that, The threshold function ω of the wavelet transform method m It is obtained using formula (2): Wherein, ω represents the distance value of the laser point cloud in the laser point cloud data, and λ represents the fixed threshold.

7. A pose estimation device for an apparatus, characterized in that, The device is equipped with a camera and a lidar; the device includes: The first pose estimation module is used to perform visual pose estimation on the device based on the image data acquired by the camera, and obtain the first pose estimation result. The process of performing visual pose estimation on the device based on the image data acquired by the camera and obtaining the first pose estimation result includes: dividing the image data into N rectangles as N buckets based on a pre-set bucketing method, where N is an integer greater than 1; extracting feature points from each of the N buckets to obtain ORB feature points in the image data; initializing a local map based on the image data; and performing visual pose estimation on the device based on the ORB feature points and feature points in the last frame of the local map to obtain the first pose estimation result. The second pose estimation module is used to perform lidar pose estimation on the device based on the lidar point cloud data collected by the lidar, and obtain the second pose estimation result. The acquisition module is used to obtain the target pose estimation result of the device based on the first pose estimation result and the second pose estimation result.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the pose estimation method of the device as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the pose estimation method of the device as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the pose estimation method of the device as described in any one of claims 1 to 6.

Images