Visual inertial navigation tight coupling SLAM method based on four-eye panorama camera

Through the visual inertial tight coupling SLAM method of four-eye panoramic camera and combined with IMU data, the problem of insufficient robustness and stability of SLAM in extreme environments is solved, and high-precision pose tracking and map construction are achieved, which significantly improves the system robustness and stability.

CN118967795AActive Publication Date: 2024-11-15QUANZHOU INST OF EQUIP MFG +2

Patent Information

Application Number
CN202411440551.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-10-16
Publication Date
2024-11-15
Estimated Expiration
2044-10-16

AI Technical Summary

Technical Problem

Existing SLAM technologies are not robust and stable in extreme environments, especially in the case of dynamics, sparse textures or large illumination changes, visual SLAM is prone to tracking loss and pose estimation failure, and the noise error of IMU is difficult to be effective. inhibition.

Method used

Using the visual inertial tight coupling SLAM method based on the four-eye panoramic camera, the robot position position is realized by extracting the image features of the fisheye camera, correcting distortion, panoramic projection, feature matching and optimization, and combining IMU data to perform pre-integration and joint adjustment. High precision tracking and map building.

Benefits of technology

It significantly improves the robustness and stability in extreme environments, expands the field of view, reduces positioning errors, ensures high-precision pose estimation and map construction, and can more reliably cope with complex environment changes.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118967795A_ABST
    Figure CN118967795A_ABST
Patent Text Reader

Abstract

The invention provides a visual inertial navigation tight coupling SLAM method based on a four-eye panorama camera, and belongs to the technical field of robot autonomous positioning and mapping, and the method comprises the following steps: extracting image features of images of four independent and synchronous fisheye cameras; correcting the distortion of the fisheye camera to obtain corrected feature points corresponding to the image features; performing panoramic projection on the corrected feature points to obtain corresponding panoramic spherical coordinates; pose restoration is carried out, and the restored pose is optimized by adopting a panoramic vision re-projection angle error function; after n1 frames of pure vision are operated, vision and IMU fusion optimization is carried out, and an error function adopted by the fusion optimization is a joint adjustment function constructed by a panoramic vision re-projection angle error and an IMU pre-integration error; and when pose tracking is lost, establishing a new map, and executing loopback fusion or map fusion according to loopback detection. According to the method, the robustness and the stability in an extreme environment can be effectively improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous localization and mapping technology, specifically involving a visual-inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera. Background Technology

[0002] With the rapid development of automation and artificial intelligence technologies, mobile robots are increasingly widely used in industrial automation, service robots, autonomous vehicles, drones, and other fields. However, to perform tasks safely and effectively, mobile robots must possess the ability to perceive their environment and self-localize. In known environments, localization can be achieved through pre-built maps, but in unknown or dynamically changing environments, robots must simultaneously perform self-localization and mapping. This is the main motivation behind research into SLAM (Simultaneous Localization and Mapping) technology. Through SLAM technology, robots can autonomously build environmental maps and use them to navigate and plan paths to avoid obstacles. SLAM enables robots to work in environments with weak or no GPS signals (such as indoors, underground spaces, and deep-sea and space missions). In disaster sites such as earthquake collapses, SLAM technology can help search and rescue robots map navigable paths, enabling safe and rapid search and rescue operations. As a key technology for mobile robots to achieve environmental perception and intelligent navigation, SLAM not only continues to receive attention in academia but also has broad application prospects in industry, serving as one of the important driving forces for the development of intelligent devices.

[0003] The fusion of sensors such as cameras, lidar, and IMUs (inertial sensors) can greatly improve the robustness and accuracy of SLAM in various extreme environments. However, they all have certain drawbacks: LiDAR SLAM can obtain accurate distance information in the scene and build a dense point cloud map, which is more robust, but it lacks texture information and is prone to localization failure in scenes with few geometric features. Moreover, its high price makes it difficult to be widely used. In visual SLAM, the image data acquired by the camera has rich texture information, and due to the large price difference between cameras and LiDAR, it can greatly reduce the application cost of localization and mapping systems. However, the choice of camera has a significant impact on SLAM. For example, monocular visual SLAM often fails to track in dynamic environments due to its small field of view. While panoramic visual SLAM can show stronger robustness in dynamic environments, in pure visual SLAM, issues such as lighting sensitivity, motion blur, and scale information acquisition are still very challenging problems. IMU is not affected by the external environment and can ensure the acquisition of high-precision pose data in high-speed motion and low-texture scenes. However, IMU has zero-bias drift and noise errors, especially at low speeds, which can have a huge impact on the IMU's measurement values.

