Binocular vision inertial navigation system parameter optimization method, device and equipment and storage medium

Abstract

The application discloses a binocular vision inertial navigation system parameter optimization method and device, equipment and a storage medium, and the method comprises the steps of: dividing a current image frame into a plurality of grid regions, performing feature point detection on each grid region, screening effective feature grids from the grid regions, performing feature point uniform distribution processing on the effective feature grids, obtaining stable feature points, calculating the associated depth information corresponding to each stable feature point by using triangulation, constructing a to-be-optimized variable set and a target optimization model based on the stable feature points and the associated depth information, and inputting the to-be-optimized variable set into the target optimization model to perform parameter optimization. Since the effective feature grids are screened and the feature points are uniformly distributed, the adverse effects of motion blur and noise on the feature points are effectively reduced, the depth calculation discontinuity problem caused by the feature point confusion is greatly reduced, and through parameter optimization, the scale drift problem of the visual tracking of the binocular vision inertial navigation system is effectively solved.

Description

Technical Field

[0001] This invention relates to the field of visual inertial systems, and more particularly to a method, apparatus, device, and storage medium for optimizing parameters of a binocular visual inertial navigation system. Background Technology

[0002] Cameras and inertial measurement units (IMUs) have become crucial sensor components in Simultaneous Localization and Mapping (SLAM) systems due to their low cost, small size, and lightweight characteristics. Cameras provide rich environmental image information to SLAM systems, while IMUs provide key data such as motion state and attitude estimation by measuring acceleration and angular velocity in real time. The combination of these two technologies offers a feasible path for the development of SLAM systems towards lightweight and low-cost designs. Based on this, visual-inertial SLAM technology, also known as a visual-inertial navigation system (VINS), was developed.

[0003] In VINS (Vision Inertial Navigation System) built on binocular cameras, the system performance is constrained by the inherent characteristics of the cameras themselves due to the over-reliance on the cameras as the source of external information. Furthermore, the relatively short measurement range of binocular cameras makes it difficult for the system to obtain accurate and long-range depth information estimates, which limits the accuracy of 3D reconstruction in large-scale environments and gradually becomes an obstacle to improving pose estimation accuracy. Current visual inertial systems suffer from discontinuous depth information estimation due to factors such as motion blur and noise, and also exhibit scale drift problems in visual tracking. Summary of the Invention

[0004] The main objective of this invention is to provide a method, apparatus, device, and storage medium for optimizing parameters of a binocular visual inertial navigation system, aiming to solve the technical problems of discontinuous depth information estimation and scale drift in visual tracking caused by factors such as motion blur and noise in existing visual inertial systems.

[0005] To achieve the above objectives, the present invention provides a parameter optimization method for a binocular vision inertial navigation system. This method is applied to a binocular vision inertial navigation system and includes the following steps:

[0006] Divide the current image frame into multiple grid regions;

[0007] Feature points are detected in each grid region based on a feature detection algorithm. Valid feature grids are selected from the grid regions. The valid feature grids satisfy the grid feature conditions and contain multiple target feature points. Each target feature point satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm.

[0008] The target feature points in the effective feature grid are uniformly distributed to obtain stable feature points, and triangulation is used to calculate the associated depth information corresponding to each stable feature point.

[0009] Based on the stable feature points and the associated depth information, a set of variables to be optimized and a target optimization model are constructed. The set of variables to be optimized includes the inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point.

[0010] The set of variables to be optimized is input into the target optimization model for parameter optimization. Based on the parameter optimization results output by the target optimization model, the current state parameters of the binocular visual inertial navigation system are updated. The updated current state parameters are then used as the new set of variables to be optimized, and the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization is repeated.

[0011] Optionally, the step of constructing the set of variables to be optimized and the target optimization model based on the stable feature points and the association depth information includes:

[0012] Based on the associated depth information, obtain the three-dimensional coordinates of the stable feature point in the world coordinate system;

[0013] Based on the associated depth information, inverse depth information of each stable feature point is generated, and a set of variables to be optimized is constructed based on the inverse depth information. The set of variables to be optimized is represented as follows:

[0014]

[0015] in, Represents the set of variables to be optimized. Represents an inertial state sequence. Indicates the first The inertial sensor state of the frame. Indicates the first Frame-time inertial sensor in world coordinate system The position in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The speed in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The posture in the middle, Indicates acceleration bias. This indicates the gyroscope bias. This indicates the relative attitude relationship between the stereo camera and the inertial sensor. Representing the camera coordinate system In the inertial sensor coordinate system The position in the middle, Representing the camera coordinate system In the IMU coordinate system The posture in the middle, Represents the inverse depth information sequence. Indicates the first Inverse depth information of a stable feature point;

[0016] Based on the set of variables to be optimized and the three-dimensional coordinates of the stable feature points, a visual reprojection error residual term is constructed, which includes:

[0017]

[0018] in, This represents the visual reprojection error residual term. Indicates that the camera is from the first Frame motion to the first The frame's feature observables, wherein the feature observables are the two-dimensional coordinates of stable feature points. Represents the camera projection model. Represents the stable feature point. The three-dimensional coordinates of the frame Represents inverse depth information;

[0019] Construct the IMU pre-integration error residual term;

[0020] A target optimization model is constructed based on the visual reprojection error residual term and the IMU pre-integration error residual term.

[0021] Optionally, the construction of the IMU pre-integration error residual term includes:

[0022] The IMU pre-integration error residual term is constructed based on the inertial data of consecutive adjacent image frames within a sliding window of a binocular vision inertial navigation system. The IMU pre-integration error residual term is expressed as follows:

[0023]

[0024] in, This represents the IMU pre-integration error residual. Indicates system status information. Indicates the inertial sensor from the first Frame to the The pre-integral measurement of the frame, Indicates the inertial sensor from the first Frame to the Frame rotation residuals Indicates the inertial sensor from the first Frame to the Frame velocity residual, Indicates the inertial sensor from the first Frame to the Frame position residual, This indicates that the accelerometer has zero bias residual. This indicates that the gyroscope has zero bias residual. Indicates the world coordinate system rotation. Rotation matrix of the frame IMU coordinate system and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame location information, The gravitational acceleration vector in the world coordinate system. The time interval between two IMU measurement frames, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame speed information, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame quaternion pose information, This represents a function that converts a quaternion into an axis-angle vector representation. Quaternion information for rotational residuals, and They represent the first Frame and the Zero bias of the accelerometer in the frame. and They represent the first Frame and the Zero bias of the gyroscope in the frame.

[0025] Optionally, the step of constructing the target optimization model based on the visual reprojection error residual term and the IMU pre-integration error residual term includes:

[0026] A target optimization model is constructed using a sliding window optimization strategy, the visual reprojection error residual term, and the IMU pre-integration error residual term. The target optimization model is expressed as follows:

[0027]

[0028] in, This represents the objective optimization model function. Indicates marginalized residuals, This represents the IMU pre-integration error residual term. This represents the visual reprojection error residual term. This represents the Huber loss function. The measurement estimation matrix represents the marginalization. This represents the covariance matrix of each term. This represents the set of stable feature points observed in the current sliding window. This represents the set of all IMU measurements.

[0029] Optionally, the step of constructing the target optimization model using the sliding window optimization strategy, the visual reprojection error residual term, and the IMU pre-integration error residual term includes:

[0030] An initial optimization model is constructed using a sliding window optimization strategy, the visual reprojection error residual term, and the IMU pre-integration error residual term.

[0031] A quality assessment is performed on each stable feature point to determine the observation quality of each stable feature point, and a weighting factor for each stable feature point is configured based on the observation quality.

[0032] The visual reprojection error residual term is updated based on the weighting factor. The updated visual reprojection error residual term includes:

[0033]

[0034] in, Represents stable feature points The corresponding weighting factor;

[0035] The initial optimization model is adjusted based on the updated visual reprojection error residual term to obtain the target optimization model, which is expressed as:

[0036]

[0037] in, This represents the updated visual reprojection error residual term.

[0038] Optionally, the step of detecting feature points in each grid region based on the feature detection algorithm and selecting valid feature grids from the grid regions includes:

[0039] Feature points are detected in each grid region based on the feature detection algorithm to determine the candidate feature points and the response values ​​of each candidate feature point in each grid region.

[0040] Select target feature points whose response values ​​are not lower than the response threshold from the candidate feature points;

[0041] Texture filtering is performed on each grid region to determine candidate grids whose texture features satisfy the texture feature conditions in the grid region;

[0042] Determine whether the number of target feature points in each candidate grid reaches the feature point number threshold;

[0043] The grids in the candidate grids whose number of target feature points reaches the feature point number threshold are considered as valid feature grids.

[0044] Based on the feature detection algorithm, re-detect the target feature points in the candidate grids where the number of target feature points has not reached the feature point number threshold, add the newly detected target feature points to the candidate grids, and return to execute the step of determining whether the number of target feature points in each candidate grid has reached the feature point number threshold.

[0045] Optionally, the step of calculating the associated depth information corresponding to each stable feature point using triangulation includes:

[0046] Based on the disparity information and camera calibration parameters of the binocular camera, triangulation is used to calculate the initial depth information corresponding to each stable feature point:

[0047]

[0048] in, Indicates initial depth information. Indicates the distance between the optical centers of the left and right cameras. This represents the distance from the optical center of the camera to the two-dimensional plane. and Representing three-dimensional points respectively Projected coordinates on the left and right camera planes;

[0049] Stable feature points whose initial depth information does not exceed the effective depth range are marked as normal depth feature points, and the initial depth information of the normal depth feature points is used as associated depth information.

[0050] Stable feature points whose initial depth information exceeds the effective depth range are designated as abnormal depth feature points. The initial depth information of the abnormal depth feature points is cleared to zero, and the initial depth information of the abnormal depth feature points is recalculated.

[0051] Furthermore, to achieve the above objectives, the present invention also proposes a parameter optimization device for a binocular vision inertial navigation system. The device is applied to a binocular vision inertial navigation system and includes:

[0052] The grid division module is used to divide the current image frame into multiple grid regions;

[0053] The feature detection module is used to detect feature points in each grid region based on the feature detection algorithm, and to select effective feature grids from the grid region. The effective feature grids satisfy the grid feature conditions and contain multiple target feature points. Each target feature point satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm.

[0054] The feature homogenization module is used to homogenize the distribution of each target feature point in the effective feature grid to obtain stable feature points, and to calculate the associated depth information corresponding to each stable feature point using triangulation.

[0055] An optimization construction module is used to construct a set of variables to be optimized and a target optimization model based on the stable feature points and the associated depth information. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point.

[0056] The parameter optimization module is used to input the set of variables to be optimized into the target optimization model for parameter optimization, update the current state parameters of the binocular vision inertial navigation system based on the parameter optimization results output by the target optimization model, and use the updated current state parameters as a new set of variables to be optimized, and re-execute the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization.

[0057] Furthermore, to achieve the above objectives, this application also proposes a device for optimizing parameters of a binocular vision inertial navigation system. The device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. The computer program is configured to implement the steps of the binocular vision inertial navigation system parameter optimization method described above.

[0058] In addition, to achieve the above objectives, this application also proposes a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the binocular vision inertial navigation system parameter optimization method described above.

[0059] In addition, to achieve the above objectives, this application also provides a computer program product, which includes a computer program that, when executed by a processor, implements the steps of the binocular vision inertial navigation system parameter optimization method described above.

[0060] This invention divides the current image frame into multiple grid regions, detects feature points in each grid region based on a feature detection algorithm, and selects effective feature grids from these regions. These effective feature grids satisfy grid feature conditions and contain multiple target feature points, each satisfying a feature point response value condition. The feature detection algorithm is based on the ORB algorithm and performs uniform distribution processing on the target feature points in the effective feature grids to obtain stable feature points. Triangulation is then used to calculate the associated depth information corresponding to each stable feature point. Based on the stable feature points and the associated depth information, a set of variables to be optimized and a target optimization model are constructed. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the stereo camera and the inertial sensor, and the inverse depth information sequence of each stable feature point. The set of variables to be optimized is input into the target optimization model for parameter optimization. The current state parameters of the binocular vision inertial navigation system are updated based on the parameter optimization results output by the target optimization model. The updated current state parameters are then used as a new set of variables to be optimized, and the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization is repeated. This invention improves the accuracy of system scale observation by dividing the image frame into grids, then selecting effective feature grids and homogenizing feature points in each grid unit, and then associating depth information with the homogenized feature points. This results in more accurate feature tracking, effectively reducing the adverse effects of motion blur and noise on feature points, and significantly reducing the problem of discontinuous depth calculation caused by feature point chaos. Through parameter optimization, the scale drift problem of visual tracking in the binocular vision inertial navigation system is effectively solved, thus better adapting to different scenes and environmental changes and improving the navigation accuracy of the binocular vision navigation system. Attached Figure Description

[0061] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 This is a schematic diagram of the structure of a binocular vision inertial navigation system parameter optimization device for the hardware operating environment involved in the embodiments of the present invention;

[0063] Figure 2 This is a flowchart illustrating an embodiment of the parameter optimization method for a binocular vision inertial navigation system according to the present invention.

[0064] Figure 3 This is a system structure block diagram of a binocular vision inertial navigation system according to an embodiment of the present invention;

[0065] Figure 4This is a schematic diagram of grid feature extraction in one embodiment of the present invention;

[0066] Figure 5 This is a schematic diagram of binocular depth detection in one embodiment of the present invention;

[0067] Figure 6 This is a structural block diagram of an embodiment of the parameter optimization device for a binocular vision inertial navigation system of the present invention.

[0068] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0069] It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0070] Reference Figure 1 , Figure 1 This is a schematic diagram of the device structure for optimizing the parameters of a binocular vision inertial navigation system in the hardware operating environment involved in the embodiments of the present invention.

[0071] like Figure 1 As shown, the parameter optimization device for the binocular vision inertial navigation system may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wireless-Fidelity (Wi-Fi) interface). The memory 1005 may be high-speed random access memory (RAM) or stable non-volatile memory (NVM), such as a disk storage device. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0072] Those skilled in the art will understand that Figure 1 The structure shown does not constitute a limitation on the parameter optimization device for a binocular vision inertial navigation system, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0073] like Figure 1As shown, the memory 1005, which is a computer-readable storage medium, may include an operating system, a network communication module, a user interface module, and a parameter optimization program for a binocular vision inertial navigation system.

