Positioning and dense mapping method and device based on multi-sensor fusion, medium and product

Abstract

The invention discloses a positioning and dense mapping method and device based on multi-sensor fusion, a medium and a product, and relates to the field of positioning and three-dimensional reconstruction. The method comprises the following steps: fusing motion data acquired by a motor encoder and an inertial measurement unit based on a Kalman filter; carrying out pixel inspection estimation on a binocular video stream obtained by a binocular camera by adopting a lightweight stereo matching network, and carrying out linear transformation to obtain a depth map; on the basis of the composite objective function, performing pose and mapping optimization processing according to the initialized information data, and determining a key frame on the basis of a set common-view area threshold value; and carrying out joint optimization and global consistency optimization processing on the observation data according to the composite objective function in the key frame by adopting a sliding window strategy, and carrying out global pruning and merging processing by adopting a Gaussian body distribution pruning and merging strategy so as to realize positioning of the mobile robot and three-dimensional reconstruction of dense mapping. The invention aims to realize high-precision positioning and three-dimensional reconstruction.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of positioning and three-dimensional reconstruction, and particularly relates to a positioning and dense mapping method based on multi-sensor fusion, equipment, medium and product. BACKGROUND

[0002] With the increasing demand for laboratory automation, mobile robots are increasingly widely used in sample delivery, experimental operation and other scenarios. However, the laboratory environment usually has the characteristics of high structurization (such as experimental tables, instrument racks), weak texture areas (such as single-color desktops, smooth walls) and high precision requirements (millimeter-level positioning), which poses a severe challenge to the environmental perception and mapping capabilities of the robot. At present, the positioning and mapping methods based on a single sensor have obvious limitations: 1) pure vision SLAM (such as ORB-SLAM3) is prone to lose tracking in weak texture or light change environment, and the depth estimation noise is large; 2) laser radar SLAM (such as LOAM) has high accuracy, but is limited by the reflection interference of transparent objects (such as glassware), and the hardware cost is high, which is difficult to deploy on small robots; 3) inertial navigation (IMU) + odometer can provide high-frequency motion estimation, but there is a cumulative error, which cannot meet the long-time accurate positioning requirement. In addition, the existing dense mapping methods (such as TSDF, point cloud map) face the following problems in the laboratory scene: 1) low computational efficiency: traditional voxel or point cloud mapping requires high computing resources, which is difficult to run in real time on an embedded system; 2) large storage overhead: high-resolution dense maps occupy a large amount of memory, which is not conducive to long-term operation; 3) inflexible geometric expression: existing methods are difficult to efficiently model the regular structures (such as experimental tables, instrument racks) and complex geometries (such as curved containers) in the laboratory. SUMMARY

[0003] The purpose of the present application is to provide a positioning and dense mapping method based on multi-sensor fusion, equipment, medium and product, which can realize high-precision positioning and three-dimensional reconstruction.

[0004] To achieve the above purpose, the present application provides the following solutions:

[0005] In a first aspect, the present application provides a positioning and dense mapping method based on multi-sensor fusion, which is implemented by an environmental perception system applied to a mobile robot; the environmental perception system comprises an inertial measurement unit, a binocular camera, an odometer, a motor encoder and a Kalman filter; wherein the binocular camera and the odometer are arranged in parallel; the motor encoder and the inertial measurement unit are connected with the odometer through the Kalman filter;

[0006] The positioning and dense mapping method based on multi-sensor fusion comprises:

[0007] Fusing motion data collected by a motor encoder and an inertial measurement unit in observation data based on a Kalman filter to obtain fused motion data;

[0008] Performing pixel view estimation on a binocular video stream obtained by a binocular camera in observation data by using a lightweight stereo matching network, and performing linear transformation to obtain a depth map;

