Positioning method and device, computing equipment and program product

Abstract

The invention provides a positioning method and device, computing equipment and a program product applied to the technical field of robots, and the method comprises the steps: obtaining a target image, target point cloud data and target inertial test unit (IMU) data according to a positioning frequency; extracting image feature points from the target image, assigning depth values of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extracting point cloud feature points from the target point cloud data; determining an IMU odometer factor according to the target IMU data, determining a camera IMU odometer factor according to the depth feature points and the IMU odometer factor, and determining a radar IMU odometer factor according to the point cloud feature points and the IMU odometer factor; and determining the target pose of the robot according to the IMU odometer factor, the camera IMU odometer factor and the radar IMU odometer factor. According to the invention, mutual constraint is carried out by combining multi-sensor data to realize positioning, and the problem of inaccurate positioning of the robot can be solved.

Description

Technical Field

[0001] The present disclosure relates to the field of robotics, and in particular to a positioning method, apparatus, computing device, and program product. Background Art

[0002] Intelligent warehousing, a crucial process in logistics management, can effectively improve cargo management efficiency through its rational use. Within intelligent warehousing, inventory counting is a crucial task that requires the use of various counting robots. However, current robots often suffer from inaccurate positioning, impacting both efficiency and accuracy. Summary of the Invention

[0003] The embodiments of the present disclosure at least provide a positioning method, apparatus, computing device, and program product.

[0004] In a first aspect, an embodiment of the present disclosure provides a positioning method, including: As the robot moves in the warehouse, it obtains target images, target point cloud data, and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency. Extracting image feature points from the target image, assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extracting point cloud feature points from the target point cloud data; Determine an IMU odometry factor based on the target IMU data, and determine a camera IMU odometry factor under IMU constraints based on the depth feature points and the IMU odometry factor, and determine a radar IMU odometry factor under IMU constraints based on the point cloud feature points and the IMU odometry factor; Determine a target pose of the robot in a world coordinate system according to the IMU odometry factor, the camera IMU odometry factor, and the radar IMU odometry factor.

[0005] This implementation method locates the robot in the warehouse by fusing data from multiple sensors. During positioning, a camera-IMU subsystem and a radar-IMU subsystem are constructed, and the camera and radar poses are optimized and constrained in combination with the IMU data. This can improve the accuracy of the obtained camera odometry factor and radar odometry factor. The robot's pose is then determined using the radar odometry factor, camera odometry factor, and IMU odometry factor. This allows the robot's pose in the IMU coordinate system to be accurately converted to the world coordinate system, resulting in the robot's accurate pose in the world coordinate system. The entire positioning process, by combining data from multiple sensors and using these data to constrain each other to achieve positioning, can effectively improve the robot's positioning accuracy.

[0006] In a possible implementation, obtaining target images, target point cloud data, and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency includes: When the robot moves in the warehouse, each frame of initial image captured by the robot's camera at the image acquisition frequency, each piece of initial IMU data captured by the robot's IMU device at the IMU acquisition frequency, and each frame of initial point cloud data captured by the robot's lidar at the point cloud acquisition frequency are acquired; the image acquisition frequency is lower than the IMU acquisition frequency, and the point cloud acquisition frequency is lower than the image acquisition frequency; the positioning frequency is higher than the point cloud acquisition frequency and lower than the image acquisition frequency; When it is determined that it is not the first positioning based on the positioning frequency, the target image frames between the current positioning and the previous positioning are determined from the initial image, and the target IMU data between the current positioning and the previous positioning are determined from the initial IMU data, and the target point cloud data collected before the current positioning is determined from the initial point cloud data.

[0007] In one possible implementation, the method further includes: When the first positioning is determined according to the positioning frequency, the target IMU data required for the first positioning is obtained from the initial IMU data; The IMU odometry factor corresponding to the first positioning is determined according to the target IMU data, and the IMU odometry factor corresponding to the first positioning is used as the camera IMU odometry factor and radar IMU odometry factor for the first positioning.

[0008] In a possible implementation, extracting image feature points from the target image and assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points includes: Determine, from each frame of the target image, an image to be projected whose acquisition time is closest to the target point cloud data, and extract image feature points from the image to be projected; Reprojecting each point cloud point in the target point cloud data onto the image to be projected to obtain each projection feature point; According to the depth information and position of each projection feature point, a depth value is assigned to each image feature point in the image to be projected to obtain each depth feature point.

[0009] In one possible implementation, the IMU odometry factor includes a first IMU odometry factor obtained by pre-integrating the IMU data to be used; the IMU data to be used is target IMU data whose acquisition time is between the acquisition time of the image to be projected and the acquisition time of an adjacent target image of the image to be projected; Determining a camera IMU odometry factor under IMU constraints according to the depth feature point and the IMU odometry factor includes: Extracting image feature points from an adjacent target image of the image to be projected, and determining a camera odometry factor of the camera under current positioning according to the depth feature points of the image to be projected and the image feature points of the adjacent target image; Determine a camera IMU odometry factor under IMU constraints according to the first IMU odometry factor and the camera odometry factor.

[0010] In a possible implementation, determining the camera IMU odometry factor under IMU constraints based on the first IMU odometry factor and the camera odometry factor includes: Determining whether there is a target image to be projected similar to the image to be projected corresponding to the current positioning based on the image feature points of the image to be projected corresponding to the current positioning and the image feature points of the images to be projected corresponding to the positionings before the current positioning; If so, the camera IMU odometry factor of the camera at the current positioning is determined according to the camera IMU odometry factor corresponding to the target image to be projected, the camera odometry factor at the current positioning, and the first IMU odometry factor.

[0011] In a possible implementation, determining the camera odometry factor of the camera in the current positioning according to the depth feature points of the image to be projected and the image feature points of the adjacent target image includes: In the target image, if there are other images other than the image to be projected and the adjacent target image, determining similarities between image feature points of the other images and image feature points of the image to be projected; When the similarity satisfies a preset similarity, a camera odometry factor of the camera under the current positioning is determined according to the image feature points of the other images, the depth feature points of the image to be projected, and the image feature points of the adjacent target image.

[0012] In a possible implementation, determining the camera IMU odometry factor under IMU constraints based on the first IMU odometry factor and the camera odometry factor includes: Determine a first pre-integration error corresponding to the current positioning according to a first IMU odometry factor corresponding to the current positioning and first IMU odometry factors corresponding to each positioning other than the current positioning; Determining a reprojection error corresponding to a current positioning according to each image feature point in the image to be projected and the camera odometry factor; The camera IMU odometry factor under the IMU constraint is determined with the goal of minimizing the sum of the first pre-integration error and the reprojection error.

[0013] In one possible implementation, the IMU odometry factor includes a second IMU odometry factor obtained by pre-integrating the target IMU data; The determining, based on the point cloud feature points and the IMU odometry factor, a radar IMU odometry factor under IMU constraints includes: Determining a radar odometry factor of the lidar based on the point cloud feature points corresponding to the current positioning and the constructed warehouse point cloud map corresponding to the current positioning; Determine a radar IMU odometry factor under IMU constraints according to the radar odometry factor and the second IMU odometry factor.

[0014] In a possible implementation, determining the radar IMU odometry factor under IMU constraints based on the radar odometry factor and the second IMU odometry factor includes: Determine whether there is matching point cloud data similar to the target point cloud data corresponding to the current positioning based on the point cloud feature points corresponding to the current positioning and the point cloud feature points corresponding to each positioning before the current positioning; If so, determine the radar IMU odometry factor under the IMU constraint according to the radar odometry factor corresponding to the current positioning, the radar IMU odometry factor corresponding to the matching point cloud data, and the second IMU odometry factor.

[0015] In a possible implementation, determining the radar IMU odometry factor under IMU constraints based on the radar odometry factor and the second IMU odometry factor includes: Determine a second pre-integration error corresponding to the current positioning according to the second IMU odometry factor corresponding to the current positioning and the second IMU odometry factors corresponding to each positioning other than the current positioning; Determine the matching error corresponding to the current sub-positioning based on the radar odometry factor corresponding to the current sub-positioning and the point cloud feature points corresponding to the current sub-positioning; With the goal of minimizing the sum of the second pre-integration error and the matching error, a radar IMU odometry factor under IMU constraints is determined.

[0016] In one possible implementation, after determining the radar IMU odometry factor under the IMU constraint, the method further includes: According to the radar IMU odometry factor corresponding to the current positioning, the point cloud feature points corresponding to the current positioning are filtered to obtain the filtered feature points corresponding to the current positioning; According to the filtered feature points of the current positioning, the constructed warehouse point cloud map is supplemented to obtain the supplemented warehouse point cloud map; In response to the completion of positioning, the supplemented warehouse point cloud map corresponding to the last positioning is used as the global point cloud map of the warehouse.

[0017] In a possible implementation, extracting point cloud feature points from the target point cloud data includes: Determine that the target point cloud data includes each point cloud line and each point cloud point in the point cloud line; For each of the point cloud lines, determining the smoothness of each point cloud point in the point cloud line according to the coordinates of each point cloud point in the radar coordinate system, and determining the edge points and surface points in the point cloud line according to the smoothness; The edge points and surface points in each of the point cloud lines are used as the point cloud feature points.

[0018] In a possible implementation, before determining that the point cloud feature points include each point cloud line and each point cloud point in the point cloud line, the method further includes: Using the target IMU data corresponding to the current positioning, determine the motion trajectory of the laser radar when collecting the target point cloud data; According to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0019] In a possible implementation, after determining the target pose of the robot in the world coordinate system, the method further includes: According to the target posture obtained in each positioning, the movement trajectory of the robot in the warehouse is drawn.

[0020] In a second aspect, an embodiment of the present disclosure further provides a positioning device, including: The acquisition module is used to obtain the target image, target point cloud data and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency during the robot's movement in the warehouse; an extraction module, configured to extract image feature points from the target image, assign depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extract point cloud feature points from the target point cloud data; a first determination module, configured to determine an IMU odometry factor based on the target IMU data, and determine a camera IMU odometry factor under IMU constraints based on the depth feature points and the IMU odometry factor, and determine a radar IMU odometry factor under IMU constraints based on the point cloud feature points and the IMU odometry factor; The second determination module is used to determine the target pose of the robot in the world coordinate system according to the IMU odometry factor, the camera IMU odometry factor and the radar IMU odometry factor.