[0004] To compensate for the shortcomings of pure vision and pure IMU, visual-inertial SLAM systems have become a research hotspot in recent years due to the inherent complementary characteristics of inertial measurement units and visual sensors. When visual SLAM loses tracking due to drastic changes in lighting or texture loss, IMUs can provide better localization results in a short time, which can be used to constrain the camera's estimated pose and estimate scale information, effectively improving the robustness of visual SLAM. Although monocular visual-inertial SLAM provides a relatively reliable pose estimation scheme, it can still fail to estimate pose in high-dynamic, sparsely textured environments or when the camera's field of view is obstructed for extended periods. Summary of the Invention

[0005] The purpose of this invention is to propose a visual-inertial tightly coupled SLAM method based on a four-eye panoramic camera, which can effectively improve robustness and stability in extreme environments.

[0006] This invention is achieved through the following technical solution: The vision-inertial tightly coupled SLAM method based on a four-eye panoramic camera includes the following steps: Step S1: Extract image features from images captured by four independent and synchronized fisheye cameras, and perform pre-integration processing on the data from the IMU sensor to obtain the relative displacement, relative rotation, and relative velocity between two adjacent frames. Step S2: Based on the extracted image features, correct the geometric distortion and manufacturing process distortion of the fisheye camera respectively to obtain the corrected feature points corresponding to each image feature. Step S3: Based on the extrinsic parameters between the coordinate system of each fisheye camera and the panoramic coordinate system, perform panoramic projection on the corrected feature points to obtain the corresponding panoramic spherical coordinates. The extrinsic parameters include the rotation matrix and offset of each fisheye camera coordinate system to the panoramic coordinate system. Step S4: Perform feature matching on the panoramic spherical coordinates of two adjacent moments and restore the robot pose to track the local map. If the tracking is lost, proceed to step S5. Otherwise, after determining the new keyframe image, proceed to step S1 and step S6 at the same time. During the tracking of the local map, the panoramic visual reprojection angle error function is used to optimize the restored pose by measuring the angle difference of image reprojection. Step S5: When tracking is lost, a new local map is created. The new local map is set as the active map, and the old local map is saved as the inactive map. Proceed to step S1. Step S6: Create new map points in the local map based on the new keyframes, when the number of processed image frames is less than or equal to nAt frame 1, the panoramic visual reprojection angle error function is used to optimize the local map after creating new map points. After optimization, proceed to step S7 and wait for a new keyframe to arrive before proceeding to step S6. When the number of processed image frames is greater than... n At frame 1, the local map after creating new map points is optimized using a joint adjustment function constructed from the panoramic visual reprojection angle error and the IMU pre-integration error. After optimization, the process proceeds to step S7 and waits for a new keyframe before proceeding to step S6. Step S7: Perform loop closure detection. If the detected loop belongs to an active map, perform loop fusion. If the detected loop belongs to an inactive map, perform map fusion.

[0007] Furthermore, in step S2, according to The distortion of the fisheye camera is corrected, among which, This indicates that the geometric distortion of the fisheye camera is being corrected. This indicates the correction of manufacturing distortions in fisheye cameras, among which... X d Let r be the feature coordinates of the extracted image features, and r be the distance from the feature coordinates to the corresponding image center. λ Indicates the image scaling factor. f For the focal length of a fisheye camera lens, X r for X d The corresponding corrected feature points, Represents the radial distortion term. Indicates the tangential distortion term. Indicates the zenith angle of the incident light. Indicates the horizontal angle of the incident light. This represents the polar radius of the image after distortion correction. u r It is a radial unit vector. u t It is a tangential unit vector. C 1. C 2 are both tangential distortion parameters.