[0074] exist Figure 1 In the binocular vision inertial navigation system parameter optimization device shown, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and memory 1005 in the binocular vision inertial navigation system parameter optimization device of the present invention can be set in the binocular vision inertial navigation system parameter optimization device. The binocular vision inertial navigation system parameter optimization device calls the binocular vision inertial navigation system parameter optimization program stored in the memory 1005 through the processor 1001 and executes the binocular vision inertial navigation system parameter optimization method provided in the embodiment of the present invention.

[0075] This invention provides a method for optimizing parameters of a binocular vision inertial navigation system, referring to... Figure 2 , Figure 2 This is a flowchart illustrating an embodiment of the parameter optimization method for a binocular vision inertial navigation system according to the present invention.

[0076] In this embodiment, the binocular vision inertial navigation system parameter optimization method is applied to a binocular vision inertial navigation system, and the binocular vision inertial navigation system parameter optimization method includes the following steps:

[0077] Step S10: Divide the current image frame into multiple grid regions.

[0078] It should be noted that this embodiment applies to a binocular visual inertial navigation system, which may include a visual inertial odometry, a feature tracking module, and a sliding window optimization module. The feature tracking module is the core of the visual front-end processing, primarily responsible for detecting, matching, and tracking feature points in image sequences. It extracts features from images and provides key input to the visual inertial odometry. The visual inertial odometry integrates inertial data (IMU measurements) and visual data (camera images) to process the system's real-time attitude data. The sliding window optimization module starts with the initial pose estimated by the front-end and employs a tightly coupled nonlinear optimization method to jointly optimize data from multiple sensors within a fixed time window, ensuring the system's long-term accuracy and global consistency.

[0079] Reference Figure 3 , Figure 3The diagram shows the system architecture of a binocular vision-inertial navigation system in one embodiment. The system employs the SFM algorithm to fuse IMU and visual information to obtain initial values ​​including sensor pose, velocity, IMU bias, gravity direction, and 3D positions of feature points for initialization. Based on prior information from image frames and pre-integrated residuals, rotational extrinsic parameters and gyroscope bias are estimated. Simultaneously, coarse initial estimates of gravity, scale, and velocity information are performed. Finally, visual-inertial data alignment and joint initialization are conducted. After initialization, the camera and IMU states and biases are continuously estimated within a sliding window based on local VIO. In this sliding window-based VIO system, to address the accumulation of errors caused by sensor and computational accuracy errors, the binocular vision-inertial navigation system uses a global pose graph optimization algorithm based on the Gaussian-Newton and Levenberg-Marquadt methods to distribute errors across all poses. In common applications such as handheld devices and drones, since the accelerometer in the IMU can directly estimate the roll and pitch angles of the vehicle by measuring the direction of gravity, only pose optimization of the four degrees of freedom (x, y, z axes and yaw angle) needs to be considered in global attitude map optimization. Binocular vision inertial navigation systems, through improved feature tracking and optimization strategies, significantly enhance the stability and accuracy of the system in complex environments such as low-texture scenes and motion blur.

[0080] It should be understood that the executing entity of this embodiment can be a computing service device with data processing, network communication, and program execution functions, such as a tablet computer, personal computer, or mobile phone, or a terminal electronic device capable of performing the above functions. The following description uses a binocular vision inertial navigation system parameter optimization device (hereinafter referred to as the optimization device) as an example to illustrate this embodiment and the following embodiments.

[0081] It is understood that this embodiment can use a grid-based management method to filter and track feature points, thereby ensuring the quality of continuously tracked feature points, reducing the impact of noise, occlusion, etc. on feature points, avoiding feature tracking interruption, and preventing discontinuous depth information tracking and scale drift.

[0082] In some embodiments, the optimization device may divide a visual image frame into several regions of the same size according to the number of grids and the size of a preset image, and initialize parameters such as the threshold number of feature points within the grid.

[0083] Step S20: Based on the feature detection algorithm, feature points are detected in each grid region, and valid feature grids are selected from the grid regions.

[0084] It should be noted that in a binocular vision-inertial navigation system, feature points are one of the core inputs of visual inertial odometry (VIO), used to characterize salient structural information in the image and to estimate camera motion through cross-frame matching. This embodiment uses a gridded management approach to control the uniform distribution of image feature points, thereby achieving feature point homogenization and improving the robustness and efficiency of feature extraction.

[0085] It should be noted that the effective feature grid satisfies the grid feature condition and contains multiple target feature points, each of which satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm.

[0086] In some embodiments, the mesh feature condition may be that the texture feature value of the mesh region reaches a texture threshold, and the number of feature points in the mesh region reaches a number threshold. The above response value condition may be that the response value of the feature points in the mesh region reaches a response threshold.

[0087] In some embodiments, the optimization device can divide the visual image frame into several regions of the same size according to the number of grids and the size of a preset image, and initialize parameters such as the threshold of the number of feature points in the grid; then, the ORB algorithm is used to detect feature points in each grid region to determine candidate feature points in each grid region, and feature points are filtered based on the response values ​​of each candidate feature point to determine the target feature points in each grid region that meet the response value conditions. Finally, effective feature grids that meet the grid feature conditions are filtered from the grid regions based on the texture feature values ​​and the number of target feature points in each grid region.

[0088] Furthermore, to improve the stability of feature tracking and enhance the robustness and efficiency of feature extraction, in one embodiment, step S20 may include:

[0089] Step S201: Based on the feature detection algorithm, perform feature point detection on each grid region to determine the candidate feature points in each grid region and the response value of each candidate feature point;

[0090] Step S202: Select target feature points from the candidate feature points whose response values ​​are not lower than the response threshold;

[0091] Step S203: Perform texture filtering on each grid region to determine candidate grids whose texture features satisfy the texture feature conditions in the grid region;

[0092] Step S204: Determine whether the number of target feature points in each candidate grid has reached the feature point number threshold;

[0093] Step S205: Select the grids in the candidate grids whose number of target feature points reaches the feature point number threshold as valid feature grids;

[0094] Step S206: Based on the feature detection algorithm, re-detect the target feature points in the candidate grids where the number of target feature points has not reached the feature point number threshold, add the newly detected target feature points to the candidate grids, and return to execute the step of determining whether the number of target feature points in each candidate grid has reached the feature point number threshold.

[0095] In the specific implementation, the optimized device uses the ORB algorithm to detect feature points in each grid region. ORB quickly locates candidate feature points through improved FAST corner detection, then calculates the principal direction of the feature points using the gray-scale centroid method to achieve rotation invariance, and finally uses the rBRIEF descriptor to generate a rotation-invariant binary feature description. Detected feature points are filtered based on the score calculated for each candidate point by the feature detection algorithm, i.e., the response value, to ensure the quality of feature points within each grid. During feature point detection, a texture threshold is used to determine whether the image region has sufficient texture to extract effective features. Low-texture regions typically cannot provide stable feature points, so cells with low texture thresholds that cannot detect effective features are skipped to avoid extracting particularly poor-quality feature points. Furthermore, for grids where the number of feature points does not reach the threshold, the system will re-detect and add the newly detected points to the corresponding grid, thus ensuring a sufficient number and uniform distribution of feature points in the image. Figure 4 As shown, Figure 4 This is a schematic diagram of grid feature extraction, where black dots are the initially detected feature points and white dots are the feature points added by the grid detector.

