An intelligent navigation method and system adaptable to dynamic environments

Abstract

The present invention relates to the field of navigation technology, and proposes an intelligent navigation method and system adaptable to dynamic environments. After correcting the image with pose prior, target detection and tracking are performed, the dynamic probability of feature points is estimated by Bayesian filtering under multi-keyframe observations, the weights of feature points are dynamically optimized by combining the time decay factor and reprojection error, static map points are extracted, and the pose is jointly optimized based on the IMU pre-integration residual and visual error to construct a high-precision SLAM map; innovatively introduce the multi-view dynamic probability estimation and the feature point weight optimization mechanism based on reprojection error and time decay factor to synergistically improve the feature screening accuracy and pose estimation stability of the SLAM system in dynamic environments; effectively solve the problems of decreased positioning accuracy, unstable mapping, and insufficient real-time performance existing in traditional SLAM systems in dynamic environments.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation, and more specifically, to an intelligent navigation method and system adaptable to a dynamic environment. Background Art

[0002] The statements in this section merely provide background technical information related to the present invention and do not necessarily constitute prior art.

[0003] With the rapid development of technologies such as mobile robots, drones, autonomous driving vehicles, augmented reality, and virtual reality, higher requirements are put forward for autonomous navigation and environmental perception systems. In this context, the Simultaneous Localization and Mapping (SLAM) technology, as a key technology for an intelligent agent to achieve self-localization and environmental mapping in an unknown environment, has received extensive attention. Early SLAM systems mostly relied on lidar for two-dimensional mapping. With the improvement of computing power and the development of image sensors, visual SLAM has gradually become the mainstream due to its advantages such as low hardware cost and rich information acquisition. There are a large number of dynamic factors in the real world, such as moving pedestrians, vehicles, animals, etc. These dynamic objects significantly increase the difficulty of SLAM in practical applications. How to achieve robust, real-time, and high-precision positioning and mapping in a dynamic environment has become one of the core challenges in current SLAM research.

[0004] Although the SLAM technology has achieved remarkable results in many fields, traditional SLAM systems are usually based on the assumption of a static environment, which severely limits their practicality in dynamic scenarios. The core problems of existing SLAM methods in a dynamic environment include: First, it is difficult to effectively detect and distinguish dynamic and static objects, resulting in dynamic feature points being misused for pose estimation, thereby causing trajectory drift and mapping errors; Second, SLAM systems usually need to rely on deep learning models when detecting dynamic targets. Although the accuracy is high, the demand for computing resources is large, and it is difficult to meet the real-time requirements, especially in embedded systems. Such methods cannot effectively support the real-time recognition and elimination of dynamic points due to excessive inference delay or large deployment difficulty; In addition, although semantic SLAM can identify object categories, it is difficult to judge their motion states, and it is easy to mislabel static objects as dynamic, affecting the accuracy and robustness of the system. Especially when the camera moves violently, the image tilt causes the target bounding box to cover a large amount of background, further exacerbating the difficulty of feature point extraction and target recognition. It can be seen that existing methods do not have accurate recognition of dynamic points, making existing SLAM systems generally have defects such as decreased accuracy, poor robustness, and poor real-time performance in a dynamic environment. Summary of the Invention

[0005] To solve the above problems, the present invention proposes an intelligent navigation method and system adaptable to dynamic environments, innovatively introducing a multi-view dynamic probability estimation and a feature point weight optimization mechanism based on reprojection error and time decay factor, synergistically improving the feature screening accuracy and pose estimation stability of the SLAM system in dynamic environments, and thus achieving real-time high-precision navigation.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] One or more embodiments provide an intelligent navigation method adaptable to dynamic environments, including the following steps:

[0008] Perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information to obtain an estimation of the motion state of the target object, and extract key frames;

[0009] Based on the multi-frame observation information of the current key frame and historical key frames, use Bayesian filtering to recursively estimate the dynamic probability of each feature point, and calculate the dynamic probability of the feature point to screen out dynamic points;

[0010] For the screened feature points, according to the dynamic probability estimation results of the feature points, combine the time decay factor and reprojection error to perform weight feedback update, dynamically adjust the weights of each feature point to screen the feature points, and obtain static feature points with weights higher than the set value as map points;

[0011] Based on the feature point projection error and the pre-integrated residual of the IMU, jointly optimize the camera pose and the IMU state, and construct a SLAM map based on the optimized camera pose and map points.

[0012] One or more embodiments provide an intelligent navigation system adaptable to dynamic environments, including:

[0013] A visual odometer configured to perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information to obtain an estimation of the motion state of the target object, and extract key frames;

[0014] A multi-view probability estimation module configured to recursively estimate the dynamic probability of each feature point using Bayesian filtering based on the multi-frame observation information of the current key frame and historical key frames, and calculate the dynamic probability of the feature point to screen out dynamic points;

[0015] A feature point weight optimization module configured to, for the screened feature points, perform weight feedback update according to the dynamic probability estimation results of the feature points, combine the time decay factor and reprojection error, dynamically adjust the weights of each feature point to screen the feature points, and obtain static feature points with weights higher than the set value as map points;

[0016] The IMU and camera joint optimization module is configured to jointly optimize the camera pose and the IMU state based on the feature point projection error and the pre-integrated residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0018] This embodiment effectively solves the problems existing in traditional SLAM systems in dynamic environments, such as decreased positioning accuracy, unstable mapping, and insufficient real-time performance. On the one hand, by fusing the IMU motion prior information, the accuracy and speed of image correction can be significantly improved, and the adaptability of the system to violently moving scenes can be enhanced; on the other hand, the Bayesian dynamic probability estimation method under multi-frame observations is introduced to effectively identify and eliminate dynamic feature points, improving the accuracy of map point selection. In addition, the feedback mechanism combining the reprojection error and the time decay factor further improves the stability of feature point screening, making the map construction more robust. Through the tight coupling optimization of visual and IMU data, the continuity and accuracy of pose estimation are improved, and at the same time, the IMU error accumulation is suppressed, enabling the visual odometry to still operate stably in complex dynamic environments.

[0019] The advantages of the present invention and the advantages of additional aspects will be described in detail in the following specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] The specification drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute a limitation to the present invention.

[0021] Figure 1 is a flowchart of an intelligent navigation method for adapting to dynamic environments in Embodiment 1 of the present invention;

[0022] Figure 2 is a structural diagram of the DVI-SLAM system model in Embodiment 1 of the present invention;

[0023] Figure 3 is a comparison diagram of the directions of camera stable and moving shooting images in Embodiment 1 of the present invention;

[0024] Figure 4 is a schematic diagram of image correction in Embodiment 1 of the present invention;

[0025] Figure 5 is a schematic diagram of using the IMU to estimate the motion state of dynamic objects in Embodiment 1 of the present invention;

[0026] Figure 6 is a flowchart of feature point weight optimization in Embodiment 1 of the present invention;