[0008] Furthermore, in step S3, according to the formula Obtain panoramic spherical coordinates ,in, R n Indicates the first n The rotation matrix from the fisheye camera to the panoramic coordinate system, ( t xn , t yn , t zn) indicates the first n The offset of a fisheye camera to the panoramic coordinate system, ( x cn , y cn , z cn ) indicates the corrected feature point at the th position. n Three-dimensional coordinates in the coordinate system of a fisheye camera. k n Indicates the first n The scale of a 3D coordinate point in the coordinate system of a fisheye camera projected onto a sphere is given by the formula... Please solve. rs This represents the radius of the panoramic sphere.

[0009] Furthermore, in step S4, the ORB feature matching algorithm is used to match the panoramic spherical coordinates at two different times, and the RANSAC algorithm is used to remove mismatched points.

[0010] Furthermore, the panoramic visual reprojection angle error function is expressed as follows: ,in, Indicates the first i The panoramic center coordinates of the sphere at time 1 to the 1st j A spherical feature point, Indicates the first i The vector from the panoramic sphere center coordinates to the panoramic camera optical center at any given time. Indicates the first i The panoramic center coordinates of the sphere at time 1 to the 1st j The vector representing the mapping of map points onto the panoramic sphere, specifically represented as: , Indicates the first i The coordinates of the center of the sphere in the panoramic image at any given time. Indicates the pose of the panoramic camera. Indicates the first j The map point is composed of four fisheye cameras, which can observe a 360° view.

[0011] Furthermore, in step S5, the joint adjustment function is expressed as follows: ,in, This represents the set of keyframes for the observed map point j. This represents the state quantity at time k. Represents the coordinates of a map point. , , , , , , Let represent the ideal pre-integral values ​​of rotation, velocity, and displacement during the time interval from time i-1 to time i, respectively. Let Ri-1 represent the rotation matrix of the robot relative to the world coordinate system at time i-1, vi represent the velocity of the robot at time i, pi represent the displacement of the robot relative to the world coordinate system at time i, and g represent the gravitational acceleration. This represents the change in time between time i-1 and time i. This represents the IMU pre-integration error term. Let represent the covariance of the IMU pre-integration error term, l represent the number of map points matched in the search, and rij represent the reprojection angle residual term. This represents the covariance of the reprojection angle residual term. Represents the kernel function.

[0012] Furthermore, in step S5, n The value of 1 ranges from 10 to 20.

[0013] The present invention has the following beneficial effects: This invention first extracts image features from four independent and synchronized fisheye cameras. Then, it corrects the distortion of the fisheye cameras to obtain the corrected feature points corresponding to each image feature. Next, it performs panoramic projection on the corrected feature points to obtain the corresponding panoramic spherical coordinates. Then, it performs feature matching on the panoramic spherical coordinates of two adjacent time points and performs robot pose reconstruction to track the local map. This process is effective when the number of processed image frames is no greater than a set threshold. n In frame 1, pure vision-based operation is performed, meaning the local map after creating new map points is optimized using a panoramic visual reprojection angle error function. Otherwise, a fusion optimization of vision and IMU is performed, again using the panoramic visual reprojection angle error function to optimize the local map after creating new map points. Loop closure detection is performed after each keyframe fusion optimization to execute loop closure fusion or map fusion. For camera distortion correction, geometric distortion and manufacturing process distortion of the fisheye camera are corrected separately, effectively improving the accuracy of the corrected feature points and significantly reducing positioning errors caused by distortion, thereby improving the overall system performance. Panoramic integration of information from four independent cameras achieves 360° full field of view coverage, effectively expanding the field of view, minimizing the impact of dynamic information, and significantly improving the robustness and stability of the invention in extreme environments, enabling it to more reliably cope with complex environmental changes. The designed panoramic visual reprojection angle error function and the error function used for fusion optimization are not affected by the radius of the sphere, thereby further improving the positioning accuracy in the panoramic visual environment, ensuring the minimization of reprojection error, and improving the accuracy and reliability of the invention. In other words, it effectively improves the robustness and stability of the invention in extreme environments such as high exposure, high dynamic range, and low light. Attached Figure Description

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

[0015] Figure 1 This is a flowchart of the present invention.

[0016] Figure 2 This is a schematic diagram of the structure for mounting the four fisheye cameras of the present invention.

[0017] Figure 3 This is a schematic diagram of the panoramic projection of the present invention. Detailed Implementation