[0009] Based on a composite target function, performing pose and mapping optimization processing according to initialized information data, and determining a key frame based on a set common view area threshold; the initialized information data includes: pose estimation initialization data obtained by performing pose estimation based on a mileage counter, and a 3D Gaussian body initialized according to determined 3D Gaussian parameters after back projection of a three-dimensional space based on a depth map;

[0010] By using a sliding window strategy, performing joint optimization and global consistency optimization processing on observation data in the key frame according to the composite target function, and performing global pruning and merging processing by using a Gaussian body distribution pruning and merging strategy, so as to realize positioning and dense mapping three-dimensional reconstruction of a mobile robot.

[0011] In a second aspect, the present application provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor executes the computer program to realize the positioning and dense mapping method based on multi-sensor fusion.

[0012] In a third aspect, the present application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to realize the positioning and dense mapping method based on multi-sensor fusion.

[0013] In a fourth aspect, the present application provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to realize the positioning and dense mapping method based on multi-sensor fusion.

[0014] According to the embodiments provided in the present application, the following technical effects are disclosed:

[0015] The application provides a positioning and dense mapping method based on multi-sensor fusion, equipment, medium and product, motion data collected by motor encoders and inertial measurement units are fused based on a Kalman filter, so that the mobile robot has better robustness in a variable light and low texture environment in the laboratory; a lightweight stereo matching network is used to estimate the pixel view of the binocular video stream obtained by the binocular camera, and linear transformation is performed to obtain a depth map, so that it can be more suitable for a wide range of practical application scenarios; based on a composite target function, the information data is initialized to perform pose and mapping optimization processing, and the key frame is determined based on a set common view area threshold; a sliding window strategy is used to jointly optimize and globally optimize the observation data in the key frame according to the composite target function, and a Gaussian body distribution pruning and merging strategy is used to perform global pruning and merging processing, so as to realize high-precision positioning and dense mapping of the mobile robot. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0017] Figure 1 The flowchart of the positioning and dense mapping method based on multi-sensor fusion;

[0018] Figure 2 The overall flowchart of the positioning and dense mapping method based on multi-sensor fusion in practical application;

[0019] Figure 3 The structural schematic diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0021] The present application realizes high-precision positioning and real-time dense reconstruction in a laboratory environment by tightly coupling the data of IMU, foot / wheel odometry and binocular camera. It provides solid technical support for subsequent autonomous navigation, sample handling and precision operation tasks.

[0022] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0023] In an exemplary embodiment, a positioning and dense mapping method based on multi-sensor fusion is provided, which is implemented using an environmental perception system applied to a mobile robot; the environmental perception system includes: an inertial measurement unit, a binocular camera, an odometer, a motor encoder and a Kalman filter; wherein the binocular camera and the odometer are arranged in parallel; the motor encoder and the inertial measurement unit are connected to the odometer via the Kalman filter.

[0024] Such as Figure 1 As shown, the positioning and dense mapping method based on multi-sensor fusion includes:

[0025] Step 100: Based on the Kalman filter, the motion data collected by the motor encoder and the inertial measurement unit contained in the observation data are fused to obtain fused motion data.

[0026] In one embodiment, the motion data collected by the motor encoder and the inertial measurement unit contained in the observation data are fused based on the Kalman filter to obtain the fused motion data, which specifically includes:

[0027] The motion data collected by the inertial measurement unit is pre-integrated to determine the relative motion constraints between frames. The corresponding expression for the pre-integration process is:

[0028]

[0029] Where, ΔR ij is the rotation pre-integration component; ω k is the reading of the gyroscope at the kth moment; is the bias of the gyroscope in the i-th frame; Δt k is the kth sampling time interval of IMU, that is, the time increment between adjacent samples; i, j, k are all serial numbers; Δv ij is the velocity pre-integrated component; R ik is the rotation from the i-th frame to the k-th frame; a k is the reading of the accelerometer at the kth moment; is the bias of the accelerometer in the i-th frame; Δp ij is the position pre-integrated component; v ik is the velocity term accumulated from time i to time k. In other words, it is the intermediate velocity state obtained by the previous discrete integration. exp(·) is the exponential mapping.

