Initialization method and system for monocular vision-inertial system fused with time-of-flight ranging

Abstract

A kind of fusion time-of-flight ranging monocular vision inertial system initialization method and system, comprising: synchronizing monocular camera, IMU and ToF sensor;Visual SfM preliminary reconstruction;Data collected by ToF sensor is converted to camera coordinate system by external parameter and projected to image plane, to each projection point as center establishes search domain, looks for the nearest neighbor visual feature point in search domain, and outputs feature point-depth pair set after verification;Dual-mode strategy is used to restore metric scale;In sliding window, construct target function, joint optimization, iterative solution, so that depth constraint and motion estimation form strong coupling;System determines initialization success, injects the optimized system state into VIO back-end process, sends low-power instruction to close ToF sensor to sleep mode, and switches to pure monocular-IMU mode operation.The present application guarantees the robustness and accuracy of the initialization stage of the monocular vision inertial system of unmanned aerial vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of monocular vision inertial system, in particular to a monocular vision inertial system initialization method and system fusing time-of-flight ranging. BACKGROUND

[0002] The existing visual inertial odometer initialization adopts a loose coupling method, and solves the monocular scale uncertainty and IMU (Inertial measurement unit) bias problem through pure vision SFM (Structure from Motion) and IMU pre-integration alignment. The process is as follows: first, pure vision SFM is performed in a sliding window, a reference frame that meets the parallax and matching feature point number is selected, the relative rotation and translation are recovered by calculating the essential matrix through the five-point method, and the feature points are triangulated, then the PnP (Perspective-n-Point) method is used to solve the pose of other frames, and finally the camera pose in the sliding window is globally optimized and converted to the IMU coordinate system. Then, visual inertial alignment is performed, including gyro bias calibration, velocity / gravity / scale initialization, gravity vector optimization and world coordinate system alignment.

[0003] However, when such a visual inertial odometer is applied to a UAV platform, the shortcomings of the existing initialization technology will be magnified in the typical operating scenarios of the UAV, seriously restricting the reliability and practicality of the system:

[0004] 1. Rigid dependence on initial translational motion leads to initialization failure in take-off or hovering phase: the UAV is often in a hovering or small-amplitude attitude adjustment (such as calibration) state after starting, at which time there is a lack of sufficient translational excitation. The existing technology will directly fail to initialize in this "pure rotation" or "very small translation" scenario due to the unobservable visual scale information.

[0005] 2. Slow / slow scale convergence under weak translation affects flight safety and control: Even in the presence of slight translation (such as indoor hovering, fine positioning), the scale converges slowly and inaccurately. For a UAV, an inaccurate or drifting scale estimate will directly translate into incorrect displacement and velocity perception, causing it to drift while hovering, or produce trajectory deviations when performing tasks such as route flight, autonomous landing, etc., which may cause safety accidents such as collision with obstacles.

[0006] 3. Dynamic object interference causes deviation in pose and scale estimation: The UAV operating environment (such as cities, warehouses, farmland) often has vehicles, pedestrians and other dynamic objects. Feature points on these objects will interfere with feature tracking and optimization in visual SFM, introducing incorrect pose and scale estimates.

[0007] An initial map and scale affected by abnormal data in the initialization stage will cause a basic deviation in the subsequent navigation process of the UAV, and seriously reduce its robustness in dynamic environment. SUMMARY

[0008] The technical problem to be solved by the present application is how to initialize a monocular vision inertial system of a UAV to further improve the robustness and accuracy of the initialization stage.

[0009] The present application solves the above technical problems by the following technical means: a monocular vision inertial system initialization method fusing time-of-flight ranging, comprising the following steps:

[0010] Step 1, after the system is powered on, the monocular camera, IMU and ToF sensor are synchronized;

[0011] Step 2, visual SfM preliminary reconstruction;

[0012] Step 3, the depth point cloud data collected by the ToF sensor is converted to the camera coordinate system through pre-calibration external parameters, and is projected to the image plane, and a search domain is established around each projection point, the nearest neighbor visual feature point in the search domain is found, and three-level verification is performed: low confidence depth points are filtered based on the confidence map built-in the ToF sensor; depth outliers are removed according to the depth continuity of the projection point neighborhood; the optical flow vector caused by its own rotation is compensated by combining the angular velocity information measured by the current IMU, and a feature point-depth pair set is output;

[0013] Step 4, based on the feature point-depth pair set, a dual-mode strategy is used to restore the metric scale;

