A deep camera-based indoor mobile robot dense mapping and autonomous navigation integrated method

Abstract

The application discloses a kind of indoor mobile robot dense mapping and autonomous navigation method based on depth camera, belong to robot simultaneous localization and mapping, robot navigation field.The application uses depth camera and based on interframe matching to image ORB feature to realize real-time positioning to robot, by fusing color image and depth image, and in order to eliminate redundant video frame, key frame extraction method in space domain is introduced, real-time dense three-dimensional point cloud map construction is realized, and it is converted into octree map format and grid map format suitable for navigation, then by ROS Navigation function package, it is combined with existing mobile robot navigation method, realizes indoor mobile robot autonomous navigation scheme based on pure vision scheme.

Description

Technical Field

[0001] This invention belongs to the fields of simultaneous localization and mapping of robots and robot navigation, specifically involving an integrated method for dense mapping and autonomous navigation of indoor mobile robots based on a depth camera. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) refers to the process by which a mobile robot, without prior environmental information, uses its onboard sensors to build a map model of its environment while simultaneously estimating its own motion. SLAM encompasses both localization and mapping problems and is considered one of the key issues in achieving robot autonomy, holding significant research importance for robot navigation, control, and task planning.

[0003] Depending on the sensor chosen, SLAM can be broadly categorized into LiDAR-based SLAM and camera-based visual SLAM. LiDAR SLAM is already quite mature, but the high cost and maintenance difficulties of LiDAR hardware contribute to its high overall cost. In contrast, visual SLAM solutions have lower hardware costs and are easier to use, gradually becoming the mainstream research direction in SLAM.

[0004] Existing SLAM systems mostly focus on the localization problem, including localization via feature points, direct localization methods, and backend optimization, with less emphasis on mapping. The sparse feature point maps created primarily serve the localization task. However, in practical applications, maps serve many other purposes beyond localization, such as path planning (navigation) and obstacle avoidance for robots. It's necessary to inform the robot which areas on the map are passable and which are impassable, which exceeds the capabilities of sparse feature point maps. A dense map is required; furthermore, integrating this dense map with existing mobile robot path planning schemes is a crucial problem that needs to be solved. Summary of the Invention

[0005] This invention provides an integrated method for dense mapping and autonomous navigation of indoor mobile robots based on depth cameras, which solves the problem of dense mapping of the surrounding environment of indoor mobile robots. At the same time, it is combined with the path planning function of mobile robots to realize the integrated realization of autonomous navigation of indoor mobile robots based on pure vision.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for integrating dense mapping and autonomous navigation of indoor mobile robots based on depth cameras includes the following steps:

[0008] 1) Establish a SLAM scheme for mobile robot localization and sparse mapping based on ORB features;

[0009] 2) Construct a dense map suitable for autonomous navigation of mobile robots and convert it to a grid map by using the keyframes selected in step 1).

[0010] 3) Achieve integrated dense mapping and autonomous navigation for mobile robots based on ROS.

[0011] In the steps described above, step 1) of mobile robot localization and sparse mapping based on ORB features includes the following steps:

[0012] (a) Extracting ORB features from the image

[0013] ORB features are extracted from the color image stream input from the depth camera. These extracted ORB features are used as landmarks throughout the entire SLAM system process. Then, feature matching is performed on the landmark points instead of processing the entire image to speed up the process.

[0014] (b) Tracking and localization based on ORB feature points

[0015] ORB features are extracted and matched from consecutive image frames, and the color image and depth image are fused to obtain the depth and 3D coordinates of ORB map points. The camera pose of subsequent images is estimated by tracking motion model, reference keyframes and relocalization. The pose of the current frame is optimized by minimizing reprojection error. Then, it is determined whether to generate a new keyframe according to preset conditions.

[0016] (c) Local mapping for positioning purposes

[0017] The keyframes created in the previous step are processed, the correspondence between map points and keyframes is updated, newly added map points that are rarely observed are removed, and then map points are recovered from keyframes with high co-visibility through triangulation. Duplicate map points between keyframes and adjacent keyframes are checked. When all keyframes in the keyframe queue have been processed, local bundle adjustment is performed on the current keyframe, adjacent keyframes and observed map points, and the keyframe pose and map point accuracy are optimized by minimizing reprojection error.

