Robot navigation positioning method based on machine vision

Abstract

The invention discloses a robot navigation and positioning method based on machine vision. The robot navigation and positioning method comprises the following steps: acquiring and preprocessing an environment image sequence; performing feature point detection and feature description by using an improved DELF model to generate a visual feature point set; obtaining a visual pose estimation result; obtaining fusion pose information; constructing a local environment map by adopting a sparse point cloud reconstruction method; generating a global environment map; generating an optimal advancing path by using a path planning method; and a robot motion control instruction is generated, path adjustment and navigation correction are performed, and high-precision autonomous positioning and navigation of the robot in a complex environment are realized.

Description

Technical Field

[0001] This invention relates to the field of intelligent robots, and more particularly to a robot navigation and positioning method based on machine vision. Background Technology

[0002] With the rapid development of artificial intelligence and robotics, mobile robots are increasingly being used in services, inspections, warehousing, healthcare, and unmanned transportation. To enable robots to navigate and locate autonomously in complex and dynamic environments, researchers have proposed various environmental perception and pose estimation methods. Among these, sensor fusion-based navigation and localization technology is one of the core research directions. Traditional robot localization methods mainly rely on lidar, ultrasonic sensors, or odometry sensors, achieving environmental mapping and self-localization through ranging, angle measurement, and path integration. However, these methods suffer from drawbacks such as high equipment costs, strong environmental dependence, and significant cumulative errors, making it difficult to meet the application requirements of lightweight, low-cost robot systems in various scenarios.

[0003] In recent years, with breakthroughs in computer vision and deep learning technologies, machine vision-based navigation and localization methods have become a research hotspot. These methods acquire environmental image sequences via cameras and use the detection and matching of image feature points to infer robot pose changes, achieving visual simultaneous localization and SLAM mapping. Common visual feature extraction algorithms include SIFT, SURF, ORB, and DELF, which extract key points and descriptors to reflect the visual features of the environment. However, traditional visual feature extraction and matching methods exhibit significant performance degradation under varying lighting conditions, dynamic scenes, and sparse texture environments, leading to insufficient feature point matching, poor feature stability, and low pose estimation accuracy. Especially during continuous robot movement, factors such as image blurring, viewpoint changes, and occlusion can easily cause feature drift, affecting overall localization accuracy. Summary of the Invention

[0004] One objective of this invention is to propose a robot navigation and localization method based on machine vision. This invention fully utilizes an improved DELF model, a pose estimation algorithm based on feature point matching, an extended Kalman filter multi-source fusion algorithm, and sparse point cloud mapping technology. Through joint modeling of visual sensor data and inertial measurement data, it achieves intelligent localization and path planning throughout the entire process, from environmental perception and pose estimation to global navigation. The method details how, within a collaborative framework of visual sensors and inertial measurement units, high-precision environmental mapping and optimal path generation are achieved using visual feature detection, pose change estimation, and fused pose optimization. It possesses advantages such as high positioning accuracy, strong fusion robustness, good environmental adaptability, and superior real-time performance.

[0005] A robot navigation and localization method based on machine vision according to an embodiment of the present invention includes the following steps:

[0006] The robot acquires and preprocesses environmental image sequences using its vision sensors.

[0007] An improved DELF model is used to detect and describe feature points in the preprocessed environmental image sequence, generating a set of visual feature points.

[0008] Based on the set of visual feature points, a feature matching algorithm is used to establish the correspondence between feature points. The pose change parameters of the visual sensor are calculated based on the matching results. A pose estimation algorithm based on feature point matching is used to obtain the visual pose estimation result.

[0009] The visual pose estimation results are fused with the acceleration and angular velocity data collected by the inertial measurement unit to obtain fused pose information.

[0010] A local environment map is constructed using a sparse point cloud reconstruction method based on fused pose information;

[0011] Based on visual simultaneous localization and SLAM algorithm, the local environment map is used as input, and incremental fusion is performed through keyframe constraint optimization and pose graph optimization algorithm. Spatial registration and fusion of local environment map are completed under unified reference coordinate system to generate global environment map.

[0012] The optimal travel path is generated using path planning methods based on the global environment map.

[0013] The robot motion control commands are generated based on the optimal travel path to control the robot to perform navigation movements. The robot's pose deviation is monitored in real time, and path adjustments and navigation corrections are made based on feedback information.

[0014] Optionally, the preprocessing includes image denoising, distortion correction, brightness equalization, and edge enhancement.

[0015] Optionally, the generation of the visual feature point set specifically includes:

[0016] The preprocessed environmental image sequence is input into the improved DELF model for feature point detection and feature description. The improved DELF model includes a multi-scale feature extraction module, a structure-frequency domain joint attention module, and a feature description and encoding module. The multi-scale feature extraction module refers to performing multi-scale feature extraction on the preprocessed environmental image sequence. The structure-frequency domain joint attention module refers to performing saliency enhancement on key regions in the convolutional feature map. The feature description and encoding module refers to performing high-dimensional feature description and encoding on the detected visual feature points.

[0017] The preprocessed environmental image sequence is subjected to multi-scale feature extraction through a multi-scale feature extraction module to construct a multi-scale environmental image pyramid and generate a convolutional feature map. The generation of the convolutional feature map is to perform a convolution operation between the environmental image at each scale and the weight matrix to extract the spatial texture and semantic features at that scale.

