Positioning method, device, mobile robot and medium based on multi-sensor fusion

Abstract

The present invention discloses a positioning method, device, mobile robot and medium based on multi-sensor fusion, the method comprising: obtaining positioning data collected by a sensor and image data collected by a camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data; constructing a reprojection error based on the RGB image data and the depth image data; obtaining an IMU pre-integration error in the IMU data; constructing a visual inertial odometry based on the reprojection error and the IMU pre-integration error; and fusing the data in the visual inertial odometry with the UWB data to obtain target positioning data. By obtaining positioning data and on-site environmental information, the present invention realizes real-time positioning error correction, eliminates the need to set landmarks in advance and eliminates the need to restrict the path planning of the mobile robot, and realizes application scenarios applicable to a variety of mobile robot positioning.

Description

Technical Field

[0001] The present invention relates to the field of measurement and positioning technology, and in particular to a positioning method, device, mobile robot and medium based on multi-sensor fusion. Background Art

[0002] With the rapid development of artificial intelligence (AI) technology and the support of hardware computing power, mobile robots have been widely used in indoor locations such as shopping malls, hospitals, smart manufacturing plants, and smart warehouses, performing tasks such as cleaning, delivering medicine, loading and unloading materials on production lines, and transporting goods in transit. Due to the complex environments, high population density, and high risk of dangerous situations in these areas, a high-precision robot positioning system is required. This system enables mobile robots to accurately obtain information about the on-site environment and their own location, and to adjust their strategies based on this information to achieve the desired functions.

[0003] Existing methods for precise positioning of mobile robots primarily rely on three-dimensional laser radar and landmark acquisition to achieve accurate positioning. This method requires the pre-setting of landmarks and their detection during the positioning process to correct positioning errors and ultimately achieve precise positioning. This requirement for pre-setting landmarks limits the mobile robot's path planning and makes it unsuitable for many mobile robot positioning applications. Summary of the Invention

[0004] The embodiments of the present invention provide a positioning method, device, mobile robot and medium based on multi-sensor fusion to adapt to various application scenarios of mobile robot positioning.

[0005] In a first aspect, an embodiment of the present invention provides a positioning method based on multi-sensor fusion, which includes:

[0006] Acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data;

[0007] constructing a reprojection error based on the RGB image data and the depth image data;

[0008] Obtaining an IMU pre-integration error in the IMU data;

[0009] Constructing a visual inertial odometry based on the reprojection error and the IMU pre-integration error;

[0010] The data in the visual inertial odometry is fused with the UWB data to obtain target positioning data.

[0011] In a second aspect, an embodiment of the present invention provides a positioning device based on multi-sensor fusion, which includes:

[0012] A data acquisition unit, configured to acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data;

[0013] a reprojection error construction unit, configured to construct a reprojection error based on the RGB image data and the depth image data;

[0014] A pre-integration error acquisition unit, configured to acquire an IMU pre-integration error from the IMU data;

[0015] an odometry construction unit, configured to construct a visual inertial odometry based on the reprojection error and the IMU pre-integration error;

[0016] The target positioning data generating unit is used to fuse the data in the visual inertial odometer with the UWB data to obtain target positioning data.

[0017] In a third aspect, an embodiment of the present invention provides a mobile robot, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the positioning method based on multi-sensor fusion described in the first aspect is implemented.

[0018] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the positioning method based on multi-sensor fusion described in the first aspect above.

[0019] An embodiment of the present invention provides a positioning method, device, mobile robot and medium based on multi-sensor fusion, the method comprising: obtaining positioning data collected by a sensor and image data collected by a camera, wherein the positioning data comprises IMU data and UWB data, and the image data comprises RGB image data and depth image data; constructing a reprojection error based on the RGB image data and the depth image data; obtaining an IMU pre-integration error in the IMU data; constructing a visual inertial odometry based on the reprojection error and the IMU pre-integration error; fusing the data in the visual inertial odometry with the UWB data to obtain target positioning data. By obtaining positioning data and on-site environmental information, the embodiment of the present invention achieves real-time positioning error correction, without the need to set landmarks in advance and without restricting the path planning of the mobile robot, thus achieving application scenarios applicable to a variety of mobile robot positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 A flowchart of a positioning method based on multi-sensor fusion provided by an embodiment of the present invention;

[0022] Figure 2 A schematic diagram of the hardware composition of a positioning method based on multi-sensor fusion provided in an embodiment of the present invention;