[0018] (d) Loop closure detection for map relocation

[0019] The processing of keyframes inserted during local mapping mainly involves three steps: loop closure detection, calculation of the similarity transformation matrix, and loop closure correction. Loop closure detection selects candidate keyframes by calculating bag-of-words similarity scores. Then, a similarity transformation matrix is ​​calculated for each candidate keyframe, and the optimal keyframe is selected through random sampling consistency. Next, the keyframe pose is optimized using the essential graph. Finally, global bundle adjustment is performed to obtain a globally consistent environment map and camera trajectory. The optimized pose formula is as follows:

[0020]

[0021] Where e i，j for:

[0022]

[0023] Among them, X i，j Is position X w，j To X w，i Similarity transformation, log sim(3) Mapping the pose error of the transfer matrix to 7-dimensional Euclidean space, Δ i，j Let be the information matrix of the edges.

[0024] Step 2) is achieved by performing 3D dense mapping on the keyframes selected in Step 1). Specifically, this includes the following steps: fusing the color map and depth map of the keyframes to calculate the spatial coordinates of the 3D point cloud in the world coordinate system. The calculation formula is as follows:

[0025]

[0026] Among them, f x ,f y ,c x ,c y Here, (u, v) represents the camera intrinsic parameters, (x, y, z) represents the image coordinates, (x, y, z) represents the image coordinate system coordinates (i.e., the camera pose of the current keyframe), d represents the distance to the pixel measured by the depth camera in millimeters, and s represents the scaling factor between the actual distance and the measured depth d. The transformation formula from the camera coordinate system point cloud to the global coordinate system point cloud is as follows:

[0027]

[0028] in, Let i be the pose of the i-th keyframe. For the point cloud in the coordinate system of the i-th keyframe, W w，j It is the point cloud obtained in the global coordinate system after transformation;

[0029] Then, an outlier filter and a downsampling filter are used to filter the 3D point cloud to remove outliers and duplicate points. The method is as follows: a frame is considered a keyframe only when the camera has moved a certain amount in adjacent frames, and its point cloud is superimposed onto the existing map. The calculation formula is as follows:

[0030] min_norm≤||Δt||+min(2π-||r||, ||r||)≤max_norm (5)

[0031] Where Δt is the displacement vector of the camera between adjacent frames; r is the rotation angle of the camera between adjacent frames, and their norm sum is used to describe the magnitude of the camera motion; min_norm is the minimum motion of the camera, and max_norm is the maximum motion of the camera. That is, when the motion is greater than max_norm, it is considered that the camera pose estimation is wrong and the frame is discarded.

[0032] Finally, the ICP method is used to register the point clouds between the two keyframes and stitch the point cloud maps to construct a dense map. The Octomap package is used to compress the dense point cloud map and filter out ground information to convert it into an octree map format with less resource consumption. Then, in order to realize the integrated autonomous navigation function, the octree map is converted into an occupied raster map format through 2D projection.

[0033] Step 3) This is achieved by calling ROS Navigation. The specific process is as follows:

[0034] First, the entire SLAM system from steps 1) and 2) is encapsulated into a ROS node, and the constructed grid map is published under the ` / projected_map` topic. Second, the `costmap_2d` package in the ROS Navigation package is used to subscribe to the grid map topic, and a global cost map (`costmap`) is generated based on the grid map. Then, the `move_base` package is used to generate a global planning path based on the costmap using the A* algorithm and is published under the `nav_msgs / Path` topic format. Finally, the `base_controller` package calculates the speed commands required by the differential chassis motors based on the generated path and publishes them to the underlying motors under the `cmd_vel` topic format to drive the mobile robot and achieve autonomous navigation.

[0035] Beneficial Effects: This invention provides an integrated method for dense mapping and autonomous navigation of indoor mobile robots based on a depth camera. It designs an indoor mobile robot localization and dense mapping system based on ORB features. Real-time localization of the mobile robot is achieved through inter-frame tracking of ORB features, confirming the robot's real-time pose. Dense point clouds of each keyframe are obtained by fusing color and depth images from the depth camera, constructing a dense map of the scene. Simultaneously, a keyframe extraction method in the spatial domain is introduced to eliminate redundant video frames, and the mapping effect is optimized for downstream path planning tasks. A dense map of the indoor scene is constructed and converted into a smaller octree map format and an occupied grid map format that can be directly used by the move_base path planning package. Finally, the proposed dense mapping method is combined with the ROS Navitation package to integrate dense mapping and autonomous navigation, realizing an autonomous navigation scheme for indoor mobile robots based on a pure vision approach. Attached Figure Description

