A visual SLAM method for wheeled robots considering dynamic features

Abstract

A wheeled robot visual SLAM method considering dynamic features relates to the field of robot feature point tracking technology. A wheel odometer motion model is established, and a pre-integration operation is performed on the wheel speed encoder data between two adjacent visual frames. The covariance matrix is ​​derived based on the pre-integration measurement value, visual features are extracted, sparse stereo matching of binocular feature points is performed, the depth of feature points is calculated, the system is initialized, and the initial value of the pose of the current frame is obtained using the pre-integration value. Motion degradation detection is performed to obtain the initial value of the pose, and then dynamic feature discrimination, joint pose optimization, and map maintenance are performed. Combining the spatial geometric information obtained by the binocular camera with the motion increment measured by the wheel odometer can accurately screen dynamic feature points, and at the same time, the complementarity of multi-source heterogeneous information is used to ensure the continuity and accuracy of positioning, which can effectively improve the positioning accuracy and robustness of wheeled robots under complex working conditions.

Description

Technical Field

[0001] The invention relates to the technical field of robot feature point tracking, in particular to a wheeled robot visual SLAM method taking dynamic features into consideration. Background Art

[0002] In recent years, the development of sensor technology, robotics technology and artificial intelligence technology has promoted the commercialization of wheeled robots. Wheeled robot systems generally rely on sensors such as cameras and radars to locate and perceive the surrounding environment. Based on the positioning information and environmental information, wheeled robots make real-time decisions and control their own movements.

[0003] Wheeled robots often need to face positioning under complex conditions such as dynamic scenes, light changes, less texture, and rapid movement. In the above environment, when using visual sensors to track feature points, it is very easy to cause problems such as feature point failure and feature point mismatch. At this time, the positioning of wheeled robots is extremely unreliable. To solve this problem, the current mainstream solution is to use the camera's geometric constraints or semantic information segmentation to screen dynamic feature points, and then perform subsequent processing. However, the geometric constraint solution mostly relies on the camera's own pose estimation, which will cause the system to crash when tracking is lost. The semantic information segmentation method requires the introduction of a deep learning model, which greatly increases the computing power requirements.

[0004] In view of this, the present invention introduces the wheel odometer commonly used in wheeled robots to provide motion increment information to assist visual information in completing the screening of dynamic feature points. It has low computing power requirements and accurate screening accuracy, thereby improving the positioning accuracy and robustness of the wheeled robot under complex working conditions. Summary of the invention

[0005] In order to address the shortcomings of the background technology, the present invention provides a wheeled robot visual SLAM method taking dynamic characteristics into consideration, which combines the spatial geometric information obtained by a binocular camera with the motion increments measured by a wheel odometer, can accurately screen dynamic feature points, and at the same time utilizes the complementarity of multi-source heterogeneous information to ensure the continuity and accuracy of positioning, which can effectively improve the positioning accuracy and robustness of the wheeled robot under complex working conditions.

[0006] To achieve the above object, the present invention adopts the following technical solution: a wheeled robot visual SLAM method considering dynamic characteristics, comprising the following steps:

[0007] Step 1: Build a wheel odometer motion model

[0008] Performing a pre-integration operation on the wheel speed encoder data between two adjacent visual frames, deriving a pre-integration measurement value, and at the same time, deriving a covariance matrix based on the pre-integration measurement value;

[0009] Step 2: Visual feature extraction

[0010] The image received by the binocular camera is converted into a grayscale image, and ORB feature points are extracted. Then, sparse stereo matching of the binocular feature points is performed. After the matching is completed, the parallax is calculated to obtain the depth of the feature points.

[0011] Step 3: System initialization

[0012] Initialize the binocular vision-wheel odometry system:

[0013] If the current frame is the first frame, the coordinate system of the first frame is taken as the world coordinate system of the system. At the same time, the first frame is selected as the key frame, and the feature points detected by the binocular camera are projected to the world coordinate system to initialize the global map. Then, a batch of queues with the same number of feature points as the key frame are established. Each feature point in the key frame has a unique queue. The queue is subsequently used for dynamic feature identification and map maintenance. If the current frame is not the first frame, it is skipped;

