Navigation method of visual-inertial indoor robot based on dynamic environment

Abstract

The application discloses a navigation method of a visual inertial indoor robot based on a dynamic environment, and comprises the following steps: a depth camera acquires image information of the robot, pre-integrates an inertial detection unit, detects feature points of an image and tracks an optical flow; a potential dynamic target of the image is detected to generate a mask image; the mask image is predicted, and compensation is made for segmentation failure and slow segmentation operation; the mask depth is set to filter dynamic feature points; information obtained by restoring and pre-integrating the inertial detection unit is corrected in terms of a gyroscope error and an estimated scale; the pre-integration result of the inertial detection unit and feature point information are subjected to nonlinear optimization; the inertial detection unit pre-integrates a pose of a predicted image frame, triangulates feature points and reconstructs; time alignment of key frames and image frames of depth information is performed, and a point cloud is established; a point cloud map is spliced by using an iterative closest point algorithm, and an environment map with navigation information is realized. The application has high positioning and mapping precision in a non-static environment, and has good path planning robustness.

Description

Technical Field

[0001] This invention belongs to the fields of computer vision, communication, and simultaneous localization and mapping (SMR), specifically relating to a navigation method for a visual-inertial indoor robot in a dynamic environment. Background Technology

[0002] Currently, most SLAM map building relies on static environments, but real-world scenarios often involve non-static environments. This poses a significant challenge to robot navigation.

[0003] With ongoing research, researchers have begun to incorporate deep learning and geometric methods. For example, in existing technologies, Zhong Yuanhong et al. proposed a solution based on epipolar geometry constraints in Volume 43, Issue 3, Page 11 of *Computer Engineering and Design*. This solution utilizes the property that the movement of dynamic feature points in the epipolar plane causes a distance between the projected point and the epipolar line for filtering. However, this method has low accuracy and is only applicable to low-dynamic environments; it fails when there are many dynamic targets in the environment. Patent application CN112308921A discloses a method that combines semantic information and epipolar geometry to remove dynamic feature points. It uses a semantic segmentation network to remove feature points of potential dynamic objects in high-dynamic environments, and then removes dynamic feature points in low-dynamic environments based on geometric constraints. This method cannot meet real-time requirements in high-dynamic environments due to network segmentation failures and slow segmentation operation. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a navigation method for visual-inertial indoor robots in dynamic environments, so as to solve the problems of poor accuracy in positioning and mapping and poor robustness of path planning in non-static environments.

[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution.

[0006] A navigation method for a visual-inertial indoor robot in a dynamic environment includes the following steps:

[0007] Step 1: Acquire image information of the indoor robot using a depth camera equipped with an inertial unit, and complete pre-integration processing of the inertial detection unit, feature point detection of the image, and optical flow tracing of the feature points.

[0008] Step 2: The semantic segmentation network performs potential dynamic target detection in the image and generates a mask image; the optical flow estimation network predicts the mask image to compensate for the segmentation failure and slow segmentation operation of the semantic segmentation network; the mask depth is set on the mask image to filter dynamic feature points;

[0009] Step 3: Information is obtained through visual motion structure recovery and pre-integration by the inertial detection unit to correct gyroscope errors, estimation scale, acceleration bias, and gravitational acceleration;

[0010] Step 4: Input the pre-integration result and feature point information of the inertial detection unit after front-end processing, and perform nonlinear optimization; predict the pose of the image frame through the pre-integration of the inertial detection unit, and then triangulate the feature points to complete the reconstruction of the feature points;

[0011] Step 5: Perform time alignment on keyframes and image frames containing depth information, directly build point clouds from image frames containing depth information, and stitch point cloud maps together using an iterative nearest-point algorithm; finally, implement an environment map with navigation information using an octree data structure.

[0012] Specifically, step 1 includes the following process:

[0013] 1.1 Image information with a resolution of 848×480 is acquired at a frequency of 15Hz using a depth camera with an inertial unit mounted on an indoor robot;

[0014] The depth camera described in 1.2 acquires data from the inertial detection unit at a frequency of 250Hz;

[0015] 1.3 Calculate the measurement values ​​of the inertial detection unit; the inertial detection unit includes a three-axis accelerometer and a three-axis gyroscope. Considering white noise and random walk noise, the model of the inertial detection unit is represented as follows:

[0016]

[0017] In the formula, the subscript ω represents the true angular velocity measured by the inertial detection unit; the subscript a represents the acceleration measured by the inertial detection unit; the superscript ω represents the gyroscope; the superscript a represents the accelerometer; the superscript w represents the world coordinate system; the superscript b represents the coordinate system of the inertial detection unit; the subscript b represents the bias; the subscript g represents the gravitational acceleration; and n represents white noise. b represents the rotation amount from the world coordinate system to the inertial detection unit coordinate system; ω Indicates the gyroscope angular velocity bias; b a Indicates acceleration bias;

[0018] 1.4 Establish the differential equations of the motion model of the inertial detection unit; based on Newton's second law of motion and the kinematic model of quaternions, the differential equations of the motion model of the inertial detection unit can be expressed as:

[0019]

[0020] In the formula, t represents time; b tThis represents the coordinate system of the inertial detection unit at time t; This represents the derivative of the translation from the inertial detection unit coordinate system to the world coordinate system with respect to time. This represents the derivative of the rotation amount from the inertial detection unit coordinate system to the world coordinate system with respect to time. This represents the velocity relative to the time coordinate system at time t; This represents the accelerometer reading relative to the time coordinate system at time t. This represents the derivative of the velocity relative to the time coordinate system with respect to the world at time t. This represents the velocity transformed from the inertial detection unit's coordinate system to the world coordinate system; Represents the angular velocity in the coordinate system of the inertial detection unit;

[0021] 1.5 Establish a pre-integral model; by deriving from equation (2) and eliminating quantities related to the world coordinate system, so that the integral term is only related to the previous time step, the pre-integral model can be expressed as:

[0022]

[0023] In the formula, Δt represents the time interval; b j Let j represent the coordinate system of the inertial detection unit at time j; This represents the rotation of the inertial detection unit coordinate system from time i to time t; This represents the translation from the world coordinate system to the inertial detection unit coordinate system at time j; This represents the rotation matrix from the world coordinate system to the inertial detection unit coordinate system;

[0024] For equation (3) Both sides ride at the same time We can obtain:

[0025]

[0026] The pre-integral term of the inertial detection unit is then expressed as:

[0027]

[0028] 1.6 Feature point detection based on accelerated segmentation test; Calculation of the absolute difference between the center and the circumference: Take a pixel on the image, and draw a circle with a radius of 3 pixels centered on this pixel. Then there are 16 pixels on the circumference; Let the brightness of the center pixel be I. P Calculate the difference between the center of the circle and 16 pixels on the circumference; if there are 9 consecutive points on the circumference whose absolute difference with the center pixel is greater than the threshold t, then the center pixel is taken as a feature point.

[0029] 1.7 Feature Point Tracking; Tracking the coordinates of feature points in different images using Lucas-King optical flow; In optical flow, the grayscale of the same pixel is consistent across different images, and its mathematical expression is:

[0030] I(x+dx,y+dy,t+dt)≈I(x,y,t) (6)

[0031] After applying the Taylor approximation and simplifying the formula, it becomes:

[0032]

[0033] In the formula, The gradient of a pixel along the x and y axes is denoted as I. x ,I y ; The translational velocities of a pixel along the x and y axes are denoted as u and v; similarly, The partial derivative of the grayscale value with respect to time is denoted as I. t Then equation (7) can be expressed as:

[0034]

[0035] Based on an m×m window, it is represented as:

[0036]

[0037] In the formula, I tk Let [I] represent the partial derivative of the grayscale value of the k-th pixel at time t with respect to time. x I y ] k This represents the gradient of the k-th pixel within the window along both the x and y axes.

