A positioning method, device, computing equipment and program product

Abstract

The present disclosure provides a positioning method, device, computing equipment and program product applied to the field of robot technology, wherein the method includes: obtaining a target image, target point cloud data and target inertial measurement unit IMU data according to the positioning frequency; extracting image feature points from the target image, and assigning the depth value of the point cloud point in the target point cloud data to the image feature point to obtain the depth feature point, and extracting the point cloud feature point from the target point cloud data; determining the IMU odometer factor according to the target IMU data, and determining the camera IMU odometer factor according to the depth feature point and the IMU odometer factor, and determining the radar IMU odometer factor according to the point cloud feature point and the IMU odometer factor; determining the target posture of the robot according to the IMU odometer factor, the camera IMU odometer factor and the radar IMU odometer factor. The present disclosure realizes positioning by combining multi-sensor data for mutual constraint, which can solve the problem of inaccurate robot positioning.

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] As an important process in logistics management, intelligent warehousing can effectively improve the efficiency of cargo management through the rational use of intelligent warehousing. In intelligent warehousing, cargo inventory is an important task that requires the use of various inventory robots to perform the task. However, when robots are currently used to perform inventory tasks, there is a problem of inaccurate robot positioning, which affects the efficiency and accuracy of cargo inventory. 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:

[0005] 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;

[0006] 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;

[0007] 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;

[0008] 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.

[0009] This implementation method 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, and the pose of the robot in the IMU coordinate system can be accurately converted to the world coordinate system to obtain the accurate pose of the robot in the world coordinate system. The entire positioning process can effectively improve the positioning accuracy of the robot by combining data from multiple sensors and using these data to constrain each other to achieve positioning.

[0010] In a possible implementation, obtaining the target image, target point cloud data, and target inertial measurement unit (IMU) data required for each positioning according to the positioning frequency includes:

[0011] 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;

[0012] 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.

[0013] In a possible implementation, the method further includes:

[0014] 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;

[0015] 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.

[0016] 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:

[0017] 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;

[0018] Reprojecting each point cloud point in the target point cloud data to the image to be projected to obtain each projection feature point;

[0019] 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.

[0020] In a possible implementation, 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 an adjacent target image of the image to be projected;

[0021] Determining a camera IMU odometry factor under an IMU constraint according to the depth feature point and the IMU odometry factor includes:

[0022] 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;

[0023] A camera IMU odometry factor under IMU constraints is determined according to the first IMU odometry factor and the camera odometry factor.

[0024] In a possible implementation, determining the camera IMU odometer factor under the IMU constraint according to the first IMU odometer factor and the camera odometer factor includes:

[0025] 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;

[0026] 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.

[0027] In a possible implementation, determining the camera odometer factor of the camera under 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:

[0028] 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;

[0029] 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.

[0030] In a possible implementation, determining the camera IMU odometer factor under the IMU constraint according to the first IMU odometer factor and the camera odometer factor includes:

[0031] Determine a first pre-integrated 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;

[0032] 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;

[0033] 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.

[0034] In a possible implementation, the IMU odometer factor includes a second IMU odometer factor obtained by pre-integrating the target IMU data;

[0035] 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:

[0036] 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;

[0037] A radar IMU odometry factor under IMU constraints is determined according to the radar odometry factor and the second IMU odometry factor.

[0038] In a possible implementation, determining the radar IMU odometer factor under IMU constraints according to the radar odometer factor and the second IMU odometer factor includes:

[0039] 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;

[0040] 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.

[0041] In a possible implementation, determining the radar IMU odometer factor under IMU constraints according to the radar odometer factor and the second IMU odometer factor includes:

[0042] 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;

[0043] 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;

[0044] 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.

[0045] In a possible implementation, after determining the radar IMU odometer factor under the IMU constraint, the method further includes:

[0046] 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;

[0047] 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;

[0048] 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.

[0049] In a possible implementation, extracting point cloud feature points from the target point cloud data includes:

[0050] Determine that the target point cloud data includes each point cloud line and each point cloud point in the point cloud line;

[0051] 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;

[0052] The edge points and surface points in each of the point cloud lines are used as the point cloud feature points.

[0053] In a possible implementation manner, 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:

[0054] 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;

[0055] According to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0056] In a possible implementation manner, after determining the target position and posture of the robot in the world coordinate system, the method further includes:

[0057] According to the target posture obtained in each positioning, the movement trajectory of the robot in the warehouse is drawn.

[0058] In a second aspect, an embodiment of the present disclosure further provides a positioning device, including:

[0059] 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;

[0060] 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;

[0061] 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;

[0062] 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.

[0063] 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.

[0064] 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 executed, the steps of the above-mentioned first aspect, or any possible implementation of the first aspect are executed.

[0065] 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.

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

[0067] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the following is a brief introduction to the drawings required for use in the embodiments. The drawings herein are incorporated into the specification and constitute a part of the specification. These drawings illustrate embodiments consistent with the present disclosure and are used together with the specification 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 ordinary technicians in this field, other relevant drawings can also be obtained based on these drawings without creative work.

[0068] Figure 1 A flowchart of a positioning method provided by an embodiment of the present disclosure is shown;

[0069] Figure 2 A schematic diagram of a process for extracting image feature points provided by an embodiment of the present disclosure is shown;

[0070] Figure 3 A schematic diagram of a reprojection process provided by an embodiment of the present disclosure is shown;

[0071] Figure 4 A schematic diagram of a process for determining a camera-IMU odometer factor provided by an embodiment of the present disclosure is shown;