[0014] Use the pre-integrated measurement value of rotation and the pre-integrated value of translation in the wheel odometer motion model to directly obtain the initial value of the pose of the current frame:

[0015]

[0016]

[0017] In the formula, R j is the rotation matrix at time j, R i is the rotation matrix at time i, p j is the translation vector at time j, p i is the translation vector at time i, is the angular velocity measurement value of the wheeled robot at time k, is the pre-integrated measurement value of the rotation from time i to time k, is the linear velocity measurement value of the wheeled robot at time k, Δt1 is the time interval between two measurements of the wheel speed encoder;

[0018] Step 4: Motion degradation detection

[0019] Use the interior point matching method to complete the detection of motion degradation scenarios where the wheel odometer faces insufficient friction and causes slipping. Project the map points of the previous key frame into the current frame and perform the nearest neighbor search within the set Euclidean distance. After the matching is completed, the inconsistent matching points are removed to obtain the number of successfully matched feature points. If the number of successfully matched feature points is less than the set threshold N1, the uniform velocity model is used to obtain the initial value of the pose. Otherwise, the initial value of the pose of the current frame obtained by the wheel odometer motion model is still used.

[0020] Step 5: Dynamic feature identification

[0021] Select the feature point of the most recent key frame as the matching object, and obtain the coordinates P of the feature point in the current frame image that has a matching relationship with it in the camera coordinate system C , and the last feature point in the queue to which the feature point of the key frame belongs, and calculate P C The offset S C :

[0022] S C =P C -P KC

[0023] Where P KC The three-dimensional coordinates of the last feature point in the queue in the camera coordinate system;

[0024] Get the offset S C Then divided by the time interval Δt2 between the two frames, and combined with the average speed information V between the two frames provided by the wheel odometer W , get the offset velocity information ΔV of the feature point j，m :

[0025]

[0026] Add the feature point to the queue of its matching key frame feature points and define the marking parameter γ of the mth feature point at time j j，m :

[0027]

[0028] When ΔV j，m Greater than or equal to the set threshold Th v When , the feature point is considered to be a dynamic feature point, otherwise it is considered to be a static feature point;

[0029] Step 6: Joint Pose Optimization

[0030] The dynamic feature discrimination matching results are used to construct the reprojection error and calculate the wheel odometer error. The process is as follows:

[0031]

[0032] In the formula, is the reprojection error of the mth feature point at the defined time j, u m is the pixel coordinate of the mth feature point on the image, is the depth of the mth feature point, K is the internal reference of the binocular camera, P m is the three-dimensional coordinate of the mth feature point in the map;

[0033]

[0034] In the formula, is the wheel odometer error at the defined time j, is the pre-integrated measurement value of the rotation from time i to time j, is the pre-integrated measurement value of the translation from time i to time j;

[0035] Then the objective function is:

[0036]

[0037] Where M is the number of feature points involved in the optimization of the frame. is the covariance matrix of the mth feature point at time j, is the covariance matrix of the pre-integrated measurements from time i to time j;

[0038] The least squares method is used to optimize the initial pose value to obtain a more accurate pose estimation result. The success of the pose estimation result is determined by the interior point matching method with a threshold of N2. If the pose estimation result is unsuccessful, the initial pose value is directly regarded as the pose of the current frame. If the pose estimation result is successful, the pose result optimized by the least squares method is used as the pose of the current frame.

[0039] Step 7: Map maintenance

[0040] In addition to the first frame being directly identified as a key frame, other frames that meet one of the following two rules are selected as new key frames: Rule 1: more than 80% of the queues have no newly added feature points in the frame; Rule 2: the time from the last selected key frame is more than 2s;

[0041] If the current frame is selected as a new key frame, the map points are maintained first, and the following processing is performed on each queue:

[0042] If the dynamic feature point rate in the queue is less than the set threshold Ra low , take the average of the three-dimensional coordinates of the static feature points in the queue and their matching key frame feature points in the world coordinate system as the new map point to replace the original map point;

[0043] If the dynamic feature point rate in the queue is greater than the set threshold Ra high , the map point information is considered unreliable and deleted;