[0038] The pixel coordinates can be solved by establishing the corresponding least squares method for equation (9) using the Ceres library.

[0039] Specifically, step 2 includes the following process:

[0040] 2.1 The semantic segmentation network Mask R-CNN was trained on the COCO dataset, which already has different objects labeled with different colors, to obtain the semantic segmentation weights; the optical flow estimation network PWC-Net was trained on the Sintel dataset to obtain the optical flow weights.

[0041] 2.2 Semantic segmentation of image frames is performed using the Mask-RCNN semantic segmentation network to obtain semantic mask images;

[0042] 2.3 Based on the optical flow estimation network PWC-Net, the optical flow estimation information of each frame of the image is obtained. Then, the pixel coordinates (x, y, z) in the current image frame are calculated. c ,y c Optical flow can be estimated as follows:

[0043] (x c ,y c )=(x p +f x ,y p +f y (10)

[0044] In the formula, f x f represents the pixel displacement along the x-axis from the previous frame to the current frame. y The y-axis displacement of a pixel from the previous frame to the current frame is given by (x). c ,y c () represents the coordinates of the pixel in the previous frame of the image;

[0045] 2.4 Setting the mask depth using a semantic mask image, assuming D k Let be the mask depth of the k-th frame mask image, where The mask depth for each pixel is then The possible values ​​are as follows:

[0046]

[0047] In the formula, No coverage age is the static part of the mask image, Coverage age is the dynamic part of the mask image, and d M This is the maximum depth value of the camera;

[0048] Then, the depth information d corresponding to each detected feature point is compared with the depth value of the corresponding pixel on the semantic mask. The system performs a judgment and filters based on the category, with the judgment criteria as follows:

[0049]

[0050] In the formula, 1 represents a dynamic feature point; 0 represents a static feature point.

[0051] Specifically, step 3 includes the following process:

[0052] 3.1 Image pose transformation to the inertial detection unit coordinate system; through the pose matrix The conversion is performed using the following formula:

[0053]

[0054] In the formula, c0 represents the first camera coordinate system obtained from the image frame, i.e., the visual motion structure recovery reference coordinate system; c k b represents the camera's coordinate system at time k; k This represents the coordinate system of the inertial detection unit at time k; This represents the amount of rotation from the camera coordinate system to the inertial detection unit coordinate system; represents the translation amount from the camera coordinate system to the inertial detection unit coordinate system; s represents the scale factor;

[0055] 3.2 Gyroscope Bias Calibration: Using the pose and velocity information corresponding to the image frames within the sliding window obtained through pre-integration by the inertial detection unit, the gyroscope bias is calculated according to the objective function:

[0056]

[0057] In the formula, B represents the set of image frames; δb ω This represents the error in the amount of change in gravitational acceleration. The true value of the pre-integral; For the relative rotation γ to b ω Differentiate the first-order Jacobian;

[0058] 3.3 For the velocity, scale, and gravity vectors of the coordinate system of the first frame image from the camera, the optimization variables are:

[0059]

[0060] In the formula, This indicates the speed at which the k-th frame of the image was captured; This represents gravity in the coordinate system of the first frame image.

[0061] The pre-integral representation in the c0 coordinate system is:

[0062]

[0063] In equation (16), Let c be the rotation matrix from the inertial detection unit coordinate system to the c0 coordinate system in the k-th frame. Substitute this matrix into the visual motion structure reconstruction... and And transform it to the c0 coordinate system, which is represented as:

[0064]

[0065] Equation (17) can be changed to Hx = b, which can be expressed as:

[0066]

[0067] In the formula, The equation (18) is initialized using a sliding window. A constrained linear equation is then established based on this equation. The velocity, gravity, and scale factor are obtained by solving the least squares problem through Cholesky decomposition. The formula is expressed as:

[0068]

[0069] Specifically, step 4 includes the following process:

[0070] 4.1 Constructing the measurement equation: In the sliding window, its state is defined as follows:

[0071]

[0072] Where X represents the estimated state quantity within the sliding window; n represents the total number of keyframes; x k This indicates the state of the inertial detection unit corresponding to the k-th frame within the sliding window; λ represents the relative pose between the inertial detection unit and the vision system; λ represents the inverse depth of the feature points; m represents the number of feature points.

[0073] 4.2 Residual Equation Construction: The residual equation includes inertial detection unit residual terms, visual residual terms, and marginalization residual terms;

[0074] The residual term of the inertial detection unit is defined as follows:

[0075]

[0076] In the formula, This indicates the IMU pre-integral values ​​containing zero bias of the accelerometer and gyroscope in two consecutive frames within the sliding window; The error represents the amount of positional change; The error representing the change in velocity; The error represents the change in angle; the visual residual projection equation is:

[0077]

[0078] In equation (22), Represents the coordinates of the observed feature point; This represents the image observed in the i-th frame; Indicates the nth time it is observed within the sliding window, when A value of 1 represents the left eye image, and a value of 2 represents the right eye image; This is the camera intrinsic parameter projection matrix;

[0079] right Normalization, expressed as:

[0080]

[0081] In the formula, X c ,Y c Z c Let represent the coordinates in the camera coordinate system. Substituting equation (23) into equation (19), the residual term is expressed as:

[0082]

[0083] The marginalization residual term is defined as follows:

[0084]

[0085] In equation (25), E prior This represents prior information, i.e., the residual information after the sliding window is edged out; The error of the pre-integration term is, in equation (21)

[0086] 4.3 Nonlinear Optimization: The least squares problem is solved using Ceres tools. The optimization of the state variables in the sliding window term is achieved by minimizing the sum of the residual terms. The total cost function is expressed as:

[0087]

[0088] In equation (26), H p The second-order Hesse matrix is ​​used to marginalize prior information pairs with state variables; ρ(·) is the robust kernel function; (Υ) p -H p X) represents the marginalized residual; The pre-integrated residual between two adjacent frames of the sliding window; f represents the residual of the visual reprojection; f represents the image feature points tracked continuously; c j Represents an image frame.