[0018] The scale-convolutional feature map is input into the structure-frequency domain joint attention module, and the feature locations are jointly weighted using the anisotropy of the structure tensor and the local frequency domain energy density to obtain the saliency weights.

[0019] Non-maximum suppression is performed on the saliency weights to extract the set of visual feature points with the largest local response from the convolutional feature map. The non-maximum suppression operation means retaining the feature points with the strongest response and removing redundancy from the convolutional feature map.

[0020] The extracted set of visual feature points is input into the feature description and encoding module, and a visual feature description vector is generated using a deep embedding network. The generation of the visual feature description vector is to extract local neighborhood feature blocks in the convolutional feature map of the corresponding scale with the feature points of the set of visual feature points as the center.

[0021] The coordinates, scale, principal direction, and visual feature description vector of the visual feature point set are combined to form the visual feature point set.

[0022] Optionally, obtaining the visual pose estimation result specifically includes:

[0023] The set of visual feature points is matched by a feature matching algorithm based on descriptor distance to form a preliminary matching result. The preliminary matching result is formed by comparing the visual feature points in adjacent environmental image frames one by one, calculating the similarity between the visual feature descriptor vectors corresponding to each feature point, measuring the degree of difference between feature vectors by Euclidean distance, and selecting the optimal matching candidate pair based on the similarity ranking result.

[0024] The preliminary matching results are filtered, and the nearest neighbor to second nearest neighbor distance ratio criterion is used for matching verification. When the ratio of the minimum matching distance to the second minimum matching distance is less than a set threshold, a valid matching point pair is obtained.

[0025] The effective matching point pairs are optimized using a random consistency sampling algorithm. The reprojection error of each matching point pair is calculated based on geometric constraints. Mismatched points are eliminated to obtain the set of visual matching interior points constrained by the geometric model.

[0026] Based on the visually matched inlier set, a geometric constraint model between adjacent environmental image frames is constructed to obtain the essential matrix. The construction process is based on the polar geometric relationship model established by the visual feature point pairs matched in adjacent environmental image frames. The generation of the essential matrix is ​​achieved by using a pose estimation algorithm to solve the relative motion relationship of the visual sensors and obtaining the optimal essential matrix parameters by minimizing the reprojection error of the matched inlier.

[0027] The essential matrix is ​​decomposed by singular value decomposition, and the rotation matrix and translation vector are extracted to form the pose transformation matrix between adjacent image frames.

[0028] The pose transformation matrix between adjacent image frames is continuously accumulated and optimized according to the time series to obtain the visual pose estimation result.

[0029] Optionally, obtaining the fused pose information specifically includes:

[0030] The visual pose estimation results are fused with the acceleration and angular velocity data collected by the inertial measurement unit to form a time-aligned multi-source observation dataset. The multi-source observation dataset is generated by synchronizing the timestamps of different sensors and registering and interpolating the data in a unified time coordinate system.

[0031] A fusion state model is established based on a time-aligned multi-source observation dataset. A fusion state vector is defined, and the fusion state vector is predicted in time according to the dynamic equation of the inertial measurement unit. The robot pose change is calculated using acceleration and angular velocity data to obtain the predicted pose and prediction covariance matrix. The establishment of the fusion state model refers to the construction of a mathematical model of the dynamic behavior of the system and the relationship between observations during the information fusion process of the visual sensor and the inertial measurement unit, based on the robot kinematics and sensor observation characteristics. The fusion state vector includes position, velocity, attitude quaternions and accelerometer zero bias and gyroscope zero bias.

[0032] The predicted pose is used as the prior estimation result, and the visual pose estimation result provided by the visual sensor is used as the observation input. A fusion observation equation is established, and the observation residual is formed by calculating the deviation between the prior predicted pose and the visual observation pose.

[0033] Using the extended Kalman filter algorithm, the Kalman gain matrix is ​​calculated based on the predicted covariance matrix and the preset observation noise covariance matrix. The fused state vector and covariance matrix are then updated based on the Kalman gain matrix to obtain the updated fused state vector.

[0034] Extract fused pose information from the updated fused state vector.

[0035] Optionally, the construction of the local environment map specifically includes:

[0036] Based on the fused pose information, the pose parameter set of the robot in the continuous time series is extracted, and the pose parameter set includes the position vector and the attitude rotation matrix;

[0037] Based on the pose parameter set, the environmental image frames acquired by the visual sensor are synchronized in time and registered in space with the corresponding fused pose information. A mapping relationship between the environmental image frames and three-dimensional spatial points is established in a unified coordinate system to obtain the three-dimensional point coordinates. The generation of the three-dimensional point coordinates is based on the intrinsic parameters, pixel depth values ​​and pose parameters of the visual sensor, and the pixel coordinates are converted into three-dimensional point coordinates in the world coordinate system.

[0038] The coordinates of the 3D point cloud are aligned and fused. A sparse point cloud reconstruction method based on fused pose information is adopted to generate a preliminary local sparse point cloud model.

[0039] The initial local sparse point cloud model is subjected to noise suppression and redundant point removal. An outlier point removal algorithm based on statistical distance is used to filter out outliers, resulting in an optimized sparse point cloud model.

[0040] A local environment map is constructed based on the optimized sparse point cloud model. The construction process involves indexing and associating the point cloud data of the sparse point cloud model with the fused pose information to form a local environment map containing pose, depth, and spatial feature information.

[0041] Optionally, the generation of the global environment map specifically includes:

[0042] Using the local environment map as input, we extract local point cloud data, visual feature information and fused pose information from the local environment map, and establish a keyframe index structure with keyframes as the basic unit.

[0043] Based on the keyframe index structure, spatial constraint relationships between keyframes are established according to the fused pose information between adjacent local environment maps. A pose graph model is constructed using visual simultaneous localization and SLAM algorithms. The pose graph model includes nodes of the fused pose of keyframes and edges of spatial constraints between keyframes.

[0044] The pose graph model is subjected to graph optimization processing, and incremental fusion is performed using a nonlinear least squares optimization method to obtain a keyframe fused pose set. The keyframe fused pose set is obtained by minimizing the fused pose constraint residual between keyframes as the objective function, and the fused pose information of the keyframes is updated.

[0045] Based on the keyframe fusion pose set, the local environment map is spatially aligned, and the local environment map is spatially registered in a unified global coordinate system through a coordinate transformation matrix to generate a global environment map.

[0046] Optionally, the generation of the optimal travel path specifically includes:

[0047] A path planning configuration space is established in the global environment map. The establishment process is to perform a safety expansion model of the obstacle area, use the robot pose parameters as state variables, define the feasible motion space in the global coordinate system, and form the path planning configuration space.

[0048] Based on the obstacle distribution information in the global environment map, the distance from each map location point to the obstacle boundary is calculated in the path planning configuration space, and the feasible movement area is divided according to the preset safe distance threshold.

[0049] Discretize the feasible motion region to construct a search graph containing nodes and edges, and define the path cost function of adjacent nodes. The path cost is defined by weighting and summing the distance between nodes, obstacle potential field cost and orientation change cost using a unified weight coefficient.

[0050] A heuristic path search algorithm is used to search for paths in the search graph, and the cost is evaluated through a path cost function. An initial planned path is generated according to the principle of minimum cost.

[0051] The initial planned path is made continuous and the trajectory is optimized to generate a continuous and executable optimal path. The optimal path is a continuous and executable trajectory obtained by minimizing the path cost function, under the premise of satisfying the robot's kinematic constraints, with the optimization objectives of maximizing path smoothness, curvature continuity and obstacle safety distance.

[0052] Optionally, the generation and modification of the robot motion control commands specifically includes:

[0053] The optimal travel path is used as input to extract the discrete path point sequence in the optimal travel path. The desired linear velocity and desired angular velocity are calculated based on the spatial coordinate relationship between adjacent path points to generate the desired motion trajectory.

[0054] The robot's current pose is obtained by combining fused pose information, and the pose deviation between the current pose and the desired motion trajectory is calculated. The pose deviation includes a position error vector and an attitude error angle.

[0055] A path tracking control model is established based on pose deviation. A proportional-integral-derivative controller is used to calculate the control increment, dynamically correct the linear velocity and angular velocity parameters of the robot motion control module, and generate real-time control commands.

[0056] Real-time control commands are input into the robot's motion control module to drive the robot to navigate along the optimal path. At the same time, environmental image data and motion state data are collected in real time through the robot's vision sensor and inertial measurement unit to update and fuse pose information.

[0057] Based on the updated fused pose information, pose deviation is continuously calculated. When the pose deviation is detected to be greater than a preset threshold, the local path replanning module is triggered. Based on the global environment map and the current fused pose information, a new local correction path is generated, and motion control commands are updated in real time to adaptively correct the navigation path.

[0058] The beneficial effects of this invention are:

[0059] This invention proposes a machine vision-based robot navigation and localization method, achieving high-precision autonomous localization and path navigation for robots in complex dynamic environments. The invention obtains more stable and discriminative visual feature points through an improved DELF model and combines it with a pose estimation algorithm based on feature point matching to accurately calculate pose changes from the visual sensor. Simultaneously, it fuses the visual pose estimation results with acceleration and angular velocity information collected by the inertial measurement unit (IMU), and uses an extended Kalman filter (EPF) algorithm to dynamically optimize and correct the robot's pose, effectively eliminating drift errors caused by single-vision measurements under conditions of illumination changes, motion blur, and occlusion. Based on the fused pose information, a geometrically consistent local environment map is constructed using a sparse point cloud reconstruction method. Furthermore, incremental fusion of the local map and dynamic optimization of the global coordinate system are achieved through visual simultaneous localization (VLS) and SLAM algorithms, generating a high-precision global environment map. During the path planning phase, this invention generates a continuously executable optimal path based on the global environment map and robot kinematic constraints through cost function optimization and heuristic search algorithms. It also incorporates a real-time feedback mechanism to correct robot pose deviations, ensuring the smoothness and reliability of the navigation process.

[0060] Through the synergistic application of the above methods, this invention achieves closed-loop optimization of the entire process from visual perception, information fusion, environmental mapping to path planning and motion control. Compared with existing technologies, this invention has the following beneficial effects: First, it significantly improves the robot's positioning accuracy and robustness in complex environments, enabling stable navigation in environments with changing lighting, dynamic scenes, and sparse textures; second, it achieves adaptive fusion of multi-source information, effectively suppressing sensor noise and drift errors; third, the constructed global environment map has high consistency and update efficiency, providing accurate environmental information support for subsequent path planning; fourth, the generated path is smooth and executable, allowing the robot to autonomously avoid obstacles and adjust its path in real time, improving the safety and intelligence of task execution. In summary, this invention has achieved significant improvements in visual perception accuracy, information fusion reliability, and intelligent navigation control, possessing good engineering practical value and potential for widespread application. Attached Figure Description

