A method for vision navigation of an indoor mobile robot

Abstract

The application discloses an indoor mobile robot vision navigation method, comprising the following steps: scanning the environment by a laser radar to obtain point cloud information, constructing a two-dimensional plane map, scanning objects in the environment by a depth camera to obtain semantic information; selecting a point cloud area corresponding to the semantic information, segmenting the point cloud area by using a RANSAC algorithm to obtain a plane and a cylindrical surface; fitting the point cloud information, extracting the plane after fitting, fitting a three-dimensional model to obtain three-dimensional object coordinates; converting the three-dimensional object coordinates into a robot coordinate system and then into a two-dimensional plane map to obtain an enhanced semantic map; and sending a destination to the mobile robot to plan a path. The indoor mobile robot vision navigation method provided by the application can accurately construct a real-time semantic map, is beneficial to a robot to make a deeper understanding of the environment, can reduce the data amount of point cloud information of the semantic map, and improves the object recognition accuracy in the environment.

Description

Technical Field

[0001] This invention relates to a visual navigation method for indoor mobile robots, belonging to the field of robot navigation. Background Technology

[0002] With the rapid development of robotics technology, mobile robots are finding increasingly wider applications in our lives. Especially in indoor settings, mobile robots are gradually replacing complex tasks in factories, homes, and other indoor environments, thanks to the booming automation industry and rising living standards. For example, in warehouses, mobile robots can transport goods according to instructions and successfully reach their destinations; in school buildings with multiple teachers' offices on each floor, teachers can use mobile robots to transfer documents or items, saving them time to handle other important tasks.

[0003] Currently, most indoor mobile robot mapping methods are general mapping methods that lack semantic information, preventing robots from gaining a deeper understanding of their environment. Furthermore, the large amount of point cloud data required for semantic map creation, after conversion to a world coordinate system and placement on the map, results in low efficiency of semantic mapping algorithms, slow map search speeds, and low accuracy in object recognition. Finally, the reliance on a single local path planning method leads to redundancy in robot movement, limiting human-robot interaction and autonomous functions. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to overcome the defects of the prior art and provide a visual navigation method for indoor mobile robots that can accurately construct real-time semantic maps, which is conducive to the robot's deeper understanding of the environment. At the same time, it can reduce the amount of point cloud information in the semantic map and improve the accuracy of object recognition in the environment.

[0005] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0006] A visual navigation method for indoor mobile robots includes the following steps:

[0007] The system controls the movement of a mobile robot, scans the environment using a lidar on the robot to obtain point cloud information, constructs a two-dimensional map based on the point cloud information, and scans objects in the environment using a depth camera on the robot to obtain semantic information of target objects in the environment.

[0008] Select the point cloud region corresponding to the semantic information of the target object in the two-dimensional planar map, and use the RANSAC algorithm to segment the corresponding point cloud region to obtain the planar and cylindrical surfaces of the point cloud region corresponding to the target object.

[0009] The point cloud information of the plane and the cylindrical surface of the point cloud region corresponding to the target object is fitted. After the plane is fitted and extracted, a three-dimensional model is fitted. The two-dimensional coordinates of the point cloud region corresponding to the target object are expanded into three-dimensional coordinates to obtain the three-dimensional object coordinates.

[0010] The coordinates of a 3D object are transformed from the camera coordinate system to the robot coordinate system, and then from the robot coordinate system to a 2D map, thus calibrating the coordinates of the 3D object on the 2D map and obtaining an enhanced semantic map.

[0011] Based on the enhanced semantic map, the destination is sent to the mobile robot, enabling the mobile robot to plan a path between its own location and the destination location.

[0012] Two-dimensional map construction includes:

[0013] Point cloud information obtained at different times or locations is matched to determine the relative positional relationship between point cloud data and obtain point cloud matching results;

[0014] The attitude information of the mobile robot is acquired by the IMU sensor, and the position and orientation of the robot in three-dimensional space are estimated based on the attitude information to obtain the attitude estimation result.

[0015] Based on the point cloud matching results and pose estimation results, estimate the pose of the mobile robot in the global coordinate system;

[0016] Project the point cloud information obtained under all known poses into a global coordinate system, and use the SLAM algorithm to construct a two-dimensional planar map;

[0017] When the mobile robot moves, repeat the above steps to update the two-dimensional map.

[0018] Semantic information acquisition of target objects in the environment includes: object images obtained by scanning objects in the environment with a depth camera are used to create a dataset using labelme, and after creation, they are imported into the yolov5s algorithm for training to achieve object detection in the environment.

[0019] The RANSAC algorithm process includes:

[0020] A set of samples is randomly selected from the original dataset as the initial set of interior points, and an initial model is estimated based on these samples;

[0021] Fit a model using selected samples and apply it to the entire dataset;

[0022] For each data point, determine whether it belongs to an interior point based on a preset threshold. If it does, add it to the interior point set; otherwise, add it to the exterior point set.

[0023] The model parameters are re-estimated based on the current set of inliers, and the number of inliers is recorded as an evaluation metric. If this evaluation metric is better than any previous iteration, the current optimal model and set of inliers are updated.

[0024] Point cloud information fitting includes first performing planar fitting, and then performing 3D model fitting. We first assume P is the input point cloud and the output is the optimal shape parameters BS. For a cylinder {P*, a, r}, P* is a point on the cylinder's axis, a is the cylinder's directional axis, and r is the radius. We arbitrarily select three points from the original point cloud and extract a plane based on these three points. The equation of this plane is calculated as follows:

[0025] z = Ax + By + Cz (1)

[0026] Plane fitting includes the following steps:

[0027] Select any three points from the point cloud dataset.

[0028] {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)};

[0029] A plane S is determined based on the positions of {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)}, and parameters A, B, and C are determined by formula (2).

[0030]

[0031] Count the number of points on plane S, set the plane thickness threshold ε to 0.2, and calculate the number of points P at any point P in P. i Distance d to plane S i ,

[0032]

[0033] Statistics d i The number of points less than ε is denoted as the score of S;

[0034] Step d1: Repeat the above steps K times to select the plane S* with the highest score, where K is calculated using the following formula:

[0035]

[0036] In the above formula, m refers to the total number of points in the point cloud, and n represents the number of feature points in the point cloud. Since the values ​​of m and n are relatively large, an approximation is used for calculation, resulting in the following formula:

[0037] 1-(1-(1-τ) 3 ) k=Φ (5)

[0038]

[0039] In the formula, τ refers to the proportion of points located outside the plane S*, and Φ represents the probability of the plane being selected after K samplings;

[0040] 3D model fitting includes the following steps:

[0041] Assume the shape equation of the cylinder surface, and that the distance from a point on the cylinder surface to its axis is always equal to the radius r0. Assume an arbitrary point P on the cylinder surface, and let P0(x0, y0, z0) represent a point on the cylinder axis. Let (a, b, c) be the cylinder axis vector, i.e.:

[0042]

[0043] Find the seven parameters: x0, y0, z0, a, b, c, and r0.

[0044] Randomly search for several neighboring points of a point on the cylindrical surface to form a plane, and calculate the unit normal vector of each point on the cylindrical surface;

[0045] Treat the unit normal vector of each point as a point, and then fit these points to a plane to obtain the plane normal vector, which is the initial value of the cylinder axis vector x0, y0, z0;

[0046] After obtaining the axis, the cylinder is transformed into a coordinate system, so that the coordinates of the cylinder axis vector (x0, y0, z0) are transformed into a vector parallel to the Z-axis. In this way, the coordinates of the point (x0, y0) on the cylinder surface are a planar circle. The center of the circle, i.e. (x1, y1, z1), and the radius r are fitted together.

[0047] The destination is sent by the user via Wi-Fi from the user's device, which can be a PC or a mobile phone.

[0048] Path planning for mobile robots is divided into global path planning and local path planning. The global path planning algorithm uses the A-Star algorithm. The local path planning algorithm uses the TEB algorithm.

[0049] The A-star algorithm includes the following steps:

[0050] The starting point of the indoor mobile robot is denoted as node Q and placed in the Open List. At the same time, nodes in the environment that have been identified as obstacle coverage areas or unknown nodes are denoted as obstacle nodes and stored in the Closed List.

[0051] Search around Q for reachable nodes and add nodes with a cost value of 0 from the cost grid map to the Open List.

[0052] Pop node S from the Open List and add it to the Closed List. Simultaneously calculate the f(r) value of each reachable node, and select the node with the smallest f(r) value as the next node for the indoor mobile robot ROS vehicle to move to.

[0053] If the Open List is empty at this point, it means the search failed, i.e., no feasible route was found. If the target node is in the Closed List, the algorithm search is successful; otherwise, it will traverse all the grids in the neighborhood of the current node r, calculate the f(n) value of each grid, and select the node t with the smallest f(n) as the next node for the indoor mobile robot ROS to move to. Then, it will take the current node out of the Open List and put it into the Closed List.