[0021] In a third aspect, an optional implementation of the present disclosure further provides a computer device, a processor, and a memory, wherein the memory stores machine-readable instructions executable by the processor, and the processor is used to execute the machine-readable instructions stored in the memory, and when the machine-readable instructions are executed by the processor, the steps of the above-mentioned first aspect or any possible implementation of the first aspect are performed.

[0022] In a fourth aspect, an optional implementation of the present disclosure further provides a computer program product, on which a computer-readable storage medium stores the computer program product, and when the computer program product is run, it executes the steps of the above-mentioned first aspect, or any possible implementation of the first aspect.

[0023] For a description of the effects of the above-mentioned positioning device, computer equipment, and computer program product, please refer to the description of the above-mentioned positioning method, which will not be repeated here.

[0024] In order to make the above-mentioned objectives, features and advantages of the present disclosure more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the following briefly introduces the drawings required for use in the embodiments. The drawings herein are incorporated into and constitute a part of the specification. These drawings illustrate embodiments consistent with the present disclosure and, together with the specification, are used to illustrate the technical solutions of the present disclosure. It should be understood that the following drawings only illustrate certain embodiments of the present disclosure and should not be regarded as limiting the scope. For those of ordinary skill in the art, other relevant drawings can be obtained based on these drawings without inventive effort.

[0026] Figure 1 A flowchart of a positioning method provided by an embodiment of the present disclosure is shown; Figure 2 A schematic diagram of a process for extracting image feature points provided by an embodiment of the present disclosure is shown; Figure 3 A schematic diagram of a reprojection process provided by an embodiment of the present disclosure is shown; Figure 4 A schematic diagram of a process for determining a camera-IMU odometry factor provided by an embodiment of the present disclosure is shown; Figure 5 A schematic diagram of a process for determining a radar-IMU odometry factor provided by an embodiment of the present disclosure is shown; Figure 6 A comparison diagram of point clouds before and after motion compensation provided by an embodiment of the present disclosure is shown; Figure 7-1 A top view comparison diagram of a trajectory map obtained by using a positioning method according to an embodiment of the present disclosure and a trajectory map obtained by using a mapping method in the prior art is shown; Figure 7-2 A side view comparison diagram of a trajectory map obtained by using a positioning method according to an embodiment of the present disclosure and a trajectory map obtained by using a mapping method in the prior art is shown; Figure 7-3 A trajectory comparison diagram corresponding to the XYZ axes is shown, which is obtained by using the positioning method of the embodiment of the present disclosure to perform trajectory mapping and using a mapping method in the prior art; Figure 7-4 A schematic diagram of a global point cloud map constructed using the positioning algorithm provided by an embodiment of the present disclosure is shown; Figure 7-5 A schematic diagram of a global point cloud map constructed using a laser-IMU mapping algorithm in the prior art provided by an embodiment of the present disclosure is shown; Figure 8 A schematic diagram of a system framework of a posture determination system provided by an embodiment of the present disclosure is shown; Figure 9 A schematic diagram of a positioning device provided by an embodiment of the present disclosure is shown; Figure 10 A schematic structural diagram of a computer device provided by an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0027] In order to make the purpose, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only part of the embodiments of the present disclosure, rather than all of the embodiments. The components of the embodiments of the present disclosure generally described and shown here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present disclosure is not intended to limit the scope of the present disclosure for protection, but merely represents the selected embodiments of the present disclosure. Based on the embodiments of the present disclosure, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present disclosure.

[0028] In addition, the terms "first", "second", etc. in the description and claims of the embodiments of the present disclosure and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. The "plurality or severalness" mentioned in this article refers to two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships can exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the objects associated before and after are in an "or" relationship.

[0029] Research has found that when robots perform tasks like picking up goods or taking inventory in a warehouse, they need to locate their position in real time and plan their movement paths based on this real-time location to improve their task execution efficiency. However, conventional robot positioning methods suffer from low positioning accuracy, which affects the robot's task execution efficiency and is a significant drawback.

[0030] Based on the above research, the present disclosure provides a positioning method, device, computing equipment and program product, which locates the position of the robot in the warehouse by fusing data from multiple sensors, and during positioning, by constructing a camera-IMU subsystem and a radar-IMU subsystem, and optimizing and constraining the camera pose and radar pose in combination with the IMU data, the accuracy of the obtained camera odometer factor and radar odometer factor can be improved. Then, the robot pose is determined by using the radar odometer factor, the camera odometer factor and the IMU odometer factor, which can realize the accurate conversion of the robot's pose in the IMU coordinate system to the world coordinate system, and obtain the accurate pose of the robot in the world coordinate system. The entire positioning process, by combining data from multiple sensors and using these data to constrain each other to achieve positioning, can effectively improve the positioning accuracy of the robot.

[0031] The defects in the above solutions are the results obtained by the inventors after practice and careful research. Therefore, the process of discovering the above problems and the solutions proposed by this disclosure for the above problems below should be the contributions made by the inventors to this disclosure during the disclosure process.

[0032] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, it does not need to be further defined or explained in subsequent drawings.

[0033] It should be noted that the specific terms mentioned in the embodiments of the present disclosure include: Georgia Tech Smoothing and Mapping (GTSAM), a nonlinear optimization library, is widely used for localization and mapping problems in robots and autonomous vehicles. It includes a series of efficient algorithms for processing sensor data and map information to estimate the robot or vehicle's path and surrounding environment. GTSAM can also solve more complex estimation problems, making it suitable for a variety of scenarios requiring precise estimation and optimization.

[0034] To facilitate understanding of this embodiment, a positioning method disclosed in an embodiment of the present disclosure is first introduced in detail. The execution subject of the positioning method provided in the embodiment of the present disclosure is generally a terminal device or other processing device with certain computing capabilities, where the terminal device can be a user equipment (UE), a mobile device, a user terminal, a terminal, a personal digital assistant (PDA), a handheld device, a computer device, etc.; in some possible implementations, the positioning method can be implemented by a processor calling computer-readable instructions stored in a memory.

[0035] The positioning method provided by the embodiment of the present disclosure is described below by taking a computer device as an example.

[0036] like Figure 1 FIG. 1 is a flowchart of a positioning method provided by an embodiment of the present disclosure, which may include the following steps: S101: When the robot moves in the warehouse, the target image, target point cloud data and target inertial measurement unit (IMU) data required for each positioning are obtained according to the positioning frequency.

[0037] Here, the robot can be any intelligent warehouse robot capable of performing tasks such as cargo handling, inventory counting, and warehouse mapping. A warehouse can include multiple shelves, each of which can contain multiple layers of storage space. The positioning frequency can be a pre-set frequency based on experience, such as 5 Hz or 10 Hz. The robot can be equipped with various types of sensors, each used to collect different types of data. Specifically, these sensors can include any type of camera, any type of lidar, and any type of inertial measurement unit (IMU). For example, the robot can be equipped with a high-precision camera, a mechanical lidar, and a nine-axis IMU. The camera can capture warehouse images during the robot's movement. During each positioning, the image captured by the camera between the current and previous positioning is defined as the target image. The lidar can be the point cloud data collected by the robot's lidar during movement. During each positioning, the point cloud data collected by the camera between the current and previous positioning is defined as the target point cloud data. The IMU device can collect IMU data from the robot during movement, such as linear velocity and acceleration data. During each positioning, the images collected by the IMU device between the current positioning and the previous positioning are defined as target IMU data.

[0038] Optionally, the positioning frequency, the camera's image acquisition frequency, the lidar's point cloud acquisition frequency, and the IMU device's IMU acquisition frequency can be different or partially the same. Typically, the IMU acquisition frequency is higher than the other frequencies.

[0039] In specific implementations, when the robot moves while performing tasks in the warehouse, the camera, lidar, and IMU devices on the robot can all collect corresponding data according to their respective acquisition frequencies. At the same time, the timing of each positioning can be determined according to the positioning frequency. When the positioning timing is reached, the target image frames, target IMU data, and target point cloud data required for each positioning can be determined based on the acquisition time of each frame of image collected by the camera, the acquisition time of the point cloud data collected by the lidar, the acquisition time of each piece of IMU data collected by the IMU device, and the positioning time of each positioning.

[0040] S102: extracting image feature points from the target image, assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extracting point cloud feature points from the target point cloud data.

[0041] Here, image feature points can be key points with obvious features in the target image, such as corner points and edge points in the image. Corner points can be understood as points formed at the intersection of boundaries in different directions. Such points are very conspicuous and easy to detect in the target image, and can maintain good characteristics even when the camera moves. Edge points can be points at the edge of an object in the image. Such points have obvious pixel changes in the image. The lines drawn by the detected edge points can reflect the contour information of the object. For example, in a warehouse, the detected edge points can reflect the contour information of each shelf. Depth feature points are image feature points with depth values.

[0042] The target point cloud data includes a large number of point cloud points. The point cloud feature points may be key point cloud points with obvious features in the target point cloud data. For example, the point cloud feature points may be edge points and surface points in the target point cloud data.

[0043] In specific implementation, for the current positioning in any positioning, feature points of the target image corresponding to the current positioning can be extracted to obtain the corner points and edge points in the target image, and these corner points and edge points are used as image feature points. Figure 2 FIG. 1 is a schematic diagram of a process for extracting image feature points provided by an embodiment of the present disclosure, wherein: Figure 2Figure 2-1 is the target image, and the points in Figure 2-2 are the identified image feature points. Optionally, to avoid an excessive number of image feature points of each type, a number threshold can be set for each type of image feature point. For example, a first threshold is set for corner points, and a second threshold is set for edge points. Corner points that do not exceed the first preset number and edge points that do not exceed the second threshold are extracted from the target image. When extracting feature points from the target image, a deep learning model can be used, or the points can be determined based on the position and pixel value of the pixels. After obtaining the image feature points corresponding to the current positioning, the target point cloud data corresponding to the current positioning can be projected onto the target image. Based on the position of the projected point cloud point in the target image and the position of the image feature point in the target image, the closest projected point cloud point to each image feature point is determined. The depth value of the closest projected point cloud point is assigned to the image feature point to obtain the depth feature point corresponding to each image feature point. At the same time, for the target point cloud data corresponding to the current positioning, a point cloud detection algorithm can be used to extract the surface points and edge points from the target point cloud data as point cloud feature points. Among them, when extracting point cloud feature points, corresponding quantity thresholds can also be used for quantity control. For the specific quantity control process, please refer to the quantity control process of image point cloud points, which will not be repeated here.