[0061] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0062] Figure 1 This is an overall flowchart of a robot navigation and localization method based on machine vision proposed in this invention;

[0063] Figure 2 This is a schematic diagram of the module structure of an improved DELF model for a robot navigation and localization method based on machine vision proposed in this invention. Detailed Implementation

[0064] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0065] refer to Figure 1-2 A robot navigation and localization method based on machine vision includes the following steps:

[0066] The robot acquires and preprocesses environmental image sequences using its vision sensors.

[0067] An improved DELF model is used to detect and describe feature points in the preprocessed environmental image sequence, generating a set of visual feature points.

[0068] Based on the set of visual feature points, a feature matching algorithm is used to establish the correspondence between feature points. The pose change parameters of the visual sensor are calculated based on the matching results. A pose estimation algorithm based on feature point matching is used to obtain the visual pose estimation result.

[0069] The visual pose estimation results are fused with the acceleration and angular velocity data collected by the inertial measurement unit to obtain fused pose information.

[0070] A local environment map is constructed using a sparse point cloud reconstruction method based on fused pose information;

[0071] Based on visual simultaneous localization and SLAM algorithm, the local environment map is used as input, and incremental fusion is performed through keyframe constraint optimization and pose graph optimization algorithm. Spatial registration and fusion of local environment map are completed under unified reference coordinate system to generate global environment map.

[0072] The optimal travel path is generated using path planning methods based on the global environment map.

[0073] The robot motion control commands are generated based on the optimal travel path to control the robot to perform navigation movements. The robot's pose deviation is monitored in real time, and path adjustments and navigation corrections are made based on feedback information.

[0074] In this embodiment, the preprocessing includes image denoising, distortion correction, brightness equalization, and edge enhancement.

[0075] In this embodiment, the generation of the visual feature point set specifically includes:

[0076] The preprocessed environmental image sequence is input into the improved DELF model for feature point detection and feature description. The improved DELF model includes a multi-scale feature extraction module, a structure-frequency domain joint attention module, and a feature description and encoding module. The multi-scale feature extraction module refers to performing multi-scale feature extraction on the preprocessed environmental image sequence. The structure-frequency domain joint attention module refers to performing saliency enhancement on key regions in the convolutional feature map. The feature description and encoding module refers to performing high-dimensional feature description and encoding on the detected visual feature points.

[0077] The preprocessed environmental image sequence is subjected to multi-scale feature extraction through a multi-scale feature extraction module to construct a multi-scale environmental image pyramid and generate a convolutional feature map. The generation of the convolutional feature map is to perform a convolution operation between the environmental image at each scale and the weight matrix to extract the spatial texture and semantic features at that scale.

[0078] The scale-convolutional feature map is input into the structure-frequency domain joint attention module. The feature locations are jointly weighted using the anisotropy of the structure tensor and the local frequency domain energy density to obtain the saliency weights.

[0079] ;

[0080] in, For significance weight, For learnable weight parameters, The anisotropy of the structure tensor. For local frequency domain energy density, Image frame number, For scale indexing, It is a set of feature map pixels. It is an exponential function. The current pixel coordinates, To iterate through pixel coordinates;

[0081] Non-maximum suppression is performed on the saliency weights to extract the set of visual feature points with the largest local response from the convolutional feature map. The non-maximum suppression operation means retaining the feature points with the strongest response and removing redundancy from the convolutional feature map.

[0082] The extracted set of visual feature points is input into the feature description and encoding module, and a visual feature description vector is generated using a deep embedding network. The generation of the visual feature description vector is to extract local neighborhood feature blocks in the convolutional feature map of the corresponding scale with the feature points of the set of visual feature points as the center.

[0083] The coordinates, scale, principal direction, and visual feature description vector of the visual feature point set are combined to form the visual feature point set.

[0084] In this embodiment, obtaining the visual pose estimation result specifically includes:

[0085] The set of visual feature points is matched by a feature matching algorithm based on descriptor distance to form a preliminary matching result. The preliminary matching result is formed by comparing the visual feature points in adjacent environmental image frames one by one, calculating the similarity between the visual feature descriptor vectors corresponding to each feature point, measuring the degree of difference between feature vectors by Euclidean distance, and selecting the optimal matching candidate pair based on the similarity ranking result.

[0086] The preliminary matching results are filtered, and the nearest neighbor to second nearest neighbor distance ratio criterion is used for matching verification. When the ratio of the minimum matching distance to the second minimum matching distance is less than a set threshold, a valid matching point pair is obtained.

[0087] The effective matching point pairs are optimized using a random consistency sampling algorithm. The reprojection error of each matching point pair is calculated based on geometric constraints. Mismatched points are eliminated to obtain the set of visual matching interior points constrained by the geometric model.

[0088] Based on the visually matched inlier set, a geometric constraint model between adjacent environmental image frames is constructed to obtain the essential matrix. The construction process is based on the polar geometric relationship model established by the visual feature point pairs matched in adjacent environmental image frames. The generation of the essential matrix is ​​achieved by using a pose estimation algorithm to solve the relative motion relationship of the visual sensors and obtaining the optimal essential matrix parameters by minimizing the reprojection error of the matched inlier.

