Online Camera Intrinsic Calibration and Target Localization Method Based on IMU and Monocular Vision
By simultaneously acquiring monocular camera images and IMU data in a mobile robot, and utilizing IMU pre-integration and visual feature matching for online calibration of camera intrinsic parameters and target depth estimation, the problems of camera intrinsic parameter drift and target localization error accumulation in mobile robots are solved, achieving high-precision and robust target localization.
Patent Information
- Application Number
- CN202611304804.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-08-26
- Publication Date
- 2026-11-17
AI Technical Summary
Existing technologies cannot effectively calibrate camera intrinsic parameters online in mobile robots, leading to the accumulation of target localization errors. Furthermore, existing solutions do not fully utilize the high-frequency motion information provided by IMU sensors, making it difficult to maintain high accuracy and robustness in complex environments.
By simultaneously acquiring images from a monocular camera and IMU data, inter-frame motion priors are obtained using IMU pre-integration. Combined with visual feature matching, bundle adjustment optimization is constructed to achieve online calibration of camera intrinsic parameters. The target depth is calculated using the triangulation principle, and sliding window joint optimization is used to continuously correct the estimation accuracy.
It enables real-time calibration of camera intrinsic parameters and target depth estimation during robot movement, effectively compensating for intrinsic parameter drift, improving target positioning accuracy and robustness, reducing system cost, and is suitable for lightweight design of inspection robot platforms.
Smart Images