[0054] Determine if node t is in both the Open List and the Closed List: If it is neither in the Open List nor in the Closed List, add it to the Open List; if it is in the Open List, then combine the calculated node t f(n) with the epoch value f pre-stored in the Open List. pre Compare the magnitudes of f(r). If f(r) is less than f... pre If f(r), then update the already stored f with the value of f(r). pre (r) value, and add node m to the Closed List; if it is in the Closed List, skip this node, return to the previous step to compare and expand other child nodes.

[0055] Repeat the above steps until the target point is found or the Open List is empty;

[0056] The TEB algorithm includes the following steps:

[0057] Obtain the global path planned by the global path planning algorithm;

[0058] Under the global path, w state points are inserted at equal time intervals. The velocity is calculated based on the distance and time difference between each point, and then the acceleration and angular velocity are calculated by differentiation.

[0059] By adding constraints to the objective function, the constraint function enables the car to better achieve obstacle avoidance, moderate speed, smooth turning trajectory, and the fastest path.

[0060] Trajectory optimization is performed using g2o. Since the local trajectory generated by the TEB algorithm is composed of a series of discrete poses, these discrete pose trajectories need to be optimized to truly achieve the goals of short time, short distance, and obstacle avoidance.

[0061] After optimization, the output speed command is used as a condition for the movement of the motion vehicle.

[0062] An indoor mobile robot visual navigation device, comprising:

[0063] The 2D planar map construction and semantic information acquisition module is used to control the movement of the mobile robot. It scans the environment with the LiDAR on the mobile robot to obtain point cloud information, constructs a 2D planar map based on the point cloud information, and scans objects in the environment with the depth camera on the mobile robot to obtain the semantic information of the target objects in the environment.

[0064] The point cloud information acquisition and segmentation module is used to select the point cloud region corresponding to the semantic information of the target object in the two-dimensional planar map, and use the RANSAC algorithm to segment the corresponding point cloud region to obtain the planar and cylindrical surfaces of the point cloud region corresponding to the target object.

[0065] The point cloud information fitting module is used to fit the point cloud information of the plane and the cylindrical surface of the point cloud region corresponding to the target object. After the plane is fitted and extracted, a three-dimensional model is fitted, and the two-dimensional coordinates of the point cloud region corresponding to the target object are expanded into three-dimensional coordinates to obtain the three-dimensional object coordinates.

[0066] The 3D object target determination module is used to transform the coordinates of 3D objects from the camera coordinate system to the robot coordinate system, and then from the robot coordinate system to a 2D plane map, so as to realize the calibration of the coordinates of 3D objects on the 2D plane map and obtain an enhanced semantic map.

[0067] The path planning module is used to send the destination to the mobile robot based on the enhanced semantic map, enabling the mobile robot to plan a path between its own location and the destination location.

[0068] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the visual navigation method for an indoor mobile robot.

[0069] The beneficial effects of this invention are as follows: This invention provides a visual navigation method for indoor mobile robots. Addressing the shortcomings in semantic map construction, it utilizes LiDAR combined with a graph-optimized LiDAR SLAM algorithm for mapping. Simultaneously, it employs a depth camera combined with an object detection algorithm to acquire the types and depth information of objects in the environment. Furthermore, it combines the RANSAC algorithm for point cloud segmentation to obtain the position of the 3D model, thus constructing a more accurate real-time semantic map and improving object recognition accuracy. The previously segmented planes and cylinders are fitted and extracted to form a 3D model. Based on the obtained object types, the model is simplified into simple 3D models, thereby improving the efficiency of point cloud labeling, saving map space, and accelerating map search speed. The global path planning A-star algorithm is combined with the local path planning TEB algorithm. Comparison through simulation and real indoor environment experiments shows that the combined algorithm significantly improves the path planning efficiency compared to using only the global path planning A-star algorithm, even on a known map. Attached Figure Description

[0070] Figure 1 This is a flowchart illustrating the visual navigation method for an indoor mobile robot according to the present invention.

[0071] Figure 2 This is a framework diagram of the Cartographer algorithm in this invention;

[0072] Figure 3 This is a structural diagram of the Cartographer map in this invention;

[0073] Figure 4 This is a diagram of the RANSAC linear fitting model in this invention;

[0074] Figure 5 This is a schematic diagram of planar fitting in this invention;

[0075] Figure 6 To determine the cylindrical diagram in this invention;

[0076] Figure 7 This is a diagram illustrating the fitting process of the three-dimensional model of the cylinder in this invention;

[0077] Figure 8 This is a fitting diagram of the three-dimensional model in this invention;

[0078] Figure 9 This is a diagram illustrating the map coordinate transformation process in this invention.

[0079] Figure 10 This is a flowchart of the TEB algorithm in this invention. Detailed Implementation