[0089] The essential matrix is ​​decomposed by singular value decomposition, and the rotation matrix and translation vector are extracted to form the pose transformation matrix between adjacent image frames.

[0090] The pose transformation matrix between adjacent image frames is continuously accumulated and optimized according to the time series to obtain the visual pose estimation result.

[0091] In this embodiment, obtaining the fused pose information specifically includes:

[0092] The visual pose estimation results are fused with the acceleration and angular velocity data collected by the inertial measurement unit to form a time-aligned multi-source observation dataset. The multi-source observation dataset is generated by synchronizing the timestamps of different sensors and registering and interpolating the data in a unified time coordinate system.

[0093] A fusion state model is established based on a time-aligned multi-source observation dataset. A fusion state vector is defined, and the fusion state vector is predicted in time according to the dynamic equation of the inertial measurement unit. The robot pose change is calculated using acceleration and angular velocity data to obtain the predicted pose and prediction covariance matrix. The establishment of the fusion state model refers to the construction of a mathematical model of the dynamic behavior of the system and the relationship between observations during the information fusion process of the visual sensor and the inertial measurement unit, based on the robot kinematics and sensor observation characteristics. The fusion state vector includes position, velocity, attitude quaternions and accelerometer zero bias and gyroscope zero bias.

[0094] The predicted pose is used as the prior estimation result, and the visual pose estimation result provided by the visual sensor is used as the observation input. A fusion observation equation is established, and the observation residual is formed by calculating the deviation between the prior predicted pose and the visual observation pose.

[0095] Using the extended Kalman filter algorithm, the Kalman gain matrix is ​​calculated based on the predicted covariance matrix and the preset observation noise covariance matrix. The fused state vector and covariance matrix are then updated based on the Kalman gain matrix to obtain the updated fused state vector.

[0096] Extract fused pose information from the updated fused state vector.

[0097] In this embodiment, the construction of the local environment map specifically includes:

[0098] Based on the fused pose information, the pose parameter set of the robot in the continuous time series is extracted, and the pose parameter set includes the position vector and the attitude rotation matrix;

[0099] Based on the pose parameter set, the environmental image frames acquired by the visual sensor are synchronized in time and registered in space with the corresponding fused pose information. A mapping relationship between the environmental image frames and three-dimensional spatial points is established in a unified coordinate system to obtain the three-dimensional point coordinates. The generation of the three-dimensional point coordinates is based on the intrinsic parameters, pixel depth values ​​and pose parameters of the visual sensor, and the pixel coordinates are converted into three-dimensional point coordinates in the world coordinate system.

[0100] The coordinates of the 3D point cloud are aligned and fused. A sparse point cloud reconstruction method based on fused pose information is adopted to generate a preliminary local sparse point cloud model.

[0101] The initial local sparse point cloud model is subjected to noise suppression and redundant point removal. An outlier point removal algorithm based on statistical distance is used to filter out outliers, resulting in an optimized sparse point cloud model.

[0102] A local environment map is constructed based on the optimized sparse point cloud model. The construction process involves indexing and associating the point cloud data of the sparse point cloud model with the fused pose information to form a local environment map containing pose, depth, and spatial feature information.

[0103] In this embodiment, the generation of the global environment map specifically includes:

[0104] Using the local environment map as input, we extract local point cloud data, visual feature information and fused pose information from the local environment map, and establish a keyframe index structure with keyframes as the basic unit.

[0105] Based on the keyframe index structure, spatial constraint relationships between keyframes are established according to the fused pose information between adjacent local environment maps. A pose graph model is constructed using visual simultaneous localization and SLAM algorithms. The pose graph model includes nodes of the fused pose of keyframes and edges of spatial constraints between keyframes.

[0106] The pose graph model is subjected to graph optimization processing, and incremental fusion is performed using a nonlinear least squares optimization method to obtain a keyframe fused pose set. The keyframe fused pose set is obtained by minimizing the fused pose constraint residual between keyframes as the objective function, and the fused pose information of the keyframes is updated.

[0107] Based on the keyframe fusion pose set, the local environment map is spatially aligned, and the local environment map is spatially registered in a unified global coordinate system through a coordinate transformation matrix to generate a global environment map.

[0108] In this embodiment, the generation of the optimal travel path specifically includes:

[0109] A path planning configuration space is established in the global environment map. The establishment process is to perform a safety expansion model of the obstacle area, use the robot pose parameters as state variables, define the feasible motion space in the global coordinate system, and form the path planning configuration space.

[0110] Based on the obstacle distribution information in the global environment map, the distance from each map location point to the obstacle boundary is calculated in the path planning configuration space, and the feasible movement area is divided according to the preset safe distance threshold.

[0111] Discretize the feasible motion region to construct a search graph containing nodes and edges, and define the path cost function of adjacent nodes. The path cost is defined by weighting and summing the distance between nodes, obstacle potential field cost and orientation change cost using a unified weight coefficient.

[0112] A heuristic path search algorithm is used to search for paths in the search graph, and the cost is evaluated through a path cost function. An initial planned path is generated according to the principle of minimum cost.

[0113] The initial planned path is made continuous and the trajectory is optimized to generate a continuous and executable optimal path. The optimal path is a continuous and executable trajectory obtained by minimizing the path cost function, under the premise of satisfying the robot's kinematic constraints, with the optimization objectives of maximizing path smoothness, curvature continuity and obstacle safety distance.