[0096] Step S30: Perform uniform distribution processing on each target feature point in the effective feature grid to obtain stable feature points, and use triangulation to calculate the associated depth information corresponding to each stable feature point.

[0097] Understandably, in this embodiment, feature depth association can be achieved by converting 2D feature points into 3D spatial points through triangulation or binocular parallax, establishing a reliable 2D-3D correspondence and providing geometric constraints for subsequent BA optimization. By managing and controlling the image through gridding to uniformly distribute feature points, stable feature points are obtained, and their depth information is calculated using triangulation.

[0098] In some embodiments, the optimization device can segment image frames by establishing a grid detector, remove abnormal feature points based on feature response values, and re-extract features for grids with insufficient features. Then, on a unit basis, the features in each grid are associated with the depth information initially obtained by the stereo camera. For feature points that have not obtained depth information or have abnormal depth information, the depth is recalculated using triangulation to improve the accuracy of system-scale observation and perform more accurate feature tracking.

[0099] Furthermore, in order to continuously track the three-dimensional coordinates of stable feature points and avoid scale drift, in one embodiment, step S30 above may include:

[0100] Step S301: Based on the parallax information of the binocular camera and the camera calibration parameters, triangulation is used to calculate the initial depth information corresponding to each stable feature point;

[0101] Step S302: Mark stable feature points whose initial depth information does not exceed the effective depth range as normal depth feature points, and use the initial depth information of the normal depth feature points as associated depth information;

[0102] Step S303: Stable feature points whose initial depth information exceeds the effective depth range are designated as abnormal depth feature points. The initial depth information of the abnormal depth feature points is cleared to zero, and the initial depth information of the abnormal depth feature points is recalculated.

[0103] It should be noted that binocular cameras can provide fixed baseline information between the two cameras, thereby enabling more accurate triangulation and improving the accuracy and reliability of depth estimation, such as... Figure 5 As shown, Figure 5 This is a schematic diagram of binocular depth detection in one embodiment.

[0104] Understandably, the optimized equipment breaks down the depth measurement process into two steps: First, using the parallax information and camera calibration parameters of the binocular camera, the depth values ​​of feature points are obtained based on the principle of triangulation. Next, the obtained depth information is correlated with the corresponding feature points. To ensure accuracy, depth information exceeding the effective depth range is cleared to zero to avoid the impact of outlier depth values ​​on system accuracy. Subsequently, the depth values ​​of uncorrelated feature points are recalculated using triangulation.

[0105]

[0106] in, Indicates initial depth information. Indicates the distance between the optical centers of the left and right cameras. This represents the distance from the optical center of the camera to the two-dimensional plane. and Representing three-dimensional points respectively Projected coordinates on the left and right camera planes.

[0107] In some embodiments, after the optimization device performs uniform distribution processing on the target feature points, it recovers the camera motion through epipolar constraints to obtain information about the rotation matrix R and translation vector t. The epipolar constraint formula is used to recover the spatial 3D position of the feature points in subsequent triangulation, providing a key geometric basis for binocular depth estimation and serving as an important input for parameter optimization of subsequent PnP pose estimation and target optimization models. The epipolar constraint formula includes:

[0108]

[0109] in, K For camera intrinsic parameters; 3D points P Projected onto two camera frames; corresponding pixel positions are , ; R For camera rotation matrix; t This represents the translation amount.

[0110] During the initialization phase, the system selects an initial frame and a reference frame as keyframes from consecutive image frames. The selection is typically based on factors such as image parallax, tracking quality, or feature distribution. New keyframes are selected when the baseline between the current frame and existing keyframes is long, the tracking performance deteriorates, or a large amount of new information is added. After selecting keyframes, the camera pose is obtained by establishing epipolar constraints. Then, triangulation is used to find the common viewpoints between the two frames. Due to the continuity of optical flow tracking, these common viewpoints are also observed in intermediate visual frames. Combining these 3D common viewpoints with the 2D coordinates observed in the reference frame, the pose of the next frame can be solved using the Perspective-n-Point (PnP) algorithm. This process is iterated continuously to obtain the poses of all keyframes, gradually recover the poses of all keyframes, and reconstruct the 3D scene structure.

[0111] The initial frame is the first image selected after the system starts, used as a reference for system initialization, and involved in the initial estimation of scale, gravity direction, pose, etc.

[0112] The current frame is the image frame that the system is currently processing. Its function is to extract and filter feature points from the image; match it with existing keyframes or reference frames to perform pose estimation; and determine whether to promote it to a keyframe.

[0113] Reference frame: The reference frame is the frame that is used for matching and triangulation with the current frame. It is usually the most recent keyframe. Its main function is to provide epipolar geometric constraints for estimating depth through triangulation.

[0114] Keyframes: In a continuous image sequence, whether to select a keyframe is usually determined based on factors such as image disparity, tracking quality, or feature distribution. Their main function is to be stored in a sliding window, participate in joint optimization, improve global consistency, and save the feature points and depth information of the keyframes, which can be used as reference frames by subsequent frames.

[0115] It should be noted that the purpose of obtaining the associated depth information of each stable feature point is mainly to attach true 3D coordinates to these feature points by obtaining their depth information, thus avoiding scale drift. It also provides a 2D–3D correspondence for PnP as a key input. Simultaneously, this depth information will be included as part of the set of variables to be optimized, inputting it into the target optimization model to provide initial values ​​and constraints for subsequent optimization of the target model.

[0116] Step S40: Construct a set of variables to be optimized and a target optimization model based on the stable feature points and the association depth information.

[0117] It should be noted that the set of variables to be optimized includes the inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point.

[0118] It is understood that this embodiment can construct a joint objective optimization model by combining the residual terms constructed based on visual reprojection error and the residual terms constructed based on IMU pre-integration error, and adopting a sliding window optimization strategy. The objective optimization model can be constructed based on the BA optimization model.

[0119] Furthermore, in order to accurately construct the optimization model and improve optimization efficiency and accuracy, in one embodiment, step S40 above may include:

[0120] Step S401: Obtain the three-dimensional coordinates of the stable feature point in the world coordinate system based on the associated depth information.

[0121] It should be noted that the optimization device acquires the associated depth information corresponding to each stable feature point, thereby tracking and associating the depth information of each stable feature point, attaching real three-dimensional coordinates to these stable feature points, avoiding the problem of scale drift. At the same time, this depth information is used as part of the set of variables to be optimized and input into the target optimization model, providing initial values ​​and constraints for subsequent optimization of the target optimization model.

[0122] Step S402: Generate inverse depth information for each stable feature point based on the associated depth information, and construct a set of variables to be optimized based on the inverse depth information.

[0123] It should be noted that the set of variables to be optimized is represented as follows:

[0124]

[0125] in, Represents the set of variables to be optimized. Represents an inertial state sequence. Indicates the first The frame's inertial sensor state includes the IMU's position, velocity, and orientation in the world coordinate system, as well as acceleration bias and gyroscope bias. Indicates the first Frame-time inertial sensor in world coordinate system The position in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The speed in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The posture in the middle, Indicates acceleration bias. This indicates the gyroscope bias. This indicates the relative attitude relationship between the stereo camera and the inertial sensor. Representing the camera coordinate system In the inertial sensor coordinate system The position in the middle, Representing the camera coordinate system In the IMU coordinate system The posture in the middle, Represents the inverse depth information sequence. Indicates the first The inverse depth information of a stable feature point is the reciprocal of the associated depth information of the stable feature point.

