Indoor service robot repositioning method based on fusion laser and vision

Abstract

The invention discloses an indoor service robot repositioning method based on fusion of laser and vision, and the method comprises the steps: enabling a robot to carry out the positioning and mapping of an indoor environment, constructing a feature point map through visual information, and obtaining a visual mapping track; constructing a grid map through the laser radar point cloud information, and obtaining a laser radar mapping track; processing a laser radar mapping track by adopting a linear interpolation method, and calculating a pose corresponding to the camera key frame sequence; key frames of laser radar mapping and visual mapping are aligned, and robot pose relation mapping between a grid map and a feature point map is established; based on pose mapping of the visual mapping track and the laser radar mapping track, coarse positioning of the mobile robot in a grid map is established, and coarse positioning pose information is obtained; and performing fusion positioning on the coarse positioning pose information serving as prior information, laser radar point cloud information and odometer data.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor robot positioning methods, and particularly to a method for relocating an indoor service robot based on the fusion of laser and vision. Background Art

[0002] With the rapid development of artificial intelligence and automation technologies, indoor service robots have gradually become important tools in daily life and are widely used in various fields such as home cleaning, security patrol, and health monitoring. To achieve efficient and safe autonomous navigation, precise positioning and path planning technologies are the key for indoor service robots to complete tasks independently. However, in complex indoor environments, robots face various challenges, such as large environmental changes, incomplete map information, and frequent movement of obstacles, which seriously affect the positioning accuracy of the robots.

[0003] Currently, the positioning technologies of indoor service robots are mainly based on the following methods: lidar SLAM (Simultaneous Localization and Mapping), visual SLAM, and inertial navigation system (INS). Among them, lidar-based positioning technologies, such as Cartographer, have been widely used in indoor robots. Through lidar sensors, robots can achieve precise mapping of the environment and self-positioning. Cartographer generates a 2D map through lidar echo data and performs map-based positioning through the AMCL (Adaptive Monte Carlo Localization) algorithm. However, although lidar positioning performs excellently in some static environments, its positioning accuracy has certain limitations in dynamic environments and complex scenarios. Especially when the environment changes, or when the robot enters an area with a repetitive structure or occlusions, the lidar positioning system is prone to positioning errors, resulting in positioning drift or failure. To solve these problems, vision sensors (such as cameras) have begun to be applied to robot positioning. Visual SLAM technology obtains environmental images through cameras and uses image feature points for positioning and map construction. Compared with lidar, vision sensors have obvious advantages in detail capture and performance in complex environments. However, traditional vision-based positioning methods also face problems such as large computational amounts and poor real-time performance, especially in dynamic and highly variable lighting environments. Although deep learning-based visual SLAM methods have improved in accuracy, due to the complexity of deep learning models and high computational requirements, it is difficult to perform real-time deployment in resource-constrained environments, especially for application scenarios with high requirements for real-time positioning accuracy, and there are still deficiencies.

[0004] For lidar-based localization, Sun Guoxiang et al. first used the Extended Kalman Filter (EKF) for lidar data filtering and the Particle Filter (PF) for indoor environment localization and mapping. Their method achieved relatively stable localization results in relatively simple environments. However, due to the high computational complexity of the particle filter, it faces significant computational pressure in large-scale environments, resulting in poor real-time performance. Wang Chao et al. proposed a lidar data matching method based on the Genetic Algorithm (GA), which improved the localization accuracy by optimizing the data matching process. However, the genetic algorithm requires a large amount of computation and iteration, with a long computation time, leading to poor real-time performance, especially in dynamic environments where it is difficult to maintain high efficiency.

[0005] For vision-based methods, Li Tao et al. used SIFT feature extraction and combined it with the RANSAC algorithm to match the extracted features, and achieved indoor localization by calculating the camera's motion trajectory. This method can achieve good localization accuracy in relatively simple indoor environments, but in scenarios with large lighting changes, the SIFT features will be greatly affected and the accuracy is unstable. Shang Ren et al. further used SURF feature extraction and combined it with Bundle Adjustment to optimize the localization accuracy, achieving good results. However, these traditional methods have high requirements for computational volume and time, especially for real-time localization tasks, the algorithm processing speed is slow and cannot meet the requirements of high-dynamic change scenarios.

[0006] In practical applications, when relying solely on lidar for localization, due to dynamic changes in the environment and the presence of complex obstacles, the localization accuracy is prone to drift or misalignment. In contrast, although vision sensors can provide rich environmental information, they are affected by lighting changes and the field of view, easily leading to localization errors and decreased accuracy.