[0044] Optionally, before projecting the target point cloud data onto the target image, the target point cloud data may be subjected to point cloud dedistortion processing to obtain undistorted point cloud data. The undistorted target point cloud data is then projected onto the target image. Any dedistortion algorithm known in the art may be used for the dedistortion processing.

[0045] S103: Determine an IMU odometry factor based on the target IMU data, and determine a camera IMU odometry factor under IMU constraints based on the depth feature points and the IMU odometry factor, and determine a radar IMU odometry factor under IMU constraints based on the point cloud feature points and the IMU odometry factor.

[0046] Here, the embodiment of the present disclosure can use the IMU coordinate system as the main coordinate system, and pre-calibrate the internal and external parameters between the radar coordinate system L, the camera coordinate system C, and the world coordinate system W. The main goal is to estimate the relative position change between the IMU coordinate system and the world coordinate system W. According to the relative change, the IMU posture of the robot can be converted into the posture in the world coordinate system. At the same time, a posture determination system can be constructed, which includes a camera-IMU subsystem, a radar-IMU subsystem, and an IMU subsystem. By determining the odometry factor under each subsystem, the target posture of the robot in the world coordinate system and the coordinates of each three-dimensional point in the warehouse in the world coordinate system are determined. The three-dimensional points can be point cloud feature points selected from the point cloud feature points included in the point cloud data.

[0047] The odometry factor can be understood as posture. The IMU odometry factor is used to characterize the posture of the IMU device in the IMU subsystem. The camera IMU odometry factor is used to characterize the posture of the camera in the camera-IMU subsystem. The radar IMU odometry factor is used to characterize the posture of the radar in the radar-IMU subsystem.

[0048] In specific implementations, for the current positioning, the IMU odometry factor corresponding to the current positioning can be obtained by pre-integrating the IMU data corresponding to the current positioning. The initial camera odometry factor of the camera is determined based on the depth feature points corresponding to the current positioning, and the IMU odometry factor is used to constrain and optimize the initial odometry factor to obtain the camera-IMU odometry factor (i.e., the camera-IMU odometry factor). The initial lidar odometry factor of the lidar is determined based on the point cloud feature points, and the IMU odometry factor is used to constrain and optimize the initial radar odometry factor to obtain the radar-IMU odometry factor (i.e., the radar-IMU odometry factor).

[0049] For any positioning, the above S101 to S103 can be used to determine the IMU odometry factor, camera-IMU odometry factor, and radar-IMU odometry factor corresponding to the positioning.

[0050] S104: Determine the target pose of the robot in the world coordinate system based on the IMU odometry factor, the camera IMU odometry factor, and the radar IMU odometry factor.

[0051] Here, the target pose represents the robot's position in the world coordinate system, and one positioning step corresponds to one target pose. The target pose can include the robot's coordinate information, orientation information, rotation information, translation information, velocity information, etc. The zero bias information represents the robot's linear velocity and acceleration.

[0052] In specific implementation, for any positioning, the factor graph model provided by the GTSAM library can be used to model and optimize the factors based on the IMU odometry factor, camera-IMU odometry factor, and radar-IMU odometry factor corresponding to that positioning, thereby outputting the robot's IMU pose in the IMU coordinate system. Then, based on the conversion relationship between the IMU coordinate system and the world coordinate system, the IMU pose is converted to the robot's target pose. Through each positioning at the positioning frequency, the target pose corresponding to each positioning can be determined.

[0053] In this way, the robot's position in the warehouse is determined by fusing data from multiple sensors. During positioning, a camera-IMU subsystem and a radar-IMU subsystem are constructed, and the camera and radar poses are optimized and constrained using IMU data. This improves the accuracy of the camera and radar odometry factors. The radar, camera, and IMU odometry factors are then used to determine the robot's pose. The robot's pose in the IMU coordinate system can be accurately converted to the world coordinate system, resulting in the robot's accurate pose in the world coordinate system. This entire positioning process, by combining data from multiple sensors and using these data to constrain each other, effectively improves the robot's positioning accuracy.

[0054] In one embodiment, the above S101 may be implemented as follows: S101-1: When the robot moves in the warehouse, obtain each frame of initial image collected by the camera on the robot according to the image acquisition frequency, each piece of initial IMU data collected by the IMU device on the robot according to the IMU acquisition frequency, and each frame of initial point cloud data collected by the lidar on the robot according to the point cloud acquisition frequency.

[0055] Among them, the image acquisition frequency is lower than the IMU acquisition frequency, and the point cloud acquisition frequency is lower than the image acquisition frequency; the positioning frequency is higher than the point cloud acquisition frequency and lower than the image acquisition frequency.

[0056] Here, because the image acquisition frequency is lower than the IMU acquisition frequency, the IMU device can collect multiple frames of IMU data between two adjacent frames of images captured by the camera. Because the point cloud acquisition frequency is lower than the image acquisition frequency, the camera can collect at least two frames of images for adjacent point cloud data captured by the lidar. Because the positioning frequency is higher than the point cloud acquisition frequency but lower than the image acquisition frequency, the lidar may not have collected point cloud data during the first positioning. Therefore, the first positioning may only obtain image and IMU data, not point cloud data. In each positioning except the first positioning, the lidar may have collected point cloud data before or simultaneously with the positioning. Therefore, each positioning except the first positioning can obtain the target point cloud data required for use. While the positioning frequency is higher than the point cloud acquisition frequency, it is necessary to ensure that at the same starting time, the lidar will perform the first point cloud data acquisition before the second positioning according to the positioning frequency.

[0057] Each frame of image captured by the camera can be defined as the initial image first. During each positioning, the target image can be determined from the captured initial image; the IMU data captured by the IMU device can be defined as the initial IMU data first. During each positioning, the target initial IMU data can be determined from the captured initial IMU data; each frame of point cloud data captured by the lidar can be defined as the initial point cloud data first. During each positioning, the target initial point cloud data can be determined from the captured initial point cloud data.

[0058] In specific implementations, as the robot moves within the warehouse, the camera, lidar, and IMU devices on the robot can all collect data at their respective acquisition frequencies, generating initial images for each frame from the camera, initial IMU data for each frame from the IMU device, and initial point cloud data for each frame from the lidar. In other words, as the robot moves within the warehouse, each device on the robot will continue to operate at its respective acquisition frequency.

[0059] S101-2: When it is determined that it is not the first positioning based on the positioning frequency, the target image frames between the current positioning and the previous positioning are determined from the initial image, and the target IMU data between the current positioning and the previous positioning are determined from the initial IMU data, and the target point cloud data collected before the current positioning is determined from the initial point cloud data.

[0060] In specific implementation, when positioning is performed according to the positioning frequency, for any non-first positioning, the initial image frames collected by the camera between the current positioning and the previous positioning can be used as target images, or the two latest initial images taken by the camera before the current positioning can be used as target images. At the same time, the initial IMU data collected by the IMU device between the current positioning and the previous positioning can be used as target IMU data, or the initial IMU data between the two filtered target images can be used as target IMU data. At the same time, the latest frame of initial point cloud data collected by the radar before the current positioning can be used as target point cloud data. Then, the target image, target point cloud data and target IMU data corresponding to the current positioning can be used to determine the target pose under the current positioning.

[0061] In another embodiment, when the first positioning is determined based on the positioning frequency, the target IMU data required for the first positioning is obtained from the initial IMU data; the IMU odometer factor corresponding to the first positioning is determined based on the target IMU data, and the IMU odometer factor corresponding to the first positioning is used as the camera IMU odometer factor and radar IMU odometer factor of the first positioning.

[0062] Since the point cloud acquisition frequency is lower than the positioning frequency, when the first positioning is determined according to the positioning frequency, the lidar has not yet collected point cloud data. Therefore, in order to achieve the first positioning, the camera-IMU subsystem and the radar-IMU subsystem can also be initialized. Specifically, during the first positioning, the initial IMU data collected by the IMU device before the positioning can be used as the target IMU data. Then, by pre-integrating the target IMU data, the IMU odometer factor corresponding to the first positioning is determined. At the same time, the IMU odometer factor corresponding to the first positioning can be used as the camera IMU odometer factor and radar IMU odometer factor for the first positioning, thereby achieving the initialization of the camera-IMU subsystem and the radar-IMU subsystem.

[0063] Optionally, during initial positioning, the initial image captured by the camera before initial positioning can be used as the target image. Image feature points are then extracted from each frame of the target image. The image feature points in each frame of the target image are used to determine the initial camera odometry factor for initial positioning. The initial camera odometry factor is then constrained and adjusted using the IMU odometry factor corresponding to the initial positioning to obtain the camera-IMU odometry factor. Furthermore, the IMU odometry factor corresponding to the initial positioning can be used as the radar IMU odometry factor for the initial positioning.

[0064] Furthermore, the IMU odometry factor corresponding to the first positioning, the initialized camera-IMU odometry factor, and the radar-IMU odometry factor can be used to determine the target pose of the robot in the first positioning using a factor graph.

[0065] In one embodiment, the step of "obtaining depth feature points" in S102 may be implemented according to the following steps A1 to A3: A1: From each frame of the target image, determine the image to be projected whose acquisition time is closest to the target point cloud data, and extract image feature points from the image to be projected.

[0066] In specific implementations, for the current positioning (not the first positioning), the acquisition time of each target image frame corresponding to this positioning and the acquisition time of the target point cloud data corresponding to this positioning can be used to select the image with the smallest time interval between the acquisition time of the target image frame and the acquisition time of the target point cloud data. This image is then used as the image to be projected. Feature points can then be extracted from the image to be projected to obtain the individual image feature points.

[0067] A2: Reproject each point cloud point in the target point cloud data to the image to be projected to obtain each projection feature point.

[0068] In specific implementation, the extrinsic parameter matrix between the calibrated camera and radar can be used to reproject each point cloud point in the target point cloud data to the image to be projected, and obtain the projection feature points corresponding to each point cloud point. Alternatively, before reprojection, edge points and occluded points can be removed from the point cloud points based on the position of each point cloud point in the radar coordinate system and the sparsity of the point cloud points. The retained point cloud points can then be projected to the image to be projected to obtain each projection feature point. Here, the reason for elimination is that edge points and occluded points belong to point cloud points with unreliable depth spectrum. By eliminating such point cloud points, the influence of unreliable depth on the positioning results can be avoided.