[0080] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to illustrate the technical solution of the present invention more clearly, and should not be used to limit the scope of protection of the present invention.

[0081] like Figure 1 As shown, this invention discloses a visual navigation method for indoor mobile robots, specifically including the following steps:

[0082] Step 1, 2D map construction: When the mobile robot is in an unfamiliar environment, the user first controls the robot's movement via Bluetooth. The LiDAR scans and acquires information about the surrounding environment to perceive the robot's position and surroundings. The robot then uses the Cartographer algorithm based on graph optimization to construct a 2D map, converting the environmental information into point cloud data. This data is then processed using SLAM technology to construct a 2D semantic map, ultimately generating a high-precision map.

[0083] The Cartographer algorithm framework diagram is as follows: Figure 2 As shown, local SLAM is responsible for quickly constructing a map of the robot's environment using data received from sensors. It employs a "segmented scan matching" method, dividing the data scanned by the LiDAR into multiple small blocks and matching each block against a known map. The Cartographer map is structured as follows: Figure 3 As shown in the figure, `scan` represents a LiDAR scan frame, `submap` represents a local submap, and `submaps` represents the global map. The entire map of the Cartographer algorithm is organized from local submaps, where each local submap consists of multiple LiDAR scan frames. Finally, all local submaps further form the global map. During local SLAM, loop closure detection is a constant process. Loop closure detection refers to identifying already scanned areas during map building to avoid duplicate scans and merging these areas into a whole. Loop closure detection typically uses sensor data matching to determine if there is similarity between two or more locations and merges them into the same region. The main role of global SLAM is to optimize loop closure detection. This problem is similar to scan matching, transforming it into a nonlinear least squares problem. The Ceres method is used to optimize the optimization problem constructed through SPA (Sparse Position Adjustment).

[0084] The algorithm implementation in 2D map construction specifically includes the following steps:

[0085] a. LiDAR scanning: Using LiDAR to scan the environment and obtain point cloud data.

[0086] b. Attitude estimation: Obtain the robot's attitude information through sensors such as IMU, and estimate the robot's position and orientation in three-dimensional space based on the attitude.

[0087] c. Point cloud matching: Matching point cloud data obtained at different times or locations to determine their relative positional relationships.

[0088] d. Pose estimation: Based on the point cloud matching results and pose estimation results, the robot's pose in the global coordinate system is estimated using optimization methods.

[0089] e. Map building: Project the point cloud data obtained under all known poses into a global coordinate system, and use the SLAM algorithm to build a high-precision map.

[0090] f. Map Update: When the robot moves, repeat the above steps to update the map.

[0091] Step 2, Semantic Information Acquisition: This invention primarily employs YOLOv5s as the object detection algorithm in the environment to identify the types of objects. A dataset is created by using LabelMe to collect images of the main objects in the application scene obtained by the mobile robot's depth camera. After dataset creation, these images are imported into the YOLOv5s algorithm for training, enabling the detection of objects in the environment.

[0092] Step 3, Depth Point Cloud Information Acquisition and Segmentation: After acquiring the semantic information of the target object through the depth camera, it is necessary to obtain the coordinate relationship of the target semantic object with respect to the robot. However, the target semantic information is an RGB image, which only provides two-dimensional information and lacks three-dimensional information. Therefore, it is necessary to use the point cloud information of the target object obtained by the depth camera to determine the position of the target object. The point cloud information has the same dimension as the RGB image. By selecting the point cloud region corresponding to the RGB image of the target object, the RANSAC algorithm is used to segment the point cloud information of the target object, obtaining the planar and cylindrical surfaces of the point cloud region corresponding to the target object.

[0093] The basic idea of ​​the RANSAC (Random Sample Consensus) algorithm is to randomly select a set of sample points, then fit a model using these points, and determine which data points belong to this model based on a certain threshold—the basic idea of ​​region growing. The method for segmenting regions starts with a seed, finds similar points in the surrounding area, and continues to expand outwards until no more points meet the criteria. The RANSAC algorithm can... Figure 4 An example of fitting a straight line is used to illustrate this.

[0094] The basic process of the RANSAC algorithm is as follows: First, a subset of samples from the dataset is randomly selected to construct a model, which is then applied to all data points. Next, based on a preset threshold, data points that conform to the model are identified; these are called inliers, and those that do not are called outliers. Finally, the model parameters are iteratively selected by selecting inliers and re-estimated until a preset number of iterations is reached or a stopping condition is met.

[0095] The RANSAC algorithm mainly consists of the following four steps:

[0096] a. Random sampling: Randomly select a set of samples from the original dataset as the initial set of interior points, and estimate an initial model based on these samples.