[0007] To improve the localization accuracy of indoor service robots, a data fusion technology based on lidar and vision sensors was selected. However, due to the high computational complexity between the lidar and the vision system and the need for a large amount of computing resources, the running speed of the localization algorithm is slow, and thus cannot meet the real-time requirements.

[0008] In a multi-sensor fusion SLAM system, how to effectively synchronize data from different types of sensors is a key technical challenge. Different sensors (such as lidar, visual cameras, IMUs, etc.) often have different sampling frequencies and timestamps, which makes it complex to precisely match and fuse their data. Data delays, noise, and synchronization errors between sensors may cause deviations in the fused pose estimation, thereby affecting the positioning accuracy of the entire system. Especially in high-dynamic environments, differences in the response speed and data processing capabilities of sensors are more likely to trigger time synchronization problems, which directly affect the stability of real-time positioning and map construction. During the sensor fusion process, how to handle the different scales and characteristics of each sensor's data is also a challenge. For example, lidar and visual sensors differ in data representation, perception range, and accuracy. How to balance these differences and perform effective fusion is the key to ensuring the efficient operation of the system. The fusion algorithm not only has to solve how to efficiently synchronize data but also consider how to weight information between different sensors to ensure that the final pose estimation can accurately reflect the actual situation of the environment. Therefore, how to solve these problems of error and real-time performance is a research focus and of great significance. Summary of the Invention

[0009] In view of the above-mentioned disadvantages of the prior art, the purpose of the present invention is to provide an indoor service robot relocalization method based on the fusion of laser and vision, which is used to solve technical problems such as how to fuse data from different types of sensors and improve the real-time performance and accuracy of the robot in an environment with dynamic changes and complex obstacles.

[0010] To achieve the above purpose, the present invention provides an indoor service robot relocalization method based on the fusion of laser and vision, including:

[0011] S1 The robot locates and maps the indoor environment through a camera and a lidar. Among them, a feature point map is constructed through visual information and a visual mapping trajectory is obtained.

[0012] S2 A grid map is constructed through lidar point cloud information and a lidar mapping trajectory is obtained.

[0013] S3 The linear interpolation method is used to process the lidar mapping trajectory, and the poses corresponding to the key frame sequence of the camera in this trajectory are calculated.

[0014] S4 A least squares problem is constructed. By optimizing the coordinate transformation relationship between the feature point map and the grid map, the sum of the squares of the pose observation residuals in the two mapping trajectories is minimized, aligning the key frames of the lidar mapping and the visual mapping, and establishing a mapping of the robot pose relationship between the grid map and the feature point map.

[0015] S5 Based on the pose mapping of the visual mapping trajectory and the lidar mapping trajectory, establish the rough localization of the mobile robot in the grid map to obtain the rough localization pose information;

[0016] S6 Use the rough localization pose information as prior information to fuse and localize with the lidar point cloud information and the odometer data.

[0017] The beneficial effects of the present invention are as follows: Compared with the pure lidar algorithm for localization, the present invention improves the accuracy, reduces the relocalization time and the localization error. Specifically, the pure lidar localization method infers the current position by matching the currently scanned point cloud with the existing grid map. However, in a dynamic environment or a complex scene, it is prone to localization drift or failure. While constructing the feature point map and the visual mapping trajectory through visual information, the present invention uses the lidar to construct the grid map and the lidar mapping trajectory, and then obtains the rough localization pose of the robot through pose mapping, and uses it as prior information for relocalization. Because there is prior information, that is, the general position information of the robot is obtained in advance, and the prior information is passed to the lidar localization algorithm. The lidar localization algorithm allocates more laser scanning particle points near the current position of the robot according to the prior information. The particle points match the grid map faster and more accurately, making the localization more accurate, that is, compared with the pure lidar localization algorithm, the robot can find the current position faster and more accurately. Brief Description of the Drawings

[0018] Figure 1 It is a flowchart of the indoor service robot relocalization method based on the fusion of lidar and vision in the embodiment of the present invention;

[0019] Figure 2 It is a flowchart of constructing the feature point map by visual information and obtaining the visual mapping trajectory in the embodiment of the present invention;

[0020] Figure 3 It is a schematic diagram of the Cartographer kidnapped relocalization experiment in the embodiment of the present invention;

[0021] Figure 4 It is a broken line graph of the relocalization error based on Cartographer in the embodiment of the present invention;