[0044] After completing the maintenance of the map points, the feature points in the new key frame are projected into the world coordinate system, added to the global map, and a batch of queues with the same number of feature points as the key frame are established. The matching objects of the feature points in subsequent frames will be updated to the key frame, and the queues used for dynamic feature identification and map maintenance will be the newly established queues.

[0045] Furthermore, in step 1, the pre-integrated measurement values ​​include the following:

[0046]

[0047] In the formula, is the pre-integrated measurement of the rotation, is the pre-integrated measurement of translation, and the subscripts indicate the corresponding moments between two visual frames.

[0048] Furthermore, the measured values ​​of the angular velocity and linear velocity of the wheeled robot are obtained by a two-wheel differential model established by a wheel speed encoder, and are respectively:

[0049]

[0050] In the formula, is the angular velocity measurement value of the wheeled robot, is the measured linear velocity of the wheeled robot, is the measured value of the linear velocity of the right wheel of the wheeled robot, is the measured value of the left wheel linear velocity of the wheeled robot, L is the wheelbase, η w and η v are the noise of angular velocity and linear velocity respectively.

[0051] Furthermore, the covariance matrix derived in step 1 is as follows:

[0052]

[0053] In the formula, ∑ W is the pre-integrated measurement covariance matrix, the subscripts represent the corresponding moments between two visual frames, I is the identity matrix, ∑ η is η w and η v The variance values ​​of are combined into a diagonal matrix.

[0054] Furthermore, in the step 2, when performing sparse stereo matching of binocular feature points: first, count the row feature points between the binocular cameras, compare the feature points of the left camera with the feature points of the right camera one by one, select the feature point with the smallest Hamming distance as the preliminary matching point, and then use the absolute sum of the difference to determine the final matching point in the sliding window around the preliminary matching point, and then obtain the optimal sub-pixel coordinates through the sub-pixel interpolation method.

[0055] Furthermore, in step 2, the depth of the feature point is calculated as follows:

[0056]

[0057] In the formula, Z Cis the depth of the feature point, f is the focal length of the binocular camera, b is the baseline length of the binocular camera, and are the optimal sub-pixel coordinates of the feature points in the left and right cameras, respectively.

[0058] Furthermore, the feature points in step 3 are projected to the world coordinate system using the following formula:

[0059]

[0060] In the formula, is the coordinate of the map point in the global map, z is the label of the map point in the global map, T WC is the posture information, is the coordinate of the mth feature point at time j in the camera coordinate system.

[0061] Furthermore, the threshold N2 in step six is ​​greater than the threshold N1 in step four.

[0062] Furthermore, the threshold Ra in step seven low According to the application scenario of wheeled robots, the threshold Ra is [0, 0.3]. high Take [0.4, 0.7] according to the application scenario of the wheeled robot.

[0063] Furthermore, the coordinates of the new map point in step 7 are:

[0064]

[0065] In the formula, are the coordinates of the new map point, is the coordinate of the mth feature point in the queue at time j in the world coordinate system.

[0066] Compared with the prior art, the present invention has the following beneficial effects:

[0067] 1. The present invention makes full use of the complementarity between the visual sensor and the wheel odometer, and uses the wheel odometer to assist in optimizing the initial value selection to ensure the stability and continuity of the positioning and tracking state, thereby improving the robustness of the wheeled robot positioning;

[0068] 2. The method of the present invention using wheeled odometer to assist in dynamic feature point screening and mapping can better resist the influence of dynamic environment, and use a tightly coupled joint state estimation model to ensure the positioning accuracy of the wheeled robot;

[0069] 3. The present invention uses the wheel odometer commonly used on wheeled robots to provide motion increment information, and integrates it with the binocular camera to screen dynamic feature points, thereby improving the accuracy of dynamic feature point screening by the wheeled robot visual SLAM. In complex challenging environments such as high-speed motion and strong light changes, it can demonstrate advantages in positioning accuracy and robustness, without the need for additional sensors, thus saving the cost of additional sensors for wheeled robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a flowchart of the method of the present invention. DETAILED DESCRIPTION