[0072] Figure 5 A schematic diagram of a process for determining a radar-IMU odometer factor provided by an embodiment of the present disclosure is shown;

[0073] Figure 6 A point cloud comparison diagram before and after motion compensation provided by an embodiment of the present disclosure is shown;

[0074] Figure 7-1 A top view comparison diagram of a trajectory map obtained by using a positioning method provided by an embodiment of the present disclosure and a trajectory map obtained by using a mapping method in the prior art is shown;

[0075] Figure 7-2 A side view comparison diagram of a trajectory map obtained by using a positioning method provided by an embodiment of the present disclosure and a trajectory map obtained by using a mapping method in the prior art is shown;

[0076] Figure 7-3 A trajectory comparison diagram corresponding to the XYZ axes respectively is shown, which is a trajectory map obtained by using the positioning method of the embodiment of the present disclosure and the trajectory map obtained by using the mapping method in the prior art;

[0077] Figure 7-4 A schematic diagram of a global point cloud map constructed using a positioning algorithm provided by an embodiment of the present disclosure is shown;

[0078] 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;

[0079] Figure 8 A schematic diagram of a system framework of a posture determination system provided by an embodiment of the present disclosure is shown;

[0080] Fig. 9 A schematic diagram of a positioning device provided by an embodiment of the present disclosure is shown;

[0081] Fig.10 A schematic diagram of the structure of a computer device provided by an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0082] In order to make the purpose, technical scheme and advantages of the embodiments of the present disclosure clearer, the technical scheme 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 claimed 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 belong to the scope of protection of the present disclosure.

[0083] In addition, the terms "first", "second", etc. in the specification 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 several" mentioned in this article refers to two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may 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.

[0084] Research has found that when a robot performs a picking task or inventory task in a warehouse, it is necessary to locate the robot in the warehouse in real time and plan a moving path for the robot based on the robot's real-time positioning, thereby improving the robot's task execution efficiency. However, conventional robot positioning methods have the problem of low positioning accuracy, which affects the robot's task execution efficiency and has significant disadvantages.

[0085] Based on the above research, the present disclosure provides a positioning method, device, computing device 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, so that the robot's pose in the IMU coordinate system can be accurately converted to the world coordinate system, and the accurate pose of the robot in the world coordinate system can be obtained. The entire positioning process can effectively improve the positioning accuracy of the robot by combining data from multiple sensors and using these data to constrain each other to achieve positioning.

[0086] The defects existing in the above solutions are the results obtained by the inventor after practice and careful research. Therefore, the discovery process of the above problems and the solutions proposed by the present disclosure for the above problems below should be the contributions made by the inventor to the present disclosure during the disclosure process.

[0087] 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, further definition and explanation thereof is not required in subsequent drawings.

[0088] It should be noted that the specific terms mentioned in the embodiments of the present disclosure include:

[0089] The nonlinear optimization library: Georgia Tech Smoothing and Mapping, or GTSAM for short, is widely used in localization and mapping problems for robots and autonomous vehicles. It contains a series of efficient algorithms for processing sensor data and map information to estimate the path and surrounding environment of a robot or vehicle. In addition, GTSAM can also solve more complex estimation problems and is suitable for a variety of scenarios that require accurate estimation and optimization.

[0090] 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, wherein the terminal device may 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 may be implemented by a processor calling computer-readable instructions stored in a memory.

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

[0092] like Figure 1 As shown, it is a flowchart of a positioning method provided by an embodiment of the present disclosure, which may include the following steps:

[0093] 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.

[0094] Here, the robot can be any intelligent warehouse robot, which can perform tasks such as cargo handling, cargo inventory, warehouse mapping, etc. The warehouse can include multiple shelves, and each shelf can include multiple layers of cargo. The positioning frequency can be a frequency pre-set based on experience, for example, 5 Hz, 10 Hz, etc. The robot can be provided with multiple types of sensors, and different types of sensors are used to collect different types of data. Specifically, the multiple types of sensors can include any type of camera, any type of laser radar, and any type of inertial measurement unit (IMU) device. For example, the machine can be provided with a high-precision camera, a mechanical laser radar, and a nine-axis IMU device. The camera can collect warehouse images during the movement of the robot. At each positioning, the image collected by the camera between the current positioning and the previous positioning is defined as the target image. The laser radar can be the point cloud data collected by the laser radar on the robot during the movement of the robot. At each positioning, the point cloud data collected by the camera between the current positioning and the previous positioning is defined as the target point cloud data. The IMU device can collect the IMU data of the robot during the movement of the robot, for example, linear velocity data 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.

[0095] Optionally, the positioning frequency, the image acquisition frequency of the camera, the point cloud acquisition frequency of the laser radar, and the IMU acquisition frequency of the IMU device may be different or partially the same. Usually, the IMU acquisition frequency is higher than other frequencies.

[0096] In specific implementation, when the robot moves while performing tasks in the warehouse, the camera, lidar and IMU equipment on the robot can collect corresponding data according to their respective collection 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 images, target IMU data and target point cloud data required for each positioning can be determined according to the collection time of each frame image collected by the camera, the collection time of the point cloud data collected by the lidar, the collection time of each IMU data collected by the IMU device, and the positioning time of each positioning.

[0097] 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.

[0098] 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, where 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 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, and 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.

[0099] 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.