[0097] b. Model Fitting: Fit a model using selected samples and apply it to the entire dataset.

[0098] c. Inner Point Filtering: For each data point, determine whether it belongs to an inner point based on a preset threshold. If it does, add it to the inner point set; otherwise, add it to the outer point set.

[0099] d. Model Evaluation: Re-estimate the model parameters based on the current set of interior points and record the number of interior points as an evaluation metric. If this evaluation metric is better than any previous iteration, update the current optimal model and the set of interior points.

[0100] Step 4: Point Cloud Information Fitting. Since point cloud data has three-dimensional coordinates, while the traditional RANSAC algorithm only has two-dimensional coordinates, the point cloud information fitting in this invention needs to extend the RANSAC algorithm to three-dimensional coordinates. The extension process from two-dimensional to three-dimensional is similar to that in two-dimensional space; it only requires dimensional expansion. First, the plane needs to be fitted and extracted before fitting the three-dimensional model. Therefore, this section first introduces plane fitting based on the RANSAC algorithm, and then performs three-dimensional model fitting. We need to assume that P is the input point cloud, and the output is the optimal shape parameters BS. For a cylinder {P*, a, r}, P* is a point on the cylinder's axis, a is the direction axis of the cylinder, and r is the radius.

[0101] Three points are randomly selected from the original point cloud, and a plane is extracted based on these three points. The equation of this plane is calculated as follows:

[0102] z = Ax + By + Cz (1)

[0103] a. Select any three points {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)} in the point cloud dataset.

[0104] b. Determine a plane S based on the positions of {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)}.

[0105] Parameters A, B, and C are determined by formula (3-15).

[0106]

[0107] c. Count the number of points on plane S. Set the plane thickness threshold to 0.2, and calculate the number of points P at any point P in plane S. i Distance d to plane S i .

[0108]

[0109] Statistics d i The number of points less than ε is denoted as the score of S.

[0110] d. Repeat Step 1 to Step 3 K times, and select the plane S* with the highest score, where K is calculated using the following formula:

[0111]

[0112] In the above formula, m refers to the total number of points in the point cloud, and n represents the number of feature points in the point cloud. Since the values ​​of m and n are relatively large, an approximation is used for calculation, resulting in the following formula:

[0113] 1-(1-(1-τ) 3 ) k =Φ (5)

[0114]

[0115] In the formula, τ refers to the estimated proportion of points located outside the plane S*, and Φ represents the probability of the plane being selected after K samplings. Figure 5 This is a schematic diagram of plane fitting. The gray area represents the ground, which is outside the fitting area and is therefore filtered out. The light gray area represents the selected point cloud region, and the black area represents the final fitted plane.

[0116] Fitting a cylindrical surface is similar to fitting a plane. First, assume the shape equation of the cylindrical surface, where the distance from a point on the cylindrical surface to its axis is always equal to the radius r0. Assume we take any point P on the cylindrical surface, and let P0(x0,y0,z0) represent a point on the cylindrical axis. Let (a,b,c) be the vector of the cylindrical axis, i.e.:

[0117]

[0118] By finding these seven parameters, a cylinder can be uniquely determined, such as... Figure 6 As shown.

[0119] Randomly search for several neighboring points of a point on the cylindrical surface to fit a plane. Calculate the unit normal vector of each point on the cylindrical surface. Treat each point's unit normal vector as a point, and then fit these points to a plane to obtain the plane normal vector, i.e., the initial values ​​of the cylinder axis vector x0, y0, z0. After obtaining the axis, perform a coordinate transformation on the cylinder so that the cylinder axis vector coordinates (x0, y0, z0) are transformed into a vector parallel to the Z-axis. Thus, the coordinates of the point (x0, y0) on the cylindrical surface are a planar circle. Fit these coordinates to the center of the circle, i.e., (x1, y1, z1), and the radius r. The point cloud fitting result is as follows. Figure 7 As shown.

[0120] Because point cloud data is large in volume, directly converting the coordinates of the point cloud data to the world coordinate system and placing it on the map occupies a significant amount of space. Furthermore, the 3D shapes of these objects are generally quite fixed; for example, a door can be enclosed as a flat 3D model, and a human as a cylindrical 3D model. To save map space and accelerate map search speed, this invention proposes a model envelopment scheme that combines and simplifies the object types obtained from the YOLOv5 algorithm for fire extinguisher boxes, doors, and trash cans into simpler 3D models. Figure 8 The above method was used to fit the selected fire extinguisher, door, and trash can.

