Dynamic environment self-adaptive intelligent navigation method and system

Abstract

The invention relates to the technical field of navigation, and provides a dynamic environment self-adaptive intelligent navigation method and system, target detection and tracking are performed after an image is corrected through pose priori, and the dynamic probability of feature points is estimated by fusing Bayesian filtering under multi-key-frame observation; dynamically optimizing the weights of the feature points in combination with a time decay factor and a re-projection error, extracting static map points, and jointly optimizing a pose based on an IMU pre-integration residual error and a visual error to construct a high-precision SLAM map; multi-view dynamic probability estimation and a feature point weight optimization mechanism based on a reprojection error and a time decay factor are innovatively introduced, and the feature screening precision and the pose estimation stability of the SLAM system in a dynamic environment are cooperatively improved; the problems of reduced positioning precision, unstable mapping and insufficient real-time performance of a traditional SLAM system in a dynamic environment are effectively solved.

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, unmanned aerial vehicles, autonomous driving vehicles, augmented reality, and virtual reality, higher requirements are imposed on autonomous navigation and environment perception systems. In this context, the Simultaneous Localization and Mapping (SLAM) technology, as a key technology for enabling an intelligent agent to achieve self-localization and environment mapping in an unknown environment, has received extensive attention. Early SLAM systems mostly relied on lidar for 2D 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 usually assume 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 they have high accuracy, they have high computational resource requirements and are difficult to meet real-time requirements, especially in embedded systems. These methods cannot effectively support the real-time recognition and elimination of dynamic points due to excessive inference latency or large deployment difficulty; In addition, although semantic SLAM can identify object categories, it is difficult to judge their motion states and is prone to mislabeling 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 none of the existing methods accurately identify dynamic points, resulting in common defects such as decreased accuracy, poor robustness, and poor real-time performance in existing SLAM systems 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: One or more embodiments provide an intelligent navigation method adaptable to dynamic environments, including the following steps: 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 target object's motion state, and extract key frames; 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 points to screen out dynamic points; 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; 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.

[0007] One or more embodiments provide an intelligent navigation system adaptable to dynamic environments, including: 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 target object's motion state, and extract key frames; A multi-view probability estimation module configured to, 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 points to screen out dynamic points; A feature point weight optimization module configured to, 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; An IMU and camera joint optimization module configured to, 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.

[0008] Compared with the prior art, the present invention has the following beneficial effects: This implementation effectively solves the problems of traditional SLAM systems in dynamic environments, such as decreased positioning accuracy, unstable mapping, and lack of real-time performance. 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.

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

[0010] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their description are used to explain the present invention but do not constitute a limitation of the present invention.

[0011] Figure 1 is a flow chart of a dynamic environment adaptive intelligent navigation method according to embodiment 1 of the present invention; Figure 2 It is a structural diagram of the DVI-SLAM system model of embodiment 1 of the present invention; Figure 3 is a comparison diagram of the directions of images captured by the camera in a stable and moving state according to Embodiment 1 of the present invention; Figure 4 is a schematic diagram of image correction in Example 1 of the present invention; Figure 5 is a schematic diagram of using an IMU to estimate the motion state of a dynamic object in Embodiment 1 of the present invention; Figure 6 is a flow chart of feature point weight optimization in Example 1 of the present invention; FIG. 7 ( a ) is a feature point extraction result in a dynamic scene using the ORB-SLAM3 algorithm in a simulation experiment of Example 1 of the present invention; FIG. 7( b ) is a result of extracting feature points in a dynamic scene using the DVI-SLAM system model of this embodiment in a simulation experiment of Embodiment 1 of the present invention. DETAILED DESCRIPTION

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

[0013] It should be noted that the following detailed description is exemplary and is intended to provide further explanation 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.

[0014] It should be noted that the terms used herein are only for describing specific embodiments 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 in the present invention and the features in the embodiments can be combined with each other. The embodiments will be described in detail below with reference to the drawings.

[0015] Technical term explanations: IMU Pre-integration Residuals: The pre-integration residuals refer 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 Visual Inertial Navigation System (VINS) to optimize the accuracy of the system.

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

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

[0018] IMU Accelerometer: The 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 velocity changes of the object for the navigation system.

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

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

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

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