[0100] In specific implementation, for the current sub-positioning in any positioning, feature points can be extracted from the target image corresponding to the current sub-positioning to obtain various 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 2 Figure 2-1 in the figure is the target image, and each point in Figure 2-2 is the identified image feature point. Optionally, in order to avoid too many image feature points of each type, a quantity 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, and corner points that do not exceed the first preset 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 it can be determined based on the position and pixel value of the pixel point. After obtaining the image feature points corresponding to the current positioning, the target point cloud data corresponding to the current positioning can be projected to the target image, and the nearest projected point cloud point to each image feature point is determined 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 depth value of the nearest 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 each face point and edge point from the target point cloud data as a point cloud feature point. 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.

[0101] Optionally, before projecting the target point cloud data to 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 to the target image. Any dedistortion algorithm in the prior art may be used for dedistortion processing.

[0102] S103: Determine an IMU odometry factor according to the target IMU data, and 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.

[0103] Here, the disclosed embodiment 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 odometer 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 point can be a point cloud feature point selected from the point cloud feature points included in the point cloud data.

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

[0105] In specific implementation, for the current sub-positioning, the IMU odometry factor corresponding to the current sub-positioning can be obtained by pre-integrating the IMU data corresponding to the current sub-positioning; the initial camera odometry factor of the camera is determined according to the depth feature points corresponding to the current sub-positioning, and the initial odometry factor is constrained and optimized using the IMU odometry factor to obtain the camera-IMU odometry factor (i.e., the camera IMU odometry factor). The initial radar odometry factor of the lidar is determined according to the point cloud feature points, and the initial radar odometry factor is constrained and optimized using the IMU odometry factor to obtain the radar-IMU odometry factor (i.e., the radar IMU odometry factor).

[0106] Among them, for any positioning, the above S101~S103 can be used to determine the IMU odometer factor, camera-IMU odometer factor and radar-IMU odometer factor corresponding to the positioning.

[0107] S104: Determine the target position and 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.

[0108] Here, the target posture is used to characterize the posture of the robot in the world coordinate system, and one positioning corresponds to one target posture. The target posture can specifically include the robot's coordinate information, orientation information, rotation information, translation information, speed information, etc., among which the zero bias information is used to characterize the robot's linear velocity and acceleration.

[0109] In specific implementation, for any positioning, the factor graph model provided by the GTSAM library can be used to model and optimize the factors according to the IMU odometer factor, camera-IMU odometer factor and radar-IMU odometer factor corresponding to the positioning, so as to output the IMU pose of the robot in the IMU coordinate system, and then convert the IMU pose into the target pose of the robot according to the conversion relationship between the IMU coordinate system and the world coordinate system. Through each positioning at the positioning frequency, the target pose corresponding to each positioning of the robot can be determined.

[0110] In this way, the robot is positioned in the warehouse by fusing data from multiple sensors, and during positioning, the camera-IMU subsystem and radar-IMU subsystem are constructed, and the camera pose and radar pose are optimized and constrained in combination with IMU data, so as to improve the accuracy of the obtained camera odometer factor and radar odometer factor. Then, the robot pose is determined by using the radar odometer factor, camera odometer factor and IMU odometer factor, so that the robot's pose in the IMU coordinate system can be accurately converted to the world coordinate system, and the robot's accurate pose in the world coordinate system can be obtained. The entire positioning process can effectively improve the positioning accuracy of the robot by combining data from multiple sensors and using these data to constrain each other to achieve positioning.

[0111] In one embodiment, the above S101 may be implemented according to the following steps:

[0112] 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 laser radar on the robot according to the point cloud acquisition frequency.

[0113] 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.

[0114] Here, since 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 collected by the camera. Since 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 collected by the laser radar. Since the positioning frequency is higher than the point cloud acquisition frequency and lower than the image acquisition frequency, the laser radar may not have collected point cloud data at the time of the first positioning, so the first positioning may only obtain image and IMU data, and cannot obtain point cloud data. In each positioning except the first positioning, the laser radar may have collected point cloud data before or at the same time as the positioning, so each positioning except the first positioning can obtain the target point cloud data required for use. When the positioning frequency is higher than the point cloud acquisition frequency, it is necessary to ensure that at the same time starting point, before the second positioning is performed according to the positioning frequency, the laser radar will collect the first point cloud data.

[0115] 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 initial image that has been captured. The IMU data collected 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 initial IMU data that has been captured. 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 initial point cloud data that has been captured.

[0116] In specific implementation, when the robot moves in the warehouse, the camera, lidar and IMU equipment on the robot can collect corresponding data according to their respective collection frequencies, and obtain each frame of initial image collected by the camera, each piece of initial IMU data collected by the IMU equipment, and each frame of initial point cloud data collected by the lidar. That is, when the robot moves in the warehouse, each device on the robot will keep working according to its own collection frequency.

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

[0118] In specific implementation, when positioning is performed according to the positioning frequency, for any non-first positioning, each frame of initial image collected by the camera between the current positioning and the previous positioning can be used as the target image, or the two latest initial images taken by the camera before the current positioning can be used as the target image. At the same time, each piece of initial IMU data collected by the IMU device between the current positioning and the previous positioning can be used as the target IMU data, or each piece of initial IMU data located between the two selected target images can be used as the 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 the 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.

[0119] In another embodiment, 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.

[0120] Since the point cloud acquisition frequency is lower than the positioning frequency, when the first positioning is determined according to the positioning frequency, the laser radar 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 of the first positioning, thereby achieving the initialization of the camera-IMU subsystem and the radar-IMU subsystem.