[0027] Figure 7(a) shows the feature point extraction results in a dynamic scenario using the ORB-SLAM3 algorithm in the simulation experiment of Embodiment 1 of the present invention;

[0028] Figure 7(b) shows the feature point extraction results in a dynamic scenario using the DVI-SLAM system model of this embodiment in the simulation experiment of Embodiment 1 of the present invention. Detailed implementation manners

[0029] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0030] It should be noted that the following detailed descriptions are all exemplary and are intended to provide further explanations of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0031] It should be noted that the terms used herein are only for describing specific implementation manners and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof. It should be noted that, without conflict, the various embodiments and features in the present invention can be combined with each other. The embodiments will be described in detail below in conjunction with the accompanying drawings.

[0032] Technical term explanations:

[0033] IMU Pre-integration Residuals: The pre-integration residual refers to the difference between the measured error and the actual motion trajectory during the pre-integration of IMU data (i.e., integrating acceleration and angular velocity). It is usually used in SLAM (Simultaneous Localization and Mapping) or vision-inertial navigation systems (VINS) to optimize the accuracy of the system.

[0034] IMU State: The IMU state usually refers to the current estimated state of the IMU, including information such as position, velocity, and attitude (rotation angle). These state variables are used to describe the dynamic changes of the IMU in space and are key estimated quantities in many navigation systems.

[0035] IMU Motion Priors: IMU Motion Priors refer to the prior knowledge about the motion state provided based on the data obtained from IMU sensors (such as acceleration and angular velocity). These prior information are usually used to estimate the motion trajectory of an object, helping the positioning system to more accurately infer the position and orientation of the object in the absence or insufficiency of visual information.

[0036] IMU Accelerometer: An IMU accelerometer is a sensor used to measure the acceleration of an object. It is usually used to detect the acceleration changes of an object in three-dimensional space and can provide information about the speed changes of the object for the navigation system.

[0037] IMU Data: IMU Data generally refers to the raw data collected by IMU sensors, mainly including the output data of accelerometers and gyroscopes. Accelerometers measure acceleration, and gyroscopes measure angular velocity. These data are the basic inputs in navigation and positioning algorithms.

[0038] IMU Pre-integration: IMU Pre-integration refers to integrating the acceleration and angular velocity data of the IMU over a short period of time. Pre-integration can help reduce the impact of sensor noise.

[0039] IMU Measurement Errors: IMU Measurement Errors refer to the measurement errors or inaccuracies caused by various factors when using an Inertial Measurement Unit (IMU) for positioning, navigation, and motion estimation. These errors will affect the accuracy and reliability of the IMU system.

[0040] IMU Residuals: IMU Residuals is a way of expressing the error between the predicted value and the actual observed value during the optimization or state estimation process, and is widely used in Visual Inertial Systems (VIO / VINS) and SLAM systems.

[0041] The present invention proposes an intelligent navigation method and system adaptable to dynamic environments, constructs a visual-inertial SLAM system model adaptable to dynamic environments, which can be abbreviated as DVI-SLAM system model; innovatively integrates object detection technology and multi-viewpoint dynamic probability estimation method, and realizes real-time correction of input images by introducing prior motion information of an Inertial Measurement Unit (IMU), effectively solving the inherent defect of traditional dynamic SLAM algorithms based on object detection in camera rotation tracking. In particular, the motion information provided by the IMU is used to assist the SLAM system in stably extracting static feature points in a highly dynamic environment, ensuring the continuous and reliable operation of the visual odometer. Furthermore, a tight-coupling optimization strategy for visual odometer and IMU data is adopted, and the error accumulation phenomenon of the IMU is effectively suppressed through the deep fusion of sensor data, giving full play to the complementary advantages of vision and inertial measurement units. The following is illustrated with specific embodiments.

[0042] Embodiment 1

[0043] In the technical solutions disclosed in one or more embodiments, as Figures 1 to 6 shown, an intelligent navigation method adaptable to dynamic environments includes the following steps:

[0044] Step 1: Perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information, obtain the motion state estimation of the target object, and extract key frames;

[0045] Step 2: Based on the multi-frame observation information of the current key frame and historical key frames, use Bayesian filtering to recursively estimate the dynamic probability of each feature point, and calculate the dynamic probability of the feature point to screen out dynamic points;

[0046] Step 3: For the screened feature points, according to the dynamic probability estimation result of the feature points, combine the time decay factor and the reprojection error to perform weight feedback update, dynamically adjust the weights of each feature point to screen the feature points, and obtain static feature points with weights higher than the set value as map points;

[0047] Step 4: Based on the feature point projection error and the pre-integration residual of the IMU, jointly optimize the camera pose and the IMU state, and construct a SLAM map based on the optimized camera pose and map points.

[0048] In this embodiment, the motion prior information provided by the IMU is used to perform real-time geometric correction on the input image to correct the image distortion caused by camera rotation or movement, thereby providing stable input for subsequent target detection and tracking. The target detection and tracking module uses a lightweight neural network model to identify dynamic or static objects in the corrected image, and estimates the motion state in combination with the target position change between multiple frames, and further determines the key frame. For the feature points extracted from the key frame image, the Bayesian filtering algorithm is used to dynamically estimate the dynamic probability of each feature point in combination with the observation information in the historical key frame, and the points with high dynamic probability are filtered out. Subsequently, the feature point weights are updated by combining the time attenuation factor (reflecting the decrease in the importance of the feature point over time) and the reprojection error (reflecting the deviation between the feature point and the predicted position) feedback, and the static feature points are selected as map points according to the set threshold. Finally, a tightly coupled optimization strategy is adopted to incorporate the pre-integrated residual of the IMU and the projection error of the feature point into the optimization target, and the six-degree-of-freedom posture of the camera and the IMU state parameters are jointly optimized to improve the mapping accuracy and stability of the entire SLAM system.

[0049] This implementation effectively solves the problems of reduced positioning accuracy, unstable mapping, and lack of real-time performance in traditional SLAM systems in dynamic environments. On the one hand, by integrating IMU motion prior information, the accuracy and speed of image correction can be significantly improved, and the system's adaptability to violent motion scenes can be enhanced; on the other hand, the Bayesian dynamic probability estimation method under multi-frame observation is introduced to effectively identify and eliminate dynamic feature points, thereby improving the accuracy of map point selection. In addition, the feedback mechanism combining reprojection error and time attenuation factor further improves the stability of feature point screening, making map construction more robust. Through the tight coupling optimization of vision and IMU data, the continuity and accuracy of pose estimation are improved, while the accumulation of IMU errors is suppressed, so that the visual odometer can still operate stably in complex dynamic environments.

[0050] In step 11, image correction is first performed. Specifically, based on the acquired posture prior information, the acquired visual image to be processed is rotated to achieve image correction so that the vertical direction of the image is aligned with the gravity direction; this can be achieved through the image correction module in the constructed DVI-SLAM system model;