[0023] 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 the 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 the 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 of visual odometer and IMU data is adopted, and the phenomenon of IMU error accumulation 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 will be described with specific embodiments.

[0024] Embodiment 1 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: Step 1: 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; 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, calculate the dynamic probability of the feature points, and filter out the dynamic points; Step 3: For the filtered feature points, according to the estimated results of the dynamic probability 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; Step 4: 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.

[0025] In this embodiment, the input image is real-time geometrically corrected by using the motion prior information provided by the IMU to correct the image distortion caused by camera rotation or movement, so as to provide stable input for subsequent object detection and tracking. The object detection and tracking module uses a lightweight neural network model to identify dynamic or static objects in the corrected image, and combines the change of object position between multiple frames to estimate the motion state, and further determine the key frames. For the feature points extracted from the key frame images, use the Bayesian filtering algorithm, combine the observation information in the historical key frames, dynamically estimate the dynamic probability of each feature point, and filter out the points with high dynamic probability. Subsequently, combine the time decay factor (reflecting the decreasing importance of feature points over time) and the reprojection error (reflecting the deviation between the feature point and the predicted position) to feedback and update the feature point weights, and select static feature points as map points according to the set threshold. Finally, adopt a tightly coupled optimization strategy, incorporate the pre-integrated residual of the IMU and the feature point projection error into the optimization objective, jointly optimize the six-degree-of-freedom pose of the camera and the IMU state parameters, and improve the mapping accuracy and stability of the entire SLAM system.

[0026] This implementation effectively solves the problems of decreased positioning accuracy, unstable mapping, and insufficient real-time performance existing in traditional SLAM systems in dynamic environments. 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 severely moving scenes can be enhanced; on the other hand, introducing the Bayesian dynamic probability estimation method under multi-frame observations can effectively identify and eliminate dynamic feature points, and improve 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 tightly coupled 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 odometer to still operate stably in complex dynamic environments.

[0027] 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; 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.

[0028] In step 11, image correction is performed, and the acquired posture prior information includes at least one of the following: (1) IMU pre-integration estimates of pose information. Specifically, the motion between the previous frame and the current frame is pre-integrated using the IMU accelerometer data to obtain the initial pose estimate information. (2) With reference to the key frame pose information, the pose of the most recent historical key frame optimized by the SLAM system backend can be used as the estimated initial value of the current frame; The image correction of this embodiment effectively solves the problem of decreased target detection accuracy when the camera moves with a large amplitude by combining IMU data and back-end key frame poses, and improves the robustness and accuracy of the system in a dynamic environment. In order to enhance fault tolerance, the pose prior information set in this embodiment includes the pose of the key frame output by the system at the last moment, which can still run stably even when the IMU fails. In this case, the reference key frame pose correction provided by the back-end optimization is relied on, and the target tracking is limited to target tracking in two-dimensional space. Although the overall performance may decline, the DVI-SLAM system model can still maintain normal operation to ensure the stability and reliability of the system.

[0029] When the camera moves a lot, the accuracy of target detection will drop significantly, which is one of the key challenges faced by semantic-assisted dynamic SLAM systems. Specifically, when the camera moves a lot, two situations may occur: First, although the YOLOv8 network used for target detection can recognize dynamic objects, due to the tilt of the image, the bounding box of the detected target may contain a large number of static background areas, resulting in the loss of static feature points, which may cause the SLAM system to fail to track; second, if the image tilt angle is too large, such as Figure 3 As shown, the camera movement causes the y-axis of the image to have an angle with the direction of gravity. , the semantic network may not be able to accurately identify dynamic objects, thus incorrectly incorporating dynamic feature points into the back-end optimization process, significantly reducing the positioning accuracy of the system. In response to the above problem, in this step 1, the acquired image is corrected using pose information.