[0069] like Figure 3 FIG. 1 is a schematic diagram of a reprojection process provided by an embodiment of the present disclosure, wherein: Figure 3 Figure 3-1 is the image to be projected, and the points in Figure 3-1 are the identified image feature points. Figure 3-2 is a schematic diagram of projecting the point cloud points onto the image to be projected. The points in Figure 3-2 include feature points, image feature points, and projection feature points.

[0070] A3: Assign a depth value to each image feature point in the image to be projected according to the depth information and position of each projection feature point to obtain each depth feature point.

[0071] In a specific implementation, the projected feature point closest to each image feature point can be determined based on the coordinates of each projected feature point in the image coordinate system and the coordinates of each image feature point in the image to be projected. The depth value of the closest projected feature point is used as the depth value of the image feature point, thereby obtaining the depth feature point corresponding to the image feature point. In addition, for each image feature point, the depth value of the closest projected feature point to the image feature point can also be associated and stored with the image feature point.

[0072] In one embodiment, during each positioning operation, the IMU odometry factor used in determining the camera-IMU odometry factor may be the same as or different from the IMU odometry factor used in determining the radar-IMU odometry factor. When different, the IMU odometry factor may include a first IMU odometry factor obtained by pre-integrating the IMU data to be used; the IMU data to be used is target IMU data whose acquisition time is between the acquisition time of the image to be projected and the acquisition time of the target image adjacent to the image to be projected.

[0073] Here, the adjacent target image may be a target image acquired before the image to be projected and adjacent to the image to be projected. The IMU data to be used may be each frame of IMU data within the time period between the image to be projected and the adjacent target image. The first IMU odometry factor can be obtained by pre-integrating each frame of the IMU data to be used and averaging the pre-integration results of each frame of the IMU data to be used.

[0074] The step of determining the camera IMU odometry factor in S103 above may be performed as follows: B1 to B2: B1: Extract image feature points from the adjacent target image of the image to be projected, and determine the camera odometry factor of the camera under the current positioning based on the depth feature points of the image to be projected and the image feature points of the adjacent target image.

[0075] Here, the camera odometry factor under the current positioning is used to characterize the camera's pose under the current positioning.

[0076] In specific implementations, for the current sub-positioning, each image feature point can be extracted from the target image adjacent to the image to be projected corresponding to the current sub-positioning. The method for extracting image feature points is similar to the method for extracting image feature points from the target image described above and will not be repeated here. The camera can then be positioned based on the depth feature points of the image to be projected and the image feature points of the adjacent target image to determine the camera odometry factor for the current sub-positioning.

[0077] B2: Determine the camera IMU odometry factor under the IMU constraint based on the first IMU odometry factor and the camera odometry factor.

[0078] In specific implementation, the factor graph model can be used to constrain and optimize the camera odometry factor according to the first IMU odometry factor and the camera odometry factor under the current positioning, so as to obtain the camera IMU odometry factor under the IMU constraint under the current positioning.

[0079] In one embodiment, the above B2 can be implemented according to the following steps: B2-1: Determine whether there is a target image to be projected that is similar to the image to be projected corresponding to the current positioning based on the image feature points of the image to be projected corresponding to the current positioning and the image feature points of the images to be projected corresponding to the positionings before the current positioning.

[0080] Here, the target image to be projected may be an image to be projected corresponding to each positioning before the current positioning, and the similarity between the image to be projected corresponding to the current positioning and the image to be projected corresponding to the current positioning satisfies a similarity threshold.

[0081] To further improve the rationality of the determined camera pose, camera loop closure detection can be performed at each positioning step except the initial positioning step. The specific steps for camera loop closure detection may include: for the current positioning step, based on the image feature points of the image to be projected corresponding to the current positioning step, a first feature vector for the current positioning step is generated. Based on the image feature points of the image to be projected corresponding to each positioning step before the current positioning step (for ease of description, the image to be projected is defined as a historical image to be projected), a second feature vector corresponding to each positioning step before the current positioning step is generated. The similarity between the first feature vector and each second feature vector is then used as the similarity between the image to be projected corresponding to the current positioning step and each historical image to be projected. A determination is made as to whether a target similarity greater than or equal to a similarity threshold exists. If not, it is determined that the target image to be projected does not exist. If so, the historical image to be projected corresponding to the target similarity can be used as the target image to be projected, and the optimized camera IMU odometry factor is determined using B2-1 below.

[0082] Optionally, during loop detection, the depth feature points of the image to be projected corresponding to the current positioning and the depth feature points of the image to be projected corresponding to each positioning before the current positioning can also be used to determine whether the target image to be projected exists.

[0083] B2-2: If yes, determine the camera IMU odometry factor of the camera at the current positioning based on the camera IMU odometry factor corresponding to the target image to be projected, the camera odometry factor at the current positioning, and the first IMU odometry factor.

[0084] Here, since the target image to be projected is the image to be projected corresponding to a positioning before the current positioning, the positioning process corresponding to the target image to be projected has been completed, and the camera-IMU odometry factor corresponding to the target image to be projected has also been calculated.

[0085] During camera loop closure detection, if the target image to be projected is detected, it means that the robot's camera pose when collecting the target image to be projected is very similar to the camera pose when collecting the current image to be projected. In this case, the camera pose when collecting the target image to be projected can be used to optimize the current camera pose. Specifically, regardless of whether there are one or multiple target images to be projected, a factor graph model can be used to constrain and optimize the camera odometry factor based on the camera IMU odometry factor corresponding to each target image to be projected, the camera odometry factor determined during the current positioning, and the first IMU odometry factor determined during the current positioning, to obtain the camera-IMU odometry factor for the current positioning.

[0086] Optionally, after optimizing the camera-IMU odometry factor under the current positioning, the camera-IMU odometry factor under the current positioning can be used as the camera-IMU odometry factor corresponding to the target image to be projected, thereby optimizing the camera-IMU odometry factor corresponding to the target image to be projected.

[0087] In another embodiment, due to its structural characteristics, the camera can stably image at low speeds, and the image captured in a short period of time has little change, which performs better in feature matching. Therefore, in order to further improve the accuracy of the determined camera odometry factor, the step of determining the camera odometry factor in B1 above can be implemented as follows: B1-1: When there are other images in the target image except the image to be projected and the adjacent target images, determine the similarity between the image feature points of the other images and the image feature points of the image to be projected.

[0088] During specific implementation, the target image corresponding to the current positioning is collected between the current positioning and the previous positioning, so it may include multiple frames. Therefore, it is possible to determine whether there are other images in the multiple frames of target images in addition to the image to be projected and the adjacent target images. If not, the camera odometer factor can be directly determined using the depth feature points of the image to be projected and the image feature points in the adjacent target images. If so, feature matching can be performed, that is, the image feature points in each frame of the other images can be extracted. Then, based on the image feature points, the third eigenvector of each other image is determined. At the same time, the first eigenvector of the image to be projected can be determined based on the image feature points of the image to be projected. Then, the similarity between the first eigenvector and each third eigenvector is calculated, and the similarity between the first eigenvector and the third eigenvector is used as the similarity between the image feature points of the other images and the image feature points of the image to be projected.

[0089] B1-2: When the similarity meets the preset similarity, determine the camera odometry factor of the camera under the current positioning based on the image feature points of other images, the depth feature points of the image to be projected, and the image feature points of the adjacent target image.

[0090] Here, the preset similarity can be set based on experience and will not be described in detail here.

[0091] In specific implementations, the similarity between the image feature points of each other image and the image feature points of the image to be projected is used to determine whether other images exist that meet a preset similarity threshold. If not, the camera odometry factor can be directly determined using the depth feature points of the image to be projected and the image feature points of the adjacent target image. If so, the camera can be positioned using the image feature points of other images that meet the preset similarity threshold, the depth feature points of the image to be projected, and the image feature points of the adjacent target image to obtain the camera odometry factor for the current positioning. In this way, by combining image feature points from multiple frames with high similarity for camera positioning, positioning accuracy can be improved.

[0092] like Figure 4 As shown, it is a schematic diagram of the process of determining the camera-IMU odometry factor provided by an embodiment of the present disclosure, wherein the camera key frame is the target image captured by the camera, the visual feature is the image feature point in the target image, and the visual features corresponding to the two adjacent camera key frames are obtained by averaging the visual features corresponding to the two adjacent camera key frames; the aligned depth map can be understood as the depth map obtained by projecting the target point cloud data onto the target image; the visual features with depth are the depth feature points with depth values; the visual features with depth can be determined by the aligned depth map. Figure 4 In the figure, an aligned depth map points to two visual features with depth, where one of the two visual features with depth can be understood as the depth feature points stored in the target image, and the other can be understood as the visual feature with depth used to determine the camera-IMU odometry factor. The IMU measurement is the measured IMU data, and the IMU constraint is the IMU odometry factor. After pre-integrating and averaging multiple IMU data, an IMU odometry factor is obtained. When multiple camera keyframes point to the same visual feature, it can be said that the multiple keyframes are similar, and the visual features of the multiple frames can be used to obtain the pointed visual feature. For each positioning, the multi-frame target image, multi-frame IMU data and target point cloud data required for the positioning can be used to determine the camera-IMU odometry factor for the positioning according to the steps for determining the camera-IMU odometry factor described above.

[0093] Due to their structural characteristics, cameras can produce stable images at low speeds, and since image changes are minimal in a short period of time, they perform well in feature matching. However, at high speeds, image blur and the rapid image changes in a short period of time can lead to large discrepancies, resulting in insufficient overlap and mismatched features. IMU devices, on the other hand, output linear velocity and acceleration data, which can still produce good results even during rapid motion. Therefore, cameras and IMU devices complement each other in both slow and fast motion. By building a camera-IMU subsystem, accurate camera positioning can be achieved.

[0094] In one embodiment, the above B2 can also be determined according to the following steps: The first pre-integration error corresponding to the current positioning is determined based on the first IMU odometry factor corresponding to the current positioning and the first IMU odometry factors corresponding to each positioning other than the current positioning. The reprojection error corresponding to the current positioning is determined based on each image feature point in the image to be projected and the camera odometry factor. The camera IMU odometry factor under the IMU constraint is determined with the goal of minimizing the sum of the first pre-integration error and the reprojection error.

[0095] Here, the first pre-integration error is used to characterize the deviation between the pre-integration measurement value and the state variable from state k-1 to state k, where state k is the target posture of the robot after k positioning times, state k-1 is the target posture of the robot after k-1 positioning times, the pre-integration measurement value is the IMU odometry factor, and the state variable is the robot state change information between state k-1 and state k.