[0030] The motion data collected by the motor encoder is used for kinematic calculation based on the kinematic model to determine the posture change information.

[0031] Based on the Kalman filter, the data fusion is performed according to the inter-frame relative motion constraint and the pose change information to synchronize the initialization of the transformation relationship between adjacent image frames and to determine the fused motion data.

[0032] When the motion data collected by the motor encoder is the motion data collected by the leg joint motor encoder, the motion data is converted into the body pose observation based on the corresponding kinematic model, and the pose change information of the mobile robot in the world coordinate is determined by backstepping the contact relationship between the body and the ground; the kinematic model is a mathematical model for mapping the joint angle and the link length into the three-dimensional position of the foot end based on the forward kinematics relationship.

[0033] The mathematical expression of the kinematic model is:

[0034]

[0035] x=cosθ1·[l1cosθ2+l2cos(θ2+θ3)+l3]。

[0036] y=sinθ1·[l1cosθ2+l2cos(θ2+θ3)+l3]。

[0037] z=-[l1sinθ2+l2sin(θ2+θ3)]。

[0038] Wherein, p foot is the position vector of the robot foot end, which is composed of three coordinate components x, y and z, which is the spatial coordinates of the foot end obtained by the joint angle through the forward kinematics; θ1 is the joint angle in the horizontal direction of the leg; θ2 is the joint angle of the leg extension; θ3 is the joint angle of the leg lifting; x, y and z are three-dimensional spatial position components of the foot end relative to the robot base coordinate system; l1 is the link length corresponding to θ1; l2 is the link length corresponding to θ2; l3 is the link length corresponding to θ3; f(·) is the forward kinematics function.

[0039] When the motion data collected by the motor encoder is the motion data collected by the wheel motor encoder, the corresponding kinematic model is determined according to the angular velocity of each wheel, the wheel track and the wheel diameter, the calculation of the overall linear velocity and the overall angular velocity, and the integral calculation.

[0040] The calculation formula of the overall linear velocity is:

[0041]

[0042] The calculation formula of the overall angular velocity is:

[0043]

[0044] where v is the overall linear velocity; r is the wheel diameter; L is the wheel base; ω is the overall angular velocity; ω1 is the angular velocity of the first wheel; ω2 is the angular velocity of the second wheel; ω3 is the angular velocity of the third wheel; and ω4 is the angular velocity of the fourth wheel.

[0045] Step 200: using a lightweight stereo matching network to perform pixel view estimation on a binocular video stream obtained by a binocular camera included in the observation data, and performing linear transformation to obtain a depth map.

[0046] In an embodiment, the lightweight stereo matching network is used to perform pixel view estimation on a binocular video stream obtained by a binocular camera included in the observation data, and linear transformation is performed to obtain a depth map, specifically including:

[0047] Using a lightweight stereo matching network, feature extraction is performed on left and right images in the binocular video stream, and three-dimensional regularization processing and splicing processing are performed, and based on the correlation pyramid formed, left-right consistency checking and edge perception optimization post-processing are performed, and an initial disparity map is output.

[0048] Based on the context network, feature extraction is performed on the left image in three size dimensions to obtain extracted feature information.

[0049] The initial disparity map and the extracted feature information are input into a convolutional gated recurrent neural network to optimize the generation of the disparity map, and a linear transformation is performed based on the baseline to determine the dense depth estimation, and a depth map is obtained.

[0050] Step 300: based on the composite target function, the pose and mapping optimization processing are performed according to the initialized information data, and the key frame is determined based on the set co-view area threshold. The initialized information data includes: pose estimation initialization data based on pose estimation based on the odometer, and 3D Gaussian body initialization based on the determined 3D Gaussian parameters after the back projection of the three-dimensional space based on the depth map.

[0051] The composite target function includes: image rendering error, IMU pre-integration error and forward kinematics error.