[0022] Figure 5 It is a broken line graph of the relocalization error in the embodiment of the present invention. Detailed Embodiments

[0023] The following describes the embodiments of the present invention through specific examples. Those skilled in the art can easily understand the other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments. Various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention.

[0024] In this embodiment, a self-made chassis of a life support robot and a laptop computer equipped with Ubuntu 20.04 are selected for control, and a single-line lidar and a RealSense d435i depth camera are configured. The lidar provides lidar point cloud information, and the camera provides visual information.

[0025] Based on the above robot, an indoor service robot relocalization method based on the fusion of lidar and vision is provided by way of example, which is basically as Figure 1 shown, and the specific steps are as follows:

[0026] S1 The robot locates and maps the indoor environment through the camera and lidar. Among them, a feature point map is constructed through visual information and a visual mapping trajectory is obtained;

[0027] S2 A grid map is constructed through lidar point cloud information and a lidar mapping trajectory is obtained;

[0028] S3 The linear interpolation method is used to process the lidar mapping trajectory, and the poses corresponding to the key frame sequence of the camera in the trajectory are calculated;

[0029] S4 A least squares problem is constructed. By optimizing the coordinate transformation relationship between the feature point map and the grid map, the sum of the squares of the pose observation residuals in the two mapping trajectories is minimized, the key frames of lidar mapping and visual mapping are aligned, and the robot pose relationship mapping between the grid map and the feature point map is established;

[0030] S5 Based on the pose mapping of the visual mapping trajectory and the lidar mapping trajectory, a rough localization of the mobile robot in the grid map is established to obtain rough localization pose information;

[0031] S6 The rough localization pose information is used as prior information to fuse and localize with the lidar point cloud information and odometer data.

[0032] Among them, a feature point map is constructed through the visual information and a visual mapping trajectory is obtained on the premise that the visual localization is accurate. As Figure 2As shown in the figure, in order to achieve accurate visual positioning, it is first necessary to calibrate the camera to obtain the internal parameters of the camera (including focal length, principal point coordinates, and distortion parameters). In this embodiment, Zhang's Calibration Method is used to calibrate the camera, and the internal parameter matrix K of the camera is obtained:

[0033]

[0034] In the formula: f x is the focal length in the horizontal direction; f y is the focal length in the vertical direction, and (c x , c y ) are the principal point coordinates.

[0035] The camera is used to obtain images, the features in the images are extracted, and based on ORB-SLAM3, ORB features are used to detect and describe the key points in the images. ORB features have rotation invariance and scale invariance, and can stably detect and describe image features under different viewpoints and illumination conditions.

[0036] Among them, for the detection of key points, the FAST (Features from Accelerated Segment Test) algorithm is used to detect corner points in the image as key points. Specifically, the FAST algorithm detects corner points by comparing the brightness values around the pixels. Then, the directions of the key points are assigned. Specifically, by calculating the gradient direction of the area around the key points, each key point is given a main direction to ensure the rotation invariance of the feature descriptor. The calculation formula for the main direction θ is:

[0037]

[0038] In this formula, i is the pixel index, representing the pixel point number in the neighborhood around the key point; w i is the weight; (x i , y i ) are the positions of the pixels in the neighborhood; ΔI i is the gradient change; x p is the horizontal coordinate of the key point; y p is the vertical coordinate of the key point.

[0039] For the feature description of the key points, the BRIEF (Binary Robust Independent Elementary Features) descriptor is used, combined with the main direction of the key points, to generate a rotation-invariant binary descriptor. The generation process of the ORB descriptor is as follows:

[0040] For each key point p, a set of predetermined pixel pairs {(p j, p k )}, the m-th bit of the ORB descriptor d p is defined as:

[0041]

[0042] where I(p j ) and I(p k ) are the grayscale values of the pixel pair (p j , p k ) respectively.

[0043] After feature extraction, key-point image feature matching is performed. First is descriptor matching. By calculating the Hamming Distance between ORB descriptors, the ORB descriptors between different frames are matched to filter out potential matching pairs. Among them, the Hamming distance D H is calculated as follows:

[0044]

[0045] where D H (d pi , d pj ) represents the Hamming distance between the ORB descriptor d pi and the ORB descriptor d pj , d pi is the descriptor of the key point in the i-th frame, d pj is the descriptor of the key point in the j-th frame, M is the length of the descriptor, represents the exclusive OR operation, m is the m-th bit in the ORB descriptor, d pi (m) is the binary value of the ORB descriptor d pi at the m-th bit, d pj (m) is the binary value of the ORB descriptor d pj at the m-th bit.