[0096] The reprojection error is used to characterize the difference between the position of the camera feature points in the target image captured by the camera and the camera odometry factor predicted based on the state variable, as well as the error when projecting the target point cloud data into the target image.

[0097] In specific implementation, the camera-IMU odometry factor for the current positioning can be determined according to the following formula: ; (Formula 1) in, 、 、……、 represents the target poses for the first, second, ..., and n+1th positioning, ρ represents the preset formula, Represents the preset IMU error formula, represents the target pose corresponding to k-1 positioning, represents the target pose corresponding to k positioning times, Represents the sum of the first odometry factors corresponding to each positioning, Express Find the mean square; c represents the coordinate set of the image feature points, (i, j) represents the image feature point at the coordinate (i, j), represents the preset reprojection error formula, represents the image feature points in row i, Represents the depth value of the image feature point at (i, j), Represents the sum of the camera odometry factors corresponding to each positioning; Express Find the mean square.

[0098] For example, during each positioning, the first IMU odometry factor, image feature points, and camera odometry factor can be substituted into the above formula 1, and the minimum value corresponding to formula 1 is taken as the goal to obtain the camera IMU odometry factor corresponding to each positioning.

[0099] In one embodiment, during each non-initial positioning, the IMU odometry factor may include a second IMU odometry factor obtained by pre-integrating the target IMU data. Here, for the current positioning, each target IMU data corresponding to the current positioning may be pre-integrated separately, and then the pre-integrated values ​​may be averaged to obtain the second IMU odometry factor corresponding to the current positioning. Regarding the step of determining the radar-IMU odometry factor in S103 above, steps C1 to C2 may be performed as follows: C1: Determine the lidar odometry factor based on the point cloud feature points corresponding to the current positioning and the constructed warehouse point cloud map corresponding to the current positioning.

[0100] Here, the radar odometry factor corresponding to each positioning is used to characterize the position and posture of the lidar under the corresponding positioning. When the robot moves in the warehouse, after each positioning, the point cloud feature points collected by the robot can be used to construct a point cloud map corresponding to the warehouse. As the positioning process progresses, the constructed warehouse point cloud map will become more and more complete. For the first positioning, since there is no constructed warehouse point cloud map at this time, the IMU odometry factor corresponding to the first positioning can be directly used as the radar odometry factor. At the same time, the IMU odometry factor corresponding to the first positioning can be used to filter the point cloud feature points of the first positioning, and filter out the point cloud feature points that match the IMU odometry factor corresponding to the first positioning. These point cloud feature points are used to construct the point cloud warehouse map corresponding to the first positioning.

[0101] In the disclosed embodiments, the current positioning, unless otherwise specified, refers to any positioning other than the initial positioning. For the current positioning, the point cloud feature points from the current positioning can be matched with the constructed warehouse point cloud map. The radar pose with the best match is used as the lidar odometry factor for the current positioning.

[0102] C2: Determine the radar IMU odometry factor under the IMU constraint based on the radar odometry factor and the second IMU odometry factor.

[0103] Exemplarily, for the current sub-positioning, a factor graph model can be used to constrain and adjust the radar odometry factor based on the radar odometry factor of the current sub-positioning and the second IMU odometry factor of the current sub-positioning to obtain the radar-IMU odometry factor of the lidar under the IMU constraint at the current sub-positioning.

[0104] In one embodiment, the above C2 can be implemented as follows: C2-1: Determine whether there is matching point cloud data similar to the target point cloud data corresponding to the current positioning based on the point cloud feature points corresponding to the current positioning and the point cloud feature points corresponding to each positioning before the current positioning.

[0105] Here, each positioning before the current positioning can be referred to as a historical positioning. The matching point cloud data is the point cloud feature points in the target point cloud data corresponding to the historical positioning, and the point cloud data whose similarity with the point cloud feature points corresponding to the current positioning meets a preset threshold.

[0106] In order to further improve the rationality of the determined radar posture, radar loop detection can be performed in each positioning except the first positioning. The specific steps of radar loop detection can be: determine the point cloud feature points corresponding to the current positioning, and the similarity of the point cloud feature points corresponding to each historical positioning. Determine whether there is a similarity that meets the preset threshold. If so, the target cloud point data under the historical positioning corresponding to the similarity can be used as the matching point cloud data, and then execute the following C2-2. If not, the radar odometer factor and the second IMU odometer factor can be directly used to determine the radar-IMU odometer factor.

[0107] C2-2: If yes, determine the radar IMU odometry factor under the IMU constraint based on the radar odometry factor corresponding to the current positioning, the radar IMU odometry factor corresponding to the matching point cloud data, and the second IMU odometry factor.

[0108] Here, since the matching point cloud data is the target point cloud data corresponding to a positioning before the current positioning, the positioning process corresponding to the matching point cloud data has been completed, and the radar-IMU odometry factor corresponding to the matching point cloud data has also been calculated.

[0109] For example, when performing radar loopback detection, if matching point cloud data is detected, it means that the radar pose of the robot when collecting the matching point cloud data is very similar to the radar pose when collecting the target point cloud data. In this case, the radar pose when collecting the matching point cloud data can be used to optimize the current radar pose. Specifically, regardless of whether there are one or more matching point cloud data, a factor graph model can be used to constrain and optimize the radar odometry factor based on the radar-IMU odometry factor corresponding to each matching point cloud data, the radar odometry factor determined during the current positioning, and the second IMU odometry factor determined during the current positioning to obtain the radar-IMU odometry factor for the current positioning.

[0110] Optionally, after optimizing the radar-IMU odometry factor for the current positioning, the radar-IMU odometry factor for the current positioning can be used as the radar-IMU odometry factor corresponding to the matching point cloud data, thereby optimizing the radar-IMU odometry factor corresponding to the matching point cloud data.

[0111] Optionally, during radar loopback detection, the point cloud feature points of the current positioning and the point cloud feature points of each historical positioning can be used to determine whether there are target point cloud feature points similar to the point cloud feature points of the current positioning. If so, the point cloud feature points of the current positioning, the target point cloud feature points, and the warehouse point cloud map constructed for the current positioning can be used to determine the radar odometry factor of the lidar at the current positioning. In this way, by combining point cloud feature points from multiple frames, the accuracy of the determined point cloud feature points can be improved.

[0112] like Figure 5 FIG. 1 is a schematic diagram of a process for determining a radar-IMU odometer factor according to an embodiment of the present disclosure, wherein: Indicates the target position of the robot in the i-th positioning, that is, Figure 5 The robot status in . 、 、……、 Represents the target point cloud data of the first, second, ... n+1th positioning, that is, Figure 5The radar keyframes in the radar frame can be extracted from the radar frame at a preset frame interval. For any radar keyframe under any positioning, the radar keyframe can be called a sub-keyframe in each positioning after that positioning. The IMU pre-integration factor is the IMU odometry factor. The laser-IMU subsystem receives 3D point cloud data and IMU sensor data from the laser radar input, and uses these data to estimate the state and trajectory of the robot. The laser-IMU subsystem consists of three constraints: IMU pre-integration constraints, laser radar odometry constraints, and loop detection constraints. The three constraints are added to the factor graph model and optimized together to obtain the results. The matching of the 3D point cloud data input by the laser radar and the constructed warehouse point cloud map constitutes the laser radar odometry constraint. The initial loop detection constraint is provided by the visual-inertial odometry mentioned earlier (that is, the initial loop detection is provided by the IMU odometry factor under the first positioning). In subsequent positioning, it can be input into the factor graph to scan and match again and further optimize. Figure 5 In, for , we can use the point cloud keyframes under the second positioning, the warehouse point cloud map corresponding to the second positioning, and the point cloud keyframes corresponding to the first positioning to determine the radar odometry factor under the second positioning, and then use the radar odometry factor under the second positioning and the second IMU data under the second positioning to determine the radar-IMU odometry factor under the second positioning (i.e. ).about 、 、…… The determination process, and Similar, no further description is given here.

[0113] In one embodiment, the above C2 may also be implemented according to the following steps: The second pre-integration error corresponding to the current sub-positioning is determined based on the second IMU odometry factor corresponding to the current sub-positioning and the second IMU odometry factors corresponding to each sub-positioning other than the current sub-positioning. The matching error corresponding to the current sub-positioning is determined based on the radar odometry factor corresponding to the current sub-positioning and the point cloud feature points corresponding to the current sub-positioning. The radar IMU odometry factor under the IMU constraint is determined with the goal of minimizing the sum of the second pre-integration error and the matching error.

[0114] Here, the second pre-integration error is used to characterize the deviation between the pre-integration measurement value from state k-1 to state k and the state variable. The determination process of the second pre-integration error is similar to the determination process of the first pre-integration error above and will not be repeated here.

[0115] The matching error is used to characterize the difference between the position of the point cloud feature point in the target point cloud data collected by the radar and the radar odometry factor predicted based on the state variable.

[0116] In one embodiment, the step of “extracting point cloud feature points from target point cloud data” in S102 may be performed as follows: D1 to D3: D1: Determine that the target point cloud data includes each point cloud line and each point cloud point in each point cloud line.

[0117] Because the laser radar used in the embodiments of the present disclosure can be a multi-line laser radar, which inherently carries line information (i.e., point cloud line information), the point cloud data collected by the laser radar includes not only the coordinate position information of the point cloud points themselves, but also the point cloud line from which the point cloud points originate, the order of the point cloud points on the same point cloud line, and so on. Therefore, after obtaining the target point cloud data, the target point cloud data can be determined to include each point cloud line and each point cloud point within each point cloud line based on the information carried by each point cloud point in the target point cloud data.

[0118] D2: For each point cloud line, determine the smoothness of each point cloud point in the point cloud line based on its coordinates in the radar coordinate system, and then determine the edge points and surface points in the point cloud line based on the smoothness.

[0119] In specific implementation, the smoothness of each point cloud point can be determined according to the following formula 2: ; (Formula 2) Among them, c represents smoothness, S represents the set of multiple point cloud points scanned by the laser radar in one scan, and i represents a point cloud point scanned by the laser radar in one scan; Represents the i-th point cloud point in the point cloud point set at the k-th scan in the radar coordinate system, Represents the point cloud points around the i-th point cloud point in the point cloud point set at the k-th scan in the radar coordinate system; represents the number of elements in the set S, The norm of , express and The norm of the difference.