[0126] It should be noted that the associated depth information is the true depth value of the stable feature point. However, since the depth of the depth information may be very large, it can lead to unstable optimization. In this embodiment, by converting the associated depth information into inverse depth information, the depth information can converge to close to 0. The inverse depth, as the variable to be optimized, can make the projection function more stable in nonlinear optimization.

[0127] Step S403: Construct a visual reprojection error residual term based on the set of variables to be optimized and the three-dimensional coordinates of the stable feature points.

[0128] It should be noted that the visual reprojection error residual term describes the positional deviation of a point in three-dimensional space after it has been projected onto a two-dimensional image plane. The visual reprojection error residual term includes:

[0129]

[0130] in, This represents the visual reprojection error residual term. Indicates that the camera is from the first Frame motion to the first The frame's feature observables, wherein the feature observables are the two-dimensional coordinates of stable feature points. This represents a camera projection model (e.g., a pinhole / fisheye camera model). Represents the stable feature point. The three-dimensional coordinates of the frame This represents inverse depth information.

[0131] It should be noted that the aforementioned feature observations can refer to the two-dimensional coordinates of feature points observed in each frame of the camera image during the sliding window optimization process. Feature points are local structures with saliency and stability in the image, while feature observations are the specific two-dimensional coordinates of feature points in a certain frame. Furthermore, a single feature point may be observed in multiple frames, thus forming a set of observations.

[0132] Step S404: Construct the IMU pre-integration error residual term.

[0133] It should be noted that the IMU pre-integration error residual term measures the inconsistency between the "IMU pre-integration result" and the "motion estimated by other sensors (such as vision)". It is a core constraint used in back-end optimization to correct IMU noise, bias and pose errors.

[0134] It should be noted that in this embodiment, the IMU pre-integration error residual term is used to estimate the motion state of the camera through the fusion of IMU and visual data, which provides a basis for the coupling optimization between IMU and visual sensor. By combining the acceleration, angular velocity and other information measured by IMU with visual information, the pose estimation accuracy can be further improved and the error accumulation caused by blurred or sparse areas of visual information can be reduced.

[0135] In some embodiments, the optimization device can construct the IMU pre-integration error residual term using IMU pre-integration theory.

[0136] Furthermore, in order to fully utilize the high-frequency motion information of the IMU and ensure global consistency of the optimization, step S404 above may include:

[0137] Step S4041: Construct the IMU pre-integration error residual term based on the inertial data of consecutive adjacent image frames within the sliding window of the binocular vision inertial navigation system.

[0138] It should be noted that, in order to make full use of the high-frequency motion information of the IMU and ensure the global consistency of the optimization, the optimization device can construct the IMU pre-integration error residual term through the IMU pre-integration theory. Through the joint constraints of multiple state variables, the high-frequency motion information of the IMU is closely combined with visual observation, providing robust initial values ​​and nonlinear optimization objectives for sliding window optimization.