[0036] Figure 1 This is a flowchart of image ORB feature extraction in an embodiment of the present invention;

[0037] Figure 2 This is a diagram showing the actual effect of ORB feature extraction in an embodiment of the present invention;

[0038] Figure 3 This is a flowchart of the tracking and localization process based on ORB feature points in an embodiment of the present invention;

[0039] Figure 4 This is a flowchart illustrating the local mapping process for positioning in an embodiment of the present invention.

[0040] Figure 5 This is a partial mapping effect in an embodiment of the present invention;

[0041] Figure 6 This is a flowchart of the closed-loop detection process for map relocation in an embodiment of the present invention;

[0042] Figure 7 This invention optimizes the closed-loop detection pose graph in this embodiment.

[0043] Figure 8 This is a structural diagram of the Interl Realsense D435i camera in an embodiment of the present invention;

[0044] Figure 9 This is the dense mapping effect of the laboratory site in an embodiment of the present invention;

[0045] Figure 10 This is the Octomap data format used in this embodiment of the invention;

[0046] Figure 11 This is a diagram illustrating the conversion of dense map construction into an octree map in an embodiment of the present invention.

[0047] Figure 12 This is the effect of converting an octree map into an occupying grid map in an embodiment of the present invention;

[0048] Figure 13 This is an experimental planning path diagram in an embodiment of the present invention;

[0049] Figure 14 This is a block diagram of a real-time localization and dense mapping system based on ORB features in an embodiment of the present invention;

[0050] Figure 15 This is a schematic diagram of autonomous navigation of a mobile robot in an embodiment of the present invention;

[0051] Figure 16 This is a block diagram of the integrated system for dense mapping and autonomous navigation of mobile robots in an embodiment of the present invention. Detailed Implementation

[0052] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments:

[0053] A method for integrating dense mapping and autonomous navigation of indoor mobile robots based on depth cameras includes the following steps:

[0054] 1. Construct a visual SLAM solution for mobile robots based on ORB features.

[0055] 1.1 Image ORB Feature Extraction

[0056] ORB features consist of two parts: "Oriented FAST" keypoints and "Rotated BRIEF" descriptors. To address the weakness of FAST corners lacking directionality and scale, ORB adds scale and rotation descriptions. Scale invariance is achieved by constructing an image pyramid and detecting corners at each level of the pyramid, while feature rotation is achieved using the gray-level centroid method, the specific calculation method of which is as follows... Figure 1 As shown in the flowchart;

[0057] After constructing the image pyramid, FAST corner points are extracted layer by layer, and octree filtering is used to select feature points evenly distributed on the image. Then, the principal direction and BRIEF descriptor of each feature point are calculated. The FAST feature point extraction method is as follows: select pixel p in the image, set the brightness as Ip, use 20% of Ip as the threshold T, and select several pixels on the circle with pixel p as the center and set the radius according to the requirements. If at least 11 consecutive points on the circle have a brightness greater than Ip+T or less than Ip-T, then pixel p is a feature point. The principal direction of the feature point is calculated by the gray-level centroid method. First, the moments of the image are calculated:

[0058]

[0059] Where (x, y) are the pixel coordinates of the target point, I(x, y) is the grayscale value of the current pixel, p and q are 0 or 1, and m pq The moments of the image, within a circular image region of radius R, are as follows along the x and y axes:

[0060]

[0061] m 00 It is the total grayscale value of all pixels within the circular region. The centroid of the image is found based on the image's moments.

[0062]

[0063] Feature point orientation and vector With the same direction, the rotation angle of the key point is recorded as:

[0064]

[0065] The BRIEF descriptor is a binary-coded descriptor that uses 256 bits to describe each feature point. Specifically, it is calculated by comparing the size relationships of 256 randomly selected pixel pairs (p, q) within an image window p near the feature point, and then assigning binary values ​​as follows.

[0066]