[0023] Figure 3 A flowchart of a positioning method based on multi-sensor fusion provided by an embodiment of the present invention;

[0024] Figure 4 A schematic flow chart of a positioning method based on multi-sensor fusion provided in another embodiment of the present invention;

[0025] Figure 5 A schematic flow chart of a positioning method based on multi-sensor fusion provided in another embodiment of the present invention;

[0026] Figure 6 A schematic flow chart of a positioning method based on multi-sensor fusion provided in another embodiment of the present invention;

[0027] Figure 7 A schematic flow chart of a positioning method based on multi-sensor fusion provided in another embodiment of the present invention;

[0028] Figure 8 A schematic flow chart of a positioning method based on multi-sensor fusion provided in another embodiment of the present invention;

[0029] Figure 9 A schematic block diagram of a positioning device based on multi-sensor fusion provided by an embodiment of the present invention;

[0030] Figure 10 A schematic block diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0031] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0032] It will be understood that when used in this specification and the appended claims, the terms “comprises” and “comprising” indicate the presence of described features, integers, steps, operations, elements and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0033] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the present invention. As used in the specification and appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms unless the context clearly indicates otherwise.

[0034] It should be further understood that the term "and / or" used in the present description and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

[0035] See also Figure 1 and Figure 2 , Figure 1 A flowchart of a positioning method based on multi-sensor fusion provided by an embodiment of the present invention; Figure 2 Schematic diagram of the hardware composition of the positioning method based on multi-sensor fusion provided by an embodiment of the present invention. The positioning method based on multi-sensor fusion provided by an embodiment of the present invention is applied to a mobile robot.

[0036] S1: Acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data.

[0037] like Figure 2 As shown, the hardware components of the embodiment of the present application are mainly composed of a decision-making planning layer, an execution layer, and a perception layer. The decision-making planning layer is mainly composed of a CPU and a graphics card, responsible for the implementation of the algorithm and strategy of the fusion positioning method, as well as the data acquisition of the RGB-D camera and the communication function with the execution layer; the core part of the execution layer is composed of a control board with an AMR processor as the core, which is mainly responsible for data acquisition and control of the UWB ultra-wideband system, as well as communication with the decision-making planning layer; the perception layer uses an RGB-D camera, an IMU inertial sensor, and a UWB ultra-wideband system.

[0038] See also Figure 3 , Figure 3 A specific implementation of S1 is shown, which is described in detail as follows:

[0039] S11: Acquire RGB image data and depth image data through an RGB-D camera.

[0040] S12: Obtain acceleration and angular acceleration data through the accelerometer and gyroscope to obtain IMU data.

[0041] S13: Acquire the distance information between the fixed anchor and the mobile terminal through carrier-free communication technology to obtain UWB data.

[0042] In the embodiment of the present application, IMU (Inertial Measurement Unit), that is, inertial measurement unit, is used to measure the three-axis attitude angle (or angular rate) and acceleration of the object. UWB (Ultra Wide Band), that is, ultra-wideband. In the embodiment of the present application, RGB image data and depth image data are obtained by an RGB-D camera installed on a mobile robot. The IMU inertial odometer is composed of an accelerometer and a gyroscope, which can provide acceleration and angular acceleration data; the UWB ultra-wideband positioning system is composed of a fixed anchor and a mobile terminal, and the distance information between the two is obtained through carrier-free communication technology, thereby obtaining UWB data. Among them, depth image data (depth image) is also called range image, which refers to an image with the distance (depth) from the image collector to each point in the scene as the pixel value, which directly reflects the geometric shape of the visible surface of the scene. Depth image data can be calculated as point cloud data after coordinate conversion, and point cloud data with rules and necessary information can also be back-calculated into depth image data.

[0043] S2: Construct reprojection error based on RGB image data and depth image data.

[0044] Specifically, the reprojection error refers to the difference between the projection and reprojection of a real 3D space point on the image plane.

[0045] See also Figure 4 , Figure 4 A specific implementation of S2 is shown, which is described in detail as follows:

[0046] S21: Extract features from the RGB image data to obtain ORB features.

[0047] S22: Match the ORB features to obtain a matching result.