[0139] Understandably, the optimization device bases its settings on two consecutive frames within the sliding window. to IMU observations With system status information (Including IMU state (IMU state includes IMU's position, velocity, and orientation in the world coordinate system, as well as acceleration bias and gyroscope bias), relative attitude relationship between the camera and IMU, and inverse depth information of feature points), defining IMU pre-integration measurement residuals. :

[0140]

[0141] in, This represents the IMU pre-integration error residual. Indicates system status information. Indicates the inertial sensor from the first Frame to the The pre-integral measurement of the frame, Indicates the inertial sensor from the first Frame to the Frame rotation residuals Indicates the inertial sensor from the first Frame to the Frame velocity residual, Indicates the inertial sensor from the first Frame to the Frame position residual, This indicates that the accelerometer has zero bias residual. This indicates that the gyroscope has zero bias residual. Indicates the world coordinate system rotation. Rotation matrix of the frame IMU coordinate system and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame location information, The gravitational acceleration vector in the world coordinate system. The time interval between two IMU measurement frames, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame speed information, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame quaternion pose information, This represents a function that converts a quaternion into an axis-angle vector representation. Quaternion information for rotational residuals, and They represent the first Frame and the Zero bias of the accelerometer in the frame. and They represent the first Frame and the Zero bias of the gyroscope in the frame.

[0142] Step S405: Construct a target optimization model based on the visual reprojection error residual term and the IMU pre-integration error residual term.

[0143] In a practical implementation, the optimization device can combine the residual terms constructed based on visual reprojection error and the residual terms constructed based on IMU pre-integration error to construct a joint optimization objective function (i.e., the objective optimization model).

[0144] Furthermore, to further improve optimization efficiency and state estimation accuracy, in some embodiments, step S405 may include:

[0145] Step S4051: Construct the target optimization model using the sliding window optimization strategy, the visual reprojection error residual term, and the IMU pre-integration error residual term.

[0146] In some embodiments, the sliding window in the sliding window optimization strategy is a set containing a certain number of keyframes and their corresponding IMU measurement data. Keyframes are representative frames selected from continuously acquired image frames, containing sufficient environmental information for localization and mapping. For example, during a robot's environmental exploration, a frame is selected as a keyframe and added to the sliding window after a certain time interval or distance traveled.

[0147] The sliding mechanism involves the window sliding forward over time as new data arrives. When the number of frames within the window reaches a pre-set maximum, the oldest keyframe and its corresponding IMU measurement data need to be removed to maintain real-time computation and limited computational resource consumption. However, before removal, a marginalization operation is performed to retain the information contained in the old frames as constraints, adding them to the optimization problem of the remaining frames. This ensures that the contribution of the old frames to the overall system state estimation is not lost.

[0148] It's important to note that in binocular vision-inertial navigation systems, as time progresses and the amount of collected data increases, optimizing all historical data would lead to an exponential increase in computational load, making real-time operation impossible. The sliding window optimization strategy limits the number of frames involved in the optimization, keeping the scale of the optimization problem within a certain range. Although the sliding window removes old frames, through marginalization, the constraint information of those old frames on the current system state is preserved, thus not significantly reducing the accuracy of state estimation. Furthermore, the sliding window optimization strategy can promptly correct the system state using the latest measurement data.

[0149] It is understandable that the optimization device can combine the residual terms constructed based on visual reprojection error and the residual terms constructed based on IMU pre-integration error, and use a sliding window optimization strategy to construct a joint objective optimization model. The objective optimization model is expressed as:

[0150]

[0151] in, This represents the objective optimization model function. Indicates marginalized residuals, This represents the IMU pre-integration error residual term. This represents the visual reprojection error residual term. This represents the Huber loss function. The measurement estimation matrix represents the marginalization. This represents the covariance matrix of each term. This represents the set of stable feature points observed in the current sliding window. This represents the set of all IMU measurements.

[0152] Furthermore, to further improve the robustness of the optimization and the accuracy of the estimated state, in one embodiment, step S4051 above may include:

[0153] Step S40511: Construct an initial optimization model using a sliding window optimization strategy, the visual reprojection error residual term, and the IMU pre-integration error residual term;

[0154] Step S40512: Perform quality assessment on each stable feature point, determine the observation quality of each stable feature point, and configure the weighting factor of each stable feature point based on the observation quality;

[0155] Step S40513: Update the visual reprojection error residual term based on the weighting factor;

[0156] Step S40514: Adjust the initial optimization model according to the updated visual reprojection error residual term to obtain the target optimization model.

[0157] It should be noted that when the device moves violently or too quickly, it can cause visual blurring and increase IMU measurement bias, leading to inaccurate state estimation. Furthermore, the calculation of weighting factors for different feature points is usually related to the quality of those feature points.

[0158] Therefore, this embodiment introduces feature weight factors based on the initial optimization model. By updating the weights of stable feature points in the image frame, the impact of outliers on optimization accuracy is reduced. The weight factors of different feature points are adaptively allocated according to their observation quality, mainly based on indicators such as reprojection error and tracking stability. Feature points with smaller reprojection errors and more stable tracking have larger weight factors, and vice versa. Referring to the following formula, the weighted reprojection residual term is defined, that is, the updated visual reprojection error residual term includes:

[0159]

[0160] in, Represents stable feature points The corresponding weighting factor, The closer a value is to 1, the more stable the feature is, and the smaller the weight factor for features with larger reprojection errors. The feature points here are the set of stable feature points observed over multiple frames within the sliding window. To further improve the robustness of the optimization, this paper expands the improved weighted projection error term into the overall objective function, forming the following objective optimization model:

[0161]

[0162] in, This represents the updated visual reprojection error residual term.

[0163] Step S50: Input the set of variables to be optimized into the target optimization model for parameter optimization, update the current state parameters of the binocular vision inertial navigation system based on the parameter optimization results output by the target optimization model, and use the updated current state parameters as the new set of variables to be optimized, and re-execute the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization.

[0164] It is understood that this embodiment can simultaneously optimize camera pose, velocity, map point position, and IMU accelerometer bias and gyroscope bias through the target optimization model.

[0165] In the specific implementation, the device first optimizes the initial observation data based on vision and IMU to make a rough estimate of the initial pose, velocity and bias of the system. Then, the state of multiple key frames is jointly optimized within a sliding window to minimize all measurement residual terms. Finally, the current state parameters are updated using the optimization results and used as the prior input for the next optimization to form continuous state estimation and feedback, thereby effectively compensating for the cumulative errors generated during the initial start-up or violent movement of the device and improving the overall robustness and accuracy of the system.

[0166] This embodiment divides the current image frame into multiple grid regions. A feature detection algorithm is used to detect feature points in each grid region, and effective feature grids are selected from these regions. These effective feature grids satisfy grid feature conditions and contain multiple target feature points, each satisfying a feature point response value condition. The feature detection algorithm is based on the ORB algorithm and performs uniform distribution processing on the target feature points in the effective feature grids to obtain stable feature points. Triangulation is used to calculate the associated depth information corresponding to each stable feature point. Based on the stable feature points and the associated depth information, a set of variables to be optimized and a target optimization model are constructed. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the stereo camera and the inertial sensor, and the inverse depth information sequence of each stable feature point. The set of variables to be optimized is input into the target optimization model for parameter optimization. The current state parameters of the binocular vision inertial navigation system are updated based on the parameter optimization results output by the target optimization model. The updated current state parameters are then used as a new set of variables to be optimized, and the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization is repeated. In this embodiment, the image frame is divided into grids, and then effective feature grids and homogenized feature points are selected in each grid unit. Then, depth information is associated with the homogenized feature points, thereby improving the accuracy of system scale observation, enabling more accurate feature tracking, effectively reducing the adverse effects of motion blur and noise on feature points, and significantly reducing the problem of discontinuity in depth calculation caused by feature point chaos. Through parameter optimization, the scale drift problem of visual tracking in the binocular vision inertial navigation system is effectively solved, thereby better adapting to different scenes and environmental changes and improving the navigation accuracy of the binocular vision navigation system.

[0167] To measure and compare the accuracy improvement effect of the binocular vision-inertial navigation system parameter optimization method described in this embodiment, the root mean square error (RMSE) is used to compare the method described in this embodiment with the mainstream VINS algorithm. OKVIS is a vision-inertial navigation system based on a monocular camera and inertial navigation system. OKVIS achieves accurate motion estimation by simultaneously optimizing the camera pose and feature point positions. Compared with traditional filter-based methods, OKVIS can provide more accurate and stable positioning performance in dynamic environments. RMSE is an important indicator for measuring VINS trajectory error; the smaller the RMSE, the smaller the average error of trajectory estimation. The statistical results are shown in Table 1 below. Table 1 is a schematic table comparing the root mean square error of the method described in this embodiment with other algorithms. With the continuous improvement of feature and depth information, this embodiment significantly reduces the position estimation error.

[0168] Table 1. Schematic diagram of root mean square error comparison

[0169]

[0170] This embodiment proposes an improved scheme to address feature anomalies and depth estimation problems caused by motion blur and other factors. It utilizes a grid detector to uniformly distribute image features, avoiding overly concentrated feature distribution. Simultaneously, a feature depth correlation mechanism ensures a sufficient number of stable feature information for accurate pose estimation. Furthermore, a feature weighting factor is introduced into the local VIO optimization module, and through iterative optimization and marginalization, the impact of feature outliers on the optimization process is effectively reduced. Testing and analysis of multiple sequences on the EuRoC dataset fully validates the accuracy and effectiveness of the method described in this embodiment in handling severe motion and visual blur.

[0171] Furthermore, this embodiment of the invention also proposes a computer-readable storage medium storing a parameter optimization program for a binocular vision inertial navigation system. When the binocular vision inertial navigation system parameter optimization program is executed by a processor, it implements the steps of the binocular vision inertial navigation system parameter optimization method described above.

[0172] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0173] The aforementioned computer-readable storage medium may be included in the binocular vision inertial navigation system parameter optimization device; or it may exist independently and not be assembled into the binocular vision inertial navigation system parameter optimization device.

[0174] Furthermore, this invention also proposes a computer program product, including a binocular vision inertial navigation system parameter optimization program, which, when executed by a processor, implements the steps of the binocular vision inertial navigation system parameter optimization method as described above.

[0175] The specific implementation of the computer program product of the present invention is basically the same as the embodiments of the above-described binocular vision inertial navigation system parameter optimization method, and will not be repeated here.

[0176] Reference Figure 6 , Figure 6 This is a structural block diagram of an embodiment of the parameter optimization device for a binocular vision inertial navigation system of the present invention.

[0177] like Figure 6 As shown, the binocular vision inertial navigation system parameter optimization device proposed in this embodiment of the invention is applied to a binocular vision inertial navigation system. The binocular vision inertial navigation system parameter optimization device includes:

[0178] The grid division module 10 is used to divide the current image frame into multiple grid regions;

[0179] The feature detection module 20 is used to detect feature points in each grid region based on the feature detection algorithm, and to select effective feature grids from the grid region. The effective feature grids satisfy the grid feature conditions and contain multiple target feature points. Each target feature point satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm.

[0180] The feature homogenization module 30 is used to homogenize the distribution of each target feature point in the effective feature grid to obtain stable feature points, and to calculate the associated depth information corresponding to each stable feature point using triangulation.

[0181] The optimization construction module 40 is used to construct a set of variables to be optimized and a target optimization model based on the stable feature points and the associated depth information. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point.

[0182] The parameter optimization module 50 is used to input the set of variables to be optimized into the target optimization model for parameter optimization, update the current state parameters of the binocular vision inertial navigation system based on the parameter optimization results output by the target optimization model, and use the updated current state parameters as a new set of variables to be optimized, and re-execute the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization.

[0183] This embodiment divides the current image frame into multiple grid regions. A feature detection algorithm is used to detect feature points in each grid region, and effective feature grids are selected from these regions. These effective feature grids satisfy grid feature conditions and contain multiple target feature points, each satisfying a feature point response value condition. The feature detection algorithm is based on the ORB algorithm and performs uniform distribution processing on the target feature points in the effective feature grids to obtain stable feature points. Triangulation is used to calculate the associated depth information corresponding to each stable feature point. Based on the stable feature points and the associated depth information, a set of variables to be optimized and a target optimization model are constructed. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the stereo camera and the inertial sensor, and the inverse depth information sequence of each stable feature point. The set of variables to be optimized is input into the target optimization model for parameter optimization. The current state parameters of the binocular vision inertial navigation system are updated based on the parameter optimization results output by the target optimization model. The updated current state parameters are then used as a new set of variables to be optimized, and the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization is repeated. In this embodiment, the image frame is divided into grids, and then effective feature grids and homogenized feature points are selected in each grid unit. Then, depth information is associated with the homogenized feature points, thereby improving the accuracy of system scale observation, enabling more accurate feature tracking, effectively reducing the adverse effects of motion blur and noise on feature points, and significantly reducing the problem of discontinuity in depth calculation caused by feature point chaos. Through parameter optimization, the scale drift problem of visual tracking in the binocular vision inertial navigation system is effectively solved, thereby better adapting to different scenes and environmental changes and improving the navigation accuracy of the binocular vision navigation system.

[0184] The binocular vision inertial navigation system parameter optimization device provided in this application employs the binocular vision inertial navigation system parameter optimization method in the above embodiments, and can solve the technical problem of binocular vision inertial navigation system parameter optimization. Compared with the prior art, the beneficial effects of the binocular vision inertial navigation system parameter optimization device provided in this application are the same as the beneficial effects of the binocular vision inertial navigation system parameter optimization method provided in the above embodiments, and other technical features in the binocular vision inertial navigation system parameter optimization device are the same as the features disclosed in the methods of the above embodiments, and will not be repeated here.

[0185] It should be understood that the above are merely illustrative examples and do not constitute any limitation on the technical solutions of the present invention. In specific applications, those skilled in the art can make settings as needed, and the present invention does not impose any restrictions on this.

[0186] It should be noted that the workflow described above is merely illustrative and does not limit the scope of protection of this invention. In practical applications, those skilled in the art can select some or all of the workflow to achieve the purpose of this embodiment according to actual needs, and no restrictions are imposed here.

[0187] In addition, for technical details not described in detail in this embodiment, please refer to the parameter optimization method of the binocular vision inertial navigation system provided in any embodiment of the present invention, which will not be repeated here.

[0188] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0189] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0190] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as read-only memory / random access memory, magnetic disk, optical disk) and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0191] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A method for optimizing parameters of a binocular vision inertial navigation system, characterized in that, The binocular vision inertial navigation system parameter optimization method is applied to a binocular vision inertial navigation system, and the binocular vision inertial navigation system parameter optimization method includes: Divide the current image frame into multiple grid regions; Feature points are detected in each grid region based on a feature detection algorithm. Valid feature grids are selected from the grid regions. The valid feature grids satisfy the grid feature conditions and contain multiple target feature points. Each target feature point satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm. The target feature points in the effective feature grid are uniformly distributed to obtain stable feature points, and triangulation is used to calculate the associated depth information corresponding to each stable feature point. Based on the stable feature points and the associated depth information, a set of variables to be optimized and a target optimization model are constructed. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point. The target optimization model includes an updated visual reprojection error residual term. The visual reprojection error residual term is updated based on a weight factor. The weight factor is configured based on the observation quality of each stable feature point. The observation quality includes a reprojection error index and a tracking stability index. The visual reprojection error residual term is used to describe the positional deviation of a three-dimensional spatial point after it is projected onto a two-dimensional image plane. The set of variables to be optimized is input into the target optimization model for parameter optimization. Based on the parameter optimization results output by the target optimization model, the current state parameters of the binocular vision inertial navigation system are updated. The updated current state parameters are used as the new set of variables to be optimized, and the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization is repeated. The feature detection algorithm is used to detect feature points in each grid region and select valid feature grids from the grid regions, including: Feature points are detected in each grid region based on the feature detection algorithm to determine the candidate feature points and the response values ​​of each candidate feature point in each grid region. Select target feature points whose response values ​​are not lower than the response threshold from the candidate feature points; Texture filtering is performed on each grid region to determine candidate grids whose texture features satisfy the texture feature conditions in the grid region; Determine whether the number of target feature points in each candidate grid reaches the feature point number threshold; The grids in the candidate grids whose number of target feature points reaches the feature point number threshold are considered as valid feature grids. Based on the feature detection algorithm, re-detect the target feature points in the candidate grids where the number of target feature points has not reached the feature point number threshold, add the newly detected target feature points to the candidate grids, and return to execute the step of determining whether the number of target feature points in each candidate grid has reached the feature point number threshold. The target optimization model is constructed using a sliding window optimization strategy, a visual reprojection error residual term, and an IMU pre-integration error residual term. The target optimization model is expressed as follows: in, This represents the objective optimization model function. Indicates marginalized residuals, This represents the IMU pre-integration error residual term. This represents the visual reprojection error residual term. This represents the Huber loss function. The measurement estimation matrix represents the marginalization. This represents the covariance matrix of each term. This represents the set of stable feature points observed in the current sliding window. Represents the set of all IMU measurements; The process of constructing the target optimization model also includes: An initial optimization model was constructed using a sliding window optimization strategy, visual reprojection error residuals, and IMU pre-integration error residuals. A quality assessment is performed on each stable feature point to determine the observation quality of each stable feature point, and a weighting factor for each stable feature point is configured based on the observation quality. The visual reprojection error residual term is updated based on the weighting factor. The updated visual reprojection error residual term includes: in, Represents stable feature points The corresponding weighting factor; The initial optimization model is adjusted based on the updated visual reprojection error residual term to obtain the target optimization model, which is expressed as: in, This represents the updated visual reprojection error residual term.