[0046] Then, key-point feature matching optimization is performed. Specifically, a two-way matching strategy is applied, that is, matching from frame A to frame B and from frame B to frame A, retaining the consistent matching pairs and removing the incorrect matching pairs. For each matching pair (p A , p B ), ensure that p A has the descriptor closest to p B in frame A, and p B has the descriptor closest to p A in frame B. p A represents the key point at frame A, and p B represents the key point at frame B.

[0047] Finally, geometric verification is performed. Specifically, the RANSAC (Random Sample Consensus) algorithm is used to eliminate incorrect matching pairs, and the fundamental matrix or homography matrix is calculated to ensure that the matching points satisfy geometric constraints.

[0048] After the key-point feature matching, the camera pose is calculated. In ORB-SLAM3, one of the key steps in camera pose estimation is to solve the PnP (Perspective-n-Point) problem, that is, to estimate the camera pose (position and orientation) through the 3D map points obtained from mapping and the corresponding 2D image points. Take the 3D map point P of the current frame key points from the mapping process i =(X i , Y i , Z i ) T and its corresponding 2D image point p i =(u i , v i ) T , and solve for the camera's rotation matrix R i and translation vector t i to minimize the projection error, and the expression is:

[0049]

[0050] In the formula, N is the number of key frames; i represents the i-th frame currently being calculated, that is, the current frame; π is the camera projection function usually determined by the camera intrinsic matrix K, and the specific expression is as follows:

[0051]

[0052] In the formula, π(X) represents the 2D image coordinates obtained after the projection transformation, f x is the focal length in the horizontal direction; f y is the focal length in the vertical direction; (c x , c y ) is the principal point coordinates; X i represents the X-axis coordinate in the 3D world coordinate system, Y i represents the Y-axis coordinate in the 3D world coordinate system, and Z i represents the Z-axis coordinate in the 3D world coordinate system.

[0053] To solve PnP, first perform an initial estimate of the camera pose, use the RANSAC algorithm to eliminate outliers in the matching points, and select the optimal set of inliers. By selecting the minimum number of points (usually 4 points), estimate the camera pose. The goal of RANSAC is to find a transformation (R i , t i ) such that the largest number of point pairs satisfy:

[0054] ||p i -π(R i P i +t i )||<∈ (7)

[0055] Where ∈ is the error threshold, π is the projection function of the camera, and P i represents the 3D map point of the i-th frame, and p i represents the 2D image point corresponding to the 3D map point P i in the i-th frame.

[0056] Then, the EPnP (Efficient PnP) algorithm is adopted to calculate the initial pose estimation of the camera. EPnP transforms the PnP problem into a system of linear equations for solution by using a linear combination of control points.

[0057] The non-linear optimization objective function is:

[0058]

[0059] Then, the camera pose is further optimized by the Levenberg-Marquardt algorithm to minimize the projection error, that is, to minimize the error between the projected position of the 3D map point on the 2D image plane and the actually observed 2D image key points, thereby improving the accuracy of the camera pose estimation. Among them, the expression for iteratively updating the camera pose parameters using the Levenberg-Marquardt algorithm is:

[0060]

[0061] Where x k represents the camera pose parameters at the k-th iteration x k+1 represents the camera pose parameters at the (k + 1)-th iteration J k is the Jacobian matrix; represents the transpose of the Jacobian matrix, which is used to calculate the gradient direction; r k is the residual vector; λ is the damping factor; I represents the identity matrix; r k represents the residual vector, that is, the error between the currently estimated projected point and the actually observed point.

[0062] To ensure the accuracy of the camera pose estimation, after the PnP solution, ORB-SLAM3 performs a consistency check on the 2D image and 3D map matching pairs (i.e., matching key points) of the same frame and executes BA (Bundle Adjustment) optimization.

[0063] For the consistency check of matching pairs, calculate the reprojection error of each matching key point, and filter out the inliers whose reprojection error e i is within a predetermined threshold:

[0064] e i = ||p i - π(R i P i + t i )|| (10)

[0065] In the formula, retain the matching pairs that satisfy e i < ∈. Use the RANSAC algorithm to eliminate the matching points that do not conform to the geometric constraints, and retain the geometrically consistent matching pairs. By verifying the fundamental matrix or the homography matrix, ensure that the matching points satisfy the geometric relationship.

