A robot positioning method and apparatus

Abstract

The application discloses a robot positioning method and device, and relates to the field of information technology, and aims to improve the positioning precision of a mobile robot. The application filters out interference features from visual data, and then calculates the pose of the robot according to the pose change of the matched feature points, so that the positioning precision is improved. On this basis, a method for fusing multi-dimensional sensing data to construct a factor graph and then optimizing the pose of the robot is further provided, the advantages of various sensing data are summarized, the positioning precision is improved, and the positioning stability is also enhanced.

Description

Technical Field

[0001] This invention relates to the field of information technology, and in particular to a robot positioning method and apparatus. Background Technology

[0002] With the rapid development of smart homes, mobile robots, such as home service robots, are being used more and more widely in daily life. Accurate positioning of these robots is fundamental to planning and ensuring their proper operation. Mobile robots face complex and ever-changing environments; factors such as furniture placement and human activity place high demands on their positioning accuracy.

[0003] Currently, known positioning technologies mainly include positioning based on single sensors such as vision, inertial measurement, odometers, or lidar. However, single-sensor positioning has limitations. For example, vision-based positioning methods are prone to errors in image feature extraction and matching under conditions of changing lighting (night / day) or obstacle occlusion, leading to inaccurate positioning. Similarly, in inertial measurement-based positioning methods, the inertial measurement unit accumulates errors over time, causing positioning accuracy to decrease. Furthermore, in odometer-based positioning methods, wheeled odometers are affected by factors such as ground friction, wheel lift-off, and wheel slippage, resulting in deviations in the measured distance traveled, affecting positioning accuracy. Finally, single-line lidar may fail to acquire complete and accurate environmental information in complex environments, leading to large point cloud matching errors and significant positioning errors. Summary of the Invention

[0004] The purpose of this invention is to provide a robot positioning method and apparatus to address all or part of the problems mentioned above, thereby improving the positioning accuracy of mobile robots.

[0005] The technical solution adopted in this invention is as follows: A robot localization method, comprising: Acquire image data of the robot; Extract feature points from the image data; Interference feature points are identified and removed from the extracted feature points to obtain static feature points; Based on the robot's pose at the previous moment, the current pose data of the robot is estimated according to the change in pose of static feature points between the image data at the previous moment and the pose data at the previous moment.

[0006] In addition, this application also provides a robot positioning device, which includes a first processor and a first storage medium, the first storage medium storing first computer instructions, and the first processor running the first computer instructions to execute the above-described robot positioning method.

[0007] On the other hand, this application provides another robot localization method, which includes: Acquire multidimensional sensor data from the robot, including image data; time alignment of sensor data across all dimensions; The robot's pose data is estimated based on the sensor data for each dimension; wherein, the robot's first pose data is calculated based on the image data using the robot localization method described above. A factor graph is constructed based on each pose data point, and the factor graph optimization method is used to fuse the pose data points to obtain the robot's final pose.

[0008] In addition, this application also provides a robot positioning device, which includes a second processor and a second storage medium, the second storage medium storing second computer instructions, the second processor running the second computer instructions to execute the above-described second robot positioning method.

[0009] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are: The robot localization method proposed in this application effectively eliminates dynamic interference features in environments with varying conditions such as human movement and object movement, reducing the interference of activity factors on visual localization and improving the localization accuracy of robots in complex and changing environments. Furthermore, the robot localization method proposed in this application effectively integrates the data characteristics of different sensors, fully leveraging the advantages of sensors in each dimension and compensating for the shortcomings of single-sensor localization, thus effectively improving the accuracy of robot localization. Simultaneously, the fusion of multi-dimensional localization makes the localization more robust to changing environments. Attached Figure Description

[0010] The present invention will be described by way of example and with reference to the accompanying drawings, wherein: Figure 1 This is a flowchart of one implementation of the robot localization method.

[0011] Figure 2 This is a flowchart of another implementation of the robot localization method.

[0012] Figure 3 yes Figure 2 Data flow diagram of the robot localization method in one embodiment.

[0013] Figure 4 This is a flowchart of one implementation of the factor graph optimization method.

[0014] Figure 5 This is a flowchart of another implementation of the factor graph optimization method.

[0015] Figure 6 , Figure 7These are structural diagrams of two different robot positioning devices. Detailed Implementation

[0016] All features disclosed in this specification, or all steps in all disclosed methods or processes, may be combined in any way, except for mutually exclusive features and / or steps.

[0017] Any feature disclosed in this specification (including any appended claims and abstract) may be replaced by other equivalent or similar features, unless specifically stated otherwise. That is, unless specifically stated otherwise, each feature is merely one example of a series of equivalent or similar features.

[0018] In view of the limitations of various known localization methods based on a single sensor (such as vision, inertial measurement, odometer or lidar), this application proposes a robot localization method and apparatus, which aims to effectively improve the localization accuracy of mobile robots in complex and ever-changing environments.

[0019] like Figure 1 As shown, in one feasible implementation of this application, the robot localization method includes the following steps: S1. Acquire image data of the robot.