[0051] In step 11, the vertical direction of the image is aligned with the direction of gravity, that is, the vertical direction of the image (the y-axis of the image coordinate system) is aligned as much as possible with the reference vertical direction determined by the gravity direction in the world coordinate system, so that the "up and down" direction in the image field of view is consistent with the "up and down" direction in the physical world, thereby improving the stability and accuracy of target detection and static feature extraction.

[0052] In step 11, image correction is performed, and the acquired posture prior information includes at least one of the following:

[0053] (1) The IMU pre-integrates to estimate pose information. Specifically, the motion between the previous frame and the current frame is pre-integrated using the data from the IMU accelerometer to obtain the initial pose estimation information.

[0054] (2) Refer to the pose information of the key frame. The pose of the nearest historical key frame obtained by the backend optimization of the SLAM system can be used as the initial estimate value of the current frame.

[0055] The image correction in this embodiment effectively solves the problem of the decrease in the accuracy of target detection when the camera motion amplitude is large by combining IMU data and the pose of the backend key frame, improving the robustness and accuracy of the system in a dynamic environment. To enhance the fault tolerance ability, the pose prior information set in this embodiment includes the pose of the key frame output by the system at the previous moment, and it can still operate stably even in the case of IMU failure. In this case, rely on the reference key frame pose correction provided by the backend optimization, and limit the target tracking to the target tracking in the two-dimensional space. Although the overall performance may decrease, the DVI-SLAM system model can still maintain normal operation to ensure the stability and reliability of the system.

[0056] When the camera motion amplitude is large, the accuracy of target detection will decrease significantly, which is one of the key challenges faced by the semantic-assisted dynamic SLAM system. Specifically, when the camera motion amplitude is large, two situations may occur: First, although the YOLOv8 network used for target detection can identify dynamic objects, due to the image tilt, the bounding box of the detected target may contain a large amount of static background area, resulting in the loss of static feature points, which may further lead to the tracking failure of the SLAM system; Second, if the image tilt angle is too large, as Figure 3 shown, the camera action causes an angle between the y-axis of the image and the direction of gravity , and the semantic network may not be able to accurately identify dynamic objects, thus misincorporating dynamic feature points into the backend optimization process, significantly reducing the positioning accuracy of the system. To address the above problems, the pose information is used to correct the acquired image in step 1.

[0057] In step 11, the method for image correction based on pose prior information includes the following steps:

[0058] Step 111: Extract the gravitational acceleration vector in the pose prior information ;

[0059] Specifically, the gravitational acceleration can be obtained by pre-integrating the data from the IMU accelerometer and gyroscope ;

[0060] Step 112: Extract the y-axis vector of the visual image to be processed, and calculate the angle between the image y-axis and the gravitational acceleration vector based on the acceleration vector and the y-axis vector of ;

[0061] When the camera moves significantly, an angle will appear between the gravitational acceleration vector and the image axis, and its calculation formula is as follows:

[0062] (1);

[0063] Step 113: Construct a rotation correction matrix based on the calculated angle to rotate the image so that the image axis is parallel to the direction of the gravitational acceleration vector ;

[0064] The pixel points of the corrected image are represented as:

[0065] (2);

[0066] where is the rotation correction matrix; is the coordinate of the pixel point of the corrected image, the position after rotation; is the coordinate of the pixel point in the original image;

[0067] Step 114: When the IMU data is unavailable, use the inverse matrix of the reference key frame pose as the correction matrix to rotate the image to the reference direction of the key frame to achieve image correction;

[0068] Although there may be a slight deviation in the reference direction of the reference key frame, precise correction of the image is not required in this embodiment. Assume the pose of the reference key frame is , then:

[0069] ;

[0070] where represents the inverse matrix of the key frame pose, which is used to "inversely rotate" the current image to the reference direction of the key frame to achieve correction;

[0071] The image correction process of this embodiment is shown in the correction schematic diagram as Figure 4As shown, it can ensure that the target detection box is not tilted, reduce the probability of mistakenly including background areas, avoid semantic recognition errors and feature point extraction errors caused by image tilt, improve the accuracy of dynamic and static feature point classification when the camera rotates at a large angle, not only reduce tracking failures, but also provide a data basis for subsequent positioning and target tracking. By combining IMU data and the pose of key frames at the back end, the problem of decreased target detection accuracy when the camera moves significantly is solved. The tilt angle of the image is calculated using the accelerometer data of the IMU, and the image is corrected by rotation, significantly improving the accuracy of target recognition when the camera rotates at a large angle and avoiding tracking failures.

[0072] In step 1, target detection, target tracking, and feature point extraction are performed on the corrected image, the motion state of the target object is estimated, and key frames are extracted.

[0073] Optionally, YOLOv8 target detection network can be used for target detection, and the detected output includes target categories and target candidate boxes; for the target detection results, the DeepSORT target tracking network can be used to achieve target tracking in the two-dimensional space.

[0074] DeepSORT (Deep Simple Online and Realtime Tracking): is a multi-target tracking algorithm for maintaining target identity consistency in image sequences, which can number and maintain the trajectories of detected targets in consecutive frames.

[0075] Step 121: Perform target detection and target tracking, which is implemented in the target detection thread of the DVI-SLAM system model. The target detection thread includes a target detection module and a target tracking module, and includes the following steps:

[0076] Step 121-1: Use the YOLOv8 network to perform target detection on the image, and output multiple target candidate boxes and category information.

[0077] Step 121-2: Screen the dynamic category targets in the detection results.

[0078] Among them, the dynamic categories include but are not limited to "person", "vehicle", etc.

[0079] Step 121-3: Input the screened targets into the DeepSORT target tracking network, and perform inter-frame matching and tracking of the targets through Re-ID features and Kalman filtering. The combination of Re-ID features and Kalman Filter can more effectively estimate the state of the targets (such as position, speed, etc.) and improve the tracking accuracy of the targets in complex environments.

[0080] Among them, the Re-ID features (Re-Identification Features) are features used to identify the same target at different times and from different perspectives in computer vision tasks such as object tracking and person re-identification (abbreviated as Re-ID).

[0081] Step 121-4: Use the Hungarian algorithm to match the tracking target with the historical trajectory to obtain the tracking target and its motion trajectory between consecutive frames;

[0082] Step 122: Extract feature points and fuse the extracted feature points with the object tracking results. This process is implemented in the tracking thread of the DVI-SLAM system model. The tracking thread includes an ORB feature extraction module and a feature point matching module, and includes the following steps:

[0083] Step 122-1: Use the ORB algorithm to extract local feature points from the current frame and historical key frames, and perform correlation fusion;

[0084] Step 122-2: Perform inter-image matching on the ORB feature points between the current frame and the historical key frames;

[0085] Among them, the historical key frames refer to the frame images within a set time before the current frame image. For example, if the current time is t, the key frames extracted at times t-1, t-2, …… t-n can be used as historical key frames, where n is a set value;

[0086] Step 122-3: Judge the association between the feature points and the tracking target through the spatial position overlap relationship, and associate the extracted ORB feature points to the corresponding object tracking trajectory to achieve the association fusion between the feature points and the tracking target;