[0067] In the formula, p(x) represents the grayscale value of pixel x within window p. The actual ORB extraction effect is as follows: Figure 2 As shown, the extracted ORB features are represented by an outer rectangle with a radius of 16 pixels and a central feature point. The extracted feature points are mostly located at the edges of objects in the image and obvious contour edges, which are sensitive to pixel changes, facilitating subsequent tracking and localization based on ORB feature points.

[0068] 1.2 Tracking and Localization Based on ORB Feature Points

[0069] like Figure 3 As shown, after extracting ORB features from the input image, if the number of extracted feature points is greater than 500, the SLAM system is initialized to load the BOW bag-of-words model, initialize the camera pose, and create an initial map.

[0070] After initializing the SLAM system, the camera pose is estimated based on the received image. Then, the local map is tracked based on the estimated initial pose, and the pose is further optimized. There are three methods for initial pose estimation:

[0071] (a) Estimating pose based on a constant velocity motion model:

[0072] Assuming the motion speed is constant across several consecutive frames, the estimated pose is calculated based on the motion speed and the camera pose of the previous frame. After map initialization, the motion speed V can be obtained from the first two keyframes.

[0073] V = T cl =T cw T wl (II)

[0074] In the formula, c represents current, i.e., the current frame; l represents last, i.e., the previous frame; and w represents world, i.e., the world coordinate system. When a new frame enters, it is assumed that the pose changes at a constant speed, meaning that the pose transformation between the previous frame and the latest frame is the same as the pose transformation between the frame before that and the previous frame. Therefore, the pose of the current frame in the world coordinate system can be estimated:

[0075] T cw VT lw (12) Of course, since the robot does not move at a constant speed, the camera pose estimated by the constant speed motion model needs to be optimized using Motion-based BA. The 3D map points generated in the previous frame are projected onto the pixel coordinates of the current frame image. The feature points that match between the two frames are determined by matching the Hamming distance of the descriptor near the projection points. Then, using the 3D-2D projection relationship, the least squares problem is constructed using the G2O graph optimization library. The Levenberg-Marquardt method is used to optimize the pose of the current frame. The specific process is as follows: First, construct the G2o optimizer. Second, add the pose of the current frame to be optimized as the vertex of the graph. Then, add the difference between the observed feature point coordinates and the projection of the map points in the current frame as a unary edge to the graph. Connect the vertex and the edge. Then, call the LM method to iteratively optimize the reprojection error. Finally, update the pose of the current frame with the optimized pose.

[0076] (b) Estimate pose based on reference frame:

[0077] When the constant velocity model fails to track, a reference keyframe is used for pose estimation. First, the BRIEF descriptor of the current frame is converted into a bag-of-words (BOW) vector. Then, the frame with the closest Hamming distance to the BOW vector is found from the keyframes. The corresponding keyframe is then used for pose optimization, similar to the constant velocity model. The calculation process for the bag-of-words vector is as follows: For the BRIEF descriptor of a feature point, a breadth-first traversal is performed from top to bottom in a pre-trained offline bag-of-words trie to find its position. Starting from the root node, the Hamming distance is calculated between the descriptor and the descriptors of each node. The node with the smallest Hamming distance is selected as the current node, and this process continues until a leaf node is reached. Finally, the word ID and weight attributes of the leaf node are assigned to this feature point as its bag-of-words vector.

[0078] (c) Estimating pose through repositioning:

[0079] When both current pose estimation methods fail, a relocalization method is used to estimate the pose. The specific process is as follows: First, calculate the bag-of-words vector of the feature points in the current frame. Use the bag-of-words vector to find candidate keyframes similar to the current frame for relocalization. Then, traverse all candidate keyframes and perform fast matching using the bag-of-words vector to obtain matching 2D point pairs. Then, use EPnP iteration to obtain a rough camera pose estimate. Finally, construct a least squares problem with the obtained rough pose and point pairs, and use the method in the constant velocity motion model to estimate the optimized pose.

[0080] After successfully estimating the initial pose of the current frame, the local map is updated based on the current pose, and the pose of the current frame is optimized. The main process is as follows:

[0081] (1) Update the common view and local map, including local keyframes and local map points;

[0082] (2) Project local map points onto the current frame and discard map points that cross the boundary;

[0083] (3) Optimize the pose of the local map points and the feature points of the current frame to optimize the pose of the current frame;