[0071] The technical solution of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0072] like Figure 1 As shown in the figure, a wheeled robot visual SLAM method considering dynamic features is provided. For a wheeled robot with a binocular vision-wheel odometer system, no additional sensors are required. The method of using wheel odometer to assist feature point mapping is used to resist the influence of dynamic objects. The wheel odometer is combined with a binocular camera for joint optimization, thereby improving the robustness and accuracy of wheeled robot positioning. The SLAM method includes the following steps:

[0073] Step 1: Build a wheel odometer motion model

[0074] The measured values ​​of the angular velocity and linear velocity of the wheeled robot are obtained by the two-wheel differential model established by the wheel speed encoder, which are:

[0075]

[0076] In the formula, is the angular velocity measurement value of the wheeled robot, is the measured linear velocity of the wheeled robot, is the measured value of the linear velocity of the right wheel of the wheeled robot, is the measured value of the left wheel linear velocity of the wheeled robot, L is the wheelbase, η w and η v are the noise of angular velocity and linear velocity respectively, both of which obey Gaussian distribution.

[0077] The wheel speed encoder data between two adjacent visual frames is pre-integrated so that the wheel odometer information between frames can be converted into the observation value of the graph optimization problem for joint pose optimization. The pre-integrated measurement values ​​derived by definition include the following:

[0078]

[0079]

[0080] In the formula, is the pre-integrated measurement of the rotation, is the pre-integrated measurement of translation, and the subscripts represent the corresponding moments between two visual frames. is the angular velocity measurement value of the wheeled robot at time k, is the linear velocity measurement value of the wheeled robot at time k, and Δt1 is the time interval between two measurements of the wheel speed encoder.

[0081] At the same time, the covariance matrix is ​​derived based on the pre-integrated measurement value, and then the inverse matrix of the covariance is added as the information matrix to the subsequent joint optimization of the pose. The recursive formula of the covariance matrix is ​​as follows:

[0082]

[0083] In the formula, ∑ W is the pre-integrated measurement covariance matrix, the subscripts represent the corresponding moments between two visual frames, I is the identity matrix, ∑ η is η w and η v The diagonal matrix composed of the variance values ​​of ;

[0084] Step 2: Visual feature extraction

[0085] The image received by the binocular camera is converted into a grayscale image, and ORB feature points are extracted. Then, sparse stereo matching of binocular feature points is performed: first, the row feature points between the binocular cameras are counted, and the feature points of the left camera are compared with the feature points of the right camera one by one. The feature point with the smallest Hamming distance is selected as the preliminary matching point, and then the absolute sum of the difference is used to determine the final matching point in the sliding window around the preliminary matching point. Then, the optimal sub-pixel coordinates are obtained by the sub-pixel interpolation method. After the matching is completed, the disparity is calculated to obtain the depth of the feature point:

[0086]

[0087] In the formula, Z C is the depth of the feature point, f is the focal length of the binocular camera, b is the baseline length of the binocular camera, and are the best sub-pixel coordinates of the feature points in the left and right cameras respectively;

[0088] Step 3: System initialization

[0089] Initialize the binocular vision-wheel odometer system: If the current frame is the first frame, take the coordinate system of the first frame as the world coordinate system of the system. At the same time, select the first frame as the key frame and project the feature points detected by the binocular camera into the world coordinate system to initialize the global map:

[0090]

[0091] In the formula, is the coordinate of the map point in the global map, z is the label of the map point in the global map, T WC is the posture information, is the coordinate of the mth feature point at time j in the camera coordinate system.

[0092] Then, a batch of queues with the same number as the feature points in the key frame are established. Each feature point in the key frame has a unique queue. The queue is subsequently used for dynamic feature identification and map maintenance. If the current frame is not the first frame, it is skipped.

[0093] Use the pre-integrated measurement value of rotation and the pre-integrated value of translation in the wheel odometer motion model to directly obtain the initial value of the pose of the current frame:

[0094]

[0095] In the formula, R j is the rotation matrix at time j, R i is the rotation matrix at time i, p j is the translation vector at time j, p i is the translation vector at time i;