[0066] For BA optimization, by jointly optimizing the camera pose and the positions of 3D map points, further reduce the reprojection error and improve the accuracy and consistency of the entire system:

[0067]

[0068] In the formula, R is the set of rotation matrices of all camera key frames, t is the set of translation vectors of all camera key frames, P is the set of coordinates of all 3D map points, R i is the rotation matrix of the i-th key frame, t i is the translation vector of the i-th key frame, P j is the j-th map point, p ij is the image projection of map point j in key frame i, N is the number of key frames, M i is the number of 3D map points observed in the i-th key frame. BA optimization usually adopts an efficient sparse optimization algorithm, such as the Levenberg-Marquardt algorithm, and minimizes the overall reprojection error by iteratively updating the parameters.

[0069] During long-term operation, the robot may return to an area it has passed through before. At this time, performing loop closure detection can effectively reduce the cumulative error and improve the global consistency of the map. First, use the Bag-of-Words (BoW) model to detect the similarity between the current key frame and historical key frames, and identify potential loops. The BoW model realizes fast image similarity retrieval by converting image feature descriptors into bag-of-words vectors. Generate a visual vocabulary through K-Means clustering, map the ORB descriptors of key frames to the words in the vocabulary to form bag-of-words vectors, and calculate the similarity of the bag-of-words vectors between the current key frame and historical key frames:

[0070]

[0071] Where A and B are two bag-of-words vectors, and S(A, B) is the similarity between bag-of-words vector A and bag-of-words vector B.

[0072] Then, through geometric verification and feature matching, the authenticity of the closed loop is confirmed, the incorrect closed loops are removed, and it is verified whether the matching points satisfy The expression of the fundamental matrix F is as follows:

[0073] F = FindFundamentalMatrix(p i , p′ i ) (13)

[0074] Where F is the fundamental matrix, representing the geometric relationship between two views; p i is the two-dimensional point coordinate of the corresponding point in the current frame image of the matching pair; p′ i is the two-dimensional point coordinate of the corresponding point in the historical frame image of the matching pair.

[0075] After detecting the closed loop, a factor graph is constructed, the constraints between the key frames of the closed loop are added to the feature point map, and the global map is optimized using the graph optimization algorithm (g2o).

[0076] Taking the camera poses of the key frames and the 3D map points as nodes and the observation relationships as edges, a factor graph is constructed. The g2o (General Graph Optimization) graph optimization library is used for non-linear least squares optimization to solve the globally consistent camera poses and 3D map points. The optimization objective function is:

[0077]

[0078] Where G is the set of state variables of the camera poses and 3D map points; ε is the set of edges in the factor graph; z ij is the observation value; h ij is the observation model; Σ ij is the observation noise covariance matrix; i is the index of the camera pose node; j is the index of the 3D map point node; G i is the state variable of the i-th camera pose, G j is the state variable of the j-th 3D map point.

[0079] Through closed loop detection and global optimization, ORB-SLAM3 can effectively eliminate the cumulative error during long-term operation and improve the accuracy and consistency of the entire map.

