Visual inertial navigation tightly coupled SLAM method based on four panoramic cameras

Abstract

The present invention provides a visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera, which belongs to the field of robot autonomous positioning and mapping technology, including: extracting image features of images from four independent and synchronous fisheye cameras; correcting the distortion of the fisheye cameras to obtain corrected feature points corresponding to each image feature; performing panoramic projection on the corrected feature points to obtain corresponding panoramic spherical coordinates; performing posture restoration, using a panoramic vision reprojection angle error function to optimize the restored posture; and running the system with pure vision. n After 1 frame, the fusion optimization of vision and IMU is carried out. The error function used in the fusion optimization is the joint adjustment function constructed by the panoramic vision reprojection angle error and the IMU pre-integration error. When the posture tracking is lost, a new map is established, and loop fusion or map fusion is performed according to the loop detection. The present invention can effectively improve the robustness and stability in extreme environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot autonomous positioning and mapping, and in particular relates to a visual inertial navigation tightly coupled SLAM method based on a four-eye panoramic camera. Background Art

[0002] With the rapid development of automation and artificial intelligence technologies, mobile robots are increasingly used in industrial automation, service robots, self-driving cars, drones, etc. However, in order to perform tasks safely and effectively, mobile robots must have the ability to perceive the environment and self-locate. In a known environment, positioning can be achieved through a pre-established map, but in an unknown or dynamically changing environment, the robot must simultaneously perform self-localization and mapping, which is the main motivation for the research of 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 allows robots to work in environments where GPS signals are weak or completely absent (such as indoors, underground spaces, and deep sea and space missions). At disaster sites such as earthquake collapses, SLAM technology can help search and rescue robots draw accessible path maps and conduct search and rescue safely and quickly. As a key technology for mobile robots to achieve environmental perception and intelligent navigation, SLAM has not only continued to receive attention in academia, but also has broad application prospects in the industry, and is one of the important driving forces for the development of intelligent devices.

[0003] The fusion of sensors such as cameras, lidar, IMU (inertial sensors), etc. can greatly improve the robustness and accuracy of SLAM in various extreme environments. But they all have certain defects: laser SLAM can obtain accurate distance information in the scene and construct dense point cloud maps, which has stronger robustness, but it lacks texture information and is prone to positioning failure in scenes with fewer geometric features. In addition, due to its relatively high price, it is difficult to be popularized and applied; the image data collected by the camera in visual SLAM has rich texture information, and due to the large price difference between cameras and lidars, the application cost of the positioning and mapping system can be greatly reduced, but the choice of camera has a great impact on SLAM. For example, monocular vision SLAM often fails to track in dynamic environments due to its small field of view. Although panoramic vision SLAM can show stronger robustness in dynamic environments, in pure visual SLAM, light sensitivity, motion blur, and scale information acquisition are still very difficult problems; IMU will not be affected by the external environment, and can ensure the acquisition of high-precision pose data in scenes with high-speed motion and insufficient texture. However, IMU has zero bias drift and noise errors, which will have a huge impact on the measurement value of IMU, especially in low-speed conditions.

[0004] In order to make up for the shortcomings of pure vision and pure IMU, visual inertial SLAM systems have become a research hotspot in recent years, given the natural complementary characteristics of inertial measurement units and visual sensors. When visual SLAM loses tracking due to drastic changes in illumination or texture loss, IMU can provide better positioning results in a short time, which can be used to constrain the camera's estimated posture and estimate scale information, effectively improving the robustness of visual SLAM. Although single-purpose visual inertial SLAM provides a relatively reliable pose estimation solution, visual inertial SLAM will also fail to estimate pose when it is in a highly dynamic, sparsely textured environment for a long time or when the camera's field of view is blocked. Summary of the invention

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

[0006] The present invention is achieved through the following technical solutions:

[0007] The visual inertial navigation tightly coupled SLAM method based on four panoramic cameras includes the following steps:

[0008] Step S1, extracting image features of images taken by four independent and synchronous fisheye cameras, and performing pre-integration processing on the data of the IMU sensor to obtain the relative displacement, relative rotation and relative speed between two adjacent frames of images;

[0009] 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;

[0010] 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;

[0011] 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;

[0012] 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;

[0013] 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;

[0014] 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.

[0015] Further, 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.

[0016] Furthermore, 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.

[0017] 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 combined to eliminate the mismatched points.

[0018] Furthermore, 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 map 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.

[0019] Furthermore, in step S6, the joint adjustment function is expressed as ,in, Represents the observed map pointj A collection of key frames, express k The state quantity at the moment, Represents the coordinates of a map point. , , , , , , Respectively expressed in i -1 time has arrived i The ideal pre-integrated quantities of rotation, velocity and displacement during this period of time, R i-1 express i -1 is the rotation matrix of the robot relative to the world coordinate system, v i express i The speed of the robot at each moment, p i express i The displacement of the robot relative to the world coordinate system at time g represents the acceleration due to gravity, express i -1 time has arrived i The amount of time change between moments, represents the IMU pre-integration error term, represents the covariance of the IMU pre-integrated error term, l Indicates the number of map points matched by the search. r ij represents the reprojection angle residual term, represents the covariance of the reprojection angle residual term, Represents the kernel function.

[0020] Furthermore, in step S5, n 1The value is 10-20.

[0021] The present invention has the following beneficial effects:

[0022] The invention first extracts the image features of the images of four independent and synchronous fisheye cameras, then corrects the distortion of the fisheye cameras, and then obtains the corrected feature points corresponding to each image feature, and then performs panoramic projection on the corrected feature points to obtain the corresponding panoramic spherical coordinates, and then performs feature matching on the panoramic spherical coordinates of the two adjacent moments, and performs robot posture restoration to track the local map. When the number of processed image frames is not greater than the set n1 frame, it runs with pure vision, that is, the panoramic vision reprojection angle error function is used to optimize the local map after the new map point is created, otherwise the vision and IMU fusion optimization is performed, that is, the panoramic vision reprojection angle error function is used to optimize the local map after the new map point is created. After each key frame fusion optimization is completed, loop detection is performed to perform loop fusion or map fusion. For camera distortion correction, the geometric distortion and manufacturing process distortion of the fisheye camera are corrected respectively, which effectively improves the accuracy of the corrected feature points and significantly reduces the positioning error caused by distortion, thereby improving the overall performance of the system. The information of four independent cameras is panoramically integrated to achieve 360° full field of view coverage, thereby effectively expanding the field of view, minimizing the impact of dynamic information, and significantly improving the robustness and stability of the present invention in extreme environments, so that it can more reliably cope with complex environmental changes. The designed panoramic vision reprojection angle error function and the error function adopted in the fusion optimization are not affected by the length of the sphere radius, thereby further improving the positioning accuracy in the panoramic vision environment, ensuring the minimization of the reprojection error, and improving the accuracy and reliability of the present invention, that is, effectively improving the robustness and stability of the present invention in extreme environments such as high exposure, high dynamics, and low light. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The present invention is further described in detail below with reference to the accompanying drawings.

[0024] Figure 1 It is a flow chart of the present invention.

[0025] Figure 2 This is a schematic diagram of the structure of the installation of four fisheye cameras of the present invention.

[0026] Figure 3 Schematic diagram of panoramic projection of the present invention. DETAILED DESCRIPTION

[0027] like Figure 1 As shown in the figure, the visual inertial navigation tightly coupled SLAM method based on four panoramic cameras includes the following steps:

[0028] Step S1, extracting image features of images from four independent and synchronized fisheye cameras, and performing pre-integration processing on the data of the IMU sensor to obtain the relative displacement, relative rotation and relative speed between two adjacent image frames;

