Joint positioning method based on vision

Abstract

The invention discloses a combined positioning method based on vision, and the method comprises the steps: A, a robot obtains image data in real time through a camera, obtains inertial data in real time through an inertial sensor, and carries out the preprocessing of the image data and the inertial data; step B, the robot obtains successfully tracked image frames from the preprocessed image data, and then extracts key frames from the successfully tracked image frames; step C, the robot carries out loopback detection on the key frame; step D, the robot screens out a target key frame from the key frames according to the loopback detection condition; and step E, performing global pose optimization on the target key frame by the robot in combination with the preprocessed inertial data to obtain an optimized world coordinate of the robot so as to compensate a pose difference formed by a difference value between a pose marking rate of the robot in the global map and a rate of acquiring the image data or the inertial data. And the influence on pose estimation in a few-texture region and a light intensity change environment is overcome.

Description

Technical Field

[0001] The present application relates to the technical field of robot visual positioning, and in particular to a vision-based joint positioning method. Background Art

[0002] The indoor environment navigation SLAM system can integrate the sensor data collected by the camera and the inertial measurement unit (IMU) to implement the SLAM algorithm. It is widely used, such as sweeping robots, companion robots, food delivery robots, medicine delivery robots, etc., among which sweeping robots are the most widely used. SLAM (simultaneous localization and mapping) refers to the robot moving from an unknown position in an unknown environment, positioning itself according to the position estimation and map during the movement, and building an incremental map based on its own positioning to achieve autonomous positioning and navigation of the robot.

[0003] Traditional visual SLAM methods mainly rely on feature points in the environment. When used in indoor navigation applications, they often encounter areas with little texture (which means that the image feature points are relatively sparse) and frequent changes in light intensity, which cause blurred images. As a result, the visual positioning ability is reduced or even fails due to the inability to effectively track features. Moreover, the main control computing power of the robot used for indoor navigation is limited. As the application scenarios become more and more complex, it is difficult to meet the system robustness requirements. Summary of the invention

[0004] This application discloses a vision-based joint positioning method, and the specific technical solution is as follows: A vision-based joint positioning method comprises: step A, a robot acquires image data in real time through a camera, acquires inertial data in real time through an inertial sensor, and then pre-processes the image data and the inertial data respectively; step B, the robot acquires image frames of successful tracking from the pre-processed image data, and then extracts key frames from the image frames of successful tracking; step C, the robot performs loop detection on the key frames; step D, the robot selects target key frames from the key frames according to the loop detection situation; step E, the robot performs global posture optimization on the target key frames in combination with the pre-processed inertial data, and obtains the optimized world coordinates of the robot, so as to compensate for the posture difference formed by the difference between the rate of marking the posture of the robot in the global map and the rate of acquiring the image data or the inertial data.

[0005] To sum up, the technical solution obtains successfully tracked image frames from the preprocessed image data, extracts key frames from the successfully tracked image frames, and then filters out target key frames from the key frames according to the loop detection situation, thereby realizing multiple screening of multiple image frames. The present embodiment selects necessary key frames (target key frames) for global pose optimization, and compared with the pure visual tracking results, reduces the number of key frames required for visual navigation and positioning, and reduces computing power requirements; it also improves the performance of data fusion between inertial data and image frames, thereby having the characteristics of faster feature search speed, higher accuracy, and stronger robustness.

[0006] In addition, the technical solution combines the preprocessed inertial data with the image feature information in the target key frame, performs global pose optimization on the target key frame, and solves the optimized pose of the robot to compensate for the pose difference formed by the difference between the rate at which the robot marks the pose in the global map and the rate at which the image data or the inertial data is acquired, so that the inertial data can be optimized, the stability of inter-frame feature tracking can be improved, and the influence of the sliding window scanning frame rate and the coordinate update frequency in the global map on the optimized world coordinate output frequency of the robot can be reduced, thereby overcoming the influence of the inertial data or the image data acquisition state, especially overcoming the influence of image blur scenes caused by low-texture areas (meaning that the image feature points are relatively sparse) and frequent changes in light intensity on the inertial data or the image data acquisition state.

[0007] Further, after the image data and the inertial data are preprocessed in step A, at least one image frame of successful tracking is sequentially scanned from the preprocessed image data by setting a sliding window in step B, so that the image frame of successful tracking is filled into the sliding window in units of frames; after the image data and the inertial data are preprocessed in step A, when the sliding window does not scan the image frame of successful tracking from the preprocessed image data, the image data acquired in real time is preprocessed again, and a new sliding window is set, so that the new sliding window scans the image frame of successful tracking from the re-preprocessed image data. In this way, the search range of subsequent point features and line features is narrowed, and the filtering of feature points and feature lines is completed.

[0008] Furthermore, step D also includes, when the sliding window fails to scan the image frame that is successfully tracked from the current preprocessed image data, re-preprocessing the real-time acquired image data and setting a new sliding window accordingly, marking the first key frame scanned by the new sliding window from the re-preprocessed image data as the target key frame, and filling the first key frame into the sliding window. Thus, necessary key frames are selectively added to the global map for posture optimization, reducing the computing power consumed by maintaining image frames.

[0009] Furthermore, in step A, the method for preprocessing the image data includes: the robot extracts matching feature point pairs and matching feature line pairs from the current image frame and the previous image frame, and then calculates the sum of the number of matching feature point pairs and the number of matching feature line pairs, and then marks the sum as the number of feature matches; wherein the previous image frame includes the previous key frame; when the number of feature matches is greater than the preset tracking number threshold, it is determined that the current image frame is the image frame with successful tracking; when the number of feature matches is less than or equal to the preset tracking number threshold, it is determined that the current image frame is not the image frame with successful tracking; wherein the image data includes point features and line features. Compared with the current traditional pure visual feature tracking technology, it has good robustness, stronger anti-interference ability, higher accuracy, and at the same time saves and narrows the feature search range, shortens the feature search time, and is suitable for scenes where pure image feature tracking cannot be performed, such as image degradation and low-texture areas. Improve the accuracy of pose estimation in low-light environments and structured scenes, and ultimately improve the accuracy of pose estimation of global maps.