[0052] The mathematical expression corresponding to the composite target function is:

[0053] E total =C vis E visual +(1-C vis )(αE imu +(1-α)E kin )。

[0054] where E total is the composite target function; C visis the visual weight hyper-parameter; E visual is the image rendering error; a is the weight hyper-parameter for adjusting the relative importance of the IMU constraint and the kinematic constraint in the loss function; E imu is the IMU pre-integration error; E kin is the forward kinematic error.

[0055] Step 400: Adopting a sliding window strategy, jointly optimizing the observation data and globally optimizing the consistency within the key frame according to the composite objective function, and adopting a Gaussian body distribution pruning and merging strategy to perform global pruning and merging processing, so as to realize the positioning and dense mapping of the mobile robot.

[0056] The Gaussian body merging strategy is adopted to merge the Gaussian body set whose Euclidean distance in space is less than a set distance and whose color similarity is less than a set value in a weighted average manner with transparency as the weight.

[0057] The method mentioned in the application is applied to the perception problem of a mobile robot (leg type or wheel type) in an automated laboratory scene, and realizes high-precision positioning and three-dimensional reconstruction of an autonomous experimental robot.

[0058] The application realizes high-precision positioning and real-time dense reconstruction in a biological / chemical laboratory environment by tightly coupling the data of an IMU, a foot / wheel odometer and a binocular camera. The method innovatively uses IMU pre-integration to provide high-frequency motion estimation, combines visual features and foot / wheel odometer data to construct a robust pose estimation framework, and effectively reduces cumulative errors; at the same time, an advanced Gaussian splashing scene expression technology is adopted, and a differentiable Gaussian ellipsoid set is used to perform 3D dense characterization of the scene, which not only realizes lightweight dense mapping, but also supports high-fidelity geometric expression, significantly reducing the computational and storage overhead. In addition, for the highly structured features of the laboratory scene, the application introduces geometric constraints in the backend optimization, fully utilizes the prior knowledge of regular structures such as experimental benches and instrument racks, and further improves the positioning and mapping accuracy.

[0059] As shown in Figure 2 , the main process of the method is as follows:

[0060] Firstly, the motion data of an inertial measurement unit (IMU) and a motor encoder are fused through a Kalman filter, wherein the IMU data is processed by pre-integration to generate inter-frame relative motion constraints. In the pre-integration process, the IMU data in a continuous time period is integrated, which eliminates the dependence on accurate pose for each step and reduces the requirement for high-frequency IMU reading.

[0061] Let the i-th frame be t i , and the j-th frame be t jThe pre-integrated quantities are denoted as position pre-integrated quantity (Δp ij ), velocity pre-integrated quantity (Δv ij ) and rotation pre-integrated quantity (Δv ij ), which are calculated as follows:

[0062]

[0063] The leg encoder data is converted into body pose observation through the quadruped robot kinematics model, and the pose change of the robot in the world coordinate system is obtained by backstepping the contact relationship between the body and the ground. The kinematics model is based on forward kinematics relationship, which maps joint angles and link lengths to three-dimensional positions of foot end. Taking one leg as an example, it is a three-degree-of-freedom structure (hip-knee-ankle), and through joint angles θ1, θ2, θ3 and corresponding link lengths l1, l2, l3, the position of the foot end in the body coordinate system can be obtained:

[0064]

[0065] Where the form of f(·) is:

[0066] x = cos θ1·[l1 cos θ2 + L2 cos (θ2 + θ3) + l3].

[0067] y = sin θ1·[l1 cos θ2 + l2 cos (θ2 + θ3) + l3].

[0068] z = -[l1 sin θ2 + l2 sin (θ2 + θ3)].

[0069] Where θ1 controls the horizontal position of the leg (rotation in the plane), i.e. determines the leg swing direction, and θ2, θ3 control the stretching degree and lifting degree of the leg, respectively.