[0087] Step 123: For the fused feature points, track the change process in multiple frame images, combine IMU data, estimate the motion state of the target in the world coordinate system, and extract the current key frame; this process is implemented in the motion state estimation module of the DVI-SLAM system model;

[0088] The motion state estimation module realizes real-time prediction and updates the motion state of dynamic objects through Kalman filtering and IMU data fusion, accurately distinguishes static and dynamic feature points, and reduces the IMU drift error through the pre-integration method;

[0089] Semantic information can only provide the category of the target object and cannot determine the motion state of the object. For example, it is impossible to distinguish between a stationary and a moving vehicle. Therefore, a SLAM system that relies solely on semantic information may misidentify feature points on a static object as dynamic points, thus affecting the accuracy and robustness of the system. To solve this problem, this embodiment proposes a motion state estimation method based on IMU data. As Figure 5 shown, there are usually a sufficient number of static landmarks (orange dots) in the environment, enabling stable operation of tracking and mapping. In this case, the estimated camera pose is accurate, so the camera can estimate the positions of the observed dynamic objects (blue dots) in the world coordinate system. When the visual odometer cannot track enough landmarks, the optimized historical data is combined with IMU pre-integration to approximate the current camera pose, enabling the estimation of the motion state of dynamic objects. Based on the estimation results of the motion state, sufficient static points are filtered out, thus solving the problem of insufficient static landmarks in the mapping process. For example, if a static vehicle is recognized, the feature points (green dots) on the static vehicle will be used to participate in the mapping, increasing the number of static feature points.

[0090] Among them, the visual odometer refers to a module that estimates the motion trajectory (pose change) of the camera in three-dimensional space by analyzing visual features extracted from consecutive image frames. In this embodiment, it includes the motion state estimation module and the modules in the target detection thread and the tracking thread;

[0091] Furthermore, for the fused feature points, the change process in multiple frames of images is tracked, and combined with IMU data, the motion state of the target in the world coordinate system is estimated, and key frames are extracted; the target object motion state estimation method, through Kalman filtering and IMU data fusion, real-time predicts and updates the motion state of dynamic objects. First, the IMU data pre-integration method is used to calculate the pose change amount between two frames of images, and then the state estimation is updated through Kalman filtering, as follows:

[0092] 1) Initialize the target state: In the initialization stage, assume the initial state of the target object is , including information such as position, velocity, and acceleration; the initial state can be expressed as:

[0093] (3);

[0094] Among them, is the initial position of the object; is the initial velocity of the object; is the initial acceleration of the object.

[0095] 2) Predict the current state based on the previous state and IMU data, and the formula is:

[0096] (4);

[0097] In the above formula, is the prior state estimate value; is the state transition matrix, which describes how the state transfers from the previous moment to the current moment; is the posterior state estimate value at the previous moment. The state transition matrix The specific form of depends on the motion model of the target object.

[0098] The corresponding prior covariance matrix to be updated:

[0099] (5);

[0100] Among them, is the prior state covariance matrix; is the posterior state covariance matrix at the previous moment; is the process noise covariance matrix, which represents the uncertainty in the state transition process.

[0101] 3) Obtain the observation data and construct the observation model, obtain the target pose or velocity observation data from the image frame obtained by visual observation or the IMU pre-integration, and update the target state estimate;

[0102] When new observation data is input, update the state estimate through Kalman filtering. The state update is divided into three steps: calculating the Kalman gain, updating the state value, and updating the covariance matrix. The Kalman gain is used to weigh the confidence of the predicted value and the observed value:

[0103] (6);

[0104] Among them, is the observation matrix, which maps the state space to the observation space; is the observation noise covariance matrix, which represents the uncertainty of the observation data.

[0105] Use the Kalman gain and the observed value to update the state estimate:

[0106] (7);

[0107] Among them, is the posterior state estimate value; is the actual observed value; is the predicted observed value.

[0108] Update the uncertainty of the state:

[0109] (8);

[0110] Among them, is the identity matrix.

[0111] The core of Kalman filter update lies in how to obtain effective observation quantities in a dynamic scenario. When the system (DVI-SLAM) obtains sufficient static feature points in a dynamic environment, the visual odometer can calculate the positions of these feature points in the world coordinate system in the frame image through camera observation and self-localization: :

[0112] (9);

[0113] Among them, represents the position of the feature points in the frame image in the camera coordinate system; represents the transformation matrix of the feature points in the

[0114] 4) Use IMU pre-integration to compensate for visual tracking failure: When vision fails in a dynamic scenario, use IMU data to restore the current camera pose, obtain the motion state of the target, and use the motion state to distinguish static targets and dynamic targets;

[0115] When there are a large number of moving objects in the scene, semantic SLAM systems based on target detection often experience tracking or localization failures. This is because there are not enough static landmark points to support the calculation, resulting in an inability to obtain an accurate pose estimate, i.e., the transformation matrix . To solve this problem, IMU data is introduced to compensate the system, thereby realizing the solution of the pose.

[0116] As an important sensor, the IMU has the advantage of being unaffected by the external environment. However, due to the error accumulation characteristic of the IMU, significant drift errors will occur during long-term operation. Therefore, a method of pre-integrating the IMU data between two image frames is adopted. Assume that the two image frames correspond to the IMU data at the moment and the moment respectively, where there are multiple moments between the moment and the moment to collect IMU data. Then, the change amounts of the rotation matrix, velocity, and position can be expressed as follows:

[0117] (10);

[0118] (11);

[0119] (12);

[0120] Wherein, represents the change in the rotation matrix of the IMU from the moment to the moment; respectively represent the rotation matrices of the IMU at the moment and the moment; represents the angular velocity of the IMU at the moment; is the time increment; represents the change in velocity of the IMU from the moment to the moment; respectively represent the velocities of the IMU at the moment and the moment; is the acceleration due to gravity; represents the acceleration of the IMU at the moment; represents the change in position of the IMU from the moment to the moment; respectively represent the positions of the IMU at the moment and the moment; and respectively represent the zero-bias estimates; and respectively represent the noise terms; represents the change in the rotation matrix of the IMU from the moment to the moment; represents the change in velocity of the IMU from the moment to the moment; represents the change in time of the IMU from the moment to the moment.

[0121] (13);

[0122] (14);

[0123] (15);

[0124] Wherein, represents the pre-integrated displacement of the IMU from the moment to the moment. represents the The position in the world coordinate system at a moment. Indicates at the Transformation matrix from the world coordinate system to the camera coordinate system at a moment. Indicates the Transformation matrix of the camera coordinate system at a moment. Indicates the Position in the camera coordinate system at a moment. Indicates the Position in the inertial coordinate system at a moment. Indicates the transformation matrix from the camera coordinate system to the inertial coordinate system; Indicates the Position in the camera coordinate system at a moment.