[0089] Specifically, step 5 includes the following process:

[0090] 5.1 Time-align the keyframes after 3D reconstruction with the depth image frames directly obtained from the depth camera to ensure the relative position of the movement trajectory and the constructed map;

[0091] 5.2 Using mask depth, dynamic targets in the depth map are divided into non-buildable regions, and the remaining parts are divided into buildable regions; the buildable pixels in the depth image are projected into 3D space, and outliers are removed; where point cloud is defined as:

[0092]

[0093] In the formula, P represents the target point set; Q represents the source point set;

[0094] The point cloud registration problem is thus transformed into a goal minimization problem; in the source point cloud set, find p i If the nearest point q is found in the target point set, then the formula is:

[0095] ‖q i -p i ||=min (28)

[0096] Solving equation (28) requires calculating the rotation and translation matrix [R|t] on p in equation (27). i Perform a rotation and translation transformation to obtain the corresponding point set p in the target point set. i ′=Rp i +t; then, calculate q using the distance formula. i p i The distance between ′; where the distance formula is as follows:

[0097]

[0098] Finally, by continuously iterating q i p i The calculation stops when the distance between the points makes d less than the threshold ε, or when the maximum number of iterations is reached; otherwise, the calculation continues.

[0099] 5.3. Octree Map Construction: Occupancy of the grid map is represented by voxels; each voxel is represented by eight smaller voxels and can be continuously expanded downwards; representation is achieved by segmenting the point cloud; when the number exceeds a threshold (minimum voxel resolution), further segmentation is performed until the number reaches the threshold; assuming each node uses a probability to represent whether it is occupied or not, with values ​​ranging from [0,1]; let y∈R be the logarithmic probability value, and x be the probability in the range [0,1], then the node occupancy probability is:

[0100]

[0101] The octree map can be updated by obtaining the logarithmic probability values.

[0102] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0103] 1. This invention utilizes semantic segmentation and optical flow estimation networks to segment potential dynamic objects, thereby improving the segmentation speed of potential dynamic objects.

[0104] 2. This invention utilizes the mask depth information set on the semantic mask to filter out dynamic feature points, thereby improving the accuracy of localization.

[0105] 3. This invention utilizes an octree structure for dense graph construction, which compresses the amount of data and reduces the burden on equipment.