[0030] In step 11, the method for image correction based on pose prior information includes the following steps: Step 111: Extract the gravitational acceleration vector in the pose prior information ; Specifically, the gravitational acceleration can be obtained by pre-integrating the IMU accelerometer and gyroscope data ; 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 the angle ; When the camera moves significantly, there will be an angle between the gravitational acceleration vector and the image axis, and its calculation formula is as follows: (1); 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 ; The pixel points of the corrected image are expressed as: (2); Among them, 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; Step 114: When the IMU data is not available, 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; Although there may be a slight deviation in the reference direction of the reference key frame, in this embodiment, it is not necessary to perform precise correction on the image. Assume that the pose of the reference key frame is , then: ; Among them, represents the inverse matrix of the key frame pose, which is used to "inverse rotate" the current image to the reference direction of the key frame to achieve correction; The image correction process of this embodiment, the correction schematic diagram is as Figure 4As shown, it can ensure that the target detection box does not tilt, reduce the probability of accidentally 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 the decline in 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.

[0031] 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. Optionally, YOLOv8 target detection network can be used for target detection, and the detected output includes the target category 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. DeepSORT (Deep Simple Online and Realtime Tracking): is a multi-object tracking algorithm for maintaining the identity consistency of targets in image sequences, which can number and maintain the trajectories of detected targets in consecutive frames. In step 121, target detection and target tracking are performed, 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 the steps are as follows: In step 121-1, the YOLOv8 network is used to perform target detection on the image, and multiple target candidate boxes and category information are output. In step 121-2, the dynamic category targets in the detection results are screened. Among them, the dynamic categories include but are not limited to "person", "car", etc. In step 121-3, the screened targets are input into the DeepSORT target tracking network, and the targets are matched and tracked between frames through Re-ID features and Kalman filtering. The combination of Re-ID features and Kalman filtering can more effectively estimate the state of the target (such as position, speed, etc.) and improve the tracking accuracy of the target in complex environments. Among them, 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).

[0032] Step 121-4: Use the Hungarian algorithm to implement the matching of the tracking target and the historical trajectory, and obtain the tracking target and its motion trajectory between consecutive frames. Step 122: Extract feature points and fuse the extracted feature points with the target tracking result. 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: Step 122-1: Use the ORB algorithm to extract local feature points from the current frame and historical key frames, and perform correlation fusion. Step 122-2: Perform inter-image matching on the ORB feature points between the current frame and historical key frames. Among them, the historical key frame refers to the frame image 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. 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 target tracking trajectory to achieve the association fusion between the feature points and the tracking target. Step 123: For the fused feature points, 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 the current key frame. This process is implemented in the motion state estimation module of the DVI-SLAM system model. The motion state estimation module realizes real-time prediction and update of 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. Semantic information can only provide the category of the target object and cannot judge 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 the 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 5As 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 dynamic objects (blue dots) in the world coordinate system based on the observed ones. When visual odometry fails to 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 estimated 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.

[0033] Among them, visual odometry refers to a module that estimates the motion trajectory (pose change) of a camera in three-dimensional space by analyzing visual features extracted from consecutive image frames. In this embodiment, it includes a motion state estimation module and modules in the target detection thread and tracking thread; Further, for the fused feature points, 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; the method for estimating the motion state of the target object, through Kalman filtering and IMU data fusion, real-time predicts and updates the motion state of the dynamic object. First, use the IMU data pre-integration method to calculate the pose change amount between two frames of images, and then update the state estimation through Kalman filtering, specifically as follows: 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: (3); Among them, is the initial position of the object; is the initial velocity of the object; is the initial acceleration of the object.

[0034] 2) Predict the current state based on the previous state and IMU data, and the formula is: (4); 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 of the previous moment. The specific form of the state transition matrix depends on the motion model of the target object.

[0035] The corresponding prior covariance matrix to be updated: (5); Wherein, is the prior state covariance matrix; is the posterior state covariance matrix at the previous moment; is the process noise covariance matrix, representing the uncertainty in the state transition process.

[0036] 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 estimation; When new observation data is input, update the state estimation 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 balance the confidence levels of the predicted value and the observed value: (6); Wherein, is the observation matrix, which maps the state space to the observation space; is the observation noise covariance matrix, representing the uncertainty of the observation data.

[0037] Use the Kalman gain and the observed value , and update the state estimation: (7); Wherein, is the posterior state estimation value; is the actual observed value; is the predicted observed value.

[0038] Update the uncertainty of the state: (8); Wherein, is the identity matrix.