[0120] That is, local smoothness can be understood as the sum and average of the ratio of the distance difference between the current point cloud point and the nearby point cloud points to the value of the current point cloud point. If a point cloud point is an edge point, then the distance difference between this point cloud point and the surrounding point cloud points will definitely be large. Combined with the fact that the lidar measurement returns distance, local smoothness can be used to measure whether a point cloud point is an edge point or a surface point.

[0121] For each point cloud line, after obtaining the smoothness of each point cloud point in the point cloud line, the point cloud point with the greatest smoothness can be used as the face point of the point cloud line, and the point cloud point with the least smoothness can be used as the edge point of the point cloud line. In this way, the face points and edge points of each point cloud line can be obtained.

[0122] D3: Use the edge points and surface points in each point cloud line as point cloud feature points.

[0123] In one embodiment, since the point cloud data collected by the LiDAR may be distorted by motion when the robot is in motion, the following steps may be used to correct the motion distortion before determining the feature points of the point cloud: The target IMU data corresponding to the current positioning is used to determine the motion trajectory of the lidar when collecting target point cloud data; based on the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0124] Here, because the LiDAR acquires information about the surrounding environment through physical rotation, the LiDAR itself moves or rotates during a scanning cycle, and the scanned point cloud data will be distorted due to the movement in this process and will not match the actual environment. The IMU device can provide continuous, high-frequency device motion information, including the device's linear velocity and acceleration, so that the precise motion trajectory of the LiDAR scanning process can be estimated. Based on this motion trajectory, motion compensation can be performed on each scanning point to offset the distortion effect caused by the LiDAR movement. LiDAR motion distortion removal with IMU assistance can effectively restore the real environment when the radar is moving at high speed.

[0125] In specific implementations, for the current positioning, the motion trajectory of the IMU device can be determined based on the target IMU data corresponding to the current positioning. This motion trajectory is then used as the motion trajectory of the lidar. This trajectory is then used to dedistort the coordinates of each point cloud point in the target point cloud data collected by the lidar during the current positioning, thereby achieving motion compensation for the point cloud points and obtaining the dedistorted coordinates. The dedistorted coordinates can then be used to determine the smoothness of the point cloud points, and the smoothness can be used to screen feature points in the point cloud.

[0126] like Figure 6As shown, a comparison diagram of point clouds before and after motion compensation provided by an embodiment of the present disclosure is shown, wherein Figure 6-1 represents the detection object when the laser radar is working normally, that is, the vertical wall of the warehouse in the box in Figure 6-1, and the green point cloud in Figure 6-2 is the point cloud data output by the laser radar after the laser radar detects the vertical wall of the warehouse in Figure 6-1. The point cloud after motion distortion correction assisted by the IMU is the red point cloud in Figure 6-2, and Figure 6-3 is an amplification of a part of the area in Figure 6-2. The red point cloud can better restore the real situation. It provides a high-quality data foundation for the subsequent extraction of point cloud feature points, map construction, and path planning.

[0127] In one embodiment, after obtaining the target posture of the robot at each positioning, the movement trajectory of the robot in the warehouse can be drawn according to the target posture obtained at each positioning. Figure 7-1 As shown, a top view comparison diagram of a trajectory map obtained by using the positioning method of the embodiment of the present disclosure and using the mapping method in the prior art is provided, as shown in FIG. Figure 7-2 As shown, a side view comparison diagram of a trajectory map obtained by using the positioning method of the embodiment of the present disclosure and using the mapping method in the prior art is provided, as shown in FIG. Figure 7-3 As shown, a trajectory comparison diagram corresponding to the XYZ axes of the trajectory map obtained by using the positioning method of the present invention and the trajectory map obtained by using the mapping method in the prior art is provided for the embodiment of the present invention. The prior art mapping methods include the VINS-Mono (visual-inertial measurement unit odometer) mapping method and the LIO-SAM (lidar-inertial measurement unit odometer) mapping method. Figure 7-1 to Figure 7-3 In the figure, the blue trajectory represents the real trajectory of the robot, which can be obtained by manual annotation. The green trajectory represents the trajectory obtained by using the positioning method of the embodiment of the present disclosure to build the trajectory map. The orange trajectory represents the trajectory built by the LIO-SAM method in the prior art. The purple trajectory represents the trajectory built by the VINS-Mono method in the prior art. Figure 7-1 In the figure, the green track and the blue track are completely overlapped, so only the green track is shown. The orange track and the purple track are different from the blue track. Figure 7-1 and 7-2 In the figure, the green track completely overlaps with the blue track, so only the green track is shown. The orange track and the purple track are different from the blue track. Figure 7-3 In the figure, on the X-axis and Y-axis, the green track, the orange track and the blue track overlap, so the track is displayed in brown; on the Z-axis, the green track and the blue track completely overlap.

[0128] based on Figure 7-1 to Figure 7-3 It can be seen that under the positioning method provided by the embodiment of the present disclosure, the mapping trajectory and the real trajectory can maintain a smaller trajectory error and the mapping quality is better, especially in the z-axis direction, which can better demonstrate the advantages of the positioning method provided by the embodiment of the present disclosure.

[0129] In one embodiment, for the current sub-positioning, after determining the radar IMU odometry factor under the IMU constraint, the point cloud feature points corresponding to the current sub-positioning can be filtered according to the radar IMU odometry factor corresponding to the current sub-positioning to obtain the filtered feature points corresponding to the current sub-positioning; based on the filtered feature points of the current sub-positioning, the constructed warehouse point cloud map is supplemented to obtain the supplemented warehouse point cloud map.

[0130] For example, for the current positioning, when matching the point cloud feature points corresponding to the current positioning using the constructed warehouse point cloud map corresponding to the current positioning, the point cloud feature points corresponding to the current positioning can be screened. Furthermore, when optimizing the radar pose using the matching error and the second pre-integrated error, the point cloud feature points corresponding to the current positioning can also be screened. Thus, the point cloud feature points retained after the two screenings can be used as screening feature points. Alternatively, the degree of matching between each point cloud feature point corresponding to the current positioning and the radar-IMU odometry factor corresponding to the current positioning can be used to screen feature points from the point cloud feature points corresponding to the current positioning, with the matching degree meeting the requirements. The screened feature points of the current positioning can then be used to update and supplement the feature points of the warehouse point cloud map constructed before the current positioning, obtaining a supplemented warehouse point cloud map. This supplemented warehouse point cloud map can be considered the warehouse point cloud map constructed for the current positioning.

[0131] Furthermore, after obtaining the supplemented warehouse point cloud map each time positioning is performed, the supplemented warehouse point cloud map corresponding to the last positioning can be used as the global point cloud map of the warehouse in response to the completion of positioning.

[0132] Here, if the robot is tasked with mapping the warehouse, it will move around the entire warehouse, using the positioning algorithm provided by the disclosed embodiments to continuously locate the robot during this movement. The warehouse point cloud map will also be updated and improved after each positioning. After positioning is completed, the robot has completed its movement around the entire warehouse. At this point, the updated warehouse point cloud map corresponding to the last positioning can be used as the global point cloud map of the warehouse.

[0133] Optionally, after each positioning, the feature points filtered out from the target point cloud data collected by each positioning using the positioning algorithm can be stored, for example, in a point map file format. Then, when a global map of the warehouse needs to be constructed later, the stored point cloud files of each positioning can be used to construct a global point cloud map of the warehouse. Figure 7-4 As shown, it is a schematic diagram of a global point cloud map constructed using the positioning algorithm provided by the embodiment of the present disclosure, as shown in Figure 7-5 The figure shows a schematic diagram of a global point cloud map constructed using the laser-IMU mapping algorithm in the prior art. Figure 7-4 and Figure 7-5 It can be seen from the comparison that compared with the laser-IMU mapping algorithm in the prior art, the point cloud map constructed by the positioning algorithm provided by the embodiment of the present disclosure can better restore the real scene of the warehouse, contain more detailed and accurate point cloud coordinates, and the robustness of the mapping is also better. Figure 7-5 compared to Figure 7-4 In terms of the image quality, it can be intuitively seen that the laser-IMU mapping algorithm fails to accurately capture the structural details of the actual scene in the upper right corner, resulting in an offset. However, the positioning algorithm provided by the embodiments of the present disclosure does not have this problem. Therefore, it can be seen that the positioning algorithm provided by the embodiments of the present disclosure can accurately reconstruct the actual three-dimensional shape of the warehouse when processing large scene mapping due to the small offset in the Z-axis direction.

[0134] like Figure 8 The figure shows a schematic diagram of the system framework of a posture determination system provided by an embodiment of the present disclosure, in which the camera-IMU odometry factor, the radar-IMU odometry factor, and the IMU odometry factor are determined. For the camera, image feature points can be extracted from the image captured by the camera, and depth registration is performed using the depth values ​​of the point cloud points to obtain depth feature points. At the same time, feature tracking can also be performed, wherein the feature tracking steps can refer to B1-1 and B1-2 above.

[0135] The point cloud data used for depth registration can be obtained by dedistorting the point cloud data collected by the LiDAR. Feature extraction can also be performed on the dedistorted point cloud data to obtain point cloud feature points (i.e., edge points and surface points). Feature matching can then be performed on these point cloud feature points. The feature matching steps can be referred to in C2-1 and C2-2 above. Figure 8The loop closure detection steps in [ 1 ] can refer to the descriptions of camera loop closure detection and radar loop closure detection mentioned above. The initialization step is to use the IMU odometry factor corresponding to the first positioning as the camera-IMU odometry factor and radar-IMU odometry factor corresponding to the first positioning during the initial positioning. The IMU measurement value is the IMU data. Radar-assisted visual odometry uses depth feature points and the IMU odometry factor (obtained by pre-integration based on the IMU data) to determine the camera IMU odometry factor. Multi-sensor factor graph optimization uses a factor graph model to determine the robot's IMU pose in the world coordinate system based on the IMU odometry factor, the camera IMU odometry factor, and the radar IMU odometry factor. Then, based on the conversion relationship between the world coordinate system and the IMU coordinate system, the IMU pose can be converted to the robot's target pose in the world coordinate system.

[0136] Those skilled in the art will understand that in the above-mentioned method of the specific implementation method, the writing order of each step does not mean a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of each step should be determined by its function and possible internal logic.

[0137] Based on the same inventive concept, a positioning device corresponding to the positioning method is also provided in the embodiment of the present disclosure. Since the principle of solving the problem by the device in the embodiment of the present disclosure is similar to the above-mentioned positioning method in the embodiment of the present disclosure, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be repeated.