[0080] Generally speaking, in the images obtained by the camera, the ORB algorithm is used to detect and describe the key points, the FAST algorithm is used to detect the corner points in the images, then the main direction θ of the key points is assigned, and the rotation-invariant ORB descriptor d p is generated. Then, the Hamming distance DH Descriptor matching is performed, and the matching is optimized through a bidirectional matching strategy and the RANSAC algorithm to eliminate incorrect matching pairs and ensure the geometric consistency of the matching point pairs. Then, the PnP algorithm is used to estimate the initial pose x v =[x v y v θ v T , that is, the initial pose of the robot is estimated. Then, the consistency check of the matching pairs is performed, and BA (Bundle Adjustment) optimization is executed. Finally, loop detection and global optimization are carried out globally to obtain the visual mapping trajectory and the feature point map.

[0081] Establish the pose mapping between maps:

[0082] In this system, while the visual information is being mapped, the lidar mapping is also being synchronized to obtain the grid map and the lidar mapping trajectory. Since the initial position of the grid map is determined by the starting information of the front-end pose tracking in the lidar mapping thread, and the initial coordinate system of the feature point map is based on the camera coordinate system of the first key frame in the visual mapping thread, there is a transformation relationship between the two map coordinate systems. Simply relying on the joint calibration of the lidar and the visual camera for coordinate conversion will result in a poor alignment effect due to the size and error optimization during the mapping process.

[0083] To effectively apply the visual rough positioning information to precise positioning, it is necessary to further obtain the positioning information of the mobile robot in the grid map. The present invention realizes the global consistent positioning of the grid map and the feature point map by establishing the pose mapping relationship between the two maps. Specifically, during the mapping stage, the global positioning algorithm aligns the feature point map and the grid map by aligning the two mapping motion trajectories.

[0084] Since the calculation frequencies of the lidar and the camera visual mapping threads are different, and the timestamps of the visual key frames and the lidar frames do not exactly correspond, the present invention uses a linear interpolation method to process the lidar mapping trajectory, calculates the poses corresponding to the camera key frame sequence in this trajectory, and through time synchronization processing, each pose in the trajectory of the mobile robot can be represented by the observation information of multiple sensors. Then, the problem of aligning the two mapping trajectories is transformed into a least squares optimization problem, thereby establishing the pose mapping relationship between the two maps. After the visual rapid positioning is completed, the rough positioning position of the mobile robot in the grid map can be accurately obtained using the pose mapping relationship.

[0085] Among them, using the linear interpolation method to process the lidar mapping trajectory and calculate the poses corresponding to the camera key frame sequence in this trajectory, that is, time synchronization processing, is as follows:

[0086] ​During the front-end pose tracking process, a rotation matrix is used to describe spatial rotation. However, the rotation matrix uses nine quantities to describe a three-degree-of-freedom rotation, which is redundant. For the convenience of calculation, quaternions are used to represent rotation during the interpolation calculation. A quaternion q includes a real part and three imaginary parts, in the form of:

[0087] q = w + xa + yb + zc (15)

[0088] In the formula, w is the real part of the quaternion; a, b, and c are the three imaginary parts of the quaternion, and satisfy:

[0089]

[0090] The conversion between quaternions and rotation vectors includes: rotating around the unit vector u = [u x , u y , u z T The process of rotating an angle ψ is represented in the form of a rotation vector as By corresponding the three imaginary parts of the quaternion to the three axes in space, the exponential logarithm mapping relationship of the corresponding quaternion is:

[0091]

[0092] uψ = log(q) (18)

[0093] Linear interpolation is achieved by establishing a function q(t) such that the robot can rotate uniformly around a fixed axis from q0 to q1 within a certain period of time. Interpolation of quaternions needs to ensure that the interpolation vector trajectory is on the unit sphere. By mapping the rotation vector to a quaternion through the exponential function, the following interpolation function is obtained:

[0094]

[0095] Finally, the interpolation result is obtained:

[0096]

[0097] In the formula, q(t) represents the quaternion at time t, indicating the rotation state of the robot at that moment; q0 represents the starting quaternion, indicating the rotation state of the robot at the initial moment; q1 represents the target quaternion, indicating the rotation state of the robot at the target moment; represents the rotation vector; t is the time parameter, controlling the interpolation progress of the rotation.

[0098] After synchronization processing, to align the key frames of lidar mapping and visual mapping, a least squares problem is constructed. By optimizing the coordinate transformation relationship between the two maps, the sum of the squares of the pose observation residuals of the lidar mapping trajectory and the visual mapping trajectory is minimized. The expression is: ​

[0099]

[0100] where p 1i represents the pose of the robot in the feature point map at the i-th key frame; p 2i represents the pose of the robot in the lidar grid map at the i-th key frame, R o is the rotation in the transformation relationship, and t o is the translation in the transformation relationship.

[0101] By solving this optimization problem, the mapping of the robot pose relationship between the grid map and the feature point map can be established. After completing the visual rapid positioning, the visual rough positioning result can be transformed to the grid map according to the mapping relationship. For the initial pose x v given by vision, it is converted to the grid map coordinate system through the following formula to obtain the rough positioning pose information of the robot:

[0102] x l = R o x v + t o (22)

[0103] where x l is the rough positioning pose information in the grid map, R o is the rotation in the transformation relationship, and t o is the translation in the transformation relationship.

[0104] In terms of positioning, currently the AMCL algorithm is the mainstream algorithm in the field of laser positioning. Its comprehensive performance has been widely recognized in theoretical research and engineering practice and is widely used in mobile robot positioning algorithms. However, AMCL has a large computational overhead when the number of particles is large, and its real-time performance is limited. In addition, during global positioning, especially when the initial position of the robot is unknown or seriously deviated, the problem of particle depletion is likely to occur, resulting in positioning failure.

[0105] Compared with traditional ACML positioning, Cartographer's Pure Localization can achieve higher-precision pose estimation in complex environments through ICP and pose graph optimization techniques, and has good robustness. In an environment rich in features, Pure Localization can effectively perform matching and positioning, avoiding the positioning failure problem of AMCL in the case of particle depletion; although it involves a complex optimization process and the repositioning process takes longer than ACML, for the positioning requirements of indoor service robots, positioning accuracy and accuracy are the primary goals. Therefore, the laser positioning solution of the present invention selects the Pure Localization positioning solution based on Cartographer.

[0106] After obtaining the rough positioning pose \(x\) l , the Pure Localization module of Cartographer is used for precise laser positioning.

[0107] To improve the matching efficiency and accuracy, the rough positioning pose information \(x\) l is used as the initial guess of Pure Localization:

[0108] \(x\) initial =\(x\) l (23)

[0109] That is, the robot pose \(x\) obtained by rough positioning initial is used as the prior information. A set of robot position data is randomly initialized around \(x\) initial , denoted as Let \(M\) be the number of positions. Each \(x\) t represents the position information of the robot in the map, excluding the pose information. Through visual rough positioning, the candidate map search area is effectively reduced, and there is no need to distribute data on the global map. For each position , it is fused with the lidar point cloud information and odometer data for localization, and the current pose of the robot is estimated. That is, for each position , the precise localization of the robot is completed using Pure Localization. During the localization process, Pure Localization simultaneously obtains the real-time lidar point cloud information and odometer data. These data are used to match with the map to estimate the current pose of the robot.

[0110] Cartographer adopts the pose graph optimization technology. Through the nonlinear least squares optimization method, the matching error between the lidar point cloud information and the grid map is further minimized:

[0111]

[0112] In the formula, is the optimal pose, \(z\) k is the \(k\)-th laser point, is the corresponding point of the grid map at the position . Pose graph optimization constructs a factor graph containing laser and odometer observations, and uses optimization algorithms (such as Gauss - Newton or Levenberg - Marquardt) to solve the optimal pose

[0113] Pose output: After scan matching and pose optimization, Pure Localization outputs a high - precision robot pose estimation result:

[0114]

[0115] In order to verify the relocalization effect of the laser and visual information fusion proposed by the present invention, a comparative experiment was carried out with the relocalization method based on the Cartographer algorithm.

[0116] Using the SLAM function of Cartographer, manually remotely control the robot to move in the experimental environment, obtain lidar data, construct a grid map of the indoor environment (Grid Map), and save the generated grid map file for subsequent relocalization. Start the Pure Localization mode of Cartographer, and conduct a total of 30 kidnapping experiments in 10 groups for each of the three positions A, B, and C of the robot with respect to the map. As Figure 3 shown in part a) of, the lidar data on the grid map has changed after the robot is kidnapped. Let the robot use the lidar scan data to perform localization in the pre-constructed map, and finally record the pose estimation results of the robot during the relocalization process, as Figure 3 shown in part b) of.