[0020] Image data is typically acquired by image acquisition devices such as cameras. As an optional implementation, image data is acquired by a binocular depth camera. Within a positioning cycle, the binocular depth camera captures multiple raw images of the left and right eyes at multiple times (timestamps) corresponding to a predetermined sampling frequency. For example, in... to In the previous positioning cycle, 30 sets of raw images for both the left and right eyes were captured. The timestamps for each set of raw images for both the left and right eyes were the same. It contains 30 timestamps.

[0021] S2. Extract feature points from image data.

[0022] As an optional implementation, in step S2, feature points are extracted from the acquired image data (such as the original left and right eye images collected above) using the FAST feature point extraction method to obtain the corner point data corresponding to the image data.

[0023] In one feasible implementation, the method for extracting feature points includes: S21. Initialize the neighborhood radius.

[0024] Before extracting feature points from image data, each pixel in the image data is traversed for preparation. A neighborhood radius r is set, which determines the range of pixel comparisons performed during feature point extraction. For example, if the radius r = 3, then pixels within a circle with a radius of 3 pixels surrounding the currently traversed pixel are used as comparison objects with the currently traversed pixel.

[0025] S22. Compare the currently traversed pixel with its neighboring pixels.

[0026] If the currently traversed pixel is represented by p, then for the currently traversed pixel p, multiple pixels are selected in the neighborhood of radius r with pixel p as the center. The selection method is, for example, to select at equal intervals within the circular neighborhood, such as selecting 16 (or other numbers) pixels at equal intervals.

[0027] Representing each selected pixel as n (where n is a positive integer), the grayscale value of the currently traversed pixel p is represented as: The grayscale values ​​of the selected pixels are represented as .

[0028] Compare the grayscale values ​​of the currently traversed pixels with those of the selected pixels.

[0029] S23. Determine corner points based on pixel comparison results.

[0030] Set a threshold k. If m (where m is a positive integer and its value does not exceed the total number of selected pixels in the neighborhood, e.g., m is 12, or any other number) of selected pixels in the neighborhood satisfy one of the following conditions, then the currently traversed pixel p is determined to be a candidate corner point: .

[0031] For example, in the comparison and judgment process, starting from a certain pixel in the neighborhood, pixel n is selected clockwise or counterclockwise and compared with the gray value of the currently traversed pixel p to determine whether one of the above conditions is met. If all m consecutive pixels meet the conditions, then the currently traversed pixel p is determined to be a candidate corner point.

[0032] S24. Redundancy suppression is performed on candidate corner points.

[0033] Considering that there may be some neighboring candidate corner points among the extracted candidate corner points, there may be redundancy among these neighboring candidate corner points. In order to obtain corner points that can accurately reflect the features of the image data and effectively reduce the number of corner points, and ensure the uniformity of feature points with sparse corner points, in an optional implementation, this application embodiment uses a non-maximum suppression method to perform redundancy suppression (filtering) on ​​the candidate corner points.

[0034] The algorithm iterates through each candidate corner point, comparing the corner response values ​​of the currently visited candidate corner point with those of other candidate corner points in its neighborhood. The corner response value can be represented by the sum of the absolute values ​​of the differences in grayscale values ​​between the currently visited pixel and its neighboring pixels. Let p represent the corner response value of pixel p. If the corner response value of the currently traversed candidate corner is not the maximum value in its neighborhood, then the candidate corner is removed, that is, the candidate corner with the largest corner response value in the neighborhood is retained as the final corner.

[0035] By extracting the feature points described above, corner data of the image data is obtained. This corner data contains the location information of the corner points detected in the left and right original images. Each corner point can be represented as a coordinate pair. , where x and y represent the x-coordinate and y-coordinate of the corner point in the image data, respectively. For example, the detected corner point data might be: ,in This represents the number of corner points detected. This corner point data will serve as input for subsequent processing (such as optical flow tracing to remove interfering feature points) to further determine the robot's pose (position and orientation).

[0036] S3. Identify and remove interfering feature points from the extracted feature points to obtain static feature points.

[0037] In this embodiment, interfering feature points are eliminated based on the logic that their motion characteristics deviate from the camera's motion patterns, while the motion patterns of static feature points conform to the camera's motion patterns. In one feasible implementation, in step S3, feature points whose motion characteristics do not conform to the camera's motion patterns are identified and determined to be interfering feature points.

[0038] As an optional implementation, step S3 includes: S31. Calculate the actual position of feature points in the previous image data in the current image data using the optical flow tracing method.

[0039] To facilitate computer processing and improve image processing efficiency, the image data is converted from a color image to a grayscale image before optical flow extraction, thereby simplifying the computational workload of optical flow extraction.

[0040] definition This indicates the current moment within the positioning period. Representing the timestamp of the previous frame, the corner data (i.e., the extracted feature points) at the current moment are represented as follows: Represent the corner data of the previous frame as Each feature point contains pixel coordinates. Alternatively, it may include feature descriptors (such as ORB, SIFT). The feature points in the frame Static feature points detected in the image (detection method is the same as above).

[0041] From frame to frame The camera motion parameters include the rotation matrix R and the translation vector T. These parameters can be estimated using a VO / SLAM system or approximated from static feature points using epipolar geometry, homography, etc. The camera intrinsic parameter matrix K is used to convert the camera's 3D motion into 2D motion in the pixel plane. Additionally, during optical flow extraction, a decision threshold needs to be set to measure the tolerance between the actual and predicted positions of feature points. This threshold can be, for example, a pixel distance threshold, with a value ranging from 3 to 5 pixels.