[0121] Step 5: 3D Object Target Determination. Since the coordinate representation of the obtained object information differs under different sensors, a unified description of the object's coordinates is needed. As the number and complexity of object coordinates increase from a small amount to a large amount, manual coordinate transformation would consume too much manpower and resources, and the sheer volume of data would easily lead to an error rate. Therefore, this paper uses the coordinate transformation (TF tree) function in the ROS package to transform the object's coordinates on the map.

[0122] During the mapping process, the robot first obtains its coordinate system. The depth point cloud information contains coordinates relative to the depth camera's coordinate system. The 3D object coordinates obtained from the depth point cloud information are first transformed from the camera coordinate system to the robot coordinate system, and then transformed from the robot coordinate system to a 2D map, completing the calibration of the 3D model on the 2D map and ultimately achieving an enhanced semantic map. The coordinate transformation process is as follows: Figure 9 As shown.

[0123] Step Six, Path Planning. After the semantic map is built, the mobile robot has a deep understanding of its environment and can now deliver items. Users can send their destination to the mobile robot via Wi-Fi from their device (PC or mobile phone), and the robot will plan a path between its current location and the destination.

[0124] Path planning for mobile robots is divided into global path planning and local path planning. Global path planning involves the robot planning a path between the starting point and the target point on a known map. Local path planning uses sensors to detect dynamic obstacles and then updates the local sub-map to maintain a certain distance from the dynamic obstacles and avoid collisions. The global path planning algorithm of this invention adopts the A-Star algorithm. The A-Star algorithm uses a heuristic evaluation function. Its principle is to perform a heuristic search on the environmental map, mainly planning the path and applying the evaluation function to determine its expansion direction. It is often used as a global path planning strategy in a static two-dimensional plane. The A-Star algorithm maintains an open list and a closed list to record the grids that have been searched and those to be searched, and prioritizes them by comparing the sizes of different grids. The open list stores the adjacent grids that have been searched as the grids to be searched next, while the closed list stores the smallest grid obtained after each sort.

[0125] The implementation process of the A-star algorithm is as follows:

[0126] a. Record the starting point of the indoor mobile robot as node Q and put it into the Open List. At the same time, record the nodes in the environment that have been identified as obstacle coverage areas or unknown nodes as obstacle nodes and store them in the Closed List.

[0127] b. Search around Q for reachable nodes and add nodes with a cost value of 0 from the cost grid map to the Open List.

[0128] c. Pop node S from the Open List and add it to the Closed List. Simultaneously calculate the f(n) value of each reachable node, and select the node with the smallest f(n) value as the next node for the indoor mobile robot ROS vehicle to move to.

[0129] d. If the Open List is empty at this point, it means the search has failed, i.e., no feasible route was found. If the target node is in the Closed List, the algorithm has succeeded in its search; otherwise, it will traverse all the grids in the neighborhood of the current node n, calculate the f(n) value of each grid, and select the node m with the smallest f(n) as the next node for the indoor mobile robot ROS to move to. Then, it will remove the current node from the Open List and put it into the Closed List.

[0130] e. Determine if node m is in both the Open List and the Closed List: If it is neither in the Open List nor in the Closed List, add it to the Open List; if it is in the Open List, then add the calculated node m f(n) and the epoch value f pre-stored in the Open List. pre Compare the sizes of f(n). If f(n) is less than f... pre If f(n), then update the already stored f with the value of f(n). pre (n) value, and add node m to the Closed List; if it is in the Closed List, skip this node, return to the previous step to compare and expand other child nodes.

[0131] f. Repeat the above steps until the target point is found or the Open List is empty.

[0132] In complex indoor environments, many objects are dynamically changing, making it highly likely that the robot will encounter temporarily changing obstacles during its operation. Therefore, for the robot to successfully reach its destination in an unknown environment, local path planning is needed to assist in obstacle avoidance. This invention employs the TEB algorithm for local path planning. The TEB algorithm essentially solves for multiple objectives. Since the local paths of many objectives depend only on a few consecutive robot states and are only related to a small set of parameters, they are all local, hence the term "local optimization." The algorithm's flowchart is as follows... Figure 10 As shown, the main steps are:

[0133] a. Obtain the global path planned by the global path planning algorithm;

[0134] b. Insert n state points at equal time intervals under the global path, calculate the velocity based on the distance and time difference between each point, and then calculate the values ​​of acceleration and angular velocity by differentiation;

[0135] c. By adding constraints to the objective function, the constraint function enables the car to better achieve obstacle avoidance, moderate speed, smooth turning trajectory, and the fastest path.

[0136] d. Optimize the trajectory through g2o. Since the local trajectory generated by the TEB algorithm is composed of a series of discrete poses, it is necessary to optimize the trajectory composed of these discrete poses to truly achieve the goals of short time, short distance, and obstacle avoidance.