2. The parameter optimization method for a binocular vision inertial navigation system as described in claim 1, characterized in that, The construction of the set of variables to be optimized and the target optimization model based on the stable feature points and the association depth information includes: Based on the associated depth information, obtain the three-dimensional coordinates of the stable feature point in the world coordinate system; Based on the associated depth information, inverse depth information of each stable feature point is generated, and a set of variables to be optimized is constructed based on the inverse depth information. The set of variables to be optimized is represented as follows: in, Represents the set of variables to be optimized. Represents a sequence of inertial states. Indicates the first The inertial sensor status of the frame. Indicates the first Frame-time inertial sensor in world coordinate system The position in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The speed in the middle, Indicates the first Frame-time inertial sensor in world coordinate system The posture in the middle, Indicates acceleration bias. This indicates the gyroscope bias. This indicates the relative attitude relationship between the stereo camera and the inertial sensor. Representing the camera coordinate system In the inertial sensor coordinate system The position in the middle, Representing the camera coordinate system In the IMU coordinate system The posture in the middle, Represents the inverse depth information sequence. Indicates the first Inverse depth information of a stable feature point; Based on the set of variables to be optimized and the three-dimensional coordinates of the stable feature points, a visual reprojection error residual term is constructed, which includes: in, This represents the visual reprojection error residual term. Indicates that the camera is from the first Frame motion to the first The frame's feature observables, which are the two-dimensional coordinates of stable feature points. Represents the camera projection model. Represents the stable feature point. The three-dimensional coordinates of the frame Represents inverse depth information; Construct the IMU pre-integration error residual term; A target optimization model is constructed based on the visual reprojection error residual term and the IMU pre-integration error residual term.