[0042] Specifically, the optical flow extraction process includes: In frame In the process, each feature point is traversed, and a window is selected centered on each feature point (the window size is pre-set, such as 3 pixels × 3 pixels, 5 pixels × 5 pixels, etc.). Within the frame... The optical flow is determined by searching for the position that best matches the pixel value within the window, thus determining the displacement of the feature point between two frames. Optical flow extraction allows us to determine the actual position of the feature point in the previous image data within the current image data.

[0043] S32. Calculate the predicted position of feature points in the previous image data in the current image data based on camera motion parameters and camera intrinsic parameter matrix.

[0044] Based on the camera motion parameters (rotation matrix R and translation vector T) and the camera intrinsic parameter matrix K, frames are predicted using the camera motion model. Feature points in the frame The position within the frame. Assuming the feature point's 3D coordinates in the camera coordinate system are X, in the frame... The pixel coordinates in According to the camera imaging model, we have: ; in, For frames The depth of the feature points in the camera coordinate system. Let be the transformation matrix.

[0045] When the camera is off frame Motion to Frame At that time, the new coordinates of the feature point in the camera coordinate system are Then, using the camera imaging model, we have: ; Therefore, we can obtain the feature points in the frame. Predicted location (pixel coordinates) in the image. .in, For frames The depth of the feature points in the camera coordinate system.

[0046] S33. Calculate the difference between the predicted position and the actual position of each feature point, and determine the feature points whose difference exceeds the determination threshold as feature points whose motion features do not conform to the camera motion law.

[0047] For frames Each feature point in Its position in the frame is obtained through optical flow tracing. The actual location in , and the predicted location The Euclidean distance d between the two points is calculated. This calculated distance d is then compared to a decision threshold. If the distance d is less than or equal to the decision threshold, the motion of the feature point is considered to conform to the overall motion characteristics (pattern) of the camera, and it is classified as a static feature point and added to the static feature point set. If the distance d is greater than the judgment threshold, the motion of the feature point is considered to deviate from the camera motion characteristics, and it is classified as a dynamic interference feature point and added to the interference feature point set. middle.

[0048] Therefore, two types of feature points can be obtained through optical flow tracing: The first type consists of static feature points that conform to the overall motion characteristics of the camera, which are grouped into a set. middle; Secondly, there are interfering feature points that deviate from the camera's motion characteristics, which are grouped into a set. In this context, these feature points belong to mobile interference factors.

[0049] Deleting a collection This allows us to remove interfering feature points from the extracted feature points, retaining only the set. Static feature points in the data.

[0050] By eliminating interfering feature points and estimating the robot's pose from static feature points, the influence of moving objects, people, and other interfering factors on the robot's localization can be avoided, thereby improving the accuracy of visual localization.

[0051] S4. Based on the robot's pose at the previous moment, estimate the robot's current pose data according to the change in pose of static feature points between the image data at the previous moment and the pose data at the previous moment.

[0052] Based on the robot's initial pose, the degree of pose change of the robot in the current frame relative to the previous frame can be calculated for each subsequent frame. Therefore, for each pair of adjacent image data, the change in pose in the previous frame... Based on the robot's pose, the current frame can be calculated by calculating the pose change. The pose of the robot.

[0053] As an optional implementation, step S4 includes: S41. Match static feature points to obtain matching point pairs.

[0054] For positioning cycle The image data of two adjacent frames collected in the middle, for the current frame And the previous frame The static feature points are matched to obtain pairs of matching points. For example, in frame... Static feature points in Frames are matched using feature matching algorithms (e.g., by calculating similarity distance). Static feature points in Then a set of matching point pairs is obtained. Where i and j represent the sets of feature points in the two frames of image data, respectively. The index in.

[0055] S42. Calculate the essential matrix based on the epipolar geometric relation of matching point pairs.

[0056] After finding matching point pairs, the essential matrix E is calculated using epipolar geometric relations.

[0057] Epipolar geometry describes the intrinsic mapping between two image frames, depending only on the camera's intrinsic parameter matrix K and the relative pose. For matching point pairs... The pixel coordinates are transformed from the camera intrinsic matrix K to the normalized planar coordinate system. According to the principle of epipolar geometry, there exists an essential matrix E that represents the static feature points. Projected onto static feature points The essential matrix E contains rotation and translation information between two frames of image data. The essential matrix E is calculated by applying algorithms such as the eight-point method to each matching point pair.

[0058] S43. Decompose the essential matrix to obtain the rotation matrix and translation vector.

[0059] After obtaining the essential matrix E, the camera is decomposed by alignment to obtain the frame. to frame The rotation matrix R and translation vector T.

[0060] For example, by using methods such as triangulation, combined with depth information in the scene (obtained through the parallax of a binocular camera or other depth estimation methods), the rotation matrix R and translation vector T can be determined from the essential matrix E.

[0061] S44. Determine the robot's current position and attitude information.