[0014] Step 5, a target function containing visual re-projection residual, IMU pre-integral residual and depth measurement residual is constructed in a sliding window, and the key frame pose, velocity, IMU zero offset and map point are optimized jointly, and the depth constraint and motion estimation are strongly coupled by iterative solution;

[0015] Step 6, when the key indicators simultaneously meet their respective threshold conditions in the iteration, the system determines whether the initialization is successful;

[0016] Step 7, after the initialization is successful, the optimized system state is injected into the VIO backend process, and a low-power instruction is immediately sent to close the ToF sensor to sleep mode, and the system is switched to a pure monocular-IMU mode.

[0017] As a further optimized technical solution, in step 1, after the system is powered on, the monocular camera, IMU and ToF sensor are synchronized by hardware trigger, and time stamp offset compensation is used to ensure that the time deviation of the three source data of the monocular camera, IMU and ToF sensor is less than the threshold Δt.

[0018] As a further optimized technical solution, step 2, visual SfM preliminary reconstruction, specifically includes: performing FAST corner point extraction and LK optical flow tracking on N consecutive frames of images, selecting the frame with a sufficiently large disparity to the current frame and the largest number of matching feature points as the reference frame, and for the reference frame... and the current frame Matching feature points between There exists a polar constraint:

[0019]

[0020] in and yes arrive rotation and translation It is a translation vector The antisymmetric matrix;

[0021] Solving the essential matrix using the five-point method This leads to the decomposition and rotation. unit direction vector of translation Furthermore, DLT triangulation is used to generate an initial map point set with uncertain scale, and a temporary environmental structure is established under the visual coordinate system.

[0022] As a further optimized technical solution, step 3, which involves transforming the depth point cloud data acquired by the ToF sensor to the camera coordinate system via pre-calibrated extrinsic parameters and projecting it onto the image plane, specifically includes:

[0023] A point measured by a ToF sensor Transform to camera coordinate system:

[0024]

[0025] Subsequently, the camera intrinsic matrix was used. Project it onto the image plane:

[0026]

[0027] in It is a pinhole camera projection model. These are the pixel coordinates after projection.

[0028] As a further optimized technical solution, in step 4, when the number of valid feature point-depth pairs in the set is greater than a set threshold, mode 1 is used; otherwise, mode 2 is enabled.

[0029] Mode 1: By constructing the re-projection error equation of all feature point-depth pairs, a global scale factor is solved in a least square manner, and all scale-uncertain initial map point coordinates from the visual SfM preliminary reconstruction are uniformly scaled by using the factor;

[0030] Mode 2: The depth value in the feature point-depth pair is directly converted into a three-dimensional absolute coordinate in the camera coordinate system, and the coordinate value is used to cover or directly replace the corresponding scale-uncertain initial map point position obtained by triangulation.

[0031] As a further optimization technical solution, in step 4, mode 1 is specifically:

[0032] For an effective feature point-depth pair, the scale-uncertain initial three-dimensional map point has a depth in the reference frame camera system , and the absolute depth provided by the ToF sensor is , and the scale residual is constructed:

[0033]

[0034] The least square problem is constructed:

[0035]

[0036] The problem has a closed-form solution: , and then, the scale factor is used to scale all scale-uncertain initial three-dimensional map points :

[0037]

[0038] The final three-dimensional map point with real physical scale is obtained ;

[0039] Mode 2 is specifically:

[0040] The ToF depth value is directly converted into a three-dimensional map point in the camera coordinate system:

[0041]

[0042] wherein is the matched feature point pixel coordinate, and then is transformed into the world coordinate system, directly replacing or covering the original scale-uncertain initial three-dimensional map point .

[0043] As a further optimization technical solution, step 5 includes:

[0044] Optimizing the state vector State of all keyframes within the sliding window:

[0045]

[0046] where the state of the k-th keyframe is: , respectively, are the position, velocity, rotation quaternion, accelerometer bias, gyroscope bias, is the inverse depth of the m-th map point;

[0047] A joint optimization objective function is constructed within the sliding window:

[0048]

[0049] where the visual re-projection error is the position of the map point in the world frame, is the k-th frame body pose, is the camera-to-IMU extrinsic;

[0050] The IMU pre-integration error :

[0051]

[0052] where , , is the pre-integration quantity of the IMU within the interval , with first-order linear approximation to the bias; the depth measurement error , or the z-axis depth difference in the camera coordinate system is used:

[0053]