[0106] 4. The effectiveness of this invention was verified through the following experiments:

[0107] The proposed method is compared with VINS-Mono: This invention was evaluated on the publicly available visual-inertial dataset OpenLORIS-Scene. It uses Absolute Trajectory Error (ATE) and Relative Pose Error (RPE) to evaluate the overall and local errors of the algorithm. The errors are measured using three metrics: Root Mean Square Error (RMSE), Median, Mean, and Standard Deviation (STD).

[0108] Experiments on the OpenLORIS-Scene dataset show that, compared with VINS-Mono, the proposed method reduces absolute trajectory error and relative error by 41.7% and 78%, respectively, in low-dynamic scenarios. In high-dynamic scenarios, the absolute trajectory error and relative error are reduced by 29.7% and 87.3%, respectively. Comparative experimental results demonstrate that this method improves the positioning accuracy and robustness of navigation systems in dynamic environments. Attached Figure Description

[0109] Figure 1 This is a system framework diagram of a navigation method for a visual-inertial indoor robot based on a dynamic environment, according to the present invention.

[0110] Figure 2 This is a schematic diagram of a sliding window model according to an embodiment of the present invention.

[0111] Figure 3 This is a flowchart illustrating the point cloud registration process according to an embodiment of the present invention.

[0112] Figure 4 This is an octree structure hierarchy diagram of one embodiment of the present invention. Detailed Implementation

[0113] The present invention will now be described in further detail with reference to the accompanying drawings.

[0114] like Figure 1 As shown, the present invention provides a navigation method for a visual-inertial indoor robot in a dynamic environment, comprising:

[0115] Step 1: Acquire image information and inertial measurement unit (IMU) information of the indoor robot through the depth camera equipped with an inertial unit on the indoor robot, and complete the pre-integration processing of the IMU, feature point detection of the image, and optical flow tracing of the feature points.

[0116] Step 2: The semantic segmentation network performs latent dynamic target detection in the image and generates a mask image. The optical flow estimation network predicts the mask image, compensating for segmentation failures and slow segmentation speeds in the semantic segmentation network. A mask depth is set on the mask image to filter dynamic feature points.

[0117] Step 3: Using information obtained from visual structure from motion (SFM) and IMU pre-integration, corrections are made for gyroscope errors, estimation scale, acceleration bias, and gravitational acceleration.

[0118] Step 4: After inputting the IMU pre-integration results and feature point information from the front-end processing, nonlinear optimization is performed. The pose of the image frame is predicted through IMU pre-integration, and then the feature points are triangulated to complete the reconstruction of the feature points.

[0119] Step 5 involves temporally aligning keyframes and image frames containing depth information. A point cloud is then directly constructed using the depth-information image frames, and the point cloud map is stitched together using the Iterative Closest Point (ICP) algorithm. Finally, an environment map with navigation information is implemented using an octree data structure.

[0120] Step 1 specifically includes:

[0121] 1.1 Image information with a resolution of 848×480 is acquired at a frequency of 15Hz using a depth camera with an inertial unit mounted on an indoor robot.

[0122] 1.2 IMU data is acquired at a frequency of 250Hz using a depth camera with an inertial unit mounted on an indoor robot.

[0123] 1.3 Calculate the IMU's measurement values. The IMU includes a three-axis accelerometer and a three-axis gyroscope. Considering white noise and random walk noise, the IMU model is represented as follows:

[0124]

[0125] In the formula, the subscript ω represents the true angular velocity measured by the IMU; the subscript a represents the acceleration measured by the IMU; the superscript ω represents the gyroscope; the superscript a represents the accelerometer; the superscript w represents the world coordinate system; the superscript b represents the coordinate system of the IMU; the subscript b represents the bias; the subscript g represents the gravitational acceleration; and n represents white noise. b represents the rotation amount used to transform from the world coordinate system to the IMU coordinate system; ω Indicates the gyroscope angular velocity bias; b a Indicates acceleration bias;

[0126] 1.4 Establishing the differential equations for the IMU motion model. Based on Newton's second law of motion and the kinematic model of quaternions, it can be expressed as:

[0127]

[0128] In the formula, t represents time; b t Indicates the IMU coordinate system at time t; This represents the time derivative of the translation from the IMU coordinate system to the world coordinate system. This represents the time derivative of the rotation amount from the IMU coordinate system to the world coordinate system. This represents the velocity relative to the time coordinate system at time t; This represents the accelerometer reading relative to the time coordinate system at time t. This represents the derivative of the velocity relative to the time coordinate system with respect to the world at time t. This indicates the velocity of the transformation from the IMU coordinate system to the world coordinate system; This represents the angular velocity in the IMU coordinate system.

[0129] 1.5 Establishing a pre-integral model. By deriving equation (32) and eliminating quantities related to the world coordinate system, ensuring the integral term is only related to the previous time step, the pre-integral model can be expressed as:

[0130]

[0131] In the formula, Δt represents the time interval; b j Indicates the IMU coordinate system at time j; This represents the rotation of the IMU coordinate system from time i to time t; This represents the translation from the world coordinate system to the IMU coordinate system at time j; Represents the rotation matrix from the world coordinate system to the IMU coordinate system;

[0132] For equation (33) Both sides ride at the same time We can obtain:

[0133]

[0134] The pre-integral term of the inertial detection unit is then expressed as:

[0135]