Figure CN123118784A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot visual perception and autonomous navigation technology, and in particular to a method for online calibration of camera intrinsic parameters and target localization based on IMU and monocular vision. Background Technology
[0002] In environmental perception and target localization tasks for mobile robots, monocular cameras are widely used due to their low cost, small size, and rich information. However, monocular cameras cannot directly acquire depth information, and the accuracy of camera parameters (focal length, principal point, distortion coefficients, etc.) directly affects the accuracy of subsequent 3D measurements.
[0003] Traditional camera calibration methods require offline calibration using calibration boards (such as checkerboard patterns), which are cumbersome and cannot adapt to camera parameter drift caused by factors such as temperature and vibration. For continuously moving robots, the intrinsic parameters calibrated offline may deviate over long-term operation, leading to accumulated target positioning errors. Therefore, there is an urgent need for a method that can perform camera intrinsic parameter calibration online during robot movement to overcome the shortcomings of offline calibration, such as cumbersome operation and inability to adapt to parameter drift.
[0004] On the other hand, monocular target depth estimation is a core challenge in calculating the relative position of a target. Existing solutions mainly include: monocular depth estimation based on deep learning, structure of motion recovery (SfM) based on multi-view geometry, and depth acquisition based on additional sensors such as LiDAR. Among these, deep learning methods rely on a large amount of training data and have limited generalization ability, making it difficult to guarantee reliability in specific scenarios such as substations and converter stations; pure vision SfM methods are prone to failure in low-texture scenes, and the tracking loss problem is particularly prominent in fast-moving or transiently occluded scenes; additional sensors increase system cost and complexity, which is not conducive to the lightweighting and miniaturization of robots.
[0005] Furthermore, for inspection robots, none of the above solutions effectively utilize the high-frequency motion information provided by the robot's onboard IMU sensors. An IMU, or Inertial Measurement Unit, is an inertial sensor capable of continuously measuring an object's three-axis acceleration and three-axis angular velocity. None of the above solutions simultaneously utilize the high-frequency motion priors provided by the IMU to assist in the online calibration of camera intrinsic parameters and the accurate estimation of target depth, resulting in the IMU's potential not being fully realized.
[0006] Therefore, there is an urgent need for a technical solution that can perform online camera intrinsic parameter calibration during robot movement and combine IMU motion information to achieve target depth estimation and relative position calculation, so as to improve the perception accuracy and robustness of mobile robots in unknown environments. This invention is proposed to address the above-mentioned technical needs. Summary of the Invention
[0007] To address the aforementioned shortcomings of existing technologies, this invention provides an online camera intrinsic parameter calibration and target localization method based on IMU and monocular vision. During robot movement, this method simultaneously acquires monocular camera image sequences and IMU measurement data. Inter-frame motion priors are obtained through IMU pre-integration, and bundle adjustment optimization is constructed using visual feature matching to achieve online calibration of camera intrinsic parameters. Based on this, the target depth is calculated using the triangulation principle, and the three-dimensional position of the target relative to the robot is obtained through back-projection of a pinhole camera model. Simultaneously, a sliding window joint optimization is used to continuously correct the estimation accuracy. This invention integrates camera intrinsic parameter calibration and target position estimation into a unified framework, eliminating the need for offline calibration boards and additional depth sensors. It effectively compensates for intrinsic parameter drift, improving the accuracy and robustness of robot target localization.
[0008] The specific technical solution of the present invention is as follows: This invention provides a method for online calibration of camera intrinsic parameters and target localization based on IMU and monocular vision. The method includes the following steps: Data acquisition and time synchronization: During the robot's movement, image sequences from the monocular camera and data from the IMU's accelerometer and gyroscope are acquired simultaneously, and the two types of data are time-stamped and aligned.
[0009] IMU pre-integration and motion estimation: The IMU data between two adjacent frames is pre-integrated to obtain the robot's relative rotation increment, relative translation increment, and relative velocity increment during that time period, which serve as the initial estimate of the camera motion.
[0010] Image feature extraction and matching: Feature points are extracted from consecutive image frames, and the feature correspondence between frames is established through matching; at the same time, the object detection network is run to output the pixel-level bounding box and category of the detected object.
[0011] Online camera intrinsic parameter calibration: Using the camera motion obtained by IMU pre-integration as a priori, and combining the correspondence of feature points in multiple frames to construct a bundle adjustment optimization problem, the camera intrinsic parameters are iteratively optimized together with the camera pose to achieve online estimation and updating of intrinsic parameters.
[0012] Target depth estimation: For the detected target, stable feature points are selected within its bounding box. Using the triangulation principle in multi-view geometry and the relative poses of adjacent frames within the calibrated camera, the three-dimensional depth of the target feature points is calculated. The depth values of multiple feature points within the target area are statistically fused to obtain the estimated depth of the target.
[0013] Target relative position calculation: Based on the pixel coordinates of the target in the image, the estimated depth, and the calibrated camera intrinsic parameters, the three-dimensional coordinates of the target in the camera coordinate system are calculated by back projection of the pinhole camera model; then, the target's three-dimensional position relative to the robot is obtained by transforming the extrinsic parameters of the camera and IMU to the robot body coordinate system.
[0014] Sliding window optimization and state update: Maintain a fixed-length sliding window containing the most recent N frames of images and their corresponding IMU data, feature points, intrinsic parameter estimates, and target positions; perform joint optimization periodically, while constraining the IMU pre-integration residual, visual reprojection residual, and target depth residual, continuously correcting the target position estimation in the camera's intrinsic parameters.
[0015] Furthermore, in the aforementioned data acquisition and time synchronization steps, the monocular camera operates at a frame rate... Acquire image sequences, IMU at frequency It outputs three-axis acceleration and three-axis angular velocity data, both of which are timestamped. It aligns the camera exposure with the IMU sampling by means of a hardware trigger signal, or integrates the IMU data to the exposure time of the corresponding image frame by means of software interpolation, thereby achieving temporal alignment between visual data and inertial data.
[0016] Furthermore, in the aforementioned IMU pre-integration and motion estimation steps, all IMU measurements between two adjacent frames are pre-integrated to obtain the relative rotation increment. Relative translation increment and relative velocity increment and the corresponding covariance matrix; the accelerometer zero bias is also considered during the pre-integration process. and gyroscope zero bias The zero bias is then incorporated as a state variable into subsequent joint optimization.
[0017] Furthermore, in the above image feature extraction and matching steps, FAST corner points or ORB features are extracted for each frame of the image. Pyramid optical flow or descriptor matching is used to establish the inter-frame feature correspondence. The matching results are then subjected to RANSAC to remove outliers, while inliers are retained for pose solving and optimization. A lightweight object detection network is used to perform forward inference on each frame of the image, outputting the category label, pixel coordinate bounding box, and confidence score of the detected object. Detection results with confidence scores higher than a set threshold are retained and enter the depth estimation process.
[0018] Furthermore, in the above-described online camera intrinsic parameter calibration steps, the camera intrinsic parameter vector is defined as follows: ,in , Focal length , Principal point coordinates , , The radial distortion coefficient is... , Let be the tangential distortion coefficient; the reprojection error introduced by visual observation and the motion error introduced by IMU pre-integration are jointly constructed into a nonlinear least squares problem, and the objective function is: in, For the feature point reprojection residual, For IMU pre-integration residuals, The intrinsic parameters are the weighting coefficients of the IMU residuals; the intrinsic parameters are used as optimization variables for iterative optimization, and an upper limit constraint is set on the intrinsic parameter update step size; when the intrinsic parameter change is lower than the set threshold for multiple consecutive frames, the intrinsic parameters are determined to converge, and the calibration update frequency is switched to periodic verification mode.
[0019] Furthermore, in the aforementioned target depth estimation step, stable feature points are selected within the bounding box and near the edges of the detected target, prioritizing points with large image gradient magnitudes and high tracking quality. For each selected feature point, its pixel observations in at least two frames are used, combined with the relative pose between adjacent frames and the currently calibrated camera intrinsic parameters, to solve for the 3D coordinates of the feature point through linear triangulation and extract its depth value. , This represents the individual depth value obtained after triangulation of the feature point; the depth values of multiple feature points within the same target area are statistically analyzed, outliers are removed, and the median is taken, or a weighted average is calculated based on tracking quality and reprojection error to obtain the estimated target depth. , This represents the final estimated depth of the entire target; when the target is stably detected in multiple consecutive frames, the target depth is optimized using bundle adjustment based on multi-frame observations.
[0020] Furthermore, in the above-mentioned target relative position calculation step, the depth This is the target estimated depth. Based on the pinhole camera model, the pixel coordinates of the target ( ),depth and camera intrinsic parameter matrix Through back projection Obtain the three-dimensional coordinates of the target in the camera coordinate system ; Through the extrinsic parameter matrix between the camera and the IMU The target point in the camera coordinate system is transformed to the IMU coordinate system, and then transformed to the robot base coordinate system through the extrinsic parameters between the IMU and the robot body, thus obtaining the three-dimensional position of the target relative to the robot body. .
[0021] Furthermore, in the above sliding window optimization and state update steps, the sliding window length is 10 to 20 frames, and the window stores the camera pose, velocity, IMU bias, camera intrinsic parameters, feature point observation and target detection results for each frame; when a new frame enters the window and the oldest frame slides out, local joint optimization is performed, and the optimization variables include the pose and velocity of all frames in the window, IMU bias, camera intrinsic parameters, feature point 3D coordinates and target depth; the residual term includes IMU pre-integration residual, feature point reprojection residual and target depth consistency residual, and the nonlinear least squares problem is solved by the Levenberg-Marquardt algorithm.
[0022] The above method is applied to intelligent inspection scenarios of robot dogs or drones in substations or converter stations, and is used to calibrate camera intrinsic parameters online and estimate the three-dimensional position of the target relative to the robot during robot movement.
[0023] The beneficial technical effects of this invention are as follows: 1. Online self-calibration effectively adapts to intrinsic parameter drift. This invention does not rely on auxiliary tools such as calibration boards and can continuously optimize camera intrinsic parameters during normal robot movement. Compared with the one-time calibration and long-term use mode of traditional offline calibration methods, this invention can track intrinsic parameter changes in real time during robot operation, effectively compensating for intrinsic parameter drift caused by factors such as temperature changes, mechanical vibration, and lens deformation. This feature is particularly critical for inspection robots that operate continuously for long periods, such as in substations and converter stations—as the operating time increases, the accuracy of intrinsic parameter estimation not only does not degrade but also converges continuously due to the accumulation of observation data, fundamentally overcoming the inherent defect of offline calibration in accumulating positioning errors after long-term operation. At the same time, this invention dynamically adjusts the calibration update frequency according to the intrinsic parameter convergence state, with rapid convergence in the early stage and low-frequency verification in the later stage, effectively controlling computational overhead while ensuring calibration accuracy, and can be well adapted to the computing power constraints of embedded robot platforms.
[0024] 2. IMU-assisted design significantly improves system robustness. This invention fully utilizes the robot's onboard IMU sensor to obtain high-frequency motion priors between adjacent frames through pre-integration. This prior information plays a crucial role in the following scenarios: when rapid camera movement causes image blurring, when scene texture is scarce leading to insufficient feature tracking, and when the target is briefly occluded causing visual observation interruption, the continuous motion constraints provided by the IMU can still maintain relatively reliable pose estimation, effectively reducing the failure probability of pure vision methods in the above-mentioned degraded scenarios. Simultaneously, the IMU prior also provides additional observation constraints for the online calibration of camera intrinsic parameters, enabling the calibration process to maintain stable convergence even when visual observation quality fluctuates. Furthermore, when short inter-frame baselines or sparse scene textures lead to a decrease in the accuracy of pure visual triangulation, the inter-frame pose priors provided by the IMU can effectively supplement the deficiencies of visual geometric constraints, ensuring that monocular depth estimation remains continuously available during robot movement.
[0025] 3. Monocular 3D localization significantly reduces system costs. This invention relies solely on a monocular camera and an IMU (Insulated Measurement Unit) to perform target depth estimation and 3D position calculation, eliminating the need for additional depth sensors such as LiDAR or depth cameras. Since IMUs are already standard sensors on mobile robot platforms, their introduction does not increase hardware costs. The system cost and size are significantly lower than solutions relying on LiDAR or binocular stereo vision, facilitating lightweight robot design and miniaturized deployment, making it particularly suitable for cost- and size-sensitive inspection robot platforms.
[0026] 4. Sliding Window Joint Optimization for Continuously Improved Accuracy. This invention integrates camera intrinsic parameters, IMU state, feature point depth, and target position into a unified joint optimization framework using a sliding window mechanism. These state variables form mutual constraints: the IMU pre-integration residual provides motion constraints, the visual reprojection residual provides geometric constraints, and the target depth residual provides semantic constraints. As the robot continues to move and new observation data is added, the state estimates converge alternately and mutually reinforce each other in the joint optimization: improved intrinsic parameter accuracy directly improves the accuracy of target depth estimation, while a more accurate target position provides more reliable observation information for pose optimization in subsequent frames, forming a positive cycle. The positioning accuracy gradually improves over time, and its long-term operational stability is superior to schemes that optimize each module independently.
[0027] 5. Modular design for easy engineering integration. The intrinsic parameter calibration, target detection, and depth estimation modules of this invention maintain clear interfaces and decoupling. The detection network can be flexibly replaced with different lightweight models, and the feature extraction algorithm can be selected from multiple schemes as needed. The algorithm parameters of each module (such as sliding window length, optimization frequency, convergence threshold, etc.) can be independently optimized according to the computing resources of the specific robot platform and application scenario. This modular design enables this invention to quickly adapt to different robot platforms and application scenarios, exhibiting good portability and engineering practicality.
[0028] 6. Deep Feature Fusion with Both Interpretability and Generalization: This invention triangulates multiple feature points within the detection target region and fuses them into the target depth using median-based outlier removal. Compared to deep learning methods that directly regress depth, the depth estimation process of this invention is based on multi-view geometry principles, possessing clear physical and geometric interpretability. Furthermore, it does not rely on a large amount of labeled scene training data and exhibits good generalization ability across various inspection scenarios. Attached Figure Description
[0029] Figure 1 This is a schematic diagram of the overall process of the online camera intrinsic parameter calibration and target localization method based on IMU and monocular vision of the present invention; Figure 2 This is a block diagram illustrating the principle of the camera intrinsic parameter online calibration module in this invention. Figure 3 This is a geometric schematic diagram of target depth estimation and three-dimensional position calculation in this invention; Figure 4 This is a schematic diagram of the data relationships in the sliding window joint optimization of this invention. Detailed Implementation
[0030] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.
[0031] Figure 1 This diagram illustrates the overall flow of the online camera intrinsic parameter calibration and target localization method based on IMU and monocular vision according to the present invention. Figure 1 As shown, the method provided by this invention mainly includes the following steps: data acquisition and time synchronization, IMU pre-integration and motion estimation, image feature extraction and matching, online calibration of camera intrinsic parameters, target depth estimation, target relative position calculation, and sliding window optimization and state update. The specific implementation methods of each step are described in detail below with reference to the accompanying drawings.
[0032] The system involved in this invention mainly consists of five modules: a data synchronization module, an IMU pre-integration module, a vision processing module, an intrinsic parameter online calibration module, and a target depth and position estimation module. Each module shares state data through a sliding window manager and periodically performs joint optimization.
[0033] Step S1: Data acquisition and time synchronization.
[0034] During the robot's normal movement, sensor data is collected first. The monocular camera uses a frame rate... Acquire grayscale or color image sequences as the raw input for visual perception, where the subscripts... Indicates camera. IMU is expressed in frequency. Synchronously outputs three-axis acceleration and three-axis angular velocity data, providing continuous observation of the robot's own motion information, where subscripts... This refers to the IMU. Both types of data are accompanied by high-precision timestamps for subsequent time alignment. In this embodiment, the camera frame rate... The preferred frequency is 30Hz, which can be adjusted to 15-60Hz depending on the platform's computing power; IMU sampling frequency The preferred frequency is 200Hz, and it is recommended to use no less than 100Hz to ensure the accuracy of pre-integration.
[0035] Since cameras and IMUs are two different types of sensors with different sampling times and frequencies, the time synchronization of these two types of data must be addressed. Otherwise, subsequent IMU pre-integration and visual feature matching will result in serious errors due to time misalignment. This embodiment employs a hardware-software combined time synchronization scheme: hardware trigger signals are prioritized to align the camera exposure time with the IMU sampling time; if the platform does not support hardware triggering, software interpolation is used to integrate or interpolate the IMU data to the exposure time of the corresponding image frame, thereby ensuring strict time alignment between visual and inertial data.
[0036] Step S2: IMU pre-integration and motion estimation.
[0037] After data time synchronization is completed, pre-integration is performed on all IMU measurements between two adjacent frames. The purpose of IMU pre-integration is to... Frame to the All high-frequency IMU observations between frames are compressed into a single relative motion constraint, while avoiding the need to reintegrate all the original IMU data in each optimization iteration, thereby improving computational efficiency.
[0038] Specifically, for the first Frame to the Pre-integrate the IMU data between frames to obtain the robot's relative rotation increment within that time period. Relative translation increment and relative velocity increment ,in Indicates rotation. Indicates translation (position). The velocity is represented. The corresponding covariance matrix is also calculated. These pre-integrated quantities serve as initial estimates of camera motion between two frames, providing reliable prior information for subsequent vision-inertial joint optimization.
[0039] During the pre-integration process, the accelerometer zero bias is also considered. and gyroscope zero bias The impact of this change in IMU bias is significant. When the IMU bias changes, the pre-integrated quantity needs to be updated using a first-order approximation based on the change in bias to avoid redundant integration. Incorporating the bias as a state variable into subsequent joint optimization allows for real-time estimation and correction of the IMU bias error during robot operation, further improving the accuracy of motion estimation. In practice, if the IMU bias changes significantly, pre-integration needs to be performed again to ensure accuracy.
[0040] Step S3: Image feature extraction and matching.
[0041] The vision processing module includes two parallel tasks: feature extraction and matching, and object detection.
[0042] (1) Feature Extraction and Matching. Image feature points are extracted for each frame. In this embodiment, FAST corner points or ORB features are preferred. FAST (Features from Accelerated Segment Test) is a corner detection algorithm known for its extremely fast computation speed, suitable for real-time applications; ORB (Oriented FAST and Rotated BRIEF) features have rotation invariance and scale description capabilities, suitable for use in scenes with large motion amplitude. After feature extraction, the pyramid optical flow method or descriptor matching is used to establish the inter-frame feature correspondence. The pyramid optical flow method is suitable for handling small motions, while descriptor matching is suitable for handling inter-frame matching with large viewpoint changes. To ensure matching quality, the RANSAC (Random Sample Consensus) algorithm is used to remove outliers from the matching results, and high-quality interior points are retained for subsequent pose solving and optimization.
[0043] (2) Object Detection. In this embodiment, a lightweight object detection network is used to perform forward inference for each frame of image, preferably a lightweight model such as YOLOv8-nano or MobileNet-SSD. YOLO (You Only Look Once, a deep learning-based object detection algorithm) and SSD (Single Shot MultiBox Detector, a deep learning-based object detection algorithm) are both common object detection network architectures in this field. Their lightweight versions have a small computational load while ensuring detection accuracy, making them suitable for deployment on embedded robot platforms with limited computing power. The detection network outputs the object's category label and pixel coordinate bounding box (…). , , , The system calculates the depth estimation result and its confidence score. Only detection results with a confidence score higher than a set threshold (preferably 0.5 in this embodiment) are retained in the subsequent depth estimation process, while detection results with low confidence scores are considered false detections and discarded.
[0044] Step S4: Online calibration of camera intrinsic parameters.
[0045] Figure 2 The diagram illustrates the principle block diagram of the online camera intrinsic parameter calibration module in this invention. Online camera intrinsic parameter calibration is one of the core steps of this invention. Using the camera motion obtained through IMU pre-integration as a priori, a bundle adjustment optimization problem is constructed by combining the correspondence of feature points from multiple frames. The camera intrinsic parameters are iteratively optimized together with the camera pose to achieve online estimation and updating of the intrinsic parameters.
[0046] The camera intrinsic vector is defined as ,in , Images direction and Focal length in direction (in pixels). , Principal point coordinates , , The radial distortion coefficient is... , This is the tangential distortion coefficient. Radial distortion mainly originates from manufacturing errors in the lens shape, causing "barrel-shaped" or "pincushion-shaped" distortion in the image; tangential distortion mainly originates from the installation parallelism deviation between the lens and the image sensor.
[0047] Initially, camera intrinsic parameters can be provided with initial values from factory settings or coarse offline calibration. During robot movement, a local bundle adjustment is performed after accumulating a sufficient number of new feature observations. The optimization problem is constructed based on multi-view geometry, while incorporating motion priors provided by IMU pre-integration.
[0048] Specifically, the reprojection error introduced by visual observation and the motion error introduced by IMU pre-integration are jointly constructed into a nonlinear least squares problem. By minimizing the weighted sum of the two types of residuals, the joint estimation of the poses involved in the camera is achieved. The optimization objective function is as follows: in, The reprojection residual of the feature point is the difference between the actual observed position of the feature point in the image and its predicted position projected onto the image plane based on the currently estimated camera pose and 3D point coordinates. The subscript reproj indicates reprojection. The IMU pre-integration residual is the difference between the relative motion given by the IMU pre-integration and the relative motion given by the camera pose estimation. These are the weighting coefficients for the IMU residuals, used to balance the dimensional differences and confidence relationships between the visual and inertial residuals. During the optimization process, camera intrinsic parameters (including distortion coefficients) and the poses of all cameras within the window are used together as optimization variables for iterative optimization.
[0049] To prevent unreasonable large jumps in intrinsic parameters due to insufficient initial observations, an upper limit constraint is set on the update step size of the intrinsic parameters. When the change in the intrinsic parameters is below a set threshold for multiple consecutive frames, the intrinsic parameters are considered to have converged. In this embodiment, the intrinsic parameter convergence threshold is set to a focal length change of less than 0.5%, and the intrinsic parameters are considered to have converged when this condition is met for 5 consecutive frames. At this point, the calibration update frequency is reduced, and a periodic verification mode is switched to save computational resources.
[0050] Step S5: Target depth estimation.
[0051] Figure 3 A geometrical schematic diagram of target depth estimation and three-dimensional position calculation in this invention is shown. For example... Figure 3 As shown in the left-hand section, target depth estimation is another core step of this invention. For the detected target, several stable feature points are selected within its bounding box and near its edges. Points with larger image gradients and higher optical flow tracking quality are preferred during selection, as these points exhibit better observation consistency across different viewpoints and achieve higher triangulation accuracy.
[0052] For each selected feature point, using its pixel observations in at least two frames, combined with the relative pose between the two frames (obtained by IMU pre-integration and visual optimization) and the currently calibrated camera intrinsics, the coordinates of the feature point in 3D space are solved using the Linear Triangulation method, and the depth value is extracted from it. , This represents the individual depth value obtained after triangulation of the feature point. Triangulation requires at least two frames of observation; in this embodiment, three frames are preferably used for triangulation to obtain more robust depth estimation results. The basic principle of triangulation is as follows: Figure 3 As shown, the projected rays of the same 3D point under different camera poses intersect in space, and the intersection point is the 3D position of that point. Due to observation noise, multiple rays usually do not intersect precisely at one point, so the optimal 3D point coordinates need to be solved using the least squares method.
[0053] For multiple feature points within the same target area, each is independently triangulated to obtain multiple depth values. Statistical analysis is then performed on these depth values: first, outliers with significant deviations are removed, and then the median is taken as the representative depth of the target; alternatively, a weighted average is calculated based on the tracking quality and reprojection error of each feature point to obtain the final estimated target depth. , The median represents the final estimated depth of the entire target, which is the result of statistically fusing the depth values of multiple feature points within the target area. The subscript "target" indicates the target. The median method is more robust to outliers, while the weighted average method relies more on the contributions of high-quality feature points.
[0054] The target region feature depth fusion strategy adopted in this embodiment is to triangulate multiple feature points within the target bounding box and then fuse them using median fusion. This effectively utilizes the rich texture information within the target region. At the same time, the outlier removal mechanism suppresses the interference of mismatched feature points on depth estimation. Compared with the scheme that directly regresses the target depth based on deep learning, the depth estimation process of this invention has clear geometric interpretability and does not require a large amount of scene training data. It also has good generalization ability in different application scenarios.
[0055] If the target is stably detected across multiple consecutive frames, bundle adjustment is performed on the target depth using multi-frame observations to further improve the accuracy of depth estimation. By accumulating more observations over time, the length of the triangulated baseline increases, thereby reducing the uncertainty in depth estimation.
[0056] Step S6: Calculate the relative position of the target.
[0057] like Figure 3As shown on the right, after obtaining the target depth, the 3D position of the target relative to the robot is further calculated. Based on the pinhole camera model, this is achieved using the target's pixel coordinates ( ),depth and camera intrinsic parameter matrix The three-dimensional coordinates of the target in the camera coordinate system are obtained through back projection. : in, subscript Represents the camera coordinate system and depth. That is, the target estimated depth output in step S5 The pixel coordinates of the target ( Take the center point of the detection bounding box. If the target bounding box is large, the center point of the bottom edge of the bounding box can also be taken as the target ground point to obtain a more accurate estimate of the ground target position.
[0058] Subsequently, through the extrinsic parameter matrix between the camera and the IMU (This external parameter was obtained through offline system calibration, subscript) This represents the transformation from the camera coordinate system to the IMU coordinate system, converting the target point in the camera coordinate system to the IMU coordinate system; further, through the extrinsic parameter relationship between the IMU and the robot body, it is transformed to the robot base coordinate system, finally obtaining the three-dimensional position of the target relative to the robot body. subscript This represents the robot's body coordinate system. This three-dimensional position is provided to the robot's navigation decision-making module for subsequent target tracking or autonomous operation.
[0059] Step S7: Sliding window optimization and status update Figure 4 The data relationships for joint optimization using a sliding window in this invention are illustrated. For example... Figure 4 As shown, sliding window joint optimization is a key mechanism to ensure the long-term accuracy and stability of the system. The system maintains a sliding window of fixed length. In this embodiment, the window length is preferably 15 frames, and generally ranges from 10 to 20 frames. This value is a trade-off between accuracy and computational load—the longer the window, the more constraints are optimized and the higher the accuracy, but the computational load also increases accordingly; if the window is too short, the constraints are insufficient and the accuracy decreases.
[0060] The data stored in the window includes: camera pose, velocity, and IMU bias (accelerometer bias) for each frame. and gyroscope zero bias The system includes camera intrinsic parameters, observations of feature points in each frame, and target detection results. When a new frame enters the window, the oldest frame is slid out of the window to control computational complexity.
[0061] Each time a new frame is added, a local joint optimization is performed. Optimization variables include: pose and velocity of all frames within the window, IMU bias, camera intrinsic parameters, 3D coordinates of feature points within the window, and target depth. The goal of the optimization is to minimize the weighted sum of the following three types of residual terms: The first type is the IMU pre-integration residual, which constrains the relative rotation, relative translation, and relative velocity between adjacent frames to be consistent with the IMU pre-integration results. The second type is visual reprojection residual, which constrains the observed position of feature points in each frame to be consistent with the predicted position projected onto the image plane based on their three-dimensional coordinates and camera pose. The third type is target depth consistency residual, which constrains the estimated depth values of the same target to remain consistent across different frames.
[0062] The above nonlinear least squares problem is solved using the Levenberg-Marquardt algorithm, which is a nonlinear optimization algorithm that combines the advantages of the Gauss-Newton method and the gradient descent method. It is widely used in bundle adjustment and vision-inertial joint optimization to achieve optimal estimation of all state variables within the window.
[0063] As the robot continues to move and new observation data is added, the various state variables continue to converge in the joint optimization, and the accuracy of the target position estimation within the camera gradually improves with running time.
[0064] The underlying mechanism by which the aforementioned sliding window joint optimization framework can continuously improve the accuracy of intrinsic parameter calibration and target localization is as follows: Traditional methods treat camera intrinsic parameter calibration and target depth and position estimation as two separate processes. Intrinsic parameters are calibrated offline and used permanently, while target localization is based on triangulation using these fixed intrinsic parameters. This fragmented approach has two inherent drawbacks: first, intrinsic parameter errors are directly transmitted to the localization result and cannot be corrected during robot operation; second, target observation information is not used for intrinsic parameter optimization, wasting potentially effective constraints. This invention incorporates camera intrinsic parameters and target position into the same sliding window optimization framework, forming a tight bidirectional coupling between the two through reprojection residuals. (1) The positive effect of intrinsic parameter accuracy on positioning accuracy. The accuracy of target depth triangulation directly depends on the accuracy of camera intrinsic parameters: focal length parameter , The principal point determines the angular resolution of the projected ray. , It determines the correspondence between pixel coordinates and the origin of ray mapping, the radial distortion coefficient. , , and tangential distortion coefficient , This determines the amount of correction for the deviation of the pixel coordinates relative to the ideal ray. Intrinsic parameter error. Jacobian matrix after triangulation Magnified to depth error And then through the back projection formula Transmission as three-dimensional position error When the intrinsic parameters continue to converge toward the truth during the joint optimization process, Monotonically decreasing, and As the target size decreases synchronously, the accuracy of target positioning steadily improves.
[0065] (2) The reverse promoting effect of target observation on intrinsic parameter convergence. Pixel observation of the target region in multiple frames of images provides additional and high-quality constraints for intrinsic parameter optimization. Pixel positions of each target in multiple frames. Compared with the estimated three-dimensional position Camera internal parameters and the pose of the corresponding frame They jointly satisfy the projection equation The aforementioned constraints have the same mathematical power as the reprojection constraints of ordinary feature points, and the targets typically possess more stable tracking quality (detection and tracking are performed jointly) and more explicit semantic correspondences, thus providing high-quality observations with long baselines and low mismatches for intrinsic parameter optimization. As the number of target observations increases and their distribution range in the image expands, the observability of each parameter of the camera's intrinsic parameters gradually increases, and the convergence speed and convergence accuracy are correspondingly improved.
[0066] (3) Convergence guarantee of the joint optimization framework. In sliding window bundle adjustment, the optimization variables include camera intrinsic parameters. , each frame pose (including rotation) With translation IMU zero bias (including accelerometer zero bias) Zero bias of gyroscope ), 3D coordinates of feature points and target location The residual term includes the feature point reprojection residual. IMU pre-integration residuals Consistency residual with target depth This nonlinear least squares problem exhibits good local convergence under the following conditions: initial intrinsic parameters are close to true values (guaranteed by factory calibration or coarse offline calibration), sufficient disparity variation exists within the window (guaranteed by normal robot movement), and the IMU provides a reliable scale reference and high-frequency motion constraints. Under these conditions, the Levenberg-Marquardt algorithm can obtain a stable local optimum, and the accuracy of the intrinsic parameters and target position estimation continuously improves with the accumulation of the number of effective observations within the window. Under the assumption of zero-mean observation noise, the estimated covariance monotonically decreases with the increase of the rank of the information matrix.
[0067] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, and for those of ordinary skill in the art, various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention. Therefore, the present invention is not limited to the specific details without departing from the general concept defined by the claims and their equivalents.
Claims
1. A method for online calibration of camera intrinsic parameters and target localization based on IMU and monocular vision, characterized in that, Includes the following steps: Data acquisition and time synchronization: During the robot's movement, image sequences from the monocular camera and data from the IMU's accelerometer and gyroscope are acquired simultaneously, and the two types of data are time-stamped and aligned. IMU pre-integration and motion estimation: The IMU data between two adjacent frames is pre-integrated to obtain the robot's relative rotation increment, relative translation increment, and relative velocity increment during that time period, which serve as the initial estimate of the camera motion. Image feature extraction and matching: Extract feature points from consecutive image frames and establish inter-frame feature correspondences through matching; Simultaneously, the object detection network outputs pixel-level bounding boxes and categories of the detected objects; Online camera intrinsic parameter calibration: Using the camera motion obtained by IMU pre-integration as a priori, and combining the correspondence of feature points in multiple frames to construct a bundle adjustment optimization problem, the camera intrinsic parameters and camera pose are iteratively optimized together to achieve online estimation and updating of intrinsic parameters; Target depth estimation: For the detected target, stable feature points are selected within its bounding box. Using the triangulation principle in multi-view geometry and the relative poses of the target feature points in adjacent frames after calibration, the three-dimensional depth of the target feature points is calculated. The depth values of multiple feature points in the target area are statistically fused to obtain the estimated depth of the target. Target relative position calculation: Based on the pixel coordinates of the target in the image, the estimated depth, and the calibrated camera intrinsic parameters, the three-dimensional coordinates of the target in the camera coordinate system are calculated by back projection of the pinhole camera model; then, the target's three-dimensional position relative to the robot is obtained by transforming the extrinsic parameters of the camera and IMU to the robot body coordinate system. Sliding window optimization and state update: Maintain a fixed-length sliding window containing the most recent N frames of images and their corresponding IMU data, feature points, intrinsic parameter estimates, and target positions; perform joint optimization periodically, while constraining the IMU pre-integration residual, visual reprojection residual, and target depth residual, continuously correcting the target position estimation in the camera's intrinsic parameters.
2. The method according to claim 1, characterized in that, In the data acquisition and time synchronization steps, the monocular camera operates at a frame rate Acquire image sequences, IMU at frequency Output triaxial acceleration and triaxial angular velocity data, both types of data are accompanied by timestamps; By aligning camera exposure with IMU sampling through hardware trigger signals, or by integrating IMU data to the exposure time of the corresponding image frame through software interpolation, the temporal alignment of visual data and inertial data can be achieved.
3. The method according to claim 1, characterized in that, In the IMU pre-integration and motion estimation step, all IMU measurements between two adjacent frames are pre-integrated to obtain the relative rotation increment. Relative translation increment and relative velocity increment and the corresponding covariance matrix; the accelerometer zero bias is also considered during the pre-integration process. and gyroscope zero bias Furthermore, zero bias is incorporated as a state variable into subsequent joint optimization.
4. The method according to claim 1, characterized in that, The image feature extraction and matching steps include: Feature extraction and matching: Extract FAST corner points or ORB features for each frame of image, establish inter-frame feature correspondence using pyramid optical flow or descriptor matching, and perform RANSAC to remove outliers from the matching results, retaining inliers for pose solving and optimization. Object detection: A lightweight object detection network is used to perform forward inference on each frame of the image, and outputs the category label, pixel coordinate bounding box and confidence score of the detected object. Detection results with confidence scores higher than a set threshold are retained and entered into the depth estimation process.
5. The method according to claim 1, characterized in that, In the online calibration step of the camera intrinsic parameters, the camera intrinsic parameter vector is defined as follows: ,in , Focal length , Principal point coordinates , , The radial distortion coefficient is... , The tangential distortion coefficient; Using the camera motion obtained from IMU pre-integration as a priori, and combining the correspondence of feature points from multiple frames, a bundle adjustment optimization problem is constructed, with the objective function being: in, For the feature point reprojection residual, For IMU pre-integration residuals, These are the weighting coefficients for the IMU residuals; The intrinsic parameters are iteratively optimized by taking the poses of all cameras within the window as optimization variables, and an upper limit constraint is set on the step size of the intrinsic parameter update. When the change in the intrinsic parameters is lower than the set threshold for several consecutive frames, the intrinsic parameters are considered to have converged, and the calibration update frequency is switched to periodic verification mode.
6. The method according to claim 1, characterized in that, In the target depth estimation step, stable feature points are selected inside the bounding box of the detected target and near its edge, and points with large image gradient magnitude and high tracking quality are selected. For each selected feature point, using its pixel observations in at least two frames, combined with the relative pose between adjacent frames and the currently calibrated camera intrinsic parameters, the 3D coordinates of the feature point are solved by linear triangulation, and the depth value is extracted. ; The depth values of multiple feature points within the same target area are statistically analyzed. After removing outliers, the median is taken, or a weighted average is calculated based on tracking quality and reprojection error to obtain the estimated target depth. .
7. The method according to claim 1, characterized in that, In the target relative position calculation step, the depth Estimate the depth of the target Based on the pinhole camera model, the pixel coordinates of the target ( ),depth and camera intrinsic parameter matrix Through back projection Obtain the three-dimensional coordinates of the target in the camera coordinate system ; Through the extrinsic parameter matrix between the camera and the IMU The target point in the camera coordinate system is transformed to the IMU coordinate system, and then transformed to the robot base coordinate system through the extrinsic parameters between the IMU and the robot body, thus obtaining the three-dimensional position of the target relative to the robot body. .
8. The method according to claim 1, characterized in that, In the sliding window optimization and state update steps, the sliding window length is 10 to 20 frames, and the window stores the camera pose, velocity, IMU zero bias, camera intrinsic parameters, feature point observation and target detection results for each frame. When a new frame enters the window and the oldest frame slides out, local joint optimization is performed. The optimization variables include the pose and velocity of all frames in the window, IMU zero bias, camera intrinsic parameters, feature point 3D coordinates and target depth. The residual terms include: IMU pre-integration residual, feature point reprojection residual, and target depth consistency residual, which are solved using the Levenberg-Marquardt algorithm to solve a nonlinear least squares problem.
9. The method according to claim 1, characterized in that, When the target is stably detected in multiple consecutive frames, the target depth is optimized by bundle adjustment using multi-frame observations to further improve the depth estimation accuracy.
10. The method according to any one of claims 1-9, characterized in that, The method is applied to intelligent inspection scenarios of robot dogs or drones in substations or converter stations, and is used to calibrate camera intrinsic parameters online and estimate the three-dimensional position of the target relative to the robot during robot movement.