[0084] (4) Update the map point observation values, count the number of points in the area and use this to determine whether the tracking was successful;

[0085] The method for selecting keyframes is as follows: determine whether to filter a keyframe based on the pose of the current frame. If the relative motion distance between the current frame and the previous keyframe is greater than a threshold, the frame is discarded. If the number of feature points matched between the current frame and the previous keyframe is less than a threshold, the frame is discarded. Keyframes are finally selected based on these conditions.

[0086] 1.3 Local mapping for positioning purposes

[0087] After the keyframes are selected, the loop closure detection and dense mapping steps are all based on the selected keyframes to save computing resources. After inserting the keyframes, the bag-of-words vectors and co-view information of the keyframes are calculated, and the camera pose of the current keyframe is added to the map.

[0088] During the process of continuously processing keyframes to build a local map, redundant bad map points need to be removed. If the number of frames in which a map point is tracked is less than 25% of the expected number of frames in which it can be observed, it is deleted from the map. Alternatively, if a map point cannot be observed simultaneously in the created keyframe and its two adjacent keyframes, it is considered a bad map point and deleted. If a map point remains intact after three consecutive keyframes, it is considered a good map point.

[0089] After removing bad map points, based on the keyframes in the constructed map, the current keyframe is paired with the top 10 co-sighted keyframes with the highest co-sight, and new map points are generated through triangulation to compensate for the negative impact of insufficient map points on tracking after removal. Then, local BA optimization is performed on the keyframe, its formula keyframe, and map points, while optimizing the poses of keyframes and map points in the map. During the construction of the local map, map points and keyframes are redundant, so they need to be further screened more rigorously. Map points that meet the following conditions will be removed: 1) These points cannot be tracked and matched in subsequent image frames; 2) The projection rays are at low disparity points calculated by triangulation; 3) The reprojection error generated by the triangulation measurement points is large. This strict map point screening process ensures the robustness of the constructed map. To ensure the simplicity of the reconstructed map and reduce the complexity of the BA process, redundant keyframes are detected and removed. In the keyframe set, if 90% of the map points in a keyframe are detected in at least three other keyframes, then that keyframe is discarded. The final locally sparse mapping effect is as follows: Figure 5 As shown, the rectangle represents the camera pose during the tracking and positioning process, and the points represent the landmark points observed by the camera in each pose.

[0090] 1.4 Loop Closure Detection for Map Relocalization

[0091] Loop closure detection is used to reduce the cumulative error of pose tracking and for map relocalization. Its process is as follows: Figure 6 As shown, the loop closure detection process is similar to the local mapping process, both involving processing the keyframes extracted during tracking. The first step in loop closure detection is to determine whether a loop has occurred. If four consecutive keyframes can be matched with corresponding loop closure keyframe groups in the database, and these matching keyframe groups are consecutive, then a loop is considered to have occurred. The specific process is as follows:

[0092] (1) Find the closed loop candidate keyframes of the current keyframe. The closed loop candidate keyframes are taken from the keyframes that have the same BOW vector as the current keyframe but are not directly connected.

[0093] (2) Combine the closed-loop candidate keyframes with their co-view keyframes to form a candidate keyframe group;

[0094] (3) Find the continuity relationship between the current key frame group and the previous consecutive key frame groups. If the current key frame group finds a continuity relationship in the previous consecutive key frame group, the continuity length of the current consecutive key frame group is increased by 1. The continuity relationship of key frame groups refers to whether there are key frames in the two key frame groups at the same time. If the continuity length of a key frame group reaches 3, it is considered that the key frame has closed loop.

[0095] After determining the occurrence of a loop based on bag-of-words vector matching, a Sim3 transformation is required. This involves fusing the pose of the current keyframe with the matched keyframe to achieve loop closure, projecting the map points of the closed keyframe onto the current keyframe, and then performing essential graph optimization.

[0096] Essential graph optimization aims to reduce accumulated errors in visual odometry by effectively closing loops based on the interrelationships of keyframes. This optimizes the pose on the essential view, distributing accumulated errors across the pose graph. Scale offsets are corrected through similarity transformations, and after loop closure detection to optimize the pose, the map point cloud is corrected based on the optimized keyframes. First, the similarity between the bag of words of a keyframe and nearby keyframes in its data association view is calculated. Keyframes with low similarity are identified, and keyframes with similarity to keyframe K are removed. i Directly connected keyframes eventually yield closed loops. Then, when the number of closed loops reaches a certain level, the loop pose graph is optimized using a general graph optimization (G2O) framework, such as... Figure 7 As shown, the final map points are updated, duplicate map points are merged, and the pose graph optimization formula is as follows:

[0097]

[0098] Where e i，j The reprojection error to be optimized:

[0099]

[0100] Among them, X i，j Is position X w，j To X w，i Similarity transformation, log sim (3) Map the pose error of the transfer matrix to 7-dimensional Euclidean space, Δ i，j Let be the information matrix of the edges.

[0101] 2. Dense map construction and raster map conversion suitable for autonomous navigation of mobile robots

[0102] 2.1 Dense Point Cloud Map Construction

[0103] In step 1, the constructed map is a 3D sparse point cloud map, which cannot be applied in practical robotics fields such as robot navigation and path planning. To overcome this shortcoming, this invention proposes to use the Intel RealSense D435i depth camera as a sensor device to construct a 3D dense map. If the point cloud of each frame is fused into the map, the map capacity will be very large, thereby reducing the real-time performance of the system. Since adjacent image frames are continuous during robot movement, that is, the pose of adjacent frames changes little in space, a keyframe extraction method in the spatial domain is proposed to further filter suitable image frames from the keyframes in step 1.

[0104] The Intel Realsense D435i camera integrates monocular, binocular, and RGBD cameras with an inertial measurement unit (IMU). Its depth sensor consists of an infrared emitter and an infrared receiver. Figure 8 As shown. A color camera can obtain the RGB values ​​of each pixel, i.e., a color image, while a depth sensor can measure the distance information of each pixel, i.e., a depth image. The 3D point cloud is calculated by fusing the color image and the depth image, using the following formula:

[0105]

[0106] Among them, f x ,f y ,c x ,c y Here, (u, v) represents the camera intrinsic parameters, (x, y, z) represents the image coordinates, (x, y, z) represents the image coordinate system coordinates (i.e., the camera pose of the current keyframe), d represents the distance to the pixel measured by the depth camera in millimeters, and s represents the scaling factor between the actual distance and the measured depth d, which is 1000. The transformation formula from the camera coordinate system point cloud to the global coordinate system point cloud is as follows:

[0107]

[0108] in, Let i be the pose of the i-th keyframe. For the point cloud in the coordinate system of the i-th keyframe, X w，jThis is the point cloud obtained after transformation in the global coordinate system. To avoid redundancy in the 3D point cloud and unnecessary computation, a spatial domain-based method is used to extract keyframes. The idea is that a frame is considered a keyframe only when the camera has moved a certain amount in adjacent frames, and its point cloud is superimposed onto the existing map. The calculation formula is as follows:

[0109] min_norm≤||Δt||+min(2π-||r||, ||r||)≤max_norm (17)

[0110] Where Δt is the displacement vector of the camera between adjacent frames; r is the rotation angle of the camera between adjacent frames, and their norms are used to describe the magnitude of the camera motion; min_norm is the minimum motion of the camera, and max_norm is the maximum motion of the camera. That is, when the motion is greater than max_norm, it is considered that the camera pose estimation is wrong and the frame is discarded. In this embodiment, the values ​​are 0.4 and 5, respectively.

[0111] 2.2 Raster Map Format Conversion

[0112] Since dense point cloud maps cannot be directly used for mobile robot navigation, they need to be converted into occupancy grid maps suitable for path planning in planar mobile robots. This process will be implemented using the Octomap package. Octomap is a new map representation based on dense point clouds. Compared to point clouds, it has the advantages of small footprint and easy updating, making it suitable for robot navigation. Octomap constructs an octree structure by dividing a cubic space into eight smaller cubes. The larger cube is called the parent node, and the smaller cubes are called child nodes. The octree can be continuously expanded downwards until the minimum resolution is reached, called a leaf node, such as... Figure 9 As shown, Octomap uses probability to describe the occupancy status of nodes. A probability greater than 0.5 indicates occupation, and less than 0.5 indicates unoccupied. This method of using probability to represent occupancy information allows for convenient updates to node states based on sensor data. Furthermore, when all child nodes have the same state, all child nodes can be deleted, and only the parent node represents the spatial state, significantly saving storage space. Octomap uses an octree format instead of point clouds to represent information in 3D space, greatly compressing the data volume.