[0048] Specifically, the ORB (Oriented FAST and Rotated BRIEF) feature consists of key points and descriptors. In the embodiment of the present application, the Fast Approximate Nearest Neighbor (FLANN) algorithm is used to perform feature matching on the ORB feature to obtain a matching result. The matching result includes multiple matching feature points and the number of matching feature points.

[0049] S23: Filter the matching results to remove incorrect matching feature points in the matching results to obtain target matching feature points.

[0050] See also Figure 5 , Figure 5 A specific implementation of S23 is shown, which is described in detail as follows:

[0051] S231: Compare the number of matching feature points in the matching result with a preset value.

[0052] S232: If the number of matching feature points is higher than a preset value, the PROSAC algorithm is used to eliminate incorrect matching feature points in the matching results to obtain target matching feature points.

[0053] S233: If the number of matching feature points is less than or equal to a preset value, obtaining the distance between each pair of matching feature points in the matching result, and obtaining the minimum matching feature point distance in the matching result.

[0054] S234: Based on the distance between each pair of matching feature points and the minimum matching feature point distance, the matching results are screened to obtain target matching feature points.

[0055] Specifically, after the FLANN algorithm performs feature matching, a certain number of false matches will occur. These false matches are relatively small compared to the total number of feature points, but they will cause errors in global motion estimation. Therefore, in order to further improve the matching accuracy of feature points, false matching points need to be eliminated.

[0056] In the embodiment of the present application, the number of matching feature points in the matching result is first compared with a preset value. If the number of matching feature points is higher than the preset value, the PROSAC algorithm is used to eliminate the erroneous matching feature points in the matching result to obtain the target matching feature points. If the number of matching feature points is lower than or equal to the preset value, the distance between each pair of matching feature points in the matching result is obtained, and the minimum matching feature point distance in the matching result is obtained as the minimum distance. Among them, the distance between matching feature points is the Hamming distance. Then the interval: the distance between matching feature points < twice the minimum distance < 30, and whether the distance between each pair of matching feature points falls within the above interval is determined in turn. If not, the two matching feature points are eliminated; if they fall within the above interval, the two matching feature points are retained. Finally, the target matching feature points are obtained.

[0057] PROSAC (Progressive Sampling Consensus) is a semi-random method that evaluates the quality of all point pairs and calculates a Q value. These points are then sorted in descending order based on their Q values. Empirical model assumptions are only made and verified on high-quality point pairs, significantly reducing the computational effort. It should be noted that the preset value is determined based on actual conditions and is not limited here. In one specific embodiment, the preset value is 400.

[0058] S24: Perform pose estimation based on the target matching points and the depth image data to construct a reprojection error.

[0059] See also Figure 6 , Figure 6 A specific implementation of S24 is shown, which is described in detail as follows:

[0060] S241: Generate point cloud data by matching the depth image data.

[0061] S242: Estimating the pose of the target matching points and the depth image data by solving the point pair motion of the point cloud data and the target matching points from three dimensions to two dimensions, and constructing a reprojection error.

[0062] Specifically, after successful feature matching, the pose estimate of the RGB-D camera is obtained. Because indoor environments are complex and changeable, fewer feature points can be obtained. Therefore, the PnP algorithm is used to solve the 3D to 2D point pair motion, which can obtain a better pose estimate using fewer matching points. The BA algorithm is used to construct the PnP problem as a minimum reprojection error problem. Therefore, the PnP algorithm and BA algorithm are used to solve the RGB-D camera pose and optimize it to construct the reprojection error. Among them, PnP (Perspective-n-Point) is a method for solving 3D to 2D point pair motion. The Bat Algorithm (BA) is a heuristic search algorithm proposed by Professor Yang in 2010 based on swarm intelligence. It is an effective method for searching for the global optimal solution.

[0063] S3: Get the IMU pre-integration error in the IMU data.

[0064] Specifically, the measurement model of IMU pre-integration is first obtained; then the IMU pre-integration in the IMU data is obtained in sequence; then the IMU pre-integration ideal value and the pre-integration measurement value are obtained according to the measurement model, and the difference between the two is calculated to obtain the IMU pre-integration error.

[0065] S4: Construct a visual inertial odometry based on the reprojection error and the IMU pre-integration error.

[0066] See also Figure 7 , Figure 7 A specific implementation of S4 is shown, which is described in detail as follows:

[0067] S41: Constructing a first cost function of the reprojection error, and performing optimization estimation processing on the state variables of the camera based on the first cost function to obtain a first optimization estimation result.