[0121] Optionally, during the first positioning, the initial image taken by the camera before the first positioning can be used as the target image, and then the image feature points are 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 during the first positioning, and then the initial camera odometry factor is constrained and adjusted using the IMU odometry factor corresponding to the first positioning to obtain the camera-IMU odometry factor. At the same time, the IMU odometry factor corresponding to the first positioning can be used as the radar IMU odometry factor corresponding to the first positioning.

[0122] Furthermore, the target position of the robot in the first positioning can be determined by using the IMU odometry factor corresponding to the first positioning, the initialized camera-IMU odometry factor and the radar-IMU odometry factor, and using the factor graph method.

[0123] In one embodiment, the step of “obtaining depth feature points” in S102 may be implemented according to the following steps A1 to A3:

[0124] 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.

[0125] In specific implementation, for the current positioning (not the first positioning), the image with the smallest time interval between the acquisition time and the acquisition time of the target point cloud data can be selected from each frame of the target image according to the acquisition time of each frame of the target image corresponding to the positioning and the acquisition time of the target point cloud data corresponding to the positioning, and the image is used as the image to be projected. Then, feature points can be extracted from the image to be projected to obtain feature points of each image.

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

[0127] 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 spectra. By eliminating such point cloud points, the influence of unreliable depth on the positioning results can be avoided.

[0128] 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 each point in Figure 3-1 is the identified image feature point. 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.

[0129] 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.

[0130] In a specific implementation, the projection feature point closest to each image feature point can be determined according to the coordinates of each projection feature point in the image coordinate system and the coordinates of each image feature point in the image to be projected in the image coordinate system, and the depth value of the projection feature point closest to each image 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 projection feature point closest to the image feature point can also be associated and stored with the image feature point.

[0131] In one embodiment, at each positioning, 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 the 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 to be projected.

[0132] Here, the adjacent target image may be a target image whose acquisition time is before the acquisition time of 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 in the target IMU data within the time period of acquiring the image to be projected and the adjacent target image. The first IMU odometer factor may 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.

[0133] The step of determining the camera IMU odometer factor in S103 above may be performed according to B1 to B2 as follows:

[0134] B1: Extract image feature points from the adjacent target image of the image to be projected, and determine the camera odometer factor of the camera under 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.

[0135] Here, the camera odometer factor at the current positioning is used to characterize the position and posture of the camera at the current positioning.

[0136] In specific implementation, for the current positioning, each image feature point can be extracted from the adjacent target image of the image to be projected corresponding to the current positioning. The method for extracting image feature points is similar to the method for extracting image feature points from the target image above, and will not be repeated here. Then, the camera can be positioned according to the depth feature points of the image to be projected and the image feature points of the adjacent target image, and the camera odometer factor of the camera under the current positioning can be determined.

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

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

[0139] In one embodiment, the above B2 can be implemented according to the following steps:

[0140] 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.

[0141] Here, the target image to be projected may be an image among the images to be projected corresponding to the respective positionings before the current positioning, the image whose similarity with the image to be projected corresponding to the current positioning satisfies the similarity threshold.

[0142] In order to further improve the rationality of the determined camera pose, camera loop detection can be performed in each positioning except the first positioning. The specific steps of camera loop detection can be: for the current positioning, according to the image feature points of the image to be projected corresponding to the current positioning, generate the first feature vector of the current positioning. According to the image feature points of the image to be projected corresponding to each positioning before the current positioning (for the convenience of description, the image to be projected is defined as the historical image to be projected), generate the second feature vectors corresponding to each positioning before the current positioning. Then, the similarity between the first feature vector and each second feature vector is used as the similarity between the image to be projected corresponding to the current positioning and each historical image to be projected. Determine whether there is a target similarity greater than or equal to the similarity threshold. If not, it is determined that there is no target image to be projected. 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 odometer factor is determined using the following B2-1.

[0143] 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 may be used to determine whether there is a target image to be projected.

[0144] B2-2: If yes, determine the camera IMU odometer factor of the camera at the current positioning 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.

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

[0146] When performing camera loop detection, if the target image to be projected is detected, it means that the camera pose when the robot collects the target image to be projected is very similar to the camera pose when the image to be projected is collected for the current time, and the camera pose when the target image to be projected is collected can be used to optimize the current camera pose. Specifically, whether there is one or more target images to be projected, the factor graph model can be used to constrain and optimize the camera odometer factor according to the camera IMU odometer factor corresponding to each target image to be projected, the camera odometer factor determined during the current positioning, and the first IMU odometer factor determined during the current positioning, to obtain the camera-IMU odometer factor under the current positioning.

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

[0148] In another embodiment, due to its structural characteristics, the camera can stably image at low speed, and the image captured in a short time has little change, so it performs better in feature matching. Therefore, in order to further improve the accuracy of the determined camera odometer factor, the step of determining the camera odometer factor in B1 above can be implemented as follows:

[0149] B1-1: When there are other images in the target image except the image to be projected and the adjacent target images, the similarity between the image feature points of the other images and the image feature points of the image to be projected is determined.

[0150] In 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 can be determined whether there are other images other than the image to be projected and the adjacent target images in the multiple frames of target images. If not, the camera odometer factor can be determined directly 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 feature vector of each other image is determined. At the same time, the first feature vector 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 feature vector and each third feature vector is calculated, and the similarity between the first feature vector and the third feature vector 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.

[0151] B1-2: When the similarity meets the preset similarity, the camera odometer factor of the camera under the current positioning is determined according to 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.

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