[0039] The core of Kalman filter update lies in how to obtain effective observation quantities in a dynamic scene. When the system (DVI-SLAM) obtains sufficient static feature points in a dynamic environment, the visual odometer can calculate the position of these feature points in the world coordinate system in the th frame of the image through camera observation and self-localization : (9); Wherein, represents the position of the feature points in the th frame of the image in the camera coordinate system; represents the The transformation matrix of the feature points of the frame image from the world coordinate system to the camera coordinate system; 4) Use IMU pre-integration to compensate for visual tracking failure: When vision fails in a dynamic scene, use IMU data to recover the current camera pose, obtain the motion state of the target, and use the motion state to distinguish between static and dynamic targets; When there are a large number of moving objects in the scene, semantic SLAM systems based on object 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 achieving the solution of the pose. Solution.

[0040] 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 at which IMU data can be collected. Then, the change amounts of the rotation matrix, velocity, and position can be expressed as follows: (10); (11); (12); Among them, 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 gravitational acceleration; represents the acceleration of the IMU at the moment; represents the IMU from the Position change from time to time respectively represent the positions of the IMU at time and time ; and respectively represent the zero - bias estimation values; and respectively represent the noise terms; represents the change in the rotation matrix of the IMU from time to time ; represents the change in the velocity of the IMU from time to time ; represents the change in time of the IMU from time to time .

[0041] (13); (14); (15); Among them, represents the pre - integrated displacement of the IMU from time to time . represents the position in the world coordinate system at time . represents the transformation matrix from the world coordinate system to the camera coordinate system at time . represents the transformation matrix of the camera coordinate system at time . represents the position in the camera coordinate system at time . represents the position in the inertial coordinate system at time . represents the transformation matrix from the camera coordinate system to the inertial coordinate system; represents the position in the camera coordinate system at time .

[0042] 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 moving objects. 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. The feature points determined as real static points are screened out through semantic information and incorporated into the mapping step of the subsequent steps.

[0043] The above motion state estimation method of this embodiment predicts and updates the motion state of dynamic objects in real time through Kalman filter and IMU data fusion. 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 dynamic objects.

[0044] Key frames are usually automatically selected and generated based on indicators such as the inter-frame matching quality of the SLAM system and the number of co-visible feature points between frames; Step 2 can be implemented in the multi-view probability estimation module to 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.

[0045] Based on the multi-frame observation information of the current key frame and historical key frames, a method for recursively estimating the dynamic probability of each feature point using Bayesian filtering and calculating the dynamic probability to screen out dynamic points includes the following steps: Step 21: Extract the co-visible information between the current frame and historical key frames and identify the observation data of the feature points; Specifically, first calculate the co-visibility rate between the current frame and historical key frames, that is, the number of co-observed feature points 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 in the reference frame set, including the image position, viewing angle, and timestamp; 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 ; (16); Among them, represents the pixel position of the feature point in the key frame image at time t; Step 23: Calculate the dynamic probability of the feature point based on the Bayesian formula for the observation sequence and construct a recursive model: (17); 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 that under a given state the probability of observing ; Based on the Markov assumption, a recursive model is constructed as a dynamic probability propagation model, and its state transition process can be expressed as: (18); Step 24: For each frame of observation in the observation sequence, repeat the Bayesian update process based on the constructed recursive model, using the posterior probability of the previous frame as the prior input of the current frame to obtain the dynamic probability sequence of each feature point; Step 25: Based on the obtained dynamic probability sequence, eliminate the dynamic points through the threshold judgment method; 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 be adapted to multi-type visual inputs such as binocular, monocular, or RGB-D, etc., with strong generality and practicality. The dynamic points are eliminated through dynamic probability, realizing the primary screening of feature points, and transmitting the points that may be static downward for subsequent mapping; 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. Using the N key frames with the highest co-visibility rate with the current key frame, calculate the dynamic probability of feature points, and recursively update the state estimation through Bayesian filtering, significantly improving the classification accuracy of dynamic and static feature points, and solving the problems of heavy computational burden in the scheme based on image segmentation and insufficient accuracy in the scheme based on object detection.