[0136] 1.6 Feature Point Detection Based on Accelerated Segment Test (FAST) and Calculation of the Absolute Difference Between the Center and the Circumference. Take a pixel in the image, and draw a circle with a radius of 3 pixels centered on that pixel. The circle will then contain 16 pixels. Let the brightness of the center pixel be I. P Calculate the difference between the center of the circle and 16 pixels on the circumference. If there are 9 consecutive points on the circumference whose absolute difference from the center pixel is greater than a threshold t, then the center pixel is considered a feature point.

[0137] 1.7 Feature Point Tracking. The coordinates of feature points in different images are tracked using Lucas-Kanade (LK) optical flow. In optical flow, the grayscale of the same pixel remains consistent across different images. The mathematical expression is:

[0138] I(x+dx,y+dy,t+dt)≈I(x,y,t) (36)

[0139] After applying the Taylor approximation and simplifying the formula, it becomes:

[0140]

[0141] In the formula, The gradient of a pixel along the x and y axes is denoted as I. x I y . The translational velocities of a pixel along the x and y axes are denoted as u and v, respectively. Similarly, The partial derivative of the grayscale value with respect to time is denoted as I. t Equation (37) can then be expressed as:

[0142]

[0143] Based on an m×m window, it is represented as:

[0144]

[0145] In the formula, I tk Let [I] represent the partial derivative of the grayscale value of the k-th pixel at time t with respect to time. x I y ] kThis represents the gradient of the k-th pixel within the window in both the x and y directions.

[0146] The pixel coordinates can be solved by establishing the corresponding least squares method for equation (39) using the Ceres library.

[0147] Step 2 specifically includes:

[0148] 2.1 Obtaining the trained weights. The semantic segmentation network Mask R-CNN was trained on the COCO dataset, which already labels different objects with different colors, to obtain the semantic segmentation weights. The optical flow estimation network PWC-Net was trained on the Sintel dataset to obtain the optical flow weights.

[0149] 2.2 Semantic segmentation of image frames is performed using the semantic segmentation network Mask-RCNN to obtain semantic mask images.

[0150] 2.3 Based on the optical flow estimation network PWC-Net, the optical flow estimation information of each frame of the image is obtained. Then, the pixel coordinates (x, y, z) in the current image frame are calculated. c ,y c Optical flow can be estimated as follows:

[0151]

[0152] In the formula, f x f represents the pixel displacement along the x-axis from the previous frame to the current frame. y The y-axis displacement of a pixel from the previous frame to the current frame is given by (x). c ,y c () represents the coordinates of the pixel in the previous frame of the image.

[0153] 2.4 Setting the mask depth using a semantic mask image, assuming D k Let be the mask depth of the k-th frame mask image, where The mask depth for each pixel is then The possible values ​​are as follows:

[0154]

[0155] In the formula, No coverage age is the static part of the mask image, Coverage age is the dynamic part of the mask image, and d M This represents the maximum depth value of the camera.

[0156] Furthermore, the depth information d corresponding to each detected feature point is compared with the depth value of the corresponding pixel on the semantic mask. The system performs a judgment and filters based on the category, with the judgment criteria as follows:

[0157]

[0158] In the formula, 1 represents a dynamic feature point; 0 represents a static feature point.

[0159] Step 3 specifically includes:

[0160] 3.1 Image pose transformation to IMU coordinate system. This is achieved through the pose matrix. The conversion is performed using the following formula:

[0161]

[0162] In the formula, c0 represents the first camera coordinate system obtained from the image frame, i.e., the SFM reference coordinate system; c k b represents the camera's coordinate system at time k; k Let k represent the coordinate system of the IMU at time k; This represents the amount of rotation from the camera coordinate system to the IMU coordinate system. represents the translation amount from the camera coordinate system to the IMU coordinate system; s represents the scale factor.

[0163] 3.2 Gyroscope Bias Calibration. The pose and velocity information corresponding to the image frames within the sliding window, obtained through IMU pre-integration, are used to calculate the gyroscope bias according to the objective function. The objective function is:

[0164]

[0165] In the formula, B represents the set of image frames; δb ω The error representing the change in angular velocity offset; The true value of the pre-integral; For the relative rotation γ to b ω Differentiate the first-order Jacobian;

[0166] 3.3 The velocity, scale, and gravity vectors of the coordinate system for the first frame image from the camera. The optimization variables are:

[0167]

[0168] In the formula, This indicates the speed at which the k-th frame of the image was captured; This represents the gravity in the coordinate system of the first frame image.

[0169] The pre-integral representation in the c0 coordinate system is:

[0170]

[0171] In equation (46), Let c be the rotation matrix from the IMU coordinate system to the c0 coordinate system in the k-th frame; The visual SFM obtained and And transform it to the c0 coordinate system, which is represented as:

[0172]

[0173] Replacing equation (47) with Hx = b, it can be expressed as:

[0174]

[0175] In the formula, The equation (48) is initialized using a sliding window. A constrained linear equation is then established for equation (48). The velocity, gravity, and scale factor are obtained by solving the least squares problem through Cholesky decomposition. The formula is expressed as:

[0176]

[0177] Step 4 specifically includes:

[0178] 4.1 Constructing the measurement equation. In the sliding window, its state is defined as:

[0179]