[0062] Based on the rotation matrix R and translation vector T, the previous frame... By transforming each pixel, we can obtain the camera (the robot and the camera are synchronized) in the current frame. The pose in the image. The transformation method is the same as before, that is, assuming the camera is in the previous frame. The pixel coordinates in the current frame are X. The pixel coordinates in Then we have: This is how the robot's position in the current frame is obtained. The pose. If only the robot's position change is considered, then only in the previous frame... The position in the current frame can be obtained by adding a translation vector to the original position.

[0063] The above method effectively eliminates the influence of dynamic factors on robot localization in complex and ever-changing environments, ensuring that vision-based localization data accurately reflects static environmental information, enabling robots to achieve reliable localization even in dynamic environments.

[0064] On the other hand, embodiments of this application also provide another robot localization method, which avoids the limitations of localization based on a single sensor by fusing data from multiple sensors, and effectively improves the accuracy and stability of localization.

[0065] like Figure 2 As shown, the robot localization method includes the following steps: S5. Acquire multi-dimensional sensor data of the robot; time alignment of sensor data in each dimension.

[0066] As an optional implementation, the aforementioned multidimensional sensing data includes image data, odometry data, inertial navigation data, and laser point cloud data. Each sensor collects sensing data at multiple timestamps within the same positioning cycle. The sensing data from each dimension are temporally aligned, meaning that robot positioning at that moment is achieved using sensing data from each dimension at the same time (or near that time point).

[0067] S6. Estimate the robot's pose data based on the sensor data of each dimension.

[0068] Taking image data, odometry data, inertial navigation data, and laser point cloud data as examples, such as Figure 3 As shown, step S6 includes: S61. Estimate the robot's pose data based on image data.

[0069] For image data, the robot localization method described in the above embodiments calculates the robot's pose in the current frame based on the acquired image data. For ease of explanation, the pose calculated based on the image data is referred to as the first pose data.

[0070] S62. Estimating robot pose data based on inertial navigation data.

[0071] Inertial navigation data is acquired by an inertial measurement unit (IMU). The IMU acquires the robot's acceleration and angular velocity in real time. Using pre-integration, the robot's position and angle at each timestamp can be calculated. The pose calculated based on the inertial navigation data is called the second pose data.

[0072] For example, suppose in During the positioning cycle, the IMU samples 200 sets of inertial navigation data, each set containing acceleration and angular velocity data at the corresponding moment. Since the IMU sampling frequency is higher than the camera's shooting frequency, when aligning the inertial navigation data with the image data, the timestamp of the image data is used as a reference, and the inertial navigation data closest in time to that timestamp is searched as the time-aligned inertial navigation data.

[0073] For each pair of adjacent image data, the inertial navigation data aligned with it is pre-integrated to obtain the displacement and attitude changes of the robot within those two adjacent frames.

[0074] Pre-integration methods for inertial navigation data include: S621. Initialize variables.

[0075] Set initial displacement Initial rotation quaternion (Unit quaternion), initial velocity These variables will be continuously updated during the integration process.

[0076] S622, Position and attitude update.

[0077] The inertial navigation data acquired by the IMU is discrete. Pre-integration uses a discrete-time integration method, applying it to every two inertial navigation data points (with a defined sampling time interval of 1). Integrate (where z is the total number of sampled inertial navigation data points and i is the index of the inertial navigation data point): Rotation Update: Update the rotation quaternion for each inertial navigation data point i based on the angular velocity. ; Velocity update: Update the velocity for each inertial navigation data point i based on the acceleration and the updated rotation quaternion. ; Displacement update: Update the displacement of each inertial navigation data point i according to the updated velocity. .

[0078] Based on the total inertial navigation data sampled between two adjacent frames, each time interval is processed according to the method described above. The calculation is repeated, continuously updating the rotation quaternion, velocity, and displacement, until all inertial navigation data for the entire positioning cycle has been processed.

[0079] The final updated rotation quaternion represents the change in the robot's pose information, and the displacement represents the change in the robot's position information. Based on the robot's position and pose in the previous frame, updates are made according to the changes in pose and position information to obtain the robot's position and pose information in the current frame.

[0080] S63. Estimating robot pose data based on odometry data.

[0081] Odometry data is collected by sensors such as wheeled odometry. Wheeled odometry directly collects the distance data moved by the robot, and the displacement and direction of the robot's movement can be deduced from the distance data. The pose calculated based on odometry data is called third pose data.

[0082] As an optional implementation, step S63 includes: S631, sampling odometer data.

[0083] Wheel odometer During the localization cycle, odometry data is sampled at multiple timestamps, with each timestamp including the robot's acceleration and angular velocity. The alignment method between the odometry data and the image data is the same as that for the inertial navigation data.

[0084] Specifically, the data collected by the wheeled odometer is in the format of [sampling timestamp, left wheel pulse increment, right wheel pulse increment]. Known robot hardware parameters include: wheel diameter d and wheelbase L between the left and right wheels. These hardware parameters are used to convert the pulse increments into actual distance and angle data of the motion.

[0085] S632, Calculate the distance the wheels travel.

[0086] The distance traveled by the left and right wheels is calculated based on the pulse increment of the left and right wheels and the wheel diameter.