[0046] In step 3, to realize the optimization of feature point weights, it can be achieved 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; In step 3, the method for dynamically adjusting the weights of each feature point, such as Figure 6 shown, includes the following steps: Step 31: Initialize the feature weights: For the screened feature points, initialize them to the same weight value; Among them, the screened feature points are the remaining feature points after the dynamic feature points are screened out in step 2; In step 31, first, initialize a weight value for each screened feature point. Suppose there are N feature points, and the weight of the feature point It can be initialized to 1, indicating that each feature point has equal importance at the initial moment, that is: (19); Step 32: Obtain the dynamic probability, construct a weight update formula based on 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; Based on the dynamic probability of the feature points output by the dynamic probability estimation module , dynamically adjust the weights of the feature points. The higher the dynamic probability, the lower the weight; the lower the dynamic probability, the higher the weight. The specific weight update formula is: (20); Among them, represents the weight of the feature point at time , is the dynamic probability calculated in step 4.

[0047] Step 33: Time decay weight update: Introduce a time decay factor in the weight update formula to adjust the amplitude of weight adjustment; To further optimize the weight adjustment process, introduce a time decay factor to reflect the change of the dynamic probability of the 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: (21); Among them, is the time interval between the current frame and the previous frame, is the decay coefficient.

[0048] 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: (22); Among them, is the normalized weight, is the total number of feature points.

[0049] Step 34: Introduce the weights of the feature points into the optimization objective function to participate in the optimization calculation, and adjust the weights of the feature points based on the reprojection error feedback; In the backend optimization of step 4 of the SLAM system, introduce the weights of the feature points 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: (23); Among them, Indicates the status of the key frame; Is the camera pose of the current key frame; Are the 3D coordinates of the feature points; Is the projection function that projects 3D points onto the image plane; Is the observed position of the feature points in the image.

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

[0051] Specifically, after each backend optimization iteration of the SLAM system, the weights of the feature points are adjusted based on 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: (24); Where, Is the reprojection error of the feature point; Is the feedback adjustment coefficient used to control the amplitude of weight adjustment.

[0052] Step 35, Dynamic feature rejection: Reject feature points with weights lower than the set weight threshold based on the threshold; Specifically, for feature points with weights lower than a certain threshold, they are directly removed from the optimization process to reduce their negative impact on the system accuracy. The rejection condition is: ; Where, Is the weight threshold, usually set to 0.1.

[0053] The weight optimization in Step 3 dynamically adjusts the weights of each feature point according to the dynamic probability estimation results of multiple views, and combines the time decay factor and the reprojection error for weight feedback update to enhance the contribution of static feature points in the optimization process and reduce the interference of dynamic points. The low-weight feature points are screened out through weights to obtain a set of high-weight static feature points participating in the backend optimization, that is, the set of removed low-weight (high dynamic probability) feature points, and the optimized weights are output. 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.

[0054] Step 4 uses a tightly coupled optimization method to fuse the target object motion state estimation results and IMU data to reduce the influence of dynamic objects on the system and simultaneously optimize the accuracy of the system pose estimation; Step 4, Joint optimization of the camera pose and feature points based on the feature point projection error and IMU data, includes the following steps: Step 41: Construct the combined state quantity of the camera pose and the IMU state: 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; Step 42: Based on the reprojection error of the visual feature points and the IMU pre-integration residual, construct a combined optimization objective function according to the weight information of the dynamic feature points obtained in Step 3; 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: (25); 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; The inertial residual of the optimization function is , represents the information matrix in the optimization process, specifically as follows: (26); (27); (28); (29); Among them, , , respectively represent the residuals of the 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 the 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 speeds of the IMU at the i-th moment and the th moment; is the gravitational acceleration; represents transpose. 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 moment;

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

[0056] In this step, by using the IMU data, the interference of dynamic objects to the system accuracy is effectively reduced. At the same time, the errors of the IMU itself are also effectively compensated through the joint optimization with the visual odometer. 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 tight-coupling optimization method, the visual odometer and IMU data are fused to reduce the impact of dynamic objects on the system. The IMU data and the visual inertial odometer data are tightly coupled and optimized, and the optimization objective function includes the visual residual and the IMU residual, which significantly improves the accuracy of the pose estimation.