[0070] The wheeled robot is calculated through the kinematics of the four wheel motor encoders, and the angular velocity ω i (i = 1, 2, 3, 4) is read through the motor encoder of each wheel, the overall linear velocity v and the overall angular velocity ω are calculated by combining the wheel diameter r and the wheel base L, and the pose change is obtained by integration.

[0071]

[0072] The fusion process synchronously initializes the pose transformation relationship between adjacent image frames, and provides dynamic adjustment of the multi-sensor weight coefficient for the subsequent tracking link.

[0073] Secondly, the lightweight stereo matching network is used to process the binocular video stream and estimate the pixel-level disparity, and then the depth map is obtained by linear transformation combined with the baseline to initialize the 3D Gaussian body. The specific structure of the network is the improved RAFT-Stereo architecture. The logic of the network is as follows: first, the multi-level Vision Mamba module is used to replace the traditional convolution as the feature extractor (feature network), and the left and right images (left and right input frames) are respectively extracted. After three-dimensional regularization processing, the correlation pyramid is formed by splicing. After left-right consistency test and edge perception optimization, the high-quality initial depth map (initial time difference map) is output. At the same time, the context network is constructed, and the left image (left and right input frames) is extracted for three times with a size of 64x64. These are input into the three-dimensional gated recurrent neural network (convolutional gated recurrent neural network), and finally the disparity map (depth map) is generated by 3D cost volume optimization. After linear transformation according to the baseline, the dense depth estimation is obtained. These depth information is only used to initialize the spatial position parameters of the 3D Gaussian distribution, avoiding directly introducing system error as a supervision signal.

[0074] Thirdly, the RGB pixel points in the binocular image and the corresponding depth values are back projected to the three-dimensional space, and the 3D Gaussian distribution containing the position mean, covariance matrix and transparency parameters is initialized for each spatial point to realize efficient spatial dynamic update and establish the differentiable 3D Gaussian parameters.

[0075] Fourthly, the pose tracking system integrating multi-source sensors is constructed, and the compound objective function containing the visual re-projection error and the IMU pre-integral- forward kinematics error is optimized. The system is based on the sliding window graph optimization, and the compound objective function is constructed in the time window, which contains the following three types of residual terms:

[0076] Image rendering error E visual :

[0077]

[0078] Where I i represents the real image, represents the image rendered from the current frame camera pose T i and the Gaussian voxel set , and the error term measures the difference between the estimated Gaussian voxel scene and the actual observed image.

[0079] IMU pre-integral error E imu :

[0080] The IMU pre-integral model is used to constrain the pose change of adjacent frames, and the inter-frame pre-integral measurement is set as The error term in the optimization is:

[0081]

[0082] where δα ij , δβ ij , δγ ij are the differences between the current estimate and the pre-integrated values.

[0083] The forward kinematic error E kin :

[0084] The residual between the body motion constraint calculated using forward kinematics and the estimated pose is:

[0085]

[0086] where, is the pose calculated from the foot encoder and the kinematic model, is the estimated pose.

[0087] The overall optimization objective function E total is a weighted combination of the residuals:

[0088] E total = λ1E visual + λ2E imu + λ3E kin .

[0089] where λ1, λ2, λ3 are the weight coefficients of each error term. For these weight parameters, by evaluating the confidence of each sensor online, the weight coefficients of vision and inertia are dynamically adjusted, and when visual degradation is detected, the weight ratio of inertial-kinematic odometer fusion is automatically increased. The application introduces a vision confidence evaluation mechanism based on depth map quality for the dense mapping characteristics of Gaussian splash vision-inertial SLAM. Each frame of image estimates a depth map D i based on binocular stereo matching network, calculates the vision confidence based on indicators such as the proportion of effective pixels and depth stability, and determines the vision weight super parameter C vis ∈ [0, 1]:

[0090]

[0091] where N valid represents the number of effective (non-empty, non-extreme value) pixels in the depth map, N total is the total number of pixels, and the confidence is used to dynamically adjust the weight of each error term in the multi-source sensor fusion objective function. The final optimization objective function is:

[0092] E total = C vis E visual + (1-C vis)(αE imu +(1-α)E kin )。

[0093] From the above, in the factor graph optimization, the respective covariance matrix can already adjust the weight, here, two parameters (C vis and α) are set to adjust the weight, because:

[0094] C vis and α do not exist in the standard factor graph optimization formula, in order to adjust the weight of each factor according to the actual situation, the weight hyperparameter that can be manually added / changed is set, that is, in addition to adjusting the weight of each sensor through the covariance matrix in the standard factor graph optimization, the two hyperparameters (C vis and α) are added to adjust more directly.

[0095] When visual degradation leads to the decline of the effectiveness of the depth map (such as low light, texture missing), the system automatically reduces the weight of the visual term, and enhances the role of the IMU and the kinematics term in the pose estimation, to ensure the robustness of the system in complex scenes.

[0096] In the fifth step, the global consistency map optimization is implemented, including local bundle adjustment based on a sliding window, pose graph optimization through sub-maps and global long-term. The sliding window strategy is adopted in the local optimization stage of the sub-map, the recent N frames of key frames and their observations are jointly optimized, and the sum of the visual error, the IMU pre-integration error and the kinematics error is minimized, that is, the optimization objective function above, the global sub-map optimization is to introduce relative pose constraints between multiple local sub-maps, establish a connection across time periods, construct a constraint graph between sub-maps, jointly consider continuous frame constraints and loop constraints, and use a nonlinear least squares method for global consistency optimization.

[0097] According to the characteristics of 3D Gaussian representation, an adaptive Gaussian body distribution pruning and merging strategy is designed to effectively control the computational complexity while maintaining scene details. For Gaussian body pruning, after processing each frame, all 3D Gaussian bodies in the current map are evaluated one by one, and if one of the following two conditions is met:

[0098] The number of observations in the last 5 frames is less than 2.

[0099] Transparency α i <0.2.

[0100] is removed from the map. In addition, a Gaussian body merging strategy is adopted, and if the Euclidean distance in space is less than τ dThe Gaussian body set with the size of =2.5cm and the color similarity (RGB Euclidean distance) less than 15 is merged in a weighted average manner with transparency as a weight, and the new Gaussian kernel can approximate the original information, but significantly reduces the number of points and improves the rendering and optimization speed. The pruning and merging operation is executed once every 10 frames, and at most 5% of the Gaussian bodies in the map are processed each time to prevent sudden changes from affecting the continuity of the map. If the number of Gaussian bodies exceeds 20000, a forced global pruning and merging is triggered immediately to maintain the stability and detail fidelity of the map representation.

[0101] In the sixth step, real-time performance optimization is realized on the embedded platform. By computing task pipelining, the visual front-end processing, pose estimation and map updating are executed in parallel, and finally the real-time running efficiency can be achieved on the Jetson AGX Orin edge computing device, supporting accurate positioning and three-dimensional reconstruction of mobile robots in limited biochemistry laboratory space.

[0102] The application has the following advantages:

[0103] 1. Compared with the traditional Gaussian splash SLAM which highly depends on depth cameras, the binocular stereo matching neural network developed in the application is more suitable for a wide range of practical application scenarios.

[0104] 2. The application fuses IMU, foot / wheel odometry on the basis of visual positioning, so that the robot has better robustness in the laboratory with variable lighting and low-texture environment.

[0105] 3. The application has low requirements for the robot on-board operation platform and needs less hardware resources, but can realize real-time running.

[0106] In an exemplary embodiment, a computer device, which can be a server or a terminal, is provided, and an internal structure diagram of the computer device can be as shown in Figure 3As shown in the figure. The computer device includes a processor, a memory, an input / output interface (I / O for short) and a communication interface. Among them, the processor, the memory and the input / output interface are connected through the system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capability. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the positioning and dense mapping data based on multi-sensor fusion. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with the terminal outside through network connection. The computer program is executed by the processor to implement the positioning and dense mapping method based on multi-sensor fusion.