[0113] The actual dense mapping effect in the laboratory setting is as follows: Figure 10 As shown, the effect of compressing and converting the laboratory's 3D dense map into an octree map is as follows: Figure 11 As shown, further, by projecting Octomap onto a 2D plane, Octomap is transformed into a 2D occupied raster map, such as... Figure 12 As shown, this allows for direct use during path planning by the planar mobile robot.

[0114] 3. Integrated implementation of dense mapping and autonomous navigation for mobile robots based on ROS (e.g.) Figure 16 (As shown)

[0115] ROS, as the most popular robot system framework, provides a powerful general framework, enabling the integrated implementation of various functions. First, the dense mapping process is encapsulated into a ROS node, and the final grid map is published as ` / projected_map / rostopic`. The map is saved and reused through the `map_server` package. Autonomous navigation is implemented through the ROS Navigation package, which is designed specifically for 2D planar differential motion robots. The entire package is centered around `move_base`, which receives odometry information, sensor information, localization information, map, and target points as input. After planning, `move_base` outputs velocity commands. `move_base` consists of three key parts: global planner, local planner, and recovery behaviors. These three parts are implemented as plugins, allowing for easy switching between planners with different algorithms. Recovery behaviors are triggered when the robot encounters an abnormal state during movement, aiming to help the robot overcome this abnormal situation. Additionally, `move_base` includes `global_costmap` and `local_costmap`. The planner needs to perform navigation planning on the cost map. It uses the `costmap_2d` package to subscribe to the published ` / projeed_map` grid map topic to generate the global cost map. Then, it uses the `global_planner` package to read the cost map, performs planning using the A* algorithm, and publishes the planned path in the `nav_msgs / Path` topic. Finally, the `base_controller` package subscribes to the path, calculates the required output speed for the motor, and publishes it in the ` / cmd_vel` topic. This is then transmitted to the underlying differential motor via the driver to complete the motion. The actual motion trajectory in this case is as follows: Figure 13 As shown.

[0116] In summary, the indoor mobile robot dense mapping and autonomous navigation method based on depth camera designed in this invention performed well in the experiment. It can effectively locate the mobile robot in real time and reconstruct the surrounding three-dimensional environment, and convert it into a grid map to serve the autonomous navigation system.

[0117] The above are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. For those skilled in the art, within their knowledge and without departing from the principles of the present invention, several improvements can be made to the present invention, and these are also considered to be within the scope of protection of the present invention.

Claims

1. A method for integrating dense mapping and autonomous navigation of an indoor mobile robot based on a depth camera, characterized in that, Includes the following steps: 1) Establish a SLAM scheme for mobile robot localization and sparse mapping based on ORB features; 2) Construct a dense map suitable for autonomous navigation of mobile robots and convert it to a raster map by using the keyframes selected in step 1). Specifically, the steps include: fusing the color image and depth image of the keyframes to calculate the spatial coordinates of the 3D point cloud in the world coordinate system, using the following formula: ， in, , , , For camera internal parameters, For image coordinates, These are the coordinates in the image coordinate system, i.e., the camera pose of the current keyframe. The distance between pixels measured by the depth camera, in millimeters. For actual distance and measured depth The scaling factor between them, and the transformation formula from the camera coordinate system point cloud to the global coordinate system point cloud, are as follows: ， in, For the first Keyframe pose In the first Point cloud on a keyframe coordinate system It is the point cloud obtained in the global coordinate system after transformation; Then, an exterior point filter and a downsampling filter are used to filter the 3D point cloud to remove exterior points and duplicate points; Finally, the ICP method is used to register the point clouds between the two keyframes and stitch the point cloud maps to construct a dense map. The Octomap package is used to compress the dense point cloud map and filter out ground information to convert it into an octree map format with less resource consumption. The octree map is then converted into an occupied raster map format through 2D projection. 3) Encapsulate the entire SLAM system from steps 1) and 2) into a ROS node. Publish the constructed grid map using the ` / projected_map` topic. Subscribe to the grid map topic using the `costmap_2d` package in the ROS Navigation package. Generate a global cost map (`costmap`) based on the grid map. Then, use the `move_base` package to generate a global planning path using the A* algorithm based on the costmap and publish it in the `nav_msgs / Path` topic format. Finally, the `base_controller` package calculates the speed commands required by the differential chassis motors based on the generated path and publishes them to the underlying motors in the `cmd_vel` topic format to drive the mobile robot's movement, thus achieving the integration of dense mapping and autonomous navigation for the ROS-based mobile robot.