[0057] A further technical solution also includes a loop detection step, including candidate detection and loop fusion; Step 51: Candidate detection: Step 511: In the candidate detection stage, the DVI - SLAM system model determines potential candidate frames through feature matching by extracting the significant features of the current key frame and historical key frames and calculating the feature descriptors.

[0058] Specifically, for feature matching, high - similarity frames in the historical key frames are selected as loop candidate frames by calculating the matching degree; Step 512: Exclude incorrect matches through geometric verification (RANSAC algorithm) to obtain the final loop candidate frames.

[0059] Step 52: Loop fusion: Step 521: Based on the obtained loop candidate frames, establish the relative pose relationship between key frames through loop constraints; Step 522: Use the graph optimization method to optimize the poses of all key frames to reduce the cumulative error; Specifically, the optimization objective is to minimize the error between poses to ensure the accuracy of the loop detection result. Finally, the optimized pose information is output as the basis for subsequent map update and path planning.

[0060] The above-mentioned intelligent navigation method adaptable to dynamic environments can be implemented by constructing a DVI-SLAM system model, such 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; 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 an estimation of the motion state of the target object, and extract key frames; 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; The feature point weight optimization module is configured to, for the screened feature points, perform weight feedback update by combining the dynamic probability estimation result of the feature points, a time decay factor, and the reprojection error, dynamically adjust the weights of the feature points to screen the feature points, and obtain static feature points with weights higher than the set value as map points; 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-integration residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points.

[0061] 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; The image correction module is configured to perform image correction on the acquired to-be-processed visual image by rotation based on the acquired pose prior information, so that the vertical direction of the image is aligned with the gravity direction; The object recognition module is configured to perform object detection on the image using the YOLOv8 network and output multiple object candidate boxes and class information; 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; The ORB feature extraction module is configured to extract local feature points from the image using the ORB algorithm; The feature point matching module is configured to perform inter-image matching of the ORB feature points between the current frame and the historical key frames; 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 IMU data, estimate the motion state of the target in the world coordinate system, and extract key frames; Among them, the target recognition module and the target tracking module constitute the target detection thread, and 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; Furthermore, it also includes a loop detection thread, including a candidate detection module and a loop fusion module; Step 51, the candidate detection module is configured to perform the following process: Step 511, in the candidate detection stage, by extracting the significant features of the current key frame and the historical key frames and calculating the feature descriptors, perform feature matching to determine potential candidate frames.

[0062] Specifically, for feature matching, the high similarity frames in the historical key frames are selected as loop candidate frames by calculating the matching degree; Step 512, exclude the incorrect matches through geometric verification (RANSAC algorithm) to obtain the final loop candidate frames.

[0063] Step 52, loop fusion, is configured to perform the following process: Step 521, based on the obtained loop candidate frames, establish the relative pose relationship between the key frames through loop constraints; Step 522, use the graph optimization method to optimize the poses of all key frames to reduce the cumulative error; Specifically, the optimization goal is to minimize the error between the poses to ensure the accuracy of the loop detection result. Finally, output the optimized pose information as the basis for subsequent map update and path planning.

[0064] The DVI-SLAM system model constructed in this embodiment significantly improves the robustness and accuracy of the system in a dynamic environment 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 when the camera moves at a large angle; 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 jointly enhance the performance and stability of the system in a complex dynamic environment.

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

[0066] 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 mapping. 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.

[0067] Embodiment 2 Based on Embodiment 1, an intelligent navigation system adaptable to a dynamic environment is provided in this embodiment, including: 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; 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 points to screen out dynamic points; 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; 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-integration residual of the IMU, and construct a SLAM map based on the optimized camera pose and map points.

[0068] 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; The image correction module is configured to perform image correction by rotating the acquired to-be-processed visual image based on the acquired pose prior information, so that the vertical direction of the image is aligned with the gravity direction; The object recognition module is configured to perform object detection on the image using the YOLOv8 network and output multiple object candidate boxes and class information; The object tracking module adopts the DeepSORT object tracking network to perform inter-frame matching and tracking of the object through Re-ID features and Kalman filtering; The ORB feature extraction module is configured to extract local feature points from the image using the ORB algorithm; The feature point matching module is configured to perform inter-image matching of the ORB feature points between the current frame and the 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 object tracking trajectories output by the object tracking module, track the change process in multiple frames of images, jointly use IMU data, estimate the motion state of the object in the world coordinate system, and extract key frames containing the object; Among them, the object recognition module and the object tracking module constitute the object detection thread, and 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; Further, a 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: Initialize the feature points filtered by the multi-view probability estimation module 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 adjust the weights of the feature points based on the reprojection error feedback; Based on a threshold, eliminate the feature points whose weights are lower than the set weight threshold.

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