[0029] like Figure 2As shown, four fisheye cameras are arranged on four faces of a rectangular frame, and the four fisheye cameras form a four-eye panoramic camera, which can observe 360° scenes. In the specific implementation, it is necessary to first build a movable robot platform, including a robot and fisheye cameras arranged on the robot, and then calibrate the sensor, including the intrinsic calibration of the camera and IMU, the extrinsic calibration between the camera and IMU, and use the IMU as the panoramic coordinate system. In the feature extraction process, multiple technologies such as corner detection and descriptor calculation are used to ensure that the key information in the image is captured. The extracted features are ORB image features, and the specific extraction process is the existing technology. The pre-integration processing of the IMU sensor data is to support subsequent pose estimation and fusion.

[0030] 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;

[0031] The correction method adopts an improved method based on orthogonal projection and general universal model. Through this method, not only can the distortion of fisheye lens be effectively corrected, but also the geometric structure and detail information of the image can be retained to the greatest extent, and the quality and accuracy of the image can be improved.

[0032] 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, which is specifically expressed as , represents the tangential distortion term, which is specifically expressed as , represents the zenith angle of the incident light, represents the horizontal angle of the incident light, l 1 , l 2 , l 3 represents the polynomial coefficients related to radial distortion, i 1 , i2 , i 3 , i 4 Indicates the angle with the horizontal The 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 angle with the horizontal The correlation coefficient, Represents the polar coordinate radius of the image after distortion correction, specifically expressed as , k 1 , k 2 , k 3 , k 4 , k 5 is the distortion parameter obtained by the calibration method, u r is the radial unit vector, u t is the tangent unit vector, C 1. C 2 are tangential distortion parameters.

[0033] 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;

[0034] The data of the four cameras are integrated into a unified coordinate system through panoramic spherical projection, and the pixel points are projected onto the sphere through the external parameters of the camera and panoramic coordinate system, such as Figure 3 As shown, Figure 3 In the figure, S represents the sphere center of the panoramic camera, O represents the optical center of each independent camera, u represents the coordinates of the feature point on the fisheye image, and u' represents the mapping of the feature point u on the panoramic sphere. The panoramic spherical projection not only ensures the integrity of the field of view, but also helps to eliminate the perspective deviation caused by the difference in camera position, significantly improving the continuity and overall consistency of the image. This method optimizes resource usage, enhances real-time response capabilities, and ensures a high degree of integration of spatial information.

[0035] Use the focal length as the depth information of the feature point to restore the three-dimensional coordinates of the point in the camera coordinate system, and then construct a quadratic equation through the constraint of the panoramic spherical radius ,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, rs represents the radius of the panoramic sphere, which is obtained by the quadratic equation k n Then, substitute it into the formula That is, we get the panoramic spherical coordinates .

[0036] 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;

[0037] Among them, 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. The specific matching process and the epipolar geometry pose restoration process are existing technologies. The epipolar geometry technology can be roughly described as follows: first, find the corresponding pixel positions of the scene points in the two images, and use these corresponding points to estimate the basic matrix, which describes the epipolar constraints between the two images. Then, by decomposing the basic matrix, the relative rotation and translation of the camera can be obtained, thereby restoring the relative pose of the two cameras.

[0038] The panoramic vision reprojection angle error function is constructed by calculating the difference between the angle that a three-dimensional point should have under the panoramic camera's perspective and the actual observed angle position. The map point is converted to the panoramic camera coordinate system through the relative pose relationship between the world coordinate system and the panoramic coordinate system. The converted coordinate point is then mapped to the sphere. The center of the panoramic sphere is used as a reference point to calculate the vector from the spherical mapping point to the center of the sphere, as well as the principal optical axis vector from the center of the sphere to the camera's optical center. These two vectors are then used to calculate the angular cosine value of the map point relative to the camera's principal optical axis. The map point generation process is an existing technology, which uses feature point matching technology to track between multiple frames, and uses triangulation methods to restore three-dimensional coordinates from panoramic spherical feature points from different perspectives to generate map points.