[0114] In this embodiment, the generation and modification of the robot motion control commands specifically includes:

[0115] The optimal travel path is used as input to extract the discrete path point sequence in the optimal travel path. The desired linear velocity and desired angular velocity are calculated based on the spatial coordinate relationship between adjacent path points to generate the desired motion trajectory.

[0116] The robot's current pose is obtained by combining fused pose information, and the pose deviation between the current pose and the desired motion trajectory is calculated. The pose deviation includes a position error vector and an attitude error angle.

[0117] A path tracking control model is established based on pose deviation. A proportional-integral-derivative controller is used to calculate the control increment, dynamically correct the linear velocity and angular velocity parameters of the robot motion control module, and generate real-time control commands.

[0118] Real-time control commands are input into the robot's motion control module to drive the robot to navigate along the optimal path. At the same time, environmental image data and motion state data are collected in real time through the robot's vision sensor and inertial measurement unit to update and fuse pose information.

[0119] Based on the updated fused pose information, pose deviation is continuously calculated. When the pose deviation is detected to be greater than a preset threshold, the local path replanning module is triggered. Based on the global environment map and the current fused pose information, a new local correction path is generated, and motion control commands are updated in real time to adaptively correct the navigation path.

[0120] Example 1:

[0121] The machine vision-based robot navigation and localization method of this invention was verified and its performance evaluated on a machine vision-based mobile robot test platform. The experimental environment was set in a complex indoor scene with an area of ​​20m × 15m, including tables, chairs, filing cabinets, walls, and several movable obstacles, with varying lighting and occlusion interference. The robot was equipped with a forward-facing binocular vision sensor (1920×1080 resolution, 30Hz frame rate) and an inertial measurement unit (IMU, 200Hz sampling frequency). The computing platform was an embedded NVIDIA Xavier NX, and the software environment was based on the ROS2 framework and the algorithm module of this invention. The experiment aimed to verify the performance of this invention in terms of positioning accuracy, navigation, and localization. Figure 1 Performance advantages in terms of consistency and navigation stability.

[0122] In the experiment, the robot first acquired environmental image sequences through a visual sensor and performed preprocessing operations such as noise suppression and brightness equalization. Then, an improved DELF model was used for multi-scale feature extraction and description, generating a stable set of visual feature points. Experimental results showed that the system extracted an average of 830 visual feature points per frame, maintaining a high detection rate even in areas with varying illumination and sparse texture. Next, a pose estimation algorithm based on feature point matching was used to calculate the pose changes between adjacent image frames, and extended Kalman filtering was performed to fuse the acceleration and angular velocity data from the IMU, generating high-precision fused pose information. This process significantly reduced the drift error of pure vision-based localization in dynamic scenes, decreasing the average localization error from 0.084m to 0.031m.

[0123] During the mapping phase, the system generates a local environment map using a sparse point cloud reconstruction method based on fused pose information. Spatial registration and incremental fusion between local maps are achieved through keyframe constraint optimization and pose graph optimization, ultimately constructing a global environment map under a unified reference coordinate system. The generated global point cloud model exhibits high spatial consistency and structural integrity, with a point cloud density of 125 points / m² and an average map error variance of only 0.0048m². In the path planning phase, the system automatically calculates the optimal path from the starting point to the target point (approximately 14.2m) based on the global environment map. It then optimizes the path cost function by combining obstacle distribution and robot kinematic constraints, ultimately generating a smooth and executable path. During navigation, the system monitors pose deviations in real time. When the deviation exceeds 2cm or the attitude angle shift exceeds 2°, a path correction mechanism is automatically triggered for dynamic adjustment.

[0124] In this test scenario, the robot underwent multiple rounds of experiments under three environmental conditions: normal lighting, low illumination, and partial occlusion. The results show that the method of this invention exhibits high robustness and accuracy in complex visual environments, and its overall performance is shown in the table below.

[0125] Table 1 Performance test results of the method of the present invention under different environmental conditions

[0126] Test environment Average positioning error (m) Attitude angle error (°) land Figure 1 Consistency error (m²) Point cloud density (points / m²) Path planning time (ms) Navigation success rate (%) Normal lighting 0.030 0.86 0.0046 128 57 98.9 Low light environment 0.042 1.12 0.0051 122 59 97.8 Partial occlusion 0.047 1.25 0.0054 119 61 96.7

[0127] As shown in Table 1, the method of this invention maintains high-precision pose estimation and stable navigation performance under different lighting and occlusion conditions. Compared with traditional ORB-SLAM and VINS-Mono systems, the method of this invention reduces positioning error by approximately 48% in low-light environments. Figure 1 Consistency error is reduced by approximately 35%, while navigation success rate is improved by approximately 6 percentage points. Its performance advantages are primarily attributed to the improved DELF visual feature extraction module and the extended Kalman filter multi-source fusion mechanism, enabling the system to stably identify features and perform dynamic pose optimization even under complex visual conditions.

[0128] The above experiments demonstrate that this invention achieves closed-loop optimization of the entire process from visual perception, feature matching, information fusion to path planning and control. It possesses technical advantages of high precision, high robustness, and strong real-time performance, effectively enhancing the robot's autonomous navigation capability in complex dynamic environments. It has promising engineering application prospects and promotional value.

Claims