[0153] In specific implementation, according to the similarity between the image feature points of each other image and the image feature points of the image to be projected, it is determined whether there are other images whose similarity meets the preset similarity. If not, the camera odometer factor can be directly determined by using the depth feature points of the image to be projected and the image feature points in the adjacent target image. If so, the camera can be positioned using the image feature points of other images whose similarity meets the preset similarity, the depth feature points of the image to be projected, and the image feature points of the adjacent target image to obtain the camera odometer factor of the camera under the current positioning. In this way, by combining the image feature points in multiple frames of images with high similarity for camera positioning, the positioning accuracy can be improved.

[0154] like Figure 4 As shown, it is a schematic diagram of a process for determining a camera-IMU odometer factor provided by an embodiment of the present disclosure, wherein a camera key frame is a target image captured by a camera, a visual feature is an image feature point in the target image, and the visual features corresponding to two adjacent camera key frames are obtained by averaging the visual features corresponding to the two adjacent camera key frames; an aligned depth map can be understood as a depth map obtained by projecting the target point cloud data onto the target image; a visual feature with depth is a depth feature point with a depth value; and a visual feature with depth can be determined by an aligned depth map. Figure 4In , an aligned depth map points to two visual features with depth, wherein 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 odometer factor. The IMU measurement is the measured IMU data, the IMU constraint is the IMU odometer factor, and after pre-integration and averaging of multiple IMU data, an IMU odometer factor is obtained. When multiple camera key frames point to the same visual feature, it can be said that the multiple key frames are similar, and the visual features of the multiple frames can be used to obtain a pointed visual feature. For each positioning, the multiple target images, multiple IMU data, and target point cloud data required for the positioning can be used, and the camera-IMU odometer factor for the positioning can be determined according to the steps for determining the camera-IMU odometer factor described above.

[0155] Due to its structural characteristics, the camera can stably image at low speeds, and because the image changes little in a short period of time, it performs well in feature matching. However, when the camera moves at high speed, the image is blurred and the image changes too quickly in a short period of time, resulting in too large differences, which will lead to too few overlapping areas and feature mismatches. As for the IMU device, it outputs linear velocity data and acceleration data, which can still produce good results during fast motion. Therefore, the camera and IMU device are complementary in slow and fast motion, so by building a camera-IMU subsystem, the camera can be accurately positioned.

[0156] In one embodiment, for the above B2, it can also be determined according to the following steps:

[0157] According to the first IMU odometer factor corresponding to the current positioning and the first IMU odometer factors corresponding to each positioning other than the current positioning, the first pre-integration error corresponding to the current positioning is determined; according to each image feature point in the image to be projected and the camera odometer factor, the reprojection error corresponding to the current positioning is determined; with the goal of minimizing the sum of the first pre-integration error and the reprojection error, the camera IMU odometer factor under the IMU constraint is determined.

[0158] Here, the first pre-integration error is used to characterize the deviation between the pre-integrated 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, state k-1 is the target posture of the robot after k-1 positioning, the pre-integrated measurement value is the IMU odometer factor, and the state variable is the robot state change information between state k-1 and state k.

[0159] 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 variables, as well as the error when projecting the target point cloud data onto the target image.

[0160] In specific implementation, the camera-IMU odometer factor under the current positioning can be determined according to the following formula 1:

[0161] ; (Formula 1)

[0162] in, , ,……, represents the target poses for the first, second, ..., and n+1th positioning, respectively, ρ 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 odometer 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 of the i-th row, Represents the depth value of the image feature point at (i, j), Represents the sum of the camera odometer factors corresponding to each positioning; Express Find the mean square.

[0163] Exemplarily, at each positioning, the first IMU odometer factor, image feature points, and camera odometer factor can be substituted into the above formula 1, and the camera IMU odometer factor corresponding to each positioning can be obtained with the goal of minimizing the value corresponding to formula 1.

[0164] In one embodiment, at each non-initial positioning, the IMU odometer factor may include a second IMU odometer 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, and then the pre-integrated values ​​may be averaged to obtain the second IMU odometer factor corresponding to the current positioning. For the step of determining the radar-IMU odometer factor in S103 above, it may be performed as follows C1~C2:

[0165] C1: Determine the radar odometer 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.

[0166] 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 the point cloud map corresponding to the warehouse. As the positioning process progresses, the constructed point cloud map of the warehouse 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 each point cloud feature point that matches the IMU odometry factor corresponding to the first positioning, and use these point cloud feature points to construct the point cloud warehouse map corresponding to the first positioning.

[0167] In the embodiments of the present disclosure, the current positioning, without special explanation, may refer to any positioning except the first positioning. For the current positioning, the point cloud feature points under the current positioning may be matched with the constructed warehouse point cloud map, and the radar pose with the best matching effect may be used as the radar odometer factor of the laser radar under the current positioning.

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

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

[0170] In one embodiment, the above C2 can be implemented according to the following steps:

[0171] 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.

[0172] 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.

[0173] 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.

[0174] C2-2: If yes, 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.

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

[0176] For example, when performing radar loop detection, if matching point cloud data is detected, it means that the radar pose of the robot when collecting matching point cloud data is very similar to the radar pose when collecting target point cloud data in the current time, and the radar pose when collecting matching point cloud data can be used to optimize the current radar pose. Specifically, whether there is one or more matching point cloud data, the factor graph model can be used to constrain and optimize the radar odometer factor according to the radar-IMU odometer factor corresponding to each matching point cloud data, the radar odometer factor determined in the current positioning, and the second IMU odometer factor determined in the current positioning, so as to obtain the radar-IMU odometer factor under the current positioning.