[0180] Where X represents the estimated state quantity within the sliding window; n represents the total number of keyframes; x k This indicates the IMU state corresponding to the k-th frame within the sliding window; λ represents the relative pose between the IMU and the vision; λ represents the inverse depth of the feature points; m represents the number of feature points.

[0181] 4.2 Residual equation construction; The residual equation includes IMU residual terms, visual residual terms, and marginalization residual terms.

[0182] The IMU residual term is defined as:

[0183]

[0184] In the formula, This represents the pre-integrated value of the inertial detection unit (IMU) in two consecutive frames within the sliding window that contain zero bias of the accelerometer and gyroscope. The error represents the amount of positional change; The error representing the change in velocity; The error represents the change in angle; the visual residual projection equation is:

[0185]

[0186] In equation (52), Represents the coordinates of the observed feature point; This represents the image observed in the i-th frame; Indicates the nth time it is observed within the sliding window, when 1 represents the left eye image, and 2 represents the right eye image; π c -1 This is the camera intrinsic projection matrix.

[0187] right Normalization, expressed as:

[0188]

[0189] In the formula, X c ,Y c Z c Let represent the coordinates in the camera coordinate system. Substituting equation (53) into equation (49), the residual term is expressed as:

[0190]

[0191] The marginalization residual term is defined as follows:

[0192]

[0193] In equation (55), E prior This represents prior information, i.e., the residual information after the sliding window is edged out; The error of the pre-integration term is, in equation (51)

[0194] 4.3 Nonlinear Optimization. The least squares problem is solved using Ceres tools. The optimization of the state variables in the sliding window term is achieved by minimizing the sum of the residual terms. The total cost function is expressed as:

[0195]

[0196] In equation (56), H p The second-order Hesse matrix is ​​used to marginalize prior information pairs with state variables; ρ(·) is the robust kernel function; (Υ) p -H p X) represents the marginalized residual; The pre-integrated residual between two adjacent frames of the sliding window; f represents the residual of the visual reprojection; f represents the image feature points tracked continuously; c j Represents an image frame.

[0197] Step 5 specifically includes:

[0198] 5.1 Time-align the keyframes after 3D reconstruction with the depth image frames obtained directly from the depth camera to ensure the relative position of the movement trajectory and the constructed map.

[0199] 5.2 Using mask depth, dynamic targets in the depth map are divided into non-buildable regions, and the remaining parts are divided into buildable regions. This is achieved by projecting the pixels of the buildable regions in the depth image into 3D space and removing outliers. The point cloud is defined as follows:

[0200]

[0201] In the formula, P represents the target point set; Q represents the source point set.

[0202] The point cloud registration problem is thus transformed into a goal minimization problem. In the source point cloud set, find p... i If the nearest point q is found in the target point set, then the formula is:

[0203] ||q i -p i ||=min (58)

[0204] Solve equation (58). Equation (57) needs to be solved by calculating the rotation and translation matrix [R|t] on p. i Perform a rotation and translation transformation to obtain the corresponding point set p in the target point set. i ′=Rp i +t. Then, calculate q using the distance formula. i p i The distance between ′. The formula for the distance is as follows:

[0205]

[0206] Finally, by continuously iterating q i p i The calculation stops when the distance between the points ' and ' makes d less than the threshold ε, or when the maximum number of iterations is reached. Otherwise, the calculation continues.

[0207] 5.3. Octree-based map construction; voxels represent the occupancy of the grid map. Each voxel is represented by eight smaller voxels and can be continuously expanded downwards. This is achieved by segmenting the point cloud. When the number exceeds a threshold (minimum voxel resolution), further segmentation is performed until the threshold is reached. Assume each node is represented by a probability indicating whether it is occupied or not, with values ​​ranging from [0,1]. Let y∈R be the logarithmic probability, and x be the probability in the range [0,1], then the node occupancy probability is:

[0208]

[0209] The octree map can be updated by obtaining the logarithmic probability values.

Claims

1. A navigation method for a visual-inertial indoor robot in a dynamic environment, characterized in that, Includes the following steps: Step 1: Acquire image information of the indoor robot using a depth camera equipped with an inertial unit, and complete pre-integration processing of the inertial detection unit, feature point detection of the image, and optical flow tracing of the feature points. Step 2: The semantic segmentation network performs potential dynamic target detection in the image and generates a mask image; the optical flow estimation network predicts the mask image to compensate for the segmentation failure and slow segmentation operation of the semantic segmentation network; the mask depth is set on the mask image to filter dynamic feature points; Step 3: Information is obtained through visual motion structure recovery and pre-integration by the inertial detection unit to correct gyroscope errors, estimation scale, acceleration bias, and gravitational acceleration; Step 4: Input the pre-integration results and feature point information of the inertial detection unit after front-end processing, and perform nonlinear optimization; The pose of the image frame is predicted by pre-integration of the inertial detection unit, and then the feature points are triangulated to complete the reconstruction of the feature points. Step 5: Perform time alignment on keyframes and image frames containing depth information, directly build point clouds from image frames containing depth information, and stitch point cloud maps together using an iterative nearest-point algorithm; finally, implement an environment map with navigation information using an octree data structure.