1. A method for robot navigation and localization based on machine vision, characterized in that, The method comprises the following steps: acquiring an environment image sequence through a robot vision sensor and preprocessing; detecting and describing feature points of the preprocessed environment image sequence by using an improved DELF model to generate a visual feature point set; establishing a feature point correspondence relationship based on the visual feature point set by using a feature matching algorithm, calculating a visual sensor pose change parameter according to a matching result, and obtaining a visual pose estimation result by using a feature point matching-based pose estimation algorithm; performing multi-source information fusion on the visual pose estimation result and acceleration data and angular velocity data collected by an inertial measurement unit to obtain fused pose information; constructing a local environment map by using a sparse point cloud reconstruction method according to the fused pose information; generating a global environment map by using a path planning method according to the global environment map; generating robot motion control instructions according to the optimal travel path, controlling the robot to perform navigation movement, and monitoring a robot pose deviation in real time, and adjusting a path and correcting navigation according to feedback information. The preprocessing comprises image denoising, distortion correction, brightness equalization and edge enhancement processing.

2. The robot navigation and positioning method based on machine vision according to claim 1, characterized in that, The generation of the visual feature point set specifically comprises:

3. The robot navigation and localization method based on machine vision according to claim 1, characterized in that, inputting the preprocessed environment image sequence into the improved DELF model to detect and describe feature points, wherein the improved DELF model comprises a multi-scale feature extraction module, a structure-frequency joint attention module and a feature description and encoding module, the multi-scale feature extraction module refers to multi-scale feature extraction of the preprocessed environment image sequence, the structure-frequency joint attention module refers to significant enhancement of key regions in a convolution feature map, and the feature description and encoding module refers to high-dimensional feature description and encoding of detected visual feature points; performing multi-scale feature extraction on the preprocessed environment image sequence by using the multi-scale feature extraction module, constructing a multi-scale environment image pyramid, and generating a convolution feature map, wherein the convolution feature map is generated by performing convolution operation on each scale of environment image and a weight matrix to extract spatial texture and semantic features at the scale; inputting the scale convolution feature map into the structure-frequency joint attention module, and performing joint weighting on feature positions by using structure tensor anisotropy and local frequency energy density to obtain a significant weight; performing a non-maximum suppression operation on the significant weight to extract a visual feature point set with the largest local response from the convolution feature map, wherein the non-maximum suppression operation refers to retaining the strongest feature points from the convolution feature map and removing redundancies; inputting the extracted visual feature point set into the feature description and encoding module, and generating a visual feature description vector by using a deep embedding network, wherein the visual feature description vector is generated by extracting a local neighborhood feature block in the corresponding scale of the convolution feature map with the feature points of the visual feature point set as the center. ​ The coordinates, scales, principal directions and visual feature description vectors of the visual feature point set are combined to form the visual feature point set.

4. The robot navigation and localization method based on machine vision according to claim 1, wherein, The visual pose estimation result is obtained specifically as follows: The visual feature point set is matched, a feature matching algorithm based on descriptor distance is adopted to form a preliminary matching result, the preliminary matching result is formed by one-by-one comparison of the visual feature points in adjacent environment image frames, calculation of the similarity between the visual feature description vectors corresponding to each feature point pair, measurement of the difference between the feature vectors by the Euclidean distance, and selection of the optimal matching candidate pair according to the similarity sorting result; The preliminary matching result is screened, a nearest neighbor and second nearest neighbor distance ratio criterion is adopted for matching verification, and when the ratio of the minimum matching distance to the second minimum matching distance is less than a set threshold, an effective matching point pair is obtained; The effective matching point pair is optimized by a random consistency sampling algorithm, the re-projection error of each matching point pair is calculated based on a geometric constraint condition, the mismatching points are removed, and a visual matching inlier set constrained by a geometric model is obtained; A geometric constraint model between adjacent environment image frames is constructed according to the visual matching inlier set, an essential matrix is obtained, the construction process is to establish an epipolar geometry relationship model based on the matched visual feature point pairs in adjacent environment image frames, and the essential matrix is generated by solving the relative motion relationship of the visual sensor by using a pose estimation algorithm, and the optimal essential matrix parameters are obtained by minimizing the re-projection error of the matching inliers; The essential matrix is decomposed by a singular value decomposition method, a rotation matrix and a translation vector are extracted, and a pose transformation matrix between adjacent image frames is formed; The pose transformation matrix between adjacent image frames is continuously accumulated and optimized according to the time sequence, and a visual pose estimation result is obtained.

5. The robot navigation and localization method based on machine vision according to claim 1, characterized in that, The fusion pose information is obtained specifically as follows: The visual pose estimation result and the acceleration data and angular velocity data collected by the inertial measurement unit are fused to form a time-aligned multi-source observation data set, the multi-source observation data set is generated by synchronizing the time stamps of different sensors and registering and interpolating the data in a unified time coordinate system; A fusion state model is established based on the time-aligned multi-source observation data set, a fusion state vector is defined, the fusion state vector is time-predicted according to the dynamic equation of the inertial measurement unit, the acceleration and angular velocity data are used to calculate the robot pose change, a predicted pose and a predicted covariance matrix are obtained, the fusion state model is established by constructing a mathematical model of the system dynamic behavior and observation relationship according to the robot kinematics and sensor observation characteristics in the information fusion process of the visual sensor and the inertial measurement unit, and the fusion state vector includes position, velocity, attitude quaternion, accelerometer bias and gyroscope bias; The predicted pose is taken as a priori estimation result, the visual pose estimation result provided by the visual sensor is taken as observation input, a fusion observation equation is established, and an observation residual is formed by calculating the deviation between the a priori predicted pose and the visual observation pose; The extended Kalman filtering algorithm is used to calculate a Kalman gain matrix according to a prediction covariance matrix and a preset observation noise covariance matrix, and the fusion state vector and the covariance matrix are updated according to the Kalman gain matrix to obtain an updated fusion state vector; Fusion pose information is extracted from the updated fusion state vector.