[0138] like Figure 9 FIG. 1 is a schematic diagram of a positioning device provided by an embodiment of the present disclosure, comprising: The acquisition module 901 is used to acquire the target image, target point cloud data and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency during the movement of the robot in the warehouse; An extraction module 902 is configured to extract image feature points from the target image, assign depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extract point cloud feature points from the target point cloud data; A first determination module 903 is configured to determine an IMU odometry factor based on the target IMU data, and determine a camera IMU odometry factor under IMU constraints based on the depth feature points and the IMU odometry factor, and determine a radar IMU odometry factor under IMU constraints based on the point cloud feature points and the IMU odometry factor; The second determination module 904 is configured to determine the target pose of the robot in the world coordinate system according to the IMU odometry factor, the camera IMU odometry factor, and the radar IMU odometry factor.

[0139] In a possible implementation, the acquisition module 901, when acquiring the target image, target point cloud data, and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency, is configured to: When the robot moves in the warehouse, each frame of initial image captured by the robot's camera at the image acquisition frequency, each piece of initial IMU data captured by the robot's IMU device at the IMU acquisition frequency, and each frame of initial point cloud data captured by the robot's lidar at the point cloud acquisition frequency are acquired; the image acquisition frequency is lower than the IMU acquisition frequency, and the point cloud acquisition frequency is lower than the image acquisition frequency; the positioning frequency is higher than the point cloud acquisition frequency and lower than the image acquisition frequency; When it is determined that it is not the first positioning based on the positioning frequency, the target image frames between the current positioning and the previous positioning are determined from the initial image, and the target IMU data between the current positioning and the previous positioning are determined from the initial IMU data, and the target point cloud data collected before the current positioning is determined from the initial point cloud data.

[0140] In a possible implementation, the acquisition module 901 is further configured to: When the first positioning is determined according to the positioning frequency, the target IMU data required for the first positioning is obtained from the initial IMU data; The IMU odometry factor corresponding to the first positioning is determined according to the target IMU data, and the IMU odometry factor corresponding to the first positioning is used as the camera IMU odometry factor and radar IMU odometry factor for the first positioning.

[0141] In a possible implementation, the extraction module 902, when extracting image feature points from the target image and assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, is configured to: Determine, from each frame of the target image, an image to be projected whose acquisition time is closest to the target point cloud data, and extract image feature points from the image to be projected; Reprojecting each point cloud point in the target point cloud data onto the image to be projected to obtain each projection feature point; According to the depth information and position of each projection feature point, a depth value is assigned to each image feature point in the image to be projected to obtain each depth feature point.

[0142] In one possible implementation, the IMU odometry factor includes a first IMU odometry factor obtained by pre-integrating the IMU data to be used; the IMU data to be used is target IMU data whose acquisition time is between the acquisition time of the image to be projected and the acquisition time of an adjacent target image of the image to be projected; The first determining module 903, when determining the camera IMU odometry factor under the IMU constraint based on the depth feature point and the IMU odometry factor, is configured to: Extracting image feature points from an adjacent target image of the image to be projected, and determining a camera odometry factor of the camera under current positioning according to the depth feature points of the image to be projected and the image feature points of the adjacent target image; Determine a camera IMU odometry factor under IMU constraints according to the first IMU odometry factor and the camera odometry factor.

[0143] In a possible implementation, the first determining module 903, when determining the camera IMU odometry factor under IMU constraints based on the first IMU odometry factor and the camera odometry factor, is configured to: Determining whether there is a target image to be projected similar to the image to be projected corresponding to the current positioning based on the image feature points of the image to be projected corresponding to the current positioning and the image feature points of the images to be projected corresponding to the positionings before the current positioning; If so, the camera IMU odometry factor of the camera at the current positioning is determined according to the camera IMU odometry factor corresponding to the target image to be projected, the camera odometry factor at the current positioning, and the first IMU odometry factor.

[0144] In a possible implementation, the first determining module 903, when determining the camera odometry factor of the camera in the current positioning according to the depth feature points of the image to be projected and the image feature points of the adjacent target image, is configured to: In the target image, if there are other images other than the image to be projected and the adjacent target image, determining similarities between image feature points of the other images and image feature points of the image to be projected; When the similarity satisfies a preset similarity, a camera odometry factor of the camera under the current positioning is determined according to the image feature points of the other images, the depth feature points of the image to be projected, and the image feature points of the adjacent target image.

[0145] In a possible implementation, the first determining module 903, when determining the camera IMU odometry factor under IMU constraints based on the first IMU odometry factor and the camera odometry factor, is configured to: Determine a first pre-integration error corresponding to the current positioning according to a first IMU odometry factor corresponding to the current positioning and first IMU odometry factors corresponding to each positioning other than the current positioning; Determining a reprojection error corresponding to a current positioning according to each image feature point in the image to be projected and the camera odometry factor; The camera IMU odometry factor under the IMU constraint is determined with the goal of minimizing the sum of the first pre-integration error and the reprojection error.

[0146] In one possible implementation, the IMU odometry factor includes a second IMU odometry factor obtained by pre-integrating the target IMU data; The first determining module 903, when determining the radar IMU odometry factor under the IMU constraint based on the point cloud feature points and the IMU odometry factor, is configured to: Determining a radar odometry factor of the lidar based on the point cloud feature points corresponding to the current positioning and the constructed warehouse point cloud map corresponding to the current positioning; Determine a radar IMU odometry factor under IMU constraints according to the radar odometry factor and the second IMU odometry factor.

[0147] In a possible implementation, the first determining module 903, when determining the radar IMU odometry factor under IMU constraints based on the radar odometry factor and the second IMU odometry factor, is configured to: Determine whether there is matching point cloud data similar to the target point cloud data corresponding to the current positioning based on the point cloud feature points corresponding to the current positioning and the point cloud feature points corresponding to each positioning before the current positioning; If so, determine the radar IMU odometry factor under the IMU constraint according to the radar odometry factor corresponding to the current positioning, the radar IMU odometry factor corresponding to the matching point cloud data, and the second IMU odometry factor.

[0148] In a possible implementation, the first determining module 903, when determining the radar IMU odometry factor under IMU constraints based on the radar odometry factor and the second IMU odometry factor, is configured to: Determine a second pre-integration error corresponding to the current positioning according to the second IMU odometry factor corresponding to the current positioning and the second IMU odometry factors corresponding to each positioning other than the current positioning; Determine the matching error corresponding to the current sub-positioning based on the radar odometry factor corresponding to the current sub-positioning and the point cloud feature points corresponding to the current sub-positioning; With the goal of minimizing the sum of the second pre-integration error and the matching error, a radar IMU odometry factor under IMU constraints is determined.

[0149] In a possible implementation, the device further includes: Mapping module 905, after determining the radar IMU odometry factor under the IMU constraints, is used to: According to the radar IMU odometry factor corresponding to the current positioning, the point cloud feature points corresponding to the current positioning are filtered to obtain the filtered feature points corresponding to the current positioning; According to the filtered feature points of the current positioning, the constructed warehouse point cloud map is supplemented to obtain the supplemented warehouse point cloud map; In response to the completion of positioning, the supplemented warehouse point cloud map corresponding to the last positioning is used as the global point cloud map of the warehouse.

[0150] In a possible implementation, the extraction module 902, when extracting point cloud feature points from the target point cloud data, is configured to: Determine that the target point cloud data includes each point cloud line and each point cloud point in the point cloud line; For each of the point cloud lines, determining the smoothness of each point cloud point in the point cloud line according to the coordinates of each point cloud point in the radar coordinate system, and determining the edge points and surface points in the point cloud line according to the smoothness; The edge points and surface points in each of the point cloud lines are used as the point cloud feature points.

[0151] In a possible implementation, the extraction module 902, before determining that the point cloud feature points include each point cloud line and each point cloud point in the point cloud line, is further configured to: Using the target IMU data corresponding to the current positioning, determine the motion trajectory of the laser radar when collecting the target point cloud data; According to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0152] In a possible implementation, the device further includes: The drawing module 906, after determining the target pose of the robot in the world coordinate system, is used to: According to the target posture obtained in each positioning, the movement trajectory of the robot in the warehouse is drawn.

[0153] For descriptions of the processing flow of each module in the device and the interaction flow between each module, reference can be made to the relevant descriptions in the above method embodiment, which will not be described in detail here.

[0154] Based on the same technical concept, the embodiment of the present application also provides a computer device. Figure 10 FIG. 1 is a schematic diagram of a computer device according to an embodiment of the present invention, comprising: Processor 1001, memory 1002, and bus 1003. Memory 1002 stores machine-readable instructions executable by processor 1001. Processor 1001 is configured to execute the machine-readable instructions stored in memory 1002. When the machine-readable instructions are executed by processor 1001, processor 1001 executes steps S101 to S104.

[0155] The above-mentioned memory 1002 includes internal memory 1021 and external memory 1022; the memory 1021 here is also called internal memory, which is used to temporarily store the calculation data in the processor 1001, as well as the data exchanged with the external memory 1022 such as the hard disk. The processor 1001 exchanges data with the external memory 1022 through the memory 1021. When the computer device is running, the processor 1001 and the memory 1002 communicate through the bus 1003, so that the processor 1001 executes the execution instructions mentioned in the above method embodiment.

[0156] The present disclosure also provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the computer program executes the steps of the positioning method described in the above method embodiment. The storage medium can be a volatile or non-volatile computer-readable storage medium.

[0157] The computer program product of the positioning method provided in the embodiments of the present disclosure includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the steps of the positioning method described in the above method embodiments. For details, please refer to the above method embodiments and will not be repeated here.

[0158] The computer program product may be implemented in hardware, software, or a combination thereof. In one embodiment, the computer program product is implemented as a computer storage medium. In another embodiment, the computer program product is implemented as a software product, such as a software development kit (SDK).

[0159] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the device described above can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here. In the several embodiments provided in the present disclosure, it should be understood that the disclosed device and method can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some communication interfaces, and the indirect coupling or communication connection of the device or unit can be electrical, mechanical or other forms.