2. The navigation method for a visual-inertial indoor robot based on a dynamic environment according to claim 1, characterized in that, Step 1 includes the following process: 1.1 848 data points were collected at a frequency of 15Hz using a depth camera with an inertial unit mounted on an indoor robot. Image information at 480 resolution; The depth camera described in 1.2 acquires data from the inertial detection unit at a frequency of 250Hz; 1.3 Calculate the measurement values ​​of the inertial detection unit; the inertial detection unit includes a three-axis accelerometer and a three-axis gyroscope. Considering white noise and random walk noise, the model of the inertial detection unit is represented as follows: （1）； In the formula, the parameter Indicates the true angular velocity measured by the inertial detection unit; parameters Indicates the acceleration measured by the inertial detection unit; superscript Indicates gyroscope; superscript Indicates accelerometer; superscript Indicates in the world coordinate system; superscript The coordinate system representing the inertial detection unit; parameters Indicates bias; parameter Represents gravitational acceleration; Represented as white noise; This represents the rotation amount involved in the transformation from the world coordinate system to the inertial detection unit coordinate system. Indicates the gyroscope angular velocity bias; Indicates acceleration bias; 1.4 Establish the differential equations of the motion model of the inertial detection unit; based on Newton's second law of motion and the kinematic model of quaternions, the differential equations of the motion model of the inertial detection unit can be expressed as: （2）； In the formula, Indicates time; express The coordinate system of the inertial detection unit at that moment; This represents the derivative of the translation from the inertial detection unit coordinate system to the world coordinate system with respect to time. This represents the derivative of the rotation amount from the inertial detection unit coordinate system to the world coordinate system with respect to time. express The velocity relative to the time coordinate system at a given moment; express Accelerometer reading relative to the time coordinate system at a given moment; Indicates in The derivative of the velocity relative to the time coordinate system with respect to the world at a given moment; This represents the velocity transformed from the inertial detection unit's coordinate system to the world coordinate system; express The angular velocity of the coordinate system of the inertial detection unit at that moment; 1.5 Establish a pre-integral model; derive from equation (2) and eliminate quantities related to the world coordinate system, so that the integral term is only related to the previous time step, then the pre-integral model can be expressed as: （3）； In the formula, Indicates a time interval; express The coordinate system of the inertial detection unit at that moment; express Time's up The amount of rotation of the coordinate system of the inertial detection unit at any given time; express The translation from the world coordinate system to the inertial detection unit coordinate system at any given moment; This represents the rotation matrix from the world coordinate system to the inertial detection unit coordinate system; For equation (3) Both sides ride at the same time We can obtain: （4）； The pre-integral term of the inertial detection unit is then expressed as: （5）； 1.6 Feature point detection based on accelerated segmentation testing; Calculating the absolute difference between the center and the circumference: Take a pixel on the image, and draw a circle with a radius of 3 pixels centered on that pixel. The circumference will then have 16 pixels. Let the brightness of the center pixel be... Calculate the difference between the center of the circle and 16 pixels on the circumference; if there are 9 consecutive points on the circumference whose absolute difference from the center pixel is greater than a threshold... If so, then the pixel at the center of the circle is taken as the feature point; 1.7 Feature Point Tracking; Tracking the coordinates of feature points in different images using Lucas-King optical flow; In optical flow, the grayscale of the same pixel is consistent across different images, and its mathematical expression is: （6）； After applying the Taylor approximation and simplifying the formula, it becomes: （7）； In the formula, The gradient of a pixel along the x and y axes is denoted as . ; The translational velocity of a pixel along the x and y axes is denoted as . Similarly, The partial derivative of the grayscale value with respect to time is denoted as . Then equation (7) can be expressed as: （8）； based on The window is represented as: （9）； In the formula, Indicates the first in the window 1 pixel The partial derivative of the grayscale value at any given time with respect to time. Indicates the first in the window The gradient of each pixel in the x and y directions; The pixel coordinates can be solved by establishing the corresponding least squares method for equation (9) using the Ceres library.

3. The navigation method for a visual-inertial indoor robot based on a dynamic environment according to claim 1, characterized in that, Step 2, Includes the following processes: 2.1 The semantic segmentation network Mask R-CNN was trained on the COCO dataset, which already has different objects labeled with different colors, to obtain semantic segmentation weights; the optical flow estimation network PWC-Net was trained on the Sintel dataset to obtain optical flow weights. 2.2 Semantic segmentation of image frames is performed using the Mask-RCNN semantic segmentation network to obtain semantic mask images; 2.3 Based on the optical flow estimation information obtained from the PWC-Net optical flow estimation network for each frame of the image, the pixel coordinates in the current image frame are then determined. ; Optical flow can be estimated as follows: （10）； In the formula, This represents the pixel displacement along the x-axis from the previous frame to the current frame. This represents the y-axis displacement of a pixel from the previous frame to the current frame. The coordinates of the pixel in the previous frame image; 2.4 Setting the mask depth using a semantic mask image, assuming... For the first The mask depth of the frame mask image, where The mask depth for each pixel is then The possible values ​​are as follows: （11）； In the formula, This refers to the static portion of the mask image. This refers to the dynamic part of the mask image. This is the maximum depth value of the camera; Then, the depth information corresponding to each detected feature point is... Depth value of the corresponding pixel on the semantic mask The system performs a judgment and filters based on the category, with the judgment criteria as follows: （12）； In the formula, 1 represents a dynamic feature point; 0 represents a static feature point.