[0010] Further, in step B, the method for extracting key frames from successfully tracked image frames includes: among at least two successfully tracked image frames acquired in sequence, performing parallax comparison between the current image frame and the previous image frame to obtain the parallax of two adjacent image frames; wherein the previous image frame includes the previous key frame; extracting matching feature point pairs from at least two successfully tracked image frames acquired in sequence, and counting the number of matching feature point pairs acquired; when the parallax of two adjacent image frames is greater than a preset parallax threshold, and the number of matching feature point pairs is greater than a preset feature number threshold, and the uniformity of feature points in the current image frame is less than a preset uniform number threshold, determining that the current image frame is the key frame, and filling the current image frame into the sliding window; the uniformity of feature points in the current image frame represents the number of feature points distributed within a unit area in the current image frame. Therefore, step B selects key frames based on disparity and the number of feature points, forming a screening operation on the image frames after the robot completes image tracking, narrowing the search range of point and line feature loop detection, and improving the accuracy and efficiency of robot pose estimation.

[0011] Furthermore, the step C includes: controlling the currently acquired key frame to perform feature matching with all key frames in the key frame image library in turn, and filtering out matching frames from the key frame image library, and then counting the number of matching frames; judging whether the number of matching frames is greater than a preset matching number threshold, if so, determining that the loop detection of the currently acquired key frame is successful, otherwise determining that the loop detection of the currently acquired key frame is unsuccessful and marking the currently acquired key frame as the target key frame. Thus, the robot detects the matching relationship between the current key frame and the preset key frame through loop detection, so that the estimated position and posture of the current key frame in the global map can enhance the adaptability to complex environments, and can make full use of the key frame image library to detect more comprehensive and robust loops to cope with the impact of system cumulative errors.

[0012] Further, the step E includes: sequentially adding the nodes and boundary lines in the target keyframes of each frame to the global map, and then detecting the nodes and boundary lines added to the global map; in the global map, when it is detected that the nodes added in sequence and the boundary lines added are sequentially connected to form a circular path, the robot detects a circular path, and determines that there are corresponding nodes and boundary lines in the global map that are sequentially connected to form a path that repeatedly passes through the same node; whenever a circular path is detected, the nodes on the circular path are globally optimized; wherein, the nodes in a target keyframe represent the robot's posture points at the timestamp of the target keyframe of the frame. This can significantly reduce the cumulative error and help the robot to perform obstacle avoidance navigation more accurately and quickly.

[0013] Furthermore, the method for performing global posture optimization in step E includes: when the robot detects a circular passage, setting a frame of the image currently traversed by the robot as the image frame to be positioned, and then calculating the initial posture difference between the image frame to be positioned and the target key frame of the previous frame, wherein the timestamp of the target key frame of the previous frame is the timestamp of the posture of the robot most recently marked in the global map; then using the initial posture difference to construct a residual model between the two frames of images, and then optimizing the residual model to obtain a posture tracking compensation value, so that each type of residual amount included in the residual model reaches the expected minimum value; then controlling the posture of the robot most recently marked in the global map to be added to the posture tracking compensation value to obtain the optimized world coordinates of the robot, so as to compensate for the posture difference formed by the difference between the rate at which the robot marks the posture in the global map and the rate at which the image data or the inertial data is obtained; wherein, when the frame of the image currently traversed is updated to the target key frame of the previous frame, the optimized world coordinates of the robot are updated to the posture of the robot most recently marked in the global map, so as to achieve continuous optimization of the posture of the robot.

[0014] Furthermore, when the residual amount represented by the residual model includes the residual amount of line features and the residual amount of point features to constitute the visual reprojection error, it is determined that the posture tracking compensation value represents the posture difference tracked by the sliding window within a certain period of time when the residual is minimized; wherein the residual amount of line features is introduced from the matching feature line pairs, and the residual amount of point features is introduced from the matching feature point pairs; when the residual amount represented by the residual model includes the residual amount of acceleration, the residual amount of velocity, the residual amount of rotation angle and the residual amount of coordinates, it is determined that the posture tracking compensation value represents the posture difference integrated by the inertial sensor according to a certain period of time when the residual is minimized; wherein the certain time is the difference between the timestamp of the image frame to be positioned and the timestamp of the target key frame of the previous frame.