[0087] S633, Calculate the robot's linear velocity and angular velocity.

[0088] Calculate the robot's linear velocity and angular velocity based on the distance traveled by the left wheel, the distance traveled by the right wheel, and the wheelbase: Linear velocity calculation: The robot's linear velocity v can be calculated by the average distance traveled by the left and right wheels and the sampling time interval. Let's calculate it. Assume the timestamps of the two samples are t1 and t2, Then linear velocity .

[0089] Angular velocity calculation: the robot's angular velocity It is related to the difference in the distance traveled by the left and right wheels and the wheelbase, and the calculation formula is as follows: For example, if the wheelbase L = 0.5m, the distance the left wheel travels... Distance traveled by the right wheel Then angular velocity =(12 - 10) / 0.5×1000 = 0.004rad / s.

[0090] S634, Calculate the robot's orientation and position.

[0091] The robot's position information is obtained by integrating the angular velocity and linear velocity.

[0092] Specifically, assume the robot moves in a two-dimensional plane with an initial position of (x0, y0) and an initial direction of... In each sampling time interval Inside, the position and orientation are updated based on the linear velocity v and angular velocity w: direction The update formula is: ; Location The update formulas are as follows: ; ; in, These represent the directions at timestamps t and t-1, respectively. For the robot's position at timestamp t-1, Let t be the position of the robot at timestamp t.

[0093] Starting from the beginning timestamp t1 of the positioning cycle, following the method described above, linear velocity and angular velocity are calculated sequentially based on the odometry data corresponding to each timestamp, and then the direction and position are updated. Through continuous iterative calculations, the position data corresponding to each timestamp in the positioning cycle is obtained. For example, given n timestamps within a positioning period, performing the above calculations sequentially on each timestamp yields the position sequence (x1, y1), (x2, y2), ..., (x...). n ,y nThese are the location data for each timestamp, used to describe the robot's position at different times within the localization cycle. The direction sequence of each timestamp is represented as... This is used to describe the robot's orientation at different times during the positioning cycle.

[0094] S64. Estimating robot pose data based on laser point cloud data.

[0095] Laser point cloud data is acquired by LiDAR. Similar to robot localization using image data, laser point cloud data estimates the robot's pose in the current frame by matching feature points (point clouds) between two consecutive frames and calculating the rotation matrix R and translation vector T. The pose calculated based on laser point cloud data is called fourth pose data.

[0096] As an optional implementation, step S64 includes: S641, Collect laser point cloud data.

[0097] LiDAR in During the positioning cycle, laser point cloud data is sampled at multiple timestamps. Each timestamp scans one frame of laser point cloud data. Typically, adjacent frames of laser point cloud data will partially overlap. The alignment method between the laser point cloud data and the image data is the same as that for inertial navigation data.

[0098] S642. Calculate the position and attitude changes between two adjacent frames.

[0099] By iteratively calculating the position and attitude changes between two adjacent frames, the laser point cloud data of the two frames are eventually made to completely overlap. The resulting position and attitude changes represent the robot's position and attitude changes between the two adjacent frames of laser point cloud data. By integrating the position and attitude changes based on the robot's position and attitude in the previous frame, the current position and attitude of the robot can be obtained.

[0100] Specifically, methods for calculating robot position and orientation include: (1) Matching laser point cloud data.

[0101] For the previous frame of laser point cloud data Each point in In the current frame of laser point cloud data Find the point closest to it. As corresponding points. Euclidean distance is typically used to measure the distance between points. and Distance between .Right now: ; In the formula, For point The three-axis coordinates, For point The three-axis coordinates. Traverse the previous frame of laser point cloud data. Each point in By calculating the laser point cloud data of the current frame Each point in (with) (represents) and the points traversed Calculate the Euclidean distance between them, and select the point with the smallest distance. As a point Paired points.

[0102] (2) Calculate the transformation matrix.

[0103] Based on the found matching points The calculation can Transformation of points in to The transformation matrix Tr is defined to make corresponding points in the matrix coincide as much as possible. Tr contains the rotation matrix R1 and the translation vector T1. The transformation matrix Tr can be solved by minimizing the sum of squared errors between corresponding points. Specifically, the objective function is defined as follows: as follows: ; In the formula, N represents the number of matching point pairs. This can be achieved through methods such as singular value decomposition. (3) Update the laser point cloud data.

[0104] Using the transformation matrix Tr to transform the laser point cloud data of the previous frame Perform the transformation to obtain .

[0105] (4) Determine whether the updated (i.e. transformed) laser point cloud data meets the convergence condition. If not, repeat the above steps (1)-(3) until the convergence condition is met.

[0106] The convergence condition can be designed as follows: the average distance between the updated laser point cloud data and the matching points is less than a set threshold; or the change in the transformation matrix Tr in consecutive iterations is less than a set threshold. If the updated... If the convergence condition is not met, the steps of searching for matching points, updating the transformation matrix, and updating the laser point cloud data are repeated until the updated laser point cloud data meets the convergence condition. The final calculated transformation matrix Tr reflects the changes in the robot's position and attitude between two adjacent frames.

[0107] S643. Calculate the robot's pose data based on the changes in position and attitude.