[0054] where denotes taking the z-coordinate component, and this error directly introduces absolute scale information as a strong constraint into the optimization problem;

[0055] Iterative solution by Levenberg-Marquardt nonlinear optimization algorithm, synchronous optimization of the rotation and translation pose, velocity, IMU gyroscope and accelerometer bias of all keyframes, and three-dimensional coordinates of all map points, so that the sparse but accurate absolute depth measurement constraint and continuous visual-inertial motion estimation are tightly coupled, and the initial value error is corrected together, so as to determine the system scale, motion state and sensor bias.

[0056] As a further optimization technical solution, the key indicators in step 6 include:

[0057] ​Standard deviation of scale factor estimation: evaluate the stability of scale solution, need to be less than a set threshold ;

[0058] Mean of depth measurement residuals: evaluate the consistency of optimized map points with ToF depth measurements, need to be less than a threshold ;

[0059] Variation of IMU gyroscope bias: evaluate whether the bias estimation is converging, need to be less than a threshold .

[0060] As a further optimization scheme, when the initialization fails or the data association quality is poor, it also includes steps 8, abnormal handling and fallback mechanism:

[0061] Process reset: clear all images, IMU data and temporary map points in the current sliding window, reset the initialization state machine;

[0062] Traditional method fallback: switch to a traditional visual-inertial alignment method that does not rely on ToF sensors;

[0063] Conditional retrigger: the system continuously monitors the motion conditions in subsequent runs, and when sufficient parallax changes are detected, it will automatically retrigger the complete initialization process starting from step 1, trying to obtain better initial values with ToF assistance.

[0064] The present application also provides a monocular visual-inertial system initialization system fusing time-of-flight ranging, comprising:

[0065] Synchronization module, for synchronizing monocular camera, IMU and ToF sensor after system power-on;

[0066] Reconstruction module, for visual SfM preliminary reconstruction;

[0067] Feature point-depth pair set generation module, for converting depth point cloud data collected by ToF sensor to camera coordinate system through pre-calibration extrinsic parameters, and projecting to image plane, establishing search domain centered on each projected point, finding the nearest neighbor visual feature point in the search domain, and performing three-level verification: filtering low-confidence depth points based on ToF sensor's built-in confidence map; excluding depth outliers according to depth continuity in the projected point neighborhood; combining the current IMU measured angular velocity information to compensate for the optical flow vector caused by its own rotation, outputting the feature point-depth pair set;

[0068] Scale recovery module, for recovering the metric scale based on the feature point-depth pair set using a dual-mode strategy;

[0069] A strong coupling module is configured to build a target function including visual re-projection residuals, IMU pre-integration residuals and depth measurement residuals within a sliding window, jointly optimize key frame poses, velocities, IMU biases and map points, and iteratively solve the strong coupling between depth constraints and motion estimation;

[0070] A determination module is configured to determine whether the initialization is successful when the key indicators simultaneously satisfy their respective threshold conditions in iterations.

[0071] A switching module is configured to inject the optimized system state into a VIO backend process after the initialization is successful, immediately send a low-power instruction to turn off the ToF sensor to a sleep mode, and switch to a pure monocular-IMU mode.

[0072] The present application has the following advantages:

[0073] 1. The present application temporarily enables the ToF sensor only in the system initialization stage, acquires absolute depth information of the key frame scene through single or short-time acquisition, and immediately turns off the ToF sensor after the initialization is successful to maintain low power consumption. The present application introduces instantaneous absolute depth information provided by a low-cost and low-power ToF sensor in a short key initialization window after the system is started, and these depth information is directly used to constrain the scale of visual feature points and assist in building an initial map, thereby significantly reducing the translational excitation requirement for the initial motion, enabling the initialization (restoring scale, gravity direction and IMU bias initial value) to be successfully, quickly and accurately completed under pure rotation or minimal translation motion. At the same time, the present application overcomes the rigid dependence of the traditional method on strong translation motion, and significantly improves the robustness and startup efficiency of the system in the degenerate motion scene.

[0074] 2. A projection-neighbor matching-confidence screening three-level association mechanism is designed for a sparse and non-uniform depth point array. The mechanism realizes high-precision and robust association of depth information and dense visual features by preliminary positioning through geometric projection, fine matching through neighborhood and confidence screening to remove false associations, and greatly improves the utilization efficiency of limited depth data. Based on the above depth association mechanism, feature points with inconsistent depth-visual observations caused by dynamic objects can be effectively identified and filtered, thereby avoiding the influence of dynamic interference on the initialization optimization. At the same time, the mechanism can provide absolute scale information anchors for weak texture areas, make up for the information degradation caused by the lack of visual features, and enhance the initialization robustness of the system in dynamic objects and weak texture environments. BRIEF DESCRIPTION OF DRAWINGS