[0096] Step 4: Motion degradation detection

[0097] Due to road conditions and other reasons, the wheel odometer may face a motion degradation scenario where insufficient friction causes slipping. Therefore, the interior point matching method is used to complete the detection of this motion degradation scenario. The map points of the previous key frame are projected into the current frame, and the nearest neighbor search is performed within the set Euclidean distance. After the matching is completed, the inconsistent matching points are eliminated to obtain the number of successfully matched feature points. If the number of successfully matched feature points is less than the set threshold N1, the motion is considered to be degraded. At this time, the uniform velocity model is used to obtain the initial value of the pose. Otherwise, the initial value of the pose of the current frame obtained by the wheel odometer motion model is still used;

[0098] Step 5: Dynamic feature identification

[0099] Select the feature point of the most recent key frame as the matching object, and obtain the coordinates P of the feature point in the current frame image that has a matching relationship with it in the camera coordinate system C , and the last feature point in the queue to which the feature point of the key frame belongs, and calculate P C The offset S C :

[0100] S C =P C -P KC

[0101] Where P KC The 3D coordinates of the last feature point in the queue in the camera coordinate system.

[0102] Get the offset S C Then divided by the time interval Δt2 between the two frames, and combined with the average speed information V between the two frames provided by the wheel odometer W , get the offset velocity information ΔV of the feature point j，m :

[0103]

[0104] Add the feature point to the queue of its matching key frame feature points and define the marking parameter γ of the mth feature point at time j j，m :

[0105]

[0106] When ΔV j，m Greater than or equal to the set threshold Th v When , the feature point is considered to be a dynamic feature point, otherwise it is considered to be a static feature point;

[0107] Step 6: Joint Pose Optimization

[0108] The dynamic feature discrimination matching results are used to construct the reprojection error and calculate the wheel odometer error. The process is as follows:

[0109]

[0110] In the formula, is the reprojection error of the mth feature point at the defined time j, u m is the pixel coordinate of the mth feature point on the image, is the depth of the mth feature point, K is the internal reference of the binocular camera, P m is the three-dimensional coordinate of the mth feature point in the map.

[0111]

[0112] In the formula, is the wheel odometer error at the defined time j.

[0113] Then the objective function is:

[0114]

[0115] Where M is the number of feature points involved in the optimization of the frame. is the covariance matrix of the mth feature point at time j.

[0116] The least squares method is used to optimize the initial pose value to obtain a more accurate pose estimation result. The success of the pose estimation result is determined by the interior point matching method with a threshold of N2. The threshold N2 here should be greater than the threshold N1 to make the requirement more stringent. If the pose estimation result is unsuccessful, the initial pose value is directly regarded as the pose of the current frame. If the pose estimation result is successful, the pose result optimized by the least squares method is used as the pose of the current frame.

[0117] Step 7: Map maintenance

[0118] Except for the first frame which is directly identified as a key frame, other frames that meet one of the following two rules are selected as new key frames: Rule 1: more than 80% of the queues have no newly added feature points in the frame; Rule 2: the time from the last selected key frame is more than 2s.

[0119] If the current frame is selected as a new key frame, the map points are maintained first, and the following processing is performed on each queue:

[0120] If the dynamic feature point rate in the queue is less than the set threshold Ra low , the threshold can be set to [0, 0.3] according to the application scenario of the wheeled robot. The average of the three-dimensional coordinates of the static feature points in the queue and the key frame feature points matched with them in the world coordinate system is taken as the new map point to replace the original map point. The coordinates of the new map point are:

[0121]

[0122] In the formula, are the coordinates of the new map point, is the coordinate of the mth feature point in the queue at time j in the world coordinate system.

[0123] If the dynamic feature point rate in the queue is greater than the set threshold Ra high , the threshold can be set to [0.4, 0.7] according to the application scenario of the wheeled robot. In this case, the map point information is considered unreliable and is deleted.

[0124] If neither of the above two situations occurs, the map point information does not change.