[0108] Assume the robot's pose in frame t-1 is ,in Let represent the robot's pose and position in frame t-1, respectively. Then, the robot's pose in frame t is... The calculation method is as follows: Attitude in frame t: ; Position in frame t: .

[0109] During the positioning cycle In this process, at least two keyframes can be set. Each keyframe calculates the robot's pose once using sensor data from each dimension. In this way, at least two positions and orientations of the robot can be obtained in one positioning cycle.

[0110] S7. Construct a factor graph based on each pose data, and use the factor graph optimization method to fuse the pose data to obtain the robot's final pose.

[0111] Based on the calculation of the pose data corresponding to the sensor data in each dimension, such as Figure 4 As shown, in one optional implementation, the method of fusing pose data using a factor graph optimization method includes: S711. Establish factor nodes using robot pose as a factor, and construct a factor graph based on each pose data.

[0112] (1) Define factor nodes.

[0113] In step S711, a factor node (i.e., a pose node) is defined for each timestamp. Assume there are M timestamps within the positioning period: For each timestamp ti, create a factor node. , It contains location information (i.e., three-dimensional coordinates) at timestamp ti. The robot's pose information (represented by rotation quaternions) and the pose information. Factor nodes represent the robot's pose that needs to be estimated.

[0114] (2) Define the factor.

[0115] Taking the sensor data containing four dimensions as an example, a total of four pose data are obtained, so four factors can be defined.

[0116] 1) Visual Factor. This factor is constructed based on the first pose data obtained from a stereo depth camera at different time stamps. For example, the pose change calculated from the static feature point matching relationship detected by the stereo depth camera between time stamps ti and ti+1 can be used to define a factor. This is the visual factor. This factor describes the constraint relationship of pose between two timestamps. It is based on the uncertainty of binocular vision measurement and is constructed by considering factors such as feature point matching error, so that the pose change that satisfies the constraint is more consistent with binocular vision observation.

[0117] 2) Inertial Navigation Factor. This factor is constructed based on the second pose data obtained from the IMU at different time stamps. Between time stamps ti and ti+1, the relative displacement and rotation changes obtained from the pre-integration of the inertial navigation data can be used to define a factor. This is the inertial navigation factor. This factor reflects the pose change constraint of the IMU measurement, takes into account IMU measurement noise, and ensures that the estimated pose change is consistent with the pre-integration result of the inertial navigation data.

[0118] 3) Odometer Factor. This factor is constructed based on the third pose data obtained from the wheeled odometer at different timestamps. The factor defines the changes in position and orientation measured by the wheeled odometer between timestamps ti and ti+1. This is the odometry factor. This factor reflects the constraint of the wheeled odometry on the robot's pose changes and is constructed based on the error characteristics of odometry measurements.

[0119] 4) Point Cloud Factor. This factor is constructed based on the fourth pose data obtained from the LiDAR at different timestamps. Between timestamps ti and ti+1, the rotation and translation changes calculated by LiDAR point cloud matching define the factor. This refers to the point cloud factor. This factor reflects the pose constraints of the lidar measurement and takes into account the uncertainties of the lidar measurement and the point cloud matching error.

[0120] A factor graph can be constructed using the factors and factor nodes defined above.

[0121] S721. Construct multi-dimensional error functions based on the difference between the pose change measured by the sensor data of each dimension in the factor graph and the actual pose change.

[0122] With visual factors For example, suppose the pose change from timestamp ti to timestamp ti+1 is measured using image data. And the current factor node and The actual pose change between them is Then the error function of the visual factor dimension between the two factor nodes Similarly, for other factors... , and Define the corresponding error function: Inertial Navigation Factor Dimension Error Function Odometry factor dimension error function Point cloud factor dimension error function .

[0123] S731. Construct a global optimization objective function based on the error functions of each dimension. The global optimization objective function aims to minimize the sum of squares of all factor errors.

[0124] By combining the error functions of all factors, a global optimization objective function is constructed. For example, a global optimization objective function can be constructed using the least squares form, with the objective of minimizing the sum of squares of the errors of all factors.

[0125] ; In the formula, , , , The information matrices, representing the errors of the visual factor, inertial navigation factor, odometry factor, and point cloud factor, are used to weigh the importance of different factors. In other words, in the global optimization objective function, the error of each factor at each factor node is weighted by the information matrix of the corresponding factor at that factor node. The information matrix uses a norm to weight the errors. The information matrix represents the measurement accuracy of the corresponding sensor; that is, the error of each factor at each factor node is weighted by the measurement accuracy of the corresponding factor at that factor node. The higher the measurement accuracy, the greater the weight of the information matrix, and the greater its influence in the optimization process of the global optimization objective function. Clearly, based on the ideas of the embodiments of this application, the robot localization method of this application can be extended to support more types of sensors.

[0126] S741. Iteratively update the robot's factor nodes according to the global optimization objective function until the cutoff condition is met.

[0127] As an alternative implementation, nonlinear optimization algorithms (such as the Gauss-Newton method, the Levenberg-Marquardt method, etc.) are used to solve the aforementioned global optimization objective function. These nonlinear optimization algorithms iteratively update the values ​​of the pose nodes, gradually decreasing the value of the objective function until it converges to a local optimum.