[0125] Integrate the obtained new data into the Kalman filter, and integrate the pre-integration result into the current camera pose estimation for use by the Kalman filter, so as to be able to predict the speed of a moving object. If the speed or acceleration in the target state changes significantly, it is determined as a dynamic target; if the state remains stable, it is determined as a static target; the dynamic target will be used for further dynamic modeling or feature elimination. The feature points on the static target can be incorporated into the SLAM mapping, and the feature points determined to be real static points are screened out through semantic information and incorporated into the mapping step of the subsequent steps.

[0126] The above motion state estimation method of this embodiment, through the Kalman filter and IMU data fusion, predicts and updates the motion state of a dynamic object in real time. The IMU data pre-integration method is used to calculate the pose change amount between two frames, and the state estimation is updated through the Kalman filter, significantly improving the estimation accuracy of the motion state of a dynamic object.

[0127] Key frames are usually automatically selected and generated based on indicators such as the frame-to-frame matching quality and the number of co-visible feature points in the SLAM system;

[0128] Step 2 can be implemented in the multi-view probability estimation module, fuse the observation data from multiple key frames, estimate and update the dynamic probability of each feature point based on Bayesian filtering, determine whether it is a dynamic point, and decide whether to eliminate it.

[0129] Based on the multi-frame observation information of the current key frame and historical key frames, use Bayesian filtering to recursively estimate the dynamic probability of each feature point, and calculate the method of screening out dynamic points by the dynamic probability of the feature point, including the following steps:

[0130] Step 21: Extract the co-visible information between the current frame and the historical key frames, and identify the observation data of the feature points;

[0131] Specifically, first calculate the co-visibility rate between the current frame and historical key frames, that is, the number of feature points jointly observed or the overlap degree; then select the N key frames with the highest co-visibility rate from them to form the reference frame set of the current frame; obtain the historical observation data of the target feature points in the key frames of the reference frame set, including image position, viewing angle, and timestamp.

[0132] Step 22: For each feature point extracted from the current frame, extract the observation data in the historical key frames to form an observation sequence ;

[0133] (16);

[0134] Among them, represents the pixel position of the feature point in the key frame image at time t;

[0135] Step 23: Calculate the dynamic probability of the feature point based on the Bayesian formula for the observation sequence, and construct a recursive model:

[0136] (17);

[0137] Among them, is the state at the current moment, is the observation sequence, that is, all the observation data from the initial moment to the current moment, is the initial state, is the normalization factor; is the observation model, indicating the probability of observing under the given state ;

[0138] Based on the Markov assumption, construct a recursive model as the dynamic probability propagation model, and its state transition process can be expressed as:

[0139] (18);

[0140] Step 24: For each frame of observation of the observation sequence, repeatedly execute the Bayesian update process based on the constructed recursive model, use the posterior probability of the previous frame as the prior input of the current frame, and obtain the dynamic probability sequence of each feature point;

[0141] Step 25: Based on the obtained dynamic probability sequence, eliminate the dynamic points by the threshold judgment method;

[0142] In the above implementation manner of this embodiment, for the classification of whether the feature points are static or dynamic, it does not rely on depth information and can adapt to multiple types of visual inputs such as binocular, monocular, or RGB-D, etc., with strong generality and practicability. By using dynamic probability to eliminate dynamic feature points, the first screening of feature points is realized, and the points that may be static are transmitted downward for subsequent mapping;

[0143] In the above implementation manner, based on Bayesian filtering and multi-view information fusion, the estimation and classification of the dynamic probability of feature points are realized. By using the N key frames with the highest co-visibility rate with the current key frame, the dynamic probability of feature points is calculated, and the state estimation is recursively updated through Bayesian filtering, significantly improving the classification accuracy of dynamic and static feature points, and solving the problems of heavy computational burden of the scheme based on image segmentation and insufficient accuracy of the scheme based on object detection.

[0144] In step 3, to achieve the optimization of feature point weights, it can be realized through the constructed feature point weight optimization module. By dynamically adjusting the weights of feature points, the interference of dynamic points to the SLAM system is reduced, and at the same time, the contribution of static points in the optimization is enhanced, thereby improving the accuracy and robustness of the system in a dynamic environment;

[0145] In step 3, the method for dynamically adjusting the weights of each feature point is as Figure 6 shown and includes the following steps:

[0146] Step 31, initialize the feature weights: For the screened feature points, they are initialized to the same weight value;

[0147] Among them, the screened feature points are the remaining feature points after the dynamic feature points are removed in step 2;

[0148] In step 31, first, a weight value is initialized for each screened feature point. Suppose there are N feature points, and the weight of the feature point can be initialized to 1, indicating that each feature point has equal importance at the initial moment, that is:

[0149] (19);

[0150] Step 32, obtain the dynamic probability, construct a weight update formula according to the dynamic probability of the feature points, and dynamically adjust the weights of the feature points so that the higher the dynamic probability of the feature points, the lower the weight;

[0151] According to the dynamic probability of the feature points output by the dynamic probability estimation module , the weights of the feature points are dynamically adjusted. The higher the dynamic probability, the lower the weight; the lower the dynamic probability, the higher the weight. The specific weight update formula is:

[0152] (20);

[0153] Among them, represents the feature point at time weight, is the dynamic probability calculated in step 4.

[0154] Step 33, Time decay weight update: Introduce a time decay factor into the weight update formula to adjust the amplitude of weight adjustment;

[0155] To further optimize the weight adjustment process, introduce a time decay factor to reflect the change of the dynamic probability of feature points over time. The longer the time interval, the more significant the decay of the dynamic probability, and the corresponding reduction in the amplitude of weight adjustment. The weight update formula with time decay:

[0156] (21);

[0157] Among them, is the time interval between the current frame and the previous frame, is the decay coefficient.

[0158] To avoid the weight value being too large or too small, normalize the weight so that its range remains between 0 and 1. The formula for weight normalization is:

[0159] (22);

[0160] Among them, is the normalized weight, is the total number of feature points.

[0161] Step 34, Introduce the weight of the feature point into the optimization objective function to participate in the optimization calculation, and adjust the feature point weight based on the reprojection error feedback;

[0162] In the backend optimization of step 4 of the SLAM system, introduce the weight of the feature point into the optimization objective function to reduce the influence of dynamic points on the optimization result in a weighted form. The constructed optimization objective function:

[0163] (23);

[0164] Among them, represents the state of the key frame; is the camera pose of the current key frame; is the 3D coordinate of the feature point; is the projection function that projects the 3D point onto the image plane; is the observed position of the feature point in the image.

[0165] Through the above optimization objectives, high-weight feature points have stronger constraints, and the influence of low-weight (i.e., high dynamic probability) feature points is automatically weakened.

[0166] Specifically, after each backend optimization iteration of the SLAM system, the feature point weights are feedback-adjusted according to the optimization results. If the reprojection error of a certain feature point remains large, its weight is further reduced; otherwise, its weight is appropriately increased. Feedback adjustment formula:

[0167] (24);