[0068] S42: Based on the IMU pre-integration error, construct a second cost function, and based on the second cost function, perform optimization estimation processing on the IMU data to obtain a second optimization estimation result.

[0069] S43: Based on the first optimization estimation result, the sitting posture in the camera coordinate system is transformed to the robot coordinate system, and based on the second optimization estimation result, a joint objective function is constructed to construct a visual inertial odometry.

[0070] Specifically, the reprojection error and the IMU pre-integration error are obtained and used as inputs. The measurement error is then fused with the reprojection error of the visual odometry to construct a visual-inertial odometry. The visual-inertial odometry here refers to an RGB-D visual-inertial odometry.

[0071] The specific implementation process is: obtain the data of the depth camera, construct the cost function of the camera's reprojection error term, and optimize and estimate its state variables to obtain the first optimized estimation result. Please refer to formula (1) for the specific calculation process:

[0072]

[0073] Among them, wX k is a point in three-dimensional space, x k is the pixel plane coordinate, χ c Is a list of settings, R is the rotation, P is the coordinate point in the camera coordinate system, X is the three-dimensional coordinate point in the world coordinate system, Ev k,j is the cost function of the camera's reprojection error term.

[0074] After obtaining the IMU inertial odometry data, a cost function is constructed and its state is optimized and estimated through nonlinear optimization to obtain the second optimization estimation result. For the specific calculation process, please refer to formula (2):

[0075]

[0076] in, is the cost function of IMU, b g 、b a is the bias of the IMU, the subscript value is the number of points, the subscripts C, B, and W indicate the amount of data in the camera coordinate system, the IMU coordinate system, and the world coordinate system, χi and χ * i It is a list of settings, R is the rotation, v is the velocity, P is the coordinate point in the camera coordinate system, and X is the three-dimensional coordinate point in the world coordinate system.

[0077] Then, the pose estimated by the depth camera is transformed into the coordinate system of the mobile robot, and then a joint objective function is constructed to construct the visual inertial odometry. For the specific calculation process, please refer to formula (3):

[0078] Among them, χ, χ imu , χ cam and χ * refers to each objective function in the joint objective function, b g 、b a It is the bias of the IMU, the subscript value is the number of points, the subscripts C, B, and W indicate the amount of data in the camera coordinate system, the IMU coordinate system, and the world coordinate system. R is the rotation, v is the speed, P is the coordinate point in the camera coordinate system, and X is the three-dimensional coordinate point in the world coordinate system. Visual-Inertial Odometry (VIO), also known as visual-inertial odometry, is sometimes called a visual-inertial system (VINS). It is an algorithm that integrates camera and IMU data to achieve SLAM. It is divided into loose coupling and tight coupling according to the different fusion frameworks. In this application, the visual-inertial odometry is constructed based on a joint objective function, which is a partial parameter variable in the visual-inertial odometry.

[0079]

[0080] S5: Fuse the data from the visual inertial odometry with the UWB data to obtain target positioning data.

[0081] See also Figure 8 , Figure 8 A specific implementation of S5 is shown, which is described in detail as follows:

[0082] S51: Construct an augmented state vector, and calculate an extended Kalman filter based on the augmented state vector.

[0083] S52: Based on the extended Kalman filter, the data from the visual inertial odometry is fused with the UWB data to obtain the target positioning data.

[0084] In an embodiment of the present application, an extended Kalman filter is used to fuse the data from the visual inertial odometry with the UWB data to obtain target positioning data. Furthermore, a state vector of acceleration deviation is added to the three axes on the coordinate axis, and then an augmented state vector is constructed based on the state vector of acceleration deviation. Then, based on the augmented state vector, an extended Kalman filter (EKF) is calculated, and the acceleration from the RGB-D visual perceptual odometry is fused into the UWB data as a control input, and the acceleration deviation is estimated as part of the state vector. Since RGB-D cameras are greatly affected by external factors, their data is unstable. Therefore, when adjusting the covariance matrix, it tends to rely more on UWB sensor data, at the cost of jumps in UWB readings, which will lead to sudden changes in the estimated position. To alleviate these problems, a positioning difference threshold is constructed when calculating the predicted distance and the actual UWB measurement value. If the short-term change is too fast, the positioning result is discarded. The target positioning data can include three-dimensional coordinate points in the world coordinate system and acceleration deviations.