[0018] like Figure 1 As shown, the vision-inertial tightly coupled SLAM method based on a four-eye panoramic camera includes the following steps: Step S1: Extract image features from images from four independent and synchronized fisheye cameras, and perform pre-integration processing on the data from the IMU sensor to obtain the relative displacement, relative rotation, and relative velocity between two adjacent image frames; like Figure 2 As shown, four fisheye cameras are positioned on the four faces of a rectangular frame, forming a four-lens panoramic camera system capable of observing a 360° view. In practice, a mobile robotic platform is first constructed, comprising the robot and the fisheye cameras mounted on it. Then, the sensors are calibrated, including intrinsic parameter calibration of the cameras and the IMU, and extrinsic parameter calibration between the cameras and the IMU, using the IMU as the panoramic coordinate system. Feature extraction utilizes various techniques such as corner detection and descriptor calculation to ensure the capture of key information in the image. The extracted features are ORB image features, and the specific extraction process follows existing techniques. Pre-integration of the IMU sensor data supports subsequent pose estimation and fusion.

[0019] Step S2: Based on the extracted image features, correct the geometric distortion and manufacturing process distortion of the fisheye camera respectively to obtain the corrected feature points corresponding to each image feature. The correction method employs an improved approach based on orthogonal projection and a general model. This method not only effectively corrects the distortion of fisheye lenses but also preserves the geometric structure and detail information of the image to the greatest extent, improving image quality and accuracy. Specifically, it is based on... The distortion of the fisheye camera is corrected, among which, This indicates that the geometric distortion of the fisheye camera is being corrected. This indicates the correction of manufacturing distortions in fisheye cameras, among which... X d Let r be the feature coordinates of the extracted image features, and r be the distance from the feature coordinates to the corresponding image center. λ Indicates the image scaling factor. f For the focal length of a fisheye camera lens, X r for X d The corresponding corrected feature points, Represents the radial distortion term, specifically as follows: , This represents the tangential distortion term, specifically expressed as: , Indicates the zenith angle of the incident light. Indicates the horizontal angle of the incident light. l 1 , l 2 , l 3 The polynomial coefficients represent the radial distortion correlation. i 1 , i 2 , i 3 , i 4 Indicates the horizontal angle Correlation coefficient m 1 , m 2 , m 3 Represents the polynomial coefficients related to tangential distortion. j 1 , j 2 , j 3 , j 4 Indicates the horizontal angle Correlation coefficient The polar radius of the image after distortion correction is specifically expressed as: , k 1 , k 2 , k 3 , k 4 , k 5 These are the distortion parameters obtained through calibration. u r It is a radial unit vector. u t It is a tangential unit vector. C 1. C Both 2 are tangential distortion parameters.

[0020] Step S3: Based on the extrinsic parameters between the coordinate system of each fisheye camera and the panoramic coordinate system, perform panoramic projection on the corrected feature points to obtain the corresponding panoramic spherical coordinates. The extrinsic parameters include the rotation matrix and offset of each fisheye camera coordinate system to the panoramic coordinate system. Data from four cameras is integrated into a unified coordinate system via panoramic spherical projection. Pixels are projected onto the sphere using extrinsic parameters from both the cameras and the panoramic coordinate system. Figure 3 As shown, Figure 3 In this diagram, S represents the spherical center of the panoramic camera, O represents the optical center of each individual camera, u represents the coordinates of a feature point on the fisheye image, and u' represents the mapping of feature point u onto the panoramic sphere. Panoramic spherical projection not only ensures the integrity of the field of view but also helps eliminate viewing angle deviations caused by differences in camera position, significantly improving image continuity and overall consistency. This method optimizes resource utilization, enhances real-time responsiveness, and ensures a high degree of integration of spatial information.

[0021] Using the focal length as the depth information of the feature point, the three-dimensional coordinates of the point in the camera coordinate system are reconstructed. Then, a quadratic equation is constructed by constraining the radius of the panoramic sphere. ,in, R n Indicates the first n The rotation matrix from the fisheye camera to the panoramic coordinate system, ( t xn , t yn , t zn ) indicates the first n The offset of a fisheye camera to the panoramic coordinate system, ( x cn , y cn , z cn ) indicates the corrected feature point at the th position. n Three-dimensional coordinates in the coordinate system of a fisheye camera. k n Indicates the first n The scale of a 3D coordinate point in the coordinate system of a fisheye camera projected onto a sphere. rs The radius of the panoramic sphere is obtained through this quadratic equation. k n Then, substitute it into the formula. That is, the panoramic spherical coordinates are obtained. .