[0168] Among them, is the reprojection error of the feature point; is the feedback adjustment coefficient, which is used to control the amplitude of weight adjustment.

[0169] Step 35. Dynamic feature elimination: Eliminate feature points whose weights are lower than the set weight threshold based on the threshold;

[0170] Specifically, for feature points whose weights are lower than a certain threshold, they are directly eliminated from the optimization process to reduce their negative impact on the system accuracy. The elimination condition is:

[0171] ;

[0172] Among them, is the weight threshold, usually set to 0.1.

[0173] In the weight optimization of Step 3, according to the dynamic probability estimation results of multiple views, the weights of each feature point are dynamically adjusted, and weight feedback update is combined with the time decay factor and reprojection error to enhance the contribution of static feature points in the optimization process and reduce the interference effect of dynamic points. The low-weight feature points are screened out through weights, and a set of high-weight static feature points participating in the backend optimization is obtained, that is, the set of low-weight (high dynamic probability) feature points that have been eliminated. The optimized weights are output, and the updated weight sequence is used for the next round of optimization; it can effectively improve the positioning accuracy and map construction stability of the system in a dynamic environment.

[0174] Step 4 uses a tightly coupled optimization method to fuse the target object motion state estimation results and IMU data, reduce the influence of dynamic objects on the system, and at the same time optimize the accuracy of the system pose estimation;

[0175] Step 4. Joint optimization of the camera pose and feature points based on the feature point projection error and IMU data, including the following steps:

[0176] Step 41. Construct a joint state quantity of the camera pose and IMU state:

[0177] Among them, a camera is mounted on the agent to collect image data. The agent can be an ontology for intelligent navigation such as a robot or an intelligent vehicle; the camera pose includes position and attitude; the IMU state includes the speed, gravitational acceleration, and bias of the IMU.

[0178] Step 42: Based on the reprojection error of the visual feature points and the IMU pre-integration residual, construct a joint optimization objective function according to the weight information of the dynamic feature points output in Step 3.

[0179] Perform tight-coupling optimization on the IMU data and the visual-inertial odometry data. The optimization objective function includes a visual residual term and an IMU residual term. The optimization objective function is as follows:

[0180] (25);

[0181] In the formula, represents the IMU state; represents the projection function that projects the three-dimensional position onto the two-dimensional image plane; represents the observed value of the pixel;

[0182] The inertial residual of the optimization function is , represents the information matrix in the optimization process, specifically as follows:

[0183] (26);

[0184] (27);

[0185] (28);

[0186] (29);

[0187] Among them, , , respectively represent the residuals of rotation, speed, and position of the IMU from the th moment to the th moment. represents the change in the rotation matrix of the IMU from the th moment to the th moment; represents the change in speed of the IMU from the th moment to the th moment. represents the displacement change of the IMU from the th moment to the th moment. represents the logarithmic mapping operation on the matrix. respectively represent the velocities of the IMU at the $i$-th moment and the moment; is the gravitational acceleration; represents the transpose of. represents the transpose of the rotation matrix at the moment. represents the time interval between the moment and the moment, which is used for time calculation. respectively represent the positions at the moment and the represents the time interval.

[0188] Step 43: Solve based on the joint optimization objective function, perform non-linear optimization on the joint state variables, and obtain the camera pose estimation and IMU state estimation of the current frame; use the optimized camera pose and map points to construct or update the SLAM map.

[0189] In this step, the interference of dynamic objects to the system accuracy is effectively reduced by using IMU data. At the same time, the errors of the IMU itself are also effectively compensated through the joint optimization with visual odometry. By combining the advantages of the camera and the IMU, the robustness of the SLAM system in a dynamic environment is significantly improved. Through the tightly coupled optimization method, the visual odometry and IMU data are fused to reduce the impact of dynamic objects on the system. The IMU data and the visual inertial odometry data are tightly coupled and optimized, and the optimization objective function includes visual residuals and IMU residuals, which significantly improves the accuracy of pose estimation.

[0190] A further technical solution also includes a loop detection step, including candidate detection and loop fusion;

[0191] Step 51: Candidate detection:

[0192] Step 511: In the candidate detection stage, the DVI-SLAM system model determines potential candidate frames by extracting the significant features of the current key frame and historical key frames and calculating feature descriptors for feature matching.

[0193] Specifically, for feature matching, frames with high similarity in historical key frames are selected as loop candidate frames by calculating the matching degree;

[0194] Step 512: Exclude incorrect matches through geometric verification (RANSAC algorithm) to obtain the final loop candidate frames.

[0195] Step 52: Loop fusion:

[0196] Step 521: Based on the obtained loop candidate frames, establish the relative pose relationship between key frames through loop constraints;

[0197] Step 522: Use the graph optimization method to optimize the poses of all key frames to reduce the cumulative error;

[0198] Specifically, the optimization objective is to minimize the error between poses to ensure the accuracy of loop detection results. Finally, output the optimized pose information as the basis for subsequent map update and path planning.

[0199] The above intelligent navigation method for adapting to dynamic environments can be implemented by constructing a DVI-SLAM system model. As Figure 2 shown, the DVI-SLAM system model includes: a visual odometer, a multi-view probability estimation module, a feature point weight optimization module, and an IMU and camera joint optimization module;

[0200] The visual odometer is configured to perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information, obtain the motion state estimation of the target object, and extract key frames;

[0201] The multi-view probability estimation module is configured to recursively estimate the dynamic probability of each feature point based on the multi-frame observation information of the current key frame and historical key frames, and calculate the dynamic probability of the feature points to screen out dynamic points;

[0202] The feature point weight optimization module is configured to, for the screened feature points, perform weight feedback update based on the dynamic probability estimation result of the feature points, combine the time decay factor and the reprojection error, dynamically adjust the weights of each feature point to screen the feature points, and obtain static feature points with weights higher than the set value as map points;

[0203] The IMU and camera joint optimization module is configured to jointly optimize the camera pose and the IMU state based on the feature point projection error and the pre-integrated residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points.

[0204] Furthermore, the visual odometer includes an image correction module, an object recognition module, an object tracking module, an ORB feature extraction module, a feature point matching module, and a motion state estimation module;

[0205] The image correction module is configured to rotate the obtained to-be-processed visual image based on the obtained pose prior information to perform image correction so that the vertical direction of the image is aligned with the gravity direction;

[0206] The target recognition module is configured to perform target detection on images using the YOLOv8 network, and output multiple target candidate boxes and class information;

[0207] The target tracking module adopts the DeepSORT target tracking network to perform inter-frame matching and tracking of targets through Re-ID features and Kalman filtering;

[0208] The ORB feature extraction module is configured to extract local feature points from images using the ORB algorithm;

[0209] The feature point matching module is configured to perform inter-image matching of ORB feature points between the current frame and historical key frames;

[0210] The motion state estimation module is configured to associate the ORB feature points output by the feature point matching module to the corresponding target tracking trajectories output by the target tracking module, track the change process in multiple frames of images, combine IMU data, estimate the motion state of the target in the world coordinate system, and extract key frames;