6. The robot navigation and localization method based on machine vision according to claim 1, wherein, The construction of the local environment map specifically includes: According to the fusion pose information, a set of pose parameters of the robot at a continuous time sequence is extracted, the set of pose parameters including a position vector and an attitude rotation matrix; Based on the set of pose parameters, the environment image frames collected by the visual sensor and the corresponding fusion pose information are time-synchronized and spatially registered, a mapping relationship between the environment image frames and the three-dimensional space points is established in a unified coordinate system, and three-dimensional point coordinates are obtained, the generation of the three-dimensional point coordinates being based on intrinsic parameters of the visual sensor, pixel depth values and attitude parameters, and the pixel coordinate points being converted into three-dimensional point coordinates in a world coordinate system; The three-dimensional point cloud coordinates are aligned and fused, a sparse point cloud reconstruction method based on the fusion pose information is adopted, and a preliminary local sparse point cloud model is generated; The preliminary local sparse point cloud model is subjected to noise suppression and redundant point elimination, an outlier elimination algorithm based on statistical distance is used to filter out abnormal points, and an optimized sparse point cloud model is obtained; The local environment map is constructed according to the optimized sparse point cloud model, and the construction process is to index and associate the point cloud data of the sparse point cloud model with the fusion pose information, so as to form a local environment map containing pose, depth and spatial feature information.

7. The robot navigation and localization method based on machine vision according to claim 1, wherein, The generation of the global environment map specifically includes: The local environment map is taken as input, local point cloud data, visual feature information and fusion pose information in the local environment map are extracted, and a key frame index structure is established taking a key frame as a basic unit; Based on the key frame index structure, a spatial constraint relationship between key frames is established according to the fusion pose information between adjacent local environment maps, a pose graph model is constructed by using visual simultaneous localization and SLAM algorithm, and the pose graph model includes nodes of key frame fusion poses and edges of spatial constraints between key frames; The pose graph model is subjected to graph optimization processing, an incremental fusion is performed by using a nonlinear least square optimization method, a set of key frame fusion poses is obtained, and the set of key frame fusion poses is obtained by taking minimization of constraint residual errors of the key frame fusion poses as an objective function to update the fusion pose information of the key frames; Based on the set of key frame fusion poses, the local environment maps are spatially aligned, the spatial registration of the local environment maps in a unified global coordinate system is performed by using a coordinate transformation matrix, and a global environment map is generated.

8. The robot navigation and localization method based on machine vision according to claim 1, wherein, The generation of the optimal travel path specifically includes: A path planning configuration space is established in the global environment map, the establishment process being that a safety inflation model is established for an obstacle region, a feasible motion space is defined in a global coordinate system by taking a robot pose parameter as a state variable, and the path planning configuration space is formed; In the path planning configuration space, according to the obstacle distribution information in the global environment map, the distance from each map position point to the obstacle boundary is calculated, and the feasible motion region is divided according to the preset safety distance threshold; Discretization processing is carried out in the feasible motion region, a search graph containing nodes and edges is constructed, and a path cost function of adjacent nodes is defined, wherein the definition of the path cost is weighted summation of the distance between nodes, obstacle potential field cost and heading change cost by using a unified weight coefficient; A heuristic path search algorithm is used to search the path in the search graph, and the cost is evaluated by the path cost function, and the initial planning path is generated according to the principle of minimum cost; The initial planning path is processed by continuous and trajectory optimization to generate a continuous executable optimal travel path, wherein the optimal travel path refers to a continuous executable trajectory obtained by minimizing the path cost function under the premise of meeting the kinematic constraint conditions of the robot, and the optimization objective is to maximize the path smoothness, curvature continuity and obstacle safety distance.

9. The robot navigation and localization method based on machine vision of claim 1, wherein, The generation and correction of the robot motion control instruction specifically include: Taking the optimal travel path as input, extracting the discrete path point sequence in the optimal travel path, calculating the expected linear velocity and expected angular velocity according to the spatial coordinate relationship between adjacent path points, and generating an expected motion trajectory; Combining the fusion pose information to obtain the current pose of the robot, calculating the pose deviation between the current pose and the expected motion trajectory, wherein the pose deviation includes a position error vector and an attitude error angle; Based on the pose deviation, a path tracking control model is established, a proportional-integral-derivative controller is used to calculate the control increment, and the linear velocity and angular velocity parameters of the robot motion control module are dynamically corrected to generate real-time control instructions; The real-time control instructions are input into the robot motion control module to drive the robot to navigate along the optimal travel path, while the robot vision sensor and inertial measurement unit are used to collect environmental image data and motion state data in real time to update the fusion pose information; According to the updated fusion pose information, the pose deviation is continuously calculated, and when the pose deviation is greater than the preset threshold, the local path re-planning module is triggered, a new local correction path is generated according to the global environment map and the current fusion pose information, and the motion control instruction is updated in real time to adaptively correct the navigation path.

Images