[0128] In each iteration, the optimization algorithm calculates the error function and its Jacobian matrix based on the current value of the factor node, and then updates the factor nodes according to the optimization algorithm's strategy. For example, the Gauss-Newton method solves a system of linear equations. To update pose increment Where J is the Jacobian matrix of the error function, It is the diagonal block matrix of the information matrix, and e is the error vector. Updated factor nodes. for , This is the factor node before the update. Repeat this process until the value of the global optimization objective function converges to a sufficiently small value (preset threshold), or until the preset maximum number of iterations is reached, i.e., the cutoff condition is met.

[0129] S751, the factor node updated when the cutoff condition is met is used as the robot's final pose.

[0130] When the optimization algorithm converges, the factor nodes are obtained. This refers to the final pose data obtained by fusing pose data from various dimensions across different time stamps. This pose data integrates information from four types of sensors, leveraging the strengths of each sensor to improve the accuracy and reliability of localization. For example, for a specific time stamp tj, factor nodes... The position in the middle and posture It refers to the robot's final pose data at that moment.

[0131] In another alternative implementation, such as Figure 5 As shown, the methods for fusing pose data using factor graph optimization include: S712, fusion of odometer data and laser point cloud data to construct laser SLAM.

[0132] By utilizing laser point cloud data and odometry data, a scan-matching algorithm (such as the ICP-Iterative ClosestPoint algorithm and its variants) is used to match the current laser point cloud data with a previously constructed map based on odometry data to determine the robot's position and pose within the map. During this process, the map is continuously updated, and the robot's pose estimation results are optimized based on the new laser point cloud data. Through continuous scan-matching and map updates, a laser SLAM system based on the fusion of LiDAR and wheeled odometry is constructed, providing foundational data for subsequent factor map optimization.

[0133] S722. Calculate the loop closure constraints of laser SLAM based on the map continuously built by laser SLAM.

[0134] As the robot moves, a map is continuously built based on laser SLAM, and loop closure detection is performed in real time to obtain the loop closure constraints of laser SLAM.

[0135] Specifically, as the robot moves, laser SLAM continuously builds a map. During this process, loop closure detection is used to identify whether the robot has returned to a previously visited area. Common loop closure detection methods include appearance-based methods (such as bag-of-words models) and geometry-based methods (such as point cloud matching). When a loop is detected, it means that the robot's current position is spatially consistent with a previous position, thus forming a loop constraint. Optimizing the factor map based on loop constraint conditions can effectively reduce accumulated errors, thereby improving localization accuracy.

[0136] S732. Establish factor nodes using robot pose as a factor, and construct the first factor graph based on the third pose data and the fourth pose data.

[0137] S742. Optimize the first factor map through closure constraints to obtain optimized globally consistent pose data; use the difference between the globally consistent pose data before and after optimization as the change in the first transformation relationship between the map coordinate system and the odometer coordinate system.

[0138] The first factor graph is optimized using closure constraints to obtain optimized globally consistent pose data. Then, the difference between the optimized and unoptimized globally consistent pose data is calculated and used as the change from map (map coordinate system) to odom (odometry coordinate system), and finally published in TF (Transform) format to obtain the optimized change. The above difference represents the change in the transformation relationship from the map coordinate system to the odom coordinate system.

[0139] S752. Construct a second factor map based on the first, second, and third pose data. Construct multi-dimensional error functions based on the differences between the pose changes measured from sensor data in each dimension of the second factor map and the actual pose changes. Construct a global optimization objective function based on the error functions of each dimension. This global optimization objective function aims to minimize the sum of squares of all factor errors. Iteratively update the robot's factor nodes according to this global optimization objective function until a cutoff condition is met. Use the updated factor nodes when the cutoff condition is met as the robot's initial pose to obtain the change in the second transformation relationship between the odometry coordinate system and the robot's base coordinate system.

[0140] A first factor map is constructed using odometry factors and point cloud factors, and a second factor map is constructed using visual factors, inertial navigation factors, and odometry factors. The methods for constructing the first and second factor maps are the same as those for constructing factor maps in the previous embodiments. Except for the different factors, the rest of the process is the same, so it will not be repeated here.

[0141] The second transformation relationship describes the change from odom (odometry coordinate system) to base_link (robot base coordinate system), that is, the transformation relationship from the odometry (odom) coordinate system to the robot base coordinate system (base_link).

[0142] The factor graph optimization method in step S752 is the same as the method in steps S721-S751 in the previous embodiment, except for the number of factors being optimized (i.e., omitting point cloud factors). Therefore, it will not be described in detail here.

[0143] S762. Combining the changes in the first and second transformation relationships, the changes in the third transformation relationship between the map coordinate system and the robot base coordinate system are obtained, which is the robot's final pose.

[0144] The transformation from map->odom (i.e., the optimized transformation) given by laser SLAM is combined with the transformation from odom->base_link (i.e., the initial pose data) obtained through optimization of the factor map fusion from vision, IMU, and wheel odometry. Based on the principle of coordinate transformation combination, multiplying these two transformations by matrix multiplication yields the transformation from map->base_link. This transformation from map->base_link accurately describes the robot's position and orientation in the map coordinate system, and is the robot's final pose.