[0211] Among them, the target recognition module and the target tracking module constitute the target detection thread, the ORB feature extraction module, the feature point matching module, and the motion state estimation module constitute the tracking thread; the multi-view probability estimation module, the feature point weight optimization module, and the IMU and camera joint optimization module constitute the local mapping thread;

[0212] Furthermore, it also includes a loop detection thread, including a candidate detection module and a loop fusion module;

[0213] Step 51, The candidate detection module is configured to perform the following process:

[0214] Step 511, In the candidate detection stage, by extracting the significant features of the current key frame and historical key frames and calculating the feature descriptors, perform feature matching to determine potential candidate frames.

[0215] Specifically, for feature matching, high similarity frames in the historical key frames are selected as loop candidate frames by calculating the matching degree;

[0216] Step 512, Exclude incorrect matches through geometric verification (RANSAC algorithm) to obtain the final loop candidate frames.

[0217] Step 52, Loop fusion, is configured to perform the following process:

[0218] Step 521, Based on the obtained loop candidate frames, establish the relative pose relationship between key frames through loop constraints;

[0219] Step 522, Use the graph optimization method to optimize the poses of all key frames to reduce the cumulative error;

[0220] Specifically, the optimization goal is to minimize the error between poses to ensure the accuracy of loop closure detection results. Finally, the optimized pose information is output as the basis for subsequent map update and path planning.

[0221] The DVI-SLAM system model constructed in this embodiment significantly improves the robustness and accuracy of the system in dynamic environments through the innovative design of an image correction module, a multi-view probability estimation module, a feature point weight optimization module, a motion state estimation module, and a backend optimization module. The image correction module combines IMU data and key frame poses to solve the problem of decreased target detection accuracy during large-angle camera motion; the multi-view probability estimation module realizes the accurate classification of dynamic and static feature points through Bayesian filtering and multi-view information fusion; the feature point weight optimization module reduces the interference of dynamic points on system optimization by dynamically adjusting weights; the motion state estimation module uses Kalman filtering and IMU data fusion to predict and update the motion state of dynamic objects in real time; the backend optimization module optimizes the pose estimation accuracy by fusing visual odometry and IMU data through a tightly coupled optimization method. These improvements together enhance the performance and stability of the system in complex dynamic environments.

[0222] The navigation method of this embodiment can be applied to intelligent navigation of robots such as autonomous vehicles, automated guided vehicles, and unmanned ground vehicles in dynamic environments. This method can effectively detect and estimate the motion states of dynamic objects in the environment to ensure the accuracy, robustness, and real-time performance of the navigation system in complex dynamic environments.

[0223] To illustrate the effect of the navigation method of this embodiment, a simulation experiment was conducted. As shown in Figures 7(a) and 7(b), which are comparison diagrams of simulation results, Figure 7(a) shows the feature point extraction results of the ORB-SLAM3 algorithm; ORB-SLAM3 is a SLAM (Simultaneous Localization and Mapping) algorithm based on ORB (Oriented FAST and Rotated BRIEF) features. It is a widely used and verified algorithm in the field of visual SLAM currently. It has performed excellently in multiple application scenarios, especially in real-time localization and map construction. As a mainstream algorithm, ORB-SLAM3 can provide a standard benchmark, and other new SLAM algorithms usually use it as a comparison object to demonstrate their advantages. Figure 7(b) shows the feature point extraction results of the DVI-SLAM algorithm. It can be seen that the DVI-SLAM system model of this embodiment can effectively estimate the true motion state of objects, accurately remove dynamic points in the scene, and make full use of the information of static points.

[0224] Embodiment 2

[0225] Based on Embodiment 1, in this embodiment, an intelligent navigation system with dynamic environment adaptability is provided, including:

[0226] A visual odometer, configured to perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information, obtain an estimated motion state of the target object, and extract key frames;

[0227] A multi-view probability estimation module, configured to recursively estimate the dynamic probability of each feature point using Bayesian filtering based on multi-frame observation information of the current key frame and historical key frames, and calculate the dynamic probability of the feature points to screen out dynamic points;

[0228] A feature point weight optimization module, configured to, for the screened feature points, perform weight feedback update according to the estimated result of the dynamic probability of the feature points, combine the time decay factor and the reprojection error, dynamically adjust the weights of each feature point to screen the feature points, and obtain static feature points with weights higher than the set value as map points;

[0229] An IMU and camera joint optimization module, configured to jointly optimize the camera pose and the IMU state based on the feature point projection error and the pre-integrated residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points.

[0230] Further, the visual odometer includes an image correction module, an object recognition module, an object tracking module, an ORB feature extraction module, a feature point matching module, and a motion state estimation module;

[0231] The image correction module is configured to rotate the acquired to-be-processed visual image based on the acquired pose prior information to perform image correction so that the vertical direction of the image is aligned with the gravity direction;

[0232] The object recognition module is configured to use the YOLOv8 network to perform object detection on the image and output multiple object candidate boxes and class information;

[0233] The object tracking module uses the DeepSORT object tracking network to perform inter-frame matching and tracking of the object through Re-ID features and Kalman filtering;

[0234] The ORB feature extraction module is configured to extract local feature points from the image using the ORB algorithm;

[0235] The feature point matching module is configured to perform inter-image matching on the ORB feature points between the current frame and the historical key frames;

[0236] The motion state estimation module is configured to associate the ORB feature points output by the feature point matching module with the corresponding target tracking trajectories output by the target tracking module, track the change process in multiple frames of images, combine the IMU data, estimate the motion state of the target in the world coordinate system, and extract the key frames containing the target;

[0237] Among them, the target recognition module and the target tracking module constitute the target detection thread, the ORB feature extraction module, the feature point matching module, and the motion state estimation module constitute the tracking thread; the multi-view probability estimation module, the feature point weight optimization module, and the IMU and camera joint optimization module constitute the local mapping thread;

[0238] Furthermore, the feature point weight optimization module is used to dynamically adjust the weights of each feature point. The method for dynamically adjusting the weights of each feature point includes the following steps:

[0239] For the feature points screened by the multi-view probability estimation module, initialize them to the same weight value;

[0240] Construct a weight update formula according to the dynamic probability of the feature points, and dynamically adjust the weights of the feature points, so that the higher the dynamic probability of the feature points, the lower the weight;

[0241] Introduce a time decay factor into the weight update formula to adjust the amplitude of weight adjustment;

[0242] Introduce the weights of the feature points into the optimization objective function to participate in the optimization calculation, and adjust the feature point weights based on the reprojection error feedback;

[0243] Based on a threshold, eliminate the feature points whose weights are lower than the set weight threshold.

[0244] It should be noted here that each module in this embodiment corresponds to each step in Embodiment 1, and its specific implementation process is the same, so it will not be repeated here.