[0177] Optionally, after optimizing the radar-IMU odometer factor under the current positioning, the radar-IMU odometer factor under the current positioning can also be used as the radar-IMU odometer factor corresponding to the matching point cloud data, thereby optimizing the radar-IMU odometer factor corresponding to the matching point cloud data.

[0178] Optionally, when performing radar loop detection, it is also possible to determine whether there are target point cloud feature points similar to the point cloud feature points under the current sub-positioning based on the point cloud feature points under the current sub-positioning and the point cloud feature points under each historical sub-positioning. If so, the radar odometer factor of the laser radar under the current sub-positioning can be determined by using the point cloud feature points under the current sub-positioning, the target point cloud feature points, and the warehouse point cloud map constructed corresponding to the current sub-positioning. In this way, by combining multi-frame point cloud feature points, the accuracy of the determined point cloud feature points can be improved.

[0179] like Figure 5 As shown, it is a schematic diagram of a process for determining a radar-IMU odometer factor provided by an embodiment of the present disclosure, wherein: represents 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 5 The radar keyframes in the radar frame can be extracted from the radar frame at a preset frame interval. For any radar keyframe under a positioning, the radar keyframe can be called a sub-keyframe in each positioning after this positioning. The IMU pre-integration factor is the IMU odometry factor. The laser-IMU subsystem receives 3D point cloud data from the laser radar and IMU sensor data, 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 result. 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 previously mentioned visual-inertial odometry (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 , the radar odometer factor under the second positioning can be determined by using the point cloud keyframe under the second positioning, the warehouse point cloud map corresponding to the second positioning, and the point cloud keyframe corresponding to the first positioning. Then, the radar odometer factor under the second positioning and the second IMU data under the second positioning are used to determine the radar-IMU odometer factor under the second positioning (i.e. ).about , ,…… The determination process, and Similar, no further description is given here.

[0180] In one embodiment, the above C2 may also be implemented according to the following steps:

[0181] According to the second IMU odometer factor corresponding to the current sub-positioning and the second IMU odometer factors corresponding to each sub-positioning other than the current sub-positioning, the second pre-integration error corresponding to the current sub-positioning is determined; according to the radar odometer factor corresponding to the current sub-positioning and the point cloud feature points corresponding to the current sub-positioning, the matching error corresponding to the current sub-positioning is determined; with the goal of minimizing the sum of the second pre-integration error and the matching error, the radar IMU odometer factor under the IMU constraint is determined.

[0182] 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 mentioned above and will not be repeated here.

[0183] 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 odometer factor predicted based on the state variable.

[0184] 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:

[0185] D1: Determine that the target point cloud data includes each point cloud line and the point cloud points in each point cloud line.

[0186] Since the laser radar used in the embodiment of the present disclosure may be a multi-line laser radar, and the multi-line laser radar naturally 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 point itself, but also the point cloud line from which the point cloud point comes, the order of the point cloud points on the same point cloud line, etc. Therefore, after obtaining the target point cloud data, it can be determined that the target point cloud data includes each point cloud line and each point cloud point in each point cloud line according to the information carried by each point cloud point in the target point cloud data.

[0187] D2: For each point cloud line, determine 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 determine the edge points and surface points in the point cloud line according to the smoothness.

[0188] In specific implementation, the smoothness of each point cloud point can be determined according to the following formula 2:

[0189] ; (Formula 2)

[0190] 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 represented by express and The norm of the difference.

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

[0192] 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 largest smoothness can be used as the surface point in the point cloud line, and the point cloud point with the smallest smoothness can be used as the edge point in the point cloud line. In this way, the surface points and edge points in each point cloud line can be obtained.

[0193] D3: The edge points and surface points in each point cloud line are used as point cloud feature points.

[0194] In one embodiment, since the point cloud data collected by the laser radar may be distorted by motion when the robot is moving, the motion distortion may be corrected according to the following steps before determining the feature points of the point cloud:

[0195] The target IMU data corresponding to the current positioning is used to determine the motion trajectory of the laser radar when collecting target point cloud data; according to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0196] Here, since the laser radar acquires information about the surrounding environment through physical rotation, the laser radar 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 linear velocity and acceleration of the device, so that the precise motion trajectory of the laser radar scanning process can be estimated, and then motion compensation can be performed on each scanning point based on the motion trajectory, thereby offsetting the distortion effect caused by the laser radar movement. The laser radar motion distortion removal with IMU assistance can effectively restore the real environment when the radar moves at high speed.

[0197] In specific implementation, 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, and the motion trajectory can be used as the motion trajectory of the laser radar. Then, the trajectory is used to dedistort the coordinates of each point cloud point in the target point cloud data collected by the laser radar in the current positioning, thereby realizing motion compensation of the point cloud points and obtaining the dedistorted coordinates. Then, the dedistorted coordinates can be used to determine the smoothness of the point cloud points, and the smoothness can be used to screen the feature points of the point cloud.

[0198] like Figure 6 As shown, it is a point cloud comparison diagram before and after motion compensation provided by an embodiment of the present disclosure, 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, and the point cloud after motion distortion correction assisted by IMU is the red point cloud in Figure 6-2, and Figure 6-3 is an enlargement 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.