3. The parameter optimization method for a binocular vision inertial navigation system as described in claim 2, characterized in that, The construction of the IMU pre-integration error residual term includes: The IMU pre-integration error residual term is constructed based on the inertial data of consecutive adjacent image frames within a sliding window of a binocular vision inertial navigation system. The IMU pre-integration error residual term is expressed as follows: in, This represents the IMU pre-integration error residual. Indicates system status information. Indicates the inertial sensor from the first Frame to the The pre-integral measurement of the frame, Indicates the inertial sensor from the first Frame to the Frame rotation residuals Indicates the inertial sensor from the first Frame to the Frame velocity residual, Indicates the inertial sensor from the first Frame to the Frame position residual, This indicates that the accelerometer has zero bias residual. This indicates that the gyroscope has zero bias residual. Indicates the world coordinate system rotation. Rotation matrix of the frame IMU coordinate system and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame location information, The gravitational acceleration vector in the world coordinate system. The time interval between two IMU measurement frames, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame speed information, and These represent the inertial sensor's position in the world coordinate system. Frame and the Frame quaternion pose information, This represents a function that converts a quaternion into an axis-angle vector representation. Quaternion information for rotational residuals, and They represent the first Frame and the Zero bias of the accelerometer in the frame. and They represent the first Frame and the Zero bias of the gyroscope in the frame.

4. The parameter optimization method for a binocular vision inertial navigation system as described in any one of claims 1 to 3, characterized in that, The calculation of the associated depth information corresponding to each stable feature point using triangulation includes: Based on the disparity information and camera calibration parameters of the binocular camera, triangulation is used to calculate the initial depth information corresponding to each stable feature point: in, Indicates initial depth information. Indicates the distance between the optical centers of the left and right cameras. This represents the distance from the optical center of the camera to the two-dimensional plane. and Representing three-dimensional points respectively Projected coordinates on the left and right camera planes; Stable feature points whose initial depth information does not exceed the effective depth range are marked as normal depth feature points, and the initial depth information of the normal depth feature points is used as associated depth information. Stable feature points whose initial depth information exceeds the effective depth range are designated as abnormal depth feature points. The initial depth information of the abnormal depth feature points is cleared to zero, and the initial depth information of the abnormal depth feature points is recalculated.

5. A parameter optimization device for a binocular vision inertial navigation system, characterized in that, The device is applied to a binocular vision inertial navigation system, and the binocular vision inertial navigation system parameter optimization device includes: The grid division module is used to divide the current image frame into multiple grid regions; The feature detection module is used to detect feature points in each grid region based on the feature detection algorithm, and to select effective feature grids from the grid region. The effective feature grids satisfy the grid feature conditions and contain multiple target feature points. Each target feature point satisfies the feature point response value condition. The feature detection algorithm is constructed based on the ORB algorithm. The feature homogenization module is used to homogenize the distribution of each target feature point in the effective feature grid to obtain stable feature points, and to calculate the associated depth information corresponding to each stable feature point using triangulation. An optimization construction module is used to construct a set of variables to be optimized and a target optimization model based on the stable feature points and the associated depth information. The set of variables to be optimized includes an inertial state sequence, the relative attitude relationship between the binocular camera and the inertial sensor, and the inverse depth information sequence of each stable feature point. The target optimization model includes an updated visual reprojection error residual term, which is updated based on a weight factor. The weight factor is configured based on the observation quality of each stable feature point. The observation quality includes a reprojection error index and a tracking stability index. The visual reprojection error residual term is used to describe the positional deviation of a three-dimensional spatial point after it is projected onto a two-dimensional image plane. The parameter optimization module is used to input the set of variables to be optimized into the target optimization model for parameter optimization, update the current state parameters of the binocular vision inertial navigation system based on the parameter optimization results output by the target optimization model, and use the updated current state parameters as a new set of variables to be optimized, and re-execute the step of inputting the set of variables to be optimized into the target optimization model for parameter optimization. The feature detection module is further configured to perform feature point detection on each grid region based on the feature detection algorithm, determine candidate feature points in each grid region and the response value of each candidate feature point; filter target feature points with response values ​​not lower than the response threshold from the candidate feature points; perform texture filtering on each grid region, determine candidate grids in the grid region whose texture features meet the texture feature conditions; determine whether the number of target feature points in each candidate grid reaches the feature point number threshold; take the grids in the candidate grids whose number of target feature points reaches the feature point number threshold as valid feature grids; re-detect feature points on the grids in the candidate grids whose number of target feature points does not reach the feature point number threshold based on the feature detection algorithm, add the newly detected target feature points to the candidate grids, and return to execute the step of determining whether the number of target feature points in each candidate grid reaches the feature point number threshold; The optimization construction module is further used to construct a target optimization model using a sliding window optimization strategy, a visual reprojection error residual term, and an IMU pre-integration error residual term. The target optimization model is expressed as follows: in, This represents the objective optimization model function. Indicates marginalized residuals, This represents the IMU pre-integration error residual term. This represents the visual reprojection error residual term. This represents the Huber loss function. The measurement estimation matrix represents the marginalization. This represents the covariance matrix of each term. This represents the set of stable feature points observed in the current sliding window. Represents the set of all IMU measurements; The optimization construction module is further configured to construct an initial optimization model using a sliding window optimization strategy, a visual reprojection error residual term, and an IMU pre-integration error residual term; to perform quality assessment on each stable feature point, determine the observation quality of each stable feature point, and configure weight factors for each stable feature point based on the observation quality; and to update the visual reprojection error residual term based on the weight factors. The updated visual reprojection error residual term includes: in, Represents stable feature points The corresponding weighting factor; The initial optimization model is adjusted based on the updated visual reprojection error residual term to obtain the target optimization model, which is expressed as: in, This represents the updated visual reprojection error residual term.

6. A parameter optimization device for a binocular vision inertial navigation system, characterized in that, The binocular vision inertial navigation system parameter optimization device includes: a memory, a processor, and a binocular vision inertial navigation system parameter optimization program stored in the memory and executable on the processor. The binocular vision inertial navigation system parameter optimization program is configured to implement the binocular vision inertial navigation system parameter optimization method as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a parameter optimization program for a binocular vision inertial navigation system, which, when executed by a processor, implements the parameter optimization method for a binocular vision inertial navigation system as described in any one of claims 1 to 4.

Images