[0160] The units described as separate components may or may not be physically separate, and 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 these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0161] In addition, each functional unit in each embodiment of the present disclosure may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0162] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium that is executable by a processor. Based on this understanding, the technical solution of the present disclosure, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present disclosure. The aforementioned storage medium includes: various media that can store program code, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0163] If the technical solution of this application involves personal information, the product that applies the technical solution of this application has clearly informed the personal information processing rules and obtained the individual's voluntary consent before processing personal information. If the technical solution of this application involves sensitive personal information, the product that applies the technical solution of this application has obtained the individual's separate consent before processing sensitive personal information, and at the same time meets the "explicit consent" requirement. For example, on personal information collection devices such as cameras, a clear and prominent sign is set to inform that the personal information collection scope has been entered and personal information will be collected. If the individual voluntarily enters the collection scope, it is deemed that they agree to the collection of their personal information; or on the personal information processing device, when the personal information processing rules are notified by obvious signs / information, the individual's authorization is obtained through pop-up information or by asking the individual to upload their personal information; among which, the personal information processing rules may include information such as the personal information processor, the purpose of personal information processing, the processing method, and the type of personal information processed.

[0164] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present disclosure, which are used to illustrate the technical solutions of the present disclosure, rather than to limit them. The scope of protection of the present disclosure is not limited thereto. Although the present disclosure has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that any person skilled in the art can modify or easily conceive of changes to the technical solutions described in the above-mentioned embodiments within the technical scope disclosed in the present disclosure, or replace some of the technical features therein with equivalents. Such modifications, changes, or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present disclosure, and should be included in the scope of protection of the present disclosure. Therefore, the scope of protection of the present disclosure should be based on the scope of protection of the claims.

Claims

1. A positioning method, characterized in that: include: When the robot moves in the warehouse, it obtains the target image, target point cloud data and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency; Extracting image feature points from the target image, assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extracting point cloud feature points from the target point cloud data; Determine an IMU odometry factor according to the target IMU data, determine a camera IMU odometry factor under IMU constraints according to the depth feature points and the IMU odometry factor, and determine a radar IMU odometry factor under IMU constraints according to the point cloud feature points and the IMU odometry factor; The target position and posture of the robot in a world coordinate system are determined according to the IMU odometer factor, the camera IMU odometer factor, and the radar IMU odometer factor.

2. The method according to claim 1, characterized in that The target image, target point cloud data and target inertial measurement unit (IMU) data required for each positioning are obtained according to the positioning frequency, including: When the robot moves in the warehouse, each frame of initial image collected by the camera on the robot at the image collection frequency, each piece of initial IMU data collected by the IMU device on the robot at the IMU collection frequency, and each frame of initial point cloud data collected by the laser radar on the robot at the point cloud collection frequency are obtained; the image collection frequency is lower than the IMU collection frequency, and the point cloud collection frequency is lower than the image collection frequency; the positioning frequency is higher than the point cloud collection frequency and lower than the image collection frequency; When it is determined that it is not the first positioning according to the positioning frequency, the target image frames between the current positioning and the previous positioning are determined from the initial image, and the target IMU data between the current positioning and the previous positioning are determined from the initial IMU data, and the target point cloud data collected before the current positioning is determined from the initial point cloud data.

3. The method according to claim 2, characterized in that The method further comprises: When the first positioning is determined according to the positioning frequency, the target IMU data required for the first positioning is obtained from the initial IMU data; The IMU odometer factor corresponding to the first positioning is determined according to the target IMU data, and the IMU odometer factor corresponding to the first positioning is used as the camera IMU odometer factor and radar IMU odometer factor of the first positioning.

4. The method according to claim 2, characterized in that The step of extracting image feature points from the target image and assigning depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points includes: From each frame of the target image, determine the image to be projected whose acquisition time is closest to the target point cloud data, and extract image feature points from the image to be projected; Reprojecting each point cloud point in the target point cloud data to the image to be projected to obtain each projection feature point; According to the depth information and position of each projection feature point, a depth value is assigned to each image feature point in the image to be projected to obtain each depth feature point.

5. The method according to claim 4, characterized in that The IMU odometer factor includes a first IMU odometer factor obtained by pre-integrating the IMU data to be used; the IMU data to be used is target IMU data whose acquisition time is between the acquisition time of the image to be projected and the acquisition time of the adjacent target image of the image to be projected; Determining a camera IMU odometry factor under an IMU constraint according to the depth feature point and the IMU odometry factor includes: Extracting image feature points from an adjacent target image of the image to be projected, and determining a camera odometer factor of the camera under current positioning according to the depth feature points of the image to be projected and the image feature points of the adjacent target image; A camera IMU odometry factor under IMU constraints is determined according to the first IMU odometry factor and the camera odometry factor.

6. The method according to claim 5, characterized in that The determining, according to the first IMU odometer factor and the camera odometer factor, a camera IMU odometer factor under the IMU constraint includes: Determine whether there is a target image to be projected similar to the image to be projected corresponding to the current positioning according to the image feature points of the image to be projected corresponding to the current positioning and the image feature points of the images to be projected corresponding to the positionings before the current positioning; If so, the camera IMU odometer factor of the camera at the current positioning is determined according to the camera IMU odometer factor corresponding to the target image to be projected, the camera odometer factor at the current positioning, and the first IMU odometer factor.

7. The method according to claim 5 or 6, characterized in that: The determining, according to the depth feature points of the image to be projected and the image feature points of the adjacent target image, a camera odometer factor of the camera under current positioning includes: In the target image, when there are other images other than the image to be projected and the adjacent target image, determining the similarity between the image feature points of the other images and the image feature points of the image to be projected; When the similarity satisfies a preset similarity, a camera odometer factor of the camera under the current positioning is determined according to the image feature points of the other images, the depth feature points of the image to be projected, and the image feature points of the adjacent target image.

8. The method according to claim 5, characterized in that The determining, according to the first IMU odometer factor and the camera odometer factor, a camera IMU odometer factor under the IMU constraint includes: Determine a first pre-integration error corresponding to the current positioning according to a first IMU odometer factor corresponding to the current positioning and first IMU odometer factors corresponding to each positioning other than the current positioning; Determine a reprojection error corresponding to a current positioning according to each image feature point in the image to be projected and the camera odometer factor; The camera IMU odometer factor under the IMU constraint is determined with the goal of minimizing the sum of the first pre-integration error and the reprojection error.

9. The method according to claim 2, characterized in that: The IMU odometer factor includes a second IMU odometer factor obtained by pre-integrating the target IMU data; The step of determining the radar IMU odometer factor under the IMU constraint according to the point cloud feature point and the IMU odometer factor includes: Determine the radar odometer factor of the laser radar according to the point cloud feature points corresponding to the current positioning and the constructed warehouse point cloud map corresponding to the current positioning; A radar IMU odometry factor under IMU constraints is determined according to the radar odometry factor and the second IMU odometry factor.

10. The method according to claim 9, characterized in that The step of determining the radar IMU odometer factor under the IMU constraint according to the radar odometer factor and the second IMU odometer factor includes: Determine whether there is matching point cloud data similar to the target point cloud data corresponding to the current positioning according to the point cloud feature points corresponding to the current positioning and the point cloud feature points corresponding to each positioning before the current positioning; If so, determine the radar IMU odometer factor under the IMU constraint according to the radar odometer factor corresponding to the current positioning, the radar IMU odometer factor corresponding to the matching point cloud data, and the second IMU odometer factor.

11. The method according to claim 9 or 10, characterized in that: The step of determining the radar IMU odometer factor under the IMU constraint according to the radar odometer factor and the second IMU odometer factor includes: Determine a second pre-integration error corresponding to the current positioning according to the second IMU odometer factor corresponding to the current positioning and the second IMU odometer factors corresponding to each positioning other than the current positioning; Determine the matching error corresponding to the current sub-positioning according to the radar odometer factor corresponding to the current sub-positioning and the point cloud feature point corresponding to the current sub-positioning; Taking the sum of the second pre-integration error and the matching error as the minimum as the goal, the radar IMU odometer factor under the IMU constraint is determined.

12. The method according to claim 9, characterized in that After determining the radar IMU odometry factor under the IMU constraints, it also includes: According to the radar IMU odometer factor corresponding to the current positioning, the point cloud feature points corresponding to the current positioning are screened to obtain the screened feature points corresponding to the current positioning; According to the selected feature points of the current positioning, the constructed warehouse point cloud map is supplemented to obtain a supplemented warehouse point cloud map; In response to the end of positioning, the supplemented warehouse point cloud map corresponding to the last positioning is used as the global point cloud map of the warehouse.

13. The method according to claim 1, characterized in that The step of extracting point cloud feature points from the target point cloud data comprises: Determine that the target point cloud data includes each point cloud line and each point cloud point in the point cloud line; For each of the point cloud lines, determine the smoothness of each point cloud point in the point cloud line according to the coordinates of each point cloud point in the point cloud line in the radar coordinate system, and determine the edge points and surface points in the point cloud line according to the smoothness; The edge points and surface points in each of the point cloud lines are used as the point cloud feature points.

14. The method according to claim 13, characterized in that Before determining that the point cloud feature points include each point cloud line and each point cloud point in the point cloud line, the method further includes: Using the target IMU data corresponding to the current positioning, determine the motion trajectory of the laser radar when collecting the target point cloud data; According to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

15. The method according to claim 1, characterized in that After determining the target position and posture of the robot in the world coordinate system, the method further includes: According to the target posture obtained in each positioning, the movement trajectory of the robot in the warehouse is drawn.

16. A positioning device, characterized in that: include: An acquisition module is used to acquire the target image, target point cloud data and target inertial measurement unit IMU data required for each positioning according to the positioning frequency during the movement of the robot in the warehouse; An extraction module, used to extract image feature points from the target image, assign depth values ​​of point cloud points in the target point cloud data to the image feature points to obtain depth feature points, and extract point cloud feature points from the target point cloud data; A first determination module is used to determine an IMU odometry factor according to the target IMU data, and to determine a camera IMU odometry factor under the IMU constraint according to the depth feature point and the IMU odometry factor, and to determine a radar IMU odometry factor under the IMU constraint according to the point cloud feature point and the IMU odometry factor; The second determination module is used to determine the target posture of the robot in the world coordinate system according to the IMU odometer factor, the camera IMU odometer factor and the radar IMU odometer factor.

17. A computer device, characterized in that: include: A processor and a memory, wherein the memory stores machine-readable instructions executable by the processor, and the processor is used to execute the machine-readable instructions stored in the memory. When the machine-readable instructions are executed by the processor, the processor executes the steps of the positioning method according to any one of claims 1 to 15.

18. A computer program product, characterized in that When the computer program product is executed by a computer device, the computer device executes the steps of the positioning method according to any one of claims 1 to 15.

Images