[0107] Those skilled in the art can understand that, Figure 3 The structure shown in the figure is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. The specific computer device can include more or fewer components than those shown in the figure, or combine certain components, or have a different component arrangement.

[0108] In one exemplary embodiment, a computer device is also provided, including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the steps in the above method embodiments.

[0109] In one exemplary embodiment, a computer readable storage medium is provided, storing a computer program, which is executed by a processor to implement the steps in the above method embodiments.

[0110] In one exemplary embodiment, a computer program product is provided, including a computer program, which is executed by a processor to implement the steps in the above method embodiments.

[0111] It should be noted that the user information (including but not limited to user equipment information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of related data need to comply with relevant regulations.

[0112] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, database or other media used in the embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory may include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0113] The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may include, but are not limited to, general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic units, data processing logic units based on quantum computing, and the like.

[0114] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0115] The principles and implementation manners of the present application are described herein by using specific examples, and the above examples are only used to help understand the method of the present application and its core idea; meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation manners and application ranges will have changes. In conclusion, the content of the specification should not be understood as a limitation of the present application.

Claims

1. A positioning and dense mapping method based on multi-sensor fusion, characterized in that: The system is implemented using an environmental perception system applied to a mobile robot; the environmental perception system includes an inertial measurement unit, a binocular camera, an odometer, a motor encoder, and a Kalman filter; wherein the binocular camera and the odometer are arranged in parallel; the motor encoder and the inertial measurement unit are connected to the odometer via the Kalman filter; The multi-sensor fusion-based positioning and dense mapping method includes: The motion data collected by the motor encoder and the inertial measurement unit contained in the observation data are fused based on the Kalman filter to obtain the fused motion data; A lightweight stereo matching network is used to estimate the pixel view of the binocular video stream obtained by the binocular camera contained in the observation data, and a linear transformation is performed to obtain a depth map; Based on the composite objective function, pose and mapping optimization is performed based on the initialized information data, and key frames are determined based on the set common view area threshold. The initialized information data includes: pose estimation initialization data based on the odometry, and a 3D Gaussian volume initialized according to the determined 3D Gaussian parameters after back-projection into 3D space based on the depth map. A sliding window strategy is adopted to perform joint optimization and global consistency optimization processing on the observation data within the key frame according to the composite objective function, and a Gaussian volume distribution pruning and merging strategy is adopted to perform global pruning and merging processing to achieve mobile robot positioning and three-dimensional reconstruction of dense mapping.

2. The positioning and dense mapping method based on multi-sensor fusion according to claim 1 is characterized in that: The motion data collected by the motor encoder and the inertial measurement unit contained in the observation data are fused based on the Kalman filter to obtain the fused motion data, which specifically includes: The motion data collected by the inertial measurement unit is pre-integrated to determine the relative motion constraints between frames. The corresponding expression for the pre-integration process is: Where, ΔR ij is the rotation pre-integration component; ω k is the reading of the gyroscope at the kth moment; is the bias of the gyroscope in the i-th frame; Δt k is the kth sampling time interval of IMU; i, j, k are all serial numbers; Δv ij is the velocity pre-integrated component; R ik is the rotation from the i-th frame to the k-th frame; a k is the reading of the accelerometer at the kth moment; is the bias of the accelerometer in the i-th frame; Δp ij is the position pre-integrated quantity; v ik is the velocity term accumulated from time i to time k; exp(·) is the exponential mapping; Perform kinematic calculations on the motion data collected by the motor encoder based on the kinematic model to determine the posture change information; Based on the Kalman filter, data fusion is performed according to the relative motion constraints between frames and the posture change information to synchronously initialize the transformation relationship between adjacent image frames and determine the fused motion data.