[0199] 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 for the embodiment of the present disclosure, such as 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 for the embodiment of the present disclosure, such as Figure 7-3As shown, a trajectory comparison diagram corresponding to the XYZ axes of the trajectory map obtained by using the positioning method of the present disclosure embodiment and the trajectory map obtained by using the mapping method in the prior art is provided for the present disclosure embodiment, wherein the mapping method in the prior art includes the mapping method of VINS-Mono (visual-inertial measurement unit odometer) and the mapping method of LIO-SAM (lidar-inertial measurement unit odometer). 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, and 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 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-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.

[0200] based on Figure 7-1 to Figure 7-3 It can be seen that under the positioning method provided in 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, the advantages of the positioning method provided in the embodiment of the present disclosure can be better demonstrated.

[0201] In one embodiment, for the current positioning, after determining the radar IMU odometer factor under the IMU constraint, the point cloud feature points corresponding to the current positioning can be screened according to the radar IMU odometer factor corresponding to the current positioning to obtain the screened feature points corresponding to the current positioning; according to the screened feature points of the current positioning, the constructed warehouse point cloud map is supplemented to obtain the supplemented warehouse point cloud map.

[0202] Exemplarily, for the current sub-positioning, when the point cloud feature points corresponding to the current sub-positioning are matched by using the warehouse point cloud map corresponding to the current sub-positioning, the point cloud feature points corresponding to the current sub-positioning can be screened, and when the radar posture is optimized by using the matching error and the second pre-integrated error, the point cloud feature points corresponding to the current sub-positioning can also be screened, so the point cloud feature points retained after two screenings can be used as screening feature points. Alternatively, the radar-IMU odometer factor corresponding to the current sub-positioning can be used to match the respective point cloud feature points corresponding to the current sub-positioning with the radar-IMU odometer factor corresponding to the current sub-positioning, and the screening feature points with the matching degree meeting the requirements can be screened from the respective point cloud feature points corresponding to the current sub-positioning. Then, the screening feature points of the current sub-positioning can be used to update the warehouse point cloud map that has been constructed before the current sub-positioning and supplement the feature points, so as to obtain the supplemented warehouse point cloud map, which can be regarded as the warehouse point cloud map constructed corresponding to the current sub-positioning.

[0203] Furthermore, after obtaining the replenished warehouse point cloud map each time after positioning, the replenished 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 end of positioning.

[0204] Here, when the robot is performing a task of mapping a warehouse, the robot will move around the entire warehouse, and during the movement, the positioning algorithm provided by the disclosed embodiment will be used to continuously position the robot, and the warehouse point cloud map will be updated and improved after each positioning. After the positioning is completed, the robot has also completed the movement around the entire warehouse, and at this time, the supplemented warehouse point cloud map corresponding to the last positioning can be used as the global point cloud map of the warehouse.

[0205] Optionally, after each positioning, the selected feature points selected from the target point cloud data collected by each positioning using the positioning algorithm can be stored, for example, in a file format of a point map. 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 an embodiment of the present disclosure, such as 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-5compared to Figure 7-4 It can be seen intuitively that the laser-IMU mapping algorithm fails to accurately capture the structural details of the actual scene in the upper right corner, and an offset occurs, while the positioning algorithm provided by the embodiment of the present disclosure will not have this problem. Therefore, it can be seen that when the positioning algorithm provided by the embodiment of the present disclosure processes large scene mapping, it can accurately reconstruct the actual three-dimensional shape of the warehouse because of the small offset in the Z-axis direction.

[0206] like Figure 8 As shown, it is 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 odometer factor, radar-IMU odometer factor, and IMU odometer factor are determined. For the camera, image feature points can be extracted from the image captured by the camera, and depth feature points can be obtained by depth registration using the depth values ​​of the point cloud points. At the same time, feature tracking can also be performed, wherein the steps of feature tracking can refer to B1-1 and B1-2 above.

[0207] The point cloud data used in the depth registration can be obtained by performing point cloud dedistortion on the point cloud data collected by the laser radar. At the same time, for the point cloud data collected by the laser radar, feature extraction can be performed on the dedistorted point cloud data to obtain point cloud feature points (i.e., edge points and surface points), and feature matching can be performed on the point cloud feature points, wherein the feature matching steps can refer to the above C2-1 and C2-2. Figure 8 The loop detection steps in can refer to the above descriptions of camera loop detection and radar loop detection. The initialization step is to use the IMU odometer factor corresponding to the first positioning as the camera-IMU odometer factor and radar-IMU odometer factor corresponding to the first positioning during the first positioning. The IMU measurement value is the IMU data. The radar-assisted visual odometry is to determine the camera IMU odometer factor using the depth feature points and the IMU odometer factor (obtained by pre-integration based on the IMU data). The multi-sensor factor graph optimization is to use the factor graph model to determine the IMU pose of the robot in the world coordinate system based on the IMU odometer factor, the camera IMU odometer factor and the radar IMU odometer factor. Then, the IMU pose can be converted into the target pose of the robot in the world coordinate system based on the conversion relationship between the world coordinate system and the IMU coordinate system.

[0208] Those skilled in the art will appreciate that, in the above method of specific implementation, the order in which the steps are written does not imply a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of the steps should be determined by their functions and possible internal logic.

[0209] 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.

[0210] like Fig. 9 FIG. 1 is a schematic diagram of a positioning device provided in an embodiment of the present disclosure, comprising:

[0211] 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;

[0212] An extraction module 902 is 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;