[0137] e. After optimization, the output speed command is used as the condition for the movement of the trolley.

[0138] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A visual navigation method for an indoor mobile robot, characterized in that: Includes the following steps: The system controls the movement of a mobile robot, scans the environment using a lidar on the robot to obtain point cloud information, constructs a two-dimensional map based on the point cloud information, and scans objects in the environment using a depth camera on the robot to obtain semantic information of target objects in the environment. Select the point cloud region corresponding to the semantic information of the target object in the two-dimensional planar map, and use the RANSAC algorithm to segment the corresponding point cloud region to obtain the planar and cylindrical surfaces of the point cloud region corresponding to the target object. The point cloud information of the plane and the cylindrical surface of the point cloud region corresponding to the target object is fitted. After the plane is fitted and extracted, a three-dimensional model is fitted. The two-dimensional coordinates of the point cloud region corresponding to the target object are expanded into three-dimensional coordinates to obtain the three-dimensional object coordinates. The coordinates of a 3D object are transformed from the camera coordinate system to the robot coordinate system, and then from the robot coordinate system to a 2D map, thus calibrating the coordinates of the 3D object on the 2D map and obtaining an enhanced semantic map. Based on the enhanced semantic map, the destination is sent to the mobile robot, enabling the mobile robot to plan a path between its own location and the destination location.

2. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: Two-dimensional map construction includes: Point cloud information obtained at different times or locations is matched to determine the relative positional relationship between point cloud data and obtain point cloud matching results; The attitude information of the mobile robot is acquired by the IMU sensor, and the position and orientation of the robot in three-dimensional space are estimated based on the attitude information to obtain the attitude estimation result. Based on the point cloud matching results and pose estimation results, estimate the pose of the mobile robot in the global coordinate system; Project the point cloud information obtained under all known poses into a global coordinate system, and use the SLAM algorithm to construct a two-dimensional planar map; When the mobile robot moves, repeat the above steps to update the two-dimensional map.

3. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: Semantic information acquisition of target objects in the environment includes: object images obtained by scanning objects in the environment with a depth camera are used to create a dataset using labelme, and after creation, they are imported into the yolov5s algorithm for training to achieve object detection in the environment.

4. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: The RANSAC algorithm process includes: A set of samples is randomly selected from the original dataset as the initial set of interior points, and an initial model is estimated based on these samples; Fit a model using selected samples and apply it to the entire dataset; For each data point, determine whether it belongs to an interior point based on a preset threshold. If it does, add it to the interior point set; otherwise, add it to the exterior point set. The model parameters are re-estimated based on the current set of inliers, and the number of inliers is recorded as an evaluation metric. If this evaluation metric is better than any previous iteration, the current optimal model and set of inliers are updated.

5. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: Point cloud information fitting includes first performing planar fitting, and then performing 3D model fitting. We first assume P is the input point cloud and the output is the optimal shape parameters BS. For a cylinder {P*, a, r}, P* is a point on the cylinder's axis, a is the cylinder's directional axis, and r is the radius. We arbitrarily select three points from the original point cloud and extract a plane based on these three points. The equation of this plane is calculated as follows: z = Ax + By + Cz (1) Plane fitting includes the following steps: Select any three points from the point cloud dataset. {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)}; A plane S is determined based on the positions of {P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)}, and parameters A, B, and C are determined by formula (2). Count the number of points on plane S, set the plane thickness threshold ε to 0.2, and calculate the number of points P at any point P in P. i Distance d to plane S i , Statistics d i The number of points less than ε is denoted as the score of S; Step d1: Repeat the above steps K times to select the plane S* with the highest score, where K is calculated using the following formula: In the above formula, m refers to the total number of points in the point cloud, and n represents the number of feature points in the point cloud. Since the values ​​of m and n are relatively large, an approximation is used for calculation, resulting in the following formula: 1-(1-(1-t) 3 ) k =Φ (5) In the formula, τ refers to the proportion of points located outside the plane S*, and Φ represents the probability of the plane being selected after K samplings; 3D model fitting includes the following steps: Assume the shape equation of the cylinder surface, and that the distance from a point on the cylinder surface to its axis is always equal to the radius r0. Assume an arbitrary point P on the cylinder surface, and let P0(x0, y0, z0) represent a point on the cylinder axis. Let (a, b, c) be the cylinder axis vector, i.e.: Find the seven parameters: x0, y0, z0, a, b, c, and r0. Randomly search for several neighboring points of a point on the cylindrical surface to form a plane, and calculate the unit normal vector of each point on the cylindrical surface; Treat the unit normal vector of each point as a point, and then fit these points to a plane to obtain the plane normal vector, which is the initial value of the cylinder axis vector x0, y0, z0; After obtaining the axis, the cylinder is transformed into a coordinate system, so that the coordinates of the cylinder axis vector (x0, y0, z0) are transformed into a vector parallel to the Z-axis. In this way, the coordinates of the point (x0, y0) on the cylinder surface are a planar circle. The center of the circle, i.e. (x1, y1, z1), and the radius r are fitted together.

6. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: The destination is sent by the user via Wi-Fi from the user's device, which can be a PC or a mobile phone.

7. The visual navigation method for an indoor mobile robot according to claim 1, characterized in that: Path planning for mobile robots is divided into global path planning and local path planning. The global path planning algorithm uses the A-Star algorithm, and the local path planning algorithm uses the TEB algorithm. The A-Star algorithm includes the following steps: The starting point of the indoor mobile robot is denoted as node Q and placed in the Open List. At the same time, nodes in the environment that have been identified as obstacle coverage areas or unknown nodes are denoted as obstacle nodes and stored in the Closed List. Search around Q for reachable nodes and add nodes with a cost value of 0 from the cost grid map to the OpenList; Pop node S from the Open List and add it to the Closed List. At the same time, calculate the f(r) value of each reachable node and select the node with the smallest f(r) value as the node that the indoor mobile robot ROS car will move to next. If the Open List is empty at this time, it means that the search has failed, that is, no feasible route has been found. If the target node is in the Closed List, the algorithm has succeeded in the search. Otherwise, it will traverse all the grids in the neighborhood of the current node r, calculate the f(n) value of each grid, and select the node t with the smallest f(n) as the next node to be moved by the indoor mobile robot ROS car. Then, the current node is taken out from the Open List and put into the Closed List. Determine if node t is in the Open List and Closed List: If it is neither in the Open List nor in the Closed List, add it to the Open List; If it is in the Open List, then the calculated node t f(n) and the epoch value f pre-stored in the Open List will be combined. pre Compare the magnitudes of f(r). If f(r) is less than f... pre If f(r), then update the already stored f with the value of f(r). pre (r) value, and add the m node to the Closed List; If it is in the Closed List, skip this node, return to the previous step to compare and expand other child nodes; Repeat the above steps until the target point is found or the Open List is empty; The TEB algorithm includes the following steps: Obtain the global path planned by the global path planning algorithm; Under the global path, w state points are inserted at equal time intervals. The velocity is calculated based on the distance and time difference between each point, and then the acceleration and angular velocity are calculated by differentiation. By adding constraints to the objective function, the constraint function enables the car to better achieve obstacle avoidance, moderate speed, smooth turning trajectory, and the fastest path. Trajectory optimization is performed using a general graph optimization algorithm. Since the local trajectory generated by the TEB algorithm is composed of a series of discrete poses, these discrete pose trajectories need to be optimized to truly achieve the goals of short time, short distance, and obstacle avoidance. After optimization, the output speed command is used as a condition for the movement of the motion vehicle.

8. An indoor mobile robot visual navigation device, characterized in that, include: The 2D planar map construction and semantic information acquisition module is used to control the movement of the mobile robot. It scans the environment with the LiDAR on the mobile robot to obtain point cloud information, constructs a 2D planar map based on the point cloud information, and scans objects in the environment with the depth camera on the mobile robot to obtain the semantic information of the target objects in the environment. The point cloud information acquisition and segmentation module is used to select the point cloud region corresponding to the semantic information of the target object in the two-dimensional planar map, and use the RANSAC algorithm to segment the corresponding point cloud region to obtain the planar and cylindrical surfaces of the point cloud region corresponding to the target object. The point cloud information fitting module is used to fit the point cloud information of the plane and the cylindrical surface of the point cloud region corresponding to the target object. After the plane is fitted and extracted, a three-dimensional model is fitted, and the two-dimensional coordinates of the point cloud region corresponding to the target object are expanded into three-dimensional coordinates to obtain the three-dimensional object coordinates. The 3D object target determination module is used to transform the coordinates of 3D objects from the camera coordinate system to the robot coordinate system, and then from the robot coordinate system to a 2D plane map, so as to realize the calibration of the coordinates of 3D objects on the 2D plane map and obtain an enhanced semantic map. The path planning module is used to send the destination to the mobile robot based on the enhanced semantic map, enabling the mobile robot to plan a path between its own location and the destination location.

9. A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the indoor mobile robot visual navigation method as described in any one of claims 1-7.

Images