[0117] Taking 15s, 20s, and 25s as limits respectively, calculate the relocalization success rate as shown in Table 1.

[0118]

[0119] Table 1 Relocalization success rate of Cartographer (relocalization success rate of the prior art)

[0120] Record the localization error after each localization, and use Matlab to draw a line chart of the localization error in the pure localization mode based on Cartographer as Figure 4 shown. In the figure, parts a), b), and c) respectively correspond to the relocalization errors in the x and y directions of point A, point B, and point C.

[0121] Then, conduct a relocalization experiment using the present invention. First, perform visual rough localization, then perform relocalization through Pure Localization, and finally record the success rate, time used, and pose estimation results of the robot's relocalization. Taking 15s, 20s, and 25s as limits respectively, calculate the relocalization success rate as shown in Table 2.

[0122]

[0123] Table 2 Relocalization success rate of the present invention

[0124] Record the localization error after each localization, and use Matlab to draw a line chart as Figure 5The broken line graph of the positioning error in the visual and lidar fusion positioning mode shown; similarly, the a), b) and c) parts in the figure correspond to the repositioning errors in the x and y directions at points A, B and C respectively.

[0125] Further, the average values of the repositioning error, the time used and the success rate at each point are statistically calculated, and the repositioning of the method of the present invention and the Cartographer algorithm is compared, as shown in Table 3.

[0126]

[0127] Table 3 Comparison of repositioning experiments between the present invention and the prior art