[0213] A first determination module 903 is used to determine an IMU odometry factor according to the target IMU data, and determine a camera IMU odometry factor under the IMU constraint according to the depth feature point and the IMU odometry factor, and determine a radar IMU odometry factor under the IMU constraint according to the point cloud feature point and the IMU odometry factor;

[0214] The second determination module 904 is used to determine the target pose 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.

[0215] 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 used to:

[0216] 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;

[0217] 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.

[0218] In a possible implementation manner, the acquisition module 901 is further configured to:

[0219] 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;

[0220] 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.

[0221] 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 used to:

[0222] 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;

[0223] Reprojecting each point cloud point in the target point cloud data to the image to be projected to obtain each projection feature point;

[0224] 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.

[0225] In a possible implementation, 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 an adjacent target image of the image to be projected;

[0226] The first determination module 903, when determining the camera IMU odometer factor under the IMU constraint according to the depth feature point and the IMU odometer factor, is used to:

[0227] 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;

[0228] A camera IMU odometry factor under IMU constraints is determined according to the first IMU odometry factor and the camera odometry factor.

[0229] In a possible implementation manner, the first determination module 903, when determining the camera IMU odometer factor under the IMU constraint according to the first IMU odometer factor and the camera odometer factor, is used to:

[0230] 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;

[0231] 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.

[0232] In a possible implementation manner, the first determination module 903, when determining the camera odometer 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:

[0233] 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;

[0234] 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.

[0235] In a possible implementation manner, the first determination module 903, when determining the camera IMU odometer factor under the IMU constraint according to the first IMU odometer factor and the camera odometer factor, is used to:

[0236] Determine a first pre-integrated 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;

[0237] 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;

[0238] 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.

[0239] In a possible implementation, the IMU odometer factor includes a second IMU odometer factor obtained by pre-integrating the target IMU data;

[0240] The first determination module 903, when determining the radar IMU odometer factor under the IMU constraint according to the point cloud feature point and the IMU odometer factor, is used to:

[0241] 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;

[0242] A radar IMU odometry factor under IMU constraints is determined according to the radar odometry factor and the second IMU odometry factor.

[0243] In a possible implementation manner, the first determination module 903, when determining the radar IMU odometer factor under the IMU constraint according to the radar odometer factor and the second IMU odometer factor, is used to:

[0244] 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;

[0245] 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.

[0246] In a possible implementation manner, the first determination module 903, when determining the radar IMU odometer factor under the IMU constraint according to the radar odometer factor and the second IMU odometer factor, is used to:

[0247] 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;

[0248] 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;

[0249] 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.

[0250] In a possible implementation, the device further includes:

[0251] Mapping module 905, after determining the radar IMU odometer factor under the IMU constraint, is used to:

[0252] 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;

[0253] 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;

[0254] 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.

[0255] In a possible implementation, the extraction module 902, when extracting point cloud feature points from the target point cloud data, is used to:

[0256] Determine that the target point cloud data includes each point cloud line and each point cloud point in the point cloud line;

[0257] 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;

[0258] The edge points and surface points in each of the point cloud lines are used as the point cloud feature points.

[0259] 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:

[0260] 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;

[0261] According to the motion trajectory, the coordinates of each point cloud point in the target point cloud data are dedistorted.

[0262] In a possible implementation, the device further includes:

[0263] The drawing module 906, after determining the target position of the robot in the world coordinate system, is used to:

[0264] According to the target posture obtained in each positioning, the movement trajectory of the robot in the warehouse is drawn.

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

[0266] Based on the same technical concept, the embodiment of the present application also provides a computer device. Fig.10 FIG. 1 is a schematic diagram of a computer device provided in an embodiment of the present application, including:

[0267] Processor 1001, memory 1002 and bus 1003. The memory 1002 stores machine-readable instructions executable by the processor 1001, and the processor 1001 is used to execute the machine-readable instructions stored in the memory 1002. When the machine-readable instructions are executed by the processor 1001, the processor 1001 executes the above steps S101 to S104.

[0268] 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 internal memory 1021. When the computer device is running, the processor 1001 communicates with the memory 1002 through the bus 1003, so that the processor 1001 executes the execution instructions mentioned in the above-mentioned method embodiment.

[0269] The present disclosure also provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the steps of the positioning method described in the above method embodiment are executed. The storage medium may be a volatile or non-volatile computer-readable storage medium.

[0270] 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, and 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, which will not be repeated here.

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

[0272] Those skilled in the art can clearly understand that, for the convenience and simplicity 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.

[0273] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0274] 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.

[0275] 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, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform 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 codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0276] If the technical solution of this application involves personal information, the product using the technical solution of this application has clearly informed the personal information processing rules and obtained the individual's voluntary consent before processing the personal information. If the technical solution of this application involves sensitive personal information, the product using the technical solution of this application has obtained the individual's separate consent before processing the sensitive personal information, and at the same time meets the "explicit consent" requirement. For example, on personal information collection devices such as cameras, clear and prominent signs are 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 he or she agrees to the collection of his or her personal information; or on the device that processes personal information, the personal information processing rules are notified by obvious signs / information, and the individual's authorization is obtained through pop-up information or by asking the individual to upload his or her personal information; among them, 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.

[0277] 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 protection scope of the present disclosure is not limited thereto. Although the present disclosure is described in detail with reference to the aforementioned embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the aforementioned embodiments within the technical scope disclosed in the present disclosure, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present disclosure, and should be included in the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure shall be based on the protection scope 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