[0022] Step S4: Perform feature matching on the panoramic spherical coordinates of two adjacent moments and restore the robot pose to track the local map. If the tracking is lost, proceed to step S5. Otherwise, after determining the new keyframe image, proceed to step S1 and step S6 at the same time. During the tracking of the local map, the panoramic visual reprojection angle error function is used to optimize the restored pose by measuring the angle difference of image reprojection. The ORB feature matching algorithm is used to match the panoramic spherical coordinates at two different times, and the RANSAC algorithm is combined to remove mismatched points. The specific matching process and epipolar geometry pose reconstruction process are existing technologies. Epipolar geometry techniques can be roughly described as follows: First, the corresponding pixel positions of scene points are found in the two images, and the fundamental matrix is ​​estimated using these corresponding points. The fundamental matrix describes the epipolar constraints between the two images. Then, by decomposing the fundamental matrix, the relative rotation and translation of the cameras can be obtained, thereby recovering the relative pose of the two cameras.

[0023] The panoramic visual reprojection angle error function is constructed by calculating the difference between the angle that a 3D point should have from the panoramic camera's viewpoint and the angle position actually observed. Map points are transformed into the panoramic camera's coordinate system using the relative pose relationship between the world coordinate system and the panoramic coordinate system. The transformed coordinate points are then mapped onto a sphere. Using the panoramic sphere's center as a reference point, the vector from the mapped point to the sphere's center and the principal optical axis vector from the sphere's center to the camera's optical center are calculated. These two vectors are then used to calculate the cosine of the angle between the map point and the camera's principal optical axis. The map point generation process utilizes existing technology, namely, tracking across multiple frames using feature point matching techniques and recovering the 3D coordinates from panoramic spherical feature points at different viewpoints using triangulation methods to generate map points.

[0024] After calculating the aforementioned angle cosine values, the image feature points are mapped panoramically to obtain their spherical coordinates. Then, the cosine of the angle between the panoramic spherical coordinates of the feature points and the principal optical axis of the panoramic camera is calculated. This cosine is then used to construct a residual function (i.e., the panoramic visual reprojection angle error function) with the cosine values ​​calculated from the map points. The panoramic visual reprojection angle error function is expressed as follows: ,in, Indicates the first i The panoramic center coordinates of the sphere at time 1 to the 1st j A spherical feature point, Indicates the first i The vector from the panoramic sphere center coordinates to the panoramic camera optical center at any given time. Indicates the first i The panoramic center coordinates of the sphere at time 1 to the 1st j The vector representing the mapping of map points onto the panoramic sphere, specifically represented as: , Indicates the first i The coordinates of the center of the sphere in the panoramic image at any given time. Indicates the pose of the panoramic camera. Indicates the first j There are 10 map points. Among them, the center of the panoramic sphere can be calculated from the physical structure when the robot platform is built, and is a known quantity.

[0025] Step S5: When tracking is lost, a new local map is created. The new local map is set as the active map, and the old local map is saved as the inactive map. Proceed to step S1. Step S6: Create new map points in the local map based on the new keyframes, when the number of processed image frames is less than or equal to n At frame 1, the panoramic visual reprojection angle error function is used to optimize the local map after creating new map points. After optimization, proceed to step S7 and wait for a new keyframe to arrive before proceeding to step S6. When the number of processed image frames is greater than... n At frame 1, the local map after creating new map points is optimized using a joint adjustment function constructed from the panoramic visual reprojection angle error and the IMU pre-integration error. After optimization, the process proceeds to step S7 and waits for a new keyframe before proceeding to step S6. In this embodiment, n 1 = 10. When the processed image is less than or equal to n At 1 frame, it indicates that the current operation should be in the pure vision stage. When the number of image frames processed is greater than 1 frame, the operation should be in the pure vision stage. n At frame 1, it means that the current operation should be in the vision and IMU fusion operation phase. During the IMU fusion operation phase, an IMU initialization is required.