[0128] As can be seen from Table 3, the repositioning method of the present invention reduces the positioning error by an average of 39.7% and the repositioning time by 23.2% compared with the positioning method based on Cartographer, and the repositioning success rate limited by different time lengths is also significantly better than the comparative method.

[0129] In summary, the present invention improves the accuracy, reduces the repositioning time and positioning error compared with the pure lidar algorithm positioning.

[0130] The above embodiments merely illustrate the principles and effects of the present invention, rather than limiting the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those with ordinary knowledge in the technical field without departing from the spirit and technical idea disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. A method for relocating an indoor service robot based on the fusion of laser and vision, characterized in that, Including: The S1 robot locates and maps the indoor environment through a camera and a lidar. Among them, a feature point map is constructed through visual information and a visual mapping trajectory is obtained; A grid map is constructed through lidar point cloud information and a lidar mapping trajectory is obtained; S2 uses a linear interpolation method to process the lidar mapping trajectory and calculates the poses corresponding to the camera key frame sequence in this trajectory; S3 constructs a least squares problem. By optimizing the coordinate transformation relationship between the feature point map and the grid map, the sum of the squares of the pose observation residuals in the two mapping trajectories is minimized, aligning the key frames of lidar mapping and visual mapping, and establishing a mapping of the robot pose relationship between the grid map and the feature point map; S4 based on the pose mapping of the visual mapping trajectory and the lidar mapping trajectory, establishes a rough localization of the mobile robot in the grid map and obtains the rough localization pose information; S5 fuses the rough localization pose information as prior information with the lidar point cloud information and odometer data for localization.

2. The method according to claim 1, characterized in that, The expression of the least squares problem constructed in S3 is: where p 1i represents the pose of the robot in the feature point map at the i-th key frame; p 2i represents the pose of the robot in the lidar grid map at the i-th key frame, R o is the rotation in the transformation relationship, and t o is the translation in the transformation relationship.

3. The method according to claim 2, wherein In S4, the starting pose given visually is converted into the grid map coordinate system through the following formula to obtain the rough localization pose information of the robot: x l = R o x v + t o Among them, x l is the rough positioning pose information in the grid map, R o is the rotation in the transformation relationship, x v is the initial pose given visually, t o is the translation in the transformation relationship.

4. The method according to claim 3, wherein In S5, a set of robot position data is randomly initialized around the rough positioning pose information, denoted as where M is the number of positions, and each position is fused and positioned with the lidar point cloud information and odometer data to estimate the current pose of the robot.

5. The method according to claim 4, wherein In S5, through the nonlinear least squares optimization method, the matching error between the lidar point cloud information and the grid map is further minimized, and the expression is: In the formula, is the optimal pose, z k is the k-th laser point, is the corresponding point of the grid map at the position below.

6. The method according to any one of claims 1-5, characterized in that, In S2, a linear interpolation method is used to process the lidar mapping trajectory and calculate the poses corresponding to the camera key frame sequence in this trajectory, including: The rotation is represented using quaternions. The quaternion q includes a real part and three imaginary parts, in the form of: q = w + xa + yb + zc In the formula, w is the real part of the quaternion; a, b, and c are the three imaginary parts of the quaternion, and satisfy: Converting quaternions and rotation vectors includes: rotating around the unit vector u = [u x , u y , u z T The process of rotating by an angle ψ is represented in the form of a rotation vector as Corresponding the three imaginary parts of the quaternion to the three axes in space, the exponential-logarithmic mapping relationship of the quaternion is as follows:​ uψ = log(q) By establishing a function q(t), the robot can rotate uniformly around a fixed axis from q0 to q1 within a period of time. The rotation vector is exponentiated to a quaternion, and the following interpolation function is obtained: Wherein, \(q(t)\) represents the quaternion at time \(t\), indicating the rotation state of the robot at that moment; \(q_0\) represents the initial quaternion, indicating the rotation state of the robot at the initial moment; \(q_1\) represents the target quaternion, indicating the rotation state of the robot at the target moment; represents the rotation vector; \(t\) is the time parameter.

7. The method according to any one of claims 1-5, characterized in that, In S1, the process of constructing a feature point map through visual information and obtaining a visual mapping trajectory includes: Extracting the image features of the key points; Performing key point image feature matching through the Hamming distance and performing matching optimization; Calculating the camera pose and performing camera pose consistency check and BA optimization; When the robot returns to the area it passed through before, performing loop closure detection; After detecting the loop closure, constructing a factor graph, adding the constraints between the loop closure key frames to the feature point map, and using the graph optimization algorithm to optimize the global map.

Images