[0039] After calculating the above angle cosine value, the image feature points are panoramically mapped to obtain the full spherical coordinates of the feature points. Then, the cosine value of the angle between the panoramic spherical coordinates of the feature points and the main optical axis of the panoramic camera is solved, and the residual function (i.e., panoramic vision reprojection angle error function) is constructed with the cosine value calculated from the map point. 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 map points, Indicates i The vector from the panoramic sphere center coordinates at the moment to the panoramic camera optical center, 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 center of the panoramic sphere can be calculated through the physical structure when the robot platform is built and is a known quantity.

[0040] 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;

[0041] 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 thann 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;

[0042] In this embodiment, n 1=10. When the processed image is less than or equal to n 1 frame, it means that the current stage should be pure visual operation. When the number of processed image frames is greater than n When the frame is 1, it means that the current state should be in the vision and IMU fusion operation stage, and IMU initialization is required during the IMU fusion operation stage.

[0043] The joint adjustment function is expressed as ,in, Represents the observed map point j A collection of key frames, express k The state quantity at the moment, Represents the coordinates of a map point. , , , , , , Respectively expressed in i -1 time has arrived i The ideal pre-integrated quantities of rotation, velocity and displacement during this period of time, R i-1 express i -1 is the rotation matrix of the robot relative to the world coordinate system, v i express i The speed of the robot at each moment, p i express i The displacement of the robot relative to the world coordinate system at time g represents the acceleration due to gravity, express i -1 time has arrived i The amount of time change between moments, represents the IMU pre-integration error term, represents the covariance of the IMU pre-integrated error term, l Indicates the number of map points matched by the search. r ij represents the reprojection angle residual term, represents the covariance of the reprojection angle residual term, Represents the kernel function.

[0044] Step S7: loop detection is performed. 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.

[0045] When the operation starts, a new map is created to record the robot's position and map points. 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). A key frame refers to an image frame that the system considers to be more important among all image frames. For example, if the current frame is very different from the previous frame, the system considers this to be a very important frame and needs to be selected and recorded. The process of determining the key frame is an existing technology. The optimization of the key frame refers to completing the fusion optimization in step S5.

[0046] Loop closure refers to the robot moving to a place it has been before. Loop detection refers to the system extracting and storing keyframes containing rich feature points, and using the ORB feature descriptor to match the current keyframe with the previous keyframe. To speed up the matching, the system uses the Bag of Words (BoW) model to describe the keyframes and identify possible loop candidate frames through fast search. Once a loop is detected, the system will correct the posture through the similarity transformation optimization algorithm to reduce the drift error. This process is an existing technology.

[0047] Loop fusion means that the system detects a loop in the current active map and aligns the map through similarity transformation. Map fusion means that the system detects a loop that is not in the current active map, and aligns the detected inactive map with the current active map.

[0048] The above description is only a preferred embodiment of the present invention, and therefore cannot be used to limit the scope of implementation of the present invention. That is, equivalent changes and modifications made according to the scope of the patent application of the present invention and the contents of the specification should still fall within the scope covered by 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 performing pre-integration processing on 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: loop detection is performed. 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. 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 map points, Indicates i The vector from the panoramic sphere center coordinates at the moment to the panoramic camera optical center, 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, which can observe 360° scenes. rs Indicates the radius of the panoramic sphere; In step S6, the joint adjustment function is expressed as ,in, Represents the observed map point j A collection of key frames, express k The state quantity at the moment, Represents the coordinates of a map point. , , , , , , Respectively expressed in i -1 time has arrived i The ideal pre-integrated quantities of rotation, velocity and displacement during this period of time, R i-1 express i -1 is the rotation matrix of the robot relative to the world coordinate system, v i express i The speed of the robot at each moment, p i express i The displacement of the robot relative to the world coordinate system at time g represents the acceleration due to gravity, express i -1 time has arrived i The amount of time change between moments, represents the IMU pre-integration error term, represents the covariance of the IMU pre-integrated error term, l Indicates the number of map points matched by the search. r ij represents the reprojection angle residual term, represents the covariance of the reprojection angle residual term, Represents the kernel function.

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: In the step S5, n 1The value is 10-20.

Images