[0085] In this embodiment, the positioning data collected by the sensor is obtained, wherein the positioning data includes RGB image data, depth image data, IMU data and UWB data; based on the RGB image data and the depth image data, a reprojection error is constructed; the IMU pre-integration error between adjacent key frames in the IMU data is obtained; based on the reprojection error and the IMU pre-integration error, a visual inertial odometry is constructed; the data in the visual inertial odometry is fused with the UWB data to obtain target positioning data. The embodiment of the present invention realizes real-time positioning error correction by acquiring positioning data and on-site environmental information, without the need to set landmarks in advance and without restricting the path planning of the mobile robot, realizing application scenarios applicable to a variety of mobile robot positioning; and the present application can also construct a centimeter-level precision positioning system by using an RGB-D camera, an IMU sensor and a UWB sensor, which has the characteristics of low cost, high robustness and high precision, further reducing the manufacturing cost of indoor mobile robots.

[0086] The embodiment of the present invention further provides a positioning device based on multi-sensor fusion, which is used to execute any embodiment of the aforementioned positioning method based on multi-sensor fusion. Figure 9 , Figure 9 A schematic block diagram of a positioning device based on multi-sensor fusion provided by an embodiment of the present invention.

[0087] Among them, Figure 9 As shown, the positioning device 6 based on multi-sensor fusion includes a data acquisition unit 61, a reprojection error construction unit 62, a pre-integration error acquisition unit 63, an odometer construction unit 64 and a target positioning data generation unit 65.

[0088] A data acquisition unit 61 is configured to acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data;

[0089] a reprojection error construction unit 62 for constructing a reprojection error based on the RGB image data and the depth image data;

[0090] A pre-integration error acquisition unit 63 is used to obtain an IMU pre-integration error in the IMU data;

[0091] an odometry construction unit 64 for constructing a visual inertial odometry based on the reprojection error and the IMU pre-integration error;

[0092] The target positioning data generating unit 65 is used to fuse the data in the visual inertial odometer with the UWB data to obtain target positioning data.

[0093] Furthermore, the reprojection error construction unit 62 includes:

[0094] A feature extraction unit is used to extract features from RGB image data to obtain ORB features;

[0095] The matching unit is used to match the ORB features and obtain the matching results;

[0096] A screening unit is used to screen the matching results, eliminate erroneous matching feature points in the matching results, and obtain target matching feature points;

[0097] The error construction unit is used to perform pose estimation based on the target matching points and the depth image data to construct the reprojection error.

[0098] Furthermore, the screening unit includes:

[0099] A comparison unit, used to compare the number of matching feature points in the matching result with a preset value;

[0100] The first screening unit is configured to use the PROSAC algorithm to eliminate incorrect matching feature points in the matching results and obtain target matching feature points if the number of matching feature points is higher than a preset value;

[0101] a distance calculation unit, configured to obtain the distance between each pair of matching feature points in the matching result and the minimum matching feature point distance in the matching result if the number of matching feature points is less than or equal to a preset value;

[0102] The second screening unit is configured to screen the matching results based on the distance between each pair of matching feature points and the minimum matching feature point distance to obtain target matching feature points.

[0103] Furthermore, the error construction unit includes:

[0104] A point cloud generation unit, configured to generate point cloud data by matching the depth image data;

[0105] The pose estimation unit is used to estimate the pose of the target matching points and the depth image data by solving the point pair motion from three dimensions to two dimensions of the point cloud data and the target matching points, and construct a reprojection error.

[0106] Furthermore, the odometer construction unit 64 includes:

[0107] A first optimization estimation result generating unit is used to construct a first cost function of the reprojection error, and perform optimization estimation processing on the state variables of the camera based on the first cost function to obtain a first optimization estimation result;

[0108] a second optimization estimation result generating unit, configured to construct a second cost function based on the IMU pre-integration error, and perform optimization estimation processing on the IMU data based on the second cost function to obtain a second optimization estimation result;

[0109] The joint objective function construction unit is used to transform the sitting posture in the camera coordinate system to the robot coordinate system based on the first optimization estimation result, and to construct a joint objective function based on the second optimization estimation result to construct a visual inertial odometry.

[0110] Furthermore, the target positioning data generating unit 65 includes:

[0111] An augmented state vector construction unit, used to construct an augmented state vector and calculate an extended Kalman filter based on the augmented state vector;

[0112] The data fusion unit is used to fuse the data from the visual inertial odometry with the UWB data based on the extended Kalman filter to obtain target positioning data.