[0145] Based on the concept of this application, an embodiment of this application also proposes a robot positioning device, such as... Figure 6 As shown, it includes a first processor and a first storage medium. The first storage medium stores first computer instructions. The first processor runs the first computer instructions to execute the first robot localization method in the above embodiments.

[0146] This application also provides another robot positioning device, such as... Figure 7 As shown, it includes a second processor and a second storage medium, the second storage medium storing second computer instructions, the second processor running the second computer instructions to execute the second robot positioning method in the above embodiments.

[0147] It should be noted that the terms "first processor," "second processor," "first storage medium," and "second storage medium" used here are merely for distinguishing between similar objects and do not constitute any special restriction on the specific selection of the corresponding objects.

[0148] In summary, the robot localization method provided in this application has the following characteristics: (1) High-precision positioning: By integrating data from four sensors—a binocular depth camera, an IMU, a wheeled odometer, and a single-line lidar—the advantages of each sensor are fully utilized, and their shortcomings are compensated for, which effectively improves the positioning accuracy of the robot in a variable environment and can meet the positioning accuracy requirements of various tasks.

[0149] (2) Enhanced stability: Multi-sensor data fusion makes the positioning system more robust to environmental changes and interference. For example, when the visual sensor is affected by lighting, the IMU and odometer data can continue to provide positioning support; when the odometer wheel slips, the lidar and visual data can correct the positioning deviation, thereby ensuring the stability of the positioning results.

[0150] (3) Effectively cope with dynamic environment: The optical flow tracing method is used to remove the feature points of dynamic objects in the binocular depth camera image, ensuring that the vision-based positioning data accurately reflects the static environment information, so that the robot can achieve accurate and reliable positioning in dynamic environment.

[0151] (4) Data collaborative utilization: Through processing methods such as timestamp alignment and pre-integration, the effective fusion of sensor data with different sampling frequencies and data types in the same time dimension is realized, fully exploring the value of each sensor data and improving the overall positioning performance.

[0152] This invention is not limited to the specific embodiments described above. The invention extends to any new feature or combination disclosed in this specification, as well as any new method or process step or combination disclosed herein.

Claims

1. A robot localization method, characterized in that, include: Acquire multidimensional sensor data from the robot, including image data, odometry data, inertial navigation data, and laser point cloud data; time-align the sensor data of each dimension; The robot's pose data is estimated based on sensor data for each dimension. Specifically, the robot's first pose data is calculated based on the image data using the following method: acquiring the robot's image data; extracting feature points from the image data; identifying and removing interfering feature points from the extracted feature points to obtain static feature points; estimating the robot's current pose data based on the pose change of the static feature points between the robot's previous pose and the previous image data; calculating the robot's second pose data based on inertial navigation data; calculating the robot's third pose data based on odometry data; and calculating the robot's fourth pose data based on laser point cloud data. A factor graph is constructed based on the pose data of each position, and the factor graph optimization method is used to fuse the pose data of each position to obtain the robot's final pose, including: Laser SLAM is constructed by fusing the odometry data and the laser point cloud data; The loop closure constraints of the laser SLAM are calculated based on the map continuously built by the laser SLAM. Factor nodes are established using robot pose as a factor. A first factor graph is constructed based on the third pose data and the fourth pose data. A second factor graph is constructed based on the first pose data, the second pose data, and the third pose data. The first factor map is optimized by the lapsing constraint to obtain optimized global consistent pose data; the difference between the global consistent pose data before and after optimization is used as the change in the first transformation relationship between the map coordinate system and the odometer coordinate system. A multi-dimensional error function is constructed based on the difference between the pose change measured by the sensor data of each dimension in the second factor graph and the actual pose change. A global optimization objective function is constructed based on the error functions of each dimension, with the objective function being to minimize the sum of squares of all factor errors. The robot's factor nodes are iteratively updated according to the global optimization objective function until a cutoff condition is reached. The updated factor nodes when the cutoff condition is reached are used as the robot's initial pose, and the change in the second transformation relationship between the odometry coordinate system and the robot's base coordinate system is obtained. Combining the changes in the first and second transformation relationships, the change in the third transformation relationship between the map coordinate system and the robot base coordinate system is obtained, which is the robot's final pose.

2. The robot localization method as described in claim 1, characterized in that, Methods for identifying interfering feature points among the extracted feature points include: Among the extracted feature points, those whose motion characteristics do not conform to the camera's motion patterns are identified as interfering feature points.

3. The robot localization method as described in claim 2, characterized in that, Among the extracted feature points, those whose motion characteristics do not conform to the camera's motion characteristics include: The actual position of feature points in the previous image data is calculated in the current image data using optical flow tracing. Calculate the predicted position of feature points in the previous image data in the current image data based on camera motion parameters and camera intrinsic parameter matrix; Calculate the difference between the predicted position and the actual position of each feature point, and determine the feature points whose difference exceeds the determination threshold as feature points whose motion characteristics do not conform to the camera motion law.

4. A robot positioning device, characterized in that, It includes a second processor and a second storage medium, the second storage medium storing second computer instructions, the second processor executing the second computer instructions to perform the robot localization method as described in any one of claims 1-3.

Images