[0075] Figure 1 is a flowchart of the monocular visual inertial system initialization method of the present application. DETAILED DESCRIPTION

[0076] In order to make the purposes, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0077] With reference to Figure 1 , the monocular vision inertial system initialization method fusing time-of-flight ranging includes the following steps:

[0078] S1, multi-source sensor synchronous start

[0079] The monocular vision inertial system is integrated into a UAV platform as a core component of a visual inertial navigation system of the UAV platform. After the monocular vision inertial navigation system is powered on, the monocular camera, the IMU and the ToF (Time of Flight) sensor are synchronously started by a dedicated hardware trigger signal of an airborne main control processor. The IMU is directly integrated into a flight control module of the UAV and is connected through an onboard I2C or SPI bus; the monocular camera and the ToF sensor are connected with the monocular vision inertial system through an MIPI CSI-2 interface and an I2C bus respectively as external devices, to ensure low-delay transmission of data streams. In physical installation, the monocular camera and the ToF sensor are rigidly fixed to the UAV body and are pre-calibrated to make their optical centers as close as possible and have most of the fields of view overlap, to facilitate subsequent data association; the relative pose relationship between the IMU and the monocular camera / ToF sensor is also determined through offline calibration. During data acquisition, a timestamp offset compensation algorithm is adopted to ensure that the time deviation between the IMU, the monocular camera and the ToF sensor is less than a set threshold Δt, to provide basis data streams of strict spatio-temporal alignment for subsequent processing.

[0080] S2, visual SfM preliminary reconstruction

[0081] FAST corner points are extracted and LK optical flow tracking is performed on continuous N frames of images. The selection of the reference frame follows the following criteria: in a sliding window, a frame with a large enough parallax (i.e. more than p pixels) and the largest number of matched feature points with the current frame is selected as the reference frame. Specifically, the system starts from the first frame, and each subsequent frame is sequentially taken as the current frame, the parallax and the number of matched features between the current frame and the potential reference frame are calculated, and when the parallax threshold and the minimum number of matched points are both satisfied, the potential frame is determined as the reference frame.

[0082] For the matched feature points between the reference frame and the current frame , there is an epipolar constraint:

[0083]

[0084] in , Representing reference frames respectively and the current frame The pixel coordinates of the matched feature points and Reference frame Up to the current frame rotation and translation It is a translation vector antisymmetric matrix, This is the camera's intrinsic parameter matrix. The essential matrix is ​​solved using the five-point method. This leads to the decomposition and rotation. unit direction vector of translation (Scale unknown, i.e.) , (where is the scale factor to be determined).

[0085] An initial 3D map point set with uncertain scale is generated using the DLT (Direct Linear Transform) triangulation method. For a pair of points on the normalized plane... and ,have and The corresponding initial 3D map points with uncertain scale satisfy:

[0086]

[0087] in It is a matrix constructed from the poses (rotation and translation) of two frames and points on the normalized plane. It is obtained by solving using SVD. This allows for the establishment of a temporary environmental structure within the visual coordinate system.

[0088] S3, ToF - Dynamic Association of Visual Features

[0089] The depth point cloud data acquired by the ToF sensor is transformed to the camera coordinate system using pre-calibrated extrinsic parameters and projected onto the image plane. A circular search domain with a pixel radius of m is established centered on each projection point, and a KD-tree data structure is used to accelerate the search for the nearest neighbor visual feature points within the search domain.

[0090] A point measured by a ToF sensor Transformation from (homogeneous coordinates) to the camera coordinate system:

[0091]

[0092] in, is the transformation matrix from ToF sensor coordinate system to camera coordinate system, containing rotation matrix and translation vector , then, project it to image plane using camera intrinsic matrix

[0093]

[0094] where is pinhole camera projection model, is projected pixel coordinate.

[0095] To guarantee the quality of association, three-level verification mechanism is performed: first, filter low-confidence depth points based on ToF sensor's built-in confidence map (threshold can be set); second, remove depth outliers according to the depth continuity of the neighborhood of the projected point; finally, combine the angular velocity information measured by the current IMU to compensate for the optical flow vector caused by its own rotation, in order to exclude the interference of dynamic objects. Finally, a set of strictly verified feature point-depth pairs with absolute scale information is output.