[0113] Furthermore, the data acquisition unit 61 includes:

[0114] An image data acquisition unit, configured to acquire RGB image data and depth image data through an RGB-D camera;

[0115] An IMU data acquisition unit, used to acquire acceleration and angular acceleration data through an accelerometer and a gyroscope to obtain IMU data;

[0116] The UWB data acquisition unit is used to acquire the distance information between the fixed anchor and the mobile terminal through the carrier-free communication technology to obtain UWB data.

[0117] In this embodiment, the positioning data collected by the sensor is obtained, wherein the positioning data includes RGB image data, depth image data, IMU data and UWB data; based on the RGB image data and the depth image data, a reprojection error is constructed; the IMU pre-integration error between adjacent key frames in the IMU data is obtained; based on the reprojection error and the IMU pre-integration error, a visual inertial odometry is constructed; the data in the visual inertial odometry is fused with the UWB data to obtain target positioning data. The embodiment of the present invention realizes real-time positioning error correction by acquiring positioning data and on-site environmental information, without the need to set landmarks in advance and without restricting the path planning of the mobile robot, realizing application scenarios applicable to a variety of mobile robot positioning; and the present application can also construct a centimeter-level precision positioning system by using an RGB-D camera, an IMU sensor and a UWB sensor, which has the characteristics of low cost, high robustness and high precision, further reducing the manufacturing cost of indoor mobile robots.

[0118] The above positioning device based on multi-sensor fusion can be implemented in the form of a computer program. The computer program can be used in Figure 10 Runs on the electronic devices shown.

[0119] See also Figure 10 , Figure 10 5 is a schematic block diagram of a mobile robot provided by an embodiment of the present invention. The mobile robot 500 includes a processor 502 , a memory, and a network interface 505 connected via a device bus 501 , wherein the memory may include a storage medium 503 and an internal memory 504 .

[0120] The storage medium 503 may store an operating device 5031 and a computer program 5032. When the computer program 5032 is executed, the processor 502 may execute a positioning method based on multi-sensor fusion.

[0121] The processor 502 is used to provide computing and control capabilities to support the operation of the entire mobile robot 500.

[0122] The internal memory 504 provides an environment for the operation of the computer program 5032 in the storage medium 503. When the computer program 5032 is executed by the processor 502, the processor 502 can execute a positioning method based on multi-sensor fusion.

[0123] The network interface 505 is used for network communication, such as providing data information transmission. Those skilled in the art will understand that Figure 10The structure shown in the figure is merely a block diagram of a portion of the structure related to the solution of the present invention, and does not constitute a limitation on the mobile robot 500 to which the solution of the present invention is applied. The specific mobile robot 500 may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0124] The processor 502 is configured to run a computer program 5032 stored in the memory to implement the positioning method based on multi-sensor fusion disclosed in the embodiment of the present invention.

[0125] Those skilled in the art will understand that Figure 10 The embodiment of the mobile robot shown in the figure does not constitute a limitation on the specific structure of the mobile robot. In other embodiments, the mobile robot may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently. For example, in some embodiments, the mobile robot may only include a memory and a processor. In such an embodiment, the structure and function of the memory and processor are the same as those in the figure. Figure 10 The embodiments shown are consistent and will not be described again here.

[0126] It should be understood that in the embodiment of the present invention, the processor 502 may be a central processing unit (CPU), and the processor 502 may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0127] In another embodiment of the present invention, a computer-readable storage medium is provided. The computer-readable storage medium may be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium. The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the multi-sensor fusion positioning method disclosed in an embodiment of the present invention.

[0128] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described equipment, devices and units can refer to the corresponding processes in the aforementioned method embodiments, and will not be repeated here. Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented with electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0129] In the several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of units is merely a logical function division. In actual implementation, there may be other division methods, or units with the same function may be combined into one unit. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interfaces, devices or units, or may be an electrical, mechanical or other form of connection.

[0130] Units described as separate components may or may not be physically separate, and components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected based on actual needs to achieve the objectives of the embodiments of the present invention.

[0131] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0132] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the existing technology, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a background server, or a network device, etc.) to execute all or part of the steps of the various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), disk or optical disk, etc. Various media that can store program codes.