[0245] The above are only the preferred embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0246] Although the specific implementation manners of the present invention are described above in conjunction with the drawings, it is not a limitation to the protection scope of the present invention. Those skilled in the art should understand that various modifications or deformations that can be made without creative efforts on the basis of the technical solutions of the present invention are still within the protection scope of the present invention.

Claims

1. An intelligent navigation method adaptable to dynamic environments, characterized in that, The steps include: Perform target detection, target tracking and feature point extraction on the visual image to be processed after correction based on the prior information of posture, obtain the motion state estimation of the target object, and extract the key frame; Based on the multi-frame observation information of the current key frame and the historical key frames, the dynamic probability of each feature point is recursively estimated using Bayesian filtering, and the dynamic probability of the feature point is calculated to filter out the dynamic points; For the filtered feature points, according to the dynamic probability estimation results of the feature points, combined with the time decay factor and the reprojection error, the weight feedback update is performed, and the weight of each feature point is dynamically adjusted to filter the feature points, and the static feature points with weights higher than the set value are obtained as map points; Based on the feature point projection error and the IMU pre-integrated residual, the camera pose and IMU state are jointly optimized, and the SLAM map is constructed based on the optimized camera pose and map points; The method for dynamically adjusting the weight of each feature point comprises the following steps: For the filtered feature points, initialize them to the same weight value; Construct a weight update formula based on the dynamic probability of feature points, and dynamically adjust the weight of feature points so that the higher the dynamic probability of feature points, the lower the weight; Introduce the time decay factor into the weight update formula to adjust the magnitude of weight adjustment; The weights of feature points are introduced into the optimization objective function to participate in the optimization calculation, and the weights of feature points are adjusted based on the reprojection error feedback; Based on the threshold, feature points with weights lower than the set weight threshold are eliminated.

2. The intelligent navigation method adaptable to a dynamic environment according to claim 1, wherein The acquired pose prior information includes at least one of the following: IMU pre-integration estimates the pose information, and pre-integrates the motion between the previous frame and the current frame through the data of the IMU accelerometer to obtain the initial pose estimation information; Referring to the key frame pose information, the pose of the most recent historical key frame is used as the estimated initial value of the current frame.

3. The dynamic environment adaptive intelligent navigation method according to claim 1, characterized in that: The method for image correction based on posture prior information includes the following steps: Extract the gravity acceleration vector from the posture prior information; Extract the y-axis vector of the visual image to be processed, and calculate the angle between the y-axis of the image and the gravity acceleration vector based on the acceleration vector and the y-axis vector; Construct a rotation correction matrix based on the calculated included angle to rotate the image so that the axis is parallel to the direction of the gravitational acceleration vector, and obtain the corrected image; When IMU data is not available, the inverse matrix of the reference keyframe pose is used as the correction matrix to rotate the image to the keyframe reference direction to achieve image correction.

4. The method of intelligent navigation in dynamic environment adaptation as claimed in claim 1, characterized in that: The target object motion state estimation method uses Kalman filtering and IMU data fusion to first calculate the pose change between two frames of images using the IMU data pre-integration method, and then updates the state estimation through Kalman filtering.

5. The intelligent navigation method adaptable to a dynamic environment according to claim 1, wherein: Based on the multi-frame observation information of the current key frame and the historical key frames, the dynamic probability of each feature point is recursively estimated by using Bayesian filtering, and the method of calculating the dynamic probability of the feature point to filter out the dynamic points includes the following steps: Extract the common view information between the current frame and the historical key frames, and identify the observation data of the feature points; For each feature point extracted in the current frame, the observation data in the historical key frames are extracted to form an observation sequence; Calculate the dynamic probability of feature points based on the Bayesian formula for the observation sequence and construct a recursive model; For each frame of observation in the observation sequence, repeatedly execute the Bayesian update process based on the constructed recursive model, using the posterior probability of the previous frame as the prior input for the current frame to obtain the dynamic probability sequence of each feature point; Based on the obtained dynamic probability sequence, eliminate dynamic points through the threshold judgment method.

6. The intelligent navigation method adaptable to a dynamic environment according to claim 1, characterized in that: Based on the feature point projection error and IMU data, jointly optimize the camera pose and feature points, including the following steps: Construct a joint state quantity of the camera pose and IMU state; Based on the reprojection error of the visual feature points and the IMU pre-integration residual, construct a joint optimization objective function according to the obtained weight information of the dynamic feature points; Based on the solution of the joint optimization objective function, perform non-linear optimization on the joint state quantity to obtain the camera pose estimation and IMU state estimation of the current frame.

7. An intelligent navigation system adaptable to dynamic environments, characterized in that, Including: Visual odometer, configured to perform object detection, object tracking, and feature point extraction on the to-be-processed visual image corrected based on pose prior information to obtain the motion state estimation of the target object and extract key frames; Multi-view probability estimation module, configured to recursively estimate the dynamic probability of each feature point using Bayesian filtering based on the multi-frame observation information of the current key frame and historical key frames, and calculate the dynamic probability of the feature points to eliminate dynamic points; Feature point weight optimization module, configured to, for the filtered feature points, perform weight feedback update in combination with the time decay factor and reprojection error according to the dynamic probability estimation result of the feature points, dynamically adjust the weights of each feature point to filter the feature points, and obtain static feature points with weights higher than the set value as map points; IMU and camera joint optimization module, configured to jointly optimize the camera pose and IMU state based on the feature point projection error and the pre-integration residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points; The method for dynamically adjusting the weights of each feature point includes the following steps: For the filtered feature points, initialize them to the same weight value; Construct a weight update formula according to the dynamic probability of the feature points, and dynamically adjust the weights of the feature points so that the higher the dynamic probability of the feature points, the lower the weight; Introduce a time decay factor into the weight update formula to adjust the amplitude of weight adjustment; Introduce the weights of the feature points into the optimization objective function to participate in the optimization calculation, and feedback-adjust the weights of the feature points based on the reprojection error; Eliminate feature points with weights lower than the set weight threshold based on the threshold.

8. The intelligent navigation system with dynamic environment adaptability according to claim 7, characterized in that: Visual odometer, including an image correction module, an object recognition module, an object tracking module, an ORB feature extraction module, a feature point matching module, and a motion state estimation module; Image correction module, configured to perform image correction by rotating the obtained to-be-processed visual image based on the obtained pose prior information so that the vertical direction of the image is aligned with the gravity direction; Object recognition module, configured to perform object detection on the image using the YOLOv8 network and output multiple object candidate boxes and category information; The target tracking module adopts the DeepSORT target tracking network to perform inter-frame matching and tracking of targets through Re-ID features and Kalman filtering; The ORB feature extraction module is configured to extract local feature points from images using the ORB algorithm; The feature point matching module is configured to perform inter-image matching of ORB feature points between the current frame and historical key frames; The motion state estimation module is configured to associate the ORB feature points output by the feature point matching module to the corresponding target tracking trajectories output by the target tracking module, track the change process in multiple frames of images, combine IMU data, estimate the motion state of the target in the world coordinate system, and extract key frames containing the target.

Images