[0096] S4, absolute scale solving

[0097] Based on the feature point-depth pair set output in step 3, a dual-mode strategy is used to recover the metric scale. When the number of valid feature point-depth pairs in the set is greater than the set threshold, mode 1 (least squares optimization mode) with higher accuracy is preferred; otherwise, to ensure real-time performance, mode 2 (direct overlay mode) is enabled.

[0098] Mode 1: By constructing the re-projection error equation of all feature point-depth pairs, a global scale factor is optimized and solved in a least squares manner, and the factor is used to uniformly scale all scale-uncertain initial three-dimensional map points coordinates obtained by visual SfM preliminary reconstruction.

[0099] For an effective feature point-depth pair, the depth of the scale-uncertain initial three-dimensional map point in the reference frame camera system is , the absolute depth provided by the ToF sensor is , and the scale residual is constructed:

[0100]

[0101] The least squares problem is constructed:

[0102]

[0103] This problem has a closed-form solution, and the scale factor to be solved is ​​

[0104]

[0105] denotes the th , denotes the th , then, scale all scale-uncertain initial 3D map points generated in step two with a scale factor :

[0106]

[0107] get the final 3D map points with real physical scale .

[0108] Mode 2: directly convert the depth value in a feature point-depth pair into a 3D absolute coordinate in the camera coordinate system, and use this coordinate value to overwrite (or directly replace) its corresponding scale-uncertain initial 3D map point obtained by triangulation coordinate.

[0109] directly convert the ToF depth value into a 3D map point in the camera coordinate system.

[0110]

[0111] where is the matched feature point pixel coordinate. Then transform to the world coordinate system, directly replace (overwrite) the original scale-uncertain initial 3D map point obtained by triangulation .

[0112] S5, tightly coupled multi-source joint optimization

[0113] On all keyframes within the sliding window, construct a unified objective function for joint optimization. This function integrates three types of constraint residuals: visual re-projection residuals (characterizing the difference between the observed position of a feature point in the image plane and its projected position from the 3D map point), IMU pre-integration residuals (characterizing the consistency between the relative motion integrated from IMU measurements and the estimated values of optimization variables between adjacent keyframes), and depth measurement residuals (characterizing the difference between the absolute depth value of a map point obtained from step 3 and the depth value of the optimized map point).

[0114] Optimize the state vector contains the states of all keyframes within the sliding window:

[0115]

[0116] where the state of the kth keyframe​ are the position, velocity, rotation quaternion, accelerometer bias, gyroscope bias, respectively, is the inverse depth of the th three-dimensional map point.

[0117] The joint optimization objective function is constructed within a sliding window:

[0118]

[0119] where the visual re-projection error is the position of the map point in the world frame, is the body pose of the kth frame, is the camera-to-IMU extrinsic.

[0120] The IMU pre-integration error :

[0121]

[0122] where , , is the pre-integration quantity of the IMU within the interval , with first-order linear approximation to the bias.

[0123] The depth measurement error , or more simply, the difference in z-axis depth in the camera coordinate frame:

[0124]

[0125] where denotes taking the z-coordinate component. This error directly introduces absolute scale information as a strong constraint into the optimization problem.

[0126] The Levenberg-Marquardt nonlinear optimization algorithm is used to iteratively solve, simultaneously optimizing the poses (rotation and translation) of all keyframes, velocities, IMU gyroscope and accelerometer biases, and three-dimensional coordinates of all map points. This process tightly couples sparse but accurate absolute depth measurement constraints with continuous visual-inertial motion estimation, jointly correcting initial value errors, thereby accurately determining system scale, motion state, and sensor bias.

[0127] S6, convergence criterion detection

[0128] Three key convergence indicators are monitored in real time during the optimization process:

[0129] 1. Standard deviation of scale factor estimation: assesses the stability of the scale solution, which needs to be less than a set threshold .​

[0130] 2. Mean of depth measurement residuals: evaluate the consistency between optimized map points and ToF depth measurements, need to be less than a threshold .

[0131] 3. Change of IMU gyro bias: evaluate whether the bias estimation is converging, need to be less than a threshold .