4. The navigation method for a visual-inertial indoor robot based on a dynamic environment according to claim 1, characterized in that, Step 3, Includes the following processes: 3.1 Image pose transformation to the inertial detection unit coordinate system; through the pose matrix ( The conversion is performed using the following formula: （13）； In the formula, This represents the first camera coordinate system obtained from the image frame, i.e., the visual motion structure recovery reference coordinate system; express The coordinate system of the camera at that moment; express The coordinate system of the inertial detection unit at that moment; This represents the amount of rotation from the camera coordinate system to the inertial detection unit coordinate system; This represents the translation amount from the camera coordinate system to the inertial detection unit coordinate system; Indicates the scale factor; 3.2 Gyroscope Bias Calibration: Based on the pose and velocity information corresponding to the image frames within the sliding window obtained through pre-integration by the inertial detection unit, the gyroscope bias is calculated according to the objective function, which is: （14）； In the formula, Represents a collection of image frames; The error representing the change in angular velocity offset; The true value of the pre-integral; Relative rotation amount right Differentiate the first-order Jacobian; 3.3 The optimization variables for the velocity, scale, and gravity vectors in the coordinate system of the first frame image from the camera are: （15）； In the formula, Indicates the first The speed at which frame images are captured; This represents gravity in the coordinate system of the first frame image. The pre-integral representation in the coordinate system is: （16）； In equation (16), For the first Frame Inertial Detection Unit Coordinate System to The rotation matrix of the coordinate system; Visual motion structure reconstruction and and convert to In a coordinate system, it is represented as: （17）； Replacing equation (17) with Hx=b, it can be expressed as: （18）； In the formula, The equation (18) is initialized using a sliding window. A constrained linear equation is then established based on this equation. The velocity, gravity, and scale factor are obtained by solving the least squares problem through Cholesky decomposition. The formula is expressed as: （19）。 5. The navigation method for a visual-inertial indoor robot based on a dynamic environment according to claim 1, characterized in that, Step 4, Includes the following processes: 4.1 Constructing the measurement equation: In the sliding window, its state is defined as follows: （20）； in, This represents the estimated state variables within the sliding window; Indicates the total number of keyframes; Indicates the first [number]th ... The state of the inertial detection unit corresponding to the frame; This indicates the relative pose between the inertial detection unit and the vision system; Indicates the inverse depth of the feature point; Indicates the number of feature points; 4.2 Residual Equation Construction: The residual equation includes inertial detection unit residual terms, visual residual terms, and marginalization residual terms; The residual term of the inertial detection unit is defined as follows: （21）； In the formula, This represents the pre-integrated value of the inertial detection unit (IMU) in two consecutive frames within the sliding window that contain zero bias of the accelerometer and gyroscope. The error represents the amount of positional change; The error representing the change in velocity; The error represents the change in angle; the visual residual projection equation is: （22）； In equation (22), Represents the coordinates of the observed feature point; This represents the image observed in the i-th frame; Indicates the nth time it is observed within the sliding window, when A value of 1 represents the left eye image, and a value of 2 represents the right eye image; The camera intrinsic projection matrix; right Normalization, expressed as: （23）； In the formula, Let represent the coordinates in the camera coordinate system. Substitute equation (23) into equation (19): (19); The residual term is then expressed as: （24）； The marginalization residual term formula is defined as follows: （25）； In equation (25), This represents prior information, i.e., the residual information after the sliding window is edged out; The error of the pre-integration term is, in equation (21) ; 4.3 Nonlinear Optimization: The least squares problem is solved using the Ceres tool. The optimization of the state variables in the sliding window term is achieved by minimizing the sum of the residual terms. The total cost function is expressed as: （26）； In equation (26), To marginalize prior information on the second-order Hesse matrix of the state variables; For robust kernel functions; For marginalized residuals; The pre-integrated residual between two adjacent frames of the sliding window; The residual of visual reprojection; For continuously tracked image feature points, Represents an image frame.

6. The navigation method for a visual-inertial indoor robot based on a dynamic environment according to claim 1, characterized in that, Step 5, as described above Includes the following processes: 5.1 Time-align the keyframes after 3D reconstruction with the depth image frames directly obtained from the depth camera to ensure the relative position of the movement trajectory and the constructed map; 5.2 Using mask depth, dynamic targets in the depth map are divided into non-buildable regions, and the remaining parts are divided into buildable regions; the pixels of the buildable regions in the depth image are projected into 3D space, and outliers are removed; where the point cloud is defined as: （27）； In the formula, This indicates that the target points are clustered together; This indicates a cluster of source points; The point cloud registration problem is thus transformed into a goal minimization problem; In the source point cloud cluster, find Find the nearest point in the target point set. The formula is: （28）； Solving equation (28) requires calculating the rotation and translation matrix of equation (27). right Perform rotation and translation transformations to obtain the corresponding point set in the target point set. Then, calculate using the distance formula. , The distance between; where the distance formula is as follows: （29）； Finally, through continuous iteration , The distance between them makes Less than the threshold If the maximum number of repetitions is reached, the calculation stops; otherwise, the calculation continues. 5.

3. Octree-based map construction: The occupancy of the grid map is represented by voxels; each voxel is represented by eight smaller voxels and can be continuously expanded downwards; it is represented by segmenting the point cloud; When the number exceeds the threshold, i.e., the minimum voxel resolution, further cutting is performed until the number reaches the threshold; assuming each node uses a probability to represent whether the current node is occupied or not, with a value range of [0,1]; let... The logarithmic value of the probability. If the probability is [0,1], then the node occupancy probability is: （30） The octree map can be updated by obtaining the logarithmic probability values.

Images