[0026] The joint adjustment function is expressed as follows: ,in, This represents the set of keyframes for the observed map point j. This represents the state quantity at time k. Represents the coordinates of a map point. , , , , , , Let represent the ideal pre-integral values ​​of rotation, velocity, and displacement during the time interval from time i-1 to time i, respectively. Let Ri-1 represent the rotation matrix of the robot relative to the world coordinate system at time i-1, vi represent the velocity of the robot at time i, pi represent the displacement of the robot relative to the world coordinate system at time i, and g represent the gravitational acceleration. This represents the change in time between time i-1 and time i. This represents the IMU pre-integration error term. Let represent the covariance of the IMU pre-integration error term, l represent the number of map points matched in the search, and rij represent the reprojection angle residual term. This represents the covariance of the reprojection angle residual term. Represents the kernel function.

[0027] Step S7: Perform loop closure detection. If the detected loop belongs to an active map, perform loop closure fusion. If the detected loop belongs to an inactive map, perform map fusion. At startup, a new map is created, and the robot's pose and map points are recorded. If tracking fails during operation, the current map is saved as an inactive map, and then a new active map is created (the active map refers to the map currently in use). Keyframes are image frames that the system considers important among all image frames. For example, if the current frame differs significantly from the previous frame, the system considers it a crucial frame that needs to be selected and recorded. The process of determining keyframes is a current technology. Keyframe optimization refers to completing the fusion optimization in step S5.

[0028] Loop closure refers to a robot moving to a previously visited location. Loop closure detection involves the system extracting and storing keyframes containing rich feature points, and using ORB feature descriptors to match the current keyframe with previous keyframes. To accelerate matching, the system employs a Bag of Words (BoW) model to describe the keyframes, quickly searching to identify potential loop closure candidate frames. Once a loop is detected, the system corrects the pose using a similarity transformation optimization algorithm to reduce drift error; this process is existing technology.

[0029] Loop fusion refers to the system aligning a detected loop in the currently active map using a similarity transformation. Map fusion, on the other hand, involves aligning the detected inactive map with the currently active map if the loop is not present in the current active map.

[0030] The above description is merely a preferred embodiment of the present invention and should not be construed as limiting the scope of the present invention. All equivalent changes and modifications made in accordance with the scope of the patent application and the contents of the specification of the present invention should still fall within the scope of the patent of the present invention.

Claims

1. A visual inertial navigation tightly coupled SLAM method based on four panoramic cameras, characterized by: The steps include: Step S1, extracting image features of images taken by four independent and synchronous fisheye cameras, and pre-integrating the data of the IMU sensor to obtain the relative displacement, relative rotation and relative speed between two adjacent frames of images; Step S2: Correcting the geometric distortion and manufacturing process distortion of the fisheye camera according to the extracted image features to obtain corrected feature points corresponding to each image feature; Step S3, performing panoramic projection on the corrected feature points according to the external parameters between the coordinate system of each fisheye camera and the panoramic coordinate system to obtain corresponding panoramic spherical coordinates, wherein the external parameters include the rotation matrix and offset from the coordinate system of each fisheye camera to the panoramic coordinate system; Step S4, feature matching is performed on the panoramic spherical coordinates of the two adjacent moments, and the robot posture is restored to track the local map. If the tracking is lost, step S5 is entered. Otherwise, after determining the new key frame image, step S1 is entered, and step S6 is entered at the same time. In the process of tracking the local map, the panoramic vision reprojection angle error function is used to optimize the restored posture by measuring the angle difference of the image reprojection; Step S5: When tracking is lost, a new local map is created, the new local map is set as the active map, and the old local map is saved as the inactive map, and the process goes to step S1; Step S6: Create a new map point in the local map based on the new key frame. When the number of processed image frames is less than or equal to n 1 frame, the panoramic vision reprojection angle error function is used to optimize the local map after the new map point is created. After the optimization is completed, step S7 is entered and step S6 is entered after waiting for the arrival of a new key frame. When the number of processed image frames is greater than n 1 frame, the local map after the new map point is created is optimized by using the joint adjustment function constructed by the panoramic vision reprojection angle error and the IMU pre-integration error. After the optimization is completed, step S7 is entered and step S6 is entered after waiting for the arrival of a new key frame; Step S7: perform loop detection. When the detected loop belongs to an active map, loop fusion is performed. When the detected loop belongs to an inactive map, map fusion is performed.

2. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 1, characterized in that: In step S2, according to Correct the distortion of the fisheye camera, where: Indicates the correction of the geometric distortion of the fisheye camera. Indicates the correction of the manufacturing process distortion of the fisheye camera, where: X d is the feature coordinate of the extracted image feature, r represents the distance from the feature coordinate to the corresponding image center, λ represents the image scaling factor, f is the focal length of the fisheye camera lens, X r for X d The corresponding corrected feature points are represents the radial distortion term, represents the tangential distortion term, represents the zenith angle of the incident light, represents the horizontal angle of the incident light, represents the polar coordinate radius of the image after distortion correction, u r is the radial unit vector, u t is the tangent unit vector, C 1. C 2 are tangential distortion parameters.

3. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 2, characterized in that: In step S3, according to the formula Get the panoramic spherical coordinates ,in, R n Indicates n The rotation matrix from the fisheye camera to the panoramic coordinate system, ( t xn , t yn , t zn ) indicates the n The offset from the fisheye camera to the panoramic coordinate system, ( x cn , y cn , z cn ) indicates that the corrected feature point is n The three-dimensional coordinates of the fisheye camera in the coordinate system, k n Indicates n The scale of the projection of the three-dimensional coordinate points in the coordinate system of the fisheye camera onto the spherical surface is calculated according to the formula Solution, rs Indicates the radius of the panoramic sphere.

4. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 1, 2 or 3, characterized in that: In step S4, the ORB feature matching algorithm is used to match the panoramic spherical coordinates at two different times, and the RANSAC algorithm is combined to eliminate the mismatched points.

5. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 4, characterized in that: The panoramic vision reprojection angle error function is expressed as ,in, Indicates i The coordinates of the center of the panoramic sphere at the moment j spherical feature points, Indicates i The vector from the coordinates of the panoramic sphere center at the moment to the optical center of the panoramic camera, Indicates i The coordinates of the center of the panoramic sphere at the moment j The specific representation of the vector of the mapping of a map point on the panoramic sphere is , Indicates i The coordinates of the center of the panoramic image at the moment, represents the position of the panoramic camera, Indicates j The panoramic camera consists of four fisheye cameras and can observe 360° scenes.

6. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 5, characterized in that: In step S5, the joint adjustment function is expressed as ,in, represents the set of key frames for the observed map point j, represents the state quantity at time k, Represents the coordinates of a map point. , , , , , , They represent the ideal pre-integrated quantities of rotation, velocity and displacement from time i-1 to time i, Ri-1 represents the rotation matrix of the robot relative to the world coordinate system at time i-1, vi represents the velocity of the robot at time i, pi represents the displacement of the robot relative to the world coordinate system at time i, g represents the acceleration of gravity, It represents the change in time from time i-1 to time i. represents the IMU pre-integration error term, represents the covariance of the IMU pre-integration error term, l represents the number of map points searched and matched, rij represents the reprojection angle residual term, represents the covariance of the reprojection angle residual term, Represents the kernel function.

7. The visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera according to claim 6, characterized in that: In the step S5, n 1The value is 10-20.

Citation Information

Patent Citations

  • Robust panoramic SLAM system and method based on IMU and wheel speed meter tight coupling

    CN113223161A

  • Semantic vision SLAM map construction method, electronic equipment and storage medium

    CN116817887A

  • Method and system for three-dimensional reconstruction of complete indoor scene based on depth camera

    WO2018133119A1

  • Method for tightly-coupling visual slam, terminal and computer readable storage medium

    WO2019169540A1

  • Digital retina-based photographing device positioning method

    WO2020155616A1

Cited By

  • Multi-view-angle-based stage fusion panoramic look-around image generation method and device

    CN120410889A

  • Omnidirectional fisheye image feature tracking and extracting method based on non-parallel virtual binocular

    CN120876897A

  • An omnidirectional fisheye image feature tracking and extraction method based on non-parallel virtual binoculars

    CN120876897B

  • A rotation-invariant panoramic vision encoding system and method based on spherical harmonic transform

    CN122952153A