[0132] Only when all the three indicators meet their respective threshold conditions in consecutive iterations, the system determines that the initialization is successful, and then enters step 7. If the number of optimization iterations exceeds the upper limit and still does not meet the convergence condition, or any indicator shows a divergence trend, the initialization is determined to fail, and then enters step 8. This multi-index joint criterion effectively avoids misjudgment under insufficient information or interference, ensuring the reliability of the initialization result.

[0133] S7, state transfer and sensor management

[0134] Once the initialization is determined to be successful, the system immediately performs state transfer and mode switching:

[0135] 1. State injection: the final system state obtained after joint optimization, including the key frame pose in the world coordinate system, velocity vector, IMU bias estimate value, and three-dimensional map points in metric scale, is injected into the subsequent visual-inertial odometry (VIO) backend tracking and optimization thread as its accurate initial state.

[0136] 2. Sensor management: send low-power instructions to the ToF sensor immediately, making it enter sleep or complete shutdown mode. The system switches to pure visual-inertial mode operation from this point on, relying only on monocular cameras and IMUs for subsequent pose estimation and map construction. This design ensures that the ToF sensor only works briefly during the key window period at the beginning of system startup, maximizing the overall power consumption of the system.

[0137] S8, abnormal handling and rollback mechanism

[0138] To deal with the situation of initialization failure or poor data association quality (such as insufficient number of valid feature point-depth pairs), the system has a complete rollback mechanism:

[0139] 1. Process reset: clear all images, IMU data and temporary map points in the current sliding window, and reset the initialization state machine.

[0140] 2. Traditional method fallback: switch to traditional vision-inertial alignment method which does not rely on ToF sensor. This method mainly aligns the IMU pre-integration result with the relative motion estimated by vision SfM, to solve the initial scale factor, gravity direction and velocity vector. In this mode, ToF sensor remains off to save power consumption.

[0141] 3. Conditional retrigger: the system continuously monitors the motion condition in subsequent runs. When sufficient parallax change is detected (e.g. the UAV has started significant translational motion), the complete initialization procedure starting from step 1 will be automatically retriggered to try to obtain more optimal initial values with ToF assistance. This mechanism ensures that the system can still achieve basic initialization through fallback scheme in extremely unfavorable conditions (such as completely stationary start), and automatically pursue more accurate initialization results when the condition improves, ultimately achieving robust coverage of the entire scene.

[0142] The application also provides a system corresponding to the monocular vision-inertial system initialization method based on fusion time-of-flight ranging, comprising:

[0143] A synchronization module is configured to synchronize the monocular camera, the IMU and the ToF sensor after the system is powered on.

[0144] A reconstruction module is configured to perform preliminary reconstruction of vision SfM.

[0145] A feature point-depth pair set generation module is configured to convert the depth point cloud data collected by the ToF sensor to the camera coordinate system through pre-calibration extrinsic parameters, project it to the image plane, and establish a search domain centered on each projected point, find the nearest neighbor visual feature point in the search domain, and perform three-level verification: filter low-confidence depth points based on the confidence map built-in the ToF sensor; remove depth outliers according to the depth continuity of the projection point neighborhood; combine the angular velocity information measured by the current IMU to compensate for the optical flow vector caused by its own rotation, and output the feature point-depth pair set.

[0146] A scale recovery module is configured to recover the metric scale based on the feature point-depth pair set using a dual-mode strategy.

[0147] A strong coupling module is configured to construct a target function containing visual re-projection residuals, IMU pre-integration residuals and depth measurement residuals within a sliding window, jointly optimize keyframe poses, velocities, IMU biases and map points, and iteratively solve them to form a strong coupling between depth constraints and motion estimation.

[0148] A determination module is configured to determine whether the initialization is successful when the key indicators simultaneously meet their respective threshold conditions in the iteration.

[0149] The switching module is used for injecting the optimized system state into the VIO backend process after the initialization is successful, immediately sending a low-power instruction to close the ToF sensor to the sleep mode, and switching to the pure monocular-IMU mode (i.e., the monocular-IMU combined mode) for running.

[0150] The steps performed in each module are the same as the above method.