[0133] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and such modifications or substitutions are intended to be within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. A positioning method based on multi-sensor fusion, characterized in that: include: Acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data; constructing a reprojection error based on the RGB image data and the depth image data; Obtaining an IMU pre-integration error in the IMU data; Constructing a first cost function of the reprojection error, and performing optimization estimation processing on the state variables of the camera based on the first cost function to obtain a first optimization estimation result; Constructing a second cost function based on the IMU pre-integration error, and performing optimization estimation processing on the IMU data based on the second cost function to obtain a second optimization estimation result; Based on the first optimization estimation result, transforming the sitting posture in the camera coordinate system to the robot coordinate system, and constructing a joint objective function based on the second optimization estimation result to construct a visual inertial odometry; The data in the visual inertial odometry is fused with the UWB data to obtain target positioning data.

2. The positioning method based on multi-sensor fusion according to claim 1, characterized in that: The constructing a reprojection error based on the RGB image data and the depth image data includes: Performing feature extraction on the RGB image data to obtain ORB features; Matching the ORB features to obtain a matching result; By screening the matching results, excluding erroneous matching feature points in the matching results, and obtaining target matching feature points; Performing pose estimation based on the target matching points and the depth image data to construct the reprojection error.

3. The positioning method based on multi-sensor fusion according to claim 2, characterized in that: The step of filtering the matching results and removing incorrect matching feature points in the matching results to obtain target matching feature points includes: Comparing the number of matching feature points in the matching result with a preset value; If the number of matching feature points is higher than the preset value, the PROSAC algorithm is used to eliminate the incorrect matching feature points in the matching result to obtain the target matching feature points; If the number of matching feature points is less than or equal to the preset value, obtaining the distance between each pair of matching feature points in the matching result, and obtaining the minimum matching feature point distance in the matching result; The matching results are screened based on the distance between each pair of matching feature points and the minimum matching feature point distance to obtain the target matching feature points.

4. The positioning method based on multi-sensor fusion according to claim 2, characterized in that: The performing pose estimation based on the target matching point and the depth image data to construct the reprojection error includes: Generating point cloud data by matching the depth image data; The reprojection error is constructed by solving the point pair motion of the point cloud data and the target matching point from three dimensions to two dimensions to estimate the posture of the target matching point and the depth image data.

5. The positioning method based on multi-sensor fusion according to claim 1, characterized in that: The step of fusing the data in the visual inertial odometry with the UWB data to obtain target positioning data includes: Constructing an augmented state vector, and calculating an extended Kalman filter based on the augmented state vector; Based on the extended Kalman filter, the data in the visual inertial odometry is fused with the UWB data to obtain the target positioning data.

6. The positioning method based on multi-sensor fusion according to any one of claims 1 to 5, characterized in that: The acquiring of positioning data collected by the sensor and image data collected by the camera includes: Acquire the RGB image data and the depth image data through an RGB-D camera; Acquire acceleration and angular acceleration data through an accelerometer and a gyroscope to obtain the IMU data; The distance information between the fixed anchor and the mobile terminal is acquired through a carrier-free communication technology to obtain the UWB data.

7. A positioning device based on multi-sensor fusion, characterized in that: include: A data acquisition unit, configured to acquire positioning data collected by the sensor and image data collected by the camera, wherein the positioning data includes IMU data and UWB data, and the image data includes RGB image data and depth image data; a reprojection error construction unit, configured to construct a reprojection error based on the RGB image data and the depth image data; A pre-integration error acquisition unit, configured to acquire an IMU pre-integration error from the IMU data; a first optimization estimation result generating unit, configured to construct a first cost function for the reprojection error, and perform optimization estimation processing on the state variables of the camera based on the first cost function to obtain a first optimization estimation result; a second optimization estimation result generating unit, configured to construct a second cost function based on the IMU pre-integration error, and perform optimization estimation processing on the IMU data based on the second cost function to obtain a second optimization estimation result; a joint objective function construction unit, configured to transform the sitting posture in the camera coordinate system to the robot coordinate system based on the first optimization estimation result, and to construct a joint objective function based on the second optimization estimation result to construct a visual inertial odometry; The target positioning data generating unit is used to fuse the data in the visual inertial odometer with the UWB data to obtain target positioning data.

8. A mobile robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the positioning method based on multi-sensor fusion according to any one of claims 1 to 6 is implemented.

9. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, which, when executed by a processor, causes the processor to perform the positioning method based on multi-sensor fusion according to any one of claims 1 to 6.

Images