2. The method for integrated dense mapping and autonomous navigation of indoor mobile robots based on depth cameras according to claim 1, characterized in that, Step 1) of mobile robot localization and sparse mapping based on ORB features includes the following steps: (a) Extracting ORB features from the image ORB features are extracted from the color image stream input from the depth camera. The extracted ORB features are used as landmarks throughout the entire SLAM system process. Then, feature matching is performed on the landmark points instead of processing the entire image to speed up the process. (b) Tracking and localization based on ORB feature points ORB features are extracted and matched from consecutive image frames, and the color image and depth image are fused to obtain the depth and 3D coordinates of ORB map points. The camera pose of subsequent images is estimated by tracking motion model, reference keyframes and relocalization. The pose of the current frame is optimized by minimizing reprojection error. Then, it is determined whether to generate a new keyframe according to preset conditions. (c) Local mapping for positioning purposes Process the keyframes created in the previous step, update the correspondence between map points and keyframes, remove newly added map points that are rarely observed, then restore map points for keyframes with high co-visibility through triangulation, check for duplicate map points between keyframes and adjacent keyframes, and when all keyframes in the keyframe queue have been processed, perform local bundle adjustment on the current keyframe, adjacent keyframes and observed map points, and optimize keyframe pose and map point accuracy through minimum reprojection error. (d) Loop closure detection for map relocation Processing keyframes inserted during local mapping mainly involves three steps: loop closure detection, calculation of similarity transformation matrix, and loop closure correction.

3. The method for integrated dense mapping and autonomous navigation of indoor mobile robots based on depth cameras according to claim 2, characterized in that, The specific process for handling keyframes inserted during local mapping is as follows: Loop closure detection selects candidate keyframes by calculating bag-of-words similarity scores, then calculates the similarity transformation matrix for each candidate keyframe, selects the optimal keyframe through random sampling consistency, optimizes the keyframe pose using the essential graph, and finally performs global bundle adjustment to obtain a globally consistent environment map and camera trajectory. The pose optimization formula is as follows: ， in for: ， in, It is a position arrive Similarity transformation, Map the pose error of the transfer matrix to a 7-dimensional Euclidean space. Let be the information matrix of the edges.

4. The method for integrated dense mapping and autonomous navigation of indoor mobile robots based on depth cameras according to claim 1, characterized in that, The method for filtering outliers and duplicate points in a 3D point cloud using an outlier filter and a downsampling filter is as follows: a frame is considered a keyframe only when the camera has moved a certain amount in adjacent frames, and its point cloud is superimposed onto the existing map. The calculation formula is as follows: ， in, This is the displacement vector of the camera between adjacent frames; Let the camera rotation angle between adjacent frames be the norm sum of the angles between frames, and let the magnitude of the camera motion be described by the norm sum of the angles between frames. For the minimum movement of the camera, The maximum motion of the camera, i.e., when the motion is greater than... If the frame is incorrect, it is considered to be due to a camera pose estimation error and the frame is discarded.

5. The method for integrated dense mapping and autonomous navigation of indoor mobile robots based on depth cameras according to claim 1, characterized in that, The Octomap package is used to compress dense point cloud maps and filter out ground information to convert them into a more resource-efficient octree map format. The specific process is as follows: Octomap divides a cubic space into eight smaller cubes to construct an octree structure. The larger cube is called the parent node, and the smaller cubes are called child nodes. The octree continues to expand downwards until it reaches the minimum resolution. When all child nodes have the same state, all child nodes can be deleted, and only the parent node is used to represent the spatial state.

6. The method for integrated dense mapping and autonomous navigation of indoor mobile robots based on depth cameras according to claim 5, characterized in that, Octomap uses probabilities to describe the occupancy status of nodes; a probability greater than 0.5 indicates that a node is occupied, and a probability less than 0.5 indicates that it is not occupied.

Images