[0125] After completing the maintenance of the map points, project the feature points in the new keyframe into the world coordinate system, add them to the global map, and create a batch of queues with the same number of feature points as the keyframe. Similarly, each feature point in the keyframe has a unique queue. The matching object of the feature points in the subsequent frames will be updated to the keyframe, and the queue used for dynamic feature identification and map maintenance will be the newly created queue.

[0126] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other forms of assembly without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered in all respects as exemplary and non-restrictive, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations within the meaning and range of equivalents of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0127] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A wheeled robot visual SLAM method considering dynamic features, characterized in that: The following steps are involved: Step 1: Build a wheel odometer motion model Performing a pre-integration operation on the wheel speed encoder data between two adjacent visual frames, deriving a pre-integration measurement value, and at the same time, deriving a covariance matrix based on the pre-integration measurement value; Step 2: Visual feature extraction The image received by the binocular camera is converted into a grayscale image, and ORB feature points are extracted. Then, sparse stereo matching of the binocular feature points is performed. After the matching is completed, the parallax is calculated to obtain the depth of the feature points. Step 3: System initialization Initialize the binocular vision-wheel odometry system: If the current frame is the first frame, the coordinate system of the first frame is taken as the world coordinate system of the system. At the same time, the first frame is selected as the key frame, and the feature points detected by the binocular camera are projected to the world coordinate system to initialize the global map. Then, a batch of queues with the same number of feature points as the key frame are established. Each feature point in the key frame has a unique queue. The queue is subsequently used for dynamic feature identification and map maintenance. If the current frame is not the first frame, it is skipped; Use the pre-integrated measurement value of rotation and the pre-integrated value of translation in the wheel odometer motion model to directly obtain the initial value of the pose of the current frame: In the formula, R j is the rotation matrix at time j, R i is the rotation matrix at time i, p j is the translation vector at time j, p i is the translation vector at time i, is the angular velocity measurement value of the wheeled robot at time k, is the pre-integrated measurement value of the rotation from time i to time k, is the linear velocity measurement value of the wheeled robot at time k, Δt1 is the time interval between two measurements of the wheel speed encoder; Step 4: Motion degradation detection Use the interior point matching method to complete the detection of motion degradation scenarios where the wheel odometer faces insufficient friction and causes slipping. Project the map points of the previous key frame into the current frame and perform the nearest neighbor search within the set Euclidean distance. After the matching is completed, the inconsistent matching points are removed to obtain the number of successfully matched feature points. If the number of successfully matched feature points is less than the set threshold N1, the uniform velocity model is used to obtain the initial value of the pose. Otherwise, the initial value of the pose of the current frame obtained by the wheel odometer motion model is still used. Step 5: Dynamic feature identification Select the feature point of the most recent key frame as the matching object, and obtain the coordinates P of the feature point in the current frame image that has a matching relationship with it in the camera coordinate system C , and the last feature point in the queue to which the feature point of the key frame belongs, and calculate P C The offset S C : S C =P C -P KC Where P KC The three-dimensional coordinates of the last feature point in the queue in the camera coordinate system; Get the offset S C Then divided by the time interval Δt2 between the two frames, and combined with the average speed information V between the two frames provided by the wheel odometer W , get the offset velocity information ΔV of the feature point j，m : Add the feature point to the queue of its matching key frame feature points and define the marking parameter γ of the mth feature point at time j j，m : When ΔV j，m Greater than or equal to the set threshold Th v When , the feature point is considered to be a dynamic feature point, otherwise it is considered to be a static feature point; Step 6: Joint Pose Optimization The dynamic feature discrimination matching results are used to construct the reprojection error and calculate the wheel odometer error. The process is as follows: In the formula, is the reprojection error of the mth feature point at the defined time j, u m is the pixel coordinate of the mth feature point on the image, is the depth of the mth feature point, K is the internal reference of the binocular camera, P m is the three-dimensional coordinate of the mth feature point in the map; In the formula, is the wheel odometer error at the defined time j, is the pre-integrated measurement value of the rotation from time i to time j, is the pre-integrated measurement value of the translation from time i to time j; Then the objective function is: Where M is the number of feature points involved in the optimization of the frame. is the covariance matrix of the mth feature point at time j, is the covariance matrix of the pre-integrated measurements from time i to time j; The least squares method is used to optimize the initial pose value to obtain a more accurate pose estimation result. The success of the pose estimation result is determined by the interior point matching method with a threshold of N2. If the pose estimation result is unsuccessful, the initial pose value is directly regarded as the pose of the current frame. If the pose estimation result is successful, the pose result optimized by the least squares method is used as the pose of the current frame. Step 7: Map maintenance In addition to the first frame being directly identified as a key frame, other frames that meet one of the following two rules are selected as new key frames: Rule 1: more than 80% of the queues have no newly added feature points in the frame; Rule 2: the time from the last selected key frame is more than 2s; If the current frame is selected as a new key frame, the map points are maintained first, and the following processing is performed on each queue: If the dynamic feature point rate in the queue is less than the set threshold Ra low , take the average of the three-dimensional coordinates of the static feature points in the queue and their matching key frame feature points in the world coordinate system as the new map point to replace the original map point; If the dynamic feature point rate in the queue is greater than the set threshold Ra high , the map point information is considered unreliable and deleted; After completing the maintenance of the map points, the feature points in the new key frame are projected into the world coordinate system, added to the global map, and a batch of queues with the same number of feature points as the key frame are established. The matching objects of the feature points in subsequent frames will be updated to the key frame, and the queues used for dynamic feature identification and map maintenance will be the newly established queues.