3. The positioning and dense mapping method based on multi-sensor fusion according to claim 2 is characterized in that: When the motion data collected by the motor encoder is the motion data collected by the leg joint motor encoder, the motion data is converted into body posture observation based on the corresponding kinematic model, and the posture change information of the mobile robot in the world coordinate is determined by inversely deducing the contact relationship between the body and the ground; the kinematic model is a mathematical model determined based on the forward kinematic relationship, which maps the joint angle and the connecting rod length to the three-dimensional position of the foot end; The mathematical expression of the kinematic model is: x=cosθ1·[l1cosθ2+l2cos(θ2+θ3)+l3]; y=sinθ1·[l1cosθ2+l2cos(θ2+θ3)+l3]; z=-[l1sinθ2+l2sin(θ2+θ3)]; Among them, p foot is the position vector of the robot foot; θ1 is the joint angle of the leg in the horizontal direction; θ2 is the joint angle of the leg extension; θ3 is the joint angle of the leg elevation; x, y, and z are the three-dimensional spatial position components of the foot relative to the robot base coordinate system; l1 is the connecting rod length corresponding to θ1; l2 is the connecting rod length corresponding to θ2; l3 is the connecting rod length corresponding to θ3; and f(·) is the forward kinematic function.

4. The positioning and dense mapping method based on multi-sensor fusion according to claim 2, characterized in that: When the motion data collected by the motor encoder is the motion data collected by the wheel motor encoder, the corresponding kinematic model is determined by calculating the overall linear velocity and overall angular velocity based on the angular velocity of each wheel, combined with the wheelbase and wheel diameter, and then performing integral calculation; The calculation formula of the overall linear velocity is: The calculation formula of the overall angular velocity is: Where v is the overall linear velocity; r is the wheel diameter; L is the wheelbase; ω is the overall angular velocity; ω1 is the angular velocity of the first wheel; ω2 is the angular velocity of the second wheel; ω3 is the angular velocity of the third wheel; and ω4 is the angular velocity of the fourth wheel.

5. The positioning and dense mapping method based on multi-sensor fusion according to claim 1, characterized in that: The composite objective function includes: image rendering error, IMU pre-integration error and forward kinematics error; The mathematical expression corresponding to the composite objective function is: AND total =C vis AND visual +(1-C vis )(αE imu +(1-α)E kin ); Among them, E total is the composite objective function; C vis is the visual weight hyperparameter; E visual is the image rendering error; α is the weight hyperparameter used to adjust the relative importance of IMU constraints and kinematic constraints in the loss function; E imu is the IMU pre-integration error; E kin is the forward kinematic error.

6. The positioning and dense mapping method based on multi-sensor fusion according to claim 1, characterized in that: A lightweight stereo matching network is used to estimate the pixel view of the binocular video stream obtained by the binocular camera contained in the observation data, and a linear transformation is performed to obtain a depth map, specifically including: A lightweight stereo matching network is used to extract features from the left and right images in the binocular video stream, perform 3D regularization and splicing processing, and output the initial disparity map through left-right consistency verification and edge-aware optimization based on the constructed correlation pyramid. Based on the context network, the feature extraction of the left image in three dimensions is performed to obtain the extracted feature information; The initial disparity map and the extracted feature information are input into a convolutional gated recurrent neural network to optimize and generate a disparity map, and a dense depth estimate is determined after performing a linear transformation based on the baseline to obtain a depth map.

7. The positioning and dense mapping method based on multi-sensor fusion according to claim 1, characterized in that: The Gaussian volume merging strategy is used to merge Gaussian volumes whose Euclidean distance in space is less than the set distance and whose color similarity is less than the set value in a weighted average manner with transparency as the weight.

8. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the positioning and dense mapping method based on multi-sensor fusion according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the positioning and dense mapping method based on multi-sensor fusion according to any one of claims 1 to 7 is implemented.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the positioning and dense mapping method based on multi-sensor fusion according to any one of claims 1 to 7 is implemented.

Images