[0015] In summary, in the process of executing steps A to D, multiple image frames are screened multiple times, and while introducing point feature and line feature constraints, some key frames that have been tracked and loop detected are removed, and then the target key frame is output. Then, under the triggering of the circular path formed between the nodes (representing the robot's posture) contained in the target key frame and the boundary line, the initial posture difference between the two image frames is calculated and determined through the strategy of dynamically estimating the timestamp, and a residual model is constructed. Then, the residual model is minimized. Compared with the existing visual tracking algorithm, the posture tracking compensation value is solved under the condition of smaller computing power, so that the inertial data can be optimized, a large amount of computing is saved, computing efficiency is improved, and positioning and map construction speed are improved.

[0016] Furthermore, the source of the initial posture difference includes: when the robot extracts matching feature point pairs and matching feature line pairs from the image frame to be positioned and the previous target key frame by executing step A, the corresponding preprocessed inertial data at the timestamp of each image frame is controlled to be subtracted to obtain the initial posture difference.

[0017] In summary, before performing global pose optimization, the real-time acquired and preprocessed inertial data is called to calculate the initial pose difference. Then, by fusing the preprocessed inertial data, the initial positioning of the image features in the target key frame can be predicted, so as to construct a residual model between the two frames of images and optimize the residual model, thereby obtaining the optimal initial pose difference while minimizing the residual, and then compensating it to the latest marked pose of the robot in the global map, thereby improving the performance of data fusion and enhancing the constraint relationship between inertial data and image data.

[0018] Furthermore, in step A, the method for preprocessing the inertial data includes: integrating the inertial data acquired in real time by the inertial sensor to obtain preprocessed inertial data, so that: when the inertial data is acquired by the code disk, the preprocessed inertial data represents the relative displacement between two image frames; when the inertial data is acquired by the gyroscope, the preprocessed inertial data represents the relative rotation angle between two image frames. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A flowchart of a vision-based joint positioning method disclosed in an embodiment of the present application. Implementation

[0020] Below, in conjunction with the drawings of the specification, by further describing the specific implementation of the present application, the technical solution of the present application and its beneficial effects are made clearer and clearer. The following description of the embodiments with reference to the drawings is exemplary, intended to explain the present application, and cannot be understood as a limitation of the present application. In the present application, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "includes", "comprising" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment including a series of elements includes not only those elements, but also includes other elements that are not clearly listed, or also includes elements inherent to such process, method, article or equipment. In the absence of more restrictions, the elements defined by the sentence "including one..." do not exclude the existence of other identical elements in the process, method, article or equipment including the elements.

[0021] In this application, an inertial measurement unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of a robot. Usually, an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer detects the acceleration signals of the robot in the coordinate system in three independent axes, while the gyroscope detects the angular velocity signals of the robot relative to the navigation coordinate system, thereby measuring the angular velocity and acceleration of the robot in an indoor environment, so that the optimal posture of the robot can be solved by integration, least squares method construction and other operations.

[0022] Traditional visual SLAM methods mainly rely on feature points in the environment and track feature points for positioning and mapping. However, in areas with little texture (meaning that image feature points are relatively sparse) and in scenes with blurred images caused by frequent changes in light intensity, the visual positioning ability is reduced or even fails due to the inability to effectively track features. This application is an extension and improvement of the traditional visual SLAM method. It extracts the pose information between two frames of images, as well as the matching point features and line features in each frame of images, constructs loop constraints, determines the association between feature data of different sensor types, and constructs a map of point and line features by tracking and detecting image frames; fuses image data and inertial data to solve, and compensates to overcome the influence of errors to optimize pose.

[0023] As an embodiment, a vision-based joint positioning method is disclosed, such as Figure 1 As shown, the joint positioning method includes: Step A: The robot obtains image data in real time through the camera, which can be recorded as the robot obtaining the current image frame; and obtains inertial data in real time through the inertial sensor, and then pre-processes the image data and the inertial data respectively.

[0024] In this embodiment, the joint positioning method is a robot, including but not limited to a sweeping robot, a companion robot, a transport robot, etc. A camera is mounted on the front end of the robot, and the lens of the camera points in the forward direction of the robot, and is used to collect image data in front of the robot, including point features and line features of one or more preset landmarks.

[0025] According to the frame count, when the robot obtains image data in real time through the camera, the robot obtains feature points and feature lines from the current image frame, wherein the feature point refers to an environmental element that exists in the form of a point in the environment where the camera is located, and can be matched with a previously collected image frame in the form of a point feature during the preprocessing of the image data; and the feature line refers to an environmental element that exists in the form of a boundary line in the environment where the camera is located, and can be matched with a previously collected image frame in the form of a line feature during the preprocessing of the image data, thereby extracting matching image features and tracking the current image frame.

[0026] Inertial sensors are generally installed inside the robot body. For example, the encoder is installed in the driving wheel of the robot to obtain the displacement information generated during the movement of the robot. Inertial sensors (such as gyroscopes) are used to obtain the angle information generated during the movement of the robot. Among them, the encoder and the inertial measurement unit constitute the inertial system, both of which are inertial sensors used to obtain inertial data. In the process of preprocessing the inertial data, the inertial navigation data integral can be obtained, including the speed obtained by the integral of the acceleration, the angle obtained by the integral of the angular velocity, etc. In the process of preprocessing the inertial data, the maximum and minimum values, and the large jump values ​​in the middle can also be processed. The zero drift value is obtained in the static state, and the zero drift value is subtracted from all valid inertial data to obtain the calibrated inertial data.

[0027] The camera and the inertial sensor can be configured to synchronously sense data and synchronously preprocess the data sensed by each, so that step A provides a good initial value for subsequent steps, including the initial pose difference between two adjacent image frames, which facilitates modeling of the image data using a residual model.

[0028] Step B: The robot obtains the image frames of successful tracking from the preprocessed image data, and then extracts the key frames from the image frames of successful tracking. Each time a current image frame is obtained, multiple feature points and multiple feature lines on the current image frame are tracked based on the current inertial data, and the image frames of successful tracking are obtained based on the matching relationship between these feature points and the matching relationship between the feature lines. On this basis, the key frames are further extracted from the image frames of successful tracking in combination with the parallax between two adjacent image frames. The key frames are expressed in the form of, for example, filling a sliding window or being scanned and covered by a sliding window.

[0029] The key frame information includes point features, line features, and the position information of the key frame in the robot's environment; the point features include the pixel position information of the feature points in the key frame, and the line features include the pixel position information of the feature lines in the key frame. The key frame information is output when the current image frame is determined to be a key frame.

[0030] Step C, the robot performs loop detection on the key frames; specifically, when performing loop detection, it is necessary to locate and match the key frame of the current frame with each image frame in the pre-set image library one by one, and select the necessary key frames based on the pre-set map library as the matching standard. In this embodiment, loop detection is set as a loop optimization for the key frames extracted in step B. It has good robustness in low-texture and light source change scenes, and does not require high computing power.

[0031] In the positioning matching, the camera pose transformation relationship between the current frame image and the standard frame image will be determined. The robot can use this pose transformation relationship to match feature points and feature lines, filter out matching key frames and obtain the pose information of matching key frames to achieve robot positioning; among them, in the pose transformation relationship of the camera between the two frames of images, the initial state quantities of the rotation matrix and the initial state quantities of the translation vector involved are pre-set.

[0032] The matching between feature points and the matching between feature lines will involve the matching between SIFT descriptors or LBD descriptors, which is suitable for matching point features and line features within the SLAM algorithm architecture.

[0033] Step D: The robot selects target key frames from the key frames according to the loop detection situation. In this embodiment, the target key frame may be a key frame that fails to participate in the loop detection, or the first key frame generated in the image data that is re-preprocessed after the point and line feature tracking fails. In this embodiment, only these types of key frames are added to the robot's pre-set global map. Compared with the prior art, the number of key frames used for positioning in the global map is reduced without missing useful feature information, thereby reducing the computing power consumed in maintaining the robot's navigation map.

[0034] The robot can filter out map points from the feature points of the target key frame to perform posture optimization, feature point or feature line depth optimization of the robot.

[0035] Step E: The robot optimizes the global posture of the target key frame in combination with the preprocessed inertial data to obtain the optimized world coordinates of the robot to compensate for the posture difference formed by the difference between the rate at which the robot marks the posture in the global map and the rate at which the image data or the inertial data is acquired. Overcome the influence of the acquisition state of the inertial data or the image data, especially overcome the influence of the image blur scene caused by the low-texture area (meaning that the image feature points are relatively sparse) and the frequent changes in light intensity on the acquisition state of the inertial data or the image data.

[0036] The optimized world coordinates can be updated to the optimized world coordinates of the robot, wherein the output rate of the optimized world coordinates depends on the rate of global pose optimization of the target key frame, specifically the rate of global optimization after preprocessing the inertial data and image data acquired in real time. At the same stage, the robot keeps acquiring the image data or the inertial data in real time. The optimized world coordinates of the robot are outputted when the output timestamp of the optimized world coordinates of the robot and the output timestamp of the preprocessed inertial data are aligned. Preferably, step A configures the camera and the inertial sensor to synchronously acquire sensor data, the sensor data acquired by the camera is image data, and the sensor data acquired by the inertial sensor is inertial data. The rate at which the robot's real-time posture is marked in the global map (the rate at which the posture of the global map is updated) is lower than the rate at which the inertial sensor acquires inertial data. Therefore, in the process of global posture optimization, step E can compensate for the difference between the rate at which the robot's posture is marked in the global map and the rate at which the inertial sensor acquires inertial data by adding the posture tracking compensation value to the latest posture marked in the global map to obtain the optimized world coordinates of the robot. This allows the output of a world coordinate in a time-stamp-aligned manner.

[0037] It should be noted that each time a node in a frame of target keyframe is added to the global map, the posture marked by the robot in the global map will be updated. In some embodiments, the update frequency is lower than the rate of preprocessing the image data or the inertial data. Therefore, it is necessary to use a posture tracking compensation value for timestamp alignment so that the robot's optimized world coordinate output frequency is faster than the frequency of the robot's posture marking in the global map, so that the relevant posture features marked in real time can effectively track the sensor sensing data.

[0038] In step E, a residual model or a reprojection error formula can be constructed by combining the image data preprocessed in step A with the inertial data preprocessed in step A, wherein the inertial data preprocessed in step A can represent the pose difference between two matching image frames (such as key frames whose point and line features are matched) in the image data preprocessed in step A; then, based on the residual model or the reprojection error formula, the pose in the target key frame is globally optimized, including first solving the initial pose difference between the two image frames, and then on this basis, the robot relies on the inertial sensor to sense the change in inertial data between two target key frames successively acquired by the camera (the displacement change sensed by the code disk between two target key frames successively acquired by the camera (the result from the integration of the code disk data), and the angle change sensed by the gyroscope between two target key frames successively acquired by the camera (the result from the integration of the gyroscope data)), construct the residual and use the least squares method for optimization, that is, complete the construction of the least squares problem to find the optimal compensation value, and then use the optimal compensation value to correct the coordinates of the robot's latest mark in the global map to obtain the optimized world coordinates of the robot.

[0039] To summarize, this embodiment obtains successfully tracked image frames from preprocessed image data, extracts key frames from successfully tracked image frames, and then filters out target key frames from key frames according to loop detection conditions, thereby achieving multiple screening of multiple image frames. This embodiment selects necessary key frames (target key frames) for global pose optimization, and compared with pure visual tracking results, reduces the number of key frames required for visual navigation and positioning, and reduces computing power requirements; it also improves the performance of data fusion between inertial data and image frames, thereby having the characteristics of faster feature search speed, higher accuracy, and stronger robustness.

[0040] This embodiment combines the preprocessed inertial data with the image feature information in the target key frame, performs global pose optimization on the target key frame, and solves the optimized pose of the robot to compensate for the pose difference formed by the difference between the rate at which the robot marks the pose in the global map and the rate at which the image data or the inertial data is acquired, so that the inertial data can be optimized, the stability of inter-frame feature tracking is improved, and the influence of the sliding window scanning frame rate and the coordinate update frequency in the global map on the optimized world coordinate output frequency of the robot is reduced, thereby overcoming the influence of the acquisition state of the inertial data or the image data, especially overcoming the influence of the image blur scene caused by the low-texture area (meaning that the image feature points are relatively sparse) and the frequent changes in light intensity on the acquisition state of the inertial data or the image data.

[0041] In the above embodiment, after the image data and the inertial data are preprocessed respectively in step A, step B sequentially scans out at least one frame of the successfully tracked image frame from the preprocessed image data by setting a sliding window, so that the successfully tracked image frame is filled into the sliding window in units of frames, and the position and posture of each image frame relative to the walking starting position point of the robot are obtained in sequence, and the position and posture difference between two adjacent image frames or two adjacent successfully tracked image frames can be calculated in sequence, which can correspond to a pair of matching point features and a pair of matching line features in the two image frames. After the image data and the inertial data are preprocessed in step A, when the sliding window fails to scan the image frame of successful tracking from the preprocessed image data, the real-time acquired image data is preprocessed again, and a new sliding window is set so that the new sliding window scans the image frame of successful tracking from the re-preprocessed image data. The new sliding window may be translated relative to the original sliding window to scan the new image frame; the old image frame filled in it may also be deleted and the new image frame may be added, which is equivalent to scanning the image frame of successful tracking from the re-preprocessed image data. Thereby, the search range of subsequent point features and line features is narrowed, and the filtering of feature points and feature lines is completed.

[0042] It should be noted that the sliding window is pre-created by the robot to scan the acquired image frames and allow the image frames with successful tracking to be filled in the sliding window. The sliding window can be configured to accommodate a cache space for at least one frame of the image frame with successful tracking, and can slide within a frame of image, and also shift in the global coordinate system following the walking of the robot.

[0043] The integral of the angular velocity measured by the gyroscope, the integral of the rotational speed measured by the encoder, and the line feature constraints and point feature constraints in the image frame can all become inputs to the sliding window, optimizing the window of displacement between each image frame filled in the sliding window, thereby obtaining the position and posture of each frame relative to the robot's walking starting position.

[0044] As an embodiment, step D also includes, when the sliding window fails to scan the image frame of successful tracking from the current preprocessed image data, re-preprocessing the real-time acquired image data and setting a new sliding window accordingly, marking the first key frame scanned by the new sliding window from the re-preprocessed image data as the target key frame, and filling the first key frame into the sliding window, and when the first key frame of the new sliding window is to be added to the global map, the current key frame and the previous key frame are directly linked by the integral result of inertial data to represent the posture difference between the two frames of images. Wherein, the situation that the sliding window fails to scan the image frame of successful tracking from the current preprocessed image data can be understood as the sliding window fails to track the image frame, or loses the key frame, and the image data needs to be re-preprocessed and a new sliding window is set to accommodate the new image frame of successful tracking. In order to reduce the usage of key frames, the first key frame scanned by the new sliding window from the re-preprocessed image data is marked as the target key frame, so that the necessary key frames are selectively added to the global map for posture optimization. Therefore, when the target keyframe is added to the global map, the number of keyframes in the global map can be minimized, and since the tracking is successful without missing useful feature information, the computing power consumed in maintaining the image frame or map is reduced.

[0045] As an embodiment, the method for preprocessing the image data in step A includes: The robot extracts matching feature point pairs and matching feature line pairs from the current image frame and the previous image frame, calculates the sum of the number of matching feature point pairs and the number of matching feature line pairs, and marks the sum as the number of feature matches; wherein the previous image frame includes the previous key frame; the previous image frame may be derived from the image in the aforementioned sliding window, that is, it may be a predetermined key frame.

[0046] Image data includes point features and line features. A feature point pair can be understood as a pair of feature points separating two image frames, or a pair of point features; a feature line pair can be understood as a pair of feature lines separating two image frames, or a pair of line features, thereby introducing line feature constraints, greatly improving the ability to adapt to the working environment, and significantly enhancing the robustness of the method. Preferably, a feature line can include multiple feature points, so the number of feature matches can also be the total number of matched feature point pairs.

[0047] When the number of feature matches is greater than a preset tracking number threshold, it is determined that the current frame image frame is the image frame with successful tracking, and it can be determined that the sliding window tracking is successful, or the robot tracking image is successful, and then it is determined that the camera has completed calibration; the preprocessed inertial data disclosed in this embodiment can represent the conversion relationship between the coordinate system of the current frame image frame and the coordinate system of the previous frame image frame, including a translation conversion relationship, a rotation conversion relationship, a displacement difference, an angle increment, etc. The completion of camera calibration indicates that the conversion relationship between the coordinate system of the current frame image frame and the coordinate system of the previous frame image frame has completed calibration.

[0048] When the number of feature matches is less than or equal to a preset tracking number threshold, it is determined that the current image frame is not the image frame that has been successfully tracked, and it is determined that the sliding window tracking has failed, or the robot has failed to track the image. Then the robot can clear each image frame in the sliding window and create a new sliding window scanning image frame, which corresponds to the new sliding window disclosed in the aforementioned embodiment that scans and tracks the image frame successfully from the re-preprocessed image data.

[0049] It should be noted that, in this embodiment, image frame tracking is used to represent the matching between feature points of a previously acquired image and feature points of a current frame image, and the matching between feature lines of a previously acquired image and feature lines of a current frame image. Step A makes full use of point feature and line feature fusion matching to extract a more accurate and robust image frame.

[0050] Compared with the current traditional pure visual feature tracking technology, this embodiment has good robustness, stronger anti-interference ability, higher accuracy, and the advantages of saving and narrowing the feature search range and shortening the feature search time. It is suitable for scenes where pure image feature tracking cannot be performed, such as image degradation and low-texture areas. It improves the accuracy of pose estimation in low-light environments and structured scenes, and ultimately improves the accuracy of global map pose estimation.

[0051] Preferably, the robot extracts feature points from the current image frame through a pre-configured operator (e.g., Shi-Tomasi operator), and can extract SIFT feature points; then the optical flow method is used to find matching points in the previous image frame for tracking these feature points, and a pair of feature points that match before and after are obtained. The robot extracts line segments in the current image frame and the previous image frame as feature lines by using the LSD (least significant difference method) algorithm, and then a pre-set descriptor (e.g., LBD descriptor) can be used to describe and match line features. In this way, point features and line features in the image are extracted, and then the extracted point features and line features are tracked and used for subsequent key frame judgment. Accordingly, the preset tracking quantity threshold is preferably 20.

[0052] In short, tracking is to match features between two frames collected successively. Point and line features and feature descriptions are extracted from each frame. The feature description is usually a multi-dimensional vector, so the distance between two feature descriptions can be calculated.

[0053] As an embodiment, in the step B, the method of extracting key frames from the successfully tracked image frames includes: Among at least two image frames that have been successfully tracked and acquired in sequence, a parallax comparison is performed between the current image frame and the previous image frame to obtain the parallax between the two adjacent image frames; wherein the previous image frame includes the previous key frame, that is, the current image frame is allowed to be compared with the previously acquired key frame for parallax. The parallax comparison here includes: calculating the displacement difference and angle difference between the current image frame and the previous image frame; if the displacement difference is greater than a certain displacement threshold, or the angle difference is greater than a certain angle threshold, it is considered that the parallax between the two adjacent image frames is greater than the preset parallax threshold.

[0054] Matching feature point pairs are extracted from at least two image frames that are successfully tracked and acquired in sequence, and the number of matched feature point pairs acquired is counted; preferably, feature points are extracted from the current image frame, and SIFT feature points can be extracted, and then the optical flow method is used to track these feature points by searching for matching points in the previous image frame to obtain a pair of feature points that match each other, that is, a matched feature point pair.

[0055] When the disparity between two adjacent frames of images is greater than a preset disparity threshold, and the number of matching feature point pairs is greater than a preset feature number threshold, and the uniformity of feature points within the current frame of images is less than a preset uniform number threshold, the current frame of images is determined to be the key frame, and the current frame of images is filled into the sliding window, including filling in the 3D point features, line features, and key frame pose of the current frame of images in the world coordinate system, and loop detection can be performed in subsequent steps. The uniformity of feature points within the current frame of images represents the number of feature points distributed within a unit area within the current frame of images, which can be the maximum number of feature points distributed within each unit area divided within the current frame of images. If the maximum number of feature points distributed within each unit area divided within the current frame of images is within the uniform number threshold, it can be directly determined that the feature points within the current frame of images are uniformly distributed.

[0056] When the disparity between two adjacent image frames is less than or equal to a preset disparity threshold, and / or the number of matching feature point pairs is less than or equal to a preset feature number threshold, and / or the uniformity of feature points within the current image frame is greater than or equal to a preset uniform number threshold, it is determined that the current image frame is not the key frame and can be discarded to reduce the number of image frames that need to be stored and processed.

[0057] It should be noted that, in the two image frames in which tracking is successful acquired in sequence, the disparity of the two adjacent frames of images in which tracking is successful is greater than the preset disparity threshold, and the number of matching feature point pairs is greater than the preset feature number threshold, and when the uniformity of the feature points in the image frame in which the current frame is successfully tracked is less than the preset uniform number threshold, the number of matching feature point pairs in the two adjacent frames of images in which tracking is successful can be large enough but not too dense, so the number of feature points distributed per unit area in the image frame in which tracking is successful in the current frame is configured to be less than the preset uniform number threshold, and the preset feature number threshold is less than the preset tracking number threshold (the judgment threshold for the total number of point feature and line feature matching pairs), so that the feature points in the current frame image frame configured as a key frame are relatively evenly distributed.

[0058] Therefore, step B selects key frames based on disparity and the number of feature points, forming a screening operation on the image frames after the robot completes image tracking, narrowing the search range of point and line feature loop detection, and improving the accuracy and efficiency of robot pose estimation.

[0059] As an embodiment, the step C includes: Control the currently acquired key frame to perform feature matching with all key frames in the key frame image library in turn, and filter out matching frames from the key frame image library, and then count the number of matching frames; optionally, the key frame image library can also be stored in the form of a map library, which is used to store the position information of the pre-collected key frames, the key information of the movement between two key frames, the pixel coordinates involved in the point and line features in the same key frame, etc. The feature matching disclosed in this embodiment includes the matching of point features and the matching of line features.

[0060] Determine whether the number of matching frames is greater than a preset matching number threshold, if yes, determine that the loop closure detection of the currently obtained key frame is successful, otherwise determine that the loop closure detection of the currently obtained key frame is unsuccessful and mark the currently obtained key frame as the target key frame. Preferably, the preset matching number threshold may be less than or equal to the preset tracking number threshold, or be unrelated to the preset tracking number threshold. The currently obtained key frame that fails the loop closure detection may be updated to the map library.

[0061] Specifically, when it is determined that the loop detection of the currently obtained key frame is unsuccessful, the currently obtained key frame is marked as the target key frame, and then the various feature points and boundary lines in the target key frame are added to the global map. In some embodiments, in the sliding window, when it is determined through step C that the loop detection of one of the key frames is unsuccessful and the loop detection of other key frames is successful, the one of the key frames is marked as the target key frame, and then the nodes and boundary lines in the target key frame are added to the global map. The nodes in the target key frame represent the posture information of the robot at the time point corresponding to the target key frame, and can be marked as map points in the global map, which is convenient for detecting the repositioning ability of the robot. Therefore, the robot detects the matching relationship between the current frame key frame and the preset key frame through loop closure, so that the posture of the current frame key frame is estimated in the global map, which enhances the adaptability to complex environments, can make full use of the key frame image library to detect more comprehensive and robust loops, and deal with the influence of system cumulative errors.

[0062] As an embodiment, step E includes: after obtaining the target keyframes of each frame from the sliding window in sequence through step D, the robot sequentially adds the nodes and boundary lines in the target keyframes of each frame to the global map, and then detects the nodes and boundary lines added to the global map, specifically, the connectivity between each node and each boundary line is detected one by one. It should be noted that whenever a node in a target keyframe of a frame is added to the global map, it means that the posture of the node and the target keyframe in which it is located are updated to the global map, that is, the posture of the node or the node is marked in the global map.

[0063] In the global map, when it is detected that the nodes added in sequence and the boundary lines added are connected in sequence to form a circular path, it is determined that the robot has detected a circular path, and it is determined that there are corresponding nodes and boundary lines in the global map. The path formed by connecting in sequence repeatedly passes through the same node, and it is determined that the robot supports identifying a position point that has been reached, so that the global map has the property of a closed loop.

[0064] Whenever a circular passage is detected, a global pose optimization is performed on the nodes on the circular passage, wherein before the global pose optimization is performed, the pose information of some nodes may be updated to the global map, and by performing a global pose optimization on the target key frame where the nodes on the circular passage are located, the accuracy of the pose estimation of the global map can be improved. Among them, the nodes in a frame of target key frame represent the pose points of the robot at the timestamp of the target key frame of the frame, specifically the body center coordinates and pose angle of the robot. A node corresponds to a frame of target key frame, and also corresponds to the point features, line features and pose information of the target key frame of the frame. Specifically, in the process of repeatedly executing steps A to D, different target key frames are continuously generated, and the nodes and boundary lines corresponding to the target key frames are continuously inserted into the global map, which will continuously form a circular passage, that is, a loop is formed, which can also be understood as finding a loop frame from the global map. Every time a loop is formed, a global pose optimization is triggered for the poses corresponding to each node on the loop. This can significantly reduce the cumulative error and help the robot perform obstacle avoidance and navigation more accurately and quickly.

[0065] As an embodiment, the method for performing global pose optimization in step E includes: When the robot detects a circular passage, it means that a global pose optimization is triggered. The robot sets the current traversed image frame as the image frame to be positioned, and then calculates the initial pose difference between the image frame to be positioned and the previous target key frame, wherein the timestamp of the previous target key frame is the timestamp of the robot's latest marked pose in the global map, indicating the moment when the pose in the global map was most recently updated, and also the timestamp of the node most recently added to the global map (the moment when the pose of the node is updated to the global map); the timestamp of the current image frame traversed by the robot is recorded as the current moment, which can be the moment of the most recently filtered target key frame; therefore, the initial pose difference between the image frame to be positioned and the previous target key frame can be expressed as the relative pose difference between the current moment and the timestamp of the most recent pose update in the global map, which can be the pose quantity tracked by the sliding window or the pose quantity integrated by the inertial sensor.

[0066] The initial posture difference is then used to construct a residual model between the two frames of images, including calculation of errors such as residual quantities based on the initial posture difference (initial displacement difference and initial angle difference); the residual model is then optimized to obtain a posture tracking compensation value so that all types of residual quantities included in the residual model reach the expected minimum value; preferably, the residual model can be optimized using the least squares method to minimize the residual calculated under the residual model.

[0067] Then, the robot's latest marked posture in the global map (also the coordinate and angle information last updated in the map) is added to the posture tracking compensation value to obtain the optimized world coordinates of the robot, which are compensated to the robot position coordinates in the global map coordinate system to compensate for the posture difference formed by the difference between the rate of marking the robot's posture in the global map and the rate of acquiring the image data or the inertial data; wherein, when the currently traversed image frame is updated to the previous target key frame, the robot's optimized world coordinates are updated to the robot's latest marked posture in the global map to achieve continuous optimization of the robot's posture. Further reduce the frame rate of the sliding window scanning key frame and the update rate of the feature points in the key frame.

[0068] On the basis of the foregoing embodiment, when the residual amount represented by the residual model includes the residual amount of the line feature and the residual amount of the point feature to constitute the visual reprojection error, it is determined that the posture tracking compensation value represents the residual minimization, and the posture difference tracked by the sliding window within a certain period of time can represent the relative posture between the two currently calculated image frames; wherein, the certain time is the difference between the timestamp of the image frame to be positioned and the timestamp of the target key frame of the previous frame. The residual amount of the line feature is introduced from the matching feature line pair, and the residual amount of the point feature is introduced from the matching feature point pair, so that the point-line feature is used to model the posture difference. It also improves the performance of camera and inertial sensor data fusion.

[0069] When the residual amount of the point feature or the residual amount of the line feature is smaller, the corresponding projection position relative to the polar line deviation in the imaging plane to which it is converted is smaller, and the corresponding feature matching degree is higher, otherwise, the corresponding feature matching degree is lower.

[0070] During the pose optimization process, the transformation matrix can be adjusted by minimizing the visual reprojection error. The transformation matrix is ​​the rotation matrix corresponding to the rotation angle between the two target key frames currently calculated.

[0071] When the residuals represented by the residual model include the residuals of acceleration, velocity, rotation angle and coordinates, it is determined that the posture tracking compensation value represents the minimum residual, and the posture difference integrated by the inertial sensor according to a certain time can represent the relative posture between the two currently calculated image frames; wherein the certain time is the difference between the timestamp of the image frame to be positioned and the timestamp of the target key frame of the previous frame. Thus, the posture difference between the two image frames is modeled using the integration result of the inertial data, and the optimized world coordinates of the robot are output synchronously with the inertial data, or the optimized world coordinates of the robot are output synchronously with the preprocessed inertial data.

[0072] In summary, in the process of executing steps A to D, multiple image frames are screened multiple times, and while introducing point feature and line feature constraints, some key frames that have been tracked and loop detected are removed, and then the target key frame is output. Then, under the triggering of the circular path formed between the nodes (representing the robot's posture) contained in the target key frame and the boundary line, the initial posture difference between the two image frames is calculated and determined through the strategy of dynamically estimating the timestamp, and a residual model is constructed. Then, the residual model is minimized. Compared with the existing visual tracking algorithm, the posture tracking compensation value is solved under the condition of smaller computing power, so that the inertial data can be optimized, a large amount of computing is saved, computing efficiency is improved, and positioning and map construction speed are improved.

[0073] In the above embodiment, the sources of the initial posture difference include: when the robot extracts matching feature point pairs and matching feature line pairs from the image frame to be positioned and the previous target key frame by executing step A, the corresponding preprocessed inertial data at the timestamp of each image frame is controlled to be subtracted to obtain the initial posture difference, wherein the image frame to be positioned is an image frame acquired when the robot adds the node of the latest acquired target key frame to the global map and performs global posture optimization or detects a circular path, which can be recorded as the current image frame, and the previous target key frame is a target key frame that is newly acquired and updated to the global map before the current image frame is acquired.

[0074] In summary, before performing global pose optimization, the real-time acquired and preprocessed inertial data is called to calculate the initial pose difference. Then, by fusing the preprocessed inertial data, the initial positioning of the image features in the target key frame can be predicted, so as to construct a residual model between the two frames of images and optimize the residual model, thereby obtaining the optimal initial pose difference while minimizing the residual, and then compensating it to the latest marked pose of the robot in the global map, thereby improving the performance of data fusion and enhancing the constraint relationship between inertial data and image data.

[0075] In some embodiments, in the process of preprocessing the inertial data in step A, the speed of the feature points of different image frames is introduced to model the time deviation (or time offset). Since the motion time between two consecutive image frames is extremely small, the camera motion is regarded as uniform motion. Assuming that the camera moves from Ck to Ck+1 in the time period tk to tk+1, the coordinates of the same feature point in the corresponding normalized plane move from (uk, vk) to (uk+1, vk+1), the optical flow method disclosed in the prior art can be used to calculate the speed of the feature point; and then a td visual image can be constructed. The reprojection error is compensated by the product of the feature point velocity and the time difference td. The residual calculation model involved can be found in Chinese patent CN116380070A. The Chinese patent CN116380070A describes a specific mathematical formula containing the td visual reprojection error, which is associated with the residual model disclosed in this application. After adjusting some parameters in Chinese patent CN116380070A, those skilled in the art can obtain the residual model disclosed in this application and implement corresponding optimization to minimize the reprojection error, which represents the aforementioned posture tracking compensation value.

[0076] It should be noted that the method for preprocessing the inertial data in step A includes: integrating the inertial data acquired by the inertial sensor in real time to obtain preprocessed inertial data, so that: when the inertial data is acquired by the code disk, the preprocessed inertial data represents the relative displacement between two image frames; when the inertial data is acquired by the gyroscope, the preprocessed inertial data represents the relative rotation angle between two image frames.

[0077] In some preprocessing embodiments, the sensing data of the inertial sensor is firstly corrected for zero offset, so that the initial value of the speed and the initial value of the rotation angle involved in the inertial data acquired in real time by the corrected inertial sensor are both 0, and then the inertial data acquired in real time by the corrected inertial sensor can be configured as calibrated inertial data, and then the calibrated inertial data is integrated to obtain the preprocessed inertial data, so that: when the inertial data is acquired by the code disk, the preprocessed inertial data represents the relative displacement between the current image frame and the previous image frame, and constitutes the translation vector after the coordinate system is calibrated; when the inertial data is acquired by the gyroscope, the preprocessed inertial data represents the relative rotation angle between the current image frame and the previous image frame, and constitutes the rotation matrix after the coordinate system is calibrated.

[0078] It should be understood that although Figure 1The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0079] The above is only a schematic description of the specific implementation of the present invention, and the present invention is not limited thereto. The splitting, merging, changes in the execution order of steps, the splitting, merging, and changes in information transmission of modules are all within the protection scope of the present invention. In addition, it should be understood that although this specification is described in accordance with the implementation methods, not every implementation method contains only one independent technical solution. This narrative method 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 can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A vision-based joint positioning method, characterized in that: The joint positioning method comprises: Step A: The robot obtains image data in real time through a camera and obtains inertial data in real time through an inertial sensor, and then pre-processes the image data and inertial data respectively; Step B, the robot obtains the image frame of successful tracking from the pre-processed image data, and then extracts the key frame from the image frame of successful tracking; Step C: The robot performs loop detection on the key frame; Step D: The robot selects the target key frame from the key frames according to the loop detection situation; Step E: The robot performs global pose optimization on the target key frame in combination with the preprocessed inertial data to obtain the optimized world coordinates of the robot to compensate for the pose difference caused by the difference between the rate at which the robot marks the pose in the global map and the rate at which the image data or the inertial data is acquired.

2. The joint positioning method according to claim 1, characterized in that: After the image data and the inertial data are preprocessed in step A, in step B, at least one frame of the successfully tracked image frame is sequentially scanned from the preprocessed image data by setting a sliding window, so that the successfully tracked image frame is filled into the sliding window in units of frames; After the image data and the inertial data are preprocessed respectively in step A, when the sliding window fails to scan out the image frame of successful tracking from the preprocessed image data, the image data acquired in real time is preprocessed again, and a new sliding window is set so that the new sliding window scans out the image frame of successful tracking from the re-preprocessed image data.

3. The joint positioning method according to claim 2, characterized in that: Step D also includes, when the sliding window fails to scan the successfully tracked image frame from the current preprocessed image data, re-preprocessing the real-time acquired image data and setting a new sliding window accordingly, marking the first key frame scanned by the new sliding window from the re-preprocessed image data as the target key frame, and filling the first key frame into the sliding window.

4. The joint positioning method according to claim 3, characterized in that: The method for preprocessing the image data in step A includes: The robot extracts matching feature point pairs and matching feature line pairs from the current image frame and the previous image frame, calculates the sum of the number of matching feature point pairs and the number of matching feature line pairs, and marks the sum as the feature matching number; wherein the previous image frame includes the previous key frame; When the number of feature matches is greater than a preset tracking number threshold, determining that the current image frame is the image frame with successful tracking; When the number of feature matches is less than or equal to a preset tracking number threshold, determining that the current image frame is not the image frame with successful tracking; The image data includes point features and line features.

5. The joint positioning method according to claim 3, characterized in that: In the step B, the method of extracting key frames from the successfully tracked image frames includes: Among at least two image frames that are successfully tracked and are acquired in sequence, a disparity comparison is performed between a current image frame and a previous image frame to obtain a disparity between two adjacent image frames; wherein the previous image frame includes a previous key frame; Extracting matching feature point pairs from at least two image frames that are successfully tracked and obtained in sequence, and counting the number of matching feature point pairs obtained; When the disparity between two adjacent image frames is greater than a preset disparity threshold, and the number of matching feature point pairs is greater than a preset feature number threshold, and the uniformity of feature points in the current image frame is less than a preset uniform number threshold, the current image frame is determined to be the key frame, and the current image frame is filled into the sliding window; the uniformity of feature points in the current image frame represents the number of feature points distributed within a unit area in the current image frame.

6. The joint positioning method according to claim 4, characterized in that: The step C comprises: Control the currently acquired key frame to perform feature matching with all key frames in the key frame image library in turn, select matching frames from the key frame image library, and then count the number of matching frames; It is determined whether the number of matching frames is greater than a preset matching number threshold. If yes, it is determined that the loop detection of the currently obtained key frame is successful. Otherwise, it is determined that the loop detection of the currently obtained key frame is unsuccessful and the currently obtained key frame is marked as the target key frame.

7. The joint positioning method according to claim 6, characterized in that: The step E comprises: Add the nodes and boundary lines in the target key frames of each frame to the global map in turn, and then detect the nodes and boundary lines added to the global map; In the global map, when it is detected that the nodes added in sequence and the boundary lines added in sequence are connected in sequence to form a circular path, the robot detects a circular path and determines that there exists in the global map a path formed by the corresponding nodes and boundary lines connected in sequence that repeatedly passes through the same node; Whenever a circular path is detected, a global pose optimization is performed on the nodes on the circular path; Among them, the nodes in a target key frame represent the robot's position point at the timestamp of the target key frame of the frame.

8. The joint positioning method according to claim 7, characterized in that: The method for performing global pose optimization in step E includes: When the robot detects a circular path, it sets the current image frame it is traversing as the image frame to be positioned, and then calculates the initial pose difference between the image frame to be positioned and the previous target key frame, where the timestamp of the previous target key frame is the timestamp of the robot's latest marked pose in the global map; Then, the residual model between the two frames of images is constructed using the initial posture difference, and the residual model is optimized to obtain a posture tracking compensation value, so that each type of residual amount included in the residual model reaches an expected minimum value; Then, the robot's latest marked posture in the global map is controlled to be added to the posture tracking compensation value to obtain the optimized world coordinates of the robot, so as to compensate for the posture difference formed by the difference between the rate at which the robot's posture is marked in the global map and the rate at which the image data or the inertial data is acquired; wherein, when the currently traversed image frame is updated to the previous target key frame, the robot's optimized world coordinates are updated to the robot's latest marked posture in the global map, so as to achieve continuous optimization of the robot's posture.

9. The joint positioning method according to claim 8, characterized in that: When the residual amount represented by the residual model includes the residual amount of the line feature and the residual amount of the point feature to constitute the visual reprojection error, it is determined that the pose tracking compensation value represents the pose difference tracked by the sliding window within a certain period of time when the residual is minimized; wherein the residual amount of the line feature is introduced from the matched feature line pair, and the residual amount of the point feature is introduced from the matched feature point pair; When the residual amount represented by the residual model includes the residual amount of acceleration, the residual amount of velocity, the residual amount of rotation angle and the residual amount of coordinates, it is determined that the posture tracking compensation value represents the posture difference integrated by the inertial sensor according to a certain time when the residual is minimized; The certain time is the difference between the timestamp of the image frame to be located and the timestamp of the previous target key frame.

10. The joint positioning method according to claim 9, characterized in that: The sources of the initial pose difference include: When the robot extracts matching feature point pairs and matching feature line pairs from the image frame to be positioned and the previous target key frame by executing step A, the robot controls the subtraction of the corresponding preprocessed inertial data at the timestamp of each image frame to obtain the initial posture difference.

11. The joint positioning method according to claim 10, characterized in that: The method for preprocessing inertial data in step A includes: The inertial data acquired by the inertial sensor in real time is integrated to obtain preprocessed inertial data, so that: when the inertial data is acquired by the code disk, the preprocessed inertial data represents the relative displacement between two image frames; when the inertial data is acquired by the gyroscope, the preprocessed inertial data represents the relative rotation angle between two image frames.

Images