[0070] 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 modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

[0071] 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, based on the technical solutions of the present invention, various modifications or deformations that can be made without creative efforts by those skilled in the art are still within the protection scope of the present invention.

Claims

1. An intelligent navigation method for dynamic environment adaptation, 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 pre-integrated residual of the IMU, the camera pose and IMU state are jointly optimized, and the SLAM map is constructed based on the optimized camera pose and map points.

2. The method for dynamic environment adaptive intelligent navigation according to claim 1, characterized in that: 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; According to the calculated angle, a rotation correction matrix is ​​constructed to rotate the image so that the image The axis is parallel to the direction of the gravity acceleration vector, and the corrected image is obtained; 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 method of intelligent navigation in dynamic environment adaptation as claimed in claim 1, characterized in that: 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 build a recursive model; For each frame observation in the observation sequence, the Bayesian update process is repeatedly performed based on the constructed recursive model, and the posterior probability of the previous frame is used as the prior input of the current frame to obtain the dynamic probability sequence of each feature point; Based on the obtained dynamic probability sequence, dynamic points are eliminated through the threshold judgment method.

6. The method of intelligent navigation in dynamic environment adaptation as claimed in claim 1, characterized in that: The method for dynamically adjusting the weight 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 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.

7. The method of intelligent navigation in dynamic environment adaptation as claimed in claim 1, characterized in that: Based on the feature point projection error and IMU data, the camera pose and feature points are jointly optimized, including the following steps: Construct the joint state quantity of camera pose and IMU state; Based on the reprojection error of visual feature points and the IMU pre-integration residual, a joint optimization objective function is constructed according to the weight information of the acquired dynamic feature points; Based on the solution of the joint optimization objective function, the joint state quantity is nonlinearly optimized to obtain the camera pose estimation and IMU state estimation of the current frame.

8. An intelligent navigation system capable of dynamic environment adaptation, characterized in that: include: The visual odometry is configured to perform target detection, target tracking, and feature point extraction on the visual image to be processed after correction based on the prior information of the position and posture, obtain the motion state estimation of the target object, and extract the key frame; 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 the historical key frames using Bayesian filtering, calculate the dynamic probability of the feature points and filter out the dynamic points; The feature point weight optimization module is configured to perform weight feedback update for the filtered feature points based on the dynamic probability estimation results of the feature points, combined with the time decay factor and the reprojection error, and dynamically adjust the weight of each feature point to filter the feature points, and obtain static feature points with weights higher than the set value as map points; The IMU and camera joint optimization module is configured to jointly optimize the camera pose and IMU state based on the feature point projection error and the IMU pre-integrated residual, and build a SLAM map based on the optimized camera pose and map points.

9. The intelligent navigation system with dynamic environment adaptation as claimed in claim 8, characterized in that: Visual odometer, including image correction module, target recognition module, target tracking module, ORB feature extraction module, feature point matching module and motion state estimation module; An image correction module is configured to rotate the acquired visual image to be processed to achieve image correction based on the acquired posture prior information, so that the vertical direction of the image is aligned with the gravity direction; The target recognition module is configured to use the YOLOv8 network to detect targets in images and output multiple target candidate boxes and category information; The target tracking module uses the DeepSORT target tracking network to match and track the target between frames through Re-ID features and Kalman filtering; An ORB feature extraction module is configured to extract local feature points from an image using an ORB algorithm; A feature point matching module, configured to perform inter-image matching of ORB feature points between a current frame and a historical key frame; 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 trajectory 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 the key frames containing the target.

10. The dynamic environment adaptive intelligent navigation system according to claim 8, characterized in that: The feature point weight optimization module is used to dynamically adjust the weight of each feature point. The method for dynamically adjusting the weight 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 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.

Images