[0151] The above examples are only used to illustrate the technical solutions of the present application, but not to limit it; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that the technical solutions recorded in the foregoing examples can still be modified, or some technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. An initialization method for a monocular vision-inertial system fused with time-of-flight ranging, characterized in that: Includes the following steps: Step 1: After the system is powered on, synchronize the monocular camera, IMU, and ToF sensor; Step 2: Preliminary visual SfM reconstruction; Step 3: Transform the depth point cloud data acquired by the ToF sensor into the camera coordinate system using pre-calibrated extrinsic parameters, and project it onto the image plane. Establish a search domain centered on each projection point, find the nearest neighbor visual feature points within the search domain, and perform three-level verification: filter low-confidence depth points based on the confidence map built into the ToF sensor; remove outliers based on the depth continuity test of the projection point's neighborhood; and compensate for the optical flow vector caused by its own rotation by combining the angular velocity information measured by the current IMU, and output a set of feature point-depth pairs. Step 4: Based on the feature point-depth pair set, a dual-mode strategy is used to recover the metric scale. When the number of valid feature point-depth pairs in the set is greater than a set threshold, mode 1 is used; otherwise, mode 2 is enabled. Mode 1: By constructing the reprojection error equations for all feature point-depth pairs, a global scale factor is obtained through least squares optimization. This factor is then used to uniformly scale the coordinates of all initial map points with uncertain scales obtained from the initial visual SfM reconstruction. Specifically: For a valid feature point-depth pair, the initial 3D map points with uncertain scale... The depth in the reference frame camera system is The absolute depth provided by the ToF sensor is Construct scale residuals: Constructing the least squares problem: Subsequently, using the scaling factor Scaling all initial 3D map points with uncertain scale : Obtain the final 3D map points with true physical scale ; Mode 2: Directly convert the depth values ​​in the feature point-depth pair into 3D absolute coordinates in the camera coordinate system, and use these coordinate values ​​to cover or directly replace the corresponding initial map point positions with uncertain scales obtained from triangulation. Specifically: Directly convert the ToF depth values ​​into 3D map points in the camera coordinate system: in It is the pixel coordinates of the matched feature points, then Transform to the world coordinate system to directly replace or cover the initial 3D map points with uncertain scale obtained from the triangulation. ; Step 5: Construct an objective function within the sliding window that includes visual reprojection residuals, IMU pre-integration residuals, and depth measurement residuals. Jointly optimize keyframe pose, velocity, IMU zero bias, and map points, and solve iteratively to make depth constraints and motion estimation strongly coupled. Step 6: When the key indicators simultaneously meet their respective threshold conditions during the iteration, the system determines whether the initialization was successful. Step 7: After successful initialization, inject the optimized system state into the VIO backend process, immediately send a low-power command to shut down the ToF sensor to sleep mode, and switch to pure monocular IMU mode.

2. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: In step 1, after the system is powered on, the monocular camera, IMU and ToF sensor are synchronized by hardware trigger. Timestamp offset compensation is used to ensure that the time deviation of the three source data of the monocular camera, IMU and ToF sensor is less than the threshold Δt.

3. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: Step 2, the preliminary visual SfM reconstruction, specifically includes: performing FAST corner point extraction and LK optical flow tracking on N consecutive frames of images, selecting the frame with a sufficiently large disparity to the current frame and the largest number of matching feature points as the reference frame. and the current frame Matching feature points between There exists a polar constraint: in and Reference frame arrive rotation and translation It is a translation vector antisymmetric matrix, It is the camera's intrinsic parameter matrix; Solving the essential matrix using the five-point method This leads to the decomposition and rotation. unit direction vector of translation Furthermore, DLT triangulation is used to generate an initial map point set with uncertain scale, and a temporary environmental structure is established under the visual coordinate system.

4. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: Step 3, which involves transforming the depth point cloud data acquired by the ToF sensor to the camera coordinate system via pre-calibrated extrinsic parameters and projecting it onto the image plane, specifically includes: A point measured by a ToF sensor Transform to camera coordinate system: in, It is the transformation matrix from the ToF sensor coordinate system to the camera coordinate system, including the rotation matrix. Translation vector Then, using the camera intrinsic parameter matrix Project it onto the image plane: in It is a pinhole camera projection model. These are the pixel coordinates after projection.

5. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: Step 5 includes: Optimize state vector Includes the state of all keyframes within the sliding window: Wherein, the state of the kth keyframe for: These are the position, velocity, rotation quaternion, accelerometer zero bias, and gyroscope zero bias, respectively. It is the first Inverse depth of a map point; Construct a joint optimization objective function within a sliding window: Among them, visual reprojection residual It refers to the location of a map point within the world system. It is the pose of the machine in the k-th frame. It refers to the external parameters from the camera to the IMU; IMU pre-integral residuals : in , , Is the IMU in the range The pre-integral quantity within the zero bias is approximated by a first-order linear approximation; depth measurement residuals Alternatively, the depth difference along the z-axis in the camera coordinate system can be used: in This indicates that the z-coordinate component is taken, and the residual directly introduces the absolute scale information as a strong constraint into the optimization problem. The Levenberg-Marquardt nonlinear optimization algorithm is used to iteratively solve the problem, simultaneously optimizing the pose, velocity, IMU gyroscope and accelerometer zero bias of rotation and translation of all keyframes, as well as the three-dimensional coordinates of all map points. This tightly couples the sparse but accurate absolute depth measurement constraints with the continuous visual inertial motion estimation, jointly correcting the initial value error, thereby determining the system scale, motion state and sensor bias.

6. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: The key indicators in step 6 include: The standard deviation of the scaling factor estimate: used to assess the stability of the scaling solution, it must be less than a set threshold. ; The average value of the depth measurement residuals: used to evaluate the consistency between the optimized map points and the ToF depth measurements, it must be less than a threshold. ; Changes in IMU gyroscope bias: To assess whether the bias estimate tends to stabilize, its norm must be less than a threshold. .

7. The initialization method for a monocular vision-inertial system fused with time-of-flight ranging as described in claim 1, characterized in that: When initialization fails or data association quality is poor, step 8, exception handling, and rollback mechanism are also included: Process Reset: Clear all images, IMU data, and temporary map points within the current sliding window, and reset the initial state machine; Traditional approach fallback: Switch to a traditional vision-inertial alignment method that does not rely on ToF sensors; Conditional re-triggering: The system continuously monitors motion conditions during subsequent operation. When sufficient parallax changes are detected, it will automatically re-trigger the complete initialization process starting from step 1, attempting to obtain better initial values ​​with the help of ToF.

8. An initialization system for a monocular vision-inertial system that integrates time-of-flight ranging, characterized in that: include: The synchronization module is used to synchronize the monocular camera, IMU, and ToF sensor after the system is powered on. Reconstruction module, used for initial visual SfM reconstruction; The feature point-depth pair set generation module is used to transform the depth point cloud data acquired by the ToF sensor to the camera coordinate system through pre-calibrated extrinsic parameters and project it onto the image plane. A search domain is established with each projection point as the center, and the nearest neighbor visual feature points within the search domain are found. Three-level verification is performed: low confidence depth points are filtered based on the confidence map built into the ToF sensor; depth outliers are removed based on the depth continuity test of the projection point's neighborhood; and the optical flow vector caused by its own rotation is compensated by combining the angular velocity information measured by the current IMU, and the feature point-depth pair set is output. The scale recovery module is used to recover the metric scale based on the feature point-depth pair set using a dual-mode strategy. When the number of valid feature point-depth pairs in the set is greater than a set threshold, mode 1 is used; otherwise, mode 2 is enabled. Mode 1: By constructing the reprojection error equations for all feature point-depth pairs, a global scale factor is obtained through least squares optimization. This factor is then used to uniformly scale the coordinates of all initial map points with uncertain scales obtained from the initial visual SfM reconstruction. Specifically: For a valid feature point-depth pair, the initial 3D map points with uncertain scale... The depth in the reference frame camera system is The absolute depth provided by the ToF sensor is Construct scale residuals: Constructing the least squares problem: Subsequently, using the scaling factor Scaling all initial 3D map points with uncertain scale : Obtain the final 3D map points with true physical scale ; Mode 2: Directly convert the depth values ​​in the feature point-depth pair into 3D absolute coordinates in the camera coordinate system, and use these coordinate values ​​to cover or directly replace the corresponding initial map point positions with uncertain scales obtained from triangulation. Specifically: Directly convert the ToF depth values ​​into 3D map points in the camera coordinate system: in It is the pixel coordinates of the matched feature points, then Transform to the world coordinate system to directly replace or cover the initial 3D map points with uncertain scale obtained from the triangulation. ; The strongly coupled module is used to construct an objective function containing visual reprojection residuals, IMU pre-integration residuals and depth measurement residuals within a sliding window, jointly optimize keyframe pose, velocity, IMU zero bias and map points, and solve iteratively to make depth constraints and motion estimation strongly coupled. The determination module is used to determine whether the initialization was successful when key indicators simultaneously meet their respective threshold conditions during iteration. The switching module is used to inject the optimized system state into the VIO backend process after successful initialization, immediately send a low-power command to shut down the ToF sensor to sleep mode, and switch to pure monocular IMU mode.

Images