2. A wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: In step 1, the pre-integrated measurement values ​​include the following: In the formula, is the pre-integrated measurement of the rotation, is the pre-integrated measurement of translation, and the subscripts indicate the corresponding moments between two visual frames.

3. A wheeled robot visual SLAM method considering dynamic characteristics according to claim 2, characterized in that: The measured values ​​of the angular velocity and linear velocity of the wheeled robot are obtained by the two-wheel differential model established by the wheel speed encoder, which are: In the formula, is the angular velocity measurement value of the wheeled robot, is the measured linear velocity of the wheeled robot, is the measured value of the linear velocity of the right wheel of the wheeled robot, is the measured value of the left wheel linear velocity of the wheeled robot, L is the wheelbase, η w and η v are the noise of angular velocity and linear velocity respectively.

4. A wheeled robot visual SLAM method considering dynamic characteristics according to claim 3, characterized in that: The covariance matrix derived in step 1 is as follows: In the formula, ∑ W is the pre-integrated measurement covariance matrix, the subscripts represent the corresponding moments between two visual frames, I is the identity matrix, ∑ η is η w and η v The variance values ​​of are combined into a diagonal matrix.

5. The wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: In the step 2, when performing binocular feature point sparse stereo matching: first, count the row feature points between the binocular cameras, compare the feature points of the left camera with the feature points of the right camera one by one, select the feature point with the smallest Hamming distance as the preliminary matching point, and then use the absolute sum of the difference to determine the final matching point in the sliding window around the preliminary matching point, and then obtain the optimal sub-pixel coordinates through the sub-pixel interpolation method.

6. A wheeled robot visual SLAM method considering dynamic characteristics according to claim 5, characterized in that: In step 2, the depth of the feature point is calculated as follows: In the formula, Z C is the depth of the feature point, f is the focal length of the binocular camera, b is the baseline length of the binocular camera, and are the optimal sub-pixel coordinates of the feature points in the left and right cameras, respectively.

7. The wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: In step 3, the feature points are projected to the world coordinate system using the following formula: In the formula, is the coordinate of the map point in the global map, z is the label of the map point in the global map, T WC is the posture information, is the coordinate of the mth feature point at time j in the camera coordinate system.

8. The wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: The threshold value N2 in step six is ​​greater than the threshold value N1 in step four.

9. The wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: The threshold value Ra in step seven low According to the application scenario of wheeled robots, the threshold Ra is [0, 0.3]. high Take [0.4,0.7] according to the application scenario of the wheeled robot.

10. A wheeled robot visual SLAM method considering dynamic characteristics according to claim 1, characterized in that: The coordinates of the new map point in step 7 are: In the formula, are the coordinates of the new map point, is the coordinate of the mth feature point in the queue at time j in the world coordinate system.

Images